BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2:...

30
1 BEST2015 — Autonomous Mobile Robots Lecture 2: Mobile Robot Kinematics and Control Renaud Ronsse [email protected] École polytechnique de Louvain, UCLouvain July 2015 2 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

Transcript of BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2:...

Page 1: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

1

BEST2015 — Autonomous Mobile RobotsLecture 2: Mobile Robot Kinematics and Control

Renaud [email protected]

École polytechnique de Louvain, UCLouvain

July 2015

2

Introduction

Mobile robot and manipulator arm characteristics

Arm is fixed to the ground and usually comprised of a singlechain of actuated links.

Mobile robot motion is defined through rolling and slidingconstraints taking effect at the wheel-ground contact points.

robot arm ride – Youtube PR2 Robot Fetches Beer from theRefrigerator – Youtube

Page 2: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

3

Introduction

Kinematics is the subfield of Mechanics which deals withmotions of bodies.

Industrial vs. Mobile Robot Kinematics:

Both are concerned with forward and inverse kinematicsHowever, for mobile robots, encoder values don’t map tounique robot posesMobile robots can move unbound with respect to theirenvironmentThere is no direct (=instantaneous) way to measure therobot’s positionPosition must be integrated over time, depends on path takenLeads to inaccuracies of the position (motion) estimateUnderstanding mobile robot motion starts with understandingwheel constraints placed on the robot’s mobilityUsually, mobile robots are less limited by dynamics thanmanipulators, except maybe to limit the turning radius.

4

Non-Holonomic Systems

Differential equations are not integrable to the final position.

Measuring the traveled distance of each wheel is not sufficientto calculate the final position of the robot. One has also toknow how this movement was executed as a function of time.

This is in stark contrast to actuator arms.

s1 = s2; s1R = s2R; s1L = s2L

BUT

x1 Ó= x2 AND y1 Ó= y2

c© Siegwart et al., 2011, ETH-Z lecture slides

Page 3: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

5

Non-Holonomic Systems

A differential-drive mobile robot is running along a trajectorys(t).

At every instant of the movement itsvelocity v(t) is:

v(t) =∂s

∂t=

∂x

∂tcos θ +

∂y

∂tsin θ

so, ds = dx cos θ + dy sin θ. c© Siegwart et al., 2011, ETH-Zlecture slides

The velocity function v(t) is said to be integrable (holonomic)if there exists a trajectory function s(t) that can be describedby the values of x, y, and θ only: s = s(x, y, θ).

This is the case if:

∂2s

∂x∂y=

∂2s

∂y∂x;

∂2s

∂x∂θ=

∂2s

∂θ∂x;

∂2s

∂y∂θ=

∂2s

∂θ∂y

Condition for s to be integrable function.

6

Non-Holonomic Systems

The trajectory of a holonomic robot can be described as s = s(x, y, θ),i.e.:

ds =∂s

∂xdx +

∂s

∂ydy +

∂s

∂θdθ

Is this compatible with the differential-drive robot equation?

ds = dx cos θ + dy sin θ

Yes, if

⇒∂s

∂x= cos θ ;

∂s

∂y= sin θ ;

∂s

∂θ= 0

Homonomic?

∂2s

∂x∂y= 0 =

∂2s

∂y∂x;

∂2s

∂x∂θ= 0Ó=

∂2s

∂θ∂x= − sin θ ;

∂2s

∂y∂θ= 0Ó=

∂2s

∂θ∂y= cos θ

These two conditions cannot be simultaneously satisfied

→ a differential-drive robot is not holonomic!

Page 4: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

7

1 Introduction

2 Kinematic Models

3 Kinematic Constraints

4 Mobile Robot Maneuverability

5 Mobile Robot Workspace

6 Motion Control

7 References

8

Forward and Inverse Kinematics

Forward kinematics: Transformation from joint- to physicalspace.

Inverse kinematics: Transformation from physical- to jointspace ⇒ Required for motion control.

Due to nonholonomic constraints in mobile robotics, we deal withdifferential (inverse) kinematics

Transformation between velocities instead of positions

Page 5: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

9

Differential Kinematics Model

Goal: establish the robot speed ξ =[

x, y, θ]T

