Positioning methods, devices, inspection robots, and storage media for confined underwater spaces

Abstract

This invention relates to a positioning method, device, inspection robot, and storage medium in confined underwater spaces, belonging to the field of robotics. The method includes: constructing an effective control input vector based on real-time motion data of the target object; constructing a state prediction model based on the effective control input vector; using the state prediction model to calculate the geometric constraint relationship between the laser beam and the boundary of the confined underwater space in real time, and analytically reconstructing the nonlinear observation Jacobian matrix; based on the observation Jacobian matrix, using a combination of an adaptive observation noise expansion mechanism based on the incident angle and a physical hard constraint correction method, evaluating the observation signal-to-noise ratio of the state prediction model online and forcibly constraining the state boundary to obtain the predicted position of the target object. This invention utilizes omnidirectional ray tracing and an adaptive weighting strategy based on the incident angle to effectively suppress specular reflection and non-Gaussian noise, solving the problem of positioning divergence in harsh acoustic environments and achieving precise autonomous navigation in confined underwater spaces.

Description

Technical Field

[0001] This invention relates to the field of robotics, and more particularly to a positioning method, apparatus, inspection robot, and storage medium for confined underwater spaces. Background Technology

[0002] Miniature underwater spherical robots, with their unique biomimetic streamlined structure and zero-turn radius mobility, are highly valuable for precise exploration tasks in complex and narrow waters (such as pipelines and underground rivers).

[0003] However, the miniaturized design imposes stringent payload limitations, preventing the integration of high-precision inertial navigation devices such as Doppler Velocity Logs (DVL). Therefore, existing low-cost spherical robots primarily rely on multi-source fusion dead reckoning using micro-electro-mechanical systems (MEMS), inertial measurement units (IMUs), depth gauges, and vision sensors. This aims to leverage the complementary advantages of heterogeneous data to assist in correcting attitude deviations and improve the system's perception robustness in dynamic environments.

[0004] However, the inherent zero-bias instability and high-frequency noise of low-end MEMS sensors cause the position error to diverge rapidly over time after double integration, making it difficult to meet the requirements of autonomous navigation. To suppress the error, laser-assisted correction is often introduced. However, laser observation faces severe challenges in confined underwater spaces such as pools: on the one hand, the multipath effect in enclosed spaces is significant, easily generating non-Gaussian noise; on the other hand, when the laser beam contacts a smooth wall at a small grazing angle, specular reflection is very likely to occur, leading to echo loss or serious outlier values ​​in the ranging. Summary of the Invention

[0005] In view of this, it is necessary to provide a positioning method, device, inspection robot and storage medium for confined underwater spaces to solve the problem of inaccurate positioning in harsh acoustic environments in the prior art.

[0006] To address the above problems, the present invention provides a positioning method for confined underwater spaces, comprising: S1, construct an effective control input vector based on the real-time motion data of the target object; S2, Based on the effective control input vector, and combined with the discrete kinematic equations of the linear hydrodynamic damping term, an extended Kalman filter state prediction model is constructed. S3. Using the state prediction model, the geometric constraint relationship between the laser beam and the confined underwater space boundary is calculated in real time, and the nonlinear observation Jacobian matrix is ​​reconstructed analytically. S4. Based on the observation Jacobian matrix, the observation signal-to-noise ratio of the state prediction model is evaluated online and the state boundary is forcibly constrained using a combination of an adaptive observation noise expansion mechanism based on the incident angle and a physical hard constraint correction method, so as to obtain the predicted position of the target object.

[0007] In one possible implementation, step S1 includes: S11, based on the real-time motion data, obtain the state vector of the target object. The real-time motion data includes the three-dimensional position coordinates and heading angle in the global coordinate system, and the linear velocity and yaw rate in the positive directions of the three coordinate axes in the carrier coordinate system. ; in, Indicates the sampling time. Indicates the first The state vector at the sampling time, , , They represent the first The X-axis, Y-axis, and Z-axis coordinates of the sampling time in the global coordinate system. Indicates the first The heading angle in the global coordinate system at the sampling time. , , Indicates the first The linear velocities along the positive X-axis, positive Y-axis, and positive Z-axis in the carrier coordinate system at the sampling time. Indicates the first The yaw rate in the carrier coordinate system at the sampling time; S12, calculate the projection matrix for gravitational acceleration; ; in, Represents the projection matrix, Represents gravitational acceleration. This indicates the pitch angle of the target object. This indicates the roll angle of the target object; S13, Based on the projection matrix, an effective control input vector is constructed by introducing an attitude rotation matrix and static zero-bias compensation: ; in, Indicates the first The effective control input vector at the sampling time. , Indicates the first Acceleration in the X-axis direction at the sampling time, Indicates the first Acceleration in the Y-axis direction at the sampling time. Indicates the first The acceleration in the Z-axis direction at the sampling time. Indicates the first Angular velocity in the Z-axis direction at the sampling time, This represents a pre-calibrated zero bias vector.

[0008] In one possible implementation, step S2 includes: S21, how the velocity increment in the carrier coordinate system, after damping attenuation, is projected onto the global coordinate system via a rotation matrix to update the position of the target object: ; in, Indicates from Sampling time has arrived Prior estimates at the sampling time, , , They represent the first The X-axis, Y-axis, and Z-axis coordinates of the sampling time in the global coordinate system. Indicates the first The heading angle in the global coordinate system at the sampling time. , , Indicates the first The linear velocities along the positive X-axis, positive Y-axis, and positive Z-axis in the carrier coordinate system at the sampling time. Indicates the first The yaw rate in the carrier coordinate system at the sampling time. This indicates the preset linear damping coefficient. Indicates the sampling time interval.

