现代机器人系统，尤其是腿式机器人和人形机器人，其控制的核心挑战在于处理复杂的动力学约束。与传统的固定基座机械臂不同，这些系统的基座不受外部固定，而是通过与环境的接触来实现运动和平衡。本章将深入探讨浮动基座机器人的运动学与动力学建模，为后续的规划和控制算法奠定理论基础。
本章的学习目标包括：
从固定基座到浮动基座的转变带来了本质性的复杂度提升。固定基座机器人的运动完全由关节角度决定，而浮动基座机器人需要同时考虑基座的位姿和关节配置。这种耦合关系使得系统呈现出高度非线性和欠驱动特性，要求我们采用更加精细的建模和控制方法。
浮动基座机器人的配置可以分解为两部分：基座的位姿和关节角度。设机器人具有 $n$ 个自由度的关节，则系统的总自由度为 $6 + n$。我们定义广义坐标为：
\[\mathbf{q} = \begin{bmatrix} \mathbf{p}_b \\ \boldsymbol{\phi}_b \\ \mathbf{q}_j \end{bmatrix} \in \mathbb{R}^{6+n}\]其中 $\mathbf{p}_b \in \mathbb{R}^3$ 是基座位置，$\boldsymbol{\phi}_b \in \mathbb{R}^3$ 是基座姿态的最小表示（如欧拉角或轴角），$\mathbf{q}_j \in \mathbb{R}^n$ 是关节角度向量。
基座的运动更自然地在特殊欧几里得群 SE(3) 上描述。基座位姿可表示为：
\[\mathbf{T}_b = \begin{bmatrix} \mathbf{R}_b & \mathbf{p}_b \\ \mathbf{0} & 1 \end{bmatrix} \in \text{SE}(3)\]广义速度定义为：
\[\mathbf{v} = \begin{bmatrix} \mathbf{v}_b \\ \boldsymbol{\omega}_b \\ \dot{\mathbf{q}}_j \end{bmatrix} \in \mathbb{R}^{6+n}\]其中 $\mathbf{v}_b$ 和 $\boldsymbol{\omega}_b$ 分别是基座的线速度和角速度。注意广义速度的维数与广义坐标相同，但它们之间的关系是非线性的：
\[\dot{\mathbf{q}} = \mathbf{S}(\mathbf{q}) \mathbf{v}\]其中 $\mathbf{S}(\mathbf{q})$ 是依赖于姿态参数化的变换矩阵。
世界坐标系
|
v
┌─────────┐
│ 基座 │ <- 6 DOF (x,y,z,roll,pitch,yaw)
└────┬────┘
│
┌────┴────┐
│ 关节1 │ <- 关节自由度
└────┬────┘
│
...
│
┌────┴────┐
│ 末端链节│
└─────────┘
浮动基座系统的质心动力学遵循牛顿-欧拉定律。系统总质量为 $m$，质心位置 $\mathbf{p}_{CoM}$ 的动力学方程为：
\[m \ddot{\mathbf{p}}_{CoM} = \sum_{i} \mathbf{f}_i + m\mathbf{g}\]其中 $\mathbf{f}_i$ 是作用在接触点 $i$ 的外力，$\mathbf{g}$ 是重力加速度。
系统的角动量 $\mathbf{L}$ 相对于质心的变化率为：
\[\dot{\mathbf{L}} = \sum_{i} (\mathbf{p}_i - \mathbf{p}_{CoM}) \times \mathbf{f}_i\]这两个方程构成了质心动力学的基础，在全身控制中起到关键约束作用。
零力矩点（ZMP） 是地面反力的等效作用点，其水平方向力矩为零：
\[\mathbf{p}_{ZMP} = \frac{\sum_i \mathbf{p}_i^{xy} f_i^z}{\sum_i f_i^z}\]其中上标 $xy$ 表示水平分量，$z$ 表示垂直分量。ZMP必须位于支撑多边形内以保证静态稳定性。
质心投影（CMP） 定义为质心在地面的垂直投影点。动态行走中，CMP可以暂时离开支撑域，但需要通过角动量调节来恢复平衡。CMP与ZMP的关系为：
\[\mathbf{p}_{ZMP} = \mathbf{p}_{CMP} - \frac{z_{CoM}}{m g} \ddot{\mathbf{p}}_{CoM}^{xy}\]这个关系表明，在动态运动中，ZMP偏离CMP的程度取决于质心的水平加速度。
浮动基座系统的拉格朗日量定义为动能减去势能：
\[\mathcal{L} = T - V = \frac{1}{2} \mathbf{v}^T \mathbf{M}(\mathbf{q}) \mathbf{v} - V(\mathbf{q})\]其中 $\mathbf{M}(\mathbf{q}) \in \mathbb{R}^{(6+n) \times (6+n)}$ 是广义质量矩阵，具有以下分块结构：
\[\mathbf{M}(\mathbf{q}) = \begin{bmatrix} \mathbf{M}_{bb} & \mathbf{M}_{bj} \\ \mathbf{M}_{bj}^T & \mathbf{M}_{jj} \end{bmatrix}\]应用欧拉-拉格朗日方程，得到系统动力学方程：
\[\mathbf{M}(\mathbf{q})\dot{\mathbf{v}} + \mathbf{C}(\mathbf{q}, \mathbf{v})\mathbf{v} + \mathbf{g}(\mathbf{q}) = \mathbf{S}^T \boldsymbol{\tau} + \mathbf{J}_c^T \mathbf{f}_c\]其中：
质量矩阵 $\mathbf{M}(\mathbf{q})$ 具有以下重要性质：
这些性质在控制器设计和稳定性分析中至关重要。
机器人系统中的约束可以分为几类，每类都有其特定的数学表达和物理含义。
完整约束（Holonomic Constraints） 可以表示为配置空间中的代数方程：
\[\mathbf{h}(\mathbf{q}) = \mathbf{0}\]例如，闭链机构中的环约束、两个链接之间的固定距离约束等。完整约束减少了系统的配置空间维数。
非完整约束（Nonholonomic Constraints） 涉及速度，不能积分为位置约束：
\[\mathbf{A}(\mathbf{q})\mathbf{v} = \mathbf{0}\]典型例子是轮式机器人的无侧滑约束。非完整约束限制了瞬时运动方向，但不减少可达配置空间。
单边约束（Unilateral Constraints） 表示为不等式：
\[\mathbf{c}(\mathbf{q}) \geq \mathbf{0}\]接触约束是最常见的单边约束：物体不能穿透地面，但可以离开地面。对于接触点 $i$，其高度约束为：
\[c_i(\mathbf{q}) = z_i(\mathbf{q}) - z_{ground} \geq 0\]双边约束（Bilateral Constraints） 始终保持激活：
\[\mathbf{c}(\mathbf{q}) = \mathbf{0}\]例如，机械臂末端固定在某个位置时的约束。
约束的速度级和加速度级表达通过微分得到：
\[\mathbf{J}_c \mathbf{v} + \dot{\mathbf{J}}_c \mathbf{v} = \mathbf{0}\]其中约束雅可比矩阵定义为：
\[\mathbf{J}_c = \frac{\partial \mathbf{c}}{\partial \mathbf{q}}\]刚体接触假设 是机器人动力学中的基本假设：
库仑摩擦模型 描述了接触点的摩擦特性。对于接触点 $i$，摩擦锥约束为：
\[\|\mathbf{f}_{t,i}\| \leq \mu_i f_{n,i}\]其中 $\mathbf{f}{t,i}$ 是切向力，$f{n,i}$ 是法向力，$\mu_i$ 是摩擦系数。
在三维空间中，摩擦锥可以线性化为多面锥：
\[\mathbf{f}_i \in \mathcal{K}_i = \{\mathbf{f} : \mathbf{A}_i \mathbf{f} \leq \mathbf{0}\}\]典型的4面锥近似使用4个摩擦边界：
法向力 f_n
↑
│
╱─────┼─────╲
╱ │ ╲ <- 摩擦锥
╱ │ ╲
╱ μf_n │ μf_n ╲
╱─────────┴─────────╲
切向力 f_t
接触力的互补性条件 是接触动力学的核心，表达了接触状态的离散性：
\[0 \leq c_i \perp f_{n,i} \geq 0\]这意味着：
对于滑动摩擦，互补性条件扩展为：
\[\begin{cases} \|\mathbf{f}_{t,i}\| < \mu_i f_{n,i} &\Rightarrow \mathbf{v}_{t,i} = \mathbf{0} \text{ (粘滞)} \\ \|\mathbf{f}_{t,i}\| = \mu_i f_{n,i} &\Rightarrow \exists \lambda \geq 0, \mathbf{v}_{t,i} = -\lambda \mathbf{f}_{t,i} \text{ (滑动)} \end{cases}\]拉格朗日乘子法 将约束力作为拉格朗日乘子引入动力学方程：
\[\mathbf{M}\dot{\mathbf{v}} + \mathbf{C}\mathbf{v} + \mathbf{g} = \mathbf{S}^T\boldsymbol{\tau} + \mathbf{J}_c^T \boldsymbol{\lambda}\]配合加速度级约束：
\[\mathbf{J}_c \dot{\mathbf{v}} + \dot{\mathbf{J}}_c \mathbf{v} = \mathbf{0}\]联立求解得到：
\[\begin{bmatrix} \mathbf{M} & -\mathbf{J}_c^T \\ \mathbf{J}_c & \mathbf{0} \end{bmatrix} \begin{bmatrix} \dot{\mathbf{v}} \\ \boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \mathbf{S}^T\boldsymbol{\tau} - \mathbf{C}\mathbf{v} - \mathbf{g} \\ -\dot{\mathbf{J}}_c \mathbf{v} \end{bmatrix}\]这是一个混合线性互补问题（MLCP），可用Lemke算法或内点法求解。
投影动力学方法 将动力学投影到约束允许的子空间：
\[\mathbf{P} = \mathbf{I} - \mathbf{J}_c^T(\mathbf{J}_c\mathbf{M}^{-1}\mathbf{J}_c^T)^{-1}\mathbf{J}_c\mathbf{M}^{-1}\]投影后的动力学为：
\[\mathbf{P}\mathbf{M}\dot{\mathbf{v}} = \mathbf{P}(\mathbf{S}^T\boldsymbol{\tau} - \mathbf{C}\mathbf{v} - \mathbf{g})\]这种方法避免了显式计算约束力，适用于运动规划。
接触力优化分配 当系统过约束时（如多点接触），接触力不唯一。通常通过优化问题求解：
\[\begin{align} \min_{\mathbf{f}_c} \quad & \|\mathbf{f}_c\|^2_{\mathbf{W}} \\ \text{s.t.} \quad & \mathbf{M}\dot{\mathbf{v}} + \mathbf{C}\mathbf{v} + \mathbf{g} = \mathbf{S}^T\boldsymbol{\tau} + \mathbf{J}_c^T \mathbf{f}_c \\ & \mathbf{f}_i \in \mathcal{K}_i, \quad \forall i \in \text{接触点} \end{align}\]其中 $\mathbf{W}$ 是权重矩阵，用于调节不同接触点的力分配偏好。这是一个二次规划（QP）问题，可实时求解。
递归算法利用树形结构的特性，将计算复杂度从 $O(n^3)$ 降至 $O(n)$，是实时控制的关键技术。
递归牛顿-欧拉算法（RNEA） 是最经典的逆向动力学算法，分为三个阶段：
第一阶段：正向运动学递归（从基座到末端）
对于 i = 0 到 n-1:
计算链接 i+1 的速度和加速度：
v[i+1] = v[i] + ω[i] × r[i,i+1] + q̇[i+1]·z[i+1]
ω[i+1] = ω[i] + q̇[i+1]·axis[i+1]
a[i+1] = a[i] + α[i] × r[i,i+1] + ω[i] × (ω[i] × r[i,i+1]) + q̈[i+1]·z[i+1]
α[i+1] = α[i] + q̈[i+1]·axis[i+1] + ω[i] × (q̇[i+1]·axis[i+1])
第二阶段：力/力矩计算（每个链接）
对于每个链接 i:
F[i] = m[i]·(a[i] - g) // 惯性力
N[i] = I[i]·α[i] + ω[i] × (I[i]·ω[i]) // 惯性力矩
第三阶段：反向力递归（从末端到基座）
对于 i = n 到 1:
f[i-1] = f[i] + F[i] // 链接间传递的力
n[i-1] = n[i] + N[i] + r[i-1,i] × f[i] // 链接间传递的力矩
τ[i] = n[i-1]ᵀ·axis[i] // 关节力矩
RNEA算法的计算复杂度为 $O(n)$，适合实时控制。对于浮动基座系统，需要特殊处理基座的6个虚拟关节。
正向动力学求解加速度：给定 $\mathbf{q}$，$\mathbf{v}$，$\boldsymbol{\tau}$，求 $\dot{\mathbf{v}}$。
铰接体算法（ABA） 是Featherstone提出的高效算法，核心思想是利用铰接体惯量（articulated-body inertia）的递归关系：
\[\mathbf{I}_i^A = \mathbf{I}_i + \sum_{j \in children(i)} \mathbf{I}_j^{Ac}\]其中 $\mathbf{I}_j^{Ac}$ 是子链接 $j$ 对父链接 $i$ 的等效惯量。
ABA算法步骤：
第一遍：计算偏置力和铰接体惯量（从末端到基座）
对于 i = n 到 1:
计算铰接体惯量 I^A[i]
计算偏置力 p[i] = v[i] × I^A[i]·v[i]
传播到父节点
第二遍：计算加速度（从基座到末端）
对于 i = 1 到 n:
计算关节加速度 q̈[i]
计算链接加速度 a[i]
传播到子节点
ABA的计算复杂度同样为 $O(n)$，但常数系数略大于RNEA。
复合刚体算法（CRBA） 用于计算质量矩阵 $\mathbf{M}(\mathbf{q})$：
\[\mathbf{M}_{ij} = \mathbf{z}_i^T \mathbf{I}_j^C \mathbf{z}_j\]其中 $\mathbf{I}_j^C$ 是复合刚体惯量，包含了链接 $j$ 及其所有后代的惯量。CRBA的复杂度为 $O(n^2)$。
正向运动学的递归实现：
对于串联机构，末端位姿通过连续的齐次变换计算：
\[\mathbf{T}_{0,n} = \mathbf{T}_{0,1}(\mathbf{q}_1) \mathbf{T}_{1,2}(\mathbf{q}_2) \cdots \mathbf{T}_{n-1,n}(\mathbf{q}_n)\]递归实现：
T[0] = I // 基座坐标系
对于 i = 1 到 n:
T[i] = T[i-1] · T_local[i](q[i])
雅可比矩阵的高效计算：
几何雅可比矩阵的第 $i$ 列对应关节 $i$ 的运动螺旋：
\[\mathbf{J}_i = \begin{cases} \begin{bmatrix} \mathbf{z}_i \\ \mathbf{0} \end{bmatrix} & \text{移动关节} \\ \begin{bmatrix} \mathbf{z}_i \times (\mathbf{p}_e - \mathbf{p}_i) \\ \mathbf{z}_i \end{bmatrix} & \text{转动关节} \end{cases}\]其中 $\mathbf{z}_i$ 是关节 $i$ 的轴向，$\mathbf{p}_i$ 是关节位置，$\mathbf{p}_e$ 是末端位置。
递归计算避免了重复的矩阵乘法：
对于 i = 1 到 n:
z[i] = R[i-1] · axis_local[i] // 关节轴在世界坐标系
p[i] = p[i-1] + R[i-1] · r_local[i] // 关节位置
J[:, i] = compute_column(z[i], p[i], p[n])
奇异性检测与处理：
雅可比矩阵的奇异性通过条件数或最小奇异值检测：
\[\kappa(\mathbf{J}) = \frac{\sigma_{max}}{\sigma_{min}}\]当 $\kappa > \kappa_{threshold}$（如1000）时，系统接近奇异配置。
奇异性处理策略：
对于浮动基座系统，扩展雅可比需要考虑基座运动：
\[\mathbf{J}_{ext} = \begin{bmatrix} \mathbf{J}_{base} & \mathbf{J}_{joint} \end{bmatrix}\]其中 $\mathbf{J}_{base} \in \mathbb{R}^{6 \times 6}$ 通常是单位矩阵（在基座坐标系表达时）。
逆运动学的数值迭代：
牛顿-拉夫逊法是求解逆运动学的标准方法。给定期望末端位姿 $\mathbf{x}_d$，迭代更新关节角度：
\[\Delta \mathbf{q} = \mathbf{J}^+(\mathbf{x}_d - \mathbf{x}(\mathbf{q}_k))\]收敛条件通常设定为位置误差小于1mm，姿态误差小于0.1度。对于冗余机器人，可以在零空间中优化次要目标：
\[\Delta \mathbf{q} = \mathbf{J}^+\Delta \mathbf{x} + (\mathbf{I} - \mathbf{J}^+\mathbf{J})\mathbf{z}\]其中 $\mathbf{z}$ 是零空间运动，可用于避障、避奇异或优化关节配置。
ANYmal是ETH Zurich开发的四足机器人，代表了当前腿式机器人的先进水平。其机械结构和动力学特性为理解浮动基座系统提供了典型案例。
机械结构参数：
坐标系定义：
前进方向 x
↑
│
┌────────┼────────┐
│ LF │ RF │ <- 左前/右前腿
│ │ │
│ 基座 O │ y ← 侧向
│ │ │
│ LH │ RH │ <- 左后/右后腿
└────────┴────────┘
z ⊗ 垂直向下
每条腿的运动链从基座到足端：
基座 → HAA关节 → 大腿 → HFE关节 → 小腿 → KFE关节 → 足端
(横滚) (俯仰) (俯仰)
动力学参数识别： 系统的准确动力学模型需要识别以下参数：
参数识别通过最小二乘法，利用激励轨迹的实验数据：
\[\min_{\boldsymbol{\theta}} \sum_{t} \|\boldsymbol{\tau}_{measured}(t) - \boldsymbol{\tau}_{model}(t, \boldsymbol{\theta})\|^2\]其中 $\boldsymbol{\theta}$ 是待识别的动力学参数向量。
广义坐标选择：
ANYmal的广义坐标定义为： \(\mathbf{q} = [\mathbf{p}_b^T, \boldsymbol{\phi}_b^T, \mathbf{q}_{LF}^T, \mathbf{q}_{RF}^T, \mathbf{q}_{LH}^T, \mathbf{q}_{RH}^T]^T \in \mathbb{R}^{18}\)
其中每条腿的关节角度为： \(\mathbf{q}_{leg} = [q_{HAA}, q_{HFE}, q_{KFE}]^T\)
质量矩阵结构：
由于四条腿的对称性，质量矩阵具有特殊的稀疏结构：
\[\mathbf{M} = \begin{bmatrix} \mathbf{M}_{bb} & \mathbf{M}_{b,LF} & \mathbf{M}_{b,RF} & \mathbf{M}_{b,LH} & \mathbf{M}_{b,RH} \\ \mathbf{M}_{b,LF}^T & \mathbf{M}_{LF} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{M}_{b,RF}^T & \mathbf{0} & \mathbf{M}_{RF} & \mathbf{0} & \mathbf{0} \\ \mathbf{M}_{b,LH}^T & \mathbf{0} & \mathbf{0} & \mathbf{M}_{LH} & \mathbf{0} \\ \mathbf{M}_{b,RH}^T & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{M}_{RH} \end{bmatrix}\]腿间的耦合仅通过基座产生，这大大简化了计算。每条腿的质量矩阵 $\mathbf{M}_{leg} \in \mathbb{R}^{3 \times 3}$ 可独立计算。
接触雅可比计算：
每个足端的接触雅可比将足端速度映射到广义速度空间：
\[\mathbf{v}_{foot} = \mathbf{J}_{foot} \mathbf{v}\]对于左前腿（LF），其雅可比矩阵为：
\[\mathbf{J}_{LF} = \begin{bmatrix} \mathbf{J}_{LF,base} & \mathbf{J}_{LF,leg} & \mathbf{0}_{3 \times 9} \end{bmatrix}\]其中 $\mathbf{J}{LF,base} \in \mathbb{R}^{3 \times 6}$ 是基座运动对足端的影响，$\mathbf{J}{LF,leg} \in \mathbb{R}^{3 \times 3}$ 是腿部关节对足端的影响。
单腿逆运动学解析解：
ANYmal的腿部结构允许解析求解逆运动学。给定足端在髋关节坐标系的位置 $\mathbf{p}_{foot}^{hip} = [x, y, z]^T$：
HAA角度（髋关节横滚）： \(q_{HAA} = \arctan2(y, -z)\)
腿在矢状面的投影长度： \(l_{xz} = \sqrt{x^2 + (z/\cos(q_{HAA}))^2} - l_{hip}\)
KFE角度（膝关节）通过余弦定理： \(q_{KFE} = \pi - \arccos\left(\frac{l_{thigh}^2 + l_{shank}^2 - l_{xz}^2}{2 l_{thigh} l_{shank}}\right)\)
HFE角度（髋关节俯仰）： \(q_{HFE} = \arctan2(x, -z/\cos(q_{HAA})) - \arcsin\left(\frac{l_{shank}\sin(q_{KFE})}{l_{xz}}\right)\)
解析解的存在极大提高了运动规划和控制的效率。
动力学一致性测试：
验证动力学模型的正确性通过能量守恒测试：
自由落体测试：机器人从高度 $h$ 自由落下，验证势能转换为动能： \(mgh = \frac{1}{2}\mathbf{v}^T\mathbf{M}\mathbf{v}\)
零重力漂浮测试：在零重力环境下，验证动量和角动量守恒： \(\frac{d}{dt}(m\mathbf{v}_{CoM}) = \mathbf{0}, \quad \frac{d}{dt}\mathbf{L} = \mathbf{0}\)
关节空间惯量测试：固定基座，摆动单腿，验证关节惯量的正确性。
接触力验证：
静态平衡测试验证接触力计算：
计算性能基准：
在标准硬件（Intel i7 @ 2.8GHz）上的计算时间：
这些性能指标确保了1 kHz的控制频率要求。
模型不确定性处理：
实际系统存在多种不确定性来源：
鲁棒控制策略包括：
接触检测与切换：
接触状态的可靠检测对于模式切换至关重要：
实时约束满足：
硬实时要求下的优化策略：
驱动器饱和处理：
实际驱动器有力矩和速度限制： \(|\tau_i| \leq \tau_{max}, \quad |\dot{q}_i| \leq \dot{q}_{max}\)
饱和处理策略：
传感器噪声和延迟：
实际传感器存在噪声和延迟，影响状态估计：
状态估计通常采用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF），融合多传感器信息以提高精度和鲁棒性。
本章深入探讨了机器人运动学与动力学的核心理论，为后续的规划和控制算法奠定了坚实基础。我们从浮动基座系统的特殊性出发，系统地构建了完整的动力学框架。
关键概念回顾：
核心公式汇总：
| 概念 | 公式 | 说明 |
|---|---|---|
| 动力学方程 | $\mathbf{M}\dot{\mathbf{v}} + \mathbf{C}\mathbf{v} + \mathbf{g} = \boldsymbol{\tau}_{gen}$ | 广义形式 |
| 质心动力学 | $m\ddot{\mathbf{p}}_{CoM} = \sum\mathbf{f}_i + m\mathbf{g}$ | 牛顿定律 |
| 角动量变化 | $\dot{\mathbf{L}} = \sum(\mathbf{p}i - \mathbf{p}{CoM}) \times \mathbf{f}_i$ | 欧拉方程 |
| 摩擦锥约束 | $|\mathbf{f}_t| \leq \mu f_n$ | 库仑摩擦 |
| 互补性条件 | $0 \leq c \perp f_n \geq 0$ | 接触逻辑 |
| 雅可比关系 | $\mathbf{v}_{end} = \mathbf{J}\mathbf{v}$ | 速度映射 |
与后续章节的联系：
掌握本章内容是理解现代机器人控制的关键。动力学模型不仅是仿真的基础，更是所有基于模型的规划和控制算法的核心。
在机器人动力学建模和实现中，即使经验丰富的工程师也容易犯一些常见错误。本节总结这些陷阱并提供调试建议。
1. 坐标系混淆
最常见的错误来源是坐标系定义不一致：
2. 姿态表示的奇异性
3. 数值积分累积误差
4. 接触力计算的数值问题
5. 摩擦锥线性化误差
6. 忽略关节限位和速度限制
关节限位：q_min ≤ q ≤ q_max
速度限制：|q̇| ≤ q̇_max
加速度限制：|q̈| ≤ q̈_max
力矩限制：|τ| ≤ τ_max
在优化问题中明确加入这些约束
7. 质量矩阵的错误计算
# 检查对称性
assert norm(M - M.T) < 1e-10
# 检查正定性
eigenvalues = eig(M)[0]
assert all(eigenvalues > 0)
# 检查能量守恒性质
assert norm(dM/dt - 2*C) < 1e-10 # 应为反对称
8. 时间延迟和因果性
9. 单位不一致
10. 碰撞和穿透处理
调试建议总结：
练习3.1 浮动基座与固定基座的本质区别
考虑一个3自由度的机械臂，分别计算其作为固定基座和浮动基座（如安装在卫星上）时的自由度、质量矩阵维度和动力学方程形式。讨论两种情况下的可控性差异。
练习3.2 ZMP与质心投影的关系
一个双足机器人质心高度 $h = 1.0$ m，质量 $m = 50$ kg，当前质心水平加速度 $\ddot{x}_{CoM} = 2.0$ m/s²。计算ZMP相对于质心投影（CMP）的偏移量。如果支撑多边形宽度为0.2 m，判断机器人是否稳定。
练习3.3 摩擦锥约束的线性化
给定摩擦系数 $\mu = 0.5$，将三维摩擦锥约束 $|\mathbf{f}_t| \leq \mu f_n$ 线性化为4面锥。写出线性不等式约束 $\mathbf{A}\mathbf{f} \leq \mathbf{0}$ 的矩阵 $\mathbf{A}$。
练习3.4 递归算法复杂度分析
证明RNEA算法的计算复杂度为 $O(n)$，其中 $n$ 是自由度数。提示：分析每个递归步骤的操作数，考虑向量叉乘（6次乘法）和矩阵向量乘法的开销。
练习3.5 接触力优化的物理意义
多点接触时，为什么常用 $\min |\mathbf{f}|^2$ 作为目标函数？讨论其他可能的目标函数（如 $\min |\mathbf{f}|1$，$\min |\mathbf{f}|\infty$）的物理意义和计算特性。
练习3.6 浮动基座系统的动量映射
推导浮动基座机器人的质心角动量 $\mathbf{L}$ 与广义速度 $\mathbf{v}$ 的关系：$\mathbf{L} = \mathbf{A}(\mathbf{q})\mathbf{v}$。说明质心角动量矩阵 $\mathbf{A}(\mathbf{q})$ 的结构和计算方法。
练习3.7 奇异配置的几何解释
对于平面3R机械臂（3个转动关节），描述所有可能的奇异配置，给出几何解释，并提出避免奇异的运动规划策略。
练习3.8 多体系统的能量方法
使用拉格朗日方法推导双摆（两个串联的摆）的动力学方程。验证系统的能量守恒性，并讨论如何扩展到n连杆情况。