硅微航姿系统惯性/地磁组合方法专利检索-均值漂移过程视听技术与设备专利检索查询-专利查询网

硅微航姿系统惯性/地磁组合方法

阅读：922发布：2020-12-30

专利汇可以提供硅微航姿系统惯性/地磁组合方法专利检索，专利查询，专利分析的服务。并且本发明硅微航姿系统惯性/地磁组合方法。具体涉及：利用重力向量和地磁向量的观测量确定载体的初始姿态；基于重力场、地磁场这两个姿态参考向量选取最佳观测向量的方法；组合Kalman 滤波器观测方程坐标系的选取原则；惯性/地磁Kalman组合滤波器的实现方法。通过上述发明，可以用低精度硅微惯性元件与磁传感器组合实现硅微航姿系统，实时给出载体的航向姿态。本发明对低精度惯性/地磁组合方法具有通用性，能减小载体运动加速度对重力的影响和外界磁场对地磁场的影响，可广泛应用于硅微航姿系统。，下面是硅微航姿系统惯性/地磁组合方法专利的具体信息内容。

权利要求

1.硅微航姿系统惯性/地磁组合方法，其特征是：
(1)利用微惯性测量组件MIMU中的硅微传感器感应载体运动：硅微陀螺敏感载体运动沿其轴向的角速度信号，硅微加速度计敏感载体运动沿其轴向的加速度及重力加速度信号，利用MIMU中的磁强计敏感地磁向量，并将信号送至导航计算机；
(2)利用加速度计和磁强计对重力向量和地磁向量的观测来确定载体的初始姿态：重力向量a和地磁向量m在地理坐标系n系的投影 $a^{n} = [\begin{matrix} 0 \\ 0 \\ g \end{matrix}]$ 和 $m^{n} = [\begin{matrix} m_{0 x}^{n} \\ m_{0 y}^{n} \\ m_{0 z}^{n} \end{matrix}]$ 为已知常量， mn为地磁场在当地地理坐标系n系中的投影向量，在载体坐标系b系中的投影为ab和mb，构造三个正交向量：
q＝a
r＝a×m
s＝r×a
它们在n系中的投影构成矩阵[]，由an和mn求得；在b系中的投影构成矩阵 []，由ab和mb求得，
$\{\begin{matrix} q^{b} = C_{n}^{b} \cdot q^{n} \\ r^{b} = C_{n}^{b} \cdot r^{n} \\ s^{b} = C_{n}^{b} \cdot s^{n} \end{matrix},$
$[q^{b} \begin{matrix} \cdot \\ \cdot \\ \cdot \end{matrix} r^{b} \begin{matrix} \cdot \\ \cdot \\ \cdot \end{matrix} s^{b}] = C_{n}^{b} \cdot [q^{n} \begin{matrix} \cdot \\ \cdot \\ \cdot \end{matrix} r^{n} \begin{matrix} \cdot \\ \cdot \\ \cdot \end{matrix} s^{n}]$
$C_{n}^{b} = [q^{b} \begin{matrix} \cdot \\ \cdot \\ \cdot \end{matrix} r^{b} \begin{matrix} \cdot \\ \cdot \\ \cdot \end{matrix} s^{b}] \cdot {[q^{n} \begin{matrix} \cdot \\ \cdot \\ \cdot \end{matrix} r^{n} \begin{matrix} \cdot \\ \cdot \\ \cdot \end{matrix} s^{n}]}^{- 1} - - - (1)$
由此，求出姿态矩阵Cn b的估计根据下面的两个公式求得Kalman滤波器的初值
$| {\hat{q}}_{01} | = \frac{1}{2} \sqrt{1 + C_{11} - C_{22} - C_{33}}$ $sig ({\hat{q}}_{00}) = 1$
$| {\hat{q}}_{02} | = \frac{1}{2} \sqrt{1 - C_{11} + C_{22} - C_{33}}$ $sig ({\hat{q}}_{01}) = sign (C_{32} - C_{23})$
$| {\hat{q}}_{03} | = \frac{1}{2} \sqrt{1 - C_{11} - C_{22} + C_{33}}$ $sig ({\hat{q}}_{02}) = sign (C_{13} - C_{31})$
$| {\hat{q}}_{00} | = \frac{1}{2} \sqrt{1 + q_{01}^{2} - q_{02}^{2} - q_{03}^{2}}$ $sig ({\hat{q}}_{03}) = sign (C_{21} - C_{12})$
则
(3)利用加速度计和磁强计对重力向量和地磁向量的观测来修正陀螺给出的姿态角，即把这两个参考向量在n系中的理论值通过陀螺给出的姿态角投影到b系中，与加速度计和磁强计的测量值求差作为观测量来完成对姿态的修正；本步骤包含如下步骤：
①建立卡尔曼滤波的状态方程：
$\overset{\cdot}{X} (t) = F (t) X (t) + G (t) W (t) - - - (3)$
式中，X(t)是t时刻系统的状态向量，F(t)、G(t)分别为系统状态矩阵和噪声矩阵，W(t) 为系统的噪声向量；
系统的状态矢量为：
$X = [\begin{matrix} q_{e} \\ ΔB \end{matrix}] = {[\begin{matrix} q_{e 1} & q_{e 2} & q_{e 3} & {ΔB}_{x} & Δ B_{y} & Δ B_{z} \end{matrix}]}^{T} - - - (4)$
系统的状态噪声向量为：
$W (t) = (\begin{matrix} W_{1} (t) \\ W_{2} (t) \end{matrix}) = {(\begin{matrix} ω_{rx} & ω_{ry} & ω_{rz} & ω_{gx} & ω_{gy} & ω_{gz} \end{matrix})}^{T} - - - (5)$
其中，qe＝[qe1 qe2 qe3]T为姿态四元数向量估计误差，ΔB＝[ΔBx ΔBy ΔBz]T为陀螺零偏估计误差，W1(t)＝(ωrx ωry ωrz)T为陀螺误差噪声，W2(t)＝(ωgx ωgy ωgz)T为陀螺零偏估计误差随机游走模型的零均值白噪声；
{W(t)}为独立零均值高斯白噪声过程，其协方差矩阵：
E{W(t)WT(τ)}＝Q(t)δ(t-τ)
其中， $Q (t) = [\begin{matrix} Q_{1} & 0_{3 \times 3} \\ 0_{3 \times 3} & Q_{2} \end{matrix}]$ 为对角阵，Q1的对角线元素根据陀螺噪声来确定，取为噪声RMS 值的平方除以带宽f*：
Q1＝[(RMSgyro)2/f*]·I3×3
Q2的对角线元素根据陀螺零位漂移的特点来确定，
系统的噪声系数矩阵为： $G (t) = [\begin{matrix} - \frac{1}{2} I_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I_{3 \times 3} \end{matrix}] - - - (6)$
系统的状态转移矩阵为：
上式中，为载体角速度向量在载体坐标系中的估计值，为向量的反对称矩阵；
②根据加速度计测量值判断系统加速运动情况和磁强计测量值判断地磁受干扰情况，如果重力向量和地磁向量没有受到干扰，转到步骤③；如果重力向量或地磁向量受到干扰，转到步骤④：
③建立卡尔曼滤波的观测方程，并转到步骤⑤：
Z(t)＝H(t)X(t)+V(t)    (8)
式中，Z(t)为t时刻系统的量测向量，H(t)为系统量测矩阵，V(t)为系统的量测噪声向量；
系统量测向量为：
观测向量
式中，ax ay az为三轴加速度计输出的加速度；
观测向量
式中，mx my mz为三轴磁强计输出的磁强；
观测噪声向量为： $V (t) = [\begin{matrix} V_{1} (t) \\ V_{2} (t) \end{matrix}] - - - (10)$
{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：
E{V(t)VT(τ)}＝Rd(t)δ(t-τ)
$R_{d} (t) = [\begin{matrix} R_{d 1} & 0_{3 \times 3} \\ 0_{3 \times 3} & R_{d 2} \end{matrix}]$ 为对角阵，Rd1和Rd2的对角线元素分别根据加速度计和磁强计的噪声来确定，工程上可近似取为噪声RMS值的平方：
Rd1＝(RMSacc)2·I3×3，Rd2＝(RMSmag)2·I3×3
系统观测矩阵为：H(t)＝[H1(t) 06×3]    (11)
上式中，
④构造重力向量和地磁向量的法向量：
r＝a×m
在n系中投影的法向量理论值为rn＝an×mn，可以由an和mn求得；
在b系中法向量的测量值为rb＝ab×mb，可以由ab和mb求得；建立卡尔曼滤波的观测方程：
Z(t)＝H(t)X(t)+V(t)
式中，Z(t)为t时刻系统的量测向量；H(t)为系统量测矩阵；V(t)为系统的量测噪声向量；
(a)地磁向量受到干扰的情况：
系统量测向量为：
观测向量
观测噪声向量为： $V (t) = [\begin{matrix} V_{1} (t) \\ V_{3} (t) \end{matrix}] - - - (10)$
V3(t)为法向量的测量噪声，与V1(t)和V2(t)有关 {V(t)}为独立均值高斯白噪声过程，其协方差矩阵：
E{V(t)VT(τ)}＝Rd(t)δ(t-τ)
$R_{d} (t) = [\begin{matrix} R_{d 1} & 0_{3 \times 3} \\ 0_{3 \times 3} & R_{d 3} \end{matrix}]$ 为对角阵
系统观测矩阵为：H(t)＝[H1(t)06×3]
上式中，
(b)重力向量受到干扰的情况：
系统量测向量为：
观测噪声向量为： $V (t) = [\begin{matrix} V_{3} (t) \\ V_{2} (t) \end{matrix}] - - - (10)$
{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：
E{V(t)VT(τ)}＝Rd(t)δ(t-τ)
$R_{d} (t) = [\begin{matrix} R_{d 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & R_{d 2} \end{matrix}]$ 为对角阵
系统观测矩阵为：H(t)＝[H1(t)06×3]
上式中，
⑤根据扩展卡尔曼滤波递推方程和步骤①、③或④的结果，建立卡尔曼滤波的时间传播方程：

式中ωb＝[ωx ωy ωz]T为当前时刻三轴陀螺的输出；为上一时刻更新后的陀螺零偏估计；Δt为计算步长；
$\overset{\cdot}{\overline{q}} = \frac{1}{2} \overline{q} ⊗ \overline{ω} = \frac{1}{2} \overline{q} \otimes [\begin{matrix} 0 \\ \overline{ω} (t^{-}) \end{matrix}]$
式中，qω分别表示四元数向量，表示四元数乘法，具体为：

P(t+Δt)-)＝Φ(t+Δt，t)P(t+)ΦT(t+Δt，t)+Qd(t)    (13)
式中，Φ(t+Δt，t)＝I+F(t)Δt，Qd(t)＝Q(t)Δt
⑥根据扩展卡尔曼滤波递推方程和步骤①、②的结果，建立卡尔曼滤波的测量修正方程：
K(t)＝P(t-)HT(HP(t-)HT+Rd)-1    (14)
P(t+)＝(I-K(t)H)P(t-)    (15)

经过补偿更新后需进行规一化处理；

(4)输出载体姿态参数：由步骤(3)估计得到的姿态四元数即可由公式(2)求得姿态矩阵由姿态矩阵的元素计算得出姿态角，具体步骤：
航向角主值
 俯仰角主值φ主＝-sin-1C13
横滚角主值
由上述主值按照如下公式判断真值：
φ＝φ主

2.如权利要求1所述的硅微航姿系统惯性/地磁组合方法，其特征是：步骤(3)中步骤 ③或步骤④卡尔曼滤波器观测方程的构造，量测向量Z在载体坐标b系中构造。
3.如权利要求1所述的硅微航姿系统惯性/地磁组合方法，其特征是：重力向量或地磁向量受到干扰时，采用步骤(3)中步骤④的卡尔曼滤波器观测方程。

说明书全文

技术领域

本发明涉及一种测量方法，特别是涉及一种捷联航姿系统的组合导航方法，尤其涉及一种硅微航姿系统惯性/地磁组合导航方法。

背景技术

硅微航姿系统包括由三个硅微陀螺仪、三个硅微加速度计和三轴磁强计组成的微惯性测量单元(MIMU)和导航计算机，是一种捷联式惯性组合导航系统，硅微陀螺仪和加速度计直接固连在运载体上，测量出载体的旋转运动角速率和线运动加速度信息(包括重力加速度)，然后送至导航计算机中进行实时的姿态矩阵解算，并从捷联姿态矩阵的有关元素中计算出载体的姿态角。硅微加速度计及硅微陀螺是基于MEMS技术的一种新型惯性传感器，目前其精度相对常规惯性器件要低很多，陀螺的漂移和噪声水平要高于地球的自转速率，因此目前航姿系统大多采用微惯性器件与磁强计组合使用的方案。
硅微航姿系统用加速度计和磁强计测量的重力向量和地磁向量为参考信息通过Kalman 组合滤波来补偿陀螺的漂移、修正陀螺测量的载体运动角速度积分后的角度漂移，确定的载体姿态信息既有长期的稳定精度又具有载体动态运动的快速实时性，明显的提高了航姿系统的精度、增强了系统的鲁棒性。因此如何构建一种更加充分利用加速度计、磁强计和陀螺测量信息的Kalman组合滤波方法对于实现硅微航姿系统具有重要的意义。
在已报道和所能查阅到的国内外相关文献中，给出文献综合分析结果：
由于国外的硅微惯性技术发展得较为成熟，目前研究内容均集中在硅微惯性组合系统在各行业的应用研究，而国外公司的产品技术出于商业保密原因，并未见诸详细报道；而国内的相关研究则大多局限在系统的各项具体技术的理论研究上，如惯性器件的测试、误差建模与补偿、矩阵更新算法等。

发明内容

本发明的目的是提供一种能够有效抑制载体机动影响和地磁干扰的硅微航姿系统惯性/ 地磁组合方法。
为了达到上述的发明目的，本发明包括下列步骤：
(1)利用微惯性测量组件MIMU中的硅微传感器感应载体运动：硅微陀螺敏感载体运动沿其轴向的角速度信号，硅微加速度计敏感载体运动沿其轴向的加速度及重力加速度信号；利用MIMU中的磁强计敏感地磁向量，并将信号送至导航计算机。
(2)利用加速度计和磁强计对重力向量和地磁向量的观测来确定载体的初始姿态重力向量a和地磁向量m在地理坐标系n系的投影

a^{n} = [\begin{matrix} 0 \\ 0 \\ g \end{matrix}]

和

m^{n} = [\begin{matrix} m_{0 x}^{n} \\ m_{0 y}^{n} \\ m_{0 z}^{n} \end{matrix}]

为已知常量， mn为地磁场在当地n系的投影向量，可根据经纬度和海拔高度由国际参考地磁场查得。在载体坐标系b系中的投影ab和mb，可由加速度计和磁强计测得。
载体坐标系可通过导航坐标系(地理坐标系n系)按航向角(ψ)、俯仰角(φ)、横滚角(θ)三次转动确定，如附图2所示。
构造三个正交向量：
q＝a
r＝a×m
s＝r×a
则它们在n系中的投影构成矩阵可由an和mn求得；在b系中的投影构成矩阵可由ab和mb求得。
因为

\{\begin{matrix} q^{b} = C_{n}^{b} \cdot q^{n} \\ r^{b} = C_{n}^{b} \cdot r^{n} \\ s^{b} = C_{n}^{b} \cdot s^{n} \end{matrix},

所以

[\begin{matrix} q^{b} & \begin{matrix} . \\ . \\ . \end{matrix} & r^{b} & \begin{matrix} . \\ . \\ . \end{matrix} & s^{b} \end{matrix}] = C_{n}^{b} \cdot [\begin{matrix} q^{n} & \begin{matrix} . \\ . \\ . \end{matrix} & r^{n} & \begin{matrix} . \\ . \\ . \end{matrix} & s^{n} \end{matrix}]

即

C_{n}^{b} = [\begin{matrix} q^{b} & \begin{matrix} . \\ . \\ . \end{matrix} & r^{b} & \begin{matrix} . \\ . \\ . \end{matrix} & s^{b} \end{matrix}] \cdot {[\begin{matrix} q^{n} & \begin{matrix} . \\ . \\ . \end{matrix} & r^{n} & \begin{matrix} . \\ . \\ . \end{matrix} & s^{n} \end{matrix}]}^{- 1} - - - (1)

由此，可求出姿态矩阵Cn b的估计具体地根据下面的两个公式即可求得Kalman滤波器的初值

| {\hat{q}}_{01} | = \frac{1}{2} \sqrt{1 + C_{11} - C_{22} - C_{33}}

\begin{matrix} | {\hat{q}}_{02} | = \frac{1}{2} \sqrt{1 - C_{11} + C_{22} - C_{33}} & \begin{matrix} sig ({\hat{q}}_{00}) = 1 \\ sig ({\hat{q}}_{01}) = sign (C_{32} - C_{23}) \end{matrix} \end{matrix}

\begin{matrix} | {\hat{q}}_{03} | = \frac{1}{2} \sqrt{1 - C_{11} - C_{22} + C_{33}} & \begin{matrix} sig ({\hat{q}}_{02}) = sign (C_{13} - C_{31}) \\ sig ({\hat{q}}_{03}) = sign (C_{21} - C_{12}) \end{matrix} \end{matrix}

| {\hat{q}}_{00} | = \frac{1}{2} \sqrt{1 + q_{01}^{2} - q_{02}^{2} - q_{03}^{2}}

则
(3)利用加速度计和磁强计对重力向量和地磁向量的观测来修正陀螺给出的姿态角。具体地，把这两个参考向量在n系中的理论值通过陀螺给出的姿态角投影到b系中，与加速度计和磁强计的测量值求差作为观测量来完成对姿态的修正。本步骤包含如下步骤：
①建立卡尔曼滤波的状态方程：

\overset{\cdot}{X} (t) = F (t) X (t) + G (t) W (t) - - - (3)

式中，X(t)是t时刻系统的状态向量；F(t)、G(t)分别为系统状态矩阵和噪声矩阵；W(t) 为系统的噪声向量。
系统的状态矢量为：

X = [\begin{matrix} q_{e} \\ ΔB \end{matrix}] = {[\begin{matrix} q_{e 1} & q_{e 2} & q_{e 3} & {ΔB}_{x} & {ΔB}_{y} & {ΔB}_{z} \end{matrix}]}^{T} - - - (4)

系统的状态噪声向量为：

W (t) = (\begin{matrix} W_{1} (t) \\ W_{2} (t) \end{matrix}) = {(\begin{matrix} ω_{rx} & ω_{ry} & ω_{rz} & ω_{gx} & ω_{gy} & ω_{gz} \end{matrix})}^{T} - - - (5)

其中，qe＝[qe1 qe2 qe3]T为姿态四元数向量估计误差，ΔB＝[ΔBx ΔBy ΔBz]T为陀螺零偏估计误差，W1(t)＝(ωrx ωry ωrz)T为陀螺误差噪声，W2(t)＝(ωgx ωgy ωgz)T为陀螺零偏估计误差随机游走模型的零均值白噪声。
{W(t)}为独立零均值高斯白噪声过程，其协方差矩阵：
E{W(t)WT(τ)}＝Q(t)δ(t-τ)
其中，

Q (t) = [\begin{matrix} Q_{1} & 0_{3 \times 3} \\ 0_{3 \times 3} & Q_{2} \end{matrix}]

为对角阵。Q1的对角线元素根据陀螺噪声来确定，工程上可近似取为噪声RMS值的平方除以带宽f*：
Q1＝[(RMSgyro)2/f*]·I3×3
Q2的对角线元素根据陀螺零位漂移的特点来确定。
系统的噪声系数矩阵为：

G (t) = [\begin{matrix} - \frac{1}{2} I_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I_{3 \times 3} \end{matrix}] - - - (6)

系统的状态转移矩阵为：
上式中，为载体角速度向量在载体坐标系中的估计值，为向量的反对称矩阵，。
②根据加速度计测量值判断系统加速运动情况和磁强计测量值判断地磁受干扰情况，如果重力向量和地磁向量没有受到干扰，转到步骤③；如果重力向量或地磁向量受到干扰，转到步骤④；
③建立卡尔曼滤波的观测方程，并转到步骤⑤：
Z(t)＝H(t)X(t)+V(t) (8)
式中，Z(t)为t时刻系统的量测向量；H(t)为系统量测矩阵；V(t)为系统的量测噪声向量。
系统量测向量为：
观测向量
式中，ax ay az为三轴加速度计输出的加速度。
观测向量
式中，mx my mz为三轴磁强计输出的磁强。
观测噪声向量为：

V (t) = [\begin{matrix} V_{1} (t) \\ V_{2} (t) \end{matrix}] - - - (10)

{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：
E{V(t)VT(τ)}＝Rd(t)δ(t-τ)

R_{d} (t) = [\begin{matrix} R_{d 1} & 0_{3 \times 3} \\ 0_{3 \times 3} & R_{d 2} \end{matrix}]

为对角阵，Rd1和Rd2的对角线元素分别根据加速度计和磁强计的噪声来确定，工程上可近似取为噪声RMS值的平方：
Rd1＝(RMSacc)2·I3×3，Rd2＝(RMSmag)2·I3×3
系统观测矩阵为：H(t)＝[H1(t)06×3] (11)
上式中，
④构造重力向量和地磁向量的法向量：
r＝a×m
重力向量、地磁向量和两者叉乘构造的法向量如附图3所示。
在n系中投影的法向量理论值为rn＝an×mn，可以由an和mn求得；
在b系中法向量的测量值为rb＝ab×mb，可以由ab和mb求得；
建立卡尔曼滤波的观测方程：
Z(t)＝H(t)X(t)+V(t)
式中，Z(t)为t时刻系统的量测向量；H(t)为系统量测矩阵；V(t)为系统的量测噪声向量。
(a)地磁向量受到长期干扰的情况：
系统量测向量为：
观测向量
观测噪声向量为：

V (t) = [\begin{matrix} V_{1} (t) \\ V_{3} (t) \end{matrix}] - - - (10)

V3(t)为法向量的测量噪声，与V1(t)和V2(t)有关
{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：
E{V(t)VT(τ)}＝Rd(t)δ(t-τ)

R_{d} (t) = [\begin{matrix} R_{d 1} & 0_{3 \times 3} \\ 0_{3 \times 3} & R_{d 3} \end{matrix}]

为对角阵
系统观测矩阵为：H(t)＝[H1(t)06×3]
上式中，
(b)重力向量受到载体长期机动运动干扰的情况：
系统量测向量为：
观测噪声向量为：

V (t) = [\begin{matrix} V_{3} (t) \\ V_{2} (t) \end{matrix}] - - - (10)

{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：
E{V(t)VT(τ)}＝Rd(t)δ(t-τ)

R_{d} (t) = [\begin{matrix} R_{d 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & R_{d 2} \end{matrix}]

为对角阵
系统观测矩阵为：H(t)＝[H1(t)06×3]
上式中，
⑤根据扩展卡尔曼滤波递推方程和步骤①、③或④的结果，建立卡尔曼滤波的时间传播方程：

式中ωb＝[ωx ωy ωz]T为当前时刻三轴陀螺的输出；为上一时刻更新后的陀螺零偏估计；Δt为计算步长。

\overset{\cdot}{\overline{q}} = \frac{1}{2} \overline{q} \otimes \overline{ω} = \frac{1}{2} \overline{q} \otimes [\begin{matrix} 0 \\ \overline{ω} (t^{-}) \end{matrix}]

式中，q ω分别表示四元数向量，表示四元数乘法，具体为：

P((t+Δt)-)＝Φ(t+Δt，t)P(t+)ΦT(t+Δt，t)+Qd(t)    (13)
式中，Φ(t+Δt，t)＝I+F(t)Δt，Qd(t)＝Q(t)Δt
⑥根据扩展卡尔曼滤波递推方程和步骤①、②的结果，建立卡尔曼滤波的测量修正方程：
K(t)＝P(t-)HT(HP(t-)HT+Rd)-1   (14)
P(t+)＝(I-K(t)H)P(t-)          (15)

经过补偿更新后需进行规一化处理。

(4)输出载体姿态参数：由步骤(3)估计得到的姿态四元数即可由公式2求得姿态矩阵姿态角可以由姿态矩阵的元素计算得出，具体步骤：
航向角主值
俯仰角主值    φ主＝-sin-1C13
横滚角主值
由上述主值按照如下公式判断真值：
φ＝φ主

本发明使用的硅微航姿系统中硬件包括微惯性测量单元(简称：MIMU，包括三个相互垂直的硅微陀螺、三个相互垂直的硅微加速度计和三个相互垂直的磁强计)、导航计算机，具体硬件组成如附图1所示；MIMU的测量坐标系(为载体坐标系b系)与地理坐标系n系的关系及载体姿态角如附图2所示。
本发明的方法具有以下优点：(1)航姿系统体积小、重量轻；(2)具有完全的自主性，不需要外部输入信息；(3)受载体机动和地磁干扰的影响小，能够有效提高航姿系统的输出姿态精度；(4)航姿系统可以为其它装置提供每秒50次以上的实时姿态信号。
对本发明的有益效果说明如下：
(1)航姿系统仿真实验
航姿系统置于姿态零位，从第10秒到第20秒，在南北方向加入0.1M(M为地磁场的垂直分量)的磁场偏置；从第35秒到第45秒，在东西方向加入0.1M的磁场偏置，如附图 4a所示。系统采用重力向量和地磁向量作为参考向量的姿态仿真结果如附图4c所示。由于地磁场受到干扰，利用本发明所述方法的系统采用重力向量和法向量作为参考向量的姿态仿真结果如附图4d所示。由图4b可见法向量的方向总保持水平，因此图4d中没有产生图4c的水平姿态的影响，本发明所述方法可以减小地磁场干扰对航姿系统姿态的影响。
(2)航姿系统的三轴转台实验
将采用本发明研制的硅微航姿系统安装在三轴转台上进行静态和三轴摇摆试验，所用硅微航姿系统的器件精度如下：
硅微陀螺零位稳定性：5°/s
硅微陀螺刻度因数线性度：0.1％
硅微陀螺刻度因数温度系数：0.1％/℃
硅微陀螺噪声：
硅微加速度计零位稳定性：0.02m/s2
硅微加速度计刻度因数线性度：0.2％
硅微加速度计刻度因数稳定性：0.05m/s2(-25℃～+80℃)
硅微加速度计噪声：
磁强计零位稳定性：0.5mGauss
磁强计刻度因数线性度：2％
磁强计刻度因数稳定性：0.5mGauss
磁强计噪声：0.5mGauss
利用发明所述方法得到三轴转台指北、水平静止状态硅微航姿系统航向角、俯仰角、横滚角曲线分别如附图5a、图5b和图5c所示；三轴摇摆状态航向角、俯仰角、横滚角曲线分别如附图6a、图6b和图6c所示。结果表明硅微航姿系统具有较好的精度。
(3)航姿系统的跑车实验
动态跑车实验在一段较为平直的公路上进行，车辆的姿态大致水平(其中一段较为颠簸)，航向角基本保持为315°，跑车过程包括匀速运动(时速40km/h)、加速运动、减速运动过程。以车辆上另外一套高精度光纤捷联组合惯导系统(PHINS)提供的姿态输出作为参考，得到了跑车过程中硅微航姿系统的航向姿态。颠簸路段横滚角曲线图7a所示，平坦路段横滚角曲线图7b所示，俯仰角、航向角曲线分别如图8、图9所示。
动态跑车实验结果表明，航姿系统利用本发明的方法在动态跑车实验中达到了实用的水平。

附图说明

图1为本发明的硅微航姿系统惯性/地磁组合原理图；
图2为载体坐标系(b系)与地理坐标系(n系)的关系图；
图3为重力向量、地磁向量与法向量的关系图；
图4a-图4d为地磁场受干扰的航姿系统仿真结果图；
图5a-图5c为静态状态航姿系统测试结果图；
图6a-图6c为摇摆状态航姿系统测试结果图；
图7a-图7b为动态跑车航姿系统横滚角测试结果图；
图8为动态跑车航姿系统俯仰角测试结果图；
图9为动态跑车航姿系统航向角测试结果图；
图10为本发明的硅微航姿系统软件流程图。

具体实施方式

下面举例对本发明作更详细地描述，本实施例包括以下步骤：
(1)利用微惯性测量组件MIMU中的硅微传感器感应载体运动：硅微陀螺敏感载体运动沿其轴向的角速度信号，硅微加速度计敏感载体运动沿其轴向的加速度及重力加速度信号；利用MIMU中的磁强计敏感地磁向量，并将信号送至导航计算机。
(2)利用加速度计和磁强计对重力向量和地磁向量的观测来确定载体的初始姿态重力向量a和地磁向量m在地理坐标系n系的投影

a^{n} = [\begin{matrix} 0 \\ 0 \\ g \end{matrix}]

和

m^{n} = [\begin{matrix} m_{0 x}^{n} \\ m_{0 y}^{n} \\ m_{0 z}^{n} \end{matrix}]

\{\begin{matrix} q^{b} = C_{n}^{b} \cdot q^{n} \\ r^{b} = C_{n}^{b} \cdot r^{n} \\ s^{b} = C_{n}^{b} \cdot s^{n} \end{matrix},

所以

[\begin{matrix} q^{b} & \begin{matrix} . \\ . \\ . \end{matrix} & r^{b} & \begin{matrix} . \\ . \\ . \end{matrix} & s^{b} \end{matrix}] = C_{n}^{b} \cdot [\begin{matrix} q^{n} & \begin{matrix} . \\ . \\ . \end{matrix} & r^{n} & \begin{matrix} . \\ . \\ . \end{matrix} & s^{n} \end{matrix}]

即

C_{n}^{b} = [\begin{matrix} q^{b} & \begin{matrix} . \\ . \\ . \end{matrix} & r^{b} & \begin{matrix} . \\ . \\ . \end{matrix} & s^{b} \end{matrix}] \cdot {[\begin{matrix} q^{n} & \begin{matrix} . \\ . \\ . \end{matrix} & r^{n} & \begin{matrix} . \\ . \\ . \end{matrix} & s^{n} \end{matrix}]}^{- 1} - - - (1)

由此，可求出姿态矩阵Cn b的估计具体地根据下面的两个公式即可求得Kalman滤波器的初值

| {\hat{q}}_{01} | = \frac{1}{2} \sqrt{1 + C_{11} - C_{22} - C_{33}}

\begin{matrix} | {\hat{q}}_{02} | = \frac{1}{2} \sqrt{1 - C_{11} + C_{22} - C_{33}} & \begin{matrix} sig ({\hat{q}}_{00}) = 1 \\ sig ({\hat{q}}_{01}) = sign (C_{32} - C_{23}) \end{matrix} \end{matrix}

\begin{matrix} | {\hat{q}}_{03} | = \frac{1}{2} \sqrt{1 - C_{11} - C_{22} + C_{33}} & \begin{matrix} sig ({\hat{q}}_{02}) = sign (C_{13} - C_{31}) \\ sig ({\hat{q}}_{03}) = sign (C_{21} - C_{12}) \end{matrix} \end{matrix}

| {\hat{q}}_{00} | = \frac{1}{2} \sqrt{1 + q_{01}^{2} - q_{02}^{2} - q_{03}^{2}}

\overset{\cdot}{X} (t) = F (t) X (t) + G (t) W (t) - - - (3)

式中，X(t)是t时刻系统的状态向量；F(t)、G(t)分别为系统状态矩阵和噪声矩阵；W(t) 为系统的噪声向量。
系统的状态矢量为：

X = [\begin{matrix} q_{e} \\ ΔB \end{matrix}] = {[\begin{matrix} q_{e 1} & q_{e 2} & q_{e 3} & {ΔB}_{x} & {ΔB}_{y} & {ΔB}_{z} \end{matrix}]}^{T} - - - (4)

系统的状态噪声向量为：

W (t) = (\begin{matrix} W_{1} (t) \\ W_{2} (t) \end{matrix}) = {(\begin{matrix} ω_{rx} & ω_{ry} & ω_{rz} & ω_{gx} & ω_{gy} & ω_{gz} \end{matrix})}^{T} - - - (5)

Q (t) = [\begin{matrix} Q_{1} & 0_{3 \times 3} \\ 0_{3 \times 3} & Q_{2} \end{matrix}]

G (t) = [\begin{matrix} - \frac{1}{2} I_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I_{3 \times 3} \end{matrix}] - - - (6)

V (t) = [\begin{matrix} V_{1} (t) \\ V_{2} (t) \end{matrix}] - - - (10)

{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：
E{V(t)VT(τ)}＝Rd(t)δ(t-τ)

R_{d} (t) = [\begin{matrix} R_{d 1} & 0_{3 \times 3} \\ 0_{3 \times 3} & R_{d 2} \end{matrix}]

V (t) = [\begin{matrix} V_{1} (t) \\ V_{3} (t) \end{matrix}] - - - (10)

V3(t)为法向量的测量噪声，与V1(t)和V2(t)有关
{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：
E{V(t)VT(τ)}＝Rd(t)δ(t-τ)

R_{d} (t) = [\begin{matrix} R_{d 1} & 0_{3 \times 3} \\ 0_{3 \times 3} & R_{d 3} \end{matrix}]

为对角阵
系统观测矩阵为：H(t)＝[H1(t)06×3]
上式中，
(b)重力向量受到载体长期机动运动干扰的情况：
系统量测向量为：
观测噪声向量为：

V (t) = [\begin{matrix} V_{3} (t) \\ V_{2} (t) \end{matrix}] - - - (10)

{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：
E{V(t)VT(τ)}＝Rd(t)δ(t-τ)

R_{d} (t) = [\begin{matrix} R_{d 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & R_{d 2} \end{matrix}]

\overset{\cdot}{\overline{q}} = \frac{1}{2} \overline{q} \otimes \overline{ω} = \frac{1}{2} \overline{q} \otimes [\begin{matrix} 0 \\ \overline{ω} (t^{-}) \end{matrix}]

标题	发布/更新时间	阅读量
一种基于行为的室内定位和地标语义标识的方法	2020-05-14	204
一种水下航行器的水下信标定位方法	2020-05-14	82
一种用于水损害检测的探地雷达图谱自适应选取方法	2020-05-08	564
一种基于海洋环境噪声的声学监测系统及方法	2020-05-15	270
眼部疲劳度检测系统及方法	2020-05-08	737
一种多MEMS传感器组合测姿方法	2020-05-14	393
判断目标行人在非重叠相机视角中出现的匹配方法	2020-05-11	320
结合判别式深度置信网络和主动学习的心电分类方法	2020-05-13	453
融合时空聚类和支持向量机的居民出行特征提取方法	2020-05-14	117
一种改进SPC的小区供水漏损监测预警方法	2020-05-15	678

硅微航姿系统惯性/地磁组合方法

技术领域

背景技术

发明内容

附图说明

具体实施方式

该功能需要专业版企业版VIP权限，您可以：