KINEMATICS, DYNAMICS AND

LOW-LEVEL CONTROL OF ROBOTS

Kinematics, Dynamics and low-level control of robots MRGCV

The problem of mobility
• How to move a robot from A to B
• Robot design
• Environment

• Divide the problem in layers

• Low level (actuator) control
• Trajectory tracker & Motion Controller
• Path planning

• Different levels of abstraction

• Different frequency of execution

Kinematics, Dynamics and low-level control of robots MRGCV 2

Kinematics and Dynamics
• Study of how mechanical systems behave

• Kinematics associated to velocities

• Dynamics associated to accelerations (forces)

• Representing the robot

• Degrees of freedom (state), 𝐱𝐱
• Actuators, 𝐮𝐮
• Motion constraints
• Differential equations that relate all of them

𝐱𝐱̇ = 𝑓𝑓 𝐱𝐱, 𝐮𝐮
𝐱𝐱̈ = 𝑔𝑔 𝐱𝐱, 𝐱𝐱,̇ 𝐮𝐮

Kinematics, Dynamics and low-level control of robots MRGCV 3

Unicycle robot
• State of the robot 𝐱𝐱 = 𝑥𝑥, 𝑦𝑦, 𝜃𝜃
• Actuators 𝐮𝐮 = 𝑣𝑣, 𝜔𝜔
• Kinematic model (general)

𝑥𝑥̇ cos 𝜃𝜃 0 𝑣𝑣
𝑦𝑦̇ = sin 𝜃𝜃 0 𝜔𝜔
𝜃𝜃̇ 0 1

x y
k k d
ωr

θk r
v

Differential-drive Omnidirectional Tricycle / Car

Kinematics, Dynamics and low-level control of robots MRGCV 4

Unicycle robot: Low level control for differential drive
• Two wheels (left and right) that can move at different velocities
• Actuators 𝐮𝐮 = 𝜔𝜔𝑟𝑟 , 𝜔𝜔ℓ

• Forward model: From wheel velocities to linear and angular

𝑣𝑣𝑑𝑑 +𝑣𝑣ℓ 𝑟𝑟 𝜔𝜔𝑟𝑟 +𝜔𝜔ℓ

𝑣𝑣 = =
2 2
𝑣𝑣𝑑𝑑 −𝑣𝑣ℓ 𝑟𝑟 𝜔𝜔𝑟𝑟 −𝜔𝜔ℓ
𝜔𝜔 = =
𝐿𝐿 𝐿𝐿

• Wheel radius,𝑟𝑟
• Distance between wheels, 𝐿𝐿

𝜔𝜔𝑟𝑟 (𝑡𝑡) 1⁄ 𝐿𝐿⁄ 𝑣𝑣(𝑡𝑡)

𝑟𝑟 2𝑟𝑟
• Inverse model = 1⁄
𝜔𝜔ℓ (𝑡𝑡) 𝑟𝑟
−𝐿𝐿⁄
2𝑟𝑟 𝜔𝜔(𝑡𝑡)

Kinematics, Dynamics and low-level control of robots MRGCV 5

Unicycle robot: Low level control for synchronized steer
• Three wheels (6 actuators)
• Assume synchronization of the three wheels
• Actuators 𝐮𝐮 = 𝜔𝜔𝑟𝑟 , 𝜃𝜃

• Forward model: From wheel velocities to linear and angular

𝑣𝑣 = 𝑟𝑟𝜔𝜔𝑟𝑟
𝜔𝜔 = 𝜃𝜃̇ ωr 𝜃𝜃

• Wheel radius, 𝑟𝑟
• Wheel orientation around the z-axis, 𝜃𝜃
𝑣𝑣
• Inverse model: 𝜔𝜔𝑟𝑟 = , 𝜃𝜃 = ∫ 𝜔𝜔
𝑟𝑟

Kinematics, Dynamics and low-level control of robots MRGCV 6

Unicycle robot: Low level control for tricycle
• Two motor wheels and one steer wheel α
• Actuators 𝐮𝐮 = 𝜔𝜔𝑟𝑟 , 𝛼𝛼
ωr

• Forward model x y
k k d

𝑣𝑣 = 𝑟𝑟𝜔𝜔𝑟𝑟 ωr

