# Reactive Path Planning Approach for Docking Robots in Unknown Environment

Authors: Peng Cui; Weisheng Yan; Yintao Wang
Journal: Journal of Advanced Transportation
 (2017)

---

## Abstract

Autonomous robots need to be recharged and exchange information with the host through docking in the long-distance tasks. Therefore, feasible path is required in the docking process to guide the robot and adjust its pose. However, when there are unknown obstacles in the work area, it becomes difficult to determine the feasible path for docking. This paper presents a reactive path planning approach named Dubins-APF (DAPF) to solve the path planning problem for docking in unknown environment with obstacles. In this proposed approach the Dubins curves are combined with the designed obstacle avoidance potential field to plan the feasible path. Firstly, an initial path is planned and followed according to the configurations of the robot and the docking station. Then when the followed path is evaluated to be infeasible, the intermediate configuration is calculated as well as the replanned path based on the obstacle avoidance potential field. The robot will be navigated to the docking station with proper pose eventually via the DAPF approach. The proposed DAPF approach is efficient and does not require the prior knowledge about the environment. Simulation results are given to validate the effectiveness and feasibility of the proposed approach.


---

## Body

## 1. Introduction

Nowadays autonomous mobile robots such as autonomous drones, autonomous underwater vehicles, and automated vehicles are widely used in the complex environment and undertake the dangerous and heavy tasks [1–8]. However, their durations are constrained by their limited battery capacities and data storage spaces [9]. To solve this problem docking stations are designed and deployed in the work areas of the robots to maintain them in practical applications [10, 11]. To ensure the safety and the success in the docking process, a feasible path is required to be planned firstly [12]. This is because, for one thing, the area for docking may be unknown in advance or dynamically changing and there may be static and moving obstacles that threat the safety of the robots [12, 13]. For another, the final configuration (pose and velocity) of the robot should be adjusted properly to avoid the impact with the docking station. The kinematic characters of the robot should be considered as well for path tracking and energy saving [14–16].In recent years, a variety of approaches have been proposed to solve the path planning problem in unknown environment. Among them a biologically inspired neural network approach is presented in [17] to plan path for the autonomous underwater vehicle in unknown two-dimension (2D) environment, which is achieved via updating the environment maps according to Dempster-Shafer theory in steps. In [18] the online path planning problem with prescribed target in environment with unknown obstacles is considered and the neural networks trained by the reinforcement learning approach are adopted to solve this problem. In [19] the rapidly exploring random trees star (RRT∗) algorithm is employed to plan the path for autonomous underwater vehicles, where the mutual information between the scalar field model and observations is used to improve the path planning result. In [20] the path planning problem for household robot in unknown environment is considered and solved by the modified artificial potential field (APF) method based on the motion characteristics of household animals. In [21] the collision-free path planning for autonomous container truck is achieved via utilizing the improved ant colony optimization (ACO) algorithm. In this algorithm the local path is generated according to the selected local target which is determined and updated by the rolling window approach. In [22] a dynamic planning algorithm is presented to determine the collision-free path for the mobile terrestrial robot in unknown environment. In this algorithm the local objectives are determined by the genetic algorithm (GA) and the optimum routes are generated dynamically towards the global object. It can be concluded from the efforts above that the efficiency of the path planning approach is considered emphatically in the unknown environment. Meanwhile the reactive frameworks are adaptive in solving the path planning problem in the unknown environment in spite of the fact that different various local path planning approaches such as modified bioinspired method, RRT algorithm, and APF method are adopted. However, the smoothness of the planned path is rarely considered in these efforts as well as the pose and velocity of the robot, which is critical in the docking tasks [23].In this paper, a reactive path planning approach named Dubins-APF is proposed to solve the path planning problem for docking in unknown environment based on combination of the Dubins curves and the APF approach. Dubins curves have been proved to be the optimal paths with minimal turning radius that connect two points with prescribed poses in 2D space [24, 25]. However, if there are obstacles in the environment for docking, it is difficult to determine the feasible path based on Dubins curves [26, 27]. Since the APF approach is efficient in obstacle avoidance, in this paper it is combined with the Dubins curves to determine the feasible path for docking [28–31]. The proposed path planning approach works in a reactive mode which is described as follows. Once the planned path is infeasible, feasible intermediate configurations are determined based on the obstacle avoidance potential field according to the configurations of the detected obstacles. Then feasible Dubins curves are generated as the replanned path based on the intermediate configuration. Through implementing this path planning-replanning strategy continually, the DAPF algorithm will solve the path planning problem for docking.The main contributions of this paper are as follows:(1)
The geometrical approach to determine the 3D Dubins curves is proposed in this paper. It can be utilized to generate the docking path and evaluate the feasibility of the planned path as well.(2)
The conception of combining the advantages of the Dubins curves and the artificial potential field is proposed and implemented in this paper to improve the quality of the docking path while avoiding obstacles.The structure of this paper is presented as follows. The path planning problem for docking and the notion of the DAPF approach are introduced in Section1. The problem statement and the path generation approach with 3D Dubins curves are described in Section 2. In Section 3 the Dubins-APF path planning approach is proposed and illustrated in detail. The simulation result and discussion about the DAPF approach are presented in Section 4. Some conclusions and future works are provided in Section 5.


## 2. Path Planning with 3D Dubins Curves

### 2.1. Problem Statement

The path planning problem for docking is to determine a collision-free path to connect the initial position of the autonomous robot and the docking station with prescribed poses under certain constraints of the robot [28]. In this paper the docking station with unidirectional entrance is considered and it is assumed to be static. The position of the docking station is written as Pd and the direction of its entrance is expressed as V→d. Meanwhile the velocity of the autonomous robot is assumed be constant and its position and velocity are written as Pr and V→r, where V→r=v. The turning ability of the robot is assumed to be limited and the minimal turning radius is written as Rt. Hence to avoid collision with the docking station, the final position and velocity of the robot should be close to Pd and V→d. Additionally the work area for docking is assumed to be unknown in advance, which means the autonomous robot can only acquire the environment information within its sensor range Rs.


### 2.2. 3D Dubins Curves

