CMPUT 412 Motion Control Wheeled robots. Csaba Szepesvári University of Alberta

Transcription

1 CMPUT 412 Motion Control Wheeled robots Csaba Szepesvári University of Alberta 1

2 Motion Control (wheeled robots) Requirements 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 2

3 Mobile Robot Kinematics Kinematics Actuator motion effector motion What happens to the pose when I change the velocity of wheels/joints? Neglects mass, does not consider forces ( dynamics) Aim Description of mechanical behavior for design and control Mobile robots vs. manipulators Manipulators: f: Θ X, x = f(θ) Mobile robots: x(t)=x(0)+ T 0 ẋ(t)dt, ẋ=f( θ) (#1 challenge!) Physics: Wheel motion/constraints robot motion 3

4 Kinematics Model Goal: robot speed & ξ = Wheel speeds Steering angles and speeds Geometric parameters forward kinematics Inverse kinematics [ x& y& & θ ] T ϕ& i x& & ξ = y& = & θ f β i β & i & ϕ, K& ϕ, β, Kβ, & β, K& β ) ( 1 n 1 m 1 m [ ] T & ϕ L & ϕ β K β & β K & β f ( x&, y&, &) 1 n 1 m 1 m = θ y I v(t) θ s(t) x I Why not? x y = θ f ϕ, Kϕ, β, Kβ ) ( 1 n 1 m 4

5 Representing Robot Positions 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 & ξ R = R θ & ξi ( ) = R( θ ) [ x& y& & θ ] T P X I cosθ R( θ ) = sinθ 0 sinθ cosθ Y I X R Example: Robot aligned with Y I Y R θ X I 5

6 Example 6

7 Differential Drive Robot: Kinematics wheel s spinning speed translation of P ẋ R,i = 1 2 r φ i, i=1,2, ẏ R =0 Spinning speed rotation around P Y I wheel 2 Y R X R θ R,i =( 1) i+1 r φ i 2l, i=1,2 P θ X I wheel 1 7

8 Differential Drive Robot: Kinematics II. Y I Y R X R ẋ ri = 1 2 r φ i, i=1,2, P θ ẏ r =0 X I ω i =( 1) i+1 r φ i 2l, i=1,2 ω i =R(θ) 1 r 2 r 2l ( φ1 + φ 2 ) 0 ( φ1 ) φ 2 8

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

10 Wheel Kinematic Constraints: Fixed Standard Wheel v=r φ, v =0 10

11 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 this sliding constraint reduces to: 11

12 Wheel Kinematic Constraints: Steered Standard Wheel 12

13 Wheel Kinematic Constraints: Castor Wheel 13

14 Wheel Kinematic Constraints: Swedish Wheel 14

15 Wheel Kinematic Constraints: Spherical Wheel 15

16 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 J1 ( β ) ( θ) & s R ξi + J2& ϕ = 0 Lateral movement ϕ( t) = ϕ f ( t) ( ) ϕs t ( + ) 1 N f N s J ( βs) = J J1 f ( β ) 1 1s s N f N s 2 = diag( r 1 Lr N J ( + ) 3 ) C1 ( β s) R( θ) & ξi = 0 C1 f C1 ( βs) = ( ) C β 1s s N f N s ( + ) 3 16

17 Example: Differential Drive Robot J 1 (β s )=J 1f C 1 (β s )=C 1f α =? β =? Y I Y R Right weel: α = -π/2,β = π Left wheel: α = π/2, β = 0 P θ X R X I 1 0 l 1 0 l R(θ) ξ I = ( J2 φ 0 ) J1 ( β ) ( θ )& s R ξ I + J 2 & ϕ = C 1 ( β s ) R ( θ )& ξ I =

18 Mobile Robot Maneuverability Maneuverability = mobility available based on the sliding constraints + additional freedom contributed by the steering How many wheels? 3 wheels static stability 3+ wheels synchronization Degree of maneuverability Degree of mobility Degree of steerability Robots maneuverability δ m δ s δ = δ + δ M m s 18

19 Degree of Mobility To avoid any lateral slip the motion vector has to satisfy the following constraints: R( θ ) & ξ I C 1 f R ( θ ) & ξ I = 0 C1 f C1 ( βs) = C C1 s( βs) 1 s( β s) R( θ) & ξi = 0 Mathematically: R( θ ) & ξ I must belong to the null space of the C1( βs) matrix Geometrically this can be shown by the Instantaneous Center of Rotation (ICR) 19

20 Instantaneous Center of Rotation Ackermann Steering Bicycle 20

21 Degree of Mobility++ Robot chassis kinematics is a function of the set of independent constraints rank[ C 1 ( β s )] C ( β ) the greater the rank of, 1 s the more constrained is the mobility Mathematically δm dimn[ C1( βs) ] = 3 rank[ C1( βs) ] no standard wheels rank C rank C Examples: = 0 rank[ C1 ( βs) ] 3 [ 1 ( βs) ] = 0 [ ( β )] 3 all direction constrained 1 s = Unicycle: One single fixed standard wheel Differential drive: Two fixed standard wheels wheels on same axle wheels on different axle 21

22 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 : 0 δ 2 δ s s Examples: one steered wheel: Tricycle two steered wheels: No fixed standard wheel car (Ackermann steering): N f = 2, N s =2 common axle 22

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

24 Wheel Configurations: δ M =2 Differential Drive Tricycle 24

25 Basic Types of 3-Wheel Configs 25

26 Synchro Drive δ M = δm + δs = 1 + 1= 2 26

27 Workspace: Degrees of Freedom 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: δ δ + δ =1+1 DDOF=1; DOF=3 M = m s Omni Drive: δm = δm + δs =1+1 DDOF=3; DOF=3 Maneuverability DOF! δ m 27

28 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 nonholonomic constraints 28

29 Non-Holonomic Systems 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 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! differential equations are not integrable to the final position x I 29

30 Path / Trajectory Considerations: Omnidirectional Drive 30

31 Path / Trajectory Considerations: Two-Steer 31

32 Beyond Basic Kinematics Speed, force & mass dynamics Important when: High speed High/varying mass Limited torque Friction, slip, skid,.. How to actuate: motorization How to get to some goal pose: controllability 32

33 Motion Control (kinematic control) Objective: follow a trajectory position and/or velocity profiles as function of time. Motion control is not straightforward: mobile robots are non-holonomic systems! Most controllers are not considering the dynamics of the system 33

34 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 34

35 Feedback Control y R ω(t) start v(t) θ x R e goal Find a control matrix K, if exists k K = k with k ij =k(t,e) such that the control of v(t) and ω(t) drives the error e to zero. k k k k v( t) = K e = K ω( t) R x y θ lim e( t) = 0 t 35

36 Kinematic Position Control: Differential Drive Robot Kinematics: I x& cosθ = y& sinθ & θ 0 0 v 0 ω 1 y α: 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 36

37 Coordinate Transformation Coordinate transformation into polar coordinates with origin at goal position: y System description, in the new polar coordinates for for 37

38 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, i.e. it is unbounded For the forward direction of the robot points toward the goal, for 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. a 38

39 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 movement parking maneuver is performed always in the most natural way and without ever inverting its motion. 39

40 Kinematic Position Control: Resulting Path 40

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

42 Summary Kinematics: actuator motion effector motion Mobil robots: Relating speeds Manipulators: Relating poses Wheel constraints Rolling constraints Sliding constraints Maneuverability (DOF) = mobility (=DDOF) + steerability Zero motion line, instantaneous center of rotation (ICR) Holonomic vs. non-holonomic Path, trajectory Feedforward vs. feedback control Kinematic control 42