CN102637158A - 一种六自由度串联机器人运动学逆解的求解方法 - Google Patents

Abstract

本发明公开了一种六自由度串联机器人运动学逆解的方法，其步骤是：(1)建立连杆坐标系，设定变量θ₁，θ₂，θ₃，θ₄，θ₅，θ₆；(2)设定初始位形；(3)利用几何法求解θ₄，θ₅，θ₆；(4)利用代数消元法θ₁，θ₂，θ₃；并在求非正交球型或者非正交非球型的末端结构时引入禁忌搜索法从而求解出相应的数字解。本发明构思巧妙，利用几何法和代数消元法综合求解，避免了任意构造方程导致方程系数行列式的秩小于阶数的问题，更高效准确地求解6个轴的解析解，且对于复杂机构的三角函数关系利用几何法可以有效地使用消元法将二元一次方程转化为一元一次，从而得到对应唯一的解析解。

Description

一种六自由度串联机器人运动学逆解的求解方法

技术领域

本发明涉及一种机器人运动学的求解方法，具体地讲，尤其是涉及一种六自由度串联机器人运动学逆解的求解方法。

背景技术

目前，对六自由度串联机器人运动学逆解的求解方法是将机器人各轴旋转角度设为变量带入正运动学方程 $f({\mathrm{\θ}}_1,{\mathrm{\θ}}_2,{\mathrm{\θ}}_3,{\mathrm{\θ}}_4,{\mathrm{\θ}}_5,{\mathrm{\θ}}_3)=T_2^1{}_1{}^0{T}_3^2{T}_4^3{T}_5^4{T}_6^{5}T$ 中，根据矩阵乘法原理T_c＝T_a·T_b，其中T_b＝(T_a)^-1·T_c可推导出运动学方程 $T_6^0=$ $\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right]=T_1^0\left({\mathrm{\θ}}_1\right)T_2^1\left({\mathrm{\θ}}_2\right)T_3^2\left({\mathrm{\θ}}_3\right)T_4^3\left({\mathrm{\θ}}_4\right)T_5^4\left({\mathrm{\θ}}_5\right)T_6^5\left({\mathrm{\θ}}_6\right),$ 方程左边数据已知，整理方程可将含有θ₁的部分移动到方程的左边得到

通过类似方法结合三角函数相关和差化积公式可以分离变量求解每个轴的角度。此方法主要利用等式原理在机器人连杆的姿态和位置的矩阵里构造含一个变量的方程，从而求解。此方法缺点在于任意构造方程可能导致方程系数行列式的秩小于阶数，无法得到对应解析解，而且对于复杂机构的三角函数关系求解有更多冗余，要得到其中能够唯一表达解析解的方程相当困难。

发明内容

本发明的目的在于提供一种六自由度串联机器人运动学逆解的求解方法，解决目前求解方法中任意构造方程导致方程系数行列式的秩小于阶数而无法得到对应解析解的缺陷。

为了实现上述目的，本发明采用的技术方案如下：

一种六自由度串联机器人运动学逆解的求解方法，当机器人连杆的后三个关节的轴线交于一点时，其步骤包括：

(1)建立连杆坐标系：将机器人连杆放入坐标系，固定端为基坐标，依次往后的六个关节轴旋转角度分别设为变量θ₁，θ₂，θ₃，θ₄，θ₅，θ₆；

(2)当机器人连杆的后三个关节的轴线交于一点时，利用几何法，即根据Paden-Kahan子问题思想建立与后三个关节相关的函数SubProb_3R(ξ₄，ξ₅，ξ₆，p，q)求解θ₄，θ₅，θ₆的值，其中ξ₄，ξ₅，ξ₆为三个零节距的单位运动旋量，p，q为空间两点；

(3)利用矩阵乘法变换

此时已知θ₄，θ₅，θ₆，通过θ₁，θ₂，θ₃三个变量构造三角函数方程，利用代数消元法即可求得θ₁，θ₂，θ₃。

