数学部分#

控制与输入处理#

在操纵时，driver希望机器人可以按照他们的想法行动，而为了将driver的想法与实际程序联系起来，我们定义了三个输入变量，\(FWD\), \(STR\) 与 \(ROT\)，它们的取值范围是-1到1。其中，0代表没有输入数据的情况。有许多方法来将driver的输入映射到该范围中。而在本案例中，我们决定用一个摇杆来控制移动速率（即 \(FWD\) 与 \(STR\) ），另一个摇杆来控制旋转速率（即ROT）。如图所示。这些摇杆采用WPILib的标准库实现，输入经过筛选和变换以便于driver使用。除了driver的输入变量，我们也可以把别的输入变量纳入考虑。

https://pic.imgdb.cn/item/626dfbe0239250f7c590c457.png

除了driver的输入变量，我们也可以把别的输入变量纳入考虑。

以场地为参照系的数据变换#

一旦我们确定了 \(FWD\), \(STR\), 与 \(ROT\) 的值。我们就要对这些数据进行一些数学变换。简单来说，我们要将以场地为参照系的数据，基于机器人与场地的夹角（由陀螺仪测得），变换为以机器人为参照系的数据，进而使机器人以场地为参照系移动。

我们定义 \(FWD\) 和 \(STR\) 为我们所预期的、来自摇杆的平移指令。它们的取值在1到-1之间，我们假设它们都被正确地过滤。同时两者（作为向量）的和向量的模不应大于1，但晚些时候我们会发现这并不重要。

我们定义机器人与场地的夹角为θ，当机器人的正面与场地的边框相平行，远离驾驶员时，θ的数值为0。该角度的取值范围既可以是0-360°，也可以是-180°到180°——在该案例中进行数学运算时，两者并无实际区别。进一步地，我们可以将 \(FWD\) 与 \(STR\) 作如下变换：

\[FWDnew = FWD * cosθ + STR * sinθ\]
\[STRnew = STR * cosθ − FWD * sinθ\]

首先，我们将我们的:math:FWD`new计算出来。其可以被认我们原先的指令（ :math:`FWD`与 :math:`STR）在机器人的方向上的分向量之和。类似地，我们将 \(STR\) 也算出来。这一举措将我们的以场地为参照系的平移指令转变为以机器人为参照系的平移指令。如果你学过线性代数，你可能会意识到这实际上是对向量( \(FWD\), \(STR\))进行 坐标系旋转。

为了进一步深化对这两个式子的理解，让我们来看看下面的几个例子。

例子1：机器人与场地对齐

这里的关键是思考θ的值是多少，以及我们的正弦和余弦的值如何随之而变化。在这里，由于我们是与场地对齐的，\(θ=0°\)。让我们来看看我们的正弦和余函数图像。

https://pic.imgdb.cn/item/626dfd69239250f7c5944f63.jpg

如果我们想象一个直角三角形，其中一个角是0°，斜边是1，那么这个三角形的对边就没有高度也就是0–同时邻边就有斜边的长度，也就是1。让我们看看我们的式子的处理结果。

\[FWD_{new} = FWD * cos(0°) + STR * sin(0°) = FWD * 1 + STR * 0 = FWD\]
\[STR_{new} = STR * cos(0°) − FWD * sin(0°) = STR * 1 − FWD * 0 = STR\]

我们的式子告诉我们，我们并不需要改变任何东西! 事实上，这是有道理的；如果机器人与场地对齐，它的前方与场地的前方是一样的；我们根本不需要修改我们的指令。

例子2：机器人与场地呈90度

假设我们的机器人与场地成直角，即机器人是侧向的，所以我们的角度 \(θ=90°\)。让我们再看看三角函数的结果。 \(cos90°\) = 0, \(sin90°\) = 1. 再让我们看看式子的处理结果。

\[FWD_{new} = FWD * cos(90°) + STR * sin(90°) = FWD * 0 + STR * 1 = STR\]
\[STR_{new} = STR * cos(90°) − FWD * sin(90°) = STR * 0 − FWD * 1 = −FWD\]

我们的指令被变换了（并且符号有一个小的变化）！事实上，这也是有道理的。机器人是侧向的；为了向前走，它必须做它认为的横向移动，而为了横向移动，它必须走它认为的前进。

例子3：机器人与场地呈30度

