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

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 deﬁned cost function. The eﬀects 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 eﬀective and feasible to plan a secure route for rescue and detection robot under complex environment.


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. erefore, 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 realtime 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. e 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.
e 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. e main idea of RRT algorithm is to extend search tree incrementally by browsing the state space in a rapid and uniform way. is 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. e 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. e 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. is 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. e 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. e 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). e 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. e 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. e 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. e 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. e 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.

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, xoy denotes global reference frame and x R Cy R denotes local reference frame. C denotes the center of robot mass, v(t) is robot forward speed, and ω(t) is robot angular speed.
Kinematics model of rescue and detection robot [19] can be described as 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 where 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 erefore, the output equation of robot motion is expressed as Equations (2) and (5) are the kinematics model of rescue and detection robot.

Predictive Model of Motion Planning.
Derivatives of x, y, and θ at the time k can be calculated from equation (1) and expressed as where _ ϕ 1 (k) is the rotation speed of left wheel at the time k and _ ϕ 2 (k) is the rotation speed of right wheel at the time k. x(k + 1), y(k + 1), and θ(k + 1) can be calculated through integration and expressed as 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.

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 ‖Y(k + i + 1) − Y(k + i)‖ Q(i) represents the edge cost of planning path between the current predictive output Y(k + i) and the next predictive represents the energy consumption in robot motion process. In the constraint (13), G 0 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. e constraint (13), which reflects the requirement that robot should reach the target G 0 with the accuracy ε, is a key condition for ensuring the convergence of planning algorithm. Q(i) and S(i) are the orders of corresponding norms. erefore, the goal-directed optimization is adopted here, and in this method, the goal is Discrete Dynamics in Nature and Society implicitly considered through the calculation of a rigorous lower bound for the cost from a particular state to G 0 .

Principle of Motion Planning Based on Sampling.
Sampling-based motion planning algorithms include Rapidly Exploring Random Trees (RRTs) and randomized A * 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: G(V, 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 C free (the set of candidate nodes). Γ s satisfies the following constraints: 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. e 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 G(V, 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. e 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.

Input Sampling.
ere are two primary disadvantages to use output sampling commonly done in traditional sampling methods.
e first limitation is that the algorithm must determine the most ideal node to expand [22]. is 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. e 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]. is problem is essentially a two-point boundary value problem that connects one output or state to another.
ere 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 (x, y) and orientation θ, which are the outputs of the robot kinematics model. e 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.
ere are a larger number of dots (sampled outputs) in the output space that do not lie in the achievable region (mesh surface). is means those sampled outputs are not physically achievable, so traditional sampling-based methods would have to finish the search in the whole output space. is 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.

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. is 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. is problem is especially evident when encountering a local minimum problem associated with the A * 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. is 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. e 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. e concept of an implicit state grid is introduced as a solution to both of the challenges generated by input sampling. e 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  Search for the optimal path planning solution in Regard τ new , as the starting node in the next time Discrete Dynamics in Nature and Society contain nodes, which is increasingly important for higherdimensional problems. As shown in Figure 6, 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 . e 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.

Goal-Directed Optimization.
ere 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 goaldirected 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 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. e 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.

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 LPA * 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 G 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. e cost of traversing from node τ to node τ ′ ∈ SUCC(τ) is denoted by c(τ, τ ′ ), where τ satisfies 0 < c(τ, τ ′ ) < 1. e 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. e 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 τ. is paper will focus on determining the shortest path through SBMPC algorithm. SBMPC maintains two estimates of g * (τ). e first estimate g(τ) is essentially the current cost from τ start to the node τ while the second estimate rhs(τ) is a one-step lookahead estimate based on g(τ ′ ) for τ ′ ∈ PRED(τ) and provides more information than the estimate g(τ). e rhs(τ) value satisfies A 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. is 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 function h(τ, τ goal ) to focus the search so that it only updates g(τ) for nodes necessary to obtain the shortest path. e 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. e priority of a node is determined by a two-component key vector , rhs(τ)) + h τ, τ goal min(g(τ), rhs(τ)) where the keys are ordered lexicographically with the smaller key values having a higher priority. e SBMPC algorithm contains three main functions: SBMPC, SBMPO, and Neighbor Generation. e 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. ese steps are repeated until the goal state is reached. e 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 ). e node τ best with the highest priority (lowest key value) is on top of the priority que. e 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. e successors of τ are then updated. e node updating process includes recalculating rhs(τ) 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. is change can affect the node along with its successors which then goes through the node updating process.
e 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. e branching factor N b determines the number of paths that will be generated. e path is represented by a sequence of states τ(k) for k � k 1 , k 1 + Δk, . . . , k 2 , where Δk is the model step size. e 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.

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. 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). e sample size is 441 at each time k. e 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.  e simulation result of uniform sampling is not optimal although robot can avoid all the obstacles and attain the goal position along the planning path. e 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. 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). e sample size is 441 at each time k. e 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.

Halton's Sampling.
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. e 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. 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). e sample size is 441 at each time k. e 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.

CVT Sampling.
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  8 Discrete Dynamics in Nature and Society approaches 1 when fitting performance is improved. e 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. erefore, 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). e sample size is 441 at each time k. e 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). e sample size is 441 at each time k. e total length of planning path in Figure 16 is 41.39 m. e time consumption from start position to goal position is 206.94 s. Figure 17 shows the variation of direction angle θ during the process. 10 0 x (m) Discrete Dynamics in Nature and Society 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). e sample size is 1681 at each time k. e 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. e 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.

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. e 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
e simulation data used to support the findings of this study are included within the article.

Conflicts of Interest
e authors declare that they have no conflicts of interest regarding the publication of this paper.