1 of 60

ME5751�Robotics Motion Planning

Yizhe Chang chang@cpp.edu

Lecture Note Set #2

Partial lecture slide from

C. Clark�Harvey Mudd College, Claremont, CA�D. J. Cappelleri, M. Salman

Stevens I.T., Hoboken, NJ

2 of 60

Outline

  • Homogeneous Transformation
  • Differential driving: velocity
    • From local coordinate to global coordinate
    • Velocity forward kinematics
  • Wheel odometry
    • From wheel displacement to pose change
    • From local coordinate to global coordinate

3 of 60

Last Lecture

  • Rotation matrix in 2D
  • Rotation matrix in 3D
    • 3 basic rotation matrices
    • Euler angles (φ, θ, ψ)

​

​

4 of 60

How about translation?

​

​

​

P0

5 of 60

Homogeneous transformation

​

​

​

6 of 60

Homogeneous transformation

​

​

​

7 of 60

Homogeneous transformation

​

​

​

8 of 60

Translation first or rotation first?

​

​

​

​

  • Do we rotate first then translate,

or

  • Do we translate first then rotate?

x0

y0

z0

x1

y1

z1

x2

y2

z2

x0

y0

z0

x1

y1

z1

x2

y2

z2

9 of 60

Translation first or rotation first?

  •  

x0

y0

z0

x1

y1

z1

x2

y2

z2

10 of 60

Translation first or rotation first?

  •  

x0

y0

z0

x1

y1

z1

x2

y2

z2

11 of 60

Translation first or rotation first?

​

​

​

  • We first translate the coordinate by d, then rotate by R

12 of 60

Homogeneous transformation practice

  • Find H that represents the following in order

​

  • Rotation by angle α about current x-axis
  • Translation of b units along current x-axis
  • Translation of d units along current z-axis
  • Rotation by angle θ about current z-axis

​

​

​

​

13 of 60

Rotation example

  • Find H that represents the following in order

​

​

​

​

​

14 of 60

Coordinate system on bot

zc

yc

xc

xR

yR

zR

xR

yR

zc

yc

xI

yI

P

15 of 60

Coordinate system on bot

  • If we “see” a point at sensor frame at position (in the sensor frame) at (lxc, lyc, lzc), say (0,0,1)
  • What is the point’s position in the global frame?

zc

yc

xc

xR

yR

zR

xR

yR

zc

yc

xI

yI

P

16 of 60

Coordinate system

  •  

xR

yR

zc

yc

xI

yI

P

17 of 60

Coordinate system

  •  

zc

yc

xc

xR

yR

zR

xc'

yc'

zc'

18 of 60

Coordinate system

  •  

zc

yc

xc

xR

yR

zR

xc'

yc'

zc'

19 of 60

Final H transformation

  •  

20 of 60

Mini-summary

  • Homogeneous transformation
    • Homogeneous transformation reversion is not simple matrix inversion!
    • For a Matrix,����we first translate the coordinate by d, then rotate by R

​

​

​

​

21 of 60

Continuous Representations

  • Homogeneous Transformation
  • Differential driving: velocity
    • From local coordinate to global coordinate
    • Velocity forward kinematics
  • Wheel odometry
    • From wheel displacement to pose change
    • From local coordinate to global coordinate

​

22 of 60

Velocity: Local to global

​

  • Question: What is the relationship between velocity in local coordinate and global coordinate?

XI

YI

23 of 60

Review: when we detect something

  • When we detect an object as shown below, assume the robot coordinate frame is derived from rotating θ

​

24 of 60

How about velocity?

  • When we detect an object as shown below, assume the robot coordinate frame is derived from rotating θ

​

25 of 60

Velocity transformation

  • Let’s obtain the transformation matrix, starting with XI direction

 

26 of 60

Velocity transformation

  • Now the YI direction:

 

27 of 60

Velocity transformation

  • What about rotational velocity?

28 of 60

Velocity transformation

  • What about rotational velocity?

 

 

 

 

What is this?

29 of 60

Velocity transformation

  •  

 

30 of 60

