Underwater cleaning robot hull surface positioning and tracking method

Abstract

The invention relates to an underwater cleaning robot positioning and tracking method. Positioning comprises the steps that data of multiple sensors on an underwater cleaning robot are fused through an extended Kalman filter, and coordinates of the underwater cleaning robot on the surface of a ship body are obtained after sensor measurement noise is filtered through a first-order low-pass filtering algorithm. The tracking method comprises the step of realizing the path tracking of the underwater cleaning robot based on the positioning method in combination with an iterative linear quadratic regulator (ILQR). According to the invention, a multi-sensor fusion scheme of the encoder, the IMU and the depth meter is innovatively provided, and the positioning problem of the underwater cleaning robot in a wall attaching state is solved. And double-plane differential treatment is adopted, so that the working state of the robot on the ship wall or the ship bottom is better fitted, and positioning is more accurate. A target point updating mechanism of geometric distance constraint is provided, the nearest neighbor point of the reference trajectory is adjusted in real time, and the problem of tracking lag caused by updating rate mismatch in a traditional method is solved.

Description

TECHNICAL FIELD

[0001] The application relates to the field of underwater cleaning robots, in particular to a method for positioning and tracking a ship surface of an underwater cleaning robot. BACKGROUND

[0002] Underwater positioning is the basis for autonomous navigation and path tracking of an underwater cleaning robot. Most of the underwater positioning researches at home and abroad focus on sensor fusion methods such as acoustics and vision. However, the foregoing positioning methods are not applicable to the underwater cleaning robot in the wall-adhering state due to the special working conditions of the underwater cleaning robot, such as large environmental noise and blurred vision.

[0003] The existing patent CN119291694A discloses a method for constructing residual errors of a Doppler velocimeter for underwater robot positioning. The positioning method is obtained by nonlinear optimization of a covariance matrix. However, the sensors used are different. The DVL and IMU are used in CN119291694A, while the wheel encoder, IMU and depth gauge are used in the present scheme. The wheel encoder is used for state prediction, and the IMU and depth gauge are used for state correction. The application scenarios of the two schemes are different. The scenario of the present scheme is the positioning of the robot relative to the wall surface in the wall-adhering state, and the position coordinates of the robot on the wall are obtained in real time. The CN119291694A is for trajectory positioning of the robot swimming in water. SUMMARY

[0004] The purpose of the present application is to provide a method for positioning and tracking a ship surface of an underwater cleaning robot, which aims to solve the problems existing in the prior art.

[0005] The underwater cleaning robot ship surface positioning method provided by the embodiments of the present application comprises the following steps:

[0006] S1, obtaining the forward speed and the heading angular velocity of the robot through the wheel encoder mounted on the underwater cleaning robot, obtaining the yaw angle and the pitch angle of the robot through the inertial measurement unit (IMU), and obtaining the depth data of the robot through the depth gauge; and establishing an inertial coordinate system on the ship surface;

[0007] S2, fusing the above data through an extended Kalman filter;

[0008] In the behavior prediction stage, the coordinate position and the heading angle of the robot at the previous moment are combined with the linear speed and the angular speed of the robot at the current moment to obtain the process prediction of the state vector. The Jacobian matrix of the state vector is linearized, and the covariance of the previous moment to the current moment is introduced to reflect the uncertainty of the state vector estimation.

[0009] In the measurement update stage, the yaw angle and the yaw angle velocity of the inertial measurement unit (IMU) are used to correct the heading angle estimated by the horizontal plane in real time, the pitch angle and the pitch angle velocity of the inertial measurement unit (IMU) are used to correct the heading angle estimated by the vertical plane in real time, and the water depth of the depth gauge is used to correct the Z-axis position estimated by the vertical plane in real time.

[0010] S3, obtaining the coordinates of the underwater cleaning robot on the surface of the ship body after filtering the sensor measurement noise by a first-order low-pass filtering algorithm.

[0011] In step S2, the components of the state vector include: the distance traveled by the robot along the x-axis of the inertial coordinate system , the distance traveled by the robot along the y-axis of the inertial coordinate system , the heading angle of the robot , and the state vector is represented as follows: (1-18)

[0012] In the prediction stage, the process prediction equation of the state vector is as follows:

[0013] (1-19)

[0014] In the formula, is the linear velocity of the robot at time t; is the heading angle velocity of the robot at time t;

[0015] By solving the Jacobian matrix of the state vector of the above formula and further linearizing the above formula, the state transition Jacobian matrix is as follows: (1-20)

[0016] Then, formula (1-19) can be converted into the following matrix form: (1-21)

[0017] In the formula, is the control matrix; is the control vector, characterizing the process noise;

[0018] The covariance matrix at time t-1 is introduced to reflect the uncertainty of the state estimation of formula (1-21) at time t, and the covariance matrix estimation equation is as follows: (1-22)

[0019] In the formula, is the system noise at time t-1.

[0020] In the measurement update stage, the measurement error of the extended Kalman filter is expressed as follows: (1-23)

