Collision-free and dynamically feasible trajectory planning for omnidirectional mobile robots using a novel B-spline based rapidly exploring random tree

Generating a collision-free and dynamically feasible trajectory with a better clearance in a cluttered environment is still a challenge. We propose two dynamically feasible B-spline based rapidly exploring random tree (RRT) approaches, which are named DB-RRT and FMDB-RRT, to achieve path planning and trajectory planning simultaneously for omnidirectional mobile robots. DB-RRT combines the convex hull property of the B-spline and RRT’s rapid expansion capability to generate a safe and dynamically feasible trajectory. Firstly, we analyze the tree’s sustainable growth ability and put forward the dynamically feasible region. A geometric method is proposed to judge whether finding a dynamically feasible trajectory quickly. Secondly, we design two steer functions to guide the tree’s growth, improve efficiency, and decrease the number of iterations. To further increase the clearance and reduce the randomness of the trajectory, we propose FMDB-RRT, which uses the path of fast marching to guide the rapid growth of DB-RRT. Then, assuming that the number of sampled points is sufficient to represent the dynamically feasible region, the DB-RRT is proved to be probabilistically complete. Finally, by conducting experimental comparisons with other algorithms in different environments and deploying the proposed algorithm to an omnidirectional mobile robot, the effectiveness and good performance of the algorithm have been verified.


Introduction
The modern manufacturing industry often changes products and production lines to react faster to market changes. Compared with the traditional automated guided vehicle, the autonomous omnidirectional mobile robot is more flexible and can better adapt to the production process of modern manufacturing. 1,2 Motion planning, as a bridge connecting perception and control, is the cornerstone for robots to realize autonomous navigation. 3,4 Many motion planning algorithms have been proposed. The search-based planning algorithms, such as Dijkstra 5 and A*, 6 can find the optimal trajectory with respect to discretization. Some variants [7][8][9] have been put forward to solve kinodynamic planning. However, computation time and memory usage of search-based planning approaches will rise exponentially with the increase of the statespace dimensionality. Nonlinear optimization-based methods [10][11][12] use nonlinear optimization to handle the trajectory optimization problem using the gradient information of the environment. However, there is no theoretical guarantee that the obtained trajectory must not collide with obstacles. These methods also have local minimum problems. Sampling-based exploring algorithms, such as rapidly exploring random tree (RRT) 13 with its variants 14 and probabilistic roadmap, 15 are capable of solving the motion planning problem with complex constraints and can save computation, attracting more and more researchers' attention. 16 However, generating a safe and dynamically feasible trajectory with a better clearance in a cluttered environment is still a challenge.
To solve the above problems, we propose DB-RRT and FMDB-RRT to achieve path planning and trajectory planning simultaneously for omnidirectional mobile robots. The contributions of this work are as follows: 1. DB-RRT combines the convex hull property of the B-spline and RRT's rapid expansion capability to generate a safe and dynamically feasible trajectory. With a mild assumption, the DB-RRT is proved to be probabilistically complete. 2. In the case where the velocity and acceleration control points of the initial state meet the dynamical limits, we analyze the DB-RRT's sustainable growth ability. A geometric method is proposed to determine whether a dynamically feasible trajectory is found quickly. 3. To reduce the number of iterations, two steer functions are designed to guide the tree growth. To further increase the clearance and reduce the randomness of the trajectory, we propose FMDB-RRT, which uses the path of fast marching (FM) to guide the rapid growth of DB-RRT.
The remainder of this article is organized as follows. The related works are introduced in the second section. In the third section, we introduce the velocity and acceleration space of the omnidirectional mobile robot and the basic uniform B-spline concept firstly. Secondly, we explain the essential functions of DB-RRT in detail. Then, we explain the strategy of using the path of FM to guide the growth DB-RRT. At the end of this section, we analyze the probability completeness of DB-RRT. In the fourth section, we conduct experimental comparisons and apply the algorithm to an omnidirectional mobile robot. We give the conclusion and future work in the fifth section.

