# Research on SBMPC Algorithm for Path Planning of Rescue and Detection Robot

Authors: Lin-Lin Wang; Li-Xin Pan
Journal: Discrete Dynamics in Nature and Society
 (2020)

---

## Abstract

This research aims to improve autonomous navigation of coal mine rescue and detection robot, eliminate the danger for rescuers, and enhance the security of rescue work. The concept of model predictive control is introduced into path planning of rescue and detection robot in this paper. Sampling-Based Model Predictive Control (SBMPC) algorithm is proposed basing on the construction of cost function and predictive kinematics model. Firstly, input sampling is conducted in control variable space of robot motion in order to generate candidate path planning solutions. Then, robot attitude and position in future time, which are regarded as output variables of robot motion, can be calculated through predictive kinematics model and input sampling data. The optimum solution of path planning is obtained from candidate solutions through continuous moving optimization of the defined cost function. The effects of the three sampling methods (viz., uniform sampling, Halton’s sampling, and CVT  sampling) on path planning performance are compared in simulations. Statistical analysis demonstrates that CVT sampling has the most uniform coverage in two-dimensional plane when sample amount is the same for three methods. Simulation results show that SBMPC algorithm is effective and feasible to plan a secure route for rescue and detection robot under complex environment.


---

## Body

## 1. Introduction

As an important tool for processing coal mine accidents, rescue and detection robot can substitute rescuers to enter the accident area for implementation of detecting and rescuing. Real-time information can be transferred to rescue command center rapidly. Rescue and detection robot can provide scientific basis for the rescue decision, and the rescue plan can be made as accurately as possible. Therefore, it has critical significance for safe production in coal mine and can reduce the loss of public property and lives to develop rescue and detection robot.For recent years, a large number of research findings are obtained in path planning which is the key technology of robot to realize regional search. To some extent, path planning is regarded as a synthesis of global planning and local planning [1]. In practical application, the robot needs to select an optimal path under some certain criteria such as shortest path, minimal energy consumption, or time consuming at least. In the process of robot exploring along its desired path, local planning is generally implemented according to real-time information from sensor feedback, and motion states are often adjusted for realization of real-time obstacle avoidance.Path planning method based on random sampling has started in 1990 and originates from Randomized Potential Planner (RPP) algorithm which is firstly proposed by Barraquand and Latombe [2]. RPP is used for solving the problem of local minimum in artificial potential field method and improving planning efficiency in high-dimensional space. Currently, there are three main methods aiming at the problem of path planning for mobile robot with multidegree of freedom, namely, Probabilistic Roadmap Methods (PRM) algorithm [3], Rapidly Exploring Random Tree (RRT) algorithm [4], and Vector Field (VF) algorithm.The main idea of PRM algorithm is to construct a probabilistic roadmap through local planner firstly, then implement global path planning on this roadmap, and finally attempt to find a collision-free path with graph search algorithm. PRM has the advantage of easy operation, but random sampling points cannot be distributed uniformly in the whole space due to limited sampling in practical application. As a result, connectivity of roadmap is reduced, especially for narrow channel environment, where PRM often fails to find a feasible path. In order to solve this problem, researchers propose many enhanced algorithms for PRM. In [5], the coverage of a graph is defined as a performance index of its optimality as constructed by a sampling-based algorithm and an optimization algorithm is proposed which can maximize graph coverage in the configuration space. The simulation results confirm that the roadmap graph obtained by the proposed algorithm can generate results of satisfactory quality in path-finding tests under various conditions.The main idea of RRT algorithm is to extend search tree incrementally by browsing the state space in a rapid and uniform way. This method proved to be probability complete; namely, a collision-free path satisfying planning conditions must be found if the amount of nodes is larger than a certain value of threshold. In [6], a new method of dynamic replanning is proposed, which is based on an extended rapidly exploring random tree. An efficient method of node sharing is applied to allow the multirobot team to quickly develop path plans. Various experimental results in both single- and multirobot scenarios show the effectiveness of the proposed methods.The main idea of VF algorithm is similar to that of potential field algorithm, which has been widely used as a tool for path planning in the robotics community [7]. Vector fields are different from potential fields in that they do not necessarily represent the gradient of a potential [8]. Rather, the vector field simply indicates a desired direction of travel [9]. In [10], a method for accurate path following for miniature air vehicles is developed. The method is based on the notion of vector fields, which are used to generate the desired course inputs to inner-loop attitude control laws. Lyapunov’s stability analysis demonstrates asymptotic decay of path-following errors in the presence of constant wind disturbances. This method can be applied for path following of straight-line and circular arcs effectively.In addition to the above research findings, there are many new methods proposed for path planning [11, 12]. In [13], the theory of charged particles’ potential fields is utilized by assigning a potential function for each individual obstacle. The interaction of all scattered obstacles is integrated in a scalar potential surface (SPS) which strongly depends on the physical features of the mobile robot and obstacles. The achieved results demonstrate a feasible, fast, oscillation-free, and collision-free path planning of the proposed method. In [14], a new method called laser simulator (LS) is proposed in order to solve the path planning problem of a nonholonomic three-wheeled mobile robot (WMR). The path planning and roundabout detection are determined based on LS and sensor fusion of a laser range finder, camera, and odometry measurements. Experimental results show the capability of this proposed algorithms to robustly drive the robot.In the field of Model Predictive Control (MPC) application, there are also successful cases which are worthy of studying. In [15], nonlinear terminal sliding mode control (TSMC) and linear MPC are combined to solve the robust optimal three-axis attitude tracking problem of spacecrafts. The global stability and robust attitude tracking are guaranteed by this method. In [16], a disturbance rejection MPC scheme is developed for tracking nonholonomic vehicle with coupled input constraint and matched disturbances. The whole system is input-to-state stable if no information about the disturbance is available and can reach an offset-free tracking performance if the harmonic frequencies of the disturbance are known. In [17], MPC is used to model and implement controllers for dynamic encirclement of UAVs team. The contributions of this paper lie in the implementation of MPC to solve the problem of dynamic encirclement of a team of UAVs in real time and the application of theoretical stability analysis to the problem.In this paper, path planning is implemented through Sampling-Based Model Predictive Control (SBMPC) method combined with nonlinear kinematics model of rescue and detection robot. In SBMPC method, cost function with the form used by Model Predictive Control (MPC) [18] is adopted and input of control system is optimized. Different from traditional numerical optimization, SBMPC applies the method of objective-oriented optimization which is widely used in robotics and artificial intelligence field. The process of path planning through SBMPC algorithm is deduced in this paper, and the selection of sampling methods is also analyzed from the viewpoint of effect on planning performance.


