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 PDF

Info

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
Application number
CN201811015791.XA
Other languages
Chinese (zh)
Other versions
CN109240288B (en
Inventor
周春辉
顾尚定
杜哲
肖长诗
文元桥
黄亮
张帆
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan University of Technology WUT
Original Assignee
Wuhan University of Technology WUT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Wuhan University of Technology WUT filed Critical Wuhan University of Technology WUT
Priority to CN201811015791.XA priority Critical patent/CN109240288B/en
Publication of CN109240288A publication Critical patent/CN109240288A/en
Application granted granted Critical
Publication of CN109240288B publication Critical patent/CN109240288B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02Control of position or course in two dimensions
    • G05D1/0206Control 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

Unmanned boat collision prevention paths planning method in the case of a kind of barrier based on trajectory unit
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,y00,v00), the state after the t time is Send(xt,yt, ψt,vtt).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, δ21).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≈δ21.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.
CN201811015791.XA 2018-08-31 2018-08-31 Unmanned ship collision avoidance path planning method based on track unit under condition of obstacle Active CN109240288B (en)

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 (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110196598A (en) * 2019-06-25 2019-09-03 北京航天控制仪器研究所 A kind of dynamic collision prevention method of unmanned boat
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

Citations (16)

* Cited by examiner, † Cited by third party
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
CN108445879A (en) * 2018-03-12 2018-08-24 上海大学 A kind of unmanned boat barrier-avoiding method based on prediction collision risk region
CN108445894A (en) * 2018-06-15 2018-08-24 哈尔滨工程大学 A kind of secondary paths planning method considering unmanned boat movenent performance

Patent Citations (16)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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 (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110196598A (en) * 2019-06-25 2019-09-03 北京航天控制仪器研究所 A kind of dynamic collision prevention method of unmanned boat
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
CN111984006B (en) * 2020-07-24 2021-07-06 哈尔滨工程大学 Unmanned ship multi-target meeting collision avoidance method integrating ocean current and scale difference influences

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
CN108459602B (en) Autonomous berthing method of under-actuated unmanned ship in multi-obstacle complex environment
Xu Collision avoidance strategy optimization based on danger immune algorithm
CN109597417A (en) A kind of more USV multi-agent synergy collision-avoidance planning methods based on collision prevention criterion
Li et al. Ship’s trajectory planning based on improved multiobjective algorithm for collision avoidance
Zhou et al. Optimal collision-avoidance manoeuvres to minimise bunker consumption under the two-ship crossing situation
Wang et al. A distributed model predictive control using virtual field force for multi-ship collision avoidance under COLREGs
Junmin et al. Mechanism of dynamic automatic collision avoidance and the optimal route in multi-ship encounter situations.
Wang et al. Local path optimization method for unmanned ship based on particle swarm acceleration calculation and dynamic optimal control
CN110501014A (en) A kind of unmanned boat motion planning method towards restricted waters
Wu et al. Long-voyage route planning method based on multi-scale visibility graph for autonomous ships
Lee et al. Multi-resolution path planning for marine surface vehicle considering environmental effects
Du et al. Trajectory-cell based method for the unmanned surface vehicle motion planning
Yunsheng et al. Design of information network and control system for USV
Kagabo et al. Trajectory determination for energy efficient autonomous soaring
CN111123903B (en) Unmanned ship obstacle avoidance method based on circular track unit
Olindersson et al. Developing a maritime safety index using fuzzy logics
Zhou et al. Motion planning for an unmanned surface vehicle based on topological position maps
Aarsæther et al. Adding the human element to ship manoeuvring simulations
Zhang et al. Ship Heading Control Based on Fuzzy PID Control
CN110908369A (en) Ship berthing data acquisition method based on manual controller
Haugen Guidance algorithms for planar path-based motion control scenarios
Wen et al. Collision-free Trajectory Planning for Autonomous Surface Vehicle
CN110444045B (en) Non-conflict state ship cluster situation analysis method

Legal Events

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