我目前正在为一个学校项目工作，该项目需要为带有激光扫描仪的点机器人实现扩展的卡尔曼滤波器。机器人可以以0度转弯半径旋转并向前行驶。所有运动都是分段线性的（驱动，旋转，驱动）。

我们使用的模拟器不支持加速，所有运动都是瞬时的。

我们还需要定位一个已知的地图（png图像）。我们可以在图像中进行光线跟踪以模拟激光扫描。

我和我的搭档对我们需要使用的运动和传感器模型感到困惑。

到目前为止，我们将状态建模为向量$（x，y，\ theta）$。

我们使用如下更新方程式

void kalman::predict(const nav_msgs::Odometry msg){
    this->X[0] += linear * dt * cos( X[2] ); //x
    this->X[1] += linear * dt * sin( X[2] ); //y
    this->X[2] += angular * dt; //theta

    this->F(0,2) = -linear * dt * sin( X[2] ); //t+1 ?
    this->F(1,2) =  linear * dt * cos( X[2] ); //t+1 ?

    P = F * P * F.t() + Q;

    this->linear = msg.twist.twist.linear.x;
    this->angular = msg.twist.twist.angular.z;
    return;
}


我们以为一切正常，直到我们注意到我们忘记初始化P且它初始化为零，这意味着没有更正发生。显然，由于我们尚未将噪声引入系统，因此传播非常精确。

对于运动模型，我们将以下矩阵用于F：

$ F = \ begin {bmatrix} 1＆0＆-v * \ Delta t * sin（\ theta）
\\ 0＆1＆v * \ Delta t * cos（\ theta）
\\ 0＆ 0＆1
\ end {bmatrix} $

作为我们更新公式的雅可比行列式。这是正确的吗？

对于传感器模型，我们通过对机器人$ x $，$ y $和$ \ theta $位置以及地图中的光线跟踪进行有限差分来逼近Jacobian（H） 。我们与电讯局长交谈，电讯局长说这会奏效，但我仍不确定会否。我们的教授不在了，所以我们不能遗憾地问他。我们每个校正步骤使用3次激光测量，因此H为3x3。

另一个问题是如何初始化P。我们尝试了1,10,100，当地图只有50x50时，他们都将机器人放置在地图的（-90，-70）之外。

我们项目的代码可以在这里找到：https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

任何建议都将不胜感激。

编辑：

此时，我已经使滤波器在基本运动噪声但没有实际运动的情况下稳定下来。一旦机器人开始移动，过滤器就会迅速发散并离开地图。

#1 楼


将EKF用于基于激光扫描和已知图的定位是一个坏主意，因为EKF的主要假设之一是无效的，因此无法使用：测量模型不可区分。

我建议研究一下Monte Carlo本地化（粒子过滤器）。这不仅可以解决您的测量模型的问题，还可以进行全局定位（地图上的初始姿势完全未知）。

编辑：很抱歉，我忘了提到另一个要点：


如果要应用EKF，则测量模型以及运动模型可能仅包含高斯噪声。这意味着您需要将测量模型记为$ z_t = h（x_t）+ N（0，Q_t）$。
这是一个严重的限制，因为它不允许稍微复杂一些的模型就像Probabilistic Robotics中的beam_range_finder_model一样，它也考虑了机器人前面的动态对象，无效（最大）测量值和完全随机的读数。对于$ h（x_t）$部分，您将被投射射线束缚，并增加了高斯噪声。


评论

$ \ begingroup $
^^“使用EKF进行基于激光扫描和已知地图的定位是一个坏主意”，谁这么说？请提供参考。 EKF是最成功的估计器，许多论文建议使用EKF解决本地化问题。其实，我对您说的话感到惊讶。 EKF的主要假设是运动，观测模型是线性的，噪声是高斯的。我不知道“度量模型不可区分”是什么意思。
$ \ endgroup $
– Croco
2014年6月11日下午6:52


$ \ begingroup $
原始海报提到了光线追踪，这意味着他打算在概率机器人中使用类似于“测距仪光束模型”的测量模型。此测量模型显示出跳跃（想象一下激光束几乎没有撞到障碍物并错过它：它只需要很小的旋转就可以分辨出跳跃）。
$ \ endgroup $
–sebsch
2014年6月11日在8:51


#2 楼

首先，您没有提到所使用的本地化类型。它具有已知或未知的对应关系吗？

现在要在Matlab中获得雅可比矩阵，您可以执行以下操作

>> syms x y a Vtran Vrotat t
>> f1 = x + Vtran*t*cos(a);
>> f2 = y + Vtran*t*sin(a);
>> f3 = a + Vrotat*t;
>> input  = [x y a];
>> output = [f1 f2 f3];
>> F = jacobian(output, input)


结果

F =
[ 1, 0, -Vtran*t*sin(a)]
[ 0, 1,  Vtran*t*cos(a)]
[ 0, 0,               1]


扩展的卡尔曼滤波器要求系统是线性的，噪声是高斯的。这里的系统意味着运动和观察模型必须是线性的。激光传感器提供了从机器人框架朝向界标的范围和角度，因此测量模型不是线性的。关于P，如果您假设它很大，则您的EKF估计器在开始时就很差，并且它依赖于测量，因为先前的状态向量不可用。但是，一旦您的机器人开始移动和感应，EKF就会变得更好，因为它使用运动和测量模型来估计机器人的姿势。如果您的机器人没有感应到任何地标，则不确定性将急剧增加。另外，您需要注意角度。在C ++中，cos and sin的角度应为弧度。您的代码中没有关于此问题的任何内容。如果在以弧度计算时假设角度的噪声为度，则可能会得到怪异的结果，因为在以弧度计算时，以1度作为噪声是巨大的。