四足机器人摆线规划程序

发布于：2024-05-06 ⋅ 阅读:(20) ⋅ 点赞:(0)

一、标准摆线公式

{ x = r ∗ ( θ − sin ⁡ ( θ ) ) y = r ∗ ( 1 − cos ⁡ ( θ ) ) \left\{\begin{array}{l} x=r *(\theta-\sin (\theta)) \\ y=r *(1-\cos (\theta)) \end{array}\right. {x=r∗(θ−sin(θ))y=r∗(1−cos(θ))​
这里的r表示摆线的圆的半径， θ \theta θ是圆的半径所经过的弧度（滚动角）。

for index = 0:1:1000
    theta = 2*pi/1000*index
    x(index+1) = r*(theta - sin(theta));
    y(index+1) = r*(1 - cos(theta));
end
plot (x, y,'-r','linewidth',1);
axis equal
grid on
xlabel('X')
ylabel('Y')

二、摆动相的摆线公式

{ x ( t ) = S 0 2 π ( 2 π t T y − sin ⁡ ( 2 π t T y ) ) y ( t ) = S 0 2 π ( 1 − cos ⁡ ( 2 π t T y ) ) \left\{\begin{array}{c} x(t)&=&\frac{S_{0}}{2 \pi}\left(2 \pi \frac{t}{T_{y}}-\sin \left(2 \pi \frac{t}{T_{y}}\right)\right) \\ y(t) &=&\frac{S_{0}}{2 \pi}\left(1-\cos \left(2 \pi \frac{t}{T_{y}}\right)\right) \end{array}\right. ⎩ ⎨ ⎧​x(t)y(t)​==​2πS0​​(2πTy​t​−sin(2πTy​t​))2πS0​​(1−cos(2πTy​t​))​
此处的 T y T_y Ty​表示摆动相时间，t是

三、足端轨迹约束

z ( t ) = a t + b sin ⁡ ( 2 π 1 2 T y t ) + c z(t)=a t+b \sin \left(\frac{2 \pi}{\frac{1}{2} T_{y}} t\right)+c z(t)=at+bsin(21​Ty​2π​t)+c

四、摆动相完整描述

$$

五、步态周期摆线规划完整描述

% clc;
% clear;
close all
%% 改进的摆线规划
S_0 = 0.4; % 步长给定150mm
H = 0.2;
r = S_0/(2*pi); % 摆线的半径
Ty = 2;      %运行时间2s
Tst = 2;
Tt = Ty + Tst; % 
Point_num_Tt = 1000;% 这些点是一个步态周期的点数
Point_num = 8*Point_num_Tt;% 这些点是一个步态周期的点数

Point_num_dv = round((Ty/Tt)*Point_num);
Tsa = Tt/(Point_num_Tt-1);
LF = zeros(Point_num,2);
RF = zeros(Point_num,2);
LB = zeros(Point_num,2);
RB = zeros(Point_num,2);
Trun = zeros(Point_num_Tt,1);
T = zeros(Point_num,1);
n = 4;
Sgn_1 = 0;
T_gait = [0, 0.25, 0.5, 0.75;
          0,  0.5, 0.5, 0    ];%步态相位矩阵
gait_mode = 2; % 1表示walk，2表示trot
%% 运动学部分
du_trans = 180/pi;
rad_trans = pi/180;
True = 1;
Flase = 0;
% syms L1 L2 L3 alpha beta gama alpha_du beta_du gama_du xx yy zz
L1 = 0.5;
L2 = 1;
L3 = 1;
% for i = 1:1:Point_num
alpha_du = 0;
beta_du  = 45;%%限定初始角度
gama_du  = -90;  
%% 运动学正解
[Trans_LF,Rot_LF,Pos_LF_init] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du);% function [T,R,P] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du)%初始位置的位置
[Trans_RF,Rot_RF,Pos_RF_init] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du);% function [T,R,P] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du)%初始位置的位置
[Trans_RB,Rot_RB,Pos_RB_init] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du);% function [T,R,P] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du)%初始位置的位置
[Trans_LB,Rot_LB,Pos_LB_init] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du)% function [T,R,P] = Kine(L1,L2,L3,alpha_du,beta_du,gama_du)%初始位置的位置
%% 运动学逆解
theta_LF = Kine_inv(L1,L2,L3,Pos_LF_init);% 
theta_RF = Kine_inv(L1,L2,L3,Pos_RF_init);% 
theta_RB = Kine_inv(L1,L2,L3,Pos_RB_init);% 
theta_LB = Kine_inv(L1,L2,L3,Pos_LB_init)% 
Pos_LF = zeros(Point_num,3);
Pos_RF = zeros(Point_num,3);
Pos_RB = zeros(Point_num,3);
Pos_LB = zeros(Point_num,3);
Theta_LF = zeros(Point_num,3);
Theta_RF = zeros(Point_num,3);
Theta_RB = zeros(Point_num,3);
Theta_LB = zeros(Point_num,3);
%% 摆动相与支撑相分别规划
% 取模运算
for index = 1:1:Point_num
    index_LF = mod(index + round(T_gait(gait_mode,1).*Point_num_Tt),Point_num_Tt) + 1;  %
    index_RF = mod(index + round(T_gait(gait_mode,2).*Point_num_Tt),Point_num_Tt) + 1;  %
    index_RB = mod(index + round(T_gait(gait_mode,3).*Point_num_Tt),Point_num_Tt) + 1;  %
    index_LB = mod(index + round(T_gait(gait_mode,4).*Point_num_Tt),Point_num_Tt) + 1;  %
    T(index) = (index - 1)*Tsa ;
    [LF(index,1), LF(index,2)] = Gait_cal(S_0,H,Ty,Tst,index_LF,Point_num_Tt); %[X, Y] = Gait_cal(S_0,H,Ty,Tst,index,Point_num_Tt)
    Pos_LF(index,1) = Pos_LF_init(1) - LF(index,2);
    Pos_LF(index,2) = Pos_LF_init(2);
    Pos_LF(index,3) = Pos_LF_init(3) + LF(index,1); 
    [theta] = Kine_inv(L1,L2,L3,Pos_LF(index,:));% 
    Theta_LF(index,:) = theta';
    [RF(index,1), RF(index,2)] = Gait_cal(S_0,H,Ty,Tst,index_RF,Point_num_Tt); %[X, Y] = Gait_cal(S_0,H,Ty,Tst,index,Point_num_Tt)
    Pos_RF(index,1) = Pos_RF_init(1) - RF(index,2);
    Pos_RF(index,2) = Pos_RF_init(2);
    Pos_RF(index,3) = Pos_RF_init(3) + RF(index,1); 
    [theta] = Kine_inv(L1,L2,L3,Pos_RF(index,:));% 
    Theta_RF(index,:) = theta';
    [RB(index,1), RB(index,2)] = Gait_cal(S_0,H,Ty,Tst,index_RB,Point_num_Tt); %[X, Y] = Gait_cal(S_0,H,Ty,Tst,index,Point_num_Tt)
    Pos_RB(index,1) = Pos_RB_init(1) - RB(index,2);
    Pos_RB(index,2) = Pos_RB_init(2);
    Pos_RB(index,3) = Pos_RB_init(3) + RB(index,1); 
    [theta] = Kine_inv(L1,L2,L3,Pos_RB(index,:));% 
    Theta_RB(index,:) = theta';
    [LB(index,1), LB(index,2)] = Gait_cal(S_0,H,Ty,Tst,index_LB,Point_num_Tt); %[X, Y] = Gait_cal(S_0,H,Ty,Tst,index,Point_num_Tt)
    Pos_LB(index,1) = Pos_LB_init(1) - LB(index,2);
    Pos_LB(index,2) = Pos_LB_init(2);
    Pos_LB(index,3) = Pos_LB_init(3) + LB(index,1); 
    [theta] = Kine_inv(L1,L2,L3,Pos_LB(index,:));% 
    Theta_LB(index,:) = theta';
