Motion Control (wheeled robots)

Transcription

1 3 Motion Control (wheeled robot) Requirement for Motion Control Kinematic / dynamic model of the robot Model of the interaction between the wheel and the ground Definition of required motion -> peed control, poition control Control law that atifie the requirement Localization "Poition" Global Map Cognition Environment Model Local Map Path Perception Real World Environment Motion Control

2 3 Introduction: Mobile Robot Kinematic Aim Decription of mechanical behavior of the robot for deign and control Similar to robot manipulator kinematic However, mobile robot can move unbound with repect to it environment o there i no direct way to meaure the robot poition o Poition mut be integrated over time o Lead to inaccuracie of the poition (motion) etimate -> the number 1 challenge in mobile robotic Undertanding mobile robot motion tart with undertanding whee l contraint placed on the robot mobility

3 Introduction: Kinematic Model Goal: etablih the robot peed ξ & = [ x& y& θ& ] T a a function of the wheel peed ϕ&, teering angle, teering peed β & i β i i and the geometric parameter of the robot (configuration coordinate). forward kinematic x& ξ & = y& = f ( ϕ&, K ϕ& n, β1, Kβm, β& 1, Kβ& θ& Invere kinematic 1 m T [ ϕ& L ϕ& β K β β& K β& ] f ( x&, y&, &) 1 n 1 m 1 m = θ why not x y = θ f ( ϕ, Kϕn, β1, Kβ 1 m ) ) y I v(t) (t) θ -> not traight forward x I

4 Repreenting Robot Poition Repreenting to robot within an arbitrary initial frame Initial frame: Robot frame: { X I, Y I } { X R, Y R } Y I Y R Robot poition: ξ = I [ x y θ ] T θ X R Mapping between the two frame ξ & = R θ ξ& = R θ x& y& θ& R ( ) ( ) [ ] T I P X I ( ) R θ = coθ inθ 0 inθ coθ Y I X R θ Example: Robot aligned with Y I Y R X I

5 Example 3.2.1

6 Forward Kinematic Model Preented on blackboard

7 Wheel Kinematic Contraint: Aumption Y I Movement on a horizontal plane Point contact of the wheel Wheel not deformable ϕ& r Pure rolling v v = 0 at contact point No lipping, kidding or liding No friction for rotation around contact point Steering axe orthogonal to the urface Wheel connected by rigid frame (chai) Y R P θ X R X I

8 Wheel Kinematic Contraint: Fixed Standard Wheel 3.2.3

9 3.2.3 Example Suppoe that the wheel A i in poition uch that α = 0 and β = 0 Thi would place the contact point of the wheel on X I with the plane of the wheel oriented parallel to Y I. If θ = 0, then th liding contraint reduce to:

10 Wheel Kinematic Contraint: Steered Standard Wheel 3.2.3

11 Wheel Kinematic Contraint: Cator Wheel 3.2.3

12 Wheel Kinematic Contraint: Swedih Wheel 3.2.3

13 Wheel Kinematic Contraint: Spherical Wheel 3.2.3

14 Robot Kinematic Contraint Given a robot with M wheel each wheel impoe zero or more contraint on the robot motion only fixed and teerable tandard wheel impoe contraint What i the maneuverability of a robot conidering a combination of different wheel? Suppoe we have a total of N=N f + N tandard wheel We can develop the equation for the contraint in matrix form : Rolling J1 ( β ) ( θ ) ξ& R I + J2ϕ& = 0 Lateral movement C ( β ) R ( θ ) ξ & 0 1 I = ϕ( t) = ϕ ϕ f ( t) ( t) N f N ( + ) 1 J 1 C 1 ( β J f ) = J ( J ( β ) ) 2 = diag r 1 Lr N 1 ( β 1 N f N ( + ) 3 C1 f ) = C ( β 1 N f N ) ( + ) 3

15 Example: Differential Drive Robot Preented on blackboard

16 Example: Omnidirectional Robot Preented on blackboard

17 Mobile Robot Maneuverability 3.3 The maneuverability of a mobile robot i the combination of the mobility available baed on the liding contraint plu additional freedom contributed by the teering Three wheel i ufficient for tatic tability additional wheel need to be ynchronized thi i alo the cae for ome arrangement with three wheel It can be derived uing the equation een before Degree of mobility Degree of teerability Robot maneuverability δ m δ δ = δ + δ M m