[0009] In one possible implementation, step S3 includes: S31, by calculating the partial derivative of the nonlinear function with respect to the state vector, the state transition Jacobian matrix is ​​obtained: ; in, Indicates the first The state transition Jacobian matrix at the sampling time. , This represents the linear velocity values ​​of the target object along the positive X-axis and the positive Y-axis in the carrier coordinate system. S32, Calculate the prior error covariance matrix: ; in, Indicates the first The prior error covariance matrix at the sampling time, express The covariance matrix at the sampling time, Indicates the first The noise covariance matrix at the sampling time; S33, Calculate the observation vector: ; in, This represents the depth observation value of the target object. This represents the first distance measurement observation. This represents the second distance measurement observation. This represents the third distance measurement observation. This represents the fourth distance measurement observation. Represents the observed angular velocity value. Indicates the magnetic heading observation value; S34, Calculate the pose of the probe of the target object in the global coordinate system: ; in, Indicates the first The X-axis coordinates of each probe in the global coordinate system Indicates the first X-axis prior state estimation at sampling time Indicates the prior heading angle, Indicates the first The distance of each probe from the origin along the X-axis in the carrier coordinate system. Indicates the first The distance of each probe from the origin along the Y-axis in the carrier coordinate system. Indicates the first The Y-axis coordinates of each probe in the global coordinate system Indicates the first Y-axis prior state estimation at sampling time Indicates the first The installation angle of each probe after transformation in the global coordinate system. The function represents constraining the angle to... Inside, Indicates the first The original installation angle of each probe in the global coordinate system. It is a positive integer, and its maximum value is 4.

[0010] S35, Define the candidate distance vector: ; in, This represents the distance along the X-axis from the first boundary of the confined underwater space. This represents the distance of the second boundary of the confined underwater space along the Y-axis. This indicates the X-axis coordinate of the selected probe in the carrier coordinate system. This indicates the Y-axis coordinate of the selected probe in the carrier coordinate system. This indicates the installation angle of the selected probe.

[0011] S36, Define the lever arm effect partial derivative term of the sensor relative to the center of rotation of the target object: ; ; in, Indicates the first The partial derivative of the X-axis lever arm effect of the probe in the global coordinate system. Indicates the first The probe's Y-axis lever arm effect partial derivative in the global coordinate system.

[0012] S37, the observed Jacobian matrix is ​​calculated as follows: ; ; ; in, This represents the cosine value of the installation angle of the first probe. This represents the cosine value of the installation angle of the second probe. This represents the sine value of the installation angle of the third probe. This represents the sine value of the installation angle of the fourth probe. This represents the X-axis coordinate of the first probe in the carrier coordinate system. This represents the X-axis coordinate of the second probe in the carrier coordinate system. This represents the Y-axis coordinate of the third probe in the carrier coordinate system. This represents the sine value of the installation angle of the first probe. This represents the sine value of the installation angle of the second probe. This represents the cosine value of the installation angle of the third probe. This represents the cosine value of the installation angle of the fourth probe. This represents the sine value of the installation angle of the fourth probe. This represents the partial derivative of the X-axis lever arm effect of the first probe in the global coordinate system. This represents the partial derivative of the X-axis lever arm effect of the second probe in the global coordinate system. This represents the partial derivative of the Y-axis lever arm effect of the third probe in the global coordinate system. This represents the Y-axis lever arm effect partial derivative of the fourth probe in the global coordinate system.

[0013] Wherein, the first ranging observation value is acquired by the first probe, the second ranging observation value is acquired by the second probe, the third ranging observation value is acquired by the third probe, and the fourth ranging observation value is acquired by the fourth probe.

[0014] In one possible implementation, the method described in step S4, which combines an adaptive observation noise dilation mechanism based on the observation Jacobian matrix with physical hard constraint correction, includes: S41, Calculate the physical incident angle of the sound beam in the current pose: ; in, This represents the physical angle of incidence of the sound beam at the current pose. Indicates the angle of incidence of the target wall determined by ray tracing; S42, Construct an adaptive observation noise dilation model based on the incident angle: ; in, Indicates the first The observation noise dilation model at the sampling time, Let V be the reference noise variance of the laser under perpendicular incidence. Indicates the coefficient of thermal expansion; S43, Calculate the Kalman gain: ; in, Represents the Kalman gain matrix; S44, Corrected prior state estimation: ; in, This represents the corrected prior estimate. This represents the theoretically predicted observed value; S45, perform covariance correction: .

[0015] In one possible implementation, step S4, which involves online evaluation of the observation signal-to-noise ratio of the state prediction model and enforced constraints on the state boundaries to obtain the predicted position of the target object, includes: S46, introduces a hard constraint correction module at the filter output: , ; in, Indicates the resultant velocity. This indicates the preset maximum speed.

[0016] S47, forcibly clamps the position estimate to the physical boundary of the confined underwater space: , ; This represents the predicted X-axis coordinate position at the k-th sampling time. This represents the predicted Y-axis coordinate position at the k-th sampling time. This represents the corrected prior estimate of the X-axis coordinates. This represents the corrected prior estimate of the Y-axis coordinate.

[0017] In one possible implementation, step S4 is followed by: The accuracy of the positioning method is verified by the error between the experimental true data and the predicted position of the target object at each sampling time.

[0018] The present invention also provides a positioning device for a confined underwater space, comprising: The input module is used to construct an effective control input vector based on the real-time motion data of the target object; The state module is used to construct an extended Kalman filter state prediction model based on the effective control input vector and the discrete kinematic equations of the linear hydrodynamic damping term. The matrix module is used to solve the geometric constraint relationship between the laser beam and the confined underwater space boundary in real time using the state prediction model, and to analyze and reconstruct the nonlinear observation Jacobian matrix. The prediction module is used to evaluate the observation signal-to-noise ratio of the state prediction model online and forcibly constrain the state boundary based on the observation Jacobian matrix, using a combination of an adaptive observation noise expansion mechanism based on the incident angle and a physical hard constraint correction method, so as to obtain the predicted position of the target object.

[0019] The present invention also provides an inspection robot, including a memory and a processor, wherein the processor is coupled to the memory and is used to execute the program stored in the memory to realize the above-described positioning method in confined underwater space.

[0020] The present invention also provides a computer-readable storage medium for storing a computer-readable program or instruction, which, when executed by a processor, enables the aforementioned positioning method in confined underwater space.

[0021] The beneficial effects of this invention are: by utilizing the hybrid observation model theory, the physical boundary information of a confined underwater space is transformed into dynamic geometric observation constraints, enabling better error analysis and state correction. For laser observation problems within confined underwater spaces, by combining omnidirectional ray tracing technology and the hybrid observation model, the geometric relationship between the laser beam and the environmental boundary is calculated in real time, thereby accurately identifying the physical validity of the observation data.

