点击上方“小白学视觉”,选择加"星标"或“置顶”
重磅干货,第一时间送达
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是将预测状态估计值转换为预测传感器测量值的测量矩阵。
基于上述讨论,我们做出了以下两个假设:
状态模型根据控制输入估计机器人的状态
观测模型使用预测状态推断传感器测量
扩展卡尔曼滤波(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有一个线性化误差,基本上取决于函数的非线性程度以及用于线性化的工作点的距离。线性化误差可能会对自动驾驶汽车产生灾难性影响,因为它会导致估计器对错误答案过于自信。此外,推导雅可比矩阵是一个繁琐且容易出错的过程。如果自动驾驶汽车移动得很快,那么线性化误差就更大。
链接:/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“。请按照格式备注,否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告,否则会请出群,谢谢理解~
如果觉得《基于扩展卡尔曼滤波(EKF)的机器人状态估计》对你有帮助,请点赞、收藏,并留下你的观点哦!