Complexity
Volume 2018 (2018), Article ID 4359036, 17 pages
https://doi.org/10.1155/2018/4359036

Research Article

Smooth Path Planning for Robot Docking in Unknown Environment with Obstacles

Peng Cui,1,2 Weisheng Yan,1,2 Rongxin Cui,1,2 and Jiahui Yu³

¹Research & Development Institute of Northwestern Polytechnical University in Shenzhen, Shenzhen, Guangdong 518057, China
²School of Marine Science and Technology, Northwestern Polytechnical University, Xi’an, Shaanxi 710072, China
³School of Automation and Electrical Engineering, Shenyang Ligong University, Shenyang, Liaoning 110159, China

Correspondence should be addressed to Weisheng Yan; wsyan@nwpu.edu.cn

Received 8 July 2018; Accepted 26 September 2018; Published 8 November 2018

Guest Editor: Zhaojie Ju

Copyright © 2018 Peng Cui et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

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.

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 Section 2. 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 by . The configuration of the robot is defined as

where is the position of the robot and is the velocity of the robot. Since the cruising speed of the robot is constant, we have where 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 .

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 as and

where is its position and 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 , its center is written as , its radius is written as , and its velocity is written as . The obstacle area of obstacle can be described as

For a feasible path for robot docking, for any obstacle , there are

and

where is the elapsed time during which the robot reaches the docking station.

To simplify the expression, several functions are defined as Table 1 presents.

Table 1: Mathematical function definition.

Function
Expression

calculate the sign of the variable

calculate the angle between and

normalizes

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 Figure 1. The CSC curve consists of two circular segments, (green circular arc) and (red circular arc), and one straight line segment (black straight line). Its initial configuration is written as (green arrow) and its final configuration is written as (red arrow). The terminal configurations and are on two different planes which are named plane 1 and plane 2 as Figure 1 shows.

Figure 1: A typical CSC curve starts from with and ends at with .

To outline the geometric characters of the CSC curve, three auxiliary straight lines are adopted as shown in Figure 1, which are , , and . and are the colinear lines with and , respectively. 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 and . Likewise, plane 2 can be determined by and . Furthermore, since the circular segments and are on plane 1 and plane 2 correspondingly and intersect with at and separately, once , , and 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 of with and are written as and , we have

where and are nonzero constants. Because and are on , they can be obtained as

where and are constants. is the auxiliary vector along with the curves propagation direction.

According to the spatial-temporal relations of the CSC curve, we have

Meanwhile, and are also the tangent points of and with the S segment; then

and

It is derived from (9) and (10) that and are on the angle bisectors of and , which are described as

Nonlinear equations to solve to 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 . 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 the Dubins 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

branches
the Dubins curve that connects and

roots
the initial and terminal nodes
,

Figure 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 from to which are collision-free and shortest. The collision-free set of branches that connects and is defined as the candidate path , which can be written as

where all the branches are collision-free. In this case, there are

and

