专业网站设计制作服务定制化网站建设

设机器人当前位姿（位置和姿态）表示为齐次变换矩阵 ${\mathbf{T}}_{current}$，该矩阵将局部坐标系（机器人自身坐标系）的点坐标映射到世界坐标系：
$${\mathbf{P}}_{world}={\mathbf{T}}_{current}\cdot {\mathbf{P}}_{local}$$其中，${\mathbf{P}}_{local}$ 是点在局部坐标系中的坐标，${\mathbf{P}}_{world}$ 是其在世界坐标系中的坐标。

1. 全局坐标系下机器人位姿更新的左乘法则​

若变换基于全局坐标系（例如，沿世界坐标系X轴平移3米），则增量变换 $\Delta \mathbf{T}$ 的定义需满足：$\Delta \mathbf{T}$ 的旋转轴/平移方向由​​世界坐标系的轴向​​决定，与机器人当前姿态无关。

因此，先将局部坐标 ${\mathbf{P}}_{\text{local}}$ 通过 ${\mathbf{T}}_{current}$ 映射到世界坐标系，得 ${\mathbf{P}}_{world}$。再在世界坐标系中应用全局变换 $\Delta \mathbf{T}$，得到新坐标 $\Delta \mathbf{T}\cdot {\mathbf{P}}_{world}$。即有：
$$\Delta \mathbf{T}\cdot ({\mathbf{T}}_{current}\cdot {\mathbf{P}}_{\text{local}})=(\Delta \mathbf{T}\cdot {\mathbf{T}}_{current})\cdot {\mathbf{P}}_{\text{local}}\Rightarrow {\mathbf{T}}_{new}=\Delta \mathbf{T}\cdot {\mathbf{T}}_{current}$$此即左乘公式​​，表明新位姿是全局变换与当前位姿的左乘组合。

2. 局部坐标系下机器人位姿增量更新的右乘法则​

如果机器人需执行一个在当前局部坐标系（例如机器人自身坐标系）中描述的运动变换 $\Delta \mathbf{T}$。例如：

• 沿自身 X 轴移动 3 米：$\Delta \mathbf{T}=\left[\begin{array}{cccc}1& 0& 0& 3\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$
• 绕自身 Z 轴旋转 90°：$\Delta \mathbf{T}=\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

则执行该运动后的机器人新位姿 ${\mathbf{T}}_{\text{new}}$ 为当前位姿 ${\mathbf{T}}_{\text{current}}$ 右乘该增量变换：
$${\mathbf{T}}_{\text{new}}={\mathbf{T}}_{\text{current}}\Delta \mathbf{T}$$对于在运动前的局部坐标系中定义的任意点 ${\mathbf{P}}_{\text{local}}$，应用变换 ${\mathbf{T}}_{\text{new}}$ 将其映射到世界坐标系的结果，等同于先在原局部坐标系中应用增量变换 $\Delta \mathbf{T}$ 得到 $\Delta \mathbf{T}{\mathbf{P}}_{\text{local}}$，然后将该结果再通过原当前位姿 ${\mathbf{T}}_{\text{current}}$ 映射到世界坐标系。即：
$${\mathbf{T}}_{\text{new}}{\mathbf{P}}_{\text{local}}={\mathbf{T}}_{\text{current}}(\Delta \mathbf{T}{\mathbf{P}}_{\text{local}})$$

3. 相对位姿的计算

相对位姿描述了两个坐标系之间的变换关系，包括旋转和平移，是机器人导航、三维重建、多传感器融合等领域的核心问题。

设 ${\mathbf{T}}_1$ 和 ${\mathbf{T}}_2$ 分别表示相机在时刻 $t_1$ 和 $t_2$ 位姿变换矩阵。现从两个角度计算 ${\mathbf{T}}_1$ 到 ${\mathbf{T}}_2$ 的相对位姿 $\Delta \mathbf{T}$。

3.1. 基于世界坐标系（全局变换）

相对位姿 $\Delta {\mathbf{T}}_{\text{global}}$ 在世界坐标系中定义，其数学表达为：
$$\Delta {\mathbf{T}}_{\text{global}}={\mathbf{T}}_2^{-1}{\mathbf{T}}_1$$其中，$\Delta {\mathbf{T}}_{\text{global}}$ 的平移和旋转方向由世界坐标系的轴向决定。
推导如下：
世界坐标系中的点 ${\mathbf{P}}_w$ 在 $t_1$ 和 $t_2$ 时刻相机坐标系中的位置分别为 ${\mathbf{p}}_1$ 和 ${\mathbf{p}}_2$，根据位姿定义：
$${\mathbf{P}}_w={\mathbf{T}}_1{\mathbf{p}}_1={\mathbf{T}}_2{\mathbf{p}}_2$$则有：
$${\mathbf{p}}_2={\mathbf{T}}_2^{-1}{\mathbf{P}}_w={\mathbf{T}}_2^{-1}{\mathbf{T}}_1{\mathbf{p}}_1\Rightarrow \Delta {\mathbf{T}}_{\text{global}}={\mathbf{T}}_2^{-1}{\mathbf{T}}_1$$设 ${\mathbf{T}}_1=\left[\begin{array}{cc}{\mathbf{R}}_1& {\mathbf{t}}_1\\ 0^T& 1\end{array}\right]$，${\mathbf{T}}_2=\left[\begin{array}{cc}{\mathbf{R}}_2& {\mathbf{t}}_2\\ 0^T& 1\end{array}\right]$，则有：
$$\Delta {\mathbf{T}}_{\text{global}}={\mathbf{T}}_2^{-1}{\mathbf{T}}_1=\left[\begin{array}{cc}{\mathbf{R}}_2^T& -{\mathbf{R}}_2^T{\mathbf{t}}_2\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{\mathbf{R}}_1& {\mathbf{t}}_1\\ 0^T& 1\end{array}\right]=\left[\begin{array}{cc}{\mathbf{R}}_2^T{\mathbf{R}}_1& {\mathbf{R}}_2^T{\mathbf{t}}_1-{\mathbf{R}}_2^T{\mathbf{t}}_2\\ 0^T& 1\end{array}\right]$$

3.2. 基于 $t_1$ 时刻相机的局部坐标系（局部变换）

相对位姿 $\Delta {\mathbf{T}}_{\text{local}}$ 在 $t_1$ 时刻相机自身的坐标系中定义，其数学表达为：
$$\Delta {\mathbf{T}}_{\text{local}}={\mathbf{T}}_1^{-1}\cdot {\mathbf{T}}_2$$其中，$\Delta {\mathbf{T}}_{\text{local}}$ 的平移和旋转方向由 $t_1$ 时刻相机的朝向决定。
上述公式根据局部坐标系下机器人位姿增量更新的右乘法则​易得，且有：
$$\Delta {\mathbf{T}}_{\text{local}}={\mathbf{T}}_1^{-1}\cdot {\mathbf{T}}_2=\left[\begin{array}{cc}{\mathbf{R}}_1^T& -{\mathbf{R}}_1^T{\mathbf{t}}_1\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{\mathbf{R}}_2& {\mathbf{t}}_2\\ 0^T& 1\end{array}\right]=\left[\begin{array}{cc}{\mathbf{R}}_1^T{\mathbf{R}}_2& {\mathbf{R}}_1^T{\mathbf{t}}_2-{\mathbf{R}}_1^T{\mathbf{t}}_1\\ 0^T& 1\end{array}\right]$$

3.3. 两者区别

角度	数学公式	坐标系依赖	乘法顺序
世界坐标系（全局）	$\Delta {\mathbf{T}}_{\text{global}}={\mathbf{T}}_2^{-1}{\mathbf{T}}_1$	世界坐标系轴向固定	左乘
$t_1$ 相机坐标系（局部）	$\Delta {\mathbf{T}}_{\text{local}}={\mathbf{T}}_1^{-1}{\mathbf{T}}_2$	依赖 $t_1$ 时刻相机自身朝向	右乘