神奇的移动背后的数学原理#
自动模式下的移动与手动模式下最核心的区别就在于自动模式下的机器人不能从driver那里获得任何信息。因此，为了可以精确地并且重复地控制机器人的移动与动作，机器人需要知道它在赛场的何方——或者至少有个大概的猜测。这一过程在机器人的领域中被称为 odometry。如果没有办法精确地预测机器人的位置，我们就智能被迫采用 dead reckoning ，即死记硬背法。甚至，我们可能会需要采用基于时间的控制方法
为了执行我们的odometry，我们决定将我们的车轮编码器与陀螺仪相结合。执行与我们在手动控制中所做的反向运动学计算，设定一个预定的输出姿态，并找出每个单独的这轮应该如何运动来实现该姿态。同时，一个姿态只是一组描述机器人在某一瞬间的位置或运动的独特特征。而通过获取每个瞬间各个车轮的速度和方向的信息，并使用我们在第2.1节中描述的运动学进行组合，我们可以得到机器人在两个方向上的整体速度，进而通过对时间积分（只需将此时的速度与控制器的时间步长相乘），得到我们的位置，这一个过程被称为“正向运动学”，即FK
正向运动学（FK）#
当然，从车轮编码器到整体速度并不是完全直接的。当我们看到能获取的信息和我们期望的结果时，问题就来了：我们有八个变量，四个轮子的速度和四个方向，而我们想要三个输出：机器人的前进、侧向和旋转速度。如果你熟悉代数，特别是线性代数，你可能会认识到，这使得我们的系统被过度定义了——3个未知数，但是8个方程。我们有更多的信息，无法得到一个精确的结果。有几种方法来解决这个问题。通过 设置精确的方程并将其放入计算机代数系统 ，或者通过将方程以矩阵-向量格式组合并使用线性方程求解器，我们可以获得系统的 “最佳拟合”。
然而，这种不精确的拟合可能很难实现，往往需要加入额外的库。由于本赛季的时间很紧，我们选择了一种简化的方法。由2841队的Kyle Lanman提供，我们改编了 这种算法 ，对变量直接取均值，直到我们从8个下降到我们需要的3个。虽然我们预计这不如更先进的方法准确，但我们发现它在校准后是非常准确的。只要轮速和加速度保持不会让机器打滑，这就是一种适合15秒自动时段的里程表测量方法，特别是当与其他传感器结合使用以 “校正”导航时。
在这种形式的正向运动学中，我们先假设，在某一时刻，我们与到我们的车轮速度与朝向——我们将其定义为：\(ws_{FR}\), \(ws_{FL}\), \(wa_{RL}\) ，以此类推，F/R与F/R分别表示左右、前后。通过电机内置的编码器——例如CTRE Falcon 500或者REV Neos内置的编码器，来决定轮速是极其麻烦的。
我们一定需要确保我们的车轮速度有一致的、有物理意义的数值。无论我们使用的是内置在电机中的编码器还是手动安装的编码器，它们都可能向我们提供的是以刻度和转数为单位的值，或一些刻度/秒或转数/秒的速度。我们需要将其与我们的传动系统减速和车轮直径结合起来，以获得一些转换率，从而能够获得我们的车轮速度，以每一时间的距离为单位，如英寸/秒、英尺/秒或米/秒。
我们可以先用我们的轮速与角度计算ABCD四个数值，但这次，我们对每个轮子都要计算一次，单位是速度（每分钟的距离）因为我们只是将我们的轮子的转速（度每分钟）乘以一个角度分量——即每度对应的距离。
好吧，这对简化我们的变量没有任何帮助! 为了使之更合理，我们要开始取均值。对于A、B、C和D的每一个值，我们都要取两个值，然后取其平均值。这些平均值仍然以速度为单位，因为它们只是两个速度值的均值。
很好，我们终于把我们的复杂度降下来了。现在我们需要找到我们的旋转速度，或ROT。然后，我们将根据机器前后（A/B）和左右（C/D）速度的值，计算出可能的旋转速度，并再次取平均值，得到一个单一的值。请注意，我们也可以从陀螺仪上得到这个ROT值（单位是弧度/秒），我们的机器人上应该已经有了这个陀螺仪，使得我们可以以场地为中心进行驱动。
在此，我们将简单地将这与我们的几何学和A、B、C、D值结合起来，以（再次）获得前后和左右移动的的两个值，并取其平均值，以获得我们对向前和扫射速度的最终估计。
太好了! 现在终于又可以用的数据了。不过，如果我们希望我们的距离值真正可用，这些速度应该被转换为以场地为中心的。为了再把数据变换为我们可靠的以场地为中心的数据，我们将再次需要相对于场地的角度，通常由陀螺仪提供。
里程表#
由此，我们现在可以算出我们实际上是以多快的速度沿着场地前进的——相当巧妙！我们可以把这些数据整合到一起。为了得到一个位置，我们只需要在时间上整合这些数据。这就像初始化一个定时器，并将其在当前循环运行时的值与上一次运行时的值相比较，以确定我们的循环的时间间隔也很简单。对于大多数机器人代码来说，这个值一般约为0.020秒，或20毫秒；然而，这只是一个名义上的数值，它可以根据机器人代码的行为而上下变化（主要是上升）。无论如何，我们总是可以把这个值和我们的速度整合到一个累加器中，以得到我们相对于开始记录时的位置。
timeStep = LoopTimer.Get() − lastTime
positionAlongField = positionAlongField + FWD * timeStep
positionAcrossField = positionAcrossField + STR * timeStep
顺带一提，我们用前后和左右来称呼我们的各个方向，这可比xyz轴易于沟通多了
我们现在有了我们的里程表。当在自动时段中使用它时，我们在开始时将这个值重置为零，并将我们程序内的坐标与机器人开始的位置相对应。这意味着机器人的初始位置拜访是非常重要的，因为任何误差都会通过里程表来体现。
当你在每个周期添加变化时，你总是知道某个轮子的位置，对另外3个也是。我们用这些信息来定位机器人的中心。机器人的方向也可以用同样的方式来确定，但我们选择用我们的陀螺仪来做这个。我们只存储机器人中心的当前位置。我们将该位置发送到Dashboard，并在那里记录数据。我们能够据此绘制出机器人的位置图，并将其画在在场地地图上，使我们能够在不接触机器人的情况下对我们的自动进行改进。
下一个问题是告诉机器人接下来要怎么走。我们把跟踪过程倒过来，告诉每个轮子下一步要去哪里。我们在子系统中输入所需的x、y坐标。每个轮子需要面对的角度被计算出来。除非机器人在移动时要旋转，否则所有车轮的角度都是一样的。如果机器人将在移动中改变航向，那么转弯修正量将被计入，并使得车轮面对不同的方向，并有不同的相对速度，直到实现旋转部分。我们使用一个定位循环来分配车轮的速度。我们只使用kP*误差。我们可以通过几个不同的标准来改变状态。我们可能会使用已实现的距离、intake传感器或瞄准反馈来告诉机器人，它已经完成了该任务。然后，机器人就会转到下一个任务。
校准#
如果我们使用我们的理想车轮直径和齿轮比，这些数值应该非常接近真实世界的数值，但它们可能并不完美。我们要通过乘上一个常数来校准我们的整体速度。这种校准可以很简单，就是标出一个设定的距离（越长越好），然后驾驶机器人穿过这个距离，尽可能保持直线。一旦达到这个距离，就可以将报告的距离值与实际距离值进行比较，并将其比值产生一个修正系数–实际距离大于报告距离。这也可以用来计算车轮的磨损，车轮的磨损会改变有效的车轮直径，如果不计算在内，会导致距离测量不准确。