CN104655131A

CN104655131A - Initial inertial navigation alignment method based on terated strong tracking spherical simplex radial cubature Kalman filter (ISTSSRCKF)

Info

Publication number: CN104655131A
Application number: CN201510063634.6A
Authority: CN
Inventors: 徐晓苏; 田泽鑫; 张涛; 刘义亭; 孙进; 姚逸卿
Original assignee: Southeast University
Current assignee: Southeast University
Priority date: 2015-02-06
Filing date: 2015-02-06
Publication date: 2015-05-27
Anticipated expiration: 2035-02-06
Also published as: CN104655131B

Abstract

The invention discloses an initial alignment method based on Iterated Strong Tracking Spherical Simplex-Radial Cubature Kalman Filter (ISTSSRCKF), which adopts the SSR rule to select the volume point of CKF; The fading factor in the tracking filter is introduced into the time and measurement update equation of CKF; then the Gauss-Newton iterative algorithm is introduced and the corresponding innovation variance and covariance in the iterative process are improved. For the initial alignment problem under the condition of large misalignment angle and shaking base, the present invention adopts SSR rule sampling to reduce too many sampling points in high-order CKF, and introduces strong tracking algorithm to overcome the problem of CKF performance degradation when the model is inaccurate problem, using an iterative process to make full use of the latest measurement information, thereby effectively reducing the state estimation error and obtaining an initial alignment accuracy better than the standard CKF.

Description

Inertial Navigation Initial Alignment Method Based on ISTSSRCKF

技术领域technical field

本发明主要涉及导航技术领域，尤其涉及一种基于ISTSSRCKF的惯性导航初始对准方法。The present invention mainly relates to the field of navigation technology, in particular to an ISTSSRCKF-based inertial navigation initial alignment method.

背景技术Background technique

惯性导航系统进入工作状态之前先要进行初始对准，捷联惯导系统将惯性传感器——陀螺仪和加速度计直接与载体固联，通过运算得到计算平台来代替物理平台，因此捷联惯导系统的初始对准问题就是求解初始时刻的姿态矩阵。SINS初始对准直接影响捷联系统的导航性能，即导航精度和反应时间。水下自主航行器(AUV)是一种综合了人工智能和其它先进计算技术的任务控制器，要使水下航行器完成预定的使命，离不开水下导航技术，它是决定水下航行器技术发展与应用的瓶颈问题。在实际应用环境下，特别是在大失准角和剧烈晃动条件下，无法使用以线性小失准角为对准模型的经典卡尔曼滤波方法进行滤波，只能建立非线性对准模型，而传统的EKF、UKF等非线性滤波算法存在高维状态下对准精度较低和数值不稳定问题，而且应对不确定因素能力差的缺点，因此发明有效应对复杂环境，具有一定强跟踪能力的SINS非线性晃动基座下的对准方法具有重要的意义。Before the inertial navigation system enters the working state, the initial alignment must be carried out. The strapdown inertial navigation system directly connects the inertial sensors—gyroscope and accelerometer to the carrier, and obtains a computing platform through calculation to replace the physical platform. Therefore, the strapdown inertial navigation system The initial alignment problem of the system is to solve the attitude matrix at the initial moment. SINS initial alignment directly affects the navigation performance of the strapdown system, that is, navigation accuracy and reaction time. Autonomous underwater vehicle (AUV) is a mission controller that integrates artificial intelligence and other advanced computing technologies. In order for an underwater vehicle to complete a predetermined mission, it is inseparable from underwater navigation technology. The bottleneck of technology development and application. In the actual application environment, especially under the conditions of large misalignment angle and severe shaking, it is impossible to use the classical Kalman filtering method with a linear small misalignment angle as the alignment model for filtering, and only a nonlinear alignment model can be established. Traditional nonlinear filtering algorithms such as EKF and UKF have the problems of low alignment accuracy and numerical instability in high-dimensional states, and have the disadvantages of poor ability to deal with uncertain factors. Therefore, SINS is invented to effectively deal with complex environments and has a certain strong tracking ability. Alignment methods under nonlinear shaking pedestals are of great significance.

发明内容Contents of the invention

发明目的：为了克服现有非线性对准技术中存在的不足，本发明提供了一种提高SINS对准精度的基于ISTSSRCKF的惯性导航初始对准方法。Purpose of the invention: In order to overcome the deficiencies in the existing nonlinear alignment technology, the present invention provides an initial alignment method for inertial navigation based on ISTSSRCKF that improves SINS alignment accuracy.

技术方案：为解决上述技术问题，本发明提供了一种基于ISTSSRCKF的惯性导航初始对准方法，其步骤包括如下：Technical solution: In order to solve the above technical problems, the present invention provides a method for initial alignment of inertial navigation based on ISTSSRCKF, the steps of which include the following:

步骤1：建立大失准角状态下的SINS晃动基座对准模型，所述对准模型包括SINS的非线性误差模型、非线性滤波状态模型和非线性滤波量测模型：Step 1: Establish the SINS shaking base alignment model under the state of large misalignment angle, the alignment model includes the nonlinear error model of SINS, the nonlinear filter state model and the nonlinear filter measurement model:

其中，所述建立SINS非线性误差模型过程如下：Wherein, the described process of establishing the SINS nonlinear error model is as follows:

步骤1.1：以东北天地理坐标系作为导航坐标系n，以载体右前上方向矢量右手定则构成坐标系作为载体坐标系b，真实姿态角为真实速度为 $v_{s}^{n} = {[\begin{matrix} v_{e}^{n} & v_{n}^{n} & v_{u}^{n} \end{matrix}]}^{T},$ 真实地理坐标为p＝[L λ H]^T；SINS实际解算出的姿态为速度为 ${\tilde{v}}_{s}^{n} = {[\begin{matrix} {\tilde{v}}_{e}^{n} & {\tilde{v}}_{n}^{n} & {\tilde{v}}_{u}^{n} \end{matrix}]}^{T},$ 地理坐标为 $\tilde{p} = {[\begin{matrix} \tilde{L} & \tilde{λ} & \tilde{H} \end{matrix}]}^{T},$ 记由SINS解算的地理位置建立的坐标系为计算导航坐标系n′，定义SINS姿态角和速度误差分别为则φ、δνⁿ的微分方程如下：Step 1.1: Take the northeast sky geographic coordinate system as the navigation coordinate system n, and use the carrier coordinate system b as the carrier coordinate system b with the vector right-hand rule of the right front and upper direction vector, and the real attitude angle is The real speed is $v_{the s}^{no} = {[\begin{matrix} v_{e}^{no} & v_{no}^{no} & v_{u}^{no} \end{matrix}]}^{T},$ The real geographic coordinates are p=[L λ H] ^T ; the attitude calculated by the actual solution of SINS is speed is ${\tilde{v}}_{the s}^{no} = {[\begin{matrix} {\tilde{v}}_{e}^{no} & {\tilde{v}}_{no}^{no} & {\tilde{v}}_{u}^{no} \end{matrix}]}^{T},$ The geographic coordinates are $\tilde{p} = {[\begin{matrix} \tilde{L} & \tilde{λ} & \tilde{h} \end{matrix}]}^{T},$ Note that the coordinate system established by the geographical location calculated by SINS is the calculation navigation coordinate system n′, and the SINS attitude angle and velocity error are defined as Then the differential equations of φ, δν ⁿ are as follows:

$\overset{\cdot &Center Dot;}{φ φ} = = {C C}_{ω ω}^{- - 11} [[(({I I - - C C}_{n no}^{{n no}^{' '}})) {\overset{~ ~}{ω ω}}_{m m}^{n no} + + {C C}_{n no}^{{n no}^{' '}} {δω δω}_{in in}^{n no} - - {C C}_{b b}^{{n no}^{' '}} (({ϵ ϵ}^{b b} + + {ω ω}_{g g}^{b b}))]]$

