UR3机械臂正逆运动学详解及c++完整代码

2024-06-17 04:48

本文主要是介绍UR3机械臂正逆运动学详解及c++完整代码,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

博主也是刚开始学习机械臂,不断搜索网上的教程,找论文,费尽千辛万苦终于计算正确,学习之路太过坎坷

D-H参数表

i i i α i − 1 \alpha_{i-1} αi1 d i d_{i} di a i − 1 a_{i-1} ai1 θ 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{*} ii1T=cisicαi1sisαi10sicicαi1cisαi100sαi1cαi10ai1sαi1dicαi1di1(*)
所以我们可以得到六个变换矩阵(这是代入 α \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)=c1s100s1c100001000d11(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)=c20s20s20c2001000001(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)=c3s300s3c3000010a2001(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)=c4s400s4c4000010a30d41(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)=c50s50s50c5001000d501(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)=c60s60s60c6001000d601(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
ur3正运动学求解范例

逆运动学

准备工作

首先推得
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} 10T1(θ1)=c1s100s1c100001000d11(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=c230s230s230c2300100c2a20s2a21(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=c2340s2340s2340c23400100c23a3+c2a2d4s23a3+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=c234c5s5s234c50c234s5c5s234s50s2340c2340s234d5+c23a3+c2a2d4c234d5+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=c234c5c6s234s6s5c6s234c5c6+c234s60c234c5s6s234c6s5s6s234c5s6+c234c60c234s5c5s234s50c234s5d6+s234d5+c23a3+c2a2c5d6d4s234s5d6c234d5+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} 10T160T=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} 10T160T=c1r11+s1r21s1r11+c1r21r310c1r12+s1r22s1r12+c1r22r320c1r13+s1r23s1r13+c1r23r330c1x+s1ys1x+c1yzd11(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=c5d6d4(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} (r13d6x)s1+(r23d6y)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+B2C2 )(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=r13d6xB=r23d6yC=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=s1r13c1r23(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(s1r13c1r23)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(s1r13c1r23)2 ,s1r13c1r23)(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=s5s1r12+c1r22c6=s5s1r11c1r21(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(s5s1r12+c1r22,s5s1r11c1r21)(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): s234s5d6c234d5+s23a3+s2a2=zd1(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=s234s5d6c234d5C=c1x+s1yD=zd1(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=CAc2a2s23a3=DBs2a2(34)
这里再设
M = C − A N = D − B (35) \begin{aligned} &M=C-A \\ &N=D-B \tag{35} \end{aligned} M=CAN=DB(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+a22a32(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+a22a32(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+F2G2 )(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=a3Ns2a2c23=a3Mc2a2(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(a3Ns2a2,a3Mc2a2)(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 <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;//ur3数据,这是学长给我的数据,适用于我的仿真环境,大家可以根据自己的环境进行修改
const double d[6+1] = { 0,0.1519,0,0,0.11235,0.08535,0.0819 };//第0个不用
const double a[6] = { 0,0,-0.24365,-0.21325,0,0 };//a有0,没有6
const double alpha[6] = { 0,90,0,0,90,-90 };//alpha有0,没有6
double theta[8 + 1][6 + 1];//八组解,每组解六个角,第0个都不用//正运动学,用于检验
void kinematics(double* theta_input)
{Eigen::Matrix4d T[6 + 1];//为了和theta对应,0不用for (int i = 1; i <= 6; i++){T[i](0, 0) = cos(theta_input[i]);T[i](0, 1) = -sin(theta_input[i]);T[i](0, 2) = 0;T[i](0, 3) = a[i - 1];T[i](1, 0) = sin(theta_input[i]) * cos(alpha[i - 1] / 180 * EIGEN_PI);T[i](1, 1) = cos(theta_input[i]) * cos(alpha[i - 1] / 180 * EIGEN_PI);T[i](1, 2) = -sin(alpha[i - 1] / 180 * EIGEN_PI);T[i](1, 3) = -sin(alpha[i - 1] / 180 * EIGEN_PI) * d[i];T[i](2, 0) = sin(theta_input[i]) * sin(alpha[i - 1] / 180 * EIGEN_PI);T[i](2, 1) = cos(theta_input[i]) * sin(alpha[i - 1] / 180 * EIGEN_PI);T[i](2, 2) = cos(alpha[i - 1] / 180 * EIGEN_PI);T[i](2, 3) = cos(alpha[i - 1] / 180 * EIGEN_PI) * d[i];T[i](3, 0) = 0;T[i](3, 1) = 0;T[i](3, 2) = 0;T[i](3, 3) = 1;}Eigen::Matrix4d T06 = T[1] * T[2] * T[3] * T[4] * T[5] * T[6];cout << "检验得:X=" << T06(0, 3) * 1000 << "    Y=" << T06(1, 3) * 1000 << "     Z=" << T06(2, 3) * 1000;
}
int main()
{double x, y, z, RX, RY, RZ;printf("Please input x,y,z,RX,RY,RZ:\n");scanf("%lf%lf%lf%lf%lf%lf", &x, &y, &z, &RX, &RY, &RZ);//由于仿真软件的单位不同,这边进行转换x = x / 1000;y = y / 1000;z = z / 1000;//1.旋转向量转旋转矩阵,即求T06中的rEigen::Vector3d v(RX, RY, RZ);double t_alpha = v.norm();//求模v.normalize();//标准化Eigen::AngleAxisd rv(t_alpha, v);//旋转向量Eigen::Matrix3d rm;rm = rv.matrix();//2.求解double A, B, C, D, E, F, G, M, N;//用大写字母替代常数//注意,由于数组下标从0开始的问题,矩阵第一行第一列的元素是(0,0)//theta1A = rm(0, 2) * d[6] - x;B = rm(1, 2) * d[6] - y;C = d[4];//第一个解,赋给一到四组theta[1][1] = atan2(B, A) - atan2(C, sqrt(A * A + B * B - C * C));theta[2][1] = theta[1][1];theta[3][1] = theta[1][1];theta[4][1] = theta[1][1];//第二个解,赋给五到八组theta[5][1] = atan2(B, A) - atan2(C, -sqrt(A * A + B * B - C * C));theta[6][1] = theta[5][1];theta[7][1] = theta[5][1];theta[8][1] = theta[5][1];//theta5//由theta[1][1]产生的第一个解,赋给一到二组A = sin(theta[1][1]) * rm(0, 2) - cos(theta[1][1]) * rm(1, 2);theta[1][5] = atan2(sqrt(1 - A * A), A);theta[2][5] = theta[1][5];//由theta[1][1]产生的第二个解,赋给三到四组theta[3][5] = atan2(-sqrt(1 - A * A), A);theta[4][5] = theta[3][5];//由theta[5][1]产生的第一个解,赋给五到六组A = sin(theta[5][1]) * rm(0, 2) - cos(theta[5][1]) * rm(1, 2);theta[5][5] = atan2(sqrt(1 - A * A), A);theta[6][5] = theta[5][5];//由theta[5][1]产生的第二个解,赋给七到八组theta[7][5] = atan2(-sqrt(1 - A * A), A);theta[8][5] = theta[7][5];//theta6for (int i = 1; i <= 8; i++){A = (-sin(theta[i][1]) * rm(0, 1) + cos(theta[i][1]) * rm(1, 1)) / theta[i][5];B = (sin(theta[i][1]) * rm(0, 0) - cos(theta[i][1]) * rm(1, 0)) / theta[i][5];theta[i][6] = atan2(A, B);}//theta2、theta3、theta4for (int i = 1; i <= 8; i = i + 2){//先算theta2+theta3+theta4double theta234[8 + 1];A = rm(2, 2) / sin(theta[i][5]);B = (cos(theta[i][1]) * rm(0, 2) + sin(theta[i][1]) * rm(1, 2)) / sin(theta[i][5]);theta234[i] = atan2(-A, -B) - EIGEN_PI;theta234[i + 1] = theta234[i];//消去theta2+theta3,计算theta2A = -cos(theta234[i]) * sin(theta[i][5]) * d[6] + sin(theta234[i]) * d[5];B = -sin(theta234[i]) * sin(theta[i][5]) * d[6] - cos(theta234[i]) * d[5];C = cos(theta[i][1]) * x + sin(theta[i][1]) * y;D = z - d[1];M = C - A;N = D - B;E = -2 * N * a[2];F = 2 * M * a[2];G = M * M + N * N + a[2] * a[2] - a[3] * a[3];theta[i][2] = atan2(F, E) - atan2(G, sqrt(E * E + F * F - G * G));theta[i + 1][2] = atan2(F, E) - atan2(G, -sqrt(E * E + F * F - G * G));//用theta2反求theta2+theta3double theta23[8 + 1];theta23[i] = atan2((N - sin(theta[i][2]) * a[2]) / a[3], (M - cos(theta[i][2]) * a[2]) / a[3]);theta23[i + 1] = atan2((N - sin(theta[i + 1][2]) * a[2]) / a[3], (M - cos(theta[i + 1][2]) * a[2]) / a[3]);//theta3theta[i][3] = theta23[i] - theta[i][2];theta[i + 1][3] = theta23[i + 1] - theta[i + 1][2];//theta4theta[i][4] = theta234[i] - theta23[i];theta[i + 1][4] = theta234[i + 1] - theta23[i + 1];}//输出并检验for (int i = 1; i <= 8; i++){cout << "第" << i << "组解:" << endl;for (int j = 1; j <= 6; j++)cout << "theta" << j << "=" << theta[i][j] * 180 / EIGEN_PI << "  ";cout << endl;kinematics(theta[i]);cout << endl << endl;}
}

测试结果

(数据皆从仿真环境获得,不同的环境可能有不一样的数据配置)

测试数据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
在这里插入图片描述

不足之处

  1. 没有考虑角度为0°或90°的特殊情况,这边只是计算出了普遍的结果
  2. 一些角度计算出来和实际值相差360°,虽然不影响计算,但说明程序不够完善

代码仅供学习交流使用,如有错误或者疑问可以在评论区提出。转载请标明出处!

这篇关于UR3机械臂正逆运动学详解及c++完整代码的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/1068513

相关文章

Spring Security基于数据库验证流程详解

Spring Security 校验流程图 相关解释说明(认真看哦) AbstractAuthenticationProcessingFilter 抽象类 /*** 调用 #requiresAuthentication(HttpServletRequest, HttpServletResponse) 决定是否需要进行验证操作。* 如果需要验证,则会调用 #attemptAuthentica

大模型研发全揭秘:客服工单数据标注的完整攻略

在人工智能(AI)领域,数据标注是模型训练过程中至关重要的一步。无论你是新手还是有经验的从业者,掌握数据标注的技术细节和常见问题的解决方案都能为你的AI项目增添不少价值。在电信运营商的客服系统中,工单数据是客户问题和解决方案的重要记录。通过对这些工单数据进行有效标注,不仅能够帮助提升客服自动化系统的智能化水平,还能优化客户服务流程,提高客户满意度。本文将详细介绍如何在电信运营商客服工单的背景下进行

【C++ Primer Plus习题】13.4

大家好,这里是国中之林! ❥前些天发现了一个巨牛的人工智能学习网站,通俗易懂,风趣幽默,忍不住分享一下给大家。点击跳转到网站。有兴趣的可以点点进去看看← 问题: 解答: main.cpp #include <iostream>#include "port.h"int main() {Port p1;Port p2("Abc", "Bcc", 30);std::cout <<

C++包装器

包装器 在 C++ 中,“包装器”通常指的是一种设计模式或编程技巧,用于封装其他代码或对象,使其更易于使用、管理或扩展。包装器的概念在编程中非常普遍,可以用于函数、类、库等多个方面。下面是几个常见的 “包装器” 类型: 1. 函数包装器 函数包装器用于封装一个或多个函数,使其接口更统一或更便于调用。例如,std::function 是一个通用的函数包装器,它可以存储任意可调用对象(函数、函数

OpenHarmony鸿蒙开发( Beta5.0)无感配网详解

1、简介 无感配网是指在设备联网过程中无需输入热点相关账号信息,即可快速实现设备配网,是一种兼顾高效性、可靠性和安全性的配网方式。 2、配网原理 2.1 通信原理 手机和智能设备之间的信息传递,利用特有的NAN协议实现。利用手机和智能设备之间的WiFi 感知订阅、发布能力,实现了数字管家应用和设备之间的发现。在完成设备间的认证和响应后,即可发送相关配网数据。同时还支持与常规Sof

C++11第三弹:lambda表达式 | 新的类功能 | 模板的可变参数

🌈个人主页: 南桥几晴秋 🌈C++专栏: 南桥谈C++ 🌈C语言专栏: C语言学习系列 🌈Linux学习专栏: 南桥谈Linux 🌈数据结构学习专栏: 数据结构杂谈 🌈数据库学习专栏: 南桥谈MySQL 🌈Qt学习专栏: 南桥谈Qt 🌈菜鸡代码练习: 练习随想记录 🌈git学习: 南桥谈Git 🌈🌈🌈🌈🌈🌈🌈🌈🌈🌈🌈🌈🌈�

【C++】_list常用方法解析及模拟实现

相信自己的力量,只要对自己始终保持信心,尽自己最大努力去完成任何事,就算事情最终结果是失败了,努力了也不留遗憾。💓💓💓 目录   ✨说在前面 🍋知识点一:什么是list? •🌰1.list的定义 •🌰2.list的基本特性 •🌰3.常用接口介绍 🍋知识点二:list常用接口 •🌰1.默认成员函数 🔥构造函数(⭐) 🔥析构函数 •🌰2.list对象

活用c4d官方开发文档查询代码

当你问AI助手比如豆包,如何用python禁止掉xpresso标签时候,它会提示到 这时候要用到两个东西。https://developers.maxon.net/论坛搜索和开发文档 比如这里我就在官方找到正确的id描述 然后我就把参数标签换过来

06 C++Lambda表达式

lambda表达式的定义 没有显式模版形参的lambda表达式 [捕获] 前属性 (形参列表) 说明符 异常 后属性 尾随类型 约束 {函数体} 有显式模版形参的lambda表达式 [捕获] <模版形参> 模版约束 前属性 (形参列表) 说明符 异常 后属性 尾随类型 约束 {函数体} 含义 捕获:包含零个或者多个捕获符的逗号分隔列表 模板形参:用于泛型lambda提供个模板形参的名

poj 1258 Agri-Net(最小生成树模板代码)

感觉用这题来当模板更适合。 题意就是给你邻接矩阵求最小生成树啦。~ prim代码:效率很高。172k...0ms。 #include<stdio.h>#include<algorithm>using namespace std;const int MaxN = 101;const int INF = 0x3f3f3f3f;int g[MaxN][MaxN];int n