CN102032922A - Inertial-navigation quick and initial alignment method under robust optimal significance - Google Patents

Abstract

The invention discloses an inertial-navigation quick and initial alignment method under robust optimal significance, comprising the following steps of: introducing an auxiliary matrix and an auxiliary feedback control vector into an initial alignment system; solving a robust optimal control problem according to a maxmin index, determining a state feedback control matrix; and in an initial alignment process, recursively calculating a state vector in an error equation of an inertial-navigation system in real time by using an improved H infinite filter, forming a closed loop after feedback, and obtaining three misalignment angles after converging the system. The inertial-navigation quick and initial alignment method has good system structure and good stable parameter robustness, improves dynamic responding quality and increases the initial alignment rapidity of the initial alignment system.

Description

Inertial navigation fast initial alignment method under the optimum meaning of a kind of robust

Technical field

The present invention relates to the initial alignment technology in inertial navigation field, relate to robust control theory simultaneously.

Background technology

Initial alignment is one of the gordian technique in inertial navigation field.Initial alignment is meant that inertial navigation system formally enters before the duty, platform coordinate system and the navigation coordinate system that selectes are coincided, for navigation calculation provides necessary starting condition, comprise alignment precision and aligning time two important indicators, it has a direct impact the later serviceability of system.

Because the observability of partial status variable is poor, it is long that traditional initial alignment method is aimed at the time.And reduce the initial alignment time rapidity that improves the armament systems response is extremely important.

Kalman wave filter commonly used is estimated the error state vector in the initial alignment.But the Kalman wave filter requires that the model of system is accurate, noise statistics is known and satisfies the assumed condition of correlativity.During practical application, but exist the external interference signals that statistical property is short in understanding in the system, and there is the perturbation in the certain limit in system model itself.

At the problems such as uncertainty of initial alignment time length, filtering system, the present invention is with L ₂The uncertain robust theory of optimal control of bounded and H _∞Wave filter is introduced the initial alignment process of inertial navigation system, solves with a kind of new thinking and method.

Summary of the invention

The purpose of this invention is to provide a kind of inertial navigation system fast initial alignment method with robust optimal performance.

The implementation procedure of initial alignment is as follows:

A: according to the mathematical model of inertial navigation system error state equation

