tingzhi_guihua.zip_returnlhp_三次样条插值；双腿停止步态

%启动阶段的规划 %先规划髋关节和踝关节的位置 clc; clear all; % close all; t0=0; t1=0.4; %单脚支撑时间一半 t2=0.8; %单脚支撑时间 t3=0.8; %周期时间 t5=0.4; %周期时间一半 t=0:0.01:t3; t4=(t1+t3)/2; thtas=10*pi/180; %手臂摆动角规划正 A=[1,t0,t0^2,t0^3,t0^4,t0^5,t0^6;0,1,2*t0,3*t0^2,4*t0^3,5*t0^4,6*t0^5;0,0,2,6*t0,12*t0^2,20*t0^3,30*t0^4;... 1,t5,t5^2,t5^3,t5^4,t5^5,t5^6;1,t3,t3^2,t3^3,t3^4,t3^5,t3^6;... 0,1,2*t3,3*t3^2,4*t3^3,5*t3^4,6*t3^5;0,0,2,6*t3,12*t3^2,20*t3^3,30*t3^4]; B=[thtas;0;0;thtas/2;0;0;0]; x_s=inv(A)*B for n=1:1:100*t3+1 s=(n-1)/100; y_s(n)=x_s(1)+x_s(2)*s+x_s(3)*s.^2+x_s(4).*s.^3+x_s(5)*s.^4+x_s(6)*s.^5+x_s(7)*s.^6; y_sf(n)=-y_s(n); end figure(19); plot(t,y_s); %侧向摆动角规划 A=[1,t1,t1^2,t1^3,t1^4,t1^5,t1^6;0,1,2*t1,3*t1^2,4*t1^3,5*t1^4,6*t1^5;0,0,2,6*t1,12*t1^2,20*t1^3,30*t1^4;... 1,t4,t4^2,t4^3,t4^4,t4^5,t4^6;1,t3,t3^2,t3^3,t3^4,t3^5,t3^6;... 0,1,2*t3,3*t3^2,4*t3^3,5*t3^4,6*t3^5;0,0,2,6*t3,12*t3^2,20*t3^3,30*t3^4]; B=[thtas;0;0;thtas/2;0;0;0]; x_c=inv(A)*B for n=1:1:100*t1+1 s=(n-1)/100; y_c(n)=thtas; end for n=100*t1+1:1:100*t3+1 s=(n-1)/100; y_c(n)=x_c(1)+x_c(2)*s+x_c(3)*s.^2+x_c(4).*s.^3+x_c(5)*s.^4+x_c(6)*s.^5+x_c(7)*s.^6; end figure(9); plot(t,y_c); %计算摆动腿踝关节X方向 A=[1,0,0,0,0,0,0;0,1,0,0,0,0,0;0,0,2,0,0,0,0;1,t1,t1^2,t1^3,t1^4,t1^5,t1^6;1,t2,t2^2,t2^3,t2^4,t2^5,t2^6;... 0,1,2*t2,3*t2^2,4*t2^3,5*t2^4,6*t2^5;0,0,2,6*t2,12*t2^2,20*t2^3,30*t2^4]; B=[-35;0;0;-17.5;0;0;0]; x_h=inv(A)*B for n=1:1:100*t2+1 s=(n-1)/100; y_h(n)=x_h(1)+x_h(2)*s+x_h(3)*s.^2+x_h(4).*s.^3+x_h(5)*s.^4+x_h(6)*s.^5+x_h(7)*s.^6; end for n=100*t2+1:1:100*t3+1 s=(n-1)/100; y_h(n)=0; end figure(1); plot(t,y_h); %计算摆动腿踝关节Z方向 A=[1,0,0,0,0,0;0,1,0,0,0,0;1,t1,t1^2,t1^3,t1^4,t1^5;0,1,2*t1,3*t1^2,4*t1^3,5*t1^4;1,t2,t2^2,t2^3,t2^4,t2^5;... 0,1,2*t2,3*t2^2,4*t2^3,5*t2^4]; B=[52;0;62;0;52;0]; x_h_z=inv(A)*B for n=1:1:100*t2+1 s=(n-1)/100; y_h_z(n)=x_h_z(1)+x_h_z(2)*s+x_h_z(3)*s.^2+x_h_z(4).*s.^3+x_h_z(5)*s.^4+x_h_z(6)*s.^5; end for n=100*t2+1:1:100*t3+1 s=(n-1)/100; y_h_z(n)=52; end figure(2); plot(t,y_h_z); %计算髋关节X方向2（初始速度为0） A=[1,0,0,0,0;0,1,0,0,0;1,t5,t5^2,t5^3,t5^4;1,t3,t3^2,t3^3,t3^4;... 0,1,2*t3,3*t3^2,4*t3^3]; B=[-5;0;-2.5;0;0]; x_k_x=inv(A)*B for n=1:1:100*t3+1 s=(n-1)/100; y_k_x(n)=x_k_x(1)+x_k_x(2)*s+x_k_x(3)*s.^2+x_k_x(4).*s.^3+x_k_x(5)*s.^4; end figure(3); plot(t,y_k_x); %开始进行逆运算 x1=0; z1=52; l1=52; l2=50; l3=63; zh=160; xh=y_k_x; xr=y_h; zr=y_h_z; thta1=acos(((zh-z1).^2+(xh-x1).^2+l2.^2-l3.^2)./(2*l2*sqrt((zh-z1).^2+(xh-x1).^2)))+... atan((xh-x1)./(zh-z1)); figure(6); plot(t,thta1*180/pi,'r'); hold on; thta2=-acos(((zh-z1).^2+(xh-x1).^2+l3.^2-l2.^2)./(2*l3*sqrt((zh-z1).^2+(xh-x1).^2)))+atan((xh-x1)./(zh-z1)); % figure(5); plot(t,thta2*180/pi,'b'); thta4=-(acos(((zh-zr).^2+(xh-xr).^2+l3^2-l2^2)./(2*l3*sqrt((zh-zr).^2+(xh-xr).^2)))+atan((xr-xh)./(zh-zr))); thta5=-(-acos(((zh-zr).^2+(xh-xr).^2+l2^2-l3^2)./(2*l2*sqrt((zh-zr).^2+(xh-xr).^2)))+atan((xr-xh)./(zh-zr))); % figure(6); plot(t,thta4*180/pi,'g'); %figure(7); plot(t,thta5*180/pi,'k'); th1=thta1*180/pi; th2=thta2*180/pi; th4=-thta4*180/pi; th5=-thta5*180/pi; tsts=0:0.01:5.07;