Kinematic Constrained RRT Algorithm with Post Waypoint Shift for the Shortest Path Planning of Wheeled Mobile Robots

Abstract

This paper presents a rapidly exploring random tree (RRT) algorithm with an effective post waypoint shift, which is suitable for the path planning of a wheeled mobile robot under kinematic constraints. In the growth of the exploring tree, the nearest node that satisfies the kinematic constraints is selected as the parent node. Once the distance between the new node and the target is within a certain threshold, the tree growth stops and a target connection based on minimum turning radius arc is proposed to generate an initial complete random path. The most significant difference from traditional RRT-based methods is that the proposed method optimizes the path based on Dubins curves through a post waypoint shift after a random path is generated, rather than through parent node selection and rewiring during the exploring tree growth. Then, it is proved that the method can obtain an optimal path in terms of the shortest length. The optimized path has good convergence and almost does not depend on the state of the initial random path. The comparative test results show that the proposed method has significant advantages over traditional RRT-based methods in terms of the sampling point number, the tree node number, and the path node number. Subsequently, an efficient method is further proposed to avoid unknown obstacles, which utilizes the original path information and thus effectively improves the new path planning efficiency. Simulations and real-world tests are carried out to demonstrate the effectiveness of this method.

1. Introduction

Planning a collision-free path for specific tasks in an obstacle-prone environment is one of the fundamental task in robotics. For decades, various planning algorithms have been proposed and applied successfully in different applications [1]. Due to the requirements of robots in improving working performance, path optimization has always been an important issue [2]. To achieve different objectives, many optimization criteria have been proposed, such as shortest length [3], path smoothness [4], and least energy consumption [5]. In addition, a feasible path needs to satisfy the robot’s kinematic constraints, such as the turning radius of the mobile robot [6], the position of the robotic manipulator, and the speed and acceleration [7].

Different planning algorithms have their own characteristics. For mobile robots, some algorithms are based on grid maps, such as Dijkstra, A*, and D*. Each grid represents the location of free space or obstacles. These algorithms can find an optimal path with the smallest cost associated with the edges of the workspace structure. However, the workspace discretization resolution affects the rationality of the generated path. As the workspace size and the discretization resolution increase, computing time cost and memory requirements will increase exponentially, resulting in lower efficiency. The artificial potential field is a well-known path planning algorithm [8], in which the workspace is described using a potential field. A smooth path can be obtained by following the gradient of the established potential field. The local minimum and the non-reachable with the goal nearby obstacles are two important problems affecting these algorithms’ stability. Evolutionary algorithms such as genetic algorithms [9], differential algorithms [10], and ant colony algorithms [11] are also widely used in path planning. By mimicking biological evolutionary behavior, judging the optimal solution through adaptability function, and inheriting excellent features through selection algorithm, the generation path can be optimized. Recently, reinforcement learning has become an important issue in path planning research. It is a machine learning paradigm based on environment interaction, and can achieve the most strategic goals through trial and error and reward and punishment mechanisms. Q-learning is a typical path planning algorithm that uses reinforcement learning, which has self-learning ability and does not require a priori knowledge of the environment [12]. On this basis, many improved algorithms incorporating deep learning and neural networks have been proposed [13,14,15]. The advantage of these algorithms is that they can deal with high dimensional state space and action space, and thus be able to solve complex decision problems. However, learning in complex working environments is very time-consuming and the generated path is frequently not optimal. Local path planning algorithms, such as the dynamic window method (DWA), are sub-optimal methods based on predictive control theory. DWA can safely and effectively avoid obstacles in unknown environments, and has the characteristics of small computation, rapid response, and strong operability [16]. In the field of robot autonomous operations, environment mapping and localization is also an important topic, as it is a prerequisite for the robot to be able to travel on the planned paths. Simultaneous localization and mapping (SLAM) is mainly used to solve the problem of localization and map construction when robots move in unknown environments, and can assist robots in performing tasks such as path planning, autonomous exploration, and navigation [17]. Adaptive Monte Carlo localization (AMCL) is a probabilistic positioning system for robots in two-dimensional motion. Particle filters are used to track robot positions in known maps, which works well for a wide range of local positioning problems [18].

Sampling-based algorithms, such as the rapidly exploring random tree (RRT) [19] and the probabilistic roadmap [20], are widely used today. They have been demonstrated to offer highly effective performance, especially in high-dimensional and large-scale workspaces. The RRT algorithm constructs a random search tree from the initial state and incrementally extends the tree to the target state by continuously sampling waypoints. The generated path consists of a series of consecutive state nodes. Due to the lack of an optimization process, the generated path is not optimal and contains a large number of redundant nodes. Subsequently, a variety of RRT variants such as RRT* [21], RRT*-Connect [22], and Informed RRT* [23] have been proposed and extensively studied to improve path planning performance. RRT* is a significant improvement on the RRT algorithm. It does not directly connect the new sampling node to the nearest node on the tree, but introduces a cost value to optimize the selection of the parent node to achieve a near-optimal solution. It has been proved that the generated path converges to the optimal solution as the sampling nodes tend to infinity [21]. RRT*-Connect grows two trees incrementally simultaneously, one from the start state and another from the goal state. The two trees try to find a connection to obtain a path solution. When the target is in a narrow space or hidden behind an obstacle, it can effectively improve the path search efficiency. To accelerate the convergence, Informed RRT* limits the search area to a subset of the workspace to return a near-optimal solution quickly.

Recently, more and more optimized versions of RRT have been proposed. Qureshi and Ayaz [24] incorporated the artificial potential field algorithm in RRT*. It can greatly reduce the number of iterations, thus improving the memory utilization rate and accelerating the convergence speed. In order to solve the optimal path planning problem in narrow channels, Huang et al. [25] proposed adaptive informed RRT* (AI-RRT*), which can shorten the path length and asymptotically converge to the optimal solution. Wang et al. [26] proposed an elastic band-based rapidly exploring random tree (EB-RRT), which can achieve real-time optimal motion planning for the mobile robot in the dynamic environment. For path planning with kinematic constraints, the parent node selection and rewiring need to satisfy the constraint [27]. Moon and Chung [28] proposed a dual-tree rapidly exploring random tree (DT-RRT). It is composed of a workspace tree and a state tree. The workspace tree search takes is set in the target workspace without considering robot kinematics. Then the path is generated by the extension of the state tree under kinematic constraints. Wang [29] presented a kinematic constrained bi-directional rapidly exploring random tree (KB-RRT*), in which an efficient branch-pruning strategy is proposed to find a better parent node. Kuwata et al. [30] used closed-loop prediction in the RRT algorithm to enable the robot to track paths with kinematic constraints. Hu et al. [31] proposed an extended RRT algorithm to plan the motion of wheeled robots under kinematic constraints. The algorithm uses a path deformation strategy to shift the waypoint away from obstacles to generate short and smooth trajectories.

One problem that should also be noted is that the state of a robot in a workspace includes position and orientation information. In RRT-based algorithms with kinematic constraints, when the position distance and orientation difference between a new node and the target are both smaller than the two set thresholds, the target is considered to be connected [29]. One important reason for this is that, under kinematic constraints, it is difficult for a new node to satisfy both the position and orientation of the target at the same time. The shortest path planning is a classical problem. It has received a lot of attention due to the path length being one of the most useful metrics in the application of mobile robots. The objective is to find the shortest collision-free path between two state nodes in a workspace. Bounded curvature is the fundamental constraint in the motion planning of a car-like robot. Dubins has proved the geometric form of the shortest path with minimum curvature in a 2-D space. Based on this theorem, scholars have further discovered the characteristics of the shortest curvature-constrained path. This greatly extends the length optimal problem under the curvature constraints and enriches the corresponding solution.

In general, RRT*-based algorithms are essentially local optimization methods. They explore the whole configuration space randomly and uniformly to guarantee probabilistic completeness, resulting in slow convergence. The sampling node has a significant influence on the rationality of the path. Currently, most research focuses on how to sample waypoints and how to connect a pair of waypoints, which are two key factors affecting the quality of the path. Under kinematic constraints, the optimization is mainly in the new node generation stage. When a full connection path is generated, the waypoint state (position and orientation) cannot be changed. It is time-consuming to perform parent node selection and rewiring for each sampling node, and it is also not practical to obtain an optimal path through infinite samples. Therefore, the motivation of this paper is to combine the path optimization method with RRT path planning, and then propose an effective post waypoint shift strategy that can converge the initial generated random path to the shortest.

The main contributions of this study can be summarized as follows:

• (1). A random complete path generation method under kinematic constraints is proposed, which incorporates fast RRT growth and goal connection algorithms.

• (2). An effective waypoint shift algorithm based on Dubins curves is proposed to optimize random paths to the shortest length.

• (3). In dynamic environments, an avoidance method for unknown obstacles on the planning path is proposed.

The remainder of this paper is organized as follows. The details of the path planning procedure of the proposed approach are introduced in Section 2. The simulation results and analysis of the generated paths in different working workspaces are presented in Section 3. In Section 4, the convergence path is proved to be the shortest. Then, the path planning performance of the proposed method is compared with other RRT-based methods, and real-world experiments are presented to show the effectiveness of the method. Finally, the feature property of the proposed model is summarized in Section 5.

2. Proposed Method

This section introduces the proposed method in detail, including the RRT growth strategy, goal connection method, and waypoint shift algorithm. Then, an efficient unknown obstacle avoidance method is presented.

2.1. RRT Growth Strategy

Consider a kinematic model of a car-like mobile robot. The two rear wheels are fixed parallel to the robot body and allowed to roll or spin. The two front wheels can turn to the left or right to change the forward direction. The state of a robot in a 2-D Cartesian coordinates is defined as S = (x, y, θ), and the kinematic model is described by the following equation

(1)$\begin{array}{c}\dot{x}=v\cdot \cos \phi \cdot \cos \theta \\ \dot{y}=v\cdot \cos \phi \cdot \sin \theta \\ \dot{\theta }=v\cdot \sin \phi /l\end{array}$

where v is the driving velocity of the rear wheels, θ is the orientation angle of the robot body with respect to the X-axis, φ is the steering angle of the front wheels, and l is the wheelbase.

Constrained by mechanical structure, the steering angle φ is bounded by |φ| ≤ φ_max. Therefore, the turning radius ρ cannot be less than the minimum turning radius.

(2)$\rho \ge {\rho }_{\min }=l/\tan {\phi }_{\max }$

The RRT path planning is a sampling-based approach. The basic principle is that a random state S_rand is selected from the state space. Then, an extended function selects the nearest node S_nearest already in the tree to connect the sampling state S_rand. If there is no obstacle, a new node S_new is generated from S_nearest to S_rand with an incremental distance ε. This procedure is repeated until the shortest distance between the goal state S_goal and S_new is smaller than a threshold. An extended tree will be generated from the initial state of the robot S_init to the goal state S_goal, and a collision-free path can be obtained. However, since the sampling point is random, the nearest node may not meet the kinematic constraints. It reduces the success rate of the sampling nodes and occupies a lot of computation resources and reduces computing efficiency. This problem is particularly prominent when the bounded front-wheel steering angle |φ| is small.

