A method for controlling boat navigation

Abstract

The present invention relates to the field of ship navigation control, and specifically to a ship navigation control method. The energy consumption of the ship under different terrain and water flow conditions, the straight-line distance from the node to the target, and the environmental risk cost are comprehensively considered. In a complex underwater environment, the advantages and disadvantages of the path can be evaluated more comprehensively, thereby searching for a globally optimal path, improving the navigation efficiency of the ship, and reducing unnecessary navigation distance and energy consumption. This is to achieve the search for the global optimal path. Candidate paths are generated according to the real-time status of the ship and the surrounding environment, and are screened through a scientific comprehensive evaluation function. During the navigation process, in the face of environmental changes, the path can be quickly adjusted to ensure that the ship always moves along a safe and efficient path. The navigation path of the ship can be adjusted in real time in a complex and changeable underwater environment to ensure the smooth execution of the task. It is particularly suitable for navigation control of unmanned boats.

Description

Technical Field

[0001] The present invention relates to the field of boat navigation control, and in particular to a boat navigation control method. Background Art

[0002] With the continuous advancement of technology, boats, especially unmanned boats, have been increasingly used in fields such as ocean exploration and environmental monitoring. In complex water environments, boats need to be able to autonomously plan safe and efficient navigation paths to complete various tasks. However, existing boat navigation control technology still faces many challenges, such as:

[0003] The inability to effectively respond to real-time changes in water currents and sudden obstacles results in the planned path being at risk of collision or experiencing a surge in energy loss during actual navigation.

[0004] The optimization objective is singular, ignoring the impact of multiple factors. For example, the shortest path may pass through a strong countercurrent zone, increasing total energy consumption; a low-risk path may cause mission timeouts due to excessive detours; and the dynamic relationship between endurance and terrain resistance is not considered, which can easily lead to energy crises.

[0005] Traditional replanning methods lack dynamic responsiveness and rely on periodic global searches, which are computationally intensive and result in delayed responses. When encountering sudden changes in rapids or moving obstacles, the vehicle may be trapped in localized danger zones due to delayed path updates, leading to mission failure in severe cases. Summary of the Invention

[0006] In order to overcome the above-mentioned deficiencies of the prior art, the present invention provides a boat navigation control method that can more comprehensively evaluate the pros and cons of the path, adjust the boat's navigation path in real time, and ensure the smooth execution of the mission. It is particularly suitable for the navigation control of unmanned boats.

[0007] In order to achieve the above object, the present invention is implemented through the following technical solutions:

[0008] A method for controlling a boat's navigation is characterized by comprising the following steps:

[0009] A. Based on the underwater digital twin model, confirm the preliminary path, including the following steps a1-a5:

[0010] a1. Enter the target point, initialize the open list and closed list, and add the boat's current position as the starting node to the open list;

[0011] a2. Select the node with the lowest total cost in the open list as the extension point to expand the adjacent node m;

[0012] a3. Determine whether the adjacent node m is in the open list and the closed list. If not, add the adjacent node m to the open list and record the current extension point as the parent node of the adjacent node m. If so, proceed to the next step.

[0013] a4. Determine whether the theoretical energy consumption of the new path through the extension point to the adjacent node m is less than the theoretical energy consumption of the old path backtracking to the adjacent node m's existing parent node. If so, update the parent node of the adjacent node m to the extension point; if not, move the extension point to the closed list;

[0014] a5. Repeat steps a2-a4 until the target point is reached, and trace back from the target point to the parent node to form the initial path;

[0015] The total cost value includes at least the sum of the theoretical energy consumption, distance cost value and environmental risk cost value to the corresponding node. The distance cost value is the straight-line distance from the corresponding node to the target point. The environmental risk cost value is determined by the obstacle density and terrain complexity of the area where the node is located.

[0016] B. Path adjustment, including the following steps b1-b4:

[0017] b1. As the vessel navigates along the initial path, a dynamic window is formed based on the vessel's current position, motion state, and pre-set motion constraints. The dynamic window generates several combinations of velocity and angular velocity (v, ω) for a future time interval Δt. Each combination represents a possible motion trajectory. b2. For each velocity and angular velocity combination within the dynamic window, the vessel's position and attitude after Δt are calculated using a kinematic model to form a candidate path.

[0018] b3. Evaluate each candidate path based on a fusion model generated by fusing the virtual model of the vessel and the underwater digital twin model;

[0019] b4. Select the candidate path with the best evaluation value as the real-time navigation path, and send the corresponding speed and angular velocity instructions to the control system of the boat.

[0020] It should be noted that the path formed by tracing back to the parent node of a node is the path formed by connecting the current node n, the parent node n-1 of the node, the parent node n-2 of the parent node, and so on to the starting node. The new path passing through the extension point is the new path formed by tracing back to the corresponding parent node of the extension point.

[0021] Furthermore, in a method for controlling navigation of a boat in the present application, the theoretical energy consumption to node n is g(n)=g(n-1)+E*ΔT, where node n-1 is the parent node of node n or node n is the adjacent node of extension point n-1, E is the energy consumption per unit time, ΔT is the movement time interval, and E=0.5ρSC d(v n -v w cosθ) 2 v n , where ρ is the seawater density, S is the windward area of ​​the boat, and C d is the drag coefficient, v n is the speed of the boat at node n, v w is the water velocity, and θ is the angle between the water direction and the boat's sailing direction.

[0022] Furthermore, in a method for controlling navigation of a vessel in the present application, the environmental risk cost value r=w1d+w2k*, w1 and w2 are preset weight coefficients, d is the obstacle density, d is calculated by counting the proportion of the obstacle area within a preset radius around the node in the underwater digital twin model, and k* is a dimensionless quantity obtained by normalizing the curvature k of the terrain in the area where the node is located to characterize the complexity of the terrain. Specifically, the minimum and maximum values ​​of the curvature in the task area are k respectively. min , k max ; then K*=(kk min ) / (k max -k min ), K*∈[0,1], w1+w2=1.

[0023] Furthermore, in a boat navigation control method in the present application, in step b2, the equation corresponding to the kinematic model is:

