Đang tải... (xem toàn văn)
Bằng cách áp dụng ràng buộc nonholonomic và phương trình Lagrange cho hệ thống nonholonomic, một phương pháp được đưa ra để mô hình và điều khiển robot di động lái trượt 4 bánh chạy theo quỹ đạo cho trước. Đầu tiên, các cơ sở của hệ thống nonholonomic được giới thiệu. Tiếp theo, mô hình động học và động lực học của robot lái trượt được khảo sát. Để điều khiển robot dò theo quỹ đạo, một giải thuật mới được đưa ra bằng cách ứng dụng tuyến tính hóa hồi tiếp và bộ điều khiển PD. Hơn nữa, kết quả mô phỏng đã chứng tỏ tính hiệu quả của thuật toán.
TẠP CHÍ PHÁT TRIỂN KH&CN, TẬP 13, SỐ K4 - 2010 TRAJECTORY TRACKING CONTROL FOR WHEEL SKID-STEERING MOBILE ROBOT Dang Van Nghin(1), Nguyen Van Quoc Khanh(2) (1) Ho Chi Minh Institute of Mechanics and Informatics (2) University of Techonology, VNU-HCM (Manuscript Received on July 09th, 2009, Manuscript Revised December 29th, 2009) ABSTRACT: By applying a nonholonomic constraints and Lagrange equation for nonholonomic system, a method is given to model and control the 4-wheel skid-steering mobile robot which tracks a given trajectory First at all, a fundamental of nonholonomic system is introduced Next, the skid steering robot’s kinematic model and dynamic model are considered To control the robot tracking a trajectory, a new algorithm is given by applying feedback linearization and PD control In addition, simulation results show the good performance in tracking trajectories Keywords: tracking control, skid steering robot, nonholonomic constraints INTRODUCTION The skid steering robot is considered as all- working in hard environmental conditions but terrain vehicle, and has many advantages than the mechanism is quite simple The following other off-road robots, for example, a high figure and table show major steering types and maneuverability, high-power, an ability of a steering system evaluation [1] Fig Kinematics of major steering types Trang 83 Science & Technology Development, Vol 13, No.K4- 2010 Table A steering system evaluation The skid steering robot is navigated by the example, the angular velocity of each wheel angular velocity difference between left wheels can be determined without the inertia moment and right wheels [2] Because of lateral and the mass of the robot Furthermore, this skidding, velocity constraints occurring in skid algorithm can be applied to not only the steering robot are quite different from the ones wheel skid-steering mobile robot but also all met in other mobile platforms wheels are not types of the mobile robot whose equations of supposed to skid An example for this steering motion are similar to equation‘s Lagrange type is ATRV-J robot designed by Irobot Fields of application of the skid steering robot company can be extended For instance, the manipulator Recently, Kozlowski et al (2004) developed the skid steering robot’s model based on Dixon’s kinematic controller [3], [4], [5] Kozlowski extended new time differentiable and time-varying control scheme based on the strategy of forcing some transformed states to track an exogenous exponentially decaying signal produced by a tunable oscillator [6], [7] In this paper, a new control algorithm based on feedback linearization and PD control is presented It allows us to control a reference point fixing in the wheel skid steering mobile robot tracks a given trajectory The first advantage of the algorithm is kinematics and dynamics can be studied separately For Trang 84 or GPR radar can be stuck on the robot to inspect the geology NONHOLONOMIC SYSTEM Major wheeled mobile robot is a typical example of mechanical systems with nonholonomic constraints Although navigation and planning of mobile robots have been investigated extensively over the past decade, the work on dynamic control of mobile robots with nonholonomic constraints is much more recent We consider mechanical systems that are subject to nonholonomic constraints characterized by the following equation: A(q)q&= (1) TẠP CHÍ PHÁT TRIỂN KH&CN, TẬP 13, SỐ K4 - 2010 Where q is the n-dimensional generalized coordinates A(q) is an m x n dimensional matrix Because the constraints are assumed to be nonholonomic, (1) is not integrable It will be assumed that these constraints are independent In another words, A(q) has rank m Using the vector λ of Lagrange multiplier, the equations of motion of nonholonomically constrained systems are governed by: Fig The robot in the inertial frame &+ V (q, q&) + G (q) = E (q)u + A (q)λ ( M (q)q& T 2) Where: M(q) is the n x n dimensional positive definite inertia matrix V (q, q&) is the n dimensional velocitydependent force vector G(q) is the gravitational force vector u is the r dimensional vector of actuator force/torque E(q) is the n x r dimensional matrix mapping the actuator space into the generalized coordinate It has been established that nonholonomic system described by the constraint equation (1) and the motion equation (2) [8] MODEL OF A SKID STEERING MOBILE ROBOT 3.1 Kinematic model Fig Schematic of the skid steering robot The notation is shown in fig 2, x y z Select the inertial frame (COM l l l ), where COM is center of mass Let (X, Y, Z) to be robot’s barycentric coordinates in the world frame, vx v = v y Note: , 0 ω = ω , X q = Y θ ω = θ& Trang 85 Science & Technology Development, Vol 13, No.K4- 2010 d = d The radius vector i ix d c = d cx d cy d iy T and T are defined with respect to the local frame from the instantaneous center of rotation (IRC) Thus: Fig Velocities of one wheel Or vi v = =ω di dc (5) v v v vix = x = iy = y = ω − diy − d yC dix d xC (6) Coordinates of ICR in the local frames: ICR ( xirc , yirc ) = ( −d xC , Writing (6) as -d yC ) follows: vy vx =− =ω yirc xirc (7) Otherwise, from the figure we have: d1 y = d y = dCy + c d3 y = d y = dCy − c Fig Wheel velocities We have: X& cosθ & = Y sin θ − sin θ vx cosθ v y d x = d3 x = dCx + b (8) (3) The i-th wheel rotates with an angular velocity ωi (t ) ,where i=1;2;3;4 The longitudinal velocity can be obtained: vix = rix ωi (4) In contrast to most wheeled mobile robot, the lateral velocity of the skid steering robot viy is generally nonzero d1x = d x = dCx − a vL v R vF v Hence, B = v1x = v2 x = v3 x = v4 x = v2 y = v3 y = v1 y = v4 y −c vL v 1 c vx R = vF − xirc + b ω − xirc − a v And, B (10) Assuming that Trang 86 (9) r1 = r2 = r3 = r4 = r TẠP CHÍ PHÁT TRIỂN KH&CN, TẬP 13, SỐ K4 - 2010 Because v1x = v2 x and this is a skid- 3.2 Dynamic model steering robot, the angular velocity of the first wheel equals the angular velocity of the second wheel So, let ωL , ωR be respectively angular velocities of lefts and right wheels We can write: ω L v L ω = r v R R (11) Fig The forces acting on one wheel Wheel forces are depicted in Fig.6 Combining (10) and (11), a control input at kinematic level is defined as: The active force is obtained ωL + ωR vx η = = r ω −ωL + ωR 2.c (12) To complete the kinematic (6), characterized by: the velocity v y + xirc θ&= we r (18) obtain the following equation of equilibrium: model, N1.a = N b N a = N b constraint ∑N i =1 (13) i = mg (19) Where m denotes the robot mass and g is Thus, the gravity acceleration Using the symmetry [ − sin θ cosθ Or, A (q) q&=0(14) T xirc ] X& Y& θ& = The kinematic equation of the robot is obtained: τi Neglecting additional dynamic properties, nonholonomic constraint is considered From Fi = q&= S (q).η (15) Where S cosθ S (q ) = sin θ is the following xirc sin θ − xirc cosθ matrix along the longitudinal midline, we obtain b N1 = N = 2(a + b) mg a N = N = mg 2(a + b) (20) The friction acting one wheel is obtained: Ff (σ ) = µC N sgn(σ ) + µv (σ ) (16) T T which satisfies S (q ) A ( q ) = (17) Where σ denotes (21) the linear velocity N is force perpendicular to the surface Trang 87 Science & Technology Development, Vol 13, No.K4- 2010 µC , µ v are respectively the coefficients Coulumb and viscous friction In the dynamic model of this robot, the following relation is valid: µC N ? µv σ Consequently, µv σ the term can be neglected The following function is considered to approximate sgn(σ ) : the ˆ σ) = sgn( where the π function k s satisfies the k ? and relations: s lim kS →∞ π (22) force friction for one wheel can be written as: forces causing the dissipation of energy: 4 i =1 i =1 4 i =1 i =1 Fry (q&) = sin θ ∑ Fsi (vxi ) + cos.∑ Fli (v yi ) (28) (29) Mr (q&) = −a.[Fl1(vy1) + Fl (vy4 )] + b[Fl (vl ) + Fl (vl )] + c[ −Fs1(vx1) − Fs (vx2 ) + Fs3 (vx3 ) + Fs4 (vx4 )] Letting & Fry (q) & M x (q)] &T R ( q&) = [Frx (q) (23) by actuators can be calculated in the inertial µlci and µ sci denote respectively the coefficients of the lateral and longitudinal frame as follow: Fx = cosθ ∑ Fi i =1 forces It is assumed that the potential energy of the robot ∏ = because of the planar motion Neglecting the energy of rotating wheels, the kinetic energy of this robot can be rewritten: 1 T = m( X&2 + Y&2 ) + I θ&2 2 (30) Consequently, the active force generated ˆ (vxi ) Fsi = µ sci mg sgn (24) Fy = sin θ ∑ Fi i =1 (31) The active torque around the center of mass is obtained: M ' = c(− F1 − F2 + F3 + F4 ) (25) (32) The vector of active forces has the following form: Trang 88 the of mass can be obtained as Applying to the skid steering robot, the where Considering (27) The resistant of moment around the center arctan(ks σ ) = sgn(σ ) ˆ (v yi ) Fli = µlci mg sgn Where, m 0 M = m 0 I Frx (q&) = cosθ ∑ Fsi (vxi ) − sin θ ∑ Fli (v yi ) arctan(ks σ ) constant & mX& d ∂T & & ( ) = mY& = M q& dt ∂q& & Iθ& (26) Hence, F = [ Fx Fy M ' ]T TẠP CHÍ PHÁT TRIỂN KH&CN, TẬP 13, SỐ K4 - 2010 Using (18), (31), (32), we get: cosθ ∑τ i i =1 1 F= sin θ ∑τ i r i =1 c(−τ − τ + τ + τ ) (33) τ + τ τ = 2 τ + τ The term τ is defined by: cosθ 1 B (q ) = sin θ r −c cosθ sin θ c (34) 0 θ& C = S T MS&= m.xirc & −θ x&irc (41) m M = S T MS = m.xirc + I (42) Frx (q&) R = ST R = xirc Fry (q&) + M r (43) 1 B = ST B = r −c c (44) CONTROL LAW (35) We have: F = B (q ).τ 4.1 Operational Constraint (36) xo be an arbitrary constant which Let Using (26), (30), (36), and equation’s sacrifices: Lagrange we get: &+ R(q&) = B(q).τ M (q ).q& The constraint equation (13) is rewritten (37) Eq (37) describes only the dynamic of a free body and does not include the nonholonomic constraint (14) Therefore, the constraint has to be imposed on (37) To solve this problem, a vector of Lagrange multiplier λ is considered [2], and (37) becomes as following equation: (38) T Multiplying from the left side by S (q ) , and simplifying by using eq (15), and the (45) Let S be a 3x2 dimensional matrix which sacrifices the equation (17) cosθ S (q ) = sin θ xo sin θ − x0 cosθ (46) Let k = [X k be the state Y θ vx ω] space vector (47) To simplify the formula (15), (40), the matrix following equation, & we obtain: M η + C.η + R = B.τ v y + xo θ&= as: 4.2 Control Algorithm &+ R (q&) = B (q ).τ + AT (q ).λ M (q ).q& &= S&(q ).η + S ( q).η& q& xo ∈ (-a, b) −1 (39) (40) f = M (−C.η − R) (48) is introduced, where Where, Trang 89 Science & Technology Development, Vol 13, No.K4- 2010 θ& C = S T MS&= m.x0 & −θ x&0 (49) m M = S MS = m x + I (50) (51) 1 B = ST B = r −c c (52) equation and the dynamic equation are written: S η k&= + − τ f M B state equation (53) can be xo sin θ − xrc sin θ − yrc cosθ − xo cosθ + xrc cosθ − yrc sin θ further (59) xo ≠ xrc , Φ is regular From (58) we get: (60) Hence, &η ) u = Φ −1 (η − Φ yd be a (61) desired trajectory, and e = y − y be a feedback error d & y&= η = & y&d + K d ( y&d − y&) + K p ( y d − y ) (62) By using equations (54), (55), (61), (62), a S η k&= + u I (54) new algorithm has been presented It is easy to control the angular velocities of wheels in other −1 τ = ( M B )(u − f ) (55) Let a reference point be denoted in the (x , yrc ) c r The robot is controlled so that the reference point tracks the given trajectory that a skid steering robot tracks a given trajectory SIMULATION RESULTS To validate the performance of the control algorithm, the motion of skid steering mobile The world coordinates of the reference point are obtained as: robot is simulated by Matlab The robot is designed to track a given trajectory The X r = X c + xrc cosθ − yrc sin θ c c Yr = X c + xr sin θ + yr cosθ advantage of the algorithm is the angular (56) The output equation is obtained: y = h(q ) = [ X r By taking Let simplified as: local inertial frame by cosθ Φ= sin θ &.η + Φ.η& & y&= Φ Combining (15) and (40), the kinematic This (58) where T Frx (q&) R = ST R = x0 Fry ( q&) + M r ∂h(q) y&= q&= Φ.η ∂q Yr ] velocity of each wheel can be determined without the inertia moment and the mass of the robot Therefore, dynamic parameters aren’t T (57) considered for simplicity The dimensions’ robot are chosen as a = b = c = 1( m) The robot starts at location (-3; 8) with the Trang 90 TẠP CHÍ PHÁT TRIỂN KH&CN, TẬP 13, SỐ K4 - 2010 θ= angle ,and the reference π Case 1: A desired trajectory is given by: , the horizontal velocity vx = angular point is velocity the ω =0 center of x = 4* t y = 2* t The mass c r follow ( m) The controller parameters are chosen as x = y = The constant xo is chosen as c r ( m) follow: k P = 52, k D = 15 xo = 3.2(m) (a) (b) Fig The simulation result of case (a) robot trajectory, and (b) tracking error Figure 7(a) shows the reference trajectory, and figure 7(b) shows the tracking error in the fixed frame It is clearly seen from the plots trajectory) quickly converges to the given trajectory (desired trajectory) Case 2: A desired trajectory is given by: that the reference point’s trajectory (robot Trang 91 Science & Technology Development, Vol 13, No.K4- 2010 The controller parameters are chosen as follow: k P = 10, k D = (a) (b) Fig The simulation result of case (a) robot trajectory, and (b) tracking error Similarly, the reference point’s trajectory quickly converges to the given trajectory CONCLUSION In this paper, a new algorithm of trajectory tracking control for 4-wheel skid steering mobile robot is presented The output equation is chosen to be the coordinates of the reference Trang 92 point fixing in the robot Because the mobile robot is subject to nonholonomic constraints, dynamics system is nonlinear (see eq 40) However, the number of output coordinates equals the number of input commands Thus, one can use nonlinear state feedback law in order to transform the nonlinear robot kinematics, dynamics into a linear system The TẠP CHÍ PHÁT TRIỂN KH&CN, TẬP 13, SỐ K4 - 2010 effectiveness of this algorithm is validated by completely a skid steering mobile robot as well simulations on two different trajectories as apply a Lyapunov stability analysis to In the future, we will integrate this guarantee the stability of this controller algorithm with stepper motor control to design ĐIỀU KHIỂN THEO QUĨ ĐẠO MỘT RƠBỐT DI ĐỘNG LÁI TRƯỢT BÁNH Đặng Văn Nghìn(1), Nguyễn Văn Quốc Khánh(2) (1) Viện Cơ Tin học Tp.HCM (2) Trường Đại học Bách Khoa, ĐHQG-HCM TÓM TẮT: Bằng cách áp dụng ràng buộc nonholonomic phương trình Lagrange cho hệ thống nonholonomic, phương pháp đưa để mơ hình điều khiển robot di động lái trượt bánh chạy theo quỹ đạo cho trước Đầu tiên, sở hệ thống nonholonomic giới thiệu Tiếp theo, mơ hình động học động lực học robot lái trượt khảo sát Để điều khiển robot dò theo quỹ đạo, giải thuật đưa cách ứng dụng tuyến tính hóa hồi tiếp điều khiển PD Hơn nữa, kết mơ chứng tỏ tính hiệu thuật tốn Từ khóa: điều khiển đồng chỉnh, robot lái trượt, ràng buộc nonholonomic Engineering Systems, A Lyapunov-Based REFERENCES Approach — Boston: Birkhäuser (2003) [1] Lakkad S.: Modeling and simulation of steering systems for autonomous vehicle, MSc thesis, the Florida state university, (2004) Trajectory tracking control of a fourwheel differentially driven mobile robot IEEE E and Behal A Nonlinear Control of Wheeled Mobile Robots — London:Springer (2001) [2] Caracciolo L., De Luca A and Iannitti S: — [4] Dixon W.E., Dawson D.M., Zergeroglu Int Conf Robotics and Automation, Detroit, MI, pp 2632–2638, (1999) [3] Dixon W.E., Behal A., Dawson D.M and Nagarkatti S.P Nonlinear Control of [5] Yoshio Yamamoto and Xiaoping Yun, Coordinating Locomotion and Manipulation of a Mobile Manipulator IEEE Transactions on Automatic Control, Vol 39, No 6, pp 1326-1332 (1994) [6] K Kozłowski, D Pazderski, Modeling and control of a 4-wheel skid-steering mobile robot Int J Appl Math Comput Sci, Vol 14, No 4, 477–496, (2004) Trang 93 Science & Technology Development, Vol 13, No.K4- 2010 [7] K Kozłowski, D Pazderski, I.Rudas, [8] R Fierro and F L Lewis, Control of a J.Tar, Modeling and control of a 4-wheel Nonholonomic skid-steering mobile robot, From theory Backstepping Kinematics into Dynamics, to Journal practice, Poznan University Technology, No DS 93/121/04 Trang 94 of of Mobile Robotic pp.149–163, (1997) Systems Robot, 14(3), ... hình điều khiển robot di động lái trượt bánh chạy theo quỹ đạo cho trước Đầu tiên, sở hệ thống nonholonomic giới thiệu Tiếp theo, mơ hình động học động lực học robot lái trượt khảo sát Để điều khiển. .. stability of this controller algorithm with stepper motor control to design ĐIỀU KHIỂN THEO QUĨ ĐẠO MỘT RÔBỐT DI ĐỘNG LÁI TRƯỢT BÁNH Đặng Văn Nghìn(1), Nguyễn Văn Quốc Khánh(2) (1) Viện Cơ Tin học Tp.HCM... khiển robot dò theo quỹ đạo, giải thuật đưa cách ứng dụng tuyến tính hóa hồi tiếp điều khiển PD Hơn nữa, kết mô chứng tỏ tính hiệu thuật tốn Từ khóa: điều khiển đồng chỉnh, robot lái trượt, ràng