基於HOPF振盪器的CPG單元模型matlab實現

代碼爲實現仿生四足機器人技術（羅慶生等）中關於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:

手動歐拉法