TECHIN 517 - ROBOTICS LAB II

ROBOT NAVIGATION

WEEK 3 | WEDNESDAY | APR 13, 2022

ASSINGMENT #3

• Post #1: Labs 11-15, visualizations with Markers and InteractiveMarkers in RViz
• Post #2: Labs 16-17, mapping, localization, navigation
• Post #3: Lab 18, interactive navigation

ROBOT NAVIGATION

Getting from point A to point B

How do I get to point B?

Where am I?

LOCALICATION

NAVIGATION

ROBOT NAVIGATION

• Move towards B
• Check where you are

How do I get to point B?

Where am I?

ROBOT NAVIGATION

Relative to:

• A perceived entity
• Where I started
• A map

How do I get to point B?

Where am I?

ROBOT: WHERE AM I?

• Relative to sensed entity using external sensors:
• Servoing: Continuously try to change/maintain sensor reading

ROBOT: WHERE AM I?

• Relative to where I started:
• Based on commands sent to the wheels -> Dead reckoning
• Based on movement sensors -> Odometry
• Wheel encoders + wheel contact switches
• IMU (Inertial Measurement Unit) / Gyro
• Example method: Combine the two: Extended Kalman filtering

ROBOT: WHERE AM I?

• Relative to a map:
• Match sensor readings to a map
• Map can be formed from readings from previously visited known locations on a map
• Example method: Adaptive Monte Carlo Localization (AMCL): sample candidates, imagine what the sensors should read given the map + sensor model, see if it matches.

ROBOT NAVIGATION

• Move towards B
• Check where you are

How do I get to point B?

Where am I?

COORDINATE TRANSFORMS

DIFFERENTIAL DRIVE ROBOTS

two independent drive wheels mounted on a common axis

STEERING

Differential drive steering is controlled by setting the relative speeds of the wheels

simple cases

complex cases

d_L=d_R

-d_L =-d_R

d_L=-d_R

-d_L=d_R

d_L<d_R

d_L>d_R

-d_L<-d_R

-d_L>-d_R

DIFFERENTIAL DRIVE

b

r₂

x₁-x₀

y₁-y₀

d

ϕ

x= t (x₁-x₀)

˙

˙

y= t (y₁-y₀)

𝜑_{L =} t𝜑_L

˙

𝜑_{R =} t𝜑_R

˙

EXTRA SLIDES

COORDINATE TRANSFORMS

COORDINATE FRAMES

y

x’

COORDINATE FRAMES

y

{A}

{B}

x’

COORDINATE FRAMES

y

{A}

{B}

^BP

x’

^AP

COORDINATE FRAMES

(0,0)

y

{A}

x

COORDINATE TRANSFORMS

y

{A}

{B}

^BP

x’

What is ^AP ?

COORDINATE TRANSFORMS

y

{A}

{B}

x’

Start with ^BP

Translate to (x₀, y₀)

Rotate by 𝛩₀

What is ^AP ?

^AP

TRANSLATIONS

x

y

{A}

^AP =

x_{0 +} x₁

y_{0 +} y₁

(x₀, y₀)

x₀

x₁

{B}

ROTATIONS

x

y

{A}

𝛩₀

x

{B}

ROTATIONS

x

y

^AP =

x₂

y₂

x

x₁

y₁

y₂

x₂

{B}

{A}

ROTATIONS

x

y

^AP =

x₂

y₂

x

x₁

y₁

y₂

x₂

{B}

{A}

x₁ = ^AP. ^Au_Bx

y₁ = ^AP. ^Au_By

ROTATIONS

x

y

^AP =

x₂

y₂

x

x₁

y₁

y₂

x₂

{B}

{A}

x₁ = x₂ cos(𝛩₀) + y₂ sin(𝛩₀)

y₁ = -x₂ sin(𝛩₀) + y₂ cos(𝛩₀)

ROTATIONS

x

y

^AP =

x₂

y₂

x

x₁

y₁

y₂

x₂

{B}

{A}

x₂ = x₁ cos(𝛩₀) - y₁ sin(𝛩₀)

y₂ = x₁ sin(𝛩₀) + y₁ cos(𝛩₀)

x₁ = x₂ cos(𝛩₀) + y₂ sin(𝛩₀)

y₁ = -x₂ sin(𝛩₀) + y₂ cos(𝛩₀)

revert

ROTATIONS

x

y

x

x₁

y₁

y₂

x₂

{B}

{A}

ROTATIONS AS MATRIX MULTIPLICATIONS

x

y

x

x₁

y₁

y₂

x₂

{B}

{A}

^BP

^AR_B

TRANSLATIONS AS MATRIX MULTIPLICATIONS?

x

y

{A}

^AP =

x_{0 +} x₁

y_{0 +} y₁

(x₀, y₀)

x₀

x₁

{B}

a, b

c, d

^BP

TRANSLATIONS AS MATRIX MULTIPLICATIONS?

x

y

{A}

^AP =

x_{0 +} x₁

y_{0 +} y₁

(x₀, y₀)

x₀

x₁

{B}

a, b

c, d

^BP

Does not

work!

TRANSLATIONS AS MATRIX MULTIPLICATIONS?

x

y

{A}

^AP’ =

x_{0 +} x₁

y_{0 +} y₁

1

(x₀, y₀)

x₀

x₁

{B}

1, 0, x₀

0, 1, y₀

0, 0, 1

Instead

