基于自适应无迹卡尔曼滤波的惯导姿态补偿方法技术

本发明专利技术公开了一种基于自适应无迹卡尔曼滤波的惯导姿态补偿方法，主要解决现有技术采用无迹卡尔曼滤波精度不准的问题，其实现方案是，确定每个时刻自适应无迹卡尔曼滤波的状态向量；确定每个时刻的自适应无迹卡尔曼滤波的量测向量；通过量测向量建立关于自适应系数的最大似然函数；通过比较不同状态下每一个时刻的似然函数，取得最优的自适应系数；根据选取的自适应系数求得状态协方差矩阵和量测协方差矩阵；根据状态协方差矩阵和量测协方差矩阵求得自适应无迹卡尔曼滤波的增益系数；根据增益系数更新每一个时刻惯导姿态信息的状态，得到补偿后的状态。本发明专利技术能准确的测量惯导姿态数据，提高了室内定位精度，可用于室内定位系统。统。统。

【技术实现步骤摘要】
基于自适应无迹卡尔曼滤波的惯导姿态补偿方法


[0001]本专利技术属于导航
，特别是涉及一种惯导定位方法，可用于室内定位系统。

技术介绍

[0002]室内定位技术在当今生活中不可或缺，常用的室外定位导航的全球定位系统在室内复杂环境中，信号弱且不能保证信息有效性，为了满足各种室内定位的需求，研究人员已经研究开发了很多适合室内环境的定位系统，从单一数据源定位到多种方式相互辅助定位，部分技术可取得较为满意的定位效果。
[0003]上个世纪六十年代开始，卡尔曼滤波理论就被使用来对根据系统的量测值去消除系统的随机干扰。卡尔曼滤波通过实时更新均值和协方差实现滤波过程，是一种适用于线性系统的估计方法，而在现实中，需要估计状态所组成的系统常常是非线性的。因此在随后的这几十年间发展了一系列基于卡尔曼滤波的非线性估计算法，如拓展卡尔曼滤波，无迹卡尔曼滤波，粒子滤波。其中：
[0004]拓展卡尔曼滤波，是先通过将非线性系统线性化，再使用卡尔曼滤波的方式进行估计，这种滤波应对非线性较低的系统有良好的性能，但在非线性强的场合却难以给出可靠的估计。
[0005]无迹卡尔曼滤波，是通过无迹变换后实现滤波，这种滤波能够提供更高阶的估计精度，同时计算量更小，应对较高非线性的系统也能够给出较好的估计结果。
[0006]粒子滤波，是一个完全的非线性估计器，对于非线性系统具有更好的性能表现，但是计算量将大幅增加，使用于特定高精度场合。
[0007]上述卡尔曼滤波及其拓展形式都是建立在已知模型的情况下，虽然对于稳定的系统可以实现很好的估计，但是对于模型不确定的情况就会出现滤波效果变差，估计结果偏差变大，从而导致在多种运动结合的情况下定位结果变差差的问题。
[0008]北京理工大学在其申请号为CN201710365995.5的专利文献中公开了“一种基于无迹卡尔曼滤波的组合定位方法”，其首先通过惯性导航模块测得惯性导航运动数据，再通过可见光定位单元解算出可见光定位位置坐标；最后通过无迹卡尔曼滤波算法将两者数据进行融合，达到提高定位精度的目的。但该方法采用的无迹卡尔曼滤波，由于其尺度参数在滤波实验之前进行选择，且在整个过程中尺度参数不变，使得其设计要受到尺度参数的限制，因而严重影响了状态估计的质量，导致定位精度较差。

技术实现思路