[0022] To address ranging distortion and outliers at grazing angles, this invention requires dynamic evaluation of observation noise. By combining an adaptive observation noise dilation mechanism based on the incident angle with an extended Kalman filter algorithm, the filter weights are dynamically adjusted according to the laser incident angle. This effectively suppresses sensor drift and laser outliers without adding expensive hardware, achieving highly robust millimeter-level positioning. Attached Figure Description

[0023] Figure 1 A flowchart illustrating a positioning method for a confined underwater space, as provided in an embodiment of the present invention; Figure 2 A layout diagram of sensors in an inspection robot provided in an embodiment of the present invention; Figure 3 A flowchart illustrating the algorithm for a positioning method in a confined underwater space, as provided in an embodiment of the present invention. Figures 4 to 6 A comparison diagram of a positioning method for confined underwater space provided in this embodiment of the invention and a traditional positioning method under three trajectories: straight line, rectangle, and circle. Figure 7 This is a structural diagram of a positioning device for a confined underwater space provided in an embodiment of the present invention. Detailed Implementation

[0024] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not intended to limit the scope of the present invention.

[0025] The Extended Kalman Filter (EKF) has demonstrated mature engineering applicability in state estimation of nonlinear systems, enabling effective fusion of multi-sensor data. While the standard EKF typically assumes a constant observation noise covariance matrix, the introduction of hybrid observation models and geometric computation methods such as ray casting makes it possible to explicitly utilize environmental geometric constraints during the filtering process, thus enabling the handling of non-Gaussian noise in complex acoustic environments. Furthermore, underwater spherical inspection robots face challenges in confined underwater space, including large cumulative errors from low-cost sensors and severe influence of incident angle on sonar observations, significantly impacting the robot's positioning accuracy.

[0026] Therefore, to address the problems of poor positioning accuracy of the aforementioned low-cost micro underwater spherical inspection robot in confined underwater spaces and severe interference from multipath effects and specular reflections in laser observation, a positioning method based on a geometrically constrained hybrid observation model and adaptive extended Kalman filtering (Hybrid-EKF) is proposed. Figure 1 A flowchart illustrating a positioning method for a confined underwater space provided in an embodiment of the present invention is shown below. Figure 1 As shown, the method includes: S1, construct an effective control input vector based on the real-time motion data of the target object; S2, Based on the effective control input vector, and combined with the discrete kinematic equations of the linear hydrodynamic damping term, an extended Kalman filter state prediction model is constructed. S3. Using the state prediction model, the geometric constraint relationship between the laser beam and the confined underwater space boundary is calculated in real time, and the nonlinear observation Jacobian matrix is ​​reconstructed analytically. S4. Based on the observation Jacobian matrix, the observation signal-to-noise ratio of the state prediction model is evaluated online and the state boundary is forcibly constrained using a combination of an adaptive observation noise expansion mechanism based on the incident angle and a physical hard constraint correction method, so as to obtain the predicted position of the target object.

[0027] In this embodiment of the invention, the confined underwater space can be a narrow body of water, such as a pool, pipe, or underground river; the target object can be a spherical inspection robot. This embodiment uses a spherical inspection robot as the target object for illustration, but it is not limited to this. The spherical inspection robot is equipped with four laser rangefinders, one depth sensor, one IMU inertial navigation module, and one camera. The IMU inertial navigation module integrates a magnetometer.

[0028] In terms of hardware layout and data acquisition, to minimize the interference of centripetal acceleration generated by the rotational motion of the inspection robot on measurement accuracy, the IMU inertial navigation module is precisely installed at the geometric center of the spherical inspection robot (i.e., the center of the chassis). As the core driving source for system state prediction, the IMU inertial navigation module is responsible for real-time acquisition of three-axis high-frequency acceleration and angular velocity data in the carrier coordinate system, providing basic dynamic input for pose estimation in a short time.

[0029] At the environmental perception level, four laser rangefinders are symmetrically distributed in a cross shape on the equatorial plane of the inspection robot, pointing in four orthogonal directions: front, back, left, and right. All four laser sensors are equidistant from the center. Crucially, each laser sensor has a pre-calibrated fixed installation offset relative to the center of the sphere. These geometric parameters not only describe the physical structure of the hardware, but also serve as the mathematical basis for subsequent algorithms to perform lever arm effect compensation and precise coordinate mapping.

[0030] In addition, the depth sensor is mounted laterally inside the inspection robot. It senses external water pressure through a pressure-conducting pipe and converts it into absolute depth information, which is used to independently correct the vertical axis position. The overall multi-sensor layout is as follows: Figure 2 As shown.

[0031] In this embodiment of the invention, the physical boundary information of a confined underwater space is transformed into dynamic geometric observation constraints using a hybrid observation model theory, enabling better error analysis and state correction. For laser observation within a confined underwater space, by combining omnidirectional ray tracing technology and a hybrid observation model, the geometric relationship between the laser beam and the environmental boundary is calculated in real time, allowing the system to accurately identify the physical validity of the observation data.

[0032] To address ranging distortion and outliers at grazing angles, this invention dynamically assesses observation noise. By combining an adaptive observation noise expansion mechanism based on the incident angle with the EKF algorithm, this invention dynamically adjusts the filtering weights according to the laser incident angle, effectively suppressing IMU drift and laser outliers without adding expensive hardware, achieving highly robust millimeter-level positioning.