进一步地，所述步骤(2)中，ξ₄，ξ₅，ξ₆的三个空间转轴之间的关系是ξ₄空间垂直于ξ₅和ξ₆，ξ₅空间平行于ξ₆；对θ₄，θ₅，θ₆三个旋转角度的求解包括对一个单轴旋转角度θ₆的求解和对一个双轴旋转角度θ₄，θ₅的求解。

更进一步地，对一个单轴旋转角度θ₆的求解步骤包括：

(1)设d为转轴上一点，定义u＝p-d，v＝q-d，根据位置不变原则有： $e^{\widehat{{\mathrm{\θ}}_6}{\mathrm{\ξ}}_6}p=q,$ $e^{\widehat{{\mathrm{\θ}}_6}{\mathrm{\ξ}}_6}d=d,$ 则 $e^{\widehat{{\mathrm{\θ}}_6}{\mathrm{\ξ}}_6}u=v;$

(2)定义u′，v′为u，v在垂直于转轴ξ₆的平面上的投影，则u′＝u-ωω^Tu，v′＝v-ωω^Tv，其中ω为关节旋转的法线；

(3)当且仅当u，v在轴上的投影等长时，在轴ω上垂直的平面上投影也等长，即ω^Tu＝ω^Tv，||u′||＝||v′||，那么根据式子和投影矢量u′，v′求得旋转角度，即对应第六关节的旋转角度θ₆。

再进一步地，对一个双轴旋转角度θ₄，θ₅的求解步骤包括：

(1)假定将点p绕给定轴ξ₅旋转θ₅，再绕给定轴ξ₄旋转θ₄到点q重合；

(2)令q₁是转轴ξ₄上的任一点，由距离保持不变原则得到：

令δ＝||q-q₁||，则

令q₂是转轴ξ₅上任一点，并定义u＝p-q₂，v＝q₁-q₂，则 ${\left|\right|e^{\widehat{{\mathrm{\θ}}_5}{\mathrm{\ξ}}_5}-q_1\left|\right|}^2={\mathrm{\δ}}^2;$

(3)将上述两点向垂直于转轴ξ₅的平面做投影，并定义u′，v′为u，v在垂直于转轴ξ₅的平面上的投影，则u′＝u-ω₂ω₂ ^Tu，v′＝v-ω₂ω₂ ^Tv，其中ω₂为关节旋转的法线，同样对δ投影，得u′²＝δ²-||ω₂ ^T(p-q₁)||²，即

(4)设θ为矢量u′与v′之间的夹角，则θ＝tan^-12(ω^T(u′×v′)，u′^Tv′)，利用余弦定理求解φ＝θ-θ₅，有||u′||²+||v′||²-2||u′||||v′||cosφ＝δ²，因此， ${\mathrm{\θ}}_5=\mathrm{\θ}\±{\cos }^{-1}\left(\frac{{\left|\right|u^{\′}\left|\right|}^2+{\left|\right|v^{\′}\left|\right|}^2-{\mathrm{\δ}}^{\′2}}{2\left|\right|u^{\′}\left|\right|\left|\right|v^{\′}\left|\right|}\right);$

(5)已知θ₅，则由求得p₁，再根据

计算θ₄，得双轴旋转角度，即分别对应第四和第五关节的旋转角度θ₄和θ₅。

一种六自由度串联机器人运动学逆解的求解方法，当机器人连杆的后三个关节的轴线不交于一点时，其步骤包括：

(1)建立连杆坐标系：将机器人连杆放入坐标系，固定端为基坐标，依次往后的六个关节轴旋转角度分别设为变量θ₁，θ₂，θ₃，θ₄，θ₅，θ₆；

