State transformation extended Kalman filter–based tightly coupled strapdown inertial navigation system/global navigation satellite system/laser Doppler velocimeter integration for seamless navigation of unmanned ground vehicle in urban areas

Abstract

With the rapid development of unmanned ground vehicle industry, how to achieve continuous, reliable, and high-accuracy navigation becomes very important. At present, the integrated navigation with global navigation satellite system and strapdown inertial navigation system is the most mature and effective navigation technology for unmanned ground vehicle. However, this technique depends on the signal accuracy of global navigation satellite system. When the receiver cannot capture four or more satellite signals for a long time or the satellite completely invalid, it cannot provide accurate navigation and positioning information for the unmanned ground vehicle. Therefore, this article combine the observation information of strapdown inertial navigation system, global navigation satellite system, and laser Doppler velocimeter to propose a high-precision seamless navigation technique of unmanned ground vehicle based on state transformation extended Kalman filter. Under different land vehicle driving environments and global navigation satellite system signal quality conditions, the seamless navigation technique is evaluated through global navigation satellite system interruption simulation and land vehicle experiments. The experimental results show that the strapdown inertial navigation system/global navigation satellite system/laser Doppler velocimeter tightly coupled integration seamless navigation has good environmental adaptability and reliability and can maintain high navigation accuracy under high frequency global navigation satellite system–signal blockage conditions in urban areas.

Introduction

In the process of research and application of unmanned ground vehicle (UGV), due to the more diverse task requirements, the navigation technology which can keep high positioning accuracy in various driving environments has been widely concerned. It has also become the key to promote the further development of unmanned technologies. Strapdown inertial navigation system (SINS) is an autonomous navigation system that does not rely on external information. It has the advantages of good concealment and strong anti-interference ability. The disadvantage is that the navigation error accumulates with time. Global navigation satellite system (GNSS) can provide users with all-weather, all-time, continuous 3D position and 3D velocity information, but its disadvantages are poor dynamic performance, susceptibility to electromagnetic interference, and easy to block satellite signals. The integration of SINS and GNSS can overcome the shortcomings of low long-term accuracy of SINS, poor dynamic performance of satellite navigation system, and obtain better performance than using any kind of navigation sensors alone. At present, SINS/GNSS is the most commonly used navigation mode of land vehicle navigation system, which can provide high-precision output with low error for a long time and meet the requirements of real-time positioning. ^{1 –6} However, SINS/GNSS still has shortcomings in some challenging environments. For example, the signal blocking caused by buildings, bridges, and trees in urban environment and the multipath effect in canyon and tunnel will lead to the GNSS equipment unable to output effective navigation information. If GNSS denied for a long time, the integrated system will degenerate into a pure inertial navigation system, and the navigation error will accumulate rapidly with time.

Therefore, in GNSS-signal blockage environments, other sensors are usually introduced to maintain the navigation accuracy of UGV, such as OD, cameras, and laser Doppler velocimeter (LDV) and so on.

Odometer is one of the most commonly used sensors in land vehicle self-contained navigation system. It can provide the vehicle velocity and distance independently by measuring the wheel rotation angle and has good anti-interference performance. ^{7 –11} However, the performance of the OD will also be affected by the state of wheel and the road condition. The jumping, slipping, and deformation of the wheel also will produce errors, which will affect the accuracy of the forward velocity of the vehicle estimated by the encoder. ¹²

Visual navigation is a new navigation method with the development of computer vision, photogrammetry, and other science and technology. It uses stereo vision, optical flow, pattern recognition, and feature tracking matching to generate navigation information to help the vehicle achieve navigation and positioning. ^{13 –16} However, as an optical device, the camera used in visual navigation is easily affected by the problems of shadow, field of vision, dynamic movement of the target, and cannot provide stable observation information when the visibility is low.

LDV is a new type of navigation sensor, which uses the good characteristics of laser to measure the driving velocity of the vehicle. ^{17 –19} It has the advantages of long-distance measurement, high velocity measurement accuracy, and fast dynamic response. At the same time, as the measurement sensor which can provide vehicle velocity information, LDV has higher measurement accuracy and reliability than OD, because the less influence of vehicle driving conditions due to the noncontact measuring method. ^18,20,21 Therefore, SINS/GNSS/LDV integrated navigation is a feasible method to improve the positioning accuracy of UGV in GNSS-signal blockage environments. Compared with the vision sensors, LDV occupies less computing resources, has higher measurement accuracy, and is more reliable and practical in challenging environments.

At present, EKF is one of the most commonly used filter estimation methods in SINS-based land vehicle integrated navigation system. Assuming that the estimation error is small enough, the Kalman gain is calculated by the first-order linearization propagation of dynamics about the estimated trajectory. However, the variance of EKF may be inconsistent if the estimation result is far from the real state value, that is, the error state covariance matrix calculated by the filter is inconsistent with the real value. ^10,22,23 Wang et al. proposed a new Kalman filtering method based on state transformation, which uses approximately constant gravity vector instead of specific force vector, thus avoiding the problem of inaccurate calculation of traditional EKF error state covariance matrix, and effectively improving the robustness of filtering. ^10,23

