基于物理的控制和控制系统：多层控制

AKA：一个强大的车辆控制器所需的所有控制回路的完整描述，包括自平衡系统，如两轮自平衡赛格威类机器人，或四轴飞行器/无人机。

在任何复杂的控制系统中，都必须有多层控制器。
从最内层的控制器到最外层的控制器，以下是您需要的：
1.俯仰角控制器：在你的情况下，你的最里面的控制器听起来像是俯仰角：我认为你正在使用LQR控制器来“调节车轮电机的油门来控制俯仰角*。你也可以为此使用一个PID控制器，或者想出一个基于物理的前馈解决方案，再加上一个消除误差的PID反馈解决方案。
如果将俯仰Angular 设为0度，那么只要没有外力作用，只要机器人开始静止，机器人就会保持直立不动。如果您推动机器人，它将开始以您赋予它的恒定速度线性平移(例如：向前或向后移动)，同时保持固定的垂直Angular 。从本质上讲，将俯仰角设置值保持在0度就相当于推动一个无马达的球或手推车--根据牛顿第一运动定律，它会沿着你推动它的方向不断滚动，这是关于惯性的：运动中的物体保持运动。
1.直线加速控制器：您需要增加一个外部控制器，在这里您可以调整俯仰Angular 来控制直线加速(向前或向后)。
这样想吧：这是一个物理问题：一个两轮的赛格威式机器人向前倾斜得越多，重力就会越快地导致它向前倒下。它越快“向前坠落”，你就必须越快地驾驶这些轮子，试图让这些轮子回到它下面，使它保持固定的倾斜Angular ，而不是继续倾斜，直到它撞到地面。移动它下面的轮子以防止它倒下，会使它朝那个方向加速。
对于固定高度(对于飞行器；对于地面车辆，对于水平表面)和在固定倾斜角的倾斜车辆，线加速度a是：a = g*tan(theta)，其中g = acceleration due to gravity = 9.81 m/s^2和theta = tilt angle。这是我刚刚画的“力量平衡”图(侧视图：朝下朝地，朝上朝天)：

求解theta(倾斜角)，得到：theta = atan(a/g)。
因此，平移(向前或向后)方向的a以m/s^2为单位。随着时间的推移(s)，这种平移加速度会产生一定的平移速度(m/s^2 * s = m/s)。所以，如果你让它平铺一会儿，让它加速，然后再把它竖直起来，你现在就会以固定的速度继续前进，假设地面是水平的，没有摩擦。当有人推它或踢它时，这就是发生在你身上的事情！为了对抗这种平移的“速度”，你需要一个“速度控制器”，它向相反的方向施加必要的“加速度”，以便停止车辆的运动。
1.**线速度控制器：**下一个控制器是速度控制器。您需要设置所需的速度(例如：0 m/s以停止车辆)。在此控制器中，您可以调整线加速度来控制线速度。
然后，您可以设置一个调整参数tau [sec]，它是您希望在哪段时间内达到所需速度的“时间常数”。对于快速响应和低惯性的系统，将其调整为非常小；对于慢响应和高惯性的系统，将其调整为非常大。如果你把它调得太低，系统就会有严重的抖动，并对噪声做出React--就像将你的控制回路设置得太快，或者你的导数增益在PID控制器中太高一样。如果将tau调得太高，系统将非常缓慢且React迟缓。从本质上讲，tau就像一个“增益”调优参数，其中response_gain = 1/tau。因此，较大的时间常数tau会导致较慢的响应或“低增益”，而较小的tau会导致较快的响应或“较高的增益”。
您可以在我的四轴飞行器飞行控制器中看到我的tau值，在这里用黄色圈出：

(video link to this moment in time)。如图所示，当前设置为0.75 sec。我对上图中该参数的注解是：
Time_const，tau(秒)(增益与1/tau成正比)：0.75
<--(增加“较低增益”，减少“较高增益”)
因此，这个“线速度控制器”是另一层基于物理的控制器。你需要这个的物理运动方程：dv [m/s] = a [m/s^2] * dt [sec]。求a，得到a = dv/dt。要完成此操作的时间段tau是您的dt(时间更改，单位为秒)，结果是**a = dv/tau**。

• 完整示例计算*至此：

