一种动态目标定位方法技术

本发明专利技术公开了一种动态目标定位方法，利用惯性测量单元和超宽带模块实时采集数据，过程中重复以下步骤：利用当前时刻采集的数据初始化状态估计和误差协方差矩阵；对误差协方差矩阵SVD分解计算容积点；通过非线性状态模型对容积点进行评估得到传播的体积点；利用体积点更新预测状态和误差协方差。基于更新的预测状态和误差协方差，采用第一卡尔曼滤波器进行预测量更新和卡尔曼增益计算，采用第二卡尔曼滤波器更新状态估计和误差协方差矩阵。采用联邦滤波框架融合两个卡尔曼滤波器的局部估计信息，再更新状态估计和误差协方差矩阵。本发明专利技术在确保了定位精度的同时，减小了滤波算法的计算量；解决了计算过程中产生的舍入误差导致的收敛性问题。收敛性问题。收敛性问题。

【技术实现步骤摘要】
一种动态目标定位方法


[0001]本专利技术涉及导航定位领域，尤其是一种在定位盲区利用基于SVD的衍生容积卡尔曼滤波器和联邦滤波框架进行动态目标定位的方法。

技术介绍

[0002]实现动态目标定位在数学上可以转化成状态滤波问题。目前存在很多用于状态融合的算法，例如基于滤波的方法，统计推断法，以及人工智能方法。其中，扩展卡尔曼滤波器(EKF)是用于非线性系统的最常用状态估计算法。该滤波算法采用一种近似思想，通过泰勒展开式将非线性系统转换为线性系统。但是，当系统的非线性程度较高时，EKF将产生较大的截断误差，从而导致融合结果不准确。无迹卡尔曼滤波器(UKF)使用有限数量的sigma点来近似概率分布，其主要思想就是“近似概率分布要比近似非线性函数更容易”。该方法是将系统作为“黑箱”模型来处理，因此无需考虑非线性的具体模式。因此，UKF是姿态估计和多传感器信息融合最常用的方法之一。在高维系统中，UKF仍需要合理地调整参数以获得更好的滤波效果，这在实际系统中是十分困难的。
[0003]Ienkaran Arasaratnam和Simon Haykin最近提出了容积卡尔曼滤波器(CKF)。从理论上讲，这是迄今为止最接近贝叶斯滤波的方法。CKF基于三阶球面径向容积准则，因此其一个显着优势是它在数学上具有严谨性。CKF的独特之处在于球面径向容积准则会导致存在偶数个等权重的容积点，这些容积点分布在球面上。目前，基于容积卡尔曼滤波的状态估计和目标跟踪方法已逐渐受到学者的关注和重视。
[0004]在系统的状态方程是非线性的，而测量方程是线性的情况下，由于测量方程的线性性质，如果使用联邦滤波和传统容积卡尔曼滤波的组合，当通过测量方程传播容积点时，将产生大量的冗余计算。同时，容积卡尔曼滤波中的Cholesky分解要求矩阵为正定矩阵，具有很强的局限性。而SVD(奇异值分解)可以分解任何形式的矩阵，并且可以将其转换为容积卡尔曼滤波器所需的形式。因此，本专利技术考虑利用SVD，且兼顾状态方程的线性性质和测量方程的非线性性质的卡尔曼滤波方法。

技术实现思路

