Professional Documents
Culture Documents
Motion Planning For Non-Holonomic Mobile Robots Using The I-PID
Motion Planning For Non-Holonomic Mobile Robots Using The I-PID
I. INTRODUCTION
Robot motion planning has been studied for many years,
and there are many different studies using different approaches for motion planning problems [1], such as A [2],
D [3], artificial potential field [4], visibility graph [5] and
methods based on optimal control problem [6].
Artificial potential field is proposed by Khatib [7] and has
been widely studied because of its simplicity and interesting
mathematical analysis [8], [9]. The basic idea of this approach is to fill the robot workspace with attractive potential
field caused by the target and the repulsive potential field
caused by obstacles. There are also some extensions for the
potential field method. Moving obstacles are considered in
[10]. In [11] the velocity of obstacles and target are taken
into consideration, and the unreachable target with obstacle
nearby problem is discussed in [12]. Although the basic
idea of potential field approach seems easy, there exist some
situations that the avoidance of obstacles can not be achieved,
and oscillatory movement is also an important issue.
As for non-holonomic mobile robots, the design of feasible
trajectory joining the starting position and the target is not
straightforward because of the non-holonomic constraint.
Both open-loop and close-loop methods are designed [13].
However open-loop methods are not robust to the disturbance
in the system. Closed-loop methods of non-holonomic mobile robots have been studied, but according to Brocketts
theorem [14], there exists no smooth feedback control that
stabilizes the given configuration. This means that the class
of the controllers should be extended to take into account
time-varying or non-smooth controllers [15].
PROBLEM STATEMENT
III. M OTION
ROBOTS VIA
zhaopeng.qiu}@ec-lille.fr
i-PID
CONTROLLER
Wilfrid Perruquetti and Gang Zheng are with Non-A team, INRIA Lille Nord Europe, 40 avenue Halley, 59650 Villeneuve dAscq, France
A. Problem formulation
{wilfrid.perruquetti, gang.zheng}@inria.fr
3618
1 The
y
6
Obstacle
Robot
[16] is as follows:
Target
Stotal
-
Satt
Srep
x
-
y
6
Obstacle
Stotal
v(t)
--
Target
Robot
x
-
h
i1
vr,x
GT
= GT G
vr,y
x
y
=
=
=
v cos
v sin
(1)
where G() =
cos
sin
= G()
0
0
(2)
i
, thus the controller proposed in
(3)
where
=
G
cos
sin
u
= G
v sin
v cos
(4)
(5)
and u
= [v,
]T being the new control input. The following
section is then devoted to using the robust i-PID controller
to achieve the tracking task.
C. i-PID controller
The i-PID controller was proposed in [17], and it exhibits
robustness to the unmodeled dynamics and disturbance in
the system [18]. Let us briefly present the basic idea of this
controller.
Generally speaking, this method locally approximates the
system model by a local model with an unknown term, and
the unknown term can be estimated by the measurements of
the input and output of the system, then a so-called i-PID
controller can be deduced to realize the control goal.
In this paper the system model (4) is approximated by
the following local model over a small time interval T =
[tk , tk+1 ], with k Z + :
= F (t) + (x, y, x,
y)
u(t)
(6)
where (x, y, x,
y)
is a non singular 22 dimensional matrix
which should be well chosen in order to achieve the control
goal. F (t) R2 represents all unknown terms including disturbances, which can be estimated by using the information
of u, (x, y, x,
y)
and the output of the system.
For the above continuous local model over time T , one can
estimate F (t) by discretizing it. Denote Ts as the sampling
time period, so that at each step k = t/Ts we have
x
k
y
k
= Fk + (x, y, x,
y)
uk
(7)
then it yields
Fk =
x
k
yk
(x, y, x,
y)
uk
(8)
where [
xk , yk ]T and u
k are measurable signals at time k. If
it assumed that Ts is small enough such that Fk1 Fk ,
then the i-PID controller can be designed as follows:
3619
u
k = 1 (x, y, x,
y)(F
k1 + ek )
(9)
where
ek =
vr,x
vr,y
+ Kp
x vr,x
y vr,y
+ KI
Z
x vr,x
y vr,y
(10)
6
T
(2 1)
0
x
y
d + 6(x, y, x,
y)
(2 )
ud (11)
FIELD FUNCTION
Kpv P (P,P 1 )P p1 cos d ,
m
0
d
obs
(12)
(15)
Uatt (P)
= 2Katt kPtar P(t)knRT
P
(13)
() =
where nRT is the unit vector pointing from the robot to the
target.
Urep (, ) =
2
0 d 2 ,
K2 p Pd (P, Pobs )
if VRO > 0 and Pd < p and d < ()
1
1
(t)
2max
(16)
(17)
(14)
3620
(18)
y
0
v2 (t)
v1 (t)
P RO =
Obstacle
Pobs
P d = P k RO k
nRO
P d = P ( RO ) =
x
-
nRO
P d = P (RO ) =
Obstacle
Pobs
v(t)
^
nRO
Fig. 4.
(29)
Pd (P, Pobs )
*VRO nRO
nRO
*
i
RO
]b
Robot
P(t)
(28)
Pd (P, Pobs )
If RO , we have
(27)
If > RO , we obtain
()
Robot
P(t)
VRO (t)nRO
(26)
Therefore
v3 (t)
Fig. 3.
nRO
Pd (P, Pobs )
P d =
RO
(30)
Pd (P, Pobs )
Therefore we have
Srep (P, v) =
x
-
(31)
Srep1 + Srep2 ,
if 0 < Pd (P, Pobs ) Pm < p0 and VRO > 0
where
Thus the corresponding repulsive force Srep is defined as
the negative gradient of the repulsive potential function with
respect to robot position and velocity. Since v U (, ) = 0,
we have
Srep = Srep (P, v) + Srep (, )
= P Urep (P, v) v Urep (P, v) P Urep (, )
Srep1 =
(22)
where
nT
RO nRO = 0
and
VRO nRO
kPobs P(t)k
nRO
= 4K1 MH 2 (d ())(0 d )2
Pd (P, Pobs )
nRO
+ 2M 2 H 2 (0 d )
Pd (P, Pobs )
2
H(0 d )2 nRO
Srep5 = 2K2
2
H 2 (0 d )
Srep6 = 2K2
where
(23)
(24)
(25)
nRO
Srep (, ) =
Srep3 + Srep4
Srep5 + Srep6 ,
if VRO > 0 and Pd (P, Pobs ) < p and d < ()
where
VRO (t) =
(32)
We also have
VRO
amax
+ Kpv sin d
Pd (P, Pobs ) Pm
p0 Pd (P, Pobs )
(20)
1+
Srep2 =
and
(19)
Kpv cos d
(Pd (P, Pobs ) Pm )2
M = K1 (d ())2 + K2
nRO
Pd (P, Pobs )
and
(34)
(35)
(36)
(37)
(38)
H = p Pd (P, Pobs ).
3621
1.4
S K
}rep2
]
a
Robot
P(t)
Srep3 (Srep5 )
Srep
v(t)
Obstacle
Pobs
(P,v)
rep
Target
Robot Trajectory
1.2
O12
4
1
O7
O6
3
Y axis
Srep4 (Srep6 )
O10
O2
O5
0.8
O9
O3
0.6
O11
O1
0.4
O8
Srep1
O4
1
1
0.2
3
X axis
10
12
14
time (s)
5
Target
Trajectory with old potential function
Trajectory with new potential function
4.5
4
3.5
Yaxis
3
2.5
2
Obstacle
1.5
1
0.5
0
Xaxis
Fig. 6. Situation with the connection between the robot and the target and
the robot velocity pass through the center of the obstacle
n
obs
X
Srep,j
j=1
(39)
RESULTS
Fig. 9.
Fig. 10. Linear velocity control and angular velocity control with original
Srep (P, v)
3622
Target
Robot Trajectory
O12
4
O7
O6
Y axis
O10
O2
O5
O9
O3
O11
O1
O8
O4
1
1
Fig. 11.
3
X axis
Fig. 14. Linear velocity control and angular velocity control with both
Srep (, ) and Srep (P, v)
R EFERENCES
Fig. 12. Repulsive force Srep (P, v) and Srep (, ) with both Srep (, )
and Srep (P, v)
Fig. 13. Velocity tracking in X and Y direction with both Srep (, ) and
Srep (P, v)
3623