$\left\{\begin{array}{c}\stackrel{\·}{x}=\mathrm{Ax}+B_{2}v\\ y=\mathrm{Cx}+\mathrm{Dv}\end{array}\right.$

Select auxiliary FEEDBACK CONTROL vector u, determine companion matrix B ₁

Determine the maxmin index:

Q positive semidefinite wherein;

B: utilize L ₂The theory of bounded uncertain system linear quadratic robust optimum control is asked for feedback of status and is separated, and determines state feedback matrix K ₁

C: in initial alignment process, utilize improved H _∞The real-time recursion of wave filter is calculated the error state vector of inertial navigation system;

D:H _∞Error state vector estimated value and state feedback matrix K that wave filter obtains ₁Multiply each other, feed back to the initial alignment loop, form closed-loop control system;

E: obtain three misalignment φ after system's convergence _E, φ _N, φ _UFor strapdown inertial navitation system (SINS), revise the strapdown matrix, finished the initial alignment of mathematical platform.

According to this method, the selection of matrix must meet the following conditions among the step a: (A, B ₁) can control or can calm, (A C) can see, and (A Q) can see.

According to this method, need real symmetric matrix P of structure among the step b, make state feedback matrix

Real symmetric matrix P and selected parameter η need satisfy algebraic riccati equation:

PA+A ^TP+Q-P(B ₁B ₁ ^T-B ₂η ^-1B ₂ ^T)P＝0。

Parameter η span 0＜η＜1 wherein.

Real symmetric matrix P and selected parameter η, matrix U (η) need satisfy the Lyapunov equation:

[A-(B ₁B ₁ ^T-B ₂η ^-1B ₂ ^T)P]TU(η)+U(η)[A-(B ₁B ₁ ^T-B ₂η ^-1B ₂ ^T)P]+[η ^-1B ₂ ^TP] ^T[η ^-1B ₂ ^TP]＝0。

According to this implementation method, among the step c improved H is adopted in the estimation of error state vector _∞Filtering algorithm.

After the inertial navigation system error state equation is discrete

$\left\{\begin{array}{c}{X}_{k+1}={\mathrm{\Φ}}_k{X}_k+{\mathrm{\Γ}}_k{W}_k\\ y_k=H_k{X}_k+V_k\\ z_k=L_k{X}_k\end{array}\right.$

L in the linear combination herein _kCan be taken as unit matrix I.

Improved H _∞Filtering algorithm is:

$\left\{\begin{array}{c}{\hat{X}}_{k+1}={\mathrm{\Φ}}_k{\hat{X}}_k+K_{k+1}(y_{k+1}-H_{k+1}{\mathrm{\Φ}}_k{\hat{X}}_k)\\ K_{k+1}=P_{k+1}{H}_{k+1}^T{[I+H_{k+1}{P}_{k+1}{H}_{k+1}^T]}^{-1}\\ P_{k+1}={\mathrm{\Φ}}_k{P}_{k}S{\mathrm{\Φ}}_k^T+{\mathrm{\Γ}}_k{\mathrm{\Γ}}_k^T-{\mathrm{\Φ}}_k{P}_{k}S[H_k^T,L_k^T]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k}S{\mathrm{\Φ}}_k^T\\ {\hat{z}}_k=L_k{\hat{X}}_k\end{array}\right.$

Improved H _∞In the wave filter, adopt factor matrix S to guarantee the stability of structure and parameter in the filtering.

According to this method, in the steps d, the auxiliary FEEDBACK CONTROL vector u=K linear with inertial navigation system error state vector x introduced in the initial alignment loop ₁X forms closed-loop control system.This control system does not have actual electronic circuit, and the initial alignment loop is realized by computer program.

Obtain the horizontal misalignment φ of east orientation after system's convergence _E, the horizontal misalignment φ of north orientation _NWith the sky to orientation misalignment φ _U

Advantage or beneficial effect that initial alignment method of the present invention has:

(1) reduce the initial alignment time, outstanding tool is that the sky is to orientation misalignment φ _UCan restrain quickly and stablize;

(2) the filtering algorithm structure and parameter is stable, and output is had smoothing effect preferably;

(3) there is good anti-interference and anti-model perturbation ability in the initial alignment system of Shi Xianing;

(4) parameter designing is flexible.

Description of drawings

Fig. 1 is the initial alignment design cycle;

Fig. 2 is the initial alignment frame principle figure;

Fig. 3 is that the feedback of status of the uncertain robust optimum control of L2 bounded is separated and found the solution process flow diagram;

Fig. 4 is the closed-loop control system synoptic diagram in initial alignment loop.

Embodiment

In order to make technical matters of the present invention, solution, technical thought, technical scheme clearer, be described in further detail below with reference to the implementation procedure of accompanying drawing to the inertial navigation system fast initial alignment.

The initial alignment that the present invention relates to is that the inertial navigation system Navigator enters the critical process of navigator fix before resolving, and has represented the full accuracy of navigational system work.As shown in Figure 1, the design process of initial alignment comprises following committed step: at first, determine inertial navigation system error state equation, auxiliary control vector u, companion matrix B ₁With the maxmin index; Secondly, utilize the robust theory of optimal control to find the solution and determine state feedback matrix K ₁Then, select factor matrix S, to H _∞Wave filter improves; At last, the closed-loop control system of carrying out the initial alignment loop realizes.

Concrete implementation in the design cycle:

(1) determines inertial navigation system error state equation, auxiliary control vector, companion matrix and maxmin index

The error state variable is got east orientation velocity error, north orientation velocity error, the horizontal misalignment of east orientation, the horizontal misalignment of north orientation, sky to the orientation misalignment, i.e. x=[δ V _E, δ V _N, φ _E, φ _N, φ _U] ^TOutput state vector y=[δ V _E, δ V _N] ^TSo it is as follows that the mathematical model of inertial navigation system error state equation is determined:

$\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}_E\\ \mathrm{\δ}{\stackrel{\·}{V}}_N\\ {\stackrel{\·}{\mathrm{\φ}}}_E\\ {\stackrel{\·}{\mathrm{\φ}}}_N\\ {\stackrel{\·}{\mathrm{\φ}}}_U\end{array}\right]=\left[\begin{array}{ccccc}0& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -g& 0\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& g& 0& 0\\ 0& -\frac{1}{R}& 0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ \frac{1}{R}& 0& -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ \frac{\mathrm{tgL}}{R}& 0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\δV}}_E\\ \mathrm{\δ}{V}_N\\ {\mathrm{\φ}}_E\\ {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U\end{array}\right]+\left[\begin{array}{c}{\▿}_E\\ {\▿}_N\\ {\mathrm{\ϵ}}_E\\ {\mathrm{\ϵ}}_N\\ {\mathrm{\ϵ}}_U\end{array}\right]$

