# Smooth Path Planning for Robot Docking in Unknown Environment with Obstacles

Authors: Peng Cui; Weisheng Yan; Rongxin Cui; Jiahui Yu
Journal: Complexity
 (2018)

---

## Abstract

This paper presents an integrated approach to plan smooth path for robots docking in unknown environments with obstacles. To determine the smooth collision-free path in obstacle environment, a tree structure with heuristic expanding strategy is designed as the foundation of path planning in this approach. The tree employs 3D Dubins curves as its branches and foundation for path feasibility evaluation. For the efficiency of the tree expanding in obstacle environment, intermediate nodes and collision-free branches are determined inspired by the elastic band theory. A feasible path is chosen as the shortest series of branches that connects to the docking station after the sufficient expansion of the tree. Simulation results are presented to show the validity and feasibility of the proposed approach.


---

## Body

## 1. Introduction

Nowadays autonomous robots are widely used in danger and complex tasks such as aerial surveillance, ocean exploration, and automated transportation [1–5]. As the persistent tasks of robots are increasing, their performances are constrained by the battery capacity and the driver efficiency [6–9]. To address this problem, docking robots to the deployed stations and recharging them attract more and more attention of the researchers in practical applications for their convenience and efficiency [10, 11]. However, if the surrounding environment of the docking station is unknown or partially unknown beforehand, especially for nonholonomic robots such as autonomous underwater vehicles (AUVs) and unmanned ground vehicles, the safety of the robot in docking process may be endangered by the existing obstacles [12–16]. Furthermore, short-sighted collision avoidance maneuvers may also lead to the failure of docking if the docking pose adjustment is not fully considered [17–19]. In this case, the study of smooth path planning technique for robot docking in unknown environment with obstacle is meaningful and practical.Several efforts have been made by researchers to address the path planning problem for robots docking already [13, 15, 20–27]. Among them, one of the most popular approaches is the artificial potential field (APF) method. The basic concept of the APF method assumes that the robot is attracted by the goal and repulsed by the barriers. The attractive effect and repulsive effect are indicated by the attractive potential field and repulsive potential field, respectively. With properly designed potential functions, the robot will be navigated to the goal point and avoid collision in real time. However, the local minimal points of the total potential field in the navigation process may cause the local minima problem of the APF method which will prevent the robot from approaching the destination [24]. In [22], the APF approach is adopted to generate the desired trajectory for an AUV homing and docking. Through modeling the environment with designed potential fields, the minimum field vector is used to navigate the robot to dock in environment with known stationary obstacles. In [23], the collision-free path for AUV docking to a submarine is obtained via the APF approach while considering the kinematic constraints of the vehicle. Local minima problem is also considered in this paper and addressed with particularly designed potential functions and parameters.Another popular approach applied in robot docking is to integrate feasible curves with optimization strategies for path generation [13, 25, 26, 28]. In this approach, the resultant path consists of several path segments which are connected to the waypoints. Firstly, the waypoints are determined and optimized by the global path planners which are usually based on certain searching algorithm or optimization technique. Then, path segments are generated in the form of space curves such as Dubins curves and B-splines so that the motion characters of the robots can be easily addressed [15]. Thus, the quality and efficiency of the integrated approach mainly depend on the feasibility of the global path planner. In [25], the determination of the feasible path for AUV docking is considered in an environment with stationary obstacles. Resultant path is obtained as a series of 2D lines and circles determined by the shortest path faster algorithm. In [27], the path planning problem for multiple underwater gliders rendezvous is addressed while considering the minimal energy consumption target. Modified Dubins curves are presented and combined with the genetic algorithm (GA) to determine the resultant paths. In [28], an integrated motion planning method is proposed for the robots with nonlinear constraints. The rapidly exploring random tree star (RRT∗) algorithm is adopted as the searching strategy in the clustered environment. The RRT∗ approach is a famous sampling-based path planning approach, which determines the collision-free path via sampling and searching of the workspace. As the increase of sample nodes in the workspace, it can provide optimal solution for the path planning problem. A neural network is designed to generate the feasible curves for the RRT∗ algorithm to obtain the near-optimal trajectory.Although various approaches for robot docking have been proposed, the study of smooth path planning technique for robot docking in unknown environment is still meaningful. To the best of the authors’ knowledge, the problem of balancing the feasibility and efficiency of collision-free path planning for docking in unknown environment, especially in the environment with moving obstacles, has not been well addressed yet. For example, the path planning result of the APF approach is sensitive to the parameters of the designed potential fields. Designing the feasible potential fields and choosing proper parameters in the application in unknown environment require the skill and dexterity. Besides, most of the integrated approaches prescribed above adopt the biological intelligence or geometrical model searching method to determine the waypoints in the workspace of robots. The feasibilities of the resultant paths rely on the ample prior information about the environment for robots docking, which is hard to satisfy in practical applications.To address this problem, this paper presents a smooth path planning approach for robot docking in unknown environment with obstacles. Our approach is motivated by the notion of integrating space curves with heuristic searching algorithms. Considering the practical applications, 3D Dubins curves are adopted as the basic path segments in our approach because of their smoothness and simplicity. Their characters are also utilized to design the collision prediction and path feasibility evaluation functions, which provide the foundation of our path planning approach. Then we designed a tree structure with the Dubins branches, which is named the Dubins tree, to explore the environment for feasible path determination and help avoid collision with obstacles. The feasible path for docking is obtained based on the sufficiently expanded Dubins tree. Our approach is executed in a typical planning and replanning mode and only needs the local information, which improves the adaptability of our approach in dynamic environment. To enhance the efficiency of the tree expansion, a heuristic searching strategy is proposed to keep the feasibility of the resultant path based on the elastic band theory.The main contribution of this paper can be summarized as follows. The first contribution is that we presented the multisegment path calculation and feasibility evaluation method, which are derived from the detailed analysis of the characters of 3D Dubins curves in obstacle environment and thus applicable for nonholonomic robots path planning. The second contribution is that we proposed an efficient path replanning strategy for the integrated approaches, where both the necessity of collision avoidance and the feasibility of resultant path are sufficiently considered to optimize the space searching process. This effort can reduce the workload of path planning in crowded environment, which makes our approach promising to apply in time-sensitive tasks. The third contribution of this paper is that our approach can be used to address the smooth path planning problem for the robot with kinematic constraints in unknown environment with both stationary obstacles and moving obstacles.The rest of this paper is organized as follows. The smooth path planning problem for robot docking is formulated in Section2. In Section 3, the analysis and utilization of the Dubins curves in the path planning tasks are presented in detail. The conception and procedures, as well as the algorithm, of our path planning approach are presented in Section 4. Simulations and discussions of the proposed approach are given in Section 5. The conclusion and future work are provided in Section 6.


## 2. Problem Statement

In this paper, we formulate the path planning task for robot docking as a shortest trajectory planning problem with prescribed terminal conditions. To be practical, a 3D mobile robot with kinematic constraints and constant cruising speed is considered as the mathematic model for path planning. In this case, the constrained mobility of the robot is assumed to be indicated by the minimal turning radius which is given byRmin. The configuration of the robot Cr is defined as (1)Cr=Pr,V→r, where Pr is the position of the robot and V→r is the velocity of the robot. Since the cruising speed of the robot is constant, we have V→r=v where v is a positive constant. The limited detection ability of the robot is also considered in this paper, which is common in the practical applications outdoors, and written as the maximal detection range Rs.The docking station considered in this approach is assumed to be static and its configuration is assumed to be known by the robot beforehand. Therefore, the configuration of the docking station is expressed asCd and (2)Cd=Pd,V→d, where Pd is its position and V→d denotes its designed entrance direction.Irregular obstacles are considered in this paper and they are assumed to have been decomposed into several spherical obstacles already. This decomposition technique can be found in [29]. Hence, for an obstacle i, its center is written as Oi, its radius is written as Ri, and its velocity is written as V→i. The obstacle area ΓOi of obstacle i can be described as (3)ΓOi=Px,y,z∣P-Oi≤Ri. For a feasible path L for robot docking, for any obstacle i, there are(4)L∉ΓOit and(5)CrTf=Cd, where Tf is the elapsed time during which the robot reaches the docking station.To simplify the expression, several functions are defined as Table1 presents.Table 1
Mathematical function definition.
Function Expression s(v) calculate the sign of the variablev Θa→,b→ calculate the angle betweena→ and b→ n(S→) normalizesS→


## 3. Design of Dubins Tree

In this section, we present the notation and techniques about the design of Dubins tree in our path planning approach. The collision prediction in our approach is achieved based on the characters of 3D Dubins curves as well. The main results of this section provide the foundations for the proposed path planning approach.


### 3.1. 3D Dubins Curves

