腿式里程计：从关节和接触估计身体速度

腿式里程计：从关节和接触估计身体速度

第一次接触腿式里程计时，可以先把它理解成“用脚当临时路标”。轮式机器人可以通过轮子转了多少圈估计车体走了多远；腿式机器人没有连续滚动的轮子，但它有脚。只要某只脚正踩在地面上，并且没有打滑，那么这只脚在世界坐标系中就近似静止。机器人身体相对这只脚怎么动，就能反推出身体速度。

这里有一个关键假设：

$${\mathbf{v}}_{foot,i}^W\approx 0$$

其中 ${\mathbf{v}}_{foot,i}^W\in {\mathbb{R}}^3$ 是第 $i$ 条腿足端在世界坐标系 $W$ 中的速度，$i$ 是腿编号，例如左前、右前、左后、右后。上标 $W$ 表示这个向量用世界坐标系表达。这个式子的意思是：如果第 $i$ 只脚处于稳定支撑相，那么它相对地面几乎不动。

在进入推导前，先明确腿式里程计的输入和输出。腿式里程计通常不是直接输出机器人位置，而是输出一个身体速度观测：

$${\mathbf{z}}_{leg}={\mathbf{v}}_B+{\mathbf{r}}_{leg}$$

其中 ${\mathbf{z}}_{leg}\in {\mathbb{R}}^3$ 是腿式里程计输出的机体系速度观测，${\mathbf{v}}_B\in {\mathbb{R}}^3$ 是机器人机体在机体系 $B$ 下的真实线速度，${\mathbf{r}}_{leg}\in {\mathbb{R}}^3$ 是腿式里程计误差。比如：

$${\mathbf{z}}_{leg}=\left[\begin{array}{c}0.42\\ 0.01\\ -0.02\end{array}\right]m/s$$

表示机器人身体大约以 $0.42m/s$ 向前运动，侧向速度和竖直速度都比较小。这个速度观测随后会进入 EKF，用来约束 IMU 积分带来的速度漂移。

那腿式里程计的输入从哪里来？最核心的输入来自每个关节上的编码器。编码器会实时测量关节当前转到了多少角度，也就是关节角；控制器或驱动器通常还会给出关节角速度，有些机器人还会给出关节力矩。实际系统里，这些量常被组织成关节状态消息，概念上包含：

• 关节角 $\mathbf{q}$：每个关节当前的位置。
• 关节角速度 $\dot{\mathbf{q}}$：每个关节转动的速度。
• 关节力矩 $\mathbf{\tau }$：每个关节受到或输出的力矩，可辅助判断足端是否接触地面。

其中 $\mathbf{\tau }$ 是关节力矩向量，单位通常是 $\mathrm{N}\cdot \mathrm{m}$。腿式里程计会用关节角计算足端位置，用关节角速度计算足端相对身体的速度，再结合接触状态反推出身体速度。

为了从“脚不动”推到“身体怎么动”，先定义第 $i$ 条腿的关节角向量：

$${\mathbf{q}}_i=\left[\begin{array}{c}{q}_{i,1}\\ q_{i,2}\\ q_{i,3}\end{array}\right]$$

其中 ${\mathbf{q}}_i\in {\mathbb{R}}^3$ 表示第 $i$ 条腿的三个关节角，$q_{i,1}$、$q_{i,2}$、$q_{i,3}$ 分别表示这条腿上第 1、2、3 个关节的角度。这里的“角向量”不是说某个角本身有方向，而是把多个关节角按固定顺序排成一个列向量，方便统一计算。不同机器人每条腿的关节数可能不同，这里用三关节腿作为四足机器人的常见例子。

比如一条腿有三个转动关节：髋关节外展/内收、髋关节前后摆动、膝关节弯曲。若当前三个关节角分别是：

$$q_{i,1}=10^{\circ },q_{i,2}=30^{\circ },q_{i,3}=-60^{\circ }$$

则可以写成：

$${\mathbf{q}}_i=\left[\begin{array}{c}10^{\circ }\\ 30^{\circ }\\ -60^{\circ }\end{array}\right]$$

计算时通常会把角度转成弧度，因为三角函数和求导更自然使用弧度：

$$10^{\circ }=\frac{10\pi }{180}\mathrm{rad}$$

正运动学告诉我们：只要知道关节角，就能算出足端相对机体的位置：

$${\mathbf{r}}_{i,B}={\mathbf{f}}_i({\mathbf{q}}_i)$$

其中 ${\mathbf{r}}_{i,B}\in {\mathbb{R}}^3$ 是第 $i$ 个足端相对于机体坐标系 $B$ 原点的位置，${\mathbf{f}}_i(\cdot )$ 是第 $i$ 条腿的正运动学函数，机体坐标系 $B$ 固定在机器人身体上。下标 $i,B$ 表示“第 $i$ 条腿足端、用机体系描述”。

所谓“相对位置”，就是从机体坐标系原点指向足端的向量。如果机体坐标系 $B$ 的原点在躯干中心，$x$ 轴指向机器人前方，$y$ 轴指向左方，$z$ 轴指向上方，那么：

$${\mathbf{r}}_{i,B}=\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]$$

其中 $x_i$ 是足端相对躯干中心的前后位置，$y_i$ 是左右位置，$z_i$ 是上下位置。比如 $x_i=0.3$ 表示脚在身体前方 $0.3\mathrm{m}$，$z_i=-0.5$ 表示脚在身体下方 $0.5\mathrm{m}$。

为了把公式讲具体，我们先看一个平面二连杆腿的例子。假设腿只在 $x$-$z$ 平面内运动，有两个关节角：

$$\mathbf{q}=\left[\begin{array}{c}{q}_1\\ q_2\end{array}\right]$$

其中 $q_1$ 是髋关节角，$q_2$ 是膝关节角。设大腿长度为 $l_1$，小腿长度为 $l_2$，二者单位都是米。为了简化，令髋关节就是机体坐标系原点，并且 $x$ 轴向前、$z$ 轴向下。

这里要特别小心：正运动学真正需要的不是“膝关节处两根杆子的夹角本身”，而是每一段连杆相对于机体坐标系的方向。设大腿相对于机体坐标系的方向为：

$${\theta }_1$$

设小腿相对于机体坐标系的方向为：

$${\theta }_2$$

其中 ${\theta }_1$ 是大腿的绝对方向角，${\theta }_2$ 是小腿的绝对方向角。所谓“绝对方向”，是指相对于机体坐标系 $B$ 的角度，而不是相对于上一段连杆的角度。

在这个定义下，足端相对髋关节的位置最一般地写成：

$$x=l_1\sin {\theta }_1+l_2\sin {\theta }_2$$

$$z=l_1\cos {\theta }_1+l_2\cos {\theta }_2$$

这才是本质：$l_1$ 用大腿的绝对方向 ${\theta }_1$，$l_2$ 用小腿的绝对方向 ${\theta }_2$。

很多教材或机器人模型会进一步令：

$${\theta }_1=q_1$$

如果膝关节变量 $q_2$ 被定义为“小腿相对大腿延长方向的有符号偏转角””，也就是：

$$q_2={\theta }_2-{\theta }_1$$

那么才有：

$${\theta }_2={\theta }_1+q_2=q_1+q_2$$

于是公式会变成常见写法：

$$x=l_1\sin q_1+l_2\sin (q_1+q_2)$$

$$z=l_1\cos q_1+l_2\cos (q_1+q_2)$$

但如果说 $q_2$ 是“大腿和小腿之间的几何夹角”，也就是两根杆子的内夹角，那么就不能直接写成 $q_1+q_2$。设这个几何夹角为：

$$\alpha$$

其中 $\alpha$ 是大腿和小腿之间的内夹角。若 $\alpha =180^{\circ }$，表示大腿和小腿完全伸直、方向相同，这时确实应该有：

$${\theta }_2={\theta }_1$$

因此在这种夹角定义下，小腿相对大腿的偏转量不是 $\alpha$，而是相对于伸直状态的弯曲量。常见的一种约定是：

$${\theta }_2={\theta }_1+(\pi -\alpha )$$

其中 $\pi$ 表示 $180^{\circ }$。当 $\alpha =\pi$ 时：

$${\theta }_2={\theta }_1+(\pi -\pi )={\theta }_1$$

这就符合你说的情况：如果大腿和小腿夹角是 $180^{\circ }$，小腿相对于机体的方向应该和大腿相同，而不是再额外加 $180^{\circ }$。

所以文中后续使用 $q_1+q_2$ 时，默认采用的是“$q_2$ 表示小腿相对大腿延长方向的有符号偏转角”这个约定，而不是“$q_2$ 表示两根杆子的几何内夹角”。具体机器人里到底是哪一种，要看 URDF、关节零位和编码器正方向如何定义。

也可以把足端位置拆成“髋关节到膝关节”和“膝关节到足端”两段。膝关节位置为：

$$x_{knee}=l_1\sin q_1,z_{knee}=l_1\cos q_1$$

其中 $x_{knee}$ 和 $z_{knee}$ 是膝关节相对髋关节的位置。小腿这一段应该使用小腿的绝对方向 ${\theta }_2$，所以小腿对足端位置的贡献是：

$$x_{shank}=l_2\sin {\theta }_2,z_{shank}=l_2\cos {\theta }_2$$

其中 $x_{shank}$ 和 $z_{shank}$ 是从膝关节指向足端的向量分量。最后足端位置就是两段相加：

$$x=x_{knee}+x_{shank}$$

$$z=z_{knee}+z_{shank}$$

举例来说，如果采用“有符号偏转角”约定，并且 $q_1=30^{\circ }$、$q_2=-60^{\circ }$，那么小腿相对机体坐标系的方向不是 $-60^{\circ }$，而是：

$$q_1+q_2=30^{\circ }-60^{\circ }=-30^{\circ }$$

所以计算 $l_2$ 这一段的位置贡献时，要使用 $-30^{\circ }$。

但如果采用“几何内夹角”约定，并且大腿和小腿夹角为 $\alpha =180^{\circ }$，则：

$${\theta }_2=q_1+(\pi -\alpha )=q_1$$

这时小腿方向和大腿方向完全一致。

于是足端相对位置可以写成：

$$\mathbf{r}=\left[\begin{array}{c}x\\ z\end{array}\right]=\left[\begin{array}{c}{l}_1\sin q_1+l_2\sin (q_1+q_2)\\ l_1\cos q_1+l_2\cos (q_1+q_2)\end{array}\right]$$

这就是一个非常具体的正运动学函数：

$$\mathbf{r}=\mathbf{f}(\mathbf{q})$$

其中 $\mathbf{r}\in {\mathbb{R}}^2$ 是平面足端位置，$\mathbf{q}\in {\mathbb{R}}^2$ 是平面二连杆的关节角向量。

举一个数值例子。设：

$$l_1=0.25\mathrm{m},l_2=0.25\mathrm{m}$$

$$q_1=30^{\circ },q_2=-60^{\circ }$$

因为：

$$\sin 30^{\circ }=0.5,\sin (-30^{\circ })=-0.5$$

$$\cos 30^{\circ }\approx 0.866,\cos (-30^{\circ })\approx 0.866$$

所以：

$$x=0.25\times 0.5+0.25\times (-0.5)=0$$

$$z=0.25\times 0.866+0.25\times 0.866\approx 0.433\mathrm{m}$$

这表示在这个简化平面模型里，脚大约位于髋关节正下方 $0.433\mathrm{m}$。

举个直观例子：如果机器人身体原点在躯干中心，某只脚在身体前方 $0.3\mathrm{m}$、左侧 $0.2\mathrm{m}$、下方 $0.5\mathrm{m}$，那么可以写成：

$${\mathbf{r}}_{i,B}=\left[\begin{array}{c}0.3\\ 0.2\\ -0.5\end{array}\right]\mathrm{m}$$

足端位置会随关节角变化。对时间求导，可以得到足端因为关节运动产生的速度：

$$\dot{\mathbf{r}}=\frac{d}{dt}\mathbf{f}(\mathbf{q})$$

因为 $\mathbf{r}$ 不是直接随时间变化，而是通过关节角 $\mathbf{q}(t)$ 随时间变化，所以要使用链式法则：

$$\dot{\mathbf{r}}=\frac{\partial \mathbf{f}}{\partial \mathbf{q}}\dot{\mathbf{q}}$$

其中 $\dot{\mathbf{r}}$ 是足端相对机体的速度，$\frac{\partial \mathbf{f}}{\partial \mathbf{q}}$ 是正运动学函数对关节角的偏导矩阵，$\dot{\mathbf{q}}$ 是关节角速度向量。这个偏导矩阵就是雅可比矩阵：

$$\mathbf{J}(\mathbf{q})=\frac{\partial \mathbf{f}}{\partial \mathbf{q}}$$

所以：

$${\dot{\mathbf{r}}}_{i,B}={\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{q}}}_i$$

其中 ${\dot{\mathbf{r}}}_{i,B}\in {\mathbb{R}}^3$ 是足端相对机体的速度，${\mathbf{J}}_i({\mathbf{q}}_i)\in {\mathbb{R}}^{3\times 3}$ 是第 $i$ 条腿的雅可比矩阵，${\dot{\mathbf{q}}}_i\in {\mathbb{R}}^3$ 是第 $i$ 条腿的关节角速度。雅可比矩阵可以理解为“关节速度到足端速度的转换器”：关节每秒转多少，足端就会以怎样的线速度相对身体移动。

继续用平面二连杆例子推导雅可比。前面有：

$$x=l_1\sin q_1+l_2\sin (q_1+q_2)$$

$$z=l_1\cos q_1+l_2\cos (q_1+q_2)$$

分别对 $q_1$ 和 $q_2$ 求偏导：

$$\frac{\partial x}{\partial q_1}=l_1\cos q_1+l_2\cos (q_1+q_2)$$

$$\frac{\partial x}{\partial q_2}=l_2\cos (q_1+q_2)$$

$$\frac{\partial z}{\partial q_1}=-l_1\sin q_1-l_2\sin (q_1+q_2)$$

$$\frac{\partial z}{\partial q_2}=-l_2\sin (q_1+q_2)$$

因此雅可比矩阵为：

$$\mathbf{J}(\mathbf{q})=\left[\begin{array}{cc}\frac{\partial x}{\partial q_1}& \frac{\partial x}{\partial q_2}\\ \frac{\partial z}{\partial q_1}& \frac{\partial z}{\partial q_2}\end{array}\right]=\left[\begin{array}{cc}{l}_1\cos q_1+l_2\cos (q_1+q_2)& l_2\cos (q_1+q_2)\\ -l_1\sin q_1-l_2\sin (q_1+q_2)& -l_2\sin (q_1+q_2)\end{array}\right]$$

如果关节角速度为：

$$\dot{\mathbf{q}}=\left[\begin{array}{c}{\dot{q}}_1\\ {\dot{q}}_2\end{array}\right]$$

其中 ${\dot{q}}_1$ 是髋关节角速度，${\dot{q}}_2$ 是膝关节角速度，单位是 $rad/s$，那么足端速度就是：

$$\dot{\mathbf{r}}=\left[\begin{array}{c}\dot{x}\\ \dot{z}\end{array}\right]=\mathbf{J}(\mathbf{q})\dot{\mathbf{q}}$$

代入刚才的数值 $l_1=l_2=0.25\mathrm{m}$、$q_1=30^{\circ }$、$q_2=-60^{\circ }$，可得：

$$\mathbf{J}(\mathbf{q})\approx \left[\begin{array}{cc}0.433& 0.217\\ 0& 0.125\end{array}\right]$$

若关节角速度为：

$$\dot{\mathbf{q}}=\left[\begin{array}{c}1.0\\ -2.0\end{array}\right]rad/s$$

则：

$$\dot{\mathbf{r}}\approx \left[\begin{array}{cc}0.433& 0.217\\ 0& 0.125\end{array}\right]\left[\begin{array}{c}1.0\\ -2.0\end{array}\right]=\left[\begin{array}{c}-0.001\\ -0.25\end{array}\right]m/s$$

这个结果表示：在这个姿态和关节速度下，足端相对身体主要沿 $z$ 方向移动，$x$ 方向速度几乎为零。三维四足机器人的计算更复杂，但本质完全一样：先用正运动学 ${\mathbf{r}}_{i,B}={\mathbf{f}}_i({\mathbf{q}}_i)$ 算位置，再用雅可比 ${\mathbf{J}}_i({\mathbf{q}}_i)=\frac{\partial {\mathbf{f}}_i}{\partial {\mathbf{q}}_i}$ 把关节角速度变成足端相对速度。

现在考虑身体本身也在动。足端在世界中的速度由三部分组成：

1. 身体平移带来的速度。
2. 身体旋转带来的速度。
3. 关节运动让足端相对身体运动产生的速度。

我们从足端的世界坐标位置开始推导。设机器人机体原点在世界坐标系中的位置为：

$${\mathbf{p}}_B^W$$

其中 ${\mathbf{p}}_B^W\in {\mathbb{R}}^3$ 表示机体坐标系 $B$ 的原点相对于世界坐标系 $W$ 的位置。设第 $i$ 个足端在世界坐标系中的位置为：

$${\mathbf{p}}_{foot,i}^W$$

其中 ${\mathbf{p}}_{foot,i}^W\in {\mathbb{R}}^3$ 表示第 $i$ 个足端相对于世界坐标系 $W$ 的位置。足端世界位置可以写成：

$${\mathbf{p}}_{foot,i}^W={\mathbf{p}}_B^W+{\mathbf{R}}_{WB}{\mathbf{r}}_{i,B}$$

这里 ${\mathbf{r}}_{i,B}\in {\mathbb{R}}^3$ 是足端相对于机体原点的位置，并且用机体系表达；${\mathbf{R}}_{WB}\in {\mathbb{R}}^{3\times 3}$ 是从机体系 $B$ 转到世界系 $W$ 的旋转矩阵。这个式子的意思很直观：足端的世界位置等于“身体原点的世界位置”加上“从身体原点指向足端的向量转换到世界系后的结果”。

因为足端位置 ${\mathbf{r}}_{i,B}$ 会随着关节运动变化，所以更完整地写成：

$${\mathbf{p}}_{foot,i}^W={\mathbf{p}}_B^W+{\mathbf{R}}_{WB}{\mathbf{r}}_{i,B}({\mathbf{q}}_i)$$

其中 ${\mathbf{q}}_i$ 是第 $i$ 条腿的关节角向量，${\mathbf{r}}_{i,B}({\mathbf{q}}_i)$ 表示足端相对机体的位置由关节角决定。

对时间 $t$ 求导，就得到足端速度：

$$\frac{d}{dt}{\mathbf{p}}_{foot,i}^W=\frac{d}{dt}{\mathbf{p}}_B^W+\frac{d}{dt}\left({\mathbf{R}}_{WB}{\mathbf{r}}_{i,B}({\mathbf{q}}_i)\right)$$

左边定义为足端世界速度：

$${\mathbf{v}}_{foot,i}^W=\frac{d}{dt}{\mathbf{p}}_{foot,i}^W$$

第一项定义为机体原点世界速度：

$${\mathbf{v}}_B^W=\frac{d}{dt}{\mathbf{p}}_B^W$$

第二项要用乘积求导法则：

$$\frac{d}{dt}\left({\mathbf{R}}_{WB}{\mathbf{r}}_{i,B}\right)={\dot{\mathbf{R}}}_{WB}{\mathbf{r}}_{i,B}+{\mathbf{R}}_{WB}{\dot{\mathbf{r}}}_{i,B}$$

其中 ${\dot{\mathbf{R}}}_{WB}$ 是旋转矩阵对时间的导数，${\dot{\mathbf{r}}}_{i,B}$ 是足端相对机体的位置变化率，也就是关节运动造成的足端相对速度。

先看 ${\dot{\mathbf{R}}}_{WB}{\mathbf{r}}_{i,B}$。如果机体有角速度 ${\mathbf{\omega }}_B^W$，那么一个固定在机体上的向量会因为机体旋转而在世界系中产生速度。这个结论可以用李群 $SO(3)$ 和李代数 $\mathfrak{so}(3)$ 推导出来。

旋转矩阵 ${\mathbf{R}}_{WB}$ 属于三维旋转群：

$${\mathbf{R}}_{WB}\in SO(3)$$

其中 $SO(3)$ 表示所有三维旋转矩阵组成的集合。角速度向量 ${\mathbf{\omega }}_B^W$ 可以通过“帽算子”变成一个反对称矩阵：

$${\left({\mathbf{\omega }}_B^W\right)}^{\wedge }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$

其中 ${\mathbf{\omega }}_B^W=[{\omega }_x,{\omega }_y,{\omega }_z{]}^T$，${\omega }_x$、${\omega }_y$、${\omega }_z$ 分别是角速度在世界坐标系 $x$、$y$、$z$ 轴上的分量。这个反对称矩阵属于李代数 $\mathfrak{so}(3)$。帽算子的关键性质是：对任意向量 $\mathbf{a}\in {\mathbb{R}}^3$，都有：

$${\left({\mathbf{\omega }}_B^W\right)}^{\wedge }\mathbf{a}={\mathbf{\omega }}_B^W\times \mathbf{a}$$

其中 $\times$ 是叉乘。

当角速度使用世界坐标系表达时，旋转矩阵的导数满足下面这个关系。我们先从一个很短的时间间隔 $\Delta t$ 看起。设当前时刻为 $t$，下一时刻为 $t+\Delta t$。如果机体在这段极短时间内的角速度近似为常量 ${\mathbf{\omega }}_B^W$，那么这段时间内新增的小旋转可以写成指数映射：

$$\exp \left({\left({\mathbf{\omega }}_B^W\right)}^{\wedge }\Delta t\right)$$

其中 $\exp (\cdot )$ 是矩阵指数映射，它把李代数 $\mathfrak{so}(3)$ 中的反对称矩阵转换成李群 $SO(3)$ 中的旋转矩阵。因为 ${\mathbf{\omega }}_B^W$ 用世界坐标系表达，所以这个小旋转是左乘到当前旋转矩阵上的：

$${\mathbf{R}}_{WB}(t+\Delta t)=\exp \left({\left({\mathbf{\omega }}_B^W\right)}^{\wedge }\Delta t\right){\mathbf{R}}_{WB}(t)$$

这里容易误解：左乘还是右乘，不是由“世界坐标系是否静止、机体坐标系是否运动”直接决定的，而是由“小旋转增量用哪个坐标系表达”决定的。

如果小旋转用世界坐标系表达，记为：

$$\Delta {\mathbf{R}}^W=\exp \left({\left({\mathbf{\omega }}_B^W\right)}^{\wedge }\Delta t\right)$$

其中 $\Delta {\mathbf{R}}^W$ 表示用世界坐标系轴描述的短时间旋转增量。因为 ${\mathbf{R}}_{WB}$ 的输出已经在世界坐标系中，所以这个增量作用在世界系一侧，需要左乘：

$${\mathbf{R}}_{WB}(t+\Delta t)=\Delta {\mathbf{R}}^W{\mathbf{R}}_{WB}(t)$$

如果小旋转用机体坐标系表达，记为：

$$\Delta {\mathbf{R}}^B=\exp \left({\left({\mathbf{\omega }}_B^B\right)}^{\wedge }\Delta t\right)$$

其中 ${\mathbf{\omega }}_B^B$ 是机体角速度在机体系 $B$ 中的表达，$\Delta {\mathbf{R}}^B$ 表示用机体系轴描述的短时间旋转增量。因为它作用在机体系输入一侧，所以需要右乘：

$${\mathbf{R}}_{WB}(t+\Delta t)={\mathbf{R}}_{WB}(t)\Delta {\mathbf{R}}^B$$

也就是：

$${\mathbf{R}}_{WB}(t+\Delta t)={\mathbf{R}}_{WB}(t)\exp \left({\left({\mathbf{\omega }}_B^B\right)}^{\wedge }\Delta t\right)$$

两种写法都正确，只是角速度表达坐标系不同。二者满足：

$${\mathbf{\omega }}_B^W={\mathbf{R}}_{WB}{\mathbf{\omega }}_B^B$$

直观地说，左乘表示“用世界坐标系的轴描述这次增量旋转”，右乘表示“用机体坐标系的轴描述这次增量旋转”。本文这里使用的是 ${\mathbf{\omega }}_B^W$，也就是世界系表达，所以采用左乘。

当 $\Delta t$ 很小时，矩阵指数可以用一阶近似：

$$\exp \left({\left({\mathbf{\omega }}_B^W\right)}^{\wedge }\Delta t\right)\approx \mathbf{I}+{\left({\mathbf{\omega }}_B^W\right)}^{\wedge }\Delta t$$

其中 $\mathbf{I}$ 是 $3\times 3$ 单位矩阵。代回上一式：

$${\mathbf{R}}_{WB}(t+\Delta t)\approx \left(\mathbf{I}+{\left({\mathbf{\omega }}_B^W\right)}^{\wedge }\Delta t\right){\mathbf{R}}_{WB}(t)$$

展开：

$${\mathbf{R}}_{WB}(t+\Delta t)\approx {\mathbf{R}}_{WB}(t)+{\left({\mathbf{\omega }}_B^W\right)}^{\wedge }{\mathbf{R}}_{WB}(t)\Delta t$$

两边减去 ${\mathbf{R}}_{WB}(t)$：

$${\mathbf{R}}_{WB}(t+\Delta t)-{\mathbf{R}}_{WB}(t)\approx {\left({\mathbf{\omega }}_B^W\right)}^{\wedge }{\mathbf{R}}_{WB}(t)\Delta t$$

再除以 $\Delta t$：