$δ δ {\overset{\cdot &Center Dot;}{v v}}^{n no} = = [[I I - - {(({C C}_{n no}^{{n no}^{' '}}))}^{T T}]] {C C}_{b b}^{{n no}^{' '}} {\overset{~ ~}{f f}}^{b b} + + {(({C C}_{n no}^{{n no}^{' '}}))}^{T T} {C C}_{b b}^{{n no}^{' '}} (({&dtri; &dtri;}^{b b} + + {ω ω}_{a a}^{b b})) - - (({22 δω δω}_{ie ie}^{n no} + + {δω δω}_{en en}^{n no})) \times \times {\overset{~ ~}{v v}}_{s the s}^{n no} - - ((22 {\overset{~ ~}{ω ω}}_{en en}^{n no} + + {\overset{~ ~}{ω ω}}_{en en}^{n no})) \times \times δ δ {v v}^{n no}$

其中，φ＝[φ_e φ_n φ_u]^T为纵摇角、横摇角和航向角误差，δvⁿ＝[δv_e δv_n δv_u]^T为东向、北向和天向速度误差， $ϵ^{b} = {[\begin{matrix} ϵ_{x}^{b} & ϵ_{y}^{b} & ϵ_{z}^{b} \end{matrix}]}^{T}$ 为b系下三轴陀螺的常值误差，为b系下三轴陀螺的随机误差， ${&dtri;}^{b} = {[\begin{matrix} {&dtri;}_{x}^{b} & {&dtri;}_{y}^{b} & {&dtri;}_{z}^{b} \end{matrix}]}^{T}$ 为b系下三轴加速度计的常值误差，为b系下三轴加速度计的随机误差，为加速度计的实际输出，为SINS解算的速度，为计算的导航坐标系下的旋转角速度，为计算的地球旋转角速度，为计算的导航坐标系相对地球的旋转角速度，为对应的计算误差，为n系依次旋转角度φ_e、φ_n、φ_u得到n′系所形成的方向余弦矩阵，为b系到n′系状态转移矩阵，即计算的姿态矩阵，C_w为欧拉角微分方程的系数矩阵，上标T表示转置，矩阵和C_w具体为：Among them, φ＝[φ _e φ _n φ _u ] ^T is pitch angle, roll angle and heading angle error, δv ⁿ ＝[δv _e δv _n δv _u ] ^T is eastward, northward and skyward speed error, $ϵ^{b} = {[\begin{matrix} ϵ_{x}^{b} & ϵ_{the y}^{b} & ϵ_{z}^{b} \end{matrix}]}^{T}$ is the constant value error of the three-axis gyroscope under the b system, is the random error of the three-axis gyro in the b system, ${&dtri;}^{b} = {[\begin{matrix} {&dtri;}_{x}^{b} & {&dtri;}_{the y}^{b} & {&dtri;}_{z}^{b} \end{matrix}]}^{T}$ is the constant value error of the three-axis accelerometer under the b system, is the random error of the three-axis accelerometer under the b system, is the actual output of the accelerometer, is the speed of the SINS solution, is the calculated rotation angular velocity in the navigation coordinate system, is the calculated angular velocity of the Earth's rotation, is the calculated rotational angular velocity of the navigation coordinate system relative to the earth, for correspondence calculation error, Rotate the angles φ _e , φ _n , φ _u for the n system in turn to obtain the direction cosine matrix formed by the n′ system, is the state transition matrix from the b system to the n′ system, that is, the calculated attitude matrix, C _w is the coefficient matrix of the Euler angle differential equation, the superscript T represents the transpose, and the matrix and C _w specifically as:

${C C}_{n no}^{{n no}^{,,}} = = [\begin{matrix} {cos cos φ φ}_{n no} cos cos {φ φ}_{u u} - - sin sin {φ φ}_{n no} {sin sin φ φ}_{e e} {sin sin φ φ}_{u u} & {cos cos φ φ}_{n no} {sin sin φ φ}_{u u} + + {sin sin φ φ}_{n no} {sin sin φ φ}_{e e} cos cos {φ φ}_{u u} & - - {sin sin φ φ}_{n no} cos cos {φ φ}_{e e} \\ {- - cos cos φ φ}_{e e} sin sin {φ φ}_{u u} & {cos cos φ φ}_{e e} cos cos {φ φ}_{u u} & sin sin {φ φ}_{e e} \\ {sin sin φ φ}_{n no} cos cos {φ φ}_{u u} + + {cos cos φ φ}_{n no} {sin sin φ φ}_{e e} sin sin {φ φ}_{u u} & {sin sin φ φ}_{n no} sin sin {φ φ}_{u u} - - cos cos {φ φ}_{n no} sin sin {φ φ}_{e e} {sin sin φ φ}_{u u} & cos cos {φ φ}_{n no} {cos cos φ φ}_{e e} \end{matrix}]$

${C C}_{w w} = = [\begin{matrix} {cos cos φ φ}_{n no} & 00 & {- - sin sin φ φ}_{n no} {cos cos φ φ}_{e e} \\ 00 & 11 & {sin sin φ φ}_{e e} \\ sin sin {φ φ}_{n no} & 00 & {cos cos φ φ}_{n no} {cos cos φ φ}_{e e} \end{matrix}]$

所述非线性滤波状态模型建立过程如下：The establishment process of the nonlinear filtering state model is as follows:

步骤1.2：取SINS的欧拉平台误差角φ_e、φ_n、φ_u，速度误差δv_e、δv_n，b系下的陀螺常值误差b系下的加速度计常值误差构成状态向量 $x = {[\begin{matrix} φ_{e} & φ_{n} & φ_{u} & δ v_{e} & δ v_{n} & ϵ_{x}^{b} & ϵ_{y}^{b} & ϵ_{z}^{b} & {&dtri;}_{x}^{b} & {&dtri;}_{y}^{b} \end{matrix}]}^{T},$ 在晃动基座下和近似为零，则非线性滤波的状态方程可简化为：Step 1.2: Take the Euler platform error angles φ _e , φ _n , φ _u of SINS, the velocity errors δv _e , δv _n , and the gyroscope constant value errors under the b system Accelerometer constant value error under b system form the state vector $x = {[\begin{matrix} φ_{e} & φ_{no} & φ_{u} & δ v_{e} & δ v_{no} & ϵ_{x}^{b} & ϵ_{the y}^{b} & ϵ_{z}^{b} & {&dtri;}_{x}^{b} & {&dtri;}_{the y}^{b} \end{matrix}]}^{T},$ under a rocking pedestal and is approximately zero, the state equation of nonlinear filtering can be simplified as:

$\{\begin{matrix} \overset{\cdot &Center Dot;}{φ φ} = = {C C}_{ω ω}^{- - 11} [[((I I - - {C C}_{n no}^{{n no}^{' '}})) {\overset{^^}{ω ω}}_{m m}^{n no} + + {C C}_{n no}^{{n no}^{' '}} {δω δω}_{in in}^{n no} - - {C C}_{b b}^{{n no}^{' '}} {ϵ ϵ}^{b b}]] + + {ω ω}_{g g} \\ δ δ {\overset{\cdot \cdot}{v v}}^{n no} = = [[I I - - {(({C C}_{n no}^{{n no}^{' '}}))}^{T T}]] {C C}_{b b}^{{n no}^{' '}} {\overset{^^}{f f}}^{b b} + + {(({C C}_{b b}^{{n no}^{' '}}))}^{T T} {C C}_{b b}^{{n no}^{' '}} {&dtri; &dtri;}^{b b} - - ((22 {\overset{^^}{ω ω}}_{ie ie}^{n no} + + {\overset{^^}{ω ω}}_{en en}^{n no})) \times \times {δv δv}^{n no} + + {ω ω}_{a a} \\ {\overset{\cdot &Center Dot;}{ϵ ϵ}}^{b b} = = 00 \\ {\overset{\cdot &Center Dot;}{&dtri; &dtri;}}^{b b} = = 00 \end{matrix}$

其中，只取前两维状态，ω(t)＝[ω_g ω_a 0_1×3 0_1×2]^T为零均值高斯白噪声过程,将该非线性滤波状态方程简记为：in, Only the first two dimensions are taken, ω(t)=[ω _g ω _a 0 _1×3 0 _1×2 ] ^T is a zero-mean Gaussian white noise process, and the nonlinear filtering state equation is simply written as:

$\overset{\cdot &Center Dot;}{x x} ((t t)) = = f f ((x x,, t t)) + + ω ω ((t t))$

所述非线性量测方程的建立如下：The establishment of the nonlinear measurement equation is as follows:

步骤1.3：记在b系下的真实速度为v^b，在b系下的实际速度为利用SINS解算的姿态矩阵把转换成以和中的东向和北向速度分量作为匹配信息源，则非线性滤波的量测方程为：Step 1.3: Record the real speed under the b system as v ^b , and the actual speed under the b system is The attitude matrix calculated by SINS converted to by and The eastward and northward velocity components in are used as matching information sources, then the measurement equation of nonlinear filtering is:

$z z = = {\overset{~ ~}{v v}}_{s the s}^{n no} - - {C C}_{b b}^{{n no}^{' '}} {\overset{~ ~}{v v}}^{b b} = = δ δ {v v}^{n no} - - [[I I - - {(({C C}_{n no}^{{n no}^{' '}}))}^{T T}]] {C C}_{b b}^{{n no}^{' '}} {v v}^{b b} + + v v$

其中，取z的前两维为观测值，v为零均值高斯白噪声过程，并将该连续非线性滤波量测方程简记为：Among them, the first two dimensions of z are taken as observed values, v is the zero-mean Gaussian white noise process, and the measurement equation of continuous nonlinear filtering is abbreviated as:

z(t)＝h(x,t)+v(t)z(t)=h(x,t)+v(t)

步骤2：以采样周期T_s作为滤波周期，并以T_s为步长进行离散化，将离散化为x_k＝x_k-1+[f(x_k-1,t_k-1)+ω(t_k-1)]T_s，可得状态方程：Step 2: Take the sampling period T _s as the filtering period, and use T _s as the step size for discretization, and set Discretization is x _k ＝x _k-1 +[f(x _k-1 ,t _k-1 )+ω(t _k-1 )]T _s , the state equation can be obtained:

x_k＝f(x_k-1)+ω_k-1 x _k ＝f(x _k-1 )+ω _k-1

将z(t)＝h(x,t)+v(t)离散化为z_k＝h(x_k,t_k)+v(t_k)，可得量测方程：Discretizing z(t)=h(x,t)+v(t) into z _k ＝h(x _k ,t _k )+v(t _k ), the measurement equation can be obtained:

z_k＝h(x_k)+v_k z _k ＝h(x _k )+v _k

由状态方程式和量测方程式组成如下离散非线性系统：The following discrete nonlinear system is composed of state equation and measurement equation:

$\{\begin{matrix} {x x}_{k k} = = f f (({x x}_{k k - - 11})) + + {ω ω}_{k k - - 11} \\ {z z}_{k k} = = h h (({x x}_{k k})) + + {v v}_{k k} \end{matrix}$

式中，x_k为k时刻系统的状态向量；z_k为k时刻系统状态的量测值；随机系统噪声ω_k～N(0,Q_k)；随机观测噪声v_k～N(0,R_k)；In the formula, x _k is the state vector of the system at time k; z _k is the measured value of the system state at time k; random system noise ω _k ～N(0,Q _k ); random observation noise v _k ～N(0,R _k );

步骤3：利用当前量测值z_k减去相同时刻的量测预测值得到当前时刻的残差ε_k，依据强跟踪原理计算次优渐消因子λ_k，然后利用λ_k修正滤波时间更新过程；Step 3: Subtract the measured predicted value at the same time from the current measured value z _k Get the residual ε _k at the current moment, calculate the suboptimal fading factor λ _k according to the principle of strong tracking, and then use λ _k to modify the filtering time update process;

步骤4：以时间更新的预测估计值和预测方差P_k/k-1为初始值进行迭代过程，进行最大迭代次数N_max次迭代，完成量测更新；Step 4: Forecast estimates updated in time and the prediction variance P _k/k-1 as the initial value for an iterative process, the maximum number of iterations is N _max iterations, and the measurement update is completed;

其中的N_max为迭代的次数，可以根据需要选取，在此取为10。Among them, N _max is the number of iterations, which can be selected according to needs, and is taken as 10 here.

步骤5：利用滤波器滤波获得的欧拉平台角估计值和速度估计值修正SINS解算的姿态矩阵和速度将修正之后的值作为下一次捷联解算的初始值，利用当前获得的陀螺常值误差估计值和加速度计的常值误差估计值修正下一时刻的陀螺输出和加速度计的输出具体修正公式按下式进行计算：Step 5: Estimated value of Euler plateau angle obtained by filter filtering and speed estimates Corrected attitude matrix for SINS solution and speed Use the corrected value as the initial value for the next strapdown solution, and use the currently obtained gyroscope constant value error estimate and the constant error estimate of the accelerometer Correct the gyro output at the next moment and the output of the accelerometer The specific correction formula is calculated as follows:

${C C}_{b b}^{n no} = = {\overset{^^}{C C}}_{{n no}^{' '}}^{n no} {C C}_{b b}^{{n no}^{' '}},, {v v}_{s the s}^{n no} = = {\overset{~ ~}{v v}}_{s the s}^{n no} - - δ δ {\overset{~ ~}{v v}}_{k k}^{n no},, {ω ω}_{ib ib}^{b b} = = {\overset{~ ~}{ω ω}}_{ib ib}^{b b} - - {\overset{~ ~}{ϵ ϵ}}_{k k}^{b b},, {f f}^{b b} = = {\overset{~ ~}{f f}}^{b b} - - {\overset{^^}{&dtri; &dtri;}}_{k k}^{b b}$

若精度达到预设的初始对准要求则结束，否则继续循环执行步骤2～5，直至初始对准结束。If the accuracy reaches the preset initial alignment requirement, it ends; otherwise, it continues to perform steps 2-5 in a loop until the initial alignment ends.

进一步地，所述2)中，所述依据得到的离散化模型在容积卡尔曼滤波的框架下进行时间更新步骤具体为：Further, in the above-mentioned 2), the step of performing time update according to the discretization model obtained under the framework of the volumetric Kalman filter is specifically:

步骤2.2：设置系统状态初始值初始误差协方差阵：Step 2.2: Set the initial value of the system state Initial error covariance matrix:

P₀＝diag{(1°)²(1°)²(10°)²(0.1)²(0.1)² P ₀ ＝diag{(1°) ² (1°) ² (10°) ² (0.1) ² (0.1) ²

(0.01°/h)²(0.01°/h)²(0.01°/h)²(100μg)²(100μg)²}*10(0.01°/h) ² (0.01°/h) ² (0.01°/h) ² (100μg) ² (100μg) ² }*10

并对P₀进行Cholesky分解，得到初始误差协方差阵的特征平方根S₀；And carry out Cholesky decomposition to P ₀ to obtain the characteristic square root S ₀ of the initial error covariance matrix;

步骤2.3：采用SSR规则选取采样点，取如下一组向量：Step 2.3: Use the SSR rule to select sampling points, and take the following set of vectors:

a_j＝[a_j,1 a_j,2 … a_j,n]^T,j＝1,2,…,n+1，其中n为状态量的个数，则a _j =[a _j,1 a _j,2 … a _j,n ] ^T ,j=1,2,…,n+1, where n is the number of state quantities, then