18 Mobile Robot Maneuverability: Degree of Mobility To avoid any lateral lip the motion vector following contraint: R( θ ) ξ & I ha to atify the C 1 f R ( θ ) ξ& I = 0 C1 ( β ) R( θ ) ξ& I = 0 C 1 ( β ) = C1 f C1 ( β ) Mathematically: R( θ ) ξ & I mut belong to the null pace of the projection matrix C1( β ) Null pace of C ( β ) i the pace N uch that for any vector n in N 1 C1 ( β ) n = 0 Geometrically thi can be hown by the Intantaneou Center of Rotation (ICR)

19 3.3.1 Mobile Robot Maneuverability: Intantaneou Center of Rotation Ackermann Steering Bicycle

20 3.3.1 Mobile Robot Maneuverability: More on Degree of Mobility Robot chai kinematic i a function of the et of independent contraint rank[ C 1 ( β )] the greater the rank of, C ( β ) the more contrained i the mobility 1 Mathematically δ m = dim N[ C1( β )] = 3 rank[ C1( β )] 0 rank[ C1( β )] 3 rank[ C1 ( β )] = 0 rank[ C ( β )] 3 o no tandard wheel o all direction contrained Example: Unicycle: One ingle fixed tandard wheel Differential drive: Two fixed tandard wheel o wheel on ame axle o wheel on different axle 1 =

21 Mobile Robot Maneuverability: Degree of Steerability Indirect degree of motion δ = rank C1 ( β ) The particular orientation at any intant impoe a kinematic contraint However, the ability to change that orientation can lead additional degree of maneuverability Range of : 0 δ 2 Example: δ [ ] one teered wheel: Tricycle two teered wheel: No fixed tandard wheel car (Ackermann teering): N f = 2, N =2 -> common axle

22 Mobile Robot Maneuverability: Robot Maneuverability Degree of Maneuverability δ = δ + δ M m Two robot with ame are not neceary equal Example: Differential drive and Tricycle (next lide) δ M For any robot with to lie on a line δ M = 2 the ICR i alway contrained For any robot with δ M = 3 the ICR i not contrained an can be et to any point on the plane The Synchro Drive example: δ M = δ m + δ = = 2

23 Mobile Robot Maneuverability: Wheel Configuration Differential Drive Tricycle

24 Five Baic Type of Three-Wheel Configuration 3.3.3

25 Synchro Drive δ M = δ m + δ = = 2

26 Mobile Robot Workpace: Degree of Freedom Maneuverability i equivalent to the vehicle degree of freedom (DOF) But what i the degree of vehicle freedom in it environment? Car example Workpace how the vehicle i able to move between different configuration in it workpace? The robot independently achievable velocitie = differentiable degree of freedom (DDOF) = Bicycle: δm Omni Drive: = δm + δ δ δ M δ m =1 +1 DDOF = 1; DOF=3 + δ =1 +1 DDOF=3; DOF=3 = m

27 3.4.2 Mobile Robot Workpace: Degree of Freedom, Holonomy DOF degree of freedom: Robot ability to achieve variou poe DDOF differentiable degree of freedom: Robot ability to achieve variou path Holonomic Robot DDOF δ m DOF A holonomic kinematic contraint can be expreed a an explicit function of poition variable only A non-holonomic contraint require a different relationhip, uch a the derivative of a poition variable Fixed and teered tandard wheel impoe non-holonomic contraint

28 Mobile Robot Workpace: Example of Holonomic Robot 3.4.2

29 Path / Trajectory Conideration: Omnidirectional Drive 3.4.3

30 Path / Trajectory Conideration: Two -Steer 3.4.3

31 Beyond Baic Kinematic 3.5

32 Motion Control (kinematic control) 3.6 The objective of a kinematic controller i to follow a trajectory decribed by it poition and/or velocity profile a function o f time. Motion control i not traight forward becaue mobile robot are nonholonomic ytem. However, it ha been tudied by variou reearch group and ome adequate olution for (kinematic) motion control of a mobile robot ytem are available. Mot controller are not conidering the dynamic of the ytem

