1. 引言

腿式机器人在复杂地形中行走的能力使其成为探索、救援和军事等领域的重要研究对象。在本篇博文中，我们将探讨腿式机器人行走周期、行走运动公式、零力矩点（ZMP）和模型预测控制（MPC）等相关理论与实践方法。

2. 行走周期

腿式机器人的行走周期可以分为支撑相和摆动相两个主要阶段。在支撑相中，机器人的一只或多只脚与地面接触，支撑整个身体的重量。在摆动相中，抬起的脚移动到下一个支撑点。

支撑相：支撑相的主要任务是保持平衡和支持身体重量。该阶段要求机器人能稳定地控制重心，使得零力矩点（ZMP）始终保持在支撑多边形内。
摆动相：摆动相的主要任务是将抬起的脚移动到下一个预定的支撑点。这个过程需要考虑行走速度、步幅和避障等因素。

3. 行走运动公式

线性倒立摆模型（Linear Inverted Pendulum Model, LIPM） 是一个广泛用于描述和分析机器人行走运动的简化模型。它假设机器人的质量集中在一个质心上，并且质心通过一个不可伸缩的杆连接到地面。以下是LIPM的详细解释和公式推导。

3.1 动力学方程

LIPM 的主要动力学方程是：

其中：

是质心在水平面上的位置（可以是二维向量）。
是质心的高度，假设恒定不变。
是重力加速度。

这个方程描述了质心在水平面上的加速度与质心位置的关系。

3.2 解方程

该二阶微分方程可以通过超越函数（双曲函数）进行求解，得到质心的位置和速度随时间的变化：

质心位置的解为：

质心速度的解为：

其中：

和分别是初始位置和初始速度。
是时间常数。

3.3 时间常数

时间常数是一个重要参数，定义为：

它反映了系统的动态特性，与质心高度和重力加速度有关。

3.4 相关频率

系统的自然频率和周期可以通过以下公式计算：

自然频率：
自然频率对应的周期：

LIPM 模型提供了一种简化的方法来描述机器人行走时的质心运动，通过假设质心的高度不变，并使用双曲函数来描述质心的位置和速度随时间的变化。这个模型对于分析机器人行走稳定性和控制策略非常有用。

动力学方程：描述质心在水平面上的加速度与质心位置的关系。
位置和速度方程：通过双曲函数描述质心的位置和速度随时间的变化。
时间常数和自然频率：与质心高度和重力加速度有关，反映了系统的动态特性。

通过这些公式和参数，可以有效地分析和设计机器人的行走运动控制系统。

4. LIPM 行走运动

4.1 质心的位置和速度

在LIPM模型中，质心的位置和速度可以通过以下公式计算：

$$
\begin{aligned}
&\text{-} \boldsymbol{x}{1}=\boldsymbol{x}{0}\operatorname{cosh}\left(\frac{\Delta t}{T_{c}}\right)+T_{c}\dot{\boldsymbol{x}}{0}\operatorname{sinh}\left(\frac{\Delta t}{T{c}}\right) \
&\dot{\boldsymbol{x}}{1}=\frac{\boldsymbol{x}{0}}{T_{c}}\operatorname{sinh}\left(\frac{\Delta t}{T_{c}}\right)+\dot{\boldsymbol{x}}{0}\operatorname{cosh}\left(\frac{\Delta t}{T{c}}\right) \
&\cosh\boldsymbol{x}=\frac{e^{\boldsymbol{x}}+e^{-\boldsymbol{x}}}{2}\quad\sinh\boldsymbol{x}=\frac{e^{\boldsymbol{x}}-e^{-\boldsymbol{x}}}{2} \
&\boldsymbol{x}{1}=\frac{\boldsymbol{x}{0}+T_{c}\dot{\boldsymbol{x}}{0}}{2}e^{\frac{\Delta t}{T{c}}}+\frac{\boldsymbol{x}{0}-T{c}\dot{\boldsymbol{x}}{0}}{2}e^{-\frac{\Delta t}{T{c}}} \
&\boldsymbol{\dot{x}}{1}=\frac{\boldsymbol{x}{0}+T_{c}\dot{\boldsymbol{x}}{0}}{2T{c}}e^{\frac{\Delta t}{T_{c}}}-\frac{\boldsymbol{x}{0}-T{c}\dot{\boldsymbol{x}}{0}}{2T{c}}e^{-\frac{\Delta t}{T_{c}}} \
&\boldsymbol{x}{1}+T{c}\boldsymbol{\dot{x}}{1}=\left(\boldsymbol{x}{0}+T_{c}\dot{\boldsymbol{x}}{0}\right)e^{\frac{\Delta t}{T{c}}}& \Delta t=T_{c}\operatorname{ln}\frac{\boldsymbol{x}{0}-T{c}\boldsymbol{\dot{x}}{0}}{\boldsymbol{x}{1}-T_{c}\boldsymbol{\dot{x}}_{1}}
\end{aligned}
$$