$$\frac{{\mathbf{R}}_{WB}(t+\Delta t)-{\mathbf{R}}_{WB}(t)}{\Delta t}\approx {\left({\mathbf{\omega }}_B^W\right)}^{\wedge }{\mathbf{R}}_{WB}(t)$$

令 $\Delta t\to 0$，左边就是旋转矩阵的时间导数：

$${\dot{\mathbf{R}}}_{WB}={\left({\mathbf{\omega }}_B^W\right)}^{\wedge }{\mathbf{R}}_{WB}$$

这个式子可以理解为：${\dot{\mathbf{R}}}_{WB}$ 描述旋转矩阵正在怎样变化，而这个变化由当前世界系角速度对应的李代数元素 ${\left({\mathbf{\omega }}_B^W\right)}^{\wedge }$ 生成。

将它乘到 ${\mathbf{r}}_{i,B}$ 上：

$${\dot{\mathbf{R}}}_{WB}{\mathbf{r}}_{i,B}={\left({\mathbf{\omega }}_B^W\right)}^{\wedge }{\mathbf{R}}_{WB}{\mathbf{r}}_{i,B}$$

再利用帽算子和叉乘的等价关系，令：

$$\mathbf{a}={\mathbf{R}}_{WB}{\mathbf{r}}_{i,B}$$

就得到：

$${\dot{\mathbf{R}}}_{WB}{\mathbf{r}}_{i,B}={\mathbf{\omega }}_B^W\times \left({\mathbf{R}}_{WB}{\mathbf{r}}_{i,B}\right)$$

这就是刚体旋转速度公式 $\mathbf{v}=\mathbf{\omega }\times \mathbf{r}$ 的矩阵形式：一个点相对旋转中心的位置是 $\mathbf{r}$，刚体角速度是 $\mathbf{\omega }$，那么这个点仅由旋转产生的线速度就是 $\mathbf{\omega }\times \mathbf{r}$。

需要注意表达坐标系。如果角速度用机体系表达，记作 ${\mathbf{\omega }}_B^B$，则旋转矩阵导数常写成：

$${\dot{\mathbf{R}}}_{WB}={\mathbf{R}}_{WB}{\left({\mathbf{\omega }}_B^B\right)}^{\wedge }$$

如果角速度用世界系表达，记作 ${\mathbf{\omega }}_B^W$，则写成：

$${\dot{\mathbf{R}}}_{WB}={\left({\mathbf{\omega }}_B^W\right)}^{\wedge }{\mathbf{R}}_{WB}$$

二者通过旋转矩阵联系：

$${\mathbf{\omega }}_B^W={\mathbf{R}}_{WB}{\mathbf{\omega }}_B^B$$

本文这里使用的是世界系表达的角速度 ${\mathbf{\omega }}_B^W$，所以采用左乘形式。

令：

$${\mathbf{r}}_{i,W}={\mathbf{R}}_{WB}{\mathbf{r}}_{i,B}$$

其中 ${\mathbf{r}}_{i,W}\in {\mathbb{R}}^3$ 是“从机体原点指向足端”的相对位置向量，但用世界坐标系表达。于是旋转造成的速度可以写成：

$${\mathbf{\omega }}_B^W\times {\mathbf{r}}_{i,W}$$

这个叉乘项的直觉是：如果身体绕某个轴旋转，那么离旋转中心越远的点线速度越大，速度方向垂直于角速度方向和半径方向。

再看 ${\mathbf{R}}_{WB}{\dot{\mathbf{r}}}_{i,B}$。前面已经推导过，关节运动造成的足端相对速度为：

$${\dot{\mathbf{r}}}_{i,B}={\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{q}}}_i$$

其中 ${\mathbf{J}}_i({\mathbf{q}}_i)$ 是第 $i$ 条腿的雅可比矩阵，${\dot{\mathbf{q}}}_i$ 是第 $i$ 条腿的关节角速度向量。因此关节运动对世界系足端速度的贡献是：

$${\mathbf{R}}_{WB}{\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{q}}}_i$$

把三项合在一起，就得到世界坐标系中的足端速度公式。

把这三部分写在世界坐标系中：

$${\mathbf{v}}_{foot,i}^W={\mathbf{v}}_B^W+{\mathbf{\omega }}_B^W\times {\mathbf{r}}_{i,W}+{\mathbf{R}}_{WB}{\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{q}}}_i$$

其中 ${\mathbf{v}}_B^W\in {\mathbb{R}}^3$ 是机体原点在世界坐标系中的线速度，${\mathbf{\omega }}_B^W\in {\mathbb{R}}^3$ 是机体在世界坐标系中的角速度，${\mathbf{r}}_{i,W}\in {\mathbb{R}}^3$ 是足端相对机体原点的位置但用世界坐标系表达，${\mathbf{R}}_{WB}\in {\mathbb{R}}^{3\times 3}$ 是把机体系向量转换到世界系的旋转矩阵。

因为本文前面把滤波器速度主要写成机体系速度 ${\mathbf{v}}_B$，所以我们把上式转回机体系。下面推导机体系下第 $i$ 个足端速度公式。

坐标系转换的方法是左乘 ${\mathbf{R}}_{WB}^T$。这里 ${\mathbf{R}}_{WB}^T$ 是 ${\mathbf{R}}_{WB}$ 的转置，也是从世界系转回机体系的旋转矩阵。于是：

