无陀螺惯性测量系统约束角速度估计方法
【专利摘要】本发明提供了一种无陀螺惯性测量系统约束角速度估计方法，该方法具有估计步骤清晰明了、估计误差不随时间变化的特点，可用于无陀螺惯性测量系统冗余加速度计配置情况下角速度的估计，能够有效提高角速度的估计精度。
【专利说明】
无陀螺惯性测量系统约束角速度估计方法
技术领域
[0001] 本发明属于惯性测量应用领域，涉及惯性测量系统高精度导航计算方法，特指一 种基于扩展Kalman滤波技术的无陀螺惯性测量系统约束角速度估计方法。
【背景技术】
[0002] 传统的惯性测量系统包括三个陀螺仪以测量载体角速度，三个加速度计测量三个 方向比力，直接积分角速度可用，从而得到载体姿态，所以所得数值解(估计值）中的姿态误 差随着时间发生线性增长，而位置涉及到比力和姿态的二次积分，使得位置误差随着时间 的增长发生立方级数的增长。在无陀螺惯性导航系统中，载体的角加速度正比于加速度计 输出，需经过一次积分才能得到载体的角速度估计值，由于加速度计测量误差的影响，这使 得角速度估计值的误差随时间的增加而增大，而传统的有陀螺仪惯性测量系统，可以直接 用于测量角速度，测量精度只与陀螺仪本身的精度相关，避免了误差增大对角速度结果的 影响。正是因为无陀螺惯性测量系统需要积分才能得到角速度，其姿态误差与时间的平方 成正比，而位置误差与时间的四次方成正比，所以无陀螺惯性测量系统误差累积速度比传 统惯性系统更快。但正是由于无陀螺惯性测量系统中没有陀螺仪部件，所以具有可靠性高、 成本低的优势。
[0003] 对于无陀螺惯性测量系统而言，研究高精度的角速度估计方法非常重要，可以有 效降低无陀螺惯性测量系统导航计算误差积累速度，提高导航精度。这可以从两个方面考 虑：一方面是利用优良的误差补偿方法提高导航计算精度;另一方面是直接从导航解算方 法上入手。由于无陀螺惯性测量系统一般都采用冗余加速度计设计，加速度计个数一般都 大于六个，例如采用九加速度计构型、十二加速度计构型，每个加速度计都能够获得一个独 立测量量，而待解导航参数是加速度和角加速度，只有六个量，因此无陀螺惯性测量系统中 存在大量的冗余信息，利用无陀螺惯性测量系统加速度计输出信息可以直接得到角速度乘 积项、平方项的估计结果，但并不能够据此直接得到准确度较高的角速度的估计值。
【发明内容】
[0004] 本发明提供了一种无陀螺惯性测量系统约束角速度估计方法，该方法解决了现有 技术中对无陀螺惯性测量系统的冗余信息利用度低、所得角速度估计值与真实值之间的误 差会随的技术问题。
[0005] 本发明提供的一种无陀螺惯性测量系统约束角速度估计方法，步骤(1):对于所含 加速度计大于12个且能获得角速度二次项估计值的无陀螺惯性测量系统，利用所有加速度 计的输出得到线加速度、角加速度、角速度平方项、角速度交叉项次项的线性估计和估计方 差；
[0006] 步骤(1)包括以下步骤：
[0007] 假设无陀螺惯性测量系统包含N，其中N 2 12个加速度计，加速度计矢量Y的输出表 示为：
[0009]其中?为载体待测的三维角加速度矢量，妒为待测的载体三维视加速度矢量，ωχ， ωγ，ωζ*角速度ω的三个分量，Υ为Ν个加速度计组成的测量矢量；
，di为第i个加速度计敏感轴方向，<表示矢量di的转 置，η为第i个加速度计中心位置矢量4 ^ ?3^，1 = 1，2，-，小为加速度计个数，了 称为构型矩阵：
[0011]
:，则?ω为NX6维矩阵
，其中，Χ3为角速度交叉 项矢量，Χ4为角速度平方项矢量，Χ3和Χ4称为角速度二次项，贝lj:
[0013]其中X3为角速度交叉项、X4为角速度平方项，根据式(6)，角加速度矢量、视加速度 矢量、角速度交叉项和角速度平方项的线性估计为：
[0015] 此处所得为关于角加速度矢量?、视加速度矢量妒、角速度交叉项X3和角速度平 方项Χ4的中间估计量；
[0016] 无陀螺惯性测量系统中的各加速度计误差已完成标定和补偿，加速度计测量误差 为零均值高斯白噪声，测量方差为σ2,无陀螺系统中的各加速度计测量方差都相同，式(7) 的估计方差为：
[0017] Qest=([J D?]T[J DU])-1D[Y] = ([J D?]t[J Dm])-V (8)
[0018] 其中Qest为12X12维矩阵；
[0019] 步骤(2):根据角加速度的估计值，建立角速度微分方程，根据角加速度的估计方 差，构建角速度状态方程过程噪声，并将状态方程离散化；
[0020] 步骤(2)包括以下步骤：
[0021] 实际待估变量是角速度矢量ω，令Kalman滤波中的状态变量为角速度矢量ω = [ωχ c0y ωζ]τ，在离散情况下，角速度差分方程可表示为：
[0023]其中，Δ t为时间步长，cox,k、coy,k、co z,k*别为k时刻的Χ、Υ、Ζ三个方向的角速度 值，ω χ,η、ω y,η、ω ζ,η分别为k-Ι时刻的X、Υ、Ζ三个方向的角速度值，△ t为滤波时间步 长，'^,1{-1，《^,1^1，'?2,1^1为1^-1时刻角加速度解算的随机噪声，定义过程量 ：
[0025]实际计算中u可以根据式(7)估计的角加速度石乘以时间步长得到为：
[0027] 将式（11)代入式(9)，同时写成矢量形式，角速度ω的离散化状态动力学模型可表 示为：
[0028] ωk= ωk-i+Uk-i+Wk-i Δ t (2)
[0029] 根据式(8)得到的角加速度估计方差是加速度计随机误差的线性组合，为矩阵Qest 的第一行至第三行以及第一列至第三列组成的矩阵，式（12)动力学模型的过程噪声方差 为：
[0030] Q" = ( At)2Qest(l:3,l:3) (13)
[0031] 假设角速度状态变量初值为：
[0032] ?0,ο=[4? 4α]Γ (Μ)
[0033] 其中?Λ。，今.。，黾。是1^ = 0的初始时刻X、Υ、Ζ三个方向的角速度初始值，角速度初始 值的方差假设为Ρο/ο，为3 X 3矩阵，具体计算时根据情况给定；
[0034]角速度状态变量的预测值为：
[0035] =iVw-i+?i--i (!5)
[0036] 其中，表示k-Ι时刻的估计值，％/A_AKalman滤波中k时刻的预测值，Uk-i表 示第k-Ι时刻的计算值，根据Kalman滤波算法，得到预测值％的方差为：
[0037] Pk/k-i = Pk-i/k-i+QMk-i (16)
[0038] 其中Pk-i/k-i为k_ 1时刻估计值的方差矩阵，Pk/k-i为k时刻角速度预测值, A 4的方差 矩阵，Quk-i为k-Ι时刻过程的方差矩阵，由（13)式给出；
[0039] 步骤(3):根据角速度二次项的估计值和估计方差，建立角速度测量方程以及测量 噪声的方差矩阵，并将方程离散化；
[0040] 步骤(3)包括以下步骤：
[0041] 将式(7)给出的角速度二次项估计值作为滤波过程中的测量模型，同时考虑加速 度计测量所存在的随机误差，得到角速度二次项的测量模型表示为：
[0043 ]其中Vk是角速度二次项估计值的随机误差；
[0044] 随机误差方差表示为：
[0045] Rk = Qest(7:12,7:12) (18)
[0046] 同样根据式(8)可知，式（12)所示动力学模型的过程噪声与式（17)所示测量模型 噪声的协方差矩阵为矩阵式(8)所示Q est的第1行到3行和第7列到12列组成的矩阵：
[0047] Mk = Qest(l:3,7:12)At (19)
[0048] Mk为动力学模型过程噪声与测量模型噪声协方差阵为3X6维矩阵，Δ t为时间步 长，与式(10)中时间步长相同；
[0049] 测量模型的Jacobian矩阵Hk为：
[0051 ]其中为式(15)中的角速度预报值，S为求偏导数符号；
[0052]则Kalman滤波增益矩阵的更新方程为：
[0054]滤波方差的修正方程为：
[0056]其中，Pk/k为k时刻角速度估计值的方差矩阵，角速度变量的修正方程为：
[0058] 其中，表示k时刻的无陀螺惯性系统角速度估计值：
是根据(7)式计 算得到的角速度二次项解向量；
[0059] 根据结果，式（15)、式（16)、式(21)、式(22)和式(23)即为无陀螺惯性系统角速度 约束估计中Ka 1 man滤波递推公式，根据Ka 1 man滤波递推公式和滤波初值和初始方差 Ρθ/0即可得出％jPPk/k。
[0060] 本发明的技术效果：
[0061] 1、本发明提供无陀螺惯性测量系统约束角速度估计方法充分利用冗余加速度计 测量所得输出，首先获得角加速度、角速度二次项的线性估计，然后基于Kalman滤波法得到 准确度较高的角速度估计值，本发明提供方法得到的角速度估计值的误差不随测量时间的 增加而增大。
[0062] 2、本发明提供的无陀螺惯性测量系统约束角速度估计方法简单明了，所得结果准 确可靠，误差不随时间的增加而变化。
[0063] 3、本发明提供的无陀螺惯性测量系统约束角速度估计方法中角速度估计误差不 随测量时间增加而变大，使得所得估计值能等价于陀螺仪的角速度测量结果，减少所得估 计值与真实值之间的差值。
[0064] 4、本发明提供的无陀螺惯性测量系统约束角速度估计方法可根据实际使用中的 初始值进行实时递推计算。因此该方法可以有效提高无陀螺惯性测量系统角速度估计精 度。
[0065] 具体请参考根据本发明的无陀螺惯性测量系统约束角速度估计方法提出的各种 实施例的如下描述，将使得本发明的上述和其他方面显而易见。
【附图说明】
[0066] 图1是本发明优选实施例无陀螺惯性测量系统约束角速度估计方法的流程示意 图；
[0067] 图2是典型12加速度计无陀螺惯性测量系统的几何构型示意图，箭头方向表示加 速度计的敏感轴指向，加速度计中心在立方体棱的中点处，共四组12个加速度计，立方体边 长为0.2m;
[0068] 图3是本发明优选实施例方法用于仿真计算中时所得X方向角速度估计值减去角 速度真值的差值随滤波时间变化过程的示意图；
[0069] 图4是本发明优选实施例方法用于仿真计算中时所得Y方向角速度估计值减去角 速度真值的差值随滤波时间的变化过程示意图；
[0070] 图5是本发明优选实施例方法用于仿真计算中时所得Z方向角速度估计值减去角 速度真值时的差值随滤波时间的变化过程示意图；
[0071] 图6是本发明优选实施例方法用于仿真计算中时所得X、Y、Z三个方向上角速度估 计平均平方误差根随时间变化的情况示意图。
【具体实施方式】
[0072] 构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实 施例及其说明用于解释本发明，并不构成对本发明的不当限定。
[0073] 本文中得到的利用冗余信息提高角加速度和线加速度的计算精度，是无陀螺惯性 测量系统导航计算方法研究中的主要方向。众多研究人员对这一问题提出了多种角速度计 算方法，核心思想是利用无陀螺惯性系统中的冗余估计值估计载体角速度，但其算法设计 和估计精度上仍然有待完善和提尚。
[0074]为完成无陀螺惯性测量系统角速度的高精度估计，首先根据无陀螺惯性测量系统 构型特点和冗余加速度计输出，给出线加速度、角加速度、以及角速度二次项的线性估计； 其次根据角速度微分方程建立动力学模型，根据角速度二次项估计建立测量模型；最后利 用Kalman滤波技术得到角速度的高精度估计。
[0075] 本发明的技术方案是:首先根据无陀螺惯性系统构型特点和冗余加速度计输出， 给出线加速度、角加速度以及角速度二次项的线性估计;其次根据角速度微分方程建立动 力学模型，根据角速度二次项估计值建立测量模型;最后利用扩展Kalman滤波技术得到角 速度的高精度估计。本发明适用于具有12个以上加速度计且能获得角速度二次项估计值的 无陀螺惯性系统。
[0076] 参见图1，本发明提供的一种无陀螺惯性系统约束角速度估计方法，包括以下步 骤：
[0077] 本发明提供的一种无陀螺惯性测量系统约束角速度估计方法，步骤(1):对于所含 加速度计大于12个且能获得角速度二次项估计值的无陀螺惯性测量系统，利用所有加速度 计的输出得到线加速度、角加速度、角速度平方项、角速度交叉项次项的线性估计和估计方 差；
[0078] 步骤(1)包括以下步骤：
[0079]假设无陀螺惯性测量系统包含N，其中N 2 12个加速度计，加速度计矢量Y的输出表 示为：
[0081]其中?为载体待测的三维角加速度矢量，炉为待测的载体三维视加速度矢量， ωχ，ωγ，（〇2为角速度ω的三个分量，Υ为Ν个加速度计组成的测量矢量；
di为第i个加速度计敏感轴方向，<表示矢量di的转 置，ri为第i个加速度计中心位置矢量，ri = [rxi ryi rzi]T，i = 1，2，··，，N为加速度计个数，J 称为构型矩阵：
[0083]
<则0?为~乂6维矩阵
其中，X3为角速度交叉 项矢量，X4为角速度平方项矢量，X3和X4称为角速度二次项，贝1J:
[0085]其中X3为角速度交叉项、X4为角速度平方项，根据式(6)，角加速度矢量、视加速度 矢量、角速度交叉项和角速度平方项的线性估计为：
[0087] 此处所得为关于角加速度矢量?、视加速度矢量#、角速度交叉项X3和角速度平 方项X4的中间估计量；
[0088] 无陀螺惯性测量系统中的各加速度计误差已完成标定和补偿，加速度计测量误差 为零均值高斯白噪声，测量方差为σ2,无陀螺系统中的各加速度计测量方差都相同，式(7) 的估计方差为：
[0089] Qest=([J D?]T[J DU])-1D[Y] = ([J D?]t[J ?ω])^2 (8)
[0090] 其中Qest为12X12维矩阵；
[0091]步骤(2):根据角加速度的估计值，建立角速度微分方程，根据角加速度的估计方 差，构建角速度状态方程过程噪声，并将状态方程离散化；
[0092] 步骤(2)包括以下步骤：
[0093]实际待估变量是角速度矢量ω，令Kalman滤波中的状态变量为角速度矢量ω = [ωχ c0y ωζ]τ，在离散情况下，角速度差分方程可表示为：
[0095] 其中，Δ t为时间步长，cox,k、coy,k、co z,k分别为k时刻的Χ、Υ、Ζ三个方向的角速度 值，ω χ,η、ω y,η、ω ζ,η分别为k-Ι时刻的X、Υ、Ζ三个方向的角速度值，△ t为滤波时间步 长，'^,1{-1，《^,1^1，'?2,1^1为1^-1时刻角加速度解算的随机噪声，定义过程量 ：
[0096] u = [?.Δ/' ?. Δ/' ? Δ/'j (10)
[0097] 实际计算中u可以根据式(7)估计的角加速度?乘以时间步长得到为： |- ^ -.J'
[0098] u- ojM (h^At ? Α? 〇 ^)
[0099] 将式（11)代入式(9)，同时写成矢量形式，角速度ω的离散化状态动力学模型可表 示为：
[0100] ωk= ωk-i+Uk-i+Wk-i Δ t (3)
[0101] 根据式(8)得到的角加速度估计方差是加速度计随机误差的线性组合，为矩阵Qest 的第一行至第三行以及第一列至第三列组成的矩阵，式（12)动力学模型的过程噪声方差 为：
[0102] Q" = ( At)2Qest(l:3,l:3) (13)
[0103] 假设角速度状态变量初值为：
[0105] 其中仓"，今."，黾Q是1^ = 0的初始时刻X、Y、Z三个方向的角速度初始值，角速度初始 值的方差假设为Po/o，为3 X 3矩阵，具体计算时根据情况给定；
[0106] 角速度状态变量的预测值为：
[0107] i〇klk_, =i〇k_Vk^+uk_i (15)
[0108] 其中，表示k-1时刻的估计值，Ai^Kalman滤波中k时刻的预测值，Uk-1表 示第k-Ι时刻的计算值，根据Kalman滤波算法，得到预测值％,η的方差为：
[0109] Pk/k-i = Pk-i/k-i+QMk-i (16)
[0110] 其中Pk-Vk-Ak-l时刻估计值的方差矩阵，Pk/k-Ak时刻角速度预测值的方差 矩阵，Quk-i为k-Ι时刻过程的方差矩阵，由（13)式给出；
[0111] 步骤(3):根据角速度二次项的估计值和估计方差，建立角速度测量方程以及测量 噪声的方差矩阵，并将方程离散化；
[0112] 步骤(3)包括以下步骤：
[0113] 将式(7)给出的角速度二次项估计值作为滤波过程中的测量模型，同时考虑加速 度计测量所存在的随机误差，得到角速度二次项的测量模型表示为：
[0115] 其中外是角速度二次项估计值的随机误差；
[0116] 随机误差方差表示为：
[0117] Rk = Qest(7:12,7:12) (18)
[0118] 同样根据式(8)可知，式（12)所示动力学模型的过程噪声与式（17)所示测量模型 噪声的协方差矩阵为矩阵式(8)所示Q est的第1行到3行和第7列到12列组成的矩阵：
[0119] Mk = Qest(l:3,7:12)At (19)
[0120] Mk为动力学模型过程噪声与测量模型噪声协方差阵为3X6维矩阵，Δ t为时间步 长，与式(10)中时间步长相同；
[0121] 测量模型的Jacobian矩阵Hk为：
[0123] 其中为式(15)中的角速度预报值，(3为求偏导数符号；
[0124] 贝ijKalman滤波增益矩阵的更新方程为：
[0126]滤波方差的修正方程为：
[0128]其中，Pk/k为k时刻角速度估计值化,&的方差矩阵，角速度变量的修正方程为：
[0130] 其中，4,1表示k时刻的无陀螺惯性系统角速度估计值： 是根据(7)式计 算得到的角速度二次项解向量；
[0131] 根据结果，式（15)、式（16)、式(21)、式(22)和式(23)即为无陀螺惯性系统角速度 约束估计中Kalman滤波递推公式，根据Kalman滤波递推公式和滤波初值(UP初始方差Po/o 即可得出和Pk/k。
[0132] 通过上述方法可以实现对角速度的估计，所得角速度估计值的误差不随测量时间 的增加而增大。
[0133] 具体步骤如下：
[0134] 步骤(1):计算无陀螺惯性系统角速度二次项估计
[0135] 假设无陀螺惯性测量系统包含N(N 2 12)个加速度计，无陀螺惯性测量系统基本测 量方程为
[0137]其中&为载体待测的三维角加速度矢量，妒为待测的载体三维视加速度矢量，Ω 为角速度矢量形成的反对称矩阵，
，ωχ，《y，ωζ为角速度ω的三个分 量，Υ为Ν个加速度计组成的测量矢量，cU为第i个加速度计敏感轴方向，rff表示矢量cU的转 置，ri为第i个加速度计中心位置矢量，N为加速度计个数，J称为构型矩阵：
[0139] 在加速度计输出方程式（1)中，= 项可改写为：
[0145]
，则?ω为NX 6维矩阵
，.其中，X3为角速度交 叉项矢量，X4为角速度平方项矢量，X3和X4称为角速度二次项，贝lj
[0147]无陀螺惯性系统要求能获得角速度二次项估计，则角加速度矢量威、线视加速度 矢量#、角速度交叉项和角速度平方项的线性估计为
[0149] 假设无陀螺惯性测量系统中的加速度计误差已完成标定和补偿，加速度计测量误 差为零均值高斯白噪声，测量方差为σ2,无陀螺系统中加速度计测量方差都相同。可以看 出，此时角加速度?和线视加速度#的解算不需要角速度信息，同时还可以得到角速度交 叉项和平方项二次项的线性估计。式(7)的估计方差为
[0150] Qest=([J D?]T[J DU])-1D[Y] = ([J D?]t[J ?ω])^2 (8)
[0151] 其中Qest为12X12维矩阵，角加速度、线视加速度以及角速度二项项的估计方差只 与加速度计测量精度和无陀螺惯性测量系统构型参数有关，而与时间无关，从而避免了工 作时间对估计结果的影响。
[0152] 步骤(2):根据角加速度估计值，建立角速度动力学方程，根据角加速度的估计方 差，给出动力学方程噪声，并将动力学方程离散化；
[0153] 令状态变量为角速度矢量ω=[ωχ c0y ωζ]τ，在离散情况下，角速度差分方程可 表示为，
[0155] 其中Δ t为时间步长，cox,k、coy,k、《^分别为k时刻的Χ、Υ、Ζ三个方向的角速度值， ωχ^、ωγ,!^、ωζ,Η分别为k-Ι时刻的Χ、Υ、Ζ三个方向的角速度值，Δ t为滤波时间步长， ￥\1{-1，《^,1^1，'?2,1^1为1^-1时刻角加速度解算的随机噪声，定义过程量
[0156] u - 0)t At (b (10)
[0157] 实际计算中u可以根据式(7)估计的角加速度石乘以时间步长得到，
[0158] u - ?λΑ? ?νΑ? (11)
[0159] 将式（11)代入式(9)，同时写成矢量形式，角速度ω的离散化状态动力学模型可表 示为
[0160] ωk= ω k-i+Uk-i+Wk-i Δ t (4)
[0161] 根据式(8)得到的角加速度估计方差是加速度计随机误差的线性组合，为矩阵Qest 的第一行至第三行和第一列至第三列组成的矩阵，式（12)动力学模型的过程噪声方差为
[0162] Q" = ( At)2Qest(l:3,l:3) (13)
[0163] 假设角速度状态变量初值为
[0164] ?〇/?=[??0 0r0 ωζ0] (14)
[0165] 其中命c，黾。是初始时刻X、Υ、Ζ三个方向的角速度值初值，角速度初始值方差假 设为Ρ〇/〇，为3 X 3矩阵，初值<&_和初始方差Ρο/ο需要在实际应用时给定；
[0166] 角速度状态变量的预测值为
[0167] (hklk^ =&k^,k_i + uk^ Π 5)
[0168] 其中，％.__表示k-1时刻的估计值，为Kalman滤波中k时刻的预测值，Uk-i表 示第k-Ι时刻的计算值，根据Kalman滤波算法，得到预测值化^的方差为：
[0169] Pk/k-i = Pk-i/k-i+Quk-i (16)
[0170] 其中Pk-Vk-Ak-l时刻估计值的方差矩阵，Pk/k-Ak时刻角速度预测值的方差 矩阵，Qcjk-i为k_l时刻过程方差矩阵，由（13)式给出；
[0171] 步骤(3):根据角速度二次项的估计值和估计方差，建立角速度测量方程以及测量 噪声的方差矩阵，并将方程离散化；由于步骤（1)中并未得到角速度估计值，只得到了角加 速度和角速度二次项估计值，所以还需要进一步得到角速度估值。
[0172] 将式(7)给出的角速度二次项估计值作为滤波过程中的测量模型，同时考虑加速 度计测量所存在的随机误差，所得角速度二次项的测量模型可以表示为：
[0174] 其中Vk是角速度二次项估计值的随机误差。根据式(8)可知，该随机误差方差是加 速度计随机误差的线性组合，为矩阵Q est的第7行到第12行和第7列到第12列组成的方阵，为
[0175] Rk = Qest(7:12,7:12) (18)
[0176] 同样根据式(8)可知，式（12)所示动力学模型的过程噪声与式（17)所示测量模型 噪声的协方差矩阵为矩阵式(8)所示Q est的第1行到3行和第7列到12列组成的矩阵
[0177] Mk = Qest(l:3,7:12)At (19)
[0178] Mk为动力学模型过程噪声与测量模型噪声协方差阵，At为时间步长，为3X6维。
[0179] 测量模型的Jacobian矩阵Hk为：
[0181] 其中％_为式(15)中的角速度预报值，Θ为求偏导数符号。
[0182] 贝ijKalman滤波增益矩阵的更新方程为：
[0184]滤波方差的修正方程为
[0186]其中Pk/k为k时刻角速度估计值的方差矩阵，角速度变量的修正方程为
[0188] 其中A,A表示k时刻的估计值 是根据(7)式计算得到的角速度二次项解
向量。
[0189] 根据结果，式（15)、式（16)、式(21)、式(22)、式(23)即是无陀螺惯性系统角速度约 束估计中Kalman滤波递推公式，本发明同时给出了上述5个公式中所有变量的意义及计算 公式，这是无陀螺惯性系统角速度约束估计的关键所在。在实际应用中给定滤波初值0_和 初始方差Po/o,即可递推得到相应的估计值。％?Sk时刻无陀螺惯性系统角速度估计值， Pk/k为k时刻角速度估计方差矩阵。
[0190] 为了验证本发明提供的方法，具体算例如下：
[0191]根据载体建立体坐标系，为笛卡尔直角坐标系。载体三个方向的加速度分别为[1 0.5 2]m/s2,载体三轴都存在角运动，三方向角加速度为0.1X[-1 0.4t-l 2]rad/s2,角速 度初值为[3 -2 4]rad/s，其中t为时间，计算步长为0.01秒。为简便起见，假设重力加速度 始终被一外力平衡。
[0192] 为验证本发明的结果，采用一种十二加速度计无陀螺惯性测量系统作为计算例 子，图2给出了该系统的构型几何，但本发明并不仅限于该构型，可以适用于满足前提条件 的任意构型。MEMS加速度计测量随机误差为零均值高斯白噪声，根据目前MEMS加速度计商 业货架产品的主流精度水平，选择输出噪声均方差~=lmg，则加速度计测量方差〇 2 = 0.0001m2/s4〇
[0193] 步骤(1):根据图2中所示的12加速度计无陀螺惯性系统，构型矩阵为
[0198]则加速度计输出矢量可表示为
[0200]根据式(7)角加速度矢量?、线视加速度矢量妒、角速度交叉项和角速度平方项的 线性估计为
[0202]式(7)的估计方差为
[0204] Qest为12X12维矩阵，与时间无关。
[0205] 步骤(2):根据角加速度估计值，建立角速度动力学方程，根据角加速度的估计方 差，给出动力学方程噪声，并将动力学方程离散化，时间步长为0.01秒。
[0206] 令状态变量为角速度矢量ω=[ωχ c0y ωζ]τ，在离散情况下，角速度差分方程可 表示为，
[0208] 其中cox,k、coy,k、ωζ>1^别为k时刻的X、Y、Z三个方向的角速度值，co x,k-!、coy,k- ?★:分别为k-i时刻的Χ、γ、ζ三个方向的角速度值，mwy.k^wz.k-Ak-i时刻角加速度 解算的随机噪声，定义过程量
[0210]实际计算中u可以根据式⑴估计的角加速度I乘以时间步长得到，
[0212] 将式（11)代入式(9)，同时写成矢量形式，角速度ω的离散化状态动力学模型可表 示为
[0213] ω k = ω k-i+Uk-i+0.01 wk-i (5)
[0214] 根据式(8)得到的角加速度估计方差是加速度计随机误差的线性组合，为矩阵Qest 的第一行至第三行和第一列至第三列组成的矩阵，式（12)动力学模型的过程噪声方差为
[0216]角速度状态变量滤波初值取为
[0218] 角速度状态变量的预测值为
[0219] ioklk_{ =cbt^t+ut^ (15j
[0220] 预测值U勺方差为
[0221] Pk/k-i = Pk-ι/k-l+Qcjk-1 (16)
[0222] 步骤(3):根据角速度二次项的估计值和估计方差，建立角速度测量方程以及测量 噪声的方差矩阵，并将方程离散化；
[0223] 将式(7)给出的角速度二次项估计值作为滤波过程中的测量模型，同时考虑加速 度计测量所存在的随机误差，所得角速度二次项的测量模型可以表示为
[0225]其中vk是角速度二次项估计值的随机误差。根据式(8)可知，该随机误差方差是加 速度计随机误差的线性组合，为矩阵Qest的第7行到第12行和第7列到第12列组成的方阵，为
[0226]
[0227] 式（12)所示动力学模型的过程噪声与式（17)所示测量模型噪声的协方差矩阵为 矩阵式(8)所示Qest的第1行到3行和第7列到12列组成的矩阵
[0228]
[0229] Mk为动力学模型过程噪声与测量模型噪声协方差阵，为3 X 6维矩阵。
[0230] 测量模型的Jacobian矩阵Hk见式（20)，Kalman滤波增益矩阵的更新方程见（21)， 滤波方差的修正方程见(22 )，角速度变量的修正方程见(23)式。
[0231 ]滤波初始方差取为
[0233] 根据式(15)、式（16)、式(21)、式(22)、式(23)给出的递推公式，式(11)、式（13)、式 (18)、式（19)、式(20)给出的相关变量计算公式，以及滤波初值0_和初始方差p Q/Q，即可完 成无陀螺惯性系统角速度的递推估计。
[0234] 所得结果列于图3~6中，由图3可见X方向角速度估计的误差不会随时间的增加而 变大，最大值不超过〇.〇1弧度/秒，有图4可见，Y方向角速度估计的误差不会随时间的增加 而变大，同样的图5所示为Z方向角速度估计误差不会随时间的增加而变大。图6中所示为X、 Y、Z三个方向角速度估计值的均方差，可以看出均方差随时间增加缓慢变小。这说明本发明 提供的角速度估计方法所得角速度误差估计不随时间增加而变大，可以有效提供无陀螺惯 性测量系统角速度估计精度。
[0235] 本发明所提出无陀螺惯性测量系统约束角速度估计方法具有以下优点：（1)估计 方法步骤清晰，简单明了，合理可行；（2)角速度估计误差不随测量时间增加而变大，等价于 陀螺仪的角速度测量功能；（3)该方法可实时递推计算。因此该方法可以有效提高无陀螺惯 性测量系统角速度估计精度。
[0236] 本领域技术人员将清楚本发明的范围不限制于以上讨论的示例，有可能对其进行 若干改变和修改，而不脱离所附权利要求书限定的本发明的范围。尽管己经在附图和说明 书中详细图示和描述了本发明，但这样的说明和描述仅是说明或示意性的，而非限制性的。 本发明并不限于所公开的实施例。
[0237] 通过对附图，说明书和权利要求书的研究，在实施本发明时本领域技术人员可以 理解和实现所公开的实施例的变形。在权利要求书中，术语"包括"不排除其他步骤或元素， 而不定冠词"一个"或"一种"不排除多个。在彼此不同的从属权利要求中引用的某些措施的 事实不意味着这些措施的组合不能被有利地使用。权利要求书中的任何参考标记不构成对 本发明的范围的限制。
【主权项】
1. 一种无陀螺惯性测量系统约束角速度估计方法，其特征在于，包括以下步骤： 步骤（1):对于所含加速度计大于12个且能获得角速度二次项估计值的无陀螺惯性测 量系统，利用所有所述加速度计的输出得到线加速度、角加速度、角速度平方项、角速度交 叉项次项的线性估计和估计方差； 所述步骤(1)包括以下步骤： 所述无陀螺惯性测量系统包含N个加速度计，其中N 2 12，所述加速度计矢量Y的输出表 示为：(5) 其中?为载体待测的三维角加速度矢量，#为待测的载体三维视加速度矢量，ωχ，ωγ， ω ζ为角速度ω的三个分量，Υ为Ν个加速度计组成的测量矢量；，di为第i个加速度计敏感轴方向，< 表示矢量di的 转置，ri为第i个加速度计中心位置矢量，ri = [rxi ryi rzi]T，i = l，2，···Ν，Ν为加速度计个 数，J为构塑矩阵： ?2ι i_\ j， "/ -i/yx0 -，则为NX6维矩阵，，其中，X3为角速度交叉项矢 量，X4为角速度平方项矢量，X3和X4为角速度二次项，则：(6) 其中X3为角速度交叉项、X4为角速度平方项，根据式(6)，所述角加速度矢量、视加速度 矢量、角速度交叉项和角速度平方项的线性估计为： (7) 此处所得为关于角加速度矢量士、.视加速度矢量#、角速度交叉项X3和角速度平方项X4 的中间估计量； 所述无陀螺惯性测量系统中的各加速度计误差已完成标定和补偿，加速度计测量误差 为零均值高斯白噪声，测量方差为σ2,所述无陀螺系统中的各所述加速度计测量方差都相 同，式(7)的估计方差为： Qest=([J D?]T[J DU])-1D[Y] = ([J D?]t[J ?ω])-ν (8) 其中Qest为12X12维矩阵； 步骤(2):根据所述角加速度的估计值，建立角速度微分方程，根据所述角加速度的估 计方差，构建角速度状态方程过程噪声，并将所述状态方程离散化； 所述步骤(2)包括以下步骤： 实际待估变量是角速度矢量ω，令Kalman滤波中的状态变量为角速度矢量ω = [ωχ ?y ωζ]τ，在离散情况下，角速度差分方程表示为：(9) 其中，At为时间步长，cox,k、coy,k、co z,k分别为k时刻的Χ、Υ、Ζ三个方向的角速度值， ωχ,^、coz,k_^别为k-Ι时刻的Χ、Υ、Ζ三个方向的角速度值，At为滤波时间步长，通 过解算公式(9)，得到所述角速度矢量ω，1,1 {-1，^,1{-1，1,1{- 1为卜1时刻角加速度解算的随机 噪声，定义过程量：(10) 实际计算中u可以根据式(7)估计的角加速度?乘以时间步长得到： L」 （11) 将式（11)代入式（9)，同时写成矢量形式，角速度ω的离散化状态动力学模型可表示 为： ω k= ω k-i+Uk-i+Wk-i A t (1) 根据式(8)得到的所述角加速度估计方差是加速度计随机误差的线性组合，为矩阵Qest 的第一行至第三行以及第一列至第三列组成的矩阵，式（12)动力学模型的过程噪声方差 为： Θω=( At)2Qest(l：3,l：3) (13) 假设角速度状态变量初值为：(14) 其中4。，今.。，是k = 0的初始时刻X、Y、Z三个方向的角速度初始值，所述角速度初始值 (15) 的方差假设为Po/o，为3 X 3矩阵，具体计算时根据情况给定； 角速度状态变量的预测值为： 其中，表示k-Ι时刻的估计值，为Kalman滤波中k时刻的预测值，Uk-i表示第 k-Ι时刻的计算值，根据Kalman滤波算法，得到预测值的方差为： Pk/k-l - Pk-1/k-1+QcJk-1 ( 16 ) 其中Pk-Vk-^k-l时刻估计值的方差矩阵，pk/k-Ak时刻角速度预测值的方差矩 阵，Qui^Sk-l时刻过程的方差矩阵，由（13)式给出； 步骤(3):根据角速度二次项的估计值和估计方差，建立角速度测量方程以及测量噪声 的方差矩阵，并将方程离散化； 所述步骤(3)包括以下步骤： 将式(7)给出的角速度二次项估计值作为滤波过程中的测量模型，同时考虑所述加速 度计测量所存在的随机误差，得到角谏度二次烦的测量樽型表示为：Π 7) 其中Vk是角速度二次项估计值的随机误差； 所述随机误差方差表示为： Rk = Qest(7:12,7:12) (18) 同样根据式(8)可知，式（12)所示动力学模型的过程噪声与式（17)所示测量模型噪声 的协方差矩阵为矩阵式(8)所示Qd的第1行到3行和第7列到12列组成的矩阵： Mk = Qest(l:3,7:12) At (19) Mk为动力学模型过程噪声与测量模型噪声协方差阵为3X6维矩阵，At为时间步长，与 式(10)中时间步长相同； 所述测量模型的Jacobian矩阵Hk为：(2Π) 其中为式(15)中的角速度预报值，δ为求偏导数符号； 贝ijKalman滤波增益矩阵的更新方程为：(2D 滤波方差的修正方程为：(22) 其中，Pk/k为k时刻角速度估计值的方差矩阵，角速度变量的修正方程为： (23) 其中，,表示k时刻的无陀螺惯性系统角速度估计值，只=&是根据(7)式计算得 到的角速度二次项解向量； 根据所述结果，式（15)、式（16)、式(21)、式(22)和式(23)即为无陀螺惯性系统角速度 约束估计中Kalman滤波递推公式，根据所述Kalman滤波递推公式和滤波初值4/()和初始方 差Pq/q即可得出所述％ /4_和所述Pk/k。
【文档编号】G01P3/00GK105866459SQ201610180095
【公开日】2016年8月17日
【申请日】2016年3月25日
【发明人】杨华波, 李安梁, 张士峰
【申请人】中国人民解放军国防科学技术大学