CN106378778A - 一种采用马达代数求解机械臂运动学的方法 - Google Patents

Abstract

本发明公开了一种采用马达代数求解机械臂运动学的方法，利用螺旋位移描述各个连杆相对于固定参考坐标系的空间几何关系，用马达代数变换量描述相邻两连杆的空间关系，推导出末端效应器坐标系相对于基坐标系的等价马达代数变换量，建立机械臂的运动方程。该方法通过马达代数表示三维欧几里得空间内的点、线、面的运动来表达连杆坐标系之间的位姿关系，求出机械臂的正运动学和逆运动学，有较强的灵活性和较高的计算效率。

Description

一种采用马达代数求解机械臂运动学的方法

技术领域

本发明涉及机器人技术领域，具体是一种采用马达代数求解机械臂运动学的方法。

背景技术

机械臂的运动学分为正运动学和逆运动学，其中正运动学是已知机械臂的各个关节角，计算末端效应器坐标系相对于基坐标系的位姿，逆运动学是已知末端效应器相对应于基坐标系的期望位姿，计算一系列满足期望要求的关节角。

一般的机械臂运动学建模采用D-H四参数法。D-H法是1995年由Denavit和HartenBerg提出的一种建立相对位姿的矩阵方法。利用齐次变换描述各个连杆相对于固定参考坐标系的空间几何关系，用一个4×4的齐次变换矩阵描述相邻两连杆的空间关系，推导出末端效应器坐标系相对于基坐标系的等价齐次坐标变换矩阵，建立机械臂的运动方程。但是齐次变换矩阵需要12个参数来表达空间位形，表达姿态的单位正交阵的各个参数之间有6个约束条件，而且表达方式较为复杂，计算量也比较大。

马达代数(Motor Algebra)是由Clifford提出的一种代数，该代数充分利用对偶数代数表达能力，对偶四元数的扩充，其中“Motor”是“moment and vector”的缩写，它将Grassmann扩张代数、Hamilton四元数和复数等统一起来，避免了计算时各种代数语言之间的转换，可以直观地表示几何体的空间运动。马达代数作为几何代数的分支，可以看作四维几何代数空间G_3,0,1的一个子空间，标记为。已知三维空间中的刚体运动或欧式变换是非线性映射，而四维空间几何代数就可以使三维欧式空间的刚体运动实现齐次性，因此马达代数可以对高阶几何体的空间变换进行直接计算。因此，对于机械臂的运动学问题，可采用马达代数表示点或直线等几何体的变换来计算。

发明内容

本发明要解决的技术问题是提供一种采用马达代数求解机械臂运动学的方法，该方法能够在适宜的时间内建立机械臂的运动学方程，从而求解出正运动学方程和逆运动学参数。

一种采用马达代数求解机械臂正运动学方程的方法，包括以下步骤：

15)建立机械臂连杆坐标系；

16)建立机械臂的连杆参数表；

17)通过采用马达代数表示的三维欧几里得空间内的点、线、平面的变换来表达连杆坐标系之间的位姿关系；

18)根据连杆之间的位姿关系求解出末端效应坐标系与基坐标系的位姿关系，即机械臂的运动学方程。

进一步，所述马达代数表示的三维欧几里得空间由如下所示的基张成：

1,γ₂γ₃,γ₃γ₁,γ₁γ₂,γ₄γ₁,γ₄γ₂,γ₄γ₃,I (1)

其中，1表示标量；该马达代数表示的三维欧几里得空间的四个正交基γ满足γ_i ²＝1(i＝1,2,3)，且γ₄ ²＝0；I为伪标量，I＝γ₁γ₂γ₃γ₄，并且I²＝0，I表示四维几何代数空间中的一个单位体积元；γ_iγ_j＝γ_i·γ_j+γ_i∧γ_j＝γ_i∧γ_j表示两个不同正交基之间的几何积，称为一个二重向量，前三组二重向量的对偶分别对应后三组二重向量；

所述马达代数表示的三维欧几里得空间的“点”表示为：

X＝1+x₁γ₄γ₁+x₂γ₄γ₂+x₃γ₄γ₃

＝1+I(x₁γ₂γ₃+x₂γ₃γ₁+x₃γ₁γ₂)

＝1+Ix≡(1,0)+I(0,x) (2)

最后一行的x代表该点的坐标(x₁ x₂ x₃)。

所述马达代数表示的三维欧几里得空间的“线”表示为：

L≡(0,n)+I(0,m) (3)

