通过四元数计算机器人重力投影：从世界坐标系到机体坐标系

AAA苹果批发老爵爷牛顿
9 阅读2分钟

通过四元数计算机器人重力投影：从世界坐标系到机体坐标系

一、什么是重力投影

“机体重力投影”这个词，简单说就是：

看看重力在机器人自己身体坐标系里，朝哪个方向。

我们都知道，现实里重力永远是竖直向下的。
但是机器人会转身、前倾、侧倾，所以虽然重力在世界里一直向下，在机器人自己看来，重力方向会变。

所以：

  • 机器人站直时，重力在它眼里就是“正下方”
  • 机器人前倾时，重力在它眼里就会“有点朝前下方”
  • 机器人左歪时，重力在它眼里就会“有点朝左下方”

这就是“机体重力投影”。

二、为什么强化学习里要用它

因为机器人要学会保持平衡、走路、跑步、跳跃，就必须知道自己有没有歪。

而“机体重力投影”正好能告诉它：

  • 我是不是前倾了
  • 我是不是后仰了
  • 我是不是左歪了
  • 我是不是右歪了
  • 我是不是快翻了

所以这个量本质上就是：

一种表示机器人姿态是否倾斜的观测量。

三、先分清两个坐标系

1. 世界坐标系

这是固定不动的坐标系。比如：

  • xx：前后
  • yy：左右
  • zz：上下

在世界坐标系里，重力永远是：

gw=[0, 0, −1]g_w = [0,\ 0,\ -1]

或者也有人写成：

gw=[0, 0, −9.81]g_w = [0,\ 0,\ -9.81]

区别只是一个是单位方向，一个是真实加速度大小。

2. 机体坐标系

这是绑在机器人身上的坐标系。通常可以理解成：

  • xx：机器人前方
  • yy：机器人左/右方
  • zz：机器人上方

因为机器人会动，所以机体坐标系也会跟着转。

四、四元数是什么

四元数你可以先把它理解成：

一种专门用来描述三维旋转的数学工具。

通常写成：

q=[w, x, y, z]q = [w,\ x,\ y,\ z]

其中：

  • ww：实部
  • x,y,zx, y, z：虚部

它的作用就是描述机器人当前“朝向什么方向”。

比如机器人：

  • 有没有转身
  • 有没有前倾
  • 有没有侧倾

这些姿态信息都可以用四元数表示。

五、四元数乘法是什么

四元数乘法就是两个四元数之间的一种特殊乘法，记作：

q1⊗q2q_1 \otimes q_2

注意：

四元数乘法不是普通乘法，而且一般不满足交换律。

也就是说：

q1⊗q2≠q2⊗q1q_1 \otimes q_2 \neq q_2 \otimes q_1

这一点很重要，因为旋转的先后顺序不同，结果通常也不同。

1. 四元数乘法公式

设两个四元数分别为：

q1=[w1, x1, y1, z1]q_1 = [w_1,\ x_1,\ y_1,\ z_1]
q2=[w2, x2, y2, z2]q_2 = [w_2,\ x_2,\ y_2,\ z_2]

那么它们的乘法结果是：

q1⊗q2=[w1w2−x1x2−y1y2−z1z2w1x2+x1w2+y1z2−z1y2w1y2−x1z2+y1w2+z1x2w1z2+x1y2−y1x2+z1w2]q_1 \otimes q_2 = \begin{bmatrix} w_1 w_2 - x_1 x_2 - y_1 y_2 - z_1 z_2 \\ w_1 x_2 + x_1 w_2 + y_1 z_2 - z_1 y_2 \\ w_1 y_2 - x_1 z_2 + y_1 w_2 + z_1 x_2 \\ w_1 z_2 + x_1 y_2 - y_1 x_2 + z_1 w_2 \end{bmatrix}

这个公式看起来有点长，但你先不用硬背，只要知道：

它是四元数做旋转运算时最基本的运算规则。

2. 四元数乘法的直观理解

你可以把它理解成：

把两个旋转“接起来”

例如：

  • q1q_1：表示先转一次
  • q2q_2：表示再转一次

那 q1⊗q2q_1 \otimes q_2 表示一种组合后的旋转。

所以四元数乘法本质上是在做：

旋转的叠加

3. 为什么这里一定会用到四元数乘法

因为我们想把“世界系里的重力向量”变成“机体系里的重力向量”，本质上就是在做旋转变换。

而四元数进行旋转时，标准形式就是：

q⊗v⊗q−1q \otimes v \otimes q^{-1}

或者：

q−1⊗v⊗qq^{-1} \otimes v \otimes q

所以只要你看到“用四元数旋转一个向量”，就一定会遇到四元数乘法。

六、为什么向量也能参与四元数乘法

普通三维向量不能直接和四元数算乘法，所以要先把它写成“纯虚四元数”。

例如向量：

v=[vx, vy, vz]v = [v_x,\ v_y,\ v_z]

要写成：

v~=[0, vx, vy, vz]\tilde{v} = [0,\ v_x,\ v_y,\ v_z]

也就是说：

  • 前面的 00 是实部
  • 后面三个数才是向量本身

比如世界系重力：

gw=[0, 0, −1]g_w = [0,\ 0,\ -1]

要先写成：

g~w=[0, 0, 0, −1]\tilde{g}_w = [0,\ 0,\ 0,\ -1]

这样它才能参与四元数乘法。

七、四元数的逆是什么

四元数旋转里常用到逆四元数：

q−1q^{-1}

如果 qq 是单位四元数，那么它的逆很简单：

q−1=[w, −x, −y, −z]q^{-1} = [w,\ -x,\ -y,\ -z]

也就是：

实部不变，虚部变号。

这个操作也叫四元数共轭。

八、机体重力投影到底要算什么

我们本来知道的是：

gw=[0, 0, −1]g_w = [0,\ 0,\ -1]

这是世界坐标系里的重力方向。

但强化学习更想知道的是：

gbg_b

也就是：

重力在机器人自己身体坐标系里的表达。

这个 gbg_b 就叫“机体重力投影”。

九、四元数下，重力投影怎么计算

如果四元数 qq 表示的是：

机体坐标系 →\rightarrow 世界坐标系

那么机体重力投影的公式就是：

gb=q−1⊗gw⊗qg_b = q^{-1} \otimes g_w \otimes q

这里的 gwg_w 其实是写成纯虚四元数后的形式。

这句话怎么理解

你可以直接记住：

已知重力在世界系里的方向，要变成机体系里的方向，就要做一次逆旋转。

因为：

  • 现在已知的是世界里的重力
  • 想得到的是机体里的重力
  • 所以要从“世界”转回“机体”

这就是为什么要用：

q−1q^{-1}

十、这个公式每一部分是什么意思

gb=q−1⊗gw⊗qg_b = q^{-1} \otimes g_w \otimes q

含义如下：

  • gwg_w：世界系里的重力
  • qq：机器人姿态四元数
  • q−1q^{-1}：四元数的逆，用来做反向旋转
  • ⊗\otimes：四元数乘法

最终得到的：

gb=[gx, gy, gz]g_b = [g_x,\ g_y,\ g_z]

表示：

  • gxg_x：重力在机器人前后方向上的分量
  • gyg_y：重力在机器人左右方向上的分量
  • gzg_z：重力在机器人上下方向上的分量

十一、最直观的理解方法

你可以把这个过程想成：

世界里看

重力永远向下。

机器人自己看

如果自己歪了，那么“向下”这个方向在自己眼里就不再是纯粹的身体负 zz 方向了。

比如：

1. 机器人站得很正

那么：

gb≈[0, 0, −1]g_b \approx [0,\ 0,\ -1]

表示重力就是朝机器人身体正下方。

2. 机器人向前倾

那么：

gb≈[0.3, 0, −0.95]g_b \approx [0.3,\ 0,\ -0.95]

表示重力在机体前方也有一点分量。
也就是说，机器人“感觉自己前倾了”。

3. 机器人向左歪

那么：

gb≈[0, 0.2, −0.98]g_b \approx [0,\ 0.2,\ -0.98]

表示重力在左右方向上也有分量。
也就是说，机器人“感觉自己侧倾了”。

avatar
文章
阅读
粉丝