Centre for Autonomous Systems

Transcription

1 Robot Henrik I Centre for Autonomous Systems Kungl Tekniska Högskolan hic@kth.se 27th April 2005

2 Outline 1 duction 2 Kinematic and Constraints 3 Mobile Robot 4 Mobile Robot 5 Beyond Basic 6 Kinematic 7

3 Motion Requirements Kinematic / dynamic model of the robot Model of ground/wheel interaction Definition of required motion velocity / position control Design of control law to satisfy constriants

4 Mobile Robot Model of mechanical behaviour of robot for design and control can be used both for mobile systems and manipulators Manipulators allow direct estimation of position, which is not always true for mobile systems Position to be derived from integration over time Motion is not free. There are constraints to be considered in the design and control generation.

5 Outline 1 duction 2 Kinematic and Constraints 3 Mobile Robot 4 Mobile Robot 5 Beyond Basic 6 Kinematic 7

6 Reference frames Inertial reference frame (I) Robot references frame (R) Robot pose ξ I = x y θ

7 Transformation between reference frames The relation between the references frame is through the standard orthogonal rotation transformation: cos(θ) sin(θ) 0 R(θ) = sin(θ) cos(θ) Enable handling of motion between frames ξ R = R(θ) ξ I

8 Simple 90 rotation Now with ξ R = R(θ) ξ I ξ R = R( π 2 ) ξ I Implies ξ R = R( π 2 ) ξ I = R( π 2 ) = ẋ ẏ θ = ẏ ẋ θ

9 Kinematic modelling Goal: Determine the robot speed ξ = [ ] T ẋ ẏ θ as a function of wheel speed ϕ, steering angle β, steering speed β and the geometric parameters of the robot. Forward kinematics ξ = ẋ ẏ θ = f ( ϕ 1,..., ϕ n, β 1,..., β m, β 1,..., β m ) Inverse kinematics [ ] ϕ 1... ϕ n β 1... β m β 1... β T m = f (ẋ, ẏ, θ) Why not x y θ = f (ϕ 1,..., ϕ n, β 1,..., β m ) the relation is not straight forward. See later.

10 Forward kinematic model differential drive r Assume a set up with two drive wheels. Wheels have radius r, and a places at a distance l from the center. Wheels rotate at speeds ϕ 1 and ϕ 2 Prediction of the motion of the robot motion in the global frame l P l ξ I = ẋ ẏ θ = f (l, r, θ, ϕ 1, ϕ 2 )

11 Differential drive model Given ξ I = R(θ) 1 ξ R Speed of each wheel is r ϕ i, the translational speed is the average velocity ẋ R = r ϕ 1 + ϕ 2 2 The instantaneous rotation of P for one wheel is The total rotation is then ω 1 = r ϕ 1 2l θ = r 2l ( ϕ 1 ϕ 2 )

12 Differential drive model Given ξ I = R(θ) 1 ξ R The full model is then: ξ I = R(θ) 1 r 2 ϕ 1 + ϕ 2 0 ϕ 1 ϕ 2 l The rotation matrix is trivial to invert cos(θ) sin(θ) 0 R(θ) 1 = sin(θ) cos(θ)

13 Differential motion ICCor ICR The Instantaneous Center of Rotation (ICR) or Instantaneous Center of Curvature is relevant. Characterised by rotation ω and radius R: R = l ϕ 1 + ϕ 2 ϕ 1 ϕ 2 With ϕ 1 = ϕ 2 R =

14 Kinematic constraints Assumptions Plane of wheel is always vertical Single point of contact with surface Motion is purely by rolling (no slippage) Rotation of wheel is around the vertical axis

15 Fixed wheel constraint r The motion of the wheel must be in the plane of the wheel Speed of wheel v = r ϕ The motion must then be (rolling constraint): ˆ sin(α + β) cos(α + β) ( l) cos(β) R(θ) ξ I r ϕ = 0 Motion in the orthogonal plane must be zero (sliding constraint), i.e. beta ˆ cos(α + β) sin(α + β) l sin(β) R(θ) ξi = 0 l alpha v Wheels cannot slide sideways! Similar models can be developed for Swedish and Spherical wheels (see the book!)

16 Steering wheel constraint d r The motion of the wheel must be in the plane of the wheel Speed of wheel v = r ϕ, Rotation speed is β The motion must then be (rolling constraint): ˆ sin(α + β) cos(α + β) ( l) cos(β) R(θ) ξ I r ϕ = 0 Motion in the orthogonal plane must be zero (sliding constraint), i.e. beta ˆ cos(α + β) sin(α + β) (d + l) sin(β) R(θ) ξi + β = 0 l alpha d v Rolling constraint similar to fixed wheel, but the sliding constraint is wrt the wheel-contact point

17 Robot Kinematic Constraints Combination of the wheel constraints imposes the overall constraints for the vehicle Differential between fixed and steerable wheels Assume N wheels divided into N f fixed and N s steerable wheels β f is orientation of fixed wheels β s (t) is the steering angle of the controllable wheels Define motion of wheels as: ϕ(t) = [ ϕf (t) ϕ s (t) ]

18 Kinematic constraints The rolling constraints can be collected: where J 1 (β s )R(θ) ξ I J 2 ϕ = 0 [ J 1 (β s ) = J 1f J 1s (β s ) J 1f is an N f 3 constant matrix and J 1s is an N s 3 matrix of constraints. ] J 2 is a diagonal matrix with wheel radii r i A similar set of constraints can be defined for sliding (C)

19 Differential drive constraints example Fusing rolling and sliding constraints we obtain: [ ] [ ] J1 (β s ) R(θ) C 1 (β s ) ξ J2 ϕ I = 0 Assume robot axis along +X R, then α = π 2, and β = π for right wheel and α = π 2 and β = 0 for left wheel