其中，(n,m)是直线的Plucker坐标，n表示直线方向的单位向量；||m||等于坐标系原点到直线的距离，且m与n正交。

所述马达代数表示的三维欧几里得空间的“线”表示为：

H≡(0,n)+I(d,0) (4)

其中，

n＝L_n1γ₂γ₃+L_n2γ₃γ₁+L_n3γ₁γ₂ (5)

m＝L_m1γ₂γ₃+L_m2γ₃γ₁+L_m3γ₁γ₂ (6)

任何一个刚体的一般运动都可以通过绕某个轴的转动和沿该轴的平移实现，而马达代数G_3,0,1可以由平移和旋转的旋量形式来表示M＝T_sR_s，其中T_s为平移子，R_s为旋转子，设某一物体时刻0时位姿的马达代数为L₀，经过位姿变换后时刻1时的位姿为L₁，则用马达代数形式可以表示为其中M为马达代数变换量，为其共轭。

进一步的,所述步骤4)中的求解出末端效应坐标系与基坐标系的位姿关系的过程为:

根据机械臂的连杆参数求得相邻两个关节坐标系变换方程的表示形式^i-1M_i，可求得一般n个关节的机械臂从坐标系{n}到基坐标系{0}的变换方程：

${M_0}_n={M_0}_1{M_1}_2{M_2}_3\mathrm{...}{M_{n-1}}_n=\underset{i=1}{\overset{n}{\Π}}{M_{i-1}}_i---\left(7\right)$

点的变换方程为:

$X_0={M_0}_{n}X_n{\widetilde{\stackrel{\‾}{M}}_0}_n=\underset{i=1}{\overset{n}{\Π}}{M_{i-1}}_i{X_n}^0\underset{i=1}{\overset{n}{\Π}}{\widetilde{\stackrel{\‾}{M}}_{n-i}}_{n+1-i}---\left(8\right)$

直线的变换方程为:

$L_0={M_0}_{n}L_n{\widetilde{\stackrel{\‾}{M}}_0}_n=\underset{i=1}{\overset{n}{\Π}}{M_{i-1}}_{i}L_n\underset{i=1}{\overset{n}{\Π}}{\widetilde{\stackrel{\‾}{M}}_{n-i}}_{n+1-i}---\left(9\right)$

由以上变换关系式便可求出机械臂的运动学方程。

马达代数也可以用来解决机械臂的运动学逆问题。利用马达代数表示特定的点，线和平面的变换，由此计算出机械臂运动学的逆解。机械臂的末端效应器坐标系相对于基坐标系的位姿是已知的，即末端效应器坐标系中的元素在基坐标系的表达式是已知的。

马达代数求解机械臂逆运动学的具体实现步骤如下：

21)建立机械臂的运动学模型；

22)对末端效应器坐标系中的点、线、面元素在基坐标系中的表达式做正向变换，得到一系列正向表达式；

23)对末端效应器坐标系中的点、线、面元素在末端效应器坐标系中的表达式做逆向变换，得到一系列逆向表达式；

24)由同一坐标系中正向表达式和逆向表达式相等得到交会方程；

25)交会方程求解，输出运动学的逆解。

所述步骤22)中，正向变换的解释如下：将一个空间位置姿态根据连杆每个关节的角度从连杆的基座坐标系变换至末端坐标系下。例如将变换到F₁中，得到并如此依次向后进行变换。逆向变换的解释如下：已知一个空间位置姿态在末端坐标系下的表示，从而求解连杆每个关节的角度。所述步骤23)中，逆向变换的解释如下：例如将变换到F_n-1中，得到并如此依次向前进行变换。所述步骤24)中，交会方程的解释如下：正向变换和逆向变换在某坐标系处交会，由于都是同一个元素的表达式，该坐标系中的正向变换表达式与逆向变换表达式相等，由此得到交会方程。在所有坐标系中的交会方程都是等价的。所述步骤25)中，使得交会方程成立的未知数的值即机械臂运动学的逆解。

与一般采用的D-H法建模相比，本发明提出了一种采用马达代数求解机械臂运动学的方法。D-H法用一个4×4的齐次变换矩阵描述相邻两连杆的空间关系，推导出末端效应器坐标系相对于基坐标系的等价齐次坐标变换矩阵，来建立机械臂的运动方程。但是D-H法的局限性在于它无法表示一些复杂的关节，而且表达方式较为复杂，计算量也比较大。马达代数用8个参数来表达机械臂各个坐标系之间的空间位形，将位置及姿态进行统一的描述，由此推导出运动学方程，是一种效率较高的机械臂运动学建模方法。在求解逆运动学方面，马达代数也有较大的灵活性，相对于D-H法计算量更小一些，有较强的灵活性和较高的计算效率

附图说明

图1为本发明采用马达代数求解机械臂正运动学方程的方法的流程方框图；

图2为本发明实施例刚体运动位姿变换图；

图3为本发明实施例SCARA机器人结构简图；

图4为本发明实施例机械臂相邻连杆坐标系之间的位姿关系图

图5为本发明实施例坐标系中特定实体的符号标注图

图6为本发明实施例正向变换和逆向变换的关系图。

具体实施方式

下面结合附图，对本发明提出的一种采用马达代数求解机械臂运动学的方法进行详细说明。

参图1所示，本发明实施例公开的一种采用马达代数的机械臂运动学表示方法，该方法具体实现步骤如下：

S1、建立机械臂连杆坐标系；

S2、写出机械臂的连杆参数表；

S3、通过用马达代数表示的三维欧几里得空间内的点、线、面的变换来表达连杆坐标系之间的位姿关系；

S4、根据连杆之间的位姿关系推导出末端效应坐标系与基坐标系的位姿关系，即机械臂的运动学方程。

下面对本发明实施例中的技术方案进行更详细地描述。

马达代数是一种充分利用对偶数代数表达能力而由Clifford提出的一种代数。它将Grassmann扩张代数、Hamilton四元数和复数等统一起来，避免了计算时各种代数语言之间的转换，可以直观地表示几何体的空间运动。

马达代数空间可由如下所示的基张成：

1,γ₂γ₃,γ₃γ₁,γ₁γ₂,γ₄γ₁,γ₄γ₂,γ₄γ₃,I

其中：1表示标量。该空间的四个正交基满足γ_i ²＝1(i＝1,2,3)且γ₄ ²＝0。I＝γ₁γ₂γ₃γ₄为伪标量，并且I²＝0。它可以表示四维几何代数空间中的一个单位体积元。γ_iγ_j＝γ_i·γ_j+γ_i∧γ_j＝γ_i∧γ_j表示两个不同正交基之间的几何积，称为一个二重向量。前三组与后三组二重向量分别满足对偶关系，例如，γ₂γ₃的对偶是Iγ₂γ₃也可写作γ₄γ₁。

马达代数中“点”的表示：

X＝1+x₁γ₄γ₁+x₂γ₄γ₂+x₃γ₄γ₃

＝1+I(x₁γ₂γ₃+x₂γ₃γ₁+x₃γ₁γ₂)

＝1+Ix≡(1,0)+I(0,x)

“线”的表示：

L≡(0,n)+I(0,m)

“平面”的表示：

H＝n+x∧n＝n+I(x·n)＝n+Id

≡(0,n)+I(d,0)

任何一个刚体的一般运动都可以通过绕某个轴的转动和沿该轴的平移实现，而马达代数G_3,0,1可以由平移和旋转的旋量形式来表示M＝T_sR_s，参图2所示，其中T_s为平移子，R_s为旋转子，设某一物体时刻0时位姿的马达代数为L₀，经过位姿变换后时刻1时的位姿为L₁，则用马达代数形式可以表示为其中M为马达代数变换量，为其共轭。

根据机械臂的连杆参数求得相邻两个关节坐标系变换方程的表示形式^i-1M_i，可求得一般n个关节的机械臂从坐标系{n}到基坐标系{0}的变换方程：

${M_0}_n={M_0}_1{M_1}_2{M_2}_3\mathrm{...}{M_{n-1}}_n=\underset{i=1}{\overset{n}{\Π}}{M_{i-1}}_i$

点的变换方程为

$X_0={M_0}_{n}X_n{\widetilde{\stackrel{\‾}{M}}_0}_n=\underset{i=1}{\overset{n}{\Π}}{M_{i-1}}_i{X_n}^0\underset{i=1}{\overset{n}{\Π}}{\widetilde{\stackrel{\‾}{M}}_{n-i}}_{n+1-i}$

直线的变换方程为

$L_0={M_0}_{n}L_n{\widetilde{\stackrel{\‾}{M}}_0}_n=\underset{i=1}{\overset{n}{\Π}}{M_{i-1}}_{i}L_n\underset{i=1}{\overset{n}{\Π}}{\widetilde{\stackrel{\‾}{M}}_{n-i}}_{n+1-i}$

