买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：重庆邮电大学

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

主权项：1.一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法，其特征在于，包括：S1.构建量测丢失下的非线性机器人跟踪系统模型，所述系统模型包括量测丢失模型、机器人跟踪的状态方程和量测方程；步骤S1中构建的非线性机器人跟踪系统模型包括：机器人跟踪的状态方程和量测方程：其中，k为采样时刻，xk∈Rn×n为机器人的状态向量，fk-1·为机器人的状态转移函数，wk-1为机器人的过程噪声，且wk-1服从均值为0，协方差矩阵为Qk-1的高斯分布，记作wk-1～Nwk-1；0,Qk-1，yk∈Rm×m为传感器的量测向量，hk·为传感器的量测函数，vk为量测噪声，量测丢失模型表示为： 其中，ξk为伯努利随机变量，其取值为0或者1，τk为传感器的量测丢失概率；量测噪声服从学生t分布，表示为： 其中，Rk为传感器量测噪声的协方差矩阵，λk为引入的辅助参数，v表示学生t分布的自由度参数，Gλk；v2,v2表示辅助参数λk服从分布参数为v2的Gamma分布；S2.初始化系统模型的参数，设置时间间隔T＝1，采样时刻为k，整体运行时间t，确定机器人的初始状态向量及其对应的初始误差协方差矩阵；S3.获取机器人前一时刻的后验状态向量，根据状态方程预测机器人当前时刻的先验状态向量和预测误差协方差矩阵；步骤S3中通过容积采样规则，对机器人k-1时刻的后验状态向量进行容积采样，结合机器人跟踪的状态方程，获得机器人k时刻的先验状态向量和预测误差协方差，表示为： 其中，和Pk|k-1分别为机器人k时刻的先验状态向量和预测误差协方差，Xj,k-1为容积采样点，为Xj,k-1经状态转移函数变换后的采样点，Sk-1为机器人k-1时刻的误差协方差矩阵经Cholesky分解得到的分解阵，rj为全对称区域n维空间中以1为生成元的向量组成的集合的第j列，j＝1,2,...2n，为机器人k-1时刻的后验状态向量，Qk表示过程噪声的协方差矩阵；S4.获取前一时刻的量测丢失概率的分布参数，通过遗忘因子对量测丢失概率的分布参数进行时间更新；S5.基于S3和S4的预测更新结果进行N次迭代，迭代更新修正量测噪声协方差矩阵，根据更新后的修正噪声协方差矩阵计算状态估计向量和估计误差协方差矩阵；步骤S5中第i,i＝{1,2,...,N}次迭代的过程为：S11.获取第i-1次迭代更新后的伯努利随机变量期望Ei-1[ξk]和辅助参数期望Ei-1[λk]，通过Ei-1[ξk]和Ei-1[λk]更新修正量测噪声协方差矩阵S12.基于修正量测噪声协方差矩阵计算第i次迭代时机器人当前时刻的状态估计向量和估计误差协方差矩阵S13.计算并更新伯努利随机变量期望Ei[ξk]和辅助参数期望Ei[λk]；步骤S11中更新修正量测噪声协方差矩阵的公式为： 其中，表示在k时刻的第i次迭代更新后的修正量测噪声协方差矩阵，Rk表示传感器在k时刻的量测噪声的协方差矩阵；步骤S12中，机器人当前时刻的状态估计向量和估计误差协方差矩阵的更新公式为： 其中，为机器人k时刻的先验状态向量，Pk|k-1为机器人k时刻的预测误差协方差矩阵，yk为传感器的量测向量，为k时刻的预测量测向量，为量测误差协方差矩阵，为卡尔曼系数，其更新公式为： 其中，为互协方差矩阵；量测误差协方差矩阵和互协方差矩阵的更新公式分别为： 其中，为经量测函数hk·变换后的采样点，为经量测函数hk·变换后的预测量测向量，为容积采样点，表示在k时刻的第i次迭代更新后的修正量测噪声协方差矩阵；步骤S13中，第i次迭代更新的期望Ei[ξk]的计算公式为： 其中，qiξk＝1、qiξk＝0分别为伯努利随机变量ξk取1或0时的后验概率密度函数，其第i次迭代的更新公式为：qiξk＝1＝c1exp{Ei-1[log1-τk]-0.5Ei-1[λk]trAkiRk-1+0.5mEi-1[logλk]}qiξk＝0＝c2exp{Ei-1[logτk]-0.5Ei-1[λk]trBkiRk-1+0.5mEi-1[logλk]}其中，c1和c2为第i次迭代时的常量，Aki、Bki表示过程参数，τk为量测丢失概率，Ei-1[logτk]为第i-1次迭代更新后logτk的期望，Ei-1[log1-τk]为第i-1次迭代更新后log1-τk的期望，Ei-1[logλk]为第i-1次迭代更新后logλk的期望，其中，logτk、log1-τk、logλk的期望在第i次迭代的更新公式分别为： 其中，ψ·表示digamma函数，为第i次迭代时τk的分布参数，为第i次迭代时λk的分布参数，这四个分布参数在第i次迭代的更新公式分别为： 其中，m表示量测向量维度，v表示学生t分布的自由度参数，tr·表示求取矩阵迹操作，在第i次迭代时，根据更新后的计算更新辅助参数期望Ei[λk]，表示为： S6.N次迭代后输出最优的状态估计向量和估计误差协方差矩阵作为当前时刻的后验状态向量和误差协方差矩阵，程序采样时刻更新为k＝k+T；判断是否满足k≥t-1，若是，则机器人跟踪过程结束，否则，将当前时刻记作前一时刻，返回步骤S3。

全文数据：

权利要求：

百度查询： 重庆邮电大学 一种量测随机丢失下基于卡尔曼滤波的机器人跟踪方法