这些公式可以用于预测质心在时间间隔内的位置和速度变化。

以下是每个公式的详细解释：

质心位置的变化

$$
\boldsymbol{x}{1} = \boldsymbol{x}{0}\cosh\left(\frac{\Delta t}{T_{c}}\right) + T_{c}\dot{\boldsymbol{x}}{0}\sinh\left(\frac{\Delta t}{T{c}}\right)
$$
这个公式表明，在时间之后，质心的位置 $\boldsymbol{x}{1} $是初始位置$ \boldsymbol{x}{0} $和初始速度$ \dot{\boldsymbol{x}}_{0} $的函数，通过双曲函数$ \cosh $和$ \sinh$ 计算得到。

质心速度的变化

$$
\dot{\boldsymbol{x}}{1} = \frac{\boldsymbol{x}{0}}{T_{c}} \sinh\left(\frac{\Delta t}{T_{c}}\right) + \dot{\boldsymbol{x}}{0}\cosh\left(\frac{\Delta t}{T{c}}\right)
$$
这个公式表示，在时间之后，质心的速度 $\dot{\boldsymbol{x}}{1} $是初始位置$ \boldsymbol{x}{0} $和初始速度$ \dot{\boldsymbol{x}}_{0}$ 的函数，通过相同的双曲函数计算得到。

双曲函数的定义

这两个公式定义了双曲函数和。和分别表示双曲余弦和双曲正弦。

质心位置的等效表达式

$$
\boldsymbol{x}{1} = \frac{\boldsymbol{x}{0} + T_{c}\dot{\boldsymbol{x}}{0}}{2}e^{\frac{\Delta t}{T{c}}} + \frac{\boldsymbol{x}{0} - T{c}\dot{\boldsymbol{x}}{0}}{2}e^{-\frac{\Delta t}{T{c}}}
$$
这个公式是利用双曲函数的指数形式将质心位置重新表示。通过将和用它们的指数形式表示，得到了质心位置的等效表达式。

质心速度的等效表达式

$$
\dot{\boldsymbol{x}}{1} = \frac{\boldsymbol{x}{0} + T_{c}\dot{\boldsymbol{x}}{0}}{2T{c}}e^{\frac{\Delta t}{T_{c}}} - \frac{\boldsymbol{x}{0} - T{c}\dot{\boldsymbol{x}}{0}}{2T{c}}e^{-\frac{\Delta t}{T_{c}}}
$$
这个公式是利用双曲函数的指数形式将质心速度重新表示。通过将和用它们的指数形式表示，得到了质心速度的等效表达式。

合并位置和速度

$$
\boldsymbol{x}{1} + T{c}\dot{\boldsymbol{x}}{1} = \left(\boldsymbol{x}{0} + T_{c}\dot{\boldsymbol{x}}{0}\right)e^{\frac{\Delta t}{T{c}}}
$$
这个公式表示在时间之后，质心的位置和速度的加权和。它展示了如何通过初始位置和速度来计算新的位置和速度的加权和。

时间步长的计算

$$
\Delta t = T_{c} \ln \frac{\boldsymbol{x}{0} - T{c}\dot{\boldsymbol{x}}{0}}{\boldsymbol{x}{1} - T_{c}\dot{\boldsymbol{x}}_{1}}
$$
这个公式给出了时间步长的计算方法。通过已知的初始位置和速度，以及目标位置和速度，可以反推出所需的时间步长。

这些公式是描述线性倒立摆模型中质心位置和速度变化的重要工具。通过双曲函数和其等效的指数形式，这些公式提供了计算机器人质心位置和速度在时间步长之后变化的方法，并可以反推出所需的时间步长。

