CN113359762B - Dynamic planning method for unmanned surface vehicle - Google Patents
Dynamic planning method for unmanned surface vehicle Download PDFInfo
- Publication number
- CN113359762B CN113359762B CN202110751457.6A CN202110751457A CN113359762B CN 113359762 B CN113359762 B CN 113359762B CN 202110751457 A CN202110751457 A CN 202110751457A CN 113359762 B CN113359762 B CN 113359762B
- Authority
- CN
- China
- Prior art keywords
- ship
- dynamic
- unmanned surface
- surface vehicle
- force
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to 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
The invention relates to a dynamic planning method for an unmanned surface vehicle, which comprises the following steps: considering that most of dynamic obstacles encountered by unmanned surface vehicles on the sea are ships, an elliptical model is established according to the shape characteristics of the ships; on the basis of building an ellipse model, analyzing the ellipse model, determining the influence range of the dynamic barrier of the ship, and building an ellipse repulsion force field in the influence range to provide repulsion force, so as to guide the unmanned surface vehicle to avoid the dynamic barrier; further considering the instability and unpredictability of the motion of the dynamic barriers of the ship, the unmanned surface vehicle needs to avoid the dynamic barriers in a short time, so that a method of changing the coefficient of repulsion is adopted, repulsion is increased, and the direction of resultant force is changed, so that the effect of quickly guiding the unmanned surface vehicle to avoid the dynamic barriers is achieved.
Description
Technical Field
The invention relates to a dynamic planning method for an unmanned surface vehicle, and belongs to the field of motion control and dynamic planning of marine unmanned vehicles.
Background
As demands such as ocean defense and the like are continuously expanded, all countries are building ocean unmanned comprehensive platforms, and water surface unmanned boats are comprehensively developed as novel unmanned equipment. Dynamic planning of surface unmanned vehicle motion maneuvers is not yet mature.
Commonly used dynamic programming methods include velocity repulsion field algorithms, dynamic window algorithms, visual algorithms, artificial potential field methods, and the like. Compared with other algorithms, the artificial potential field method has the advantages of short operation time, strong real-time performance, good adaptability and the like. However, the application of the traditional artificial potential field method in the fields of unmanned surface vehicle motion control and dynamic planning has the following problems:
(1) the problem of difficult modeling of dynamic barriers is solved, because most of the unmanned surface craft meet the dynamic barriers, the dynamic barriers are considered as particles by the traditional artificial potential field method, the influence range of repulsive force is circular, the influence range of the dynamic barriers of the ships is difficult to determine, and the planned path cannot be safely guaranteed;
(2) the method is characterized in that the problem of rapid obstacle avoidance is solved, a repulsion coefficient and a attraction coefficient of a traditional manual potential field method are fixed, the generated repulsion is increased along with the reduction of the distance between a water surface unmanned ship and a ship dynamic obstacle, the attraction is reduced along with the reduction of the distance between the water surface unmanned ship and a target point, the position of the water surface unmanned ship is changed constantly, meanwhile, the instability and unpredictability of the motion of the ship dynamic obstacle need to be considered, and the coefficient is fixed, so that rapid, reliable and real-time dynamic planning is difficult to realize.
The improved method provided by the patent of unmanned ship ocean dynamic obstacle avoidance control algorithm based on ellipse clustering-collision cone deduction has the following problems:
(1) the method carries out elliptical clustering on the dynamic ship, clusters the dynamic ship into dynamic elliptical barriers according to the size and the shape of the dynamic ship, but the shape of the elliptical barriers is not determined, and the planned path cannot be safely guaranteed;
(2) the invention needs to acquire the motion state of the dynamic barrier, and has the advantages of large amount of information, high calculation difficulty and poor real-time performance.
In summary, how to solve the application of the artificial potential field method in the dynamic environment of the unmanned surface vehicle becomes a difficult point to be solved urgently.
Disclosure of Invention
The invention aims to provide a dynamic planning method for an unmanned surface vehicle, which solves the problems of difficult dynamic obstacle modeling, rapid obstacle avoidance and the like when the traditional artificial potential field method is applied to the dynamic planning process of the unmanned surface vehicle.
The invention adopts the following technical scheme for solving the problems: designing a dynamic planning method of the unmanned surface vehicle, establishing an ellipse equivalent model for dynamic barriers of a ship, and setting an influence range meeting constraint conditions; and the repulsion coefficient is changed according to the relationship between the attraction force and the repulsion force, so that the repulsion force is increased, the resultant force is changed, the barrier is quickly avoided, and the efficient, reliable and real-time dynamic planning is realized in the next movement. The method specifically comprises the following steps:
step 1:
setting a gravitational coefficient mu and a repulsive coefficient eta in an artificial potential field method, and setting a semi-major axis a of the unmanned surface vehicleqSemi-minor axis bqFirst, firstStarting position qsAnd target point position qgSemi-major axis a of ship dynamic barrier modelpSemi-minor axis bpSemi-focal length cpLeft focal position cp1And right focal position cp2Semi-major axis A of influence range of dynamic barrier of shippAnd semi-minor axis BpThe following constraints need to be satisfied:
then entering step 2;
step 2:
judging whether the current position q of the unmanned surface vehicle reaches qgIf yes, ending, otherwise, entering step 3;
and step 3:
constructing a gravity function:
Fa(q)＝μ·d(q,qg) (2)
in the formula: fa(q) is gravitational force, d (q, q)g) Is a vector with modulo lengths q and qgIn the direction q points to qgGravitational force Fa(q) generating attraction force on the unmanned surface vehicle, and guiding the unmanned surface vehicle to go to a target point under the action of the attraction force;
r＝d(q,cp1)+d(q,cp2) (3)
in the formula: r is a vector and r is a vector obtained by dividing d (q, c) by the parallelogram rulep1) And d (q, c)p2) Are added to obtain d (q, c)p1) Is a vector with modulo lengths q and cp1In the direction of cp1Pointing to q, same, d (q, c)p2) Is also a vector with modulo lengths q and cp2In the direction of cp2Pointing to q;
constructing a repulsion function:
in the formula: fe(q) As a repulsive force, a repulsive force Fe(q) and r are in the same direction, and | r | is the module length of the vector r, and when the unmanned surface vessel is in the influence range of the dynamic barrier of the vessel, the repulsive force Fe(q) generating a repulsive force to the ship, keeping the ship away from the ship, and then entering the step 4;
and 4, step 4:
judgment of repulsive force Fe(q) if the value is 0, entering a step 6 if the value is 0, and otherwise, entering a step 5;
and 5:
at the moment, the water surface unmanned ship changes the repulsion coefficient eta in order to avoid the dynamic barrier of the ship as soon as possible within the influence range of the dynamic barrier of the ship, so that the repulsion is increased in the next step of movement, and the change formula of the repulsion coefficient eta is as follows:
then entering step 6;
step 6:
constructing a resultant force formula:
Ft(q)＝Fa(q)+Fe(q) (6)
in the formula: ft(q) is resultant force, wherein the resultant force direction is the next movement direction of the unmanned surface vehicle, and then the step 7 is carried out;
and 7:
and calculating the next motion, and then returning to the step 2 to circulate.
The invention has the following beneficial effects:
1. the method establishes an elliptic equivalent model of the dynamic barrier of the ship, determines the influence range of the dynamic barrier of the ship, constructs a repulsive force potential field in the influence range, generates repulsive force on the unmanned ship, ensures that the unmanned ship can smoothly avoid the dynamic barrier and meets the requirement of safe navigation;
2. the method adopts a variable repulsion coefficient method to change repulsion force and further change resultant force, thereby achieving the effect that the unmanned surface vehicle can quickly avoid dynamic obstacles of the ship and realizing efficient, reliable and real-time dynamic planning;
3. the method of the invention defines the shape of the dynamic elliptical barrier, so that the established model is accurate and the planned path is safe;
4. the method mainly utilizes the position information, has low acquisition difficulty, small calculated amount and good real-time performance, and can achieve the purpose of rapidly avoiding obstacles.
Drawings
FIG. 1 is a flow chart of a dynamic planning method for an unmanned surface vehicle;
FIG. 2 is an elliptical model of a vessel dynamic barrier;
FIG. 3 is a stress analysis diagram of the unmanned surface vehicle;
FIG. 4 is a dynamic planning diagram of the unmanned surface vehicle when t is 1 s;
FIG. 5 is a dynamic planning diagram of the unmanned surface vehicle when t is 14 s;
FIG. 6 is a dynamic planning diagram of the unmanned surface vehicle when t is 28 s;
FIG. 7 is a dynamic planning diagram of the unmanned surface vehicle when t is 47 s;
FIG. 8 is a dynamic planning diagram of the unmanned surface vehicle when t is 62 s;
FIG. 9 is a dynamic planning diagram of the unmanned surface vehicle when t is 68 s;
FIG. 10 is a dynamic planning diagram of the unmanned surface vehicle when t is 69 s;
FIG. 11 is a dynamic planning diagram of the unmanned surface vehicle when t is 81 s;
FIG. 12 is a dynamic planning diagram of the unmanned surface vehicle when t is 97 s;
fig. 13 is a dynamic planning diagram of the unmanned surface vehicle when t is 122 s.
Detailed Description
Fig. 1 is a flow chart of a dynamic planning method for the unmanned surface vehicle, which comprises the following steps:
step 1:
setting a gravitational coefficient mu and a repulsive coefficient eta in an artificial potential field method, and setting a semi-major axis a of the unmanned surface vehicleqSemi-minor axis bqInitial position qsAnd target point position qgSemi-major axis a of ship dynamic barrier modelpSemi-minor axis bpSemi-focal length cpLeft focal position cp1And right focal position cp2Semi-major axis A of influence range of dynamic barrier of shippAnd semi-minor axis BpThe following constraints need to be satisfied:
the ship dynamic barrier model and the influence range thereof are shown in FIG. 2, and then step 2 is carried out;
step 2:
judging whether the current position q of the unmanned surface vehicle reaches qgIf yes, ending, otherwise, entering step 3;
and step 3:
constructing a gravity function:
Fa(q)＝μ·d(q,qg) (2)
in the formula: fa(q) is gravitational force, d (q, q)g) Is a vector with modulo lengths q and qgIn the direction q points to qgGravitational force Fa(q) generating attraction force on the unmanned surface vehicle, and guiding the unmanned surface vehicle to go to a target point under the action of the attraction force;
r＝d(q,cp1)+d(q,cp2) (3)
in the formula: r is a vector and r is a vector obtained by dividing d (q, c) by the parallelogram rulep1) And d (q, c)p2) Are added to obtain d (q, c)p1) Is a vector with modulo lengths q and cp1In the direction of cp1Pointing to q, same, d (q, c)p2) Is also a vector with modulo lengths q and cp2In the direction of cp2Pointing to q.
Constructing a repulsion function:
in the formula: fe(q) is a repulsive force, repulsive force Fe(q) and r are in the same direction, and | r | is the module length of the vector r, and when the unmanned surface vessel is in the influence range of the dynamic barrier of the vessel, the repulsive force Fe(q) generating a repulsive force to the ship, keeping the ship away from the ship, and then entering the step 4;
and 4, step 4:
judgment of repulsive force Fe(q) if the value is 0, entering a step 6 if the value is 0, and otherwise, entering a step 5;
and 5:
at the moment, the water surface unmanned ship changes the repulsion coefficient eta in order to avoid the dynamic barrier of the ship as soon as possible within the influence range of the dynamic barrier of the ship, so that the repulsion is increased in the next step of movement, and the change formula of the repulsion coefficient eta is as follows:
then entering step 6;
step 6:
constructing a resultant force formula:
Ft(q)＝Fa(q)+Fe(q) (6)
in the formula: ft(q) is the resultant force, the direction of the resultant force is the next movement direction of the unmanned surface vehicle, as shown in fig. 3, q is the current position of the unmanned surface vehicle, and q is the current position of the unmanned surface vehiclegIs a target point, cp1And cp2Left and right foci of the elliptical model of the ship, Fa(q) is gravitational force, Fe(q) as a repulsive force, synthesizing a resultant force F using the parallelogram rulet(q) then proceeding to step 7;
and 7:
and calculating the next motion, and then returning to the step 2 to circulate.
The simulation results are shown in fig. 4-13, where the solid line is the unmanned boat path, the dotted line is the ship path, and the arrows indicate the respective directions of motion. When t is 1s, as shown in fig. 4, the unmanned boat starts towards the target point; when t is 14s, as shown in fig. 5, the unmanned ship enters the influence range of the ship ellipse model; fig. 6, 7, 8 and 9 show that the unmanned boat moves under the action of resultant force in the influence range of the dynamic barrier of the ship and rapidly leaves the influence range of the dynamic barrier of the ship; FIG. 10 shows that the unmanned surface vehicle continues to advance toward the target point after leaving the dynamic barrier influence range of the vessel; FIG. 11 shows that the unmanned surface vehicle enters the dynamic obstacle influence range of the ship again under the action of gravity and rapidly leaves the dynamic obstacle influence range of the ship again; fig. 12 is the drone heading toward the target point; fig. 13 shows the final arrival of the unmanned boat at the target point.
Claims (1)
1. A dynamic planning method for an unmanned surface vehicle solves the problems of difficult modeling of dynamic barriers of ships and rapid obstacle avoidance, and is characterized in that:
step 1:
setting a gravitational coefficient mu and a repulsive coefficient eta in an artificial potential field method, and setting a semi-major axis a of the unmanned surface vehicleqSemi-minor axis bqInitial position qsAnd target point position qgSemi-major axis a of ship dynamic barrier modelpSemi-minor axis bpSemi-focal length cpLeft focal position cp1And right focal position cp2Semi-major axis A of influence range of dynamic barrier of shippAnd semi-minor axis BpThe following constraints need to be satisfied:
then entering step 2;
step 2:
judging whether the current position q of the unmanned surface vehicle reaches qgIf yes, ending, otherwise, entering step 3;
and step 3:
constructing a gravity function:
Fa(q)＝μ·d(q,qg) (2)
in the formula: fa(q) is gravitational force, d (q, q)g) Is a vector with modulo lengths q and qgIn the direction q points to qgGravitational force Fa(q) generating attraction force on the unmanned surface vehicle, and guiding the unmanned surface vehicle to go to a target point under the action of the attraction force;
r＝d(q,cp1)d(q,cp2) (3)
in the formula:r is a vector and r is a vector obtained by dividing d (q, c) by the parallelogram rulep1) And d (q, c)p2) Are added to obtain d (q, c)p1) Is a vector with modulo lengths q and cp1In the direction of cp1Pointing to q, same, d (q, c)p2) Is also a vector with modulo lengths q and cp2In the direction of cp2Pointing to q;
constructing a repulsion function:
in the formula: fe(q) is a repulsive force, repulsive force Fe(q) and r are in the same direction, and | r | is the module length of the vector r, and when the unmanned surface vessel is in the influence range of the dynamic barrier of the vessel, the repulsive force Fe(q) generating a repulsive force to the ship, keeping the ship away from the ship, and then entering the step 4;
and 4, step 4:
judgment of repulsive force Fe(q) if the value is 0, entering a step 6 if the value is 0, and otherwise, entering a step 5;
and 5:
at the moment, the water surface unmanned ship changes the repulsion coefficient eta in order to avoid the dynamic barrier of the ship as soon as possible within the influence range of the dynamic barrier of the ship, so that the repulsion is increased in the next step of movement, and the change formula of the repulsion coefficient eta is as follows:
then entering step 6;
step 6:
constructing a resultant force formula:
Ft(q)＝Fa(q)+Fe(q) (6)
in the formula: ft(q) is resultant force, wherein the resultant force direction is the next movement direction of the unmanned surface vehicle, and then the step 7 is carried out;
and 7:
and calculating the next motion, and then returning to the step 2 to circulate.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110751457.6A CN113359762B (en) | 2021-07-02 | 2021-07-02 | Dynamic planning method for unmanned surface vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110751457.6A CN113359762B (en) | 2021-07-02 | 2021-07-02 | Dynamic planning method for unmanned surface vehicle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113359762A CN113359762A (en) | 2021-09-07 |
| CN113359762B true CN113359762B (en) | 2022-01-18 |
Family
ID=77537988
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110751457.6A Active CN113359762B (en) | 2021-07-02 | 2021-07-02 | Dynamic planning method for unmanned surface vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113359762B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114077255B (en) * | 2021-11-22 | 2023-07-18 | 江苏理工学院 | Intelligent vehicle road-finding method based on elliptical model artificial potential field method |
| CN115562291B (en) * | 2022-10-19 | 2023-12-12 | 哈尔滨理工大学 | Path planning method for improving potential field dynamic coefficient based on artificial potential field method |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105717923B (en) * | 2016-01-16 | 2019-02-19 | 上海大学 | The unmanned boat ocean dynamic obstacle avoidance control algolithm deduced based on oval cluster-collision cone |
| KR101948410B1 (en) * | 2017-02-15 | 2019-02-14 | 국방과학연구소 | Path Planning System of Unmanned Surface Vehicle for Autonomous Tracking of Underwater Acoustic Target |
| CN107608346A (en) * | 2017-08-30 | 2018-01-19 | 武汉理工大学 | Ship intelligent barrier avoiding method and system based on Artificial Potential Field |
| CN110134130A (en) * | 2019-06-14 | 2019-08-16 | 西交利物浦大学 | A kind of unmanned boat automatic obstacle avoiding method based on improvement angle potential field method |
| CN110850873B (en) * | 2019-10-31 | 2021-06-08 | 五邑大学 | Unmanned ship path planning method, device, equipment and storage medium |
| CN112379672B (en) * | 2020-11-24 | 2022-05-10 | 浙大宁波理工学院 | Intelligent unmanned ship path planning method based on improved artificial potential field |
-
2021
- 2021-07-02 CN CN202110751457.6A patent/CN113359762B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113359762A (en) | 2021-09-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113359762B (en) | Dynamic planning method for unmanned surface vehicle | |
| CN106444806B (en) | The drive lacking AUV three-dimensional track tracking and controlling method adjusted based on biological speed | |
| CN113189984B (en) | Unmanned ship path planning method based on improved artificial potential field method | |
| CN110906934B (en) | Unmanned ship obstacle avoidance method and system based on collision risk coefficient | |
| CN111123923B (en) | Unmanned ship local path dynamic optimization method | |
| CN112965471B (en) | Artificial potential field path planning method considering angular velocity constraint and improving repulsive field | |
| CN109141421A (en) | Expected path building method in the underwater dynamic target tracking of drive lacking AUV | |
| CN114089763B (en) | Multi-underwater robot formation and collision prevention control method for submarine optical cable laying | |
| CN109460058B (en) | Underwater butt joint transverse moving control method for tail-propelled low-speed underwater vehicle | |
| CN109916400B (en) | Unmanned ship obstacle avoidance method based on combination of gradient descent algorithm and VO method | |
| CN110262473B (en) | Unmanned ship automatic collision avoidance method based on improved Bi-RRT algorithm | |
| Jiang et al. | The application of PSO-AFSA method in parameter optimization for underactuated autonomous underwater vehicle control | |
| Taubert et al. | Model identification and controller parameter optimization for an autopilot design for autonomous underwater vehicles | |
| CN110471425B (en) | Improved fuzzy artificial potential field unmanned ship obstacle avoidance method based on sonar | |
| Guggilla et al. | CFD Investigation on the hydrodynamic characteristics of blended wing unmanned underwater gliders with emphasis on the control surfaces | |
| CN113110458B (en) | Unmanned ship virtual target tracking control system | |
| CN109613924B (en) | Tail propulsion underwater vehicle pitching docking control method considering docking inclination angle | |
| CN113359773A (en) | Unmanned ship navigation path decision method and system | |
| CN114167880A (en) | Time-optimal-based multi-underwater glider path planning system | |
| CN113970887B (en) | Multi-underwater glider path planning system based on optimal energy consumption | |
| Suresh et al. | Design and analysis of remotely amphibious drone | |
| CN112130562B (en) | Multi-rolling-window-based artificial potential field unmanned surface vessel obstacle avoidance method | |
| Wan et al. | Research on the Optimization of Underwater Vehicle Dynamic System | |
| CN114217521B (en) | Full-attitude motion control method of underwater robot based on propeller vector layout | |
| Liu et al. | Research on autonomous path planning of unmanned surface vehicle in ocean environment |
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 |