(2)假设θ₆为已知值，利用几何法和代数消元法求取θ₁，θ₂，θ₃，θ₄，θ₅得到θ₁＝β₁，θ₂＝β₂，θ₃＝β₃，θ₄＝β₄，θ₅＝β₅，其中β₁，β₂，β₃，β₄，β₅为解析解表达式；

(3)根据六自由度机器人运动学正解的求解公式，得到正解表达式

$T_{\′}^6{}_0=\left[\begin{array}{cccc}{r}_{11}^{\′}& r_{12}^{\′}& r_{13}^{\′}& p_x^{\′}\\ r_{21}^{\′}& r_{22}^{\′}& r_{23}^{\′}& p_y^{\′}\\ r_{31}^{\′}& r_{32}^{\′}& r_{33}^{\′}& p_z^{\′}\\ 0& 0& 0& 1\end{array}\right]=T_1^0\left({\mathrm{\β}}_1\right)T_2^1\left({\mathrm{\β}}_2\right)T_3^2\left({\mathrm{\β}}_3\right)T_4^3\left({\mathrm{\β}}_4\right)T_5^4\left({\mathrm{\β}}_5\right)T_6^5\left({\mathrm{\β}}_6\right);$

(4)将机器人连杆的结果位姿与初始位姿进行对比，得表达式

其中为初始位姿；

(5)利用优化算法以γ为目标，调整θ₆的值使其满足|γ|≤σ，其中σ为误差阈值，得到θ₁，θ₂，θ₃，θ₄，θ₅，θ₆的数字解。

进一步地，所述优化算法为禁忌搜索法或爬山法。

更进一步地，所述步骤(5)具体包括以下步骤：

(5a)设θ₆为具体数值；

(5b)根据步骤(2)～(4)得到γ的值；

(5c)对比|γ|与误差阈值σ的大小；

(5d)若|γ|≤σ，则该具体数值为θ₆的数字解，若|γ|＞σ，则根据禁忌搜索法或者爬山法重新设定θ₆为具体数值；

(5e)重复执行步骤(5b)～(5d)，直至得到符合要求的θ₆的数字解，此时，该θ₆对应的由步骤(2)得到的θ₁，θ₂，θ₃，θ₄，θ₅的值即为其相应的数字解。

与现有技术相比，本发明的有益效果是：

(1)本发明构思巧妙，以几何法和代数消元法综合求解代替了原有的单一求解方式，避免了任意构造方程导致方程系数行列式的秩小于阶数的问题。

(2)本发明利用基于Paden-Kahan子问题的几何法求解出三个轴的解，再利用代数消元法求解剩余的三个轴的解，减少了直接利用三角函数关系求解计算的冗余，从而提高了求解效率。

(3)本发明利用几何法的优点，有效地避免了复杂机构的三角函数关系使用消元法不容易将二元一次方程转化为一元一次方程的问题，使所得的解析解更加准确，从而得到对应唯一的解析解。

(4)本发明通过结合禁忌搜索法，还可以求得五个轴的解析解表达式和一个轴的数字解，使机器人连杆末端为非正交球型或者非正交非球型的结构时求解困难的情况得以改善。

(5)本发明利用几何法、代数消元法、以及禁忌搜索法的结合运用，能够求解出连杆机器人的各种情况，大大增加了本发明的适用范围。

附图说明

图1是本发明-实施例1中利用代数消元法建立连杆坐标系的结构示意图。

图2是图1中连杆坐标系的示意图。

图3是本发明-实施例1中利用几何法求解的原理示意图。

图4是图3中单轴旋转角度求解的原理示意图。

图5是图3中双轴旋转角度求解的原理示意图。

图6是本发明-实施例2的连杆结构示意图。

具体实施方式

下面结合附图和具体的实施例对本发明作进一步说明。本发明的实施方式包括但不限于下列实施例。

实施例1

如图1和图2所示，以The Unimation PUMA 560机器人为例，建立连杆坐标系，将机器人连杆放入坐标系，固定端为基坐标，依次往后的六个关节轴旋转角度分别设为变量θ₁，θ₂，θ₃，θ₄，θ₅，θ₆；并写出Denavit-Hartenberg参数表如下：