as a function of the

wheel speeds ϕi, steering angles βi, steering speeds βi and thegeometric parameters of the robot (configuration coordinates).

forward kinematics:[

x, y, θ]T

= fF (ϕ1, . . . , ϕn, β1, . . . , βm, β1, . . . , βm)

inverse kinematics:[

ϕ1, . . . , ϕn, β1, . . . , βm, β1, . . . , βm

]T= fI(x, y, θ)

But this is generally not integrable into:

ξ =

xyθ

= fF 2(ϕ1, . . . , ϕn, β1, . . . , βm)

because the robot is not holonomic. . .

10

Representing Robot Pose

Mapping the robot pose ξ = [x, y, θ]T from an inertial XI , YI toa robot-attached XR, YR frame:

ξR = R(θ)ξI = R(θ)[

x, y, θ]T

with

R(θ) =

cos θ sin θ 0− sin θ cos θ 0

0 0 1

c© Siegwart et al., 2011, Fig. 3.1, p. 59

Page 6: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

11

Example: Robot aligned with YI

R(θ) =

cos θ sin θ 0− sin θ cos θ 0

0 0 1

c© Siegwart et al., 2011, Fig. 3.2, p. 60

with θ = π2 :

ξR =

0 1 0−1 0 00 0 1

xy

θ

=

y−x

θ

12

Forward kinematics models

Forward kinematics:

ξI =[

x, y, θ]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, steeringangles βi, steering speeds βi and the geometric parameters of the

robot?

Example of a differential-drive robot:

ξR =

rϕ1

2 + rϕ2

20

rϕ1

2l− rϕ2

2l

Problem of wheel kinematic constraints.

Page 7: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

13

1 Introduction

2 Kinematic Models

3 Kinematic Constraints

4 Mobile Robot Maneuverability

5 Mobile Robot Workspace

6 Motion Control

7 References

14

Wheel kinematic constraintsAssumptions

Movement on a horizontal plane

Vertical wheel plane and point contact of the wheels

Wheels not deformable

Pure rolling (vc = 0 at contact point)

c©http://faculty.wwu.edu/vawter/PhysicsNet/Topics/RotationalKinematics/RollingWithoutSlipping.html

No slipping, skidding or sliding

No friction for rotation around contact point

Steering axes orthogonal to the surface

Wheels connected by rigid frame (chassis)

Page 8: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

15

1- Fixed Standard Wheel

Fixed angle to thechassis (α).

Motion back andforth in the wheelplane.

Rotation around thecontact point

c© Siegwart et al., 2011, Fig. 3.4, p. 64

Rolling constraint:[

sin (α + β) − cos (α + β) −l cos (β)]

R(θ)ξI︸ ︷︷ ︸

ξR

−rϕ = 0

No sliding constraint:[

cos (α + β) sin (α + β) l sin (β)]

R(θ)ξI = 0

16

1- Fixed Standard Wheel / Example

Suppose that the wheel Ais such that α = β = 0.The contact point is thuson XR and the wheel isparallel to YR.

c© Siegwart et al., 2011, Fig. 3.4, p. 64

Suppose moreover that θ = 0:

[

0 −1 −l]

I[

x, y, θ]T

− rϕ = 0

⇒ −y − lθ = rϕ

[

1 0 0]

I[

x, y, θ]T

= 0

⇒ x = 0

Page 9: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

17

2- Steered Standard Wheel

c© Siegwart et al., 2011, Fig. 3.5, p. 66

Rolling constraint:[

sin (α + β(t)) − cos (α + β(t)) −l cos (β(t))]

R(θ)ξI−rϕ = 0

No sliding constraint:[

cos (α + β(t)) sin (α + β(t)) l sin (β(t))]

R(θ)ξI = 0

β(t) is a new (second) degree of freedom of the wheel but has nodirect impact on the motion constraint.

18

3- Castor Wheel

Able to steer arounda vertical axis (pointA).

The vertical axis ofrotation does notpass through thecontact point.

c© Siegwart et al., 2011, Fig. 3.6, p. 67

Rolling constraint:[

sin (α + β) − cos (α + β) −l cos (β)]

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).