This paper proposes an improved RRT growth strategy. The main idea is to select the closest node that satisfies the kinematic constraints as the parent node and extends the tree. When a random sampling node is generated, the first step is to find the closest node to the sampling node in the existing tree. Then, starting from this node, a new node is generated in the direction of the sampling node with the step size of ε. If the new node satisfies the kinematic constraints and there are no obstacles on the path, this new node will be added to the tree. Otherwise, the closest node is infeasible, and the next closest node is searched from the remaining nodes to determine its feasibility. If all the nodes in the tree do not meet the requirements, the sampling node will be rejected and another sampling point will be regenerated.

As shown in Figure 1, the initial state of the robot is S_init. A sampling point node 1 is randomly generated. There is only the initial node S_init in the exploring tree and node 1 does not satisfy the kinematic constraints. Therefore, node 1 is rejected. Similarly, node 2 is also rejected. When the sampling point is S₁, the kinematic constraints of the robot are satisfied from S_init to S₁, so, S₁ is added to the exploring tree and a branch is constructed. Suppose there are Nodes S_init, S₁ and S₂ in the tree and the sampling point is node 3: Node 3 will be rejected, because neither S₁ nor S₂ satisfy the kinematic constraints, and there is an obstacle from S_init to node 3. Similarly, nodes 4 and 5 are both rejected. When the sampling point is S_rand, the existing node closest to S_rand is S₈, followed by S₉. However, none of them satisfy the kinematic constraints. The third closest node S₆ satisfies the kinematic constraints, and there is no obstacle within the distance ε from S₆ to the S_rand. Therefore, a new node S_new is generated and a new branch from S₆ to S_new will be constructed. Through continuous sampling towards the goal with a certain probability, the node tree will gradually grow towards the goal area. Once the distance between the goal and the new node is less than the threshold ε₀, the growth of the node tree will stop immediately.

The proposed RRT growth strategy has two characteristics. One is that it can make full use of all nodes in the tree to improve the success rate of the sampling points. When the bounded steering angle of the robot φ_max is small, it is difficult to find a parent node that satisfies the kinematic constraints in a certain area. A large number of sampling points will be rejected. Using the proposed method, any node in the RRT tree can satisfy the kinematic constraints, the sampling point will not be rejected, and a new branch will be generated. The increase in the success rate of the sampling points can effectively improve the growth rate of RRT to the target region, thus improving the efficiency of the initial path generation. The second is that the criterion for stopping tree growth is loose. With the growth of RRT, the orientation of the node near the target is random. It is impossible for the robot to reach the specified position and orientation from a random state in a small area, resulting in the failure to establish a complete path or the need to repeatedly move forward and backward to adjust the state. The proposed method does not consider this problem in the RRT growth process; instead, the goal connection method is subsequently proposed. This allows the tree to quickly find the target area and avoids being trapped near the goal due to kinematic constraints. The algorithm of the tree growth is shown in Algorithm 1.

Algorithm 1. RRT growth algorithm.

Function RRT GrowthInout: Sinit, Sgoal, ρmin, ε, ε0, MapOutput: Node tree T    1:   T0 ← {Sinit}    2:   while k ≤ LimitedNumber do    3:          Srand ← RandomSample(Map)    4:          Distance Li between Srand and Si, ‖Srand − Si‖, i = 1 … N    5:          for i = 1 ··· N do    6:                   Fine the smallest Lj    7:                   Snew j = Sj + ε·(Srand − Sj)/‖Srand − Sj‖    8:                   Subpath Sj → Snew j    9:                   if Feasibility (Sj → Snew j) == True then    10:                        Extend(T, Snew j)    11:                        if ‖Snew j − Sgoal‖ < ε0 then    12:                               Reture T    13:                               end while    14:                        end if    15:                else    16:                        Feasibility Sj == False    17:                end if    18:          end    19:   end while    20:   return failure

2.2. Goal Connection

The tree growth described above can obtain a node path S₁ → S₂ → S₃ → S₄ → S₅, whose distance between the last node S₅ and goal S_goal is less than the threshold ε₀, as shown in Figure 2. According to the RRT growth strategy, the kinematic constraints can be satisfied from S₁ to S₅. Both S_goal and S₅ have their definite position and orientation information. This is needed to plan for the robot to reach the target position in the desired orientation.

This paper proposes a goal connection method based on the minimum turning radius. The first step is to find the minimum turning radius circle according to the goal state. Then, two arcs tangent to the circle can be constructed from a node in the path, and the two arcs rotate in opposite directions. A suitable arc needs to be chosen according to the orientation of S_goal; Figure 2a is clockwise and Figure 2b is counterclockwise. The third step is to calculate the radius of the corresponding tangent arc from S₅ to S₁ sequentially. As shown in Figure 2a, when the radii of tangent arcs corresponding to Nodes S₅, S₄ and S₃ are less than the minimum turning radius ρ_min, they are rejected. When the radius of tangent arc S₂ S₁′ is greater than ρ_min, the arc S₂ S₁′ is taken as the new path segment. The complete planning path is S₁ → S₂ → S₁′ → S_goal. The planned path fully satisfies the robot kinematic constraints and can reach the goal position at the desired orientation. Similarly, the complete path to the goal state in Figure 2b is S₁ → S₂ → S₃ → S₄ → S₁′ → S_goal. The algorithm of the goal connection is shown in Algorithm 2.

Algorithm 2. Goal connection algorithm.

Function Goal ConnectionInput: T, Sgoal, ρminOutput: complete path P    1:   The minimum turning radius circle, C min    2:   for j = M ··· 1 do    3:           Determine the arc tangent to the circle, C j    4:           Calculate the radius of C j, ρj    5:           if ρj ≥ ρmin then    6:                   Calculate the tangent point, S1′    7:                   P ←{ T i = 1 : j, S1′, Sgoal}    8:           end if    9:   end

