基于扩展卡尔曼滤波(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是将预测状态估计值转换为预测传感器测量值的测量矩阵。
基于上述讨论,我们做出了以下两个假设:
状态模型根据控制输入估计机器人的状态
观测模型使用预测状态推断传感器测量
扩展卡尔曼滤波(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扩展模块中文版教程
下载2:Python视觉实战项目52讲
下载3:OpenCV实战项目20讲
交流群
上一篇:【行业分析】2021年湖北武汉汽车市场分析
下一篇:众多新款车型亮相广州车展
最近更新汽车资讯
- 哲学沙龙
- 高原痛风临床研究
- 快播插件(SPSS无法启动因为应用程序的并行配置不正确)
- 教育部2021年工作要点:加强普通高校毕业生就业工作
- 【陪你母乳喂养】 哎哟喂,没生孩子也能泌乳?
- 重组家庭没有血缘关系的兄妹可以结婚吗
- 非现役人员生活待遇经费保障与管理探析
- 《发展心理学·从生命早期到青春期》基础理论读书笔记--结合教育动画的相关研究
- 2023届河南省郑州市(二模)高中毕业第二次质量预测语文试题及答案.docx
- 《那片星空那片海》全集剧情简介 分集剧情介绍
- 给6-15岁男孩女孩的精选主题书单(分性别,暑假必备)
- 优秀家长家庭教育经验分享5篇
- 郭洪雷:汪曾祺小说“衰年变法”考论
- 章子怡汪峰吵架原因 章子怡与汪峰吵架事件详情
- 情感故事:不回家的女人
- 道德伪善的教育学思考
- 2018级高职医学影像技术专业人才培养方案
- 以国家之名的罪恶——评德国影片《窃听风暴》
- 影视传播范文10篇
- 九天揽月一一敢峰证明四色定理之谜
- 挪威流产或人工流产后妊娠的妊娠间隔和不良妊娠结局(2008-2016 年):一项
- 好看的美剧排行榜(12部高分高质美剧推给你)
- 解放思想大讨论心得体会
- 长谈 | 内蒙古电影新浪潮:现实的结构与质感
- 美国《国家性教育标准》及其启示(下)