33 3.6.1 Motion Control: Open Loop Control trajectory (path) divided in motion egment of clearly defined hape: traight line and egment of a circle. control problem: pre-compute a mooth trajectory baed on line and circle egment Diadvantage: It i not at all an eay tak to pre -compute a feaible trajectory limitation and contraint of the robot velocitie and acceleration doe not adapt or correct the trajectory if dynamical change of the environment occur. The reulting trajectorie are uually not mooth y I goal x I

34 Motion Control: Feedback Control, Problem Statement Find a control matrix K, if exit y R ω(t) tart θ v(t) x R e K k = k with k ij =k(t,e) uch that the control of v(t) and ω(t) k k k k goal x v( t) = K e = K y ω( t) θ drive the error e to zero. lim e( t) = 0 t R

35 Motion Control: Kinematic Poition Control The kinematic of a differential drive mobile robot decribed in the initial frame {x I, y I, θ} i given by, y I x& coθ = y& in θ θ& 0 0 v 0 ω 1 where and are the linear velocitie in the direction of the x I and y I of the initial frame. Let α denote the angle between the x R axi of the robot reference frame and the vector connecting the center of the axle of the wheel with the final poition.

36 Kinematic Poition Control: Coordinate Tranformation Coordinate tranformation into polar coordinate with it origin at goal poition: y Sytem decription, in the new polar coordinate for for

37 Kinematic Poition Control: Remark The coordinate tranformation i not defined at x = y = 0; a in uch a point the determinant of the Jacobian matrix of the tranformation i not defined, i.e. it i unbounded For the goal, for the forward direction of the robot point toward it i the backward direction. By properly defining the forward direction of the robot at it i nitial configuration, it i alway poible to have at t=0. However thi doe not mean that α remain in I 1 for all time t.

38 3.6.2 Kinematic Poition Control: The Control Law It can be hown, that with the feedback controlled ytem will drive the robot to ( ρ, α, β) = ( 0, 0, 0) The control ignal v ha alway contant ign, the direction of movement i kept poitive or negative during movement parking maneuver i performed alway in the mot natural way and without ever inverting it motion.

39 Kinematic Poition Control: Reulting Path 3.6.2

40 Kinematic Poition Control: Stability Iue It can further be hown, that the cloed loop control ytem i locally exponentially table if k ρ > β α ρ 0 ; k < 0 ; k k > 0 Proof: for mall x > cox = 1, inx = x and the characteritic polynomial of the matrix A of all root have negative real part.

41 Mobile Robot Kinematic: Non-Holonomic Sytem 3.XX y I x 1, y 1 1 = 2 ; 1R = 2R ; 1L = 2L but: x 1 = x 2 ; y 1 = y 2 1L 1 1R 2L x 2, y 2 2 2R Non-holonomic ytem differential equation are not integrable to the final poition. the meaure of the traveled ditance of each wheel i not uffic ient to calculate the final poition of the robot. One ha alo to know how thi movement wa executed a a function of time. x I

42 Non-Holonomic Sytem: Mathematical Interpretation 3.XX A mobile robot i running along a trajectory (t). At every intant of the movement it velocity v(t) i: v( t) = t x = t y coθ + in θ t y I v(t) θ (t) d = dxcoθ + dyin θ Function v(t) i aid to be integrable (holonomic) if there exit a trajectory function (t) that can be decribed by the value x, y, and θ only. = ( x, y, θ ) x I Thi i the cae if 2 xy = 2 yx ; 2 xθ = 2 θx ; 2 yθ = 2 θy With = (x,y,θ) we get for d d = x dx + Condition for integrable function dy + y θ dθ

43 Non-Holonomic Sytem: The Mobile Robot Example In the cae of a mobile robot where and by comparing the equation above with we find Condition for an integrable (holonomic) function: the econd (-inθ=0) and third (coθ=0) term in equation do not hold! θ in θ dxco dy d + = θ dθ dy y dx x d + + = 0 ; in ; co = = = θ θ θ y x y y x x x y y x = = = θ θ θ θ ; ; 3.XX