A dynamic safety trajectory planning method for unmanned vehicles considering dynamic water depth

Abstract

The present invention relates to a method for planning a dynamic safety trajectory for an unmanned boat taking into account dynamic water depth. The method uses the basic parameters of the unmanned boat, the target information provided by the perception system, and the time position and speed information output by the navigation system as input parameters, and ultimately outputs a safe trajectory for the unmanned boat that comprehensively considers the dynamic water depth and dynamic and static obstacles on the water surface. The trajectory planning method calculates the minimum safe water depth for navigation using the basic parameters of the unmanned boat; defines the water depth hazard and obstacle hazard by comprehensively considering waves, currents, and dynamic and static obstacles, and quantitatively evaluates the path safety level; and updates the trajectory based on the target information output in real time by the perception system and the navigation information output by the navigation system. The advantage of the present invention is that it fully considers the dynamic water depth factors and surface obstacle factors that threaten navigation safety under interference conditions such as currents and waves in the actual ocean environment, overcomes the defect that the traditional A* algorithm can only perform discrete angle searches when using rasterized maps, and effectively improves the safety and practicality of the planned trajectory.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned boat path planning, and in particular to a method for planning a dynamic and safe trajectory of an unmanned boat taking dynamic water depth into consideration. Background Art

[0002] Autonomous unmanned vehicles (UAVs) and surface robots, as a key component of marine robotics, serve as crucial links between the air, space, and deep sea. This makes them a crucial tool for marine scientific research, marine resource development, marine environmental monitoring, and safeguarding national maritime security, and they are gaining increasing attention worldwide. However, motion planning and intelligent decision-making technologies for UAVs remain a bottleneck in achieving a high degree of autonomy.

[0003] Achieving high levels of autonomy for individual and swarm unmanned vehicles (UAVs) in highly dynamic and uncertain marine environments requires consideration of numerous factors, including chart recognition and environmental modeling, behavioral decision-making and motion planning, and robust path-following control methods. Compared to unmanned vehicles (UAVs) and drones, UAVs operate in a more complex environment. In addition to complex environmental disturbances such as wind, waves, and currents, UAVs also have greater inertia and motion response time than UAVs and UAVs, introducing greater uncertainty into actual control and planning. Traditional motion planning methods still have significant shortcomings in addressing UAV decision-making and planning challenges in these complex environments. Reliable behavioral decision-making and motion planning methods are needed under complex constraints, including environmental constraints, kinematic and dynamic constraints, navigational rules constraints, and optimization objective constraints. Summary of the Invention

[0004] The purpose of the present invention is to overcome the shortcomings of the existing technology and propose a dynamic and safe trajectory planning method for unmanned boats that takes into account dynamic water depth. It optimizes the traditional shortest path planning algorithm and solves the problem that traditional methods do not consider the impact of environmental disturbances caused by waves and currents, thereby generating non-smooth routes with high collision risks. It can effectively improve the safety and practicality of path planning.

[0005] The present invention solves the technical problem by adopting the following technical solutions:

[0006] A method for planning a dynamic safety trajectory of an unmanned vehicle considering dynamic water depth includes the following steps:

[0007] Step 1: Set the parameters related to the unmanned boat planning task;

[0008] Step 2: Build a global environment model based on the parameters of step 1;

[0009] Step 3: Use the LT-D*Lite algorithm to perform global path planning;

[0010] Step 4: Based on the dynamic target information input by the perception system of the unmanned boat, a local situation map is established based on the speed position and speed information of the unmanned boat;

[0011] Step 5: Determine whether the current position of the unmanned boat is the end point. If it reaches the end point, the planning ends. Otherwise, repeat step 4 until the boat reaches the end point.

[0012] Moreover, the specific implementation method of step 1 is: the mission-related parameters include: the size, draft, operating performance, starting position, end position, set mission start time and navigation speed of the unmanned boat.

[0013] Furthermore, the step 2 includes the following steps:

[0014] Step 2.1: Based on the coordinates of the starting and ending points of the unmanned boat in step 1, extract the pre-processed vector electronic nautical chart from the chart library, and then extract the electronic nautical chart attribute data, navigation area chart water depth data, current data and obstacle data, and chart positioning error from the vector chart, and perform corresponding format conversion and attribute data processing;