[0033] Based on the above embodiments, step S1 includes: S11, based on the real-time motion data, obtain the state vector of the target object. The real-time motion data includes the three-dimensional position coordinates and heading angle in the global coordinate system, and the linear velocity and yaw rate in the positive directions of the three coordinate axes in the carrier coordinate system. (1) in, Indicates the sampling time. Indicates the first The state vector at the sampling time, , , They represent the first The X-axis, Y-axis, and Z-axis coordinates of the sampling time in the global coordinate system. Indicates the first The heading angle in the global coordinate system at the sampling time. , , Indicates the first The linear velocities along the positive X-axis, positive Y-axis, and positive Z-axis in the carrier coordinate system at the sampling time. Indicates the first The yaw rate in the carrier coordinate system at the sampling time; S12, calculate the projection matrix for gravitational acceleration; (2) in, Represents the projection matrix, Represents gravitational acceleration. This indicates the pitch angle of the target object. This indicates the roll angle of the target object; S13, Based on the projection matrix, an effective control input vector is constructed by introducing an attitude rotation matrix and static zero-bias compensation: (3) in, Indicates the first The effective control input vector at the sampling time. , Indicates the first Acceleration in the X-axis direction at the sampling time, Indicates the first Acceleration in the Y-axis direction at the sampling time. Indicates the first The acceleration in the Z-axis direction at the sampling time. Indicates the first Angular velocity in the Z-axis direction at the sampling time, This represents a pre-calibrated zero bias vector.

[0034] To accurately describe the six-degree-of-freedom motion of a miniature underwater spherical inspection robot in three-dimensional space, this invention first defines the system's state space and coordinate reference. Consider a discrete-time nonlinear dynamic system, whose navigation coordinate system is defined as the global coordinate system, where the Z-axis is defined as the depth axis, and the vertically downward direction is the positive Z-axis direction; the carrier coordinate system is fixed to the geometric center of the spherical inspection robot. Let... For discrete time steps, For the system in the first The state vector at the sampling time.

[0035] The carrier coordinate system, also known as the b-system, has its origin at the center of gravity of the inspection robot. The X-axis points to the right side of the carrier, the Y-axis points directly in front of the carrier (i.e., along the longitudinal axis of the carrier), and the Z-axis is vertically downward. The X, Y, and Z axes form a right-handed coordinate system.

[0036] To fully describe the pose and dynamic characteristics of the inspection robot, the state vector is defined as shown in formula (1). The advantage of this hybrid coordinate state definition in this embodiment of the invention is that defining the velocity in the carrier coordinate system allows for direct description of water resistance characteristics through fluid dynamics equations, while defining the position in the global coordinate system facilitates observation and updates in conjunction with environmental geometric constraints, thereby reducing the complexity of nonlinear modeling.

[0037] Before obtaining the state evolution equation, a clean system control input must be constructed. Since the IMU inertial navigation module readings contain gravity components and sensor zero bias, they cannot be directly used for dynamic integration. The projection matrix of the gravitational acceleration g is obtained as shown in Equation (2).

[0038] For each At the sampling time, the original measurement input is defined as... By introducing an attitude rotation matrix and static zero bias compensation, an effective control input vector for the system is constructed. As shown in formula (3).

[0039] This invention employs mathematical projection to remove the gravitational component, ensuring that the input vector contains only the motion acceleration generated by the robot's internal thrusters. This prevents spurious velocity integral drift caused by gravity leakage when the robot is tilted, thus improving the physical authenticity of the input data. The robot's state evolution follows nonlinear kinematic laws. Unlike an ideal vacuum environment, underwater motion is significantly affected by hydrodynamic damping.

[0040] Based on the above embodiments, step S2 includes: S21, how the velocity increment in the carrier coordinate system, after damping attenuation, is projected onto the global coordinate system via a rotation matrix to update the position of the target object: (4) in, Indicates from Sampling time has arrived Prior estimates at the sampling time, , , They represent the first The X-axis, Y-axis, and Z-axis coordinates of the sampling time in the global coordinate system. Indicates the first The heading angle in the global coordinate system at the sampling time. , , Indicates the first The linear velocities along the positive X-axis, positive Y-axis, and positive Z-axis in the carrier coordinate system at the sampling time. Indicates the first The yaw rate in the carrier coordinate system at the sampling time. This indicates the preset linear damping coefficient. Indicates the sampling time interval.

[0041] To improve prediction accuracy, this embodiment of the invention introduces a linear damping coefficient into the state transition function. The system state changed from... Sampling time has arrived Prior estimation at sampling time It can be done through nonlinear functions The description is shown in Equation (4). This set of equations describes how the velocity increment in the carrier coordinate system, after being damped and attenuated, is projected onto the global coordinate system through a rotation matrix to update the position.

[0042] Based on the above embodiments, step S3 includes: S31, by calculating the partial derivative of the nonlinear function with respect to the state vector, the state transition Jacobian matrix is ​​obtained: (5) in, Indicates the first The state transition Jacobian matrix at the sampling time. , This represents the linear velocity values ​​of the target object along the positive X-axis and the positive Y-axis in the carrier coordinate system. S32, Calculate the prior error covariance matrix: (6) in, Indicates the first The prior error covariance matrix at the sampling time, express The covariance matrix at the sampling time, Indicates the first The noise covariance matrix at the sampling time; S33, Calculate the observation vector: (7) in, This represents the depth observation value of the target object. This represents the first distance measurement observation. This represents the second distance measurement observation. This represents the third distance measurement observation. This represents the fourth distance measurement observation. Represents the observed angular velocity value. Indicates the magnetic heading observation value; S34, Calculate the pose of the probe of the target object in the global coordinate system: (8) in, Indicates the first The X-axis coordinates of each probe in the global coordinate system Indicates the first X-axis prior state estimation at sampling time Indicates the prior heading angle, Indicates the first The distance of each probe from the origin along the X-axis in the carrier coordinate system. Indicates the first The distance of each probe from the origin along the Y-axis in the carrier coordinate system. Indicates the first The Y-axis coordinates of each probe in the global coordinate system Indicates the first Y-axis prior state estimation at sampling time Indicates the first The installation angle of each probe after transformation in the global coordinate system. The function represents constraining the angle to... Inside, Indicates the first The original installation angle of each probe in the global coordinate system. It is a positive integer, and its maximum value is 4.

[0043] S35, Define the candidate distance vector: (9) in, This represents the distance along the X-axis from the first boundary of the confined underwater space. This represents the distance of the second boundary of the confined underwater space along the Y-axis. This indicates the X-axis coordinate of the selected probe in the carrier coordinate system. This indicates the Y-axis coordinate of the selected probe in the carrier coordinate system. This indicates the installation angle of the selected probe.