2.3. Waypoint Shift Algorithm

The RRT growth under kinematic constraints is random and there is no rewiring optimization. Therefore, the initial generated path is not optimal. Here, an effective path optimization method that satisfies the robot kinematics constraints is proposed.

On two consecutive sub-paths, S₁S₂ and S₂S₃, the turning direction of the robot may be the same or the opposite, as shown in Figure 3. The first step is to find the two circles C _{min S1} and C _{min S3} with the minimum turning radius ρ_min corresponding to nodes S₁ and S₃. The second step is to determine the common tangents of the two circles. Obviously, there are four common tangents of the two circles. For the same turning direction, the outer common tangent that is close to nodes S₁ and S₃ is selected (Figure 3a). For the opposite turning direction, the inner common tangent is selected (Figure 3b). The third step is to find the center point S₂′ of the common tangent. If there are no obstacles on the tangent path, the point S₂′ will be used as a new node and the orientation is the direction of the common tangent θ₂′. The optimized path becomes S₁ → S₂′ → S₃. This new path is the shortest path that can satisfy the states of nodes S₁ and S₃ under the kinematic constraints. If there are obstacles on the tangent path, the state of node S₂ remains unchanged, and the path is still S₁ → S₂ → S₃. The fourth step is to select the next group of two consecutive sub-paths formed by S_2′, S₃ and the next node S₄, and use the same method to optimize the path to obtain a new node S₃′. From the initial state S_init to the goal state S_goal, a new optimized path can be generated by adjusting the state of the intermediate nodes in turn. The essence is to use the Dubins curves satisfying the minimum curvature to optimize the path step by step. The algorithm of the waypoint shift is shown in Algorithm 3. Path optimization is a repeated iterative calculation process. As long as the state of the intermediate nodes does not change, an optimal path can be obtained.

Algorithm 3. Waypoint shift algorithm.

Function Waypoint Shift,Input: complete path P, ρmin, mapOutput: optimal path Poptimal    1:   for p = 1 ··· PathStep do     2:           The minimum turning radius circles, C min p, C min p+2    3:           Direction of rotation of subpaths SpSp+1 and Sp+1Sp+2    4:           Determine the arc tangent to the circle, C p,p+2    5:           Center point of C p,p+2, Sp+1′    6:           if Collide(Sp→Sp+1′→Sp+2) == False then    7:               Poptimal p,p+2 ← { Sp, S p+1′, S p+2}    8:               else    9:               Poptimal p,p+2 ← { Sp, S p+1, S p+2}    10:       end    11: end

2.4. Unknown Obstacle Avoidance

The work environment is often dynamic. While the robot is moving, the on-board radar will monitor the surrounding environment in real time. As shown in Figure 4, this is assuming that the original optimized path is S₁ → S₂ → … → S₁₀ → S₁₁ are marked as the green point ‘•’. When the robot moves to node S₃ is marked as the blue point ‘•’, it finds an unknown obstacle blocking the planned path. In this case, the robot needs to re-plan the path to bypass the obstacle.

When the edge of an unknown obstacle is detected, the robot will turn to the corner of the obstacle and travel in a straight line in a tangential orientation. When the robot bypasses the obstacle and reaches node S₁′, it can find some original path nodes, S₈ to S₁₁, that are not occupied by the obstacle. It will use the goal connection method shown in Figure 2 to search the nearest node S₉ that satisfies the kinematic constraints. The re-planned path becomes S₁′→S₉→S₁₀→S₁₁, since the path after node S₉ is feasible when there are no new unknown obstacles. Using the proposed waypoint shift algorithm, the shortest path can be optimized in the current situation.

3. Simulations and Analysis

In this section, to verify the performance of the proposed method, the path planning simulations are carried out in an open workspace, a maze workspace, and a workspace cluttered with small obstacles. The initial random path generation and the path optimization are analyzed. Subsequently, one simulation is performed in a dynamic environment with unknown obstacles.

3.1. Open Workspace

To demonstrate the basic characteristics of the proposed approach, the path planning is first carried out in an open workspace, as shown in Figure 5. It is assumed here that the minimum turning radius of the robot ρ_min is 12, the incremental distance ε is 12, and the threshold ε₀ is 15. The size of the workspace is 400 × 400 (unit: pixel). The initial state S_init = (350, 50, π/4) and the goal state S_goal = (50, 350, π/2). The path planning is conducted twice, and two initial random paths are generated. Then, the proposed waypoint shift method is used to optimize the two random paths. The robot will turn counterclockwise along the minimum turning circle of the initial state 𝒞_init. Then, it moves straight along the inner tangent of 𝒞_init and the minimum turning circle of the goal state 𝒞_goal, and finally, it turns clockwise to the goal state along 𝒞_goal. Although the initial paths are different, the optimized paths can gradually converge to the same line, as marked with the red point ‘•’.

3.2. Maze Workspace

Figure 6 shows a maze workspace with a size of 800 × 400. There is a spike obstacle and a concave obstacle. The initial state of the robot is S_init = (100, 80, π/2).

When the goal state is S_goal = (300, 700, π/2), Figure 6a shows the path planning result. In the RRT growth stage, the paths to the goal area are obtained, which are marked as blue empty circles ‘○’. Then, the tangent point S₁′ to the minimum turning radius arc is calculated according to the goal connection method, and the complete path is generated, which is marked as the pink point ‘•’. Due to the constraint of the minimum turning radius, some nodes close to the goal are discarded. Then, the proposed waypoint shift algorithm is used to optimize the initial random paths. Path optimization is an iterative process from the initial state to the convergence state. With the increase in the iterative calculation, the path will become smoother. When the number of iteration calculations is 100, the obtained path is marked as the blue point ‘•’. By further increasing the number of iteration calculations, the path will gradually converge, which is marked as the red point ‘•’. The robot adjusts its forward direction along the arc with the minimum turning radius from the initial state S_init, travels in a straight line to the tangent point of the arc with the minimum turning radius at point A, and then uses the same method to bypass points C and D to reach the goal state S_goal. It is a concave obstacle between points C and D. The proposed optimization method does not fall into this trap, but selects the shortest path to bypass it. This demonstrates that the proposed waypoint shift method has good convergence, and the optimized path does not depend on the initial random path.

