移動ロボットKobukiの制御

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 のサンプルのコンパイルが自動で行われます。

Raspberry Pi と Kobuki の接続

下図は Kobuki のメインパネルです。

kobuki_panel.png
Kobuki DC 出力コネクタ

Raspberry Pi への電源供給に 5V1A DC出力コネクタ、Raspberry Pi との接続には USBコネクタを利用します。

電源

Kobuki には 5V 1A 出力可能な DC出力コネクタがあり、Raspberry Pi の電源をここから供給することができます。

5V1A 出力コネクタは以下の型番のものを使用します。

Kobuki 5V1A用コネクタ
ハウジング Molex PN : 43645-0200
ターミナル Molex PN : 43030-0001

kobuki5v_connector.png
Kobuki DC5V1A用コネクタ

RTロボットショップなどでも購入できます。

下図のような DCコネクタと USBの変換ケーブルを作成することで、Raspberry Pi へ電源を供給します。

kobuki_raspberry_dccable.png
Raspberry Pi用 DCケーブル

近年ではスマートフォン用の USB出力端子がついたバッテリーが多数発売されていますので、こういった電源も利用できます。

battery.png
スマートフォン用バッテリー

USB

Kobuki に付属している USB ケーブルで Kobuki と Raspberry Pi を接続します。 Raspberry Pi側からは /dev/ttyUSB0 として見えます。

 $ ls /dev/ttyUSB*
 /dev/ttyUSB0

接続

Raspberry Pi を Kobuki に搭載して、電源と USBを接続します。Raspberry Pi を無線LAN接続にすれば、無線遠隔操作可能な Kobuki になります。

kobuki_and_raspi.png
Raspberry Pi を搭載したKobuki

Kobuki 動作時に脱落する可能性がありますので、Raspberry Pi はマジックテープなどで固定するとよいでしょう。

KobukiAIST RTコンポーネントのコンパイル

前節でも 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

以上で、KobukiAIST RTCがビルドされ、実行ファイルが
  • /usr/lib/openrtm-1.1/rtc/KobukiAISTComp にインストールされるはずです。

試しに起動してみます。デバイスファイル /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 コンポーネントの自動起動

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 コンポーネントは常駐し続けるようになっています。

Kobuki コンポーネントの操作

TkJoystick による操作

TkJoystick は OpenRTM-aist-Python にサンプルとして含まれているコンポーネントです。ただし、出力はジョイスティックのX-Y値と、対向2輪型移動ロボットの車輪速度用の出力のみで、2次元速度ベクトル (TimedVelocity2D) 出力は有りません。

TkJoyStick コンポーネントを改良して、2次元速度ベクトル (TimedVelocity2D) の出力をするようにし、Kobukiと接続して操作してみてください。

Windows では以下のディレクトリーもインストールされています。(x.yはバージョン)

  • C:\Program Files (x86)\OpenRTM-aist\x.y\examples\Python\TkJoyStick
  • C:\Program Files\OpenRTM-aist\x.y\examples\Python\TkJoyStick

ヒント

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;

となります。

timed_velocity_2d.png
移動ロボットの座標系と TimedVelocity2D

InPort にデータが来ていたら、データを読み込みバンパ情報を取り出します。バンパ情報は、TimedBoolSeq というデータ型の .data という配列のメンバに格納されており、上の表で0,1,2番目の要素であることがわかります。 これらのどれかが true になっていたらバンパで衝突を検知したということですから、いったん下がり、旋回します。そして、再び前進します。 これらの動きを TimedVelocity2D のメンバーにそれぞれ設定して OutPort に writeすれば、速度指令データは Kobuki に伝達されます。 アルゴリズムのフローチャートを下に示します。

kobuki_auto.png
フローチャート

どのくらい下がるか・下がったか、あるいは旋回するか・したかを計測しながら制御するのが理想ですが、簡単のために sleep関数を使用することもできます。 Linuxではcoil::sleep が使えます

 coil::sleep(coil::TimeValue(0.01); // 10ms待つ

Windows では coil::sleep の精度が悪いので、Sleep関数を利用したほうがよいでしょう。 これらのヒントを基に、Kobuki を自律移動させるような制御コンポーネントを作成してみてください

解答

解答として、上記の TkJoyStick コンポーネントと自律移動コンポーネントを以下に示します。

ダウンロード

最新バージョン : 2.0.1-RELESE

統計

Webサイト統計
ユーザ数:2160
プロジェクト統計
RTコンポーネント307
RTミドルウエア35
ツール22
文書・仕様書2

Choreonoid

モーションエディタ/シミュレータ

OpenHRP3

動力学シミュレータ

OpenRTP

統合開発プラットフォーム

産総研RTC集

産総研が提供するRTC集

TORK

東京オープンソースロボティクス協会

DAQ-Middleware

ネットワーク分散環境でデータ収集用ソフトウェアを容易に構築するためのソフトウェア・フレームワーク