An underwater cleaning robot hull surface positioning and tracking method
By employing a multi-sensor fusion scheme combining wheel encoders, IMUs, and depth gauges, along with EKF and ILQR algorithms, the positioning and path tracking problems of underwater cleaning robots in a wall-hull-hull state were solved, achieving high-precision hull surface positioning and autonomous path tracking.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- HARBIN INST OF TECH AT WEIHAI
- Filing Date
- 2025-07-18
- Publication Date
- 2026-07-07
AI Technical Summary
In the existing technology, the positioning method of underwater cleaning robots in the wall-hugging state is not suitable for special working conditions with high environmental noise and blurred vision. Traditional acoustic and visual positioning methods cannot be effectively applied.
A multi-sensor fusion scheme using a wheel encoder, inertial measurement unit (IMU), and depth gauge is adopted. Data fusion is performed using an extended Kalman filter (EKF), real-time correction is performed using the IMU and depth gauge, noise is filtered by a first-order low-pass filter algorithm, and path tracking is achieved by combining an iterative linear quadratic regulator (ILQR).
It improves the positioning accuracy and path tracking capability of underwater cleaning robots when they are attached to the wall, solves the positioning problem caused by high environmental noise and blurred vision, and realizes accurate positioning and autonomous path tracking of the robot on the surface of the ship.
Smart Images
Figure CN120778115B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of underwater cleaning robots, and in particular to a method for positioning and tracking the surface of an underwater cleaning robot hull. Background Technology
[0002] Underwater positioning is the foundation for autonomous navigation and path tracking of underwater cleaning robots. Most domestic and international research on underwater positioning focuses on sensor fusion methods such as acoustics and vision. However, for underwater cleaning robots in special working conditions with high environmental noise and blurred vision, the aforementioned positioning methods are no longer suitable for underwater cleaning robots in the wall-hugging state.
[0003] Existing patent application CN119291694A discloses a method for constructing Doppler velocity meter residuals for underwater robot positioning, which obtains the positioning method through nonlinear optimization of the covariance matrix. However, the sensors used differ. CN119291694A uses DVL and IMU, while this solution uses a wheel encoder, IMU, and depth gauge. The wheel encoder is used for state prediction, and the IMU and depth gauge are used for state correction. The two solutions are applicable to different scenarios. This solution is for the robot's positioning relative to the wall when it is in a wall-hugging state, obtaining the robot's position coordinates on the wall in real time, while CN119291694A is for the trajectory positioning of the robot swimming in water. Summary of the Invention
[0004] The purpose of this application is to provide a method for positioning and tracking the surface of an underwater cleaning robot hull, aiming to solve the problems existing in the prior art.
[0005] This application provides a method for positioning the hull surface of an underwater cleaning robot, including the following steps:
[0006] S1. Obtain the robot's forward velocity and heading angular velocity through the wheel encoder mounted on the underwater cleaning robot, obtain the robot's yaw angle and pitch angle through the inertial measurement unit (IMU), obtain the robot's depth data through the depth gauge, and establish an inertial coordinate system on the hull surface.
[0007] S2. The above data are fused using an extended Kalman filter;
[0008] In the behavior prediction stage, the robot's coordinate position and heading angle at the previous moment are combined with the robot's current linear velocity and angular velocity to obtain a process prediction of the state vector; the state vector is linearized by the Jacobian matrix, and the covariance of the previous moment with respect to the current moment is introduced to reflect the uncertainty of the state vector estimation.
[0009] During the measurement update phase, the yaw angle and yaw rate of the inertial measurement unit (IMU) are used to correct the estimated heading angle of the horizontal plane in real time, the pitch angle and pitch rate of the IMU are used to correct the estimated heading angle of the vertical plane in real time, and the water depth of the depth gauge is used to correct the estimated Z-axis position of the vertical plane in real time.
[0010] S3. After filtering the sensor measurement noise using a first-order low-pass filter algorithm, the coordinates of the underwater cleaning robot on the hull surface are obtained.
[0011] In step S2, the state vector consists of the distance the robot travels along the x-axis of the inertial coordinate system. The robot travels a distance along the y-axis of the inertial coordinate system. The robot's heading angle The state vector is represented as follows:
[0012] During the prediction phase, the process prediction equation for the state vector is shown below:
[0013]
[0014] In the formula, Let be the linear velocity of the robot at time t; Let t be the robot's heading angular velocity;
[0015] By solving the above equation, the state vector is obtained. The Jacobian matrix is then used to linearize the above equation, and the state transition Jacobian matrix is obtained. As shown in the following formula:
[0016]
[0017] Equation (1-19) can then be transformed into the following matrix form:
[0018]
[0019] In the formula, For control matrix; For control vectors, Characterization process noise;
[0020] Introducing the covariance matrix of time t-1 to time t in the prediction phase To reflect the uncertainty of the state estimate in equation (1-21), the covariance matrix estimation equation is as follows:
[0021]
[0022] In the formula, The system noise at time t-1 is given.
[0023] During the measurement update phase, the measurement error of the extended Kalman filter... The expression is as follows:
[0024]
[0025] In the formula, Estimate the matrix for the observations, for the horizontal plane, For the vertical plane, For the sensor observation matrix, in the horizontal plane, For the vertical plane, ;
[0026] Measurement estimation matrices in both horizontal and vertical planes For the state vector respectively The Jacobian matrix is used to obtain the measurement transition matrix. Measurement transfer matrix of horizontal plane positioning measurement equation as follows:
[0027]
[0028] Measurement transfer matrix of vertical plane positioning measurement equation as follows:
[0029]
[0030] Therefore, the observation estimation matrix can be written in the following matrix form:
[0031]
[0032] In the formula, For sensor observation noise;
[0033] By calculating the Kalman gain The reliability of each sensor is adjusted based on its measurement accuracy, thereby improving the accuracy of position estimation and the Kalman gain. It can be obtained through the updated covariance and measurement transition matrix The calculation is obtained, and the calculation equation is as follows:
[0034]
[0035] In the formula, Let be the measurement noise covariance matrix of the sensor;
[0036] Based on the Kalman gain, and combining prior estimates and measurements, the state vector is updated. This state vector is the position obtained after fusing the encoder, IMU, and depth gauge data, as shown in the following equation:
[0037]
[0038] The posterior error covariance matrix is updated as shown in the following equation:
[0039]
[0040] This completes one multi-sensor data fusion. Then, return to step (1-19) to continue the next data fusion. This process is repeated until the system outputs the fused positioning trajectory points.
[0041] In step S3, the first-order low-pass filtering algorithm is characterized by the following difference equation:
[0042]
[0043] In the formula, This is the filtered output value at the current moment; Input signal for the current moment; This is the filtered output of the previous time period; For smoothing coefficients, ;
[0044] Cutoff frequency The smoothing coefficient satisfies the following relationship:
[0045]
[0046] In the formula, T is the sampling period.
[0047] A path tracking method for the hull surface of an underwater cleaning robot is provided, which combines the aforementioned positioning method with an iterative linear quadratic regulator (ILQR) to achieve path tracking of the underwater cleaning robot.
[0048] Based on the robot localization method, the relationship between the robot's state and control input is represented by a nonlinear dynamic model as follows:
[0049]
[0050] In the formula, These represent the x-axis coordinates before and after the update; These represent the ordinates before and after the update, respectively. These represent the heading angles before and after the update, respectively. These represent the robot's forward speed before and after the update, respectively; To control the input angular velocity; To control the input forward speed;
[0051] The quadratic cost function is as follows:
[0052]
[0053] In the formula, The state cost function; Let Q be the terminal state cost function; Q is the state cost matrix. where represent the weights of the horizontal and vertical coordinate errors, yaw angle error, and velocity error, respectively; R is the cost matrix of the control input. These represent the control weight values for forward speed and angular velocity, respectively. The cost matrix represents the terminal state. The state of the reference trajectory; For control input; The system state at time k; This is the control input at time k.
[0054] For a given cost function, using Bellman's optimality theorem, the optimal cost is defined through recursion, and the cost gradient and Hessian matrix are shown below:
[0055]
[0056] definition and The cost gradient and the Hessian matrix are approximated respectively, and the values in equation (2-33) are... Substituting the expression, we get:
[0057]
[0058] The control input sequence is updated by finding the optimal value of the residual cost function. The first two dimensions of the control input sequence are the output control quantities of the wheel tracker, which are the expected values of the robot's forward speed and angular velocity, respectively. The expected rotational speeds of the left and right drive wheels are then solved and sent to the robot, thereby achieving the robot's desired path tracking.
[0059] The Jacobian matrix for the state and control inputs is as follows:
[0060]
[0061] In the formula, and These are the Jacobian matrices of the system state equation and the control input equation, respectively; using these two matrices, a nonlinear dynamic system can be approximated as a linear system, thus enabling its use in the ILQR algorithm.
[0062] When the robot's path tracking reaches near the water surface (water depth < 80cm), the retraction control logic is set based on the dynamic threshold of the pitch angle: if the pitch angle is less than -45°, a reverse motion command is triggered, forcing the robot to retreat to a safe working depth; if the pitch angle is greater than 45°, a forward motion command is executed; in other states, the rotation heading command is executed first.
[0063] Forward, backward, and heading rotation commands are all controlled by a PID controller. The controller expressions for the forward and backward commands are as follows:
[0064]
[0065] In the formula, These represent the robot's target depth and current depth, respectively. This refers to the distance gain parameter of the back-off controller;
[0066] The expression for the robot's heading angle rotation controller is as follows:
[0067]
[0068] In the formula, These are the robot's target heading angle and current heading angle, respectively. This refers to the angle gain parameter of the back-off controller.
[0069] This invention proposes a robot localization method for ship hull surfaces based on multi-sensor fusion. A composite localization system is constructed by integrating measurement data from a wheel encoder, inertial measurement unit (IMU), and depth gauge. At the technical implementation level, an extended Kalman filter (EKF) is used to establish a multi-source information fusion framework. A fusion localization simulation experiment is designed, and the sensitivity of the fusion localization algorithm to encoder wheel slip rate is analyzed.
[0070] Based on the constructed hull sidewall / bottom positioning method, to achieve complete closed-loop control for autonomous operation of the underwater cleaning robot, it is necessary to further address the path tracking problem under special contact conditions. The underwater hull surface exhibits complex dynamic characteristics such as low friction coefficient and high track slip ratio, and the adaptability of traditional methods in this scenario needs to be verified.
[0071] This invention employs a path tracking control framework based on an iterative linear quadratic regulator (ILQR). By constructing a nonlinear kinematic model of a tracked robot, it innovatively introduces a dynamic update mechanism for the reference trajectory and designs a PID backoff compensation controller for critical states in near-water operations. To evaluate the algorithm's performance, horizontal and vertical plane tracking simulation experiments were conducted to compare and analyze the differences in control effects before and after trajectory update strategy optimization. Simultaneously, multi-stage track slippage sensitivity tests were performed to verify the system's robustness.
[0072] The beneficial effects of this invention are:
[0073] 1. In response to the challenges of high environmental noise and blurred vision when underwater cleaning robots are operating close to walls, this paper proposes an innovative multi-sensor fusion scheme of encoder + IMU + depth gauge, abandoning traditional acoustic / visual positioning methods, to solve the positioning problem of underwater cleaning robots in the wall-hugging state.
[0074] 2. Dual-plane differential processing: Depth gauges and IMUs (3D correction) are introduced in the ship-side positioning, while only IMUs (2D correction) are used for the ship-bottom positioning. This better matches the robot's working state on the ship wall or bottom, making the positioning more accurate.
[0075] 3. A target point update mechanism with geometric distance constraints is proposed to adjust the nearest neighbor points of the reference trajectory in real time, thus solving the tracking lag problem caused by update rate mismatch in traditional methods. Attached Figure Description
[0076] Figure 1 Let be the inertial coordinate system of the ship's hull.
[0077] Figure 2 Let be the inertial coordinate system of the ship's bottom.
[0078] Figure 3 This is a tracked model of an underwater cleaning robot.
[0079] Figure 4 Differences and similarities in the positioning of the ship's side and bottom.
[0080] Figure 5 A logic diagram for positioning an underwater cleaning robot on the surface of a ship.
[0081] Figure 6 A flowchart for updating target points during path tracing.
[0082] Figure 7 This is a block diagram of the rollback control logic. Detailed Implementation
[0083] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0084] I. Methods for Positioning Underwater Robots on Their Hulls
[0085] 1. Establishing a coordinate system
[0086] To facilitate simulation analysis, this method simplifies the ship's side and bottom to a plane. A model is established as follows: Figure 1 and Figure 2 The hull surface coordinate systems O-XY and O-XZ shown are used as inertial coordinate systems to simulate the robot's side-crawling and bottom-crawling movements. The origin is located near the bow. The XOZ plane ( Figure 1 ) represents the side of the hull, XOY plane ( Figure 2 () represents the bottom surface of the hull. Establish a body coordinate system with the center of gravity as the origin. The transformation matrix between the body coordinate system and the inertial coordinate system is: .
[0087] This method uses data fusion for localization based on measurements from three different sensors: an integrated wheel encoder, an inertial measurement unit (IMU), and a depth gauge. The encoder measures the robot's forward velocity and yaw angular velocity. IMU measures the robot's yaw and pitch angles. The depth gauge measures the depth of the robot's location. Specifically, when the robot is attached to a sidewall, the IMU pitch angle represents the robot's heading angle; when the robot is attached to the bottom of a ship, the IMU yaw angle represents the robot's heading angle. The robot's state parameters are represented as coordinate position and heading angle. Therefore, the measurements from the three sensors can be used as a basis. To determine the robot's state parameters Make an estimate.
[0088] 2. Sensor Model Construction
[0089] 2.1 Encoder Measurement Model Establishment
[0090] A kinematic analytical model based on a dual-track structure was established for a tracked underwater cleaning robot. The robot employs a symmetrically distributed, independently driven track system, such as... Figure 3 As shown, its structural parameters include drive wheel radius, track width, track spacing and effective length of ground contact section (see Table 1 for details).
[0091] Table 1 Track structure parameters
[0092]
[0093] To improve the accuracy of mileage data, this method uses independent encoder wheels as mileage acquisition sensors, which are installed at the middle positions on both sides of the robot's bottom. Figure 3 For ease of analysis, this method makes the following assumptions about the tracked walking mechanism:
[0094] (1) Both the track and the encoder wheel are considered as rigid bodies, and the track and the encoder wheel have no elastic deformation;
[0095] (2) When no slip parameters are added, the contact surfaces between the track and the hull, and between the encoder wheel and the hull, are all pure rolling surfaces;
[0096] (3) Assuming that the dynamic characteristics (adhesion, friction coefficient) of the tracks on the left and right sides are exactly the same, and the pressure distribution between the track and the hull is uniform, the dynamic characteristics of the encoder wheels on both sides are likely to be exactly the same.
[0097] (4) Assume that the robot’s walking process takes place on a plane and does not need to cross obstacles. When the robot turns, it rotates around a certain instantaneous center.
[0098] Establish the kinematic equations for the crawling motion of the tracked robot:
[0099]
[0100] In the formula, These are the linear velocities of the encoder wheels on the left and right sides, respectively. This represents the robot's forward velocity. This represents the robot's heading angular velocity.
[0101] Relationship between encoder wheel linear velocity and rotational speed:
[0102]
[0103] In the formula, The encoder wheel linear velocity; —Encoder wheel speed, measured in revolutions per second.
[0104] 2.2 Establishment of IMU Measurement Model
[0105] (1) Accelerometer noise model
[0106] The output noise of an accelerometer consists of white noise, random walk noise, and bias instability noise.
[0107]
[0108] In the formula, This is the ideal acceleration value; For acceleration bias; The bias attenuation factor, It is the time constant of the accelerometer bias error; For random walk noise, Noise density; This is bias-unstable noise. Noise density; It is white noise. Noise density; This is the rotation matrix from the navigation coordinate system to the body coordinate system; It is the acceleration due to gravity. This is the accelerometer sampling time.
[0109] (2) Gyroscope noise model
[0110] The output noise of a gyroscope consists of white noise, random walk noise, and bias instability noise.
[0111]
[0112] In the formula, This is the ideal angular velocity value; For gyroscope bias; The bias attenuation factor, It is the bias time constant; It is white noise. Noise density; For random walk noise, Noise density; This is bias-unstable noise. Noise density; This is the gyroscope sampling time.
[0113] 2.3 Depth Gauge Measurement Model Establishment
[0114] (1) Water depth measurement model
[0115] Vertical position of underwater robot This is derived from wave kinematics and carrier kinematics:
[0116]
[0117] In the formula, z represents the actual water depth at the location of the underwater robot; The wave height model based on linear wave theory has the following formula:
[0118]
[0119] In the formula, H is the significant wave height; k is the wave number; Angular frequency; T is the wave period; Wavelength; This is the phase shift.
[0120] According to my country's wave classification standards, this method selects a level 2 minor wave as the wave environment, and the specific parameter settings are as follows:
[0121] (2) Measurement noise model
[0122] Sensor measurements include theoretical values from robot sensors and noise:
[0123]
[0124] In the formula, This is Gaussian noise for the depth sensor.
[0125] 3. Multi-sensor fusion localization method based on EKF
[0126] To address the nonlinear characteristics of the underwater robot's wall-hugging motion, this method employs the Extended Kalman Filter (EKF) as the core data fusion framework. The EKF overcomes the limitations of traditional Kalman filters through local linearization. Its core principle can be summarized as: [The text abruptly shifts to a different topic] ...for the nonlinear state transition equation... and observation equations Perform a first-order Taylor expansion to construct the Jacobian matrix. and This transforms the nonlinear system into a time-varying linear system for recursive estimation.
[0127] The EKF model used in this study is as follows:
[0128]
[0129] In the formula, Let t be the state vector of the Kalman filter at time t; The process noise at time t-1 It is a process function; The measurement vector for the extended Kalman filter; To measure noise for the sensor, For measurement functions.
[0130] The EKF filter method is divided into a state prediction phase and a measurement update phase, as detailed below:
[0131] 1) State prediction: Calculate the predicted state, determine the state transition Jacobian matrix, and use it to calculate the covariance.
[0132]
[0133] In the formula, For the predicted state; This refers to the state at the previous moment; Ft-1 is the control vector at the previous time step; Ft-1 is the state transition Jacobian matrix. is the covariance matrix; Qt-1 is the prediction process noise.
[0134] 2) Prediction Update: Calculate the Jacobian matrix and Kalman filter gain of the observation equation; combine state prediction with measurement to provide posterior state and covariance estimates.
[0135]
[0136]
[0137] In the formula, Ht is the Jacobian matrix of the observation equation; Kt is the Kalman filter gain;
[0138] Rk represents the measurement noise; This is the updated state vector; It is a unit diagonal matrix; This is the updated covariance matrix.
[0139] Figure 4 By comparing the similarities and differences between bottom and side positioning of underwater ship cleaning robots, it can be seen that the main difference lies in the type of sensor input. In side positioning, a depth sensor is introduced, increasing the measurement dimension from two to three. Using these two methods to locate the bottom and side of the ship respectively helps to compare the relationship between the positioning accuracy of the EKF method and the sensor input type.
[0140] The complete localization process of the robot is as follows Figure 5 As shown, the details are as follows:
[0141] 1) State prediction stage
[0142] In the hull surface positioning state prediction stage, the prediction methods for the hull wall and hull bottom are the same, employing dual encoders to collect real-time linear velocity data of the robot's two side tracks to achieve motion state estimation. Based on the differential kinematics model, the robot's longitudinal velocity is derived by calculating the mean and difference of the linear velocities of the encoder wheels on both sides. With steering angular velocity .
[0143] The state vector consists of the distance the robot has traveled along the x-axis of the inertial coordinate system. The robot travels a distance along the y-axis of the inertial coordinate system. The robot's heading angle The state vector is represented as follows:
[0144]
[0145] Extended Kalman filtering (EKB) consists of two steps: the first step is to predict system behavior, and the second step is to update the system behavior measurements. In the prediction phase, the robot's coordinates and heading angle from the previous moment are combined with the robot's current linear and angular velocities, using methods such as... Figure 3 The transformation relationship between the body coordinate system and the inertial coordinate system shown can be used to obtain the process prediction equation for the state vector:
[0146]
[0147] In the formula, Let be the linear velocity of the robot at time t; Let t be the angular velocity of the robot's heading at time t.
[0148] Equation (1-19) is a nonlinear prediction equation. Solving the above equation allows us to predict the state vector. The Jacobian matrix is then used to linearize the above equation, and the state transition Jacobian matrix is obtained. As shown in the following formula:
[0149]
[0150] Equation (1-19) can then be transformed into the following matrix form:
[0151]
[0152] In the formula, For control matrix; For control vectors, Characterization process noise.
[0153] Introducing the covariance matrix of time t-1 to time t in the prediction phase To reflect the uncertainty of the state estimate in equation (1-21), the covariance matrix estimation equation is as follows:
[0154]
[0155] In the formula, The system noise at time t-1 is given.
[0156] 2) Measurement Update Phase
[0157] During the measurement update phase, the encoder types differ between the horizontal and vertical planes. In the horizontal plane, the sensor involved in the correction is an IMU, which uses its yaw angle and yaw rate to correct the heading angle estimated by the encoder in real time. In the vertical plane, the sensors involved in the position correction include an IMU and a depth gauge. The pitch angle and pitch rate of the IMU are used to correct the heading angle estimated by the encoder in real time. In addition, the water depth data measured by the depth gauge is used to correct the Z-axis position of the robot relative to the side wall of the hull in real time.
[0158] So, what is the measurement error of the extended Kalman filter? The expression is as follows:
[0159]
[0160] In the formula, Estimate the matrix for the observations, for the horizontal plane, For the vertical plane, For the sensor observation matrix, in the horizontal plane, For the vertical plane, .
[0161] Measurement estimation matrices in both horizontal and vertical planes For the state vector respectively The Jacobian matrix is used to obtain the measurement transition matrix. Measurement transfer matrix of horizontal plane positioning measurement equation as follows:
[0162]
[0163] Measurement transfer matrix of vertical plane positioning measurement equation as follows:
[0164]
[0165] Therefore, the observation estimation matrix can be written in the following matrix form:
[0166]
[0167] In the formula, This refers to the noise observed by the sensor.
[0168] By calculating the Kalman gain The reliability of each sensor is adjusted based on its measurement accuracy, thereby improving the accuracy of position estimation. (Kalman gain) It can be obtained through the updated covariance and measurement transition matrix The calculation is obtained, and the calculation equation is as follows:
[0169]
[0170] In the formula, Let be the measurement noise covariance matrix of the sensor.
[0171] Based on the Kalman gain, and combining prior estimates and measurements, the state vector is updated. This state vector is the position obtained after fusing the encoder, IMU, and depth gauge data, as shown in the following equation:
[0172]
[0173] The posterior error covariance matrix is updated as shown in the following equation:
[0174]
[0175] This completes one multi-sensor data fusion. Then, return to step (1-19) to continue the next data fusion. This process is repeated until the system outputs the fused positioning trajectory points.
[0176] 3. Elimination of high-frequency noise in the positioning trajectory
[0177] To address the high-frequency interference caused by sensor measurement noise in the target positioning trajectory, which manifests as spikes in the trajectory result, this method employs a first-order low-pass filtering algorithm to filter out high-frequency noise in the positioning trajectory. Its digital implementation can be characterized by the following difference equation:
[0178]
[0179] In the formula, This is the filtered output value at the current moment; Input signal for the current moment; This is the filtered output of the previous time period; For smoothing coefficients, .
[0180] Cutoff frequency The smoothing coefficient satisfies the following relationship:
[0181]
[0182] In the formula, T is the sampling period.
[0183] In this method, the sampling period of the filter is set to 0.005 s, and the cutoff frequency is set to 5 Hz.
[0184] II. Path Tracking Method for Underwater Cleaning Robots on Ship Hull Surface
[0185] 1. ILQR control path tracing method
[0186] The ILQR algorithm is an optimization-based control method widely used in path tracking and dynamic control problems of nonlinear systems. The core idea of ILQR is to iteratively linearize the nonlinear dynamic model and the quadratic approximation cost function, gradually optimizing the control input to achieve optimal system performance and effectively solve path tracking problems in complex systems. The implementation of the ILQR control method relies on the discretization of the nonlinear dynamic model and the minimization of the comprehensive cost function. This comprehensive cost function consists of two parts: the state cost and the control input cost. The state cost measures the deviation between the current state and the reference trajectory, while the control cost penalizes the magnitude of the control input. (State sequence) and control sequence The algorithm is initialized at the beginning by estimating the current state of the underwater robot. The state dimensions include position coordinates (x, y) and heading angle. and speed Let x = The control input dimension includes the speed of the tracked robot. and angular velocity ,remember The discrete-time nonlinear dynamics model and cost function are as follows:
[0187]
[0188] In the formula, Let be the state cost function, which is quadratically differentiable; Let the terminal cost function be a quadratic differentiable function; The system state at time k; This is the control input at time k.
[0189] For a given cost function, the optimal cost can be defined using Bellman's optimality theorem through recursion. , means as follows:
[0190]
[0191] remember ,So:
[0192]
[0193] Within the local neighborhood of the reference trajectory, the residual cost function is represented as follows:
[0194]
[0195] The nonlinear system in equation (2-1) can be approximated using a first-order Taylor series expansion:
[0196]
[0197] In the formula, for The Jacobian matrix of x at time k; for The Jacobian matrix of u at time k.
[0198] Based on the current state sequence and the control sequence, the next state sequence is predicted. Then, equation (2-1) is represented in the state space as follows:
[0199]
[0200] Therefore, in equation (2-7):
[0201]
[0202]
[0203] In the formula
[0204]
[0205] Equation (2-7) right Find the partial derivative:
[0206]
[0207] To find the optimal solution To obtain the value, the partial derivative obtained from equation (2-20) must be equal to zero. The optimal value is denoted as :
[0208]
[0209] remember
[0210]
[0211] Therefore, equation (2-21) can be simplified as follows:
[0212]
[0213] definition and They are respectively The gradient and Hessian approximation, based on dynamic programming, can solve problems with N time steps. Tail end problem, and The definition is as follows:
[0214]
[0215] Equation (2-24) Substituting into equation (2-7) and taking the first and second partial derivatives of equation (2-7) with respect to δx, then... and The update can be performed using the following formula:
[0216]
[0217] Control increments calculated based on backpropagation ILQR can update the control input sequence. Control input The update formula is
[0218]
[0219] Based on the robot localization method of this application, the relationship between the robot's state and control input is represented by a nonlinear dynamic model as follows:
[0220]
[0221] In the formula, These represent the x-axis coordinates before and after the update; These represent the ordinates before and after the update, respectively. represents the heading angle before and after the update; represents the robot's forward speed before and after the update; To control the input angular velocity; To control the forward speed of the input.
[0222] In the walking mechanism of a tracked robot and These are the Jacobian matrices of the system state equation and the control input equation, respectively, representing the linear approximation of the system dynamics model to the state and control input. Using these two matrices, a nonlinear dynamic system can be approximated as a linear system, thus enabling its use in the ILQR algorithm.
[0223] and The specific details are as follows:
[0224]
[0225] The quadratic cost function is set as follows:
[0226]
[0227] In the formula, The state cost function; Let Q be the terminal state cost function; Q is the state cost matrix. represents the weight values of the horizontal and vertical coordinate errors, yaw angle errors, and velocity errors, respectively. These weight values reflect the importance of the variables; the larger the weight value, the faster the corresponding state variable converges. R is the cost matrix of the control input. These represent the control weight values for forward speed and angular velocity, respectively. The cost matrix represents the terminal state. The state of the reference trajectory; To control the input.
[0228] Then, the cost gradient and Hessian matrix in equations (2-15) to (2-19) are as follows:
[0229]
[0230] Then, in equation (2-33) Substituting the expressions into equations (2-25) and (2-26), we get:
[0231]
[0232] The control sequence is calculated according to equation (2-29). The first two dimensions of the data in the sequence are the output control quantities of the wheel diameter tracking controller, which are the expected values of the robot's forward speed and angular velocity, respectively. The expected rotation speeds of the left and right drive wheels are solved by formulas (1-1) and (1-2) and sent to the robot, thereby realizing the robot's expected path tracking.
[0233] To address the tracking lag issue caused by target point update rate mismatch in basic trajectory tracking, this method proposes a dynamic look-ahead strategy based on geometric distance constraints. By calculating the nearest neighbor between the robot's current position and the reference trajectory in real-time, an adaptive target point update rule is established: when the Euclidean distance between the robot's motion state and the look-ahead target point exceeds a dynamic threshold, the system recalculates the nearest neighbor on the reference trajectory as the next tracking target. This mechanism effectively suppresses phase delay in trajectory tracking through closed-loop feedback iterative updates of the target sequence. Figure 6 As shown, the algorithm framework embeds geometric correlation constraints into the ILQR optimization loop, forming a closed-loop architecture of dynamic calibration of target points and collaborative optimization of the controller, which significantly improves the robustness of path tracking.
[0234] 2. Near-water surface retreat control methods
[0235] Propeller cavitation is a typical phenomenon in fluid dynamics. Its physical essence is that when propeller blades rotate, the local flow field pressure drops below the fluid's saturated vapor pressure, causing a phase change in liquid water and the formation of vapor cavitation. These cavitation bubbles collapse as they migrate to high-pressure regions, generating high-frequency shock waves and microjets. This process leads to problems such as nonlinear thrust decay and robot adhesion imbalance. Therefore, to address the adhesion imbalance problem caused by propeller cavitation in the near-water surface region of underwater cleaning robots, this method proposes an adaptive backtracking control method based on attitude feedback. When the liquid level sensor reading is below 80 cm, the robot is determined to have entered the near-water surface boundary region. At this point, the propeller thrust decays nonlinearly due to the flow field cavitation effect, inducing uneven track adhesion distribution and a surge in slip probability, making path tracking difficult to continue.
[0236] The retraction control logic is set based on a dynamic threshold for pitch angle: if the pitch angle is less than -45°, a reverse motion command is triggered; if the pitch angle is greater than 45°, a forward motion command is executed; in other states, the rotation and heading commands are executed first. This control logic, through attitude-position coordinated adjustment, forces the robot to retreat to a safe operating depth (liquid level ≥ 80 cm) to avoid disruption of the flow field continuity caused by cavitation. Figure 7 As shown, this strategy effectively maintains the dynamic stability of the contact mobile platform through closed-loop state monitoring and multi-modal control command switching.
[0237] During the process, the forward, backward, and heading rotation commands are all controlled using a PID controller. The controller expressions for the forward and backward commands are as follows:
[0238]
[0239] In the formula, These represent the robot's target depth and current depth, respectively. This refers to the distance gain parameter of the back-off controller.
[0240] The expression for the robot's heading angle rotation controller is as follows:
[0241]
[0242] In the formula, These are the robot's target heading angle and current heading angle, respectively. This refers to the angle gain parameter of the back-off controller.
[0243] The two PID controllers calculate the speed control quantity of the track motors, which is then mapped to the speed and sent to the two track motors to drive the tracked robot to complete movements such as changing course, moving forward and backward. Finally, the robot moves to the vicinity of the reference tracking trajectory at a water depth of 80cm. Then, the robot continues to use the ILQR control algorithm for path tracking.
[0244] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other specific forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and not restrictive.
Claims
1. A method for positioning the hull surface of an underwater cleaning robot, characterized in that, Includes the following steps: S1. Obtain the robot's forward velocity and heading angular velocity through the wheel encoder mounted on the underwater cleaning robot, obtain the robot's yaw angle and pitch angle through the inertial measurement unit (IMU), obtain the robot's depth data through the depth gauge, and establish an inertial coordinate system on the hull surface. S2. The above data are fused using an extended Kalman filter; In the behavior prediction stage, the robot's coordinate position and heading angle at the previous moment are combined with the robot's current linear velocity and angular velocity to obtain a process prediction of the state vector; the state vector is linearized by the Jacobian matrix, and the covariance of the previous moment with respect to the current moment is introduced to reflect the uncertainty of the state vector estimation. During the measurement update phase, the yaw angle and yaw rate of the inertial measurement unit (IMU) are used to correct the estimated heading angle of the horizontal plane in real time, the pitch angle and pitch rate of the IMU are used to correct the estimated heading angle of the vertical plane in real time, and the water depth of the depth gauge is used to correct the estimated Z-axis position of the vertical plane in real time. S3. After filtering the sensor measurement noise using a first-order low-pass filter algorithm, the coordinates of the underwater cleaning robot on the hull surface are obtained. In step S2, the state vector consists of the distance the robot travels along the x-axis of the inertial coordinate system. The robot travels a distance along the y-axis of the inertial coordinate system. The robot's heading angle The state vector is represented as follows: During the prediction phase, the process prediction equation for the state vector is shown below: In the formula, Let be the linear velocity of the robot at time t; Let t be the robot's heading angular velocity; By solving the above equation, the state vector is obtained. The Jacobian matrix is then used to linearize the above equation, and the state transition Jacobian matrix is obtained. As shown in the following formula: Equation (1-19) can then be transformed into the following matrix form: In the formula, For control matrix; For control vectors, Characterization process noise; Introducing the covariance matrix of time t-1 to time t in the prediction phase To reflect the uncertainty of the state estimate in equation (1-21), the covariance matrix estimation equation is as follows: In the formula, The system noise at time t-1; During the measurement update phase, the measurement error of the extended Kalman filter... The expression is as follows: In the formula, Estimate the matrix for the observations, for the horizontal plane, For the vertical plane, ; For the sensor observation matrix, in the horizontal plane, , for The robot's yaw angle, for the vertical plane, , The depth of the robot's location. The robot's pitch angle; Measurement estimation matrices in both horizontal and vertical planes For the state vector respectively The Jacobian matrix is used to obtain the measurement transition matrix. Measurement transfer matrix of horizontal plane positioning measurement equation as follows: Measurement transfer matrix of vertical plane positioning measurement equation as follows: Therefore, the observation estimation matrix can be written in the following matrix form: In the formula, For sensor observation noise; By calculating the Kalman gain The reliability of each sensor is adjusted based on its measurement accuracy, thereby improving the accuracy of position estimation. (Kalman gain) It can be obtained through the updated covariance and measurement transition matrix The calculation is obtained, and the calculation equation is as follows: In the formula, Let be the measurement noise covariance matrix of the sensor; Based on the Kalman gain, and combining prior estimates and measurements, the state vector is updated. This state vector is the position obtained after fusing the encoder, IMU, and depth gauge data, as shown in the following equation: The posterior error covariance matrix is updated as shown in the following equation: This completes one multi-sensor data fusion. Then, return to step (1-19) to continue the next data fusion. This process is repeated until the system outputs the fused positioning trajectory points.
2. The underwater cleaning robot hull surface positioning method according to claim 1, characterized in that, In step S3, the first-order low-pass filtering algorithm is characterized by the following difference equation: In the formula, This is the filtered output value at the current moment; Input signal for the current moment; This is the filtered output of the previous time period; For smoothing coefficients, ; Cutoff frequency The smoothing coefficient satisfies the following relationship: In the formula, T is the sampling period.
3. A method for tracking the path on the hull surface of an underwater cleaning robot, characterized in that, Based on the positioning method described in claim 1, combined with the iterative linear quadratic regulator ILQR, path tracking of the underwater cleaning robot is achieved.
4. The underwater cleaning robot hull surface path tracking method as described in claim 3, characterized in that, Based on the robot localization method, the relationship between the robot's state and control input is represented by a nonlinear dynamic model as follows: In the formula, These represent the x-axis coordinates before and after the update, respectively. These represent the ordinates before and after the update, respectively. These represent the heading angles before and after the update, respectively. These represent the robot's forward speed before and after the update, respectively; To control the input angular velocity; To control the input forward speed; The quadratic cost function is as follows: In the formula, The state cost function; Let Q be the terminal state cost function; Q is the state cost matrix. where represent the weights of the horizontal and vertical coordinate errors, yaw angle error, and velocity error, respectively; R is the cost matrix of the control input. These represent the control weight values for forward speed and angular velocity, respectively. The cost matrix represents the terminal state. The state of the reference trajectory; For control input; The system state at time k; This is the control input at time k.
5. The underwater cleaning robot hull surface path tracking method as described in claim 4, characterized in that, For a given cost function, using Bellman's optimality theorem, the optimal cost is defined through recursion, and the cost gradient and Hessian matrix are shown below: definition and The cost gradient and the Hessian matrix are approximated respectively, and the values in equation (2-33) are... Substituting the expression, we get: The control input sequence is updated by finding the optimal value of the residual cost function. The first two dimensions of the control input sequence are calculated to obtain the output control quantities of the path tracking controller, which are the expected values of the robot's forward speed and angular velocity, respectively. The expected rotational speeds of the left and right drive wheels are then obtained by inverse solving and sent to the robot, thereby achieving the robot's desired path tracking.
6. The underwater cleaning robot hull surface path tracking method as described in claim 4, characterized in that, The Jacobian matrix for the state and control inputs is as follows: In the formula, and These are the Jacobian matrices of the system state equation and the control input equation, respectively. These two matrices can be used to approximate a nonlinear dynamic system as a linear system, thus enabling its use in the ILQR algorithm; v is the velocity in the state dimension; θ is the heading angle in the state dimension.
7. The underwater cleaning robot hull surface path tracking method as described in claim 3, characterized in that, When the robot's path tracking reaches near the water surface, the retraction control logic is set based on the dynamic threshold of the pitch angle: if the pitch angle is less than -45°, a reverse motion command is triggered, forcing the robot to retreat to a safe working depth; if the pitch angle is greater than 45°, a forward motion command is executed; in other states, the rotation heading command is executed first.
8. The underwater cleaning robot hull surface path tracking method as described in claim 7, characterized in that, Forward, backward, and heading rotation commands are all controlled by a PID controller. The controller expressions for the forward and backward commands are as follows: In the formula, These represent the robot's target depth and current depth, respectively. For the distance gain parameter of the back-off controller; The expression for the robot's heading angle rotation controller is as follows: In the formula, These are the robot's target heading angle and current heading angle, respectively. This refers to the angle gain parameter of the back-off controller.