The Dubins curve only consists of two kinds of segments which are the circle (C) segment and the straight-line (S) segment, where the radius of the C segment is equal to the minimal turning radius of the robot and this curve is smooth at the intersections of the adjacent segments. In 2D environment the optimality of the two Dubins curves has been proved and the CCC curve or the CSC curve is reckoned as the shortest path [24]. However, the determination of the Dubins curve becomes complex in 3D environment due to the increase of the dimensionality, which means not all the segments of the Dubins curves are coplanar [32, 33]. Therefore considering the efficiency of its application in path planning, a 3D Dubins curve determination approach is presented in this paper based on the geometric characters of the typical CSC curve.The typicalCSC curve is shown in Figure 1. The initial configuration of position and pose (green vector) and the final configuration (red vector) are presented as [P1,V→1] and [P2,V→2] and the feasible path that connects them is a 3D Dubins curve which consists of one S segment and two C segments. The C segments C1 (red arc) and C2 (green arc) are two circular arcs with centers O1 and O2 and radii R1 and R2, respectively. They are connected by the S segment (blue straight line) with intersections P5 and P6 separately. To indicate the coplanarity of the Dubins curve, two auxiliary lines L1 and L2 are drawn as Figure 1 shows. L1 has P1 on it and parallels with V→1 and L2 has P2 on it and parallels with V→2. They intersect with the elongation of the S segment at P3 and P4, respectively. Based on the spatial relations of L1, L2, and the S segment, the segments of the CSC curve can be divided into two planes determined by L1 and the S segment and L2 and the S segment separately. The intersection line of these two planes is the S segment.Figure 1
The illustration of the typical 3DCSC curve.Inspired by the geometric relations of these two planes, the determination process of the 3D Dubins curve is implemented as follows. The intersections ofL1, L2, and the elongation of S are presented as(1)P3=P1+k3V→1,P4=P2+k4V→2,where k3 and k4 are nonzero constants. A special case of (1) should be noticed where either k3 or k4 is infinite. It means that the corresponding C segment is semicircular arc and this case will be discussed later. Once k3 and k4 are determined, the intersections of C1 and C2 with S can be expressed as(2)P5=P3+k5V→3,P6=P4+k6V→3,where k5 and k6 are constants and V→3 is the vector that indicates the moving direction of the robot on the S segment. Since the Dubins curve is smooth at the intersections of every two adjacent segments, V→3 can be obtained(3)V→3=signk3gO1P1→×O1P5→×O1P5→=-signk4gO2P6→×O2P2→×O2P6→=gP5P6→,where sign(s) presents the sign function which returns the sign of the variable s and g(S→) is the function which normalizes the vector S→ and it is defined as(4)gS→=S→S→,ifS→≠0,0,0,0,ifS→=0.Due to the fact that P5 and P6 are the tangent points of C1 and C2 with the S segment, there are(5)P5O1→×V→3=0,P1O1→×V→1=0,P6O2→×V→3=0,P2O2→×V→2=0,(6)P5O1¯=P1O1¯=R1,P6O2¯=P2O2¯=R2.Furthermore, it can be concluded from (6) that P3O1¯ and P4O2¯ are the angle bisector of ∠P3P1O1 and ∠P4P2O2; therefore there are(7)O1=P3+k1signk3-V→1+V→3O2=P4+k2signk4-V→2+V→3.The parametersk1–k6 can be calculated based on the specified set of equations to determine the CSC curve by substituting the variables in (5) and (6) with corresponding expression in (1)–(3) and (7) if there is no semicircular C segment in the CSC curve. Likewise, other types of Dubins curves, such as S, C, CS, and SC curves, can be determined easily due to the coplanarity of their initial and final configurations and segments. The key points and constants for their determinations are presented in Table 1 and the set of equations for calculation can be obtained as well based on the presented equations above.Table 1
Points for other Dubins curves.
Type
S
C
C
S
S
C
C
S
C
Points
P
1
,
P
2
P
1
,
P
2
P
1
,
P
5
,
P
2
P
1
,
P
6
,
P
2
P
1
,
P
5
,
P
6
,
P
2
Constants
/
O
1
,
P
3
O
1
,
P
3
O
2
,
P
4
O
1
,
P
3
,
P
4
,
O
2
Parameters
/
k
1
,
k
3
k
1
,
k
3
,
k
5
k
2
,
k
4
,
k
6
k
1
,
k
2
,
…
,
k
6If theCSC curve has semicircular C segments, the equations for calculation are built through modifying the corresponding equations above based on the curve’s geometric characters. For example, for a CSC curve with semicircular C1, the modified equations for its calculation are presented as(8)V→3=-V→1,P5O1→=P1O1→.Meanwhile, (2) is substituted by(9)P6=P5+k5V→3,where k5 is constant that indicates the length of the S segment. With these equations there will be sufficient equations to determine the feasible CSC Dubins curve. The other Dubins curves can be determined by a similar technique.


### 2.3. Dubins Path Planning

Considering the efficiency and optimality in path planning, several candidate Dubins curves are adopted in this paper which areS, C, CS, SC, and CSC in the order of priority from high to low. Meanwhile, these curves can be divided into two classes which are the coplanar curves and the noncoplanar curves according to the coplanarity of the initial and final configurations. The coplanar curves contain the S, C, CS, SC, and few CSC curves because the initial and final configurations of these curves lie in the same plane. The noncoplanar curves consist of the CSC curves with noncoplanar segments. Therefore, the Dubins curve determination approach is expressed as follows. Firstly the coplanarity of the initial and final configurations is evaluated. If they are coplanar, the coplanar curves are selected as the candidate curves and checked in the order of priority. Otherwise the noncoplanar curve is selected and checked instead. Once the feasible curve is obtained, it is adopted as the feasible path. The algorithm of this approach is presented as Algorithm 1.Algorithm 1:Dubins curve determination algorithm.
Input:  P1,V→1, P2,V→2, R
Output:  feasible Dubins curve L
(1)if[P1,V→1], [P2,V→2] are coplanar, then
(2)if  V→1=V→2=gP1P2→,then
(3)L←TS,[P1,V→1],[P2,V→2]
(4)break
(5)end  if
(6)if  P1P2→=±Rgn→×V→1+gn→×V→2, then
(7)L←TC,[P1,V→1],[P2,V→2],R
(8)break
(9)end  if
(10)if  ∃P5, TC,[P1,V→1],[P5,V→2],R≠0  and
TS,[P5,V→2],[P2,V→2]≠0, then
(11)L←TCS,[P1,V→1],[P2,V→2],R
(12)break
(13)end  if
(14)if  ∃P6, TS,[P1,V→1],[P6,V→1]≠0  and
TC,[P6,V→1],[P2,V→2],R≠0,  then
(15)L←TSC,[P1,V→1],[P2,V→2],R
(16)break
(17)end  if
(18)L←TCSC,[P1,V→1],[P2,V→2],[R,R]
(19)break
(20)else
(21)L←TCSC,[P1,V→1],[P2,V→2],[R,R]
(22)break
(23)end  ifTo simplify the expression of the Dubins curve determination approach, the calculation process of a certain curve is presented as(10)TPt,Cf1,Cf2,Cr=L,ifthecurveexists0,otherwise,where Pt is the desired type of the Dubins curve for determination while Cf1 and Cf2 are the initial and final configurations of Pt, respectively, and Cr presents the collection of the feasible radii according to Pt. The function T returns the desired Dubins curve if this curve can be determined; otherwise it returns 0. Besides, the vector n→ is defined to help evaluate the colinearity and coplanarity of the initial and final configurations, where n→=gV→1×V→2.To prove the validity and efficiency of Algorithm1, an example is presented and analyzed on the CPU Intel Core I3 6300@3.8 GHz with Matlab. The conditions of this example are set as (11)P1=0,0,0,V→1=1,0,0,P2=6,5,4,V→2=-1,-1,-1,R=2.In this example the Dubins curve determination problem is converted into a least squares problem and solved with the Levenberg-Marquardt (L-M) algorithm where λ=0.005. The time consumption of Algorithm 1 is 0.683 s with computational accuracy 10-5 and the simulation result is illustrated in Figure 2. If the computation resource of the robot is abundant, the evaluation processes of the candidate curves in Algorithm 1 can be implemented at the same time to reduce the time consumption.Figure 2
The result of Algorithm1 in Dubins curve determination.


## 2.1. Problem Statement

The path planning problem for docking is to determine a collision-free path to connect the initial position of the autonomous robot and the docking station with prescribed poses under certain constraints of the robot [28]. In this paper the docking station with unidirectional entrance is considered and it is assumed to be static. The position of the docking station is written as Pd and the direction of its entrance is expressed as V→d. Meanwhile the velocity of the autonomous robot is assumed be constant and its position and velocity are written as Pr and V→r, where V→r=v. The turning ability of the robot is assumed to be limited and the minimal turning radius is written as Rt. Hence to avoid collision with the docking station, the final position and velocity of the robot should be close to Pd and V→d. Additionally the work area for docking is assumed to be unknown in advance, which means the autonomous robot can only acquire the environment information within its sensor range Rs.