$${\mathbf{v}}_{foot,i}={\mathbf{R}}_{WB}^T{\mathbf{v}}_{foot,i}^W$$

$${\mathbf{v}}_B={\mathbf{R}}_{WB}^T{\mathbf{v}}_B^W$$

$${\mathbf{\omega }}_B={\mathbf{R}}_{WB}^T{\mathbf{\omega }}_B^W$$

$${\mathbf{r}}_{i,B}={\mathbf{R}}_{WB}^T{\mathbf{r}}_{i,W}$$

其中 ${\mathbf{v}}_{foot,i}$ 是足端速度的机体系表达，${\mathbf{v}}_B$ 是机体原点速度的机体系表达，${\mathbf{\omega }}_B$ 是机体角速度的机体系表达。

叉乘项在旋转变换下保持结构（矩阵具有结合律），也就是说：

$${\mathbf{R}}_{WB}^T\left({\mathbf{\omega }}_B^W\times {\mathbf{r}}_{i,W}\right)={\mathbf{\omega }}_B\times {\mathbf{r}}_{i,B}$$

关节速度项本来就是先在机体系中由 ${\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{q}}}_i$ 得到，再通过 ${\mathbf{R}}_{WB}$ 转到世界系。所以转回机体系后：

$${\mathbf{R}}_{WB}^T\left({\mathbf{R}}_{WB}{\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{q}}}_i\right)={\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{q}}}_i$$

因此世界系公式整体转回机体系后，就得到：

$${\mathbf{v}}_{foot,i}={\mathbf{v}}_B+{\mathbf{\omega }}_B\times {\mathbf{r}}_{i,B}+{\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{q}}}_i$$

其中：

• ${\mathbf{v}}_{foot,i}\in {\mathbb{R}}^3$ 是第 $i$ 个足端在机体系表达下的速度。
• ${\mathbf{v}}_B\in {\mathbb{R}}^3$ 是机体原点的线速度，用机体系表达。
• ${\mathbf{\omega }}_B\in {\mathbb{R}}^3$ 是机体角速度，用机体系表达。
• ${\mathbf{\omega }}_B\times {\mathbf{r}}_{i,B}$ 是身体旋转导致足端产生的速度。
• ${\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{q}}}_i$ 是关节运动导致足端相对身体产生的速度。

这个式子是腿式里程计最重要的一步。它说：脚的速度不是只由关节决定，也不是只由身体决定，而是身体平移、身体旋转和关节运动三者叠加的结果。

当足端稳定接触地面时，足端世界速度近似为零。由于旋转坐标变换不会改变“零向量”这个事实，所以机体系中也可以把足端速度近似看成零：

$${\mathbf{v}}_{foot,i}\approx 0$$

把它代入刚才的速度关系：

$$0\approx {\mathbf{v}}_B+{\mathbf{\omega }}_B\times {\mathbf{r}}_{i,B}+{\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{q}}}_i$$

移项后得到由第 $i$ 条支撑腿估计出的机体速度：

$${\mathbf{v}}_{B,i}\approx -{\mathbf{\omega }}_B\times {\mathbf{r}}_{i,B}-{\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{q}}}_i$$

其中 ${\mathbf{v}}_{B,i}\in {\mathbb{R}}^3$ 表示仅由第 $i$ 条腿反推出的机体速度估计。注意它是一个观测值，不等于真实速度本身；它会受到关节编码器噪声、接触判断错误、足端打滑、运动学模型误差等影响。

用一个一维简化例子帮助理解。假设机器人只有前后方向运动，忽略身体旋转，即 ${\mathbf{\omega }}_B=0$。某条支撑腿的关节运动让脚相对身体向后移动，速度为：

$${\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{q}}}_i=\left[\begin{array}{c}-0.4\\ 0\\ 0\end{array}\right]m/s$$

脚踩在地上不动，所以如果从身体上看脚在向后跑，那么从地面上看身体就在向前走。代入公式：

$${\mathbf{v}}_{B,i}\approx -\left[\begin{array}{c}-0.4\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}0.4\\ 0\\ 0\end{array}\right]m/s$$

这就是腿式里程计的直觉：支撑脚相对身体向后运动，身体相对地面向前运动。

真实四足机器人通常同时有多条腿接触地面。每条支撑腿都能给出一个速度估计：

$${\mathbf{v}}_{B,1},{\mathbf{v}}_{B,2},{\mathbf{v}}_{B,3},{\mathbf{v}}_{B,4}$$

其中 ${\mathbf{v}}_{B,1}$ 到 ${\mathbf{v}}_{B,4}$ 分别是四条腿各自给出的机体速度估计。如果每条腿都同样可靠，可以简单平均：

$${\mathbf{z}}_{leg}=\frac{1}{N_c}\sum _{i\in \mathcal{C}}{\mathbf{v}}_{B,i}$$

其中 ${\mathbf{z}}_{leg}\in {\mathbb{R}}^3$ 是最终输出给滤波器的腿式速度观测，$\mathcal{C}$ 是当前被认为处于接触状态的腿集合，$N_c$ 是接触腿数量。

但现实中每条腿并不一样可靠。刚落脚的腿可能发生冲击，快离地的腿可能接触不稳，踩在松软地面上的腿可能打滑。因此更常见的是给每条腿一个接触置信度。令 $s_i\in [0,1]$ 表示第 $i$ 条腿处于稳定支撑状态的概率或权重，$s_i=0$ 表示完全不相信这条腿，$s_i=1$ 表示非常相信这条腿。加权融合后：

