Path Planning Method for Underwater Gravity-Aided Inertial Navigation Based on PCRB

1. Introduction

Autonomous underwater vehicles (AUV) usually use Inertial Navigation System (INS) as the primary navigation device, but INS requires regular calibration to be able to perform long-term missions. In underwater situation, traditional navigation methods (e.g., GPS) are greatly limited due to the complexity and variability in the underwater environment[1,2,3]. In contrast, the Gravity-Aided Inertial Navigation System (GAINS) is an advanced technology used for underwater navigation that enables highly accurate position estimation without emitting or receiving signals. To achieve this objective, GAINS utilizes a specially designed navigation algorithm to compare the gravimeter’s measurements of gravity anomalies at the current position with the stored gravity field data to effectively correct the INS position.[4,5,6,7]. However, the variability of the gravity field significantly affects the performance of GAINS, making the selection of suitable navigation areas crucial[8,9]. In this regard, researchers have explored several quantitative characteristics such as variance, roughness, slope, coefficient of variation, fractal dimension, and their combinations to determine the efficiency of using gravity fields for navigation[10,11,12]. The navigation map is then divided into two categories : informative (suitable for positioning) and non-informative (not suitable for positioning) based on empirical thresholds[13]. Informative areas are free to move around, while non-informative areas are to be avoided. And Then, intelligent optimization algorithms like genetic algorithms, simulated annealing algorithms, ant colony algorithms, and particle swarm algorithms, etc. are employed to optimize the underwater path based on the position and motion information of underwater objects[14,15,16,17,18]. Those approaches ensure optimal obstacle avoidance and the shortest possible paths for efficient navigation.

Previous path planning algorithms for GAINS often assumed that non-informative areas were off-limits, leading to significant navigation errors. This assumption, however, may not always hold true. Unsuitable places do not necessarily equate to restricted areas, for example, a flat area unsuitable for navigation but without real obstacles. Furthermore, research suggests that areas suitable for positioning based on local characteristics are typically discrete and small, making it difficult to find connected informative areas [13,19,20]. As a result, path planning using this approach is impossible in arbitrary sea areas, particularly when non-informative areas surround the start or endpoints. Therefore, the main objective of this study is to minimize the positioning error between departure and destination, rather than focusing on factors such as avoiding islands or maintaining motion control.

The performance of path planning is closely related to the navigation algorithm. Therefore, in this paper, the optimal paths will be considered by the results of the navigation filtering algorithm. The posterior Cramér-Rao bound (PCRB) integrates information from kinetics and measurement models; therefore, it comprises all sensor-obtained gravity field information without the need for actually implementing the filtering algorithm[21,22,23,24,25,26]. A* algorithm is one of the most common heuristic search algorithms, swiftly investigating a possible set of solutions for a given issue using heuristic search techniques, focusing more on the end answer than on sub-problems, which enables it to produce pathways fast and with improved results[27,28]. This study uses PCRB as the cost function of A *, precisely fusing the path planning and navigation filter estimation results. The characteristics of the gravity fields at departure and destination are not limitations on the proposed path-planning method. This study doesn’t add distance directly to the cost or heuristic function like most shortest path trajectory designs do. Instead, the distance factor is included in the INS error divergence variance. Hence, this method can plan the quickest path while maintaining positioning accuracy.

The following arrangement of this paper is as follows. Section II derives the PCRB model of GAINS, and Section III combines PCRB with A* to form the PCRB-based A* path planning method. In Section IV, two simulation tests are done to see if the two planned paths are the best ones. Finally, conclusions are given in Section V.

2. PCRB for Gravity-Aided Navigation System

In most cases, GAINS is modeled as a hidden Markov model (HMM), as shown in Figure 1, i.e., the observation depends only on the state of the Markov chain at that moment, independent of the other states; the current moment state is related only to the previous moment.

The mathematical model of GAINS can be reduced to the following two discrete-time equations.The first equation describes the evolution of the state vector, i.e., the INS error propagation process

$${\mathbf{x}}_{t+1}={\mathbf{x}}_t+{\mathbf{u}}_t+{\mathbf{v}}_t$$

(1)

