【Matlab 六自由度机器人】运动学正解（附MATLAB机器人正解完整代码）

用户9613193

发布于 2026-06-16 20:26:52

160

【Matlab 六自由度机器人】求运动学正解

往期回顾
前言
正文
- 一、运动学正解
- - 1. 齐次变换矩阵
  - 2. 总变换
- 二、代码实现
- - 1. 定义各连杆参数
  - 2. 齐次变换矩阵及总变换
  - 3. 代码运行结果
总结
参考文献

往期回顾

【汇总】

相关资源：

\fcolorbox{green}{aqua}{【Matlab 六自由度机器人】系列文章汇总 }

【主线】

【补充说明】

前言

本篇介绍机器人运动学正解的有关问题，介绍了如何理解正向运动学，并利用D-H参数求解机器人运动学正解。

以下是本篇文章正文内容，包含对正解的含义的理解和代码的分步解析。

正文

一、运动学正解

定义：已知各关节的运动参数，求末端执行器的相对参考坐标系的位姿。

求解步骤：

各连杆首尾相连；
确定各连杆间的齐次变换矩阵；
得到最后的总变换矩阵。

且该总变换矩阵内的未知数只有各轴的旋转角度，因此得到旋转角度即可得到六自由度机器人的末端笛卡尔空间坐标。

1. 齐次变换矩阵

在上一篇的Matlab建立六自由度机器人模型中详细解释了如何定义机器人的DH参数、如何设置

z_{i}

轴等等，在此只叙述部分内容。对DH参数的符号约定

DH约定参数	符号约定
θ θ θ	joint angle 关节转角
d d d	link offset 连杆偏移
a a a	link length 连杆长度
α ( a l p h a ) α(alpha) α(alpha)	link twist 连杆扭角
制定DH参数的程序规则

joint angle 关节转角

link offset 连杆偏移

link length 连杆长度

α(alpha)

link twist 连杆扭角制定DH参数的程序规则

参数

是轴

z_{0}

和轴

z_{1}

之间沿轴线

x_{1}

测得的距离；

角度

是在垂直于

x_{1}

平面内测得的轴线

z_{0}

和

z_{1}

之间的夹角。角度

的正向取值定义为从

z_{0}

到

z_{1}

，由右手规则来确定；

参数

为从原点

o_{0}

到轴线

x_{1}

与

z_{0}

交点之间的距离，该距离沿

z_{0}

轴线进行测量得到；

是在垂直于

z_{0}

的平面内测得的从

x_{0}

到

x_{1}

的角度。

理解了上述的符号约定及程序规则后，在此基础上，每个齐次变换矩阵

T_{i}

都可以表示为是个基本矩阵的乘积，

对于标准型D-H参数 其乘积顺序如下：

^{i-1}T_i =Rot(z_,θ_i)×Trans(z，d_i)×Trans(x_，a_i)×Rot(x，α_i)

通用齐次变换矩阵如下：

^{i-1}T_i =

\left[ \begin{matrix} cosθ_i&-sinθ_icosα_i&sinθ_isinα_i&a_{i}cosθ_i \\ sinθ_i&cosθ_icosα_i&-cosθ_isinα_i&a_{i}sinθ_i\\ 0&sinα_i&cosα_i&d_{i} \\ 0&0&0&1 \end{matrix} \right]

对于改进型D-H参数 其乘积顺序如下：

^{i-1}T_i =Rot(x_{i}，α_{i-1})×Trans(x_{i}，a_{i-1})×Rot(z_{i},θ_i)×Trans(z_{i}，d_i)

通用齐次变换矩阵如下：

^{i-1}T_i =

\left[ \begin{matrix} cosθ_i&-sinθ_i&0&a_{i} \\ sinθ_icosα_{i}&cosθ_icosα_{i}&-sinα_{i}&-sinα_{i}d_{i} \\ sinθ_isinα_{i}&cosθ_isinα_{i}&cosα_{i}&cosα_{i}d_{i} \\ 0&0&0&1 \end{matrix} \right]

2. 总变换

确定好DH参数建立方式并构建出各关节的DH参数后，代入各自的通用齐次变换矩阵，得到

^{0}T_1

、

^{1}T_2

、

^{2}T_3

、

^{3}T_4

、

^{4}T_5

、

^{5}T_6

共六个矩阵。在此作者选择的是改进型的D-H参数，因此各矩阵分别如下所示：

^{0}T_1 =

\left[ \begin{matrix} cosθ_1&-sinθ_1&0&a_1 \\ cosα_1sinθ_1&cosα_1cosθ_1&-sinα_1&-d_{1}sinα_1\\ sinα_{1}sinθ_1&sinα_{1}cosθ_1&cosα_{1}&d_{1}cosα_{1} \\ 0&0&0&1 \end{matrix} \right]

^{1}T_2 =

\left[ \begin{matrix} cosθ_2&-sinθ_2&0&a_2 \\ cosα_2sinθ_2&cosα_2cosθ_2&-sinα_2&-d_{2}sinα_2\\ sinα_{2}sinθ_2&sinα_{2}cosθ_2&cosα_{2}&d_{2} cosα_{2}\\ 0&0&0&1 \end{matrix} \right]

^{2}T_3 =

\left[ \begin{matrix} cosθ_3&-sinθ_3&0&a_3 \\ cosα_3sinθ_3&cosα_3cosθ_3&-sinα_3&-d_{3}sinα_3\\ sinα_{3}sinθ_3&sinα_{3}cosθ_3&cosα_{3}&d_{3} cosα_{3}\\ 0&0&0&1 \end{matrix} \right]

^{3}T_4 =

\left[ \begin{matrix} cosθ_4&-sinθ_4&0&a_4 \\ cosα_4sinθ_4&cosα_4cosθ_4&-sinα_4&-d_{4}sinα_4\\ sinα_{4}sinθ_4&sinα_{4}cosθ_4&cosα_{4}&d_{4} cosα_{4}\\ 0&0&0&1 \end{matrix} \right]

^{4}T_5 =

\left[ \begin{matrix} cosθ_5&-sinθ_5&0&a_5 \\ cosα_5sinθ_5&cosα_5cosθ_5&-sinα_5&-d_{5}sinα_5\\ sinα_{5}sinθ_5&sinα_{5}cosθ_5&cosα_{5}&d_{5} cosα_{5}\\ 0&0&0&1 \end{matrix} \right]

^{5}T_6 =

\left[ \begin{matrix} cosθ_6&-sinθ_6&0&a_6 \\ cosα_6sinθ_6&cosα_6cosθ_6&-sinα_6&-d_{6}sinα_6\\ sinα_{6}sinθ_6&sinα_{6}cosθ_6&cosα_{6}&d_{6} cosα_{6}\\ 0&0&0&1 \end{matrix} \right]

对该六个齐次变换矩阵按顺序相乘后得到六自由度机器人的总变换：

^{0}T_6 =

^{0}T_1×

^{1}T_2×

^{2}T_3×

^{3}T_4×

^{4}T_5×

^{5}T_6=

\left[ \begin{matrix} n_x&o_x&a_x&p_x \\ n_y&o_y&a_y&p_y\\ n_z&o_z&a_z&p_z \\ 0&0&0&1 \end{matrix} \right]

二、代码实现

1. 定义各连杆参数

对连杆参数进行符号变量的定义有两种方式，代码如下：

第一种方式 是将所有参数定义为未知量

syms   theta1   d1    a1     alpha1;
syms   theta2   d2    a2     alpha2;
syms   theta3   d3    a3     alpha3;
syms   theta4   d4    a4     alpha4;
syms   theta5   d5    a5     alpha5;
syms   theta6   d6    a6     alpha6;

但由于设置太多的未知量会导致总变换的结果过于冗长，因此在此设置 第二种方式 来规避复杂的结果，以下参数请参考上一篇的Matlab建立六自由度机器人模型

%连杆偏移
d1 = 398;
d2 = -0.299;
d3 = 0;
d4 = 556.925;
d5 = 0;
d6 = 165;
%连杆长度
a1 = 0;
a2 = 168.3;
a3 = 650.979;
a4 = 156.240;
a5 = 0;
a6 = 0;
%连杆扭角
alpha1 = 0;
alpha2 = pi/2;
alpha3 = 0;
alpha4 = pi/2;
alpha5 = -pi/2;
alpha6 = pi/2;
%由于我们需要分析各轴θi所对应的机器人末端位置
%因此theta1 theta2 theta3 theta4 theta5 theta6仍设为未知量
syms theta1 theta2 theta3 theta4 theta5 theta6

有了以上参数的设置，接下来对参数进行齐次变换矩阵的设置。

2. 齐次变换矩阵及总变换

首先对各参数归纳到一个统一的矩阵中，方便后续对参数的引用

% 参数矩阵取名为MDH
MDH = [theta1          d1       a1       alpha1;
       theta2+pi/2     d2       a2       alpha2;    
       theta3          d3       a3       alpha3;
       theta4          d4       a4       alpha4;
       theta5          d5       a5       alpha5;
       theta6          d6       a6       alpha6];

注意！！！ 由于作者的六自由度机器人在第二个关节中有一个关节变量偏移量，该偏移量是导致MDH中的theta2需要加pi/2的的关键。

接下来在齐次变换矩阵中引用参数矩阵的数值，对各关节的齐次变换矩阵进行一个定义，代码如下：

T01=[cos(MDH(1,1))               -sin(MDH(1,1))                 0              MDH(1,3);
     sin(MDH(1,1))*cos(MDH(1,4))  cos(MDH(1,1))*cos(MDH(1,4))  -sin(MDH(1,4)) -sin(MDH(1,4))*MDH(1,2);
     sin(MDH(1,1))*sin(MDH(1,4))  cos(MDH(1,1))*sin(MDH(1,4))   cos(MDH(1,4))  cos(MDH(1,4))*MDH(1,2);
      0                           0                             0              1];
T12=[cos(MDH(2,1))               -sin(MDH(2,1))                 0              MDH(2,3);
     sin(MDH(2,1))*cos(MDH(2,4))  cos(MDH(2,1))*cos(MDH(2,4))  -sin(MDH(2,4)) -sin(MDH(2,4))*MDH(2,2);
     sin(MDH(2,1))*sin(MDH(2,4))  cos(MDH(2,1))*sin(MDH(2,4))   cos(MDH(2,4))  cos(MDH(2,4))*MDH(2,2);
      0                           0                             0              1];
T23=[cos(MDH(3,1))               -sin(MDH(3,1))                 0              MDH(3,3);
     sin(MDH(3,1))*cos(MDH(3,4))  cos(MDH(3,1))*cos(MDH(3,4))  -sin(MDH(3,4)) -sin(MDH(3,4))*MDH(3,2);
     sin(MDH(3,1))*sin(MDH(3,4))  cos(MDH(3,1))*sin(MDH(3,4))   cos(MDH(3,4))  cos(MDH(3,4))*MDH(3,2);
      0                           0                             0              1];
T34=[cos(MDH(4,1))               -sin(MDH(4,1))                 0              MDH(4,3);
     sin(MDH(4,1))*cos(MDH(4,4))  cos(MDH(4,1))*cos(MDH(4,4))  -sin(MDH(4,4)) -sin(MDH(4,4))*MDH(4,2);
     sin(MDH(4,1))*sin(MDH(4,4))  cos(MDH(4,1))*sin(MDH(4,4))   cos(MDH(4,4))  cos(MDH(4,4))*MDH(4,2);
      0                           0                             0              1];
T45=[cos(MDH(5,1))               -sin(MDH(5,1))                 0              MDH(5,3);
     sin(MDH(5,1))*cos(MDH(5,4))  cos(MDH(5,1))*cos(MDH(5,4))  -sin(MDH(5,4)) -sin(MDH(5,4))*MDH(5,2);
     sin(MDH(5,1))*sin(MDH(5,4))  cos(MDH(5,1))*sin(MDH(5,4))   cos(MDH(5,4))  cos(MDH(5,4))*MDH(5,2);
      0                           0                             0              1];
T56=[cos(MDH(6,1))               -sin(MDH(6,1))                 0              MDH(6,3);
     sin(MDH(6,1))*cos(MDH(6,4))  cos(MDH(6,1))*cos(MDH(6,4))  -sin(MDH(6,4)) -sin(MDH(6,4))*MDH(6,2);
     sin(MDH(6,1))*sin(MDH(6,4))  cos(MDH(6,1))*sin(MDH(6,4))   cos(MDH(6,4))  cos(MDH(6,4))*MDH(6,2);
      0                           0                             0              1];

通过上述，可知总变换代码如下：

T06 = T01*T12*T23*T34*T45*T56;

3. 代码运行结果

在代码实例中，我们将对

θ_i

进行如下赋值

theta1 = pi/3;
theta2 = pi/4;
theta3 = pi/5;
theta4 = pi/3;
theta5 = pi/4;
theta6 = pi/5;

运行程序后如下图所示

通过上篇文章的robot.teach() 进行仿真后，其结果如下图所示

总结

以上就是正向运动学的内容，本文详细介绍了如何理解正向运动学及代码的实现，机器人工具箱提供了如何处理连杆长度、连杆扭曲等函数和方法。

参考文献

本文参与腾讯云自媒体同步曝光计划，分享自作者个人站点/博客。

原始发表：2022-05-19，如有侵权请联系 cloudcommunity@tencent.com 删除

matlab

本文分享自作者个人站点/博客前往查看

如有侵权，请联系 cloudcommunity@tencent.com 删除。

本文参与腾讯云自媒体同步曝光计划，欢迎热爱写作的你一起参与！

登录后参与评论

0 条评论

热度