Prerpints.org logo
Preprint
Article

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

Altmetrics

Downloads

142

Views

33

Comments

0

A peer-reviewed article of this preprint also exists.

This version is not peer-reviewed

Submitted:

05 April 2023

Posted:

06 April 2023

You are already at the latest version

Alerts
Abstract
Gravity-aided inertial navigation system (GAINS) is an important development in autonomous underwater vehicle (AUV) navigation. An effective path planning algorithm plays an important role in the performance of navigation in long-term underwater missions. By combining the gravity information obtained at each position with the error information from the INS, the posterior Cram\'er-Rao bound (PCRB) of GAINS is derived in this paper. The PCRB is the estimated lower bound of position variance for navigation along the planned trajectory. And the sum of PCRB is used as the minimum cost from the initial state to the current state in the state space, and the position error prediction variance of inertial navigation system (INS) is used as the minimum estimated cost of the path from the current state to the goal state in the A* algorithm. Thus, a path planning method with optimal navigation accuracy is proposed. According to simulation results, traveling along the path planed by the proposed method can rapidly improve the positioning accuracy while consuming just slightly more distance. Even when measuring noise changes, the planned path can still maintain optimal positioning accuracy, and high positioning accuracy is possible for any trajectory located within a certain range of the planned path.
Keywords: 
Subject: Engineering  -   Marine Engineering

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
x t + 1 = x t + u t + v t
where x t is the state vector indicating the carrier position. u t represents the state increment of INS. v t represents the state noise from IMU measurement error, Gaussian distribution, and with covariance matrix 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 x t + ε t x t + w t
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. ε 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 = σ m 2 + C 0 β 2 δ 4 4 + σ g 2
where R t includes a combination of sensor measurements and map uncertainty. δ is the map spatial resolution, σ m 2 is the map mapping error variance. C 0 is the variance of the local gravity anomaly. β is the inverse of the square of the correlation distance, and σ 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 ( X t , Y t ) of X t ( x 0 , ⋯ , x t ) and Y t ( y 0 , ⋯ , y t ) at any moment t with a known p ( x 0 ) .
p ( X t + 1 , Y t + 1 ) = p ( X t , Y t ) · p x t + 1 ∣ X t , Y t · p y t + 1 ∣ x t + 1 , X t , Y t = p ( X t , Y t ) · p x t + 1 ∣ x t · p y t + 1 ∣ x t + 1
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 x t + 1 ∣ X t , Y t = p x t + 1 ∣ x t , and p y t + 1 ∣ x t + 1 , X t , Y t = p y t + 1 ∣ x t + 1 .
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 X ^ t is based on the sequence Y t , the mean square error for any unbiased estimator X ^ t should satisfy the following condition.
E X ^ t − X t X ^ t − X t T ≥ P t = J t − 1
where J t is the Fisher information matrix.
J t = E − Δ X t X t log p Y t , X t
where log p Y t , X t denotes the joint probability density of X t and Y t . Here and below, ∇ is the operator of the first order derivative, and Δ 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 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 P t , i.e.
E X ^ t − 1 − X t − 1 x ^ t − x t X ^ t − 1 − X t − 1 x ^ t − x t T ≥ * * P t | t
Correspondingly,
J t = E − Δ X t − 1 X t − 1 − Δ X t − 1 x t − Δ x t X t − 1 − Δ x t x t log p Y t , X t = A t B t B t T C t
From (8) it follows that
P t | t − 1 = C t − B t T A t − 1 B t
Insert (4) into (8) to get the recursive update
J t + 1 = E − Δ X t − 1 X t − 1 Δ X t − 1 x t Δ X t − 1 x t + 1 Δ x t X t − 1 Δ x t x t Δ x t x t + 1 Δ x t + 1 X t − 1 Δ x t + 1 x t Δ x t + 1 x t + 1 log p Y t + 1 , X t + 1 = A t B t 0 B t T C t + D t S t 0 S t T Z t
where 0 denotes the block matrix of zero entries of appropriate dimensionality.
Comparing with (9), it follows that
P t + 1 | t + 1 − 1 = Q t − 0 S t T A t B t B t T C t + D t − 1 0 S t = Z t − S t T P t | t − 1 + D t − 1 S t
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
− log p x t + 1 ∣ x t = c 1 + 1 2 x t + 1 − x t T Q t − 1 x t + 1 − x t
− log p y t ∣ x t = c 2 + 1 2 y t − h x t T R t − 1 y t − h x t
where c 1 and c 2 are constants. Thus, in the equation (11),
D t = E − Δ x t x t log p x t + 1 ∣ x t = Q t − 1
S t = E − Δ x t x t + 1 log p x t + 1 ∣ x t = − Q t − 1 Z t = E − Δ x t + 1 x t + 1 log p x t + 1 ∣ x t + E − Δ x t + 1 x t + 1 log p y t + 1 ∣ x t + 1
= Q t − 1 + H t + 1 T R t + 1 − 1 H t + 1
where H t = ∇ h ( x t ) 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 • is computed only at the current true position.
Bringing the equation (14)-() into the equation (11) gives
P t + 1 | t + 1 = H t + 1 T R t + 1 − 1 H t + 1 + Q t − 1 − Q t − 1 P t | t − 1 + Q t − 1 − 1 Q t − 1 − 1 = H t + 1 T R t + 1 − 1 H t + 1 + Q t + P t | t − 1 − 1
The derivation of (17) makes use of the Woodbury matrix identity[30].
( A + U C V ) − 1 = A − 1 − A − 1 U C − 1 + V A − 1 U − 1 V A − 1
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 = t r { H t T R t − 1 H t }
A further expansion of equation (17) using the Woodbury matrix identity gives a more interesting form
P t + 1 | t = P t | t + Q t
P t + 1 | t + 1 = P t + 1 | t − P t + 1 | t H t + 1 T H t + 1 P t + 1 | t H t + 1 T + R t + 1 − 1 H t + 1 P t + 1 | t
(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 ( n ) = G ( n ) + H ( n )
Where, F ( n ) is the valuation function of a node, which indicates the combined priority of that node considered in the selection of node n. G ( n ) denotes the actual generation value from the starting point to the current node. H ( n ) 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.
d f = ( x c − x f ) 2 + ( y c − y f ) 2
d g = ( x c − x g ) 2 + ( y c − y g ) 2
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 ( n )
where V ( n ) 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 ( n ) for the current node in the A algorithm.
P n + 1 | n = P n | n + Q n
P n + 1 | n + 1 = P n + 1 | n − P n + 1 | n H n + 1 n H n + 1 P n + 1 | n H n + 1 T + R n + 1 − 1 H n + 1 P n + 1 | n
(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 ( n ) + t r { P n + 1 | n + 1 }
where t r { • } 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 ( n ) = t r { Q t · d g / V ( n ) }
Assuming that the current node is n and the actual cost is G ( n ) , 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 )
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:
Preprints 70558 i001

4. Simulation

The numerical gravity map of the simulation shown in Figure 2 is derived from the global ocean gravity field 1 ′ × 1 ′ model produced by Sandwell’s team [31], and the accuracy of the gravity field of the selected experimental region is better than 1.7 m G a l . The maximum gravity anomaly in the region is 40.8 m G a l and the minimum is − 37.2 m G a l . 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 ′ latitude of 1 ′ 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 ()
d E = ( λ 1 − λ 0 ) cos ( φ 0 ) R n π / 180
d N = ( φ 1 − φ 0 ) R m π / 180
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. φ 0 is the latitude of the coordinate origin. Earth’s radius (long semi-axis) R e = 6378137 m . Oblateness of the earth f = 1 / 298.3 . The radius of principal curvature of the meridian circle R m = R e ( 1 − 2 f + 3 f ( sin ( φ 0 ) ) 2 ) . The radius of the Earth’s circle of latitude R n = R e ( 1 + f ( sin ( φ 0 ) ) 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 1 k m .
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.
RMSE = 1 N ∑ i = 1 n { x ^ i } − { x i }
where { x i ^ } is the estimated value of the trajectory heading coordinates, { x i } is the true value of the trajectory heading coordinates, and N is the number of Monte Carlo experiments.
The estimated coordinates x ^ are calculated using the SITAN algorithm, which is essentially an extended Kalman filter algorithm. The state error propagation is
Δ x t + 1 = Δ x t + v t
where Δ x t is the position error of INS, i.e. x t ^ = x I N S − Δ x t .
SITAN fits a linear observation model using stochastic linearization techniques
z t = h ( x t ) − y t ≈ H Δ x t + ε
The fitting expression for H = [ h λ , h φ ] in the observed model is
h λ = 1 L ( 2 L + 1 ) ( L + 1 ) δ ∑ n = i − L m = j − L n = i − 1 m = j + L Δ g M ( n , m ) − ∑ n = i + 1 m = j − L n = i + L m = j + L Δ g m ( n , m )
h φ = 1 L ( 2 L + 1 ) ( L + 1 ) δ ∑ n = i − L m = j − L n = i + L m = j − 1 Δ g M ( n , m ) − ∑ n = i − L m = j + 1 n = i + L m = j + L Δ g m ( n , m )
(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; Δ g M = h ( x t ) which is the gravity anomaly extracted from the digital map based on the INS indicated position; Δ g m = y t which is the gravity anomaly measured in real-time; δ 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 10 k m 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.1 k m after reaching 20 k m .
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.5 k m . 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.75 k m 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 50 k m , 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 42 k m , which makes it the optimal path among all 12 trajectories. L4 has the second-best performance, with an RMSE of 44 k m . 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 80 k m . 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 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.

Author Contributions

Conceptualization, B.W.; methodology, B.W.; software, B.W.; validation, B.W.; formal analysis, B.W.; investigation, B.W.; resources, B.W.; data curation, B.W.; writing—original draft preparation, B.W.; writing—review and editing, B.W.; visualization,B.W.; supervision, T.C.; project administration,T.C.; funding acquisition, T.C.. All authors have read and agreed to the published version of the manuscript.

Funding

This research was funded by the National Key R&D Program of China, Grant Nos. 2017YFC0601601.

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

For the results and data generated during the study, please contact the corresponding author.

Conflicts of Interest

The authors declare no conflict of interest.

Abbreviations

The following abbreviations are used in this manuscript:
GAINS Gravity-aided inertial navigation system
INS Inertial Navigation System
PCRB Postiror Cramér-Rao bound
RMSE Root Mean Square Error

References

  1. Huang, H.; Tang, J.; Liu, C.; Zhang, B.; Wang, B. Variational Bayesian-Based Filter for Inaccurate Input in Underwater Navigation. IEEE Transactions on Vehicular Technology 2021, 70, 8441–8452. [Google Scholar] [CrossRef]
  2. Bao, J.; Li, D.; Qiao, X.; Rauschenbach, T. Integrated navigation for autonomous underwater vehicles in aquaculture: A review. Information Processing in Agriculture 2020, 7, 139–151. [Google Scholar] [CrossRef]
  3. Jalal, F.; Nasir, F. Underwater Navigation, Localization and Path Planning for Autonomous Vehicles: A Review. 2021 International Bhurban Conference on Applied Sciences and Technologies (IBCAST), 2021, pp. 817–828. [CrossRef]
  4. Liu, H.; Wu, L.; Bao, L.; Li, Q.; Zhang, P.; Xi, M.; Wang, Y. Comprehensive Features Matching Algorithm for Gravity Aided Navigation. IEEE Geoscience and Remote Sensing Letters 2022, 19, 1–5. [Google Scholar] [CrossRef]
  5. Wang, Z.; Huang, Y.; Wang, M.; Wu, J.; Zhang, Y. A Computationally Efficient Outlier-Robust Cubature Kalman Filter for Underwater Gravity Matching Navigation. IEEE Transactions on Instrumentation and Measurement 2022, 71, 1–18. [Google Scholar] [CrossRef]
  6. Wang, B.; Ma, Z.; Huang, L.; Deng, Z.; Fu, M. A Filtered-Marine Map-Based Matching Method for Gravity-Aided Navigation of Underwater Vehicles. IEEE/ASME Transactions on Mechatronics 2022, 27, 4507–4517. [Google Scholar] [CrossRef]
  7. Gao, S.; Cai, T.; Fang, K. Gravity-Matching Algorithm Based on K-Nearest Neighbor. Sensors 2022, 22. [Google Scholar] [CrossRef] [PubMed]
  8. Stepanov, O.; Nosov, A. Algorithm for Planning an Informative Route for Map-Aided Navigation. 2021 28th Saint Petersburg International Conference on Integrated Navigation Systems (ICINS), 2021, pp. 1–5. [CrossRef]
  9. Liu, F.; Li, F.; Jing, X. Navigability Analysis of Local Gravity Map With Projection Pursuit-Based Selection Method by Using Gravitation Field Algorithm. IEEE Access 2019, 7, 75873–75889. [Google Scholar] [CrossRef]
  10. Feng, X.; Wang, B.; Deng, Z.; Fu, M. Internal path planning method for underwater vehicle with gravity aided navigation. 2016 35th Chinese Control Conference (CCC), 2016, pp. 5537–5541. [CrossRef]
  11. Yang, J.; Huo, J.; Xi, M.; He, J.; Li, Z.; Song, H.H. A Time-Saving Path Planning Scheme for Autonomous Underwater Vehicles With Complex Underwater Conditions. IEEE Internet of Things Journal 2023, 10, 1001–1013. [Google Scholar] [CrossRef]
  12. Wang, B.; Cai, T.; Fang, K. The Quantification Method of Matching Capability of Areas in Gravity-Aided Inertial Navigation. IEEE Sensors Journal 2022, 22, 20958–20967. [Google Scholar] [CrossRef]
  13. Stepanov, O.A.; Nosov, A.S.; Toropov, A.B. Navigation informativity of geophysical fields in map-aided navigation. 2017 DGON Inertial Sensors and Systems (ISS), 2017, pp. 1–19. [CrossRef]
  14. Panda, M.; Das, B.; Subudhi, B.; Pati, B.B. A Comprehensive Review of Path Planning Algorithms for Autonomous Underwater Vehicles. Int. J. Autom. Comput. 2020, 17, 321–352. [Google Scholar] [CrossRef]
  15. Sang, H.; You, Y.; Sun, X.; Zhou, Y.; Liu, F. The hybrid path planning algorithm based on improved A* and artificial potential field for unmanned surface vehicle formations. Ocean Engineering 2021, 223, 108709. [Google Scholar] [CrossRef]
  16. Chong, Y.; Chai, H.; Li, Y.; Yao, J.; Xiao, G.; Guo, Y. Automatic Recognition of Geomagnetic Suitability Areas for Path Planning of Autonomous Underwater Vehicle. Marine Geodesy 2021, 44, 287–305. [Google Scholar] [CrossRef]
  17. Ma, Y.; Gong, Y.; Xiao, C.; Gao, Y.; Zhang, J. Path Planning for Autonomous Underwater Vehicles: An Ant Colony Algorithm Incorporating Alarm Pheromone. IEEE Transactions on Vehicular Technology 2019, 68, 141–154. [Google Scholar] [CrossRef]
  18. Wang, L.; Liu, L.; Qi, J.; Peng, W. Improved Quantum Particle Swarm Optimization Algorithm for Offline Path Planning in AUVs. IEEE Access 2020, 8, 143397–143411. [Google Scholar] [CrossRef]
  19. Wang, B.; Zhu, Y.; Deng, Z.; Fu, M. The Gravity Matching Area Selection Criteria for Underwater Gravity-Aided Navigation Application Based on the Comprehensive Characteristic Parameter. IEEE/ASME Transactions on Mechatronics 2016, 21, 2935–2943. [Google Scholar] [CrossRef]
  20. Wang, B.; Zhou, M.; Deng, Z.; Fu, M. Sum Vector-Difference-Based Matching Area Selection Method for Underwater Gravity-Aided Navigation. IEEE Access 2019, 7, 123616–123624. [Google Scholar] [CrossRef]
  21. Bergman, N. Recursive bayesian estimation: Navigation and tracking applications. PhD thesis, Linkoping University, 1999.
  22. Tichavsky, P.; Muravchik, C.; Nehorai, A. Posterior Cramer-Rao bounds for discrete-time nonlinear filtering. IEEE Transactions on Signal Processing 1998, 46, 1386–1396. [Google Scholar] [CrossRef]
  23. Zhong, X.; Premkumar, A.B.; Madhukumar, A.S. Particle Filtering and Posterior Cramér-Rao Bound for 2-D Direction of Arrival Tracking Using an Acoustic Vector Sensor. IEEE Sensors Journal 2012, 12, 363–377. [Google Scholar] [CrossRef]
  24. Hernandez, M.; Farina, A. PCRB and IMM for Target Tracking in the Presence of Specular Multipath. IEEE Transactions on Aerospace and Electronic Systems 2020, 56, 2437–2449. [Google Scholar] [CrossRef]
  25. Lee, H.; Chun, J.; Jeon, K. Experimental Results and Posterior Cramér–Rao Bound Analysis of EKF-Based Radar SLAM With Odometer Bias Compensation. IEEE Transactions on Aerospace and Electronic Systems 2021, 57, 310–324. [Google Scholar] [CrossRef]
  26. Zhang, S.; Chen, D.; Fu, T.; Cao, H. Approximating Posterior Cramér–Rao Bounds for Nonlinear Filtering Problems Using Gaussian Mixture Models. IEEE Transactions on Aerospace and Electronic Systems 2021, 57, 984–1001. [Google Scholar] [CrossRef]
  27. OuYang, M.; Ma, Y. Path planning for gravity aided navigation based on improved A* algorithm. Chinese J. Geophys 2020, 63, 4361–4368. [Google Scholar] [CrossRef]
  28. Liu, X.; Ma, D.; Yang, M.; Xia, X.; Guo, P. Modified Block A* Path-Planning Method for Hybrid-Driven Underwater Gliders. IEEE Journal of Oceanic Engineering 2022, 47, 20–31. [Google Scholar] [CrossRef]
  29. Anderson, R.C.; Davenport, J.A.; Jekeli, C. Determination of Gravity Data Spacing Required For Inertial Navigation. Navigation 2000, 47, 1–6. [Google Scholar] [CrossRef]
  30. Higham, N.J. Accuracy and Stability of Numerical Algorithms, second ed.; Society for Industrial and Applied Mathematics, 2002; p. 258. [CrossRef]
  31. Sandwell, D.; Garcia, E.; Soofi, K.; Wessel, P.; Chandler, M.; Smith, W.H.F. Toward 1-mGal Accuracy in Global Marine Gravity from CryoSat-2, Envisat, and Jason-1. The Leading Edge 2013, 32, 892–899. [Google Scholar] [CrossRef]
Preprints 70558 g001
Figure 1. Hidden Markov Model for GAINS.
Figure 1. Hidden Markov Model for GAINS.
Preprints 70558 g001
Preprints 70558 g002
Figure 2. Simulation map.
Figure 2. Simulation map.
Preprints 70558 g002
Preprints 70558 g003
Figure 3. Planed path, Direct path, and trajectory L3, L4, ..., L9 in Test 1.
Figure 3. Planed path, Direct path, and trajectory L3, L4, ..., L9 in Test 1.
Preprints 70558 g003
Preprints 70558 g004
Figure 4. RMSE curves for each trajectory in Test 1.
Figure 4. RMSE curves for each trajectory in Test 1.
Preprints 70558 g004
Preprints 70558 g005
Figure 5. Sum of RMSE for each trajectory in Test 1.
Figure 5. Sum of RMSE for each trajectory in Test 1.
Preprints 70558 g005
Preprints 70558 g006
Figure 6. Planned path, Direct path, and trajectories L3, L4, ..., L12 in Test 2.
Figure 6. Planned path, Direct path, and trajectories L3, L4, ..., L12 in Test 2.
Preprints 70558 g006
Preprints 70558 g007
Figure 7. RMSE curves of Test 2.
Figure 7. RMSE curves of Test 2.
Preprints 70558 g007
Preprints 70558 g008
Figure 8. The sum of RMSE for each trajectory of Test 2.
Figure 8. The sum of RMSE for each trajectory of Test 2.
Preprints 70558 g008
Table 1. Simulation parameters setting.
Table 1. Simulation parameters setting.
Carrier velocity V 5 m / s
Initial position covariance P ( x 0 ) d i a g ( [ 1 , 1 ] ) k m 2 / s
Process noise covariance Q t d i a g ( [ 0.01 , 0.01 ] 2 ) m 2 / s
Measurement noise covariance R t 4 m G a l 2
Map Spatial Resolution δ 1 k m
Number of Monte Carlo runs 1000
Table 2. Sum of RMSE of the trajectories in Test 1.
Table 2. Sum of RMSE of the trajectories in Test 1.
R t
( m G a l )		 sum of RMSE ( k m )
Planed Direct L3 L4 L5 L6 L7 L8 L9
1 8.1 12.8 21.6 19.5 13.8 10.0 9.5 9.0 9.6
2 14.4 22.8 38.0 34.3 24.6 17.7 16.7 15.7 16.8
3 20.9 33.4 56.0 50.4 36.3 25.7 24.0 22.5 24.2
4 27.4 44.6 75.6 67.1 49.2 34.0 31.4 29.3 31.7
Table 3. Sum of RMSE of the trajectories in Test 2.
Table 3. Sum of RMSE of the trajectories in Test 2.
R t
( m G a l )		 sum of RMSE ( k m )
Planed Direct L3 L4 L5 L6 L7 L8 L9 L10 L11 L12
1 30.4 66.4 40.0 31.3 35.8 41.5 54.1 52.5 53.9 49.3 44.7 50.0
2 41.8 78.5 55.4 43.4 46.7 54.3 67.6 64.2 68.6 62.8 63.0 67.7
3 54.6 93.3 73.2 56.6 59.0 68.7 84.5 79.4 86.3 80.5 84.3 89.0
4 68.1 110.0 92.5 70.7 72.2 84.7 104.0 97.1 106.0 101.0 108.0 112.2
Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.
Copyright: This open access article is published under a Creative Commons CC BY 4.0 license, which permit the free download, distribution, and reuse, provided that the author and preprint are cited in any reuse.
Path Planning Method for Underwater Gravity-Aided Inertial Navigation Based on PCRB

Bo Wang

et al.

,

2023

Path Planning of Unmanned Surface Vehicle Based on Improved Sparrow Search Algorithm

Guang zhong Liu

et al.

,

2023

Research on Unmanned Vehicle Path Planning Based on Improved A and DWA Fusion Algorithm

Ruoming Wang

et al.

,

2024

Prerpints.org logo

Preprints.org is a free preprint server supported by MDPI in Basel, Switzerland.

facebook logotwitter logolinkedin logo
MDPI Initiatives
Important Links
Subscribe

Disclaimer

Terms of Use

Privacy Policy

© 2024 MDPI (Basel, Switzerland) unless otherwise stated