$y=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\δV}}_E\\ \mathrm{\δ}{V}_N\\ {\mathrm{\φ}}_E\\ {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U\end{array}\right]$

Auxiliary control vector u and companion matrix B ₁All be taken as 5 rank, u=[u ₁, u ₂, u ₃, u ₄, u ₅] ^T, B ₁=(b _1ij) _{5 * 5}, B ₂=I.For the side makes design, might as well get-2＜b _1ij＜2.

The performance index of system adopt the maxmin index

Q positive semidefinite wherein.

The initial state constraint satisfaction of system wherein

Interference constraints satisfies

Constant weight matrices Q is taken as 5 rank unit matrix I during design.

Introduce the thinking of auxiliary control vector according to the inertial navigation system error state equation of determining and initial alignment, the frame principle figure of initial alignment as shown in Figure 2, u=[f ₁(x), f ₂(x), f ₃(x), f ₄(x), f ₅(x)] ^T=f (x) is the auxiliary FEEDBACK CONTROL vector of introducing in order to realize initial alignment, and in robust optimum control feedback, auxiliary FEEDBACK CONTROL vector u and state vector x have linear relationship u=K ₁X.

(2) the robust theory of optimal control is found the solution state feedback matrix K under the maxmin index ₁

As shown in Figure 3, concrete solution procedure is as follows:

1. system's controllability and controllability are judged: (A, B ₁) can control or can calm, (A, C) and (A Q) can see.Must satisfy the constraint condition that the robust optimum control is found the solution;

2. constant η of initial option;

3. find the solution algebraic riccati equation PA+A ^TP+Q-P (B ₁B ₁ ^T-B ₂η ^-1B ₂ ^T) P=0, try to achieve real symmetric matrix P;

4. ask

Characteristic root, the real part that requires all characteristic roots is less than 0;

5. find the solution the Lyapunov equation

Try to achieve matrix U (η);

6. try to achieve U (η) characteristic root, judge whether its characteristic root approaches

7. 3.～6. going on foot in the solution procedure has a step not satisfy, and then increases step-length, makes η=η+Δ η, restarts to calculate.

Determine state feedback matrix according to the real symmetric matrix that last structure finishes

(3) improved H _∞The wave filter recursion is calculated the error state vector in the initial alignment process

Obtain after the inertial navigation system error state equation discretize