Dubins curve is proved to be the optimal trajectory which connects two configurations with bounded curvature in 2D space [30, 31]. Dubins curves are simply composed of simple segments (straight line segments (S) and circular arcs (C)) and smooth enough for robots to follow. These curves are also attractive in the studies of 3D path planning for nonholonomic robots because of their simpleness and smoothness, even if their optimality is not ensured in this situation [32–35]. However, as the dimension increases compared to 2D environment, the calculation consumption of the Dubins curves increases as well. It is disadvantageous especially when plenty of 3D Dubins curves are required, i.e., in the applications of searching in the environment with numerous obstacles. Rather than using modified Dubins curves, i.e., in [32–34], we attempt to overcome this disadvantage through determining 3D Dubins curves based on their geometrical characters [36].A typical form of 3D Dubins curves is named the CSC curve, which is presented in Figure1. The CSC curve consists of two circular segments, C1 (green circular arc) and C2 (red circular arc), and one straight line segment PC1PC2¯ (black straight line). Its initial configuration is written as C1=[P1,V→1] (green arrow) and its final configuration is written as C2=[P2,V→2] (red arrow). The terminal configurations C1 and C2 are on two different planes which are named plane 1 and plane 2 as Figure 1 shows.Figure 1
A typical CSC curve starts fromP1 with V1→ and ends at P2 with V2→.To outline the geometric characters of the CSC curve, three auxiliary straight lines are adopted as shown in Figure1, which are L1, L2, and Li. L1 and L2 are the colinear lines with C1 and C2, respectively. Li is the intersection line of plane 1 and plane 2. In this case, it can be seen from Figure 1 that plane 1 can be determined by L1 and Li. Likewise, plane 2 can be determined by Li and L2. Furthermore, since the circular segments C1 and C1 are on plane 1 and plane 2 correspondingly and intersect with Li at PC1 and PC2 separately, once L1, L2, and Li are determined, all the segments of the CSC curve are determined as well. On this basis, the determination approach of the CSC curve is presented as follows.Assuming that the intersection points ofLi with L1 and L2 are written as PL1 and PL2, we have(6)PL1=P1+k3V→1,PL2=P2+k4V→2,where k3 and k4 are nonzero constants. Because PC1 and PC2 are on Li, they can be obtained as(7)PC1=PL1+k5V→i,PC2=PL2+k6V→i,where k5 and k6 are constants. V→i is the auxiliary vector along with the curves propagation direction.According to the spatial-temporal relations of the CSC curve, we have(8)V→i=sk3nO1P1→×O1PC1→×O1PC1→=-sk4nO2PC2→×O2P2→×O2PC2→.Meanwhile,PC1 and PC2 are also the tangent points of C1 and C2 with the S segment; then(9)PC1O1→×V→i=0,P1O1→×V→1=0,PC2O2→×V→i=0,P2O2→×V→2=0,and(10)PC1O1¯=P1O1¯=R1,PC2O2¯=P2O2¯=R2.It is derived from (9) and (10) that O1 and O2 are on the angle bisectors of ∠PC1PL1P1 and ∠PC2PL2P2, which are described as(11)O1=PL1+k1sk3-V→1+V→i,O2=PL2+k2sk4-V→2+V→i.Nonlinear equations to solvek1 to k6 can be derived from (9) and (10). The parameters of the CSC curve can be determined exactly by solving these nonlinear equations. The length of the Dubins curve can be calculated by adding up its segments, which can be written as P1PC1⌢+PC1PC2+PC2P2⌢. Similarly, the other Dubins curves can be determined via the equations described above with slight modifications.


### 3.2. Dubins Tree

Based on the determination method of 3D Dubins curves, we present theDubins tree as the foundation of path planning for robot docking in obstacle environment. The notion of the Dubins tree is illustrated in Figure 2. A Dubins tree consists of three elements which are the nodes, the branches, and the roots, as the example illustrated in Table 2.Table 2
Dubins tree symbol definitions.
Name Definition Expression node the intermediate configuration C i branches the Dubins curve that connectsCi and Cj A ( i , j ) roots the initial and terminal nodes C 0, CfFigure 2
Outline of the Dubins tree. In this figure, the Dubins tree has 2 roots, 6 nodes, and 10 branches.Hence, the path planning task for robot docking based on the Dubins tree is equal to determine a set of branches that connect each other fromC0 to Cf which are collision-free and shortest. The collision-free set of branches that connects C0 and Cf is defined as thecandidate pathLk(0,f), which can be written as(12)Lk0,f=A0,1,…,Ai,j,…,Ah,f, where all the branches are collision-free. In this case, there are(13)Pr∈Ai,jand(14)Pr∉ΓOmt∣m=1,…,Nofor t∈[Ti,Tj], where No is the number of obstacles. For instance, in Figure 2, the candidate path L1=A0,1,A1,3,A3,f (light blue line) and it is a collision-free candidate path if only stationary obstacles exist in the environment.


### 3.3. Path Planning Based on Dubins Tree

The Dubins treeG(0,f) consists of several candidate paths, which can be described as(15)G0,f=∑k=1NLLk0,f, where NL is the number of the candidate paths.The scheme of the path planning approach based on Dubins tree is presented as follows. Once the planned pathL is infeasible, a Dubins tree G(0,f) is planted and expanded from the initial root to the final root while avoiding obstacles. Sufficient nodes and branches are generated to spread the branches of the Dubins tree through the collision-free space, namely, the tree expansion. They are generated under the guidance of an optimized tree expanding strategy, considering the constraints and requirements of the task. Then, candidate paths are formed by these nodes and branches that connect to the docking station. After the tree expanding process is completed, the shortest candidate path in G(0,f) is selected as the resultant path L.


## 3.1. 3D Dubins Curves

Dubins curve is proved to be the optimal trajectory which connects two configurations with bounded curvature in 2D space [30, 31]. Dubins curves are simply composed of simple segments (straight line segments (S) and circular arcs (C)) and smooth enough for robots to follow. These curves are also attractive in the studies of 3D path planning for nonholonomic robots because of their simpleness and smoothness, even if their optimality is not ensured in this situation [32–35]. However, as the dimension increases compared to 2D environment, the calculation consumption of the Dubins curves increases as well. It is disadvantageous especially when plenty of 3D Dubins curves are required, i.e., in the applications of searching in the environment with numerous obstacles. Rather than using modified Dubins curves, i.e., in [32–34], we attempt to overcome this disadvantage through determining 3D Dubins curves based on their geometrical characters [36].A typical form of 3D Dubins curves is named the CSC curve, which is presented in Figure1. The CSC curve consists of two circular segments, C1 (green circular arc) and C2 (red circular arc), and one straight line segment PC1PC2¯ (black straight line). Its initial configuration is written as C1=[P1,V→1] (green arrow) and its final configuration is written as C2=[P2,V→2] (red arrow). The terminal configurations C1 and C2 are on two different planes which are named plane 1 and plane 2 as Figure 1 shows.Figure 1
A typical CSC curve starts fromP1 with V1→ and ends at P2 with V2→.To outline the geometric characters of the CSC curve, three auxiliary straight lines are adopted as shown in Figure1, which are L1, L2, and Li. L1 and L2 are the colinear lines with C1 and C2, respectively. Li is the intersection line of plane 1 and plane 2. In this case, it can be seen from Figure 1 that plane 1 can be determined by L1 and Li. Likewise, plane 2 can be determined by Li and L2. Furthermore, since the circular segments C1 and C1 are on plane 1 and plane 2 correspondingly and intersect with Li at PC1 and PC2 separately, once L1, L2, and Li are determined, all the segments of the CSC curve are determined as well. On this basis, the determination approach of the CSC curve is presented as follows.Assuming that the intersection points ofLi with L1 and L2 are written as PL1 and PL2, we have(6)PL1=P1+k3V→1,PL2=P2+k4V→2,where k3 and k4 are nonzero constants. Because PC1 and PC2 are on Li, they can be obtained as(7)PC1=PL1+k5V→i,PC2=PL2+k6V→i,where k5 and k6 are constants. V→i is the auxiliary vector along with the curves propagation direction.According to the spatial-temporal relations of the CSC curve, we have(8)V→i=sk3nO1P1→×O1PC1→×O1PC1→=-sk4nO2PC2→×O2P2→×O2PC2→.Meanwhile,PC1 and PC2 are also the tangent points of C1 and C2 with the S segment; then(9)PC1O1→×V→i=0,P1O1→×V→1=0,PC2O2→×V→i=0,P2O2→×V→2=0,and(10)PC1O1¯=P1O1¯=R1,PC2O2¯=P2O2¯=R2.It is derived from (9) and (10) that O1 and O2 are on the angle bisectors of ∠PC1PL1P1 and ∠PC2PL2P2, which are described as(11)O1=PL1+k1sk3-V→1+V→i,O2=PL2+k2sk4-V→2+V→i.Nonlinear equations to solvek1 to k6 can be derived from (9) and (10). The parameters of the CSC curve can be determined exactly by solving these nonlinear equations. The length of the Dubins curve can be calculated by adding up its segments, which can be written as P1PC1⌢+PC1PC2+PC2P2⌢. Similarly, the other Dubins curves can be determined via the equations described above with slight modifications.


## 3.2. Dubins Tree

Based on the determination method of 3D Dubins curves, we present theDubins tree as the foundation of path planning for robot docking in obstacle environment. The notion of the Dubins tree is illustrated in Figure 2. A Dubins tree consists of three elements which are the nodes, the branches, and the roots, as the example illustrated in Table 2.Table 2
Dubins tree symbol definitions.
Name Definition Expression node the intermediate configuration C i branches the Dubins curve that connectsCi and Cj A ( i , j ) roots the initial and terminal nodes C 0, CfFigure 2
Outline of the Dubins tree. In this figure, the Dubins tree has 2 roots, 6 nodes, and 10 branches.Hence, the path planning task for robot docking based on the Dubins tree is equal to determine a set of branches that connect each other fromC0 to Cf which are collision-free and shortest. The collision-free set of branches that connects C0 and Cf is defined as thecandidate pathLk(0,f), which can be written as(12)Lk0,f=A0,1,…,Ai,j,…,Ah,f, where all the branches are collision-free. In this case, there are(13)Pr∈Ai,jand(14)Pr∉ΓOmt∣m=1,…,Nofor t∈[Ti,Tj], where No is the number of obstacles. For instance, in Figure 2, the candidate path L1=A0,1,A1,3,A3,f (light blue line) and it is a collision-free candidate path if only stationary obstacles exist in the environment.


## 3.3. Path Planning Based on Dubins Tree

The Dubins treeG(0,f) consists of several candidate paths, which can be described as(15)G0,f=∑k=1NLLk0,f, where NL is the number of the candidate paths.The scheme of the path planning approach based on Dubins tree is presented as follows. Once the planned pathL is infeasible, a Dubins tree G(0,f) is planted and expanded from the initial root to the final root while avoiding obstacles. Sufficient nodes and branches are generated to spread the branches of the Dubins tree through the collision-free space, namely, the tree expansion. They are generated under the guidance of an optimized tree expanding strategy, considering the constraints and requirements of the task. Then, candidate paths are formed by these nodes and branches that connect to the docking station. After the tree expanding process is completed, the shortest candidate path in G(0,f) is selected as the resultant path L.


## 4. Smooth Path Planning for Docking in Unknown Environment

In this section, we present the design and implementation of the smooth path planning approach for docking in unknown environment.


### 4.1. Collision Prediction

