はじめに
こんばんは
酔った勢いで買ったもののちょっと遊んだだけで置物にしてしまっているので
また遊んでみようと思います。
今回使ったのはこちらのDobot magicianです。
ロボットビジョンしてみたいので今回はその方法と準備編です。
※尚、私はロボットビジョンとかそれに必要な数式をよくわからず雰囲気でやっています。
間違っている部分も多々あると思いますのでご容赦ください
ロボットビジョンとは
こちらやこちらのサイトを見ると分かるかと思います。
ロボットは同じ動作を繰り返すのが得意です。基本的には人が教えた(ティーチング)動作そのままで動きます。
↓の公式サイトの動画が分かりやすいと思います。
ピッキングの様子
このようなピッキングを行う場合、ブロックの位置は常に同じ場所にいないといけません。
そのブロックの位置を指定し、ピッキングさせているから当然ですね。
■ちゃんとつかめる
■ちゃんとつかめない
ブロックの位置が毎回定まらない場合でもピッキングを行いたい。
そういった時にカメラが目の代わりをしてあげるのがロボットビジョンです、多分。
やりたいのは下のような感じ。
ブロックの元あった場所からのずれ量 か ロボット座標系での位置を教えてあげればいい感じでしょうか。
どうすればいいか考えてみる
今回は2Dカメラを使うので高さ情報は取れません。
カメラから2次元情報(X,Y)をロボットに渡して2次元平面での位置を教えてあげます。
ではカメラから得た情報をどう解釈してロボットに伝えてあげればいいか考えてみます。
まずは座標系の違いを考えます。
本当はカメラ座標系も考えないといけないみたいですが
今回は画像座標系とロボット座標系だけを考えます。
(あとワールド座標系？)
カメラの取り付け方向で変わりますが今の二つの座標系はこんな感じです。
方向がてんでばらばらですね。
単純に考えると軸の向きが合うように正負を反転し、対応する軸に置き換えてあげればよさそうです。
・画像座標系でのX軸の正負を反転したものがロボット座標系でのY軸
・画像座標系でのY軸の正負を反転したものがロボット座標系でのX軸
ただこれは二つの座標系平面が平行かつ直行していないとけっこう誤差が出るはずです。
例えばカメラの取り付けが撮像面と平行になっていないとレンズに近い方が大きく見え、遠い方が小さく見えてしまい同じ物体でも画像内の映る位置で誤差が出てしまいます。
また、直行していない場合は
軸の傾きの分だけ誤差が大きくなります。
これらの問題ってどうやって解消できるんでしょう。。。
傾きの問題はずれている角度が分かれば三角関数でなんとかなりそうですが。。。
その角度を出す方法がわからないのでとりあえずスルーします。
あとはスケールを考えなければいけないみたいです。
画像のピクセル情報だけでは画像における大きさは分かっても現実世界でどれくらいの大きさなのかは分かりません。
今回使ったカメラは200万画素(1920×1080)なのでそれで考えてみます。
縮尺ガバガバですが気にしないでください。
だいたいの値は画像内にものさしを映してみるのが一番早いと思います。
↓は歪み補正後の画像です。歪み補正についてはこちらを参照ください。
だいたい1920ピクセルが43.2cm(432mm)なので1ピクセルあたり0.25677083333000000342cm(0.025677083333mm)になりますね。
なので10ピクセルの物体はだいたい2.5cmくらいと分かります。
そしてこの1ピクセルあたりの大きさが精度に大きく関わってきます。
今の私の環境では0.25677083333000000342cm(0.025677083333mm)がロボットに教えてあげる事のできる最小の値になります。
※サブピクセル処理はしません。
もっと小さい値でロボットに指示したい場合は撮像範囲を狭めてレンズを望遠のものにするかカメラをより解像度の大きいものにする必要があります。
色々書きましたがこれら諸々を頭に入れて画像座標系の値をロボット座標系の値に変換します。
座標変換(画像座標系 → ロボット座標系)
座標の変換方法って色々あると思うのですが今回は射影変換を使ってみました。
画像処理では歪みとか傾きの補正によく使われているやつですね。
詳しいことは省きますが射影変換は変換前座標と変換後座標の4点から変換式を作ってくれるすごいやつです。最少が4点なので4点以上でもいいみたいです。
今回はこの変換式を作るのをやってみたいと思います。
とりあえず必要なのが画像座標系での4点とロボット座標系での4点になります。
この4点の場所はそれぞれ一致している必要があります。
とりあえずコードを書いていきましょう。
画像座標の4点を取得する為にOpenCVのArUcoを使います。
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using HomographySharp.Double;
using MathNet.Numerics.LinearAlgebra.Double;
using OpenCvSharp;
namespace ArUcoSample
{
class Program
{
static void Main(string[] args)
{
const string YamlFilePath = @"calib.yml";
// webカメラキャプチャ
var camera = new OpenCvSharp.VideoCapture(0)
{
//// 解像度の指定
FrameWidth = 1920,
FrameHeight = 1080
};
using (camera)
{
var src = new Mat();
// ずっとループ
while (true)
{
// カメラ内部パラメータ格納用
Mat mtx = new Mat();
Mat dist = new Mat();
// ymlファイルを読み来み計算パラメータを取得
using (var fs = new FileStorage(YamlFilePath, FileStorage.Mode.Read))
{
mtx = fs["mtx"].ReadMat();
dist = fs["dist"].ReadMat();
}
// 撮影画像の読み取り
camera.Read(src);
if (src.Empty())
{
break;
}
if (Cv2.WaitKey(300) == 13)
{
Console.WriteLine("Enterキーが押されました");
break;
}
Mat calib = new Mat();
// 歪み補正
Cv2.Undistort(src, calib, mtx, dist);
var p_dict = OpenCvSharp.Aruco.CvAruco.GetPredefinedDictionary(OpenCvSharp.Aruco.PredefinedDictionaryName.Dict4X4_50);
Point2f[][] corners, rejectedImgPoints;
var detect_param = OpenCvSharp.Aruco.DetectorParameters.Create();
int[] ids;
// マーカー検出
OpenCvSharp.Aruco.CvAruco.DetectMarkers(calib, p_dict, out corners, out ids, detect_param, out rejectedImgPoints);
if (ids.Length != 0)
{
// 検出されたマーカ情報の描画
OpenCvSharp.Aruco.CvAruco.DrawDetectedMarkers(calib, corners, ids, new Scalar(0, 255, 0));
// 番号順にする為にids,cornersを固めて
// dictionaryにしてソート
var markers = Enumerable.Zip(ids, corners, (i, c) => new { i, c })
.ToDictionary(x => x.i, x => x.c)
.OrderBy(i => i.Key);
List<Point2f> midllePoints = new List<Point2f>();
int cnt = 0;
foreach (var marker in markers)
{
var average_X = marker.Value.Average(p => p.X);
var average_Y = marker.Value.Average(p => p.Y);
// マーカーの中心座標を取得
midllePoints.Add(new Point2f(average_X, average_Y));
Console.WriteLine($"marker{cnt} X:{average_X}, Y:{average_Y}");
cnt++;
}
// マーカーの中心座標を描画
midllePoints.ForEach(mp => calib.Circle(
(int)mp.X, (int)mp.Y, 1, new Scalar(0, 0, 255), 3, LineTypes.AntiAlias
));
Cv2.ImShow("marker", calib);
mtx.Dispose();
dist.Dispose();
calib.Dispose();
}
}
Cv2.DestroyAllWindows();
}
}
}
}
4点あればいいのでmarker0,2,6,8の値を覚えておきます。
【画像座標系】
marker0(左上): 667, 241
marker2(右上): 1111, 237
marker8(右下): 1115.75, 680.75
marker6(左下): 671, 684.5
次にロボット座標系の対応する4点を取得します。
Dobot Studioを使ってアームを動かして座標を取得します。
こんな感じでマーカーの中心位置にアームの先端を持ってきてボタンを押します。
4点取れました
【ロボット座標系】
marker0(左上): 271.2993, 48.5128
marker2(右上): 270.3106, -51.8232
marker8(右下): 171.3913, -50.9119
marker6(左下): 175.4043, 47.8111
上記で各4点を使って変換式を作成します。
OpenCvでも出来ると思うのですがこちらの射影変換ライブラリを使用しました。
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using HomographySharp.Double;
using MathNet.Numerics.LinearAlgebra.Double;
using OpenCvSharp;
namespace ArUcoSample
{
class Program
{
static void Main(string[] args)
{
const string YamlFilePath = @"calib.yml";
// webカメラキャプチャ
var camera = new OpenCvSharp.VideoCapture(0)
{
//// 解像度の指定
FrameWidth = 1920,
FrameHeight = 1080
};
using (camera)
{
var src = new Mat();
// ずっとループ
while (true)
{
// カメラ内部パラメータ格納用
Mat mtx = new Mat();
Mat dist = new Mat();
// ymlファイルを読み来み計算パラメータを取得
using (var fs = new FileStorage(YamlFilePath, FileStorage.Mode.Read))
{
mtx = fs["mtx"].ReadMat();
dist = fs["dist"].ReadMat();
}
// 撮影画像の読み取り
camera.Read(src);
if (src.Empty())
{
break;
}
if (Cv2.WaitKey(300) == 13)
{
Console.WriteLine("Enterキーが押されました");
break;
}
Mat calib = new Mat();
// 歪み補正
Cv2.Undistort(src, calib, mtx, dist);
Cv2.ImShow("calib", calib);
var p_dict = OpenCvSharp.Aruco.CvAruco.GetPredefinedDictionary(OpenCvSharp.Aruco.PredefinedDictionaryName.Dict4X4_50);
Point2f[][] corners, rejectedImgPoints;
var detect_param = OpenCvSharp.Aruco.DetectorParameters.Create();
int[] ids;
// マーカー検出
OpenCvSharp.Aruco.CvAruco.DetectMarkers(calib, p_dict, out corners, out ids, detect_param, out rejectedImgPoints);
if (ids.Length != 0)
{
// 検出されたマーカ情報の描画
// OpenCvSharp.Aruco.CvAruco.DrawDetectedMarkers(calib, corners, ids, new Scalar(0, 255, 0));
// 番号順にする為にids,cornersを固めて
// dictionaryにしてソート
var markers = Enumerable.Zip(ids, corners, (i, c) => new { i, c })
.ToDictionary(x => x.i, x => x.c)
.OrderBy(i => i.Key);
List<Point2f> midllePoints = new List<Point2f>();
int cnt = 0;
foreach (var marker in markers)
{
var average_X = marker.Value.Average(p => p.X);
var average_Y = marker.Value.Average(p => p.Y);
// マーカーの中心座標を取得
midllePoints.Add(new Point2f(average_X, average_Y));
//Console.WriteLine($"marker{cnt} X:{average_X}, Y:{average_Y}");
cnt++;
}
// マーカーの中心座標を描画
midllePoints.ForEach(mp => calib.Circle(
(int)mp.X, (int)mp.Y, 1, new Scalar(0, 0, 255), 3, LineTypes.AntiAlias
));
// ここから座標変換
var srcList = new List<DenseVector>(4);
var dstList = new List<DenseVector>(4);
srcList.Add(DenseVector.OfArray(new double[] { 667, 241 }));
srcList.Add(DenseVector.OfArray(new double[] { 1111, 237 }));
srcList.Add(DenseVector.OfArray(new double[] { 1115.75, 680.75 }));
srcList.Add(DenseVector.OfArray(new double[] { 671, 684.5 }));
dstList.Add((HomographyHelper.CreateVector2(271.2993, 48.5128)));
dstList.Add(HomographyHelper.CreateVector2(270.3106, -51.8232));
dstList.Add(HomographyHelper.CreateVector2(171.3913, -50.9119));
dstList.Add(HomographyHelper.CreateVector2(175.4043, 47.8111));
// 射影変換行列を求めて
var homo = HomographyHelper.FindHomography(srcList, dstList);
// 入力平面から出力平面上の座標に変換
// 試しにmarker0の位置を変換(画像座標系→ロボット座標系)
(double X, double Y) = homo.Translate(667, 241);
Console.WriteLine($"homography_ROBOT:X:{X}, Y:{Y}");
mtx.Dispose();
dist.Dispose();
calib.Dispose();
}
}
Cv2.DestroyAllWindows();
}
}
}
}
試しにmarker0(左上)の位置をロボット座標に変換してみました。
結果は↓になります。
(変換前)667, 241 ⇒ (変換後): 271.2993, 48.51279
実際のロボット座標系でのmarker0の位置は
marker0(左上): 271.2993, 48.5128
だったので良さそうです。
他のマーカー位置でも試して問題なさそうなら座標の変換がこれでいけそうです。
次回は簡単にアプリを作ってロボットビジョン事してみたいと思います。
参考URL・出典元
https://www.keyence.co.jp/ss/products/vision/fa-robot/robot_vision/
https://www.keyence.co.jp/ss/products/vision/visionbasics/use/robot_vision.jsp
https://blog.neno.dev/entry/2019/01/12/homographysharp/
https://github.com/maxosprojects/dobot-arm-CAD