[0024] x n =x0+vΔtcosθ0,y n =y0+vΔtsinθ0,θ n =θ0+ωΔt; (x0, y0), θ0 are the position coordinates and heading angle of the boat at the current moment, (x n ,y n ),θ n are the position coordinates of the vessel (i.e., the candidate path endpoint) and the heading angle after the current moment Δt, respectively.

[0025] Furthermore, in a method for controlling navigation of a vessel in the present application, the evaluation index of step b3 includes at least safety, target direction consistency and speed utilization; wherein safety is evaluated by calculating the minimum distance d between the candidate path and the surrounding obstacles. min Evaluation, the target direction consistency is evaluated by calculating the angle α between the line connecting the candidate path end point and the target point and the current heading angle. The speed utilization is obtained by comparing the boat speed with the preset maximum speed. The calculation function of the evaluation index is S=w3α*+w4d min / d max +w5v / v m , where w3, w4, and w5 are preset weight coefficients, dmax is the preset maximum sensing distance, v m is the maximum navigation speed, α*=(cosα+1) / 2. Among them, w3, w4, and w5 are preset weight coefficients, which can be dynamically adjusted according to mission requirements. For example, w4 can be increased in areas with dense obstacles, and w5 can be increased in scenarios where rapid arrival is required. m is the maximum sailing speed. The route is optimized by balancing safety, efficiency, and goal orientation.

[0026] Furthermore, in a boat navigation control method in the present application, during the boat navigation process, the boat's sensors continuously sense the surrounding environment. If a new obstacle or environmental change is detected, the dynamic window is immediately regenerated, and the parameters corresponding to the evaluation index are updated according to the actual detected obstacle or environment. Steps b3 and b4 are repeated to achieve real-time adjustment of the path.

[0027] Traditional vessel navigation control methods are mostly based on simple environmental models. For example, early algorithms considered only the water surface, simplifying the underwater environment into a flat surface and ignoring the complex underwater topography and obstacle distribution. In shallow waters, there are numerous obstacles such as reefs and sunken ships. If a vessel follows a path planned by such a simple model, a collision is highly likely, resulting in mission failure or even damage to the vessel.

[0028] While some navigation control methods based on electronic nautical charts can provide general information about the seafloor topography, chart data is often not updated in a timely manner. The ocean environment is dynamic, and the seafloor topography may change due to factors such as earthquakes and currents. Electronic nautical charts cannot reflect these changes in real time. Furthermore, the accuracy of electronic nautical charts is limited, making it difficult to accurately mark small underwater obstacles, such as small rocks within a reef system. This poses a potential risk when approaching these areas.

[0029] Other navigation control methods rely on sensors to detect the environment in real time, but these methods are limited in detection range and accuracy. The detection range of common sonar sensors is affected by factors such as water turbidity and salinity. In turbid estuaries, the effective detection range of sonar can be significantly reduced, making it impossible to plan a safe path long enough for the vessel in advance. Furthermore, sensors are prone to misjudgment when detecting complex underwater structures, such as mistaking a school of swimming fish for an obstacle. This can lead to unnecessary route adjustments and reduce navigation efficiency.

[0030] Therefore, when faced with complex and changing underwater environments, it is difficult to meet the needs of safe and efficient navigation of boats.

[0031] Furthermore, a method for controlling a vessel navigation in the present application includes a process for constructing an underwater digital twin model, comprising the following steps c1-c4:

[0032] c1. Use a multi-beam echo sounder to collect seabed topography data and obtain depth profile data along the track to construct a preliminary outline of the seabed topography;

[0033] c2. Use side-scan sonar to detect underwater obstacles and targets, and obtain information such as the distance, shape, and reflection characteristics of underwater objects;

[0034] c3. De-noise the information obtained in steps c1 and c2 and fuse the de-noised information to construct an underwater digital twin model;

[0035] c4. Build a time-series-based state-space model. When new sensor data arrives, use the extended Kalman filter algorithm to update the underwater digital twin model state. Through continuous iteration, the underwater digital twin model accurately reflects changes in the actual underwater environment in real time.

[0036] Furthermore, a method for controlling a boat navigation in the present application includes the following steps: creating a virtual boat model in the underwater digital twin model, inputting parameters of the virtual boat model, and converting the parameters of the boat in its own coordinate system into the global coordinate system of the underwater digital twin model through a transformation matrix, wherein the parameters of the virtual boat model include the boat's outer contour dimensions and the boat's water depth h;

[0037] The boat's water depth h=(P-P0) / (ρg), where P is the seawater pressure and ρ is the seawater density, which changes with the temperature and salinity of the seawater. The formula ρ=ρ0(1-γ(T s -T0)+δ(S-S0)), ρ0 is the density of seawater under standard conditions, T s is the seawater temperature, T0 is the standard seawater temperature, S is the salinity, S0 is the standard salinity, γ and δ are preset constants, g is the acceleration of gravity, and P0 is the atmospheric pressure. To improve the measurement accuracy, the temperature compensation technology is used to correct the measurement error of the seawater pressure P caused by the change of temperature T. The seawater pressure measurement deviation caused by temperature is ΔVT=k1(TT r )+k2(TT r ) 2 , where k1 and k2 are preset coefficients, T r During data processing, the pressure sensor output is corrected according to the real-time measured temperature to obtain a more accurate pressure value, thereby calculating the precise water depth.

[0038] Furthermore, a method for controlling a boat's navigation in this application is provided.

[0039] In step c1, the measurement depth d of the corresponding beam i =0.5VΔt i, where the speed of sound waves in seawater is V=1449.2+4.6T s -0.055T s 2 +0.00029T s 3 +(1.34-0.01T s )(S-35)+0.016P,T s , S, and P are the temperature, salinity, and pressure of seawater, respectively. i represents the time difference between the transmission and reception of the i-th beam;

[0040] In step c2, information such as the distance, shape, and reflection characteristics of underwater objects is calculated using the sound propagation loss model. The specific equation is as follows:

[0041] 10log 10 (I0 / I)=20log 10 (r)+Ar,

[0042] The transmitted sound intensity is I0, the seawater absorption coefficient is A, the received sound intensity is I, and the sound wave propagation distance is r;

[0043] By measuring the changes in sound intensity at different locations and combining it with the sound propagation loss model, information such as the distance, shape, and reflection characteristics of underwater objects can be inverted. The image data generated by the side-scan sonar contains rich texture and grayscale features, which are used for subsequent target recognition and modeling.

[0044] In step c3, the depth data collected by the multi-beam echo sounder is subject to interference from various factors. Adaptive Kalman filtering is used to improve its accuracy and stability. The process of preprocessing the information obtained in step c1 is as follows:

[0045] According to the system state equation, the state estimation value at the previous moment is updated in time to obtain the prior estimation value;

[0046] Using the observation equation combined with R k Calculate the Kalman gain K k , and then according to K k Correct the prior estimate to obtain a more accurate state estimate at the current moment;

[0047] Through iteration, the accuracy and stability of depth data are continuously improved;

[0048] Among them, the system state equation is X k =F k X k-1 +B k u k +w k , where X kis the state vector at time k, which contains information such as depth and related change rate. The depth-related change rate includes not only the change rate of depth over time, but also the movement state of the multi-beam echo sounder itself and the depth measurement change rate caused by ocean environmental factors. k B is the state transfer matrix, which describes the change of state over time; k is the control input matrix; u k is the control input vector; w k is the process noise, which obeys the normal distribution N(0,Q k ), Q k is the process noise covariance matrix, which reflects the uncertainty of the system model itself and has an important impact on the update of the state estimate. k When it is larger, the algorithm relies more on new observation data to update the state estimate, and vice versa, it trusts the previous state estimate more;

[0049] Among them, the observation equation is Z k =H k X k +v k , where Z k is the observation vector, i.e. the measured depth value; H k is the observation matrix, which determines the relationship between the measurement value and the state vector; v k is the observation noise, which obeys the normal distribution N(0,R k ), R k is the observation noise covariance matrix, which is used to measure the noise level of the observation data, R k When it is smaller, the credibility of the observation data is high, and the algorithm will pay more attention to the observation data Z when updating the state estimate. k ; R k When it is larger, the opposite is true;

[0050] The denoised depth data is subjected to a tetrahedral meshing algorithm based on the Delaunay triangulation principle to generate a preliminary three-dimensional mesh model of the seabed topography. When meshing, a larger initial mesh side length l0 is set for areas with flat terrain. For areas with complex terrain, when it is detected that the terrain curvature K is greater than the preset upper limit, the mesh side length is reduced for local encryption. The reduced mesh side length l=l0*exp(-βK), where β is a preset constant.

[0051] Furthermore, in a method for controlling a boat navigation in the present application, in order to achieve precise fusion, the process of converting the parameters of the boat in its own coordinate system into the global coordinate system of the underwater digital twin model through a transformation matrix is ​​as follows: a transformation matrix T is defined to convert the parameters of the boat in its own coordinate system into the global coordinate system of the underwater digital twin model, and the position vector of the boat in its own coordinate system is set as P local =[xlocal ,y local ,z local ] T The position vector in the global coordinate system is P glocal =[x glocal ,y glocal ,z glocal ] T , then P glocal =TP local , where z local 、z glocal Related to the water depth; the transformation matrix T contains rotation and translation information, which is calculated by the heading angle θ, roll angle φ, pitch angle ψ of the boat and the initial position (x0, y0, z0) in the global coordinate system. The rotation matrix R is expressed by Euler angles as follows:

[0052] ,

[0053] Translation vector t=[x0,y0,z0] T ,but ;

[0054] As the boat sails, the boat’s status in the underwater digital twin model is updated in real time.

[0055] It can be seen from the above technical solutions that the beneficial effects of the boat navigation control method of the present application include:

[0056] Globally optimal path search: This system comprehensively considers the vessel's energy consumption, the straight-line distance from a node to a target, and environmental risk costs under varying terrain and current conditions. In complex underwater environments, it enables a more comprehensive assessment of path quality, thereby finding the optimal global path, improving vessel navigation efficiency and reducing unnecessary distance and energy consumption.

[0057] Real-time dynamic path adjustment: A dynamic window method generates candidate paths based on the vessel's real-time status and surrounding environment, and screens them using a scientific, comprehensive evaluation function. During navigation, the path can be rapidly adjusted to accommodate environmental changes, ensuring the vessel maintains a safe and efficient path. The system can adjust the vessel's navigation path in real time in complex and changing underwater environments, ensuring smooth mission execution.

[0058] Precise parameter measurement and integration: Pressure sensors combined with temperature compensation technology are used to measure water depth, while laser ranging technology combined with a data filtering algorithm measures vessel dimensions, ensuring accurate parameter measurement. These parameters are integrated into the underwater digital twin model through a carefully constructed transformation matrix, ensuring that the virtual vessel's state in the model accurately corresponds to the actual vessel. This provides a data foundation for path planning that is highly consistent with the vessel's actual state, avoiding path planning errors caused by parameter errors.

[0059] Intuitive Safety Assessment: Real-time updates of the vessel's status within the digital twin model provide a visual representation of the spatial relationship between the vessel and the surrounding underwater environment. Operators or automated decision-making systems can clearly see information such as the distance and relative position of the vessel to obstacles, enabling timely assessment of navigation safety and proactive adjustments to ensure safe navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 This is a flowchart for confirming the preliminary path in the embodiment of the present application. DETAILED DESCRIPTION

[0061] Existing ship navigation control technologies face many challenges, such as:

[0062] The inability to effectively respond to real-time changes in water currents and sudden obstacles results in the planned path being at risk of collision or experiencing a surge in energy loss during actual navigation.

[0063] The optimization objective is singular, ignoring the impact of multiple factors. For example, the shortest path may pass through a strong countercurrent zone, increasing total energy consumption; a low-risk path may cause mission timeouts due to excessive detours; and the dynamic relationship between endurance and terrain resistance is not considered, which can easily lead to energy crises.

