KMR-M6をリモートブレインで動かす(7回目)

Posted on 2012.05.22 in

 

7回目:お掃除ロボットのように距離データをもとに考えて行動する

◆前回のおさらい

前回までは、距離データの取得やモーションの再生など各主要機能の実装を行ってきました。最終回はそれらの機能を統合してお掃除ロボットの行動を真似するようなプログラムを実装します。

◆前記事

1回目:導入

2回目:お掃除ロボットの動きを再現させるための下準備

3回目:RCB4ライブラリの紹介と使い方

4回目:アナログデータを取り込み距離データを取得する

5回目:サーボ単体を動かし、周囲の距離を測定する

6回目:モーションを再生させ、ロボットを移動させる


◆PSDセンサをロボットに取り付ける

KMR-M6の頭のダミーサーボをPSDセンサーつきのサーボモータに取り替えてください。

PSDセンサの取り付け2

◆Windowsプログラムの作成

Windowsのプログラムを作成します。Visual C#でWindowsフォームアプリケーションを新規作成しプログラムを作りますが、今回はプログラムが長いため、先にサンプルプログラムを配布します。参考にしてください。

【サンプルプログラムダウンロード】AutoMove.zip

必要なコントロールを下記のように実装します。

Form作成画面_ノーマル_文字付

プロパティは以下のように変更してください。

コントロールNameプロパティ名プロパティ値機能
Form AutoMoveNoSave (Name) AutoMoveNoSave 作成したwindowの表示
Text 自律ロボット制御プログラム
Size 580,500
PictureBox pictureBox1 Size 400,400 地図の表示
Button button1 Text 障害物ストップ 障害物があると止まる
button2 回避1方向 一定方向回避を開始する
button3 停止 自律移動を停止させる
button6 5点スキャンマッピング 前方5点の距離測定をして移動させる
button7 11点スキャンマッピング 前方11点の距離測定をして移動させる
button8 行動計画つき 距離データをもとに行動パターンを何種類も変える
Label roboPosition (Name) roboPosition ロボットの位置座標の表示
roboAngleLabel roboAngleLabel ロボットの向きを表示

プログラムを書きやすくするために4回目で説明いたしました「using ~ 」を追加します。

次に、4回目~6回目で実装した下記の関数をコピーしFormクラス内に貼り付けます。

関数名紹介箇所機能説明
コピーして実装する関数
GetRangeADData 4回目 PSDセンサーのデータを取得
GetADData アナログデータを取得
RangeADToConvert PSDセンサーのAD値を距離データに変換
OneServoMove 5回目 サーボモーターを動かす
servoAngleConvert 角度をポジションに変換する
MotionPlay 6回目 モーションを再生する
MotionSuspend モーションをサスペンドする
PgCounterReset プログラムカウンタのリセット
MotionAddressSet モーションのアドレスをセットする
MotionRestart モーションをリスタートする
MySleep 自作Sleep関数
GetConfigData RCB-4HVのSYSTEMレジスタ値を読み出す

下記プログラムのように、public partial class Form1 : Formの後の中括弧で囲まれたところに貼り付けます。メンバ変数も同様です。

その他描画等に必要な関数をサンプルプログラムからコピーしてFormに貼り付けます。

関数名機能説明
サンプルプログラムからコピーする関数
RangeDataScan サーボモーターとPSDセンサーで周囲の距離を取得する
RangeDataScan2 RangeDataScanに距離データを返す機能を付加
PictDrawPoint pictureBox1の指定した座標に点を打つ
PictDrawLine pictureBox1の始点の座標と大きさと角度を指定し線を引く
MoveForward 前進モーションを再生し、ロボット座標を変更し、地図に表示も行う
TurnRight 右旋回を行う
TurnLeft 左旋回を行う
RangePointDraw 地図(pictureBox1)に距離データの点を打つ

Form全体で使う変数や規定値などをメンバ変数として宣言するため、以下をコピーし、Formのクラス内に貼り付けます。

次に通信部分のプログラムを作成します。本記事3回目を参考にしてserialPort1とFormのLoadイベント関数、FormClosingイベント関数を実装します。serialPort1のプロパティ値は本記事3回目をご覧ください。今回もSYSTEMレジスタ値が必要になりますので、本記事6回目を参考にして、Loadイベント関数内で通信設定が終了した後、GetConfigData関数でSYSTEMレジスタ値を読みます。

最後に、ロボットを停止させるためのボタンを実装します。自律移動で繰り返し動作を行っていますが、止める場合はstopFlag変数を見てtrueの時に止まるようにしました。そのstopFlag変数を変えるタイミングは停止ボタンを押したときです。停止ボタン(button3)のClickイベント関数を実装し、下記プログラムを記述します。


◆センサデータをもとにモーションを変更する

*一定距離になると止まる

始めに一定距離になったら止まるプログラムを作成します。距離を計測して、一定距離より離れている場合は前進、一定距離以内になると停止させます。

障害物ストップボタン(button1)のClickイベント関数に以下のプログラムを記述します。このプログラムでは、測定した距離が30cm以内になるとモーション番号1(ホームポジション)を再生し前進をやめます。

実際に動かすと、以下の動画の通りになります。

*一方向に回避する

距離データをもとにKMR-M6の動作を変化させることができました。次に、もう少し複雑な動きを実装してみましょう。次のプログラムは、前方に障害物があると左に旋回し、回避するプログラムです。障害物がない場合は前進します。回避1方向ボタン(button2)のclickイベント関数に以下のプログラムを実装します。

実際に動かすと、以下の動画の通りになります。


◆自分の位置を把握する方法

次に、地図を作成するプログラムを作成します。地図を作成するためには、動作中のロボット位置情報が必要です。ロボットの進む前の位置に進んだ距離や方向を足し合わせてフィールド上の位置を求めます。

ここで、ロボットを中心(原点)とした座標をロボット座標、ロボットが移動する平面(フィールド)をワールド座標と呼びます。ロボットを中心とした移動量はロボット座標であり、ロボットの移動量を元にフィールドのどこにいるのか位置を求めるにはロボット座標からワールド座標に変換し、計算する必要があります。回転(旋回)方向は1軸なので、ロボットの向きに関してはそのまま足し合わせることができます。

ロボット座標にあるロボットの移動量をワールド座標に変換するには、下記図を参考にロボットが進んだ距離rとロボットの向きθrから求めることができます。

移動距離の積算

xr=r * cos θr
yr=r * sin θr

進んだ距離をワールド座標に変換することができましたので、移動前の座標(X,Y)から移動後の座標(X',Y')を求める方法は以下のようになります。

X' = X + xr
Y' = Y + yr

これらの計算式をプログラムに直すと以下のようになります。C#で使う三角関数は、「度(deg)」ではなく「ラジアン(rad)」を用いるのプログラム中では角度にπ/180°をかけて変換します。

このプログラムを組み込み、初期位置からモーション再生毎に進んだ距離を足し合わせば、ロボット自身の位置を算出する事ができます。サンプルプログラムの前進モーションを行う関数(MoveForward関数)内でそれらの計算を行っています。

◆ロボットの中心とPSDセンサーの位置補正

これまでは、ロボット座標の中心(原点)をロボットの中央にして計算を行ってきましたが、PSDセンサーはロボットの中心にはありません。ロボット前方約9cmの位置に配置されます(下記図)ので、PSDセンサーの座標位置を考慮して計算します。

PSDセンサの位置

ロボット座標にあるPSDセンサーの位置座標(Xs,Ys)をワールド座標に変換するには、ロボットの座標(X,Y)とロボットの向き(θr)を用いて以下のように計算します。

Xs = X + 9.0 * cos θr
Ys = Y + 9.0 * sin θr

次に、測定した距離データをワールド座標に変換します。

距離データをワールド座標系に

取得した距離データ(d)を元に障害物の位置をワールド座標(Xd,Yd)に変換するには、PSDセンサーの位置座標(Xs,Ys)とセンサーを振った角度(θd)を用いて以下の式になります。

Xd = Xs + d * cos(θrd)
Yd = Ys + d * sin(θrd)

これらの式を使うと、ロボットで取得した距離データを地図に反映することができます。先ほどの距離データから障害物の位置をワールド座標に変換する式をプログラムにすると以下のようになります。このプログラムは地図に障害物をプロットするRangePointDraw関数内に実装しています。

◆地図作成プログラムと複数点の距離を取得したときの違い

PSDセンサーを回転させ、距離を測定すると距離データは地図上にプロットされます。また、ロボットを動作させたあと自動的に現在位置もプロットしていますので、ロボットを動かせば自動的に地図が作成されます。このプログラムを左下図のフィールドで実行したとき作成された地図は右下図のようになりました。

長方形マップとり直しリサイズ 1点マップ
計測フィールド 前方スキャンのみの地図

フィールドは、90cm×180cmです。地図の青い点がロボットの経路、黒い点が距離データをプロットした点です。地図画像の元データは1pxあたり1cmで表示しています。前方の1点しか見ていないので、壁がほとんど見えておらず、地図と呼べるほどではありません。そこで、PSDセンサーがついている頭のサーボモーターを動かし、周囲の5点および11点の距離データを取得するようなプログラムに変更します(下記図)。本記事5回目で 計測した方法と同じように中心から角度と計測数を指定して距離を計測します。RangeDataScan関数およびRangeDataScan2関数は全体角度と計測数を渡すと、PSDセンサーついたサーボモータを回転させ周囲の距離を取得します。

5点、11点スキャン

5点スキャンマッピングボタン(button6)および11点スキャンマッピングボタン(button7)に次のようにClick関数を実装します。違いは、RangeDataScan関数の計測数のみです。5点スキャンマッピングボタンのときRangeDataScan関数の第1引数は5、11点スキャンマッピングボタンのときRangeDataScan関数の第1引数は11にします。下記はbutton6のClickイベント関数を記述しています。

実装したプログラムを実行し比較します。以下が作成された地図です。

5点マップ 11点マップ
5点計測地図 11点計測地図

11点計測を行い、地図を作成したほうが障害物(壁)が細かくプロットされていることがわかります。最初の1点のみ計測した場合と比較して明らかに壁があるということがわかる地図になりました。この結果から11点の周囲距離の計測を行う事にします。

◆センサ情報をもとに行動計画を生成

今までは障害物を見つけると左旋回しかしないプログラムでしたが、複数点距離を計測することで、行動の幅が広がります。そこで距離のデータをもとに、左旋回や右旋回など複数の行動パターンでロボットを動かし、より詳細な地図を作るプログラムを作成します。障害物との距離やロボットの向きなどを考慮すると、たくさんの動作パターンがありますので、フローチャート(下記図)を参考に説明していきます。フローチャートで出てくる左右の判別ですが、PSDセンサー付きのサーボモータを左から順に回転させ距離データを取得しています。つまり、計測位置の中央を基準とすると最初のほうに取得した距離データは左側、最後のほうに取得した距離データは右側に位置します。左右一定距離以内に障害物がある場合は、左右それぞれの距離データの最小値を計算し、どちら側に障害物が近いか判断しています。

回避アルゴリズム

①距離データを取得する

②両足元に障害物がある場合は8回左旋回をして後ろを向く

周囲の距離を11点で取得しているので、左右どちらとも18cm以内に物があれば、前進することができないと判断して、後ろを向くことにしました。

③片方の足元に障害物がある場合は障害物のない方向に旋回する

左右どちらか18cm以内に障害物がある場合は、障害物がない方向に旋回します。

④前方一定距離(30cm)以内に障害物がある場合はランダムで旋回する

18cm以内に物がない場合で前方30cm以内に物がある場合は前進してもぶつかるため、左右どちらかフィールドが開いているほうにランダムな回数だけ旋回します。左右どちらかフィールドが開いているほうの判別は、左右それぞれ取得した距離データの合計を計算し、結果が大きいほうを開いている側とします。

⑤何もなければ前進する

①で測定した距離データを基に②~⑤の行動を選択します。これらの行動を繰り返します。

以下のように行動計画つきボタン(button8)のClickイベント関数に先ほどのロボットの行動を実装します。

最初の例よりも少々複雑なL型のフィールドで実行を行った結果、以下のようになりました。上の動画で再生した時の地図が下の動画になります。おおよそのフィールドの形が出来上がっていくことがわかります。

 

◆まとめ

ある程度限定的ではありますが、お掃除ロボットの動きのまねができるようになりました。作成した地図をもとに、まだロボットが行ったことのない方面に積極的に向かうようなプログラムを作成するなど応用できると思います。


◆サンプルプログラム(フルバージョン)

BMP画像で地図を保存する機能をつけたサンプルプログラムをZIPファイルにて配布します。地図を保存したい場合はこちらのサンプルをお使いください。ただし、説明は省略させていただきます。本連載では、このサンプルプログラムで、地図作成経過を記録し、動画作成に使用しています。

【ダウンロード】AutoMove_Full.zip


◆最後に

KMR-M6をリモートブレインで動かし、お掃除ロボットの行動をまねできるようになりました。リモートブレインで動かせるということで、ほかにも様々な可能性があります。

たとえば、天井カメラなどでロボットの位置を取得し、ロボットを指定した場所に移動させることもできます。

KBT-1接続先を増やすこともできますので、複数台のロボットをPC側のひとつのソフトウェア(プログラム)で同時に動かし、協調作業をさせることも可能です。

読者の皆様も、応用を考えてリモートブレインでロボットを動かしてみてはいかがでしょうか?

今まで7回にわたり読んでいただきありがとうございました。

 

KHR-3HV Ver.2 リフェバッテリー付きセットの詳細をみる KHR-3HV Ver.2の詳細をみる KHR-3HV Ver.2 セレクトパックの詳細をみる KMR-M6 Ver.2 リフェバッテリー付きセットの詳細をみる カメ型ロボット02 Ver.1.5の詳細をみる KMR-P4 Ver.1.5 リフェバッテリー付きセットの詳細をみる BluetoothモジュールKBT-1の詳細をみる