## 2.2. 3D Dubins Curves

The Dubins curve only consists of two kinds of segments which are the circle (C) segment and the straight-line (S) segment, where the radius of the C segment is equal to the minimal turning radius of the robot and this curve is smooth at the intersections of the adjacent segments. In 2D environment the optimality of the two Dubins curves has been proved and the CCC curve or the CSC curve is reckoned as the shortest path [24]. However, the determination of the Dubins curve becomes complex in 3D environment due to the increase of the dimensionality, which means not all the segments of the Dubins curves are coplanar [32, 33]. Therefore considering the efficiency of its application in path planning, a 3D Dubins curve determination approach is presented in this paper based on the geometric characters of the typical CSC curve.The typicalCSC curve is shown in Figure 1. The initial configuration of position and pose (green vector) and the final configuration (red vector) are presented as [P1,V→1] and [P2,V→2] and the feasible path that connects them is a 3D Dubins curve which consists of one S segment and two C segments. The C segments C1 (red arc) and C2 (green arc) are two circular arcs with centers O1 and O2 and radii R1 and R2, respectively. They are connected by the S segment (blue straight line) with intersections P5 and P6 separately. To indicate the coplanarity of the Dubins curve, two auxiliary lines L1 and L2 are drawn as Figure 1 shows. L1 has P1 on it and parallels with V→1 and L2 has P2 on it and parallels with V→2. They intersect with the elongation of the S segment at P3 and P4, respectively. Based on the spatial relations of L1, L2, and the S segment, the segments of the CSC curve can be divided into two planes determined by L1 and the S segment and L2 and the S segment separately. The intersection line of these two planes is the S segment.Figure 1
The illustration of the typical 3DCSC curve.Inspired by the geometric relations of these two planes, the determination process of the 3D Dubins curve is implemented as follows. The intersections ofL1, L2, and the elongation of S are presented as(1)P3=P1+k3V→1,P4=P2+k4V→2,where k3 and k4 are nonzero constants. A special case of (1) should be noticed where either k3 or k4 is infinite. It means that the corresponding C segment is semicircular arc and this case will be discussed later. Once k3 and k4 are determined, the intersections of C1 and C2 with S can be expressed as(2)P5=P3+k5V→3,P6=P4+k6V→3,where k5 and k6 are constants and V→3 is the vector that indicates the moving direction of the robot on the S segment. Since the Dubins curve is smooth at the intersections of every two adjacent segments, V→3 can be obtained(3)V→3=signk3gO1P1→×O1P5→×O1P5→=-signk4gO2P6→×O2P2→×O2P6→=gP5P6→,where sign(s) presents the sign function which returns the sign of the variable s and g(S→) is the function which normalizes the vector S→ and it is defined as(4)gS→=S→S→,ifS→≠0,0,0,0,ifS→=0.Due to the fact that P5 and P6 are the tangent points of C1 and C2 with the S segment, there are(5)P5O1→×V→3=0,P1O1→×V→1=0,P6O2→×V→3=0,P2O2→×V→2=0,(6)P5O1¯=P1O1¯=R1,P6O2¯=P2O2¯=R2.Furthermore, it can be concluded from (6) that P3O1¯ and P4O2¯ are the angle bisector of ∠P3P1O1 and ∠P4P2O2; therefore there are(7)O1=P3+k1signk3-V→1+V→3O2=P4+k2signk4-V→2+V→3.The parametersk1–k6 can be calculated based on the specified set of equations to determine the CSC curve by substituting the variables in (5) and (6) with corresponding expression in (1)–(3) and (7) if there is no semicircular C segment in the CSC curve. Likewise, other types of Dubins curves, such as S, C, CS, and SC curves, can be determined easily due to the coplanarity of their initial and final configurations and segments. The key points and constants for their determinations are presented in Table 1 and the set of equations for calculation can be obtained as well based on the presented equations above.Table 1
Points for other Dubins curves.
Type
S
C
C
S
S
C
C
S
C
Points
P
1
,
P
2
P
1
,
P
2
P
1
,
P
5
,
P
2
P
1
,
P
6
,
P
2
P
1
,
P
5
,
P
6
,
P
2
Constants
/
O
1
,
P
3
O
1
,
P
3
O
2
,
P
4
O
1
,
P
3
,
P
4
,
O
2
Parameters
/
k
1
,
k
3
k
1
,
k
3
,
k
5
k
2
,
k
4
,
k
6
k
1
,
k
2
,
…
,
k
6If theCSC curve has semicircular C segments, the equations for calculation are built through modifying the corresponding equations above based on the curve’s geometric characters. For example, for a CSC curve with semicircular C1, the modified equations for its calculation are presented as(8)V→3=-V→1,P5O1→=P1O1→.Meanwhile, (2) is substituted by(9)P6=P5+k5V→3,where k5 is constant that indicates the length of the S segment. With these equations there will be sufficient equations to determine the feasible CSC Dubins curve. The other Dubins curves can be determined by a similar technique.


## 2.3. Dubins Path Planning

Considering the efficiency and optimality in path planning, several candidate Dubins curves are adopted in this paper which areS, C, CS, SC, and CSC in the order of priority from high to low. Meanwhile, these curves can be divided into two classes which are the coplanar curves and the noncoplanar curves according to the coplanarity of the initial and final configurations. The coplanar curves contain the S, C, CS, SC, and few CSC curves because the initial and final configurations of these curves lie in the same plane. The noncoplanar curves consist of the CSC curves with noncoplanar segments. Therefore, the Dubins curve determination approach is expressed as follows. Firstly the coplanarity of the initial and final configurations is evaluated. If they are coplanar, the coplanar curves are selected as the candidate curves and checked in the order of priority. Otherwise the noncoplanar curve is selected and checked instead. Once the feasible curve is obtained, it is adopted as the feasible path. The algorithm of this approach is presented as Algorithm 1.Algorithm 1:Dubins curve determination algorithm.
Input:  P1,V→1, P2,V→2, R
Output:  feasible Dubins curve L
(1)if[P1,V→1], [P2,V→2] are coplanar, then
(2)if  V→1=V→2=gP1P2→,then
(3)L←TS,[P1,V→1],[P2,V→2]
(4)break
(5)end  if
(6)if  P1P2→=±Rgn→×V→1+gn→×V→2, then
(7)L←TC,[P1,V→1],[P2,V→2],R
(8)break
(9)end  if
(10)if  ∃P5, TC,[P1,V→1],[P5,V→2],R≠0  and
TS,[P5,V→2],[P2,V→2]≠0, then
(11)L←TCS,[P1,V→1],[P2,V→2],R
(12)break
(13)end  if
(14)if  ∃P6, TS,[P1,V→1],[P6,V→1]≠0  and
TC,[P6,V→1],[P2,V→2],R≠0,  then
(15)L←TSC,[P1,V→1],[P2,V→2],R
(16)break
(17)end  if
(18)L←TCSC,[P1,V→1],[P2,V→2],[R,R]
(19)break
(20)else
(21)L←TCSC,[P1,V→1],[P2,V→2],[R,R]
(22)break
(23)end  ifTo simplify the expression of the Dubins curve determination approach, the calculation process of a certain curve is presented as(10)TPt,Cf1,Cf2,Cr=L,ifthecurveexists0,otherwise,where Pt is the desired type of the Dubins curve for determination while Cf1 and Cf2 are the initial and final configurations of Pt, respectively, and Cr presents the collection of the feasible radii according to Pt. The function T returns the desired Dubins curve if this curve can be determined; otherwise it returns 0. Besides, the vector n→ is defined to help evaluate the colinearity and coplanarity of the initial and final configurations, where n→=gV→1×V→2.To prove the validity and efficiency of Algorithm1, an example is presented and analyzed on the CPU Intel Core I3 6300@3.8 GHz with Matlab. The conditions of this example are set as (11)P1=0,0,0,V→1=1,0,0,P2=6,5,4,V→2=-1,-1,-1,R=2.In this example the Dubins curve determination problem is converted into a least squares problem and solved with the Levenberg-Marquardt (L-M) algorithm where λ=0.005. The time consumption of Algorithm 1 is 0.683 s with computational accuracy 10-5 and the simulation result is illustrated in Figure 2. If the computation resource of the robot is abundant, the evaluation processes of the candidate curves in Algorithm 1 can be implemented at the same time to reduce the time consumption.Figure 2
The result of Algorithm1 in Dubins curve determination.


