FPGA×ROSでFPGAをプラットフォームとしたロボットを実現するプロジェクトOpenReroc

#はじめに

##自律ロボットの発展

最近はロボットに対する注目がかなり高まっているように思います。
特に、産業用ロボットのようなものではなく、ロボット自身が周囲の状況を把握し動作するような「自律ロボット」の発展が目覚しいように思えます。（完全に主観的笑）
流行りのロボットといえば、おそらく皆さんご存知のお掃除ロボットのルンバがあると思います。また、教育用の自律走行ロボットとしてはこのルンバを応用した、Turtlebotというものがあります(下の写真)。
実はこのロボットはバッテリ駆動ですが、PC1台を搭載していて電力もそれなりに消費するため、連続駆動時間はそれほどありません。PCを搭載するのにもいろいろ理由がありますが、一番はロボットの性能の問題です。ロボットが自律的に行動するためにはさまざまな知的情報処理（おもに画像処理）が必要で、たいていその処理のどれもが非常に膨大な処理量です。つまりPCほど高性能なCPUを搭載しないと、処理しきれないというわけですね。

画像引用：http://www.nihonbinary.co.jp/Products/Robot/TurtleBot.html

##低い電力のなかでも高い処理性能のためにFPGAを･･･！

電力性能比が一般的に高いといわれているFPGAであれば、バッテリ駆動で限られた電力制約のなかでも性能を発揮しうるプラットフォームです。
ただ、このFPGAは開発がとても大変。ロボットを開発するということは、ロボットを動かすための物理的な原理だとか、電気的な知識のほかに、自律行動させるためのアルゴリズムや制御構造を考慮しなければなりません。とてもではありませんが、FPGAを導入する余裕がないというのが現状であると思います。せっかくFPGAを使えばいいことがあるのに使わないのはもったいない。

##OpenRerocとは

OpenReroc (Open source Reconfigurable Robot Component)はFPGAの回路をロボットのためのコンポーネントとして開発するための研究プロジェクトです（ちなみに筆者も開発メンバー）。もう少し具体的にすると、ロボット向けのソフトウェアプラットフォームであるROSで使用できるFPGAの回路を含めたパッケージをリリースします。***ROSを用いてFPGAの回路をコンポーネント化をすることで、ロボットシステムへのFPGA導入を容易にするというモチベーションです。***図に示しているのはSoC(FPGA+CPU)上に構築したシステム例です。破線で囲まれた四角1つ1つがFPGAの回路を含むコンポーネントで、CPUサイドではROSとUbuntuが動作します。FPGAの回路を含むコンポーネントは他のROSノードと同等に通信ができますのでユーザはFPGAを意識することなくFPGAを導入できます。

今回はこのプロジェクトのROSパッケージであるopenreroc_pwmを紹介します。

#openreroc_pwmの導入

##openreroc_pwmとは
openreroc_pwmはモータのPWM制御を行うためのコンポーネントです。
openreroc_pwmは以下のサイトから入手可能です。

• ROSのwiki：http://www.ros.org/browse/details.php?distro=indigo&name=openreroc_pwm
• Github：https://github.com/Kumikomi/openreroc_pwm

##openreroc_pwmのシステム構成

openreroc_pwmでは**”openreroc_pwm”というトピックにモータへのパラメータを入力することによってモータの回転方向と回転速度を設定することができます。
このコンポーネントはROSにおいてsample_input**、openreroc_pwmの2つのノードを起動することで動作検証が可能です。
また、コンポーネントのより詳細な要素は以下のとおりです。

• mortor_ctl.v : CPUとFPGAのデータ通信を行なうためのインターフェイス
• pwm_ctl.v : PWMのアルゴリズムの実装
• openreroc_pwm.cpp : 他のノードと通信するためのROSノードを記述したcpp

##準備するもの

openreroc_pwmを導入する際の要求環境です。なお、ここから説明することは、下記に示した要求環境が整っていることを前提とします。

###ハードウェア

• Xilinx Zynq-7000 series -- Zedboard

• PmodHB3(モータドライバ) × 2
  

• PWM制御に対応したモータ × 2

###ソフトウェア

• xillinux-1.3c
  • Xillybus社からリリースされているFPGA-ARM間の通信が可能な、Zedboard用のディストリビューション
  • XillinuxのZedboardへの導入はこちらをご参照ください。XillinuxをつかってFPGA－ARMプロセッサのデータ通信をしよう
• ROS　(groovy以上のバージョン)
  • xillinux上でインストールしてください。
  • 詳細なインストール方法はそのうち記事にしたいと思います。それまでの参考記事 → Ubuntu ARM install of ROS Groovy
  • ROSにおける基本的な概念の勉強、開発方法はこちらをご覧ください。ROSの基本的な開発をしよう