$${\mathbf{z}}_{leg}=\frac{{\sum }_i{s}_i{\mathbf{v}}_{B,i}}{{\sum }_i{s}_i}$$

其中 ${\mathbf{v}}_{B,i}$ 是由第 $i$ 条腿反推出的身体速度。

接下来要回答一个问题：$s_i$ 从哪里来？最直接的方法是根据地面反作用力判断。设 $F_{z,i}$ 表示第 $i$ 条腿竖直方向的地面反作用力，$F_{th}$ 表示接触判断阈值。如果 $F_{z,i}$ 大于阈值，就认为脚踩在地上：

$$s_i=\{\begin{array}{ll}1,& F_{z,i}>F_{th}\\ 0,& F_{z,i}\le F_{th}\end{array}$$

其中 $F_{z,i}$ 的单位是牛顿，$F_{th}$ 的单位也是牛顿。比如 $F_{th}=82\mathrm{N}$，如果某条腿测得 $F_{z,i}=120\mathrm{N}$，则认为它在支撑；如果只测得 $F_{z,i}=20\mathrm{N}$，则认为它没有稳定接触。

硬阈值容易在临界点附近抖动。比如 $F_{z,i}$ 在 $80\mathrm{N}$ 和 $85\mathrm{N}$ 之间来回跳，接触状态就会频繁切换。为了更稳定，可以使用迟滞判断：进入接触用较高阈值 $F_{high}$，离开接触用较低阈值 $F_{low}$，并要求状态持续一小段时间才切换。也可以使用逻辑回归，把力、关节速度等信息转成平滑概率：

$$s_i=\frac{1}{1+\exp (-({\beta }_0+{\beta }_1{F}_{z,i}))}$$

其中 $\exp (\cdot )$ 是指数函数，${\beta }_0$ 和 ${\beta }_1$ 是通过数据拟合得到的参数。这个式子输出总在 $0$ 到 $1$ 之间，适合当作接触概率。

现在再看一个多腿加权例子。假设四条腿给出的速度估计分别为：

$${\mathbf{v}}_{B,1}=\left[\begin{array}{c}0.42\\ 0.02\\ 0.00\end{array}\right],{\mathbf{v}}_{B,2}=\left[\begin{array}{c}0.39\\ 0.01\\ 0.01\end{array}\right],{\mathbf{v}}_{B,3}=\left[\begin{array}{c}0.90\\ -0.20\\ 0.10\end{array}\right],{\mathbf{v}}_{B,4}=\left[\begin{array}{c}0.41\\ 0.00\\ 0.00\end{array}\right]$$

单位都是 $m/s$。其中第三条腿的估计明显异常，可能是打滑或接触判断不稳。假设接触权重为：

$$s_1=1.0,s_2=0.8,s_3=0.1,s_4=1.0$$

则融合速度为：

$${\mathbf{z}}_{leg}=\frac{1.0{\mathbf{v}}_{B,1}+0.8{\mathbf{v}}_{B,2}+0.1{\mathbf{v}}_{B,3}+1.0{\mathbf{v}}_{B,4}}{1.0+0.8+0.1+1.0}$$

计算后大约为：

$${\mathbf{z}}_{leg}\approx \left[\begin{array}{c}0.425\\ 0.003\\ 0.007\end{array}\right]m/s$$

可以看到，第三条腿虽然给出了离谱速度，但因为权重只有 $0.1$，它对最终结果影响很小。这就是接触概率或接触权重的意义：不是盲目相信每一条腿，而是根据接触可信度决定每条腿说话的分量。

腿式里程计最终进入滤波器的是速度观测：

$${\mathbf{z}}_{leg}={\mathbf{v}}_B+{\mathbf{r}}_{leg}$$

其中 ${\mathbf{r}}_{leg}\in {\mathbb{R}}^3$ 是腿式速度观测噪声。这个式子表达的是：腿式里程计观测到的速度 ${\mathbf{z}}_{leg}$ 等于真实机体速度 ${\mathbf{v}}_B$ 加上一些误差。

观测协方差可写为：

$${\mathbf{R}}_{leg}=\mathrm{diag}({\sigma }_{vx}^2,{\sigma }_{vy}^2,{\sigma }_{vz}^2)$$

其中 ${\mathbf{R}}_{leg}\in {\mathbb{R}}^{3\times 3}$ 是腿式里程计观测协方差，${\sigma }_{vx}$、${\sigma }_{vy}$、${\sigma }_{vz}$ 分别是机体系 $x$、$y$、$z$ 方向速度观测标准差。标准差越大，表示越不相信腿式里程计。

例如：

$${\sigma }_{vx}=0.05,{\sigma }_{vy}=0.05,{\sigma }_{vz}=0.15$$

表示系统比较相信前后和左右速度，但不太相信竖直速度。原因很直观：腿式机器人在崎岖地面上行走时，足端接触高度、地形起伏和冲击会让竖直方向速度更难估计。

腿式里程计解决的问题是：限制 IMU 的速度漂移，尤其在视觉不稳定或视觉频率较低时，提供强有力的短时速度约束。