人形机器人建模与控制（五） - 稳定性与平衡控制

1. 简介

作为线性倒摆模型（LIPM）的步行

线性倒摆模型（LIPM）是步行的最基本模型，假设身体的臀部高度是恒定的，其自然频率仅取决于高度。

在LIPM模型中，假设人体在步行时可以简化为一个线性倒摆（Linear Inverted Pendulum）。这个模型假设身体可以近似看作一个竖直摆动的摆杆，其重心在固定的臀部高度上。LIPM模型中，步行的周期与身体的高度有关，并且只受重力加速度和摆杆长度的影响。

方程中的表示摆杆的摆动加速度，和表示重心在水平和垂直方向的加速度。是重力加速度，是摆杆的长度。方程说明了在LIPM模型中，身体在步行过程中的摆动受到重力和摆杆长度的约束。

2. 零力矩点（Zero Moment Point, ZMP）

零力矩点是指地面反作用力矩为零的点。

$$
\begin{aligned}
\boldsymbol{f}{\varrho} & =\int{\text {sole }} \boldsymbol{f}(\rho) \mathrm{d} \rho \
\boldsymbol{\eta}{\varrho} & =\int{\text {sole }}(\rho-\varrho) \boldsymbol{f}(\rho) \mathrm{d} \rho
\end{aligned}
$$

上述方程中，$\boldsymbol{f}{\varrho}\boldsymbol{\eta}{\varrho}$表示相对于零力矩点的力矩，是对脚底上的力分布与其距离的乘积进行积分得到的结果。通过计算这两个积分可以确定零力矩点的位置。

零力矩点在步行和运动控制中非常重要，因为它是保持身体稳定的关键点。控制步行时，需要确保零力矩点位于支撑脚的位置，以保持平衡。通过控制身体姿势和支撑脚的位置，可以调整零力矩点的位置，从而实现稳定的步行或运动。

$$
\mathrm{p}{\text {Foot }}=\frac{\int{\text {sole }} \rho \boldsymbol{f}(\rho) \mathrm{d} \rho}{\int_{\text {sole }} \boldsymbol{f}(\rho) \mathrm{d} \rho}
$$

上述公式表示脚底的位置（）可以通过对脚底上的力分布与其距离的乘积进行积分，并除以总的作用力来计算。

$$
\begin{aligned}
\text { Foot }{x} & =\frac{\int{\text {sole }} x \boldsymbol{f}(x, y) \boldsymbol{d} \text { sole }}{\int_{\text {sole }} \boldsymbol{f}(x, y) \boldsymbol{d} \text { sole }} \
\text { Foot }{y} & =\frac{\int{\text {sole }} y \boldsymbol{f}(x, y) \boldsymbol{d} \text { sole }}{\int_{\text {sole }} \boldsymbol{f}(x, y) \boldsymbol{d} \text { sole }}
\end{aligned}
$$

上述公式表示脚底的位置坐标（和）可以通过对脚底上的力分布与其坐标的乘积进行积分，并除以总的作用力来计算。

$$
\begin{array}{l}
\boldsymbol{p}{x}=\frac{\boldsymbol{p}{R_{x}} \boldsymbol{f}{R{z}}+\boldsymbol{p}{L{x}} \boldsymbol{f}{L{z}}}{\boldsymbol{f}{R{z}}+\boldsymbol{f}{L{z}}} \
\boldsymbol{p}{y}=\frac{\boldsymbol{p}{R_{y}} \boldsymbol{f}{R{z}}+\boldsymbol{p}{L{y}} \boldsymbol{f}{L{z}}}{\boldsymbol{f}{R{z}}+\boldsymbol{f}{L{z}}}
\end{array}
$$

上述公式表示身体重心的位置坐标（$\boldsymbol{p}x\boldsymbol{p}yxy\boldsymbol{p}{R_x}\boldsymbol{p}{R_y}\boldsymbol{p}{L_x}\boldsymbol{p}{L_y}z\boldsymbol{f}{R_z}\boldsymbol{f}{L_z}$）的加权平均来计算。

支撑多边形（Supporting Polygon, SP）

支撑多边形是包含所有接触点的最小面积凸包。它用于评估人体的稳定性和平衡状态。

ZMP和SP的测量

经典方法：踝关节力矩传感器

