Research on Navigation Method for Subsea Drilling Robot Based on Inertial Navigation and Odometry

1. Introduction

With the increasing global efforts to develop marine energy, especially the refined exploration of deep-sea strategic resources such as natural gas hydrates [1,2,3], the autonomous operation capability of seabed drilling robots, as a key piece of equipment capable of in-situ detection and sampling in deep seabed sedimentary layers, has attracted much attention. However, such robots usually work in extreme environments with high pressure, no GNSS signals and limited underwater acoustic channels in the deep sea, and traditional navigation and positioning methods face severe challenges. Although the combined navigation scheme based on strapdown inertial navigation (SINS) and Doppler velocimeter (DVL) is widely used in underwater vehicles, in drilling operations, the shearing action of the drill bit and the track on the seabed will agitate a large amount of sediment, resulting in extremely turbid bottom waters, which can easily cause DVL acoustic beam lock-up or a sharp drop in accuracy [4,5]. Therefore, using a high-precision odometer or tail cable encoder to assist SINS has become a highly reliable alternative [6].

Although the SINS/OD combination scheme is stable on hard surfaces, its reliability is severely constrained by the “terrain-mechanical” coupling effect on soft clay or silt geology widely distributed in the shallow surface of the deep sea. The soft seabed is a typical deformable terrain. Multiple studies [7,8,9] have pointed out that when the robot travels or drills on this type of geology, it is very easy to generate nonlinear longitudinal slip and lateral slip. This “slip” phenomenon will cause the “cable length” or “wheel mileage” recorded by the encoder to be much greater than the actual displacement of the robot, introducing non-Gaussian error with heavy tail characteristics into the observation equation. If the traditional Kalman filter (KF) or extended Kalman filter (EKF) is used, these abnormal observations will quickly contaminate the state estimation loop, causing the navigation system to diverge.

To cope with the above nonlinear and non-Gaussian interference, the filtering algorithm has evolved from EKF to capacitive Kalman filter (CKF) [10]. CKF, based on the third-order spherical radial volume criterion, avoids the linearization truncation error of EKF and has higher accuracy when dealing with high-dimensional nonlinear systems [9]. However, the standard CKF is susceptible to rounding errors in long-term operation, resulting in a non-positive definite covariance matrix and lacks resistance to outliers. Recently, the academic community has proposed a series of improved algorithms, such as the introduction of Huber robustness theory and maximum cross-correlation entropy criterion in references [11,12] to enhance the filtering robustness; and the use of variational Bayes to solve the problem of unknown noise statistical characteristics in references [13]. Nevertheless, in the specific scenario of seabed drilling, how to balance the rapid response to sudden faults (such as severe slippage) and the continuous correction of slowly varying errors (such as IMU zero bias) is still a key problem to be solved [14].

In addition, fully exploring the kinematic constraints of the robot itself is also an effective way to improve accuracy. Non-holonomic constraints (NHC) and zero-velocity correction (ZUPT) are often used to suppress SINS drift [15,16,17]. However, on rugged seabeds, forcibly applying a hard constraint of “zero lateral velocity” may introduce model bias [18,19]. Therefore, it is particularly important to construct a soft constraint mechanism that can adaptively adjust weights according to the interaction state of the seabed.

In view of this, this paper takes the independently developed subsea drilling robot as the object and studies a high-precision robust navigation method suitable for soft mud geology. The main contributions of this paper are as follows:

1. In view of the special characteristics of the distance measurement of the tail cable encoder of the drilling robot, a unified error state space model containing time-varying scaling factors was established, and NHC and ZUPT auxiliary measurement equations adapted to the seabed operation mode were designed;

2. A robust square root commensurate Kalman filter (RSRCKF) algorithm was proposed, which uses QR decomposition to ensure numerical stability, and realizes online detection and “soft isolation” of slippage anomalies by constructing new chi-square statistics and adaptive robust factors;

3. Through field tests on the mudflats of Zhoushan, the slippage suppression ability and long-term positioning accuracy of the method in real soft mud environment were verified.

2. System Architecture of Submarine Drilling Robot

The seabed stratum drilling robot system is a type of operational equipment that operates in high pressure, low visibility and strong disturbance environment [20]. Its navigation system needs to work autonomously for a long time under conditions of no GNSS and weak external reference. The overall system architecture consists of two parts: the seabed base station platform and the drilling robot system. The seabed base station platform provides deployment, retrieval, power supply, and communication functions for the drilling robot. The drilling robot system has built-in control modules, drive modules, data acquisition modules, and sensor modules, enabling the robot to drill freely in the seabed.

Figure 1. Overall Structure Model of Submarine Base Station.

Figure 1. Overall Structure Model of Submarine Base Station.

Figure 2. Overall Structure Model of Submarine Bottom Drilling Robot.

Figure 2. Overall Structure Model of Submarine Bottom Drilling Robot.

3. Error Model of Inertial/Odometry Combined Navigation

The core of building a high-precision integrated navigation system lies in accurately describing the error propagation mechanism of each subsystem and their mutual coupling relationship under specific operating conditions. For the special working conditions of seabed drilling robots operating in deep-sea soft mud sediment layers, relying solely on raw sensor data cannot meet the long-term positioning requirements. This chapter will combine the latest robust filtering theory and seabed surface mechanical properties to establish a unified state-space model that includes strapdown inertial navigation (SINS) system errors and odometer (OD) errors considering soft mud slippage, and will deeply analyze the physical applicability boundaries of non-integrity constraints (NHC) and zero-velocity correction (ZUPT) in drilling operations.

3.1. Coordinate System Definition

Inertial coordinate system: abbreviated as i-frame. The origin of the coordinate system is located at the Earth’s center. This coordinate system is absolutely stationary and does not rotate with the Earth.

Earth coordinate system: abbreviated as e-system. The origin of the coordinate system is located at the Earth’s center. This coordinate system rotates with the Earth.

Carrier coordinate system: abbreviated as b-system. The origin of the coordinate system is located at the center of gravity of the carrier. The coordinate axis direction is related to the motion direction of the carrier.

Navigation coordinate system: abbreviated as n-system. The origin of the coordinate system is located at the center of gravity of the vehicle. The geographical location of the vehicle (longitude φ, latitude λ) determines the orientation of this coordinate system.

3.2. Error Model of Inertial Navigation System

Inertial navigation systems (INS) are classified as inferential navigation, which infers the position of the next point from the position of a known point based on continuous measurements of the heading angle and velocity of a moving object. Therefore, the current position of a moving object can be continuously determined. A coordinate system is established based on the measurement axis of the gyroscope in the INS, and the gyroscope provides the direction and attitude angle of the moving object. The accelerometer and the gyroscope share the same measurement axis. The acceleration of the moving object is measured by the accelerometer. The velocity is obtained by integrating the velocity over time once, and the displacement is obtained by integrating the velocity over time once [21,22,23,24].

The attitude angle error vector of the inertial navigation system in the n-coordinate system is:

$$\phi ={\left[\begin{array}{ccc}\mathsf{\delta }{\phi }_{\mathrm{E}}& \mathsf{\delta }{\phi }_{\mathrm{N}}& \mathsf{\delta }{\phi }_{\mathrm{U}}\end{array}\right]}^{\mathrm{T}}$$

where, ${\phi }_{\mathrm{E}}$、${\phi }_{\mathrm{N}}$and ${\phi }_{\mathrm{U}}$These are referred to as east, north, and sky attitude angle errors, respectively. The attitude angle error equation of the inertial navigation system (INS) can be expressed as:

$$\dot{\phi }=-{\mathsf{\omega }}_{\mathrm{i}\mathrm{n}}^{\mathrm{n}}\times \phi +\mathsf{\delta }{\mathsf{\omega }}_{\mathrm{i}\mathrm{n}}^{\mathrm{n}}-{\mathrm{c}}_{\mathrm{b}}^{\mathrm{n}}{\mathsf{\epsilon }}^{\mathrm{b}}$$

where,${\mathsf{\omega }}_{\mathrm{i}\mathrm{n}}^{\mathrm{n}}={\mathsf{\omega }}_{\mathrm{i}\mathrm{e}}^{\mathrm{n}}+{\mathsf{\omega }}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}$, ${\mathsf{\omega }}_{\mathrm{i}\mathrm{e}}^{\mathrm{n}}={\left[0{\mathrm{w}}_{\mathrm{i}\mathrm{e}}\cos \left(\mathsf{\lambda }\right){\mathrm{w}}_{\mathrm{i}\mathrm{e}}\sin \left(\mathsf{\lambda }\right)\right]}^{\mathrm{T}}$are the components of the Earth’s rotational angular velocity${\mathrm{w}}_{\mathrm{i}\mathrm{e}}$in the navigation coordinate system.${\mathsf{\omega }}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}={\left[\begin{array}{ccc}-{\displaystyle \frac{{\mathrm{v}}_{\mathrm{n}}}{{\mathrm{R}}_{\mathrm{n}}+\mathrm{h}}}& {\displaystyle \frac{{\mathrm{v}}_{\mathrm{e}}}{{\mathrm{R}}_{\mathrm{e}}+\mathrm{h}}}& {\displaystyle \frac{{\mathrm{v}}_{\mathrm{e}}\tan \left(\mathsf{\lambda }\right)}{{\mathrm{R}}_{\mathrm{e}}+\mathrm{h}}}\end{array}\right]}^{\mathrm{T}}$, ${\mathrm{v}}_{\mathrm{n}}{,\mathrm{v}}_{\mathrm{e}}$represent the speed of the vehicle in the north and east directions respectively.${\mathrm{R}}_{\mathrm{n}},{\mathrm{R}}_{\mathrm{e}}$represent the radii of curvature of the meridian and the zonal circle at the current latitude respectively.$\mathrm{h}$represent the current altitude of the vehicle.${\mathsf{\epsilon }}^{\mathrm{b}}={\left[{\mathsf{\epsilon }}_{\mathrm{x}}{\mathsf{\epsilon }}_{\mathrm{y}}{\mathsf{\epsilon }}_{\mathrm{z}}\right]}^{\mathrm{T}}$This represents the constant drift error of the gyroscope installed in the carrier coordinate system. These errors accumulate during the attitude calculation process, causing the attitude angle error to increase over time, which is one of the main error sources of INS.${\mathrm{C}}_{\mathrm{b}}^{\mathrm{n}}$is the transformation matrix from the b-coordinate system to the n-coordinate system, defined by the following formula:

