里程计#

里程计是一种定位形式，它使用来自编码器等传感器的数据来推导相对于起点的估计位置。定位是一种能够在某个时间点定位机器人位置的手段。里程计在自主程序中特别有用，因为它允许了解自己的位置而更容易地在场地上执行不同的任务。

姿态#

我们所说的姿态是指某个物体（如机器人）的位置，通常是在二维空间中，因为机器人的运动通常受限于单一平面。我们将机器人的姿势记为 \(\vec{x}\)。一个姿势包含两个条目：机器人的位置和方向；位置一般采用直角坐标，因此姿势可以用 \(x\), \(y\) 和 \(\theta\) 表示。方向 “是指机器人前方所朝的方向。因此，机器人坐标系的全局 x 轴与 0 方向一致。

我们可以把机器人当前的姿态（\(\vec{x}_0\)）称为 \(\begin{pmatrix} x_0 \\ y_0 \\ \theta_0 \end{pmatrix}\) 。这只是一个花哨的符号，表示在 \((x_0, y_0)\) 场上的一个点，机器人有一个指定的方向–标题 \(theta_0\) 。一个姿态通常在坐标系中有一个起始原点。

更新姿态#

在极短的时间内姿态的变化是 \(\Delta \vec{x}\)。当前姿态与上次姿态之间的时间差应尽可能小，以改善数学近似值。参赛队应在控制循环的每个周期更新机器人姿态。

更新姿态非常简单，只需将变换后的变化添加到之前的姿态中即可，其中 \(\varphi = \Delta\theta\)

\[\begin{split}\begin{pmatrix} x \\ y \\ \theta \end{pmatrix} = \begin{pmatrix} x_0 \\ y_0\\ \theta_0 \end{pmatrix} + \begin{pmatrix} \Delta x \\ \Delta y \\ \varphi \end{pmatrix}\end{split}\]

里程测量法的原理是利用传感器数据和数学运算，对机器人在一段时间内的姿态算出近似值。

查找位置变化#

为了确定机器人的当前位置并更新其姿态，必须使用从传感器读取的数据来计算变化。对于机器人来说，有三个传感器可供使用：两个在 \(x\) 方向上与机器人身体平行，一个与 \(y\) 运动方向对齐（垂直于驱动轮）。

角度和位移#

左边传感器的位移（或位置变化）为 \(\Delta x_l\)，右边传感器的位移为 \(\Delta x_r\)。这两个传感器之间的横向距离称为轨迹宽度，记为 \(L\)。这对于确定转弯近似角度非常重要。这个值需要调整，也就是反复测试，然后达到某个接近实际测量值的趋近值。

传感器的横向距离、前偏和位置 — 17508 Rising Tau’s 2019/20 Skystone Bot#

因此，推导 \(\varphi\) 的值就变得简单了：

\[\varphi = \frac{\Delta x_l - \Delta x_r}{L}\]

为了进行后面的计算，我们需要知道机器人在 x 方向上相对于其中心的位移，而不是两个平行传感器的位移。为此，我们取平均值得出 \(\Delta x_c\)，即中心位移：

\[\Delta x_c = \frac{\Delta x_l + \Delta x_r}{2}\]

在确定姿态变化之前，我们需要的最后一个位移是水平位移 \(\Delta x_\perp\)。这是垂直传感器的位移 \(\Delta x_h\) 加上前向偏移的修正 \(F\)。为了得到精确的近似值，需要考虑前向偏移。当传感器更靠近后方时，偏移量为负，但当传感器更靠近前方时，偏移量为正。这是为了考虑其位置在点转动时的变化。

因此，我们可以将水平位移定义为

\[\Delta x_\perp = \Delta x_h - (F * \varphi)\]

备注

如果没有垂直传感器（如果机器人不能横向移动，则不需要垂直传感器），则不需要 \(\Delta x_\perp\)。

如果没有水平传感器，该值应为 0。

机器人相关变化量#

让我们用一种简化的、非最佳的方法来计算机器人相对姿态差，然后将其转换为相对场地坐标变化。为此，我们需要通过一个旋转矩阵来转换机器人的相对位置差，在这个矩阵中，我们用原始方向来旋转相对位置差。我们可以得出 \(\Delta x\) 和 \(\Delta y\) 的值。

由此，我们可以计算出我们的姿态的相对场地变化：

\[\begin{split}\begin{pmatrix} \Delta x \\ \Delta y \\ \varphi \end{pmatrix} = \begin{pmatrix} \Delta x_c \cos(\theta_0) - \Delta x_\perp \sin(\theta_0) \\ \Delta x_c \sin(\theta_0) + \Delta x_\perp \cos(\theta_0)\\ \varphi \end{pmatrix}\end{split}\]

备注

这种近似位置的方法被称为欧拉积分法，但我们用它来计算精确的姿态变化量，而不是对速度进行积分（本质上，这是原始理论的一个非常简化的版本）。

警告

这是为高级程序员准备的；虽然从头开始实施是一个很好的练习，但这可能不是获得最佳自动驾驶的最佳方法。现在有很多资源可以制作出优秀的、经过良好测试的、易于实施的里程计。

里程计伪代码#

while robot_is_active():
   delta_left_encoder_pos = left_encoder_pos - prev_left_encoder_pos
   delta_right_encoder_pos = right_encoder_pos - prev_right_encoder_pos
   delta_center_encoder_pos = center_encoder_pos - prev_center_encoder_pos

   phi = (delta_left_encoder_pos - delta_right_encoder_pos) / trackwidth
   delta_middle_pos = (delta_left_encoder_pos + delta_right_encoder_pos) / 2
   delta_perp_pos = delta_center_encoder_pos - forward_offset * phi

   delta_x = delta_middle_pos * cos(heading) - delta_perp_pos * sin(heading)
   delta_y = delta_middle_pos * sin(heading) + delta_perp_pos * cos(heading)

   x_pos += delta_x
   y_pos += delta_y
   heading += phi

   prev_left_encoder_pos = left_encoder_pos
   prev_right_encoder_pos = right_encoder_pos
   prev_center_encoder_pos = center_encoder_pos

使用姿态指数#

在曲率不变的情况下，这种方法使用微分方程来求解机器人的非线性位置。欧拉积分法假定机器人在两次更新之间遵循直线路径，这可能会导致机器人在曲线上行驶时的近似值不准确。如果你对数学本身感兴趣，我们建议你查看这本书有关 FRC® 控制的内容。

我们将把本页中的求解方式视为一个黑盒子，并通过在欧拉积分机器人相关三角方程中对非线性曲率进行修正来推导出公式：

\[\begin{split}\begin{pmatrix} \Delta x \\ \Delta y \\ \varphi \end{pmatrix} = \begin{pmatrix} \cos(\theta_0)&-\sin(\theta_0)&0\\ \sin(\theta_0)&\cos(\theta_0)&0\\ 0&0&1\end{pmatrix} \begin{pmatrix} \frac{\sin(\varphi)}{\varphi}&\frac{\cos(\varphi)-1}{\varphi}&0\\ \frac{1-\cos(\varphi)}{\varphi}&\frac{\sin(\varphi)}{\varphi}&0\\ 0&0&1\end{pmatrix} \begin{pmatrix} \Delta x_c\\ \Delta x_\perp\\ \varphi \end{pmatrix}\end{split}\]

里程计资源#

在里程计方面，有几种很好的资源。我们强烈推荐 Road Runner 。如需了解 Road Runner 背后的数学原理（它利用了姿态指数），你还可以阅读 Ryan 的论文。有关 Road Runner 的另一个资源是 Learn Road Runner ，这是一个分步骤的程序指南，解释了如何使用 Road Runner quickstart 。

我们还推荐泰勒的著作，因为书中详细介绍了 FIRST® 机器人技术中的各种控制。

如果你使用其他资源，请务必不要使用使用欧拉积分的资源，因为它对于现实生活中的机器人姿态近似不太理想。