$$
\begin{aligned}
\boldsymbol{p}{x} & =\frac{-\boldsymbol{\tau}{y}-\boldsymbol{f}{x} d}{\boldsymbol{f}{z}} \
\boldsymbol{p}{y} & =\frac{\boldsymbol{\tau}{x}-\boldsymbol{f}{y} d}{\boldsymbol{f}{z}} \
\boldsymbol{p} & =\left[\begin{array}{c}
\boldsymbol{p}{x} \
\boldsymbol{p}{y} \
0
\end{array}\right]
\end{aligned}
$$

通过踝关节力矩传感器测量重心位置（和）。其中，和表示踝关节力矩（在轴和轴上的力矩），和表示作用在身体上的力（在轴和轴上的力），表示作用在身体上的力在轴上的分量，表示身体的长度或者摆杆的长度。

通过测量踝关节力矩和力的数值，可以使用上述公式计算出重心位置的坐标。重心位置的坐标可以用来表示重心相对于支撑面的位置，以评估身体的平衡状态。

ZMP的测量问题

1. ZMP是重心在地面的投影吗？

   • 只有在静态条件下。当重心在移动时，如果没有外力作用于上半身，ZMP可以是静态的。ZMP是LIPM模型的支点。

2. ZMP可以在支撑区域外吗？

   • 不能。在静态接触（无滑动）的情况下，ZMP只存在于支撑区域内。

3. ZMP可以在支撑区域的边缘吗？

   • 不能。如果脚在倾斜（只有边缘接触），接触点的力矩不为零（因为身体已经在倾斜）。

ZMP和动量的关系

力矩（）可以由重心位置（）、作用力（）和重心位置关于作用力的力矩（）计算得到：

动量分为线性动量（）和角动量（）：

线性动量表示质点的线性运动量，可以通过质量（）和重力加速度（）以及作用力（）的和来计算：

角动量表示质点的旋转动量，可以通过重心位置（）、质量（）和重力加速度（）以及力矩

（）的叉乘来计算：

重心位置:
$$
\begin{array}{l}
\boldsymbol{p}{x}=\frac{m g \boldsymbol{x}{x}+\boldsymbol{p}{z} \dot{\mathcal{P}}{x}-\dot{\mathcal{L}}{y}}{m g+\dot{\mathcal{P}}{z}} \
\boldsymbol{p}{y}=\frac{m g \boldsymbol{x}{y}+\boldsymbol{p}{z} \dot{\mathcal{P}}{y}-\dot{\mathcal{L}}{x}}{m g+\dot{\mathcal{P}}{z}}
\end{array}
$$

在没有力传感器的情况下估计ZMP

$$
\begin{array}{c}
\mathcal{P}=m \dot{\boldsymbol{x}} \
\mathcal{L}=\boldsymbol{x} \times m \dot{\boldsymbol{x}} \
\boldsymbol{p}{x}=\boldsymbol{x}{x}-\frac{\left(\boldsymbol{x}{z}-\boldsymbol{p}{z}\right) \ddot{\boldsymbol{x}}{x}}{\ddot{\boldsymbol{x}}{z}+g} \
\boldsymbol{p}{y}=\boldsymbol{x}{y}-\frac{\left(\boldsymbol{x}{z}-\boldsymbol{p}{z}\right) \ddot{\boldsymbol{x}}{y}}{\ddot{\boldsymbol{x}}{z}+g}
\end{array}
$$

上述公式描述了在没有力传感器的情况下估计ZMP（零力矩点）的方法。通过估计重心位置和加速度的信息来计算ZMP在不同轴上的位置。

3. 足部旋转指示器（Foot Rotation Indicator, FRI）

FRI是指地面反作用力需要作用的点，以保持脚部不加速。

$$
\begin{aligned}
O F_{x} & =\frac{m_{1} O G_{1 y} g+\sum_{i=2}^{n} m_{i} O G_{i y}\left(a_{i z}+g\right)}{m_{1} g+\sum_{i=2}^{n} m_{i}\left(a_{i z}+g\right)} -\frac{\sum_{i=2}^{n} m_{i} O G_{i z} a_{i y}+\sum_{i=2}^{n} \dot{H}{G i x}}{m{1} g+\sum_{i=2}^{n} m_{i}\left(a_{i z}+g\right)} \
O F_{y} & =\frac{m_{1} O G_{1 x} g+\sum_{i=2}^{n} m_{i} O G_{i x}\left(a_{i z}+g\right)}{m_{1} g+\sum_{i=2}^{n} m_{i}\left(a_{i z}+g\right)} -\frac{\sum_{i=2}^{n} m_{i} O G_{i z} a_{i x}-\sum_{i=2}^{n} \dot{H}{G i y}}{m{1} g+\sum_{i=2}^{n} m_{i}\left(a_{i z}+g\right)}
\end{aligned}
$$

