DWA and APF path planning method based on marine digital vehicle

Abstract

The invention discloses a DWA and APF path planning method based on an ocean digital vehicle, and is applied to the technical field of path planning. Comprising the following steps: initializing an ocean digital carrier, constructing a grid map and setting a target point and a starting point, planning a path by adopting an improved DWA algorithm, updating a current position to be a newly generated trajectory end point, determining a current local optimal plan by utilizing the DWA algorithm, and judging whether the current position is close to a final target point or not. And if yes, ending the algorithm and performing final path planning by using the APF, and if not, repeatedly adopting the improved DWA algorithm to plan a local path. According to the method, the improved DWA algorithm and the improved APF algorithm are fused, random unknown obstacles can be effectively avoided, and the algorithm is prevented from falling into a local minimum value in the final stage.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and more particularly to a DWA and APF path planning method based on an ocean digital vehicle. Background Art

[0002] In today's era of rapid development of unmanned vehicle technology, the application of unmanned driving technology is no longer limited to land. Unmanned ship technology in the sea field also has great potential and broad application prospects. Therefore, accelerating the development of unmanned ship path planning technology is of great significance to promoting the transformation and upgrading of the marine economy and the development and utilization of marine resources. The progress of unmanned ship path planning technology can provide efficient and accurate solutions for fields such as marine observation, monitoring and exploration. Through path planning and autonomous navigation, unmanned ships can complete tasks such as data collection and environmental monitoring in complex marine environments, greatly reducing labor costs and risks. Efficient path planning technology can improve the transportation efficiency of unmanned ships and reduce operating costs. At the same time, it can also quickly reach the destination in an emergency and improve rescue efficiency. Path planning is the core issue faced by unmanned ship autonomous tasks. A qualified path planning algorithm needs to meet the efficient and safe working requirements of unmanned ships. The path planning algorithm hopes to find an optimal path from the starting point to the target point within a limited time. The path planning algorithm can prevent unmanned vehicles from colliding with other vehicles, pedestrians, and obstacles. Safety is the primary requirement for autonomous driving of unmanned vehicles. The path planning algorithm must be able to fully consider changes in the surrounding environment and plan obstacle avoidance paths in a timely manner.

[0003] The local path planning algorithm is responsible for planning the obstacle avoidance path in a real-time environment, and usually adopts DWA (dynamic window algorithm), artificial potential field algorithm, etc. It can avoid obstacles very well in the face of obstacles, and then integrate and import them into the digital platform for better path planning. The DWA algorithm can quickly calculate the optimal speed in each control cycle, so that the carrier can respond to environmental changes in real time, and is suitable for use in dynamic environments. The APF algorithm can usually generate a continuous and smooth motion path, reduce the sudden turning and acceleration of the carrier, and improve the stability of driving. The combination of the two forms a complete obstacle avoidance measure. However, the existing DWA algorithm is prone to falling into the local optimal solution. When there are obstacles near the target point, the carrier may fall into the local minimum and cannot find a path to reach the target point directly. Therefore, how to provide a DWA and APF path planning method based on marine digital carriers is a problem that technicians in this field urgently need to solve. Summary of the invention

[0004] In view of this, the present invention provides a DWA and APF path planning method based on an ocean digital vehicle, which solves the defect that the DWA algorithm is prone to fall into a local optimal solution when performing local path planning, and improves the feasibility and safety of vehicle path planning.

[0005] In order to achieve the above object, the present invention provides the following technical solutions:

[0006] A DWA and APF path planning method based on an ocean digital vehicle comprises the following steps:

[0007] S1. Initialize the ocean digital vehicle and map information according to the input information of the ocean digital vehicle interface, build a map using the grid method, and simulate terrain and obstacle information;

[0008] S2, using the improved DWA algorithm for local path planning, extracting and updating the path and vehicle status information;

