控制回路¶

控制回路是用于快速、可控地运行动力传输系统（如传动系统或线性滑轨）的软件。控制回路不仅能让你快速运行机械装置而不必担心失控，而且在许多情况下，它们还能通过减少外加电机电压的快速变化来延长机械装置的使用寿命。

什么是误差？¶

在讨论控制回路时，首先必须定义误差的概念。

误差的定义是你所处的位置与你希望达到的位置之间的差值。例如，你告诉你的动力传动系统以每秒 30 英寸的速度行驶，但实际上，动力传动系统以每秒 28 英寸的速度行驶。由于 \(30-28=2\)，\(T\) 时刻传动系统的速度误差为每秒 2 英寸。换句话说，在 \(t=T\) 时，\(e(t)=2\)。

PID¶

PID 控制器（或称比例积分微分控制器）是一种仅利用误差来控制系统的控制回路。PID 是 反馈控制回路 或 闭环控制 的一种形式。这意味着，为了让环路控制该变量，需要关于所控制变量的数据。在这种情况下，要使用 PID 控制器对系统进行控制，就需要有关系统误差的信息。

可选微积分¶

下式表示 PID 控制器 \(f\) 在任何给定时间 \(t\) 的输出的严格数学定义：

\[f(t) = K_p e(t) + K_i \int_o^t e(t) \mathrm{d}t + K_d \frac{mathrm{d}e(t)}{\mathrm{d}t}\]

其中 \(K_p\)、\(K_i\) 和 \(K_d\) 为常数，\(e(t)\) 如前所述，为系统误差。

如果你没有微积分经验，也不用担心；虽然 PID 从根本上源于微积分，但你不需要任何微积分经验就能理解它，只需要基础代数即可。不过，无论是否有微积分经验，我们还是建议你阅读本节的其余部分，因为仅凭公式并不能说明其工作原理。

简化 PID 公式¶

以下是 PID 公式的简化版本：\(f(t)=K_p P(t)+K_i I(t)+K_d D(t)\)

我们所做的只是简单地拿掉完整的公式，并将PID的每一项替换为函数：\(P(t)\)、\(I(t)\) 和 \(D(t)\)。

比例项¶

函数的第一个分量 \(K_p P(t)\) 是最简单易懂的，因为 \(P(t) = e(t)\)。举例来说，假设 \(K_i=0\) 和 \(K_d=0\) （只有比例常数的 PID 控制器称为 P 控制器）。系统将如何运行？如果误差大，输出就会大。同样，如果误差小，输出也会小。此外，在理想情况下，只要有足够的时间，假定 \(K_p\) 的符号正确，系统总会接近目标。

假设我们将此应用于传动系统。你想行驶一段距离 \(D\)，并决定使用 P 控制器设置电机功率来实现这一目标。在这种情况下，你的误差就是机器人距离目标位置的距离。当你开始向前行驶时，你的误差很大，因此你会快速向前行驶，这也是可取的。毕竟，如果距离目标较远，你还不用担心会超速。

但当机器人与目标的距离趋近于 0 时，你将开始减速，从而获得对机器人更多的控制权。理想情况下，一旦误差为零，机器人就会停下来，而你也就到达了目的地。如果碰巧超速，误差将变为负值，机器人将返回，重复上述过程。

微分项¶

这个项 \(K_d D(t)\) 的目的是抑制误差的变化率。换句话说，它试图保持误差不变。如何做到这一点呢？

对于有微积分经验的人来说，\(D(t)=\frac{de(t)}{dt}\)。对于没有微积分经验的人来说，它表示误差变化的速度。从图形上看，\(D(t)\) 就是任意给定时间 \(t\) 下误差的斜率。

这个斜率可以通过跟踪控制环连续迭代的误差来计算。一次迭代的时间为 \(t_n\)，误差为 \(e(t_n)\)。下一次迭代的时间为 \(t_{n+1}\)，误差为 \(e(t_{n+1})\)。因此，若要求出 \(D(t)\) ，只需根据这两点求出 \(e(t)\) 的斜率即可。

积分项¶

诚然，积分项是FTC® 调节PID控制回路中最不重要的项。通过适当调整 \(K_p\) 和 \(K_d\) ，你通常可以将 \(K_i\) 设置为0，然后就到此为止。

不过，它在某些情况下仍然有用。就像微分项一样，积分项的目的也是修正超调。如果系统认为已经到达目的地，它就会停止，即使事实上误差还没有为 0。那么，只要有足够的时间，积分项就会增加输出（这里指的是电机功率），从而使电机向目的地运动。

如果不用微积分的知识来解释的话，积分项实质上是将特定时间间隔内的误差相加。为此，每次循环迭代中的误差都会加到一个变量中（本例中为 \(I(t)\)）。

然而，用这种方法计算误差总和有一个不幸的副作用：循环完成一次迭代所需的时间越长，误差总和增加的速度就越慢，这显然是不可取的，因为我们不希望滞后影响机器人的移动。为了弥补这一点，在将误差添加到 \(I(t)\) 之前，先将其乘以上一个循环完成所需的时间，即 \(t_{n+1}-t_n\)，以防止滞后使系统总和增加得更慢。

假设机器人在目标附近停下。P和D组合不足以将其向前移动到目的地。你可以调整 \(K_p\) 和 \(K_d\) 以进行补偿（建议使用），也可以添加积分项以增加输出（这也有效，但需要更多的关注和调整才能获得相同的结果）。

PID 伪代码¶

while True:
   current_time = get_current_time()
   current_error = desire_position-current_position

   p = k_p * current_error

   i += k_i * (current_error * (current_time - previous_time))

   if i > max_i:
       i = max_i
   elif i < -max_i:
       i = -max_i

   D = k_d * (current_error - previous_error) / (current_time - previous_time)

   output = p + i + d

   previous_error = current_error
   previous_time = current_time

调整 PID 回路¶

在调整 PID 循环时，最重要的是了解每个项对输出的影响。这可以让你了解哪些增益需要调整。

例如，如果没有达到目标后停止，而是围绕目标摆动，则说明 D 增益不够。如果最终达到目标，但是速度非常缓慢，则说明 P 增益不足或 D 增益过高。

简而言之，P 变量推动误差趋于零，I 变量修正稳态误差，D 变量抑制 P 变量的影响，当误差趋于零时，D 变量的影响更大，从而防止超调。

调整 PID 控制器最常用的方法如下：

将 I 增益和 D 增益设为零
增大 P 增益，直到目标附近出现振荡
增大 D 增益，直到不出现过冲为止
如果存在稳态误差，则增大 I 增益，直至误差得到纠正

需要注意的是，大多数系统并不需要 I 和 D 控制。一般来说，没有大量摩擦的系统不需要 I 项控制，但需要更多的 D 项控制。另一方面，摩擦力较大的系统一般不需要 D 控制，因为摩擦力有助于减速，但需要 I 控制，因为摩擦力会阻止系统达到目标。

如需更深入的解释，请点击这里

内置 PID 控制器¶

如果需要控制单个电机的速度或位置，可以使用内置的 PID 控制器。将运行模式更改为 “RUN_USING_ENCODER”，即可启用 PID。

提示

许多人误解了 RUN_USING_ENCODER 的使用，误以为必须使用该模式编码器才能工作，但事实并非如此。相反，RUN_USING_ENCODER 可以使用编码器实现速度反馈。如果使用的是外部 PID 控制器，例如自己实现的控制器，一般建议使用 RUN_WITHOUT_ENCODER。

有关内置 PID 控制器的官方文档，请参见

调试内置 PID 控制器¶

问题	解决方案
电机全速运转，与速度设定值无关	大多数情况下，出现这种情况的原因有两种： #1：编码器连接不正确。诊断：将编码器的位置记录到遥测系统中，如果位置在 0 - 1 之间摆动，请确保电缆正确且安装正确。 #2: 你的电机运转方向错误。诊断：将速度记录到遥测系统中，如果参考速度为正，而输出为负，或者反之亦然，则说明电机插反了。
使用 `.setPower` 时电机未达到全速	使用 DcMotorEx 的 `.setVelocity` 方法，或设置 `RUN_WITHOUT_ENCODER` 并使用外部 PID 控制器。

问题

解决方案

电机全速运转，与速度设定值无关

大多数情况下，出现这种情况的原因有两种：

#1：编码器连接不正确。

诊断：将编码器的位置记录到遥测系统中，如果位置在 0 - 1 之间摆动，请确保电缆正确且安装正确。

#2: 你的电机运转方向错误。

诊断：将速度记录到遥测系统中，如果参考速度为正，而输出为负，或者反之亦然，则说明电机插反了。

使用 .setPower 时电机未达到全速

使用 DcMotorEx 的 .setVelocity 方法，或设置 RUN_WITHOUT_ENCODER 并使用外部 PID 控制器。

PID 控制器采样率¶

对于希望 PID 控制器发挥最大性能的团队来说，必须考虑控制器的采样率。采样率是指控制器根据新的传感器数据更新其输出的时间。较高的采样率可实现更稳定的控制，并允许使用更重要的 PID 系数来减少沉淀时间。请参阅本视频，了解采样率在实际电机控制示例中如何影响稳定性。内置 PID 控制器的刷新率锁定在 20 赫兹（50 毫秒采样率）。许多顶尖的 FTC 团队会优化他们的机器人回路，使其运行频率高达 80hz，从而通过外部 PID 实现 更稳定的控制。

前馈控制¶

前馈控制是一种被称为 “开环 “控制的方法。它与闭环控制相反，主要区别在于前馈控制并不主动使用传感器来控制系统。相反，它根据模型 “预测 “所需的输入。

前馈通常用于控制变化率或消除系统中的已知干扰。

前馈功能非常强大，因为它不受噪声或其他传感器误差的影响。这是因为它不是主动测量系统，而是预测所需的输入。不过，这也意味着它不善于纠正错误。这就是为什么它经常与 PID 等闭环控制器结合使用。

Kv Ka 前馈模型¶

Kv-Ka 前馈模型是最常见的前馈模型，也是 road-runner 等程序库所使用的前馈模型：

\[f(t) = K_v \cdot \text{Velocity} + K_a \cdot \text{Acceleration}\]

其中 \(K_v\) 是速度增益，\(K_a\) 是加速度增益，\(f(t)\) 是发送给电机的前馈输出。

这些增益可以通过给控制器一系列斜坡输入（如用运动曲线计算的输入）来估算，测量输出，然后改变这些增益，直到机器人达到所需的运动状态。

备注

增益会根据机器人的质量、摩擦力和其他因素发生变化。建议每次对机器人进行重大改动时，都重新估算这些增益。

Kv Ka 前馈伪代码¶

while True:
     targetVelocity = getTargetVelocity(time)
     targetAcceleration = getTargetAcceleration(time)
     output = targetVelocity * Kv + targetAcceleration * Ka;

静态摩擦前馈¶

每个系统都必然存在一定的静摩擦力。这意味着，在施加一定量的动力之前，机器人机构不会移动。这可以通过在想要移动的方向上添加一个恒定的前馈项来模拟。

while True:
     error = desire_position - current_position;
     sign = signum(error) # sign of error, will be -1, 0, or 1
     output = sign * staticFriction + PID(error); # PID Controller + Friction Feedforward

Gravity Compensated Feedforward¶

In Gravity Compensation we derive the effect of gravity upon an arm as \(F_g = g\cos{\theta}\). Here we can use that with the following logic.

while True:
    error = desire_position - current_position; # no effect on gravity compensation
    current_angle = (TICKS_AT_ZERO - current_tick) * DEGREE_PER_TICK
    output = PID(error) + cos(radians(current_angle)) * kF

This code uses a kF constant to approximate how much power the arm needs to counteract gravity. In theory, this is something related to gravity multiplied by the rotational moment of inertia of your arm. Calculating this is impractical and kF is instead found empirically. This can be done by setting the gains on the PID to 0, and increasing kF until the arm can hold itself up at any position. If your arm is still falling, increase kF, and if your arm is moving upwards, decrease kF. FTC team 16379 Kookybotz has an excellent video on Arm programming, where they demonstrate how to increase kF.

运动曲线¶

小技巧

运动曲线不是一种特定类型的控制回路，而是一种与其他控制回路（如 PID 和前馈）结合使用的技术。

运动曲线是一种在 FRC® 中得到推广的技术，目前正开始应用于 FTC。运动曲线是一种函数，用于通过逐渐而非瞬时改变所需的速度，以受控和一致的方式改变动力传输系统的速度。

让我们举个例子来说明这一点：假设你希望初始不动的传动系统全速前进。通常情况下，你会在代码中将传动系统的所有电机设置为全速。但是，这样做可能会有问题，因为即使你让电机瞬间全速前进，传动系统也需要时间才能达到全速。这可能会导致不受控制的运动，从而降低自主稳定性，更重要的是可能会损坏机械装置。

运动曲线试图解决这个问题。

优势¶

动作更可控、更可预测
降低外加电机电压的快速变化

劣势¶

可能慢一些

运动曲线主要有两种类型：梯形曲线 和 S 曲线 。梯形曲线以恒定的速度加速系统，而 S 曲线则假定加速度变化的速度是恒定的。鉴于 S 曲线并不是控制二维轨迹（如驾驶）的最佳选择，而且其存在是为了减少打滑（通常只在 FTC 驾驶时才会发生），因此建议在大多数 FTC 应用中使用梯形剖面。

梯形曲线的名称来源于速度随时间变化的图形形状：

梯形运动曲线的位置随时间变化图、速度随时间变化图和加速度随时间变化图 — 这些是前文提到的速度和加速度随时间变化的“神奇函数”。¶

下面是梯形曲线的一些伪代码：

while True:
   current_velocity = get_current_velocity()
   current_time = get_current_time()

   direction_multiplier = 1

   if position_error < 0:
       direction_multiplier = -1

   # if maximum speed has not been reached
   if MAXIMUM_SPEED > abs(current_velocity):
       output_velocity = current_velocity + direction_multiplier * MAX_ACCELERATION * (current_time - previous_time)
       output_acceleration = MAX_ACCELERATION

   #if maximum speed has been reached, stay there for now
   else:
       outputVelocity = MAXIMUM_SPEED
       outputAcceleration = 0

   #if we are close enough to the object to begin slowing down
   if position_error <= (output_velocity * output_velocity) / (2 * MAX_ACCELERATION)):
       output_velocity = current_velocity - direction_multiplier * MAX_ACCELERATION * (current_time - previous_time)
       output_acceleration = -MAX_ACCELERATION

   previous_time = current_time

然后将上述伪代码的结果用于前馈和/或 PID 环路，以平滑和可预测的方式控制系统的位置。

在 Road Runner 库中使用的运动曲线生成数学的更高级示例可在本 Jupyter Notebook 中找到。