所以，如果你的实际速度是2.5 m/s，你想要的速度是0 m/s，那么你需要的期望速度变化是。您最终需要将tau调优为调优参数，方法是通过实验或模拟和建模，或者两者兼而有之。假设您选择的初始tau值为tau = 2 sec。然后，在该时间段tau内实现速度变化所需的加速度为**a = dv/dt = dv/tau = 2.5m/s / 2 sec = 1.25 m/s^2。
您的线速度控制器所要求的线加速度这里是您对上面的线加速控制器的输入。从上面的线加速控制器解算俯仰角theta：theta = atan(a/g) = atan(1.25 m/s^2 / 9.81 m/s^2) = atan(0.12742) = 0.1267 rad x 180 deg/pi rad = 7.26 deg。因此，将7.25 deg(根据您的情况，带有正确的符号)作为设定点输入到您的俯仰角控制器，以便在2秒的时间段(Tau)内开始从2.5 m/s减速到0 m/s。
以合理的速度运行内部控制回路，可能是50至500赫兹。
以合理的速度运行外部控制回路，可能是25至50赫兹。
请注意，您的控制回路越“外”，运行控制回路的速度就越慢。例如，自动驾驶汽车最外层的控制回路运行在10赫兹左右，部分原因是这已经足够好了，部分原因是计算复杂性如此之高，以至于计算机只能做到这一点。

总而言之，您的线性速度控制器计算所需的加速度，输入到您的线性加速控制器，计算所需的倾斜角或俯仰，输入到您的俯仰角控制器，后者调整电机油门以实现该俯仰(例如，使用PID或LQR控制器，或您可以为这个内部自平衡回路设计的基于物理的控制器)。

• 为了实现你所说的不再向前滚动的目标，你应该止步于此。*

但是，我会走得更远：
1.线位置控制器：随时间调整线速度，控制线位置。使用你的车轮编码器，你可以计算出你已经走了多远，并控制位置让机器人回到它开始的地方。或者，你可以简单地命令任何任意位置，让它行驶一定距离，在房间里导航。这是另一个基于简单物理/数学的前馈控制器，其中运动方程简单地为v*t = d，其中v [m/s]是速度，t [sec]是时间，d [m]是距离。
有多种方法可以做到这一点。
如果你的目标是去一个位置然后停下来：
一种方法是在给定的时间内控制一定的速度，以达到所需的距离。例如：命令0.5m/s，持续3秒，以0.5 m/s * 3 sec = 1.5 m速度行驶。然后，命令0m/s在该点停止。你可能不得不使用一些经验数据和启发式方法，提前一点命令0米/秒的速度，让车辆有时间做出React，并在你想停的地方停车，而不是超速。
这可能被称为“滴答控制器”(我现在发明了这个术语)，您可以编写一个函数，以X速度驱动N秒，以实现该方向上距离移动的Y编码器“滴答”，并根据需要进行经验调整。您可以将此控制器调整为甚至能够使用快速速度脉冲在短时间内处理小至1个编码器滴答的刻度，以便在接近您的命令位置时到达您想要的精确位置。每次控制循环迭代时，您都会根据您现在所处的位置和想要到达的位置，传递要移动的所需距离编码器“滴答”数的新值。因此，这个基于物理的“前馈”控制器的有趣之处在于，它也隐含着一种“反馈”控制器，这很奇怪。一些书生气十足的学者可能会想出一些特殊的方式来谈论这个问题，甚至可能会有一些特殊的术语来描述它，但我不知道它们是什么。
另一种方法是控制一个固定的速度，例如：0.5m/s，直到你在某个最小的距离误差范围内，比如：0.5m，然后切换到一个基于位置误差来命令速度*的PID反馈控制器。这样，当你的“位置误差”接近于零时，你的“指令速度”也将接近于零，这是有道理的。如果您的PID增益足够强，或者相反，如果您的位置误差足够大，这就相当于在位置上使用PID反馈控制器，通过将速度命令修剪到固定的最大值来使其饱和。当然，即使对于微弱的增益，在一些足够大的距离误差下，指令速度仍然会达到最大允许(饱和)值并被削波。
如果您的目标是在以此速度遵循2D路径的同时保持固定速度：

然后，您可以将速度控制器设置为固定值，同时更改命令方向以始终指向路径下方。我使用的是“引导点”技术或“纯追踪”算法，就像我在这里的三个视频中演示的那样：1，2，3。然而，由于我的车辆是一架悬停的四轴飞行器，我可以奢侈地改变我的命令推力矢量而不是我的航向，所以如果我想要的话，我可以只命令一个固定的方向(即：保持无人机始终指向北方)，同时改变命令的推力矢量向不同的2D(x-y)方向移动。

精确测量：

虽然随着时间的推移积分速度将获得距离，但在这种情况下，最好使用数值积分或估计来计算控制器的前馈部分的命令速度输出，该控制器将在特定的时间内输出所需的速度命令*，以实现所需的位置改变。
使用车轮编码器，您可以测量行驶的“实际”距离，而不是使用随时间推移的速度进行“估计”。
但是，如果您确实需要通过对速度随时间*进行积分来“估计”位置，则应该使用“梯形”数值积分，就像我在这里解释的那样：Numerical derivation and integration in code for physics, mapping, robotics, gaming, dead-reckoning, and controls，因为它比矩形积分更精确，而且实现起来很简单。同样，对于数值积分，简单的想法是velocity [m/s] * time [s] = distance [m]。
再说一次，记住你不应该用积分来“估计”实际行驶的距离。相反，测量通过计算编码器滴答声所走的实际距离，因为这是一种更精确的测量。

如何测量编码器的“滴答”或移动距离：

• 请记住，对于所有这些控制器：*

1.您可以阅读车轮编码器来确定车轮运动。
1.如果你没有车轮编码器，可以读出电机霍尔效应传感器。它们的分辨率较差，但可以用来代替编码器。
1.如果您没有霍尔效应传感器，但您使用的是三线(三相)无刷电机，那么您可以读取反电动势换相波形或周期，以读取换相频率，从而估计电机转速(每分钟旋转数)。你可以计算换向周期来估计位置，就像通过编码器或霍尔效应传感器，但分辨率仍然较低。
1.这很复杂，但可以通过Arduino这样的微控制器在软件中完成(我已经做到了)。你需要做的是将Arduino Nano上的接地连接到你车辆的电池接地。然后，通过二极管限幅电路将整流电压从V_BAT峰值降低到~4.5V峰值，然后再将另一根导线连接到3条无刷电机的任何一条相线上，然后进入Arduino引脚D8，这是ATMega328微控制器的输入捕获引脚。使用软件中的输入捕获，通过中断读取相上的所有脉冲。
1.下面是这些脉冲的样子(image source)：

。
1.小脉冲为8 kHz~16 kHz电机的脉宽调制节流脉冲。你需要在软件中对这些数据进行数字过滤。大的梯形换相频率波是换相波形，它们的频率与电机转速成正比。因此，您可以测量换向频率来计算电机旋转频率，或称RPM。这种关系是由无刷电机中的永磁极数量来衡量的。我必须找出我的笔记，但我相信方程式是这样的：RPM = freq_commutation/(num_magnetic_poles*120)。关键是，换相频率可以通过微控制器的输入捕获引脚在软件中读取，然后通过基于电机中永磁极数量的简单公式转换为电机转速。请注意，高端电机驱动器(ESCS--电子速度控制器)使用正弦换相波形，它比梯形更高效，扭矩更大，但换相频率原理是相同的。
在您的案例中，您将使用的控制器类型也是从最内层控制器到最外层控制器：
1.**俯仰角控制器：**LQR(与您所说的一致)。您也可以使用PID。
1.**线性加速控制器：**基于物理的前馈(控制输入的大部分)。
1.可选地，添加关于实际与命令线加速度的误差的PID反馈来调整它，以使实际线加速度接近命令线加速度。
1.将前馈物理控制器和PID反馈控制器的输出相加。
1.**线速度控制器：**基于物理的前馈(低速时的大部分控制输入)。

1.可选择增加一层物理控制，以补偿随速度增加的空气阻力。Drag [N] = C_D*q*A，其中C_D [unitless]是特定车辆形状属性的实验阻力系数，以及该形状如何与感兴趣的流体(在本例中为空气)相互作用，q [N/m^2] = dynamic pressure = 1/2 * rho * v^2，其中rho [kg/m^3]是空气密度，v [m/s]是速度，A [m^2]是正面面积(这是基于几何的，因为它随着机器人倾斜的程度而减小)。
让我们用快速的单位检查来验证阻力方程：Drag [N = kg*m/s^2] = C_D [no units] * q [kg/m^3 * m^2/s^2 = kg*m*m/(m^3*s^2) = kg*m/s^2 * m/m^3 = N/m^2] * A [m^2] = [N/m^2 * m^2 = N]是的！结果是正确的：阻力方程的右侧(C_D*q*A)确实有牛顿单位([N])。我说的方程式是正确的。
然后，在考虑到机器人加速时空气阻力的额外的基于物理的层的顶部：
1.可选的是，在实际线速度与命令线速度的误差上添加一个PID反馈，以进行调整，使实际线速度接近命令线速度。
1.将所有控制器的输出相加。
1.**线性位置控制器：**一个简单的基于物理的前馈控制器(速度与时间的积分)，以在给定的时间量内实现所需的位置变化。
1.到达所需位置并停止：
1.可选地，在实际与期望的位置上添加一个PID反馈控制器(根据位置误差来控制速度)。保持一个总的+/-轮编码器位移计数，并设法将其归零--位置或距离“误差”越大，朝向目标点的指令线速度就越强。
1.当你的位置误差在一定范围内时，你可以运行一个基于物理的前馈控制器，然后再运行一个PID反馈控制器，或者就像我上面描述的那样，使用一个调整得很好的“滴答控制器”，或者，如果你愿意，只使用PID反馈控制器，当然可以使最大指令速度输出饱和到某个合理的水平。
1.在遵循2D路径的同时保持固定速度：
1.你应该控制固定的速度，同时不断地沿着目标路径移动瞄准点，不断调整你的命令方向，让你朝着正确的方向前进。考虑根据命令的转弯半径动态调整速度，以1)防止倾覆，2)允许你更容易地沿着路径的困难部分前进。在所需路径下调谐引导点向前投影距离是一种基于命令路径的锐度的低通滤波。这可能听起来有点抽象，但看看我上面的3个四轴飞行器视频，我想你会明白我的意思。
1.示例：在这张来自this video at this point in time的图像中，蓝色路径是我的四轴飞行器的所需路径的俯视图，而红色路径是命令路径*，它本质上是通过前导点距离作为调整参数进行低通滤波的。引导点距离越短，红色命令路径将越紧密地覆盖蓝色所需路径，而引导点距离越“长”，红色命令路径将越“平滑”和圆形。对于非常大的引线点距离，红色命令路径几乎是圆形的。