## 2. Design of Path Planning Based on SBMPC

### 2.1. Kinematics Model of Rescue and Detection Robot

In this paper, rescue and detection robot is regarded as the research subject. As shown in Figure1, xoy denotes global reference frame and xRCyR denotes local reference frame. C denotes the center of robot mass, vt is robot forward speed, and ωt is robot angular speed.Figure 1
Rescue and detection robot in global reference frame.Kinematics model of rescue and detection robot [19] can be described as(1)x˙y˙θ˙=r⋅cosθ4r⋅cosθ4r⋅sinθ4r⋅sinθ4−r2lr2l⋅ϕ˙1ϕ˙2,where x,y is the robot position in global reference frame and θ is the angle deviation between local reference frame and global reference frame. r is the diameter of motivation wheel, l is the distance from left wheel to right wheel, ϕ˙1 is the rotation speed of left wheel, and ϕ˙2 is the rotation speed of right wheel. In this paper, r is 0.5 m and l is 0.8 m. Equation (1) can be expressed briefly as(2)X˙=B⋅U,where(3)X=xyθT,B=r⋅cosθ4r⋅cosθ4r⋅sinθ4r⋅sinθ4−r2lr2l,U=ϕ˙1ϕ˙2T.Equation (2) is the state equation of robot motion. X is the state variable, U is the control input variable, and B is the control input matrix. In the process of path planning, robot attitude and position is regarded as output variable of robot motion, which can be defined as(4)Y=xyθT.Therefore, the output equation of robot motion is expressed as(5)Y=X.Equations (2) and (5) are the kinematics model of rescue and detection robot.


### 2.2. Predictive Model of Motion Planning

Derivatives ofx, y, and θ at the time k can be calculated from equation (1) and expressed as(6)x˙k=r⋅cosθk4⋅ϕ˙1k+ϕ˙2k,(7)y˙k=r⋅sinθk4⋅ϕ˙1k+ϕ˙2k,(8)θ˙k=r2l⋅−ϕ˙1k+ϕ˙2k,k=1,2,3,…,where ϕ˙1k is the rotation speed of left wheel at the time k and ϕ˙2k is the rotation speed of right wheel at the time k. xk+1, yk+1, and θk+1 can be calculated through integration and expressed as(9)xk+1=xk+x˙k⋅T,(10)yk+1=yk+y˙k⋅T,(11)θk+1=θk+θ˙k⋅T,where T is the sampling period.Equations (6)∼(11) constitute the predictive model of robot motion and can be used for calculating robot attitude and position in future time through iterative computation.


### 2.3. Cost Function of Motion Planning

If Shannon’s sampling constraints [20] are satisfied, the cost function of robot motion planning based on SBMPC can be described as(12)minUk,…,Uk+N−1,NJ=∑i=0NYk+i+1−Yk+iQi+∑i=0N−1Uk+i−Uk+i−1Si.Variables in equation (12) should satisfy the following inequality conditions; viz.,(13)Yk+N−G0≤ε,(14)Qi≥0,Si>0.In equation (12), the term Yk+i+1−Yk+iQi represents the edge cost of planning path between the current predictive output Yk+i and the next predictive output Yk+i+1. The term Uk+i−Uk+i−1Si represents the energy consumption in robot motion process. In the constraint (13), G0 denotes the goal state which is used for construction of the terminal constraint of path planning. ε is a small positive number which represents the proximity of robot output to the goal state. The constraint (13), which reflects the requirement that robot should reach the target G0 with the accuracy ε, is a key condition for ensuring the convergence of planning algorithm. Qi and Si are the orders of corresponding norms. Therefore, the goal-directed optimization is adopted here, and in this method, the goal is implicitly considered through the calculation of a rigorous lower bound for the cost from a particular state to G0.


### 2.4. Principle of Motion Planning Based on Sampling

Sampling-based motion planning algorithms include Rapidly Exploring Random Trees (RRTs) and randomizedA∗ algorithms [21]. A common feature of these algorithms is that they are applied in the output space of robot and used for generating samples through various strategies. In essence, as shown in Figure 2, sampling-based motion planning methods are applied by using sampling to construct a tree that connects the root (initial state) with a goal region.Figure 2
Diagram of tree connecting the root with a goal region.Sampling-based path planning algorithm is shown in Figure3.Figure 3
Diagram of sampling-based path planning algorithm.Sampling-based path planning algorithm follows the steps below.Step 1. Initialization: GV,E represents a search graph where V denotes the set of nodes and E denotes the set of edges. During the initialization, V only contains start node and E does not contain any edge.Step 2. Node selection: select a node τ in V, which is regarded as the current node.Step 3. New edge generation: generate corresponding routes Γs for all candidate nodes τnew in Cfree (the set of candidate nodes). Γs satisfies the following constraints:(15)Γs0=τ,Γs1=τnew.If the routeΓs does not exist for a certain candidate node, select the next candidate node to generate its corresponding route. Until above processing covers all candidate nodes, then go to Step 4.Step 4. New edge insertion: all the routes Γs generated in Step 3 are regarded as new edges connecting the node τ to the node τnew and incorporated to E. The node τnew is also incorporated to V if V does not contain τnew.Step 5. Search for path planning solutions: start from the current node τ in the graph GV,E and search for the optimal edge as the best route for node refreshing; meanwhile, the terminal node τnew of the optimal edge is regarded as the starting node of planning path in the next time.Step 6. Return to Step 2. Repeat the above steps until τnew arrives at the goal node or the path planning time is over.The novelty of SBMPC method is mainly reflected in three aspects, i.e., input sampling, implicit state grid, and goal-directed optimization, which are described in the following parts.


### 2.5. Input Sampling