Collision prediction is a key issue for robot path planning in the practical applications especially when the prior information is not sufficiently provided, i.e., in the unknown environment [37–40]. Since the parameters of the branches in our approach can be obtained via the techniques provided in Section 3, inspired by this notion, this issue is addressed based on the characters of Dubins curves [36].Because the candidate paths only consist of C segments and S segments, based on the feasibility evaluation of these segments, the collision prediction process can be implemented efficiently. The descriptions of the planned trajectories are presented as follows. The planned trajectory of the S segment can be described as(16)Prt=P1+tV→1, where P1 represents the start point of S segment and V→1 represents the planned velocity of the robot at P1.There are three types of the C segments with prescribed radiiR which are the minor C curve, the semicircular C curve, and the major C curve. Hence, the last two types are divided into several minor subsegments for calculation efficiency as Figure 3 shows. For the minor C segment case P1P2m⌢, Pr(t) can be expressed as(17)Prt=O+RsinvtRV→x+cosvtRV→y,where t∈[0,RΘOP1→,OP2m→/v], V→x=nOP1→, and V→2=nV→1.Figure 3
Divisions of the C segments. The minor C segment case isP1P2m⌢, the semicircular C segment is P1P2s⌢, and the major C segment is P1P2j⌢.For the semicircular C segment caseP1P2s⌢, it is divided into two minor C subsegments which are P1P2m⌢ and P2mP2s⌢. The division point P2m can be chosen as(18)P2m=O+RnV→1.Thus, the trajectories of these minor C subsegments P1P2m⌢ and P2mP2s⌢ can be expressed according to (17) correspondingly.For the major C segment caseP1P2j⌢, it is divided into three minor subsegments which are P1P2m⌢, P2mP2s⌢, and P2sP2j⌢. In this case, the division points P2m and P2s are chosen as(19)P2m=O+RnP2jO→,P2s=O+RnP1O→.P1P2m⌢, P2mP2s⌢, and P2sP2j⌢ can be expressed according to (17)-(19) correspondingly.In this case, the description of certain trajectory can be derived from the combination of these basic subsegments easily. For instance, the planned trajectory of the CSC curve, as Figure1 shows, is written as(20)Prt=C1t,t∈0,T1St-T1,t∈T1,T2C2t-T2,t∈T2,T3 where C1(t), S(t-T1), and C2(t-T2) represent the trajectories of C1, S, and C2, respectively. Meanwhile, T1=(R/v)ΘO1P1→,O1PC1→, T2=T1+PC1PC2¯/v, and T3=T2+(R/v)ΘO2PC2→,O2P2→.Since the candidate paths consist of several Dubins curves, their planned trajectories can be determined based on the combination of these curves in spatial-temporal order, as well as their feasibilities. In this paper, the movements of the obstacles are assumed to be predictable; the candidate pathLk is collision-free if (13) and (14) hold for all its branches.IfLk is not collision-free, we define the obstacle i that the robot would collide firstly as themaximal collision obstacle, which is written as Oc. The position of the robot when the distance between Pr(t) and Oc will be minimal is defined as themaximal collision pointPc. The moment maximal collision would take place is defined as themaximal collision timeTc. In this case, if there are multiple obstacles in the area, Tc and Pc can be obtained as(21)Tc=minTci,Pc=PrTc, where i=1,2,…,No. Additionally, themaximal collision poseVc→ is defined as(22)Vc→=Vr→Tc.


### 4.2. Heuristic Tree Expanding Strategy

To improve the performance of the proposed approach, the elastic band theory is integrated into the tree expansion strategy which is named the Heuristic Tree Expanding (HTE) strategy [41, 42]. The HTE strategy assumes that there exist interactive forces between each neighbor node connected with one branch as if they were connected by the potential spring. Hence, there are potential spring forces along the branches. In addition, the obstacles are assumed to be able to provide sufficient support forces for these nodes when the nodes contact their surfaces. Hence, according to the elastic band theory, although various nodes are generated in the attempt of expanding, only these nodes whose total forces can be balanced remain [43]. This strategy ensures that not only the collision avoidance but also the optimality of expanding branches is considered in the determination of nodes.The design of the HTE strategy is presented as follows. For the infeasible pathL, its maximal collision obstacle Oc, point Pc, time Tc, and pose Vc→ can be acquired as Section 4.1 presents. According to the state of Oc, two situations are considered in the determination of nodes for Dubins tree expansion. If Oc is a static obstacle, this situation is called the static collision situation. If Oc is a moving obstacle, this situation is called the moving collision situation. The implementation of the HTE strategy is slightly different in the two situations, which are provided as follows.


#### 4.2.1. Static Collision Situation

In the static collision situation, the obstacle avoidance is not urgent because the configuration of the obstacle environment is stable. Hence, the optimality of the resultant path is considered firstly in the tree expanding process. In this case, the nodes for tree expanding are generated on the surfaces of all the static obstacles, which provides the potential for the resultant branches to form collision-free and short candidate paths.For a known static obstaclei, two nodes are determined which are Cia=[Oi+RiN→,Vc→] and Cib=[Oi-RiN→,Vc→], where(23)N→=nOcPc→×Vc→×Vc→,ifOcPc→×Vc→≠0,nS→,ifOcPc→×Vc→=0, and S→ is a nonzero random 3D vector.Furthermore, three angles∠PrPikOi, ∠OiPikPd, and ∠PrPikPd, k∈a,b, are measured to evaluate the feasibilities of these nodes. They are utilized to simulate the potential spring forces of Cik and then the feasibility of this node is determined based on the following inequalities.(24)∠PrPikOi>π2,∠OiPikPd>π2,and(25)∠PrPikOi>∠PrPikPd,∠OiPikPd>∠PrPikPd.Cik is infeasible and excluded from the Dubins tree if one or more of these inequalities hold. It is because, for Cik, its total force cannot be unbalanced in this situation if the branches are generated to connect it and tightened up. Otherwise, Cik is accepted and added to the Dubins tree.An example of the node selection in static collision situation is illustrated in Figure4. In this example, the initial path (black solid line) is infeasible and four nodes, C1a, C1b and Cia, Cib, are determined according to the HTE strategy. The potential spring forces of the nodes are along the dash lines towards C0 and Cf. Besides, the total potential spring forces of these nodes are indicated by the red arrows. In this case, Cia is evaluated to be infeasible according to the HTE strategy and excluded from the tree expanding. The other three nodes are evaluated to be feasible for branches generation so that A0,1a (black solid curve) is accepted as one of the generated branches rather than A0,2a.Figure 4
The implementation example of the HTE strategy in static collision situation.


#### 4.2.2. Moving Collision Situation

In the moving collision situation, the collision avoidance is considered previously for the sake of safety due to the configuration changes of the obstacle environment. Thus, feasible node is solely selected on the maximal collision obstacle in this case to avoid the immediate danger. For a moving maximal collision obstaclei, the node Ci is determined as Ci=[Oc+RcN→,Vc→], where N→ is a certain radial direction of the obstacle i. The velocity of i is also considered in the determination of N→ and Ci for the efficiency of the obstacle-avoiding maneuver, which is inspired by the implement of the velocity obstacle method in [44]. N→ is obtained as(26)N→=-gVc→×VOc→×Vc→,ifVOc→×Vc→≠0gS→,ifVOc→×Vc→=0, where VOc→ is the velocity of Oc.An implementation example of the HTE strategy in moving obstacle situation is presented in Figure5, where the original path (black solid line) is infeasible and C1 is the determined node for branch generation.Figure 5
The implementation example of the HTE strategy in moving obstacle situation.After the determination of nodes for tree expanding, new candidate paths are formed via these generated nodes to the docking station, which is expressed as(27)Lnew0,f=Lnew0,i+Ai,f,where(28)Lnew0,i=Lold0,m+Am,i. Besides, Lnew represents the new candidate path and Lold represents the old infeasible path. These new candidate paths will be evaluated according to the HTE strategy in the further tree expansion.Additionally, to improve the performance of the proposed path planning approach in complex unknown environment, a searching acceleration technique is designed based on the Dubins tree structure and applied in the tree expanding process. Considering that the candidate paths can only grow longer in the expanding process, Proposition1 is derived for the comparison between two candidate paths Li(0,x) and Lj(0,y) with a common node C0.Proposition 1.
IfLi(0,x)≥Lj(0,y) and Lk(0,y)=Li(0,x)+A(x,y), then Lk(0,y)>Lj(0,y).Proposition1 evaluates the potential of the branches to form the feasible path in future and abandons the undesirable nodes. It will obviously reduce the computation burden and enhance the quality of generated branches in the obstacle environment.Consider that, in the environment crowded with obstacles, there will be superabundant nodes generated for processing which will take excessive computation time in the implementation of path planning.To enhance the efficiency of our path planning approach, both the time constraint in practical applications and the sufficiency of the environment exploration are considered in the HTE strategy.Thespreading degree of a node Ci is defined as(29)KCi=-Pi-Oc, where Pi is the position of the node Ci and i is the index of this node. K indicates the compactness of the generated nodes and the larger K is, the more compact the node is.To reduce the computation consumption in tree expansion, the notionmaximal spreading ratioSmax is employed in the HTE strategy to control the number of the generated nodes for efficiency. It indicates the maximal number of the generated nodes Ci in one tree expanding process, where(30)Ci=Ck∣KCk≥Kmaxandk=1,2,…,Smax, and Kmax is the maximal spreading degree.


### 4.3. Smooth Path Planning Algorithm

Based on the Dubins tree, the smooth path planning approach for robot docking is implemented under the guidance of the HTE strategy, as presented below. For the simplification of expression, several useful variables are defined firstly. The unchecked path set is defined asHu and the candidate path set is defined as Hf. Hence, the smooth path planning approach is implemented in four procedures which are illustrated as follows.


#### 4.3.1. Activation

The proposed approach is activated when the configuration of the obstacle environment changes, which is described as(31)O′≠O, where O′ represents the present configuration and O represents the known configuration. This change is probably caused by the detection of new obstacles in path following or the configuration change of certain obstacle. In this case, the planned path L is added into Hu for further evaluation.


#### 4.3.2. Path Evaluation

The feasibility of each planned path inHu is evaluated firstly according to the equations in Section 4.1. If the planned path, i.e., Lk, is feasible, it is added into Hf for the selection. Otherwise, it will be handled in the path generation procedure for path replanning. Then Lk is popped out from Hu.


#### 4.3.3. Path Generation

For the infeasibleLk, the HTE strategy is adopted to generate the nodes and branches for the expansion of G(0,f). Firstly, a level of nodes and branches are determined and connected for collision avoidance. Then branches that connect the nodes to the docking station are generated, as (27) represents. They provide the new planned paths for further evaluation which are saved into Hu. The path evaluation and path generation procedures will be repeated iteratively till the termination condition is satisfied.


#### 4.3.4. Termination