[0015] Step 2.2: Calculate the minimum safe water depth and determine the grid resolution based on the hydrodynamic characteristics of the unmanned boat and the size of the nautical chart.

[0016] Step 2.3: Based on the navigation area chart water depth data and obstacle data obtained in step 2.1 and the resolution of the grid obtained in step 2.2, an interpolation algorithm is used to interpolate the water depth to obtain a gridded predicted water depth;

[0017] Step 2.4: Based on the mission start time and navigation speed set in step 1, obtain hydrological, meteorological, and tidal information of the mission waters through forecast data;

[0018] Step 2.5: Based on the grid resolution obtained in step 2.2, process the hydrological, meteorological, and tidal information of the mission waters obtained in step 2.4, and use the interpolation method based on step 2.3 to obtain the dynamic water depth distribution and ocean current distribution;

[0019] Step 2.6: Construct a global environmental model based on the minimum safe water depth calculated in step 2.2 and the dynamic water depth distribution and ocean current distribution obtained in step 2.5.

[0020] Furthermore, the specific implementation method of step 2.2 is: by establishing a hydrodynamic model or conducting a tank experiment, analyzing and calculating the characteristics of the navigation attitude change of the unmanned vehicle under different hydrodynamic conditions, and estimating the navigation attitude change in the actual marine environment:

[0021] according to Calculate the minimum safe water depth,

[0022] Among them, S min is the minimum safe water depth, z max is the maximum amplitude of the downward heave motion generated by the unmanned boat sailing in irregular waves at different speeds; L is the length of the unmanned boat; θ max is the maximum amplitude of the pitching motion produced by the unmanned vehicle sailing in irregular waves under different working conditions; is the average draft of the unmanned boat; e enc is the water depth error of the electronic chart;

[0023] The grid resolution is determined based on the hydrodynamic characteristics of the unmanned boat and the size of the chart: min , chart positioning error e p , the length of the unmanned boat L and the resolution of the electronic chart range calculation grid, and the grid size should be selected so that the number of grids is not greater than 10 8 , and the grid size should be no less than 3L+R min +e p , no more than 10L+4R min +2e p .

[0024] Moreover, the specific implementation method of step 2.3 is: using the spline function interpolation algorithm with obstacles, taking the obstacle data obtained in step 2.1 as the obstacle input element, taking the water depth data as the interpolation input data, setting the smoothing coefficient in the spline interpolation function, interpolating the sparse water depth points, and obtaining the rasterized predicted water depth.

[0025] Moreover, the specific implementation method of step 2.5 is: the instantaneous water depth is:

[0026] S(x,y,t)=D(x,y)+T(x,y,t)+Δ(x,y,t)

[0027] Where S(x,y,t) is the instantaneous water depth value of the (x,y) grid at time t; D(x,y) is the static water depth value of the grid point; T(x,y,t) is the tide level value of the grid point at time t; Δ(x,y,t) is the water depth change caused by other factors affecting the water level change at time t at the grid point; the sum of T(x,y,t) and Δ(x,y,t) constitutes the dynamic water depth term in the instantaneous water depth.

[0028] Moreover, the specific calculation method of step 3 is:

[0029]

[0030] Among them, g(N i ) is the i-th node N i The actual distance cost to the starting node S; h(Ni ) is node N i The estimated distance cost to the target node G; is the sum of the water depth hazards from the starting node to the current node; α is a parameter that controls the impact of water depth risk and is set according to the path planning target preference; is the sum of the hazard levels of water obstacles from the starting node to the current node; β is the parameter that controls the risk impact of water obstacles and is set according to the path planning target preference.

[0031] Furthermore, the step 4 includes the following steps:

[0032] Step 4.1: Create a local situation map based on the global path planning obtained in step 3 and the target information input by the UAV's perception system;

[0033] Step 4.2: Predict the trajectory of the dynamic obstacle based on the target information input by the perception system of the unmanned boat;

[0034] Step 4.3: Determine whether there is a collision risk along the current path based on the predicted trajectory in step 4.2. If there is a risk, proceed to step 4.4; otherwise, proceed to step 5.

[0035] Step 4.4: Start re-planning, update the grid in the area according to the predicted trajectory information and dynamic water depth, generate a local path, and return to step 4.1 to update the local situation map.

[0036] Moreover, the perception system in step 4.1 includes navigation radar, lidar, AIS, panoramic camera and depth sounder.

[0037] Furthermore, the specific implementation method of step 4.3 is as follows: Grid water surface collision risk:

[0038]

[0039] Where d is the node N i To obstacle node O j Euclidean distance, a is the risk factor corresponding to different types of obstacles, v Ni is grid N i The speed of the ocean current c Ni is the direction coefficient, is the direction of the current relative to true north, φ g ∈[0,2π) is the node N i With node O j The azimuth of the connecting line relative to the true north. When there are k obstacle grids near a navigable node, the maximum value is selected as the surface obstacle hazard of the node:

[0040] r S (N i)=max{r S [N i ,1],r S [N i ,2],...,r S [N i ,k]}.

[0041] The advantages and positive effects of the present invention are:

[0042] 1. The present invention uses the basic parameters of the unmanned boat, the target information provided by the perception system, and the time position and speed information output by the navigation system as input parameters, and ultimately outputs a safe trajectory for the unmanned boat that comprehensively considers the dynamic water depth and dynamic and static obstacles on the water surface. The trajectory planning method uses the basic parameters of the unmanned boat to calculate the minimum safe water depth for navigation; comprehensively defines the water depth hazard and obstacle hazard based on waves, currents, and dynamic and static obstacles, and quantitatively evaluates the path safety level; and updates the trajectory based on the target information output in real time by the perception system and the navigation information output by the navigation system. The advantage of the present invention is that it fully considers the dynamic water depth factors and surface obstacle factors that threaten navigation safety under interference conditions such as currents and waves in the actual ocean environment, overcomes the defect that the traditional A* algorithm can only perform discrete angle searches when using rasterized maps, and can effectively improve the safety and practicality of the planned trajectory.

[0043] 2. The calculation process of the present invention takes into account the potential risks caused by waves, current interference and dynamic obstacles, and can effectively balance the threats of underwater obstacles and surface obstacles as well as the path length of the unmanned boat trajectory planning method. At the same time, the planning results generated by this method are safer and more reasonable, and the planned global path has fewer turning points and is smoother. If the sea conditions are not taken into account and the traditional path planning method is used for solution, it will cause the situation of touching underwater obstacles when the water depth is dangerously high; when sailing along the global path, collisions with surface obstacles may occur due to ignoring the dynamic changes in water depth and the interference effects of ocean currents. The use of the present invention can effectively solve the above problems and effectively improve the safety and practicality of the global path planning of unmanned boats. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 Flowchart of the present invention. DETAILED DESCRIPTION

[0045] The present invention is further described below in conjunction with the accompanying drawings.

[0046] A method for planning a dynamic safety trajectory of an unmanned vehicle considering dynamic water depth includes the following steps:

[0047] Step 1: Set the parameters related to the UAV planning task.

[0048] The task-related parameters in this step include: starting and ending positions, departure time, size of the unmanned boat and maneuverability parameters, including but not limited to the unmanned boat attitude statistics under various sea conditions, average draft, minimum turning radius, maximum speed, starting position, end position, set task start time and sailing speed, etc.

[0049] Step 2: Build a global environment model based on the parameters of step 1.

[0050] Step 2.1. Based on the coordinates of the starting and ending positions of the unmanned boat in step 1, extract the pre-processed vector electronic chart from the chart library, and then extract the electronic chart attribute data, navigation area chart water depth data, current data and obstacle data, chart positioning error from the vector chart and perform corresponding format conversion and attribute data processing.

[0051] Among them, obstacle data includes but is not limited to land areas, navigation aids, bridge piers, no-fly zones, dry reefs, seabed areas, etc.; water depth point layers, seawater coverage area layers, ocean current layers and chart quality layers are extracted.

[0052] Step 2.2: Calculate the minimum safe water depth and determine the grid resolution based on the hydrodynamic characteristics of the unmanned boat and the size of the nautical chart.

