機器人學——1.0-位置與姿態概述

機器人和計算機視覺中的一個基本要求是能夠表示物體在環境中的位置和方向。這些物體包括機器人、攝像機、工件、障礙物和路徑。

空間中的點是數學中一個熟悉的概念，它可以被描述為一個坐標向量，也被稱為一個約束向量，如圖 (a) 所示。向量表示點相對於某個參考坐標系的位移。一個坐標系或笛卡兒坐標系統，是由一組正交軸構成的，這些軸相交於一個被稱為原點的點。

更多時候我們需要考慮組成物體的一組點。我們認為物體是剛性的，構成它的點相對於物體坐標系保持固定的相對位置，如圖 (b) 所示。然而我們表示物體位置和方向時並不是描述其上單獨的點，而是用該物體坐標系的位置和方向來描述。坐標系有自己的標記，比如這里的 $\{B\}$ ，其坐標軸 $x_B$ 和 $y_B$ 采用坐標系的標記作為其下標。

坐標系的位置和方向總稱為位姿，圖形上表示為一組坐標軸。相對於一個參考坐標系的某個坐標系的相對位姿用符號 $\xi$ 表示。下圖顯示了兩個坐標系 $\{A\}$ 、 $\{B\}$ ，以及 $\{B\}$ 相對於 $\{A\}$ 的相對位姿 $^A\xi_B$ 。前面的上標表示參考坐標系，下標表示被描述的坐標系。我們也可以認為 $^A\xi_B$ 描述了一組動作——對 $\{A\}$ 施加平移和旋轉使它轉化為 $\{B\}$ 。如果沒有初始上標，我們默認位姿的變化是相對於用 $O$ 表示的世界坐標系的。在圖中的 $P$ 點可用任何一個坐標系表示。用式子表示為
$\tag{1} ^Ap={}^A\xi_B \cdot {}^Bp$
等式右側表示從 $\{A\}$ 到 $\{B\}$ 然后到 $P$ 的動作。運算符“ $\cdot$”將一個向量轉換為一個新的向量，它們是用一個不同的坐標系來描述的相同點。

相對位姿一個重要的特點是它們可以被合成或組合。如下圖所示情況為例，如果一個坐標系可以被其他坐標系用相對位姿描述，那么它們的關系可以記為
$^A\xi_C={}^A\xi_B \oplus{} ^B\xi_C$

用語言可以表述為， $\{C\}$ 相對於 $\{A\}$ 的位姿可由 $\{B\}$ 相對於 $\{A\}$ 的位姿和 $\{C\}$ 相對於 $\{B\}$ 的位姿合成得到。我們利用運算符“ $\oplus$”表示相對位姿的合成。此時，點 $P$ 可以被表示為
$^Ap= \left( ^A\xi_B \oplus{} ^B\xi_C \right) \cdot {}^Cp$

在MATLAB中，“ $\xi$”、“ $\cdot$”、“ $\oplus$”，這些抽象的符號都可以被實現為標准的數學對象和運算符。

在前面的例子中主要討論了二維坐標系的情況，能適用於一大類機器人的問題，特別是在平面世界里作業的移動機器人。對於其他問題，我們需要用三維坐標系來表示三維世界中的物體，如飛行機器人、水下機器人的位姿或者機器人手臂夾持的工具末端。

在相對位姿的合成中，我們可以通過確認運算符 $\oplus$兩側的上標和下標字母匹配來檢驗相對坐標系是否正確。我們可以去掉中間的上、下標字母，只留下左右兩頭加圈顯示的上、下標字母。

下圖給出了一個更復雜的三維變換的例子，其中每個實體上都固接了一個三維坐標系,並表示出了它們之間的相對位姿。固定攝像頭從固定的視角觀察物體,並估計物體相對於攝像頭本身的位姿。另一個攝像頭是不固定的，它固接到機器人上，與機器人有固定的相對位姿，並估計出物體相對於該攝像頭的位姿。

另一個表示空間關系的方式是有向圖，如下圖所示。圖中的每個節點代表一個位姿，每條邊代表一個相對位姿。 從 $X$ 到 $Y$ 的箭頭記作 $^X\xi_Y$，表示 $Y$ 相對於 $X$ 的位姿。前面我們提到過可以用運算符 $\oplus$ 來復合成相對位姿，這里可以用它寫出一些空間關系：
$\xi_F \oplus{}^F\xi_B=\xi_R \oplus{}^R\xi_C \oplus{}^C\xi_B$
$\xi_F \oplus {}^F\xi_R={}^O\xi_R$

每個方程表示了圖中的一個閉環。方程等號兩側的每一邊各表示一條網絡的通路，即一組按照從頭到尾順序連接的邊（箭頭線）。等式兩邊的起始節點和結束節點必須相同。

位姿表示的一個非常有用的屬性是其代數運算能力。以上面第二個閉環方程為例，它表示機器人的位姿等同於兩個相對位姿的合成：從世界坐標系到固定攝像機的相對位姿，以及從固定攝像機到機器人的相對位姿。

在數學對象術語中，位姿組構成了一個群，即一系列對象的集合，它支持一種運算結果仍屬於該群的關聯二元運算符（復合算子），一種求逆運算和一種單位元素。在這里，該群是二維或三維的特殊歐幾里得群，分別記作 $SE(2)$或 $SE(3)$。

我們可以通過在等式兩邊同時加上一個逆 $\xi_F$ 來消除 $\xi_F$，記作
$\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}$
其結果就是機器人相對於固定攝像機的位姿。

下面是幾個代數運算規則：
$\begin{array}{rr} \xi\oplus 0=\xi,& \xi \ominus 0 = \xi \\ \xi \ominus \xi = 0,& \ominus \xi \oplus \xi = 0 \end{array}$其中 $0$ 表示一個零相對位姿。一個位姿可以有逆位姿：
$\ominus {}^X\xi_Y={}^Y\xi_X$該逆位姿在圖中可表示為從 $Y$ 到 $X$ 的一條箭頭線。相對位姿可以被復合或合成，之前已經說明，不再贅述。
需要注意的是位姿的代數運算規則不同於一般的代數運算規則，位姿復合運算中各個項的位置不能交換：
$\xi_1 \oplus \xi_2 \ne \xi_2 \oplus \xi_1$ $\xi_1\oplus\xi_2=0$時才可以互換位置。可以用相對位姿將一個點從一個坐標系中的一個向量轉換為另一個坐標系中的另一個向量，這里也不再贅述。

那么，什么是 $\xi$ ?它可以是支持上述代數運算的任何數學對象，並適用於我們身邊常見的問題。它還取決於我們討論的是二維還是三維的問題。以后我們將討論一些對象，其中包括向量以及一些更奇特的數學對象，如齊次變換矩陣、正交旋轉矩陣和四元數等。所有這些數學對象都適用於MATLAB的編程環境。

回顧

1. 一個點用坐標向量來表示，它代表該點在參考坐標系中的位移；
2. 一個剛體可以由其上的一組點代表，該剛體可以用單獨一個坐標系描述，並且組成它的點可以用它們在該坐標系中的位移來表示；
3. 一個物體在坐標系中的位置和方向稱為它的位姿；
4. 一個相對位姿表示一個坐標系相對於另一個坐標系的位姿，記作代數變量 $\xi$；
5. 一個點可以用不同坐標系中的不同坐標向量來描述，向量之間通過坐標系相對位姿來轉換，其運算符為“ $\cdot$”；
6. 用相對位姿寫成的代數表達式是可以進行代數運算的。