Related works
Although RRT, 13 as the successful single-query method, has a tortuous and nonoptimal path. Some optimization or pruning methods are proposed to get a shorter or optimal path. RRT* 17 uses branch trimming and rewiring mechanisms to ensure asymptotic optimality. RRT*-Smart 18 employs intelligent sampling and path trimming to obtain optimal or near-optimal path. P-RRT* 19 introduces artificial potential field (APF) to providing a direction for exploration, obtaining a quicker convergence. RRT*-SV 20 uses the Sukharev grids and convex vertices to enhance the sampling process, reducing planning time. The asymptotical optimality of Kinodynamic RRT* 21 has been proved, but complicated calculations cause it to not run in real time. In the literature, 22 a method was proposed for the RoboCup robot by adding gravitational components and path smoothing strategies to Bi-RRT, reducing the randomness and the length of the path. The path obtained by the above algorithms is often short. However, ignoring distance to the obstacle, the obtained path is often unsafe for the robot. When the robot executes this kind of path, it is easy to scratch with the obstacle due to the uncertainty of positioning and execution. 23,24 Some path-biasing approaches have been presented to decrease planning time and enhance path quality. In the literature, 25 a hierarchical planning algorithm was proposed by focusing the RRT* growing in the highdimensional state space along an initial path. Theta*-RRT 26 combines any-angle search with RRT for nonholonomic wheeled robots. However, those methods' initial path does not consider the clearance with obstacles so that they may generate a path close to the obstacle. The Skilled-RRT 27 method was proposed to solve the problem, which can be used for path planning of nonholonomic and holonomic robots in the building environment. This method first extracts the shortest path in the skeleton of the environment and grows the RRT along this path quickly. As the Skilled-RRT extracts the shortest path on the environment skeleton, the obtained path does not always have a better clearance. Moreover, Skilled-RRT only considers the velocity constraint, and velocity is not continuous, which makes Skilled-RRT not suitable for massive robots or vehicles. Our method can get a trajectory with better clearance, which is also a safer trajectory. Moreover, our method can generate a dynamically feasible trajectory with continuous acceleration.
There are some works combining RRT and spline. In the literature, 28 a method was proposed using a spline to smooth the path obtained by RRT, and the path can be better tracked by the vehicle. However, the path after smooth deviating from the RRT path may collide with obstacles. Some works [29][30][31] aim to plan a path for mobile robots or vehicles with differential constrain. In the literature, 31 a motion planning algorithm for autonomous vehicles was presented, which employs two B-spline search trees to generate a continuous and feasible path. SRRT 30 uses spline curve parameterization to generate continuous curvature path satisfying curvature constraints. Unlike the above works, we solve the problem of path planning and trajectory planning simultaneously of holonomic robots and directly obtain the time-parameterized dynamically feasible trajectory.
Unlike the above method, we propose two novel methods combining B-spline and RRT, which can simultaneously achieve path planning and trajectory planning for omnidirectional mobile robots within a reasonable time. The generated trajectory satisfies the dynamic constraints so that the omnidirectional robot can execute it without further velocity planning. Besides, different from the path-guided method mentioned earlier, the proposed FMDB-RRT method can obtain better clearance in a cluttered environment.