${a a}_{j j,, i i} &equiv; &equiv; \{\begin{matrix} - - \sqrt{\frac{n no + + 11}{n no ((n no - - i i + + 22)) ((n no - - i i + + 11))}},, & i i < < j j \\ \sqrt{\frac{((n no + + 11)) ((n no - - j j + + 11))}{n no ((n no - - j j + + 22))}},, & i i = = j j \\ 00,, & i i > > j j \end{matrix}$

选用ξ_i表示第i个容积点，m＝2(n+1)个容积点为：Choose ξ _i to represent the i-th volume point, m=2(n+1) volume points are:

${ξ ξ}_{i i} = = \{\begin{matrix} \sqrt{n no} {a a}_{j j},, & i i = = 1,2 1,2,, \cdot &Center Dot; \cdot \cdot \cdot &Center Dot;,, n no + + 11 \\ - - \sqrt{n no} {a a}_{j j},, & i i = = n no + + 22,, n no + + 33,, \cdot \cdot \cdot &Center Dot; \cdot &Center Dot;,, 22 ((n no + + 11)) \end{matrix}$

式中n为状态量的个数，本文中n＝10；简记非线性多维积分为Q(f)，SSR规则下 $Q (f) = \frac{1}{2 (n + 1)} Σ_{i = 1}^{m} f (ξ_{i});$ In the formula, n is the number of state variables, n=10 in this paper; abbreviated nonlinear multidimensional integral is Q(f), under the SSR rule $Q (f) = \frac{1}{2 (no + 1)} Σ_{i = 1}^{m} f (ξ_{i});$

步骤2.4：计算状态一步预测值和一步预测误差协方差阵P_k/k-1：通过Cholesky分解误差协方差阵P_k-1/k-1得到S_k-1/k-1：Step 2.4: Calculate the state one-step forecast value And one-step prediction error covariance matrix P _k/k-1 : S _k-1 _{/k-1 is obtained by Cholesky decomposition error covariance matrix P k-1/k} -1:

P_k-1/k-1＝S_k-1/k-1S^T _k-1/k-1 P _k-1/k-1 = S _k-1/k-1 S ^T _k-1/k-1

计算Cubature点(i＝1,2,…,n+1；m＝2(n+1))：Calculate Cubature points (i=1,2,...,n+1; m=2(n+1)):

${X x}_{i i,, k k - - 11 / / k k - - 11} = = {S S}_{k k - - 11 / / k k - - 11} {ξ ξ}_{i i} + + {\overset{^^}{x x}}_{k k - - 11 / / k k - - 11}$

通过状态方程传播Cubature点：Propagate Cubature points through the state equation:

X^* _i,k/k-1＝f(X_i,k-1/k-1)X ^* _i,k/k-1 ＝f(X _i,k-1/k-1 )

估计k时刻的状态预测值：Estimate the state prediction value at time k:

${\overset{^^}{x x}}_{k k / / k k - - 11} = = {Σ Σ}_{j j = = 11}^{m m} \frac{11}{m m} {X x}^{* *}_{i i,, k k / / k k - - 11}$

估计k时刻的状态误差协方差：Estimate the state error covariance at time k:

${P P}_{k k / / k k - - 11} = = \frac{11}{m m} {Σ Σ}_{j j = = 11}^{m m} {X x}^{* *}_{i i,, k k / / k k - - 11} {X x}^{* * T T}_{i i,, k k / / k k - - 11} - - {\overset{^^}{x x}}_{k k / / k k - - 11} {\overset{^^}{x x}}_{k k / / k k - - 11}^{T T} + + {Q Q}_{k k - - 11} . .$

进一步地，所述步骤3中：Further, in the step 3:

所述依据强跟踪原理计算次优渐消因子λ_k运算过程如下：The operation process of calculating the suboptimal fading factor _λk according to the principle of strong tracking is as follows:

利用当前量测值z_k减去相同时刻的量测预测值得到当前时刻的残差为：Subtract the measured predicted value at the same time from the current measured value z _k The residual at the current moment is obtained as:

${ϵ ϵ}_{k k} = = {z z}_{k k} - - {\overset{^^}{z z}}_{k k / / k k - - 11}$

取实际输出序列的协方差阵:Take the covariance matrix of the actual output sequence:

${V V}_{k k} = = \{\begin{matrix} {ϵ ϵ}_{11} {ϵ ϵ}_{11}^{T T},, & ((k k = = 11)) \\ \frac{ρ ρ {V V}_{k k - - 11} + + {ϵ ϵ}_{k k} {ϵ ϵ}_{k k}^{T T}}{11 + + ρ ρ},, & ((k k > > 11)) \end{matrix}$

式中ρ为遗忘因子，取值范围一般为0.95≤ρ≤0.98；In the formula, ρ is the forgetting factor, and the value range is generally 0.95≤ρ≤0.98;

计算矩阵:Calculation matrix:

${N N}_{k k} = = {V V}_{k k} - - {P P}^{T T}_{xz xz,, k k / / k k - - 11} {P P}_{k k / / k k - - 11}^{- - 11} {Q Q}_{k k - - 11} {P P}_{k k / / k k - - 11}^{- - 11} {P P}_{xz xz,, k k / / k k - - 11} - - {R R}_{k k}$

M_k＝P_zz,k/k-1+N_k-V_k M _k =P _zz,k/k-1 +N _k -V _k

式中P_zz,k/k-1为输出预测协方差阵，P_xz,k/k-1为互协方差阵。In the formula, Pzz _,k/k-1 is the output prediction covariance matrix, and P _xz,k/k-1 is the cross-covariance matrix.

计算次优渐消因子λ_k：Compute the suboptimal fading factor λ _k :

${λ λ}_{k k} = = \{\begin{matrix} {λ λ}_{00},, & (({λ λ}_{00} &GreaterEqual; &Greater Equal; 11)) \\ 11,, & (({λ λ}_{00} < < 11)) \end{matrix},, {λ λ}_{00} = = \frac{tr tr [[{N N}_{k k}]]}{tr tr [[{M m}_{k k}]]}$

式中tr[□]表示矩阵的迹。Where tr[□] represents the trace of the matrix.

进一步地，所述步骤4中的迭代CKF算法：Further, the iterative CKF algorithm in the step 4:

步骤4.1.1：利用步骤2.4中各式，计算状态一步预测值和一步预测误差协方差阵P_k/k-1；Step 4.1.1: Use the formulas in step 2.4 to calculate the one-step forecast value of the state and one-step forecast error covariance matrix P _k/k-1 ;

步骤4.1.2：计算渐消因子λ_k，通过Cholesky分解P_k/k-1：Step 4.1.2: Calculate the fading factor λ _k , and decompose P _k/k-1 by Cholesky:

P_k/k-1＝S_k/k-1S^T _k/k-1 P _k/k-1 = S _k/k-1 S ^T _k/k-1

${X x}_{i i,, k k / / k k - - 11} = = {S S}_{k k / / k k - - 11} {ξ ξ}_{i i} + + {\overset{^^}{x x}}_{k k / / k k - - 11}$

通过观测方程传播Cubature点：Propagate Cubature points through the observation equation:

Z_i,k/k-1＝h(X_i,k/k-1)Z _i,k/k-1 ＝h(X _i,k/k-1 )

估计k时刻的观测预测值：Estimate the predicted value of the observation at time k:

${\overset{^^}{z z}}_{k k / / k k - - 11} = = \frac{11}{m m} {Σ Σ}_{j j = = 11}^{m m} {Z Z}_{i i,, k k / / k k - - 11}$

估计自相关协方差阵：Estimate the autocorrelation covariance matrix:

${P P}_{zz zz,, k k / / k k - - 11} = = - - {\overset{^^}{z z}}_{k k / / k k - - 11} {\overset{^^}{z z}}_{k k / / k k - - 11}^{T T} + + \frac{11}{m m} {Σ Σ}_{j j = = 11}^{m m} {Z Z}_{i i,, k k / / k k - - 11} {Z Z}^{T T}_{i i,, k k / / k k - - 11} + + {R R}_{k k}$

估计互相关协方差阵：Estimate the cross-correlation covariance matrix:

${P P}_{xz xz,, k k / / k k - - 11} = = - - {\overset{^^}{x x}}_{k k / / k k - - 11} {\overset{^^}{z z}}_{k k / / k k - - 11}^{T T} + + \frac{11}{m m} {Σ Σ}_{j j = = 11}^{m m} {X x}_{i i,, k k / / k k - - 11} {Z Z}^{T T}_{i i,, k k / / k k - - 11}$

根据步骤3各式计算渐消因子λ_k；Calculate fading factor λ _k according to various formulas in step 3;

步骤4.1.3：利用求出的渐消因子λ_k，对状态预测协方差阵P_k/k-1进行如下变换：Step 4.1.3: Use the obtained fading factor λ _k to transform the state prediction covariance matrix P _k/k-1 as follows:

${P P}_{k k / / k k - - 11} = = {λ λ}_{k k} ((\frac{11}{m m} {Σ Σ}_{j j = = 11}^{m m} {X x}^{* *}_{i i,, k k / / k k - - 11} {X x}_{i i,, k k / / k k - - 11}^{* * T T} - - {\overset{^^}{x x}}_{k k / / k k - - 11} {\overset{^^}{x x}}_{k k / / k k - - 11}^{T T})) + + {Q Q}_{k k - - 11}$

步骤4.2.1：量测更新是以和P_k/k-1为初始值的迭代过程。记第i次迭代的估计值和方差为和(i＝0，1，2，…，N_max)：Step 4.2.1: The measurement update is based on Iterative process with P _k/k-1 as the initial value. Denote the estimated value and variance of the ith iteration as and (i=0, 1, 2, . . . , N _max ):

步骤4.2.2：产生新的因式分解和容积点，chol(□)表示矩阵的Cholesky分解：Step 4.2.2: Generate new factorization and volume points, chol(□) represents the Cholesky decomposition of the matrix:

${\overset{^^}{S S}}_{k k}^{((i i))} = = chol chol (({P P}_{k k}^{((i i))}))$

${X x}_{j j,, k k}^{((i i))} = = {\overset{^^}{S S}}_{k k}^{((i i))} {ξ ξ}_{j j} + + {\overset{^^}{x x}}_{k k}^{((i i))}$

其中ξ_j表示第j个容积点；where ξ _j represents the jth volume point;

步骤4.2.3：用步骤4.1.3中的P_k/k-1重新计算步骤4.1.2各式，可依次得到P_zz,k/k-1、P_xz,k/k-1。Step 4.2.3: Use the P _k/k-1 in step 4.1.3 to recalculate the formulas in step 4.1.2 to obtain P _zz,k/k-1 and P _xz,k/k-1 in sequence.

计算第i次迭代的状态和方差估计：Compute the state and variance estimates for the ith iteration:

${W W}_{k k}^{((i i))} = = {P P}_{xz xz,, k k}^{((i i))} {(({P P}_{zz zz,, k k}^{((i i))}))}^{- - 11}$

${\overset{^^}{x x}}_{k k}^{((i i + + 11))} = = {\overset{^^}{x x}}_{k k / / k k - - 11} + + {W W}_{k k}^{((i i))} [[{z z}_{k k} - - h h (({\overset{^^}{x x}}_{k k}^{((i i))})) - - {(({P P}_{xz xz,, k k}^{((i i))}))}^{T T} {(({P P}_{k k / / k k - - 11}))}^{- - 11} (({\overset{^^}{x x}}_{k k / / k k - - 11} - - {\overset{^^}{x x}}_{k k}^{((i i))}))]]$

${P P}_{k k}^{((i i + + 11))} = = {P P}_{k k / / k k - - 11} - - {W W}_{k k}^{((i i))} (({P P}_{zz zz,, k k}^{((i i))})) {(({W W}_{k k}^{((i i))}))}^{T T}$

步骤4.2.4：若迭代终止时迭代次数为N，则k时刻的状态估计和协方差为：Step 4.2.4: If the number of iterations is N when the iteration terminates, then the state estimation and covariance at time k are:

${\overset{^^}{x x}}_{k k} = = {\overset{^^}{x x}}_{k k}^{((N N))},, {P P}_{k k} = = {P P}_{k k}^{((N N))} . .$

本发明公开了一种基于ISTSSRCKF的初始对准方法，采用SSR规则选取CKF的容积点；将强跟踪滤波中的渐消因子引入到CKF的时间和量测更新方程之中；再将Gauss-Newton迭代算法引入并改进迭代过程中相应的新息方差和协方差。对于在大失准角和晃动基座条件下的初始对准问题，本发明采用SSR规则采样减少了高阶CKF中过多的采样点，引入强跟踪算法克服了模型不准确时CKF性能下降的问题，使用迭代过程充分利用最新量测信息，从而有效地降低状态估计误差，获得优于标准CKF的初始对准精度。The invention discloses an initial alignment method based on ISTSSRCKF, which adopts SSR rules to select the volume point of CKF; introduces the fading factor in strong tracking filter into the time and measurement update equation of CKF; and then uses Gauss-Newton The iterative algorithm introduces and improves the corresponding innovation variance and covariance in the iterative process. For the initial alignment problem under the condition of large misalignment angle and shaking base, the present invention adopts SSR rule sampling to reduce too many sampling points in high-order CKF, and introduces strong tracking algorithm to overcome the problem of CKF performance degradation when the model is inaccurate problem, using an iterative process to make full use of the latest measurement information, thereby effectively reducing the state estimation error and obtaining an initial alignment accuracy better than the standard CKF.

有益效果：本发明相对于现有技术具有以下优点：Beneficial effect: the present invention has the following advantages compared with the prior art:

1)解决了晃动基座和大失准角条件下CKF对准精度下降的问题，为捷联惯导系统的精确导航定位提供了高精度的初始姿态信息。1) Solve the problem of CKF alignment accuracy decline under the condition of shaking base and large misalignment angle, and provide high-precision initial attitude information for the precise navigation and positioning of the strapdown inertial navigation system.

2)采用SSR规则选取采样点，克服了高阶CKF采样点过多的问题，不仅降低了运算复杂度，而且提高了计算精度。2) The SSR rule is used to select sampling points, which overcomes the problem of too many high-order CKF sampling points, not only reduces the computational complexity, but also improves the calculation accuracy.