$${\mathrm{C}}_{\mathrm{b}}^{\mathrm{n}}=\left[\begin{array}{ccc}\cos \left(\mathsf{\psi }\right)\cos \left(\mathsf{\gamma }\right)+\sin \left(\mathsf{\psi }\right)\sin \left(\mathsf{\gamma }\right)\sin \left(\mathsf{\theta }\right)& \sin \left(\mathsf{\gamma }\right)\cos \left(\mathsf{\theta }\right)& \sin \left(\mathsf{\psi }\right)\cos \left(\mathsf{\gamma }\right)-\cos \left(\mathsf{\psi }\right)\sin \left(\mathsf{\gamma }\right)\sin \left(\mathsf{\theta }\right)\\ -\cos \left(\mathsf{\psi }\right)\sin \left(\mathsf{\gamma }\right)+\sin \left(\mathsf{\psi }\right)\cos \left(\mathsf{\gamma }\right)\sin \left(\mathsf{\theta }\right)& \cos \left(\mathsf{\gamma }\right)\cos \left(\mathsf{\theta }\right)& -\sin \left(\mathsf{\psi }\right)\sin \left(\mathsf{\gamma }\right)-\cos \left(\mathsf{\psi }\right)\cos \left(\mathsf{\gamma }\right)\sin \left(\mathsf{\theta }\right)\\ -\sin \left(\mathsf{\psi }\right)\cos \left(\mathsf{\theta }\right)& \sin \left(\mathsf{\theta }\right)& \cos \left(\mathsf{\psi }\right)\cos \left(\mathsf{\theta }\right)\end{array}\right]$$

where$\mathsf{\psi }$represents the heading angle,$\mathsf{\gamma }$represents the roll angle,$\mathsf{\theta }$represents the pitch angle.

According to the specific force equation of inertial navigation, the inertial error equation of the inertial navigation system can be formulated as:

$$\mathsf{\delta }{\dot{\mathrm{v}}}^{\mathrm{n}}={\mathrm{f}}^{\mathrm{n}}\times \phi -\left(2{\mathsf{\omega }}_{\mathrm{i}\mathrm{e}}^{\mathrm{n}}+{\mathsf{\omega }}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}\right)\times \mathsf{\delta }{\mathrm{v}}^{\mathrm{n}}+{\mathrm{v}}^{\mathrm{n}}\times \left(2\mathsf{\delta }{\mathsf{\omega }}_{\mathrm{i}\mathrm{e}}^{\mathrm{n}}+\mathsf{\delta }{\mathsf{\omega }}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}\right)+{\mathrm{c}}_{\mathrm{b}}^{\mathrm{n}}{\mathsf{\sigma }}^{\mathrm{b}}$$

where,$\mathsf{\delta }{\mathrm{v}}^{\mathrm{n}}={\left[\mathsf{\delta }{\mathrm{v}}_{\mathrm{E}}^{\mathrm{n}}\mathsf{\delta }{\mathrm{v}}_{\mathrm{N}}^{\mathrm{n}}\mathsf{\delta }{\mathrm{v}}_{\mathrm{U}}^{\mathrm{n}}\right]}^{\mathrm{T}}$  is the speed error of INS,${\mathrm{f}}^{\mathrm{n}}={\left[\begin{array}{ccc}{\mathrm{f}}_{\mathrm{e}}& {\mathrm{f}}_{\mathrm{n}}& {\mathrm{f}}_{\mathrm{u}}\end{array}\right]}^{\mathrm{T}}$is the specific force acceleration in the navigation coordinate system.${\mathrm{v}}^{\mathrm{n}}={\left[\begin{array}{ccc}{\mathrm{v}}_{\mathrm{e}}& {\mathrm{v}}_{\mathrm{n}}& {\mathrm{v}}_{\mathrm{u}}\end{array}\right]}^{\mathrm{T}}$is the velocity in the three directions of the navigation coordinate system.${\mathsf{\sigma }}^{\mathrm{b}}={\left[{\mathsf{\sigma }}_{\mathrm{x}}{\mathsf{\sigma }}_{\mathrm{y}}{\mathsf{\sigma }}_{\mathrm{z}}\right]}^{\mathrm{T}}$is the zero bias of acceleration in the three directions under the b-coordinate system.

The position error equation is expressed by Taylor expansion as:

$$\left[\begin{array}{c}\delta \dot{\mathrm{L}}\\ \delta \dot{\mathsf{\lambda }}\\ \delta \dot{\mathrm{h}}\end{array}\right]=\left[\begin{array}{ccc}0& {\displaystyle \frac{1}{{\mathrm{R}}_{\mathrm{n}}+\mathrm{h}}}& 0\\ {\displaystyle \frac{\sec \mathrm{L}}{{\mathrm{R}}_{\mathrm{n}}+\mathrm{h}}}& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\delta {\mathrm{v}}_{\mathrm{e}}\\ \delta {\mathrm{v}}_{\mathrm{n}}\\ \delta {\mathrm{v}}_{\mathrm{u}}\end{array}\right]+\left[\begin{array}{ccc}0& 0& -{\displaystyle \frac{{\mathrm{v}}_{\mathrm{N}}}{{\left({\mathrm{R}}_{\mathrm{e}}+\mathrm{h}\right)}^2}}\\ {\displaystyle \frac{{\mathrm{v}}_{\mathrm{e}}\sec \mathrm{Ltan}\mathrm{L}}{{\mathrm{R}}_{\mathrm{n}}+\mathrm{h}}}& 0& -{\displaystyle \frac{{\mathrm{v}}_{\mathrm{e}}\sec \mathrm{L}}{{\left({\mathrm{R}}_{\mathrm{n}}+\mathrm{h}\right)}^2}}\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}\delta L\\ \delta \lambda \\ \delta h\end{array}\right]$$

Traditional inertial navigation error modeling usually assumes that the process noise is white noise. However, references [13] point out that in the complex seabed environment, noise often has heavy tail characteristics or time-varying characteristics. The simple white noise model cannot describe the noise change of the drilling machine under the different modes of “driving” and “drilling”. Therefore, the variational Bayes method proposed in reference [13] will be introduced in the following chapters of this paper to estimate the noise parameters online in order to correct the system noise covariance matrix of the above linear model.

3.3. Odometer Error Model

The odometer measures the velocity of the carrier in the forward direction by collecting the number of rotations of the odometer wheel within a fixed time. The forward speed of the carrier measured by the odometer is defined as${\mathrm{v}}_{\mathrm{o}\mathrm{d}\mathrm{o}}^{\mathrm{b}}$. Since the composite cable at the tail of the drilling robot is fixed by the base station pulley, the movement of the composite cable drives the angle encoder to rotate using the spring preload, so the lateral and vertical speeds are zero. The forward velocity in the b-coordinate system output by the odometer has zero velocity in both the x and z directions. Ideally, the speed of the drilling rig is expressed as:

$${\mathrm{v}}_{\mathrm{o}\mathrm{d}\mathrm{o}}^{\mathrm{b}}=[0{\mathrm{v}}_{\mathrm{o}\mathrm{d}\mathrm{o}-\mathrm{y}}^{\mathrm{b}}0{]}^{\mathrm{T}}$$

In actual situations, the odometer has a scale coefficient error and an installation angle deviation angle. Let the scale coefficient error be${\mathsf{\delta }}_{\mathrm{K}}$, and the installation angle error be$\mathsf{\alpha }={\left[\begin{array}{ccc}{\mathsf{\alpha }}_{\mathsf{\theta }}& {\mathsf{\alpha }}_{\mathsf{\gamma }}& {\mathsf{\alpha }}_{\mathsf{\psi }}\end{array}\right]}^{\mathrm{T}}$, then the relationship between its odometer output speed${\mathrm{V}}_{\mathrm{o}\mathrm{d}\mathrm{o}}^{\mathrm{b}}$and the speed in the navigation coordinate system${\mathrm{V}}_{\mathrm{o}\mathrm{d}\mathrm{o}}^{\mathrm{n}}$is:

$${\mathrm{v}}_{\mathrm{o}\mathrm{d}\mathrm{o}}^{\mathrm{n}}=\left(\begin{array}{c}I-\phi \times \end{array}\right){\mathrm{C}}_{\mathrm{b}}^{\mathrm{n}}\left(\begin{array}{c}I-\alpha \times \end{array}\right)\left(\begin{array}{c}1+\delta K\end{array}\right){\mathrm{v}}_{\mathrm{o}\mathrm{d}\mathrm{o}}^{\mathrm{b}}$$

