■自律ロボット作例 ver.2
以前『自律ロボット作例』の記事で自律ロボットの制作例を紹介しましたが、第二回ビーチフラッグ競技会を開催するにあたって、進化した自律ロボットをご紹介します。
<制作例マニュアルダウンロード>
<サンプルプログラム>
今回は頭部のセンサーをＰＳＤ測距センサーからUSRX-1超音波センサー（受信部）に変更しました。このセンサーでフラッグから発信されている超音波を受信し、フラッグの位置を検出します。
前回、KRS4013シリアルサーボに変更していた頭部は、KRS-788に戻しKCB-1のPIOポートに接続して制御しています。KCB-1でのPWMサーボの制御は『KRS-788HVをKCB-1で動かす① ②』で紹介した方法で行っています。
サンプルプログラムでは最初の起き上がりからフラッグを倒すまでのプログラム例を公開しています。
内容：
・kyoris_s02_cc.c ― コントロールコード再生で制御する自律ロボットのプログラムです。
・kyoris_s02_mp.c ― コマンド再生で制御する自律ロボットのプログラムです。
・sonic_search2.h ― バブルソートなど、自律ロボットに必要な関数をまとめたヘッダーファイルです。
作例のように、KCB-1とRCB-3を使用すれば簡単に自律ロボットを製作することができます。是非お試しください！
■KHR-1自律ロボット作例
今回はKHR-1での自律ロボットを紹介します。
画像のKHR-1には前後左右に計４つのPSDセンサーを実装しています。これらのセンサーの値を読み取り、最も近い物体へKHR-1が向かうようにプログラムを組みました。
<画像 クリックして拡大>
写真は「KCB-1でKHR-1を動かす①、②」でKCB-1を実装したKHR-1にPSDを取り付けた画像です。頭部のフレームの前後と、両肩にPSDセンサーをひとつずつ取り付けました。
<ムービー (2.31 MB)>
画像のKHR-1を実際に動かした動画です。それぞれのPSDセンサーに手をかざすと、KHR-1が近づいてきます。
<サンプルプログラム ダウンロード>
サンプルプログラムは動画のKCB-1に書き込まれているものと同じです。以下はこのプログラムを説明します。
・サンプルプログラムの流れ：
①. ボードの初期化、RCB-1と通信開始。通信の準備ができたら赤LEDが点灯する
②. スイッチが押されるまで（AD2がHになるまで）待機している間、ニュートラル信号を流し続ける
③.スイッチが押されると、お辞儀モーションを一回再生
④ 無限ループ開始。ループ内では１回ループする度に必ずニュートラル信号を送信すること
⑤. 前後左右に取り付けたセンサーの値を読み取り、グローバルで宣言した配列に代入
⑥. センサーの値をバブルソートして、最も高い値を求める
⑦. バブルソートで求められた値を元に、モーション再生分岐
⑧. ④～⑦の処理を繰り返す
・解説：
②、④のように、モーションを再生していないときはなるべくニュートラル信号を送信するようにしてください。ニュートラル信号なしにモーションを再生すると、うまく動作しません。
⑥のバブルソートは複数の値の中で最も大きい値、または小さい値を求める処理です。詳しくは「センサーで目標を探す（KRS788使用）③」の解説をご覧ください。
■シリアルサーボの簡易位置制御
シリアルサーボモーターを回す場合は、sio_set_pos関数などで指定した目標角度に到達するまでには時間がかかります。かといって次のようなプログラムでは、むだ時間が発生します。
while (1) {
pos == sio1_set_pos(id, 目標)
if ((pos <= 目標 + 5) && (pos >= 目標 - 5) {
//目標位置に到達した（誤差付き）と見なして、何か処理をする
}
else {
wait (100000); // モーターが目標に到達するまでちょっと待つ
}
}
シリアルサーボモーターは目標位置へ到達すると停止しますので、現在位置を数回調べても移動していない場合は、目標へきちんと到達したと判断します。
while (1){
wait (300); // タイミング調整
現在位置 = sio1_set_pos(ID, 目標); //シリアルサーボに目標位置を送って現在位置を取得
if (前回位置 == 現在位置) { // もし、現在位置が前回位置と同じだったら
cnt = cnt + 1; // cntが一つ増える
}
if (cnt == 適当な数){ // cntが適当な数になったら、目標位置で停止したと判断して
break; // ループを抜ける
}
前回位置 = 現在位置; //現在の値を保持
}
sio_set_posは、処理の後に現在の位置を返します。その現在の位置と前回の位置を比べて変化が無かった場合は、サーボが目標の位置に到着していると判断してcntを増やします。数回命令を送って同じ位置にいた場合は、サーボが完全に目標の位置にいると判断し、ループを抜けて次の処理に移ることができます。
同一サーボに連続して命令を送るときには、ある程度間隔をあける必要があるので、ここでは約500us（wait(300)）待ちます。
ビーチフラッグロボットでは８回一致したら目標に到達したと判断しています。
■センサーで目標を探す（KRS788使用）④
前回からの続きです。
４） ロボットを物体の方向に歩かせる
前回までで、目標の位置を特定することができました。今回は、ロボットが見つけた目標に向かって歩くためのモーションの分岐方法を紹介します。
・switch文でモーションを分岐
上記は、モーション分岐のプログラムです。読み取ったアナログ値の最大値の位置の番号は、前回説明したバブルソートによってadk[0]に代入されています。このadk[0]を元にどのモーション再生するか決めます。
switch (adk[0]){
case 0:
case 1:
case 2:
rcb3_motion_play(9); //左旋回２回
rcb3_motion_play(9);
break;
case 3:
・・・（中略）
case 8
rcb3_motion_play(9); //左旋回１回
break;
case 9:
rcb3_motion_play(3); //左サイドステップ
break;
case 10:
・・・（中略）
case 14:
rcb3_motion_play(1); //前進
break;
・・・（中略）
}
画像は、それぞれにモーション割り当てを図式化したものです。0～24までの数字は、adk[ ]に代入したモーションを読み取った位置の番号で、Left turnなどは再生するモーションの名前です。
（画像 クリックで拡大）
上記のプログラムと画像を見て、adk[0]が「3」だった場合を考えてみます。「3」は左側の緑のエリアの番号です。目標に向かうためには、記述のLeft turn（左旋回）を１回再生すれば、ロボットは目標へ向くことになります。
以上で「センサーで目標を探す（KRS788使用）」の説明を終わります。この方法を応用すれば、いろいろ自律ロボットを作ることができると思います。お試しください！
■センサーで目標を探す（KRS788使用）③
前回のプログラムの続きです。
３） どの物体が一番近いのかを求める
void search(void){ // アナログ値の読み取りとバブルソート
unsigned int i;
int s, t, tmp;
rcb3_motion_play(37); // 正面から左に向くモーション再生
wait(300000); // サーボが動ききるまで少し待つ
rcb3_motion_play(38, mesure_distance); // 180度旋回中にアナログ値を読む
wait(600000); // サーボが動ききるまで少し待つ
rcb3_motion_play(39); // 右から方面に向くモーション再生
wait(300000); // サーボが動ききるまで少し待つ
}
// ここからバブルソート開始
for(s = 0; s <= 7; s++){
for (t = s+1; t <= 8; t++){
if (adj[t] > adj[s]){ //adj[t]の値がadj[s]よりも大きかったら順番を入れ替える
tmp = adj[t];
adj[t] = adj[s];
adj[s] = tmp;
tmp = adk[t]; //adj[t] > adj[s]だったらadkも同じ処理を行う。
adk[t] = adk[s]; //最大値のアナログ値の場所はadk[0]に代入される。
adk[s] = tmp;
}
}
}
この関数では首を旋回する一連のモーションの再生と、最大値を求めるためのバブルソートの処理を行います。
・コマンド再生の特殊な使い方
rcb3_motion_play(38, mesure_distance) は、「モーション番号38を再生している間に関数mesure_distanceの処理を行う」という命令です。モーション番号38は頭部を180度旋回させるモーションなので、前回紹介したmesure_distanceの処理を同時に行うと、弧を描くように正面全体のアナログ値を検出することができます。
・バブルソートについて
バブルソートとは、複数の数値の中でどの数値が最も大きいかを求めるための処理です。rcb3_motion_play(38, mesure_distance)で読み取ったアナログ値をバブルソートにかけて最大値を求め、どこに目標があるのかを割り出します。
配列に代入されたアナログ値を2つずつ比べます。もし、ひとつ前の値より今の値のほうが大きかったら順番を入れ替えます。その際、そのまま大きい値を小さい値の配列番号に入れてしまうと、小さい値が消えてしまいます。そこで、小さい値は別の変数(tmp)に一時保存して、大きい値が移動した後に、大きい値がいた配列のところにもう一度小さい値を代入します。これを繰り返すと、最も大きな値が配列の先頭に来て、最も小さな値が配列の一番後ろに来ることになります。 （画像1）
（画像1 クリックで拡大）
アナログ値(adj[ ])が移動すると、読み取った番号(adk[ ])も同時に移動するので、adk[0]には最も高いアナログ値を読み取ったときの番号が代入されます。あとは、この番号にモーションを割り当て、分岐すれば、ロボットは目標に向かって歩き出します。
④へ続く
■センサーで目標を探す（KRS788使用）②
前回の続きです。
２） 頭部が旋回している間にセンサーの値を読み取る
・プログラムの解説
void mesure_distance ( ) // アナログ値を一定間隔で読み取る関数
{
unsigned int i;
wait(47370);
for(i = 0; i < 8; i++){ // 頭を一度振る間に9回アナログ値を読み取り、
adj[i] = ad7_read(); // adj[i]に代入していく
ledgrn_switch();
wait (47370); // アナログ値を読む間隔は47370
// 426330(約0.7秒) / 9 = 47370
}
adj[8] = ad7_read( ); // 最後の1回
for (i = 0; i <= 8; i++) { //読みとった場所の番号を0からadk[]に代入
adk[i] = i;
}
}
この関数はアナログ値を一定の間隔で読み取る処理を行います（画像）。この処理を頭部が旋回している間に行うことによって、ロボットの正面180°のどこに物体があるのかを知ることができます。
読み取るタイミングはwaitで調整します。正面で読み取ることを基準にして、左右がほぼ同じ角度を読み取るように調節してください。読み取ったアナログ値は配列（例ではadj[ ]）に代入してください。
同時に、アナログ値を読み取った場所を、別の配列（例ではadk[ ]）に代入します。この配列は次回の③で説明するバブルソートで使います。
（画像 クリックして拡大）
・自律ロボット実装例で頭部サーボがシリアルサーボになっている理由
アナログ値を読み取るタイミングを作るために、PWMサーボKRS788では時計を使って旋回時間を計り、waitを調整して読み取る角度を作らなくてはいけませんでした。この方法だと、正確な角度でアナログ値を読み取るために調整を繰り返す手間があります。
ところが、シリアルサーボは、正確に回転角を指定することができるため、角度や読み込むタイミングを調節する手間が大幅に減ります。プログラムで細かい角度を指定し、それを連続して命令するだけで物体を簡単に探すことができるようになるのです。
③に続く
■センサーで目標を探す（KRS788使用）①
ビーチフラッグや、自律バトル競技など、自律ロボット競技に参加するためには、目標を探す処理が必要になります。今回はKRS788を頭部に使用して物体を探すプログラムを解説します。
・目標を探すには
目標を探す方法のひとつに、頭部を旋回している間に一定のタイミングでアナログ値を読み取り、最大値を求めるという方法があります。ＰＳＤセンサーや超音波センサーは、物体が近いほど高い値を示します。ビーチフラッグや自律バトル競技の場合、フィールド上で最も近い物体が目標になる確率が高いので、読み取った値の最大値を目標と判断し、その方向へ近づくプログラムを組みます。
（画像１ クリックで拡大）
画像1はロボットが目標を探しているところを、上から見た図です。0～24の番号はアナログ値を取るタイミングと順番です。
・プログラム作成の流れ
１） 頭部を180°旋回させるモーションをつくり、その動作時間を計る
２） 頭部が旋回している間にセンサーの値を読み取る
３） どの物体が一番近いのかを求める
４） ロボットを物体の方向に歩かせる
・用意するもの
１） 頭部を180°旋回させるモーションをつくり、その動作時間を計る
・モーション作成
まず、首を180°旋回するモーションをHeart to Heartで作成します。下記の三つのモーションを作ってください。
- モーション①. 頭部を正面（ホームポジション）から左に向かせる
- モーション②. 左に向いた頭部を180°旋回して右に向かせる
- モーション③. 右に向いた頭部を正面（ホームポジション）に向かせる
モーションを３つに分けた理由は、アナログ値を読むモーション②を別にして、読み込むタイミングを正確に取るためです。
・モーション②の移動時間を測る
左に向いている頭部が右へ動きだしたところから、右に向いて停止するところまでの時間を計ります。計った時間を分割してアナログ値を読む取るタイミングを取ります。
②へ続く
■2種類のモーション設定の違い
KCB-1とRCB-3をつないでのモーション再生方法は下記の2種類あります。
①コマンド再生 （rcb3_motion_play）
②コントロールコード再生 (rcb3_put_2Bcode / rcb3_put_7Bcode)
これらの再生方法には、Heart to HeartのオプションによるRCB-3の設定と、KCB-1とRCB3の接続に違いがあります。表1に違いをまとめたので、これを元に解説をしていきます。
RCB3のオプション設定とボードの接続方法 （）内は解説番号です。
| |
|
|
① rcb3_motion_play（コマンド再生） |
② rcb3_put_XBcode（コントロールコード再生） |
| Heart to Heart |
チェック項目 |
モーション終了時に返事をもらう |
○指定する（①－１）
|
×指定しない（②－１） |
| 送信機でのモーション・シナリオ再生を有効にする |
×指定しない（①－２） |
○指定する（②－２） |
| KRC-1から制御する |
△どちらでもよい（①－３） |
△2Byteのときのみ指定（②－３） |
| モーション選択項目 |
電源投入後、モーション・シナリオを再生する |
ＯＦＦを選択（start upはKCB-1から再生）（①－４） |
RCB-3から再生（②－４） |
| 通信ケーブル・ポート |
ケーブルの違い |
クロスケーブル |
接続ケーブル |
| RCB-3側接続ポート |
高速シリアルポート |
低速シリアルポート |
①コマンド再生
コマンド再生は高速シリアル通信でロボットを動かす関数です。再生方法は簡単ですが、モーション内分岐ができないため連続した動作ができません。
・Heart to Heartのオプション設定
- モーション終了の返事をもらってから次の処理に移るので、
「モーション再生終了時に返事をもらう」には必ずチェックを入れてください。
- 「送信機でのモーション・シナリオ再生を有効にする」にはチェックを入れないでください。RCB-3が受信機とつながっている場合に誤動作をする場合があります。
- 「KRC１から制御する」は低速シリアル通信の設定なので、チェックの必要はありません。
- 「電源投入時にモーション・シナリオを再生する」はＯＦＦ設定にして、スタートアップモーションはKCB-1から再生するようにしてください。
・通信ケーブルの種類と接続
ストレートケーブルで接続をすると、KCB-1の送信とRCB-3の送信、受信と受信がぶつかり、通信ができないため、接続にはクロスケーブルを使います。
・電源について
電源はKCB-1のシリアルポートとRCB3の空いているサーボ端子を接続してください。その際、通信線（白線）を必ず抜いてください。
②コントロールコード再生
コントロールコード再生は送信機と同じコマンドを低速シリアルで通信して、ロボットを動かす関数です。モーション内分岐ができるため連続歩行などが可能ですが、モーション終了後に返事が返ってこないため、プログラムに工夫が必要です。
・Heart to Heartのオプション設定
- コントロールコード再生のときはモーション終了の返事が来ないため、「モーション終了時に返事をもらう」にチェックの必要はありません。
- KCB-1は送信機と同じコマンドをRCB-3の低速シリアルに送信してモーションを再生するので、「送信機でのモーション・シナリオ再生を有効にする」にチェックを入れてください。
- 2Byteモードでモーションを再生するときは「KRC1から制御する」にチェックを入れてください。7Byteモードで再生するときはチェックをはずします。
- 「電源投入時にモーション・シナリオを再生する」を設定し、KCB-1起動後にstart upモーション再生終了を待ちます。KCB-1から再生してもかまいませんが、RCB-3のが起動するまでしばらく待つ必要があります。
・通信ケーブルの種類と接続
通常の接続ケーブルをKCB-1のCOMポートとRCB3の低速シリアル接続端子に接続してください。
・電源について
電源はKCB-1のシリアルポートとRCB3の空いているサーボ端子を接続してください。その際、通信線（白線）を必ず抜いてください。
■超音波センサー（受信部）
KHR４thアニバーサリーにて行われる自律ビーチフラッグ大会に、心強い味方「USRX-1 超音波センサー（受信部）」が発売されます！
フラッグから発信する中心周波数40ｋHｚの超音波を受信し、フラッグの位置を知るためのセンサーです。
このセンサーは、KCB-1のアナログポートに差し込み、関数を書き込めばすぐにアナログ値を検出できます。また、約５ｍまでならフラッグの方向を検出できますので、サッカーコートのどこにいてもフラッグを見つけることが可能です。
また、USTX-1 超音波センサー（送信部）の発売も予定されていますので、ご自宅での実験も可能です。
販売はロボスポット限定です。また、KHRの頭部に取り付けの際は専用のフレームもありますので是非ご利用ください！
■自律ロボット作例
今回は自律ロボットの作例を紹介します。
使用センサー：
-
SHARP社製GP2D12 PSD距離計測センサー（頭部 ｘ 1、 足部先端 x 2）
（現在はGP2Y0A21YK0Fが販売されております）
両足のセンサーで壁を検知し、頭部のセンサーで目標を探します。肩部の加速度センサーは起き上がりの分岐に使用しています。また、腰関節をピボットターンユニットに交換することによって、KHR-2HVの細かい旋回を可能にしました。
自律ロボットの詳しい情報はこちらをご覧ください。
<自律ロボット制作例マニュアル>
ビーチフラッグ用のサンプルプログラムも公開しています。下からダウンロードしてください。
＜サンプルプログラム ダウンロード＞
今大会はセンサーの制限はありませんので、競技レギュレーションに準拠していればどんなセンサーも実装可能です（*1。
フラッグ（目標）には超音波送信機を実装しています。この信号を頼りにフラッグの位置を認識するのもいいし、フラッグ表面の赤を認識して探すのもいいかもしれません。
*1 機体から超音波を送信することは禁止ですので注意してください。