公式中，表示FRI在x轴上的分量，表示FRI在y轴上的分量。代表质量，代表第i个质点的质心相对于惯性坐标系的y轴坐标，代表第i个质点在惯性坐标系中的z轴加速度，代表重力加速度。$\dot{H}{Gi x}\dot{H}{Gi y}$分别代表第i个质点的质心在惯性坐标系中的x轴和y轴动量。

通过计算和的值，可以确定FRI在平面上的位置，从而了解地面反作用力的作用点，以使脚部不发生加速度。

4. 质心力矩支点（Centroidal Moment Pivot, CMP）

质心力矩支点是指通过质心的一条与地面反作用力平行的直线与外部接触面相交的点。

CMP是地面反作用力需要作用的点，以保持全身角动量的水平分量恒定。

$$
\begin{array}{c}
\left(\boldsymbol{r}{CMP}-\boldsymbol{x}\right) \times \boldsymbol{f}{GR}=0 \
\boldsymbol{r}{x}=\boldsymbol{x}{x}-\frac{\boldsymbol{f}{x}}{\boldsymbol{f}{z}} \boldsymbol{x}{z} \
\boldsymbol{r}{y}=\boldsymbol{x}{y}-\frac{\boldsymbol{f}{y}}{\boldsymbol{f}{z}} \boldsymbol{x}{z}
\end{array}
$$

公式中，$\boldsymbol{r}{CMP}\boldsymbol{x}\boldsymbol{f}{GR}$表示地面反作用力的矢量。通过计算CMP的位置，可以确定地面反作用力需要作用的点，以保持全身角动量的水平分量不变。

从零力矩点（ZMP）可以得到CMP的位置：

其中，表示ZMP的位置矢量，表示地面反作用力产生的力矩，表示地面反作用力在z轴上的分量。

捕获点（Capture Point, CP）

捕获点是指机器人在地面上可以迈向的点，以使其完全停止。

$$
\begin{array}{c}
\ddot{\boldsymbol{x}}=\omega^{2}\left(\boldsymbol{x}{p}-\boldsymbol{p}\right) \
\boldsymbol{\omega}=\sqrt{\frac{g}{\boldsymbol{x}{z}}} \
\boldsymbol{\xi}=\boldsymbol{x}{p}+\frac{\dot{\boldsymbol{x}}{p}}{\boldsymbol{\omega}}
\end{array}
$$

公式中，表示机器人的加速度矢量，表示角速度，表示CP的位置矢量，表示机器人的位置矢量，表示重力加速度，表示机器人的高度坐标。第一个公式表示机器人的加速度与CP和机器人当前位置之间的关系。表示角速度，通过重力加速度和机器人高度坐标的比值计算得到。表示CP的位置矢量，通过CP位置和CP位置的导数与角速度之间的比值计算得到。

当代表零力矩点（ZMP）时，我们可以得到以下关系：

这些方程表示捕获点（）的导数与角速度、捕获点与ZMP之间的差值成正比。这意味着捕获点的速度是由角速度和捕获点与ZMP之间的差值决定的。

同样地，机器人的质心位置（）的导数与角速度、捕获点与机器人质心位置之间的差值成正比。这意味着机器人质心位置的速度是由角速度和捕获点与机器人质心位置之间的差值决定的。

通过这些方程，我们可以得出结论：捕获点会从ZMP偏离，而机器人的质心位置会被吸引到捕获点。这种关系确保机器人能够稳定地向捕获点移动，以实现停止动作。

5. 平衡控制

踝关节策略（Ankle Strategy）

踝关节策略（Ankle Strategy）是一种用于控制人体站立和行走的策略。根据给出的方程，我们可以得到以下关系：

加速度$\ddot{\boldsymbol{x}}=\frac{g}{\boldsymbol{x}{z}}(\boldsymbol{x}-\boldsymbol{p})+\frac{\tau}{M \boldsymbol{x}{z}}g\boldsymbol{x}$表示

