Motion Control (wheeled robots)

Transcription

1 Motion Control (wheeled robots) Requirements for Motion Control Kinematic / dynamic model of the robot Model of the interaction between the wheel and the ground Definition of required motion -> speed control, position control Control law that satisfies the requirements Localization "Position" Global Map Cognition Environment Model Local Map Path Perception Real World Environment Motion Control

2 Introduction: Mobile Robot Kinematics Aim Description of mechanical behavior of the robot for design and control Similar to robot manipulator kinematics However, mobile robots can move unbound with respect to its environment o there is no direct way to measure the robot s position o Position must be integrated over time o Leads to inaccuracies of the position (motion) estimate -> the number 1 challenge in mobile robotics Understanding mobile robot motion starts with understanding wheel constraints placed on the robots mobility

3 Representing Robot Position Representing robot within an arbitrary initial frame Initial frame: Robot frame: { X I, Y I } { X R, Y R } Y I Y R Robot position: ξ = I [ x y θ] T X R Mapping between the two frames P ξ R = R ( θ) ξ ( ) [ ] T I = R θ x y θ X I ( ) R ( = & cos( $ $ ' sin( $ % 0 sin( cos( 0 0# 0 1 " Y I X R Y R X I

4 Mapping Between Frames: Details cosθ sinθ 0 ξ R = R(θ) ξ I = -sin θ cosθ 0 y θ x = xcosθ + ysinθ -xsinθ + ycosθ θ θ A B Y I Y R What is A cos cosθ? Recall cos(π / 2 - θ) ) = sin(θ) cos(π / 2 + θ) ) = -sin(θ) P X R

5 Example

6 Introduction: Kinematics Model Goal: establish the robot speed [ ] T ξ = x y as a function of the wheel speeds steering angles, steering speeds θ ϕ i β and the geometric parameters of th i robot (configuration coordinates) ϕ β ϕ measured in radians/sec, so ϕ/2π is revolutions/sec In one revolution wheel translates 2πr 2 r linear units Translational velocity is 2πr( r(ϕ/2π) ) = r rϕ

7 Introduction: Kinematics Model Goal: establish the robot speed [ ] T ξ = x y as a function of the wheel speeds steering angles, steering speeds θ ϕ i β and the geometric parameters of th i robot (configuration coordinates) Forward kinematics - If I do this, what will happen? x ξ = y = f ( ϕ1, ϕn, β1, βm, β1, βm) θ Inverse kinematics - If I want this to happen, what should I do? [ ] T ϕ 1 ϕ β 1 β β n m 1 βm = f ( x, y, θ )

8 Forward Kinematic Models - Differential Drive The robot Two wheels - radius r Point P centered between wheels Each wheel is distance l from P Wheels have rotational velocity ϕ 1 and ϕ 2 Forward kinematic model ξ I = x y θ r = f(l, r, θ, ϕ 1, ϕ 2 ) Use ξ R = R(θ) ξ I so ξ I = R -1 (θ) ξ R Y R X R p l

9 Differential Drive (cont) Use ξ R = R(θ) ξ I so ξ I = R -1 (θ) ξ R Compute how wheel speeds influence ξ R Translate to ξ I via R -1 (θ) Y R X R r p l Contribution to translation along X R If one wheel spins and the other remains still P will move at half the translational velocity of the wheel: 1/2rϕ 1 or 1/2rϕ 2 Sum these components to account for both wheels spinning X R = 1/2rϕ 1 + 1/2rϕ 2 Suppose they spin in opposite directions, same direction

10 Differential Drive (cont) X R Wheel rotation never contributes to Y R Why? What about θ? Wheel 1 spin makes robot rotate counterclockwise Pivot around wheel 2 (left wheel) Translational velocity is rϕ Traces circle with radius 2l Rotational velocity 2π * rϕ / (2π * 2l) = rϕ / 2l Wheel 2 spin makes robot rotate clockwise Sum to get net effect: θ = (rϕ 1 - rϕ 2 ) / 2l Y R r p l

11 Differential Drive: The Punch Line ξ I = R -1 (θ) ξ R = R -1 (θ) r(ϕ 1 + ϕ 2 )/ 2 0 r(ϕ 1 - ϕ 2 )/ 2l

12 Wheel Kinematic Constraints: Assumptions Y I Y R Movement on a horizontal plane Point contact of the wheels Wheels not deformable Pure rolling No slipping, skidding or sliding No friction for rotation around contact point Steering axes orthogonal to the surface Wheels connected by rigid frame (chassis) v ϕ r P X R

13 Wheel Kinematic Constraints: Fixed Standard Wheel Rolling constraint: all motion along wheel plane (in the direction of v) must be accompanied by the same amount of wheel spin so that there is pure rolling at contact point

14 Wheel Kinematic Constraints: Fixed Standard Wheel Sliding constraint: there can be no motion orthogonal to wheel plane (perpendicular to v), otherwise wheel skids

15 Wheel Kinematic Constraints: Fixed Standard Wheel: Rolling Constraint

16 Wheel Kinematic Constraints: Fixed Standard Wheel: Sliding Constraint

17 Example Suppose that the wheel A is in position such that α = 0 and β = 0 This 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 the sliding constraint reduces to:

18 Not Omnidirectional: Why? Can constraints be satisfied for ANY ξ I?

19 Wheel Kinematic Constraints: Steered Standard Wheel Same as fixed wheel, but β is changes over time Instantaneously, it is fixed

20 Wheel Kinematic Constraints: Castor Wheel Wheel contact point at B Steering at A Rigid connector AB

21 Wheel Kinematic Constraints: Castor Wheel: Omnidirectional (why?) [cos(α + β) ) sin(α + β) d + lsinβ]r( ]R(θ)ξ I + d dβ = 0

22 Wheel Kinematic Constraints: Swedish Wheel

23 Wheel Kinematic Constraints: Spherical Wheel

24 Robot Kinematic Constraints Given a robot with M wheels each wheel imposes zero or more constraints on the robot motion only fixed and steerable standard wheels impose constraints What is the maneuverability of a robot considering a combination of different wheels? Suppose we have a total of N=N f + N s standard wheels We can develop the equations for the constraints in matrix forms: Rolling J 1 ( $ ) (# )"& s R I + J 2 & = 0 Lateral movement C (# ) (" )& 0 1 s R I = ( t) = '( f % &( ( t) $ ( t) " # s ( N f + N s )1 J 1 C ' J1 f $ (( s ) = % ( ) " J = diag( r & J ( 1 L # 1 (( s ) 1s s N f N s ( + ) 3 ' C1 f $ = % ( ) " & C ( # 1s s N f N s ( + ) 3 r 2 N

25 Example: Differential Drive Robot Presented on blackboard

26 Example: Omnidirectional Robot Presented on blackboard

27 Mobile Robot Maneuverability The maneuverability of a mobile robot is the combination of the mobility available based on the sliding constraints plus additional freedom contributed by the steering Three wheels is sufficient for static stability additional wheels need to be synchronized this is also the case for some arrangements with three wheels It can be derived using the equation seen before Degree of mobility Degree of steerability Robots maneuverability m s M = m + s

28 Mobile Robot Maneuverability: Degree of Mobility To avoid any lateral slip the motion vector following constraints: R (" ) & I has to satisfy the C 1 f R( " ) & I = 0 & C1 f # C1( ' s ) = $ C (# ) R( " )& 0 % C1s (' s )" 1 s s I = Mathematically: R (" ) & I must belong to the null space of the projection matrix Null space of C 1( s ) is the space N such that for any vector n in N C 1 ( C1 (" s ) n = 0 Geometrically this can be shown by the Instantaneous Center of Rotation (ICR)

29 Mobile Robot Maneuverability: Instantaneous Center of Rotation Ackermann Steering Bicycle

30 Mobile Robot Maneuverability: More on Degree of Mobility Robot chassis kinematics is a function of the set of independent constraints rank [ C 1 ( s )] the greater the rank of, C 1( s ) the more constrained is the mobility Mathematically o no standard wheels o all direction constrained Examples: Unicycle: One single fixed standard wheel Differential drive: Two fixed standard wheels o wheels on same axle o wheels on different axle [ C1( s )] = 3 rank[ C ( s )] rank [ C1 ( s )] = 0 rank [ C ( )] 3 " m = dim N # 1 1 s = [ C ( )] 3 0 rank 1 " s

31 Mobile Robot Maneuverability: Degree of Steerability Indirect degree of motion " s = rank C1 s ( s ) The particular orientation at any instant imposes a kinematic constraint However, the ability to change that orientation can lead additional degree of maneuverability Range of : s 0 " s 2 [ ] Examples: one steered wheel: Tricycle two steered wheels: No fixed standard wheel car (Ackermann steering): N f = 2, N s =2 -> common axle

32 Mobile Robot Maneuverability: Robot Maneuverability Degree of Maneuverability + M = m s Two robots with same M are not necessary equal Example: Differential drive and Tricycle (next slide) For any robot with to lie on a line M = 2 the ICR is always constrained For any robot with M = 3 the ICR is not constrained an can be set to any point on the plane The Synchro Drive example: = m + s M = = 2

33 Mobile Robot Maneuverability: Wheel Configurations Differential Drive Tricycle

34 Five Basic Types of Three-Wheel Configurations

35 Synchro Drive = m + s M = = 2

36 Mobile Robot Workspace: Degrees of Freedom Maneuverability is equivalent to the vehicle s degree of freedom (DOF) But what is the degree of vehicle s freedom in its environment? Car example Workspace how the vehicle is able to move between different configuration in its workspace? The robot s independently achievable velocities = differentiable degrees of freedom (DDOF) = Bicycle: Omni Drive: M = m + s M = m + s =1+1 m DDOF = 1; DOF=3 =1+1 DDOF=3; DOF=3

37 Mobile Robot Workspace: Degrees of Freedom, Holonomy DOF degrees of freedom: Robots ability to achieve various poses DDOF differentiable degrees of freedom: Robots ability to achieve various path Holonomic Robots DDOF " m DOF A holonomic kinematic constraint can be expressed a an explicit function of position variables only A non-holonomic constraint requires a different relationship, such as the derivative of a position variable Fixed and steered standard wheels impose non-holonomic constraints

38 Mobile Robot Workspace: Examples of Holonomic Robots

39 Path / Trajectory Considerations: Omnidirectional Drive

40 Path / Trajectory Considerations: Two-Steer

41 Beyond Basic Kinematics

42 Motion Control (kinematic control) The objective of a kinematic controller is to follow a trajectory described by its position and/or velocity profiles as function of time Motion control is not straight forward because mobile robots are nonholonomic systems However, it has been studied by various research groups and some adequate solutions for (kinematic) motion control of a mobile robot system are available Most controllers are not considering the dynamics of the system

43 Motion Control: Open Loop Control trajectory (path) divided in motion segments of clearly defined shape: straight lines and segments of a circle control problem: pre-compute a smooth trajectory based on line and circle segments Disadvantages: It is not at all an easy task to pre-compute a feasible trajectory limitations and constraints of the robots velocities and accelerations does not adapt or correct the trajectory if dynamical changes of the environment occur The resulting trajectories are usually not smooth y I goal x I

44 Motion Control: Feedback Control, Problem Statement Find a control matrix K, if exists y R (t) start v(t) " x R e K & k = $ % k with k ij =k(t,e) such that the control of v(t) and ω(t) k k k k # " goal & x# & v( t) # $ $ = K ' e = K' $ y %) ( t) " $ %( " drives the error e to zero lim e( t) = 0 t" R

45 Motion Control: Kinematic Position Control The kinematic of a differential drive mobile robot described in the initial frame {x I, y I, θ } is given by, Δy I & x& # & cos( $ = $ $ y& $ sin( $ %( & " $ % 0 0# & v # 0 $ %' 1 " " where and are the linear velocities in the direction of the x I and y I of the initial frame Let α denote the angle between the x R axis of the robots reference frame and the vector connecting the center of the axle of the wheels with the final position

46 Kinematic Position Control: Coordinates Transformation Coordinates transformation into polar coordinates with its origin at goal position: Δy System description, in the new polar coordinates for for

47 Kinematic Position Control: Remarks The coordinates transformation is not defined at x = y = 0; as in such a point the determinant of the Jacobian matrix of the transformation is not defined, ie it is unbounded For the goal, for the forward direction of the robot points toward it is the backward direction By properly defining the forward direction of the robot at its initial configuration, it is always possible to have at t=0 However this does not mean that α remains in I 1 for all time t

48 Kinematic Position Control: The Control Law It can be shown, that with the feedback controlled system will drive the robot to (#,",) = ( 0, 0, 0) The control signal v has always constant sign, the direction of movement is kept positive or negative during movemen parking maneuver is performed always in the most natural way and without ever inverting its motion

49 Kinematic Position Control: Resulting Path

50 Kinematic Position Control: Stability Issue It can further be shown, that the closed loop control system is locally exponentially stable if k " > $ # k" 0 ; k < 0 ; k > 0 Proof: for small x > cosx = 1, sinx = x and the characteristic polynomial of the matrix A of all roots have negative real parts

51 Mobile Robot Kinematics: Non-Holonomic Systems 3 y I x 1, y 1 s 1 =s 2 ; s 1R =s 2R ; s 1L =s 2L but: x 1 = x 2 ; y 1 = y 2 s 1L s 1 s 1R s 2L x 2, y 2 s 2 s 2R Non-holonomic systems differential equations are not integrable to the final position the measure of the traveled distance of each wheel is not sufficient to calculate the final position of the robot One has also to know how this movement was executed as a function of time x I

52 Non-Holonomic Systems: Mathematical Interpretation 3 A mobile robot is running along a trajectory s(t) At every instant of the movement its velocity v(t) is: v( t) = " s " t = " x " y cos + sin " t " t y I v(t) s(t) ds = dx cos + dy sin Function v(t) is said to be integrable (holonomic) if there exists a trajectory function s(t that can be described by the values x, y, and θ only s = s( x, y, ) This is the case if 2 s x y = 2 s y x ; 2 s x " = 2 s " x ; 2 s y " = 2 s " y With s = s(x,y,θ) we get for ds ds = " s " x dx + Condition for integrable func " s dy + " y " s " d

53 Non-Holonomic Systems: The Mobile Robot Example In the case of a mobile robot where and by comparing the equation above with we find Condition for an integrable (holonomic) function: the second (-sinθ=0) and third (cosθ=0) term in equation do not hold sin dx cos dy ds + = d s dy y s dx x s ds " " + " " + " " = 0 ; sin ; cos = = = " " " s y s x s y s y s x s x s x y s y x s = = = " " " " ; ; 3