## 3. Dubins-APF Path Planning

Although the Dubins curves are feasible to form the path for docking with posture constraints, it is difficult to determine the collision-free path and ensure the safety of the robot only using Algorithm1 when there are unknown obstacles in the workspace. To overcome this shortcoming, a path planning strategy is designed and introduced in this section via combining the Dubins curves with the APF approach to avoid collision with obstacles.In this paper both static obstacles and moving obstacles are considered. The position and velocity of the obstaclei are written as Oi and Vo→i separately. Assuming that the minimal safe distance between the robot and obstacle is δs, the radius of the obstacle i can be written as Ri′=Ri+δs, where Ri is the physical radius of the obstacle.


### 3.1. Collision Prediction

Collision prediction is one of the fundamental issues of the obstacle avoidance researches [34, 35]. If the planned path is written as L, the motion of the robot following L is presented as Pr(t), where t∈[0,tf] and tf is the final time the robot follows L. This issue is easy to address in this paper because the planned path solely consists of the Dubins curves. Therefore once L is obtained, the parameters of these Dubins curves can be utilized to evaluate the feasibility of the planned path in collision prediction. First of all, Θ(V→i,V→j) is defined as the operator which calculates and returns the angle between the initial vector V→i and the final vector V→j.For following theS segment P1P2¯, the movement of the robot on it is simple and can be described as(12)Prt=P1+tV1,where t∈[0,P1P2¯/v].There are three types ofC segments that one Dubins curve might have which are the minor C segment, the semicircular C segment, and the major C segment. These C segments are illustrated in Figure 3, respectively.Figure 3
Different types ofC segments. (a) The minor C segment; (b) the semicircular C segment; (c) the major C segment.
(a)
(b)
(c)The illustration of the minorC segment is presented in Figure 3(a). For following the minor C segment P1P2⌢, Pr(t) is expressed as (13)Prt=O+sinθ-tv/RV→1+sintv/RV→2sinθ,where t∈[0,(R/v)ΘOP1→,OP2→], V→1=gOP1→, V→2=gOP2→, and θ=ΘV→1,V→2.Similarly, the majorC segment is shown in Figure 3(c). Pr(t) can be determined by dividing the major C segment into three minor C segments which are named c1, c2, and c3. If the initial vector and the final vector of the major C segment are expressed as V→1=gOP1→ and V→2=gOP2→, the parameters of c1, c2, and c3 are concluded in Table 2. Meanwhile, the movement on each minor C segment can be determined according to (13) and Pr(t) of the major C segment can be obtained by their combination.Table 2
Parameters ofc1, c2, and c3.
Index
V
→
i and V→j
Central angleθ
Initial and final time
c1
V
→
1, -V→2
Θ
(
V
→
1
,
-
V
→
2
)
0, θ1R/v
c2
-
V
→
2, -V→1
Θ
(
-
V
→
2
,
-
V
→
1
)
t
1, t1+θ2R/v
c3
-
V
→
1, V→2
Θ
(
-
V
→
1
,
V
→
2
)
t
2, t2+θ3R/vThe semicircularC segment is shown in Figure 3(b). Likewise, it can be divided into two minor C segments which are c1 and c2. The initial and final vectors of c1 and c2 are V→1, V→r(0) and V→r(0), V→2, respectively, where V→r(0) is the initial pose of the robot following the semicircular C segment. The movements on c1 and c2 can be determined according to (13) and Pr(t) can be obtained by their combination.Based onPr(t) of each C segment and S segment of the planned path, the movement of the robot following certain Dubins curve can be specified as well. The feasibility of L can be determined by evaluating these path segments in the temporal order. If there are Pr(t)-Oi(t)≥Ri′ for all the obstacles, L is collision-free. If L is not collision-free as Figure 4 shows, the time when Pr(t)-Oi(t) is minimal is defined as the maximal collision time Tci for Oi. At Tci the distance between Oi and Pr is defined as the minimal collision distance Dci for Oi and Dci=Pr(Tci)-Oi(Tci). Therefore if Dci<Ri′, the robot will collide with the obstacle Oi by following L and new L is needed.Figure 4
The planned path is not collision-free.


### 3.2. Path Replanning Strategy

Once the planned path is not collision-free, new path is required immediately to avoid collision for the safety of robot. However, sometimes only local knowledge about the environment can be obtained in practice while the replanning process could not be implemented in advance [36–39]. Moreover, if the area for docking is crowded with obstacles, the efficiency of the path replanning process is critical.To solve this problem, a reactive path replanning strategy is proposed in this paper. The conception of this replanning strategy is to construct the obstacle avoidance potential field to determine the feasible intermediate configuration and the pass-by path is generated to avoid the obstacle based on this intermediate configuration. Then the follow-up path from the intermediate configuration to the docking station is generated to complete the path for docking. The details of this proposed replanning strategy are described as follows.


#### 3.2.1. Obstacle Avoidance Potential Field Design

The obstacle avoidance potential field is proposed in this paper to help determine the intermediate configuration for path replanning inspired by the methods proposed in [30, 31, 40]. The obstacle avoidance potential field is a compound repulsive potential field which consists of two components Up and Uv. These components are caused by the positions and velocities of the obstacles, respectively. For the obstacle Oi, its corresponding repulsive potential field Upi is designed as(14)UpiPrOi→=kse-∥PrOi→∥,PrOi→<PrPd→0,otherwise,where ks is a constant. The corresponding repulsive potential field force Fp→i is designed as(15)Fp→iPrOi→=-∇Upi=-kse-∥PrOi→∥gPrOi→,when PrOi→<PrPd→.If the obstacleOi is a moving obstacle, the velocity difference between it and the robot is expressed as δV→=V→r-Vo→i. If H→i is defined that H→i=gPrOi→×δV→×PrOi→, the corresponding repulsive potential field Uvi is expressed as(16)UviH→i=-kvδV→e-∥PrOi→∥,PrOi→<PrPd→0,otherwise,where θi=ΘV→r,PrOi→ and kv is a constant. The corresponding repulsive force is expressed as(17)Fv→i=-∇Uvi=kvδV→e-∥PrOi→∥H→i,when PrOi→<PrPd→.The total repulsive field force caused byOi can be derived from Fpi and Fvi as(18)F→r=∑i=1N-∇Upi-∇Uvi=∑i=1NFp→i+Fv→i,where N is the number of the detected obstacles that PrOi→<PrPd→.In this paperF→r is used to determine the feasible intermediate configuration for obstacle avoidance while considering both the positions and velocities of the obstacles.


#### 3.2.2. Intermediate Configuration Determination