由以上变换关系式便可求出机械臂的运动学方程。

正运动学求解实例：

参图3所示，SCARA机器人有3个旋转关节，其轴线相互平行，在平面内进行定位和定向。另一个关节是移动关节，用于完成末端件在垂直于平面的运动。

表1 SCARA机械臂连杆参数表

其中，θ_i是绕轴，从旋转到的角度；

d_i是沿轴，从移动到的距离；

α_i是绕轴，从旋转到的角度；

a_i是沿轴，从移动到的距离。

参图4所示，建立基坐标系到末端效应器坐标系五个坐标系F₀～F₄，n＝4。由各关节参数求得F₄到F₀的变换方程⁰M₄的表达式：

参图5所示，是F₄的原点。

在F₄中的表示为

$P_4^o{}_4=1+I\left(\begin{array}{c}0\\ 0\\ 0\end{array}\right)=1$

在F₀中记为 到的变换可表示为

$\begin{array}{c}P_4^o{}_0={M_0}_{4}P_4^o{}_4{\widetilde{\stackrel{\‾}{M}}_0}_4={M_0}_4\left(1+I\left(\begin{array}{c}0\\ 0\\ 0\end{array}\right)\right){\widetilde{\stackrel{\‾}{M}}_0}_4\\ =1+I\left(\begin{array}{c}{a}_3\cos ({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)+a_2\cos \left({\mathrm{\θ}}_1\right)\\ a_3\sin ({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)+a_2\sin \left({\mathrm{\θ}}_1\right)\\ d_1+d_2+d_4\end{array}\right)\end{array}$

即为马达代数表示的SCARA正运动学方程。

逆运动学求解实例：

马达代数也可以用来解决机械臂的运动学逆问题。利用马达代数表示特定的点，线和平面的变换，由此计算出机械臂运动学的逆解。

机械臂的末端效应器坐标系相对于基坐标系的位姿是已知的，即末端效应器坐标系中的元素在基坐标系的表达式是已知的。参图5所示，坐标系F_n的原点x轴所在直线xy面在F₀中的表达式都是已知的。

马达代数求解机械臂逆运动学的具体实现步骤如下：

1)建立机械臂的运动学模型；

2)对末端效应器坐标系中的点、线、面元素在基坐标系中的表达式做正向变换，得到一系列正向表达式；

3)对末端效应器坐标系中的点、线、面元素在末端效应器坐标系中的表达式做逆向变换，得到一系列逆向表达式；

4)由同一坐标系中正向表达式和逆向表达式相等得到交会方程；

5)交会方程求解，输出运动学的逆解。

所述步骤2)中，正向变换的解释如下：将末端效应器坐标系中的点、线、面元素在基坐标系F₀中的表达式分别依次变换到F₁,F₂…F_n中。参图6所示，将变换到F₁中，得到并如此依次向后进行变换。

所述步骤3)中，逆向变换的解释如下：将末端效应器坐标系中的点、线、面元素在基坐标系F₀中的表达式分别依次变换到F₁,F₂…F_n中。参图6所示，将变换到F_n-1中，得到并如此依次向前进行变换。所述步骤4)中，交会方程的解释如下：参图6所示，正向变换和逆向变换在某坐标系处交会，由于都是同一个元素的表达式，该坐标系中的正向变换表达式与逆向变换表达式相等，由此得到交会方程。在所有坐标系中的交会方程都是等价的。所述步骤5)中，使得交会方程成立的未知数的值即机械臂运动学的逆解。

实例：

以SCARA机器人为例。已知

$P_4^o{}_4=1+I\left(\begin{array}{c}0\\ 0\\ 0\end{array}\right)=1$

可求得

$P_4^o{}_3={M_3}_{4}P_4^o{}_4{\widetilde{\stackrel{\‾}{M}}_3}_4=1+I\left(\begin{array}{c}0\\ 0\\ d_4\end{array}\right)$

$P_4^o{}_2={M_2}_{3}P_4^o{}_3{\widetilde{\stackrel{\‾}{M}}_2}_3=1+I\left(\begin{array}{c}{a}_3\\ 0\\ d_4\end{array}\right)$

$P_4^o{}_1={M_1}_{2}P_4^o{}_2{\widetilde{\stackrel{\‾}{M}}_1}_2=1+I\left(\begin{array}{c}{a}_{3}cos\left({\mathrm{\θ}}_2\right)+a_2\\ a_3\sin \left({\mathrm{\θ}}_2\right)\\ d_2+d_4\end{array}\right)$

