CN116974282A - Unmanned ship-based marine search area coverage path planning method - Google Patents
Unmanned ship-based marine search area coverage path planning method Download PDFInfo
- Publication number
- CN116974282A CN116974282A CN202310936442.6A CN202310936442A CN116974282A CN 116974282 A CN116974282 A CN 116974282A CN 202310936442 A CN202310936442 A CN 202310936442A CN 116974282 A CN116974282 A CN 116974282A
- Authority
- CN
- China
- Prior art keywords
- unmanned ship
- obstacle
- unmanned
- search area
- ship
- 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 58
- 238000005192 partition Methods 0.000 claims abstract description 22
- 238000001514 detection method Methods 0.000 claims description 14
- 230000009466 transformation Effects 0.000 claims description 12
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 230000033001 locomotion Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000008602 contraction Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000004888 barrier function Effects 0.000 claims description 2
- 101100001677 Emericella variicolor andL gene Proteins 0.000 claims 1
- 230000008569 process Effects 0.000 abstract description 12
- 230000003068 static effect Effects 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000002146 bilateral effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a method for planning a coverage path of an offshore search area based on an unmanned ship, which comprises the following steps of S1: acquiring an offshore search area; s2: constructing an unmanned ship system model; s3: determining a searching direction, and performing global path planning on a searching area; s4: dividing a local path by adopting an artificial potential field rule, constructing a repulsive potential field function, setting a plurality of unmanned ship obstacle avoidance decision partitions, and configuring a corresponding partition repulsive potential field function for each unmanned ship obstacle avoidance decision partition; wherein the repulsive potential field function can represent the distance relation between the unmanned ship and the target point; s5: the unmanned ship navigates in the search area according to the original planned path and detects the target and the unknown obstacle: s6: and after the unmanned ship searches the complete search area, ending the task. The invention builds a static route by using double-layer planning; and a dynamic route is constructed by adopting a multi-obstacle avoidance algorithm in the unmanned ship navigation process, so that the safe navigation of the unmanned ship is ensured, and the requirement on uniform coverage of a search area is met.
Description
Technical Field
The invention relates to the technical field of unmanned ship intelligent control, in particular to a method for planning a coverage path of an offshore search area based on an unmanned ship.
Background
With the rapid development of shipping industry, maritime traffic trade is more frequent, maritime emergencies are more complex, and safety problems of maritime personnel must be more important. Maritime search and rescue are necessary means for guaranteeing maritime life property safety and creating good maritime transportation environment, the search and rescue comprises two parts, the search is the premise and the key of all rescue works, but from the current situation of maritime search and rescue works at present, the maritime search and rescue works mainly depend on manual driving ships to execute search tasks, are influenced by search facilities, coordination capacity and the like, and have the problems of long search time, low efficiency, easiness in omission and false detection and the like.
The unmanned ship on the water surface is used as an intelligent ship with functions of autonomous navigation, control, communication transmission and the like, has the characteristics of autonomy, flexibility, intelligence and informatization, provides a new way for the development of maritime search and rescue work by utilizing the unmanned ship to execute the search task, has high maneuverability, low cost and wide search range, can greatly improve the search efficiency, expands the search range and continuously executes the search task in a severe marine environment day and night.
In practice, when the search environment is completely known, a satisfactory result can be obtained by the currently available path planning method, but in an unknown environment, for example, when the number and positions of targets or obstacles are unknown, the existing path planning method cannot meet the uniform coverage of the search area, resulting in the problems of target omission and the like, so that when planning an unmanned-plane marine search path, consideration needs to be given to how to realize the maximum coverage of the search area in the unknown environment.
Disclosure of Invention
The invention provides an unmanned ship-based method for planning a coverage path of an offshore search area, which aims to solve the problem that the existing path planning method cannot meet the requirement of uniformly covering the search area under an unknown environment, so that a target is omitted.
In order to achieve the above object, the technical scheme of the present invention is as follows:
an unmanned ship-based marine search area coverage path planning method comprises the following specific steps:
s1: acquiring an offshore search area and reading electronic chart information of the search area;
s2: constructing an unmanned ship system model based on a ship body coordinate system and a global coordinate system;
s3: determining a searching direction, carrying out global path planning on a searching area based on parallel line scanning searching, and setting the interval width between two parallel line paths in the searching area, namely setting the width of a scanning area when the unmanned ship is in direct line along a parallel line as D;
s4: according to known obstacle information in a search area provided by an electronic chart, constructing a repulsive potential field function U when a manual potential field method is adopted for local path planning o (X) setting a plurality of unmanned ship obstacle avoidance decision partitions, and configuring a corresponding partition repulsive potential field function for each unmanned ship obstacle avoidance decision partition; wherein the repulsive potential field function U o (X) being capable of characterizing the distance relationship d (X, X) between the unmanned aerial vehicle and the target point g )；
S5: the unmanned ship navigates in a search area according to an original planning path determined by global path planning and local path planning, and detects targets and unknown obstacles through sensors:
when a target is detected, transmitting the target coordinate under the global coordinate system to a shore base or a search and rescue ship, and continuing to navigate along an original planning path for searching;
when an unknown obstacle is detected, the unmanned ship judges whether an obstacle avoidance target point exists according to the coordinates of the unknown obstacle under a global coordinate system, if the obstacle avoidance target point exists, a path is planned by adopting an artificial potential field method to avoid the obstacle, otherwise, the path is planned by adopting a Bug1 algorithm or a Bug2 algorithm to avoid the obstacle, and after the unmanned ship leaves the unknown obstacle, the unmanned ship navigates back to the original planned path to continue to navigate for searching;
s6: and after the unmanned ship searches the whole search area, ending the search task.
Further, in the step S2, the equation of motion of the unmanned ship system model is set as:
wherein: x is the longitudinal displacement of the unmanned ship, y is the transverse displacement of the unmanned ship, mu is the longitudinal speed of the unmanned ship, v is the transverse speed of the unmanned ship, psi is the heading angle, and r is the turning heading angle speed.
Further, in the step S3,
s31: determining the width of the search area, and determining the search direction according to the width of the search area:
1) If the search area is rectangular, the width of the rectangle is the width of the search area;
2) If the search area is a convex polygon, the width calculation method of the convex polygon search area is as follows: setting the vertex of the convex polygon to be P= { P 1 ，p 2 ，…，p n Each side is S= { S 1 ，S 2 ，…，S n Sequentially calculating the distance D from all the other vertexes except the two vertexes on each edge to the edge p3-s1 ，D p4-s1 ，…，D pn-s1 Taking the maximum value as the span D of each edge s1 ＝max{D p3-s1 ，D p4-s1 ，…，D pn-s1 Taking the minimum value in the span as the width W=min { D { of the convex polygon } si I = 1,2, …, n }; when global path planning is performed, the searching direction is the same or opposite to the width direction of the searching areaOrientation;
s32: and setting the interval width D between two parallel line paths in the search area according to the width of the inscribed square in the detection range of the unmanned ship, namely the width of the area swept by the unmanned ship when the unmanned ship is in direct line along the parallel line.
Further, in S4, according to the known obstacle information in the search area provided by the electronic chart, when the artificial potential field method is adopted to perform local path planning, a repulsive potential field function U is constructed o (X) regarding a round area with the ship position of the unmanned ship as the center and tau as the radius as an area where the unmanned ship body is located, namely a forbidden area, and setting the safe distance of the unmanned ship as D safe The maximum detection distance of the unmanned ship sensor is D sen Setting up a plurality of unmanned boats and keeping away barrier decision partition, include:
t < d (X, X) o )≤D safe The annular area of the (C) is set as an emergency external expansion area; will D safe ＜d(X，X o )≤D s Is set as a conventional obstacle avoidance zone, wherein D s ＝D sen -D safe The method comprises the steps of carrying out a first treatment on the surface of the Will D s ＜d(X，X o )≤D sen Is set as an emergency contraction zone; will d (X, X) o )＞D sen The area of (2) is set as a safe area;
configuring a corresponding partition repulsive potential field function for each unmanned ship obstacle avoidance decision partition, wherein the repulsive potential field function U o (X) being capable of characterizing the distance relationship d (X, X) between the unmanned aerial vehicle and the target point g ) The corresponding formula is:
in U o (X) is a repulsive potential field function; k (k) o Is a gain coefficient; d (X, X) o ) Is the distance between the unmanned ship and the obstacle; d, d o Is the influence distance of the repulsive force of the obstacle; d (X, X) g ) Is the distance between the unmanned boat and the target point.
Further, in the step S5, the unmanned ship firstly acquires the coordinates of the target or the unknown obstacle in the sensor coordinate system through the sensor, and then converts the coordinates of the target or the unknown obstacle in the sensor coordinate system into the coordinates of the target or the unknown obstacle in the global coordinate system, and the specific steps are as follows:
s51: it is assumed that the sensor is located at a point (x r 0) the coordinates of the unknown obstacle in the sensor coordinate system are (d, θ) 1 ) Firstly converting the coordinates of an unknown obstacle in a sensor coordinate system into coordinates (x ', y') in a ship body coordinate system according to a coordinate transformation formula, wherein the corresponding coordinate transformation formula is as follows:
s52: the obtained coordinates of the unknown obstacle in the hull coordinate system are calculated according to the global coordinates (x u ，y u ) And a coordinate transformation formula for transforming the coordinates (x b ，y b ) The corresponding coordinate transformation formula is:
wherein alpha is u Is unmanned ship navigation direction and global coordinate system O 0 x 0 Included angle of axial direction.
Further, in the step S5,
when the unmanned aerial vehicle detects an unknown obstacle through the sensor and can detect the boundary of the unknown obstacle, if the unmanned aerial vehicle can find an obstacle avoidance target point according to the coordinates of the unknown obstacle under the global coordinate system, an artificial potential field rule path is adopted for obstacle avoidance;
if the unmanned aerial vehicle cannot find the obstacle avoidance target point according to the coordinates of the unknown obstacle in the global coordinate system, namely the unmanned aerial vehicle detects the unknown obstacle through the sensor and the boundary L of the unknown obstacle in the Ox axis direction of the ship body coordinate system x Out of the detection range of unmanned boats, namely L x ＞(D sen -D safe ) When the unmanned ship is in the forward direction, the boundary length L of the unknown obstacle on the right side of the origin O in the Oy axis direction of the ship body coordinate system is determined y_right Boundary length L located to the left of origin O y_left And the comparison relation between the swept area widths D when the unmanned ship is in a straight line along a parallel line path is selected to avoid the obstacle by adopting a Bug1 algorithm or a Bug2 algorithm, and the method specifically comprises the following steps:
mode 1: when the sensor detects L of unknown obstacle y_right Not less than D/2 and L y_left When the speed is less than D/2, the unmanned ship breaks away from the unknown obstacle by adopting a Bug2 algorithm, sets m-line as a global path, and selects to turn to the left to navigate so as to bypass the unknown obstacle, and leaves the unknown obstacle when meeting the m-line again, and re-navigates to the original planning path to continue navigating;
mode 2: when the sensor detects L of unknown obstacle y_right < D/2 and L y_left When the distance between the unmanned aerial vehicle and the unknown obstacle is not less than D/2, the unmanned aerial vehicle breaks away from the unknown obstacle by adopting a Bug2 algorithm, an m-line is set as a global path, the unmanned aerial vehicle selectively turns to the right to navigate so as to bypass the unknown obstacle, and when the unmanned aerial vehicle meets the m-line again, the unmanned aerial vehicle leaves the unknown obstacle and sails on the original planning path again;
mode 3: when the sensor detects an unknown obstacle L y_right < D/2 and L y_left When the speed is less than D/2, the unmanned ship bypasses the unknown obstacle by adopting a Bug1 algorithm, an m-line is set as a global path, the unmanned ship returns to the initial position after bypassing the unknown obstacle for a circle, and the length L of the route before and after meeting the m-line is recorded 1 And L 2 If L 1 ＞L 2 The unmanned ship is L along the length of the route 1 And vice versa along the path length L 2 And (3) when the unmanned ship meets the m-line again, the unmanned ship leaves the unknown obstacle, and sails back to the original planning path to continue sailing.
Further, in S5, when the unmanned aerial vehicle navigates in the search area according to the determined original planned path, a determination rule is set as to whether the unmanned aerial vehicle enters a deadlock state in the navigation process, and if it is determined that the unmanned aerial vehicle enters the deadlock state, the following steps are:
<F a (X)，F o (X)>＝acos(dot(F a (X)，F o (X))/(norm(F a (X))*norm(F o (X))))*180/pi
at this time, a random deflection angle θ of the unmanned boat is set 2 ，θ 2 The value range of (2) is 0, 90 DEG]The unmanned ship is separated from the deadlock state, namely:
F a ′(X)＝cos -1 θ 2 *F a (X)
wherein F is a (X) is the attraction of unmanned boat, F a 'X' is a random deflection angle theta of the unmanned ship 2 Attraction force of unmanned ship after F o (X) is the repulsive force of the unmanned ship,is the navigation direction of the unmanned ship.
The beneficial effects are that: according to the invention, a parallel line scanning searching rule is applied to define a global path, and a local path is planned through an improved artificial potential field method, in the navigation process of the unmanned aerial vehicle according to an original planning path, environment information of the unmanned aerial vehicle during navigation is obtained in real time through an unmanned aerial vehicle sensor, unknown obstacles are perceived, a mode of combining multiple obstacle avoidance algorithms is adopted in the process of approaching the unknown obstacles, different obstacle avoidance methods are selected according to different unknown obstacles, the safe autonomous navigation of the unmanned aerial vehicle is ensured, the requirement on uniform coverage of a searching area is met, and the problem that the target is missed due to the fact that the uniform coverage of the searching area cannot be achieved is solved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions of the prior art, the drawings that are needed in the embodiments or the description of the prior art will be briefly described below, it will be obvious that the drawings in the following description are some embodiments of the present invention, and that other drawings can be obtained according to these drawings without inventive effort to a person skilled in the art.
FIG. 1 is a flow chart showing the steps of the unmanned ship-based method for planning a coverage path of an offshore search area according to the present invention;
FIG. 2 is a schematic view of the vertical and horizontal viewing angles detected by the unmanned boat sensor of the present invention;
FIG. 3 is a schematic view of the width of a region swept by an unmanned ship along a parallel path in the present invention;
FIG. 4 is a schematic diagram of a plurality of unmanned boats obstacle avoidance decision partitions arranged in the invention;
FIG. 5a is a schematic diagram of an unmanned boat detecting obstacle avoidance target point according to the present invention;
FIG. 5b is a schematic illustration of an unmanned boat not detecting obstacle avoidance target point in the present invention;
FIG. 6 is a block avoidance flow chart of the unmanned boat based on the Bug1 and Bug2 algorithms;
FIG. 7 is a schematic diagram of the unmanned boat of the present invention sailing with the Bug2 algorithm turning to the left;
FIG. 8 is a schematic diagram of an unmanned boat steering to the right for sailing by adopting a Bug2 algorithm;
FIG. 9 is a schematic diagram of the unmanned boat of the present invention following a complete round of unknown obstacles using the Bug1 algorithm, and selecting a voyage route.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments of the present invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
In the invention, double-layer planning is utilized, a global path is firstly planned by known environmental information and parallel line scanning search regulation, and static obstacle avoidance is completed on known obstacles by an improved artificial potential field method, so that a static route is constructed; in the navigation process of the unmanned ship, the original planning path is changed into an unreachable area by the unknown obstacle, so that the unmanned ship acquires the environment information during navigation in real time through the sensor, perceives the unknown obstacle, adopts a mode of combining a plurality of obstacle avoidance algorithms in the process of approaching the unknown obstacle, adopts different obstacle avoidance methods, efficiently avoids the obstacle, meets the requirement of covering a search area in the obstacle avoidance process, and accordingly constructs a dynamic route, and finally guarantees the safe autonomous navigation of the unmanned ship through a global-local two-stage obstacle avoidance method.
Based on the above principle, the present embodiment provides a method for planning a coverage path of an offshore search area based on an unmanned ship, as shown in fig. 1, which specifically includes the following steps:
s1: acquiring an offshore search area and reading electronic chart information of the search area;
s2: constructing an unmanned ship system model based on a ship body coordinate system and a global coordinate system;
the ship body coordinate system O-xyz takes the front-back bilateral symmetry point of the unmanned ship as an origin O, is fixed on a rectangular coordinate system on the ship body, moves along with the movement of the unmanned ship, and sets the Ox axis as the navigation direction of the unmanned ship, and the Ox axis rotates 90 degrees clockwise as the direction of the Oy axis, wherein the Oz axis is perpendicular to the water plane;
ground coordinate system, global coordinate system O 0 -x 0 y 0 z 0 Is an inertial coordinate system fixed on the earth's surface, the position of which can be arbitrarily selected, and which prescribes O 0 x 0 The axis is in the horizontal plane, the direction of the axis can be arbitrarily selected, once the axis is selected, the axis is fixed, O is 0 x 0 The clockwise rotation of the shaft by 90 deg. is O 0 y 0 Direction of axis, O 0 z 0 The axis is perpendicular to the horizontal plane.
The following assumptions are made in this embodiment: neglecting the effects of earth curvature and treating the sea surface as a plane; ignoring the change in the Z-axis direction, the unmanned boat moves only in the XY direction; the influence of wind and wave current on unmanned ship navigation is not considered.
The equation of motion of the unmanned ship system model is set as:
wherein: x is the longitudinal displacement of the unmanned ship, y is the transverse displacement of the unmanned ship, mu is the longitudinal speed of the unmanned ship, v is the transverse speed of the unmanned ship, psi is the heading angle, and r is the turning heading angle speed;
s3: determining a searching direction, carrying out global path planning on a searching area based on parallel line scanning searching, and setting the interval width between two parallel line paths in the searching area, namely setting the width of a scanning area when the unmanned ship is in direct line along a parallel line as D;
s4: according to known obstacle information in a search area provided by an electronic chart, constructing a repulsive potential field function U when a manual potential field method is adopted for local path planning o (X) setting a plurality of unmanned ship obstacle avoidance decision partitions, and configuring a corresponding partition repulsive potential field function for each unmanned ship obstacle avoidance decision partition; wherein the repulsive potential field function U o (X) being capable of characterizing the distance relationship d (X, X) between the unmanned aerial vehicle and the target point g )；
S5: the unmanned ship navigates in a search area according to an original planning path determined by global path planning and local path planning, and detects targets and unknown obstacles through sensors:
when a target is detected, transmitting the target coordinate under the global coordinate system to a shore base or a search and rescue ship, and continuing to navigate along an original planning path for searching;
when an unknown obstacle is detected, the unmanned ship judges whether an obstacle avoidance target point exists according to the coordinates of the unknown obstacle under a global coordinate system, if the obstacle avoidance target point exists, a path is planned by adopting an artificial potential field method to avoid the obstacle, otherwise, the path is planned by adopting a Bug1 algorithm or a Bug2 algorithm to avoid the obstacle, and after the unmanned ship leaves the unknown obstacle, the unmanned ship navigates back to the original planned path to continue to navigate for searching;
in this embodiment, when the unmanned aerial vehicle navigates in the search area according to the determined original planned path, a determination rule is set as to whether the unmanned aerial vehicle enters a deadlock state in the navigation process, and if it is determined that the unmanned aerial vehicle enters the deadlock state, the following steps are:
<F a (X)，F o (X)>＝acos(dot(F a (X)，F o (X))/(norm(F a (X))*norm(F o (X))))*180/pi
at this time, a random deflection angle θ of the unmanned boat is set 2 ，θ 2 The value range of (2) is 0, 90 DEG]The unmanned ship is separated from the deadlock state, namely:
F a ′(X)＝cos -1 θ 2 *F a (X)
wherein F is a (X) is the attraction of unmanned boat, F a 'X' is a random deflection angle theta of the unmanned ship 2 Attraction force of unmanned ship after F o (X) is the repulsive force of the unmanned ship,is the navigation direction of the unmanned ship.
S6: and after the unmanned ship searches the whole search area, ending the search task.
In a specific embodiment, in S3,
s31: determining the width of the search area, and determining the search direction according to the width of the search area:
1) If the search area is rectangular, the width of the rectangle is the width of the search area;
2) If the search area is a convex polygon, the width calculation method of the convex polygon search area is as follows: setting the vertex of the convex polygon to be P= { P 1 ，p 2 ，…，p n Each side is S= { S 1 ，S 2 ，…，S n Sequentially calculating the distance D from all the other vertexes except the two vertexes on each edge to the edge p3-s1 ，D p4-s1 ，…，D pn-s1 Taking the maximum value as the span D of each edge s1 ＝max{D p3-s1 ，D p4-s1 ，…，D pn-s1 Taking the minimum value in the span as the width W=min { D { of the convex polygon } si |i＝1，2，…，n}；
When global path planning is carried out, the searching direction is consistent with or opposite to the direction of the width of the corresponding searching area, and the coverage of the searching area is realized by minimum turning times;
in practice, if the search area is a concave polygon, dividing the concave polygon into a plurality of convex polygons, and determining the search direction by referring to the convex polygons to perform path planning;
s32: when global route planning is carried out, as shown in fig. 3, the interval width D between two parallel line paths in a search area is set according to the width of a square connected in the detection range of the unmanned ship, namely the width of a scanning area when the unmanned ship is in direct line along the parallel line path, so that the search coverage rate of the sensor is ensured;
in this embodiment, in consideration of the movement characteristics of the unmanned ship, a bezier transition curve is constructed for processing the turning point and the turning point, and smooth optimization processing is performed.
Because the traditional artificial potential field method has the problems of easy sinking of local minimum points and unreachable targets, and the requirement on area coverage needs to be met when the traditional artificial potential field method is applied to search path planning, the embodiment improves the artificial potential field method as follows:
according to known obstacle information in a search area provided by an electronic chart, constructing a repulsive potential field function U when a manual potential field method is adopted for local path planning o (X) regarding a round area with the ship position of the unmanned ship as the center and tau as the radius as an area where the unmanned ship body is located, namely a forbidden area, and setting the safe distance of the unmanned ship as D safe The maximum detection distance of the unmanned ship sensor is D sen Setting a plurality of unmanned boats obstacle avoidance decision partitions comprising：
T < d (X, X) o )≤D safe Is set as an emergency outer expansion area, D safe ＜d(X，X o )≤D s Is set as a conventional obstacle avoidance zone, wherein D s ＝D sen -D safe D is to s ＜d(X，X o )≤D sen Is set as an emergency contraction zone, d (X, X o )＞D sen The area of the unmanned aerial vehicle is set as a safety area, and each unmanned aerial vehicle obstacle avoidance decision partition is provided with a corresponding partition repulsive potential field function, wherein the repulsive potential field function U o (X) being capable of characterizing the distance relationship d (X, X) between the unmanned aerial vehicle and the target point g ) The corresponding formula is:
in U o (X) is a repulsive potential field function; k (k) o Is a gain coefficient; d (X, X) o ) Is the distance between the unmanned ship and the obstacle; d, d o Is the influence distance of the repulsive force of the obstacle; d (X, X) g ) Is the distance between the unmanned boat and the target point;
in the present embodiment, the distance relationship d (X, X) g ) Introducing repulsive potential field function U o In (X), namely:
the repulsive force of the obstacle to the unmanned ship is gradually reduced in the process that the unmanned ship approaches to the target position, the resultant force is directed towards the direction of the attractive force, and the unmanned ship can always move towards the target position.
As shown in fig. 4, the safe zone: distance d (X, X) between unmanned ship and obstacle o ) Greater than the maximum detection distance D of the unmanned ship sensor sen The distance between the unmanned ship and the obstacle in the area is far, and obstacle avoidance is not needed;
normally avoid the obstacle region: the distance between the unmanned ship and the obstacle in the area is proper, and the repulsive force coefficient function is not required to be adjusted;
emergency constriction zone: in order to prevent the obstacle from leaving the detection range of the unmanned ship sensor, the repulsive force potential field function needs to be adjusted to reduce the repulsive force born by the unmanned ship;
emergency out-expansion zone: the obstacle invades the safety field of unmanned ship and practically has collision danger, and in this region, in order to prevent unmanned ship and obstacle from colliding, the repulsive force potential field function needs to be adjusted to increase the repulsive force that unmanned ship receives for unmanned ship keeps away from the obstacle.
In a specific embodiment, in S5, as shown in fig. 2, the unmanned ship first obtains the coordinates of the target or the unknown obstacle in the sensor coordinate system through the sensor, and converts the coordinates of the target or the unknown obstacle in the sensor coordinate system into the coordinates of the target or the unknown obstacle in the global coordinate system, which specifically includes the following steps:
s51: it is assumed that the sensor is located at a point (x r 0), during the navigation of the unmanned aerial vehicle, the position of the sensor coordinate system of the sensor relative to the ship body coordinate system is fixed, the azimuth angle and the distance of the target or the unknown obstacle relative to the unmanned aerial vehicle can be measured through the unmanned aerial vehicle sensor, and if the coordinates of the unknown obstacle under the sensor coordinate system are (d, theta) 1 ) Firstly converting the coordinates of an unknown obstacle in a sensor coordinate system into coordinates (x ', y') in a ship body coordinate system according to a coordinate transformation formula, wherein the corresponding coordinate transformation formula is as follows:
s52: the obtained coordinates of the unknown obstacle in the hull coordinate system are calculated according to the global coordinates (x u ，y u ) And a coordinate transformation formula for transforming the coordinates (x b ，y b ) The corresponding coordinate transformation formula is:
wherein alpha is u Is unmanned ship navigation direction and global coordinate system O 0 x 0 Included angle of axial direction.
In this embodiment, a method of multi-sensor fusion is adopted to detect the target and the unknown obstacle, so as to improve the detection precision and further improve the efficiency of the searching process.
Because the unknown obstacle can change the situation in the unmanned ship sailing process, the original planning path possibly becomes an unreachable area because of the intrusion of the unknown obstacle, and therefore, a local obstacle avoidance method used in the process of approaching the dynamic obstacle is adopted to finish the local dynamic obstacle avoidance of the unknown obstacle.
In a specific embodiment, in the step S5, when the unknown obstacle is detected, the unmanned aerial vehicle determines whether there are clear obstacle avoidance target points according to the coordinates of the unknown obstacle in the global coordinate system, and selects different methods for obstacle avoidance:
as shown in fig. 5a, when the unmanned aerial vehicle detects an unknown obstacle through the sensor and can detect the boundary of the unknown obstacle, if the unmanned aerial vehicle can find an obstacle avoidance target point according to the coordinates of the unknown obstacle under the global coordinate system, an artificial potential field method is adopted to plan a path for obstacle avoidance, but the artificial potential field method is based on path planning with an accurate target point, and when the size of the unknown obstacle exceeds the detection range of the unmanned aerial vehicle, the boundary of the unknown obstacle in the navigation direction cannot be confirmed, and further the clear obstacle avoidance target point cannot be obtained.
As shown in fig. 5b, if the unmanned aerial vehicle cannot find the obstacle avoidance target point according to the coordinates of the unknown obstacle in the global coordinate system, the unmanned aerial vehicle detects the unknown obstacle through the sensor and the boundary L of the unknown obstacle in the Ox axis direction of the hull coordinate system x Out of the detection range of unmanned boats, namely L x ＞(D sen -D safe ) When the unmanned ship is in the forward direction, the boundary length L of the unknown obstacle on the right side of the origin O in the Oy axis direction of the ship body coordinate system is determined y_right Boundary length L located to the left of origin O y_left The comparison relation between the swept area widths D of the unmanned ship in the straight line along the parallel line is selected to avoid the obstacle by adopting a Bug1 algorithm or a Bug2 algorithm, the Bug algorithm is simple and convenient to calculate, the shape of a global map and the shape of an obstacle do not need to be known, in order to meet the coverage requirement of a search area when the unmanned ship avoids an unknown obstacle, the existing Bug algorithm is improved, and different obstacle avoidance methods are provided based on the improved Bug algorithm, as shown in fig. 6, and the method specifically comprises the following steps of;
mode 1: as shown in fig. 7, when the sensor detects L of an unknown obstacle y_right Not less than D/2 and L y_left When the speed is less than D/2, the unmanned ship breaks away from the unknown obstacle by adopting a Bug2 algorithm, sets m-line as a global path, and selects to turn to the left to navigate so as to bypass the unknown obstacle, and leaves the unknown obstacle when meeting the m-line again, and re-navigates to the original planning path to continue navigating;
mode 2: as shown in fig. 8, when the sensor detects L of an unknown obstacle y_right < D/2 and L y_left When the distance between the unmanned aerial vehicle and the unknown obstacle is not less than D/2, the unmanned aerial vehicle breaks away from the unknown obstacle by adopting a Bug2 algorithm, an m-line is set as a global path, the unmanned aerial vehicle selectively turns to the right to navigate so as to bypass the unknown obstacle, and when the unmanned aerial vehicle meets the m-line again, the unmanned aerial vehicle leaves the unknown obstacle and sails on the original planning path again;
mode 3: as shown in fig. 9, when the sensor detects an unknown obstacle L y_right < D/2 and L y_left When the speed is less than D/2, the unmanned ship bypasses the unknown obstacle by adopting a Bug1 algorithm, an m-line is set as a global path, the unmanned ship returns to the initial position after bypassing the unknown obstacle for a circle, and the length L of the route before and after meeting the m-line is recorded 1 And L 2 If L 1 ＞L 2 The unmanned ship is L along the length of the route 1 And vice versa along the path length L 2 And (3) when the unmanned ship meets the m-line again, the unmanned ship leaves the unknown obstacle, and sails back to the original planning path to continue sailing.
In this embodiment, the original planned global path, i.e., the parallel line path, is set as the m-line, and the m-line is not changed with the movement of the unmanned ship.
Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present invention, and not for limiting the same; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some or all of the technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit of the invention.
Claims (7)
1. An unmanned ship-based marine search area coverage path planning method is characterized by comprising the following specific steps:
s1: acquiring an offshore search area and reading electronic chart information of the search area;
s2: constructing an unmanned ship system model based on a ship body coordinate system and a global coordinate system;
s3: determining a searching direction, carrying out global path planning on a searching area based on parallel line scanning searching, and setting the interval width between two parallel line paths in the searching area, namely setting the width of a scanning area when the unmanned ship is in direct line along a parallel line as D;
s4: according to known obstacle information in a search area provided by an electronic chart, constructing a repulsive potential field function U when a manual potential field method is adopted for local path planning o (X) setting a plurality of unmanned ship obstacle avoidance decision partitions, and configuring a corresponding partition repulsive potential field function for each unmanned ship obstacle avoidance decision partition; wherein the repulsive potential field function U o (X) being capable of characterizing the distance relationship d (X, X) between the unmanned aerial vehicle and the target point g )；
S5: the unmanned ship navigates in a search area according to an original planning path determined by global path planning and local path planning, and detects targets and unknown obstacles through sensors:
when a target is detected, transmitting the target coordinate under the global coordinate system to a shore base or a search and rescue ship, and continuing to navigate along an original planning path for searching;
when an unknown obstacle is detected, the unmanned ship judges whether an obstacle avoidance target point exists according to the coordinates of the unknown obstacle under a global coordinate system, if the obstacle avoidance target point exists, a path is planned by adopting an artificial potential field method to avoid the obstacle, otherwise, the path is planned by adopting a Bug1 algorithm or a Bug2 algorithm to avoid the obstacle, and after the unmanned ship leaves the unknown obstacle, the unmanned ship navigates back to the original planned path to continue to navigate for searching;
s6: and after the unmanned ship searches the whole search area, ending the search task.
2. The unmanned ship-based marine search area coverage path planning method according to claim 1, wherein in S2, the equation of motion of the unmanned ship system model is set as:
wherein: x is the longitudinal displacement of the unmanned ship, y is the transverse displacement of the unmanned ship, mu is the longitudinal speed of the unmanned ship, v is the transverse speed of the unmanned ship, psi is the heading angle, and r is the turning heading angle speed.
3. The unmanned aerial vehicle-based marine search area coverage path planning method of claim 1, wherein, in S3,
s31: determining the width of the search area, and determining the search direction according to the width of the search area:
1) If the search area is rectangular, the width of the rectangle is the width of the search area;
2) If the search area is a convex polygon, the width calculation method of the convex polygon search area is as follows: setting the vertex of the convex polygon to be P= { P 1 ，p 2 ，…，p n Each side is S= { S 1 ，S 2 ，…，S n Sequentially calculating the distance D from all the other vertexes except the two vertexes on each edge to the edge p3-s1 ，D p4-s1 ，…，D pn-s1 Taking the maximum value as the span D of each edge s1 ＝max{D p3-s1 ，D p4-s1 ，…，D pn-s1 Taking the minimum value in the span as the width W=min { D { of the convex polygon } si I = 1,2, …, n }; when global path planning is carried out, the searching direction is the direction consistent with or opposite to the width direction of the searching area;
s32: and setting the interval width D between two parallel line paths in the search area according to the width of the inscribed square in the detection range of the unmanned ship, namely the width of the area swept by the unmanned ship when the unmanned ship is in direct line along the parallel line.
4. The unmanned ship-based marine search area coverage path planning method according to claim 1, wherein in S4, a repulsive potential field function U is constructed when local path planning is performed by using a manual potential field method according to known obstacle information in a search area provided by an electronic sea chart o (X) regarding a round area with the ship position of the unmanned ship as the center and tau as the radius as an area where the unmanned ship body is located, namely a forbidden area, and setting the safe distance of the unmanned ship as D safe The maximum detection distance of the unmanned ship sensor is D sen Setting up a plurality of unmanned boats and keeping away barrier decision partition, include:
will tau<d(X，X o )≤D safe The annular area of the (C) is set as an emergency external expansion area; will D safe <d(X，X o )≤D s Is set as a conventional obstacle avoidance zone, wherein D s ＝D sen -D safe The method comprises the steps of carrying out a first treatment on the surface of the Will D s <d(X，X o )≤D sen Is set as an emergency contraction zone; will d (X, X) o )＞D sen The area of (2) is set as a safe area;
each unmanned ship obstacle avoidance decision partition is configured with a corresponding partition repulsive potential field function, wherein the repulsive potential field function Uo (X) can represent the distance relationship d (X, X g ) The corresponding formula is:
in U o (X) is a repulsive potential field function; k (k) o Is a gain coefficient; d (X, X) o ) Is the distance between the unmanned ship and the obstacle; d, d o Is the influence distance of the repulsive force of the obstacle; d (X, X) g ) Is the distance between the unmanned boat and the target point.
5. The method for planning a coverage path of an offshore search area based on an unmanned aerial vehicle according to claim 4, wherein in S5, the unmanned aerial vehicle first obtains coordinates of a target or an unknown obstacle in a sensor coordinate system by a sensor, and converts the coordinates of the target or the unknown obstacle in the sensor coordinate system into coordinates of the target or the unknown obstacle in a global coordinate system, comprising the following steps:
s51: it is assumed that the sensor is located at a point (x r 0) the coordinates of the unknown obstacle in the sensor coordinate system are (d, θ) 1 ) Firstly converting the coordinates of an unknown obstacle in a sensor coordinate system into coordinates (x ', y') in a ship body coordinate system according to a coordinate transformation formula, wherein the corresponding coordinate transformation formula is as follows:
s52: the obtained coordinates of the unknown obstacle in the hull coordinate system are calculated according to the global coordinates (x u ，y u ) And a coordinate transformation formula for transforming the coordinates (x b ，y b ) The corresponding coordinate transformation formula is:
wherein alpha is u Is unmanned ship navigation direction and global coordinate system O 0 x 0 Included angle of axial direction.
6. The unmanned aerial vehicle-based marine search area coverage path planning method of claim 1, wherein, in S5,
when the unmanned aerial vehicle detects an unknown obstacle through the sensor and can detect the boundary of the unknown obstacle, if the unmanned aerial vehicle can find an obstacle avoidance target point according to the coordinates of the unknown obstacle under the global coordinate system, an artificial potential field rule path is adopted for obstacle avoidance;
if the unmanned aerial vehicle cannot find the obstacle avoidance target point according to the coordinates of the unknown obstacle in the global coordinate system, namely the unmanned aerial vehicle detects the unknown obstacle through the sensor and the boundary L of the unknown obstacle in the Ox axis direction of the ship body coordinate system x Out of the detection range of unmanned boats, namely L x ＞(D sen -D safe ) When the unmanned ship is in the forward direction, the boundary length L of the unknown obstacle on the right side of the origin O in the Oy axis direction of the ship body coordinate system is determined y_right Boundary length L located to the left of origin O y_left And the comparison relation between the swept area widths D when the unmanned ship is in a straight line along a parallel line path is selected to avoid the obstacle by adopting a Bug1 algorithm or a Bug2 algorithm, and the method specifically comprises the following steps:
mode 1: when the sensor detects L of unknown obstacle y_right Not less than D/2 and L y_left <D/2, separating the unknown obstacle by adopting a Bug2 algorithm, setting m-line as a global path, selecting the unmanned ship to navigate to the left to bypass the unknown obstacle, separating the unknown obstacle when meeting the m-line again, and re-navigating to the original planning path to continue navigating;
mode 2: when the sensor detects L of unknown obstacle y_right <D/2 and L y_left When the distance between the unmanned aerial vehicle and the unknown obstacle is not less than D/2, the unmanned aerial vehicle breaks away from the unknown obstacle by adopting a Bug2 algorithm, an m-line is set as a global path, the unmanned aerial vehicle selectively turns to the right to navigate so as to bypass the unknown obstacle, and when the unmanned aerial vehicle meets the m-line again, the unmanned aerial vehicle leaves the unknown obstacle and sails on the original planning path again;
mode 3: when the sensor detects an unknown obstacle L y_right <D/2 andL y_left <d/2, the unmanned ship bypasses the unknown obstacle by adopting a Bug1 algorithm, sets m-line as a global path, returns to the initial position after the unmanned ship bypasses the unknown obstacle for a circle, and records the length L of the route before and after meeting the m-line 1 And L 2 If L 1 ＞L 2 The unmanned ship is L along the length of the route 1 And vice versa along the path length L 2 And (3) when the unmanned ship meets the m-line again, the unmanned ship leaves the unknown obstacle, and sails back to the original planning path to continue sailing.
7. The unmanned ship-based marine search area coverage path planning method according to claim 1, wherein in S5, when the unmanned ship navigates in the search area according to the determined original planned path, a determination rule is set as to whether the unmanned ship enters a deadlock state during navigation, and if it is determined that the unmanned ship enters the deadlock state, the following steps are:
<F a (X)，F o (X)>＝acos(dot(F a (X)，F o (X))/(norm(F a (X))*norm(F o (X))))*180/pi
at this time, a random deflection angle θ of the unmanned boat is set 2 ，θ 2 The value range of (2) is 0, 90 DEG]The unmanned ship is separated from the deadlock state, namely:
F a ′(X)＝cos -1 θ 2 *F a (X)
wherein F is a (X) is the attraction of unmanned boat, F a 'X' is a random deflection angle theta of the unmanned ship 2 Attraction force of unmanned ship after F o (X) is the repulsive force of the unmanned ship,is the navigation direction of the unmanned ship.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310936442.6A CN116974282A (en) | 2023-07-27 | 2023-07-27 | Unmanned ship-based marine search area coverage path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310936442.6A CN116974282A (en) | 2023-07-27 | 2023-07-27 | Unmanned ship-based marine search area coverage path planning method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116974282A true CN116974282A (en) | 2023-10-31 |
Family
ID=88470774
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310936442.6A Pending CN116974282A (en) | 2023-07-27 | 2023-07-27 | Unmanned ship-based marine search area coverage path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116974282A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117472066A (en) * | 2023-12-27 | 2024-01-30 | 成都流体动力创新中心 | Obstacle avoidance control method with locally optimal course angular velocity |
-
2023
- 2023-07-27 CN CN202310936442.6A patent/CN116974282A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117472066A (en) * | 2023-12-27 | 2024-01-30 | 成都流体动力创新中心 | Obstacle avoidance control method with locally optimal course angular velocity |
| CN117472066B (en) * | 2023-12-27 | 2024-03-26 | 成都流体动力创新中心 | Obstacle avoidance control method with locally optimal course angular velocity |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11899465B2 (en) | Autonomous and assisted docking systems and methods | |
| Wang et al. | Roboat: An autonomous surface vehicle for urban waterways | |
| US11505292B2 (en) | Perimeter ranging sensor systems and methods | |
| US8215252B1 (en) | System and method for dynamic stabilization and navigation in high sea states | |
| US11307589B2 (en) | Vessel navigation system and navigation method thereof | |
| CN113759939B (en) | Intelligent navigation method and device for limited water area | |
| Bandyophadyay et al. | A simple reactive obstacle avoidance algorithm and its application in singapore harbor | |
| WO2021230356A1 (en) | Method for autonomously guiding vessel, program for autonomously guiding vessel, system for autonomously guiding vessel, and vessel | |
| CN102999050A (en) | Automatic obstacle avoidance method for intelligent underwater robots | |
| CN110837255B (en) | Autonomous danger avoiding method suitable for high-speed water surface unmanned ship | |
| KR101894674B1 (en) | Ship navigation apparatus and method for providing route information for ship | |
| KR101799216B1 (en) | Ship navigation apparatus and method for providing route information for ship | |
| KR101719142B1 (en) | Ship navigation apparatus and method for providing route information for ship | |
| CN116974282A (en) | Unmanned ship-based marine search area coverage path planning method | |
| JP2020097406A (en) | System and method for proximity sensing for ocean vessel | |
| CN109916400B (en) | Unmanned ship obstacle avoidance method based on combination of gradient descent algorithm and VO method | |
| CN112068565A (en) | Unmanned ship autonomous navigation method and system in structured environment | |
| JP6686249B2 (en) | Ship automatic control system, ship, and automatic ship control method | |
| CN114061565B (en) | Unmanned ship SLAM and application method thereof | |
| Sans-Muntadas et al. | Spiral path planning for docking of underactuated vehicles with limited FOV | |
| Yan et al. | An Obstacle Avoidance Algorithm for Unmanned Surface Vehicle Based on A Star and Velocity-Obstacle Algorithms | |
| Sauze et al. | A raycast approach to collision avoidance in sailing robots | |
| Jo et al. | Development of autonomous algorithm for boat using robot operating system | |
| CN112880678A (en) | Unmanned ship navigation planning method in complex water area environment | |
| CN115202366A (en) | Unmanned ship autonomous berthing method and system based on environment perception |
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 |