EKF在机器人目标跟踪中的应用

温彬彬 温鹏智 胡雪花

摘要：扩展卡尔曼滤波在机器人目标跟踪中有非常重要的应用，该文重点介绍了扩展卡尔曼滤波的工作流程以及如何应用在机器人目标跟踪中。但EKF算法应用在非线性系统中会产生二次项以上的截断误差，累计会影响机器人的定位精度。该文主要从拓展卡尔曼滤波的收敛性上进行分析，利用双估计用观测残差对过程噪声进行补偿，利用新息和观测值估测观测噪声矩阵，通过多次预测、更新迭代从而达到对机器人更精准的定位。

关键词：扩展卡尔曼滤波;目标跟踪;噪声补偿;观测噪声

中图分类号：TP3        文献标识码：A

文章编号：1009-3044（2021）36-0116-03

开放科学（资源服务）标识码（OSID）：

Application of EKF in Robot Target Tracking

WEN Bin-bin1， WEN Peng-zhi2，HU Xue-hua1

（1.Hebei Vocational University of Industry and Technology， Shijiazhuang 050000， China; 2.Xingtai Modern Occupation School， Xingtai 054000， China）

Abstract： Extended Kalman filter has a very important application in robot target tracking. This paper focuses on the workflow of extended Kalman filter and how to apply it in robot target tracking. However， when EKF algorithm is applied in nonlinear system， it will produce truncation errors above quadratic term， which will affect the positioning accuracy of the robot. This paper mainly analyzes the convergence of extended Kalman filter， uses double estimation to compensate the process noise with observation residuals， uses innovation and observation values to estimate the observation noise matrix， and achieves more accurate positioning of the robot through multiple prediction and update iterations

Key words： Extended Kalman filter; target tracking; noise compensation; observation noise

在人工智能時代 ，运动目标跟踪与检测的研究有非常重要的意义，广泛地应用在智能机器人、智慧交通、无人驾驶汽车等领域。在目标跟踪与检测中，首先要获取一个目标的准确位置，就要建立一个良好的滤波型。目前常用有卡尔曼滤波KF、扩展卡尔曼滤波EKF，两者都是基于区域的跟踪方法，用来解决状态估计问题。卡尔曼滤波器是目前最优化的线性滤波，要求观测方程是线性的，符合高斯分布的。但是实际生活中，目标的移动由于环境变化出现加速或者减速，影响机器人判断目标的准确性，为了解决这个问题且能继续使用卡尔曼滤波来进行预测，人们使用扩展卡尔曼滤波，将非线性系统利用数学中一阶泰勒级数展开，将状态转移方程和观测方程线性化，再利用卡尔曼滤波进行递归估计，这样能够对突然加速、减速或环境变化引起的位置和速度变化判断较为精准。

1 扩展卡尔曼滤波模型

扩展卡尔曼滤波重点在解决实际跟踪过程中非线性问题，基本思想就是利用线性最小方差估计理论从与被提取信号有关的测量中递推估算出所需的信号，计算非线性转换后实际概率分布的高斯近似分布，线性化后继续使用卡尔曼滤波来进行状态估计。

对于非线性的系统来说，状态空间表达式为：

[xt=fxt−1，μt−1，wt−1]                        （1）

[zt=h（xt，vt）]

其中，非线性函数[f]描述的是不同时刻下状态量变化之间的关系，非线性函数[h]描述的状态变量[zt]与观测变量[xt]的关系。随机变量[wt]代表过程激励噪声，随机变量[vt]代表观测噪声 [1]。

非线性系统预测下一时刻状态，需要在某一时刻点线性化，然后进行卡尔曼滤波。但是由于系统存在误差，无法准确地获取真实点，所以非线性函数[fxt]在[xt−1]处线性化，即t-1时刻的后验估计，系统的噪声不确定故[wt]设为0。

扩展卡尔曼滤波器的预测方程为：

[x-^t=f（x^t-1，ut-1，0）p-t=AtPt-1AtT+WtQt-1WtT]                            （2）

[At]和[Wt]是[t]时刻的过程协方差矩阵，[Qt]为[t]时刻的过程激励噪声协方差矩阵。

扩展卡尔曼滤波器的校正方程如公式（3）所示：

[Kt=Pt-HtT（HtPt-HtT+VtRtVtT）-1x^t=x-^t+Kt（zt-h（x-^t，0））Pt=（I-KtHt）Pt-]                        （3）

状态更新方程利用观测值变量[zt]的值校正状态估计和协方差估计。[h]为反映了状态变量[zt]与观测变量[xt]关系的非线性函数，[Ht]和[Vt]是[t]时刻的测量雅克比矩阵，[Rt]为[t]时刻的观测噪声协方差矩阵[2]。

2扩展卡尔曼滤波在机器人目标跟踪中使用

在实际运动中，只有机器人做一维运动时，才能保证运动方程是线性的，针对速度模型，机器人的運动方程和观测方程是非线性，当转换函数是非线性时，尽管自变量是高斯分布，因变量的分布也不再是高斯分布，机器人在跟踪人，可以看作一个非线性系统。使用扩展卡尔曼滤波用在机器人目标跟踪[2]。在机器人坐标系中，人体在[t]时刻的位置定义为[（xr，yr，zr）]。扩展卡尔曼滤波器的状态向量为：

[Xr=[xr xr zr xr. yr.]]                               （4）

其中[xr.， yr.]为机器人在水平面的速度。

在机器人运动状态下的状态方程可表示为：

[Xrt+1=ft（Xrt，controlt）+Rtwt]v                   （5）