When the goal state is S_goal = (300, 700, 3π/2), the generated initial complete path and the optimized path are shown in Figure 6b. It can be found that the convergence path uses the same method to bypass obstacles, and the difference is that it turns counterclockwise along the minimum turning radius arc of S_goal to reach the goal position with the desired orientation of 3π/2, rather than clockwise as shown in Figure 6a.

3.3. Small Obstacle Avoidance

Obstacle avoidance performance is important in evaluating models. Small obstacles often appear in the actual workspaces. To verify the influence of small obstacles on path optimization, simulations are carried out in a complex workspace. As shown in Figure 7, the workspace has a size of 400 × 400 with a rectangular obstacle in the middle area. Some small obstacles are randomly scattered. The initial state and the goal state are S_init = (100, 80, π/4) and S_goal = (300, 700, π/4), respectively. The initially planned path may choose to bypass the rectangular obstacle from above or below to reach the goal state. Using the proposed waypoint shift method, the path can avoid small obstacles and reach the optimal paths. The avoided small obstacles are marked as red dots ‘▪’. This is because the waypoint shift only analyzes whether there are obstacles in the optimized path. As shown in Figure 3, obstacles in the area surrounded by the paths S₁S₂S₃ and S₁S₂′S₃ have no effect on node shift. However, when the obstacle is exactly on the optimal path, the waypoint shift is not feasible. Because the position and number of nodes in the initial paths are different, the optimized paths will be slightly different.

3.4. Dynamic Environment

As shown in Figure 8, it is assumed here that there is a convergence path under a known environment, marked as the pink point ‘•’. When the robot moves to node S₁₂, it detects an unknown obstacle on the planned path. Then, the robot changes its original path and drives to point S₁′ on the edge of the obstacle. After passing point S₁′, the robot finds the original planned path after node S₁₇. Since node S₁₇ does not meet the kinematic constraint, point S₁₈ is chosen as the path node connecting point S₁′. A new convergence path is generated through the waypoint shift, which is marked as the red point ‘•’. The connection method of points S₁′ and S₁₈ is the same as the goal connection method given in Figure 2. Since the path from S₁′ to S₁₈ satisfies the minimum curvature, the convergence path is also composed of segmented Dubins curves. When no new unknown obstacles are found, the original path nodes after node S₁₈ are feasible. This makes it possible to utilize the prior knowledge without performing a new path search. Therefore, this re-planning method is efficient. It should also be noted that there is uncertainty in avoiding an unknown obstacle because the shape of the obstacle is variable. In some worst-case scenarios, when the robot avoids unknown obstacles, it cannot find the original path nodes, or none of the original path nodes satisfy the kinematic constraints. This means that the original path behind the obstacle becomes infeasible, which requires a re-RRT path planning operation to generate a new path. In general, the occurrence of this case is random and accidental, and it is mainly related to the shape of the obstacle and the location in the working environment.

4. Discussion and Experiments

In this section, it is proved that the convergence path is the shortest. Then, the influence of model parameters on the path convergence performance is analyzed. To illustrate the characteristics of the proposed method, the path planning results of the proposed method and previous RRT-based algorithms are compared. Finally, one experiment is carried out in real-world environments.

4.1. Shortest Path Proof

In an open workspace, it can be proved that the convergence path optimized using the waypoint shift is the optimal shortest path. As is well-known, the Dubins curve is the shortest path connecting two states in the plane under curvature constraint. Therefore, the idea is to prove that the convergence path coincides with the Dubins curves.

Proof. 

Assume that the obtained convergence path P is S_init → S₁ → ··· → S_p → S_goal. We can prove by contradiction that nodes S₁ to S_p form a straight line with the same orientation. According to the waypoint shift illustrated in Figure 3, node S₁ is always on the tangent of C _{min Sinit} with the orientation of θ₁. Then, nodes S₁, S₂, and S₃ are consecutive nodes. If they are not in a straight line or the orientation angles are different. There must be a common tangent between C _{min S1} and C _{min S3}, and nodes S₂ and S₂′ cannot coincide. Thus, the path does not converge and the waypoint shift will not stop. This contradicts the assumption that the path is convergent. If there are three consecutive nodes S_i–1, S_i, S_i+1 ⸦ {S₁, S₂, ··· S_p}, they also form a straight line with the orientation of θ. This is due to the fact that node S_p is always on the tangent of C _{min Sgoal} with the orientation of θ_p. Therefore, the orientation of the straight line, θ = θ₁ = ··· = θ_p, can only be the common tangent direction of C _{min Sinit} and C _{min Sgoal}. The convergence path P coincides with the Dubins curves. In the environment with obstacles, the waypoint shift will transfer node S_i to the position closest to the obstacle. The straight path in the open workspace will be tangent to the minimum radius circle C _{min Si} and bypass the obstacle. Therefore, the full path is composed of segmented Dubins curves, as shown in Figure 5. □

4.2. Parameter Sensitivity