There are two primary disadvantages to use output sampling commonly done in traditional sampling methods. The first limitation is that the algorithm must determine the most ideal node to expand [22]. This selection is typically made based on the proximity of nodes in the graph to a sampled output node and involves a potentially costly nearest-neighbor search. The Local Planning Method (LPM) presents the second and perhaps more troublesome problem, which is determining an input that connects a newly sampled node to the current node [23]. This problem is essentially a two-point boundary value problem that connects one output or state to another. There is no guarantee that such an input exists. Also, for systems with complex dynamics, the search itself can be computationally expensive, which leads to a computationally inefficient planner. In contrast, when the input space is sampled as proposed in this paper, the need for a nearest-neighbor search is eliminated, and the LPM is reduced to the integration of a system model, and therefore, only outputs that are achievable for the system are generated.In order to visualize this concept, consider a robot that has positionx,y and orientation θ, which are the outputs of the robot kinematics model. The model restricts the attainable outputs. All the dots in Figure 4 are output nodes obtained from sampling the output space even though only the dots on the mesh surface can physically be achievable by the robot. There are a larger number of dots (sampled outputs) in the output space that do not lie in the achievable region (mesh surface). This means those sampled outputs are not physically achievable, so traditional sampling-based methods would have to finish the search in the whole output space. This leads to an inefficient search that can substantially increase the computational time of the planner. In essence, sampling in the input space leads to more efficient results since each of the corresponding dots in the output space is allowed by the model.Figure 4
Potential problems in output space sampling.


### 2.6. Implicit State Grid

Although extension from several existing sampling-based paradigms can lead to input sampling algorithms like SBMPC, input sampling has not been used in most planning research. This is most likely due to the fact that input sampling is seen as an inefficient method because it can result in highly dense samples in the output space since input sampling does not inherently lead to a uniformly discrete output space, such as a uniform grid. This problem is especially evident when encountering a local minimum problem associated with theA∗ algorithm [24], which can occur when planning in the presence of a large concave obstacle while the goal is on the other side of the obstacle. This situation is considered in depth for discrete 2D path planning in the work of this paper, which discusses that the A∗ algorithm must explore all the states in the neighborhood of the local minimum, shown as the shaded region of Figure 5, before progressing to the final solution. The issue presented to input sampling methods is that the number of states within the local minimum is infinite due to the lack of a discrete output space.Figure 5
Diagram of implicit state grid.The concept of an implicit state grid is introduced as a solution to both of the challenges generated by input sampling. The implicit grid ensures that the graph generated by the SBMPC algorithm is constructed such that only one active output (state) exists in each grid cell, limiting the number of nodes that can exist within any finite region of the output space. In essence, the implicit state grid provides a discrete output space. It also allows for the efficient storage of potentially infinite grids by only storing the grid cells that contain nodes, which is increasingly important for higher-dimensional problems.As shown in Figure6, a, b, and c denote the edge length of planning path. Node τ1 is selected for expansion after which the lowest-cost node is τ3. The implicit state grid then recognizes that τ2 and τ3 are close enough to be considered the same and updates the path to their grid cell; then the modified planning path is c since c<a+b.Figure 6
Necessity for introducing implicit state grid.


### 2.7. Goal-Directed Optimization

There is a class of discrete optimization techniques that have their origin in graph theory and have been further developed in the path planning literature. In this study, these techniques will be called goal-directed optimization and refer to graph search algorithms such as Dijkstra’s algorithm [25], A∗ algorithm, D∗ algorithm [26], and LPA∗ algorithm [27]. Given a graph, these algorithms find a path that optimizes some cost of moving from a start node to some given goal. In contrast to discrete optimization algorithms such as branch-and-bound optimization, which is used for solving continuous optimization problems, the goal-directed optimization methods are inherently discrete and usually used for real-time path planning.Although not commonly recognized, goal-directed optimization methods are capable of solving control theory problems for which the ultimate objective is to plan an optimal trajectory and control inputs to reach a goal (or set point) while optimizing a cost function. Hence, graph search algorithms can be applied to terminal constraint optimization problems and set point control problems. To observe this, consider the tree graph of Figure2. Each node of this tree can correspond to a system state and the entire tree may be generated by integrating sampled inputs to a system model. Assume that the trajectory cost is given by the sum of corresponding edge (i.e., branch) cost, where each edge cost is dependent not only on the states it connects but also on the inputs that are used to connect those states. The use of the system kinematics model can be viewed simply as a means to generate the directed graph and associated edge cost. Figure 7 shows the block diagram of the optimal node selection based on SBMPC.Figure 7
Optimal node selection based on SBMPC.


### 2.8. Rescue and Detection Robot Path Planning Based on SBMPC