IfHf≠∅, two termination situations are considered, namely, the mature situation and the immature situation. The mature situation represents the fact that all the possible candidate paths are obtained, which is indicated by Hu=∅. In this situation, the Dubins tree has been sufficiently expanded and its shortest candidate path in Hf is chosen as the resultant path. In the immature situation, plenty of candidate paths have been determined; however, Hu≠∅. In this case, the optimality and computation consumption should be balanced to ensure the efficiency. The termination condition in the immature situation is described as(32)Nb≥NoSmaxNd+bjSmax+No,NL≥Noj+0.5,where Nb is the number of whole generated branches; NL is the number of the candidate paths in Hf. Meanwhile, bj is the number of the new generated branches at the jth path generation execution, No is the number of detected obstacles, and Nd is the depth of G(0,f).If one or more equations in (32) hold, which indicates either plenty of branches or candidate paths are generated, the path planning approach terminates and the shortest candidate path is chosen as the resultant path.The algorithm of the prescribed smooth path planning approach is presented as Algorithm1, where the definitions of the functions are given in Table 3.Table 3
Function definition in Algorithm1.
Function Expression T(Ci,Cj,Ri,Rj) determineA(i,j) with Ri and Rj f(L) true ifL is infeasible and vise versa CPL,O collision prediction procedure ofL TEOc,Pc,Vc→,O,Smax tree expanding procedure TBG0,f returns generated branches’ numberAlgorithm 1:The smooth path planning approach.
Input: C r, Cd, Rmin, V→r, Rs Output: L (1) C0←Cr (2) L←T(C0,Cd,Rmin,Rmin) (3) whileCr≠Cddo (4)ifO′≠{O}then (5){O}←{O}∪O′ (6) Push L into Hu, set L=+∞, Nd=0 (7) whileHu≠∅do (8) Pop Lk from Hu (9) iff(Lk)=0then (10) [Oc,Pc,Vc→]←CPLk,{O} (11) {Ci,A(s,i)}←TE[Oc,Pc,Vc→],O,Smax (12) {Li}←{Lk+A(Ci,Cd)} (13)  Push {Li} into Hu (14) else (15) Push {Lk} into Hf (16) ifL<Lkthen (17) L←Lk (18) end if (19) end if (20) ifHf≠∅∧Hu=∅∨TBG(0,f)>bmaxthen (21) break (22) end if (23) end while (24) else (25) follow L (26) end if (27) end while


### 4.4. Case Study

A case study is provided in this section to illustrate the workflow of the prescribed smooth path planning algorithm. Parameters of the robot in this case are set as(33)v=1m/s,Rs=6m,Rmin=1m.The configurations of the obstacles in this case are set as (34)Pr0=0,0,0T,V→r0=1,0,0T,Pd=20,0,0T,V→d=1,0,0T,O1=3,2,1T,V→1=0,0,0T,R1=1m,O2=7,-2,-1T,V→2=0,0,0.2T,R2=2m,O3=12,0,0T,V→3=0,0,0T,R3=3m,O4=16,3,1T,V→4=0,-0.2,0T,R4=1m,O5=19,-3,0T,V→5=0,0,0T,R5=2m.The implementation of Algorithm1 in an unknown environment with both static and moving obstacles is illustrated in Figure 6, where the simulation step length is set as 0.5s and the parameter of Algorithm 1 is set as Smax=2. In these figures, the planned paths, the candidate paths, and generated branches are indicated by the black dash lines, the red dash lines, and cyan dash lines, respectively. The docking station is presented by the surface of a red cylinder. Meanwhile, the motion directions of the moving obstacles are indicated by the red arrows. Black solid lines and the green arrows are used to present the path and the vector of the robot separately.Figure 6
Simulation results of the smooth path planning approach in unknown environment with both static and moving obstacles.
(a)
Initial environment (t=0.00s) (b)
Path replanning to avoidO2 (t=0.00s) (c)
Path replanning for docking (t=3.50s) (d)
Path replanning for docking (t=11.00s) (e)
Path replanning for docking (t=13.70s) (f)
Docking successfully (t=21.38s)The initial configuration of the workspace is illustrated in Figure6(a), where the initial path is not collision-free for the robot docking. Hence, the smooth path planning algorithm is utilized to generate the maneuver of avoiding the moving obstacle O2, which is presented in Figure 6(b). As Figure 6(c) shows, new static obstacle (O3) is detected in the collision avoidance maneuvers and the planned path is blocked by O3. In this case, new path for docking is determined by the proposed path planning algorithm and followed by the robot. The static obstacle O5 and moving obstacle O4 are detected in the path following process chronologically, as Figures 6(d) and 6(e) illustrate, respectively. Because they are evaluated to be collision-free by the proposed path planning algorithm, the planned path navigates the robot to dock successfully. In the path planning process, 24 Dubins curves are generated to determine the feasible path for docking, whose length is 21.38m.


## 4.1. Collision Prediction

Collision prediction is a key issue for robot path planning in the practical applications especially when the prior information is not sufficiently provided, i.e., in the unknown environment [37–40]. Since the parameters of the branches in our approach can be obtained via the techniques provided in Section 3, inspired by this notion, this issue is addressed based on the characters of Dubins curves [36].Because the candidate paths only consist of C segments and S segments, based on the feasibility evaluation of these segments, the collision prediction process can be implemented efficiently. The descriptions of the planned trajectories are presented as follows. The planned trajectory of the S segment can be described as(16)Prt=P1+tV→1, where P1 represents the start point of S segment and V→1 represents the planned velocity of the robot at P1.There are three types of the C segments with prescribed radiiR which are the minor C curve, the semicircular C curve, and the major C curve. Hence, the last two types are divided into several minor subsegments for calculation efficiency as Figure 3 shows. For the minor C segment case P1P2m⌢, Pr(t) can be expressed as(17)Prt=O+RsinvtRV→x+cosvtRV→y,where t∈[0,RΘOP1→,OP2m→/v], V→x=nOP1→, and V→2=nV→1.Figure 3
Divisions of the C segments. The minor C segment case isP1P2m⌢, the semicircular C segment is P1P2s⌢, and the major C segment is P1P2j⌢.For the semicircular C segment caseP1P2s⌢, it is divided into two minor C subsegments which are P1P2m⌢ and P2mP2s⌢. The division point P2m can be chosen as(18)P2m=O+RnV→1.Thus, the trajectories of these minor C subsegments P1P2m⌢ and P2mP2s⌢ can be expressed according to (17) correspondingly.For the major C segment caseP1P2j⌢, it is divided into three minor subsegments which are P1P2m⌢, P2mP2s⌢, and P2sP2j⌢. In this case, the division points P2m and P2s are chosen as(19)P2m=O+RnP2jO→,P2s=O+RnP1O→.P1P2m⌢, P2mP2s⌢, and P2sP2j⌢ can be expressed according to (17)-(19) correspondingly.In this case, the description of certain trajectory can be derived from the combination of these basic subsegments easily. For instance, the planned trajectory of the CSC curve, as Figure1 shows, is written as(20)Prt=C1t,t∈0,T1St-T1,t∈T1,T2C2t-T2,t∈T2,T3 where C1(t), S(t-T1), and C2(t-T2) represent the trajectories of C1, S, and C2, respectively. Meanwhile, T1=(R/v)ΘO1P1→,O1PC1→, T2=T1+PC1PC2¯/v, and T3=T2+(R/v)ΘO2PC2→,O2P2→.Since the candidate paths consist of several Dubins curves, their planned trajectories can be determined based on the combination of these curves in spatial-temporal order, as well as their feasibilities. In this paper, the movements of the obstacles are assumed to be predictable; the candidate pathLk is collision-free if (13) and (14) hold for all its branches.IfLk is not collision-free, we define the obstacle i that the robot would collide firstly as themaximal collision obstacle, which is written as Oc. The position of the robot when the distance between Pr(t) and Oc will be minimal is defined as themaximal collision pointPc. The moment maximal collision would take place is defined as themaximal collision timeTc. In this case, if there are multiple obstacles in the area, Tc and Pc can be obtained as(21)Tc=minTci,Pc=PrTc, where i=1,2,…,No. Additionally, themaximal collision poseVc→ is defined as(22)Vc→=Vr→Tc.


## 4.2. Heuristic Tree Expanding Strategy

To improve the performance of the proposed approach, the elastic band theory is integrated into the tree expansion strategy which is named the Heuristic Tree Expanding (HTE) strategy [41, 42]. The HTE strategy assumes that there exist interactive forces between each neighbor node connected with one branch as if they were connected by the potential spring. Hence, there are potential spring forces along the branches. In addition, the obstacles are assumed to be able to provide sufficient support forces for these nodes when the nodes contact their surfaces. Hence, according to the elastic band theory, although various nodes are generated in the attempt of expanding, only these nodes whose total forces can be balanced remain [43]. This strategy ensures that not only the collision avoidance but also the optimality of expanding branches is considered in the determination of nodes.The design of the HTE strategy is presented as follows. For the infeasible pathL, its maximal collision obstacle Oc, point Pc, time Tc, and pose Vc→ can be acquired as Section 4.1 presents. According to the state of Oc, two situations are considered in the determination of nodes for Dubins tree expansion. If Oc is a static obstacle, this situation is called the static collision situation. If Oc is a moving obstacle, this situation is called the moving collision situation. The implementation of the HTE strategy is slightly different in the two situations, which are provided as follows.


### 4.2.1. Static Collision Situation

In the static collision situation, the obstacle avoidance is not urgent because the configuration of the obstacle environment is stable. Hence, the optimality of the resultant path is considered firstly in the tree expanding process. In this case, the nodes for tree expanding are generated on the surfaces of all the static obstacles, which provides the potential for the resultant branches to form collision-free and short candidate paths.For a known static obstaclei, two nodes are determined which are Cia=[Oi+RiN→,Vc→] and Cib=[Oi-RiN→,Vc→], where(23)N→=nOcPc→×Vc→×Vc→,ifOcPc→×Vc→≠0,nS→,ifOcPc→×Vc→=0, and S→ is a nonzero random 3D vector.Furthermore, three angles∠PrPikOi, ∠OiPikPd, and ∠PrPikPd, k∈a,b, are measured to evaluate the feasibilities of these nodes. They are utilized to simulate the potential spring forces of Cik and then the feasibility of this node is determined based on the following inequalities.(24)∠PrPikOi>π2,∠OiPikPd>π2,and(25)∠PrPikOi>∠PrPikPd,∠OiPikPd>∠PrPikPd.Cik is infeasible and excluded from the Dubins tree if one or more of these inequalities hold. It is because, for Cik, its total force cannot be unbalanced in this situation if the branches are generated to connect it and tightened up. Otherwise, Cik is accepted and added to the Dubins tree.An example of the node selection in static collision situation is illustrated in Figure4. In this example, the initial path (black solid line) is infeasible and four nodes, C1a, C1b and Cia, Cib, are determined according to the HTE strategy. The potential spring forces of the nodes are along the dash lines towards C0 and Cf. Besides, the total potential spring forces of these nodes are indicated by the red arrows. In this case, Cia is evaluated to be infeasible according to the HTE strategy and excluded from the tree expanding. The other three nodes are evaluated to be feasible for branches generation so that A0,1a (black solid curve) is accepted as one of the generated branches rather than A0,2a.Figure 4
The implementation example of the HTE strategy in static collision situation.