PUMA560机器人的连杆参数

已知初始位形的旋转角度值，记作θ_s，设G(θ_t)为机器人位于初始位形时惯性坐标系的刚体变换，则每个关节构造一个单位运动旋量，用G(θ_i)表示第i个关节的旋转运动，此时除了第i个关节之外的所有其他关节均固定于初始位形；

The Unimation PUMA 560机器人的结构特点为，前三个轴控制末端法兰的空间位置，后三个轴控制末端法兰的空间姿态，即机器人连杆的后三个关节的轴线交于一点。如图3所示，利用几何法，即根据Paden-Kahan子问题思想建立与后三个关节相关的函数SubProb_3R(ξ₄，ξ₅，ξ₆，p，q)求解θ₄，θ₅，θ₆的值，其中ξ₄，ξ₅，ξ₆为三个零节距的单位运动旋量，p，q为空间两点，ξ₄，ξ₅，ξ₆的三个空间转轴之间的关系是ξ₄空间垂直于ξ₅和ξ₆，ξ₅空间平行于ξ₆；对θ₄，θ₅，θ₆三个旋转角度的求解包括对一个单轴旋转角度θ₆的求解和对一个双轴旋转角度θ₄，θ₅的求解。

如图4所示，对一个单轴旋转角度θ₆的求解步骤包括：

(1)设d为转轴上一点，定义u＝p-d，v＝q-d，根据位置不变原则有： $e^{\widehat{{\mathrm{\θ}}_6}{\mathrm{\ξ}}_6}p=q,$ $e^{\widehat{{\mathrm{\θ}}_6}{\mathrm{\ξ}}_6}d=d,$ 则 $e^{\widehat{{\mathrm{\θ}}_6}{\mathrm{\ξ}}_6}u=v;$

(2)定义u′，v′为u，v在垂直于转轴ξ₆的平面上的投影，则u′＝u-ωω^Tu，v′＝v-ωω^Tv，其中ω为关节旋转的法线；

(3)当且仅当u，v在轴上的投影等长时，在轴ω上垂直的平面上投影也等长，即ω^Tu＝ω^Tv，||u′||＝||v′||，那么根据式子