3)为了克服模型不准确时CKF性能下降问题，将强跟踪算法中的渐消因子引入到CKF的时间和量测更新方程之中。引入Gauss-Newton迭代算法，在迭代过程中充分利用最新量测信息并改进迭代过程中产生的新息方差和协方差，可有效地降低状态估计误差，从而获得优于标准CKF的对准精度。3) In order to overcome the problem of CKF performance degradation when the model is inaccurate, the fading factor in the strong tracking algorithm is introduced into the time and measurement update equation of CKF. The Gauss-Newton iterative algorithm is introduced to make full use of the latest measurement information and improve the innovation variance and covariance generated in the iterative process, which can effectively reduce the state estimation error and obtain alignment accuracy better than the standard CKF.

附图说明Description of drawings

图1为本发明三轴陀螺和三轴加速度计的安装图。FIG. 1 is an installation diagram of a three-axis gyroscope and a three-axis accelerometer of the present invention.

图2为本发明SINS晃动基座对准方案图。Fig. 2 is a diagram of the alignment scheme of the SINS shaking base of the present invention.

图3为本发明基于ISTSSRCKF的初始对准方法原理图。FIG. 3 is a schematic diagram of the initial alignment method based on ISTSSRCKF in the present invention.

图4为本发明晃动基座下纵摇角、横摇角和航向角摇摆模拟图。Fig. 4 is a simulation diagram of pitch angle, roll angle and yaw angle swing under the swaying base of the present invention.

