# Fast Path Planning for On-Water Automatic Rescue Intelligent Robot Based on Constant Thrust Artificial Fluid Method

Authors: Yongqiang Qi; Yi Ke
Journal: Scientific Programming
 (2020)

---

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


---

## Body

## 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 Figure1. 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.Figure 1
On-water automatic rescue intelligent robot.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 inSection 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 systemo−xyz is defined as the path planning space, and R=x,y,z 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:(1)fu,v=x=xu,v,y=yu,v,z=zu,v,where u∈a,b and v∈c,d. Assuming that Qu,v is any point on the parametric surface (1), R=x,y,z is transformed as Ru,v, and then the normal vector at point Qu,v is(2)nq=fu×fv,where fu and fv are the partial derivative of fu,v on u and v. Obviously, in order to get the minimum distance between Ru,v and Qu,v, RQshould be parallel to the normal vector nq:(3)RQ×nq=0.Equation (3) can be written as follows:(4)f1u,v=0,f2u,v=0,f3u,v=0,where f1,f2,f3 are nonlinear equations. Then, equation (4) can be written as follows:(5)Fu,v=f1u,vf2u,vf3u,v=0.The derivative matrix of equation (5) is as follows:(6)F′u,v=∂f1u,v∂u∂f1u,v∂v∂f2u,v∂u∂f2u,v∂v∂f3u,v∂u∂f3u,v∂v.Assume thatX∗u∗,v∗ is the optimal solution of equation (5); then, the following results can be obtained by using the Taylor formula of multivariate functions.(7)fiX∗+Δu∂∂u+Δv∂∂vfiX∗=0,where i=1,2,3, Δu=u−u∗, and Δv=v−v∗. Xk is the solution of equation (7) and can be seen as an approximate solution of X∗. Equation (7) can be transformed into a matrix equation:(8)FX∗+F′X∗Xk−X∗=0.Then, the least squares solution ofX∗ can be obtained:(9)X∗=Xk+F′X∗TF′X∗−1F′X∗TFX∗.Therefore, the shortest distance between the robot and the obstacle isD∗u,v=RQ∗, where ⋅ represents the modular of vector from Ru,v to Q∗u,v. 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η1=x,y,φT in the earth coordinate system, the state of three degrees of freedom is represented by η2=u,v,rT in the body coordinate system, and the control input is represented by T1,T2,T3. Then, the kinematics model is expressed as follows:(10)x˙y˙φ˙=cosφsinφ0sinφcosφ0001uvr.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:(11)Mv˙+C+Dv=τ,where M is the inertial mass matrix of the robot, C is the coefficient matrix of centripetal force and Coriolis force, D 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:(12)u˙=m22m11vτ−d11m11u+τ1m11,v˙=m11m22ur−d22m22v,r˙=m11−m22m33uv−d33m33u+τ3m33,where mii and dii are the main diagonal elements of the M and D. The resultant force vectors in the three directions generated by the robot’s thrusters are as follows:(13)TxTyTz=1110000l−lT1T2T3.where l is the distance from the thruster to the centroid of the robot, Td=Tx,Ty,TzT is the expected output force, τ=T1,T2,T3T is thrust output of the thruster, and B 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:(14)minJ=Bτ−TdTBτ−Td,s.t.,τmin≤τ≤τmax.