Parameter sensitivity is an important index for model evaluation. In the proposed method, the incremental distance ε is a major parameter that affects the path growth rate and the number of nodes in the path. When ε is set to 18, Figure 6c shows the path planning and optimization results. It can be seen that the incremental distance ε has little effect on the convergence path, and the obtained convergence paths are essentially consistent. However, ε will affect the path convergence rate. The reason for this is mainly related to the waypoint shift method. As shown in Figure 3, the adjustment distance from S₂ to S₂′ depends not only on the orientation angles of the three consecutive nodes S₁, S₂, and S₃, but also on the distance between them. As ε increases, the displacement of each node adjustment will also increase, which can accelerate the convergence rate of the initial random path. It should also be pointed out that this convergence rate is related to the initial path. The initial generated path is random. The more it deviates from the convergence path, the more iterative calculations are required.

4.3. Comparative Discussion

In summary, compared to the existing RRT-based approach, the main characteristic of the proposed method is that it post optimizes the initial random path to the shortest path under kinematic constraints.

Since the RRT method was proposed, path optimization has been a focus of this research field. RRT* is the most successful method, in which parent node selection and rewiring are performed. The common problem is that once a new node is added to the tree, its state cannot be changed again, and this is also the main reason why the RRT* algorithm can only obtain near-optimal solutions, especially under kinematic constraints. In this paper, a novel waypoint shift method is proposed. It can optimize the complete path after it is generated, rather than at the new node generation stage. The optimized path has good convergence and robustness, and can effectively avoid the interference of small obstacles. An important feature of the post waypoint shift is that the optimization is limited to the nodes of the path, rather than all exploring nodes on the tree. Taking Figure 6a as an example, to generate an initial path, the total exploring nodes on the tree usually need to number more than 450, and the number of nodes on the path is only about 90. The RRT* algorithm needs to perform parent node selection and rewiring optimization during each node generation [19,28,29]. This means that a lot of node optimization calculations are redundant. With the increase in the size and complexity of the environment, the number of sampling points will increase exponentially. This calculation workload is enormous and time consuming. The proposed method can avoid a lot of unnecessary calculations, thereby improving the efficiency of path optimization.

Another feature is the initial path generation method. In current RRT-based algorithms, when the permitted steering angle of the robot is small, a large proportion of random sampling points cannot meet the kinematic constraints in this way. The workspace is shown in Figure 6a, and the minimum turning radius of the robot ρ_min is 6. The average number of sampling points is about 2600, and the average number of nodes in the exploring tree is about 700. The success rate is about 27%. Using the proposed tree growth strategy, the activity of each node on the tree can be motivated to maximize the success rate of the sampling points. The average number of sampling points and the number of nodes in the exploring tree is about 850, and the corresponding success rates can be increased to about 57%. In addition, the number of nodes in the paths is reduced accordingly. This will significantly improve the efficiency of the initial random path generation, and the subsequent path optimization. This advantage is present even in an open workspace.

4.4. Real-World Experiment

To verify the effectiveness of the proposed method, the experiment was carried out in an office environment using an AKM R550 mobile robot by WHEELTEC in Guangdong, China. The robot is equipped with a Jetson Nano processor, and a positioning and navigation system, as shown in Figure 9a. The size of the robot is 440 mm × 350 mm and the wheelbase is 300 mm. Limited by the steering angle of the front wheels, the minimum turning radius is about 0.5 m. Driven by two MG513 DC deceleration motors, the movement velocity of the robot is set to 0–0.2 m/s. A Radon N10 LiDARby WHEELTEC in Guangdong, China, is mounted on the front of the robot. Its measuring radius is 25 m, scanning frequency is 10 Hz and angular resolution is 0.8°. The actual office workspace size is 6.8 m × 3.1 m, and the test situation is shown in Figure 9b.

The robot first scans the office to obtain prior workspace information, as shown in Figure 9c. Then, an unknown obstacle occurs on the planned path. Its shape and location are unknown. When the on-board LiDAR detects an unknown obstacle while traveling, it scans the boundary of the obstacle in real time and updates the workspace information, as shown in Figure 9d. If the unknown obstacle is on the pre-planned path, the robot will re-plan its path and march to the target state according to the profile occupation. Figure 10 presents the initial planning path and the actual traveling path of the robot center. This shows that the proposed method is effective in practical applications.

5. Conclusions

In this paper, an RRT algorithm with an effective post waypoint shift was proposed. It selects the nearest existing node that satisfies the kinematic constraints as the parent node, thereby improving the success rate of the sampling nodes. Then, the goal connection method based on the minimum turning radius arc is used to generate an initial random path. Finally, waypoint shift iteration is performed under kinematic constraints to achieve the optimal path. The simulation results indicate that the proposed method can present a better solution compared with other RRT-based algorithms in terms of path length and convergence. Several points are worth noting about the proposed model:

• (1). A parent node selection and RRT growth strategy is proposed, which can improve the success rate of sampling nodes. Under kinematic constraints, a goal connection method is proposed, which enables the robot to reach the goal position with the desired orientation.

• (2). An effective post waypoint shift algorithm is proposed to obtain the optimal path. It does not perform parent node selection and rewiring for each sampling node during RRT growth, but performs optimization after establishing an initial random path.

• (3). The proposed waypoint shift method can effectively avoid small obstacles to obtain the convergence path. The obtained path is composed of Dubins curves, which is in the sense of the shortest path in an open workspace and the local optimal path in a multi-channel workspace.

• (4). An efficient method to avoid unknown obstacles is proposed, which makes it possible to utilize the information of the original path instead of re-planning the path. The generated path can still converge to the shortest one in the current state.

• (5). The proposed method is effective and robust. The initial random path, and model parameters such as incremental distance have little influence on the convergence path; however, they will have some influence on the convergence speed. Compared with the existing RRT-based algorithms, it has significant advantages in terms of the sampling point number, the tree node number, and the path node number.

