CN110262492B - Real-time collision avoidance and target tracking method for unmanned ship - Google Patents
Real-time collision avoidance and target tracking method for unmanned ship Download PDFInfo
- Publication number
- CN110262492B CN110262492B CN201910555673.6A CN201910555673A CN110262492B CN 110262492 B CN110262492 B CN 110262492B CN 201910555673 A CN201910555673 A CN 201910555673A CN 110262492 B CN110262492 B CN 110262492B
- Authority
- CN
- China
- Prior art keywords
- unmanned ship
- unmanned
- constraint
- ship
- model
- 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
- 238000000034 method Methods 0.000 title claims abstract description 36
- 230000001133 acceleration Effects 0.000 claims abstract description 40
- 238000005265 energy consumption Methods 0.000 claims abstract description 22
- 238000005457 optimization Methods 0.000 claims abstract description 6
- 230000004888 barrier function Effects 0.000 claims description 18
- 230000003068 static effect Effects 0.000 claims description 7
- 239000000126 substance Substances 0.000 claims description 7
- 230000003044 adaptive effect Effects 0.000 claims description 5
- 238000010586 diagram Methods 0.000 claims description 5
- 238000000926 separation method Methods 0.000 claims description 4
- 230000001105 regulatory effect Effects 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 description 4
- 238000002121 ultrasonic speckle velocimetry Methods 0.000 description 4
- 238000011161 development Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 102200110775 rs75909145 Human genes 0.000 description 1
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/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)
- Traffic Control Systems (AREA)
Abstract
A real-time collision avoidance and target tracking method for an unmanned ship aims at the characteristics that the motion characteristics of the unmanned ship, the unmanned ship path planning process and the unmanned ship need to comply with international maritime rule convention and can be interfered by wind, wave and current and the like, and the unmanned ship target tracking process and the requirement on speed smoothness, and a maritime rule, steering collision avoidance model, a self-adaptive tracking model, an unmanned ship kinematics model and an unmanned ship energy consumption model are constructed. Then, two working modes of navigation and target tracking are set for the unmanned ship. And setting corresponding middle end point conditions for the two working modes, adding various constraint conditions into the algorithm, and calculating to obtain the optimal path by using a least square and G2O graph optimization algorithm. The method can be applied to the field of path planning of the unmanned ship, and an optimal smooth track considering the navigation time, the collision risk, the international maritime rule convention, the energy consumption, the kinematic limit of the unmanned ship, the speed/acceleration limit and other constraint conditions can be obtained in real time.
Description
Technical Field
The invention relates to the field of unmanned ship path planning, in particular to a real-time collision avoidance and target tracking method for an unmanned ship
Background
With the rapid development of Unmanned systems and artificial intelligence technologies, Unmanned vehicles (USVs) are becoming more and more popular in military and civilian fields following Unmanned planes and Unmanned vehicles, and play an important role in various marine applications such as maritime search and rescue, maritime evidence collection, environmental monitoring, enemy reconnaissance and the like.
The path planning technology is one of core technologies in the field of unmanned surface vehicles, is a key step from manned to unmanned, is an intelligent key development direction of unmanned vehicles, and marks the autonomous navigation capability of the unmanned surface vehicles to a certain extent. Therefore, the research on the path planning technology of the unmanned ship can help the further development of the unmanned ship. The path planning of the unmanned surface vehicle refers to how to find a proper motion path from a given starting point to a terminal point in a working environment with obstacles, so that the unmanned surface vehicle can safely and seamlessly bypass all the obstacles in the motion process. Real-time collision avoidance and target tracking are more difficult points and are contents needing important research. The real-time collision avoidance and target tracking are guided by global path or tracked target information, the real-time position of the unmanned ship is determined through sensor information, and the distribution condition of obstacles in a local range is obtained; a motion scheme meeting certain evaluation criteria and constraints is found, the course and the navigation speed can be adjusted, and various marine obstacles can be avoided in a highly intelligent and self-adaptive manner.
Most of the existing path planning methods, such as a potential field method, a grid method, an A-algorithm, a genetic algorithm and the like, obtain an optimal path by means of planning coordinate points, time sequence information and motion constraint of an unmanned ship are not considered in the planning process, so that the planned path is over-ideal, and the situation that the path cannot be reached exists, namely, a controller cannot follow the path.
The TEB algorithm can plan the track of the unmanned ship in real time, and plan the speed and the acceleration of the unmanned ship when the unmanned ship executes tasks by comprehensively considering the motion model and the motion constraint of the unmanned ship so as to meet the requirements of smoothness, speed controllability and the like.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the invention overcomes the defects of the prior art, provides a real-time collision avoidance and target tracking method for an unmanned ship, aiming at unmanned ship navigation and target tracking scenes, a track containing information such as a path coordinate point sequence, a time sequence, and speed, angular speed, direction and the like reaching any path point can be planned in real time, the path can be ensured to be executed by a controller, the planned path conforms to the international maritime collision avoidance rule convention, and in the target tracking process, the unmanned ship can track at a smooth speed, so that the control difficulty of the controller is reduced.
The technical scheme adopted by the invention is as follows: a real-time collision avoidance and target tracking method for an unmanned ship comprises the following steps:
step S10, initializing a state B with a time sequence according to the starting point and the end point of the unmanned ship navigation track, the time interval and the number of the state sequences, wherein the state B is (Q, tau),
wherein: set of states Q ═ Qi}i＝1...nN is N, and the state set τ is { Δ T ═ Nj}j＝1...n-1,n∈N，qi＝(pi,βi)T＝(xi,yi,βi)T，pi＝(xi,yi) Is the current position coordinate, beta, of the unmanned ship in the mapiThe current steering angle of the unmanned ship is defined as east direction of 0 degrees, anticlockwise direction of positive, and delta TjIs the time resolution;
step S20, constructing a maritime affair rule model and a steering and collision avoidance model of the unmanned ship according to COLREGS;
the maritime rule model is as follows: according to COLREGS, ship collision avoidance experience data and the navigation range of a target ship in a marine environment, four collision situations of chase OT, encounter HO, right cross CFR and left cross CFL are defined for the unmanned ship;
the steering collision avoidance model of the unmanned ship comprises the following steps:
obtaining navigation rule table and relation function of conflict situation according to maritime rule model
Wherein P is the navigation range of the target ship in the marine environment, Delta theta is the difference of the course angle between the unmanned ship and the target ship, the north direction is defined as 0 degree, the clockwise direction is positive,
φ1,φ2respectively a set upper angle limit value and a set lower angle limit value, USVturnThe direction of the corner of the unmanned boat is shown;
when the unmanned ship is in a left crossing CFL situation or a pursuit OT situation and delta theta is in an element of 0, phi1) When the unmanned ship turns left; when the unmanned ship is in a right cross CFR situation, or in an encounter HO situation, or in a catch-up OT situation, and delta theta is epsilon [ phi ]22 pi), the unmanned boat turns right;
step S30, constructing an adaptive tracking model;
the self-adaptive tracking model is an end point tracking speed
Wherein the content of the first and second substances,
the maximum speed of the unmanned ship is L ═ d-dhD is the distance between the unmanned ship and the target ship, dhTo maintain the distance, voIs the current speed of the target vessel and,
the maximum acceleration of the unmanned ship is shown, and alpha is a regulating factor;
step S40, constructing a kinematics model of the unmanned ship;
the unmanned ship kinematic model is as follows:
wherein (x)i+1,yi+1) For the position of the unmanned boat in the map at the next moment, viIs the current speed, omega, of the unmanned boatiIs the current angular velocity, theta, of the unmanned boatiThe current course angle of the unmanned ship is shown, and t is time;
step S50, constructing an unmanned ship energy consumption model;
the energy consumption model of the unmanned ship is as follows:
unmanned ship energy consumption
Wherein the content of the first and second substances,
vi groundbottom velocity, vi currentIs the velocity of the wind and wave stream, vi usvThe speed of the unmanned boat; eta is a wave flow force factor of the unmanned ship;
separation distance
Step S60, determining the navigation working mode of the unmanned ship;
the unmanned ship navigation working mode comprises a target tracking mode and a navigation mode;
if the unmanned ship is in a target tracking mode, automatically setting the current terminal and the speed of reaching the terminal according to the self-adaptive tracking model; if the unmanned ship is in a sailing mode, manually setting a terminal coordinate, and automatically setting the speed of reaching the terminal to be 0;
step S70, constructing an attraction point constraint, a static barrier constraint, a dynamic barrier constraint, a speed and acceleration constraint, an unmanned ship kinematics constraint, a minimum time constraint and a minimum energy consumption constraint according to the maritime rule model, the steering collision avoidance model, the self-adaptive tracking model, the unmanned ship kinematics model and the unmanned ship energy consumption model, and establishing an objective function:
f(B)＝Υ1fa+Υ2fs+Υ3fd+Υ4fv+Υ5fω+Υ6facc+Υ7fk+Υ8fr+Υ9ft+Υ10fe；
wherein γ is the weight factor.
The attraction point constraints are: f. ofa＝||pb-pa||；
Wherein p isaAs attraction points, pbIs a separation in a sequence of statespaThe nearest coordinate point;
the static obstacle constraints are:
wherein d issDistance between unmanned boat and obstacle, dsafeIs a safe distance;
the dynamic barrier constraint is:
non-cooperative dynamic obstacle constraints:
wherein the distance between the unmanned surface vehicle and the non-cooperative target
For the ith coordinate point in the state sequence at time t,
the position of the dynamic barrier predicted at the time t;
cooperative dynamic barrier constraint:
wherein f ispFor the purpose of the corner deflection constraint,
βjsteering angle, beta, for unmanned boat at the moment of starting steering and avoiding collisionj+1The steering angle of the unmanned ship at the next moment is avoided.
The speed and acceleration constraints are respectively:
speed constraint
vmin、vmaxRespectively the minimum speed and the maximum speed of the unmanned ship;
constraint of angular velocity
ωmin、ωmaxRespectively the minimum speed and the maximum speed of the unmanned ship;
restraint of acceleration
aiIs the current acceleration of the unmanned ship, amin、amaxRespectively the minimum acceleration and the maximum acceleration of the unmanned ship;
restraint of angular acceleration
biIs the current angular acceleration of the unmanned ship, bmin、bmaxRespectively the minimum angular acceleration and the maximum angular acceleration of the unmanned ship;
wherein the content of the first and second substances,
the angular velocity, acceleration and angular acceleration of the unmanned ship are respectively as follows:
the kinematic constraint is:
the minimum bend radius constraint is:
wherein the current turning radius of the unmanned ship
RmThe minimum turning radius of the unmanned boat;
the minimum time constraint is:
the minimum energy consumption constraint is:
step S80, aiming at the objective function f (B), obtaining the optimal state sequence by adopting a least square method and a G2O diagram optimization algorithm according to the constraint conditions constructed in the step S70
I.e. the optimal path.
Compared with the prior art, the invention has the advantages that:
(1) the invention establishes a maritime regulation model and a steering collision avoidance model, and considers the influence of the international maritime regulation convention on dynamic collision avoidance, thereby realizing real-time dynamic collision avoidance on a cooperative target.
(2) The unmanned ship energy consumption model is established, and the influence of energy consumption on a path and the influence of wind wave flow interference on the speed and the angular speed of the unmanned ship are considered, so that the energy consumption of the unmanned ship in the sailing process can be saved, and the working time of the unmanned ship is prolonged.
(3) The invention establishes the self-adaptive tracking model, realizes smooth speed tracking, solves the problem of sudden speed change in the tracking process, reduces the control difficulty of the controller and further ensures the safety.
(4) The method combines the TEB algorithm, and solves the problem that the planned path is too ideal because the time sequence information and the motion constraint of the unmanned ship are not considered in the traditional algorithm. The method provided by the invention considers various constraint conditions of navigation time, collision risk, international maritime rule convention, energy consumption, kinematics limitation of unmanned ships and speed/acceleration limitation, can plan a track with time, speed, angular speed and position information in real time, and obtains a more optimal path compared with the traditional method.
Drawings
FIG. 1 is a schematic flow chart of the present invention;
FIG. 2 is a schematic illustration of the position of a cooperative target vessel of the present invention;
FIG. 3 is a schematic diagram illustrating the influence of the wind and wave flow velocity on the unmanned surface vehicle;
FIG. 4 is a schematic view of the kinematics constraint of an unmanned boat;
FIG. 5 is a hypergraph constructed in the present invention with various constraints added.
FIG. 6 is a schematic view of static collision avoidance according to the present invention.
FIG. 7 is a schematic view of dynamic collision avoidance in the present invention.
Fig. 8 is a speed profile of the sailing mode of the present invention.
Fig. 9 is a velocity profile in the target tracking mode of the present invention.
Detailed Description
The technical solution of the present invention will be further described in detail with reference to the accompanying drawings.
The embodiment of the invention provides a real-time collision avoidance and target tracking method for an unmanned ship, the implementation flow of which is shown in figure 1, and the method comprises the following steps:
preparation work: the starting point of the unmanned ship and the position, speed and attribute information of the obstacle are known.
Step S10, initializing a state B with a time sequence according to the starting point and the end point of the unmanned ship navigation track, the time interval and the number of the state sequences, wherein the state B is (Q, tau),
wherein: set of states Q ═ Qi}i＝1...nN is N, and the state set τ is { Δ T ═ Nj}j＝1...n-1,n∈N，qi＝(pi,βi)T＝(xi,yi,βi)T，pi＝(xi,yi) Is the current position coordinate, beta, of the unmanned ship in the mapiIs the current steering angle, delta T, of the unmanned shipjIs the time resolution;
and step S20, establishing a maritime affair rule and a steering collision avoidance model.
According to the international maritime collision avoidance rule Convention (COLREGS), the empirical data of ship collision avoidance and the navigation range of an actual ship in a complex marine environment, four collision situations of chase OT, encounter HO, right cross CFR and left cross CFL are defined for the unmanned ship;
and obtaining a navigation rule table (see table 1) and a relation function of the conflict situation:
in the formula: p is the range of the target ship, as shown in FIG. 2, Delta theta is the angular difference of the heading angles of the unmanned ship and the target ship, and the heading of the unmanned ship in the figure is defined as 0 degree,
according to COLREGS, a collision avoidance steering model of the unmanned ship meeting the target ship is established.
φ1,φ2Respectively setting an upper angle limit value and a lower angle limit value;
in the present embodiment, it is preferred that,
wherein the USVturnThe direction of the corner of the unmanned boat is shown;
when the unmanned ship is in a left crossing CFL situation or in a pursuit OT situation and
when the unmanned ship is in use, the unmanned ship can avoid obstacles leftwards; when the unmanned ship is in a right cross CFR situation or an encounter HO situation or a catch-up OT situation and
the unmanned boat needs to avoid the barrier rightwards;
TABLE 1 voyage rule table
The white areas and the unlisted data indicate that in this case the target vessel does not pose a threat to the unmanned vehicle or the target vessel should actively avoid the unmanned vehicle. The "avoidance" condition in the table is because the target vessel is now on the starboard side of the unmanned vehicle, the USV needs to be actively avoided according to COLREGS, and because the target vessel is on the rear side of the unmanned vehicle, the unmanned vehicle can be avoided only by accelerating.
And step S30, establishing an adaptive tracking model.
In order to enable the unmanned ship to track the target ship at a smooth speed and track the target as soon as possible, the terminal tracking speed of the unmanned ship needs to be constrained to be at a distance d from the target shiphIs the terminal point of the planning
dhTo maintain the distance. Then the endpoint tracking velocity
Comprises the following steps:
wherein
Is the maximum speed of the unmanned boat,
maximum acceleration, v, of unmanned boatoFor the current speed of the target vessel, L ═ d-dhD is the distance between the unmanned boat and the target ship, and alpha is an adjusting factor.
Step S40, establishing an unmanned ship motion model:
wherein (x)i+1,yi+1) For the position of the unmanned boat in the map at the next moment, viIs the current speed, omega, of the unmanned boatiIs the current angular velocity, theta, of the unmanned boatiThe current course angle of the unmanned ship is shown, and t is time;
and step S50, establishing an unmanned ship energy consumption model.
Wherein: v. ofi groundBottom velocity, vi currentIs the velocity of the wind and wave stream, vi usvThe speed of the unmanned boat, as shown in FIG. 3
Wherein FcurrentThe force of the storm flow on the unmanned ship is shown, and eta is a storm flow force factor of the unmanned ship.
An unmanned ship energy consumption model can be obtained:
wherein: separation distance
In step S60, the operation mode is determined,
the unmanned ship navigation working mode comprises a target tracking mode and a navigation mode;
if the unmanned ship is in a target tracking mode, automatically setting the current terminal and the speed of reaching the terminal according to the self-adaptive tracking model; if the unmanned ship is in a sailing mode, manually setting a terminal coordinate, and automatically setting the speed of reaching the terminal to be 0;
in step S70, a state sequence B is initialized based on the start point, the end point, the current speed and direction, and the end point speed and direction.
And step S80, adding attraction point constraint to ensure that the unmanned ship can return to the global path point after obstacle avoidance.
fa＝||pb-pa||
Wherein p isaAs attraction points, pbIs a distance p in the sequence of statesaThe nearest coordinate point;
and step S90, adding static obstacle restraint to enable the unmanned ship to avoid the static obstacle.
Wherein d issDistance between unmanned boat and obstacle, dsafeIs a safe distance;
and step S100, adding dynamic barrier constraint.
When the dynamic barrier is detected to be a non-cooperative target, adding dynamic barrier constraint in the whole time sequence, and predicting the position of the dynamic barrier by combining the time sequence to ensure that the dynamic barrier does not collide with the dynamic barrier.
Wherein the distance between the unmanned surface vehicle and the non-cooperative target
For the ith coordinate point in the state sequence at time t,
the position of the dynamic barrier predicted at the time t;
when the dynamic obstacle is detected as a cooperative target, namely the target ship which also follows COLREGS, adding a corner deviation constraint fpEnsuring steering in compliance with COLREGS while avoiding collisions safely.
Wherein, betajSteering angle, beta, for unmanned boat at the moment of starting steering and avoiding collisionj+1Steering angle for steering unmanned ship to avoid collision at next moment。
The dynamic barrier constraints for the cooperative target are as follows, at which time the unmanned boat will be subject to dynamic collision avoidance while following COLREGS.
And step S110, adding speed and acceleration constraints to ensure that the planned path has speed controllability.
Speed constraint
Wherein v ismin、vmaxRespectively the minimum speed and the maximum speed of the unmanned ship;
constraint of angular velocity
Wherein, ω ismin、ωmaxRespectively the minimum speed and the maximum speed of the unmanned ship;
restraint of acceleration
Wherein, aiIs the current acceleration of the unmanned ship, amin、amaxRespectively the minimum acceleration and the maximum acceleration of the unmanned ship;
restraint of angular acceleration
Wherein, biIs the current angular acceleration of the unmanned ship, bmin、bmaxRespectively the minimum angular acceleration and the maximum angular acceleration of the unmanned ship;
wherein the content of the first and second substances,
is measured by a sensor, and the direction of the sensor is deltai；
In the direction of betai＝arctan(pi+1-pi)；
Can calculate out
The sizes and directions of the components are respectively as follows:
therefore, the angular velocity of the unmanned ship can be obtained, and the acceleration and the angular acceleration are respectively as follows:
step S120, add kinematic constraints to ensure that the planned path is smooth and can be executed by the controller.
Assuming that the rotation angles of two successive state points are the same, as shown in FIG. 4, then
The following kinematic constraints can be obtained:
in addition, during actual navigation, when the unmanned boat is stableIn order to prevent the unmanned boat from capsizing during the turning at a constant speed, a minimum turning radius R existsm。
The current turning radius of the unmanned ship can be calculated as follows:
obtaining a minimum turning radius constraint:
step S130, add a minimum time constraint.
The shortest path is expected to be planned no matter during the navigation process of the unmanned ship or during the target tracking process, and under the condition of being in accordance with the kinematics of the unmanned ship, the shortest path is equivalent to the path which is navigated in the minimum time, so that the minimum time constraint is added:
and step S140, adding minimum energy consumption constraint according to the unmanned ship energy consumption model.
Step S150, the path planning problem is converted into the following multi-objective optimization problem:
f(B)＝Υ1fa+Υ2fs+Υ3fd+Υ4fv+Υ5fω+Υ6facc+Υ7fk+Υ8fr+Υ9ft+Υ10fe；
wherein, γ1～Υ10Are all weighting factors.
Step S160, solving the optimal solution by using the G2O graph optimization method, and building a hypergraph according to the multi-objective optimization function, as shown in fig. 5.
And step S170, solving by adopting a least square method, and continuously updating the state sequence B by adopting an LM (Levenberg-Marquardt) method and a sparse optimizer to carry out iterative computation.
After iteration for a certain number of times, the optimal solution is obtained:
the optimal solution is composed of a series of optimal states, comprises path point coordinates, directions and time sequences, and can also calculate information such as speed, angular speed and the like of the unmanned ship at each path point.
And sending the coordinate point or the speed and angular speed information of the unmanned ship to the controller according to the requirements of the controller.
Fig. 6 shows a static collision avoidance routing diagram with black circles as obstacles and gray circles representing the starting points of the drones.
Fig. 7 shows a schematic diagram of dynamic obstacles for a cooperating vessel, the squares representing the dynamic obstacles and the grey circles representing the unmanned boat starting point. It can be seen from the figure that the unmanned ship gets out of the way to the left in the CFL situation at present, and the requirement of COLREGS is met.
Fig. 8 shows a velocity profile of the unmanned vehicle in cruise mode with a start velocity of 0 and an end velocity of 0.
Fig. 9 shows a velocity profile of the unmanned vehicle in target tracking mode with a starting velocity of 0 and an ending velocity of the arrival velocity according to the adaptive tracking model.
The present invention has not been described in detail, partly as is known to the person skilled in the art.
Claims (7)
1. A real-time collision avoidance and target tracking method for an unmanned ship is characterized by comprising the following steps:
step S10, initializing a state B with a time sequence according to the starting point and the end point of the unmanned ship navigation track, the time interval and the number of the state sequences, wherein the state B is (Q, tau),
wherein: set of states Q ═ Qi}i＝1...nN is N, and the state set τ is { Δ T ═ Nj}j＝1...n-1,n∈N，qi＝(pi,βi)T＝(xi,yi,βi)T，pi＝(xi,yi) Is the current position coordinate, beta, of the unmanned ship in the mapiThe current steering angle of the unmanned ship is defined as east direction of 0 degrees, anticlockwise direction of positive, and delta TjIs the time resolution;
step S20, constructing a maritime affair rule model and a steering and collision avoidance model of the unmanned ship according to COLREGS;
in step S20, the maritime rule model is: according to COLREGS, ship collision avoidance experience data and the navigation range of a target ship in a marine environment, four collision situations of chase OT, encounter HO, right cross CFR and left cross CFL are defined for the unmanned ship;
the steering collision avoidance model of the unmanned ship comprises the following steps:
obtaining navigation rule table and relation function of conflict situation according to maritime rule model
Wherein P is the navigation range of the target ship in the marine environment, Delta theta is the difference of the course angle between the unmanned ship and the target ship, the north direction is defined as 0 degree, the clockwise direction is positive,
φ1,φ2respectively a set upper angle limit value and a set lower angle limit value, USVturnThe direction of the corner of the unmanned boat is shown;
when nobodyThe boat is in a left crossing CFL situation or in a pursuit OT situation and delta theta is in an element of 0, phi1) When the unmanned ship turns left; when the unmanned ship is in a right cross CFR situation, or in an encounter HO situation, or in a catch-up OT situation, and delta theta is epsilon [ phi ]22 pi), the unmanned boat turns right;
step S30, constructing an adaptive tracking model;
in step S30, the adaptive tracking model is an end point tracking velocity
Wherein the content of the first and second substances,
the maximum speed of the unmanned ship is L ═ d-dhD is the distance between the unmanned ship and the target ship, dhTo maintain the distance, voIs the current speed of the target vessel and,
the maximum acceleration of the unmanned ship is shown, and alpha is a regulating factor;
step S40, constructing a kinematics model of the unmanned ship;
step S50, constructing an unmanned ship energy consumption model;
in step S50, the unmanned surface vehicle energy consumption model is:
unmanned ship energy consumption
Wherein the content of the first and second substances,
vi groundbottom velocity, vi currentIs the velocity of the wind and wave stream, vi usvThe speed of the unmanned boat; eta is a wave flow force factor of the unmanned ship;
separation distance
(xi+1,yi+1) The position of the unmanned boat in the map at the next moment is shown;
step S60, determining the navigation working mode of the unmanned ship; the unmanned ship navigation working mode comprises a target tracking mode and a navigation mode;
step S70, constructing an attraction point constraint f according to the maritime affair rule model, the steering collision avoidance model, the self-adaptive tracking model, the unmanned ship kinematics model and the unmanned ship energy consumption modelaStatic obstacle restraint fsDynamic obstacle constraint, speed and acceleration constraint, unmanned ship kinematics constraint fkMinimum time constraint ftMinimum energy consumption constraint feAnd establishing an objective function:
wherein, C1～C12Are all weight factors; f. ofrIs a minimum bend radius constraint;
the dynamic obstacle constraints include non-cooperative dynamic obstacle constraints
Cooperative dynamic barrier constraint
The velocity and acceleration constraints include a velocity constraint fvConstraint of angular velocity fωAcceleration constraint fvaccAngular acceleration constraint fωacc；
Step S80, aiming at the objective function f (B), obtaining the optimal state by adopting a least square method and a G2O diagram optimization algorithm according to the constraint conditions constructed in the step S70Sequence of
I.e. the optimal path.
2. The real-time collision avoidance and target tracking method of the unmanned ship according to claim 1, characterized in that: in step S40, the unmanned surface vehicle kinematic model is:
wherein (x)i+1,yi+1) For the position of the unmanned boat in the map at the next moment, viIs the current speed, omega, of the unmanned boatiIs the current angular velocity, theta, of the unmanned boatiThe current course angle of the unmanned ship is shown, and t is time.
3. The real-time collision avoidance and target tracking method of the unmanned ship according to claim 1 or 2, characterized in that: in the step S60, if the unmanned ship is in a target tracking mode, automatically setting the current terminal and the speed of reaching the terminal according to the self-adaptive tracking model; if the unmanned ship is in a sailing mode, the terminal coordinate is set manually, and the speed of reaching the terminal is set to be 0 automatically.
4. The real-time collision avoidance and target tracking method of the unmanned ship according to claim 3, characterized in that: in step S70, the attraction point constraint is: f. ofa＝||pb-pa||；
Wherein p isaAs attraction points, pbIs a distance p in the sequence of statesaThe nearest coordinate point;
the static obstacle constraints are:
wherein d issDistance between unmanned boat and barrier，dsafeIs a safe distance.
5. The real-time collision avoidance and target tracking method of the unmanned ship according to claim 4, characterized in that: in step S70, the dynamic obstacle constraints include non-cooperative dynamic obstacle constraints and cooperative dynamic obstacle constraints:
non-cooperative dynamic obstacle constraints:
wherein the distance between the unmanned surface vehicle and the non-cooperative target
For the ith coordinate point in the state sequence at time t,
the position of the dynamic barrier predicted at the time t;
cooperative dynamic barrier constraint:
wherein f ispFor the purpose of the corner deflection constraint,
βjsteering angle, beta, for unmanned boat at the moment of starting steering and avoiding collisionj+1The steering angle of the unmanned ship at the next moment is avoided.
6. The real-time collision avoidance and target tracking method of the unmanned ship according to claim 5, characterized in that: in step S70, the speed and acceleration constraints include:
speed constraint
vmin、vmaxRespectively the minimum speed and the maximum speed of the unmanned ship;
constraint of angular velocity
ωmin、ωmaxRespectively the minimum speed and the maximum speed of the unmanned ship;
restraint of acceleration
aiIs the current acceleration of the unmanned ship, amin、amaxRespectively the minimum acceleration and the maximum acceleration of the unmanned ship;
restraint of angular acceleration
biIs the current angular acceleration of the unmanned ship, bmin、bmaxRespectively the minimum angular acceleration and the maximum angular acceleration of the unmanned ship;
wherein the content of the first and second substances,
the angular velocity, acceleration and angular acceleration of the unmanned ship are respectively as follows:
angular velocity
Acceleration of a vehicle
Angular acceleration
7. The real-time collision avoidance and target tracking method of the unmanned ship according to claim 6, characterized in that: in step S70, the kinematic constraint is:
the minimum bend radius constraint is:
wherein the current turning radius of the unmanned ship
RmThe minimum turning radius of the unmanned boat;
the minimum time constraint is:
the minimum energy consumption constraint is:
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910555673.6A CN110262492B (en) | 2019-06-25 | 2019-06-25 | Real-time collision avoidance and target tracking method for unmanned ship |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910555673.6A CN110262492B (en) | 2019-06-25 | 2019-06-25 | Real-time collision avoidance and target tracking method for unmanned ship |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110262492A CN110262492A (en) | 2019-09-20 |
| CN110262492B true CN110262492B (en) | 2022-04-08 |
Family
ID=67921407
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910555673.6A Active CN110262492B (en) | 2019-06-25 | 2019-06-25 | Real-time collision avoidance and target tracking method for unmanned ship |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110262492B (en) |
Families Citing this family (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111256694A (en) * | 2019-12-16 | 2020-06-09 | 中国船舶工业系统工程研究院 | Method for determining path of unmanned surface vehicle |
| CN111061273B (en) * | 2019-12-26 | 2023-06-06 | 航天时代(青岛)海洋装备科技发展有限公司 | Autonomous obstacle avoidance fusion method and system for unmanned ship |
| CN111045428B (en) * | 2019-12-27 | 2023-12-12 | 达闼机器人股份有限公司 | Obstacle avoidance method, mobile robot, and computer-readable storage medium |
| CN111158380B (en) * | 2020-01-19 | 2023-08-04 | 中电科（宁波）海洋电子研究院有限公司 | Water surface unmanned ship obstacle avoidance method with towing line array |
| CN111879324A (en) * | 2020-06-29 | 2020-11-03 | 智慧航海(青岛)智能系统工程有限公司 | Path planning method and device based on ship angular speed limitation |
| CN112147899B (en) * | 2020-09-30 | 2022-05-20 | 沈阳工业大学 | Underwater robot autonomous obstacle avoidance control method based on fuzzy sliding mode algorithm |
| CN112882380B (en) * | 2021-01-07 | 2022-06-14 | 上海交通大学 | Multi-unmanned-vessel system cooperative control method, terminal and medium under sequential logic task |
| CN113419522B (en) * | 2021-05-21 | 2022-08-12 | 航天时代(青岛)海洋装备科技发展有限公司 | Simulation method and system for unmanned ship path planning algorithm |
| CN113296509B (en) * | 2021-05-21 | 2022-11-08 | 上海海事大学 | Autonomous trajectory tracking fusion control method for unmanned surface vessel |
| CN113325846B (en) * | 2021-05-31 | 2024-02-27 | 西安建筑科技大学 | Strip mine unmanned mine card dynamic path planning method based on improved TEB method |
| CN114326798A (en) * | 2021-12-16 | 2022-04-12 | 珠海云洲智能科技股份有限公司 | Unmanned aerial vehicle, unmanned ship, unmanned submersible vehicle motion planning method and storage medium |
| CN114995425B (en) * | 2022-06-02 | 2024-05-07 | 航天时代(青岛)海洋装备科技发展有限公司 | Unmanned ship autonomous tracking method based on time optimization and energy optimization |
| CN114861554B (en) * | 2022-06-02 | 2023-04-07 | 广东工业大学 | Unmanned ship target track prediction method based on collective filtering |
Family Cites Families (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101833335B (en) * | 2010-05-10 | 2013-10-30 | 珠海云洲智能科技有限公司 | Small-size water surface robot device and self-adaptive flow optimizing navigation method |
| US8849483B2 (en) * | 2011-04-13 | 2014-09-30 | California Institute Of Technology | Target trailing with safe navigation with colregs for maritime autonomous surface vehicles |
| CN108459503B (en) * | 2018-02-28 | 2021-03-30 | 哈尔滨工程大学 | Unmanned surface vehicle track planning method based on quantum ant colony algorithm |
| CN108445879B (en) * | 2018-03-12 | 2021-02-23 | 上海大学 | Unmanned ship obstacle avoidance method based on collision danger prediction area |
| CN108416152B (en) * | 2018-03-18 | 2021-12-24 | 哈尔滨工程大学 | Unmanned ship ant colony energy consumption optimal global path planning method based on electronic chart |
| CN109213164A (en) * | 2018-09-07 | 2019-01-15 | 上海大学 | The urgent barrier-avoiding method of the high speed of Model Predictive Control based on unmanned surface vehicle hull model |
| CN109828566B (en) * | 2019-01-30 | 2022-05-24 | 华南理工大学 | Autonomous sailing method for unmanned surface vehicle |
| CN109839934A (en) * | 2019-02-26 | 2019-06-04 | 华南理工大学 | Unmanned water surface ship default capabilities tracking and controlling method based on RISE technology |
| CN109765914A (en) * | 2019-03-12 | 2019-05-17 | 哈尔滨工程大学 | A kind of unmanned surface vehicle collision prevention method based on sliding window population |
-
2019
- 2019-06-25 CN CN201910555673.6A patent/CN110262492B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN110262492A (en) | 2019-09-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110262492B (en) | Real-time collision avoidance and target tracking method for unmanned ship | |
| Svec et al. | Dynamics-aware target following for an autonomous surface vehicle operating under COLREGs in civilian traffic | |
| CN107168335B (en) | Water surface unmanned ship path tracking guidance method considering hybrid multi-target obstacle avoidance | |
| CN109597417B (en) | Multi-USV group collaborative collision avoidance planning method based on collision avoidance criterion | |
| CN112130566B (en) | Unmanned ship, unmanned plane hybrid formation control method and control system thereof based on fuzzy logic and sliding mode control strategy | |
| WO2020253028A1 (en) | Dynamic collision avoidance method for unmanned surface vessel based on route replanning | |
| Wiig et al. | Collision avoidance for underactuated marine vehicles using the constant avoidance angle algorithm | |
| CN110196598B (en) | Dynamic collision avoidance method for unmanned ship | |
| CN108073175B (en) | Under-actuated unmanned ship formation intelligent control method based on virtual ship self-adaptive planning | |
| Lekkas et al. | Line-of-sight guidance for path following of marine vehicles | |
| US8215252B1 (en) | System and method for dynamic stabilization and navigation in high sea states | |
| CN109460045B (en) | Improved ant colony optimization-based collision avoidance planning method for USV under dynamic obstacle online perception | |
| CN109871031B (en) | Trajectory planning method for fixed-wing unmanned aerial vehicle | |
| WO2021230356A1 (en) | Method for autonomously guiding vessel, program for autonomously guiding vessel, system for autonomously guiding vessel, and vessel | |
| CN111045453A (en) | Cooperative control system and method based on unmanned ship and multi-underwater robot | |
| CN112650231B (en) | Under-actuated ship formation control system for realizing collision and obstacle avoidance | |
| CN113419428B (en) | Machine/ship cooperative path tracking controller design method based on 3D mapping guidance | |
| CN112327872B (en) | Double unmanned ship cooperative track tracking method for oil spill containment | |
| CN114895673A (en) | Ship collision avoidance decision method based on deep reinforcement learning under rule constraint | |
| CN109916400B (en) | Unmanned ship obstacle avoidance method based on combination of gradient descent algorithm and VO method | |
| CN114995409A (en) | Unmanned ship autonomous cruise control system and cruise method | |
| CN113093804B (en) | Unmanned ship formation control method and control system based on inversion sliding mode control | |
| CN113741433A (en) | Distributed formation method for unmanned surface ship | |
| JP6658220B2 (en) | Navigation control method and device | |
| CN117369441A (en) | Self-adaptive intelligent ship path planning method considering ship kinematics and COLLEGs |
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 | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20220412 Address after: 266200 aoshanwei sub district office, Jimo District, Qingdao City, Shandong Province Patentee after: Aerospace Times (Qingdao) marine equipment technology development Co.,Ltd. Address before: 142 box 403, box 100854, Beijing, Beijing, Haidian District Patentee before: BEIJIGN INSTITUTE OF AEROSPACE CONTROL DEVICES |