同时本地化和绘图(SLAM)是机器人学中的关键技术,它使机器人能够绘制未知环境的地图,同时确定它在地图中的位置. 实施扩展卡尔曼滤镜(EKF)可以增强SLAM的稳健性,特别是在有噪音和不确定因素的环境中. 这个教程为实施EKF来强化SLAM提供了一步步的指南.

理解扩展的卡尔曼过滤器

EKF是卡尔曼滤镜的延伸,旨在处理非线性系统,它将非线性函数线性化,围绕当前估计值,允许递归状态估计. 在SLAM中,EKF既估计机器人的姿势,也估计地标在环境中的位置.

步骤1:界定国家和共同资源

状态矢量一般包括机器人的位置和方向,以及标志性位置:

  • 机器人姿势:x,y,]
  • 标记位置: x i, y i ]

初始共变矩阵代表初始估算的不确定性.

步骤2: 预告步骤

使用机器人的运动模型,预测下一个状态并更新共变矩阵. 非线性运动方程使用雅各比亚语进行线性化:

状态预测:

x k ⁇ k-1 ]=f(x]k-1 ],u k ]]).

共变预测:

P k ⁇ k-1 ]]=Fk ]]Pk-1 ] ]k ]] ]+Q]k ]]]]]]

步骤3:更新步骤

收到传感器测量时,更新状态估计值。使用Jacobians对测量模型进行线性化:

测量预测:

z k ]=h(xk ]] ]]]]] (中文(简体) ).

创新:

y k ]=z 测量 - h(x ]k ⁇ k-1 ] ]]]]]

卡尔曼增益计算 :

K k =P ]k ⁇ k-1 ]]H k ]]T ] [Hk ]P k ⁇ k-1 ] ]T ]+R ]k ]] ]-1 ]]]]

状态更新 :

x k ⁇ k =x ]k ⁇ k-1 ]+ K ]k ] y k ]]]].

共变更新 :

Pk ⁇ k=(I-K]k]hk]]pk ⁇ k-1]]]]]].

步骤4: 纳入地标

地标在检测到时会添加到状态矢量中. EKF根据测量结果更新了估计值,随着时间的推移,提高了本地化的准确性.

结论

应用 SLAM 的 EKF 包括定义状态、预测机器人的运动以及用传感器数据更新估计。 适当的线性化和共变管理对于在吵闹环境中的稳健性至关重要。