Kobuki は Yujin Robotics から発売されている研究用移動ロボットです。 掃除機ロボット Roomba とほぼ同様の大きさで、USBシリアル接続でPC等から制御ができるようになっています。 また、IO、シリアル入出力、電源コネクタ、ボタン、LED等が装備されており、実験用ロボットとしての利用に適しています。
以下の Kobuki のサンプルを動作させるために必要なソフトウエアのインストールなどを行うスクリプトがこちらからダウンロードできます。
$ wget http://svn.openrtm.org/Embedded/trunk/RaspberryPi/tools/rpi.sh $ chmod 755 rpi.sh $ sudo ./rpi.sh hostname --type kobuki
で、以下の解説で行っている環境構築と Kobuki のサンプルのコンパイルが自動で行われます。
下図は Kobuki のメインパネルです。
Raspberry Pi への電源供給に 5V1A DC出力コネクタ、Raspberry Pi との接続には USBコネクタを利用します。
Kobuki には 5V 1A 出力可能な DC出力コネクタがあり、Raspberry Pi の電源をここから供給することができます。
5V1A 出力コネクタは以下の型番のものを使用します。
Kobuki 5V1A用コネクタ | |
ハウジング | Molex PN : 43645-0200 |
ターミナル | Molex PN : 43030-0001 |
RTロボットショップなどでも購入できます。
下図のような DCコネクタと USBの変換ケーブルを作成することで、Raspberry Pi へ電源を供給します。
近年ではスマートフォン用の USB出力端子がついたバッテリーが多数発売されていますので、こういった電源も利用できます。
Kobuki に付属している USB ケーブルで Kobuki と Raspberry Pi を接続します。 Raspberry Pi側からは /dev/ttyUSB0 として見えます。
$ ls /dev/ttyUSB* /dev/ttyUSB0
Raspberry Pi を Kobuki に搭載して、電源と USBを接続します。Raspberry Pi を無線LAN接続にすれば、無線遠隔操作可能な Kobuki になります。
Kobuki 動作時に脱落する可能性がありますので、Raspberry Pi はマジックテープなどで固定するとよいでしょう。
前節でも RTコンポーネントのコンパイルのテストを行いましたが、ここで再度おさらいします。まずは、KobukiAIST RTコンポーネントを以下のリポジトリからチェックアウトしビルドします。
$ svn co http://svn.openrtm.org/components/trunk/mobile_robots/kobuki $ cd kobuki $ mkdir build $ cd build $ cmake -DCMAKE_INSTALL_PREFIX=/usr .. $ make $ cd src $ sudo make install
試しに起動してみます。デバイスファイル /dev/ttyUSB0 へのアクセスには root 権限が必要ですので、sudo を使って起動しています。
$ rtm-naming $ sudo /usr/lib/openrtm-1.1/rtc/KobukiAISTComp
RTSystemEditor を起動し、Raspberry Pi のホスト名またはIPアドレスに接続すると、KobukiAIST0 というコンポーネントが見えるはずです。 クリックして Configuration ダイアログを表示させてみてください。
LED1 や LED2 などの操作が行えるようになっていますので、radio ボタンで RED や GREEN などをクリックしてください。LED が点灯します。
KobukiAIST コンポーネントをRaspberry Pi 起動時に自動的に起動するようにします。 これにより、Kobuki に電源を投入すると、Raspberry Pi および KobukiAIST コンポーネントが自動的に起動するようになり、Raspberry Pi にいちいちログインしなくとも Kobuki を RTC 経由で操作できるようになります。
以下のようなスクリプトを /etc/kobuki.sh として作成します。
$ sudo vi /etc/kobuki.sh
kobuki.sh の内容な以下の通りです。
#!/bin/sh # # KobukiAIST RTC launch script # # Copyright Noriaki Ando <n-ando@openrtm.org> # 2011.03.27 # # This script should be executed from rc script like a rc.local # as the following command line. # # ns=/usr/bin/rtm-naming kobukiRTC=/usr/lib/openrtm-1.1/rtc/KobukiAISTComp workdir=/tmp/kobuki \$ns sleep 5 if test -d $workdir ; then echo "" else mkdir \$workdir fi cd $workdir while : do rm -f \$workdir/*.log \$kobukiRTC sleep 5 done
実行権限をつけます。
$ sudo chmod 755 /etc/kobuki.sh
さらに、自動的に起動するように /etc/rc.local の最後の exit 0 の手前に以下のような一行を挿入します。
/etc/kobuki.sh 2>&1 | perl -p -e 's/\n/\r\n/g' 1>&2 & exit 0
これで、Raspberry Pi が起動すると、KobukiAIST コンポーネントも自動的に起動します。 また、万一 KobukiAIST コンポーネントを exit で終了させても、5秒後再度起動します。 Kobuki に電源が入っている限り、KobukiAIST コンポーネントは常駐し続けるようになっています。
TkJoystick は OpenRTM-aist-Python にサンプルとして含まれているコンポーネントです。ただし、出力はジョイスティックのX-Y値と、対向2輪型移動ロボットの車輪速度用の出力のみで、2次元速度ベクトル (TimedVelocity2D) 出力は有りません。
TkJoyStick コンポーネントを改良して、2次元速度ベクトル (TimedVelocity2D) の出力をするようにし、Kobukiと接続して操作してみてください。
Windows では以下のディレクトリーもインストールされています。(x.yはバージョン)
TkJoystick.py の中では、左右の車輪速度を計算しています。ここから、移動ロボットの運動学を考慮すれば、速度 v および角速度 ω を計算することができます。 (参考のため、東北学院大学の熊谷先生のページへのリンクを張っておきます。)
なお、TimedVelocity2D は以下のようなデータ構造となっています。
struct Velocity2D { double va; // 角速度 [rad/s] double vx; // 並進速度(前方) [m/s] double vy; // 並進速度(横方向) [m/s] 対向2輪型では0 }; struct TimedVelocity2D { Time tm; Velocity2D data; };
Kobuki をセンサーを利用して自律的に移動させてみます。 Kobuki は、バンパセンサー、近接センサー(遠・近)、Cliffセンサーを装備されています。 ここでは、Roomba っぽく、前進し続けて壁を検知したら、少し下がり、回転し、再び前進をする、というアルゴリズムで動かしてみます。 (Roomba はもう少し賢く動きますが。。。)
KobukiAIST RTC のセンサ出力は以下のようになっています。 IRセンサはドックからの赤外線による信号を受けるためのもので、障害物検知には使用できません。したがって、バンパおよび崖センサーのみが障害物・崖検知に利用できます。
No. | enum | 意味 |
0 | RIGHT_BUMPER | 右バンパ |
1 | CENTER_BUMPER | 中央バンパ |
2 | LEFT_BUMPER | 左バンパ |
3 | RIGHT_WHEEL_DROP | 右車輪脱輪 |
4 | LEFT_WHEEL_DROP | 左車輪脱輪 |
5 | RIGHT_CLIFF | 右崖センサ |
6 | CENTER_CLIFF | 中央崖センサ |
7 | LEFT_CLIFF | 左崖センサ |
8 | RIGHT_IRFAR_RIGHT | 右IR/ドック右遠 |
9 | RIGHT_IRFAR_CENTER | 右IR/ドック中央遠 |
10 | RIGHT_IRFAR_LEFT | 右IR/ドック左遠 |
11 | RIGHT_IRNEAR_RIGHT | 右IR/ドック右近 |
12 | RIGHT_IRNEAR_CENTER | 右IR/ドック中央近 |
13 | RIGHT_IRNEAR_LEFT | 右IR/ドック左近 |
14 | CENTER_IRFAR_RIGHT | 中央IR/ドック右遠 |
15 | CENTER_IRFAR_CENTER | 中央IR/ドック中央遠 |
16 | CENTER_IRFAR_LEFT | 中央IR/ドック左遠 |
17 | CENTER_IRNEAR_RIGHT | 中央IR/ドック右近 |
18 | CENTER_IRNEAR_CENTER | 中央IR/ドック中央近 |
19 | CENTER_IRNEAR_LEFT | 中央IR/ドック左近 |
20 | LEFT_IRFAR_RIGHT | 左IR/ドック右遠 |
21 | LEFT_IRFAR_CENTER | 左IR/ドック中央遠 |
22 | LEFT_IRFAR_LEFT | 左IR/ドック左遠 |
23 | LEFT_IRNEAR_RIGHT | 左IR/ドック右近 |
24 | LEFT_IRNEAR_CENTER | 左IR/ドック中央近 |
25 | LEFT_IRNEAR_LEFT | 左IR/ドック左近 |
26 | KOBUKI_DOCKED | ドック完了 |
これらの出力を受けるため RTC::TimedBooleanSeq 型の InPort が一つ必要になります。 また、Kobuki に移動速度指令を出力するための TimedVelocity2D型の OutPort が一つ必要になります。
基本プロファイル | |
コンポーネント名称 | KobukiAutoMove |
モジュール概要 | Kobuki auto move component |
バージョン | 1.0.0 |
ベンダ名 | AIST |
アクティビティ | |
onInitialize、onFinalize、onActivated、onDeactivated、onExecute | |
データポート | |
[in] bumper | |
概要 | センサ情報 true:障害物検出(バンパ接触、車輪落下、崖検知) false:障害物無し |
データ型 | TimedBooleanSeq |
詳細 | data[0]: 右バンパ、data[1]: 中央バンパ、... data[7]: 左崖センサ(上の表参照) |
[out] targetVelocity | |
概要 | 移動ロボットの速度ベクトル |
データ型 | TimedVelocity2D |
詳細 | vx: 並進速度、vy: 0.0、va: 角速度 |
単位 | vx [m/s]、va [rad/s] |
以上の情報を手掛かりに、Kobuki を自律移動させる簡単なコンポーネントを作成してみてください。 コンポーネントの接続でうまく行かない場合は、トラブルシューティング をご覧ください。
Kobuki の速度指令は TimedVelocity2D という型で、上でも示しましたが、以下のようなデータ構造です。
struct Velocity2D { double va; // 角速度 [rad/s] double vx; // 並進速度(前方) [m/s] double vy; // 並進速度(横方向) [m/s] 対向2輪型では0 }; struct TimedVelocity2D { Time tm; Velocity2D data; };
対向2輪型移動ロボットでは、vyは常に0.0であると考え、たとえば直進なら
va = 0.0; vx = 0.2; vy = 0.0;
となります。後進するなら、
va = 0.0; vx = -0.2; vy = 0.0;
であり、その場で旋回するなら
va = 0.0; vx = 0.0; vy = 1.0;
となります。
InPort にデータが来ていたら、データを読み込みバンパ情報を取り出します。バンパ情報は、TimedBoolSeq というデータ型の .data という配列のメンバに格納されており、上の表で0,1,2番目の要素であることがわかります。 これらのどれかが true になっていたらバンパで衝突を検知したということですから、いったん下がり、旋回します。そして、再び前進します。 これらの動きを TimedVelocity2D のメンバーにそれぞれ設定して OutPort に writeすれば、速度指令データは Kobuki に伝達されます。 アルゴリズムのフローチャートを下に示します。
どのくらい下がるか・下がったか、あるいは旋回するか・したかを計測しながら制御するのが理想ですが、簡単のために sleep関数を使用することもできます。 Linuxではcoil::sleep が使えます
coil::sleep(coil::TimeValue(0.01); // 10ms待つ
Windows では coil::sleep の精度が悪いので、Sleep関数を利用したほうがよいでしょう。 これらのヒントを基に、Kobuki を自律移動させるような制御コンポーネントを作成してみてください
解答として、上記の TkJoyStick コンポーネントと自律移動コンポーネントを以下に示します。