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.

Claims

1. A strict reverse navigation method for an optimal estimation of a fine alignment to initialize a navigation system, wherein the navigation system comprises a computer and a gyroscope, and wherein the strict reverse navigation method is performed by the navigation system and comprising the following steps: step 1: establishing, by the computer, an adaptive control function, wherein a total sampling time is denoted as m, time periods in a sampling process are denoted as a sequence t.sub.1, t.sub.2, t.sub.3, . . . , a number of forward and reverse calculations in each of the time periods is denoted as q.sub.1, q.sub.2, q.sub.3, . . . , and an expression of the adaptive control function is denoted as: $q_i=\alpha \frac{m}{t_i},$  i∈[1, k], wherein α is a control coefficient for a number of times determined by a main frequency of the computer; step 2: obtaining, by the computer, measurements from the gyroscope, and performing, by the computer, a forward navigation calculation process, wherein a forward navigation process comprises 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.sub.bk.sup.n=C.sub.bk−1.sup.n(I+T.sub.sΩ.sub.nbk.sup.b), wherein C.sub.b.sup.n is a posture matrix, T.sub.s is a sampling period of a strap-down inertial navigation system, Ω.sub.nbk.sup.b=(ω.sub.nbk.sup.nx) wherein (●x) represents an antisymmetric matrix composed of a vector ●, ω.sub.nbk.sup.b=ω.sub.ibk.sup.b−(C.sub.bk−1.sup.n).sup.T(ω.sub.iek−1.sup.n+ω.sub.enk−1.sup.n), wherein ω.sub.ib.sup.b represents an angular speed measurement from the gyroscope, ω.sub.iek.sup.n=[0 ω.sub.ie cos L.sub.k ω.sub.ie sin L.sub.k].sup.T, wherein ω.sub.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,$  wherein V.sub.N and V.sub.E respectively represent a northward speed and an eastward speed, R.sub.M and R.sub.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.sub.k.sup.n=v.sub.k−1.sup.n+T.sub.s[C.sub.bk−1.sup.nf.sub.sfk.sup.b−(2ω.sub.iek−1.sup.n+ω.sub.enk−1.sup.n)×v.sub.k−1.sup.n+g.sup.n], wherein v.sup.n=[v.sub.E.sup.n v.sub.N.sup.n v.sub.U.sup.n].sup.T represents a speed, v.sub.U represents an upward speed, f.sub.sf.sup.b represents a specific force measurement from an accelerometer, and g.sup.n represents a 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,$ wherein λ represents a longitude; step 3: performing, by the computer, a reverse navigation calculation process, wherein a reverse navigation process comprises updating the posture, the speed, and the position, and specifically: a reverse posture updating process is:
C.sub.bk−1.sup.n=C.sub.bk.sup.n(I+T.sub.s+{tilde over (Ω)}.sub.nbk−1.sup.b) wherein {tilde over (Ω)}.sub.nbk−1.sup.b=−Ω.sub.nbk[I+T.sub.sΩ.sub.nbk].sup.−1; a reverse speed updating process is:
−v.sub.k−1.sup.n=−v.sub.k.sup.n+T.sub.sã.sub.k−1,k.sup.n, wherein ã.sub.k−1,k.sup.n=a.sub.k,k−1.sup.n=C.sub.bk−1.sup.nf.sub.sfk.sup.b−(2ω.sub.iek−1.sup.n+ω.sub.enk−1.sup.n)×v.sub.k−1.sup.n+g.sup.n; by defining ← as a way to represent a reverse direction, parameters in the reverse posture updating process and the reverse speed updating process are obtained as follows: .sub.bm−j.sup.n=C.sub.bj.sup.n, .sub.m-j.sup.n=−v.sub.j.sup.n, .sub.m-j=L.sub.j, .sub.m-j=λ.sub.j, .sub.m-j=h.sub.j, .sub.sfm−j.sup.n=f.sub.sfj.sup.n, .sub.k−1,k.sup.n=a.sub.k,k−1.sup.n, .sub.iem−j.sup.n=−ω.sub.iej.sup.n, .sub.enm−j.sup.n=−ω.sub.enj.sup.n, and .sub.nbm−j.sup.b={tilde over (Ω)}.sub.nbj.sup.b, and further let p=m−k+1, and the following subscript conversions occur:
C.sub.bk−1.sup.n=C.sub.bm−p.sup.n=.sub.bp.sup.n,
C.sub.bk.sup.n=C.sub.bm+1−p.sup.n=.sub.bp−1.sup.n, and
{tilde over (Ω)}.sub.nbk−1.sup.b=.sub.nbp.sup.b, and therefore the reverse posture updating process is written as:
.sub.bp.sup.n=.sub.bp−1.sup.n(I+T.sub.s.sub.nbp.sup.b), the reverse speed updating process is written as:
.sub.p.sup.n=.sub.p-1.sup.n+T.sub.s.sub.p-1,p, the reverse position updating process 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}},{\overleftarrow{h}}_p={\overleftarrow{h}}_{p-1}+T_s{\overleftarrow{v}}_{\mathrm{Up}-1}^n;$ step 4: performing, by the computer, an adaptive control for the number of the forward and reverse calculations, wherein the adaptive control is performed for the number of the forward and reverse calculations within a period of time through the adaptive control function $q_i=\alpha \frac{m}{t_i},$  wherein q.sub.i represents the number of the forward and reverse calculations, and after the forward and reverse 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 fine alignment of the navigation system is completed; step 5: initializing the navigation system by aligning the navigation system using the optimal estimation of the fine alignment.

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

3. The strict reverse navigation method according to claim 1, wherein steps 1-5 are performed by the navigation system while the navigation system is on a moving base on a water surface or underwater.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) 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

(2) 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.

(3) 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.sub.1 to t.sub.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.sub.k to t.sub.1 are updated. Specifically, as shown in the FIGURE, the method of the present invention includes the following steps.

(4) Step 1: Establish an Adaptive Control Function.

(5) A total sampling (alignment) time is denoted as m, time periods in a sampling process are denoted as a sequence t.sub.1, t.sub.2, t.sub.3, . . . , and a number of forward and reverse calculations in each of the time periods is denoted as q.sub.1, q.sub.2, q.sub.3, . . . , 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:

(6) $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.sub.i is decreased from fast to slow as t.sub.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.sub.1, t.sub.2, t.sub.3, . . . is to be used as the initial value of a next stage to continue the iterative calculation, which can greatly accelerate convergence. t.sub.i is selected depending on the actual scene. A duration of each stage is generally 30-60 s.

(7) Step 2: Perform a Forward Navigation Calculation Process.

(8) 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.

(9) A forward posture updating process is: C.sub.bk.sup.n=C.sub.bk−1.sup.n(I+T.sub.sΩ.sub.nbk.sup.b), where C.sub.b.sup.n is a posture matrix, T.sub.s is a sampling period of a strap-down inertial navigation system, Ω.sub.nbk.sup.b=(ω.sub.nbk.sup.nx), where (.circle-solid.x represents an antisymmetric matrix composed of a vector .circle-solid., ω.sub.nbk.sup.b=ω.sub.ibk.sup.b−(C.sub.bk−1.sup.n).sup.T(ω.sub.iek−1.sup.n+ω.sub.enk−1.sup.n), where ω.sub.ib.sup.b represents an angular speed measurement from a gyroscope, ω.sub.iek.sup.n[0ω.sub.ie cos L.sub.k ω.sub.ie sin L.sub.k].sup.T, where ω.sub.ie is an angular speed of Earth's rotation, L represents a latitude, and

(10) ${\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.sub.M and R.sub.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.sub.k.sup.n=v.sub.k−1.sup.n+T.sub.s[C.sub.bk−1.sup.nf.sub.sfk.sup.b−(2ω.sub.iek−1.sup.n+ω.sub.enk−1.sup.n)×v.sub.k−1.sup.n+g.sup.n], where v.sup.n=[v.sub.E.sup.n v.sub.N.sup.nv.sub.U.sup.n].sup.T represents a speed, v.sup.U represents an upward speed, f.sub.sf.sup.b represents a specific force measurement from an accelerometer, and g.sup.n represents gravitational acceleration.

(11) A forward position updating process is:

(12) $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.sub.k=h.sub.k−1+T.sub.sv.sub.Uk−1.sup.n, where λ represents a longitude.

(13) Step 3: Perform a Reverse Navigation Calculation Process.

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

(15) a reverse posture updating process is:
C.sub.bk−1.sup.n=C.sub.bk.sup.n(I+T.sub.s+{tilde over (Ω)}.sub.nbk−1.sup.b) where {tilde over (Ω)}.sub.nbk−1.sup.b=−Ω.sub.nbk[I+T.sub.sΩ.sub.nbk].sup.−1;

(16) a reverse speed updating process is:
−v.sub.k−1.sup.n=−v.sub.k.sup.n+T.sub.sã.sub.k−1,k.sup.n, where ã.sub.k−1,k.sup.n=a.sub.k,k−1.sup.n=C.sub.bk−1.sup.nf.sub.sfk.sup.b−(2ω.sub.iek−1.sup.n+ω.sub.enk−1.sup.n)×v.sub.k−1.sup.n+g.sup.n;

(17) by defining ← as a way to represent a reverse direction, parameters in the reverse processes are obtained as follows: .sub.bm−j.sup.n=C.sub.bj.sup.n, .sub.m-j.sup.n=−v.sub.j.sup.n, .sub.m-j=L.sub.j, .sub.m-j=λ.sub.j, .sub.m-j=h.sub.j, .sub.sfm−j.sup.n=f.sub.sfi.sup.n, .sub.k−1,k.sup.n=a.sub.k,k−1.sup.n, .sub.iem−j.sup.n=−ω.sub.iej.sup.n, .sub.enm−j.sup.n=−ω.sub.enj.sup.n, and .sub.nbm−j.sup.b={tilde over (Ω)}.sub.nbj.sup.b, further let p=m−k+1, and the following subscript conversions occur:
C.sub.bk−1.sup.n=C.sub.bm−p.sup.n=.sub.bp.sup.nC.sub.bk.sup.n=C.sub.bm+1−p.sup.n=.sub.bp−1.sup.n, and {tilde over (Ω)}.sub.nbk−1.sup.n=.sub.nbp.sup.b, and therefore

(18) the reverse posture updating is written as:
.sub.bp.sup.n=.sub.bp−1.sup.n(I+T.sub.s.sub.nbp.sup.b)

(19) the reverse speed updating is written as:
.sub.p.sup.n=.sub.p-1.sup.n+T.sub.s.sub.p-1,p

(20) the reverse position updating is written as:

(21) ${\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.$

(22) 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.sub.k to t.sub.1.

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

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

(25) 0$q_i=\alpha \frac{m}{t_i},i\in \left[1,k\right],$
where q.sub.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.sub.1 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.

(26) 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.

(27) 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.