20 Differential drive contraint Resulting in 2» l 4 ˆ 1 0 l 5 R(θ) ξ I = Inverting the equation results in:» J2 ϕ 0 2 ξ I = R(θ) l 3 2 3» 1 0 l 5 1 J2 ϕ = » R(θ) J2 ϕ 1 l l

21 Outline 1 duction 2 Kinematic and Constraints 3 Mobile Robot 4 Mobile Robot 5 Beyond Basic 6 Kinematic 7

22 Degree of mobility The sliding constraints can be separated between fixed and steerable wheels: C 1f R(θ) ξ I = 0 C 1s (β s )R(θ) ξ I = 0 Motion of the robot must belong to the null space of C 1, i.e. C 1 (β s ) m = 0, m null(c 1 ) Constraint is also shown by the instantaneous centre of rotation (ICR) mentioned earlier

23 Instantaneous centre of rotation ICR ICR Ackermann steering Bicycle

24 Mobility constraints The rank(c 1 ) defines the number of independent constraints The degree of mobility is defined by the dimensionality of the null space of C 1 which for a mobile platform is equal to: δ m = dim(null(c 1 )) = 3 rank(c 1 ) Examples: Robot δ m Differential drive 2 Bicycle 1

25 Degree of steerability Steerability is the number of independent DOF that can be controlled δ s = rank(c 1s ) Similarly the degree of maneuverability is defined as δ M = δ m + δ s

26 examples

27 Outline 1 duction 2 Kinematic and Constraints 3 Mobile Robot 4 Mobile Robot 5 Beyond Basic 6 Kinematic 7

28 The / Degree of Freedom :controllable degree of freedom The workspace: the space of possible configurations The velocity space: independent degree of motion that can be controlled. Sometimes referred to as the Differentiable degree of freedom (DDOF). DDOF = δ m. DDOF δ M DOF

29 Holonomic Systems Holonomy is frequently used in robotics Holonomic kinematic constraints: defined by position / pose variables Non-holonomic kinematic constraints: defined by differential variables. The pose cannot be recovered by integration. Non-holonomic systems are also referred to as non-integrable systems.

30 Holonomic systems Example, fixed wheel sliding constraint: [ cos(α + β) sin(α + β) l sin(β) ] R(θ) ξ I = 0 the constraint is in terms of ξ rather the ξ as it constraint the motion not the final configuration. A systems is only holonomic iff DDOF = DOF.

31 Path/trajectory considerations The constraints only define what can be achieved and the limitations. For systems there is a a need to consider how to achieve different configuration. Trajectory planning: partly covered in final lecture Trajectory control/tracking: given a specification how can the robot be moved to achieve the specified trajectory

32 Example of trajectory control

33 Outline 1 duction 2 Kinematic and Constraints 3 Mobile Robot 4 Mobile Robot 5 Beyond Basic 6 Kinematic 7

34 Beyond Basic The above analysis has been performed under a strong set of assumptions: only rolling motion with no sliding Tanks, rigid vehicles, etc use skid steering in which sliding motion is utilized to ensure control of the vehicle. Requires consideration of dynamic models beyond pure kinematics The friction model for interaction between surface and wheel must be considered. High speed motion also requires explicit modelling of system dynamics for the control. Dynamics is a separate field of research in robotics. Beyond the scope of this course.

35 Outline 1 duction 2 Kinematic and Constraints 3 Mobile Robot 4 Mobile Robot 5 Beyond Basic 6 Kinematic 7

36 Kinematic Start Goal

37 Trajectory following Kinematic control is following of a pre-specified trajectory described in terms of positions and velocities Often the trajectory is divided into trajectory segments Simple controllers use a combination of arcs and line segments (as done on American roads). Others use clothoids in which the curvature changes linearly with time, as done on European roads An entire field of robotics is devoted to path planning See for comprehensive / free book on the topic.

38 Feedback control A more appropriate strategy is a trajectory feedback controller that uses the path specification as control points to drive the robot system

39 Problem statement In the robot reference frame the error is e = [x, y, θ] T R The task is now to design a control matrix K [ ] k11 k K = 12 k 13 k k 21 k 22 k ij = k(t, e) 23 Such that [ v(t) ω(t) ] = Ke = K drives the error to zero lim t e(t) = 0 x y θ R

40 The basic setup for control β ρ α θ x

41 Kinematic model Consider a differential drive robot in the inertial frame ẋ cos(θ) 0 [ ] ẏ = sin(θ) 0 v ω θ 0 1 In polar coordinates the error is now ρ = x 2 + y 2 α = θ + atan2( y, x) β = θ α

42 Kinematic control Rephrased in polar coordinates: ρ cos(α) 0 α sin(α) = ρ 1 β sin(α) ρ 0 [ v ω ]

43 The control law the linear control law: v = k ρ ρ ω = k α α + k β β Which generates a closed loop system of: ρ k ρ ρ cos(α) α = k ρ sin(α) k α α k β β β k ρ sin(α)

44 Stability requirement It can be shown that the system is exponentially stable if: k ρ > 0 k β < 0 k α k ρ > 0 Sketch of proof (cos x = 1, sin x = x): ρ α β = k ρ (k α k ρ ) k β 0 k ρ 0 ρ α β

45 Sketch of stability requirement if A has all eigenvalues where the real part is negative it is exponentially stable Characteristic polynomial: (λ + k ρ )(λ 2 + λ(k α k ρ ) k ρ k β )

46 examples

47 Outline 1 duction 2 Kinematic and Constraints 3 Mobile Robot 4 Mobile Robot 5 Beyond Basic 6 Kinematic 7

48 WRAP-UP Brief introduction to kinematic modelling of mobile systems Presentation of constraints and its use in models Example models for robots Brief example of kinematic control