【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が対応するロボット座標系となります
【ラズベリーパイ４】OSなどの初期設定おこなった
目次
１．ラズベリーパイを購入した理由
数年前から気になっていたアイテムです。よくあるワンボードマイコンとは異なり、OSが入っており、PCのように運用できること、WIFI、USB、など様々な機能が入っているのにこの小ささ。
もちろん、価格はワンボードマイコンに比べたらお高いですが、IoTの何かを作るには画期的なボードだなーと思っていました。
そんな中、最近よくニュースで「さい銭泥棒」の映像を目にする機会があり、犯人逮捕！ってな感じです。そういえば、自分、今年、お寺の氏子を担当しているのですが、毎年、さい銭泥棒の被害にあっているようで、ことしは、鍵を破壊されたため、警察を呼んだそうです。
近所とはいえ、正月の期間にさい銭泥棒のために、昼夜監視するのは避けたいなって思ったときに、「ラズパイ！」て安直な考えで監視カメラをつくろうとおもったのが経緯です。
ネットには、監視カメラ関係のネタが多数あるため、活用させて頂きながら製作できたらと思い、まずは、購入しました。
２．準備
ラズパイ単体だと、送料込みで6500円くらいでしょうか。
その他に、ケース、３A近くだせるUSB-電源、マイクロHDMI、マイクロSDカードなど必要で、1万5千円近くなるようです。
たかい・・・・
てなことで、中古でないかなーってことで、１万円以下でラズパイ4 メモリ4Gがあったので、ここで手を打ちました。
3日後 ピンポーン きた！
ガサガサ、開封。。。。。
おいおい
汚ねー
髪の毛ついてるし、中身ぐちゃぐちゃだし、
マイクロSDカードないし！（しぶしぶ、家電屋で購入）
でも、とりあえず、本物で間違いなさそう
とりあえず、電源入れてみる。電源供給のため、ＵＳＢ－Ｃに接続。キーボードとマウスをＵＳＢに接続。最後に映像出力のため、マイクロＨＤＭＩを接続した。
電源ＯＮ！
とりあえずＦＡＮは回ってるし、映像が出た。
なんかこの画面から変わらんし・・・・
よくわからんこと書いている・・・・・
SDカードがない？
ちゃんと調べずにいきなりやったので当然ですが、
OSなどはSDカードに入ってないと動かないらしい。
ネットからダウンロードしないといけないらしい。
３．初期設定
OSの設定には、いろいろとあるようですが、現時点でもっとも新しく、簡単な方法は、「Raspberry Pi imager」というソフトを使ってSDカードに書き込む方法がよいそうです。
とりあえずやってみる
公式サイトからWindows用をダウンロード
このソフトは、OSやそれ以外のソフトフェアをこのSDカードに書き込むときに利用するときに使用するようです。
今回は、OSを選択
次に書き込みしたいSDを選択
既に自動で検出されているのがすごい！
※SDカードの容量は、32Gが推奨されているらしい
SDカードの書き込みが完了後、ラズベリーパイにＳＤカードをセットして電源ＯＮすると、ＯＳが自動で立ち上がりました。
【三菱16ccl-m】CC-LINK 設定方法について
目次
２ 設定方法
２．２ パラメータ設定方法
①プロジェクトからCC-LINKを選択
②接続ブロック「あり」を選択してアクテイブにする。
③総接続台数を入力（今回は、２台）
④局情報を選択
⑤接続する機器の情報を入力
今回は、リモートI/Oは、１局（入力１６点、出力１６点）と
IAI リモートインテリジェント局が４局（入力６４点、出力６４点）となります。
設定自体は以上です。
接続したい機器が何局及び局の識別が分かれば簡単に接続できます。
３．ラダーの記述方法
本当は、通信状態などの情報を取得して、通信が確立したことを条件に入れるべきでしょうが、サクッと運用したい場合は、下記のラダーを一文書くだけでデータの送受信ができ、自動でデータがはいってきます。
■接点データの受信（外部機器→マスタ）
16ccl-mの接点データの先頭アドレス「H0E0」からM1000を先頭にK6（16 点×6つ）分のデータを受信する。
■接点データの送信（外部機器←マスタ）
16ccl-mの接点データの先頭アドレス「H160」からM1200を先頭にK6（16 点×6つ）分のデータを送信する。
■デバイスデータの受信（外部機器→マスタ）
16ccl-mのデバイスデータの先頭アドレス「H2E0」からD1000を先頭にK20（D1000～D1020）分のデータを受信する。
■デバイスデータの送信（外部機器←マスタ）
16ccl-mのデバイスデータの先頭アドレス「H1E0」からD1200を先頭にK20（D1200～D1220）分のデータを送信する。
【三菱FX3G＋16ccl-m】CC-LINKでIAI RCONを制御する
簡単な装置であれば、IOやプログラムステップ数も少ないため、今回余っている三菱FXシーケンサを使って、IAI RCONをCC-LINKで制御するシステムを構築しました。
目次
１. 三菱シーケンサ （ＰＬＣ）
三菱シーケンサ（PLC）には、大きく２つの種類があると思っています。ひとつは、Qシリーズのように電源、ＣＰＵ、入出力、インテリジェントユニットなど必要な機能によって、入れ替えが可能で、メモリ容量も多く、比較的大きな設備を作る場合は、Ｑシリーズを選定します。
今回は、I/Oも合計30点程度と規模の小さい設備にＱシリーズはもったいないってことで、そういうときには、FXシリーズを選定するようにしています。
２. FXシリーズ
FXシリーズは、電源、CPU、I/Oが一体となったシーケンサでこれ１つで簡単に装置の制御ができます。FXシリーズにも細かく仕様がことなりますが、高速カウンタやEEPROMメモリを内蔵、バッテリレスでメンテナンスフリーといった十分な仕様です。自分的には、コスパと用途からFX3Gシリーズが好みです。
EEPROMメモリ：シーケンサ電源を切っても、デバイスデータを保持することのできるメモリです。FX3Gだと、M384～M1535の152点、D128～D1099の972点が固定値で設定されています。
３. IAI RCON
装置アクチュエータにはエアシリンダー以外に、電動シリンダーにて位置決めで治具の位置を動的に変更する仕様を考えています。今までは、PCONやSCONを使っていたのですが、選定したアクチュエータはRCONを使うようで、このような商品があることを知りませんでした。
RCONの特徴として、１台のコントローラで最大2台制御できるという点で、2台以上制御する場合におすすめのようです。また、幅ば22ｍｍとかなり小さく今までのユーザーからすると驚きました。
また、簡単に増設できるところが新しいです。今までは追加で1台1台買って配線していたので、素晴らしい仕様です。
ただ、入力電圧はDC24V仕様です。今回4台接続させるため電流は15A近く必要となります。（100～200V仕様だとうれしいのですが）
てなことで、仕方なく、IAIのパワーサプライPSA-24Lを2台購入して並列で結線しています。
https://www.iai-robot.co.jp/download/catalog/pdf/CHIRASHI/CHIRASHI_PSA-24(20200508-01).pdf
４. システム構成
手元にあったFX3GでRCONとCCLINKにて通信し、電動シリンダを制御するものです。FX3GのI/Oが足りなかったので、リモートI/Oを追加して入出力接点を追加しました。
【三菱PLC＋エンコーダー】送り量を計測する
目次
１．やりたいこと
サーボモータを使ったワイヤを送る機構を使った装置を検討中。但し、モータの指令値（どのくらいモーターを回転させる＝どのくらいの長さを送る）を出しても、実際に送られるワイヤが指令通りワイヤが送られているかわからない。ワイヤと送り機構の間で滑ったりしてしまい、指令値に対して正しく出力（今回だとワイヤの出量）が出ているかを確認するために、エンコーダーを送り機構に取り付け、ワイヤの送り量を計測したい。
２．システム構成
2.1 PLC（シーケンサ）
今回は、使いなれた三菱シーケンサQシリーズで構成しました。
アドレスは以下のようになっています。
スロット１：00～1F（QH42P 入出力ユニット）
スロット２：20～3F（QJ61BT11N CCリンクユニット）
スロット３：40～4F（Q68ADV アナログ変換ユニット）
スロット４：50～5F（QD62 高速カウンタ）
2.2 高速カウンタとの接続するために
エンコーダーの入力を受けるためにQD62高速カウンタに接続します。
当然、ユニットには直接接続できないため、下記の端子台（A6TBXY36）及び接続ケーブル（AC20TB）を利用しました。
補足1：AC20TBの数値は、ケーブル長さにより変わります。
補足2：使用する電源はDC24V
引用：三菱電機
高速カウンタユニットユーザーズマニュアル（詳細編）
配線図はこのようになります
引用：三菱電機
MELSEC iQ-R 入出力ユニットユーザーズマニュアル
2.4 端子台とエンコーダーの接続
上記マニュアルを参考に、このように配線しました。
Z相は回転方向（時計まわり、反時計回り）に使用するようですが、今回不要だったため、処置しませんでした。（どうやって使うのか調べてなかったです・・・・）
【PYTHON＋OPENCV】回転不変位相限定相関法（RIPOC）を実装する
目次
１．やりたいこと
２つの画像間で特徴点を抽出して、その差分から誤差が最小になるように画像を幾何学的（回転、平行）変換から２つの画像間の式を求めたい。この式が精度良く求めることができると、カメラのキャリブレーション、時空間フレームの処理、ステレオカメラなどの視差画像など画像間の「照合」や「位置合わせ」に応用できる。
これら、「照合」や「位置合わせ」（画像１の画像座標Xが画像２の画像座標X"と対応していることを求めること）の方法として、いろいろなアルゴリズムがあるが、今回は回転不変位相限定相関法について実装してみた。
２．RIPOCとは
回転不変位相限定相関 (RIPOC)は位相限定相関 (POC)の応用の一つであり、登録画像に対して回転している入力画像に対しても相関をとることができ、相関結果から回転角度を推定することができるという特徴を持っているとのことです。
https://www.jstage.jst.go.jp/article/itetr/22.45/0/22.45_55/_article/-char/ja/www.jstage.jst.go.jp
理論は、下記が参考になるかと思います。私には数式の難易度が高くて
ついていけないなかったです。
東北大学 青木研究室様が位相限定相関法で有名のようです。
https://www.jstage.jst.go.jp/article/itej/63/7/63_7_935/_pdf/-char/ja
３．実装
実装にあたっては以下のサイト様を参考にしました。
import cv2 import numpy as np #回転角度を算出する関数 def CalcDeg(dy, len): deg = dy / len * 360 return deg #回転不変位相限定相関法 def Ripoc(): #画像の読み込み im1 = cv2.imread("Lenna.jpg",0) im2 = cv2.imread("Lenna90.jpg",0) #画像サイズの取得 cy,cx = im1.shape #窓関数の生成 win = cv2.createHanningWindow((cx,cy), cv2.CV_64F) #入力画像１，２に窓関数をかける wim1 = im1 * win wim2 = im2 * win #極座標変換 im1p = cv2.linearPolar(wim1, ((cx/2.0,cy/2.0)), r, cv2.WARP_FILL_OUTLIERS) im2p = cv2.linearPolar(wim2, ((cx/2.0,cy/2.0)), r, cv2.WARP_FILL_OUTLIERS) #32Bit画像に変換 im11 = np.float32(im1p) im12 = np.float32(im2p) #位相限定相関法によるマッチング xy, match = cv2.phaseCorrelate(im11,im12) #得られた結果から、画像１と画像２間の角度を取得 scalar, r = xy deg = CalcDeg(r, cy) return deg
４．処理結果
実行結果は、以下の通りです。
入力画像１のオリジナルの画像に対して、90度回転させた画像を入力画像２とした２つを用意し、下記のプログラムを実行し回転角度を計測した結果、ほとんど誤差がない結果が出ました。
但し、これはオリジナルの画像とオリジナルの画像を90度回転させたものである。入力画像２を別の画像でとった場合、その他の回転方向の誤差により、誤差が多くなるかもしれません。
>>> import ripoc as r >>> deg = r.Ripoc() >>> deg -89.78454078999397
【入力画像 0度】
【入力画像 90度】
【極座標変換 0度】
【極座標変換 90度】
【業務日誌】明日は憂鬱なイベントが・・・
本日の生産技術サラリーマンの一日です。
- 6時半：ワクチン起床。やはり副反応がのこる。37.1度だった。とはいえ、解熱剤を飲んでいく。
- 8時：朝の朝礼。担当から今日の業務内容を聞く。派遣さんひとりの仕事の進捗が思わしくない。聞き取りを行い、たまっている業務を確認。バックアップのため、２件引き取る。
- 8時半：引き取った２件の着手。治工具の改修依頼書と作業指示書の改定。内容は軽微な訂正のため、サクッと終了
- 9時：課長が来る。不具合の件を聞かれる。自部門に責任があるわけでないが、状況や進捗を聞かれる。不具合の状況を正確に把握していないので、別途調査して報告しますとお伝えする。
- 10時半：不具合の事象、原因を調査開始。担当者からの情報や図面をみて不具合の原因に至った過程をまとめ、課長に報告
- 12時：昼飯。今日は、冷やしきしめん
- 13時：現場へ移動。修理日程の確認し、お客さんへの説明の日時を決めてほしいと現場品証さんへ依頼
- 13時半：自動化試作装置の組立作業。9月から動作検証開始したが、まだ、いろいろと終わっていない。治具テーブルの再配置を行いロボットの可動範囲内に移動。（基本の基本ができてなかった）。
- 15時：昨日動作しなかった自動化設備に移動。再度電源を投入したところ動いた。昨日はなぜ動かかなった。。。動作確認を行い、予想通りの結果だったため、事務所に戻り、設備メーカーに連絡
- 16時：社内の新しいシステムについて、質問される。即答できないため、わかる人に聞いて、回答する
- 17時：担当から治工具の確認／点検依頼をされる。見切れないため、明日に回す。
- 19時：帰ろうとするが、メールを見ると不吉なタイトル。恐る恐るメールを見ると、会議に参加しろとの指示。最近担当された新しい業務だが知らないことが多すぎで、気が乗らない。しかも、よく見たら明日の9時。場所すら書いてない。考えるのやめます。帰ります。
本日の勤務終了です。
