L2: Advanced Kinematic and Dynamic Modeling

1. Introduction

在第一篇博客中，我们介绍了人形机器人建模的基础概念，包括空间运动的平移和旋转。本篇博客将深入探讨机器人学的运动学和动力学建模方法，特别是针对人形机器人的具体应用。

2. Kinematic Modeling

运动学方程

对于一个$n$自由度的机器人系统，其从基座到末端执行器的齐次变换矩阵可以表示为：

$$H_n^0=H_1^0\cdot H_2^1\cdot \dots \cdot H_n^{n-1}=\left[\begin{array}{cc}{R}_n^0& t_n^0\\ 0& 1\end{array}\right]\in \mathbf{SE}(3)$$

其中，$R_n^0$是旋转矩阵，$t_n^0$是平移向量，描述了末端执行器相对于基座的位姿。

速度的雅可比矩阵

机器人的末端执行器速度$\dot{\mathbf{x}}$与关节速度$\dot{\mathbf{q}}$之间的关系由雅可比矩阵$\mathbf{J}(\mathbf{q})$表示：

$$\dot{\mathbf{x}}=\left[\begin{array}{c}\mathbf{w}\\ \mathbf{v}\end{array}\right]=\mathbf{J}(\mathbf{q})\dot{\mathbf{q}}\mathbf{J}(\mathbf{q})\in {\mathbb{R}}^{6\times n}$$

其中，$\mathbf{w}$是角速度，$\mathbf{v}$是线速度，$\mathbf{q}$是关节角度。

雅可比矩阵的两种形式

分析雅可比矩阵（Analytical Jacobian）

计算与表示依赖的雅可比矩阵，通过偏微分获得：

$$\dot{\mathcal{X}}=\left[\begin{array}{c}\mathbf{v}\\ \dot{\mathbf{\theta }}\end{array}\right]={\mathbf{J}}_A\dot{\mathbf{q}}$$

几何雅可比矩阵（Geometric Jacobian）

将关节速度映射到线速度和角速度：

$$\dot{\mathbf{X}}=\left[\begin{array}{c}\mathbf{v}\\ \mathbf{\omega }\end{array}\right]={\mathbf{J}}_G\dot{\mathbf{q}}$$

这两者之间存在映射关系：$\mathbf{\omega }=E(\mathbf{\theta })\dot{\mathbf{\theta }}$

差分逆运动学

差分逆运动学通过求解雅可比矩阵的逆矩阵来计算关节角速度，使得末端执行器的速度尽可能接近期望速度：

$$\dot{\mathbf{q}}=\mathbf{J}(\mathbf{q}{)}^{-1}\dot{\mathbf{x}}$$

当雅可比矩阵不可逆时，可以使用伪逆（pseudo-inverse）来求解：

$$\dot{\mathbf{q}}={\mathbf{J}}^{+}(\mathbf{q})\dot{\mathbf{x}}$$

其中，${\mathbf{J}}^{+}(\mathbf{q})$ 是雅可比矩阵的Moore-Penrose伪逆。

3. Dynamic Modeling

动力学基本方程

a) 线性运动

• 动能：$K=\frac{1}{2}m{v}^2$
• 势能：$P=mgh$
• 运动方程：$F=m\ddot{x}$

b) 角运动

• 动能：$K=\frac{1}{2}I{\omega }^2=\frac{1}{2}m{l}^2{\dot{\theta }}^2$
• 运动方程：$m{l}^2\ddot{\theta }=\tau$

c) 弹簧系统

• 势能：$P=\frac{1}{2}k{x}^2$
• 运动方程：$Kx=F$

d) 阻尼系统

• 能量损失：$D=\frac{1}{2}\beta {\dot{x}}^2$
• 运动方程：$\beta \dot{x}=F$

拉格朗日力学

对于具有质心位置${\mathbf{p}}_{\text{com}}$、质量$m$、线速度$\mathbf{v}$和角速度$\omega$的物体，其动能、势能和拉格朗日量分别为：

• 动能：$K=\frac{1}{2}m{\mathbf{v}}^{\top }\mathbf{v}+\frac{1}{2}{\omega }^{\top }\mathbf{\Psi }\omega$
• 势能：$P=m{\mathbf{g}}^{\top }{\mathbf{p}}_{\text{com}}$
• 拉格朗日量：$L=K-P$

系统的总动能和势能：

$$\begin{array}{rl}{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{array}$$

拉格朗日方程：

$$L=K_T-P_T$$

$$\frac{d}{\mathrm{dt}}\left(\frac{\partial L}{\partial {\dot{q}}_i}\right)-\frac{\partial L}{\partial q_i}={\tau }_i$$

系统方程：

$$\begin{array}{rl}\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\\ & ⋮\\ \frac{d}{\mathrm{dt}}\left(\frac{\partial L}{\partial {\dot{q}}_n}\right)-\frac{\partial L}{\partial q_n}={\tau }_n& \end{array}$$

矩阵形式：

$$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)=\tau$$

动力学方程的特性

