EECE 5550 Mobile Robotics

Lecture 3

Kinematics and Motion Models

Dr. Xian Li

Department of Electrical and Computer Engineering

Northeastern University

EECE 5550 Mobile Robotics

Today

The model your robot actually runs.

● What a model is for, and what it leaves out

● The simplest models: integrators

● The unicycle, and the constraint that defines it

● Differential drive, derived

Reading

Lynch & Park, Modern Robotics

●Ch. 13.1 to 13.3, wheeled mobile robots

Siegwart et al., Autonomous Mobile Robots

●Ch. 3, Mobile Robot Kinematics

iRobot Create 3 documentation, Odometry

Lecture 2 covered where things are. This one covers how they move.

One question, all lecture

You command 0.3 m/s forward for 4 seconds. Where are you?

1.20 mwhat you commanded

1.28 mwhat the wheels report

1.20 m ± ?where the robot actually is

Three different numbers. By the end you will know which to trust, and for what.

EECE 5550 Mobile Robotics

●Sense: Process sensor data to construct a model of the world

●Think: Construct a plan to move from the current state to the goal state

●Act: Control actuators to execute plan

Sense → Think → Act

Next: Basic models of robot motion

Recap: The Central Dogma of Robotics

EECE 5550 Mobile Robotics

Modeling

Mathematical: describing important system characteristics via equations

●For example, applying laws of physics to model a physical system

Models may involve unknown parameters, estimated from experiments

When well-established laws are not available, empirical data is used

●Input and output relationships based on data

EECE 5550 Mobile Robotics

Simplicity against accuracy

Models with higher accuracy are usually more complicated to use and analyze

●A compromise has to be made between simplicity and accuracy

●Which aspects are negligible, and which are essential for the task at hand?

All models are flawed, but some are very useful.

EECE 5550 Mobile Robotics

Robot motion

● Robots with rigid components under rotational and/or translational motion

● The position of any point on each body, in its attached frame, is fixed

EECE 5550 Mobile Robotics

Rigid motion

● Actuators, for example motors, apply force or torque to induce motion

● The position and orientation of the body frame, the pose, changes as a result

EECE 5550 Mobile Robotics

Kinematics or Dynamics

How is the pose influenced by the actuators?

● Kinematics: Geometry of motion. Ignores forces/torques.

● Dynamics: Physics of motion. Relates motion to forces/torques.

Choosing the model

Kinematic model: velocities, first-order ODEs

Dynamic model: accelerations, second-order ODEs. More accurate but more complex.

The decision depends on inertia, abrupt motion, and required precision.

Front Zoom. iRobot - Roomba 614 Robot Vacuum - Black.

da Vinci Robotic Surgery | Robotic Prostatectomy | Thomas Ahlering, M.D. | Department of Urology

Husky UGV - Outdoor Field Research Robot by Clearpath

EECE 5550 Mobile Robotics

When kinematics is not enough

A balancing robot is a counterexample.

A TurtleBot

● Stable whether or not you model it

● Stop commanding it and it stays put

● Geometry predicts where it goes

● A kinematic model is sufficient

A balancing robot

● Falls over if you do nothing

● Stop commanding it and it hits the floor

● Geometry says nothing about falling

● Needs a dynamic model, and a controller

Same wheels, same motors, same encoders. Different model, because the physics matters.

EECE 5550 Mobile Robotics

Single integrator

A point that moves in whatever direction you ask. No heading, no constraints.

X_{I}

Y_{I}

(x, y)

●State: position $(x, y)$

○Orientation does not matter since the robot is assumed to move freely in any direction

●Control Input: velocity $(v_{x}, v_{y})$

[\begin{matrix} \dot{x} \\ \dot{y} \end{matrix}] = [\begin{matrix} v_{x} \\ v_{y} \end{matrix}]

Which kind of robot does it look like? Link

EECE 5550 Mobile Robotics

●Too simplified for most robots, but very useful for high-level planning and control

○where should the robot go?

○Design the inputs for single integrators first, then add the real complexity

Single integrator

[\begin{matrix} \dot{x} \\ \dot{y} \end{matrix}] = [\begin{matrix} v_{x} \\ v_{y} \end{matrix}]

