[发明专利]一种基于超球体采样的初始对准方法

权利要求书

1.一种基于超球体采样的初始对准方法，其特征在于，包括以下几个步骤：

步骤一，建立SINS静基座初始对准的非线性状态方程；

地心惯性坐标系i为x_iy_iz_i；导航坐标系选为地理系，地理系t为x_ty_tz_t；计算地理系c 为x_cy_cz_c；

假设计算地理系和实际地理系之间的姿态误差角分别为水平两个方向的水平误差角 Φ_x，Φ_y和垂直方向上得方位误差角Φ_z，则地理系到计算地理系通过以下转动方式实现：

t系绕着z_t轴转动Φ_z，得到t₁系；t₁系绕轴转动Φ_x，得到t₂系；t₂系绕轴转动Φ_y， 得到c系；水平误差角Φ_x，Φ_y在1°以内，则sinΦ_x≈Φ_x，sinΦ_y≈Φ_y，cosΦ_x＝cosΦ_y≈1， t系和c系之间的关系用下面的矩阵表示：

Ctc=Ct2cCt1t2Ctt1]]>

=cosΦy0-sinΦy010sinΦy0cosΦy1000cosΦxsinΦx0-sinΦxcosΦxcosΦzsinΦz0-sinΦzcosΦz0001]]>

其中：表示由t系到c系的转换矩阵，表示由t₂系到c系的转换矩阵，表示由 t₁系到t₂系的转换矩阵，表示由t系到t₁系的转换矩阵，由于sinΦ_x≈Φ_x，sinΦ_y≈Φ_y， cosΦ_x＝cosΦ_y≈1，得

Ctc=cosΦzsinΦz-Φy-sinΦzcosΦzΦxcosΦz+ΦxsinΦzΦysinΦz-ΦxcosΦz1]]>

姿态误差角Φ_t＝(Φ_x，Φ_y，Φ_z)^T，由姿态微分方程得：

C·tc=-Ωtc·Ctc]]>

其中，Ω_tc＝Ω_ic-Ω_it，Ω_it为坐标系t相对于坐标系i的角速度ω_it的反对称阵，Ω_ic为坐标 系c相对于坐标系i的角速度ω_it的反对称阵，Ω_tc为坐标系i相对于坐标系t的角速度ω_it的反对 称阵，得到：

C·tc=-Ωic·Ctc+Ctc·Ωit]]>

其中：Ωit=0-ωizωiyωiz0-ωix-ωiyωix0,]]>ω_ix表示角速度ω_it在x轴的分量，ω_iy表示角速度ω_it在y 轴的分量，ω_iz表示角速度ω_it在z轴的分量；

ωit=(ωix,ωiy,ωiz)T=(-L·,(ωie+λ·)cosL,(ωie+λ·)sinL)T]]>

式中，λ为当地的经度，L是当地的纬度，ω_ie是地球自转角速率；

当Φ_x，Φ_y都在1°以内时，得到：

Φ·t=ωtc=(ωtx,ωty,ωtz)T]]>

其中：ω_tc表示坐标系c相对于坐标系t的角速度，ω_tx表示角速度ω_tc在x轴的分量，ω_ty表 示角速度ω_tc在y轴的分量，ω_tz表示角速度ω_tc在z轴的分量，又因为：根据向量及其反对称阵的对应关系，上式写成：

ωtc=ωic-Ctcωit]]>

其中，ω_ic＝ω_it+δω_it+ε，ε是陀螺仪的漂移在地理系的投影，δω_it是在SINS中计算ω_it时的误差；综上所述，得到SINS静基座下的非线性的对准方程；

Φ·t=ωtc=ωit+δωit+ϵ-Ctcωit=(I-Ctc)ωit+δωit+ϵ]]>

其中：I表示3阶单位矩阵；

在静基座下，ω_it＝(0，ω_ie cosL，ω_ie sinL)^T，R是地球半径； 得到：

Φ·x=-sinΦzωiecosL+ΦyωiesinL-ΔVy/R+ϵx]]>

Φ·y=(1-cosΦz)ωiecosL-ΦxωiesinL+ΔVx/R+ϵy]]>

Φ·z=(-ΦysinΦz+ΦxcosΦz)ωiecosL+ΔVxtgL/R+ϵz]]>

其中：，ε_x，ε_y，ε_z分别为ε在三个坐标轴方向的投影分量，ΔV_x，ΔV_y，ΔV_z是速度误差在 地理系的投影；

由比力方程：

V·t=at-Δnt+gt]]>

其中，V_t-载体的速度在t系的投影，a_t-比力在t系的投影，Δn_t-有害加速度在t系的 投影，g_t＝[0 0 -g]^T是重力加速度；

由惯导计算出的速度方程为：

V‾·t=a‾t-Δn‾t+gt]]>

而加速度计的读数为：a‾t=ac+▿c=Ctcat+▿c;]]>

将上式左乘变形得：at=Ccta‾t-▿t]]>