Many facets remain for future investigation. The proposed convergence path will choose the minimum turning radius to achieve a shorter path. The curvature of the path is not continuous, and the robot needs to bypass the obstacle at a low speed. We are working on building transition curves to make the path more comfortable. Furthermore, combining the proposed method with APF and other sampling methods to further improve the RRT growth rate and convergence speed would also be worth studying.

Author Contributions

S.L.: Conceptualization, Methodology, Writing—review & editing. Z.Z.: Data curation, Writing—original draft. J.W.: Software, Writing—original draft. Q.Z.: Investigation, Software. All authors have read and agreed to the published version of the manuscript.

Data Availability Statement

The data are contained within the article.

Conflicts of Interest

The authors declare no conflicts of interest.

Footnotes

Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.

Figures

Figure 1. Demonstration of RRT growth method.

Figure 2. Demonstration of goal connection method. (a) clockwise (b) counterclockwise.

Figure 3. Demonstration of waypoint shift method. (a) the same turning direction (b) the opposite turning direction.

Figure 4. Demonstration of unknown obstacle avoidance.

Figure 5. Path planning in an open workspace.

Figure 6. Path planning in a maze workspace. (a) ε = 12 (b) ε = 12 (c) ε = 18.

Figure 6. Path planning in a maze workspace. (a) ε = 12 (b) ε = 12 (c) ε = 18.

Figure 7. Small obstacle avoidance on path optimization.

Figure 8. Path planning in a workspace with an unknown obstacle.

Figure 9. Real-world experiment situation. (a) the robot body (b) the test situation (c) the prior workspace information (d) updating the workspace information.

Figure 10. Illustration of the robot’s trajectory.

References