Page 10: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

19

4- Swedish Wheel

Airtrax Cobra – Youtube

20

4- Swedish Wheel

No vertical axis ofrotation.

Rollers attached onthe periphery withrotation axes beinganti-parallel to themain 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 γ − rswϕsw = 0

By setting the value of ϕsw, any lateral movement can be reached(omnidirectional). γ = 0 ensures full decoupling of the equationconstraints, but causes some mechanical issues. . .

Page 11: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

21

5- Spherical Wheel

No principal axis ofrotation.

No constraints on therobot chassiskinematics(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 (bydefinition):

[

cos (α + β) sin (α + β) l sin (β)]

R(θ)ξI = 0

In this case, β is a free variable deduced from the second equation.It can be discontinuous. . .

22

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 = Nf + Ns standard wheels.Matrix form of the constraint equations:

Rolling:J1(βs)R(θ)ξI − J2ϕ = 0

with ϕ(t) =

[ϕf (t)ϕs(t)

]

(N × 1), J1(βs) =

[J1f

J1s(βs)

]

(N × 3)

and J2 = diag(r1, . . . , rN ) (N × N).

Lateral movement (no slidding):

C1(βs)R(θ)ξI = 0

with C1(βs) =

[C1f

C1s(βs)

]

(N × 3).

This last constraint has the most significant impact on the robotmaneuverability.

Page 12: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

23

Example 1: differential-drive

[

J1f

C1f

]

R(θ)ξI =

[

J2ϕ0

]

Right wheel: α = −π2 , β = π.

Left wheel: α = π2 , β = 0.

c© Siegwart et al., 2011, ETH-Z lecture slides

ξI = R(θ)−1

[1 0 l

1 0 −l

0 1 0

]−1 [J2ϕ

0

]

= R(θ)−1

[1

2

1

20

0 0 11

2l−1

2l0

] [J2ϕ

0

]

and we retrieve: ξI = R(θ)−1

rϕ1

2 + rϕ2

20

rϕ1

2l− rϕ2

2l

24

Example 2: omnidirectional

c© Siegwart et al., 2011, Figs. 3.10 and 3.11, pp. 74-75

No lateral constraints (only Swedish wheels): ξI = R(θ)−1J−11f J2ϕ.

With α1 = π3 , α2 = π, and α3 = −π

3 , β1 = β2 = β3 = 0, andγ1 = γ2 = γ3 = 0 (90 wheels):

J−11f =

√3

2−12 −l

0 1 −l−

√3

2−12 −l

−1

=

1√3

0 −1√3

−13

23

−13

−13l

−13l

−13l

Page 13: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

25

1 Introduction

2 Kinematic Models

3 Kinematic Constraints

4 Mobile Robot Maneuverability

5 Mobile Robot Workspace

6 Motion Control

7 References

26

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 C1(βs)R(θ)ξI = 0):

Degree of mobility δm

Degree of steerability δs

Robots maneuverability δM = δm + δs

Page 14: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

27

Degree of Mobility δm

To avoid any lateral slip the motion vector has to satisfy the

constraint C1(βs)R(θ)ξI = 0, with C1(βs) =

[

C1f

C1s(βs)

]

.

Mathematically, R(θ)ξI must belong to the null space of theprojection matrix C1(βs).The null space of C1(βs) is the space N such that for any vector nin N:

C1(βs)n = 0

Geometrically this can be shown by the Instantaneous Center ofRotation (ICR).

28

Instantaneous Center of RotationCar-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 onthe robot’s motion, not the number of wheels!

Page 15: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

29

Degree of Mobility δm

The robot chassis kinematics is a function of the set ofindependent constraints: the larger the rank of C1(βs), the moreconstrained is the mobility. Mathematically:

δm = dim N[C1(βs)] = 3 − rank [C1(βs)]

No standard wheels: rank [C1(βs)] = 0 → δm = 3.

All direction constrained: rank [C1(βs)] = 3 → δm = 0.

Examples:

Unicycle: One single fixed standard wheel

Differential drive: Two fixed standard wheels (on same axle?on different axle?)

Bicycle

30

Degree of Steerability δs

