刚体速度

描述一个运动刚体上无数个点的复杂速度场，其实只需要一组简单的参数。​ 这组参数就是刚体速度，也称为空间速度或扭转。

考虑在空间中的一个刚体，这个刚体有很多的点Point{P_i} 每个点有不同的速度

para:对于刚体上所有点的共同参数，约束

所有点的速度都不是相互独立的，都是相互影响的，因为有连杆的连接，并且它们可以使用一组参数来描述

1.问题提出：复杂的速度场

• 原文: Consider a rigid body in motion. The body has infinitely many points {p_i} with different velocities {v_{p_i}}

• 解释:

  • 当一个刚体（如机器人的连杆、无人机的机身）在空间中运动时，其上的每一个点都在运动。

  • 由于刚体本身不会变形，这些点之间的相对位置是固定的，但它们的绝对速度（相对于一个固定参考系的速度）通常是各不相同的。例如，一个旋转的轮子，轮毂中心的速度和轮缘上某点的速度，其大小和方向都不同。

  • 这就形成了一个复杂的速度场​ {v_{p_i}}

2. 速度场的关联性（非独立性）

• 原文: All these velocities v_{p_i}'s are not independent

• 解释:

  • 这是最关键的一点。虽然各点速度不同，但它们不是相互独立的。

  • 这种“关联性”源于刚体的“刚性”约束：任意两点之间的距离保持不变。这个约束条件将所有点的运动“锁”在了一起。

  • 因此，你知道刚体上某一部分的运动，就能完全确定其他所有部分的运动。

3. 解决方案：统一的参数化

• 原文: They can be expressed by the same set of parameter

• 解释:

  • 正因为这种关联性，我们不需要用无限多个速度矢量来描述刚体的运动。

  • 我们可以找到一组有限的参数，只要知道了这组参数，就能计算出刚体上任意一点的速度。

  • 幻灯片中用公式 v_{p_i} = g(p_i, p_para)抽象地表示了这种关系。其中 p_para就是这组“统一的参数”，而函数 g描述了如何利用这组参数和点的位置 p_i来计算该点的速度。

4. 核心概念引入：刚体速度/扭转

• 原文: Rigid body velocity (i.e. spatial velocity, twist) is one such parameterization

• 解释:

  • 这组“统一的参数”就是刚体速度，在学术上更常被称为空间速度或扭转。

  • 扭转​ 是一个六维向量，它包含了描述刚体瞬时运动状态的全部信息。这六个维度可以分解为两部分：

    1. 线速度矢量 (v)： 描述了刚体上某一特定点（通常选为质心或坐标系原点）的平移运动速度。

    2. 角速度矢量 (ω)： 描述了刚体整体的旋转运动（绕哪根轴、以多快的速度旋转）。

如何描述一个运动刚体上无数个点的速度？

第一部分：纯旋转情况

图片原文

• 假设: Assume P₀ on the rotation axis.(假设点P₀在旋转轴上)

• 公式: v_{p_i} = ω × p₀p_i

• 备注: V_{p₀} = 0(点P₀的速度为0)