和投影矢量u′，v′求得旋转角度，即对应第六关节的旋转角度θ₆。如果u′≠0，则有 $\left\{\begin{array}{c}{u}^{\′}\·v^{\′}=\left|\right|u^{\′}\left|\right|\left|\right|v^{\′}\left|\right|\cos \mathrm{\θ}\\ u^{\′}\×v^{\′}=\mathrm{\ω}\left|\right|u^{\′}\left|\right|\left|\right|v^{\′}\left|\right|\sin \mathrm{\θ}\end{array}\right.,$ 故θ＝tan^-12(ω^T(u′×v′)，u′^Tv′)；如果u′＝0，则存在无穷多个解，此时p＝q且两点都在旋转轴上。

如图5所示，对一个双轴旋转角度θ₄，θ₅的求解步骤包括：

(1)假定将点p绕给定轴ξ₅旋转θ₅，再绕给定轴ξ₄旋转θ₄到点q重合；

(2)令q₁是转轴ξ₄上的任一点，由距离保持不变原则得到：

令δ＝||q-q₁||，则

令q₂是转轴ξ₅上任一点，并定义u＝p-q₂，v＝q₁-q₂，则 ${\left|\right|e^{\widehat{{\mathrm{\θ}}_5}{\mathrm{\ξ}}_5}-q_1\left|\right|}^2={\mathrm{\δ}}^2;$

(3)将上述两点向垂直于转轴ξ₅的平面做投影，并定义u′，v′为u，v在垂直于转轴ξ₅的平面上的投影，则u′＝u-ω₂ω₂ ^Tu，v′＝v-ω₂ω₂ ^Tv，其中ω₂为关节旋转的法线，同样对δ投影，得u′²＝δ²-||ω₂ ^T(p-q₁)||²，即

(4)设θ为矢量u′与v′之间的夹角，则θ＝tan^-12(ω^T(u′×v′)，u′^Tv′)，利用余弦定理求解φ＝θ-θ₅，有||u′||²+||v′||²-2||u′||||v′||cosφ＝δ²，因此， ${\mathrm{\θ}}_5=\mathrm{\θ}\±{\cos }^{-1}\left(\frac{{\left|\right|u^{\′}\left|\right|}^2+{\left|\right|v^{\′}\left|\right|}^2-{\mathrm{\δ}}^{\′2}}{2\left|\right|u^{\′}\left|\right|\left|\right|v^{\′}\left|\right|}\right),$ 此式有无解取决于半径为||u′||的圆与半径为δ′²的圆的交点数目；

(5)已知θ₅，则由

求得p₁，再根据

计算θ₄，计算方法与求单轴角度类似，求得双轴旋转角度，即分别对应第四和第五关节的旋转角度θ₄和θ₅。

再根据矩阵齐次关系通用式：

$T_i^{i-1}=\left[\begin{array}{cccc}\cos \left({\mathrm{\θ}}_i\right)& -\sin \left({\mathrm{\θ}}_i\right)& 0& {\mathrm{\α}}_{i-1}\\ \sin \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_{i-1}\right)& \cos \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_{i-1}\right)& -\sin \left({\mathrm{\α}}_{i-1}\right)& -\sin \left({\mathrm{\α}}_{i-1}\right)d_i\\ \sin \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_{i-1}\right)& \cos \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_{i-1}\right)& \cos \left({\mathrm{\α}}_{i-1}\right)& \cos \left({\mathrm{\α}}_{i-1}\right)d_i\\ 0& 0& 0& 1\end{array}\right],$

结合参数可以得到每一个连杆变换矩阵：

$T_1^0=\left[\begin{array}{cccc}{\mathrm{c\θ}}_1& -s{\mathrm{\θ}}_1& 0& 0\\ {\mathrm{s\θ}}_1& {\mathrm{c\θ}}_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$ $T_2^1=\left[\begin{array}{cccc}{\mathrm{c\θ}}_2& -{\mathrm{s\θ}}_2& 0& 0\\ 0& 0& 1& 0\\ -{\mathrm{s\θ}}_2& -{\mathrm{c\θ}}_2& 0& 0\\ 0& 0& 0& 1\end{array}\right],$

$T_3^2=\left[\begin{array}{cccc}{\mathrm{c\θ}}_3& -{\mathrm{s\θ}}_3& 0& {\mathrm{\α}}_2\\ {\mathrm{s\θ}}_3& {\mathrm{c\θ}}_3& 0& 0\\ 0& 0& 1& d_3\\ 0& 0& 0& 1\end{array}\right],$ $T_4^3=\left[\begin{array}{cccc}{\mathrm{c\θ}}_4& -{\mathrm{s\θ}}_4& 0& {\mathrm{\α}}_3\\ 0& 0& 1& d_4\\ -{\mathrm{s\θ}}_4& -{\mathrm{c\θ}}_4& 0& 0\\ 0& 0& 0& 1\end{array}\right],$

$T_5^4=\left[\begin{array}{cccc}{\mathrm{c\θ}}_5& -s{\mathrm{\θ}}_5& 0& 0\\ 0& 0& -1& 0\\ {\mathrm{s\θ}}_5& {\mathrm{c\θ}}_5& 0& 0\\ 0& 0& 0& 1\end{array}\right],$ $T_6^5=\left[\begin{array}{cccc}{\mathrm{c\θ}}_6& -s{\mathrm{\θ}}_6& 0& 0\\ 0& 0& 1& 0\\ -{\mathrm{s\θ}}_6& -{\mathrm{c\θ}}_6& 0& 0\\ 0& 0& 0& 1\end{array}\right],$

根据 $T_6^0=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right]=T_1^0\left({\mathrm{\θ}}_1\right)T_2^1\left({\mathrm{\θ}}_2\right)T_3^2\left({\mathrm{\θ}}_3\right)T_4^3\left({\mathrm{\θ}}_4\right)T_5^4\left({\mathrm{\θ}}_5\right)T_6^5\left({\mathrm{\θ}}_6\right),$

可以得到 $T_6^0=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right]$ 的值，将上式变换为：

$\left[\begin{array}{cccc}{c}_1& s_1& 0& 0\\ -s_1& c_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right]=$

$T_1^0\left({\mathrm{\θ}}_1\right)T_2^1\left({\mathrm{\θ}}_2\right)T_3^2\left({\mathrm{\θ}}_3\right)T_4^3\left({\mathrm{\θ}}_4\right)T_5^4\left({\mathrm{\θ}}_5\right)T_6^5\left({\mathrm{\θ}}_6\right),$ 由于已经通过几何法计算出θ₄，θ₅，θ₆三个数据，联立利用代数消元法即可求出θ₁，θ₂，θ₃。

实施例2

如图6所示，以Kawasaki EE10机器人结构为例说明禁忌搜索法的应用，本实施例与实施例1的区别在于，当机器人需要控制的手腕为非正交球型手腕或者非正交非球型手腕的结构，即机器人连杆的后三个关节的轴线不交于一点时，直接利用几何法和代数消元法求运动学逆解非常困难，因此在计算过程中引入禁忌搜索法。其步骤包括：

(1)建立连杆坐标系：将机器人连杆放入坐标系，固定端为基坐标，依次往后的六个关节轴旋转角度分别设为变量θ₁，θ₂，θ₃，θ₄，θ₅，θ₆；

(2)假设θ₆为已知值，利用几何法和代数消元法求取θ₁，θ₂，θ₃，θ₄，θ₅得到θ₁＝β₁，θ₂＝β₂，θ₃＝β₃，θ₄＝β₄，θ₅＝β₅，其中β₁，β₂，β₃，β₄，β₅为解析解表达式；

(3)根据六自由度机器人运动学正解的求解公式，得到正解表达式

$T_{\′}^6{}_0=\left[\begin{array}{cccc}{r}_{11}^{\′}& r_{12}^{\′}& r_{13}^{\′}& p_x^{\′}\\ r_{21}^{\′}& r_{22}^{\′}& r_{23}^{\′}& p_y^{\′}\\ r_{31}^{\′}& r_{32}^{\′}& r_{33}^{\′}& p_z^{\′}\\ 0& 0& 0& 1\end{array}\right]=T_1^0\left({\mathrm{\β}}_1\right)T_2^1\left({\mathrm{\β}}_2\right)T_3^2\left({\mathrm{\β}}_3\right)T_4^3\left({\mathrm{\β}}_4\right)T_5^4\left({\mathrm{\β}}_5\right)T_6^5\left({\mathrm{\β}}_6\right);$

(4)将机器人连杆的结果位姿与初始位姿进行对比，得表达式

其中为初始位姿；

(5)由于设计机器人结构过程中机器人的行程范围已经确定，所以必然存在能够满足位姿逆解的关节值，故利用禁忌搜索法或爬山法等优化算法以γ为目标，调整θ₆的值使其满足|γ|≤σ，其中σ为误差阈值，得到θ₁，θ₂，θ₃，θ₄，θ₅，θ₆的数字解，具体步骤包括：

(5a)设θ₆为具体数值；

(5b)根据步骤(2)～(4)得到γ的值；

(5c)对比|γ|与误差阈值σ的大小；