In this article, our first aim is to improve the positioning accuracy of UGV in GNSS-signal blockage conditions, by introducing appropriate sensors to assist SINS and GNSS, so as to realize the seamless navigation of UGV in different environments. Our second aim is to adopt a new filtering method, which can make the integrated navigation system have better filtering stability and higher positioning accuracy. Our main contributions are summarized as follows:

A technique for high-precision seamless navigation based on integrated SINS/GNSS/LDV is proposed, which is suitable for richer and more complex application environments;

The measurement information of LDV is introduced to suppress the divergence of positioning error of UGV navigation system, when GNSS cannot provide high-quality and effective navigation information;

State transformation extended Kalman filter (ST-EKF) is used to replace the traditional EKF as the information fusion framework, and a new SINS/GNSS/LDV error state model and observation model are derived to improve the filtering robustness of the integrated navigation system;

The long-distance land vehicle test is carried out, including different driving environments and GNSS signal quality conditions. The application effect of the technique proposed in this article is evaluated by post-processing the experimental data.

The remainder of this article is organized as follows: The second section describes the system error state model of SINS/GNSS/LDV seamless navigation technique, the third section describes the system observation model and technology route, the fourth section introduces the experimental design and equipment, the fifth section discusses the experimental results, and the conclusions are given in the sixth section.

System error state model

The ST-EKF algorithm was first proposed in Wang et al. ²³ and has been extended to be applied in integrated navigation systems such as SINS/GNSS, SINS/OD. ^10,23 Experiments showed that ST-EKF had better filtering robustness than EKF in high dynamic environment. In this section, a new velocity error equation under the framework of ST-EKF was deduced and explained from the perspective of solving the covariance inconsistency problem. As well as that, the state transition matrix of the system was redefined, and the error state model and observation model of the SINS/GNSS/LDV integrated navigation system were established.

Definition of velocity error based on ST-EKF

The commonly used linear velocity error differential equation is defined as ²⁴

$\begin{array}{l}\delta {\dot{\text{v}}}_{eb}^n=\left[{\text{f}}^n\wedge \right]{\text{ϕ}}^n+{\text{C}}_b^n\delta {\text{f}}^b-\left[\left(2{\text{ω}}_{ie}^n+{\text{ω}}_{en}^n\right)\wedge \right]\delta {\text{v}}_{eb}^n\\ -\left[\left(2\delta {\text{ω}}_{ie}^n+\delta {\text{ω}}_{en}^n\right)\wedge \right]{\text{v}}_{eb}^n\end{array}$

1

where ${\varphi }^n$ and $\delta {\text{v}}_{eb}^n$ are attitude misalignment angle vector and velocity error vector, respectively; ${\text{C}}_b^n$ is the direction cosine matrix; ${\text{f}}^n$ is specific force vector in navigation frame; $\delta {\text{f}}^b$ is accelerometer bias vector; ${\text{ω}}_{ie}^n$ is the earth rotation vector; ${\text{ω}}_{in}^n$ is the angular rate vector of the navigation frame relative to the inertial frame; ${\text{ω}}_{en}^n={\text{ω}}_{in}^n-{\text{ω}}_{ie}^n$ ; $[\wedge ]$ represents the transformation of a vector into a skew symmetric matrix; $\delta {\text{ω}}_{ie}^n$ and $\delta {\text{ω}}_{en}^n$ are the errors of ${\text{ω}}_{ie}^n$ and ${\text{ω}}_{en}^n$ , respectively; ${\text{v}}_{eb}^n$ is the velocity in the real navigation frame.

Since (1) contains specific force term and $\delta {\text{v}}_{eb}^n={\stackrel{\sim }{\text{v}}}_{eb}^n-{\text{v}}_{eb}^n$ , the calculation accuracy of ${\stackrel{\sim }{\text{v}}}_{eb}^n$ is easily affected by the accuracy of specific force and attitude solution, which leads to the inconsistency of covariance estimation in the calculation of system matrix of the traditional integrated navigation system based on EKF, ²² thus damaging the accuracy of integrated navigation.

In ST-EKF, the velocity error is defined more strictly in the same coordinate system ^10,23

$\begin{array}{l}\delta v_{\varphi }^n={\text{C}}_b^n{\stackrel{\sim }{\text{C}}}_n^b{\stackrel{\sim }{v}}_{eb}^n-v_{eb}^n=({\stackrel{\sim }{v}}_{eb}^n-v_{eb}^n)+({\text{C}}_b^n{\stackrel{\sim }{\text{C}}}_n^b-\text{I}){\stackrel{\sim }{v}}_{eb}^n\\ =\delta v_{eb}^n+(\text{exp}({\text{ϕ}}^n\wedge )-\text{I}){\stackrel{\sim }{v}}_{eb}^n\\ \approx \delta v_{eb}^n-({\stackrel{\sim }{v}}_{eb}^n\wedge ){\text{ϕ}}^n\end{array}$

2

where $\delta v_{\varphi }^n$ is the new definition of velocity error; $\text{exp}(·)$ represents the exponent of the matrix.

The following formula can be obtained by differentiating the new velocity error state

