CN105589464B - A UUV Dynamic Obstacle Avoidance Method Based on Velocity Obstacle Method - Google Patents

Abstract

A kind of UUV dynamic obstacle avoidance method based on Speed Obstacles method, is related to a kind of UUV dynamic obstacle avoidance method.Solve the problems, such as that the paths planning method of existing UUV is poor in the presence of the accuracy for avoiding moving obstacle in dynamic environment.The movement uncertainty of barrier is converted locational uncertainty by the present invention；It is uncertain according to the movement of barrier, obtain minimum safe angle α₁With maximum safety angle α₂；Obtain multi-obstacle avoidance comprehensive speed danger level caused by UUV, according to UUV Dynamic Constraints rule, the velocity space for determining UUV movement obtains the minimum collision time of UUV and all barriers according to the maximum effect range of the locational uncertainty of barrier and barrier；Using the comprehensive speed danger level and UUV of UUV and the minimum collision time of all barriers, the optimization aim path function of UUV is obtained；The minimum point that objective function is found using Speed Obstacles method realizes the planning to UUV motion path as next way point of UUV movement.The present invention is suitable for UUV dynamic obstacle avoidance.

Description

A UUV Dynamic Obstacle Avoidance Method Based on Velocity Obstacle Method

technical field

The invention relates to a UUV dynamic obstacle avoidance method.

Background technique

With the continuous increase of human demand for marine resources, the space for human activities has gradually expanded from nearshore and shallow seas to deeper waters. UUV technology has received more and more attention, especially in the military field of various countries. important role.

The route planning of UUV (Unmanned Underwater Vehicle) is an important guarantee for efficient and safe completion of operational tasks. UUVs may encounter dynamic obstacles during navigation, which requires UUVs to make rapid and effective obstacle avoidance responses to moving obstacles. Many scholars at home and abroad have conducted in-depth research on the dynamic obstacle avoidance problem, and proposed many effective planning methods, such as artificial potential field method (APF), vector field histogram method, dynamic window method (DWA) and behavior method, etc. It has strong adaptability to the local environment, and only relies on limited sensor information to avoid collisions online, with high efficiency. However, for dynamic obstacles, the potential collision area is the real danger area, and the non-current obstacle area has the problem of poor accuracy in avoiding moving obstacles.

SUMMARY OF THE INVENTION

In order to solve the problem that the existing UUV path planning method has poor accuracy in avoiding moving obstacles in a dynamic environment, the present invention. A UUV dynamic obstacle avoidance method based on the speed obstacle method is proposed.

A UUV dynamic obstacle avoidance method based on the speed obstacle method according to the present invention, the specific steps of the method are:

Step 1: Convert the motion uncertainty of the obstacle into the position uncertainty; obtain the minimum safety angle α ₁ and the maximum safety angle α ₂ according to the motion uncertainty of the obstacle;

Step 2: According to the minimum safe angle α ₁ and the maximum safe angle α ₂ obtained in step 1, obtain the risk degree VR _i (v _r ) of the ith obstacle when the UUV sails at the speed v _r as:

Among them, γ is the collision angle between the UUV and the obstacle; i=1,2,...,n, n is the number of obstacles;

n obstacles Multiple different speed dangers are generated for the sailing speed v _r of the UUV, and the comprehensive speed danger caused by n obstacles to the UUV is:

Step 3: Determine the velocity space Ω _r of UUV motion according to the UUV dynamic constraint law;

Step 4. According to the position uncertainty of the obstacle and the maximum action range of the obstacle, establish the UUV to reach the obstacle The shortest time function of the edge; obtain the minimum collision time t _col (v _r ) of the UUV and all obstacles; where X _ro is the distance between the UUV and the obstacle and is the upper limit estimate of the radius of the obstacle O _i ;

Step 5. Obtain the optimized target path function of the UUV by using the comprehensive speed risk VR(v _r ) of the UUV and the minimum collision time t _col (v _r ) between the UUV and all obstacles;

Step 6: Use the speed obstacle method to find the minimum point of the objective function as the next waypoint of the UUV motion, so as to realize the planning of the UUV motion path.

The invention converts the motion uncertainty of the obstacle into the position uncertainty, which not only reduces the collision impact caused by the motion uncertainty of the obstacle, but also avoids the conservative problem of collision avoidance caused by the direct expansion of the moving obstacle. Touch safety.

Through the comprehensive speed risk degree of UUV and the maximum forward speed in the achievable speed space, the optimized target path function of UUV is determined, so that the UUV avoids collision decision-making speed is fast, and it can obtain better collision avoidance effect in the dynamic obstacle environment. It has good adaptability to unknown dynamic environment.

Description of drawings

1 is a schematic diagram of UUV environment modeling in the present invention;

Fig. 2 is the schematic diagram of the collision avoidance process of UUV speed obstacle in the present invention;

3 is a schematic diagram of the collision angle between the UUV and an obstacle according to Embodiment 1;

FIG. 4 is a simulation diagram of UUV dynamic collision avoidance in the present invention.

Detailed ways

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT 1. This embodiment will be described with reference to FIG. 1 , FIG. 2 and FIG. 3 . A UUV dynamic obstacle avoidance method based on the speed obstacle method described in this embodiment. The specific steps of the method are:

Step 1: Convert the motion uncertainty of the obstacle to the position uncertainty, and obtain the lower bound estimated value R _ro of the radius of the obstacle O _i and the upper bound estimated value of the radius of the obstacle O _i

According to the motion uncertainty of the obstacle, the lower bound estimated value R _ro of the radius of the obstacle O _i is obtained: R _{ro =} R _o +R _r +δp

The upper bound estimate of the radius of the obstacle O _i

δ _p is the estimated deviation of the obstacle radius, ΔR _o is the estimated upper limit of the obstacle motion uncertainty, R _safe is the safe distance, R _o is the obstacle radius, and R _r is the UUV radius;

Estimate the lower limit R _ro according to the radius of the obstacle, and calculate the minimum safe angle Estimated upper bound based on obstacle radius maximum safety angle γ is the collision angle between the UUV and the obstacle. When α ₁ < γ < α ₂ , due to the motion uncertainty of the obstacle, it is a probabilistic event that the UUV maintains the current speed and heading to collide with the obstacle. The closer γ is to α ₁ , The greater the probability of collision between the UUV and the obstacle, the closer γ is to α ₂ , the smaller the probability of the collision between the UUV and the obstacle; the risk of the ith obstacle when the UUV sails at the speed v _r VR _i (v _r ):

Due to the collision angle γ: When the speed v _o of the obstacle is determined, for any speed v _r in the speed change space of the UUV, find the relative movement speed v _ro of the UUV and the obstacle and the distance X _ro of the UUV and the obstacle to obtain the corresponding speed danger, That is, the safety risk brought by the UUV sailing at the speed v _r ; for multiple obstacles A number of different speed hazards will be generated for the sailing speed v _r of the UUV, and the comprehensive speed hazard caused to the UUV is:

Step 2: In the speed space, considering the kinematic constraints of UUV, the achievable speed space of UUV is:

In order to reduce the computational complexity, the maximum magnitude change of UUV linear velocity and heading change in the decision period ΔT are used to approximate its kinematic constraints; where Δt _f is the prediction time of the attainable speed, ΔT is the decision period, Δw _max is the maximum change of the heading angular velocity within the period ΔT, θ _r is the heading angle of the UUV motion, θ _r is the lower limit of the heading angle of the UUV motion, is the upper limit of the heading angle of the UUV motion, Δv _max is the maximum variation of the linear velocity within the period ΔT, is the maximum forward speed, It is the minimum forward speed. In the case of no auxiliary thrust and the main thrust differential is not allowed, in order to ensure the rudder effect when the UUV adjusts the course, the The value of the threshold is set; v _x is the horizontal speed of the UUV, v _y is the vertical speed of the UUV, V _r is the lower limit of the forward speed, is the upper limit of the forward speed, and for the predicted time of the attainable speed, let Δt _f =4ΔT;

Step 3: Calculate the collision time between the UUV and the obstacle according to the position uncertainty of the obstacle and the maximum action range of the obstacle;

The collision time is the shortest time for the UUV to collide with the obstacle when the relative speed of the UUV and the obstacle remains unchanged. It is a general measure for evaluating the collision risk, and the collision time also reflects the UUV's own safety time limit;

After the obstacle is puffed up, the maximum action range of the obstacle is obstacles of radius The speed barrier formed is VO _i , when v _r ∈ VO _i , the collision time τ represents reaching the barrier with relative velocity v _ro The shortest time for the edge, τ satisfies the following formula:

in, express , when there are multiple solutions in equation (8), take the minimum time solution as the collision time of v _ro , UUV and obstacle The conditions for a collision to occur are:

Among them, λ(v _ro )={tv _ro |t>0} is the ray from the origin along the relative velocity direction v _ro of the UUV and the obstacle;

when hour, UUV sails in v _ro not with obstacles Collision;

and when , v _r ∈ VO _i , when the UUV sails with the relative speed v _ro to the obstacle, The closest collision distance RA at which a collision occurs is:

UUV vs obstacles when sailing at relative speed v _ro The time of the collision is:

When there are multiple obstacles distributed in the environment, when the UUV navigates with v _r , the minimum collision time with all obstacles is t _col (v _r ), namely: t _col (v _r )=min(t _col (v _ro ) ,O _i )), i=1,2,...,n;

Step 5: Optimize the objective function;

Find the optimal collision avoidance speed v ^* of the UUV relative to the target point in the speed space:

v ^* =arg min(J _d (v _r )), X _r →X _G , v _r ∈Ω _r (11)

Among them, J _d (v _r ) is the target path function of the UUV; X _r is the position coordinate of the UUV, and X _G is the position coordinate of the target point. In order to make the UUV sail toward the target point, the speed of the UUV relative to the target point is:

The collision avoidance speed is determined by two factors: safety and approaching the target; reducing the risk of collision, improving safety, and rapidly approaching the target position, therefore, the optimization objective function of collision avoidance decision-making is defined as:

The optimization objective function of collision avoidance decision is composed of three parts: collision risk, target speed deviation and collision time; where ω _p , ω _v , ω _t are weight coefficients; ω _p +ω _v =1; when VR When (v _r )=1, the UUV traveling at the speed v _r must collide with the obstacle, so v _r is not desirable.

Due to the limited reachable speed space Ω _r of UUV, sometimes Ω _r is within the speed barrier, and UUV cannot find a safe speed to escape the collision zone in Ω _r , that is, J(v _r )=∞, It means that any speed in the reachable speed space will cause a collision; in order to avoid a collision, the UUV must decelerate to the minimum speed and adjust the heading with the maximum steering capability, θ _r takes the boundary heading θ _r and The heading with the smallest deviation from the target heading. Realize the collision avoidance of UUV motion. Using the speed obstacle method to find the minimum point of the objective function value as the next waypoint, and gradually realize the UUV route planning.

Specific examples:

The UUV sails from the starting point (0, 0) to the ending point (450, 450), and the starting heading is 45°. Three animal obstacles O ₁ , O ₂ , O ₃ are designed to intersect with the UUV, assuming that the animal obstacles are set as rectangle. The position and motion information of obstacles are unknown, and forward-looking sonar is used as the collision avoidance perception device. Use formula (1) to get the speed danger of moving obstacles, use formula (3) to obtain the reachable speed space of moving obstacles, and use formula (13) to obtain the coefficients in the optimization objective function. Then, use the speed obstacle method to find the minimum point of the objective function value as the next waypoint, and gradually realize the UUV route planning. The simulation trajectory is shown in Figure 4.

Claims (5)

1. A UUV dynamic obstacle avoidance method based on a speed obstacle method is characterized by comprising the following specific steps:

step one, converting the motion uncertainty of the obstacle into position uncertainty, and obtaining a minimum safety angle α according to the motion uncertainty of the obstacle₁And a maximum safety angle α₂；

Step two, obtaining the minimum safety angle α according to the step one₁And a maximum safety angle α₂Obtaining velocity v of i-th obstacle to UUV_rRisk VR during voyage_i(v_r) Comprises the following steps:

wherein γ is the impingement angle of the UUV with the obstacle; 1,2, n, n is the number of obstacles;

multi-obstacleSpeed v of sailing UUV_rGenerating a plurality of different speed hazards, a comprehensive speed hazard VR (v) caused by n obstacles to UUV_r) Comprises the following steps:

step three, determining the velocity space omega of UUV motion according to the UUV dynamics constraint law_r；

Step four, according to the position uncertainty of the obstacle and the maximum action range of the obstacle, establishing UUV to reach the obstacleThe shortest time function of the edge; obtaining the minimum collision time t of the UUV and all the obstacles_col(v_r) (ii) a Wherein, X_roIs the sum of the distances between UUV and obstacleIs an obstacle O_iAn upper estimate of the radius;

step five, utilizing comprehensive speed risk degree VR (v) of UUV_r) And minimum collision time t of UUV with all obstacles_col(v_r) Obtaining an optimized target path function of the UUV;

and step six, searching a minimum value point of the target function by using a speed obstacle method, and using the minimum value point as a next route point of the UUV motion to plan the UUV motion path.

2. The UUV dynamic obstacle avoidance method based on the velocity obstacle method as claimed in claim 1, wherein in the step one, the minimum safety angle α is obtained according to the motion uncertainty of the obstacle₁And a maximum safety angle α₂The method comprises the following steps:

obtaining an obstacle O according to the motion uncertainty of the obstacle_iLower limit estimate of radiusR _ro：R _ro＝R_o+R_r+δ_p；

Obstacle O_iUpper estimate of radiusδ_pIs the estimated deviation of the obstacle radius, △ R_oIs the upper limit of the estimate of the uncertainty of the obstacle motion, R_safeIs a safe distance, R_oIs the radius of the obstacle, R_rIs the UUV radius;

estimating a lower bound from an obstacle radiusR _roCalculating the minimum safety angleUpper limit of estimation based on radius of obstacleGet the maximum safety angle

3. The UUV dynamic obstacle avoidance method based on the velocity obstacle method according to claim 1 or 2, wherein the velocity space Ω of UUV motion is determined in the step three_rBy:

obtaining a mixture of, in which,

wherein, △ t_fIs the predicted time of UUV velocity, let △ t_f4 △ T, △ T is decision period, △ w_maxIs the maximum change in heading angular velocity, θ, over a period △ T_rIs the heading angle of the UUV motion,θ_r is the lower limit of the heading angle of the UUV motion,is the upper limit of the heading angle of UUV motion, △ v_maxIs the maximum amount of change in linear velocity over the period △ T,is the maximum forward speed of the vehicle,is the UUV minimum forward velocity, v_xIs the horizontal velocity, v, of the UUV_yIs the vertical velocity of the UUV and,V_r is the lower limit of the forward speed,is the upper limit of the forward speed.

4. The UUV dynamic obstacle avoidance method based on the speed obstacle method according to claim 1 or 2, wherein the fourth step is to establish that the UUV reaches the obstacleThe shortest time function of the edge; obtaining the minimum collision time t of the UUV and all the obstacles_col(v_r) The specific process comprises the following steps:

after the barrier is expanded, the barrier has the largest effect range so as toIs a barrier of radiusThe formed speed obstacle is VO_i(ii) a When the velocity v of UUV movement_r∈VO_iTime of impact τ expressed in relative velocity v_roObstacle to reachThe shortest time of the edge τ, τ satisfies the formula:

wherein,to representWhen there are a plurality of solutions in equation (8), the minimum time solution is taken as v_roThe time of the collision of (a),

UUV and obstacleThe conditions for collision are:

wherein, λ (v)_ro)＝{tv_ro|t>0 is the relative velocity direction v from the origin along UUV and obstacle_roThe ray of (a);

when in useWhen the temperature of the water is higher than the set temperature,UUV with relative speed v to obstacle_roDuring navigation, UUV and obstacleNo collision occurs;

when in useWhen, v_r∈VO_iUUV with relative velocity with obstacle as v_roDuring navigation, UUV and obstacleThe closest collision distance at which a collision occurs is:

at a relative velocity v_roDuring navigation, UUV and obstacleThe time of collision is:

UUV with speed v when several obstacles are distributed in the environment_rThe minimum collision time with all barriers is t when navigating_col(v_r) Namely: t is t_col(v_r)＝min(t_col(v_ro,O_i))。

5. The UUV dynamic obstacle avoidance method based on the velocity obstacle method as claimed in claim 4, wherein in the fifth step, comprehensive velocity risk VR (v) of the UUV is utilized_r) And minimum collision time t of UUV with all obstacles_col(v_r) The process of determining the optimized target path function of the UUV is as follows:

finding out collision avoidance optimal speed v of UUV relative target point on speed space^*：

v^*＝arg min(J_d(v_r)),X_r→X_G,v_r∈Ω_r (11)

Wherein, J_d(v_r) Is a target path function of the UUV; x_rIs the position coordinate, X, of UUV_GAs the position coordinates of the target point, in order to make the UUV sail towards the target point, the speed of the UUV relative to the target point:

the collision avoidance speed is determined by two factors of safety and approaching targets; reducing collision risk and improving safety, and simultaneously approaching the target position quickly, so the optimization objective function of the collision avoidance decision is as follows:

the optimized objective function of the collision avoidance decision consists of three parts, namely collision risk, target speed deviation and collision time; wherein ω is_p,ω_v,ω_tAre all weight coefficients; omega_p+ω_v＝1。