First-order, discrete-time approximation

[\begin{matrix} x (t + Δ) \\ y (t + Δ) \end{matrix}] = [\begin{matrix} x (t) \\ y (t) \end{matrix}] + Δ [\begin{matrix} v_{x} \\ v_{y} \end{matrix}]

[\begin{matrix} v_{x} \\ v_{y} \end{matrix}]

[\begin{matrix} x (t) \\ y (t) \end{matrix}]

[\begin{matrix} x (t + Δ) \\ y (t + Δ) \end{matrix}]

Sliding right is not possible, but there is a way to go there.

EECE 5550 Mobile Robotics

Double integrator

Now the input is acceleration, and velocity becomes part of the state.

X_{I}

Y_{I}

(x, y)

[\begin{matrix} \ddot{x} \\ \dot{x} \\ \ddot{y} \\ \dot{y} \end{matrix}] = [\begin{matrix} 0 & 0 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{matrix}] [\begin{matrix} \dot{x} \\ x \\ \dot{y} \\ y \end{matrix}] + [\begin{matrix} 1 & 0 \\ 0 & 0 \\ 0 & 1 \\ 0 & 0 \end{matrix}] [\begin{matrix} a_{x} \\ a_{y} \end{matrix}]

(v_{x}, v_{y})

State: position $(x, y)$ , velocity $(\dot{x}, \dot{y}) = (v_{x}, v_{y})$

Control input: acceleration $(a_{x}, a_{y}) = (\ddot{x}, \ddot{y})$

EECE 5550 Mobile Robotics

Triple integrator planning

for aggressive 3D collision avoidance

Lopez & How, “Aggressive 3-D Collision Avoidance for High-Speed Navigation”, ICRA 2017

In this video we discuss the development and experimental verification of a collision avoidance algorithm, the Triple Integrator Planner (TIP), that is nearly an order of magnitude faster than the state-of-the-art and can operate at a level of agility near the physical limits of the vehicle. The key property of TIP is its ability to generate collision-free, dynamically feasible paths within 5ms (worst case) of receiving a point cloud. The short computation time is achieved by 1) planning with instantaneous perception data, 2) using the closed-form solution to a minimum-time optimal control problem to generate motion primitives online, and 3) intelligently sampling motion primitives for collisions. Hardware experiments demonstrate the utility of the algorithm. ICRA '17 video submission.

EECE 5550 Mobile Robotics

Why this is the wrong model for TurtleBot 4

An integrator can move in any direction from rest. A TurtleBot cannot.

Single integrator

any direction, instantly

no heading in the state at all

TurtleBot 4

forward and turn only

heading is in the state, and it constrains motion

Everything from here on is about that one difference.

EECE 5550 Mobile Robotics

•A helicopter with four rotors

•Rotors are (often) placed

•in square formation

Quadrotor

EECE 5550 Mobile Robotics

•A helicopter with four rotors

•Rotors are (often) placed

•in square formation

•equal distance from the center of mass

Center of mass

Quadrotor

EECE 5550 Mobile Robotics

•A helicopter with four rotors

•Rotors are (often) placed

•in square formation

•equal distance from the center of mass

•Controlled by adjusting the angular velocity

Quadrotor

EECE 5550 Mobile Robotics

•A helicopter with four rotors

•Rotors are (often) placed

•in square formation

•equal distance from the center of mass

•Controlled by adjusting the angular velocity

Quadrotor

T_{i} = \frac{1}{2}

ρ

↑air density

C_{T}

↑rotor design

ω_{i}^{2}

↑rotor velocity

= k_{T} ω_{i}^{2}

EECE 5550 Mobile Robotics

Increase the angular velocity

Thrust generation

Quadrotor

EECE 5550 Mobile Robotics

Increase the angular velocity

Thrust generation

\ddot{h}

Quadrotor

EECE 5550 Mobile Robotics

Quadrotor

Imbalance in left/right thrust generation

EECE 5550 Mobile Robotics

\ddot{φ}

Quadrotor

Imbalance in left/right thrust generation

Roll

EECE 5550 Mobile Robotics

Quadrotor

Imbalance in front/rear thrust generation

EECE 5550 Mobile Robotics

Imbalance in front/rear thrust generation

