机器人工具箱实战 matlab机器人工具箱

目的： 【机器人工具箱实战】取一条腿为研究对象，对单腿的原地踏步进行模拟与仿真，给出运动轨迹，并得到相应的关节角的运动参数
后续想法->编写函数，输入轨迹，输出运动参数：
过程与实现：创建单腿模型

L(1)=Link([0,0,0,pi/2]); L(2)=Link([0,0,1,0]); L(3)=Link([0,0,1,0]); L(4)=Link([0,0,0,pi/2]); L(5)=Link([0,0,0,0]); L(6)=Link([0,0,0,0]); two_link=SerialLink(L,'name','twolink');

其中1456是为了使用内置函数而设置的冗余自由度，对于该模拟无影响
期望运动轨迹：位姿状态T1：

文章图片

位姿状态T2：

文章图片

键入初始和结束关节角并得到变化矩阵：

qn=[0,-pi/4,-pi/2,0,0,0]; q1=[0,-70/180*pi,-40/180*pi,0,0,0]; T1=two_link.fkine(qn); T2=two_link.fkine(q1);

设置末端执行器从T1状态移动到T2状态，用时1s，每100ms计算一次关节角

t=[0:0.1:1];

计算：

[q v a]=jtraj(qn,q1,t); %%该函数默认将始末位置的速度加速度取0或趋于0

q= 0 -0.785398163397448 -1.57079632679490 0 0 0 0 -0.789133167996716 -1.56332631759636 0 0 0 0 -0.810670530966326 -1.52025159165714 0 0 0 0 -0.856555237001257 -1.42848217958728 0 0 0 0 -0.923907492835718 -1.29377766791836 0 0 0 0 -1.00356431989674 -1.13446401379631 0 0 0 0 -1.08322114695776 -0.975150359674272 0 0 0 0 -1.15057340279222 -0.840445848005349 0 0 0 0 -1.19645810882715 -0.748676435935486 0 0 0 0 -1.21799547179676 -0.705601709996267 0 0 0 0 -1.22173047639603 -0.698131700797730 0 0 0

令s1为关节1(q中的第二列)所记录的关节角矩阵,同理得s2：

s1=q(:,2)'; s2=q(:,3)';

三次多形式拟合

p1=polyfit(t,s1,3); p2=polyfit(t,s2,3);

f1=polyval(p1,t); f2=polyval(p2,t); plot(t,s1,'o',t,f1,t,s2,'x',t,f2) ;

得到曲线：

文章图片

p1 =1.3526-2.02890.2463-0.7885p2 =-2.70534.0579-0.4925-1.5645

我们可以得到关节1和关节2的三次多项式：
θ 1 = 1.3526 t 3 ? 2.0289 t 2 + 0.2463 t ? 0.7885 \theta_1=1.3526t^3-2.0289t^2+0.2463t-0.7885 θ1?=1.3526t3?2.0289t2+0.2463t?0.7885
θ 2 = ? 2.7053 t 3 + 4.0579 t 2 ? 0.4925 t ? 1.5645 \theta_2=-2.7053t^3+4.0579t^2-0.4925t-1.5645 θ2?=?2.7053t3+4.0579t2?0.4925t?1.5645
以上是比较特殊的情况，接下来将讨论更为一般的情况
实际上只要能给出末端执行器的位姿随时间的变化，我们就能得到各关节的运动参数