【双足轮机器人】SK8O技术详解--（3）控制系统【翻译】

3 控制系统

在本章中，我将介绍用于稳定和运动控制的控制器，状态观测器以及跳跃轨迹的计算。 基于线性化的3D轮式倒立摆模型，我设计了状态反馈控制律。为了过滤和融合所有可用的测量，我使用了无迹卡尔曼滤波器。最后，我通过求解非线性优化问题来描述跳跃状态轨迹的计算。

3.1 稳定与运动控制

最初，打算基于线性化的平面模型为机器人的左侧和右侧设计两个独立的LQ状态控制器。不幸的是，由于这种系统的非最小相位动力学，该方法在偏航运动中变得不稳定，因为未考虑两侧的耦合。通过引入一个外部比例偏航阻尼器，我设法在仿真中克服了这个问题。 当在实物机器人上应用时，该解决方案被证明是无效的，因为在仿真中起作用的阻尼器增益太大，从而导致真实机器人的抽搐和摔倒。另一方面，具有较小增益值的阻尼器无法稳定偏航动力学。因此，我必须使用包含偏航耦合器的模型来进行控制器设计。我自然选择了完整的机器人3D模型。然而，本文旨在为机器人的第一个原型设计一种控制系统。因此，我打算尽可能简化数学模型和控制系统。我决定使用机器人的倒立摆简化来实现轮子稳定和身体电机的PD控制，与[3]中类似。

3.1.1 非线性模型的平衡

在进行控制器设计之前，我需要找到机器人的配置-平衡-使机器人稳定。 由于导出的运动方程是非线性且复杂的，用解析的方法计算它的解将是一项乏味甚至不可能完成的任务。因此，我采用了数值优化。

我将平衡时的角加速度和速度设置为零，因此根据（2.27）：

这与（2.31）中的降阶模型一起给出了平衡条件:

可以进一步简化为:

如果$g$项是由输入${\tau }_{in}$补偿的，或者如果向量 $-g+{\tau }_{in}$是$G^T$的零空间的元素，则满足条件。为了找到这样的平衡点，我制定了以下优化任务：

在条件（3.3）和闭环运动约束（2.15）的情况下，以指定的期望值${\theta }_1$来最小化输入力矩：

这个问题有多种解决方案。其中有一些会导致相交。为了避免这些配置，我限制了剩余角度的值，以使其位于可接受的范围内。我用Matlab中的约束非线性问题求解器找到了理想的解决方案，其结果如表3.1所示。

3.1.2 3D轮式倒立摆模型

大众熟知大多数的摆式系统模型，图3.1中所示的轮式摆也不例外。因此，我不必自己推导它，而使用文献[9]推导的那个模型。为了方便读者阅读，这里写出它的方程式。 状态空间模型如下：

表3.1：平衡角和输入值

图3.1：简化的3D轮式倒立摆

其中$I_{0xy}$在垂直方向上轮子的转动惯量，$m_p$，$I_{px}$，$I_{py}$，$I_{pz}$是不带轮子的整个摆的质量和转动惯量，l是连杆的长度，$w$是轮子的距离，$x$是摆的前进距离，$\phi$是俯仰角，$\psi$是偏航角，并且$u_{0L}$，$u_{0R}$分别是左右轮子的力矩。参数$m_0$，$r_0$，$I_0$，$b$，$g$与平面机器人模型中的参数相同。

接下来，使用上一部分的平面模型和平衡姿态来计算摆的参数。得到的摆的质量是除轮子以外的所有机器人零件质量的总和：

连杆长度l作为平衡位置下轮子与身体之间的距离来计算：

我决定忽略连杆的转动惯量，因为它们的值比身体的值小两个数量级。因此，我设置摆的惯性与Gazebo仿真器中的身体参数相匹配。距离$w$也是根据3D Gazebo模型选择的。

3.1.3 线性化和离散化

接下来，继续进行3D轮式倒立摆模型的线性化（3.6）:

通过计算线性连续系统$A_c$，$B_c$的状态空间矩阵：

从线性连续系统中，我使用零阶保持器获得了状态空间矩阵$A_d$，$B_d$的离散时间系统：

其中$T=0.001s$是预期的采样周期。

3.1.4 控制器设计

有了线性化的模型之后，我进行了控制器的设计。首先，我通过消除偏航角和前进距离来简化模型。这些状态对稳定或操纵都不重要，其余状态均与它们无关。因此，减少仅仅关系到从$A_d$中删除第4行第4列和第6行第6列，以及从$B_d$中删除第4行和第6行。

获得简化模型的状态空间矩阵$A_r$和$B_r$后，我创建了一个增强系统，在LQR中积分[10]以进行速度和偏航率的参考跟踪。