In this section, path planning algorithm based on SBMPC is presented. Firstly, variables used in this algorithm are defined. Sampling-Based Model Predictive Optimization (SBMPO) is similar toLPA∗ algorithm, and the difference between the two methods lies in the way neighborhood solution is generated. In this paper, states for future time are generated by introducing predictive model of robot motion with certain constraints shown in Part B of Section 2.SBMPC operates on a dynamic directed graphG which is a set of all nodes and edges currently in the graph. Node τ denotes the motion states of rescue and detection robot, namely, τ=x,y,θT. SUCCτ represents the set of successors (children) of node τ∈G while PREDτ denotes the set of all predecessors (parents) of node τ∈G. The cost of traversing from node τ to node τ′∈SUCCτ is denoted by cτ,τ′, where τ satisfies 0<cτ,τ′<1. The optimization component of SBMPC is SBMPO which is an algorithm that determines the optimal cost (i.e., shortest path, shortest time, least energy, etc.) from a start node τstart∈G to a goal node τgoal∈G. The start distance of node τ∈G is given by g∗τ which is the cost of the optimal path from the given start node τstart to the current node τ. This paper will focus on determining the shortest path through SBMPC algorithm.SBMPC maintains two estimates ofg∗τ. The first estimate gτ is essentially the current cost from τstart to the node τ while the second estimate rhsτ is a one-step look-ahead estimate based on gτ′ for τ′∈PREDτ and provides more information than the estimate gτ. The rhsτ value satisfies(16)rhsτ=0,ifτ=τstart,(17)rhsτ=minτ′∈PREDτgτ′+cτ′,τ,otherwiseA nodeτ is locally consistent if gτ=rhsτ and locally inconsistent if gτ≠rhsτ. If all nodes are locally consistent, then gτ satisfies equation (17) for all τ∈G and is therefore equal to the start distance. This enables the ability to trace the shortest path from τstart to any node τ by starting at τ and traversing to any predecessor τ′ that minimizes gτ′+cτ′,τ until τstart is reached.To facilitate fast replanning, SBMPO does not make every node locally consistent after an edge cost changes and instead it uses a heuristic functionhτ,τgoal to focus the search so that it only updates gτ for nodes necessary to obtain the shortest path. The heuristic is used to approximate the goal distances and must follow the triangle inequality hτgoal,τgoal=0 and hτ,τgoal≤cτ,τ′+hτ′,τgoal for all nodes τ∈G and τ′∈SUCCτ. SBMPO employs the heuristic function along with the start distance estimates to rank the priority queue containing the locally inconsistent nodes and thus all the nodes that need to be updated in order for them to be locally consistent. The priority of a node is determined by a two-component key vector(18)keyτ=k1τk2τ=mingτ,rhsτ+hτ,τgoalmingτ,rhsτ,where the keys are ordered lexicographically with the smaller key values having a higher priority.The SBMPC algorithm contains three main functions: SBMPC, SBMPO, and Neighbor Generation. The main SBMPC algorithm follows the general structure of MPC where SBMPO repeatedly computes the optimal path between the current stateτcurrent and the goal state τgoal. After a single path is generated, τcurrent is updated to reflect the implementation of the first control input and the graph G is updated to reflect any system changes. These steps are repeated until the goal state is reached.The second algorithm SBMPO repeatedly generates the neighbors of locally inconsistent nodes untilτgoal is locally consistent or the key of the next node in the priority queue is not smaller than key (τgoal). The node τbest with the highest priority (lowest key value) is on top of the priority que. The algorithm then deals with two potential cases based on the consistency of the expanded node τbest. If the node is locally overconsistent, gτ>rhsτ, the g-value is set to rhsτ making the node locally consistent. The successors of τ are then updated.The node updating process includes recalculatingrhsτ and key values, checking for local consistency and either adding or removing the node from the priority queue accordingly. For the case when the node is locally underconsistent, gτ<rhsτ, the g-value is set to ∞ making the node either locally consistent or overconsistent. This change can affect the node along with its successors which then goes through the node updating process.The Neighbor Generation algorithm determines the successor nodes of the current node. In the input space, a set of quasirandom samples are generated that are then used with the predictive model of robot motion to calculate a new set of outputs (states) withτcurrent being the initial condition. The branching factor Nb determines the number of paths that will be generated. The path is represented by a sequence of states τk for k=k1,k1+Δk,…,k2, where Δk is the model step size. The set of states that do not violate any state or obstacle constraints is called τfree. If τk∈τfree, then the new neighbor node τnew and the connecting edge can be added to the graph. If τnew∈STATE_GRID, then the node currently exists in the graph and only the new path to reach the existing node needs to be added.


## 2.1. Kinematics Model of Rescue and Detection Robot

In this paper, rescue and detection robot is regarded as the research subject. As shown in Figure1, xoy denotes global reference frame and xRCyR denotes local reference frame. C denotes the center of robot mass, vt is robot forward speed, and ωt is robot angular speed.Figure 1
Rescue and detection robot in global reference frame.Kinematics model of rescue and detection robot [19] can be described as(1)x˙y˙θ˙=r⋅cosθ4r⋅cosθ4r⋅sinθ4r⋅sinθ4−r2lr2l⋅ϕ˙1ϕ˙2,where x,y is the robot position in global reference frame and θ is the angle deviation between local reference frame and global reference frame. r is the diameter of motivation wheel, l is the distance from left wheel to right wheel, ϕ˙1 is the rotation speed of left wheel, and ϕ˙2 is the rotation speed of right wheel. In this paper, r is 0.5 m and l is 0.8 m. Equation (1) can be expressed briefly as(2)X˙=B⋅U,where(3)X=xyθT,B=r⋅cosθ4r⋅cosθ4r⋅sinθ4r⋅sinθ4−r2lr2l,U=ϕ˙1ϕ˙2T.Equation (2) is the state equation of robot motion. X is the state variable, U is the control input variable, and B is the control input matrix. In the process of path planning, robot attitude and position is regarded as output variable of robot motion, which can be defined as(4)Y=xyθT.Therefore, the output equation of robot motion is expressed as(5)Y=X.Equations (2) and (5) are the kinematics model of rescue and detection robot.


## 2.2. Predictive Model of Motion Planning

Derivatives ofx, y, and θ at the time k can be calculated from equation (1) and expressed as(6)x˙k=r⋅cosθk4⋅ϕ˙1k+ϕ˙2k,(7)y˙k=r⋅sinθk4⋅ϕ˙1k+ϕ˙2k,(8)θ˙k=r2l⋅−ϕ˙1k+ϕ˙2k,k=1,2,3,…,where ϕ˙1k is the rotation speed of left wheel at the time k and ϕ˙2k is the rotation speed of right wheel at the time k. xk+1, yk+1, and θk+1 can be calculated through integration and expressed as(9)xk+1=xk+x˙k⋅T,(10)yk+1=yk+y˙k⋅T,(11)θk+1=θk+θ˙k⋅T,where T is the sampling period.Equations (6)∼(11) constitute the predictive model of robot motion and can be used for calculating robot attitude and position in future time through iterative computation.


## 2.3. Cost Function of Motion Planning

