BEST2015 Autonomous Mobile Robots Lecture 2: Mobile Robot Kinematics and Control

Transcription

1 BEST2015 Autonomous Mobile Robots Lecture 2: Mobile Robot Kinematics and Control Renaud Ronsse École polytechnique de Louvain, UCLouvain July Introduction Mobile robot and manipulator arm characteristics Arm is fixed to the ground and usually comprised of a single chain of actuated links. Mobile robot motion is defined through rolling and sliding constraints taking effect at the wheel-ground contact points. robot arm ride Youtube PR2 Robot Fetches Beer from the Refrigerator Youtube 2

2 Introduction Kinematics is the subfield of Mechanics which deals with motions of bodies. Industrial vs. Mobile Robot Kinematics: Both are concerned with forward and inverse kinematics However, for mobile robots, encoder values don t map to unique robot poses Mobile robots can move unbound with respect to their environment There is no direct (=instantaneous) way to measure the robot s position Position must be integrated over time, depends on path taken Leads to inaccuracies of the position (motion) estimate Understanding mobile robot motion starts with understanding wheel constraints placed on the robot s mobility Usually, mobile robots are less limited by dynamics than manipulators, except maybe to limit the turning radius. 3 Non-Holonomic Systems Differential equations are not integrable to the final position. Measuring 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. This is in stark contrast to actuator arms. s 1 = s 2 ; s 1R = s 2R ; s 1L = s 2L BUT x 1 x 2 AND y 1 y 2 c Siegwart et al., 2011, ETH-Z lecture slides 4

3 Non-Holonomic Systems A differential-drive 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 θ + t t sin θ so, ds = dx cos θ + dy sin θ. c Siegwart et al., 2011, ETH-Z lecture slides The velocity function v(t) is said to be integrable (holonomic) if there exists a trajectory function s(t) that can be described by the values of 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 Condition for s to be integrable function. 5 Non-Holonomic Systems The trajectory of a holonomic robot can be described as s = s(x, y, θ), i.e.: ds = s s s dx + dy + x y θ dθ Is this compatible with the differential-drive robot equation? ds = dx cos θ + dy sin θ Yes, if Homonomic? s x = cos θ ; s y = sin θ ; s θ = 0 2 s x y = 0 = 2 s y x ; 2 s x θ = 0 2 s θ x = sin θ ; 2 s y θ = 0 2 s θ y = cos θ These two conditions cannot be simultaneously satisfied a differential-drive robot is not holonomic! 6

4 1 Introduction 2 Kinematic Models 3 Kinematic Constraints 4 Mobile Robot Maneuverability 5 Mobile Robot Workspace 6 Motion Control 7 References 7 Forward and Inverse Kinematics Forward kinematics: Transformation from joint- to physical space. Inverse kinematics: Transformation from physical- to joint space Required for motion control. Due to nonholonomic constraints in mobile robotics, we deal with differential (inverse) kinematics Transformation between velocities instead of positions 8

5 Differential Kinematics Model Goal: establish the robot speed ξ = ẋ, ẏ, θ T as a function of the wheel speeds ϕ i, steering angles β i, steering speeds β i and the geometric parameters of the robot (configuration coordinates). forward kinematics: ẋ, ẏ, θ T = ff ( ϕ 1,..., ϕ n, β 1,..., β m, β 1,..., β m ) inverse kinematics: ϕ 1,..., ϕ n, β 1,..., β m, β 1,..., β m T = fi (ẋ, ẏ, θ) But this is generally not integrable into: ξ = because the robot is not holonomic... x y θ = f F 2 (ϕ 1,..., ϕ n, β 1,..., β m ) 9 Representing Robot Pose Mapping the robot pose ξ = x, y, θ T from an inertial {X I, Y I } to a robot-attached {X R, Y R } frame: ξ R = R(θ) ξ I = R(θ) ẋ, ẏ, θ T with R(θ) = cos θ sin θ 0 sin θ cos θ c Siegwart et al., 2011, Fig. 3.1, p