[0021] wherein, is the observation estimate matrix, for the horizontal plane, for the vertical plane, ; is the sensor observation matrix, for the horizontal plane, for the vertical plane, ;

[0022] The measurement estimate matrix in both the horizontal and vertical planes is respectively the Jacobian matrix of the state vector , and the measurement transition matrix is obtained, the measurement transition matrix of the horizontal plane positioning measurement equation is as follows: (1-24)

[0023] The measurement transition matrix of the vertical plane positioning measurement equation is as follows: (1-25)

[0024] Then, the observation measurement estimate matrix can be written in the following matrix form: (1-26)

[0025] wherein, is the sensor observation noise;

[0026] By calculating the Kalman gain , the reliability of each sensor is adjusted according to the measurement accuracy of each sensor, thereby improving the accuracy of the position estimate, and the Kalman gain can be calculated by the updated covariance and the measurement transition matrix , and the calculation equation is as follows: (1-27)

[0027] wherein, is the measurement noise covariance matrix of the sensor;

[0028] According to the Kalman gain, the state vector is updated by combining the prior estimate and the measurement, and the state vector is the position obtained by fusing the encoder, the IMU, and the depth meter, as shown in the following formula: (1-28)

[0029] The posterior error covariance matrix is updated, as shown in the following formula: (1-29)

[0030] This completes one multi-sensor data fusion, at this time return formula (1-19) continue next data fusion, so on and so forth, the system output fusion positioning track point.

[0031] In step S3, the first-order low-pass filter algorithm is characterized by the following difference equation: (1-30)

[0032] In the formula, is the filter output value at the current time; is the input signal at the current time; is the filter output of the previous time sequence period; is the smoothing coefficient,

[0033] The cut-off frequency and the smoothing coefficient satisfy the following relationship: (1-31)

[0034] In the formula, T is the sampling period.

[0035] An underwater cleaning robot ship surface path tracking method, based on the positioning method of claim 1 combined with an iterative linear quadratic regulator ILQR, realizes the path tracking of the underwater cleaning robot.

[0036] Based on the robot positioning method, the relationship between the state of the robot and the control input is represented by a nonlinear dynamic model as follows: (2-30)

[0037] In the formula, , respectively represent the horizontal axis coordinates before and after updating; , respectively represent the vertical axis coordinates before and after updating; , respectively represent the heading angles before and after updating; , respectively represent the robot forward speeds before and after updating; is the control input angular velocity; is the control input forward speed;

[0038] The quadratic cost function is as follows: (2-33)

[0039] In the formula, is the state cost function; is the terminal state cost function; Q is the cost matrix of the state,​ respectively represent the weight values of the horizontal and vertical coordinate errors, the yaw angle error and the velocity error; R is the cost matrix of the control input, respectively represent the control weight values of the forward speed and the angular velocity; is the cost matrix of the terminal state; is the state of the reference trajectory; is the control input; is the system state at time k; is the control input at time k.

[0040] For a given cost function, the optimal cost is defined by the recursive relationship using the Bellman optimality theorem, and the cost gradient and Hessian matrix are as follows:

[0041] (2-34)

[0042] (2-35)

[0043] (2-36)

[0044] (2-37)

[0045] (2-38)

[0046] Definition and are the cost gradient and Hessian matrix approximation, respectively, and the expression of in equation (2-33) is substituted to obtain:

[0047] (2-39)

[0048] (2-40)

[0049] The control input sequence is updated by the optimal value of the residual cost function, and the first two dimensions of the control input sequence are calculated, which are the output control quantities of this round of path tracking controller, and are the expected values of the forward speed and the angular velocity of the robot, respectively. The expected speeds of the left and right drive wheels are solved and sent to the robot, and then the expected path tracking of the robot is realized.

[0050] The Jacobian matrices of the state and the control input are as follows: (2-31) (2-32)

[0051] In the formula, ​​and Jacobian matrices of system state equation and control input equation respectively; the two matrices can be used to approximate the nonlinear dynamic system as a linear system, so that the ILQR algorithm can be used.

[0052] When the robot path tracking is close to the water surface (water depth < 80 cm), a fallback control logic based on the pitch angle dynamic threshold is set: if the pitch angle is less than-45°, the reverse motion instruction is triggered, and the robot is forced to retreat to the safe working depth; if the pitch angle is greater than 45°, the forward motion instruction is executed; otherwise, the rotation heading command is executed preferentially.

[0053] The forward command, the backward command and the rotation heading command are all controlled by PID controllers, and the controller expressions of the forward and backward commands are as follows: (2-41)

[0054] In the formula, , are the target depth and the current depth of the robot respectively; , , is the distance gain parameter of the fallback controller;

[0055] The expression of the robot heading angle rotation controller is as follows: (2-42)

[0056] In the formula, , are the target heading angle and the current heading angle of the robot respectively;

[0057] , , are the angle gain parameters of the fallback controller.

[0058] The application provides a ship body surface robot positioning method based on multi-sensor fusion, and a composite positioning system is constructed by integrating measurement data of a wheel encoder, an inertial measurement unit (IMU) and a depth gauge. In the technical implementation aspect, an extended Kalman filter (EKF) is used to establish a multi-source information fusion framework, a fusion positioning simulation experiment is designed, and research on sensitivity of the fusion positioning algorithm to the encoder wheel slip rate is carried out.

