CN117519138A - Path planning method based on improved DWA for dense obstacles and ocean current interference - Google Patents

Path planning method based on improved DWA for dense obstacles and ocean current interference Download PDF

Info

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 boat
speed
unmanned
unmanned ship
path planning
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
Application number
CN202311375777.1A
Other languages
Chinese (zh)
Inventor
杨立鑫
刘畅
林泽龙
欧伟
罗显涛
陈仲铭
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangdong University of Technology
Shenzhen Institute of Information Technology
Original Assignee
Guangdong University of Technology
Shenzhen Institute of Information Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangdong University of Technology, Shenzhen Institute of Information Technology filed Critical Guangdong University of Technology
Priority to CN202311375777.1A priority Critical patent/CN117519138A/en
Publication of CN117519138A publication Critical patent/CN117519138A/en
Pending legal-status Critical Current

Links

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

Path planning method based on dense obstacle and ocean current interference oriented improved DWA
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:
β=β wob
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:
β=β wob
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.基于面向密集障碍和海流干扰的改进DWA的路径规划方法,其特征在于:包括以下步骤:1. A path planning method based on improved DWA for dense obstacles and ocean current interference, which is characterized by: including the following steps: 步骤S1:构建无人艇路径规划的环境模型;Step S1: Construct an environment model for unmanned boat path planning; 步骤S2:在无人艇路径规划的环境模型中采用改进的动态窗口算法(DWA)对无人艇进行局部路径规划,得到无人艇航行最优规划路径。Step S2: Use the improved dynamic window algorithm (DWA) in the environment model of the unmanned boat path planning to perform local path planning for the unmanned boat, and obtain the optimal planned path for the unmanned boat navigation. 2.根据权利要求1所述的基于面向密集障碍和海流干扰的改进DWA的路径规划方法,其特征在于:在步骤S1中,采用栅格法构建无人艇路径规划的环境模型,具体的构建步骤如下:2. The path planning method based on improved DWA for dense obstacles and ocean current interference according to claim 1, characterized in that: in step S1, the grid method is used to construct an environment model for unmanned boat path planning, and the specific construction Proceed as follows: 将整个无人艇路径规划地图空间area划为m×m个栅格,每一个栅格均代表相应的环境状态,无人艇路径规划地图空间area表示为:The entire unmanned vehicle path planning map space area is divided into m×m grids. Each grid represents the corresponding environmental state. The unmanned vehicle path planning map space area is expressed as: area=∑Mij,i∈[1,m],j∈[1,m];area=∑M ij ,i∈[1,m],j∈[1,m]; 其中,Mij表示每个栅格相应的环境状态,每个栅格相应的环境状态表示为:Among them, M ij represents the corresponding environmental state of each grid, and the corresponding environmental state of each grid is expressed as: 3.根据权利要求1所述的基于面向密集障碍和海流干扰的改进DWA的路径规划方法,其特征在于:在步骤S2中,具体包括以下子步骤:3. The path planning method based on improved DWA for dense obstacles and ocean current interference according to claim 1, characterized in that: in step S2, it specifically includes the following sub-steps: 步骤S21:构建无人艇的运动学模型;Step S21: Construct the kinematic model of the unmanned boat; 步骤S22:计算无人艇的约束速度、无人艇电机的运转约束速度以及无人艇的制动约束速度,其中,Step S22: Calculate the constrained speed of the unmanned boat, the operating constrained speed of the unmanned boat motor, and the braking constrained speed of the unmanned boat, where, 无人艇自身的约束速度的计算公式如下:The calculation formula for the constrained speed of the unmanned boat itself is as follows: vm={(v,w)|v∈[vmin,vmax]∩w∈[wmin,wmax]};v m ={(v,w)|v∈[v min ,v max ]∩w∈[w min ,w max ]}; 其中,vm表示无人艇的约束速度;v表示无人艇的线速度;w表示无人艇的角速度;vmin表示无人艇的最小线速度;vmax表示无人艇的最大线速度;wmin表示无人艇的最小角速度;wmax表示无人艇的最大角速度;Among them, v m represents the constrained speed of the unmanned boat; v represents the linear speed of the unmanned boat; w represents the angular speed of the unmanned boat; v min represents the minimum linear speed of the unmanned boat; v max represents the maximum linear speed of the unmanned boat. ; w min represents the minimum angular speed of the unmanned boat; w max represents the maximum angular speed of the unmanned boat; 无人艇电机的运转约束速度的计算公式如下:The calculation formula for the operating constraint speed of the unmanned boat motor is as follows: 其中,vd表示无人艇电机的运转约束速度;v′表示无人艇电机的运转线速度;w′表示无人艇电机的运转角速度;表示无人艇电机的最小运转线速度;/>表示无人艇电机的最大运转线速度;/>表示无人艇电机的最小运转角速度;/>表示无人艇电机的最大运转角速度;t表示时间;Among them, v d represents the operating constraint speed of the unmanned boat motor; v′ represents the operating linear speed of the unmanned boat motor; w′ represents the operating angular speed of the unmanned boat motor; Indicates the minimum operating linear speed of the unmanned boat motor;/> Indicates the maximum operating linear speed of the unmanned boat motor;/> Indicates the minimum operating angular speed of the unmanned boat motor;/> represents the maximum operating angular speed of the unmanned boat motor; t represents time; 无人艇的制动约束速度的计算公式如下:The calculation formula of the braking constraint speed of the unmanned boat is as follows: 其中,vs表示无人艇的制动约束速度;v″表示无人艇的制动线速度;w″表示无人艇的制动角速度;dist(v″,w″)表示无人艇的制动距离;Among them, v s represents the braking constraint speed of the unmanned boat; v″ represents the braking linear speed of the unmanned boat; w″ represents the braking angular speed of the unmanned boat; dist(v″,w″) represents the braking speed of the unmanned boat. Braking distance; 步骤S23:根据无人艇的约束速度、无人艇电机的运转约束速度和无人艇的制动约束速度,计算无人艇的速度集合,以确保无人艇的运动学模型,其中,无人艇的速度集合如下:Step S23: Calculate the speed set of the unmanned boat according to the constrained speed of the unmanned boat, the operating constrained speed of the unmanned boat motor and the braking constrained speed of the unmanned boat to ensure the kinematic model of the unmanned boat, where The speed set of human boats is as follows: vγ=vm∩vd∩vsv γ =v m ∩v d ∩v s ; 其中,vγ表示无人艇的速度集合。Among them, v γ represents the speed set of the unmanned boat. 4.根据权利要求3所述的基于面向密集障碍和海流干扰的改进DWA的路径规划方法,其特征在于:在步骤S21中,无人艇的运动学模型表示为:4. The path planning method based on improved DWA for dense obstacles and ocean current interference according to claim 3, characterized in that: in step S21, the kinematic model of the unmanned boat is expressed as: 其中,x(t)表示t时刻无人艇在世界坐标系下的横坐标;y(t)表示t时刻无人艇在世界坐标系下的纵坐标;v表示无人艇的线速度;w表示无人艇的角速度;Δt表示时间间隔;θ(t)表示t时刻无人艇在世界坐标系下的角度。Among them, 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 time t; v represents the linear speed of the unmanned ship; w represents the angular velocity of the unmanned ship; Δt represents the time interval; θ(t) represents the angle of the unmanned ship in the world coordinate system at time t. 5.根据权利要求3所述的基于面向密集障碍和海流干扰的改进DWA的路径规划方法,其特征在于:在步骤S2中,还包括以下子步骤:5. The path planning method based on improved DWA for dense obstacles and ocean current interference according to claim 3, characterized in that: in step S2, it also includes the following sub-steps: 采用改进的DWA对无人艇进行局部路径规划,得到多个无人艇航行预测规划路径,并采用改进的DWA的评价函数对多个无人艇航行预测规划路径进行评分,选取评分最高的无人艇航行预测规划路径作为无人艇航行最优规划路径;The improved DWA is used to carry out local path planning for the unmanned boat, and multiple unmanned boat navigation predicted planning paths are obtained. The evaluation function of the improved DWA is used to score the multiple unmanned boat sailing predicted planning paths, and the unmanned boat with the highest score is selected. The predicted and planned path of the human vessel navigation is used as the optimal planning path of the unmanned vessel navigation; 改进的DWA的评价函数表达式如下:The evaluation function expression of the improved DWA is as follows: G(v,w)=α*heading(v,w)+β*dist(v,w)+γ*vel(v,w);G(v,w)=α*heading(v,w)+β*dist(v,w)+γ*vel(v,w); 其中,G(v,w)表示改进的DWA的评价函数;heading(v,w)表示方位角评价子函数;dist(v,w)表示无人艇与障碍物的距离评价子函数,即无人艇下一时刻预测轨迹与障碍物的最优距离,具体的计算公式如下:Among them, G(v,w) represents the evaluation function of the improved DWA; heading(v,w) represents the azimuth angle evaluation subfunction; dist(v,w) represents the distance evaluation subfunction between the unmanned boat and the obstacle, that is, none The optimal distance between the human and boat's predicted trajectory and obstacles at the next moment is as follows: 其中,(xusv,yusv)表示无人艇下一时刻预测轨迹末端的位置坐标;(xob,yob)表示障碍物的位置坐标;Among them, (x usv , y usv ) represents the position coordinates of the end of the predicted trajectory of the unmanned ship at the next moment; (x ob , y ob ) represents the position coordinates of the obstacle; vel(v,w)表示无人艇速度评价子函数,即无人艇安全避障最优的速度,具体的计算公式如下:vel(v,w) represents the speed evaluation subfunction of the unmanned boat, which is the optimal speed for the unmanned boat to safely avoid obstacles. The specific calculation formula is as follows: vel(v,w)=|vusv|;vel(v,w)=|v usv |; 其中,vusv表示无人艇预测轨迹的速度;Among them, v usv represents the speed of the unmanned boat’s predicted trajectory; α表示方位角评价子函数的权重参数;β表示无人艇与障碍物的距离评价子函数的权重参数;γ表示无人艇速度评价子函数的权重参数。α represents the weight parameter of the azimuth angle evaluation sub-function; β represents the weight parameter of the distance evaluation sub-function between the unmanned craft and the obstacle; γ represents the weight parameter of the unmanned craft speed evaluation sub-function. 6.根据权利要求5所述的基于面向密集障碍和海流干扰的改进DWA的路径规划方法,其特征在于:在步骤S2中,还包括以下子步骤:6. The path planning method based on improved DWA for dense obstacles and ocean current interference according to claim 5, characterized in that: in step S2, it also includes the following sub-steps: 根据水流与无人艇的相对运动,以及无人艇感知范围内的障碍物占比系数来分别调整改进的DWA的评价函数中方位角评价子函数的权重参数α、无人艇与障碍物的距离评价子函数的权重参数β和无人艇速度评价子函数的权重参数γ;According to the relative motion between the water flow and the unmanned boat, and the proportion coefficient of obstacles within the sensing range of the unmanned boat, the weight parameter α of the azimuth angle evaluation subfunction in the improved DWA evaluation function, the weight parameter α of the unmanned boat and the obstacle are adjusted respectively. The weight parameter β of the distance evaluation sub-function and the weight parameter γ of the unmanned boat speed evaluation sub-function; 其中,方位角评价子函数的权重参数α的计算公式如下:Among them, the calculation formula of the weight parameter α of the azimuth angle evaluation subfunction is as follows: 其中,vusv表示无人艇预测轨迹的速度;vw表示水流速度;α0表示原始输入的方位角评价子函数的权重参数;Among them, v usv represents the speed of the predicted trajectory of the unmanned boat; v w represents the water velocity; α 0 represents the weight parameter of the original input azimuth angle evaluation subfunction; 无人艇与障碍物的距离评价子函数的权重参数β的表达式为:The expression of the weight parameter β of the distance evaluation subfunction between the unmanned boat and the obstacle is: β=βwobβ=β wob ; 式中in the formula 其中,vmax为无人艇的最大推进速度,βw表示无人艇与障碍物的距离评价子函数中受水流影响的部分;βob表示无人艇与障碍物的距离评价子函数中受周边障碍物数量影响的部分;β0表示原始输入的无人艇与障碍物的距离评价子函数的权重参数;nall表示以无人艇上传感器探测范围D中均匀生成当前nall个位置点,nob表示范围D内的生成的位置点中落在障碍物栅格的点的总数;Among them, v max is the maximum propulsion speed of the unmanned boat, β w represents the part affected by the water flow in the distance evaluation sub-function of the unmanned boat and the obstacle; β ob represents the part affected by the water flow in the distance evaluation sub-function of the unmanned boat and the obstacle. The part affected by the number of surrounding obstacles; β 0 represents the weight parameter of the original input distance evaluation subfunction between the unmanned ship and the obstacles; n all represents the current n all position points uniformly generated in the detection range D of the sensor on the unmanned ship , n ob represents the total number of points falling on the obstacle grid among the generated position points within the range D; 无人艇速度评价子函数的权重参数γ的表达式为:The expression of the weight parameter γ of the unmanned boat speed evaluation sub-function is: 其中,γ0表示原始输入的无人艇速度评价子函数的权重参数。Among them, γ 0 represents the weight parameter of the original input unmanned boat speed evaluation subfunction.
CN202311375777.1A 2023-10-20 2023-10-20 Path planning method based on improved DWA for dense obstacles and ocean current interference Pending CN117519138A (en)

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 improved DWA for dense obstacles and ocean current interference

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 improved DWA for dense obstacles and ocean current interference

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 improved DWA for dense obstacles and ocean current interference