The above formula yields the odometer error equation:

$$\mathsf{\delta }{\mathrm{v}}_{\mathrm{o}\mathrm{d}\mathrm{o}}^{\mathrm{n}}={\mathrm{v}}_{\mathrm{o}\mathrm{d}\mathrm{o}}^{\mathrm{n}}\times \phi +{\mathrm{v}}_{\mathrm{o}\mathrm{d}\mathrm{o}}^{\mathrm{m}}\left[\begin{array}{ccc}-{\mathrm{C}}_{13}& {\mathrm{C}}_{12}& {\mathrm{C}}_{11}\\ -{\mathrm{C}}_{23}& {\mathrm{C}}_{22}& {\mathrm{C}}_{21}\\ -{\mathrm{C}}_{33}& {\mathrm{C}}_{23}& {\mathrm{C}}_{31}\end{array}\right]{\left[\begin{array}{ccc}{\mathsf{\alpha }}_{\mathsf{\theta }}& \mathsf{\delta }\mathrm{K}& {\mathsf{\alpha }}_{\mathsf{\psi }}\end{array}\right]}^{\mathrm{T}}$$

Most existing studies model the scaling factor $\mathsf{\delta }\mathrm{K}$ is considered a constant. However, the research in references [1] reveals that the seabed sediment is deformable and the shear strength of the sediment varies with depth. This means $\mathsf{\delta }\mathrm{K}$ is actually a function that varies with the terrain and substrate parameters (such as internal friction angle and cohesion), rather than a constant value. If this is ignored in the model, significant nonlinear errors will occur when the substrate changes abruptly (such as from hard ground to soft mud). To this end, this paper refers to the real-time slip identification method proposed in reference [6], which will $\mathsf{\delta }\mathrm{K}$ as a first-order Markov process for estimation.

3.4. Analysis of Non-Integrity Constraints and Zero-Velocity Correction Principle

For an ideal underwater drilling robot, its motion is constrained by the contact with the strata and the structural configuration. In the body coordinate system only the forward direction is allowed to generate effective motion speed, while the lateral and vertical velocity components should theoretically be zero, that is, satisfying the non-integrity motion constraint.

$${\mathrm{v}}^{\mathrm{b}}={\left[\begin{array}{ccc}{\mathrm{v}}_{\mathrm{x}}^{\mathrm{b}}& {\mathrm{v}}_{\mathrm{y}}^{\mathrm{b}}& {\mathrm{v}}_{\mathrm{z}}^{\mathrm{b}}\end{array}\right]}^{\mathrm{T}}$$

where ${\mathrm{v}}_{\mathrm{x}}^{\mathrm{b}}=0$, ${\mathrm{v}}_{\mathrm{z}}^{\mathrm{b}}=0$. This constraint reflects the physical fact that the drilling rig cannot generate free slip velocity components in the lateral and vertical directions. Its essence belongs to the linear constraint on the velocity space of the system, rather than the geometric constraint on the pose. Therefore, it is typically classified as a non-holonomic constraint. In the inertial navigation system, due to the existence of accelerometer zero bias and attitude error, the lateral and vertical velocity errors often show an unbounded accumulation trend. By constructing the NHC constraint as a pseudo-observation and introducing it into the filter, the velocity drift of the inertial navigation in the non-primary motion direction can be effectively suppressed.

During the drilling robot’s stop drilling, attitude adjustment, or waiting for work, its actual motion state is close to stationary. At this time, the velocity in the navigation coordinate system should satisfy:

$${\mathrm{v}}^{\mathrm{n}}=0$$

Zero velocity correction is essentially to introduce high-confidence observations by utilizing the motion state switching characteristics of the system. By constructing measurement information with “zero velocity”, the velocity error of the inertial navigation is strongly constrained, and the accelerometer zero bias and attitude error are indirectly corrected. Non-holonomic constraints play a role in the continuous motion phase, while zero-velocity correction mainly takes effect in the stationary or quasi-stationary phase. Under complex seabed strata conditions, unifying the modeling of NHC, ZUPT, and odometry observations can form a multi-source measurement fusion form.

4. Inertial Navigation-Odometry Error Modeling and Measurement Equations Based on Nonholonomic Constraint Analysis and Zero-Velocity Update

Based on the construction of the single-system error propagation model, this chapter aims to solve the problem of unified description of state space in the process of multi-source information fusion. In view of the characteristics of “long-term passive” and “intermittent stop” faced by the seabed drilling robot in deep-sea operations, relying solely on the odometer assistance cannot completely eliminate inertial navigation drift. Therefore, this paper introduces the non-holonomic constraints (NHC) and zero velocity update (ZUPT) mechanism to construct a high-dimensional state space model containing sensor intrinsic parameter errors and motion state errors, and achieves effective suppression of system divergence by designing measurement equations based on kinematic constraints.

4.1. Construction of High-Dimensional Error State Space Model

To achieve optimal estimation of the integrated navigation system error within the Kalman filter framework, it is first necessary to establish a linearized state equation describing the evolution of system error. Considering the influence of the seabed environment on sensor accuracy and the time-varying characteristics of the odometry scaling factor, this paper extends the odometry calibration coefficient error to the traditional 15-dimensional SINS error state, constructing a 16-dimensional (or higher, depending on the specific dimension) error state vector$\mathrm{X}$：

$$\mathrm{X}(\mathrm{t})={\left[\begin{array}{cccccc}{\mathsf{\varphi }}^{\mathrm{T}}& \mathsf{\delta }{\mathrm{v}}^{\mathrm{T}}& \mathsf{\delta }{\mathrm{p}}^{\mathrm{T}}& {\mathsf{\epsilon }}^{{\mathrm{b}}^{\mathrm{T}}}& {\nabla }^{{\mathrm{b}}^{\mathrm{T}}}& \mathsf{\delta }\mathrm{K}\end{array}\right]}^{\mathrm{T}}$$

where:$\mathsf{\varphi }\in {\mathrm{R}}^3$ is the attitude misalignment angle;$\mathsf{\delta }{\mathrm{v}}^{\mathrm{n}},\mathsf{\delta }{\mathrm{p}}^{\mathrm{n}}\in {\mathrm{R}}^3$ are the velocity error and position error, respectively;${\mathsf{\epsilon }}^{\mathrm{b}},{\nabla }^{\mathrm{b}}\in {\mathrm{R}}^3$ represent the zero bias of the gyroscope and accelerometer, respectively, modeled as a first-order Gaussian-Markov process to describe its random drift characteristics;$\mathsf{\delta }\mathrm{K}\in {\mathrm{R}}^1$ is the odometry scaling factor error. Given the complexity of the mechanical properties of seabed sediments, it is regarded as a random walk process for real-time estimation.

Based on the error dynamics equation derived in Chapter 2, the continuous-time system state equation can be expressed as:

$$\dot{\mathrm{X}}\left(\mathrm{t}\right)=\mathrm{F}\left(\mathrm{t}\right)\mathrm{X}\left(\mathrm{t}\right)+\mathrm{G}\left(\mathrm{t}\right)\mathrm{W}\left(\mathrm{t}\right)$$

where,$\mathrm{F}\left(\mathrm{t}\right)\mathrm{i}\mathrm{s}16\times 16$ dimensional system state transition matrix, the specific form of which is jointly determined by the SINS error propagation characteristics and the sensor error model:

$$\begin{array}{c}F(t)=\left[\begin{array}{cccccc}{\mathrm{M}}_{\mathrm{a}\mathrm{a}}& {\mathrm{M}}_{\mathrm{a}\mathrm{v}}& {\mathrm{M}}_{\mathrm{a}\mathrm{p}}& -{\mathrm{C}}_{\mathrm{b}}^{\mathrm{n}}& 0_{3\times 3}& 0_{3\times 1}\\ {\mathrm{M}}_{\mathrm{v}\mathrm{a}}& {\mathrm{M}}_{\mathrm{v}\mathrm{v}}& {\mathrm{M}}_{\mathrm{v}\mathrm{p}}& 0_{3\times 3}& {\mathrm{C}}_{\mathrm{b}}^{\mathrm{n}}& 0_{3\times 1}\\ 0_{3\times 3}& {\mathrm{M}}_{\mathrm{p}\mathrm{v}}& {\mathrm{M}}_{\mathrm{p}\mathrm{p}}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ 0_{6\times 9}& & \mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}(-{\displaystyle \frac{1}{{\mathsf{\tau }}_{\mathrm{i}}}})& & 0_{6\times 1}& \\ 0_{1\times 15}& & & & 0& \end{array}\right]\end{array}$$

Here ${\mathrm{M}}_{\mathrm{x}\mathrm{y}}$ represents the corresponding error coupling sub-matrix,${\mathsf{\tau }}_{\mathrm{i}}$ is the relevant time constant of the inertial device.$\mathrm{W}\left(\mathrm{t}\right)$is the system noise vector containing the white noise of the gyroscope and accelerometer measurements. The construction of this model not only encompasses the error propagation of rigid body kinematics, but also provides a theoretical framework for solving the scaling failure problem of odometers in soft mud geology by augmenting the state variables.

4.2. Design of Measurement Equations Assisted by Kinematic Constraints

Compared to the terrestrial environment, deep-sea navigation lacks absolute position reference. Therefore, fully utilizing the kinematic characteristics of the drilling robot itself as virtual observation information is key to improving navigation robustness. Based on the robot’s operating modes, this paper constructs static constraint measurement based on ZUPT and dynamic constraint measurement based on NHC respectively.

4.2.1. Zero Velocity Correction (ZUPT) Measurement Model

When the drilling robot is in the bottom drilling, sampling or communication waiting stage, its chassis remains relatively stationary with the seabed. At this time, theoretically, the actual velocity under the navigation system should be zero. References [17] point out that by using this physical fact to construct “zero velocity observation”, the integral accumulation of velocity error can be effectively cut off, and the observation degree of IMU zero bias can be improved.

The ZUPT measurement equation is constructed as follows:

$${\mathrm{Z}}_{\mathrm{Z}\mathrm{U}\mathrm{P}\mathrm{T}}={\mathrm{v}}_{\mathrm{S}\mathrm{I}\mathrm{N}\mathrm{S}}^{\mathrm{n}}-0={\mathrm{H}}_{\mathrm{Z}\mathrm{U}\mathrm{P}\mathrm{T}}\mathrm{X}+{\mathrm{V}}_{\mathrm{Z}\mathrm{U}\mathrm{P}\mathrm{T}}$$

where, the measurement matrix ${\mathrm{H}}_{\mathrm{Z}\mathrm{U}\mathrm{P}\mathrm{T}}$ directly selects the velocity error term in the state vector:

$${\mathrm{H}}_{\mathrm{Z}\mathrm{U}\mathrm{P}\mathrm{T}}=\left[\begin{array}{ccc}{0}_{3\times 3}& {\mathrm{I}}_{3\times 3}& 0_{3\times 10}\end{array}\right]$$

Although the ZUPT principle is simple, under complex sea conditions, ocean current impact may cause the body to produce small vibrations. Therefore, it is necessary to combine robust algorithms such as chi-square test to dynamically manage the trigger threshold of ZUPT to prevent non-zero micro-movements from being misjudged as a stationary state.

4.2.2. Non-Integrity Constraint (NHC) Measurement Model

During the drilling robot’s maneuvering drilling process, it is assumed that its lateral slip and vertical jump are extremely weak, approximately satisfying the non-holonomic constraint conditions. That is, in the carrier coordinate system ($\mathrm{b}$system), the lateral velocity ${\mathrm{v}}_{\mathrm{x}}^{\mathrm{b}}$ and the vertical velocity ${\mathrm{v}}_{\mathrm{z}}^{\mathrm{b}}$ approach zero.

Based on this assumption, the NHC measurement equation is constructed. First, the velocity is calculated using SINS. ${\mathrm{v}}_{\mathrm{S}\mathrm{I}\mathrm{N}\mathrm{S}}^{\mathrm{n}}$ is projected onto the carrier coordinate system and compared with the virtual constraint value ${\left[0,{\mathrm{v}}_{\mathrm{y}},0\right]}^{\mathrm{T}}$:

$${\mathrm{Z}}_{\mathrm{N}\mathrm{H}\mathrm{C}}={\mathrm{C}}_{\mathrm{n}}^{\mathrm{b}}{\mathrm{v}}_{\mathrm{S}\mathrm{I}\mathrm{N}\mathrm{S}}^{\mathrm{n}}-\left[\begin{array}{c}0\\ {\mathrm{v}}_{\mathrm{o}\mathrm{d}\mathrm{o}}^{\mathrm{b}}\\ 0\end{array}\right]\approx \left[\begin{array}{c}\delta {\mathrm{v}}_{\mathrm{x}}^{\mathrm{b}}\\ \delta {\mathrm{v}}_{\mathrm{y}}^{\mathrm{b}}\\ \delta {\mathrm{v}}_{\mathrm{z}}^{\mathrm{b}}\end{array}\right]$$

Considering the influence of lever arm effect and installation error angle, the linearized NHC measurement matrix is derived.${\mathrm{H}}_{\mathrm{N}\mathrm{H}\mathrm{C}}$. In the derivation process, the coupling term of attitude error to velocity projection needs to be retained to ensure the consistency of filtering. After expansion, we get:

$${\mathrm{Z}}_{\mathrm{N}\mathrm{H}\mathrm{C}}={\mathrm{H}}_{\mathrm{N}\mathrm{H}\mathrm{C}}\mathrm{X}+{\mathrm{V}}_{\mathrm{N}\mathrm{H}\mathrm{C}}$$

$${\mathrm{H}}_{\mathrm{N}\mathrm{H}\mathrm{C}}=\left[\begin{array}{ccccc}({\mathrm{C}}_{\mathrm{n}}^{\mathrm{b}}{\mathrm{v}}^{\mathrm{n}}\times )& {\mathrm{C}}_{\mathrm{n}}^{\mathrm{b}}& 0_{3\times 3}& 0_{3\times 6}& {\mathrm{J}}_{\mathsf{\delta }\mathrm{K}}\end{array}\right]$$

where,$\left({\mathrm{C}}_{\mathrm{n}}^{\mathrm{b}}{\mathrm{v}}^{\mathrm{n}}\times \right)$ is the antisymmetric matrix formed by the velocity vectors, which reflects the influence of attitude error on velocity observation;${\mathrm{J}}_{\mathsf{\delta }\mathrm{K}}$ is the Jacobian term of odometer scale error.

Since the output of strapdown inertial navigation (SINS) contains the velocity and position information of the carrier, the obtained velocity and position calculation results and the SINS output value can be used to construct the observation vector of the integrated navigation system. The measurement equation is:

$$\mathrm{z}=\mathrm{H}\mathrm{x}+\mathrm{V}$$

where $\mathrm{H}=\left[\begin{array}{ccc}{\mathrm{H}}_{\mathrm{Z}\mathrm{U}\mathrm{P}\mathrm{T}}& {\mathrm{H}}_{\mathrm{N}\mathrm{H}\mathrm{C}1}& {\mathrm{H}}_{\mathrm{N}\mathrm{H}\mathrm{C}2}\end{array}\right]$.

Define vectors${\mathrm{E}}_1$、${\mathrm{E}}_2$ and${\mathrm{E}}_3$:

$${\mathrm{E}}_1=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]\text{，}{\mathrm{E}}_2=\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]\text{，}{\mathrm{E}}_3=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$$

as the observations constructed with zero velocity correction. The observation matrix obtained for zero-velocity correction is${\mathrm{H}}_{\mathrm{Z}\mathrm{U}\mathrm{P}\mathrm{T}}\mathrm{a}\mathrm{s}:$

$${\mathrm{H}}_{\mathrm{Z}\mathrm{U}\mathrm{P}\mathrm{T}}=\left[0_{3\mathrm{x}3}{\mathrm{I}}_{3\mathrm{x}3}{0}_{3\mathrm{x}12}\right]$$

Regarding the solution of NHC${\mathrm{H}}_{\mathrm{N}\mathrm{H}\mathrm{C}1}$and${\mathrm{H}}_{\mathrm{N}\mathrm{H}\mathrm{C}2}$. Define${\mathrm{A}}_{\mathrm{x},\mathrm{z}\mathrm{G}\mathrm{R}}\mathrm{a}\mathrm{s} \mathrm{t}\mathrm{h}\mathrm{e} \mathrm{d}\mathrm{a}\mathrm{t}\mathrm{a} \mathrm{f}\mathrm{r}\mathrm{o}\mathrm{m} \mathrm{t}\mathrm{h}\mathrm{e} \mathrm{g}\mathrm{y}\mathrm{r}\mathrm{o}\mathrm{s}\mathrm{c}\mathrm{o}\mathrm{p}\mathrm{e}{\mathsf{\omega }}_{\mathrm{e}\mathrm{b}}^{\mathrm{n}}$The acceleration on the xz plane of the carrier is obtained.${\mathrm{A}}_{\mathrm{y}\mathrm{A}\mathrm{C}}^{\mathrm{n}}$The acceleration component of the y-axis of the carrier system obtained by calculating the information from the triaxial accelerometer. ${\mathrm{A}}_{\mathrm{y}-\mathrm{o}\mathrm{d}\mathrm{o}}$The acceleration of the carrier in the y-axis direction measured by the odometer. The tangential acceleration and normal acceleration of the accelerometer and odometer are used as observations:

The first observation is obtained by subtracting the tangential acceleration:

$${\mathrm{Z}}_1={\tilde{\mathrm{A}}}_{\mathrm{y}\mathrm{A}\mathrm{C}}^{\mathrm{n}}-{\tilde{\mathrm{A}}}_{\mathrm{y}\mathrm{o}\mathrm{d}\mathrm{o}}^{\mathrm{n}}={\tilde{\mathrm{C}}}_{\mathrm{b}}^{\mathrm{n}}\left[\begin{array}{ccc}0& {\mathrm{E}}_2& 0\end{array}\right]{\tilde{\mathrm{C}}}_{\mathrm{n}}^{\mathrm{b}}{\tilde{\mathrm{F}}}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}-{\tilde{\mathrm{C}}}_{\mathrm{b}}^{\mathrm{n}}{\mathrm{E}}_2{\displaystyle \frac{\mathrm{d}{\tilde{\mathrm{V}}}_{\mathrm{o}\mathrm{d}\mathrm{o}}}{\mathrm{d}\mathrm{t}}}|\mathrm{b}$$

