1. 简介

在仿人双足机器人的控制里面，可以将机器人模型简化为一个线性倒立摆模型（下图来自梶田秀司教授的《仿人机器人》[1]，中文电子版详见代码仓库），用于机器人的平衡控制和步态规划。线性倒立摆模型及其后续扩展的其他模型在双足机器人的研究中被广泛采用，取得了不错的控制效果。今天，我们就来聊一聊其中最经典，也是最基础的模型：线性倒立摆模型。

2. 线性倒立摆模型

在矢状平面内，线性倒立摆模型（Linear Inverted Pendulum Model, LIPM）由质心（COM）和一个可伸缩的轻质量的腿构成，如图所示。

倒立摆的输入包括作用于支点处的力矩  和沿腿连杆方向伸缩关节上的伸缩力$f$。一般情况下，由于双足机器人的足底与地面的接触面（脚板）较小，我们不能输入很大的力矩  ，因此，这里假设  。这种情况下，倒立摆与地面接触的点只有一个支点，是不稳定的，总会倒下，除非我们让线性倒立摆的质心一直保持在水平线上，可以通过  控制腿伸长缩短来做到，保持一个恒定的质心高度，这样的模型被称为线性倒立摆，是一种最简单的倒立摆模型。

在平地上运动时，我们假设了倒立摆的质心只在水平方向沿水平约束线运动，而在垂直方向的运动被忽略。这里假设腿可以无限伸长，那么倒立摆的质心将沿着水平线一直运动下去。因此，腿上的伸缩力  的垂直分量  与重力大小相等，方向相反，而质心在在水平方向的运动由的水平分量  决定，所以我们可以得到线性倒立摆的水平运动方程：

即：

其初始状态  和  确定以后，倒立摆的运动状态可以描述为：

那么，对于具有2条腿的双足机器人来说，其水平的运动模式可以简化为具有2条腿的线性倒立摆模型在水平方向上的运动，双腿交替运动的过程示意图如下所示：

我们可以通过控制两条腿迈步的步长以及切换的时间来控制倒立摆的运动状态。但是，为了倒立摆模型能够持续稳定的运动下去，我们需要一种方式来表征和度量倒立摆的运动状态，并以此为参考依据来动态计算每一步的落脚点。轨道能量（Orbital Energy）就是其中一种。

3. 轨道能量

轨道能量是一种虚拟的势能，我们可以简单的理解为轨道能量反映的是质心COM的运动状态。线性倒立摆的初始状态确定以后，在后面的运动过程中其轨道能量守恒，即保持不变。轨道能量  的计算方法:

其中，  为质心的水平运动速度， 为质心的水平位置（相对于支撑脚），  为重力加速度常数,  为质心的高度。依据初始状态  的不同，其轨道能量的大小不同，可能为正值，也可能为负值。根据倒立摆轨道能量的符号和倒立摆模型的运动方向，可以分为以下4种情况：

其中：

• 第一行的2幅图片表示倒立摆向右运动，其中第1幅图片中其轨道能量小于零，即初始速度不够大，质心无法越过势能最高峰，所以质心在到达势能最高点之前速度降为零，然后反向运动；第2幅图中其轨道能量大于零，所以质心可以越过势能最高峰而继续向前运动；
• 第二行的2幅图片表示倒立摆向左运动，第1幅图中轨道能量大于零，质心可以越过势能最高点继续向前运动；第2幅图中轨道能量小于零，质心在到达势能最高点之前速度降为零并开始反向运动， 上述4种情况下质心运动的速度变化图如下：

在单个摆动周期中，一旦倒立摆模型的初始状态确定，则轨道能量就确定了。但在具有双腿的倒立摆模型的运动中，我们可以通过控制支撑腿切换来在下一个摆动周期中获取一个新的轨道能量，这样我们就能通过控制支撑腿的切换时刻来控制轨道能量，达到控制倒立摆模型运动状态的目的。