其中$\epsilon$是积分跟踪误差，$I$是2×2单位矩阵，$r$是参考向量。然后遵循标准无限视野$LQR$设计程序[11]，获得轮子稳定控制律的状态反馈增益$K$和输入权重矩阵$Q_{LQR}$，$R_{LQR}$：

反馈回路如图3.2所示。

摆的状态到已测量（或估计）的机器人状态的映射如下：

图3.2：建议的$LQR$的控制回路。

下标$R$和$L$表示机器人的右侧或左侧。

身体电机通过简单的$PD$控制律进行控制：

通过实验调整了$p$，$d$值。参考值用于更改机器人腿的长度，默认值为${\Delta }_{4，ref}=0$。

3.1.5 平衡性能

我检查了控制器对三个不同腿的长度值的平衡性能。第一个是由图2.2中的平衡值给出的默认长度。其他两种情况分别对应于图3.3中的拉伸和蹲坐机器人姿势，分别为${\Delta }_{4，ref}=0.15$ rad和${\Delta }_{3，ref}=-0.15$ rad。

在测试场景中，我首先向静止的机器人背部施加了一个$2kN$的$0.2ms$长的冲击力，仿真了明显的向前推动。两秒钟后，我在机器人一侧施加了相同的冲击力，以查看身体控制器的性能。

系统的响应根据Gazebo仿真器的设置变化而发生变化，特别是约束力混合（CFM）参数值的变化。该参数用于通过将硬关节约束转换为软约束来提高仿真稳定性，从而降低精度。将CFM设置为较低的值或零时，在仿真过程中会出现关节角度和速度的峰值。另一方面，对于高的CFM值，仿真的行为是不自然的。我总是试图将CFM设置得尽可能低。在大多数仿真中，该值介于$10^3$和$10^4$之间。

图3.3：拉伸和蹲下的平衡姿势

在默认姿势下的机器人可以很好地处理两种干扰，如图3.4和3.5所示。后推力的力矩峰值，$u_0$是$-0.7Nm$，$u_4$是$0.4Nm$。侧面碰撞后，$u_4$的峰值达到$\pm 0.5Nm$，并且轮子上几乎没有施加力矩。侧面推动后，见到的振动是由机器人双腿之间的重量负载转移引起的。

图3.4：默认姿势-后推的响应

图3.5：默认姿势-侧推的响应

图3.6：拉伸姿势-后推的响应

图3.7：拉伸姿势-侧推的响应

图3.8：蹲姿-后推的响应

图3.9：蹲姿-侧推的响应

拉伸机器人的动作与默认姿势相似，如图3.6和3.7所示。唯一的例外是向后推动后立即出现$u_4$峰值，这可能是由与CFM参数相关的数值误差引起的。$u_4$和${\Delta }_{\phi }$的非零稳态值只是非平衡姿态稳定的结果。对于后推，$u_0$再次在约$-0.7Nm$处达到峰值，而$u_4$在与稳态值相差$\pm 0.4Nm$处达到峰值（如果未计算峰值）。侧推性能与在稳态处忽略轮子力矩的$u_4$峰值相差$\pm 0.5Nm$。

如图3.8所示，下蹲姿势的后推响应再次与默认姿势行为相匹配，轮子力矩稍高一些，峰值为$-0.8Nm$。 图3.9中捕获的侧推行为对应于其他两个姿势，它们具有相同的稳态差值，也是$u_4$峰值$\pm 0.5Nm$。

3.1.6 转向性能

我用简单的参考轨迹测试了三个姿势的转向性能。首先，要求机器人达到$0.5m/s$的速度。然后，它应以$1.1rad/s$的偏航率开始向左转，同时保持速度。旋转3秒钟后，机器人应该停止移动。

图3.10：默认姿势-转向

图3.11：拉伸姿势-转向

图3.12：蹲姿-转向

从图3.10、3.11和3.12可以看出，在所有情况下，机器人都能毫无问题地跟随参考轨迹。它在$1s$后达到速度和偏航参考值（在顶部曲线图中由蓝色和黄色虚线表示），并且轮子力矩峰值在$-0.2Nm$附近。在默认的拉伸姿势下，转弯时可能在离心力的作用下，在腿之间发生了载荷转移。 当转弯停止时，负载转移会导致与侧推平衡测试中类似的振荡行为。下蹲姿势中的离心力作用可以忽略不计，因此未观察到振动。

尽管所选择的方法很简单，即使腿长有所变化，控制器也可以稳定和操纵机器人。

第4章所述的真实机器人的实验也证实了这一点。

3.2 状态观察器