## 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 isT and the shortest switching time interval is ΔT. There are M shortest switching time intervals and N target maneuver positions. Ti represents the time of the i−th thrust arc, and Mi denotes the number of shortest switching time intervals in the i−th thrust arc.(15)T=MΔT,Ti=MiΔT,M=∑j=1MMi.Suppose that the actual constant thrusts of the robot isF, the maximum thrusts is F^, and the theoretical continuous thrusts is F∗. The thrusters can provide different sizes of constant thrust to meet different thrust requirements. There are N different sizes of constant thrust which can be denoted as follows:(16)F^N,2F^N,3F^N,…,N−1F^N,F^.The size of the constant thrust is calculated as follows. There areN+1 thrust levels that can be selected: LF^/N,L=0,1,2,…,N, and the level of the constant thrust can be calculated as follows, taking the i−th thrust arc as example:(17)L=N∫TiTi+ΔTF∗tdtF^ΔT,where [] represents the bracket function and F∗t represents the absolute value of F∗t.The constant thrust fitting should be discussed in several categories, for convenience, taking thei−th thrust arc as example. Suppose that the theoretical working time in the i−th thrust arc is t∗. The simplest case is the theoretical working time in the i−th thrust arc t∗=0; then, the actual constant thrust is F=F∗=0. If the theoretical working time in the i−th thrust arc is ΔT≤t∗≤Ti=MiΔT, then the constant thrust fitting should be discussed in the following situations.Case 1.
t∗=ΔT<Ti.
Without loss of generality, assume thatΔIi is the impulse error in the i−th thrust arc and can be calculated as follows:(18)ΔIi=sgnF∗t∫TiTi+ΔTF∗tdt−LF^ΔTN.
Suppose that the value of the impulse compensation threshold is a positive constantγ>0. When ΔIi satisfies the following condition:(19)∫TiTi+ΔTF∗tdt−F^ΔTNN∫TiTi+ΔTF∗tdtF^ΔT≤γ,the actual constant thrust of the robot can be calculated as follows:(20)F=sgnF∗tF^ΔTNN∫TiTi+ΔTF∗tdtF^ΔT.
Then, the robot does not need to carry out impulse compensation. When the impulse errorΔIi satisfies the following condition:(21)γ<∫TiTi+ΔTF∗tdt−F^ΔTNN∫TiTi+ΔTF∗tdtF^ΔT,then the robot should carry out impulse compensation and the size of the constant thrust impulse compensation can be calculated as follows:(22)ΔIi=sgnF∗tF^ΔTN.Case 2.
t∗=MiΔT.
For this situation, the impulse error in thei−th thrust arc ΔIi can be calculated as follows:(23)ΔIi=∑j=0Mi∫Ti+jΔTTi+j+1ΔTF∗t−sgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt.
If there existn1 shortest switching time intervals satisfying the following conditions, without loss of generality, suppose that these time intervals are the first n1 shortest switching time interval, taking the j−th shortest switching time interval as an example:(24)∫Ti+jΔTTi+j+1ΔTF∗tdt≤γ.
Then, the size of the impulse compensation can be calculated as follows. When the impulse errorΔIi satisfies the following condition:(25)∑j=0Mi∫Ti+jΔTTi+j+1ΔTF∗t−sgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt≤γ,the actual constant thrust of the robot can be calculated as follows, taking the j−th shortest switching time interval as an example:(26)F=∫Ti+jΔTTi+j+1ΔTsgnF∗tdtF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔT,j=0,1,…,Mi.
Then, the robot will not carry out impulse compensation.
When the impulse errorΔIzi satisfies the following condition:(27)∫TiTi+MiΔTF∗tdt−∫Ti+n1+1ΔTTi+MiΔTsgnF∗tF^NzN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt≤γ,and there is a constant n2 that satisfies the following equation:(28)∫TiTi+MiΔTF∗tNzF^ΔTdt−∫Ti+n1+1ΔTTi+MiΔTsgnF∗tΔTN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt=n2,the actual constant thrust of the robot can be calculated as follows:(29)∫Ti+n1+1ΔTTi+MiΔTsgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt.
Then, the robot does not need to carry out impulse compensation. When the impulse errorΔIzi satisfies the following condition:(30)∫TiTi+MiΔTF∗tdt−∫Ti+n1+1ΔTTi+MiΔTsgnF∗tF^ΔTN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt>γ,then the robot should carry out impulse compensation and the size of the constant thrust impulse compensation can be calculated as follows:(31)ΔIi=n2sgnF∗tΔT.Case 3.
t∗=M1ΔT1<m1<Mi.
Without loss of generality, suppose thatt∗ is the first m1 shortest switching time interval and the impulse error in the i−th thrust arc ΔIi can be calculated as follows:(32)ΔIi=∑j=0M1∫Ti+jΔTTi+j+1ΔTF∗t−sgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt.
Furthermore, if there arem1 shortest switching time intervals satisfying the following conditions, without loss of generality, suppose that these time intervals are the top m1 shortest switching time interval, taking the j−th shortest switching time interval as an example:(33)∫Ti+jΔTTi+j+1ΔTF∗tdt≤γ.
Then, the size of the impulse compensation can be calculated as follows. WhenΔIi satisfies the following condition:(34)∑j=0M1∫Ti+jΔTTi+j+1ΔTF∗t−sgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt≤γ,the actual constant thrust of the robot can be calculated as follows, taking the j−th shortest switching time interval as an example:(35)F=∫Ti+jΔTTi+j+1ΔTsgnF∗tdtF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔT.
Then, the robot will not carry out impulse compensation. When the impulse errorΔIi satisfies the following condition:(36)∫TiTi+MiΔTF∗tdt−∫Ti+m1+1ΔTTi+M1ΔTsgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt≤γ,and there is a constant m2 that satisfies the following equation:(37)∫TiTi+M1ΔTF∗tNF^ΔTdt−∫Ti+m1+1ΔTTi+MiΔTsgnF∗tΔTN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt=m2,then the actual constant thrust of the robot can be calculated as follows:(38)F=∫Ti+m1+1ΔTTi+M1ΔTsgnF∗tdtF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔT.
The robot will not carry out impulse compensation. When the impulse errorΔIi satisfies the following condition:(39)∫TiTi+M1ΔTF∗tdt−∫Ti+m1+1ΔTTi+M1ΔTsgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt>γ,then the robot should carry out impulse compensation and the constant thrust impulse compensation can be calculated as follows:(40)ΔIi=m2sgnF∗tΔT.