$P_4^o{}_0={M_0}_{1}P_4^o{}_0{\widetilde{\stackrel{\‾}{M}}_0}_1=1+I\left(\begin{array}{c}{a}_{3}cos({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)+a_{2}cos\left({\mathrm{\θ}}_1\right)\\ a_{3}sin({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)+a_{2}sin\left({\mathrm{\θ}}_1\right)\\ d_1+d_2+d_4\end{array}\right)$

表2展示了(F₄原点)在F₀,F₁,F₂,F₃和F₄中的交会方程。其中c₁＝cos(θ₁)、s₁＝sin(θ₁)、c₁₂＝cos(θ₁+θ₂)、s₁₂＝sin(θ₁+θ₂)，以此类推。

表2-在F₀,F₁,F₂,F₃和F₄中的交会方程

通过观察，可采用F₁中的三个方程求解。

$\left\{\begin{array}{c}{P}_x{c}_1+P_y{s}_1=a_3{c}_2+a_2\\ -P_x{s}_1+P_y{c}_1=a_3{s}_2\\ P_z-d_1=d_2+d_4\end{array}\right.$

最后求得

$\left\{\begin{array}{c}{d}_4=P_z-d_1-d_2\\ {\mathrm{\θ}}_1={\arctan }_2(x,y)-{\arctan }_2(z,\±\sqrt{x^2+y^2-z^2})\end{array}\right.$

其中x＝P_x，y＝-P_y，

对应于式中的正负号，θ₁有两个解。θ₁已知，则第二个式子的左边为已知，可求得θ₂的解

${\mathrm{\θ}}_2=\arcsin \left(\frac{-P_{x}sin\left({\mathrm{\θ}}_1\right)+P_{y}cos\left({\mathrm{\θ}}_1\right)}{a_3}\right)$

还有一个未知量，即θ₃，这时候采用线变换来求。由末端姿态可推导出在F₀中表示

$L_4^x{}_0=\left(\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right)+I\left(\begin{array}{c}{B}_x\\ B_y\\ B_z\end{array}\right)$

参图5所示，为F₄坐标系x轴所在的直线，它在F₄中表示为

$L_4^x{}_4=\left(\begin{array}{c}1\\ 0\\ 0\end{array}\right)+I\left(\begin{array}{c}0\\ 0\\ 0\end{array}\right)$

由上式逆向推导在F₀中的表达式

$L_4^x{}_3={M_3}_{4}L_4^x{}_4{\widetilde{\stackrel{\‾}{M}}_3}_4$

$L_4^x{}_2={M_2}_{3}L_4^x{}_3{\widetilde{\stackrel{\‾}{M}}_2}_3$

$L_4^x{}_1={M_1}_{2}L_4^x{}_2{\widetilde{\stackrel{\‾}{M}}_1}_2$

$L_4^x{}_0={M_0}_{1}L_4^x{}_0{\widetilde{\stackrel{\‾}{M}}_0}_1$

最后求得

$L_4^x{}_0=\left(\begin{array}{c}{s}_{123}\\ c_{123}\\ 0\end{array}\right)+I\left(\begin{array}{c}-(d_1+d_2+d_4)s_{123}\\ (d_1+d_2+d_4)c_{123}\\ a_3{s}_3+a_2{s}_{23}\end{array}\right)$

由此可知A_x＝s₁₂₃，即θ₃＝arcsin(A_x)-θ₁-θ₂。

综上所述，本发明的方法在求解机械臂正运动学和逆运动学时，通过利用马达代数表示三维空间内的点、线、面的运动变换来表达连杆坐标系之间的位姿关系，得出机械臂的运动学方程，即机械臂的正运动学。利用末端效应器坐标系中点、线、面的两个已知量分别变换到不同的坐标系中，而表达式相同的特点，建立方程组来求解运动学逆解，解决了机械臂的运动学逆问题。与传统的D‐H法比较，本发明提出的方法在解决机械臂运动学问题时拥有较强的灵活性和较高的计算效率，表达式简洁直观，可以表示一些复杂的机械臂关节。

本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，这些改进也应视为本发明的保护范围。

Claims (8)

1.一种采用马达代数求解机械臂正运动学方程的方法，其特征在于，包括以下步骤：

11)建立机械臂连杆坐标系；

12)建立机械臂的连杆参数表；

13)通过采用马达代数表示的三维欧几里得空间内的点、线、平面的变换来表达连杆坐标系之间的位姿关系；

14)根据连杆之间的位姿关系求解出末端效应坐标系与基坐标系的位姿关系，即机械臂的运动学方程。