𝑣𝑣 𝑟𝑟𝜔𝜔 sin 𝛼𝛼 r
𝜔𝜔 = = 𝑟𝑟 θk v
𝐷𝐷 𝑑𝑑

• Wheel radius, 𝑟𝑟 v
• Distance between the motor and steer wheels, 𝑑𝑑 α
D ω
𝑣𝑣 𝑑𝑑𝑑𝑑
• Inverse model: 𝜔𝜔𝑟𝑟 = , 𝛼𝛼 = asin ICC
α ω d
𝑟𝑟 𝑣𝑣
vc
R

Kinematics, Dynamics and low-level control of robots MRGCV 7

Unicycle robot: Low level control for car (Ackermann)
• Two motor wheels and two steer wheels 𝛼𝛼1 𝛼𝛼 𝛼𝛼2
• Actuators 𝐮𝐮 = 𝜔𝜔𝑟𝑟 , 𝛼𝛼1 , 𝛼𝛼2

• Solved by means of the equivalent tricycle model 𝑑𝑑

𝛼𝛼1
𝑑𝑑 𝛼𝛼2
𝛼𝛼 = atan ℓ
𝑅𝑅 𝑅𝑅1
𝑅𝑅2

𝑑𝑑 ℓ 𝑑𝑑 ℓ
𝑅𝑅 = + = −
tan 𝛼𝛼1 2 tan 𝛼𝛼2 2

𝑣𝑣 𝑑𝑑𝑑𝑑
• Inverse model: 𝜔𝜔𝑟𝑟 = , 𝛼𝛼 = asin
𝑟𝑟 𝑣𝑣

Kinematics, Dynamics and low-level control of robots MRGCV 8

Unicycle robot: Motion control
• Not every motion is possible

• Concatenation of feasible motions

• Turn – Move forward – Turn

• Generate a feasible circular trajectory

𝑥𝑥 2 +𝑦𝑦 2
• 𝑅𝑅 = , 𝜃𝜃 = atan 2𝑥𝑥𝑥𝑥, 𝑥𝑥 2 − 𝑦𝑦 2
2𝑦𝑦
• Fix one velocity and compute the other from 𝑣𝑣 = 𝑅𝑅𝑅𝑅

y
x x
q1

(x,y) q
q2

Turn – Move forward – Turn Circular trajectory

Kinematics, Dynamics and low-level control of robots MRGCV 9
Unicycle robot: Motion control (LAB 2)
• Feedback control
• Transform kinematic equations to polar coordinates

𝜌𝜌̇ −cos 𝛼𝛼 0
sin 𝛼𝛼⁄ −1 𝑣𝑣
𝛼𝛼̇ = 𝜌𝜌
𝜔𝜔
𝛽𝛽̇ −sin 𝛼𝛼⁄
𝜌𝜌 0

• Distance to objective, 𝜌𝜌 = 𝑥𝑥 2 + 𝑦𝑦 2
• Bearing angle, 𝛼𝛼 = −𝜃𝜃 + atan2 𝑦𝑦, 𝑥𝑥
• Relative orientation, 𝛽𝛽 = −𝜃𝜃 − 𝛼𝛼

• Controller to go to the origin