• ssh

###その他開発環境

• 母艦PC（FPGA回路の合成のため）
• Zedboardへのログインのためのターミナルソフト(任意)
• FPGAの回路の合成ツール
  • 今回はISE

##openreroc_pwmのクローンとディレクトリ構造
Zedboard上のXillinuxにおいて
以下のコマンドでopenreroc_pwmのリポジトリをROSのワークスペースにクローンしてください。

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
git clone https://github.com/Kumikomi/openreroc_pwm.git 

クローンした中身は以下のようになっているかと思います。

openreroc_pwm 
|-include/ 
|-msg/ 
|-hardware
    |-src/
    |-image/
|-src/ 
|-CMakeLists.txt 
|-package.xml  
|-LICENSE.txt

また、各ディレクトリの中身は以下のとおりです。

• include：ヘッダファイル
• msg：openreorc_pwmで使用するメッセージ型の定義ファイル
• hardware
  • src：モータ制御のためのverilogファイル
  • image：bit stream
• src：ROSノードのcpp

##ハードウェアのビルド（母艦PC上でのビルド）
パッケージの中にはbitファイルも含まれていますが、今回はハードウェアのビルド方法を紹介します。
Xillinuxの導入では回路データ(xillinux-eval-zedboard-1.3c)をダウンロードし、合成したかと思います。
このとき用いたプロジェクトを開いていください。なお、プロジェクトはxillinux-eval-zedboard-1.3c/verilog内のxillydemo.xiseをWクリックです。

###ソースの追加とインスタンスの追加
プロジェクトを開いたら上のメニューバーからproject → Add sourceでプロジェクトに
openreroc_pwm/hardware/srcのmortor_ctl.vとpwm_ctl.vを追加してください。
ソースを追加した後は、トップモジュールへインスタンスを作ります。
xillydemo.vを以下ように編集してください。

//xillydemo.v
//267行目あたり
   // 32-bit loopback
//   fifo_32x512 fifo_32
//     (
//      .clk(bus_clk),
//      .srst(!user_w_write_32_open && !user_r_read_32_open),
//      .din(user_w_write_32_data),
//      .wr_en(user_w_write_32_wren),
//      .rd_en(user_r_read_32_rden),
//      .dout(user_r_read_32_data),
//      .full(user_w_write_32_full),
//      .empty(user_r_read_32_empty)
//      );

motor_ctl motor_ctl
(
.clk(bus_clk),
.rst_32(!user_w_write_32_open && !user_r_read_32_open),
.din_32(user_w_write_32_data),
.wr_en_32(user_w_write_32_wren),
.rd_en_32(user_r_read_32_rden),
.dout_32(user_r_read_32_data),
.full_32(user_w_write_32_full),
.empty_32(user_r_read_32_empty),

 .dir_out_r(dir_out_r),
 .dir_out_l(dir_out_l),
 .en_out_r(en_out_r),
 .en_out_l(en_out_l)
);

インスタンスを作成したあとにセーブするとプロジェクトの階層構造は以下のようになります。

###トップモジュールへのポートの追加

xillydemo.vのIOポート宣言に以下のポートを追加してください。

output dir_out_r,
output dir_out_l,
output en_out_r,
output en_out_l,
input [3:0] sub_port

###配線の設定

Zedboardとモータの接続にはPmodのインターフェイスを使いますが
現段階ではPmodの口となるポートが使えません。
したがって、ポートを使用できるようにします。今回Pmodで使用するのは写真に示したJBのポートです。
Zedboardの配線を設定するためucfファイル編集します。xillydemo.ucfを以下のように編集してください。

# "xillydemo.ucf"
# "86行あたり"
# Pmod JB
NET  dir_out_r  LOC = W12 | IOSTANDARD = LVCMOS33;
NET  en_out_r   LOC = W11 | IOSTANDARD = LVCMOS33;
NET  dir_out_l LOC = V12 | IOSTANDARD = LVCMOS33;
NET  en_out_l  LOC = W10 | IOSTANDARD = LVCMOS33;
NET sub_port[0] LOC = V10 | IOSTANDARD = LVCMOS33;
NET sub_port[1] LOC = W8 | IOSTANDARD = LVCMOS33;
NET sub_port[2] LOC = V9 | IOSTANDARD = LVCMOS33;
NET sub_port[3] LOC = V8 | IOSTANDARD = LVCMOS33;