If Shannon’s sampling constraints [20] are satisfied, the cost function of robot motion planning based on SBMPC can be described as(12)minUk,…,Uk+N−1,NJ=∑i=0NYk+i+1−Yk+iQi+∑i=0N−1Uk+i−Uk+i−1Si.Variables in equation (12) should satisfy the following inequality conditions; viz.,(13)Yk+N−G0≤ε,(14)Qi≥0,Si>0.In equation (12), the term Yk+i+1−Yk+iQi represents the edge cost of planning path between the current predictive output Yk+i and the next predictive output Yk+i+1. The term Uk+i−Uk+i−1Si represents the energy consumption in robot motion process. In the constraint (13), G0 denotes the goal state which is used for construction of the terminal constraint of path planning. ε is a small positive number which represents the proximity of robot output to the goal state. The constraint (13), which reflects the requirement that robot should reach the target G0 with the accuracy ε, is a key condition for ensuring the convergence of planning algorithm. Qi and Si are the orders of corresponding norms. Therefore, the goal-directed optimization is adopted here, and in this method, the goal is implicitly considered through the calculation of a rigorous lower bound for the cost from a particular state to G0.


## 2.4. Principle of Motion Planning Based on Sampling

Sampling-based motion planning algorithms include Rapidly Exploring Random Trees (RRTs) and randomizedA∗ algorithms [21]. A common feature of these algorithms is that they are applied in the output space of robot and used for generating samples through various strategies. In essence, as shown in Figure 2, sampling-based motion planning methods are applied by using sampling to construct a tree that connects the root (initial state) with a goal region.Figure 2
Diagram of tree connecting the root with a goal region.Sampling-based path planning algorithm is shown in Figure3.Figure 3
Diagram of sampling-based path planning algorithm.Sampling-based path planning algorithm follows the steps below.Step 1. Initialization: GV,E represents a search graph where V denotes the set of nodes and E denotes the set of edges. During the initialization, V only contains start node and E does not contain any edge.Step 2. Node selection: select a node τ in V, which is regarded as the current node.Step 3. New edge generation: generate corresponding routes Γs for all candidate nodes τnew in Cfree (the set of candidate nodes). Γs satisfies the following constraints:(15)Γs0=τ,Γs1=τnew.If the routeΓs does not exist for a certain candidate node, select the next candidate node to generate its corresponding route. Until above processing covers all candidate nodes, then go to Step 4.Step 4. New edge insertion: all the routes Γs generated in Step 3 are regarded as new edges connecting the node τ to the node τnew and incorporated to E. The node τnew is also incorporated to V if V does not contain τnew.Step 5. Search for path planning solutions: start from the current node τ in the graph GV,E and search for the optimal edge as the best route for node refreshing; meanwhile, the terminal node τnew of the optimal edge is regarded as the starting node of planning path in the next time.Step 6. Return to Step 2. Repeat the above steps until τnew arrives at the goal node or the path planning time is over.The novelty of SBMPC method is mainly reflected in three aspects, i.e., input sampling, implicit state grid, and goal-directed optimization, which are described in the following parts.


## 2.5. Input Sampling

There are two primary disadvantages to use output sampling commonly done in traditional sampling methods. The first limitation is that the algorithm must determine the most ideal node to expand [22]. This selection is typically made based on the proximity of nodes in the graph to a sampled output node and involves a potentially costly nearest-neighbor search. The Local Planning Method (LPM) presents the second and perhaps more troublesome problem, which is determining an input that connects a newly sampled node to the current node [23]. This problem is essentially a two-point boundary value problem that connects one output or state to another. There is no guarantee that such an input exists. Also, for systems with complex dynamics, the search itself can be computationally expensive, which leads to a computationally inefficient planner. In contrast, when the input space is sampled as proposed in this paper, the need for a nearest-neighbor search is eliminated, and the LPM is reduced to the integration of a system model, and therefore, only outputs that are achievable for the system are generated.In order to visualize this concept, consider a robot that has positionx,y and orientation θ, which are the outputs of the robot kinematics model. The model restricts the attainable outputs. All the dots in Figure 4 are output nodes obtained from sampling the output space even though only the dots on the mesh surface can physically be achievable by the robot. There are a larger number of dots (sampled outputs) in the output space that do not lie in the achievable region (mesh surface). This means those sampled outputs are not physically achievable, so traditional sampling-based methods would have to finish the search in the whole output space. This leads to an inefficient search that can substantially increase the computational time of the planner. In essence, sampling in the input space leads to more efficient results since each of the corresponding dots in the output space is allowed by the model.Figure 4
Potential problems in output space sampling.


## 2.6. Implicit State Grid

Although extension from several existing sampling-based paradigms can lead to input sampling algorithms like SBMPC, input sampling has not been used in most planning research. This is most likely due to the fact that input sampling is seen as an inefficient method because it can result in highly dense samples in the output space since input sampling does not inherently lead to a uniformly discrete output space, such as a uniform grid. This problem is especially evident when encountering a local minimum problem associated with theA∗ algorithm [24], which can occur when planning in the presence of a large concave obstacle while the goal is on the other side of the obstacle. This situation is considered in depth for discrete 2D path planning in the work of this paper, which discusses that the A∗ algorithm must explore all the states in the neighborhood of the local minimum, shown as the shaded region of Figure 5, before progressing to the final solution. The issue presented to input sampling methods is that the number of states within the local minimum is infinite due to the lack of a discrete output space.Figure 5
Diagram of implicit state grid.The concept of an implicit state grid is introduced as a solution to both of the challenges generated by input sampling. The implicit grid ensures that the graph generated by the SBMPC algorithm is constructed such that only one active output (state) exists in each grid cell, limiting the number of nodes that can exist within any finite region of the output space. In essence, the implicit state grid provides a discrete output space. It also allows for the efficient storage of potentially infinite grids by only storing the grid cells that contain nodes, which is increasingly important for higher-dimensional problems.As shown in Figure6, a, b, and c denote the edge length of planning path. Node τ1 is selected for expansion after which the lowest-cost node is τ3. The implicit state grid then recognizes that τ2 and τ3 are close enough to be considered the same and updates the path to their grid cell; then the modified planning path is c since c<a+b.Figure 6
Necessity for introducing implicit state grid.


## 2.7. Goal-Directed Optimization

