STRICT REVERSE NAVIGATION METHOD FOR OPTIMAL ESTIMATION OF FINE ALIGNMENT

Abstract

A strict reverse navigation method for optimal estimation of fine alignment is provided. The strict reverse navigation method including: establishing an adaptive control function; performing a forward navigation calculation process; performing a reverse navigation calculation process; and performing the adaptive control for a number of forward and reverse calculations. The strict reverse navigation method shortens an alignment time for the optimal estimation of fine alignment while ensuring an alignment accuracy. The strict reverse navigation method provided effectively solves a problem that an error of an initial value of filtering in an initial stage of the optimal estimation of fine alignment affects convergence speeds of subsequent stages. In the initial stage, a larger number of the forward and reverse navigation calculations are adopted to reduce an error of the initial value as much as possible and increase a convergence speed of the filtering.

Description

TECHNICAL FIELD

The present invention relates to the field of navigation technologies, and in particular, to a strict reverse navigation method for optimal estimation of fine alignment.

BACKGROUND

A general navigation calculation process is a process in which data sampled by a gyroscope and an accelerometer in a strap-down inertial navigation system is processed in sequence as a set of time series. The process is performed in real time, and storage of data is not required in the process. With the continuous development of contemporary computer technologies, data storage capabilities and computing capabilities of computers have been greatly improved. Data sampled in the entire navigation process may be stored. In addition to conventional forward operations, the stored data may also be processed in a reverse chronological order. In a navigation scene without real-time requirements, repeated forward and reverse analysis and calculations of the stored data are beneficial to improvement of a navigation accuracy. In this process, in order to simplify the calculation, a small-angle approximation is often adopted for an angular speed of reverse rotation when a posture matrix is reversely updated. As a result, errors of each iteration are accumulated. When there is a small number of forward and reverse calculations, an approximation error has basically no effect on the result. When the number of forward and reverse calculations is larger and reaches a certain number of times, errors of the repeated calculations are to be increased to the same multiple as the number of times, resulting in convergent oscillation and even failure of convergence of the result due to drifting, which ultimately affects the accuracy of navigation and alignment. The strict reverse process adopts a strict reverse derivation process. There is no small-angle approximation and the solution is accurate. Repeated forward and reverse calculations can still ensure an accuracy, avoid the accumulation of approximation errors, and achieve rapid convergence of high-precision alignment, but the amount of calculations is larger than that of an ordinary reverse process.

Before performing normal navigation, the strap-down inertial navigation system needs to establish an initial value of a strap-down matrix to complete an initial alignment process of a system. When the system is in a static state, the initial alignment process is relatively easy to achieve. However, for the field of water surface and underwater navigation, the initial alignment process is to interfered by various external conditions, which does not conform to alignment conditions of a static base, and generally requires auxiliary information provided by other devices to complete initial alignment of a moving base. A prerequisite for the initial alignment research is to establish a reasonable strap-down inertial navigation system error model. In general, the initial alignment error model is essentially a set of non-linear equations. In a traditional process of initial alignment error modelling with a small misalignment angle, linearized approximation is performed on a non-linear model, then the linear error model is established with the small misalignment angle on this basis, and an optimal estimation value is given by using a Kalman filter. This process is common optimal estimation of alignment. The optimal estimation method through the Kalman filter is currently a typical method adapted to the optimal estimation method of fine alignment. According to the currently available information, reverse navigation algorithms are mostly used in compass alignment scenarios. The greatest advantage of compass alignment is a small number of calculations and allows enough forward and reverse iterations, but requires a relatively long alignment time. However, a larger number of calculations is required for the optimal estimation of alignment.

SUMMARY

In order to solve the above problems, the present invention provides a strict reverse navigation calculation method with process control, which is expected to reduce an error of an initial value of optimal estimation of fine alignment, accelerate subsequent alignment, save an alignment time, and reduce an amount of calculations as much as possible. According to the method of the present invention, adaptive control is performed on a number of forward and reverse navigation calculations in different time periods in the alignment process.

To achieve the foregoing objective, the present invention provides the following technical solutions:

A strict reverse navigation method for optimal estimation of fine alignment, including the following steps.

Step 1: Establish an Adaptive Control Function.

A total sampling time is denoted as m, time periods in a sampling process are denoted as a sequence t₁, t₂, t₃, . . . , a number of forward and reverse calculations in each of the time periods is denoted as q₁, q₂, q₃, . . . , and an expression of the control function is denoted as:

$q_i=\alpha \frac{m}{t_i},i\in \left[1,k\right]$