其中，[controlt=[vl，vr]]，状态转移函数[f]为：

[ft（xtr，controlt）=（X tr+Δt X tr-ΔXr）cosΔθ+（Y tr+Δt Ytr-ΔYr）sinΔθ-（X tr+Δt X tr-ΔXr）sinΔθ+（Y tr+ΔtYtr -ΔYr）cosΔθZ trXtr cosΔθ+Ytr sinΔθ-v-Xtr sinΔθ+YtrcosΔθ]

（6）

假设相机[t]时刻观测到的人体3D位置为[ytr]，观测方程如7所示：

[Ytr=HtXtr+pt]

[Ytr=xtr ytr ztrT]，[Ht=1 0 0 0 00 1 0 0 00 0 1 0 0]                （7）

其中观测噪声[pt]的协方差矩阵分别为：

[Rt=Cov（pt）=EptpTt=σ2p100010001.]          （8）

扩展卡尔曼滤波应用在机器人目标跟踪的过程如图1所示：

通过扩展卡尔曼滤波，可以从上一时刻[t-1]对当前时刻[t]的人体三维位置与它的不确定性进行预测，得到观测方程，在观测过程中获得新的观测，对人体状态进行更新，得到人体的后验估计，同时更新该时刻的协方差矩阵，为下一次迭代做准备，通过多次迭代的，得到人体的准确位置。

3 改进的扩展卡尔曼滤波

上述机器人目标跟中扩展卡尔曼滤波先验人体位置，默认噪声方差为0，但是对于实际的跟踪系统，肯定会存在噪声，噪声的存在会影响预估，会导致系统发散，影响跟踪性能和准确率。本文主要从拓展卡尔曼滤波的收敛性上进行分析，通过对过程噪声和观测噪声进行估计[3]，让其收敛，从而预测下一时刻的状态，得到下一时刻的协方差，从而达到对机器人更精准的定位。

对于一个运动状态坐标系：

用[X（n）=[r_x （n）  （r_x （n）  ） ̇r_y （n）  （r_y （n）  ） ̇]  ]表示目标X，Y方向的位移、速度量。T是采样周期，n表示第n次采样，其观测方程为：

[Z（n+1）=F（n+1，n）X（n）+T∆V（n）+v（n）]             （9）

[T∆V（n）]是加速度引起的偏量，通常它和[v（n）]作为混合噪声，因此公式（4）就可以写为：

[Z（n+1）=F（n+1，n）X（n）+q（n）]             （10）

测量方程为：[Y（n）=ℎ[n，X（n）]+w（n）]，其线性化方程为：

[Yn=ℎn，Xn|yn−1+HnXn−Xn|yn−1+v（n）]         （11）

将[Q1（n）=[q（n） q T（n）]]定义为过程噪声，将[Q2（n）=[v（n） v T（n）]]定义为观测噪声矩阵。

对于一个运动目标跟踪系统模型来说，如果在建模过程中不能考虑所有的噪声，那么肯定会影响卡尔曼滤波的性能，不能使其达到最优。本文用双估计[4]来消除加速度带来混合噪声，将测量方程等式两边求微分得：

[dYn=∂ℎn，Xn∂XdX（n）]

设[∆θn=dYn，qn+1=dX（n）]，在采样周期内[∆θn≈Yn−Y（n−1）]，实际上q（n）可以看成加速度带来的混合过程噪声。可以用方程来估计q（n）。

[qn+1=Fn+1，nqn+vn]                （12）

[∆θn=Hnqn+v（n）]                     （13）

通过上式可以得出，加速度的变化可以由观测值Y（n）的残差测量来实时估计[4]。

接下来利用新息和观测值来求修正的观测噪声矩阵，假设均值和方差都为某一常数[5]，由观测方程可以得到[v（n）=Y（n）−ℎ[n，X（n）]]，将观测方程的线性方程代入有：

[an=Yn−ℎn，Xn|yn−1=Hnξn，n−1+v（n）]  （14）

其中[ξn，n−1]为n时刻的预测误差，新息的均值和测量噪声的均值相等，新息协方差矩阵为：

[P=E[an−an∗an−anT]                （15）

修正的观测噪声矩阵为：

[Q2n=P−E{HnKn，n?1HT（n）}]     （16）

将公式（12）和公式（16）列入扩展卡尔曼滤波流程中，通过多次迭代递归，进而得到机器人目标跟踪的更加准确位置。

4 仿真结果及结论

改进的卡尔曼滤波算法在仿真中进行测试，当有一定加速度时，改进后的算法有着明显的优势，X、Y 方向位置的均方误差均有明显减少，证明算法的可行性。利用双估计和观测残差对过程噪声进行补偿，消除加速度带来的影响，利用新息和观测值估计观测噪声矩阵，可以较好地消除由观测噪声带来的误差，通过这些自适应算法的加入提高了EKF算法的稳定性，提升了跟踪性能。

参考文献：

[1] 白聪轩.多线索分块匹配的移动机器人目标跟踪[D].北京：北京工业大学，2015.

[2] 许亚朝，何秋生，王少江，等.一种改进的自适应卡尔曼滤波算法[J].太原科技大学学报，2016，37（3）：163-168.

[3] 秦阳.基于深度学习的运动目标识别跟踪系统设计[D].沈阳：沈阳理工大学，2021.

[4] 唐涛，黄永梅.改进的EKF算法在目标跟踪中的运用[J].光电工程，2005，32（9）：16-18.

[5] 董俊松.基于扩展卡尔曼滤波的单目标跟踪算法研究[D].兰州：西北师范大学，2019.

【通联编辑：代影】