There is a class of discrete optimization techniques that have their origin in graph theory and have been further developed in the path planning literature. In this study, these techniques will be called goal-directed optimization and refer to graph search algorithms such as Dijkstra’s algorithm [25], A∗ algorithm, D∗ algorithm [26], and LPA∗ algorithm [27]. Given a graph, these algorithms find a path that optimizes some cost of moving from a start node to some given goal. In contrast to discrete optimization algorithms such as branch-and-bound optimization, which is used for solving continuous optimization problems, the goal-directed optimization methods are inherently discrete and usually used for real-time path planning.Although not commonly recognized, goal-directed optimization methods are capable of solving control theory problems for which the ultimate objective is to plan an optimal trajectory and control inputs to reach a goal (or set point) while optimizing a cost function. Hence, graph search algorithms can be applied to terminal constraint optimization problems and set point control problems. To observe this, consider the tree graph of Figure2. Each node of this tree can correspond to a system state and the entire tree may be generated by integrating sampled inputs to a system model. Assume that the trajectory cost is given by the sum of corresponding edge (i.e., branch) cost, where each edge cost is dependent not only on the states it connects but also on the inputs that are used to connect those states. The use of the system kinematics model can be viewed simply as a means to generate the directed graph and associated edge cost. Figure 7 shows the block diagram of the optimal node selection based on SBMPC.Figure 7
Optimal node selection based on SBMPC.


## 2.8. Rescue and Detection Robot Path Planning Based on SBMPC

In this section, path planning algorithm based on SBMPC is presented. Firstly, variables used in this algorithm are defined. Sampling-Based Model Predictive Optimization (SBMPO) is similar toLPA∗ algorithm, and the difference between the two methods lies in the way neighborhood solution is generated. In this paper, states for future time are generated by introducing predictive model of robot motion with certain constraints shown in Part B of Section 2.SBMPC operates on a dynamic directed graphG which is a set of all nodes and edges currently in the graph. Node τ denotes the motion states of rescue and detection robot, namely, τ=x,y,θT. SUCCτ represents the set of successors (children) of node τ∈G while PREDτ denotes the set of all predecessors (parents) of node τ∈G. The cost of traversing from node τ to node τ′∈SUCCτ is denoted by cτ,τ′, where τ satisfies 0<cτ,τ′<1. The optimization component of SBMPC is SBMPO which is an algorithm that determines the optimal cost (i.e., shortest path, shortest time, least energy, etc.) from a start node τstart∈G to a goal node τgoal∈G. The start distance of node τ∈G is given by g∗τ which is the cost of the optimal path from the given start node τstart to the current node τ. This paper will focus on determining the shortest path through SBMPC algorithm.SBMPC maintains two estimates ofg∗τ. The first estimate gτ is essentially the current cost from τstart to the node τ while the second estimate rhsτ is a one-step look-ahead estimate based on gτ′ for τ′∈PREDτ and provides more information than the estimate gτ. The rhsτ value satisfies(16)rhsτ=0,ifτ=τstart,(17)rhsτ=minτ′∈PREDτgτ′+cτ′,τ,otherwiseA nodeτ is locally consistent if gτ=rhsτ and locally inconsistent if gτ≠rhsτ. If all nodes are locally consistent, then gτ satisfies equation (17) for all τ∈G and is therefore equal to the start distance. This enables the ability to trace the shortest path from τstart to any node τ by starting at τ and traversing to any predecessor τ′ that minimizes gτ′+cτ′,τ until τstart is reached.To facilitate fast replanning, SBMPO does not make every node locally consistent after an edge cost changes and instead it uses a heuristic functionhτ,τgoal to focus the search so that it only updates gτ for nodes necessary to obtain the shortest path. The heuristic is used to approximate the goal distances and must follow the triangle inequality hτgoal,τgoal=0 and hτ,τgoal≤cτ,τ′+hτ′,τgoal for all nodes τ∈G and τ′∈SUCCτ. SBMPO employs the heuristic function along with the start distance estimates to rank the priority queue containing the locally inconsistent nodes and thus all the nodes that need to be updated in order for them to be locally consistent. The priority of a node is determined by a two-component key vector(18)keyτ=k1τk2τ=mingτ,rhsτ+hτ,τgoalmingτ,rhsτ,where the keys are ordered lexicographically with the smaller key values having a higher priority.The SBMPC algorithm contains three main functions: SBMPC, SBMPO, and Neighbor Generation. The main SBMPC algorithm follows the general structure of MPC where SBMPO repeatedly computes the optimal path between the current stateτcurrent and the goal state τgoal. After a single path is generated, τcurrent is updated to reflect the implementation of the first control input and the graph G is updated to reflect any system changes. These steps are repeated until the goal state is reached.The second algorithm SBMPO repeatedly generates the neighbors of locally inconsistent nodes untilτgoal is locally consistent or the key of the next node in the priority queue is not smaller than key (τgoal). The node τbest with the highest priority (lowest key value) is on top of the priority que. The algorithm then deals with two potential cases based on the consistency of the expanded node τbest. If the node is locally overconsistent, gτ>rhsτ, the g-value is set to rhsτ making the node locally consistent. The successors of τ are then updated.The node updating process includes recalculatingrhsτ and key values, checking for local consistency and either adding or removing the node from the priority queue accordingly. For the case when the node is locally underconsistent, gτ<rhsτ, the g-value is set to ∞ making the node either locally consistent or overconsistent. This change can affect the node along with its successors which then goes through the node updating process.The Neighbor Generation algorithm determines the successor nodes of the current node. In the input space, a set of quasirandom samples are generated that are then used with the predictive model of robot motion to calculate a new set of outputs (states) withτcurrent being the initial condition. The branching factor Nb determines the number of paths that will be generated. The path is represented by a sequence of states τk for k=k1,k1+Δk,…,k2, where Δk is the model step size. The set of states that do not violate any state or obstacle constraints is called τfree. If τk∈τfree, then the new neighbor node τnew and the connecting edge can be added to the graph. If τnew∈STATE_GRID, then the node currently exists in the graph and only the new path to reach the existing node needs to be added.


## 3. Simulation

In order to verify the effectiveness of SBMPC algorithm proposed in this paper, path planning simulation of rescue and detection robot is conducted under the conditions of uniform sampling [28], Halton’s sampling [29, 30], and Centroidal Voronoi Tessellation (CVT) sampling [31, 32], respectively. Simulation results show that local minimum value is avoided successfully by using SBMPC algorithm. Specific conditions relating to simulation are described as follows.


### 3.1. Uniform Sampling

Figure8 shows the path planning simulation of SBMPC algorithm with uniform sampling, where obstacles are denoted by red circles and robot dimensions are considered for the safety of obstacle avoidance. In Figure 8, the start position is (1, 1) and the goal position is (19, 19). The sample size is 441 at each time k. The total length of planning path in Figure 8 is 30.15 m. Time consumption from start position to goal position is 150.75 s. Figure 9 shows the variation of direction angle θ during the process.Figure 8
Simulation of SBMPC algorithm with uniform sampling.Figure 9
Curve of direction angle while uniform sampling.It can be seen from Figures8 and 9 that the direction angle varies sharply when the sample time is near 15 sec and 30 sec. The simulation result of uniform sampling is not optimal although robot can avoid all the obstacles and attain the goal position along the planning path. The reason for this problem is characteristic information of control input space cannot be extracted completely and accurately through uniform sampling and the candidate solutions are not selected within the global scope of solution space, which leads to the nonideal planning path.


### 3.2. Halton’s Sampling

Figure10 shows the path planning simulation of SBMPC algorithm with Halton’s sampling. In Figure 10, the start position is (1, 1) and the goal position is (19, 19). The sample size is 441 at each time k. The total length of planning path in Figure 10 is 26.82 m. Time consumption from start position to goal position is 134.10 s. Figure 11 shows the variation of direction angle θ during the process.Figure 10
Simulation of SBMPC algorithm with Halton’s sampling.Figure 11
Curve of direction angle while Halton’s sampling.It can be seen from Figures10 and 11 that the planning path based on SBMPC algorithm with Halton’s sampling is not smooth but the variation of direction angle is relatively steady compared with the simulation of uniform sampling. The robot can avoid all the obstacles and attain the goal position along the planning path. Since the distribution distance between any two points of Halton’s stochastic sequence is not limited and the sample point cannot be inserted randomly between any two points of uniform sampling sequence, the point position information obtained by Halton’s sampling is more comprehensive and the point coverage is more reasonable than uniform sampling when the sample size is same.


### 3.3. CVT Sampling

Figure12 shows the path planning simulation of SBMPC algorithm with CVT sampling. In Figure 12, the start position is (1, 1) and the goal position is (19, 19). The sample size is 441 at each time k. The total length of planning path in Figure 12 is 26.18 m. Time consumption from start position to goal position is 130.90 s. Figure 13 shows the variation of direction angle θ during the process.Figure 12
Simulation of SBMPC algorithm with CVT sampling.Figure 13
Curve of direction angle while CVT sampling.It can be seen from Figures12 and 13 that the planning path based on SBMPC algorithm with CVT  sampling is more smooth and the variation of direction angle is more steady compared with the simulation of Halton sampling.As shown in Table1, fitting performances of sampling points generated by uniform sampling, Halton’s sampling, and CVT sampling are compared and analyzed through fitting toolbox in MATLAB software. SSE denotes the sum of squares for error between fitting data and corresponding original data. SSE indicates better fitting performance when it approaches zero. RMES denotes the square root of SSE mean, and R-square denotes the certain coefficient, which approaches 1 when fitting performance is improved. The plane generated by CVT sampling points is closest to the original plane, and this demonstrates that CVT sampling points can accurately reflect the shape information of a measured plane. Therefore, the performance of SBMPC algorithm is more ideal when the control input variable is sampled through CVT method.Table 1
Fitting performance comparison of three methods.
UniformHaltonCVTSSE0.061250.043070.01518RMES0.021440.016820.01256R-square0.99910.99940.9999Figure14 shows the path planning simulation of SBMPC algorithm while the robot is encountering U-shaped obstacle. In Figure 14, the start position is (16.5, 11.5) and the goal position is (14, 27.5). The sample size is 441 at each time k. The total length of planning path in Figure 14 is 34.53 m. Time consumption from start position to goal position is 172.65 s. Figure 15 shows the variation of direction angle θ during the process.Figure 14
Simulation of SBMPC algorithm with U-shaped obstacle.Figure 15
Curve of direction angle while encountering U-shaped obstacle.It can be seen from Figures14 and 15 that the robot can avoid U-shaped obstacle safely and reach the goal position quickly through path planning based on SBMPC algorithm.Figure16 shows the path planning simulation of SBMPC algorithm while the robot is encountering cross obstacle. In Figure 16, the start position is (5, 2.5) and the goal position is (7.5, 31). The sample size is 441 at each time k. The total length of planning path in Figure 16 is 41.39 m. The time consumption from start position to goal position is 206.94 s. Figure 17 shows the variation of direction angle θ during the process.Figure 16
Simulation of SBMPC algorithm with cross obstacle (sample size is 441).Figure 17
Curve of direction angle while encountering cross obstacle (sample size is 441).Figure18 shows the path planning simulation of SBMPC algorithm while the robot is encountering cross obstacle. In Figure 18, the start position is (5, 2.5) and the goal position is (7.5, 31). The sample size is 1681 at each time k. The total length of planning path in Figure 18 is 40.18 m. Time consumption from start position to goal position is 200.93 s. Figure 19 shows the variation of direction angle θ during the process.Figure 18
Simulation of SBMPC algorithm with cross obstacle (sample size is 1681).Figure 19
Curve of direction angle while encountering cross obstacle (sample size is 1681).Figures16 and 18 demonstrate that the robot can avoid cross obstacle safely through path planning based on SBMPC algorithm. The planning path in Figure 18 is more smooth than the planning path in Figure 16. It can be seen from the total length of planning path and time consumption from start position to goal position that path planning performance is improved when the sample size increases.


## 3.1. Uniform Sampling

