An intelligent positioning and navigation method for water propeller based on multi-modal fusion

Abstract

The application discloses a kind of water propeller intelligent positioning navigation method based on multi-modal fusion, belong to water unmanned vehicle technology and navigation positioning technical field.It includes the following steps:S1, multi-modal sensing data synchronous acquisition and pre-processing: synchronous acquisition GNSS, IMU, vision, sonar and flow sensor data, complete time alignment and quality evaluation;S2, cross-modal feature fusion and water environment modeling: adopt cross-modal attention mechanism to fuse feature output pose increment, combine extended Kalman filter with GNSS data for data-level fusion, while constructing water semantic map including obstacles and flow field;S3, adaptive positioning navigation decision under propeller constraint: based on fusion positioning result, plan path under dynamics constraint, generate control command by introducing flow compensation term in cost function.The application solves the problem of positioning failure in GPS shadow area, realizes high-precision positioning and energy-saving navigation in complex water area.

Description

Technical Field

[0001] This invention belongs to the field of unmanned watercraft technology and navigation and positioning technology, specifically relating to an intelligent positioning and navigation method for water propulsion based on multimodal fusion. Background Technology

[0002] In recent years, unmanned surface vessels (USVs) and intelligent propulsion systems have been increasingly widely used in port inspection, inland waterway transportation, maritime search and rescue, and hydrological monitoring. Accurate and robust real-time positioning and navigation capabilities are the core foundation for the safe and efficient operation of these vessels. Compared to land-based unmanned systems, the operating environment of surface vessels is characterized by high dynamism, strong interference, and weak infrastructure coverage, placing more stringent technical requirements on positioning and navigation systems.

[0003] In terms of positioning technology, current navigation systems for water propulsion vehicles primarily rely on Global Navigation Satellite Systems (GNSS) to provide absolute position information. GPS / BeiDou systems can provide positioning services with an accuracy of approximately 1 to 5 meters in open water conditions, basically meeting general navigation needs. However, as the application scenarios of water propulsion vehicles in complex aquatic environments continue to expand, GNSS positioning solutions have revealed serious reliability issues. In typical scenarios such as densely built-up port areas, under bridges spanning rivers, and waterways with steep cliffs along the shore, satellite signals are severely affected by multipath interference and shielding effects, leading to a sharp decline in positioning accuracy or even complete failure. Statistics show that in typical inland port environments, GPS signal shielding time can account for 20% to 40% of the total navigation time, posing a serious safety hazard to propulsion systems that rely on a single satellite for positioning.

[0004] In inertial navigation technology, while inertial measurement units (IMUs) possess advantages such as high autonomy and short-term accuracy, their inherent error accumulation characteristics limit the feasibility of their independent application. Both the gyroscopes and accelerometers of IMUs exhibit systematic errors such as bias, scaling factor error, and temperature drift. Without external correction, the position error increases quadratically over time. Taking a typical MEMS IMU as an example, after 60 seconds of continuous calculation without GPS assistance, the horizontal position error can exceed 5 to 10 meters, making it difficult to meet the navigation requirements for obstacle avoidance and precise berthing in port areas.

[0005] In terms of environmental perception technology, most existing solutions only use visual cameras and sonar for obstacle detection, failing to fully explore their value in assisting localization. Visual odometry technology can estimate relative displacement through continuous inter-frame feature point matching, providing effective drift correction for inertial navigation; sonar technology can not only be used for ranging but also assist in localization through underwater terrain matching. However, due to the special characteristics of the water surface environment—drastic changes in lighting, strong water surface reflection, and low underwater visibility—existing solutions generally lack adaptive preprocessing mechanisms tailored to the characteristics of water areas when processing multi-sensor data, resulting in insufficient perception robustness.

[0006] In hydrodynamics, the influence of water flow on the motion of water propulsion vehicles has long been neglected by existing navigation schemes. In inland rivers, harbors, and other bodies of water with high flow velocities, which can reach 0.5 to 2.0 m / s, this significantly deviates the propulsion vehicle's trajectory. If the navigation system does not actively compensate for the water flow, the propulsion vehicle must consume additional thrust to overcome the resistance, leading to increased energy consumption and greater trajectory tracking errors. Most existing path planning algorithms are based on the assumption of still water, exhibiting significant trajectory deviation problems in real-world waters with flowing currents.

[0007] In terms of multi-sensor fusion technology, existing solutions mostly adopt weighted averaging based on fixed weights or simple loose / tightly coupled Kalman filtering, which have two fundamental shortcomings: First, fixed fusion weights cannot adapt to the dynamic changes in the working state of sensors. When a sensor outputs low-quality data due to occlusion, interference, or failure, the system cannot automatically reduce its fusion contribution ratio, resulting in a deterioration in overall positioning accuracy. Second, existing fusion solutions fail to fully utilize the semantic association information between different modal sensors. The data from each sensor are fused only at the numerical level, ignoring the potential spatial correspondence between visual features and sonar features, which restricts further improvement in fusion accuracy.

