论文阅读：
Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control
摘要：
提出了一种基于模型预测控制（MPC）的力矩控制四足机器人地面反作用力计算方法。机器人动力学被简化为凸优化问题，同时仍能捕捉系统的完整3D特性。根据简化模型，地面反作用力规划问题的预测范围为0.5秒，并以20-30 Hz的频率在1毫秒内求解到最佳值。尽管使用了简化模型，但该机器人能够以各种速度进行稳健的运动。实验结果展示了对步态的控制，包括站立、小跑、飞跑、快步、跳跃、步速、三条腿步态和全3D疾驰。该机器人的前进速度高达3米/秒，横向速度高达1米/秒，角速度高达180度/秒。我们的方法足够通用，可以用相同的增益和权重来执行所有这些行为。
简介：
由于在许多步态中身体的欠驱动以及地面反作用力的限制，高动态腿部机器人的控制是一个具有挑战性的问题。目前针对高动态运动的解决方案包括用于跳跃和边界的启发式控制器[1]，它们是有效的，但很难调整；二维平面简化[2]，仅适用于没有侧向或滚动动力学的步态；以及用于疾驰的进化优化[3]，目前还不能足够快地解决这一问题，以供在线使用。硬件方面的最新成果包括执行HyQ[4]离线发现的边界极限环，以及在StarlETH[5]上学习腾空、小跑和边界步态。
预测控制可以通过预测飞行或欠驱动的周期来稳定这些动态步态，但由于腿式机器人的非线性动力学以及大量的状态和控制输入，预测控制很难解决。非线性优化已被证明对跳跃机器人[6]、类人机器人[7]、[8]和四足动物[9]的预测控制是有效的，其中[9]展示了启发式算法在调整优化方面的实用性。另一种常见的方法是使用高级计划员（如[10]、[11]中所述）和低级控制器来跟踪计划。最近，文献[12]中的实验结果表明，全身非线性MPC可用于稳定小跑和跳跃。
[13]中讨论的使用凸优化的四足机器人HyQ的稳定性证明了凸优化的实用性，但由于对机器人模型进行了准静态简化，该方法不能立即推广到动态步态。类似地，在双足运动中，凸优化被用于寻找满足瞬时动力学要求的最佳力[14]，并用线性倒立摆模型规划足迹[15]，但后一种方法不包括预测模型中的方向。
尽管在生物学领域[16]，[17]对飞驰进行了深入研究，但令人惊讶的是，几乎没有飞驰的硬件实现。第一个演示飞驰的机器人是欠驱动四足机器人Scout II[18]，它达到了1.3米/秒，但偏航控制有限。麻省理工学院猎豹1号机器人[19]实现了高速疾驰，但被限制在平地上。据我们所知，之前唯一一个带有偏航控制的全3D飞驰实现是在波士顿动力公司开发的液压驱动野猫机器人[20]上。不幸的是，关于WildCat及其控制系统的具体细节尚未公布。
本文的主要贡献是一个预测控制器，它可以稳定大量的步态，包括具有复杂方向动力学的步态。在硬件上，我们实现了180度/秒的最大横摆角速度和3度/秒的最大线速度。在全3D飞驰过程中，速度为3.0米/秒，我们认为这是电动机器人中最快的飞驰，也是所有腿机器人中最快的角速度，在规模上类似于猎豹3。我们的控制器可以表述为一个单一的凸优化问题，该问题考虑了机器人的三维12自由度模型。优化问题的解可以直接用于计算电机转矩，从而稳定机器人，而无需使用额外的反馈控制器。
凸优化方法具有避免非线性优化技术的优点，与凸优化求解器不同，非线性优化技术不能保证找到全局最优解，并且可能会遇到数值问题。凸优化[21]和模型预测控制应用[22]的最新进展导致了许多开源解算器的开发，如ECOS[23]和Qpoas[24]，它们能够快速可靠地解决模型预测控制问题。
该控制器在麻省理工学院的猎豹3四足机器人跑步机上进行了测试，并在地面上进行了放松，可以站立、小跑、飞跑、配速、跳跃、快跑和疾驰。此外，还测试了攻击性转弯动作和全方位移动。机器人由车载电池供电，所有控制计算都在车载处理器上完成。在测试过程中，机器人遇到了许多障碍和干扰，包括一个全尺寸台阶上覆盖着碎片的楼梯，它可以在飞跑时盲目攀爬。为了应对较大的地形干扰，机器人使用了[25]中描述的接触检测算法。对于包括爬楼梯在内的所有测试，机器人除了在电机控制器中使用惯性测量单元（IMU）、关节位置编码器和电流传感器外，不使用其他传感器，后者仅用于电流控制反馈回路。
本文的组织结构如下。第二节描述了猎豹3机器人上使用的控制框架。第三节介绍了动力学模型，第四节介绍了模型预测控制问题，第五节包含实施细节和结果。
控制框架
1.MIT Cheetah 3 Robot
该机器人采用了为猎豹2号机器人[27]开发的本体感知执行器技术，该机器人具有低惯性、易于向后驱动的臀部和膝盖执行器，以减轻冲击。猎豹3在其臀部、膝盖以及另外的ab/ad关节中使用了相同的执行器技术，这使得机器人能够执行动态转向。机器人的四条腿各有三个扭矩控制关节：ab/ad、髋关节和膝关节。每个接头可产生250 Nm的最大扭矩，并可以21 rad/sec的最大速度运行。电机转矩控制通过调整电机电流控制回路的设定值来实现；没有扭矩或力传感器。
2.约定
身体坐标系和世界坐标系如图所示。身体坐标系中的所有量都有一个左下标B。没有下标的量都在世界坐标系中。向量为粗体、直立和小写（a，ω），矩阵为粗体、直立和大写（A，Ω), 标量是小写和斜体（a，ω）。1n用于表示一个n×n单位矩阵。
3.状态机
本文提出的控制器确定了固定时间的脚放置和起飞行为所需的三维地面反作用力。当一只脚被安排在空中时，机器人会运行一个摆动腿控制器，该控制器对早期着陆具有鲁棒性。否则，地面部队控制器将运行。
4.摆动腿控制
摆腿控制器计算并跟踪脚在世界坐标系中的轨迹。用于跟踪轨迹的控制器使用反馈项和前馈项的总和来计算关节力矩。
用于计算腿i关节力矩的控制律为
Ji（R3*3）是脚i的雅各比矩阵，Kp，Kd∈R3×3是对角正定比例和微分增益矩阵，βPi，βVi（∈R3）是第i只脚的位置和速度，βPi,ref ;βVi ,ref （∈R3）分别是摆动腿轨迹的位置和速度的相应参考，τi，ff∈R3是一个前馈扭矩
∧i∈R3×3是操作空间惯性矩阵，ai,ref∈R3是车身框架中的参考加速度，qi∈R3是关节位置的向量，Ci˙qi+Gi 是腿部重力和科里奥利力产生的扭矩。
为了在各种支腿配置中确保高增益稳定性，必须调整Kp矩阵，以保持闭环系统的固有频率相对恒定，以响应支腿视质量的变化。
保持第i轴恒定固有频率ωi所需的第i对角线输入Kp可近似为
Λi,i是质量矩阵中的第i个条目，对应于沿第i轴的腿的表观质量。
所需的足迹位置通过第五节中描述的简单启发式选择。此外，接触检测算法[25]检测并处理早期或晚期接触。如果检测到早期接触，控制器立即切换到站姿并开始地面部队控制。
5.地面力量控制
在地面力控制过程中，使用
其中R是从物体坐标转换到世界坐标的旋转矩阵，J∈R3×3是脚雅可比矩阵，F是根据第四节中描述的模型预测控制器计算的力矢量
简化机器人动力学
预测控制器将机器人建模为一个单独的刚体，在接触面处受力。尽管忽略腿部动力学是一个主要的简化，但控制器仍然能够稳定高自由度系统，并且对这些多体效应具有鲁棒性。对于猎豹3机器人来说，这种简化是合理的；腿的质量大约是机器人总质量的10%。对于每个地面反作用力fi∈R3，从质心（COM）到施力点的矢量为ri∈R3。世界坐标系下的刚体动力学由下式给出：
p∈R3是机器人的位置，m是机器人的质量，g∈R3是重力加速度，I∈R3是机器人的惯性张量，ω∈R3是机器人的角速度，R是从身体坐标转换到世界坐标的旋转矩阵。[x]×∈R3×3定义为斜对称矩阵，（6）和（7）中的非线性动力学激发了下一小节中的近似，以避免模型预测控制所需的非凸优化。
1.近似角速度动力学
机器人的方向表示为Z-Y-X欧拉角[28]Θ=[φ θ ψ]T的矢量，其中ψ是偏航，θ是俯仰，φ是横摇。这些角度对应于一系列旋转，因此从身体坐标到世界坐标的转换可以表示为
式中，Rn（α）表示α绕轴正旋转。在世界坐标系中的角速度可以从这些角随时间的变化率中找到
如果机器人不是垂直指向的（cos（θ）≠ 0），则可以颠倒上述方程式来找到
对于较小的横摇和纵摇值（φ，θ），上述方程式可近似为
这相当于
请注意，欧拉角旋转的定义顺序很重要；在交替旋转的情况下，对于合理的机器人方向，近似值将不准确。
方程式（6）动力学方程第二个 可近似为：
这种近似已在[13]等控制器中进行，并丢弃了旋转体的进动和章动效应。ω×（Iω）项对于角速度较小的物体来说很小，对机器人的动力学没有显著影响。
世界坐标系中的惯性张量可以用
对于较小的横滚角和俯仰角，可以近似为
其中Bi是物体坐标系中的惯性张量。
2.简化机器人动力学
近似定向动力学和平移动力学可以组合成以下形式：
方程（16）可以用额外的重力状态重写，以将动力学转换为方便的状态空间形式：
其中Ac∈R13×13 Bc∈R13×3n，这种形式只取决于偏航和足迹位置。如果可以提前计算这些参数，则动力学将变为线性时变，这适用于凸模型预测控制。
模型预测控制
用离散时间有限时域模型预测控制器求解期望地面反作用力。因为我们考虑地面反作用力而不是关节力矩，预测控制器不需要知道腿的结构或运动学。一般来说，模型预测控制器包含待控制系统的模型，在每次迭代中，从当前状态开始，在有限长度预测范围内，根据状态轨迹和控制输入的约束，找到控制输入的最佳序列和相应的状态轨迹。因为这个过程在每次迭代中都会重复，所以在控制器再次运行并计算新的控制输入之前，只应用计算出的控制输入轨迹的第一个时间步。在这一节中，我们考虑标准长度的地平线长度k的MPC问题。
xi是时间步长i的系统状态，ui是时间步长i的控制输入，Qi和Ri是权的对角半正定矩阵，Ai和Bi离散时间系统动力学，Ci、Ci和Ci表示控制输入上的不等式约束，Di是一个矩阵，它选择与时间步i时不与地面接触的脚相对应的力。符号| | a | | s用于表示加权标准 a'Sa。这种形式的控制器试图找到一系列控制输入，这些输入将引导系统沿着轨迹 Xref 运行,在遵守约束条件的同时，以跟踪精度换取控制努力。如果系统无法准确跟踪参考轨迹（通常是由于欠驱动期间的不可控动态或约束），预测控制器会在预测范围内找到最小二乘意义上的最佳解决方案。在考虑未来约束的情况下，这种超视距优化使预测控制器能够提前规划飞行周期，并在身体始终处于欠驱动状态（如跳跃）的步态期间调节身体的状态。
1.力约束
（21）中的相等约束用于将从脚离开地面的所有力设置为零，从而实现所需的步态。（20）中的不等式约束用于为地面上的每只脚设置以下10个不等式约束
这些约束限制了最小和最大z力，以及摩擦锥的方棱锥近似。
2.参考轨迹生成
所需的机器人行为用于构建参考轨迹。在我们的应用中，我们的参考轨迹很简单，只包含非零xy速度、xy位置、z位置、偏航和偏航率。除偏航和xy位置外，所有参数均由机器人操作员直接控制，这是通过积分适当的速度来确定的。其他状态（横滚、俯仰、横滚率、俯仰率和Z速度）始终设置为0。参考轨迹还用于确定动力学约束和未来的脚放置位置。在实践中，参考轨迹很短（在0.5到0.3秒之间），并且经常重新计算（每0.05到0.03秒一次），以确保简化动力学在机器人受到干扰时保持准确。
3.线性离散时间动力学
对于参考轨迹中的每个点，根据（17）中定义的Bc（r1，…，rn，ψ）矩阵，使用参考轨迹和脚位控制器中的ψ和ri的期望值，计算近似ˆBc[n]∈R13×3n矩阵。类似地，使用参考轨迹期间ψ的平均值，计算整个参考轨迹的单个ˆAc∈R13×13矩阵。使用扩展线性系统的状态转移矩阵，将Bc（r1，…，rn，ψ）和C矩阵转换为零阶保持离散时间模型：
这种简化使我们能够以离散时间形式表达动力学
只有当机器人能够遵循参考轨迹时，（26）上式中的近似值才是准确的。可能由外部或地形干扰引起的与参考轨迹的大偏差将导致ˆB[n]不准确。然而，对于第一个时间步，ˆB[n]是根据当前机器人状态计算的，并且总是正确的。如果在任何时候，机器人受到干扰，无法遵循参考轨迹，则MPC的下一次迭代（干扰发生后最多40 ms）将根据受干扰的机器人状态重新计算参考轨迹，从而允许其补偿干扰。
4.QP二次规划
对（18）中的优化问题进行了重新表述，以减小问题的规模。虽然能够利用问题的稀疏性和结构的解算器可能是最快的解决方案，但忽略结构和稀疏性，转而致力于缩小问题的规模，则更直接、速度也足够快。对于不利用问题稀疏性的解算器，等式（18）中的公式在地平线长度、状态数（控制输入和状态轨迹）和约束数（力和动力学）方面具有立方时间复杂性，因此，我们通过从约束和优化变量中移除动力学约束和状态轨迹，并将其包含在成本函数中，从而实现了显著的加速。该公式还允许我们消除受（21）约束为零的琐碎优化变量，以便我们只优化地面上脚的力。这是基于[29]中讨论的浓缩公式，但包括时变动力学和状态参考。
浓缩公式允许将动力学写成
X∈R13k 是预测区间内所有状态的向量，且 u∈R3nk 是预测范围内所有控制输入的向量。
使参考轨迹和加权力大小的加权最小二乘偏差最小化的目标函数为：
L∈R13k×13k是状态偏差权重的对角矩阵，K∈R3nk×3nk是力大小权重的对角矩阵，U和X是预测范围内所有控制输入和状态的向量。
在机器人上，我们选择了相等权重的力K=α13nk。问题可以改写为：
其中C是约束矩阵
所需的地面反作用力是U的前3n个元素，注意，H∈R3nk×3nk和g∈R3nk×1都不依赖于状态数，只依赖于英尺数n和地平线长度k。
结果
控制器设置和机器人数据
1.实验装置
该控制器在MIT Cheetah 3机器人[26]上实现，其水平长度为一个步态周期（0.33到0.5秒），根据所选步态细分为10到16个时间步。根据选择的步态，在25至50赫兹的频率下进行了新的预测。根据视频游戏控制器的输入生成参考轨迹和期望步态。上表包含用于预测控制器的所有参数。
在（4）中，直接使用优化结果，而不使用过滤器来计算关节扭矩。状态估计、摆动腿规划和腿阻抗控制发生在1 kHz。
受[1]启发的一个简单启发式函数用于在世界坐标系的XY平面上规划所需的脚位置：
∆t是脚在地面上停留的时间，Pref是机器人臀部下方地面位置的前缀，vCoM是机器人质心投影到xy平面上的速度。状态估计器用于确定当前时间步长的这些值，参考轨迹用于未来值。Pdes用于摆动腿控制器，并用于寻找预测控制器的ri。
该机器人有一台车载电脑使用英特尔i7笔记本电脑处理器，并运行Ubuntu Linux，高级控制代码（包括状态估计、摆动腿控制和动力学计算）在MA TLAB/Simulink中实现，并转换为C代码。本文描述的MPC设置是用C++语言实现的。该软件使用了Egeng3线性代数库[30]和qpOASES[24]二次规划库。qpOASES solver专门使用在线活动集策略[31]解决MPC的二次规划，并用于解决凸优化问题。
为了评估预测控制器，对所有常见的步态进行了测试：站立、小跑、飞跑、踱步、跳跃、旋前和飞奔，以及原始的三条腿步态。
2.Trotting
一个简单的小跑步态是非常有效的速度低于1.2米/秒。小跑时，机器人没有首选的行进方向；它可以很容易地向前、向后、横向和以一定速度沿对角线移动，甚至可以在绕中心旋转时沿直线移动。图5显示了旋转速度超过150度/秒时的攻击性原地转弯，导致脚部在地面时相对于身体的线性速度超过1米/秒。
此外，该图显示了在两秒钟内从-1到1 m/s的单独加速测试。我们在10米乘1.3米的平台上实现这些速度的能力证明了机器人的灵活性。小跑步态也在室外进行了测试，测试速度可达1.2米/秒，机器人未固定。为了证明机器人的健壮性，机器人在泥泞和倾斜的地面上行驶，即使双脚在泥泞中滑动，也能保持稳定。
当以高速向前小跑时，摆动的脚可能会与抬离地面的脚发生碰撞，从而有效地将小跑步态限制在低速。飞行小跑增加了飞行周期，将最高速度提高到1.7米/秒，允许机器人以180度/秒的速度原地旋转。
3.干扰和楼梯
为了测试控制器的抗干扰能力，机器人在小跑时被狠狠地踢了一脚。踢的力度足以使机器人的身体侧向加速1米/秒以上，但机器人能够恢复，同时保持水平10度以内的滚动和俯仰，如图7所示。
在有4个全尺寸台阶的楼梯上测试了飞跑步态，它能够可靠地爬升。楼梯上散落了大约15块模拟木屑，故意让机器人的脚打滑。增加了摆动轨迹的高度，以允许双脚到达下一步，但机器人对楼梯一无所知。接触检测算法[25]允许在脚比预期更早或更晚落地时实时修改步态。我们曾三次试图爬下被碎石覆盖的楼梯。
4.腾空/跳跃
为了证明我们控制器的鲁棒性，我们测试了一个腾空步态，这是一系列大约15厘米高的短跳。机器人四只脚都在地面上的时间为150毫秒，然后是350毫秒的飞行时间。在150毫秒的接地过程中，机器人能够着陆并稳定自身，使其能够无限期地向前移动。
5.奔腾
由于步态频率更高（3.3赫兹）和飞行时间更长，在猎豹3上跳跃能够获得比小跑更快的速度。在飞行时间较短的情况下跳跃时，前腿必须收窄，以便后脚可以摆动过去。对于我们的跳跃步态，每只脚在地面上停留90毫秒，摆动210毫秒。跳跃时，机器人能够以高达180度/秒的速度原地旋转。为了证明控制器的稳定性，左右脚之间的间距减小，直到机器人摔倒。机器人能够将双脚无限期地分开10厘米。由于双脚相距5厘米，机器人能够跳跃，但对侧向速度的控制较差，限制了测试大约10步。图6显示了2.25 m/s边界期间的方向。边界测试速度高达2.5 m/s，可能会更快，但观察到驰振更平稳，因此所有高速测试都使用它。疾驰能够达到3米/秒的最高速度，在这一点上，让机器人保持在跑步机上变得很困难。图10显示了飞驰至最大速度的准确速度跟踪，图8和图9显示了2.5 m/s飞驰期间的腿和控制器数据。
6.高速时的限制
麻省理工学院猎豹3机器人的研发目的是为了高效运作，在室内和人类附近携带有效载荷。因此，它的驱动器和腿的设计速度低于猎豹2号，限制了脚的线速度。在测试高速运动时，安装在猎豹3号上的腿的最大速度为15rad/s，明显低于在猎豹2号上观察到的24rad/s的最大速度。在接近髋关节速度极限时，摆动腿控制器不能很好地跟踪。在我们试图以超过3米/秒的速度前进时，腿无法及时向前摆动足够远，导致机器人的方向控制变得不那么准确。机器人能够处理不断增加的横摇和俯仰误差，但由于横摆速度漂移，将机器人保持在1.3米宽的跑步机上变得很困难。我们所有的高速测试都以机器人意外地从跑步机一侧被驱动或机器人机械故障而结束。我们怀疑，随着腿部速度的增加，可以达到更高的速度。在我们对机器人进行的完整的多体仿真中，当我们将腿部速度提高到最大30 rad/s时，我们已经实现了最高6 m/s的速度。
结论和未来工作
尽管对机器人动力学模型进行了显著简化，但该控制器在各种步态下都具有良好的性能。我们相信我们的控制器是成功的，因为我们的近似模型仍然捕捉到了运动的许多重要细节，包括地面反作用力约束。我们发现，在预测期内，机器人动力学的高精度模型不如机器人瞬时动力学的精确模型重要。我们的方法在预测期内频繁更新动态，延迟最小，确保瞬时近似始终准确。在我们的控制算法迭代之间，动力学的线性近似似乎就足够了。
在未来，我们计划更彻底地调查不同的步态和攻击行为。通过设计更有利的步态，很可能可以实现更高的速度和更大的稳定性。到目前为止，所有的机器人都是用人工创造和模仿腿部步态的。我们计划将这些预先确定的步态替换为由更高级别的规划人员确定的接触模式，以提高我们运动的鲁棒性和性能。该框架将允许高级规划器缓慢运行，而低级预测控制器可以继续快速运行。