Research on real-time inverse kinematics algorithms for 6R robots

Transcript

1 Control Theory & Applications Vol. 25 No. 6 Dec : (2008) R,,, (, 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 : ; :

2 R,. 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,, : ; 8. P L, 0 0 c 2 s 2 0 h P = 0 λ 2 µ 2 s 2 c 2 0 h 2 0 µ 2 λ h 3 a 2 c 3 s 3 0 f 0 = s 3 c 3 0 f 2, d f c 2 s 2 0 n L = 0 λ 2 µ 2 s 2 c 2 0 n 2 = 0 µ 2 λ n 3 c 3 s 3 0 r s 3 c 3 0 r 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,

3 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 ). 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,, PUMA PUMA560D-H Table D-H parameters of PUMA560 robot θ/( ) d/mm a/mm α/( ) θ θ θ θ θ θ (Combined algorithm) 6R ν ω6, d δ6 D, (ν ) ω = lim t 0 t ( ) d = lim δ t 0 D. (2) t

4 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, : 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, I(Combined algorithm I) Pieper6R,,,, -.,., Pieper 6R 6R, II(Combined algorithm II) 6R Pieper,,.,,,,. 3 (Experiments and performance) Pentium IV 2.4G CPU, 52M RAM, Windows 2000, VC++ NEWMAT CLAPCAK. Table Eight solutions of inverse kinematics θ/( ) θ/( )

5 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 PUMA PUMA560 I I 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): [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): [4] ANGELES J. On the numerical solution to the inverse kinematics problem[j]. International Journal of Robotics Research, 985, 4(2): [5] RAGHAVAN M, ROTH B. Kinematic analysis of the 6R manipulator of general geometry[c]//international Journal of Robotics Research. Tokyo: MIT Press, 989: [6] MANOCHA D, CANNY J F. Efficient inverse kinematics for general 6R manipulators[j]. IEEE Transaction on Robotics and Automation, 994, 5(9): [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): [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(): [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: : (980 ),,,, sgliu@zju.edu.cn; (966 ),,,, sqzhu@zju.edu.cn; (983 ),,,, ernist@zju.edu.cn; (966 ),,,, xywang@zju.edu.cn.