In this paper the feasible intermediate configuration is utilized to generate collision-free path according toF→r. The intermediate configuration [Pm,Vm→] is determined as follows. Firstly, the obstacle Og is defined as the target obstacle Og for determining the intermediate configuration, where(19)Og=Og∣Tcg=minTc1,Tc2,…,Tci,…,TcN.Then the final pose of the path segment that PrTcg is on is chosen as the intermediate pose. Therefore, the original intermediate point Pm is expressed as(20)Pm=OgTcg+Rg′+αgF→m,where α is the parameter to adjust the position of the intermediate point. The larger α is, the more conservative the path replanning strategy is. F→m is defined as the intermediate direction where(21)F→m=gVm→×F→r×Vm→,Vm→×F→r≠0gVm→×F→rand×Vm→,otherwise,where F→rand is a nonzero random vector and gF→rand≠gF→r. At last the Dubins curve L′ from [Pr,V→r] to [Pm,Vm→] with radii Rt and Rg is generated as the pass-by path L′.IfL′ is collision-free, the intermediate configuration is adopted and L′ is adopted as the pass-by path Lpass-by. If L′ is not collision-free, the new target obstacle can be determined and the new intermediate point Pmg′ can be obtained as well. The pose of the new intermediate vector is chosen as Vm→′=Vm→ and the intermediate configuration determination process is repeated.


#### 3.2.3. Path Generation

The replanned pathL consists of two subpaths which are the pass-by path Lpass-by and the follow-up path Lfollow-up. The initial and final configurations of Lpass-by are [Pr,V→r] and [Pm,Vm→], respectively, while the initial and final configurations of Lfollow-up are [Pm,Vm→] and [Pd,V→d]. Since Lpass-by is obtained in the intermediate configuration determination process, Lfollow-up is generated after Lpass-by is determined. If Lfollow-up is collision-free, the combination of Lpass-by and Lfollow-up is set as the replanned path for docking and followed till this path is evaluated to be infeasible. Otherwise, Lfollow-up is replanned according to the replanning strategy as well.This proposed path replanning strategy is concluded in Algorithm2. In Algorithm 2  rpflag is the flag bit which indicates whether a feasible path has been planned (rpflag==1) or not (rpflag==0). Once Algorithm 2 is called, the minimal maximal collision time Tmin is calculated firstly to evaluate whether a new path should be replanned. If Tmin>0, it means the new path is required and the path replanning strategy is implemented. If Tmin≤0, it means the planned path L is collision-free and the replanned path is accepted as the feasible path for docking.Algorithm 2:Path replanning algorithm.
Input:[Pr,V→r], [Pd,V→d], Rt, [O,Vo→,R′], L, R1, R2, ks,
kv, α
Output:  L
(1)rpflag=0
(2)while true do
(3)Tmin=min{Tci∣Dci<Ri′,i∈[0,tf]}
(4)if  Tmin>0  then
(5)Tg←Tmin, obtain Og,Rg′,Pm,Vm→
(6)if  rpflag==0  then
(7)R1←R2, R2←Rg′
(8)end if
(9)Lpass-by←T[Pr,V→r],[Pm,Vm→],[R1,R2]
(10)if  Lpass-by is not collision-free then
(11)L←Lpass-by, rpflag=1
(12)continue
(13)end if
(14)R1←R2, R2←Rt
(15)Lfollow-up←T[Pm,Vm→],[Pd,V→d],[R1,R2]
(16)if  Lfollow-up is not collision-free then
(17)     ReplanLfollow-up with Algorithm 2
(18)else
(19)L←Lpass-by+Lfollow-up
(20)end if
(21)else
(22)break
(23)end if
(24)end whileThe parameters of Algorithm2 are selected according the following rules. The obstacle avoidance strategy is affected by ks/kv. The larger ks/kv is, the more attentions are paid on avoiding the static obstacles and vice versa. Therefore if the moving obstacles in the environment for docking are large and fast, a small ks/kv is preferred to make the path planner sensitive to the moving obstacles. α provides the margin for robot controller in handling the emergency situations such as the velocity change of the moving obstacle and ocean currents. The larger α is, the more potentials are preserved for robot control but, however, the longer the planned path may be. Therefore α is chosen based on the estimation of the environment for docking.


### 3.3. DAPF Path Planning Approach

The DAPF path planning approach can be concluded as Algorithm3 based on the Dubins curves and the path replanning strategy. In Algorithm 3 the configuration of the known obstacles is written as C. The DAPF path planning approach works in a reactive way to the change of the environment as follows. Firstly, an initial path is generated as the target path L and followed by the robot. Then the feasibility of L is evaluated when C is changed. If L is infeasible, then the new path is generated and L is substituted by the new path. Otherwise C is updated and L is followed.Algorithm 3:DAPF path planning algorithm.
Input:  [Pr,V→r], [Pd,V→d], Rt, ks, kv, α
Output:  L
(1)L←T[Pr,V→r],[Pd,V→d],[Rt,Rt]
(2)R1←Rt, R2←Rt
(3)C′←[O,Vo→,R′]
(4)while  [Pr,V→r]≠[Pd,V→d]  do
(5)if  C′⊄C  then
(6)L← Algorithm 2
(7)end  if
(8)C←C∪C′
(9)    followL
(10)end  whileAn example of the DAPF path planning approach is given in Figure5, where(22)Pr=0,0,0,V→r=1,0,0,Pd=6,5,4,V→d=1,-0,0,Rt=2,Rs=6,α=0.1,O1=9,0,-4,Vo→1=0,0,0.5,R1′=3.The result of this scenario shows that the proposed path planning strategy is feasible in generating collision-free path.Figure 5
Example of the DAPF path planning strategy in environment with one obstacle. (a) The intermediate configuration is determined as well asLpass-by. (b) The planned path is collision-free and feasible for docking.
(a)
Path replanning to avoid collision
(b)
Feasible planned path for docking


## 3.1. Collision Prediction

Collision prediction is one of the fundamental issues of the obstacle avoidance researches [34, 35]. If the planned path is written as L, the motion of the robot following L is presented as Pr(t), where t∈[0,tf] and tf is the final time the robot follows L. This issue is easy to address in this paper because the planned path solely consists of the Dubins curves. Therefore once L is obtained, the parameters of these Dubins curves can be utilized to evaluate the feasibility of the planned path in collision prediction. First of all, Θ(V→i,V→j) is defined as the operator which calculates and returns the angle between the initial vector V→i and the final vector V→j.For following theS segment P1P2¯, the movement of the robot on it is simple and can be described as(12)Prt=P1+tV1,where t∈[0,P1P2¯/v].There are three types ofC segments that one Dubins curve might have which are the minor C segment, the semicircular C segment, and the major C segment. These C segments are illustrated in Figure 3, respectively.Figure 3
Different types ofC segments. (a) The minor C segment; (b) the semicircular C segment; (c) the major C segment.
(a)
(b)
(c)The illustration of the minorC segment is presented in Figure 3(a). For following the minor C segment P1P2⌢, Pr(t) is expressed as (13)Prt=O+sinθ-tv/RV→1+sintv/RV→2sinθ,where t∈[0,(R/v)ΘOP1→,OP2→], V→1=gOP1→, V→2=gOP2→, and θ=ΘV→1,V→2.Similarly, the majorC segment is shown in Figure 3(c). Pr(t) can be determined by dividing the major C segment into three minor C segments which are named c1, c2, and c3. If the initial vector and the final vector of the major C segment are expressed as V→1=gOP1→ and V→2=gOP2→, the parameters of c1, c2, and c3 are concluded in Table 2. Meanwhile, the movement on each minor C segment can be determined according to (13) and Pr(t) of the major C segment can be obtained by their combination.Table 2
Parameters ofc1, c2, and c3.
Index
V
→
i and V→j
Central angleθ
Initial and final time
c1
V
→
1, -V→2
Θ
(
V
→
1
,
-
V
→
2
)
0, θ1R/v
c2
-
V
→
2, -V→1
Θ
(
-
V
→
2
,
-
V
→
1
)
t
1, t1+θ2R/v
c3
-
V
→
1, V→2
Θ
(
-
V
→
1
,
V
→
2
)
t
2, t2+θ3R/vThe semicircularC segment is shown in Figure 3(b). Likewise, it can be divided into two minor C segments which are c1 and c2. The initial and final vectors of c1 and c2 are V→1, V→r(0) and V→r(0), V→2, respectively, where V→r(0) is the initial pose of the robot following the semicircular C segment. The movements on c1 and c2 can be determined according to (13) and Pr(t) can be obtained by their combination.Based onPr(t) of each C segment and S segment of the planned path, the movement of the robot following certain Dubins curve can be specified as well. The feasibility of L can be determined by evaluating these path segments in the temporal order. If there are Pr(t)-Oi(t)≥Ri′ for all the obstacles, L is collision-free. If L is not collision-free as Figure 4 shows, the time when Pr(t)-Oi(t) is minimal is defined as the maximal collision time Tci for Oi. At Tci the distance between Oi and Pr is defined as the minimal collision distance Dci for Oi and Dci=Pr(Tci)-Oi(Tci). Therefore if Dci<Ri′, the robot will collide with the obstacle Oi by following L and new L is needed.Figure 4
The planned path is not collision-free.