Continuous Representations

  • Differential driving: velocity
    • From local coordinate to global coordinate
    • From wheel speed to /cmd_vel (Forward kinematics)
  • Wheel odometry
    • From wheel displacement to pose change
    • From local coordinate to global coordinate

​

31 of 60

Forward Kinematics

  •  

 

 

32 of 60

Forward Kinematics

  • Angular velocity and linear velocity

33 of 60

Forward Kinematics

  • If we consider left wheel to be locked. The relationship between linear velocity v1 and angular velocity ω1 is:

34 of 60

Forward Kinematics

  • If we consider left wheel to be locked. The relationship between linear and angular velocity is:

 

35 of 60

Forward Kinematics

  • If we consider right wheel to be locked:

 

36 of 60

Forward Kinematics

 

 

37 of 60

Forward Kinematics: local to global

 

38 of 60

Mini-Conclusion

  • We constructed relationship between wheel speed to:�linear velocity and angular velocity of the vehicle

​

​

​

​

  • We build velocity relationship between local frame and global frame

 

 

 

We will use it someday!

39 of 60

Outline

  • Differential driving: velocity
    • From local coordinate to global coordinate
  • Wheel odometry
    • From wheel displacement to pose change
    • From local coordinate to global coordinate

​

40 of 60

Incremental encoder

  •  

41 of 60

Incremental encoder

  • In a small period of time Δt, If a robot starts from a pose p, and the right and left wheels move respective distances Δsr and Δsl , what is the resulting new pose p’ ?

42 of 60

Incremental decoder

In short Δt, our robot follow a circular arc

Question #1: Relationship between (R, Δs, Δθ) and (L, Δsr , Δsl)

43 of 60

Some basic equations

  •  

44 of 60

Some equations

  •  

45 of 60

Some equations

  •  

46 of 60

Some equations

  •  

47 of 60

Mini Summary

  •  

48 of 60

Outline

  • Differential driving: velocity
    • From local coordinate to global coordinate
  • Wheel odometry
    • From wheel displacement to pose change
    • From local coordinate to global coordinate
  • C-Space

​

​

49 of 60

Local to global

  • We know we are at pose p(px, py, θ) in XIYI frame, what is the pose of p’ in XIYI frame?

​

  • Transformation from XIYI to XRYR
  • Coordinate of P’ in XRYR frame

YI

XI

XR

YR

P

P’

θ

50 of 60

Local to global

  •  

 

51 of 60

Alternate segment theorem

  •  

Alternate segment theorem

P’

52 of 60

Coordinate of P’ in XRYR frame

  •  

P’

53 of 60

Transformation from XIYI to XRYR

  •  

 

 

54 of 60

Local to global

  • After small period of time Δt, the new p’ pose is:

 

 

 

 

55 of 60

Mini-summary

  • In odometry chapter, we constantly update the pose of the robot p’ at time interval Δt

 

 

 

 

56 of 60

Summary Discussion

  •  

 

 

57 of 60

Summary on Odometry

  •  

58 of 60

Summary on Odometry

  • For odometry
  • What assumption we made?
  • How this assumption may affect the accuracy of the result?
  • What else factor may affect the accuracy of odometry?

59 of 60

Practice

  • Our odometer measured:

Gap between time sequence: 1s, L=0.1m

unit in m,

​

​

​

​

​

Where is our robot?

Anything to comment about this practice? Is our result accurate?

Time sequence

delta SR

delta SL

1s

0.1

0.15

2s

0.2

0.2

3s

0.1

0.2

4s

0.3

0.2

60 of 60

Practice

delta SR

delta SL

ds

dtheta

theta

px

py

px'

py'

0.10

0.15

0.13

-0.25

0.00

0.00

0.00

0.12

-0.02

0.20

0.20

0.20

0.00

-0.25

0.12

-0.02

0.32

0.07

0.15

0.20

0.18

-0.25

-0.25

0.32

0.07

0.48

0.25

0.30

0.20

0.25

0.50

-0.50

0.48

0.25

0.72

0.42

  • Our odometer measured:

Gap between time sequence: 1s, L=0.1m

unit in m

​

​

​

​

​

Where is our robot?

Anything to comment about this practice? Is our result accurate?