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.
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 Figure 1, denotes global reference frame and denotes local reference frame. denotes the center of robot mass, is robot forward speed, and is robot angular speed.
Kinematics model of rescue and detection robot [19] can be described aswhere is the robot position in global reference frame and is the angle deviation between local reference frame and global reference frame. is the diameter of motivation wheel, is the distance from left wheel to right wheel, is the rotation speed of left wheel, and is the rotation speed of right wheel. In this paper, is 0.5 m and is 0.8 m. Equation (1) can be expressed briefly aswhere
Equation (2) is the state equation of robot motion. is the state variable, is the control input variable, and 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
Therefore, the output equation of robot motion is expressed as
Equations (2) and (5) are the kinematics model of rescue and detection robot.
2.2. Predictive Model of Motion Planning
Derivatives of , , and at the time can be calculated from equation (1) and expressed aswhere is the rotation speed of left wheel at the time and is the rotation speed of right wheel at the time . , , and can be calculated through integration and expressed aswhere 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
Variables in equation (12) should satisfy the following inequality conditions; viz.,
In equation (12), the term represents the edge cost of planning path between the current predictive output and the next predictive output . The term represents the energy consumption in robot motion process. In the constraint (13), 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 with the accuracy , is a key condition for ensuring the convergence of planning algorithm. and 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 .
2.4. Principle of Motion Planning Based on Sampling
Sampling-based motion planning algorithms include Rapidly Exploring Random Trees (RRTs) and randomized 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.
Sampling-based path planning algorithm is shown in Figure 3.
Sampling-based path planning algorithm follows the steps below. Step 1. Initialization: represents a search graph where denotes the set of nodes and denotes the set of edges. During the initialization, only contains start node and does not contain any edge. Step 2. Node selection: select a node in , which is regarded as the current node. Step 3. New edge generation: generate corresponding routes for all candidate nodes in (the set of candidate nodes). satisfies the following constraints: If the route 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 generated in Step 3 are regarded as new edges connecting the node to the node and incorporated to . The node is also incorporated to if does not contain . Step 5. Search for path planning solutions: start from the current node in the graph and search for the optimal edge as the best route for node refreshing; meanwhile, the terminal node 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 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 position 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.
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 the 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 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.
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 Figure 6, , , and denote the edge length of planning path. Node is selected for expansion after which the lowest-cost node is . The implicit state grid then recognizes that and are close enough to be considered the same and updates the path to their grid cell; then the modified planning path is since .
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], algorithm, algorithm [26], and 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 Figure 2. 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.
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 to 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 graph which is a set of all nodes and edges currently in the graph. Node denotes the motion states of rescue and detection robot, namely, . represents the set of successors (children) of node while denotes the set of all predecessors (parents) of node . The cost of traversing from node to node is denoted by , where satisfies . 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 to a goal node . The start distance of node is given by which is the cost of the optimal path from the given start node to the current node . This paper will focus on determining the shortest path through SBMPC algorithm.
SBMPC maintains two estimates of . The first estimate is essentially the current cost from to the node while the second estimate is a one-step look-ahead estimate based on for and provides more information than the estimate . The value satisfies
A node is locally consistent if and locally inconsistent if . If all nodes are locally consistent, then satisfies equation (17) for all and is therefore equal to the start distance. This enables the ability to trace the shortest path from to any node by starting at and traversing to any predecessor that minimizes until 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 function to focus the search so that it only updates for nodes necessary to obtain the shortest path. The heuristic is used to approximate the goal distances and must follow the triangle inequality and for all nodes and . 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 vectorwhere 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 and the goal state . After a single path is generated, is updated to reflect the implementation of the first control input and the graph 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 is locally consistent or the key of the next node in the priority queue is not smaller than key (). The node 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 . If the node is locally overconsistent, , the -value is set to making the node locally consistent. The successors of are then updated.
The node updating process includes recalculating 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, , the -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 being the initial condition. The branching factor determines the number of paths that will be generated. The path is represented by a sequence of states for , where is the model step size. The set of states that do not violate any state or obstacle constraints is called . If , then the new neighbor node and the connecting edge can be added to the graph. If , 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
Figure 8 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 . 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.
It can be seen from Figures 8 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
Figure 10 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 . 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.
It can be seen from Figures 10 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
Figure 12 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 . 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.
It can be seen from Figures 12 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 Table 1, 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.
Figure 14 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 . 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.
It can be seen from Figures 14 and 15 that the robot can avoid U-shaped obstacle safely and reach the goal position quickly through path planning based on SBMPC algorithm.
Figure 16 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 . 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 18 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 . 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.
Figures 16 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.
Data Availability
The simulation data used to support the findings of this study are included within the article.
Conflicts of Interest
The authors declare that they have no conflicts of interest regarding the publication of this paper.
Acknowledgments
Portions of this work were performed under the projects supported by the Natural Science Foundation of Inner Mongolia, China (Grant no. 2016MS0607), Science Research Foundation of Inner Mongolia University of Technology (Grant no. X201520), and 2019 Talent Development Foundation of Inner Mongolia.