[0008] To address the aforementioned issues, a key technical problem urgently needing to be solved in the field is how to efficiently fuse multi-source heterogeneous sensor data to construct a water propulsion positioning and navigation method that can maintain high-precision positioning even in scenarios where GPS signals fail, while also possessing the ability to sense water flow and adaptive weight adjustment. Summary of the Invention

[0009] To address the aforementioned problems in the existing technology, the purpose of this invention is to provide a smart positioning and navigation method for water propulsion vehicles based on multimodal fusion, as detailed below:

[0010] S1. Synchronous acquisition and preprocessing of multimodal sensor data: Deploy GPS / BeiDou, IMU, visual camera, sonar and water flow velocity sensor to complete the acquisition of multi-source data, and perform time synchronization alignment, noise filtering and data quality assessment on the acquired data.

[0011] S2. Cross-modal feature fusion and aquatic environment modeling: Extract modal features from each sensor, and use cross-modal attention mechanism and extended Kalman filter to achieve joint fusion of feature level and data level to construct a semantic map of aquatic environment.

[0012] S3. Adaptive positioning and navigation decision under thruster constraints: Based on the fused positioning state estimation results, path planning is performed under thruster dynamic constraints to generate real-time navigation control commands and form a closed-loop feedback with the thruster.

[0013] In step S1, the detailed steps for synchronous acquisition and preprocessing of multimodal sensor data are as follows:

[0014] S1.1 Sensor Array Configuration and Time Synchronization: Five types of sensors are deployed on the water propulsion system, typically including dual-frequency RTK GPS / BeiDou receivers, 6-axis MEMS inertial measurement units, monocular / binocular RGB vision cameras, multibeam sonar rangefinders, and three-dimensional electromagnetic current sensors; the sampling frequencies of each sensor are significantly different.

[0015] In some implementations, for example:

[0016] The GPS / BeiDou module has a sampling rate of 10 Hz, the IMU has a sampling rate of 200 Hz, the visual camera has a frame rate of 30 FPS, the sonar has a sampling rate of 20 Hz, and the water flow sensor has a sampling rate of 50 Hz. To address the issue of multi-rate asynchronous sampling, the GPS / BeiDou timing pulse (PPS signal) is used as the main clock reference, and nanosecond-level hardware timestamps are applied to the data streams of each sensor through an FPGA timestamp controller. At the software layer, a strategy combining linear interpolation and nearest-neighbor interpolation is employed to unify all sensor data to a 10 Hz common time grid. A time alignment error of no more than 5 milliseconds is used as the criterion for data validity; data frames exceeding this threshold are discarded and supplemented by extrapolation from the previous frame, ensuring strict temporal alignment of the multimodal data entering the fusion layer.

[0017] S1.2, GPS / BeiDou signal quality assessment and downweighting: Extracting the carrier-to-noise ratio from the raw GPS / BeiDou observation data. A comprehensive quality scoring function is constructed based on four quality indicators: pseudorange residual, horizontal accuracy factor (HDOP), and number of visible satellites.

[0018] In some implementations, the determination can be made based on a preset quality threshold, for example:

[0019] when When the mean is below 35 dBHz, or HDOP is greater than 3.0, or the number of visible satellites is less than 4, the GPS observation data for the corresponding epoch is marked as low quality, and its observation noise covariance matrix R in the fusion filter is...GPS Automatically amplified by 10 to 100 times, achieving soft weighting rather than hard removal to avoid state estimation jumps caused by sudden switching. When the above three indicators simultaneously deteriorate to extreme values ​​( When the frequency is below 25 dBHz, or the HDOP is greater than 6.0, or the number of satellites is less than 3, GPS data is completely isolated and the system switches to a pure inertial navigation / visual combination mode.

[0020] S1.3, IMU Error Compensation and Pre-integration: Zero-velocity correction (ZUPT) and zero-angular-velocity correction (ZARU) are applied to the raw accelerometer and gyroscope data output by the IMU to remove stationary drift.

[0021] In some implementations, for example:

[0022] To address the temperature-related scaling factor error of the MEMS IMU, a sixth-order polynomial temperature compensation model was established. Online calibration was completed during the thruster startup phase (approximately 30 seconds in a static state), improving the gyroscope bias estimation accuracy to the order of 0.01° / s. A pre-integration technique was employed within the IMU's high-frequency sampling interval, accumulating 200 Hz IMU measurements into a 10 Hz incremental pose. This aligned the IMU data with the time grid of other sensors, while preserving covariance information for subsequent EKF measurement updates.

[0023] S1.4 Robust Preprocessing of Visual Images: Multi-stage preprocessing is performed on visual images to address interference factors unique to the water surface environment, such as strong reflections, fog, and dynamic water waves.

[0024] In some implementations, for example:

[0025] First, adaptive histogram equalization (CLAHE) is applied to the image, dividing it into an 8×8 tile grid. Local equalization is performed with a contrast limit parameter of 2.0, effectively suppressing local overexposure areas caused by water surface reflection. Second, a background modeling method (Gaussian mixture model) is used to separate dynamic water ripple textures from static scenes, avoiding interference from dynamic water surface feature points in odometry estimation. Third, in extremely poor lighting conditions (mean brightness below 30 or above 220), the near-infrared channel is automatically switched to assist imaging. Finally, an ORB feature extractor is used, extracting 500 to 800 keypoints per frame. Outliers are eliminated using FLANN nearest neighbor matching and RANSAC geometric verification to construct inter-frame motion estimation. The relative pose accuracy of visual odometry is better than 0.3% of the travel distance in typical port area scenes.

[0026] S1.5 Sonar Data Filtering and Point Cloud Processing: The raw multibeam sonar data contains impulse noise caused by bubbles, schools of fish, suspended particles, etc. in the water. Adaptive median filtering is used to denoise the distance time series of each beam. The length of the filtering window is adaptively adjusted according to the noise density (e.g., between 3 and 9 points). After filtering, the multibeam sonar data is converted into a polar coordinate point cloud with the thruster as the origin (e.g., coverage area of ​​180° sector in front, angular resolution of 0.5°, distance resolution of 0.1 meters, maximum detection distance of 50 meters), which is used for underwater terrain contour extraction and surface obstacle detection.

[0027] In step S2, the detailed steps of cross-modal feature fusion and aquatic environment modeling are as follows:

[0028] S2.1 Single-modal feature extraction and standardized representation: For the physical characteristics of each sensor modality, feature extraction strategies are designed to map heterogeneous raw data into feature vectors of a unified dimension.

[0029] In some implementations, for example:

[0030] The unified dimension adopts 256 dimensions. For GPS / BeiDou data, three-dimensional position coordinates (latitude, longitude, and altitude), three-dimensional velocity vector, horizontal precision factor (HDOP), and carrier-to-noise ratio mean are extracted and mapped to a 256-dimensional feature space through a fully connected encoder. For IMU pre-integration data, incremental rotation (quaternion representation), incremental velocity, and incremental position, as well as the upper triangular elements (45 dimensions) of the corresponding 9×9 covariance matrix, are extracted and mapped by the encoder. For visual data, the optical flow field of inter-frame feature point matching pairs is extracted and 256-dimensional visual motion features are extracted through a convolutional neural network (lightweight MobileNetV3 backbone). For sonar data (sonar point cloud), 128-dimensional geometric features are extracted using a PointNet lightweight structure and then upscaled to 256 dimensions. For water flow sensor data, three-dimensional flow velocity vectors (magnitude and direction) and their confidence are extracted and mapped to 256 dimensions through a multilayer perceptron.

[0031] Each modal encoder adopts a standard structure of batch normalization plus ReLU activation. The output feature vector is subjected to L2 normalization before being fed into the fusion layer to ensure that the features of each modality are at the same numerical scale.

[0032] S2.2 Cross-modal attention fusion and EKF data-level fusion: This step adopts a two-layer fusion strategy of feature level and data level.

[0033] In some implementations, for example:

[0034] At the feature-level fusion layer, a cross-modal multi-head attention network is designed, using GPS / BeiDou feature vectors as queries and the concatenation of feature vectors from IMU, vision, sonar, and water flow as keys and values. The attention weights are calculated as shown in formula (1), where Q, K, and V are the query, key, and value matrices, respectively, and d k The key vector dimension is set to 8, and the number of heads for multi-head attention is set to 8.

[0035] (1);

[0036] After attention weights are normalized using Softmax, the attention scores for low-quality modalities (such as GPS in occluded areas and vision in highly reflective areas) are automatically reduced, while high-quality modalities automatically receive higher weights, thus achieving data-driven adaptive feature fusion. The fused feature vector F_fused is transformed into pose increment estimates by a feedforward network and used as the virtual observation input for EKF.

[0037] At the data-level fusion layer, an extended Kalman filter (EKF) is used to loosely fuse GPS absolute position observations and IMU estimated states. The measurement update equation of EKF is shown in equation (2), where K k For Kalman gain, Let H be the prior error covariance matrix. k To linearize the measurement matrix, R k To measure the noise covariance matrix, z k The difference between the actual measurement vector and the predicted measurement vector (innovation vector):

[0038] (2);

[0039] The EKF state vector is defined as 15-dimensional, including three-dimensional position, three-dimensional velocity, three-dimensional attitude angle, and the IMU's six-dimensional error states (three-axis accelerometer zero bias and three-axis gyroscope zero bias). The process noise covariance matrix Q is updated online based on the IMU pre-integrated covariance, and the measurement noise covariance matrix R... GPS R is dynamically adjusted based on GPS quality scores. Visual The system dynamically adjusts the in-point ratio based on visual features to achieve fully adaptive multi-source fusion.

[0040] S2.3 Construction of a three-layer semantic map of the aquatic environment: Using the pose estimation output by EKF as the central coordinate reference, the sonar data (sonar point cloud), visual image data (visual depth map) and water flow sensor data are jointly input into the Occupied Grid Mapping (OGM) module to construct a three-layer semantic map. The map coordinate system adopts a local Cartesian coordinate system with the initial position of the thruster as the origin.

[0041] In some implementations, for example:

[0042] The map is updated at a frequency of 10 Hz. The first layer is a binary obstacle occupancy grid map with a grid resolution of 0.5 m × 0.5 m. The sonar and visual detection results are fused using a Bayesian occupancy probability update rule. Grids with an occupancy probability greater than 0.7 are marked as obstacles, and those with an occupancy probability less than 0.3 are marked as free space for collision detection in path planning. The second layer is a navigable water confidence map, which comprehensively considers water depth (measured by sonar), obstacle distance, and historical trajectory information. Each grid is assigned a navigability confidence score of 0 to 1. Areas with a score less than 0.5 are considered no-go zones in path planning. The third layer is a water flow velocity field vector map, which uses real-time measurements from electromagnetic flow velocity sensors and a hydrodynamic model (a simplified interpolation model based on shallow water equations) to perform spatiotemporal interpolation of water flow velocity across the entire map, providing real-time flow velocity field input for the water flow compensation term in path planning.

[0043] In step S3, the detailed steps of the adaptive positioning and navigation decision under thruster constraints are as follows:

[0044] S3.1, Fusion Positioning State Estimation and Mode Switching: The EKF fusion result obtained in step S2 is used as the current pose estimate of the thruster. The output includes three-dimensional position (x, y, z) and three-dimensional velocity (v). x , v y , v z The system includes the heading angle ψ and its corresponding covariance matrix, where z is the water surface elevation correction value based on sonar measurements. The system automatically switches positioning modes based on the current available sensor status, and three operating modes are designed:

[0045] In some implementations, for example:

[0046] The first preset threshold is set to HDOP greater than 3.0 or The threshold is set below 35 dBHz; the second preset threshold is set to a feature point matching in-point rate of less than 30%.

[0047] When the GPS signal quality is normal, the GPS / IMU / visual three-modal tightly coupled fusion mode is adopted, and the positioning accuracy is better than 0.5 meters (CEP95).

[0048] When the GPS signal quality deteriorates (i.e. below the first preset threshold), it automatically switches to the IMU / visual / sonar three-modal fusion degradation mode, using visual odometry and sonar ranging to correct the IMU calculation error, and the positioning accuracy is maintained at 1 to 2 meters.

[0049] When the visual signal is simultaneously severely interfered with (i.e. below the second preset threshold), it further switches to the IMU / sonar dual-modal emergency hold mode, using the sonar sidewall distance constraint to limit lateral drift, while longitudinal positioning relies on IMU calculation.

[0050] The switching between the three modes adopts a hysteresis mechanism to avoid frequent switching near the threshold, which would cause state estimation jitter. The hysteresis bandwidth for switching judgment is set to 20% of the main threshold.

[0051] S3.2, Flow Compensation Path Planning: A modified Dynamic Window (DWA) method is used for local path planning. The planning starts from the current state of the thruster and searches for the optimal velocity command that satisfies the kinematic constraints in the velocity space. A flow compensation term is introduced based on the standard DWA evaluation function. The path cost function is shown in formula (3), where v c The velocity modulus of the water flow at the current location (obtained by looking up a table in the third layer of the three-layer semantic map). The angle between the propulsion direction and the water flow direction is λ, which is the water flow compensation weight coefficient (e.g., the value ranges from 0.1 to 0.5, and is adaptively adjusted according to the flow velocity). α, β, and γ are the weight coefficients for heading deviation, obstacle distance, and propulsion speed, respectively.

[0052] (3);

[0053] The water flow compensation term in the cost function is a negative term, and its physical meaning is: when the propulsion direction is consistent with the water flow direction ( When the angle approaches 0°, the negative cost that contributes the most to the compensation is the highest reward, causing the planner to tend to choose the direction with the current; when the direction of propulsion is opposite to the direction of the current ( When the angle approaches 180°, the compensation term contributes a positive cost, i.e., a penalty, to make the planner avoid direct countercurrent propagation as much as possible.

[0054] In some implementations, for example:

[0055] The prediction time domain for path planning is set to 8 to 15 meters ahead (dynamically adjusted according to the advancing speed), with a time step of 0.1 seconds, a velocity space search resolution of 0.05 m / s, and an angular velocity resolution of 0.05 rad / s. A single planning calculation on the embedded platform takes approximately 25 milliseconds, meeting the planning frequency requirement of 10 Hz.

[0056] S3.3, Propeller Closed-Loop Control Based on Model Predictive Control:

[0057] In some implementations, for example:

[0058] The path planning module outputs a reference trajectory sequence (containing reference position, velocity, and heading within a 10-step prediction time domain) to the thruster control layer at a frequency of 10 Hz. The thruster control layer employs Model Predictive Control (MPC) to establish a hydrodynamic model of the thruster, using fused positioning state variables as real-time feedback. The optimization objective of MPC is to minimize the reference trajectory tracking error within the future prediction time domain, while simultaneously satisfying the thruster's physical constraints (propeller speed 0 to 3000 RPM, rudder angle ±35°) and kinematic constraints (maximum speed not exceeding 3 m / s, maximum angular velocity not exceeding 0.5 rad / s). The MPC prediction time domain is set to 10 steps (1 second), and the control time domain is set to 3 steps, using a quadratic programming (QP) solver for real-time solving, with a single solution taking approximately 8 milliseconds. The control output consists of propeller speed and rudder angle commands, sent to the underlying actuators of the thruster at a frequency of 100 Hz, forming a high-frequency control closed loop. When the location covariance matrix trace output by EKF exceeds a preset threshold, MPC automatically increases the tracking error weight to improve robustness to positioning uncertainty.

[0059] S3.4 Sensor Fault Detection and Degraded Operation: The system design is based on a sensor fault detection mechanism using modal consistency verification, which monitors the innovation vector between the output of each sensor mode and the EKF prediction value in real time. For each sensor mode, the Normalized Innovation Square (NIS) statistic is calculated, which follows a chi-square distribution when the sensor is operating normally.

[0060] In some implementations, for example:

[0061] When the NIS value exceeds the upper bound of the 99% confidence interval of the chi-square distribution (i.e., exceeds the 3σ threshold) for three consecutive sampling periods, the sensor is deemed to have malfunctioned. After fault determination, the system isolates the fault within 0.1 seconds, amplifying the measurement noise covariance of the faulty mode to infinity (equivalent to removing it from the fusion process), while simultaneously reallocating the fusion weights of the remaining normal modes (normalized according to the current information content ratio of each mode) to ensure the continuity of the fused positioning. The system records a complete fault log, including the time of fault occurrence, fault type, and changes in positioning accuracy before and after the switchover, for post-event analysis.

[0062] By employing the above-described technology, the beneficial effects of the present invention compared to the prior art are as follows:

[0063] 1) In this invention, a three-layer fusion architecture based on cross-modal attention feature fusion, extended Kalman filter data fusion, and three-layer semantic map modeling is constructed. It uses visual and sonar data to compensate for the positioning error caused by GNSS occlusion. Through the layered modeling of obstacles, navigable areas, and water flow fields, it effectively solves the problem of positioning continuity in complex water area GPS signal occlusion scenarios, and improves the continuity and robustness of positioning results.

[0064] 2) This invention establishes a kinematic model that considers the influence of water kinetic energy by introducing water flow velocity field information into the path planning cost function. This model can guide the thruster to actively avoid upstream resistance and take advantage of downstream thrust, thereby reducing the energy consumption of the propulsion system and improving energy utilization efficiency while meeting the arrival time constraint.

[0065] 3) This invention integrates real-time closed-loop feedback of positioning error to the thruster MPC control layer, enabling the controller to perceive the current positioning uncertainty and dynamically adjust the aggressiveness and fault tolerance margin of trajectory tracking accordingly. Compared with the open-loop control scheme, it can reduce the root mean square error of trajectory tracking, and its trajectory maintenance accuracy is significantly improved in waters with high flow rates.

