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.
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 () 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 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.
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 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 . 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 .
2.2. 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 and the feasible path that connects them is a 3D Dubins curve which consists of one segment and two segments. The segments (red arc) and (green arc) are two circular arcs with centers and and radii and , respectively. They are connected by the segment (blue straight line) with intersections and separately. To indicate the coplanarity of the Dubins curve, two auxiliary lines and are drawn as Figure 1 shows. has on it and parallels with and has on it and parallels with . They intersect with the elongation of the segment at and , respectively. Based on the spatial relations of , , and the segment, the segments of the curve can be divided into two planes determined by and the segment and 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 , , and the elongation of are presented aswhere and are nonzero constants. A special case of (1) should be noticed where either or is infinite. It means that the corresponding segment is semicircular arc and this case will be discussed later. Once and are determined, the intersections of and with can be expressed aswhere and are constants and 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, can be obtainedwhere presents the sign function which returns the sign of the variable and is the function which normalizes the vector and it is defined asDue to the fact that and are the tangent points of and with the segment, there areFurthermore, it can be concluded from (6) that and are the angle bisector of and ; therefore there are
The parameters – 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 , the modified equations for its calculation are presented asMeanwhile, (2) is substituted bywhere 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.
2.3. 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 aswhere is the desired type of the Dubins curve for determination while and 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 . Besides, the vector is defined to help evaluate the colinearity and coplanarity of the initial and final configurations, where .
To prove the validity and efficiency of Algorithm 1, an example is presented and analyzed on the CPU Intel Core I3 [email protected] GHz 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 (-) algorithm where . The time consumption of Algorithm 1 is 0.683 s with computational accuracy 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.
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 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.
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 , the motion of the robot following is presented as , where 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 , the movement of the robot on it is simple and can be described aswhere .
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.
(a)
(b)
(c)
The illustration of the minor segment is presented in Figure 3(a). For following the minor segment , is expressed as where , , , and .
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 , , and . If the initial vector and the final vector of the major segment are expressed as and , the parameters of , , and 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(b). Likewise, it can be divided into two minor segments which are and . The initial and final vectors of and are , and , , respectively, where is the initial pose of the robot following the semicircular segment. The movements on and 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.
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 and . These components are caused by the positions and velocities of the obstacles, respectively. For the obstacle , its corresponding repulsive potential field is designed aswhere is a constant. The corresponding repulsive potential field force is designed aswhen .
If the obstacle is a moving obstacle, the velocity difference between it and the robot is expressed as . If is defined that , the corresponding repulsive potential field is expressed aswhere and is a constant. The corresponding repulsive force is expressed aswhen .
The total repulsive field force caused by can be derived from and aswhere 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.
3.2.2. Intermediate Configuration Determination
In this paper the feasible intermediate configuration is utilized to generate collision-free path according to . The intermediate configuration is determined as follows. Firstly, the obstacle is defined as the target obstacle for determining the intermediate configuration, whereThen the final pose of the path segment that is on is chosen as the intermediate pose. Therefore, the original intermediate point is expressed aswhere 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 wherewhere is a nonzero random vector and . At last the Dubins curve from to with radii and is generated as the pass-by path .
If is collision-free, the intermediate configuration is adopted and is adopted as the pass-by path . 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.
3.2.3. Path Generation
The replanned path consists of two subpaths which are the pass-by path and the follow-up path . The initial and final configurations of are and , respectively, while the initial and final configurations of are and . Since is obtained in the intermediate configuration determination process, is generated after is determined. If is collision-free, the combination of and is set as the replanned path for docking and followed till this path is evaluated to be infeasible. Otherwise, is replanned according to the replanning strategy as well.
This proposed path replanning strategy is concluded in Algorithm 2. In Algorithm 2 is the flag bit which indicates whether a feasible path has been planned () or not (). Once Algorithm 2 is called, the minimal maximal collision time is calculated firstly to evaluate whether a new path should be replanned. If , it means the new path is required and the path replanning strategy is implemented. If , 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 . The larger 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 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 Algorithm 3 based on the Dubins curves and the path replanning strategy. In Algorithm 3 the configuration of the known obstacles is written as . 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 is changed. If is infeasible, then the new path is generated and is substituted by the new path. Otherwise is updated and is followed.
| ||||||||||||||||||||||||
An example of the DAPF path planning approach is given in Figure 5, whereThe result of this scenario shows that the proposed path planning strategy is feasible in generating collision-free path.
(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 and the step length of these simulations is set as . The traditional APF approach is designed as where and . The parameters of the DAPF approach are , , and .
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 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 . 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.
4.2. Scenario 2
In scenario 2, the local minima problem is considered and modeled as Figure 7 shows, where the configurations of the obstacles are set asand the velocities of these obstacles are all set as . 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.
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.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.
Acknowledgments
This work was supported by the National Natural Science Foundation of China under Grant nos. 51579210, 61472325, 61472326, 61733014, and 61633002.