Ship cleaning robot water and underwater combined positioning and navigation method and system

Abstract

The application discloses a ship cleaning robot water and underwater combined positioning and navigation method and system, relates to the technical field of ship cleaning robots, and comprises the following steps: obtaining water and underwater positioning results respectively according to multi-source observation data; calculating current environmental state parameters, combining water depth and water level calibration values, and calculating soft switching weights; taking the water positioning result as an anchor point, taking the current environmental state parameters as a judgment condition, adjusting the underwater multi-source observation data, thereby updating the underwater positioning result; weighting the latest water positioning result and the underwater positioning result according to the soft switching weights, and obtaining a combined positioning result; determining a position deviation according to the latest water positioning result and the underwater positioning result, compensating the combined positioning result according to a linear time factor, obtaining a final positioning result, and performing navigation control according to the final positioning result. The method improves the operation stability and navigation reliability of the robot in a complex water environment.

Description

Technical Field

[0001] This invention relates to the field of ship cleaning robot technology, and in particular to a method and system for combined above-water and underwater positioning and navigation of a ship cleaning robot. Background Technology

[0002] When existing ship cleaning robots perform operations in complex waters (such as the hull of a ship, waterline areas, and near the propeller), they typically rely on a single positioning source (such as an inertial measurement unit or odometer) for attitude estimation and path positioning and navigation.

[0003] However, in environments where the surface and underwater alternate, this single-source positioning method has the following problems: positioning accuracy drifts over time, and errors cannot be effectively constrained; after long-term operation, the attitude of the inertial measurement unit (IMU) drifts significantly, and the IMU is affected by factors such as propulsion wheel slippage and water flow disturbance, resulting in a rapid increase in accumulated errors and a gradual decrease in positioning accuracy; satellite positioning or ultra-wideband positioning cannot be used in underwater environments. When the robot enters the underwater working area, GPS signals are completely blocked, and ultra-wideband positioning signals are severely attenuated, failing to provide reliable absolute position references and causing the robot to lose its global positioning capability; existing multi-source positioning switching methods are hard switching, resulting in positioning jumps, and existing systems usually directly switch the positioning source after detecting changes in the surface / underwater state, lacking transition processing, causing sudden changes or discontinuities in the output pose at the moment of switching, which seriously affects the accuracy of path tracking and trajectory cleaning.

[0004] As a result, existing ship cleaning robots are prone to problems such as inaccurate positioning, discontinuous trajectory, sudden changes in environment, and unstable control response when operating alternately on and under water, which seriously affects the coverage accuracy and safety of cleaning operations. Summary of the Invention

[0005] To address the aforementioned issues, this invention proposes a method and system for joint above-water and underwater positioning and navigation of a ship cleaning robot. By constructing a dual-positioning source fusion architecture for above-water and underwater environments and implementing dynamic soft-switching positioning during the switching between above-water and underwater environments, the spatiotemporal continuity and overall accuracy of the positioning results are guaranteed, thereby improving the robot's operational stability and navigation reliability in complex aquatic environments.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] In a first aspect, the present invention provides a method for joint above-water and underwater positioning and navigation of a ship cleaning robot, comprising:

[0008] Based on multi-source observation data from above and below water, above-water positioning results and underwater positioning results were obtained respectively;

[0009] Calculate the current environmental state parameters, and then combine them with the water depth and water level calibration values ​​to calculate the soft handover weight;

[0010] Using the surface positioning results as anchor points and the current environmental state parameters as judgment conditions, the underwater multi-source observation data are adjusted to update the underwater positioning results;

[0011] Based on the soft handover weights, the latest surface positioning results and underwater positioning results are weighted to obtain the joint positioning results;

[0012] The position deviation is determined based on the latest surface and underwater positioning results. The joint positioning results are then compensated using a linear time factor to obtain the final positioning result, which is then used for navigation control.

[0013] As an alternative implementation method, the above-water and underwater positioning process includes:

[0014] Construct a noise model based on the measurement noise covariance matrix and the process noise covariance matrix;

[0015] The measurement vector is defined by using multi-source observation data as the state vector and combining it with underwater measurement noise.

[0016] After updating the state using extended Kalman filtering based on the measurement vector and noise model, the planar pose component is taken as the surface positioning result and the underwater positioning result.

[0017] As an alternative implementation method, the environmental state parameter S is:

[0018] ;

[0019] ;

[0020] in, Here, h is the weighting coefficient, and h0 is the water level calibration value. For the short window variance of IMU sensor data, The maximum dynamic threshold of the IMU. The maximum depth deviation threshold; Let k be the water depth. It is the signal-to-noise ratio of the UWB measurement signal at time k; These represent the maximum signal-to-noise ratio and the minimum signal-to-noise ratio, respectively.

[0021] As an alternative implementation method, soft handover weighting for;

[0022] ; ;

[0023] The joint positioning result is ;

[0024] Position deviation is ;

[0025] The final location result is ;

[0026] in, Let the water depth be at time k. For water level calibration values; This is the coefficient for the steepness of the curve; Base steepness; The adaptive gain is S, where S is the environmental state parameter. It is the average deviation at multiple moments within the anchor window A; The result of the water positioning at time k; The underwater positioning result at time k; Let be the linear time factor at time t. T represents the total time.

[0027] As an alternative implementation method, the process of adjusting underwater multi-source observation data includes:

[0028] ;

[0029] ;

[0030] in, To adjust the step size; The true angular velocity and true acceleration are derived from the water positioning results; It refers to the zero bias of the IMU gyroscope and the zero bias of the acceleration. This is the current Odom scaling factor; This is due to scale bias. To fuse the true distance increment extracted from the pose within the anchor point window, The distance measured by the odometer within the same window.

[0031] As an alternative implementation, the process of performing navigation control based on the final positioning result includes: based on the final positioning result... Reference Path Calculate the target point With current location Linear velocity per unit time control quantity With angular velocity ;

[0032] ;

[0033] ;

[0034] ;

[0035] in, Forward search distance; Let i be the i-th reference point on the reference path; For angular error, The function limits the angle to between; It is a saturation function; This is the acceleration proportionality coefficient; The initial minimum speed, For planar position components, This represents the heading angle component.

[0036] Secondly, the present invention provides a combined surface and underwater positioning and navigation system for a ship cleaning robot, comprising:

[0037] The positioning module is configured to obtain surface positioning results and underwater positioning results based on multi-source observation data from both above and below water.

[0038] The switching parameter determination module is configured to calculate the current environmental state parameters, and then combine these with the water depth and water level calibration values ​​to calculate the soft handover weight.

[0039] The correction module is configured to use the surface positioning results as anchor points and the current environmental state parameters as judgment conditions to adjust the underwater multi-source observation data, thereby updating the underwater positioning results;

[0040] The joint positioning module is configured to weight the latest surface positioning results and underwater positioning results according to the soft handover weights to obtain the joint positioning result;

[0041] The navigation module is configured to determine the position deviation based on the latest surface and underwater positioning results, compensate the joint positioning results according to the linear time factor, obtain the final positioning result, and perform navigation control based on the final positioning result.

[0042] Thirdly, the present invention provides an electronic device including a memory and a processor, and computer instructions stored in the memory and running on the processor, wherein the computer instructions, when executed by the processor, perform the method described in the first aspect.

[0043] Fourthly, the present invention provides a computer-readable storage medium for storing computer instructions, which, when executed by a processor, perform the method described in the first aspect.

[0044] Fifthly, the present invention provides a computer program product, including a computer program that, when executed by a processor, implements the method described in the first aspect.

[0045] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0046] This invention proposes a method and system for joint above-water and underwater positioning and navigation of a ship cleaning robot. By performing above-water and underwater positioning separately and identifying environmental state parameters, it achieves adaptive switching between above-water and underwater modes, making the pose of the switching segment continuous without step changes, significantly reducing switching errors, achieving continuous positioning across media, and solving the problem of frequent peak fluctuations in positioning errors in traditional hard switching methods.

[0047] This invention proposes a method and system for joint underwater and surface positioning and navigation of a ship cleaning robot. It utilizes a covariance-driven dynamic weighting mechanism under the extended Kalman filter framework to achieve adaptive weighted fusion of data from various sensors, thereby improving positioning accuracy and system robustness. It also solves the problem that traditional methods are prone to divergence when ultra-wideband positioning signals are blocked or odometer slippage occurs.

[0048] To address the systematic errors between surface and underwater positioning, this invention proposes a progressive compensation and zero-bias feedback mechanism based on the anchor point deviation between surface and underwater positioning results. Using the surface positioning result as the anchor point, the underwater odometer is gently corrected, enabling the trajectory to quickly align and stabilize after entering the water, achieving trajectory consistency. Furthermore, multiple surfacing operations can form periodic calibrations, effectively suppressing long-term drift.

[0049] Advantages of additional aspects of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0050] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0051] Figure 1 This is a flowchart of the combined above-water and underwater positioning and navigation process for a ship cleaning robot provided in Embodiment 1 of the present invention;

[0052] Figure 2 This is a schematic diagram of the combined above-water and underwater positioning and navigation principle of the ship cleaning robot provided in Embodiment 1 of the present invention;

[0053] Figure 3 This is a schematic diagram of the ship cleaning robot provided in Embodiment 1 of the present invention;

[0054] Figure 4This is a schematic diagram of the sensors on the ship cleaning robot provided in Embodiment 1 of the present invention;

[0055] Figure 5 The figure shows the experimental results provided in Embodiment 1 of the present invention. Detailed Implementation

[0056] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0057] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0058] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, unless the context clearly indicates otherwise, the singular form is intended to include the plural form as well. Furthermore, it should be understood that the terms “comprising” and “including”, and any variations thereof, are intended to cover non-exclusive inclusion, for example, a process, method, system, product, or apparatus that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0059] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0060] Example 1

[0061] like Figure 1 As shown in the figure, this embodiment provides a method for joint above-water and underwater positioning and navigation of a ship cleaning robot, which specifically includes the following steps:

[0062] Based on multi-source observation data from above and below water, above-water positioning results and underwater positioning results were obtained respectively;

[0063] Calculate the current environmental state parameters, and then combine them with the water depth and water level calibration values ​​to calculate the soft handover weight;

[0064] Using the surface positioning results as anchor points and the current environmental state parameters as judgment conditions, the underwater multi-source observation data are adjusted to update the underwater positioning results;

[0065] Based on the soft handover weights, the latest surface positioning results and underwater positioning results are weighted to obtain the joint positioning results;

[0066] The position deviation is determined based on the latest surface and underwater positioning results. The joint positioning results are then compensated using a linear time factor to obtain the final positioning result, which is then used for navigation control.

[0067] The following is combined Figure 2 The method of this embodiment will be described in detail.

[0068] S1: Acquire multi-source observation data, including IMU (Inertial Measurement Unit) sensor data, Odom (Odometer) sensor data, UWB (Ultra Wide Band) sensor data, and depth sensor data.

[0069] The UWB sensor data includes the planar coordinates of the ship cleaning robot (hereinafter referred to as the robot). ;

[0070] Odom sensor data includes robot pose. ; For the robot's position, For heading angle;

[0071] IMU sensor data includes acceleration and angular velocity. ; These are the x and y axis components of the acceleration, respectively. Angular velocity;

[0072] Depth sensing data includes water depth This value is used to identify whether the robot is in an underwater environment at the current time k. If the robot is not underwater, this value is zero, indicating that there is no valid depth data.

[0073] During operation, the ship cleaning robot uses an inertial measurement unit, odometer, ultra-wideband positioning module, and depth sensor to collect IMU sensor data, Odom sensor data, UWB sensor data, and depth sensor data, respectively. The IMU sensor data is used to provide acceleration and angular velocity information; the Odom sensor data is used to measure the robot's motion distance; the UWB sensor data is used to calculate the absolute position on the water; and the depth sensor is used to identify whether the robot is currently in an underwater environment.

[0074] S2: Timestamp synchronization is performed using a unified clock to form complete data frames, ensuring that multi-source observation data are aligned within milliseconds for unified fusion. The resulting multi-source observation data with unified coordinates and a unified time axis is... .

[0075] S3: Data preprocessing and noise modeling.

[0076] The sensor data were filtered and error modeled. The IMU sensor data were processed with zero bias compensation and low-pass filtering; the Odom sensor data were processed with moving average and scale factor correction; the UWB sensor data were processed with RSSI (Received Signal Strength Indicator) intensity to determine the measurement variance and remove multipath outliers; and the depth sensor data were processed with first-order Kalman filter (KF) smoothing.

[0077] Specifically:

[0078] (1) IMU zero bias and filtering.

[0079] angular velocity at time k and linear acceleration A sliding window static estimation with zero bias compensation and low-pass filtering are employed.

[0080] ;

[0081] ;

[0082] ;

[0083] in, The zero bias of angular velocity and zero bias of linear acceleration are estimated after filtering, which correspond to the zero bias of the gyroscope and the zero bias of the accelerometer, respectively. The time window is defined as starting from the current moment. Heading back A time window at time t is used to estimate the zero bias; It is a smoothing factor that controls the degree of filtering; It is the filtered value of the acceleration (or angular velocity) at time k in the world coordinate system; It is the filtered value of the acceleration (or angular velocity) at time k-1 in the world coordinate system; It is a zero-bias estimate of the IMU sensing data at time k; The acceleration (or angular velocity) at time k is the measured value in the world coordinate system.

[0084] (2) Odom scale and slip detection.

[0085] First, the scale factor of the Odom sensor data is corrected using the short-term arc length of the UWB sensor data:

[0086] ;

[0087] in, This represents the odometry scale factor at a given time. This is the scaling factor adjustment amount; This represents the UWB sensor data collected at time i; This represents the UWB sensor data collected at time i-1; This represents the Odom sensor data collected at time i, including position and heading; This represents the Odom sensor data collected at time i-1; Indicates the length of the historical time window; To adjust the step size.

[0088] Subsequently, slippage is detected by speed residual detection; if the speed residual... The difference between the instantaneous linear velocity calculated from the odometer and the predicted velocity predicted from the previous cycle exceeds a set threshold. This indicates that Odom is experiencing significant slippage or wheel speed distortion, which necessitates increasing the Odom measurement noise covariance. This reduces the credibility of Odom in subsequent fusions, and the arrows in the formula indicate that it is amplified.

[0089] ;

[0090] ;

[0091] in, Linear velocity obtained from pose difference (dedicated to velocity residual detection). This represents the velocity residual at time k, i.e., Odom's actual linear velocity. and prediction speed The difference between them; It is a preset speed tolerance threshold used to determine whether slippage has occurred; The Odom sensor data at time k; The data is the Odom sensor data at time k-1; For time intervals.

[0092] (3) UWB variance mapping.

[0093] Based on the real-time signal quality, i.e., the signal-to-noise ratio (SNR), provided by the UWB module, which reflects the reliability of the UWB ranging signal, the measurement covariance is adaptively adjusted.

[0094] ;

[0095] in, It is the noise covariance measured by UWB at time k. It is the initial noise covariance. It is the signal-to-noise ratio of the UWB measured signal at time k, and is an indicator of signal quality; These are the maximum signal-to-noise ratio (SNR) and the minimum signal-to-noise ratio (SNR), which fall within the UWB factory parameter range. Specifically, a high SNR leads to a decrease in covariance and an increase in weight, while a low SNR leads to an increase in covariance and a decrease in weight. The adjustment factor determines the relationship between signal quality and noise covariance.

[0096] (4) Water depth measured by depth sensor A first-order Kalman filter is used to obtain the smoothed result:

[0097] ;

[0098] in, For the variance of depth measurement; The predicted water depth (prior estimate) is calculated at time k-1. The Kalman gain parameter determines the degree to which we "trust the sensor" or "trust the prediction." The updated water depth at time k (posterior estimate) incorporates water depth measurements from depth sensors. water depth predicted by KF ; The prediction error covariance matrix represents the reliability of the prediction. The larger the value, the less reliable the prediction; the smaller the value, the more reliable the prediction.

[0099] Establish a noise model:

[0100] ;

[0101] in, This is the process noise increment matrix, which includes uncertainties in system dynamics, IMU drift, odometer cumulative error, etc. The measurement noise increment matrix includes sensor reading errors, UWB signal jumps, depth measurement drift, etc. The measurement noise covariance matrix is ​​composed of the diagonals of each measurement channel block. That is, IMU / Odom / UWB / depth; process noise covariance matrix These are position process noise, heading angle process noise, linear velocity process noise, angular velocity process noise, gyro zero-bias drift noise, acceleration zero-bias drift noise, and Odom scale factor process noise, each... This represents the noise variance of the corresponding sensor along its respective axis.

[0102] S4: Establishment of dual-source filtering model.

[0103] Two positioning models, one for surface and one for underwater, are established. Based on the noise model established in step S3, the weights are adjusted in real time using the measurement covariance to achieve adaptive sensor fusion.

[0104] (1) Water model: Odom+IMU+UWB is fused, and the global pose is estimated by Extended Kalman Filter (EKF);

[0105] (2) Underwater model: Integrating Odom+IMU, relative positioning is achieved solely by inertial and odometer data.

[0106] Specifically:

[0107] (1) Whether above or below water, EKF is based on the same state vector, which is defined as:

[0108] ;

[0109] in, Let k be the plane coordinates at time k; Let be the heading angle at time k; Let K be the linear velocity and angular velocity at time k. The IMU gyroscope and accelerometer have zero bias at time k; Let be the Odom scaling factor at time k.

[0110] Constructing system motion equations based on state vectors (combined with IMU calculations) and using this to predict motion is the foundation for joint prediction of surface and underwater positioning.

[0111] ;

[0112] ;

[0113] The right side of the first equation forms the state vector at time k+1, which will not be elaborated further. The output of the IMU accelerometer at time k; The IMU angular velocity output at time k; The process noise of the IMU gyroscope zero bias, accelerometer zero bias, and scale factor all satisfy a Gaussian distribution. , The process noise covariance matrix; Let k be the state vector at time k-1. This is the prediction error covariance matrix.

[0114] (2) Water positioning.

[0115] Based on Odom sensor data at time k and UWB sensor data Define a waterborne measurement vector, where, For the robot's position, For heading angle; For the robot's planar coordinates;

[0116] ;

[0117] in, This is a waterborne measurement function used to convert the state vector... Mapped to the sensor measurement space; Reflecting the actual measurement noise of Odom and UWB in a water surface environment, it follows a zero-mean Gaussian distribution, and the measurement noise covariance is... The output is dynamically adjusted according to the reliability of the signal. That is, the UWB noise is adjusted by the environmental reliability S. The larger S is, the better the UWB signal and the higher the UWB weight. The smaller S is, the lower the UWB weight.

[0118] Filtered update output: Each This represents the noise variance of the corresponding sensor along its respective axis; Then from the updated Extracting planar pose components as the result on water .

[0119] (3) Underwater positioning.

[0120] Based on Odom sensor data at time k Define underwater measurement vectors:

[0121] ;

[0122] Among these, underwater positioning lacks UWB, and Odom is the only external constraint. For underwater measurement functions, This refers to underwater measurement noise, which includes Odom noise and scale drift error.

[0123] Filtered output: Similarly, the pose components are extracted as the underwater positioning result. .

[0124] Therefore, surface EKF positioning and underwater EKF positioning share the same set of state vectors and the same motion model, only using different measurements during the update phase; the prediction steps are completely identical for both; the update steps are updated using surface measurements and underwater measurements respectively, thus obtaining two soft-switching positioning results. Both originate from the same state model, but differences arise after updates from different measurement sources (with or without UWB). Subsequent soft handover fusion and feedback correction are used to gradually bring them into agreement.

[0125] S5: Environmental identification and status determination.

[0126] Based on the aforementioned dual-source localization model, to achieve adaptive adjustment of perception weights under different environments, it is necessary to further determine whether the robot is currently above or below water. Therefore, environmental state recognition is introduced. Based on depth sensing data, UWB sensing data, and IMU sensing data, the robot's environmental state (trusted environment above water or untrusted environment underwater) is determined in real time, and continuous environmental state parameters S are output.

[0127] ;

[0128] Where x represents the input normalized weight; the clip function is used to restrict the input data to a specified numerical range, that is, to limit x to the interval [0,1], to avoid weight abrupt changes caused by measurement quality fluctuations, and to make the environmental state parameter S more stable and controllable.

[0129] Based on the above three criteria, a weighted fusion model is used to calculate the environmental state parameters:

[0130] ;

[0131] ;

[0132] in, Here, h is the weighting coefficient, and h0 is the water level calibration value. For the short window variance of IMU acceleration / angular velocity, The maximum dynamic threshold of the IMU is set. The set depth deviation allowable range, i.e. the maximum depth deviation threshold, is used for water depth changes, not the maximum water depth value; the final output is the current environmental state parameter S∈[0,1], the larger the value, the closer it is to the reliable water environment.

[0133] S6: Soft handover weight calculation and fusion output.

[0134] Because the reliability of the positioning source varies significantly with environmental changes, the errors of the two sets of filters will change abruptly when crossing the water surface. In order to achieve a smooth transition at the interface between the surface and underwater, soft handover weights are introduced. ;

[0135] ;

[0136] in, Because of the water depth, For water level calibration values; To control the curve steepness coefficient, the harsher the environment, the smoother the weight switching. Steepness adapts to change;

[0137] ;

[0138] in, Base steepness; For adaptive gain based on changes in environmental conditions, underwater, Increased switching speed, especially on water. Reduced switching speed makes the transition smoother.

[0139] The final joint localization result is: .

[0140] S7: Continuous compensation and dynamic bias correction.

[0141] Although soft handover achieves smoothness, small residual accumulation may still occur due to differences in cross-domain noise. Therefore, in step S7, high-precision waterborne positioning results are utilized. Underwater positioning results Perform feedback correction to achieve cross-domain consistency correction:

[0142] ;

[0143] in, This represents the average deviation (anchor point deviation) between the surface positioning results and the underwater positioning results. It is the average deviation of multiple moments within the anchor window A, in order to reduce the impact of instantaneous noise.

[0144] And based on the linear time factor at time t Compensation will be provided.

[0145] ;

[0146] Where T represents the total time.

[0147] Final output fused pose To avoid amplifying slight deviations caused by switching between above and below the water surface; .

[0148] In this embodiment, the positioning results of the water surface section are used as anchor points to perform gentle correction on the underwater IMU+Odom calculation, suppress long-term drift, and enable the diving trajectory to quickly return to the true position.

[0149] When the environmental state parameter S is stable (the change in value between different time points is less than the set threshold) and close to 1, it indicates that the system is in a stable state above water. If it starts to change, the system collects the above-water and underwater pose data to estimate the anchor point deviation. The anchor point window is then opened. That is, before entering the underwater environment, the above-water positioning results are used as anchor points, and the difference between the two positioning results is estimated, i.e., the anchor point deviation estimation.

[0150] ;

[0151] in, This is due to scale bias. To fuse the true distance increment extracted from the pose within the anchor point window, The distance measured by the odometer within the same window.

[0152] Parameter fine-tuning:

[0153] ;

[0154] in, To adjust the step size; The true angular velocity and true acceleration are derived from the above-water positioning results, and the zero bias of the IMU's gyroscope and acceleration is automatically corrected to make the underwater calculation more stable. It refers to the zero bias of the IMU gyroscope and the zero bias of the acceleration. This is the current Odom scale factor, which aligns the scale of the underwater Odom with the actual movement distance, preventing it from drifting further and further away.

[0155] When the environmental state parameter S is stable, the robot is considered to be in a reliable water environment. At this point, the waterborne positioning result is used as an anchor point to estimate the deviation of the underwater positioning result. The above formulas are used to fine-tune the parameters of the underwater positioning model, including scale error and IMU zero bias error, so that it gradually aligns with the true pose and improves the overall positioning accuracy.

[0156] S8: Navigation control.

[0157] After the soft handover and bias correction described above, the final stable fused pose can be obtained. As navigation control input Reference Path The Pure-Pursuit algorithm (a path tracking control algorithm based on geometric relationships) is used to calculate the linear velocity control quantity per unit time between the target point and the current position. With angular velocity fusion planar position components Used as the linear velocity unit-time control input in the Pure-Pursuit algorithm, and simultaneously with the heading angle component. Used for angular error calculation. The third formula adjusts the linear velocity based on the distance between the current position and the forward-looking target point, and limits the maximum velocity using the SAT function to avoid impact.

[0158] ;

[0159] ;

[0160] ;

[0161] ;

[0162] ;

[0163] in, Forward search distance, The discrete reference path obtained from the planning; Let i be the i-th reference point on the reference path; The forward target point in the Pure-Pursuit algorithm; This indicates the path point of the index within the reference path; For angular error, The function limits the angle to Avoid abrupt changes between them; To start from the current position Point to forward target point ; It is a saturation function, limiting the maximum speed to a safe range; This is the acceleration proportionality coefficient; For heading angle; Set the initial minimum speed to ensure the speed is not zero and avoid stagnation.

[0164] In this embodiment, surface positioning primarily uses UWB, with IMU and Odom providing short-term constraints; underwater positioning primarily uses IMU and Odom, with IMU providing attitude reference and Odom constraining planar motion; the depth sensor is responsible for environment switching determination; the fusion module... The system automatically adjusts the ratio of above-water / underwater positioning to achieve dynamic soft switching; the continuous compensation module ensures smooth connection of trajectories before and after switching; and the navigation module uses fused pose for path tracking to achieve closed-loop control.

[0165] The steps S1-S8 above constitute the complete positioning fusion link of this embodiment, from multi-source data acquisition → preprocessing and noise modeling → dual-source filtering construction → environment identification → soft handover → continuous compensation and bias feedback. First, synchronous acquisition of multi-source sensor data such as IMU, Odom, UWB, and depth is completed. Then, noise modeling and measurement quality improvement are achieved through zero-bias estimation, scale correction, variance mapping, and Kalman filtering. Based on this, a filtering framework for dynamic adjustment of process noise / measurement noise with reliability is established, providing a foundation for adaptive fusion. Then, two sets of EKF positioning models are constructed, one for surface and one for underwater, and independent estimation of the two sources is achieved through state equations and measurement equations. To automatically select the appropriate model when crossing the water surface, the environmental state parameter S is calculated using depth changes, UWB signal quality, and IMU dynamic characteristics, enabling real-time discrimination of the surface / underwater environment. Based on this indicator, a soft handover weight is introduced to... A smooth transition can be achieved near the water surface, avoiding abrupt changes. Considering that soft handover may still introduce slight cumulative deviations, a continuous compensation and anchor feedback mechanism is used to fine-tune the underwater model using high-reliability output from the surface, ensuring the continuity and consistency of the overall attitude output. Finally, the fused pose obtained after soft handover fusion and deviation correction is used as the input for navigation control.

[0166] The method described in this embodiment can be deployed on ship cleaning robots. It does not rely on GPS or lidar environments and can achieve continuous navigation and positioning even in environments without maps or with weak signals. It is suitable for various tasks such as port cleaning, underwater inspection, and surface maintenance. Figures 3-4 As shown, the ship cleaning robot specifically includes:

[0167] (1) The hull structure module consists of a waterproof shell, a buoyancy adjustment device, a propulsion mechanism, and a cleaning component. The buoyancy adjustment device is used to achieve smooth switching between the above-water and underwater environments, the propulsion mechanism provides heading and speed control, and the cleaning component completes the brushing and adsorption of dirt on the hull surface.

[0168] (2) Sensor module: including inertial measurement unit (IMU), odometer (Odom), ultra-wideband positioning module (UWB) and depth sensor (i.e., depth sensor). The IMU is used to provide acceleration and angular velocity information; Odom is used to measure the motion distance of the propulsion device; UWB is used to calculate the absolute position on the water; depth sensor is used to identify whether the current environment is underwater.

[0169] (3) Main control and fusion module: The embedded computing platform is used to run data fusion algorithms, environmental status identification, soft switching logic and error compensation mechanism, etc.

[0170] (4) Navigation and control module: Calculate the path tracking error based on the fused robot pose, and output linear velocity and angular velocity commands using the Pure-Pursuit algorithm or other nonlinear control algorithms.

[0171] In this embodiment, experiments were conducted using both surface and underwater Fusion positioning (Fusion positioning refers to the planar tracking technology in Da Vinci Fusion) and Model positioning (a positioning method that relies on Odom+IMU for state estimation, with pure calculation and no measurement correction). In the underwater environment, due to the lack of UWB measurement compensation, Model positioning relies entirely on odometry calculation, so the error will gradually accumulate over time, and its accuracy is lower than that of Fusion positioning.

[0172] like Figure 5 As shown, the surface fusion segment positioning results (UWB+Odom+IMU) provide stable and accurate path tracking. While the underwater fusion segment positioning results (UWB+Odom+IMU) are slightly less accurate, they still maintain high accuracy. In contrast, the actual underwater model segment positioning results (Odom+IMU) have a larger path error, but this error gradually decreases and approaches the underwater fusion segment positioning results as the experiment progresses. This process indicates that the surface / underwater fusion segment positioning results, acting as anchor points, periodically correct the underwater model segment positioning results (Odom+IMU) in practical applications. Through a soft handover mechanism, the underwater positioning gradually aligns with the surface positioning, improving accuracy. Simultaneously, the red jitter in the transition region demonstrates the hard handover effect of the transition zone, while the soft handover effectively suppresses this fluctuation, maintaining a smooth path transition.

[0173] Example 2

[0174] This embodiment provides a combined surface and underwater positioning and navigation system for a ship cleaning robot, including:

[0175] The positioning module is configured to obtain surface positioning results and underwater positioning results based on multi-source observation data from both above and below water.

[0176] The switching parameter determination module is configured to calculate the current environmental state parameters, and then combine these with the water depth and water level calibration values ​​to calculate the soft handover weight.

[0177] The correction module is configured to use the surface positioning results as anchor points and the current environmental state parameters as judgment conditions to adjust the underwater multi-source observation data, thereby updating the underwater positioning results;

[0178] The joint positioning module is configured to weight the latest surface positioning results and underwater positioning results according to the soft handover weights to obtain the joint positioning result;

[0179] The navigation module is configured to determine the position deviation based on the latest surface and underwater positioning results, compensate the joint positioning results according to the linear time factor, obtain the final positioning result, and perform navigation control based on the final positioning result.

[0180] It should be noted that the above modules correspond to the steps described in Embodiment 1, and the examples and application scenarios implemented by the above modules and the corresponding steps are the same, but are not limited to the content disclosed in Embodiment 1. It should also be noted that the above modules, as part of the system, can be executed in a computer system such as a set of computer-executable instructions.

[0181] In further embodiments, the following is also provided:

[0182] An electronic device includes a memory and a processor, as well as computer instructions stored in the memory and running on the processor, wherein the computer instructions, when executed by the processor, perform the method described in Embodiment 1. For brevity, further details are omitted here.

[0183] It should be understood that in this embodiment, the processor can be a central processing unit (CPU), or it can be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or any conventional processor, etc.

[0184] Memory may include read-only memory and random access memory, and provides instructions and data to the processor. A portion of memory may also include non-volatile random access memory. For example, memory may also store information about the device type.

[0185] A computer-readable storage medium for storing computer instructions, which, when executed by a processor, perform the method described in Embodiment 1.

[0186] The method in Example 1 can be directly implemented by a hardware processor, or implemented by a combination of hardware and software modules within the processor. The software modules can reside in readily available storage media in the field, such as random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, or registers. This storage medium is located in memory, and the processor reads information from the memory and, in conjunction with its hardware, completes the steps of the above method. To avoid repetition, a detailed description is not provided here.

[0187] A computer program product includes a computer program that, when executed by a processor, implements the method described in Embodiment 1.

[0188] The present invention also provides at least one computer program product tangibly stored on a non-transitory computer-readable storage medium. The computer program product includes computer-executable instructions, such as instructions included in program modules, which execute in a device on a target real or virtual processor to perform the processes / methods described above. Typically, program modules include routines, programs, libraries, objects, classes, components, data structures, etc., that perform specific tasks or implement specific abstract data types. In various embodiments, the functionality of program modules can be combined or divided among program modules as needed. The machine-executable instructions for the program modules can execute within a local or distributed device. In a distributed device, the program modules can reside in both local and remote storage media.

[0189] The computer program code used to implement the methods of the present invention may be written in one or more programming languages. This computer program code may be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, such that when executed by the computer or other programmable data processing device, the program code causes the functions / operations specified in the flowcharts and / or block diagrams to be implemented. The program code may be executed entirely on a computer, partially on a computer, as a stand-alone software package, partially on a computer and partially on a remote computer, or entirely on a remote computer or server.

[0190] In the context of this invention, computer program code or related data may be carried by any suitable carrier to enable a device, apparatus, or processor to perform the various processes and operations described above. Examples of carriers include signals, computer-readable media, and the like. Examples of signals may include electrical, optical, radio, sound, or other forms of propagation signals, such as carrier waves, infrared signals, etc.

