Vol. 18 No. 6, pp.823 830, 2000 823 Motion Planning of Discrete time Nonholonomic Systems with Difference Equation Constraints Hirohiko Arai The concept of discrete time nonholonomic systems, in which the constraints cannot be represented as algebraic equations of generalized coordinates, is introduced. In particular, the constraints formulated as differece equations of generalized coordinates are considered. Such systems can be seen in the digital control of continuous time nonholonomic systems, and in mechanical systems with repetitive and discontinuous constraints. Atwo wheeled mobile robot and pivoting manipulation of a polyhedral object are described as simple examples. The k step reachable region is defined as the set of the k th state which the system can reach from the initial state, and the reachability of such systems is discussed. Amotion planning method using the Jacobian matrix of the state with regard to the input series is proposed. Key Words: Nonholonomic System, Discrete Time System, Difference Equation, Nonlinear System, Step Reachability, Motion Planning 1. [1] [2] 2 3 1998 11 27 Mechanical Engineering Laboratory, AIST, MITI 4 5 2. 2. 1 q t h(q,t)=0 1 1 q q 1 h(q, q) =0 2 u q = G(q, u) 3 2 [3] 18 6 65 2000 9
824 u q under actuated 2 h(q) q = 0 4 Pfaffian q = G(q)u 5 1 [4] 2 2. 2 1 5 k q k =(q1 k,...,qn) k T M, u k =(u k 1,...,u k m) T Ω m<n 1 k h(q k,k)=0 6 q k k 1 6 2 6 2 3 q k+1 = G(q k, u k ) 7 7 u k h(q k+1, q k )=0 8 n m 6 2 q k+2, q k+3,... 7 8 1 Monaco Normand Cyrot [5] [7] Chained Chained 3 q = G cont(q, u) T q(t) q k q k = q(k T ) (k =0, 1,...) k T t<(k + 1) T u(t) =u k (k+1) T q k+1 = q k + G cont(q(t), u k )dt 9 k T u k, T 2 q k u k 9 7 u(t) u k JRSJ Vol. 18 No. 6 66 Sept., 2000
825 3 1 7 q (q k+1 q k )/ T Euler q k+1 = q k + G cont(q k, u k ) T 10 Kelly Murray [8] Goodwine Burdick stratified manifold [9] [10] 3. 3. 1 2 Fig. 1 Fig. 1 Two wheeled mobile robot (x, y) θ ω L,ω R r l ẋ = r(ω R + ω L) cos θ/2 ẏ = r(ω R + ω L) sin θ/2 11 θ = r(ω R ω L)/l ω R,ω L T ω k R,ω k L v k = r(ω k R + ω k L)/2, x k+1 = x k + T 0 y k+1 = y k + T 0 θ k+1 = θ k + T 0 u k v = v k T, ω k = r(ω k R ω k L)/l v k cos(θ k + ω k t)dt v k sin(θ k + ω k t)dt ω k dt u k ω = ω k T u k ω 0 x k+1 = x n + {sin(θ k + u k ω) sin θ k }u k v/u k ω y k+1 = y n {cos(θ k + u k ω) cos θ k }u k v/u k ω θ k+1 = θ k + u k ω u k ω =0 12 13 x k+1 = x k + cos θ k u k v y k+1 = y k + sin θ k u k v 14 θ k+1 = θ k 13 u k v, u k ω (x k+1 x k )(cos θ k+1 cos θ k )+ (y k+1 y k )(sin θ k+1 sin θ k )=0 15 14 15 4 Pfaffian 5 13 7 q k =(x k,y k,θ k ) T u k =(u k v,u k ω) T G(q k, u k ) 1 0 0 q k q k = 0 1 0 0 u k 0 0 1 0 1 0 G(q k, u k ) u k q k = 0 0 0 u k 0 1 0 14 18 6 67 2000 9
826 1 0 0 1 0 AB u k A, u k q k+1 = 0 1 0 q k B AB l + 0 0 u k 0 0 1 0 1 x k+1 = x k + l cos θ k l cos(u k B θ k ) y k+1 = y k + l sin θ k + l sin(u k B θ k ) 16 θ k+1 = θ k + u k A u k B 13 14 q 0 =(0, 0, 0) T 7 [12] Fig. 4 u 0 =(δ, 0) T q 1 =(δ, 0, 0) T 2 u 0 =(δ, π) T, u 1 =(δ, π) T q 2 =(0, 4δ/π,0) T u 0 =(0,δ) T q 1 =(0, 0,δ) T 16 u k A, u k B u k v,u k ω x, y, θ 6 3. 2 [11] Fig. 2 [11] Fig. 3 B AB u k B A u k A 1 A x k, y k AB θ k (x k+1 x k l cos θ k ) 2 +(y k+1 y k l sin θ k ) 2 = l 2 17 8 A B 16 q k =(x k,y k,θ k ) T u k =(u k A,u k B) T 1 0 0 0 0 q k+1 = 0 1 0 q k + 0 l u k 0 0 1 1 1 q 0 =(0, 0, 0) T u 0 =(δ, δ) T, u 1 =( δ, δ) T q 2 =(2l(1 cos δ), 0, 0) T u 0 =(2δ, δ) T, u 1 =(0,δ) T q 2 =(0, 2l sin δ, 0) T u 0 =(δ, 0) q 1 =(0, 0,δ) T Fig. 5 Fig. 2 Pivoting of an object Fig. 4 Biped robot equivalent to pivoting Fig. 3 Model of planar pivoting Fig. 5 Motion in each coordinate JRSJ Vol. 18 No. 6 68 Sept., 2000
827 4. 4. 1 7 q 1 = G(q 0, u 0 ) = G 1(q 0, u 0 ) q 2 = G(q 1, u 1 ) = G 2(q 0, u 0, u 1 )... q k = G(q k 1, u k 1 ) = G k (q 0, u 0,..., u k 1 ) 18 u i u j (i j) 3 7 q 0 k q k q 0 k Λ(q 0,k) Λ(q 0,k) 19 = {q k ; q j+1 = G(q j, u j ), u j Ω, 0 j k 1} 3.2 (0, 0, 0) T 1 2 Fig. 6 1 17 l (a) 1 2 (a) (b) 4. 2 [13] Fig. 6 Step reachable region 4 7 q k M q 0 M k Λ(q 0,k) M q 0 k 1 k k 2 q 0 M q 0 Λ(q 0, 1) k j k j 18 q k = G k (q 0, u 0, u 1,..., u k 1 ) k m k U k =(u 0T,..., u k 1T ) T q 0 G k U k J k = G ( ) k Gk = U k u,..., G k 20 0 u k 1 U k = U k0 + δu k G k (q 0, U k0 )+δq k δq k = J k δu k + O( δu k 2 ) J k q k M U k M 1 7 20 J k rank J k = dim M(= n) 21 18 6 69 2000 9
828 U k k Λ(q 0,k) M k J k G k (q 0, U k ) 1 G(q, u) G j G(q, u) q = q q = q j G j G(q, u) u = u q = q j u = u j u = u j J k 18 G k u i q... G i+1 q G i u 22 Jakubczyk Sontag [14] [14] 21 21 J k q k+1 = Aq k + Bu k J k 20 22 J k =(A k 1 B,..., AB, B) 23 n n ν ν =min{k : rank(a k 1 B,..., AB, B) =n} 1 k 1 3.2 (0, 0, 0) T 2 J 2 detj 2J2 T = l 4 {3 cos u 0 A cos(2u 0 A) + cos(u 0 A 2u 1 B) cos(2(u 0 A u 1 B)) + cos(2u 0 A u 1 B) cos u 1 B cos(2u 1 B)} u 0 A =0,u 1 B =0 J 2 3 J 2 3 1 2 4. 3 7 G j u 8 q k h(q k )=0 24 n m U k q k 24 m G k 1 u m u k 1 q k 2 7 24 k U k 20 J k rank J k = dimω(= m) 25 1 25 2 1 2 j G j+1 q G j u G j+1 u G j+1 q G j u = G j+1 u m Q j 22 G k u i q...g i+2 q...g i+2 Q j q G i+1 q G i u q G i+1 u Q i q...g i+2 u Q i+1q ir... u Q k 2...Q i+1q i 20 J k G k 1 u 3 7 j G j+1 q G j u G j+1 u 25 1 JRSJ Vol. 18 No. 6 70 Sept., 2000
829 5. 7 q 0 k qd k U k =(u 0,..., u k 1 ) qd k q 0 k k 21 k 18 U k q k d = G k (q 0, U k ) 26 Fig. 7 Motion planning algorithm 26 U k qd k 7 J k = G k / U k [15] U k q 0 q k 18 J k 20 22 J k J + k = J k T (J k Jk T ) 1 U k = J + k Kp(qk d q k )+(I J + k J k)f(u k ) 27 1 q k qd k K p 2 f(u k ) 2 1 V (U k ) f(u k )= V / U k I J + k J k q k 27 U k U k + U k q k qd k q k U k Fig. 7 3.1 2 k = 8 2 f(u k ) = Heimann [16] Fig. 8 (K vu 0 v,k ωu 0 ω,...,k vu k 1 v Planned motion of mobile robot,k ωuω k 1 ) k 1 i=0 {Kv(ui v) 2 + K ω(u i ω) 2 } Fig. 8 (a) (0, 0, π/2) (5, 10, π/2) (b) (0, 0, 0) (0, 10, π) (c) (0, 0, π/2) (0, 10, π/2) u i v =1,u i ω =0 (i =0,...,k 1) (c) 2 (a) (a) 2 (b) Fig. 9 100 CPU 150 [MHz] 0.17 [sec] 3.2 8 2 f(u k )= K u(u 0 A,u 0 B,...,u k 1 A,uk 1 B ) 18 6 71 2000 9
830 Fig. 9 Fig. 10 Error and cost function Planned motion of pivoting k 1 i=0 {(ui A) 2 +(u i B) 2 } Fig. 10 (a) (0, 0, 0) (3, 6, 0) (b) (0, 0, π/2) (0, 6, π/2) (c) (0, 0, π/2) (0, 6, π/2) l =1 u i A = u i B =0.5 (i =0,...,k 1) 6. 2 [1] 1 5 vol.11, no.4 7, 1993, vol.12, no.2, 1994. [2] vol.36, no.6, pp.384 416, 1997. [3] 1 1994. [ 4 ] G. Oriolo and Y. Nakamura: Free Joint Manipulators: Motion Control under Second Order Nonholonomic Constraints, Proc. IEEE/RSJ Int. Workshop on Intelligent Robots and Systems (IROS 91), pp.1248 1253, 1991. [ 5 ] S. Monaco and D. Normand Cyrot: An Introduction to Motion Planning Under Multirate Digital Control, Proc. 31st IEEE Conf. Decision and Control, pp.1780 1785, 1992. [ 6 ] A. Chelouah et al.: Digital Control of Nonholonomic Systems: Two Case Studies, Proc. 32nd IEEE Conf. Decision and Control, pp.2664 2669, 1993. [ 7 ] P. Di Giamberadino et al.: Piecewise Continuous Control for a Car Like Robot: Implementation and Experimental Results, Proc. 35th IEEE Conf. Decision and Control, pp.3564 3569, 1996. [ 8 ] S. Kelly and R. Murray: Geometric Phases and Robotic Locomotion, J. Robotic Systems, vol.12, no.6, pp.417 431, 1995. [ 9 ] B. Goodwine and J. Burdick: Trajectory Generation for Kinematic Legged Robots, Proc. 1997 IEEE Int. Conf. Robotics and Automation, pp.2689 2696, 1997. [10] B. Goodwine and J. Burdick: Gait Controllability for Legged Robots, Proc. 1998 IEEE Int. Conf. Robotics and Automation, pp.484 489, 1998. [11] vol.14, no.1, pp.114 121, 1996. [12] T. Yano, S. Numao and Y. Kitamura: Development of a Self Contained Wall Climbing Robot with Scanning Type Suction Cups, Proc. IEEE/RSJ Int. Conference on Intelligent Robots and Systems (IROS 98), pp.249 254, 1998. [13] 1993. [14] B. Jakubczyk and E. Sontag: Controllability of Nonlinear Discrete time Systems: A Lie Algebraic Approach, SIAM J. Control and Optimization, vol.28, no.1, pp.1 33, 1990. [15] Y. Nakamura, H. Hanafusa and T. Yoshikawa: Task Priority Based Redundancy Control of Robot Manipulators, Int. J. Robotics Research, vol.6, no.2, pp.3 15, 1987. [16] B. Heimann: Feedback Linearization of Underactuated Manipulation Systems, Proc. 3rd Int. Conf. Advanced Mechatronics (ICAM 98), pp.163 166, 1998. Hirohiko Arai 1959 7 9 1982 1984 IEEE JRSJ Vol. 18 No. 6 72 Sept., 2000