[0059] Based on the constructed ship body side wall / bottom positioning method, in order to realize complete closed-loop control of the autonomous operation of the underwater cleaning robot, the path tracking problem under special contact working conditions needs to be further solved. There are complex dynamic characteristics such as low friction coefficient and high track slip rate on the underwater ship body surface, and the adaptability of the traditional method in this scene needs to be verified.

[0060] The application adopts a path tracking control framework based on an iterative linear quadratic regulator (ILQR), innovatively introduces a reference trajectory dynamic updating mechanism by constructing a nonlinear kinematic model of a tracked robot, and designs a PID fallback compensation controller for a critical state of near-water operation.

[0061] The application has the following beneficial effects:

[0062] 1. In view of the characteristics of large environmental noise and blurred vision of the underwater cleaning robot during wall adhesion operation, the traditional acoustic / visual positioning method is abandoned, and a multi-sensor fusion scheme of encoder+IMU+depth gauge is innovatively proposed to solve the positioning problem of the underwater cleaning robot in the wall adhesion state.

[0063] 2. The double plane is differentiated and processed, the depth gauge and IMU (three-dimensional correction) are introduced in the ship side positioning, and only the IMU (two-dimensional correction) is used in the ship bottom positioning, so as to be more suitable for the working state of the robot on the ship wall or the ship bottom, and the positioning is more accurate.

[0064] 3. A target point updating mechanism with geometric distance constraint is proposed to adjust the nearest neighbor point of the reference trajectory in real time, and solve the tracking lag problem caused by the mismatch of the updating rate of the traditional method. BRIEF DESCRIPTION OF DRAWINGS

[0065] Figure 1 is the inertial coordinate system of the ship wall.

[0066] Figure 2 is the inertial coordinate system of the ship bottom.

[0067] Figure 3 is the tracked model of the underwater cleaning robot.

[0068] Figure 4 is the similarity and difference of the ship side and the ship bottom fusion positioning.

[0069] Figure 5 is the logic block diagram of the underwater cleaning robot positioning on the ship surface.

[0070] Figure 6 is the target point updating block diagram in the path tracking process.

[0071] Figure 7 is the fallback control logic block diagram. DETAILED DESCRIPTION

[0072] With reference to the accompanying drawings: clearly and fully describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the scope of the present application.

[0073] I. Underwater robot hull surface positioning method

[0074] 1. Coordinate system establishment

[0075] In order to facilitate simulation experiment analysis, the present method simplifies the ship side and the ship bottom into planes. The hull surface coordinate systems O-XY and O-XZ shown in Figure 1 and Figure 2 are established as inertial coordinate systems to simulate robot side climbing and bottom climbing, and the origin position is close to the bow side, wherein, the X-O-Z plane Figure 1 represents the hull side, and the X-O-Y plane Figure 2 represents the hull bottom. The body coordinate system with the center of gravity as the origin is established, and the conversion matrix between the body coordinate system and the inertial coordinate system is .

[0076] The present method fuses data for positioning through the measurement data of three different sensors: integrated wheel encoder, inertial measurement unit (IMU) and depth gauge. The encoder measures the forward speed and heading angular velocity of the robot, the IMU measures the yaw angle and pitch angle of the robot, and the depth gauge measures the depth of the position where the robot is located. Among them, when the robot adheres to the side wall, the IMU pitch angle represents the heading angle of the robot, and when the robot adheres to the ship bottom, the IMU yaw angle represents the heading angle of the robot. The state parameters of the robot are represented as coordinate position and heading angle . Therefore, the state parameters of the robot can be estimated according to the measurement values of the three sensors .

[0077] 2. Sensor model construction

[0078] 2.1. Establishment of encoder measurement model

[0079] A kinematic analytical model based on a double-track structure is established for the tracked underwater cleaning robot, and the robot adopts a symmetrically distributed independent drive track system, as shown in Figure 3As shown, the structural parameters thereof include a driving wheel radius, a track width, a track spacing, and an effective length of a ground contact section (see Table 1 for details).

[0080] Table 1 Track structure parameters

[0081] Name Symbol Value Unit Track drive wheel radius r driver ]]> 0.046 m Encoder wheel radius r encoder ]]> 0.0398 m Track pitch ​ 1.3 m Encoder wheel pitch <L2> 0.44 m Track ground contact length [[ L belt ]]> 0.85 m

[0082] In order to improve the accuracy of the mileage data, the present method uses an independent encoder wheel as a mileage acquisition sensor, which is respectively installed at the middle positions of the two sides of the bottom of the robot Figure 3 For ease of analysis, the present method makes the following assumptions for the track walking mechanism:

[0083] (1) The track and the encoder wheel are both regarded as rigid bodies, and the track and the encoder wheel have no elastic deformation;

[0084] (2) When no slip parameter is added, the contact surfaces between the track and the ship body and between the encoder wheel and the ship body are both pure rolling;

