局部坐标系下相对位置的计算方法辨析

在考虑人形机器人追踪参考运动时，经常需要计算机器人与参考运动之间的“局部位置差”。一说到局部位置计算，简单粗暴的理解是：只要有两者做差和坐标系变化的操作就行。然而，不同的计算方法有着很大的区别。实际上，“局部位置差”应该表达为机器人坐标系下参考位置与当前位置之间的距离。

符号说明

关键点的当前位置：$p_{c}\in\mathbb{R}^3$
根关节的当前位置：$p_{co}\in\mathbb{R}^3$
根关节的当前四元数：$q_{co}\in\mathbb{R}^4$
关键点的参考位置：$p_{r}\in\mathbb{R}^3$
根关节的参考位置：$p_{ro}\in\mathbb{R}^3$
根关节的参考四元数：$q_{ro}\in\mathbb{R}^4$
四元数乘法：$\otimes$

以上符号均是在世界坐标系下的表达，局部坐标系是以人形机器人根关节为原点！

计算方法一

先计算两者全局坐标之差

$$
\begin{aligned}
p_{d} = p_{c} - p_{r}
\end{aligned}\tag{1}
$$

再转换到机器人当前坐标系

$$
\begin{aligned}
p_{ld}=q_{co}^{-1}\otimes p_{d}\otimes q_{co}
\end{aligned}\tag{2}
$$

该计算方法中式(1)表达的是当前位置与参考位置在全局坐标下的位置之差，式(2)表达的是把全局位置转到机器人当前坐标系下的位置表达。可以理解为一个人去看另一个人相对于自己的位置。

注：四元数旋转计算公式为夹心乘积

计算方法二

先把两个位置在各自坐标系下表达
$$
\begin{aligned}
p_{lc}=q_{co}^{-1}\otimes p_{c}\otimes q_{co} \\
p_{lr}=q_{ro}^{-1}\otimes p_{r}\otimes q_{ro}
\end{aligned}\tag{3}
$$

$$
\begin{aligned}
p_{ld}=q_{lc} - q_{lr}
\end{aligned}\tag{4}
$$

这个方法计算是错误的！因为直接把人形机器人关键点在全局坐标下的位置，用根关节的四元数变换到局部坐标系下，其物理意义不是局部坐标系下的位置。

计算方法三

若局部坐标系是以根关节为原点，先把人形机器人关键点的位置与根的位置相减
$$
\begin{aligned}
p_{cd}=p_{c}-p_{co} \\
p_{rd}=p_{r}-p_{ro}
\end{aligned}\tag{3}
$$

然而，在各自坐标系下表达
$$
\begin{aligned}
p_{lc}=q_{co}^{-1}\otimes p_{cd}\otimes q_{co} \\
p_{lr}=q_{ro}^{-1}\otimes p_{rd}\otimes q_{ro}
\end{aligned}\tag{3}
$$

最后，两者再相减

$$
\begin{aligned}
p_{ld}=q_{lc} - q_{lr}
\end{aligned}\tag{4}
$$

该计算方法表达的就是：各自坐标下位置的相减，没有视角的说法。可以理解为两个人各自报出手臂相对pelivs的位置，然后再进行相减。

总结

由此可见，AI与机器人的融合，不仅要懂AI，也要明白计算的物理意义。不然，就是“差之毫厘，缪以千里”。