Indirect degree of motion: after steering, the robot change its poseonly after a movement.

δs = rank [C1s(βs)]

There is a compromise between δm and δs!

The particular orientation at any instant imposes a kinematicconstraint

However, the ability to change that orientation can leadadditional degree of maneuverability

0 ≤ δs ≤ 2

Examples:

one steered wheel: Tricycle

two steered wheels: No fixed standard wheel Nf = 0 (ICR canbe placed anywhere in the plane)

car (Ackermann steering): Nf = 2, Ns = 2 → common axle

Page 16: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

31

Robot ManeuverabilityDegree of Maneuverability:

δM = δm + δs

It includes both the degrees of freedom that the robot manipulatesdirectly through wheel velocity and the degrees of freedom that itindirectly 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 ona line

For any robot with δM = 3, the ICR is not constrained and can beset to any point on the plane

32

Five Basic Types of Three-Wheel Configurations

c© Siegwart et al., 2011, Fig. 3.14, p. 83

Page 17: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

33

Last example: Synchro-Drive

c© Siegwart et al., 2011, Fig. 2.28, p. 43

Nf = 0 and Ns = 3, so rank [C1s(βs)] can be used to determineboth δm and δs.

rank [C1s(β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!

34

1 Introduction

2 Kinematic Models

3 Kinematic Constraints

4 Mobile Robot Maneuverability

5 Mobile Robot Workspace

6 Motion Control

7 References

Page 18: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

35

Degrees of Freedom

The Degree of Freedom (DOF) is the robot’s ability to achievevarious poses.

But what is the degree of vehicle’s freedom in its environment?

How is the vehicle able to move between different configurations inits workspace?

The robot’s independently achievable velocities = differentiabledegrees 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 achievevarious poses, so the robot’s DDOF governs its ability to achievevarious paths.

36

Degrees of Freedom, Holonomy

DDOF = δm ≤ δM ≤ DOF

Holonomic Robots:

holonomic kinematic constraint can be expressed as an explicitfunction 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-holonomicconstraints

robots with δm = 3 are always holonomic

robots with δM < 3 can be holonomic (this depends on thedimension of their workspace)

An omnidirectional robot is a holonomic robot with DDOF = 3.

Caveas: nonholonomic constraints can drastically improve thestability of movements (e.g. lateral forces counteracted by slidingconstraints).

Page 19: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

37

Path/Trajectory Considerations

Omnidirectional Drive:

c©Siegwart et al., 2011, Fig. 3.15, p. 88

Can follow any path in the workspace: δM = δm = 3.

38

Path/Trajectory Considerations

Two-Steer:

c©Siegwart et al., 2011, Fig. 3.16, p. 89

Can follow any path in the workspace: δM = 3, but δm = 1: anytrajectory?

Page 20: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

39

Beyond Basic Kinematics

At higher speeds, and in difficult terrain, dynamics becomeimportant.

For other vehicles, the no-sliding constraints, and simplekinematics presented in this lecture do not hold.

Autonomous Audi TTS ascends PikesPeak without a driver – Youtube

c© Siegwart et al., 2011, ETH-Z lecture slides

40

1 Introduction

2 Kinematic Models

3 Kinematic Constraints

4 Mobile Robot Maneuverability

5 Mobile Robot Workspace

6 Motion Control

7 References

Page 21: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

41

Overview

The objective of a kinematic controller is to follow a trajectorydescribed by its position and/or velocity profiles as function oftime.

Motion control is not straightforward because mobile robotsare 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 desiredwheels velocity from the point where you want to go, or thepotential field force, etc.Low-level controller: computes the actuator control (voltage)from the desired and actual velocity.

42

Open Loop ControlTrajectory (path) divided in motion seg-ments of clearly defined shape.Control problem:

pre-compute a smooth trajectorybased on line, circle (and clothoid)segments

not taking the robot’s actual positioninto account

c© Siegwart et al., 2011, Fig. 3.17, p.91

Disadvantages:

it is not an easy task to pre-compute a feasible trajectory

limitations and constraints of the robots velocities andaccelerations

does not adapt or correct the trajectory if dynamical changesof the environment or perturbations occur.

the resulting trajectories are usually not smooth (acceleration,jerk, etc.)

Page 22: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

43

Feedback Control

c© Siegwart et al., 2011, Fig. 3.18, p. 92

Find a control matrix K

K =

[

k11 k12 k13

k21 k22 k23

]

with kij = kij(t, e), such that the con-trol of v(t) and ω(t):

[

v(t)ω(t)

]

= Ke = K

xyθ

R

drives the error e to zero:limt→∞ e(t) = 0.e = [x, y, θ]TR, i.e. the goal coordinatesis the robot’s reference frame.

44

Kinematic Position ControlThe kinematics of a differential drive mobile robot described in theinertial frame XI , YI , θ is given by

xy

θ

I

=

cos θ 0sin θ 0

0 1

[

v(t)ω(t)

]

where x and y are the linear velocities in the direction of XI andYI in the inertial frame.

c© Siegwart et al., 2011, Fig. 3.19, p. 93

Let α denote the angle be-tween the XR axis of therobots reference frame and ρthe vector connecting the cen-ter of the axle of the wheelswith the final position.

Page 23: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

45

Kinematic Position Control: Coordinates Transformation

c© Siegwart et al., 2011, Fig. 3.19, p. 93

Coordinates transformationinto polar coordinates withorigin at goal position:

ρ =√

∆x2 +∆y2

α = −θ + arctan2 (∆y,∆x)

β = −θ − α

System description, in the new polar coordinates:

α

β

]

=

[− cos α 0

sin αρ

−1− sin α

ρ0

][

v(t)ω(t)

]

;

if α ∈ I1 =(

−π2

, π2

].

α

β

]