Pitch

\ddot{θ}

Quadrotor

EECE 5550 Mobile Robotics

Imbalance in ??? generation

Quadrotor

EECE 5550 Mobile Robotics

Imbalance in angular momentum generation

Yaw

\ddot{ψ}

Quadrotor

EECE 5550 Mobile Robotics

m g

m a = \sum_{i = 1}^{4} T_{i} - m g

m \ddot{h} = k_{T} \sum_{i = 1}^{4} ω_{i}^{2} - m g

m \ddot{h} = 4 k_{T} ω^{2} - m g

h

h (0) = h_{0}

\dot{h} (0) = {\dot{h}}_{0}

I.C.

A simple model for altitude dynamics

Question: Double-integrator or Triple-integrator?

EECE 5550 Mobile Robotics

Unicycle

A simple but reasonable model for many mobile robots

Unlike the single integrator, the heading matters

●Output: pose (x, y, θ)

●Input: linear speed v and angular speed ω

X_{I}

Y_{I}

X_{B}

Y_{B}

θ

v

EECE 5550 Mobile Robotics

Unicycle

A simple but reasonable model for many mobile robots

Unlike the single integrator, the heading matters

●Output: pose (x, y, θ)

●Input: linear speed v and angular speed ω

X_{I}

Y_{I}

X_{B}

Y_{B}

θ

v

[\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{θ} \end{matrix}] = [\begin{matrix} \cos θ & 0 \\ \sin θ & 0 \\ 0 & 1 \end{matrix}] [\begin{matrix} v \\ ω \end{matrix}]

Two inputs on the right. Three state derivatives on the left.

EECE 5550 Mobile Robotics

Kinematics of wheeled vehicles

LOTS of different designs are possible for wheeled robots.

Examples:

●Bicycle model

●Differential drive, two or three wheels

●Ackermann steering, as in a car

EECE 5550 Mobile Robotics

Kinematics of wheeled vehicles

LOTS of different designs are possible for wheeled robots.

EECE 5550 Mobile Robotics

Kinematics of wheeled vehicles

LOTS of different designs are possible for wheeled robots.

Examples:

●Bicycle model

●Differential drive, two or three wheels

●Ackermann steering, as in a car

Main question: how does wheel geometry relate to robot motion (i.e. kinematics)?

EECE 5550 Mobile Robotics

Rolling without slipping

Main assumption: all wheels rotate without slipping.

Key observation

These conditions correspond to constraints on possible robot motion.

●The contact point has zero velocity relative to the ground

●Wheel rotation and robot motion are locked together

●slip_status exists because this assumption fails sometimes

EECE 5550 Mobile Robotics

Rolling without slipping

Main assumption: all wheels rotate without slipping.

Question:
Does the "no-slip" assumption apply to omniwheels?

Source: https://learn.browndoggadgets.com/Guide/Omni+Wheel+Robot/310

Forward velocity

No side-slip!

Lateral motion is prohibited

EECE 5550 Mobile Robotics

Wheel constraints on robot motion

For all wheels to roll without slipping, they must share one instantaneous center of curvature (ICC).

ICC

Differential drive, derived

1The question

2The ICC

3Two equations

4Solve

5Body velocity

6World frame

7Check it

x

y

O

θ

x_{r}

y_{r}

(x, y)

w

{\dot{φ}}_{l}

{\dot{φ}}_{r}

ICC

Δ θ

x_{r}

y_{r}

s_{l}

s

s_{r}

R

w

ICC

ω

v_{l}

v

v_{r}

R

w

x_{r}

y_{r}

v_{l}

v_{r}

v

{\dot{y}}_{r} = 0

x

y

O

v

v \cos θ

v \sin θ

θ

ICC

ω

v_{l}

v

v_{r}

R

w

You knowthe wheel speeds

{\dot{φ}}_{l}

and

{\dot{φ}}_{r}

from the encoders, the wheel radius

r

and the wheel separation

w

You wantthe world-frame rates

\dot{x}, \dot{y}, \dot{θ}

Do the geometry in the robot’s own frame, where it is simple. Rotate into the world frame only at the end.

Four assumptions