机器人质心位置，表示零力矩点（ZMP），表示关节力矩，表示机器人的质量，表示机器人质心位置的高度坐标。

力矩，表示关节力矩与机器人质心位置的加速度和重力的关系。

速度指令$\dot{\boldsymbol{x}}{d}=\dot{\boldsymbol{x}}{ref}-\mathbf{K}{x}\left(\boldsymbol{x}{d}-\boldsymbol{x}{ref}\right)+\mathbf{K}{p}\left(\boldsymbol{p}-\boldsymbol{p}{ref}\right)\dot{\boldsymbol{x}}{d}\dot{\boldsymbol{x}}{ref}\boldsymbol{x}{d}\boldsymbol{x}{ref}\mathbf{K}{x}\mathbf{K}{p}\boldsymbol{p}{ref}$表示参考零力矩点。

根据给出的不等式条件，当$0<\mathbf{K}{p}<\omega<\mathbf{K}{x}$时，系统是稳定的。

踝关节策略通过调节关节力矩和质心速度来实现对机器人姿态和平衡的控制。通过调整控制器的增益参数，可以实现稳定的站立和行走。这种策略通常用于仿真和实际机器人控制中，以实现稳定且自然的运动。

踝关节策略（Cart table模型）可以描述为以下方程组：

$$
\left{

\right.
$$

在和附近进行线性化，并设定，我们可以得到：

然后，我们可以定义目标质心（CoM）为：

$$
\begin{array}{c}
\ddot{x}{d}=\frac{g}{z{c}}\left(x_{d}-p_{d}\right) \
\ddot{\theta}=\frac{g z_{c}}{x_{d}^{2}+z_{c}^{2}} \theta+\frac{g}{x_{d}^{2}+z_{c}^{2}} p_{d}
\end{array}
$$

其中，和分别表示目标质心位置和目标零力矩点。这些方程描述了踝关节策略中的目标质心加速度和目标关节加速度的计算方法。

髋关节策略（Hip Strategy）

$$
\begin{array}{c}
\ddot{\boldsymbol{x}}=\frac{g}{\boldsymbol{x}{z}}(\boldsymbol{x}-\boldsymbol{p})+\frac{\tau}{M \boldsymbol{x}{z}} \
\mathbf{I} \dot{\omega}=\tau \
\boldsymbol{p}=-\frac{\boldsymbol{x}{z} \ddot{\boldsymbol{x}}}{g}+\boldsymbol{x}+\frac{\mathbf{I} \dot{\omega}}{g M} \
\boldsymbol{\tau}=\mathbf{K}{\theta}\left(\boldsymbol{\theta}{d}-\boldsymbol{\theta}\right)-\mathbf{K}{\omega} \dot{\omega}
\end{array}
$$

髋关节策略（Hip Strategy）是一种用于控制人体姿态和平衡的策略。通过调节质心加速度和关节力矩来实现对机器人姿态和平衡的控制。通过计算零力矩点和关节力矩，可以实现稳定的姿态和平衡。

N步后的可捕获性（Capturability after N steps）

假设：

• 恒定步长时间T
• 最大步长
• 摆动腿开始时捕获点与ZMP之间的最大距离
• 初始接触点相对于ZMP的距离

$$
\begin{aligned}
\ddot{\boldsymbol{x}} & =\omega^{2}\left(\boldsymbol{x}{p}-\boldsymbol{p}\right) \quad \boldsymbol{\xi}=\boldsymbol{x}{p}+\frac{\dot{\boldsymbol{x}}_{p}}{\boldsymbol{\omega}} \
\dot{\boldsymbol{\xi}} & =\omega(\boldsymbol{\xi}-\boldsymbol{p})
\end{aligned}
$$

解：

对于步长时间：

通过这些方程，我们可以推导出机器人在步长时间后捕获点位置与ZMP位置之间的变化。这对于步态和运动规划非常重要，以确保机器人能够保持稳定并在步行过程中适时调整捕获点位置，以实现稳定的步行运动。

总结

通过本文，我们介绍了步行中的LIPM模型、零力矩点（ZMP）、支撑多边形（SP）、足部旋转指示器（FRI）、质心力矩支点（CMP）和捕获点（CP）等概念，并讨论了踝关节策略（Ankle Strategy）和髋关节策略（Hip Strategy）在平衡控制中的应用。这些概念和策略对于实现稳定、自然的机器人步态控制非常重要。