我们现在的这个角度并不是很易于想象。机器人的排列方式与场地不同，所以要想直接向前或直接向侧边走。我们需要进行一些比较复杂的数学变换。我们的角度θ=30°， \(cos30°\) = 0.866, \(sin30°\) = 0.5. 顺着式子走，那么：

\[FWD_{new} = FWD * cos(30°) + STR * sin(30°) = FWD * 0.866 + STR * 0.5\]
\[STR_{new} = STR * cos(30°) − FWD * sin(30°) = STR * 0.866 − FWD * 0.5\]

:math:`FWD`和 :math:`STR`都对每个转换值有贡献。这种计算方式，将贯穿于任何非直角的运算，而只要我们正确地计算我们的角度（顺时针为正），就可以得到正确的运算结果了！

逆向运动学（计算轮子转速与朝向）#

现在我们的 \(FWD\) 与 \(STR\) 都已经被正确转换了。我们接着需要算出每一个轮子的运动参数。这一段的内容通常被称为 inverse kinematics，简称IK。我们有我们想要达到的目标输出，以及可以调整的执行器参数（也称作关节），即我们的车轮速度和朝向。逆向运动学的本质计算一组独特的输出数据，从而得到我们的整体目标输出。正向运动学（我们将在其他地方介绍）与此相反–根据我们的执行器在做的事情来确定我们所处的状态（即姿态）。

首先，我们应该考虑到车轮的布局。我们将假设我们的车轮位于一个矩形的顶点上。换句话说，所有四个轮轴都能处在某个圆上。这代表了大多数swerve驱动的底盘的样子（我们的就是方形）。如果swerve模块不在一个矩形上（例如，如果机器人不是矩形），我们将在计算时考虑到这一点。

接着，让我们把轴距的长度和宽度定义为L和W，同时，我们定义一个额外的值 \(R\), 其值为 \(R = \sqrt{L^2 + W^2}\) , 代表接触所有四个轮轴的圆的直径。这些值的单位并不重要，因为我们在计算车轮的时候只需要它们的比值。

https://pic.imgdb.cn/item/626dfe54239250f7c5964d5d.jpg

理想模型#

为了知道如何将我们的对于机器人整体的指令（我们希望机器人作为一个整体做什么）转化为每个轮子的指令，我们应该建立一些理想模型。即使我们不这样做，我们也完全可以借用数学知识并将其编程到位，但这确实有助于了解代码的工作原理。

当我们推导反运动学时，我们建立了一个理想模型，我们将机器人的底盘视作一个刚体——在其自身的参照系内，一个点不能靠近或远离另一个点。想象一下，桌子上有一个金属立方体。你可以向前、向后移动它，也可以旋转它，但它是刚体，它的形状是不变的。从物理学的角度来看，我们可以推导出一个重要的结论。对于机器人的每一条边，它上面的所有点都必须具有相同的向前或向后的速度。如果它们有不同的速度，那么这个侧面的长度就会发生变化。真希望这种情况不会发生在赛场上，如果我们的代码写得足够好，就不应该发生这种情况。

https://pic.imgdb.cn/item/626f5b68239250f7c56f4711.jpg

我们可以通过做一些向量运算来满足这一点。ChiefDelphi用户Ether的 Derivation of Inverse Kinematics for Swerve 文件详细解释了这一点。要做到这一点，我们可以将我们所需的横向运动和前后运动指令合并为一个平移矢量，而我们的旋转指令可以与我们的机器人几何形状相结合，形成一个旋转分量。通过添加这些矢量，我们可以得到我们所需的车轮运动矢量，其形式为朝向和转速。

在此，我们定义一些变量来节省我们的工作量。一般我们把它们定义作A、B、C与D。具体定义式如下：

\[A = STR - ROT * L/R\]
\[B = STR + ROT * L/R\]
\[C = FWD - ROT * W/R\]
\[D = FWD - ROT * W/R\]

我们运用这些数值来计算我们各个轮子的转速与朝向（即wheel speed 与wheel angles，下式中简写为Ws与Wa）

\[ws_{FR} = \sqrt{B^2 + C^2}\]
\[wa_{FR} = atan2(B,C)\]
\[ws_{FL} = \sqrt{B^2 + D^2}\]
\[wa_{FL} = atan2(B,D)\]
\[ws_{RR} = \sqrt{A^2 + C^2}\]
\[wa_{RR} = atan2(A,C)\]
\[ws_{RL} = \sqrt{A^2 + D^2}\]
\[wa_{RL} = atan2(A,D)\]

备注

此处，atan2函数是一个在许多语言中都被定义的函数，它的返回值是括号内两数之比的arctan值。值得一提的是，它的返回值是 -π 到 π 而不是0 到 π/2。

在此，我们可以看到，不同的车轮之间有共同的因子。对于两个前轮，我们有一个共同的水平因子B，对于两个后轮，有一个共同因子A。对于两个右轮和左轮，同样地，有共同的前后移动分量C和D。如果你知道一点向量知识，你会意识到我们实际上做了一个从平面直角坐标到极坐标的转换。

其中FR、FL、RR和RL指的是右前、左前、右后和左后轮。我们的算得的朝向的数值是从-π到π，正数为顺时针方向，零为直行。这些可以根据需要转换为度数——只要乘以180/π。我们的轮速的绝对值应该在0到1之间。为此，我们只需检查我们的ws值的最大值是否大于1，如果是，就将这些值按比例放缩，使ws的最大值为1。

ws_{max} = max(ws_{FR}, ws_{FL}, ws_{RR}, ws_{RL})
if ws_{max} > 1.0 :
ws_{FR} = ws_{FR}/ws_{max}
ws_{FL} = ws_{FL}/ws_{max}
ws_{RR} = ws_{RR}/ws_{max}
ws_{RL} = ws_{RL}/ws_{max}

现在，我们算出了所有的ws值，范围从0到1。ws的值不应为负数，因为这意味着轮子的朝向（即wa）值应当再转180°

但现在，我们的算法带来了一个问题。 如果我们的控制输入快速从全速向前变为全速向后移动，我们的轮舱需要旋转180°才能执行新命令。 鉴于我们正在使用可以全功率前进或后退的电机，这是一件低效的事情。 理想情况下，我们只需将电机反转并继续移动。

为了解决这个问题，我们实现了所谓的“智能反转”。如果我们知道车轮当前的方位角，并且我们已经计算出我们需要的方位角，我们可以检查我们是否需要 “反转”我们的模块。如果需要，我们只需反转速度输出，并重新调整方位角的方向，然后继续移动。我们将在稍后实现这一点。

落实控制算法（改变轮子的速度并旋转朝向）#

终于，我们可以开始行动了。我们有了我们想要的参数，一组轮子的速度和角度，从我们想要的输出中产生，被变换为相对于机器人的方向。现在我们要让我们的转向模块执行这些指令。接下来的这些代码可以被认为类似于计算机系统中的驱动程序–在提供一个抽象的控制界面的同时，处理琐碎的低级通信和控制的代码层。在我们的案例中，我们将方位角和车轮速度归纳为一个模块，我们可以用它来设置车轮的目标状态。

改变朝向#

朝向控制是通过一个比例反馈控制器来完成的，该控制器通过一个轴编码器感应车轮的角度。根据所使用的编码器的不同，读到的数可能是一个度值、一个电压或整数个刻度的形式。对于我们的US Digital MA3编码器，输出是0-5V信号的形式，代表0-360°。在许多情况下，编码器外壳不能精确地定向到 “正确 “的物理位置。这意味着，当我们的目的是是直行时（Wa=0°，轮子是朝前的），我们的编码器的读数可以不为0。因此，我们将在每个模块的基础上有一些偏移值，我们将把它纳入我们的计算。

对于我们的控制器，我们需要计算我们的目标点和当前位置之间的偏差，即Error值。因为我们是用角度计算的，而且范围是0-360°，所以我们需要使用取余函数来确保我们的误差得到正确计算。我们也会用它来计算我们的偏移量。假设我们有这个轮子想要的角度（wa），我们可以据此计算出我们的偏差值。

encoder = Encoder.GetValue()
azimuthAngle = remainder(Encoder.GetValue() − wheelAngleOffset)
azimuthError = remainder(azimuthAngle − wa)

智能反转#

使用Talon FX的SetInverted函数可以非常直接地实现反转控制。而我们只需方位角偏差 “翻转 “到另一边。

azimuthError = azimuthPosition − wa ;
if abs(azimuthError) > 90 : //assuming our angles are in degrees
  azimuthError = azimuthError − 180 * sgn(azimuthError)
  SpeedMotorController.SetInverted(true)
else :
  SpeedMotorController.SetInverted(false)