Omnidirectional mobile robot and uniform B-spline
The omnidirectional mobile robot can be independently planned in three directions. 32 Due to the unique structure, a velocity range that satisfies v i 2 ½Àv max ; v max and angular velocity range that satisfies ! 2 ½À! max ; ! max ; i 2 fx; yg can be found in its velocity space. Acceleration a i ; and angular acceleration a; i 2 fx; yg have similar restrictions 33,34 as velocity. When the orientation is unchanged during the movement, the ranges of velocity and acceleration in the x-y direction can become larger, making the robot moving faster. In some practical applications, when the robot arrives at the location, it adjusts its direction according to handling requirements. Based on the above considerations, during the robot's movement, we keep the robot's rotation unchanged. In this way, the constraints of velocity and acceleration can be regarded as two independent rectangles v i 2 ½Àv max ; v max and a i 2 ½Àa max ; a max ; i 2 fx; yg.
A p'th B-spline is defined by Nþ1 control points fP 0 , P 1 , . . . P N g, P i 2 R 2 , and knot vector [t 0 , t 1 , .
A uniform B-spline, as a kind of Bspline, is parameterized by time t, and t 2 ½t p ; t MÀp with each knot span has identical value Dt. To get the position at the corresponding time, t is normalized as Then, the position can be evaluated using the matrix representation 35 pðaðtÞ ¼ aðtÞ T M pþ1 P aðtÞ ¼ ½a p ðtÞ; a pÀ1 ðtÞ; . . . aðtÞ; 1 T P ¼ ½P iÀp ; P iÀpþ1 ; P iÀpþ2 ; . . . ; P i T (1) where M pþ1 is constant matrix determined by p. We choose the cubic uniform B-spline in this work as it can guarantee the continuity of velocity and acceleration. B-spline has a convex hull property, which means that all control points are restricted to a convex hull, and the derivative of B-spline is still B-spline. V i ; i 2 ½0; N À 1 and A i ; i 2 ½0; N À 2 represent the velocity control points and acceleration control points, respectively If the control points are known, we can get velocity control points, acceleration control points using equation (2), where Dt is the knot span. As shown in Figure 1, as long as we constrain all velocity control points within v i 2 ½Àv max ; v max ; i 2 fx; yg and acceleration control points within a i 2 ½Àa max ; a max ; i 2 fx; yg, then the generated trajectory must meet the dynamical limits.

