CN117519138A - Path planning method based on dense obstacle and ocean current interference oriented improved DWA - Google Patents
Path planning method based on dense obstacle and ocean current interference oriented improved DWA Download PDFInfo
- Publication number
- CN117519138A CN117519138A CN202311375777.1A CN202311375777A CN117519138A CN 117519138 A CN117519138 A CN 117519138A CN 202311375777 A CN202311375777 A CN 202311375777A CN 117519138 A CN117519138 A CN 117519138A
- Authority
- CN
- China
- Prior art keywords
- unmanned ship
- unmanned
- speed
- representing
- obstacle
- 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 29
- 238000011156 evaluation Methods 0.000 claims description 64
- 101100057959 Mus musculus Atxn1l gene Proteins 0.000 claims description 43
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 24
- 238000004364 calculation method Methods 0.000 claims description 21
- 230000006870 function Effects 0.000 claims description 14
- 230000007613 environmental effect Effects 0.000 claims description 11
- 238000001514 detection method Methods 0.000 claims description 7
- 230000008447 perception Effects 0.000 claims description 6
- 238000010276 construction Methods 0.000 claims description 4
- 238000005457 optimization Methods 0.000 claims description 3
- 230000004888 barrier function Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000005265 energy consumption Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000004134 energy conservation Methods 0.000 description 1
- 238000004387 environmental modeling Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a path planning method based on an improved DWA (dense obstacle and ocean current interference), which comprises the following steps: step S1: constructing an environment model of unmanned ship path planning; step S2: and carrying out local path planning on the unmanned ship by adopting an improved Dynamic Window Algorithm (DWA) in an environment model of unmanned ship path planning to obtain an optimal unmanned ship navigation planning path. The invention solves the problems that the traditional DWA is adopted to carry out local path planning on the unmanned ship, the complicated marine environment factors can interfere the DWA to select the optimal path, and the safe autonomous navigation and energy saving of the unmanned ship are very unfavorable.
Description
Technical Field
The invention relates to the technical field of unmanned ship path planning, in particular to a path planning method based on an improved DWA (discrete wavelet transform) for dense obstacle and ocean current interference.
Background
The unmanned ship is a small offshore platform with environment sensing and autonomous navigation capabilities and capable of autonomously completing corresponding tasks, and is widely applied to the fields of maritime scientific investigation, maritime search and rescue, water quality monitoring and the like in recent years, however, under the actual application scene, the unmanned ship is required to have the capability of accurately and safely planning when the unmanned ship performs tasks in an unknown sea area, and the unmanned ship is helped to complete maritime operations smoothly.
In practical application, the unmanned ship is planned by taking a global path point as a local target point, static obstacles are avoided, algorithms which are generally adopted are DWA, TEB, an artificial potential field method and the like, but the factors considered by the algorithms are single, the real marine environment is quite complex, the unmanned ship not only comprises dense static obstacles, but also has severe weather factors such as large ocean currents, large waves and the like, the factors can interfere the selection of the optimal path by the DWA, the energy loss of the Unmanned Ship (USV) can be increased, the planned path cannot be guaranteed to be feasible, safe and less in energy loss, and the unmanned ship is very unfavorable for safe autonomous navigation and energy saving of the USV.
Disclosure of Invention
Aiming at the defects, the invention provides a path planning method based on an improved DWA for dense obstacle and ocean current interference, and aims to solve the problems that the traditional DWA is adopted to carry out local path planning on an unmanned ship, complicated ocean environment factors can interfere the selection of the DWA on an optimal path, and the safe autonomous navigation and energy conservation of the unmanned ship are very unfavorable.
To achieve the purpose, the invention adopts the following technical scheme:
the path planning method based on the improved DWA facing dense obstacle and ocean current interference comprises the following steps:
step S1: constructing an environment model of unmanned ship path planning;
step S2: and carrying out local path planning on the unmanned ship by adopting an improved Dynamic Window Algorithm (DWA) in an environment model of unmanned ship path planning to obtain an optimal unmanned ship navigation planning path.
Preferably, in step S1, an environmental model of unmanned ship path planning is constructed by using a grid method, and the specific construction steps are as follows:
dividing the whole unmanned ship path planning map space area into m multiplied by m grids, wherein each grid represents a corresponding environment state, and the unmanned ship path planning map space area is expressed as:
area=∑M ij ,i∈[1,m],j∈[1,m];
wherein M is ij Representing a respective environmental state for each grid, the respective environmental state for each grid being represented as:
preferably, in step S2, the following substeps are specifically included:
step S21: constructing a kinematic model of the unmanned ship;
step S22: calculating the restraint speed of the unmanned ship, the operation restraint speed of the unmanned ship motor and the brake restraint speed of the unmanned ship, wherein,
the calculation formula of the restraint speed of the unmanned ship is as follows:
v m ={(v,w)|v∈[v min ,v max ]∩w∈[w min ,w max ]};
wherein v is m Representing the restraint speed of the unmanned ship; v represents the linear velocity of the unmanned boat; w represents the angular velocity of the unmanned boat; v min Representing a minimum linear velocity of the unmanned boat; v max Representing the maximum linear velocity of the unmanned boat; w (w) min Representing a minimum angular velocity of the unmanned boat; w (w) max Representing a maximum angular velocity of the unmanned boat;
the calculation formula of the operation constraint speed of the unmanned ship motor is as follows:
wherein v is d Representing the operation constraint speed of the unmanned ship motor; v' represents the running linear speed of the unmanned ship motor; w' represents the rotational speed of the unmanned ship motor;representing the minimum running linear speed of the unmanned ship motor; />Representing the maximum running linear speed of the unmanned ship motor; />Representing the minimum operational angular speed of the unmanned ship motor; />Representing the maximum operational angular speed of the unmanned ship motor; t represents time;
the calculation formula of the braking constraint speed of the unmanned ship is as follows:
wherein v is s Representing the braking restraint speed of the unmanned ship; v "represents the braking linear velocity of the unmanned boat; w "represents the braking angular velocity of the unmanned boat; dist (v ", w") represents the braking distance of the unmanned boat;
step S23: according to the restraint speed of the unmanned ship, the operation restraint speed of the unmanned ship motor and the brake restraint speed of the unmanned ship, calculating a speed set of the unmanned ship to ensure a kinematic model of the unmanned ship, wherein the speed set of the unmanned ship is as follows:
v γ =v m ∩v d ∩v s ;
wherein v is γ Representing a set of speeds for the unmanned boat.
Preferably, in step S21, the kinematic model of the unmanned boat is expressed as:
wherein x (t) represents the abscissa of the unmanned ship in the world coordinate system at time t; y (t) represents the ordinate of the unmanned ship in the world coordinate system at the moment t; v represents the linear velocity of the unmanned boat; w represents the angular velocity of the unmanned boat; Δt represents a time interval; θ (t) represents the angle of the unmanned boat in the world coordinate system at time t.
Preferably, in step S2, the following sub-steps are further included:
carrying out local path planning on the unmanned aerial vehicle by adopting an improved DWA (discrete wavelet transform), obtaining a plurality of unmanned aerial vehicle navigation prediction planning paths, grading the plurality of unmanned aerial vehicle navigation prediction planning paths by adopting an improved DWA evaluation function, and selecting the unmanned aerial vehicle navigation prediction planning path with the highest grading as an unmanned aerial vehicle navigation optimal planning path;
the evaluation function expression of the improved DWA is as follows:
G(v,w)=α*heading(v,w)+β*dist(v,w)+γ*vel(v,w);
wherein G (v, w) represents an evaluation function of the improved DWA; head (v, w) represents the azimuth evaluation sub-function; dist (v, w) represents a distance evaluation sub-function of the unmanned ship and the obstacle, namely, the optimal distance between the predicted track and the obstacle at the next moment of the unmanned ship, and a specific calculation formula is as follows:
wherein, (x) usv ,y usv ) The position coordinates of the tail end of the predicted track at the next moment of the unmanned ship are represented; (x) ob ,y ob ) Representing position coordinates of the obstacle;
and vel (v, w) represents an unmanned ship speed evaluation sub-function, namely the speed of unmanned ship safety obstacle avoidance optimization, and a specific calculation formula is as follows:
vel(v,w)=|v usv |;
wherein v is usv Representing the speed of the unmanned ship predicted track;
alpha represents a weight parameter of the azimuth evaluation sub-function; beta represents a weight parameter of a distance evaluation sub-function of the unmanned ship and the obstacle; gamma denotes the weight parameter of the unmanned ship speed evaluation sub-function.
Preferably, in step S2, the following sub-steps are further included:
according to the relative motion of the water flow and the unmanned ship and the ratio coefficient of the obstacle in the perception range of the unmanned ship, respectively adjusting the weight parameter alpha of the azimuth evaluation sub-function, the weight parameter beta of the distance evaluation sub-function between the unmanned ship and the obstacle and the weight parameter gamma of the speed evaluation sub-function of the unmanned ship in the improved DWA;
the calculation formula of the weight parameter alpha of the azimuth angle evaluation sub-function is as follows:
wherein v is usv Representing the speed of the unmanned ship predicted track; v w Indicating the water flow rate; alpha 0 A weight parameter representing an azimuth evaluation sub-function of the original input;
the expression of the weight parameter beta of the distance evaluation sub-function of the unmanned ship and the obstacle is as follows:
β=β w +β ob ;
in the middle of
Wherein v is max Is the maximum propulsion speed of the unmanned ship, beta w Representing unmanned boats and obstructionsA portion of the distance-evaluating sub-function affected by the water flow; beta ob Representing the part of the unmanned ship and obstacle distance evaluation subfunction affected by the number of surrounding obstacles; beta 0 A weight parameter representing a distance evaluation sub-function of the original input unmanned ship and the obstacle; n is n all Representing the uniform generation of the current n in the detection range D of the sensor on the unmanned ship all A plurality of position points, n ob Representing a total number of points falling on the obstacle grid among the generated position points within the range D;
the expression of the weight parameter gamma of the unmanned ship speed evaluation sub-function is as follows:
wherein, gamma 0 Weight parameters representing the original input unmanned ship speed evaluation sub-function.
The technical scheme provided by the embodiment of the application can comprise the following beneficial effects:
in the scheme, the improved Dynamic Window Algorithm (DWA) is adopted to carry out local path planning on the unmanned ship, so that an optimal planning path for unmanned ship navigation is obtained, and the energy consumption of the unmanned ship can be saved. Meanwhile, the improved Dynamic Window Algorithm (DWA) can enable the unmanned ship to efficiently avoid dense obstacles, improve the robustness of the unmanned ship under the action of water flow, adapt to complex environments and improve the sailing safety of the unmanned ship.
Drawings
Fig. 1 is a flow chart of the steps of a path planning method based on an improved DWA for dense obstacles and ocean current disturbances.
Detailed Description
Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to like or similar elements or elements having like or similar functions throughout. The embodiments described below by referring to the drawings are exemplary only for explaining the present invention and are not to be construed as limiting the present invention.
The path planning method based on the improved DWA facing dense obstacle and ocean current interference comprises the following steps:
step S1: constructing an environment model of unmanned ship path planning;
step S2: and carrying out local path planning on the unmanned ship by adopting an improved Dynamic Window Algorithm (DWA) in an environment model of unmanned ship path planning to obtain an optimal unmanned ship navigation planning path.
According to the path planning method based on the dense obstacle and ocean current interference-oriented improved DWA, as shown in fig. 1, the first step is to construct an environment model of unmanned ship path planning, and specifically, the precondition of unmanned ship path planning is to construct a proper environment model according to the current environment conditions. Currently, modeling methods are grid methods, geometric methods and topology methods. Since raster modeling describes a geospatial using a grid-like data structure, high accuracy of geographic data can be provided, the present approach uses raster for environmental modeling. The second step is to adopt an improved Dynamic Window Algorithm (DWA) to conduct local path planning on the unmanned ship in an environment model of unmanned ship path planning to obtain an optimal unmanned ship navigation planning path, and specifically, the Dynamic Window Algorithm (DWA) is an obstacle avoidance local path planning method, and the local path planning of the unmanned ship is to determine an optimal movement route from the current position to the target position through analysis and perception of the current position of the unmanned ship and perception of surrounding obstacles and the target position in an unknown complex environment. Aiming at the problems that the traditional dynamic window algorithm is low in obstacle avoidance efficiency and easy to sink into local optimum in a water area with dense obstacles, the scheme adopts the improved DWA combined with the area coefficient of the obstacle and the relevant parameters of the water flow, so that the unmanned ship can more efficiently wind out the dense obstacles, and meanwhile, the robustness of the unmanned ship under the action of the water flow is enhanced.
In the scheme, the improved Dynamic Window Algorithm (DWA) is adopted to carry out local path planning on the unmanned ship, so that an optimal planning path for unmanned ship navigation is obtained, and the energy consumption of the unmanned ship can be saved. Meanwhile, the improved Dynamic Window Algorithm (DWA) can enable the unmanned ship to efficiently avoid dense obstacles, improve the robustness of the unmanned ship under the action of water flow, adapt to complex environments and improve the sailing safety of the unmanned ship.
Preferably, in step S1, an environmental model of unmanned ship path planning is constructed by using a grid method, and the specific construction steps are as follows:
dividing the whole unmanned ship path planning map space area into m multiplied by m grids, wherein each grid represents a corresponding environment state, and the unmanned ship path planning map space area is expressed as:
area=∑M ij ,i∈[1,m],j∈[1,m];
wherein M is ij Representing a respective environmental state for each grid, the respective environmental state for each grid being represented as:
in this embodiment, the environmental model of unmanned ship path planning is a rasterized unmanned ship path planning map space, and the specific operation method for constructing the rasterized unmanned ship path planning map space is as follows: firstly, redundant information of an unmanned ship path planning map, such as background color, map icons and the like, is removed by utilizing a pixel binary method; secondly, rasterizing a black-and-white image with higher contrast, and expanding the obstacle with incomplete occupation; and finally, a rasterized unmanned ship path planning map is obtained.
Preferably, in step S2, the method specifically comprises the following substeps:
step S21: constructing a kinematic model of the unmanned ship;
step S22: calculating the restraint speed of the unmanned ship, the operation restraint speed of the unmanned ship motor and the brake restraint speed of the unmanned ship, wherein,
the calculation formula of the restraint speed of the unmanned ship is as follows:
v m ={(v,w)|v∈[v min ,v max ]∩w∈[w min ,w max ]};
wherein v is m Representing the restraint speed of the unmanned ship; v represents the linear velocity of the unmanned boat; w representsAngular velocity of unmanned boats; v min Representing a minimum linear velocity of the unmanned boat; v max Representing the maximum linear velocity of the unmanned boat; w (w) min Representing a minimum angular velocity of the unmanned boat; w (w) max Representing a maximum angular velocity of the unmanned boat;
the calculation formula of the operation constraint speed of the unmanned ship motor is as follows:
wherein v is d Representing the operation constraint speed of the unmanned ship motor; v' represents the running linear speed of the unmanned ship motor; w' represents the rotational speed of the unmanned ship motor;representing the minimum running linear speed of the unmanned ship motor; />Representing the maximum running linear speed of the unmanned ship motor; />Representing the minimum operational angular speed of the unmanned ship motor; />Representing the maximum operational angular speed of the unmanned ship motor; t represents time;
the calculation formula of the braking constraint speed of the unmanned ship is as follows:
wherein v is s Representing the braking restraint speed of the unmanned ship; v "represents the braking linear velocity of the unmanned boat; w "represents the braking angular velocity of the unmanned boat; dist (v ", w") represents the braking distance of the unmanned boat;
step S23: according to the restraint speed of the unmanned ship, the operation restraint speed of the unmanned ship motor and the brake restraint speed of the unmanned ship, calculating a speed set of the unmanned ship to ensure a kinematic model of the unmanned ship, wherein the speed set of the unmanned ship is as follows:
v γ =v m ∩v d ∩v s ;
wherein v is γ Representing a set of speeds for the unmanned boat.
Specifically, unmanned boats have limitations on speed, acceleration, angular velocity, angular acceleration, and braking distance under hardware limitations. In the embodiment, the improved DWA restrains the speed of the unmanned ship, the running speed of the motor of the unmanned ship and the braking speed of the unmanned ship, so that the unmanned ship is ensured to stop before random obstacles, and the obstacle avoidance effect is realized. Further, the improved DWA is capable of finding the optimal control speed of the unmanned boat, enabling rapid and safe arrival at the destination.
Preferably, in step S21, the kinematic model of the unmanned boat is expressed as:
wherein x (t) represents the abscissa of the unmanned ship in the world coordinate system at time t; y (t) represents the ordinate of the unmanned ship in the world coordinate system at the moment t; v represents the linear velocity of the unmanned boat; w represents the angular velocity of the unmanned boat; Δt represents a time interval; θ (t) represents the angle of the unmanned boat in the world coordinate system at time t.
In the embodiment, the position information of the unmanned ship under the world coordinate system can be clearly known through the construction of the kinematic model of the unmanned ship, so that the motion state of the unmanned ship can be known.
Preferably, in step S2, the following sub-steps are further included:
carrying out local path planning on the unmanned aerial vehicle by adopting an improved DWA (discrete wavelet transform), obtaining a plurality of unmanned aerial vehicle navigation prediction planning paths, grading the plurality of unmanned aerial vehicle navigation prediction planning paths by adopting an improved DWA evaluation function, and selecting the unmanned aerial vehicle navigation prediction planning path with the highest grading as an unmanned aerial vehicle navigation optimal planning path;
the evaluation function expression of the improved DWA is as follows:
G(v,w)=α*heading(v,w)+β*dist(v,w)+γ*vel(v,w);
wherein G (v, w) represents an evaluation function of the improved DWA; head (v, w) represents the azimuth evaluation sub-function; dist (v, w) represents a distance evaluation sub-function of the unmanned ship and the obstacle, namely, the optimal distance between the predicted track and the obstacle at the next moment of the unmanned ship, and a specific calculation formula is as follows:
wherein, (x) usv ,y usv ) The position coordinates of the tail end of the predicted track at the next moment of the unmanned ship are represented;
(x ob ,y ob ) Representing position coordinates of the obstacle;
and vel (v, w) represents an unmanned ship speed evaluation sub-function, namely the speed of unmanned ship safety obstacle avoidance optimization, and a specific calculation formula is as follows:
vel(v,w)=|v usv |;
wherein v is usv Representing the speed of the unmanned ship predicted track;
alpha represents a weight parameter of the azimuth evaluation sub-function; beta represents a weight parameter of a distance evaluation sub-function of the unmanned ship and the obstacle; gamma denotes the weight parameter of the unmanned ship speed evaluation sub-function.
In this embodiment, a plurality of unmanned ship navigation prediction planning paths are obtained, each unmanned ship navigation prediction planning path is scored by combining an improved DWA evaluation function, the speed corresponding to the unmanned ship navigation prediction planning path with the highest current score is selected for execution, and the unmanned ship can quickly and safely reach a destination at the speed.
Further describing, the azimuth angle evaluation sub-function head (v, w) is used for calculating the target guidance between the heading and the target point of the unmanned ship, and the specific formula of the azimuth angle evaluation sub-function head (v, w) is as follows:
wherein θ goal The included angle between the current heading of the unmanned ship and the connecting line of the target point is shown, the larger the value of the included angle is, the more the heading deviates from the shortest linear distance, and the specific calculation formula is as follows:
wherein, (x) goal ,y goal ) Coordinates of the target point; (x) usv ,y usv ) The position coordinates of the tail end of the predicted track at the next moment of the unmanned ship are represented;
θ j representing the flow direction vector θ of the water flow c And the current heading theta of the unmanned ship usv The specific calculation formula of the included angle is as follows:
θ p heading theta representing predicted route of unmanned ship pre Vector θ to flow direction of water flow c The specific calculation formula of the included angle is as follows:
θ p for use with theta j Make a judgment and comparison, θ j The larger the value, the more the heading is biased to the opposite side of the water flow.
Preferably, in step S2, the following sub-steps are further included:
according to the relative motion of the water flow and the unmanned ship and the ratio coefficient of the obstacle in the perception range of the unmanned ship, respectively adjusting the weight parameter alpha of the azimuth evaluation sub-function, the weight parameter beta of the distance evaluation sub-function between the unmanned ship and the obstacle and the weight parameter gamma of the speed evaluation sub-function of the unmanned ship in the improved DWA;
the calculation formula of the weight parameter alpha of the azimuth angle evaluation sub-function is as follows:
wherein v is usv Representing the speed of the unmanned ship predicted track; v w Indicating the water flow rate; alpha 0 A weight parameter representing an azimuth evaluation sub-function of the original input;
the expression of the weight parameter beta of the distance evaluation sub-function of the unmanned ship and the obstacle is as follows:
β=β w +β ob ;
in the middle of
Wherein v is max Is the maximum propulsion speed of the unmanned ship, beta w Representing the part affected by water flow in the distance evaluation sub-function of the unmanned ship and the obstacle; beta ob Representing the part of the unmanned ship and obstacle distance evaluation subfunction affected by the number of surrounding obstacles; beta 0 A weight parameter representing a distance evaluation sub-function of the original input unmanned ship and the obstacle; n is n all Representing the uniform generation of the current n in the detection range D of the sensor on the unmanned ship all A plurality of position points, n ob Representing a total number of points falling on the obstacle grid among the generated position points within the range D;
the expression of the weight parameter gamma of the unmanned ship speed evaluation sub-function is as follows:
wherein, gamma 0 Weight parameters representing the original input unmanned ship speed evaluation sub-function.
In this embodiment, each sub-function in the evaluation function of the improved DWA needs a weight to measure the effect of the term on the optimal path selection, whereas in the conventional DWA, the weights cannot be adjusted according to the environmental parameters after the unmanned ship moves, which often results in a local optimal situation, so that the objective point cannot be reached in a more reasonable and efficient path. According to the scheme, weights of the sub-functions are respectively adjusted according to relative movement of water flow and the unmanned ship and the ratio coefficient of the obstacle in the perception range of the unmanned ship.
Specifically, the weight α is in the range of (0, 1), v w The weight of the function term can be adjusted when v w When=0, α≡α 0 I.e. the term remains low; when v w When the flow rate of the water is not equal to 0, the weight is adjusted by the ratio of the flow rate of the water to the current speed of the unmanned ship, and when the flow rate of the water is faster, that is, the influence on the motion state of the unmanned ship is larger, the unmanned ship needs to adjust the heading by using higher weight and more energy, so that the influence of the water flow on the unmanned ship is reduced; when the water flow is slower, the effect on the unmanned boat is smaller, the weight of the function is relatively reduced.
Aiming at the problems that the unmanned ship is low in obstacle avoidance efficiency, easy to sink into local optimum and the like in the dense obstacle environment, the weight beta and the weight gamma are correspondingly adjusted according to the size of the obstacle ratio in the detection range of the current navigation position of the unmanned ship, so that the predicted track of the unmanned ship is adjusted according to the obstacle ratio in the detection range of the current position of the unmanned ship, and the predicted path is selected to avoid the dense obstacle more efficiently.
Specifically, each point within the range D is marked with a sitting sign (x i ,y i ) Taking the upper right corner coordinates of the barrier grid as barrier coordinates, and marking a barrier coordinate set as ob;
point fall within detection rangeNumber n in obstacle grid ob The statistics are as follows:
wherein,is the coordinates (x) i ,y i ) And (5) rounding upwards.
According to the weight parameter expression of the distance evaluation sub-function of the unmanned ship and the obstacle, when the number of the obstacles in the detection range of the current position of the unmanned ship becomes dense, the weight beta is increased, and meanwhile, the weight gamma is reduced, so that the unmanned ship safely bypasses the obstacle.
Furthermore, functional units in various embodiments of the present invention may be integrated into one processing module, or each unit may exist alone physically, or two or more units may be integrated into one module. The integrated modules may be implemented in hardware or in software functional modules. The integrated modules may also be stored in a computer readable storage medium if implemented in the form of software functional modules and sold or used as a stand-alone product.
While embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and not to be construed as limiting the invention, and that variations, modifications, alternatives and variations of the above embodiments may be made by those skilled in the art within the scope of the invention.
Claims (6)
1. The path planning method based on the improved DWA oriented to dense obstacle and ocean current interference is characterized by comprising the following steps of: the method comprises the following steps:
step S1: constructing an environment model of unmanned ship path planning;
step S2: and carrying out local path planning on the unmanned ship by adopting an improved Dynamic Window Algorithm (DWA) in an environment model of unmanned ship path planning to obtain an optimal unmanned ship navigation planning path.
2. The path planning method based on dense obstacle and ocean current interference oriented improved DWA of claim 1, wherein: in step S1, an environmental model of unmanned ship path planning is constructed by adopting a grid method, and the specific construction steps are as follows:
dividing the whole unmanned ship path planning map space area into m multiplied by m grids, wherein each grid represents a corresponding environment state, and the unmanned ship path planning map space area is expressed as:
area=∑M ij ,i∈[1,m],j∈[1,m];
wherein M is ij Representing a respective environmental state for each grid, the respective environmental state for each grid being represented as:
3. the path planning method based on dense obstacle and ocean current interference oriented improved DWA of claim 1, wherein: in step S2, the method specifically includes the following substeps:
step S21: constructing a kinematic model of the unmanned ship;
step S22: calculating the restraint speed of the unmanned ship, the operation restraint speed of the unmanned ship motor and the brake restraint speed of the unmanned ship, wherein,
the calculation formula of the restraint speed of the unmanned ship is as follows:
v m ={(v,w)|v∈[v min ,v max ]∩w∈[w min ,w max ]};
wherein v is m Representing the restraint speed of the unmanned ship; v represents the linear velocity of the unmanned boat; w represents the angular velocity of the unmanned boat; v min Representing a minimum linear velocity of the unmanned boat; v max Representing the maximum linear velocity of the unmanned boat; w (w) min Representing a minimum angular velocity of the unmanned boat; w (w) max Representing unmannedMaximum angular velocity of the boat;
the calculation formula of the operation constraint speed of the unmanned ship motor is as follows:
wherein v is d Representing the operation constraint speed of the unmanned ship motor; v' represents the running linear speed of the unmanned ship motor; w' represents the rotational speed of the unmanned ship motor;representing the minimum running linear speed of the unmanned ship motor; />Representing the maximum running linear speed of the unmanned ship motor; />Representing the minimum operational angular speed of the unmanned ship motor; />Representing the maximum operational angular speed of the unmanned ship motor; t represents time;
the calculation formula of the braking constraint speed of the unmanned ship is as follows:
wherein v is s Representing the braking restraint speed of the unmanned ship; v "represents the braking linear velocity of the unmanned boat; w "represents the braking angular velocity of the unmanned boat; dist (v ", w") represents the braking distance of the unmanned boat;
step S23: according to the restraint speed of the unmanned ship, the operation restraint speed of the unmanned ship motor and the brake restraint speed of the unmanned ship, calculating a speed set of the unmanned ship to ensure a kinematic model of the unmanned ship, wherein the speed set of the unmanned ship is as follows:
v γ =v m ∩v d ∩v s ;
wherein v is γ Representing a set of speeds for the unmanned boat.
4. A path planning method based on dense obstacle and ocean current disturbance oriented improved DWA according to claim 3, characterized in that: in step S21, the kinematic model of the unmanned boat is expressed as:
wherein x (t) represents the abscissa of the unmanned ship in the world coordinate system at time t; y (t) represents the ordinate of the unmanned ship in the world coordinate system at the moment t; v represents the linear velocity of the unmanned boat; w represents the angular velocity of the unmanned boat; Δt represents a time interval; θ (t) represents the angle of the unmanned boat in the world coordinate system at time t.
5. A path planning method based on dense obstacle and ocean current disturbance oriented improved DWA according to claim 3, characterized in that: in step S2, the following sub-steps are also included:
carrying out local path planning on the unmanned aerial vehicle by adopting an improved DWA (discrete wavelet transform), obtaining a plurality of unmanned aerial vehicle navigation prediction planning paths, grading the plurality of unmanned aerial vehicle navigation prediction planning paths by adopting an improved DWA evaluation function, and selecting the unmanned aerial vehicle navigation prediction planning path with the highest grading as an unmanned aerial vehicle navigation optimal planning path;
the evaluation function expression of the improved DWA is as follows:
G(v,w)=α*heading(v,w)+β*dist(v,w)+γ*vel(v,w);
wherein G (v, w) represents an evaluation function of the improved DWA; head (v, w) represents the azimuth evaluation sub-function; dist (v, w) represents a distance evaluation sub-function of the unmanned ship and the obstacle, namely, the optimal distance between the predicted track and the obstacle at the next moment of the unmanned ship, and a specific calculation formula is as follows:
wherein, (x) usv ,y usv ) The position coordinates of the tail end of the predicted track at the next moment of the unmanned ship are represented; (x) ob ,y ob ) Representing position coordinates of the obstacle;
and vel (v, w) represents an unmanned ship speed evaluation sub-function, namely the speed of unmanned ship safety obstacle avoidance optimization, and a specific calculation formula is as follows:
vel(v,w)=|v usv |;
wherein v is usv Representing the speed of the unmanned ship predicted track;
alpha represents a weight parameter of the azimuth evaluation sub-function; beta represents a weight parameter of a distance evaluation sub-function of the unmanned ship and the obstacle; gamma denotes the weight parameter of the unmanned ship speed evaluation sub-function.
6. The path planning method based on dense obstacle and ocean current disturbance oriented improved DWA of claim 5, wherein: in step S2, the following sub-steps are also included:
according to the relative motion of the water flow and the unmanned ship and the ratio coefficient of the obstacle in the perception range of the unmanned ship, respectively adjusting the weight parameter alpha of the azimuth evaluation sub-function, the weight parameter beta of the distance evaluation sub-function between the unmanned ship and the obstacle and the weight parameter gamma of the speed evaluation sub-function of the unmanned ship in the improved DWA;
the calculation formula of the weight parameter alpha of the azimuth angle evaluation sub-function is as follows:
wherein v is usv Representing the speed of the unmanned ship predicted track; v w Indicating the water flow rate; alpha 0 Azimuth evaluation subfunction representing original inputA weight parameter;
the expression of the weight parameter beta of the distance evaluation sub-function of the unmanned ship and the obstacle is as follows:
β=β w +β ob ;
in the middle of
Wherein v is max Is the maximum propulsion speed of the unmanned ship, beta w Representing the part affected by water flow in the distance evaluation sub-function of the unmanned ship and the obstacle; beta ob Representing the part of the unmanned ship and obstacle distance evaluation subfunction affected by the number of surrounding obstacles; beta 0 A weight parameter representing a distance evaluation sub-function of the original input unmanned ship and the obstacle; n is n all Representing the uniform generation of the current n in the detection range D of the sensor on the unmanned ship all A plurality of position points, n ob Representing a total number of points falling on the obstacle grid among the generated position points within the range D;
the expression of the weight parameter gamma of the unmanned ship speed evaluation sub-function is as follows:
wherein, gamma 0 Weight parameters representing the original input unmanned ship speed evaluation sub-function.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311375777.1A CN117519138A (en) | 2023-10-20 | 2023-10-20 | Path planning method based on dense obstacle and ocean current interference oriented improved DWA |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311375777.1A CN117519138A (en) | 2023-10-20 | 2023-10-20 | Path planning method based on dense obstacle and ocean current interference oriented improved DWA |
Publications (1)
Publication Number | Publication Date |
---|---|
CN117519138A true CN117519138A (en) | 2024-02-06 |
Family
ID=89740866
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311375777.1A Pending CN117519138A (en) | 2023-10-20 | 2023-10-20 | Path planning method based on dense obstacle and ocean current interference oriented improved DWA |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117519138A (en) |
-
2023
- 2023-10-20 CN CN202311375777.1A patent/CN117519138A/en active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108564202B (en) | Unmanned ship route optimization method based on environment forecast information | |
US12013243B2 (en) | Passage planning and navigation systems and methods | |
CN109753068B (en) | Multi-USV group collaborative collision avoidance planning method considering communication situation | |
US11988513B2 (en) | Imaging for navigation systems and methods | |
CN108803313B (en) | Path planning method based on ocean current prediction model | |
CN111580518B (en) | Unmanned ship layered obstacle avoidance method based on improved drosophila optimization and dynamic window method | |
US20210166568A1 (en) | Collision avoidance systems and methods | |
CN111913406B (en) | Ship-shore collaborative simulation system for intelligent navigation and safety of ship | |
Lan et al. | Improved RRT algorithms to solve path planning of multi-glider in time-varying ocean currents | |
CN110837255B (en) | Autonomous danger avoiding method suitable for high-speed water surface unmanned ship | |
Bandyophadyay et al. | A simple reactive obstacle avoidance algorithm and its application in singapore harbor | |
CN109597417A (en) | A kind of more USV multi-agent synergy collision-avoidance planning methods based on collision prevention criterion | |
CN114610046A (en) | Unmanned ship dynamic safety trajectory planning method considering dynamic water depth | |
US12091137B1 (en) | Intelligent assistance system and method for berthing and unberthing based on multi-tugboat collaboration | |
CN114088094A (en) | Intelligent route planning method and system for unmanned ship | |
Jing et al. | Path planning and navigation of oceanic autonomous sailboats and vessels: A survey | |
CN109916400B (en) | Unmanned ship obstacle avoidance method based on combination of gradient descent algorithm and VO method | |
CN114387822B (en) | Ship collision prevention method | |
Liang et al. | Economic MPC-based planning for marine vehicles: Tuning safety and energy efficiency | |
CN110580045A (en) | obstacle avoidance method for unmanned ship | |
CN114061565A (en) | Unmanned ship SLAM and application method thereof | |
Ennong et al. | Design and experiment of a sea-air heterogeneous unmanned collaborative system for rapid inspection tasks at sea | |
Wu et al. | An overview of developments and challenges for unmanned surface vehicle autonomous berthing | |
Gao et al. | Artificial intelligence algorithms in unmanned surface vessel task assignment and path planning: A survey | |
CN117519138A (en) | Path planning method based on dense obstacle and ocean current interference oriented improved DWA |
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 |