=

[cos α 0

− sin αρ

−1sin α

ρ0

][

v(t)ω(t)

]

if α ∈ I2 =(

−π, −π2

]∪

(π2

, π].

46

Kinematic Position Control: Remarks

The coordinates transformation is not defined at x = y = 0;

For α ∈ I1, the forward direction of the robot points towardthe goal, for α ∈ I2, it is the backward direction.

By properly defining the forward direction of the robot at itsinitial configuration, it is always possible to have α ∈ I1 att = 0. However this does not mean that α remains in I1 forall time t.

Page 24: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

47

Kinematic Position Control: The Control LawBy using v(t) = kρρ(t) and ω(t) = kαα(t) + kββ(t), theclosed-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) ∈ I1, negative otherwise:

the direction of movement is kept positive or negative duringmovement;

parking maneuver is performed always in the most natural wayand without ever inverting its motion.

The closed loop control system is locally exponentially stable if

kρ > 0, kβ < 0, kα − kρ > 0

48

Kinematic Position Control: Resulting Path

The goal is in the center and the initial position on the circle:

c© Siegwart et al., 2011, Fig. 3.20, p. 97

(kρ, kα, kβ) = (3, 8, −1.5).

Page 25: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

49

Low-level Velocity ControlHow do we control the robot wheels to reach the desired velocity?Dynamics of a permanent magnet DC-motor:

c© Spong et al., 2006, Fig. 6.2, p. 205

τm = K1φia = Kmia

Vb = K2φωm = Kbωm

Km = Kb =Rτ0

Vrin MKS units.

Vb is the back electromotive force (back emf).

c© Spong et al., 2006, Fig. 6.3, p. 207

50

Coupling to the wheel dynamics

c© Spong et al., 2006, Figs. 6.3 and 6.5, pp. 207-208

Ldia

dt+Ria = V − Vb = V − Kbωm

Jmdωm

dt+ Fmωm = τm −

τl

r= Kmia −

τl

r,

Note: Fm is denoted as Bm in the Figure, but the notation Fm is preferred to

represent friction.

Page 26: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

51

Coupling to the wheel dynamicsLaplace domain:

c© Spong et al., 2006, Fig. 6.6, p. 209

Ωm(s)

V (s)=

Km

(Ls+R)(Jms+ Fm) +KbKm

=Km/R

Jms+ Fm +KbKm/R

Ωm(s)

τl(s)=

−(Ls+R)/r

(Ls+R)(Jms+ Fm) +KbKm

=−1/r

Jms+ Fm +KbKm/R

if the electrical time constant can be neglected (L/R ≪ Jm/Fm).

52

Coupling to the wheel dynamics

Back to the time domain:

Jm ˙ωmm(t) + (Fm + KbKm/R)ωm(t) = (Km/R)V (t) − τl(t)/r

or equivalently:

Jω(t) + Fvω(t) = u(t) − d(t)

where Fv = Fm + KbKm/R (effective damping), u = (Km/R)V(control input), and d = τl/r (disturbance input).

c© Spong et al., 2006, Fig. 6.7, p. 210

Page 27: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

53

Proportional (P) control

U(s) = Kp(Ωr(s)− Ω(s))

Closed-loop system:

Ω(s) =Kp

Js+ Fv +KpΩr(s)−

1

Js+ Fv +KpD(s)

and the system pole (root of the characteristic polynomial

s+Fv+Kp

J) can be tuned with Kp.

Tracking error:

E(s) = Ωr(s)− Ω(s) =Js+ Fv

Js+ Fv +Kp

Ωr(s) +1

Js+ Fv +Kp

D(s)

with Ωr(s) = Ωr/s (step) and D(s) = D/s (constantdisturbance), steady-state error:

ess = lims→0

sE(s) =D

Fv +Kp

which is never 0 for finite Kp.

54

Tuning example

Tuning the characteristic polynomial s+ ω (with Kp = ωJ − Fv):

c© Spong et al., 2006, Figs. 6.10 and 6.11, pp. 213-214

with disturbance. . .

Page 28: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

55

Proportional-integral (PI) control

U(s) =

(

Kp +Ki

s

)

(Ωr(s)− Ω(s))

Closed-loop system:

Ω(s) =Ki +Kps

Js2 + (Fv +Kp)s+KiΩr(s)−

s

Js2 + (Fv +Kp)s+KiD(s)

and both system poles (roots of the characteristic polynomial

s2 +Fv+Kp

Js+ Ki

J) can be tuned with Kp and Ki.

Tracking error:

E(s) = Ωr(s)−Ω(s) =Js2 + Fvs

Js2 + (Fv +Kp)s+Ki

Ωr(s)+s

Js2 + (Fv +Kp)s+Ki

D(s)

with Ωr(s) = Ωr/s (step) and D(s) = D/s (constantdisturbance), steady-state error:

ess = lims→0

sE(s) = 0

as soon as Ki Ó= 0.

56

Tuning example

Tuning the characteristic polynomial s2 + 2ζωs+ ω2 (withKi = ω2J and Kp = 2ζωJ − Fv), with ζ = 1 (critical damping):

c© Spong et al., 2006, Fig. 6.10, p. 213

Page 29: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

57

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, alsobecause of non-linear effects like saturations (due to physical limitson the maximum torque, can be improved by using anti-windup)and neglected dynamics (joint flexibility, electrical time constant,etc.).

58

Summary

Mobile robot motion is defined through rolling and slidingconstraints;

wheel kinematic constraints require pure rolling and no slidingconditions;

due to nonholonomic constraints in mobile robotics,differential (inverse) kinematics is required instead of positionkinematics;

only fixed and steerable standard wheels impose constraintson the robot motion (non-holonomic constraints);

holonomic robots have only holonomic kinematic constraintswhich can be expressed as an explicit function of positionvariables only

Page 30: BEST2015—AutonomousMobileRobots Lecture2 ... · 1 BEST2015—AutonomousMobileRobots Lecture2: MobileRobotKinematicsandControl RenaudRonsse renaud.ronsse@uclouvain.be École polytechnique

59

Summary

the maneuverability of a mobile robot is the combination ofthe 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 locallyexponentially stable and reaches the goal with a desiredorientation (maneuver).

P and PI controllers are fundamental blocks for achievinglow-level velocity control. The integral term is necessary tocancel the static error.

60

References

Autonomous Mobile Robots (2nd Edition)Siegwart et al.; The MIT Press, 2011http://www.mobilerobots.ethz.ch/

Chapter 3

Robot Modeling and ControlSpong et al.; Wiley, 2006Chapters 6 and 8

Special tribute to Guy Campion (UCL)!