Overview of DB-RRT
In Figure 2, the left part shows the process of DB-RRT growth. The right part of Figure 2 shows the selection process of x new , which will be introduced in "Different steer function" section. The pseudocode of DB-RRT is shown in Algorithm 1. We utilize the control points P of B-spline as the RRT node and the connection between the control  points as the edge. M represents the environment, and Dt is the knot of uniform B-spline. State init and State goal refer to initial state and goal state, respectively. k; d min and Sam-pleNum will introduce in "Different steer function" section. Dynlimits refers to the maximum acceleration and velocity restraints in the x-y directions. Using the ComputeControl-Points function, which calculates control points, the initial and goal state control points can be calculated (lines1,2). DynamicallyFeasibleCheck is used to judge whether the velocity control points and acceleration control points derived from the calculated control points meet dynamical limits. If these control points do not meet dynamical limits, a reasonable initial state and goal state need to be provided (lines 3,4). When the tree grows, unlike the normal RRT algorithm, we also store the control points related to initial state and goal state.
The ConnectToGoalControlPoints function is designed to determine whether the newly added node can connect to the control points of the goal state (lines 9-11). If successful, the dynamically feasible trajectory is obtained, the algorithm ends (lines 25,26). If not, the tree continues to grow until the dynamically feasible trajectory is successfully found or the number of nodes that reach the maximum expansion limit returns planning failure (lines 27,28). The ConnectToGoalControlPoints function will be described in detail in "Quickly determine whether finding a trajectory" section. The sampling process and finding the nearest node are the same as the RRT algorithm (lines 12-16).
We designed different steer functions for biasing to goal state or x rand (lines [17][18][19][20]. These two functions use the distance field (line 5) to select the collision-free x new . We will introduce these two functions in detail in "Different steer function" section. The ObstacleFree function detects whether the edges of the x new node and the x near node collide with obstacles. It also detects whether the newly generated piecewise B-spline collides with obstacles (line 22). The process of adding nodes and edges is the same as the RRT algorithm (lines 23,24).

Analysis of the sustainable growth of DB-RRT
In this part, we give an analysis that the tree can grow continuously. It is assumed that the velocity control points and acceleration control points derived from the current control points satisfy dynamic constraints. Then, a new control point must be added to get a new piecewise B-spline, and the velocity control points and acceleration control points derived from this new added control point still meet the dynamical limits.
As shown in Figure 3, it is assumed that the velocity and acceleration control points derived from P i , P iþ1 , and P iþ2 satisfy dynamic constraints. P iþ3 is the newly added control point. According to equation (2), the above control points can get the following velocity and acceleration control points where V iþ2 and A iþ1 are derived from the new control point P iþ3 , which need to meet the following dynamical limits From equation (4), the value range of P iþ3 can be obtained When P iþ3 takes a value within the above range, the derived velocity and acceleration control points must satisfy the dynamic constraints. In Figure 3, the red rectangle represents the range of P iþ3 determined by equation (5). The blue rectangle represents the range of P iþ3 determined by equation (6). The shaded part represents the intersection of equations (5) and (6). The critical question is whether the shaded part, representing the value range of P iþ3 , always exists.
According to the counter-proof method, we know that value range of P iþ3 does not exist when any of the following equations are satisfied For equation (7), move P iþ2 to the left of the equation As P iþ2 À P iþ1 v max Dt on the left side of the inequality, we can get P iþ2 À P iþ1 À v max Dt 0. However, on the right side of the inequality, we can get a max Dt 2 > 0. After the above analysis, we can see that this inequality is not satisfied. Similarly, equation (8) has the same conclusion.
In conclusion, if the current control point satisfies dynamic constraints, then a new control point can be added to get a new part B-spline. The velocity control points and acceleration control points derived from this new control point still meet the dynamical limits. Because the control points related to initial state satisfy the constraints, it means that the tree must continue to expand.
For the convenience of description, we will name the value range of the control point P iþ3 as the dynamically feasible region in the rest of this article. The dynamically feasible region is essential in our work. In Figure 3, the dynamically feasible region of P iþ3 is indicated by the blue shading region. In "Quickly determine whether finding a trajectory" section, we use the dynamically feasible region to determine whether the trajectory is found. In "Different steer function" section, the nodes selected in the dynamically feasible region must satisfy the dynamic constraints, and no additional calculation is required.

Quickly determine whether finding a trajectory
Based on the proposed dynamically feasible region, we propose a geometric method to determine whether the newly added node can quickly connect to the goal state's control points, which is used to judge whether a dynamically feasible trajectory is found. Algorithm 2 shows the pseudocode of ConncetToGoalControlPoints function.
As shown in Figure 4, P n , P nÀ1 , P nÀ2 , and P nÀ3 (n ! 3) are the goal state's corresponding control points. P nÀ3 is the fourth control point from the end, indicated by a green dot in Figure 4. Because the goal state is set to static in this case, the control points P n , P n-1 , and P n-2 are coincident, indicated by blue dots in Figure 4.P iþ3 i >¼ 0 ð Þ is the newly added node, indicated by blue dots in Figure 4. P i , P iþ1 , and P iþ2 are former parent nodes of P iþ3 . Due to the size limitation of the figure, only P iþ3 is shown in Figure 4.
This ConncetToGoalControlPoints function is used to determine whether the newly added node P iþ3 can connect to P nÀ3 with the derived velocity control points and acceleration control points meeting dynamical limits. Based on the previous section's discussion, we transformed this into a problem to determine whether the dynamically feasible regions overlap. The GetPriviousControlPoints function can get the newly added node and the previous three nodes (line 2). The GetDynamicallyFeasibleRegion function based on equation (10) can calculate the dynamically feasible region of the next control point through nodes P iþ3 , P iþ2 , and P iþ1 , which is named region1 (line 3). In Figure 4, region1 is represented by a blue square Similarly, based on equation (11), we can calculate the dynamically feasible region of the next control point through P n , P n-1 , and P n-2 , which is named region2 (line 4). In Figure 4, region2 is represented by a red square If region1 and region2 overlap, further judgment is made, otherwise false is returned (lines 5,6).
The ControlPointsCheck function ensures that the derived velocity and acceleration control points related to P iþ3 and P nÀ3 meet the dynamical limits. We get a new dynamically feasible region, which is named region3 (line 8). Region3 is represented by black squares in Figure 4. The ControlPointsCheck function is based on the following analysis. Figure 5 shows the affected nodes, which can be divided into four groups. After the above discussion, control points in the first and fourth groups must meet the dynamical limits. It is only necessary to judge whether the velocity control points and acceleration control points derived from control points in the second and third groups meet the dynamical limits. At this time, only P nÀ3 is a variable, and we can get the inequality of P n-3 . Remove the constraint that repeats with equation (12), we get the following constraints If the intersection of region1, region2, and region3 is empty, return false (lines 9,10). If the intersection of region1, region2, and region3 is not empty, select the center of the intersection of the three regions as P n-3 (line 12). The green rectangular area in Figure 4 represents the intersection of region1, region2, and region3. If there is no collision with obstacles, then add the corresponding nodes and edges to the tree and output true. Otherwise, false is output. It should be noted that this function can also ensure the trajectory obtained by our algorithm indeed reach the goal state.

Different steer function
Considering the efficiency and effect, we have designed two different functions to find x new , which are used to guide the growth of tree toward x rand or goal state.
SteerToRand function. The SteerToRand function is used to guide the growth of tree toward x rand by generating x new in the dynamically feasible region. The GetDynamicallyFeasible Figure 4. Black regions are obstacles, and yellow regions indicate dynamically feasible regions. Schematic diagram of the connection between the newly added node P iþ3 and control points of the goal state which are P n , P nÀ1 , P nÀ2 , and P nÀ3 . Determine whether the dynamically feasible trajectory is found by judging whether the three regions overlap. function uses x near and its two former parent nodes to obtain the dynamically feasible region (line 3). We first take sam-pleNum points uniformly in this region and then add the points that are not close to obstacles to SamplesCollisionFree (lines 4-7). The d min is a small value to avoid the nodes and obstacles being too close. Choose a node in SamplesCollisionFree, which is closest to x rand as x new (line 8). We use Euclidean distance to measure how close the node is to x rand . The control points affect the shape of B-spline. Choosing the point closest to x rand also corresponds to the B-spline that is most inclined to grow toward x rand . If the found x new is too close to x near , it often causes the velocity and acceleration to change too much in a short time. To avoid the above situation, if the distance is less than minstepsize, select the point furthest away from x near as x new (lines 9,10).
SteerToGoal function. The SteerToGoal function is used to guide the growth of the tree toward goal state by generating x new in the dynamically feasible region. The x rand only has position information, while goal state includes position, velocity, and acceleration. We calculate an optimal boundary value problem between the end state of the piecewise B-spline, which is related to each node in SamplesCollision Free and the goal state. Select the trajectory with the smallest cost, which is also most inclined to expand toward the goal state. Solve the corresponding cost for these nodes according to equation (16) and select the node with the smallest cost as x new . The growing process of the tree and the selection process of x new are shown in Figure 2.
We consider the omnidirectional mobile robot as a second-order integrator model. We can define the statespace model as The s 0 and uðtÞ represent the initial state and the control input, respectively The cost function is designed as A closed-form trajectory is computed that minimizes J from the end state of the piecewise b-spline corresponding, which is related to each node in SamplesCollisionFree to goal state using Pontryagin's minimum principle 36,37 where p i0 ; p if ; v i0 ; v if ;i 2 fx; yg denote the position and velocity of the robot's initial state and goal state. k is the coefficient of balance trajectory time and control cost. It can be seen that J is only related to T, and the optimal trajectory can be obtained by minimizing J. The control point with the smallest J will be selected as x new .

Guiding the growth of DB-RRT using fast marching
The FM 38 is proposed by Sethian to solve wave propagation problems for image processing. As the FM method is local minimum free, it has also been used for path planning in recent years. 39 Figure 6 shows the steps of guiding the growth of DB-RRT using FM.
To reduce the search time, we use bilinear interpolation to shrink the map and use it to generate the distance field. The centers of the dynamically feasible regions derived from the control points of initial and goal states  are used as the start point and the endpoint. Using the generated distance field, FM generates a path. The forward propagation velocity of each grid point is related to its distance to the nearest obstacle. A path away from obstacles is generated to guide the expansion of DB-RRT.
We reduce the number of obtained waypoints by the following method. First, we calculate the length L of the path, and then, uniformly sample ns, ns ¼ L=minðv max Á DT ; a max Á DT 2 Þ, points on the path. The angle is calculated for every three points of the above path, which represents the changes of curvature. If the angle corresponding to the path point is less than 175 , the path point is selected as the guide path. Select the points of the guide path as x rand to guide the tree's growth. The newly added node is selected as x near . When the distance between x new and the guidance point is less than 1:2 min ðv max Á DT ; a max Á DT 2 Þ, then, in the next iteration, the next point of the guidance path is selected as the x rand . Following the above steps, the tree continues to grow toward the goal along the guide path. When the tree's growth is blocked, clear the last generated nodes and use the DB-RRT algorithm for planning. For convenience, we name the DB-RRT guided by the path of FM as FMDB-RRT.

Probabilistic completeness analysis
Based on the discussion of "Quickly determine whether finding a trajectory" section, let q be the fourth control point from the end of any dynamically feasible collision-free goal state and q 2 C free . The distance between q and the closest vertex in DB-RRT is presented as D k ðqÞ. Let d k indicate the value of D k and k is the number of vertices in tree.

Lemma 1.
Suppose C free is n-dimensional subset of an n-dimensional configuration space, in this work, n is 2. For any q 2 C free , the dynamically feasible region of q is N and N \ C free 6 ¼ :. There must be a node v k , which derives dynamically feasible region M, O 2 M \ N meet lim k!1 PðO = 2:Þ ¼ 1 and equation (12).
Proof. Let q be any point in C free , the dynamically feasible region of q is N. Let q 0 denote any initial DB-RRT vertex, which derives dynamically feasible region M. With the number of samples rises, the value of k increases, d k is getting smaller, which means more nodes close to q. Simultaneously, the probability of overlapping between the M and N approaches 1, and the probability of satisfying equation (12) approaches 1. In conclusion, as the number of k increases, there must be a node v k , which derives dynamically feasible region M, O 2 M \ N meet lim k!1 PðO = 2:Þ ¼ 1 and equation (12).
Theorem 1. Assume that the number of sampled points is sufficient to represent the dynamically feasible region and the DB-RRT algorithm is probabilistically complete.
Proof. Let X indicates a random variable that represents the sampling process a random variable X k represents the DB-RRT vertices' distribution. It is assumed that the number of sampled points is sufficient to represent the dynamically feasible region. Similar to the RRT algorithm, the growth of DB-RRT's new node X k is extended toward X, which does not change the randomness of the sampling. As the amount of samples increases, k approaches infinity and X k converges to X in probability. Combined with Lemma 1, the DB-RRT satisfies uniform sampling, and as the number of samples increases, the existing trajectories satisfying the dynamic constraints can be found. In summary, DB-RRT is probabilistically complete. When using the guided path to guide the growth of DB-RRT, no trajectory was found. The last generated nodes and edges will be cleared, and then, the DB-RRT algorithm is used for planning. This will not have any effect on the growth of DB-RRT and do not adversely affect the convergence results.

Experimental tests
The proposed algorithm is compared with RRT, RRT*, 17 RRT-connect, 14 and the improved RRT methods, 22,27 which are suitable for omnidirectional mobile robots. To ensure the correctness of the results, we use different types of environments for testing. In the next part, we deploy the proposed algorithm to an omnidirectional mobile robot to verify the algorithm's effectiveness.

Comparisons and analysis
Introduction to environments and parameters. As shown in Figure 7, the first line shows randomly generating maps with obstacles of different sizes, numbers, and shapes. It should be noted that there are overlapping obstacles. The second line shows three maps representing different building environments with randomly generated obstacles. All maps have a resolution of 2 cm. We focus on seven factors: planning time, iteration number, clearance, path length, dynamic limits, execution time, and success rate. Each algorithm runs 100 times in each environment, and the results were averaged. The parameter selection of each algorithm is introduced as follows. For RRT, RRT*, RRT-connect, and the method in the literature, 22 the step size and threshold to the goal or to the other tree are set to 0.5 m. In the cluttered environment, to improve the success rate of the method in the literature, 22 we set kp to 0.1. The goal bias for RRT, RRT*, the method in the literature, 22 and DB-RRT is set to 10%. For DB-RRT and FMDB-RRT, the maximum acceleration and velocity in the x and y directions are set to 0.4 m/s 2 and 0.5 m/s. The minstepsize is set to u max Dt=5. The dmin is set to 0.2 m in our implementation. The sampleNum is set to 25. The k is set to 1. The Dt is set to 1 s. The map used to run FM is set to one-fifth of the original size. For Skilled-RRT, we consider the environmental size and calculate the parameters according to the method proposed in the literature. 27 We use an omnidirectional car for calculation, where the vehicle velocity v c is set to 0.4 m/s and delta q is set to 0.5.
Random maps are used to test RRT, RRT*, RRTconnect, the method in the literature, 22 DB-RRT, and FMDB-RRT. Skilled-RRT is limited to a building environment, so we use the two environments in the factory and an office environment to compare this algorithm with other algorithms in "Experiments in building environments" section. It should be noted that the RRT-connect algorithm grows branch multiple times each cycle, which is different from the other methods. To be fair, the number of iterations in this article refers to the number of growth of RRT branches. Each algorithm sets the same maximum time limit 10 s in each environment.
All experiments are carried out on a laptop, which equipped with Intel i5-8250U CPU, 8G RAM, and 240G ROM. All codes are implemented in Matlab 2018b on Windows 10. All algorithms use the same functions that can be shared. Note that, since the above algorithms are implemented by ourselves, the comparisons may not be completely fair.
Experiments in random environments. Table 1 presents the experimental data and Figure 8 shows the performance of different algorithms in three random environments. The best value in each item is shown in bold.
Contrasted with the RRT, RRT*, RRT-connect and the method in the literature, 22 the proposed DB-RRT algorithm has fewer iterations. RRT* uses some mechanisms such as parent node selection and rewire to generate the shortest path, but it takes a long time. The method in the literature 22 has the step of path trimming, it can obtain a shorter path, but the generated path is often too close to the obstacle. The guide path points considering the path curvature are far from obstacles. Therefore, FMDB-RRT can often significantly reduce the number of iterations. The uncertainty of FMDB-RRT is smaller than that of DB-RRT, which reduces the execution time of the trajectory. FMDB-RRT can obtain a higher clearance trajectory, which is safer for the robot that executes this trajectory.
In the environment of random map2 and random map3, FMDB-RRT can obtain shorter planning time through fewer iterations. The DB-RRT algorithm considers the robot's dynamic constraints, so the planning problem is more complicated than other RRT algorithms that only consider the geometric information. The time of each iteration of DB-RRT is longer than RRT and the method in the literature. 22 As shown in Figure 8, the trajectories obtained by DB-RRT and FMDB-RRT are smoother than other algorithms, and FMDB-RRT has proper clearance. It is worth noting that those trajectories generated by DB-RRT and FMDB-RRT satisfy the dynamical feasibility constraints and can be directly executed by the robot.
Experiments in building environments. The building environment is used to test skilled-RRT and all previous algorithms. At the same time, we also test the FMDB-RRT  algorithm with different velocity and acceleration. Table 2 presents the experimental data and the best value in each item is shown in bold. The different performance all algorithms in three building environments is shown in Figure 9.
Using the same path guidance, FMDB-RRT trajectories of different accelerations and velocities are too similar, so we draw a trajectory of FMDB-RRT with the maximum acceleration set to 0.4 m/s 2 and velocity set to 0.5 m/s in the x and y directions. Similar to the experimental conclusion of "Experiments in random environments" section, FMDB-RRT has better clearance and fewer planning time than other algorithms. DB-RRT has fewer iterations in the nonpath-guided RRT algorithms. Among all the algorithms, only FMDB-RRT and DB-RRT satisfy C 2 continuity, which is smoother than other algorithms. At the same time, they can generate dynamically feasible trajectories, which robots can directly execute. RRT* still has the shortest path in all environments. RRT-connect has a short planning time in the office environment due to its heuristic growth characteristic.
Both FMDB-RRT and Skilled-RRT are methods that use paths to guide RRT growth. As presented in Table 2, both algorithms have a reasonable success rate. However, FMDB-RRT has fewer iterations, less calculation time, and better clearance. Skilled-RRT following the definition of a small-time controllable system only considers the velocity constraint, but the velocity is not continuous. The noncontinuous velocity causes poor smoothness of the path, see the  third row of Figure 9, making Skilled-RRT not applicable to massive robots or vehicles. Furthermore, Skilled-RRT extracts the shortest path on the environment's skeleton, so the path obtained in a cluttered environment does not have a proper clearance. In our tests, FMDB-RRT with different velocities and accelerations can generate safer and higher success rate trajectories. When the velocity and acceleration are larger, FMDB-RRT has fewer iterations and shorter planning time.

Experiments on an omnidirectional mobile robot
As FMDB-RRT has smaller uncertainty and higher clearance than DB-RRT, we deploy FMDB-RRT to an omnidirectional robot. To verify the practicability of the algorithm, we set up three different obstacle scenarios in the room of 16.8 Â 8.5 m 2 for the experiment. We use Matlab for trajectory planning and send the planned trajectory to an omnidirectional robot platform for execution. A detailed introduction to the omnidirectional mobile robot platform used in this experiment can be found in the literature. 40 We use the cartographer 41 for mapping and get an accurate map with a 5 cm resolution. We inflated the map according to the radius of the robot, which is 0.7 m for planning. The calculation of clearance is relative to the inflated map. In this experiment, according to the robot's physical constraints, the maximum velocity and acceleration in the x and y directions are set to 0.35 m/s and 0.3 m/s 2 , respectively. The remaining parameters are the same as those in the previous experiment. Table 3 presents the experimental data. Figure 10 shows the scene of three experiments. This position of the robot is obtained using the KLD-sampling Monte Carlo localization approach. 42,43 The robot's position in the environment during the experiment is shown in the right part of Figure 10. In the three experiments, the trajectories obtained  by the proposed method have the proper clearance with obstacles. The robot can better track the trajectory obtained by the proposed algorithm.
The planned velocity and the actual velocity of the first experiment are shown in Figure 11. The dotted red line denotes the maximum velocity limits. We can see that the planning velocity does not exceed the limit and can be better tracked by the robot. The actual velocities are calculated from the data, which are read from the servo motor's encoder, and there will be noise because there is no filtering. To sum up, the trajectory generated by our algorithm is smooth and meets dynamical limits, which also have proper clearance with obstacles.

Conclusion
The proposed DB-RRT method employing the convex hull property of B-spline and the rapid expansion capability of RRT can directly generate a collision-free dynamically feasible trajectory. To further increase the clearance and reduce the randomness of the trajectory, we propose FMDB-RRT.
Comparative experiments show that FMDB-RRT further improves clearance of DB-RRT and reduces planning time. Experiments on an omnidirectional robot indicate that the robot can perform obstacle avoidance smoothly and safely in a clustered environment, verifying the effectiveness of the proposed algorithm.
We plan to expand this work, including extending it to a three-dimensional environment and combining it with a dynamic obstacle prediction module for obstacle avoidance in a dynamic environment.

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

Funding
The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: The work was supported by Shandong Key Research and Development Program (Major Science and Technology Innovation Project) [Grant No.2020CXGC010208].

Supplemental material
Supplemental material for this article is available online.