[0044] S36, Define the lever arm effect partial derivative term of the sensor relative to the center of rotation of the target object: ; (10) in, Indicates the first The partial derivative of the X-axis lever arm effect of the probe in the global coordinate system. Indicates the first The probe's Y-axis lever arm effect partial derivative in the global coordinate system.

[0045] S37, the observed Jacobian matrix is ​​calculated as follows: (11) ; ; in, This represents the cosine value of the installation angle of the first probe. This represents the cosine value of the installation angle of the second probe. This represents the sine value of the installation angle of the third probe. This represents the sine value of the installation angle of the fourth probe. This represents the X-axis coordinate of the first probe in the carrier coordinate system. This represents the X-axis coordinate of the second probe in the carrier coordinate system. This represents the Y-axis coordinate of the third probe in the carrier coordinate system. This represents the sine value of the installation angle of the first probe. This represents the sine value of the installation angle of the second probe. This represents the cosine value of the installation angle of the third probe. This represents the cosine value of the installation angle of the fourth probe. This represents the sine value of the installation angle of the fourth probe. This represents the partial derivative of the X-axis lever arm effect of the first probe in the global coordinate system. This represents the partial derivative of the X-axis lever arm effect of the second probe in the global coordinate system. This represents the partial derivative of the Y-axis lever arm effect of the third probe in the global coordinate system. This represents the Y-axis lever arm effect partial derivative of the fourth probe in the global coordinate system.

[0046] Wherein, the first ranging observation value is acquired by the first probe, the second ranging observation value is acquired by the second probe, the third ranging observation value is acquired by the third probe, and the fourth ranging observation value is acquired by the fourth probe.

[0047] It should be noted that the first probe, second probe, third probe, and fourth probe here correspond to the four laser ranging sensors in front, back, left, and right, respectively.

[0048] Since the Extended Kalman Filter (EKF) needs to propagate the error covariance within a linear frame, a linearized approximation of the nonlinear system at the current state point must be obtained. This is achieved by calculating the nonlinear function. For the state vector The partial derivatives are used to obtain the state transition Jacobian matrix. As shown in formula (5).

[0049] The non-zero elements in the state transition Jacobian matrix and The cross-coupling effect of heading and velocity errors on position prediction was characterized. The process noise covariance matrix... It is a diagonal matrix, and its specific values ​​are set according to the disturbance characteristics of the underwater environment. For example, the variance of the velocity term is taken as... To compensate for the truncation error of the linear damping model. Used to absorb unmodeled flow disturbances and higher-order hydrodynamic terms through statistical methods. , previous time step posterior covariance and By substituting the values ​​into the matrix and performing matrix operations, the predicted covariance value at the current time can be obtained.

[0050] Finally, the prior error covariance matrix of the system is obtained. The evolution over time is shown in Equation (6). The prior error covariance matrix... The uncertainty in the prediction stage is passed on to the subsequent observation update stage, thereby dynamically adjusting the system's trust weight for new observation data according to the degree of prediction uncertainty to complete the state correction.

[0051] To address the highly nonlinear characteristics of laser observation models in confined underwater spaces where robot pose changes drastically, this invention proposes a "prediction-synthesis" dynamic observation strategy. This strategy reconstructs the observation equations in real time using a ray tracing algorithm. The overall control system schematic of the algorithm is shown below. Figure 3 As shown.

[0052] First, define the system's observation vector. The data includes depth measurement, four lasers (front F, rear B, left L, right R), and IMU inertial navigation module. The definition is shown in formula (7). It should be noted that F corresponds to the first probe (i.e., the front laser sensor), B corresponds to the second probe (i.e., the rear laser sensor), L corresponds to the third probe (i.e., the left laser sensor), and R corresponds to the fourth probe (i.e., the right laser sensor).

[0053] To establish the geometric constraints between the laser beam and the environmental boundaries, the pose of the sensors fixed to the carrier must be mapped to the global coordinate system. Let the inspection robot carry the... The installation positions of the laser probes in the carrier coordinate system are as follows: The installation angle is Based on the prior state estimate of the previous time step. The pose of the probe in the global coordinate system It can be calculated through coordinate transformation, as shown in formula (8).

[0054] This step, by introducing a rotation matrix and a translation vector, achieves a mathematical mapping of the sensor's observation reference from the local carrier space to the global physical space, whereby... The function is used to constrain the angle to Within the interval. Utilizing the calculated global beam orientation. We construct the equations for the rays emanating from the center of the sensor.

[0055] Given the boundaries of the four perpendicular walls of the pool, i.e., the front wall Back wall Left wall Right wall Under the premise of [condition], calculate the geometric intersections of the ray with each of the four planes. Define candidate distance vectors. As shown in formula (9), the first two terms correspond to The walls along the axial direction, the last two items correspond to Walls along the axial direction.

[0056] In order to filter out the physically true observational prediction values ​​from the candidate solutions, solutions with values ​​less than zero or exceeding the physical range are logically eliminated, representing the values ​​of facing away from the wall or d being out of range, respectively. The minimum value among the remaining positive values ​​is taken as the "theoretical predicted observation value" of the laser. And record the index of the target wall hit. This index determines the specific analytical form of the observation function.

[0057] To meet the EKF's requirement for linearizing the nonlinear observation function, this embodiment of the invention utilizes the chain rule to analytically derive the observation Jacobian matrix in real time. First, define the lever arm effect partial derivative of the sensor relative to the center of rotation. and The position partial derivative in the Jacobian matrix reflects the "slope amplification effect," indicating that tilted observations are extremely sensitive to positional changes. The heading partial derivative further couples the "sweeping effect" caused by the slippage of the light spot due to angle changes with the "lever arm effect" caused by the probe rotating with the vehicle body, as shown in Equation (10). By accurately characterizing these two types of physical phenomena, the observation Jacobian matrix is ​​finally obtained, as shown in Equation (11), ensuring the accuracy of the linearized model.

