四足机器人步态研究与控制步态算法生成 matlab下建立hopf振荡器数学模型matlab下walk trot步态算法生成

%Ezekiel %2018年4月22日 %%% edited by Ezekiel /2017/12/14 % 参考仿生四足机器人技术&Min程序 % 振荡器模型 %四足机器人CPG控制网络二四足 %8.29改进：用循环代替大部分数组赋值 %8.30改进：增加足端轨迹 %基本概念 %Alpha收敛速度，Beta占空比,Wsw摆动相频率，Wst支撑相频率 clc; clear; %CPG构建基本参数 Alpha=10;%收敛速度%10 leg_num=4;%腿的数量 gait=1;%步态选择，1walk,2trot,3pace,4gallop u=1;%影响曲线幅值，幅值为开根号 a=50;%50;%决定w在Wsw和Wst之间变化的速度 Psa=1;%关节形式，膝式-1，肘式1 Wsw=2*pi;%摆动相频率,影响信号周期，T为2*pi/Wsw u1=0; u2=0;%误差，影响x,y的平衡位置 %关节参数，此处参数需实测，试验数据参考书 h=0.02; %抬腿高度 v=1; %行走速度 T=1; %步态周期%0.4 S=v*T; %步长 l=0.4; %腿节长度 theta0=30/180*pi;%髋关节和膝关节平衡位置与垂直线夹角 L=2*l*cos(theta0);%髋关节与足端之间长度 Ah=15;%asin((Beta*S)/(2*L));%髋关节摆动幅度 Ak=10;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度 %负载系数对相关参数影响 switch gait case 1 Beta=0.5;%0.75;%负载系数,walk为0.75,trot,pace,gallop为0.5，影响振荡信号上升时间和下降时间 Ah=asin((Beta*S)/(2*L));%7.5;%髋关节摆动幅度 Ak=acos((l*cos(theta0)-h)/l)-theta0;%5.3;%膝关节摆动幅度 case 2 Beta=0.5; Ah=asin((Beta*S)/(2*L));%髋关节摆动幅度 Ak=acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度 case 3 Beta=0.5; Ah=10;%asin((Beta*S)/(2*L));%髋关节摆动幅度 Ak=5.3;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度 case 4 Beta=0.5; Ah=13.4;%asin((Beta*S)/(2*L));%髋关节摆动幅度 Ak=10;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度 end Wst=((1-Beta)/Beta)*Wsw;%支撑相频率 Rhk=(1-Beta)*[cos(pi) -sin(pi);sin(pi) cos(pi)];%髋关节和膝关节之间的耦合矩阵 Ah=15;%asin((Beta*S)/(2*L));%髋关节摆动幅度 Ak=10;%acos((l*cos(theta0)-h)/l)-theta0;%膝关节摆动幅度 %时间 Pon_num=2000; t_begin=0; t_end=20; t_step=(t_end-t_begin)/(Pon_num-1); t=zeros(Pon_num,1); for n=1:Pon_num t(n)=(n-1)*t_step+t_begin; end %初始值，非0即可 leg_x=zeros(leg_num,1); leg_y=zeros(leg_num,1); for i=1:leg_num leg_x(i)=0.5; leg_y(i)=0.5; end %CPG曲线，左前1，右前2，右后3，左后4 leg_h_Point_x=zeros(Pon_num,leg_num);%髋关节 leg_h_Point_y=zeros(Pon_num,leg_num); leg_k_Point_x=zeros(Pon_num,leg_num);%膝关节 leg_k_Point_y=zeros(Pon_num,leg_num); Foot_end_x=zeros(Pon_num,leg_num);%足端 Foot_end_y=zeros(Pon_num,leg_num); Phi=zeros(leg_num,1); switch gait case 1 %walk相位 Phi(1)=0*2*pi; %Phi_LF Phi(2)=0.5*2*pi; %Phi_RF Phi(3)=0.25*2*pi; %Phi_RH Phi(4)=0.75*2*pi; %Phi_LH case 2 %trot相位矩阵 Phi(1)=0*2*pi; Phi(2)=0.5*2*pi; Phi(3)=0*2*pi; Phi(4)=0.5*2*pi; case 3 %pace相位矩阵 Phi(1)=0*2*pi; Phi(2)=0.5*2*pi; Phi(3)=0.5*2*pi; Phi(4)=0*2*pi; case 4 %gallop相位矩阵 Phi(1)=0*2*pi; Phi(2)=0*2*pi; Phi(3)=0.5*2*pi; Phi(4)=0.5*2*pi; end %相对相位矩阵 R_cell={leg_num,leg_num}; for i=1:leg_num for j=1:leg_num R_cell{j,i}=[cos(Phi(i)-Phi(j)) -sin(Phi(i)-Phi(j));sin(Phi(i)-Phi(j)) cos(Phi(i)-Phi(j))]; end end for n=1:Pon_num for i=1:leg_num r_seq=(leg_x(i)-u1)^2+(leg_y(i)-u2)^2; W=(Wst/(exp(-a*leg_y(i))+1))+(Wsw/(exp(a*leg_y(i))+1)); V=[Alpha*(u-r_seq) -W;W Alpha*(u-r_seq)]*[leg_x(i)-u1;leg_y(i)-u2]+R_cell{1,i}*[leg_x(1)-u1;leg_y(1)-u1]+R_cell{2,i}*[leg_x(2)-u2;leg_y(2)-u2]+R_cell{3,i}*[leg_x(3)-u1;leg_y(3)-u2]+R_cell{4,i}*[leg_x(4)-u1;leg_y(4)-u2]; leg_x(i)=leg_x(i)+V(1,1)*t_step; leg_y(i)=leg_y(i)+V(2,1)*t_step; leg_h_Point_x(n,i)=leg_x(i); leg_h_Point_y(n,i)=leg_y(i); if leg_y(i)>0 leg_k_Point_x(n,i)=0; else if(i<3) leg_k_Point_x(n,i)=-sign(Psa)*(Ak/Ah)*leg_y(i); else leg_k_Point_x(n,i)=sign(Psa)*(Ak/Ah)*leg_y(i); end end Foot_end_x(n,i)=sin(leg_k_Point_x(n,i)/sqrt(u)*Ak*(pi/180)+theta0)*l-sin(theta0+leg_h_Point_x(n,i)/sqrt(u)*Ah*(pi/180))*l; Foot_end_y(n,i)=L-(cos(leg_h_Point_x(n,i)/sqrt(u)*Ah*(pi/180)+theta0)*l+cos(leg_k_Point_x(n,i)/sqrt(u)*Ak*(pi/180)+theta0)*l); end end % % 画图 % figure(1) % for i=1:4 % subplot(4,3,i+2*(i-1)) % plot(leg_h_Point_x(:,i),leg_h_Point_y(:,i)) % subplot(4,3,i+1+2*(i-1)) % hold on % plot(t,leg_h_Point_x(:,i)) % plot(t,leg_k_Point_x(:,i)) % hold off % end figure(1) subplot(4,1,1) %创建四行一列，在从左到右的第一个上面创建坐标系，并在上面作图 plot(t,leg_h_Point_x(:,1)); hold on plot(t,leg_k_Point_x(:,1),'green'); grid on ylabel('LF') axis([5,15,-1.5,1.5])%XY坐标均衡 %title('Hopf振荡器的输出'); grid on; subplot(4,1,2) plot(t,leg_h_Point_x(:,2)); hold on plot(t,leg_k_Point_x(:,2),'green'); grid on ylabel('RF') axis([5,15,-1.5,1.5])%XY坐标均衡 subplot(4,1,3) plot(t,leg_h_Point_x(:,3)); hold on plot(t,leg_k_Point_x(:,3),'green'); grid on ylabel('RB') axis([5,15,-1.5,1.5])%XY坐标均衡 subplot(4,1,4) plot(t,leg_h_Point_x(:,4)); hold on plot(t,leg_k_Point_x(:,4),'green'); grid on ylabel('LB') xlabel('时间t/s') axis([5,15,-1.5,1.5])%XY坐标均衡 % subplot(4,3,3) % plot(Foot_end_x(:,3), Foot_end_y(:,3)) % subplot(4,3,6) % for i=1:leg_num % hold on % plot(t,leg_h_Point_x(:,i)) % hold off % end % % subplot(4,3,9) % for i=1:leg_num % hold on % plot(t,leg_h_Point_y(:,i)) % hold off % end附录