1.上述线性位置控制器都是基于“航位推算”的，使用轮子编码盘通过观察已知起始位置的相对距离变化来测量距离。添加任何绝对位置测量将需要绝对位置“真值源”，例如机外基于相机的定位系统，如Vicon motion capture system或OptiTrack系统(由@Stuff Made使用)、基于声学的定位系统、GPS等，以获得绝对位置的“真实数据”。随着时间的推移，这些“真实数据”可以用来调整机器人内部的航位推算位置估计。
无论如何，在我看来，这就是我们的想法。这就是我在我的四轴飞行器控制器Quadrotor 2 - Physics-based Flight controller demo w/lead point navigation & Arduino interface to RC Tx上采用的方法，这种控制器也可以完美地适用于两轮赛格威式的自平衡机器人，因为四轴飞行器的控制原理与自平衡机器人相同。控件具有许多选项和层。物理应该是其中的一部分，如果不是大多数的话。
我还会提到，我相信上面的所有事情都可以在几乎任何*计算平台上完成，从单一的Arduino Nano(ATmega328微控制器)到功能强大的台式Linux笔记本电脑。我认为，这只需要适量的软件技能。工程学是很难的。编程很复杂。但是，如果你对两者都有足够的了解，你可以在非常弱的处理器上做非常复杂的事情，比如ATmega328(Arduino Uno，Nano等)。我已经在这些处理器上做了一些非常复杂的事情，我还有很多事情要做和学习。

参考资料：

1.我的答案是：Numerical derivation and integration in code for physics, mapping, robotics, gaming, dead-reckoning, and controls
1.我的大学物理笔记：eRCaGuy_Engineering/Equation_Sheets/Physics 110 (General Physics I) - Constants and Equations - Staples#2.pdf
1.我的四轴飞行器模拟：1/3 - Autonomous quadcopter guidance and control (physics-based pure pursuit simulation)

1.请参阅本视频下方的说明。我在那里记下了a = F/m = g*tan(tilt_angle)。这就是我如何记住如何画出上面的“力量平衡”图的原因。
1.我的四轴飞行器现场演示了我上面描述的基于物理的控制器：3/3 - Autonomous quadcopter guidance & control (physics-based pure pursuit demonstration)
1.我所有的3个飞行控制视频：https://github.com/ElectricRCAircraftGuy#flight-controller-videos
1.https://en.wikipedia.org/wiki/Equations_of_motion

另见：

1.我的答案是：Quadcopter PID Controller for distance
1.我可以购买的潜在工具，用于试验我所描述的完整算法：ELEGOO Tumbller Self-Balancing Robot Car Kit Compatible with Arduino, STEM Kits STEM Toys for Kids。这将是一个非常有趣的项目。
1.谷歌搜索"how to write an lqr controller"(LQR=线性二次型调节器)
1.谷歌搜索"cascade control"。这指的是“嵌套控制循环”的概念，这就是我上面描述的控制器。

感官系统的类型、船上的计算单元等肯定会定义你正在采取的方法。由于您没有提供有关设置的更多细节，我们假设您有一个与车身框架对齐的IMU，并且您有一种计算机器人在给定时刻的横摇、俯仰和偏航的方法。还有，假设你计算rpy的速度，至少是主系统循环速度的两倍。
您可能想要从设计三个独立的PID控制器开始，每个控制器用于三个轴，0度是您想要保持的目标状态。很久以前，我通过约束两个轴，一次调整一个轴，使我的四旋翼能够自我平衡。在您的情况下，您首先要使负责其中一个轴的PID能够使机器人处于空档位置，以应对您预计系统在运行过程中将面临的一系列外部干扰。如果你说调整了5-10N的力量踢，但后来又受到了100N的踢，那么PID将不能足够快地做出React。
试一试，也许可以提供关于机器人、你使用的轮子类型等细节的问题。
祝好运。