[0009] S3, judging whether the ocean digital vehicle has reached the vicinity of the target point, if so, calling the APF algorithm for path planning, otherwise continuing to use the improved DWA algorithm for local path planning;

[0010] S4. After the ocean digital vehicle arrives at the target point, the path and the ocean digital vehicle parameter information are input into the ocean digital vehicle platform to display the path.

[0011] Optionally, the ocean digital carrier in S1 creates a new map information for subsequent path planning based on the map information converted from the nautical chart data, and sends the required data after the planning is completed.

[0012] Optionally, the improved DWA algorithm is specifically as follows: setting the initial state of the ocean digital vehicle, including the initial position, speed and target point position, setting the parameters of the improved DWA algorithm, including the range of speed and heading angle, time step, and prediction window, sampling within the preset speed and heading angle range based on the current speed, generating a series of local paths, evaluating each sampled path, selecting the path with the highest comprehensive evaluation based on the evaluation results, determining the speed and heading angle of the ocean digital vehicle at the next moment, and the ocean digital vehicle performing actions according to the selected speed and heading angle.

[0013] Optionally, the dynamic window calculation in the improved DWA algorithm is specifically as follows:

[0014] V s ={(v,w)|v∈[v min ,v max ],w∈[w min ,w max ]}

[0015] V d ={(v,w)|v∈[v c -v a dt,v c +v a dt],w∈[w c -w a dt,w c +w a dt]}

[0016] Among them, v min 、v max 、w min 、w max 、v c 、v a 、w c 、w a and dt are the minimum speed, maximum speed, minimum angular velocity, maximum angular velocity, current speed, maximum acceleration, current heading angle, maximum angular acceleration and time step allowed by the ocean digital vehicle respectively.

[0017] Optionally, a series of local paths are generated in the improved DWA algorithm as follows:

[0018] The number of optional speed combinations (v, w) n is uniformly sampled in the dynamic window of the improved DWA algorithm:

[0019] n=[(v max -v min ) / M v ]·[(w max -w min ) / M w ]

[0020] Where M v 、M w They are speed resolution and angular velocity resolution respectively;

[0021] x k+1 =x k +v·cosθ·Δt

[0022] y k+1 =x k +v·sinθ·Δt

[0023] θ k+1 =θ k +w·Δt

[0024] Where (x, y, θ) is the current position and heading angle of the ocean digital vehicle.

[0025] Optionally, the evaluation function of the improved DWA algorithm is composed of a heading evaluation function, a speed evaluation function, and a weighted obstacle evaluation function, wherein the heading evaluation function is based on the heading difference evaluation between the ocean digital vehicle and the target, the speed evaluation function is evaluated based on the maximum speed of the ocean digital vehicle in the future time, and the obstacle evaluation function is evaluated based on the maximum distance between the ocean digital vehicle and the obstacle.

[0026] Optionally, improve the evaluation function f of the DWA algorithm eval(v,w) Specifically:

[0027]

[0028] In the formula, goal(v,w) is the heading evaluation function, vel(v) is the speed evaluation function, obs(v,w) is the obstacle evaluation function, c goal 、c vel 、c obs are the weight coefficients corresponding to the heading evaluation function, speed evaluation function and obstacle evaluation function respectively, and t is the step time.

[0029] Optionally, the APF algorithm is specifically:

[0030] Calculate the average angle Theta between the current point and the obstacle and the target point:

[0031]

[0032] In the formula, x1 is the position of the current point, x2 is the position of the obstacle and the target point, T(x1,x2) calculates the vector between the two points, performs vector sum and obtains the heading information;

[0033] Calculate the gravitational potential energy U between the current point and the target point a :

[0034]

[0035] In the formula, k a is the gravitational gain function, p1 is the current point, p2 is the target point, and f(p1,p2) is the distance between the current point and the target point;

[0036] Calculate the repulsive potential energy U between the obstacle and the current point r :

[0037]

[0038] In the formula, k r is the repulsion gain function, dis is the threshold of the obstacle influence range, q1 is the obstacle coordinate, and q2 is the current point coordinate;

[0039] The gravitational potential energy and repulsive potential energy are superimposed to obtain the total potential field. The direction and speed are changed according to the total potential field force to obtain the path information.

[0040] Optionally, the ocean digital vehicle platform displays the collision avoidance route output by the automatic collision avoidance algorithm on the global situation interface, displays the output of the expected route and the collision avoidance route on the real-scene perception interface, and displays the expected heading and speed of the expected route.

[0041] It can be seen from the above technical solutions that, compared with the prior art, the present invention provides a DWA and APF path planning method based on an ocean digital vehicle, which has the following beneficial effects:

[0042] 1. In the traditional DWA algorithm, the present invention adds distance adaptive weight to adjust the impact of various evaluations, which effectively improves the trajectory and speed oscillation of the DWA algorithm when facing obstacles;

[0043] 2. The present invention combines the nautical chart with the simulation chart, and can input the longitude and latitude to simulate and generate the raster map, which can also save some memory space and improve the algorithm efficiency;

[0044] 3. The present invention integrates the improved DWA algorithm and the APF algorithm, and further adjusts the evaluation function, so that the DWA algorithm can efficiently avoid obstacles, and integrates the APF algorithm to prevent DWA from falling into the local minimum situation. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0046] Figure 1 is a flow chart of the path planning method of the present invention;

[0047] Figure 2 is a schematic diagram of grid map information in an embodiment of the present invention;

[0048] Figure 3 A path diagram for overall DWA and APF algorithm planning in an embodiment of the present invention;

[0049] Figure 4 A detailed path diagram for local DWA and APF algorithm planning in an embodiment of the present invention;

[0050] Figure 5 This is a real scene interface diagram of the ocean digital carrier in an embodiment of the present invention;

[0051] Figure 6 It is a top view of the real scene interface of the ocean digital carrier in an embodiment of the present invention. DETAILED DESCRIPTION

[0052] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0053] The embodiment of the present invention discloses a DWA and APF path planning method based on an ocean digital vehicle, such as Figure 1 As shown, the following steps are included:

[0054] S1. Initialize the ocean digital vehicle and map information according to the input information of the ocean digital vehicle interface, build a map using the grid method, and simulate terrain and obstacle information;

[0055] S2, using the improved DWA algorithm for local path planning, extracting and updating the path and vehicle status information;

[0056] S3, judging whether the ocean digital vehicle has reached the vicinity of the target point, if so, calling the APF algorithm for path planning, otherwise continuing to use the improved DWA algorithm for local path planning;

[0057] S4. After the ocean digital vehicle arrives at the target point, the path and the ocean digital vehicle parameter information are input into the ocean digital vehicle platform to display the path.

[0058] Furthermore, the ocean digital carrier in S1 creates a new map information for subsequent path planning based on the map information converted from the nautical chart data, and sends the required data after the planning is completed.

[0059] In the embodiment of the present invention, the input information includes the longitude, latitude and depth, heading angle and speed of the carrier. S1 marks the latitude and longitude coordinate information of the end point on the carrier. According to the electronic chart data, the carrier start point and the marked end point information, based on the latitude and longitude data of the end point and the start point, a grid map is established and the edge is extended by 0.2 latitude and longitude. The grid map is as follows: Figure 2 As shown, the initialization is set according to the vehicle motion model and performance limit as the input information of the algorithm. The nautical chart is converted to generate a mat file to extract the obstacle range part required in the mat file, read the range set by the start and end points, and add 0.2 longitude and latitude as the current dynamic window grid map. The map contains a 01 array, 0 is the sea area, and 1 is the obstacle information.