$\left\{\begin{array}{c}{X}_{k+1}={\mathrm{\Φ}}_k{X}_k+{\mathrm{\Γ}}_k{W}_k\\ y_k=H_k{X}_k+V_k\\ z_k=L_k{X}_k\end{array}\right.$

L in the linear combination herein _kBe taken as unit matrix I.

At H _∞Increase factor matrix S in the wave filter, be used for guaranteeing the stability of structure and parameter in the filtering, be so improve algorithm:

$\left\{\begin{array}{c}{\hat{X}}_{k+1}={\mathrm{\Φ}}_k{\hat{X}}_k+K_{k+1}(y_{k+1}-H_{k+1}{\mathrm{\Φ}}_k{\hat{X}}_k)\\ K_{k+1}=P_{k+1}{H}_{k+1}^T{[I+H_{k+1}{P}_{k+1}{H}_{k+1}^T]}^{-1}\\ P_{k+1}={\mathrm{\Φ}}_k{P}_{k}S{\mathrm{\Φ}}_k^T+{\mathrm{\Γ}}_k{\mathrm{\Γ}}_k^T-{\mathrm{\Φ}}_k{P}_{k}S[H_k^T,L_k^T]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k}S{\mathrm{\Φ}}_k^T\\ {\hat{z}}_k=L_k{\hat{X}}_k\end{array}\right.$

In the formula $R_{e,k}=\left[\begin{array}{cc}I& 0\\ 0& {-\mathrm{\γ}}^{2}I\end{array}\right]+\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_k\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right].$

Factor matrix S is taken as diagonal matrix S=diag{s ₁, s ₂, s ₃, s ₄, s ₅, s _i=1+ β _iβ _iRepresent the size of adjusting range, its span | β _i|＜1, β when not doing any adjustment _i=0.If former wave filter ‖ P ‖ ₂, ‖ K ‖ ₂Dull in time the rising got β _i＜0; If former wave filter ‖ P ‖ ₂, ‖ K ‖ ₂Dull in time decline then got β _i＞0.In the method for the present invention, factor matrix value-1＜β _i＜0.

(4) closed-loop control system of initial alignment realizes

The initial Alignment of Inertial Navigation System loop that the inventive method realizes comprises original system, state observer, FEEDBACK CONTROL three parts as shown in Figure 4.With the input of the observation output valve of original system, according to improved H as state observer _∞Wave filter carries out recursion and calculates, and obtains error state vector estimated value; In feedback controller, error state vector estimated value and feedback matrix carry out the linear matrix computing, and controlled vector is as the input of original system, so form a closed-loop control system.

Obtain the horizontal misalignment φ of east orientation after system's convergence _E, the horizontal misalignment φ of north orientation _NWith the sky to orientation misalignment φ _USo obtain

$C_n^{n^{\&prime;}}=\left[\begin{array}{ccc}1& {\mathrm{\&phi;}}_z& -{\mathrm{\&phi;}}_y\\ -{\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="DEST\_PATH\_HSB00000429718400012.png,DEST\_PATH\_HSB00000429718400031.png"\; state="\{\{state\}\}">z</figure-callout>}}& 1& {\mathrm{\&phi;}}_x\\ {\mathrm{\&phi;}}_y& -{\mathrm{\&phi;}}_x& 1\end{array}\right]$

For strapdown inertial navitation system (SINS), the strapdown matrix is revised

Initial alignment finishes.

Claims (9)

1. the inertial navigation fast initial alignment method under the optimum meaning of a robust is with L ₂The uncertain robust theory of optimal control of bounded and H _∞Wave filter is introduced the initial alignment process of inertial navigation system, it is characterized in that comprising:

A: according to the mathematical model of inertial navigation system error state equation

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=\mathrm{Ax}+B_{2}v\\ y=\mathrm{Cx}+\mathrm{Dv}\end{array}\right.$

Select auxiliary FEEDBACK CONTROL vector u, determine companion matrix B ₁

Determine the maxmin index:

Q positive semidefinite wherein;

B: utilize L ₂The theory of bounded uncertain system linear quadratic robust optimum control is asked for feedback of status and is separated, and determines state feedback matrix K ₁