图5为本发明SINS三轴陀螺仪输出的模拟图。Fig. 5 is a simulation diagram of the output of the SINS three-axis gyroscope of the present invention.

图6为本发明SINS三轴加速度计输出的模拟图。Fig. 6 is a simulation diagram of the output of the SINS three-axis accelerometer of the present invention.

图7为本发明SINS晃动基座下初始对准纵摇角误差图。Fig. 7 is an error diagram of the initial alignment pitch angle under the SINS shaking base of the present invention.

图8为本发明SINS晃动基座下初始对准横摇角误差图。Fig. 8 is an error diagram of the initial alignment roll angle under the SINS shaking base of the present invention.

图9为本发明SINS晃动基座下初始对准航向角误差图。Fig. 9 is a diagram of the initial alignment heading angle error under the SINS shaking base of the present invention.

具体实施方式Detailed ways

下面结合附图对本发明作更进一步的说明。The present invention will be further described below in conjunction with the accompanying drawings.

步骤1：建立大失准角状态下的SINS晃动基座对准模型，所述对准模型包括SINS的非线性误差模型、非线性滤波状态模型和非线性滤波量测模型：Step 1: Establish the alignment model of the SINS shaking base under the state of large misalignment angle, the alignment model includes the nonlinear error model of SINS, the nonlinear filtering state model and the nonlinear filtering measurement model:

其中，所属建立SINS非线性误差模型过程如下：Among them, the process of establishing the SINS nonlinear error model is as follows:

$\{\begin{matrix} \overset{\cdot \cdot}{φ φ} = = {C C}_{ω ω}^{- - 11} [[((I I - - {C C}_{n no}^{{n no}^{' '}})) {\overset{^^}{ω ω}}_{m m}^{n no} + + {C C}_{n no}^{{n no}^{' '}} {δω δω}_{in in}^{n no} - - {C C}_{b b}^{{n no}^{' '}} {ϵ ϵ}^{b b}]] + + {ω ω}_{g g} \\ δ δ {\overset{\cdot &Center Dot;}{v v}}^{n no} = = [[I I - - {(({C C}_{n no}^{{n no}^{' '}}))}^{T T}]] {C C}_{b b}^{{n no}^{' '}} {\overset{^^}{f f}}^{b b} + + {(({C C}_{b b}^{{n no}^{' '}}))}^{T T} {C C}_{b b}^{{n no}^{' '}} {&dtri; &dtri;}^{b b} - - ((22 {\overset{^^}{ω ω}}_{ie ie}^{n no} + + {\overset{^^}{ω ω}}_{en en}^{n no})) \times \times {δv δv}^{n no} + + {ω ω}_{a a} \\ {\overset{\cdot &Center Dot;}{ϵ ϵ}}^{b b} = = 00 \\ {\overset{\cdot &Center Dot;}{&dtri; &dtri;}}^{b b} = = 00 \end{matrix}$

z(t)＝h(x,t)+v(t)z(t)=h(x,t)+v(t)

x_k＝f(x_k-1)+ω_k-1 x _k ＝f(x _k-1 )+ω _k-1

量测方程z(t)＝h(x,t)+v(t)离散化为z_k＝h(x_k,t_k)+v(t_k)，可得量测方程：The measurement equation z(t)=h(x,t)+v(t) is discretized into z _k ＝h(x _k ,t _k )+v(t _k ), and the measurement equation can be obtained:

z_k＝h(x_k)+v_k z _k ＝h(x _k )+v _k

步骤2.1：由状态方程式和量测方程式组成如下离散非线性系统：Step 2.1: The following discrete nonlinear system is composed of state equation and measurement equation:

式中，x_k为k时刻系统的状态向量；z_k为k时刻系统状态的量测值；随机系统噪声ω_k～N(0,Q_k)；随机观测噪声v_k～N(0,R_k)。In the formula, x _k is the state vector of the system at time k; z _k is the measured value of the system state at time k; random system noise ω _k ～N(0,Q _k ); random observation noise v _k ～N(0,R _k ).

步骤2.3：本文采用SSR规则取采样点，取如下一组向量：a_j＝[a_j,1 a_j,2 … a_j,n]^T,j＝1,2,…,n+1，其中n为状态量的个数，则Step 2.3: In this paper, SSR rules are used to select sampling points, and the following set of vectors is taken: a _j =[a _j,1 a _j,2 ... a _j,n ] ^T ,j=1,2,...,n+1, where n is the number of state quantities, then

${a a}_{j j,, i i} &equiv; &equiv; \{\begin{matrix} - - \sqrt{\frac{n no + + 11}{n no ((n no - - i i + + 22)) ((n no - - i i + + 11))}},, & i i < < j j \\ \sqrt{\frac{((n no + + 11)) ((n no - - j j + + 11))}{n no ((n no - - j j + + 22))}},, & i i = = j j \\ 00,, & i i > > j j \end{matrix},,$

${ξ ξ}_{i i} = = \{\begin{matrix} \sqrt{n no} {a a}_{j j},, & i i = = 1,2 1,2,, . . . . . .,, n no + + 11 \\ - - \sqrt{n no} {a a}_{j j},, & i i = = n no + + 22,, n no + + 33,, . . . . . .,, 22 ((n no + + 11)) \end{matrix}$

式中n为状态量的个数，本文中n＝10；简记非线性多维积分为Q(f)，SSR规则下 $Q (f) = \frac{1}{2 (n + 1)} Σ_{i = 1}^{m} f (ξ_{i}) .$ In the formula, n is the number of state variables, n=10 in this paper; abbreviated nonlinear multidimensional integral is Q(f), under the SSR rule $Q (f) = \frac{1}{2 (no + 1)} Σ_{i = 1}^{m} f (ξ_{i}) .$

步骤2.4：计算状态一步预测值和一步预测误差协方差阵通过Cholesky分解误差协方差阵P_k-1/k-1得到S_k-1/k-1：Step 2.4: Calculate the state one-step forecast value and one-step forecast error covariance matrix S _k-1/k-1 is obtained by Cholesky decomposition of the error covariance matrix P _k-1/k-1 :

P_k-1/k-1＝S_k-1/k-1S^T _k-1/k-1 P _k-1/k-1 = S _k-1/k-1 S ^T _k-1/k-1

X^* _i,k/k-1＝f(X_i,k-1/k-1)X ^* _i,k/k-1 ＝f(X _i,k-1/k-1 )

${P P}_{k k / / k k - - 11} = = \frac{11}{m m} {Σ Σ}_{j j = = 11}^{m m} {X x}^{* *}_{i i,, k k / / k k - - 11} {X x}^{* * T T}_{i i,, k k / / k k - - 11} - - {\overset{^^}{x x}}_{k k / / k k - - 11} {\overset{^^}{x x}}_{k k / / k k - - 11}^{T T} + + {Q Q}_{k k - - 11}$

步骤3：利用当前量测值z_k减去相同时刻的量测预测值得到当前时刻的残差为：Step 3: Subtract the measured predicted value at the same time from the current measured value z _k The residual at the current moment is obtained as:

${ϵ ϵ}_{k k} = = {z z}_{k k} - - {\overset{^^}{z z}}_{k k / / k k - - 11}$

计算矩阵:Calculation matrix:

M_k＝P_zz,k/k-1+N_k-V_k M _k =P _zz,k/k-1 +N _k -V _k

计算次优渐消因子λ_k：Compute the suboptimal fading factor λ _k :

步骤4：以时间更新的预测估计值和预测方差P_k/k-1为初始值进行迭代过程，进行N_max次迭代，完成量测更新；Step 4: Forecast estimates updated in time and the prediction variance P _k/k-1 as the initial value for an iterative process, and N _max iterations are performed to complete the measurement update;

所述迭代CKF算法过程如下：The iterative CKF algorithm process is as follows:

步骤4.1.1：利用步骤2.4中各式，计算状态一步预测值和一步预测误差协方差阵P_k/k-1。Step 4.1.1: Use the formulas in step 2.4 to calculate the one-step forecast value of the state and one-step forecast error covariance matrix P _k/k-1 .

P_k/k-1＝S_k/k-1S^T _k/k-1 P _k/k-1 = S _k/k-1 S ^T _k/k-1

Z_i,k/k-1＝h(X_i,k/k-1)Z _i,k/k-1 ＝h(X _i,k/k-1 )

估计k时刻的观测预测值：Estimate the observed predicted value at time k:

估计自相关协方差阵：Estimate the autocorrelation covariance matrix:

估计互相关协方差阵：Estimate the cross-correlation covariance matrix:

根据步骤3各式计算渐消因子λ_k。Calculate the fading factor λ _k according to the various formulas in step 3.

步骤4.2.1：量测更新是以和P_k/k-1为初始值的迭代过程。记第i次迭代的估计值和方差为和(i＝0,1,2,…,N_max)：Step 4.2.1: The measurement update is based on Iterative process with P _k/k-1 as the initial value. Denote the estimated value and variance of the ith iteration as and (i=0,1,2,...,N _max ):

${\overset{^^}{S S}}_{k k}^{((i i))} = = chol chol (({P P}_{k k}^{((i i))}))$

其中ξ_j表示第j个容积点。where _ξj denotes the jth volume point.

${\overset{^^}{x x}}_{k k} = = {\overset{^^}{x x}}_{k k}^{((N N))},, {P P}_{k k} = = {P P}_{k k}^{((N N))}$

步骤5：利用当前获得的欧拉平台角估计值和速度估计值修正SINS解算的姿态矩阵和速度将修正之后的值作为下一次捷联解算的初始值，利用当前获得的陀螺常值误差估计值和加速度计的常值误差估计值修正下一时刻的陀螺输出和加速度计的输出具体修正公式按下式进行计算：Step 5: Use the currently obtained Euler plateau angle estimate and speed estimates Corrected attitude matrix for SINS solution and speed Use the corrected value as the initial value for the next strapdown solution, and use the currently obtained gyroscope constant value error estimate and the constant error estimate of the accelerometer Correct the gyro output at the next moment and the output of the accelerometer The specific correction formula is calculated as follows:

${C C}_{b b}^{n no} = = {\overset{^^}{C C}}_{{n no}^{' '}}^{n no} {C C}_{b b}^{{n no}^{' '}},, {v v}_{s the s}^{n no} = = {\overset{~ ~}{v v}}_{s the s}^{n no} - - δ δ {\overset{~ ~}{v v}}_{k k}^{n no},, {ω ω}_{ib ib}^{b b} = = {\overset{~ ~}{ω ω}}_{ib ib}^{b b} - - {\overset{~ ~}{ϵ ϵ}}_{k k}^{b b},, {f f}^{b b} {\overset{~ ~}{f f}}^{b b} - - {\overset{^^}{&dtri; &dtri;}}_{k k}^{b b}$

若精度达到初始对准要求则结束，否则继续循环执行步骤2～5，直至初始对准结束。If the accuracy meets the initial alignment requirements, it ends; otherwise, continue to perform steps 2 to 5 in a loop until the initial alignment ends.

三个陀螺和三个加速度计正交固定安装在AUV内部如图1所示，图2为本发明SINS晃动基座对准方案原理图，图3为本发明基于ISTSSRCKF的初始对准方法原理图。Three gyroscopes and three accelerometers are installed orthogonally and fixedly inside the AUV as shown in Figure 1, Figure 2 is a schematic diagram of the alignment scheme of the SINS shaking base of the present invention, and Figure 3 is a schematic diagram of the initial alignment method based on ISTSSRCKF of the present invention .

以下叙述均针对普通水下航行器。The following descriptions are all aimed at common underwater vehicles.

使用如下的仿真实例说明本发明的实际效果：Use following simulation examples to illustrate the actual effect of the present invention:

1)舰船运动情况及参数设置1) Ship movement and parameter setting

仿真的初始时刻在北纬32°，东经118°的水下20m处，在水下环境的作用下分别绕纵摇轴、横摇轴和航向轴作三轴正弦摇摆运动，纵摇角θ、横摇角γ和航向角ψ的摇摆幅值依次为10°、8°、6°，对应的摇摆周期为6s、8s、10s，以上参数所模拟的曲线如图4所示。The initial moment of the simulation is at 32° north latitude and 118° east longitude at 20m underwater. Under the action of the underwater environment, three-axis sinusoidal rocking motions are performed around the pitch axis, roll axis and heading axis respectively. The swing amplitudes of the swing angle γ and the heading angle ψ are 10°, 8°, and 6° in turn, and the corresponding swing periods are 6s, 8s, and 10s. The curves simulated by the above parameters are shown in Figure 4.

2)陀螺和加速度计的参数设置2) Parameter setting of gyroscope and accelerometer

