CN113485388A - AUV local obstacle avoidance method based on collision detection model and artificial potential field method - Google Patents

AUV local obstacle avoidance method based on collision detection model and artificial potential field method Download PDF

Info

Publication number
CN113485388A
CN113485388A CN202110850059.XA CN202110850059A CN113485388A CN 113485388 A CN113485388 A CN 113485388A CN 202110850059 A CN202110850059 A CN 202110850059A CN 113485388 A CN113485388 A CN 113485388A
Authority
CN
China
Prior art keywords
auv
path
point
local
axis
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110850059.XA
Other languages
Chinese (zh)
Other versions
CN113485388B (en
Inventor
郝琨
赵家乐
王贝贝
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tianjin Chengjian University
Original Assignee
Tianjin Chengjian University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tianjin Chengjian University filed Critical Tianjin Chengjian University
Priority to CN202110850059.XA priority Critical patent/CN113485388B/en
Publication of CN113485388A publication Critical patent/CN113485388A/en
Application granted granted Critical
Publication of CN113485388B publication Critical patent/CN113485388B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/04—Control of altitude or depth
    • G05D1/06—Rate of change of altitude or depth
    • G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

An AUV local obstacle avoidance method based on a collision detection model and an artificial potential field method comprises the steps of screening key path points in a global path by using the collision detection model; and taking the key path point as a local sub-target point and carrying out local obstacle avoidance under the guidance of an artificial potential field method. When the AUV does not enter the repulsive force field of the obstacle, the AUV normally runs along the global path, and when the AUV enters the repulsive force field of the obstacle, the AUV takes the current path point as the starting point and the first key path point which does not pass through as the local sub-target point, and the AUV is switched to the manual potential field method to carry out local obstacle avoidance operation until the AUV reaches the local sub-target point. The method not only can enable the AUV to effectively avoid various dynamic and static obstacles, but also can effectively avoid the phenomenon of AUV energy consumption sudden increase caused by obstacle avoidance behavior, so that the AUV can drive along the global path as far as possible to ensure the quality of the planned path and improve the real-time performance of AUV obstacle avoidance.

Description

AUV local obstacle avoidance method based on collision detection model and artificial potential field method
Technical Field
The invention belongs to the field of AUV local obstacle avoidance, and particularly relates to an AUV local obstacle avoidance method based on a collision detection model and an artificial potential field method.
Background
In recent years, with the continuous development of science and technology, the utilization and cognition of the ocean are continuously increasing. As an important tool for performing Underwater tasks, Autonomous Underwater Vehicles (AUV) have high flexibility and autonomy. Therefore, the AUV is widely applied to various tasks such as marine resource exploration, marine environment monitoring, underwater military operation and the like. Due to the complexity, uncertainty and unknowns of marine environments, it is far from sufficient to consider global path planning in static environments. The large number of static obstacles, dynamic obstacles and sudden obstacles in the ocean pose a great threat to the global path planned in advance by the AUV. Therefore, it is necessary to take a measure to avoid these static obstacles, dynamic obstacles and sudden obstacles.
The traditional obstacle avoidance method adopts a residual path re-planning strategy. That is, when the AUV predicts that a collision with an obstacle is about to occur, the AUV will take the current position as a starting point, the predicted collision point as a static obstacle, and the end point is unchanged, and replan a path around the collision point. However, such obstacle avoidance needs to spend a lot of time to re-plan a path, so that the real-time performance is not strong, and the method is difficult to adapt to a marine environment with complex terrain and many dynamic obstacles. The obstacle avoidance method should consider real-time first. On the basis of the traditional obstacle avoidance mode, a plurality of scholars propose different obstacle avoidance strategies. For example, schkelton et al classify the types of collisions as frontal collision side, side collision, chase collision, and the like. And (4) aiming at different collision modes, obstacle avoidance strategies such as local path re-planning, waiting, steering, reversing and the like are adopted. The obstacle avoidance strategies have high real-time performance. However, such collision strategies may not consider all collision types completely, and there may be situations where the collision type is not considered completely, which may result in a potential collision risk. Furthermore, such collision strategies are based on the possibility of predicting the movement path of dynamic obstacles. For irregularly moving dynamic obstacles, this type of collision strategy will fail completely. Some scholars adopt a mode of combining a global path planning algorithm and a local path planning algorithm to carry out local obstacle avoidance. For example, Liang et al use a combination of genetic and a-algorithms to perform local obstacle avoidance. And generating a global path by using a genetic algorithm, and switching to an A-algorithm to generate a subsequent path for obstacle avoidance when the AUV encounters a dynamic obstacle. However, this type of method also has certain drawbacks: first, the algorithm switching can only be performed by a global path planning algorithm to a local path planning algorithm in a single direction. Although this operation can avoid dynamic obstacles, the quality of the planned path (including path length, path smoothness, path safety, and AUV energy consumption) is greatly reduced. Second, for irregularly moving dynamic obstacles, this operation may not be able to work.
The AUV obstacle avoidance strategy requires that the AUV drives along a global path as far as possible on the premise of avoiding static obstacles, dynamic obstacles and sudden obstacles so as to ensure the quality of the planned path and improve the real-time performance of AUV obstacle avoidance.
Disclosure of Invention
In order to solve the above problems, the present invention provides an AUV local obstacle avoidance method based on a collision detection model and an artificial potential field method. Screening key path points in the global path by using a collision detection model; and taking the key path point as a local sub-target point and carrying out local obstacle avoidance under the guidance of an artificial potential field method.
In order to achieve the purpose, the invention is realized by adopting the following technical scheme:
an AUV local obstacle avoidance method based on a collision detection model and an artificial potential field method comprises the following steps in sequence:
the method comprises the following steps: establishing a collision detection model under a three-dimensional environment according to the global path, and detecting whether a path section formed by any two path points collides with the three-dimensional terrain; taking the coordinate with the maximum distance between the three coordinate components of the two path points as path detection, starting from the initial coordinate point, increasing the coordinate value of a specific step length each time, and gradually probing towards the target coordinate point in sequence; if the coordinate component of a certain path point on the z axis is tried out and is not higher than the height value of the three-dimensional terrain corresponding to the path point, outputting a connecting line of the two path points to collide with the three-dimensional terrain; if the coordinate components of the path points on the z axis from beginning to end are all higher than the height value of the three-dimensional terrain corresponding to the path points, outputting a connecting line of the two path points not to collide with the three-dimensional terrain;
step two: screening key path points in the global path by using a collision detection model under the condition that the global path is known; the critical path point refers to an indispensable core path node for supporting a global path. This means that a high quality feasible path can be constructed by only connecting the critical path points in sequence in the global path, and some common path nodes other than the critical path points only serve as filling paths.
Step three: using the key path points as local sub-target points and carrying out local obstacle avoidance under the guidance of an artificial potential field method; the design idea is to screen out the key path points in the known global path through a collision detection model in a three-dimensional environment. When the AUV does not enter the repulsive force field of the obstacle, the AUV normally travels along the global path. When the AUV enters a repulsive force field of an obstacle, the AUV takes the current path point as a starting point, the first key path point which does not pass through is taken as a local sub-target point, and the AUV is switched to an artificial potential field method to carry out local obstacle avoidance operation until the AUV reaches the local sub-target point. And when the AUV reaches the local sub-target point, the AUV continues to normally run along the global path.
In the above technical solution, the specific process of the collision detection model in the three-dimensional environment in the step one is as follows:
suppose that the coordinates of two path points are respectively Pi(xi,yi,zi)，Pi+1(xi+1,yi+1,zi+1) Then the linear parameter equation of the connection line of the two path points can be obtained as
Figure BDA0003182070760000021
Figure BDA0003182070760000031
The formulas (1) to (2) are calculation processes of linear parameter equations of the connection line of the two path points. Where t is a parameter variable, xi,yi,ziRespectively, a path point piCoordinate components in the x, y, z axes, xi+1,yi+1,zi+1Respectively, a path point pi+1Coordinate components on the x-axis, y-axis and z-axis, a, b and c are intermediate variables. Then x, y, z can all be represented by the parameter variable t, i.e.:
Figure BDA0003182070760000032
let dz be | zi+1-zi|，dy＝|yi+1-yi|，dx＝|xi+1-xiL. If dz is equal to max { dx, dy, dz }, then the z coordinate is used for probing. If dy is equal to max { dx, dy, dz }, then the y coordinate is used for probing. If dx is equal to max { dx, dy, dz }, the x coordinate is used for probing. Where max { dx, dy, dz } represents the maximum of dx, dy, dz.
Assuming that dx is maximum, i.e. x coordinate is used for detection, the following procedure is followed:
1.1 let x1 be min { x ═ xi+1，xi}，x2＝max{xi+1，xi}. Where min { x }i+1，xiDenotes xi+1，xiMinimum value of, max { xi+1，xiDenotes xi+1，xiMaximum value of (2).
1.2 if x1< x2, then x1 is x1+ c1 and go to step 1.3, otherwise the heuristic process ends and the output two-way point connection does not collide with the three-dimensional terrain. Where c1 is the detection step size and is a fixed constant.
1.3 solving y1 and z1 corresponding to x1 by using formula (3) can obtain corresponding coordinates (x1, y1, z 1). If z1> high (x1, y1), step 1.2 is entered. If z1 is not more than high (x1, y1), the connecting line of the two path points is output to collide with the three-dimensional terrain. Wherein high (x1, y1) represents the height value of the three-dimensional terrain corresponding to (x1, y 1).
Assuming that dy is the maximum, i.e. the y coordinate is used for detection, the following process is performed:
1.4 let y1 be min { y ═ yi+1，yi}，y2＝max{yi+1，yi}. Where min { y }i+1，yiDenotes yi+1，yiMinimum value of, max { y }i+1，yiDenotes yi+1，yiMaximum value of (2).
1.5 if y1< y2, then y1 is y1+ c1 and go to step 1.6, otherwise the heuristic is finished and the output two-way point connection does not collide with the three-dimensional terrain. Where c1 is the detection step size and is a fixed constant.
1.6 solving x1 and z1 corresponding to y1 by using formula (3) can obtain corresponding coordinates (x1, y1, z 1). If z1> high (x1, y1), proceed to step 1.5. If z1 is not more than high (x1, y1), the connecting line of the two path points is output to collide with the three-dimensional terrain. Wherein high (x1, y1) represents the height value of the three-dimensional terrain corresponding to (x1, y 1).
Assuming that dz is maximum, i.e. using z coordinate for detection, the following procedure is followed:
1.7 let z1 be min { z ═i+1，zi}，z2＝max{zi+1，zi}. Where min { z }i+1，ziDenotes zi+1，ziMinimum value of, max { z }i+1，ziDenotes zi+1，ziMaximum value of (2).
1.8 if z1< z2, then z1 is z1+ c1 and step 1.9, otherwise the heuristic is finished and the output two-way point connection does not collide with the three-dimensional terrain. Where c1 is the detection step size and is a fixed constant.
1.9 solving x1 and y1 corresponding to z1 by using formula (3) can obtain corresponding coordinates (x1, y1, z 1). If z1> high (x1, y1), proceed to step 1.8. If z1 is not more than high (x1, y1), the connecting line of the two path points is output to collide with the three-dimensional terrain. Wherein high (x1, y1) represents the height value of the three-dimensional terrain corresponding to (x1, y 1).
As can be seen from the flow of the collision detection model, the essence of the collision detection model is that starting from the start coordinate point, the c1 coordinate value is increased each time, and the model is gradually explored toward the destination coordinate point. If a certain trial finds that z1 is not more than high (x1, y1), the connecting line of the two path points is output to collide with the three-dimensional terrain. If z1> high (x1, y1) is maintained all the way through z1, the output two-way point connection does not collide with the three-dimensional terrain.
In the above technical solution, the specific process of screening the key path points in the global path by using the collision detection model in the step two is as follows:
assuming that a global path contains m path points, key _ path _ point is a collection of stored critical path points. Point _ i represents the ith path point in the global path, and point _ j represents the jth path point in the global path. The key path point obtaining process of the global path is as follows:
2.1 let i equal 1 and j equal 2.
2.2, judging whether the linear path section formed by the point _ i and the point _ j collides with the three-dimensional terrain or not through the collision detection model. If the path segment collides with the three-dimensional terrain, the step 2.3 is carried out. If the path segment does not collide with the three-dimensional terrain, step 2.4 is performed.
2.3 store point _ i in key _ path _ point, and then let i equal j-1. And (5) transferring to the step 2.5.
2.4 let temp j + 1. And (5) transferring to the step 2.5. Where temp acts as an intermediate variable.
2.5 determine whether point _ j is the endpoint. If the point _ j is the end point, the point _ i is first stored into the key _ path _ point, and then the point _ j is stored into the key _ path _ point, and the step 2.6 is carried out. If point _ j is not the end point, let j equal temp, go to step 2.2.
2.6 the key _ path _ point is output after the searching of the key path point is finished, and the process is finished.
In the above technical solution, the specific process of using the key path point as the local sub-target point and performing local obstacle avoidance under the guidance of the artificial potential field method in the third step is as follows:
3.1 judging whether the current position of the AUV is positioned in a repulsive force field of the dynamic obstacle. If the AUV is not within the repulsive field of the dynamic barrier, step 3.2 is entered. If the AUV is in the repulsive field of the dynamic barrier, proceed to step 3.3.
3.2AUV continues to travel along the global path to the next path point. And (5) transferring to the step 3.5.
3.3 the AUV switches to the artificial potential field method by taking the current path point as a starting point and the first key path point which does not pass through as a local sub-target point, and then drives to the next path point. And (5) transferring to the step 3.4.
And 3.4, judging whether the AUV runs to the local sub-target point. If the AUV is driven to the local sub-target point, the step 3.5 is carried out. If the AUV has not traveled to the local sub-target point, step 3.3 is entered.
And 3.5, judging whether the current position of the AUV is an end point. And if the current position of the AUV is not the end point, the step 3.1 is carried out. If the current position of the AUV is the terminal, the process is ended, and the AUV is output to successfully avoid the dynamic barrier and arrive at the terminal.
In the above technical solution, the specific implementation process of the artificial potential field method in step 3.3 is as follows:
3.3.1 determining whether the AUV reaches the local subtarget point. And if the AUV does not reach the local sub-target point, the step 3.3.2 is carried out. And if the AUV reaches the local sub-target point, ending the process and outputting that the AUV reaches the local sub-target point.
3.3.2 judging whether the current position of the AUV falls into the local minimum value. And if the current position of the AUV falls into the local minimum value, jumping the AUV out of the local minimum value by adopting a random point method, and then turning to the step 3.3.1. And if the current position of the AUV does not fall into the local minimum value, the step 3.3.3 is carried out.
3.3.3 calculating the attraction force of the AUV caused by the local sub-target points, calculating the repulsion force of the AUV caused by the dynamic obstacle, and calculating the resultant force of the AUV. And (5) transferring to the step 3.3.4. Wherein: the calculation formula of the gravity generated by the local sub-target points of the AUV is as follows:
|Fatt|＝k1*dis(p,g)w1 (4)
wherein, FattThe gravity generated by local sub-target points is applied to the AUV. I FattAnd | is the size of the attraction force generated by the local sub-target point on the AUV. FattAre vectors. FattIs of size | FattAnd FattIs pointed by the AUV current position to the position of the local child target point. K1 is a gravity gain coefficient and is a fixed constant. p denotes the AUV current position, g denotes the position of the local child target point, and dis (p, g) denotes the distance between the AUV current position and the position of the local child target point. w1 is a distance weight coefficient and is a fixed constant.
The calculation formula of the repulsive force generated by the AUV under the dynamic obstacle is as follows:
Figure BDA0003182070760000051
wherein, FrepRepulsive forces generated by dynamic obstacles are applied to the AUV. I FrepAnd | is the magnitude of the repulsive force generated by the AUV subjected to the dynamic obstacle. FrepAre vectors. FrepIs of size | FrepAnd FrepIs pointed to the AUV current position by the dynamic barrier current position. K2 is a repulsive gain factor and is a fixed constant. p represents the current position of the AUV, g represents the position of the local child target point, o represents the current position of the dynamic obstacle, dis (p, g) represents the distance between the current position of the AUV and the position of the local child target point, and dis (p, o) represents the distance between the current position of the AUV and the current position of the dynamic obstacle. w2 and w3 are both distance weighting coefficients and are fixed constants. r is the repulsive field radius of the dynamic barrier and is a fixed constant.
The calculation formula of the resultant force to which the AUV is subjected is as follows:
Figure BDA0003182070760000061
wherein, FsumIs the resultant force experienced by the AUV. FsumIs the vector sum of the attractive force experienced by the AUV and all the repulsive forces experienced by the AUV. FattThe gravity generated by local sub-target points is applied to the AUV. FrepRepulsive forces generated by dynamic obstacles are applied to the AUV. n is the number of dynamic obstacles in the three-dimensional environment, and i represents the number of dynamic obstacles.
3.3.4 the AUV moves to the next path point according to the set step length in the direction of the resultant force. And (6) transferring to the step 3.3.1.
In the above technical solution, the specific process of defining the local minimum value and detecting the local minimum value in step 3.3.2 is as follows:
the local minimum value is a phenomenon that the AUV repeatedly vibrates in a certain small range area under the action of resultant force, so that the AUV cannot reach or delays to reach a local sub-target point. The criterion of the local minimum is as follows: and taking the current position of the AUV as a center of a circle and r1 as a radius to make a circle. If the number of waypoints within the circle that the AUV has passed reaches some threshold ε 2, it may be determined that the AUV falls into a local minimum. Otherwise, it may be determined that the AUV is not stuck in the local minimum. Assuming that the current position of the AUV is the kth waypoint in the path, i ═ n represents the nth waypoint in the path, and count is the number of waypoints that the AUV has passed through within the circle. Then the local minimum is determined as follows:
3.3.2.1 let i equal to 1 and count equal to 0.
3.3.2.2 if the distance between the ith waypoint and the kth waypoint is less than or equal to r1, then proceed to step 3.3.2.3. If the distance from the ith waypoint to the kth waypoint is greater than r1, proceed to step 3.3.2.4.
3.3.2.3 let count be count + 1. If the value of count reaches the threshold value epsilon 2, the flow ends, and the current position of the AUV is output to be trapped in a local minimum value. If the count value does not reach the threshold ε 2, the process proceeds to step 3.3.2.4.
3.3.2.4 let i be i + 1. If i > k, the flow ends and the current position of the AUV is output without falling into the local minimum value. If i is less than or equal to m, proceed to step 3.3.2.2.
In the above technical solution, the specific process of making the AUV jump out of the local minimum value by using the random point method in step 3.3.2.2 is as follows:
3.3.2.5 a non-obstacle coordinate point is randomly selected in the three-dimensional environment to serve as a local sub-target point, and the original local sub-target point is abandoned temporarily.
3.3.2.6, because the local child target point has changed, the direction of the resultant force will also change. The AUV moves to the next waypoint in the new direction of the resultant force in the set step.
3.3.2.7, it is determined whether the current position of the AUV falls within the local minimum. If the AUV current position is still at the local minimum, then the process proceeds to step 3.3.2.5. If the current position of the AUV is not at the local minimum value, the process is ended, the AUV is output to jump out the local minimum value, and meanwhile, the first un-passed key path point is taken as a local sub-target point of the current AUV.
In the above technical solution, the specific process of moving the AUV to the next route point according to the predetermined step length in the direction of the resultant force in step 3.3.4 is as follows:
3.3.4.1 calculating the gravity F of AUVattThe gravitational components on the x-axis, y-axis, and z-axis.
Figure BDA0003182070760000071
Figure BDA0003182070760000072
Where α 1, β 1, γ 1 are the three directional angles of the vector pg. cos α 1, cos β 1, and cos γ 1 are directional cosines of the vector pg. Fattx、Fatty、FattzAre respectively FattThe gravitational components on the x-axis, y-axis, and z-axis. px, py, and pz are coordinate components of the current position p of the AUV on the x-axis, the y-axis, and the z-axis, respectively. And gx, gy and gz are coordinate components of the position g of the local sub-target point on an x axis, a y axis and a z axis respectively.
3.3.4.2 calculating the repulsive force F to which AUV is subjectedrepThe respective repulsive force components on the x-axis, y-axis, and z-axis.
Figure BDA0003182070760000073
Figure BDA0003182070760000074
Where α 2, β 2, γ 2 are the three directional angles of the vector op. cos α 2, cos β 2, cos γ 2 are the directional cosines of the vector op. Frepx、Frepy、FrepzAre respectively FrepThe respective repulsive force components on the x-axis, y-axis, and z-axis. px, py and pz are the x-axis, y-axis and z of the current position p of AUV respectivelyCoordinate components on the axis. ox, oy and oz are coordinate components of the current position o of the dynamic obstacle on an x axis, a y axis and a z axis respectively.
3.3.4.3 calculating the resultant force F received by AUVsumThe resultant force components on the x-axis, y-axis, and z-axis.
Figure BDA0003182070760000081
Wherein, Fsumx、Fsumy、FsumzAre respectively FsumThe resultant force components on the x-axis, y-axis, and z-axis. n is the number of dynamic obstacles in the three-dimensional environment, and i represents the number of dynamic obstacles.
3.3.4.4 calculate the coordinates of the next waypoint of the AUV.
Figure BDA0003182070760000082
Figure BDA0003182070760000083
Wherein alpha 3, beta 3 and gamma 3 are vectors FsumThree directional angles of (a). cos alpha 3, cos beta 3, cos gamma 3 are vectors FsumDirection cosine of (c). p1(px1, py1, pz1) is the coordinate of the next waypoint of the AUV. p (px, py, pz) is the coordinates of the current waypoint of the AUV. l is the step size of the AUV and is typically a fixed constant.
In the above technical solution, the specific process of determining whether the AUV is driven to the local subtarget point in step 3.4 is as follows:
when the distance between the current position of the AUV and the local sub-target point is smaller than a certain threshold value, the AUV can be judged to reach the local sub-target point. Let the coordinates of the current position of the AUV be p (px, py, pz). The coordinates of the position of the local child target point are g (gx, gy, gz). The decision formula is as follows:
(gx-px)2+(gy-py)2+(gz-pz)2≤ε1 (14)
where ε 1 is a decision threshold and is typically a fixed constant. px, py, and pz are coordinate components of the current position p of the AUV on the x-axis, the y-axis, and the z-axis, respectively. And gx, gy and gz are coordinate components of the position g of the local sub-target point on an x axis, a y axis and a z axis respectively.
The AUV local obstacle avoidance method based on the collision detection model and the artificial potential field method has the following beneficial effects: firstly, the method utilizes the key path point as a local sub-target point and carries out local obstacle avoidance under the guidance of an artificial potential field method. The AUV can avoid static obstacles, sudden obstacles, dynamic obstacles moving regularly and dynamic obstacles moving irregularly. The obstacle avoidance method has a good obstacle avoidance effect in a complex underwater environment; secondly, the method screens key path points in the global path by using a collision detection model and takes the key path points as guide points, so that the AUV can be flexibly switched between the global path and a local obstacle avoidance method, thereby not only ensuring that the AUV can successfully avoid obstacles, but also ensuring the quality of the AUV driving path (including path length, path smoothness, path safety and AUV energy consumption); thirdly, because the next path point in the resultant force direction is calculated by adopting the space vector, compared with the traditional method of classifying and discussing the resultant force angle and then calculating the next path point in the resultant force direction, the method has the advantages that the real-time performance of obstacle avoidance is greatly improved; fourthly, the invention provides a collision detection model for detecting whether the AUV collides with the underwater three-dimensional terrain, and the model can prevent the AUV from colliding with the underwater three-dimensional terrain while avoiding obstacles; fifth, the present invention improves upon the traditional artificial potential field method. The improvement work comprises adding a distance factor into a repulsion formula, redefining a local minimum value point and providing a method for jumping out of the local minimum value point by the AUV. These improvements can significantly reduce the likelihood of the AUV getting into deadlock.
Drawings
Fig. 1 is a schematic diagram of a working principle of the AUV local obstacle avoidance method based on the collision detection model and the artificial potential field method provided by the present invention.
Detailed Description
The following describes in detail an AUV local obstacle avoidance method based on a collision detection model and an artificial potential field method, which is provided by the present invention, with reference to the accompanying drawings and specific embodiments.
As shown in fig. 1, the AUV local obstacle avoidance method based on the collision detection model and the artificial potential field method provided by the present invention includes the following steps in sequence:
the method comprises the following steps: establishing a collision detection model in a three-dimensional environment according to the global path; the collision detection model is a method for detecting whether a path segment formed by any two path points collides with a three-dimensional terrain. Taking the coordinate with the maximum distance between the three coordinate components of the two path points as path detection, starting from the initial coordinate point, increasing the coordinate value of a specific step length each time, and gradually probing towards the target coordinate point in sequence; if the coordinate component of a certain path point on the z axis is tried out and is not higher than the height value of the three-dimensional terrain corresponding to the path point, outputting a connecting line of the two path points to collide with the three-dimensional terrain; if the coordinate components of the path points on the z axis from beginning to end are all higher than the height value of the three-dimensional terrain corresponding to the path points, outputting a connecting line of the two path points not to collide with the three-dimensional terrain;
the specific process of the collision detection model in the three-dimensional environment in the step one is as follows:
suppose that the coordinates of two path points are respectively Pi(xi,yi,zi)，Pi+1(xi+1,yi+1,zi+1) Then the linear parameter equation of the connection line of the two path points can be obtained as
Figure BDA0003182070760000101
Figure BDA0003182070760000102
The formulas (1) to (2) are calculation processes of linear parameter equations of the connection line of the two path points. Where t is a parameter variable, xi,yi,ziRespectively, a path point piIn the x, y, and z axesCoordinate component, xi+1,yi+1,zi+1Respectively, a path point pi+1Coordinate components on the x-axis, y-axis and z-axis, a, b and c are intermediate variables. Then x, y, z can all be represented by the parameter variable t, i.e.:
Figure BDA0003182070760000103
let dz be | zi+1-zi|，dy＝|yi+1-yi|，dx＝|xi+1-xiL. If dz is equal to max { dx, dy, dz }, then the z coordinate is used for probing. If dy is equal to max { dx, dy, dz }, then the y coordinate is used for probing. If dx is equal to max { dx, dy, dz }, the x coordinate is used for probing. Where max { dx, dy, dz } represents the maximum of dx, dy, dz.
Assuming that dx is maximum, i.e. x coordinate is used for detection, the following procedure is followed:
1.1 let x1 be min { x ═ xi+1，xi}，x2＝max{xi+1，xi}. Where min { x }i+1，xiDenotes xi+1，xiMinimum value of, max { xi+1，xiDenotes xi+1，xiMaximum value of (2).
1.2 if x1< x2, then x1 is x1+ c1 and go to step 1.3, otherwise the heuristic process ends and the output two-way point connection does not collide with the three-dimensional terrain. Where c1 is the detection step size and is a fixed constant.
1.3 solving y1 and z1 corresponding to x1 by using formula (3) can obtain corresponding coordinates (x1, y1, z 1). If z1> high (x1, y1), step 1.2 is entered. If z1 is not more than high (x1, y1), the connecting line of the two path points is output to collide with the three-dimensional terrain. Wherein high (x1, y1) represents the height value of the three-dimensional terrain corresponding to (x1, y 1).
Assuming that dy is the maximum, i.e. the y coordinate is used for detection, the following process is performed:
1.4 let y1 be min { y ═ yi+1，yi}，y2＝max{yi+1，yi}. Where min { y }i+1，yiDenotes yi+1，yiMedium minimum value, maxyi+1，yiDenotes yi+1，yiMaximum value of (2).
1.5 if y1< y2, then y1 is y1+ c1 and go to step 1.6, otherwise the heuristic is finished and the output two-way point connection does not collide with the three-dimensional terrain. Where c1 is the detection step size and is a fixed constant.
1.6 solving x1 and z1 corresponding to y1 by using formula (3) can obtain corresponding coordinates (x1, y1, z 1). If z1> high (x1, y1), proceed to step 1.5. If z1 is not more than high (x1, y1), the connecting line of the two path points is output to collide with the three-dimensional terrain. Wherein high (x1, y1) represents the height value of the three-dimensional terrain corresponding to (x1, y 1).
Assuming that dz is maximum, i.e. using z coordinate for detection, the following procedure is followed:
1.7 let z1 be min { z ═i+1，zi}，z2＝max{zi+1，zi}. Where min { z }i+1，ziDenotes zi+1，ziMinimum value of, max { z }i+1，ziDenotes zi+1，ziMaximum value of (2).
1.8 if z1< z2, then z1 is z1+ c1 and step 1.9, otherwise the heuristic is finished and the output two-way point connection does not collide with the three-dimensional terrain. Where c1 is the detection step size and is a fixed constant.
1.9 solving x1 and y1 corresponding to z1 by using formula (3) can obtain corresponding coordinates (x1, y1, z 1). If z1> high (x1, y1), proceed to step 1.8. If z1 is not more than high (x1, y1), the connecting line of the two path points is output to collide with the three-dimensional terrain. Wherein high (x1, y1) represents the height value of the three-dimensional terrain corresponding to (x1, y 1).
As can be seen from the flow of the collision detection model, the essence of the collision detection model is that starting from the start coordinate point, the c1 coordinate value is increased each time, and the model is gradually explored toward the destination coordinate point. If a certain trial finds that z1 is not more than high (x1, y1), the connecting line of the two path points is output to collide with the three-dimensional terrain. If z1> high (x1, y1) is maintained all the way through z1, the output two-way point connection does not collide with the three-dimensional terrain.
Step two: screening key path points in the global path by using a collision detection model under the condition that the global path is known; the critical path point refers to an indispensable core path node for supporting a global path. This means that a high quality feasible path can be constructed by only connecting the critical path points in sequence in the global path, and some common path nodes other than the critical path points only serve as filling paths.
The specific process of screening the key path points in the global path by using the collision detection model in the step two is as follows:
assuming that a global path contains m path points, key _ path _ point is a collection of stored critical path points. Point _ i represents the ith path point in the global path, and point _ j represents the jth path point in the global path. The key path point obtaining process of the global path is as follows:
2.1 let i equal 1 and j equal 2.
2.2, judging whether the linear path section formed by the point _ i and the point _ j collides with the three-dimensional terrain or not through the collision detection model. If the path segment collides with the three-dimensional terrain, the step 2.3 is carried out. If the path segment does not collide with the three-dimensional terrain, step 2.4 is performed.
2.3 store point _ i in key _ path _ point, and then let i equal j-1. And (5) transferring to the step 2.5.
2.4 let temp j + 1. And (5) transferring to the step 2.5. Where temp acts as an intermediate variable.
2.5 determine whether point _ j is the endpoint. If the point _ j is the end point, the point _ i is first stored into the key _ path _ point, and then the point _ j is stored into the key _ path _ point, and the step 2.6 is carried out. If point _ j is not the end point, let j equal temp, go to step 2.2.
2.6 the key _ path _ point is output after the searching of the key path point is finished, and the process is finished.
Step three: using the key path points as local sub-target points and carrying out local obstacle avoidance under the guidance of an artificial potential field method; the design idea is to screen out the key path points in the known global path through a collision detection model in a three-dimensional environment. When the AUV does not enter the repulsive force field of the obstacle, the AUV normally travels along the global path. When the AUV enters a repulsive force field of an obstacle, the AUV takes the current path point as a starting point, the first key path point which does not pass through is taken as a local sub-target point, and the AUV is switched to an artificial potential field method to carry out local obstacle avoidance operation until the AUV reaches the local sub-target point. And when the AUV reaches the local sub-target point, the AUV continues to normally run along the global path.
The specific process of using the key path point as a local sub-target point and performing local obstacle avoidance under the guidance of the artificial potential field method in the third step is as follows:
3.1 judging whether the current position of the AUV is positioned in a repulsive force field of the dynamic obstacle. If the AUV is not within the repulsive field of the dynamic barrier, step 3.2 is entered. If the AUV is in the repulsive field of the dynamic barrier, proceed to step 3.3.
3.2AUV continues to travel along the global path to the next path point. And (5) transferring to the step 3.5.
3.3 the AUV switches to the artificial potential field method by taking the current path point as a starting point and the first key path point which does not pass through as a local sub-target point, and then drives to the next path point. And (5) transferring to the step 3.4.
And 3.4, judging whether the AUV runs to the local sub-target point. If the AUV is driven to the local sub-target point, the step 3.5 is carried out. If the AUV has not traveled to the local sub-target point, step 3.3 is entered.
And 3.5, judging whether the current position of the AUV is an end point. And if the current position of the AUV is not the end point, the step 3.1 is carried out. If the current position of the AUV is the terminal, the process is ended, and the AUV is output to successfully avoid the dynamic barrier and arrive at the terminal.
In the above implementation process, the specific implementation process of the artificial potential field method in step 3.3 is as follows:
3.3.1 determining whether the AUV reaches the local subtarget point. And if the AUV does not reach the local sub-target point, the step 3.3.2 is carried out. And if the AUV reaches the local sub-target point, ending the process and outputting that the AUV reaches the local sub-target point.
3.3.2 judging whether the current position of the AUV falls into the local minimum value. And if the current position of the AUV falls into the local minimum value, jumping the AUV out of the local minimum value by adopting a random point method, and then turning to the step 3.3.1. And if the current position of the AUV does not fall into the local minimum value, the step 3.3.3 is carried out.
3.3.3 calculating the attraction force of the AUV caused by the local sub-target points, calculating the repulsion force of the AUV caused by the dynamic obstacle, and calculating the resultant force of the AUV. And (5) transferring to the step 3.3.4. Wherein:
the calculation formula of the gravity generated by the local sub-target points of the AUV is as follows:
|Fatt|＝k1*dis(p,g)w1 (4)
wherein, FattGravitation generated by local sub-target points is applied to the AUV; i FattI is the size of the gravity of the AUV caused by the local sub-target points; fattIs a vector; fattIs of size | FattAnd FattThe direction of the AUV points to the position of the local sub-target point from the current position of the AUV; k1 is a gravity gain coefficient and is a fixed constant; p represents the current position of the AUV, g represents the position of the local child target point, and dis (p, g) represents the distance between the current position of the AUV and the position of the local child target point; w1 is a distance weight coefficient and is a fixed constant;
the calculation formula of the repulsive force generated by the AUV under the dynamic obstacle is as follows:
Figure BDA0003182070760000131
wherein, FrepRepulsive forces generated by dynamic obstacles for the AUV; i FrepL is the magnitude of repulsive force generated by the AUV under the dynamic barrier; frepIs a vector; frepIs of size | FrepAnd FrepThe direction of the AUV is pointed to by the current position of the dynamic barrier; k2 is a repulsive gain factor and is a fixed constant; p represents the current position of the AUV, g represents the position of the local child target point, o represents the current position of the dynamic obstacle, dis (p, g) represents the distance between the current position of the AUV and the position of the local child target point, and dis (p, o) represents the distance between the current position of the AUV and the current position of the dynamic obstacle; w2 and w3 are both distance weightsThe coefficient is a fixed constant; r is the repulsive force field radius of the dynamic barrier and is a fixed constant;
the calculation formula of the resultant force to which the AUV is subjected is as follows:
Figure BDA0003182070760000132
wherein, FsumIs the resultant force experienced by the AUV; fsumThe direction of (1) is the vector sum of the attractive force borne by the AUV and all repulsive forces borne by the AUV; fattGravitation generated by local sub-target points is applied to the AUV; frepRepulsive forces generated by dynamic obstacles for the AUV; n is the number of dynamic obstacles in the three-dimensional environment, and i represents the number of the dynamic obstacles;
3.3.4 the AUV moves to the next path point according to the set step length in the direction of the resultant force. And (6) transferring to the step 3.3.1.
In the above implementation process, the specific process of defining the local minimum value and detecting the local minimum value in step 3.3.2 is as follows:
the local minimum value is a phenomenon that the AUV repeatedly vibrates in a certain small range area under the action of resultant force, so that the AUV cannot reach or delays to reach a local sub-target point. The criterion of the local minimum is as follows: and taking the current position of the AUV as a center of a circle and r1 as a radius to make a circle. If the number of waypoints within the circle that the AUV has passed reaches some threshold ε 2, it may be determined that the AUV falls into a local minimum. Otherwise, it may be determined that the AUV is not stuck in the local minimum. Assuming that the current position of the AUV is the kth waypoint in the path, i ═ n represents the nth waypoint in the path, and count is the number of waypoints that the AUV has passed through within the circle. Then the local minimum is determined as follows:
3.3.2.1 let i equal to 1 and count equal to 0.
3.3.2.2 if the distance between the ith waypoint and the kth waypoint is less than or equal to r1, then proceed to step 3.3.2.3. If the distance from the ith waypoint to the kth waypoint is greater than r1, proceed to step 3.3.2.4.
3.3.2.3 let count be count + 1. If the value of count reaches the threshold value epsilon 2, the flow ends, and the current position of the AUV is output to be trapped in a local minimum value. If the count value does not reach the threshold ε 2, the process proceeds to step 3.3.2.4.
3.3.2.4 let i be i + 1. If i > k, the flow ends and the current position of the AUV is output without falling into the local minimum value. If i is less than or equal to m, proceed to step 3.3.2.2.
In the above implementation process, the specific process of making the AUV jump out of the local minimum value by using the random point method in step 3.3.2 is as follows:
3.3.2.5 a non-obstacle coordinate point is randomly selected in the three-dimensional environment to serve as a local sub-target point, and the original local sub-target point is abandoned temporarily.
3.3.2.6, because the local child target point has changed, the direction of the resultant force will also change. The AUV moves to the next waypoint in the new direction of the resultant force in the set step.
3.3.2.7, it is determined whether the current position of the AUV falls within the local minimum. If the AUV current position is still at the local minimum, then the process proceeds to step 3.3.2.5. If the current position of the AUV is not at the local minimum value, the process is ended, and the AUV is output to jump out the local minimum value and simultaneously the first un-passed key path point is taken as the local sub-target point of the current AUV.
In the above implementation process, the present invention calculates the next route point in the resultant force direction by using the space vector, and the specific process of moving the AUV to the next route point in the resultant force direction according to the predetermined step length in step 3.3.4 is as follows:
3.3.4.1 calculating the gravity F of AUVattThe gravitational components on the x-axis, y-axis, and z-axis.
Figure BDA0003182070760000141
Figure BDA0003182070760000142
Wherein alpha 1, beta 1 and gamma 1 are three direction angles of vector pg. cos α 1, cos β 1, and cos γ 1 are directional cosines of the vector pg. Fattx、Fatty、FattzAre respectively FattThe gravitational components on the x-axis, y-axis, and z-axis. px, py, and pz are coordinate components of the current position p of the AUV on the x-axis, the y-axis, and the z-axis, respectively. And gx, gy and gz are coordinate components of the position g of the local sub-target point on an x axis, a y axis and a z axis respectively.
3.3.4.2 calculating the repulsive force F to which AUV is subjectedrepThe respective repulsive force components on the x-axis, y-axis, and z-axis.
Figure BDA0003182070760000151
Figure BDA0003182070760000152
Where α 2, β 2, γ 2 are the three directional angles of the vector op. cos α 2, cos β 2, cos γ 2 are the directional cosines of the vector op. Frepx、Frepy、FrepzAre respectively FrepThe respective repulsive force components on the x-axis, y-axis, and z-axis. px, py, and pz are coordinate components of the current position p of the AUV on the x-axis, the y-axis, and the z-axis, respectively. ox, oy and oz are coordinate components of the current position o of the dynamic obstacle on an x axis, a y axis and a z axis respectively.
3.3.4.3 calculating the resultant force F received by AUVsumThe resultant force components on the x-axis, y-axis, and z-axis.
Figure BDA0003182070760000153
Wherein, Fsumx、Fsumy、FsumzAre respectively FsumThe resultant force components on the x-axis, y-axis, and z-axis. n is the number of dynamic obstacles in the three-dimensional environment, and i represents the number of dynamic obstacles.
3.3.4.4 calculate the coordinates of the next waypoint of the AUV.
Figure BDA0003182070760000161
Figure BDA0003182070760000162
Wherein alpha 3, beta 3 and gamma 3 are vectors FsumThree directional angles of (a). cos alpha 3, cos beta 3, cos gamma 3 are vectors FsumDirection cosine of (c). p1(px1, py1, pz1) is the coordinate of the next waypoint of the AUV. p (px, py, pz) is the coordinates of the current waypoint of the AUV. l is the step size of the AUV and is typically a fixed constant.
In the above implementation process, the specific process of determining whether the AUV travels to the local subtarget point in step 3.4 is as follows:
when the distance between the current position of the AUV and the local sub-target point is smaller than a certain threshold value, the AUV can be judged to reach the local sub-target point. Let the coordinates of the current position of the AUV be p (px, py, pz). The coordinates of the position of the local child target point are g (gx, gy, gz). The decision formula is as follows:
(gx-px)2+(gy-py)2+(gz-pz)2≤ε1 (14)
where ε 1 is a decision threshold and is typically a fixed constant. px, py, and pz are coordinate components of the current position p of the AUV on the x-axis, the y-axis, and the z-axis, respectively. And gx, gy and gz are coordinate components of the position g of the local sub-target point on an x axis, a y axis and a z axis respectively.

Claims (9)

1. An AUV local obstacle avoidance method based on a collision detection model and an artificial potential field method is characterized by comprising the following steps: the AUV local obstacle avoidance method based on the collision detection model and the artificial potential field method comprises the following steps in sequence:
the method comprises the following steps: establishing a collision detection model under a three-dimensional environment according to the global path, and detecting whether a path section formed by any two path points collides with the three-dimensional terrain; taking the coordinate with the maximum distance between the three coordinate components of the two path points as path detection, starting from the initial coordinate point, increasing the coordinate value of a specific step length each time, and gradually probing towards the target coordinate point in sequence; if the coordinate component of a certain path point on the z axis is tried out and is not higher than the height value of the three-dimensional terrain corresponding to the path point, outputting a connecting line of the two path points to collide with the three-dimensional terrain; if the coordinate components of the path points on the z axis from beginning to end are all higher than the height value of the three-dimensional terrain corresponding to the path points, outputting a connecting line of the two path points not to collide with the three-dimensional terrain;
step two: screening key path points in the global path by using a collision detection model under the condition that the global path is known;
step three: using the key path points as local sub-target points and carrying out local obstacle avoidance under the guidance of an artificial potential field method; when the AUV does not enter the repulsive force field of the obstacle, the AUV normally runs along the global path; when the AUV enters a repulsive force field of an obstacle, the AUV takes the current path point as a starting point, the first key path point which does not pass through is taken as a local sub-target point, and the AUV is switched to an artificial potential field method to carry out local obstacle avoidance operation until the AUV reaches the local sub-target point; and when the AUV reaches the local sub-target point, the AUV continues to normally run along the global path.
2. The AUV local obstacle avoidance method based on the collision detection model and the artificial potential field method according to claim 1, wherein the collision detection model under the three-dimensional environment in the step one comprises the specific processes of:
suppose that the coordinates of two path points are respectively Pi(xi,yi,zi)，Pi+1(xi+1,yi+1,zi+1) Then, the linear parameter equation of the connection line of the two path points is obtained as follows:
Figure FDA0003182070750000011
Figure FDA0003182070750000012
the formula (1) to the formula (2) are two path pointsCalculating a linear parameter equation of the connecting line; where t is a parameter variable, xi,yi,ziRespectively, a path point piCoordinate components in the x, y, z axes, xi+1,yi+1,zi+1Respectively, a path point pi+1Coordinate components on an x axis, a y axis and a z axis, wherein a, b and c are intermediate variables; then x, y, z can all be represented by the parameter variable t, i.e.:
Figure FDA0003182070750000013
let dz be | zi+1-zi|，dy＝|yi+1-yi|，dx＝|xi+1-xiL, |; if dz is equal to max { dx, dy, dz }, then the z coordinate is used for detection; if dy is equal to max { dx, dy, dz }, then the y coordinate is used for detection; if dx is equal to max { dx, dy, dz }, then the x coordinate is used for detection; where max { dx, dy, dz } represents the maximum of dx, dy, dz;
assuming that dx is maximum, i.e. x coordinate is used for detection, the following procedure is followed:
1.1 let x1 be min { x ═ xi+1，xi}，x2＝max{xi+1，xi}; where min { x }i+1，xiDenotes xi+1，xiMinimum value of, max { xi+1，xiDenotes xi+1，xiMaximum value of (1);
1.2 if x1 is less than x2, then x1 is equal to x1+ c1 and the process goes to step 1.3, otherwise, the heuristic process is finished, and the connecting line of the two output path points does not collide with the three-dimensional terrain; wherein c1 is a detection step length and is a fixed constant;
1.3 solving y1 and z1 corresponding to x1 by using formula (3), and obtaining corresponding coordinates (x1, y1, z 1); if z1> high (x1, y1), go to step 1.2; if z1 is not more than high (x1, y1), outputting the connection line of the two path points to collide with the three-dimensional terrain; wherein high (x1, y1) represents the height value of the three-dimensional terrain corresponding to (x1, y 1);
assuming that dy is the maximum, i.e. the y coordinate is used for detection, the following process is performed:
1.4 let y1 be min { y ═ yi+1，yi}，y2＝max{yi+1，yi}; where min { y }i+1，yiDenotes yi+1，yiMinimum value of, max { y }i+1，yiDenotes yi+1，yiMaximum value of (1);
1.5 if y1< y2, then y1 is equal to y1+ c1 and go to step 1.6, otherwise, the heuristic process is finished, and the connecting line of the two output path points does not collide with the three-dimensional terrain; wherein c1 is a detection step length and is a fixed constant;
1.6 solving x1 and z1 corresponding to y1 by using formula (3), and obtaining corresponding coordinates (x1, y1, z 1); if z1> high (x1, y1), go to step 1.5; if z1 is not more than high (x1, y1), outputting the connection line of the two path points to collide with the three-dimensional terrain; wherein high (x1, y1) represents the height value of the three-dimensional terrain corresponding to (x1, y 1);
assuming that dz is maximum, i.e. using z coordinate for detection, the following procedure is followed:
1.7 let z1 be min { z ═i+1，zi}，z2＝max{zi+1，ziWhere min { z }i+1，ziDenotes zi+1，ziMinimum value of, max { z }i+1，ziDenotes zi+1，ziMaximum value of (1);
1.8 if z1< z2, then z1 is z1+ c1 and go to step 1.9, otherwise, the heuristic process is ended, and the connecting line of the two output path points does not collide with the three-dimensional terrain; wherein c1 is a detection step length and is a fixed constant;
1.9 solving x1 and y1 corresponding to z1 by using formula (3), and obtaining corresponding coordinates (x1, y1, z 1); if z1> high (x1, y1), go to step 1.8; if z1 is not more than high (x1, y1), outputting the connection line of the two path points to collide with the three-dimensional terrain; wherein high (x1, y1) represents the height value of the three-dimensional terrain corresponding to (x1, y 1).
3. The AUV local obstacle avoidance method based on the collision detection model and the artificial potential field method according to claim 1, wherein the specific process of screening the key path points in the global path by using the collision detection model in the second step is as follows:
assuming that a certain global path contains m path points, and the key _ path _ point is a set of stored key path points; point _ i represents the ith path point in the global path, and point _ j represents the jth path point in the global path; the key path point obtaining process of the global path is as follows:
2.1 let i equal to 1 and j equal to 2;
2.2 judging whether a linear path section formed by the point _ i and the point _ j collides with the three-dimensional terrain or not through a collision detection model; if the path section collides with the three-dimensional terrain, the step 2.3 is carried out; if the path section does not collide with the three-dimensional terrain, the step 2.4 is carried out;
2.3 store point _ i into key _ path _ point, and then let i equal to j-1; turning to step 2.5;
2.4 let temp ═ j + 1; turning to step 2.5; where temp acts as an intermediate variable;
2.5 judging whether point _ j is an end point; if the point _ j is the end point, the point _ i is firstly stored into the key _ path _ point, then the point _ j is stored into the key _ path _ point, and the step 2.6 is carried out; if point _ j is not the end point, let j equal temp, go to step 2.2;
2.6 the key _ path _ point is output after the searching of the key path point is finished, and the process is finished.
4. The AUV local obstacle avoidance method based on the collision detection model and the artificial potential field method according to claim 1, wherein the specific process of using the key path points as local sub-target points and performing local obstacle avoidance under the guidance of the artificial potential field method in the third step is as follows:
3.1 judging whether the current position of the AUV is positioned in a repulsive force field of the dynamic barrier; if the AUV is not in the repulsive field of the dynamic barrier, then step 3.2 is carried out; if the AUV is in the repulsive field of the dynamic barrier, then step 3.3 is carried out;
3.2 the AUV continues to travel to the next path point along the global path; turning to step 3.5;
3.3 the AUV switches to an artificial potential field method by taking the current path point as a starting point and the first key path point which does not pass through as a local sub-target point, and then drives to the next path point; turning to step 3.4;
3.4 judging whether the AUV runs to a local sub-target point; if the AUV runs to the local sub-target point, the step 3.5 is carried out; if the AUV does not drive to the local sub-target point, the step 3.3 is carried out;
3.5 judging whether the current position of the AUV is the end point, if not, turning to the step 3.1, if so, ending the process, and outputting the AUV to successfully avoid the dynamic barrier and arrive at the end point.
5. The AUV local obstacle avoidance method based on the collision detection model and the artificial potential field method according to claim 4, wherein the specific implementation process of the artificial potential field method in the step 3.3 is as follows:
3.3.1 judging whether the AUV reaches the local sub-target point, if the AUV does not reach the local sub-target point, turning to the step 3.3.2; if the AUV reaches the local sub-target point, the process is ended, and the AUV is output to reach the local sub-target point;
3.3.2 judging whether the current position of the AUV falls into a local minimum value; if the current position of the AUV falls into the local minimum value, the AUV jumps out of the local minimum value by adopting a random point method, and then the step 3.3.1 is carried out; if the current position of the AUV does not fall into the local minimum value, the step 3.3.3 is carried out;
3.3.3 calculating the attraction force generated by the local sub-target points on the AUV, calculating the repulsion force generated by the dynamic obstacle on the AUV, and calculating the resultant force on the AUV; turning to step 3.3.4; wherein:
the calculation formula of the gravity generated by the local sub-target points of the AUV is as follows:
|Fatt|＝k1*dis(p,g)w1 (4)
wherein, FattGravitation generated by local sub-target points is applied to the AUV; i FattI is the size of the gravity of the AUV caused by the local sub-target points; fattIs a vector; fattIs of size | FattAnd FattIs directed from the current position of the AUV to the position of the local sub-target point(ii) a K1 is a gravity gain coefficient and is a fixed constant; p represents the current position of the AUV, g represents the position of the local child target point, and dis (p, g) represents the distance between the current position of the AUV and the position of the local child target point; w1 is a distance weight coefficient and is a fixed constant;
the calculation formula of the repulsive force generated by the AUV under the dynamic obstacle is as follows:
Figure FDA0003182070750000041
wherein, FrepRepulsive forces generated by dynamic obstacles for the AUV; i FrepL is the magnitude of repulsive force generated by the AUV under the dynamic barrier; frepIs a vector; frepIs of size | FrepAnd FrepThe direction of the AUV is pointed to by the current position of the dynamic barrier; k2 is a repulsive gain factor and is a fixed constant; p represents the current position of the AUV, g represents the position of the local child target point, o represents the current position of the dynamic obstacle, dis (p, g) represents the distance between the current position of the AUV and the position of the local child target point, and dis (p, o) represents the distance between the current position of the AUV and the current position of the dynamic obstacle; w2 and w3 are distance weight coefficients and are fixed constants; r is the repulsive force field radius of the dynamic barrier and is a fixed constant;
the calculation formula of the resultant force to which the AUV is subjected is as follows:
Figure FDA0003182070750000042
wherein, FsumIs the resultant force experienced by the AUV; fsumThe direction of (1) is the vector sum of the attractive force borne by the AUV and all repulsive forces borne by the AUV; fattGravitation generated by local sub-target points is applied to the AUV; frepRepulsive forces generated by dynamic obstacles for the AUV; n is the number of dynamic obstacles in the three-dimensional environment, and i represents the number of the dynamic obstacles;
3.3.4 the AUV moves to the next path point according to the set step length in the direction of resultant force; and (6) transferring to the step 3.3.1.
6. The AUV local obstacle avoidance method based on the collision detection model and the artificial potential field method according to claim 5, wherein the specific process of local minimum detection in the step 3.3.2 is as follows:
assuming that the current position of the AUV is the kth path point in the path, wherein i equals n and represents the nth path point in the path, and count is the number of path points which the AUV in the circle has already passed through; then the local minimum is determined as follows:
3.3.2.1 let i equal to 1 and count equal to 0;
3.3.2.2 if the distance between the ith path point and the kth path point is less than or equal to r1, go to step 3.3.2.3; if the distance from the ith path point to the kth path point is greater than r1, go to step 3.3.2.4;
3.3.2.3 let count be count + 1; if the value of the count reaches the threshold value epsilon 2, the process is ended, and the current position of the AUV is output to be trapped in a local minimum value; if the count value does not reach the threshold value ε 2, go to step 3.3.2.4;
3.3.2.4 let i ═ i + 1; if i is larger than k, ending the process, and outputting the current position of the AUV not falling into the local minimum value; if i is less than or equal to m, proceed to step 3.3.2.2.
7. The AUV local obstacle avoidance method based on the collision detection model and the artificial potential field method according to claim 5, wherein the specific process of making the AUV jump out of the local minimum value by adopting the random point method in the step 3.3.2 is as follows:
3.3.2.5 randomly selecting a non-obstacle coordinate point in a three-dimensional environment to serve as a local sub-target point, and abandoning the original local sub-target point temporarily;
3.3.2.6 because the local child target point has changed, the direction of the resultant force will also change; the AUV moves to the next path point along the new resultant force direction according to the set step length;
3.3.2.7 judging whether the current position of AUV falls into local minimum value; if the AUV current position is still at the local minimum, go to step 3.3.2.5; if the current position of the AUV is not at the local minimum value, the process is ended, the AUV is output to jump out the local minimum value, and meanwhile, the first un-passed key path point is taken as a local sub-target point of the current AUV.
8. The AUV local obstacle avoidance method based on the collision detection model and the artificial potential field method according to claim 5, wherein the specific process of moving the AUV to the next waypoint according to the set step length in the direction of the resultant force in step 3.3.4 is as follows:
3.3.4.1 calculating the gravity F of AUVattGravitational components on the x-axis, y-axis, and z-axis;
Figure FDA0003182070750000051
Figure FDA0003182070750000061
wherein alpha 1, beta 1 and gamma 1 are three direction angles of the vector pg; cos alpha 1, cos beta 1 and cos gamma 1 are directional cosines of the vector pg; fattx、Fatty、FattzAre respectively FattGravitational components on the x-axis, y-axis, and z-axis; px, py and pz are coordinate components of the current position p of the AUV on an x axis, a y axis and a z axis respectively; gx, gy and gz are coordinate components of the position g of the local sub-target point on an x axis, a y axis and a z axis respectively;
3.3.4.2 calculating the repulsive force F to which AUV is subjectedrepThe respective repulsive force components on the x-axis, y-axis, and z-axis;
Figure FDA0003182070750000062
Figure FDA0003182070750000063
wherein alpha 2, beta 2 and gamma 2 are three direction angles of the vector op; cos alpha 2, cos beta 2 and cos gamma 2 are directional cosines of the vector op; frepx、Frepy、FrepzAre respectively FrepThe respective repulsive force components on the x-axis, y-axis, and z-axis; px, py and pz are coordinate components of the current position p of the AUV on an x axis, a y axis and a z axis respectively; ox, oy and oz are coordinate components of the current position o of the dynamic barrier on an x axis, a y axis and a z axis respectively;
3.3.4.3 calculating the resultant force F received by AUVsumThe resultant force components on the x-axis, y-axis, and z-axis;
Figure FDA0003182070750000064
wherein, Fsumx、Fsumy、FsumzAre respectively FsumThe resultant force components on the x-axis, y-axis, and z-axis; n is the number of dynamic obstacles in the three-dimensional environment, and i represents the number of the dynamic obstacles;
3.3.4.4 calculating the coordinates of the next path point of AUV;
Figure FDA0003182070750000071
Figure FDA0003182070750000072
wherein alpha 3, beta 3 and gamma 3 are vectors FsumThree direction angles of (d); cos alpha 3, cos beta 3, cos gamma 3 are vectors FsumDirection cosine of (d); p1(px1, py1, pz1) is the coordinate of the next waypoint of the AUV; p (px, py, pz) is the coordinate of the current waypoint of the AUV; l is the step size of the AUV and is a fixed constant.
9. The AUV local obstacle avoidance method based on the collision detection model and the artificial potential field method according to claim 4, wherein the specific process of judging whether the AUV drives to the local sub-target point in the step 3.4 is as follows:
when the distance between the current position of the AUV and the local sub-target point is smaller than a certain threshold value, judging that the AUV reaches the local sub-target point; assuming that the coordinates of the current position of the AUV are p (px, py, pz) and the coordinates of the position of the local child target point are g (gx, gy, gz), the decision formula is as follows:
(gx-px)2+(gy-py)2+(gz-pz)2≤ε1 (14)
wherein epsilon 1 is a decision threshold and is a fixed constant; px, py and pz are coordinate components of the current position p of the AUV on an x axis, a y axis and a z axis respectively; and gx, gy and gz are coordinate components of the position g of the local sub-target point on an x axis, a y axis and a z axis respectively.
CN202110850059.XA 2021-07-27 2021-07-27 AUV local obstacle avoidance method based on collision detection model and artificial potential field method Active CN113485388B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110850059.XA CN113485388B (en) 2021-07-27 2021-07-27 AUV local obstacle avoidance method based on collision detection model and artificial potential field method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110850059.XA CN113485388B (en) 2021-07-27 2021-07-27 AUV local obstacle avoidance method based on collision detection model and artificial potential field method

Publications (2)

Publication Number Publication Date
CN113485388A true CN113485388A (en) 2021-10-08
CN113485388B CN113485388B (en) 2022-07-29

Family

ID=77944002

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110850059.XA Active CN113485388B (en) 2021-07-27 2021-07-27 AUV local obstacle avoidance method based on collision detection model and artificial potential field method

Country Status (1)

Country Link
CN (1) CN113485388B (en)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108274465A (en) * 2018-01-10 2018-07-13 上海理工大学 Based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method
CN111897328A (en) * 2020-07-17 2020-11-06 武汉理工大学 Path planning method, device and equipment based on improved artificial potential field method
CN112344943A (en) * 2020-11-20 2021-02-09 安徽工程大学 Intelligent vehicle path planning method for improving artificial potential field algorithm
CN112577491A (en) * 2020-12-14 2021-03-30 上海应用技术大学 Robot path planning method based on improved artificial potential field method
CN112859871A (en) * 2021-01-21 2021-05-28 江苏大学 Dynamic obstacle avoidance control method based on model prediction
CN112859866A (en) * 2021-01-18 2021-05-28 西安电子科技大学 Robot rolling path planning method, system, storage medium, equipment and application
US20210272466A1 (en) * 2020-02-28 2021-09-02 Pablo Air Co., Ltd. Method of avoiding collision of unmanned aerial vehicle

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108274465A (en) * 2018-01-10 2018-07-13 上海理工大学 Based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method
US20210272466A1 (en) * 2020-02-28 2021-09-02 Pablo Air Co., Ltd. Method of avoiding collision of unmanned aerial vehicle
CN111897328A (en) * 2020-07-17 2020-11-06 武汉理工大学 Path planning method, device and equipment based on improved artificial potential field method
CN112344943A (en) * 2020-11-20 2021-02-09 安徽工程大学 Intelligent vehicle path planning method for improving artificial potential field algorithm
CN112577491A (en) * 2020-12-14 2021-03-30 上海应用技术大学 Robot path planning method based on improved artificial potential field method
CN112859866A (en) * 2021-01-18 2021-05-28 西安电子科技大学 Robot rolling path planning method, system, storage medium, equipment and application
CN112859871A (en) * 2021-01-21 2021-05-28 江苏大学 Dynamic obstacle avoidance control method based on model prediction

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
BINGJIA JIA: "Path Planning Based on the Method of Integrating Adjacent Obstacles", 《THE 31TH CHINESE CONTROL AND DECISION CONFERENCE (2019 CCDC)》 *
LINGYU LI: "A path planning strategy unified with a COLREGS collision avoidance function based on deep", 《APPLIED OCEAN RESEARCH》 *
YIMU JI: "Research on security of key algorithms in intelligent driving system", 《2018 IEEE THIRD INTERNATIONAL CONFERENCE ON DATA SCIENCE IN CYBERSPACE》 *
刘佳: "固定翼无人机在线航迹规划方法", 《计算机应用》 *
曹博: "改进人工势场法的冗余机械臂避障算法", 《哈尔滨工业大学学报》 *
熊超: "基于碰撞锥改进人工势场的无人机避障路径规划", 《计算机工程》 *

Also Published As

Publication number Publication date
CN113485388B (en) 2022-07-29

Similar Documents

Publication Publication Date Title
CN111399506B (en) Global-local hybrid unmanned ship path planning method based on dynamic constraint
JP6854549B2 (en) AUV action planning and motion control methods based on reinforcement learning
CN108705532A (en) A kind of mechanical arm obstacle-avoiding route planning method, equipment and storage device
CN105629974A (en) Robot path planning method and system based on improved artificial potential field method
US20200174483A1 (en) Obstacle avoiding method in state-time space, recording medium storing program for executing same, and computer program stored in recording medium for executing same
US20140148989A1 (en) Autonomous Moving Device and Control Method Thereof
US20200174482A1 (en) Online bidirectional trajectory planning method in state-time space, recording medium storing program for executing same, and computer program stored in recording medium for executing same
Kästner et al. Connecting deep-reinforcement-learning-based obstacle avoidance with conventional global planners using waypoint generators
CN111930141A (en) Three-dimensional path visual tracking method for underwater robot
Chen et al. Tracking with UAV using tangent-plus-Lyapunov vector field guidance
CN111830978A (en) Under-actuated unmanned ship obstacle avoidance path planning and control method and system
Kim et al. Improvement of dynamic window approach using reinforcement learning in dynamic environments
CN113391633A (en) Urban environment-oriented mobile robot fusion path planning method
Kobayashi et al. Local path planning method based on virtual manipulators and dynamic window approach for a wheeled mobile robot
CN113485388B (en) AUV local obstacle avoidance method based on collision detection model and artificial potential field method
Zhang et al. Research on Path Planning Method of Unmanned Boat Based on Improved Artificial Potential Field Method
KR100578942B1 (en) A Method and a System for Avoiding Collision of an Unmanned Aerial Vehicle Using Proportional Navigation Guidance Law
CN110471425B (en) Improved fuzzy artificial potential field unmanned ship obstacle avoidance method based on sonar
CN112799414B (en) AUV relaxation trajectory planning method
CN115657683B (en) Unmanned cable-free submersible real-time obstacle avoidance method capable of being used for inspection operation task
CN111427368A (en) Improved multi-target collision-prevention driving method for unmanned intelligent vehicle
Clark Randomized motion planning for groups of nonholonomic robots
Cheng et al. Trajectory optimization for ship navigation safety using genetic annealing algorithm
Abichandani et al. Multi-vehicle path coordination under communication constraints
CN115586774A (en) Mobile robot obstacle avoidance method based on improved dynamic window method

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant