A hull cleaning robot relative positioning method based on multi-source fusion and curved surface gradient correction

Abstract

The application discloses a hull cleaning robot relative positioning method based on multi-source fusion and curved surface gradient correction, belongs to the positioning technology field of cleaning robots, and comprises the following steps: establishing a mapping relationship between an attitude angle and a hull curved surface gradient by using a relative basic positioning result in a local coordinate system, completing gradient constraint correction, and outputting a gradient correction positioning result of a hull cleaning robot body coordinate system; comprehensively utilizing the relative basic positioning result of the hull cleaning robot in the local coordinate system and the gradient correction positioning result of the hull cleaning robot body coordinate system, constructing an adaptive weight position fusion model, and outputting a relative positioning result of the hull cleaning robot relative to an initial original point. The application can better adapt to the positioning requirements of long-time hull cleaning operation, the overall scheme does not rely on external positioning equipment such as acoustic navigation, is simple to deploy, is not disturbed by cleaning operation vibration and water flow noise, and the hardware cost and on-site layout difficulty are effectively reduced.

Description

Technical Field

[0001] This invention discloses a relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction, belonging to the field of cleaning robot positioning technology. Background Technology

[0002] With the continuous development of global maritime transport and the shipbuilding industry, ships have become the core carriers of international trade and ocean-going operations. During long voyages and anchorages, the hull is constantly submerged in seawater, making it highly susceptible to the adhesion of marine organisms such as algae, shellfish, and barnacles, resulting in severe biofouling. Hull fouling significantly increases surface roughness and navigation resistance, leading to increased fuel consumption and operating costs, while also exacerbating carbon emissions, which is inconsistent with the requirements of green and low-carbon shipping development. Furthermore, biofouling accelerates hull structural corrosion and aging, shortens ship lifespan, increases maintenance and dry-docking costs, and can even affect ship maneuverability and navigation safety. Therefore, conducting regular, efficient, and safe cleaning operations on the hull surface to promptly remove attached organisms and fouling is a crucial aspect of daily ship maintenance, and is of great significance for improving navigation efficiency, reducing operating costs, extending ship lifespan, ensuring navigation safety, and promoting the development of green shipping.

[0003] Compared to traditional manual cleaning operations by divers, using ship cleaning robots can effectively improve the efficiency and standardization of ship surface cleaning. These robots can operate continuously while the ship is docked in port, avoiding the safety hazards associated with manual underwater operations, and do not disrupt normal ship operations. To ensure stable and reliable path tracking, comprehensive cleaning coverage, and safe recovery of the robot, accurately obtaining the robot's real-time position relative to the ship has become a core prerequisite for the successful completion of cleaning operations.

[0004] Currently, commonly used positioning technologies for ship cleaning robots mainly include three categories: inertial navigation, acoustic navigation, and dead reckoning. Inertial navigation relies on inertial sensors to autonomously calculate pose without external information support, possessing strong environmental adaptability. However, it inherently has fixed errors that accumulate over time, making it difficult to meet the accuracy requirements of long-term cleaning operations. Acoustic navigation relies on underwater acoustic signals for positioning. While it offers high accuracy in some scenarios, it suffers from high equipment costs, complex installation and deployment, and is susceptible to interference from vibrations, water flow, and noise generated during ship cleaning, making stability difficult to guarantee. Dead reckoning calculates the robot's motion state using Doppler velocimetry (DVL) and attitude measurement modules. Its algorithms are simple and low-cost, but it also suffers from error accumulation. To overcome the shortcomings of single methods, the industry often employs multi-source information fusion positioning, which is primarily based on filtering algorithms to balance errors. While this can improve positioning performance to some extent, it generally does not fully incorporate prior geometric information about the ship's curved surfaces, failing to effectively suppress positioning drift from a geometric constraint perspective. Therefore, it still struggles to guarantee positioning stability under strong interference and long-term operation scenarios. Summary of the Invention

[0005] The purpose of this invention is to provide a relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction, so as to solve the problem in the prior art that it is difficult to achieve low-cost, high-stability, and low-drift relative positioning in ship hull cleaning scenarios with complex curved surfaces, strong interference, and no external auxiliary equipment.

[0006] A relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction is disclosed. The ship hull cleaning robot is equipped with multiple sensing units. Starting from an initial origin, the multiple sensing units perform real-time observation in a time-synchronized state. The multiple sensing units include an inertial navigation system, an electronic compass, an odometer, and a depth gauge. The relative positioning method of the hull cleaning robot includes S1, using the final relative positioning result of the previous real-time observation cycle as the initial value, constructing a multi-source fusion nonlinear state equation and observation equation using the observation data of the sensing unit, solving it using an extended Kalman filter (EKF), and outputting the relative basic positioning result of the hull cleaning robot in the local coordinate system. The state equation is: ; In the formula, for Predicted values ​​of the time error covariance matrix. for The Jacobian matrix of state transition at each time step. for The optimal estimate of the time error covariance matrix. Indicates matrix transpose. It is the process noise covariance matrix; The observation equation is: ; In the formula, It is obtained from the state update. The optimal state at any given moment. for Predicted system state vector at time 10:00. for The observation vector at time t, yes Moment-time Kalman gain, It is the observation matrix; S2. Using the relative basic positioning results in the local coordinate system, establish the mapping relationship between the attitude angle and the gradient change rate of the hull surface. Combine the gradient change rate of the hull surface, the final relative positioning result of the previous real-time observation cycle, and the displacement information of the odometer to complete the gradient constraint correction and output the gradient correction positioning result in the body coordinate system of the hull cleaning robot. S3. By comprehensively utilizing the relative basic positioning results of the hull cleaning robot in the local coordinate system and the gradient correction positioning results in the body coordinate system of the hull cleaning robot, an adaptive weighted position fusion model is constructed to output the relative positioning results of the hull cleaning robot relative to the initial origin. S3 includes defining the spatial deviation of the dual-path fusion output. As a benchmark for adjusting adaptive weights: ; In the formula, for The spatial Euclidean distance deviation between the two positioning paths in the XZ plane at any given time. and yes The time relative to the baseline positioning result, yes Positioning results on the X-axis after time gradient correction yes Positioning results on the Z-axis after time gradient correction; An adaptive weighting strategy based on an exponential decay function is designed to allocate the acceptance ratio of the two positioning paths in real time, and gradient constraints are used to correct the fusion weights of the positioning results. for: ; In the formula, It is the maximum fusion weight of the gradient correction path. It is the weight sensitivity coefficient. Perform location result fusion: ; Output final positioning coordinates .

[0007] The inertial navigation system outputs the three-axis acceleration in the body coordinate system of the hull cleaning robot. Triaxial angular velocity Attitude angle ,speed and location ; ; ; ; ; ; In the formula, yes The three components on the three axes yes The three components on the three axes yes The three components on the three axes yes The three components on the three axes yes Three components on the three axes; Electronic compass output roll angle Pitch angle Heading angle Depth gauge outputs working depth The odometer outputs the displacement increment of the hull cleaning robot. forward speed angular velocity of the four drive wheels , , , ; Real-time observation data from multiple sensing units are unified into the coordinate system of the ship cleaning robot.

[0008] S1 includes S1.1, defining a local coordinate system, with the initial origin of the hull cleaning robot as the origin of the local coordinate system. The X-axis is along the length of the hull, consistent with the robot's initial working direction, pointing towards the bow direction as positive. The Y-axis is the depth direction, perpendicular to the hull surface, pointing downwards towards the hull shell as positive. The Z-axis is along the width of the hull, perpendicular to the length of the hull, pointing towards the port side of the hull as positive. Build System state vector at time step : ; In the formula, for The relative three-dimensional position of the point at time in the local coordinate system with respect to the initial origin. , , for The roll angle, pitch angle, and yaw angle of the ship's hull cleaning robot are constantly monitored. for The three-dimensional velocity components in the body coordinate system at any given time.

[0009] S1 includes S1.2. The initial time is based on the observations of each sensing unit, and the state vector and error covariance matrix are initialized. The initial position three-dimensional coordinates corresponding to the initial origin are also initialized. for: ; In the formula, These are the initial observations from the depth gauge; Initial attitude angle for: ; In the formula, These are the initial roll, pitch, and yaw angles. and These are the roll and pitch angles output by the inertial navigation system at the initial moment. initial velocity for: ; In the formula, The initial forward velocity of the odometer; The initial covariance matrix is ​​set as a diagonal matrix. The noise covariance matrix of the process is set according to the accuracy of each sensing unit. Observation noise covariance matrix .