[0060] Furthermore, the improved DWA algorithm is specifically as follows: setting the initial state of the ocean digital vehicle, including the initial position, speed and target point position, setting the parameters of the improved DWA algorithm, including the range of speed and heading angle, time step, and prediction window, sampling is performed within the preset speed and heading angle range based on the current speed, generating a series of local paths, evaluating each sampled path, and selecting the path with the highest comprehensive evaluation based on the evaluation results, determining the speed and heading angle of the ocean digital vehicle at the next moment, and the ocean digital vehicle performs actions according to the selected speed and heading angle.

[0061] Furthermore, the dynamic window calculation in the improved DWA algorithm is specifically as follows:

[0062] V s ={(v,w)|v∈[v min ,v max ],w∈[w min ,w max ]}

[0063] V d ={(v,w)|v∈[v c -v a dt,v c +v a dt],w∈[w c -w a dt,w c +w a dt]}

[0064] Among them, v min 、v max 、w min 、w max 、v c 、v a 、w c 、w a and dt are the minimum speed, maximum speed, minimum angular velocity, maximum angular velocity, current speed, maximum acceleration, current heading angle, maximum angular acceleration and time step allowed by the ocean digital vehicle respectively.

[0065] Furthermore, the improved DWA algorithm generates a series of local paths as follows:

[0066] The number of optional speed combinations (v, w) n is uniformly sampled in the dynamic window of the improved DWA algorithm:

[0067] n=[(v max -v min ) / M v ]·[(w max -wmin ) / M w ]

[0068] Where M v 、M w They are speed resolution and angular velocity resolution respectively;

[0069] x k+1 =x k +v·cosθ·Δt

[0070] y k+1 =x k +v·sinθ·Δt

[0071] θ k+1 =θ k +w·Δt

[0072] Where (x, y, θ) is the current position and heading angle of the ocean digital vehicle.

[0073] Furthermore, the evaluation function of the improved DWA algorithm is composed of a heading evaluation function, a speed evaluation function, and a weighted obstacle evaluation function. The heading evaluation function is based on the heading difference between the ocean digital vehicle and the target, the speed evaluation function is based on the maximum speed of the ocean digital vehicle in the future, and the obstacle evaluation function is based on the maximum distance between the ocean digital vehicle and the obstacle.

[0074] In the embodiment of the present invention, when performing the DWA algorithm, it is also necessary to consider whether the next point will touch the obstacle, select the speed and angle, set the obstacle's influence range, traverse the obstacle position range, and prevent the next point from hitting the obstacle.

[0075] Furthermore, the evaluation function f of the DWA algorithm is improved eval(v,w) Specifically:

[0076]

[0077] In the formula, goal(v,w) is the heading evaluation function, vel(v) is the speed evaluation function, obs(v,w) is the obstacle evaluation function, c goal 、c vel 、c obs are the weight coefficients corresponding to the heading evaluation function, speed evaluation function and obstacle evaluation function respectively, and t is the step time.

[0078] In the embodiment of the present invention, the evaluation function ratio is selected before the evaluation. By judging whether there is an obstacle between the starting point and the end point, when there is no obstacle between the starting point and the end point, the speed is selected, and the heading is prioritized. When the carrier is too close to the obstacle, the distance from the obstacle is used, and the heading is prioritized. When there is an obstacle in the middle and it is too close to the obstacle, the heading and the distance from the obstacle are prioritized. The evaluation functions are heading, speed, and the distance to the obstacle as evaluation objects. The heading evaluation function is the difference between the azimuth between the position of the carrier and the target point after dt time and the current heading angle. The larger the difference, the lower the heading score for the heading angle, and the higher the speed, the higher the score. As for the obstacle distance, it is divided into static obstacles and dynamic obstacles. The obstacle ranges of the two are different. The range of dynamic obstacles is larger than that of static obstacles. The farther away from the obstacle, the higher the score. The highest score calculated comprehensively will become the heading and position of the next point.

