系列文章目录
【现代机器人学——螺旋理论】UR5机器人正运动学求解
【现代机器人学——螺旋理论】UR5机器人逆运动学求解
提示：本文介绍基于螺旋理论分析的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)已知 e [ S ] θ = [ R p 0 1 ] {e^{\left[ S \right]\theta }} = \left[ {\begin{array}{ccc} R&p\\ 0&1 \end{array}} \right] e[S]θ=[R0?p1?]
可得 e ? [ S ] θ = [ R ? 1 ? R ? 1 p 0 1 ] {e^{ - \left[ S \right]\theta }} = \left[ {\begin{array}{ccc} {{R^{ - 1}}}&{ - {R^{ - 1}}p}\\ 0&1 \end{array}} \right] e?[S]θ=[R?10??R?1p1?]
(2)已知 ? sin ? ( θ ) p x + cos ? ( θ ) p y = d - \sin (\theta ){p_x} + \cos (\theta ){p_y} = d ?sin(θ)px?+cos(θ)py?=d
可得 θ = A tan ? 2 ( p y , p x ) ? A tan ? 2 ( d , ± p x 2 + p y 2 ? d 2 ) \theta = A\tan 2({p_y},{p_x}) - A\tan 2(d, \pm \sqrt {{p_x}^2 + {p_y}^2 - {d^2}} ) θ=Atan2(py?,px?)?Atan2(d,±px?2+py?2?d2?)
三、逆运动学
1.准备工作
本解算方案采用解析法，首先对最终表达式进行变形，将旋转轴一左移
e
?
[
S
1
]
θ
1
T
(
θ
)
=
e
[
S
2
]
θ
2
e
[
S
3
]
θ
3
e
[
S
4
]
θ
4
e
[
S
5
]
θ
5
e
[
S
6
]
θ
6
M
{e^{ - \left[ {{S_1}} \right]{\theta _1}}}T(\theta ) = {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
e?[S1?]θ1?T(θ)=e[S2?]θ2?e[S3?]θ3?e[S4?]θ4?e[S5?]θ5?e[S6?]θ6?M
扩写公式得
[
r
11
c
1
+
r
21
s
1
r
12
c
1
+
r
22
s
1
r
13
c
1
+
r
23
s
1
p
1
c
1
+
p
2
s
1
?
r
11
s
1
+
r
21
c
1
?
r
12
s
1
+
r
22
c
1
?
r
13
s
1
+
r
23
c
1
?
p
1
s
1
+
p
2
c
1
r
31
r
32
r
33
p
3
0
0
0
1
]
=
[
?
c
234
c
5
c
6
+
s
234
s
6
?
c
234
c
5
s
6
+
s
234
c
6
c
234
s
5
L
2
c
23
+
L
1
c
2
?
H
2
s
234
+
W
2
c
234
s
5
s
5
c
6
s
5
s
6
c
5
W
1
+
W
2
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
]
\begin{array}{l} \left[ {\begin{array}{ccc} {{r_{11}}{c_1} + {r_{21}}{s_1}}&{{r_{12}}{c_1} + {r_{22}}{s_1}}&{{r_{13}}{c_1} + {r_{23}}{s_1}}&{{p_1}{c_1} + {p_2}{s_1}}\\ { - {r_{11}}{s_1} + {r_{21}}{c_1}}&{ - {r_{12}}{s_1} + {r_{22}}{c_1}}&{ - {r_{13}}{s_1} + {r_{23}}{c_1}}&{ - {p_1}{s_1} + {p_2}{c_1}}\\ {{r_{31}}}&{{r_{32}}}&{{r_{33}}}&{{p_3}}\\ 0&0&0&1 \end{array}} \right] = \left[ {\begin{array}{ccc} { - {c_{234}}{c_5}{c_6} + {s_{234}}{s_6}}&{ - {c_{234}}{c_5}{s_6} + {s_{234}}{c_6}}&{{c_{234}}{s_5}}&{{L_2}{c_{23}} + {L_1}{c_2} - {H_2}{s_{234}} + {W_2}{c_{234}}{s_5}}\\ {{s_5}{c_6}}&{{s_5}{s_6}}&{{c_5}}&{{W_1} + {W_2}{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] \end{array}
?????r11?c1?+r21?s1??r11?s1?+r21?c1?r31?0?r12?c1?+r22?s1??r12?s1?+r22?c1?r32?0?r13?c1?+r23?s1??r13?s1?+r23?c1?r33?0?p1?c1?+p2?s1??p1?s1?+p2?c1?p3?1??????=??????c234?c5?c6?+s234?s6?s5?c6?s234?c5?c6?+c234?s6?0??c234?c5?s6?+s234?c6?s5?s6?s234?c5?s6?+c234?c6?0?c234?s5?c5??s234?s5?0?L2?c23?+L1?c2??H2?s234?+W2?c234?s5?W1?+W2?c5?H1??H2?c234??L2?s23??L1?s2??W2?s234?s5?1???????
利用该公式可以较好的完成逆运动学解算
参考文献《UR机械臂正逆运动学求解》中左移了
θ
1
\theta_1
θ1?和
θ
6
\theta_6
θ6?，我尝试过，效果并不是很好，得到公式不太适合解析，有相关思路同学欢迎交流。
[
?
c
6
(
r
11
c
1
+
r
21
s
1
)
+
s
6
(
r
12
c
1
+
r
22
s
1
)
r
13
c
1
+
r
23
s
1
s
6
(
r
11
c
1
+
r
21
s
1
)
+
c
6
(
r
12
c
1
+
r
22
s
1
)
(
r
11
c
1
+
r
21
s
1
)
(
(
L
1
+
L
2
)
c
6
?
(
H
1
?
H
2
)
s
6
)
+
(
r
12
c
1
+
r
22
s
1
)
(
?
(
H
1
?
H
2
)
c
6
+
(
L
1
+
L
2
)
s
6
)
?
(
r
13
c
1
+
r
23
s
1
)
(
W
1
+
W
2
)
+
p
1
c
1
+
p
2
s
1
?
c
6
(
?
r
11
s
1
+
r
21
c
1
)
+
s
6
(
?
r
12
s
1
+
r
22
c
1
)
?
r
13
s
1
+
r
23
c
1
s
6
(
?
r
11
s
1
+
r
21
c
1
)
+
c
6
(
?
r
12
s
1
+
r
22
c
1
)
(
?
r
11
s
1
+
r
21
c
1
)
(
(
L
1
+
L
2
)
c
6
?
(
H
1
?
H
2
)
s
6
)
+
(
?
r
12
s
1
+
r
22
c
1
)
(
?
(
H
1
?
H
2
)
c
6
+
(
L
1
+
L
2
)
s
6
)
?
(
?
r
13
s
1
+
r
23
c
1
)
(
W
1
+
W
2
)
?
p
1
s
1
+
p
2
c
1
?
r
31
c
6
+
r
32
s
6
r
33
r
31
s
6
+
r
32
c
6
r
31
(
(
L
1
+
L
2
)
c
6
?
(
H
1
?
H
2
)
s
6
)
+
r
32
(
?
(
H
1
?
H
2
)
c
6
+
(
L
1
+
L
2
)
s
6
)
?
r
33
(
W
1
+
W
2
)
+
p
3
0
0
0
1
]
=
[
c
234
c
5
s
5
c
234
s
234
(
L
1
+
L
2
)
(
c
23
?
c
234
c
5
)
+
L
1
(
c
2
?
c
23
)
?
H
1
s
234
?
W
1
c
234
s
5
?
s
5
c
5
0
W
1
(
1
?
c
5
)
+
(
L
1
+
L
2
)
s
5
?
s
234
c
5
?
s
234
s
5
c
234
H
1
(
1
?
c
234
)
+
(
L
1
+
L
2
)
(
?
s
23
+
s
234
c
5
)
+
L
1
(
?
s
2
+
s
23
)
?
W
1
s
234
s
5
0
0
0
1
]
\begin{array}{l} \left[ {\begin{array}{ccc} { - {c_6}({r_{11}}{c_1} + {r_{21}}{s_1}) + {s_6}({r_{12}}{c_1} + {r_{22}}{s_1})}&{{r_{13}}{c_1} + {r_{23}}{s_1}}&{{s_6}({r_{11}}{c_1} + {r_{21}}{s_1}) + {c_6}({r_{12}}{c_1} + {r_{22}}{s_1})}&{({r_{11}}{c_1} + {r_{21}}{s_1})(({L_1} + {L_2}){c_6} - ({H_1} - {H_2}){s_6}) + ({r_{12}}{c_1} + {r_{22}}{s_1})( - ({H_1} - {H_2}){c_6} + ({L_1} + {L_2}){s_6}) - ({r_{13}}{c_1} + {r_{23}}{s_1})({W_1} + {W_2}) + {p_1}{c_1} + {p_2}{s_1}}\\ { - {c_6}( - {r_{11}}{s_1} + {r_{21}}{c_1}) + {s_6}( - {r_{12}}{s_1} + {r_{22}}{c_1})}&{ - {r_{13}}{s_1} + {r_{23}}{c_1}}&{{s_6}( - {r_{11}}{s_1} + {r_{21}}{c_1}) + {c_6}( - {r_{12}}{s_1} + {r_{22}}{c_1})}&{( - {r_{11}}{s_1} + {r_{21}}{c_1})(({L_1} + {L_2}){c_6} - ({H_1} - {H_2}){s_6}) + ( - {r_{12}}{s_1} + {r_{22}}{c_1})( - ({H_1} - {H_2}){c_6} + ({L_1} + {L_2}){s_6}) - ( - {r_{13}}{s_1} + {r_{23}}{c_1})({W_1} + {W_2}) - {p_1}{s_1} + {p_2}{c_1}}\\ { - {r_{31}}{c_6} + {r_{32}}{s_6}}&{{r_{33}}}&{{r_{31}}{s_6} + {r_{32}}{c_6}}&{{r_{31}}(({L_1} + {L_2}){c_6} - ({H_1} - {H_2}){s_6}) + {r_{32}}( - ({H_1} - {H_2}){c_6} + ({L_1} + {L_2}){s_6}) - {r_{33}}({W_1} + {W_2}) + {p_3}}\\ 0&0&0&1 \end{array}} \right]\\ = \left[ {\begin{array}{ccc} {{c_{234}}{c_5}}&{{s_5}{c_{234}}}&{{s_{234}}}&{({L_1} + {L_2})\left( {{c_{23}} - {c_{234}}{c_5}} \right) + {L_1}\left( {{c_2} - {c_{23}}} \right) - {H_1}{s_{234}} - {W_1}{c_{234}}{s_5}}\\ { - {s_5}}&{{c_5}}&0&{{W_1}\left( {1 - {c_5}} \right) + \left( {{L_1} + {L_2}} \right){s_5}}\\ { - {s_{234}}{c_5}}&{ - {s_{234}}{s_5}}&{{c_{234}}}&{{H_1}\left( {1 - {c_{234}}} \right) + ({L_1} + {L_2})( - {s_{23}} + {s_{234}}{c_5}) + {L_1}( - {s_2} + {s_{23}}) - {W_1}{s_{234}}{s_5}}\\ 0&0&0&1 \end{array}} \right] \end{array}
??????c6?(r11?c1?+r21?s1?)+s6?(r12?c1?+r22?s1?)?c6?(?r11?s1?+r21?c1?)+s6?(?r12?s1?+r22?c1?)?r31?c6?+r32?s6?0?r13?c1?+r23?s1??r13?s1?+r23?c1?r33?0?s6?(r11?c1?+r21?s1?)+c6?(r12?c1?+r22?s1?)s6?(?r11?s1?+r21?c1?)+c6?(?r12?s1?+r22?c1?)r31?s6?+r32?c6?0?(r11?c1?+r21?s1?)((L1?+L2?)c6??(H1??H2?)s6?)+(r12?c1?+r22?s1?)(?(H1??H2?)c6?+(L1?+L2?)s6?)?(r13?c1?+r23?s1?)(W1?+W2?)+p1?c1?+p2?s1?(?r11?s1?+r21?c1?)((L1?+L2?)c6??(H1??H2?)s6?)+(?r12?s1?+r22?c1?)(?(H1??H2?)c6?+(L1?+L2?)s6?)?(?r13?s1?+r23?c1?)(W1?+W2?)?p1?s1?+p2?c1?r31?((L1?+L2?)c6??(H1??H2?)s6?)+r32?(?(H1??H2?)c6?+(L1?+L2?)s6?)?r33?(W1?+W2?)+p3?1??????=?????c234?c5??s5??s234?c5?0?s5?c234?c5??s234?s5?0?s234?0c234?0?(L1?+L2?)(c23??c234?c5?)+L1?(c2??c23?)?H1?s234??W1?c234?s5?W1?(1?c5?)+(L1?+L2?)s5?H1?(1?c234?)+(L1?+L2?)(?s23?+s234?c5?)+L1?(?s2?+s23?)?W1?s234?s5?1???????
2.求解 θ 1 \theta_1 θ1?
从等式中观测得
{ ? p 1 s 1 + p 2 c 1 = W 1 + W 2 c 5 ( 2 , 4 ) c 5 = ? r 13 s 1 + r 23 c 1 ( 2 , 3 ) \left\{ {\begin{array}{ccc} { - {p_1}{s_1} + {p_2}{c_1} = {W_1} + {W_2}{c_5}} \qquad (2,4)\\ {{c_5} = - {r_{13}}{s_1} + {r_{23}}{c_1}}\qquad (2,3) \end{array}} \right. {?p1?s1?+p2?c1?=W1?+W2?c5?(2,4)c5?=?r13?s1?+r23?c1?(2,3)?
可得
?
p
1
s
1
+
p
2
c
1
=
W
1
+
W
2
(
?
r
13
s
1
+
r
23
c
1
)
- {p_1}{s_1} + {p_2}{c_1} = {W_1} + {W_2}( - {r_{13}}{s_1} + {r_{23}}{c_1})
?p1?s1?+p2?c1?=W1?+W2?(?r13?s1?+r23?c1?)
整理为
?
(
p
1
?
W
2
r
13
)
s
1
+
(
p
2
?
W
2
r
23
)
c
1
=
W
1
- ({p_1} - {W_2}{r_{13}}){s_1} + ({p_2} - {W_2}{r_{23}}){c_1} = {W_1}
?(p1??W2?r13?)s1?+(p2??W2?r23?)c1?=W1?
使用公式得
θ
1
=
A
tan
?
2
(
(
p
2
?
W
2
r
23
)
,
(
p
1
?
W
2
r
13
)
)
?
A
tan
?
2
(
d
,
±
(
p
1
?
W
2
r
13
)
2
+
(
p
2
?
W
2
r
23
)
2
?
W
1
2
{\theta _1} = A\tan 2(({p_2} - {W_2}{r_{23}}),({p_1} - {W_2}{r_{13}})) - A\tan 2(d, \pm \sqrt {{{({p_1} - {W_2}{r_{13}})}^2} + {{({p_2} - {W_2}{r_{23}})}^2} - {W_1}^2}
θ1?=Atan2((p2??W2?r23?),(p1??W2?r13?))?Atan2(d,±(p1??W2?r13?)2+(p2??W2?r23?)2?W1?2?
3.求解 θ 5 \theta_5 θ5?
由前文得知
c
5
=
?
r
13
s
1
+
r
23
c
1
{{c_5} = - {r_{13}}{s_1} + {r_{23}}{c_1}}
c5?=?r13?s1?+r23?c1?
解得
θ
5
=
±
arccos
?
(
?
r
13
s
1
+
r
23
c
1
)
{\theta _5} = \pm \arccos ( - {r_{13}}{s_1} + {r_{23}}{c_1})
θ5?=±arccos(?r13?s1?+r23?c1?)
4.求解 θ 6 \theta_6 θ6?
从等式中观测得
?
r
11
s
1
+
r
21
c
1
=
s
5
c
6
(
2
,
1
)
- {r_{11}}{s_1} + {r_{21}}{c_1} = {s_5}{c_6}\qquad (2,1)
?r11?s1?+r21?c1?=s5?c6?(2,1)
解得
θ
6
=
±
arccos
?
(
?
r
11
s
1
+
r
21
c
1
s
5
)
{\theta _6} =\pm \arccos (\frac{{ - {r_{11}}{s_1} + {r_{21}}{c_1}}}{{{s_5}}})
θ6?=±arccos(s5??r11?s1?+r21?c1??)
5.求解 θ 2 + θ 3 + θ 4 \theta_2+\theta_3+\theta_4 θ2?+θ3?+θ4?
机器人2、3、4关节属于并联关节，该三个关节关系紧密，计算的时候会有很多有趣的现象。
从等式中观测得
{
r
13
c
1
+
r
23
s
1
=
c
234
s
5
r
33
=
?
s
234
s
5
\left\{ {\begin{array}{ccc} {{r_{13}}{c_1} + {r_{23}}{s_1} = {c_{234}}{s_5}}\\ {{r_{33}} = - {s_{234}}{s_5}} \end{array}} \right.
{r13?c1?+r23?s1?=c234?s5?r33?=?s234?s5??
解得
θ
2
+
θ
3
+
θ
4
=
A
tan
?
2
(
?
r
33
,
r
13
c
1
+
r
23
s
1
)
{\theta _2} + {\theta _3} + {\theta _4} = A\tan 2({-r_{33}},{r_{13}}{c_1} + {r_{23}}{s_1})
θ2?+θ3?+θ4?=Atan2(?r33?,r13?c1?+r23?s1?)
6.求解 θ 3 \theta_3 θ3?
从等式中观测得
{
L
2
c
23
+
L
1
c
2
?
H
2
s
234
+
W
2
c
234
s
5
=
p
1
c
1
+
p
2
s
1
H
1
?
H
2
c
234
?
L
2
s
23
?
L
1
s
2
?
W
2
s
234
s
5
=
p
3
\left\{ {\begin{array}{ccc} {{L_2}{c_{23}} + {L_1}{c_2} - {H_2}{s_{234}} + {W_2}{c_{234}}{s_5} = {p_1}{c_1} + {p_2}{s_1}}\\ {{H_1} - {H_2}{c_{234}} - {L_2}{s_{23}} - {L_1}{s_2} - {W_2}{s_{234}}{s_5} = {p_3}} \end{array}} \right.
{L2?c23?+L1?c2??H2?s234?+W2?c234?s5?=p1?c1?+p2?s1?H1??H2?c234??L2?s23??L1?s2??W2?s234?s5?=p3??
简化得
{
L
2
c
23
+
L
1
c
2
=
p
1
c
1
+
p
2
s
1
+
H
2
s
234
?
W
2
c
234
s
5
=
m
L
2
s
23
+
L
1
s
2
=
?
p
3
+
H
1
?
H
2
c
234
?
W
2
s
234
s
5
=
n
\left\{ {\begin{array}{ccc} {{L_2}{c_{23}} + {L_1}{c_2} = {p_1}{c_1} + {p_2}{s_1} + {H_2}{s_{234}} - {W_2}{c_{234}}{s_5} = m}\\ {{L_2}{s_{23}} + {L_1}{s_2} = {-p_3} + {H_1} - {H_2}{c_{234}} - {W_2}{s_{234}}{s_5} = n} \end{array}} \right.
{L2?c23?+L1?c2?=p1?c1?+p2?s1?+H2?s234??W2?c234?s5?=mL2?s23?+L1?s2?=?p3?+H1??H2?c234??W2?s234?s5?=n?
平方得
{
L
2
2
c
23
2
+
2
L
1
L
2
c
2
c
23
+
L
1
2
c
2
2
=
m
2
L
2
2
s
23
2
+
2
L
1
L
2
s
2
s
23
+
L
1
2
s
2
2
=
n
2
\left\{ {\begin{array}{ccc} {{L_2}^2{c_{23}}^2 + 2{L_1}{L_2}{c_2}{c_{23}} + {L_1}^2{c_2}^2 = {m^2}}\\ {{L_2}^2{s_{23}}^2 + 2{L_1}{L_2}{s_2}{s_{23}} + {L_1}^2{s_2}^2 = {n^2}} \end{array}} \right.
{L2?2c23?2+2L1?L2?c2?c23?+L1?2c2?2=m2L2?2s23?2+2L1?L2?s2?s23?+L1?2s2?2=n2?
求和
L
1
2
+
L
2
2
+
2
L
1
L
2
c
3
=
m
2
+
n
2
{L_1}^2 + {L_2}^2 + 2{L_1}{L_2}{c_3} = {m^2} + {n^2}
L1?2+L2?2+2L1?L2?c3?=m2+n2
解得
θ
3
=
±
arccos
?
(
m
2
+
n
2
?
L
1
2
?
L
2
2
2
L
1
L
2
)
{\theta _3} = \pm \arccos (\frac{{{m^2} + {n^2} - {L_1}^2 - {L_2}^2}}{{2{L_1}{L_2}}})
θ3?=±arccos(2L1?L2?m2+n2?L1?2?L2?2?)
7.求解 θ 5 \theta_5 θ5?
已知
L
2
c
23
+
L
1
c
2
=
p
1
c
1
+
p
2
s
1
+
H
2
s
234
?
W
2
c
234
s
5
=
m
{{L_2}{c_{23}} + {L_1}{c_2} = {p_1}{c_1} + {p_2}{s_1} + {H_2}{s_{234}} - {W_2}{c_{234}}{s_5} = m}
L2?c23?+L1?c2?=p1?c1?+p2?s1?+H2?s234??W2?c234?s5?=m
简化为
?
L
2
s
3
?
s
2
+
(
L
2
c
3
+
L
1
)
?
c
2
=
m
- {L_2}{s_3} \cdot {s_2} + ({L_2}{c_3} + {L_1}) \cdot {c_2} = m
?L2?s3??s2?+(L2?c3?+L1?)?c2?=m
解得
θ
2
=
A
tan
?
2
(
(
L
2
c
3
+
L
1
)
,
L
2
s
3
)
?
A
tan
?
2
(
m
,
±
L
2
2
s
3
2
+
(
L
2
c
3
+
L
1
)
2
?
m
2
)
{\theta _2} = A\tan 2(({L_2}{c_3} + {L_1}), {L_2}{s_3}) - A\tan 2(m, \pm \sqrt {{L_2}^2{s_3}^2 + {{({L_2}{c_3} + {L_1})}^2} - {m^2}} )
θ2?=Atan2((L2?c3?+L1?),L2?s3?)?Atan2(m,±L2?2s3?2+(L2?c3?+L1?)2?m2?)
8.求解 θ 6 \theta_6 θ6?
依据前三项解算直接获得 θ 6 \theta_6 θ6?
9.关于Paden-Kahan子问题
使用子问题解析尝试过，最终失败，网上查了一下资料也没找到太多信息，放一点之前的草稿在这里供大家参考下。
e
[
S
1
]
θ
1
e
[
S
2
]
θ
2
e
[
S
3
]
θ
3
e
[
S
4
]
θ
4
e
[
S
5
]
θ
5
e
[
S
6
]
θ
6
g
s
t
(
0
)
=
g
d
e
[
S
1
]
θ
1
e
[
S
2
]
θ
2
e
[
S
3
]
θ
3
e
[
S
4
]
θ
4
e
[
S
5
]
θ
5
e
[
S
6
]
θ
6
=
g
d
g
s
t
(
0
)
?
1
=
g
1
e
[
S
1
]
θ
1
e
[
S
2
]
θ
2
e
[
S
3
]
θ
3
e
[
S
4
]
θ
4
p
w
=
g
1
p
w
e
[
S
1
]
θ
1
e
[
S
2
]
θ
2
e
[
S
3
]
θ
3
e
[
S
4
]
θ
4
p
w
?
p
s
t
=
g
1
p
w
?
p
s
t
e
[
S
1
]
θ
1
e
[
S
2
]
θ
2
(
e
[
S
3
]
θ
3
e
[
S
4
]
θ
4
p
w
?
p
s
t
)
=
g
1
p
w
?
p
s
t
∥
e
[
S
3
]
θ
3
e
[
S
4
]
θ
4
p
w
?
p
s
t
∥
=
∥
g
1
p
w
?
p
s
t
∥
\begin{array}{l} {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}}}{g_{st}}\left( 0 \right) = {g_d}\\ {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}}} = {g_d}{g_{st}}{\left( 0 \right)^{ - 1}} = {g_1}\\ {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}}}{p_w} = {g_1}{p_w}\\ {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}}}{p_w} - {p_{st}} = {g_1}{p_w} - {p_{st}}\\ {e^{\left[ {{S_1}} \right]{\theta _1}}}{e^{\left[ {{S_2}} \right]{\theta _2}}}\left( {{e^{\left[ {{S_3}} \right]{\theta _3}}}{e^{\left[ {{S_4}} \right]{\theta _4}}}{p_w} - {p_{st}}} \right) = {g_1}{p_w} - {p_{st}}\\ \left\| {{e^{\left[ {{S_3}} \right]{\theta _3}}}{e^{\left[ {{S_4}} \right]{\theta _4}}}{p_w} - {p_{st}}} \right\| = \left\| {{g_1}{p_w} - {p_{st}}} \right\| \end{array}
e[S1?]θ1?e[S2?]θ2?e[S3?]θ3?e[S4?]θ4?e[S5?]θ5?e[S6?]θ6?gst?(0)=gd?e[S1?]θ1?e[S2?]θ2?e[S3?]θ3?e[S4?]θ4?e[S5?]θ5?e[S6?]θ6?=gd?gst?(0)?1=g1?e[S1?]θ1?e[S2?]θ2?e[S3?]θ3?e[S4?]θ4?pw?=g1?pw?e[S1?]θ1?e[S2?]θ2?e[S3?]θ3?e[S4?]θ4?pw??pst?=g1?pw??pst?e[S1?]θ1?e[S2?]θ2?(e[S3?]θ3?e[S4?]θ4?pw??pst?)=g1?pw??pst?∥∥?e[S3?]θ3?e[S4?]θ4?pw??pst?∥∥?=∥g1?pw??pst?∥?
印象里一个月前查阅外网信息表示难以解算，有相关经历的同学欢迎交流。
四、简单的代码实现
临时写的测试小代码，仅限
?
π
2
-\frac{\pi}{2}
?2π?到
π
2
\frac{\pi}{2}
2π?之间，仅供参考。
由于希望避免多解问题，这里我将所有关节旋转
π
4
\frac{\pi}{4}
4π?来校验是否正确
float trig_func(float p_y, float p_x, float d)//用于解-p_x*sin+p_y*cos=d
{
float temp = sqrt(p_x * p_x + p_y * p_y - d * d);
return atan2(p_y, p_x) - atan2(d, temp);
}
void Inverse_Kinematics(float T[4][4], float theta[6])
{
theta[0] = trig_func(T[1][3] - W_2 * T[1][2], T[0][3] - W_2 * T[0][2], W_1);
float sin_0 = sin(theta[0]);
float cos_0 = cos(theta[0]);
theta[4] = acos(-T[0][2] * sin_0 + T[1][2] * cos_0);//忽略负解
float sin_4 = sin(theta[4]);
theta[5] = acos((-T[0][0] * sin_0 + T[1][0] * cos_0)/sin_4);//忽略负解
float cos_123 = (T[0][2] * cos_0 + T[1][2] * sin_0) / sin_4;
float sin_123 = -T[2][2] / sin_4;
float theta_123 = atan2(sin_123 , cos_123);
//printf("%.3f\r\n", theta_123);
float m = T[0][3] * cos_0 + T[1][3] * sin_0 + H_2 * sin_123 - W_2 * cos_123 * sin_4;
float n = -T[2][3] + H_1 - H_2 * cos_123 - W_2 * sin_123 * sin_4;
theta[2] = acos((m * m + n * n - L_1 * L_1 - L_2 * L_2)/(2 * L_1 * L_2));//忽略负解
float sin_2 = sin(theta[2]);
float cos_2 = cos(theta[2]);
theta[1] = trig_func(L_2 * cos_2 + L_1, L_2 * sin_2, m);
theta[3] = theta_123 - theta[1] - theta[2];
}
总结
公式中可能存在少量错误，有些是后期代码验证后检测出的。
预祝大家新年快乐！散会！