NET PS_GPIO[32] LOC = M20  | IOSTANDARD=LVCMOS33;  # "FMC-LA00_CC_N"
NET PS_GPIO[33] LOC = M19  | IOSTANDARD=LVCMOS33;  # "FMC-LA00_CC_P"
NET PS_GPIO[34] LOC = N20  | IOSTANDARD=LVCMOS33;  # "FMC-LA01_CC_N"
NET PS_GPIO[35] LOC = N19  | IOSTANDARD=LVCMOS33;  # "FMC-LA01_CC_P"
NET PS_GPIO[36] LOC = P18  | IOSTANDARD=LVCMOS33;  # "FMC-LA02_N"
NET PS_GPIO[37] LOC = P17  | IOSTANDARD=LVCMOS33;  # "FMC-LA02_P"
NET PS_GPIO[38] LOC = P22  | IOSTANDARD=LVCMOS33;  # "FMC-LA03_N"
NET PS_GPIO[39] LOC = N22  | IOSTANDARD=LVCMOS33;  # "FMC-LA03_P"

###回路の合成と回路の導入

Generate Programming Fileをダブルクリックして論理合成を開始します。
論理合成が終わったら、生成されたbitファイル(xillydemo.bit)をブートSDにコピーします。
なお、パッケージ内のbitファイルを使用する際はopenreorc_pwm.bitをもとあるbitファイルと差し替えてください。

##ソフトウェアのビルド(ZedboardのXillinux上におけるビルド)

以下のコマンドでopenreroc_pwmをビルドします。

cd ~/catkin_ws
source devel/setup.bash
catkin_make 

##Zedboardとモータの接続

注意：Zedboardとモータの接続はZedboardの電源が落ちた状態で行ってください。

###PmodHB3について
PmodHB3はPmodのモータドライバです。各ピンは以下の役割があります。左のピンはFPGAのPmodの口にそのまま接続できます。
また、右のポートはモータの＋、－と直流電源のVcc、GNDへそれぞれつなぎます。
なお、モータの制御はDIR、ENピンにたいしてPWM制御をすることで行います。

今回使うピンは、SA、SB以外のピンを使います。FPGA側のPmodのポートアサインは以下のようになっています。

モータドライバとFPGAを接続した様子

##テスト実行

テスト実行は以下のコマンドで行ってください。

ターミナル1

cd ~/catkin_ws/
source devel/setup.bash
roscore &
rosrun openreroc_pwm openreroc_pwm

ターミナル 2

cd ~/catkin_ws/
source devel/setup.bash
rosrun openreroc_pwm sample_input

sample_inputではコマンドラインにおいて以下の入力形式でモータの速度や回転方向を決めることができます。

type parameter [dir_left] [para_left] [dir_right] [para_right]
1 6000 1 6000
0 10000 1 10000
.
.
.

入力形式は左から
左モータの回転方向 左モータの回転速度　右モータの回転方向のパラメータ 右モータの回転速度のパラメータ
です。現時点での回転方向に対するパラメータは 0 or 1、回転速度に対するパラメータの値域は 0 ~ 19999であり、
**値を大きくすればするほど回転速度が下がります。**入力してモータの挙動が変わったら正常に動作しています。
(なお、回転速度に対するパラメータを小さい値にするとシステム全体の電圧が低い場合、
Zedboardの電源が落ちてしまう場合がありますので6000~程度から検証することをお勧めします。)
(PS：そのうち直します。)

実際のロボットシステムへ活用する場合はたとえばsample_inputが、制御計画が記述されたノードと交換されるというケースを想定しています。

#おわりに

いかがだったでしょうか。今回はFPGA×ROSでFPGAをプラットフォームとしたロボットを実現するプロジェクトOpenRerocの紹介と
そのパッケージの1つであるopenreroc_pwmを紹介しました。
FPGA×ROS、FPGA×ロボットの一大旋風を起こすためにも私自身、がんばっていこうと思います。
ご質問、ご意見ございましたら、コメントしていただけると幸いです。

###連絡先と筆者のwebサイト
Email：kazushi_at_virgo.is.utsunomiya-u.ac.jp
HP : Kazushi Yamashina

###関連リンクまとめ

• OpenReroc (Open source Reconfigurable Robot Component)
• openreroc_pwm
• Xilinx Zynq-7000 series -- Zedboard
• PmodHB3(モータドライバ)
• xillinux-1.3c
• Xillybus
• ROS
• ROS wiki
• Ubuntu ARM install of ROS Groovy
• ROSの基本的な開発をしよう
• XillinuxをつかってFPGA－ARMプロセッサのデータ通信をしよう
• ルンバ
• Turtlebot