C: in initial alignment process, utilize improved H _∞The real-time recursion of wave filter is calculated the error state vector of inertial navigation system;

D:H _∞Error state vector estimated value and state feedback matrix K that wave filter obtains ₁Multiply each other, feed back to the initial alignment loop, form closed-loop control system;

E: obtain three misalignment φ after system's convergence _E, θ _N, φ _UFor strapdown inertial navitation system (SINS), revise the strapdown matrix, finish the initial alignment of mathematical platform.

2. the inertial navigation fast initial alignment method under the optimum meaning of a kind of robust as claimed in claim 1 is characterized in that the selection of matrix must meet the following conditions among the described step a: (A, B ₁) can control or can calm, (A C) can see, and (A Q) can see.

3. the inertial navigation fast initial alignment method under the optimum meaning of a kind of robust as claimed in claim 1 is characterized in that, needs real symmetric matrix P of structure among the described step b, makes

4. the inertial navigation fast initial alignment method under the optimum meaning of a kind of robust as claimed in claim 3 is characterized in that, described real symmetric matrix P and selected parameter η need satisfy algebraic riccati equation:

PA+A ^TP+Q-P(B ₁B ₁ ^T-B ₂η ^-1B ₂ ^T)P＝0。

5. the inertial navigation fast initial alignment method under the optimum meaning of a kind of robust as claimed in claim 4 is characterized in that, described real symmetric matrix P and selected parameter η, matrix U (η) need satisfy the Lyapunov equation:

[A-(B ₁B ₁ ^T-B ₂η ^-1B ₂ ^T)P] ^TU(η)+U(η)[A-(B ₁B ₁ ^T-B ₂η ^-1B ₂ ^T)P]+[η ^-1B ₂ ^TP] ^T[η ^-1B ₂ ^TP]＝0。

6. the inertial navigation fast initial alignment method under the optimum meaning of a kind of robust as claimed in claim 1 is characterized in that, among the described step c improved H is adopted in the estimation of error state vector _∞Filtering algorithm,

The discrete back of inertial navigation system error state equation:

$\left\{\begin{array}{c}{X}_{k+1}={\mathrm{\&Phi;}}_k{X}_k+{\mathrm{\&Gamma;}}_k{W}_k\\ y_k=H_k{X}_k+V_k\\ z_k=L_k{X}_k\end{array}\right.$

L in the linear combination herein _kCan be taken as unit matrix I,

Improved H _∞Filtering algorithm is:

$\left\{\begin{array}{c}{\hat{X}}_{k+1}={\mathrm{\&Phi;}}_k{\hat{X}}_k+K_{k+1}(y_{k+1}-H_{k+1}{\mathrm{\&Phi;}}_k{\hat{X}}_k)\\ K_{k+1}=P_{k+1}{H}_{k+1}^T{[I+H_{k+1}{P}_{k+1}{H}_{k+1}^T]}^{-1}\\ P_{k+1}={\mathrm{\&Phi;}}_k{P}_{k}S{\mathrm{\&Phi;}}_k^T+{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T-{\mathrm{\&Phi;}}_k{P}_{k}S[H_k^T,L_k^T]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k}S{\mathrm{\&Phi;}}_k^T\\ {\hat{z}}_k=L_k{\hat{X}}_k\end{array}\right..$

7. the inertial navigation fast initial alignment method under the optimum meaning of a kind of robust as claimed in claim 6 is characterized in that, adopts factor matrix S to described H _∞Wave filter improves.

8. the inertial navigation fast initial alignment method under the optimum meaning of a kind of robust as claimed in claim 1 is characterized in that, introduce and the linear auxiliary FEEDBACK CONTROL vector of inertial navigation system error state vector in the initial alignment loop in the described steps d.

9. the inertial navigation fast initial alignment method under the optimum meaning of a kind of robust as claimed in claim 1 is characterized in that the control system in the described steps d does not have actual electronic circuit, and described initial alignment loop is realized by computer program.

Images