[0058] Based on the above embodiments, the method combining the adaptive observation noise expansion mechanism based on the incident angle with physical hard constraint correction in step S4 includes: S41, Calculate the physical incident angle of the sound beam in the current pose: (12) in, This represents the physical angle of incidence of the sound beam at the current pose. Indicates the angle of incidence of the target wall determined by ray tracing; S42, Construct an adaptive observation noise dilation model based on the incident angle: (13) in, Indicates the first The observation noise dilation model at the sampling time, Let V be the reference noise variance of the laser under perpendicular incidence. Indicates the coefficient of thermal expansion; S43, Calculate the Kalman gain: (14) in, Represents the Kalman gain matrix; S44, Corrected prior state estimation: (15) in, This represents the corrected prior estimate. This represents the theoretically predicted observed value; S45, perform covariance correction: (16) Since the standard extended Kalman filter (EKF) typically assumes that the observation noise covariance matrix R is constant, this assumption often fails in underwater acoustic environments. Underwater lasers are prone to specular reflection at small grazing angles, leading to frequent non-Gaussian noise and outliers. Therefore, this invention introduces a physically-based adaptive adjustment mechanism to dynamically adjust the observation weights by evaluating the "laser incident angle" online.

[0059] First, based on the target wall normal vector determined by the aforementioned ray tracing... Calculate the physical incident angle of the sound beam at the current pose. As shown in formula (12), this angle directly reflects the geometric contact state between the laser and the wall.

[0060] Next, an adaptive observation noise dilation model based on the incident angle is constructed, the mathematical form of which is shown in formula (13). The physical meaning of this formula is: when hour, As the term approaches zero, the noise remains at the baseline level; with Increase This term leads to a sharp increase in noise variance. Mathematically, this forces the Kalman gain matrix to... The weighting of observations with large tilt angles is automatically reduced, thereby achieving robust fusion that is "trustworthy but not blind".

[0061] To completely eliminate unreliable observation data, this invention designs a dual gating mechanism combining physical and statistical methods. At the physical level, a critical angle threshold is set. If the incident angle exceeds the threshold, the laser is determined to be in the total reflection blind zone. The update path of the corresponding row of the Jacobian matrix is ​​cut off by forcibly setting it to zero. At the statistical level, the observation innovation is calculated, and outliers are eliminated using the 3-Sigma criterion and the absolute error threshold. If the limit is exceeded, the information is determined to be unreliable, and the matrix is ​​set to zero. After completing the observation quality assessment and screening, the state update step is performed. The Kalman gain is calculated and the prior state estimate is corrected, as shown in Equations (14) and (15), respectively.

[0062] It is worth noting that, in order to prevent computer floating-point rounding errors from affecting the posterior covariance matrix... Since the positive definiteness is lost, this invention abandons the standard simple update formula and instead adopts the Joseph form for covariance correction, as shown in formula (16). Although this form increases the computational cost slightly, it can guarantee... The symmetric positive definiteness of the algorithm significantly improves its numerical stability during long-term operation on embedded platforms.

[0063] Based on the above embodiments, step S4, which involves online evaluation of the observation signal-to-noise ratio of the state prediction model and forced constraint of the state boundary to obtain the predicted position of the target object, includes: S46, introduces a hard constraint correction module at the filter output: , (17) in, Indicates the resultant velocity. Indicates the preset maximum speed; S47, forcibly clamps the position estimate to the physical boundary of the confined underwater space: , (18) This represents the predicted X-axis coordinate position at the k-th sampling time. This represents the predicted Y-axis coordinate position at the k-th sampling time. This represents the corrected prior estimate of the X-axis coordinates. This represents the corrected prior estimate of the Y-axis coordinate.

[0064] To address the potential for physical constraint violations (such as velocity exceeding limits or position passing through walls) in unconstrained EKF optimization, this invention introduces a hard constraint correction module at the filter output. For dynamic constraints, the resultant velocity is calculated. If the maximum design speed is exceeded If the velocity direction remains unchanged, the velocity component is scaled proportionally, as shown in formula (17).

[0065] For spatial geometric constraints, the position estimate is forcibly clamped to the physical boundary of the confined submerged space. As shown in formula (18), this correction utilizes prior physical knowledge to perform a secondary correction on the filter output, fundamentally eliminating the occurrence of non-physical phenomena.

[0066] In some embodiments, step S4 is followed by: The accuracy of the positioning method is verified by the error between the experimental true data and the predicted position of the target object at each sampling time.

[0067] Figures 4 to 6 The paper demonstrates a comparison of the positioning performance of the proposed Hybrid-EKF algorithm with traditional pure IMU dead reckoning under different trajectory inputs, including straight lines, rectangles, and circular reciprocating motions. The 3D simulation results, which include both horizontal and vertical coordinates, show that even when the trajectory of pure IMU reckoning suffers severe drift due to accumulated errors (red dashed line), the fused trajectory (blue solid line) using the algorithm of this invention still closely matches the actual physical path.

[0068] Especially in right-angle turns on rectangular trajectories and large-angle continuous turns on circular trajectories, this invention effectively suppresses positioning jumps caused by laser multipath effects and specular reflections through ray-tracing wall selection and adaptive noise adjustment. Compared with traditional methods, this invention significantly improves positioning accuracy in confined underwater spaces, and its trajectory smoothness and convergence speed are superior to models without geometric constraints. This verifies the strong robustness of the adaptive hybrid observation model to non-Gaussian noise in harsh acoustic environments, thereby significantly improving the autonomous positioning performance of micro underwater robots.

[0069] Figure 7 A structural diagram of a positioning device for a confined underwater space provided in an embodiment of the present invention is shown below. Figure 7 As shown, the device includes: The input module 710 is used to construct an effective control input vector based on the real-time motion data of the target object; State module 720 is used to construct an extended Kalman filter state prediction model based on the effective control input vector and the discrete kinematic equations of the linear hydrodynamic damping term. Matrix module 730 is used to solve the geometric constraint relationship between the laser beam and the confined underwater space boundary in real time using the state prediction model, and to analyze and reconstruct the nonlinear observation Jacobian matrix. The prediction module 740 is used to evaluate the observation signal-to-noise ratio of the state prediction model online and forcibly constrain the state boundary based on the observation Jacobian matrix by using a combination of an adaptive observation noise expansion mechanism of the incident angle and physical hard constraint correction, so as to obtain the predicted position of the target object.

[0070] This embodiment is a system embodiment corresponding to the above method. Its specific implementation process is the same as that of the above method embodiment. For details, please refer to the above method embodiment. This system embodiment will not repeat the details.