a) 惯性矩阵 $\mathbf{M}(q)$ 是有界对称正定矩阵：

$$\mathbf{M}(q{)}_{+}=\mathbf{M}(q{)}_{+}^{\top }0<{ϵ}_1\le \Vert \mathbf{M}(q)\Vert \le {ϵ}_2<\infty ,\forall q\in {\mathbb{R}}^n$$

b) 反对称性质：

$$\begin{array}{rl}N& =\dot{M}-2C\\ \text{Skew-symmetric}& N=-N^T\\ & x^{T}Nx=0\end{array}$$

c) 参数的线性关系：

$$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)=\tau =:Y(q,\dot{q},\ddot{q})\Theta$$

功率关系

功率与广义速度之间的关系：

$$\begin{array}{rl}{\tau }^{\top }\dot{\mathbf{q}}& ={\mathbf{f}}^{\top }\dot{\mathbf{x}}\\ \tau & =\mathbf{J}(\mathbf{q}{)}^{\top }\mathbf{f}\end{array}$$

动力学方程包含外力

机器人动力学方程考虑外部力影响：

$$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}+\mathbf{G}(\mathbf{q})=\tau +\mathbf{J}(\mathbf{q}{)}^{\top }{\mathbf{f}}_{\text{ext}}$$

这个方程描述了机器人在外

部力作用下的动力学行为，其中${\mathbf{f}}_{\text{ext}}$是作用在末端执行器上的外部力，通过雅可比矩阵$\mathbf{J}(\mathbf{q})$传递到关节力矩上。

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[\begin{array}{c}{\dot{\mathbf{v}}}_b\\ \ddot{\mathbf{q}}\end{array}\right]\in {\mathbb{R}}^{n+6}\end{array}$$

需要注意的是：
$${\mathbf{x}}_b\ne {\mathbf{v}}_b$$

在浮动基准机器人中：

• $\mathbf{x}$ 表示机器人的状态向量，包括浮动基准（floating base）的位姿 ${\mathbf{x}}_b$ 和广义坐标 $\mathbf{q}$。
• $\nu$ 表示机器人的速度向量，包括浮动基准的速度 ${\mathbf{v}}_b$ 和广义速度 $\dot{\mathbf{q}}$。
• $\dot{\nu }$ 表示机器人的加速度向量，包括浮动基准的加速度 ${\dot{\mathbf{v}}}_b$ 和广义加速度 $\ddot{\mathbf{q}}$。

通过引入浮动基准，这些表示允许我们处理那些具有移动底座和不完全受控（欠驱动）的机器人系统。在浮动基准机器人中，浮动基准的位姿和速度是重要的，因为它们涉及机器人的整体运动和控制。广义坐标和广义速度则描述了机器人的内部自由度和关节运动。

动力学扩展

在浮动基准机器人的动力学建模中，惯性矩阵、科里奥利斯和离心力项以及重力项需要扩展：

$$\begin{array}{rl}\mathbf{G}(\mathbf{q})& \to \mathbf{G}(\mathbf{x})\in {\mathbb{R}}^{n+6}\\ \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})& \to \mathbf{C}(\mathbf{x},\nu )\in {\mathbb{R}}^{(n+6)\times (n+6)}\end{array}$$

$$\mathbf{M}(\mathbf{q})\to \mathbf{M}(\mathbf{x})=\left[\begin{array}{cc}{\mathbf{M}}_x(\mathbf{x})& {\mathbf{M}}_{xq}(\mathbf{x})\\ {\mathbf{M}}_{qx}(\mathbf{x})& {\mathbf{M}}_q(\mathbf{x})\end{array}\right]\in {\mathbb{R}}^{(n+6)\times (n+6)}$$

在上述方程中：

• $\mathbf{G}(\mathbf{q})$ 描述了机器人系统中的重力效应。对于浮动基准机器人，扩展为 $\mathbf{G}(\mathbf{x})$。
• $\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})$ 表示科里奥利斯和离心力项，扩展为 $\mathbf{C}(\mathbf{x},\nu )$。
• $\mathbf{M}(\mathbf{q})$ 表示惯性矩阵，扩展为 $\mathbf{M}(\mathbf{x})$。

扩展的力矩和外部力表示

$$\begin{array}{c}\tau \in {\mathbb{R}}^n\to {\mathbb{R}}^{n+6}\\ \mathbf{Q}=\left[\begin{array}{c}{0}_{6\times n}\\ {\mathbf{I}}_{n\times n}\end{array}\right]\mathbf{Q}\tau \in {\mathbb{R}}^{n+6}\end{array}$$

• $\tau$ 是维度为 ${\mathbb{R}}^n$ 的力矩向量。通过选择矩阵 $\mathbf{Q}$ 扩展到 ${\mathbb{R}}^{n+6}$。