4.2 能量分析和稳定性分析

接下来我们看一下线性倒立摆模型（LIPM）中的能量分析和稳定性分析。

最大动能与最大势能可以用以下公式表示：
$$
KE_{max} = \frac{1}{2}M\dot{\boldsymbol{x}}{max}^2

PE{max} = Mgz_{max}

KE_{max} = PE_{max}
$$
这些公式表示在系统中，最大动能（）和最大势能（）是相等的，这是能量守恒的体现。

动力学方程与能量守恒方程之间的关系可以用以下公式表示：

这个方程表示质心加速度和位置之间的关系，并引出了能量守恒的关系：

其中，是常数，表示系统的总能量。

通过积分可以得到：
$Missing or unrecognized delimiter for \left \int \left{\ddot{x}\dot{x} - \frac{g}{z}x\dot{x}\right} dt = constant$
这展示了系统的动能（KE）和势能（PE）之间的转换。

能量状态的比较：

这两个公式表示系统在两个不同状态下的能量，和，其中可能是某个特定的距离或位移。

通过能量状态的比较，可以得到质心的位置和速度：
$$
\boldsymbol{x}{f} = \frac{z}{gs}\left(E_2 - E_1\right) + \frac{s}{2}

\boldsymbol{v}{f} = \sqrt{2E_1 + \frac{g}{z}\boldsymbol{x}f^2}
$$
这些公式计算质心在特定状态下的位置 $\boldsymbol{x}{f} $和速度$ \boldsymbol{v}_{f}$。

4.3 3D 线性倒立摆模型

在这个部分，我们研究一个3D线性倒立摆模型，涉及坐标转换和能量表达式的计算。

坐标转换

首先，给出坐标转换的公式。我们从旋转角度出发，将坐标从转换到：

其中和。

能量表达式

给出了一个在新坐标系下的能量表达式：

这个能量表达式由两部分组成：第一部分是位能项，第二部分是动能项。

能量对角度的偏导数

我们需要计算能量对旋转角度的偏导数：

其中：

最小化能量

为了最小化能量，我们需要找到使得。解这个方程，我们得到：