[0085] (3) It is assumed that the dynamic characteristics (adhesion, friction coefficient) of the left and right tracks are completely the same, and the pressure distribution between the track and the ship body is uniform, and the dynamic characteristics of the two encoder wheels are completely the same;

[0086] (4) It is assumed that the walking process of the robot is carried out on a plane, without the need to cross obstacles, and the robot rotates around a certain instantaneous center when turning.

[0087] The track robot crawling kinematics equation is established: (1-1)

[0088] In the formula, , are the linear velocities of the left and right encoder wheels respectively; is the forward speed of the robot; is the heading angular velocity of the robot.

[0089] The relationship between the linear velocity and the rotation speed of the encoder wheel: (1-2)

[0090] In the formula, is the linear velocity of the encoder wheel; is the rotation speed of the encoder wheel, and the unit is revolutions per second.

[0091] 2.2, IMU measurement model establishment

[0092] (1) Accelerometer noise model

[0093] The output noise of the accelerometer is composed of white noise, random walk noise and bias instability noise: (1-3)

[0094] where, is the ideal acceleration value; is the acceleration bias; is the bias decay factor, , is the time constant of the accelerometer bias error; is the random walk noise, is the noise density; is the bias instability noise, is the noise density; is the white noise, is the noise density; is the rotation matrix from the navigation coordinate system to the body coordinate system; is the gravitational acceleration, ; is the accelerometer sampling time.

[0095] (2) Gyroscope noise model

[0096] The output noise of the gyroscope is composed of white noise, random walk noise and bias instability noise: (1-4)

[0097] where, is the ideal angular velocity value; is the gyroscope bias; is the bias decay factor, , is the bias time constant; is the white noise, is the noise density; is the random walk noise, is the noise density; is the bias instability noise, is the noise density; is the gyroscope sampling time.

[0098] 2.3, Depth gauge measurement model

[0099] (1) Water depth measurement model

[0100] The vertical position of the underwater robot is derived from the wave kinematics and the carrier kinematics: (1-5)

[0101] where z is the true water depth of the position of the underwater robot; is the wave height model based on the linear wave theory, whose formula is as follows:

[0102] (1-6)

[0103] (1-7)

[0104] (1-8)

[0105] where H is the significant wave height; k is the wave number; is the angular frequency; T is the wave period; is the wavelength; is the phase shift.

[0106] According to the sea wave level standard of China, the method selects 2 small waves as the wave environment, and the parameters are set as follows:

[0107] (2) Measurement noise model

[0108] The sensor measurement value includes the theoretical value of the robot sensor measurement and noise: (1-9)

[0109] where, is the depth sensor Gaussian noise.

[0110] 3. Multi-sensor fusion positioning method based on EKF

[0111] In view of the nonlinear characteristics of the wall motion of the underwater robot, the method adopts EKF (extended Kalman filter) as the core data fusion framework. EKF overcomes the limitations of traditional Kalman filter through local linearization method, and its core principle can be summarized as follows: the first-order Taylor expansion is performed on the nonlinear state transition equation and the observation equation to construct the Jacobian matrix and , so as to convert the nonlinear system into a time-varying linear system for recursive estimation.

[0112] The EKF model used in this study is as follows: (1-10)

[0113] where, is the state vector of the Kalman filter at time t; is the process noise at time t-1, ; is the process function; is the measurement vector of the extended Kalman filter; is the sensor measurement noise, ; is the measurement function.

[0114] The EKF filter method is divided into a state prediction stage and a measurement update stage, as follows:

[0115] 1) State prediction: calculate the predicted state, determine the state transition Jacobian matrix, and use it to calculate the covariance. (1-11)

[0116] (1-12)

[0117] (1-13)

[0118] In the formula, is the predicted state; is the state at the previous time; is the control vector at the previous time; Ft-1 is the state transition Jacobian matrix; is the covariance matrix; Qt-1 is the predicted process noise.

[0119] 2) Prediction update: calculate the Jacobian matrix of the observation equation and the Kalman filter gain; combine the state prediction with the measurement to provide the posterior state and covariance estimates.

[0120] (1-14)

[0121] (1-15)

[0122] (1-16)

[0123] (1-17)

[0124] In the formula, Ht is the observation equation Jacobian matrix; Kt is the Kalman filter gain;

[0125] Rk is the measurement noise; is the updated state vector; is the unit diagonal matrix; is the updated covariance matrix.

[0126] Figure 4 The similarities and differences between the underwater ship cleaning robot positioning on the ship bottom and the ship side are compared. It can be seen that the main difference between the two is the type of sensor input. In the ship side positioning, a depth gauge sensor is introduced, and the measurement dimension is increased from two to three. By positioning the ship bottom and the ship side through these two methods, it is helpful to compare the relationship between the positioning accuracy of the EKF method and the type of sensor input.

[0127] The complete positioning process of the robot is as follows Figure 5The specific implementation is as follows:

[0128] 1) State prediction stage

[0129] In the hull surface positioning state prediction stage, the ship wall and the ship bottom prediction method is the same, and the double encoder is used to collect the robot double-sided track line speed data in real time to realize the motion state estimation. Based on the differential kinematics model, the average value and the difference value of the two-sided encoder wheel line speed are calculated, and the longitudinal speed and the steering angular velocity of the robot are derived respectively.

[0130] The components of the state vector include: the distance traveled by the robot along the x-axis of the inertial coordinate system , the distance traveled by the robot along the y-axis of the inertial coordinate system , and the heading angle of the robot , and the state vector is represented as follows: (1-18)

[0131] The extended Kalman filter is divided into two steps, the first step is to predict the system behavior, and the second step is to update the system behavior measurement. In the prediction stage, by combining the robot coordinate position and heading angle at the previous moment with the current linear speed and angular speed of the robot, the process prediction equation of the state vector can be obtained by using the conversion relationship between the body coordinate and the inertial coordinate system as shown in Figure 3

[0132] (1-19)

[0133] In the formula, is the linear speed of the robot at time t; is the heading angular velocity of the robot at time t.

[0134] The prediction equation of formula (1-19) is nonlinear, and by solving the Jacobian matrix of the state vector of the above formula, the state transition Jacobian matrix is linearized, and the state transition Jacobian matrix is as follows: (1-20)

[0135] Then formula (1-19) can be converted into the following matrix form: (1-21)

[0136] In the formula, is the control matrix; is the control vector, characterizes the process noise.

[0137] ​The covariance matrix at time t is introduced at time t-1 to reflect the uncertainty of the state estimation of equation (1-21), and the covariance matrix estimation equation is as follows:

[0138] (1-22)

[0139] In the equation, is the system noise at time t-1.

[0140] 2) Measurement update stage

[0141] In the measurement update stage, the encoder types of the horizontal plane and the vertical plane are different, the sensors participating in the correction of the horizontal plane are IMU, and the heading angle estimated by the encoder is corrected in real time by using the yaw angle and yaw angle velocity of the IMU, the sensors participating in the position correction in the vertical plane include IMU and depth gauge, the heading angle estimated by the encoder is corrected in real time by using the pitch angle and pitch angle velocity of the IMU, and in addition, the Z-axis position of the robot relative to the side wall of the ship body is corrected in real time by using the water depth data measured by the depth gauge.

[0142] Then, the expression of the measurement error of the extended Kalman filter is as follows:

[0143] (1-23)

[0144] In the equation, is the observation value estimation matrix, for the horizontal plane, , for the vertical plane, ; is the sensor observation value matrix, for the horizontal plane, , for the vertical plane, .

[0145] The measurement estimation matrix in the horizontal and vertical planes is respectively multiplied by the Jacobian matrix of the state vector to obtain the measurement transfer matrix , the measurement transfer matrix of the horizontal plane positioning measurement equation is as follows: (1-24)

[0146] The measurement transfer matrix of the vertical plane positioning measurement equation is as follows:

[0147] (1-25)

[0148] Then, the observation measurement estimation matrix can be written in the following matrix form: ​​(1-26)

[0149] wherein, is the sensor observation noise.

[0150] The Kalman gain is calculated by adjusting the reliability of each sensor according to the measurement accuracy of each sensor, thereby improving the accuracy of the position estimate, the Kalman gain can be obtained by calculating the updated covariance and the measurement transition matrix , the calculation equation is as follows: (1-27)

[0151] wherein, is the measurement noise covariance matrix of the sensor.

[0152] According to the Kalman gain, the prior estimate and the measurement are combined to update the state vector, which is the position obtained by fusing the encoder, IMU and depth meter, as shown in the following formula: (1-28)

[0153] The posterior error covariance matrix is updated, as shown in the following formula: (1-29)

[0154] This completes one multi-sensor data fusion, at this time return to formula (1-19) to continue the next data fusion, so on and so forth, the system outputs the fusion positioning trajectory point.

[0155] 3. Elimination of high-frequency noise of positioning trajectory

[0156] In view of the high-frequency interference phenomenon caused by sensor measurement noise in the target positioning trajectory, which is manifested as burrs in the trajectory route in the positioning trajectory result, a first-order low-pass filtering algorithm is adopted to filter out the high-frequency noise of the positioning trajectory, and the digital implementation form can be represented by the following difference equation: (1-30)

[0157] wherein, is the current time filtering output value; is the current time input signal; is the previous time sequence period filtering output; is the smoothing coefficient, .

[0158] The cut-off frequency and the smoothing coefficient satisfy the following relationship: (1-31)

[0159] wherein T is the sampling period.

[0160] In this method, the sampling period of the filter is set to 0.005 s, and the cutoff frequency is set to 5 Hz.

[0161] II. Path tracking method for underwater cleaning robot ship surface

[0162] 1. ILQR control path tracking method

[0163] 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 algorithm is to iteratively linearize the nonlinear dynamic model and approximate the cost function, and gradually optimize the control input, so as to realize the optimization of system performance. ILQR control method cannot be implemented without the discretization of nonlinear dynamic model and the minimization of comprehensive cost function, which includes two parts: one is the cost of state, and the other is the cost of control input. The state cost measures the deviation between the current state and the reference trajectory, and the control cost punishes the size of the control input. State sequence and control sequence In the early stage of the algorithm, the current state of the underwater robot is initialized by estimation. The state dimension includes position coordinates x, y, heading angle and velocity , denoted as x = , the control input dimension includes the velocity and angular velocity of the tracked robot, denoted as The discrete-time nonlinear dynamic model and the cost function are as follows: (2-1) (2-2)

[0164] In the formula, is the state cost function, which is twice differentiable; is the terminal cost function, which is twice differentiable; is the system state at time k; is the control input at time k.

[0165] For a given cost function, the Bellman optimality theorem can be used to define the optimal cost , which is represented as follows: (2-3)

[0166] (2-4)

[0167] Let , then:

[0168] (2-5)

[0169] In the local neighborhood of the reference trajectory, the remaining cost function is expressed as follows:

[0170] (2-6)

[0171] (2-7)

[0172] The nonlinear system of expression (2-1) is approximated using a first-order Taylor series expansion:

[0173] (2-8)

[0174] where, The Jacobian matrix of x at time k; The Jacobian matrix of u at time k.

[0175] Based on the current state sequence and the control sequence, the next state sequence is predicted, and expression (2-1) is expressed in the state space as follows:

[0176] (2-9)

[0177] Then, in expression (2-7):

[0178] (2-10)

[0179] (2-11)

[0180] (2-12)

[0181] (2-13)

[0182] (2-14)

[0183] where

[0184] (2-15)

[0185] (2-16)

[0186] (2-17)

[0187] (2-18)

[0188] (2-19) ​​

[0189] The partial derivative of the cost function (2-7) with respect to δx is The partial derivative of the cost function (2-7) with respect to δx is

[0190] (2-20)

[0191] To solve the optimal value of the cost function (2-20), the partial derivative of the cost function (2-20) with respect to δx must be equal to zero, and the optimal value of δx is solved and denoted as

[0192] (2-21)

[0193] Let

[0194] (2-22)

[0195] (2-23)

[0196] Then, the cost function (2-21) can be simplified as follows:

[0197] (2-24)

[0198] Let and be the gradient and Hessian approximation of , respectively. Based on the dynamic programming method, the tail problem of with N time steps can be solved, and and are defined as follows:

[0199] (2-25)

[0200] (2-26)

[0201] Substitute of the cost function (2-24) into the cost function (2-7), and take the first and second order partial derivatives of the cost function (2-7) with respect to δx, then and can be updated by

[0202] (2-27)

[0203] (2-28)

[0204] Based on the control increment calculated by backpropagation, ILQR can update the control input sequence. The control input ​​​​The update formula of is:

[0205] (2-29)

[0206] Based on the robot positioning method of the present application, the relationship between the state of the robot and the control input is represented by a nonlinear dynamic model as follows: (2-30)

[0207] In the formula, , respectively represent the horizontal axis coordinates before and after updating; , respectively represent the vertical axis coordinates before and after updating; , respectively represent the heading angles before and after updating; , respectively represent the robot forward speeds before and after updating; is the control input angular velocity; is the control input forward speed.

[0208] In the tracked robot walking mechanism, and are the Jacobian matrices of the system state equation and the control input equation respectively, representing the linear approximation of the system dynamic model to the state and the control input, and using these two matrices can approximate the nonlinear dynamic system to a linear system, so that it can be used in the ILQR algorithm.

[0209] and The specific content is as follows:

[0210] (2-31)

[0211] (2-32)

[0212] The quadratic cost function is set as follows: (2-33)

[0213] In the formula, is the state cost function; is the terminal state cost function; Q is the cost matrix of the state, , respectively represent the weight values of the horizontal and vertical coordinate errors, the yaw angle error and the speed error, and the weight values can reflect the importance of the variables, and the greater the weight value, the faster the corresponding state variable converges; R is the cost matrix of the control input, , respectively represent the control weight values of the forward speed and the angular velocity; cost matrix for terminal state; state for reference trajectory; control input.

[0214] Then, the cost gradient and Hessian matrix in equations (2-15) to (2-19) are as follows:

[0215] (2-34)

[0216] (2-35)

[0217] (2-36)

[0218] (2-37)

[0219] (2-38)

[0220] Then, the cost gradient and Hessian matrix in equations (2-15) to (2-19) are as follows: Substituting the expressions into equations (2-25) and (2-26), we have:

[0221] (2-39)

[0222] (2-40)

[0223] According to equation (2-29), the control sequence , The first two dimensions of the sequence are the output control quantities of the current round of path tracking controller, which are the expected values of the forward speed and angular speed of the robot, respectively. The expected speeds of the left and right drive wheels are solved according to equations (1-1) and (1-2) and sent to the robot, thereby realizing the expected path tracking of the robot.

[0224] To solve the tracking lag problem caused by the mismatch of the target point update rate in basic trajectory tracking, this method proposes a dynamic look-ahead strategy based on geometric distance constraints. By calculating the current position of the robot and the nearest neighbor point of 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 the dynamic threshold, the system will recalculate the nearest neighbor point on the reference trajectory as the next tracking target. This mechanism updates the target sequence through closed-loop feedback iteration, effectively suppressing the phase delay phenomenon of trajectory tracking. As shown in Figure 6 The algorithm framework embeds geometric correlation constraints into the ILQR optimization loop, forming a closed-loop architecture of dynamic target point calibration and controller collaborative optimization, which significantly improves the robustness of path tracking.

[0225] 2. Water surface proximity fallback control method

[0226] Propeller cavitation is a typical phenomenon in fluid dynamics, and its physical nature is that when the propeller blade rotates, the local flow field pressure drops below the saturated vapor pressure of the fluid, causing the phase change of liquid water into steam bubbles. The bubbles collapse when they migrate to high pressure areas, producing high frequency shock waves and micro jets. This process causes problems such as nonlinear attenuation of propeller thrust and imbalance of robot adhesion. Therefore, for the adhesion imbalance problem of underwater cleaning robots caused by propeller cavitation in the near water surface area, this method proposes a self-adaptive fallback control method based on attitude feedback. When the liquid level sensor monitoring value is lower than 80 cm, it is determined that the robot enters the near water surface critical area, at this time the propeller causes the thrust to be nonlinearly attenuated due to the flow field cavitation effect, inducing uneven distribution of track adhesion and a sharp increase in slip probability, at this time it is difficult to continue path tracking.

[0227] The fallback control logic is based on the dynamic threshold setting of the pitch angle: if the pitch angle is less than -45°, the reverse motion command is triggered; if the pitch angle is greater than 45°, the forward motion command is executed; otherwise, the rotation heading command is executed. This control logic adjusts the attitude and position cooperatively, forcing the robot to retreat to a safe operating depth (liquid level ≥ 80 cm) to avoid the continuous destruction of the flow field caused by cavitation. As shown in Figure 7 , this strategy effectively maintains the dynamic stability of the contact mobile platform through closed-loop state monitoring and multi-modal control command switching.

[0228] During the process, the forward command, the backward command, and the rotation heading command are all controlled by PID controllers. Among them, the controller expressions of the forward and backward commands are as follows:

[0229] (2-41)

[0230] In the formula, , are the target depth and the current depth of the robot, respectively; , , are the distance gain parameters of the fallback controller.

[0231] The expression of the robot heading angle rotation controller is as follows: (2-42)

[0232] In the formula, , are the target heading angle and the current heading angle of the robot, respectively;

[0233] , , for the angle gain parameter of the back-off controller.

[0234] The speed control amount of the track motor is calculated by the above two PID controllers, and is sent to the two track motors after mapping to the speed to drive the track robot to complete the turning, forward and backward movement, etc. Finally, the robot moves to the vicinity of the reference tracking trajectory below the water depth of 80 cm, and then the robot continues to perform the ILQR control algorithm to track the path.

[0235] It will be obvious to a person skilled in the art that, without departing from the spirit or essential characteristics of the application, the present application can be implemented in other specific forms. The present examples are therefore to be considered in all respects as illustrative and not restrictive.

Claims

1. A method for positioning a hull surface of an underwater cleaning robot, characterized in that: The following steps are involved: S1. Obtain the forward velocity and heading angular velocity of the underwater cleaning robot through the wheel encoder carried by the robot, obtain the yaw angle and pitch angle of the robot through the inertial measurement unit (IMU), and obtain the depth data of the robot through the depth meter; and establish an inertial coordinate system on the hull surface; S2, fuse the above data through extended Kalman filter; In the behavior prediction phase, 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 between the previous moment and the current moment in the prediction phase is introduced to reflect the uncertainty of the state vector estimate. During the measurement update phase, the yaw angle and yaw rate of the inertial measurement unit (IMU) are used to correct the heading angle estimated in the horizontal plane in real time. The pitch angle and pitch rate of the inertial measurement unit (IMU) are used to correct the heading angle estimated in the vertical plane in real time. The water depth of the depth meter is used to correct the Z-axis position estimated in the vertical plane in real time. S3. After filtering the sensor measurement noise using a first-order low-pass filtering algorithm, the coordinates of the underwater cleaning robot on the hull surface are obtained.

2. The underwater cleaning robot positioning and tracking method according to claim 1, characterized in that: In step S2, the components of the state vector include: the distance the robot travels along the x-axis of the inertial coordinate system , the robot travels along the y-axis of the inertial coordinate system , the robot's heading angle , the state vector is expressed as follows: (1-18) In the prediction stage, the process prediction equation of the state vector is as follows: (1-19) Where, is the linear velocity of the robot at time t; is the robot's heading angular velocity at time t; By solving the above equation, the state vector The Jacobian matrix of the above formula is linearized, and the state transfer Jacobian matrix As shown in the following formula: (1-20) Then formula (1-19) can be converted into the following matrix form: (1-21) Where, is the control matrix; is the control vector, Characterize process noise; Introduce the covariance matrix of time t-1 to time t in the prediction stage , to reflect the uncertainty of the state estimation of formula (1-21), the covariance matrix estimation equation is as follows: (1-22) Where, is the system noise at time t-1.

3. The underwater cleaning robot positioning and tracking method according to claim 2, characterized in that: In the measurement update phase, the measurement error of the extended Kalman filter The expression is as follows: (1-23) Where, is the observation value estimation matrix, for the horizontal plane, , for the vertical plane, ; is the sensor observation matrix, for the horizontal plane, , for the vertical plane, ; The measurement estimation matrices in the horizontal and vertical planes For the state vector Jacobian matrix, obtain the measurement transfer matrix , the measurement transfer matrix of the horizontal plane positioning measurement equation as follows: (1-24) Measurement transfer matrix of vertical plane positioning measurement equation as follows: (1-25) Then, the observation measurement estimation matrix can be written as the following matrix form: (1-26) Where, Observe noise for the sensor; By calculating the Kalman gain , adjust the reliability of each sensor according to the measurement accuracy of each sensor, thereby improving the accuracy of position estimation, Kalman gain The updated covariance can be and the measurement transfer matrix The calculation equation is as follows: (1-27) Where, is the sensor’s measurement noise covariance matrix; According to the Kalman gain, combined with the prior estimation and measurement, the state vector is updated. The state vector is the position obtained after the encoder, IMU, and depth meter are fused, as shown in the following formula: (1-28) Update the posterior error covariance matrix as shown below: (1-29) At this point, a multi-sensor data fusion is completed. At this time, the return formula (1-19) continues to perform the next data fusion, and this cycle repeats until the system outputs the fused positioning trajectory point.

4. The underwater cleaning robot positioning and tracking method according to claim 3, characterized in that: In step S3, the first-order low-pass filtering algorithm is characterized by the following difference equation: (1-30) Where, The filtered output value at the current moment; Input signal for the current moment; Filter output for the previous timing cycle; is the smoothing coefficient, ; Cutoff frequency The smoothing coefficient satisfies the following relationship: (1-31) Where T is the sampling period.

5. A method for tracking the path of a hull surface by an underwater cleaning robot, characterized in that: Based on the positioning method described in claim 1 and combined with the iterative linear quadratic regulator ILQR, the path tracking of the underwater cleaning robot is realized.

6. The method for tracking the path of the hull surface of an underwater cleaning robot according to claim 5, wherein: Based on the robot localization method, the relationship between the robot state and control input is expressed by a nonlinear dynamic model as follows: (2-30) Where, 、 Respectively represent the horizontal axis coordinates before and after the update; 、 Respectively represent the vertical axis coordinates before and after the update; 、 Respectively represent the heading angles before and after the update; 、 Respectively represent the robot's forward speed before and after the update; To control the input angular velocity; To control the input forward speed; The quadratic cost function is as follows: (2-33) Where, is the state cost function; is the terminal state cost function; Q is the state cost matrix, , Represent the weight values ​​of horizontal and vertical coordinate errors, yaw angle errors, and speed errors respectively; R is the cost matrix of control input, , Represents the control weight values ​​of forward speed and angular velocity respectively; is the cost matrix of the terminal state; is the state of the reference trajectory; is the control input; is the system state at time k; is the control input at time k.

7. The method for tracking the path of the hull surface of an underwater cleaning robot according to claim 6, wherein: For a given cost function, using Bellman's optimality theorem, the optimal cost is defined through a recursive relationship, and the cost gradient and Hessian matrix are as follows: (2-34) (2-35) (2-36) (2-37) (2-38) definition and are cost gradient and Hessian matrix approximations respectively, and the formula (2-33) Substituting the expression into the equation yields: (2-39) (2-40) The control input sequence is updated by the optimal value of the residual cost function. The first two dimensions of the control input sequence are calculated as the output control quantities of the wheel 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 inversely solved and sent to the robot, thereby realizing the robot's expected path tracking.

8. The method for tracking the path of the hull surface of an underwater cleaning robot according to claim 6, wherein: The Jacobian matrix of the state and control input is expressed as follows: (2-31) (2-32) Where, and are the Jacobian matrices of the system state equation and the control input equation respectively; using these two matrices, the nonlinear dynamic system can be approximated as a linear system, which can be used in the ILQR algorithm.

9. The method for tracking the path of a hull surface by an underwater cleaning robot according to claim 5, wherein: 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°, the reverse motion command is triggered, forcing the robot to retreat to the safe operating depth; if the pitch angle is greater than 45°, the forward motion command is executed; in other states, the rotation heading command is executed first.

10. The method for tracking the path of the hull surface of an underwater cleaning robot according to claim 9, wherein: The forward command, backward command, and rotation heading command are all controlled by PID controller. The controller expressions of the forward and backward commands are as follows: (2-41) Where, 、 They are the target depth and current depth of the robot respectively; 、 、 is the distance gain parameter of the fallback controller; The expression of the robot's heading angle rotation controller is as follows: (2-42) Where, 、 are the robot's target heading angle and current heading angle respectively; 、 、 is the angle gain parameter of the backoff controller.

Images