$\begin{array}{l}\delta {\dot{\text{v}}}_{\varphi }^n=-[{\text{g}}^n\wedge +({\stackrel{\sim }{\text{v}}}_{eb}^n\wedge )({\stackrel{\sim }{\text{ω}}}_{ie}^n\wedge )]{\text{ϕ}}^n\\ -(2{\text{ω}}_{ie}^n\wedge +{\text{ω}}_{en}^n\wedge )\delta {\text{v}}_{\varphi }^n+ {\text{C}}_b^n({\text{∇}}^b+{\text{w}}_a)\\ +({\stackrel{\sim }{\text{v}}}_{eb}^n\wedge ){\text{C}}_b^n({\text{ε}}^b+{\text{w}}_g)+{\stackrel{\sim }{\text{v}}}_{eb}^n\wedge \delta {\text{ω}}_{ie}^n\end{array}$

3

where ${\text{g}}^n$ is the gravity vector in navigation frame; ${\text{ε}}^b$ is the vector of gyro constant biases; ${\text{∇}}^b$ is the vector of accelerometer constant biases; ${\text{w}}_g$ and ${\text{w}}_a$ are the white noises of the gyros and accelerometers, respectively.

It can be seen from (3) that the specific force term is no longer included, but is replaced by gravity vector related term. For general local land vehicle navigation problem, the variation of ${\text{g}}^n$ is very small. Therefore, integrated navigation based on ST-EKF can effectively avoid the problem of inaccurate calculation caused by specific force noise and drastic changes in attitude, and ensure the robustness of filtering. ^10,23

System error model of SINS/GNSS/LDV

The 20-dimensional error state vector x of SINS/GNSS/LDV integrated navigation system is defined as

$\text{x}={\left[\begin{array}{ccc}{\left({\text{x}}_{SINS}\right)}^T& ({\text{x}}_{GNSS})& {\left({\text{x}}_{LDV}\right)}^T\end{array}\right]}^T$

4

where ${\text{x}}_{SINS}$ , $x_{GNSS}$ , ${\text{x}}_{LDV}$ represent the error state vectors corresponding to SINS, GNSS, and LDV respectively, which can be expressed as

$\begin{array}{l}{\text{x}}_{SINS}=[\begin{array}{ccc}{({\mathit{\varphi }}^n)}^T& {(\delta {\text{v}}_{\varphi }^n)}^T& {(\delta {\text{r}}^n)}^T\end{array}\\ {\begin{array}{cc}{({\text{ε}}^b)}^T& {({\text{∇}}^b)}^T\end{array}]}^T\end{array}$

5

${\text{x}}_{GNSS}={\left[\begin{array}{cc}c\delta t_u& c\delta t_{ru}\end{array}\right]}^T$

6

${\text{x}}_{LDV}={\left[\begin{array}{ccc}\delta k_D& \delta {\alpha }_y& \delta {\alpha }_z\end{array}\right]}^T$

7

where $\delta {\text{r}}^n$ is the position error vector; $c\delta t_u$ is the equivalent distance error relates to the receiver clock error; $c\delta t_{ru}$ is the equivalent distance rate error relates to the clock drift; $\delta k_D$ is the scale factor error of LDV; $\delta \text{α}$ represents the installation error between SINS and vehicle body. Generally, only the pitch angle installation error $\delta {\alpha }_y$ and yaw angle installation error $\delta {\text{α}}_z$ is considered. $\delta {\alpha }_y$ is generally not constant because of the bumps in the road, so can be modeled as the first-order Markov process. Similarly, the scale factor error of LDV will also be unstable for long-distance land vehicle navigation, so it can also be modeled as first-order Markov process.

The system error equation of SINS/GNSS/ LDV integration can be constructed as

$\dot{\text{x}}=\text{Fx}+\text{Gw}$

8

where x is the error state vector; F is the state transition matrix; G is the noise shaping matrix; w is the process noise vector. These terms are defined as

$\text{F=}\left[\begin{array}{ccc}{\text{F}}_{SINS}& 0_{15\times 2}& 0_{15\times 3}\\ 0_{2\times 15}& {\text{F}}_{GNSS}& 0_{2\times 3}\\ 0_{3\times 15}& 0_{3\times 2}& {\text{F}}_{LDV}\end{array}\right]$

9

$\text{G}=\left[\begin{array}{ccc}{\text{G}}_{SINS}& 0_{15\times 2}& 0_{15\times 2}\\ 0_{2\times 6}& {\text{G}}_{GNSS}& 0_{2\times 2}\\ 0_{3\times 6}& 0_{3\times 2}& {\text{G}}_{LDV}\end{array}\right]$

10

$\begin{array}{l}\text{w=}[\begin{array}{cccc}{\left({\text{w}}_{bg}\right)}^T& {\left({\text{w}}_{ba}\right)}^T& w_{tu}& w_{tru}\end{array}\\ {\begin{array}{cc} w_{\delta kldv}& w_{\delta a_{yldv}}\end{array}]}^T\end{array}$

11

The elements in w respectively represent white noises of gyros, accelerometers, equivalent distance error relates to the receiver clock error, equivalent distance rate error relates to the clock drift, scale factor error Markov process, and pitch angle installation deviation error Markov process.

The submatrix related to each sensor in the system matrix F and the noise transfer matrix G can be expressed as