Country Status (1)

Country Link
CN (1) CN117519138A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN119179328A (en) * 2024-11-22 2024-12-24 铭派科技集团有限公司 Real-time intelligent sensing and real-time obstacle avoidance system and operation method of offshore unmanned ship
CN119356369A (en) * 2024-12-23 2025-01-24 铭派科技集团有限公司 Unmanned navigation path planning system and method based on adaptive planning

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN119179328A (en) * 2024-11-22 2024-12-24 铭派科技集团有限公司 Real-time intelligent sensing and real-time obstacle avoidance system and operation method of offshore unmanned ship
CN119356369A (en) * 2024-12-23 2025-01-24 铭派科技集团有限公司 Unmanned navigation path planning system and method based on adaptive planning

Similar Documents

Publication Publication Date Title
US11988513B2 (en) Imaging for navigation systems and methods
US20240410699A1 (en) Passage planning and navigation systems and methods
CN109753068B (en) Multi-USV group collaborative collision avoidance planning method considering communication situation
CN111580518B (en) A Layered Obstacle Avoidance Method for Unmanned Vehicles Based on Improved Drosophila Optimization and Dynamic Window Method
CN117519138A (en) Path planning method based on improved DWA for dense obstacles and ocean current interference
Lan et al. Improved RRT algorithms to solve path planning of multi-glider in time-varying ocean currents
CN109960262B (en) Unmanned ship dynamic obstacle avoidance method and system based on geometric method
CN110837255B (en) Autonomous danger avoiding method suitable for high-speed water surface unmanned ship
CN108664020A (en) A kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method
CN110262492A (en) A Real-time Collision Avoidance and Target Tracking Method for Unmanned Vehicles
CN108445879A (en) A kind of unmanned boat barrier-avoiding method based on prediction collision risk region
CN111045453A (en) Cooperative control system and method based on unmanned ship and multi-underwater robot
Han et al. An efficient motion planning based on grid map: Predicted Trajectory Approach with global path guiding
CN108416152A (en) The optimal global path planning method of unmanned boat ant colony energy consumption based on electronic chart
Gao et al. Artificial intelligence algorithms in unmanned surface vessel task assignment and path planning: A survey
CN114610046A (en) Unmanned ship dynamic safety trajectory planning method considering dynamic water depth
CN113778099A (en) Unmanned ship path planning method based on NDT algorithm and Hybrid A algorithm
CN114387822B (en) Ship collision prevention method
CN113296505B (en) A multi-mode path tracking control method for unmanned ship based on fast variable LOS
Liang et al. Economic MPC-based planning for marine vehicles: Tuning safety and energy efficiency
CN111412918A (en) Unmanned ship global safety path planning method
CN109916400B (en) An Obstacle Avoidance Method for Unmanned Vehicle Based on the Combination of Gradient Descent Algorithm and VO Method
CN110320907B (en) A double-deck collision avoidance method for unmanned surface vehicles based on improved ant colony algorithm and elliptical collision cone deduction model
CN114690772A (en) Encounter spatiotemporal prediction method and system for non-tracking ships in sea area
CN117908540A (en) Unmanned ship cluster obstacle avoidance method based on formation mapping

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