双足机器人ZMP预观控制算法及代码实现

1. 简介

本文的主要内容参考了Kajita等人2003年的论文，Biped Walking Pattern Generation by using Preview control of Zero-Moment Point 以及《仿人机器人》一书，算法的代码已开源，详见文末链接。

把双足机器人简化为一个桌子小车（Cart-Table）模型，会比较方便于处理ZMP问题。本算法提出了一种混合了ZMP和倒立摆模型的预观控制算法，可用于伺服跟踪ZMP的轨迹。基于预观控制算法，就算我们的仿真模型是简单的倒立摆模型，也可以解决更加复杂的双足机器人的动力学控制问题。

2. 双足机器人的动力学模型

2.1 三维线性倒立摆模型

如图所示，考虑在三维笛卡尔空间当中运动的三维线性倒立摆模型（3D-LIPM），该模型的绕其支点 $O$ 转动，质心可以在一个虚拟的约束平面运动内运动。$XY$ 所在的平面为水平地面，$Z$ 轴竖直向上，与地面垂直，并且我们指定 $X$ 的正方向为机器人前进的方向。

约束平面可使用该平面的法向量表示为 $(k_x, k_y, -1)$ ，并且 $Z$ 轴与该平面的交点为 $z_c$ ，则对于约束平面来说：$z = k_x x + k_y y + z_c$ 如果约束平面是水平的，则有$k_x=k_y=0$ ，此时，倒立摆模型的质心在该约束平面内的运动过程由下面的运动方程表示：

其中，$m$ 表示质心的质量，$g$ 表示重力加速度常数，$\tau_x, \tau_y$ 分别表示倒立摆模型绕$X$ 轴和$Y$ 轴旋转的扭矩。即使对于约束平面非水平（$k_x, k_y \neq 0$ ）的坡道步行过程，我们也可以为扭矩添加额外的约束来使得该动力学过程成立：$\tau_x x + \tau_y y = 0$ 对于约束平面水平的情况，零力矩点（ZMP）的计算方式为：

其中，$(p_x, p_y)$ 表示ZMP的位置。将上述公式代入前面的运动方程中可以得到：

2.2 ZMP方程和桌子-小车模型

在前面的章节，我们把ZMP作为三维线性倒立摆的输入，用于计算质心的运动轨迹。从本节开始，为了控制ZMP的轨迹，我们需要把它作为控制系统的输出，因此将上个章节最后的那个公式重写为ZMP方程：

上面这个方程在后文中将被多次提到，后面凡是使用ZMP方程的地方都是指这个方程。

桌子小车模型的示意图如下图所示：

水平地面上的桌子表面有一个沿着桌面运动的小车，尽管桌脚的面积很小，桌子呈现头重脚轻的情况，容易倾倒。但我们可以控制小车在桌面上做加速运动，产生对应的加速度来获得桌子的瞬时平衡，此刻ZMP是在桌子支撑脚的范围内的。图中所示为沿着 $X$ 方向运动的桌子小车模型，在三维环境中，我们还需要一个沿着$Y$ 方向运动的桌子小车模型，情况是相似的，因此，这里我们以$X$ 方向的桌子小车为例来进行分析。

由于我们要求绕ZMP点的力矩必须为零（否则会导致机器人翻倒），因此：

该公式表示小车产生的加速度等效到ZMP点的力矩作用要能抵消小车的质量收到重力作用后在ZMP点处产生的力矩作用，这样才能保证桌子是平衡的。并且，我们可以发现，这个公式与前面的ZMP方程是等价的。

3. 基于ZMP产生步行模式

我们的总体目标是要给定ZMP来求解质心的运动过程，前面的ZMP方程已经给出了ZMP点和质心运动之间的关系，求解这个微分方程可以使用快速傅里叶变换（FFT）在频域内解决，然后再使用FFT的逆变换得到质心的轨迹。这里我们将介绍另一种更加简单的方法。

首先，我们把质心水平加速度的微分定义为 $u_x$ ：$u_x = \frac{d}{dt} \ddot x$ 把$u_x$ 当做ZMP方程的输入，我们可以把ZMP方程写作如下的系统状态方程：

对于 $Y$ 方向来说，我们可以定义$u_y$ 来获取相似的结果。

利用上述公式我们把步态模式生成器构造为一个ZMP跟踪系统，如下图所示：

该系统可以生成COM的运动轨迹，同时ZMP可以跟踪预定义的参考位置。

除此之外，我们还需要考虑另一个问题，如下图所示：

图中展示了在理想情况下，双足向前机器人迈出步长为$30 cm$ 的一步时ZMP和COM的运动轨迹。在第1.5秒的时候进行支撑腿的切换，切换时刻的前后ZMP的参考轨迹都保持不变，在切换时刻支撑腿由其中一条腿换做另一条腿进行支撑运动。在进行支撑腿切换的时刻，ZMP点有一个位置上的突变，COM的运动在此之前已经开始了，如图中Output箭头所指的位置。从前面的控制框图可以看出，我们必须要引入未来的信息来影响当前的状态。这是什么意思？也就是说我们要做提前的规划预测，把“眼光”放长远一点，不能仅仅依靠当前的信息反馈来做判断。这里就需要引入我们的算法了：预观控制算法。

我们先把前面定义的ZMP的系统状态方程离散化，设采样时间为$T$ ：

我们把上述方程称为离散化的系统方程，其中，

以及

给定ZMP的参考位置$p^{ref}(k)$ ，为了使得系统的输出$p(k)$ 尽可能精确地跟踪目标ZMP位置$p^{ref}(k)$ ，我们定义一个评价函数来作为损失函数，从而构造一个优化问题。评价函数为：

其中，$e(i) \equiv p(i) - p^{ref}(i)$ 表示伺服误差，$Q_e>0, R>0$ ，$Q_x$ 表示一个 $3 \times 3$ 的半正定的对称矩阵。$\Delta \mathbf{x}(k) \equiv \mathbf{x}(k) - \mathbf{x}(k-1)$ 表示状态向量的增量，$\Delta u(k) \equiv u(k) - u(k-1)$ 表示输入的增量。在任意时刻$k$ ，满足使得$J$ 最小化的解$u(k)$ 就是我们要的最优解。

在每一个采样时间，如果可以预先知道ZMP的未来$N_L$ 步的参考量，则使得评估指标最小化的最优控制器为：

其中，$G_i, G_x$ 和 $G_p(j)$ 表示基于权重$Q_e, Q_x, R$ 以及离散化的系统方程的系统参数计算得到的增益。因此，预观控制过程由三部分组成：跟踪误差的积分动作、状态反馈、使用未来参考的预动作。

下图所示为采用预动作后的收益：

我们可以看到，控制器并不需要太长未来时间的信息，因为预收益 $G_p$ 在未来 $2$ 秒以后已经变得非常小了。

下图是采用$1.6$ 秒的预观时间周期时的步态模式生成结果，包含向前走三步的COM和ZMP轨迹：

其中，上面的子图为沿着$X$ 轴的矢量平面内运动过程，下面的子图为沿着$Y$ 轴的侧平面内的运动过程。我们可以看到，COM的轨迹非常平滑，实际的ZMP轨迹也能完全跟踪参考的ZMP轨迹。整个步行过程可以分为多个步行周期，在单个步行周期中又可以分为单腿支撑相位和双腿支撑相位。在单腿支撑的步相中，ZMP的参考轨迹要始终落在支撑脚的区域内，在切换支撑腿以后要从旧的支撑脚区域转移到新的支撑脚区域当汇总。为了获得一个平滑的ZMP轨迹，我们使用了Cubic Spline进行轨迹插值。

下图展示的是采用更短预观时间（0.8秒）的COM轨迹预测和ZMP轨迹跟踪结果：

从图中我们可以看到，ZMP的跟踪轨迹不如前面采用更长预观时间的算法结果，这里不能很好地跟踪ZMP的参考轨迹。尽管这样，整个系统还是稳定的，因为有状态反馈作为系统的输入。

以上为Kajit等人在2003年论文中提出的ZMP预观控制算法的基本内容，读者也可参考《仿人机器人》一书中的第4.4章节。

4.参考文献

基础版ZMP预观控制算法：Biped Walking Pattern Generation by using Preview control of Zero-Moment Point

改进版ZMP预观控制算法：General ZMP Preview Control for Bipedal Walking

《仿人机器人》，关注公众号【博士的沙漏】，后台回复【HR】获取下载链接。

5. 代码实现

代码使用Python语言编写，一共实现了3种ZMP预观控制算法，包括最基础款ZMP预观控制、考虑当前时刻ZMP误差的预观控制和考虑历史ZMP误差之和的预观控制，可根据给定的每一步落脚点生成质心和ZMP的运动轨迹。其中，基础款ZMP预观控制算法由于没有考虑ZMP的误差反馈，在实际使用的时候随着前进距离的增加会累积误差，可能会与所规划的ZMP参考点有差异，如下图所示：

而考虑了当前时刻ZMP误差的预观控制算法虽然有所改善，但改善微乎其微，如下图所示：

改进版的ZMP预观控制算法考虑了历史积累的ZMP误差因此其控制效果最好，能完全跟踪参考的ZMP轨迹：

该算法生成的COM和ZMP参考轨迹可用于双足机器人的步行控制过程：

代码开源在Github仓库，地址为：https://github.com/chauby/ZMP_preview_control.git