移動ロボットkobukiの制御

✏️ Edit this page on GitHub

#contents

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 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
Kobuki DC5V1A用コネクタ

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

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

Raspberry Pi用 DCケーブル

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

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

USB

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

 $ ls /dev/ttyUSB*
 /dev/ttyUSB0

接続

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

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;

となります。

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

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 コンポーネントと自律移動コンポーネントを以下に示します。