25 6 2008 2 Control Theory & Applications Vol. 25 No. 6 Dec. 2008 : 000 852(2008)06 037 05 6R,,, (, 30027) : 6R. 6 6R6.., -, 6R., 2.03 ms, 6R. : 6R; ; ; : TP242.2 : A Research on real-time inverse kinematics algorithms for 6R robots LIU Song-guo, ZHU Shi-qiang, LI Jiang-bo, WANG Xuan-yin (State Key Lab of Fluid Power Transmission and Control, Zhejiang University, Hangzhou Zhejiang 30027, China) Abstract: A set of real-time algorithms for inverse kinematics of all type of 6R robots is proposed. The general algorithm obtains 6 inverse kinematics solutions in total for general 6R robots based on vector operations and eigenvaluedecomposition of a 6 order target matrix. The closed-form algorithm selects proper kinematics equations directly and solves for the joint variables analytically. The obtained results are employed by the combined algorithm as the initial values in the iterative Newton-Raphson method for finding the exact solutions of the inverse kinematics, which can be used for 6R robots that approximately meet the requirements of the closed-form algorithm or the general algorithm. Experimental results show that the proposed set of algorithms solves the inverse kinematics problem of 6R robots with any geometry configuration in 2.03 ms, and provides effective solutions for the inverse kinematics problem of 6R robots applied in strong real-time systems. Key words: 6R robots; inverse kinematics; real-time algorithm; combined algorithm (Introduction) 6R,. 6R Pieper [], 33, [2], PUMA560. Pieper 6R, [3],,. 6R Pieper, 6R, [5 0]. Raghavan Roth 6 4 [5], 24, 6, 8. Manocha 24 Raghavan [6],. [7,8] 7R, 0, 6,, 6R. 6R. Pieper 6R. Manocha 24 6, 6R. - [4], Pieper 6R : 2007 05 8; : 2008 0 0.
038 25 6R,. C++, 6R 2.03 ms. 2 (Inverse kinematics algorithms) D-H, 6R T End = T T 2 T 3 T 4 T 5 T 6, () : T i = R z (θ i )T z (d i )T x (a i )R x (α i )(i =, 2,, 6), a i, α i, d i, θ i, T End. 6R d i, a i α i T End, θ i. 2. (General algorithm) 6R. [6]24 24,, : 24 24 ; 8. P L, 0 0 c 2 s 2 0 h P = 0 λ 2 µ 2 s 2 c 2 0 h 2 0 µ 2 λ 2 0 0 h 3 a 2 c 3 s 3 0 f 0 = s 3 c 3 0 f 2, d 2 0 0 f 3 0 0 c 2 s 2 0 n L = 0 λ 2 µ 2 s 2 c 2 0 n 2 = 0 µ 2 λ 2 0 0 n 3 c 3 s 3 0 r s 3 c 3 0 r 2. 0 0 r 3 : µ i = sin α i, λ i = cos α i, h v = F SC(c, s, c 2, s 2 ), f v = F SC(c 4, s 4, c 5, s 5 ), n v = F SC(c, s, c 2, s 2 ), r v = F SC(c 4, s 4, c 5, s 5 ), v =, 2, 3, i =, 2,, 5, FSC, s i = sin θ i, c i = cos θ i. P L, P, L, P P, P L, P L (P P )L (2P L)P, 4 : EQ : P L (θ 3, θ 4, θ 5 ) = P R (θ, θ 2 ); EQ2 : P L2 (θ 3, θ 4, θ 5 ) = P R2 (θ, θ 2 ); EQ3 : P L3 (θ 4, θ 5 ) = P R3 (θ, θ 2 ); EQ4 : L L (θ 3, θ 4, θ 5 ) = L R (θ, θ 2 ); EQ5 : L L2 (θ 3, θ 4, θ 5 ) = L R2 (θ, θ 2 ); EQ6 : L L3 (θ 4, θ 5 ) = L R3 (θ, θ 2 ); EQ7 : (P P ) L (θ 4, θ 5 ) = (P P ) R (θ, θ 2 ); EQ8 : (P L) L (θ 4, θ 5 ) = (P L) R (θ, θ 2 ); EQ9 : (P L) L (θ 3, θ 4, θ 5 )=(P L) R (θ, θ 2 ); EQ0 : (P L) L2 (θ 3, θ 4, θ 5 )=(P L) R2 (θ, θ 2 ); EQ : (P L) L3 (θ 4, θ 5 ) = (P L) R3 (θ, θ 2 ); EQ2 : [(P P )L (2P L)P ] L (θ 3, θ 4, θ 5 ) = [(P P )L (2P L)P ] R (θ, θ 2 ); EQ3 : [(P P )L (2P L)P ] L2 (θ 3, θ 4, θ 5 ) = [(P P )L (2P L)P ] R2 (θ, θ 2 ); EQ4 : [(P P )L (2P L)P ] L3 (θ 4, θ 5 ) = [(P P )L (2P L)P ] R3 (θ, θ 2 ). : L R,, 2 3., EQ3, EQ6, EQ7, EQ8, EQ EQ4 6 θ 3,, L 6 [s 4 s 5 s 4 c 5 c 4 s 5 c 4 c 5 s 4 c 4 s 5 c 5 s 2 c 2 ] T = R 6 6 [s s 2 s c 2 c s 2 c c 2 s c ] T. (2) : L 6 R 6 6. x 3 = tan(θ 3 /2), θ 3 8, 8 : EQ L + x 3 EQ2 L = EQ R + x 3 EQ2 R, EQ2 L x 3 EQ L = EQ2 R x 3 EQ R, EQ4 L + x 3 EQ5 L = EQ4 R + x 3 EQ5 R, EQ5 L x 3 EQ4 L = EQ5 R x 3 EQ4 R, EQ9 L + x 3 EQ0 L = EQ9 R + x 3 EQ0 R, EQ0 L x 3 EQ9 L = EQ0 R x 3 EQ9 R, EQ2 L + x 3 EQ3 L = EQ2 R + x 3 EQ3 R, EQ3 L x 3 EQ2 L = EQ3 R x 3 EQ2 R., L2 8 (x 3 )[s 4 s 5 s 4 c 5 c 4 s 5 c 4 c 5 s 4 c 4 s 5 c 5 s 2 c 2 ] T = R2 8 6 [s s 2 s c 2 c s 2 c c 2 s c ] T. (3) : R2 8 6, L2 8 (x 3 ) x 3. rank(r 6 6 ) = 6, (2) R 6 6,
6 : 6R 039 x i = tan(θ i /2), i = 4, 5, (3), ( + x 2 4)( + x 2 5), L3 8 (x 3 ) [ x 2 4x 2 5 x 2 4x 5 x 4 x 2 5 x 4 x 5 x 2 4 x 4 x 2 5 x 5 s 2 k c 2 k ] T = 0 8. (4) k = ( + x 2 4)( + x 2 5), L3 8 (x 3 ) L2 8 (x 3 ). (4) x 4, 8, (4) 8, L4 6 6 (x 3 )[x 3 4x 2 5 x 3 4x 5 x 2 4x 2 5 x 2 4x 5 x 3 4 6R6R,. PUMA560, D-H. L(i, j) R(i, j) 4 4 L R i j, : L = T T End = T 2 T 3 T 4 T 5 T 6 = R, L(3, 4) = R(3, 4), L(, 4) = R(, 4), L(2, 4) = R(2, 4) (8) x 2 4 x 4 x 2 5 x 4 x 5 x 4 s 2 k x 4 c 2 k x 4 x 2 5 x 5 s 2 k c 2 k ] T = 0 6., L4(x 3 )(V 245 ) = 0. (5) : L4(x 3 ) = Ax 3 + B, A, B R 6 6, V 245 θ 4 θ 5 θ 2. (5) det[l4(x 3 )] = 0. (6). A, L4(x 3 ) = 0 Ix 3 + A B = 0. (7) I 6 6. M = A B,, M 6 x 3 6, V 245, V 245 x 4, x 5, s 2 c 2, θ 2, θ 3, θ 4 θ 5, (2), θ, 5 (), θ 6. 6R 3, ( ) 02 6 R 6 6 =. R 4 6 6R3, A = ( R 6 2 0 6 4 ). 6R Pieper,, Pieper 6R. Pieper 6R,. 2.2 (Close form algorithm) 6R Pieper,. Pieper 6R,. Pieper, PUMA560. Pieper θ θ 3 [ 80, 80 ]., L = (T T 2 T 3 ) T End = T 4 T 5 T 6 = R, L(, 4) = R(, 4), L(3, 4) = R(3, 4), (9) L(, 3) = R(, 3), θ 2 [ 80, 80 ] θ 4. L = (T T 2 T 3 T 4 ) T End = T 5 T 6 = R, L(, 3) = R(, 3), (0) θ 5 [ 80, 80 ]. L = (T T 2 T 3 T 4 T 5 ) T End = T 6 = R, L(, 3) = R(, 3), () θ 6 [ 80, 80 ]. θ, θ 3 θ 4, θ 2, θ 5 θ 6,, PUMA560 8. PUMA560D-H Table D-H parameters of PUMA560 robot θ/( ) d/mm a/mm α/( ) θ 2 50 90.0 2 θ 2 0 550 0 3 θ 3 0 75 90.0 4 θ 4 650 0 90.0 5 θ 5 0 0 90.0 6 θ 6 0 0 0 2.3 (Combined algorithm) 6R ν ω6, d δ6 D, (ν ) ω = lim t 0 t ( ) d = lim δ t 0 D. (2) t
040 25, 6R 6 6 J : ( ) ν = J(θ) dθ ω dt. (3) (2) (3) lim t 0 t D = J(θ)dθ dt, D = J(θ)dθ. 6R T End : T End = (I + TCur ), = T Cur T End I., 0 δ z δ y d x δ = z 0 δ x d y δ y δ x 0 d z, 0 0 0 : d x, d y d z d, δ x, δ y δ z δ. J(θ), dθ = J (θ)d., - 6R, : Step θ Cur, θ Cur (), ; Step 2 T End : = T Cur T End I; Step 3 D, dθ = J (θ Cur )D; Step 4 dθ ξ, θ Cur, ξ; N,, N ; dθ > ξ N, θ Cur = θ Cur + dθ, = T (θ Cur ), Step 2 Step 4,. 2.3. I(Combined algorithm I) Pieper6R,,,, -.,., Pieper 6R 6R,. 2.3.2 II(Combined algorithm II) 6R Pieper,,.,,,,. 3 (Experiments and performance) Pentium IV 2.4G CPU, 52M RAM, Windows 2000, VC++ NEWMAT CLAPCAK. Table 2 2 8 Eight solutions of inverse kinematics θ/( ) θ/( ) 89.06600660 2 74.8564945 3 79.7228035 4 70.845305 5 94.4794234 6 6.3625337 88.5542942 2 75.5308876 2 3 79.605998 4 9.060946030 5 94.77339928 6 64.8684275 93.04835838 2 6.535200275 3 3 60.390667 4 7.380443 5 7.8629364 6 72.575594 93.23458698 2 6.72560300 4 3 60.5885754 4 05.9482565 5 72.080762 6 0.0052334 90.00000000 2 40.0000000 5 3 49.99999999 4 9.999999999 5 80.0000000 6 20.0000000 90.47589037 2 39.3355973 6 3 49.2995095 4 70.0932884 5 79.95845685 6 60.492374 85.24308956 2 65.0634692 7 3 29.930875 4 60.048599 5 7.4032793 6 5.45247788 85.5559833 2 65.0269727 8 3 30.24760494 4 2.529607 5 7.870577 6 73.2035008
6 : 6R 04 PUMA560, D- H, θ = 90, θ 2 = 40, θ 3 = 50, θ 4 = 0, θ 5 = 80 θ 6 = 20, : d 2 = d 3 = d 5 = d 6 = a 4 = a 5 = a 6 = 2, α 2 = α 6 =. I8, 2. 6R, 3. Manocha, 4.6 ms,.37 ms,,. 3 Table 3 Applicability and real-time performance of algorithms Pieper /ms PUMA560 0. 0. PUMA560 I 0.77.37 I 0.77.37 0..37 II 2.03, 6R,, Pieper 6R, 6R. 4 (Conclusion) 6R. 6R Pieper, Pieper,. Pieper 6R, 6. I II Pieper 6R 6R. C++, 2.03 ms, 0. ms, 6R. (References): []. [M]. :, 996. (XIONG Youlun. Fundamentals of Robot Techniques[M]. Wuhan: Huazhong University of Science and Technology Press, 996.) [2] PAUL R P, ZHANG H. Computational efficient kinematics for manipulators with spherical wrists based on homogeneous transformation representation[j]. International Journal of Robotics Research, 986, 5(2): 30 42. [3] DOLINSKY J U, JENKINSON I D, COLQUHOUN G J. Application of genetic programming to the calibration of industrial robots[j]. Computers in Industry, 2007, 58(3): 255 264. [4] ANGELES J. On the numerical solution to the inverse kinematics problem[j]. International Journal of Robotics Research, 985, 4(2): 2 37. [5] RAGHAVAN M, ROTH B. Kinematic analysis of the 6R manipulator of general geometry[c]//international Journal of Robotics Research. Tokyo: MIT Press, 989: 34 320. [6] MANOCHA D, CANNY J F. Efficient inverse kinematics for general 6R manipulators[j]. IEEE Transaction on Robotics and Automation, 994, 5(9): 648 657. [7],, 7R [J], 986, 22(3): 9. (LIAO Qizheng, LIANG Chonggao, ZHANG Qixian. A novel approach to the displacement analysis of general spatial 7R mechanism[j]. Chinese Journal of Mechanical Engineering, 986, 22(3): 9.) [8] LEE H Y, LIANG C G. Displacement analysis of the general spatial 7-link 7R mechanism[j]. Mechanism and Machine Theory, 988, 23(3): 29 226. [9] HUSTY M L, PFURNER M, SCHROCKER H P. A new and efficient algorithm for the inverse kinematics of a general serial 6R manipulator[j]. Mechanism and Machine Theory, 2007, 42(): 66 8. [0] CHAPELLE F, BIDAUD P. A closed form for inverse kinematics approximation of general 6R manipulators using genetic programming[c]//proceedings of the 200 IEEE International Conference on Robotics & Automation. Seoul, USA: IEEE Press, 200: 3364 3369. : (980 ),,,, E-mail: sgliu@zju.edu.cn; (966 ),,,, E-mail: sqzhu@zju.edu.cn; (983 ),,,, E-mail: ernist@zju.edu.cn; (966 ),,,, E-mail: xywang@zju.edu.cn.