我一直在论坛中四处逛逛,但是找不到如何为定制的平面机器人获取车轮里程数协方差矩阵的任何答案(我找到了一些与EKF相关的帖子,但找不到任何明确的解决方案)。我需要这个以便例如保险丝轮里程表与其他类型的里程表,等等...我真的只需要全局平面速度的协方差(与编码器/输入直接相关),确定位置或加速度协方差似乎可以从那里派生。

我会在找到答案后尝试发布这些答案,这也许还会对希望构建新的移动机器人但可能对此感到困惑的未来的机器人专家(爱好!!)有所帮助。 br />
比方说,我有一个自定义的平面机器人,带有N个电机/车轮/编码器,并定义了运动学模型。 >

(Vx,Vy,AngVel)->(W1,W2,...,Wn)


其中W是每个电动机的角速度。我不确定逆映射是否始终存在,但我可以假设它现在才存在。

通过在论坛上阅读,我发现首先我们应该校准系统误差(例如,由于车轮直径不等,等等。)对于差动轮式机器人,可以使用UMBMark算法来完成。这仍然没有提供有关如何获得协方差矩阵tho的任何特定信息。通过卡尔曼滤波器)。

静态协方差矩阵的准确性可能较低,但确定起来更简单。但是,我不知道如何选择这些值(我应该让机器人来回移动几次并将误差用作Vx方差吗?)。是否有基本准则来静态填充协方差矩阵?

另一个(更困难的)选择似乎是使用卡尔曼滤波器,并动态更新协方差矩阵。但是我不确定选择什么作为输入,也不确定用于过程/观测的高斯白噪声值。

想象有一种本地控制器,我只是给出所需的角速度,然后它试图产生它们。我是否应该像将输入定义为电流一样低,然后通过电动机模型?还是我应该选择输入作为我指令的角速度?

但是,如果我的输入是所需的角速度,则该状态似乎不依赖于先前的状态,并且不遵循该过程的卡尔曼滤波约定(即,新状态将仅取决于输入,因为我直接从车轮控制平面速度,并且不受先前状态的影响)!
至少传感器模型似乎很容易从运动学模型导出。

您可能会看到,对于大多数移动机器人最有可能需要确定的东西,我感到非常困惑。我发现很少甚至没有清晰的文档,这对于这样的(很常见!)问题很奇怪。如果有人能指出我正确的方向,我将非常高兴!

谢谢!

#1 楼

测量并知道控制输入的协方差矩阵。

即,按照本页上的EKF方程,控件的协方差$ Q $通常是对角矩阵,其中对角项是控制效果的方差。

在将里程表用作控制替代的平面2d机器人中,$ Q $矩阵具有两个非零元素,这与里程表的距离测量误差相对应。换句话说,如果里程表返回的距离为$ d $,并且误差为$ \ sigma_d $,则控制的协方差矩阵为$ \ begin {bmatrix} \ sigma ^ 2_d,0 \\ 0,\ sigma ^ 2_d \ end {bmatrix} $

就这样。只需估算一下您的测量结果有多差。


我不会一直下​​降到电压,我会相信本地控制器并测量使sigma高于上面的差值
/>如果您的机器人没有惯性,那么可以,角速度在各个状态之间都是独立的。这不是KF问题,如果需要,可以将其包括在内。
您需要一本书,并且从简单开始。从里程表开始。进行3D(x,y,theta)状态,看看是否可以从那里去。到控制输入,并且是EKF更新方程式的一部分。 (或者,对于非参数估计量,通常只是样本的实际方差)。


一些详细信息和参考资料:

如果您的状态(位置,速度)是车轮速度(控件)的函数,首先构造函数$ x = f(u),y = f(u),s = f(u),\ omega = f(u)$。 。(用于x,y位置,速度和旋转速度)。您可以以$ u $的价格疯狂地疯狂,它可能只是车轮速度,或者是整个世界的某些非常复杂的建模,如果汞正在逆行等。

我通常使用这。

然后,按照ekf方程,构造jacobian,并将其命名为$ F $等。

#2 楼

Josh,谢谢您的帮助!

根据我的理解,确定具有N个轮子的3D(X,Y,Theta)机器人的速度协方差矩阵的基本过程似乎是(请如果我错了,请纠正我!):


定义机器人的状态。例如,可以是(x Velocity $ \ dot {x} $,y velocity $ \ dot {y} $,theta velocity $ \ dot {\ theta} $)。
定义映射函数$ x =控件的f_x(u),y = f_y(u),\ theta = f _ {\ theta}(u)$向状态变量输入$ u $。这将取决于机器人的运动学模型。例如,控制输入可以是$ N $个轮子的轮子速度$ w_1,w_2,...,w_n $。
将机器人放置在地板上的参考位置。提供控制输入以使其沿X方向移动(例如0.5 m / s),测量机器人沿X方向移动的实际速度(例如0.45 m / s)。对向前方向重复几次,并根据统计测量获得x速度的标准偏差。对y速度和Theta速度执行相同的过程。
我们将把这个常数协方差矩阵称为Q: sigma _ {\ dot {x}} ^ 2&0&0 \\
0&\ sigma _ {\ dot {y}} ^ 2&0 \\
0&0-1
\ end {bmatrix}
$$


计算过渡矩阵$ F $,该过渡矩阵是根据状态函数$ f $的雅可比构造的到各州。有关矩阵方程式,请参阅Wikipedia。
定义传感器模型。这会将状态变量映射到传感器值。对于带有$ N $编码器$ p_1,p_2,...,p_N $的轮式机器人,这将是形式为$ p_1 = h_1(\ dot {x},\ dot {y},\ dot {\ theta }); p_2 = h_2(\ dot {x},\ dot {y},\ dot {\ theta}); ... $。
定义传感器误差协方差矩阵$ R $。可以将车轮转动到特定值,并将实际测量值与编码器输出进行比较。完成几次后,即可确定其统计信息(与上述类似):

$$
R = \ begin {bmatrix}
\ sigma_ {p_1} ^ 2&0&0 \\
0&\ sigma_ {p_2} ^ 2&0 \\
0&0&...
\ end {bmatrix}
$$


将矩阵$ H $计算为传感器模型函数$ h $的雅可比式到状态变量。
现在您可以启动卡尔曼滤波器循环。选择矩阵$ P_0 $作为初始猜测(您可以选择全0作为开始)。遵循Wikipedia中的方程式,并在每帧求解状态和里程数协方差矩阵$ P $!例如,可以通过ROS中的robot_localization包读取此结果(状态,协方差),以融合多组数据并获得更好的测距近似值!

PS我不确定这是否正确,但我希望有人能告诉我这是否正确(或我在此过程中可能犯的任何错误!)!