Model and Control of a differential drive mobile robot

Marinho T. Ramos J.L.

May 23, 2013

1 Kinematics of the Differential Drive Robot

(φ̇r + φ̇l )r
v0 = (1)
2
vc is the speed of the center of mass of the robot in the robot coordinates.
vc = v0 + d × ω (2)
       
ẋr −d 0 ẋr
vc =  ẏr  +  0  ×  0  = ẏr + ωd (3)
0 0 ω 0
(φ̇r − φ̇l )r
ωc = ω0 = θ̇ = (4)
2b
World coordinates

ẋ = ẋr cos(θ) − ẏr sin(θ) (5)

ẏ = ẋr sin(θ) + ẏr cos(θ) (6)
Solving

ẋr = ẋ cos(θ) + ẏ sin(θ) (7)

ẏr = −ẋ sin(θ) + ẏ cos(θ) (8)
Kinematic constraints of the differential drive in robot coordinates
 (φ̇ +φ̇ )r   
r
2
l ẋ cos(θ) + ẏ sin(θ)
vc =  0  = −ẋ sin(θ) + ẏ cos(θ) + ωd (9)
0 0
0
 

ωc =  0  (10)
(φ̇r −φ̇l )r
2b
That can be rewritten as:

ẋ sin(θ)−ẏ cos(θ) − d · θ̇ = 0 (11)

ẋ cos(θ)+ẏ sin(θ) + b · θ̇ = r · φ̇r (12)
ẋ cos(θ)+ẏ sin(θ) − b · θ̇ = r · φ̇r (13)
In matrix form

AT (q) · q̇ = 0 (14)
 
sin(θ) − cos(θ) −d 0 0
AT (q) = cos(θ) sin(θ) b −r 0 (15)
cos(θ) sin(θ) −b 0 −r

1
2 Dynamics
The Langrage equations of nonholonomic system are given by [1]
 T  T
d ∂L ∂L
− = S(q)τ + A(q)λ (16)
dt ∂ q̇ ∂q

Where for the mobile differential drive robot we have

   
0 0 x
0 0   y
  τr  
S(q) = 0 0 ; τ = τl ; q =  θ  (17)
 
1 0 φr 
0 1 φl

Let us solve left side of the dynamic equation for each generalized coordinate of q.

We know that L(q, q̇) = T (q, q̇)−U(q). However the mobile robot has a horizontal restricted dynamics
that yields U(q) = 0.
1 1 1 1
L(q, q̇) = T (q, q̇) = mr vc2 + Ir θ̇2 + Iw φ̇2r + Iw φ̇2l (18)
2 2 2 2

vc2 = hvc , vc i = ẋ2 + ẏ 2 − 2d sin(θ)ẋθ̇ + 2d cos(θ)ẏ θ̇ + θ̇2 d2

(φ̇r − φ̇l )2 r2
θ̇2 =
4b2

1   1 1 1
L(q, q̇) = mr ẋ2 + ẏ 2 − 2d sin(θ)ẋθ̇ + 2d cos(θ)ẏ θ̇ + θ̇2 d2 + Ir θ̇2 + Iw φ̇2r + Iw φ̇2l (19)
2 2 2 2
• For x
   
d ∂L ∂L
− = mẍ − md sin(θ)θ̈ − md cos(θ)θ̇2
dt ∂ ẋ ∂x

• For y    
d ∂L ∂L
− = mÿ − md cos(θ)θ̈ − md sin(θ)θ̇2
dt ∂ ẏ ∂y

• For θ    
d ∂L ∂L
− = −md sin(θ)ẍ + md cos(θ)ÿ + Ir + md2 θ̈
dt ∂ θ̇ ∂θ

• For φr
   
d ∂L ∂L mrd sin(θ) mrd cos(θ) mrd cos(θ) mrd sin(θ)
− =− ẍ + ÿ − ẋθ̇ − ẏ θ̇+
dt ∂ φ˙r ∂φr b b b b
md2 r2 Ir r2 md2 r2 Ir r2
   
Iw + + 2 φ̈r − + 2 φ̈l (20)
4b2 4b 4b2 4b

• For φl
   
d ∂L ∂L mrd sin(θ) mrd cos(θ) mrd cos(θ) mrd sin(θ)
− = ẍ − ÿ + ẋθ̇ + ẏ θ̇
dt ∂ φ̇l ∂φl b b b b
md2 r2 Ir r2 md2 r2 Ir r 2
   
+ Iw + + 2 φ̈l − + 2 φ̈r (21)
4b2 4b 4b2 4b

2
 The dynamical model can be written in matrix form where

B(q)q̈ + n(q, q̇) = S(q)τ + A(q)λ (22)

 
m 0 −md sin(θ) 0 0

 0 m md cos(θ) 0 0 