### 4.2. Path Planning Based on the Artificial Fluid Method

Based on the description in Section3, the procedure for path planning is as follows. First, the improved perturbation matrix Pu,v is calculated. Next, the disturbed fluid velocity vd is calculated by modifying the target velocity vT. Then, the planned path is obtained by the recursive integration of vd. 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 Pu,v is defined as follows [16]:(41)Pu,v=I−nqnqTD∗u,vρu,vnq2+λu,vfunqTD∗u,vσu,vfunq,where I is a 2×2 identity matrix, nq is a column vector, fu is a tangential vector (the partial derivative of fu,v on u) at the point Qu,v, λu,v is a saturation function defining the orientation of tangential velocity, and ⋅ is 2-norm of a vector or a matrix. Besides, ρu,v and σu,v are defined as the weight of nq and tq, respectively.(42)ρu,v=ρ0exp1D0−D∗u,v,σu,v=σ0exp1D0−D∗u,v,λu,v=1,vTTfunqTvT>λ0,vTTfunqTvTλ0,−λ0≤vTTfunqTvT≤λ0,−1,vTTfunqTvT<−λ0,where ρ0 is the repulsive parameter, σ0 is the tangential parameter, D0 is the minimum permitted distance between the boundary of on-water automatic rescue intelligent robot, and λ0 is a small positive threshold of the saturation function λu,v. Then, the disturbed fluid velocity vd can be calculated by(43)vd=Pu,vvT.


### 4.3. The Planned Path Can Avoid Obstacles Safely

Proof.
Vectorsnq and fu are perpendicular exactly: nqTfu=0. Suppose that the distance between the boundary of on-water automatic rescue intelligent robot is close to the minimum permitted distance D0. Suppose that D∗=D0+δu,v is a monotonically decreasing function.(44)D∗u,vρu,v=D0+δu,vρ0e−1/δu,v,=D0ρ0e−1/δu,v1+δu,vD0D0/δu,vδu,v/D0ρ0e−1/δu,v.
It can be inferred thatδu,v⟶0+; then, D0ρ0e−1/δu,v⟶0, 1+δu,v/D0D0/δu,v⟶e, and δu,v/D0ρ0e−1/δu,v⟶0; therefore, D∗u,vρu,v⟶1.(45)Pu,v=I−nqnqTnq2+λu,vfunqTfunq.
Therefore,(46)nqTvd=nqTI−nqnqTnq2+λu,vfunqTfunqvT.
BecausenqTfu=0 means that nqTvd=0, 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., velocitiesvT and vd should satisfy vTTvd≥0, and the planned path will converge to the target point. Besides, the condition vd≈vT should be satisfied when the on‐water automatic rescue intelligent robot is near to the destination ξT=xT,yT,zT.(47)vTTvd=vT21−cos2vT,nqD∗u,vρu,v+λu,vvTTfunqTvTD∗u,vσu,vfunq,where ⋅ is inner product of vectors and vT,nq denotes the angle between vT and nq. It is obvious that when the on-water automatic rescue intelligent robot approaches the destination, then D∗u,v⟶0+ and D∗u,vρ0exp1/D0−D∗u,v⟶+∞. Furthermore, 1−cos2vT,nq/D∗u,vρu,v≥0 and vTTvd≥0 hold.
When the robot approaches the destination,D∗u,vρu,v⟶+∞ and D∗u,vσu,v⟶+∞. It can be inferred that Pu,v⟶I and vd≈vT hold.


### 4.5. Analysis of the Disturbed Fluid Velocityvd

