CN108664020A - A kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method - Google Patents
A kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method Download PDFInfo
- Publication number
- CN108664020A CN108664020A CN201810318563.3A CN201810318563A CN108664020A CN 108664020 A CN108664020 A CN 108664020A CN 201810318563 A CN201810318563 A CN 201810318563A CN 108664020 A CN108664020 A CN 108664020A
- Authority
- CN
- China
- Prior art keywords
- unmanned boat
- barrier
- speed
- avoidance
- dynamic
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 40
- 230000004888 barrier function Effects 0.000 claims abstract description 137
- 239000013598 vector Substances 0.000 claims description 75
- 230000001133 acceleration Effects 0.000 claims description 19
- 238000011156 evaluation Methods 0.000 claims description 8
- 238000012545 processing Methods 0.000 claims description 6
- 230000003068 static effect Effects 0.000 abstract description 7
- 238000013461 design Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 230000007613 environmental effect Effects 0.000 description 4
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- DMBHHRLKUKUOEG-UHFFFAOYSA-N diphenylamine Chemical compound C=1C=CC=CC=1NC1=CC=CC=C1 DMBHHRLKUKUOEG-UHFFFAOYSA-N 0.000 description 2
- 238000004387 environmental modeling Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000005204 segregation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The present invention relates to a kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method.The present invention considers the big feature of ship length-width ratio on the basis of Speed Obstacles method, with ellipse representation unmanned boat and barrier, provides a kind of method solving oval tangent line；The kinematics performance for considering unmanned boat only uses speed and direction progress avoidance calculating that unmanned boat can reach within given time.Determining when to start avoidance with the time needed for unmanned boat avoiding obstacles by comparing collision time, terminating avoidance when by determining that unmanned boat moves along desired direction of navigating and moves all safe to target point direction.And virtual obstacles are increased to reduce the influence of barrier movable information error according to the speed of barrier and direction of motion error.The present invention ensures that unmanned boat can effectively avoid the static state encountered and dynamic barrier during autonomous navigation across the sea.
Description
Technical field
It is specifically a kind of based on Speed Obstacles method and dynamic the invention patent relates to mobile robot local paths planning field
The unmanned boat dynamic obstacle avoidance algorithm of state window technique.
Background technology
Unmanned surface vehicle, abbreviation unmanned boat are a kind of light-duty intelligent surface vehicles, have small, low cost, speed
The features such as degree is fast, mobility strong.With control technology, the progress of sensing technology, wireless communication technique, unmanned surface vehicle obtains
Prodigious development.By carrying different equipment, unmanned boat can be applied in different fields, for example, when carrying simple beam, more
When the sonar sets such as wave beam, shallow bottom section plotter, can be used for seabed mapping, detect a mine it is antisubmarine etc.；When carrying water quality sampling or inspection
When measurement equipment, it can be used for environment measuring；When carrying weapon, it can be used for Regional patrolling, coast environmental protection, convoy, operation etc.
Task.
To ensure that unmanned boat can normally and safely navigate by water in ocean, unmanned boat is allowed for being met during navigation
Other barriers such as ship of island, submerged reef, beacon, buoy and the navigation arrived carry out automatic obstacle avoiding.Unmanned boat automatic obstacle avoiding category
In path planning field.
Path planning is usually divided into global path planning and local paths planning.Global path planning usually assumes that environment
Information is completely known, and before unmanned boat starts running, off-line calculation goes out a Complete Path from origin-to-destination；But work as environment
Global path planning cannot cook up safe path when known to not exclusively, and real-time is bad, when the environment changes, cannot be fast
Speed calculates new path.The component environment information that local paths planning is obtained by sensor calculates feasible in real time online
Path, local paths planning can be divided into two classes：It is known as static avoidance when barrier is static, is known as when barrier is dynamic
Dynamic obstacle avoidance, dynamic obstacle avoidance algorithm can also be used for static avoidance.Wherein dynamic obstacle avoidance is relatively difficult, because passing through unmanned boat itself
The sensor of carrying is difficult the accurate movable information for obtaining dynamic barrier, and it is accurate to be carried out to the movement tendency of barrier
Prediction.Local paths planning calculation amount is small, and real-time is good, but because therefore environmental information is not exclusively it is found that be easily trapped into pole
Small value point.The characteristics of according to global path and local path, at present generally use overall situation and partial situation path planning be combined two
Layer avoidance structure, in first layer, by known environmental information, usually map, segregation reasons go out one from starting point to target
The feasible path of point, which should avoid known static-obstacle thing on map；In the second layer, unmanned boat is planned along first layer
Route, when detecting new barrier by sensor in the process of moving, then pass through sensor obtain obstacle
The details of object carry out local avoidance.Also there are a kind of three layers of avoidance structure, increased again on the basis of two layers of avoidance structure
Third layer carries out reactive obstacle avoidance, to cope with the unknowable avoidance scene of barrier movable information.
Therefore, no matter in which kind of avoidance structure, in order to ensure the execution task of the smooth safety of unmanned boat, it is required for one kind
Local obstacle avoidance algorithm ensures that unmanned boat avoids the static state encountered and dynamic barrier.
Invention content
The present invention is directed to the deficiency of prior art, and it is dynamic to provide a kind of unmanned boat based on Speed Obstacles method and dynamic window method
State obstacle avoidance algorithm, this method safely, effectively, reliably, ensure that unmanned boat, can keeping away with safety and stability during autonomous navigation
The dynamic barriers such as the static-obstacle things such as small-sized island, beacon, the buoy encountered and the ship of other navigation are opened, nobody is avoided
Ship collides.
The purpose of the present invention is what is realized by following steps：
Step 1 obtains obstacle information, unmanned boat information, desired speed and direction：
The shaped position and movable information of unmanned boat peripheral obstacle are obtained, shaped position information is taken by unmanned boat itself
The Fusion of band carries out environmental modeling and obtains, and is modeled as barrier greatly according to the actual size situation of barrier
Small different elliptical shape, movable information are obtained by relevant algorithm.Position, movement and the posture for obtaining unmanned boat hull are believed
Breath, location information are obtained by GPS or the Big Dipper, and movement is obtained with posture information by inertial navigation sensor.Unmanned boat is obtained it is expected
Speed and directional information, according to the difference of task, which obtains from upper layer algorithm.
Step 2, barrier processing：
It regards unmanned boat as oval, oval barrier is expanded, i.e., by the semi-major axis and semi-minor axis of oval barrier
Length add respectively oval unmanned boat semi-major axis length, barrier is amplified.By the velocity vector error of barrier from
An error vector set VE is dissipated into, each error vector in the set adds the barrier velocity vector that sensor obtains
It can obtain a set of vectors VP, each vector regards the velocity vector of a virtual obstacles as in the set, virtual to hinder
Hinder position, the size of object identical as the position for the barrier that sensor obtains, size.
Step 3, judges whether unmanned boat can collide with barrier：
Left and right two tangent line of the oval barrier relative to unmanned boat center is found out first, then finds out unmanned boat and the barrier
Hinder relative velocity size and the direction of object, if relative velocity direction is fallen into the two tangent line angles in left and right, unmanned boat can be with barrier
Hinder object to collide, enters step four；If relative velocity direction is not fallen in the two tangent line angles in left and right, unmanned boat will not
It collides with barrier, enters step nine.
Step 4 judges whether to need to start avoidance：
Assuming that unmanned boat and barrier are all linear uniform motion, unmanned boat is found out since current time to colliding
Required time tCPA；Assuming that unmanned boat velocity magnitude does not change, unmanned boat carries out avoidance with constant angular acceleration turning, asks
It is t to go out the time needed for avoiding obstaclesa.If tCPA＞ k × ta, then need not start avoidance, enter step nine；If tCPA≤k×
ta, then need to start avoidance, into avoidance speed and direction module is calculated, that is, enter step five.
Step 5, calculating speed window and bow are to window
Assuming that the acceleration of unmanned boat and angular acceleration are constant, the attainable linear velocity of unmanned boat in the Δ t times is calculated, is claimed
For velocity window；Unmanned boat bow is calculated in the Δ t times to the angle that can reach, referred to as bow is to window.By velocity window and bow
To window discretization, the reachable velocity vector set RV of composition unmanned boat.
Step 6 calculates reachable avoidance velocity vector
The collection for the velocity vector composition that can be collided with barrier in RV set is collectively referred to as VO set, is arranged from RV set
Except VO set is reachable avoidance velocity vector set RAV.
Step 7 selects optimal avoidance velocity vector
All velocity vectors all can serve as avoidance velocity vector in RAV set, build evaluation function G, gather from RAV
It is middle to select one to make the velocity vector of evaluation function value minimum as avoidance velocity vector.
Step 8 judges whether that avoidance can be terminated
Judge that unmanned boat will not collide along initial desired speed with direction running, and is arrived with present speed size edge
When the movement of target point direction will not collide, you can terminate avoidance.When avoidance can be terminated, nine are entered step；When can not
When terminating avoidance, avoidance speed and the direction of selection are exported, step 5 is again introduced into.
Step 9 exports initial desired speed and direction
When into the step, it is believed that will not collide when unmanned boat is along initial desired speed and direction running, then
Directly export initial desired speed and direction.
Patent of the present invention compared with prior art, has following obvious prominent substantive distinguishing features and notable technology
Progress：The present invention considers the big feature of ship length-width ratio, with ellipse representation unmanned boat and obstacle on the basis of Speed Obstacles method
Object provides a kind of method solving oval tangent line；The kinematics performance for considering unmanned boat, only uses unmanned boat within given time
The speed and direction that can be reached carry out avoidance calculating.Come by comparing the time needed for collision time and unmanned boat avoiding obstacles
Determine when to start avoidance, when by determining that unmanned boat is moved along desired direction of navigating with moving all safe to target point direction
To terminate avoidance.And virtual obstacles are increased to reduce barrier movable information according to the speed of barrier and direction of motion error
The influence of error.Patent of the present invention ensure unmanned boat can effectively avoid during autonomous navigation across the sea the static state encountered and
Dynamic barrier.
Description of the drawings
Fig. 1 is that the present invention starts the flow chart before avoidance；
Fig. 2 is that the present invention starts the course diagram after avoidance；
Fig. 3 A are in the velocity space, and the accessible velocity vector schematic diagram of unmanned boat, Fig. 3 B are the nothings in the velocity space
The accessible avoidance velocity vector schematic diagram of people's ship；
Fig. 4 is that there are the design sketch that when a dynamic barrier, unmanned boat avoidance emulates；
Fig. 5 is that there are the design sketch that when three dynamic barriers, unmanned boat avoidance emulates.
Specific implementation mode
Patent of the present invention is described further with preferred embodiment below in conjunction with the accompanying drawings.
Embodiment one
Referring to Fig. 1, Fig. 2, Fig. 3, based on the unmanned boat dynamic obstacle avoidance algorithm of Speed Obstacles method and dynamic window method, including
Following steps：
Step 1：Obtain obstacle information, unmanned boat information, desired speed and direction；
Step 2：Barrier processing；
Step 3：Judge whether unmanned boat can collide with barrier；
Step 4：Judge whether to need to start avoidance；
Step 5：Calculating speed window and bow are to window；
Step 6：Calculate reachable avoidance velocity vector；
Step 7：Select optimal avoidance velocity vector；
Step 8：Judge whether that avoidance can be terminated；
Step 9：Export initial desired speed and direction.
Embodiment two
The present embodiment and embodiment one are essentially identical, and special feature is as follows：
Obtain obstacle information, unmanned boat information, desired speed and direction.The obstacle information of acquisition includes position letter
Breath, dimension information, movable information.Location information indicates that movable information is used under unmanned boat hull coordinate system with distance and angle
Speed and the direction of motion indicate that the direction of motion is on the basis of direct north.The unmanned boat information of acquisition includes location information, movement
With posture information.Location information in east northeast uses (x, y) coordinate representation under coordinate system, and movable information includes velocity magnitude, speed
Direction, acceleration, angular speed and angular acceleration.Posture information be bow to angle, on the basis of direct north.Desired speed and side
It is provided to by upper layer algorithm, direction is on the basis of direct north.Above-described angle and direction are all clockwise for just
Value.
Barrier processing.The length of the semi-major axis of oval barrier and semi-minor axis is added to half length of oval unmanned boat respectively
The length of axis.According to the speed and deflection error of barrier, the velocity vector set VP for finding out barrier actual capabilities is：
It is in formulaVelocity vector for the barrier obtained by sensor, VE are barrier velocity vector error collection
It closes,For Minkowski vector sum operation
Judge whether unmanned boat can collide with barrier.Unmanned boat hull coordinate system and barrier coordinate system are established,
Barrier coordinate system is using barrier center as origin, and long axis direction is x-axis, and it is y-axis that x-axis, which rotates clockwise 90 degree,.By unmanned boat
Coordinate in hull coordinate system is gone in barrier coordinate system, and oval barrier is found out in barrier coordinate system relative to nothing
The point of contact of two tangent lines in people's ship center, two point of contact coordinates are gone under hull coordinate system, are found out oval under hull coordinate system
Left and right two tangent line of the barrier relative to unmanned boat center.Relative velocity of the unmanned boat relative to barrier is found out, if relatively
Directional velocity is fallen into the two tangent line angles in left and right, then unmanned boat can collide with barrier.
Judge whether to need to start avoidance.Unmanned boat is calculated according to the following formula to collide the required time with barrier
tCPA：
In formulaWithFor unmanned boat and the current position vector of barrier,WithFor unmanned boat and barrier
Current velocity vector.Unmanned boat is found out according to the following formula to avoid hindering the time t needed for barriera：
In formula Chinese style (x, y), (xA,yA) it is respectively the coordinate of P points, A points in barrier coordinate system, v, θ0,ω0, α points
Not Wei the current speed of unmanned boat, bow is to, angular speed, angular acceleration, PC1+PC2For in barrier coordinate system P points to ellipse two
The distance of focus and, a is oval semi-major axis.Work as tCPA≤k×taWhen unmanned boat start avoidance, otherwise unmanned boat does not start avoidance.
Calculating speed window and bow are to window.Calculate the attainable speed of unmanned boat, referred to as velocity window in the Δ t times；
Unmanned boat bow is calculated in the Δ t times to the angle that can reach, referred to as bow is to window.Velocity window is discrete for M speed, bow
Discrete to window is N number of direction, and discrete speed and direction form M × N number of velocity vector, by these unmanned boats in the Δ t times
The collection of interior accessible discrete velocity vector composition is collectively referred to as RV.
Calculate reachable avoidance velocity vector.The collection for the velocity vector composition that can be collided with barrier during RV is gathered
It is collectively referred to as VO set, is calculate by the following formula reachable avoidance velocity vector set RAV：
Select optimal avoidance velocity vector.Build evaluation function：
V in formulac, θhIndicate unmanned boat current speed and bow to (v respectivelyi,θj) indicate a discrete velocity vector.
One is selected to make the velocity vector of evaluation function value minimum as avoidance velocity vector from RAV set.
Judge whether that avoidance can be terminated.Judge that unmanned boat will not collide along initial desired speed with direction running,
And with present speed along moved to target point direction will not collide when, you can end avoidance.
Embodiment three
Unmanned boat dynamic obstacle avoidance algorithm concrete operation step based on Speed Obstacles method and dynamic window method is as follows：
Step 1, such as Fig. 1 obtain obstacle information, unmanned boat information, desired speed and direction：
Environmental information is obtained by the self-contained radar of unmanned boat, vision, laser radar, sonar sensor, into line number
The shape and location information of barrier are obtained with environmental modeling according to fusion, is built barrier according to the actual size situation of barrier
Mould is elliptical shape of different sizes, and the shape of barrier is indicated by elliptical semi-major axis length and semi-minor axis length, barrier
Location information indicates that distance is distance of the unmanned boat center to barrier center under hull coordinate system with distance with angle,
Angle be barrier direction relative to unmanned boat bow to angle, angular range be [- 180.0,180.0), unit be degree, up time
Needle orientation angle is positive value；The movable information of barrier is calculated by track algorithm, the movable information speed of barrier is big
It is small to indicate with directional velocity, directional velocity on the basis of direct north, direction scope be [- 180.0,180.0), unit is degree,
It is positive value clockwise.
The longitude and latitude positional information that unmanned boat is obtained by GPS or Big Dipper sensor, the longitude and latitude position of unmanned boat is believed
Breath is lower (x, y) coordinate with switching to east northeast, east northeast coordinate system using direct north as x-axis, due east direction is y-axis, earth table
Face is some coordinate origin.Movement by inertial navigation sensor acquisition unmanned boat and posture information, movable information velocity magnitude,
Directional velocity, acceleration, angular speed and angular acceleration indicate；Posture information is mainly unmanned boat bow to angle.Directional velocity and bow
Be degree to angular unit, ranging from [- 180.0,180.0), the unit of angular speed is degrees second, the unit of angular acceleration be degree/
Second2, it is positive value clockwise to be all.
The initial desired speed and direction of unmanned boat, i.e., in the case where not needing avoidance, nothing are obtained by upper layer algorithm
People's ship is in order to execute task, it should the speed of navigation and direction.
Step 2, such as Fig. 1, barrier processing
It regards unmanned boat as oval, oval barrier is expanded, i.e., by the semi-major axis and semi-minor axis of oval barrier
Length add respectively oval unmanned boat semi-major axis length, barrier is amplified.
The velocity vector of barrier obtained by sensor isBarrier velocity vector error collection is combined into VE, finds out
The velocity vector set VP of barrier actual capabilities is：
In formulaFor Minkowski vector sum operation.
Each velocity vector during VP is gathered is seen as the velocity vector of a virtual obstacles, the position of virtual obstacles
Set, size it is identical as the position for the barrier that sensor obtains, size.
Step 3, judges whether unmanned boat can collide with barrier：
Unmanned boat hull coordinate system and barrier coordinate system are established, hull coordinate system is using hull center as coordinate origin, ship
Bow direction is x-axis, and starboard direction is y-axis, and for barrier coordinate system using barrier center as origin, long axis direction is x-axis, and x-axis is suitable
Hour hands are rotated by 90 ° as y-axis.Oval barrier is indicated with ellipse normal equation, unmanned boat exists first in barrier coordinate system
Coordinate in hull coordinate system is gone in barrier coordinate system, such as following formula：
USVobs=R-1(USVboat-C)
USV in formulaboatFor coordinate of the unmanned boat in hull coordinate system, USVobsIt is unmanned boat in barrier coordinate system
Coordinate, C be coordinate of the barrier central point in hull coordinate system, R be barrier coordinate system relative to hull coordinate system
Spin matrix：
Simultaneous standard ellipse equation and oval tangential equation, such as following formula in barrier coordinate system：
The oval opposite coordinate with the two tangent line point of contacts in unmanned boat center of barrier in barrier coordinate system can be acquired
(xT,yT), then pass through following formula again：
Tboat=RTobs+C
Calculate point of contact (xT,yT) coordinate in hull coordinate system, finding out in hull coordinate system can count after two point of contact coordinates
Calculate left and right two tangent line of the oval barrier relative to unmanned boat in hull coordinate system.
Seek relative velocity of the unmanned boat relative to barrier：
In formulaFor the speed of unmanned boat,For the speed of barrier,It is relatively speed of the unmanned boat relative to barrier
Degree.
If relative velocity direction is fallen into the two tangent line angles in left and right, unmanned boat can collide with barrier, enter
Step 4；If relative velocity direction is not fallen in the two tangent line angles in left and right, unmanned boat will not collide with barrier,
Enter step nine.
Step 4, such as Fig. 1 judge whether to need to start avoidance：
Assuming that unmanned boat and barrier all be linear uniform motion, find out unmanned boat since current time to barrier
The time t for colliding requiredCPA：
In formulaWithFor unmanned boat and the current position vector of barrier,WithFor unmanned boat and barrier
Current velocity vector.
When unmanned boat and barrier collide, the position of unmanned boat central point is known as CPAusv.Assuming that unmanned boat is fast
Degree size remains unchanged, and can be turned with constant angular acceleration.Unmanned boat drives towards CPA with constant speed and directionusvThe mistake of point
Cheng Zhong is turned in certain point A with constant angular acceleration, and the curvilinear path of unmanned boat is tangential on P points with oval barrier just, then recognizes
Barrier is avoided just since P points for unmanned boat, and it is t to ask the time that unmanned boat is driven to from A points needed for P pointsa：
In formula Chinese style (x, y), (xA,yA) it is respectively the coordinate of P points, A points in barrier coordinate system, v, θ0,ω0, α points
Not Wei the current speed of unmanned boat, bow is to, angular speed, angular acceleration, PC1+PC2For in barrier coordinate system P points to ellipse two
The distance of focus and, a is oval semi-major axis.
taTime as needed for unmanned boat avoiding obstacles.If tCPA＞ k × ta, then need not start avoidance, into step
Rapid nine, wherein k are the coefficient more than 1；If tCPA≤k×ta, then need to start avoidance, into calculating avoidance speed and direction mould
Block enters step five.
Step 5, such as Fig. 2, calculating speed window and bow are to window
Assuming that the acceleration of unmanned boat and angular acceleration are constant, the attainable linear velocity of unmanned boat in the Δ t times is calculated, is claimed
For velocity window：
Unmanned boat bow is calculated in the Δ t times to the angle that can reach, referred to as bow is to window：
V in formula (9)cFor the current movement speed of unmanned boat,For the acceleration of unmanned boat, ω in formula (10)cFor unmanned boat
Current angular speed,For the angular acceleration of unmanned boat, θhFor the current bow of unmanned boat to.
By velocity window vdDiscrete is M speed, and bow is to window θdIt is discrete be N number of direction, it is discrete after a speed viWith
One direction θiForm a velocity vector (vi,θi), therefore a total of M × N number of velocity vector, by these unmanned boats in Δ t
The collection of interior accessible discrete velocity vector composition is collectively referred to as RV, as shown in Figure 3A.
Step 6, such as Fig. 2 calculate reachable avoidance velocity vector
The velocity vector that can be collided with barrier in RV set, meeting and barrier are calculated using the method in step 3
The collection of the velocity vector composition to collide is collectively referred to as VO set, reachable avoidance velocity vector set RAV is calculated, such as Fig. 3 B institutes
Show：
Step 7, such as Fig. 2 select optimal avoidance velocity vector
All velocity vectors all can serve as avoidance velocity vector, structure evaluation function G in RAV set：
V in formulac, θhIndicate unmanned boat current speed and bow to (v respectivelyi,θj) indicate a discrete velocity vector.
One is selected to make the velocity vector of evaluation function value minimum as avoidance velocity vector from RAV set.
Step 8, such as Fig. 2 judge whether that avoidance can be terminated
Judge that unmanned boat will not collide along initial desired speed with direction running, and target is arrived with present speed edge
When the movement of point direction will not collide, it is believed that avoidance can be terminated, that is, when meeting following formula：
And
It can terminate avoidance, enter step nine；When avoidance can not be terminated, avoidance speed and the direction of selection are exported, again
Enter step five.In formulaFor initial desired speed vector,Target point is arrived with present speed size edge for unmanned boat
Velocity vector when direction moves.
Step 9 exports initial desired speed and direction
When into the step, it is believed that will not collide when unmanned boat is along initial desired speed and direction running, then
Directly export initial desired speed and direction.
It can ensure that unmanned boat safely, effectively, reliably avoids the static state and dynamic that are encountered in navigation by above step
Barrier.
Fig. 4 is the design sketch of unmanned boat avoidance emulation there are when a dynamic barrier, in figure unmanned boat along track line to
Direct north navigates by water, and a barrier carries out avoidance from barrier rear, avoid obstacle to due west direction uniform motion, unmanned boat
After object, unmanned boat continues on initial desired speed and is moved with direction.
Fig. 5 is the design sketch of unmanned boat avoidance emulation there are when three dynamic barriers, in figure unmanned boat along track line to
Direct north navigates by water, and barrier 1 and barrier 3 are to due west direction uniform motion, and barrier 2 is to due east direction uniform motion, nothing
People's ship from the rear avoiding obstacles of three barriers, moves respectively then along initial desired speed and direction.
Claims (9)
1. a kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method, it is characterised in that including following step
Suddenly：
Step 1：Obtain obstacle information, unmanned boat information, desired speed and direction；
Step 2：Barrier processing；
Step 3：Judge whether unmanned boat can collide with barrier；
Step 4：Judge whether to need to start avoidance；
Step 5：Calculating speed window and bow are to window；
Step 6：Calculate reachable avoidance velocity vector；
Step 7：Select optimal avoidance velocity vector；
Step 8：Judge whether to terminate avoidance；
Step 9：Export initial desired speed and direction.
2. the unmanned boat dynamic obstacle avoidance algorithm according to claim 1 based on Speed Obstacles method and dynamic window method, special
Sign is：The step 1 obtains obstacle information, unmanned boat information, desired speed and direction：The obstacle information packet of acquisition
Include location information, dimension information, movable information；Location information is indicated under unmanned boat hull coordinate system with distance and angle, is transported
Dynamic information indicates that for the direction of motion on the basis of direct north, the unmanned boat information of acquisition includes position with speed and the direction of motion
Information, movement and posture information, location information in east northeast use (x, y) coordinate representation under coordinate system, and movable information includes speed
Size, directional velocity, acceleration, angular speed and angular acceleration, posture information be bow to angle, on the basis of direct north, it is expected that
Speed and direction provided by upper layer algorithm, on the basis of direct north, above-described angle and direction are all up times in direction
Needle direction is positive value.
3. the unmanned boat dynamic obstacle avoidance algorithm according to claim 1 based on Speed Obstacles method and dynamic window method, special
Sign is：The step 2, barrier processing：The length of the semi-major axis of oval barrier and semi-minor axis is added respectively it is oval nobody
The length of the semi-major axis of ship finds out the velocity vector set of barrier actual capabilities according to the speed and deflection error of barrier
VP is：
It is in formulaVelocity vector for the barrier obtained by sensor, VE are barrier velocity vector error set,For
Minkowski vector sum operation.
4. the unmanned boat dynamic obstacle avoidance algorithm according to claim 1 based on Speed Obstacles method and dynamic window method, special
Sign is：The step 3, judges whether unmanned boat can collide with barrier：Establish unmanned boat hull coordinate system and barrier
Coordinate system, barrier coordinate system is using barrier center as origin, and long axis direction is x-axis, and it is y-axis that x-axis, which rotates clockwise 90 degree, will
Coordinate of the unmanned boat in hull coordinate system is gone in barrier coordinate system, and oval barrier phase is found out in barrier coordinate system
For the point of contact of two tangent lines in unmanned boat center, two point of contact coordinates are gone under hull coordinate system, are found out in hull coordinate system
Left and right two tangent line of the lower ellipse barrier relative to unmanned boat center, finds out relative velocity of the unmanned boat relative to barrier,
If relative velocity direction is fallen into the two tangent line angles in left and right, unmanned boat can collide with barrier.
5. the unmanned boat dynamic obstacle avoidance algorithm according to claim 1 based on Speed Obstacles method and dynamic window method, special
Sign is：The step 4 judges whether to need to start avoidance：It calculates according to the following formula needed for unmanned boat and barrier collide
Time tCPA：
In formulaWithFor unmanned boat and the current position vector of barrier,WithIt is current with barrier for unmanned boat
Velocity vector；
Unmanned boat is found out according to the following formula to avoid hindering the time t needed for barriera：
In formula Chinese style (x, y), (xA,yA) it is respectively the coordinate of P points, A points in barrier coordinate system, v, θ0,ω0, α is respectively nothing
The current speed of people's ship, bow is to, angular speed, angular acceleration, PC1+PC2For in barrier coordinate system P points to oval bifocal
Distance and, a is oval semi-major axis；
Work as tCPA≤k×taWhen unmanned boat start avoidance, otherwise unmanned boat does not start avoidance.
6. the unmanned boat dynamic obstacle avoidance algorithm according to claim 1 based on Speed Obstacles method and dynamic window method, special
Sign is：The step 5, calculating speed window and bow are to window：The attainable speed of unmanned boat in the Δ t times is calculated, it is referred to as fast
Spend window；Unmanned boat bow is calculated in the Δ t times to the angle that can reach, referred to as bow to window, by velocity window it is discrete be M
Speed, it is N number of direction that bow is discrete to window, and discrete speed and direction form M × N number of velocity vector, these unmanned boats existed
The collection of accessible discrete velocity vector composition is collectively referred to as RV in the Δ t times.
7. the unmanned boat dynamic obstacle avoidance algorithm according to claim 1 based on Speed Obstacles method and dynamic window method, special
Sign is：The step 6 calculates reachable avoidance velocity vector：The velocity vector that can be collided with barrier during RV is gathered
The collection of composition is collectively referred to as VO set, is calculate by the following formula reachable avoidance velocity vector set RAV：
8. the unmanned boat dynamic obstacle avoidance algorithm according to claim 1 based on Speed Obstacles method and dynamic window method, special
Sign is：The step 7 selects optimal avoidance velocity vector：Build evaluation function：
V in formulac, θhIndicate unmanned boat current speed and bow to (v respectivelyi,θj) indicate a discrete velocity vector,
One is selected to make the velocity vector of evaluation function value minimum as avoidance velocity vector from RAV set.
9. the unmanned boat dynamic obstacle avoidance algorithm according to claim 1 based on Speed Obstacles method and dynamic window method, special
Sign is：The step 8 judges whether to terminate avoidance：Judge that unmanned boat will not occur along initial desired speed with direction running
Collision, and with present speed along moved to target point direction will not collide when, that is, terminate avoidance.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810318563.3A CN108664020A (en) | 2018-04-11 | 2018-04-11 | A kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810318563.3A CN108664020A (en) | 2018-04-11 | 2018-04-11 | A kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108664020A true CN108664020A (en) | 2018-10-16 |
Family
ID=63783276
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810318563.3A Pending CN108664020A (en) | 2018-04-11 | 2018-04-11 | A kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108664020A (en) |
Cited By (23)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109358615A (en) * | 2018-09-07 | 2019-02-19 | 上海大学 | Unmanned water surface ship VO barrier-avoiding method based on sensor detection barrier velocity error |
| CN109828574A (en) * | 2019-02-22 | 2019-05-31 | 深兰科技（上海）有限公司 | A kind of barrier-avoiding method and electronic equipment |
| CN109828566A (en) * | 2019-01-30 | 2019-05-31 | 华南理工大学 | A kind of unmanned surface vehicle autonomous navigation method |
| CN109916400A (en) * | 2019-04-10 | 2019-06-21 | 上海大学 | A kind of unmanned boat barrier-avoiding method combined based on gradient descent algorithm with VO method |
| CN109960261A (en) * | 2019-03-22 | 2019-07-02 | 北京理工大学 | A kind of dynamic barrier preventing collision method based on collision detection |
| CN110018689A (en) * | 2019-05-15 | 2019-07-16 | 福州大学 | A kind of more virtual target point overall situation active path planning algorithms based on dynamic window |
| CN110083159A (en) * | 2019-05-14 | 2019-08-02 | 江苏科技大学 | The autonomous dynamic collision prevention method of unmanned boat constrained based on SBG and dynamic window |
| CN110208816A (en) * | 2019-06-04 | 2019-09-06 | 浙江海洋大学 | For the automatic differentiating obstacle of marine unmanned boat and recognition methods |
| CN110580045A (en) * | 2019-08-30 | 2019-12-17 | 安徽科微智能科技有限公司 | obstacle avoidance method for unmanned ship |
| CN110597271A (en) * | 2019-10-12 | 2019-12-20 | 河北工业大学 | Mobile robot obstacle avoidance method based on hierarchical velocity obstacle algorithm |
| CN110608744A (en) * | 2019-10-30 | 2019-12-24 | 集美大学 | Water quality sampling unmanned ship path planning method with dynamic obstacle avoidance function |
| CN110837255A (en) * | 2019-11-08 | 2020-02-25 | 哈尔滨工程大学 | Autonomous danger avoiding method suitable for high-speed water surface unmanned ship |
| CN110865640A (en) * | 2019-10-11 | 2020-03-06 | 黄少豪 | Obstacle avoidance structure of intelligent robot |
| CN111121812A (en) * | 2019-12-31 | 2020-05-08 | 深圳前海达闼云端智能科技有限公司 | Path optimization method, electronic device and storage medium |
| CN111580518A (en) * | 2020-05-12 | 2020-08-25 | 哈尔滨工程大学 | Unmanned ship layered obstacle avoidance method based on improved drosophila optimization and dynamic window method |
| CN111709633A (en) * | 2020-06-09 | 2020-09-25 | 吉林大学 | Method, device and equipment for determining collision risk degree and storable medium |
| CN111964678A (en) * | 2020-07-16 | 2020-11-20 | 武汉长江船舶设计院有限公司 | River channel navigation decision-making method, device and system |
| CN112330959A (en) * | 2020-10-30 | 2021-02-05 | 东北大学 | Optimal peer-to-peer collision avoidance method for unmanned vehicle |
| CN113232025A (en) * | 2021-06-07 | 2021-08-10 | 上海大学 | Mechanical arm obstacle avoidance method based on proximity perception |
| CN113419522A (en) * | 2021-05-21 | 2021-09-21 | 北京航天控制仪器研究所 | Simulation method and system for unmanned ship path planning algorithm |
| CN113625711A (en) * | 2021-08-02 | 2021-11-09 | 福州大学 | Self-adaptive dynamic window unmanned vehicle real-time obstacle avoidance method based on danger coefficient |
| CN113671968A (en) * | 2021-08-25 | 2021-11-19 | 大连海事大学 | Unmanned surface vessel real-time collision avoidance method based on improved dynamic optimal reciprocal collision avoidance mechanism |
| WO2023066012A1 (en) * | 2021-10-18 | 2023-04-27 | 灵动科技（北京）有限公司 | Motion control method for mobile robot, and computer program product |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105717923A (en) * | 2016-01-16 | 2016-06-29 | 上海大学 | Unmanned surface vessel ocean dynamic obstacle avoiding control algorithm based on ellipse clustering-collision awl deduction |
| WO2017129863A1 (en) * | 2016-01-29 | 2017-08-03 | Rolls-Royce Oy Ab | Autonomous operation of a vessel |
| CN107037809A (en) * | 2016-11-02 | 2017-08-11 | 哈尔滨工程大学 | A kind of unmanned boat collision prevention method based on improvement ant group algorithm |
| CN108459602A (en) * | 2018-02-28 | 2018-08-28 | 哈尔滨工程大学 | The autonomous method of docking of drive lacking unmanned boat under more obstacle complex environments |
-
2018
- 2018-04-11 CN CN201810318563.3A patent/CN108664020A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105717923A (en) * | 2016-01-16 | 2016-06-29 | 上海大学 | Unmanned surface vessel ocean dynamic obstacle avoiding control algorithm based on ellipse clustering-collision awl deduction |
| WO2017129863A1 (en) * | 2016-01-29 | 2017-08-03 | Rolls-Royce Oy Ab | Autonomous operation of a vessel |
| CN107037809A (en) * | 2016-11-02 | 2017-08-11 | 哈尔滨工程大学 | A kind of unmanned boat collision prevention method based on improvement ant group algorithm |
| CN108459602A (en) * | 2018-02-28 | 2018-08-28 | 哈尔滨工程大学 | The autonomous method of docking of drive lacking unmanned boat under more obstacle complex environments |
Non-Patent Citations (1)
| Title |
|---|
| 张洋洋,瞿栋,柯俊,李小毛: "基于速度障碍法和动态窗口法的无人水面艇动态避障", 《上海大学学报（自然科学版）》 * |
Cited By (36)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109358615A (en) * | 2018-09-07 | 2019-02-19 | 上海大学 | Unmanned water surface ship VO barrier-avoiding method based on sensor detection barrier velocity error |
| CN109828566A (en) * | 2019-01-30 | 2019-05-31 | 华南理工大学 | A kind of unmanned surface vehicle autonomous navigation method |
| CN109828566B (en) * | 2019-01-30 | 2022-05-24 | 华南理工大学 | Autonomous sailing method for unmanned surface vehicle |
| CN109828574A (en) * | 2019-02-22 | 2019-05-31 | 深兰科技（上海）有限公司 | A kind of barrier-avoiding method and electronic equipment |
| CN109828574B (en) * | 2019-02-22 | 2022-05-03 | 深兰机器人(上海)有限公司 | Obstacle avoidance method and electronic equipment |
| CN109960261A (en) * | 2019-03-22 | 2019-07-02 | 北京理工大学 | A kind of dynamic barrier preventing collision method based on collision detection |
| CN109960261B (en) * | 2019-03-22 | 2020-07-03 | 北京理工大学 | Dynamic obstacle avoiding method based on collision detection |
| CN109916400A (en) * | 2019-04-10 | 2019-06-21 | 上海大学 | A kind of unmanned boat barrier-avoiding method combined based on gradient descent algorithm with VO method |
| CN110083159B (en) * | 2019-05-14 | 2022-03-08 | 江苏科技大学 | Unmanned ship autonomous dynamic collision avoidance method based on SBG and dynamic window constraint |
| CN110083159A (en) * | 2019-05-14 | 2019-08-02 | 江苏科技大学 | The autonomous dynamic collision prevention method of unmanned boat constrained based on SBG and dynamic window |
| CN110018689A (en) * | 2019-05-15 | 2019-07-16 | 福州大学 | A kind of more virtual target point overall situation active path planning algorithms based on dynamic window |
| CN110208816A (en) * | 2019-06-04 | 2019-09-06 | 浙江海洋大学 | For the automatic differentiating obstacle of marine unmanned boat and recognition methods |
| CN110208816B (en) * | 2019-06-04 | 2023-05-16 | 浙江海洋大学 | Automatic obstacle recognition system and recognition method for unmanned submarine |
| CN110580045A (en) * | 2019-08-30 | 2019-12-17 | 安徽科微智能科技有限公司 | obstacle avoidance method for unmanned ship |
| CN110865640A (en) * | 2019-10-11 | 2020-03-06 | 黄少豪 | Obstacle avoidance structure of intelligent robot |
| CN110865640B (en) * | 2019-10-11 | 2024-03-12 | 黄少豪 | Obstacle avoidance structure of intelligent robot |
| CN110597271A (en) * | 2019-10-12 | 2019-12-20 | 河北工业大学 | Mobile robot obstacle avoidance method based on hierarchical velocity obstacle algorithm |
| CN110608744A (en) * | 2019-10-30 | 2019-12-24 | 集美大学 | Water quality sampling unmanned ship path planning method with dynamic obstacle avoidance function |
| CN110837255B (en) * | 2019-11-08 | 2022-07-15 | 哈尔滨工程大学 | Autonomous danger avoiding method suitable for high-speed water surface unmanned ship |
| CN110837255A (en) * | 2019-11-08 | 2020-02-25 | 哈尔滨工程大学 | Autonomous danger avoiding method suitable for high-speed water surface unmanned ship |
| CN111121812B (en) * | 2019-12-31 | 2022-04-08 | 达闼机器人有限公司 | Path optimization method, electronic device and storage medium |
| CN111121812A (en) * | 2019-12-31 | 2020-05-08 | 深圳前海达闼云端智能科技有限公司 | Path optimization method, electronic device and storage medium |
| CN111580518B (en) * | 2020-05-12 | 2022-08-02 | 哈尔滨工程大学 | Unmanned ship layered obstacle avoidance method based on improved drosophila optimization and dynamic window method |
| CN111580518A (en) * | 2020-05-12 | 2020-08-25 | 哈尔滨工程大学 | Unmanned ship layered obstacle avoidance method based on improved drosophila optimization and dynamic window method |
| CN111709633A (en) * | 2020-06-09 | 2020-09-25 | 吉林大学 | Method, device and equipment for determining collision risk degree and storable medium |
| CN111709633B (en) * | 2020-06-09 | 2022-09-06 | 吉林大学 | Method, device and equipment for determining collision risk degree and storable medium |
| CN111964678A (en) * | 2020-07-16 | 2020-11-20 | 武汉长江船舶设计院有限公司 | River channel navigation decision-making method, device and system |
| CN112330959A (en) * | 2020-10-30 | 2021-02-05 | 东北大学 | Optimal peer-to-peer collision avoidance method for unmanned vehicle |
| CN113419522B (en) * | 2021-05-21 | 2022-08-12 | 航天时代(青岛)海洋装备科技发展有限公司 | Simulation method and system for unmanned ship path planning algorithm |
| CN113419522A (en) * | 2021-05-21 | 2021-09-21 | 北京航天控制仪器研究所 | Simulation method and system for unmanned ship path planning algorithm |
| CN113232025B (en) * | 2021-06-07 | 2022-04-22 | 上海大学 | Mechanical arm obstacle avoidance method based on proximity perception |
| CN113232025A (en) * | 2021-06-07 | 2021-08-10 | 上海大学 | Mechanical arm obstacle avoidance method based on proximity perception |
| CN113625711A (en) * | 2021-08-02 | 2021-11-09 | 福州大学 | Self-adaptive dynamic window unmanned vehicle real-time obstacle avoidance method based on danger coefficient |
| CN113671968A (en) * | 2021-08-25 | 2021-11-19 | 大连海事大学 | Unmanned surface vessel real-time collision avoidance method based on improved dynamic optimal reciprocal collision avoidance mechanism |
| CN113671968B (en) * | 2021-08-25 | 2023-12-12 | 大连海事大学 | Real-time collision prevention method for unmanned surface vessel |
| WO2023066012A1 (en) * | 2021-10-18 | 2023-04-27 | 灵动科技（北京）有限公司 | Motion control method for mobile robot, and computer program product |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108664020A (en) | A kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method | |
| CN108445879B (en) | Unmanned ship obstacle avoidance method based on collision danger prediction area | |
| Whitcomb et al. | Combined Doppler/LBL based navigation of underwater vehicles | |
| CN108698677B (en) | Method for setting path of underwater vehicle, method for optimally controlling underwater vehicle using the same, and underwater vehicle | |
| US7389735B2 (en) | Docking supporting apparatus, and marine vessel including the apparatus | |
| Bandyophadyay et al. | A simple reactive obstacle avoidance algorithm and its application in singapore harbor | |
| CN105547290B (en) | It is a kind of based on ultra short baseline locating system from latent device air navigation aid | |
| Matsuda et al. | Performance analysis on a navigation method of multiple AUVs for wide area survey | |
| CN110727282B (en) | AUV docking method and device and underwater docking system | |
| CN110262482A (en) | A kind of unmanned boat speed of a ship or plane control method and unmanned boat | |
| CN107976188A (en) | A kind of AUV led to based on ranging sound returns depressed place air navigation aid | |
| CN114061565B (en) | Unmanned ship SLAM and application method thereof | |
| Xinjing et al. | Horizontal path following for underactuated AUV based on dynamic circle guidance | |
| CN112666936B (en) | Intelligent obstacle avoidance system and method for semi-submersible unmanned ship | |
| Hover et al. | A vehicle system for autonomous relative survey of in-water ships | |
| Mandić et al. | Mobile beacon control algorithm that ensures observability in single range navigation | |
| CN110471455B (en) | Black box acoustic beacon search and exploration route planning method based on deep submergence vehicle | |
| JPH11249734A (en) | Autonomous guidance device | |
| CN116540727A (en) | Under-actuated unmanned surface vessel autonomous berthing method based on multi-sensor fusion positioning | |
| CN108287538A (en) | One kind being based on the unmanned boat system of RTK technologies | |
| CN114089750A (en) | Semi-physical unmanned ship berthing system based on artificial potential field method and evaluation method | |
| CN110456786A (en) | UUV sea-floor relief tracking system and tracking based on Extraction of Topographic Patterns | |
| CN116486252A (en) | Intelligent unmanned search and rescue system and search and rescue method based on improved PV-RCNN target detection algorithm | |
| Noguchi et al. | Wide area seafloor imaging by a low-cost AUV | |
| CN108761467A (en) | A kind of underwater map constructing method of three-dimensional based on Forward-looking Sonar |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20181016 |