正如我在上一节中提到的那样，我最初打算在平面模型的基础上使用两个状态反馈控制器。腿的闭环中只有角度$({\theta }_4)$和角速度$({\theta }_4)$中的一个被测量，编码器仅提供${\theta }_0$，${\theta }_1$和${\dot{\theta }}_0$，${\dot{\theta }}_1$之和。因此，必须对其进行计算或估计。为此，我实现了无迹卡尔曼滤波器，该滤波器除了计算未测量的数量外，还对所有可用的测量值进行滤波和融合。即使对于具有数据的摆态的反馈也很方便。

3.2.1 无迹卡尔曼滤波器

无迹卡尔曼滤波器（UKF）[12] [13]是一种卡尔曼滤波器变体，它使用所谓的无迹变换来估计通过非线性动力学和系统测量传播的状态分布的均值和协方差。UKF比扩展卡尔曼滤波器（依赖于系统线性化的分布估计）更精确[12]，[13]，且计算复杂度相同。

在无迹转换中，选择了一组$sigma$点，以使其样本均值和样本协方差与状态分布相同。然后分别在每个点上应用非线性变换。他们的新加权样本均值和样本协方差用于估计新的分布。

假设只有白色加性高斯过程和具有协方差Q，R的测量噪声，我用无参量无迹变换[14]来实现UKF。在数据更新步骤中，$2n+1$ $sigma$点$s_i^{\sigma +}$及其权重$W_i$是从先前的预测值${\hat{s}}_{k-1}^{+}$及其协方差$P_{k-1}^{+}$中计算：

其中，状态数中的$n$是缩放参数，$\kappa$是一个标度参数，$(\sqrt{(n+k)P_{k-1}^{+}}{)}_i$的矩阵$(n+k)P_{k-1}^{+}$平方根的第$i$列（如果矩阵是正定的，则从Cholesky分解获得）。通过输出函数$h（s）$对$sigma$点进行转换，然后将其与实际测量值$y$进行比较，并更新状态估计值：

在时间更新步骤中，类似于数据更新步骤（3.27），（3.28），（3.29），根据${\hat{s}}_k^{-}$和$P_k^{-}$计算西格玛点$s_i^{\sigma -}$。然后，使用输入为$u$的离散（或离散）系统动力学$z（s，u）$对它们进行变换，并预测状态：

由于仅通过伪速度将角度约束$a（s）$（2.15）包括在模型中，因此此类实现不执行角度约束$a（s）$（2.15）。因此，将无法正确估计它的值。我通过引入这些约束条件作为完美的测量值来克服了这一点[15]，这是通过扩展度量转换来完成的：

我为机器人的每一侧实施了两个单独的UKF，他们具有共享的俯仰和俯仰速率数据。没有使用侧倾（侧倾率）和偏航（偏航率）信息，因为这需要侧面之间的耦合，而简化的平面模型中未包括这些信息。根据对实物机器人测量的最坏情况预期选择测量噪声协方差（在设计时尚未构建实物机器人）。

3.2.2 估计和控制性能

我在与之前相同的两种情况下，使用了包含在反馈回路中的观察器对估计和控制性能进行了测试（请参阅第3.1.5和3.1.6小节）。三个腿长的估计性能都相似，这并不奇怪，因为UKF是非线性观察器。因此，我只会展示默认姿势的结果。

在平衡测试中，机器人成功地承受了背部和侧部的侧推力，如图3.13和3.15所示。响应比理想情况下稍慢，但这是预料中的。否则，性能是不错的，并且状态估计值与实际值紧密匹配，如图3.14和3.16所示。

从图3.17可以看出，机器人也可以遵循转向测试中的参考，但是转弯后的振荡行为要差得多。如图3.18所示，当机器人向左转时，˙1和˙3的估计值偏离了实际值。转弯结束后，估算值在剧烈波动的同时向后移动。同样，这是由于以下事实造成的：在转向时不考虑两侧之间的耦合和载荷传递。

总体而言，状态估计对于平衡和前进（后退）运动效果很好。即使估计在转弯过程中发生了变化，控制系统也能够操纵机器人。理想情况下，应该引入两个机器人侧面的耦合以改进估计，方法是将完整的机器人模型用于UKF设计，而不要使用两个独立的平面模型。

图3.13：UKF的后推响应

图3.14：向后推—实际值（红色）和估计值（蓝色）的左侧比较

图3.15：UKF的侧推响应

图3.16：侧推-实际值（红色）和估计值（蓝色）的左侧比较

图3.17：UKF的转向性能

图3.18：转向-实际值（红色）和估计值（蓝色）的左侧比较

3.3 跳跃动作

