一、标准摆线公式
{
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πTyt−sin(2πTyt))2πS0(1−cos(2πTyt))
此处的
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(21Ty2π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.