Quaternion underwater positioning method applied to aquaculture pond decontamination robot

Abstract

The invention discloses a quaternion underwater positioning method applied to a trash cleaning robot of an aquaculture pond, and relates to the technical field of underwater positioning, comprising the following steps: collecting size parameters of the aquaculture pond and judging the shape of the aquaculture pond; iMU data of an inertial measurement unit and distance measurement data and position parameters of an underwater ultrasonic sensor are collected, an extended Kalman filter is adopted for state vector prediction, an observation model is constructed according to the shape of the fishpond to update the extended Kalman filter, and accurate coordinates are extracted from predicted state vectors; regularly carrying out state vector geometric optimization solution according to the shape of the fishpond to obtain a geometric state vector; differences between the state vectors and the geometric state vectors are compared, accurate coordinates are extracted from the geometric state vectors when the differences are large, and the geometric state vectors serve as current state vectors. The method is suitable for a circular or regular polygonal culture fishpond to realize accurate positioning of the cleaning robot, and the positioning reliability and efficiency are improved.

Description

Technical Field

[0001] This invention relates to the field of underwater positioning technology, and more specifically to a quaternion underwater positioning method applied to a cleaning robot for aquaculture ponds. Background Technology

[0002] In factory-scale recirculating aquaculture systems, the ponds are typically circular or polygonal, with a sloping conical bottom designed to facilitate water circulation and allow bottom sediments to flow towards the center and be discharged. However, in actual operation, some fish feces, uneaten feed, and fish secretions still adhere to the bottom, especially accumulating in areas where the slope changes or in low-lying areas, making it difficult for water to remove them completely. These accumulated pollutants not only contaminate the water quality and affect the health of the aquaculture environment but may also hinder the normal operation of the water flow and aeration systems, ultimately negatively impacting the growth and health of the farmed fish.

[0003] To address the challenge of cleaning bottom debris in aquaculture ponds, underwater cleaning robots are increasingly becoming key equipment in industrialized aquaculture. They replace the arduous labor of traditional manual scrubbing, automating pond bottom cleaning and improving efficiency while reducing labor intensity. However, for stable and efficient automated cleaning, underwater cleaning robots must be able to accurately locate and navigate within ponds of varying shapes and with sloping conical bottoms. Because aquaculture ponds often have low visibility and contain large amounts of suspended debris, and because water temperature, salinity, and the distribution and dynamic changes of fish feces, food, and other sediments are uneven, commonly used optical positioning methods such as laser ranging on land, or conventional underwater acoustic positioning methods, often fail to work effectively in this environment, resulting in a significant decrease in positioning accuracy and reliability.

[0004] Furthermore, while iterative algorithms can provide accurate positioning results, they are computationally intensive and demanding on computing resources, especially in real-time positioning in dynamic environments. This can lead to slow system response times, affecting the system's real-time performance and robustness. In some extreme environments, the algorithm may converge slowly or fail to converge, impacting the accuracy and efficiency of real-time positioning. Overall, while existing technologies can provide high-precision positioning, they still face challenges in terms of adaptability, deployment complexity, and computational resource requirements.

[0005] Therefore, how to improve the positioning accuracy and efficiency of underwater cleaning robots in complex underwater environments is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0006] In view of the above problems, the present invention is proposed to provide a quaternion underwater positioning method for aquaculture pond cleaning robots that overcomes or at least partially solves the above problems. This method is applicable to circular or regular polygonal fish ponds to achieve accurate positioning of cleaning robots, thereby improving positioning reliability and efficiency.

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

[0008] In a first aspect, embodiments of the present invention provide a quaternion-based underwater positioning method for a cleaning robot used in aquaculture ponds. The cleaning robot is equipped with an inertial measurement unit and underwater ultrasonic sensors are located at the center of its four sides (front, back, left, and right). The method includes the following steps: Step 1: Collect the size parameters of the aquaculture pond and determine the shape of the fish pond; Step 2: Collect IMU data from the inertial measurement unit and ranging data and position parameters from the underwater ultrasonic sensor. Use an extended Kalman filter to predict the state vector. Build an observation model based on the shape of the fishpond to update the extended Kalman filter and extract precise coordinates from the predicted state vector. Step 3: Periodically optimize the state vector of the cleaning robot by using distance measurement data, position parameters and the current state vector based on the shape of the fish pond; Step 4: Calculate the difference between the predicted state vector and the geometric state vector. If the difference is within the preset fusion range, use the predicted state vector as the current state vector and return to Step 2; otherwise, extract the precise coordinates from the geometric state vector, use the geometric state vector as the current state vector, and return to Step 2.

[0009] Preferably, the aquaculture pond includes a circular fish pond or a regular polygonal fish pond; the top surface of the circular fish pond is circular, and the bottom surface is conical, with dimensional parameters including radius. Pool bottom inclination angle and slope The top surface of the regular polygonal fishpond is a regular polygon, and the bottom surface is a pyramidal surface. Dimensional parameters include the number of sides N, side length L, and the angle of inclination of the pond bottom. Given the vertex azimuth, the coordinates of the bottom vertex, and the slope S, calculate the azimuth of the i-th vertex. circumcircle radius Inscribed circle radius The coordinates of the i-th bottom vertex Vertical height of the bottom vertex .

[0010] Preferably, the specific process of step 2 is as follows: Step 21: Acquire IMU data from the inertial measurement unit and ranging data and position parameters from the underwater ultrasonic sensor. The IMU data includes angular velocity and acceleration; the initial state vector of the cleaning robot and the covariance matrix of the extended Kalman filter. Step 22: Based on the current state vector and IMU data, use the kinematic model and extended Kalman filter to predict the state vector, obtain the state prior estimate, and update the covariance matrix; Step 23: Construct an observation model based on the shape of the fishpond using the state prior estimate, update the state prior estimate using the ranging data to obtain the state posterior estimate, normalize the attitude quaternions in the state posterior estimate and update the covariance matrix. Step 24: Use the posterior state estimate as the predicted state vector.

[0011] Preferably, in step 22, an extended Kalman filter (EKF) is used to predict the state vector of the cleaning robot in real time based on precise coordinates, and the state vector at the next moment is used to construct a state prior estimate. The state vector at the next moment includes the position, attitude quaternion, linear velocity, and angular velocity of the cleaning robot at the next moment; the IMU data includes three-axis angular velocities. Triaxial acceleration and heading angle; the specific process is as follows: Based on the current state vector of the cleaning robot, the EKF algorithm is used to predict its position, velocity, attitude, and covariance. The expression is as follows: Location prediction formula: ; Speed ​​prediction formula: ; Attitude prediction formula: ; ; Covariance prediction formula: ; in, To determine the location of the cleaning robot in the next moment; The linear velocity of the cleaning robot at the next moment; The quaternion for the posture of the cleaning robot in the next moment; This is the updated lower difference matrix; The position of the cleaning robot in the state vector at the current time k. = , These are the x-axis, y-axis, and z-axis coordinates of the cleaning robot in the robot coordinate system M, respectively. Let k be the linear velocity of the cleaning robot in the current state vector. = ; Let k be the quaternion of the robot's posture in the state vector at the current time k. = ; Angular velocity in IMU data = , These represent the angular velocities of the cleaning robot along the x, y, and z axes in the robot coordinate system M, respectively. This refers to the total acceleration of the cleaning robot. Acceleration in IMU data; It is the acceleration due to gravity; Let be the component of gravity on the inclined plane; n be the normal vector of the bottom of the fish; F be the state transition matrix; Q be the process noise covariance matrix. Let be the covariance matrix of the current state; Indicates the time interval for prediction; This represents the attitude quaternion generated within the predicted time interval; T represents the transpose; the acceleration of the cleaning robot includes the component of gravity on the inclined plane. , , The current state vector of the cleaning robot. , These represent the x-axis, y-axis, and z-axis coordinates of the cleaning robot in the robot coordinate system M. These are the attitude quaternions of the cleaning robot in robot coordinate system B. These represent the linear velocities of the cleaning robot along the x, y, and z axes in the robot coordinate system M. These represent the angular velocities of the cleaning robot along the x, y, and z axes in the robot coordinate system M.

[0012] Preferably, the specific process of obtaining the posterior state estimate in step 23 is as follows: Step 231: Transform the position parameters of the underwater ultrasonic sensor from the robot coordinate system to the world coordinate system using the attitude quaternion in the state prior estimation; Step 232: Based on the shape of the fishpond, establish a geometric intersection model of the ultrasonic ray and the pond wall for the converted underwater ultrasonic sensor, and solve for the theoretical distance value based on the state prior estimation as the observation model; Step 233: Calculate the observation residuals based on the observation model and the actual distance values ​​in the distance measurement data; Step 234: Calculate the Kalman gain based on the covariance matrix; Step 235: Correct the state prior estimate based on the Kalman gain and observation residuals to obtain the state posterior estimate.

[0013] Preferably, the world coordinate system W is a three-dimensional coordinate system established with the geometric center of the bottom of the aquaculture pond as the origin. The robot coordinate system M is a three-dimensional coordinate system established with the geometric center of the cleaning robot as the origin. ; , , These represent the robot's forward direction, the robot's left side, and the robot's top upward direction, respectively; the position parameters collected by the underwater ultrasonic sensor. Including position coordinates and orientation M represents the robot coordinate system, F represents the front, B represents the rear, L represents the left, and R represents the right. Based on the attitude quaternions in the state prior estimation, the position parameters are transformed from the robot coordinate system M to the world coordinate system W, expressed as:

[0014]

[0015]

[0016] in, This represents the starting point of the ultrasonic ray emitted by the j-th underwater ultrasonic sensor after transformation to the world coordinate system W, i.e., the position of the j-th underwater ultrasonic sensor; m represents the center coordinates of the cleaning robot in the world coordinate system. R(q) represents the rotation matrix that rotates the robot coordinate system M to the world coordinate system W. Represents the attitude quaternion in the world coordinate system W; This indicates the direction in which the ultrasonic rays emitted by the j-th underwater ultrasonic sensor are transformed into the world coordinate system.

[0017] Preferably, in step 232, the geometric intersection model is constructed based on the shape of the fishpond using underwater ultrasonic sensor ranging data and the equation of the bottom surface of the aquaculture pond. The specific process is as follows: Step 2321: Construct a geometric intersection model based on the ultrasonic ray equation and the equation of the bottom surface of the aquaculture pond corresponding to the shape of the fishpond; When the aquaculture pond is a circular fishpond, the geometric intersection model expression is:

[0018]

[0019] Where t represents the theoretical ranging value, and r(t) represents the equation of the ultrasonic ray emitted by the underwater ultrasonic sensor; The equation for the conical surface of a circular fishpond. The angle of inclination of the pond bottom is represented by x and y, which represent the x-axis and y-axis coordinates of the bottom surface of the aquaculture pond in the world coordinate system, respectively. This represents the starting point of the ultrasonic ray emitted by the j-th underwater ultrasonic sensor in the world coordinate system. This represents the direction in which the ultrasonic beam emitted by the j-th underwater ultrasonic sensor is transformed to the world coordinate system; by simultaneously solving the above two equations, we can derive: ,make:

[0020] Organized into The quadratic equation is obtained as follows: ; in, These represent the x-axis, y-axis, and z-axis coordinates of the starting point of the ultrasonic ray emitted by the j-th underwater ultrasonic sensor in the world coordinate system, respectively. Let x, y, and z represent the coordinate components of the direction of the ultrasonic ray emitted by the j-th underwater ultrasonic sensor in the world coordinate system; t represents the theoretical distance measurement value. When the aquaculture pond is a regular polygonal fishpond, the geometric intersection point model expression is:

[0021]

[0022] in, Let i be the equation of the i-th side bottom surface. Let represent the normal azimuth angle of the i-th side bottom surface; by simultaneously solving the above two equations, we can derive: ; Where N represents the number of sides on the top face of the regular polygonal fishpond; This represents the theoretical distance measurement value of the i-th side bottom surface; Step 2322: Obtain the starting point of the ultrasonic rays emitted by the underwater ultrasonic sensor (the position of the underwater ultrasonic sensor) based on the position in the state prior estimation. Calculate the orientation of the ultrasonic rays emitted by the underwater ultrasonic sensor based on the attitude quaternion in the state prior estimation. Solve for the theoretical ranging value as the observation model. For a circular fishpond, find the smallest positive real root as the theoretical distance measurement value. ; For a regular polygonal fishpond, the smallest valid positive real root satisfying the validity constraint is used as the theoretical distance measurement value t; the validity constraint is expressed as:

[0023] Traverse all vertex segments Check the validity of the intersection points and select the smallest valid positive real root. The corresponding intersection point obtained by substituting it into the ultrasonic ray equation The theoretical intersection point; among which, Represents vertices x-axis coordinates and y-axis coordinates Represents vertices x-axis coordinates and y-axis coordinates They represent the intersection points respectively. x-axis and y-axis coordinates; This indicates a constant value to be set.

[0024] Preferably, the observation model for the j-th ultrasound is as follows: This model represents the distance that ultrasound should be able to measure; then, ground-level constraints are established, especially when the aquaculture pond is a circular fishpond. When the aquaculture pond is a regular polygonal fish pond, the ground-attached constraint This model represents the difference between the robot's current position height and the height of the bottom surface; The angle of inclination of the pool bottom. is the vertical height of the vertex of the base; r is the radius of the inscribed circle corresponding to the pyramidal surface; For the first The normal azimuth angle of each side bottom surface; x, y, and z represent the x-axis, y-axis, and z-axis coordinates of the bottom surface of the aquaculture pond in the world coordinate system W obtained from the state vector.

[0025] Preferably, in step 232, the observation residual y is calculated based on the observation model and the actual distance measurement value in the distance measurement data; The observation residual corresponding to the j-th underwater ultrasonic sensor is , To utilize state prior estimation Calculated theoretical observations; Given the actual distance measured in the distance measurement data; calculate the total observation residual y by combining all observations into observation vectors z and h(X); ; ; .

[0026] Preferably, in step 233, the Kalman gain is calculated based on the covariance matrix; the expression is: ; ; Where K represents the Kalman gain; H is the observation Jacobian matrix; S is the covariance matrix corresponding to the state prior estimate; S represents the innovation covariance, which is the covariance of the difference between the theoretical observation and the actual distance measurement, consisting of the prediction error projected onto the measurement space and the measurement noise inherent in the sensor; R is the observation noise covariance.

[0027] Preferably, in step 234, the state prior estimate is corrected based on the Kalman gain and the observation residual to obtain the state posterior estimate; pass Perform state vector update, where This is a priori estimation of the state based solely on kinematic model predictions; This represents the posterior state estimate; to ensure the validity of the attitude, the attitude quaternions in the posterior state estimate are normalized. Then, the covariance matrix is ​​further updated. .

[0028] Preferably, the specific process of step 3 is as follows: Step 31: Calculate the attitude quaternion of the cleaning robot based on the shape and size parameters of the fish pond, and transform the position parameters of the underwater ultrasonic sensor from the robot coordinate system to the world coordinate system based on the attitude quaternion; Step 32: Solve for the theoretical intersection point between the ultrasonic rays emitted by the underwater ultrasonic sensor and the pool wall based on the geometric intersection model and ranging data; Step 33: Based on the shape of the fishpond, construct a nonlinear least squares objective function containing constraint equations using theoretical intersection points and state vectors, and perform iterative optimization using the Gauss-Newton method to obtain the geometric optimization solution as the geometric state vector.

[0029] Preferably, the specific process of step 31 is as follows: Step 311: The cleaning robot runs close to the bottom of the aquaculture pond. (In the robot's coordinate system...) Approximate the normal vector to the bottom of the aquaculture pond, based on the pond's shape and corresponding dimensional parameters, including the number of sides N, side length L, and vertex azimuth. The slope S determines the normal vector of the bottom surface of the aquaculture pond; For a circular fishpond, the base is a conical surface, and the formula for expressing a conical surface is: The formula for expressing the normal of its conical surface is: ,Right now , Normalize it to obtain the normal vector in the world coordinate system W. Simplified ; For a regular polygonal fishpond with a pyramidal base, the formula for the i-th lateral base is: ,in The vertical height of the bottom vertex. Given the normal azimuth angle of the i-th side bottom surface, its gradient can be used to obtain the plane normal vector. Calculate the horizontal projection polar angle ( Adjust to Find satisfaction Let the integer i be the value of the cleaning robot belonging to the i-th side bottom surface. Substitute i into the plane normal vector and normalize it to the world coordinate system W to obtain the normal vector of the i-th side bottom surface in the world coordinate system W. ; Step 312: After obtaining the cone surface normal vector, first set the vertical axis of the world coordinate system W... Rotate to , where the rotation axis The z-axis and normal vector of the world coordinate system W The common perpendicular vector obtained by the cross product, and the rotation angle The z-axis and normal vector of the world coordinate system W The angle between them, from which the tilted quaternion can be obtained. ; Step 313: Superimpose the heading angle from the IMU data onto the tilt quaternion. Obtain the heading quaternion. ; Step 314: Calculate the attitude quaternions based on the inclination quaternion and the heading quaternion. , This indicates a product operation.

[0030] Preferably, in step 32, a geometric intersection model is constructed based on the shape of the fishpond using distance measurement data and the equation of the bottom surface of the aquaculture pond to determine the theoretical intersection point of the ultrasonic rays and the pond wall; the specific process is as follows: Step 321: Construct a geometric intersection model based on the ultrasonic ray equation and the equation of the bottom surface of the aquaculture pond corresponding to the shape of the fishpond; When the aquaculture pond is a circular fishpond, the geometric intersection model expression is:

[0031]

[0032] Where t represents the ranging value, and r(t) represents the equation of the ultrasonic ray emitted by the underwater ultrasonic sensor; The equation for the conical surface of a circular fishpond. This represents the starting point of the ultrasonic ray emitted by the j-th underwater ultrasonic sensor in the world coordinate system. This represents the direction in which the ultrasonic beam emitted by the j-th underwater ultrasonic sensor is transformed to the world coordinate system; by simultaneously solving the above two equations, we can derive:

[0033] Organized into The quadratic equation is obtained as follows: ; When the aquaculture pond is a regular polygonal fishpond, the geometric intersection point model expression is:

[0034]

[0035] in, Let's consider the equation for the i-th vertical pool wall; by simultaneously solving the two equations above, we can derive: ; Step 322: Solve for the theoretical intersection point of the theoretical distance measurement; When the aquaculture pond is a circular fishpond, solve... We then take the smallest positive real root as the solution and substitute it into the ultrasonic ray equation to obtain the intersection point of the ultrasonic ray and the pool wall. , As a theoretical intersection; When the aquaculture pond is a regular polygonal fishpond, solve the following problem. Substituting into the ultrasonic ray equation, we obtain the intersection point of the ultrasonic ray and the i-th vertical pool wall. ; Traverse all vertices and segments Check the validity of the intersection points and select the smallest valid positive real root. As the theoretical distance measurement for the corresponding underwater ultrasonic sensor, the intersection point corresponding to the theoretical distance measurement is the theoretical intersection point.

[0036] Preferably, constraint equations are constructed based on theoretical intersection points and state vectors, an objective function is established, and the Gauss-Newton iteration method is used to solve the objective function to obtain a geometrically optimized solution; the specific process is as follows: Step 331: The constraint equations include ultrasonic range constraints and ground-hugging constraints. The ultrasonic range constraints are constructed based on the theoretical intersection points. The constraint equations are expressed as follows: The ultrasonic distance constraint is: ; When the aquaculture pond is a circular fishpond, the ground constraint is: ; When the aquaculture pond is a regular polygonal fish pond, the ground constraint is: ; X represents the predicted current state vector of the cleaning robot. ; This represents the actual distance measured in the ranging data, which is the actual distance measured by the underwater ultrasonic sensor. Step 332: Describe the coordinates m of the cleaning robot using the nonlinear least squares method based on the constraint equations, and establish the objective function. , Let represent the weight matrix, where the constraints are: q represents the attitude quaternion; Step 333: Solve the state vector of the cleaning robot according to the objective function using the Gauss-Newton iteration method to obtain the geometric optimization solution. The process is as follows: Step 3331: Set the initial guess state vector based on the current state vector. Then set the convergence threshold. And the maximum number of iterations K; Step 3332: Iterate using the initial guessed state vector. For the temporary state vector of the current l-th iteration... , l=0~K-1, calculate the augmented residual vector ; indicates as: ; Step 3333: Calculate the Jacobian matrix based on the augmented residual vector, expressed as: ; Step 3334: Calculate the Gauss-Newton step based on the Jacobian matrix, expressed as: ; Step 3335: Update the state vector according to the Gauss-Newton step to obtain the geometric optimization solution. And normalize the quaternions, if they satisfy If the result is positive, stop iterating and obtain the precise coordinates based on the geometric optimization solution; otherwise, let l = l + 1 and continue iterating until the maximum number of iterations is reached.

[0037] As can be seen from the above technical solution, compared with the prior art, this invention discloses a quaternion underwater positioning method for aquaculture pond cleaning robots. It aims to solve the problem of high-precision, high-robust real-time 3D positioning of underwater cleaning robots in sloping factory aquaculture ponds. It achieves precise robot positioning through a dual-frequency fusion positioning architecture using underwater ultrasonic sensors and an inertial measurement unit (IMU) in a general environment. Specifically, to meet the real-time positioning requirements in different dynamic environments, an extended Kalman filter (EKF) is used to process the IMU's motion data for high-frequency prediction, enabling rapid pose prediction and updating. Distance data from four ultrasonic sensors are used to perform low-frequency calculations through geometric relationships to obtain more accurate position information. The high-frequency positioning results are then verified and corrected. Simultaneously, by uniformly using quaternions to represent the robot's posture in all stages, the numerical stability and algorithm consistency of posture updates are ensured, effectively overcoming the influence of complex terrain such as slopes on posture representation. The beneficial effects specifically include: (1) It solves the problem of attitude representation singularity and projection error amplification in the environment of sloping pool bottom; it uniformly adopts quaternion to represent the robot's full attitude, which fundamentally avoids the singularity of "universal lock" that is easy to occur when the robot runs to the area near the center of the pool bottom and the slope changes drastically due to the pitch angle approaching 90°. Furthermore, by directly establishing a precise geometric intersection model of ultrasonic ray and cone surface in three-dimensional space, it completely abandons the projection approximation with serious error amplification and achieves consistent high precision throughout the entire pool area.

[0038] (2) It balances the contradiction between high-precision positioning and high real-time requirements, and eliminates long-term IMU drift. It innovatively proposes a dual-frequency fusion architecture of high-frequency EKF prediction update + low-frequency geometric optimization verification. The high-frequency EKF channel ensures the real-time response capability of the system and meets the control requirements. The low-frequency geometric optimization channel periodically generates a high-precision reference solution that does not drift over time, which is used to detect and correct the cumulative error of EKF. The two complement each other and achieve the ideal effect of uninterrupted real-time output and no drift in long-term accuracy.

[0039] (3) Improved the convergence and robustness of the algorithm in dynamic and complex underwater environments; the real-time output of EKF was used as the initial value for the "hot start" of the geometric optimization iteration. Since EKF itself has good short-term prediction ability, the initial value it provides is very close to the real state, which greatly improves the convergence speed and success rate of the geometric optimization solver and enhances the overall robustness of the system under complex disturbances.

[0040] (4) It possesses superior engineering practicality and cost-effectiveness; this invention only requires the robot's own conventional IMU and four ultrasonic sensors with known installation locations, without the need to deploy additional acoustic beacons, hydrophone arrays, or modify infrastructure in complex pool bottom environments, making deployment simple and cost-effective. The algorithm's requirements for computing resources are more reasonable, with low computational load for high-frequency EKF and fewer iterations for low-frequency optimization after obtaining high-quality initial values, making the scheme easy to implement and deploy on the embedded computing platform of the cleaning robot. This invention can simultaneously adapt to common fishpond shapes such as circles and regular polygons for high-precision positioning, greatly improving the versatility of the positioning scheme.

[0041] Therefore, this invention can be widely applied in the field of aquaculture, especially in the navigation and positioning system of automated cleaning robots. It can effectively solve problems such as insufficient applicability to fish pond shapes, poor positioning accuracy on sloping fish pond ground, high computational complexity, poor real-time performance and stability, and decreased positioning accuracy due to IMU error accumulation. Attached Figure Description

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

[0043] Figure 1 This is a flowchart of a quaternion underwater positioning method for a cleaning robot in aquaculture ponds, provided in an embodiment of the present invention. Figure 2 This is a top view of the overall system layout of the aquaculture pond provided in this embodiment of the invention; Figure 3 This is a side view of the overall system layout of the aquaculture pond provided in an embodiment of the present invention.

[0044] In the attached diagram: 1-Aquaculture pond, 2-Ultrasonic ray, 3-Underwater ultrasonic sensor, 4-Inertial measurement unit, 5-Stainless steel cleaning robot. Detailed Implementation

[0045] 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.

[0046] This invention discloses a quaternion-based underwater positioning method for aquaculture pond cleaning robots, such as... Figure 2-3 As shown, the cleaning robot 5 is equipped with an inertial measurement unit (IMU) 4, and underwater ultrasonic sensors 3 are installed at the center of its four sides (front, back, left, and right). The process is as follows: Figure 1 As shown, it includes the following steps: S1: Collect the size parameters of the aquaculture pond and determine the shape of the pond; S2: Collect IMU data from the inertial measurement unit and ranging data and position parameters from the underwater ultrasonic sensor. Use an extended Kalman filter to predict the state vector. Then, build an observation model based on the shape of the fishpond to update the extended Kalman filter and extract precise coordinates from the predicted state vector. S3: Periodically optimize the geometric state vector of the cleaning robot by using distance measurement data, position parameters and the current state vector based on the shape of the fish pond. S4: Calculate the difference between the predicted state vector and the geometric state vector. If the difference is within the preset fusion range, use the predicted state vector as the current state vector and return to S2; otherwise, extract the precise coordinates from the geometric state vector, use the geometric state vector as the current state vector and return to S2.

[0047] Furthermore, the aquaculture pond 1 includes a circular fish pond or a regular polygonal fish pond; the top surface of the circular fish pond is circular, and the bottom surface is conical, with dimensional parameters including radius. Pool bottom inclination angle and slope The top surface of the regular polygonal fishpond is a regular polygon, and the bottom surface is a pyramidal surface. Dimensional parameters include the number of sides N, side length L, and the angle of inclination of the pond bottom. Given the vertex azimuth, the coordinates of the bottom vertex, and the slope S, calculate the azimuth of the i-th vertex. circumcircle radius Inscribed circle radius The coordinates of the i-th bottom vertex Vertical height of the bottom vertex .

[0048] Furthermore, the specific process of S2 is as follows: S21: Acquire IMU data from the inertial measurement unit and ranging data and position parameters from the underwater ultrasonic sensor. The IMU data includes angular velocity and acceleration; the initial state vector of the cleaning robot and the covariance matrix of the extended Kalman filter; S22: Based on the current state vector and IMU data, the extended Kalman filter (EKF) is used with the kinematic model to predict the state vector, obtain the state prior estimate, and update the covariance matrix; S23: Based on the shape of the fishpond, construct an observation model using the state prior estimate, update the state prior estimate using the ranging data to obtain the state posterior estimate, normalize the attitude quaternions in the state posterior estimate and update the covariance matrix. S24: Use the posterior state estimate as the predicted state vector.

[0049] Furthermore, in S22, EKF is used to predict the state vector of the cleaning robot in real time based on IMU data, and the state vector of the next moment is used to construct the state prior estimate. The state vector of the next moment includes the position, attitude quaternion, linear velocity, and angular velocity of the cleaning robot at the next moment; the IMU data includes three-axis angular velocities. Triaxial acceleration and heading angle; the specific process is as follows: Based on the current state vector of the cleaning robot, the EKF algorithm is used to predict its position, velocity, attitude, and covariance. The expression is as follows: Location prediction formula: ; Speed ​​prediction formula: ; Attitude prediction formula: ; ; Covariance prediction formula: ; in, To determine the location of the cleaning robot in the next moment; The linear velocity of the cleaning robot at the next moment; The quaternion for the posture of the cleaning robot in the next moment; This is the updated lower difference matrix; The position of the cleaning robot in the state vector at the current time k. = , These are the x-axis, y-axis, and z-axis coordinates of the cleaning robot in the robot coordinate system M, respectively. Let k be the linear velocity of the cleaning robot in the current state vector. = ; Let k be the quaternion of the robot's posture in the state vector at the current time k. = ; Angular velocity in IMU data = , These represent the angular velocities of the cleaning robot along the x, y, and z axes in the robot coordinate system M, respectively. This refers to the total acceleration of the cleaning robot. Acceleration in IMU data; It is the acceleration due to gravity; Let be the component of gravity on the inclined plane; n be the normal vector of the bottom of the fish; F be the state transition matrix; Q be the process noise covariance matrix. Let be the covariance matrix of the current state; Indicates the time interval for prediction; This represents the attitude quaternion generated within the predicted time interval; T represents the transpose; the acceleration of the cleaning robot includes the component of gravity on the inclined plane. , , The current state vector of the cleaning robot. , These represent the x-axis, y-axis, and z-axis coordinates of the cleaning robot in the robot coordinate system M. These are the attitude quaternions of the cleaning robot in robot coordinate system B. These represent the linear velocities of the cleaning robot along the x, y, and z axes in the robot coordinate system M. These represent the angular velocities of the cleaning robot along the x, y, and z axes in the robot coordinate system M.

[0050] Furthermore, the specific process by which S23 obtains the posterior state estimate is as follows: S231: Transform the position parameters of the underwater ultrasonic sensor from the robot coordinate system to the world coordinate system using the attitude quaternion in the state prior estimation; S232: Based on the shape of the fishpond, establish a geometric intersection model of the ultrasonic ray and the pond wall for the converted underwater ultrasonic sensor, and solve the theoretical distance value based on the state prior estimation as the observation model. S233: Calculate the observation residuals based on the observation model and the actual distance values ​​in the distance measurement data; S234: Calculate the Kalman gain based on the covariance matrix; S235: Correct the state prior estimate based on the Kalman gain and observation residuals to obtain the state posterior estimate.

[0051] Furthermore, the world coordinate system W is a three-dimensional coordinate system established with the geometric center of the bottom of the aquaculture pond as its origin. The robot coordinate system M is a three-dimensional coordinate system established with the geometric center of the cleaning robot as the origin. ; , , These represent the robot's forward direction, the robot's left side, and the robot's top upward direction, respectively; the position parameters collected by the underwater ultrasonic sensor. Including position coordinates and orientation M represents the robot coordinate system, F represents the front, B represents the rear, L represents the left, and R represents the right. Based on the attitude quaternions in the state prior estimation, the position parameters are transformed from the robot coordinate system M to the world coordinate system W, expressed as:

[0052]

[0053]

[0054] in, This represents the starting point of the ultrasonic ray emitted by the j-th underwater ultrasonic sensor after transformation to the world coordinate system W, i.e., the position of the j-th underwater ultrasonic sensor; m represents the center coordinates of the cleaning robot in the world coordinate system. R(q) represents the rotation matrix that rotates the robot coordinate system M to the world coordinate system W. Represents the attitude quaternion in the world coordinate system W; This indicates the direction in which the ultrasonic rays emitted by the j-th underwater ultrasonic sensor are transformed into the world coordinate system.

[0055] Furthermore, in step 232, based on the shape of the fishpond, a geometric intersection model is constructed using the ranging data from the underwater ultrasonic sensor and the equation of the bottom surface of the aquaculture pond. The specific process is as follows: Step 2321: Construct a geometric intersection model based on the ultrasonic ray equation and the equation of the bottom surface of the aquaculture pond corresponding to the shape of the fishpond; When the aquaculture pond is a circular fishpond, the geometric intersection model expression is:

[0056]

[0057] Where t represents the theoretical ranging value, and r(t) represents the equation of the ultrasonic ray emitted by the underwater ultrasonic sensor; The equation for the conical surface of a circular fishpond. The angle of inclination of the pond bottom is represented by x and y, which represent the x-axis and y-axis coordinates of the bottom surface of the aquaculture pond in the world coordinate system, respectively. This represents the starting point of the ultrasonic ray emitted by the j-th underwater ultrasonic sensor in the world coordinate system. This represents the direction in which the ultrasonic beam emitted by the j-th underwater ultrasonic sensor is transformed to the world coordinate system; by simultaneously solving the above two equations, we can derive: ,make:

[0058] Organized into The quadratic equation is obtained as follows: ; in, These represent the x-axis, y-axis, and z-axis coordinates of the starting point of the ultrasonic ray emitted by the j-th underwater ultrasonic sensor in the world coordinate system, respectively. Let x, y, and z represent the coordinate components of the direction of the ultrasonic ray emitted by the j-th underwater ultrasonic sensor in the world coordinate system; t represents the theoretical distance measurement value. When the aquaculture pond is a regular polygonal fishpond, the geometric intersection point model expression is:

[0059]

[0060] in, Let i be the equation of the i-th side bottom surface. Let represent the normal azimuth angle of the i-th side bottom surface; by simultaneously solving the above two equations, we can derive: ; Where N represents the number of sides on the top face of the regular polygonal fishpond; This represents the theoretical distance measurement value of the i-th side bottom surface; Step 2322: Obtain the starting point of the ultrasonic rays emitted by the underwater ultrasonic sensor (the position of the underwater ultrasonic sensor) based on the position in the state prior estimation. Calculate the orientation of the ultrasonic rays emitted by the underwater ultrasonic sensor based on the attitude quaternion in the state prior estimation. Solve for the theoretical ranging value as the observation model. For a circular fishpond, find the smallest positive real root as the theoretical distance measurement value. ; For a regular polygonal fishpond, the smallest valid positive real root satisfying the validity constraint is used as the theoretical distance measurement value t; the validity constraint is expressed as:

[0061] Traverse all vertex segments Check the validity of the intersection points and select the smallest valid positive real root. The corresponding intersection point obtained by substituting it into the ultrasonic ray equation The theoretical intersection point; among which, Represents vertices x-axis coordinates and y-axis coordinates Represents vertices x-axis coordinates and y-axis coordinates They represent the intersection points respectively. x-axis and y-axis coordinates; This indicates a constant value to be set.

[0062] Furthermore, The observation model for the i-th ultrasound is This model represents the distance that ultrasound should be able to measure; then, ground-level constraints are established, especially when the aquaculture pond is a circular fishpond. When the aquaculture pond is a regular polygonal fish pond, the ground-attached constraint This model represents the difference between the robot's current position height and the height of the bottom surface; The angle of inclination of the pool bottom. is the vertical height of the vertex of the base; r is the radius of the inscribed circle corresponding to the pyramidal surface; For the first The normal azimuth angle of each side bottom surface; x, y, and z represent the x-axis, y-axis, and z-axis coordinates of the bottom surface of the aquaculture pond in the world coordinate system W obtained from the state vector.

[0063] Furthermore, in step 232, the observation residual y is calculated based on the observation model and the actual distance values ​​in the distance measurement data; The observation residual corresponding to the j-th underwater ultrasonic sensor is , To utilize state prior estimation Calculated theoretical observations; Given the actual distance measured in the distance measurement data; calculate the total observation residual y by combining all observations into observation vectors z and h(X); ; ; .

[0064] Furthermore, the Kalman gain is calculated based on the covariance matrix; the expression is: Innovation Covariance Kalman gain ; Where K represents the Kalman gain; H is the observation Jacobian matrix; S is the covariance matrix corresponding to the state prior estimate; S represents the innovation covariance, which is the covariance of the difference between the theoretical observation and the actual distance measurement, consisting of the prediction error projected onto the measurement space and the measurement noise inherent in the sensor; R is the observation noise covariance.

[0065] Furthermore, the state prior estimate is corrected based on the Kalman gain and the observation residuals to obtain the state posterior estimate; pass Perform state vector update, where This is a priori estimation of the state based solely on kinematic model predictions; This represents the posterior estimate of the state; To ensure the validity of the pose, quaternion normalization is required. Finally, the covariance also needs to be updated. .

[0066] Furthermore, the specific process of S3 is as follows: S31: Calculate the attitude quaternion of the cleaning robot based on the shape and size parameters of the fish pond, and transform the position parameters of the underwater ultrasonic sensor from the robot coordinate system to the world coordinate system based on the attitude quaternion; S32: Solve for the theoretical intersection point between the ultrasonic rays emitted by the underwater ultrasonic sensor and the pool wall based on the geometric intersection model and ranging data; S33: Based on the shape of the fishpond, construct a nonlinear least squares objective function containing constraint equations using theoretical intersection points and state vectors, and use the Gauss-Newton method for iterative optimization to obtain the geometric optimization solution as the geometric state vector.

[0067] Furthermore, in S31, the attitude quaternion of the cleaning robot is calculated based on the shape and slope S of the fishpond. The specific process is as follows: S311: The cleaning robot operates close to the bottom of the aquaculture pond. (In the robot's coordinate system...) Approximate the normal vector to the bottom of the aquaculture pond, based on the pond's shape and corresponding dimensional parameters, including the number of sides N, side length L, and vertex azimuth. The slope S determines the normal vector of the bottom surface of the aquaculture pond; For a circular fishpond, the base is a conical surface, and the formula for expressing a conical surface is: The formula for expressing the normal of its conical surface is: ,Right now , Normalize it to obtain the normal vector in the world coordinate system W. Simplified ; For a regular polygonal fishpond with a pyramidal base, the formula for the i-th lateral base is: ,in The vertical height of the bottom vertex. Given the normal azimuth angle of the i-th side bottom surface, its gradient can be used to obtain the plane normal vector. Calculate the horizontal projection polar angle. ( Adjust to Find satisfaction Let the integer i be the value of the cleaning robot belonging to the i-th side bottom surface. Substitute i into the plane normal vector and normalize it to the world coordinate system W to obtain the normal vector of the i-th side bottom surface in the world coordinate system W. ; S312: After obtaining the cone surface normal vector, first set the vertical axis of the world coordinate system W... Rotate to , where the rotation axis The z-axis and normal vector of the world coordinate system W The common perpendicular vector obtained by the cross product, and the rotation angle The z-axis and normal vector of the world coordinate system W The angle between them, from which the tilted quaternion can be obtained. ; S313: Overlaying heading angle from IMU data onto tilt quaternions Obtain the heading quaternion. ; S314: Calculate attitude quaternions based on inclination and heading quaternions. , This indicates a product operation.

[0068] Furthermore, in S32, based on the shape of the fishpond, a geometric intersection model is constructed using distance measurement data and the equation of the bottom surface of the aquaculture pond to determine the theoretical intersection point of the ultrasonic ray 2 and the pond wall; the specific process is as follows: S321: Construct a geometric intersection model based on the ultrasonic ray equation and the equation of the bottom surface of the aquaculture pond corresponding to the shape of the fish pond; When the aquaculture pond is a circular fishpond, the geometric intersection model expression is:

[0069]

[0070] Where t represents the ranging value, and r(t) represents the equation of the ultrasonic ray emitted by the underwater ultrasonic sensor; The equation for the conical surface of a circular fishpond. This indicates the starting point of the ultrasonic ray 2 emitted by the j-th underwater ultrasonic sensor in the world coordinate system; This represents the orientation of ultrasonic ray 2 emitted by the j-th underwater ultrasonic sensor after transformation to the world coordinate system; by simultaneously solving the above two equations, we can derive:

[0071] Organized into The quadratic equation is obtained as follows: ; When the aquaculture pond is a regular polygonal fishpond, the geometric intersection point model expression is:

[0072]

[0073] in, Let's consider the equation for the i-th vertical pool wall; by simultaneously solving the two equations above, we can derive: ; S322: Solve for the theoretical distance measurement to calculate the theoretical intersection point; When the aquaculture pond is a circular fishpond, solve... We then take the smallest positive real root as the solution and substitute it into the ultrasonic ray equation to obtain the intersection point of the ultrasonic ray and the pool wall. , As a theoretical intersection; When the aquaculture pond is a regular polygonal fishpond, solve the following problem. Substituting into the ultrasonic ray equation, we obtain the intersection point of the ultrasonic ray and the i-th vertical pool wall. ; Traverse all vertices and segments Check the validity of the intersection points and select the smallest valid positive real root. As the theoretical distance measurement for the corresponding underwater ultrasonic sensor, the intersection point corresponding to the theoretical distance measurement is the theoretical intersection point.

[0074] Furthermore, constraint equations are constructed based on the theoretical intersection points and state vectors, an objective function is established, and the Gauss-Newton iteration method is used to solve the objective function to obtain the geometrically optimized solution; the specific process is as follows: S331: The constraint equations include ultrasonic range constraints and ground-level constraints. The ultrasonic range constraints are constructed based on the theoretical intersection points. The constraint equations are expressed as follows: The ultrasonic distance constraint is: ; When the aquaculture pond is a circular fishpond, the ground constraint is: ; When the aquaculture pond is a regular polygonal fish pond, the ground constraint is: ; X represents the state vector of the cleaning robot predicted in S2. ; This represents the actual distance measured in the ranging data, which is the actual distance measured by the ultrasonic wave. S332: Based on the constraint equations, the coordinates m of the cleaning robot are described using a nonlinear least squares method, and an objective function is established. , Let represent the weight matrix, where the constraints are: q represents the attitude quaternion; S333: The state vector of the cleaning robot is solved using the Gauss-Newton iteration method based on the objective function to obtain the geometric optimization solution. The process is as follows: S3331: Set the initial guessed state vector based on the predicted state vector from the previous time step. Then set the convergence threshold. And the maximum number of iterations K; S3332: Iterate using the initial guessed state vector, for the temporary state vector of the current l-th iteration. , l=0~K-1, calculate the augmented residual vector ; indicates as: ; S3333: Calculate the Jacobian matrix based on the augmented residual vector, expressed as: ; S3334: Calculate the Gauss-Newton step based on the Jacobian matrix, expressed as: ; S3335: Update the state vector based on the Gauss-Newton step to obtain the geometric optimization solution. And normalize the quaternions, if they satisfy If the condition is met, stop the iteration and obtain the precise coordinates based on the geometric optimization solution; otherwise, let l = l + 1 and return to S3332 to continue iterating until the maximum number of iterations is reached.

[0075] On the other hand, in one specific embodiment, a quaternion underwater positioning method of the present invention for a cleaning robot in aquaculture ponds is used to accurately position the cleaning robot. An inertial measurement unit (IMU) is installed at the center of the top surface of the cleaning robot, and a set of underwater ultrasonic sensors is installed at the center of each of the four sides (front, back, left, and right). The specific steps for accurate positioning are as follows: S1: Initialization settings; S11: Establish a world coordinate system W with the top of the cone at the bottom of the fishpond as the origin, with the x-axis pointing east, the y-axis pointing north, and the z-axis pointing vertically upward; establish a robot coordinate system M for the cleaning robot body, with the x-axis pointing in front of the cleaning robot body, the y-axis pointing to the left of the cleaning robot body, and the z-axis pointing above the cleaning robot body. S12: Collect the size parameters of the aquaculture pond and the position parameters of the underwater ultrasonic sensors on the cleaning robot. Determine the shape of the pond based on the size parameters. The pond shape is either circular or a regular polygon. Load the known size parameters; for a circular fishpond, load the fishpond radius. Pool bottom inclination angle For a regular polygonal fishpond, load the number of sides N, the side length L, and the slope angle of the bottom. Load the installation positions of the four underwater ultrasonic sensors in the robot's coordinate system M, and transform them to the world coordinate system W to obtain their positions. and orientation ; S13: Set the robot state vector for the extended Kalman filter (EKF) The initial values ​​and their covariance matrix The robot's state vector includes position, velocity, attitude quaternions, and body angular velocity; at the same time, the parameters required for geometry optimization are initialized, including the initial position and attitude quaternions, using the state vector predicted by EKF at the previous time step.

[0076] S2: Acquire IMU data at a data update frequency of 100Hz and use EKF for prediction to achieve real-time pose tracking of the cleaning robot; S21: Real-time reading of the three-axis angular velocity provided by the IMU and triaxial acceleration The data is processed and calibrated and filtered as necessary. S22: Prediction is performed using EKF; Based on the state estimate from the previous time step and current three-axis angular velocity Predicting the prior state estimate at the current moment using a kinematic model The kinematic model used for prediction fully considers the motion characteristics of the robot attached to the inclined bottom of the fishpond. The acceleration model includes the projection of the gravitational component onto the inclined surface. Furthermore, the covariance matrix of the state estimate is updated based on the process noise of the kinematic model. ; S23: Update EKF based on the prediction results; Based on the ranging data from an underwater ultrasonic sensor, a theoretical ranging value is derived from a priori state estimation to establish an ultrasonic observation model, and then the observation residuals are calculated. Then the Kalman gain was calculated. And use the observation residuals to estimate the state prior. After making corrections, the posterior state estimate is obtained. Finally, the pose quaternion part of the robot's state vector is normalized and the covariance matrix is ​​updated. S24: Post-EKF State Estimation As a real-time positioning output, it is provided to the navigation and control module of the cleaning robot; at the same time, this state is written to a shared storage area as the initial value for the iteration of the low-frequency geometry optimization channel.

[0077] S3: Triggered by an independent timer, it runs at a frequency of 10Hz, uses the ranging data collected by the underwater ultrasonic sensor to solve a high-precision geometric positioning problem, and verifies the EKF results; S31: When the timer reaches the preset period, read the current EKF estimated state from the shared memory area as the initial guess value for optimization iteration. It also simultaneously reads the current ranging values ​​from four underwater ultrasonic sensors. ; S32: Calculate the tilt quaternion of the cleaning robot based on the fishpond slope and the heading angle provided by the IMU. and directional quaternion The two are superimposed to obtain the complete robot attitude quaternion q, ensuring the consistency of attitude representation throughout the localization solution and avoiding the singularity problem that may occur with Euler angles when the slope is large. Using this attitude quaternion q and its corresponding rotation matrix R(q), the position and orientation of the underwater ultrasonic sensor in the robot coordinate system M are transformed to the world coordinate system W. The IMU data provided by the IMU includes three-axis angular velocities. Triaxial acceleration and heading angle; S33: For each underwater ultrasonic sensor, solve for the ray parameters by combining the emitted rays with the known pool wall equations. The quadratic equation yields the theoretical intersection point between the ultrasonic ray and the pool wall. , The theoretical value is the distance that the ultrasonic wave should be measured in the current pose; S34: Construction and solution of nonlinear optimization problems; First, two constraint equations are established: one for ultrasonic distance and the other for robot ground contact. Then, the sum of the squares of these constraints is used to construct a equation relating the robot's state. The nonlinear least squares objective function (position and attitude) is used; finally, with the EKF state as the initial value, iterative optimization is performed using the Gauss-Newton method. Iteration continues until convergence (the state increment or objective function value is less than a set threshold) or the maximum number of iterations is reached, outputting a high-precision geometric optimization solution. ; S35: Geometric optimization solution Post-EKF State Estimation A comparison is made; if the difference is within the preset tolerance range, the EKF is considered to be operating normally, the confidence level is improved, and the state posterior estimation is performed. Extract positioning information; if the difference exceeds the tolerance range, but the geometric optimization solution itself converges well and the residual is small (high confidence), then it is determined that the EKF may have accumulated error or temporary divergence, and the output robot pose is determined by the geometric optimization solution. Extract from and and The state vector is input into the EKF for subsequent prediction, thereby achieving EKF correction.

[0078] S4: Continuous loop and output; The S2-S3 cycle runs continuously; the EKF loop in S2 provides stable and continuous real-time pose information, ensuring the real-time performance of the cleaning robot control; the geometry optimization loop in S3 periodically provides a set of high-precision anchor point solutions for monitoring and correcting the EKF, effectively suppressing the cumulative drift error of the IMU, thus achieving a balance between high real-time performance and high long-term accuracy.

[0079] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0080] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A quaternion underwater positioning method applied to a water pollution cleaning robot for aquaculture ponds, an inertial measurement unit is arranged on the water pollution cleaning robot, and underwater ultrasonic sensors are arranged at the centers of the front, rear, left and right four sides, characterized in that, Includes the following steps: Step 1: Collect the size parameters of the aquaculture pond and determine the shape of the fish pond; Step 2: Collect IMU data from the inertial measurement unit and ranging data and position parameters from the underwater ultrasonic sensor. Use an extended Kalman filter to predict the state vector. Build an observation model based on the shape of the fishpond to update the extended Kalman filter and extract precise coordinates from the predicted state vector. Step 3: Periodically perform geometric optimization of the state vector based on the shape of the fishpond, using distance measurement data, position parameters, and the current state vector to obtain the geometric state vector of the cleaning robot; Step 4: Calculate the difference between the predicted state vector and the geometric state vector. If the difference is within the preset fusion range, use the predicted state vector as the current state vector and return to Step 2; otherwise, extract the precise coordinates from the geometric state vector, use the geometric state vector as the current state vector, and return to Step 2.

2. The quaternion underwater positioning method for a cleaning robot in aquaculture ponds according to claim 1, characterized in that, The fishpond shape includes a circular fishpond or a regular polygon fishpond; the top surface of the circular fishpond is a circular surface, the bottom surface is a conical surface, and the size parameters include a radius , a bottom surface inclination angle , and a slope ; the top surface of the regular polygon fishpond is a regular polygon surface, the bottom surface is a pyramid surface, and the size parameters include the number of sides N, the side length L, the bottom surface inclination angle , the vertex azimuth angle, and the slope S, wherein the i-th vertex azimuth angle , the i-th bottom surface vertex coordinate , represents the vertical height of the bottom surface vertex, , represents the circumscribed circle radius , and the inscribed circle radius .

3. The quaternion underwater positioning method applied to the water pollution cleaning robot of the aquaculture pond according to claim 2, characterized in that, The specific process of step 2 is as follows: Step 21: Acquire IMU data from the inertial measurement unit and ranging data and position parameters from the underwater ultrasonic sensor. The IMU data includes angular velocity and acceleration; the initial state vector of the cleaning robot and the covariance matrix of the extended Kalman filter. Step 22: Based on the current state vector and IMU data, use the kinematic model and extended Kalman filter to predict the state vector, obtain the state prior estimate, and update the covariance matrix; Step 23: Construct an observation model based on the shape of the fishpond using the state prior estimate, update the state prior estimate using the ranging data to obtain the state posterior estimate, normalize the attitude quaternions in the state posterior estimate and update the covariance matrix. Step 24: Use the posterior state estimate as the predicted state vector.

4. The quaternion underwater positioning method applied to the water pollution cleaning robot of the aquaculture pond according to claim 3, characterized in that, In step 22, an extended Kalman filter is used to predict the state vector of the cleaning robot in real time, and the state vector at the next moment is used to construct a state prior estimate. The state vector at the next moment includes the position, attitude quaternion, linear velocity, and angular velocity of the cleaning robot at the next moment; the specific process is as follows: Based on the current state vector of the cleaning robot, the EKF algorithm is used to predict its position, velocity, attitude, and covariance. The expression is: Position prediction formula: ; Speed prediction formula: ; Pose prediction formula: ; ； Covariance prediction formula: ; wherein, is the position of the cleaning robot at the next time instant; is the linear velocity of the cleaning robot at the next time instant; is the attitude quaternion of the cleaning robot at the next time instant; is the updated covariance matrix; is the position of the cleaning robot in the state vector at the current time instant k = , are the coordinate values of the x-axis, y-axis and z-axis of the cleaning robot in the robot coordinate system M, respectively; is the linear velocity of the cleaning robot in the state vector at the current time instant k = ; is the attitude quaternion of the cleaning robot in the state vector at the current time instant k = ; is the angular velocity in the IMU data = , are the angular velocities of the x-axis, y-axis and z-axis of the cleaning robot in the robot coordinate system M, respectively; is the total acceleration of the cleaning robot; is the acceleration in the IMU data; is the gravitational acceleration; is the component of the gravitational force on the inclined plane; n is the normal vector of the fish bottom surface; F is the state transition matrix; Q is the process noise covariance matrix; is the covariance matrix of the current state; denotes the predicted time interval; denotes the attitude quaternion generated within the predicted time interval; T denotes the transpose.

5. The quaternion underwater positioning method applied to the water pollution cleaning robot of the aquaculture pond according to claim 4, characterized in that, The specific process of obtaining the posterior state estimate in step 23 is as follows: Step 231: Transform the position parameters of the underwater ultrasonic sensor from the robot coordinate system to the world coordinate system based on the attitude quaternion in the state prior estimation; Step 232: Based on the shape of the fishpond, establish a geometric intersection model of the ultrasonic ray and the pond wall for the converted underwater ultrasonic sensor, and solve for the theoretical distance value based on the state prior estimation as the observation model; Step 233: Calculate the observation residuals based on the observation model and the actual distance values ​​in the distance measurement data; Step 234: Calculate the Kalman gain based on the covariance matrix; Step 235: Correct the state prior estimate based on the Kalman gain and observation residuals to obtain the state posterior estimate.

6. The quaternion underwater positioning method for a cleaning robot in aquaculture ponds according to claim 5, characterized in that, The world coordinate system W is with the geometric center of the bottom of the aquaculture pond as the origin, and the robot coordinate system M is with the geometric center of the cleaning robot as the origin; the position parameters of the collected underwater ultrasonic sensor , including position coordinates and orientation , M represents the robot coordinate system, F represents the front, B represents the back, L represents the left, and R represents the right; the position parameters are converted from the robot coordinate system M to the world coordinate system W according to the attitude quaternion, which is represented as: wherein represents the origin of the j-th underwater ultrasonic sensor transmitting ultrasonic rays converted to the world coordinate system W; m represents the center coordinate of the clean-up robot in the world coordinate system, ; R(q) represents a rotation matrix rotating the robot coordinate system M to the world coordinate system W, represents a pose quaternion in the world coordinate system W; represents the orientation of the j-th underwater ultrasonic sensor transmitting ultrasonic rays converted to the world coordinate system.

7. The quaternion underwater positioning method applied to the water pollution cleaning robot of the aquaculture pond according to claim 6, characterized in that, In step 232, a geometric intersection model is constructed based on the shape of the fishpond, using ranging data from an underwater ultrasonic sensor and the equation of the bottom surface of the aquaculture pond. The specific process is as follows: Step 2321: Construct a geometric intersection model based on the ultrasonic ray equation and the equation of the bottom surface of the aquaculture pond corresponding to the shape of the fishpond; When the aquaculture pond is a circular fishpond, the geometric intersection model expression is: ； Wherein, t represents a theoretical ranging value; represents a pool bottom inclination angle; respectively represent the coordinate values of the x-axis, y-axis and z-axis of the origin of the ultrasonic wave ray emitted by the jth underwater ultrasonic sensor in the world coordinate system; respectively represent the coordinate components of the x-axis, y-axis and z-axis of the direction of the ultrasonic wave ray emitted by the jth underwater ultrasonic sensor in the world coordinate system; When the aquaculture pond is a regular polygonal fishpond, the geometric intersection point model expression is: ； wherein, represents the normal azimuth angle of the i-th side bottom surface; N represents the number of the top surface edges of the regular polygonal fishpond; represents the circumscribed circle radius; represents the theoretical ranging value of the i-th side bottom surface; Step 2322: Obtain the starting point of the ultrasonic rays emitted by the underwater ultrasonic sensor based on the position in the state prior estimation, calculate the orientation of the ultrasonic rays emitted by the underwater ultrasonic sensor based on the attitude quaternion in the state prior estimation, and solve for the theoretical ranging value as the observation model.

8. The quaternion underwater positioning method applied to the aquaculture pond cleaning robot of claim 6, wherein, The specific process of step 3 is as follows: Step 31: Calculate the attitude quaternion of the cleaning robot based on the shape and size parameters of the fish pond, and transform the position parameters of the underwater ultrasonic sensor from the robot coordinate system to the world coordinate system based on the attitude quaternion; Step 32: Solve for the theoretical intersection point between the ultrasonic rays emitted by the underwater ultrasonic sensor and the pool wall based on the geometric intersection model and ranging data; Step 33: Based on the shape of the fishpond, construct a nonlinear least squares objective function containing constraint equations using theoretical intersection points and state vectors, and perform iterative optimization using the Gauss-Newton method to obtain the geometric optimization solution as the geometric state vector.

9. The quaternion underwater positioning method applied to the aquaculture pond cleaning robot of claim 8, characterized in that, Step 32 is as follows: Step 321: Use the position coordinates in the position parameters as the starting point for the ultrasonic rays emitted by the underwater ultrasonic sensor, and calculate the theoretical distance value based on the geometric intersection model in combination with the orientation in the position parameters. Step 322: Substitute the theoretical distance measurement value into the ultrasonic ray equation to determine the theoretical intersection point; Substitute the theoretical ranging value t into the ultrasonic ray equation to obtain the theoretical intersection point of the ultrasonic ray and the bottom surface , .

10. The quaternion underwater positioning method applied to the aquaculture pond cleaning robot of claim 9, wherein, Constraint equations are constructed based on theoretical intersection points and state vectors, an objective function is established, and the Gauss-Newton iteration method is used to solve the objective function to obtain a geometrically optimized solution; the specific process is as follows: Step 331: The constraint equations include ultrasonic range constraints and ground-hugging constraints. The ultrasonic range constraints are constructed based on the theoretical intersection points. The constraint equations are expressed as follows: The ultrasonic distance constraint is: ; When the aquaculture pond is a circular fish pond, the ground attachment constraint is: ; When the aquaculture pond is a regular polygon fish pond, the ground attachment constraint is: ; wherein X represents a current state vector of the cleaning robot, ; is a real ranging value in the ranging data; Step 332: Describe the coordinates of the cleaning robot using the nonlinear least squares method based on the constraint equations, and establish the objective function. , Let represent the weight matrix, where the constraints are as follows: q represents the attitude quaternion; Step 333: Solve the state vector of the cleaning robot according to the objective function using the Gauss-Newton iteration method to obtain the geometric optimization solution. .

Images