[0053] By establishing a hydrodynamic model or conducting tank experiments, the characteristics of the unmanned boat's navigation posture changes under different hydrodynamic conditions are analyzed and calculated, and the navigation posture changes in the actual ocean environment are estimated. The hydrodynamic parameters include but are not limited to the maximum downward swing value, maximum pitch angle, average draft, and minimum turning radius.

[0054] according to Calculate the minimum safe water depth,

[0055] Among them, S min is the minimum safe water depth, z max is the maximum amplitude of the downward heave motion generated by the unmanned boat sailing in irregular waves at different speeds; L is the length of the unmanned boat; θ max is the maximum amplitude of the pitching motion produced by the unmanned vehicle sailing in irregular waves under different working conditions; is the average draft of the unmanned boat; e enc is the water depth error of the electronic nautical chart.

[0056] The resolution of the electronic chart is determined by comprehensively considering the hydrodynamic characteristic parameters and the size of the electronic chart. The resolution of the grid is determined according to the hydrodynamic characteristic parameters of the unmanned boat and the size of the chart: according to the minimum turning radius R min , chart positioning error e p, the length of the unmanned boat L and the resolution of the electronic chart range calculation grid, and the grid size should be selected so that the number of grids is not greater than 10 8 , and the grid size should be no less than 3L+R min +e p , no more than 10L+4R min +2e p .

[0057] Step 2.3: Based on the water depth data and obstacle data of the navigation area chart obtained in step 2.1 and the resolution of the grid obtained in step 2.2, an interpolation algorithm is used to interpolate the water depth to obtain a gridded predicted water depth.

[0058] The interpolation algorithm can adopt the spline function interpolation algorithm with obstacles. The obstacle data obtained in step 2.1 is used as the obstacle input element, and the water depth data is used as the interpolation input data. The smoothing coefficient in the spline interpolation function is set, and the sparse water depth points are interpolated to obtain the rasterized predicted water depth.

[0059] Step 2.4: Based on the mission start time and navigation speed set in step 1, obtain the hydrological, meteorological and tidal information of the mission waters through forecast data.

[0060] Step 2.5: Based on the grid resolution obtained in step 2.2, process the hydrological, meteorological, and tidal information of the mission waters obtained in step 2.4, and use the interpolation method based on step 2.3 to obtain the dynamic water depth distribution and ocean current distribution.

[0061] Cubic spline interpolation is used to interpolate the tide level data from the tide table to obtain the function of the tide level changing over time. The instantaneous water depth of the grid at a certain moment can be obtained by superimposing the static water depth and tide level of the grid point. The instantaneous water depth is:

[0062] S(x,y,t)=D(x,y)+T(x,y,t)+Δ(x,y,t)

[0063] Where S(x,y,t) is the instantaneous water depth value of the (x,y) grid at time t; D(x,y) is the static water depth value of the grid point; T(x,y,t) is the tide level value of the grid point at time t; Δ(x,y,t) is the water depth change caused by other factors affecting the water level change at time t at the grid point; the sum of T(x,y,t) and Δ(x,y,t) constitutes the dynamic water depth term in the instantaneous water depth.

[0064] Step 2.6: Construct a global environmental model based on the minimum safe water depth calculated in step 2.2 and the dynamic water depth distribution and ocean current distribution obtained in step 2.5.

[0065] Step 3: Use the LT-D*Lite algorithm to perform global path planning.

[0066]

[0067] Among them, g(N i ) is the i-th node N i The actual distance cost to the starting node S; h(N i ) is node N i The estimated distance cost to the target node G; is the sum of the water depth hazards from the starting node to the current node; α is a parameter that controls the impact of water depth risk and is set according to the path planning target preference; is the sum of the hazard levels of water obstacles from the starting node to the current node; β is the parameter that controls the risk impact of water obstacles and is set according to the path planning target preference.

[0068] Step 4: Based on the dynamic target information input by the perception system of the unmanned boat, a local situation map is established based on the speed position and speed information of the unmanned boat.

[0069] Step 4.1: Based on the global path planning obtained in step 3 and the target information input by the perception system of the unmanned boat, a local situation map is established.

[0070] The specific implementation method of this step is: determine the update situation area according to the sensor detection range, and form the grids in the area into a local search space.

