Luận án Phát triển hệ thống định vị và lập kế hoạch chuyển động hiệu quả cho robot dạng bánh trong môi trường động

phase uses information obtained from to update the
particle weights to accurately describe the pdf of the robot. In an-
other word, using a current sensor measurement (3.14) to correct the
predicted state. At line 20 (in Algorithm 3) calculates the importance
of weight w
[j]
k for each particle x
[j]
k . The importance of weight is used
to correct the mismatch between the proposal distribution and the de-
sired target distribution [53]. According to the measurement model,
weights are assigned by likelihood response (in Algorithm 3). The mea-
surement likelihood function computes the likelihood for each predicted
particle based on the error norm (EN) between predicted measurement
and actual measurement (line 18). In this particular situation, predicted
particle considered as the predicted is an Nx3 vector and measurement
is a 1x n vector (n is the number of measured elements, in this section
has three different cases of measurement).
Resampling (for more detail in algorithm 4): One of the problems
that appear with the use of PF is the depletion of the population after
a few iterations. Most of the particles have drifted far enough for their
weight to become too small to contribute to the pdf of the moving robot.
In this section, the Multinomial method has been chosen.
The proposed PF-based localization algorithm is summarized in Al-
gorithm 3. It inputs the particle set Sk−1 at time k-1, motion control
uk−1, measurements zk. It outputs the particle set Sk. Here, N denotes
the total number of particles used in this algorithm.
60
Algorithm 4: Presents resampling methods based on the cumulative sum of
the normalized weights
input : Particle filter input (Sk, wk)
output: Sk
1 begin
2 [xˆ
[i]
k ] = Resembel[{x[j]k , w[j]k }, N ]
3 Multinomial choice
4 N = length(w);
5 Q = CumulativeSum(w);
6 Q(N)=1;
7 i=0;
8 While(i≤N)
9 Sampl = rand(1,1);
10 j=1;
11 While(Q
[j]
k < Sampl)
12 j=j+1;
13 endwhile
14 Index(i) =j;
15 i= i+1;
16 xˆ
[i]
k = x
[j]
k
17 add xˆ
[i]
k to Sk
18 endwhile
19 Return Sk
3.2.2. Results and discussions
3.2.2.1. Simulation setup
To verify the usefulness of the proposed localization system, the pro-
posed algorithm have been implemented and tested in a MATLAB based
simulation. In order to accomplish that, four scenarios are created ,
which their size are 12 × 8[m]2, as shown in Fig. 3.8. The robot kine-
matic model and measurement model are presented in (3.7), (3.14), re-
spectively. The trajectory of the mobile robot is the curve trajectory.
The initial pose of the robot is [0, 0, 0]
T
. The robot will navigate fol-
lowing the trajectory with the traveling time of 20[s]. The maximum
linear velocity and angular velocity of mobile robot are 80[cm/s] and
61
0.008[rad/s], respectively. Empirically setting the number of particles
is 500 particles. The state of all particles is randomly initialized using
a normal distribution. Each particle contains 3 state variables [x, y,
θ]. Assume that, the measurement of all three pose components have
the same error distribution. Here the sensor reading is just simulated by
adding Gaussian noise to the ground truth data (with the noise standard
deviation sd = 0.2) and defused noise into the system model with noise
standard deviations: sd1 = 4; sd2 = 1.2; sd3 = 0.35. The trajectory
of the mobile robot is divided into three parts to show the performance
of the particle filter-based localization system of the mobile robot, as
shown in Fig. 3.8.
In the first part, the mobile robot navigates in a good environment
condition, where the observation system works well. In other words, the
localization system receives all the sensor data.
In the second part, the mobile robot navigates into the roofed area.
In this area, all of the signals are lost or a part of them is received by the
localization system. If the entire signals are lost, the particle filter-based
localization only uses the prediction model to predict the pose of the
robot. While in the later, the localization system can use the available
signal to correct the prediction step.
In the third part, the mobile robot goes out of the roofed area. With
new measurements, the estimated pose gradually converges back to the
actual pose.
In order to compare the simulation results between approaches in
terms of quantity, statistical data analysis of all the simulations is car-
62
ried out. To accomplish that, the mean error (ME), which is computed
in (3.20), is utilized In this section.
ME =
1
n
n∑
k=1
NEk (3.20)
where n=500 is the number of samples, NE is calculated in
NEk =
√
(xPF − xtrue)2 + (yPF − ytrue)2 (3.21)
3.2.2.2. Simulation results
The simulation results are shown in Fig. 3.8, and Table 3.3. The Fig-
ure 3.8 shows four scenarios such as (a) robot receives [x, y, θ] in the
entire trajectory, (b) robot gets [x, y] in the roofed area, (c) the [θ] infor-
mation is available in the roofed area, (d) all the sensor signals are lost
in the roofed area. The figure also illustrates the trajectories of the mo-
bile robot, including the black line is the desired trajectory; the magenta
dots are the GPS data, and the blue crosses are the estimated trajectory
of the mobile robot using the proposed algorithm. Whereas, Table 3.3
illustrates the mean square errors of four approaches when the localiza-
tion system uses or does not use the particle filter algorithm. Figure
3.8(a) shows the simulation results when the localization system of the
mobile robot can receive all signals [x, y, θ] from the observation system.
As can be seen in Fig. 3.8(a), the estimated trajectory is approximated
to the actual trajectory of the robot. As a result, the value of mean error
0.1098 is the smallest, as seen in Table 3.3.
In Fig. 3.8(b), the mobile robot navigates through the roofed area and
the signal θ is lost. In other words, the localization system can only use
63
(a) Observed signal (x,y,θ) (b) Observed signal (x, y)
(c) Observed signal (θ) (d) Observed signal (∅)
Figure 3.8: The simulation results using PF localization
the observed [x, y] signal in the correction phase. In this case, the mean
error value is 0.1451. It indicates that the performance of the localization
system is decreased, as also shown in Fig. 3.8(b).
In Fig. 3.8(c), the mobile robot navigates through the roofed area and
the signals [x, y] are lost. In another word, the localization system can
only use the observed θ signal in the correction phase. In this case, the
mean error value is 0.2406. It indicates that the performance of the
localization system is decreased, as also shown in Fig. 3.8(c).
In the final case, all of the signals [x, y, θ] from the sensor system are
lost (observed [∅]). In other words, in the roofed area, the localization
64
only uses the prediction phase to estimate the pose of the mobile robot.
As a result, mean error value 0.3957 is greatest and the estimated robot
trajectory is far from the actual trajectory, as shown in the last row in
Table 3.3 and Fig. 3.8(d).
The comparison of estimated trajectories among the three approaches
illustrates that using the particle filter algorithm the localization can
track the robot’s position and trajectory after going out of the roofed
area. In other words, after lost signal in interval time the mobile robot
still likely follows the trajectory. However, the performance of the mea-
sured parameters is gradually reduced according to the measured pa-
rameters. In addition to the results shown in Fig. 3.8, the comparison
Table 3.3: The mean error of the proposed localization system and existing localiza-
tion system.
Observed Signal Encoder-based odometry algorithm Proposed localization algorithm
[x, y, θ] 0.2510 0.1098
[x, y] 0.2479 0.1451
[θ] 0.9648 0.2406
[∅] 0.9722 0.3957
of mean error of the proposed particle filter-based localization algorithm
and the conventional algorithm are made. The comparison results are
shown in Table 3.3. The first column shows the mean error values of
the conventional algorithm, while the second column shows the mean
error values of our proposed system. The results in Table I illustrates
that the mobile robot equipped with our proposed localization system
outperforms the conventional localization system in term of mean errors.
In summary, the simulation results indicate that the proposed parti-
cle filter-based localization algorithm is able to apply and improve the
65
performance of the autonomous mobile robot when it navigates in inter-
rupted sensor data information.
3.3. Remarks and discussions
In this chapter, two efficient localization algorithms have been pro-
posed for autonomous mobile robots in dynamic environments including
EKF-based localization algorithm and PF-based localization algorithm.
The main idea of the proposed algorithms are to fuse the data sources
from the sensor system including wheel encoders, IMU, GPS sensors, to
enhance the accuracy of the robot’s pose estimation when it navigates
in two different cases.
The first case, when sensor signals are sufficient and noise distribu-
tions are Gaussian distribution, the EKF - based localization algorithm
has been made of used. Two simulation results corresponding with two
sinusoidal and circular trajectories of the autonomous mobile robots have
conducted. The simulation results indicate that, the proposed localiza-
tion algorithm is capable of providing higher accuracy mobile robot’s
pose than conventional localization systems.
The second case, when information get from sensor systems is insuf-
ficient or the sensor data signals are interrupted, and noises have Non-
Gaussian/Gaussian distribution, PF - based localization algorithm has
been proposed. To illustrate the effectiveness of the proposed system,
we implement it and conduct simulation in a simulation environment.
The simulation results show that, although the mobile robot navigates
into areas where a part or entire of the signals can be lost, the robot’s
66
positions are still close to ground truth data. In other words, the simula-
tion results illustrate that the performance of the proposed localization
system is significantly improved in terms of accuracy.
The output of the proposed localization systems are the robot’s pose
including robot’s position and orientation, which are then used as the
input of the motion planning system, as shown in Fig. 1.1. In the next
chapter, three proposed motion planning systems are going to present
for the autonomous mobile robots in the dynamic environments.
Note that the researches of this chapter have been published in [47]
and [48].
67
Chapter 4
DEVELOPING EFFICIENT MOTION
PLANNING SYSTEMS
The ability to autonomously and safely navigate in real-world dynamic
environments is crucial for autonomous mobile robots. To achieve that
ability, the most important issue is that the mobile robots must avoid
both static and dynamic obstacles during its navigation and navigate
towards a given target. Therefore, it is very necessary to develop nav-
igation systems which are able to drive the mobile robots in real-world
environments with dynamic obstacles. In the previous chapter, we pro-
posed the two localization algorithms which were used to improve ac-
curacy of output signals of the localization system. In other words, the
input signals of the motion planning system including robot’s position
and orientation are enhanced significantly, as shown in Fig. 4.1.
Perception Localization Motor control 
Real – world environment 
Local planner 
Global planner 
Motion planning 
Figure 4.1: The navigation framework for autonomous mobile robot.
68
In this chapter, we propose motion planning systems of the mobile
robots which are capable of driving the mobile robots to proactively and
safely avoid dynamic obstacles in the real-world environments.
The motion planning systems include of two sub-systems, as shown
in Fig. 4.1: (i) global planner (or path planning); (ii) local planner (or
obstacle avoidance). The Global planner is used to construct safe and
collision free paths of the robot from an initial point to the given goal
point with a given map. In contrast, thelocal planner means recalculat-
ing the constructed paths to avoid possible collision, especially moving
obstacles. Therefore, in order for the mobile robots to move safely in
the dynamic environments, we focus on developing the local planning al-
gorithms (or obstacle avoidance algorithms) in the motion planning sys-
tems. The proposed motion planning systems should take into account
the kinodynamic constraints of the robot, and the potential collisions of
the robots with surrounding obstacles. Particularly, the robots should
predict obstacle’s future states including position and orientation as well
as future trajectory of the obstacles in their vicinity.
To address the aforementioned issues, in this chapter, three new local
planners of the motion planing system for the autonomous mobile robots
in the dynamic environment are proposed, including the enhanced dy-
namic window approach (EDWA), proactive timed elastic band (PTEB),
and extended timed elastic band (ETEB) algorithm. In addition, an ef-
ficient navigation system, which integrates the proposed EKF-based
localization algorithm and a proposed ETEB algorithm for online tra-
jectory planning, is also introduced. The effectiveness and feasibility of
69
the proposed algorithms are validated through a series of experiments in
both simulated and real-world environments.
This chapter is organized as follows. The proposed hybrid (EDWA)
algorithm based on DWA technique and HRVO model to enhance the
performance navigation system of the robot in the dynamic environment
is presented in Section 4.1. To deal with the limitation of the EDWA
algorithm, the description of the proposed PTEB algorithm, combining
HRVO technique and TEB algorithm, is given in Section 4.2. In Section
4.3, we introduce the proposed ETEB algorithm: using motion predic-
tion algorithm incorporating into TEB model. In order to demonstrate
the feasibility of the proposed algorithms in our thesis, a completed
navigation system for the autonomous mobile robot in real -world en-
vironment is shown in Section 4.4. Finally, we reach a conclusion in
Section 4.5.
4.1. Proposed enhanced dynamic window approach algorithm
The dynamic environments are dynamic, uncertain, and clustered en-
vironments with the presence of humans, vehicles, and even other au-
tonomous devices. Therefore, various navigation systems have been pro-
posed to ensure the safe navigation of the mobile robot in such environ-
ments. The navigation frameworks can be divided into two categories
based on the information used as the input of the motion planning sys-
tem: (i) position-based approaches and (ii) velocity-based techniques.
In the first group, only the position of the obstacles is used as the input
of the system. Whereas in the second group, both position and motion
70
of the obstacles are utilized to develop the navigation system. Detail
discussion of two groups is going to the following.
Several position-based navigation systems have been proposed for the
autonomous mobile robots in recent years [21], [22], [23], [24], [25] [26]
and [27]. Some mobile robot navigation systems [22], [23] and [25] are
developed using the social force model [28]. The social force-based nav-
igation systems utilize the attractive force to the goal and the repulsive
forces from the obstacles to develop the motion control model. In addi-
tion, the repulsive forces is computed using the distance form the robot
to the obstacles.
In an alternative approach, the researchers utilize the dynamic window
approach algorithm proposed by Fox et al. [29] to develop the mobile
robot navigation systems [21] and [24]. These navigation systems take
into account the motion dynamic of the mobile robot and utilize the
closest distance from the robot to the surrounding obstacles for obstacle
avoidance.
More recently, a few navigation systems [26] and [27] are developed
using the TEB technique for avoiding obstacles. To maintain the sepa-
ration from the obstacles, the TEB-based navigation systems take into
consideration the distance from the proposed robot’s trajectory to the
surrounding obstacles.
Despite the fact that the aforementioned navigation systems have been
able to generate the safe navigation for the mobile robots in real-world
environments, they do not proactively deal with potential collision with
the surrounding obstacles. Because, these methods are typically a reac-
71
tive control technique, and the navigation systems only take into account
the position of the obstacles. In other words, moving obstacles are as-
sumed to be stationary. As a results, these navigation systems might be
sufficient for the obstacle avoidance in static and semi-dynamic environ-
ments, but foresighted evasive maneuvers are not possible.
In order take into account the potential collision of the mobile robot
with the surrounding obstacles, a number of autonomous robot naviga-
tion systems [30], [31], [32] and [33] based on the concept of the velocity
obstacles presented by Fiorini et al. [34], have been proposed. The ve-
locity obstacles-based navigation system takes into account the position
and motion of all agents and selects the collision-free velocity command
from the two-dimensional velocity space in the xy−plane for each agent.
Although these methods have been successfully verified in the real-world
environments, they might not be able to handle all collision situations
in The dynamic environments [31] and [33]. In addition, the velocity
obstacles-based navigation system only utilizes the current position and
velocity of the robot and the obstacles to generate the velocity com-
mand for the mobile robot. Moreover, the system does not take into
account the motion dynamic of the mobile robot. Thus, it is difficult to
directly utilize this velocity command to control the mobile robot in the
real-world environments.
In order to overcome the aforementioned drawbacks, in this section,
an EDWA technique for autonomous mobile robot navigation systems in
dynamic environments is proposed . The main idea of the proposed tech-
nique is to combine the advantages of the DWA technique and the HRVO
72
model. Particularly, incorporating the orientation of the velocity vector
generated by the HRVO model into the DWA model. By incorporating
this information into the DWA technique, the mobile robot equipped
with the proposed EDWA algorithm can proactively avoid obstacles and
safely navigate to the given goal.
In the section, we introduce an typical example scenario to perfor-
mance the proposed framework in Subsection 4.1.1. Then, the proposed
EDWA algorithm is presented in Subsection 4.1.2. In Subsection 4.1.3,
the proposed navigation frame work that uses EDWA model is shown.
Subsection 4.1.4 and Subsection 4.1.4 show the simulation and experi-
mental results, respectively. Finally, the last Subsection 4.1.5 contains
remarks.
4.1.1. Problem description
In this study, a dynamic environment with the presence of an au-
tonomous mobile robot and O obstacles o1 and o2 in the robot’s vicinity
is considered, as shown in Fig 4.2. The robot is requested to navigate to
a goal while safely avoid the obstacles during its navigation. The curved
dashed line is the intended trajectory of the mobile robot.
Assuming that the robot state is sr = [xr, yr, θr, vr, ωr]
T , where pr =
[xr, yr]
T is the position, θr is the orientation, vr is the linear velocity,
and ωr is the angular velocity. The dynamic motion of the mobile
robot is (vmin, vmax, ωmin, ωmax, v˙max, ω˙max), where, vmin, ωmin are the mini-
mum linear and angular velociti