6 Example: Robot aligned with Y I R(θ) = cos θ sin θ 0 sin θ cos θ with θ = π 2 : ξ R = ẋ ẏ θ c Siegwart et al., 2011, Fig. 3.2, p. 60 = ẏ ẋ θ 11 Forward kinematics models Forward kinematics: ξ I = ẋ, ẏ, θ T = ff ( ϕ 1,..., ϕ n, β 1,..., β m, β 1,..., β m ) We already know that: ξi = R(θ) 1 ξr. Problem: what is ξ R as a function of the wheel speeds ϕ i, steering angles β i, steering speeds β i and the geometric parameters of the robot? Example of a differential-drive robot: ξ R = r ϕ r ϕ r ϕ 1 2l r ϕ 2 2l Problem of wheel kinematic constraints. 12

7 1 Introduction 2 Kinematic Models 3 Kinematic Constraints 4 Mobile Robot Maneuverability 5 Mobile Robot Workspace 6 Motion Control 7 References 13 Wheel kinematic constraints Assumptions Movement on a horizontal plane Vertical wheel plane and point contact of the wheels Wheels not deformable Pure rolling (v c = 0 at contact point) c No slipping, skidding or sliding No friction for rotation around contact point Steering axes orthogonal to the surface Wheels connected by rigid frame (chassis) 14