𝑣𝑣 = 𝑘𝑘𝜌𝜌 𝜌𝜌, 𝜔𝜔 = 𝑘𝑘𝛼𝛼 𝛼𝛼 + 𝑘𝑘𝛽𝛽 𝛽𝛽
𝑘𝑘𝜌𝜌 > 0, 𝑘𝑘𝛽𝛽 < 0 and 𝑘𝑘𝛼𝛼 − 𝑘𝑘𝜌𝜌 > 0
Kinematics, Dynamics and low-level control of robots MRGCV 10
Car dynamics
• Unicycle is a simplification (kinematic)
• State of the robot 𝐱𝐱 = 𝑥𝑥, 𝑦𝑦, 𝜃𝜃, 𝑥𝑥,̇ 𝑦𝑦,̇ 𝜃𝜃̇ = 𝑥𝑥, 𝑦𝑦, 𝜃𝜃, 𝑣𝑣𝑥𝑥 , 𝑣𝑣𝑦𝑦 , 𝑤𝑤𝜃𝜃
• Actuators 𝐮𝐮 = 𝛼𝛼, 𝑠𝑠r𝑥𝑥 , 𝑠𝑠f𝑥𝑥
• Dynamics
𝑚𝑚𝑣𝑣̇𝑥𝑥 = 𝐹𝐹f𝑥𝑥 cos 𝛼𝛼 − 𝐹𝐹f𝑦𝑦 sin 𝛼𝛼 + 𝐹𝐹r𝑥𝑥 + 𝑚𝑚𝑣𝑣𝑦𝑦 𝑤𝑤𝜃𝜃
𝑚𝑚𝑣𝑣̇𝑦𝑦 = 𝐹𝐹f𝑥𝑥 sin 𝛼𝛼 + 𝐹𝐹f𝑦𝑦 cos 𝛼𝛼 + 𝐹𝐹r𝑦𝑦 − 𝑚𝑚𝑣𝑣𝑥𝑥 𝑤𝑤𝜃𝜃
𝐼𝐼𝑧𝑧 𝑤𝑤̇ 𝜃𝜃 = ℓf 𝐹𝐹f𝑥𝑥 sin 𝛼𝛼 + 𝐹𝐹f𝑦𝑦 cos 𝛼𝛼 − ℓr 𝐹𝐹r𝑦𝑦

• Forces depend on many variables 𝛼𝛼

𝜃𝜃

Image extracted from [A]

Kinematics, Dynamics and low-level control of robots MRGCV 11

Car dynamics: low level control
• Differential flatness
• Define artificial variables 𝐲𝐲 such that
𝐱𝐱 = 𝐱𝐱 𝐲𝐲, 𝐲𝐲,̇ 𝐲𝐲,̈ … and 𝐮𝐮 = 𝐮𝐮 𝐲𝐲, 𝐲𝐲,̇ 𝐲𝐲,̈ …

• Controlling the flat outputs is much easier than controlling

the original system

• For the car, the flat output is 𝐩𝐩co

• Goal: determine desired trajectories
𝛼𝛼
𝐩𝐩co , 𝐩𝐩̇ co , 𝐩𝐩̈ co
𝜃𝜃
• Useful for path planning

Image extracted from [A]

Kinematics, Dynamics and low-level control of robots MRGCV 12

Quadrotor
• Four parallel rotors attached to a frame
• Equally spaced (angular)
• Two rotate clockwise
• Two rotate anti-clockwise

• Controlling the rotating speed, we 𝑟𝑟𝑖𝑖

can control the whole rigid body Image extracted from [B]
• Position, orientation and velocities

• State
𝑇𝑇
• Position 𝐪𝐪 = 𝑥𝑥, 𝑦𝑦, 𝑧𝑧, 𝜙𝜙, 𝜃𝜃, 𝜓𝜓
• Velocity 𝐪𝐪̇

• Actuators 𝐮𝐮 = 𝜔𝜔1 , 𝜔𝜔2 , 𝜔𝜔3 , 𝜔𝜔4

Kinematics, Dynamics and low-level control of robots MRGCV 13

Very very simple fluid dynamics
• Motion inside a fluid generates
• Lift force (thrust)
• Drag force
• Moments

• Proportional to the square of the velocity 𝑟𝑟𝑖𝑖

• Lift force 𝑇𝑇𝑖𝑖 ≃ 𝑐𝑐𝑇𝑇 𝜔𝜔𝑖𝑖2 Image extracted from [B]

• Drag (rotational) force 𝑄𝑄𝑖𝑖 ≃ 𝑐𝑐𝑄𝑄 𝜔𝜔𝑖𝑖2
• Moments 𝑀𝑀𝑖𝑖 ≃ 𝑟𝑟𝑖𝑖 × 𝑇𝑇𝑖𝑖

• 𝑐𝑐𝑇𝑇 and 𝑐𝑐𝑄𝑄 depend on rotor disk area and radius, air density and
rotor profile (constant coefficients previously estimated)

Kinematics, Dynamics and low-level control of robots MRGCV 14