[0066] 4) The present invention is based on the sensor fault detection mechanism of NIS statistics, which realizes the rapid identification of abnormal sensor data; after fault isolation, the system can continue to operate safely in degraded mode without human intervention, which greatly improves the operational reliability and mission completion rate of the water propulsion in complex environments. Attached Figure Description

[0067] Figure 1 This is a flowchart of the method of the present invention;

[0068] Figure 2 This is a schematic diagram of the multimodal feature fusion network structure and extended Kalman filter joint architecture of the present invention. Detailed Implementation

[0069] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0070] Conversely, this invention encompasses any substitutions, modifications, equivalent methods, and solutions made within the spirit and scope of the invention as defined in the claims. Furthermore, to provide a better understanding of the invention, certain specific details are described in detail below. However, those skilled in the art will fully understand the invention even without these detailed descriptions.

[0071] Reference Figure 1 A method for intelligent positioning and navigation of water propulsion vehicles based on multimodal fusion includes the following steps:

[0072] S1. Synchronous acquisition and preprocessing of multimodal sensor data: Deploy GPS / BeiDou, IMU, visual camera, sonar and water flow velocity sensor to complete time synchronization alignment, noise filtering and data quality assessment of multi-source data.

[0073] S2. Cross-modal feature fusion and aquatic environment modeling: Extract modal features from each sensor, and use cross-modal attention mechanism and extended Kalman filter to achieve joint fusion of feature level and data level to construct aquatic semantic map.

