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

1.本发明属于导航技术领域，特别是涉及一种惯导定位方法，可用于室内定位系统。


背景技术：

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


技术实现要素：

9.本发明的目的在于针对上述现有技术缺陷，提出了一种基于自适应无迹卡尔曼滤波的室内定位方法，以通过最大似然函数法对无迹卡尔曼滤波中的尺度参数进行最高值的选择，提高定位精度。
10.为实现上述目的，本发明采取的技术方案包括如下步骤：
11.(1)确定每个时刻自适应无迹卡尔曼滤波的状态向量xk：
12.(1a)利用bwt901blecl惯导模块测量每个时刻的姿态，组成状态向量x；
13.(1b)对每个时刻的姿态做无迹变换，得到自适应无迹卡尔曼滤波的状态向量xk；
14.(2)确定每个时刻的自适应无迹卡尔曼滤波的量测向量zk：
15.(2a)利用惯导模块测量每个时刻的速度误差，组成量测向量z；
16.(2b)对每个时刻的速度误差做无迹变换，得到自适应无迹卡尔曼滤波的量测向量zk；
17.(3)通过最大似然函数法确定每个时刻自适应无迹卡尔曼滤波的自适应系数：
18.(3a)根据量测向量zk建立如下似然函数：
[0019][0020]
其中，为量测向量zk的平均值，为协方差，α为自适应系数；
[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)根据选取的自适应系数α，求得的状态协方差矩阵和量测协方差矩阵将这两个矩阵代入卡尔曼滤波增益公式，确定每个时刻自适应无迹卡尔曼滤波的增益系数ek；
[0029]
(5)根据选取的增益系数ek更新每一时刻惯导姿态信息的状态，得到补偿后的状态
[0030][0031]
本发明根据无迹卡尔曼滤波的尺度参数会严重影响系统状态估计质量的情况，在滤波器中增加了自适应调整函数，并通过调整状态向量和观测向量的协方差来调整无迹卡尔曼的尺度参数，进一步提高了室内定位的鲁棒性和平均定位精度。
附图说明
[0032]
图1为本发明的实现流程图；
[0033]
图2为本发明中的惯导模块移动轨迹图；
[0034]
图3为用本发明与现有方法采集的航向角的仿真对比图。
具体实施方式
[0035]
下面结合附图对本发明的实施例和效果，作进一步详细描述。
[0036]
本实例的测量装置为搭载有bwt901blecl惯导模块的室内无人车，其中无人车的主控单元为stm32微控制器，bwt901blecl惯导模块以串口连接的方式和小车上的stm32微
控制器相连。
[0037]
参照图1，对本实例的实现步骤如下。
[0038]
步骤1，确定每个时刻自适应无迹卡尔曼滤波的状态向量xk。
[0039]
1.1)获取每个时刻bwt901blecl惯导模块测量的姿态值，组成状态向量x：
[0040]
搭载在无人车上的微控制器利用中断控制的方式，控制其上的bwt901blecl惯导模块每隔1秒采集一次姿态数据，组成状态向量x：
[0041][0042]
其中，δv
x
，δvy分别为x轴，y轴方向的速度误差；分别为x轴，y轴，z轴方向的方位失调角；ε
x
，εy分别为x轴，y轴方向的陀螺漂移；δ
x
，δy分别为x轴，y轴方向的加速度计零偏；
[0043]
1.2)对每个时刻的姿态做无迹变换，得到自适应无迹卡尔曼滤波的状态向量xk：
[0044]
1.2.1)计算第k个定位时刻下状态i取不同数值的状态向量
[0045][0046][0047][0048]
式中，i为状态序号，i∈[0，2n]；α是影响精度的自适应系数；n为无迹变换的展开系数n∈[1，∞)；
[0049]
1.2.2)根据第k个定位时刻的2n+1个自适应无迹卡尔曼滤波状态向量，计算第k个定位时刻最终的自适应无迹卡尔曼滤波状态向量xk：
[0050][0051]
步骤2，确定每个时刻的自适应无迹卡尔曼滤波的量测向量zk。
[0052]
2.1)搭载在无人车上的微控制器利用中断控制的方式，控制其上的bwt901blecl惯导模块每隔1秒采集一次速度误差数据，组成量测向量z：
[0053]
z＝[δv
x
，δvy]
[0054]
其中，δv
x
，δvy分别为x轴，y轴方向的速度误差；
[0055]
2.2)对每个时刻的速度误差做无迹变换，得到自适应无迹卡尔曼滤波的姿态向量zk：
[0056]
2.2.1)计算第k个定位时刻下状态i取不同数值的姿态向量
[0057][0058][0059][0060]
式中，i为状态序号，i∈[0，2n]；α是影响精度的自适应系数；n为无迹变换的展开系数n∈[1，∞)；
[0061]
2.2.2)根据第k个定位时刻的2n+1个自适应无迹卡尔曼滤波姿态向量，计算第k个
定位时刻最终的自适应无迹卡尔曼滤波姿态向量zk：
[0062][0063]
步骤3，确定每个时刻的自适应无迹卡尔曼滤波的自适应系数。
[0064]
3.1)根据量测向量zk建立如下似然函数：
[0065][0066]
其中，为量测向量zk的平均值，为协方差，α为自适应系数；
[0067]
3.2)根据(3.1)建立的似然函数，确定最适合的自适应系数：
[0068]
3.2.1)将初始状态i＝0指定为自适应系数α在k时刻的可行域：k
min
≤α≤k
max
，
[0069]
其中k
min
为bwt901blecl惯导模块测量开始时刻，k
max
为bwt901blecl惯导模块测量结束时刻；
[0070]
3.2.2)设置步进值m，计算在状态i下自适应系数α取值为i*m+k
min
时的概率密度
[0071][0072]
3.2.3)将当前状态i与前一状态i-1中自适应系数α取值时最大的概率密度进行比较，选取最优的自适应系数α：
[0073]
若当前状态i下的概率密度大于前一状态i-1时的概率密度，则选取α＝i*m+k
min
，
[0074]
否则，选取α＝(i-1)*m+k
min
；
[0075]
步骤4，确定每个时刻自适应无迹卡尔曼滤波的增益系数ek。
[0076]
4.1)计算第k个时刻自适应无迹卡尔曼滤波的状态协方差矩阵
[0077][0078]
其中，为第k个定位时刻下状态i取不同数值的状态向量，xk为第k个定位时刻的自适应无迹卡尔曼滤波状态向量；
[0079]
4.2)计算第k个时刻自适应无迹卡尔曼滤波的量测协方差矩阵
[0080][0081]
其中，为第k个定位时刻下状态i取不同数值的状态向量，zk为第k个定位时刻的自适应无迹卡尔曼滤波状态向量；
[0082]
4.3)根据自适应无迹卡尔曼滤波的状态协方差矩阵和量测协方差矩阵得到第k个定位时刻自适应无迹卡尔曼滤波的增益系数ek：
[0083][0084]
其中为取逆运算。
[0085]
步骤5，根据选取的增益系数ek更新每一时刻惯导姿态信息的状态，得到补偿后的状态
[0086]
[0087]
下面结合仿真实验对本发明的效果做进一步的说明：
[0088]
1.仿真条件。
[0089]
本发明的仿真实验的硬件平台为：处理器为intel i7-6700的笔记本电脑、搭载有bwt901blecl惯导模块的室内无人车。
[0090]
仿真软硬件环境：windows 10，matlab r2017a。
[0091]
本发明仿真实验所用到的数据，采集自西安电子科技大学西大楼实验室内，采集时间为2022年10月，样本集的内容为bwt901blecl惯导模块采集的300个采样点的航向角输出数据。
[0092]
2.仿真内容及其结果分析。
[0093]
为了验证基于自适应调谐的无迹卡尔曼滤波算法对姿态信息的补偿作用，移动平台按照如图2所示的轨迹运动，从初始位置[0，0]开始，沿直线匀速移动一段距离后，向右转过90度角，继续行驶。用本发明和现有“一种基于无迹卡尔曼滤波的组合定位方法”对惯导模块所采集的300个采样点的航向角数据的精度做对比，结果如图3所示。
[0094]
从图3可以明显的发现，本发明经过自适应调谐的无迹卡尔曼滤波的航向角信息比现有无迹卡尔曼滤波后的航向角信息更接近真实值。