• 总结: = g(p_i, ω)(点P_i的速度是点P_i的位置和角速度ω的函数

总结与示例

• 总结： 在纯旋转情况下，刚体上任意点的速度只取决于角速度ω和该点到旋转轴的矢量。

• 示例： 计算一扇门上离门轴不同距离的点的速度。离轴越远的点，其速度矢量越大（门把手处速度最大）

第二部分：一般运动

• 假设: Again, assume P₀ on rotation axis/body-fixed.(假设点P₀在旋转轴上或固连于刚体上)

• 公式:

v_{p_i} = d(p_i)/dt
        = d(p₀)/dt + d(p₀p_i)/dt
        = v_{p₀} + ω × p₀p_i
        = g(v_{p₀}, ω, p_i)

详细解释

1. 场景： 刚体在空间中进行最一般的运动，即同时发生平移和旋转。就像在空中飞行的无人机或在地上奔跑的机器人。
2. 参考点P₀的新选择： 此时，我们不再能找到速度为零的点。但我们仍然可以在刚体上任意选择一个点P₀作为参考点（通常选择质心或几何中心）。这个点P₀本身也在以速度 v_{p₀}运动。
3. 公式推导（最关键的一步）：

• 点P_i的位置可以表示为：p_i = p₀ + p₀p_i。

• 我们对这个等式两边关于时间求导（求速度）：

  v_{p_i} = d(p_i)/dt = d(p₀)/dt + d(p₀p_i)/dt

• d(p₀)/dt就是参考点P₀的速度 v_{p₀}。

• d(p₀p_i)/dt是矢量 p₀p_i的变化率。由于刚体是刚性的，点P_i相对于点P₀的距离不会改变，它们之间只能发生相对旋转。因此，这个变化率就是由于刚体旋转而产生的相对速度，其大小等于 ω × p₀p_i。

4.最终公式： 将两部分叠加，就得到了著名的刚体速度叠加公式：

v_{p_i} = v_{p₀} + ω × p₀p_i

物理意义：叠加原理

这个公式具有极其清晰的物理意义：刚体上任意一点P_i的速度可以看作是两种运动的叠加：

1. 随参考点P₀的平移： v_{p₀}。想象你“抱着”点P₀一起移动。
2. 绕参考点P₀的旋转： ω × p₀p_i。想象刚体在“抱着”P₀平移的同时，还绕着通过P₀的瞬时转动轴旋转

在机器人学中的应用（以宇树Go1为例）

这个公式是分析机器人运动的基础：

• 计算足端速度： 在Go1机器人中，如果我们知道躯干（机身）的速度​ v_torso和躯干的角速度​ ω_torso（由IMU和状态估计器提供），并且通过机器人模型知道足端相对于躯干的位置​ p_torso_foot，那么我们就可以直接计算出足端相对于地面的速度：

  v_foot = v_torso + ω_torso × p_torso_foot

  这个速度对于规划步态、保证足端平稳着地或离地至关重要。

• 逆向应用： 同样，如果我们通过运动规划给出了足端期望的速度 v_foot_desired，我们可以利用这个公式反推出躯干应有的运动速度 v_torso和 ω_torso，从而实现复杂的运动控制。

参考点P₀不在旋转轴上怎么办？

图片最后提出了一个问题：What if ref point NOT on rotation axis?(如果参考点不在旋转轴上怎么办？)

• 答案： 公式依然成立！

• 解释： 这个公式的美妙之处在于，参考点P₀可以是刚体上任意一点。无论你选哪一点，公式 v_{p_i} = v_{p₀} + ω × p₀p_i都正确。

• 区别： 如果你选的点P₀恰好在那条“瞬时转动轴”上，那么它的速度 v_{p₀}会很小甚至为零（纯旋转情况就是特例）。如果你选的点不合适，v_{p₀}可能会很大，但最终计算出的其他点的速度 v_{p_i}仍然是正确的。通常，选择质心作为参考点是最方便的，因为动力学方程在质心系下形式最简单

运动参数获取流程

要描述像宇树Go1这样的刚体上任意一点的运动速度，理论上确实只需要两个最核心的参数：

1. 刚体质心的线速度 (vₚ₀)

2. 刚体的角速度 (ω)

核心公式解读

这个公式就是描述刚体运动学的“万能钥匙”：

v_{p_i} = v_{p₀} + ω × p₀p_i

• v_{p_i}： 这是您想求的刚体上任意一点 p_i的速度。对于Go1，这个点 p_i可以是机器人的头部、足端、甚至机械臂的末端。

• v_{p₀}： 这是您提到的刚体质心的线速度。在Go1中，这个质心通常位于躯干中心附近，其速度由状态估计算法结合IMU和关节信息得出。

• ω： 这是您提到的刚体的角速度。在Go1中，这个数据直接由机身上的IMU（惯性测量单元）测量得到。

• p₀p_i： 这是从质心 p₀指向目标点 p_i的位置矢量。

结论是：只要您知道质心的速度、整个刚体的角速度，以及目标点相对于质心的位置，您就能唯一确定该点的速度。但原则上只需要这两个核心参数。但需要补充一个关键的“其他参数”

核心参数（充分必要条件）：

1.ω：刚体的角速度

对于一个刚体，其内部所有点的角速度是相同的，都等于整个刚体的角速度。即，刚体上每一个点的角速度都是 ω。

1. 角速度是刚体的整体属性，而非点的属性

图片中，无论是在“纯旋转”还是“一般运动”的情况下，符号 ω都是没有下标的。它写作 ω，而不是 ω_p_i。这本身就说明：ω是描述整个刚体旋转状态的单一物理量。

 一个生动的比喻

想象一个旋转的木盘：

• 你在木盘的中心、中间和边缘各画一个点。

• 当木盘旋转时，整个木盘转动的快慢（角速度 ω）是一样的。也就是说，这三个点在一秒钟内转过的角度（例如90度）是相同的。

• 但是，它们移动的“路程”（线速度 v）却完全不同。边缘的点走过的圆周最长，所以它的线速度最大；中心的点几乎没移动，线速度为零。

这个比喻说明：角速度描述“转动得多快”，线速度描述“跑得多快”。​ 刚体上所有点“转动得多快”是一致的，但它们“跑得多快”则取决于它们离转动轴的远近。

“如果参考点不在旋转轴上会怎样？”

• 答案： 角速度 ω依然不变！

• 解释： 即使你选择的参考点 p_0不在瞬时转动轴上，公式 v_p_i = v_p_0 + ω × p_0p_i依然成立。此时，v_p_0不再为零，但用于计算相对速度的角速度 ω仍然是同一个刚体角速度。改变的只是描述方式（v_p_0不为零了），但刚体本身的旋转状态 ω并没有改变。

“怎么确定其每一个点的角速度？”

确定方法非常简单：因为你只需要确定一次。一旦你通过陀螺仪等传感器测量或计算出了整个刚体的角速度 ω，那么你就知道了刚体上每一个点的角速度，它们都是这个 ω。

2.v_{p₀}：刚体质心的线速度

在工程实践中，质心线速度 vp0​​通常无法被直接测量，而是通过“状态估计”的方法计算出来的

第一步：理解数据来源（需要什么数据）

要估计质心速度 v_p0​​，我们需要融合以下几类传感器数据，这些数据共同提供了刚体运动的线索：

1. 惯性测量单元（IMU）：

   • 提供数据：刚体的角速度 ω（直接测量）和线性加速度 a_imu​。

   • 注意：IMU测得的线性加速度是“质心加速度”和“重力加速度”的混合信号，即 a_imu​=a_p0​​+g。

2. 关节编码器：

   • 提供数据：机器人的关节角度 θ和关节速度 θ˙。

   • 作用：通过运动学模型，可以计算出足端、连杆等部位相对于质心的位置 p0​pi​​和速度。

3. 足端接触传感器：

   • 提供数据：判断机器人的足端是否与地面接触。

   • 作用：这是最关键的一步。当足端与地面保持固定接触（无滑动）时，我们可以认为足端相对于地面的速度为零。这个“零速度”的约束条件为估计质心速度提供了最可靠的观测值

第二步：选择估计算法（如何融合数据）

最常用的方法是卡尔曼滤波器或其变种（如扩展卡尔曼滤波器EKF）。其基本流程如下图所示，它通过融合多传感器数据来最优地估计出无法直接测量的状态量（如质心速度）。

具体来说，滤波器的两个核心步骤是：

1. 预测步（依靠IMU）：

   • 利用上一时刻的估计值和对IMU加速度 a_imu​的积分，来预测当前时刻的质心速度 vp0​−​和位置 p0−​。

   • 公式：

   • 由于加速度测量存在噪声，这个预测会随着时间产生显著漂移。

2. 更新步/校正步（依靠运动学信息）：

   • 这是纠正漂移的关键。当检测到有足端稳定接触地面时，我们就获得了一个宝贵的观测信息：该足端的速度为零。

                滤波器会将这个观测值与IMU的预测值进行比较和融合，最终输出一个最优的、无漂移的估计值 v_p0​​。

运动学观测足端接触实例

步骤1：建立约束条件

• 观测来源： 当机器人的足端与地面发生固定接触且没有滑动时，我们可以认为足端相对于地面的速度为零。这是一个强有力的物理约束。“足端与地面之间没有相对滑动”（即假设地面是绝对光滑的刚性接触点）。一旦发生打滑，v_foot = 0这个约束就不成立了，计算出的 v_p₀将存在误差

• 即：v_foot = 0。

步骤2：应用运动学公式（您的图片内容）

• 我们将图片中的公式应用于此场景：

  • 参考点 p_0是质心。

  • 目标点 p_i是足端。

• 因此，公式变为：

  v_foot = v_p₀ + ω × p₀_foot

步骤3：推导观测值

• 将约束条件 v_foot = 0代入公式：

  0 = v_p₀ + ω × p₀_foot

• 由此，我们可以解出质心速度 v_p₀的观测值：

  v_p₀ (观测值) = - (ω × p₀_foot)

在这个等式中：

• ω： 由机器人的IMU直接测量得到，是已知量。

• p₀_foot： 从质心指向足端的矢量。通过机器人的运动学模型和关节编码器读取的关节角度计算得到，是已知量。

• v_p₀ (观测值)： 是我们通过运动学关系计算出的质心速度。

 为什么运动学观测如此重要？

运动学观测的价值在于它提供了一个独立、无漂移的速度估计来源，用于校正IMU的积分漂移。

1. IMU的缺陷： 如果只用IMU，我们需要对加速度进行一次积分来得到速度。加速度测量中的微小偏差和噪声会在积分过程中被放大，导致速度估计随着时间越来越不准确（即“漂移”）。

2. 运动学观测的优势： 上述计算得到的 v_p₀ (观测值)是一个瞬时、绝对的观测。它不依赖于积分，因此不会漂移。当足端接触地面时，这个观测值非常可靠。

通过运动学观测获得的质心速度 v_p₀的应用

通过运动学观测获得的质心速度 v_p₀，在足式机器人（如宇树Go1）的控制中扮演着至关重要的角色。它不再是理论上的一个数值，而是实现动态平衡、敏捷运动的核心反馈信号

核心应用一：实现平衡稳定（抗扰动）

这是最直接、最重要的应用。机器人的平衡控制器需要实时知道“我有没有在失控？我有多快地在失控？”来做出反应。

• 场景： 机器人静止站立时，你从侧面推了它一下。

• 控制逻辑：

  1. 感知： 机器人的状态估计器通过运动学观测（假设此时双腿接触地面），立刻计算出一个质心速度 v_p₀。发现质心正在产生一个向侧面（比如向左）的速度。

  2. 决策： 平衡控制器（例如一个简单的PD控制器）的目标是让速度归零以保持静止。它会产生一个控制指令：“需要产生一个向右的加速度来抵消这个向左的速度”。

  3. 执行： 这个指令被转化为对腿部关节的力矩命令，使得机器人能够：

     • 调整足底压力分布： 加大右侧腿的推力，减小左侧腿的推力，产生一个向右的合力。

     • 迈步恢复： 如果扰动很大，单靠调整足底压力不足以稳定，控制器会判断需要迈出一步来防止摔倒。此时，质心速度的方向和大小是决定哪条腿迈出、以及迈出多远的关键依据。

没有这个准确的速度反馈，机器人就无法区分是正常的运动指令还是外界的扰动，也就无法做出及时的平衡反应。

---

核心应用二：精确的轨迹跟踪与速度控制

当机器人执行行走、小跑等动态步态时，需要精确控制其质心的运动轨迹。

• 场景： 让机器人以1米/秒的速度匀速前进。

• 控制逻辑：

  1. 设定目标： 期望的质心速度 v_desired = [0.5, 0, 0]^T（假设向前为X轴）。

  2. 获取反馈： 通过运动学观测，实时得到实际的质心速度 v_actual。

  3. 计算误差： 速度误差 v_error = v_desired - v_actual。

  4. 闭环控制： 控制器根据速度误差，动态调整步态参数（如步长、步频）和足端落地点，使实际速度精准地跟踪上期望速度。

这就构成了一个闭环反馈控制系统。​ 运动学观测提供的 v_p₀是这个闭环中不可或缺的反馈信号。没有它，控制器就不知道机器人实际跑得多快，无法实现精确的速度控制。

---

核心应用三：步态规划与着地点选择

对于动态奔跑等高阶技能，质心速度是规划下一步落脚点的最关键输入。

• 场景： 机器人在奔跑中即将腾空，需要决定下一脚落在哪里才能保持稳定。

• 控制逻辑（基于线性倒立摆模型）：

  • 一个经典的着地点规划公式是：x_foot = k * v_current

  • 其中：

    • x_foot： 下一个足端相对于质心的目标落地点。

    • v_current： 当前通过运动学观测得到的质心速度。

    • k： 一个与机器人高度和动力学相关的常数。

  • 物理意义： 如果机器人向前运动的速度太快（v_current很大），就需要把脚伸到更靠前的位置（x_foot变大）来“刹车”并支撑住身体；如果速度太慢，则需要落脚点靠后来“助推”。

在这里，质心速度直接决定了运动的策略和意图。​ 错误的速到估计将导致错误的落足点，进而导致机器人失稳摔倒。

---

核心应用四：状态估计器的传感器融合与校正

如前所述，运动学观测为整个状态估计系统提供了一个无漂移的、绝对的速度观测基准。

• 场景： 机器人长时间运动。

• 工作流程：

  1. IMU漂移： 仅靠IMU积分得到的速度会随着时间越来越不准。

  2. 运动学校正： 每当足端稳定触地，运动学观测就会提供一个准确的、瞬时的质心速度​ v_kinematic。

  3. 融合优化： 状态估计器（如卡尔曼滤波器）会比较“IMU积分预测的速度”和“运动学观测的速度”，然后修正其内部状态，输出一个最优的、既平滑又无漂移的最终速度估计值。

这确保了机器人无论在何种运动状态下，都能获得可靠的速度信息，这是所有高级控制得以实现的基础

总而言之，通过运动学观测获得的质心速度 v_p₀，绝不是一個理論數值，而是机器人实现“智能”运动的“本体感觉”。它的具体应用贯穿于：

应用场景	质心速度 v_p₀的作用	类比
平衡稳定​	反馈信号，用于抵抗扰动、防止摔倒。	像人感觉到要摔倒时，会自动伸手或迈步来恢复平衡。
轨迹跟踪​	反馈信号，用于实现精确的速度控制。	像人开车时看时速表，通过油门和刹车使车速保持在限速内。
步态规划​	规划依据，用于动态决定下一步的落脚点。	像人跑步时，根据当前冲速判断下一步跨多大才能平稳落地。
状态估计​	校正基准，用于消除IMU积分漂移。	像人走路时，既靠内耳（前庭）感受平衡，也靠眼睛看地面来校正。

足端不接触地面，其质点的线速度如何计算

当足端不与地面接触时（即腾空期），我们失去了“足端速度为零”这个最强有力的约束，计算（更准确地说是估计）质心速度的方法发生了根本变化

此时，机器人无法再通过简单的运动学公式直接“计算”速度，而是必须依赖多传感器融合估计的方法。其核心思路从几何计算转向了物理预测与观测校正

方法一：惯性导航外推（预测为主，会产生漂移）

这是最基础但误差会累积的方法。

• 原理：

  1. 在腾空期开始的瞬间，机器人拥有一个相对准确的质心速度初始值 v_0（这个值来自于腾空前最后一刻的估计）。

  2. 在腾空期间，机器人依赖IMU（惯性测量单元）​ 测量的加速度（扣除重力加速度 g后）进行积分来推算速度：

     v(t) = v_0 + ∫(a_imu - g) dt

• 优点： IMU数据更新频率极高（通常1kHz），能捕捉到高速动态变化。

• 致命缺点： 积分漂移。加速度测量中微小的偏差和噪声会在积分过程中被无限放大，导致速度估计在很短时间（可能不到1秒）内就产生巨大误差。这就像蒙上眼睛走路，一开始知道方向和速度，但仅凭感觉走几步后就会迷失。

---

方法二：视觉/激光里程计（核心校正手段，对抗漂移）

这是在腾空期最重要、最有效的速度估计方法。它充当了机器人的“眼睛”。

• 原理：

  1. 通过机载的摄像头或激光雷达，持续观察周围环境（如地面的纹理、墙壁的轮廓）。

  2. 使用视觉里程计 (VO)​ 或激光里程计 (LO)​ 算法，通过分析连续图像或激光点云的变化，计算出相机（即机器人本体）相对于环境的运动。

  3. 这个相对运动直接提供了机器人在世界坐标系下的位移，对其求导即可得到线速度。这是一个绝对的、无漂移的观测值。

• 优点： 提供绝对参考，能有效校正IMU的积分漂移。

• 缺点：

  • 依赖环境特征： 在特征稀少（如白墙、雪地）或运动模糊严重时可能失效。

  • 计算延迟： 处理图像或点云需要时间，会引入延迟。

  • 精度受距离影响： 离地面或周围物体越远，估计精度可能下降。

---

方法三：运动学动量观测（辅助手段）

这是一种辅助方法，提供额外的约束。

• 原理：

  1. 即使在腾空期，机器人的腿部也不是完全僵硬的，它会按照预定轨迹摆动为落地做准备。

  2. 通过关节编码器可以知道腿部的关节角度和速度。

  3. 根据动量守恒定律，整个机器人系统（躯干+腿部）的总动量在腾空期是守恒的。腿部的摆动会产生动量，这个动量必须由躯干的反向动量来平衡。

  4. 利用机器人的动力学模型，可以根据腿部的运动来估计躯干（质心）的速度变化。

• 优点： 提供来自机器人本体的额外信息，不依赖外部环境。

• 缺点： 模型复杂，效果不如视觉里程计直接和精确，通常作为辅助。

• 应用： 在视觉暂时失效时（如镜头被遮挡）提供一定的帮助。

核心解决方案：传感器融合（状态估计器）

在实际系统中，永远不会只依赖一种方法。一个典型的状态估计器（如卡尔曼滤波器）在腾空期的工作流程，可以清晰地通过下图展示

总结

场景	核心方法	主要传感器	关键约束
足端接触期​	运动学计算​	关节编码器、IMU	足端与地面无相对运动（速度为0）
足端腾空期​	传感器融合估计​	IMU + 视觉/激光雷达​	物理定律（运动学、动力学）、环境特征

结论： 当足端不接触地面时，质心速度无法被直接“计算”，而是通过以IMU积分进行高频预测，并以视觉/激光里程计为绝对观测进行校正的融合估计方法得到的。这个过程体现了机器人从依赖“本体感觉”（接触）到依赖“视觉感知”的智能切换，是实现动态跑、跳等高级技能的基础