【C#】数式とかよく分からないけどロボットビジョンしてみる【DobotMagician】

## はじめに
こんにちは[前回投稿](https://qiita.com/mototoke/items/7650a3db93a9e60f2c2e)の続きをやろうと思います。
脳死で組んだので香ばしいコードになってしまいましたがなんとなく動いてるのでヨシ！とします。

こんな感じで動きます。
※上にwebカメラがあってそれで緑のブロックを位置検出してます。
[![IMAGE ALT TEXT HERE](http://img.youtube.com/vi/cYwpP5QCVOU/0.jpg)](http://www.youtube.com/watch?v=cYwpP5QCVOU)

## 開発環境
OS:windows10
.net framework4.7.2
使用ライブラリ：
OpenCvSharp4.Windows 4.3.0.20200421
HomographySharp 1.5.0
Newtonsoft.Json 12.0.3

## 必要な値を設定する
まずはロボットビジョンを行う為の設定をしていきます。
設定画面はこんな感じです。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/168779/49db40b3-6560-c236-ec11-c636878ed924.png)

#### キャリブレーションファイルを設定
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/168779/d5353fa1-3f2c-9f66-42c2-27592d0efa80.png)
キャリブレーションファイルのパスを設定します。
キャリブレーションファイルについては[こちら](https://qiita.com/mototoke/items/afd38d11395df57ce865)

#### 射影変換用の4点を設定する
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/168779/f4e8dc05-1d0d-7905-a5d2-e7389167b374.png)
画像座標系からロボット座標系に変換する為のパラメータ各4点を設定します。
各4点は[前回](https://qiita.com/mototoke/items/7650a3db93a9e60f2c2e)の値を使います。

#### 高さ情報の設定
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/168779/47e57e2e-d612-3909-13b5-bebf46c732d7.png)
今回は緑のブロックをつかみに行きます。
少し分かりづらいですがロボット座標系のＺ軸=0でサクションカップを付けた状態では
アーム先端と台座上面がだいたい同じ高さにきます。
台座の高さ幅が78mmくらいなのでZ軸=-78で床に先端が付きます。
一方、今回の緑ブロックの高さは25mmです。
サクションカップでくっつけようと思うとZ軸=-53あたりを指定するといい感じにくっつきます。
なのでZ軸=-53を計算させようってだけですね。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/168779/c83c3f01-160a-8262-a0ea-8292e40b240a.png)



#### 緑のブロックを見つける
動画にある緑のブロックを見つけます。
カメラ撮影画像をHSV変換し、HSVで閾値きってマスクをかけます。
私の環境では以下の値でだいたい緑のブロックを検出できます。
これは撮像環境によってものすごく値が変動するので場所を頻繁に移すような場合は照明環境が必要になります。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/168779/6f63dde7-e18e-1fd6-22e4-317205f217de.png)

```Csharp:WebCameraControl.cs
                    // 撮影画像の読み取り
                    camera.Read(src);

                    if (src.Empty()) return null;

                    Mat calib = new Mat();
                    // 歪み補正
                    Cv2.Undistort(src, calib, mtx, dist);

                    // 画像処理
                    var tmp = new Mat();
                    // OpenCVのカラーの並びに変換
                    Cv2.CvtColor(calib, tmp, OpenCvSharp.ColorConversionCodes.RGB2BGR);
                    // BGR画像をHSV画像に変換
                    var hsv = new Mat();
                    Cv2.CvtColor(tmp, hsv, OpenCvSharp.ColorConversionCodes.BGR2HSV);
                    // inRange関数で範囲指定２値化 -> マスク画像として使う
                    var msk = new Mat();
                    Cv2.InRange(hsv, new Scalar(obj.HueMin, obj.SaturationMin, obj.ValueMin), new Scalar(obj.HueMax, obj.SaturationMax, obj.ValueMax), msk);
```

#### 持っていく場所を設定する
つかんだ緑のブロックを置く場所を設定します。
ロボットアームの届く範囲で設定します。
今回はmarker6の場所に設定しました。Z座標は良い感じにおける高さを設定します。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/168779/0395d3ea-2e6f-8826-2272-bb9cd8fd6d53.png)

## 作業をコードで書く
上記の設定で緑のブロックの位置は分かるようになりました。
次は実際の動きをコーディングしていきます。
コメントでわかると思いますが動きはこんな感じです。
0,カメラを撮影して緑のブロック位置を検出
--- ロボットの動き ---
1,緑のブロックの上まで移動
2,緑のブロック上面までアームを下げる
3,吸引ONしてブロックをくっつける
4,そのまま上に持ち上げる
5,持っていく場所に移動する
6,吸引OFFしてブロックを放す(置く)
7,ブロック置いたところからちょっと上に上がる

```Csharp:DobotDllWrapper.cs
        /// <summary>
        /// 作業を開始します
        /// </summary>
        public void WorkStart(double target_x, double target_y , SettingsObj obj)
        {
            // 作業をクリア
            DobotDll.SetQueuedCmdClear();
            // コマンド開始
            DobotDll.SetQueuedCmdStartExec();

            // 現在位置を取得
            var pose = GetCurrentPose();

            // 対象の上部まで移動(Z座標は適当)
            var cmdIndex = ptp((byte)2, (float)target_x, (float)target_y, (float)0.0, pose.rHead);

            // 対象物のZ座標計算
            var object_z = obj.PedestalZ + obj.ObjectHeight;

            // 対象の位置まで下がる
            cmdIndex = ptp((byte)2, (float)target_x, (float)target_y, (float)object_z, pose.rHead);

            // 下がったらサクションカップONしてつかむ
            DobotDll.SetEndEffectorSuctionCup(true, true, true, ref cmdIndex);

            // いったん上に持ち上げる
            cmdIndex = ptp((byte)2, (float)target_x, (float)target_y, (float)0.0, pose.rHead);

            // 持っていく場所に移動
            cmdIndex = ptp((byte)2, (float)obj.PlacePoseXCoordinate, (float)obj.PlacePoseYCoordinate, (float)obj.PlacePoseZCoordinate, pose.rHead);

            // 下がったらサクションカップOFFして放す
            DobotDll.SetEndEffectorSuctionCup(false, false, false, ref cmdIndex);

            // 上がって終わり
            cmdIndex = ptp((byte)2, (float)obj.PlacePoseXCoordinate, (float)obj.PlacePoseYCoordinate, (float)0.0 ,pose.rHead);

            // コマンド終了
            DobotDll.SetQueuedCmdStopExec();
        }
```
あとはこれを動かすだけです。

## 問題点
それなりに設定すれば動画のようにわりに動きはしますが
今回作ったプログラムは色々な問題があります。
・精度が悪い
→これはロボットの精度ではなく、画像処理の方です。
 撮影環境にも影響をうけますがエイヤで緑ブロック検出を書いたので綺麗に検出できる時と一部欠けて検出できる場合があります。なので毎回絶対にブロックの中心位置にアームの先端を持っていきたいとかの場合はもうちょっと頑張らないといけません。

・クローズループ制御、フィードバック制御がない
→今回は撮影した1枚で取得した座標にいきなりロボットを動かしてます。
 普通はロボットアームを動かしつつ、カメラでアーム位置を見ながらちゃんとブロック位置まで行けてるか見たりするような制御が必要だと思います。アーム先端にカメラをつけるとかもありだと思います。というかもっと精度が必要な場合は必須ですよね。

・ちゃんとつかめたかどうかわからない
→上と似た感じですが今回はつかめたかどうか確かめずに移動させています。
 だいたいこういうのはちゃんと掴めてるかどうか別の固定カメラとか使ってやるみたいですね。それがあればつかんだ位置のずれも検出できそうです。

他にも色々問題はあると思いますが書き上げるとキリがないのでこれくらいにします。
もう少し改善したいところですが力尽きたので気が向いたらにします。
検出のところはDLに置き換えてみようかなぁ。

ソースは[こちら](https://github.com/mototoke/MyDobotVisionForm)に

## 参考URL・出典元
https://www.robotlab.com/dobot-customers-download
https://teratail.com/questions/20557