### 4.2.2. Moving Collision Situation

In the moving collision situation, the collision avoidance is considered previously for the sake of safety due to the configuration changes of the obstacle environment. Thus, feasible node is solely selected on the maximal collision obstacle in this case to avoid the immediate danger. For a moving maximal collision obstaclei, the node Ci is determined as Ci=[Oc+RcN→,Vc→], where N→ is a certain radial direction of the obstacle i. The velocity of i is also considered in the determination of N→ and Ci for the efficiency of the obstacle-avoiding maneuver, which is inspired by the implement of the velocity obstacle method in [44]. N→ is obtained as(26)N→=-gVc→×VOc→×Vc→,ifVOc→×Vc→≠0gS→,ifVOc→×Vc→=0, where VOc→ is the velocity of Oc.An implementation example of the HTE strategy in moving obstacle situation is presented in Figure5, where the original path (black solid line) is infeasible and C1 is the determined node for branch generation.Figure 5
The implementation example of the HTE strategy in moving obstacle situation.After the determination of nodes for tree expanding, new candidate paths are formed via these generated nodes to the docking station, which is expressed as(27)Lnew0,f=Lnew0,i+Ai,f,where(28)Lnew0,i=Lold0,m+Am,i. Besides, Lnew represents the new candidate path and Lold represents the old infeasible path. These new candidate paths will be evaluated according to the HTE strategy in the further tree expansion.Additionally, to improve the performance of the proposed path planning approach in complex unknown environment, a searching acceleration technique is designed based on the Dubins tree structure and applied in the tree expanding process. Considering that the candidate paths can only grow longer in the expanding process, Proposition1 is derived for the comparison between two candidate paths Li(0,x) and Lj(0,y) with a common node C0.Proposition 1.
IfLi(0,x)≥Lj(0,y) and Lk(0,y)=Li(0,x)+A(x,y), then Lk(0,y)>Lj(0,y).Proposition1 evaluates the potential of the branches to form the feasible path in future and abandons the undesirable nodes. It will obviously reduce the computation burden and enhance the quality of generated branches in the obstacle environment.Consider that, in the environment crowded with obstacles, there will be superabundant nodes generated for processing which will take excessive computation time in the implementation of path planning.To enhance the efficiency of our path planning approach, both the time constraint in practical applications and the sufficiency of the environment exploration are considered in the HTE strategy.Thespreading degree of a node Ci is defined as(29)KCi=-Pi-Oc, where Pi is the position of the node Ci and i is the index of this node. K indicates the compactness of the generated nodes and the larger K is, the more compact the node is.To reduce the computation consumption in tree expansion, the notionmaximal spreading ratioSmax is employed in the HTE strategy to control the number of the generated nodes for efficiency. It indicates the maximal number of the generated nodes Ci in one tree expanding process, where(30)Ci=Ck∣KCk≥Kmaxandk=1,2,…,Smax, and Kmax is the maximal spreading degree.


## 4.2.1. Static Collision Situation

In the static collision situation, the obstacle avoidance is not urgent because the configuration of the obstacle environment is stable. Hence, the optimality of the resultant path is considered firstly in the tree expanding process. In this case, the nodes for tree expanding are generated on the surfaces of all the static obstacles, which provides the potential for the resultant branches to form collision-free and short candidate paths.For a known static obstaclei, two nodes are determined which are Cia=[Oi+RiN→,Vc→] and Cib=[Oi-RiN→,Vc→], where(23)N→=nOcPc→×Vc→×Vc→,ifOcPc→×Vc→≠0,nS→,ifOcPc→×Vc→=0, and S→ is a nonzero random 3D vector.Furthermore, three angles∠PrPikOi, ∠OiPikPd, and ∠PrPikPd, k∈a,b, are measured to evaluate the feasibilities of these nodes. They are utilized to simulate the potential spring forces of Cik and then the feasibility of this node is determined based on the following inequalities.(24)∠PrPikOi>π2,∠OiPikPd>π2,and(25)∠PrPikOi>∠PrPikPd,∠OiPikPd>∠PrPikPd.Cik is infeasible and excluded from the Dubins tree if one or more of these inequalities hold. It is because, for Cik, its total force cannot be unbalanced in this situation if the branches are generated to connect it and tightened up. Otherwise, Cik is accepted and added to the Dubins tree.An example of the node selection in static collision situation is illustrated in Figure4. In this example, the initial path (black solid line) is infeasible and four nodes, C1a, C1b and Cia, Cib, are determined according to the HTE strategy. The potential spring forces of the nodes are along the dash lines towards C0 and Cf. Besides, the total potential spring forces of these nodes are indicated by the red arrows. In this case, Cia is evaluated to be infeasible according to the HTE strategy and excluded from the tree expanding. The other three nodes are evaluated to be feasible for branches generation so that A0,1a (black solid curve) is accepted as one of the generated branches rather than A0,2a.Figure 4
The implementation example of the HTE strategy in static collision situation.


## 4.2.2. Moving Collision Situation

In the moving collision situation, the collision avoidance is considered previously for the sake of safety due to the configuration changes of the obstacle environment. Thus, feasible node is solely selected on the maximal collision obstacle in this case to avoid the immediate danger. For a moving maximal collision obstaclei, the node Ci is determined as Ci=[Oc+RcN→,Vc→], where N→ is a certain radial direction of the obstacle i. The velocity of i is also considered in the determination of N→ and Ci for the efficiency of the obstacle-avoiding maneuver, which is inspired by the implement of the velocity obstacle method in [44]. N→ is obtained as(26)N→=-gVc→×VOc→×Vc→,ifVOc→×Vc→≠0gS→,ifVOc→×Vc→=0, where VOc→ is the velocity of Oc.An implementation example of the HTE strategy in moving obstacle situation is presented in Figure5, where the original path (black solid line) is infeasible and C1 is the determined node for branch generation.Figure 5
The implementation example of the HTE strategy in moving obstacle situation.After the determination of nodes for tree expanding, new candidate paths are formed via these generated nodes to the docking station, which is expressed as(27)Lnew0,f=Lnew0,i+Ai,f,where(28)Lnew0,i=Lold0,m+Am,i. Besides, Lnew represents the new candidate path and Lold represents the old infeasible path. These new candidate paths will be evaluated according to the HTE strategy in the further tree expansion.Additionally, to improve the performance of the proposed path planning approach in complex unknown environment, a searching acceleration technique is designed based on the Dubins tree structure and applied in the tree expanding process. Considering that the candidate paths can only grow longer in the expanding process, Proposition1 is derived for the comparison between two candidate paths Li(0,x) and Lj(0,y) with a common node C0.Proposition 1.
IfLi(0,x)≥Lj(0,y) and Lk(0,y)=Li(0,x)+A(x,y), then Lk(0,y)>Lj(0,y).Proposition1 evaluates the potential of the branches to form the feasible path in future and abandons the undesirable nodes. It will obviously reduce the computation burden and enhance the quality of generated branches in the obstacle environment.Consider that, in the environment crowded with obstacles, there will be superabundant nodes generated for processing which will take excessive computation time in the implementation of path planning.To enhance the efficiency of our path planning approach, both the time constraint in practical applications and the sufficiency of the environment exploration are considered in the HTE strategy.Thespreading degree of a node Ci is defined as(29)KCi=-Pi-Oc, where Pi is the position of the node Ci and i is the index of this node. K indicates the compactness of the generated nodes and the larger K is, the more compact the node is.To reduce the computation consumption in tree expansion, the notionmaximal spreading ratioSmax is employed in the HTE strategy to control the number of the generated nodes for efficiency. It indicates the maximal number of the generated nodes Ci in one tree expanding process, where(30)Ci=Ck∣KCk≥Kmaxandk=1,2,…,Smax, and Kmax is the maximal spreading degree.


## 4.3. Smooth Path Planning Algorithm

Based on the Dubins tree, the smooth path planning approach for robot docking is implemented under the guidance of the HTE strategy, as presented below. For the simplification of expression, several useful variables are defined firstly. The unchecked path set is defined asHu and the candidate path set is defined as Hf. Hence, the smooth path planning approach is implemented in four procedures which are illustrated as follows.


### 4.3.1. Activation

The proposed approach is activated when the configuration of the obstacle environment changes, which is described as(31)O′≠O, where O′ represents the present configuration and O represents the known configuration. This change is probably caused by the detection of new obstacles in path following or the configuration change of certain obstacle. In this case, the planned path L is added into Hu for further evaluation.


### 4.3.2. Path Evaluation

The feasibility of each planned path inHu is evaluated firstly according to the equations in Section 4.1. If the planned path, i.e., Lk, is feasible, it is added into Hf for the selection. Otherwise, it will be handled in the path generation procedure for path replanning. Then Lk is popped out from Hu.


### 4.3.3. Path Generation

For the infeasibleLk, the HTE strategy is adopted to generate the nodes and branches for the expansion of G(0,f). Firstly, a level of nodes and branches are determined and connected for collision avoidance. Then branches that connect the nodes to the docking station are generated, as (27) represents. They provide the new planned paths for further evaluation which are saved into Hu. The path evaluation and path generation procedures will be repeated iteratively till the termination condition is satisfied.


### 4.3.4. Termination