homogeneous coordinates

COMBINING ROTATIONS AND TRANSLATIONS

y

{A}

{B}

x’

^AP

^AP’ =

1, 0, x₀

0, 1, y₀

0, 0, 1

^AR_B

replace with

homogeneous transformation

^AT_B

HOMOGENOUS TRANSFORMS

COMPOSING TRANSFORMS

y

{A}

{B}

x’

^AP’ =

^AT_B

y’’

x’’

𝛩₁

{C}

^BT_C

COMPOSING TRANSFORMS

y

{world}

{robot}

x’

y’’

x’’

𝛩₂

{cube}

𝛩₁

(x₁, y₁)

DIFFERENTIAL DRIVE

KINEMATICS

DIFFERENTIAL DRIVE ROBOTS

two independent drive wheels mounted on a common axis

STEERING

Differential drive steering is controlled by setting the relative speeds of the wheels

simple cases

complex cases

d_L=d_R

-d_L =-d_R

d_L=-d_R

-d_L=d_R

d_L<d_R

d_L>d_R

-d_L<-d_R

-d_L>-d_R

Steering

• Differential drive steering is controlled by setting the relative speeds of the wheels

DIFFERENTIAL DRIVE

y

x’

robot pose

target pose

DIFFERENTIAL DRIVE

1. Rotate in place until we face the target
2. Drive to the target
3. Rotate in place to the desired heading

x’

robot pose

target pose

APPROACH 1

Issues?

Might overshoot

Not most efficient

Will not work for moving target

DIFFERENTIAL DRIVE

1. Rotate in place until we face the target
2. Drive to the target
3. Rotate in place to the desired heading

x’

robot pose

target pose

APPROACH 1

Take into account

distance to go?

DIFFERENTIAL DRIVE

Rotate and translate at the same time to make the robot go towards target pose

x’

robot pose

target pose

APPROACH 2

How does

change as a function of

?

DIFFERENTIAL DRIVE

x’

robot pose

f(

˙

˙

˙

˙

=

, b, r)

b

2r

DIFFERENTIAL DRIVE

f(

˙

˙

˙

˙

=

, b, r)

b

2r

d_L

d_R

ϕ

instant center of turning

DIFFERENTIAL DRIVE

b

2r

d_L=r𝜑_L

𝜑_L

d_L

r

robot wheel

d_R=r𝜑_R

ϕ

DIFFERENTIAL DRIVE

b

r₂

r₁

d_L=r₁ϕ

d_R=r₂ϕ

r₁=b+r₂

d_L=r𝜑_L

d_R=r𝜑_R

ϕ

DIFFERENTIAL DRIVE

b

2r

ϕ

𝛩₀

𝛩₁

𝛩₁=𝛩₀-ϕ

DIFFERENTIAL DRIVE

b

r₂

x₁-x₀

y₁-y₀

d

ϕ

d

0.5b+r₂

0.5b+r₂

ϕ

DIFFERENTIAL DRIVE

b

r₂

x₁-x₀

y₁-y₀

d

cos( )

ϕ

^𝜑L-𝜑R

sin( )

^𝜑L-𝜑R

Assume small

ϕ

DIFFERENTIAL DRIVE

b

r₂

x₁-x₀

y₁-y₀

d

cos( )

ϕ

^𝜑L-𝜑R

DIFFERENTIAL DRIVE

b

r₂

x₁-x₀

y₁-y₀

d

ϕ

x= t (x₁-x₀)

˙

˙

y= t (y₁-y₀)

𝜑_{L =} t𝜑_L

˙

𝜑_{R =} t𝜑_R

˙

DIFFERENTIAL DRIVE ROBOT KINEMATICS

b

Odometry Calculations

Odometry Calculations

DIFFERENTIAL DRIVE

b

d_L=r𝜑_L

d_R=r𝜑_R

r₂

r₁

d_L=r₁ϕ

d_R=r₂ϕ

r₁=b+r₂

ϕ

Odometry Calculations

Odometry Calculations

Differential Drive Robot Kinematics

Differential Drive Robot Kinematics

Odometry Calculations

Differential Drive

Differential Drive

Coordinate Transformation

How can we steer to a desired relative location?

Problems with this approach?

​

• Using max velocities is likely to make the robot overshoot
• Will not work well if the target is moving

Differential Drive Robot Kinematics

How can we steer to a desired relative location?

Note that this example assumes the velocity is bounded somewhere else in the software, such that you can send very high velocity values and they will always be bounded by maxVel. If this is not the case, an extra check can be added.

​

Time can also be incorporated into the formula if the target must be reached within some set period.

How can we steer to a desired relative location?

DIFFERENTIAL DRIVE

y

x’

robot pose

b

2r

COMPARISON OF DRIVING STRATEGIES

• Approach 1 will result in the shortest path, but do not perform well in practice if the target location moves
• mostly because rotating in place requires the robot to stop and reverse one of its motors, and doing this over and over adds time and slip error
• Using Approach 2 the robot travels along an arc, and is more robust to changes in the target location
• It is also possible to blend these techniques by turning in place when the angle offset is very large (e.g., the target is behind the robot), and then switching to the continuous mode

ASSUMPTIONS FOR ALL OF THE ABOVE

• Movement is only in the horizontal plane
• Single point of contact with ground
• No deformation of wheels
• Motion is purely by rolling (no slippage)
• Steering axis is normal to ground
• No rolling friction