CN109240288A - Unmanned boat collision prevention paths planning method in the case of a kind of barrier based on trajectory unit - Google Patents
Unmanned boat collision prevention paths planning method in the case of a kind of barrier based on trajectory unit Download PDFInfo
- Publication number
- CN109240288A CN109240288A CN201811015791.XA CN201811015791A CN109240288A CN 109240288 A CN109240288 A CN 109240288A CN 201811015791 A CN201811015791 A CN 201811015791A CN 109240288 A CN109240288 A CN 109240288A
- Authority
- CN
- China
- Prior art keywords
- point
- path
- unmanned boat
- bow
- trajectory unit
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
Abstract
The invention discloses unmanned boat collision prevention paths planning methods in the case of a kind of barrier based on trajectory unit, comprising the following steps: 1) determines the beginning and end of unmanned boat motion path；2) determine the point of real-time route locating for unmanned boat and bow to angle；3) path point locating for the trajectory unit and barrier that the point of the real-time route according to locating for unmanned boat and bow are moved to angle, unmanned boat, seeks reachable path point；4) path cost calculating is carried out to all reachable path points, obtains next waypoint position and corresponding bow to angle；5) judge whether the path point is that the terminal of motion path then exports final path if terminal, otherwise, using the path point and corresponding bow to angle as the point of real-time route locating for unmanned boat and bow to angle, be transferred to step 2).The present invention establishes unmanned boat trajectory unit model, so that the path of planning requires during real navigation closer to actual navigation, and can automatic obstacle avoiding.
Description
Technical field
The present invention relates to nothings in the case of maritime affairs intelligent transport technology more particularly to a kind of barrier based on trajectory unit
People's ship collision prevention paths planning method.
Background technique
The development of artificial intelligence and the exploitation of marine resources are so that unmanned surface vehicle (USV) is born respectively more and more
Kind task waterborne.And can important a part as unmanned boat independence, path planning be before completing every task waterborne
It mentions.But since unmanned boat controllable degree is lower, freedom degree is higher, so should change from particle when it is as research object
It is studied for drive lacking rigid body.Correspondingly, path planning problem also should be changed into motion planning from route planning.With
Unlike the former, motion planning not only allows for the constraint of planning space, while also at large being begged for planning behavior
By.And planning behavior is related with the movement of research object and Dynamic Constraints.Therefore in order to fully consider unmanned boat movement and
Dynamic Constraints, this paper presents the unmanned boat motion planning methods for being based on " trajectory unit ".
Summary of the invention
The technical problem to be solved in the present invention is that for the defects in the prior art, providing a kind of based on trajectory unit
Barrier in the case of unmanned boat collision prevention paths planning method.
The technical solution adopted by the present invention to solve the technical problems is: a kind of barrier situation based on trajectory unit
Lower unmanned boat collision prevention paths planning method, comprising the following steps:
1) beginning and end of unmanned boat motion path is determined；
2) determine the point of real-time route locating for unmanned boat and bow to angle；
3) trajectory unit and barrier that the point of the real-time route according to locating for unmanned boat and bow are moved to angle, unmanned boat
Locating path point seeks reachable path point；
The generating mode of the trajectory unit is as follows:
3.1) motion modeling is carried out to unmanned boat using MMG model, obtains the motion profile of unmanned boat；
3.2) motion profile of unmanned boat is constrained using unmanned boat track discretization rule；Including following discrete
Change rule:
Rule one: orbit segment is initial and finish time, the motion state of unmanned boat is stable and is consistent；
The number of coming about of regular two: every section tracks is no more than once；
Rule three: the orbit segment attainable path point of institute and the bow that can change to mapping one by one；
Rule four: the chamfered shape of all orbit segments is grid or is based on grid, to adapt to grating map；
3.3) modeling process of trajectory unit is completed according to step 3.1) and 3.2), and establishes trajectory unit collection；
4) path cost calculating is carried out to all reachable path points, obtains next waypoint position and corresponding bow
To angle；
5) judge whether the path point is that the terminal of motion path then exports final path if terminal, otherwise, by this
Path point and corresponding bow to angle as the point of real-time route locating for unmanned boat and bow to angle, be transferred to step 2).
According to the above scheme, the step 3) kind seeks reachable path point, and searching method is as follows:
The trajectory unit that the point of the real-time route according to locating for unmanned boat and bow are moved to angle and unmanned boat, utilizes track
The geometrical characteristic of unit, obtain the path point that can be reached from real-time route point and corresponding stem to；
According to the path point that can be reached, reachable internal layer path point is searched for；The internal layer path point is with locating reality
When path point centered on 8 point of proximity；
Path point association is judged whether there is, reachable outer layer path point is searched for if having；The outer layer path point is with institute
The outer layer relating dot of internal layer path point centered on the real-time route point at place.
According to the above scheme, if dangerous point in the reachable path point sought in the step 3), cast out dangerous point
Afterwards, it then scans for；Neighbor point of the dangerous point between two path points as locating for barrier.That is dangerous point and barrier
Hindering two in path point set locating for object path points is consecutive points, and between the two path points.
According to the above scheme, if in the reachable path point sought in the step 3), if the road immediately ahead of current path point
When diameter point is occupied by barrier and is unreachable, if the bow of unmanned boat to changing into 45 °, casts out the path point.
According to the above scheme, the cost in the step 4) is calculated is indicated by cost function f (x), specific as follows:
F (x)=g (x)+h (x)
Wherein, g (x) indicates that initial point is the generation in the practical path of passing by of research object to the actual cost function of current point
Value；H (x) is heuristic cost function of the current point to terminal, is to the remaining estimated value for not walking path also；
Wherein actual cost function g (x) is made of distance cost function d (x) and the cost function s (x) that comes about, heuristic
Cost function h (x) is indicated by manhatton distance.
The beneficial effect comprise that:
1. the present invention effectively solves unmanned boat by establishing a kind of trajectory unit model of unmanned boat motion planning
The big critical issue of the two of motion planning is unmanned boat Dynamic Constraints expression problem and unmanned boat Dynamic Constraints respectively and empty
Between the combination problem searched for.
Model solves unmanned boat Dynamic Constraints expression problem using the motion profile of unmanned boat.The movement of unmanned boat
Track can not only completely express its Dynamic Constraints, and the connection of each about interfascicular is also contained in geometric locus；
In view of space search is the process of spatial discretization, model is advised according to the track feature and movement of unmanned boat
The search need drawn proposes four track discretization rules.Rule not only by continuous track it is discrete be orbit segment, make every
One step space search all contains unmanned boat Dynamic Constraints；And it is still maintained after being spliced these orbit segments
The continuity of final track, to solve the problems, such as the combination of Dynamic Constraints and space search.
2, the present invention establishes unmanned boat trajectory unit model, and proposes the unmanned boat movement based on trajectory unit
Planning algorithm, so that the path of planning is during real navigation closer to actual navigation requirement.
3, passage path relating dot of the present invention and dangerous point judgement reduce the retrieval time of path planning, and can be autonomous
Avoidance.
Detailed description of the invention
Present invention will be further explained below with reference to the attached drawings and examples, in attached drawing:
Fig. 1 is the method flow diagram of the embodiment of the present invention；
Fig. 2 is the fluid dynamic schematic diagram for acting on hull of the embodiment of the present invention；
Fig. 3 is unmanned boat of the embodiment of the present invention or so turning circle analysis schematic diagram；
Fig. 4 is the spliced continuous path section schematic diagram of the embodiment of the present invention；
Fig. 5 is number of the coming about schematic diagram no more than once of the embodiment of the present invention；
Fig. 6 be the embodiment of the present invention path point and bow to corresponding schematic diagram；
Fig. 7 is that the chamfered shape of the track of the embodiment of the present invention is based on grid schematic diagram；
Fig. 8 is that the trajectory unit of the embodiment of the present invention generates schematic diagram；
Fig. 9 is the traversal schematic diagram of grid around the grating map of the embodiment of the present invention；
Figure 10 be the embodiment of the present invention unmanned boat bow to type schematic diagram；
Path point that Figure 11 is the embodiment of the present invention and possible bow are to schematic diagram；
Figure 12 be the embodiment of the present invention initial bow to be 0 ° when, the generating process schematic diagram of trajectory unit；
Path point that Figure 13 is the embodiment of the present invention and possible bow are to schematic diagram；
Figure 14 is the generating process schematic diagram of the trajectory unit of the embodiment of the present invention；
Figure 15 is the space all standing verifying schematic diagram of the embodiment of the present invention；
Figure 16 is the trajectory unit abstract schematic diagram of the embodiment of the present invention；
Figure 17 is all trajectory units abstract schematic diagram of the embodiment of the present invention；
Figure 18 be the embodiment of the present invention orbit segment curve and apart from calculate schematic diagram；
Figure 19 is the standardization schematic diagram of the orbit segment of the embodiment of the present invention；
Figure 20 is the standardization schematic diagram of the orbit segment of the embodiment of the present invention；
Figure 21 is that the internal layer path point of the embodiment of the present invention with the point of outer layer path point is associated with schematic diagram；
Figure 22 is the spiral search schematic diagram of the embodiment of the present invention；
Figure 23 is dangerous point schematic diagram in the obstacle environment of the embodiment of the present invention；
Figure 24 be the embodiment of the present invention obstacle environment under unmanned boat route planning and motion planning contrast schematic diagram.
Specific embodiment
In order to make the objectives, technical solutions, and advantages of the present invention clearer, with reference to embodiments, to this hair
It is bright to be further elaborated.It should be appreciated that described herein, specific examples are only used to explain the present invention, and does not have to
It is of the invention in limiting.
As shown in Figure 1, unmanned boat collision prevention paths planning method in the case of a kind of barrier based on trajectory unit, including
Following steps:
1) beginning and end of unmanned boat motion path is determined；
2) determine the point of real-time route locating for unmanned boat and bow to angle；
3) trajectory unit and barrier that the point of the real-time route according to locating for unmanned boat and bow are moved to angle, unmanned boat
Locating path point seeks reachable path point；
The generating mode of the trajectory unit is as follows:
3.1) motion modeling is carried out to unmanned boat using MMG model, obtains the motion profile of unmanned boat；
It is illustrated in figure 2 fluid dynamic (torque) schematic diagram for acting on hull, the method for Modeling of Ship Motion can be divided into
Two major sects: one is the monolithic devices model structure using America and Europe as representative, and main thought is to regard hull, propeller and rudder
For indivisible entirety；Another group is the divergence type model structure using Japan as representative, commonly referred to as MMG
(Maneuvering Mathematical Group) model.The main thought of MMG model is to be decomposed into hydrodynamic force (torque)
Act on ship, paddle, the component (torque) on rudder.The power (torque) that wherein hull is subject to can be divided into viscous effect power (torque) again
With inertial force (torque).Therefore MMG model is made of Viscosity Model, inertial model, model propeller and rudder model
's.
Since the adaptation scene of the model is relatively wide, flexibility and changeability, is easy to carry out partial modification for specific type of ship, simultaneously
Ship-paddle-rudder interference effect can also compactly be expressed.Therefore, the present invention move to unmanned boat and be built using MMG model
Mould.
1), based on the unmanned boat motion modeling of MMG model
In order to simplify problem, need to do before modeling it is assumed hereinafter that:
Assuming that 1, with the plane motion (Three Degree Of Freedom) of unmanned boat for research emphasis, ignore heaving in its motion process,
Roll and pitch.
Assuming that 2, do not consider the influence that the natural environments such as wind, wave, stream move unmanned boat.
Assuming that a case where 3, consideration marine main engine rotates forward.And after unmanned boat stable sea, revolution speed of propeller is kept
It is constant.
For hypothesis 3, it is necessary to remark additionally.When ship is in the state of boat, hull will receive the resistance of water flow
Power.To make the load of propeller increase, lead to revolving speed (n) reduction.At this point, speed control will improve main engine power, to support
The increased load of the propeller that disappears, and maintain original revolving speed.In above process, host is under the adjusting of rotational speed governor, control
Simulation may be expressed as:
Wherein, TDIt is time constant, IEIt is the moment of inertia of whole system, kpIt is the gain of speed control, nrIt is that order turns
Speed, K are the gain of main transmission bar to torque output, QPIt is that propeller absorbs torque.
Therefore, unmanned boat in advance (X), traversing (Y), turn the three-degree-of-freedom motion model under bow (N) and may be expressed as:
Wherein I, H, P, R are the power (torque) that inertia, viscosity, propeller and rudder generate respectively.
(1) inertial model
Ship can cause the disturbance of surrounding fluid during navigation, so that fluid media (medium) be made to generate additional momentum (momentum
Square).Wherein, added moment of inertia may be expressed as:
M in formulaxAnd myFor the additional mass in X-axis and Y direction；αxFor myCenter of gravity is acted in the coordinate value of X-axis, Jzz
For the inertia additional moment for acting on Z-direction.Therefore additional momentum and the moment of momentum may be expressed as:
U and v is respectively the speed of X-axis and Y direction in formula, and r is yawing angular speed.To formula (1-4) in respective direction
On differentiate and obtain the inertia force and moment of inertia of fluid:
(2) Viscosity Model
Viscous fluid power and torque suffered by ship and hull geometry feature, ship motion state and fluid physics
Characteristic is related.Therefore when ship type and constant flow field, the viscous-dynamic and torque acted on hull depends primarily on ship and works as
Preceding motion state (i.e. u, v and r):
In formula, X (u) is the direct route resistance of ship；Xvvv2、Xvrvr、XrrR is caused by ship moves in fluid media (medium)
Viscous drag；Yvv、YrR and Nvv、NrR is respectively linear transverse hydrodynamic force and torque, YnlHAnd NnlHIt is then nonlinear terms.
In general, the size of drift angle has the viscous force and torque that act on hull when ship moves in fluid media (medium)
Large effect.Due to not considering the influence (assuming that 2) of wind, wave, stream to ship herein, thus ship during navigation not
Drift angle can be generated.Then your island formula nonlinear hydrodynamic and torque (Y to be estimated by hereinnlHAnd NnlH):
By (1-6) formula and (1-7) formula simultaneous, it can obtain final result:
In practical applications in order to simplify problem, indicated sometimes using the second Taylor series formula viscous fluid power and
The approximation of torque:
It is the direct route resistance that X0 is ship in formula, Δ u is the knots modification of speed in X-direction, Xu, Yv, Yr, Nv and Nr
For hydrokinetic derivative.
(3) model propeller
Propeller is the main source of ship power, and while providing thrust, propeller can also generate lateral effect
Power (torque).But the influence generated compared to rudder, the effect very little of the cross force (torque) can be ignored.Therefore, rudder mould
Type can indicate are as follows:
T is the thrust that propeller generates, t in formulaPFor thrust deduction coefficient.Thrust deduction coefficient is thrust deduction and push away
The ratio of power, it is determined by several factors, such as the size of ship and propeller.And thrust T and propeller disc that propeller generates
Face diameter DP, revolving speed n, the factors such as fluid density ρ it is related.Thrust calculating formula can approximate estimation are as follows:
T=ρ n2Dp 2KT(JP) (1-11)
K in formulaTFor thrust coefficient；JPFor advanced coefficient.The two can be expressed as again
A in formulaiIt (i=0,1,2) is regression coefficient；wPIt is wake factor, when resulting from ship loxodrome on propeller.
(4) rudder model
Rudder model is most important a part in model, because it determines the direction of ship movement, and then determines ship
The motion profile of oceangoing ship.Generally by water flow to the normal pressure of rudder according to three degree of freedom be decomposed into longitudinal resistance, laterally traversing power with
And after rudder moment, rudder model can be obtained:
δ is rudder angle in formula；tRFor resistance derating coefficient；aHFor correction factor；xHFor rudder stress action spot to ship weight
The fore-and-aft distance of the heart；FNFor the normal pressure of rudder, it be may be expressed as:
U in formulaREnter effective rudder speed of rudder for fluid；ARFor effective area of rudder；αRFor effective angle of attack；faFor the oblique of lift
Rate.
Track in order to facilitate control ship to be needed, it is also necessary to establish the Controlling model of rudder:
δ in formulaEFor order rudder angle；TEFor time constant, about 2.5 seconds.
2) unmanned boat motion trail analysis
In order to verify the accuracy of the model, it is necessary to which the motion profile generated to model is analyzed.
Fig. 3 is under certain revolving speed, and unmanned boat grasps the turning circle track of two sides of a ship of left and right after identical rudder angle.It can be with from figure
See, the radius of turning circle of larboard is (R more bigger than starboardP>RS).The phenomenon belongs to normal in fact, because the quality of a ship is difficult
Accomplish that keeping absolute in two sides of a ship of left and right is averaged.So under normal circumstances, the center of gravity of ship not at the center line on.This is also demonstrate,proved
The model is illustrated to tally with the actual situation.And it can be seen that the difference of the two is not very big.Therefore in order to simplify problem,
Herein when establishing unmanned boat trajectory unit, it is believed that the motion profile of two sides of a ship be it is identical, that is, have bilateral symmetry.
In addition, it has also been found that another phenomenon, i.e., the initial stage, there are backward shift amount (kick) from figure.Backward shift amount is
Refer to the ship center of gravity distance traversing to steering opposite direction at cycle initial stage.Under normal circumstances, the value very little of backward shift amount, and most
It is no more than the 1% of its captain greatly.The factors such as usual ship's speed, rudder angle and ship type determine the size of backward shift amount, wherein ship's speed and rudder
Angle and backward shift amount are proportional.Green box is the enlarged drawing in unmanned boat initial cycle stage on the right of Fig. 3.It can be seen that
There are a very small reversed traversing process, i.e. backward shift amount in two side of a ship of or so cycle initial stage.It can thus be concluded that going out, the motion model
Meet ship actual motion process.
Therefore by analyzing above.Unmanned boat motion modeling based on MMG model is accurately and reliably, to meet practical fortune
Emotionally condition.
3.2) motion profile of unmanned boat is constrained using unmanned boat track discretization rule；Including following discrete
Change rule:
Rule one: orbit segment is initial and finish time, the motion state of unmanned boat is stable and is consistent；
In order to make it is discrete after orbit segment be spliced after still maintain continuity, a feasible way is in orbit segment
Initial and finish time keeps the motion state of unmanned boat stable and is consistent.Specifically there are two require: (1) rudder angle is
0；(2) speed is consistent.As shown in Figure 4.
Rudder angle be 0 be in order to maintain navigation stability.When rudder is fixed on certain rudder angle, ship can be in always
Turning motion state.Every kinematic parameter of ship changes in real time at this time, is unfavorable for carrying out state transition operation.And work as
When rudder angle is 0, ship is in direct route state.In the case where the environmental disturbances such as no wind, wave, stream, ship can be stablized at this time
On a certain course.This stable state is conducive to smoothly be transferred to next state from current state when track is spliced.
And speed is consistent and not only ensure that in the splicing of track, velocity magnitude is identical；And but also spelling
On contact, the curvature of front and back geometric locus is consistent.Because different ship's speed corresponds to different turning radius.And work as speed
One timing, turning radius remain unchanged, and curvature will not change.
The number of coming about of regular two: every section tracks is no more than once；
Unit refers to indivisible amount, therefore the orbit segment in a unit should be simple as far as possible.And judge rail
The standard of mark complexity is the number that it is turned to, number of coming about for unmanned boat.Therefore, turn of an orbit segment
Rudder number should not be too many；Otherwise spliced track will generate a large amount of turning points, this is clearly inappropriate for practical application
's.This rule requires during generating orbit segment, and number of coming about should not be more than primary (as shown in figure 5, not including back
Rudder).
Rule three: the orbit segment attainable path point of institute and the bow that can change to mapping one by one；
From the perspective of route searching, the result of unmanned boat orbit segment provides path point and bow for search next time
To selection.According to the mapping relations of the two, there are three types of possible combinations: the corresponding multiple bows of a path point are to a bow
To corresponding multiple path points, path point and bow are to one-to-one correspondence (Fig. 6).
Combination (1), which covers unmanned boat, can reach all possible track around put.But due to accessible paths point
Number be less than bow to number, when calculating path cost, identical path point has different cost values (because of different
Bow to corresponding different situation).This will lead in search process certain situations (bow to) and is ignored, and be unfavorable for path
Binary search.Combination (2) solves the problems, such as above-mentioned.It increases some road signs, to ensure each addressable path point
Only one bow to.But it since there are duplicate bows to point (identical bow to have different path points), can be produced in planning process
Raw unnecessary search, to increase search time.
The shortcomings that in view of the first two combination, path point and bow are best selections to (combination (3)) is mapped one by one.One
The corresponding bow of a path point is to there is different path points and only unique cost value, but also all necessary
Bow is under the premise of being all taken into account, and there is no the Searching points of redundancy.
Rule four: the chamfered shape of all orbit segments is grid or is based on grid, to adapt to grating map；
Since different orbit segments generates different path point and bow to the chamfered shape (boundary rectangle) of orbit segment exists
Theoretically various.But in path search algorithm, often planning space is handled using grating map.
In order to adapt to grating map, the baseline profile shape of orbit segment should be grid, and track is more than a grid, and shape is also answered
It should be based on this basic grid (shown in such as Fig. 7 (1)).If all orbit segment profiles are based not on the wheel of a benchmark
Wide grid, planning space cannot be completely covered after these tracks are spliced, and (shown in such as Fig. 7 (2), dash area is not have
The region covered by track), this is obviously unfavorable for the search in path.
In conclusion the generation that unmanned boat motion profile discretization rule is trajectory unit proposes specific requirement.Rule
Then one plays the role of optimizing orbit segment, it requires orbit segment starting and end-state to be consistent, and ensure that orbit segment is spelled
After connecing, the continuity of final track.Rule two plays the role of optimizing unmanned boat manipulation, it limits the behaviour of each section of track
Rudder number ensure that the stability of final track, avoid the frequent steering of unmanned boat.Rule three plays optimal path selection
Effect, it has selected path point and ship bow to the relationship mapped one by one, while simplifying search process, ensure that
The all possible situation of surrounding is taken into account.Rule four plays the role of optimization planning space, it is provided using grid as orbit segment
Base profile, adapted to the environmental modeling method of grating map, ensure that planning space is completely capped.Substantially, track
Discretization rule is bridge of the ship motion profile to trajectory unit, it has laid solid for the foundation of following trajectory unit
Basic (as shown in Figure 8).
3.3) modeling process of trajectory unit is completed according to step 3.1) and 3.2), and establishes trajectory unit collection；
The present invention is using grating map as the basic skills of environmental modeling.During grid search, once currently search
Rope will often traverse eight grids around (shown in such as Fig. 9 (1)).Since traditional path planning (route planning) will research pair
As being considered as particle, so eight grids around current point can be reached directly (shown in such as Fig. 9 (2)).
But during motion planning, due to considering the Dynamic Constraints of research object, what current point can reach
Surrounding grid is determined by current motion state.Specifically, for unmanned boat motion planning problem, present node institute
The surroundings nodes that can be arrived are from current stem to decision.Different initial bows is to correspondence up to the position and the position of node
Upper corresponding stem is to being different.Therefore, for eight grids around present node, the stem of unmanned boat is to there is eight
Possible direction.And according to symmetry, this eight bows are to can be divided into two major classes (as shown in Figure 10): type one, horizontal vertical
Bow is to that is, bow is to respectively 0 °, 90 °, 180 ° and 270 ° (0 ° is direct north, Figure 10 (1))；Type two, oblique 45 ° of bows are to that is,
Bow is to respectively 45,135 °, 225 ° and 315 ° (Figure 10 (2)).
Next it is represented with 0 ° and 45 ° of bow Xiang Wei, to introduce the foundation of two trajectory unit model of type one and type respectively
Process.
1), as shown in figure 11 for based on trajectory unit modeling type one:
When the initial bow of unmanned boat to be 0 ° when, since 135 °, 180 ° and 225 ° of direction and initial bow are on the contrary, therefore track
The target bow of section is to should be 0 °, 45 °, 90 °, 270 ° and 315 °；For reachable surrounding's grid final for orbit segment, due to
By the constraint of minimum turning radius, node d, e, f, g and h cannot be reached by primary manipulation of coming about.Therefore final can
It is only a, b and c up to path point (shown in such as Figure 11 (1)).Node b is located at the front of initial point, and bow is to should maintain just
Beginning bow is 0 ° to constant.Node a and c are located at the left front and right front of initial point, therefore corresponding stem is to should be
270 ° and 90 °.Eight grids have all been discussed around present node at this time, but bow is not distributed by node to 45 ° and 315 °.
In view of the two directions are respectively north by west and north by east, surrounding point is extended to the outside one layer, select node i and j for bow to
45 ° and 315 ° of path point (shown in such as Figure 11 (2)).
Assuming that unmanned boat current state is Sstart(x0,y0,ψ0,v0,δ0), the state after the t time is Send(xt,yt,
ψt,vt,δt).When initial bow to be 0 ° when, trajectory unit mathematical model may be expressed as:
TC={ Sstart→Send} (1-16)
In formula, xiAnd yiFor the position coordinates of unmanned boat, ψiIt is stem to viFor ship's speed, δiFor rudder angle, a is side length of element.
Formula (1-17) is to generate the constraint of trajectory unit, formula (1-18) be initial bow to be 0 ° when trajectory unit five differences
Bow is to the calculating with path point.
Based on above-mentioned model, initial bow to be 0 ° when, the generating process of trajectory unit is as shown in figure 12.Since unit ob does not have
It comes about manipulation, orbit segment is to keep bow to the straightway for 0 °.Unit oa, oc, oi and oj final bow changes to having
Become, therefore they come about manipulation.The bow of unit oa and oc to change be 90 °, and in the motion modeling of unmanned boat,
Define under identical rudder angle that two side of a ship steerages of left and right are identical, and therefore, the absolute value of order rudder angle is δ1.The bow of unit oi and oj
It is 45 ° to change, since bow at this time is less than oa and oc to changing, so order rudder angle δ2It should be less than δ1(that is, δ2<δ1).Value
It obtains it is noted that there is the operation eased the helm in every section of track in the trajectory unit for having steering (red eases the helm a little).The manipulation
It is to guarantee that rudder angle is 0 after unmanned boat reaches path point, motion state is stable and consistent with original state at this time.
It is can be found that from the generating process of such trajectory unit:
1) orbit segment is initial and the rudder angle of end-state is 0, and due to unmanned boat revolution speed of propeller it is constant (assuming that
3), after steady steaming (rudder angle 0), the unmanned boat speed of a ship or plane is constant.It therefore meets rule 1.
2) the trajectory unit generating process in need changed course, only once steering, therefore meet rule 2.
3) for each reachable path point, have and only unique bow is to being corresponding to it, therefore meet rule 3.
4) chamfered shape of main track unit (oa, ob and oc) is grid, more than the unit (oi and oj) of single grid
Shape is extended on the basis of the basic grid, therefore meets rule 4.
Therefore, the trajectory unit of type one meets track discretization rule, and the orbit segment of generation has unmanned boat movement rule
It draws and requires.
2), as shown in figure 13, for based on trajectory unit modeling type two:
Theoretically, in the trajectory unit of type one, orbit segment oa, ob and oc can handle basic motion planning and ask
Topic.Because target bow can mutually be converted to the orbit segment for 0 °, 90 °, 180 ° and 270 ° by above three unit
It generates.But in order to keep spliced final track more excellent, it joined the target of 45 ° and 315 ° in the trajectory unit of type one
Bow is to orbit segment.Therefore, in order to make the two bows to can also convert with original bow to mutual, it is necessary to be suggested for type two.
When the initial bow of unmanned boat to be 45 ° when, since 135 °, 180 ° and 270 ° of direction and initial bow are on the contrary, therefore mesh
Mark bow from 0 °, 45 °, 90 °, 225 ° and 315 ° to should select.Simultaneously as the major function of two trajectory unit of type is auxiliary
And conversion, so in order to simplify search, it is only necessary to which final bow is mutually 0 °, 45 ° and 90 °.It is final for orbit segment it is reachable around
For grid, due to the constraint by minimum turning radius, node a, b, d, e, f, g and h cannot pass through manipulation of once coming about
It reaches.Therefore final reachable path point is only c (shown in such as Figure 13 (1)), i.e., does not change rudder angle, and final bow is to being kept for 45 °
It is constant.Eight grids have all been discussed around present node at this time, but bow is not distributed by node to 0 ° and 90 °.In view of this
Both direction distinguishes north by east and east by north, surrounding point is extended to the outside one layer, selecting node j and k is bow to 0 ° and 90 °
Path point (shown in such as Figure 13 (2)).
Then when initial bow to be 45 ° when, trajectory unit mathematical model be represented by (also in formula (1-16) and
Under the premise of (1-17)):
Generate initial bow to be 45 ° when trajectory unit process and 0 ° of bow to when it is similar, as shown in figure 14.Orbit segment oc is
Keep initial bow to straightway, orbit segment oj and ok are the curved section for grasping phase counter rudder angle, and are eased the helm a little.It is worth note
Meaning, the absolute value of order rudder angle at this time are δ3.Since the rudder angle makes stem to changing 45 °, it is believed that be class
In type one bow to from 0 ° → 45 ° and 0 ° → 315 ° change inverse process.Therefore, δ3≈δ2<δ1.Simultaneously, it is easy to prove, initially
Bow also complies with this 4 track discretization rules to the trajectory unit for 45 °.
3) it, is verified as shown in figure 15 for all standing of trajectory unit space:
By the introduction of preceding two trifle, the generating process of trajectory unit model is shown completely, and two kinds of
Unit is all satisfied track discretization rule.But it before proposing searching algorithm using the model, also needs that it can be covered completely
It is verified lid search space.Also according to symmetry, with bow to being sky that track model of element is verified in representative for 0 ° and 45 °
Between all standing.
As shown in figure 15, figure (1) and figure (2) be initial bow respectively to be 0 ° and 45 ° when trajectory unit arrival surrounding point
Situation (current point is Bluepoint, and surrounding point is green point).It can be seen that one or more rails can be passed through under two states
The splicing of mark unit directly or indirectly reaches eight whole points of surrounding.Therefore, any one mesh point all may be used in search space
To be reached by the combination of trajectory unit, the not presence of missed point.Trajectory unit has all standing.
4), trajectory unit control table:
Based on the modeling process of the above trajectory unit, the trajectory unit collection of complete set be can establish.In order to plan
Understand in journey and how specifically to control unmanned boat to reach corresponding track, provide reference for motion control, it is necessary to which establishing can table
Show the trajectory unit control table of manipulation details (as shown in table 1-1 to 1-8.Wherein, H is course, RA be rudder angle (larboard is negative,
Starboard is positive), CRA is order rudder angle, and EH is to ease the helm).
0 ° of table 1-1 initial bow to trajectory unit control table
45 ° of table 1-2 initial bows to trajectory unit control table
90 ° of table 1-3 initial bows to trajectory unit control table
135 ° of table 1-4 initial bows to trajectory unit control table
180 ° of table 1-5 initial bows to trajectory unit control table
225 ° of table 1-6 initial bows to trajectory unit control table
270 ° of table 1-7 initial bows to trajectory unit control table
315 ° of table 1-8 initial bows to trajectory unit control table
4) path cost calculating is carried out to all reachable path points, obtains next waypoint position and corresponding bow
To angle；
In the search process in path, the Dynamic Constraints of unmanned boat will be converted into these reachable path points and corresponding
Bow to.Therefore, which converts geometric node searching problem for motion planning problem.
1, trajectory unit abstracts
Trajectory unit is a series of geometric locus set with different motion state.And route searching concerns and is working as
Under preceding motion state, node location and corresponding direction that unmanned boat can reach.Therefore, it before searching algorithm proposition, needs
Will the geometrical characteristic to trajectory unit carry out abstract processing.
The abstracting process of one trajectory unit of type (with 0 ° of initial heading for example) is as shown in figure 16.When initial bow to for
At 0 °, the path point that unmanned boat can reach is a, b, c, i and j.So node searching at this time should be only limited to this five
Point.Correspondingly, the corresponding five different bows of five path points to.The algorithm of the process is stated are as follows:
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
The abstract of two trajectory unit of type (with 45 ° of initial heading for example) is similar.When initial bow to be 45 ° when, nothing
The path point that people's ship can reach is c, j and k.So node searching at this time should be only limited to these three points.Similarly three roads
The corresponding three different bows of diameter point to.The algorithm of the process is stated are as follows:
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
Aggregate analysis, the trajectory unit abstracting process in all eight directions is as shown in figure 17 under two types.From figure
It is found that present node needs 16 path points around primary search.This 16 points can be divided into two classes: (eight green for internal point
Point) and peripheral point (eight blue points).Internal point is eight nodes around legacy paths planning, and each node pair
Answer unique bow to.Outside one layer of the internally positioned point of peripheral point, and these point be 45 ° of course change result (that is,
0 ° → 45 °, 45 ° → 0 °, 45 ° → 90 °, etc.).
Therefore during a cyclic search, algorithm always can judge which point is addressable according to initial bow first
, then according to one best path point of corresponding policy selection.However in the search process, it should in how determining
The search order of exterior point? obviously under free space, the sequence of interior exterior point does not influence final result；But there is obstacle
In the environment of object, effect of the different search orders to the path-dependent keyness that can find a safety.
2, collision prevention strategy under obstacle environment
When barrier occurs in planning space, path needs to carry out avoidance planning to it.And during avoidance, unmanned boat
Hull scale have direct influence to final avoidance path.So the constraint of unmanned boat hull scale should also consider
In motion planning.
The selection of path point is the key that determine final path superiority and inferiority.In numerous optimisation strategies, heuritic approach due to
It is quickly feasible, simple and effective and be easy to modify, it is frequently utilized for solving path planning problem.The core of the algorithm is cost letter
Number f (x):
F (x)=g (x)+h (x) (2-1)
It can find out from formula (2-1), which consists of two parts.Wherein g (x) indicates initial point to current point
The actual cost function of (point searched) is the cost value in the practical path of passing by of research object；H (x) is current point to eventually
The heuristic cost function (or estimate cost function) of point is to the remaining estimated value for not walking path also.Cost function f (x)
Value it is smaller, selected path point is better.Therefore, using the thought of heuritic approach, the reachable path point of unmanned boat is carried out most
Excellent assessment.
1) actual cost
In traditional path planning, due to not accounting for Dynamic Constraints, the superiority and inferiority of planning path generally uses distance value
It judges, distance is shorter, and path is more excellent.Therefore actual cost function is generally distance function.However for unmanned boat,
The quality in path is also related with practical steering number in addition to related with apart from size.Since it is considered that manipulation of physical and control,
One preferably planning path is not only path itself and is optimized, and is also easy to realize in manipulation and control.It is general and
Speech, number of coming about is fewer, and planning path is more easy to control.Therefore, the actual cost function of unmanned boat motion planning is by apart from generation
Valence function d (x) and cost function s (x) composition of coming about.
(1) apart from cost
The index of common judge path superiority and inferiority apart from cost, it determine final path apart from size, and this is right
There is direct influence in certain practical problems (such as saving time and fuel oil).Before calculating actual range size, first should
Trajectory unit is analyzed.
According to symmetry, the trajectory unit on eight directions can be divided into five class curves: 0 ° → 0 °, 0 ° → 45 °, 0 ° → 90 °,
45 ° → 45 ° and 45 ° → 0 °.But 0 ° → 45 ° of curve of inverse process can be counted as due to 45 ° → 0 ° of curve, so just
For distance, both curves should be considered as same class.Therefore according to criterion distance, all trajectory units include four classes not
Same curved section (such as Figure 18 (1)).
Since 0 ° → 0 ° and 45 ° → 45 ° of trajectory unit is straightway, the distance of the two can directly use the Euclidean of point-to-point transmission
Distance solves:
0 ° → 45 ° and 0 ° → 90 ° of trajectory unit are curved section, and the distance of the two cannot be calculated with accurately asking away from formula.
Therefore herein using circular fitting come approximate evaluation.As shown in Figure 18 (2), 0 ° → 90 ° of curved section (yellow) occupies a net
Lattice, and the curvature of curve is larger, can be the quarter circular arc of side length of element with a radius come approximate solution (yellow void
Line).0 ° → 45 ° of curved section (orange) occupies two grids, and the curvature of curve is smaller, therefore the center of circle for being fitted circular arc exists
Outside grid.By calculating, when radius is 2.5 times of side length of element, central angle circular arc corresponding when being 53 ° can be preferably
It is fitted the curve.Therefore, the distance solution of curved section may be expressed as:
θ and r is respectively the central angle and radius for being fitted circular arc in formula.
However, distance discussed above is only actual value, for cost, also need to be standardized it.
As shown in Figure 19 (1), 0 ° → 0 ° of trajectory unit (blue), 45 ° → 45 ° (purple) and 0 ° → 90 ° (yellow) occupy one
Grid.When in identical starting point, ((in the case where point d), 45 ° → 45 ° and 0 ° → 90 ° of trajectory unit only need one to point o) with terminal
A orbit segment, and 0 ° → 0 ° then needs two sections.Therefore in planning process, two 0 ° → 0 ° units and one 45 ° → 45 ° or one
A 0 ° → 90 ° units are equivalent.For 0 ° → 45 ° of trajectory unit, since it occupies two grids, so being 3 × 3 by Mesh expansion
Afterwards, shown in isoboles such as Figure 19 (2) of four class trajectory units.
It is not difficult to find out from figure, in the case where identical starting point (point A) and terminal (point B), six sections of 0 ° → 0 ° unit, three
45 ° → 45 ° of section or 0 ° → 90 ° units and 0 ° → 45 ° units have identical path effects.It can further be seen that this and first three simultaneously
Relationship consistency between section trajectory unit.Therefore, final track cost may be expressed as:
d(ψ0→ψt)=dv (ψ0→ψt)·ad (2-4)
In formula, dv is actual orbit segment distance value；adFor apart from cost coefficient, and four class trajectory unit adRatio
Relationship is 6:3:3:2.
(2) it comes about cost
Cost of coming about determines the complexity of manipulation, and the complexity manipulated determines the quality of motion control.Track
The initial bow of unit to final bow to knots modification can be quantified as actual cost value of coming about.Therefore, cost of coming about can table
It is shown as:
In formula, Δ ψmaxFor in all trajectory units, maximum bow is to knots modification；aTFor cost coefficient of coming about.
From formula (4-5) it is found that due to 0 ° → 0 ° and 45 ° → 45 ° of trajectory unit without bow to change, cost of coming about
It is 0；And 0 ° → 90 ° and 0 ° → 45 ° of unit of bow is to changing 90 ° and 45 ° respectively, so the cost of coming about of the two is aTWith
0.5aT(in all trajectory units, maximum bow to knots modification be 90 °, i.e. Δ ψmax=90 °).
Therefore total actual cost may be expressed as:
g(ψ0→ψt)=d (ψ0→ψt)+s(ψ0→ψt) (2-6)
In addition, for path point inaccessible around current point, be artificially arranged these points actual cost value be positive it is infinite.
So regardless of these point heuristic cost, total cost value (actual cost adds heuristic cost) be still positive it is infinite,
It can ignore automatically in path point selection.
2), heuristic cost
Heuristic cost is the estimation to Future Path cost, which path point it has evaluated most possibly towards target
Point.Therefore the purpose that heuristic cost is arranged, to save search time, is reduced in order to which guiding search direction is towards target
Calculation amount.But influence of the heuristic cost to entire cost function cannot be too strong.Because the path point no matter selected has more
Good, judgement is occurred under current state.And the point whether within followed by the time be still it is best, have no way of
It learns.
Based on the above analysis, select manhatton distance as the estimation of heuristic cost herein.It is calculated in certain path plannings
In method, have using Euclidean distance as the way (such as Figure 20 (1)) of heuristic cost.But due to the Euclidean distance ratio between two nodes
Manhatton distance is short, and Euclidean distance cannot indicate the heuristic cost difference of different path points well.And Euclidean distance exists
There is extracting operation in calculating process, this original original intention for reducing calculation amount with heuristic cost is runed counter to.Therefore, Manhattan is selected
Apart from relatively reasonable.
Rule 3 guarantee under, different bows to point have different manhatton distance values, and bow to angle closer to mesh
Punctuate, manhatton distance value are smaller.For example, current bow to be 0 ° when (such as Figure 20 (2) is shown), reachable path point a, b, c, i
There is different manhatton distance values with j.Obviously, path point j (bow to be 45 °) is near close-target (green octagonal, x)
Node, and the manhatton distance value of the point is also minimum at this time.Therefore, heuritic approach can indicate are as follows:
h(ψ0→ψt)=| xg-xt|+|yg-yt| (2-7)
In formula, xgAnd ygFor the position coordinates of terminal.
In process discussed above, the bow of path point can also be with to the difference of the deflection between angle and target and current point
Judgment basis as heuristic cost.The difference is smaller, indicates alternative path point closer to target.Obviously in free space
Under, which is that accurately, the path point of selection can guarantee that final path is optimal.But there is barrier
In the environment of hindering object, which is easy so that the path of planning falls into local optimum.
3) search strategy
In one cycle search process, algorithm needs to search for 16 nodes around current point.And according to the distribution of node,
This 16 points can be divided into internal layer path point and outer layer path point again.In the environment of having barrier, interior exterior point it is reachable
There are certain connections for property.
As shown in Figure 21 (1), it is assumed that unmanned boat is located at node o at this time and bow is to being 0 °, and internal point is occupied (red by barrier
Vitta shape barrier is located at a, b two o'clock).According to the feature of trajectory unit, node a, b, c, i and j are its reachable path point.By
It is occupied in a, b two o'clock by barrier, unmanned boat does not reach the two path points (blue and dotted yellow line).But at the same time,
Path point i is also unreachable.Because bar shaped barrier fully taken up a, b two o'clock and between spatial position, block track oi
Access (orange dotted line), unmanned boat not may pass through barrier reach path point i.Therefore the accessibility and a, b two of path point i
Point is related.But conversely, whether internal point is reachable not to be limited but by peripheral point when peripheral point is occupied by barrier.Therefore
By analyze above it is found that the accessibility of outer layer path point it is corresponding internal layer path point it is relevant, and internal layer path point can
It is not contacted directly up to property with outer layer path point.
Then around in the search regulation of 16 nodes, need to consider the point between outer layer path point and internal layer path point
Association.According to trajectory unit feature, shown in relevance such as Figure 21 (2) of outer layer path point and internal layer path point.By can in the figure
Know, each outer layer path point is relevant with two continuous internal layer path points: i is associated with a, b two o'clock, and j is associated with b, c two o'clock, k
It is associated with c, d two o'clock, l is associated with d, e two o'clock, and m is associated with e, f two o'clock, and n is associated with f, g two o'clock, and p is associated with g, h two o'clock, q
It is associated with h, a two o'clock.Therefore in search process, if two continuous internal layer path points are unreachable, associated outer layer path
Point is also unreachable.It is stated with algorithm are as follows:
x,y∈inner point；
x adjoin y；
X∈outer point；
if x&&y unreachable
X is unreachable；
end
Based on the associated conclusion of interior exterior point, during 16 node searchings around, it should search internal layer path point first.
When algorithm detects that continuous two internal points are occupied by barrier and is unreachable, associated outer layer path point is not specified also not
Reachable, entire search process is two layers inside and outside in being divided into.So way of search at this time is spiral search (such as Figure 22 (1)
It is shown).If but search peripheral point is entirely searched for since the accessibility of internal point does not contact directly with peripheral point first
Process only increases node searching quantity and there is no the changes in structure.So way of search and conventional search at this time
It is identical, remain as round formula search (shown in such as Figure 22 (2)).
Therefore, in order to adapt to the characteristic of trajectory unit and make search process more efficiently, the search strategy of this algorithm
For by the spiral search of internal layer outer layers.
About dangerous point:
During practical avoidance, the scale size of research object can have an impact the path of planning.Therefore unmanned boat
Hull scale be also that motion planning studies problem in need of consideration.
Front has been analyzed to obtain, and node a, b and i are infeasible paths point.The selectable path point of unmanned boat at this time
Only c and j.But due to the feature of trajectory unit, from Figure 23 (1), it can be seen that, orbit segment oj is close apart from node b.When nobody
When ship is moved near node b, there is very big probability to collide with bar shaped barrier.Therefore the hull ruler of unmanned boat is considered
Degree, node j is also infeasible paths point.Then, under the situation, final unmanned boat can bit path point be only c.Based on
Upper analysis, it is necessary to discuss again to the feature of trajectory unit, find out the orbit segment with dangerous " points of proximity ".
Due to bow to the constant trajectory unit in angle be straightway, so be not present " points of proximity ".Bow changes 90 ° of rail to angle
Although mark unit is curved section, its curvature is larger, farther out apart from other path points, so " points of proximity " also are not present.But
45 ° of trajectory unit, since curvature is smaller, path of this kind of curve in generating process, with front are changed to angle for bow
Point is closer, and there is the danger of " points of proximity ".Figure 23 (2) lists the trajectory unit that all bows change 45 ° to angle, black
Box show " points of proximity ".
Therefore, in the case where considering unmanned boat hull dimensional constraints, in order to make unmanned boat avoid " points of proximity ", if current point is just
The path point in front by barrier occupy and it is unreachable when, the bow of unmanned boat can not be 45 ° to changing.
5) judge whether the path point is that the terminal of motion path then exports final path if terminal, otherwise, by this
Path point and corresponding bow to angle as the point of real-time route locating for unmanned boat and bow to angle, be transferred to step 2).
The application example of the method for the present invention:
As shown in figure 24, three barriers are provided with, shape is respectively rectangle, bar shaped and U-shaped.Wherein U-shaped barrier
It is easy to that planning is allowed to fall into local optimum state, so that terminal is not achieved in unmanned boat.The distance between three barriers ten simultaneously
Point close, during walking, unmanned boat is easy to because hull and obstacle distance are too close to and generate danger.Therefore, should
Planning path is more demanding to the motion planning of unmanned boat, needs to consider more planning details, is otherwise easy to cause mistake
It loses.
(1) of Figure 24 is the result of route planning.It can be seen from the figure that although the space between barrier is very narrow,
But Route Planning Algorithm still has found a collisionless route between these barriers, has eventually arrived at target point.Figure 24
(2)-(4) be motion planning result.The initial bow of unmanned boat is to being 45 ° and 0 ° in (2) of Figure 24.Due to (1) of Figure 24
In direction of the planning path when starting be 45 °, so compared with bow is direct to 45 ° of motion planning and route planning.From
Space (red side of in the figure as can be seen that motion planning path there is no selection across bar shaped obstacle and U-shaped obstacle
Frame).The reason is that considering the hull scale of unmanned boat, the space is too narrow so that it cannot make unmanned boat safety.Then
Unmanned boat has selected around U-shaped barrier, and space from the upper side reaches target point.Initial bow to the experiment for 0 ° also from
Space above reaches target point.But since initial bow is to difference, which has first passed around the Rectangular Obstacles in its upper right side
Object.(3) of Figure 24 are the case where initial heading are 270 °.There are two selections in the path: turning to larboard or the right side in the incipient stage
The side of a ship.If it selects the former, space from below is reached target by path；Otherwise it will be from above to reaching.No matter unmanned boat selects
Any path is selected, both reasonably.The path of lower section is relatively short, but has the process of a racing in the initial stage,
For bow to 180 ° of velocity variations, this has higher requirement to manipulation.The path of top is longer, but all steering angles compared with
It is small.Therefore manipulation is simpler, easy to accomplish.(4) of Figure 24 are the results that initial heading is 90 ° and 180 °.Two planning paths
Space from below has all been selected to reach target point.This is because initially bow is to making unmanned boat more easily from below around item
Shape barrier reaches target.
In addition, it is all of above from the path that upper space is planned, the inside of U-shaped barrier is not all fallen into, thus again
Demonstrate the accuracy and validity of the method for the present invention.Therefore above-mentioned planning path, under obstacle environment, unmanned boat are based on
Motion planning result do not only focus on the solution of shortest path, while or be even more concerned about unmanned boat hull scale and
Maneuvering performance.This also demonstrates superiority of the method for the present invention compared to Route Planning Algorithm.
It should be understood that for those of ordinary skills, can be improved or be become according to the above description
It changes, and all these modifications and variations should all belong to the protection domain of appended claims of the present invention.
Claims (5)
1. unmanned boat collision prevention paths planning method in the case of a kind of barrier based on trajectory unit, which is characterized in that including with
Lower step:
1) beginning and end of unmanned boat motion path is determined；
2) determine the point of real-time route locating for unmanned boat and bow to angle；
3) locating for the trajectory unit and barrier that the point of the real-time route according to locating for unmanned boat and bow are moved to angle, unmanned boat
Path point seeks reachable path point；
The generating mode of the trajectory unit is as follows:
3.1) motion modeling is carried out to unmanned boat using MMG model, obtains the motion profile of unmanned boat；
3.2) motion profile of unmanned boat is constrained using unmanned boat track discretization rule；It is advised including following discretization
Then:
Rule one: orbit segment is initial and finish time, the motion state of unmanned boat is stable and is consistent；
The number of coming about of regular two: every section tracks is no more than once；
Rule three: the orbit segment attainable path point of institute and the bow that can change to mapping one by one；
Rule four: the chamfered shape of all orbit segments is grid or is based on grid, to adapt to grating map；
3.3) modeling process of trajectory unit is completed according to step 3.1) and 3.2), and establishes trajectory unit collection；
4) path cost calculating is carried out to all reachable path points, obtains next waypoint position and corresponding bow to angle；
5) judge whether the path point is that the terminal of motion path then exports final path if terminal, otherwise, by the path
Point and corresponding bow to angle as the point of real-time route locating for unmanned boat and bow to angle, be transferred to step 2).
2. unmanned boat collision prevention paths planning method in the case of the barrier according to claim 1 based on trajectory unit,
It is characterized in that, the step 3) kind seeks reachable path point, and searching method is as follows:
The trajectory unit that the point of the real-time route according to locating for unmanned boat and bow are moved to angle and unmanned boat, utilizes trajectory unit
Geometrical characteristic, obtain the path point that can be reached from real-time route point and corresponding stem to；
According to the path point that can be reached, reachable internal layer path point is searched for；The internal layer path point is with locating real-time road
8 point of proximity centered on diameter point；
Path point association is judged whether there is, reachable outer layer path point is searched for if having；The outer layer path point is with locating
The outer layer relating dot of internal layer path point centered on real-time route point.
3. unmanned boat collision prevention paths planning method in the case of the barrier according to claim 1 based on trajectory unit,
It is characterized in that, if dangerous point in the reachable path point sought in the step 3), after casting out dangerous point, then is searched
Rope；Neighbor point of the dangerous point between two path points as locating for barrier.
4. unmanned boat collision prevention paths planning method in the case of the barrier according to claim 1 based on trajectory unit,
It is characterized in that, if in the reachable path point sought in the step 3), if the path point immediately ahead of current path point is by obstacle
When object occupies and is unreachable, if the bow of unmanned boat to changing into 45 °, casts out the path point.
5. unmanned boat collision prevention paths planning method in the case of the barrier according to claim 1 based on trajectory unit,
It being characterized in that, the cost in the step 4) is calculated to be indicated by cost function f (x), specific as follows:
F (x)=g (x)+h (x)
Wherein, g (x) indicates that initial point is the cost value in the practical path of passing by of research object to the actual cost function of current point；
H (x) is heuristic cost function of the current point to terminal, is to the remaining estimated value for not walking path also；
Wherein, actual cost function g (x) is made of distance cost function d (x) and the cost function s (x) that comes about, heuristic cost
Function h (x) is indicated by manhatton 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 true CN109240288A (en) | 2019-01-18 |
| CN109240288B 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) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110108280A (en) * | 2019-03-18 | 2019-08-09 | 大连海事大学 | A kind of ship method for planning track based on the splicing of the track AIS manipulation unit |
| CN110196598A (en) * | 2019-06-25 | 2019-09-03 | 北京航天控制仪器研究所 | A kind of dynamic collision prevention method of unmanned boat |
| CN110501014A (en) * | 2019-09-26 | 2019-11-26 | 武汉理工大学 | A kind of unmanned boat motion planning method towards restricted waters |
| CN111798701A (en) * | 2020-07-07 | 2020-10-20 | 中国船舶工业系统工程研究院 | Unmanned ship path tracking control method, system, storage medium and terminal |
| CN111984006A (en) * | 2020-07-24 | 2020-11-24 | 哈尔滨工程大学 | Unmanned ship multi-target meeting collision avoidance method integrating ocean current and scale difference influences |
| CN112363512A (en) * | 2020-11-25 | 2021-02-12 | 中国船舶科学研究中心 | Intelligent ship collision avoidance path planning method based on heading and speed |
| CN112562161A (en) * | 2020-12-08 | 2021-03-26 | 福建红点科技有限公司 | Based on 5G thing networking and AI intelligent entrance guard integrated management system |
| CN113885533A (en) * | 2021-11-12 | 2022-01-04 | 江苏海洋大学 | 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 (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP2380066A2 (en) * | 2008-12-30 | 2011-10-26 | 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 |
| US20160209849A1 (en) * | 2015-01-15 | 2016-07-21 | William Dale Arbogast | System and method for decentralized, multi-agent unmanned vehicle navigation and formation control |
| US20160299507A1 (en) * | 2015-04-08 | 2016-10-13 | University Of Maryland, College Park | Surface vehicle trajectory planning systems, devices, and methods |
| CN106845716A (en) * | 2017-01-25 | 2017-06-13 | 东南大学 | A kind of unmanned surface vehicle local delamination paths planning method based on navigation error constraint |
| CN107037809A (en) * | 2016-11-02 | 2017-08-11 | 哈尔滨工程大学 | A kind of unmanned boat collision prevention method based on improvement ant group algorithm |
| CN107197806A (en) * | 2017-05-17 | 2017-09-26 | 武汉理工大学 | Spring louvers formula fixed point Autoamtic bait putting method based on unmanned boat |
| CN107544500A (en) * | 2017-09-18 | 2018-01-05 | 哈尔滨工程大学 | A kind of unmanned boat berthing action trail planing method for considering constraint |
| US20180017976A1 (en) * | 2016-07-13 | 2018-01-18 | Flytrex Aviation Ltd. | System and method for dynamically updated unmanned vehicle navigation planning |
| 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 |
| CN108445894A (en) * | 2018-06-15 | 2018-08-24 | 哈尔滨工程大学 | A kind of secondary paths planning method considering unmanned boat movenent performance |
| CN108445879A (en) * | 2018-03-12 | 2018-08-24 | 上海大学 | A kind of unmanned boat barrier-avoiding method based on prediction collision risk region |
-
2018
- 2018-08-31 CN CN201811015791.XA patent/CN109240288B/en active Active
Patent Citations (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP2380066A2 (en) * | 2008-12-30 | 2011-10-26 | 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 |
| US20160209849A1 (en) * | 2015-01-15 | 2016-07-21 | William Dale Arbogast | System and method for decentralized, multi-agent unmanned vehicle navigation and formation control |
| US20160299507A1 (en) * | 2015-04-08 | 2016-10-13 | University Of Maryland, College Park | Surface vehicle trajectory planning systems, devices, and methods |
| US20180017976A1 (en) * | 2016-07-13 | 2018-01-18 | 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 |
| CN106845716A (en) * | 2017-01-25 | 2017-06-13 | 东南大学 | A kind of unmanned surface vehicle local delamination paths planning method based on navigation error constraint |
| CN107197806A (en) * | 2017-05-17 | 2017-09-26 | 武汉理工大学 | Spring louvers formula fixed point Autoamtic bait putting method based on unmanned boat |
| 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 |
| CN108445894A (en) * | 2018-06-15 | 2018-08-24 | 哈尔滨工程大学 | A kind of secondary paths planning method considering unmanned boat movenent performance |
Non-Patent Citations (5)
| Title |
|---|
| WU PENG，等: "Autonomous obstacle avoidance of an Unmanned Surface Vehicle based on cooperative maneuvering", 《INDUSTRIAL ROBOT: AN INTERNATIONAL JOURNAL》 * |
| ZHE DU,等: "Motion planning for Unmanned Surface Vehicle based on Trajectory Unit", 《OCEAN ENGINEERING》 * |
| 杜哲，等: "基于动态复杂度地图的船舶航迹规划", 《系统仿真学报》 * |
| 汪栋，等: "一种基于有限状态机模型的局部转向避碰路径规划算法", 《海洋科学》 * |
| 潘鹏，等: "基于位置变化的轨迹单元划分及索引机制", 《小型微型计算机系统》 * |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110108280A (en) * | 2019-03-18 | 2019-08-09 | 大连海事大学 | A kind of ship method for planning track based on the splicing of the track AIS manipulation unit |
| 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 |
| CN110196598A (en) * | 2019-06-25 | 2019-09-03 | 北京航天控制仪器研究所 | A kind of dynamic collision prevention method of unmanned boat |
| CN110501014A (en) * | 2019-09-26 | 2019-11-26 | 武汉理工大学 | A kind of unmanned boat motion planning method towards restricted waters |
| CN111798701A (en) * | 2020-07-07 | 2020-10-20 | 中国船舶工业系统工程研究院 | 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 |
| CN111984006A (en) * | 2020-07-24 | 2020-11-24 | 哈尔滨工程大学 | Unmanned ship multi-target meeting collision avoidance method integrating ocean current and scale difference influences |
| CN112363512A (en) * | 2020-11-25 | 2021-02-12 | 中国船舶科学研究中心 | Intelligent ship collision avoidance path planning method based on heading and speed |
| CN112562161A (en) * | 2020-12-08 | 2021-03-26 | 福建红点科技有限公司 | Based on 5G thing networking and AI intelligent entrance guard integrated management system |
| CN113885533A (en) * | 2021-11-12 | 2022-01-04 | 江苏海洋大学 | 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 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109240288B (en) | 2021-08-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109240288A (en) | Unmanned boat collision prevention paths planning method in the case of a kind of barrier based on trajectory unit | |
| CN108876065A (en) | A kind of unmanned boat motion planning method based on trajectory unit | |
| CN109597417B (en) | Multi-USV group collaborative collision avoidance planning method based on collision avoidance criterion | |
| CN113759939B (en) | Intelligent navigation method and device for limited water area | |
| CN111123903B (en) | Unmanned ship obstacle avoidance method based on circular track unit | |
| CN108459602B (en) | Autonomous berthing method of under-actuated unmanned ship in multi-obstacle complex environment | |
| Wang et al. | Local path optimization method for unmanned ship based on particle swarm acceleration calculation and dynamic optimal control | |
| Zhou et al. | Motion planning for an unmanned surface vehicle based on topological position maps | |
| CN111665846B (en) | Water surface unmanned ship path planning method based on rapid scanning method | |
| Gu et al. | An improved RRT algorithm based on prior AIS information and DP compression for ship path planning | |
| CN113821031A (en) | Unmanned ship autonomous navigation capability assessment method | |
| Guan et al. | Autonomous collision avoidance of unmanned surface vehicles based on improved A-star and dynamic window approach algorithms | |
| CN116360434A (en) | Ship path planning method based on improved CSAC-APF algorithm | |
| Zhao et al. | Decision-making for the autonomous navigation of USVs based on deep reinforcement learning under IALA maritime buoyage system | |
| Wang et al. | A distributed model predictive control using virtual field force for multi-ship collision avoidance under COLREGs | |
| Zhang et al. | Ship heading control based on fuzzy PID control | |
| Kagabo et al. | Trajectory determination for energy efficient autonomous soaring | |
| CN116300906A (en) | Obstacle avoidance path planning method and system for intelligent ship | |
| CN110444045B (en) | Non-conflict state ship cluster situation analysis method | |
| CN114035574A (en) | Autonomous obstacle avoidance method for unmanned surface vehicle | |
| Li et al. | Model predictive control and artificial potential field based collision avoidance path planning method with ship maneuverability | |
| Yan et al. | A hybrid PSO-WG algorithm for AUV path planning in unknown oceanic environment | |
| Fang et al. | Evaluation Model of Ship Berthing Behavior Based on AIS Data | |
| Zhang et al. | A new hybrid path planning method for the sailboat architecture wave glider in the wind field environment | |
| Lv et al. | Ship trajectory control system based on fuzzy control |
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 |