${\text{F}}_{SINS}=\left[\begin{array}{cc}{\text{F}}_{11}& {\text{F}}_{12}\\ {\text{0}}_{6\times 9}& {\text{0}}_{6\times 6}\end{array}\right]$

12

${\text{F}}_{11}=\left[\begin{array}{ccc}-({\text{ω}}_{in}^n\wedge )+{\text{F}}_{ev}({\stackrel{\sim }{\text{v}}}_{\mathrm{eb}}^n\wedge )& {\text{F}}_{ev}& {\text{F}}_{er}\\ -\left[\begin{array}{l}({\text{g}}^n\wedge )\\ +({\stackrel{\sim }{\text{v}}}_{\mathrm{eb}}^n\wedge )({\text{ω}}_{ie}^n\wedge )\end{array}\right]& -\left[\begin{array}{l}2({\text{ω}}_{ie}^n\wedge )\\ +({\text{ω}}_{en}^n\wedge )\end{array}\right]& ({\stackrel{\sim }{\text{v}}}_{\mathrm{eb}}^n\wedge )\delta {\text{Ω}}_r\\ {\text{F}}_{rv}({\stackrel{\sim }{\text{v}}}_{\mathrm{eb}}^n\wedge )& {\text{F}}_{rv}& {\text{F}}_{rr}\end{array}\right]$

13

${\text{F}}_{12}=\left[\begin{array}{cc}-{\text{C}}_b^n& {\text{0}}_{3\times 3}\\ ({\stackrel{\sim }{v}}_{\mathrm{eb}}^n\wedge ){\text{C}}_b^n& {\text{C}}_b^n\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\end{array}\right]$

14

${\text{F}}_{er}=\left[\begin{array}{ccc}-\Omega \text{sin}L& 0& -\frac{v_E}{{(R_E+h)}^2}\\ 0& 0& \frac{v_N}{{(R_N+h)}^2}\\ -\Omega \text{cos}L-\frac{v_E}{(R_E+h){\text{cos}}^{2}L}& 0& \frac{v_E\text{tan}L}{{(R_E+h)}^2}\end{array}\right]$

15

${\text{F}}_{ev}=\left[\begin{array}{ccc}0& \frac{1}{R_E+h}& 0\\ -\frac{1}{R_N+h}& 0& 0\\ 0& -\frac{\text{tan}L}{R_E+h}& 0\end{array}\right]$

16

${\text{F}}_{rr}=\left[\begin{array}{ccc}0& 0& -\frac{v_N}{{(R_N+h)}^2}\\ -\frac{v_E\text{tan}L}{(R_E+h)\text{cos}L}& 0& -\frac{v_E}{{(R_E+h)}^2\text{cos}L}\\ 0& 0& 0\end{array}\right]$

17

${\text{F}}_{rv}=\left[\begin{array}{ccc}\frac{1}{R_N+h}& 0& 0\\ 0& \frac{1}{(R_E+h)\text{cos}L}& 0\\ 0& 0& -1\end{array}\right]$

18

${\text{F}}_{GNSS}\text{=}\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]$

19

${\text{F}}_{LDV}\text{=}\left[\begin{array}{ccc}-1/{\tau }_{\delta k}& 0& 0\\ 0& -1/{\tau }_{\delta \alpha y}& 0\\ 0& 0& 0\end{array}\right]$

20

${\text{G}}_{SINS}=\left[\begin{array}{cc}-{\text{C}}_b^n& 0_{3\times 3}\\ \left[{\stackrel{\sim }{v}}_{\mathrm{eb}}^n\wedge \right]{\text{C}}_b^n& {\text{C}}_b^n\\ 0_{9\times 3}& 0_{9\times 3}\end{array}\right]$

21

${\text{G}}_{GNSS}={\text{Ι}}_{2\times 2}$

22

${\text{G}}_{LDV}=\left[\begin{array}{c}{\text{I}}_{2\times 2}\\ 0_{1\times 2}\end{array}\right]$

23

where ${\tau }_{\delta k}$ is the scale factor error correlation time of LDV; ${\tau }_{\delta \alpha y}$ is the pitch angle installation error correlation time of LDV; v_N , v_E are the north and east velocities respectively; R_N is the radius of curvature in the meridian; R_E is the radius of curvature in the prime vertical; L, h are the latitude and altitude respectively; $\Omega$ is the earth rotation rate.

System observation model

In GNSS-signal blockage environments, in order to design an UGV seamless navigation system with good reliability and positioning accuracy, this article introduces LDV as auxiliary navigation equipment to improve the stability and performance of SINS/GNSS tightly integrated navigation system, and the technique route is shown in Figure 1.

Figure 1.

Technique route of SINS/GNSS/LDV tightly coupled integration seamless navigation. SINS/GNSS/LDV: strapdown inertial navigation system/global navigation satellite system/laser Doppler velocimeter.

GNSS observation model

Tightly coupled systems is a high-level integration. The pseudo range and pseudo range rate is a commonly used tightly coupled mode. The original information after receiving the pseudo range and pseudo range rate signals provided by GNSS receiver. The errors of each pseudo range and pseudo range rate signal are independent and uncorrelated. The deeply coupled systems involves the internal design of the sensor, which is relatively complex and will not be discussed here. Therefore, this article mainly adopts the tightly coupled method to integrate SINS and GNSS information.

