UR3机械臂正逆运动学详解及c++完整代码
目录
- D-H参数表
- 正运动学
-
- 公式推导
- 代码
- 测试
- 逆运动学
-
- 准备工作
- 求解 θ 1 \theta_{1} θ1
- 求解 θ 5 \theta_{5} θ5
- 求解 θ 6 \theta_{6} θ6
- 求解 θ 2 \theta_2 θ2、 θ 3 \theta_3 θ3、 θ 4 \theta_4 θ4
-
- 先求解 θ 234 \theta_{234} θ234
- 再求解 θ 2 \theta_{2} θ2
- 然后反解 θ 23 \theta_{23} θ23
- 最后求解 θ 3 \theta_3 θ3、 θ 4 \theta_4 θ4
- 完整代码
- 测试结果
- 不足之处
博主也是刚开始学习机械臂，不断搜索网上的教程，找论文，费尽千辛万苦终于计算正确，学习之路太过坎坷
D-H参数表
| i i i | α i − 1 \alpha_{i-1} αi−1 | d i d_{i} di | a i − 1 a_{i-1} ai−1 | θ i \theta_{i} θi |
|---|---|---|---|---|
| 1 1 1 | 0 0 0 | d 1 d_{1} d1 | 0 0 0 | θ 1 \theta_{1} θ1 |
| 2 2 2 | 90 90 90 | 0 0 0 | 0 0 0 | θ 2 \theta_{2} θ2 |
| 3 3 3 | 0 0 0 | 0 0 0 | a 2 a_{2} a2 | θ 3 \theta_{3} θ3 |
| 4 4 4 | 0 0 0 | d 4 d_{4} d4 | a 3 a_{3} a3 | θ 4 \theta_{4} θ4 |
| 5 5 5 | 90 90 90 | d 5 d_{5} d5 | 0 0 0 | θ 5 \theta_{5} θ5 |
| 6 6 6 | − 90 -90 −90 | d 6 d_{6} d6 | 0 0 0 | θ 6 \theta_{6} θ6 |
正运动学
公式推导
这里的 s i s_{i} si代表 sin ( θ i ) \sin(\theta_i) sin(θi)， c i c_{i} ci代表 cos ( θ i ) \cos(\theta_i) cos(θi)，以及下文逆运动学中出现的 s i j s_{ij} sij代表 sin ( θ i + θ j ) \sin(\theta_i+\theta_j) sin(θi+θj)
由于
i i − 1 T = [ c i − s i 0 a i − 1 s i c α i − 1 c i c α i − 1 − s α i − 1 − s α i − 1 d i s i s α i − 1 c i s α i − 1 c α i − 1 c α i − 1 d i 0 0 0 1 ] (*) {}^{i-1}_{i}T= \begin{bmatrix} c_{i} & -s_{i} & 0 & a_{i-1} \\ s_{i}c_{\alpha_{i-1}} & c_{i}c_{\alpha_{i-1}} & -s_{\alpha_{i-1}} & -s_{\alpha_{i-1}}d_{i} \\ s_{i}s_{\alpha_{i-1}} & c_{i}s_{\alpha_{i-1}} & c_{\alpha_{i-1}} & c_{\alpha_{i-1}}d_{i} \\ 0 & 0& 0&1 \end{bmatrix}\tag{*} ii−1T=⎣⎢⎢⎡cisicαi−1sisαi−10−sicicαi−1cisαi−100−sαi−1cαi−10ai−1−sαi−1dicαi−1di1⎦⎥⎥⎤(*)
所以我们可以得到六个变换矩阵（这是代入 α \alpha α、 d d d与 a a a中的零值后的结果）
1 0 T ( θ 1 ) = [ c 1 − s 1 0 0 s 1 c 1 0 0 0 0 1 d 1 0 0 0 1 ] (1) {}^{0}_{1}T(\theta_{1})= \begin{bmatrix} c_{1} & -s_{1} & 0 & 0 \\ s_{1} & c_{1} & 0&0 \\ 0& 0 & 1& d_{1} \\ 0 & 0& 0&1\tag{1} \end{bmatrix} 10T(θ1)=⎣⎢⎢⎡c1s100−s1c100001000d11⎦⎥⎥⎤(1)
2 1 T ( θ 2 ) = [ c 2 − s 2 0 0 0 0 − 1 0 s 2 c 2 0 0 0 0 0 1 ] (2) {}^{1}_{2}T(\theta_{2})= \begin{bmatrix} c_{2} & -s_{2} & 0 & 0 \\ 0 & 0 & -1&0 \\ s_{2} & c_{2} & 0&0 \\ 0 & 0& 0&1\tag{2} \end{bmatrix} 21T(θ2)=⎣⎢⎢⎡c20s20−s20c200−1000001⎦⎥⎥⎤(2)
3 2 T ( θ 3 ) = [ c 3 − s 3 0 a 2 s 3 c 3 0 0 0 0 1 0 0 0 0 1 ] (3) {}^{2}_{3}T(\theta_{3})= \begin{bmatrix} c_{3} & -s_{3} & 0 & a_{2} \\ s_{3} & c_{3} & 0&0 \\ 0& 0 & 1& 0 \\ 0 & 0& 0&1\tag{3} \end{bmatrix} 32T(θ3)=⎣⎢⎢⎡c3s300−s3c3000010a2001⎦⎥⎥⎤(3)
4 3 T ( θ 4 ) = [ c 4 − s 4 0 a 3 s 4 c 4 0 0 0 0 1 d 4 0 0 0 1 ] (4) {}^{3}_{4}T(\theta_{4})= \begin{bmatrix} c_{4} & -s_{4} & 0 & a_{3} \\ s_{4} & c_{4} & 0&0 \\ 0& 0 & 1& d_{4} \\ 0 & 0& 0&1\tag{4} \end{bmatrix} 43T(θ4)=⎣⎢⎢⎡c4s400−s4c4000010a30d41⎦⎥⎥⎤(4)
5 4 T ( θ 5 ) = [ c 5 − s 5 0 0 0 0 − 1 − d 5 s 5 c 5 0 0 0 0 0 1 ] (5) {}^{4}_{5}T(\theta_{5})= \begin{bmatrix} c_{5} & -s_{5} & 0 & 0 \\ 0& 0 & -1&-d_{5} \\ s_{5} & c_{5} & 0&0 \\ 0 & 0& 0&1\tag{5} \end{bmatrix} 54T(θ5)=⎣⎢⎢⎡c50s50−s50c500−1000−d501⎦⎥⎥⎤(5)
6 5 T ( θ 6 ) = [ c 6 − s 6 0 0 0 0 1 d 6 − s 6 − c 6 0 0 0 0 0 1 ] (6) {}^{5}_{6}T(\theta_{6})= \begin{bmatrix} c_{6} & -s_{6} & 0 & 0 \\ 0& 0 & 1&d_{6} \\ -s_{6} & -c_{6} & 0&0 \\ 0 & 0& 0&1\tag{6} \end{bmatrix} 65T(θ6)=⎣⎢⎢⎡c60−s60−s60−c6001000d601⎦⎥⎥⎤(6)
于是
6 0 T = 1 0 T 2 1 T 3 2 T 4 3 T 5 4 T 6 5 T (7) {}^{0}_{6}T= {}^{0}_{1}T{}^{1}_{2}T{}^{2}_{3}T{}^{3}_{4}T{}^{4}_{5}T{}^{5}_{6}T\tag{7} 60T=10T21T32T43T54T65T(7)
6 0 T {}^{0}_{6}T 60T中第四列的前三行数据就是机器人末端连杆在笛卡儿坐标系里的位置
其实在编程中无需把每一个变换矩阵的表达式计算出，只要按照式子 ( ∗ ) (*) (∗) 把对应的值代入计算即可，每计算一个变换矩阵，都做一次矩阵相乘。（可以看最后的代码来理解）但在下文的逆运动学中我会具体写出每一步的计算结果，上面的式子算作一个铺垫
代码
因为逆运动学要涉及反代入检验，所以看后面逆运动学求解的代码就行了
测试
红色框为仿真环境给出的数据，下面六个角度就是我输入的六个 θ \theta θ的值
橙色框为程序计算出的位置，这里由于单位问题，两组值相差了 1 0 3 10^{3} 103倍
逆运动学
准备工作
首先推得
1 0 T − 1 ( θ 1 ) = [ c 1 s 1 0 0 − s 1 c 1 0 0 0 0 1 − d 1 0 0 0 1 ] (8) {}^{0}_{1}T^{-1}(\theta_{1})= \begin{bmatrix} c_{1} & s_{1} & 0 & 0 \\ -s_{1} & c_{1} & 0&0 \\ 0& 0 & 1& -d_{1} \\ 0 & 0& 0&1\tag{8} \end{bmatrix} 10T−1(θ1)=⎣⎢⎢⎡c1−s100s1c100001000−d11⎦⎥⎥⎤(8)
然后逐步计算
3 1 T = 2 1 T 3 2 T = [ c 23 − s 23 0 c 2 a 2 0 0 − 1 0 s 23 c 23 0 s 2 a 2 0 0 0 1 ] (9) {}^{1}_{3}T= {}^{1}_{2}T {}^{2}_{3}T= \begin{bmatrix} c_{23} & -s_{23} & 0 & c_2a_2 \\ 0 &0 & -1&0 \\ s_{23} & c_{23} & 0 & s_2a_2 \\ 0 & 0& 0&1\tag{9} \end{bmatrix} 31T=21T32T=⎣⎢⎢⎡c230s230−s230c2300−100c2a20s2a21⎦⎥⎥⎤(9)
4 1 T = 3 1 T 4 3 T = [ c 234 − s 234 0 c 23 a 3 + c 2 a 2 0 0 − 1 − d 4 s 234 c 234 0 s 23 a 3 + s 2 a 2 0 0 0 1 ] (10) {}^{1}_{4}T= {}^{1}_{3}T {}^{3}_{4}T= \begin{bmatrix} c_{234} & -s_{234} & 0 & c_{23}a_3+c_2a_2 \\ 0 &0 & -1&-d_4 \\ s_{234} & c_{234} & 0 & s_{23}a_3+s_2a_2 \\ 0 & 0& 0&1\tag{10} \end{bmatrix} 41T=31T43T=⎣⎢⎢⎡c2340s2340−s2340c23400−100c23a3+c2a2−d4s23a3+s2a21⎦⎥⎥⎤(10)
5 1 T = 4 1 T 5 4 T = [ c 234 c 5 − c 234 s 5 s 234 s 234 d 5 + c 23 a 3 + c 2 a 2 − s 5 − c 5 0 − d 4 s 234 c 5 − s 234 s 5 − c 234 − c 234 d 5 + s 23 a 3 + s 2 a 2 0 0 0 1 ] (11) {}^{1}_{5}T= {}^{1}_{4}T {}^{4}_{5}T= \begin{bmatrix} c_{234}c_5 & -c_{234}s_5 & s_{234} & s_{234}d_5+c_{23}a_3+c_2a_2 \\ -s_5 &-c_5 & 0&-d_4 \\ s_{234}c_5 & -s_{234}s_5 & -c_{234} & -c_{234}d_5+s_{23}a_3+s_2a_2 \\ 0 & 0& 0&1\tag{11} \end{bmatrix} 51T=41T54T=⎣⎢⎢⎡c234c5−s5s234c50−c234s5−c5−s234s50s2340−c2340s234d5+c23a3+c2a2−d4−c234d5+s23a3+s2a21⎦⎥⎥⎤(11)
6 1 T = 5 1 T 6 5 T = [ c 234 c 5 c 6 − s 234 s 6 − c 234 c 5 s 6 − s 234 c 6 − c 234 s 5 − c 234 s 5 d 6 + s 234 d 5 + c 23 a 3 + c 2 a 2 − s 5 c 6 s 5 s 6 − c 5 − c 5 d 6 − d 4 s 234 c 5 c 6 + c 234 s 6 − s 234 c 5 s 6 + c 234 c 6 − s 234 s 5 − s 234 s 5 d 6 − c 234 d 5 + s 23 a 3 + s 2 a 2 0 0 0 1 ] (12) {}^{1}_{6}T= {}^{1}_{5}T {}^{5}_{6}T= \begin{bmatrix} c_{234}c_5c_6-s_{234}s_6 & -c_{234}c_5s_6-s_{234}c_6 & -c_{234}s_5 & -c_{234}s_5d_6+s_{234}d_5+c_{23}a_3+c_2a_2 \\ -s_5c_6 &s_5s_6 & -c_5&-c_5d_6-d_4 \\ s_{234}c_5c_6+c_{234}s_6 & -s_{234}c_5s_6+c_{234}c_6 & -s_{234}s_5 & -s_{234}s_5d_6-c_{234}d_5+s_{23}a_3+s_2a_2 \\ 0 & 0& 0&1 \end{bmatrix}\tag{12} 61T=51T65T=⎣⎢⎢⎡c234c5c6−s234s6−s5c6s234c5c6+c234s60−c234c5s6−s234c6s5s6−s234c5s6+c234c60−c234s5−c5−s234s50−c234s5d6+s234d5+c23a3+c2a2−c5d6−d4−s234s5d6−c234d5+s23a3+s2a21⎦⎥⎥⎤(12)
由于
6 0 T = 1 0 T 2 1 T 3 2 T 4 3 T 5 4 T 6 5 T = 1 0 T 6 1 T (13) {}^{0}_{6}T= {}^{0}_{1}T{}^{1}_{2}T{}^{2}_{3}T{}^{3}_{4}T{}^{4}_{5}T{}^{5}_{6}T= {}^{0}_{1}T{}^{1}_{6}T\tag{13} 60T=10T21T32T43T54T65T=10T61T(13)
所以有
1 0 T − 1 6 0 T = 6 1 T (14) {}^{0}_{1}T^{-1} {}^{0}_{6}T={}^{1}_{6}T\tag{14} 10T−160T=61T(14)
其中， 6 0 T {}^{0}_{6}T 60T是我们可以根据末端位姿获得的，我们设
6 0 T = [ r 11 r 12 r 13 x r 21 r 22 r 23 y r 31 r 32 r 33 z 0 0 0 1 ] (15) {}^{0}_{6}T= \begin{bmatrix} r_{11} & r_{12} & r_{13} & x \\ r_{21} & r_{22} & r_{23}&y \\ r_{31}& r_{32} & r_{33}& z \\ 0 & 0& 0&1\tag{15} \end{bmatrix} 60T=⎣⎢⎢⎡r11r21r310r12r22r320r13r23r330xyz1⎦⎥⎥⎤(15)
因此式 ( 14 ) (14) (14)左边矩阵相乘的结果为
1 0 T − 1 6 0 T = [ c 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 x + s 1 y − s 1 r 11 + c 1 r 21 − s 1 r 12 + c 1 r 22 − s 1 r 13 + c 1 r 23 − s 1 x + c 1 y r 31 r 32 r 33 z − d 1 0 0 0 1 ] (16) {}^{0}_{1}T^{-1} {}^{0}_{6}T= \begin{bmatrix} c_1r_{11}+s_1r_{21} & c_1r_{12}+s_1r_{22} & c_1r_{13}+s_1r_{23} & c_1x+s_1y \\ -s_1r_{11}+c_1r_{21} & -s_1r_{12}+c_1r_{22} & -s_1r_{13}+c_1r_{23}&-s_1x+c_1y \\ r_{31}& r_{32} & r_{33}& z-d_1 \\ 0 & 0& 0&1\tag{16} \end{bmatrix} 10T−160T=⎣⎢⎢⎡c1r11+s1r21−s1r11+c1r21r310c1r12+s1r22−s1r12+c1r22r320c1r13+s1r23−s1r13+c1r23r330c1x+s1y−s1x+c1yz−d11⎦⎥⎥⎤(16)
至此，求逆解的所有准备工作完成了，下面开始逐个计算 θ \theta θ的值
求逆解的总思路是：通过比对式 ( 12 ) (12) (12)和式 ( 16 ) (16) (16)两个矩阵获得等式，然后消元，最后用 a t a n 2 ( ) atan2() atan2()求解
求解 θ 1 \theta_{1} θ1
由于式 ( 16 ) (16) (16)中的未知数只有 θ 1 \theta_{1} θ1，因此我们选择先计算出 θ 1 \theta_{1} θ1的值。在式 ( 12 ) (12) (12)中，我们观察到第二行第三列（后文都简写为(2,3)这种形式）和(2,4)只有未知数 θ 5 \theta_5 θ5，因此两个未知数两个方程，可解。
于是有
{ ( 2 , 3 ) : − s 1 r 13 + c 1 r 23 = − c 5 ( 2 , 4 ) : − s 1 x + c 1 y = − c 5 d 6 − d 4 (17) \begin{cases} (2,3): \ -s_1r_{13}+c_1r_{23}=-c_5 \\ (2,4): \ -s_1x+c_1y=-c_5d_6-d_4 \tag{17} \end{cases} {(2,3): −s1r13+c1r23=−c5(2,4): −s1x+c1y=−c5d6−d4(17)
消去 θ 5 \theta_5 θ5得
− ( r 13 d 6 − x ) s 1 + ( r 23 d 6 − y ) c 1 = d 4 (18) -(r_{13}d_6-x)s_1+(r_{23}d_6-y)c_1=d_4\tag{18} −(r13d6−x)s1+(r23d6−y)c1=d4(18)
为什么要写成这种形式？
因为求解过程中我们发现，这样的形式可以有一个计算公式，即
若有
− A s 1 + B c 1 = C (19) {-As_1+Bc_1=C}\tag{19} −As1+Bc1=C(19)
则
θ 1 = a t a n 2 ( B , A ) − a t a n 2 ( C , ± A 2 + B 2 − C 2 ) (20) \theta_1=atan2(B,A)-atan2(C,\pm\sqrt{A^2+B^2-C^2})\tag{20} θ1=atan2(B,A)−atan2(C,±A2+B2−C2)(20)
根据式 ( 18 ) (18) (18)，可以确定这里的
A = r 13 d 6 − x B = r 23 d 6 − y C = d 4 (21) \begin{aligned} &A=r_{13}d_6-x \\ &B=r_{23}d_6-y \\ &C =d_4\tag{21} \end{aligned} A=r13d6−xB=r23d6−yC=d4(21)
于是我们可以计算出 θ 1 \theta_1 θ1的两个解
求解 θ 5 \theta_{5} θ5
由于前面求解 θ 1 \theta_1 θ1时我们也已经找到了 θ 5 \theta_5 θ5的方程，因此当 θ 1 \theta_1 θ1解出来后， θ 5 \theta_5 θ5也就自然而然地能解了
( 2 , 3 ) : − s 1 r 13 + c 1 r 23 = − c 5 (22) (2,3): \ -s_1r_{13}+c_1r_{23}=-c_5\tag{22} (2,3): −s1r13+c1r23=−c5(22)
所以有
c 5 = s 1 r 13 − c 1 r 23 (23) c_5=s_1r_{13}-c_1r_{23}\tag{23} c5=s1r13−c1r23(23)
继而有
s 5 = ± 1 − ( s 1 r 13 − c 1 r 23 ) 2 (24) s_5= \pm\sqrt{1-(s_1r_{13}-c_1r_{23})^2}\tag{24} s5=±1−(s1r13−c1r23)2(24)
因此
θ 5 = a t a n 2 ( ± 1 − ( s 1 r 13 − c 1 r 23 ) 2 , s 1 r 13 − c 1 r 23 ) (25) \theta_5=atan2(\pm\sqrt{1-(s_1r_{13}-c_1r_{23})^2},s_1r_{13}-c_1r_{23})\tag{25} θ5=atan2(±1−(s1r13−c1r23)2,s1r13−c1r23)(25)
因为 θ 1 \theta_1 θ1有两个解，所以 θ 5 \theta_5 θ5有 2 × 2 = 4 2\times 2=4 2×2=4个解
求解 θ 6 \theta_{6} θ6
我们已经求解出 θ 1 \theta_1 θ1与 θ 5 \theta_5 θ5，观察式 ( 12 ) (12) (12)能发现， θ 2 θ 3 θ 4 \theta_2\theta_3\theta_4 θ2θ3θ4牵扯在一块，与此同时(2,1)和(2,2)就显得十分简洁，因此我们第三步求解 θ 6 \theta_6 θ6
{ ( 2 , 1 ) : − s 1 r 11 + c 1 r 21 = − s 5 c 6 ( 2 , 2 ) : − s 1 r 12 + c 1 r 22 = s 5 s 6 (26) \begin{cases} (2,1): \ -s_1r_{11}+c_1r_{21}=-s_5c_6 \\ (2,2): \ -s_1r_{12}+c_1r_{22}=s_5s_6 \tag{26} \end{cases} {(2,1): −s1r11+c1r21=−s5c6(2,2): −s1r12+c1r22=s5s6(26)
整理得
s 6 = − s 1 r 12 + c 1 r 22 s 5 c 6 = s 1 r 11 − c 1 r 21 s 5 (27) \begin{aligned} &s_6=\frac{-s_1r_{12}+c_1r_{22}}{s_5} \\\tag{27} &c_6=\frac{s_1r_{11}-c_1r_{21}}{s_5} \end{aligned} s6=s5−s1r12+c1r22c6=s5s1r11−c1r21(27)
因此
θ 6 = a t a n 2 ( − s 1 r 12 + c 1 r 22 s 5 , s 1 r 11 − c 1 r 21 s 5 ) (28) \theta_6=atan2(\frac{-s_1r_{12}+c_1r_{22}}{s_5} ,\frac{s_1r_{11}-c_1r_{21}}{s_5})\tag{28} θ6=atan2(s5−s1r12+c1r22,s5s1r11−c1r21)(28)
计算 θ 6 \theta_6 θ6时只需要代入同一组的 θ 1 \theta_1 θ1与 θ 5 \theta_5 θ5就行，这边不会产生额外的解
其实因为我们找到了两个方程，所以可以通过消去 θ 5 \theta_5 θ5的方式化作上面的式 ( 19 ) (19) (19)用公式计算，这说明当 θ 1 \theta_1 θ1解出来后， θ 5 \theta_5 θ5与 θ 6 \theta_6 θ6应该是同时可以解出来的，不分先后顺序
求解 θ 2 \theta_2 θ2、 θ 3 \theta_3 θ3、 θ 4 \theta_4 θ4
先求解 θ 234 \theta_{234} θ234
观察矩阵可以看到， θ 2 \theta_2 θ2、 θ 3 \theta_3 θ3、 θ 4 \theta_4 θ4基本都在一起，所以这三个要用整体的思想去看待与求解，我们这里先求解 θ 234 \theta_{234} θ234，即 θ 2 + θ 3 + θ 4 \theta_2+\theta_3 +\theta_4 θ2+θ3+θ4
由
{ ( 1 , 3 ) : c 1 r 13 + s 1 r 23 = − c 234 s 5 ( 3 , 3 ) : r 33 = − s 234 s 5 (29) \begin{cases} (1,3): \ c_1r_{13}+s_1r_{23}=-c_{234}s_5 \\ (3,3): \ r_{33}=-s_{234}s_5 \tag{29} \end{cases} {(1,3): c1r13+s1r23=−c234s5(3,3): r33=−s234s5(29)
可得
s 234 = − r 33 s 5 c 234 = − c 1 r 13 + s 1 r 23 s 5 (30) \begin{aligned} &s_{234}=-\frac{r_{33}}{s_5} \\\tag{30} &c_{234}=-\frac{c_1r_{13}+s_1r_{23}}{s_5} \end{aligned} s234=−s5r33c234=−s5c1r13+s1r23(30)
因此
θ 2 + θ 3 + θ 4 = a t a n 2 ( − r 33 s 5 , − c 1 r 13 + s 1 r 23 s 5 ) (31) \theta_2+\theta_3 +\theta_4=atan2(-\frac{r_{33}}{s_5},-\frac{c_1r_{13}+s_1r_{23}}{s_5})\tag{31} θ2+θ3+θ4=atan2(−s5r33,−s5c1r13+s1r23)(31)
这边也只需要代入相同组的解，不会产生额外的解
在编程计算中，这里算出来的结果和实际值相差180°，暂未找出错误
再求解 θ 2 \theta_{2} θ2
由
{ ( 1 , 4 ) : − c 234 s 5 d 6 + s 234 d 5 + c 23 a 3 + c 2 a 2 = c 1 x + s 1 y ( 3 , 4 ) : − s 234 s 5 d 6 − c 234 d 5 + s 23 a 3 + s 2 a 2 = z − d 1 (32) \begin{cases} (1,4): \ -c_{234}s_5d_6+s_{234}d_5+c_{23}a_3+c_2a_2=c_1x+s_1y \\ (3,4): \ -s_{234}s_5d_6-c_{234}d_5+s_{23}a_3+s_2a_2=z-d_1 \tag{32} \end{cases} {(1,4): −c234s5d6+s234d5+c23a3+c2a2=c1x+s1y(3,4): −s234s5d6−c234d5+s23a3+s2a2=z−d1(32)
我们已经求解出 θ 1 \theta_{1} θ1、 θ 5 \theta_{5} θ5、 θ 234 \theta_{234} θ234，所以可以把常量拿字母替代掉，让式子变得整洁一点
设
A = − c 234 s 5 d 6 + s 234 d 5 B = − s 234 s 5 d 6 − c 234 d 5 C = c 1 x + s 1 y D = z − d 1 (33) \begin{aligned} &A=-c_{234}s_5d_6+s_{234}d_5 \\ &B=-s_{234}s_5d_6-c_{234}d_5 \\ &C=c_1x+s_1y \\ \tag{33} &D =z-d_1 \end{aligned} A=−c234s5d6+s234d5B=−s234s5d6−c234d5C=c1x+s1yD=z−d1(33)
则式 ( 32 ) (32) (32)可化简为
{ c 23 a 3 = C − A − c 2 a 2 s 23 a 3 = D − B − s 2 a 2 (34) \begin{cases} c_{23}a_3=C-A-c_2a_2 \\ s_{23}a_3=D-B-s_2a_2 \tag{34} \end{cases} {c23a3=C−A−c2a2s23a3=D−B−s2a2(34)
这里再设
M = C − A N = D − B (35) \begin{aligned} &M=C-A \\ &N=D-B \tag{35} \end{aligned} M=C−AN=D−B(35)
对式 ( 34 ) (34) (34)两个等式平方相加消去 θ 23 \theta_{23} θ23，整理得
− ( − 2 N a 2 ) s 2 + 2 M a 2 c 2 = M 2 + N 2 + a 2 2 − a 3 2 (36) -(-2Na_2)s_2+2Ma_2c_2=M^2+N^2+a_2^2-a_3^2\tag{36} −(−2Na2)s2+2Ma2c2=M2+N2+a22−a32(36)
根据式 ( 19 ) ( 20 ) (19)(20) (19)(20)设
E = − 2 N a 2 F = 2 M a 2 G = M 2 + N 2 + a 2 2 − a 3 2 (37) \begin{aligned} &E=-2Na_2 \\ &F=2Ma_2 \tag{37} \\ &G=M^2+N^2+a_2^2-a_3^2 \end{aligned} E=−2Na2F=2Ma2G=M2+N2+a22−a32(37)
得
θ 2 = a t a n 2 ( F , E ) − a t a n 2 ( G , ± E 2 + F 2 − G 2 ) (38) \theta_2=atan2(F,E)-atan2(G,\pm\sqrt{E^2+F^2-G^2})\tag{38} θ2=atan2(F,E)−atan2(G,±E2+F2−G2)(38)
这里在同一组解的情况下， θ 2 \theta_2 θ2会产生额外的一个解，此时解的组数为 4 × 2 = 8 4\times2=8 4×2=8
然后反解 θ 23 \theta_{23} θ23
由式 ( 34 ) ( 35 ) (34)(35) (34)(35)得
s 23 = N − s 2 a 2 a 3 c 23 = M − c 2 a 2 a 3 (39) \begin{aligned} &s_{23}=\frac{N-s_2a_2}{a_3} \\ \tag{39} &c_{23}=\frac{M-c_2a_2}{a_3} \end{aligned} s23=a3N−s2a2c23=a3M−c2a2(39)
因此
θ 23 = a t a n 2 ( N − s 2 a 2 a 3 , M − c 2 a 2 a 3 ) (40) \theta_{23}=atan2(\frac{N-s_2a_2}{a_3} ,\frac{M-c_2a_2}{a_3})\tag{40} θ23=atan2(a3N−s2a2,a3M−c2a2)(40)
最后求解 θ 3 \theta_3 θ3、 θ 4 \theta_4 θ4
θ 3 = θ 23 − θ 2 (41) \theta_3=\theta_{23}-\theta_{2}\tag{41} θ3=θ23−θ2(41) θ 4 = θ 234 − θ 23 (42) \theta_4=\theta_{234}-\theta_{23}\tag{42} θ4=θ234−θ23(42)
因为这里的 θ 3 \theta_3 θ3、 θ 4 \theta_4 θ4是直接相减得到的，因此也不会产生额外的解
完整代码
Eigen是一个方便矩阵计算（当然不止这个功能）的库，可以去官网下载：http://eigen.tuxfamily.org/
#include 测试结果
(数据皆从仿真环境获得，不同的环境可能有不一样的数据配置)
测试数据1
输入： -118.43 -268.05 157.28 0.001 -3.166 -0.040
一组解为： -91.71 -98.96 -126.22 -46.29 91.39 358.22
测试数据2
输入： -63.78 -201.25 137.28 0.192 3.109 0.036
一组解为： -76.28 -83.49 -151.01 -36.32 91.85 20.76
不足之处
- 没有考虑角度为0°或90°的特殊情况，这边只是计算出了普遍的结果
- 一些角度计算出来和实际值相差360°，虽然不影响计算，但说明程序不够完善
代码仅供学习交流使用，如有错误或者疑问可以在评论区提出。转载请标明出处！