在本节中，我将介绍跳跃轨迹的计算，这是对文献[16]中所述方法的简化。首先，我重新推导了机器人的平面模型（2.27），（2.31），所以它不直接包含恒定的轮子高度和无滑条件，而是通过代表地面反作用力的拉格朗日乘数来强制执行它们。然后，我使用非线性优化找到了满足离散化动力学的跳跃轨迹。我通过在3D仿真器中得到的控制序列作为前馈来测试该轨迹。

3.3.1 扩展模型

扩展模型的推导与第2章完全相同，唯一的不同是添加了两个表示轮子位置的广义坐标和两个约束

为了避免任何不必要的重复我跳过了推导过程。对应于（2.29）的结果模型如下：

3.3.2 优化

可行的状态轨迹受动力学的约束，与[16]中类似，我使用后向欧拉方法获得了（3.38）的式子：

令${\bar{M}}_{k+1}={\bar{M}}_{(z_k)}$，同样。

跳跃可以分为两个阶段。在第一阶段，机器人在地面上准备跳跃。这意味着约束条件$\bar{a}$（或等效条件${\bar{J}}_w$）是活动的，并且应等于零。在第二阶段，机器人在空中。因此，$\bar{\lambda }=0$，因为没有来自地面的反作用力施加在轮子上。由此，我制定了以下约束：

其中$N_{jump}$是跳跃的离散时间，$N$是整个动作的终止时间。（3.42）中的条件${\lambda }_{g,k}\ge 0$表示了这个实际情况，即阻止轮子穿透地面的反作用力是非负的。约束条件（3.44）确保轮子在空中阶段结束时处于地面位置。可以使用更多约束来调整跳转属性或限制最大和最小状态与输入值。同样，如果需要着陆参考，则可以类似地将着陆添加到跳跃准备阶段。成本函数应该是基于状态、输入和乘数的任何非线性函数。为了避免不必要的高投入，我选择了二阶投入的成本：

值得一提的是，这种简化的方法绝不是完美的。它需要针对给定的采样时间$T$手动调整$N_{jump}$和$N$，设置可变的采样时间可能会稍微不方便。可以从结果轨迹中获得适当的时间间隔值，然后将其调整为选定的固定采样时间。此外，对文献[16]中提出的完整方法的修改将解决这个问题。

我使用CasADi [17]在Matlab中提出的问题，该框架允许使用高级编程语言来制定优化任务。CasADi使用代数微分来计算必要的导数，然后调用了非线性问题求解器，尤其是内部点优化器（IPOPT）。这使得实现既简单又有效。对于约等于500的$N$值，该问题在程序初始化后的几分钟内（我的笔记本电脑配备是Intel i7-8750H CPU）收敛。

3.3.3 结果

我在3D仿真器中将输入值作为前馈控制序列，在两个测试场景中测试了该方法。操纵过程中关闭了稳定功能，之后立即将其打开。这是不理想的，因为这种方法在很大程度上取决于模型在跳跃和着陆时的稳定控制过程中的精度。在[3]中，作者实施了具有五个不同阶段的启发式前馈控制和接地检测来改善了这一点。更好的解决方案是使用跳跃轨迹跟踪控制器。尽管如此，仿真实验仍然可以证明所产生的轨迹是有效的。

第一个测试场景是向上跳跃。在图3.19中可以看到优化结果，实现的跳高高度离地面$20cm$。除了限制最大输入值外，我还在跳跃阶段的中间限制了轮子高度$y_0$以获得这样的高度。前馈3D仿真轨迹与预先计算的轨迹几乎无法区分，如图3.20所示。与图3.21进行比较时，唯一可见的区别是在3D仿真器中，机器人稍微倾斜了一些。

第二种测试场景是向前跳跃。图3.22显示了获得的状态轨迹和输入信号。跳跃高度为3.5厘米，跳跃距离约为30厘米。在这种情况下，我还限制了最大输入值，并基于时间N处的$x_0$值添加了跳跃距离的要求。同样，来自3D仿真器的轨迹几乎相同，如图3.23所示。 图3.25中的可视化比较中也证实了这一点。即使向前跳跃的轨迹与参考点紧密匹配，但跳跃后的稳定并不总是成功的，尤其是对于较高的跳跃。这仅仅突出了前馈和稳定切换方法的缺点。

图3.19：向上跳跃-状态轨迹和从优化中获得的输入序列

图3.20：向上跳跃-3D前馈仿真状态轨迹

图3.21：向上跳跃-简化模型仿真（顶部）和3D前馈仿真（底部）的视觉比较

图3.22：前跳-状态轨迹和从优化获得的输入序列

图3.23：前跳-3D前馈仿真状态轨迹

图3.25：前跳-简化模型仿真（顶部）和3D前馈仿真（底部）的视觉比较

双足轮开源技术交流QQ群：543613782

参与翻译者：灯哥开源团队和群友Boiling Tears