1. Katona, K.; Neamah, H.A.; Korondi, P. Obstacle avoidance and path planning methods for autonomous navigation of mobile robot. Sensors; 2024; 24, 3573. [DOI: https://dx.doi.org/10.3390/s24113573] [PubMed: https://www.ncbi.nlm.nih.gov/pubmed/38894362]

2. Valero-Gomez, A.; Gomez, J.V.; Garrido, S.; Moreno, L. The path to efficiency: Fast marching method for safer, more efficient mobile robot trajectories. IEEE Robot. Autom. Mag.; 2013; 20, pp. 111-120. [DOI: https://dx.doi.org/10.1109/MRA.2013.2248309]

3. Ganganath, N.; Cheng, C.; Fernando, T.; Iu, H.H.C.; Tse, C.K. Shortest path planning for energy-constrained mobile platforms navigating on uneven terrains. IEEE Trans. Ind. Electron.; 2018; 14, pp. 4264-4272. [DOI: https://dx.doi.org/10.1109/TII.2018.2844370]

4. Li, X.; Li, G.; Bian, Z. Research on autonomous vehicle path planning algorithm based on improved RRT* algorithm and artificial potential field method. Sensors; 2024; 24, 3899. [DOI: https://dx.doi.org/10.3390/s24123899]

5. Yazici, A.; Kirlik, G.; Parlaktuna, O.; Sipahioglu, A. A dynamic path planning approach for multirobot sensor-based coverage considering energy constraints. IEEE Trans. Cybern.; 2014; 44, pp. 305-314. [DOI: https://dx.doi.org/10.1109/TCYB.2013.2253605] [PubMed: https://www.ncbi.nlm.nih.gov/pubmed/23757551]

6. Ma, Q.; Li, M.; Huang, G.; Ullah, S. Overtaking path planning for CAV based on improved artificial potential field. IEEE Trans. Veh. Technol.; 2024; 73, pp. 1611-1622. [DOI: https://dx.doi.org/10.1109/TVT.2023.3314860]

7. Grenko, T.; Baressi Šegota, S.; Anđelić, N.; Lorencin, I.; Štifanić, D.; Štifanić, J.; Glučina, M.; Franović, B.; Car, Z. On the use of a genetic algorithm for determining Ho–cook coefficients in continuous path planning of industrial robotic manipulators. Machines; 2023; 11, 167. [DOI: https://dx.doi.org/10.3390/machines11020167]

8. Shangguan, L.; Thomasson, J.A.; Gopalswamy, S. Motion Planning for Autonomous Grain Carts. IEEE Trans. Veh. Technol.; 2021; 70, pp. 2112-2123. [DOI: https://dx.doi.org/10.1109/TVT.2021.3058274]

9. Xu, S.; Bi, W.; Zhang, A.; Wang, Y. A deep reinforcement learning approach incorporating genetic algorithm for missile path planning. Int. J. Mach. Learn. Cyber.; 2024; 15, pp. 1795-1814. [DOI: https://dx.doi.org/10.1007/s13042-023-01998-0]

10. Liu, Y.; Hu, L.; Ma, Z. A new adaptive differential evolution algorithm fused with multiple strategies for robot path planning. Arab. J. Sci. Eng.; 2024; 49, pp. 11907-11924. [DOI: https://dx.doi.org/10.1007/s13369-023-08380-w]

11. Zhang, C.; Jia, L.; Liu, S.; Dou, G.; Liu, Y.; Kong, B. Dynamic job allocation method of multiple agricultural machinery cooperation based on improved ant colony algorithm. Sci. Rep.; 2024; 14, 22414. [DOI: https://dx.doi.org/10.1038/s41598-024-73385-w] [PubMed: https://www.ncbi.nlm.nih.gov/pubmed/39341900]

12. Duguleana, M.; Mogan, G. Neural networks based reinforcement learning for mobile robots obstacle avoidance. Expert. Syst. Appl.; 2016; 62, pp. 104-115. [DOI: https://dx.doi.org/10.1016/j.eswa.2016.06.021]

13. Shi, H.; Li, X.; Hwang, K.-S.; Pan, W.; Xu, G. Decoupled visual servoing with fuzzy Q-learning. IEEE Trans. Ind. Inform.; 2018; 14, pp. 241-252. [DOI: https://dx.doi.org/10.1109/TII.2016.2617464]

14. Pradhan, B.; Nandi, A.; Hui, N.B.; Roy, D.S.; Rodrigues, J.J.P.C. A novel hybrid neural network-based multirobot path planning with motion coordination. IEEE Trans. Veh. Technol.; 2020; 69, pp. 1319-1327. [DOI: https://dx.doi.org/10.1109/TVT.2019.2958197]

15. Mazzia, V.; Salvetti, F.; Aghi, D.; Chiaberge, M. DeepWay: A deep learning waypoint estimator for global path generation. Comput. Electron. Agric.; 2021; 184, 106091. [DOI: https://dx.doi.org/10.1016/j.compag.2021.106091]

16. Wang, Z.; Li, G. Research on path planning algorithm of driverless ferry vehicles combining improved A* and DWA. Sensors; 2024; 24, 4041. [DOI: https://dx.doi.org/10.3390/s24134041]

17. Cadena, C.; Carlone, L.; Carrillo, H.; Latif, Y.; Scaramuzza, D.; Neira, J.; Reid, I.; Leonard, J.J. Past resent, and future of simultaneous localization and mapping: Toward the robust-perception age. IEEE Trans. Robot.; 2016; 32, pp. 1309-1332. [DOI: https://dx.doi.org/10.1109/TRO.2016.2624754]

18. Wang, Z.; Tu, H.Y.; Chan, S.X.; Huang, C.K.; Zhao, Y.W. Vision-based initial localization of AGV and path planning with PO-JPS algorithm. Egypt. Inform. J.; 2024; 27, 100527.

19. Zhao, P.; Chang, Y.; Wu, W. Dynamic RRT: Fast Feasible Path Planning in Randomly Distributed Obstacle Environments. J. Intell. Robot. Syst.; 2023; 107, 48. [DOI: https://dx.doi.org/10.1007/s10846-023-01823-4]

20. Marble, J.D.; Bekris, K.E. Asymptotically near-optimal planning with probabilistic roadmap spanners. IEEE Trans. Robot.; 2013; 29, pp. 432-444. [DOI: https://dx.doi.org/10.1109/TRO.2012.2234312]

21. Karaman, S.; Frazzoli, E. Sampling-based algorithms for optimal motion planning. Int. J. Robot. Res.; 2011; 30, pp. 846-894. [DOI: https://dx.doi.org/10.1177/0278364911406761]

22. Chen, J.; Zhao, Y.; Xu, X. Improved RRT-connect based path planning algorithm for mobile robots. IEEE Access; 2021; 9, pp. 145988-145999. [DOI: https://dx.doi.org/10.1109/ACCESS.2021.3123622]

23. Mashayekhi, R.; Idris, M.Y.I.; Anisi, M.H.; Ahmedy, I.; Ali, I. Informed RRT*-connect: An asymptotically optimal single-query path planning method. IEEE Access; 2020; 8, pp. 19842-19852. [DOI: https://dx.doi.org/10.1109/ACCESS.2020.2969316]

24. Qureshi, A.H.; Ayaz, Y. Potential functions based sampling heuristic for optimal path planning. Auton. Robot.; 2016; 40, pp. 1079-1093. [DOI: https://dx.doi.org/10.1007/s10514-015-9518-0]

25. Huang, Y.; Lee, H.H. Adaptive informed RRT*: Asymptotically optimal path planning with elliptical sampling pools in narrow passages. Int. J. Control Autom. Syst.; 2024; 22, pp. 241-251. [DOI: https://dx.doi.org/10.1007/s12555-022-0834-9]

26. Wang, J.; Meng, M.Q.-H.; Khatib, O. EB-RRT: Optimal motion planning for mobile robots. IEEE Trans. Autom. Sci. Eng.; 2020; 17, pp. 2063-2073. [DOI: https://dx.doi.org/10.1109/TASE.2020.2987397]

27. Kleinbort, M.; Solovey, K.; Littlefield, Z.; Bekris, K.E.; Halperin, D. Probabilistic completeness of RRT for geometric and kinodynamic planning with forward propagation. IEEE Robot. Automat. Lett.; 2019; 4, pp. 277-283. [DOI: https://dx.doi.org/10.1109/LRA.2018.2888947]

28. Moon, C.; Chung, W. Kinodynamic planner dual-tree RRT (DT-RRT) for two-wheeled mobile robots using the rapidly exploring random tree. IEEE Trans. Ind. Electron.; 2015; 62, pp. 1080-1090. [DOI: https://dx.doi.org/10.1109/TIE.2014.2345351]

29. Wang, J.K.; Li, B.P.; Meng, M.Q.-H. Kinematic constrained Bi-directional RRT with efficient branch pruning for robot path planning. Expert. Syst. Appl.; 2021; 170, 114541. [DOI: https://dx.doi.org/10.1016/j.eswa.2020.114541]

30. Kuwata, Y.; Teo, J.; Fiore, G.; Karaman, S.; Frazzoli, E.; How, J.P. Real-time motion planning with applications to autonomous urban driving. IEEE Trans. Control Syst. Technol.; 2009; 17, pp. 1105-1118. [DOI: https://dx.doi.org/10.1109/TCST.2008.2012116]

31. Hu, B.; Cao, Z.; Zhou, M. An efficient RRT-based framework for planning short and smooth wheeled robot motion under kinodynamic constraints. IEEE Trans. Ind. Electron.; 2021; 68, pp. 3292-3302. [DOI: https://dx.doi.org/10.1109/TIE.2020.2978701]