[0079] Specifically, the weight coefficient is determined based on the obstacles between the target point and the current position. According to experience, when there are no obstacles between the starting point and the end point, select speed, heading priority, set the heading coefficient to 0.5, set the speed coefficient to 0.5, and the obstacle coefficient to 0.3. When the vehicle is too close to the obstacle, use the distance to the obstacle, heading priority, set the heading coefficient to 0.4, set the speed coefficient to 0.3, and the obstacle coefficient to 0.5. When there is an obstacle in the middle and it is too close to the obstacle, the heading and distance to the obstacle are prioritized, set the heading coefficient to 0.3, the speed coefficient to 0.2, and the obstacle coefficient to 0.5.

[0080] Furthermore, the APF algorithm is specifically as follows:

[0081] Calculate the average angle Theta between the current point and the obstacle and the target point:

[0082]

[0083] In the formula, x1 is the position of the current point, x2 is the position of the obstacle and the target point, T(x1,x2) calculates the vector between the two points, performs vector sum and obtains the heading information;

[0084] Calculate the gravitational potential energy U between the current point and the target point a :

[0085]

[0086] In the formula, k a is the gravitational gain function, p1 is the current point, p2 is the target point, and f(p1,p2) is the distance between the current point and the target point;

[0087] Calculate the repulsive potential energy U between the obstacle and the current point r :

[0088]

[0089] In the formula, k r is the repulsion gain function, dis is the threshold of the obstacle influence range, q1 is the obstacle coordinate, and q2 is the current point coordinate;

[0090] The gravitational potential energy and repulsive potential energy are superimposed to obtain the total potential field. The direction and speed are changed according to the total potential field force to obtain the path information.

[0091] Furthermore, the ocean digital vehicle platform displays the collision avoidance route output by the automatic collision avoidance algorithm on the global situation interface, displays the output of the expected route and the collision avoidance route on the real-scene perception interface, and displays the expected heading and speed of the expected route.

[0092] In an embodiment of the present invention, a route is set using path planning, such as Figure 3 and Figure 4 As shown, the system will calculate the collision avoidance route for the route planning based on the algorithm. After selecting the path, you can click on the path to monitor, delete or modify it, or export the path as a file for next use. The arrow direction of the collision avoidance route is the adjustment direction of the actual speed and the expected speed. Forward means acceleration, and backward means deceleration.

[0093] like Figure 5 and Figure 6 As shown, the final path information is imported into the digital carrier through the data interface. The digital carrier will parse and process the received data, initialize the route planning system, and display the original planned route in red lines, that is, the ideal route of the ship or aircraft when obstacle avoidance factors are not considered. On the basis of the expected route, a route to be adjusted is initialized to provide a reference for subsequent collision avoidance path calculations, and finally realize an efficient and safe navigation planning system.

[0094] The various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referenced to each other.

[0095] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A DWA and APF path planning method based on an ocean digital vehicle, characterized in that: The following steps are involved: S1. Initialize the ocean digital vehicle and map information according to the input information of the ocean digital vehicle interface, build a map using the grid method, and simulate terrain and obstacle information; S2, using the improved DWA algorithm for local path planning, extracting and updating the path and vehicle status information; S3, judging whether the ocean digital vehicle has reached the vicinity of the target point, if so, calling the APF algorithm for path planning, otherwise continuing to use the improved DWA algorithm for local path planning; S4. After the ocean digital vehicle arrives at the target point, the path and the ocean digital vehicle parameter information are input into the ocean digital vehicle platform to display the path.

2. A DWA and APF path planning method based on an ocean digital vehicle according to claim 1, characterized in that: The ocean digital carrier in S1 creates a new map information based on the map information converted from the nautical chart data for subsequent path planning, and sends the required data after the planning is completed.