[0064] Traditional replanning methods lack dynamic responsiveness and rely on periodic global searches, which are computationally intensive and result in delayed responses. When encountering sudden changes in rapids or moving obstacles, the vehicle may be trapped in localized danger zones due to delayed path updates, leading to mission failure in severe cases.

[0065] In this regard, this embodiment provides a method for controlling a boat's navigation, comprising the following steps:

[0066] A. Based on the underwater digital twin model, confirm the preliminary path, including the following steps a1-a5:

[0067] a1. Enter the target point, initialize the open list and closed list, and add the current position of the boat as the starting node to the open list; set the weight coefficients, such as w1=0.6 and w2=0.4. These coefficients can be adjusted according to the actual water conditions and the boat's mission requirements.

[0068] a2. Select the node with the lowest total cost in the open list as the extension point to expand the adjacent node m;

[0069] a3. Determine whether the adjacent node m is in the open list and the closed list. If not, add the adjacent node m to the open list and record the current extension point as the parent node of the adjacent node m. If so, proceed to the next step.

[0070] a4. Determine whether the theoretical energy consumption of the new path through the extension point to the adjacent node m is less than the theoretical energy consumption of the old path backtracking to the adjacent node m's existing parent node. If so, update the parent node of the adjacent node m to the extension point; if not, move the extension point to the closed list;

[0071] a5. Repeat steps a2-a4 until the target point is reached, and trace back from the target point to the parent node to form the initial path;

[0072] The total cost includes at least the sum of the theoretical energy consumption, distance cost, and environmental risk cost to the corresponding node. The distance cost is the straight-line distance from the corresponding node to the target point, which can be calculated by determining the coordinates of the two points. The environmental risk cost is determined by the obstacle density and terrain complexity of the area where the node is located.

[0073] Path adjustment includes the following steps b1-b4:

[0074] b1. While the vessel is navigating along the initial path, a plurality of velocity and angular velocity combinations v, ω are generated at a preset frequency based on the current position, motion state, and preset motion constraints. Specifically, the preset motion constraints include threshold intervals for acceleration and angular velocity. These combinations are then used to form dynamic windows. Each combination represents a possible motion trajectory. Specifically, the dynamic window operates at a preset frequency, such as 10 Hz.

[0075] b2. For each velocity and angular velocity combination within the dynamic window, the position and attitude of the vessel after Δt time are calculated using the kinematic model to form a candidate path;

[0076] b3. Evaluate each candidate path based on a fusion model generated by fusing the virtual model of the vessel and the underwater digital twin model;

[0077] b4. Select the candidate path with the best (i.e., largest) evaluation value as the real-time navigation path, and send the corresponding speed and angular velocity instructions to the control system of the boat.

[0078] The beneficial effects of a boat navigation control method in this embodiment include:

[0079] Globally optimal path search: This system comprehensively considers the vessel's energy consumption, the straight-line distance from a node to a target, and environmental risk costs under varying terrain and current conditions. In complex underwater environments, it enables a more comprehensive assessment of path quality, thereby finding the optimal global path, improving vessel navigation efficiency and reducing unnecessary distance and energy consumption.

[0080] Real-time dynamic path adjustment: A dynamic window method generates candidate paths based on the vessel's real-time status and surrounding environment, and screens them using a scientific, comprehensive evaluation function. During navigation, the path can be rapidly adjusted in response to environmental changes, ensuring the vessel always follows a safe and efficient path. The system can adjust the vessel's navigation path in real time in complex and changing underwater environments to ensure smooth mission execution. For example, if a new obstacle suddenly appears, the path can be replanned in a short period of time to avoid collisions, significantly enhancing the vessel's autonomous navigation capabilities in complex and changing waters.

[0081] It should be noted that the path formed by tracing back to the parent node of a node is the path formed by connecting the current node n, the parent node n-1 of the node, the parent node n-2 of the parent node, and so on to the starting node. The new path passing through the extension point is the new path formed by tracing back to the corresponding parent node of the extension point.

[0082] Furthermore, in this embodiment, the theoretical energy consumption to node n is g(n)=g(n-1)+E*ΔT, where node n-1 is the parent node of node n or node n is the adjacent node of extension point n-1, E is the energy consumption per unit time, ΔT is the movement time interval, and E=0.5ρSC d (v n -v w cosθ) 2 v n , where ρ is the seawater density, S is the windward area of ​​the boat, and C d is the drag coefficient, v n is the speed of the boat at node n, v w is the water velocity, and θ is the angle between the water direction and the boat's sailing direction.

[0083] The relevant parameters can be adjusted according to the actual water conditions and the boat mission requirements to determine the boat's motion parameters under different terrain and water conditions, such as v n , acceleration a n The water velocity can be measured by a current meter installed on a boat or estimated based on historical hydrological data combined with the current location.

[0084] Furthermore, in this embodiment, the environmental risk cost value r=w1d+w2k*, w1 and w2 are preset weight coefficients, d is the obstacle density, d is calculated by counting the proportion of the obstacle area within a preset radius around the node in the underwater digital twin model, and k* is the dimensionless quantity obtained by normalizing the curvature k of the terrain in the area where the node is located to characterize the complexity of the terrain. Specifically, the minimum and maximum values ​​of the curvature in the task area are k respectively. min , k max Then: K*=(kk min ) / (k max-k min ), K*∈[0,1], w1+w2=1. The quadratic surface z=ax is used in the local depth grid (5×5) where the node is located. 2 +by 2 +cxy+dx+ey+f fits the seabed topography, where a, b, c, d, e, and f are the parameters of the quadratic surface z, and Gaussian curvature is the curvature of the terrain =(4ab-c 2 ) / (1+d 2 +e 2 ) 2 .

[0085] Furthermore, in this embodiment, in step b2, the equation corresponding to the kinematic model is:

[0086] x n =x0+vΔtcosθ0,y n =y0+vΔtsinθ0,θ n =θ0+ωΔt; (x0, y0), θ0 are the position coordinates and heading angle of the boat at the current moment, (x n ,y n ),θ n They are the position coordinates of the vessel after the current moment Δt, i.e., the candidate path end point and the heading angle.

