代码为实现仿生四足机器人技术(罗庆生等)中关于CPG网络控制模型的部分的绘图。
本人新手 代码不保证正确性
方程:
两种方法通用一个函数:
代码中变量含义:
a=
B=
u=
Wsw=
W=
th=
Q=
A=a
x=[x1;y1;x2;y2......]
代码中的
通过书中的公式 使用
(占空比)来得出;相位关系可参照波士顿动力真的无可企及吗?一步步剖析四足机器人技术(一)中相关内容
function dydt = f( t,x )
%F 此处显示有关此函数的摘要
% 此处显示详细说明
a=10000;
B=0.75;
u=0.4;
Wsw=5*pi;
A=100;
QLF=0;%1
QRH=0.25;%3 0.25
QRF=1;%2
QLH=1;%4
W1=(((1-B)/B)*Wsw)/(exp(-1*A*x(2))+1)+(Wsw/(exp(A*x(2))+1));
W2=(((1-B)/B)*Wsw)/(exp(-1*A*x(4))+1)+(Wsw/(exp(A*x(4))+1));
W3=(((1-B)/B)*Wsw)/(exp(-1*A*x(6))+1)+(Wsw/(exp(A*x(6))+1));
W4=(((1-B)/B)*Wsw)/(exp(-1*A*x(8))+1)+(Wsw/(exp(A*x(8))+1));
r1=x(1)^2+x(2)^2;
r2=x(3)^2+x(4)^2;
r3=x(5)^2+x(6)^2;
r4=x(7)^2+x(8)^2;
if QLH~=0
QRF=B-QRH;
QLH=B;
else
if QLH==0
QRF=QRH;
QLH=QLF;
end
end
%1行
% QLF=0;%1
% QRH=0.25;%3
%
% QRF=0;%2
% QLH=0;%4
% th11=0;
th11=2*pi*(QLF-QLF);
R11 = [cos(th11), -sin(th11);
sin(th11), cos(th11)];
%th21=-pi;
th21=2*pi*(QLF-QRF);
R21 = [cos(th21), -sin(th21);
sin(th21), cos(th21)];
%th31=-pi/2;
th31=2*pi*(QLF-QRH);
R31 = [cos(th31), -sin(th31);
sin(th31), cos(th31)];
% th41=-3*pi/2;
th41=2*pi*(QLF-QLH);
R41 = [cos(th41), -sin(th41);
sin(th41), cos(th41)];
%2行
% th12=pi;
th12=2*pi*(QRF-QLF);
R12 = [cos(th12), -sin(th12);
sin(th12), cos(th12)];
% th22=0;
th22=2*pi*(QRF-QRF);
R22 = [cos(th22), -sin(th22);
sin(th22), cos(th22)];
% th32=pi/2;
th32=2*pi*(QRF-QRH);
R32 = [cos(th32), -sin(th32);
sin(th32), cos(th32)];
% th42=-pi/2;
th42=2*pi*(QRF-QLH);
R42 = [cos(th42), -sin(th42);
sin(th42), cos(th42)];
%3行
% th13=pi/2;
th13=2*pi*(QRH-QLF);
R13 = [cos(th13), -sin(th13);
sin(th13), cos(th13)];
% th23=-pi/2;
th23=2*pi*(QRH-QRF);
R23 = [cos(th23), -sin(th23);
sin(th23), cos(th23)];
% th33=0;
th33=2*pi*(QRH-QRH);
R33 = [cos(th33), -sin(th33);
sin(th33), cos(th33)];
% th43=-pi;
th43=2*pi*(QRH-QLH);
R43 = [cos(th43), -sin(th43);
sin(th43), cos(th43)];
%4行
% th14=3*pi/2;
th14=2*pi*(QLH-QLF);
R14 = [cos(th14), -sin(th14);
sin(th14), cos(th14)];
% th24=pi/2;
th24=2*pi*(QLH-QRF);
R24 = [cos(th24), -sin(th24);
sin(th24), cos(th24)];
% th34=pi;
th34=2*pi*(QLH-QRH);
R34 = [cos(th34), -sin(th34);
sin(th34), cos(th34)];
% th44=0;
th44=2*pi*(QLH-QLH);
R44 = [cos(th44), -sin(th44);
sin(th44), cos(th44)];
RR=[R11,R21,R31,R41;
R12,R22,R32,R42
R13,R23,R33,R43
R14,R24,R34,R44];
FF=[a*(u-r1)*x(1)-W1*x(2);
a*(u-r1)*x(2)+W1*x(1);
a*(u-r2)*x(3)-W2*x(4);
a*(u-r2)*x(4)+W2*x(3);
a*(u-r3)*x(5)-W3*x(6);
a*(u-r3)*x(6)+W3*x(5);
a*(u-r4)*x(7)-W4*x(8);
a*(u-r4)*x(8)+W4*x(7);
];
RR(abs(RR)-0<0.00001)=0;
temp=RR*x;
dydt=FF+RR*x;
matlab自带函数实现
使用ode45函数绘图
[t,y] = ode45(@f,[0 5],[0.1;0;0;0;0;0;0;0]);
%这里是膝关节处理
for i=1:length(y)
if y(i,2)<=0
y(i,2)=-1*sign(-1)*0.7067*y(i,2);
else
y(i,2)=0;
end
if y(i,4)<=0
y(i,4)=-1*sign(-1)*0.7067*y(i,4);
else
y(i,4)=0;
end
if y(i,6)<=0
y(i,6)=-1*sign(-1)*0.7067*y(i,6);
else
y(i,6)=0;
end
if y(i,8)<=0
y(i,8)=-1*sign(-1)*0.7067*y(i,8);
else
y(i,8)=0;
end
end
subplot(4,1,1)
plot(t,y(:,1),t,y(:,2));
subplot(4,1,2)
plot(t,y(:,3),t,y(:,4));
subplot(4,1,3)
plot(t,y(:,5),t,y(:,6));
subplot(4,1,4)
plot(t,y(:,7),t,y(:,8));
grid;
matlab手动欧拉法
out=[]%输出变量
count=1;
temp=f([],[0.1;0;0;0;0;0;0;0]);%后面参数为初值
y=zeros(8,1);
y=y+temp*0.0001;%0.0001为步长
for i=0:0.0001:5
temp=f([],y);
y=y+temp*0.0001;
%处理膝关节
if y(2)<=0
out(count,2)=-1*sign(-1)*0.7067*y(2);
else
out(count,2)=0;
end
if y(4)<=0
out(count,4)=-1*sign(-1)*0.7067*y(4);
else
out(count,4)=0;
end
if y(6)<=0
out(count,6)=-1*sign(-1)*0.7067*y(6);
else
out(count,6)=0;
end
if y(8)<=0
out(count,8)=-1*sign(-1)*0.7067*y(8);
else
out(count,8)=0;
end
out(count,1)=y(1);
%out(count,2)=y(2);
out(count,3)=y(3);
% out(count,4)=y(4);
out(count,5)=y(5);
% out(count,6)=y(6);
out(count,7)=y(7);
% out(count,8)=y(8);
count=count+1;
end
t=0:0.0001:5;
%绘图
subplot(4,1,1)
plot(t,out(:,1),t,out(:,2));
title('LF')
subplot(4,1,2)
plot(t,out(:,3),t,out(:,4));
title('RF')
subplot(4,1,3)
plot(t,out(:,5),t,out(:,6));
title('RH')
subplot(4,1,4)
plot(t,out(:,7),t,out(:,8));
title('LH')
grid;
效果:
ode45:
手动欧拉法
simulink: