CN109240288B - Unmanned ship collision avoidance path planning method based on track unit under condition of obstacle - Google Patents
Unmanned ship collision avoidance path planning method based on track unit under condition of obstacle Download PDFInfo
- Publication number
- CN109240288B CN109240288B CN201811015791.XA CN201811015791A CN109240288B CN 109240288 B CN109240288 B CN 109240288B CN 201811015791 A CN201811015791 A CN 201811015791A CN 109240288 B CN109240288 B CN 109240288B
- Authority
- CN
- China
- Prior art keywords
- path
- point
- unmanned ship
- points
- heading
- 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 67
- 230000033001 locomotion Effects 0.000 claims abstract description 85
- 230000008569 process Effects 0.000 claims abstract description 45
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000013507 mapping Methods 0.000 claims description 6
- 238000011160 research Methods 0.000 claims description 6
- 230000000452 restraining effect Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 19
- 230000008859 change Effects 0.000 description 14
- 239000012530 fluid Substances 0.000 description 11
- 230000000694 effects Effects 0.000 description 8
- 238000004458 analytical method Methods 0.000 description 7
- 230000002093 peripheral effect Effects 0.000 description 5
- 238000013459 approach Methods 0.000 description 4
- 230000005484 gravity Effects 0.000 description 4
- 241000380131 Ammophila arenaria Species 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000010845 search algorithm Methods 0.000 description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 125000004122 cyclic group Chemical group 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 238000010606 normalization Methods 0.000 description 2
- 239000000243 solution Substances 0.000 description 2
- 235000013619 trace mineral Nutrition 0.000 description 2
- 239000011573 trace mineral Substances 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 231100000716 Acceptable daily intake Toxicity 0.000 description 1
- 241000238097 Callinectes sapidus Species 0.000 description 1
- 239000007983 Tris buffer Substances 0.000 description 1
- 238000010521 absorption reaction Methods 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 125000003636 chemical group Chemical group 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 239000000446 fuel Substances 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000000704 physical effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000002121 ultrasonic speckle velocimetry Methods 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)
Abstract
The invention discloses a method for planning collision avoidance paths of an unmanned ship under the condition of obstacles based on a track unit, which comprises the following steps: 1) determining a starting point and an end point of a movement path of the unmanned ship; 2) determining real-time path points and heading angles of the unmanned ship; 3) obtaining reachable path points according to real-time path points and heading angles of the unmanned ship, track units of unmanned ship movement and path points of obstacles; 4) calculating the path cost of all reachable path points to obtain the position of the next path point and a corresponding heading angle; 5) and judging whether the path point is the end point of the motion path, if so, outputting the final path, otherwise, taking the path point and the corresponding heading angle as the real-time path point and the heading angle of the unmanned ship, and turning to the step 2). The unmanned ship track unit model is established, so that the planned path is closer to the actual navigation requirement in the actual navigation process, and the obstacle can be avoided automatically.
Description
Technical Field
The invention relates to a maritime intelligent traffic technology, in particular to a collision avoidance path planning method for an unmanned ship under the condition of an obstacle based on a track unit.
Background
The development of artificial intelligence and the development of marine resources have made surface unmanned vehicles (USVs) increasingly take on various waterborne tasks. As an important part of the autonomy of the unmanned ship, path planning is a prerequisite for completing various water tasks. However, since the unmanned surface vehicle has a low degree of controllability and a high degree of freedom, it is considered that the unmanned surface vehicle should be converted from a mass point to an under-actuated rigid body when the unmanned surface vehicle is an object of study. Accordingly, the path planning problem should be changed from route planning to movement planning. Unlike the former, motion planning not only takes into account the constraints of the planning space, but also discusses the planning behavior in detail. Whereas the planning activity is related to the motion and dynamic constraints of the study object. Therefore, in order to fully consider the motion and dynamic constraints of the unmanned ship, the unmanned ship motion planning method based on the track unit is provided.
Disclosure of Invention
The invention aims to solve the technical problem of providing a method for planning collision avoidance paths of an unmanned ship under the condition of obstacles based on a track unit, aiming at the defects in the prior art.
The technical scheme adopted by the invention for solving the technical problems is as follows: a collision avoidance path planning method for an unmanned ship under the condition of obstacles based on a track unit comprises the following steps:
1) determining a starting point and an end point of a movement path of the unmanned ship;
2) determining real-time path points and heading angles of the unmanned ship;
3) obtaining reachable path points according to real-time path points and heading angles of the unmanned ship, track units of unmanned ship movement and path points of obstacles;
the generation mode of the track unit is as follows:
3.1) carrying out motion modeling on the unmanned ship by utilizing the MMG model to obtain the motion trail of the unmanned ship;
3.2) restraining the motion trail of the unmanned ship by adopting the unmanned ship trail discretization rule; the following discretization rules are included:
rule one is as follows: at the initial and end moments of the track section, the motion state of the unmanned ship is stable and consistent;
rule two: the number of times of steering of each section of track is not more than one;
rule three: mapping the path points which can be reached by the track segment and the heading which can be changed one by one;
rule four: the outline shapes of all track segments are squares or based on the squares so as to adapt to the grid map;
3.3) completing the modeling process of the track units according to the steps 3.1) and 3.2), and establishing a track unit set;
4) calculating the path cost of all reachable path points to obtain the position of the next path point and a corresponding heading angle;
5) and judging whether the path point is the end point of the motion path, if so, outputting the final path, otherwise, taking the path point and the corresponding heading angle as the real-time path point and the heading angle of the unmanned ship, and turning to the step 2).
According to the scheme, the reachable path point is obtained in the step 3), and the searching method comprises the following steps:
according to the real-time path point and the heading angle of the unmanned ship and the track unit of the unmanned ship motion, the geometric characteristics of the track unit are utilized to obtain the path point which can be reached from the real-time path point and the corresponding ship heading;
searching reachable inner layer path points according to the reachable path points; the inner layer path points are 8 adjacent points taking the real-time path points as the center;
judging whether path points are associated, and searching reachable outer layer path points if the path points are associated; the outer layer path point is an outer layer associated point of the inner layer path point which takes the real-time path point as the center.
According to the scheme, if dangerous points exist in the reachable path points obtained in the step 3), the dangerous points are discarded, and then searching is performed; the danger point is a neighboring point between two path points located by the obstacle. I.e. the danger point and the obstacle are located in the set of waypoints, which are both adjacent points and between these two waypoints.
According to the scheme, if the path point right in front of the current path point is occupied by the obstacle and cannot be reached in the reachable path points obtained in the step 3), if the heading of the unmanned ship is changed to 45 degrees, the path point is discarded.
According to the scheme, the cost calculation in the step 4) is represented by a cost function f (x), which is as follows:
f(x)＝g(x)+h(x)
wherein g (x) represents an actual cost function from an initial point to a current point, and is a cost value of a path actually traveled by a research object; h (x) is a heuristic cost function from the current point to the end point, and is an estimated value of the path which is left;
wherein the actual cost function g (x) is composed of a distance cost function d (x) and a steering cost function s (x), and the heuristic cost function h (x) is represented by Manhattan distance.
The invention has the following beneficial effects:
1. according to the unmanned ship motion planning method, the two key problems of unmanned ship motion planning, namely the problem of unmanned ship dynamics constraint expression and the problem of combination of unmanned ship dynamics constraint and space search, are effectively solved by establishing a track unit model of unmanned ship motion planning.
The model solves the problem of dynamics constraint expression of the unmanned ship by using the motion trail of the unmanned ship. The motion track of the unmanned ship not only can completely express the dynamic constraints, but also the connection between the constraints is contained in a track curve;
considering that the space search is a space discretization process, the model provides four track discretization rules according to the track characteristics of the unmanned ship and the search requirements of the motion planning. The rules not only disperse continuous tracks into track sections, so that each step of subspace search contains unmanned ship dynamics constraints; and the continuity of the final track is still kept after the track sections are spliced, so that the problem of combining dynamic constraint with space search is solved.
2. The unmanned ship track unit model is established, and the unmanned ship motion planning algorithm based on the track unit is provided, so that the planned path is closer to the actual navigation requirement in the actual navigation process.
3. According to the method, the retrieval time of path planning is reduced through the judgment of the path association points and the dangerous points, and the obstacle avoidance can be carried out independently.
Drawings
The invention will be further described with reference to the accompanying drawings and examples, in which:
FIG. 1 is a flow chart of a method of an embodiment of the present invention;
FIG. 2 is a schematic representation of the hydrodynamic forces acting on a hull of an embodiment of the present invention;
FIG. 3 is a schematic illustration of a side-to-side convolution analysis of an unmanned surface vehicle according to an embodiment of the present invention;
FIG. 4 is a schematic illustration of a spliced continuous track segment according to an embodiment of the invention;
FIG. 5 is a schematic diagram of an embodiment of the present invention with no more than one helm turn;
FIG. 6 is a schematic diagram of the correspondence between waypoints and heading according to an embodiment of the invention;
FIG. 7 is a grid-based schematic of the contour shape of a trajectory according to an embodiment of the present invention;
FIG. 8 is a schematic diagram of a trace unit generation according to an embodiment of the present invention;
FIG. 9 is a schematic diagram of a traversal of the grid around the grid map according to an embodiment of the present invention;
FIG. 10 is a schematic view of an embodiment of the present invention showing the type of unmanned boat heading;
FIG. 11 is a schematic diagram of waypoints and possible heading for an embodiment of the invention;
FIG. 12 is a schematic diagram of a generation process of a track unit when the initial heading is 0 ° according to an embodiment of the present invention;
FIG. 13 is a schematic diagram of waypoints and possible heading for an embodiment of the invention;
FIG. 14 is a schematic diagram of a process for generating a trace element according to an embodiment of the invention;
FIG. 15 is a schematic diagram of spatial full coverage verification according to an embodiment of the present invention;
FIG. 16 is a trace unit abstraction diagram according to an embodiment of the present invention;
FIG. 17 is an all track unit abstraction diagram according to an embodiment of the present invention;
FIG. 18 is a schematic diagram of a trajectory segment curve and distance calculation according to an embodiment of the present invention;
FIG. 19 is a diagram illustrating the normalization of a track segment according to an embodiment of the present invention;
FIG. 20 is a diagram illustrating the normalization of a track segment according to an embodiment of the present invention;
FIG. 21 is a schematic diagram illustrating the association of inner waypoints and outer waypoints according to an embodiment of the invention;
FIG. 22 is a schematic diagram of a spiral search in accordance with an embodiment of the present invention;
FIG. 23 is a schematic view of a hazard point in an obstacle environment in accordance with an embodiment of the present invention;
fig. 24 is a schematic diagram comparing unmanned ship route planning and motion planning in an obstacle environment according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail with reference to the following embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
As shown in fig. 1, a method for planning collision avoidance paths of an unmanned ship under the condition of an obstacle based on a track unit includes the following steps:
1) determining a starting point and an end point of a movement path of the unmanned ship;
2) determining real-time path points and heading angles of the unmanned ship;
3) obtaining reachable path points according to real-time path points and heading angles of the unmanned ship, track units of unmanned ship movement and path points of obstacles;
the generation mode of the track unit is as follows:
3.1) carrying out motion modeling on the unmanned ship by utilizing the MMG model to obtain the motion trail of the unmanned ship;
as shown in fig. 2, which is a schematic diagram of fluid power (moment) acting on a ship body, the ship motion modeling method can be divided into two categories: the integral model structure represented by Europe and America is the main idea of the integral model structure, namely, a ship body, a propeller and a rudder are regarded as an integral body which can not be divided; another group is a separation type model structure represented in Japan, and is generally called MMG (manipulating chemical group) model. The main idea of the MMG model is to decompose the hydrodynamic forces (moments) into component forces (moments) acting on the ship, the oar, the rudder. The force (moment) applied to the ship body can be divided into viscous acting force (moment) and inertial acting force (moment). The MMG model is therefore composed of a viscous model, an inertial model, a propeller model and a rudder model.
The model has wider adaptation scene, is flexible and variable, is easy to carry out local modification aiming at a specific ship type, and can express the interference effect of the ship-paddle-rudder concisely. Therefore, the invention utilizes the MMG model to carry out motion modeling on the unmanned ship.
1) Unmanned ship movement modeling based on MMG model
To simplify the problem, the following assumptions need to be made before modeling:
suppose 1, the planar motion (three degrees of freedom) of the unmanned boat is taken as the research focus, and the heave, roll and pitch in the motion process are neglected.
And 2, the influence of natural environments such as wind, waves and flow on the movement of the unmanned ship is not considered.
Suppose 3, consider only the case of forward rotation of the marine main engine. And when the unmanned ship sails stably, the rotating speed of the propeller keeps unchanged.
For the assumption 3, supplementary explanation is necessary. When the ship is in an underway state, the ship body can be subjected to resistance of water flow. Thereby increasing the load on the propeller resulting in a reduction of the rotational speed (n). At this point, the speed controller will increase the main machine power to offset the increased load of the propeller and maintain the original speed. In the above process, under the regulation of the rotation speed controller, the control model can be expressed as:
wherein, TDIs a time constant, IEIs the moment of inertia, k, of the entire systempIs the gain of the speed controller, nrIs the commanded speed, K is the gain of the primary drive rod to torque output, QPIs the propeller absorption moment.
Therefore, the three-freedom-degree motion model of the unmanned ship under the conditions of forward motion (X), transverse motion (Y) and turning bow (N) can be expressed as follows:
i, H, P, R are inertia, viscosity, propeller and rudder forces (moments), respectively.
(1) Inertial model
During the course of the ship, the surrounding fluid is disturbed, so that the fluid medium generates an additional momentum (moment of momentum). Wherein the additional moment of inertia can be expressed as:
in the formula mxAnd myAdditional mass in the X-axis and Y-axis directions; alpha is alphaxIs myCoordinate value, J, acting on the centre of gravity on the X-axiszzA moment is added to the inertia acting in the Z-axis direction. The additional momentum and moments of momentum can therefore be expressed as:
wherein u and v are the velocities in the X-axis and Y-axis directions, respectively, and r is the yaw rate. And differentiating the equations (1-4) in the corresponding directions to obtain the inertia force and the inertia moment of the fluid:
(2) viscous model
The viscous fluid dynamics and moments experienced by a vessel are related to the geometry of the hull, the state of motion of the vessel, and the physical properties of the fluid. Thus when the ship shape and flow field are constant, the viscous power and moment acting on the ship body are mainly dependent on the current motion state of the ship (i.e. u, v and r):
wherein X (u) is the straight sailing resistance of the ship; xvvv2、Xvrvr、Xrrr is the viscous resistance caused by the movement of the ship in the fluid medium; y isvv、Yrr and Nvv、Nrr is respectively linear transverse hydrodynamic force and moment, YnlHAnd NnlHThen it is a non-linear term.
In general, when a ship moves in a fluid medium, the size of the drift angle has a large influence on viscous force and moment acting on the ship body. Since the influence of wind, waves and currents on the ship is not considered herein (assumption 2), the ship does not generate a drift angle during sailing. The nonlinear hydrodynamic forces and moments (Y) are then estimated herein by means of the Guitar equationnlHAnd NnlH)：
The final result can be obtained by combining the formulas (1-6) and (1-7):
in practical applications, to simplify the problem, a second order taylor expansion is sometimes used to approximate viscous fluid dynamics and moments:
where X0 is the straight running resistance of the ship, Δ u is the change in speed in the X-axis direction, and Xu, Yv, Yr, Nv, and Nr are the derivatives of the hydrodynamic force.
(3) Propeller model
The propeller is a main source of power of the ship, and provides thrust and generates transverse acting force (moment). But the effect of this transverse force (moment) is negligible small compared to the effect produced by the rudder. Thus, the rudder model can be expressed as:
where T is the thrust generated by the propeller, TPIs the thrust derating coefficient. The thrust derating factor is the ratio of thrust derating to thrust, and is determined by a number of factors, such as the size of the vessel and the propeller. The thrust T generated by the propeller and the diameter D of the disk surface of the propellerPThe speed n, the fluid density ρ, etc. The thrust force calculation can be approximated as:
T＝ρn2Dp 2KT(JP) (1-11)
in the formula KTIs the thrust coefficient; j. the design is a squarePIs the advance speed coefficient. Both can also be represented as
In the formula ai(i ═ 0,1,2) is a regression coefficient; w is aPIs the wake factor generated on the propeller when the ship is in inclined navigation.
(4) Rudder model
The rudder model is the most important part of the model because it determines the direction of the ship's motion and thus the ship's motion trajectory. After the positive pressure of the water flow on the rudder is generally decomposed into longitudinal resistance, transverse lateral movement force and rudder turning moment according to three degrees of freedom, a rudder model can be obtained:
in the formula, delta is a rudder angle; t is tRIs a resistance derating coefficient; a isHIs a correction factor; x is the number ofHThe longitudinal distance from the stress action point of the rudder to the center of gravity of the ship; fNThe positive pressure of the rudder can be expressed as:
in the formula of URThe effective rudder speed at which the fluid enters the rudder; a. theRIs the effective rudder area; alpha is alphaRIs the effective angle of attack; f. ofaIs the slope of the lift.
In order to control the ship conveniently so as to obtain the required track, a control model of the rudder is also required to be established:
in the formula ofEIs a command rudder angle; t isEIs a time constant of about 2.5 seconds.
2) Unmanned ship motion trajectory analysis
In order to verify the accuracy of the model, it is necessary to analyze the motion trajectory generated by the model.
Fig. 3 shows the circling tracks of the left and right sides of the unmanned boat after operating the same rudder angle at a certain rotating speed. It can be seen that the radius of the swivel on port side is slightly larger than on starboard side (R)P>RS). This phenomenon is true because the mass of a ship is difficult to keep absolutely even on both the left and right sides. So that the center of gravity of the vessel is generally not on its centerline. This also demonstrates that the model is realistic. It can be seen that the difference between the two is not very large. Therefore, in order to simplify the problem, when establishing the unmanned boat trajectory unit, the motion trajectories of the two sides are considered to be the same,i.e. has a left-right symmetry.
In addition, another phenomenon can be found from the figure, namely the reverse shift amount (kick) in the initial stage. The reverse shift amount is a distance by which the center of gravity of the ship shifts in a direction opposite to the steering direction at the initial stage of the turning. In general, the amount of backshifting is small and does not exceed a maximum of 1% of its beam length. The reverse displacement is generally determined by factors such as ship speed, rudder angle and ship type, wherein the ship speed and the rudder angle are in direct proportion to the reverse displacement. The right green box of fig. 3 is an enlarged view of the initial spin-back stage of the unmanned boat. As can be seen from the figure, the left and right sides have a very small reverse traversing process, namely reverse traversing amount at the initial stage of the turning. It follows that the motion model is in accordance with the actual motion process of the ship.
Thus by the above analysis. The unmanned ship motion modeling based on the MMG model is accurate and reliable, and accords with the actual motion situation.
3.2) restraining the motion trail of the unmanned ship by adopting the unmanned ship trail discretization rule; the following discretization rules are included:
rule one is as follows: at the initial and end moments of the track section, the motion state of the unmanned ship is stable and consistent;
in order to maintain the continuity of the discrete track segments after splicing, it is feasible to stabilize and keep the motion state of the unmanned ship consistent at the initial and end moments of the track segments. Specifically, there are two requirements: (1) the rudder angle is 0; (2) the speeds are consistent. As shown in fig. 4.
The rudder angle is 0 to maintain the stability of the flight. When the rudder is fixed at a certain rudder angle, the ship can be always in a gyrating motion state. At the moment, various motion parameters of the ship are changed in real time, and the state conversion operation is not facilitated. And when the rudder angle is 0, the ship is in a straight sailing state. Under the condition of no environmental interference such as wind, waves, flow and the like, the ship can be stabilized in a certain sailing direction. This steady state facilitates a smooth transition from the current state to the next state during the track splice.
The speeds are kept consistent, so that the speeds are the same in the track splicing process; but also the curvature of the front and back trajectory curves remains consistent at the splice point. Since different boat speeds correspond to different turning radii. When the speed is constant, the turning radius is kept unchanged, and the curvature is not changed.
Rule two: the number of times of steering of each section of track is not more than one;
a cell refers to an indivisible quantity, and therefore a track segment in a cell should be as simple as possible. And the standard for judging the track complexity is the steering times of the unmanned ship, namely the steering times. Therefore, the number of times of steering of one track segment should not be too large; otherwise the spliced trajectory will produce a large number of turning points, which is clearly not appropriate for practical applications. The present rules require that the number of turns should not exceed once in the generation of the trajectory segment (as shown in fig. 5, no return rudder is included).
Rule three: mapping the path points which can be reached by the track segment and the heading which can be changed one by one;
from a path search perspective, the results of the unmanned boat trajectory segment provide a choice of waypoints and heading for the next search. According to the mapping relationship of the two, there are three possible combinations: a plurality of heading corresponding to one path point, a plurality of path points corresponding to one heading, and one-to-one correspondence between path points and heading (fig. 6).
The combination (1) covers all possible trajectories for which the unmanned vehicle can reach the surrounding points. However, since the number of accessible waypoints is less than the number of headings, the same waypoints have different cost values (corresponding to different situations because of different headings) when calculating the path cost. This will result in some cases (heading) being ignored during the search, which is not conducive to the secondary search of the path. The combination (2) solves the above-mentioned problems. It adds some signposts to ensure that there is only one heading for each accessible waypoint. But because of the existence of repeated heading points (different path points in the same heading), unnecessary searches are generated in the planning process, thereby increasing the search time.
In view of the shortcomings of the first two combinations, the one-to-one mapping of waypoints and heading (combination (3)) is the best choice. One path point corresponds to one heading, so that different path points have only one unique cost value, and redundant search points do not exist on the premise that all necessary headings are considered.
Rule four: the outline shapes of all track segments are squares or based on the squares so as to adapt to the grid map;
the contour shapes (circumscribed rectangles) of the track segments are theoretically diverse, since different track segments generate different waypoints and heading directions. However, in the path search algorithm, a grid map is often used to process the planning space. In order to adapt to the grid map, the reference contour shape of the track segment should be a square grid, the track has more than one square grid, and the shape of the track segment should also be based on the basic square grid (as shown in fig. 7 (1)). If all the track segment contours are not based on a reference contour grid, the tracks cannot completely cover the planning space after being spliced (as shown in fig. 7(2), the shaded part is the area not covered by the tracks), which is obviously not beneficial to the path search.
In conclusion, the discretization rule of the unmanned ship motion track provides specific requirements for the generation of the track unit. The rules together play a role in optimizing the track segments, and the rules require that the initial state and the final state of the track segments are consistent, so that the continuity of the final track after the track segments are spliced is ensured. Rule two plays a role in optimizing the operation of the unmanned ship, limits the steering times of each section of track, ensures the stability of the final track and avoids the frequent steering of the unmanned ship. The rule III plays a role in optimizing path search, selects the one-to-one mapping relation between the path points and the ship heading, simplifies the search process and simultaneously ensures that all possible conditions around are considered. The rule four plays a role in optimizing the planning space, specifies a basic outline taking the grids as track sections, adapts to an environment modeling method of the grid map, and ensures that the planning space is completely covered. In essence, the trajectory discretization rule is a bridge from the ship motion trajectory to the trajectory unit, which lays a solid foundation for the establishment of the next trajectory unit (as shown in fig. 8).
3.3) completing the modeling process of the track units according to the steps 3.1) and 3.2), and establishing a track unit set;
the invention utilizes a grid map as a basic method for environment modeling. During the grid search, a current search often traverses eight grids around (as shown in fig. 9 (1)). Since conventional path planning (route planning) treats the study objects as particles, eight grids around the current point can arrive directly (as shown in fig. 9 (2)).
However, in the motion planning process, the surrounding grid that can be reached by the current point is determined by the current motion state due to the consideration of the dynamic constraint of the study object. Specifically, for the unmanned ship motion planning problem, the surrounding nodes which can be reached by the current node are determined by the current ship heading. The positions of the reachable nodes corresponding to different initial heading directions are different from the corresponding heading directions at the positions. Thus, there are eight possible directions for the heading of the unmanned boat for the eight meshes around the current node. The eight directions can be classified into two categories (as shown in fig. 10) according to symmetry: the type I is horizontal and vertical heading, namely heading is respectively 0 degrees, 90 degrees, 180 degrees and 270 degrees (0 degree is the north direction, and figure 10 (1)); type two, an oblique 45 ° heading, i.e., 45, 135 °, 225 °, and 315 ° heading, respectively (fig. 10 (2)).
The process of building the type one and type two trajectory unit models will be described next, as represented by 0 ° and 45 ° heading, respectively.
1) FIG. 11 shows a first type of modeling based on trajectory units:
when the initial heading of the unmanned ship is 0 degree, the target heading of the track section is 0 degree, 45 degrees, 90 degrees, 270 degrees and 315 degrees because the directions of 135 degrees, 180 degrees and 225 degrees are opposite to the initial heading; for the surrounding mesh that the trajectory segment can eventually reach, nodes d, e, f, g, and h cannot be reached by one steering maneuver due to the constraint of the minimum turning radius. The final reachable path points are therefore only a, b, and c (as shown in fig. 11 (1)). Node b is located right in front of the initial point, and the heading of the node b should maintain the initial heading unchanged and be 0 degrees. Nodes a and c are located to the front left and right of the initial point, respectively, so that the respective ship heading should be 270 ° and 90 °. At this time, eight grids around the current node are all discussed, but heading 45 ° and 315 ° are not allocated by the node. Considering that the two directions are north-west and north-east, respectively, the surrounding points are expanded outward by one layer, and nodes i and j are selected as path points heading 45 ° and 315 ° (as shown in fig. 11 (2)).
Suppose that the current state of the unmanned ship is Sstart(x0,y0,ψ0,v0,δ0) The state after t time is Send(xt,yt,ψt,vt,δt). When the initial heading is 0 °, the trajectory unit mathematical model can be expressed as:
TC＝{Sstart→Send} (1-16)
in the formula, xiAnd yiPosition coordinates for unmanned boat, #iIn the direction of the bow, viIs the ship speed, deltaiThe rudder angle is defined, and a is the side length of the grid. Equations (1-17) are constraints for generating the track units, and equations (1-18) are calculations of five different heading and waypoints for the track units at an initial heading of 0 °.
Based on the above model, when the initial heading is 0 °, the generation process of the track unit is as shown in fig. 12. Since the unit ob is not steered, the track section is a straight line section keeping the heading at 0 degrees. The final heading of the units oa, oc, oi and oj all change so they all have a steering maneuver. The heading changes of the units oa and oc are both 90 degrees, and in the motion modeling of the unmanned ship, the left rudder effect and the right rudder effect are the same under the same rudder angle, so that the absolute values of the command rudder angles are delta1. The heading changes of units oi and oj are both 45 deg., since the heading change is now less than oa and oc, the commanded rudder angle delta2Should be less than delta1(i.e., δ2<δ1). It is worth noting that in a steered trajectory unit, each section of railThe traces all have the operation of steering back (red steering back point). The steering is to ensure that the rudder angle is 0 after the unmanned ship reaches a path point, and the motion state is stable and consistent with the initial state.
From the generation process of this type of trace unit, it can be found that:
1) the rudder angles for the initial and final states of the trajectory segment are both 0, and since the unmanned boat propeller speed is constant (assume 3),
after stable navigation (the rudder angle is 0), the navigation speed of the unmanned boat is unchanged. Thus, rule 1 is satisfied.
2) All the track unit generation processes needing to change the course only have one steering, so that the rule 2 is met.
3) For each reachable waypoint, there is and only a unique heading corresponding to it, thus rule 3 is satisfied.
4) The outline shapes of the basic track cells (oa, ob, and oc) are squares, and the shapes of the cells (oi and oj) exceeding a single square are expanded with reference to the basic squares, and thus satisfy rule 4.
Therefore, the track unit of the type I meets the track discretization rule, and the generated track segment meets the unmanned ship motion planning requirement.
2) As shown in fig. 13, the model is based on the track unit model type two:
in theory, in type one trajectory units, the trajectory segments oa, ob, and oc can handle the basic motion planning problem. Because the track segments with the target heading of 0 °, 90 °, 180 ° and 270 ° can be generated by the above three units being mutually converted. But target heading track sections of 45 degrees and 315 degrees are added into the track unit of the type one in order to make the spliced final track more optimal. Therefore, in order to convert the two heading directions to the original heading direction, the type two needs to be proposed.
When the initial heading of the unmanned ship is 45 °, since the directions of 135 °, 180 °, and 270 ° are opposite to the initial heading, the target heading should be selected from 0 °, 45 °, 90 °, 225 °, and 315 °. Meanwhile, as the main functions of the type two track units are assistance and conversion, only 0 degrees, 45 degrees and 90 degrees of final heading are needed for simplifying the search. For the surrounding mesh that the trajectory segment can finally reach, nodes a, b, d, e, f, g, and h cannot be reached by one steering maneuver, constrained by the minimum turning radius. The final reachable path point can only be c (as shown in fig. 13 (1)), i.e. the rudder angle is not changed, and the final heading remains 45 °. At this time, eight grids around the current node are discussed, but heading 0 ° and heading 90 ° are not allocated by the node. Considering that the two directions are north-east and north-east, respectively, the surrounding points are expanded outward by one layer, and nodes j and k are selected as path points heading 0 ° and 90 ° (as shown in fig. 13 (2)).
Thus, when the initial heading is 45 °, the trajectory unit mathematical model can be expressed (again under the premises of equations (1-16) and (1-17)):
the process of generating the track unit when the initial heading is 45 ° is similar to that when the heading is 0 °, as shown in fig. 14. Trajectory segment oc is a straight line segment that maintains the initial heading, trajectory segments oj and ok are curved segments that steer opposite rudder angles, and both have a return rudder point. Note that the absolute value of the commanded rudder angle at this time is δ3. Since the rudder angle changes the heading by 45 °, it can be considered as the reverse process of the type one in which the heading is changed from 0 ° → 45 ° and 0 ° → 315 °. Thus, δ3≈δ2<δ1. Meanwhile, it is easy to prove that the track unit with the initial heading of 45 degrees also conforms to the 4-track discretization rule.
3) And as shown in fig. 15, the track unit space full coverage verification:
through the introduction of the first two sections, the generation process of the trajectory cell model has been shown to be complete, and both types of cells satisfy the trajectory discretization rule. Before the search algorithm is proposed by using the model, whether the search space can be completely covered needs to be verified. And the space full coverage of the track unit model is verified by taking the heading of 0 degrees and 45 degrees as representatives according to the symmetry.
As shown in fig. 15, fig. 1 and 2 are the case where the track unit reaches the surrounding points (the current point is a blue point, and the surrounding points are green points) when the initial heading is 0 ° and 45 °, respectively. As can be seen from the figure, in both states all eight points around can be reached directly or indirectly by stitching one or more track elements. Therefore, any grid point in the search space can be reached through the combination of the trajectory units, and no missing point exists. The track unit has full coverage.
4) And a track unit control table:
based on the modeling process of the track units, a complete set of track units can be established. In order to understand how to specifically control the drones to reach the corresponding trajectory in the planning process, and provide a reference for motion control, it is necessary to establish a trajectory unit control table (as shown in tables 1-1 to 1-8, wherein H is the heading, RA is the rudder angle (port is negative, starboard is positive), CRA is the command rudder angle, and EH is the return rudder) that can represent the details of the maneuver.
Track unit control table for 1-10 degree initial heading
Table 1-245 degree track unit control table for initial heading
Table 1-390 degree initial heading track unit control table
TABLE 1-4135 initial heading TRACE UNIT CONTROL TABLE
TABLE 1-5180 initial heading Trace Unit control Table
TABLE 1-6225 RAIL UNIT CONTROL TABLE FOR INITIAL FORWARD
Track unit control table for initial heading of table 1-7270 deg
TABLE 1-8315 initial heading Trace Unit control Table
4) Calculating the path cost of all reachable path points to obtain the position of the next path point and a corresponding heading angle;
during the search of the path, the dynamics constraints of the unmanned boat will be converted into these reachable path points and the corresponding heading. Thus, the algorithm converts the motion planning problem into a geometric node search problem.
1. Trace unit abstraction
The track unit is a series of track curve sets with different motion states. And the path search is concerned about the node positions and corresponding directions that the unmanned ship can reach under the current motion state. Therefore, before the search algorithm is proposed, the geometric features of the track unit need to be abstracted.
The abstraction process for type one track unit (exemplified by initial heading 0 °) is shown in fig. 16. When the initial heading is 0 degrees, the path points which can be reached by the unmanned boat are a, b, c, i and j. The node search at this time should be limited to only these five points. Accordingly, the five path points correspond to five different heading directions. The algorithm of the process is expressed as:
if current heading＝＝0°
next waypoint＝a&&next heading＝270° or
next waypoint＝b&&next heading＝0° or
next waypoint＝c&&next heading＝90° or
next waypoint＝i&&next heading＝315° or
next waypoint＝j&&next heading＝45°
end
abstractions of type two trajectory units (exemplified by an initial heading of 45 °) are similar. When the initial heading is 45 degrees, the path points that can be reached by the unmanned boat are c, j and k. The node search at this time should be limited to only these three points. And the three path points correspond to three different heading directions in the same way. The algorithm of the process is expressed as:
if current heading＝＝45°
next waypoint＝c&&next heading＝45° or
next waypoint＝j&&next heading＝0° or
next waypoint＝k&&next heading＝90°
end
overall analysis, the process of abstracting the track unit in all eight directions for both types is shown in fig. 17. As can be seen from the figure, the current node needs to search 16 path points around at a time. These 16 points can be divided into two categories: inner dots (eight green dots) and outer dots (eight blue dots). The interior points are the eight surrounding nodes of a traditional path plan, and each node corresponds to a unique heading. The peripheral points are located one layer outward of the interior points, and these points are both the result of a change in heading direction of 45 ° (i.e., 0 ° → 45 °, 45 ° → 0 °, 45 ° → 90 °, etc.).
Therefore, in a cyclic search process, the algorithm first judges which points are accessible according to the initial heading, and then selects a best path point according to a corresponding strategy. However, how should the search order of the inliers and outliners be determined during the search? Obviously, in free space, the sequence of the inner and outer points has no influence on the final result; however, in the presence of obstacles, different search sequences have a critical effect on whether a safe path can be found.
2. Collision avoidance strategy in obstacle environment
When an obstacle appears in the planned space, the path needs to be subjected to obstacle avoidance planning. In the obstacle avoidance process, the size of the hull of the unmanned ship has direct influence on the final obstacle avoidance path. The constraints on the unmanned boat hull dimensions should also be considered in the motion planning.
The selection of the path point is the key to determine the quality of the final path. Among many optimization strategies, heuristic algorithms are often used to solve path planning problems because they are fast, feasible, simple, efficient, and easy to modify. The core of the algorithm is a cost function f (x):
f(x)＝g(x)+h(x) (2-1)
as can be seen from equation (2-1), the cost function consists of two parts. Wherein g (x) represents the actual cost function from the initial point to the current point (the searched point), and is the cost value of the actual path traveled by the research object; h (x) is a heuristic cost function (or an estimated cost function) from the current point to the end point, and is an estimated value of the path which is left to go. The smaller the value of the cost function f (x), the better the selected waypoint. Therefore, the reachable path points of the unmanned ship are optimally evaluated by using the idea of a heuristic algorithm.
1) Actual cost
In the conventional path planning, because the dynamic constraint is not considered, the quality of the planned path is generally judged by a distance value, and the shorter the distance is, the better the path is. The actual cost function is therefore typically a distance function. However, for unmanned boats, the path is not only related to the distance but also to the actual number of steers. Because a better planned path is not only optimized for the path itself, but also easy to implement in terms of steering control, taking into account the actual steering and control. Generally, the fewer the number of helms, the easier it is to control the planned path. Therefore, the actual cost function of the unmanned ship motion planning is composed of a distance cost function d (x) and a helm cost function s (x).
(1) Distance cost
The distance cost is a common index for judging the quality of the path, and determines the distance of the final path, and the distance has direct influence on some practical problems (such as time saving and fuel saving). Before calculating the actual distance, the trajectory unit should first be analyzed.
The locus units in the eight directions can be classified into five types of curves according to symmetry: 0 ° → 0 °, 0 ° → 45 °, 0 ° → 90 °, 45 ° → 45 ° and 45 ° → 0 °. However, since the curve 45 ° → 0 ° can be regarded as the inverse process of the curve 0 ° → 45 °, the two curves should be regarded as the same kind in terms of distance. Thus, according to the distance criterion, all track units comprise four different types of curve segments (see fig. 18 (1)).
Since the trajectory units 0 ° → 0 ° and 45 ° → 45 ° are both straight line segments, the distance between the two can be directly solved by the euclidean distance between the two points:
the trajectory units 0 ° → 45 ° and 0 ° → 90 ° are curved line segments, and the distance therebetween cannot be calculated by an accurate distance calculation formula. The arc fitting is therefore used herein to approximate the estimate. As shown in fig. 18(2), the curve segment 0 ° → 90 ° (yellow) occupies a grid, and the curve curvature is large, and can be approximated by a quarter of a circular arc with a radius equal to the side length of the grid (yellow dashed line). The curve segment 0 ° → 45 ° (orange) occupies both meshes and the curve curvature is small, so the center of the fitting arc is outside the meshes. Through calculation, when the radius is 2.5 times of the side length of the grid, and the central angle is 53 degrees, the corresponding arc can better fit the curve. Thus, the distance solution for a curve segment can be expressed as:
where θ and r are the central angle and radius of the fitted arc, respectively.
However, the distances discussed above are only actual values and are also standardized for cost. As shown in fig. 19(1), the trajectory units 0 ° → 0 ° (blue), 45 ° → 45 ° (purple), and 0 ° → 90 ° (yellow) each occupy one grid. When in the case of the same start point (point o) and end point (point d), the trajectory unit 45 ° → 45 ° and 0 ° → 90 ° each require only one trajectory segment, and 0 ° → 0 ° requires two segments. So during the planning process, two 0 ° → 0 ° cells are equivalent to one 45 ° → 45 ° or one 0 ° → 90 ° cell. For the trajectory unit 0 ° → 45 °, since it occupies two meshes, the equivalent diagram of the four types of trajectory units is shown in fig. 19(2) after the meshes are expanded to 3 × 3.
As can be readily seen from the figure, the six-stage 0 ° → 0 ° cell, the three-stage 45 ° → 45 ° or 0 ° → 90 ° cell and the 0 ° → 45 ° cell have the same path effect with the same starting point (point a) and ending point (point B). It can also be seen that this is consistent with the relationship between the first three sections of trace elements. Thus, the final trajectory cost can be expressed as:
d(ψ0→ψt)＝dv(ψ0→ψt)·ad (2-4)
in the formula, dv is the actual track segment distance value; a isdIs a distance cost coefficient, and four types of track units adThe ratio of (A) to (B) is 6:3:3: 2.
(2) Cost of steering
The steering cost determines the complexity of the maneuver, while the ease of the maneuver determines the quality of the motion control. The change amount of the initial heading and the final heading of the track unit can be quantized into the actual steering cost value. Thus, the steering cost can be expressed as:
in the formula,. DELTA.psimaxThe maximum heading change quantity in all track units; a isTIs a steering cost coefficient.
As can be seen from the equation (4-5), since the trajectory unit has no change in heading between 0 ° → 0 ° and 45 ° → 45 °, the steering cost is 0; whereas the heading of the units 0 ° → 90 ° and 0 ° → 45 ° changes by 90 ° and 45 °, respectively, so the steering penalty for both is aTAnd 0.5aT(the maximum amount of change in heading is 90 in all track units, i.e., Δ ψmax＝90°)。
The total practical cost can therefore be expressed as:
g(ψ0→ψt)＝d(ψ0→ψt)+s(ψ0→ψt) (2-6)
further, for unreachable path points around the current point, setting the actual cost values of these points artificially to be infinite. So regardless of the heuristic cost of these points, the total cost value (actual cost plus heuristic cost) remains infinite and can be automatically ignored in the path point selection.
2) Heuristic cost
The heuristic cost is an estimate of the future path cost that evaluates which waypoints are most likely to be directed toward the target point. Therefore, the heuristic cost is set to guide the search direction to the target, so that the search time is saved and the calculation amount is reduced. However, the heuristic cost cannot have too strong an impact on the overall cost function. Because the decision takes place in the current state no matter how good the path point is selected. And whether this point is still the best in the next following time is unknown.
Based on the above analysis, manhattan distance is chosen herein as the estimate of the heuristic cost. In some path planning algorithms, there is a heuristic cost of euclidean distance (see fig. 20 (1)). However, the euclidean distance between two nodes is shorter than the manhattan distance, and the euclidean distance cannot well represent the heuristic cost difference of different path points. And the Euclidean distance has an evolution operation in the calculation process, which is contrary to the original purpose of reducing the calculation amount by heuristic cost. Therefore, it is reasonable to select the manhattan distance.
With the assurance of rule 3, different heading points have different manhattan distance values, and the closer the heading angle is to the target point, the smaller the manhattan distance value. For example, when the current heading is 0 ° (as shown in fig. 20 (2)), the reachable path points a, b, c, i, and j all have different manhattan distance values. Obviously, the waypoint j (heading 45 °) is the node closest to the target (green octagon, x), and the manhattan distance value for this point is also the smallest at this time. Thus, the heuristic algorithm can be expressed as:
h(ψ0→ψt)＝|xg-xt|+|yg-yt| (2-7)
in the formula, xgAnd ygIs the position coordinate of the end point.
In the above discussion process, the difference between the heading angle of the path point and the direction angle between the target and the current point may also be used as the criterion of the heuristic cost. The smaller the difference, the closer the candidate waypoint is to the target. Obviously, in free space, the judgment strategy is accurate, and the selected path point can ensure that the final path is optimal. But in the presence of obstacles, this strategy tends to make the planned path fall into local optimality.
3) Search strategy
In one cyclic search process, the algorithm needs to search 16 nodes around the current point. The sixteen points can be divided into inner layer path points and outer layer path points according to the distribution of the nodes. In the environment with obstacles, the accessibility of the inner and outer points has a certain relation.
As shown in fig. 21(1), assuming that the unmanned boat is located at the node o and the heading is 0 °, the inner point is occupied by the obstacle (the red bar-shaped obstacle is located at two points a and b). According to the characteristics of the track unit, the nodes a, b, c, i and j are reachable path points. Since both points a, b are occupied by obstacles, the unmanned boat cannot reach both waypoints (blue and yellow dashed lines). But at the same time, path point i is not reachable. Since the bar-shaped obstacle completely occupies the space position between the two points a and b, the path of the trajectory oi is blocked (broken orange line), and the unmanned boat cannot pass through the obstacle to reach the path point i. The reachability of the path point i is thus related to two points a, b. However, when the peripheral points are occupied by the obstacle, the inner points are reachable without being limited by the peripheral points. Therefore, from the above analysis, the reachability of the outer layer waypoint is related to the corresponding inner layer waypoint, and the reachability of the inner layer waypoint is not directly related to the outer layer waypoint.
Thus, in a search procedure of 16 nodes around, the point association between the outer and inner waypoints needs to be considered. The relevance between the outer layer path point and the inner layer path point according to the characteristics of the track unit is shown in fig. 21 (2). As can be seen from this figure, each outer waypoint is associated with two consecutive inner waypoints: i is associated with two points a and b, j is associated with two points b and c, k is associated with two points c and d, l is associated with two points d and e, m is associated with two points e and f, n is associated with two points f and g, p is associated with two points g and h, and q is associated with two points h and a. Therefore, in the searching process, if two continuous inner layer path points are unreachable, the associated outer layer path point is not reachable. Expressed algorithmically as:
x,y∈inner point；
x adjoin y；
X∈outer point；
if x&&y unreachable
X is unreachable；
end
based on the conclusions of the inner and outer point associations, in the search process of the 16 surrounding nodes, the inner layer path points should be searched first. When the algorithm detects that two continuous inner points are occupied by the barrier and are unreachable, the assigned associated outer layer path point is unreachable, and the whole searching process is divided into an inner layer and an outer layer. Therefore, the search mode at this time is a spiral search (as shown in fig. 22 (1)). However, if the peripheral point is searched first, since the reachability of the internal point is not directly linked to the peripheral point, the whole searching process only increases the number of node searches without structural change. Therefore, the search mode at this time is the same as the conventional search, and is still a circular search (as shown in fig. 22 (2)).
Therefore, to adapt to the characteristics of the track unit and make the search process more efficient, the search strategy of the present algorithm is a spiral search from the inner layer to the outer layer.
Regarding the danger points:
in the actual obstacle avoidance process, the dimension of the research object influences the planned path. Therefore, the size of the hull of the unmanned boat is also a problem to be considered in motion planning research.
It has been analyzed previously that nodes a, b, and i are all unreachable path points. The path points that can be selected by the unmanned boat are only c and j. However, due to the characteristics of the trace unit, it can be seen from fig. 23(1) that the trace segment oj is very close to the node b. When the unmanned ship moves to the vicinity of the node b, there is a high probability that the unmanned ship collides with the bar-shaped obstacle. Thus, node j is also an unreachable waypoint, taking into account the hull dimensions of the unmanned boat. Thus, in this scenario, the navigable waypoint of the final unmanned boat can only be c. Based on the above analysis, it is necessary to discuss the features of the trajectory units to find the trajectory segments with dangerous "approach points".
Since the trajectory unit with a constant heading angle is a straight line segment, there is no "approach point". Although the track unit with the heading angle changed by 90 degrees is a curve segment, the curvature of the track unit is larger, and the track unit is far away from other path points, so that a 'close point' does not exist. However, for a track unit with a 45-degree change of the heading angle, due to the small curvature, the curve is closer to a path point right ahead in the generation process, and the danger of approaching the point exists. Fig. 23(2) lists all trajectory units with a 45 ° change in heading angle, and the black boxes are indicated as "approach points".
Therefore, in order for the unmanned boat to avoid the "approach point" under consideration of the unmanned boat hull dimension constraint, if a waypoint directly in front of the current point is occupied by an obstacle and is not reachable, the change in the heading of the unmanned boat may not be 45 °.
5) And judging whether the path point is the end point of the motion path, if so, outputting the final path, otherwise, taking the path point and the corresponding heading angle as the real-time path point and the heading angle of the unmanned ship, and turning to the step 2).
Application examples of the method of the invention:
as shown in fig. 24, three obstacles are provided, which are respectively rectangular, bar-shaped, and U-shaped. Wherein the U-shaped barrier easily allows the planning to fall into a locally optimal state such that the unmanned vehicle does not reach the terminal point. Meanwhile, the three obstacles are very close to each other, and during the process of passing, the unmanned boat is easy to cause danger because the distance between the boat body and the obstacles is too close. Therefore, the planning path has higher requirements on a motion planning algorithm of the unmanned ship, more planning details need to be considered, and otherwise, failure is easily caused.
Fig. 24 (1) shows the result of route planning. As can be seen from the figure, although the space between the obstacles is narrow, the route planning algorithm still finds a collision-free route between the obstacles, and finally reaches the target point. Fig. 24 (2) - (4) show the results of the exercise program. The initial heading of the unmanned boat in (2) of fig. 24 is 45 ° and 0 °. Since the planned path in fig. 24 (1) is 45 ° in the direction at the beginning, the movement plan of heading 45 ° is directly compared with the route plan. As can be seen from the figure, the movement plan path does not select to cross the space between the bar-shaped obstacle and the U-shaped obstacle (red box). The reason is that the space is too narrow to allow the unmanned boat to safely pass through, considering the hull dimensions of the unmanned boat. The drones then choose to pass around the U-shaped obstacle and reach the target point from the space above it. The experiment with an initial heading of 0 also reaches the target point from the space above. But due to the difference in initial heading, the path first bypasses the rectangular obstacle above and to the right. Fig. 24 (3) shows the case where the initial heading is 270 °. There are two options for this path: turning to port or starboard at the beginning. If it chooses the former, the path will reach the target from the space below; otherwise it will arrive from above. Whatever path is selected by the drones, both are reasonable. The lower path is relatively short, but there is a sharp turn in the initial stage, with a change in heading speed of 180 °, which places high demands on handling. The upper path is longer but all steering angles are smaller. Therefore, the operation is simpler and the realization is easy. Fig. 24 (4) is the result of initial heading directions of 90 ° and 180 °. Both planned paths select the arrival of the target point from the space below. This is because the initial heading makes it easier for the drones to reach the target from below by bypassing the bar-shaped obstacle.
In addition, all the paths planned from the upper space are not sunk into the U-shaped obstacles, so that the accuracy and the effectiveness of the method are proved again. Therefore, based on the planned path, in an obstacle environment, the motion planning result of the unmanned ship focuses not only on solving the shortest path, but also focuses or even focuses more on the ship body dimension and the handling performance of the unmanned ship. This also demonstrates the superiority of the method of the invention compared to route planning algorithms.
It will be understood that modifications and variations can be made by persons skilled in the art in light of the above teachings and all such modifications and variations are intended to be included within the scope of the invention as defined in the appended claims.
Claims (2)
1. A collision avoidance path planning method for an unmanned ship under the condition of obstacles based on a track unit is characterized by comprising the following steps:
1) determining a starting point and an end point of a movement path of the unmanned ship;
2) determining real-time path points and heading angles of the unmanned ship;
3) obtaining reachable path points according to real-time path points and heading angles of the unmanned ship, track units of unmanned ship movement and path points of obstacles;
the generation mode of the track unit is as follows:
3.1) carrying out motion modeling on the unmanned ship by utilizing the MMG model to obtain the motion trail of the unmanned ship;
3.2) restraining the motion trail of the unmanned ship by adopting the unmanned ship trail discretization rule; the following discretization rules are included:
rule one is as follows: at the initial and end moments of the track section, the motion state of the unmanned ship is stable and consistent;
rule two: the number of times of steering of each section of track is not more than one;
rule three: mapping the path points which can be reached by the track segment and the heading which can be changed one by one;
rule four: the outline shapes of all track segments are squares or based on the squares so as to adapt to the grid map;
3.3) completing the modeling process of the track units according to the steps 3.1) and 3.2), and establishing a track unit set;
the reachable path point is obtained in the step 3), and the searching method is as follows:
according to the real-time path point and the heading angle of the unmanned ship and the track unit of the unmanned ship motion, the geometric characteristics of the track unit are utilized to obtain the path point which can be reached from the real-time path point and the corresponding ship heading;
searching reachable inner layer path points according to the reachable path points; the inner layer path points are 8 adjacent points taking the real-time path points as the center;
judging whether path points are associated, and searching reachable outer layer path points if the path points are associated; the outer layer path point is an outer layer associated point of the inner layer path point which takes the real-time path point as the center;
if the acquired reachable path points have dangerous points in the step 3), discarding the dangerous points and then searching; the danger point is a neighboring point between two path points where the obstacle is located;
if the path point right in front of the current path point is occupied by the obstacle and cannot be reached, if the heading of the unmanned ship is changed to 45 degrees, the path point is discarded;
4) calculating the path cost of all reachable path points to obtain the position of the next path point and a corresponding heading angle;
5) and judging whether the path point is the end point of the motion path, if so, outputting the final path, otherwise, taking the path point and the corresponding heading angle as the real-time path point and the heading angle of the unmanned ship, and turning to the step 2).
2. The method for planning collision-avoidance path of unmanned ship under obstacle condition based on track unit as claimed in claim 1, wherein the cost calculation in step 4) is represented by cost function f (x), specifically as follows:
f(x)＝g(x)+h(x)
wherein g (x) represents an actual cost function from an initial point to a current point, and is a cost value of a path actually traveled by a research object; h (x) is a heuristic cost function from the current point to the end point, and is an estimated value of the path which is left;
the actual cost function g (x) is composed of a distance cost function d (x) and a steering cost function s (x), and the heuristic cost function h (x) is represented by Manhattan distance.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811015791.XA CN109240288B (en) | 2018-08-31 | 2018-08-31 | Unmanned ship collision avoidance path planning method based on track unit under condition of obstacle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811015791.XA CN109240288B (en) | 2018-08-31 | 2018-08-31 | Unmanned ship collision avoidance path planning method based on track unit under condition of obstacle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109240288A CN109240288A (en) | 2019-01-18 |
| CN109240288B true CN109240288B (en) | 2021-08-10 |
Family
ID=65060010
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811015791.XA Active CN109240288B (en) | 2018-08-31 | 2018-08-31 | Unmanned ship collision avoidance path planning method based on track unit under condition of obstacle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109240288B (en) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110108280B (en) * | 2019-03-18 | 2023-04-07 | 大连海事大学 | Ship track planning method based on AIS track control unit splicing |
| CN110196598B (en) * | 2019-06-25 | 2022-03-04 | 北京航天控制仪器研究所 | Dynamic collision avoidance method for unmanned ship |
| CN110501014A (en) * | 2019-09-26 | 2019-11-26 | 武汉理工大学 | A kind of unmanned boat motion planning method towards restricted waters |
| CN111798701B (en) * | 2020-07-07 | 2022-07-26 | 中国船舶工业系统工程研究院 | Unmanned ship path tracking control method, system, storage medium and terminal |
| CN111984006B (en) * | 2020-07-24 | 2021-07-06 | 哈尔滨工程大学 | Unmanned ship multi-target meeting collision avoidance method integrating ocean current and scale difference influences |
| CN112363512B (en) * | 2020-11-25 | 2023-08-01 | 中国船舶科学研究中心 | Intelligent ship collision avoidance path planning method based on heading and navigational speed |
| CN112562161A (en) * | 2020-12-08 | 2021-03-26 | 福建红点科技有限公司 | Based on 5G thing networking and AI intelligent entrance guard integrated management system |
| CN113885533B (en) * | 2021-11-12 | 2022-08-09 | 江苏海洋大学 | Unmanned driving method and system of unmanned boat |
| CN114194342A (en) * | 2021-12-29 | 2022-03-18 | 浙江大学湖州研究院 | Unmanned ship and unmanned ship control system |
| CN115903918A (en) * | 2023-01-05 | 2023-04-04 | 中国人民解放军军事科学院系统工程研究院 | Unmanned system toughness control method and device |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106845716A (en) * | 2017-01-25 | 2017-06-13 | 东南大学 | A kind of unmanned surface vehicle local delamination paths planning method based on navigation error constraint |
| CN107544500A (en) * | 2017-09-18 | 2018-01-05 | 哈尔滨工程大学 | A kind of unmanned boat berthing action trail planing method for considering constraint |
| CN207008408U (en) * | 2017-07-31 | 2018-02-13 | 武汉理工大学 | A kind of pod propulsion unmanned boat steering gear control system based on CAN |
| CN107966153A (en) * | 2017-11-24 | 2018-04-27 | 中国海洋大学 | Submarine navigation device path planning algorithm |
| CN108334086A (en) * | 2018-01-25 | 2018-07-27 | 江苏大学 | A kind of automatic driving vehicle path tracking control method based on soft-constraint quadratic programming MPC |
| CN108388250A (en) * | 2018-03-30 | 2018-08-10 | 哈尔滨工程大学 | A kind of unmanned surface vehicle paths planning method based on adaptive cuckoo searching algorithm |
| CN108416152A (en) * | 2018-03-18 | 2018-08-17 | 哈尔滨工程大学 | The optimal global path planning method of unmanned boat ant colony energy consumption based on electronic chart |
| CN108445879A (en) * | 2018-03-12 | 2018-08-24 | 上海大学 | A kind of unmanned boat barrier-avoiding method based on prediction collision risk region |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| IL196276A (en) * | 2008-12-30 | 2013-04-30 | Elbit Systems Ltd | Autonomous navigation system and method for a maneuverable platform |
| JP2015203938A (en) * | 2014-04-14 | 2015-11-16 | 株式会社Ihi | Abnormal-time return control method for autonomous vehicle and autonomous vehicle for use to implement abnormal-time return control method |
| US9436187B2 (en) * | 2015-01-15 | 2016-09-06 | The United States Of America As Represented By The Secretary Of The Navy | System and method for decentralized, multi-agent unmanned vehicle navigation and formation control |
| US10019006B2 (en) * | 2015-04-08 | 2018-07-10 | University Of Maryland, College Park | Surface vehicle trajectory planning systems, devices, and methods |
| US10719086B2 (en) * | 2016-07-13 | 2020-07-21 | Flytrex Aviation Ltd. | System and method for dynamically updated unmanned vehicle navigation planning |
| CN107037809A (en) * | 2016-11-02 | 2017-08-11 | 哈尔滨工程大学 | A kind of unmanned boat collision prevention method based on improvement ant group algorithm |
| CN107197806B (en) * | 2017-05-17 | 2019-07-16 | 武汉理工大学 | Spring louvers formula based on unmanned boat pinpoints Autoamtic bait putting method |
| CN108445894A (en) * | 2018-06-15 | 2018-08-24 | 哈尔滨工程大学 | A kind of secondary paths planning method considering unmanned boat movenent performance |
-
2018
- 2018-08-31 CN CN201811015791.XA patent/CN109240288B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106845716A (en) * | 2017-01-25 | 2017-06-13 | 东南大学 | A kind of unmanned surface vehicle local delamination paths planning method based on navigation error constraint |
| CN207008408U (en) * | 2017-07-31 | 2018-02-13 | 武汉理工大学 | A kind of pod propulsion unmanned boat steering gear control system based on CAN |
| CN107544500A (en) * | 2017-09-18 | 2018-01-05 | 哈尔滨工程大学 | A kind of unmanned boat berthing action trail planing method for considering constraint |
| CN107966153A (en) * | 2017-11-24 | 2018-04-27 | 中国海洋大学 | Submarine navigation device path planning algorithm |
| CN108334086A (en) * | 2018-01-25 | 2018-07-27 | 江苏大学 | A kind of automatic driving vehicle path tracking control method based on soft-constraint quadratic programming MPC |
| CN108445879A (en) * | 2018-03-12 | 2018-08-24 | 上海大学 | A kind of unmanned boat barrier-avoiding method based on prediction collision risk region |
| CN108416152A (en) * | 2018-03-18 | 2018-08-17 | 哈尔滨工程大学 | The optimal global path planning method of unmanned boat ant colony energy consumption based on electronic chart |
| CN108388250A (en) * | 2018-03-30 | 2018-08-10 | 哈尔滨工程大学 | A kind of unmanned surface vehicle paths planning method based on adaptive cuckoo searching algorithm |
Non-Patent Citations (5)
| Title |
|---|
| Autonomous obstacle avoidance of an Unmanned Surface Vehicle based on cooperative maneuvering;Wu Peng，等;《Industrial Robot: An International Journal》;20170114;第44卷(第1期);64-74页 * |
| Motion planning for Unmanned Surface Vehicle based on Trajectory Unit;Zhe Du,等;《Ocean Engineering》;20180301(第151(2018)期);46-56页 * |
| 一种基于有限状态机模型的局部转向避碰路径规划算法;汪栋，等;《海洋科学》;20180131;第42卷(第1期);119-127页 * |
| 基于位置变化的轨迹单元划分及索引机制;潘鹏，等;《小型微型计算机系统》;20061130;第27卷(第11期);2120-2124页 * |
| 基于动态复杂度地图的船舶航迹规划;杜哲，等;《系统仿真学报》;20180630;第30卷(第6期);2390-2397、2404页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109240288A (en) | 2019-01-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109240288B (en) | Unmanned ship collision avoidance path planning method based on track unit under condition of obstacle | |
| CN108876065B (en) | Unmanned ship motion planning method based on track unit | |
| CN111123903B (en) | Unmanned ship obstacle avoidance method based on circular track unit | |
| Han et al. | Automatic berthing for an underactuated unmanned surface vehicle: A real-time motion planning approach | |
| Guan et al. | Autonomous collision avoidance of unmanned surface vehicles based on improved A-star and dynamic window approach algorithms | |
| CN113759939B (en) | Intelligent navigation method and device for limited water area | |
| Du et al. | Motion planning for unmanned surface vehicle based on trajectory unit | |
| CN108459602B (en) | Autonomous berthing method of under-actuated unmanned ship in multi-obstacle complex environment | |
| CN110083159B (en) | Unmanned ship autonomous dynamic collision avoidance method based on SBG and dynamic window constraint | |
| Shi et al. | Study on intelligent collision avoidance and recovery path planning system for the waterjet-propelled unmanned surface vehicle | |
| CN109597417A (en) | A kind of more USV multi-agent synergy collision-avoidance planning methods based on collision prevention criterion | |
| Zhou et al. | Motion planning for an unmanned surface vehicle based on topological position maps | |
| Du et al. | Trajectory-cell based method for the unmanned surface vehicle motion planning | |
| CN103576685A (en) | Method for determining path of UUV in process of recycling mother ship | |
| CN117250960A (en) | Surface boat path planning method, device, equipment and storage medium | |
| CN116360434A (en) | Ship path planning method based on improved CSAC-APF algorithm | |
| CN111176281A (en) | Multi-surface unmanned ship coverage type collaborative search method and system based on quadrant method | |
| CN116952239A (en) | Unmanned ship path planning method based on fusion of improved A and DWA | |
| Liu et al. | Motion planning for unmanned surface vehicle based on a maneuverability mathematical model | |
| Liang et al. | Economic MPC-based planning for marine vehicles: Tuning safety and energy efficiency | |
| Li et al. | A Global Dynamic Path Planning Algorithm Based on Optimized A* Algorithm and Improved Dynamic Window Method | |
| CN111679674B (en) | Flexible meeting evasion method for unmanned ship | |
| Wang et al. | Stepwise cooperative trajectory planning for multiple BUVs based on temporal–spatial Bezier curves | |
| Kagabo et al. | Trajectory determination for energy efficient autonomous soaring | |
| Zhang et al. | A new hybrid path planning method for the sailboat architecture wave glider in the wind field 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 |