The modified velocityvd defined by equation (43) can be expressed as(48)vd=vT−nqTvTD∗u,vρu,vnq2nq+λu,vnqTvTD∗u,vσu,vfunqfu.It can be seen thatvd consists of three parts: vT can be called the target velocity; nqTvT/D∗u,vρu,vnq2nq is taken as the repulsive velocity; and λu,vnqTvT/D∗u,vσu,vfunqfu can be called the tangential velocity. Similarly, the perturbation matrix Pu,v can be divided into three parts: attractive matrix I, repulsive matrix nqnqT/D∗u,vρu,vnq2, and tangential matrix λu,vfunqT/D∗u,vσu,vfunq. It can be analyzed that the magnitudes of repulsive and tangential velocities increase with ρu,v and σu,v, respectively. Therefore, the shape of the path can be readjusted by changing parameters ρu,v and σu,v. 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 vectornξ and the tangential plane which is perpendicular to the normal vector can be accurately obtained. The smaller the angle between the tangential vector tξ and the original speed vξ, the greater the component of the tangential vector tξ in the direction of the original velocity vξ. Then, the robot can reach the destination faster and make the path shorter. Thus, the tangential vector tnewξ is redefined as follows:(49)tnewξ=argmintξ⋅nξ=0cosvξ,tξ,vξ≠ω⋅nξ,∂Fξ∂y,−∂Fξ∂x,0T,vξ=ω⋅nξ,where vξ,tξ refers to the angle between original velocity vξ and vector tξ and ω is a constant.(1)
Whenvξ=ω⋅nξ, the original speed vξ is parallel to normal vector nξ. In this case, any direction on the tangential plane is the shortest direction, and the tangential vector tξ is selected in the horizontal direction in this paper.(2)
Whenvξ≠ω⋅nξ, the direction of new tangential vector tnewξ is the vertical projection of the original velocity vξ on the tangential plane.Suppose that original speedvξ and normal vector nξ are defined as follows:(50)vξ=−Cx−xddξ,Cy−yddξ,Cz−zddξT=v1,v2,v3T,nξ=−∂Fξ∂x,∂Fξ∂y,∂Fξ∂zT=n1,n2,n3T.Then, the normal vector of tangential planeα is nξ. Original speed vξ and normal vector nξ are in the cut plane β; thus, the normal vector nβξ of cut plane β can be obtained:(51)nβξ=vξ×nξ=v2v3n2n3,v3v1n3n1,v1v2n1n2T=v2n3−v3n2,v3n1−v1n3,v1n2−v1n2T=nβ1,nβ2,nβ3T.Then,tnewξ can be calculated:(52)tnewξ=nβξ×nξ=nβ2nβ3n2n3,nβ3nβ1n3n1,nβ1nβ2n1n2T.Furthermore, the improved tangential vectorTξ is defined as follows:(53)Tξ=λ1tξtξ+λ2tnewξtnewξ,where tξ=∂Fξ/∂y,∂Fξ/∂x,0T, λ1,λ2 are inspiration factors, which are determined by the robot properties, and λ1/λ2 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 case3 as an example, if the impulse error ΔIi satisfies the condition of equation (34), the total number of the accelerating time intervals and the decelerating time intervals is M1 and the number of zero-thrust time intervals is Mi−M1. The position of the three types of time intervals is decided by the curve of the theoretical continuous thrust F∗t.If the impulse errorΔIi satisfies the condition of equation (36), the total number of the accelerating time intervals and the decelerating time intervals is Mi−M1 and the number of zero-thrust time intervals is Mi−M1+m1. The position of the three types of time intervals is decided by the curve of the theoretical continuous thrust F∗t.If the impulse errorΔIi satisfies the condition of equation (39), the total number of the accelerating time intervals and the decelerating time intervals is M1−m1+m2 and the number of zero-thrust time intervals is Mi−M1+m1−m2. The position of the three types of time intervals is decided by the curve of the theoretical continuous thrust F∗t.At last, the switching control laws for the collision avoidance maneuver can be given. For convenience, let us take the time intervals in thei−th thrust arc for example:(54)Si=Ti+jΔT,sgnF∗tF^ΔTNN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔT.


## 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 isT and the shortest switching time interval is ΔT. There are M shortest switching time intervals and N target maneuver positions. Ti represents the time of the i−th thrust arc, and Mi denotes the number of shortest switching time intervals in the i−th thrust arc.(15)T=MΔT,Ti=MiΔT,M=∑j=1MMi.Suppose that the actual constant thrusts of the robot isF, the maximum thrusts is F^, and the theoretical continuous thrusts is F∗. The thrusters can provide different sizes of constant thrust to meet different thrust requirements. There are N different sizes of constant thrust which can be denoted as follows:(16)F^N,2F^N,3F^N,…,N−1F^N,F^.The size of the constant thrust is calculated as follows. There areN+1 thrust levels that can be selected: LF^/N,L=0,1,2,…,N, and the level of the constant thrust can be calculated as follows, taking the i−th thrust arc as example:(17)L=N∫TiTi+ΔTF∗tdtF^ΔT,where [] represents the bracket function and F∗t represents the absolute value of F∗t.The constant thrust fitting should be discussed in several categories, for convenience, taking thei−th thrust arc as example. Suppose that the theoretical working time in the i−th thrust arc is t∗. The simplest case is the theoretical working time in the i−th thrust arc t∗=0; then, the actual constant thrust is F=F∗=0. If the theoretical working time in the i−th thrust arc is ΔT≤t∗≤Ti=MiΔT, then the constant thrust fitting should be discussed in the following situations.Case 1.
t∗=ΔT<Ti.
Without loss of generality, assume thatΔIi is the impulse error in the i−th thrust arc and can be calculated as follows:(18)ΔIi=sgnF∗t∫TiTi+ΔTF∗tdt−LF^ΔTN.
Suppose that the value of the impulse compensation threshold is a positive constantγ>0. When ΔIi satisfies the following condition:(19)∫TiTi+ΔTF∗tdt−F^ΔTNN∫TiTi+ΔTF∗tdtF^ΔT≤γ,the actual constant thrust of the robot can be calculated as follows:(20)F=sgnF∗tF^ΔTNN∫TiTi+ΔTF∗tdtF^ΔT.
Then, the robot does not need to carry out impulse compensation. When the impulse errorΔIi satisfies the following condition:(21)γ<∫TiTi+ΔTF∗tdt−F^ΔTNN∫TiTi+ΔTF∗tdtF^ΔT,then the robot should carry out impulse compensation and the size of the constant thrust impulse compensation can be calculated as follows:(22)ΔIi=sgnF∗tF^ΔTN.Case 2.
t∗=MiΔT.
For this situation, the impulse error in thei−th thrust arc ΔIi can be calculated as follows:(23)ΔIi=∑j=0Mi∫Ti+jΔTTi+j+1ΔTF∗t−sgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt.
If there existn1 shortest switching time intervals satisfying the following conditions, without loss of generality, suppose that these time intervals are the first n1 shortest switching time interval, taking the j−th shortest switching time interval as an example:(24)∫Ti+jΔTTi+j+1ΔTF∗tdt≤γ.
Then, the size of the impulse compensation can be calculated as follows. When the impulse errorΔIi satisfies the following condition:(25)∑j=0Mi∫Ti+jΔTTi+j+1ΔTF∗t−sgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt≤γ,the actual constant thrust of the robot can be calculated as follows, taking the j−th shortest switching time interval as an example:(26)F=∫Ti+jΔTTi+j+1ΔTsgnF∗tdtF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔT,j=0,1,…,Mi.
Then, the robot will not carry out impulse compensation.
When the impulse errorΔIzi satisfies the following condition:(27)∫TiTi+MiΔTF∗tdt−∫Ti+n1+1ΔTTi+MiΔTsgnF∗tF^NzN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt≤γ,and there is a constant n2 that satisfies the following equation:(28)∫TiTi+MiΔTF∗tNzF^ΔTdt−∫Ti+n1+1ΔTTi+MiΔTsgnF∗tΔTN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt=n2,the actual constant thrust of the robot can be calculated as follows:(29)∫Ti+n1+1ΔTTi+MiΔTsgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt.
Then, the robot does not need to carry out impulse compensation. When the impulse errorΔIzi satisfies the following condition:(30)∫TiTi+MiΔTF∗tdt−∫Ti+n1+1ΔTTi+MiΔTsgnF∗tF^ΔTN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt>γ,then the robot should carry out impulse compensation and the size of the constant thrust impulse compensation can be calculated as follows:(31)ΔIi=n2sgnF∗tΔT.Case 3.
t∗=M1ΔT1<m1<Mi.
Without loss of generality, suppose thatt∗ is the first m1 shortest switching time interval and the impulse error in the i−th thrust arc ΔIi can be calculated as follows:(32)ΔIi=∑j=0M1∫Ti+jΔTTi+j+1ΔTF∗t−sgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt.
Furthermore, if there arem1 shortest switching time intervals satisfying the following conditions, without loss of generality, suppose that these time intervals are the top m1 shortest switching time interval, taking the j−th shortest switching time interval as an example:(33)∫Ti+jΔTTi+j+1ΔTF∗tdt≤γ.
Then, the size of the impulse compensation can be calculated as follows. WhenΔIi satisfies the following condition:(34)∑j=0M1∫Ti+jΔTTi+j+1ΔTF∗t−sgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt≤γ,the actual constant thrust of the robot can be calculated as follows, taking the j−th shortest switching time interval as an example:(35)F=∫Ti+jΔTTi+j+1ΔTsgnF∗tdtF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔT.
Then, the robot will not carry out impulse compensation. When the impulse errorΔIi satisfies the following condition:(36)∫TiTi+MiΔTF∗tdt−∫Ti+m1+1ΔTTi+M1ΔTsgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt≤γ,and there is a constant m2 that satisfies the following equation:(37)∫TiTi+M1ΔTF∗tNF^ΔTdt−∫Ti+m1+1ΔTTi+MiΔTsgnF∗tΔTN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt=m2,then the actual constant thrust of the robot can be calculated as follows:(38)F=∫Ti+m1+1ΔTTi+M1ΔTsgnF∗tdtF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔT.
The robot will not carry out impulse compensation. When the impulse errorΔIi satisfies the following condition:(39)∫TiTi+M1ΔTF∗tdt−∫Ti+m1+1ΔTTi+M1ΔTsgnF∗tF^NN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔTdt>γ,then the robot should carry out impulse compensation and the constant thrust impulse compensation can be calculated as follows:(40)ΔIi=m2sgnF∗tΔT.