[0191] Those skilled in the art will recognize that the units and algorithm steps described in connection with the various examples of this embodiment can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this invention.

[0192] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A method for positioning and navigating a ship cleaning robot on and under water, characterized in that, The method comprises the following steps: According to the multi-source observation data on water and underwater, the water positioning result and the underwater positioning result are obtained respectively; The current environmental state parameter is calculated, and then the soft switching weight is calculated in combination with the water depth and the water level calibration value; The underwater multi-source observation data is adjusted by taking the water positioning result as an anchor point and the current environmental state parameter as a judgment condition, so as to update the underwater positioning result; The latest water positioning result and underwater positioning result are weighted according to the soft switching weight, and the joint positioning result is obtained; The position deviation is determined according to the latest water positioning result and underwater positioning result, and the joint positioning result is compensated according to the linear time factor to obtain the final positioning result, and the navigation control is performed according to the final positioning result.

2. A method of positioning and navigating a ship cleaning robot on and under water as claimed in claim 1, characterized in that The process of water and underwater positioning comprises: A noise model based on the measurement noise covariance matrix and the process noise covariance matrix is constructed; The multi-source observation data is taken as a state vector, and then the measurement vector is defined in combination with the underwater measurement noise; After the state is updated by using the extended Kalman filter according to the measurement vector and the noise model, the plane pose component is taken as the water positioning result and the underwater positioning result.

3. The method of claim 1, wherein, The environmental state parameter S is: ； ； wherein, is a weight coefficient, h0 is a water line calibration value, is a short window variance of IMU sensor data, is an IMU maximum dynamic threshold, is a maximum depth deviation threshold; is a water depth at time k; is a signal-to-noise ratio of the UWB measurement signal at time k; are a maximum signal-to-noise ratio and a minimum signal-to-noise ratio, respectively.

4. The method of claim 1, wherein, Soft handoff weights To; ； ； Joint positioning results are ; Position deviation is ; Final positioning result is ; wherein, is the water depth at time k, is the water level calibration value; is the curve steepness coefficient; is the base steepness; is the adaptive gain, S is the environmental state parameter; is the bias mean of multiple time points in the anchor point window A; is the water positioning result at time k; is the underwater positioning result at time k; is the linear time factor at time t, T is the total time.

5. The method of claim 1, wherein, The process of adjusting the underwater multi-source observation data comprises: ； ； wherein, is the adjustment step size; is the true angular velocity and true acceleration back-propagated from the water-based positioning result; is the IMU gyroscope bias and acceleration bias; is the current Odom scale factor; is the scale bias, is the real distance increment extracted in the fusion of poses in the anchor point window, is the distance measured by the odometer in the same window.

6. The method of claim 1, wherein, The process of navigation control according to the final positioning result includes: calculating a line speed unit time control amount between the final positioning result and a reference path with the current position The process of navigation control according to the final positioning result includes: calculating a line speed unit time control amount between the final positioning result and a reference path with the current position The process of navigation control according to the final positioning result includes: calculating a line speed unit time control amount between the final positioning result and a reference path with the current position The process of navigation control according to the final positioning result includes: calculating a line speed unit ； ； ； wherein is the look-ahead distance; is the i-th reference point on the reference path; is the angle error, the function limits the angle to between; is the saturation function; is the acceleration proportionality coefficient; is the initial minimum speed, is the plan position component, is the heading angle component.

7. A ship cleaning robot water-over-water and water-under-water combined positioning and navigation system, characterized in that, The method comprises the following steps: The positioning module is configured to obtain the water positioning result and the underwater positioning result according to the multi-source observation data on water and underwater respectively; The switching parameter determination module is configured to calculate the current environmental state parameter, and then calculate the soft switching weight in combination with the water depth and the water level calibration value; The correction module is configured to adjust the underwater multi-source observation data by taking the water positioning result as an anchor point and the current environmental state parameter as a judgment condition, so as to update the underwater positioning result; The joint positioning module is configured to weight the latest water positioning result and underwater positioning result according to the soft switching weight, and obtain the joint positioning result; The navigation module is configured to determine the position deviation according to the latest water positioning result and underwater positioning result, and compensate the joint positioning result according to the linear time factor to obtain the final positioning result, and perform navigation control according to the final positioning result.

8. An electronic device, comprising: The computer program product comprises a memory and a processor, and computer instructions stored in the memory and executed on the processor, when the computer instructions are executed by the processor, the method of any one of claims 1-6 is completed.

9. A computer-readable storage medium, characterized in that, The computer program product comprises a memory and a processor, and computer instructions stored in the memory and executed on the processor, when the computer instructions are executed by the processor, the method of any one of claims 1-6 is completed.

10. A computer program product, characterised in that, The computer program product comprises a memory and a processor, and computer instructions stored in the memory and executed on the processor, when the computer instructions are executed by the processor, the method of any one of claims 1-6 is completed.

Images