人形机器人建模与控制（二） - 高级运动学和动态建模
1. Introduction
在第一篇博客中，我们介绍了人形机器人建模的基础概念，包括空间运动的平移和旋转。本篇博客将深入探讨机器人学的运动学和动力学建模方法，特别是针对人形机器人的具体应用。
2. Kinematic Modeling
运动学方程
对于一个
其中，
速度的雅可比矩阵
机器人的末端执行器速度
其中，
雅可比矩阵的两种形式
分析雅可比矩阵（Analytical Jacobian）
计算与表示依赖的雅可比矩阵，通过偏微分获得：
几何雅可比矩阵（Geometric Jacobian）
将关节速度映射到线速度和角速度：
这两者之间存在映射关系：
差分逆运动学
差分逆运动学通过求解雅可比矩阵的逆矩阵来计算关节角速度，使得末端执行器的速度尽可能接近期望速度：
当雅可比矩阵不可逆时，可以使用伪逆（pseudo-inverse）来求解：
其中，
3. Dynamic Modeling
动力学基本方程
a) 线性运动
- 动能：
- 势能：
- 运动方程：
b) 角运动
- 动能：
- 运动方程：
c) 弹簧系统
- 势能：
- 运动方程：
d) 阻尼系统
- 能量损失：
- 运动方程：
拉格朗日力学
对于具有质心位置
- 动能：
- 势能：
- 拉格朗日量：
系统的总动能和势能：
$$
\begin{aligned}
K_{T} &= \sum_{i=1}^{n} K_{i} = \sum_{i=1}^{n}\left{\frac{1}{2} m_{i} v_{\text{com}{i}}^{2} + \frac{1}{2} I{i} \omega_{\text{com}{i}}^{2}\right} \
P{T} &= \sum_{i=1}^{n} P_{i} = \sum_{i=1}^{n} m_{i} g^{T} t_{0}^{\text{com}_{i}}
\end{aligned}
$$
拉格朗日方程：
$$
\frac{d}{\mathrm{dt}}\left(\frac{\partial L}{\partial \dot{q}{i}}\right) - \frac{\partial L}{\partial q{i}} = \tau_{i}
$$
系统方程：
$$
\begin{aligned}
\frac{d}{\mathrm{dt}}\left(\frac{\partial L}{\partial \dot{q}{1}}\right) - \frac{\partial L}{\partial q{1}} &= \tau_{1} \
\frac{d}{\mathrm{dt}}\left(\frac{\partial L}{\partial \dot{q}{2}}\right) - \frac{\partial L}{\partial q{2}} &= \tau_{2} \
&\vdots \
\frac{d}{\mathrm{dt}}\left(\frac{\partial L}{\partial \dot{q}{n}}\right) - \frac{\partial L}{\partial q{n}} = \tau_{n}
\end{aligned}
$$
矩阵形式：
动力学方程的特性
a) 惯性矩阵
$$
\mathbf{M}(q){+} = \mathbf{M}(q){+}^{\top} \quad 0 < \epsilon_{1} \leq |\mathbf{M}(q)| \leq \epsilon_{2} < \infty, \quad \forall q \in \mathbb{R}^{n}
$$
b) 反对称性质：
c) 参数的线性关系：
功率关系
功率与广义速度之间的关系：
动力学方程包含外力
机器人动力学方程考虑外部力影响：
这个方程描述了机器人在外部力作用下的动力学行为，其中
4. Floating Base (Underactuated) Robots
接下来我们要讨论浮动基准（欠驱动）机器人的动力学建模。这种机器人系统具有移动底座，不完全受控，因此需要特殊的建模方法。
在浮动基准（欠驱动）机器人中，我们引入了一些新的符号和表示：
$$
\begin{array}{l}
\mathbf{x}=\left[\begin{array}{c}
\mathbf{x}{b} \
\mathbf{q}
\end{array}\right] \in \mathbb{R}^{n+6} \
\nu=\left[\begin{array}{c}
\mathbf{v}{b} \
\dot{\mathbf{q}}
\end{array}\right] \in \mathbb{R}^{n+6} \
\dot{\nu}=\left[
\end{array}
$$
需要注意的是：
$$\mathbf{x}{b} \not ={ \mathbf{v}{b}}$$
在浮动基准机器人中：
表示机器人的状态向量，包括浮动基准（floating base）的位姿 和广义坐标 。 表示机器人的速度向量，包括浮动基准的速度 和广义速度 。 表示机器人的加速度向量，包括浮动基准的加速度 和广义加速度 。
通过引入浮动基准，这些表示允许我们处理那些具有移动底座和不完全受控（欠驱动）的机器人系统。在浮动基准机器人中，浮动基准的位姿和速度是重要的，因为它们涉及机器人的整体运动和控制。广义坐标和广义速度则描述了机器人的内部自由度和关节运动。
动力学扩展
在浮动基准机器人的动力学建模中，惯性矩阵、科里奥利斯和离心力项以及重力项需要扩展：
$$
\mathbf{M}(\mathbf{q}) \rightarrow \mathbf{M}(\mathbf{x})=\left[\begin{array}{cc}
\mathbf{M}{x}(\mathbf{x}) & \mathbf{M}{x q}(\mathbf{x}) \
\mathbf{M}{q x}(\mathbf{x}) & \mathbf{M}{q}(\mathbf{x})
\end{array}\right] \in \mathbb{R}^{(n+6) \times(n+6)}
$$
在上述方程中：
描述了机器人系统中的重力效应。对于浮动基准机器人，扩展为 。 表示科里奥利斯和离心力项，扩展为 。 表示惯性矩阵，扩展为 。
扩展的力矩和外部力表示
符号和定义
$$
\begin{array}{c}
\tau \in \mathbb{R}^{n} \rightarrow \mathbb{R}^{n+6} \
\mathbf{Q}=\left[\begin{array}{c}
\mathbf{0}{6 \times n} \
\mathbf{I}{n \times n}
\end{array}\right] \quad \mathbf{Q} \tau \in \mathbb{R}^{n+6}
\end{array}
$$
是维度为 的力矩向量。通过执行器选择矩阵（actuation selector matrix） 扩展到 。 : 这是一个维度为 的力矩向量，表示关节空间中的力矩。 : 这是一个执行器选择矩阵，它的形式为
$$
\mathbf{Q}=\left[\begin{array}{c}
\mathbf{0}{6 \times n} \
\mathbf{I}{n \times n}
\end{array}\right]
$$
这里，$\mathbf{0}{6 \times n}6 \times n \mathbf{I}{n \times n} n \times n \tau \mathbb{R}^{n+6} \mathbf{Q}\tau \in \mathbb{R}^{n+6}$。
力和雅可比矩阵
$$
\begin{array}{l}
\mathbf{J}(\mathbf{q})^{\top} \mathbf{f}{e x t} \in \mathbb{R}^{n} \
\widehat{\mathbf{J}}=\left[\begin{array}{c}
\mathbf{J}{b}(\mathbf{q})^{\top} \
\mathbf{J}(\mathbf{q})^{\top}
\end{array}\right] \
\widehat{\mathbf{J}}(\mathbf{x})=\left[\mathbf{J}{b}\left(\mathbf{x}{b}\right) \ \mathbf{J}(\mathbf{q})\right] \
\widehat{\mathbf{J}}(\mathbf{x})^{\top} \mathbf{f}_{e x t} \in \mathbb{R}^{n+6}
\end{array}
$$
表示外部力通过雅可比矩阵传递到广义坐标上的力。扩展为 。 : 这里， 是与广义坐标 相关的雅可比矩阵。$\mathbf{f}{\text{ext}} \mathbf{J}(\mathbf{q})^{\top} \mathbf{f}{\text{ext}}$，我们可以得到作用在关节空间的力。 : 扩展后的雅可比矩阵。具体形式为
$$
\widehat{\mathbf{J}}=\left[\begin{array}{c}
\mathbf{J}{b}(\mathbf{q})^{\top} \
\mathbf{J}(\mathbf{q})^{\top}
\end{array}\right]
$$
其中，$\mathbf{J}{b}(\mathbf{q})^{\top}\mathbf{J}(\mathbf{q})^{\top} \widehat{\mathbf{J}}(\mathbf{x})$ 就包含了基础和关节的雅可比矩阵信息。
具体计算
- $\widehat{\mathbf{J}}(\mathbf{x})=\left[\mathbf{J}{b}\left(\mathbf{x}{b}\right) \ \mathbf{J}(\mathbf{q})\right]$: 这里，我们考虑
是整个系统的状态向量，它包括了基础和关节的状态。 是一个将基础部分 $\mathbf{J}{b}\left(\mathbf{x}{b}\right) \mathbf{J}(\mathbf{q})$ 结合在一起的雅可比矩阵。 : 扩展后的雅可比矩阵 将外部力 从末端执行器传递到包括基础和关节在内的整个系统中，结果是一个维度为 的向量。
动力学方程
首先是一般形式的动力学方程：
: 系统的质量矩阵（惯性矩阵），它取决于机器人的配置 。 : 广义加速度向量。 : 科里奥利斯力和离心力矩阵，取决于机器人的配置 和速度 。 : 重力项，取决于配置 。 : 关节力矩向量 通过执行器选择矩阵 传递到系统中。 : 外部力 通过扩展雅可比矩阵 传递到系统中。
平板脚机器人的动力学方程
对于具有平板脚的机器人，动力学方程可以具体化为：
$$
\mathbf{M} \dot{\nu} + \mathbf{C} \nu + \mathbf{G} = \mathbf{Q} \tau + \widehat{\mathbf{J}}{l}^{\top} \mathbf{f}{l} + \widehat{\mathbf{J}}{r}^{\top} \mathbf{f}{r}
$$
其中：
$\mathbf{f}{l}, \mathbf{f}{r} \in \mathbb{R}^{6}$: 分别表示作用在左脚和右脚上的外部力。每个外部力都是六维的，表示在 3D 空间中的力和力矩。
$\widehat{\mathbf{J}}{l}, \widehat{\mathbf{J}}{r}$: 分别是左脚和右脚的雅可比矩阵，它们将这些外部力转换到系统的广义坐标空间中。
动力学方程的详细解释
惯性项
: - 这个项表示系统的惯性效应，反映了机器人的加速度如何受到质量分布的影响。
科里奥利斯和离心力项
: - 这个项包含了由机器人的速度
引起的科里奥利斯力和离心力。
- 这个项包含了由机器人的速度
重力项
: - 这个项表示重力对机器人各部分的作用。
执行器力矩项
: - 这个项是由关节执行器产生的力矩，通过选择矩阵
将这些力矩映射到系统的广义坐标中。
- 这个项是由关节执行器产生的力矩，通过选择矩阵
外部力项 $\widehat{\mathbf{J}}{l}^{\top} \mathbf{f}{l} + \widehat{\mathbf{J}}{r}^{\top} \mathbf{f}{r}$:
- 这些项表示左脚和右脚的外部力对系统的影响。通过左脚和右脚的雅可比矩阵 $\widehat{\mathbf{J}}{l}
\widehat{\mathbf{J}}{r}$，将这些外部力映射到广义坐标中。
- 这些项表示左脚和右脚的外部力对系统的影响。通过左脚和右脚的雅可比矩阵 $\widehat{\mathbf{J}}{l}
5. Center of Mass Dynamics
符号定义和解释
: 表示浮动基准的原点。 : 表示质心的位置，也就是机器人的总质心。 : 质心雅可比矩阵，它描述了质心速度与关节速度之间的关系。 : 机器人的总质量。
质心动量矩阵
质心动量矩阵
这里，
质心雅可比矩阵
质心雅可比矩阵
$$
{ }^{w} \dot{x}{c} = \mathbf{J}{c} \nu
$$
这里，${ }^{w} \dot{x}{c}
质心雅可比矩阵的具体形式为：
$$
\mathbf{J}{c} = \frac{\sum{k=0}^{n-1} m_{k} \mathbf{J}{c{k}}}{\sum_{k=0}^{n-1} m_{k}}
$$
其中：
: 机器人的第 个部分的质量。 - $\mathbf{J}{c{k}}$: 第
个部分的质心雅可比矩阵。
这个表达式表示质心雅可比矩阵
质心速度计算
通过上面的定义，我们可以计算质心在世界坐标系中的线速度：
$$
{ }^{w} \dot{x}{c} = \mathbf{J}{c} \nu
$$
质心速度与广义速度
以上方程和定义为描述机器人的质心动力学提供了一个完整的框架。质心动量矩阵
6. Constraints
先来讨论一下约束分类：
6.1 运动学约束分类
运动学约束（kinematic constraints）:
- 运动学约束是用于限制机器人运动的一组方程或不等式。
等式约束（equality）:
- 等式约束定义为
，表示某些变量之间的精确关系。 - 完整约束（holonomic）:
- 这些是仅依赖于广义坐标（位置）的约束。
- 定常约束（scleronomic）:
- 这类约束只依赖于广义坐标
，不随时间变化。表示为 。
- 这类约束只依赖于广义坐标
- 时变约束（rheonomic）:
- 这类约束依赖于广义坐标和时间
，表示为 。
- 这类约束依赖于广义坐标和时间
- 定常约束（scleronomic）:
- 这些是仅依赖于广义坐标（位置）的约束。
- 非完整约束（nonholonomic）:
- 这些约束不仅依赖于广义坐标，还依赖于广义速度
。 - 非完整约束可以表示为
。
- 非完整约束可以表示为
- 这些约束不仅依赖于广义坐标，还依赖于广义速度
- 完整约束（holonomic）:
- 等式约束定义为
不等式约束（inequality）:
- 不等式约束定义为
，表示变量之间的范围或限制关系。
- 不等式约束定义为
运动学约束方程
下面的方程描述了系统中的运动学约束：
: 运动学约束矩阵，依赖于系统状态 。 : 广义速度向量。
运动学约束矩阵的维度
这里：
: 系统的自由度数。 : 约束的数量。
这个表示意味着
6.2 运动学约束方程
极限情况下的动力学方程
当外部力 $\mathbf{f}{ext}
这表示在极限条件下，机器人仅在内在力和力矩的作用下维持平衡。
约束力和约束方程
在有约束力的情况下，约束方程表示为：
这里，
能量守恒和约束力
约束力的能量守恒性质可以表示为：
这里，
拉格朗日力学方程
在拉格朗日力学框架下，约束力引入了一个新的项
这里，
动力学方程包括约束力
将约束力显式地包括在内，动力学方程变为：
约束力显式依赖于作用力矩
最后，通过解约束力
和
结合以上方程，我们得到拉格朗日乘数
这些方程展示了浮动基准机器人在约束条件下的动力学行为，并明确了约束力对作用力矩的依赖性。具体来说：
- 基本动力学方程: 描述了机器人在惯性、科里奥利力和重力作用下的运动。
- 约束方程: 描述了系统的运动学约束。
- 能量守恒和约束力: 结合拉格朗日乘数引入约束力。
- 最终方程: 通过显式表达式描述了约束力
，其依赖于作用力矩、广义速度、惯性矩阵、科里奥利力和重力。
这些方程对理解和控制机器人在复杂约束条件下的行为非常重要，有助于设计更加稳定和高效的机器人控制系统。
- Title: 人形机器人建模与控制（二） - 高级运动学和动态建模
- Author: xiangyu fu
- Created at : 2024-05-26 23:32:03
- Updated at : 2025-12-10 16:10:05
- Link: https://redefine.ohevan.com/2024/05/26/legged_robots/blog-02/
- License: This work is licensed under CC BY-NC-SA 4.0.