Figure8 shows the path planning simulation of SBMPC algorithm with uniform sampling, where obstacles are denoted by red circles and robot dimensions are considered for the safety of obstacle avoidance. In Figure 8, the start position is (1, 1) and the goal position is (19, 19). The sample size is 441 at each time k. The total length of planning path in Figure 8 is 30.15 m. Time consumption from start position to goal position is 150.75 s. Figure 9 shows the variation of direction angle θ during the process.Figure 8
Simulation of SBMPC algorithm with uniform sampling.Figure 9
Curve of direction angle while uniform sampling.It can be seen from Figures8 and 9 that the direction angle varies sharply when the sample time is near 15 sec and 30 sec. The simulation result of uniform sampling is not optimal although robot can avoid all the obstacles and attain the goal position along the planning path. The reason for this problem is characteristic information of control input space cannot be extracted completely and accurately through uniform sampling and the candidate solutions are not selected within the global scope of solution space, which leads to the nonideal planning path.


## 3.2. Halton’s Sampling

Figure10 shows the path planning simulation of SBMPC algorithm with Halton’s sampling. In Figure 10, the start position is (1, 1) and the goal position is (19, 19). The sample size is 441 at each time k. The total length of planning path in Figure 10 is 26.82 m. Time consumption from start position to goal position is 134.10 s. Figure 11 shows the variation of direction angle θ during the process.Figure 10
Simulation of SBMPC algorithm with Halton’s sampling.Figure 11
Curve of direction angle while Halton’s sampling.It can be seen from Figures10 and 11 that the planning path based on SBMPC algorithm with Halton’s sampling is not smooth but the variation of direction angle is relatively steady compared with the simulation of uniform sampling. The robot can avoid all the obstacles and attain the goal position along the planning path. Since the distribution distance between any two points of Halton’s stochastic sequence is not limited and the sample point cannot be inserted randomly between any two points of uniform sampling sequence, the point position information obtained by Halton’s sampling is more comprehensive and the point coverage is more reasonable than uniform sampling when the sample size is same.


## 3.3. CVT Sampling

Figure12 shows the path planning simulation of SBMPC algorithm with CVT sampling. In Figure 12, the start position is (1, 1) and the goal position is (19, 19). The sample size is 441 at each time k. The total length of planning path in Figure 12 is 26.18 m. Time consumption from start position to goal position is 130.90 s. Figure 13 shows the variation of direction angle θ during the process.Figure 12
Simulation of SBMPC algorithm with CVT sampling.Figure 13
Curve of direction angle while CVT sampling.It can be seen from Figures12 and 13 that the planning path based on SBMPC algorithm with CVT  sampling is more smooth and the variation of direction angle is more steady compared with the simulation of Halton sampling.As shown in Table1, fitting performances of sampling points generated by uniform sampling, Halton’s sampling, and CVT sampling are compared and analyzed through fitting toolbox in MATLAB software. SSE denotes the sum of squares for error between fitting data and corresponding original data. SSE indicates better fitting performance when it approaches zero. RMES denotes the square root of SSE mean, and R-square denotes the certain coefficient, which approaches 1 when fitting performance is improved. The plane generated by CVT sampling points is closest to the original plane, and this demonstrates that CVT sampling points can accurately reflect the shape information of a measured plane. Therefore, the performance of SBMPC algorithm is more ideal when the control input variable is sampled through CVT method.Table 1
Fitting performance comparison of three methods.
UniformHaltonCVTSSE0.061250.043070.01518RMES0.021440.016820.01256R-square0.99910.99940.9999Figure14 shows the path planning simulation of SBMPC algorithm while the robot is encountering U-shaped obstacle. In Figure 14, the start position is (16.5, 11.5) and the goal position is (14, 27.5). The sample size is 441 at each time k. The total length of planning path in Figure 14 is 34.53 m. Time consumption from start position to goal position is 172.65 s. Figure 15 shows the variation of direction angle θ during the process.Figure 14
Simulation of SBMPC algorithm with U-shaped obstacle.Figure 15
Curve of direction angle while encountering U-shaped obstacle.It can be seen from Figures14 and 15 that the robot can avoid U-shaped obstacle safely and reach the goal position quickly through path planning based on SBMPC algorithm.Figure16 shows the path planning simulation of SBMPC algorithm while the robot is encountering cross obstacle. In Figure 16, the start position is (5, 2.5) and the goal position is (7.5, 31). The sample size is 441 at each time k. The total length of planning path in Figure 16 is 41.39 m. The time consumption from start position to goal position is 206.94 s. Figure 17 shows the variation of direction angle θ during the process.Figure 16
Simulation of SBMPC algorithm with cross obstacle (sample size is 441).Figure 17
Curve of direction angle while encountering cross obstacle (sample size is 441).Figure18 shows the path planning simulation of SBMPC algorithm while the robot is encountering cross obstacle. In Figure 18, the start position is (5, 2.5) and the goal position is (7.5, 31). The sample size is 1681 at each time k. The total length of planning path in Figure 18 is 40.18 m. Time consumption from start position to goal position is 200.93 s. Figure 19 shows the variation of direction angle θ during the process.Figure 18
Simulation of SBMPC algorithm with cross obstacle (sample size is 1681).Figure 19
Curve of direction angle while encountering cross obstacle (sample size is 1681).Figures16 and 18 demonstrate that the robot can avoid cross obstacle safely through path planning based on SBMPC algorithm. The planning path in Figure 18 is more smooth than the planning path in Figure 16. It can be seen from the total length of planning path and time consumption from start position to goal position that path planning performance is improved when the sample size increases.


## 4. Conclusion

Path planning principle of rescue and detection robot based on SBMPC is mainly analyzed in this paper. SBMPC algorithm and its detailed steps for implementation are specified by analyzing construction of cost function and predictive model.Simulation results prove the effectiveness of this algorithm when it is applied in the case of encountering intensive obstacles, U-shaped obstacle and cross obstacle. The effects of three sampling methods (viz., uniform sampling, Halton’s sampling, and CVT sampling) on path planning performance are compared. Statistical analysis demonstrates that CVT sampling points have the most uniform coverage in two-dimensional plane when the amount of sampling points is the same for the three methods. If path planning of rescue and detection robot is designed through SBMPC algorithm proposed in this paper, the optimal path can be obtained when the control input variable is sampled through CVT method.In this paper, SBMPC algorithm is mainly used for path planning research and its application of rescue and detection robot. If this algorithm becomes maturer in algorithm structure and process mode, it can also be applied for path planning of domestic service robot and used for life improvement in the future.



---

*Source: 7821942-2020-11-23.xml*