where ${\mathrm{F}}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}={\displaystyle \frac{\mathrm{d}{\mathrm{V}}_{\mathrm{e}\mathrm{n}}}{\mathrm{d}\mathrm{t}}}{|}_{\mathrm{b}}^{\mathrm{n}}+{\mathsf{\omega }}_{\mathrm{e}\mathrm{b}}^{\mathrm{n}}\times {\mathrm{V}}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}$，${\tilde{\mathrm{A}}}_{\mathrm{y}\mathrm{A}\mathrm{C}}^{\mathrm{n}},{\tilde{\mathrm{A}}}_{\mathrm{y}\mathrm{O}\mathrm{D}}^{\mathrm{n}},{\tilde{\mathrm{C}}}_{\mathrm{b}}^{\mathrm{n}},{\tilde{\mathrm{C}}}_{\mathrm{n}}^{\mathrm{b}},{\tilde{\mathrm{F}}}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}},{\tilde{\mathrm{V}}}_{\mathrm{O}\mathrm{D}}$“ is the corresponding quantity with error value in the actual experiment.

$${\tilde{\mathrm{C}}}_{\mathrm{b}}^{\mathrm{n}}=\left[\begin{array}{c}I-\left(\phi \times \right)\end{array}\right]{\mathrm{C}}_{\mathrm{b}}^{\mathrm{n}}$$

$${\tilde{\mathrm{C}}}_{\mathrm{n}}^{\mathrm{b}}={\mathrm{C}}_{\mathrm{n}}^{\mathrm{b}}\left[\mathrm{I}+\left(\phi \times \right)\right]$$

$${\tilde{\mathrm{F}}}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}=\left(\begin{array}{c}{\mathrm{F}}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}+{\tilde{\mathrm{C}}}_{\mathrm{b}}^{\mathrm{n}}{\mathsf{\sigma }}^{\mathrm{b}}\end{array}\right)$$

$${\tilde{\mathrm{v}}}_{\mathrm{o}\mathrm{d}\mathrm{o}}=\left(\begin{array}{c}1+\delta k\end{array}\right){\mathrm{v}}_{\mathrm{o}\mathrm{d}\mathrm{o}}$$

We can obtain:

$${\mathrm{H}}_{\mathrm{N}\mathrm{H}\mathrm{C}1}=\left[{\mathrm{H}}_{11} 0_{3\mathrm{x}3} 0_{3\mathrm{x}3} \mathrm{a}{\mathrm{C}}_{\mathrm{b}}^{\mathrm{n}} 0_{3\mathrm{x}3} -{\mathrm{C}}_{\mathrm{b}}^{\mathrm{n}}{\mathrm{E}}_2{\displaystyle \frac{\mathrm{d}{\mathrm{v}}_{\mathrm{o}\mathrm{d}\mathrm{o}}}{\mathrm{d}\mathrm{t}}}{|}_{\mathrm{b}}\right]$$

where $\mathrm{a}={\mathrm{C}}_{\mathrm{b}}^{\mathrm{n}}\left[0 {\mathrm{E}}_2 0\right]{\mathrm{C}}_{\mathrm{n}}^{\mathrm{b}}$，${\mathrm{H}}_{11}=-\mathrm{a}\left({\mathrm{F}}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}\times \right)$。

In the same way, the observation of the normal direction can be obtained:

$${\mathrm{Z}}_2={\tilde{\mathrm{A}}}_{\mathrm{x},\mathrm{z}\mathrm{A}\mathrm{C}}^{\mathrm{n}}-{\tilde{\mathrm{A}}}_{\mathrm{x},\mathrm{z}\mathrm{G}\mathrm{R}}^{\mathrm{n}}={\tilde{\mathrm{C}}}_{\mathrm{b}}^{\mathrm{n}}\left[\begin{array}{ccc}\mathrm{E}& 0& {\mathrm{E}}_3\end{array}\right]{\tilde{\mathrm{C}}}_{\mathrm{n}}^{\mathrm{b}}{\tilde{\mathrm{F}}}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}-{\tilde{\mathsf{\omega }}}_{\mathrm{e}\mathrm{b}}^{\mathrm{n}}\times {\tilde{\mathrm{V}}}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}$$

We can obtain:

$${\mathrm{H}}_{\mathrm{N}\mathrm{H}\mathrm{C}2}=\left[\begin{array}{cccccc}{\mathrm{H}}_{21}& -\left({\mathsf{\omega }}_{\mathrm{e}\mathrm{b}}^{\mathrm{n}}\times \right)& 0_{3\times 3}& \mathrm{b}{\mathrm{C}}_{\mathrm{b}}^{\mathrm{n}}& \left({\mathrm{V}}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}\times \right){\mathrm{C}}_{\mathrm{b}}^{\mathrm{n}}& 0\end{array}\right]$$

where $\mathrm{b}={\mathrm{C}}_{\mathrm{b}}^{\mathrm{n}}\left[{\mathrm{E}}_2 0 {\mathrm{E}}_3\right]{\mathrm{C}}_{\mathrm{n}}^{\mathrm{b}}$.

It is worth noting that the NHC constraint may fail under rugged seabed terrain or strong lateral current interference. Studies in references [18] how that if the lateral velocity is forcibly constrained to zero, it will introduce model bias. Therefore, the algorithm proposed in this paper does not simply adopt this rigid constraint, but rather adjusts the measurement noise covariance matrix adaptively by monitoring the residual statistical characteristics${\mathrm{R}}_{\mathrm{N}\mathrm{H}\mathrm{C}}$. When significant slippage or lateral displacement is detected, the measurement noise variance of the corresponding dimension is automatically increased to achieve a smooth transition from “hard constraints” to “soft constraints”.

5. Design of High-Precision Robust Navigation Algorithm

When the seabed drilling robot operates in the high-pressure environment of the deep sea, it faces the dual challenges of non-Gaussian noise interference and sensor observation anomalies. The traditional extended Kalman filter (EKF) often leads to accuracy divergence due to linearization truncation error, while the standard definite volume Kalman filter (CKF) is difficult to guarantee the positive definiteness of the covariance matrix in long-term operation [25,26,27,28]. In view of the above problems, this chapter proposes a navigation framework based on robust square root definite volume Kalman filter (RSRCKF). This algorithm enhances numerical stability by maintaining the square root form of the covariance matrix through QR decomposition and combines it with an anomaly detection mechanism based on composite cable encoder data to achieve adaptive suppression of non-line-of-sight errors and slippage disturbances.

5.1. Robust Square Root Volume Kalman Filter Theory

Time Update:

Assume $\mathrm{k}$ the posterior state estimate at time$\widehat{{\mathrm{x}}_{\mathrm{k}|\mathrm{k}}}$is${\mathrm{S}}_{\mathrm{k}|\mathrm{k}}$(i.e.,${\mathrm{P}}_{\mathrm{k}|\mathrm{k}={\mathrm{S}}_{\mathrm{k}|\mathrm{k}{\mathrm{S}}_{\mathrm{k}|{\mathrm{k}}^{\mathrm{T}}}}}$). Based on the third-order sphere diameter-volume criterion, generate $2\mathrm{n}$ Cubature Points:

$${\mathcal{X}}_{\mathrm{i},\mathrm{k}|\mathrm{k}}={\mathrm{S}}_{\mathrm{k}|\mathrm{k}}{\mathsf{\xi }}_{\mathrm{i}}+{\widehat{\mathrm{x}}}_{\mathrm{k}|\mathrm{k}},\mathrm{i}=\mathrm{1,2},\dots ,2\mathrm{n}$$

where,${\mathsf{\xi }}_{\mathrm{i}}$ is the basic volume point set, defined as ${\mathsf{\xi }}_{\mathrm{i}}=\sqrt{\mathrm{n}}{\left[\mathrm{I}\right]}_{\mathrm{i}}$ ($\mathrm{i}=1\dots \mathrm{n}$) and ${\mathsf{\xi }}_{\mathrm{i}}=-\sqrt{\mathrm{n}}{\left[\mathrm{I}\right]}_{\mathrm{i}-\mathrm{n}}$ ($\mathrm{i}=\mathrm{n}+1\dots 2\mathrm{n}$).

Substitute the volume points into the nonlinear state transition equation $\mathrm{f}\left(\cdot \right)$ for propagation:

$${\mathrm{X}}_{\mathrm{i},\mathrm{k}+1|\mathrm{k}}^{\mathrm{*}}=\mathrm{f}\left({\mathrm{X}}_{\mathrm{i},\mathrm{k}|\mathrm{k}}\right)$$

Calculate the one-step state prediction value $\widehat{{\mathrm{x}}_{\mathrm{k}+1|\mathrm{k}}}$ and the square root factor of the prediction covariance${\mathrm{S}}_{\mathrm{k}+1|\mathrm{k}}$. Numerical stability is guaranteed by QR decomposition and triangulation update operators:

$${\widehat{\mathrm{x}}}_{\mathrm{k}+1|\mathrm{k}}={\displaystyle \frac{1}{2\mathrm{n}}}\sum _{\mathrm{i}=1}^{2\mathrm{n}} {\mathcal{X}}_{\mathrm{i},\mathrm{k}+1|\mathrm{k}}^{\mathrm{*}}$$

The characteristic square root of the prediction error covariance matrix is:

$${\mathrm{S}}_{\mathrm{k}+1|\mathrm{k}}=\mathrm{T}\mathrm{r}\mathrm{i}\mathrm{a}\left(\left[{\mathcal{X}}_{1:2\mathrm{n},\mathrm{k}+1|\mathrm{k}}^{\mathrm{*}}-{\widehat{\mathrm{x}}}_{\mathrm{k}+1|\mathrm{k}},{\mathrm{S}}_{\mathrm{Q},\mathrm{k}}\right]\right)$$

where: ${\mathrm{S}}_{\mathrm{Q},\mathrm{k}}$is the square root factor of the process noise covariance matrix ${\mathrm{Q}}_{\mathrm{k}}$.

After passing through the measurement function h, the set of cubature points after propagation of the nonlinear measurement function is:

$${\mathrm{Z}}_{\mathrm{i},\mathrm{k}+1|\mathrm{k}}=\mathrm{h}\left({\mathrm{X}}_{\mathrm{i},\mathrm{k}+1|\mathrm{k}}\right)$$

At this time, the square root of the measurement prediction value, the innovation, and the innovation covariance matrix is:

$${\widehat{\mathrm{z}}}_{\mathrm{k}+1|\mathrm{k}}={\mathsf{\omega }}_{\mathrm{i}}\sum _{\mathrm{i}=1}^{2\mathrm{n}} {\mathrm{z}}_{\mathrm{i},\mathrm{k}+1|\mathrm{k}}$$