A1The robot is a rigid body on a flat floor.
A2Each wheel rolls without slipping, so a wheel spinning at $\dot{φ}$ moves forward at $r \dot{φ}$ .
A3Nothing slips sideways: a wheel cannot move along its own axle.
A4 $(x, y)$ is the midpoint of the axle, halfway between the wheels.

One point, one rate A1A3

A wheel can only roll forward or back, so it turns about a point on its axle line. Both wheels share that line and the body is rigid, so the whole robot turns about one point on it: the ICC.

Every point of a rigid body turns at the same rate $ω$ .

Arcs over a short time $Δ t$

A point at distance $ρ$ from the ICC travels $ρ Δ θ$ . The left wheel is the nearer one:

s_{l} = (R - \frac{w}{2}) Δ θ s = R Δ θ s_{r} = (R + \frac{w}{2}) Δ θ

(1)

ω = \frac{Δ θ}{Δ t}

(2)

Divide each arc by $Δ t$

Arc length per unit time is the speed along the arc:

v_{l} = ω (R - \frac{w}{2})

(3)

v_{r} = ω (R + \frac{w}{2})

(4)

Rolling without slipping A2

v_{l} = r {\dot{φ}}_{l} v_{r} = r {\dot{φ}}_{r}

(5)

Two equations, two unknowns

v_{l}

and

v_{r}

are known from the encoders.

R

and

ω

are the unknowns.

Subtract (3) from (4): the $R$ terms cancel

v_{r} - v_{l} = ω w

ω = \frac{v_{r} - v_{l}}{w}

(6)

Add (3) and (4), then use (6)

v_{r} + v_{l} = 2 ω R

R = \frac{w}{2} \frac{v_{r} + v_{l}}{v_{r} - v_{l}}

(7)

When

v_{r} = v_{l}

R

is infinite and the robot drives straight. Real code never computes

R

; it only needs

v

and

ω

The center moves at the average wheel speed A4

v = ω R = \frac{v_{r} + v_{l}}{2}

(8)

It moves along $x_{r}$ only A3

The center turns about the ICC, so it moves at right angles to the axle. It never slides sideways:

{\dot{y}}_{r} = 0

(9)

Collected, in the body frame

[\begin{matrix} {\dot{x}}_{r} \\ {\dot{y}}_{r} \\ {\dot{θ}}_{r} \end{matrix}] = [\begin{matrix} v \\ 0 \\ ω \end{matrix}] = [\begin{matrix} \frac{r}{2} ({\dot{φ}}_{r} + {\dot{φ}}_{l}) \\ 0 \\ \frac{r}{w} ({\dot{φ}}_{r} - {\dot{φ}}_{l}) \end{matrix}]

(10)

Rotate the body velocity by $θ$ , as in Lecture 2

[\begin{matrix} \dot{x} \\ \dot{y} \end{matrix}] = [\begin{matrix} \cos θ & - \sin θ \\ \sin θ & \cos θ \end{matrix}] [\begin{matrix} v \\ 0 \end{matrix}] = [\begin{matrix} v \cos θ \\ v \sin θ \end{matrix}]

The unicycle equations, now derived

\dot{x} = v \cos θ \dot{y} = v \sin θ \dot{θ} = ω

(11)

The constraint, in world coordinates

$y_{r}$ points along $(- \sin θ, \cos θ)$ in the world frame, so (9) becomes

- \dot{x} \sin θ + \dot{y} \cos θ = 0

(12)

Check it against motions you can picture

Motion	Wheels	$ω$	R	$v$
Straight line	$v_{r} = v_{l}$	$0$	$\infty$	$v_{r}$
Spin in place	$v_{r} = - v_{l}$	$2 v_{r} / w$	$0$	$0$
Pivot	$v_{l} = 0$	$v_{r} / w$	$w / 2$	$v_{r} / 2$
Left turn	$v_{r} > v_{l} > 0$	$> 0$	$> w / 2$	$> 0$
Right turn	$v_{l} > v_{r}$	$< 0$	$< 0$	any

Worked example

r = 0.036 m

w = 0.235 m

{\dot{φ}}_{l} = 4.0

{\dot{φ}}_{r} = 6.0 rad/s

v = 0.180 m/s

ω = 0.306 rad/s

R = 0.588 m

EECE 5550 Mobile Robotics

Forward kinematics, collected

Wheel angular speeds in, pose derivative out.

body frame

world frame

$r$ is the wheel radius, $w$ the wheel separation, $\dot{φ}$ the wheel angular speeds.

You measure $r$ and $w$ yourselves in Lab 1. The published values and TurtleBot 4's values differ, and that difference is the systematic part of the odometry error.

\begin{array}{r} (\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{θ} \end{matrix}) = (\begin{matrix} R (θ) & 0 \\ 0 & 1 \end{matrix}) (\begin{matrix} {\dot{x}}_{r} \\ {\dot{y}}_{r} \\ {\dot{θ}}_{r} \end{matrix}) \\ where R (θ) = (\begin{matrix} \cos θ & - \sin θ \\ \sin θ & \cos θ \end{matrix}) \end{array}

(\begin{matrix} {\dot{x}}_{r} \\ {\dot{y}}_{r} \\ {\dot{θ}}_{r} \end{matrix}) = (\begin{matrix} \frac{r}{2} ({\dot{φ}}_{r} + {\dot{φ}}_{l}) \\ 0 \\ \frac{r}{w} ({\dot{φ}}_{r} - {\dot{φ}}_{l}) \end{matrix})

EECE 5550 Mobile Robotics

Inverse kinematics

You publish v and ω. Something has to turn that into two wheel speeds.

invert the forward model

\begin{array}{rcl} v_{r} & = & v + \frac{ω w}{2} \\ v_{l} & = & v - \frac{ω w}{2} \\ {\dot{φ}}_{r} & = & \frac{v_{r}}{r} \\ {\dot{φ}}_{l} & = & \frac{v_{l}}{r} \end{array}

● This happens inside the Create 3, in firmware

● You never write it for this robot, which is why it is easy to forget it exists

● But every clamp, every saturation, and every wheel slip happens here

● And it is the first thing to check when the robot turns the wrong way

EECE 5550 Mobile Robotics

Verify on your TurtleBot

Exercise #2: find the ICC

Create 3 Actions

DriveDistance, DriveArc, and RotateAngle

If those are not listed, check the hidden namespace:

terminal

$ros2 action list --include-hidden

What to take away

The one habitBefore you trust a model, list what it assumes and check those assumptions against your robot.

1What a model is for

A model keeps what matters for the task and leaves out the rest. Kinematics is enough for a TurtleBot; a balancing robot needs dynamics and a controller.

2Integrators

A single integrator moves in any direction: $\dot{x} = v_{x}$ . Good for planning, but wrong for a robot that cannot move sideways.

3The unicycle

$\dot{x} = v \cos θ$ , $\dot{y} = v \sin θ$ , $\dot{θ} = ω$ . Two inputs, three states, and the constraint $- \dot{x} \sin θ + \dot{y} \cos θ = 0$ : no sideways motion.

4Rolling without slipping

A wheel moves forward at $r \dot{φ}$ and never along its axle. For every wheel to roll, all of them must turn about one point, the ICC.

5Differential drive, derived

$v = (v_{r} + v_{l}) / 2$ and $ω = (v_{r} - v_{l}) / w$ . Rotate the body velocity by $θ$ and the unicycle equations follow.

6Inverse kinematics

$v_{r} = v + ω w / 2$ and $v_{l} = v - ω w / 2$ run in the Create 3 firmware. Measure $r$ and $w$ yourself: errors in them are the systematic part of odometry error.

One question, all lecture

Differential drive, derived

Four assumptions

One point, one rate A1A3

Arcs over a short time Δt

Divide each arc by Δt

Rolling without slipping A2

Subtract (3) from (4): the R terms cancel

Add (3) and (4), then use (6)

The center moves at the average wheel speed A4

It moves along xr only A3

Collected, in the body frame

Rotate the body velocity by θ, as in Lecture 2

The unicycle equations, now derived

The constraint, in world coordinates

Check it against motions you can picture

What to take away

Arcs over a short time $Δ t$

Divide each arc by $Δ t$

Subtract (3) from (4): the $R$ terms cancel

It moves along $x_{r}$ only A3

Rotate the body velocity by $θ$ , as in Lecture 2