[0009]本专利技术的目的在于针对上述现有技术缺陷，提出了一种基于自适应无迹卡尔曼滤波的室内定位方法，以通过最大似然函数法对无迹卡尔曼滤波中的尺度参数进行最高值的选择，提高定位精度。
[0010]为实现上述目的，本专利技术采取的技术方案包括如下步骤：
[0011](1)确定每个时刻自适应无迹卡尔曼滤波的状态向量X
k
：
[0012](1a)利用BWT901BLECL惯导模块测量每个时刻的姿态，组成状态向量X；
[0013](1b)对每个时刻的姿态做无迹变换，得到自适应无迹卡尔曼滤波的状态向量X
k
；
[0014](2)确定每个时刻的自适应无迹卡尔曼滤波的量测向量Z
k
：
[0015](2a)利用惯导模块测量每个时刻的速度误差，组成量测向量Z；
[0016](2b)对每个时刻的速度误差做无迹变换，得到自适应无迹卡尔曼滤波的量测向量Z
k
；
[0017](3)通过最大似然函数法确定每个时刻自适应无迹卡尔曼滤波的自适应系数：
[0018](3a)根据量测向量Z
k
建立如下似然函数：
[0019][0020]其中，为量测向量Z
k
的平均值，为协方差，α为自适应系数；
[0021](3b)根据(3a)建立的似然函数，确定最适合的自适应系数：
[0022](3b1)将初始状态i＝0指定为自适应系数α在k时刻的可行域：k
min
≤α≤k
max
；
[0023](3b2)设置步进值m，计算在状态i下自适应系数α取值为i*m+k
min
时的概率密度
[0024][0025](3b3)将当前状态i与前一状态i
‑
1中自适应系数α取值时最大的概率密度进行比较，选取最优的自适应系数α：
[0026]如果当前状态i下的概率密度大于前一状态i
‑
1时的概率密度，则取α＝i*m+k
min
，
[0027]否则α＝(i
‑
1)*m+k
min
；
[0028](4)根据选取的自适应系数α，求得的状态协方差矩阵和量测协方差矩阵将这两个矩阵代入卡尔曼滤波增益公式，确定每个时刻自适应无迹卡尔曼滤波的增益系数E
k
；
[0029](5)根据选取的增益系数E
k
更新每一时刻惯导姿态信息的状态，得到补偿后的状态
[0030][0031]本专利技术根据无迹卡尔曼滤波的尺度参数会严重影响系统状态估计质量的情况，在滤波器中增加了自适应调整函数，并通过调整状态向量和观测向量的协方差来调整无迹卡尔曼的尺度参数，进一步提高了室内定位的鲁棒性和平均定位精度。
附图说明
[0032]图1为本专利技术的实现流程图；
[0033]图2为本专利技术中的惯导模块移动轨迹图；
[0034]图3为用本专利技术与现有方法采集的航向角的仿真对比图。
具体实施方式
[0035]下面结合附图对本专利技术的实施例和效果，作进一步详细描述。
[0036]本实例的测量装置为搭载有BWT901BLECL惯导模块的室内无人车，其中无人车的主控单元为stm32微控制器，BWT901BLECL惯导模块以串口连接的方式和小车上的stm32微
控制器相连。
[0037]参照图1，对本实例的实现步骤如下。
[0038]步骤1，确定每个时刻自适应无迹卡尔曼滤波的状态向量X
k
。
[0039]1.1)获取每个时刻BWT901BLECL惯导模块测量的姿态值，组成状态向量X：
[0040]搭载在无人车上的微控制器利用中断控制的方式，控制其上的BWT901BLECL惯导模块每隔1秒采集一次姿态数据，组成状态向量X：
[0041][0042]其中，δv
x
，δv
y
分别为x轴，y轴方向的速度误差；分别为x轴，y轴，z轴方向的方位失调角；ε
x
，ε
y
分别为x轴，y轴方向的陀螺漂移；Δ
x
，Δ
y
分别为x轴，y轴方向的加速度计零偏；
[0043]1.2)对每个时刻的姿态做无迹变换，得到自适应无迹卡尔曼滤波的状态向量X
k
：
[0044]1.2.1)计算第k个定位时刻下状态i取不同数值的状态向量
[0045][0046][0047][0048]式本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于自适应无迹卡尔曼滤波的惯导姿态补偿方法，其特征在于，包括如下:(1)确定每个时刻自适应无迹卡尔曼滤波的状态向量X
k
：(1a)利用BWT901BLECL惯导模块测量每个时刻的姿态，组成状态向量X；(1b)对每个时刻的姿态做无迹变换，得到自适应无迹卡尔曼滤波的状态向量X
k
；(2)确定每个时刻的自适应无迹卡尔曼滤波的量测向量Z
k
：(2a)利用惯导模块测量每个时刻的速度误差，组成量测向量Z；(2b)对每个时刻的速度误差做无迹变换，得到自适应无迹卡尔曼滤波的量测向量Z
k
；(3)通过最大似然函数法确定每个时刻自适应无迹卡尔曼滤波的自适应系数：(3a)根据量测向量Z
k
建立如下似然函数：其中，为量测向量Z
k
的平均值，为协方差，α为自适应系数；(3b)根据(3a)建立的似然函数，确定最适合的自适应系数：(3b1)将初始状态i＝0指定为自适应系数α在k时刻的可行域：k
min
≤α≤k
max
；(3b2)设置步进值m，计算在状态i下自适应系数α取值为i*m+k
min
时的概率密度时的概率密度(3b3)将当前状态i与前一状态i
‑
1中自适应系数α取值时最大的概率密度进行比较，选取最优的自适应系数α：如果当前状态i下的概率密度大于前一状态i
‑
1时的概率密度，则取α＝i*m+k
min
，否则α＝(i
‑
1)*m+k
min
；(4)根据选取的自适应系数α，求得的状态协方差矩阵和量测协方差矩阵将这两个矩阵代入卡尔曼滤波增益公式，确定每个时刻自适应无迹卡尔曼滤波的增益系数E
k
；(5)根据选取的增益系数E
k
更新每一时刻惯导姿态信息的状态，得到补偿后的状态更新每一时刻惯导姿态信息的状态，得到补偿后的状态2.根据权利要求1所述的方法，其特征在于，步骤(1a)中组成的状态向量X，表示如下：其中，δv
x
，δv
y
分别为x轴，y轴方向的速度误差；分别为x轴，y轴，z轴方向的方位失调角；ε
x
，ε
y
分别为x轴，y轴方向的陀螺漂移；Δ
x
，Δ
y
分别为x轴，y轴方向的加速度计零偏。3.根据权利要求1所述的方法，其特征在于，步骤(1b)中对每个时刻的姿...

【专利技术属性】
技术研发人员：唐影，王重月，钱海雷，邓军，
申请(专利权)人：昆山九毫米电子科技有限公司，
类型：发明
国别省市：