end
figure(1)
plot (LF(:,1), LF(:,2),'-r','linewidth',1);
hold on
plot (RF(:,1), RF(:,2),'-r','linewidth',1);
hold on
plot (LB(:,1), LB(:,2),'-r','linewidth',1);
hold on
plot (RB(:,1), RB(:,2),'-r','linewidth',1);
axis equal
xlabel('X')
ylabel('Y')
grid minor
hold on
pic_index = 1;%记录图像编号for i = 1:end
% for k = 1:10:Point_num
%     cla;
%     plot (LF(:,1), LF(:,2),'-r','linewidth',1);
%     hold on;
%     plot (LF(k,1), LF(k,2),'g*','linewidth',2);
%     [A,map] = rgb2ind(frame2im(getframe),256);
%     if pic_index == 1
%         imwrite(A,map,'test.gif', 'gif','Loopcount',inf,'DelayTime',0.2);
%     else
%         imwrite(A,map,'test.gif','gif','writeMode','append','DelayTime',0.2);
%     end
%     pic_index = pic_index + 1;
% end
% plot(X(k), Y(k), 'g*','markersize',5, 'linewidth',1.5 );
legend('quadruped robot control');
figure(2)
subplot(4,1,1)
plot (T, LF(:,1),'-r',T,LF(:,2),'-b','linewidth',1);
% axis([0,32,-0.3,0.3])
xlabel('Time')
ylabel('X & Y')
legend('X graph','X graph');
grid minor
%%
subplot(4,1,2)
plot (T, RF(:,1),'-r',T,RF(:,2),'-b','linewidth',1);
% axis([0,32,-0.2,0.2])
xlabel('Time')
ylabel('X & Y')
legend('X graph','X graph');
grid minor
%%
subplot(4,1,3)
plot (T, LB(:,1),'-r',T,LB(:,2),'-b','linewidth',1);
% axis([0,32,-0.2,0.2])
xlabel('Time')
ylabel('X & Y')
legend('X graph','X graph');
grid minor
%%
subplot(4,1,4)
plot (T, RB(:,1),'-r',T,RB(:,2),'-b','linewidth',1);
% axis([0,32,-0.3,0.3])
xlabel('Time')
ylabel('X & Y')
legend('X graph','X graph');
grid minor
%%
figure(3)
subplot(2,1,1)
plot (T,Theta_LF(:,2),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(2,1,2)
plot (T,Theta_LF(:,3),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
%%
figure(4)
subplot(2,1,1)
plot (T,Theta_RF(:,2),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(2,1,2)
plot (T,Theta_RF(:,3),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
%%
figure(5)
subplot(2,1,1)
plot (T,Theta_RB(:,2),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(2,1,2)
plot (T,Theta_RB(:,3),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
%%
figure(6)
subplot(2,1,1)
plot (T,Theta_LB(:,2),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(2,1,2)
plot (T,Theta_LB(:,3),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
%%
figure(7)
subplot(4,1,1)
plot (T,Theta_LF(:,2),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(4,1,2)
plot (T,Theta_RF(:,2),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(4,1,3)
plot (T,Theta_RB(:,2),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(4,1,4)
plot (T,Theta_LB(:,2),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
figure(8)
subplot(4,1,1)
plot (T,Theta_LF(:,3),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(4,1,2)
plot (T,Theta_RF(:,3),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(4,1,3)
plot (T,Theta_RB(:,3),'-r','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor
subplot(4,1,4)
plot (T,Theta_LB(:,3),'-b','linewidth',1);
xlabel('Time')
ylabel('X & Y')
legend('X graph');
grid minor


function [X, Y] = Gait_cal(S_0,H,Ty,Tst,index1,Point_num_Tt)
%% 改进的摆线规划
r = S_0/(2*pi); % 摆线的半径
Tt = Ty + Tst; % 
Tsa = Tt/(Point_num_Tt-1);
n = 4;
%% 摆动相与支撑相分别规划
% 取模运算
    Trun = (index1 - 1)*Tsa;%时刻 
    if Trun >= 0 && Trun < Ty/2
        Sgn_1 = 1;
    else
        Sgn_1 = -1;
    end
    if Trun >= 0 && Trun < Ty
        X = r*(2*pi*Trun/Ty - sin(2*pi*Trun/Ty)); % x方向
        if Trun  < Ty/2
            Fe = Trun/Ty - (1/(n*pi))*sin((n*pi*Trun)/(Ty));
            Y = 2*H*(Trun/Ty - (1/(n*pi))*sin((n*pi*Trun)/(Ty)));
        else
            Fe = Trun/Ty - (1/(n*pi))*sin((n*pi*Trun)/(Ty));
            Y = H*(Sgn_1*(2*Fe - 1) + 1);
        end
    else
        X = 2*pi*r - r*(2*pi*(Trun - Ty)/(Tt - Ty) - sin(2*pi*(Trun - Ty)/(Tt - Ty))); % x方向
        Y =0;
    end 
end
%%

六、摆线轨迹

在这里插入图片描述

七、walk 步态

在这里插入图片描述

八、trot 步态

在这里插入图片描述

九、参考文献

[1]陈光荣. 四足机器人静动步态行走控制策略研究[D].北京理工大学,2018.
[2]郭晖晖. 四足机器人步态规划与运动控制研究[D].南京航空航天大学,2017.
[3]张千伟. 基于虚拟样机的四足机器人设计与步态研究[D].南京理工大学,2017.
[4]张志宇. 基于ADAMS的四足机器人虚拟样机仿真及刚柔耦合分析[D].哈尔滨工业大学,2016.

网站公告

今日签到

点亮在社区的每一天
去签到

热门文章

最新发布