(5d)若|γ|≤σ，则该具体数值为θ₆的数字解，若|γ|＞σ，则根据禁忌搜索法或者爬山法优化侧重点，重新设定θ₆为具体数值；

(5e)重复执行步骤(5b)～(5d)，直至得到符合要求的θ₆的数字解，此时，该θ₆对应的由步骤(2)得到的θ₁，θ₂，θ₃，θ₄，θ₅的值即为其相应的数字解。

按照上述实施例，便可很好地实现本发明。

Claims (7)

1.一种六自由度串联机器人运动学逆解的求解方法，其特征在于，当机器人连杆的后三个关节的轴线交于一点时，其步骤包括：

(1)建立连杆坐标系：将机器人连杆放入坐标系，固定端为基坐标，依次往后的六个关节轴旋转角度分别设为变量θ₁，θ₂，θ₃，θ₄，θ₅，θ₆；

(2)利用几何法，即根据Paden-Kahan子问题思想建立与后三个关节相关的函数SubProb_3R(ξ₄，ξ₅，ξ₆，p，q)求解θ₄，θ₅，θ₆的值，其中ξ₄，ξ₅，ξ₆为三个零节距的单位运动旋量，p，q为空间两点；

(3)利用矩阵乘法变换此时已知θ₄，θ₅，θ₆，通过θ₁，θ₂，θ₃三个变量构造三角函数方程，利用代数消元法即可求得θ₁，θ₂，θ₃。

2.根据权利要求1所述的—种六自由度串联机器人运动学逆解的求解方法，其特征在于，所述步骤(2)中，ξ₄，ξ₅，ξ₆的三个空间转轴之间的关系是ξ₄空间垂直于ξ₅和ξ₆，ξ₅空间平行于ξ₆；对θ₄，θ₅，θ₆三个旋转角度的求解包括对一个单轴旋转角度θ₆的求解和对一个双轴旋转角度θ₄，θ₅的求解。

3.根据权利要求2所述的一种六自由度串联机器人运动学逆解的求解方法，其特征在于，对一个单轴旋转角度θ₆的求解步骤包括：

(1)设d为转轴上一点，定义u＝p-d，v＝q-d，根据位置不变原则有： $e^{\widehat{{\mathrm{\θ}}_6}{\mathrm{\ξ}}_6}p=q,$ $e^{\widehat{{\mathrm{\θ}}_6}{\mathrm{\ξ}}_6}d=d,$ 则 $e^{\widehat{{\mathrm{\θ}}_6}{\mathrm{\ξ}}_6}u=v;$

(2)定义u′，v′为u，v在垂直于转轴ξ的平面上的投影，则u′＝u-ωω^Tu，v′＝v-ωω^Tv，其中ω为关节旋转的法线；

(3)当且仅当u，v在轴上的投影等长时，在轴ω上垂直的平面上投影也等长，即ω^Tu＝ω^Tv，||u′||＝||v′||，那么根据式子

和投影矢量u′，v′求得旋转角度，即对应第六关节的旋转角度θ₆。

4.根据权利要求3所述的一种六自由度串联机器人运动学逆解的求解方法，其特征在于，对一个双轴旋转角度θ₄，θ₅的求解步骤包括：

(1)假定将点p绕给定轴ξ₅旋转θ₅，再绕给定轴ξ₄旋转θ₄到点q重合；

(2)令q₁是转轴ξ₄上的任一点，由距离保持不变原则得到：

令δ＝||q-q₁||，则

令q₂是转轴ξ₅上任一点，并定义u＝p-q₂，v＝q₁-q₂，则 ${\left|\right|e^{\widehat{{\mathrm{\θ}}_5}{\mathrm{\ξ}}_5}-q_1\left|\right|}^2={\mathrm{\δ}}^2;$

(3)将上述两点向垂直于转轴ξ₅的平面做投影，并定义u′，v′为u，v在垂直于转轴ξ₅的平面上的投影，则u′＝u-ω₂ω₂ ^Tu，v′＝v-ω₂ω₂ ^Tv，其中ω₂为关节旋转的法线，同样对δ投影，得u′²＝δ²-||ω₂ ^T(p-q₁)||²，即