IfHf≠∅, two termination situations are considered, namely, the mature situation and the immature situation. The mature situation represents the fact that all the possible candidate paths are obtained, which is indicated by Hu=∅. In this situation, the Dubins tree has been sufficiently expanded and its shortest candidate path in Hf is chosen as the resultant path. In the immature situation, plenty of candidate paths have been determined; however, Hu≠∅. In this case, the optimality and computation consumption should be balanced to ensure the efficiency. The termination condition in the immature situation is described as(32)Nb≥NoSmaxNd+bjSmax+No,NL≥Noj+0.5,where Nb is the number of whole generated branches; NL is the number of the candidate paths in Hf. Meanwhile, bj is the number of the new generated branches at the jth path generation execution, No is the number of detected obstacles, and Nd is the depth of G(0,f).If one or more equations in (32) hold, which indicates either plenty of branches or candidate paths are generated, the path planning approach terminates and the shortest candidate path is chosen as the resultant path.The algorithm of the prescribed smooth path planning approach is presented as Algorithm1, where the definitions of the functions are given in Table 3.Table 3
Function definition in Algorithm1.
Function Expression T(Ci,Cj,Ri,Rj) determineA(i,j) with Ri and Rj f(L) true ifL is infeasible and vise versa CPL,O collision prediction procedure ofL TEOc,Pc,Vc→,O,Smax tree expanding procedure TBG0,f returns generated branches’ numberAlgorithm 1:The smooth path planning approach.
Input: C r, Cd, Rmin, V→r, Rs Output: L (1) C0←Cr (2) L←T(C0,Cd,Rmin,Rmin) (3) whileCr≠Cddo (4)ifO′≠{O}then (5){O}←{O}∪O′ (6) Push L into Hu, set L=+∞, Nd=0 (7) whileHu≠∅do (8) Pop Lk from Hu (9) iff(Lk)=0then (10) [Oc,Pc,Vc→]←CPLk,{O} (11) {Ci,A(s,i)}←TE[Oc,Pc,Vc→],O,Smax (12) {Li}←{Lk+A(Ci,Cd)} (13)  Push {Li} into Hu (14) else (15) Push {Lk} into Hf (16) ifL<Lkthen (17) L←Lk (18) end if (19) end if (20) ifHf≠∅∧Hu=∅∨TBG(0,f)>bmaxthen (21) break (22) end if (23) end while (24) else (25) follow L (26) end if (27) end while


## 4.3.1. Activation

The proposed approach is activated when the configuration of the obstacle environment changes, which is described as(31)O′≠O, where O′ represents the present configuration and O represents the known configuration. This change is probably caused by the detection of new obstacles in path following or the configuration change of certain obstacle. In this case, the planned path L is added into Hu for further evaluation.


## 4.3.2. Path Evaluation

The feasibility of each planned path inHu is evaluated firstly according to the equations in Section 4.1. If the planned path, i.e., Lk, is feasible, it is added into Hf for the selection. Otherwise, it will be handled in the path generation procedure for path replanning. Then Lk is popped out from Hu.


## 4.3.3. Path Generation

For the infeasibleLk, the HTE strategy is adopted to generate the nodes and branches for the expansion of G(0,f). Firstly, a level of nodes and branches are determined and connected for collision avoidance. Then branches that connect the nodes to the docking station are generated, as (27) represents. They provide the new planned paths for further evaluation which are saved into Hu. The path evaluation and path generation procedures will be repeated iteratively till the termination condition is satisfied.


## 4.3.4. Termination

IfHf≠∅, two termination situations are considered, namely, the mature situation and the immature situation. The mature situation represents the fact that all the possible candidate paths are obtained, which is indicated by Hu=∅. In this situation, the Dubins tree has been sufficiently expanded and its shortest candidate path in Hf is chosen as the resultant path. In the immature situation, plenty of candidate paths have been determined; however, Hu≠∅. In this case, the optimality and computation consumption should be balanced to ensure the efficiency. The termination condition in the immature situation is described as(32)Nb≥NoSmaxNd+bjSmax+No,NL≥Noj+0.5,where Nb is the number of whole generated branches; NL is the number of the candidate paths in Hf. Meanwhile, bj is the number of the new generated branches at the jth path generation execution, No is the number of detected obstacles, and Nd is the depth of G(0,f).If one or more equations in (32) hold, which indicates either plenty of branches or candidate paths are generated, the path planning approach terminates and the shortest candidate path is chosen as the resultant path.The algorithm of the prescribed smooth path planning approach is presented as Algorithm1, where the definitions of the functions are given in Table 3.Table 3
Function definition in Algorithm1.
Function Expression T(Ci,Cj,Ri,Rj) determineA(i,j) with Ri and Rj f(L) true ifL is infeasible and vise versa CPL,O collision prediction procedure ofL TEOc,Pc,Vc→,O,Smax tree expanding procedure TBG0,f returns generated branches’ numberAlgorithm 1:The smooth path planning approach.
Input: C r, Cd, Rmin, V→r, Rs Output: L (1) C0←Cr (2) L←T(C0,Cd,Rmin,Rmin) (3) whileCr≠Cddo (4)ifO′≠{O}then (5){O}←{O}∪O′ (6) Push L into Hu, set L=+∞, Nd=0 (7) whileHu≠∅do (8) Pop Lk from Hu (9) iff(Lk)=0then (10) [Oc,Pc,Vc→]←CPLk,{O} (11) {Ci,A(s,i)}←TE[Oc,Pc,Vc→],O,Smax (12) {Li}←{Lk+A(Ci,Cd)} (13)  Push {Li} into Hu (14) else (15) Push {Lk} into Hf (16) ifL<Lkthen (17) L←Lk (18) end if (19) end if (20) ifHf≠∅∧Hu=∅∨TBG(0,f)>bmaxthen (21) break (22) end if (23) end while (24) else (25) follow L (26) end if (27) end while


## 4.4. Case Study

A case study is provided in this section to illustrate the workflow of the prescribed smooth path planning algorithm. Parameters of the robot in this case are set as(33)v=1m/s,Rs=6m,Rmin=1m.The configurations of the obstacles in this case are set as (34)Pr0=0,0,0T,V→r0=1,0,0T,Pd=20,0,0T,V→d=1,0,0T,O1=3,2,1T,V→1=0,0,0T,R1=1m,O2=7,-2,-1T,V→2=0,0,0.2T,R2=2m,O3=12,0,0T,V→3=0,0,0T,R3=3m,O4=16,3,1T,V→4=0,-0.2,0T,R4=1m,O5=19,-3,0T,V→5=0,0,0T,R5=2m.The implementation of Algorithm1 in an unknown environment with both static and moving obstacles is illustrated in Figure 6, where the simulation step length is set as 0.5s and the parameter of Algorithm 1 is set as Smax=2. In these figures, the planned paths, the candidate paths, and generated branches are indicated by the black dash lines, the red dash lines, and cyan dash lines, respectively. The docking station is presented by the surface of a red cylinder. Meanwhile, the motion directions of the moving obstacles are indicated by the red arrows. Black solid lines and the green arrows are used to present the path and the vector of the robot separately.Figure 6
Simulation results of the smooth path planning approach in unknown environment with both static and moving obstacles.
(a)
Initial environment (t=0.00s) (b)
Path replanning to avoidO2 (t=0.00s) (c)
Path replanning for docking (t=3.50s) (d)
Path replanning for docking (t=11.00s) (e)
Path replanning for docking (t=13.70s) (f)
Docking successfully (t=21.38s)The initial configuration of the workspace is illustrated in Figure6(a), where the initial path is not collision-free for the robot docking. Hence, the smooth path planning algorithm is utilized to generate the maneuver of avoiding the moving obstacle O2, which is presented in Figure 6(b). As Figure 6(c) shows, new static obstacle (O3) is detected in the collision avoidance maneuvers and the planned path is blocked by O3. In this case, new path for docking is determined by the proposed path planning algorithm and followed by the robot. The static obstacle O5 and moving obstacle O4 are detected in the path following process chronologically, as Figures 6(d) and 6(e) illustrate, respectively. Because they are evaluated to be collision-free by the proposed path planning algorithm, the planned path navigates the robot to dock successfully. In the path planning process, 24 Dubins curves are generated to determine the feasible path for docking, whose length is 21.38m.


## 5. Simulations and Discussion

To evaluate the validity and feasibility of the proposed path planning approach, several practical scenarios are presented and discussed in this section. Meanwhile, the performance of the prescribed algorithm is also discussed via the comparisons with the other three path planning approaches, which are the APF approach, the rapidly exploring random tree star (RRT∗) approach, and the DAPF approach we proposed in our previous work [36].The parameters of the robot are the same as (33) in Section 4.4. In addition, the initial configurations of the robot and the docking station in these simulation scenarios are set as (35)Pr0=0,0,0T,V→r0=1,0,0T,Pd=20,0,0T,V→d=1,0,0T, and the step length in the simulations is set as 0.5s.(1) APF Approach. The APF approach is well known for its efficiency in path planning for collision avoidance. In this paper, the potential functions of APF approach are designed as(36)UrepPrOi→=kp21PrOi→-Ri-1PrPd→2,UattPrOi→=ka2Pr-Pd→2, where Urep and Uatt are the repulsive and attractive potential functions and Oi-Pr≤Rs. Besides, ka is the parameter to adjust the strength of the attractive potential field and kr is the parameter to adjust the strength of the repulsive potential field. The parameters of the potential functions in the following simulation scenarios are set as ka=1,kp=5.(2) RRT ∗ Approach. The proposed path planning algorithm is also compared to RRT∗ approach. The algorithm of the RRT∗ approach in obstacle environment is presented as Algorithm 2.Algorithm 2:The RRT∗ path planning approach.
Input:Root N0, Goal Ng, r, Iterations In, ϵ output: L (1) fori=1 to Indo (2) Generate random sample nodes Nr within ϵ to Ni (3) Determine the nearest existent Nj to Ni within r (4) ifthe branch NjNi is collision-freethen (5) Determine Nk with lowest cost from N0 to Ni (6) Set Nk as the parent node of Ni (7) end if (8) end for (9) the shortest path N0Ng→LThe parameters of this algorithm in the following simulation scenarios are set asr=6,ϵ=0.5,In=2000.(3) DAPF Approach. The DAPF approach employs a reactive path planner to determine the maneuvers for robot docking in steps, which is designed based on potential fields and Dubins curves in the previous work of the authors. Its implementation is illustrated in Algorithm 3.Algorithm 3:The DAPF path planning approach.
Input: C r, Cd, Rmin, ks, kv, α Output: L (1)L←TCr,Cd,Rmin,Rmin (2) C′←[O,Vo→,R′] (3) whileCr≠Cddo (4) ifO′⊄Othen (5) R1←R2, R2←Rg′ (6) whilefL=0do (7) [Tg,Og,Rg′,Cm]←minTci∣Dci<Ri′ (8) Lpass-by←TCr,Cm,R1,R2 (9) iffLpass-by=0then (10) L←Lpass-by, R1←R2, R2←Rt (11) continue (12) end if (13) Lfollow-up←TCm,Cd,R1,R2 (14) iffLfollow-up=0then (15)  Replan Lfollow-up (16) else (17) L←Lpass-by+Lfollow-up (18) end if (19) end while (20) end if (21) O←O∪O′ (22) follow L (23) end whileThe parameters of the DAPF are selected asks=1,kv=1,α=0.1.


### 5.1. Scenario 1: Environment with Concave Obstacle

Concave obstacle may cause the local minimum problem in the implements of the path planning approaches based on potential fields, which will prevent the robot from approaching the goal. In scenario 1, 9 static obstacles are employed to form a concave barrier, which may cause the local minimum problem and block the original path for docking. The configurations of the obstacles are set as(37)O1=10,-2,-2T,O2=10,-2,2T,O3=10,2,2T,O4=10,2,-2T,O5=10,-22,0T,O6=10,0,22T,O7=10,0,-22T,O8=10,22,0T,O9=12,0,0T,R1=2m,R2=2m,R3=2m,R4=2m,R5=3m.Resultant paths of the prescribed path planning approaches in scenario 1 are presented in Figure7. As Figure 7(a) shows, the APF approach is unable to overcome the local minimum problem and trapped in the obstacle area. The resultant path of the DAPF approach is presented in Figure 7(b). The length of this path is 22.60m; meanwhile, it only takes 5 branches in scenario 1 to obtain the resultant path. From this figure we can also see that the planned path is free from the affection of the concave barrier and reaches the docking station smoothly. The path planned by the RRT∗ approach is presented in Figure 7(c), which shows that this approach avoids the concave barrier successfully and the path reaches the docking station as well. However, in this approach, it totally takes 2000 sample nodes and 1478 branches to determine the resultant path, whose length is 32.07m. Therefore, it is inefficient in real-time applications especially when the calculation of branches is time-consuming. The resultant path of the smooth path planning approach is presented in Figure 7(c). From this figure we can see that the smooth path planning approach is capable of handling the local minimum problem and navigating the robot to dock with smooth path. It only takes 54 Dubins branches to determine the resultant path in this scenario, whose length is 23.15m. Meanwhile, in the path planning process of our proposed approach, the feasible path for docking is selected from 8 evenly spread candidate paths in the obstacle area, which attempts to avoid the local optimal problem of the path planning result.Figure 7
Simulation results of the path planning approaches in scenario 1.
(a)
Path planned by APF (b)
Path planned by DAPF (c)
Path planned by RRT∗ (d)
Path planned by the smooth path planning approach


### 5.2. Scenario 2: Environment with Numerous Obstacles

The efficiency of path planning approaches in complex environment is critical to the applications of robots in practice. In scenario 2, we present a discussion about the prescribed approaches in the environment crowded with numerous obstacles. To verify the performance of our smooth path planning approach, comparisons are implemented in two cases with slight difference.


#### 5.2.1. Case 1: Environment Crowded with Numerous Obstacles

The configurations of these obstacles in case 1 are set as(38)O1=5,1,-1T,O2=6,-3,3T,O3=8.5,0,1T,O4=9,-4,0T,O5=10,3,3T,O6=11,-1,-1T,O7=12,-2,2T,O8=13,2,-1T,O9=16,0.5,1T,R1=2m,R2=2m,⋯R9=2m.The path planning results of the approaches prescribed above in scenario 2 are presented correspondingly in Figure8. Figure 8(a) shows that the traditional APF can navigate the robot to approach the docking station with collision-free path in the crowded environment. The length of the resultant path is 23.50m; however, the feasibility of the robot’s final pose is not ensured. Figure 8(b) presents the resultant path planned by the DAPF approach. It generates 11 branches in this approach to determine the maneuvers of the robot in the crowded environment, whose length is 21.60m. The path planned by the RRT∗ is presented in Figure 8(c), which proves that the RRT∗ is capable of planning the collision-free path as well through generating plenty of sample nodes and branches in the workspace. 1604 branches are evaluated and utilized to determine the resultant path whose length is 31.43m. Nevertheless, the calculation of this approach is time-consuming and the resultant path as well as the final pose is infeasible for the robot docking. The path planning result of our proposed approach in this paper is shown in Figure 8(d), which proves that this approach is capable of navigating the docking process of the robot smoothly in crowded environment with desired pose. It only takes 229 branches, which is less than the RRT∗, to determine the planned path whose length is 23.46.Figure 8
Simulation results of the path planning approaches in case 1, scenario 2.
(a)
Path planned by APF (b)
Path planned by DAPF (c)
Path planned by RRT∗ (d)
Path planned by the proposed approach


#### 5.2.2. Case 2: Short-Sighted Problem

The short-sighted problem indicates that the local maneuver is mainly focused on rather than the global feasibility in path planning. This problem would lead to the result that no feasible subsequent maneuver can be determined because of the previous low-quality movement.This problem is considered in our comparison in case 2. To prove the performance of the smooth path planning approach, the environment of case 2 is modified based on case 1, whereO3 is set as 8,0,1T in this case. The path planning results of the DAPF and our smooth path planning approach are compared in this case, which are presented in Figure 9 separately. Figures 9(a), 9(b), and 9(c) present the performance of the DAPF approach in this case. These figures show that the path planning process of the DAPF is trapped in the crowded environment. Because the local environment is not sufficiently searched in this path planning approach, the robot is trapped in the obstacle area because of its constrained turning ability. Compared with the DAPF approach, more branches are generated in the work area of the robot in the proposed approach, which provides more possibility to determine the feasible path for docking. Thus, as Figure 9(d) shows, the smooth path planning approach is still capable of determining the feasible path for robot docking in the crowded environment. The length of the resultant path is 23.66m, which is only slightly longer than in scenario 2. Meanwhile, our proposed path planning approach is easy to implement because only 130 branches are utilized in its path planning process in this case.Figure 9
Simulation results of the DAPF and the smooth path planning approach in case 2, scenario 2.
(a)
Path planned by DAPF (b)
Path planned by DAPF (c)
Path planned by DAPF (d)
Path planned by the proposed approach


### 5.3. Scenario 3: Simulation Results with Physical Engine

Another simulation is implemented in the virtual robot experimentation platform V-REP with physical simulation engine to prove the validity of the proposed smooth path planning approach.The obstacles in this simulation are set as(39)Pd=10,0,0T,V→d=1,0,0T,O1=3,-3,0T,O2=3,4.5,0T,O3=3.5,0.5,0T,O4=6,-1.5,0T,O5=6.5,4,0T,O6=7.5,1,0T,O7=8,-4,0T,R1′=R2′=⋯=R7′=0.5m. Because the practical constraints, such as physical size of the robot and the fitness of the ground, are considered in the path planning task, the radii of the obstacles are set as Ri=1m to remain 0.5m as the safety margin for the robot. Matlab simulation result of the smooth path planning approach in scenario 3 is presented in Figure 10(a). 129 branches are generated in the planning and replanning process to determine the feasible path for docking. Numeric simulation result proves that the proposed approach can determine the feasible path for 2D robot docking in obstacle environment.Figure 10
The path planning result of the proposed approach in scenario 3.
(a)
Simulation result in Matlab (b)
Initial path in V-REP (c)
Feasible path in V-REPMoreover, the performance of the proposed path planning approach is evaluated with physical simulation engine. We build the simulation scenario based on the walls and nature models provided in the V-REP PRO EDU software. The resultant path is replicated according to the Matlab simulation result and followed by the robot named Line Tracer in the V-REP. The Line Tracer robot only employs three sensors to trace the planned path, which are placed in its front-left, front-middle, and front-right sides. Additionally, it adopts an open-loop controller to control its mobile behavior, which means that the performance of path following mainly depends on the quality of path planning. Corresponding simulation result in V-REP is presented in Figures10(b) and 10(c). Firstly, as Figure 10(b) shows, only a part of the obstacles can be detected, and a smooth path is generated to navigate the robot which is marked by cyan in this figure. As the robot moves on, the initial path is evaluated to be infeasible and the subsequent path for docking is replanned immediately. The path replanning result is presented in Figure 10(c), where the resultant path is marked by green and the maneuvers of the robot are marked by yellow. From this figure we can see that the Line Tracer robot follows the planned path stably and reaches the docking station eventually. The trajectory of the Line Tracer robot in these figures proves that our smooth path planning approach can navigate the mobile robot to dock and its resultant path is feasible for robots to follow.


## 5.1. Scenario 1: Environment with Concave Obstacle

Concave obstacle may cause the local minimum problem in the implements of the path planning approaches based on potential fields, which will prevent the robot from approaching the goal. In scenario 1, 9 static obstacles are employed to form a concave barrier, which may cause the local minimum problem and block the original path for docking. The configurations of the obstacles are set as(37)O1=10,-2,-2T,O2=10,-2,2T,O3=10,2,2T,O4=10,2,-2T,O5=10,-22,0T,O6=10,0,22T,O7=10,0,-22T,O8=10,22,0T,O9=12,0,0T,R1=2m,R2=2m,R3=2m,R4=2m,R5=3m.Resultant paths of the prescribed path planning approaches in scenario 1 are presented in Figure7. As Figure 7(a) shows, the APF approach is unable to overcome the local minimum problem and trapped in the obstacle area. The resultant path of the DAPF approach is presented in Figure 7(b). The length of this path is 22.60m; meanwhile, it only takes 5 branches in scenario 1 to obtain the resultant path. From this figure we can also see that the planned path is free from the affection of the concave barrier and reaches the docking station smoothly. The path planned by the RRT∗ approach is presented in Figure 7(c), which shows that this approach avoids the concave barrier successfully and the path reaches the docking station as well. However, in this approach, it totally takes 2000 sample nodes and 1478 branches to determine the resultant path, whose length is 32.07m. Therefore, it is inefficient in real-time applications especially when the calculation of branches is time-consuming. The resultant path of the smooth path planning approach is presented in Figure 7(c). From this figure we can see that the smooth path planning approach is capable of handling the local minimum problem and navigating the robot to dock with smooth path. It only takes 54 Dubins branches to determine the resultant path in this scenario, whose length is 23.15m. Meanwhile, in the path planning process of our proposed approach, the feasible path for docking is selected from 8 evenly spread candidate paths in the obstacle area, which attempts to avoid the local optimal problem of the path planning result.Figure 7
Simulation results of the path planning approaches in scenario 1.
(a)
Path planned by APF (b)
Path planned by DAPF (c)
Path planned by RRT∗ (d)
Path planned by the smooth path planning approach


