CN104296780B - A kind of SINS autoregistrations based on gravity apparent motion and latitude computational methods - Google Patents

Abstract

The present invention discloses a kind of SINS autoregistrations based on gravity apparent motion and latitude computational methods, comprises the following steps：1) gravity apparent motion vector is built by gyroscope and acceleration measuring value；2) impact of instrument random noise is eliminated using parameter identification and reconstructing method；3) center point coordinate and bottom radius of circle of gravity apparent motion cone bottom circle are asked for by the gravity apparent motion vector at three moment；4) cone center axial vector is built, initial alignment is completed by vector operation method；5) latitude L calculating is completed according to each vector geometry relation in cone.

Description

A kind of SINS autoregistrations based on gravity apparent motion and latitude computational methods

Technical field

The present invention relates to a kind of SINS based on gravity apparent motion initially alignment and latitude computational methods, it is adaptable to cut out machine without Initial alignment alignment under line motion, swinging condition, does not need any external reference information in alignment procedures, and in alignment procedures In, the calculating of latitude can be completed, belongs to the technical field of navigation algorithm.

Background technology

For the inertial navigation system based on integration working method, initial alignment is premise and the basis of its work, One of and the key and Technology Difficulties of INS.For being specific to SINS, initial alignment refers to that acquisition carrier system b and navigation are n Between attitude matrix.In many SINS Initial Alignment Methods, self-aligned technology is because being navigated using external reference Information and be widely studied and concern.Conventional Alignment Method is included：Based on acceleration of gravity and rotational-angular velocity of the earth Analytic method alignment, based on compass effect compass method alignment, based on zero-speed constraint be aligned with the zero-speed of Kalman filter technology Deng.

But said method is when being initially aligned on swaying base, have the shortcomings that capacity of resisting disturbance is not enough.Such as On swaying base, because rotational-angular velocity of the earth is submerged in gyro noise completely, it is impossible to complete alignment.

Additionally, said method is required to utilize external location information in initial alignment process.Such as parsing alignment needs profit Rotational-angular velocity of the earth is decomposed with latitude information；During compass method is aligned with zero-speed, positional information is needed to constitute SINS solutions Calculate loop.

The content of the invention

Goal of the invention：It is an object of the invention under conditions of unknown latitude information, completely using inertia type instrument itself Measurement data, complete the initial alignment of SINS, and obtain latitude information to resolve for subsequent navigation.

Technical scheme：SINS autoregistrations based on gravity apparent motion of the present invention and latitude computational methods,

Compared with prior art, tool beneficial effect is the present invention：1) in initial alignment process, it is not necessary to which external reference is aided in Navigation information；2) accelerometer measures noise need not be smoothed by rate integrating method, but by parameter identification with Reconstructing method is removed；3) for the actual letter for containing all measuring values in alignment procedures of three vectors of initial alignment Breath, rather than only three time points；4), after alignment terminal procedure and alignment terminate, can externally export and refer to dimensional information, be used for The navigation calculation of SINS or other equipment are used.

Description of the drawings

Fig. 1 is the gravity apparent motion schematic diagram that the present invention is used；

Fig. 2 is that the gravity apparent motion circular cone central shaft that the present invention is used solves schematic diagram；

Fig. 3 is misalignment error estimation Error Graph of the present invention；

Fig. 4 is latitude calculation error figure of the present invention.

Specific embodiment

Below technical solution of the present invention is described in detail, but protection scope of the present invention is not limited to the enforcement Example.

Embodiment：

The cone that the present invention is constituted for gravity apparent motion in inertial system, using gyroscope and acceleration measuring value structure Gravity apparent motion vector is built, and the impact of instrument random noise is eliminated using parameter identification and reconstructing method, using three moment Gravity apparent motion vector asks for the center point coordinate and bottom radius of circle of gravity apparent motion cone bottom circle, and builds cone center axle Vector, completes initial alignment by vector operation method, completes latitude calculating according to each vector geometry relation in cone.

Below in conjunction with the accompanying drawings implementation of the present invention is described in more detail：

Fig. 1 is the gravity apparent motion schematic diagram that the present invention is used, and the weight of certain point with earth rotation is observed in inertial system Power acceleration points to the change with size, constitutes the circular cone.

Each gravity apparent motion vector of circular cone in Fig. 1 is specifically obtained using gyroscope and accelerometer, is specifically included as follows Step：

Solidification initial time t₀Carrier coordinate system b be inertial coodinate systemObtain initial time b systems withAttitude square between system Battle array beI is 3 × 3 unit matrix；

Using the measured value of gyroscopeTracking b systems andThe change of system：

In formula, subscript " ^ " represents value of calculation；"～", represents measured value.

Using attitude matrixThe measured value of accelerometer is projected toIn system, regard so as to complete gravity in inertial system The calculating of motion vector：

The impact of instrument random noise is eliminated using parameter identification and reconstructing method, is specifically included：

Gravity apparent motion cone geometric parameter in Fig. 1 is unrelated with the selection of inertial system, but embodies needs in feature Coordinate system in launch.According to projection relation, the ideal expression that can ask for gravity apparent motion is as follows：

In formula, fⁿIt is gravitational acceleration vector in n to navigate；G is acceleration of gravity；e₀For initial time earth system；e₀For Earth system；ω_ieFor rotational-angular velocity of the earth；a_ijWith b_ij(i, j=1～3) it is respectively matrixWithIn related unknown Element；A_ijFor a_ij、b_ijCombination.

Using the Practical Calculation value of gravity apparent motion in formula (2)By recursive least-squares identification method distinguishing type (3) In parameter A_ij, and the parameter obtained according to identification, reconstruct gravity apparent motionSpecifically include following steps：

21) it is rightIn systemAxle parameter is recognized, and selection state vector is X=[A₁₁ A₁₂ A₁₃]^T, measuring vector isWherein k is iteration update times, and system equation, range matrix and range equation are distinguished as follows：

X_k+1=X_k (4)

H_k=[cos (ω_iet) sin(ω_iet) 1] (5)

Z_k=H_kX_k+V_k (6)

In formula, V_kFor range noise.Recursive Least-square is specific as follows：

In formula, K_kFor gain matrix；P_kFor state covariance matrix；R_kFor V_kCovariance matrix.

22) it is rightIn systemAxle parameter is recognized, and selection state vector is X=[A₂₁ A₂₂ A₂₃]^T, measuring vector isRemaining various same formula (4-7).

23) it is rightIn systemAxle parameter is recognized, and selection state vector is X=[A₃₁ A₃₂ A₃₃]^T, measuring vector isRemaining various same formula (4-7).

According to parameter A that identification in 2) is obtained_ij, build matrixAnd according to ω_ieT, reconstructs gravity apparent motionSpecifically Selection time point is respectively t_A=-t, t_B=0 and t_C=t, t in tool_CFor current time,

As shown in Fig. 2 utilizing t_A、t_BWith t_CThe gravity apparent motion vector at three moment asks for gravity apparent motion cone bottom circle Center point coordinate and bottom radius of circle, specifically include：

Justify central point in cone bottom：

In formula,

Bottom radius of circle is：

Cone center axial vector is built, initial alignment is completed by vector operation method, is specifically included：

Build central shaft vector：

As shown in Fig. 2 in inertial system day to axial vector withOverlap, so as to build day to vector：

Using vector operation, east orientation vector is built：

Using vector operation, north orientation vector is built：

Build n systems relative toThe attitude matrix of system：

Build n systems relative toThe attitude matrix of system：

Latitude calculating is completed according to each vector geometry relation in cone, is specifically included：

Beneficial effects of the present invention are verified by following emulation：

Matlab simulates inertia type instrument data

Carrier is in zero-speed swinging condition, makees oscillating motion around three axles with sinusoidal rule, its pitching, laterally waves with course Mathematical model is：

In formula, θ, γ and ψ are respectively the angle variables in pitching, rolling and course；A_P、A_R、A_YRespectively pitching, rolling with Amplitude is waved in course；ω_P、ω_PWith ω_YRepresent pitching, rolling and course respectively waves angular frequency；WithIndulge respectively Shake, rolling and course initial phase；ψ₀Represent initial heading；ω_i=2 π/T_i, i=P, R, Y, T_iRepresent corresponding rolling period.

Sub- inertial navigation instrument gross data is obtained by above-mentioned emulation digital simulation, and is superimposed corresponding instrument error thereon Used as instrument actual acquired data, sub- inertial navigation is sampled to the instrument actual acquired data, for navigation calculation, sampling week Phase is 10ms.

The relevant parameter of emulation：

Naval vessel initial position：118 ° of east longitude, 32 ° of north latitude；

Ship speed：0m/s；

Ship sway amplitude：7 ° of pitching, 15 ° of rolling, boat shake 5 °；

The ship sway cycle：Pitching 8s, rolling 7.5s, boat shake 6s；

Ship sway initial phase：It is 0；

Naval vessel initial heading：0°；

Equatorial radius：6378165m；

Earth ellipsoid degree：1/298.3；

Earth surface acceleration of gravity：9.8m/s2；

Rotational-angular velocity of the earth：15.04088°/h；

Gyroscope constant value error：0.05°/h；

Gyro white noise error：0.05°/h；

Accelerometer bias：500ug；

Accelerometer white noise error：500ug；

The checking that alignment is calculated with latitude

Proof of algorithm is carried out in ordinary PC.Emulation carries out 1200s, and during simulation process, (1) produces instrumented data； (2) gravity apparent motion is built according to instrumented data；(3) parameter identification is carried out with reconstruct using gravity apparent motion value of calculation；(4) profit Alignment computing is carried out with the gravity apparent motion of reconstruct；(5) latitude calculating is carried out using the gravity apparent motion of reconstruct；(6) in repetition State step.Fig. 3 and 4 is respectively alignment result and latitude calculation error.

In Fig. 3, each curve shows, under swaying base, the method for present invention design has efficiently accomplished initial alignment.

In Fig. 4, curve shows, under swaying base, the method for present invention design has efficiently accomplished the calculating of latitude.

As described above, although the present invention has been represented and described with reference to specific preferred embodiment, which must not be explained It is to the restriction of itself of the invention.Under the premise of the spirit and scope of the present invention defined without departing from claims, can be right Various changes can be made in the form and details for which.

Claims (2)

1. a kind of SINS autoregistrations based on gravity apparent motion and latitude computational methods, it is characterised in that comprise the following steps：

1) gravity apparent motion vector is built by gyroscope and acceleration measuring value；

2) impact of instrument random noise is eliminated using parameter identification and reconstructing method；

3) center point coordinate and bottom circle half of gravity apparent motion cone bottom circle are asked for by the gravity apparent motion vector at three moment Footpath；

4) cone center axial vector is built, initial alignment is completed by vector operation method；

5) latitude L calculating is completed according to each vector geometry relation in cone.

2. a kind of SINS autoregistrations based on gravity apparent motion according to claim 1 and latitude computational methods, its feature Described step 1) following steps are specifically included by gyroscope and acceleration measuring value structure gravity apparent motion vector：

Solidification initial time t₀Carrier coordinate system b be inertial coodinate systemObtain initial time b systems withBetween system, attitude matrix isI is 3 × 3 unit matrix；

Using the measured value of gyroscopeTracking b systems relative toThe change of system：

${\stackrel{\·}{\hat{C}}}_b^{i_{b0}}\left(t\right)={\hat{C}}_b^{i_{b0}}\left(t\right)({\widetilde{\mathrm{\ω}}}_{i_{b0}b}^b\×)---\left(1\right)$

In formula, subscript " ^ " represents value of calculation；"～", represents measured value；

By the calculated attitude matrix of formula (1)By the measured value of accelerometerProject toIn system, inertia is obtained Gravity apparent motion vector in systemFor：

${\hat{f}}^{i_{b0}}\left(t\right)={\hat{C}}_b^{i_{b0}}\left(t\right){\tilde{f}}^b\left(t\right)---\left(2\right)$

Described step 2) using parameter identification and the impact of reconstructing method elimination instrument random noise, specifically include：

The ideal expression for asking for gravity apparent motion is as follows：

$\begin{array}{c}{f}^{i_{b0}}\left(t\right)=C_{e_0}^{i_{b0}}{C}_e^{e_0}\left(t\right)C_n^e{f}^n\\ =\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right]\left[\begin{array}{ccc}\cos \left({\mathrm{\ω}}_{ie}t\right)& -\sin \left({\mathrm{\ω}}_{ie}t\right)& 0\\ \sin \left({\mathrm{\ω}}_{ie}t\right)& \cos \left({\mathrm{\ω}}_{ie}t\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ b_{31}& b_{32}& b_{33}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]\\ =\left[\begin{array}{ccc}{A}_{11}& A_{12}& A_{13}\\ A_{21}& A_{22}& A_{23}\\ A_{31}& A_{32}& A_{33}\end{array}\right]\left[\begin{array}{c}\cos \left({\mathrm{\ω}}_{ie}t\right)\\ \sin \left({\mathrm{\ω}}_{ie}t\right)\\ 1\end{array}\right]=\left[\begin{array}{c}{A}_{11}\cos \left({\mathrm{\ω}}_{ie}t\right)+A_{12}\sin \left({\mathrm{\ω}}_{ie}t\right)+A_{13}\\ A_{21}\cos \left({\mathrm{\ω}}_{ie}t\right)+A_{22}\sin \left({\mathrm{\ω}}_{ie}t\right)+A_{23}\\ A_{31}\cos \left({\mathrm{\ω}}_{ie}t\right)+A_{32}\sin \left({\mathrm{\ω}}_{ie}t\right)+A_{33}\end{array}\right]\end{array}---\left(3\right)$

In formula, fⁿIt is gravitational acceleration vector in n to navigate；G is acceleration of gravity；e₀For initial time earth system；E is the earth System；ω_ieFor rotational-angular velocity of the earth；a_ijWith b_ij(i, j=1～3) it is respectively matrixWithIn each element；A_ijFor computing Obtain with regard to a_ij、b_ijFunction；

Using the Practical Calculation value of gravity apparent motion in formula (2)By in recursive least-squares identification method distinguishing type (3) Parameter A_ij, and the parameter obtained according to identification, reconstruct gravity apparent motionSpecifically include following steps：

21) it is rightIn systemAxle parameter is recognized, and selection state vector is X=[A₁₁A₁₂A₁₃]^T, measuring vector is Wherein k be iteration update times, system equation, range matrix H_kAnd measurement equation difference is as follows successively：

X_k+1=X_k (4)

H_k=[cos (ω_iet) sin(ω_iet) 1] (5)

Z_k=H_kX_k+V_k (6)

In formula, V_kFor range noise；Recursive Least-square is specific as follows：

$\left\{\begin{array}{c}{K}_k=P_{k-1}{H}_k^T{(H_k{P}_{k-1}{H}_k^T+R_k)}^{-1}\\ X_k=X_{k-1}+K_k(Z_k-H_k{X}_{k-1})\\ P_k=(I-K_k{H}_k)P_{k-1}\end{array}\right.---\left(7\right)$

In formula, K_kFor gain matrix；P_kFor state covariance matrix；R_kFor V_kCovariance matrix；

22) it is rightIn systemAxle parameter is recognized, and selection state vector is X=[A₂₁ A₂₂ A₂₃]^T, measuring vector isRemaining various same formula (4)-(7)；

23) it is rightIn systemAxle parameter is recognized, and selection state vector is X=[A₃₁ A₃₂ A₃₃]^T, measuring vector is Remaining various same formula (4)-(7)；

According to step 2) middle parameter A for recognizing acquisition_ij, build matrixAnd according to ω_ieT, reconstructs gravity apparent motionTool Body selection time point is respectively t_A=-t, t_B=0 and t_C=t, wherein t_CFor current time,

$\hat{A}=\left[\begin{array}{ccc}{\hat{A}}_{11}& {\hat{A}}_{12}& {\hat{A}}_{13}\\ {\hat{A}}_{21}& {\hat{A}}_{22}& {\hat{A}}_{23}\\ {\hat{A}}_{31}& {\hat{A}}_{32}& {\hat{A}}_{33}\end{array}\right]---\left(8\right)$

Described step 3) center point coordinate that gravity apparent motion cone bottom is justified is asked for by the gravity apparent motion vector at three moment And bottom radius of circle, specifically include：

Justify central point in cone bottom：

$\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]={\left[\begin{array}{ccc}{A}_1& B_1& C_1\\ A_2& B_2& C_2\\ A_3& B_3& C_3\end{array}\right]}^{-1}\left[\begin{array}{c}{D}_1\\ D_2\\ D_3\end{array}\right]---\left(10\right)$

In formula,

Bottom radius of circle is：

Described step 4) cone center axial vector is built, initial alignment is completed by vector operation method and is specially：

Build central shaft vector：

Day is built to vector：

Build east orientation vector：

Build north orientation vector：

$\hat{N}=\frac{\hat{U}\×\hat{E}}{\left|\right|\hat{U}\×\hat{E}\left|\right|}---\left(16\right)$

Build n systems relative toThe attitude matrix of system：

${\hat{C}}_{i_{b_0}}^n\left(t\right)=\left[\begin{array}{ccc}{\hat{E}}^T& {\hat{N}}^T& {\hat{U}}^T\end{array}\right]---\left(17\right)$

Build n systems relative toThe attitude matrix of system：

${\hat{C}}_b^n\left(t\right)={\hat{C}}_{i_{b_0}}^n\left(t\right){\hat{C}}_b^{i_{b_0}}\left(t\right);$

Described step 5) latitude L calculating is completed according to each vector geometry relation in cone, specially：

$L=arccos\frac{\left|\right|\left[\begin{array}{ccc}{x}_0& y_0& z_0\end{array}\right]\left|\right|}{\left|\right|{\hat{f}}^{i_{b0}}\left(t_C\right)\left|\right|}.$