where α is a control coefficient for a number of times.

Step 2: Perform a Forward Navigation Calculation Process.

A forward navigation process includes updating a posture, a speed, and a position, and specifically:

an earth-centered inertial coordinate system is denoted as a system i, a terrestrial coordinate system is denoted as a system e, an east, north, up (ENU) coordinate system is selected as a navigation coordinate system and is denoted as a system n, and a vehicle coordinate system is denoted as a system b.

A forward posture updating process is:

C_bkⁿ=C_bk−1ⁿ(I+T_sΩ_nbk^b), where C_bⁿ is a posture matrix, T_s is a sampling period of a strap-down inertial navigation system, Ω_nbk^b=(∩_nbkⁿx), where (●x) represents an antisymmetric matrix composed of a vector ●, ω_bk^b=ω_ibk^b−(C_bk−1ⁿ)^T(ω_iek−1ⁿ+ω_enk−1ⁿ), where ω_ib^b represents an angular speed measurement from a gyroscope, ω_iekⁿa=[0 ω_ie cos L_k ω_ie sin L_k]^T, where ω_ie is an angular speed of Earth's rotation, L represents a latitude, and

${\omega }_{\mathrm{enk}}^n={\left[-\frac{v_{\mathrm{Nk}}^n}{R_M+h_k}\frac{v_{\mathrm{Ek}}^n}{R_N+h_k}\frac{v_{\mathrm{En}}^n\tan L_k}{R_N+h_k}\right]}^T,$

where V_N and V_E respectively represent a northward speed and an eastward speed, R_M and R_N are respectively a radius of a meridian of the Earth in a locality and a radius of a prime vertical, h is a height, and k=1, 2, 3, . . . .

A forward speed updating process is:

v_n^k=v_k−1ⁿ+T_s[C_bk−1ⁿf_sfk^b−(2ω_iek−1ⁿ+ω_enk−1ⁿ)×v_k−1ⁿ+gⁿ] where vⁿ=[v_Eⁿ v_Nⁿ v_Uⁿ]^T represents a speed, v_U represents an upward speed, f_sf^b represents a specific force measurement from an accelerometer, and gⁿ represents gravitational acceleration.

A forward position updating process is:

$L_k=L_{k-1}+\frac{T_s{v}_{\mathrm{Nk}-1}^n}{R_M+h_{k-1}}{\lambda }_k={\lambda }_{k-1}+\frac{T_s{v}_{\mathrm{Ek}-1}^n\sec L_{k-1}}{R_N+h_{k-1}},h_k=h_{k-1}+T_s{v}_{\mathrm{Uk}-1}^n,$

where λ represents a longitude.

Step 3: Perform a Reverse Navigation Calculation Process.

A reverse navigation process includes updating a posture, a speed, and a position, and specifically: a reverse posture updating process is:

C _bk−1 ⁿ =C _bk ⁿ(I+T_s+{tilde over (Ω)}_nbk−1^b) where {tilde over (Ω)}_nbk−1^b=−Ω_nbk[I+T_sΩ_nbk]⁻¹;

a reverse speed updating process is:

−v_k−1ⁿ=−v_kⁿ+T_sã_k−1,kⁿ, where ã_k−1,kⁿ=a_k,k−1ⁿ=C_bk−1ⁿf_sfk^b−(2ω_iek−1ⁿ+ω_enk−1ⁿ)×v_k−1ⁿ+gⁿ;

by defining ← as a way to represent a reverse direction, parameters in the reverse processes are obtained as follows: _bm−jⁿ=C_bjⁿ, _m−jⁿ=−v_jⁿ, _m−j=L_j, _m−j=λ_j, _m−j=h_j, _sfm−jⁿ=f_sfiⁿ, _k−1,kⁿ=a_k,k−1ⁿ, _iem−jⁿ=−ω_iejⁿ, _enm−jⁿ=−ω_enjⁿ, and _nbm−j^b={tilde over (Ω)}_nbj^b, further let p=m−k+1, and the following subscript conversions occur:

C _bk−1 ⁿ =C _bm−p ⁿ=_bpⁿC_bkⁿ=C_bm+1−pⁿ=_bp−1ⁿ,{tilde over (Ω)}_nbk−1ⁿ=_nbp^b, and therefore

the reverse posture updating is written as:

_bp ⁿ=_bp−1ⁿ(I+T_snbp^b)

the reverse speed updating is written as:

_p ⁿ=_p−1ⁿ+T_sp−1,p

the reverse position updating is written as:

${\stackrel{\leftharpoonup }{L}}_p={\stackrel{\leftharpoonup }{L}}_{p-1}+\frac{T_s{\stackrel{\leftharpoonup }{v}}_{\mathrm{Np}-1}^n}{R_M+{\stackrel{\leftharpoonup }{h}}_{p-1}}{\stackrel{\leftharpoonup }{\lambda }}_p={\stackrel{\leftharpoonup }{\lambda }}_{p-1}+\frac{T_s{\stackrel{\leftharpoonup }{v}}_{\mathrm{Ep}-1}^n\sec {\stackrel{\leftharpoonup }{L}}_{p-1}}{R_N+{\stackrel{\leftharpoonup }{h}}_{p-1}},{\stackrel{\leftharpoonup }{h}}_p={\stackrel{\leftharpoonup }{h}}_{p-1}+T_s{\stackrel{\leftharpoonup }{v}}_{\mathrm{Up}-1}^n.$

Step 4: Perform Adaptive Control for the Number of Forward and Reverse Calculations.

The adaptive control is performed for the number of forward and reverse calculations within a period of time through the control function

$q_i=\alpha \frac{m}{t_i},$

where q_i represents the number of forward and reverse calculations, and after the forward and reverse navigation calculations within the period of time are completed, a final result value is used as an initial value of a next stage, which is repeatedly performed until the alignment process is completed.

Further, α is 50, and a duration of each stage is 30-60 s.

Compared with the prior art, the present invention has the following advantages and beneficial effects:

1. The present invention shortens an alignment time for optimal estimation of fine alignment while ensuring an alignment accuracy. The method provided in the present invention can effectively solve the problem that an error of an initial value of filtering in an initial stage of the optimal estimation of fine alignment affects convergence speeds of subsequent stages. In the initial stage, a larger number of forward and reverse navigation calculations are adopted to reduce the error of the initial value as much as possible and increase a convergence speed of the filtering. In the subsequent stages, the number of forward and reverse calculations is gradually decreased as the accuracy of the filtering is continuously improved.

2. Since the reverse calculations are strict reverse processes, approximation errors are effectively avoided, and the error is not increased even after a plurality of forward and reverse calculations, thus quickly reducing the error of the initial value and accelerating convergence in the subsequent stages.

3. The present invention adaptively controls the number of forward and reverse navigation calculations, which can control the amount of calculations while ensuring a better basic accuracy and shorter alignment time, and bring faster response speed and better system performance to a navigation system through effective control of the amount of calculations.

BRIEF DESCRIPTION OF THE DRAWINGS

FIGURE is a schematic flowchart of a strict reverse navigation method for optimal estimation of fine alignment according to the present invention.

DETAILED DESCRIPTION OF THE EMBODIMENTS

The following describes the technical solutions provided in the present invention in detail with reference to specific embodiments. It should be understood that the following specific implementations are merely intended to describe the present invention rather than to limit the scope of the present invention.

According to a strict reverse navigation method for optimal estimation of fine alignment provided in the present invention, in an initial stage of the optimal estimation of fine alignment, a coarse initial value may slow down convergence, and therefore a large number of forward and reverse calculations is required in the initial period to reduce errors of the initial value and accelerate subsequent convergence. After the forward and reverse calculations in this stage are completed, a result is used as an initial value of a next stage, and the forward and reverse navigation calculations are continued. As time goes by, the navigation accuracy is continuously improved, and the number of forward and reverse calculations is also decreased. According to the method, it is assumed that moments of the whole discretization is from t₁ to t_n, and a standard forward navigation algorithm is divided into updating a posture, a speed, and a position of a strap-down inertial navigation system, and a strict reverse navigation algorithm is transposition and processing based on the forward algorithm, that is, the postures, the speeds, and the positions from t_k to t₁ are updated. Specifically, as shown in the FIGURE, the method of the present invention includes the following steps.

Step 1: Establish an Adaptive Control Function.

A total sampling (alignment) time is denoted as m, time periods in a sampling process are denoted as a sequence t₁, t₂, t₃, . . . , and a number of forward and reverse calculations in each of the time periods is denoted as q₁, q₂, q₃, . . . , and it is found that a relationship between the sampling time and the number of forward and reverse calculations can better fit an inversely proportional relationship through observation, and therefore the control function expression is denoted as:

$q_i=\alpha \frac{m}{t_i},i\in \left[1,k\right],$

where α is a control coefficient for a number of times, which is determined by a computer main frequency. Using 2.5 GHz as an example, α may be 50. From the expression of the control function, it can be seen that q_i is decreased from fast to slow as t_i is continuously increased. This means that as time goes by, the number of forward and reverse calculations is gradually decreased. Since in an initial stage of the optimal estimation of fine alignment, a coarse initial value may slow down convergence, and therefore a large number of forward and reverse calculations is required in the initial period to reduce the error of the initial value and accelerate subsequent convergence. As the alignment accuracy is improved, the number of forward and reverse calculations is gradually decreased. In addition, it should be noted that the calculation accuracy of each stage of t₁, t₂, t₃, . . . is to be used as the initial value of a next stage to continue the iterative calculation, which can greatly accelerate convergence. t_i is selected depending on the actual scene. A duration of each stage is generally 30-60 s.

Step 2: Perform a Forward Navigation Calculation Process.

A forward navigation process includes updating a posture, a speed, and a position, and specifically: an earth-centered inertial coordinate system is denoted as a system i, a terrestrial coordinate system is denoted as a system e, an east, north, up (ENU) coordinate system is selected as a navigation coordinate system and is denoted as a system n, and a vehicle coordinate system is denoted as a system b.

A forward posture updating process is: C_bkⁿ=C_bk−1ⁿ(I+T_sΩ_nbk^b), where C_bⁿ is a posture matrix, T_s is a sampling period of a strap-down inertial navigation system, Ω_nbk^b=(ω_nbkⁿx), where (●x represents an antisymmetric matrix composed of a vector ●, ω_nbk^b=ω_ibkⁿ−(C_bk−1^b)^T(ω_iek−1ⁿ+ω_enk−1ⁿ), where ω_ib^b represents an angular speed measurement from a gyroscope, ω_iekⁿ[0ω_ie cos L_k ω_ie sin L_k]^T, where ω_ie is an angular speed of Earth's rotation, L represents a latitude, and

${\omega }_{\mathrm{enk}}^n={\left[-\frac{v_{\mathrm{Nk}}^n}{R_M+h_k}\frac{v_{\mathrm{Ek}}^n}{R_N+h_k}\frac{v_{\mathrm{En}}^n\tan L_k}{R_N+h_k}\right]}^T,$

where VN and VE respectively represent a northward speed and an eastward speed, R_M and R_N are respectively a radius of a meridian of the Earth in a locality and a radius of a prime vertical, h is a height, and k=1, 2, 3, . . . . A forward speed updating process is: v_kⁿ=v_k−1ⁿ+T_s[C_bk−1ⁿf_sfk^b−(2ω_iek−1ⁿ+ω_enk−1ⁿ)×v_k−1ⁿ+gⁿ], where vⁿ=[v_Eⁿ v_Nⁿv_Uⁿ]^T represents a speed, v^U represents an upward speed, f_sf^b represents a specific force measurement from an accelerometer, and gⁿ represents gravitational acceleration.

A forward position updating process is:

$L_k=L_{k-1}+\frac{T_s{v}_{\mathrm{Nk}-1}^n}{R_M+h_{k-1}},{\lambda }_k={\lambda }_{k-1}+\frac{T_s{v}_{\mathrm{Ek}-1}^n\sec L_{k-1}}{R_N+h_{k-1}},$

h_k=h_k−1+T_sv_Uk−1ⁿ, where λ represents a longitude.

Step 3: Perform a Reverse Navigation Calculation Process.

The present invention derives the strict reverse navigation process, and specifically:

a reverse posture updating process is:

C _bk−1 ⁿ =C _bk ⁿ(I+T_s+{tilde over (Ω)}_nbk−1^b) where {tilde over (Ω)}_nbk−1^b=−Ω_nbk[I+T_sΩ_nbk]⁻¹;

a reverse speed updating process is:

−v_k−1ⁿ=−v_kⁿ+T_sã_k−1,kⁿ, where ã_k−1,kⁿ=a_k,k−1ⁿ=C_bk−1ⁿf_sfk^b−(2ω_iek−1ⁿ+ω_enk−1ⁿ)×v_k−1ⁿ+gⁿ;

by defining ← as a way to represent a reverse direction, parameters in the reverse processes are obtained as follows: _bm−jⁿ=C_bjⁿ, _m−jⁿ=−v_jⁿ, _m−j=L_j, _m−j=λ_j, _m−j=h_j, _sfm−jⁿ=f_sfiⁿ, _k−1,kⁿ=a_k,k−1ⁿ, _iem−jⁿ=−ω_iejⁿ, _enm−jⁿ=−ω_enjⁿ, and _nbm−j^b={tilde over (Ω)}_nbj^b, further let p=m−k+1, and the following subscript conversions occur:

C _bk−1 ⁿ =C _bm−p ⁿ=_bpⁿC_bkⁿ=C_bm+1−pⁿ=_bp−1ⁿ, and {tilde over (Ω)}_nbk−1ⁿ=_nbp^b, and therefore

the reverse posture updating is written as:

_bp ⁿ=_bp−1ⁿ(I+T_snbp^b)

the reverse speed updating is written as:

_p ⁿ=_p−1ⁿ+T_sp−1,p

the reverse position updating is written as:

${\overleftarrow{L}}_p={\overleftarrow{L}}_{p-1}+\frac{T_s{\overleftarrow{v}}_{\mathrm{Np}-1}^n}{R_M+{\overleftarrow{h}}_{p-1}}{\overleftarrow{\lambda }}_p={\overleftarrow{\lambda }}_{p-1}+\frac{T_s{\overleftarrow{v}}_{\mathrm{Ep}-1}^n\sec {\overleftarrow{L}}_{p-1}}{R_N+{\overleftarrow{h}}_{p-1}},\mathrm{and}{\overleftarrow{h}}_p={\overleftarrow{h}}_{p-1}+T_s{\overleftarrow{v}}_{\mathrm{Up}-1}^n.$

By intuitively comparing the forward and reverse navigation algorithms, it can be found that representation forms of the algorithms are consistent. The items that need to be changed include taking an inverse of a sign of the angular speed of Earth's rotation, an antisymmetric matrix of an angular speed is obtained from a virtual gyro, and reverse processing is finally performed on the sampled data to achieve the strict reverse navigation calculation from t_k to t₁.

Step 4: Perform Adaptive Control for the Number of Forward and Reverse Calculations.

The number of forward and reverse calculations within a time period is adaptively controlled via the current alignment time. The control function is

$q_i=\alpha \frac{m}{t_i},i\in \left[1,k\right],$

where q_i represents the number of forward and reverse calculations, and α is a control coefficient determined by a computer main frequency. Using 2.5 GHz as an example, α may be 50, m is the total sampling (alignment) time, and t₁ represents the current time stage. After the forward and reverse navigation calculations within the period of time are completed, a final result value is used as an initial value of a next stage, which is repeatedly performed until the alignment process is completed.

According to the method of the present invention, forward and reverse navigation calculations are performed based on the sampled data of the gyroscope and accelerometer in the strap-down inertial navigation system. By virtue of powerful navigation computer storage capabilities and calculation capabilities, the reverse navigation algorithm implements processing of the sampled data in reversed order. Further, repeated forward and reverse analysis are performed on stored sampled data in a time period can effectively improve the accuracy of analysis. The reverse navigation algorithm in the present invention adopts strict reverse derivation, and there is no approximate error of the repeated forward and reverse navigation calculation process, so that the alignment accuracy of the algorithm can be guaranteed. The present invention adaptively controls the number of forward and reverse calculations in different stages. In the initial stage of the optimal estimation of fine alignment, the selection of the initial filter value generally affects a convergence speed of the filtering, and the coarse initial value may slow down the convergence. Therefore, a large number of forward and reverse calculations are performed in the initial time period to ensure the accuracy of the initial value, and the results in the time period are used as the initial value of the next stage to continue the forward and reverse calculations. The accuracy is continuously improved as time goes by, the number of forward and reverse calculations is accordingly decreased, and the process is completed when the requirements for the alignment accuracy are satisfied. The method provided in the present invention is applicable to the optimal estimation of fine alignment process, and the amount of calculations and the alignment time are reduced as much as possible while ensuring the final alignment accuracy.

The technical means disclosed in the solutions of the present invention are not limited to the technical means disclosed in the foregoing implementations, and also includes technical solutions including any combination of the foregoing technical features. It should be noted that a person of ordinary skill in the art may make several improvements and modifications without departing from the principle of the present invention. All such modifications and modifications shall fall within the protection scope of the present invention.

Claims

1. A strict reverse navigation method for an optimal estimation of fine alignment, comprising the following steps: step 1: establishing an adaptive control function, whereina total sampling time is denoted as m, time periods in a sampling process are denoted as a sequence t1, t2, t3, . . . , a number of forward and reverse calculations in each of the time periods is denoted as q1, q2, q3, . . . , and an expression of the adaptive control function is denoted as:

2. The strict reverse navigation method for optimal estimation of fine alignment according to claim 1, wherein α is 50, and a duration of each stage is 30-60 s.