MATLAB机器人工具箱（三）正逆运动学与轨迹规划

机器人工具箱中的SerialLink 类中有现成的函数：
SerialLink.fkine(theta)，可以直接对已经建立的机器人模型做运动学分析
SerialLink.ikine（T) 可以求逆运动学参数。

正/逆运动学

在上篇机器然建立的基础上，进行正运动学分析

theta=[0 0 -pi/2 pi/2 pi/2 0];
T=robot.fkine(theta)		%计算机器人正运动学，括号内为theta值
T1=transl(0.4,0.2,0)*trotx(pi);
T2=transl(0.4,0.2,0)*trotx(pi/2);
robot.plot(theta);%显示机器人图像，括号内为theta值
robot.plot([0 0 0 0 0 0]);
q1=robot.fkine([0 0 -pi/2 pi/2 pi/2 0]);%计算机器人正运动学
q7=robot.fkine([0 0 0 0 0 0]);
q2=(robot.ikine(T1)*180)/pi;%计算机器人逆运动学，关节坐标向量
q3=(robot.ikine(T2)*180)/pi;%计算机器人逆运动学，关节坐标向量
T25=robot.A([2 3 4 5],theta);%计算变换矩阵T25
T25=robot.A([2:5],theta);
cchain=robot.trchain;%转换为基本变换序列
q=robot.ikine(q1);%逆运动学
%q5=robot.ikine6s(q1,'ru');%逆运动学
j0=robot.jacob0(q);%雅可比矩阵

cchain=robot.trchain是一系列基本变换，用于描述串联机器人手臂的运动学。 字符串cchain包括多个形式为X（ARG）的令牌，其中X是Tx，Ty，Tz，Rx，Ry或Rz之一。 ARG是关节变量，角度或长度尺寸。

轨迹规划

轨迹是把机器人末端执行器平滑的从位姿A移动到位姿B，分为关节空间运动和笛卡尔空间运动
利用机器人工具箱提供的ctraj、jtraj、和trinterp函数可以实现笛卡尔规划、关节空间规划、变换插值
jtraj() 已知初始和终止的关节角度，利用五次多项式来规划轨迹
ctraj() 已知初始和终止的末端关节位姿，利用匀加速、匀减速运动来规划轨迹

关节空间/笛卡尔运动

考虑末端执行器在两个笛卡尔位姿之间移动

%% 轨迹

t=[0:2:50];%整个运动时间为2秒，采样间隔500微妙
g1=mtraj(@tpoly,q2,q3,t);%通过在q1和q2两个位形之间的平滑插值，得到一个关节空间轨迹
g2=mtraj(@lspb,q2,q3,t);%标量插补函数tpoly或lspb，多轴驱动函数mtraj
%直接运用jtraj函数
g3=jtraj(q2,q3,t);%相当于具有tpoly插值的mtraj，但是对多轴情况进行了优化，还允许使用额外参数设置初始和最终速度
[q,qd,qdd]=jtraj(q2,q3,t);%通过可选的输出参数，获得随时间变化的关节速度加速度向量
subplot(2,2,[1 1]);
robot.plot(q)%绘制动画
i=1:6;
subplot(2,2,2);
qplot(q(:,i));grid on;title('位置');%绘制每个关节位置
subplot(2,2,3);
qplot(qd(:,i));grid on;title('速度');%绘制每个关节速度
subplot(2,2,4);
qplot(qdd(:,i));grid on;title('加速度');%绘制每个关节加速度

%% 笛卡尔运动
Tc=ctraj(q1,q7,t);%输入参数为初始和最后位姿，以及时间步数，返回一个三维矩阵形式轨迹
Tjtraj=transl(Tc);%提取其位置分量
plot2(Tjtraj,'r');grid on;title('q1到q7直线轨迹');%绘制空间位置轨迹