(4)设θ为矢量u′与v′之间的夹角，则θ＝tan^-12(ω^T(u′×v′)，u′^Tv′)，利用余弦定理求解φ＝θ-θ₅，有||u′||²+||v′||²-2||u′||v′||cosφ＝δ²，因此， ${\mathrm{\θ}}_5=\mathrm{\θ}\±{\cos }^{-1}\left(\frac{{\left|\right|u^{\′}\left|\right|}^2+{\left|\right|v^{\′}\left|\right|}^2-{\mathrm{\δ}}^{\′2}}{2\left|\right|u^{\′}\left|\right|\left|\right|v^{\′}\left|\right|}\right);$

(5)已知θ₅，则由求得p₁，再根据

计算θ₄，得双轴旋转角度，即分别对应第四和第五关节的旋转角度θ₄和θ₅。

5.一种六自由度串联机器人运动学逆解的求解方法，其特征在于，当机器人连杆的后三个关节的轴线不交于一点时，其步骤包括：

(1)建立连杆坐标系：将机器人连杆放入坐标系，固定端为基坐标，依次往后的六个关节轴旋转角度分别设为变量θ₁，θ₂，θ₃，θ₄，θ₅，θ₆；

(2)假设θ₆为已知值，利用几何法和代数消元法求取θ₁，θ₂，θ₃，θ₄，θ₅得到θ₁＝β₁，θ₂＝β₂，θ₃＝β₃，θ₄＝β₄，θ₅＝β₅，其中β₁，β₂，β₃，β₄，β₅为解析解表达式；

(3)根据六自由度机器人运动学正解的求解公式，得到正解表达式 $T_{\′}^6{}_0=\left[\begin{array}{cccc}{r}_{11}^{\′}& r_{12}^{\′}& r_{13}^{\′}& p_x^{\′}\\ r_{21}^{\′}& r_{22}^{\′}& r_{23}^{\′}& p_y^{\′}\\ r_{31}^{\′}& r_{32}^{\′}& r_{33}^{\′}& p_z^{\′}\\ 0& 0& 0& 1\end{array}\right]=T_1^0\left({\mathrm{\β}}_1\right)T_2^1\left({\mathrm{\β}}_2\right)T_3^2\left({\mathrm{\β}}_3\right)T_4^3\left({\mathrm{\β}}_4\right)T_5^4\left({\mathrm{\β}}_5\right)T_6^5\left({\mathrm{\β}}_6\right);$

(4)将机器人连杆的结果位姿与初始位姿进行对比，得表达式

其中

为初始位姿；

(5)利用优化算法以γ为目标，调整θ₆的值使其满足|γ|≤σ，其中σ为误差阈值，得到θ₁，θ₂，θ₃，θ₄，θ₅，θ₆的数字解。

6.根据权利要求5所述的一种六自由度串联机器人运动学逆解的求解方法，其特征在于，所述步骤(5)中，所述优化算法为禁忌搜索法或爬山法。

7.根据权利要求6所述的一种六自由度串联机器人运动学逆解的求解方法，其特征在于，所述步骤(5)具体包括以下步骤：

(5a)设θ₆为具体数值；

(5b)根据步骤(2)～(4)得到γ的值；

(5c)对比|γ|与误差阈值σ的大小；

(5d)若|γ|≤σ，则该具体数值为θ₆的数字解，若|γ|＞σ，则根据禁忌搜索法或者爬山法重新设定θ₆为具体数值；

(5e)重复执行步骤(5b)～(5d)，直至得到符合要求的θ₆的数字解，此时，该θ₆对应的由步骤(2)得到的θ₁，θ₂，θ₃，θ₄，θ₅的值即为其相应的数字解。

Images