Reactive Path Planning Approach for Docking Robots in Unknown Environment

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.


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][2][3][4][5][6][7][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][15][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][29][30][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 planningreplanning 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 Section 1.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.

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   and the direction of its entrance is expressed as  →   .Meanwhile the velocity of the autonomous robot is assumed be constant and its position and velocity are written as   and  →   , where ‖  →   ‖ = V.The turning ability of the robot is assumed to be limited and the minimal turning radius is written as   .Hence to avoid collision with the docking station, the final position and velocity of the robot should be close to   and  →   .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   .

3D Dubins Curves.
The Dubins curve only consists of two kinds of segments which are the circle () segment and the straight-line () segment, where the radius of the  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  curve or the  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  curve.
The typical  curve is shown in Figure 1.The initial configuration of position and pose (green vector) and the final configuration (red vector) are presented as and the feasible path that connects them is a 3D Dubins curve which consists of one  segment and two  segments.The  segments  1 (red arc) and  2 (green arc) are two circular arcs with centers  1 and  2 and radii  1 and  2 , respectively.They are connected by the  segment (blue straight line) with intersections  5 and  6 separately.To indicate the coplanarity of the Dubins curve, two auxiliary lines  1 and  2 are drawn as Figure 1 shows. 1 has  1 on it and parallels with  →  1 and  2 has  2 on it and parallels with  →  2 .They intersect with the elongation of the  segment at  3 and  4 , respectively.Based on the spatial relations of  1 ,  2 , and the  segment, the segments of the  curve can be divided into two planes determined by  1 and the  segment and  2 and the  segment separately.The intersection line of these two planes is the  segment.
Inspired by the geometric relations of these two planes, the determination process of the 3D Dubins curve is implemented as follows.The intersections of  1 ,  2 , and the elongation of  are presented as where  3 and  4 are nonzero constants.A special case of (1) should be noticed where either  3 or  4 is infinite.It means that the corresponding  segment is semicircular arc and this case will be discussed later.Once  3 and  4 are determined, the intersections of  1 and  2 with  can be expressed as where  5 and  6 are constants and  →  3 is the vector that indicates the moving direction of the robot on the  segment.Since the Dubins curve is smooth at the intersections of every two adjacent segments,  →  3 can be obtained where sign() presents the sign function which returns the sign of the variable  and g(  →  ) is the function which normalizes the vector  →  and it is defined as Due to the fact that  5 and  6 are the tangent points of  1 and  2 with the  segment, there are Furthermore, it can be concluded from ( 6) that  3  1 and  4  2 are the angle bisector of ∠ 3  1  1 and ∠ 4  2  2 ; therefore there are The parameters  1 - 6 can be calculated based on the specified set of equations to determine the  curve by substituting the variables in ( 5) and ( 6) with corresponding expression in (1)-( 3) and ( 7) if there is no semicircular  segment in the  curve.Likewise, other types of Dubins curves, such as , , , and  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.
If the  curve has semicircular  segments, the equations for calculation are built through modifying the corresponding equations above based on the curve's geometric characters.For example, for a  curve with semicircular  1 , the modified equations for its calculation are presented as Meanwhile, (2) is substituted by where  5 is constant that indicates the length of the  segment.With these equations there will be sufficient equations to determine the feasible  Dubins curve.The other Dubins curves can be determined by a similar technique.

Dubins Path Planning.
Considering the efficiency and optimality in path planning, several candidate Dubins curves are adopted in this paper which are , , , , and  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 , , , , and few  curves because the initial and final configurations of these curves lie in the same plane.The noncoplanar curves consist of the  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.
To simplify the expression of the Dubins curve determination approach, the calculation process of a certain curve is presented as where  is the desired type of the Dubins curve for determination while  1 and  2 are the initial and final configurations of , respectively, and   presents the collection of the feasible radii according to .The function  returns the desired Dubins curve if this curve can be determined; otherwise it returns 0. Besides, the vector  →  is defined to help evaluate the colinearity and coplanarity of the initial and final configurations, where  →  = g(  →  1 ×  →  2 ).To prove the validity and efficiency of Algorithm 1, an example is presented and analyzed on the CPU Intel Core I3 6300@3.8GHz with Matlab.The conditions of this example are set as 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.

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 Algorithm 1 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 obstacle  are written as   and  →   separately.Assuming that the minimal safe distance between the robot and obstacle is   , the radius of the obstacle  can be written as    =   +   , where   is the physical radius of the obstacle.

Collision Prediction.
Collision prediction is one of the fundamental issues of the obstacle avoidance researches [34,35].If the planned path is written as , the motion of the robot following  is presented as   (), where  ∈ [0,   ] and   is the final time the robot follows .This issue is easy to address in this paper because the planned path solely consists of the Dubins curves.Therefore once  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, Θ(  →   ,  →   ) is defined as the operator which calculates and returns the angle between the initial vector  →   and the final vector  →   .For following the  segment  1  2 , the movement of the robot on it is simple and can be described as where There are three types of  segments that one Dubins curve might have which are the minor  segment, the semicircular  segment, and the major  segment.These  segments are illustrated in Figure 3, respectively.
The illustration of the minor  segment is presented in Figure 3(a).For following the minor  segment   1  2 ,   () is expressed as and  = Θ(  →  1 ,  →  2 ).Similarly, the major  segment is shown in Figure 3(c).() can be determined by dividing the major  segment into three minor  segments which are named  1 ,  2 , and  3 .If the initial vector and the final vector of the major  segment are expressed as  →  1 = g(   →  1 ) and  →  2 = g(   →  2 ), the parameters of  1 ,  2 , and  3 are concluded in Table 2.Meanwhile, the movement on each minor  segment can be determined according to (13) and () of the major  segment can be obtained by their combination.
The semicircular  segment is shown in Figure 3 is the initial pose of the robot following the semicircular  segment.The movements on  1 and  2 can be determined according to (13) and () can be obtained by their combination.
Based on   () of each  segment and  segment of the planned path, the movement of the robot following certain Dubins curve can be specified as well.The feasibility of  can be determined by evaluating these path segments in the temporal order.If there are ‖  () −   ()‖ ≥    for all the obstacles,  is collision-free.If  is not collision-free as Figure 4 shows, the time when ‖  () −   ()‖ is minimal is defined as the maximal collision time   for   .At   the distance between   and   is defined as the minimal collision distance   for   and   = ‖  (  ) −   (  )‖.Therefore if   <    , the robot will collide with the obstacle   by following  and new  is needed.
Figure 4: The planned path is not collision-free.

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][37][38][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.

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  and V.These components are caused by the positions and velocities of the obstacles, respectively.For the obstacle   , its corresponding repulsive potential field   is designed as where   is a constant.The corresponding repulsive potential field force  →   is designed as where  is the number of the detected obstacles that In this paper  →   is used to determine the feasible intermediate configuration for obstacle avoidance while considering both the positions and velocities of the obstacles.Then the final pose of the path segment that   (  ) is on is chosen as the intermediate pose.Therefore, the original intermediate point  is expressed as

Intermediate Configuration
where  is the parameter to adjust the position of the intermediate point.The larger  is, the more conservative the path replanning strategy is. →   is defined as the intermediate direction where If   is collision-free, the intermediate configuration is adopted and   is adopted as the pass-by path  pass-by .If   is not collision-free, the new target obstacle can be determined and the new intermediate point     can be obtained as well.The pose of the new intermediate vector is chosen as   →   =   →  and the intermediate configuration determination process is repeated.
if  pass-by is not collision-free then (11)  ←  pass-by , rpflag = 1 (12) continue (13) end if (14) if  follow-up is not collision-free then (17) Replan  follow-up with Algorithm 2 ( 18) else (19)  ←  pass-by +  follow-up (20) end if (21)  is collision-free, the combination of  pass-by and  follow-up is set as the replanned path for docking and followed till this path is evaluated to be infeasible.Otherwise,  follow-up is replanned according to the replanning strategy as well.
This proposed path replanning strategy is concluded in Algorithm 2. 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  min is calculated firstly to evaluate whether a new path should be replanned.If  min > 0, it means the new path is required and the path replanning strategy is implemented.If  min ≤ 0, it means the planned path  is collision-free and the replanned path is accepted as the feasible path for docking.
The parameters of Algorithm 2 are selected according the following rules.The obstacle avoidance strategy is affected by   / V .The larger   / V 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   / V 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.

DAPF Path Planning Approach.
The DAPF path planning approach can be concluded as Algorithm 3 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  and followed by the robot.Then the feasibility of  is evaluated when C is changed.If  is infeasible, then the new path is generated and  is substituted by the new path.Otherwise C is updated and  is followed.
An example of the DAPF path planning approach is given in Figure 5, where The result of this scenario shows that the proposed path planning strategy is feasible in generating collision-free path.

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 Input: proposed path planning approach.The common parameters of these scenarios are set as and the step length of these simulations is set as 0.

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 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   = [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.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.
As Figure 7 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.

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

Figure 1 :
Figure 1: The illustration of the typical 3D  curve.

Figure 2 :
Figure 2: The result of Algorithm 1 in Dubins curve determination.

Figure 3 :
Figure 3: Different types of  segments.(a) The minor  segment; (b) the semicircular  segment; (c) the major  segment.

Figure 5 :
Figure 5: Example of the DAPF path planning strategy in environment with one obstacle.(a) The intermediate configuration is determined as well as  pass-by .(b) The planned path is collision-free and feasible for docking.

Figure 6 :
Figure 6: Scenario 1: path planning for docking in the environment with static and moving obstacles.

Figure 7 :
Figure 7: Scenario 2: path planning for docking in environment with concave obstacle.
(b).Likewise, it can be divided into two minor  segments which are  1 and  2 .The initial and final vectors of  1 and  2 are If the obstacle   is a moving obstacle, the velocity difference between it and the robot is expressed as      ), the corresponding repulsive potential field V  is expressed as The total repulsive field force caused by   can be derived from   and V  as ‖.
and   is generated as the pass-by path   .
The replanned path  consists of two subpaths which are the pass-by path  pass-by and the followup path  follow-up .The initial and final configurations of Since  pass-by is obtained in the intermediate configuration determination process,  follow-up is generated after  pass-by is determined.If  follow-up