$$
\theta = \left{
$如果如果$
\right.
$$

这些公式和步骤帮助我们理解如何通过调整角度来最小化系统的能量，从而稳定线性倒立摆模型。这些步骤是控制系统和机器人操控中的重要组成部分，有助于设计更稳定和高效的控制策略。

计算能量对角度的偏导数

我们从之前的结果出发：

当时，上述公式变为：

于是我们得出：

条件方程

根据上述推导，我们得到：

这是一个平衡条件，表示系统中的某种守恒关系。

动能和位能表达式

动能和位能的关系如下：

这两条公式表示在 x 和 y 方向上的动能（和）是由系统的能量 ( 和 ) 以及相应方向上的位移 ( 和 ) 决定的。

能量守恒条件

结合上述动能表达式，我们得到一个能量守恒条件：

这表示在系统中 x 和 y 方向的能量分布满足一个约束条件，使得总能量守恒。

摆的运动方程

我们进一步分析摆的运动，使用双曲函数描述其运动轨迹：

$$
\overline{\boldsymbol{x}} = -\overline{\boldsymbol{x}} \cosh\left(\frac{T_{sup}}{T_{c}}\right) + T_{c} \overline{\boldsymbol{v}}{x} \sinh\left(\frac{T{sup}}{T_{c}}\right)
$$

$$
\overline{\boldsymbol{v}}{x} = \frac{\overline{\boldsymbol{x}}\left(\cosh\frac{T{sup}}{T_{c}} + 1\right)}{T_{c} \sinh\frac{T_{sup}}{T_{c}}}
$$

$$
\overline{\boldsymbol{y}} = \overline{\boldsymbol{y}} \cosh\left(\frac{T_{sup}}{T_{c}}\right) - T_{c} \overline{\boldsymbol{v}}{y} \sinh\left(\frac{T{sup}}{T_{c}}\right)
$$

$$
\overline{\boldsymbol{v}}{y} = \frac{\overline{\boldsymbol{y}}\left(\cosh\frac{T{sup}}{T_{c}} - 1\right)}{T_{c} \sinh\frac{T_{sup}}{T_{c}}}
$$

这些方程描述了摆在支撑时间内的运动。使用双曲函数和，我们可以解析摆在时间内的位置和速度变化。这些公式对于分析摆的动态行为非常有用，尤其是在控制和稳定倒立摆的过程中。

5. 行走基本步态的生成

在这一部分中，我们研究行走基本步态的生成过程。通过这些公式和表格，我们可以计算机器人在每一步中的位置和速度。

位置更新公式

机器人的当前位置通过上一步的位置和步长来确定：

$$

这个公式表明，每一步的横向步长是直接累加到上一步的横向位置，而纵向步长则交替加减。

步长表格

下面的表格给出了前五步的步长：

这表格表示，在第1步，横向步长为0，纵向步长为0.2；在第2步到第4步，横向步长为0.3，纵向步长为0.2；在第5步，横向步长为0，纵向步长仍为0.2。

中间位置的计算

机器人的中间位置由下一步的步长的一半来确定：

$$

这表明中间位置取决于下一步的步长的一半，并且纵向步长根据当前步数的奇偶性来决定符号。

中间速度的计算

中间速度则通过中间位置来确定：

$$

这里和分别为：

这个公式表明，中间速度与中间位置成正比，比例系数由双曲函数和决定。

位置更新公式

机器人的当前位置通过上一步的位置和步长以及步态旋转角度来确定：

这个公式表明，每一步的步长在旋转角度的影响下进行位置更新。

步长和旋转角度表格

步长和旋转角度的表格如下：

这个表格显示了每一步的步长和旋转角度。

中间位置的计算

机器人的中间位置由下一步的步长和旋转角度的一半来确定：

中间速度的计算

中间速度则通过中间位置和旋转角度来确定：

$$
\left[\begin{matrix}
\bar{v}{x}^{(n)}\
\bar{v}{y}^{(n)}
\end{matrix}\right] =
\left[\right]
\left[\right]
$$

其中，和分别为：

使用4阶多项式生成初始和最终位置及速度

为了设定初始和最终位置及速度，我们使用4阶多项式（样条曲线）：

这个多项式用于规划机器人从初始位置到最终位置的平滑过渡。

5. 基于ZMP的行走运动

零力矩点 (ZMP) 的概念

ZMP（Zero Moment Point，零力矩点）是机器人行走控制中的一个关键概念，它代表了机器人脚底的一个点，在该点处地面反作用力的水平力矩为零。ZMP 用于确保机器人在行走时的稳定性。

位置和加速度关系

ZMP 的位置和质心的关系如下：

通过重排公式，我们得到加速度的表达式：

这里，是质心高度，是重力加速度。

离散时间模型

对于离散时间模型，加速度可以表示为：

$$
\ddot{x}{i} = \frac{x{i-1} - 2x_{i} + x_{i+1}}{\Delta t^{2}}
$$

这里，表示时间时刻的质心位置。

ZMP位置的计算

ZMP 位置可以表示为：

其中系数、、定义如下：

这里是质心的高度。

矩阵形式的ZMP公式

我们可以将以上关系用矩阵形式表示：

其中，和是修正后的ZMP位置，考虑了初始和最终速度的影响：

迭代求解

最后，我们可以通过迭代法求解：

设定初始ZMP位置和期望的ZMP位置。
计算误差：

更新质心位置：

通过这种方法，可以不断调整质心位置以逼近期望的ZMP轨迹，确保机器人在行走过程中的稳定性。

6. 基于模型预测控制（MPC）的行走

模型预测控制（MPC）是一种先进的控制策略，它通过使用系统的数学模型来预测未来的行为并优化当前的控制输入。以下解释了MPC在机器人行走控制中的应用。

状态方程与输出方程

给定系统的状态方程和输出方程如下：

$$
\left{
\begin{array}{l}
\boldsymbol{x}{k+1} = \boldsymbol{A} \boldsymbol{x}{k} + \boldsymbol{b} u_{k} \
p_{k} = \boldsymbol{c} \boldsymbol{x}_{k}
\end{array}
\right.
$$

这里，$\boldsymbol{x}{k} $是系统的状态向量，$ u{k} $是控制输入，$ p_{k}$ 是系统的输出（即ZMP位置）。状态向量包含位置、速度和加速度：

控制输入和输出分别为：

状态矩阵、控制矩阵和输出矩阵定义如下：

目标函数

为了实现对参考轨迹的跟踪，MPC通过最小化以下目标函数来确定最佳控制输入：

$Missing or unrecognized delimiter for \left J = \sum_{j=1}^\infty \left{Q(p_j^{ref} - p_j)^2 + R u_j^2\right}$

这里，和是正权重，分别用于平衡轨迹跟踪误差和控制输入的大小。

控制律

MPC控制律可以表示为：

$$
u_{k} = -\boldsymbol{K} \boldsymbol{x}{k} + \left[ f{1}, f_{2}, \cdots, f_{N} \right] \left[

\right]
$$

其中，反馈增益矩阵和前馈增益向量定义如下：

矩阵是通过求解离散李雅普诺夫方程（DLE）得到的：

实现与计算

这些公式用于计算控制输入，使得系统在最小化性能指标的同时，实现对参考输出的跟踪。通过使用LQR设计问题的解决方案，可以得到矩阵和，例如在Matlab中可以使用 dlqr 函数。

基于模型预测控制的行走方法，通过利用系统的数学模型和优化算法来预测未来的行为并确定最优的控制输入。这种方法可以有效地实现对参考轨迹的跟踪，并在复杂环境中确保机器人的稳定行走。

7. 基于DCM的行走运动

在本部分中，我们将探讨如何使用基于模型预测控制（MPC）的方法进行行走运动控制，特别是围绕零力矩点（ZMP）和动态中心点（DCM）的控制。

状态方程与输出方程

首先给出系统的状态方程和输出方程：

$$
\left{
\begin{array}{l}
\boldsymbol{x}{k+1} = \boldsymbol{A} \boldsymbol{x}{k} + \boldsymbol{b} u_{k} \
p_{k} = \boldsymbol{c} \boldsymbol{x}_{k}
\end{array}
\right.
$$

状态向量包含位置、速度和加速度：

控制输入和输出分别为：

状态矩阵、控制矩阵和输出矩阵定义如下：

目标函数

为了实现对参考轨迹的跟踪，MPC通过最小化以下目标函数来确定最佳控制输入：

$Missing or unrecognized delimiter for \left J = \sum_{j=1}^\infty \left{Q(p_j^{ref} - p_j)^2 + R u_j^2\right}$

这里，和是正权重，分别用于平衡轨迹跟踪误差和控制输入的大小。

控制律

MPC控制律可以表示为：

$$
u_{k} = -\boldsymbol{K} \boldsymbol{x}{k} + \left[ f{1}, f_{2}, \cdots, f_{N} \right] \left[

\right]
$$

其中，反馈增益矩阵和前馈增益向量定义如下：

矩阵是通过求解离散李雅普诺夫方程（DLE）得到的：

DCM/ZMP控制

定义动态中心点（DCM）：

并且有以下关系：

步骤1：计划步态

建议至少规划三个步态：

步骤2：定义虚拟排斥点（VRP）

VRP位置为：

步骤3：定义DCM的路径点

DCM路径点为：

$$
\boldsymbol{\xi}{i} = \boldsymbol{r}{i+1} + e^{\omega t_{step}} (\boldsymbol{\xi}{i+1} + \boldsymbol{r}{i+1})
$$

其中：

步骤4：生成质心（CoM）轨迹

质心轨迹为：

$$
\boldsymbol{\xi}_r = \boldsymbol{r}i + e^{\omega (t - t{step})} (\boldsymbol{\xi}_i + \boldsymbol{r}_i)
$$

并有：

步骤5：DCM/ZMP平衡控制

平衡控制方程为：

$$
\dot{\boldsymbol{\xi}} - \dot{\boldsymbol{\xi}}r = -k\xi (\boldsymbol{\xi} - \boldsymbol{\xi}_r)
$$

期望ZMP位置为：

$$
\mathbf{p}{d} = \mathbf{p}{r} - \left(1 + \frac{k_{\xi}}{\omega}\right)(\boldsymbol{\xi}_{r} - \boldsymbol{\xi})
$$

期望ZMP速度为：

最终期望质心速度为：

通过模型预测控制（MPC），可以有效地规划和控制机器人的行走运动。基于零力矩点（ZMP）和动态中心点（DCM）的控制方法，通过状态预测和优化控制输入，实现对参考轨迹的精确跟踪，从而确保机器人在行走中的稳定性和灵活性。

8. 足迹轨迹生成

足迹轨迹生成是机器人行走过程中一个重要的步骤，通过规划足部的运动轨迹来确保机器人能够稳定地进行步态转换。

步态时间

步态时间由单支撑相（SS）和双支撑相（DS）的时间之和决定：

足部初始和终止条件

为了生成足部的轨迹，我们首先定义足部在步态开始和结束时的条件：

$$
\begin{array}{l}
\boldsymbol{x}{\text{foot}}(0) = \boldsymbol{r}{i-1} \
\dot{\boldsymbol{x}}{\text{foot}}(0) = 0 \
\ddot{\boldsymbol{x}}{\text{foot}}(0) = 0 \
\boldsymbol{x}{\text{foot}}(t{ss}) = \boldsymbol{r}{i+1} \
\dot{\boldsymbol{x}}{\text{foot}}(t_{ss}) = 0 \
\ddot{\boldsymbol{x}}{\text{foot}}(t{ss}) = 0
\end{array}
$$

这些条件表明，足部在单支撑相的开始和结束时分别位于上一步的位置 $\boldsymbol{r}{i-1} $和下一步的位置$ \boldsymbol{r}{i+1}$，并且在这两个时间点的速度和加速度都为零。

中间位置

在单支撑相的中间点（即时间时），足部的位置如下：

$$
\boldsymbol{x}\left(\frac{t_{ss}}{2}\right) = \boldsymbol{r}{i-1} + \frac{1}{2} (\boldsymbol{r}{i+1} - \boldsymbol{r}{i-1}) + \left[
\begin{array}{c}
0 \
0 \
z{\text{step}}
\end{array}
\right]
$$

这表明在单支撑相的中间点，足部的位置在上一步和下一步位置的中点上，并且抬升高度为。

足部轨迹生成方法

为了生成平滑的足部轨迹，我们通常使用多项式插值或样条曲线。给定上述边界条件，可以使用五次多项式来描述足部轨迹：

通过求解这个多项式的系数，使其满足初始和终止条件，可以得到足部轨迹。

足迹轨迹生成是确保机器人行走过程中足部运动平滑且稳定的关键步骤。通过设定足部在步态开始、中间和结束时的位置、速度和加速度条件，可以利用多项式插值方法生成符合要求的足部轨迹。这些轨迹对于实现平稳的机器人行走至关重要。

xiangyu fu's blog

人形机器人建模与控制（六） - 行走控制

1. 引言

2. 行走周期

3. 行走运动公式

3.1 动力学方程

3.2 解方程

3.3 时间常数

3.4 相关频率

4. LIPM 行走运动

4.1 质心的位置和速度

质心位置的变化

质心速度的变化

双曲函数的定义

质心位置的等效表达式

质心速度的等效表达式

合并位置和速度

时间步长的计算

4.2 能量分析和稳定性分析

4.3 3D 线性倒立摆模型

坐标转换

能量表达式

能量对角度的偏导数

最小化能量

计算能量对角度的偏导数

条件方程

动能和位能表达式

能量守恒条件

摆的运动方程

5. 行走基本步态的生成

位置更新公式

$$

步长表格

中间位置的计算

$$

中间速度的计算

$$

位置更新公式

步长和旋转角度表格

中间位置的计算

中间速度的计算

使用4阶多项式生成初始和最终位置及速度

5. 基于ZMP的行走运动

零力矩点 (ZMP) 的概念

位置和加速度关系

离散时间模型

ZMP位置的计算

矩阵形式的ZMP公式

迭代求解

6. 基于模型预测控制（MPC）的行走

状态方程与输出方程

目标函数

控制律

实现与计算

7. 基于DCM的行走运动

状态方程与输出方程

目标函数

控制律

DCM/ZMP控制

步骤1：计划步态

步骤2：定义虚拟排斥点（VRP）

步骤3：定义DCM的路径点

步骤4：生成质心（CoM）轨迹

步骤5：DCM/ZMP平衡控制

8. 足迹轨迹生成

步态时间

足部初始和终止条件

中间位置

足部轨迹生成方法