Sage Journals: Discover world-class research

Abstract

Among the huge number of functionalities that are required for autonomous navigation, the most important are localization, mapping, and path planning. In this article, investigation of the path planning problem of unmanned ground vehicle is based on optimal control theory and simultaneous localization and mapping. A new approach of optimal simultaneous localization, mapping, and path planning is proposed. Our approach is mainly affected by vehicle’s kinematics and environment constraints. Simultaneous localization, mapping, and path planning algorithm requires two main stages. First, the simultaneous localization and mapping algorithm depends on the robust smooth variable structure filter estimate accurate positions of the unmanned ground vehicle. Then, an optimal path is planned using the aforementioned positions. The aim of the simultaneous localization, mapping, and path planning algorithm is to find an optimal path planning using the Shooting and Bellman methods which minimizes the final time of the unmanned ground vehicle path tracking. The simultaneous localization, mapping, and path planning algorithm has been approved in simulation, experiments, and including real data employing the mobile robot Pioneer $P 3 - AT$ . The obtained results using smooth variable structure filter–simultaneous localization and mapping positions and the Bellman approach show path generation improvements in terms of accuracy, smoothness, and continuity compared to extended Kalman filter–simultaneous localization and mapping positions.

Keywords

Localization map building unmanned ground vehicle path planning optimal control Shooting’s method Pontryagin’s principle Bellman’s method

Introduction

In this article, an optimal path planning problem for unmanned vehicle navigation is investigated. This topic is once in a lifetime of diverse categories of control theory; however, it is one of serious researches. Path planning for unmanned ground vehicle (UGV) is very interesting for many applications (monitoring, reconnaissance, mapping, etc.). Moreover, optimal path planning requires accurate and robust UGV localization. For this purpose, we propose in this article a new approach for simultaneous localization, mapping, and path planning (SLAMPP). This system contains two important parts, accurate localization and optimal path planning.

First, a new solution of UGV localization is considered, based on the simultaneous localization and mapping (SLAM) utilizing the smooth variable structure filter (SVSF). SLAM algorithm is the mechanism that helps a UGV to localize itself and construct a map where globally accurate position data (e.g. global positioning system (GPS)) are not accessible. In this respect, UGV-SLAM solution is crucial and has shown significant promise for remote exploration.^1,2

The SLAM framework based on the stochastic map approach was introduced in a seminal paper by Smith and Cheeseman in 1986. The SLAM techniques use in formation from onboard robot sensors to provide feature relative localization give an apriori map of the environment using the extended (EKF) which is presented Moutarlier and Chatila.³ This approach focuses on stochastic estimation techniques “extended EKF” to estimate the position of the robot and to construct the map. The work on the EKF-SLAM algorithm implementations can be found in different environments, such as indoors, outdoors, aerial space, and underwater submarine.^4,5 In addition, the update of the EKF covariance matrix increases quadratically in the size of the number of detecting landmarks.^6–8 A nonlinear version of the Kalman filter proposed in Davison⁹ is the unscented Kalman filter (UKF) that employs a particular representation of a Gaussian random variable in $(N)$ dimensions using $(2 N + 1)$ samples, labeled sigma points. Among the advantages of the UKF, noise can be considered in a nonlinear mode to account for non-Gaussian noise. On the other hand, it suffers less from linearization, although it is not exempt. Like EKF, the UKF does not fully recover from poor landmarks.

Another algorithm of estimation is known as Fast-SLAM. This algorithm uses a Rao-Blackwellized Particle Filter to estimate the robot’s pose and map. Each particle represents a possible robot’s position. The uncertainty about the knowledge of this position is represented by the distribution of these particles in space. The implementation of the Fast-SLAM algorithm is resolved successfully with thousands of landmarks and compared to the EKF-SLAM algorithm which can use only a few hundreds of landmarks. The Fast-SLAM is demonstrated that it is not suitable for real-time implementation.^10,11 In 2007, the progress of a new predictor–corrector estimator was established on the variable structure theory and sliding mode concepts.^11–13

In this article, we investigate this new approach, known as the SVSF. It is used to solve SLAM problems. It provides accurate and stable solution without any assumptions about noise characteristics. The SVSF can compromise with uncertainties and modeling errors of odometer/laser sensor system of UGV. It is robust and stable to modeling uncertainties making it suitable for UGV localization and mapping problem. The strategy of SVSF-SLAM algorithm is presented, this new concept ensures the stability and robustness faculties in modeling uncertainties.

In the second part of this work, a path planning algorithm using two methods (direct and indirect methods) is investigated. Indirect methods based on the Pontryagin principle of maximum^14–16 are known for their speed and precision in the treatment of optimal control problems. The Shooting method¹⁶ is an indirect method of optimization which attempts to solve the true optimization problem by solving the two-point boundary value problem (TPBVP).¹⁶ The initial conditions sensitive to the co-state equations cause a big problem with indirect methods. Therefore, direct methods are often used to initialize indirect methods when solving function optimization problems.¹⁷ Dynamic programming (DP) based on the Bellman principle is the best which solves the direct method problem, by reducing the initial hypothesis, and bringing a discrete time approximation to the optimal function of time, since DP algorithms are of order $(N)$ in the parameter optimization.^17,18

In this article, a robust system for localization, mapping, path planning, and optimal control (see Figure 1) is developed. The local navigator is a susceptible process that depends on the current sensor data to keep vehicle safe and stable during movement quick as possible. The path planning with optimal control using the Shooting and Bellman method based on different approaches of localization, as reviewed in this work, is a deliberative technique which regards forward to the future and uses information about the world to produce a safe path for the robot.

Figure 1.

State estimation EKF/SVSF-SLAM and optimal control for mobile robot.

This article is organized as follows. Section “Process model of UGV” depicts the process model of UGV. In section “EKF-SLAM algorithm,” the EKF-SLAM algorithm is detailed and compared with the proposed SVSF-SLAM algorithm. The implementation of this latter is presented in section “SVSF-SLAM algorithm.” In section “Optimal control,” we describe path planning with optimal control. Finally, results and discussion are provided in section “Results and discussion” followed by conclusion and perspectives.

Process model of UGV

The Pioneer $P 3 - AT$ which used in this work is a non-holonomic robot with small four-wheel (see Figure 2). It is a differential drive robot, offers an installed $PC$ alternative, opening the path for locally available vision handling, Ethernet-based interchanges, laser (see Figure 3), DGPS (differential global positioning system), and different independent capacities. P3-AT’s powerful motors and can reach speeds of 0.8 m/s and carry a payload of up to 12 kg. Let the vector $[X_{r}; Y_{r}; θ_{r}]^{T}$ with $(X_{r}; Y_{r})$ the coordinates of the robot in the global coordinate and $θ_{r}$ its orientation. The non-holonomic constraint is written as follows¹⁵

X_{r} {\overset{\cdot}{X}}_{r} \sin (θ_{r}) - Y_{r} {\overset{\cdot}{Y}}_{r} \cos (θ_{r}) = 0

(1)

Figure 2.

Mobile robot (P3-AT) Robotic Laboratory—EMP.

Figure 3.

SICK 2D laser range finder (LMS-200).

From Figure 4 and Sfeir,¹⁵ the state transition equation of the robot is given by