## 4.2. Path Planning Based on the Artificial Fluid Method

Based on the description in Section3, the procedure for path planning is as follows. First, the improved perturbation matrix Pu,v is calculated. Next, the disturbed fluid velocity vd is calculated by modifying the target velocity vT. Then, the planned path is obtained by the recursive integration of vd. 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 Pu,v is defined as follows [16]:(41)Pu,v=I−nqnqTD∗u,vρu,vnq2+λu,vfunqTD∗u,vσu,vfunq,where I is a 2×2 identity matrix, nq is a column vector, fu is a tangential vector (the partial derivative of fu,v on u) at the point Qu,v, λu,v is a saturation function defining the orientation of tangential velocity, and ⋅ is 2-norm of a vector or a matrix. Besides, ρu,v and σu,v are defined as the weight of nq and tq, respectively.(42)ρu,v=ρ0exp1D0−D∗u,v,σu,v=σ0exp1D0−D∗u,v,λu,v=1,vTTfunqTvT>λ0,vTTfunqTvTλ0,−λ0≤vTTfunqTvT≤λ0,−1,vTTfunqTvT<−λ0,where ρ0 is the repulsive parameter, σ0 is the tangential parameter, D0 is the minimum permitted distance between the boundary of on-water automatic rescue intelligent robot, and λ0 is a small positive threshold of the saturation function λu,v. Then, the disturbed fluid velocity vd can be calculated by(43)vd=Pu,vvT.


## 4.3. The Planned Path Can Avoid Obstacles Safely

Proof.
Vectorsnq and fu are perpendicular exactly: nqTfu=0. Suppose that the distance between the boundary of on-water automatic rescue intelligent robot is close to the minimum permitted distance D0. Suppose that D∗=D0+δu,v is a monotonically decreasing function.(44)D∗u,vρu,v=D0+δu,vρ0e−1/δu,v,=D0ρ0e−1/δu,v1+δu,vD0D0/δu,vδu,v/D0ρ0e−1/δu,v.
It can be inferred thatδu,v⟶0+; then, D0ρ0e−1/δu,v⟶0, 1+δu,v/D0D0/δu,v⟶e, and δu,v/D0ρ0e−1/δu,v⟶0; therefore, D∗u,vρu,v⟶1.(45)Pu,v=I−nqnqTnq2+λu,vfunqTfunq.
Therefore,(46)nqTvd=nqTI−nqnqTnq2+λu,vfunqTfunqvT.
BecausenqTfu=0 means that nqTvd=0, 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., velocitiesvT and vd should satisfy vTTvd≥0, and the planned path will converge to the target point. Besides, the condition vd≈vT should be satisfied when the on‐water automatic rescue intelligent robot is near to the destination ξT=xT,yT,zT.(47)vTTvd=vT21−cos2vT,nqD∗u,vρu,v+λu,vvTTfunqTvTD∗u,vσu,vfunq,where ⋅ is inner product of vectors and vT,nq denotes the angle between vT and nq. It is obvious that when the on-water automatic rescue intelligent robot approaches the destination, then D∗u,v⟶0+ and D∗u,vρ0exp1/D0−D∗u,v⟶+∞. Furthermore, 1−cos2vT,nq/D∗u,vρu,v≥0 and vTTvd≥0 hold.
When the robot approaches the destination,D∗u,vρu,v⟶+∞ and D∗u,vσu,v⟶+∞. It can be inferred that Pu,v⟶I and vd≈vT hold.


## 4.5. Analysis of the Disturbed Fluid Velocityvd

The modified velocityvd defined by equation (43) can be expressed as(48)vd=vT−nqTvTD∗u,vρu,vnq2nq+λu,vnqTvTD∗u,vσu,vfunqfu.It can be seen thatvd consists of three parts: vT can be called the target velocity; nqTvT/D∗u,vρu,vnq2nq is taken as the repulsive velocity; and λu,vnqTvT/D∗u,vσu,vfunqfu can be called the tangential velocity. Similarly, the perturbation matrix Pu,v can be divided into three parts: attractive matrix I, repulsive matrix nqnqT/D∗u,vρu,vnq2, and tangential matrix λu,vfunqT/D∗u,vσu,vfunq. It can be analyzed that the magnitudes of repulsive and tangential velocities increase with ρu,v and σu,v, respectively. Therefore, the shape of the path can be readjusted by changing parameters ρu,v and σu,v. 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 vectornξ and the tangential plane which is perpendicular to the normal vector can be accurately obtained. The smaller the angle between the tangential vector tξ and the original speed vξ, the greater the component of the tangential vector tξ in the direction of the original velocity vξ. Then, the robot can reach the destination faster and make the path shorter. Thus, the tangential vector tnewξ is redefined as follows:(49)tnewξ=argmintξ⋅nξ=0cosvξ,tξ,vξ≠ω⋅nξ,∂Fξ∂y,−∂Fξ∂x,0T,vξ=ω⋅nξ,where vξ,tξ refers to the angle between original velocity vξ and vector tξ and ω is a constant.(1)
Whenvξ=ω⋅nξ, the original speed vξ is parallel to normal vector nξ. In this case, any direction on the tangential plane is the shortest direction, and the tangential vector tξ is selected in the horizontal direction in this paper.(2)
Whenvξ≠ω⋅nξ, the direction of new tangential vector tnewξ is the vertical projection of the original velocity vξ on the tangential plane.Suppose that original speedvξ and normal vector nξ are defined as follows:(50)vξ=−Cx−xddξ,Cy−yddξ,Cz−zddξT=v1,v2,v3T,nξ=−∂Fξ∂x,∂Fξ∂y,∂Fξ∂zT=n1,n2,n3T.Then, the normal vector of tangential planeα is nξ. Original speed vξ and normal vector nξ are in the cut plane β; thus, the normal vector nβξ of cut plane β can be obtained:(51)nβξ=vξ×nξ=v2v3n2n3,v3v1n3n1,v1v2n1n2T=v2n3−v3n2,v3n1−v1n3,v1n2−v1n2T=nβ1,nβ2,nβ3T.Then,tnewξ can be calculated:(52)tnewξ=nβξ×nξ=nβ2nβ3n2n3,nβ3nβ1n3n1,nβ1nβ2n1n2T.Furthermore, the improved tangential vectorTξ is defined as follows:(53)Tξ=λ1tξtξ+λ2tnewξtnewξ,where tξ=∂Fξ/∂y,∂Fξ/∂x,0T, λ1,λ2 are inspiration factors, which are determined by the robot properties, and λ1/λ2 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 case3 as an example, if the impulse error ΔIi satisfies the condition of equation (34), the total number of the accelerating time intervals and the decelerating time intervals is M1 and the number of zero-thrust time intervals is Mi−M1. The position of the three types of time intervals is decided by the curve of the theoretical continuous thrust F∗t.If the impulse errorΔIi satisfies the condition of equation (36), the total number of the accelerating time intervals and the decelerating time intervals is Mi−M1 and the number of zero-thrust time intervals is Mi−M1+m1. The position of the three types of time intervals is decided by the curve of the theoretical continuous thrust F∗t.If the impulse errorΔIi satisfies the condition of equation (39), the total number of the accelerating time intervals and the decelerating time intervals is M1−m1+m2 and the number of zero-thrust time intervals is Mi−M1+m1−m2. The position of the three types of time intervals is decided by the curve of the theoretical continuous thrust F∗t.At last, the switching control laws for the collision avoidance maneuver can be given. For convenience, let us take the time intervals in thei−th thrust arc for example:(54)Si=Ti+jΔT,sgnF∗tF^ΔTNN∫Ti+jΔTTi+j+1ΔTF∗tdtF^ΔT.


## 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 isf1 and f2, and the evaluation values of the three algorithms are g1 and g2. The comprehensive evaluation function can be obtained as follows:(55)E=f1f2+g1g2.According to the comprehensive evaluation function, the following statistical table can be obtained as shown in Table1.Table 1
Comprehensive evaluation value.
Number of obstacle nodes50100150200250300Comprehensive evaluation value0.510.771.341.972.212.77The 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 Figure2 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.Figure 2
Planned path based on the A-star algorithm.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 Figure3.Figure 3
Planned path based on the ant colony algorithm.It can be seen from Figure3 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 Figures4 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 ξ=x,y,z is the robot position and obstacle center is ξ0=x0,y0,z0, with axis lengths a,b,c and index parameters d,e,f. Then, construct the function as follows [16]:(56)Fξ=x−x0a2d+y−y0b2e+z−z0c2f,where parameters a,b,c,d,e,f determine the shape and size of the obstacle: if a=b=c and d=e=f=1, the obstacle is a sphere; if a=b and d=e=1,0<f<1, the obstacle is regarded as a cone; if a and b are variables meeting the condition a=b=R1+R2−R1z/c and d=e=1,f>1 holds, the obstacle is a circular truncated cone approximately, where R1,R2 are radii of two bases. The obstacles are shown in Table 2, and ρ0ξ=1 and σ0ξ=1.Figure 4
The change of best individual fitness.Figure 5
The change of best individual fitness.Table 2
The shape parameters of each obstacle.
ObstacleShapeabcdefCenter of obstacle1Sphere3.93.93.9111(20, 20, 0)2Sphere666111(30, 3, 0)3Sphere444111(20, 30, 0)4Cylinder5551110(22, 7, 0)5Cylinder5551110(35, 15, 0)6Cone232110.3(33, 27, 0)7Cone111111110.3(8, 30, 0)8Cone8813.6110.3(8, 10, 0)9Cone555110.3(30, 36, 0)10Cone232110.3(29, 22, 0)It can be seen from Figure6 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.Figure 6
The planned path with (0, 0, 0) as starting point.


### 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 Figure7 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.Figure 7
Robot bypassing the obstacle and escaping the trap area.


### 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.Figure 8
The planned path according to different saturationλ.It can be seen from Figure8 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).Figure 9
The planned path with (12, 0, 0) as starting point.Figure 10
The planned path with (0, 10, 0) as starting point.Figure 11
The planned path with (36, −2, 0) as starting point.As can be seen from Figures9–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 Figure12 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.(57)S=0,0,0;40,40,0;3ΔT,2N;…;36ΔT,3N;…;58ΔT,−2N;…;60ΔT,−7N.Figure 12
The constant thrust fitting.


## 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 isf1 and f2, and the evaluation values of the three algorithms are g1 and g2. The comprehensive evaluation function can be obtained as follows:(55)E=f1f2+g1g2.According to the comprehensive evaluation function, the following statistical table can be obtained as shown in Table1.Table 1
Comprehensive evaluation value.
Number of obstacle nodes50100150200250300Comprehensive evaluation value0.510.771.341.972.212.77The 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 Figure2 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.Figure 2
Planned path based on the A-star algorithm.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 Figure3.Figure 3
Planned path based on the ant colony algorithm.It can be seen from Figure3 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 Figures4 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 ξ=x,y,z is the robot position and obstacle center is ξ0=x0,y0,z0, with axis lengths a,b,c and index parameters d,e,f. Then, construct the function as follows [16]:(56)Fξ=x−x0a2d+y−y0b2e+z−z0c2f,where parameters a,b,c,d,e,f determine the shape and size of the obstacle: if a=b=c and d=e=f=1, the obstacle is a sphere; if a=b and d=e=1,0<f<1, the obstacle is regarded as a cone; if a and b are variables meeting the condition a=b=R1+R2−R1z/c and d=e=1,f>1 holds, the obstacle is a circular truncated cone approximately, where R1,R2 are radii of two bases. The obstacles are shown in Table 2, and ρ0ξ=1 and σ0ξ=1.Figure 4
The change of best individual fitness.Figure 5
The change of best individual fitness.Table 2
The shape parameters of each obstacle.
ObstacleShapeabcdefCenter of obstacle1Sphere3.93.93.9111(20, 20, 0)2Sphere666111(30, 3, 0)3Sphere444111(20, 30, 0)4Cylinder5551110(22, 7, 0)5Cylinder5551110(35, 15, 0)6Cone232110.3(33, 27, 0)7Cone111111110.3(8, 30, 0)8Cone8813.6110.3(8, 10, 0)9Cone555110.3(30, 36, 0)10Cone232110.3(29, 22, 0)It can be seen from Figure6 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.Figure 6
The planned path with (0, 0, 0) as starting point.


## 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 Figure7 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.Figure 7
Robot bypassing the obstacle and escaping the trap area.


## 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.Figure 8
The planned path according to different saturationλ.It can be seen from Figure8 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).Figure 9
The planned path with (12, 0, 0) as starting point.Figure 10
The planned path with (0, 10, 0) as starting point.Figure 11
The planned path with (36, −2, 0) as starting point.As can be seen from Figures9–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 Figure12 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.(57)S=0,0,0;40,40,0;3ΔT,2N;…;36ΔT,3N;…;58ΔT,−2N;…;60ΔT,−7N.Figure 12
The constant thrust fitting.


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



---

*Source: 8828039-2020-10-21.xml*