[0074] S3. Adaptive positioning and navigation decision under thruster constraints: Based on the fused positioning state estimation results, path planning is performed under thruster dynamic constraints to generate real-time navigation control commands and form a closed-loop feedback with the thruster.

[0075] Example: An unmanned surface vessel (USV) at an inland river port was used as the implementation object. The waterway was a typical inland river port environment, including four reinforced concrete bridges, several riparian storage buildings, and an area with alternating forward and reverse currents with a maximum flow velocity of approximately 1.5 m / s. The propulsion platform was equipped with the sensor array described in this invention. The entire system was deployed on an embedded computing platform (ARM Cortex-A72 quad-core processor, 1.8 GHz, with a Jetson NX GPU acceleration module). The total system power consumption was approximately 35 W, and the overall weight was approximately 2.1 kg, meeting the limitations of shipboard space and power consumption.

[0076] The specific implementation process of step S1 is as follows:

[0077] S1.1 After the thruster starts, the sensor array performs self-tests and initializations sequentially. After the GPS / BeiDou receiver completes its initial positioning in open water (approximately 45 seconds for a cold start and approximately 5 seconds for a warm start), it continuously sends GGA, RMC messages, and 1 Hz PPS timing pulses to the main control computer using the NMEA 0183 protocol (baud rate 115200). After the IMU is powered on, it enters the static alignment phase (approximately 30 seconds), during which the accelerometer is calibrated using gravity vectors, and the initial zero bias estimation of the gyroscope is completed using the zero angular velocity condition. After initialization, the IMU zero bias estimation accuracy is better than 0.005° / s. The visual camera completes automatic white balance calibration, adjusting the exposure time to between 1 / 500 and 1 / 2000 of a second to cope with the high light on the water surface. Upon initial entry into the water, the sonar measures the current water temperature and calculates the speed of sound in the water (usually within the range of 1480 to 1530 m / s), completing the speed of sound compensation calibration. The water flow sensor performs zero-point calibration to eliminate system errors caused by installation deviations.

[0078] S1.2, Multi-sensor Time Synchronization Operation Phase: The system triggers the synchronous acquisition of data from each sensor using the PPS signal. Taking a complete bridge crossing (approximately 120 meters, taking approximately 80 seconds) as an example, the system operation process is illustrated as follows: In the open water section before entering the bridge (0 to 20 seconds), the GPS signal quality is normal (…). With an average frequency of approximately 42 dBHz, an HDOP of approximately 1.2, and 12 visible satellites, the system operates in a tightly coupled GPS / IMU / visual trimodal fusion mode, achieving a positioning accuracy of approximately 0.4 meters (RMSE). Upon entering the under-bridge obstruction phase (20 to 68 seconds), the number of GPS satellites rapidly decreases to 2 to 3. The mean dropped to 28 dBHz, and the GPS data weight automatically dropped to below 5% of the normal value. The system switched to IMU / visual / sonar degraded mode. The visual odometry provided relative displacement constraints with an accuracy of 0.2% of the travel distance, and the sonar provided side wall distance constraints (the distance between the left and right bridge piers was about 8 to 12 meters). The cumulative positioning drift was about 1.5 meters within 60 seconds. After leaving the bridge (68 to 80 seconds), the GPS signal recovered, and the system switched back to the three-modal fusion mode. The first effective GPS observation was used to correct the cumulative drift. The position jump was about 1.3 meters. The jump was smoothed by the EKF state estimation covariance constraint. No obvious abnormalities were found in the thruster speed command.

[0079] The specific implementation process of step S2 is as follows:

[0080] S2.1 In a typical port area test scenario, the visual camera triggered CLAHE preprocessing in areas of strong water reflection (when the light intensity exceeds 50,000 Lux, the mirror reflection area of ​​the water surface accounts for approximately 30% to 50% of the image area). After processing, the image contrast was improved by approximately 2.3 times, the number of ORB feature points extracted increased from approximately 120 points before processing to approximately 510 points, the inter-frame matching intra-point rate increased from approximately 45% to approximately 78%, and the continuous working time of the visual odometry was significantly extended. In this test scenario, the sonar detected several floating debris and underwater reefs on the water surface. Median filtering effectively eliminated approximately 12% of bubble noise points, and the false alarm rate was reduced to below 3%.