2
B(q) =  −md
 r sin(θ) md cos(θ) I r + md 0 0 (23)
r

− md sin(θ) md cos(θ) 0 α1 α2 
b b
r
b md sin(θ) − rb md cos(θ) 0 α2 α1
mr2 d2 Ir r2
α1 = Iw + +
4b2 4b2
md2 r2 Ir r2
 
α2 = − +
4b2 4b2
 
−θ̇2 md cos(θ)

 −θ̇2 md sin(θ) 

n(q, q̇) = 
 0 
 (24)
 mrd cos(θ) mrd sin(θ)
− ẋ θ̇ − ẏ θ̇

b b 
mrd cos(θ) mrd sin(θ)
b ẋ θ̇ + b ẏ θ̇

3 Reduced Dynamic Model

From equations (11) to (13) one can derive a relation between the pseudo-velocity vector v = [vc , θ̇]T
and the generalized velocities q̇ where

q̇ = G(q)v (25)
Solving for the differential drive robot we have
 br cos(θ)+dr sin(θ) br cos(θ)−dr sin(θ) 
2b 2b
 br sin(θ)−dr cos(θ) br sin(θ)+dr cos(θ) 
 2b 2b 
G(q) = r r (26)

 2b − 2b 

 1 0 
0 1
From equation (14) one can see that G(q) columns are the null spaces of A(q)T so that A(q)T ·G(q) =
T
G(q) · A(q) = 0.
The Lagrange multipliers in equation (35) can be eliminated premultiplying both sides of the equation
by G(q)T . This leads to the reduced dynamic model

G(q)T (B(q)q̈ + n(q, q̇)) = G(q)T S(q)τ (27)

Where the differentiation of equation (25) in respect to time yields

q̈ = Ġ(q)v + G(q)v̇

The reduced dynamic model can be rewritten as

M(q)v̇ + m(q, v) = GT (q)S(q)τ (28)

if we premultiply equation (27) by GT (q)B(q) and define

M(q) = GT (q)B(q)G(q) (29)

m(q, v) = GT (q)B(q)Ġ(q)v + GT (q) n (q, G(q)v). (30)

The state space reduced model can be found by solving for q̇ and v̇

3
 q̇ = G(q)v (31)
v̇ = −M−1 (q)m(q, v) + M−1 (q)GT (q)S(q)τ (32)

4 DC Motor Model
DC motor dynamics are given by

L di + Ri + K φ̇ = E

w
dt (33)
Is φ̈ − Kt i + V φ̇ − τ = 0


as in [ 1, p.198 ] where E and i denote the armature voltage and current, R and L are respectively the
armature resistance and inductance while Is is the motor shaft inertia, V the viscous friction coefficient
and τ the dynamic load applied to the motor. Kt is the motor torque constant and Kw the voltage
constant.
A reduced order model can be achieved for the dynamic behaviour since the electric time constant
L
R can be neglected if compared to the mechanical time constant VI . Hence, we consider L = 0 and the
first equation yields

E − Kw φ̇
i=
R
that can be replaced on the second equation
!
E − Kw φ̇
Is φ̈ − Kt + V φ̇ − τ = 0
R
That gives us
 
Kt Kw Kt
Is φ̈ + + V φ̇ − E=τ (34)
R R
Therefore the new dynamic model for the mobile robot including the DC motor dynamics is

B(q)q̈ + n(q, q̇) = S(q)E + A(q)λ (35)

 
m 0 −md sin(θ) 0 0

 0 m md cos(θ) 0 0 
2
B(q) =  −md
 r sin(θ) md cos(θ) I r + md 0 0 (36)
r

− md sin(θ) md cos(θ) 0 α1 α2 
b b
r r
b md sin(θ) − b md cos(θ) 0 α2 α1
mr2 d2 Ir r2
α 1 = Iw + Is + +
4b2 4b2
2 2 2
 
md r Ir r
α2 = − 2
+ 2
4b 4b
 
−θ̇2 md cos(θ)

 −θ̇2 md sin(θ) 

n(q, q̇) = 
 0 
 (37)
 mrd cos(θ) mrd sin(θ)
ẏ θ̇ + KtRKw φ̇r 


− ẋθ̇ −

b b
mrd cos(θ)
ẋθ̇ + mrd sin(θ) ẏ θ̇ + KtRKw φ̇l


b b
 
0 0
0 0  
 ; E = Er
 
S(q) =  0 0 (38)
K  El
 t
R 0
Kt
0 R

4
References
[1] B. Siciliano, P. L. Sciavicco, L. Vilani and G. Oriolo, A Guide to LATEX, 3rd ed. Harlow, England:
Addison-Wesley, 1999.