## 3.2. Path Replanning Strategy

Once the planned path is not collision-free, new path is required immediately to avoid collision for the safety of robot. However, sometimes only local knowledge about the environment can be obtained in practice while the replanning process could not be implemented in advance [36–39]. Moreover, if the area for docking is crowded with obstacles, the efficiency of the path replanning process is critical.To solve this problem, a reactive path replanning strategy is proposed in this paper. The conception of this replanning strategy is to construct the obstacle avoidance potential field to determine the feasible intermediate configuration and the pass-by path is generated to avoid the obstacle based on this intermediate configuration. Then the follow-up path from the intermediate configuration to the docking station is generated to complete the path for docking. The details of this proposed replanning strategy are described as follows.


### 3.2.1. Obstacle Avoidance Potential Field Design

The obstacle avoidance potential field is proposed in this paper to help determine the intermediate configuration for path replanning inspired by the methods proposed in [30, 31, 40]. The obstacle avoidance potential field is a compound repulsive potential field which consists of two components Up and Uv. These components are caused by the positions and velocities of the obstacles, respectively. For the obstacle Oi, its corresponding repulsive potential field Upi is designed as(14)UpiPrOi→=kse-∥PrOi→∥,PrOi→<PrPd→0,otherwise,where ks is a constant. The corresponding repulsive potential field force Fp→i is designed as(15)Fp→iPrOi→=-∇Upi=-kse-∥PrOi→∥gPrOi→,when PrOi→<PrPd→.If the obstacleOi is a moving obstacle, the velocity difference between it and the robot is expressed as δV→=V→r-Vo→i. If H→i is defined that H→i=gPrOi→×δV→×PrOi→, the corresponding repulsive potential field Uvi is expressed as(16)UviH→i=-kvδV→e-∥PrOi→∥,PrOi→<PrPd→0,otherwise,where θi=ΘV→r,PrOi→ and kv is a constant. The corresponding repulsive force is expressed as(17)Fv→i=-∇Uvi=kvδV→e-∥PrOi→∥H→i,when PrOi→<PrPd→.The total repulsive field force caused byOi can be derived from Fpi and Fvi as(18)F→r=∑i=1N-∇Upi-∇Uvi=∑i=1NFp→i+Fv→i,where N is the number of the detected obstacles that PrOi→<PrPd→.In this paperF→r is used to determine the feasible intermediate configuration for obstacle avoidance while considering both the positions and velocities of the obstacles.


### 3.2.2. Intermediate Configuration Determination

In this paper the feasible intermediate configuration is utilized to generate collision-free path according toF→r. The intermediate configuration [Pm,Vm→] is determined as follows. Firstly, the obstacle Og is defined as the target obstacle Og for determining the intermediate configuration, where(19)Og=Og∣Tcg=minTc1,Tc2,…,Tci,…,TcN.Then the final pose of the path segment that PrTcg is on is chosen as the intermediate pose. Therefore, the original intermediate point Pm is expressed as(20)Pm=OgTcg+Rg′+αgF→m,where α is the parameter to adjust the position of the intermediate point. The larger α is, the more conservative the path replanning strategy is. F→m is defined as the intermediate direction where(21)F→m=gVm→×F→r×Vm→,Vm→×F→r≠0gVm→×F→rand×Vm→,otherwise,where F→rand is a nonzero random vector and gF→rand≠gF→r. At last the Dubins curve L′ from [Pr,V→r] to [Pm,Vm→] with radii Rt and Rg is generated as the pass-by path L′.IfL′ is collision-free, the intermediate configuration is adopted and L′ is adopted as the pass-by path Lpass-by. If L′ is not collision-free, the new target obstacle can be determined and the new intermediate point Pmg′ can be obtained as well. The pose of the new intermediate vector is chosen as Vm→′=Vm→ and the intermediate configuration determination process is repeated.


### 3.2.3. Path Generation

The replanned pathL consists of two subpaths which are the pass-by path Lpass-by and the follow-up path Lfollow-up. The initial and final configurations of Lpass-by are [Pr,V→r] and [Pm,Vm→], respectively, while the initial and final configurations of Lfollow-up are [Pm,Vm→] and [Pd,V→d]. Since Lpass-by is obtained in the intermediate configuration determination process, Lfollow-up is generated after Lpass-by is determined. If Lfollow-up is collision-free, the combination of Lpass-by and Lfollow-up is set as the replanned path for docking and followed till this path is evaluated to be infeasible. Otherwise, Lfollow-up is replanned according to the replanning strategy as well.This proposed path replanning strategy is concluded in Algorithm2. In Algorithm 2  rpflag is the flag bit which indicates whether a feasible path has been planned (rpflag==1) or not (rpflag==0). Once Algorithm 2 is called, the minimal maximal collision time Tmin is calculated firstly to evaluate whether a new path should be replanned. If Tmin>0, it means the new path is required and the path replanning strategy is implemented. If Tmin≤0, it means the planned path L is collision-free and the replanned path is accepted as the feasible path for docking.Algorithm 2:Path replanning algorithm.
Input:[Pr,V→r], [Pd,V→d], Rt, [O,Vo→,R′], L, R1, R2, ks,
kv, α
Output:  L
(1)rpflag=0
(2)while true do
(3)Tmin=min{Tci∣Dci<Ri′,i∈[0,tf]}
(4)if  Tmin>0  then
(5)Tg←Tmin, obtain Og,Rg′,Pm,Vm→
(6)if  rpflag==0  then
(7)R1←R2, R2←Rg′
(8)end if
(9)Lpass-by←T[Pr,V→r],[Pm,Vm→],[R1,R2]
(10)if  Lpass-by is not collision-free then
(11)L←Lpass-by, rpflag=1
(12)continue
(13)end if
(14)R1←R2, R2←Rt
(15)Lfollow-up←T[Pm,Vm→],[Pd,V→d],[R1,R2]
(16)if  Lfollow-up is not collision-free then
(17)     ReplanLfollow-up with Algorithm 2
(18)else
(19)L←Lpass-by+Lfollow-up
(20)end if
(21)else
(22)break
(23)end if
(24)end whileThe parameters of Algorithm2 are selected according the following rules. The obstacle avoidance strategy is affected by ks/kv. The larger ks/kv is, the more attentions are paid on avoiding the static obstacles and vice versa. Therefore if the moving obstacles in the environment for docking are large and fast, a small ks/kv is preferred to make the path planner sensitive to the moving obstacles. α provides the margin for robot controller in handling the emergency situations such as the velocity change of the moving obstacle and ocean currents. The larger α is, the more potentials are preserved for robot control but, however, the longer the planned path may be. Therefore α is chosen based on the estimation of the environment for docking.