[0071] In one embodiment, the present invention also provides an inspection robot. The inspection robot includes a memory and a processor. Furthermore, the inspection robot is equipped with four laser rangefinders, one IMU inertial navigation unit, a camera unit, and a depth sensor. The four laser rangefinders, one IMU inertial navigation unit, camera unit, and depth sensor have been fully described in the above method embodiments; please refer to the above method embodiments for details. This embodiment will not repeat these details further.

[0072] In some embodiments, the memory can be an internal storage unit of the inspection robot, such as the robot's hard drive or RAM. In other embodiments, the memory can also be an external storage device for the inspection robot, such as a plug-in hard drive, smart media card (SMC), secure digital card (SD), flash card, etc.

[0073] Furthermore, the memory can include both internal storage units and external storage devices for the inspection robot, and is used to install the inspection robot's application software and various types of data.

[0074] In some embodiments, the processor may be a single server or a group of servers. Servers may be centralized or distributed. In some embodiments, the processor may be local or remote. In some embodiments, the processor may be implemented on a cloud platform. In some embodiments, the cloud platform may include a private cloud, public cloud, hybrid cloud, community cloud, distributed cloud, internal cloud, multi-cloud, or any combination thereof.

[0075] Furthermore, when the processor executes the positioning program for the confined underwater space in memory, the following steps can be implemented: Construct an effective control input vector based on the real-time motion data of the target object; Based on the effective control input vector, and combined with the discrete kinematic equations of the linear hydrodynamic damping term, an extended Kalman filter state prediction model is constructed. Using the aforementioned state prediction model, the geometric constraint relationship between the laser beam and the confined underwater space boundary is calculated in real time, and the nonlinear observation Jacobian matrix is ​​analytically reconstructed. Based on the observation Jacobian matrix, the observation signal-to-noise ratio of the state prediction model is evaluated online and the state boundary is forcibly constrained by a combination of an adaptive observation noise expansion mechanism based on the incident angle and a physical hard constraint correction, thereby obtaining the predicted position of the target object.

[0076] It should be understood that when the processor executes the program for the method of locating confined underwater space in memory, in addition to the functions mentioned above, it can also implement other functions, as can be found in the description of the corresponding method embodiments above.

[0077] Accordingly, embodiments of the present invention also provide a computer storage medium storing a computer program, which, when executed by a processor, implements the steps of a positioning method for a confined underwater space as described above. Alternatively, when executed by a processor, the computer program implements the functions of each module / unit in the positioning device for a confined underwater space described above.

[0078] Those skilled in the art will understand that all or part of the processes of the methods described in the above embodiments can be implemented by a computer program instructing related hardware, and the program can be stored in a computer-readable storage medium. The computer-readable storage medium may be a disk, optical disk, read-only memory, or random access memory, etc.

[0079] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A method of locating a confined subsea space, characterized by, include: S1, construct an effective control input vector based on the real-time motion data of the target object; S2, Based on the effective control input vector, and combined with the discrete kinematic equations of the linear hydrodynamic damping term, an extended Kalman filter state prediction model is constructed. S3. Using the state prediction model, the geometric constraint relationship between the laser beam and the confined underwater space boundary is calculated in real time, and the nonlinear observation Jacobian matrix is ​​reconstructed analytically. S4. Based on the observation Jacobian matrix, the observation signal-to-noise ratio of the state prediction model is evaluated online and the state boundary is forcibly constrained using a combination of an adaptive observation noise expansion mechanism based on the incident angle and a physical hard constraint correction method, so as to obtain the predicted position of the target object.

2. The method of locating a confined subspace under a liquid of claim 1, wherein, Step S1 includes: S11, based on the real-time motion data, obtain the state vector of the target object. The real-time motion data includes the three-dimensional position coordinates and heading angle in the global coordinate system, and the linear velocity and yaw rate in the positive directions of the three coordinate axes in the carrier coordinate system. ； in, Indicates the sampling time. Indicates the first The state vector at the sampling time, , , They represent the first The X-axis, Y-axis, and Z-axis coordinates of the sampling time in the global coordinate system. Indicates the first The heading angle in the global coordinate system at the sampling time. , , Indicates the first The linear velocities along the positive X-axis, positive Y-axis, and positive Z-axis in the carrier coordinate system at the sampling time. Indicates the first The yaw rate in the carrier coordinate system at the sampling time; S12, calculate the projection matrix for gravitational acceleration; ； in, Represents the projection matrix, Represents gravitational acceleration. This indicates the pitch angle of the target object. This indicates the roll angle of the target object; S13, Based on the projection matrix, an effective control input vector is constructed by introducing an attitude rotation matrix and static zero-bias compensation: ； in, Indicates the first The effective control input vector at the sampling time. , Indicates the first Acceleration in the X-axis direction at the sampling time, Indicates the first Acceleration in the Y-axis direction at the sampling time. Indicates the first The acceleration in the Z-axis direction at the sampling time. Indicates the first Angular velocity in the Z-axis direction at the sampling time, This represents a pre-calibrated zero bias vector.

3. The method of locating a confined subspace under water according to claim 2, wherein, Step S2 includes: S21, how the velocity increment in the carrier coordinate system, after damping attenuation, is projected onto the global coordinate system via a rotation matrix to update the position of the target object: ； in, Indicates from Sampling time has arrived Prior estimates at the sampling time, , , They represent the first The X-axis, Y-axis, and Z-axis coordinates of the sampling time in the global coordinate system. Indicates the first The heading angle in the global coordinate system at the sampling time. , , Indicates the first The linear velocities along the positive X-axis, positive Y-axis, and positive Z-axis in the carrier coordinate system at the sampling time. Indicates the first The yaw rate in the carrier coordinate system at the sampling time. This indicates the preset linear damping coefficient. Indicates the sampling time interval.

