基于扩展卡尔曼滤波(EKF)的机器人状态估计

点击上方“小白学视觉”，选择加"星标"或“置顶”

重磅干货，第一时间送达

EKF的目的是使卡尔曼滤波器能够应用于机器人等非线性运动系统，EKF生成的状态估计比仅使用实际测量值更准确。在本文中，我们将简要介绍扩展卡尔曼滤波器，并了解传感器融合的工作原理。为了讨论EKF，我们将考虑一种机器人车（自驾车车辆在这种情况下）。如下图所示，我们可以在一个全局坐标系中为这辆车建模，坐标为：Xglobal、Yglobal和Zglobal（面朝上）。X_car和Y_car坐标属于以线速度（V）和角速度（ω）移动的车的坐标系。横向角（γ）测量汽车绕全局Z轴旋转的程度。

EKF几乎存在于机器人技术的每个领域，用于估计状态。EKF的目标是平滑汽车的噪声传感器测量值，以便更好地进行状态估计。这里的状态是指汽车的位置，为了估计车辆状态，EKF将噪声传感器测量值与预测传感器测量值相结合，以生成最佳估计值。然而，为了理解EKF的功能，我们需要了解EKF的两个数学模块：

• 状态空间模型

• 观测模型

1.状态空间和运动模型

使用状态空间模型，我们可以预测汽车的下一个状态，它也被称为状态转换模型，表示汽车或机器人从当前时间步到下一时间步的运动。例如，它显示了我们汽车的当前状态是如何根据使用转向、油门和制动器对汽车[线速度V和角速度ω]的控制而变化的。转向影响ω，而油门和制动器影响线速V。从现在起，我们将把时间t时汽车的状态表示为[xt，yt，γt]，控制输入表示为[vt，ωt]。现在让我们根据全局参考系推导汽车的速度分量：

如上图所示，通过应用三角学，我们得到角速度为ω的Vx=Vcos（y）和Vy=Vsin（y）。现在，给定我们的初始状态[xt-1，yt-1，γt-1]，我们可以如下估计下一个状态[xt，yt，γt]：

注：我们从t=1开始，初始状态为[x0，y0，γ0]

在上图中，我们用数学方法描述了机器人汽车的运动。在方程式1中，在dt时，汽车会向上移动一段距离。因为我们知道距离可以写成速度（v）和时间（t）的乘积，所以方程1很容易得到。每个项（f1、f2和f3）将用于计算雅可比矩阵F。这里我们使用雅可比矩阵，因为我们需要线性化具有余弦和正弦项的非线性方程。

在等式2中，Xt表示时间t处的运动模型。F和B是雅可比矩阵，在我们的例子中，F是一个单位矩阵，因为汽车的移动仅基于输入控制，即线性和角速度。但是，它并不总是一个单位矩阵。想想飞机的情况，在飞机上，重力起作用，如果我们不提供控制输入，它就会坠落并坠毁。所以，在这种情况下，F不是恒等式。最后，等式3表示添加了噪声项的完整状态空间模型。

2.观测模型

我们使用状态空间或运动模型对汽车进行了估计。现在，我们将平滑传感器测量，以更好地估计机器人汽车的当前状态（这称为传感器融合）。首先，我们的状态模型预测下一个状态，然后观察模型获取状态模型的预测输入[xt，yt，yt]，以生成（推断）新的测量值。

想象一下，当我们在一辆自动驾驶汽车上，传感器发生故障时的情形。现在我们希望汽车移动到目标位置。在这种情况下，观测模型可以预测未来某个时间的传感器测量结果。因此，即使汽车传感器发生故障并产生错误数据，观测模型也会试图减少误差。

yt为n维，表示n个传感器测量值。w是每个传感器的噪声。H是将预测状态估计值转换为预测传感器测量值的测量矩阵。

基于上述讨论，我们做出了以下两个假设：

1. 状态模型根据控制输入估计机器人的状态

2. 观测模型使用预测状态推断传感器测量

扩展卡尔曼滤波（EKF）

EKF计算当前时间步长t和预测传感器测量值（如上所述）的这些实际传感器测量值的加权平均值，以生成更好的当前状态估计值。EKF有两个阶段：预测和更新（如下图所示）

上图显示了扩展卡尔曼滤波器的预测和更新步骤。在预测步骤中，我们首先使用状态空间或运动模型来估计状态（Xt）（我们去除了噪声项，只是为了让它看起来干净）。然后，我们使用之前在时间t-1处的协方差矩阵P获得时间t处的状态协方差矩阵P。状态协方差矩阵包含状态的不确定性。然而，对于第一次迭代，我们没有协方差矩阵，所以我们初始化它，如上图所示。此外，汽车的初始状态向量和控制命令将为零。

矩阵F是状态转移矩阵，用于预测下一个值X和协方差矩阵P。矩阵Q是过程噪声协方差矩阵。Q的维数是（状态数*状态数），在我们的例子中，它是3x3。这个Q项很重要，因为状态测量有噪声，我们需要测量方差。

注：Rt（传感器测量噪声协方差矩阵）

K表示卡尔曼增益。如果传感器噪声高（残余协方差高），K值趋于零，传感器测量值将被忽略。如果预测的噪声很高，那么K接近1，我们将依靠传感器测量。最后，更新Xt和Pt并将其用于下一步的预测。换句话说，Kalman增益（K）包含关于当前预测X和当前观测测量z的权重的信息，这将导致最终融合更新状态向量X和过程协方差矩阵P。

至此，我们已经完成了EKF。然而，EKF有一个线性化误差，基本上取决于函数的非线性程度以及用于线性化的工作点的距离。线性化误差可能会对自动驾驶汽车产生灾难性影响，因为它会导致估计器对错误答案过于自信。此外，推导雅可比矩阵是一个繁琐且容易出错的过程。如果自动驾驶汽车移动得很快，那么线性化误差就更大。

链接：https://www.linkedin.com/in/surajit-saikia-55b717101/

下载1：OpenCV-Contrib扩展模块中文版教程

在「小白学视觉」公众号后台回复：扩展模块中文教程，即可下载全网第一份OpenCV扩展模块教程中文版，涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。

下载2：Python视觉实战项目52讲

在「小白学视觉」公众号后台回复：Python视觉实战项目，即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目，助力快速学校计算机视觉。

下载3：OpenCV实战项目20讲

在「小白学视觉」公众号后台回复：OpenCV实战项目20讲，即可下载含有20个基于OpenCV实现20个实战项目，实现OpenCV学习进阶。

交流群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~