$$\begin{array}{l}\mathbf{J}(\mathbf{q}{)}^{\top }{\mathbf{f}}_{ext}\in {\mathbb{R}}^n\\ \hat{\mathbf{J}}=\left[\begin{array}{c}{\mathbf{J}}_b(\mathbf{q}{)}^{\top }\\ \mathbf{J}(\mathbf{q}{)}^{\top }\end{array}\right]\\ \hat{\mathbf{J}}(\mathbf{x})=\left[{\mathbf{J}}_b\left({\mathbf{x}}_b\right)\mathbf{J}(\mathbf{q})\right]\\ \hat{\mathbf{J}}(\mathbf{x}{)}^{\top }{\mathbf{f}}_{ext}\in {\mathbb{R}}^{n+6}\end{array}$$

• $\mathbf{J}(\mathbf{q}{)}^{\top }{\mathbf{f}}_{\text{ext}}$ 表示外部力通过雅可比矩阵传递到广义坐标上的力。扩展为 $\hat{\mathbf{J}}(\mathbf{x})$。

动力学方程

$$\mathbf{M}(\mathbf{x})\dot{\nu }+\mathbf{C}(\mathbf{x},\nu )\nu +\mathbf{G}(\mathbf{x})=\mathbf{Q}\tau +\hat{\mathbf{J}}(\mathbf{x}{)}^{\top }{\mathbf{f}}_{\text{ext}}$$

这个方程是浮动基准机器人的动力学方程，考虑了惯性、科里奥利斯力、重力、外部力和力矩的影响。

对于具有平板脚的机器人，动力学方程可以表示为：

$$\mathbf{M}\dot{\nu }+\mathbf{C}\nu +\mathbf{G}=\mathbf{Q}\tau +{\hat{\mathbf{J}}}_l^{\top }{\mathbf{f}}_l+{\hat{\mathbf{J}}}_r^{\top }{\mathbf{f}}_r$$

其中，${\mathbf{f}}_l,{\mathbf{f}}_r\in {\mathbb{R}}^6$ 表示左脚和右脚的外部力。

5. Center of Mass Dynamics

在中心质量动力学中，我们引入了一些新的符号和表示：

$$O_b\Vert O_c$$

$O_c$ 是质心所在的坐标系，定义了质心动量矩阵：

$$\mathbf{A}(\mathbf{x})=m{\mathbf{J}}_{com}(\mathbf{x})$$

其中：

• $O_b$ 表示浮动基准的原点。
• $O_c$ 表示质心的位置。
• ${\mathbf{J}}_{com}(\mathbf{x})$ 是质心雅可比矩阵，$m$ 是机器人的总质量。

质心雅可比矩阵定义如下：

$$\begin{array}{c}{}^w{\dot{x}}_c={\mathbf{J}}_c\nu \\ {\mathbf{J}}_c=\frac{{\sum }_{k=0}^{n-1}{m}_k{\mathbf{J}}_{c_k}}{{\sum }_{k=0}^{n-1}{m}_k}\end{array}$$

6. Constraints

在给定地面固定的情况下，考虑极限情况，其中外部力 ${\mathbf{f}}_{ext}$ 和接触约束力 ${\mathbf{f}}_l,{\mathbf{f}}_r$ 趋向于无穷大。

在这种情况下，动力学方程简化为：

$$\mathbf{M}\dot{\nu }+\mathbf{C}\nu +\mathbf{G}=0$$

这表示机器人在极限条件下的平衡关系。

约束力的表达：

$$\mathbf{A}(\mathbf{x})\nu =0$$

约束矩阵 $\mathbf{A}(\mathbf{x})$ 的维度为 ${\mathbb{R}}^{n+6-k}$，其中 $k$ 是约束的自由度数目。

约束力的能量守恒性质：

$$\begin{array}{l}\mathbf{P}={\Gamma }^{\top }\nu =0\\ \Gamma =\mathbf{A}(\mathbf{x}{)}^{\top }\lambda \\ \frac{d}{dt}\frac{\partial L(q,\dot{q})}{\partial \dot{q}}-\frac{\partial L(q,\dot{q})}{\partial q}+\mathbf{A}(q{)}^{\top }\lambda =\tau \\ \mathbf{M}\dot{\nu }+\mathbf{C}\nu +\mathbf{G}+{\mathbf{A}}^{\top }\lambda =\mathbf{Q}\tau \end{array}$$

最终，约束力显式依赖于作用力矩：

$$\begin{array}{l}\mathbf{M}\dot{\nu }+\mathbf{C}\nu +\mathbf{G}+{\mathbf{A}}^{\top }\lambda =\mathbf{Q}\tau \\ \mathbf{A}\nu =0\\ \mathbf{A}\dot{\nu }+\dot{\mathbf{A}}\nu =0\\ \lambda ={\left({\mathbf{AM}}^{-1}{\mathbf{A}}^{\top }\right)}^{-1}\left(\mathbf{A}{\mathbf{M}}^{-1}(\mathbf{Q}\tau -\mathbf{C}\nu -\mathbf{G})+\dot{\mathbf{A}}\nu \right)\end{array}$$

这些方程描述了浮动基准机器人在约束条件下的动力学行为，明确了约束力对作用力矩的依赖性。这对于理解和控制受约束机器人的运动和稳定性非常重要。