Quadrotor hovering (simplified)
• Assume body and world frame are aligned
• Necessary velocities to hover
• Resultant force (Only local z axis)
• 𝐹𝐹 = 𝑇𝑇1 + 𝑇𝑇2 + 𝑇𝑇3 + 𝑇𝑇4 − 𝑚𝑚𝑚𝑚
• Hover: Each rotor supports ¼ of mass
𝑟𝑟𝑖𝑖

• Resultant moment (3 dimensions)

• 𝝉𝝉 = 𝑀𝑀1 + 𝑀𝑀2 + 𝑀𝑀3 + 𝑀𝑀4 + 𝑄𝑄1 + 𝑄𝑄2 + 𝑄𝑄3 + 𝑄𝑄4
• Hover: The three equal to zero
• Solve linear system to desired force and moments
𝑚𝑚𝑚𝑚 𝐹𝐹 𝑐𝑐𝑇𝑇 𝑐𝑐𝑇𝑇 𝑐𝑐𝑇𝑇 𝑐𝑐𝑇𝑇 𝜔𝜔12
0 𝜏𝜏𝑥𝑥 0 𝑑𝑑𝑐𝑐𝑇𝑇 0 −𝑑𝑑𝑐𝑐𝑇𝑇 𝜔𝜔22
0 = 𝜏𝜏𝑦𝑦 = −𝑑𝑑𝑐𝑐𝑇𝑇 0 𝑑𝑑𝑐𝑐𝑇𝑇 0 𝜔𝜔32
0 𝜏𝜏𝑧𝑧 −𝑐𝑐𝑄𝑄 𝑐𝑐𝑄𝑄 −𝑐𝑐𝑄𝑄 𝑐𝑐𝑄𝑄
𝜔𝜔42
Kinematics, Dynamics and low-level control of robots MRGCV 15
Quadrotor dynamics
• Actuators
• Four rotors: 𝜔𝜔1 , 𝜔𝜔2 , 𝜔𝜔3 , 𝜔𝜔4
• Produce 𝐹𝐹 and 𝛕𝛕

𝑑𝑑 𝑇𝑇2 − 𝑇𝑇4
𝝉𝝉 = 𝑑𝑑(𝑇𝑇3 − 𝑇𝑇1 )
−𝑀𝑀1 + 𝑀𝑀2 − 𝑀𝑀3 + 𝑀𝑀4 Image extracted from [B]

• State Quad dynamics:

• Pose 𝐪𝐪 = 𝑥𝑥, 𝑦𝑦, 𝑧𝑧, 𝜙𝜙, 𝜃𝜃, 𝜓𝜓 𝑇𝑇
𝐩𝐩̇ = 𝐯𝐯
• System state 𝐱𝐱 = 𝐪𝐪𝑇𝑇 𝐪𝐪̇ 𝑇𝑇 𝑇𝑇
𝑚𝑚𝐯𝐯̇ = −𝑚𝑚𝑚𝑚𝐚𝐚3 + 𝐑𝐑 0,0, 𝐹𝐹 𝑇𝑇

𝐑𝐑̇ = 𝐑𝐑𝛀𝛀∧
𝐉𝐉𝛀𝛀̇ = 𝛕𝛕 − 𝛀𝛀 × 𝐉𝐉𝐉𝐉

Kinematics, Dynamics and low-level control of robots MRGCV 16

Quadrotor dynamics
• Position and velocity are expressed
in frame {A}
• Orientation is in frame {B}

• Relation with Euler Z-X-Y angles

(roll, pitch, yaw) = 𝜙𝜙, 𝜃𝜃, 𝜓𝜓
Image extracted from [B]

𝐑𝐑 = Rot 𝐳𝐳, 𝜓𝜓 Rot 𝐱𝐱, 𝜙𝜙 Rot 𝐲𝐲, 𝜃𝜃 =

𝑐𝑐𝜓𝜓𝜓𝜓𝜓𝜓 − 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 + 𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑐𝑐𝜃𝜃

𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 + 𝑐𝑐𝜓𝜓𝜓𝜓𝜓𝜓𝜓𝜓𝜓𝜓 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑠𝑠𝜓𝜓𝜓𝜓𝜓𝜓 − 𝑐𝑐𝜓𝜓𝜓𝜓𝜓𝜓𝑐𝑐𝜃𝜃
−𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐 𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐

Kinematics, Dynamics and low-level control of robots MRGCV 17

Quadrotor control
• By increasing/decreasing the desired force the quadrotor can go
up/down
• 𝑇𝑇Σ > 𝑚𝑚𝑚𝑚 ⇒ Quadrotor goes up
• 𝑇𝑇Σ < 𝑚𝑚𝑚𝑚 ⇒ Quadrotor goes down

• By rotating (roll and pitch) the body frame the quadrotor moves
on the x-y plane
• Rotation is achieved by attitude control through the
moments

• There are additional effects (neglected here)

• Blade flapping
• Induced drag

Kinematics, Dynamics and low-level control of robots MRGCV 18

Quadrotor control
• Given a desired (smooth trajectory) 𝐩𝐩∗ 𝑡𝑡 , 𝐑𝐑∗ 𝑡𝑡
• Find actions to track it
• Underactuated problem
• Four inputs: 𝐹𝐹 and 𝛕𝛕 (or associated velocities 𝜔𝜔)
• Six states: positions and orientation
• Hierarchical control

Kinematics, Dynamics and low-level control of robots MRGCV 19

 Quadrotor control
• Hierarchical control

𝐩𝐩, 𝐯𝐯

𝐩𝐩∗ Position 𝑢𝑢1 = 𝐹𝐹

Controller 𝜙𝜙 ∗ , 𝜃𝜃 ∗ Motor
Trajectory Controller
Planner
Attitude
𝜓𝜓 ∗ Controller 𝐮𝐮𝟐𝟐 = 𝝉𝝉
𝐑𝐑, 𝛀𝛀

Kinematics, Dynamics and low-level control of robots MRGCV 20

PID (Quick review)
• Given a reference (desired value) 𝑅𝑅
• Make the real value, 𝑌𝑌, reach 𝑅𝑅
• Signal error, 𝐸𝐸 = 𝑅𝑅 − 𝑌𝑌
• Action

𝑈𝑈 = 𝐾𝐾𝑃𝑃 𝐸𝐸 + 𝐾𝐾𝐼𝐼 � 𝐸𝐸 + 𝐾𝐾𝐷𝐷 𝐸𝐸̇

𝐼𝐼 𝑘𝑘 = 𝐼𝐼 𝑘𝑘 + 𝐸𝐸 𝑘𝑘
�
𝑈𝑈 𝑘𝑘 = 𝐾𝐾𝑝𝑝 𝐸𝐸 𝑘𝑘 + 𝐾𝐾𝐼𝐼 𝐼𝐼 𝑘𝑘 + 𝐾𝐾𝑑𝑑 𝐸𝐸 𝑘𝑘 − 𝐸𝐸 𝑘𝑘 − 1

𝑅𝑅 𝑈𝑈 𝑌𝑌
+
− 𝑃𝑃𝑃𝑃𝑃𝑃 𝑠𝑠𝑠𝑠𝑠𝑠

Kinematics, Dynamics and low-level control of robots MRGCV 21

Quadrotor control: Example Hector (LAB 2-3)

Kinematics, Dynamics and low-level control of robots MRGCV 22

SUMMARY
• You should know
• The concepts of kinematics and dynamics
• The differential equations that describe car and
quadrotor dynamics
• How to compute their low-level control commands
• Define simple feasible trajectories to move them

Kinematics, Dynamics and low-level control of robots MRGCV 23

Relevant bibliography
• Unicycle kinematics
• Introduction to Autonomous Mobile Robots
• Car dynamics and control
• R. Rajamani, “Vehicle Dynamics and Control”. Springer
• [A] J. Jeon, R. V. Cowlagi, S. C. Petersy, S. Karaman, E. Frazzoli, P. Tsiotras,
K. Iagnemma, “Optimal Motion Planning with the Half-Car Dynamical
Model for Autonomous High-Speed Driving”, American Control
Conference 2013
• Drones
• [B] R. Mahony, V. Kumar, P. Corke “Multirotor Aerial Vehicles: Modeling,
Estimation and Control of Quadrotor”, IEEE Robotics & Automation
Magazine, 2012

Kinematics, Dynamics and low-level control of robots MRGCV 24