#本題に入る前に…
多関節ロボットとかにビジョンを取り付け、Pick and Place動作をするロボットシステムが最近主流になってきていますよね。まぁビジョンメーカが出しているPick and Place動作を行えるシステムを購入して（多分すごい高い…。５００−６００万円以上はザラじゃないかな？）、メーカさんの教育どおり実践し、「ハイできましたよ！」ていう、業者さん(※)も実は多そうですね。何が業者さんのコア技術なのかもわからなくなってきており、技術が一般化するのも良い面もあるし、またちゃんと基本を理解して、仕事しているのかなぁ？と思ってしまう言動や行動もある。偉そうなことはこの辺にしときますが、私が言いたいのは・・・
１０年間エンジニアやってきて、最近さらに強く思うのは、”原理や数理”て大事だよねというところ。
まぁ説教の前置きはこのへんで…。
ロボットビジョンをやるのに、大変重要となるコア技術が、そう！**キャリブレーション！よくキャリブレーション（較正）とアライメント（位置決め）**を間違えるど阿呆ぉがいますが、原理を理解していないからそういう言い間違いをするんです。まだいろいろ言いたいけど、はい。愚痴はこの辺にしときます。では、このロボットのキャリブレーションについてまとめます。
※業者：SIを指す。SI=システムインテグレータ
##”カメラキャリブレーション”と”ハンド・アイ・キャリブレーション”について
何かしらのビジョンを用意して、多関節ロボットに取り付けてさぁ！Pick and Placeやるぞとしても、下準備をしていないのでざんねながら上手く動きません。下準備てなんだよ！というと例えば…
１）カメラ内のイメージセンサとレンズとの距離（焦点距離）やイメージセンサの画素ピッチ、レンズの像面湾曲などのカメラの内部に関するパラメータ。
⇒カメラキャリブレーション（カメラ内部パラメータ）
２）レンズを中心である光軸を基としたカメラ座標系と焦点距離分を並進させて、左上を基準とした画像座標系、３次元空間の任意な点の世界座標系との位置関係や座標変換に関するパラメータ。
⇒カメラキャリブレーション（カメラ外部パラメータ）
３）ビジョン（このページではステレオビジョンを題材にする）がカメラであるとすると、多関節ロボットのTCP（ツール・センター・ポイント）の面 からエンドエフェクタ先にカメラの取り付け位置関係がわからない。
⇒ハンド・アイ・キャリブレーション
１）と２）がわからないと、ビジョンに映る画像が歪んでしまったり、ビジョンの画像にあわせてロボットを移動しても、画像の基準点がわからないため制御することができません。
３）がわからないとそもそも、カメラで見てあとどのくらい移動すればそこのワークに届くのか計算できても、アームの基準座標（ベース座標系）におけるカメラ画像上の目標点の座標を算出することができないため、ロボットを正しく動かすことができません。
ということで、このキャリブレーションは大変重要なものであり、ロボットビジョンの系統誤差（バイアス成分）に作用する重要な要素の一つであることは言うまでもありません。
スカラロボットなら、制御はかんたんですけど、多関節ロボットはロボット運動学を中心に組まなければならないことが多いです。また、ロボットビジョンに関して言ううとそれだけでなく、原理理解には、実は、多項式の最小二乗法と最適化数学（非線形最適化）の知識、言うまでもなく、基礎としては線形代数学が必要になってきます。このキーワードを語れないエンジニアは、原理を理解せずにあなたに、使い方や作業だけを平気で言う上辺だけの輩であると私は言っていいのではないかなと思います。別に原理に関して気にしない人は別にいいですが、原理は本質の部分であり、その本質を理解していないと、話が薄っぺらくなります。また、愚痴ぽくなりましたが、上記の理由で、ロボットビジョンのキャリブレーションは、とても重要技術です。
##”カメラキャリブレーション”について
###カメラ内部パラメータ
カメラの機能をかなりシンプルに捉えるとき、カメラは単なる投影による幾何変換として扱うことができる。数理的には、下記の図は、透視投影系という。まず、図の説明をすると…カメラ座標系($x$, $y$ ,$z$)の原点$O_C$はレンズ中心であり、そのZ軸は光軸と一致する。また、実際に像が生じる面を画像座標系($\mu$, $\nu$)は２次元となり、カメラ座標系より焦点距離$f$の位置に、光軸の中心である($\mu_c$, $\nu_c$)がある。
※図は違うが、画像座標系の原点は一般的に、左上が原点になることが多いので注意。
ここで、カメラ座標系($x$, $y$ ,$z$)における画像座標系($\mu$, $\nu$)は、次式で表される。
\begin{bmatrix}
s\mu\\
s\nu\\
s\\
\end{bmatrix}=\begin{bmatrix}
k_uf & 0 & \mu_c & 0 \\
0 & k_\nu f & \nu_c & 0\\
0 & 0 & 1 & 0\\
\end{bmatrix}
\begin{bmatrix}
x\\
y\\
z\\
1\\
\end{bmatrix}
ここで、$k_\mu{^{-1}}$,$k_\nu{^{-1}}$は画素ピッチ、$s$は同次座標系として表現するための媒介変数である。媒介変数を入れると、線形変換として扱いがしやすいため、ロボット工学などはこのように表現するのが通常である。
###カメラ外部パラメータ
次に、世界座標系におけるカメラ座標を回転行列$R$と並進ベクトル$t$で表現すると…
\begin{bmatrix}
x\\
y\\
z\\
1\\
\end{bmatrix}=\begin{bmatrix}
&R& & t \\
0 & 0 & 0 & 1\\
\end{bmatrix}
\begin{bmatrix}
x_w\\
y_w\\
z_w\\
1\\
\end{bmatrix}
となる。この回転行列は、ロー角・ピッチ角・ヨー角としてあらわされる。知らない人は、飛行機を考える良い。飛行機の頭側の回転がRoll、羽の方向がPich、旗はないが旗手の垂直方向がYawである。
R_x=\begin{bmatrix}
1&0&0\\
0&\cos\theta&\sin\theta\\
0&-\sin\theta&\cos\theta\\
\end{bmatrix}
R_y=\begin{bmatrix}
\cos\theta&0&-\sin\theta\\
0&1&0\\
\sin\theta&0&\cos\theta\\
\end{bmatrix}
R_z=\begin{bmatrix}
\cos\theta&\sin\theta&0\\
-\sin\theta&\cos\theta&0\\
0&0&1\\
\end{bmatrix}
このロー角・ピッチ角・ヨー角を用いて、オイラー角を表現する。オイラー角は一般的に２種類の方法があり、１）Z軸Y軸Z軸と２）Z軸Y軸X軸の順番で回転させる方法がある。それぞれの場合の回転行列を下記にしめす。
- Z軸Y軸Z軸 : $R$=$R_z$$R_y$$R_z$
R=\begin{bmatrix}
\cos\gamma&\sin\gamma&0\\
-\sin\gamma&\cos\gamma&0\\
0&0&1\\
\end{bmatrix}
\begin{bmatrix}
\cos\beta&0&-\sin\beta\\
0&1&0\\
\sin\beta&0&\cos\beta\\
\end{bmatrix}
\begin{bmatrix}
\cos\alpha&\sin\alpha&0\\
-\sin\alpha&\cos\alpha&0\\
0&0&1\\
\end{bmatrix}
- Z軸Y軸X軸 : $R$=$R_x$$R_y$$R_z$
R=\begin{bmatrix}
1&0&0\\
0&\cos\gamma&\sin\gamma\\
0&-\sin\gamma&\cos\gamma\\
\end{bmatrix}
\begin{bmatrix}
\cos\beta&0&-\sin\beta\\
0&1&0\\
\sin\beta&0&\cos\beta\\
\end{bmatrix}
\begin{bmatrix}
\cos\alpha&\sin\alpha&0\\
-\sin\alpha&\cos\alpha&0\\
0&0&1\\
\end{bmatrix}
###カメラキャリブレーション
-
カメラの内部パラメータ
焦点距離の画素数：$k_\mu$$f$ , $k_\nu$$f$
画像座標系の原点：($\mu_c$,$\nu_c$) -
カメラの外部パラメータ
回転角：$\phi$ , $\theta$ ,$\psi$
カメラ座標系の原点：( $x_ｃ$ , $y_ｃ$ , $z_ｃ$ )
上記のパラメータの１０個が未知であり決定されれば、世界座標系の三次元空間中の任意な点( $x_w$ , $y_w$ , $z_w$ )から画像座標系の唯一の点($\mu$,$\nu$)が決定される。
世界座標系の三次元空間中の任意な点( $x_w$ , $y_w$ , $z_w$ )から画像座標系の点($\mu$,$\nu$)の関係はカメラパラメータ$C_{11}$〜$C_{34}$を用いて次式のように表す。
\begin{bmatrix}
s\mu\\
s\nu\\
s\\
\end{bmatrix}=\begin{bmatrix}
k_uf & 0 & \mu_c & 0 \\
0 & k_\nu f & \nu_c & 0\\
0 & 0 & 1 & 0\\
\end{bmatrix}
\begin{bmatrix}
&R& & t \\
0 & 0 & 0 & 1\\
\end{bmatrix}
\begin{bmatrix}
x_W\\
y_W\\
z_W\\
1\\
\end{bmatrix}
=\begin{bmatrix}
C_{11} & C_{12} & C_{13} & C_{14} \\
C_{21} & C_{22} & C_{23} & C_{24}\\
C_{31} & C_{32} & C_{33} & C_{34}\\
\end{bmatrix}
\begin{bmatrix}
x_W\\
y_W\\
z_W\\
1\\
\end{bmatrix}
ここで、実際は世界座標系( $x_w$ , $y_w$ , $z_w$ )から画像座標系の点($\mu$,$\nu$)は観測が可能である。観測回数nとして、i=1,2,3,…ｎより世界座標系( $x_i$ , $y_i$ , $z_i$ )と対応する画像座標系($\mu_i$,$\nu_i$)となる。このデータセットを用いて前述のカメラパラメータを推定すると言ううことである。
よって、前述の式よりi番目のデータとして展開すると…
S=C_{31}x_i+C_{32}y_i+C_{33}z_i+C_{34}
より、代入して…
C_{11}x_i+C_{12}y_i+C_{13}z_i+C_{14}-(C_{31}x_i\mu_{i}+C_{32}y_i\mu_{i}+C_{33}z_i\mu_{i}+C_{34}\mu_{i})=0
C_{21}x_i+C_{22}y_i+C_{23}z_i+C_{24}-(C_{31}x_i\nu_{i}+C_{32}y_i\nu_{i}+C_{33}z_i\nu_{i}+C_{34}\nu_{i})=0
$C_{34}$=1であり、行列式にまとめると…
\begin{bmatrix}
x_{1}&y_{1}&z_{1}&1&0&0&0&0&-x_{1}\mu_{1}&-y_{1}\mu_{1}&-z_{1}\mu_{1}\\
0&0&0&0&x_{1}&y_{1}&z_{1}&1&-x_{1}\mu_{1}&-y_{1}\mu_{1}&-z_{1}\mu_{1}\\
& & & & & & {････} & \\
x_{n}&y_{n}&z_{n}&1&0&0&0&0&-x_{n}\mu_{n}&-y_{n}\mu_{n}&-z_{n}\mu_{n}\\
0&0&0&0&x_{n}&y_{n}&z_{n}&1&-x_{n}\mu_{n}&-y_{n}\mu_{n}&-z_{n}\mu_{n}\\
\end{bmatrix}
\begin{bmatrix}
C_{11}\\
C_{12}\\
{････} \\
C_{31}\\
C_{33}\\
\end{bmatrix}
=\begin{bmatrix}
\mu_{1}\\
\nu_{1}\\
{････} \\
\mu_{n}\\
\nu_{n}\\
\end{bmatrix}
となる。ここで、左の大きな行列を$A$ , $C$ , $U$ とすると
U = A・C
とあらわせる。ここで、$C$ を求めるために、逆行列である $A^{-1}$を両辺左からかけて…。
C= A^{-1}・U これは大間違い！
単純に、逆行列を求められない理由は、そもそもｎ回のデータセットしかないので行列$A$ は（ｎ×11）行列であり、正方行列（ｎ×ｎ）ではない。正方行列でなければそもそも正則ではないので、逆行列は存在しない。
それでは、どうするのかというと、ムーア-ペンローズの疑似逆行列を使えば良い。
確かに、行列$A$ は（ｎ×11）は正方行列ではないが、転置行列$A^{T}$の積を行うと対称行列となる。
対称行列は、行列$A$＝転置行列$A^{T}$となる行列のことである。この対称行列（※）の特徴として、正定値（すべての固有値＞０）であると、正則すると言える。また、ムーア-ペンローズの疑似逆行列は、必ず存在することも保証されている。しかし、逆行列$A^{-1}$のように一意にはならない。データセットによって無数に存在するという意味ですね。
※実数の場合は、対象行列。虚数の場合はグリムエルミネート行列という。
（グリル行列てなんで書いたんだろ…夜中に投稿するとだめだな。頭がおかしい）
よって、転置行列$A^{T}$を左から積して
A^{T}U = A^{T}A・C
疑似逆行列：$[AA^{T}]^{-1}$を両辺に積をし、行列$C$をもとめる。
なお、疑似逆行列は、逆行列のように一つには決まらないが、逆行列と同じような性質を持っている。
C = [A^{T}A]^{-1}・A^{T}U
この、疑似逆行列は興味深いことに、多項式の最小二乗法より極値における行列$C$を求めた場合にも同じ結果が得られる。なお、多項式の最小二乗法の場合は次式となる。
Ｅ=\|AC-U\|
この誤差$E$が最小となる$C$を求めればよい。
実際は、得られたデータセットの一部の中で、疑似逆行列を用いて初期値を求めて、あとは評価関数$E$を用いて、解析的な手法例えば、最急降下法やニュートン法、準ニュートン法、Lenvenberg-Marquard法などを用いて求める。
また、パラメータとともに像面湾曲の歪係数を$k$として
\begin{bmatrix}
\mu_i\\
\nu_i\\
\end{bmatrix}=\frac{1}{1+kr_i^{2}}
\begin{bmatrix}
\mu_i^{'}\\
\nu_i^{'}\\
\end{bmatrix}
r_i^{2} = \mu_i^{'{2}} + \nu_i^{'{2}}
とあらわせる。($\mu_i^{'}$,$\nu_i^{'}$)が実際の得られた歪んでいる座標である。それを上記の式に入れると補正させれる仕組みである。同様に、評価関数にいれて、解析的にとけば、歪係数を$k$も求まる。
キャリブレーションをやる場合、Zhangの手法であるチェスボードを用いた方法が有名で安定している。その場合、キャリブレーションプレートのドットの数、間隔、大きさを予め知っておき、プレートの中心を世界座標系の原点とする。世界座標系とカメラ座標系との関係は平面であるため$Z_w$=0として、プレートの姿勢を変えながら、データを取得していき、解析的に擬似逆行列を求めるとよい。
こんな感じで、カメラキャリブレーションを行います！
次回は、Zhangの手法をまとめようかなぁ…
キャリブレーションの原理初見では、難しいですよね！私は、難しかった思い出があります。
私も忘れるんで、備忘録としてまとめました。また、次回！