4. The positioning method for a confined underwater space according to claim 3, characterized in that, Step S3 includes: S31, by calculating the partial derivative of the nonlinear function with respect to the state vector, the state transition Jacobian matrix is ​​obtained: ； in, Indicates the first The state transition Jacobian matrix at the sampling time. , This represents the linear velocity values ​​of the target object along the positive X-axis and the positive Y-axis in the carrier coordinate system. S32, Calculate the prior error covariance matrix: ； wherein, represents the prior error covariance matrix at the sampling instant, represents the covariance matrix at the sampling instant, represents the noise covariance matrix at the sampling instant; S33, Calculate the observation vector: ； in, This represents the depth observation value of the target object. This represents the first distance measurement observation. This represents the second distance measurement observation. This represents the third distance measurement observation. This represents the fourth distance measurement observation. Represents the observed angular velocity value. Indicates the magnetic heading observation value; S34, Calculate the pose of the probe of the target object in the global coordinate system: ； in, Indicates the first The X-axis coordinates of each probe in the global coordinate system Indicates the first X-axis prior state estimation at sampling time Indicates the prior heading angle, Indicates the first The distance of each probe from the origin on the X-axis in the carrier coordinate system. Indicates the first The distance of each probe from the origin on the Y-axis in the carrier coordinate system. Indicates the first The Y-axis coordinates of each probe in the global coordinate system Indicates the first Y-axis prior state estimation at sampling time Indicates the first The installation angle of each probe after transformation in the global coordinate system. The function represents constraining the angle to... Inside, Indicates the first The original installation angle of each probe in the global coordinate system. It is a positive integer, and its maximum value is 4; S35, Define the candidate distance vector: ； in, This represents the distance along the X-axis from the first boundary of the confined underwater space. This represents the distance of the second boundary of the confined underwater space along the Y-axis. This indicates the X-axis coordinate of the selected probe in the carrier coordinate system. This indicates the Y-axis coordinate of the selected probe in the carrier coordinate system. This indicates the installation angle of the selected probe; S36, Define the lever arm effect partial derivative term of the sensor relative to the center of rotation of the target object: ； ； in, Indicates the first The partial derivative of the X-axis lever arm effect of the probe in the global coordinate system. Indicates the first The Y-axis lever arm effect partial derivative of the probe in the global coordinate system; S37, the observed Jacobian matrix is ​​calculated as follows: ； ； ； in, This represents the cosine value of the installation angle of the first probe. This represents the cosine value of the installation angle of the second probe. This represents the sine value of the installation angle of the third probe. This represents the sine value of the installation angle of the fourth probe. This represents the X-axis coordinate of the first probe in the carrier coordinate system. This represents the X-axis coordinate of the second probe in the carrier coordinate system. This represents the Y-axis coordinate of the third probe in the carrier coordinate system. This represents the sine value of the installation angle of the first probe. This represents the sine value of the installation angle of the second probe. This represents the cosine value of the installation angle of the third probe. This represents the cosine value of the installation angle of the fourth probe. This represents the sine value of the installation angle of the fourth probe. This represents the partial derivative of the X-axis lever arm effect of the first probe in the global coordinate system. This represents the partial derivative of the X-axis lever arm effect of the second probe in the global coordinate system. This represents the partial derivative of the Y-axis lever arm effect of the third probe in the global coordinate system. This represents the partial derivative of the Y-axis lever arm effect of the fourth probe in the global coordinate system; Wherein, the first ranging observation value is acquired by the first probe, the second ranging observation value is acquired by the second probe, the third ranging observation value is acquired by the third probe, and the fourth ranging observation value is acquired by the fourth probe.

5. The method of locating a confined subspace under a liquid of claim 4, wherein, The method described in step S4, which combines the adaptive observation noise dilation mechanism based on the observation Jacobian matrix with physical hard constraint correction, includes: S41, Calculate the physical incident angle of the sound beam in the current pose: ； wherein, denotes the physical angle of incidence of the sound beam at the current pose, denotes the angle of incidence of the target wall determined by ray tracing; S42, Construct an adaptive observation noise dilation model based on the incident angle: ； in, Indicates the first The observation noise dilation model at the sampling time, Let V be the reference noise variance of the laser under perpendicular incidence. Indicates the coefficient of thermal expansion; S43, Calculate the Kalman gain: ； wherein denotes the Kalman gain matrix; S44, Corrected prior state estimation: ； wherein, represents the corrected prior estimate, represents the theoretical predicted observation value; S45, perform covariance correction: 。 6. The method of locating a confined subspace under a liquid of claim 5, wherein, Step S4, which involves online evaluation of the observation signal-to-noise ratio of the state prediction model and forced constraint of the state boundaries to obtain the predicted position of the target object, includes: S46, introduces a hard constraint correction module at the filter output: ， ； wherein, represents the combined speed, represents a preset maximum cruising speed; S47, forcibly clamps the position estimate to the physical boundary of the confined underwater space: ， ； This represents the predicted X-axis coordinate position at the k-th sampling time. This represents the predicted Y-axis coordinate position at the k-th sampling time. This represents the corrected prior estimate of the X-axis coordinates. This represents the corrected prior estimate of the Y-axis coordinate.

7. The positioning method for a confined underwater space according to claim 1, characterized in that, Step S4 is followed by: The accuracy of the positioning method is verified by the error between the experimental true data and the predicted position of the target object at each sampling time.

8. A positioning device for a confined underwater space, characterized in that, include: The input module is used to construct an effective control input vector based on the real-time motion data of the target object; The state module is used to construct an extended Kalman filter state prediction model based on the effective control input vector and the discrete kinematic equations of the linear hydrodynamic damping term. The matrix module is used to solve the geometric constraint relationship between the laser beam and the confined underwater space boundary in real time using the state prediction model, and to analyze and reconstruct the nonlinear observation Jacobian matrix. The prediction module is used to evaluate the observation signal-to-noise ratio of the state prediction model online and forcibly constrain the state boundary based on the observation Jacobian matrix, using a combination of an adaptive observation noise expansion mechanism based on the incident angle and a physical hard constraint correction method, so as to obtain the predicted position of the target object.

9. A patrol robot characterized by comprising: The device includes a memory and a processor, the processor being coupled to the memory for executing a computer program stored in the memory to implement the positioning method for confined underwater space as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, Used to store computer-readable programs or instructions, which, when executed by a processor, enable the positioning method for confined underwater space as described in any one of claims 1 to 7.

Images