对工业机器人进行运动学分析包括正向运动学分析和反向运动学分析，运动学反解问题是当前研究的热点和难点。本文以旋量理论和几何法为基础，结合Paden-Kahan子问题解法，给出一种机器人运动学反解算法。以某六轴串联工业机器人为例，给定一组关节角度，求解出其运动学正解，以此为基础得到若干组运动学反解作为理论值，然后利用上述基于旋量理论的反解算法验证其有效性。典型应用结果表明：该机器人运动学反解算法，可避免D-H参数法中因为关节轴平行引起的奇异性问题，不仅满足对工业机器人重复定位精度的要求，并能够使机器人运动更加平稳。 The kinematics analysis of industrial robots includes forward kinematics analysis and reverse kinematics analysis. The problem of inverse kinematics solution is a hot and difficult point. Based on the screw theory and geometry method, and combined with Paden-Kahan sub-problem solution, this paper presents a kinematic inverse solution algorithm for robots. Taking a six-axis series industrial robot as an example, given a set of joint angles, the kinematic positive solution is solved. And then several sets of kinematic inverse solutions are obtained as theoretical values. Then, the above inverse solution algorithm based on the screw theory is used to verify its effectiveness. The typical application results show that the inverse kinematics solution algorithm can avoid the sin-gularity problem caused by the parallel joint axis in the D-H parameter method, which not only meets the requirement of repeated positioning accuracy of industrial robot, but also can make the robot move more stable.
孟原1，郝世鹏1，史宝军1,2*
1山东建筑大学机电工程学院，山东 济南
2河北工业大学机械工程学院，天津
收稿日期：2020年4月13日；录用日期：2020年4月17日；发布日期：2020年4月28日
对工业机器人进行运动学分析包括正向运动学分析和反向运动学分析，运动学反解问题是当前研究的热点和难点。本文以旋量理论和几何法为基础，结合Paden-Kahan子问题解法，给出一种机器人运动学反解算法。以某六轴串联工业机器人为例，给定一组关节角度，求解出其运动学正解，以此为基础得到若干组运动学反解作为理论值，然后利用上述基于旋量理论的反解算法验证其有效性。典型应用结果表明：该机器人运动学反解算法，可避免D-H参数法中因为关节轴平行引起的奇异性问题，不仅满足对工业机器人重复定位精度的要求，并能够使机器人运动更加平稳。
关键词 :旋量理论，Paden-Kahan子问题，运动学反解，工业机器人
Copyright © 2020 by author(s) and beplay安卓登录
This work is licensed under the Creative Commons Attribution International License (CC BY 4.0).
http://creativecommons.org/licenses/by/4.0/
工业机器人的运动学求解通常包括正解和反解两个过程。正解是通过给定机器人各个关节的转动角度，然后通过运动学方程求解出工业机器人末端部件的坐标位置以及机器人的姿态；而反解是根据机器人末端部件在空间中的位置和姿态，反向求解出此位置和姿态下各个关节需要运动的角度。反解问题也是当前工业机器人运动学分析的重点和难点。以串联工业机器人为例，其常见的反解的求解方法有：数值法、解析法、几何法等 [
进行工业机器人运动学分析，不论采用何种分析方法，都需要先建立机器人的运动学模型，目前主要运用的建模方法有：D-H参数法 [
本文基于旋量理论法建立机器人运动学模型，主要是以各个关节运动副的旋转轴或运动方向轴线为基础来完成模型构建，结合Paden-Kahan子问题解法，给出一种机器人运动学反解算法。文中以某型号的串联工业机器人为例，验证了所构建模型与反解算法的有效性。
Chasles定理 [
图1. 螺旋运动
在考虑刚体在空间中位姿的表示时，设S表示绝对坐标系，T为固定于刚体上的相对坐标系。则刚体在S上的位姿可以通过T对S的相对位姿来描述，这种变换矩阵的集合表示为：
{ S E ( 3 ) = { [ R t 0 1 ] } ; R ∈ R 3 × 3 t ∈ R 3 × 3 , R T R = I , det ( R ) = 1 (1)
式中， S E ( 3 ) 为三维空间中刚体变换的特殊欧式群，即李群；t为相对坐标系相对绝对坐标系的位置，R为相对坐标系相对绝对坐标系的姿态。用 s e ( 3 ) 表示李群 S E ( 3 ) 的李代数， ω = ( ω x , ω y , ω z ) 为刚体绕定轴旋转时旋转方向的单位矢量，旋转的角度为 θ ， q = ( q x , q y , q z ) 为旋转轴上的一定点。其中的元素定义如下：
ξ ^ = [ ω ^ v 0 0 ] ∈ s e ( 3 ) (2)
ξ = [ ω T v T ] T (3)
式中，v是3行的列矢量， v = − ω × q ， ω ^ 为3阶的反对称矩阵。
ω ^ = ( 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 )
ξ ^ 称为运动旋量，是一个4阶的矩阵。 ξ 为运动旋量 ξ ^ 的旋量坐标，是一个6行的列向量。
由旋量理论可知，刚体的螺旋运动，可以采用运动旋量的指数坐标来表示，即任何刚体运动 g ∈ s e ( 3 ) ，都有 g = e ξ ^ θ 。
e ξ ^ θ = { [ e ω ^ θ ( I − e ω ^ θ ) ( ω × v ) + θ ω ω T v 0 1 ] , ω ≠ 0 [ I θ v 0 1 ] , ω = 0 (4)
式中， e ω ^ θ = I + ω ^ t + ( ω ^ t ) 2 2 ! + ( ω ^ t ) 3 3 ! + ⋯ = I + ω ^ sin θ + ω ^ 2 ( 1 − cos θ ) 。
这样，可以得到在刚体上的相对坐标系相对于绝对坐标系的位姿变换用刚体运动 g s t ( θ ) 的表示，以 g s t ( 0 ) 表示相对坐标系相对于绝对坐标系的初始位姿，那么刚体相对于某一特定轴做螺旋运动，就可以表示为：
g s t ( θ ) = exp ( ξ ^ θ ) g s t ( 0 ) (5)
以某型号六关节串联机器人为例，其结构简图如图2所示。该型机器人共有6个关节，在前3个关节中，关节2、3的轴线平行，并与关节1的轴线异面垂直；其后3个关节轴线相交于一点。
图2. 某串联工业机器人结构简图
以 ω i 和 q i 分别计算相对应各连杆的运动旋量 ξ ^ i 、运动旋量坐标 ξ i 和相对应旋转运动的指数积分 exp ( ξ ^ i θ i ) ，然后根据刚体运动的指数形式，利用指数积分形式表示该机器人基于旋量理论的运动学正解为：
g s t ( θ ) = exp ( ξ ^ 1 θ 1 ) exp ( ξ ^ 2 θ 2 ) exp ( ξ ^ 3 θ 3 ) exp ( ξ ^ 4 θ 4 ) exp ( ξ ^ 5 θ 5 ) exp ( ξ ^ 6 θ 6 ) g s t ( 0 ) (6)
在进行机器人运动学反解求解时，若是纯转动问题，需要遵守三个原则：位置不变原则、距离不变原则、姿态不变原则 [
以图2所示某型号六轴工业机器人为例，对其进行运动学反解的求解，即在某一位置和姿态下，求解其6个关节轴的角度。
首先求出前3个关节的角度，然后定义后3个关节轴的交点为，根据旋量理论有：
exp ( ξ ^ 4 θ 4 ) exp ( ξ ^ 5 θ 5 ) exp ( ξ ^ 6 θ 6 ) = q 4 (7)
将公式(7)代入公式(6)进行解耦，等号两边同右乘 g s t − 1 ( 0 ) q 4 得：
exp ( ξ ^ 1 θ 1 ) exp ( ξ ^ 2 θ 2 ) exp ( ξ ^ 3 θ 3 ) q 4 = g 1 q 4 = p 1 (8)
式中： g 1 = g s t ( θ ) g s t − 1 ( 0 ) ， p 1 = [ p 1 x , p 1 y , p 1 z ] T 。
通过解耦，将机器人运动学反解问题，转化为前3个关节角和后3个关节角的求解问题，其中前3个关节的运动为从点 q 4 出发，经过旋转运动到达终点 p 1 ，如图3所示。
图3. 前3个关节的螺旋运动图
具体推导过程省略，这里直接给出前3个关节角的计算公式如下：
θ 1 = atan 2 ( ± p 1 x 2 , ± p 1 y 2 ) (9)
θ 2 = atan 2 ( ± p 1 x 2 + p 1 y 2 − a 1 , p 1 z − d 1 ) ∓ arccos ( a 1 ∓ p 1 x 2 + p 1 y 2 ) 2 + ( d 1 − p 1 z ) 2 + ( d 1 − p 1 z ) 2 + a 2 2 − d 4 2 − a 3 2 2 a 2 ( a 1 ∓ p 1 x 2 + p 1 y 2 ) 2 + ( d 1 − p 1 z ) 2 (10)
θ 3 = atan 2 ( d 4 , − a 3 1 ) ∓ arccos a 2 2 + d 4 2 + a 3 2 − ( a 1 ∓ p 1 x 2 + p 1 y 2 ) 2 − ( d 1 − p 1 z ) 2 2 a 2 d 4 2 + a 3 2 (11)
然后，基于Paden-Kahan子问题解法，具体推导过程省略，直接给出求解后3个关节角度的计算公式如下：
θ 4 = atan 2 [ ∓ p 2 y , ± ( p 2 z − d 1 − a 2 − a 3 ) ] (12)
θ 5 = atan 2 [ ∓ d 5 2 − ( p 2 x − a 1 − d 4 ) 2 , ( p 2 x − a 1 − d 4 ) ] (13)
θ 6 = atan 2 ( p 3 y , d 1 + a 2 + a 3 − p 3 z ) (14)
首先，将上述机器人运动反解算法应用于机器人末端位置参数的求解。图2所示六轴工业机器人，做正向运动时的关节角度如表1所示。
| 关节角 | θ 1 | θ 2 | θ 3 | θ 4 |
|
θ 6 |
|---|---|---|---|---|---|---|
| 角度 | π 2 | π 4 | π 3 | π 2 | π 6 | 2 π 3 |
表1. 工业机器人各个关节角度
将上述的关节角度代入机器人正向运动学方程，得到其末端部件的位置参数如表2所示。其中，n、o、a矩阵分别为相对坐标系的x、y、z轴对于参考坐标系的方向余弦；p为相对坐标系对于参考坐标系的位置向量。
| n | o | a | p |
|---|---|---|---|
| 0.28885 | −0.94859 | −0.12941 | 497.54 |
| −0.43301 | −0.25 | 0.86603 | −2.2737e-13 |
| −0.85385 | −0.19411 | −0.48296 | 1374.8 |
表2. 末端部件的位置参数
利用表2中得到的末端位置参数，结合本文给出的基于旋量理论和Paden-Kahan子问题得到的反解公式，可得到6个关节的反解。通过多次运算并排除奇异性解，得到8组反解，对应的各个关节的角度汇总如表3所示。
观察表3中的各个关节轴的角度可以发现，第一组数据与表1中给定的初始数据较为接近，因此选择表3中的第一组数据代入正向运动学方程中，最后得到的末端部件位置参数如表4所示。
将表2与表4中的位置参数数据进行对比，可以得到基于旋量理论的反解结果与理论值之间的误差值，如表5所示。
| 序号 | θ 1 ( ∘ ) | θ 2 ( ∘ ) | θ 3 ( ∘ ) | θ 4 ( ∘ ) | θ 5 ( ∘ ) | θ 6 ( ∘ ) |
|---|---|---|---|---|---|---|
| 1 | 90.0000 | 44.9979 | 60.0039 | 90.0021 | 29.9998 | −120.0002 |
| 2 | 90.0000 | 25.2156 | −263.3225 | −64.8855 | 28.5824 | 129.8278 |
| 3 | 90.0000 | 16.3985 | 14.6339 | −64.8855 | 28.5824 | 41.4952 |
| 4 | 90.0000 | −52.8691 | −217.9525 | −132.7481 | 27.903 | 66.0441 |
| 5 | −90.0000 | 44.9979 | 60.0039 | 36.4957 | 11.3563 | 55.4714 |
| 6 | −90.0000 | 25.2156 | −263.3225 | 36.7612 | −8.0598 | 52.2931 |
| 7 | −90.0000 | 16.3985 | 14.6339 | 103.5254 | 21.0919 | 71.6332 |
| 8 | −90.0000 | −52.8691 | −217.9525 | 65.6932 | 28.9971 | 81.1997 |
表3. 运动学反解求解得到的各个关节角度
| n | o | a | p |
|---|---|---|---|
| 0.288840053917237 | −0.948584702031102 | −0.129454572440119 | 497.538891432996 |
| −0.432990884874931 | −0.250031741244792 | 0.866027148526720 | −1.13686837721616e-13 |
| −0.853867856795366 | −0.194090678402296 | −0.482947711133435 | 1374.80071059150 |
表4. 由第1组反解结果求得的末端部件位置参数
| n | o | a | p |
|---|---|---|---|
| 8.57508276325136e-06 | 3.53596889834940e-06 | 4.50494401184887e-05 | 0.00380593299615839 |
| 2.18171250693233e-05 | 3.17412447920495e-05 | 1.74452672008218e-06 | 1.13313162278384e-13 |
| 1.39647953664168e-05 | 2.36055977041461e-05 | 1.52018665645626e-05 | 0.00518659150179701 |
表5. 位置参数误差
由表5中的误差数据可以看到，这里的位置参数误差的数量级均在10−5以下，而对于大型工业机器人重复定位精度的要求一般在0.2 mm左右，例如根据KUKA机器人的使用说明书，它规定机器人重复定位精度为0.15 mm [
其次，将上述机器人运动反解算法应用于机器人运动平稳性分析。机器人的运动平稳性，即机器人在运动过程中的速度/加速度变化是否平稳，我们选择加速度为评价指标，采用基于旋量理论的反解算法，求解机器人加速度变化情况，并与D-H算法得到的反解结果进行比较。图4给出了两种反解算法求得的第4关节的最大加速度变化情况，图5给出了两种反解算法求得的第4关节的最小加速度变化情况。从图4和图5中可以明显地看出，利用旋量理论法求得的关节加速度相较于D-H参数法更加平稳。
图4. 第4关节处两种反解方法对应的最大加速度对比
图5. 第4关节处两种反解方法对应的最小加速对比
本文基于旋量理论和几何法，建立机器人运动学模型，结合Paden-Kahan子问题，构建了一种用于机器人运动学反解的算法。以某六轴串联工业机器人为例，从机器人运动精度和运动稳定性方面，检验了该算法的有效性，并与D-H参数法进行了比较。
典型应用结果表明：本文给出的基于旋量理论和Paden-Kahan子问题的机器人运动学反解算法，避免了D-H参数法中因为关节轴平行造成的奇异性问题，不仅满足工业机器人重复定位精度的要求，而且可获得更好的运动平稳性。
上述机器人运动反解方法对其它工业机器人的适用性，有待后续开展更深入的研究。
国家重点研发计划(2017YFB1301300)；河北省自然科学基金(E2019202338)。
孟 原,郝世鹏,史宝军. 基于旋量理论和Paden-Kahan子问题的机器人反解算法Inverse-Solution Algorithm of Robot Based on Screw Theory and Paden-Kahan Sub-Problem[J]. 机械工程与技术, 2020, 09(02): 163-170. https://doi.org/10.12677/MET.2020.92017
https://doi.org/10.1109/CDC.1978.268148
https://doi.org/10.1109/70.326569
https://doi.org/10.1142/4257