The pseudo range rate solved by SINS of the satellite with pseudo-random number (PRN) k is defined as

${\dot{\rho }}_{INS,k}={\text{e}}_k^T\cdot \left(v_{eb}^e-v_{k,sat}^e\right)$

24

where ${\text{e}}_k$ is the sight line vector from SINS to the satellite with PRN k, ${\text{v}}_{eb}^e$ is the velocity vector of SINS in ECEF coordinate system, ${\text{v}}_{k,sat}^e$ is the velocity vector of the satellite with PRN k in ECEF coordinate system.

The pseudo range solved by SINS of the satellite with PRN k is defined as

${\rho }_{INS,k}=\sqrt{{(x_I-x_{s,k})}^2+{(y_I-y_{s,k})}^2+{(z_I-z_{s,k})}^2}$

25

where ${\text{r}}_{k,SAT}^e=(x_{s,k},y_{s,k},z_{s,k})$ is the position of the satellite with PRN k in the ECEF coordinate system, ${\text{r}}_{eb}^e=(x_I,y_I,z_I)$ is the position of the SINS in the ECEF coordinate frame. The sight line vector ${\text{e}}_k$ is calculated as

$\begin{array}{l}{\text{e}}_k=\left[\begin{array}{c}{e}_{kx}\\ e_{ky}\\ e_{kz}\end{array}\right]=\left[\begin{array}{c}\frac{\partial r_k}{\partial x}\\ \frac{\partial r_k}{\partial y}\\ \frac{\partial r_k}{\partial z}\end{array}\right]\\ ={\left[\begin{array}{ccc}\frac{x-x_{s,k}}{r_k}& \frac{y-y_{s,k}}{r_k}& \frac{z-z_{s,k}}{r_k}\end{array}\right]}^T\end{array}$

26

where $(x,y,z)$ is the true position of the receiver in the ECEF coordinate system, r_k is the true distance between the satellite with PRN k and GNSS receiver

$\delta {\text{z}}_{GNSS}$ is defined as the difference between the pseudo range and pseudo range rate measured by SINS and GNSS in the integrated navigation system. If the number of observable satellites is r, $\delta {\text{z}}_{GNSS}$ can be expressed as

${\text{δz}}_{GNSS}=\left[\begin{array}{c}\delta {\text{z}}_{\rho ,\mathrm{1...}r}\\ \delta {\text{z}}_{\dot{\rho },\mathrm{1...}r}\end{array}\right]$

27

where the observation of pseudo range $\delta {\text{z}}_{\rho ,\mathrm{1...}r}$ and the observation of pseudo range rate $\delta {\text{z}}_{\dot{\rho },\mathrm{1...}r}$ can be defined as

$\begin{array}{l}\delta {\text{z}}_{\rho ,\mathrm{1...}r}=\left[\begin{array}{c}{\rho }_{INS,1}-{\rho }_{GNSS,1}\\ \mathrm{...}\\ {\rho }_{INS,r}-{\rho }_{GNSS,r}\end{array}\right]\\ =\left[\begin{array}{ccc}{e}_{1x}& e_{1y}& e_{1z}\\ \mathrm{...}& \mathrm{...}& \mathrm{...}\\ e_{rx}& e_{ry}& e_{rz}\end{array}\right]{\text{C}}_{\lambda Lh}^{ECEF}\left[\begin{array}{c}\delta L\\ \delta \lambda \\ \delta h\end{array}\right]+{\left[\begin{array}{c}c\delta t_u\\ \mathrm{...}\\ c\delta t_u\end{array}\right]}_{r\times 1}+{\left[\begin{array}{c}{n}_{\rho 1}\\ \mathrm{...}\\ n_{\rho r}\end{array}\right]}_{r\times 1}\\ ={\mathit{H}}_{\rho }\delta {\text{r}}^n+c\delta {\mathit{t}}_{u,r\times 1}+{\mathit{n}}_{\rho ,r\times 1}\end{array}$

28

$\begin{array}{l}\delta {\text{z}}_{\dot{\rho },\mathrm{1...}r}=\left[\begin{array}{c}{\dot{\rho }}_{INS,1}-{\dot{\rho }}_{GNSS,1}\\ \mathrm{...}\\ {\dot{\rho }}_{INS,r}-{\dot{\rho }}_{GNSS,r}\end{array}\right]\\ =\left[\begin{array}{ccc}{e}_{1x}& e_{1y}& e_{1z}\\ \mathrm{...}& \mathrm{...}& \mathrm{...}\\ e_{rx}& e_{ry}& e_{rz}\end{array}\right]{\text{C}}_{NED}^{ECEF}(\delta {\text{v}}_{\varphi }^n+({\stackrel{\sim }{\text{v}}}_{eb}^n\wedge ){\text{ϕ}}^n)\\ +{\left[\begin{array}{c}c\delta t_{ru}\\ \mathrm{...}\\ c\delta t_{ru}\end{array}\right]}_{r\times 1}+{\left[\begin{array}{c}{n}_{\dot{\rho }1}\\ \mathrm{...}\\ n_{\dot{\rho }r}\end{array}\right]}_{r\times 1}\\ ={\mathit{H}}_{\dot{\rho }1}{\text{ϕ}}^n+{\mathit{H}}_{\dot{\rho }2}\delta {\text{v}}_{\varphi }^n+c\delta {\text{t}}_{ru,r\times 1}+{\text{n}}_{\dot{\rho },r\times 1}\end{array}$