## 3.2.1. Obstacle Avoidance Potential Field Design

The obstacle avoidance potential field is proposed in this paper to help determine the intermediate configuration for path replanning inspired by the methods proposed in [30, 31, 40]. The obstacle avoidance potential field is a compound repulsive potential field which consists of two components Up and Uv. These components are caused by the positions and velocities of the obstacles, respectively. For the obstacle Oi, its corresponding repulsive potential field Upi is designed as(14)UpiPrOi→=kse-∥PrOi→∥,PrOi→<PrPd→0,otherwise,where ks is a constant. The corresponding repulsive potential field force Fp→i is designed as(15)Fp→iPrOi→=-∇Upi=-kse-∥PrOi→∥gPrOi→,when PrOi→<PrPd→.If the obstacleOi is a moving obstacle, the velocity difference between it and the robot is expressed as δV→=V→r-Vo→i. If H→i is defined that H→i=gPrOi→×δV→×PrOi→, the corresponding repulsive potential field Uvi is expressed as(16)UviH→i=-kvδV→e-∥PrOi→∥,PrOi→<PrPd→0,otherwise,where θi=ΘV→r,PrOi→ and kv is a constant. The corresponding repulsive force is expressed as(17)Fv→i=-∇Uvi=kvδV→e-∥PrOi→∥H→i,when PrOi→<PrPd→.The total repulsive field force caused byOi can be derived from Fpi and Fvi as(18)F→r=∑i=1N-∇Upi-∇Uvi=∑i=1NFp→i+Fv→i,where N is the number of the detected obstacles that PrOi→<PrPd→.In this paperF→r is used to determine the feasible intermediate configuration for obstacle avoidance while considering both the positions and velocities of the obstacles.


## 3.2.2. Intermediate Configuration Determination

In this paper the feasible intermediate configuration is utilized to generate collision-free path according toF→r. The intermediate configuration [Pm,Vm→] is determined as follows. Firstly, the obstacle Og is defined as the target obstacle Og for determining the intermediate configuration, where(19)Og=Og∣Tcg=minTc1,Tc2,…,Tci,…,TcN.Then the final pose of the path segment that PrTcg is on is chosen as the intermediate pose. Therefore, the original intermediate point Pm is expressed as(20)Pm=OgTcg+Rg′+αgF→m,where α is the parameter to adjust the position of the intermediate point. The larger α is, the more conservative the path replanning strategy is. F→m is defined as the intermediate direction where(21)F→m=gVm→×F→r×Vm→,Vm→×F→r≠0gVm→×F→rand×Vm→,otherwise,where F→rand is a nonzero random vector and gF→rand≠gF→r. At last the Dubins curve L′ from [Pr,V→r] to [Pm,Vm→] with radii Rt and Rg is generated as the pass-by path L′.IfL′ is collision-free, the intermediate configuration is adopted and L′ is adopted as the pass-by path Lpass-by. If L′ is not collision-free, the new target obstacle can be determined and the new intermediate point Pmg′ can be obtained as well. The pose of the new intermediate vector is chosen as Vm→′=Vm→ and the intermediate configuration determination process is repeated.


## 3.2.3. Path Generation

The replanned pathL consists of two subpaths which are the pass-by path Lpass-by and the follow-up path Lfollow-up. The initial and final configurations of Lpass-by are [Pr,V→r] and [Pm,Vm→], respectively, while the initial and final configurations of Lfollow-up are [Pm,Vm→] and [Pd,V→d]. Since Lpass-by is obtained in the intermediate configuration determination process, Lfollow-up is generated after Lpass-by is determined. If Lfollow-up is collision-free, the combination of Lpass-by and Lfollow-up is set as the replanned path for docking and followed till this path is evaluated to be infeasible. Otherwise, Lfollow-up is replanned according to the replanning strategy as well.This proposed path replanning strategy is concluded in Algorithm2. In Algorithm 2  rpflag is the flag bit which indicates whether a feasible path has been planned (rpflag==1) or not (rpflag==0). Once Algorithm 2 is called, the minimal maximal collision time Tmin is calculated firstly to evaluate whether a new path should be replanned. If Tmin>0, it means the new path is required and the path replanning strategy is implemented. If Tmin≤0, it means the planned path L is collision-free and the replanned path is accepted as the feasible path for docking.Algorithm 2:Path replanning algorithm.
Input:[Pr,V→r], [Pd,V→d], Rt, [O,Vo→,R′], L, R1, R2, ks,
kv, α
Output:  L
(1)rpflag=0
(2)while true do
(3)Tmin=min{Tci∣Dci<Ri′,i∈[0,tf]}
(4)if  Tmin>0  then
(5)Tg←Tmin, obtain Og,Rg′,Pm,Vm→
(6)if  rpflag==0  then
(7)R1←R2, R2←Rg′
(8)end if
(9)Lpass-by←T[Pr,V→r],[Pm,Vm→],[R1,R2]
(10)if  Lpass-by is not collision-free then
(11)L←Lpass-by, rpflag=1
(12)continue
(13)end if
(14)R1←R2, R2←Rt
(15)Lfollow-up←T[Pm,Vm→],[Pd,V→d],[R1,R2]
(16)if  Lfollow-up is not collision-free then
(17)     ReplanLfollow-up with Algorithm 2
(18)else
(19)L←Lpass-by+Lfollow-up
(20)end if
(21)else
(22)break
(23)end if
(24)end whileThe parameters of Algorithm2 are selected according the following rules. The obstacle avoidance strategy is affected by ks/kv. The larger ks/kv is, the more attentions are paid on avoiding the static obstacles and vice versa. Therefore if the moving obstacles in the environment for docking are large and fast, a small ks/kv is preferred to make the path planner sensitive to the moving obstacles. α provides the margin for robot controller in handling the emergency situations such as the velocity change of the moving obstacle and ocean currents. The larger α is, the more potentials are preserved for robot control but, however, the longer the planned path may be. Therefore α is chosen based on the estimation of the environment for docking.


## 3.3. DAPF Path Planning Approach

The DAPF path planning approach can be concluded as Algorithm3 based on the Dubins curves and the path replanning strategy. In Algorithm 3 the configuration of the known obstacles is written as C. The DAPF path planning approach works in a reactive way to the change of the environment as follows. Firstly, an initial path is generated as the target path L and followed by the robot. Then the feasibility of L is evaluated when C is changed. If L is infeasible, then the new path is generated and L is substituted by the new path. Otherwise C is updated and L is followed.Algorithm 3:DAPF path planning algorithm.
Input:  [Pr,V→r], [Pd,V→d], Rt, ks, kv, α
Output:  L
(1)L←T[Pr,V→r],[Pd,V→d],[Rt,Rt]
(2)R1←Rt, R2←Rt
(3)C′←[O,Vo→,R′]
(4)while  [Pr,V→r]≠[Pd,V→d]  do
(5)if  C′⊄C  then
(6)L← Algorithm 2
(7)end  if
(8)C←C∪C′
(9)    followL
(10)end  whileAn example of the DAPF path planning approach is given in Figure5, where(22)Pr=0,0,0,V→r=1,0,0,Pd=6,5,4,V→d=1,-0,0,Rt=2,Rs=6,α=0.1,O1=9,0,-4,Vo→1=0,0,0.5,R1′=3.The result of this scenario shows that the proposed path planning strategy is feasible in generating collision-free path.Figure 5
Example of the DAPF path planning strategy in environment with one obstacle. (a) The intermediate configuration is determined as well asLpass-by. (b) The planned path is collision-free and feasible for docking.
(a)
Path replanning to avoid collision
(b)
Feasible planned path for docking


