Motion Planning of Discrete time Nonholonomic Systems with Difference Equation Constraints

Transcription

1 Vol. 18 No. 6, pp , 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] Mechanical Engineering Laboratory, AIST, MITI q t h(q,t)=0 1 1 q q 1 h(q, q) =0 2 u q = G(q, u) 3 2 [3]

2 824 u q under actuated 2 h(q) q = 0 4 Pfaffian q = G(q)u 5 1 [4] 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 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, 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 Sept., 2000

3 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] 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 ω = 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 )= Pfaffian q k =(x k,y k,θ k ) T u k =(u k v,u k ω) T G(q k, u k ) q k q k = u k G(q k, u k ) u k q k = u k

4 AB u k A, u k q k+1 = q k B AB l u k 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 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, θ [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 A B 16 q k =(x k,y k,θ k ) T u k =(u k A,u k B) T q k+1 = q k + 0 l u k 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 Sept., 2000

5 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 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 J k rank J k = dim M(= n)

6 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] J k q k+1 = Aq k + Bu k J k J k =(A k 1 B,..., AB, B) 23 n n ν ν =min{k : rank(a k 1 B,..., AB, B) =n} 1 k (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 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 k U k 20 J k rank J k = dimω(= m) 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 Sept., 2000

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 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 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 CPU 150 [MHz] 0.17 [sec] f(u k )= K u(u 0 A,u 0 B,...,u k 1 A,uk 1 B )

8 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, [2] vol.36, no.6, pp , [3] [ 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 , [ 5 ] S. Monaco and D. Normand Cyrot: An Introduction to Motion Planning Under Multirate Digital Control, Proc. 31st IEEE Conf. Decision and Control, pp , [ 6 ] A. Chelouah et al.: Digital Control of Nonholonomic Systems: Two Case Studies, Proc. 32nd IEEE Conf. Decision and Control, pp , [ 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 , [ 8 ] S. Kelly and R. Murray: Geometric Phases and Robotic Locomotion, J. Robotic Systems, vol.12, no.6, pp , [ 9 ] B. Goodwine and J. Burdick: Trajectory Generation for Kinematic Legged Robots, Proc IEEE Int. Conf. Robotics and Automation, pp , [10] B. Goodwine and J. Burdick: Gait Controllability for Legged Robots, Proc IEEE Int. Conf. Robotics and Automation, pp , [11] vol.14, no.1, pp , [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 , [13] [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, [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, [16] B. Heimann: Feedback Linearization of Underactuated Manipulation Systems, Proc. 3rd Int. Conf. Advanced Mechatronics (ICAM 98), pp , Hirohiko Arai IEEE JRSJ Vol. 18 No Sept., 2000