[0087] Furthermore, in this embodiment, the evaluation index of step b3 includes at least safety, target direction consistency and speed utilization; wherein safety is evaluated by calculating the minimum distance d between the candidate path and the surrounding obstacles. min Evaluation, the target direction consistency is evaluated by calculating the angle α between the line connecting the candidate path end point and the target point and the current heading angle. The speed utilization is obtained by comparing the boat speed with the preset maximum speed. The calculation function of the evaluation index is S=w3α*+w4d min / d max +w5v / v m , where w3, w4, and w5 are preset weight coefficients, d max is the preset maximum sensing distance, v m is the maximum navigation speed, α*=(cosα+1) / 2. w3+w4+w5=1, w3, w4, w5 can be dynamically adjusted according to mission requirements, such as increasing w4 in areas with dense obstacles and increasing w5 in scenarios that require rapid arrival. mis the maximum navigation speed. The path is optimized and adjusted by balancing safety, efficiency, and goal orientation. In one embodiment, to avoid frequent path fluctuations, the evaluation metric also includes a historical path weight, ensuring that the new path maintains a certain degree of continuity with the historical path. Simultaneously, the selected path is smoothed using Bezier curves to ensure the smoothness of the vessel's navigation trajectory. If there are multiple target points, such as multiple stations in an inspection mission, the current target point is dynamically updated during execution to achieve continuous navigation at multiple stations.

[0088] Furthermore, in this embodiment, during the navigation of the boat, the boat's sensors such as sonar and cameras continuously perceive the surrounding environment. If a new obstacle or environmental change is detected, the dynamic window is immediately regenerated, and the parameters corresponding to the evaluation index are updated according to the actual detected obstacle or environment. Steps b3 and b4 are repeated to achieve real-time adjustment of the path.

[0089] Furthermore, traditional vessel navigation control methods are mostly based on simple environmental models. For example, early algorithms considered only the water surface, simplifying the underwater environment into a flat surface and ignoring the complex underwater topography and obstacle distribution. In shallow waters, there are numerous obstacles such as reefs and sunken ships. If a vessel follows a path planned by such a simple model, a collision is highly likely, resulting in mission failure or even damage to the vessel.

[0090] While some navigation control methods based on electronic nautical charts can provide general information about the seafloor topography, chart data is often not updated in a timely manner. The ocean environment is dynamic, and the seafloor topography may change due to factors such as earthquakes and currents. Electronic nautical charts cannot reflect these changes in real time. Furthermore, the accuracy of electronic nautical charts is limited, making it difficult to accurately mark small underwater obstacles, such as small rocks within a reef system. This poses a potential risk when approaching these areas.

[0091] Other navigation control methods rely on sensors to detect the environment in real time, but these methods are limited in detection range and accuracy. The detection range of common sonar sensors is affected by factors such as water turbidity and salinity. In turbid estuaries, the effective detection range of sonar can be significantly reduced, making it impossible to plan a safe path long enough for the vessel in advance. Furthermore, sensors are prone to misjudgment when detecting complex underwater structures, such as mistaking a school of swimming fish for an obstacle. This can lead to unnecessary route adjustments and reduce navigation efficiency.

[0092] Therefore, when faced with complex and changing underwater environments, it is difficult to meet the needs of safe and efficient navigation of boats.

[0093] Furthermore, in this embodiment, a multibeam depth sounder is securely mounted in a suitable position on the hull of the vessel, ensuring that its acoustic wave emission direction is vertically downward and covers the area below the vessel's navigation path. Simultaneously, side-scan sonars are symmetrically mounted on both sides of the hull to ensure that the acoustic wave emission fan-shaped area can effectively detect underwater conditions within a certain range on both sides of the hull.

[0094] Specifically, in this embodiment, the process of constructing the underwater digital twin model includes the following steps c1-c4:

[0095] c1. Use a multi-beam echo sounder to collect seabed topography data and obtain depth profile data along the track to construct a preliminary outline of the seabed topography;

[0096] c2. Use side-scan sonar to detect underwater obstacles and targets, and obtain information such as the distance, shape, and reflection characteristics of underwater objects;

[0097] c3. De-noise the information obtained in steps c1 and c2 and fuse the de-noised information to construct an underwater digital twin model;

[0098] c4. Build a time-series-based state-space model. When new sensor data arrives, use the extended Kalman filter (EKF) algorithm to update the underwater digital twin model state. Through continuous iteration, the underwater digital twin model accurately reflects changes in the actual underwater environment in real time.

[0099] Therefore, the beneficial effects of this application also include:

[0100] High-precision environmental perception: Multi-source sensor data acquisition combined with advanced data processing algorithms enables the constructed underwater digital twin model to extremely accurately reflect the actual underwater environment. Multi-beam echo sounders accurately calculate depth using complex formulas, while side-scan sonars acquire information about underwater objects based on sound propagation loss models. Data preprocessing and fusion significantly improve model accuracy. For example, in complex reef areas, compared to traditional methods, the location and shape of reefs can be more accurately identified, providing vessels with precise environmental information and significantly reducing the risk of collision.

[0101] Real-time dynamic update: The state space model based on time series and the extended Kalman filter algorithm ensure that the underwater digital twin model can track changes in the underwater environment in real time. Whether it is due to natural changes in the seabed topography or new obstacles, the model can be updated in a timely manner. This allows the vessel to always plan its path based on the latest and most accurate environmental model throughout the entire navigation process, enhancing its adaptability to dynamic environments. Therefore, the path planning algorithm of a vessel navigation control method in this application that can accurately perceive the underwater environment and comprehensively consider the vessel's own parameters has important practical significance.

[0102] Furthermore, in this embodiment, the process of generating the fusion model includes: creating a virtual model of the boat in the underwater digital twin model, inputting parameters of the virtual model of the boat, and converting the parameters of the boat in its own coordinate system to the global coordinate system of the underwater digital twin model through a transformation matrix, wherein the parameters of the virtual model of the boat include the outer contour dimensions of the boat and the water depth h of the boat;