$$\left\{\begin{array}{l}{\mathsf{\eta }}_{\mathrm{k}+1}={\mathrm{Z}}_{\mathrm{k}+1}-{\widehat{\mathrm{Z}}}_{\mathrm{k}+1|\mathrm{k}}\\ {\mathrm{S}}_{\mathrm{Z}\mathrm{Z},\mathrm{k}+1|\mathrm{k}}=Tria\left(\left[\begin{array}{c}{\mathrm{D}}_{{\mathrm{Z}}_{\mathrm{k}+11\mathrm{k}}},{\mathrm{S}}_{\mathrm{R}}\end{array}\right]\right)\hspace{0.25em}\hspace{0.25em}\hspace{0.25em}\hspace{0.25em}\end{array}\right.$$

The cross-covariance matrix is:

$${\mathrm{P}}_{\mathrm{X}\mathrm{Z},\mathrm{k}+1|\mathrm{k}}={\mathrm{D}}_{{\mathrm{X}}_{\mathrm{k}+1|\mathrm{k}}}\ast {\mathrm{D}}_{{\mathrm{Z}}_{\mathrm{k}+1|\mathrm{k}}}^{\mathrm{T}}$$

At time k+1, the characteristic square roots of the error variance of the filter gain state estimate are:

$${\mathrm{K}}_{\mathrm{k}+1}={\displaystyle \frac{{\mathrm{P}}_{\mathrm{X}\mathrm{Z},\mathrm{k}+11\mathrm{k}}}{{\mathrm{S}}_{\mathrm{Z}\mathrm{Z},\mathrm{k}+11\mathrm{k}}{\mathrm{S}}_{\mathrm{Z}\mathrm{Z},\mathrm{k}+11\mathrm{k}}^{\mathrm{T}}}}$$

$${\mathrm{S}}_{\mathrm{k}+1|\mathrm{k}+1}=\mathrm{T}\mathrm{r}\mathrm{i}\mathrm{a}\left(\left[{\mathrm{D}}_{{\mathrm{X}}_{\mathrm{k}+1|\mathrm{k}}}-{\mathrm{K}}_{\mathrm{k}+1}{\mathrm{D}}_{{\mathrm{Z}}_{\mathrm{k}+1|\mathrm{k}}},{\mathrm{K}}_{\mathrm{k}+1}{\mathrm{S}}_{\mathrm{R}}\right]\right)$$

5.2. Odometry-Based Anomaly Detection and Adaptive Compensation Mechanism

The operation mode of the seabed drilling robot is unique: the robot initially penetrates the strata with the assistance of the seabed base station, and then drills autonomously by its own drive mechanism. During this process, the tail composite cable (carrying power supply, communication and traction functions) connecting the base station and the robot is released as the robot goes deeper. The encoder installed on the winch or cable port provides relative position (odometer) observation information by recording the cable release length.

However, in actual operation, the encoder data is easily affected by the environment and working conditions, mainly manifested as:

1. Cable slack or tension effect: Ocean current disturbance may cause nonlinear changes in the cable shape (such as the catenary effect), so that the “rope length” measured by the encoder is not equal to the robot’s “displacement”.

2. Drilling Slippage: When the drill bit encounters hard rock or the drilling torque is insufficient, the robot may “idle” or make small displacements. If the cable continues to be passively pulled out by the ocean current, false odometer increments will be generated.

The above anomalies will cause the observation innovation to violate the Gaussian white noise assumption. If a filter is directly introduced, the pollution state estimation will be affected. Therefore, this paper designs a fault detection and adaptive variance inflation mechanism based on the chi-square test.

Anomaly detection statistics construction: Define the standardized innovation vector${\mathsf{\nu }}_{\mathrm{k}+1}$. Using the measurement innovation ${\mathsf{\eta }}_{\mathrm{k}+1}={\mathrm{z}}_{\mathrm{k}+1}-\widehat{{\mathrm{z}}_{\mathrm{k}+1|\mathrm{k}}}$ and its theoretical covariance${\mathrm{P}}_{\mathrm{z}\mathrm{z},\mathrm{k}+1}={\mathrm{S}}_{\mathrm{z}\mathrm{z},\mathrm{k}+1{\mathrm{S}}_{\mathrm{z}\mathrm{z},\mathrm{k}+1}^{\mathrm{T}}}$, the squared Mahalanobis distance is constructed as the detection statistic ${\mathrm{M}}_{\mathrm{k}}^2$ ：

$${\mathrm{M}}_{\mathrm{k}}^2={\mathsf{\eta }}_{\mathrm{k}+1}^{\mathrm{T}}({\mathrm{P}}_{\mathrm{z}\mathrm{z},\mathrm{k}+1}{)}^{-1}{\mathsf{\eta }}_{\mathrm{k}+1}$$

Under normal operating conditions,${\mathrm{M}}_{\mathrm{k}}^2$ follows a chi-square distribution with $\mathrm{m}$ degrees of freedom (${\mathsf{\chi }}_{\mathrm{m}}^2$). Setting the significance level$\mathsf{\alpha }$(e.g., 0.05) and the corresponding decision threshold${\mathsf{\chi }}_{\mathrm{m},\mathsf{\alpha }}^2$。

Adaptive robust compensation: When ${\mathrm{M}}_{\mathrm{k}}^2>{\mathsf{\chi }}_{\mathrm{m},\mathsf{\alpha }}^2$ , it is determined that the current composite cable encoder data is abnormal (e.g., slippage or external disturbance). At this time, a robust adaptive factor ${\mathsf{\lambda }}_{\mathrm{k}}$ is introduced to expand the measurement noise covariance ${\mathrm{R}}_{\mathrm{k}}$ in real time to reduce the filtering gain of abnormal measurements at this moment.

Adaptive factors are constructed using a two-segment IGG weight function:

$${\mathsf{\lambda }}_{\mathrm{k}}=\left\{\begin{array}{l}1,\hspace{0.25em}\hspace{0.25em}\hspace{0.25em}\hspace{0.25em}{\mathrm{M}}_{\mathrm{k}}^2\le {\mathsf{\chi }}_{\mathrm{m},\mathsf{\alpha }}^2 (\mathrm{n}\mathrm{o}\mathrm{r}\mathrm{m}\mathrm{a}\mathrm{l})\\ {\displaystyle \frac{{\mathrm{M}}_{\mathrm{k}}^2}{{\mathsf{\chi }}_{\mathrm{m},\mathsf{\alpha }}^2}},\hspace{0.25em}\hspace{0.25em}\hspace{0.25em}\hspace{0.25em}{\mathrm{M}}_{\mathrm{k}}^2>{\mathsf{\chi }}_{\mathrm{m},\mathsf{\alpha }}^2 (\mathrm{a}\mathrm{b}\mathrm{n}\mathrm{o}\mathrm{r}\mathrm{m}\mathrm{a}\mathrm{l}, \mathrm{w}\mathrm{e}\mathrm{i}\mathrm{g}\mathrm{h}\mathrm{t}\mathrm{e}\mathrm{d} \mathrm{d}\mathrm{o}\mathrm{w}\mathrm{n}) \end{array}\right.$$

The corrected square root of the measurement noise covariance $\widetilde{{\mathrm{S}}_{\mathrm{R},\mathrm{k}}}$ is updated to:

$${\tilde{\mathrm{S}}}_{\mathrm{R},\mathrm{k}}=\sqrt{{\mathsf{\lambda }}_{\mathrm{k}}}\cdot {\mathrm{S}}_{\mathrm{R},\mathrm{k}}$$

by increasing $\mathrm{R}$ The filter will automatically “distrust” the current encoder measurements and rely more on the predictions of the inertial system, thus maintaining the smoothness and robustness of the trajectory during cable slippage or disturbances.

In summary, the pseudocode of the proposed robust square root capacitive Kalman filter algorithm is shown below:

The algorithm structure clearly demonstrates the complete logic from inertial prediction to robust correction, ensuring the robustness of the system under complex seabed drilling conditions.

Algorithm 1: Adaptive Robust Square-root Cubature Kalman Filter (AR-SRCKF)
Input:
$\mathrm{Initial} \mathrm{state}, \mathrm{error} \mathrm{covariance} \mathrm{square}\text{-}\mathrm{root} {\mathrm{S}}_0\in {\mathbb{R}}^{{\mathrm{n}}_{\mathrm{x}}\times {\mathrm{n}}_{\mathrm{x}}}$
$\mathrm{Process} \mathrm{noise} \mathrm{covariance} \mathrm{square}\text{-}\mathrm{root}{\mathrm{S}}_{\mathrm{Q}}\in {\mathbb{R}}^{{\mathrm{n}}_{\mathrm{x}}\times {\mathrm{n}}_{\mathrm{x}}}$
$\mathrm{Measurement} \mathrm{noise} \mathrm{covariance} \mathrm{square}\text{-}\mathrm{root}{\mathrm{S}}_{\mathrm{R}}\in {\mathbb{R}}^{{\mathrm{n}}_{\mathrm{z}}\times {\mathrm{n}}_{\mathrm{z}}}$
$\mathrm{Chi}\text{-}\mathrm{square} \mathrm{threshold} {\mathrm{T}}_{\mathrm{t}\mathrm{h}}$$(\mathrm{significance} \mathrm{level} \mathsf{\alpha }$)
Output:
$\mathrm{Posterior} \mathrm{state} \mathrm{estimate} {\widehat{\mathrm{x}}}_{\mathrm{k}|\mathrm{k}}$$\mathrm{and} \mathrm{covariance} \mathrm{square}\text{-}\mathrm{root} {\mathrm{S}}_{\mathrm{k}|\mathrm{k}}$
1	$\mathrm{Initialize}\text{:} {\mathrm{S}}_{0|0}={\mathrm{S}}_0,{\widehat{\mathrm{x}}}_{0|0}={\widehat{\mathrm{x}}}_0$
2	$\mathrm{For} \mathrm{k}=1$ $\mathrm{to} \mathrm{N}$ do
3	$\mathrm{Generate} \mathrm{cubature} \mathrm{points}{\mathcal{X}}_{\mathrm{i},\mathrm{k}-1|\mathrm{k}-1}\left(\mathrm{i}=1\dots 2{\mathrm{n}}_{\mathrm{x}}\right)$  $\mathrm{using} {\mathrm{S}}_{\mathrm{k}-1|\mathrm{k}-1}$
4	$\mathrm{Propagate} \mathrm{points} \mathrm{through} \mathrm{process} \mathrm{model}\text{:} {\mathcal{X}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{*}}=\mathrm{f}\left({\mathcal{X}}_{\mathrm{i},\mathrm{k}-1|\mathrm{k}-1}\right)$
5	$\$\backslash \mathrm{quad}\$ \mathrm{Predict} \mathrm{state}: {\widehat{\mathrm{x}}}_{\mathrm{k}|\mathrm{k}-1}={\displaystyle \frac{1}{2{\mathrm{n}}_{\mathrm{x}}}}\sum {\mathcal{X}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{*}}$
6	$\mathrm{Predict} \mathrm{covariance} \mathrm{factor} {\mathrm{S}}_{\mathrm{k}|\mathrm{k}-1}$$\mathrm{via} \mathrm{QR} \mathrm{decomposition}\text{:} {\mathrm{S}}_{\mathrm{k}|\mathrm{k}-1}=\mathrm{q}\mathrm{r}\left\{\right[{\mathcal{X}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{*}}-{\widehat{\mathrm{x}}}_{\mathrm{k}|\mathrm{k}-1},{\mathrm{S}}_{\mathrm{Q}}{]}^{\mathrm{T}}\}$
7	$\mathrm{Generate} \mathrm{prediction} \mathrm{cubature} \mathrm{points} {\mathcal{X}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}$$\mathrm{using} {\mathrm{S}}_{\mathrm{k}|\mathrm{k}-1}$
8	$\mathrm{Propagate} \mathrm{through} \mathrm{measurement} \mathrm{model}\text{:} {\mathcal{Z}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}=\mathrm{h}\left({\mathcal{X}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}\right)$
9	$\mathrm{Predict} \mathrm{measurement}\text{:} {\widehat{\mathrm{z}}}_{\mathrm{k}|\mathrm{k}-1}={\displaystyle \frac{1}{2{\mathrm{n}}_{\mathrm{x}}}}\sum {\mathcal{Z}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}$
10	$\mathrm{Compute} \mathrm{innovation}\text{:} {\mathsf{\eta }}_{\mathrm{k}}={\mathrm{z}}_{\mathrm{k}}-{\widehat{\mathrm{z}}}_{\mathrm{k}|\mathrm{k}-1}$
11	$\mathrm{Estimate} \mathrm{innovation} \mathrm{covariance} \mathrm{factor} {\mathrm{S}}_{\mathrm{z}\mathrm{z}}$$\mathrm{via} \mathrm{QR}\text{:} {\mathrm{S}}_{\mathrm{z}\mathrm{z}}=\mathrm{q}\mathrm{r}\left\{\right[{\mathcal{Z}}_{\mathrm{k}|\mathrm{k}-1}-{\widehat{\mathrm{z}}}_{\mathrm{k}|\mathrm{k}-1},{\mathrm{S}}_{\mathrm{R}}{]}^{\mathrm{T}}\}$
12	$\mathrm{Compute} \mathrm{Mahalanobis} \mathrm{distance} \mathrm{squared}\text{:} {\mathrm{M}}_{\mathrm{k}}^2={\mathsf{\eta }}_{\mathrm{k}}^{\mathrm{T}}({\mathrm{S}}_{\mathrm{z}\mathrm{z}}{\mathrm{S}}_{\mathrm{z}\mathrm{z}}^{\mathrm{T}}{)}^{-1}{\mathsf{\eta }}_{\mathrm{k}}$
13	$\mathrm{If} {\mathrm{M}}_{\mathrm{k}}^2>{\mathrm{T}}_{\mathrm{t}\mathrm{h}}$ then
14	$\mathrm{Calculate} \mathrm{adaptive} \mathrm{factor}\text{:} {\mathsf{\lambda }}_{\mathrm{k}}={\mathrm{M}}_{\mathrm{k}}^2/{\mathrm{T}}_{\mathrm{t}\mathrm{h}}$
15	$\mathrm{Inflate} \mathrm{noise} \mathrm{factor}\text{:} {\overline{\mathrm{S}}}_{\mathrm{R}}=\sqrt{{\mathsf{\lambda }}_{\mathrm{k}}}{\mathrm{S}}_{\mathrm{R}}$
16	$\mathrm{Re}\text{-}\mathrm{calculate} {\mathrm{s}}_{\mathrm{z}\mathrm{z}}$  $\mathrm{using} {\overline{\mathrm{S}}}_{\mathrm{R}}$
17	Else
18	${\overline{\mathrm{S}}}_{\mathrm{R}}={\mathrm{S}}_{\mathrm{R}}$
19	End If
20	$\mathrm{Calculate} \mathrm{cross}\text{-}\mathrm{covariance} {\mathrm{P}}_{\mathrm{x}\mathrm{z}}$ via centered matrices
21	$\mathrm{Compute} \mathrm{Kalman} \mathrm{Gain}\text{:} {\mathrm{K}}_{\mathrm{k}}={\mathrm{P}}_{\mathrm{x}\mathrm{z}}({\mathrm{S}}_{\mathrm{z}\mathrm{z}}{\mathrm{S}}_{\mathrm{z}\mathrm{z}}^{\mathrm{T}}{)}^{-1}$
22	$\mathrm{Update} \mathrm{state}\text{:} {\widehat{\mathrm{x}}}_{\mathrm{k}|\mathrm{k}}={\widehat{\mathrm{x}}}_{\mathrm{k}|\mathrm{k}-1}+{\mathrm{K}}_{\mathrm{k}}{\mathsf{\eta }}_{\mathrm{k}}$
23	$\mathrm{Update} \mathrm{covariance} \mathrm{factor} {\mathrm{S}}_{\mathrm{k}|\mathrm{k}}$ via QR update or downdate
24	End For

The algorithm structure clearly demonstrates the complete logic from inertial prediction to robust correction, ensuring the robustness of the system under complex seabed drilling conditions.

6. Simulation and Experimental Analysis

To comprehensively evaluate the navigation performance of the proposed robust square root commensurate Kalman filter (RSRCKF) algorithm in complex seabed geological environments, this paper designs two verification stages: numerical simulation and physical experiment. The simulation stage focuses on verifying the theoretical robustness of the algorithm under simulated slippage and non-Gaussian noise environments; the experimental stage is based on the independently developed seabed stratum drilling robot system, and field tests are carried out in the tidal flat area of the seaside wharf to verify the engineering applicability of the algorithm.

6.1. Numerical Simulation Analysis

To simulate the typical working conditions of the seabed drilling robot on the soft mud matrix, a high-fidelity simulation environment is constructed using MATLAB. Referring to relevant literature in the field of deep-sea navigation, the sensor parameters of the strapdown inertial navigation system (SINS) and the odometry (OD) are set as shown in Table 1. The initial position of the system is set to (30°N, 122°E), and the initial attitude error is set to [0.1°, 0.1°, 1°].

Design a 3000-second figure-eight motion trajectory combining straight lines to simulate the robot’s roving exploration and drilling operations on the seabed. To verify the algorithm’s ability to solve the core problem of “slippage,” an artificially introduced odometry scaling factor abrupt change interference was applied during the 1000-1200s interval of the simulation (simulating severe slippage of the robot in soft mud, with the scaling factor error suddenly increasing from 2% to 20%), and non-Gaussian measurement noise was introduced.

The following three algorithms are selected for comparative analysis:

EKF (Extended Kalman Filter): Traditional extended Kalman filter, as a benchmark algorithm, only has basic linearization processing capabilities.

SRCKF (Square-root Cubature Kalman Filter): Standard square root cubic Kalman filter, which has good nonlinear processing capabilities, but does not incorporate a robust mechanism.

Proposed RSRCKF: The robust square root capacitive Kalman filter based on chi-square test and adaptive factor proposed in this paper.

As can be seen from the comparison of the planar trajectories in Figure 3, during the normal driving phase from 0 to 1000 seconds, all three algorithms can effectively track the real trajectory. However, when entering the slip zone at 1000 seconds, due to the odometer output speed being much greater than the actual speed (slippage), the EKF algorithm (pink line) quickly diverges and completely deviates from the intended route; although the Standard SRCKF (yellow line) alleviates the nonlinear error to some extent with its volume rule, it is still inevitably contaminated by false observations due to the lack of anomaly detection mechanism, resulting in significant trajectory drift. In contrast, the RSRCKF (blue line) proposed in this paper monitors the residual Mahalanobis distance in real time.${\mathrm{M}}_{\mathrm{k}}^2$, accurately identify abnormal measurements in the slippage interval, and adaptively increase the measurement noise covariance${\mathrm{R}}_{\mathrm{k}}$, thereby reducing the weight of the odometer and always closely following the real trajectory (red solid line).

Figure 4 further illustrates the changes in position error over time in the East and North directions.

EKF: After slippage occurs, the error shows a linear divergence trend, with the maximum position error exceeding 25m.

SRCKF: Significant error accumulation occurs within the slippage zone. Although slippage ends after 1200s, the approximately 15m positional deviation cannot be eliminated.

RSRCKF: Throughout the slippage zone (gray shaded area), positional error fluctuations are minimal, with the maximum error controlled within 3.5m. This is attributed to the adjustment effect of the robustness factor${\mathsf{\lambda }}_{\mathrm{k}}$, which allows the filter to “even ignore” odometer data when observing anomalies, relying more on the short-term, high-precision calculation of SINS, thus verifying the algorithm’s strong robustness in soft mud geology.

To quantitatively evaluate the accuracy of each algorithm, the root mean square error (RMSE) of the entire process was statistically analyzed, and the results are shown in Figure 5. Statistical data shows that compared to the traditional EKF, RSRCKF improves horizontal positional accuracy by approximately 82.4%; compared to the standard SRCKF, the accuracy is improved by approximately 65.3%. Furthermore, in terms of attitude estimation, thanks to the numerical stability of the square root filter structure, the heading error of RSRCKF converges faster and has a smaller standard deviation.

6.2. Actual Test on Beach Pier and Mudflat

To verify the engineering effectiveness of the algorithm, an experimental environment was built in the tidal flat area of the Zhoushan wharf, relying on the “Natural Gas Hydrate Pilot Production Area Formation Space Drilling Monitoring System” project. This area is mainly composed of saturated soft clay and silt, and its mechanical properties are highly similar to those of shallow deep-sea sediments, which can realistically reproduce the working conditions and surrounding disturbances of the drilling robot.

The experimental subjects were a self-developed modular seabed drilling robot and a seabed base station (as shown in Figure 6). The robot’s body is approximately 2.5m long, 0.22m in diameter, and weighs approximately 150kg. The base station has dimensions of 2.5m x 2.5m x 3m. The navigation sensor unit includes: a self-developed high-precision MEMS inertial measurement unit (IMU), an encoder (as an odometer) installed on the seabed base station for monitoring the tail composite cable, and a high-precision depth sensor.

The experiment was conducted during low tide. The robot performed approximately 600 seconds of reciprocating motion and turning tests in the muddy tidal flat area. During the test, the robot transitioned from hard sand to soft mud, and the odometer data showed obvious pulse loss and idle phenomena. After clock synchronization, the collected data is post-processed and solved using SINS pure inertial navigation, SINS/OD standard combined navigation, and the proposed RSRCKF combined navigation algorithm.

Figure 7 shows the comparison between the calculated trajectory and the measured true trajectory. Due to the cumulative error of the IMU itself, the SINS pure inertial navigation calculation result drifted significantly after 200 seconds of operation. Although the standard SINS/OD combination suppressed the divergence, when the robot passed through the soft mud area (around T=300s), the trajectory deviated significantly from the true value because it failed to handle the spurious displacement caused by slippage. The RSRCKF algorithm proposed in this paper had the highest consistency with the measured true trajectory throughout the test. According to the statistical analysis of the data in the 10m×10m core test area, compared with the single odometer-assisted navigation without slippage compensation, the positioning error of the proposed method was reduced by about 83%, and the heading angle error remained stable within ±0.5°. The experimental results fully demonstrate that the navigation system can effectively identify and compensate for sensor anomalies in the soft mud environment, meeting the long-term high-precision positioning requirements of seabed drilling operations.

7. Discussion

This study aims to solve the navigation divergence problem caused by slippage and sensor anomalies in the soft seabed drilling robot. Numerical simulation and field tests on the Zhoushan mudflats verify the effectiveness of the proposed RSRCKF algorithm under complex working conditions. This section will explore the core advantages, potential limitations, and future improvement directions of the algorithm.

• Analysis of the slippage suppression mechanism under soft mud geology. The experimental results (Figure 3 and Figure 4) show that when the robot enters the soft mud area and experiences severe slippage (T=1000s-1200s), the positioning error of the traditional EKF and the standard SRCKF increases linearly or even divergently with time. This is because the odometer outputs false large displacement observations when slipping, and the standard filter mistakenly takes them as the robot’s real motion, thus forcibly correcting the SINS calculation results, causing the trajectory to “rush” out of the real path. In contrast, the RSRCKF algorithm proposed in this paper monitors the Mahalanobis distance of the innovation vector$\left({\mathrm{M}}_{\mathrm{k}}^2\right)$, can keenly capture the significant deviation between the observed data and the model prediction. When slippage occurs, ${\mathrm{M}}_{\mathrm{k}}^2$Breaking the Chi-square Threshold (${\mathsf{\chi }}_{\mathrm{m},\mathsf{\alpha }}^2$), triggering robustness mechanism. The algorithm uses a two-stage IGG weight function to rapidly increase the weight of the measurement noise covariance matrix (${\mathrm{R}}_{\mathrm{k}}$), which is equivalent to mathematically “downgrading” the reliability of the current odometer data, forcing the system to rely more on the short-time high-precision calculation of SINS and kinematic constraints (NHC/ZUPT). This adaptive switching from “hard fusion” to “soft isolation” is the core reason why the algorithm can still maintain meter-level positioning accuracy under strong slip conditions.
• The numerical stability advantage of square root filtering In a field test lasting up to 600 seconds (Figure 7), RSRCKF not only outperformed the comparison algorithm in terms of position accuracy, but also performed better in terms of convergence of heading angle. This is due to the fact that the algorithm adopts a square-root architecture. In long-term operations such as seabed drilling, the accumulation of computer rounding errors can easily lead to the loss of positive definiteness of the covariance matrix (especially when the state dimension is high), which in turn causes the filter to diverge. RSRCKF directly transmits the square root factor of the error covariance matrix (${\mathrm{S}}_{\mathrm{k}|\mathrm{k}}$), and uses QR decomposition and Cholesky update for state recursion, fundamentally ensuring the positive semidefiniteness and numerical stability of the covariance matrix. This is particularly important when dealing with state estimation in the deep-sea high dynamic and weak observation environment.
• Limitations and Future Prospects Although the method presented in this paper performs excellently in a single soft mud substrate, it still has certain limitations, guiding future research directions:

Handling extreme continuous slippage: The robust logic of this paper is based on the assumption that “SINS has high accuracy in short time”. If the robot encounters continuous slippage for tens of minutes (such as working in a large area of fluid plastic silt), the drift accumulation of SINS itself will not be effectively corrected over time. Future research can refer to references [29,30] to introduce a slippage prediction model assisted by deep learning, establish a nonlinear mapping between the bottom parameters and the slip rate, so as to achieve direct compensation of the odometry scaling factor, rather than simple weight reduction processing.

Multi-source heterogeneous fusion: At present, the system mainly relies on SINS/OD/Depth. In extremely turbid environments, although vision and DVL are limited, acoustic long baseline (LBL) or ultra-short baseline (USBL) can provide absolute position correction. The Factor Graph Optimization (FGO) framework proposed in references [31,32] has natural advantages in handling multi-rate, asynchronous sensor fusion and loop closure detection, and is a potential direction to replace the traditional Kalman filter architecture.

Refinement of constraints: The NHC constraints currently used in this paper are relatively basic. In the future, a generalized dynamic constraint including ocean current interference can be constructed by combining fluid dynamics models to further improve the adaptability of the navigation system in complex sea conditions.

In summary, RSRCKF provides an effective engineering solution for robust navigation of seabed drilling robots, but the evolution towards intelligence and full-source fusion is still the only way to achieve all-weather autonomous operation in the deep sea.