{\begin{matrix} {\overset{\cdot}{X}}_{r} = v_{x} \cos (θ_{r}) - v_{y} \sin (θ_{r}) \\ {\overset{\cdot}{Y}}_{r} = v_{y} \sin (θ_{r}) + v_{y} \cos (θ_{r}) \\ {\overset{\cdot}{θ}}_{r} = w \end{matrix}

(2)

where $(v_{x}, v_{y})$ are translation velocities of the robot following $X_{L}$ axes and $Y_{L}$ axes, respectively, and $w$ is the rotation velocity. Mobile robot’s model and measurement observation model are used to describe the motion and the status of the robot.

Figure 4.

Kinematic model of the UGV.

When $v_{y} = 0,$ the kinematic model of the robot will be

(\begin{matrix} {\overset{\cdot}{X}}_{r} \\ {\overset{\cdot}{Y}}_{r} \\ {\overset{\cdot}{θ}}_{r} \end{matrix}) = (\begin{matrix} v \cos (θ_{r}) \\ v \sin (θ_{r}) \\ w \end{matrix}) + (\begin{matrix} ε_{x_{r}} \\ ε_{y_{r}} \\ ε_{θ_{r}} \end{matrix})

(3)

The odometry depends on the assumption which is only true if there is no slipping in the robot’s wheels. The angular velocity delivered by the gyroscope can be integrated to provide the orientation ( $θ_{r} (k + 1) = θ_{r} (k) + w Δ T)$ . In this case, $w$ is obtained from the gyroscope which has a large drift error. It can be modeled by an exponential curve and determined experimentally as done by Barshan and Durrant-Whyte.¹⁹

When $v = v_{x},$ and by discretizing equation (3), the model will be written as follows

(\begin{matrix} X_{r} (k + 1) \\ Y_{r} (k + 1) \\ θ_{r} (k + 1) \end{matrix}) = (\begin{matrix} X_{r} (k) + Δ T v_{k} \cos (θ_{r} (k)) \\ Y_{r} (k) + Δ T v_{k} \sin (θ_{r} (k)) \\ θ_{r} (k) + Δ Tw (k) \end{matrix}) + (\begin{matrix} ε_{x_{r}} \\ ε_{y_{r}} \\ ε_{θ_{r}} \end{matrix})

(4)

$Δ T$ is time step and $ε_{x_{r} y_{r} θ_{r}}$ is the noise that arises from the encoder and wheels slipping. The robot evolution model reflects the relationship between the robot previous states $X_{R} (k)$ and its current state $X_{R} (k + 1) .$ Let $U_{k} = [v, w]^{T}$ be control vector, which is assigned with the robot (translation and rotation velocity). During navigation, the robot moves on a 2D plane. In SLAM, the system state vector has a position of the UGV $(X_{R})$ . It is represented by $X_{R} = [X_{r}, Y_{r}, θ_{r}]^{T} \in R^{3}$ and the map is presented by $m_{i} = [x_{1}, y_{1}; . . .; x_{M}, y_{M}]^{T} \in R^{2 M}$ with $M$ is the total number of landmarks. We can write equation (4) as follows

X_{R} (k + 1) = f (X_{R} (k), U (k)) + ε_{x_{r} y_{r} θ_{r}}

(5)

Direct observation point–based model

$m_{i}$ presents the landmark which is detected in the environment of the map. If a landmark $m_{i}$ is detected for the first time, it will be initialized and added to the state vector. If it is already observed, then the UGV uses it to update the robot pose state.^20,21 In the case of point-based landmark representation, each point of the scan is considered as landmark and is represented by two parameters $[ρ_{i}, β_{i}]$ .

The representation of point coordinates in the global frame according to its coordinates in the local frame is given by²⁰

{\begin{matrix} X_{m_{ig}} = X_{rl} \cos (θ_{r}) - Y_{rl} \sin (θ_{r}) + X_{r} \\ Y_{m_{ig}} = X_{rl} \sin (θ_{r}) + Y_{rl} \cos (θ_{r}) + Y_{r} \end{matrix}

(6)

The overall transformation to local inverse model is given as follows

{\begin{matrix} X_{rl} = (X_{m_{ig}} - X_{r}) \cos (θ_{r}) + (Y_{m_{ig}} - Y_{r}) \sin (θ_{r}) \\ Y_{rl} = - (X_{m_{ig}} - X_{r}) \sin (θ_{r}) + (Y_{m_{ig}} - Y_{r}) \cos (θ_{r}) \end{matrix}

(7)

where $(X_{m_{ig}}; Y_{m_{ig}})$ represents the coordinates of landmark in the global frame; $(X_{rl}; Y_{rl})$ represents the coordinates of landmark in the local frame; $(X_{r}, Y_{r})$ represents the position of the robot in the global frame; and $θ_{r}$ represents the orientation of the robot.

The subscript $i$ is the $i th$ sample in landmark sample sets. The observation is given by $Z_{ik} = [ρ_{ik}, β_{ik}]^{T}$ . It contains the range and bearing of a landmark in the local frame. The direct observation model is expressed as

Z_{i} = [\begin{matrix} \sqrt{{(X_{m_{ig}} - X_{r})}^{2} + {(Y_{m_{ig}} - Y_{r})}^{2}} \\ ta n^{- 1} (\frac{Y_{m_{ig}} - Y_{r}}{X_{m_{ig} - X_{r}}}) - θ_{r} \end{matrix}] + [\begin{matrix} ε_{ρ_{i}} \\ ε_{β_{i}} \end{matrix}]

(8)

where $ε_{ρ_{i}}$ is the noise of measured range and $ε_{β_{i}}$ is the noise of bearing in the UGV local coordinate frame.

Inverse observation point–based model

The landmark mapping model is an inverse observation model. Knowing the robot state and the landmark coordinate, the inverse observation model $X_{m_{new}}$ can be given by^20,22

X_{m_{new}, k} = h_{i}^{- 1} (X_{k}, Z_{ik})

(9)

X_{m_{new}} (k) = [\begin{matrix} X_{r} + ρ_{i} \cos (θ_{r}) \cos (β_{i}) - ρ_{i} \sin (θ_{r}) \sin (β_{i}) \\ Y_{r} + ρ_{i} \sin (θ_{r}) \cos (β_{i}) + ρ_{i} \cos (θ_{r}) \sin (β_{i}) \end{matrix}]

(10)

where $h$ represents a continuous observation model.

Direct observation line–based model

The direct observation model is given by^20,23

Z_{i, line} = [\begin{matrix} ρ_{il} \\ β_{il} \end{matrix}] = [\begin{matrix} ρ_{ig} - \sqrt{X_{r}^{2} + Y_{r}^{2}} \cos (β_{ig} - ta n^{- 1} (\frac{Y_{r}}{X_{r}})) \\ β_{ig} - θ_{r} \end{matrix}]

(11)

$[ρ_{il}, β_{il}]^{T}$ is the polar parameters in the local coordinate frame of the ith line feature and $[ρ_{ig}, β_{ig}]^{T}$ the polar parameters in the global coordinate frame of the ith line feature.

Inverse observation line–based model

The mapping model is an inverse observation model $m_{new, line}$ which can be written as follows

m_{new, line} = [\begin{matrix} ρ_{ig} \\ β_{ig} \end{matrix}] = [\begin{matrix} ρ_{il} + \sqrt{X_{r}^{2} + Y_{r}^{2}} \cos (β_{il} - ta n^{- 1} (\frac{Y_{r}}{X_{r}})) \\ β_{il} + θ_{r} \end{matrix}]

(12)

There are many types of features association “line to line,”^24,25“point to line,”²⁶ and “point to point.”^24,27 In this article, we will do a comparative’s survey between two types of association “point to point” and “line to line.” Since the processing time of the EKF/SVSF-SLAM approach “point to point” is very important and allocates a large memory space, thus, in the association step, we propose to use a method based on “line to line” that is quicker than the standard “point-to-point” approach. In our work, we used the method “Split and Merge” which is largely used for line extraction.^24,25

EKF-SLAM algorithm

The discretization form of the system and nonlinear model measurement used for state estimation can be written as follows

X_{k + 1} = f (X_{k}, U_{k}) + ε_{x_{r}, y_{r}, θ_{r}}

(13)

Z_{k + 1} = h (X_{k}) + ε_{ρ_{i}, β_{i}}

(14)

The EKF provides an approximation of the optimal estimate. $X_{k + 1}$ is the state of the vector and $U_{k}$ is the input control vector. The objective of the EKF-SLAM problem is to recursively estimate the state $X_{k}$ of the landmark as stated by the measurement $Z_{k + 1}$ . The noise that arises from the encoder, wheels slipping is represented by $ε_{x_{r}, y_{r}, θ_{r}}$ and $ε_{ρ_{i}, β_{i}}$ are the noise of measured range and the noise of bearing in the local frame which are having recognized by the covariances $Q_{k}$ and $R_{k}$ , respectively. The EKF Steps are presented as follows:

Initial estimates for

Project the state ${\hat{X}}_{k + 1}$ ahead

{\hat{X}}_{k + 1} = f ({\hat{X}}_{k}, U_{k + 1}) + \nabla F_{X} (X_{k} - {\hat{X}}_{k / k}) + ε_{x_{r}, y_{r}, θ_{r}}

(15)

Prediction

Project the error covariance ahead. The prediction covariance matrix $P_{k + 1 / k}$ is written as follows

P_{k + 1 / k} = \nabla F_{X} P_{k / k} \nabla {F_{X}}^{T} + \nabla F_{U} Q_{k} \nabla {F_{U}}^{T}

(16)

where $\nabla F_{X}$ and $\nabla F_{U}$ be the Jacobian matrices of $f (.)$ with respect to $X_{k + 1}$ evaluated at an elsewhere specified point denoted by

\nabla F_{X} = [\begin{matrix} J_{1} & 0 & \dots & 0 \\ 0 & 1 & \dots & 0 \\ ⋮ & ⋮ & ⋱ & ⋮ \\ 0 & 0 & \dots & 1 \end{matrix}], \nabla F_{U} = [\begin{matrix} J_{2} \\ 0 \\ ⋮ \\ 0 \end{matrix}]

where

$J_{1} = [\frac{\partial f (.)}{\partial x_{r}}; \frac{\partial f (.)}{\partial y_{r}}; \frac{\partial f (.)}{\partial Θ_{r}}] = [\begin{matrix} 1 & 0 & - v \sin (θ_{r}) Δ T \\ 0 & 1 & v \cos (θ_{r}) Δ T \\ 0 & 0 & 1 \end{matrix}]$

J_{2} = [\frac{\partial f (.)}{\partial v}; \frac{\partial f (.)}{\partial w}] = [\begin{matrix} \cos (θ_{r}) Δ T & 0 \\ \sin (θ_{r}) Δ T & 0 \\ 0 & Δ T \end{matrix}]

Q_{k} = [\begin{matrix} σ_{v}^{2} & 0 \\ 0 & σ_{w}^{2} \end{matrix}], R_{k} = [\begin{matrix} σ_{ρ_{i}}^{2} & 0 \\ 0 & β_{i}^{2} \end{matrix}]

Observation and update

Linearization of the observation model ${\hat{Z}}_{k + 1}$ around ${\hat{X}}_{k + 1 / k}$

{\hat{Z}}_{k + 1} = h ({\hat{X}}_{k + 1 / k}) + H_{k + 1} ({\hat{X}}_{k + 1 / k} - X_{k + 1}) + ε_{ρ_{i}, β_{i}}

(17)

The update cycle of the Kalman filter is applied to ${\hat{X}}_{k + 1 / k + 1}$ and $P_{k + 1 / k + 1}$

Compute the Kalman gain $K_{k + 1}$

K_{k + 1} = P_{k + 1 / k} H_{k + 1}^{T} [H_{k + 1} P_{k + 1 / k} H_{k + 1}^{T} + R_{k}]^{- 1}

(18)

Update estimate with measurement $Z_{k + 1}$

{\hat{X}}_{k + 1 / k + 1} = {\hat{X}}_{k + 1 / k} + K_{k + 1} [Z_{k + 1} - H_{k + 1} {\hat{X}}_{k + 1 / k}]

(19)

Update error covariance $P_{k + 1 / k + 1}$

P_{k + 1 / k + 1} = [I - K_{k + 1} H_{k + 1}] P_{k + 1 / k}

(20)

Even as the environment is investigated, new features are detected and should be put on the accumulation map. In this case, the state vector and the output error estimate matrix are calculated from the new observation.

Map management

Add $a_{new}$ to the state vector ${\hat{X}}_{k + 1 / k + 1}$

Initialization of the covariance matrix $P_{new} \in R^{2 * 2}$ of $a_{new}$

P_{new} = \nabla h_{R}^{- 1} P_{R} (\nabla h_{R}^{- 1})^{T} + \nabla h_{Z}^{- 1} R_{k} (\nabla h_{Z}^{- 1})^{T}

where $\nabla h_{R}^{- 1}$ and $\nabla h_{Z}^{- 1}$ are the Jacobian matrices, denoted by

\nabla h_{R}^{- 1} = [\begin{matrix} 1 & 0 & - ρ_{i} \sin (θ_{r} + β_{i}) \\ 0 & 1 & ρ_{i} \cos (θ_{r} + β_{i}) \end{matrix}]

and

\nabla h_{Z}^{- 1} = [\begin{matrix} \cos (θ_{r} + β_{i}) & - ρ_{i} \sin (θ_{r} + β_{i}) \\ \sin (θ_{r} + β_{i}) & ρ_{i} \cos (θ_{r} + β_{i}) \end{matrix}]

Initialization of the inter-covariance matrix $P_{new, X} \in R^{2 * 3 M}$ of $a_{new}$

P_{new, X} = \nabla h_{R}^{- 1} P_{R, X}

Increment the state vector ${\hat{X}}_{Incremt}$

{\hat{X}}_{Incremt} = [{\hat{X}}_{k + 1}; a_{new}]

Increment the covariance matrix $P_{Incremt}$

P_{Incremt} = [\begin{matrix} P & P_{X, new} \\ P_{new, X} & P_{new} \end{matrix}]

$M = M + 1$ is the total number of current landmarks. The EKF diverge if the linearization consecutive is not good linearized.^26,28,29

SVSF-SLAM algorithm

The variable structure filter (VSF) was created in 2003, as a new estimator using a sliding mode concept where switching gain is used to ensure that the estimates converge to true state values.³⁰ As shown in Figure 5, the SVSF utilizes a switching gain to converge the estimates to within a boundary of the true state values (i.e. existence subspace).^31,32

Figure 5.

SVSF estimation concept.

A corrective term, referred to as the SVSF gain, is computed as a function of the error in the predicted output, as well as a gain matrix and the smoothing boundary layer width.^30,32,33 The corrected term calculated in equation (21) is then used in equation (24) to find a posteriori state estimate. The priori and posteriori output error estimates are considered as critical variables which are defined by equations (25) and (26), respectively^32,33 The estimation progression makes a summary of equations (21)–(28) and is repeated iteratively. $U_{k}$ is the input control vector. The gain is computed³² as follows

\begin{array}{l} K_{k + 1}^{S V S F} = {({\hat{H}}_{k + 1})}^{+} d i a g [(| e_{z_{k + 1 / k}} |_{A b s} + γ | e_{z_{k / k}} |_{A b s}) ° \\ S a t ({\bar{φ}}^{- 1} e_{z_{k + 1 / k}})] {[d i a g (e_{z_{k + 1 / k}})]}^{- 1} \end{array}

(21)

where ∘ represents “Schur” element-by-element multiplication; + refers to the pseudo inverse of a matrix; $H_{k + 1, j} = h_{k + 1, j} (F_{X, i})$ is the measurement matrix that is derivative of $h$ with respect to the state vector $X_{k + 1}$ ; we note that $h$ depends only on the robot pose $R_{k + 1}$ and the location of the $i th$ landmark, where $i$ is the detected landmark index at time $k$ and $j$ is the index of an individual landmark observation in the $h_{k + 1, j}$ . $F_{X, i}$ is calculated as follows

F_{X, i} = [\begin{matrix} 1 & 0 & 0 & 0 & \dots & 0 & 0 & 0 & 0 & \dots & 0 \\ 0 & 1 & 0 & 0 & \dots & 0 & 0 & 0 & 0 & \dots & 0 \\ 0 & 0 & 1 & 0 & \dots & 0 & 0 & 0 & 0 & \dots & 0 \\ 0 & 0 & 0 & 0 & \dots & 0 & 1 & 0 & 0 & \dots & 0 \\ 0 & 0 & 0 & 0 & \underset{2 i - 2}{\underset{︸}{\dots}} & 0 & 0 & 1 & 0 & \underset{2 M - 2 i}{\underset{︸}{\dots}} & 0 \end{matrix}]

(22)

This equation $(H_{k + 1})^{+}$ can be written as follows

$(H_{k + 1})^{+} = (F_{X, i})^{T} (h_{k + 1, j})^{+}$ , with the knowledge that

$h_{k + 1, j} = [ρ_{i}; β_{i}] = h (X_{k + 1, m_{i}})$ is the observation model of the SVSF-SLAM algorithm.

${\bar{φ}}^{- 1}$ is a diagonal matrix constructed from the smoothing boundary layer vector $φ$ , such that

${\bar{φ}}^{- 1} = [diag (φ)]^{- 1} = (\begin{matrix} \frac{1}{φ_{1}} & 0 & 0 \\ 0 & ⋱ & 0 \\ 0 & 0 & \frac{1}{φ_{M_{i}}} \end{matrix})$

with $M_{i}$ represents the number of measurements.

$Sat ({\bar{φ}}^{- 1} e_{z_{k + 1 / k}})$ represents the saturation function

Sat ({\bar{φ}}^{- 1} e_{z_{k + 1 / k}}) = {\begin{matrix} + 1, & \frac{e_{z_{i, k + 1 / k}}}{φ_{i}} \geq 1 \\ \frac{e_{z_{i, k + 1 / k}}}{φ_{i}}, & - 1 < \frac{e_{z_{i, k + 1 / k}}}{φ_{i}} < 1 \\ - 1, & \frac{e_{z_{i, k + 1 / k}}}{φ_{i}} \leq - 1 \end{matrix}

(23)

The update of the state estimates can be calculated as follows

{\hat{X}}_{k + 1 / k + 1} = {\hat{X}}_{k + 1 / k} + K_{k + 1}^{SVSF} {\hat{e}}_{z_{k + 1 / k}}

(24)

The priori and posteriori output error estimates are defined as follows

e_{z_{k + 1 / k}} = Z_{k + 1} - {\hat{Z}}_{k + 1 / k}

(25)

e_{z_{k + 1 / k + 1}} = Z_{k + 1} - {\hat{Z}}_{k + 1 / k + 1}

(26)

The SVSF filter provides a robust and stable estimate to modeling uncertainties and errors.^31,33

The calculation of the SVSF gain is needed to use two main parameters. The first parameter is $(γ)$ which control the speed of convergence, where the second $(φ)$ refers to the boundary layer width which is used to smooth out the switching action.^30,32 These parameters must be suitably selected. $γ$ is a diagonal matrix whose elements were set to the following^21,23

0 < γ_{i} \leq 1

(27)

As maintained by Sfeir,¹⁵ the estimation process is stable and converges to the existence subspace if the following circumstances are contented³¹

| e_{k / k} |_{Abs} > | e_{k + 1 / k + 1} |_{Abs}

(28)

Two association approaches are proposed: “point to point” and “line to line.” The steps of SVSF-SLAM algorithm is illustrated in the algorithm 1.^20,22 In this work, we also treat the problem of optimal control, focusing on two main methods which have proven to be effective on mobile robots: the Shooting and Bellman methods.

Figure 6.

Steps of the EKF/SVSF-SLAM principle.

Algorithm 1: SVSF-SLAM Algorithm
§ At each step time t $♢$ Initial Estimate ${\hat{X}}_{0}$ and $e_{z_{0}}$ $♢$ Prediction Time Update $♢ -$ Project state Ahead ${\hat{X}}_{k + 1 / k} = f ({\hat{X}}_{k / k}, U_{k})$ % state prediction using process model $♢$ Observation and Update $♢$ For all features observation $Z_{k + 1, j} .$ $♢$ if the correspondence is founded before seen the feature $♢ -$ Calculate a posteriori measurements error ${\hat{e}}_{z_{k + 1, i}} = Z_{k + 1, j} - {\hat{Z}}_{k + 1 / k, i}$ $(H_{k + 1, j})^{+} = (F_{X, i})^{T} (h_{k + 1, j})^{+}$ $♢ -$ Compute the SVSF Gain $K_{k + 1}^{SVSF} .$ $K_{k + 1}^{SVSF} = ({\hat{H}}_{k + 1, j})^{+} diag [(\| {\hat{e}}_{z_{k + 1, i}} \|_{Abs} +$ $γ \| e_{z_{k, i}} \|_{Abs}) \circ Sat ({\bar{φ}}^{- 1} {\hat{e}}_{z_{k + 1, i}})] [diag (e_{z_{k + 1, i}})]^{- 1}$ $♢ -$ Update of the State estimate vector ${\hat{X}}_{k + 1 / k + 1} .$ ${\hat{X}}_{k + 1 / k + 1} = {\hat{X}}_{k + 1 / k} + K_{k + 1}^{SVSF} {\hat{e}}_{z_{k + 1, i}}$ $♢ -$ calculate a priori measurements error vector $e_{z_{k + 1, i}} = z_{k + 1, j} - h ({\hat{R}}_{k + 1}, m_{i})$ $♢$ End if $♢$ End For $♢$ Map Management $♢$ For all non-associated features. $♢ -$ Initial a new feature $a_{new} .$ $a_{new} = h^{- 1} ({\hat{X}}_{k + 1 / k + 1}, Z_{k + 1 / k})$ $♢ -$ Add $a_{new}$ to the state vector ${\hat{X}}_{k + 1 / k + 1}$ . $♢ -$ Use equation (28) to initialize a posteriori measurements error $e_{Z_{k + 1, new}}$ of the new landmark. $e_{Z_{k + 1, new}} = Z_{k + 1} - h ({\hat{X}}_{k + 1 / k + 1}, a_{new})$ $♢ -$ Add $e_{Z_{k + 1, new}}$ to a posteriori measurement error vector $e_{Z_{k + 1, i}} .$ $♢ -$ Increment the state vector ${\hat{X}}_{Incremt}$ ${\hat{X}}_{Incremt} = [{\hat{X}}_{k + 1}; a_{new}]$ $M = M + 1$ % total number of current landmarks. $♢$ End For § End

Algorithm 1: SVSF-SLAM Algorithm

§ At each step time t

♢

Initial Estimate

{\hat{X}}_{0}

and

e_{z_{0}}

♢

Prediction Time Update

♢ -

Project state Ahead

{\hat{X}}_{k + 1 / k} = f ({\hat{X}}_{k / k}, U_{k})

% state prediction using process model

♢

Observation and Update

♢

For all features observation

Z_{k + 1, j} .

♢

if the correspondence is founded before seen the feature

♢ -

Calculate a posteriori measurements error

{\hat{e}}_{z_{k + 1, i}} = Z_{k + 1, j} - {\hat{Z}}_{k + 1 / k, i}

(H_{k + 1, j})^{+} = (F_{X, i})^{T} (h_{k + 1, j})^{+}

♢ -

Compute the SVSF Gain

K_{k + 1}^{SVSF} .

K_{k + 1}^{SVSF} = ({\hat{H}}_{k + 1, j})^{+} diag [(| {\hat{e}}_{z_{k + 1, i}} |_{Abs} +

γ | e_{z_{k, i}} |_{Abs}) \circ Sat ({\bar{φ}}^{- 1} {\hat{e}}_{z_{k + 1, i}})] [diag (e_{z_{k + 1, i}})]^{- 1}

♢ -

Update of the State estimate vector

{\hat{X}}_{k + 1 / k + 1} .

{\hat{X}}_{k + 1 / k + 1} = {\hat{X}}_{k + 1 / k} + K_{k + 1}^{SVSF} {\hat{e}}_{z_{k + 1, i}}

♢ -

calculate a priori measurements error vector

e_{z_{k + 1, i}} = z_{k + 1, j} - h ({\hat{R}}_{k + 1}, m_{i})

♢

End if

♢

End For

♢

Map Management

♢

For all non-associated features.

♢ -

Initial a new feature

a_{new} .

a_{new} = h^{- 1} ({\hat{X}}_{k + 1 / k + 1}, Z_{k + 1 / k})

♢ -

Add

a_{new}

to the state vector

{\hat{X}}_{k + 1 / k + 1}

♢ -

Use equation (28) to initialize a posteriori measurements error

e_{Z_{k + 1, new}}

of the new landmark.

e_{Z_{k + 1, new}} = Z_{k + 1} - h ({\hat{X}}_{k + 1 / k + 1}, a_{new})

♢ -

Add

e_{Z_{k + 1, new}}

to a posteriori measurement error
vector

e_{Z_{k + 1, i}} .

♢ -

Increment the state vector

{\hat{X}}_{Incremt}

{\hat{X}}_{Incremt} = [{\hat{X}}_{k + 1}; a_{new}]

M = M + 1

% total number of current landmarks.

♢

End For
§ End

Optimal control

In this section, we provide a path planning algorithm based on optimal control theory. In the beginning, we use the Shooting methods based on the Pontryagin’s principle.^16,34 In this principle, we determine an optimality equation, that is, a differential algebraic equation system is provided with an initial condition and a final condition and the adjoint equations. In other words, we should note that in the adjoint equation, derived from the Pontryagin’s principle, no information is given concerning the initial or the final conditions. Consequently, this equation is hard to be used numerically. Thus, in order to determine the initial condition of the adjoint state, we use the Shooting indirect method for the numerical procedure.³⁴ After this, we present the results of numerical experiments implemented using MATLAB facilities. For classical method problems, the solution can be obtained analytically or numerically. On the other hand, we use the Bellman principle which is the direct method and gives an appropriate condition to the optimality solution.^35–37

Numerical solution with the Shooting method

The optimal control problem of the robot is stated as follows: Find the control $u = (v, w)$ that minimizes the final time $(t_{f})$

J = t_{f} \to min

(29)

subject to the differential constraints

{\begin{matrix} {\overset{\cdot}{x}}_{r} = v \cos θ_{r} \\ {\overset{\cdot}{y}}_{r} = v \sin θ_{r} \\ {\overset{\cdot}{θ}}_{r} = w \end{matrix}

(30)

The prescribed boundary conditions

\begin{matrix} t_{0} = 0, x_{r 0} = 0, y_{r 0} = 0, θ_{r 0} = 0 \\ x_{rf} = 0, y_{rf} = 0, θ_{rf} = 0 \end{matrix}

(31)

U = {(v, w) / - 1 \leq v \leq 1, - 1 \leq w \leq 1}

(32)

Equation (29) is equivalent to

\int_{0}^{t_{f}} dt \to \min

(33)

The Hamiltonian is given by

H (t, x (t), p (t), u (t)) = p_{1} (v \cos θ_{r}) + p_{2} (v \sin θ_{r}) + p_{3} w - 1

The Euler–Lagrange equations are given by

{\begin{matrix} {\overset{\cdot}{x}}_{r} = \frac{\partial H}{\partial p_{1}} \\ {\overset{\cdot}{y}}_{r} = \frac{\partial H}{\partial p_{2}} \\ {\overset{\cdot}{θ}}_{r} = \frac{\partial H}{\partial p_{3}} \\ {\overset{\cdot}{p}}_{1} = - \frac{\partial H}{\partial x_{r}} \\ {\overset{\cdot}{p}}_{2} = - \frac{\partial H}{\partial y_{r}} \\ {\overset{\cdot}{p}}_{3} = - \frac{\partial H}{\partial θ_{r}} \\ 0 = - \frac{\partial H}{\partial v} \\ 0 = - \frac{\partial H}{\partial w} \end{matrix}

{\begin{matrix} {\overset{\cdot}{x}}_{r} = v \cos θ_{r} \\ {\overset{\cdot}{y}}_{r} = v \sin θ_{r} \\ {\overset{\cdot}{θ}}_{r} = w \\ {\overset{\cdot}{p}}_{1} = 0 \\ {\overset{\cdot}{p}}_{2} = 0 \\ {\overset{\cdot}{p}}_{3} = - [- p_{1} v \sin θ_{r} + p_{2} v \cos θ_{r}] \end{matrix}

From Hamiltonian $H,$ a control $u = (v, w)$ verifies

\begin{array}{l} H (t, x (t), p (t), u (t)) = \min_{u \in U} H (t, x (t), p (t), \\ u (t)) = \min_{{- 1 \leq v \leq 1, - 1 \leq w \leq 1}} H (t, x (t), p (t), u (t)) \end{array}

The following equation is given to find a control $U$

\frac{\partial H}{\partial v} = p_{1} \cos θ_{r} + p_{2} \sin θ_{r}, \frac{\partial H}{\partial w} = p_{3}

then

\begin{array}{l} p_{1} (t) c o s θ_{r} + p_{2} (t) s i n θ_{r} = \min_{{- 1 \leq v \leq 1}} \\ (p_{1} (t) c o s θ_{r} + p_{2} (t) s i n θ_{r}), p_{3} (t) = \min_{{- 1 \leq w \leq 1}} p_{3} (t) \end{array}

Consequently, we deduce

v (t) = sign (p_{1} \cos θ_{r} + p_{2} \sin θ_{r}), w (t) = sign (p_{3} (t))

In the numerical solution, we used the Shooting indirect method to determine a final time. Then we have to solve the following system

{\begin{matrix} {\overset{\cdot}{υ}}_{1} = v \cos υ_{3} \\ {\overset{\cdot}{υ}}_{2} = v \sin υ_{3} \\ {\overset{\cdot}{υ}}_{3} = w \\ {\overset{\cdot}{υ}}_{4} = 0 \\ {\overset{\cdot}{υ}}_{5} = 0 \\ {\overset{\cdot}{υ}}_{6} = υ_{4} v \sin υ_{3} - υ_{5} v \cos υ_{3} \\ v = sign (p_{1} \cos θ_{r} + p_{2} \sin θ) \\ w = sign (p_{3} (t)) \\ υ_{1} (0) \in R, υ_{2} (0) \in R, υ_{3} (0) \in R \\ υ_{4} (0) \in R, υ_{5} (0) \in R, υ_{6} (0) \in R \end{matrix}

Let $υ (t, x_{r 0}, y_{r 0}, θ_{r 0}, p_{1}, p_{2}, p_{3})$ be the solution of the previous system at time $t$ with the initial condition $(υ_{1} (0), υ_{2} (0), υ_{3} (0), υ_{4} (0), υ_{5} (0), υ_{6} (0))$ . We construct a Shooting function which is a nonlinear algebraic equation of the variable $ψ$ at time $t = 0$ . The equation of Shooting method is computed by integrating the differential equation (using a numerical solution for example Euler method, Runge–Kutta method). The Shooting function is defined by

φ (p) = (\begin{matrix} υ_{1} (t_{f}, x_{r 0}, y_{r 0}, θ_{r 0}, p_{1}, p_{2}, p_{3}) - x_{rf} \\ υ_{2} (t_{f}, x_{r 0}, y_{r 0}, θ_{r 0}, p_{1}, p_{2}, p_{3}) - y_{rf} \\ υ_{3} (t_{f}, x_{r 0}, y_{r 0}, θ_{r 0}, p_{2}, p_{3}, p_{4}) - θ_{rf} \end{matrix})

Solving the problem, we can write: Find $p (0)$ such that $φ (p (0))$ gives the desired value of $x (t_{f}) = (x_{rf}, y_{rf}, θ_{rf})$ . The algorithm for numerical solution of this problem will then be completely defined if one gives oneself:

The integration algorithm of a differential system with initial condition (i.e. the Euler or Runge–Kutta procedure) to compute the Shooting function $φ$ (implemented in “ode45” of MATLAB which is a method of Runge–Kutta 4/5 with variable pitch).

The solution algorithm $φ (υ) = 0$ which in our case uses the quasi-Newton method (implemented in “fsolve” of MATLAB).

Numerical solution with the Bellman method

In this section, we use the Bellman principle to solve the same system with free final time which is the direct method. We consider the control of a discrete time dynamical system

{\begin{matrix} x_{r} (i + 1) = x_{r} (i) + t_{f} / Nv (i) \cos (θ_{r} (i)) \\ y_{r} (i + 1) = y_{r} (i) + t_{f} / Nv (i) \sin (θ_{r} (i)) \\ θ_{r} (i + 1) = θ_{r} (i) + tf / Nw (i) \end{matrix}

(34)

where $i = \bar{1, N}$ , $N$ is a number of steps. The values function is defined by

t_{f} = \inf {t | x (.) = (x_{r} (.), y_{r} (.), thet a_{r} (.))}

(35)

where $x (.)$ is solution of equation (34). To determine a control $(v, w)$ , we use the Pontryagin’s principle.

{\begin{matrix} p_{1} (j + 1) = p_{1} (j) + t_{f} / N \times 0 \\ p_{2} (j + 1) = p_{2} (j) + t_{f} / N \times 0 \\ p_{3} (j + 1) = p_{3} (j) + t_{f} / N \times (p_{1} (j + 1) v (i) \sin (θ (i)) \\ + p_{2} (j + 1) v (i) \cos (θ (i))) \end{matrix}

(36)

where $i = \bar{1, N}, j = \bar{1, N - i + 1}$ , and $t_{f}$ is the final time established by each method (SVSF-SLAM, EKF-SLAM, and the odometer). The initial condition of adjoint system is the values founded by the Shooting method. Then, the control $(v, w)$ is defined as follows

{\begin{matrix} \begin{matrix} v (i) = sign (p_{1} (i) \cos (θ_{r} (i)) + p_{2} (i) \sin (θ (i))) \\ w (i) = sign (p_{3} (i)) \end{matrix} \end{matrix}

(37)

where $i = \bar{1, N}$ .

Results and discussion

SLAM results

Simulation results

The simulation results of the proposed SVSF-SLAM algorithm for UGV localization problem are presented. The results of our proposed algorithm are compared with EKF-SLAM algorithm. The sampling rates used for each filter and sensors used in this study are as follows²⁰

f_{odometer} = f_{laser} = f_{EKF - SLAM} = f_{SVSF - SLAM} = 10 Hz

Figure results which are presented show the estimated position of the UGV using EKF and SVSF, with $σ_{x} = σ_{y} = 10^{- 4} m, σ_{θ} = 10^{- 4} rad$ , the convergence rate matrix $γ = diag (0.8, 0.8)$ , and the width of the smoothing boundary layer vector used is $φ = [10; 12]$ .

Test 1: zero-mean Gaussian noise

In the first experiment, we use a white centered Gaussian noise for process and observation model where $σ_{v} = 0.1 m / s, σ_{w} = 0.25 rad / s, σ_{ρ} = 0.1 m, σ_{β} = 0.25 rad$

Q_{k} = [\begin{matrix} {(0.1)}^{2} & 0 \\ 0 & {(0.25)}^{2} \end{matrix}], R_{k} = [\begin{matrix} {(0.1)}^{2} & 0 \\ 0 & {(0.25)}^{2} \end{matrix}]

Test 2: colored noise

In this experiment, we use a white noise with bias for process and observation model where $σ_{v} = 0.1 m / s, σ_{w} = 0.08 rad / s, σ_{ρ} = 0.045 m, σ_{β} = 0.045 rad$

Q_{k} = [\begin{matrix} {(0.1)}^{2} & {(0.045)}^{2} \\ {(0.045)}^{2} & {(0.08)}^{2} \end{matrix}], R_{k} [\begin{matrix} {(0.045)}^{2} & {(0.03)}^{2} \\ {(0.03)}^{2} & {(0.045)}^{2} \end{matrix}]

Figure 7 shows proposed algorithms and provide accurate positions when process and measurement noise are zero-mean and Gaussian. The position errors are shown in Figure 8. We note that the EKF-SLAM algorithm gives higher quality in estimation error terms compared with SVSF-SLAM algorithm. At 1550 iteration, we note significant increase of the SVSF-SLAM algorithm consistency in comparison to EKF-SLAM algorithm caused by closing-loop detection. The robot closes the loop making the error in the environment to decrease at 1550 iteration when it frequently moves into pre-visited regions.

Figure 7.

Pose estimation with zero-mean Gaussian noise.

Figure 8.

Error position of the robot with zero-mean Gaussian noise.

In the first test, by looking at the root mean square error (RMSE) of EKF/SVSF-SLAM algorithms, within the sight of zero-mean Gaussian noise, we can show that the EKF notices better results with zero-mean Gaussian noise. This can be found in Figure 9. EKF-SLAM algorithm gives the best position results and is more accurate than SVSF-SLAM algorithm. This can be translated as follows: In this case, the EKF gives a good accuracy when the process and observation noise are uncorrelated zero-mean Gaussian with known covariance. On other hand, when the control noise is colored, the EKF-SLAM algorithm is unsuccessful with high error in comparison to the SVSF-SLAM algorithm. For this situation, the map created by the proposed SVSF-SLAM algorithm with colored noise is not debased in quality as can be seen in Figure 10.

Figure 9.

Position RMSE under zero-mean Gaussian noise.

Figure 10.

Pose estimation with colored noise.

The position errors are appeared in Figure 11, we note that the SVSF-SLAM algorithm performs altogether superior to the EKF-SLAM algorithm in terms of estimation error. The SVSF is in a position to get the best of information in a short time and come up with a relatively estimate.

Figure 11.

Error position of the robot with colored noise.

Figure 12 puts in view the evaluation results of an investigation in comparison to the convergence RMSE of both algorithms. As for the results of the second experiment in comparison to the RMSE of EKF/SVSF-SLAM algorithms, in appearance of colored noise, we note that the RMSE showed the best outcomes with colored noise as can be seen from Figure 12. SVSF-SLAM algorithm is more accurate than EKF-SLAM algorithm in terms of position.

Figure 12.

Position RMSE under colored noise.

The structure of SVSF-SLAM algorithm reveals that it reduces the computational complexity of the algorithm. One inconvenient of the EKF-SLAM algorithm is the use of the full dimension of the state vector to update the pose and the map estimation. In this way, the computation time will increment with the size of the state vector. On the other hand, the SVSF-SLAM algorithm uses just the posture of UGV and the current feature to update the map and the pose estimation which makes the computational time independent of the size of the state vector. The SVSF-SLAM algorithm remains nearly constant per iteration depending on the number of visible landmarks (Figure 13).

Figure 13.

Computational time comparison.

Experimental results

Figure 14 shows the visited places (three (03) rooms (see (a), (b), and (c)) + corridor (see (d)) of the building (indoor environment). We made a trip over a distance of $100 m$ with a speed of $0.5 m / s$ .

Figure 14.

Shots of visited places.

When the robot moves in the map, it uses a laser to detect landmarks such as points and lines. In this experiment, the maximum range of the SICK laser is $15 m$ . Each scan provides $361$ points which are represented in polar coordinates. They are spread over a range from $0 \circ$ to $180 °$ . The experimental used data are collected from experimented locations. This environment is used for the algorithm application SLAM UGV with metric maps.

Trajectory estimated by the SVSF-SLAM algorithm, as shown in Figure 15, is more accurate in comparison to the EKF-SLAM algorithm. The SVSF-SLAM algorithm provides the best position in comparison to the EKF-SLAM algorithm. The SVSF-SLAM algorithm shows that our results are consistent and realistic in comparison to the EKF-SLAM algorithm. We see clearly as shown in Figure 15(a) that the pose errors of the EKF-SLAM algorithm increase significantly and the robot is so far and does not arrive to the initial position because of the previous errors made through its trajectory. Moreover, from Figure 15(b), at the final time, we can remark that the robot detects a common feature which improves the consistency of the SVSF-SLAM as well as the EKF-SLAM algorithm.

Figure 15.

Result of SVSF/EKF-SLAM algorithms based on point and line approaches in real time.

From the starting position to the final position (loop closure at $(0, 0)$ ), the SVSF-SLAM algorithm converges into a very accurate solution in comparison to the EKF-SLAM algorithm either with feature association–based point or with feature association–based line.

The structure of the SLAM algorithm–based line has been shown to decrease the computational time in comparison to the SLAM algorithm–based point which is proportional to the state vector length. EKF-SLAM algorithm allocates a large memory space and requires heavy significant computational time (see Figure 16).

Figure 16.

Computational time of EKF/SVSF-SLAM algorithms based on point and line approaches in real time.

In this environment, we note that the path given by the odometer is completely drift and this is due to the great range done by the robot. Besides, the SVSF-SLAM algorithm is better than EKF-SLAM algorithm. According to these results, we can confirm how a better localization of the robot makes the possibility of a coherent environment map building (see Figure 15).

Optimal control results

The path planning with optimal control

We have implemented the methods (Shooting and Bellman) using EKF/SVSF-SLAM localization. Table 1 depicts the results of the two methods we used to solve the SLAM localization. Numerical solution provides the following results with the Shooting and Bellman methods. To improve the position accuracy of the numerical solution of optimal control problems, we use the Bellman method compared to the Shooting method which has low accuracy and decreases the convergence of the robot position. We compare the true positions (Table 1) with the results given by the Shooting and Bellman methods (Tables 2 and 3), (Tables 4 and 5), and (Tables 6 and 7), respectively.

Table 1.

True and estimated positions for all different trajectories (EKF/SVSF-SLAM and odometer).

True position	Estimated position $(x_{r} (m), y_{r} (m), θ_{r} (rad))$
$(x_{r}, y_{r}, θ_{r})$	SVSF-SLAM	EKF-SLAM	Odometer
(0,0,0)	(0,0,0)	(0,0,0)	(0,0,0)
V = 0 (m/s), w = 0 (rad/s), t = 0 (s)
(4.90;0.70;0.29)	(4.87;0.85;0.30)	(4.84;0.75;0.32)	(4.92;0.67;0.27)
V = 0.35 (m/s), w = 0.02 (rad/s), t = 14.2 (s)
(7.20;1.61;0.70)	(7.16;1.81;0.70)	(7.04;1.98;0.84)	(7.30;1.50;0.59)
V = 0.2 (m/s), w = 0.13 (rad/s), t = 22.3 (s)
(7.77;2.80;1.58)	(7.77;3.03;1.571)	(7.43;3.22;1.77)	(8.02;2.61;1.42)
V = 0.2 (m/s), w = 0.13 (rad/s), t = 29.1 (s)
(6.13;4.54;2.43)	(6.19;4.77;2.37)	(5.43;4.57;2.64)	(6.72;4.69;2.21)
V = 0.2 (m/s), w = −0.15 (rad/s), t = 41.9 (s)
(5.80;6.16;1.10)	(5.93;6.33;1.05)	(4.73;6.01;1.38)	(6.80;6.32;0.81)
V = 0.2 (m/s), w = −0.15 (rad/s), t = 50.8 (s)
(8.52;8.27;1.42)	(8.67;8.41;1.39)	(7.15;8.62;1.59)	(10.06;7.47;1.03)
V = 0.3 (m/s), w = 0.01 (rad/s), t = 65.1 (s)
(8.53;10.33;2.52)	(8.75;10.39;2.48)	(6.64;10.73;2.69)	(10.97;9.34;2.00)
V = 0.1 (m/s), w = 0.2 (rad/s), t = 75.7 (s)
(−3.35;8.82;3.75)	(−3.17;9.28;3.71)	(−4.73;6.52;3.99)	(−0.50;13.42;3.24)
V = 0.1 (m/s), w = 0.2 (rad/s), t = 110.4 (s)
(−3.55;2.36;4.72)	(−3.92;2.73;4.62)	(−4.28;0.37;4.76)	(−3.07;7.58;4.42)
V = 0.3 (m/s), w = 0.2 (rad/s), t = 135.4 (s)
(−3.36;0.47;5.36)	(−3.85;0.74;5.28)	(−3.86; −1.53;5.41)	(−3.44;5.71;5.05)
V = 0.2 (m/s), w = 0.2 (rad/s), t = 142.9 (s)
(0.08;0.09;6.30)	(−0.38;0.26;6.25)	(0.09;0.02;6.42)	(−0.19;4.28;6.02)
V = 0.2 (m/s), w = 0.2 (rad/s), t = 155.1 (s)

Table 2.

The Shooting method (SVSF-SLAM).

$x_{r} (m)$	$y_{r} (m)$	$θ_{r} (rad)$
0	0	0
0.0104	0.0445	−0.0089
4.9620	1.8240	−0.0412
7.0336	2.1621	0.7743
8.6238	9.9804	−0.5499
6.5188	7.9142	2.2464
5.4368	12.5223	0.2323
7.7459	14.9626	0.0502
8.3090	21.4742	3.1229
−2.3921	16.0587	2.8101
−3.4041	2.4282	4.5972
−2.2606	1.8989	5.3109
−0.3574	0.7865	6.1960

Table 3.

The Bellman method (SVSF-SLAM).

$x_{r} (m)$	$y_{r} (m)$	$θ_{r} (rad)$
0	0	0
0.2347	−0.0126	14.0000
4.8701	0.8434	0.3049
7.1620	1.8106	0.7046
7.7710	3.0310	1.5713
6.1909	4.7703	2.3716
5.9325	6.3291	1.0534
8.6694	8.4055	1.3912
8.7541	10.3931	2.4790
−3.1722	9.2848	3.7126
−3.9216	2.7342	4.6181
−3.8461	0.7447	5.2782
−0.3823	0.2597	6.2493

Table 4.

The Shooting method (EKF-SLAM).

$x_{r} (m)$	$y_{r} (m)$	$θ_{r} (rad)$
0	0	0
0.0107	0.0358	−0.0094
5.0729	1.8867	0.0305
7.3223	1.6664	0.7740
9.4357	9.5588	2.8671
5.4607	6.0168	2.8377
3.9588	15.2038	15.6792
7.3412	14.7279	2.2658
6.7042	21.6882	3.2895
−3.4118	17.6088	2.3423
−3.3100	−0.0507	4.7511
0.5717	1.2401	5.2740
0.1316	0.8493	6.0910

Table 5.

The Bellman method (EKF-SLAM).

$x_{r} (m)$	$y_{r} (m)$	$θ_{r} (rad)$
0	0	0
−0.0241	0.1255	14.0000
4.8487	0.7518	0.3204
7.0405	1.9790	0.8427
7.4374	3.2203	1.7680
5.4386	4.5743	2.6401
4.7271	6.0134	1.3792
7.1512	8.6169	1.5855
6.6441	10.7262	2.6917
−4.7272	6.5180	3.9861
−4.2812	0.3738	4.7561
−3.8639	−1.5264	5.4088
0.0871	0.0242	6.4186

Table 6.

The Shooting method (odometer).

$x_{r} (m)$	$y_{r} (m)$	$θ_{r} (rad)$
0	0	0
0.0114	0.0412	−0.0094
5.0751	1.4653	0.0511
7.8570	0.6748	0.5899
9.9409	10.5034	0.4596
7.1834	7.8435	2.5400
7.4595	11.9491	−0.0820
9.3109	12.3822	−0.2362
10.5109	24.7952	3.1537
1.8898	28.5927	−15.8027
−2.3851	7.3407	4.6600
−2.3799	6.0224	5.2756
0.0734	5.2376	5.9845

Table 7.

The Bellman method (odometer).

$x_{r} (m)$	$y_{r} (m)$	$θ_{r} (rad)$
0	0	0
0.5441	0.1360	14.0000
4.9155	0.6565	0.2612
7.3070	1.4974	0.5892
8.0209	2.6101	1.4267
6.7214	4.6862	2.2099
6.8020	6.3225	0.8069
10.0647	7.4671	1.0277
10.9708	9.3438	2.0048
−0.5026	13.4171	3.2365
−3.0654	7.5815	4.4208
−3.4404	5.7083	5.0539
−0.1935	4.2843	6.0231

Through the two methods, best results are obtained by SVSF-SLAM compared to EKF-SLAM and the odometer; moreover, short time is required by the proposed approach (see Table 8). We can say that the result sets using the Bellman method are much better than the Shooting method. The Shooting method is not accurate compared to the Bellman method, because the initial conditions of adjoint state are sensible, that make us away from the correct positions. The solution for SLAM problem and the accuracy of the result are controlled by the accuracy of the Bellman method used as opposed to the Shooting method with the use of the Euler–Lagrange equations. The following figures show the results of optimal control with the Shooting and Bellman methods.

Table 8.

Final time with the Shooting and Bellman methods for three methods (EKF-SLAM, SVSF-SLAM, and odometer).

Method	Final time (s)
	Shooting method	Bellman method
SVSF-SLAM	158.6	156.5
EKF-SLAM	161.7	158
Odometer	163	162.4

The final time is the minimum time to come to the last time point corresponding at 155.1 s (final time) to the final position (0,0,0). From Table 9, note that the final time found by the Bellman method is better than the Shooting method. As can be seen from Table 8, the minimal final time is given with SVSF-SLAM localization. Then, we compared the true positions of our experiments for different positions of localization such as SVSF, EKF, and odometer. We determined the robot controls for each localization of two methods and we made a comparison between them. We notice that the robot control parameters using the Bellman method is better than Shooting method that the SVSF-SLAM localization is used. The results conform with our data (see Figures 17 –19).

Table 9.

Final time with the Shooting and Bellman methods for three methods (EKF-SLAM, SVSF-SLAM, and odometer).

Shooting method (EKF-SLAM)	Bellman method (EKF-SLAM)	Shooting method (SVSF-SLAM)	Bellman method (SVSF-SLAM).
327.5330 s	327.5330 s	111.3860 s	315.8968 s

Figure 17.

Control of V and W in an indoor environment–based point (SVSF-SLAM/EKF-SLAM/odometer).

Figure 18.

Control of V and W with the Shooting method in an indoor environment–based point (SVSF-SLAM).

Figure 19.

Control of V and W with the Bellman method in an indoor environment–based point (SVSF-SLAM).

From Figures 20 and 21, we noted that the results found using the Bellman principle are much better than the Shooting method, and the points $(x_{r}, y_{r})$ given by SVSF-SLAM are very close to the true points of the polygons. Consequently, SVSF-SLAM gives good results with the Bellman method (see Figure 22). For our experiment where the robot begins on the path, the trajectories produced by EKF-SLAM, SVSF-SLAM, and odometer were the same. Then on the contrary, the trajectories of EKF-SLAM and odometer bring the robot on the wrong path. Figures 23 –28 show the control of V and W with the Shooting and Bellman methods of EKF-SLAM, SVSF-SLAM, and odometer.

Figure 20.

Control optimal of UGV with the Shooting method: SVSF-SLAM, EKF-SLAM, and odometer.

Figure 21.

Control optimal of UGV with the Bellman method: SVSF-SLAM, EKF-SLAM, and odometer.

Figure 22.

Comparison of the Shooting and Bellman methods.

Figure 23.

Control of V with the Shooting and Bellman methods (EKF-SLAM).

Figure 24.

Control of W with the Shooting and Bellman methods (EKF-SLAM).

Figure 25.

Control of V with the Shooting and Bellman methods (SVSF-SLAM).

Figure 26.

Control of W with the Shooting and Bellman methods (SVSF-SLAM).

Figure 27.

Control of V with the Shooting and Bellman methods (odometer).

Figure 28.

Control of W with the Shooting and Bellman methods (odometer).

Table 9 shows the energy of the robot orientation $E = (\sum θ_{i}^{2})$ with different approaches. Smoothest trajectories present less energy (less maneuverability). As shown in this table, SVSF-SLAM presents less energy in both methods which confirm the obtained results.

Conclusion

In this article, SLAMPP algorithm have been proposed for UGV navigation. As this UGV operates at low speed, path planning and optimization strategies are developed without considering vehicle dynamics. Only the kinematics of the vehicle and the environment constraints are considered.

Based on SVSF-SLAM algorithm localization and optimal control theory, the proposed algorithm (SLAMPP) improves the path in terms of continuity by providing a smoother and more comfortable trajectory tracking. In addition, the proposed algorithm requires less computation time to achieve the improvement. The proposed approach is validated using simulation and experimental data. Several experiments are tested under realistic conditions and good performances are obtained by the SVSF-SLAM compared to the EKF-SLAM algorithm. As perspectives, we propose to extend the proposed approach for multiple robots (UGVs, UAVs, and AUVs).

Footnotes

Handling Editor: Francesco Massi

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) received no financial support for the research, authorship, and/or publication of this article.

References

Golombek

Cook

Economou

et al . Overview of the mars pathfinder mission and assessment of landing site predictions. Science 1997; 278: 1743–1748.

Thrun

Ferguson

Haehnel

et al . A system for volumetric robotic mapping of abandoned mines. In: Proceedings of the 2003 IEEE international conference on robotics and automation, Taipei, Taiwan, 14–19 September 2003. New York: IEEE.

Moutarlier

Chatila

. Stochastic multisensory data fusion for mobile robot location and environment modeling. In: Proceedings of the 5th international symposium of robotics research, Tokyo, Japan, 28–31 August 1989. MIT Press Cambridge.

Leonard

Newman

Rikoski

et al . Towards robust data association and feature modelling for concurrent mapping and localization. In: Jarvis

Zelinsky

(eds) Robotics research, vol. 6 (Springer tracts in advanced robotics). Berlin; Heidelberg: Springer, 2003, pp.7–20.

Williams

Dissanayake

Durrant-Whyte

. Constrained initialization of the simultaneous localization and mapping algorithm. In: Proceedings of the 3rd international conference on field and service robotics, Espoo, 10–13 June 2001. Helsinki, Finland: Yleissjhaljennhos-Painophorssi.

Guivant

Nebot

. Optimization of the simultaneous localization and map-building algorithm for real time implementation. IEEE T Robotic Autom 2001; 17: 242–257.

Durrant-Whyte

Bailey

. Simultaneous localisation and mapping: part I. IEEE Robot Autom Mag 2006; 13: 99–110.

Jensfelt

Kragic

Folkesson

et al . A framework for vision based bearing only 3D SLAM. In: Proceedings of the IEEE international conference on robotics and automation, Orlando, FL, 15–19 May 2006. New York: IEEE.

Davison

. Real-time simultaneous localisation and mapping with a single camera. In: Proceedings of the 9th IEEE international conference on computer vision, Nice, 13–16 October 2003. Washington, DC: IEEE.

10.

Montemerlo

Thrun

Koller

et al . FastSLAM 2.0: an improved particle filtering algorithm for simultaneous localization and mapping that provably converges. In: Proceedings of the 6th international joint conference on artificial intelligence, Acapulco, Mexico, 9–15 August 2003. San Francisco, CA: Morgan Kaufmann Publishers.

11.

Montemerlo

Thrun

Koller

et al . FastSLAM: a factored solution to the simultaneous localization and mapping problem. In: Proceedings of the AAAI 18th national conference on artificial intelligence, Edmonton, AB, Canada, 28 July–1 August 2002. Menlo Park, CA: American Association for Artificial Intelligence.

12.

Doucet

Freitas

NDE

Gordon

. Sequential Monte Carlo methods in practice (Statistics for engineering and information science). New York: Springer, 2001.

13.

Lemaire

Berger

Jung

et al . Vision-based SLAM: stereo and monocular approaches. Int J Comput Vision 2007; 74: 343–364.

14.

Arras

Siegwart

. Feature extraction and scene interpretation for map-based navigation and map building. In: Proceedings of the SPIE 3210, mobile robotics XII, Pittsburgh, PA, 25 January 1998. Bellingham, WA: SPIE.

15.

Sfeir

. Navigation D’un Robot Mobile En Environnement Inconnu Utilisant Les Champs De Potentiels Artificiels. Mémoire de maîtrise électronique, Génie de la production automatisée, École de Technologie Supérieure, Montréal, QC, Canada, 2009.

16.

Hull

. Optimal control theory for applications (Mechanical engineering series). New York: Springer-Verlag, 2003.

17.

Habibi

Burton

Chinniah

. Estimation using a new variable structure filter. In: Proceedings of the IEEE American control conference, Anchorage, AK, 8–10 May 2002. New York: IEEE.

18.

Robinett

III Eisler

Hurtado

. Applied dynamic programming for optimization of dynamical systems (Advances in design and control, Book Code: DC09, xviii-255). Philadelphia, PA: Society for Industrial and Applied Mathematics, 2005.

19.

Barshan

Durrant-Whyte

. Inertial navigation systems for mobile robots. IEEE T Robotic Autom 1995; 11: 328–342.

20.

Demim

Nemra

Louadj

. Robust SVSF-SLAM for unmanned vehicle in unknown environment. IFAC PapersOnLine 2016; 49: 386–394.

21.

Demim

Nemra

Louadj

et al . A new approach to improve the success and solving the UGVs Cooperation for SLAM Problem, using a SVSF Filter. In: Proceedings of the Mediterranean conference on pattern recognition and artificial intelligence, Tebessa, Algeria, 22–23 November 2016, pp.56–63. New York: ACM.

22.

Demim

Nemra

Louadj

et al . Simultaneous localization and mapping algorithm for unmanned ground vehicle with SVSF filter. In: Proceedings of the IEEE 8th international conference on modelling, identification and control, Algiers, Algeria, 15–17 November 2016. New York: IEEE.

23.

Demim

Boucheloukh

Nemra

et al . A new adaptive smooth variable structure filter SLAM algorithm for unmanned vehicle. In: Proceedings of the 6th international conference on systems and control, Batna, Algeria, 7–9 May 2017. New York: IEEE.

24.

Gutmanand

Schlegel

. AMOS: comparison of scan matching approaches for self-localization in indoor environments. In: Proceedings of the 1st Euromicro workshop on advanced mobile robot, Kaiserslautern, 9–11 October 1996. New York: IEEE.

25.

Yaqub

Tordon

Katubitiya

. Line segment based scan matching for concurrent mapping and localization of a mobile robot. In: Proceedings of the 9th international conference on control, automation, robotics and vision, Singapore, 5–8 December 2006. New York: IEEE.

26.

Diosi

Kleeman

. Laser scan matching in polar coordinates with application to SLAM. In: Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (IROS 2005), Edmonton, AB, Canada, 2–6 August 2005. New York: IEEE.

27.

Dissanayake

Newman

Durrant-Whyte

et al . An experimental and theoretical investigation into simultaneous localisation and map building. In: Corke

Trevelyan

(eds) Experimental robotics VI, vol. 250. 2nd ed. London: Springer, 2000, pp.265–274.

28.

Orderud

. Comparison of Kalman filter estimation approaches for state space models with nonlinear measurements. In: Proceedings of the Scandinavian conference on simulation and modeling, vol. 7491, Trondheim, 2005, pp. 7–9.

29.

Grewal

Andrews

. Kalman filtering: theory and practice using MATLAB. 2nd ed. New York: John Wiley & Sons, 1993.

30.

Habibi

Burton

. The variable structure filter. J Dyn Syst: T ASME 2003; 125: 287–293.

31.

Habibi

. The smooth variable structure filter. P IEEE 2007; 95: 1026–1059.

32.

Al-Shabi

. The general Toeplitz/observability SVSF. PhD Thesis, Department of Mechanical Engineering, McMaster University, Hamilton, ON, Canada, 2011.

33.

Gadsden

Dunne

Habibi

et al . Comparison of extended and unscented Kalman, particle, and smooth variable structure filters on a bearing-only target tracking problem. In: Proceedings of the SPIE 7445, signal and data processing of small targets, San Diego, CA, 3 September 2009, paper no. 74450B-74451. Bellingham, WA: SPIE.

34.

Trelat

. Contrôle Optimal: Théorie et Applications. Paris: Vuibert, Collection Mathématiques Concrètes, 2005.

35.

Bellman

Dreyfus

. Applied dynamic programming. Santa Monica, CA: The Rand Corporation, 1962.

36.

Mauch

. Dynamic programming. Berlin; Heidelberg: Springer, 2007.

37.

Dreyfus

. Some types of optimal control of stochastic systems. J Soc Ind Appl Math 1964; 2: 120–134.

Simultaneous localization,mapping,and path planning for unmanned vehicle using optimal control

Abstract

Keywords

Introduction

Process model of UGV

Direct observation point–based model

Inverse observation point–based model

Direct observation line–based model

Inverse observation line–based model

EKF-SLAM algorithm

SVSF-SLAM algorithm

Optimal control

Numerical solution with the Shooting method

Numerical solution with the Bellman method

Results and discussion

SLAM results

Simulation results

Experimental results

Optimal control results

The path planning with optimal control

Conclusion

Footnotes

Declaration of conflicting interests

Funding

References