本篇文章主要介绍详细介绍如何使用MATLAB机器人工具箱进行机械臂的正逆运动学求解以及轨迹规划，全文附完整的MATLAB程序，本文主要分六部分进行介绍：
一、使用改进型DH法建立机械臂模型，
二、正运动学求解，
三、逆运动学求解，
四、轨迹规划，
五、将轨迹生成视频，
六、完整的程序
一、使用改进型DH法建立机械臂模型
本篇文章以如下的GLUON-6L3型机械臂为例进行介绍 各轴的工作范围如下所示：
根据上面给出的带尺寸标注的所示姿态利用课本中所介绍的方法建立坐标系，
如下所示：
根据上面建立的坐标系写出DH表，并在matlab中使用Link函数和Serial Link函数构建机械臂模型，程序、模型及DH表如下：
L(1)=Link([0,101.5,0,0],'modified');
L(2)=Link([-pi/2,0,0,-pi/2],'modified');
L(3)=Link([0,0,173,0],'modified');
L(4)=Link([pi/2,79.2,173,0],'modified');
L(5)=Link([0,79.2,0,pi/2],'modified');
L(6)=Link([0,41.7,0,-pi/2],'modified');
myrobot= SerialLink(L, 'name', 'sixlink');
myrobot.teach
myrobot.display
我们发现其构造与我们想要的差距较大，因此我们不得不放弃课本中所介绍的那种为了让更多的参数为0，所采用的坐标系原点选取规则
于是经过几次尝试，对坐标轴原点的位置进行了移动，重新建立了如下的坐标系：
根据上面建立的坐标系写出DH表，并在matlab中使用Link函数和Serial Link函数构建机械臂模型，程序、模型及DH表如下：
% DH建模部分 设定各轴参数，参数顺序为theta、d、a、alpha
L(1)=Link([0,101.5,0,0],'modified');
L(2)=Link([-pi/2,79.2,0,-pi/2],'modified');
L(3)=Link([0,-79.2,173,0],'modified');
L(4)=Link([pi/2,79.2,173,0],'modified');
L(5)=Link([0,79.2,0,pi/2],'modified');
L(6)=Link([0,41.7,0,-pi/2],'modified');
%建立机械臂模型
myrobot= SerialLink(L, 'name', 'sixlink');
%显示建立的机器人模型
%myrobot.plot([0 0 0 0 0 0])
myrobot.teach
%打印出机器人模型的信息
myrobot.display
我们可以发现重新建立了坐标系后，所建立的机械臂模型的构造与我们所期望的较为相似，但是我们发现其初始姿态和视野与我们期望的有所不同，而且机器臂模型下面拖着一个长杆 下面我们依次来解决上面的三个问题
首先我们对Link函数里面的offset参数进行设定，对L（2）和L（4）进行设定关节变量偏移量的设定，来使模型的初始姿态与我们设想的一样
程序如下：
%定关节变量偏移量
L(2).offset=-pi/2;
L(4).offset=pi/2;
经过此步设定后的模型如下：
下面我们对视野以及坐标轴的范围进行设定，来获取我们期望的视野，并将模型下的长杆去掉，程序如下：
%调整坐标轴及视野
set(gca,'XLim',[-400,400]); %将X轴范围设定为[-400,400]
set(gca,'YLim',[-500,500]); %将X轴范围设定为[-500,500]
set(gca,'ZLim',[0,600]); %将Z轴范围设定为[0,600]，最小值设定为0，可以消除模型下面的长杆
set(gca,'XDir','reverse'); %将x轴方向设置为反向
set(gca,'YDir','reverse'); %将Y轴方向设置为反向
set(gca,'View',[-85,10]); %设定视野方向角和俯仰角
经过上面的设定后，我们得到的机械臂模型如下，已经跟我们所期望的模型几乎一样了
已经基本达到了我们的预期目标，在任务要求里对各关节变量的范围进行了限定，接下来对其进行设定，程序如下：
%设定关节变量范围
L(1).qlim=[deg2rad(-140) deg2rad(140)];
L(2).qlim=[deg2rad(-90) deg2rad(90)];
L(3).qlim=[deg2rad(-140) deg2rad(140)];
L(4).qlim=[deg2rad(-140) deg2rad(140)];
L(5).qlim=[deg2rad(-140) deg2rad(140)];
L(6).qlim=[deg2rad(-360) deg2rad(360)];
至此，机械臂建模部分就算是完成了
二、正运动学求解
这里我采用了两组关节变量，即theta_0=[0,0,0,0,0,0]，theta_1=[pi/4,0,pi/3,-pi/3,0,0]，我们使用fkine（）函数求其齐次变换矩阵T0和T1，然后使用plot函数绘制出其姿态，程序如下所示：
%正运动学求解
theta_0=[0,0,0,0,0,0];
theta_1=[pi/4,0,pi/3,-pi/3,0,0];
T0=myrobot.fkine(theta_0)
T1=myrobot.fkine(theta_1)
figure(2)
myrobot.plot(theta_0)
View_Set()
myrobot.plot(theta_1) %可在此处设置一断点
View_Set()
齐次变换矩阵T0和T1如下所示：
T0 =
1 0 0 0
0 0 1 120.9
0 -1 0 526.7
0 0 0 1
T1 =
0.7071 0 -0.7071 20.45
0.7071 0 0.7071 191.4
0 -1 0 440.2
0 0 0 1
其姿态如下所示：
上面程序中的View_Set()是我将前面介绍的对figure的视野及坐标轴的设定的程序封装成的函数方便调用，View_Set.m文件中的内容如下：
function [] = View_Set()
%调整坐标轴及视野
set(gca,'XLim',[-400,400]); %将X轴范围设定为[-400,400]
set(gca,'YLim',[-500,500]); %将X轴范围设定为[-500,500]
set(gca,'ZLim',[0,600]); %将Z轴范围设定为[0,600]，最小值设定为0，可以消除模型下面的长杆
set(gca,'XDir','reverse'); %将x轴方向设置为反向
set(gca,'YDir','reverse'); %将Y轴方向设置为反向
set(gca,'View',[-85,10]); %设定视野方向角和俯仰角
end三、逆运动学求解
我们发现在使用ikine6s函数求逆运动学的解析解/封闭解时发现会报错，下面我们用上面得到的两个其次变换矩阵T0、T1来分别用ikine函数求其数值解（注意，并不是所有的姿态都可以求其数值解）
程序如下：
%逆运动学求解
theta_0_0=myrobot.ikine(T0) %数值解
theta_1_1=myrobot.ikine(T1)
其结果如下：
theta_0_0 = 0 0 0 0 0 0
theta_1_1 = 0.7854 0.6568 -0.0590 -1.3339 0.0000 0.7361
经过对比我们发现第二组数据theta_1_1与我们之前正运动学设定的关节变量theta_1并不相同
theta_1 = 0.7854 0 1.0472 -1.0472 0 0
我们分别使用plot函数绘制出其姿态进行验证，并借助teach进行观察
通过上面的对比观察我们得知，虽然两组数据绘制出的机械臂的姿态不相同，但是其末端关节相对于基坐标系的姿态是相同的 即X=0，Y=120.900 Z=526.7
四、轨迹规划
我初步的设想是再创建一个参数相同的机械臂，然后采用不同的轨迹规划方案就行对比，于是我将之前建模部分的程序封装成一个函数，取名为Build_Robot()，该函数设置了一个输入参数Robot_name，也就是我们创建的机械臂的名字，Build_Robot.m文件中代码如下：
function [] = Build_Robot(Robot_name)
L(1)=Link([0,101.5,0,0],'modified');
L(2)=Link([0,79.2,0,-pi/2],'modified');
L(3)=Link([0,-79.2,173,0],'modified');
L(4)=Link([0,79.2,173,0],'modified');
L(5)=Link([0,79.2,0,pi/2],'modified');
L(6)=Link([0,41.7,0,-pi/2],'modified');
L(2).offset=-pi/2;
L(4).offset=pi/2;
L(1).qlim=[deg2rad(-140) deg2rad(140)];
L(2).qlim=[deg2rad(-90) deg2rad(90)];
L(3).qlim=[deg2rad(-140) deg2rad(140)];
L(4).qlim=[deg2rad(-140) deg2rad(140)];
L(5).qlim=[deg2rad(-140) deg2rad(140)];
L(6).qlim=[deg2rad(-360) deg2rad(360)];
Robot_name= SerialLink(L, 'name', 'sixlink');
%Robot_name.plot([0 0 0 0 0 0])
%Robot_name.teach
Robot_name.display
end但是后续的尝试，不得不使我放弃了这一方案，因为工具箱同一时间只支持对一个机械臂进行轨迹规划图像的展示，否则会报错，于是我纠正方案，先显示使用五次多项式tpoly函数的规划动画，再显示带抛物线过度的直线轨迹规划lspb函数的规划动画，在图像的右侧依次绘制出其角度，速度，加速度随时间（取样点）变化的曲线，上面三个是采用tpoly函数规划的，下面三个是lspb函数规划的，初始姿态和结束的姿态分别采用了之前设定的两个姿态
程序如下：
%轨迹规划部分
%首先我们先规划之前得到的两个位姿之间的轨迹
t=[0:0.05:4]; %设定步数
p=mtraj(@tpoly, theta_0_0, theta_1_1, 100);
p1=mtraj(@lspb, theta_0_0, theta_1_1, 100);
[q,dq,ddq]=mtraj(@tpoly, theta_0_0, theta_1_1, 100);
[q2,dq2,ddq2]=mtraj(@lspb, theta_0_0, theta_1_1, 100);
subplot(6,3,[1 4 7 2 5 8 10 13 16 11 14 17])
myrobot.plot([0 0 0 0 0 0])
View_Set()
myrobot.plot(p)
myrobot.plot(p1)
%绘制角度，速度，加速度曲线
subplot(6,3,3)
plot(q)
subplot(6,3,6)
plot(dq)
subplot(6,3,9)
plot(ddq)
subplot(6,3,12)
plot(q2)
subplot(6,3,15)
plot(dq2)
subplot(6,3,18)
plot(ddq2)
其图像如下：
曲线局部放大图：
五、将轨迹生成视频
为了方便观察，我们将以上轨迹生成视频，生成了名为myrobot.avi的视频文件，程序如下：
%将轨迹创建生成视频
out=VideoWriter('myrobot.avi');
out.FrameRate=10; %设定每秒10帧
open(out);
myrobot.plot([0 0 0 0 0 0])
View_Set()
for K=1:100
myrobot.plot(p(K,:))
F=getframe(gcf);
writeVideo(out,F);
end
close(out);视频如下：myrobot
六、完整的程序
% DH建模部分 设定各轴参数，参数顺序为theta、d、a、alpha
L(1)=Link([0,101.5,0,0],'modified');
L(2)=Link([-pi/2,79.2,0,-pi/2],'modified');
L(3)=Link([0,-79.2,173,0],'modified');
L(4)=Link([pi/2,79.2,173,0],'modified');
L(5)=Link([0,79.2,0,pi/2],'modified');
L(6)=Link([0,41.7,0,-pi/2],'modified');
%设定关节变量偏移量
L(2).offset=-pi/2;
L(4).offset=pi/2;
%建立机械臂模型
myrobot= SerialLink(L, 'name', 'sixlink');
%显示建立的机器人模型
%myrobot.plot([0 0 0 0 0 0])
myrobot.teach
%打印出机器人模型的信息
myrobot.display
%调整坐标轴及视野
set(gca,'XLim',[-400,400]); %将X轴范围设定为[-400,400]
set(gca,'YLim',[-500,500]); %将X轴范围设定为[-500,500]
set(gca,'ZLim',[0,600]); %将Z轴范围设定为[0,600]，最小值设定为0，可以消除模型下面的长杆
set(gca,'XDir','reverse'); %将x轴方向设置为反向
set(gca,'YDir','reverse'); %将Y轴方向设置为反向
set(gca,'View',[-85,10]); %设定视野方向角和俯仰角
%设定关节变量范围
L(1).qlim=[deg2rad(-140) deg2rad(140)];
L(2).qlim=[deg2rad(-90) deg2rad(90)];
L(3).qlim=[deg2rad(-140) deg2rad(140)];
L(4).qlim=[deg2rad(-140) deg2rad(140)];
L(5).qlim=[deg2rad(-140) deg2rad(140)];
L(6).qlim=[deg2rad(-360) deg2rad(360)];
%正运动学求解
theta_0=[0,0,0,0,0,0];
theta_1=[pi/4,0,pi/3,-pi/3,0,0];
T0=myrobot.fkine(theta_0)
T1=myrobot.fkine(theta_1)
% figure(2)
% myrobot.plot(theta_0)
% View_Set()
% myrobot.plot(theta_1) %可在此处设置一断点进行观察
% View_Set()
%逆运动学求解
theta_0_0=myrobot.ikine(T0) %数值解theta_1_1=myrobot.ikine(T1)
% myrobot.teach
%myrobot.plot(theta_1)
% View_Set()
% myrobot.plot(theta_1_1) %可在此处设置一断点进行观察% View_Set()
%创建一个跟之前相同的机械臂，取名为myrobot2
syms myrobot2
Build_Robot(myrobot2)
%轨迹规划部分
%首先我们先规划之前得到的两个位姿之间的轨迹
t=[0:0.05:4]; %设定步数
p=mtraj(@tpoly, theta_0_0, theta_1_1, 100);
p1=mtraj(@lspb, theta_0_0, theta_1_1, 100);
[q,dq,ddq]=mtraj(@tpoly, theta_0_0, theta_1_1, 100);
[q2,dq2,ddq2]=mtraj(@lspb, theta_0_0, theta_1_1, 100);
subplot(6,3,[1 4 7 2 5 8 10 13 16 11 14 17])
myrobot.plot([0 0 0 0 0 0])
View_Set()
myrobot.plot(p)
myrobot.plot(p1)
subplot(6,3,3)
plot(q)
subplot(6,3,6)
plot(dq)
subplot(6,3,9)
plot(ddq)
subplot(6,3,12)
plot(q2)
subplot(6,3,15)
plot(dq2)
subplot(6,3,18)
plot(ddq2)
%将之前绘制的图像关掉，不想关掉的话注释掉下一行程序
close all
%将轨迹创建生成视频
out=VideoWriter('myrobot.avi');
out.FrameRate=10;
open(out);
myrobot.plot([0 0 0 0 0 0])
View_Set()
for K=1:100
myrobot.plot(p(K,:))
F=getframe(gcf);
writeVideo(out,F);
end
close(out);