3. A DWA and APF path planning method based on an ocean digital vehicle according to claim 1, characterized in that: The improved DWA algorithm is specifically as follows: setting the initial state of the ocean digital vehicle, including position, speed and target point, setting the parameters of the improved DWA algorithm, including the range of speed and heading angle, time step, and prediction window, sampling within the preset speed and heading angle range based on the current speed, generating a series of local paths, evaluating each sampled path, and selecting the path with the highest comprehensive evaluation based on the evaluation results, determining the speed and heading angle of the ocean digital vehicle at the next moment, and the ocean digital vehicle performing actions according to the selected speed and heading angle.

4. A DWA and APF path planning method based on an ocean digital vehicle according to claim 3, characterized in that: The calculation of dynamic window in the improved DWA algorithm is as follows: V s ={(v,w)|v∈[v min ,v max ],w∈[w min ,w max ]} V d ={(v,w)|v∈[v c -v a dt,v c +v a dt],w∈[w c -w a dt,w c +w a dt]} Among them, v min 、v max 、w min 、w max 、v c 、v a 、w c 、w a and dt are the minimum speed, maximum speed, minimum angular velocity, maximum angular velocity, current speed, maximum acceleration, current heading angle, maximum angular acceleration and time step allowed by the ocean digital vehicle respectively.

5. The DWA and APF path planning method based on ocean digital vehicle according to claim 4 is characterized in that: The improved DWA algorithm generates a series of local paths as follows: The number of optional speed combinations (v, w) n is uniformly sampled in the dynamic window of the improved DWA algorithm: n=[(v max -v min ) / M v ]·[(w max -w min ) / M w ] Where M v 、M w They are speed resolution and heading angular velocity resolution respectively; x k+1 =x k +v·cosθ·Δt y k+1 =x k +v·sinθ·Δt i k+1 =θ k +w·Δt Where (x, y, θ) is the current position and heading angle of the ocean digital vehicle.

6. A DWA and APF path planning method based on an ocean digital vehicle according to claim 3, characterized in that: The evaluation function of the improved DWA algorithm is composed of a heading evaluation function, a speed evaluation function, and a weighted obstacle evaluation function. The heading evaluation function is based on the heading difference between the ocean digital vehicle and the target, the speed evaluation function is based on the maximum speed of the ocean digital vehicle in the future, and the obstacle evaluation function is based on the maximum distance between the ocean digital vehicle and the obstacle.

7. A DWA and APF path planning method based on an ocean digital vehicle according to claim 6, characterized in that: Improved evaluation function f of DWA algorithm eval(v,w) Specifically: In the formula, goal(v,w) is the heading evaluation function, vel(v) is the speed evaluation function, obs(v,w) is the obstacle evaluation function, c goal 、c vel 、c obs are the weight coefficients corresponding to the heading evaluation function, speed evaluation function and obstacle evaluation function respectively, and t is the step time.

8. The DWA and APF path planning method based on ocean digital vehicle according to claim 1, characterized in that: The APF algorithm is as follows: Calculate the average angle Theta between the current point and the obstacle and the target point: In the formula, x1 is the position of the current point, x2 is the position of the obstacle and the target point, T(x1,x2) calculates the vector between the two points, and performs vector sum to obtain the heading information; Calculate the gravitational potential energy U between the current point and the target point a : In the formula, k a is the gravitational gain function, p1 is the current point, p2 is the target point, and f(p1,p2) is the distance between the current point and the target point; Calculate the repulsive potential energy U between the obstacle and the current point r : In the formula, k r is the repulsion gain function, dis is the threshold of the obstacle influence range, q1 is the obstacle coordinate, and q2 is the current point coordinate; The gravitational potential energy and repulsive potential energy are superimposed to obtain the total potential field. The direction and speed are changed according to the total potential field force to obtain the path information.

9. The DWA and APF path planning method based on ocean digital vehicle according to claim 1, characterized in that: The ocean digital vehicle platform displays the collision avoidance route output by the automatic collision avoidance algorithm on the global situation interface, displays the output of the expected route and the collision avoidance route on the real-scene perception interface, and displays the expected heading and speed of the current expected route.

Images