其中，a_c为比力在c系的投影，是加速度计算系统偏差在相应坐标系上的投影；

由上可得：ΔV·t=V‾·t-V·t=a‾t-Ccta‾t+▿t-(Δn‾t-Δnt)]]>

其中，Δn‾t-Δnt=(2ωiecosL·ΔVz-2ωiesinL·ΔVy,2ωiesinL·ΔVx,2ωiecosL·ΔVx)]]>

L是当地的纬度，ω_ie是地球自转角速度，ΔV_x，ΔV_y，ΔV_z是速度误差在地理系的投影；

根据转移矩阵的正交性有：ΔCct=(ΔCtc)T=(Cct-I)T]]>

所以，当如不考虑ΔV_z，速度误差方程为：

ΔV·x=-(cosΦz-1)n‾x+sinΦzn‾y-g(ΦycosΦz+ΦxsinΦz)+2ωiesinLΔVy+▿x]]>

ΔV·y=-sinΦzn‾x-(cosΦz-1)n‾y+g(-ΦycosΦz+ΦxsinΦz)-2ωiesinLΔVx+▿y]]>

当方位误差角Φ_z大于1°时，SINS静基座初始对准的非线性状态方程为：

X·=f(X)+BW]]>

f(X)=-(cosΦz-1)n‾x+sinΦzn‾y-g(ΦycosΦz+ΦxsinΦz)+2ωiesinLΔVy+▿x-sinΦzn‾x-(cosΦz-1)n‾y+g(-ΦycosΦz+ΦxsinΦz)-2ωiesinLΔVx+▿y-sinΦzωiecosL+ΦyωiesinL-ΔVy/R+ϵx(1-cosΦz)ωiecosL-ΦxωiesinL+ΔVx/R+ϵy(-ΦysinΦz+ΦxcosΦz)ωiecosL+ΔVxtgL/R+ϵz05×1]]>

B=I5×505×5]]>

其中，状态变量X=ΔVxΔVyΦxΦyΦz▿x▿yϵxϵyϵzT,]]>

W＝[Δα_x Δα_y Δω_x Δω_y Δω_z]^T为5×1过程噪声矩阵，为加速度计漂移， ε_x，ε_y，ε_z为陀螺仪的漂移，Δα_x，Δα_y为加速度计误差的白噪声，Δω_x，Δω_y，Δω_z为陀螺仪漂移 的白噪声，ω_ie为地球自转角速度，L为当地地理纬度，g为当地重力加速度；

在静基座初始对准中，取加速度计在水平两个方向上的输出作为观测量Z，得到量测方 程：

Z＝HX+η

其中，Z为观测量，H=02×20-g0g00I2×202×3,]]>η为2×1观测噪声矩阵；

步骤二，建立基于超球体采样的强跟踪无迹滤波方法，得到状态变量估计值；

具体包括以下几个步骤：

1)状态变量初始化；

对步骤一得到的SINS静基座初始对准的非线性状态方程中的状态变量进行初始化，设置 ΔV_x、ΔV_y、Φ_x、Φ_y、Φ_z、ε_x、ε_y、ε_z的初始值，并且设定X的均值和均方差 P₀，均值为10行1列的列向量，均方差P₀为10行10列的对角矩阵，具体为：

x^0=E(x0)]]>

P0=E((x0-x^0)(x0-x^0)T)]]>

其中：x₀表示状态变量X的初始值；

2)获取强跟踪无迹滤波需要的采样点；

采用超球体采样方法获取n+2个Sigma采样点及Sigma采样点的权值W，n表示状态变 量的维数，具体为：

(1)随机确定常数W₀，0≤W₀≤1；

(2)获取12个Sigma采样点的权值，每一个Sigma采样点的权值W为：

W=1-W0n+1]]>

(3)获取10维状态变量时的第12个Sigma采样点，具体为：

首先获取状态变量维数j＝1，i＝j+2＝3时，三个Sigma采样点：

χ31=0-12W12W]]>

其中：表示1维时第0个Sigma采样点，表示1维时第1个Sigma采样点，表示1维时第2个Sigma采样点；

通过，1维状态变量进行递推计算，当状态变量维数j＝2，3…，10时，j＝2，3…，10，i＝j+2， Sigma采样点具体为：

χij=χ0j-1χ1j-1···χjj-100-1j(j-1)W···-1j(j-1)Wj-1j(j-1)W]]>

其中：表示状态为j维的第i个Sigma采样点，是一个j行i列的矩阵；

最后，得到状态变量维数为10的采样点为一个10行12列的矩阵；

(4)通过状态变量维数为10的采样点获取状态变量X的12个采样点χ₁、 χ₂…χ₁₂；

首先，利用采样点计算得到加入状态变量X的均方差P₀后的Sigma采样点χ¹²：

χ12=P0·χ1210]]>

其中：χ¹²表示加入状态变量X的均方差后的Sigma采样点，χ¹²为一个10行12列的矩阵；