## 5.2. Scenario 2: Environment with Numerous Obstacles

The efficiency of path planning approaches in complex environment is critical to the applications of robots in practice. In scenario 2, we present a discussion about the prescribed approaches in the environment crowded with numerous obstacles. To verify the performance of our smooth path planning approach, comparisons are implemented in two cases with slight difference.


### 5.2.1. Case 1: Environment Crowded with Numerous Obstacles

The configurations of these obstacles in case 1 are set as(38)O1=5,1,-1T,O2=6,-3,3T,O3=8.5,0,1T,O4=9,-4,0T,O5=10,3,3T,O6=11,-1,-1T,O7=12,-2,2T,O8=13,2,-1T,O9=16,0.5,1T,R1=2m,R2=2m,⋯R9=2m.The path planning results of the approaches prescribed above in scenario 2 are presented correspondingly in Figure8. Figure 8(a) shows that the traditional APF can navigate the robot to approach the docking station with collision-free path in the crowded environment. The length of the resultant path is 23.50m; however, the feasibility of the robot’s final pose is not ensured. Figure 8(b) presents the resultant path planned by the DAPF approach. It generates 11 branches in this approach to determine the maneuvers of the robot in the crowded environment, whose length is 21.60m. The path planned by the RRT∗ is presented in Figure 8(c), which proves that the RRT∗ is capable of planning the collision-free path as well through generating plenty of sample nodes and branches in the workspace. 1604 branches are evaluated and utilized to determine the resultant path whose length is 31.43m. Nevertheless, the calculation of this approach is time-consuming and the resultant path as well as the final pose is infeasible for the robot docking. The path planning result of our proposed approach in this paper is shown in Figure 8(d), which proves that this approach is capable of navigating the docking process of the robot smoothly in crowded environment with desired pose. It only takes 229 branches, which is less than the RRT∗, to determine the planned path whose length is 23.46.Figure 8
Simulation results of the path planning approaches in case 1, scenario 2.
(a)
Path planned by APF (b)
Path planned by DAPF (c)
Path planned by RRT∗ (d)
Path planned by the proposed approach


### 5.2.2. Case 2: Short-Sighted Problem

The short-sighted problem indicates that the local maneuver is mainly focused on rather than the global feasibility in path planning. This problem would lead to the result that no feasible subsequent maneuver can be determined because of the previous low-quality movement.This problem is considered in our comparison in case 2. To prove the performance of the smooth path planning approach, the environment of case 2 is modified based on case 1, whereO3 is set as 8,0,1T in this case. The path planning results of the DAPF and our smooth path planning approach are compared in this case, which are presented in Figure 9 separately. Figures 9(a), 9(b), and 9(c) present the performance of the DAPF approach in this case. These figures show that the path planning process of the DAPF is trapped in the crowded environment. Because the local environment is not sufficiently searched in this path planning approach, the robot is trapped in the obstacle area because of its constrained turning ability. Compared with the DAPF approach, more branches are generated in the work area of the robot in the proposed approach, which provides more possibility to determine the feasible path for docking. Thus, as Figure 9(d) shows, the smooth path planning approach is still capable of determining the feasible path for robot docking in the crowded environment. The length of the resultant path is 23.66m, which is only slightly longer than in scenario 2. Meanwhile, our proposed path planning approach is easy to implement because only 130 branches are utilized in its path planning process in this case.Figure 9
Simulation results of the DAPF and the smooth path planning approach in case 2, scenario 2.
(a)
Path planned by DAPF (b)
Path planned by DAPF (c)
Path planned by DAPF (d)
Path planned by the proposed approach


## 5.2.1. Case 1: Environment Crowded with Numerous Obstacles

The configurations of these obstacles in case 1 are set as(38)O1=5,1,-1T,O2=6,-3,3T,O3=8.5,0,1T,O4=9,-4,0T,O5=10,3,3T,O6=11,-1,-1T,O7=12,-2,2T,O8=13,2,-1T,O9=16,0.5,1T,R1=2m,R2=2m,⋯R9=2m.The path planning results of the approaches prescribed above in scenario 2 are presented correspondingly in Figure8. Figure 8(a) shows that the traditional APF can navigate the robot to approach the docking station with collision-free path in the crowded environment. The length of the resultant path is 23.50m; however, the feasibility of the robot’s final pose is not ensured. Figure 8(b) presents the resultant path planned by the DAPF approach. It generates 11 branches in this approach to determine the maneuvers of the robot in the crowded environment, whose length is 21.60m. The path planned by the RRT∗ is presented in Figure 8(c), which proves that the RRT∗ is capable of planning the collision-free path as well through generating plenty of sample nodes and branches in the workspace. 1604 branches are evaluated and utilized to determine the resultant path whose length is 31.43m. Nevertheless, the calculation of this approach is time-consuming and the resultant path as well as the final pose is infeasible for the robot docking. The path planning result of our proposed approach in this paper is shown in Figure 8(d), which proves that this approach is capable of navigating the docking process of the robot smoothly in crowded environment with desired pose. It only takes 229 branches, which is less than the RRT∗, to determine the planned path whose length is 23.46.Figure 8
Simulation results of the path planning approaches in case 1, scenario 2.
(a)
Path planned by APF (b)
Path planned by DAPF (c)
Path planned by RRT∗ (d)
Path planned by the proposed approach


## 5.2.2. Case 2: Short-Sighted Problem

The short-sighted problem indicates that the local maneuver is mainly focused on rather than the global feasibility in path planning. This problem would lead to the result that no feasible subsequent maneuver can be determined because of the previous low-quality movement.This problem is considered in our comparison in case 2. To prove the performance of the smooth path planning approach, the environment of case 2 is modified based on case 1, whereO3 is set as 8,0,1T in this case. The path planning results of the DAPF and our smooth path planning approach are compared in this case, which are presented in Figure 9 separately. Figures 9(a), 9(b), and 9(c) present the performance of the DAPF approach in this case. These figures show that the path planning process of the DAPF is trapped in the crowded environment. Because the local environment is not sufficiently searched in this path planning approach, the robot is trapped in the obstacle area because of its constrained turning ability. Compared with the DAPF approach, more branches are generated in the work area of the robot in the proposed approach, which provides more possibility to determine the feasible path for docking. Thus, as Figure 9(d) shows, the smooth path planning approach is still capable of determining the feasible path for robot docking in the crowded environment. The length of the resultant path is 23.66m, which is only slightly longer than in scenario 2. Meanwhile, our proposed path planning approach is easy to implement because only 130 branches are utilized in its path planning process in this case.Figure 9
Simulation results of the DAPF and the smooth path planning approach in case 2, scenario 2.
(a)
Path planned by DAPF (b)
Path planned by DAPF (c)
Path planned by DAPF (d)
Path planned by the proposed approach


## 5.3. Scenario 3: Simulation Results with Physical Engine

Another simulation is implemented in the virtual robot experimentation platform V-REP with physical simulation engine to prove the validity of the proposed smooth path planning approach.The obstacles in this simulation are set as(39)Pd=10,0,0T,V→d=1,0,0T,O1=3,-3,0T,O2=3,4.5,0T,O3=3.5,0.5,0T,O4=6,-1.5,0T,O5=6.5,4,0T,O6=7.5,1,0T,O7=8,-4,0T,R1′=R2′=⋯=R7′=0.5m. Because the practical constraints, such as physical size of the robot and the fitness of the ground, are considered in the path planning task, the radii of the obstacles are set as Ri=1m to remain 0.5m as the safety margin for the robot. Matlab simulation result of the smooth path planning approach in scenario 3 is presented in Figure 10(a). 129 branches are generated in the planning and replanning process to determine the feasible path for docking. Numeric simulation result proves that the proposed approach can determine the feasible path for 2D robot docking in obstacle environment.Figure 10
The path planning result of the proposed approach in scenario 3.
(a)
Simulation result in Matlab (b)
Initial path in V-REP (c)
Feasible path in V-REPMoreover, the performance of the proposed path planning approach is evaluated with physical simulation engine. We build the simulation scenario based on the walls and nature models provided in the V-REP PRO EDU software. The resultant path is replicated according to the Matlab simulation result and followed by the robot named Line Tracer in the V-REP. The Line Tracer robot only employs three sensors to trace the planned path, which are placed in its front-left, front-middle, and front-right sides. Additionally, it adopts an open-loop controller to control its mobile behavior, which means that the performance of path following mainly depends on the quality of path planning. Corresponding simulation result in V-REP is presented in Figures10(b) and 10(c). Firstly, as Figure 10(b) shows, only a part of the obstacles can be detected, and a smooth path is generated to navigate the robot which is marked by cyan in this figure. As the robot moves on, the initial path is evaluated to be infeasible and the subsequent path for docking is replanned immediately. The path replanning result is presented in Figure 10(c), where the resultant path is marked by green and the maneuvers of the robot are marked by yellow. From this figure we can see that the Line Tracer robot follows the planned path stably and reaches the docking station eventually. The trajectory of the Line Tracer robot in these figures proves that our smooth path planning approach can navigate the mobile robot to dock and its resultant path is feasible for robots to follow.


## 6. Conclusion

To solve the path planning problem for robot docking in unknown obstacle environment, a smooth path planning approach has been proposed in this paper. Considering the pose constraints, Dubins curves have been adopted as the basic path segments and combined with a tree structure for path determination. Based on the elastic band theory, a heuristic strategy has been designed and has been employed to determine the nodes and branches for obstacle avoidance. The shortest collision-free route is chosen as the resultant path for docking after the sufficient expansion. The validity and feasibility of the smooth path planning approach are proved by simulation results.Our approach can handle the obstacle avoidance problem and planning path for the robots with pose constraints. In addition, this approach is efficient in determining the high-quality path for robot to follow. Future areas of researches can be concluded as follows: (i) the parallel computing techniques can be adopted to improve the efficiency of the smooth path planning approach in complex environment with numerous obstacles. (ii) The moving docking station should be considered, which is practical in some special applications of robots. (iii) Winds, water currents, and the changes of the ground materials may affect the feasibility of the resultant path, which should be considered in the further studies.



---

*Source: 4359036-2018-11-08.xml*