一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法技术

本发明专利技术属于机器人跟踪领域，具体涉及一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法；构建量测丢失下的非线性机器人跟踪系统模型，引入服从Gamma分布的辅助参数，将量测噪声先验分布建模为学生t分布，采用服从伯努利分布的随机变量对量测丢失情形的状态空间模型建模，其次将传统量测噪声协方差矩阵定义为修正量测噪声协方差矩阵，并基于变分贝叶斯理论求取非线性系统的状态和待估计参数的近似后验分布，最终输出滤波器估计出的量测丢失概率和滤波结果，本发明专利技术能预估量测丢失概率，减小机器人跟踪的性能误差，也减小了跟踪过程中野值干扰的影响。野值干扰的影响。野值干扰的影响。

A robot tracking method based on Kalman filter under random measurement loss

【技术实现步骤摘要】
一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法


[0001]本专利技术涉及机器人跟踪领域，具体涉及一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法。

技术介绍

[0002]机器人跟踪是指传感器根据采集到的量测数据，在尽可能剔除各种噪声和干扰的情况下，捕捉机器人的运动特征，从而估计出机器人当前运动状态并预测随后运动状态的过程。卡尔曼滤波方法是一种最优化自回归数据处理算法，广泛应用于机器人追踪领域。在实际的工程应用中，由于噪声、信号传输的不稳定性等不可避免的影响，存在量测信号随机丢失和量测噪声受野值干扰的情况。传统的卡尔曼滤波方法在跟踪此类复杂系统时，极易跟踪失败并导致状态估计发散。因此研究量测随机丢失下带厚尾噪声系统的滤波方法具有重要工程意义。
[0003]中国专利(CN105931130A)公开了一种考虑量测信号丢失的改进集合卡尔曼滤波估计方法，该方法首先采用伯努利分布序列对量测信号丢失情形的状态空间模型建模，然后以传统集合卡尔曼滤波为基本工具，推导出用于量测信号丢失情形下解决状态估计问题的改进集合卡尔曼滤波。但该方法无法预估量测丢失概率，并在最终的机器人跟踪的性能上误差较大。贾光乐等提出了线性系统中量测丢失概率未知的自适应卡尔曼滤波器(G.Jia,Y.Huang,Y.Zhang and J.Chambers.A Novel Adaptive Kalman Filter With Unknown Probability of Measurement Loss[J].IEEE Signal Processing Letters,2019,26(12):1862
‑
1866)。该方法在线性系统中采用伯努利分布序列对量测信号丢失的状态空间模型建模，并通过变分贝叶斯理论对其求近似后验分布，从而估计出量测丢失概率并减小滤波误差，但上述方法在量测噪声呈厚尾分布的复杂非线性机器人跟踪系统中鲁棒性不足而导致了估计参数发散的问题。

技术实现思路

[0004]针对非线性机器人跟踪系统中，量测随机丢失下机器人跟踪状态估计易发散的问题，本专利技术提出了一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法，将传感器量测噪声的先验分布建模为学生t分布，采用伯努利随机变量描述量测随机丢失的情形，最终基于变分贝叶斯理论求取非线性系统的状态和待估计参数的近似后验分布，该专利技术可有效减小非线性机器人跟踪系统在量测信号随机丢失下的误差。
[0005]一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法，其特征在于，包括：
[0006]S1.构建量测丢失下的非线性机器人跟踪系统模型，所述系统模型包括量测丢失模型、机器人跟踪的状态方程和量测方程；
[0007]S2.初始化系统模型的参数，设置时间间隔T＝1，采样时刻为k，整体运行时间t，确定机器人的状态向量及其对应的误差协方差矩阵；
[0008]S3.获取机器人前一时刻的后验状态向量，根据状态方程预测机器人当前时刻的
先验状态向量和预测误差协方差矩阵；
[0009]S4.获取前一时刻的量测丢失概率的分布参数，通过遗忘因子对量测丢失概率的分布参数进行时间更新；
[0010]S5.基于S3和S4的预测更新结果进行N次迭代，迭代更新修正量测噪声协方差矩阵，根据更新后的修正噪声协方差矩阵计算状态估计向量和估计误差协方差矩阵；
[0011]S6.N次迭代后输出最优的状态估计向量和估计误差协方差矩阵作为当前时刻的后验状态向量和误差协方差矩阵，程序采样时刻更新为k＝k+T；判断是否满足k≥t
‑
1，若是，则机器人跟踪过程结束，否则，将当前时刻记作前一时刻，并返回步骤S3。
[0012]进一步的，步骤S1中构建的非线性机器人跟踪系统模型包括：
[0013]机器人跟踪的状态方程和量测方程：
[0014]其中，k为采样时刻，x
k
∈R
n
×
n
为机器人的状态向量，f
k
‑1(
·
)为机器人的状态转移函数，w
k
‑1为机器人的过程噪声，且w
k
‑1服从均值为0，协方差矩阵为Q
k
‑1的高斯分布，记作w
k
‑1～N(w
k
‑1；0,Q
k
‑1)，其中Q
k
‑1表示k
‑
1时刻的过程噪声的协方差矩阵，y
k
∈R
m
×
m
为传感器的量测向量，h
k
(
·
)为传感器的量测函数，v
k
为量测噪声，量测丢失模型表示为：
[0015][0016]其中，ξ
k
为伯努利随机变量，其取值为0或者1，τ
k
为传感器的量测丢失概率。
[0017]进一步的，量测噪声服从学生t分布，表示为：
[0018][0019]其中，R
k
为传感器k时刻的量测噪声的协方差矩阵，λ
k
为引入的辅助参数，v表示学生t分布的自由度参数，G(λ
k
；v/2,v/2)表示辅助参数λ
k
服从分布参数为v/2的Gamma分布，N(v
k
；0,R
k
/λ
k
)表示量测噪声v
k
服从均值为0，协方差矩阵为R
k
/λ
k
的高斯分布。
[0020]进一步的，步骤S3中通过容积采样规则，对机器人k
‑
1时刻的后验状态向量进行容积采样，结合机器人跟踪的状态方程，获得机器人k时刻的先验状态向量和预测误差协方差，表示为：
[0021][0022][0023][0024][0025]其中，和P
k|k
‑1分别为机器人k时刻的先验状态向量和预测误差协方差，X
j,k
‑1为容积采样点，为X
j,k
‑1经状态转移函数变换后的采样点，S
k
‑1为机器人k
‑
1时刻的误差协方差矩阵经Cholesky分解得到的分解阵，r
j
为全对称区域n维空间中以1为生成元的向量
组成的集合的第j列，j＝1,2,...2n，为机器人k
‑
1时刻的后验状态向量，Q
k
表示k时刻的过程噪声的协方差矩阵。
[0026]进一步的，步骤S5中第i,i＝{1,2,...,N}次迭代的过程为：
[0027]S11.获取第i
‑
1次迭代更新后的伯努利随机变量期望E
(i
‑
1)
[ξ
k
]和辅助参数期望E
(i
‑
1)
[λ
k
]，通过E
(i...

【技术保护点】

【技术特征摘要】
1.一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法，其特征在于，包括：S1.构建量测丢失下的非线性机器人跟踪系统模型，所述系统模型包括量测丢失模型、机器人跟踪的状态方程和量测方程；S2.初始化系统模型的参数，设置时间间隔T＝1，采样时刻为k，整体运行时间t，确定机器人的初始状态向量及其对应的初始误差协方差矩阵；S3.获取机器人前一时刻的后验状态向量，根据状态方程预测机器人当前时刻的先验状态向量和预测误差协方差矩阵；S4.获取前一时刻的量测丢失概率的分布参数，通过遗忘因子对量测丢失概率的分布参数进行时间更新；S5.基于S3和S4的预测更新结果进行N次迭代，迭代更新修正量测噪声协方差矩阵，根据更新后的修正噪声协方差矩阵计算状态估计向量和估计误差协方差矩阵；S6.N次迭代后输出最优的状态估计向量和估计误差协方差矩阵作为当前时刻的后验状态向量和误差协方差矩阵，程序采样时刻更新为k＝k+T；判断是否满足k≥t
‑
1，若是，则机器人跟踪过程结束，否则，将当前时刻记作前一时刻，返回步骤S3。2.根据权利要求1所述的一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法，其特征在于，步骤S1中构建的非线性机器人跟踪系统模型包括：机器人跟踪的状态方程和量测方程：其中，k为采样时刻，x
k
∈R
n
×
n
为机器人的状态向量，f
k
‑1(
·
)为机器人的状态转移函数，w
k
‑1为机器人的过程噪声，且w
k
‑1服从均值为0，协方差矩阵为Q
k
‑1的高斯分布，记作w
k
‑1～N(w
k
‑1；0,Q
k
‑1)，y
k
∈R
m
×
m
为传感器的量测向量，h
k
(
·
)为传感器的量测函数，v
k
为量测噪声，量测丢失模型表示为：其中，ξ
k
为伯努利随机变量，其取值为0或者1，τ
k
为传感器的量测丢失概率。3.根据权利要求2所述的一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法，其特征在于，量测噪声服从学生t分布，表示为：其中，R
k
为传感器量测噪声的协方差矩阵，λ
k
为引入的辅助参数，v表示学生t分布的自由度参数，G(λ
k
；v/2,v/2)表示辅助参数λ
k
服从分布参数为v/2的Gamma分布。4.根据权利要求1所述的一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法，其特征在于，步骤S3中通过容积采样规则，对机器人k
‑
1时刻的后验状态向量进行容积采样，结合机器人跟踪的状态方程，获得机器人k时刻的先验状态向量和预测误差协方差，表示为：为：
其中，和P
k|k
‑1分别为机器人k时刻的先验状态向量和预测误差协方差，X
j,k
‑1为容积采样点，为X
j,k
‑1经状态转移函数变换后的采样点，S
k
‑1为机器人k
‑
1时刻的误差协方差矩阵经Cholesky分解得到的分解阵，r
j
为全对称区域n维空间中以1为生成元的向量组成的集合的第j列，j＝1,2,...2n，为机器人k
‑
1时刻的后验状态向量，Q
k
表示过程噪声的协方差矩阵。5.根据权利要求1所述的一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法，其特征在于，步骤S5中第i,i＝{1,2,...,N}次迭代的过程为：S11.获取第i
‑
1次迭代更新后的伯努利随机变量期望E
(i
‑
1)
[ξ
k
]和辅助参数期望E
(i
‑
1)
[λ
k
]，通过E
(i
‑
1)
[ξ
k
]和E
(i
‑
1)
[λ
k
]更新修正量测噪声协方差矩阵S12.基于修正量测噪声协方差矩阵计算第i次迭代时机器人当前时刻的状态估计向量和估计误差协方差矩阵S13.计算并更新伯努利随机变量期望E
(i)
[ξ
k
]和辅助参数期望E
(i)
[λ
k
]。6.根据权利要求5所述的一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法，其特征在于，步骤S11中更新修正量测噪声协方差矩阵的公式为：其中，表示在k时刻...

【专利技术属性】
技术研发人员：李帅永，聂嘉炜，谢现乐，毛文平，
申请(专利权)人：重庆邮电大学，
类型：发明
国别省市：