然后，分别取出χ¹²矩阵的第1列，第2列…第12列，对于每一列与状态变量X的均 值相加，得到了状态变量X的12个采样点分别为χ₁、χ₂…χ₁₂，每一个采样点都是一个 10行1列的向量；

3)利用获得的状态变量X的12个采样点，带入到非线性方程中，进行状态变量X的一 步预测：

对于状态变量X的12个采样点，分别带入到非线性状态方程中，获取采样点的一步状态 预测值χ′_k为：

χk′=f(χk),]]>k＝1，2...，12

其中，k为从1到12的每个采样点的编号，f为非线性状态方程的简写符号，χ′_k为采样 点的一步预测值；

利用采样点的一步预测值χ′_k，通过与每一个Sigma采样点的权值W进行加权求和，得到 状态变量X的一步预测值

X^=Σk=112W·χk′]]>

通过χ′_k和获取状态变量X均方差的一步预测值P′：

P′=μ·(Σk=112W·(χk′-X^)(χk′-X^)T)+Q]]>

其中，μ为强跟踪因子，Q为过程噪声的均方差矩阵，是10行10列的矩阵；

对于状态变量X的12个采样点，分别带入到量测方程中计算，得到采样点的一步观测预 测值z_k为：

z_k＝h(χ_k)，k＝1，2...，12

其中，k为从1到12的每个采样点的编号，h为量测方程的简写符号；

利用采样点的一步观测预测值z_k，通过与每一个Sigma采样点的权值W进行加权求和， 得到观测量Z的一步预测值

Z^=Σk=112W·zk]]>

通过上述步骤，得到采样点的一步状态预测值χ′_k、状态变量X的一步预测值状态变 量X均方差的一步预测值P′、采样点的一步观测预测值z_k、观测量Z的一步预测值

4)根据χ′_k、P′、z_k、和测量得到的观测量Z，获得状态变量X的估计值和状 态变量X的估计均方误差矩阵

首先，利用χ′_k、z_k、求得滤波增益矩阵K：

利用矩阵P_zz，求矩阵P_zz的逆得到然后左边乘上矩阵P_xz，得到滤波增益矩阵K，如下 式所示：

K=Pxz·Pzz-1]]>

然后，利用得到的滤波增益矩阵K和测量得到的观测量Z，去修正状态变量X的一步预 测值和状态变量X均方差的一步预测值P′，得到状态变量X的估计值和状态变量X的 估计均方误差矩阵

X~=X^+K·(Z-Z^)]]>

P~=P′-K·Pzz·KT]]>

综上步骤，获得了状态变量X的估计值和状态变量X的估计均方误差矩阵得到状 态变量X的估计值中的姿态误差角的估计值Φ_x、Φ_y、Φ_z；

步骤三，由导航计算机，采集轨迹发生器的惯性器件输出信息，并完成对准过程；

由轨迹发生器产生在静止时的陀螺仪和加速度计的输出，利用加速度计的输出值作为观 测值，通过步骤一和步骤二得到姿态误差角的估计值，对姿态误差角进行补偿，得到准确的 初始姿态矩阵，完成对准。

2.根据权利要求1所述的一种基于超球体采样的初始对准方法，其特征在于，所述的 步骤二3)中μ按照下述的强跟踪算法获得：

观测量Z的一步预测值和观测量Z，求得残差γ为由残差γ计算残差矩阵 V，V的初值V(1)为：V(1)＝γ(1)·γ(1)^T，其中，γ(1)为第一步滤波时产生的残差；

当进行第二步滤波以及进一步滤波时，V的值通过以下递推关系得到：

V(λ)=ρ·V(λ-1)+γ(λ)·γT(λ)1+ρ]]>

其中，ρ为遗忘因子，一般取0.95；λ为第1步，第2步，…第100000步滤波；γ(λ)表 示为第λ步滤波时产生的残差；V(λ)表示为第λ步滤波计算得到的残差矩阵V；

利用残差矩阵V的值，通过以下公式计算，求得μ₀：

M(λ)＝H(λ)·F(λ)·P(λ-1)·F^T(λ)·H^T(λ)

N(λ)＝V(λ)-H(λ)·Q·H^T(λ)-Φ·R

其中，F(λ)是第λ步滤波时非线性状态方程的线性化矩阵，H(λ)是第λ步滤波时量测 方程的线性化矩阵，P(λ-1)为第λ-1步滤波时状态变量X的均方差阵，Q，R是过程噪声及 测量噪声的均方差矩阵；Φ是弱化因子，一般取Φ≥1；

μ₀＝trN(λ)/trM(λ)

其中，trN(k)为第λ步滤波时计算得到的矩阵N(k)的迹，trM(k)为第λ步滤波时计算得 到的矩阵M(k)的迹；当μ₀大于等于1时，强跟踪因子μ＝μ₀；当μ₀小于1时，强跟踪因子 μ＝1。