[0103] The boat's water depth h=(P-P0) / (ρg), where P is the seawater pressure measured by a pressure sensor installed on the bottom of the boat, and ρ is the seawater density, which changes with the temperature and salinity of the seawater. The formula ρ=ρ0(1-γ(T s -T0)+δ(S-S0)), ρ0 is the density of seawater under standard conditions, T s is the seawater temperature, T0 is the standard seawater temperature, S is the salinity, S0 is the standard salinity, γ and δ are preset constants, g is the acceleration of gravity, and P0 is the atmospheric pressure. To improve the measurement accuracy, the temperature compensation technology is used to correct the measurement error of the seawater pressure P caused by the change of temperature T. The seawater pressure measurement deviation caused by temperature is ΔVT=k1(TT r )+k2(TT r ) 2 , where k1 and k2 are preset coefficients, T r During data processing, the pressure sensor output is corrected according to the real-time measured temperature to obtain a more accurate pressure value, thereby calculating the precise water depth.

[0104] Furthermore, in this embodiment,

[0105] In step c1, the measurement depth d of the corresponding beam i =0.5VΔt i , where the speed of sound waves in seawater is V=1449.2+4.6T s -0.055T s 2 +0.00029T s 3 +(1.34-0.01T s )(S-35)+0.016P,T s , S, and P are the temperature, salinity, and pressure of seawater, respectively. i represents the time difference between the transmission and reception of the i-th beam;

[0106] In step c2, information such as the distance, shape, and reflection characteristics of underwater objects is calculated using the sound propagation loss model. The specific equation is as follows:

[0107] 10log 10 I0 / I=20log 10(r)+Ar,

[0108] The transmitted sound intensity is I0, the seawater absorption coefficient is A, the received sound intensity is I, and the sound wave propagation distance is r;

[0109] By measuring the changes in sound intensity at different locations and combining it with the sound propagation loss model, information such as the distance, shape, and reflection characteristics of underwater objects can be inverted. The image data generated by the side-scan sonar contains rich texture and grayscale features, which are used for subsequent target recognition and modeling.

[0110] In step c3, the depth data collected by the multi-beam echo sounder may be interfered by various factors. Furthermore, in this embodiment, an adaptive Kalman filter is used to improve its accuracy and stability. Specifically, the process of preprocessing the information obtained in step c1 is as follows:

[0111] According to the system state equation, the state estimation value at the previous moment is updated in time to obtain the prior estimation value;

[0112] Using the observation equation combined with R k Calculate the Kalman gain K k , and then according to K k Correct the prior estimate to obtain a more accurate state estimate at the current moment;

[0113] Through iteration, the accuracy and stability of depth data are continuously improved;

[0114] Among them, the system state equation is X k =F k X k-1 +B k u k +w k , where X k is the state vector at time k, which contains information such as depth and related change rate. The depth-related change rate includes not only the change rate of depth over time, but also the motion state of the multi-beam echo sounder itself, such as navigation speed, acceleration, attitude change, etc., as well as the depth measurement change rate caused by ocean environmental factors such as seawater flow and tide. k B is the state transfer matrix, which describes the change of state over time; k To control the input matrix in this scenario, if the influence of local environmental changes caused by the navigation of the boat on the measurement is considered, B k Can be used to describe this relationship; u k is the control input vector; w k is the process noise, which obeys the normal distribution (N0,Q k ), Q k is the process noise covariance matrix, which reflects the uncertainty of the system model itself and has an important impact on the update of the state estimate. kWhen it is larger, the algorithm relies more on new observation data to update the state estimate value. Conversely, it trusts the previous state estimate result more;

[0115] Among them, the observation equation is Z k =H k X k +v k , where Z k is the observation vector, that is, the measured depth value; H k is the observation matrix, which determines the relationship between the measured value and the state vector; v k is the observation noise, which follows the normal distribution N0,R k and R k is the observation noise covariance matrix, which is used to measure the noise level of the observation data. When R k is small, the credibility of the observation data is high, and the algorithm will pay more attention to the observation data Z k when updating the state estimate value; when R k is large, the opposite is true;

[0116] For the depth data after denoising, use the tetrahedral mesh generation algorithm based on the Delaunay triangulation principle to generate a preliminary three-dimensional mesh model of the seabed topography. When dividing the mesh, for the flat terrain area, set a larger initial mesh side length l0, such as 5 meters; for the complex terrain area, such as near the reef group, when it is detected that the terrain curvature K is greater than the preset upper limit, such as 0.05, reduce the mesh side length for local encryption, and the reduced mesh side length l = l0*exp-βK, where β is a preset constant, specifically 0.8 in one embodiment;

[0117] In step c3, the image data generated by the side-scan sonar contains rich texture and gray-scale features, but there is also noise; the process of preprocessing the information obtained in step c2 is as follows: use the denoising method based on wavelet transform to decompose the image into different frequency sub-bands and perform threshold processing on the high-frequency sub-band coefficients. Let the coefficient after wavelet transform be C i,j , the threshold be U, and the processed coefficient be C i,j’ . If 丨C i,j 丨≥U, then C i,j’ =C i,j ; if 丨C i,j 丨<U, then C i,j’ =0; obtain the denoised image through inverse wavelet transform, effectively remove the image noise, and retain the useful target information;

[0118] Furthermore, in this embodiment, a feature matching algorithm is used to fuse the denoised image data of the side-scan sonar with the terrain grid model. Specifically, the feature points in the two data can be extracted based on the scale-invariant feature transform (SIFT) algorithm. By calculating the Euclidean distance between the feature points, matching is performed to establish a unified spatial coordinate system, accurately integrating the contours and position information of underwater objects into the three-dimensional model, and realizing multi-source data fusion. At the same time, a convolutional neural network is used to classify the underwater object image data, identify different types of objects, such as reefs and shipwrecks, and annotate them in the model. This allows the terrain depth information obtained by the multi-beam echo sounder to be combined with the contours, position, and other information of underwater objects detected by the side-scan sonar, providing comprehensive data support for the subsequent construction of a more accurate underwater three-dimensional model.

