Abstract
In this paper, fast path planning of on-water automatic rescue intelligent robot is studied based on the constant thrust artificial fluid method. First, a three-dimensional environment model is established, and then the kinematics equation of the robot is given. The constant thrust artificial fluid method based on the isochronous interpolation method is proposed, and a novel algorithm of constant thrust fitting is researched through the impulse compensation. The effect of obstacles on original fluid field is quantified by the perturbation matrix, and the streamlines can be regarded as the planned path. Simulation results demonstrate the effectiveness of this method by comparing with A-star algorithm and ant colony algorithm. It is proved that the planned path can avoid all obstacles smoothly and swiftly and reach the destination eventually.
1. Introduction
Robot path planning is based on a certain performance index (the shortest path, the least cost, the least time, etc.) to find the best path from the starting point to the target point that can avoid obstacles. At present, robot path planning mainly includes artificial intelligence path planning technology, ant colony algorithm, A-star algorithm, artificial potential field path planning technology and map construction path planning technology, etc.
The on-water automatic rescue intelligent robot is composed of four parts: monitoring device, unmanned automatic search and rescue boat, lifebuoy, and information receiving device as shown in Figure 1. It integrates the functions of falling water detection, falling water warning, rescue, GPS positioning, image transmission, information collection, and so on. When someone enters the dangerous area, the on-water intelligent search and rescue robot will sound an alarm to prevent accidents. If it detects that someone has fallen into the water, it will calculate the position and distance through infrared and sound sensing technology, ultrasonic distance sensors, etc., and then quickly send the lifebuoy to the drowning person so that he can save himself. Therefore, the on-water automatic rescue intelligent robot can carry out rescue more efficiently and it is of great significance to study the fast path planning of on-water automatic rescue intelligent robots.
Fast path planning is very important to realize autonomous search and rescue of water robots. Fast path planning generally includes indirect path planning and direct path planning [1]. Indirect path planning is divided into outer loop guidance and inner loop control. Indirect path planning has a simple structure and is widely used in engineering [2]. It mainly includes backstepping, internal model control, fuzzy control, neural network, and so on. Among them, the backstepping method and internal model control can get a better planning path, but an accurate mathematical model of the controlled object is required [3]. Fuzzy control and neural networks do not require precise controlled object models but require designers to have rich knowledge and practical experience, so the error fluctuates greatly. The direct path planning is to integrate the two links, usually based on neural networks and other methods to achieve fast path planning [4].
The S-plane control method is a combination of fuzzy control and PID control. It has the advantages of not relying on precise mathematical models, suitable for nonlinear control, and easy to adjust parameters. The S‐plane control method is an effective and simple method for the motion control of underwater robots, and the experiment verifies that the S-plane controller has both fast and stable control effects [5]. For the research of outer loop guidance, LOS (line-of-sight) tracking has been widely used in wheeled robots, aircraft, and surface ships. Fossen et al. added an integral link to the traditional LOS guidance algorithm to eliminate the lateral deviation of the ship’s position caused by the slowly changing environment and achieved good control effects [6].
In addition, the problem of robot path planning in an unknown environment has attracted more and more researchers’ attention [7]. The main research methods include the genetic algorithm, the gradient algorithm, the nonlinear mapping algorithm, the linear mapping algorithm, the reinforcement learning algorithm, and so on [8]. The artificial potential field (APF) method has the advantages of simple principle and real-time computation [9], but there exists local minimum when the robot enters into a concave area. Besides, it is hard to obtain a feasible path sometimes even if the magnitude of attractive or repulsive force is regulated. The intelligent algorithms, such as particle swarm optimization (PSO) [10], evolutionary algorithm (EA) [11], and ant colony algorithm (ACO) [12], are also widely used to path planning. These methods can be easily employed in different environments, but it is possible to trap in a local optimum. However, the aforementioned drawback can be relieved when the intelligent methods are improved or combined with other methods [13].
These traditional approaches are improved to solve the path planning problem. However, the calculation of these algorithms tends to increase exponentially if the planning space enlarges [14]. Besides, the planned path may be not smooth enough for robot to track. As a result, extra strategy of smoothing path is usually needed [15]. A novel algorithm based on disturbed fluid and trajectory propagation is developed to solve the path planning problem of unmanned aerial vehicle in static environment [16]. The core path graph (CPG) algorithm [17] calculates the CPG where arcs are minimum-length trajectories satisfying geometrical constraints and searches the optimal trajectory between two arbitrary nodes of the graph. In addition, the conclusions about robot maneuvering such as solutions to the characteristic equation for industrial robot’s elliptic trajectories [18], experimental investigations of a highly maneuverable mobile omniwheel robot [19], navigation control and stability investigation of a mobile robot based on a hexacopter equipped with an integrated manipulator [20], and integration of inertial sensor data into control of the mobile platform [21] have also been studied in recent years, providing important theoretical and experimental reference values for this paper.
This study aims at constant thrust maneuver of on-water automatic rescue intelligent robot. The main contributions of this study are as follows: (1) the 3-D environment model is established by using the Taylor formula of multivariate functions; (2) a novel algorithm of constant thrust fitting is proposed through the impulse compensation and the isochronous interpolation method; and (3) the tangential vector and disturbance matrix of the artificial fluid method are improved by combing the interfered fluid dynamical system.
The rest of the paper is organized as follows. The 3-D environmental model is established in Section 2. Section 3 focuses on the kinematics model of on-water automatic rescue intelligent robot. The constant thrust artificial fluid method is given in Section 4. The simulation studies and results are presented in Section 5, followed by the conclusions and future work in Section 6.
2. Establishing 3-D Environmental Model
The purpose of on-water automatic rescue intelligent robot fast path planning is to avoid obstacles and reach destination. In this paper, obstacles are described approximately as 3-D space surfaces when building the 3-D environment model. The relative motion coordinate system is defined as the path planning space, and is defined as the position of the robot relative to the obstacle. Suppose that the parametric equations of the obstacle’s space curved surface are as follows:where and . Assuming that is any point on the parametric surface (1), is transformed as , and then the normal vector at point iswhere and are the partial derivative of on and . Obviously, in order to get the minimum distance between and , should be parallel to the normal vector :
Equation (3) can be written as follows:where are nonlinear equations. Then, equation (4) can be written as follows:
The derivative matrix of equation (5) is as follows:
Assume that is the optimal solution of equation (5); then, the following results can be obtained by using the Taylor formula of multivariate functions.where , , and . is the solution of equation (7) and can be seen as an approximate solution of . Equation (7) can be transformed into a matrix equation:
Then, the least squares solution of can be obtained:
Therefore, the shortest distance between the robot and the obstacle is , where represents the modular of vector from to . Then, the 3-D environment model is established using the shortest distance theory, and the specific model will be given in the simulation example.
3. Kinematics Model of On-Water Automatic Rescue Intelligent Robot
In the process of fast path planning of the on-water automatic rescue intelligent robot, it is necessary to consider the three directions of motion laws of longitudinal, lateral, and yaw. The motion state of the on-water automatic rescue intelligent robot is represented by in the earth coordinate system, the state of three degrees of freedom is represented by in the body coordinate system, and the control input is represented by . Then, the kinematics model is expressed as follows:
In order to simplify the calculation, when the dynamic modeling of the on-water automatic rescue intelligent robot is established, the resistance of the robot is considered to be linear with its speed, and the dynamic equation of the robot is as follows:where is the inertial mass matrix of the robot, is the coefficient matrix of centripetal force and Coriolis force, is the resistance coefficient matrix, and is the thrust and moment matrix of the robot. The coefficient in the formula is obtained through actual test. Therefore, the dynamic model of the robot is as follows:where and are the main diagonal elements of the and . The resultant force vectors in the three directions generated by the robot’s thrusters are as follows:where is the distance from the thruster to the centroid of the robot, is the expected output force, is thrust output of the thruster, and is the placement matrix of the thruster.
It is worth noting that because the placement matrix of the thruster is a non-full rank matrix, it cannot be calculated by the inverse of the matrix. The traditional method is to use the pseudoinverse matrix to distribute the thrust, but this method does not consider the constraint factors of the thrust amplitude, which is easy to cause the oversaturation of the thrust distribution. Therefore, in this section, the quadratic programming method is used to introduce performance indicators to establish three thrust distribution functions as follows:
4. Fast Path Planning Based on Constant Thrust Artificial Fluid Method
4.1. Constant Thrust Collision Avoidance Maneuver
In the process of path planning, collision avoidance maneuvers should be considered. Suppose that the time of on-water automatic rescue intelligent robot’s collision avoidance maneuver is and the shortest switching time interval is . There are shortest switching time intervals and target maneuver positions. represents the time of the thrust arc, and denotes the number of shortest switching time intervals in the thrust arc.
Suppose that the actual constant thrusts of the robot is , the maximum thrusts is , and the theoretical continuous thrusts is . The thrusters can provide different sizes of constant thrust to meet different thrust requirements. There are different sizes of constant thrust which can be denoted as follows:
The size of the constant thrust is calculated as follows. There are thrust levels that can be selected: , and the level of the constant thrust can be calculated as follows, taking the thrust arc as example:where [] represents the bracket function and represents the absolute value of .
The constant thrust fitting should be discussed in several categories, for convenience, taking the thrust arc as example. Suppose that the theoretical working time in the thrust arc is . The simplest case is the theoretical working time in the thrust arc ; then, the actual constant thrust is . If the theoretical working time in the thrust arc is , then the constant thrust fitting should be discussed in the following situations.
Case 1. .
Without loss of generality, assume that is the impulse error in the thrust arc and can be calculated as follows:Suppose that the value of the impulse compensation threshold is a positive constant . When satisfies the following condition:the actual constant thrust of the robot can be calculated as follows:Then, the robot does not need to carry out impulse compensation. When the impulse error satisfies the following condition:then the robot should carry out impulse compensation and the size of the constant thrust impulse compensation can be calculated as follows:
Case 2. .
For this situation, the impulse error in the thrust arc can be calculated as follows:If there exist shortest switching time intervals satisfying the following conditions, without loss of generality, suppose that these time intervals are the first shortest switching time interval, taking the shortest switching time interval as an example:Then, the size of the impulse compensation can be calculated as follows. When the impulse error satisfies the following condition:the actual constant thrust of the robot can be calculated as follows, taking the shortest switching time interval as an example:Then, the robot will not carry out impulse compensation.
When the impulse error satisfies the following condition:and there is a constant that satisfies the following equation:the actual constant thrust of the robot can be calculated as follows:Then, the robot does not need to carry out impulse compensation. When the impulse error satisfies the following condition:then the robot should carry out impulse compensation and the size of the constant thrust impulse compensation can be calculated as follows:
Case 3. .
Without loss of generality, suppose that is the first shortest switching time interval and the impulse error in the thrust arc can be calculated as follows:Furthermore, if there are shortest switching time intervals satisfying the following conditions, without loss of generality, suppose that these time intervals are the top shortest switching time interval, taking the shortest switching time interval as an example:Then, the size of the impulse compensation can be calculated as follows. When satisfies the following condition:the actual constant thrust of the robot can be calculated as follows, taking the shortest switching time interval as an example:Then, the robot will not carry out impulse compensation. When the impulse error satisfies the following condition:and there is a constant that satisfies the following equation:then the actual constant thrust of the robot can be calculated as follows:The robot will not carry out impulse compensation. When the impulse error satisfies the following condition:then the robot should carry out impulse compensation and the constant thrust impulse compensation can be calculated as follows:
4.2. Path Planning Based on the Artificial Fluid Method
Based on the description in Section 3, the procedure for path planning is as follows. First, the improved perturbation matrix is calculated. Next, the disturbed fluid velocity is calculated by modifying the target velocity . Then, the planned path is obtained by the recursive integration of . Finally, the switching control laws based on the isochronous interpolation method are given. To describe the influence of obstacle on the original flow, the improved perturbation matrix is defined as follows [16]:where is a identity matrix, is a column vector, is a tangential vector (the partial derivative of on ) at the point , is a saturation function defining the orientation of tangential velocity, and is 2-norm of a vector or a matrix. Besides, and are defined as the weight of and , respectively.where is the repulsive parameter, is the tangential parameter, is the minimum permitted distance between the boundary of on-water automatic rescue intelligent robot, and is a small positive threshold of the saturation function . Then, the disturbed fluid velocity can be calculated by
4.3. The Planned Path Can Avoid Obstacles Safely
Proof. Vectors and are perpendicular exactly: . Suppose that the distance between the boundary of on-water automatic rescue intelligent robot is close to the minimum permitted distance . Suppose that is a monotonically decreasing function.It can be inferred that ; then, , , and ; therefore, .Therefore,Because means that , the path is outside of the minimum permitted distance and there is no collision.
4.4. The Planned Path Can Reach the Destination Eventually
Proof. Because the goal of the path planning is to make the on-water automatic rescue intelligent robot reach the destination safely, the velocity of the on-water automatic rescue intelligent robot should have a component in the direction of the target velocity, i.e., velocities and should satisfy , and the planned path will converge to the target point. Besides, the condition should be satisfied when the on‐water automatic rescue intelligent robot is near to the destination .where is inner product of vectors and denotes the angle between and . It is obvious that when the on-water automatic rescue intelligent robot approaches the destination, then and . Furthermore, and hold.
When the robot approaches the destination, and . It can be inferred that and hold.
4.5. Analysis of the Disturbed Fluid Velocity
The modified velocity defined by equation (43) can be expressed as
It can be seen that consists of three parts: can be called the target velocity; is taken as the repulsive velocity; and can be called the tangential velocity. Similarly, the perturbation matrix can be divided into three parts: attractive matrix , repulsive matrix , and tangential matrix . It can be analyzed that the magnitudes of repulsive and tangential velocities increase with and , respectively. Therefore, the shape of the path can be readjusted by changing parameters and . This method is similar to the virtual force method or the artificial potential field method to some degree. However, the perturbation matrix by this method can describe the effect of obstacles on path more objectively, considering the shape of obstacles and the position of on-water automatic rescue intelligent robot.
4.6. Improved Tangential Vector
The normal vector and the tangential plane which is perpendicular to the normal vector can be accurately obtained. The smaller the angle between the tangential vector and the original speed , the greater the component of the tangential vector in the direction of the original velocity . Then, the robot can reach the destination faster and make the path shorter. Thus, the tangential vector is redefined as follows:where refers to the angle between original velocity and vector and is a constant.(1)When , the original speed is parallel to normal vector . In this case, any direction on the tangential plane is the shortest direction, and the tangential vector is selected in the horizontal direction in this paper.(2)When , the direction of new tangential vector is the vertical projection of the original velocity on the tangential plane.
Suppose that original speed and normal vector are defined as follows:
Then, the normal vector of tangential plane is . Original speed and normal vector are in the cut plane ; thus, the normal vector of cut plane can be obtained:
Then, can be calculated:
Furthermore, the improved tangential vector is defined as follows:where , are inspiration factors, which are determined by the robot properties, and is the proportion factor.
4.7. Designing Constant Thrust Switching Control Laws
The constant thrust switching control laws are obtained through the isochronous interpolation method. Without loss of generality, taking case 3 as an example, if the impulse error satisfies the condition of equation (34), the total number of the accelerating time intervals and the decelerating time intervals is and the number of zero-thrust time intervals is . The position of the three types of time intervals is decided by the curve of the theoretical continuous thrust .
If the impulse error satisfies the condition of equation (36), the total number of the accelerating time intervals and the decelerating time intervals is and the number of zero-thrust time intervals is . The position of the three types of time intervals is decided by the curve of the theoretical continuous thrust .
If the impulse error satisfies the condition of equation (39), the total number of the accelerating time intervals and the decelerating time intervals is and the number of zero-thrust time intervals is . The position of the three types of time intervals is decided by the curve of the theoretical continuous thrust .
At last, the switching control laws for the collision avoidance maneuver can be given. For convenience, let us take the time intervals in the thrust arc for example:
5. Simulation Examples
5.1. Comparison with A-Star Algorithm and Ant Colony Algorithm
In order to present some of the results of the algorithms presented, simulation examples are evaluated in some nontrivial simulation scenarios. To verify the effectiveness of the algorithm, the ant colony algorithm and A-star algorithm are compared with the algorithm proposed in this paper.
First, statistical tests are used to analyze the effectiveness of A-star, that is, to test the variance of the number of obstacles. Suppose that the path planning time of A-star algorithm and the algorithms proposed in this paper is and , and the evaluation values of the three algorithms are and . The comprehensive evaluation function can be obtained as follows:
According to the comprehensive evaluation function, the following statistical table can be obtained as shown in Table 1.
The variance test shows that the number of obstacles has a significant impact on path planning. It can be seen from the experimental data that with the increase of obstacles, the comprehensive evaluation function tends to increase. This is because the algorithms proposed in this paper do not search all nodes, so the more the obstacles are, the more it shows its superiority.
It can be seen from Figure 2 that there are 200 obstacle nodes and black points are obstacle points and boundary points, red points are close nodes, green points are open nodes, and green line is the planned path.
Then the ant colony algorithm is compared to the algorithms proposed in this paper. Suppose that the starting position of the on-water automatic rescue intelligent robot is [0, 20, 0] and the target position is [19, 2, 0] as shown in Figure 3.
It can be seen from Figure 3 that when the 3-D ant colony algorithm is used for fast path planning, with different iteration times, the number of ants, etc., the planned path is also different. Moreover, 3-D ant colony algorithm generally only seeks the shortest path, so the planned path is not very smooth.
It can be seen from Figures 4 and 5 that when the number of iterations is the same, the best individual fitness curve varies with the number of ants. In other words, ant colony algorithm relies on prior knowledge to a large extent, so it has its shortcomings in fast path planning. To solve this problem, the mathematical model of obstacles is defined using the method proposed in this paper. Suppose that the starting position of the on-water automatic rescue intelligent robot is [0, 0, 0] and the target position is [40, 40, 0]. Assume that is the robot position and obstacle center is , with axis lengths and index parameters . Then, construct the function as follows [16]:where parameters determine the shape and size of the obstacle: if and , the obstacle is a sphere; if and , the obstacle is regarded as a cone; if and are variables meeting the condition and holds, the obstacle is a circular truncated cone approximately, where are radii of two bases. The obstacles are shown in Table 2, and and .
It can be seen from Figure 6 that the planned path by the improved artificial fluid method proposed in this paper can bypass the obstacle, escape from the trap area, and reach the destination rescue point smoothly.
5.2. On-Water Automatic Rescue Intelligent Robot Can Escape the Trap Area
During the rescue process, the robot will encounter obstacles on water. It is necessary to consider the distribution of obstacles in path planning; otherwise, the on-water automatic rescue intelligent robot will fall into the dead zone of obstacle traps, making rescue not timely and effective.
It can be seen from Figure 7 that there are two planned paths for the robot. The blue curve is the shortest planned path, and the planned path does not consider how to bypass the trap area, but chose to climb directly over the obstacle; in a realistic environment, however, that would not work. The red curve is the planned path by the improved artificial fluid method proposed in this paper; it can be clearly seen that the robot can bypass the obstacle, escape from the trap area, and reach the destination rescue point smoothly.
5.3. Comparison of Different Saturation and Different Starting Points
In the process of path planning, different paths will be planned with different saturation . Two different planned paths are calculated based on two different , as shown in Figure 8.
It can be seen from Figure 8 that there are two planned paths according to different saturation : the blue curve is planned path 1 and the red curve is planned path 2. The saturation in path 1 is 1/3 and the saturation in path 2 is 1/9, and the two paths can bypass obstacles to reach the destination. It can be found that each path facing obstacles is different in the climbing height and deflection angle. Next, consider planning paths for the robot from different starting points as shown in Figures 9–11, and the destination of these planned paths is (40, 40, 0).
As can be seen from Figures 9–11, according to the planning algorithm proposed in this paper, no matter where the robot starts from, it can reach its destination smoothly. It shows the effectiveness and feasibility of the algorithm proposed in this paper.
The results in Figure 12 show the constant thrust fitting, where the red curve is the change of the ideal thrust, and the blue bars represent the size and length of the acceleration time intervals, deceleration time intervals, and zero-thrust time intervals. According to the proposed criterion in this paper, the robot should carry out impulse compensation and the size of the constant thrust impulse compensation is the same, but the time of the constant thrust impulse compensation is different. The switching control laws can be given according to the sizes and the directions of the thrust accelerations of the robot.
6. Conclusions
This paper studies the fast path planning of on-water automatic rescue intelligent robot based on the constant thrust artificial fluid method. The tangential vector and disturbance matrix of the artificial fluid method are improved. A new constant thrust fitting algorithm is proposed through pulse compensation, and the switching control law is given. Furthermore, different parameters and different starting points are discussed separately and the simulation proves that the planned path can smoothly and quickly avoid all obstacles and finally reach the destination. In the future research work, the authors will continue to study the problem of fast path planning when there are dynamic obstacles and multiple robots working at the same time.
Data Availability
The data used to support the findings of this study are included within the article.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This study was supported by the National Natural Science Foundation of China (no. 61304088), the Fundamental Research Funds for the Central Universities (no. 2013QNA37), the China Postdoctoral Science Foundation (no. 2015M581886), and the Jiangsu Province Postdoctoral Research Foundation (no. 1501080B).