在腿跟随机器人上应用卡尔曼滤波器

Question

我被要求创建一个腿部跟随机器人（我已经完成了），在此任务的第二部分中，我必须开发一个卡尔曼滤波器以改进机器人的跟随过程。机器人从人那里得到她到机器人的距离以及角度（这是一个相对角度，因为参考是机器人本身，而不是绝对的 x-y 坐标）

关于这项任务，我有一个严重的疑问。我读过的所有东西，我看到的关于卡尔曼滤波器的每个样本都是一维的（汽车行驶距离或从建筑物上掉下来的石头），根据任务，我必须将它应用到二维。是否可以应用这样的卡尔曼滤波器？

如果可以在二维中计算卡尔曼滤波器，那么我会理解，尽管一个人走路很奇怪（随机运动），但我会理解要求做的是以线性方式跟随腿--> 关于这个我对如何建立状态矩阵的函数有疑问，谁能告诉我该怎么做或告诉我在哪里可以找到更多相关信息？

谢谢。

Answer 1

你应该阅读卡尔曼滤波器。基本上它所做的是分别通过其均值和方差来估计一个状态。状态可以是任何你想要的。您可以在您的州拥有本地坐标，也可以拥有全球坐标。

请注意，后者肯定会导致非线性系统动力学，在这种情况下，您可以使用扩展卡尔曼滤波器，或者更正确的是连续离散卡尔曼滤波器，您以连续方式处理系统动力学，并且离散时间的测量。

全局坐标示例：

假设您有一个可以以速度 v 向前行驶的小立方质量。您可以仅在局部坐标中简单地建模动力学，其中您的状态 s 将是 s = [v]，这是一个线性模型。

但是，假设我们仅在平面上移动，您也可以合并全局坐标 x 和 y。然后你会有s = [x, y, phi, v]'。我们需要phi 来跟踪当前方向，因为立方体当然只能相对于它的方向向前移动。让我们将phi 定义为立方体的前进方向与x 轴之间的角度。或者换句话说：使用phi=0，立方体将沿 x 轴移动，使用phi=90°，它将沿 y 轴移动。

具有全局坐标的非线性系统动力学可以写成

s_dot = [x_dot, y_dot, phi_dot, v_dot]' 

与

x_dot = cos(phi) * v
y_dot = sin(phi) * v
phi_dot = ...
v_dot = ... (Newton's Law)

在EKF（扩展卡尔曼滤波器）预测步骤中，您将使用上面的（离散化）方程来预测第一步中状态的平均值，并使用线性化（和离散化）方程进行预测的方差。

当你决定你的状态向量s应该是什么样子时，有两件事要记住：

• 您可能想使用我的线性示例s = [v]，然后在卡尔曼滤波器之外对速度进行积分以获得全局坐标估计值。这会起作用，但你会失去卡尔曼滤波器的威力，因为你只会整合状态的平均值，而不是它的方差。换句话说，您不会知道当前全球坐标的不确定性是什么。
• 卡尔曼滤波器的第二步，即测量或校正更新，要求您可以将传感器输出描述为状态的函数。因此，您可能必须在表示中添加状态，以便您可以正确表达您的测量值as z[k] = h(s[k], w[k])，其中z 是测量值，w 是具有高斯分布的噪声向量。