【Python】治具座標系からロボット座標系への変換方法（特異値分解）
目次
１．やりたいこと
6軸ロボットのプログラムをつくっています。同じ動作の繰り返しであればそのまま教示を行ってしまえばいいのですが、複数の点を１点１点教示していくのは非常に面倒なので、基準点を１点教示してあとは、CADなどから出力した座標データをロボットに与えることにしました。
具体的には、平面上の加工物へのアプローチ（接触）を外部の制御機器からロボットへ数値情報を送り、動的にアプローチの位置を基準の位置からオフセットできるような仕組みにしたいのです。
ここで、CADデータが分かっているので、基準点からオフセットする数値座標を送ればいいじゃんとおもったのですが、治具自体に回転成分も含まれているため、そのままオフセットはつかえませんでした。
そのため、今回は、加工物のCADデータの座標点から、設備のロボット座標系に変換する方法として、DLT法の活用で対応することにしました。
２．DLT法
ここで、課題となるのが、CADデータの座標データを送付して、それをなにかしらの変換して、ロボット座標しなければならないことです。
これらを解決する方法に、ＤＬＴ法があります。direct linear transformation method（DLT 法 ; Abdel-Aziz & Karara, 1971）の略称とのことで、計測空間内での位置が既知である計測点を撮影に使用する全てのカメラで撮影し，撮影映像上での座標（以下，２次元計測座標）と実空間での座標（以下，実空間座標）とを対応させるキャリブレーションを行い，変形した共線条件式によって求めた DLT パラメータを用いて，分析対象点の３次元座標を推算する．
引用文献等
ビデオカメラでの 3 次元動作計測における問題点と測定精度向上の取り組み
中井 聖1,2
1
京都光華女子大学健康科学部健康栄養学科（２０１８年３月まで）
2 大阪電気通信大学医療福祉工学部健康スポーツ科学科（２０１８年４月より）
つまり、カメラなどの2次元平面のある座標点から3次元上の座標点に
変換することができる変換パラメータを求めることができる方法とのことです。
３．実装
実装については、以下の参照しました。
DLT法は、2次元のカメラ画像の座標系から実空間の3次元座標へ変換しています。今回は、加工物の座標系からロボットの座標系への変換を求めたいと思います
実装には下記のホームページ及び参考書を参考にしました。
Xaには加工物の座標系
Xbにはロボットの座標系を入れています。
作った行列は下記添付しておきます。
このあと、作った行列をBとすると
①BT（置換行列）× Bを行い、BTBの行列を作る。
②BTBを特異値分解し、u,s,vに分解する。
③固有値が最小となるときのv（固有ベクトル）が求めたい
a1～a12となる。
４．検証
import numpy as np ;CSVデータの読み込み B = np.loadtxt("matB.csv”,delimiter=",") ;転置行列 BT = B.T ;内積 BTB = np.dot(BT,B) ;特異値分解 u, s, v = np.linalg.svd(BTB) ;行 列の取得 row, col = v.shape ;最小固有値における固有ベクトルの抽出 vec = v[row-1, :] ;ベクトルを３×３行列に変換する A = vec.reshape(3,3)
この行列Aが加工物の座標系→ロボット座標系に変換するための
行列になります。
例えば、加工物の座標が
ｘ＝-1.14
y = 5.1
となるとき、加工物のベクトルは以下のように設定します
;加工物のベクトルを設定 kakoubutu = [-1.14, 5.1, 1] ;加工物からロボット座標系へ変換 rob = np.dot(A, kakoubutu) ;ロボット座標系を正規化 rob_x = rob[0]/rob[2] rob_y = rob[2]/rob[2]
正規化したrob_x, rob_yが対応するロボット座標系となります