29

${\mathit{H}}_{\rho }=\left[\begin{array}{ccc}{e}_{1x}& e_{1y}& e_{1z}\\ \mathrm{...}& \mathrm{...}& \mathrm{...}\\ e_{rx}& e_{ry}& e_{rz}\end{array}\right]{\text{C}}_{\lambda Lh}^{ECEF}$

30

${\mathit{H}}_{\dot{\rho }1}=\left[\begin{array}{ccc}{e}_{1x}& e_{1y}& e_{1z}\\ \mathrm{...}& \mathrm{...}& \mathrm{...}\\ e_{rx}& e_{ry}& e_{rz}\end{array}\right]{\text{C}}_{NED}^{ECEF}({\stackrel{\sim }{v}}_{eb}^n\wedge )$

31

${\mathit{H}}_{\dot{\rho }2}\text{=}\left[\begin{array}{ccc}{e}_{1x}& e_{1y}& e_{1z}\\ \mathrm{...}& \mathrm{...}& \mathrm{...}\\ e_{rx}& e_{ry}& e_{rz}\end{array}\right]$

32

where $C_{\lambda Lh}^{ECEF}$ is the transfer matrix that projects the latitude, longitude, and altitude errors into the ECEF coordinate system, $n_{\rho ,r\times 1}$ is the measurement noise vector of the pseudo range, and $n_{\dot{\rho },r\times 1}$ is the measurement noise vector of the pseudo range rate.

LDV observation model

Assume that the installation angles between IMU and the vehicle body are small angles, thus direction cosine matrix ${\stackrel{\sim }{\text{C}}}_b^m$ can be written as

${\stackrel{\sim }{\text{C}}}_b^m=\left[\text{I}-\left(\text{δα}\wedge \right)\right]{\text{C}}_b^m$

33

where $\text{δα}$ is the installation error angle between sins and vehicle body.

The projection of the velocity solved by the SINS in the vehicle frame (m frame) can be expressed as

$\begin{array}{l}{\stackrel{\sim }{\text{v}}}_{eb}^m = {\stackrel{\sim }{\text{C}}}_b^m{\stackrel{\sim }{\text{C}}}_n^b{\stackrel{\sim }{\text{v}}}_{eb}^n\\ = {\stackrel{\sim }{\text{C}}}_b^m\left[{\text{C}}_n^b\left(\text{I}+\left[{\text{ϕ}}^n\wedge \right]\right)\left({\text{v}}_{eb}^n+\delta {\text{v}}_{\varphi }^n+\left[{\stackrel{\sim }{\text{v}}}_{eb}^n\wedge \right]{\text{ϕ}}^n\right)\right]\end{array}$

34

The LDV measurement vector $\delta {\text{z}}_{LDV}$ can be obtained by computing the difference between ${\stackrel{\sim }{\text{v}}}_{eb}^m$ and the LDV velocity under m frame

$\begin{array}{l}\delta {\text{z}}_{LDV}={\stackrel{\sim }{\text{v}}}_{eb}^m-{\stackrel{\sim }{\text{v}}}_{LDV}^m\\ \approx {\text{C}}_n^m({\text{ϕ}}^n\wedge ){\text{v}}_{eb}^n+{\text{C}}_n^m({\stackrel{\sim }{\text{v}}}_{eb}^n\wedge {\text{ϕ}}^n)+{\text{C}}_n^m\delta {\text{v}}_{\varphi }^n\\ -{\text{v}}_{LDV}^m\delta k_{LDV}+{\text{v}}_{LDV}^m\wedge \delta \text{α}\\ \approx {\text{C}}_n^m\delta {\text{v}}_{\varphi }^n-\delta k_{LDV}{\text{v}}_{LDV}^m+{\text{v}}_{LDV}^m\wedge \delta \text{α}\end{array}$

35

where ${\stackrel{\sim }{\text{C}}}_b^m\approx \text{I}$ , ${\text{v}}_{LDV}^m$ is the velocity vector of LDV under m frame.

Observation model for SINS/GNSS/LDV

Combined with (27) and (35), the observation equation of SINS/GNSS/LDV integrated navigation system can be defined as

$\delta \text{z}=\left[\begin{array}{c}\delta {\text{z}}_{GNSS}\\ \delta {\text{z}}_{LDV}\end{array}\right]=\text{Hx}+{\text{v}}_{\left(2\times r+3\right)\times 1}$

36

where ${\text{v}}_{\left(2\times r+3\right)\times 1}=\left[\begin{array}{c}{\text{v}}_{GNSS}\\ {\text{v}}_{LDV}\end{array}\right]$ is the observation noise vector; $\text{H}=\left[\begin{array}{c}{\text{H}}_{GNSS}\\ {\text{H}}_{LDV}\end{array}\right]$ is the observation matrix.

