【现代机器人学——螺旋理论】UR5机器人正运动学求解
系列文章目录
【现代机器人学——螺旋理论】UR5机器人正运动学求解
【现代机器人学——螺旋理论】UR5机器人逆运动学求解
提示：本文介绍基于螺旋理论分析的UR5机器人正运动学求解思路，仅供参考。
文章目录
- 系列文章目录
- 前言
- 一、参考机器人坐标
- 二、正运动学
-
- 1.定义工作末端
- 2.定义运动轴
- 3.定义螺旋变换矩阵
- 4.基于螺旋理论的UR5机器人正向运动学公式
- 三、简单的代码实现
- 总结
前言
”让经典旋量理论惠及更多的人群’这—突破发生在20世纪80年代初°哈佛大学的RogerBrockett基于刚体运动的李群结构向读者展示了如何以数学方式来描述运动链（Brockett, l983b）。该发现的重要性在于’只是简单地通过线性代数与线性微分方程的基本理论来重构旋量理论。有了“现代旋量理论’’这一现代微分几何领域的强大工具’就可以涉及范围更为广泛的机器人问题’其中部分内容将展现在我们这本教材中’其他更优秀，更高级的内容可参考Murray等（l994）的教科书。“
——摘自《MODERN ROBOTICS MECHANICS, PLANNING, AND CONTROL》
简单说就是把课程大作业顺便整理发了
参考文章：
《MODERN ROBOTICS MECHANICS, PLANNING, AND CONTROL》
《UR机械臂正逆运动学求解》与此文章理论方法、坐标系以及参数定义不太相同，可将xy方向数据取反验证。
一、参考机器人坐标
| 变量 | 数值 |
|---|---|
| L 1 L_1 L1 | 0.425 |
| L 2 L_2 L2 | 0.3922 |
| W 1 W_1 W1 | 0.1333 |
| W 2 W_2 W2 | 0.0996 |
| H 1 H_1 H1 | 0.1625 |
| H 2 H_2 H2 | 0.0997 |
二、正运动学
1.定义工作末端
首先我们针对旋量理论支撑的运动模型末端进行定义
g s t ( 0 ) = [ L 1 + L 2 W 1 + W 2 H 1 − H 2 1 ] {g_{st}}\left( 0 \right) = \left[ {\begin{array}{ccc} {{L_1} + {L_2}}\\ {{W_1} + {W_2}}\\ {{H_1} - {H_2}}\\ 1 \end{array}} \right] gst(0)=⎣⎢⎢⎡L1+L2W1+W2H1−H21⎦⎥⎥⎤
其矩阵形式表示为
M = [ R [ L 1 + L 2 W 1 + W 2 H 1 − H 2 ] 0 1 ] = [ − 1 0 0 L 1 + L 2 0 0 1 W 1 + W 2 0 1 0 H 1 − H 2 0 0 0 1 ] M = \left[ {\begin{array}{ccc} R&{\left[ {\begin{array}{ccc} {{L_1} + {L_2}}\\ {{W_1} + {W_2}}\\ {{H_1} - {H_2}} \end{array}} \right]}\\ 0&1 \end{array}} \right] = \left[ {\begin{array}{ccc} { - 1}&0&0&{{L_1} + {L_2}}\\ 0&0&1&{{W_1} + {W_2}}\\ 0&1&0&{{H_1} - {H_2}}\\ 0&0&0&1 \end{array}} \right] M=⎣⎢⎢⎡R0⎣⎡L1+L2W1+W2H1−H2⎦⎤1⎦⎥⎥⎤=⎣⎢⎢⎡−100000100100L1+L2W1+W2H1−H21⎦⎥⎥⎤
2.定义运动轴
UR5机器人共拥有6个旋转轴，分辨根据其参数进行定义
螺旋方向
ξ 1 = [ 0 0 1 ] , ξ 2 = [ 0 1 0 ] , ξ 3 = [ 0 1 0 ] , ξ 4 = [ 0 1 0 ] , ξ 5 = [ 0 0 − 1 ] , ξ 6 = [ 0 1 0 ] {\xi _1} = \left[ {\begin{array}{ccc} 0\\ 0\\ 1 \end{array}} \right],{\xi _2} = \left[ {\begin{array}{ccc} 0\\ 1\\ 0 \end{array}} \right],{\xi _3} = \left[ {\begin{array}{ccc} 0\\ 1\\ 0 \end{array}} \right],{\xi _4} = \left[ {\begin{array}{ccc} 0\\ 1\\ 0 \end{array}} \right],{\xi _5} = \left[ {\begin{array}{ccc} 0\\ 0\\ { - 1} \end{array}} \right],{\xi _6} = \left[ {\begin{array}{ccc} 0\\ 1\\ 0 \end{array}} \right] ξ1=⎣⎡001⎦⎤,ξ2=⎣⎡010⎦⎤,ξ3=⎣⎡010⎦⎤,ξ4=⎣⎡010⎦⎤,ξ5=⎣⎡00−1⎦⎤,ξ6=⎣⎡010⎦⎤
螺旋方向
q 1 = [ 0 0 H 1 ] , q 2 = [ 0 0 H 1 ] , q 3 = [ L 1 0 H 1 ] , q 4 = [ L 1 + L 2 0 H 1 ] , q 5 = [ L 1 + L 2 W 1 H 1 ] , q 6 = [ L 1 + L 2 W 1 H 1 − H 2 ] {q_1} = \left[ {\begin{array}{ccc} 0\\ 0\\ {{H_1}} \end{array}} \right]\;,{q_2} = \left[ {\begin{array}{ccc} 0\\ 0\\ {{H_1}} \end{array}} \right]\;,{q_3} = \left[ {\begin{array}{ccc} {{L_1}}\\ 0\\ {{H_1}} \end{array}} \right]\;,{q_4} = \left[ {\begin{array}{ccc} {{L_1} + {L_2}}\\ 0\\ {{H_1}} \end{array}} \right]\;,{q_5} = \left[ {\begin{array}{ccc} {{L_1} + {L_2}}\\ {{W_1}}\\ {{H_1}} \end{array}} \right]\;,{q_6} = \left[ {\begin{array}{ccc} {{L_1} + {L_2}}\\ {{W_1}}\\ {{H_1} - {H_2}} \end{array}} \right] q1=⎣⎡00H1⎦⎤,q2=⎣⎡00H1⎦⎤,q3=⎣⎡L10H1⎦⎤,q4=⎣⎡L1+L20H1⎦⎤,q5=⎣⎡L1+L2W1H1⎦⎤,q6=⎣⎡L1+L2W1H1−H2⎦⎤
螺旋方向
v 1 = [ 0 0 0 ] , v 2 = [ − H 1 0 0 ] , v 3 = [ − H 1 0 L 1 ] , v 4 = [ − H 1 0 L 1 + L 2 ] , v 5 = [ − W 1 L 1 + L 2 0 ] , v 6 = [ H 2 − H 1 0 L 1 + L 2 ] {v_1} = \left[ {\begin{array}{ccc} 0\\ 0\\ 0 \end{array}} \right]\;,{v_2} = \left[ {\begin{array}{ccc} { - {H_1}}\\ 0\\ 0 \end{array}} \right]\;,{v_3} = \left[ {\begin{array}{ccc} { - {H_1}}\\ 0\\ {{L_1}} \end{array}} \right]\;,{v_4} = \left[ {\begin{array}{ccc} { - {H_1}}\\ 0\\ {{L_1} + {L_2}} \end{array}} \right]\;,{v_5} = \left[ {\begin{array}{ccc} { - {W_1}}\\ {{L_1} + {L_2}}\\ 0 \end{array}} \right]\;,{v_6} = \left[ {\begin{array}{ccc} {{H_2} - {H_1}}\\ 0\\ {{L_1} + {L_2}} \end{array}} \right] v1=⎣⎡000⎦⎤,v2=⎣⎡−H100⎦⎤,v3=⎣⎡−H10L1⎦⎤,v4=⎣⎡−H10L1+L2⎦⎤,v5=⎣⎡−W1L1+L20⎦⎤,v6=⎣⎡H2−H10L1+L2⎦⎤
3.定义螺旋变换矩阵
e [ S 1 ] θ 1 = [ c 1 − s 1 0 0 s 1 c 1 0 0 0 0 1 0 0 0 0 1 ] {e^{\left[ {{S_1}} \right]{\theta _1}}} = \left[ {\begin{array}{ccc} {{c_1}}&{ - {s_1}}&0&0\\ {{s_1}}&{{c_1}}&0&0\\ 0&0&1&0\\ 0&0&0&1 \end{array}} \right] e[S1]θ1=⎣⎢⎢⎡c1s100−s1c10000100001⎦⎥⎥⎤
e [ S 2 ] θ 2 = [ c 2 0 s 2 − H 1 s 2 0 1 0 0 − s 2 0 c 2 H 1 ( 1 − c 2 ) 0 0 0 1 ] {e^{\left[ {{S_2}} \right]{\theta _2}}} = \left[ {\begin{array}{ccc} {{c_2}}&0&{{s_2}}&{ - {H_1}{s_2}}\\ 0&1&0&0\\ { - {s_2}}&0&{{c_2}}&{{H_1}(1 - {c_2})}\\ 0&0&0&1 \end{array}} \right] e[S2]θ2=⎣⎢⎢⎡c20−s200100s20c20−H1s20H1(1−c2)1⎦⎥⎥⎤
e [ S 3 ] θ 3 = [ c 3 0 s 3 L 1 ( 1 − c 3 ) − H 1 s 3 0 1 0 0 − s 3 0 c 3 H 1 ( 1 − c 3 ) + L 1 s 3 0 0 0 1 ] {e^{\left[ {{S_3}} \right]{\theta _3}}} = \left[ {\begin{array}{ccc} {{c_3}}&0&{{s_3}}&{{L_1}\left( {1 - {c_3}} \right) - {H_1}{s_3}}\\ 0&1&0&0\\ { - {s_3}}&0&{{c_3}}&{{H_1}\left( {1 - {c_3}} \right) + {L_1}{s_3}}\\ 0&0&0&1 \end{array}} \right] e[S3]θ3=⎣⎢⎢⎡c30−s300100s30c30L1(1−c3)−H1s30H1(1−c3)+L1s31⎦⎥⎥⎤
e [ S 4 ] θ 4 = [ c 4 0 s 4 ( L 1 + L 2 ) ( 1 − c 4 ) − H 1 s 4 0 1 0 0 − s 4 0 c 4 H 1 ( 1 − c 4 ) + ( L 1 + L 2 ) s 4 0 0 0 1 ] {e^{\left[ {{S_4}} \right]{\theta _4}}} = \left[ {\begin{array}{ccc} {{c_4}}&0&{{s_4}}&{({L_1} + {L_2})\left( {1 - {c_4}} \right) - {H_1}{s_4}}\\ 0&1&0&0\\ { - {s_4}}&0&{{c_4}}&{{H_1}\left( {1 - {c_4}} \right) + ({L_1} + {L_2}){s_4}}\\ 0&0&0&1 \end{array}} \right] e[S4]θ4=⎣⎢⎢⎡c40−s400100s40c40(L1+L2)(1−c4)−H1s40H1(1−c4)+(L1+L2)s41⎦⎥⎥⎤
e [ S 5 ] θ 5 = [ c 5 s 5 0 ( L 1 + L 2 ) ( 1 − c 5 ) − W 1 s 5 − s 5 c 5 0 W 1 ( 1 − c 5 ) + ( L 1 + L 2 ) s 5 0 0 1 0 0 0 0 1 ] {e^{\left[ {{S_5}} \right]{\theta _5}}} = \left[ {\begin{array}{ccc} {{c_5}}&{{s_5}}&0&{\left( {{L_1} + {L_2}} \right)\left( {1 - {c_5}} \right) - {W_1}{s_5}}\\ { - {s_5}}&{{c_5}}&0&{{W_1}\left( {1 - {c_5}} \right) + \left( {{L_1} + {L_2}} \right){s_5}}\\ 0&0&1&0\\ 0&0&0&1 \end{array}} \right] e[S5]θ5=⎣⎢⎢⎡c5−s500s5c5000010(L1+L2)(1−c5)−W1s5W1(1−c5)+(L1+L2)s501⎦⎥⎥⎤
e [ S 6 ] θ 6 = [ c 6 0 s 6 ( L 1 + L 2 ) ( 1 − c 6 ) − ( H 1 − H 2 ) s 6 0 1 0 0 − s 6 0 c 6 ( H 1 − H 2 ) ( 1 − c 6 ) + ( L 1 + L 2 ) s 6 0 0 0 1 ] {e^{\left[ {{S_6}} \right]{\theta _6}}} = \left[ {\begin{array}{ccc} {{c_6}}&0&{{s_6}}&{({L_1} + {L_2})\left( {1 - {c_6}} \right) - ({H_1} - {H_2}){s_6}}\\ 0&1&0&0\\ { - {s_6}}&0&{{c_6}}&{({H_1} - {H_2})\left( {1 - {c_6}} \right) + ({L_1} + {L_2}){s_6}}\\ 0&0&0&1 \end{array}} \right] e[S6]θ6=⎣⎢⎢⎡c60−s600100s60c60(L1+L2)(1−c6)−(H1−H2)s60(H1−H2)(1−c6)+(L1+L2)s61⎦⎥⎥⎤
4.基于螺旋理论的UR5机器人正向运动学公式
T ( θ ) = e [ S 1 ] θ 1 e [ S 2 ] θ 2 e [ S 3 ] θ 3 e [ S 4 ] θ 4 e [ S 5 ] θ 5 e [ S 6 ] θ 6 M T(\theta ) = {e^{\left[ {{S_1}} \right]{\theta _1}}}{e^{\left[ {{S_2}} \right]{\theta _2}}}{e^{\left[ {{S_3}} \right]{\theta _3}}}{e^{\left[ {{S_4}} \right]{\theta _4}}}{e^{\left[ {{S_5}} \right]{\theta _5}}}{e^{\left[ {{S_6}} \right]{\theta _6}}}M T(θ)=e[S1]θ1e[S2]θ2e[S3]θ3e[S4]θ4e[S5]θ5e[S6]θ6M
完全表达式（可能有误，应该不会有人用这个吧，不会吧不会吧）
[ r 11 r 12 r 13 p 1 r 21 r 22 r 23 p 2 r 31 r 32 r 33 p 3 0 0 0 1 ] = [ − c 1 c 234 c 5 c 6 + c 1 s 234 s 6 + s 1 s 5 c 6 − c 1 c 234 c 5 s 6 + c 1 s 234 c 6 + s 1 s 5 s 6 c 1 c 234 s 5 + s 1 c 5 L 2 c 1 c 23 + L 1 c 1 c 2 − H 2 c 1 s 234 + W 2 c 1 c 234 s 5 + W 1 s 1 + W 2 s 1 c 5 s 1 c 234 c 5 c 6 − s 1 s 234 s 6 + c 1 s 5 c 6 s 1 c 234 c 5 s 6 − s 1 s 234 c 6 + c 1 s 5 s 6 − s 1 c 234 s 5 + c 1 c 5 − L 2 s 1 c 23 − L 1 s 1 c 2 + H 2 s 1 s 234 − W 2 s 1 c 234 s 5 + W 1 c 1 + W 2 c 1 c 5 s 234 c 5 c 6 + c 234 s 6 s 234 c 5 s 6 + c 234 c 6 − s 234 s 5 H 1 − H 2 c 234 − L 2 s 23 − L 1 s 2 − W 2 s 234 s 5 0 0 0 1 ] \left[ {\begin{array}{ccc} {{r_{11}}}&{{r_{12}}}&{{r_{13}}}&{{p_1}}\\ {{r_{21}}}&{{r_{22}}}&{{r_{23}}}&{{p_2}}\\ {{r_{31}}}&{{r_{32}}}&{{r_{33}}}&{{p_3}}\\ 0&0&0&1 \end{array}} \right] = \left[ {\begin{array}{ccc} { - {c_1}{c_{234}}{c_5}{c_6} + {c_1}{s_{234}}{s_6} + {s_1}{s_5}{c_6}}&{ - {c_1}{c_{234}}{c_5}{s_6} + {c_1}{s_{234}}{c_6} + {s_1}{s_5}{s_6}}&{{c_1}{c_{234}}{s_5} + {s_1}{c_5}}&{{L_2}{c_1}{c_{23}} + {L_1}{c_1}{c_2} - {H_2}{c_1}{s_{234}} + {W_2}{c_1}{c_{234}}{s_5} + {W_1}{s_1} + {W_2}{s_1}{c_5}}\\ {{s_1}{c_{234}}{c_5}{c_6} - {s_1}{s_{234}}{s_6} + {c_1}{s_5}{c_6}}&{{s_1}{c_{234}}{c_5}{s_6} - {s_1}{s_{234}}{c_6} + {c_1}{s_5}{s_6}}&{ - {s_1}{c_{234}}{s_5} + {c_1}{c_5}}&{ - {L_2}{s_1}{c_{23}} - {L_1}{s_1}{c_2} + {H_2}{s_1}{s_{234}} - {W_2}{s_1}{c_{234}}{s_5} + {W_1}{c_1} + {W_2}{c_1}{c_5}}\\ {{s_{234}}{c_5}{c_6} + {c_{234}}{s_6}}&{{s_{234}}{c_5}{s_6} + {c_{234}}{c_6}}&{ - {s_{234}}{s_5}}&{{H_1} - {H_2}{c_{234}} - {L_2}{s_{23}} - {L_1}{s_2} - {W_2}{s_{234}}{s_5}}\\ 0&0&0&1 \end{array}} \right] ⎣⎢⎢⎡r11r21r310r12r22r320r13r23r330p1p2p31⎦⎥⎥⎤=⎣⎢⎢⎡−c1c234c5c6+c1s234s6+s1s5c6s1c234c5c6−s1s234s6+c1s5c6s234c5c6+c234s60−c1c234c5s6+c1s234c6+s1s5s6s1c234c5s6−s1s234c6+c1s5s6s234c5s6+c234c60c1c234s5+s1c5−s1c234s5+c1c5−s234s50L2c1c23+L1c1c2−H2c1s234+W2c1c234s5+W1s1+W2s1c5−L2s1c23−L1s1c2+H2s1s234−W2s1c234s5+W1c1+W2c1c5H1−H2c234−L2s23−L1s2−W2s234s51⎦⎥⎥⎤
三、简单的代码实现
临时写的测试小代码，仅供参考。
这里我将所有关节旋转 π 2 \frac{\pi}{2} 2π来校验是否正确，大抵正确。
时间原因，没有设计完整功能的函数，据称MR机器人官网有完整旋量计算库，感兴趣的同学欢迎交流！
#include 实验结果
总结
无总结，预祝大家新年快乐！散会！
相关链接
【现代机器人学——螺旋理论】UR5机器人正运动学求解
【现代机器人学——螺旋理论】UR5机器人逆运动学求解