[0081] S2.2, Reference Figure 2The cross-modal attention fusion network was pre-trained offline using a port navigation dataset containing 12,000 frames of multimodal data. The training objective was to minimize the Euclidean distance error between the fused output pose and the RTK differential GPS reference value. After training, the network weights were fixed and deployed to an embedded platform. A single forward inference took approximately 6 milliseconds (GPU accelerated), meeting the real-time inference requirement of 100 Hz. In this test, the attention weight analysis results showed that in open water sections, the GPS modality attention weight accounted for approximately 62%, IMU approximately 23%, vision approximately 11%, and water flow approximately 4%. In the occluded sections under bridges, the GPS modality weight automatically decreased to below 8%, the vision modality weight increased to 45%, and the sonar modality weight increased to 35%, indicating that the network can automatically redistribute the fusion contribution according to the sensor quality, verifying the effectiveness of the adaptive fusion mechanism. EKF ran stably throughout the test, with a 96.3% pass rate for the innovation sequence whitening test, indicating good system model matching.

[0082] S2.3, a three-layer semantic map was maintained in real time at a resolution of 0.5 meters in a 50m × 50m port area test scenario, totaling 2500 grid cells. After the test, the obstacle detection recall rate was 97.5%, and the false alarm rate was 2.1%. The spatiotemporal interpolation error of the water flow velocity field (compared to a fixed reference current meter) was approximately 0.08 m / s (RMSE), meeting the accuracy requirements for path planning and water flow compensation.

[0083] The specific implementation process of step S3 is as follows:

[0084] S3.1 Positioning mode switching was triggered three times during the entire test flight (once upon entering the bridge, once upon exiting the bridge, and once due to a sudden change in light intensity). Each switch was completed within 0.15 seconds. During the switching process, the maximum fluctuation in the thruster speed command was approximately 0.08 m / s, and no manual intervention was triggered. The measured accuracy of the three positioning modes is summarized as follows: RMSE of approximately 0.42 meters in the GPS / IMU / visual trimodal mode; approximately 1.5 meters of cumulative drift within 60 seconds in the IMU / visual / sonar degraded mode; and approximately 2.8 meters of cumulative drift within 30 seconds in the IMU / sonar emergency mode. All of these are within the design specifications for their respective modes.

[0085] In the downstream navigation segment (approximately 35 meters) with a current velocity of about 1.3 m / s, the average deviation of the planned path from the target straight line is approximately 0.3 meters when the current compensation is enabled, and approximately 1.1 meters when the current compensation is disabled. The average additional thrust required by the thruster to maintain the trajectory is reduced by approximately 28 Newtons, corresponding to a reduction in energy consumption of approximately 19%. In the upstream navigation segment (approximately 30 meters, current velocity of about 0.9 m / s), the current compensation mechanism guides the thruster to select a detour path utilizing the backflow zone. Compared to the straight upstream approach, the total energy consumption is reduced by approximately 22%, and the navigation time only increases by approximately 4 seconds, demonstrating significant overall benefits.

[0086] The S3.3 and MPC controllers achieved a root mean square error (RMSE) of approximately 0.35 meters in trajectory tracking throughout the entire test voyage, with a maximum deviation of approximately 0.82 meters (occurring in the area of ​​the highest flow velocity at the confluence of water currents). Compared to the pure PID control scheme without MPC (RMSE approximately 0.61 meters, maximum deviation approximately 1.43 meters), the tracking accuracy was improved by approximately 43%. In the sonar fault simulation experiment (simulating sonar communication interruption via software injection), the system detected that the sonar modal NIS value exceeded the 99% chi-square confidence upper bound within 0.24 seconds, completed fault isolation, and switched to the GPS / IMU / visual three-modal fusion mode. The positioning accuracy remained essentially unchanged before and after the switch (RMSE difference approximately 0.05 meters), and the thruster navigation function continued to operate without interruption, fully verifying the effectiveness and real-time performance of the fault degradation strategy.

[0087] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for intelligent positioning and navigation of water propulsion vehicles based on multimodal fusion, characterized in that, Includes the following steps: S1. Synchronous acquisition and preprocessing of multimodal sensor data: Based on the sensor array installed on the water propulsion, the GNSS signal, IMU data, visual image data, sonar data and water flow velocity data of the water propulsion are acquired synchronously, and the acquired data is time-aligned, noise-filtered and data quality evaluated. S2. Cross-modal feature fusion and aquatic environment modeling: Extract modal features from data from each sensor, perform feature-level fusion using a cross-modal attention mechanism, and output pose increment estimation; Furthermore, based on extended Kalman filtering, the pose increment estimation and GNSS absolute position observations are fused at the data level to output the fused positioning results of the thruster; at the same time, based on sonar data, visual image data and water flow sensor data, a semantic map of the aquatic environment containing information on obstacles, navigable areas and water flow velocity fields is constructed. S3. Adaptive positioning and navigation decision under thruster constraints: Based on the fused positioning results and the semantic map of the water environment, path planning is performed under the dynamic constraints of the thruster. The cost function of path planning introduces a water flow compensation term based on the water flow velocity field, generates navigation control commands and sends them to the thruster actuator to form a closed-loop control.