${\text{H}}_{GNSS}=\left[\begin{array}{ccccccc}{\text{0}}_{r\times 3}& {\text{0}}_{r\times 3}& {\text{H}}_{\rho }& {\text{0}}_{r\times 6}& {\text{1}}_{r\times 1}& {\text{0}}_{r\times 1}& {\text{0}}_{r\times 3}\\ {\text{H}}_{\dot{\rho }1}& {\text{H}}_{\dot{\rho }2}& {\text{0}}_{r\times 3}& {\text{0}}_{r\times 6}& {\text{0}}_{r\times 1}& {\text{1}}_{r\times 1}& {\text{0}}_{r\times 3}\end{array}\right]$

37

${\text{H}}_{LDV}=\left[\begin{array}{cccc}{\text{0}}_{3\times 3}& {\text{C}}_b^m{\text{C}}_n^b& {\text{0}}_{3\times 11}& {\text{M}}_{v-LDV}^b\end{array}\right]$

38

${\text{M}}_{v-LDV}^b=\left[\begin{array}{ccc}-v_{LDV}& 0& 0\\ 0& 0& -v_{LDV}\\ 0& v_{LDV}& 0\end{array}\right]$

39

where $v_{LDV}$ is the forward velocity measured by LDV.

Experimental design

In order to analyze and evaluate the application effect and improvement of SINS/GNSS/LDV seamless navigation technique based on ST-EKF, three groups of land vehicle experiments under different driving environments are designed as follows

In GNSS-denied environment (such as tunnel), the positioning accuracy of pure inertial navigation system, SINS/LDV based on EKF, and SINS/LDV based on ST-EKF are compared, respectively;

In the challenge environment with less than four observable satellites, the positioning accuracy comparison of SINS/GNSS tightly integration based on EKF and SINS/GNSS tightly integration based on ST-EKF;

In the task of long-distance driving across cities, the positioning accuracy of SINS/GNSS tightly integration (EKF), SINS/GNSS tightly integration (ST-EKF), and SINS/GNSS/LDV seamless navigation (ST-EKF) are compared, respectively.

The experimental land vehicle is equipped with navigation-grade high precision FOGIMU, NEO-M8T GNSS receiver and RLDV-300 LDV. The material object of the sensors is shown in Figure 2. The sensor configuration and working mode of the experimental vehicle are shown in Figure 3. The specifications of the sensors are listed in Table 1.

Figure 2.

(a) FOG-IMU; (b) RLDV-300 LDV; (c) NEO-M8T GNSS receiver.

Figure 3.

Sensor configurations and working mode of the land vehicle experiment.

Table 1.

Specifications of the sensors.

Sensors type	Index	Parameters
Gyroscope	Measurement range ( ${ }^{\circ }/s$ )	200
	Bias stability ( ${ }^{\circ }/h$ )	$\le$ 0.002
	Angle random walk ( ${ }^{\circ }/\sqrt{h}$ )	$\le$ 0.0003
Accelerometer	Measurement range (g)	±20 (Y, Z), ±35 (X)
	Bias stability ( $\mu g$ )	$\le$ 10
	Velocity random walk ( $\mu g/\sqrt{Hz}$ )	$\le$ 5
LDV	Measurement accuracy (%)	±0.1
	Measurement range (m/s)	0 to 50
	Operating Temperature (°C)	−20 to 50
	Measurement repeatability (%)	0.05

The land vehicle carried out initial alignment with stationary period 180s. The reference position of the vehicle is calculated by the post smoothing algorithm of SINS/differential satellite integrated navigation, which is used for the subsequent comparison and analysis of positioning accuracy.

Experimental result

Land vehicle experiment in GNSS-denied environment

In the environment of high shielding such as tunnel, UGV cannot receive GNSS information, SINS/GNSS integrated navigation system degenerates into a pure inertial system, and the navigation error will accumulate rapidly with time. The method adopted in this article is to maintain the positioning accuracy of UGV by introducing the measurement information of LDV. Therefore, this experiment mainly verifies the application effect and improvement of SINS/LDV integrated navigation algorithm based on ST-EKF in GNSS-denied environment.

This land vehicle experiment was carried out on Changsha Beltway, with a driving time of 1400s and a cumulative distance of about 21 km. The driving trajectory of this experiment is shown in Figure 4.

Figure 4.

Two dimensional trajectory of the land vehicle experiment (group 1).

The horizontal position error of the three algorithms are shown in Figure 5. Table 2 shows RMSE of the horizontal position error.

Figure 5.

Horizontal position errors in the land vehicle experiments (group 1).

Table 2.

Statistical result of navigation error (group 1).

RMSE		SINS/LDV (EKF)	SINS/LDV (ST-EKF)
Position (m)	Northward	15.0942	9.1581
	Eastward	5.8610	5.2490

SINS/LDV: strapdown inertial navigation system/laser Doppler velocimeter integration; ST-EKF: state transformation extended Kalman filter.

It can be found from Figure 5 and Table 2, the navigation error of pure inertial navigation system accumulates rapidly in GNSS-denied environment, while SINS/LDV integrated navigation can effectively suppress the divergence of error. At the same time, SINS/LDV (ST-EKF) algorithm has higher positioning accuracy than SINS/LDV (EKF) under the same conditions, which can better help UGV complete navigation tasks in the absence of satellite information.