那么，假设我们给定了切换支撑腿以后的下一个摆动周期中的期望轨道能量，如何才能计算得到支撑腿的切换时刻以及摆动腿的下一个落脚点呢？

4. 捕获点（Capture Point）

Cpture Point最初由Jerry Pratt [2]提出的，基于LIPM模型的摆动腿落脚点，根据当前的轨道能量  和目标轨道能量  来计算，可以保证在切换支撑腿并迈出下一步以后的新的轨道能量为目标值  。Capture Point是通过控制迈步的长度（步长）来改变轨道能量，使得在迈出一步以后轨道能量能够不被控制到0，从而获取一个稳定的过程。当然，我们可以通过控制支撑腿的切换时间来间接控制步长。这与我们人类的平衡策略相同，例如如果有人在后背猛烈的推你一把，那么你势必将迈出去一步来保持平衡。当然，有时候如果外部冲击太大，由于我们腿长有限，因此迈一步也无法回到平衡状态，我们需要多迈几步。所以，后来Capture Point又被扩展了来处理这种更大的冲击的情况，这里不再详细介绍其过程，有兴趣的可以看相关的论文。

Capture Point最初是基于目标轨道能量为零提出的，保证控制倒立摆模型的平衡，使得其在受到外界干扰以后，能够获取一个动态平衡的控制过程。但是在实际应用的时候，目标轨道能量  不一定设置为零，我们可以分为两种情况讨论：

• 若设定为一个正值，那么可以得到下一个落脚点使得模型在切换支撑腿以后可以继续向前运动；
• 若设定为一个负值，那么可以得到下一个落脚点使得模型在切换支撑腿以后可以反向运动。

在矢状平面内，Capture Point即为落脚点的水平位置（图中的  ，此时坐标原点位于支撑腿与地面的交点，即图中的虚线与地面的焦点），因此，根据轨道能量的计算公式我们有：

那么，无论是对于情况（a），初始轨道能量足够大，模型可以越过势能最高点继续运动；还是对于情况（b），初始轨道能量不够大，模型在到达势能最高点之前速度已经降为零，开始反向运动；在切换支撑腿以后，新的坐标原点已经转换到新的支撑腿与地面接触位置（图中虚线与地面的交点），我们都有：

变换可得：

不等式恒成立。

再次变换可以得到：

注意，这里如果我们设  ，则我们将会得到Jerry Pratt提出的最原始的Capture Point，计算出的  将使得倒立摆模型迈出一步后在势能最高点的质心速度为  ，不再向前和向后运动，即迈出一步后达到禁止和平衡的状态。当然，这只是一个理论值，在实际的情况中这是一个非稳定状态。

回到我们的话题中来，显然，对于图中的情况（a），我们有  ，而对于情况（b），我们有 ，因此最终结果为：

其中，  为符号函数，返回质心运动速度  的符号，为  或者  。

5. 基于Python的倒立摆运动仿真

我们构建了倒立摆模型的运动仿真代码，使用Python实现，在Python 3.6的平台上已经测试通过了。我们分别仿真了单腿的摆动情况和双腿迈步行走的情况，给定不同的目标轨道能量，其结果分别如下：

5.1 单腿摆动，初始轨道能量大于零，模型越过势能最高点继续运动：

5.2 单腿摆动，初始轨道能量小于零，模型无法越过势能最高点，反向运动：

5.3 双腿步行，给定目标轨道能量为零，则倒立摆模型在迈出一步以后回到静止位置：

5.4 双腿步行，给定大于零的目标轨道能量，我们通过控制支撑腿的切换时间可以得到结果为：

5.5 双腿步行，给定小于零的目标轨道能量，我们通过控制支撑腿的切换时间可以得到结果为：

代码已在Github开源：

6. 参考文献

1. Shuuji Kajita, Hirohisa Hirukawa et al. Introduction to Humanoid Robots, 2004.
2. Jerry Pratt et al. Capture Point: A Step toward Humanoid Push Recovery, 2007.

《动态行走双足机器人的稳定性分析与控制》