基本原理介绍

基础定义

坐标系（Coordinate System）： 用一个原点和一组相互正交、带方向的基轴（常为 X、Y、Z）来定义的参考框架，用于在特定单位下唯一表示空间中点的位置与方向。

• 2D 含两轴（X、Y），3D 含三轴（X、Y、Z）；
• 需约定手性（常用右手系）与单位（mm、m 等）；
• 相对同一坐标系给出的向量和姿态具有可比性与可组合性。

常见坐标变换类型

平移（translation）: poseAdd()

• 含义：把同一个点在同一坐标系里挪位置，方向不变。
• 公式（点从B系表达变到A系时带平移）：

$$ \mathbf{p}{\mathrm{A}} = \mathbf{R}{\mathrm{A}}^{\mathrm{B}},\mathbf{p}{\mathrm{B}}+ \mathbf{t}{\mathrm{A}}^{\mathrm{B}} $$

这里 $\mathbf t_A^B=[t_x,t_y,t_z]^T$ 是 在A坐标系中 表达的平移向量。

• $\mathbf R$负责“转方向”，$\mathbf t$负责“加位置”。只有平移时就 $\mathbf p_A=\mathbf p_B+\mathbf t$。

旋转（rotation）: poseRotation()

• 含义：在同一坐标系里转方向，位置相对原点绕圈改变。

• 公式：$\mathbf p'_A=\mathbf R,\mathbf p_A$，$\mathbf R\in SO(3)$，满足 $\mathbf R^T\mathbf R=\mathbf I$，$\det\mathbf R=+1$。

• 常用表示法（3D）：

  • 欧拉角 ZYX（yaw-pitch-roll）：$\mathbf R=\mathbf R_z(\psi)\mathbf R_y(\theta)\mathbf R_x(\phi)$
  • 轴—角（绕单位轴 $\hat\omega$ 旋转角 $\theta$）：罗德里格公式 $\mathbf{R}=\mathbf{I}+\sin\theta,\lbrack\hat{\omega}\rbrack_\times+(1-\cos\theta),(\lbrack\hat{\omega}\rbrack_\times)^2$

平移+旋转一起（位姿/齐次变换）: poseTrans()

把旋转和平移打包成一个 4×4：

$$ \mathbf T= \begin{bmatrix} \mathbf R & \mathbf t\ \mathbf 0^T & 1 \end{bmatrix},\quad \begin{bmatrix}\mathbf p_A\1\end{bmatrix}=\mathbf T_A^B \begin{bmatrix}\mathbf p_B\1\end{bmatrix} $$

• 组合顺序：右乘离点更近的那个变换。 例如先在TCP自身系里偏移/旋转（$\Delta \mathbf T_{tcp}$），再放回基座：

$$ \mathbf{T}{\mathrm{base}}^{\mathrm{tcp,new}} = \mathbf{T}{\mathrm{base}}^{\mathrm{tcp}} ,\Delta \mathbf{T}_{\mathrm{tcp}} $$

• 逆变换 poseInverse()： 把“从 B 到 A 的位姿”转换到“从 A 到 B 的位姿”,

$$ \mathbf{T}^{-1} = \begin{pmatrix} \mathbf{R}^{\mathsf{T}} & -,\mathbf{R}^{\mathsf{T}}\mathbf{t}\ \mathbf{0} & 1 \end{pmatrix} $$

应用示例

1. 点位偏移

示例程序包

1. 方向-直到节点实现点位偏移

   

2. 0.31分支特性：相对于变量或路点实现点位偏移

   

2. 坐标系偏移

1. 通过自定义坐标系实现偏移

   

2. 坐标系 + 点位偏移