[0119] Furthermore, in this embodiment, in order to achieve accurate fusion, the process of converting the parameters of the boat in its own coordinate system into the global coordinate system of the underwater digital twin model through the transformation matrix is ​​as follows: a transformation matrix T is defined to convert the parameters of the boat in its own coordinate system into the global coordinate system of the underwater digital twin model, and the position vector of the boat in its own coordinate system is set as P local =[x local ,y local ,z local ] T The position vector in the global coordinate system is P glocal =[x glocal ,y glocal ,z glocal ] T , then P glocal =TP local , where z local 、z glocal The transformation matrix T contains rotation and translation information and is calculated using the boat's heading angle θ, roll angle φ, pitch angle ψ, and initial position x0, y0, z0 in the global coordinate system. In one embodiment, the rotation matrix R is expressed in Euler angles as:

[0120] ,

[0121] Translation vector t=[x0,y0,z0] T ,but ;

[0122] As the boat sails, its water depth, position and other parameters are constantly changing. By establishing a real-time data transmission link, the latest measured parameters are transmitted to the digital twin model. The state of the boat in the underwater digital twin model is updated in real time. In the model, the position and attitude of the virtual boat are adjusted in real time based on the new parameter values ​​using the above transformation matrix. For example, when the water depth changes, z localAfter the transformation matrix T is calculated, the z position of the virtual boat in the global coordinate system is updated. glocal The coordinates are precisely aligned, ensuring that the position of the vessel in the digital twin model corresponds precisely to the actual vessel's position in the underwater environment, providing accurate basic data for subsequent navigation control. Furthermore, the model displays in real time the spatial relationship between the vessel and the surrounding underwater environment, such as the distance to obstacles and their relative position, facilitating intuitive assessment of navigation safety.

[0123] Therefore, the beneficial effects of a boat navigation control method of the present application also include:

[0124] Precise parameter measurement and integration: Pressure sensors combined with temperature compensation technology are used to measure water depth, while laser ranging technology combined with a data filtering algorithm measures vessel dimensions, ensuring accurate parameter measurement. These parameters are integrated into the underwater digital twin model through a carefully constructed transformation matrix, ensuring that the virtual vessel's state in the model accurately corresponds to the actual vessel. This provides a data foundation for path planning that is highly consistent with the vessel's actual state, avoiding path planning errors caused by parameter errors.

[0125] Intuitive Safety Assessment: Real-time updates of the vessel's status within the digital twin model provide a visual representation of the spatial relationship between the vessel and the surrounding underwater environment. Operators or automated decision-making systems can clearly see information such as the distance and relative position of the vessel to obstacles, enabling timely assessment of navigation safety and proactive adjustments to ensure safe navigation.

[0126] The technical principles of the present invention have been described above in conjunction with specific embodiments. These descriptions are intended solely to illustrate the principles of the present invention and are not to be construed in any way as limiting the scope of protection of the present invention. Based on the explanations herein, those skilled in the art will be able to conceive of other specific embodiments of the present invention without inventive effort, and such embodiments will fall within the scope of protection of the present invention.

Claims

1. A method for controlling a boat's navigation, characterized in that: The steps include: A. Based on the underwater digital twin model, confirm the preliminary path, including the following steps a1-a5: a1. Enter the target point, initialize the open list and closed list, and add the boat's current position as the starting node to the open list; a2. Select the node with the lowest total cost in the open list as the extension point to expand the adjacent node m; a3. Determine whether the adjacent node m is in the open list and the closed list. If not, add the adjacent node m to the open list and record the current extension point as the parent node of the adjacent node m. If yes, proceed to the next step; a4. Determine whether the theoretical energy consumption of the new path through the extension point to the adjacent node m is less than the theoretical energy consumption of the old path backtracking to the adjacent node m's existing parent node. If so, update the parent node of the adjacent node m to the extension point; if not, move the extension point to the closed list; a5. Repeat steps a2-a4 until the target point is reached, and trace back from the target point to the parent node to form the initial path; The total cost value includes at least the sum of the theoretical energy consumption, distance cost value and environmental risk cost value to the corresponding node. The distance cost value is the straight-line distance from the corresponding node to the target point. The environmental risk cost value is determined by the obstacle density and terrain complexity of the area where the node is located. B. Path adjustment, including the following steps b1-b4: b1. As the vessel navigates along the initial path, a dynamic window is generated based on the current position, motion state, and preset motion constraints, generating several velocity and angular velocity combinations (v, ω) for the next time interval Δt. b2. For each velocity and angular velocity combination within the dynamic window, the position and attitude of the vessel after Δt time are calculated using the kinematic model to form a candidate path; b3. Evaluate each candidate path based on a fusion model generated by fusing the virtual model of the vessel and the underwater digital twin model; b4. Selecting the candidate path with the best evaluation value as the real-time navigation path and sending the corresponding speed and angular velocity instructions to the control system of the boat; In step b2, the equation corresponding to the kinematic model is: x n =x0+vΔtcosθ0,y n =y0+vΔtsinθ0,θ n =θ0+ωΔt; (x0, y0), θ0 are the position coordinates and heading angle of the boat at the current moment, (x n ,y n ),θ n are the position coordinates and heading angle of the boat after the current moment Δt respectively; The evaluation indicators of step b3 include at least safety, target direction consistency and speed utilization; among them, safety is calculated by calculating the minimum distance d between the candidate path and the surrounding obstacles. min Evaluation, the target direction consistency is evaluated by calculating the angle α between the line connecting the candidate path end point and the target point and the current heading angle. The speed utilization is obtained by comparing the boat speed with the preset maximum speed. The calculation function of the evaluation index is S=w3α*+w4d min / d max +w5v / v m , where w3, w4, and w5 are preset weight coefficients, d max is the preset maximum sensing distance, v m is the maximum sailing speed, α*=(cosα+1) / 2; The process of building an underwater digital twin model includes the following steps c1-c4: c1. Use a multi-beam echo sounder to collect seabed topography data and obtain depth profile data along the track to construct a preliminary outline of the seabed topography; c2. Use side-scan sonar to detect underwater obstacles and targets, and obtain information such as the distance, shape, and reflection characteristics of underwater objects; c3. De-noise the information obtained in steps c1 and c2 and fuse the de-noised information to construct an underwater digital twin model; c4. Establish a state space model based on time series. When new sensor data arrives, use the extended Kalman filter algorithm to update the underwater digital twin model state; The process of generating the fusion model includes: creating a virtual model of the boat in the underwater digital twin model, inputting the parameters of the virtual model of the boat, and converting the parameters of the boat in its own coordinate system to the global coordinate system of the underwater digital twin model through a transformation matrix. The parameters of the virtual model of the boat include the outline dimensions of the boat and the water depth h of the boat; The boat's water depth h=(P-P0) / (ρg), where P is the seawater pressure and ρ is the seawater density, which changes with the temperature and salinity of the seawater. The formula ρ=ρ0(1-γ(T s -T0)+δ(S-S0)), ρ0 is the density of seawater under standard conditions, T s is the seawater temperature, T0 is the standard seawater temperature, S is the salinity, S0 is the standard salinity, γ and δ are preset constants, g is the acceleration of gravity, and P0 is the atmospheric pressure. To improve the measurement accuracy, the temperature compensation technology is used to correct the measurement error of the seawater pressure P caused by the change of temperature T. The seawater pressure measurement deviation caused by temperature is ΔVT=k1(TT r )+k2(TT r ) 2 , where k1 and k2 are preset coefficients, T r is the reference temperature.