Land vehicle experiment in GNSS-signal blockage environment

In GNSS-signal blockage environment, the number of observable satellites received by UGV may be affected, thus affecting the positioning accuracy of SINS/GNSS integrated navigation system. This experiment mainly compares and verifies the positioning accuracy of SINS/GNSS and SINS/GNSS/LDV tightly integrated navigation algorithm based on ST-EKF when there are less than four observable satellites.

This land vehicle experiment was carried out on Hua-Chang expressway, with a driving time of 900s and a cumulative distance of about 21 km. The driving trajectory of this experiment is shown in Figure 6. During the driving process, the land vehicle has experienced different satellite observation conditions. From 1 to 200s, there are only three satellites available (from 0 to 4.5 km). From 201 to 300s, there are only two satellites available (from 4.5 to 6.7 km). From 351 to 550s, there are only one satellites available (from 7.9 to 13 km). During the rest of the land vehicle experiment, there are six observable satellites.

Figure 6.

Two dimensional trajectory of the land vehicle experiment (group 2).

The horizontal position error of the three algorithms are shown in Figure 7. Table 3 shows RMSE of the horizontal position error.

Figure 7.

Horizontal position errors in the land vehicle experiments (group 2).

Table 3.

Statistical result of navigation error (group 2).

RMSE		SINS/GNSS (EKF)	SINS/GNSS (ST-EKF)	SINS/GNSS/LDV (ST-EKF)
Position(m)	Northward	1.3824	1.3824	1.1569
	Eastward	1.0774	1.0132	0.5227

SINS/GNSS/LDV: strapdown inertial navigation system/global navigation satellite system/laser Doppler velocimeter integration; ST-EKF: state transformation extended Kalman filter.

It can be found from Figure 7 and Table 3, SINS/GNSS (ST-EKF) tightly integrated algorithm has higher positioning accuracy than SINS/GNSS (EKF) under the same conditions, that is, it can better adapt to the adverse conditions of insufficient observable satellites.

At the same time, after introducing LDV information, the positioning accuracy has been further improved, which further proves the effectiveness of the seamless navigation algorithm proposed in this paper in GNSS-signal blockage environment.

Land vehicle experiment for long distance

When UGV performs long-distance driving tasks, it may experience various complex and challenging environments due to its large travel span, which puts forward higher requirements for the seamless navigation ability of UGV navigation system in different environments. Therefore, this experiment mainly compares and verifies the environmental adaptability and positioning accuracy of SINS/GNSS/LDV seamless navigation technique based on ST-EKF in long-distance driving tasks with a distance of more than 200 km.

This land vehicle experiment starts from Yueyang and ends in Changsha, passing through three cities, with a driving time of about 4.2 h and a cumulative distance of 227 km. In the process of driving, there are many different complex scenes. It is a challenge for land vehicle to achieve high-precision navigation and positioning when it pass through tunnels and urban roads with serious signal blocked. The driving trajectory of this experiment is shown in Figure 8. The special scene encountered by the satellite in the process of land vehicle driving can be shown in Figure 9.

Figure 8.

Two dimensional trajectory of the land vehicle experiment (group 3).

Figure 9.

Satellite signal status of the land vehicle experiment (group 3).

Figure 10 show the variation trend of position error and velocity error with time.

Figure 10.

Position errors and velocity errors in the land vehicle experiments (group 3).

In order to evaluate the horizontal positioning accuracy of the three integrated navigation algorithms more intuitively, Figure 11 shows the variation trend of the horizontal positioning error and accuracy of these algorithms. Table 4 shows RMSE and the percentage of RMSE in the total distance of these algorithms.

Figure 11.

Comparison of horizontal positioning results (group 3).

Table 4.

Statistical result of navigation error (group 3)

RMSE		SINS/GNSS (EKF)	SINS/GNSS (ST-EKF)	SINS/GNSS/LDV (ST-EKF)
Position (m)	Northward	4.7871	4.7145	3.1689
	Eastward	3.9184	3.8955	3.8176
Velocity (m/s)	Northward	0.1396	0.1395	0.1361
	Eastward	0.1450	0.1448	0.1489

SINS/GNSS/LDV: strapdown inertial navigation system/global navigation satellite system/laser Doppler velocimeter integration; ST-EKF: state transformation extended Kalman filter.

By observing Figures 10 to 11 and Table 4, it can be found that after adopting SINS/GNSS/LDV seamless navigation technique, the land vehicle navigation system has stronger environmental adaptability and reliability. Compared with the traditional SINS/GNSS tightly coupled integration navigation technique, it can maintain high horizontal positioning accuracy in the GNSS-signal blockage environments, which proves that the technique has value of helping UGV realize seamless navigation.

Conclusions

Aiming at the problem of UGV navigation in the GNSS-signal blockage environments, this article proposes a SINS/GNSS/LDV tightly coupled seamless navigation technique. The post-processing of the measured vehicle experimental data shows that this technique has good reliability and can maintain high navigation and positioning accuracy even when the GNSS information cannot be received normally. Therefore, this technique can help UGV maintain enough navigation accuracy in the GNSS-signal blockage environments and realize seamless navigation in the real sense, which has good engineering application value.