2. The intelligent positioning and navigation method for water propulsion vehicles based on multimodal fusion according to claim 1, characterized in that, In S2, the specific process of adaptive feature-level fusion using a cross-modal attention mechanism is as follows: 1) Use the feature vector corresponding to the GNSS signal as the query vector; 2) Concatenate the feature vectors corresponding to IMU data, visual images, sonar data, and water flow velocity data to form key vectors and value vectors; 3) Based on the query vector, key vector, and value vector, calculate the attention weight, and then perform weighted fusion of the modal feature vectors corresponding to the query vector, key vector, and value vector according to the attention weight; 4) Based on the dynamic change of attention weight, when the GNSS signal quality deteriorates, the contribution ratio of GNSS feature vectors in feature-level fusion is reduced by adjusting the query weight coefficient in the cross-modal attention mechanism, and the fusion weight of the corresponding feature vectors of visual image data and sonar data is increased.

3. The intelligent positioning and navigation method for water propulsion vehicles based on multimodal fusion according to claim 1, characterized in that, In S2, the specific process of data-level fusion based on extended Kalman filtering is as follows: 1) The pose increment estimate output after fusing the cross-modal attention mechanism is used as a virtual observation of the extended Kalman filter, wherein the pose increment estimate incorporates feature information from the visual image data; 2) Use the absolute position observations of GNSS as the observations of extended Kalman filtering, and use the pose obtained based on IMU pre-integration as the state prediction value; 3) By extending the Kalman filter update equation, the state prediction value is corrected using virtual observations and absolute position observations, and the fused positioning result is output.

4. The intelligent positioning and navigation method for water propulsion vehicles based on multimodal fusion according to claim 3, characterized in that, The measurement noise covariance of the extended Kalman filter can be adaptively adjusted according to the data quality of the corresponding observation, as follows: Based on the real-time quality of GNSS signals, the measurement noise covariance of GNSS absolute position observations is dynamically adjusted; based on the feature matching quality of visual image data, the measurement noise covariance of the virtual observations is dynamically adjusted.

5. The intelligent positioning and navigation method for water propulsion vehicles based on multimodal fusion according to claim 1, characterized in that, In S3, the evaluation function J for path planning is expressed as: ； in, As a cost of course deviation, The cost of obstacle distance, For the price of speed, Let this be the modulus of the water flow velocity at the current location. The angle between the direction of propulsion and the direction of water flow. For water flow compensation weighting coefficients, α, β, and γ are the weighting coefficients for heading deviation, obstacle distance, and propulsion speed, respectively. The water flow compensation term in the cost function is a negative term. When the direction of advancement is consistent with the direction of water flow, the compensation term contributes the largest negative cost, which is the highest reward, making the planner inclined to choose the direction with the current. When the direction of advancement is opposite to the direction of water flow, the compensation term contributes a positive cost, which is the penalty, making the planner try to avoid direct advancement against the current.

6. The intelligent positioning and navigation method for water propulsion vehicles based on multimodal fusion according to claim 1, characterized in that, S3 also includes a sensor fault detection and degradation step, as detailed below: 1) Calculate the standardized squared innovation statistic for each sensor mode based on the innovation vector of the extended Kalman filter; 2) When the standardized innovation square statistic of a certain sensor mode exceeds the confidence threshold set based on the chi-square distribution for n consecutive sampling periods, the corresponding sensor is determined to be faulty, and its measurement noise covariance is amplified in the fusion filtering to isolate the fault. 3) Based on the status of the remaining normal sensors, automatically switch to the corresponding downgraded fusion positioning mode.

7. The intelligent positioning and navigation method for water propulsion vehicles based on multimodal fusion according to claim 6, characterized in that, The specific process of automatically switching to the corresponding downgraded fusion positioning mode is as follows: When the GNSS signal quality is higher than the first preset threshold, feature-level and data-level fusion is performed using GNSS data, IMU data and visual image data as inputs. When the GNSS signal quality is lower than the first preset threshold, feature-level and data-level fusion is performed using IMU data, visual image data, and sonar data as inputs. When the GNSS signal quality is lower than the first preset threshold and the visual signal quality is lower than the second preset threshold, feature-level and data-level fusion is performed using IMU data and sonar data as input.

8. The intelligent positioning and navigation method for water propulsion vehicles based on multimodal fusion according to claim 1, characterized in that, In S1, the specific preprocessing process is as follows: For GNSS signals, the carrier-to-noise ratio, horizontal accuracy factor, and number of visible satellites are extracted to evaluate signal quality, and their weights are dynamically adjusted in subsequent fusion based on the evaluation results. Error compensation and pre-integration are performed on the IMU data to obtain the incremental pose aligned with the common time grid. For visual images, robust enhancement processing is performed, and feature points are extracted to construct inter-frame motion estimation; The sonar data is filtered and denoised, and then converted into point cloud data. Sensor calibration is performed on the water flow velocity data to eliminate system errors.

Images