2.根据权利要求1所述的采用马达代数求解机械臂正运动学方程的方法，其特征在于，所述步骤13)中马达代数表示的三维欧几里得空间由如下所示的基张成：

1,γ₂γ₃,γ₃γ₁,γ₁γ₂,γ₄γ₁,γ₄γ₂,γ₄γ₃,I (1)

其中，1表示标量；该马达代数表示的三维欧几里得空间的四个正交基γ满足γ_i ²＝1(i＝1,2,3)，且γ₄ ²＝0；I为伪标量，I＝γ₁γ₂γ₃γ₄，并且I²＝0，I表示四维几何代数空间中的一个单位体积元；γ_iγ_j＝γ_i·γ_j+γ_i^γ_j＝γ_i^γ_j表示两个不同正交基之间的几何积，称为一个二重向量，前三组二重向量的对偶分别对应后三组二重向量；

所述马达代数表示的三维欧几里得空间的“点”表示为：

X≡(1,0)+I(0,x) (2)

其中，x为点的坐标，且x＝(x₁ x₂ x₃)；

所述马达代数表示的三维欧几里得空间的“线”表示为：

L≡(0,n)+I(0,m) (3)

其中，(n,m)是直线的Plucker坐标，n表示直线方向的单位向量；||m||等于坐标系原点到直线的距离，且m与n正交；

所述马达代数表示的三维欧几里得空间的“线”表示为：

H≡(0,n)+I(d,0) (4)。

3.根据权利要求2所述的采用马达代数求解机械臂正运动学方程的方法，其特征在于，所述步骤4)中的求解出末端效应坐标系与基坐标系的位姿关系的过程为:

根据机械臂的连杆参数求得相邻两个关节坐标系变换方程的表示形式^i-1M_i，求得n个关节的机械臂从坐标系{n}到基坐标系{0}的变换方程：

${M_0}_n={M_0}_1{M_1}_2{M_2}_3\mathrm{...}{M_{n-1}}_n=\underset{i=1}{\overset{n}{\Π}}{M_{i-1}}_i---\left(7\right)$

点的变换方程为:

$X_0={M_0}_{n}X_n{\widetilde{\stackrel{\‾}{M}}_0}_n=\underset{i=1}{\overset{n}{\Π}}{M_{i-1}}_i{X_n}^0\underset{i=1}{\overset{n}{\Σ}}{\widetilde{\stackrel{\‾}{M}}_{n-i}}_{n+1-i}---\left(8\right)$

直线的变换方程为:

$L_0={M_0}_{n}L_n{\widetilde{\stackrel{\‾}{M}}_0}_n=\underset{i=1}{\overset{n}{\Π}}{M_{i-1}}_{i}L_n\underset{i=1}{\overset{n}{\Π}}{\widetilde{\stackrel{\‾}{M}}_{n-i}}_{n+1-i}---\left(9\right)$

由以上变换关系式便可求出机械臂的运动学方程。

4.一种采用马达代数求解机械臂逆运动学参数的方法，其特征在于，包括以下步骤：

21)建立机械臂的运动学模型；

22)对末端效应器坐标系中的点、线、面元素在基坐标系中的表达式做正向变换，得到一系列正向表达式；

23)对末端效应器坐标系中的点、线、面元素在末端效应器坐标系中的表达式做逆向变换，得到一系列逆向表达式；

24)由同一坐标系中正向表达式和逆向表达式相等得到交会方程；

25)交会方程求解，输出运动学的逆解。

5.根据权利要求4所述的采用马达代数求解机械臂正运动学方程的方法，其特征在于，所述步骤22)中，正向变换为：将变换到F₁中，得到并如此依次向后进行变换。

6.根据权利要求4所述的采用马达代数求解机械臂正运动学方程的方法，其特征在于，所述步骤23)中，逆向变换为：将变换到F_n-1中，得到并如此依次向前进行变换。

7.根据权利要求4所述的采用马达代数求解机械臂正运动学方程的方法，其特征在于，所述步骤24)中，交会方程为：正向变换和逆向变换在某坐标系处交会，该坐标系中的正向变换表达式与逆向变换表达式相等，由此得到交会方程；在所有坐标系中的交会方程都是等价的。

8.根据权利要求4所述的采用马达代数求解机械臂正运动学方程的方法，其特征在于，所述步骤25)中，使得交会方程成立的未知数的值即机械臂运动学的逆解。