Mobile Robot Navigation –
some issues in controller design and
implementation
L. Huang
School of Engineering and Advanced Technology
Massey University
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Outlines
1. Introduction
2. Target tracking control schemes
based on


Lyapunov method
Potential field method
3. Speed control
4. Conclusion
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Introduction
A wheeled mobile robot (WMR) can be driven by
wheels in various formations:
Differential
Omni Directional
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Steering
Differential Wheel Robot
Omni Wheel Robot
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Two basic issues:
1. How to move a robot from posture A to
posture B stand alone ?
B
A
2. How to determine postures A and B for a robot
when a group of robots performing a task (such
as soccer playing) ?
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Strategy
Planning
A, B
Point
Path
Generation
xi , y i
Line
Trajectory
Generation
Motion
Control
Speed
Control
x(t ), y(t )
vd (t), ωd (t)
v(t), ω(t)
Line+Time
Desired Velocity
Actual Velocity
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Differential wheel driven robot (no-holonomic):
Robot’s posture (Cartesian coordinates) cannot be stablized by
time-invariant feedback control or smooth state feedback control
(Brockett R. W. etc.).
Stabilization problem was solved by discontinuous or time
varying control in Cartesian space (Campion G. B., Samson C.
etc.)
Asymptotic stabilization through smooth state feedback was
achieved by Lyapunov design in Polar coordinates – the system is
singular in origin, thus avoids the Brockett’s condition (Aicardi M.
etc ).
Trajectory tracking control is easier to achieve and is more
significant in practice (desired velocity nonzero) (Caudaus De Wit,
De Luca A etc.).
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Omni-wheel driven robot
 It is fully linearisable for the controller design (D’AndreaNovel etc.)
 Dynamic optimal control was implemented ( KalmarNagy etc.)
Robot modeled as a point-mass
 Potential field method was used for robot path planning
(Y.Koren and J. Borenstein)
Issues to be addressed
 Application of Lyapunov-based and potential field based
methods in the development of target tracking control
scheme
Seminar at Monash University, Sunway Campus, 14 Dec 2009
General control approaches
• Kinematic Model
 x   cos  0 
  
 v 
 y    sin  0  


  0

1
  
• Nonholonomic Constraint
(rolling contact without slipping)
Differential Wheel Robot
x sin   y cos  0
Nonhonolonic (No-integrable) and under actuated (2-input~3-output)
cannot be stabilized by time-invariant or smooth feedback control
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Trajectory tracking (Cartesian coordinates based)
Given
find
to make
xd , yd , xd and y d
v and 
x  xd , y  y d
Seminar at Monash University, Sunway Campus, 14 Dec 2009
It can be proved (due to Lyapunov and Barbalat), the
following control can meet the objective :
v  vd cos( d   )  k1[cos ( xd  x)  sin  ( yd  y )]
   d  k2 sgn(vd )[sin  ( xd  x)  cos ( yd  y)]  k3 ( d   )
d 
yd xd  xd y d
xd2  y d2
 d  ATAN 2( y d , x d )  k
Desired linear velocity (along
the trajectory)
Desired angular velocity
Desired direction
k1  k3  2 d2  bvd2 , k2  b vd
Note:
The trajectory needed to be specified in
prior; the controller fails when vd  0
Seminar at Monash University, Sunway Campus, 14 Dec 2009
From the planned trajectory
vd   xd2  y d2
with nonlinear modifications to adjust angular motion:
v  vd cos( d   )  k1[cos ( xd  x)  sin  ( yd  y)]
sin( d   )
   d  k 2 vd
[sin  ( xd  x)  cos ( yd  y )]  k3 (vd ,  d )( d   )
 d 
,
where
k2  b
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Goal / target tracking (Polar coordinates based)
Control task: move the robot from its original
posture: ( x p , y p , θ p ) to the target posture ( xg , y g , θ g )
( g  0 : parallel p arking).
Seminar at Monash University, Sunway Campus, 14 Dec 2009
The system model described in polar coordinates:
  v cos  ,     v
sin 

sin 

,  v
  ( x g  x) 2  ( y g  y ) 2
  tan (
1
yg  y
xg  x
)
   
The model is singular at
Seminar at Monash University, Sunway Campus, 14 Dec 2009
 0

Let
v  k1  cos 
ω = k2 a2 γ +
k1 sin 2γ
(a2 γ + a3δ )
2a2 γ
It can be proved that ( due to Lyapunov and Barbalat)
  0,   0,   0
with the Lyapunov function candidate
V
1
1
1
a1 2  a 2 2  a 3 2 , a1 , a 2 , a 3 > 0
2
2
2
2
V  a1k1 cos 2   2  k 2 a2  2  0
• large control effort or fluctuation when the angle tracking error is
near zero or the linear tracking error is big
• the target is assumed to be stationary
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Potential field approach (point mass model)
Attractive and repulsive fields:
1
U att  1 prtT prt
2
1
  2 (  1   01 ) 2 , if    0
U rep   2

0
else
Robot move along the negative gradient
of the combined field:
v   pU att ( p)   pU rep ( p)
1 prt   2 (  1   01 )   2  p  , if    0

1 prt ,
else

•The law only specifies the direction of the robot velocity
• target is assumed to be stationary
• local minima
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Lyapunov based target tracking controller
with limited control efforts
System model (extended from the
conventional one by including the
velocity of the target):

   v cos   v cos
t


sin 
sin 
 vt

  v




sin 
sin  



v

v
 t ,   0

t



     ,     t
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Controller 1: Extension of the general control approach
v  (vt cos   v  ) cos
    
   sin 2
sin 

sin 2
(
cos  
)vt  t 
v (   )

2


2
It can be proved with Lyapunov method, that under the controller,
  0,   0 and   0
1
2
(Lyapunov function candidate: V  (  2   2   2 ) )
Note:
• target motions directly affects the control efforts
• sinusoidal functions of the systems states attenuate the magnitude
of control
• tracking errors appear as the denominators in the terms of the
controller
• linear tracking and angular tracking errors are treated equally – too
demanding ?
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Controller 2: Improvement from Controller 1
Prioritise and change the control objectives:
  0,   0 (or bounded),     0 (or bounded)
and reflect them in the definition of the Lyapunov function:
1
1
1
V   2   2  (   ) 2
2
2
2
New controller:
v  (vt cos   v  ) cos

v sin 2

sin 2
  
(   t (
cos   sin  ) 
v ) 
t
2  

2
2
2  
which can also achieve the convergence of the tracking errors, but with
less control efforts
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Comparison: control efforts of Controllers 1 and Controller 2
1    
2 
or
   sin 2
sin 

sin 2
(
cos  
)vt  t 
v (   )

2


2
v sin 2

sin 2
  
(   t (
cos   sin  ) 
v ) 
t
2  

2
2
2  
1     1   
2     2
vt sin 2
sin 2
(
cos   sin  ) 
v

2
2
1 k
    t ,  1  k ,  2 
2k
    
k


Seminar at Monash University, Sunway Campus, 14 Dec 2009
• By observation, the magnitude of controller 2 is less than that
controller 1
• Analysing the factors (  ) affecting the controller magnitude,
it is obvious that, except for the region near k    2
that affecting
Controller 1 is larger in magnitude than that affecting Controller 2.
1   
 1 ____
k
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Simulation Results (tracking a target Moving along a circle)
xt  3  15 cos(0.08t ), yt  47  15 sin( 0.08t ), vt  1.2
v  0.075,   0.15
Linear tracking
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Angular tracking
Linear velocity
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Angular velocity
Experiments
Robot trajectory under Controller 1
Robot trajectory under Controller 2
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Under Controller 1:
Tracking errors
Velocities
Under Controller 2:
Tracking errors
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Velocities
Demonstrations
Controller 1
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Controller 2
Conclusions:

It is feasible to reduce the control efforts through
prioritization of control objectives
 defining of Lyapunov function to reflect that priority
 attenuation of controller outputs with some special functions of
the system states (like sinusoidal functions etc.)
while achieving the same or better control results in
comparison with the conventional controllers


The performance of the controller is affected by the noises
of the sensors for state feedback (esp. velocity).
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Potential field based control approach for
robot’s target tracking
System model:
prt  [ xrt yrt ]T
x rt  vtar cos tar  v cos
y tar  vtar sin  tar  v sin 
Potential fields:
U  U att  U rep
1
U att  1 prtT prt
2
1
  2 (  1   01 ) 2 , if    0
U rep   2

0
else
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Case 1: Moving target free of obstacles
Minimization of the angle between the gradient of the field and the direction
of robot motion relative to the target.
Seminar at Monash University, Sunway Campus, 14 Dec 2009
• Direction
Minimisati on of
U att
 p rt

3
(
U att   p U att
rt
p rt
p rt
U att
U att
x
y
y rt 
x rt )( rt y rt  rt x rt )  0
xrt
y rt


   
  arcsin(
vtar sin( tar  )
),
v
 

2
Robot direction is adjusted around the directional line pointing to the target
Seminar at Monash University, Sunway Campus, 14 Dec 2009
• Speed
Intuitively
v  vtar sin( tar  )
It is chosen to decrease U att , or
U att  1 prtT p rt  1 prt (vtar cos( tar  )  v cos )
1
2
 1 prt (vtar cos( tar  )  (v  v sin ( tar  )) )  0
2
2
tar
2
One of the choices is:
v  (v  21vtar prt cos( tar  )   prt )
2
tar
It leads to:
2
1
2
1
2
U att  U att (0)e2 t  0
1
prt  0
The speed determined by the relative linear distance, the target velocity
and there directional relationship.
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Comparison of the robot and target speeds:
1
prt
v
2 2 2

 (1  21 cos( tar   )  1  ) ,  
vtar
vtar
1  0.8
1  0.4

1  0.1
( tar  )
The robot does not need to be always faster than the target

(e.g. . when (tar  ) 
)
2
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Case 2: Moving target with moving obstacles
The approach can be extended to solve the path/speed planning
of the robot surrounded by multiple obstacles.
vtar sin( tar   )
    arcsin
v
n
2
v  v tar cos( tar   )    i vobsi cos( obsi   roi )  1 prt ) 2  vtar
sin 2 ( tar   )
i 1
n
  arctan
sin     i sin  roi
i 1
n
cos    i cos roi
i 1
i 
 i proi
,  i   2  i 2 proi
1 prt
1
(  i1   01 )
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Simulation Results:
xt  3.0  sin t , yt  2.0  cos t
 tar  t ,
vtar  1.0,
Trajectories
1  1
Relative Distance
Solid line: target
Dashed line : robot under the proposed controller
Dotted line :robot under the conventional potential field controller
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Speed
Angle
Solid line: target
Dashed line : robot under the proposed controller
Dotted line :robot under the conventional potential field controller
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Performance of the conventional field method with a high gain
Trajectories
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Speed
Conclusion:
• the speed as well as the direction of the robot motion are
determined with potential field method
• the velocity of the moving target is taken into consideration
• the proposed approach maintains or improves tracking accuracy
and reduce control efforts, in comparison to the traditional
approaches
• further study on the determination of the optimum speed of
the robot can be done by specifying additional performance
requirements.
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Speed control considering dynamic coupling
between the actuators
• Synchronisation of the wheels’ motion affects the robot’s trajectory
• Coupling between the actuators needs to be considered
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Model based adaptive control
M  C ( )  
Dynamic model:
  [r l ]T
 r2
2
 2 (mb  I )  I w
M   4b 2
 r (mb 2  I )
 4b 2
 0
C ( )  
l   r



r2
2
(mb  I )  I w 
2

4b
r2
2
(
mb
 I)
4b 2
 r  l 
0


mc dr 3

, m  mc  2mw
2
4b
I  mc d 2  2m w b 2  I c  2 I m
mc , mw , I m , I c are the inertia parameters of the robot and the wheels
b, d , r are the geometric parameters
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Introducing new variables
then
  T ,   T , T 
   1  2 T ,     1  2 T
1   r  l ,  2   r  l
 1   r   l ,  2   r   l
1 1 1 
2 1  1
Dynamic model is transformed to a more compact form :
M    2C   
0

0  1

M   T 1 MT   1
,
C


1 0 


 0 2 
mr 2
Ir 2
1 
 Iw, 2  2  Iw
2
2b
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Based on the transformed dynamic model, the adaptive speed controllers
are derived:
k1  k 2
k  k2
1
( rd   r )  1
( ld   l )  (ˆ 1 ld  ˆ 2 2 d )  ˆ 2  ld
2
2
2
k k
k k
1
 l  1 2 ( ld   l )  1 2 ( rd   r )  (ˆ 1 ld  ˆ 2 2 d )  ˆ 2  rd
2
2
2

ˆ1   1d e1 , ˆ 2   2 d e2 , ˆ   2 ( 2 1d  1 2 d )
r 
e1  1d  1 , e2   2 d   2
Modified to reduce the amplitudes of the control outputs:
k1  k 2
k k
1
(rd  r )  1 2 (ld  l )  (ˆ1 ld  ˆ 2 2 d )  kˆl
2
2
2
k k
k k
1
 l  1 2 (ld  l )  1 2 (rd  r )  (ˆ1 ld  ˆ 2 2 d )  kˆr
2
2
2

ˆ  k (  e   e )
r 
2 1
1 2
k  2   k (2 e1  1e2 )
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Simulation results
r  l
l
rad/s
Nm
t (sec)
Seminar at Monash University, Sunway Campus, 14 Dec 2009
t (sec)
Model free PID control
A loop for the coupling of the wheels’ speeds is added.
l
 ld
-
+
ul
+
K l (s)
+
+
+
 rd
+
+
l
+
Gl (s)
K a (s )
+
K r (s)
- u
r
r
Gr (s)
-
r
Seminar at Monash University, Sunway Campus, 14 Dec 2009
When
Gl (s)  Gr (s)  G(s), K l ( s)  K r (s)  K (s)
Transfer functions :
Gind ( S ) 
G ( s ) K ( s )( K ( s )  2 K s ( s ))
,
G ( s ) K ( s )( K ( s )  2 K a ( s ))  K ( s )  K a ( s )
Gind ( S ) 
K s ( s)
,
G ( s ) K ( s )( K ( s )  2 K a ( s ))  K ( s )  K a ( s )
l ( s)  Gind ( s)ld ( s)  Gsyn ( s)r ( s)
r ( s)  Gind ( s)rd ( s)  Gsyn ( s)l ( s)
G ( s) 
km
,  m  JRa / K t2
1 ms
• First order motor model is adopted:
• PID controller is used for the speed control
• Implemented with one PIC18F252 microcontroller
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Speed Control of an Omni-wheel robots
Modeling (Kinematics)
1
v1
v
r
Yr
Y

2
1
 O
Or
Xr
v3
b
3
X
2
v2
Omni Wheel Robot
Seminar at Monash University, Sunway Campus, 14 Dec 2009
3
Inverse kinematic model:
ri  b  vr vi (i  1, 2, 3)
v1  [1 0]T
v2  [cos
v3  [cos

 sin
3

sin
3

3

3
]T
]T
1  r 1 (b  vrx )
2  r 1 (b  vrx cos
3  r 1 (b  vrx cos

3

3
 vry sin
 vry sin
vr  [vrx

3

3
)
)
vry ]T
vrx  v x cos   v y sin 
vry  v x sin   v y cos 
Seminar at Monash University, Sunway Campus, 14 Dec 2009
• Chooped fed motors with drivers to drive the
wheels
• PID controller implemented with one one
80296 microcontrollers (three PWM outputs)
• Encoder resolution 512 ppr
• Sampling time 1 ms
• Control loop completed within 0.5ms
This is achieved through:
• codes written in an assembly language without
using floating point libraries (too slow)
• fixed point notation and a look up table of
whole numbers to represent a floating point
number with reasonable accuracy
• only the simple operations like addition,
substration, multiplication and bits-shifting are
used.
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Implementation
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Demonstrations
Seminar at Monash University, Sunway Campus, 14 Dec 2009
Conclusion
 Lyapunov and potential field based target tracking controllers, and speed
controller for dynamically coupled wheels for mobile robots were presented
 Both position and velocity of the target were considered in the target tracking
controller design
 Functions of the system states, especially those of the target, are
are designed to moderate the magnitude or fluctuation of the control effort
 The states of the system were assumed to be available; sensor noises affect
the performance of the controller.
 To get a good system states estimation and prediction from the sensor data
is another big issue to be addressed together with the controller design
(Kalman filtering, Bayesian method etc.)
 Further study can be undertaken on integrating open-loop optimal
control, closed-loop control and system states estimation and prediction
Seminar at Monash University, Sunway Campus, 14 Dec 2009