[0071] Step 4.2: Predict the trajectory of the dynamic obstacle based on the target information input by the perception system of the unmanned boat.

[0072] This step is implemented by predicting the trajectory of a dynamic target based on the target velocity information provided by the perception system, and obtaining a grid cost value that varies over time within the local area. If a collision risk exists for the same grid at the same time, the collision cost value for that grid at that moment is set to infinity.

[0073] Step 4.3: Determine whether there is a collision risk along the current path based on the predicted trajectory of step 4.2. If there is a risk, proceed to step 4.4; otherwise, proceed to step 5.

[0074] Grid water collision risk:

[0075]

[0076] Where d is the node N i To obstacle node O j Euclidean distance, a is the risk factor corresponding to different types of obstacles, v Ni is grid N i The speed of the ocean current c Ni is the direction coefficient, is the direction of the current relative to true north, φ g ∈[0,2π) is the node N i With node O j The azimuth of the connecting line relative to the true north. When there are k obstacle grids near a navigable node, the maximum value is selected as the surface obstacle hazard of the node: r S (N i )=max{r S [N i ,1],r S [N i ,2],...,r S [N i ,k]}.

[0077] Step 4.4: Start re-planning, update the grid in the area according to the predicted trajectory information and dynamic water depth, generate a local path, and return to step 4.1 to update the local situation map.

[0078] Step 5: Determine whether the current position of the unmanned boat is the end point. If it reaches the end point, the planning ends. Otherwise, repeat step 4 until the boat reaches the end point.

[0079] It should be emphasized that the embodiments described in the present invention are illustrative rather than restrictive. Therefore, the present invention includes but is not limited to the embodiments described in the specific embodiments. Any other embodiments derived by those skilled in the art based on the technical solutions of the present invention also fall within the scope of protection of the present invention.

Claims

1. A method for planning a dynamic and safe trajectory for an unmanned vehicle considering dynamic water depth, characterized by: The following steps are involved: Step 1: Set the parameters related to the unmanned boat planning task; Step 2: Build a global environment model based on the parameters of step 1; Step 2.1: Based on the coordinates of the starting and ending points of the unmanned boat in step 1, extract the pre-processed vector electronic nautical chart from the chart library, and then extract the electronic nautical chart attribute data, navigation area chart water depth data, current data and obstacle data, and chart positioning error from the vector chart, and perform corresponding format conversion and attribute data processing; Step 2.2: Calculate the minimum safe water depth and determine the grid resolution based on the hydrodynamic characteristics of the unmanned boat and the size of the nautical chart. Step 2.3: Based on the navigation area chart water depth data and obstacle data obtained in step 2.1 and the resolution of the grid obtained in step 2.2, an interpolation algorithm is used to interpolate the water depth to obtain a gridded predicted water depth; Step 2.4: Based on the mission start time and navigation speed set in step 1, obtain hydrological, meteorological, and tidal information of the mission waters through forecast data; Step 2.5: Based on the grid resolution obtained in step 2.2, process the hydrological, meteorological, and tidal information of the mission waters obtained in step 2.4, and use the interpolation method based on step 2.3 to obtain the dynamic water depth distribution and ocean current distribution; Step 2.6: Construct a global environmental model based on the minimum safe water depth calculated in step 2.2 and the dynamic water depth distribution and ocean current distribution obtained in step 2.5; Step 3: Use the LT-D*Lite algorithm to perform global path planning; Among them, g(N i ) is the i-th node N i The actual distance cost to the starting node S; h(N i ) is node N i The estimated distance cost to the target node G; is the sum of the water depth hazards from the starting node to the current node; α is a parameter that controls the impact of water depth risk and is set according to the path planning target preference; is the sum of the risk levels of water obstacles from the starting node to the current node; β is a parameter that controls the risk impact of water obstacles and is set according to the path planning target preference; Step 4: Based on the dynamic target information input by the perception system of the unmanned boat, a local situation map is established based on the speed position and speed information of the unmanned boat; Step 4.1: Create a local situation map based on the global path planning obtained in step 3 and the target information input by the UAV's perception system; Step 4.2: Predict the trajectory of the dynamic obstacle based on the target information input by the perception system of the unmanned boat; Step 4.3: Determine whether there is a collision risk along the current path based on the predicted trajectory in step 4.

