Global dynamic path‐planning algorithm in gravity‐aided inertial navigation system

IET Signal ProcessingVolume 15, Issue 8 p. 510-520 ORIGINAL RESEARCH PAPEROpen Access Global dynamic path-planning algorithm in gravity-aided inertial navigation system Shengwu Zhao, Shengwu Zhao orcid.org/0000-0003-3459-3062 School of Automation, Beijing Institute of Technology, Beijing, ChinaSearch for more papers by this authorLei Shi, Lei Shi School of Automation, Beijing Institute of Technology, Beijing, ChinaSearch for more papers by this authorWenzhe Zhang, Wenzhe Zhang School of Automation, Beijing Institute of Technology, Beijing, ChinaSearch for more papers by this authorZhihong Deng, Corresponding Author Zhihong Deng dzh_deng@bit.edu.cn School of Automation, Beijing Institute of Technology, Beijing, China Correspondence Zhihong Deng, School of Automation, Beijing Institute of Technology, 100081, Beijing, China. Email: dzh_deng@bit.edu.cnSearch for more papers by this author Shengwu Zhao, Shengwu Zhao orcid.org/0000-0003-3459-3062 School of Automation, Beijing Institute of Technology, Beijing, ChinaSearch for more papers by this authorLei Shi, Lei Shi School of Automation, Beijing Institute of Technology, Beijing, ChinaSearch for more papers by this authorWenzhe Zhang, Wenzhe Zhang School of Automation, Beijing Institute of Technology, Beijing, ChinaSearch for more papers by this authorZhihong Deng, Corresponding Author Zhihong Deng dzh_deng@bit.edu.cn School of Automation, Beijing Institute of Technology, Beijing, China Correspondence Zhihong Deng, School of Automation, Beijing Institute of Technology, 100081, Beijing, China. Email: dzh_deng@bit.edu.cnSearch for more papers by this author First published: 30 June 2021 https://doi.org/10.1049/sil2.12056AboutSectionsPDF ToolsRequest permissionExport citationAdd to favoritesTrack citation ShareShare Give accessShare full text accessShare full-text accessPlease review our Terms and Conditions of Use and check box below to share full-text version of article.I have read and accept the Wiley Online Library Terms and Conditions of UseShareable LinkUse the link below to share a full-text version of this article with your friends and colleagues. Learn more.Copy URL Share a linkShare onFacebookTwitterLinked InRedditWechat Abstract Signals from gyros and accelerometers in an inertial navigation system (INS) can be processed to obtain navigation information while its errors accumulate with time. A path with long-term characteristics is more likely to encounter unpredictable dynamic conditions. To deal with such problems, this work improves the global static path-planning algorithm based on the A* algorithm, and the local dynamic path-planning algorithm based on dynamic window approach (DWA). In the global planning algorithm, the gravity index and the heading index are incorporated into the cost function of the A* algorithm to consider the mismatches and the deflection of the path. Furthermore, the search method of the A* algorithm is also improved. In the local planning algorithm, the motion model of the DWA algorithm is improved to a high-precision model and expressed in a geographic coordinate system. Finally, the improved global static planning and local dynamic planning are combined as the global dynamic path-planning algorithm of an underwater vehicle. The simulation results show that the global dynamic path-planning algorithm proposed can realise obstacle-free navigation of the underwater vehicle. 1 INTRODUCTION Various sensors have their own characteristics and signals from sensors need to be processed to get useful information. In an inertial navigation system (INS), gyros and accelerometers can output angular velocity signal and acceleration signal. By integrating these signals, navigation information (attitude, velocity and position) can be obtained. Therefore, INS has been extensively studied in many fields such as underwater navigation [1, 2], indoor navigation [3, 4], automatic drive [5, 6] and so on. However, due to the errors of gyros and accelerometers, INS errors accumulate with time [7] so that other navigation systems are needed to compensate the errors of the INS. Gravity anomaly is one of the general physical properties of the Earth and it does not change over time. A gravimeter can obtain gravity signal, which can be processed for gravity anomaly values. Based on the gravity anomaly, a gravity-aided inertial navigation system is a good passive navigation method to correct the errors of the INS. The system does not need to receive external information and transmit information outside, and it has properties of high precision and reliability without being limited by terrain and time [8, 9]. In a gravity-aided inertial navigation system, when an underwater vehicle passes through adaptation areas where the gravity anomaly values change sharply, the gravity matching accuracy is improved [10]. In most cases, the shape of the adaptation area is irregular and the adaptability of each point in the adaptation area is also different. The navigation path of the underwater vehicle is not a simple connection from the start point to the end point. Therefore, in order to improve the accuracy of navigation global path planning is important in adaptation areas. Besides, it is more likely for long-term underwater vehicles to encounter unknown dynamic conditions. Hence, local path planning is required to deal with unknown situations. The coordination of global path planning and local path planning can realise obstacle-free travel for underwater vehicles. Since the 20th century, global path planning from one point on the map to another has been widely studied. The Dijkstra algorithm is proposed to solve the shortest path problem in the weighted graph by employing the greedy algorithm strategy [11]. Due to the inefficiency of the Dijkstra algorithm, the A* search algorithm is presented, which adds heuristic function to avoid goalless search and greatly improves search efficiency [12]. Later, a probabilistic roadmap (PRM) algorithm based on random networks is proposed to realise path planning [13]. In addition to the above algorithms, path-planning algorithms such as the Voronoi graph method [14] and improved intelligent algorithms (genetic algorithm [15], neural network algorithm [16], ant colony algorithm [17], etc.) have also been extensively studied. Nowadays, global path-planning algorithms are being successfully applied to mobile robots, unmanned aerial vehicles (UAVs) and so on. For instance, the PRM algorithm and the artificial bee colony algorithm are combined to address the problem of robot path planning based on energy and road characteristics [18]. The improved particle swarm optimisation (IPSO) algorithm is used as a cooperative path-planning algorithm to implement a search-and-track mission for an underwater target in [19]. In [20], an innovative artificial potential field (APF) algorithm and an enhanced genetic algorithm are proposed for the path planning of multiple mobile robots in continuous environments. [21] proposes a noble multi-robot path-planning algorithm using Deep q learning combined with the CNN (Convolution Neural Network) algorithm. In a gravity-aided inertial navigation system, more issues need to be considered as mentioned before. Hence, improvements are necessary. For instance, composite gravity characteristic parameters are incorporated into the cost function of the A* algorithm which obtains the path through the area where the gravity field changes significantly [22, 23]. The value of the gravity gradient can also be incorporated into the cost function of the A* algorithm and an adaptive threshold is used to determine the path planning in the adaptation area [24]. These algorithms consider traditional gravity characteristic parameters (gravity anomaly standard deviation, gravity anomaly gradient, longitude and latitude roughness, etc.) and realise the function of planning a path through an area where gravity anomaly values change sharply. However, there exists several problems about gravity characteristic parameters: First is the problem of complex calculations. Every gravity characteristic parameter has its own focus. Giving weight to parameters is the commonly used way which brings huge calculations if the map is big enough. Second is the problem of one-sidedness. Traditional gravity characteristic parameters only consider the data feature of gravity anomaly values instead of the spatial feature which can easily lead to mismatches in the period of gravity matching [25]. Apart from the above problems, in the gravity-aided inertial navigation system, frequent turns affect the accuracy of inertial navigation and the accuracy of positioning. Therefore, the shape of the global path is also one of the factors to be considered. In order to solve the problem of encountering an unknown situation, more and more local dynamic algorithms have been studied. A local obstacle avoidance algorithm, the dynamic window approach (DWA), is proposed to realise a robot's rapid obstacle avoidance based on the robot's motion model [26]. The D* algorithm is proposed to quickly formulate a repair plan to obtain a new path for the cost of changes near the carrier [27]. Since then, improvements to the D* algorithm, such as D* lite [28] and FD* [29], have also been continuously discussed and studied. Later, a dynamic model based on the traditional artificial potential field model is presented to improve the performance of dynamic programing [30]. An adaptive potential field algorithm based on a genetic algorithm is proposed for collision-free path planning by adding the dynamic information of the target to the path-planning algorithm [31]. Recently, the modified RRT* algorithm has been used to propose a multi-target dynamic, fast exploration random algorithm suitable for robots navigating in an unknown dynamic environment [32, 33] presents the design and implementation of sampling-based path-planning methods for a UAV to avoid collision with commercial aircraft and other moving obstacles. In [34] a double Q-network (DDQN), deep reinforcement learning is applied to the dynamic path planning of an unknown environment. An adaptive path-replanning (APReP) method is proposed in discrete urban environments to respond to dynamic urban environments and self-adjust their paths safely and efficiently in [35]. Most of the existing local dynamic path-planning algorithms are used in the field of robotics or aircraft. Compared with them, underwater vehicles have a larger range of motion and different motion models. Therefore, when using a local dynamic algorithm, it needs to be improved to an algorithm suitable for underwater vehicles. It is worth mentioning that it is unnecessary to consider the gravity factor due to the purpose of obstacle avoidance. There are problems with the aforementioned related work. In this work, we propose some solutions to these problems to provide a reliable, comprehensive and dynamic path-planning algorithm. Considering the mismatches and the problem of path deflection, the proposed algorithm can guide underwater vehicles to pass through an area where the gravity anomaly values change sharply. With the algorithm of gravity matching, INS errors can be corrected. Moreover, it can also adjust the path to respond to an unpredicted situation if the origin path is invalid. To be specific, the main contributions of this work can be concluded as follows: (a) A new gravity characteristic parameter (also called gravity index) based on density estimation which is more comprehensive and has fewer calculations than traditional gravity characteristic parameters is proposed. (b) The heading index is defined to optimise the shape of the global path. Deflections times and the degree of turning are considered to decrease the errors of the INS. (c) On the basis of the traditional A* algorithm, the gravity index and heading index proposed are added to the cost function. A variable search step in line with the degree of change in the gravity characteristic parameter in the A* algorithm is also proposed to optimise the search method. (d) The motion model in the DWA algorithm is improved to be more accurate and expressed in a geographic coordinate system. The virtual end point is set to make the DWA algorithm available. The rest of this work is arranged as follows: In Section 2, the cost function and search method of the A* algorithm are improved. In Section 3, the DWA algorithm is introduced and its motion model is improved. In Section 4, a global dynamic path-planning algorithm is presented by combining the improved A* and improved DWA algorithm. Simulations are presented in Section 5. Section 6 is the conclusion. 2 IMPROVED A* ALGORITHM 2.1 Traditional A* algorithm The traditional A* algorithm which is widely used in robot planning and in various games has high efficiency in solving the shortest path in a static road network. The A* algorithm is also a heuristic search algorithm and its core evaluates the cost of each point in the search area. The cost function is f ( n ) = g ( n ) + h ( n ) (1)where f(n) is the total cost at n, g(n) is the cost of movement from the start point to n and h(n) is the estimated distance from n to the end point. The traditional A* algorithm can get the shortest path from the start point to the end point while in the adaptive area; the optimal path passing through the points where the gravity anomaly values change sharply is not always the shortest path. At the same time, the planned path should be smooth and should have fewer turning points. Therefore, this section aims at improving the cost function and search method of the A* algorithm. 2.2 Improved cost function According to the characteristics of a gravity-aided inertial navigation system, the cost function at point n of the A* algorithm can be improved as f ( n ) = τ 1 g ( n ) + τ 2 h ( n ) + τ 3 c ( n ) + τ 4 d ( n ) (2)where c(n) is the gravity index (also called gravity characteristic parameter) that reflects the degree of change in gravity anomaly values at n, d(n) is the heading index that reflects the times and degree of deflection, and τi(i = 1, 2, 3, 4) is the weight. 2.2.1 Gravity index c(n) A method based on density estimation for calculating the gravity characteristic parameter was presented to improve the comprehensiveness of adaptability analysis. This method divides the adaptability analysis of the local area into the gravity domain and the spatial domain. The gravity domain reflects the degree of change in gravity anomaly values, while the spatial domain takes into account the effects of mismatches. Compared to traditional gravity characteristic parameters, this method considers the effects of mismatches in gravity matching and requires less calculation. The gravity index at point n based on density estimation can be calculated by c ( n ) = ∑ i = 1 m K g ( ∣ g n − g n i ∣ ) K s ( ∣ s n − s n i ∣ ) (3)where n is the current point, ni is the rest point in the local area, Ks(⋅) is the kernel function in the spatial domain, Kg(⋅) is the kernel function in the gravity domain, g is the gravity anomaly value and s is the coordinate. The kernel function in spatial domain can choose the Gaussian distribution function or the uniform distribution function. The Gaussian distribution function can be shown as K s = 1 2 π σ s e x p ( − ( d m a x − d ) 2 2 σ s 2 ) (4)where σs is the standard deviation of the mismatch position, d is the distance of the other point in the local area from the current point which equals ∣ s n − s n i ∣, dmax is the maximum distance. The calculated Ks represents the loss of mismatch at point n in the local area. In general, the loss increases with the distance from the correct matching points when mismatches happen. In addition, the uniform distribution function in the spatial domain can be shown as K s = 1 h 2 (5)where h is the width of the local area. The gravity index does not take into account the mismatches if choosing the uniform distribution function. Unlike the spatial domain, only the Gaussian distribution function can be the kernel function in the gravity domain. The reason is that when the gravity-matching algorithm is studied, we often assume that the gravity anomaly observation error satisfies the Gaussian distribution. The kernel function can be K g = 1 2 π σ g e x p − Δ g 2 σ s g (6)where σg is the standard deviation of the gravity anomaly, Δg is the difference between the gravity anomaly values of other points in the local area and the current point, which equals ∣ g n − g n i ∣, and can generally be directly set as the standard deviation of the gravity anomaly observation. The calculated Kg reflects the degree of change of gravity anomaly values. As shown in Equation (6), the smaller the Kg, the more sharply the gravity anomaly values change. The combination of the gravity domain and the spatial domain to represent the gravity characteristic parameter of the local area can reflect the degree of change in the gravity anomaly values. Points corresponding to the smaller value obtained by the proposed method are more suitable for gravity matching. However, in actual gravity matching, a mismatch situation is difficult to avoid. This method only divides the area with poor adaptability where mismatch may occur and cannot solve the mismatch situation in the matching algorithm due to similar gravity anomaly values. 2.2.2 Heading index d(n) The underwater vehicle sails along the planned path. When the shape of the planned path has more deflection times and a greater degree of turning, the errors of the inertial navigation system will increase, which will reduce the accuracy of the matching algorithm. This is contrary to adding the gravity index to reduce the matching error proposed above. Thus, properly setting the shape of the track to reduce unnecessary turns and reduce the degree of deflection is also one of the factors that reduce the navigation error. To solve the problem discussed above, the heading index is proposed as d ( n ) = γ 1 ρ 1 ( ∣ φ f → c − φ c → n ∣ ) + γ 2 ρ 2 ( ∣ s c − s n ∣ ) (7)where ρ1(⋅) and ρ2(⋅) are the functions of the deflection and distance, f is the parent node in the A* algorithm, c is the current node, n is the child node to be selected, φ is the heading angle from one node to the other, s is the coordinate and γ is the weight. In the heading index, the function ρ1 is presented to mainly reduce the number of deflections and can be set as a piecewise function shown as ρ 1 ( Δ φ ) = k ( Δ φ ) Δ φ < φ m a x T Δ φ ≥ φ m a x (8)where Δφ = |φf→c − φc→n|, φmax is the maximum angle allowed for deflection, k(⋅) is an increasing function of the deflection angle, such as the sine function, and T is a larger value. Due to the function ρ1(⋅), the path-planning algorithm will preferentially select the point that makes the path deflection angle small. The main use of function ρ2(⋅) is to reduce the degree of deflection and can be set as decreasing function such as inverse proportional function. Due to the function ρ2(⋅), The path planning algorithm will preferentially select the point far away from the current point. 2.3 Improved search method The improvement of the search method is mainly reflected in the size of the search step. A larger step size allows more deflection angles during path planning. The deflection angles at a larger step size combined with the heading index reduces the degree of deflection of the path, while some points with more gravity information may be ignored. A smaller step size makes the calculation complicated. Therefore, this work presents a variable search step size. The variable search step is based on the degree of change in the gravity characteristic parameter. When the degree of change in the gravity characteristic parameter is large, it means that the matching points in the nearby area are quite good. At this time, the search step should be reduced and each grid point should be calculated. When the degree of change of the gravity characteristic parameter is small, the adaptability of the points in the nearby area is considered to be similar. At this time, the search step can be increased to obtain a faster search. The degree of change in the gravity characteristic parameter can be calculated with the help of the statistical characteristic parameters (standard deviation, slope, etc.) in the local area or the density estimation method proposed above. For instance, if the standard deviation σg is chosen to present the degree of change in the gravity characteristic parameter, it can be calculated by σ g = 1 m n − 1 ∑ i = 1 m ∑ j = 1 n g g ( i , j ) − g g ¯ 2 (9)where gg(i, j) is the gravity characteristic parameter value at (i, j), m and n are the length and width of the local area, and is shown by g g ¯ = 1 m n ∑ i = 1 m ∑ j = 1 n g g ( i , j ) (10) If the search step size of one or two is considered, the search step size can be calculate by S t e p = 1 σ g ≥ σ t h 2 σ g < σ t h (11)where Step is the search step size, σth is the threshold which can be the median or the mean value of the gravity characteristic parameter values at all points. 2.4 Steps of the improved A* algorithm Through the above improvements to the A* algorithm, the final steps of the global static path-planning algorithm are shown in Table 1. TABLE 1. Steps of the improved A* algorithm Step Process Step 1 Determine the start point, end point and initial heading angle. Obtain the occupied grid map and calculate the gravity index. Take the start point as the current point c. Set OpenList and ClosedList as empty. Put the current point c into ClosedList. Step 2 Determine the search step by means of the degree of change in the gravity index at the current point c or set the search step to a fixed value. Step 3 For each point searched above that is not in ClosedList, calculate g(n). If the point has been in OpenList, delete this point in OpenList if the new g(n) is less than the g(n) of the point in OpenList. Calculate h(n), d(n) and c(n) of the point and then put them into OpenList. Step 4 For all the points in OpenList, normalise to four indexes and calculate f(n). Find the point corresponding to the smallest f(n) and set it as the current c. Delete this point in OpenList and put it into ClosedList. Step 5 If the current c is the end point, get the path according to the relationship of the child and parent point. Otherwise, go back to Step 2. 3 IMPROVED DWA ALGORITHM 3.1 DWA algorithm The problem of unpredicted environment is addressed by the DWA algorithm by choosing the optimal sailing speed (In this work unless otherwise specified, the speed concept in this section refers to (υ, ω) rather than the translational speed υ). The algorithm samples multiple sets of speeds in the speed space and simulates their trajectories at a certain time under these speeds. Based on the shape of the trajectory and known map information, each simulated trajectory is evaluated and then the speed corresponding to the optimal trajectory is used to drive the vehicle to move in the next unit time Δt. The meaning of the dynamic window is to limit the speed space within a certain range according to the motion state of the vehicle at different times and the performance of the vehicle itself. After that, the DWA algorithm studies the speed in the dynamic window. The main content of the DWA algorithm is divided into three aspects: motion model and status update, speed sampling and speed evaluation. However, the traditional DWA algorithm is mainly used in robotics and cannot be applied to underwater vehicles. Therefore, this section will not only explain the main content of the DWA algorithm and the determination of the end point but also improve them according to the actual situation of the underwater vehicle. 3.2 Motion model and status update Consider that the underwater vehicle is non-omnidirectional, that is, there are only two cases of forward and rotation. Then, it must be restricted by its own model and its own state when sailing. At present, the most commonly used motion model is the simplified model, which equates the motion of the vehicle with the speed (υ, ω) per unit of time to first, going ahead and then when rotating. The state update formula is as follows: x t + 1 y t + 1 θ t + 1 = x t y t θ t + υ cos ( θ t ) Δ t υ sin ( θ t ) Δ t ω Δ t (12)where [xt; yt; θt] is the position and heading angle at time t. When the unit time Δt is very short, the above simplified model Equation (12) is no different from the real model of the vehicle. However, when the unit time is longer, the trajectory at the given speed should be a circular arc rather than a line segment. This simplified model will have a large error and the error will accumulate over time, which is extremely unfavourable for the positioning of the underwater vehicle. Therefore, by converting the approximate straight-line trajectory into an arc calculation, the motion model with a higher accuracy is obtained as x t + 1 y t + 1 θ t + 1 = x t y t θ t + υ ω ( − sin θ t + sin ( θ t + ω Δ t ) ) υ ω ( cos θ t − cos ( θ t + ω Δ t ) ) ω Δ t (13) Equation (13) applies to the ω ≠ 0 situation while Equation (12) applies to the ω = 0 situation. In navigation, the latitude and longitude (λ, φ) are generally used as the positioning unit. Hence, if the direction of north by east is set as the positive direction of the angle, according to the characteristics of the Earth and the relationship between latitude, longitude and speed, the improved motion model is obtained as φ t + 1 λ t + 1 θ t + 1 = φ t λ t θ t + υ ( − sin θ t + sin ( θ t + ω Δ t ) ) ω R υ ( cos θ t − cos ( θ t + ω Δ t ) ) ω R ⋅ cos ( φ t ) ω Δ t ( ω ≠ 0 ) (14) φ t + 1 λ t + 1 θ t + 1 = φ t λ t θ t + υ cos θ t Δ t R υ sin θ t Δ t R ⋅ cos ( φ t ) ω Δ t ( ω = 0 ) (15)where R is the radius of the Earth. 3.3 Speed sampling For underwater vehicles, not all speeds in the speed space are suitable. Some restrictions are listed as follows: (1) Underwater vehicles are affected by the maximum and minimum speeds allowed by their own motion. If Vs is the speed space by this limit, V s = ( υ , ω ) υ ∈ [ υ m i n , υ m a x ] , ω ∈ [ ω m i n , ω m a x ] (16) (2) Underwater vehicles are affected by their own motors and other machinery so that there are maximum and minimum acceleration and deceleration limits. The sampling speed can be reached within a navigation unit time. If Vd is the speed space by this limit, V d = ( υ , ω ) υ ∈ [ υ c − a b Δ t , υ c + a a Δ t ] ∧ ω ∈ [ ω c − α b Δ t , ω c + α a Δ t ] (17)where aa and ab are the maximum translational acceleration and maximum translational deceleration, αa and αb are the maximum angular acceleration and maximum angular deceleration, and υc and ωc are the translational speed and angular speed of the underwater vehicle at the current moment. (3) The speed of the underwater vehicle is limited in case it is close to the obstacle. The maximum speed in the speed space depends on the minimum distance dist(υ, ω) from the obstacle on the simulated trajectory corresponding to this speed. If Va is the speed space by this limit, V a = ( υ , ω ) υ ≤ 2 ⋅ d i s t ( υ , ω ) a b ∧ ω ≤ 2 ⋅ d i s t ( υ , ω ) α b (18) In practice, a minimum value Dmin and a maximum value Dmax can be set for this minimum distance dist(υ, ω). When dist(υ, ω) is less than Dmin, the corresponding sampling speed should be discarded. When dist(υ, ω) is greater than Dmax or there are no obstacles in the map, dist(υ, ω) can be made constant. Therefore, based on the above three kinds of restrictions, the resulting speed space Vr is V r = V s ∩ V d ∩ V a (19) 3.4 Speed evaluation After speed sampling, a series of simulated trajectories are obtained and then these trajectories need to be evaluated to obtain the optimal speed. The DWA algorithm evaluates each simulated trajectory from the following three aspects: (1) heading(υ, ω) calculates the difference θh between the heading of the end of the simulated trajectory at the sampling speed (υ, ω) and the target heading that is shown as h e a d i n g ( υ , ω ) = 1 − θ h π (20) (2) In order to reach the target point as s