捷联惯导通常采用光纤陀螺和挠性加速度计，陀螺的常值漂移为0.02°/h，陀螺的随机漂移为0.01°/h，加速度计的常值偏置为100×10^-6g，加速度计的随机误差为50×10^-6g(g为重力加速度)，模拟三轴陀螺输出ω_x、ω_y、ω_z如图5所示，三轴加速度计输出f_x、f_y、f_z如图6所示。Strapdown inertial navigation usually uses fiber optic gyroscopes and flexible accelerometers. The constant drift of the gyroscope is 0.02°/h, the random drift of the gyroscope is 0.01°/h, and the constant bias of the accelerometer is 100×10 ^-6 g. The random error of the accelerometer is 50×10 ^-6 g (g is the acceleration of gravity). The simulated three-axis gyroscope outputs ω _x , ω _y , ω _z as shown in Figure 5. The three-axis accelerometer outputs f _x , f _y , f _z is shown in Figure 6.

3)仿真结果分析3) Simulation result analysis

初始失准角设为10°、10°、10°，使用本发明提出的非线性滤波算法进行SINS晃动基座初始对准，对准完成后的纵摇角误差φ_x、横摇角误差φ_y和航向角误差φ_z分别如图7、8、9所示。仿真结果表明，对于在晃动基座以及存在大失准角情况下的复杂水域条件下，使用本发明提出的非线性滤波技术有较高的对准精度，能满足的导航定位的初始对准要求。The initial misalignment angle is set to 10°, 10°, and 10°, and the nonlinear filtering algorithm proposed by the present invention is used for initial alignment of the SINS shaking base. After the alignment is completed, the pitch angle error φ _x and roll angle error φ _y and heading angle error φ _z are shown in Figures 7, 8, and 9, respectively. The simulation results show that, for complex water conditions with swaying bases and large misalignment angles, using the nonlinear filtering technology proposed by the present invention has higher alignment accuracy and can meet the initial alignment requirements of navigation and positioning .

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。The above is only a preferred embodiment of the present invention, it should be pointed out that for those of ordinary skill in the art, without departing from the principle of the present invention, some improvements and modifications can also be made, and these improvements and modifications are also possible. It should be regarded as the protection scope of the present invention.

Claims

1. An inertial navigation initial alignment method based on ISTSRCKF is characterized by comprising the following steps:

step 1: establishing an SINS shaking base alignment model under a large misalignment angle state, wherein the alignment model comprises a SINS nonlinear error model, a nonlinear filtering state model and a nonlinear filtering measurement model:

wherein, the process of establishing the SINS nonlinear error model is as follows:

step 1.1: taking a northeast geographic coordinate system as a navigation coordinate system n and a carrier right front upper direction vector right handA coordinate system is formed by the rule as a carrier coordinate system b, and the real attitude angle isTrue velocity ofThe real geographic coordinate is p ═ L lambda H]^T(ii) a The attitude actually solved by SINS isAt a speed ofThe geographic coordinates areRecording a coordinate system established by the geographical position solved by the SINS as a calculation navigation coordinate system n', and defining SINS attitude angle and speed error as Phi and vⁿThe differential equation of (a) is as follows:

wherein phi is [ phi ]_e φ_n φ_u]^TV is the pitch angle, roll angle and course angle errorⁿ＝[v_e v_n v_u]^TFor east, north and sky speed errors,b is the constant error of the lower triaxial gyro,is b is the random error of the lower three-axis gyroscope,b is the constant error of the lower triaxial accelerometer,to be b is the random error of the lower three-axis accelerometer,is the actual output of the accelerometer,for the speed of the solution of the SINS,for the calculated angular velocity of rotation in the navigation coordinate system,for the purpose of calculating the angular velocity of rotation of the earth,for the calculated angular velocity of the navigation coordinate system relative to the earth's rotation, to correspond toThe error in the calculation of (a) is,n is a rotation angle phi in sequence_e、φ_n、φ_uObtaining a directional cosine matrix formed by n',is a state transition matrix from b series to n' series, i.e. a calculated attitude matrix, C_wFor coefficient matrix of Euler angle differential equation, superscript T represents transposition, matrixAnd C_wThe method specifically comprises the following steps:

the nonlinear filtering state model establishing process comprises the following steps:

step 1.2: taking the Euler platform error angle phi of the SINS_e、φ_n、φ_uVelocity error v_e、v_nAnd b is the gyro constant errorConstant error of accelerometer under b systemConstructing state vectorsUnder the shaking baseAndapproximately zero, the state equation for the nonlinear filtering can be simplified as:

wherein,taking only the first two-dimensional state, ω (t) ═ ω_g ω_a 0_1×3 0_1×2]^TFor the zero-mean white gaussian noise process, the nonlinear filter state equation is simplified as:

the nonlinear measurement equation is established as follows:

step 1.3: the true velocity denoted by b is v^bThe actual speed under b isAttitude matrix handlebar resolved by SINSIs converted intoTo be provided withAndthe east and north velocity components in (b) are used as matching information sources, and then the measurement equation of the nonlinear filtering is:

taking the former two dimensions of z as an observed value, taking v as a zero-mean Gaussian white noise process, and summarizing the continuous nonlinear filtering measurement equation as:

z(t)＝h(x,t)+v(t)

step 2: with a sampling period T_sAs a filter period, and with T_sFor discretization of the step size, willDiscretization into x_k＝x_k-1+[f(x_k-1,t_k-1)+ω(t_k-1)]T_sThe equation of state can be found:

x_k＝f(x_k-1)+ω_k-1

discretizing z (t) h (x, t) + v (t) to z_k＝h(x_k,t_k)+v(t_k) The measurement equation can be obtained:

z_k＝h(x_k)+v_k

the following discrete nonlinear system is composed of a state equation and a measurement equation:

in the formula, x_kIs the state vector of the system at the moment k; z is a radical of_kMeasured value of system state at k moment; random system noise omega_k～N(0,Q_k) (ii) a Random observation noise v_k～N(0,R_k)；

And step 3: using current measurement value z_kSubtracting the measurement predicted value at the same timeObtaining the residual error of the current moment_kCalculating a suboptimal fading factor lambda according to the strong tracking principle_kThen using λ_kCorrection filterA wave time updating process;

and 4, step 4: time-updated predictive estimatesAnd the predicted variance P_k/k-1Performing an iteration process for the initial value, performing a maximum number of iterations N_maxPerforming secondary iteration to complete measurement updating;

and 5: euler plateau angle estimation obtained by filtering with filterAnd velocity estimateCorrecting SINS resolved attitude matrixAnd velocityTaking the corrected value as the initial value of the next strapdown calculation, and utilizing the currently obtained gyro constant error estimation valueAnd a constant error estimate of the accelerometerCorrecting the gyro output at the next momentAnd output of the accelerometerThe specific correction formula is calculated according to the following formula:

and if the precision reaches the preset initial alignment requirement, ending the process, otherwise, continuously and circularly executing the steps 2-5 until the initial alignment is ended.

2. The inertial navigation initial alignment method based on the ISTSRCKF of claim 1, wherein: in the step 2), the step of performing time update according to the obtained discretization model in the framework of the volume kalman filter specifically includes:

step 2.2: setting system state initial valueInitial error covariance matrix:

P₀＝diag{(1°)²(1°)²(10°)²(0.1)²(0.1)²

(0.01°/h)²(0.01°/h)²(0.01°/h)²(100μg)²(100μg)²}*10

and to P₀Cholesky decomposition is carried out to obtain the characteristic square root S of the initial error covariance matrix₀；

Step 2.3: sampling points are selected by adopting an SSR rule, and the following vectors are taken:

a_j＝[a_j,1 a_j,2 … a_j,n]^Tj is 1, 2., n +1, where n is the number of state quantities, then

Xi is selected_iRepresents the ith volume point, and m is 2(n +1) volume points:

wherein n is the number of state quantities, and n is 10; shorthand non-linear multi-dimensional integral

Q (f), SSR rules

Step 2.4: calculating a state one-step prediction valueCovariance matrix P of sum-step prediction errors_k/k-1：

Decomposing error covariance matrix P by Cholesky_k-1/k-1To obtain S_k-1/k-1：

P_k-1/k-1＝S_k-1/k-1S^T _k-1/k-1

Calculate the cubage point (i ═ 1,2, …, n + 1; m ═ 2(n + 1)):

propagating the Cufoundation point through the equation of state:

X^* _i,k/k-1＝f(X_i,k-1/k-1)

estimating a state prediction value at the k moment:

estimating the state error covariance at time k:

3. the inertial navigation initial alignment method based on the ISTSRCKF of claim 1, wherein: in the step 3:

said strong tracking of evidencePrinciple calculation of suboptimal evanescence factor lambda_kThe operation process is as follows:

using current measurement value z_kSubtracting the measurement predicted value at the same timeThe residual error at the current moment is obtained as follows:

taking a covariance matrix of an actual output sequence:

in the formula, rho is a forgetting factor, and the value range is generally that rho is more than or equal to 0.95 and less than or equal to 0.98;

calculating a matrix:

in the formula P_zz,k/k-1For outputting prediction covariance matrix, P_xz,k/k-1Is a cross variance matrix.

Calculating a suboptimal fading factor lambda_k：

Where tr [ □ ] represents the traces of the matrix.

4. The inertial navigation initial alignment method based on the ISTSRCKF of claim 1, wherein: the iterative CKF algorithm in step 4:

step 4.1.1: using the formulas in step 2.4 to calculate the one-step predicted value of the stateAnd a stepPrediction error covariance matrix P_k/k-1；

Step 4.1.2: calculating an evanescent factor lambda_kDecomposition of P by Cholesky_k/k-1：

P_k/k-1＝S_k/k-1S^T _k/k-1

Calculate the cubage point (i ═ 1,2, …, n + 1; m ═ 2(n + 1)):

propagating the Cufoundation point through the observation equation:

Z_i,k/k-1＝h(X_i,k/k-1)

estimating an observation predicted value at the k moment:

estimating an autocorrelation covariance matrix:

estimating cross-correlation covariance matrix:

calculating the fading factor lambda according to the step 3_k；

Step 4.1.3: using the determined fading factor lambda_kFor state prediction covariance matrix P_k/k-1The following transformations are performed:

step 4.2.1: the measurement is updated byAnd P_k/k-1An iterative process of initial values. Let the estimate and variance of the ith iteration beAnd

step 4.2.2: new factorizations and volume points are generated, chol (□) represents the Cholesky decomposition of the matrix:

in which ξ_jRepresents the jth volume point;

step 4.2.3: with P in step 4.1.3_k/k-1Recalculating the formulas in step 4.1.2 to obtain P_zz,k/k-1、P_xz,k/k-1；

Calculate the state and variance estimates for the ith iteration:

step 4.2.4: if the iteration number is N when the iteration is terminated, the state estimation and covariance at the moment k are as follows: