姿勢制御のための数理 - 1. 姿勢の記述

概要

1. (ｺｺ) 姿勢の記述 (群)
2. 姿勢変化の記述 (多様体)
3. 姿勢制御のためのリー群・リー代数
4. 付録: 回転対称性を持つ剛体の姿勢

$d$ 次元空間 (通常は $d = 3$) 内の物体について, それが存在する位置と向きを姿勢と呼び, 姿勢を数学的に (数値データとして) 記述したり姿勢の変化を扱ったり最適化したりする問題を姿勢制御 (問題) と呼ぶ.

姿勢制御には,

• そもそも「姿勢」を数学的にどう記述するか,
• 姿勢が (滑らかに) 変化する時, それを記述する数学的オブジェクトはどう変化するのか,

という問題があり, それぞれの問題の解決に用いられる群・多様体という数学的概念がリー群 (及びリー代数) として統合できることを解説するのが本シリーズの目的になる.
ただしここでは理論面の説明に留まり, 具体的な計算例や計算のテクニック等は他の記事が豊富なのでそちらに任せたい.

初回は点群による物体の姿勢の記述から始めて, それを群によって抽象化・体系化できることを見ていく.
ここでは物体の姿勢は基準となる姿勢に対して相対的に表現され, それによって離散的な姿勢の変化であれば扱えるようになる.

前提知識

線型代数, ユークリッド幾何の基礎用語は特に説明なく使う.
「姿勢制御には行列, アファイン変換を使う」という方法論にも慣れているといい.

解説

姿勢の記述

姿勢を考える時, 対象となる物体は「形が変わらない」という条件が加えられることが多く, これを剛体と呼ぶ¹.

また, 面倒を避けるため, $d$ 次元空間内の剛体として $d-1$ 次元以下の超平面内には位置しないことも仮定する.
$3$ 次元空間においては厚みがあるか, 立体的に曲がっている物体をイメージすればいい.

さらに面倒を避けるため, 扱う剛体は回転対称でないとしておく.
これは後述の姿勢集合を恒等でない任意の等長変換で写して元の姿勢集合に一致しないことを指す.
例えば球体を任意角度で回転させたり, 立方体の頂点を別の頂点に写すよう回転させても見た目上は向きを含めた形状が変わらないため, これらは回転対称であり今回 (付録以外では) 扱う対象としない.

点群による姿勢の記述

剛体の姿勢 (位置と向き) を記述するのに最も単純な方法としては, 剛体を $d$ 次元空間の部分集合 (点群) と見なす方法が挙げられる:

$$ \mathcal{R} := \{x \in \mathbb{R}^d \ \vert \ \text{座標 $x$ に剛体が存在する} \} \subset \mathbb{R}^d. $$

これを姿勢集合と呼ぶようにする.

このように剛体の姿勢は姿勢集合という点群によって記述されるため, 適当に離散化すれば数値データ (点群データ) として扱えるようになる.

アファイン²変換による相対姿勢の記述

姿勢集合によって姿勢を数値データとして扱う方法が得られたが, これは冗長な方法なのでもう少し深掘りしよう.

剛体の姿勢を記述するのは空間的・時間的に異なる物体との相対的な位置関係を明確にするのが目的である.
座標による姿勢集合の記述は他の物体の位置によらないという意味では絶対的だが, 原点の位置と座標軸方向を変えれば座標も変わることからそれらに対する相対的な記述だとも言える.

後に姿勢の変化を扱うためには異なる姿勢の同じ剛体を比較する必要があり, この場合は「一方の姿勢集合をどう動かすと他方の姿勢集合に一致するか」で表すのが自然である.
これは二つの姿勢集合 $\mathcal{R}, \mathcal{R}'$ 間の写像で記述されるが, 「$\mathcal{R}$ と $\mathcal{R}'$ は同形状である」ということを表現するため, 次で条件付けられる写像 $\varphi$ を考える:

1. $\varphi: \mathcal{R} \rightarrow \mathcal{R}'$ は全単射,
2. 任意の $x, y \in \mathcal{R}$ と $\varphi(x), \varphi(y) \in \mathcal{R}'$ の距離が変わらない,
3. 同一超平面上にない $d+1$ 点 $x_0, x_1, \dots, x_d \in \mathcal{R}$ に対して,
   $\varphi(x_0), \varphi(x_1), \dots, \varphi(x_d) \in \mathcal{R}'$ は「向き」と呼ばれる位置関係を保つ.

条件 3 は $\varphi$ が剛体を鏡写しにした形状に写さない (鏡映反転を行わない) ことを表す.
$\mathcal{R}$ と $\mathcal{R}'$ が同じ形状の剛体の姿勢集合であれば, このような $\varphi$ を構成できる ($\mathcal{R}, \mathcal{R}'$ それぞれの点が一対一に対応する)³.
このうち条件 1, 2 を満たす $\varphi$ (を $\mathbb{R}^d$ 全体に拡張したもの) を等長変換と呼び, ユークリッド距離に関する等長変換でさらに条件 3 を満たすものは特殊ユークリッド変換⁴ (またはユークリッドの運動, 等) と呼ばれるアファイン変換で書けて, 逆に特殊ユークリッド変換は鏡映反転を行わない等長変換となることが知られている.

これで特殊ユークリッド変換によって二つの姿勢の相対関係を記述できることが分かったが, 特殊ユークリッド変換はさらに「絶対的な」姿勢を記述するのにも利用できる.
つまり, 基準となる姿勢集合 $\mathcal{R}_0$ を固定すれば, $\mathcal{R}_0$ を $\mathcal{R}$ に写す特殊ユークリッド変換は $\mathcal{R}$ の絶対的な⁵姿勢を表していると考えることができる.

$$ \begin{CD}
\mathcal{R}_0 @>\varphi>> \mathcal{R}. \\
\end{CD} $$

また, 二つの姿勢を写し合う特殊ユークリッド変換は一意に決まるため⁶, 特殊ユークリッド変換が一致するかどうかで対応する姿勢が一致するかどうか判定が可能である.

特殊ユークリッド変換による体系的な姿勢制御

姿勢を特殊ユークリッド変換によって記述する方法には, 姿勢を扱うのに剛体の形状を考える必要がないという利点がある.

姿勢集合によって姿勢を扱うと例えば二つの姿勢 $\mathcal{R}, \mathcal{R}'$ を比較するのにまず「そもそも $\mathcal{R}$ と $\mathcal{R}'$ は同じ形状か」を気にしなければならず, またある剛体の姿勢制御で利用できた手法が形状の異なる剛体では利用できなくなるような問題も起こり得る.

それに対して特殊ユークリッド変換という汎用的な表現を用いれば, 例えば次の異なる剛体を扱う問題に利用できる.
ここでは $A$ という剛体の姿勢を記述する特殊ユークリッド変換 $\varphi_A$ を, 異なる剛体 $B$ の姿勢の記述に用いていることに注目しよう.
厳密には特殊ユークリッド変換の合成で得られる $\varphi_B$ がまた特殊ユークリッド変換となるかどうかも確認が必要であり, それについては後の節を参照.

---

問題と解法.
二つの剛体 $A, B$ があって, $A$ の姿勢変化は直接得られるが $B$ については $A$ に対する相対的な姿勢しか分からない時, $B$ の姿勢がどのように変化するか調べる問題を考える.

$A$ の姿勢が基準姿勢に対して $\varphi_A$ で変化し, $B$ の相対姿勢が $\varphi_{B; A}$ で変化したとする.
$B$ の $A$ に対する相対姿勢が変わらないとすると, $A$ の姿勢が $\varphi_A$ で変化した時は $B$ の姿勢も同じく $\varphi_A$ で変化するはずである.
よって, $B$ の姿勢は基準姿勢から $\varphi_A, \varphi_{B; A}$ で順に変化し,

$$ \varphi_B = \varphi_{B; A}\circ\varphi_A, $$

で導出されると考えられる.

---

特殊ユークリッド変換の構造

特殊ユークリッド変換の具体的な構造を見ていこう.

回転行列

特殊ユークリッド変換は構成要素として回転行列と呼ばれる線型変換 (行列) を持つ.

可逆な正方行列 $P$ で転置が逆行列となるもの, つまり,

$$ P^tP = PP^t = I, $$

となる $P$ を直交行列と呼ぶ ($I$ は単位行列).
直交行列による変換は等長変換であり, 線型変換のうち等長となるものは全て直交行列で書かれることが知られている.

また, 直交行列は行列式が $\pm1$ となることが知られており, このうち行列式が $1$ であるものは回転行列 (特殊直交行列) と呼ばれる.
行列式が $1$ であることは鏡映反転を行わないことを意味しており, 剛体を滑らかに動かした時, (それが鏡映対称である場合を除いて) 元の形状と鏡写しになった形状に写ることはない⁷ためこの制約が加えられる.

剛体の位置を考えず向きのみを扱う問題として姿勢制御を考える場合, 姿勢は後述の平行移動を省いて回転行列によって記述されることになる.

特殊ユークリッド変換とその行列表示

一般の $d$ 次元空間上のアファイン変換 $\varphi \in \mathrm{Aff}(d, \mathbb{R})$ は, 線型変換 $L \in M(d, \mathbb{R})$ 及びベクトル $w \in \mathbb{R}^d$ によって,

$$ \varphi(x) = Lx+w, $$

と一意に書くことができ², これを $L_\varphi := L, w_\varphi := w$ と書くようにする.
$w_\varphi$ を加える作用は平行移動と呼ばれ, アファイン変換は線型変換を行う $L_\varphi$ と平行移動を行う $w_\varphi$ の二つの構成要素を持っている.
特殊ユークリッド変換は線型変換 $L_\varphi$ として回転行列を取ったアファイン変換であると定義される.

アファイン変換は $d+1$ 正方行列として,

$$ \varphi^+ := \begin{bmatrix}
L_\varphi & w_\varphi \\
0 & 1 \\
\end{bmatrix} \in M(d+1, \mathbb{R}), $$

とも記述される.
この時 $x = [x_0, x_1, \dots, x_{d-1}]^t \in \mathbb{R}^d$ に対して $x^+ = [x_0, x_1, \dots, x_{d-1}, 1]^t \in \mathbb{R}^{d+1}$ という記号を導入することで,

$$ \varphi(x)^+ = \varphi^+x^+, $$

が成り立ち, アファイン変換 $\varphi$ による作用は線型変換 $\varphi^+$ による行列積と同値になることが分かる.
アファイン変換をそのまま扱うより線型変換の方が取り回しが良いため, アファイン変換はこの線型変換による表示で用いられることも多い.

回転群・特殊ユークリッド群

回転行列全体からなる線型変換の集合を回転群, 特殊ユークリッド変換全体からなるアファイン変換の集合を特殊ユークリッド群とそれぞれ呼ぶ.
回転群, 特殊ユークリッド群はそれぞれ $SO(d), SE(d)$ と書かれ⁸, どちらも写像の合成を演算とする群構造を持ち⁹, 特に以下の性質は姿勢の変換を扱うのに便利である.

(1) 写像の合成で閉じている.
回転行列 $P, P' \in SO(d)$ の合成は行列積による $P'P$ で得られ, 再び回転行列となる.
特殊ユークリッド変換 $\varphi, \varphi' \in SE(d)$ の合成を考えると,

$$ \varphi'\circ\varphi(x) = L_{\varphi'}L_\varphi x+L_{\varphi'}w_\varphi+w_{\varphi'}, $$

と書けて, $\varphi'\circ\varphi$ は線型変換部分が $L_{\varphi'}L_\varphi$ で平行移動部分が $L_{\varphi'}w_\varphi+w_{\varphi'}$ であるアファイン変換となる.
$\varphi, \varphi'$ が特殊ユークリッド変換となることは $L_\varphi, L_{\varphi'} \in SO(d)$ であることだったので, $L_{\varphi'}L_\varphi \in SO(d)$ より $\varphi'\circ\varphi$ は再び特殊ユークリッド変換となることが分かる.
これにより姿勢集合について $\mathcal{R}$ を $\mathcal{R}'$ に写す特殊ユークリッド変換と $\mathcal{R}'$ を $\mathcal{R}''$ に写す特殊ユークリッド変換の合成で $\mathcal{R}$ を $\mathcal{R}''$ に写す特殊ユークリッド変換が得られる.

$$ \begin{CD}
\mathcal{R} @>\varphi>> \mathcal{R}' @>\varphi'>> \mathcal{R}''. \\
\end{CD} $$

特殊ユークリッド変換の行列表示を用いた場合, $(\varphi'\circ\varphi)^+ = \varphi'^+\varphi^+$ が成り立つので特殊ユークリッド変換の合成はそれらを行列表示したものの積に対応する.

(2) 可逆である.
回転行列 $P \in SO(d)$ は直交行列であるため転置 $P^t$ が逆変換となり, $P^t$ もまた直交行列としての条件を満たして行列式が $1$ となるので $P^{-1} \in SO(d)$ である.
特殊ユークリッド変換についても, アファイン変換 $\varphi \in \mathrm{Aff}(d, \mathbb{R})$ に対して,

$$ y = \varphi(x) = L_\varphi x+w_\varphi, $$

は $L_\varphi$ が可逆ならば,

$$ x = L_\varphi^{-1}y-L_\varphi^{-1}w_\varphi, $$

と変形できて, $\varphi$ は逆写像を持ち, それは線型変換部分が $L_\varphi^{-1}$ で平行移動部分が $-L_\varphi^{-1}w_\varphi$ のアファイン変換となることが分かる.
特に回転行列は可逆で逆変換もまた回転行列となることから, 特殊ユークリッド変換 $\varphi \in SE(d)$ は常に逆変換 $\varphi^{-1}$ を持ち, それも特殊ユークリッド変換となる.
この時, $\varphi^{-1}$ の行列表示は $\varphi$ の行列表示の逆行列となっている.

(1), (2) を組み合わせると, 特殊ユークリッド変換による姿勢の絶対的な記述から二つの姿勢を写し合う相対的な記述を得ることができる.
つまり, 基準となる姿勢集合 $\mathcal{R}_0$ に対して, 二つの姿勢集合 $\mathcal{R}, \mathcal{R}'$ を表す特殊ユークリッド変換をそれぞれ $\varphi, \varphi' \in SE(d)$ とすると, $\varphi^{-1}$ は $\mathcal{R}$ を $\mathcal{R}_0$ に写し, $\varphi'$ は $\mathcal{R}_0$ を $\mathcal{R}'$ に写す特殊ユークリッド変換となるので, $\varphi'\circ\varphi^{-1}$ により $\mathcal{R}$ を $\mathcal{R}'$ に写す特殊ユークリッド変換が得られることになる.

$$ \begin{CD}
\mathcal{R} @>\varphi^{-1}>> \mathcal{R}_0 @>\varphi'>> \mathcal{R}'. \\
\end{CD} $$

特に, $\mathcal{R}_0$ から $\mathcal{R}'_0$ への特殊ユークリッド変換を $\varphi_0$ と書いた時, $\mathcal{R}_0$ を基準として $\varphi$ で表される姿勢は, $\mathcal{R}'_0$ を基準とするよう取り替えると $\varphi\circ\varphi_0^{-1}$ によって記述し直される.

$$ \begin{CD}
\mathcal{R}'_0 @>\varphi_0^{-1}>> \mathcal{R}_0 @>\varphi>> \mathcal{R}. \\
\end{CD} $$

このように基準となる姿勢を取り換える操作を座標変換と呼んだりする.

参考資料

具体的計算を伴う解説はこちらから.

1. 数学というより物理・工学の用語である. ↩

2. 「アフィン」はドイツ語読みで「アファイン」は英語読みらしい. 日本で多いのは「アフィン」だが「アファイン」の方が数学感が強い. ↩ ↩²

3. さらっと書いたが数学的には逆で, このような等長写像が存在することが「二つの集合が同形状 (幾何的合同)」の定義である. 距離を保存するだけの写像が形状も保存するかも考える必要はあるが「形状とは何か」の再定義から必要になって長くなるので割愛する. ↩

4. 鏡映反転を許したユークリッド距離に関する等長変換を単にユークリッド変換と呼ぶのに対して「特殊」という接頭語が付く. ↩

5. 「絶対的な座標」を「原点と座標軸を固定した時の相対座標」という意味で用いているのと同様に「基準姿勢を固定した時の相対姿勢」という意味での「絶対的な姿勢」である. ↩

6. 剛体が超平面に位置しないかつ回転対称でないという仮定はここで利いてくる. これらの仮定を置かないと姿勢集合と特殊ユークリッド変換が一対一に対応しなくなり, 同じ姿勢かどうかの判定が難しくなる. ↩

7. これは連結性という用語で定式化される. 集合の要素を滑らかに変化させて他の要素に写すにはその集合の連結性が必要になり, 直交行列の集合の中で行列式が $1$ の行列と $-1$ の行列は連結でないためそれらは互いに滑らかに変化し合わない. 一方で回転行列の集合 (直交行列の集合の行列式が $1$ である部分集合) は連結性を持ち, 任意の二つの行列を滑らかな変化で写し合うことができる. ↩

8. $SO, SE$ はそれぞれ Special Orthogonal, Special Euclid (ユークリッド) の頭文字である. ↩

9. ただし「$SO(d), SE(d)$ は群なので (1), (2) の性質を持つ」ではなく「$SO(d), SE(d)$ は (1), (2) (とその他いくつか) の性質を持つので群である」が正しい. ↩