2. A boat navigation control method according to claim 1, characterized in that: The theoretical energy consumption to node n is g(n)=g(n-1)+E*ΔT, where node n-1 is the parent node of node n or node n is the adjacent node of extension point n-1, E is the energy consumption per unit time, ΔT is the movement time interval, and E=0.5ρSC d (v n -v w cosθ) 2 v n , where ρ is the seawater density, S is the windward area of ​​the boat, and C d is the drag coefficient, v n is the speed of the boat at node n, v w is the water velocity, and θ is the angle between the water direction and the boat's sailing direction.

3. A method for controlling a boat's navigation according to claim 1, characterized in that: The environmental risk cost value r=w1d+w2k*, where w1 and w2 are preset weight coefficients, d is the obstacle density, which is calculated by counting the proportion of the obstacle area within a preset radius around the node in the underwater digital twin model, and k* is a dimensionless quantity obtained by normalizing the curvature k of the terrain in the node area to characterize the complexity of the terrain.

4. A method for controlling a boat's navigation according to claim 1, characterized in that: During the navigation process, the boat's sensors continuously sense the surrounding environment. If a new obstacle or environmental change is detected, the dynamic window is immediately regenerated, and the parameters corresponding to the evaluation index are updated according to the actual detected obstacle or environment, and steps b3 and b4 are repeated.

5. The method for controlling a boat navigation according to claim 1, wherein: In step c1, the measurement depth d of the corresponding beam i =0.5VΔt i , where the speed of sound waves in seawater is V=1449.2+4.6T s -0.055T s 2 +0.00029T s 3 +(1.34-0.01T s )(S-35)+0.016P,T s , S, and P are the temperature, salinity, and pressure of seawater, respectively. i represents the time difference between the transmission and reception of the i-th beam; In step c2, information such as the distance, shape, and reflection characteristics of underwater objects is calculated using the sound propagation loss model. The specific equation is as follows: 10log 10 (I0 / I)=20log 10 (r)+Ar; In step c3, the process of preprocessing the information obtained in step c1 is as follows: According to the system state equation, the state estimation value at the previous moment is updated in time to obtain the prior estimation value; Using the observation equation combined with R k Calculate the Kalman gain K k , and then according to K k Correct the prior estimate to obtain a more accurate state estimate at the current moment; Then, through iteration, the accuracy and stability of the depth data can be improved; Among them, the system state equation is X k =F k X k-1 +B k u k +w k , where X k is the state vector at time k, which contains information such as depth and related change rate. The depth-related change rate includes not only the change rate of depth over time, but also the movement state of the multi-beam echo sounder itself and the depth measurement change rate caused by ocean environmental factors. k B is the state transfer matrix, which describes the change of state over time; k is the control input matrix; u k is the control input vector; w k is the process noise, which obeys the normal distribution N(0,Q k ), Q k is the process noise covariance matrix, which reflects the uncertainty of the system model itself, Q k When it is larger, the algorithm relies more on new observation data to update the state estimate, and vice versa, it trusts the previous state estimate more; Among them, the observation equation is Z k =H k X k +v k , where Z k is the observation vector, i.e. the measured depth value; H k is the observation matrix, which determines the relationship between the measurement value and the state vector; v k is the observation noise, which obeys the normal distribution N(0,R k ), R k is the observation noise covariance matrix, which is used to measure the noise level of the observation data, R k When it is smaller, the credibility of the observation data is high, and the algorithm will pay more attention to the observation data Z when updating the state estimate. k ; R k When it is larger, the opposite is true; The denoised depth data is subjected to a tetrahedral meshing algorithm based on the Delaunay triangulation principle to generate a preliminary three-dimensional mesh model of the seabed topography. When meshing, a larger initial mesh side length l0 is set for areas with flat terrain. For areas with complex terrain, when it is detected that the terrain curvature K is greater than the preset upper limit, the mesh side length is reduced for local encryption. The reduced mesh side length l=l0*exp(-βK), where β is a preset constant.

6. A method for controlling a boat's navigation according to claim 1, characterized in that: The process of converting the parameters of the boat in its own coordinate system to the global coordinate system of the underwater digital twin model through the transformation matrix is ​​as follows: define a transformation matrix T, and let the position vector of the boat in its own coordinate system be P local =[x local ,y local ,z local ] T The position vector in the global coordinate system is P glocal =[x glocal ,y glocal ,z glocal ] T , then P glocal =TP local , where z local 、z glocal Related to the water depth; the transformation matrix T contains rotation and translation information, which is calculated by the heading angle θ, roll angle φ, pitch angle ψ of the boat and the initial position (x0, y0, z0) in the global coordinate system. The rotation matrix R is expressed by Euler angles as follows: ， Translation vector t=[x0,y0,z0] T ,but ; As the boat sails, the boat’s status in the underwater digital twin model is updated in real time.

Images