[0005]本专利技术的专利技术目的在于：针对上述存在的问题，提供一种动态目标定位方法，以提高状态估计的计算效率。
[0006]本专利技术采用的技术方案如下：
[0007]一种动态目标定位方法，包括：
[0008]利用惯性测量单元和超宽带模块实时采集数据，在采集数据过程中重复以下步骤以得到连续的位姿信息：
[0009]利用当前时刻采集的数据初始化状态估计和误差协方差矩阵；
[0010]对误差协方差矩阵SVD分解计算容积点；
[0011]通过非线性状态模型对容积点进行评估得到传播的体积点；
[0012]利用体积点更新预测状态和误差协方差；
[0013]基于更新的预测状态和误差协方差，采用第一卡尔曼滤波器进行预测量更新和卡尔曼增益计算，采用第二卡尔曼滤波器更新状态估计和误差协方差矩阵；
[0014]采用联邦滤波框架融合两个卡尔曼滤波器的局部估计信息，再更新状态估计和误差协方差矩阵。
[0015]进一步的，每一次计算位姿信息时更新的状态估计和误差协方差矩阵，用于初始化下一次位姿信息计算时的状态估计和误差协方差矩阵。
[0016]进一步的，所述利用当前时刻采集的数据初始化状态估计和误差协方差矩阵，包括：
[0017]初始化当前时刻的状态向量；
[0018]根据当前时刻惯性测量单元采集的数据信息计算当前时刻的状态向量；
[0019]根据当前时刻超宽带模块采集的测量数据计算当前时刻的测量向量；
[0020]根据当前时刻的状态向量和测量向量初始化当前时刻的状态估计和误差协方差矩阵。
[0021]进一步的，所述对误差协方差矩阵SVD分解计算容积点，包括：
[0022][0023][0024]ζ
j
被设置为：
[0025][0026]P
K
为初始化的误差协方差矩阵，S
k
为过渡参数，用于后续计算，为容积点，通过一组容积点来近似非线性系统的状态均值和协方差，为初始化的状态估计，m为容积点总数，m＝2n，n为状态量的维数，Σ是P
K
的奇异值矩阵；I
j
代表单位矩阵的第j列。
[0027]进一步的，所述通过非线性状态模型对容积点进行评估得到传播的体积点，包括：
[0028][0029]为体积点，在更新预测状态、误差协方差时使用，f(
·
)为非线性向量函数，用于描述非线性状态模型。
[0030]进一步的，所述利用体积点更新预测状态和误差协方差，包括：
[0031][0032][0033]为预测状态，ω
j
为容积点权重，P
k+1|k
误差协方差，为高斯白噪声方差。
[0034]进一步的，所述采用第一卡尔曼滤波器进行预测量更新和卡尔曼增益计算，包括：
[0035][0036][0037]为预测量，为卡尔曼增益，H
i
为测量矩阵，R
k+1
为高斯白噪声方差，υ
i,k
为高斯白噪声。
[0038]进一步的，所述采用第二卡尔曼滤波器更新状态估计和误差协方差矩阵，包括：
[0039][0040][0041]为更新的状态估计，为更新的误差协方差矩阵。
[0042]进一步的，所述采用联邦滤波框架融合两个卡尔曼滤波器的局部估计信息，再更新状态估计和误差协方差矩阵，包括：
[0043][0044][0045]为更新后的误差协方差矩阵，为更新后的状态估计。
[0046]综上所述，由于采用了上述技术方案，本专利技术的有益效果是：
[0047]1、本专利技术基于奇异值分解的联邦衍生容积卡尔曼滤波方法充分利用了待测系统状态方程非线性、测量方程线性的独特性质，在确保了定位精度的同时，减小了滤波算法的计算量。
[0048]2、本专利技术引入SVD分解代替原有的Cholesky分解，解决了计算过程中产生的舍入误差导致的收敛性问题。
附图说明
[0049]本专利技术将通过例子并参照附图的方式说明，其中：
[0050]图1、图2是本专利技术中的滤波方法与现有的卡尔曼滤波方法解算时间的比对图。
[0051]图3是本专利技术中的滤波方法与部分现有卡尔曼滤波方法的估计路径比对图。
[0052]图4是本专利技术中的滤波方法与部分现有卡尔曼滤波方法估计的姿态误差示意图。
[0053]图5是本专利技术中的滤波方法与部分现有卡尔曼滤波方法的整体误差示意图。
具体实施方式
[0054]本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合。
[0055]本说明书(包括任何附加权利要求、摘要)中公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种动态目标定位方法，其特征在于，包括：利用惯性测量单元和超宽带模块实时采集数据，在采集数据过程中重复以下步骤以得到连续的位姿信息：利用当前时刻采集的数据初始化状态估计和误差协方差矩阵；对误差协方差矩阵SVD分解计算容积点；通过非线性状态模型对容积点进行评估得到传播的体积点；利用体积点更新预测状态和误差协方差；基于更新的预测状态和误差协方差，采用第一卡尔曼滤波器进行预测量更新和卡尔曼增益计算，采用第二卡尔曼滤波器更新状态估计和误差协方差矩阵；采用联邦滤波框架融合两个卡尔曼滤波器的局部估计信息，再更新状态估计和误差协方差矩阵。2.如权利要求1所述的动态目标定位方法，其特征在于，每一次计算位姿信息时更新的状态估计和误差协方差矩阵，用于初始化下一次位姿信息计算时的状态估计和误差协方差矩阵。3.如权利要求1所述的动态目标定位方法，其特征在于，所述利用当前时刻采集的数据初始化状态估计和误差协方差矩阵，包括：初始化当前时刻的状态向量；根据当前时刻惯性测量单元采集的数据信息计算当前时刻的状态向量；根据当前时刻超宽带模块采集的测量数据计算当前时刻的测量向量；根据当前时刻的状态向量和测量向量初始化当前时刻的状态估计和误差协方差矩阵。4.如权利要求1所述的动态目标定位方法，其特征在于，所述对误差协方差矩阵SVD分解计算容积点，包括：解计算容积点，包括：ζ
j
被设置为：P
K
为初始化的误差协方差矩阵，S
k
为过渡参数，为容积点，为初始化的状态估计，m为容积点总数，m＝2n，n为状态量的维数，Σ是P
K
的奇异值...

【专利技术属性】
技术研发人员：俞成浦，许尔霈，邱凡朔，封蕴籍，郭云龙，
申请(专利权)人：北京理工大学重庆创新中心，
类型：发明
国别省市：