本文是双足机器人系列的第三篇，在前面的文章中我们介绍了2D线性倒立摆的基本理论，详见：
【双足机器人（1）】线性倒立摆及其运动控制（附代码）
在这篇文章中我们要详细介绍3D线性倒立摆的基本内容，以及使用Python来实现3D线性倒立摆的简单仿真。
话不多说，先上一个最后的仿真视频：
1. 3D-LIPM模型
1.1 模型介绍
三维倒立摆是由一个集中了所有质量的点（质心）和一条无质量的腿组成，如图所示：
支撑点处的力矩为零，可以自由转动。质心在伸缩力
f的作用下可以上下移动，伸缩力
f可以分解为
x,y,z三个方向的分量：
相对于二维的情况增加了第三维（z轴）。其中，
r为支撑点和质心之间的距离。质心只受伸缩李和重力的作用，因而其运动方程为：
其中，
M为质心的质量。类似于二位情况，对于三维倒立摆，定义约束面如下：
z = k_x x + k_y y + z_c
\label{eq: constant plane}
其中，
k_x, k_y决定平面的倾斜，
z_c决定其高度。
为了使质心在约束面上运动，其加速度应该保持与约束面的法向量垂直，即：
\left[f(\frac{x}{r}), \ f(\frac{y}{r}), \ f(\frac{z}{r})-Mg \right]
\left[
\begin{array}{c}
-k_x \\
-k_y \\
1
\end{array}
\right]
=0
从上式中解出
f，然后我们就可以得到：
f = \frac{Mg}{z_c}
因此，我们可以通过控制与腿长
r成正比的伸缩力
f来控制质心在约束面上运动：
我们可以求得质心在水平方向的运动方程如下：
上面两式是线性方程，只有约束面的截距
z_c作为参数。由于约束面的斜率参数
k_x, k_y并没有包含在上述两式中，因此约束面的斜率不影响执行的水平运动，这样的倒立摆称为三维线性倒立摆模型（3D-LIPM）。
1.2 模型的特性介绍
直观上，我们可以把三维线性倒立摆模型看做是在
x和
y方向上的两个二维线性倒立摆的组合，但其运动特性却更有趣。如果把三维线性倒立摆的空间运动轨迹投影到
XY平面上则我们可以得到以约束面的
z轴截距
[0, 0, z_c]为焦点的类似天体运动的轨迹。
1.2.1 开普勒第二定律
既然类比到天体力学，那么首先介绍一下开普勒第二定律：行星运动的面积速度守恒。
什么叫面积速度？面积速度
v_{area}是指单位时间内质心和原点的连接线所扫过的面积，在直角坐标系中可以由下式计算：
v_{area} = \frac{1}{2}(x \dot y - \dot x y)
由此可得三维线性倒立摆的面积速度的时间变化率：
由此可以得出，与行星的运动一样，三维线性倒立摆的面积速度保持恒定（在另一个意义上表示为角动量守恒）。
1.2.2 坐标系变换的影响
为什么要考虑坐标系变换的影响？因为在约束平面内运动的方位有多种情况，要考虑上述的运动过程是否与运动的方位有关。假设我们是从原来的参考坐标系
xy通过旋转
\theta角变换到新的坐标系
x'y'中，如下图所示：
那么，坐标的变幻式为：
其中，
c \equiv cos\theta, s \equiv sin\theta，我们可以得到：
对上面两个式子两边做变换并加和，如下所示：
则变换结果为：
因此，对于任意一个旋转角，这个结论都成立。因此证明了三维线性倒立摆的水平运动在两个正交方向上的分量与参考坐标系的方位无关。
1.2.3 轨迹的几何形状
参考前面的坐标变换图，我们可以根据轨道能量推导轨迹的几何形状。沿新的坐标轴
x'的轨道能量为：
这一能量随转动角
\theta而变化，当坐标系的
x'轴或
y'轴与轨迹的对称轴一致时，
E_x'取得最大值。
然后经过一系列的复杂推导（见原书4.3.2章节），我们可以整理得到三维线性倒立摆在坐标轴与轨迹对称轴一致的坐标系中的轨迹形状方程：
\frac{g}{2 z_c E_x} x^2 + \frac{g}{2 z_c E_y} y^2 + 1 = 0
这是双曲线方程，当沿
x轴方向的轨道能量
E_x和沿
y轴方向的轨道能量
E_y中的一个为负时，另一个为正。
2. 三维步行模式的生成
在二维线性倒立摆的步行模式生成中，我们允许任意的支撑腿切换时间，但是在三维的线性倒立摆中，需要在
x轴和
y轴方向上同时切换支撑腿，因此不能简单的采用与二维线性倒立摆一样的处理办法了。在后面的讨论中，我们假设支撑腿的切换周期固定，并记每一步的支撑时间为
T_{\mathrm{sup}}。
2.1 步行单元
在时间段
[0, T_{\mathrm{sup}}]内，定义三维线性倒立摆的轨迹如下图所示的一段曲线，它是关于
y轴对称的一段双曲线，我们称之为步行单元。
给定
T_{\mathrm{sup}}和
z_c，则步行单元由其终止位置
(\bar x, \bar y)唯一确定。终止速度
(\bar v_x, \bar v_y)可由下面的计算过程得到。
首先，回顾一下线性倒立摆的解析式：
则对于三维线性倒立摆，利用步行单元的对称性，假设沿
x轴的初始条件为
(-\bar x, \bar v_x)，终止位置为
\bar x，则我们有：
\bar x = - \bar x C + T_c \bar v_x S
其中，
T_c = \sqrt{z_c/g}, C \equiv \cosh \frac{T_{\mathrm{sup}}}{T_c}, S \equiv \sinh \frac{T_{\mathrm{sup}}}{T_c}。
可以求得终止速度为：
\bar v_x = \bar x(C+1)/(T_c S)
类似的，对步行单元的
y轴分量，初始条件为
(\bar y, - \bar v_y)，终止位置为
\bar y，终止位置和速度可计算为：
利用步行单元可以很容易生成步行轨迹。例如，对于步长为
2 \bar x的直线行走，只需将相同的步行单元连接起来交替改变其
y轴分量的符号即可。
2.2 步行参数
在大多数情况下，我们需要直接指定脚的落地位置，也就是落脚点。这里，使用步长和步宽来定义步行参数：
其中，
s_x^{(n)}是前进方向上的步长，
s_y^{(n)}是左右方向上的步宽，上标
n表示第
n步。表中的数据称为步行参数，描述了上图中的脚步情况。第
n个计划的落脚位置
(p_x^{(n)},p_y^{(n)})可表示如下：
\left [
\begin{array} \\
p_x^{(n)} \\
p_y^{(n)}
\end{array}
\right ]
=
\left [
\begin{array} \\
p_x^{(n-1)} + s_x^{(n)} \\
p_y^{(n-1)} -(-1)^n s_y^{(n)}
\end{array}
\right ]
\label{eq: next foot location}
其中，
(p_x^{(0)}, p_y^{(0)})表示初始的支撑脚位置，在这里是步行开始时的右脚位置。如果开始时支撑脚是左脚，那么上式中的
-(-1)^n要替换为
+(-1)^n。第
n步的步行参数由第
n+1步的步长和步宽决定，如下：
\left[
\begin{array}\\
\bar x^{(n)} \\
\bar y^{(n)}
\end{array}
\right]
=
\left[
\begin{array}\\
s_x^{(n+1)}/2 \\
(-1)^n s_y^{(n+1)}/2
\end{array}
\right]
\label{eq: bar next foot location}
步行单元的终止速度可计算为：
\left[
\begin{array}\\
\bar v_x^{(n)}\\
\bar v_y^{(n)}
\end{array}
\right]
=
\left[
\begin{array}\\
(C+1)/(T_c S) \bar x^{(n)}\\
(C-1)/(T_c S) \bar y^{(n)}
\end{array}
\right]
\label{eq: v next foot location}
用这种方法确定的步行单元序列在步行开始和结束时是不连续的，连续的和可实现的步行模式的生成方法见下个章节。
2.3 落脚点的调整
在步行周期和落脚时间固定的情况下，可以通过调整落脚点来控制步行的速度，简单来说就是，在较近处落地则速度加大，在较远处落地则速度减小。如下图所示：
为了计算实际的落脚点，我们来分析一下单个步行单元的过程。如下图所示：
我们现在考虑沿
x轴运动的情况，沿
y轴运动的结果类似。假设调整后的落脚位置为
p^*，则相对于地面上的固定坐标系，线性倒立摆的运动方程为：
\ddot x = \frac{g}{z_c}(x - p_x^*)
\label{eq: dynamics 3}
其解析解为：
其中，
x_0^{(n)},\dot x_0^{(n)}为第
n步开始时的初始条件。因此，落脚点
p_x^*与第
n步的最终状态（LIPM解析值）之间的关系如下：
\begin{array} \\
x(t) &= (x_0^{(n)} - p_x^*) \cosh(t/T_c) + T_c \dot x_0^{(n)} \sinh(t/T_c) + p_x^* \\
\dot x(t) &= \frac{x_0^{(n)} - p_x^*}{T_c} \sinh(t/T_c) + \dot x_0^{(n)} \cosh(t/T_c)
\end{array}
\left [
\begin{array} \\
x_f^{(n)} \\
\dot x_f^{(n)} \\
\end{array}
\right ]
=
\left [
\begin{array} \\
C \ \ \ \ T_c S \\
S/T_c \ \ \ \ C \\
\end{array}
\right ]
\left [
\begin{array} \\
x_0^{(n)} \\
\dot x_0^{(n)} \\
\end{array}
\right ]
+
\left [
\begin{array} \\
1-C \\
-S/T_c \\
\end{array}
\right ]
p_x^*
\label{eq: final}
至于最终状态的目标值，可用地面参考坐标系中最终步行单元的终止状态（理论递增值）表示：
\left [
\begin{array} \\
x_d \\
\dot x_d
\end{array}
\right ]
=
\left [
\begin{array} \\
p_x^{(n)} + \bar x^{(n)} \\
\bar v_x^{(n)}
\end{array}
\right]
\label{eq: target state}
为了计算落脚点导致的最终状态与目标状态之间的接近程度，可采用下面的评估函数：
L \equiv a(x_d - x_f^{(n)})^2 + b(\dot x_d - \dot x_f^{(n)})^2
其中，
a,b为取值大于零的加权因子，根据条件
\partial L / \partial p_x^* = 0，可求得使误差评估函数值
L最小的落脚点：
p_x^* = -\frac{a(C-1)}{D}(x_d - C x_0^{(n)} - T_c S \dot x_0^{(n)}) - \frac{bS}{T_c D}(\dot x_d - \frac{S}{T_c}x_0^{(n)}-C \dot x_0^{(n)})
\label{eq: position star}
其中，
D \equiv a(C-1)^2+b(S/T_c)^2
上述算法可以总结为：
- 设定步行周期
T_{\mathrm{sup}}和步行参数
(s_x, s_y)、质心的初始位置
(x,y)以及初始落脚点
(p_x^*, p_y^*)=(p_x^{(0)}, p_y^{(0)})；
- 初始化
T=0, n=0；
- 从时刻
T到
T+T_{\mathrm{sup}}，对线性倒立摆方程式积分；
T=T+T_{\mathrm{sup}}, n=n+1；
- 计算下一个落脚点
(p_x^{(n)}, p_y^{(n)})；
- 计算下一个步行单元
(\bar x^{(n)}, \bar y^{(n)})；
- 计算
x方向上的目标状态
(x_d, \dot x_d)，根据对应的式子计算
y方向上的目标状态
(y_d, \dot y_d)；
- 计算调整落脚点
(p_x^*, p_y^*)的位置（
y方向上也同样计算）；
- 返回第三步，进行下一次迈步。
我们来理解一下这个算法：
- 理论上，如果我们给定了倒立摆的初始状态
(x^{(0)}, \dot x^{(0)}), (y^{(0)}, \dot y^{(0)})和支撑阶段的持续时间
T_{\rm{sup}}，那么我们就可以根据倒立摆的动力学解析方程式计算得到在当前支撑时间结束的时刻
T_{\rm{sup}}处的倒立摆终止状态
(x_f, \dot x_f), (y_f, \dot y_f)，它在支撑阶段结束后就应该是这个状态。但是在支撑阶段结束以后，我们需要的下一步落脚点在哪里呢？这个是一个开放式的问题，还需要额外的第三个量来计算。在二维的情况下，我们可以根据切换以后我们期望的倒立摆的运动状态来计算得到，也就是切换以后下一个倒立摆的支撑周期的轨道能量。在三维的情况下，理论上我们也可以这样做，拆分为
x,y两个方向上的二维倒立摆来计算。
- 实际上，在前面的计算过程中，我们知道当前的脚的位置
(p_x, p_y)，又给定了下一步的步行参数
(s_x, s_y)，这个步行参数是用来指定下一步的落脚点的，即
(p_x + s_x, p_y \pm s_y)，我们希望倒立摆模型能够在本次支撑阶段结束后的切换时刻，另一只脚刚好到达这个落脚点，此时，在这个落脚点处，倒立摆的目标状态
(x_d, \dot x_d), (y_d, \dot y_d)可以反向计算为
(p_x + \bar x, \bar v_x), (p_y + \bar y, \bar v_y)，其中
\bar x=s_x/2, \bar y=\pm s_y/2, \bar v_x, \bar v_y分别可通过前面的公式计算得到。
但由于步行参数
s_x, s_y可能是一个手动指定的结果，如果将下一步的落脚点设置到这个步行参数确定的位置上，此时倒立摆的终止状态不一定与上面第1条使用
T_{\rm{sup}}得到的倒立摆终止状态一致，存在误差。
- 从上面的第1条和第2条我们可以得到，给定倒立摆的初始状态和支撑时间我们可以得到一种倒立摆的终止状态，给定倒立摆初始的步行参数，我们又可以得到另一种倒立摆的终止状态，并且这两种终止状态还不一定是一致的，可能存在较大的误差，因此，我们需要引入一个方法来最小化这个误差：
L \equiv a(x_d - x_f^{(n)})^2 + b(\dot x_d - \dot x_f^{(n)})^2。代入所有的公式，最后可以求得一个能同时平衡
T_\rm{sup}和
(s_x, s_y)的下一步落脚点，这个落脚点才是我们需要的。
当然，如果参考二维线性倒立摆的情况，我们可以结合当前的轨道能量和给定期望的
x,y方向上轨道能量来计算下一步的落脚点
(s_x, s_y)。
需要注意的细节是，我们需要在每次切换支撑脚的时刻就计算好下一步的切换支撑脚的落脚点了，这样，我们才能提前规划好摆动腿在这一步中的摆动过程，并控制摆动腿的关节向前运动到达下一步。如下图所示，我把书中的图拓展了一下，方便理解：
其中，在
T-T_{\mathrm{sup}}时刻，切换支撑腿，此时我们应该计算好下一步的落脚点
p_x^*，从而方便我们进行摆动腿的空间轨迹规划以及控制倒立摆从
T-T_{\mathrm{sup}}时刻到
T时刻的运动过程。
2.4 步行方向的改变
为了改变行走方向，需要在步行参数中追加有关方向的信息。假设每步踏足的方向用
s_{\theta}表示，如下图所示：
则第
n步计划的落脚点
(p_x^{(n)}, p_y^{(n)})由下式确定：
\left[
\begin{array}\\
p_x^{(n)}\\
p_y^{(n)}
\end{array}
\right]
=
\left[
\begin{array}\\
p_x^{(n-1)}\\
p_y^{(n-1)}
\end{array}
\right]
+
\left[
\begin{array}\\
\cos s_{\theta}^{(n)} \ \ \ \ -\sin s_{\theta}^{(n)}\\
\sin s_{\theta}^{(n)} \ \ \ \ \cos s_{\theta}^{(n)}
\end{array}
\right]
\left[
\begin{array}\\
s_x^{(n)}\\
-(-1)^n s_y^{(n)}
\end{array}
\right]
第
n步的步行单元给定如下：
\left[
\begin{array}\\
\bar x^{(n)}\\
\bar y^{(n)}
\end{array}
\right]
=
\left[
\begin{array}\\
\cos s_{\theta}^{(n+1)} \ \ \ \ -\sin s_{\theta}^{(n+1)}\\
\sin s_{\theta}^{(n+1)} \ \ \ \ \cos s_{\theta}^{(n+1)}
\end{array}
\right]
\left[
\begin{array}\\
s_x^{(n+1)}/2\\
-(-1)^n s_y^{(n+1)}/2
\end{array}
\right]
步行单元的速度计算方式：
\left[
\begin{array}\\
\bar v_x^{(n)}\\
\bar v_y^{(n)}
\end{array}
\right]
=
\left[
\begin{array}\\
\cos s_{\theta}^{(n+1)} \ \ \ \ -\sin s_{\theta}^{(n+1)}\\
\sin s_{\theta}^{(n+1)} \ \ \ \ \cos s_{\theta}^{(n+1)}
\end{array}
\right]
\left[
\begin{array}\\
(1+C)/(T_c S) \bar x^{(n)} \\
(C-1)/(T_c S) \bar y^{(n)}
\end{array}
\right]
使用上面的结果分别替换前面没有加入转角的各个计算结果可以得到连续转弯的步行模式。
3.仿真实验结果
向前直线运动，走几步后停止向前行走：
持续左转弯：
持续右转弯：
本文所提到的公式已经使用Python实现，详见：https://github.com/chauby/BipedalWalkingRobots
参考文献
Shuuji Kajita, Hirohisa Hirukawa et al. Introduction to Humanoid Robots, 2004.
往期文章：
【双足机器人（1）】线性倒立摆及其运动控制（附代码
【双足机器人（2）】倒立摆运动学模型构建（附代码）