8 1- Fixed Standard Wheel Fixed angle to the chassis (α). Motion back and forth in the wheel plane. Rotation around the contact point Rolling constraint: sin (α + β) cos (α + β) l cos (β) c Siegwart et al., 2011, Fig. 3.4, p. 64 R(θ) ξ I }{{} ξ R r ϕ = 0 No sliding constraint: cos (α + β) sin (α + β) l sin (β) R(θ) ξ I = Fixed Standard Wheel / Example Suppose that the wheel A is such that α = β = 0. The contact point is thus on X R and the wheel is parallel to Y R. Suppose moreover that θ = 0: 0 1 l I c Siegwart et al., 2011, Fig. 3.4, p. 64 ẋ, ẏ, θ T r ϕ = 0 ẏ l θ = r ϕ I ẋ, ẏ, θ T = 0 ẋ = 0 16

9 2- Steered Standard Wheel c Siegwart et al., 2011, Fig. 3.5, p. 66 Rolling constraint: sin (α + β(t)) cos (α + β(t)) l cos (β(t)) No sliding constraint: cos (α + β(t)) sin (α + β(t)) l sin (β(t)) R(θ) ξ I r ϕ = 0 R(θ) ξ I = 0 β(t) is a new (second) degree of freedom of the wheel but has no direct impact on the motion constraint Castor Wheel Able to steer around a vertical axis (point A). The vertical axis of rotation does not pass through the contact point. Rolling constraint: sin (α + β) cos (α + β) l cos (β) c Siegwart et al., 2011, Fig. 3.6, p. 67 R(θ) ξ I r ϕ = 0 No sliding constraint equilibrium of rotations around A: cos (α + β) sin (α + β) d+l sin (β) R(θ) ξ I +d β = 0 By setting the value of β, any lateral movement can be reached (omnidirectional). 18

10 4- Swedish Wheel Airtrax Cobra Youtube Swedish Wheel No vertical axis of rotation. Rollers attached on the periphery with rotation axes being anti-parallel to the main rotation axis. c Siegwart et al., 2011, Fig. 3.8, p. 69 Rolling constraint: sin (α + β+γ) cos (α + β+γ) l cos (β+γ) R(θ) ξi r ϕcos γ = 0 No sliding constraint free rotation of the small rollers: cos (α + β+γ) sin (α + β+γ) l sin (β+γ) R(θ) ξi r ϕ sin γ r sw ϕ sw = 0 By setting the value of ϕ sw, any lateral movement can be reached (omnidirectional). γ = 0 ensures full decoupling of the equation constraints, but causes some mechanical issues... 20

11 5- Spherical Wheel No principal axis of rotation. No constraints on the robot chassis kinematics (omnidirectional). c Siegwart et al., 2011, Fig. 3.9, p. 70 Rolling constraint roll rate ϕ in the direction of motion: sin (α + β) cos (α + β) l cos (β) R(θ) ξ I r ϕ = 0 No sliding constraint no orthogonal wheel rotation (by definition): cos (α + β) sin (α + β) l sin (β) R(θ) ξ I = 0 In this case, β is a free variable deduced from the second equation. It can be discontinuous Kinematic Constraints of the Complete Robot In sum, each wheel imposes zero or more constraints on the robot motion. Actually, only fixed and steerable standard wheels impose constraints. Suppose a robot with M wheels including N = N f + N s standard wheels. Matrix form of the constraint equations: Rolling: J 1 (β s )R(θ) ξ I J 2 ϕ = 0 ϕf (t) J with ϕ(t) = (N 1), J ϕ s (t) 1 (β s ) = 1f J 1s (β s ) and J 2 = diag(r 1,..., r N ) (N N). Lateral movement (no slidding): (N 3) with C 1 (β s ) = C 1 (β s )R(θ) ξ I = 0 C 1f (N 3). C 1s (β s ) This last constraint has the most significant impact on the robot maneuverability. 22

12 Example 1: differential-drive J1f C 1f R(θ) ξ I = J2 ϕ 0 Right wheel: α = π 2, β = π. Left wheel: α = π 2, β = 0. c Siegwart et al., 2011, ETH-Z lecture slides ξ I = R(θ) l 1 0 l J2 ϕ 0 and we retrieve: ξi = R(θ) = R(θ) r ϕ r ϕ r ϕ 1 2l r ϕ 2 2l 1 2l 1 2l 0 J2 ϕ 0 23 Example 2: omnidirectional c Siegwart et al., 2011, Figs and 3.11, pp No lateral constraints (only Swedish wheels): ξi = R(θ) 1 J1f 1 J 2 ϕ. With α 1 = π 3, α 2 = π, and α 3 = π 3, β 1 = β 2 = β 3 = 0, and γ 1 = γ 2 = γ 3 = 0 (90 wheels): J 1 1f = l 0 1 l l 1 = l 3 1 3l 3 1 3l 24

13 1 Introduction 2 Kinematic Models 3 Kinematic Constraints 4 Mobile Robot Maneuverability 5 Mobile Robot Workspace 6 Motion Control 7 References 25 Mobile Robot Maneuverability The maneuverability of a mobile robot is the combination: of the mobility available based on the sliding constraints and additional freedom contributed by steering Three wheels are sufficient for static stability: additional wheels need to be synchronized this is also the case for some arrangements with three wheels Maneuverability can be derived using the previous equations (mainly C 1 (β s )R(θ) ξ I = 0): Degree of mobility δ m Degree of steerability δ s Robots maneuverability δ M = δ m + δ s 26

14 Degree of Mobility δ m To avoid any lateral slip the motion vector has to satisfy the constraint C 1 (β s )R(θ) ξ C I = 0, with C 1 (β s ) = 1f. C 1s (β s ) Mathematically, R(θ) ξ I must belong to the null space of the projection matrix C 1 (β s ). The null space of C 1 (β s ) is the space N such that for any vector n in N: C 1 (β s )n = 0 Geometrically this can be shown by the Instantaneous Center of Rotation (ICR). 27 Instantaneous Center of Rotation Car-like Ackerman steering: Bicycle: c Siegwart et al., 2011, Fig. 3.12, p. 78 where the dashed lines are zero motion lines... The robot mobility is a function of the number of constraints on the robot s motion, not the number of wheels! 28

15 Degree of Mobility δ m The robot chassis kinematics is a function of the set of independent constraints: the larger the rank of C 1 (β s ), the more constrained is the mobility. Mathematically: δ m = dim NC 1 (β s ) = 3 rank C 1 (β s ) No standard wheels: rank C 1 (β s ) = 0 δ m = 3. All direction constrained: rank C 1 (β s ) = 3 δ m = 0. Examples: Unicycle: One single fixed standard wheel Differential drive: Two fixed standard wheels (on same axle? on different axle?) Bicycle 29 Degree of Steerability δ s Indirect degree of motion: after steering, the robot change its pose only after a movement. δ s = rank C 1s (β s ) There is a compromise between δ m and δ s! The particular orientation at any instant imposes a kinematic constraint However, the ability to change that orientation can lead additional degree of maneuverability 0 δ s 2 Examples: one steered wheel: Tricycle two steered wheels: No fixed standard wheel N f = 0 (ICR can be placed anywhere in the plane) car (Ackermann steering): N f = 2, N s = 2 common axle 30

16 Robot Maneuverability Degree of Maneuverability: δ M = δ m + δ s It includes both the degrees of freedom that the robot manipulates directly through wheel velocity and the degrees of freedom that it indirectly manipulates by changing the steering configuration and moving. Two robots with same δ M are not necessary equal. Example: c Siegwart et al., 2011, Fig. 3.13, p. 79 For any robot with δ M = 2, the ICR is always constrained to lie on a line For any robot with δ M = 3, the ICR is not constrained and can be set to any point on the plane 31 Five Basic Types of Three-Wheel Configurations c Siegwart et al., 2011, Fig. 3.14, p

17 Last example: Synchro-Drive c Siegwart et al., 2011, Fig. 2.28, p. 43 N f = 0 and N s = 3, so rank C 1s (β s ) can be used to determine both δ m and δ s. rank C 1s (β s ) = 2 (two independent constraints) δ m = 1. δ s = 1 (not 2), because there is a single steering motor! δ M = 2: there is no way for the chassis orientation to change! 33 1 Introduction 2 Kinematic Models 3 Kinematic Constraints 4 Mobile Robot Maneuverability 5 Mobile Robot Workspace 6 Motion Control 7 References 34

18 Degrees of Freedom The Degree of Freedom (DOF) is the robot s ability to achieve various poses. But what is the degree of vehicle s freedom in its environment? How is the vehicle able to move between different configurations in its workspace? The robot s independently achievable velocities = differentiable degrees of freedom (DDOF) = δ m Bicycle: δ M = δ m + δ s = 1 + 1; DDOF = 1; DOF = 3 Omni Drive: δ M = δ m + δ s = 3 + 0; DDOF = 3; DOF = 3 Just as workspace DOF governs the robot s ability to achieve various poses, so the robot s DDOF governs its ability to achieve various paths. 35 Degrees of Freedom, Holonomy DDOF = δ m δ M DOF Holonomic Robots: holonomic kinematic constraint can be expressed as an explicit function of position variables only non-holonomic constraint requires a different relationship, such as the derivative of a position variable fixed and steered standard wheels impose non-holonomic constraints robots with δ m = 3 are always holonomic robots with δ M < 3 can be holonomic (this depends on the dimension of their workspace) An omnidirectional robot is a holonomic robot with DDOF = 3. Caveas: nonholonomic constraints can drastically improve the stability of movements (e.g. lateral forces counteracted by sliding constraints). 36

19 Path/Trajectory Considerations Omnidirectional Drive: Siegwart et al., 2011, Fig. 3.15, p. 88 c Can follow any path in the workspace: δ M = δ m = Path/Trajectory Considerations Two-Steer: Siegwart et al., 2011, Fig. 3.16, p. 89 c Can follow any path in the workspace: δ M = 3, but δ m = 1: any trajectory? 38

20 Beyond Basic Kinematics At higher speeds, and in difficult terrain, dynamics become important. For other vehicles, the no-sliding constraints, and simple kinematics presented in this lecture do not hold. Autonomous Audi TTS ascends Pikes Peak without a driver Youtube c Siegwart et al., 2011, ETH-Z lecture slides 39 1 Introduction 2 Kinematic Models 3 Kinematic Constraints 4 Mobile Robot Maneuverability 5 Mobile Robot Workspace 6 Motion Control 7 References 40

21 Overview 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 straightforward because mobile robots are typically non-holonomic and MIMO systems. Most controllers are neglecting the dynamics of the system. Usually, we separate between: High- (or middle-) level controller: computes the desired wheels velocity from the point where you want to go, or the potential field force, etc. Low-level controller: computes the actuator control (voltage) from the desired and actual velocity. 41 Open Loop Control Trajectory (path) divided in motion segments of clearly defined shape. Control problem: pre-compute a smooth trajectory based on line, circle (and clothoid) segments not taking the robot s actual position into account Disadvantages: c Siegwart et al., 2011, Fig. 3.17, p. 91 it is not 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 or perturbations occur. the resulting trajectories are usually not smooth (acceleration, jerk, etc.) 42

22 Feedback Control Find a control matrix K k11 k K = 12 k 13 k 21 k 22 k 23 with k ij = k ij (t, e), such that the control of v(t) and ω(t): v(t) ω(t) = Ke = K x y θ R c Siegwart et al., 2011, Fig. 3.18, p. 92 drives the error e to zero: lim t e(t) = 0. e = x, y, θ T R, i.e. the goal coordinates is the robot s reference frame. 43 Kinematic Position Control The kinematics of a differential drive mobile robot described in the inertial frame {X I, Y I, θ} is given by ẋ cos θ 0 ẏ = sin θ 0 θ 0 1 I v(t) ω(t) where ẋ and ẏ are the linear velocities in the direction of X I and Y I in the inertial 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. c Siegwart et al., 2011, Fig. 3.19, p

23 Kinematic Position Control: Coordinates Transformation c Siegwart et al., 2011, Fig. 3.19, p. 93 Coordinates transformation into polar coordinates with origin at goal position: ρ = x 2 + y 2 α = θ + arctan2 ( y, x) β = θ α System description, in the new polar coordinates: ρ α β cos α 0 = sin α 1 ρ sin α 0 ρ v(t) ω(t) ; ρ α β cos α 0 = sin α 1 ρ sin α 0 ρ v(t) ω(t) if α I 1 = ( π 2, π 2. ( ( if α I 2 = π, π 2 π 2, π. 45 Kinematic Position Control: Remarks The coordinates transformation is not defined at x = y = 0; For α I 1, the forward direction of the robot points toward the goal, for α I 2, it is the backward direction. By properly defining the forward direction of the robot at its initial configuration, it is always possible to have α I 1 at t = 0. However this does not mean that α remains in I 1 for all time t. 46

24 Kinematic Position Control: The Control Law By using v(t) = k ρ ρ(t) and ω(t) = k α α(t) + k β β(t), the closed-loop system becomes: ρ α = β k ρ ρ cos α k ρ sin α k α α k β β k ρ sin α has a unique equilibrium point at (ρ, α, β) = (0, 0, 0). The control signal v has always constant sign, positive if α(0) I 1, negative otherwise: 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. The closed loop control system is locally exponentially stable if k ρ > 0, k β < 0, k α k ρ > 0 47 Kinematic Position Control: Resulting Path The goal is in the center and the initial position on the circle: (k ρ, k α, k β ) = (3, 8, 1.5). c Siegwart et al., 2011, Fig. 3.20, p

25 Low-level Velocity Control How do we control the robot wheels to reach the desired velocity? Dynamics of a permanent magnet DC-motor: τ m = K 1 φi a = K m i a V b = K 2 φω m = K b ω m K m = K b = Rτ 0 V r in MKS units. c Spong et al., 2006, Fig. 6.2, p. 205 V b is the back electromotive force (back emf). c Spong et al., 2006, Fig. 6.3, p Coupling to the wheel dynamics c Spong et al., 2006, Figs. 6.3 and 6.5, pp L di a dt + Ri a = V V b = V K b ω m J m dω m dt + F m ω m = τ m τ l r = K mi a τ l r, Note: F m is denoted as B m in the Figure, but the notation F m is preferred to represent friction. 50

26 Coupling to the wheel dynamics Laplace domain: c Spong et al., 2006, Fig. 6.6, p. 209 Ω m (s) V (s) Ω m (s) τ l (s) = = = = K m (Ls + R)(J m s + F m ) + K b K m K m /R J m s + F m + K b K m /R (Ls + R)/r (Ls + R)(J m s + F m ) + K b K m 1/r J m s + F m + K b K m /R if the electrical time constant can be neglected (L/R J m /F m ). 51 Coupling to the wheel dynamics Back to the time domain: J m ω mm (t) + (F m + K b K m /R)ω m (t) = (K m /R)V (t) τ l (t)/r or equivalently: J ω(t) + F v ω(t) = u(t) d(t) where F v = F m + K b K m /R (effective damping), u = (K m /R)V (control input), and d = τ l /r (disturbance input). c Spong et al., 2006, Fig. 6.7, p

27 Proportional (P) control Closed-loop system: Ω(s) = U(s) = K p (Ω r (s) Ω(s)) K p Js + F v + K p Ω r (s) 1 Js + F v + K p D(s) and the system pole (root of the characteristic polynomial s + F v+k p J ) can be tuned with K p. Tracking error: E(s) = Ω r (s) Ω(s) = Js + F v Js + F v + K p Ω r (s) + with Ω r (s) = Ω r /s (step) and D(s) = D/s (constant disturbance), steady-state error: e ss = lim s 0 se(s) = D F v + K p 1 Js + F v + K p D(s) which is never 0 for finite K p. 53 Tuning example Tuning the characteristic polynomial s + ω (with K p = ωj F v ): c Spong et al., 2006, Figs and 6.11, pp with disturbance... 54

28 Proportional-integral (PI) control U(s) = Closed-loop system: Ω(s) = ( K p + K ) i (Ω r (s) Ω(s)) s K i + K p s s Js 2 Ω r (s) + (F v + K p )s + K i Js 2 D(s) + (F v + K p )s + K i and both system poles (roots of the characteristic polynomial s 2 + F v+k p J s + K i J ) can be tuned with K p and K i. Tracking error: E(s) = Ω r (s) Ω(s) = Js 2 + F v s s Ω Js 2 r (s)+ D(s) + (F v + K p )s + K i Js 2 + (F v + K p )s + K i with Ω r (s) = Ω r /s (step) and D(s) = D/s (constant disturbance), steady-state error: as soon as K i 0. e ss = lim s 0 se(s) = 0 55 Tuning example Tuning the characteristic polynomial s 2 + 2ζωs + ω 2 (with K i = ω 2 J and K p = 2ζωJ F v ), with ζ = 1 (critical damping): c Spong et al., 2006, Fig. 6.10, p

29 Effect of the integral term c Spong et al., 2006, Fig. 6.13, p. 215 Caveat: PI controllers are prone to make the system unstable, also because of non-linear effects like saturations (due to physical limits on the maximum torque, can be improved by using anti-windup) and neglected dynamics (joint flexibility, electrical time constant, etc.). 57 Summary Mobile robot motion is defined through rolling and sliding constraints; wheel kinematic constraints require pure rolling and no sliding conditions; due to nonholonomic constraints in mobile robotics, differential (inverse) kinematics is required instead of position kinematics; only fixed and steerable standard wheels impose constraints on the robot motion (non-holonomic constraints); holonomic robots have only holonomic kinematic constraints which can be expressed as an explicit function of position variables only 58

30 Summary the maneuverability of a mobile robot is the combination of the mobility available based on the sliding constraints (δ m ) and additional freedom contributed by steering (δ s ); an omnidirectional robot is a holonomic robot with DDOF = 3; a simple control law in polar coordinates is locally exponentially stable and reaches the goal with a desired orientation (maneuver). P and PI controllers are fundamental blocks for achieving low-level velocity control. The integral term is necessary to cancel the static error. 59 References Autonomous Mobile Robots (2nd Edition) Siegwart et al.; The MIT Press, Chapter 3 Robot Modeling and Control Spong et al.; Wiley, 2006 Chapters 6 and 8 Special tribute to Guy Campion (UCL)! 60