where ${\mathbf{x}}_t$ is the state vector indicating the carrier position. ${\mathbf{u}}_t$ represents the state increment of INS. ${\mathbf{v}}_t$ represents the state noise from IMU measurement error, Gaussian distribution, and with covariance matrix ${\mathbf{Q}}_t$.

The second equation describes the observed values of the gravity sensor and the comparison with the reference map values by interpolation

$$y_t=h\left({\mathbf{x}}_t\right)+{\epsilon }_t\left({\mathbf{x}}_t\right)+w_t$$

(2)

where $y_t$ is the measurement of the gravimeter at time t, and h denotes the interpolation method of the gravity sensors model combined with the reference data map, usually sampling bilinear or Kriging interpolation. $w_t$ denotes the measurement noise of the geophysical sensor, which is assumed to be additive Gaussian white noise. ${\epsilon }_t$ denotes the reference data mapping error. Considering this error covariance depending on the location, the variance matrix of the total observation error modeled as (3) [29].

$$R_t=\frac{{\sigma }_m^2+C_0{\beta }^2{\delta }^4}{4}+{\sigma }_g^2$$

(3)

where $R_t$ includes a combination of sensor measurements and map uncertainty. $\delta$ is the map spatial resolution, ${\sigma }_m^2$ is the map mapping error variance. $C_0$ is the variance of the local gravity anomaly. $\beta$ is the inverse of the square of the correlation distance, and ${\sigma }_g^2$ is the gravimeter measurement error variance.

Under the assumption of additive Gaussian white noise, $v_t$ and $w_t$, are assumed to be independent of each other, and independent of $x_0$. (1), (2) explicitly determine the joint probability distribution $p({\mathbb{X}}_t,{\mathbb{Y}}_t)$ of ${\mathbb{X}}_t(x_0,\cdots ,x_t)$ and ${\mathbb{Y}}_t(y_0,\cdots ,y_t)$ at any moment t with a known $p\left(x_0\right)$.

$$\begin{array}{cc}\hfill p({\mathbb{X}}_{t+1},{\mathbb{Y}}_{t+1})=& p({\mathbb{X}}_t,{\mathbb{Y}}_t)·p\left(x_{t+1}\mid {\mathbb{X}}_t,{\mathbb{Y}}_t\right)·p\left(y_{t+1}\mid x_{t+1},{\mathbb{X}}_t,{\mathbb{Y}}_t\right)\hfill \\ \hfill =& p({\mathbb{X}}_t,{\mathbb{Y}}_t)·p\left(x_{t+1}\mid x_t\right)·p\left(y_{t+1}\mid x_{t+1}\right)\hfill \end{array}$$

(4)

where $p(·)$ refers to the probability density of the variables described in the parameters of p. The derivation of (4) makes use of the Markov property of the model i.e. $p\left(x_{t+1}\mid {\mathbb{X}}_t,{\mathbb{Y}}_t\right)=p\left(x_{t+1}\mid x_t\right)$, and $p\left(y_{t+1}\mid x_{t+1},{\mathbb{X}}_t,{\mathbb{Y}}_t\right)=p\left(y_{t+1}\mid x_{t+1}\right)$.

PCRB is a lower bound on the mean squared error (MSE) of a deterministic parameter estimate in parameter estimation, defined as the variance of any unbiased estimate being at least larger than the inverse of the Fisher information. Whenever the estimate ${\widehat{\mathbb{X}}}_t$ is based on the sequence ${\mathbb{Y}}_t$, the mean square error for any unbiased estimator ${\widehat{\mathbb{X}}}_t$ should satisfy the following condition.

$$\begin{array}{c}\hfill \mathrm{E}\left(\left({\widehat{\mathbb{X}}}_t-{\mathbb{X}}_t\right){\left({\widehat{\mathbb{X}}}_t-{\mathbb{X}}_t\right)}^T\right)\ge {\mathbb{P}}_t=J_t^{-1}\end{array}$$

(5)

where $J_t$ is the Fisher information matrix.