[0010] S1 includes S1.3, based on optimal state at time 1 , combined The three-axis angular velocity and three-axis acceleration of the inertial navigation system at all times are predicted using a nonlinear motion model. The state at any given moment; S1.3.1, Attitude angle prediction is: ; In the formula, for Attitude angle prediction at time step for The optimal estimate of the attitude angle at time step. The three-axis angular velocities of the hull cleaning robot in the body coordinate system are output by the inertial navigation system. The sampling time interval; S1.3.2. Velocity prediction in the body coordinate system of the ship cleaning robot: ; In the formula, for Predicted 3D velocity values ​​of the ship hull cleaning robot in its body coordinate system at any given time. for The optimal estimate of the three-dimensional velocity of the ship hull cleaning robot in its body coordinate system at any given time. for The three-axis acceleration of the hull cleaning robot in the body coordinate system is output by the inertial navigation system at all times.

[0011] S1 includes S1.3.3, through the attitude rotation matrix. Will The optimal estimate of the three-dimensional velocity of the hull cleaning robot in the body coordinate system is converted to the velocity in the local coordinate system, and then integrated to obtain the velocity. Relative position increment at time: ; The relative position prediction equation is: ; In the formula, for Three-dimensional velocity components in the local coordinate system at any given time. for The final three-dimensional coordinate values ​​output by dual-path fusion in the local coordinate system at any given time. for The relative three-dimensional position prediction value in the local coordinate system at any time, the dual-path fusion output is the fusion of the relative basic positioning result of the hull cleaning robot in the local coordinate system and the gradient correction positioning result in the body coordinate system of the hull cleaning robot; ; ; ; ; ; ; ; ; ; ; In the formula, , , , , , , , , It is the attitude rotation matrix The 9 elements; S1.3.4. Predict the error covariance matrix to characterize the uncertainty of the predicted state value, and propagate the estimation error from the previous time step to the current time step in conjunction with the state equation. The state transition Jacobian matrix at time step is: ; In the formula, It is the partial derivative symbol.

[0012] S1 includes S1.4, completion After predicting the state and covariance at time, the base The observation data output from the electronic compass, odometer, and depth gauge are used in S1.3. The state prediction value and covariance prediction value at time 1 are corrected to obtain the optimal state estimate and error covariance estimate at the current time. S1.4.1, Observation Vector for: ; ; In the formula, for The roll angle output by the electronic compass at all times. for The pitch angle output by the electronic compass at all times. for The heading angle output by the electronic compass at all times. for The working depth output by the time-of-flight depth gauge. for The robot's forward speed is output by the odometry timer. The radius of the drive wheels for the hull cleaning robot. for The angular velocities of the four drive wheels at any given moment; S1.4.2 Establish the mapping relationship between the observations and the state vector to obtain the observation matrix. : ; S1.4.3, Calculation Time Kalman Gain : ; In the formula, for Predicted values ​​of the time error covariance matrix; To observe the noise covariance matrix; S1.4.4, State update obtained optimal state at any time : S1.4.5, Perform covariance update: ; In the formula, for The optimal estimate of the time error covariance matrix. It is a 9th-order identity matrix.

[0013] S1 includes S1.5, output The hull cleaning robot in the local coordinate system ,Will and This serves as the basic relative positioning result of the hull cleaning robot in the local coordinate system.

[0014] S2 includes S2.1, obtaining Optimal estimate of attitude angle at time t : ; Calculate the rate of change of the hull surface gradient: ; In the formula, for The rate of change of the hull surface gradient in the Y direction with respect to the X direction at time . for The rate of change of the hull surface gradient in the Y direction with respect to the Z direction at time t; S2.2 Calculate the displacement increment in each direction: ; ; ; ; In the formula, yes Time relative to The displacement increment in the Y direction at time t. yes Time relative to The displacement increment in the X direction at time 1. yes Time relative to The displacement increment in the Z direction at time t. yes Time relative to The displacement distance increment of the odometer at any given time. yes The distance traveled by the odometer at any given time; S2.3. Position correction is performed. Based on the definition of the total differential of a multivariable function, the following relationship exists: ; use right Perform corrections and combine Final coordinates of the moment Solving for the given information yields the following results. and : ; ; ; In the formula, It is the corrected version Output Gradient correction positioning results of the ship hull cleaning robot in the local coordinate system .

[0015] Compared with existing technologies, this invention has the following advantages: It introduces geometric gradient features of the hull surface to assist in positioning correction, which makes up for the shortcomings of traditional methods that rely solely on sensor fusion without utilizing prior information of the hull surface and have insufficient error accumulation suppression under complex surface conditions. This effectively improves the accuracy and operational stability of the robot's relative positioning on the complex surface of the hull, and can better adapt to the positioning requirements of long-term cleaning operations on the hull. Moreover, the overall solution does not require external positioning equipment such as acoustic navigation, is simple to deploy, and is not affected by vibrations and water noise during cleaning operations. The hardware cost and on-site deployment difficulty are effectively reduced. Attached Figure Description

[0016] Figure 1 This is an overall flowchart of the present invention;

[0017] Figure 2 This is a flowchart of the surface gradient correction process of the present invention. Detailed Implementation

[0018] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention are described clearly and completely below. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0019] A relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction is disclosed. The ship hull cleaning robot is equipped with multiple sensing units. Starting from an initial origin, the multiple sensing units perform real-time observation in a time-synchronized state. The multiple sensing units include an inertial navigation system, an electronic compass, an odometer, and a depth gauge. The relative positioning method of the hull cleaning robot includes S1, using the final relative positioning result of the previous real-time observation cycle as the initial value, constructing a multi-source fusion nonlinear state equation and observation equation using the observation data of the sensing unit, solving it using an extended Kalman filter (EKF), and outputting the relative basic positioning result of the hull cleaning robot in the local coordinate system. The state equation is: ; In the formula, for Predicted values ​​of the time error covariance matrix. for The Jacobian matrix of state transition at each time step. for The optimal estimate of the time error covariance matrix. Indicates matrix transpose. It is the process noise covariance matrix; The observation equation is: ; In the formula, It is obtained from the state update. The optimal state at any given moment. for Predicted system state vector at time 10:00. for The observation vector at time t, yes Moment-time Kalman gain, It is the observation matrix; S2. Using the relative basic positioning results in the local coordinate system, establish the mapping relationship between the attitude angle and the gradient change rate of the hull surface. Combine the gradient change rate of the hull surface, the final relative positioning result of the previous real-time observation cycle, and the displacement information of the odometer to complete the gradient constraint correction and output the gradient correction positioning result in the body coordinate system of the hull cleaning robot. S3. By comprehensively utilizing the relative basic positioning results of the hull cleaning robot in the local coordinate system and the gradient correction positioning results in the body coordinate system of the hull cleaning robot, an adaptive weighted position fusion model is constructed to output the relative positioning results of the hull cleaning robot relative to the initial origin. S3 includes defining the spatial deviation of the dual-path fusion output. As a benchmark for adjusting adaptive weights: ; In the formula, for The spatial Euclidean distance deviation between the two positioning paths in the XZ plane at any given time. and yes The time relative to the baseline positioning result, yes Positioning results on the X-axis after time gradient correction yes Positioning results on the Z-axis after time gradient correction; An adaptive weighting strategy based on an exponential decay function is designed to allocate the acceptance ratio of the two positioning paths in real time, and gradient constraints are used to correct the fusion weights of the positioning results. for: ; In the formula, It is the maximum fusion weight of the gradient correction path. It is the weight sensitivity coefficient. Perform location result fusion: ; Output final positioning coordinates .

[0020] The inertial navigation system outputs the three-axis acceleration in the body coordinate system of the hull cleaning robot. Triaxial angular velocity Attitude angle ,speed and location ; ; ; ; ; ; In the formula, yes The three components on the three axes yes The three components on the three axes yes The three components on the three axes yes The three components on the three axes yes Three components on the three axes; Electronic compass output roll angle Pitch angle Heading angle Depth gauge outputs working depth The odometer outputs the displacement increment of the hull cleaning robot. forward speed angular velocity of the four drive wheels , , , ; Real-time observation data from multiple sensing units are unified into the coordinate system of the ship cleaning robot.

[0021] S1 includes S1.1, defining a local coordinate system, with the initial origin of the hull cleaning robot as the origin of the local coordinate system. The X-axis is along the length of the hull, consistent with the robot's initial working direction, pointing towards the bow direction as positive. The Y-axis is the depth direction, perpendicular to the hull surface, pointing downwards towards the hull shell as positive. The Z-axis is along the width of the hull, perpendicular to the length of the hull, pointing towards the port side of the hull as positive. Build System state vector at time step : ; In the formula, for The relative three-dimensional position of the point at time in the local coordinate system with respect to the initial origin. , , for The roll angle, pitch angle, and yaw angle of the ship's hull cleaning robot are constantly monitored. for The three-dimensional velocity components in the body coordinate system at any given time.

[0022] S1 includes S1.2. The initial time is based on the observations of each sensing unit, and the state vector and error covariance matrix are initialized. The initial position three-dimensional coordinates corresponding to the initial origin are also initialized. for: ; In the formula, These are the initial observations from the depth gauge; Initial attitude angle for: ; In the formula, These are the initial roll, pitch, and yaw angles. and These are the roll and pitch angles output by the inertial navigation system at the initial moment. initial velocity for: ; In the formula, The initial forward velocity of the odometer; The initial covariance matrix is ​​set as a diagonal matrix. The noise covariance matrix of the process is set according to the accuracy of each sensing unit. Observation noise covariance matrix .

[0023] S1 includes S1.3, based on optimal state at time 1 , combined The three-axis angular velocity and three-axis acceleration of the inertial navigation system at all times are predicted using a nonlinear motion model. The state at any given moment; S1.3.1, Attitude angle prediction is: ; In the formula, for Attitude angle prediction at time step for The optimal estimate of the attitude angle at time step. The three-axis angular velocities of the hull cleaning robot in the body coordinate system are output by the inertial navigation system. The sampling time interval; S1.3.2. Velocity prediction in the body coordinate system of the ship cleaning robot: ; In the formula, for Predicted 3D velocity values ​​of the ship hull cleaning robot in its body coordinate system at any given time. for The optimal estimate of the three-dimensional velocity of the ship hull cleaning robot in its body coordinate system at any given time. for The three-axis acceleration of the hull cleaning robot in the body coordinate system is output by the inertial navigation system at all times.

[0024] S1 includes S1.3.3, through the attitude rotation matrix. Will The optimal estimate of the three-dimensional velocity of the hull cleaning robot in the body coordinate system is converted to the velocity in the local coordinate system, and then integrated to obtain the velocity. Relative position increment at time: ; The relative position prediction equation is: ; In the formula, for Three-dimensional velocity components in the local coordinate system at any given time. for The final three-dimensional coordinate values ​​output by dual-path fusion in the local coordinate system at any given time. for The relative three-dimensional position prediction value in the local coordinate system at any time, the dual-path fusion output is the fusion of the relative basic positioning result of the hull cleaning robot in the local coordinate system and the gradient correction positioning result in the body coordinate system of the hull cleaning robot; ; ; ; ; ; ; ; ; ; ; In the formula, , , , , , , , , It is the attitude rotation matrix The 9 elements; S1.3.4. Predict the error covariance matrix to characterize the uncertainty of the predicted state value, and propagate the estimation error from the previous time step to the current time step in conjunction with the state equation. The state transition Jacobian matrix at time step is: ; In the formula, It is the partial derivative symbol.

[0025] S1 includes S1.4, completion After predicting the state and covariance at time, the base The observation data output from the electronic compass, odometer, and depth gauge are used in S1.3. The state prediction value and covariance prediction value at time 1 are corrected to obtain the optimal state estimate and error covariance estimate at the current time. S1.4.1, Observation Vector for: ; ; In the formula, for The roll angle output by the electronic compass at all times. for The pitch angle output by the electronic compass at all times. for The heading angle output by the electronic compass at all times. for The working depth output by the time-of-flight depth gauge. for The robot's forward speed is output by the odometry timer. The radius of the drive wheels for the hull cleaning robot. for The angular velocities of the four drive wheels at any given moment; S1.4.2 Establish the mapping relationship between the observations and the state vector to obtain the observation matrix. : ; S1.4.3, Calculation Time Kalman Gain : ; In the formula, for Predicted values ​​of the time error covariance matrix; To observe the noise covariance matrix; S1.4.4, State update obtained optimal state at any time : S1.4.5, Perform covariance update: ; In the formula, for The optimal estimate of the time error covariance matrix. It is a 9th-order identity matrix.

[0026] S1 includes S1.5, output The hull cleaning robot in the local coordinate system ,Will and This serves as the basic relative positioning result of the hull cleaning robot in the local coordinate system.

[0027] S2 includes S2.1, obtaining Optimal estimate of attitude angle at time t : ; Calculate the rate of change of the hull surface gradient: ; In the formula, for The rate of change of the hull surface gradient in the Y direction with respect to the X direction at time . for The rate of change of the hull surface gradient in the Y direction with respect to the Z direction at time t; S2.2 Calculate the displacement increment in each direction: ; ; ; ; In the formula, yes Time relative to The displacement increment in the Y direction at time t. yes Time relative to The displacement increment in the X direction at time 1. yes Time relative to The displacement increment in the Z direction at time t. yes Time relative to The displacement distance increment of the odometer at any given time. yes The distance traveled by the odometer at any given time; S2.3. Position correction is performed. Based on the definition of the total differential of a multivariable function, the following relationship exists: ; use right Perform corrections and combine Final coordinates of the moment Solving for the given information yields the following results. and : ; ; ; In the formula, It is the corrected version Output Gradient correction positioning results of the ship hull cleaning robot in the local coordinate system .

[0028] The existing positioning methods for various ship hull cleaning robots generally suffer from the following defects in practical applications, and these are all core problems that the technical solution of this invention aims to solve.

[0029] Both inertial navigation and dead reckoning suffer from errors that accumulate and diverge over time, making it impossible to maintain accuracy for extended periods. Inertial navigation relies on integral calculations of position and attitude, and inherent sensor drift continues to accumulate. Neither method can fundamentally suppress the continuous increase in errors, making it difficult to meet the positioning requirements of long-term, large-scale cleaning operations.

[0030] Acoustic navigation is susceptible to interference from cleaning vibrations, water flow, and noise, resulting in poor stability and applicability. The cleaning operation of the hull is accompanied by the rotation of the roller brush, the adsorption of the hull, and the vibration of walking, which can easily cause strong interference to the acoustic signal, leading to abnormal positioning data. At the same time, acoustic equipment is expensive and cumbersome to deploy, making it difficult to widely use in scenarios such as docking and temporary cleaning.

[0031] Existing multi-source fusion positioning relies solely on filtering algorithms to balance errors, without utilizing the geometric constraints of the ship's curved surface. Current fusion schemes only use filtering algorithms to weight and balance data from multiple sensors, without introducing the inherent geometric features of the ship's three-dimensional curved surface structure as a positioning auxiliary constraint. This fails to reduce the error range of the positioning results from a physical perspective, resulting in only partial mitigation of the drift problem and making it difficult to meet the high-precision positioning requirements in complex curved surface scenarios.

[0032] In situations with no external equipment, strong interference, and complex curved surfaces, it is difficult to balance low cost and positioning stability. High-precision positioning solutions often rely on high-end sensors or external auxiliary equipment, resulting in complex systems and high costs. Low-cost solutions are susceptible to error accumulation and environmental interference, and their accuracy and stability are insufficient, failing to meet the core requirements of "accurate accuracy, stable reliability, and controllable cost" in ship cleaning scenarios.

[0033] This invention indirectly calculates and obtains the surface gradient features of the hull in real time based on the collected and fused body attitude angles, overcoming the limitation of traditional positioning schemes that completely ignore the prior geometric information of the hull's inherent curved surfaces. It combines the acquired surface gradient information with odometry motion information, and uses the surface gradient constraint relationship to calculate and correct the position, achieving geometric feature-assisted positioning correction. By adding a surface gradient geometric constraint correction step to the conventional multi-sensor fusion, it overcomes the shortcomings of relying solely on sensor filtering fusion to suppress the accumulation of positioning errors and its poor adaptability to curved surfaces, thus improving the relative positioning accuracy and stability under complex hull curved surfaces. The technical process of this invention is as follows: Figure 1 As shown, the surface gradient correction process is as follows: Figure 2 As shown, during the calculation of the local gradient of the curved surface, the robot is tightly attached to the hull surface, and the Z-axis of the body coordinate system coincides completely with the normal vector of the hull surface at that point. Therefore, there is a one-to-one correspondence between the attitude angle and the depth gradient of the curved surface.

[0034] The unit vector of the Z-axis in the body coordinate system is Transform it to a local coordinate system to obtain the unit normal vector of the hull surface at that point. .

[0035] ;

[0036] Among them, the attitude rotation matrix from the body coordinate system to the local coordinate system Depend on The attitude angle at time step is calculated as follows:

[0037] ;

[0038] After expansion, we get the three components of the normal vector:

[0039] ;

[0040] In the formula, for The components of the unit normal vector of the hull surface at any given time on the X, Y, and Z axes of the local coordinate system.

[0041] For the curved surface of the hull Its unit normal vector and gradient have a strict mathematical mapping relationship, and the gradient can also be directly calculated from the attitude angle.

[0042] ;

[0043] In this invention, and These are standard parameters that can be adjusted according to the actual application scenario, and their value selection logic is as follows: The reliability setting for the gradient correction path is generally 0.2 to 0.5. The setting is based on the attitude noise level, typically 5 to 12. In the embodiments, The maximum fusion weight for the gradient correction path is set to 0.35 by default. Units are The default value is 8. Control the rate at which the weights decay with position deviation.

[0044] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction, characterized in that, The hull cleaning robot is equipped with multiple sensing units. Starting from the initial origin, the multiple sensing units perform real-time observation in a time-synchronized state. The multiple sensing units include an inertial navigation system, an electronic compass, an odometer, and a depth gauge. The relative positioning method of the hull cleaning robot includes S1, using the final relative positioning result of the previous real-time observation cycle as the initial value, constructing a multi-source fusion nonlinear state equation and observation equation using the observation data of the sensing unit, solving it using an extended Kalman filter (EKF), and outputting the relative basic positioning result of the hull cleaning robot in the local coordinate system. The state equation is: ； In the formula, for Predicted values ​​of the time error covariance matrix. for The Jacobian matrix of state transition at each time step. for The optimal estimate of the time error covariance matrix. Indicates matrix transpose. It is the process noise covariance matrix; The observation equation is: ； In the formula, It is obtained from the state update. The optimal state at any given moment. for Predicted system state vector value at time 10:

00. for The observation vector at time t, yes Moment Kalman gain, It is the observation matrix; S2. Using the relative basic positioning results in the local coordinate system, establish the mapping relationship between the attitude angle and the gradient change rate of the hull surface. Combine the gradient change rate of the hull surface, the final relative positioning result of the previous real-time observation cycle, and the displacement information of the odometer to complete the gradient constraint correction and output the gradient correction positioning result in the body coordinate system of the hull cleaning robot. S3. By comprehensively utilizing the relative basic positioning results of the hull cleaning robot in the local coordinate system and the gradient correction positioning results in the body coordinate system of the hull cleaning robot, an adaptive weighted position fusion model is constructed to output the relative positioning results of the hull cleaning robot relative to the initial origin. S3 includes defining the spatial deviation of the dual-path fusion output. As a benchmark for adjusting adaptive weights: ； In the formula, for The spatial Euclidean distance deviation between the two positioning paths in the XZ plane at any given time. and yes The time relative to the baseline positioning result, yes Positioning results on the X-axis after time gradient correction yes Positioning results on the Z-axis after time gradient correction; An adaptive weighting strategy based on an exponential decay function is designed to allocate the acceptance ratio of the two positioning paths in real time, and gradient constraints are used to correct the fusion weights of the positioning results. for: ； In the formula, It is the maximum fusion weight of the gradient correction path. It is the weight sensitivity coefficient. Perform location result fusion: ； Output final positioning coordinates .

2. The relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction according to claim 1, characterized in that, The inertial navigation system outputs the three-axis acceleration in the body coordinate system of the ship cleaning robot. Triaxial angular velocity Attitude angle ,speed and location ; ； ； ； ； ； In the formula, yes The three components on the three axes yes The three components on the three axes yes The three components on the three axes yes The three components on the three axes yes Three components on the three axes; Electronic compass output roll angle Pitch angle Heading angle Depth gauge outputs working depth The odometer outputs the displacement increment of the hull cleaning robot. forward speed angular velocity of the four drive wheels , , , ; Real-time observation data from multiple sensing units are unified into the coordinate system of the ship cleaning robot.

3. The relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction according to claim 1, characterized in that, S1 includes S1.1, defining a local coordinate system, with the initial origin of the hull cleaning robot as the origin of the local coordinate system. The X-axis is along the length of the hull, consistent with the robot's initial working direction, pointing towards the bow direction as positive. The Y-axis is the depth direction, perpendicular to the hull surface, pointing downwards towards the hull shell as positive. The Z-axis is along the width of the hull, perpendicular to the length of the hull, pointing towards the port side of the hull as positive. Build System state vector at time step : ； In the formula, for The relative three-dimensional position of the point at time in the local coordinate system with respect to the initial origin. , , for The roll angle, pitch angle, and yaw angle of the ship's hull cleaning robot are constantly monitored. for The three-dimensional velocity components in the body coordinate system at any given time.

4. The relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction according to claim 3, characterized in that, S1 includes S1.

2. The initial time is based on the observations of each sensing unit, and the state vector and error covariance matrix are initialized. The initial position three-dimensional coordinates corresponding to the initial origin are also initialized. for: ； In the formula, These are the initial observations from the depth gauge; Initial attitude angle for: ； In the formula, These are the initial roll, pitch, and yaw angles. and These are the roll and pitch angles output by the inertial navigation system at the initial moment. initial velocity for: ； In the formula, The initial forward velocity of the odometer; The initial covariance matrix is ​​set as a diagonal matrix. The noise covariance matrix of the process is set according to the accuracy of each sensing unit. Observation noise covariance matrix .

5. The relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction according to claim 4, characterized in that, S1 includes S1.3, based on optimal state at time 1 , combined The three-axis angular velocity and three-axis acceleration of the inertial navigation system at all times are predicted using a nonlinear motion model. The state at any given moment; S1.3.1, Attitude angle prediction is: ； In the formula, for Attitude angle prediction at time step for The optimal estimate of the attitude angle at time step. The three-axis angular velocities of the hull cleaning robot in the body coordinate system are output by the inertial navigation system. The sampling time interval; S1.3.

2. Velocity prediction in the body coordinate system of the ship cleaning robot: ； In the formula, for Predicted 3D velocity values ​​of the ship hull cleaning robot in its body coordinate system at any given time. for The optimal estimate of the three-dimensional velocity of the ship hull cleaning robot in its body coordinate system at any given time. for The three-axis acceleration of the hull cleaning robot in the body coordinate system is output by the inertial navigation system at all times.

6. The relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction according to claim 5, characterized in that, S1 includes S1.3.3, through the attitude rotation matrix. Will The optimal estimate of the three-dimensional velocity of the hull cleaning robot in the body coordinate system is converted to the velocity in the local coordinate system, and then integrated to obtain the velocity. Relative position increment at time: ； The relative position prediction equation is: ； In the formula, for Three-dimensional velocity components in the local coordinate system at any given time. for The final three-dimensional coordinate values ​​output by dual-path fusion in the local coordinate system at any given time. for The relative three-dimensional position prediction value in the local coordinate system at any time, the dual-path fusion output is the fusion of the relative basic positioning result of the hull cleaning robot in the local coordinate system and the gradient correction positioning result in the body coordinate system of the hull cleaning robot; ； ； ； ； ； ； ； ； ； ； In the formula, , , , , , , , , It is the attitude rotation matrix The 9 elements; S1.3.

4. Predict the error covariance matrix to characterize the uncertainty of the predicted state value, and propagate the estimation error from the previous time step to the current time step in conjunction with the state equation. The state transition Jacobian matrix at time step is: ； In the formula, It is the partial derivative symbol.

7. The relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction according to claim 6, characterized in that, S1 includes S1.4, completion After predicting the state and covariance at time, the base The observation data output from the electronic compass, odometer, and depth gauge are used in S1.

3. The state prediction value and covariance prediction value at time 1 are corrected to obtain the optimal state estimate and error covariance estimate at the current time. S1.4.1, Observation Vector for: ； ； In the formula, for The roll angle output by the electronic compass at all times. for The pitch angle output by the electronic compass at all times. for The heading angle output by the electronic compass at all times. for The working depth output by the time-of-flight depth gauge. for The robot's forward speed is output by the odometer at all times. The radius of the drive wheels for the hull cleaning robot. for The angular velocities of the four drive wheels at any given moment; S1.4.2 Establish the mapping relationship between the observations and the state vector to obtain the observation matrix. : ； S1.4.3, Calculation Time Kalman Gain : ； In the formula, for Predicted values ​​of the time error covariance matrix; To observe the noise covariance matrix; S1.4.4, State update obtained optimal state at any time : S1.4.5, Perform covariance update: ； In the formula, for The optimal estimate of the time error covariance matrix. It is a 9th-order identity matrix.

8. The relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction according to claim 7, characterized in that, S1 includes S1.5, output The hull cleaning robot in the local coordinate system ,Will and This serves as the basic relative positioning result of the hull cleaning robot in the local coordinate system.

9. The relative positioning method for a ship hull cleaning robot based on multi-source fusion and surface gradient correction according to claim 8, characterized in that, S2 includes S2.1, obtaining Optimal estimate of attitude angle at time t : ； Calculate the rate of change of the hull surface gradient: ； In the formula, for The rate of change of the hull surface gradient in the Y direction with respect to the X direction at time . for The rate of change of the hull surface gradient in the Y direction with respect to the Z direction at time t; S2.2 Calculate the displacement increment in each direction: ； ； ； ； In the formula, yes Time relative to The displacement increment in the Y direction at time t. yes Time relative to The displacement increment in the X direction at time t. yes Time relative to The displacement increment in the Z direction at time t. yes Time relative to The displacement distance increment of the odometer at any given time. yes The distance traveled by the odometer at any given time; S2.

3. Position correction is performed. Based on the definition of the total differential of a multivariable function, the following relationship exists: ； use right Perform corrections and combine Final coordinates of the moment Solving for the given information yields the following results. and : ； ； ； In the formula, It is the corrected version Output Gradient correction positioning results of the hull cleaning robot in the local coordinate system .

Images