具有激光扫描+已知图的扩展卡尔曼滤波器


10

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

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

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

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

到目前为止,我们将状态建模为向量。(x,y,θ)

我们使用更新公式如下

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=[10vΔtsin(θ)01vΔtcos(θ)001]

作为我们的更新公式的雅可比行列式。它是否正确?

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

关于如何初始化P的另一个问题。我们尝试了1,10,100,并且当地图仅为50x50时,它们都将机器人放置在地图的外部(-90,-70)。

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

任何意见是极大的赞赏。

编辑:

在这一点上,我已经使滤波器在基本运动噪声的作用下稳定下来,但没有实际运动。一旦机器人开始移动,过滤器就会迅速发散并离开地图。

Answers:


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

我建议研究一下蒙特卡洛本地化(粒子过滤器)。这不仅可以解决您的测量模型问题,还可以进行全局定位(地图上的初始姿态完全未知)。

编辑:对不起,我忘了提到另一个要点:

  • 如果要应用EKF,则测量模型以及运动模型都可能仅包含高斯噪声。这意味着您需要以记下测量模型。这是一个严重的限制,因为它不允许像Probabilistic Robotics中的beam_range_finder_model那样稍微复杂的模型,该模型还考虑了机器人前面的动态对象,无效(最大)测量值和完全随机的读数。您会被困在零件的投射光线上并添加高斯噪声。h x tzt=h(xt)+N(0,Qt)h(xt)

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

原始海报提到了光线追踪,这意味着他打算在概率机器人技术中使用类似于“测距仪的光束模型”的测量模型。该测量模型表现出跳跃(想象的激光束几乎没有碰撞障碍物和缺少它:只需要一个微小的旋转为跳跃是不可微的。)
sebsch

0

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

现在要在Matlab中获取Jacobian,可以执行以下操作

>> 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,角度应为弧度。您的代码中没有关于此问题的任何内容。如果在以弧度计算时假设角度的度数为度,则可能会得到奇怪的结果,因为在以弧度计算时将度数作为噪声是巨大的。

By using our site, you acknowledge that you have read and understand our Cookie Policy and Privacy Policy.
Licensed under cc by-sa 3.0 with attribution required.