$$\begin{array}{c}\hfill J_t=\mathrm{E}\left(-{\Delta }_{{\mathbb{X}}_t}^{{\mathbb{X}}_t}logp\left({\mathbb{Y}}_t,{\mathbb{X}}_t\right)\right)\end{array}$$

(6)

where $logp\left({\mathbb{Y}}_t,{\mathbb{X}}_t\right)$ denotes the joint probability density of ${\mathbb{X}}_t$ and ${\mathbb{Y}}_t$. Here and below, ∇ is the operator of the first order derivative, and $\Delta$ is the operator of the second order derivative.

Let $P_{t|t}$ denote the PCRB of the state $x_t$ determined for a given measurement ${\mathbb{Y}}_t$. By decomposing the lower bound (5) into subblocks, the estimated covariance of $x_t$ is lower bounded by the lower right block of ${\mathbb{P}}_t$, i.e.

$$\begin{array}{c}\hfill \mathrm{E}\left(\left[\begin{array}{c}{\widehat{\mathbb{X}}}_{t-1}-{\mathbb{X}}_{t-1}\\ {\widehat{x}}_t-x_t\end{array}\right]{\left[\begin{array}{c}{\widehat{\mathbb{X}}}_{t-1}-{\mathbb{X}}_{t-1}\\ {\widehat{x}}_t-x_t\end{array}\right]}^T\right)\ge \left[\begin{array}{cc}*& *\\ & P_{t|t}\end{array}\right]\end{array}$$

(7)

Correspondingly,

$$\begin{array}{c}\hfill J_t=\mathrm{E}\left\{\left[\begin{array}{cc}-{\Delta }_{{\mathbb{X}}_{t-1}}^{{\mathbb{X}}_{t-1}}\hfill & -{\Delta }_{{\mathbb{X}}_{t-1}}^{x_t}\hfill \\ -{\Delta }_{x_t}^{{\mathbb{X}}_{t-1}}\hfill & -{\Delta }_{x_t}^{x_t}\hfill \end{array}\right]logp\left({\mathbb{Y}}_t,{\mathbb{X}}_t\right)\right\}=\left[\begin{array}{cc}{A}_t& B_t\\ B_t^T& C_t\end{array}\right]\end{array}$$

(8)

From (8) it follows that

$$P_{t|t}^{-1}=C_t-B_t^T{A}_t^{-1}{B}_t$$

(9)

Insert (4) into (8) to get the recursive update

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathbb{J}}_{t+1}& =\mathrm{E}\left(-\left[\begin{array}{ccc}{\Delta }_{{\mathbb{X}}_{t-1}}^{{\mathbb{X}}_{t-1}}& {\Delta }_{{\mathbb{X}}_{t-1}}^{x_t}& {\Delta }_{{\mathbb{X}}_{t-1}}^{x_{t+1}}\\ {\Delta }_{x_t}^{{\mathbb{X}}_{t-1}}& {\Delta }_{x_t}^{x_t}& {\Delta }_{x_t}^{x_{t+1}}\\ {\Delta }_{x_{t+1}}^{{\mathbb{X}}_{t-1}}& {\Delta }_{x_{t+1}}^{x_t}& {\Delta }_{x_{t+1}}^{x_{t+1}}\end{array}\right]logp\left({\mathbb{Y}}_{t+1},{\mathbb{X}}_{t+1}\right)\right)=\left[\begin{array}{ccc}{A}_t& B_t& \mathbf{0}\\ B_t^T& C_t+D_t& S_t\\ \mathbf{0}& S_t^T& Z_t\end{array}\right]\hfill \end{array}\end{array}$$

(10)

where $\mathbf{0}$ denotes the block matrix of zero entries of appropriate dimensionality.

Comparing with (9), it follows that

$$\begin{array}{cc}\hfill P_{t+1|t+1}^{-1}& =Q_t-\left[\begin{array}{cc}\mathbf{0}\hfill & S_t^T\hfill \end{array}\right]{\left[\begin{array}{cc}{A}_t& B_t\\ B_t^T& C_t+D_t\end{array}\right]}^{-1}\left[\begin{array}{c}\mathbf{0}\\ S_t\end{array}\right]\hfill \\ \hfill & =Z_t-S_t^T{\left(P_{t|t}^{-1}+D_t\right)}^{-1}{S}_t\hfill \end{array}$$

(11)

Under the assumption that the noise $w_n$ and $v_n$ of (1) and (2) are zero-mean Gaussian and invertible covariance matrices, $Q_t$ and $R_t$, respectively, therefore

$$\begin{array}{cc}\hfill -logp\left(x_{t+1}\mid x_t\right)& =c_1+\frac{1}{2}{\left[x_{t+1}-x_t\right]}^T{Q}_t^{-1}\left[x_{t+1}-x_t\right]\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill -logp\left(y_t\mid x_t\right)& =c_2+\frac{1}{2}{\left[y_t-h\left(x_t\right)\right]}^T{R}_t^{-1}\left[y_t-h\left(x_t\right)\right]\hfill \end{array}$$

(13)

where $c_1$ and $c_2$ are constants. Thus, in the equation (11),

$$\begin{array}{cc}\hfill D_t=& \mathrm{E}\left(-{\Delta }_{x_t}^{x_t}logp\left(x_{t+1}\mid x_t\right)\right)=Q_t^{-1}\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill S_t=& \mathrm{E}\left(-{\Delta }_{x_t}^{x_{t+1}}logp\left(x_{t+1}\mid x_t\right)\right)=-Q_t^{-1}\hfill \\ \hfill Z_t=& \mathrm{E}\left(-{\Delta }_{x_{t+1}}^{x_{t+1}}logp\left(x_{t+1}\mid x_t\right)\right)+\mathrm{E}\left(-{\Delta }_{x_{t+1}}^{x_{t+1}}logp\left(y_{t+1}\mid x_{t+1}\right)\right)\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill =& Q_t^{-1}+H_{t+1}^T{R}_{t+1}^{-1}{H}_{t+1}\hfill \end{array}$$

(16)

where $H_t=\nabla h\left(x_t\right)$ and $H_t$ is the gradient of $h(·)$ at the true position at time t. In the above derivation, the expectation of both the measurement noise and the position is 0, so the expectation $E\left(•\right)$ is computed only at the current true position.

Bringing the equation (14)-() into the equation (11) gives

$$\begin{array}{cc}\hfill P_{t+1|t+1}& ={\left[H_{t+1}^T{R}_{t+1}^{-1}{H}_{t+1}+Q_t^{-1}-Q_t^{-1}{\left(P_{t|t}^{-1}+Q_t^{-1}\right)}^{-1}{Q}_t^{-1}\right]}^{-1}\hfill \\ \hfill & ={\left[H_{t+1}^T{R}_{t+1}^{-1}{H}_{t+1}+{\left[Q_t+P_{t|t}\right]}^{-1}\right]}^{-1}\hfill \end{array}$$

(17)

The derivation of (17) makes use of the Woodbury matrix identity[30].

$$\begin{array}{c}\hfill {(A+UCV)}^{-1}=A^{-1}-A^{-1}U{\left(C^{-1}+V{A}^{-1}U\right)}^{-1}V{A}^{-1}\end{array}$$

(18)

$H_t^T{R}_t^{-1}{H}_t$ in (17) is only related to the position and represents the gravitational positioning information obtained from the map, and the square root of the trace of this matrix is used as the scalar navigation information map.

$$M_t=\sqrt{tr\left\{H_t^T{R}_t^{-1}{H}_t\right\}}$$

(19)

A further expansion of equation (17) using the Woodbury matrix identity gives a more interesting form

$$\begin{array}{cc}\hfill P_{t+1|t}& =P_{t|t}+Q_t\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill P_{t+1|t+1}& =P_{t+1|t}-P_{t+1|t}{H}_{t+1}^T{\left(H_{t+1}{P}_{t+1|t}{H}_{t+1}^T+R_{t+1}\right)}^{-1}{H}_{t+1}{P}_{t+1|t}\hfill \end{array}$$

(21)

(20) and () constitute the recursive form of the PCRB for the state estimation in GAINS, where (20) is the one-step prediction covariance and () is the posterior filter covariance. The recursively estimated PCRB is consistent with the error covariance Riccati recursion of the Kalman filter to the system by linearizing the model around the true state sequence.

This also indicates that the Kalman filter can obtain the optimal solution under the assumption of linear Gaussian white noise, and its state covariance can reach the PCRB.

3. A* Algorithm Path Planning Design Based on PCRB

The A* algorithm is a heuristic search algorithm that is recursive in nature and can follow certain steps, starting from the original state and gradually searching to the optimal solution. The A* algorithm uses the overhead G between nodes in the graph, and a heuristic function H related to the current task to find the optimal path.

$$F\left(n\right)=G\left(n\right)+H\left(n\right)$$

(22)

Where, $F\left(n\right)$ is the valuation function of a node, which indicates the combined priority of that node considered in the selection of node n. $G\left(n\right)$ denotes the actual generation value from the starting point to the current node. $H\left(n\right)$ denotes the cost estimate of the current node to the target point, which is the prediction function.

The computation in PCRB is in the time domain and the A* algorithm works in the spatial domain. To facilitate the combination of PCRB with the A* algorithm, the coordinates at t time are assumed to be n. Correspondingly, the estimated position PCRB in n coordinates is $P_{n|n}$. The INS position error dispersion is assumed to be time-dependent only, and the coefficient of linear drift of the position error variance with time is $Q_t$. Denote the Euclidean distance from the current point $(x_c,x_f)$ to the parent node $(x_f,x_f)$ by $d_f$, and denote the distance from the current point to the target node by $d_g$ to denote the Euclidean distance from the current point to the target node.

$$\begin{array}{cc}\hfill d_f& =\sqrt{{(x_c-x_f)}^2+{(y_c-y_f)}^2}\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill d_g& =\sqrt{{(x_c-x_g)}^2+{(y_c-y_g)}^2}\hfill \end{array}$$

(24)

The position error uncertainty of INS grows linearly over a certain time period, and the relationship between $Q_n$ and distance is as follows.

$$Q_n=Q_t·d_f/V\left(n\right)$$

(25)

where $V\left(n\right)$ is the average velocity of the carrier during the sampling interval.

The main goal of this paper is to identify the path that minimizes the total posterior filtering errors of navigation at each current waypoint. We accomplish this by using the PCRB $P_{n|n}$ at the present position n as the cost function $G\left(n\right)$ for the current node in the A algorithm.

$$\begin{array}{cc}\hfill P_{n+1|n}& =P_{n|n}+Q_n\hfill \end{array}$$

(26)

$$\begin{array}{cc}\hfill P_{n+1|n+1}& =P_{n+1|n}-P_{n+1|n}{H}_{n+1}^n{\left(H_{n+1}{P}_{n+1|n}{H}_{n+1}^T+R_{n+1}\right)}^{-1}{H}_{n+1}{P}_{n+1|n}\hfill \end{array}$$

(27)

(26) represents the navigation error covariance of the one-step prediction, and () represents the correction of the one-step prediction by the posterior information from the reweighted force measurement to obtain the PCRB. Obviously the $P_{n|n}$ matrix is a square matrix whose diagonal elements represent the error variance in two orthogonal directions, X direction (eastward) and Y direction (northward). In this paper, the sum of the traces of the PCRB of the localization error at each point on the trajectory is used as a metric to quantify the navigation accuracy of this path, i.e.

$$G(n+1)=G\left(n\right)+\sqrt{tr\left\{P_{n+1|n+1}\right\}}$$

(28)

where $tr\{•\}$ means trace operation.

The heuristic function is the expected navigation error for the increase of the current position to the target position. Based on the fact that the INS position error can be considered as linearly divergent in the short term, the heuristic function is set to

$$H\left(n\right)=\sqrt{tr\{Q_t·d_g/V\left(n\right)\}}$$

(29)

Assuming that the current node is n and the actual cost is $G\left(n\right)$, the complete computation steps of $F(n+1)$ for its child node $n+1$ are as follows.

1)

Calculate the distance $d_g$ from the child node to the target node according to equation (), and bring in equation (29) to calculate the estimated value $H(n+1)$.

2)

The distance $d_f$ from the child node to the parent node is calculated according to equation (23) and brought into equation (25) to get $Q_n$.

3)

Substitute $Q_n$ into equation (26) to get $P_{n+1|n+1}$ then bring (28) to calculate $G(n+1)$ of the child nodes.

4)

Combining the obtained $G(n+1)$ and $H(n+1)$,

$$F(n+1)=G(n+1)+H(n+1)$$

(30)

The A* algorithm controls the points in the map by setting the open list and close list. The pseudo code table of PCRB-based A* path planning algorithm is as follows:

4. Simulation

The numerical gravity map of the simulation shown in Figure 2 is derived from the global ocean gravity field $1^{\prime }\times 1^{\prime }$ model produced by Sandwell’s team [31], and the accuracy of the gravity field of the selected experimental region is better than $1.7mGal$. The maximum gravity anomaly in the region is $40.8mGal$ and the minimum is $-37.2mGal$. The gravity anomaly is highly undulating and contains various topographic features such as peaks, slopes, and flats, which are suitable for path planning analysis. Since the actual horizontal distances of longitude and $1^{\prime }$ latitude of $1^{\prime }$ are different, in order to unify the navigation errors, this paper converts the earth coordinates into geographic coordinates centered on the map center position through equation (31) and equation ()

$$\begin{array}{cc}\hfill d_E& =({\lambda }_1-{\lambda }_0)cos\left({\phi }_0\right)R_n\pi /180\hfill \end{array}$$

(31)

$$\begin{array}{cc}\hfill d_N& =({\phi }_1-{\phi }_0)R_m\pi /180\hfill \end{array}$$

(32)

where $d_E$ is the eastward distance from the center of the map and $d_N$ is the northward distance from the center of the map. ${\phi }_0$ is the latitude of the coordinate origin. Earth’s radius (long semi-axis) $Re=6378137m$. Oblateness of the earth $f=1/298.3$. The radius of principal curvature of the meridian circle $R_m=R_e(1-2f+3f{(sin\left({\phi }_0\right))}^2)$. The radius of the Earth’s circle of latitude $R_n=R_e(1+f{(sin\left({\phi }_0\right))}^2)$. And then the origin is shifted to the lower left corner of the map to get the Figure 2, and the spatial resolution of the map after interpolation is $1km$.

The measure of trajectory performance uses the Root Mean Square Error (RMSE), which is the square root of the ratio of the square of the deviation of the predicted value from the true value to the number of observations n.

$$\mathrm{RMSE}=\sqrt{\frac{1}{N}\sum _{i=1}^n\left(\left\{{\widehat{\mathbf{x}}}_i\right\}-\left\{{\mathbf{x}}_i\right\}\right)}$$

(33)

where $\left\{\widehat{{\mathbf{x}}_i}\right\}$ is the estimated value of the trajectory heading coordinates, $\left\{{\mathbf{x}}_i\right\}$ is the true value of the trajectory heading coordinates, and N is the number of Monte Carlo experiments.

The estimated coordinates $\widehat{\mathbf{x}}$ are calculated using the SITAN algorithm, which is essentially an extended Kalman filter algorithm. The state error propagation is

$$\Delta {\mathbf{x}}_{t+1}=\Delta {\mathbf{x}}_t+{\mathbf{v}}_t$$

(34)

where $\Delta {\mathbf{x}}_t$ is the position error of INS, i.e. $\widehat{{\mathbf{x}}_t}={\mathbf{x}}_{INS}-\Delta {\mathbf{x}}_t$.

SITAN fits a linear observation model using stochastic linearization techniques

$${\mathbf{z}}_t=h\left({\mathbf{x}}_t\right)-{\mathbf{y}}_t\approx H\Delta {\mathbf{x}}_t+\epsilon$$

(35)

The fitting expression for $H=[h_{\lambda },h_{\phi }]$ in the observed model is

$$\begin{array}{cc}\hfill h_{\lambda }& =\frac{1}{L(2L+1)(L+1)\delta }\left(\sum _{\begin{array}{c}n=i-L\\ m=j-L\end{array}}^{\begin{array}{c}n=i-1\\ m=j+L\end{array}}\Delta g_M(n,m)-\sum _{\begin{array}{c}n=i+1\\ m=j-L\end{array}}^{\begin{array}{c}n=i+L\\ m=j+L\end{array}}\Delta g_m(n,m)\right)\hfill \end{array}$$

(36)

$$\begin{array}{cc}\hfill h_{\phi }& =\frac{1}{L(2L+1)(L+1)\delta }\left(\sum _{\begin{array}{c}n=i-L\\ m=j-L\end{array}}^{\begin{array}{c}n=i+L\\ m=j-1\end{array}}\Delta g_M(n,m)-\sum _{\begin{array}{c}n=i-L\\ m=j+1\end{array}}^{\begin{array}{c}n=i+L\\ m=j+L\end{array}}\Delta g_m(n,m)\right)\hfill \end{array}$$

(37)

(36) and () describe the process of calculating the observation matrix H for gravity anomaly data. The variables used are: $(n,m)$ which represents the grid of the INS indicator position on the digital gravity map; $\Delta g_M=h\left({\mathbf{x}}_t\right)$ which is the gravity anomaly extracted from the digital map based on the INS indicated position; $\Delta g_m=y_t$ which is the gravity anomaly measured in real-time; $\delta$ which is the spacial resolution of the digital map; and L which represents the number of grid points in the length of the fitted interval.

The center point of the fitted region is the INS position, and the size of the region is determined by the position error covariance of INS. To calculate the observation matrix H, gravity anomaly values of all grid points in the region are used. Generally, a more accurate INS indicator position leads to a smaller INS confidence interval, resulting in a more precise linearized observation model.

4.1. Test 1

The starting point (start1) of Test 1 in the planned path is located at $(58,73)$ in Figure 2. The point is located within the slope area, where contour lines are densely distributed. The ending point, on the other hand, has coordinates of $(65,9)$. Table 1 lists the simulation conditions.

Figure 3 displays the contours of Mn, calculated from equation (19). A comparison between this and the gravity anomaly contours in Figure 2 reveals that regions with denser contours correspond to larger Mn. This suggests that more gravity information is available in such areas.

Following the proposed method, the planned path intersects the region with maximum information. To determine the optimality of the planned path, we established several equally spaced curves for comparison, including the shortest Direct path and seven others labelled L3, L4, ... ,L9, from left to right. The paths are shown in Figure 3. We conducted 1000 random simulations using the Monte Carlo method and identical parameters as those outlined in Table 1. Each trajectory was divided into 100 sampling points by equal Euclidean distances.

Figure 4 provides a comparison of the localization accuracy achieved using different trajectories in Test 1. As can be seen from Figure 4, the RMSE of all paths decreases rapidly during the initial stage since the starting point 1 is located in a region with high Mn values. However, the planned trajectory outperforms all other paths in terms of localization accuracy.

The black curve represents the planned path, while the gray curve denotes the straight-line path. Although the planned trajectory covers approximately $10km$ more distance than the straight-line trajectory, the RMSE converges fastest with the path length when sailing along the planned path. Moreover, the RMSE remains the lowest among all paths, with convergence accuracy stabilizing around $0.1km$ after reaching $20km$.

The sum of RMSE of the trajectories are shown in Figure 5. Among all the trajectories, L8 shows the closest trace to the planned path, including partial path overlap. However, the trend of RMSE for these four trajectories is similar, with the planned trajectory exhibiting the smallest value. L7 and L9 also show good positioning accuracy, indicating that the trajectories near the planned path can also achieve high localization accuracy. Nevertheless, the planned path remains the better choice as it exhibits the highest positioning accuracy. Furthermore, the planned path maintains reliable positioning accuracy between L7 and L9, even if it deviates from the intended route. This ensures that the carrier does not get lost, reducing the risk of losing the carrier significantly. The planned trajectory is the optimal choice since it offers both high localization accuracy and reliability in maintaining this accuracy, especially in regions with high Mn values.

4.2. Test 2

Test 1 indicates that the RMSE of all trajectories decreases rapidly during the initial phase because the starting point is located in a gravity informative region. Test 2 changed the starting point (start2) to $(30,58)$, situated at a relatively gentle seabed. As shown in Figure 6, this location has a smaller Mn value, indicating that less gravity information can be used at the initial position. In the figure, the black curve represents the planned trajectory, while the gray curve represents the direct trajectory.

To further Test the proposed method, we generated 10 additional paths between the starting and ending points, with each path having a lateral distance of $5.5km$. All 12 trajectories are depicted in Figure 6, where the black curve corresponds to the planned path, while the gray Direct path represents the shortest path directly connecting the starting and ending points. We named these newly added paths from left to right as L3, L4, ..., L12, and each trajectory is divided into 100 sampling points by equal Euclidean distance.

Figure 6 displays the distribution of 12 trajectories. The RMSE of the position error for these trajectories is plotted in Figure 7, while the sum of RMSE values for each trajectory is presented in Figure 8. The starting point of Test 2 is situated in an area with a small Mn value. In addition to the planned trajectory, there are two lines, L3 and L4, through the region with large Mn values on the left areas. Among all lines, L4 shows a long overlap with the planned trajectory during its initial phase. From Figure 7, it can be observed that the RMSE of the planned trajectory, L3, and L4 decreases the fastest. These three trajectories have an RMSE of approximately $0.75km$ at a trajectory length of 15 km. Line L5 also passes through a large Mn area but not as much as L3, L4, and the planned trajectory. Therefore, its positioning error decreases faster than other trajectories after the three.

However, the remaining trajectories, L6-L12 and the direct trajectory, exhibit a bias towards the east in the beginning phase, resulting in slower decrease of their position errors. The RMSE of the planned trajectory reaches minimum at $50km$, followed by maintaining convergence. The speed of convergence and convergence down line of the planned trajectory are significantly better than the other trajectories. Furthermore, the sum of RMSE of the planned trajectory, presented in Figure 8 is $42km$, which makes it the optimal path among all 12 trajectories. L4 has the second-best performance, with an RMSE of $44km$. On the other hand, the direct trajectory exhibits a disappointing performance, with a large position error located in the region of very small Mn values in the beginning stage. Its RMSE curve decreases slowly, and the sum of RMSE is the largest among all trajectories, reaching $80km$. Hence, these results indicate that the positioning accuracy in the initial stage holds a significant influence on the RMSE of the entire path.

4.3. The impact of Changing the Measurement Noise on the Paths

In practice, the statistical characteristics of gyroscope and accelerometer noise in inertial navigation systems are usually known and stable after careful calibration, so the process noise is stable. Therefore, the main factor to consider is the impact of gravity measurement errors. And since gravity measurement noise is affected by carrier motion, waves, and inaccurate map modeling, its variance is difficult to estimate accurately, so path planning should be considered to adapt to different measurement noise. The calculation of the sum of RMSE of two experimental trajectories is carried out by changing the standard deviation of the measurement noise $\sqrt{R_t}$ to obtain Table 2.

In an overall view in Table 2 and Table 3, increasing the amount of measurement noise will increase the lower limit of positioning error, but will not change the ranking of the lower limit size of the curve, so the planned trajectory always has higher positioning accuracy than the other curves. Therefore, the planned trajectory is always optimal. And with the increase of the measurement noise, the improvement of the localization accuracy of the planned trajectory is more obvious. The optimality of the planned trajectory is not affected by gravimetric measurements, so that the planned trajectory can be navigated in one determined ocean mission to achieve the optimal navigation accuracy. In addition, the RMSE of a spatially close trajectory to the planned path is also close to the RMSE of the planned trajectory. Therefore, even if the real path deviates from the planned one by a small amount, it is possible to obtain a high positioning accuracy by gravity information.

5. Conclusions

This article proposed a method to plan a path that aims for the best positioning accuracy by using the Postiori Cramer-Rao bound of Gravity-aided Inertial Navigation System as the cost value in the A* algorithm. This path planning method is not limited by the ocean gravity field and can plan the trajectory with the minimum navigation error between any start and end points. The next work is to find the set of trajectories within a certain error requirement in which the carrier can achieve higher positioning accuracy without strictly following a certain trajectory, thereby improving the mobility of the carrier.