2. If there is a risk, proceed to step 4.4; otherwise, proceed to step 5. Step 4.4: Start re-planning, update the grid in the area based on the predicted trajectory information and dynamic water depth, generate a local path, and return to step 4.1 to update the local situation map; Step 5: Determine whether the current position of the unmanned boat is the end point. If it reaches the end point, the planning ends. Otherwise, repeat step 4 until the boat reaches the end point.

2. The method for planning a dynamic safety trajectory of an unmanned vehicle considering dynamic water depth according to claim 1, characterized in that: The specific implementation method of step 1 is: the mission-related parameters include: the size, draft, operating performance, starting position, end position, set mission start time and navigation speed of the unmanned boat.

3. The method for planning a dynamic safety trajectory of an unmanned vehicle considering dynamic water depth according to claim 1, characterized in that: The specific implementation method of step 2.2 is: by establishing a hydrodynamic model or conducting a tank experiment, analyzing and calculating the characteristics of the navigation attitude change of the unmanned vehicle under different hydrodynamic conditions, and estimating the navigation attitude change in the actual marine environment: according to Calculate the minimum safe water depth, Among them, S min is the minimum safe water depth, z max is the maximum amplitude of the downward heave motion generated by the unmanned boat sailing in irregular waves at different speeds; L is the length of the unmanned boat; θ max is the maximum amplitude of the pitching motion produced by the unmanned vehicle sailing in irregular waves under different working conditions; is the average draft of the unmanned boat; e enc is the water depth error of the electronic chart; The grid resolution is determined based on the hydrodynamic characteristics of the unmanned boat and the size of the chart: min , chart positioning error e p , the length of the unmanned boat L and the resolution of the electronic chart range calculation grid, and the grid size should be selected so that the number of grids is not greater than 10 8 , and the grid size should be no less than 3L+R min +e p , no more than 10L+4R min +2e p .

4. The method for planning a dynamic safety trajectory of an unmanned vehicle considering dynamic water depth according to claim 1, characterized in that: The specific implementation method of step 2.3 is: using the spline function interpolation algorithm with obstacles, taking the obstacle data obtained in step 2.1 as the obstacle input element, taking the water depth data as the interpolation input data, setting the smoothing coefficient in the spline interpolation function, interpolating the sparse water depth points, and obtaining the rasterized predicted water depth.

5. The method for planning a dynamic safety trajectory of an unmanned vehicle considering dynamic water depth according to claim 1, characterized in that: The specific implementation method of step 2.5 is: the instantaneous water depth is: S(x,y,t)=D(x,y)+T(x,y,t)+Δ(x,y,t) Where S(x,y,t) is the instantaneous water depth value of the (x,y) grid at time t; D(x,y) is the static water depth value of the grid; T(x,y,t) is the tidal level value of the grid at time t; Δ(x,y,t) is the water depth change caused by other factors affecting the water level change of the grid at time t. The sum of T(x,y,t) and Δ(x,y,t) constitutes the dynamic water depth term in the instantaneous water depth.

6. The method for planning a dynamic safety trajectory of an unmanned vehicle considering dynamic water depth according to claim 1, characterized in that: The perception system in step 4.1 includes navigation radar, lidar, AIS, panoramic camera and depth sounder.

7. The method for planning a dynamic safety trajectory of an unmanned vehicle considering dynamic water depth according to claim 1, characterized in that: The specific implementation method of step 4.3 is: Grid water surface collision risk: Where d is the node N i To obstacle node O j Euclidean distance, a is the risk factor corresponding to different types of obstacles, v Ni is grid N i The speed of the ocean current c Ni is the direction coefficient, φ c ∈[0,2π) is the direction of the ocean current relative to true north, φ g ∈[0,2π) is the node N i With node O j The azimuth of the connecting line relative to the true north. When there are k obstacle grids near a navigable node, the maximum value is selected as the surface obstacle hazard of the node: r S (N i )=max{r S [N i ,1],r S [N i ,2],...,r S [N i ,k]}。

Images