for , where is the number of obstacles. For instance, in Figure 2, the candidate path (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 tree consists of several candidate paths, which can be described as

where 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 path is infeasible, a Dubins tree 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 is selected as the resultant path .

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

where represents the start point of segment and represents the planned velocity of the robot at .

There are three types of the C segments with prescribed radii 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 , can be expressed as

where , , and .

Figure 3: Divisions of the C segments. The minor C segment case is , the semicircular C segment is , and the major C segment is .

For the semicircular C segment case , it is divided into two minor C subsegments which are and . The division point can be chosen as

Thus, the trajectories of these minor C subsegments and can be expressed according to (17) correspondingly.

For the major C segment case , it is divided into three minor subsegments which are , , and . In this case, the division points and are chosen as

, , and 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 Figure 1 shows, is written as

where , , and represent the trajectories of , , and , respectively. Meanwhile, , , and .

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 path is collision-free if (13) and (14) hold for all its branches.

If is not collision-free, we define the obstacle that the robot would collide firstly as the maximal collision obstacle, which is written as . The position of the robot when the distance between and will be minimal is defined as the maximal collision point . The moment maximal collision would take place is defined as the maximal collision time . In this case, if there are multiple obstacles in the area, and can be obtained as

where . Additionally, the maximal collision pose is defined as

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 path , its maximal collision obstacle , point , time , and pose can be acquired as Section 4.1 presents. According to the state of , two situations are considered in the determination of nodes for Dubins tree expansion. If is a static obstacle, this situation is called the static collision situation. If 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 obstacle , two nodes are determined which are and , where

and is a nonzero random 3D vector.

Furthermore, three angles , , and , , are measured to evaluate the feasibilities of these nodes. They are utilized to simulate the potential spring forces of and then the feasibility of this node is determined based on the following inequalities.

and

is infeasible and excluded from the Dubins tree if one or more of these inequalities hold. It is because, for , its total force cannot be unbalanced in this situation if the branches are generated to connect it and tightened up. Otherwise, is accepted and added to the Dubins tree.

An example of the node selection in static collision situation is illustrated in Figure 4. In this example, the initial path (black solid line) is infeasible and four nodes, , and , , are determined according to the HTE strategy. The potential spring forces of the nodes are along the dash lines towards and . Besides, the total potential spring forces of these nodes are indicated by the red arrows. In this case, 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 (black solid curve) is accepted as one of the generated branches rather than .

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 obstacle , the node is determined as , where is a certain radial direction of the obstacle . The velocity of is also considered in the determination of and for the efficiency of the obstacle-avoiding maneuver, which is inspired by the implement of the velocity obstacle method in [44]. is obtained as

where is the velocity of .

An implementation example of the HTE strategy in moving obstacle situation is presented in Figure 5, where the original path (black solid line) is infeasible and 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

where

Besides, represents the new candidate path and 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, Proposition 1 is derived for the comparison between two candidate paths and with a common node .

Proposition 1. If and , then .

Proposition 1 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.

The spreading degree of a node is defined as

where is the position of the node and is the index of this node. indicates the compactness of the generated nodes and the larger is, the more compact the node is.

To reduce the computation consumption in tree expansion, the notion maximal spreading ratio 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 in one tree expanding process, where

and 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 as and the candidate path set is defined as . 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

where represents the present configuration and 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 is added into for further evaluation.

4.3.2. Path Evaluation

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

4.3.3. Path Generation

For the infeasible , the HTE strategy is adopted to generate the nodes and branches for the expansion of . 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 . The path evaluation and path generation procedures will be repeated iteratively till the termination condition is satisfied.

4.3.4. Termination

If , 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 . In this situation, the Dubins tree has been sufficiently expanded and its shortest candidate path in is chosen as the resultant path. In the immature situation, plenty of candidate paths have been determined; however, . 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

where is the number of whole generated branches; is the number of the candidate paths in . Meanwhile, is the number of the new generated branches at the th path generation execution, is the number of detected obstacles, and is the depth of .

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 Algorithm 1, where the definitions of the functions are given in Table 3.

Table 3: Function definition in Algorithm 1.

Function
Expression

determine with and

true if is infeasible and vise versa

collision prediction procedure of

tree expanding procedure

returns generated branches’ number

Input:, , , ,

Output:

(1)

(2)

(3) whiledo

(4)ifthen

(5)

(6) Push into , set ,

(7) whiledo

(8) Pop from

(9) ifthen

(10)

(11)

(12)

(13) Push into

(14) else

(15) Push into

(16) if then

(17)

(18) end if

(19) end if

(20) if then

(21) break

(22) end if

(23) end while

(24) else

(25) follow

(26) end if

(27) end while

Algorithm 1: The smooth path planning approach.

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

The configurations of the obstacles in this case are set as

The implementation of Algorithm 1 in an unknown environment with both static and moving obstacles is illustrated in Figure 6, where the simulation step length is set as and the parameter of Algorithm 1 is set as . 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.

(a) Initial environment ()

(b) Path replanning to avoid ()

(c) Path replanning for docking ()

(d) Path replanning for docking ()

(e) Path replanning for docking ()

(f) Docking successfully ()

Figure 6: Simulation results of the smooth path planning approach in unknown environment with both static and moving obstacles.

The initial configuration of the workspace is illustrated in Figure 6(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 , which is presented in Figure 6(b). As Figure 6(c) shows, new static obstacle () is detected in the collision avoidance maneuvers and the planned path is blocked by . In this case, new path for docking is determined by the proposed path planning algorithm and followed by the robot. The static obstacle and moving obstacle 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 .

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

and the step length in the simulations is set as .

(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

where and are the repulsive and attractive potential functions and . Besides, is the parameter to adjust the strength of the attractive potential field and 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 .

(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.

Input: Root , Goal , , Iterations ,

output:

(1) for to do

(2) Generate random sample nodes within to

(3) Determine the nearest existent to within

(4) if the branch is collision-free then

(5) Determine with lowest cost from to

(6) Set as the parent node of

(7) end if

(8) end for

(9) the shortest path

Algorithm 2: The RRT path planning approach.

The parameters of this algorithm in the following simulation scenarios are set as .

(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.

Input: , , , , ,

Output:

(1)

(2)

(3) whiledo

(4) ifthen

(5) ,

(6) whiledo

(7)

(8)

(9) ifthen

(10) , ,

(11) continue

(12) end if

(13)

(14) ifthen

(15) Replan

(16) else

(17)

(18) end if

(19) end while

(20) end if

(21)

(22) follow

(23) end while

Algorithm 3: The DAPF path planning approach.

The parameters of the DAPF are selected as .

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

Resultant paths of the prescribed path planning approaches in scenario 1 are presented in Figure 7. 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 ; 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 . 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 . 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.

(a) Path planned by APF

(b) Path planned by DAPF

(c) Path planned by RRT

(d) Path planned by the smooth path planning approach

Figure 7: Simulation results of the path planning approaches in scenario 1.

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

The path planning results of the approaches prescribed above in scenario 2 are presented correspondingly in Figure 8. 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 ; 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 . 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 . 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 .

(a) Path planned by APF

(b) Path planned by DAPF

(c) Path planned by RRT

(d) Path planned by the proposed approach

Figure 8: Simulation results of the path planning approaches in case 1, scenario 2.

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, where is set as 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 , 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.

(a) Path planned by DAPF

(b) Path planned by DAPF

(c) Path planned by DAPF

(d) Path planned by the proposed approach

Figure 9: Simulation results of the DAPF and the smooth path planning approach in case 2, scenario 2.

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

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 to remain 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.

(a) Simulation result in Matlab

(b) Initial path in V-REP

(c) Feasible path in V-REP

Figure 10: The path planning result of the proposed approach in scenario 3.

Moreover, 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 Figures 10(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.

Data Availability

The data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest

The authors declare that they have no conflicts of interest.

Acknowledgments

This work was supported by the National Natural Science Foundation of China, under grant nos. 61733014, 51579210, 61472325, and 61633002, and the Science, Technology and Innovation Commission of Shenzhen Municipality under grant no. JCYJ20170817145216803.

Supplementary Materials

5 figures (.png) are provided as the supplementary materials for our manuscript. These figures are the screenshots captured while running the simulation of scenario 3 in the V-REP software. igure 1 to igure 4 are captured when the Line Tracer robot is following the planned path towards the docking station, which shows that the planned path is collision-free and smooth to follow. igure 5 is the enlarged screenshot when the Line Tracer robot docks in the station, which reveals that the pose of the robot for docking is feasible. (Supplementary Materials)

References

Y. Shi, C. Shen, H. Fang, and H. Li, “Advanced control in marine mechatronic systems: A survey,” IEEE/ASME Transactions on Mechatronics, vol. 22, no. 3, pp. 1121–1131, 2017.

C. Yoo, R. Fitch, and S. Sukkarieh, “Online task planning and control for aerial robots with fuel constraints in winds,” in Springer Tracts in Advanced Robotics, vol. 107, pp. 711–727, Springer, Cham, 2015.

J. S. Jaffe, P. J. S. Franks, P. L. D. Roberts et al., “A swarm of autonomous miniature underwater robot drifters for exploring submesoscale ocean dynamics,” Nature Communications, vol. 8, 2017.

J. Villagra, V. Milanés, J. Pérez, and J. Godoy, “Smooth path and speed planning for an automated public transport vehicle,” Robotics and Autonomous Systems, vol. 60, no. 2, pp. 252–265, 2012.

C. Yang, Y. Jiang, Z. Li, W. He, and C.-Y. Su, “Neural control of bimanual robots with guaranteed global stability and motion precision,” IEEE Transactions on Industrial Informatics, vol. 13, no. 3, pp. 1162–1171, 2017.

M. Kovac, “Learning from nature how to land aerial robots,” Science, vol. 352, no. 6288, pp. 895-896, 2016.

T. Setter and M. Egerstedt, “Energy-Constrained Coordination of Multi-Robot Teams,” IEEE Transactions on Control Systems Technology, vol. 25, no. 4, pp. 1257–1263, 2017.

H. Xiao, R. Cui, and D. Xu, “A Sampling-Based Bayesian Approach for Cooperative Multiagent Online Search With Resource Constraints,” IEEE Transactions on Cybernetics, vol. 48, no. 6, pp. 1773–1785, 2018.

R. Cui, Y. Li, and W. Yan, “Mutual information-based multi-AUV path planning for scalar field sampling using multidimensional RRT*,” IEEE Transactions on Systems, Man, and Cybernetics: Systems, vol. 46, no. 7, pp. 993–1004, 2016.

G. Song, H. Wang, J. Zhang, and T. Meng, “Automatic docking system for recharging home surveillance robots,” IEEE Transactions on Consumer Electronics, vol. 57, no. 2, pp. 428–435, 2011.

N. Mathew, S. L. Smith, and S. L. Waslander, “Multirobot rendezvous planning for recharging in persistent tasks,” IEEE Transactions on Robotics, vol. 31, no. 1, pp. 128–142, 2015.

T. Oral and F. Polat, “Mod* lite: An incremental path planning algorithm taking care of multiple objectives,” IEEE Transactions on Cybernetics, vol. 46, no. 1, pp. 245–257, 2016.

H. Dong, Q. Hu, and M. R. Akella, “Safety control for spacecraft autonomous rendezvous and docking under motion constraints,” Journal of Guidance, Control, and Dynamics, vol. 40, no. 7, pp. 1680–1692, 2017.

C. Luo, S. X. Yang, X. Li, and M. Q.-H. Meng, “Neural-Dynamics-Driven Complete Area Coverage Navigation Through Cooperation of Multiple Mobile Robots,” IEEE Transactions on Industrial Electronics, vol. 64, no. 1, pp. 750–760, 2017.

Y. Chen and G. W. Dorn II, “PINK1-phosphorylated mitofusin 2 is a parkin receptor for culling damaged mitochondria,” Science, vol. 340, no. 6131, pp. 471–475, 2013.

A. Macwan, J. Vilela, G. Nejat, and B. Benhabib, “A Multirobot Path-Planning Strategy for Autonomous Wilderness Search and Rescue,” IEEE Transactions on Cybernetics, vol. 45, no. 9, pp. 1784–1797, 2015.

H.-X. Wei, H.-Y. Li, Y. Guan, and Y.-D. Li, “A dynamics based two-stage path model for the docking navigation of a self-Assembly modular robot (Sambot),” Robotica, vol. 34, no. 7, pp. 1517–1528, 2016.

C. Yang, X. Wang, L. Cheng, and H. Ma, “Neural-learning-based telerobot control with guaranteed performance,” IEEE Transactions on Cybernetics, vol. 47, no. 10, pp. 3148–3159, 2017.

J. Park, B. Jun, P. Lee, Y. Lim, and J. Oh, “Docking problem and guidance laws considering drift for an underactuated AUV,” in Proceedings of the OCEANS 2011 - SPAIN, pp. 1–7, Santander, Spain, June 2011.

S. Jain and B. Argall, “Automated perception of safe docking locations with alignment information for assistive wheelchairs,” in Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2014, pp. 4997–5002, USA, September 2014.

A. Yazdani, K. Sammut, A. Lammas, and Y. Tang, “Real-time quasi-optimal trajectory planning for autonomous underwater docking,” in Proceedings of the 2015 IEEE International Symposium on Robotics and Intelligent Sensors (IRIS), pp. 15–20, Langkawi, Malaysia, October 2015.

P. A. Wilson, “Autonomous homing and docking tasks for an underwater vehicle,” in Proceedings of the 8th International IFAC Conference on Manoeuvring and Control of Marine Craft, MCMC 2009, pp. 304–309, Brazil, September 2009.

P. B. Sujit, A. J. Healey, and J. B. Sousa, “AUV docking on a moving submarine using a K-R navigation function,” in Proceedings of the 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems: Celebrating 50 Years of Robotics, IROS'11, pp. 3154–3159, USA, September 2011.

M. Guerra, D. Efimov, G. Zheng, and W. Perruquetti, “Avoiding local minima in the potential field method using input-to-state stability,” Control Engineering Practice, vol. 55, pp. 174–184, 2016.

C. Liu, S. Fan, B. Li, S. Chen, Y. Xu, and W. Xu, “Path planning for autonomous underwater vehicle docking in stationary obstacle environment,” in Proceedings of the OCEANS 2016 - Shanghai, China, April 2016.

Y. Li, Y. Jiang, G. Zhang, Y. Li, and P. Chen, “AUV recovery path planning method considering geometrical constraints,” Jiqiren/Robot, vol. 37, no. 4, pp. 478–485, 2015.

J. Cao, J. Cao, Z. Zeng, B. Yao, and L. Lian, “Toward Optimal Rendezvous of Multiple Underwater Gliders: 3D Path Planning with Combined Sawtooth and Spiral Motion,” Journal of Intelligent & Robotic Systems, vol. 85, no. 1, pp. 189–206, 2017.

Y. Li, R. Cui, Z. Li, and D. Xu, “Neural Network Approximation-based Near-optimal Motion Planning with Kinodynamic Constraints Using RRT,” IEEE Transactions on Industrial Electronics, vol. 65, no. 11, pp. 8718–8729, 2018.

M. Behandish and H. T. Ilies, “Analytic methods for geometric modeling via spherical decomposition,” Computer-Aided Design, vol. 70, pp. 100–115, 2016.

A. M. Shkel and V. Lumelsky, “Classification of the Dubins set,” Robotics and Autonomous Systems, vol. 34, no. 4, pp. 179–202, 2001.

Y. Meyer, P. Isaiah, and T. Shima, “On Dubins paths to intercept a moving target,” Automatica, vol. 53, pp. 256–263, 2015.

G. Ambrosino, M. Ariola, U. Ciniglio, F. Corraro, E. De Lellis, and A. Pironti, “Path generation and tracking in 3-D for UAVs,” IEEE Transactions on Control Systems Technology, vol. 17, no. 4, pp. 980–988, 2009.

V. Duindam, J. Xu, R. Alterovitz, S. Sastry, and K. Goldberg, “Three-dimensional motion planning algorithms for steerable needles using inverse kinematics,” International Journal of Robotics Research, vol. 29, no. 7, pp. 789–800, 2010.

Y. Lin and S. Saripalli, “Path planning using 3D dubins curve for unmanned aerial vehicles,” in Proceedings of the 2014 International Conference on Unmanned Aircraft Systems, ICUAS 2014, pp. 296–304, Orlando, FL, USA, May 2014.

H. J. Sussmann, “Shortest 3-dimensional paths with a prescribed curvature bound,” in Proceedings of the 1995 34th IEEE Conference on Decision and Control. Part 1 (of 4), pp. 3306–3312, December 1995.

P. Cui, W. Yan, and Y. Wang, “Reactive path planning approach for docking robots in unknown environment,” Journal of Advanced Transportation, vol. 2017, 2017.

R. Vatcha and J. Xiao, “Detection of robustly collision-free trajectories in unpredictable environments in real-time,” Autonomous Robots, vol. 37, no. 1, pp. 81–96, 2014.

Y. Lu, Z. Xi, and J.-M. Lien, “Online collision prediction among 2D polygonal and articulated obstacles,” International Journal of Robotics Research, vol. 35, no. 5, pp. 476–500, 2015.

T. Sawabe, M. Kanbara, N. Ukita et al., “Comfortable autonomous navigation based on collision prediction in blind occluded regions,” in Proceedings of the IEEE International Conference on Vehicular Electronics and Safety, ICVES 2015, pp. 75–80, Japan, November 2015.

R. Cui, B. Gao, and J. Guo, “Pareto-optimal coordination of multiple robots with safety guarantees,” Autonomous Robots, vol. 32, no. 3, pp. 189–205, 2012.

M. Keller, F. Hoffmann, T. Bertram, C. Hass, and A. Seewald, “Planning of optimal collision avoidance trajectories with timed elastic bands,” in Proceedings of the 19th IFAC World Congress on International Federation of Automatic Control, IFAC 2014, pp. 9822–9827, South Africa, August 2014.

T. Gu, J. Atwood, C. Dong, J. M. Dolan, and J.-W. Lee, “Tunable and stable real-time trajectory planning for urban autonomous driving,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2015, pp. 250–256, Germany, October 2015.

J. Hilgert, K. Hirsch, T. Bertram, and M. Hiller, “Emergency path planning for autonomous vehicles using elastic band theory,” in Proceedings of the 2003 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, AIM 2003, pp. 1390–1395, Japan, July 2003.

Y. I. Jenie, E.-J. van Kampen, C. C. de Visser, J. Ellerbroek, and J. M. Hoekstra, “Three-dimensional velocity obstacle method for UAV deconflicting maneuvers,” in Proceedings of the AIAA Guidance, Navigation, and Control Conference 2015, MGNC 2015 - Held at the AIAA SciTech Forum 2015, USA, January 2015.