机器人学——1.0-位置与姿态概述
机器人和计算机视觉中的一个基本要求是能够表示物体在环境中的位置和方向。这些物体包括机器人、摄像机、工件、障碍物和路径。
空间中的点是数学中一个熟悉的概念，它可以被描述为一个坐标向量，也被称为一个约束向量，如图 (a) 所示。向量表示点相对于某个参考坐标系的位移。一个坐标系或笛卡儿坐标系统，是由一组正交轴构成的，这些轴相交于一个被称为原点的点。
更多时候我们需要考虑组成物体的一组点。我们认为物体是刚性的，构成它的点相对于物体坐标系保持固定的相对位置，如图 (b) 所示。然而我们表示物体位置和方向时并不是描述其上单独的点，而是用该物体坐标系的位置和方向来描述。坐标系有自己的标记，比如这里的 { B } \{B\} { B} ，其坐标轴 x B x_B xB 和 y B y_B yB 采用坐标系的标记作为其下标。
坐标系的位置和方向总称为位姿，图形上表示为一组坐标轴。相对于一个参考坐标系的某个坐标系的相对位姿用符号 ξ \xi ξ 表示。下图显示了两个坐标系 { A } \{A\} { A} 、 { B } \{B\} { B} ，以及 { B } \{B\} { B} 相对于 { A } \{A\} { A} 的相对位姿 A ξ B ^A\xi_B AξB 。前面的上标表示参考坐标系，下标表示被描述的坐标系。我们也可以认为 A ξ B ^A\xi_B AξB 描述了一组动作——对 { A } \{A\} { A} 施加平移和旋转使它转化为 { B } \{B\} { B} 。如果没有初始上标，我们默认位姿的变化是相对于用 O O O 表示的世界坐标系的。在图中的 P P P 点可用任何一个坐标系表示。用式子表示为
A p = A ξ B ⋅ B p (1) \tag{1} ^Ap={}^A\xi_B \cdot {}^Bp Ap=AξB⋅Bp(1)
等式右侧表示从 { A } \{A\} { A} 到 { B } \{B\} { B} 然后到 P P P 的动作。运算符“ ⋅ \cdot ⋅”将一个向量转换为一个新的向量，它们是用一个不同的坐标系来描述的相同点。
相对位姿一个重要的特点是它们可以被合成或组合。如下图所示情况为例，如果一个坐标系可以被其他坐标系用相对位姿描述，那么它们的关系可以记为
A ξ C = A ξ B ⊕ B ξ C ^A\xi_C={}^A\xi_B \oplus{} ^B\xi_C AξC=AξB⊕BξC
用语言可以表述为， { C } \{C\} { C} 相对于 { A } \{A\} { A} 的位姿可由 { B } \{B\} { B} 相对于 { A } \{A\} { A} 的位姿和 { C } \{C\} { C} 相对于 { B } \{B\} { B} 的位姿合成得到。我们利用运算符“ ⊕ \oplus ⊕”表示相对位姿的合成。此时，点 P P P 可以被表示为
A p = ( A ξ B ⊕ B ξ C ) ⋅ C p ^Ap= \left( ^A\xi_B \oplus{} ^B\xi_C \right) \cdot {}^Cp Ap=(AξB⊕BξC)⋅Cp
在MATLAB中，“ ξ \xi ξ”、“ ⋅ \cdot ⋅”、“ ⊕ \oplus ⊕”，这些抽象的符号都可以被实现为标准的数学对象和运算符。
在前面的例子中主要讨论了二维坐标系的情况，能适用于一大类机器人的问题，特别是在平面世界里作业的移动机器人。对于其他问题，我们需要用三维坐标系来表示三维世界中的物体，如飞行机器人、水下机器人的位姿或者机器人手臂夹持的工具末端。
在相对位姿的合成中，我们可以通过确认运算符 ⊕ \oplus ⊕两侧的上标和下标字母匹配来检验相对坐标系是否正确。我们可以去掉中间的上、下标字母，只留下左右两头加圈显示的上、下标字母。
下图给出了一个更复杂的三维变换的例子，其中每个实体上都固接了一个三维坐标系,并表示出了它们之间的相对位姿。固定摄像头从固定的视角观察物体,并估计物体相对于摄像头本身的位姿。另一个摄像头是不固定的，它固接到机器人上，与机器人有固定的相对位姿，并估计出物体相对于该摄像头的位姿。
另一个表示空间关系的方式是有向图，如下图所示。图中的每个节点代表一个位姿，每条边代表一个相对位姿。 从 X X X 到 Y Y Y 的箭头记作 X ξ Y ^X\xi_Y XξY，表示 Y Y Y 相对于 X X X 的位姿。前面我们提到过可以用运算符 ⊕ \oplus ⊕ 来复合成相对位姿，这里可以用它写出一些空间关系：
ξ F ⊕ F ξ B = ξ R ⊕ R ξ C ⊕ C ξ B \xi_F \oplus{}^F\xi_B=\xi_R \oplus{}^R\xi_C \oplus{}^C\xi_B ξF⊕FξB=ξR⊕RξC⊕CξB
ξ F ⊕ F ξ R = O ξ R \xi_F \oplus {}^F\xi_R={}^O\xi_R ξF⊕FξR=OξR
每个方程表示了图中的一个闭环。方程等号两侧的每一边各表示一条网络的通路，即一组按照从头到尾顺序连接的边（箭头线）。等式两边的起始节点和结束节点必须相同。
位姿表示的一个非常有用的属性是其代数运算能力。以上面第二个闭环方程为例，它表示机器人的位姿等同于两个相对位姿的合成：从世界坐标系到固定摄像机的相对位姿，以及从固定摄像机到机器人的相对位姿。
在数学对象术语中，位姿组构成了一个群，即一系列对象的集合，它支持一种运算结果仍属于该群的关联二元运算符（复合算子），一种求逆运算和一种单位元素。在这里，该群是二维或三维的特殊欧几里得群，分别记作 S E ( 2 ) SE(2) SE(2)或 S E ( 3 ) SE(3) SE(3)。
我们可以通过在等式两边同时加上一个逆 ξ F \xi_F ξF 来消除 ξ F \xi_F ξF，记作
⊖ ξ F ⊕ ξ F ⊕ F ξ R = ⊖ ξ F ⊕ ξ R F ξ R = ⊖ ξ F ⊕ ξ R \begin{array}{r} \ominus \xi_F\oplus\xi_F\oplus{}^F\xi_R=\ominus\xi_F\oplus\xi_R \\[1em] ^F\xi_R=\ominus\xi_F\oplus\xi_R \end{array} ⊖ξF⊕ξF⊕FξR=⊖ξF⊕ξRFξR=⊖ξF⊕ξR
其结果就是机器人相对于固定摄像机的位姿。
下面是几个代数运算规则：
ξ ⊕ 0 = ξ , ξ ⊖ 0 = ξ ξ ⊖ ξ = 0 , ⊖ ξ ⊕ ξ = 0 \begin{array}{rr} \xi\oplus 0=\xi,& \xi \ominus 0 = \xi \\ \xi \ominus \xi = 0,& \ominus \xi \oplus \xi = 0 \end{array} ξ⊕0=ξ,ξ⊖ξ=0,ξ⊖0=ξ⊖ξ⊕ξ=0其中 0 0 0 表示一个零相对位姿。一个位姿可以有逆位姿：
⊖ X ξ Y = Y ξ X \ominus {}^X\xi_Y={}^Y\xi_X ⊖XξY=YξX该逆位姿在图中可表示为从 Y Y Y 到 X X X 的一条箭头线。相对位姿可以被复合或合成，之前已经说明，不再赘述。
需要注意的是位姿的代数运算规则不同于一般的代数运算规则，位姿复合运算中各个项的位置不能交换：
ξ 1 ⊕ ξ 2 ≠ ξ 2 ⊕ ξ 1 \xi_1 \oplus \xi_2 \ne \xi_2 \oplus \xi_1 ξ1⊕ξ2=ξ2⊕ξ1 ξ 1 ⊕ ξ 2 = 0 \xi_1\oplus\xi_2=0 ξ1⊕ξ2=0时才可以互换位置。可以用相对位姿将一个点从一个坐标系中的一个向量转换为另一个坐标系中的另一个向量，这里也不再赘述。
那么，什么是 ξ \xi ξ ?它可以是支持上述代数运算的任何数学对象，并适用于我们身边常见的问题。它还取决于我们讨论的是二维还是三维的问题。以后我们将讨论一些对象，其中包括向量以及一些更奇特的数学对象，如齐次变换矩阵、正交旋转矩阵和四元数等。所有这些数学对象都适用于MATLAB的编程环境。
回顾
- 一个点用坐标向量来表示，它代表该点在参考坐标系中的位移；
- 一个刚体可以由其上的一组点代表，该刚体可以用单独一个坐标系描述，并且组成它的点可以用它们在该坐标系中的位移来表示；
- 一个物体在坐标系中的位置和方向称为它的位姿；
- 一个相对位姿表示一个坐标系相对于另一个坐标系的位姿，记作代数变量 ξ \xi ξ；
- 一个点可以用不同坐标系中的不同坐标向量来描述，向量之间通过坐标系相对位姿来转换，其运算符为“ ⋅ \cdot ⋅”；
- 用相对位姿写成的代数表达式是可以进行代数运算的。