11.3正逆运动学、轨迹规划

正/逆运动学、轨迹规划

正运动学

一

1SerialLink.fkine(theta); %对已经建立的机器人模型做

运动学分析

正运动学：给定关节坐标求末端执行器的位姿1

正运动学

一

Example 3：puma560

机器人正运动学

>> mdl_puma560 %加载puma560模型

>> qz%零角度

qz= 0 0 0 0 0 0

>> qr%就绪状态，机械臂伸直且垂直

qr=0 1.5708 -1.5708 0 0 0

>> qs%伸展状态，机械臂伸直且水平

qs= 0 0 -1.5708 0 0 0

>> qn%标准状态，机械臂处于灵巧工作状态

qn=0 0.7854 3.1416 0 0.7854 0

>> p560.plot(qn)

>> T=p560.fkine(qn)

T =

0 0 1 0.5963

0 1 0 -0.1501

-1 0 0 -0.01435

0 0 0 1

Puma560标准位形

1SerialLink.ikine6s(T，config); %逆运动学封闭解config:‘l’, ‘ r’左手或右手

逆

运动学

二

逆运动学：给定末端执行器的位姿求关节坐标2

‘u’,‘ d’ 肘部在上或在下

‘f’,‘ n’手腕翻转或不翻转

2 SerialLink.ikine(T); %逆运动学数值解，

奇异位形以及非6关节型

Example 3

：puma560机器人逆运动学

逆运动学

二

>> qul=p560.ikine6s(T) %肘关节在上左手位形

qul = 2.6486 -3.9270 0.0940 2.5326 0.9743 0.3734

>> qur=p560.ikine6s(T,‘ru’) %肘关节在上右手位形

qur= -0.0000 0.7854 3.1416 -0.0000 0.7854 0.0000

>> qdr=p560.ikine(T)%肘关节在下右手位形

qdr =0.0000 -0.8335 0.0940 0.0000 -0.8312 -0.0000

注意：逆解和原始的角度可能不相同。因为，机器人工具箱中的逆运动学函数并不精确，同时机器人通常有多组逆解，而ikine函数只能求出一组。

1关节空间：jtraj

已知初始和终止的关节角度，利用五次多项式来规划轨迹。

1 [q,qd,qdd] = jtraj(q0,qf,m)

轨迹规划

三

2笛卡尔空间：ctraj

已知初始和终止的末端关节位姿，利用匀加速、匀减速运动来规划轨迹。

1 Tc = ctraj(T0,T1,n)

Example 4：sawyer机器人轨迹规划

init_ang=[0,0,0,0,0,0,0];

targ_ang=[pi/4,-pi/3,pi/5,pi/2,-pi/4,pi/2,pi/3];

step=200;

[q,qd,qdd]=jtraj(init_ang,targ_ang,step);

轨迹规划

三

T0=robot.fkine(init_ang);

Tf=robot.fkine(targ_ang);

subplot(2,4,3);i=1:7;plot(q(:,i));title('位置');grid on;

subplot(2,4,4);i=1:7;plot(qd(:,i));title('速度');grid on;

subplot(2,4,7);i=1:7;plot(qdd(:,i));title('加速度');grid on;

Example 4：sawyer机器人轨迹规划

Tc=ctraj(T0,Tf,step);

Tjtraj=transl(Tc);

subplot(2,4,8);plot2(Tjtraj,'r’);

title('p1p2');grid on;

轨迹规划

三

到直线轨迹

subplot(2,4,[1,2,5,6]);

plot3(Tjtraj(:,1),Tjtraj(:,2),Tjtraj(:,3),'b');grid on;

hold on;

qq=robot.ikine(Tc)

robot.plot(qq);

Example 4：

sawyer

机器人轨迹规划

轨迹规划

三