## 4. Simulation and Discussion

In this section simulation experiments are given to prove the validity of the proposed DAPF approach in solving the path planning problem for docking in unknown environment. Meanwhile, the traditional APF approach is adopted in these scenarios to compare the path planning performance with the proposed path planning approach. The common parameters of these scenarios are set as(23)Pr0=0,0,0,V→r0=1,0,0,Pd=15,0,0,V→d=1,0,0,Rt=2,Rs=6,and the step length of these simulations is set as 0.5. The traditional APF approach is designed as (24)UattrPrOi→=ka21PrOi→-Ri-1PrPd→2,UrepuPrOi→=kr1PrOi→-Ri,where ka=1 and kr=5. The parameters of the DAPF approach are ks=1, kv=1, and α=0.1.


### 4.1. Scenario 1

Both static and moving obstacles are considered in scenario 1. In this scenario, one of the static obstacles is set close to the docking station which simulates a goal nonreachable problem identified in [41, 42]. The configurations of the obstacles are set as (25)O1=4,2,1,V→1=0,0,0,R1′=1,O2=13,-2,0,V→2=0,0,0,R2′=2,O3=7,0,-4,V→3=0,0,0.5,R3′=3.The path planning results of the traditional APF and DAPF approaches are illustrated in Figure 6 and Table 3. From Figure 6 it can be concluded that the path planned by the APF approach (cyan cross line) is affected by the obstacle near the docking station and the final configuration of its planned path is infeasible for docking. The path planned by the DAPF (black solid line) is collision-free and its final configuration is Vr=[0.9934,0.1097,0.0331]. It is also proved in this scenario that the DAPF approach is capable of planning the feasible path even if the environment near the docking station is complex.Table 3
Results of scenario 1.
Approach
Final pose
Reach time
|
|
Δ
V
→
|
|
APF
[
0.0051,0.0564
,
-
0.9984
]
21.0000
1.4106
DAPF
[
0.9934,0.1097,0.0331
]
24.5102
0.1148Figure 6
Scenario 1: path planning for docking in the environment with static and moving obstacles.


### 4.2. Scenario 2

In scenario 2, the local minima problem is considered and modeled as Figure7 shows, where the configurations of the obstacles are set as(26)O1=7,-2,-2,O2=7,-2,2,O3=7,2,2,O4=7,2,-2,O5=7,-22,0,O6=7,0,22,O7=7,0,-22,O8=7,22,0,O9=9,0,0,R1=2,R2=2,R3=2,R4=2,R5=3,and the velocities of these obstacles are all set as [0,0,0]. Therefore, these obstacles form a compound concave obstacle together which will cause the local minima problem in the traditional APF approach.Figure 7
Scenario 2: path planning for docking in environment with concave obstacle.As Figure7 and Table 4 show, the path planned by the APF approach (cyan cross line) encounters the local minima problem and gets stuck at the local minima area while the path planned by the DAPF approach (black solid line) is free from the affection of the local minima and reaches the docking station with feasible pose in spite of the fact that a modified APF is utilized in path replanning.Table 4
Results of scenario 2.
Approach
Final pose
Reach time
|
|
Δ
V
→
|
|
APF
/
∞
/
DAPF
[
0.9999,0.0000,0.0001
]
17.9533
0.0001


## 4.1. Scenario 1

Both static and moving obstacles are considered in scenario 1. In this scenario, one of the static obstacles is set close to the docking station which simulates a goal nonreachable problem identified in [41, 42]. The configurations of the obstacles are set as (25)O1=4,2,1,V→1=0,0,0,R1′=1,O2=13,-2,0,V→2=0,0,0,R2′=2,O3=7,0,-4,V→3=0,0,0.5,R3′=3.The path planning results of the traditional APF and DAPF approaches are illustrated in Figure 6 and Table 3. From Figure 6 it can be concluded that the path planned by the APF approach (cyan cross line) is affected by the obstacle near the docking station and the final configuration of its planned path is infeasible for docking. The path planned by the DAPF (black solid line) is collision-free and its final configuration is Vr=[0.9934,0.1097,0.0331]. It is also proved in this scenario that the DAPF approach is capable of planning the feasible path even if the environment near the docking station is complex.Table 3
Results of scenario 1.
Approach
Final pose
Reach time
|
|
Δ
V
→
|
|
APF
[
0.0051,0.0564
,
-
0.9984
]
21.0000
1.4106
DAPF
[
0.9934,0.1097,0.0331
]
24.5102
0.1148Figure 6
Scenario 1: path planning for docking in the environment with static and moving obstacles.


## 4.2. Scenario 2

In scenario 2, the local minima problem is considered and modeled as Figure7 shows, where the configurations of the obstacles are set as(26)O1=7,-2,-2,O2=7,-2,2,O3=7,2,2,O4=7,2,-2,O5=7,-22,0,O6=7,0,22,O7=7,0,-22,O8=7,22,0,O9=9,0,0,R1=2,R2=2,R3=2,R4=2,R5=3,and the velocities of these obstacles are all set as [0,0,0]. Therefore, these obstacles form a compound concave obstacle together which will cause the local minima problem in the traditional APF approach.Figure 7
Scenario 2: path planning for docking in environment with concave obstacle.As Figure7 and Table 4 show, the path planned by the APF approach (cyan cross line) encounters the local minima problem and gets stuck at the local minima area while the path planned by the DAPF approach (black solid line) is free from the affection of the local minima and reaches the docking station with feasible pose in spite of the fact that a modified APF is utilized in path replanning.Table 4
Results of scenario 2.
Approach
Final pose
Reach time
|
|
Δ
V
→
|
|
APF
/
∞
/
DAPF
[
0.9999,0.0000,0.0001
]
17.9533
0.0001


## 5. Conclusion and Future Work

In this paper a path planning approach named DAPF is presented for autonomous robot docking based on the combination of the Dubins curves and the artificial potential field approach. Firstly, the determination approach of the Dubins curves is proposed and the collision prediction approach of the planned path is realized based on this approach. Then the path replanning strategy is proposed with the help of the obstacle avoidance potential field. In the replanning strategy the intermediate configuration is calculated based on the obstacle avoidance potential field and adopted to determine the new path. The path planning task is completed through implementing the planning and replanning process in a reactive mode to the changes of environment. Simulation results are also presented to prove the feasibility of the DAPF approach through comparison with the traditional APF approach.In this proposed approach, little prior knowledge about the environment is required and the planned path is feasible for autonomous robot to follow. As a kind of reactive path planning approaches, this approach is easy to execute by the CPU of the robot in practice. Better performance can be achieved in the future work via adapting parallel computation techniques to enhance the real-time capability of this approach. Specific models of the autonomous robots will be considered to improve the feasibility of this approach in practical applications such as underwater docking and parking.



---

*Source: 6716820-2017-09-12.xml*