ロボットのアーム位置制御や反射神経をFPGAに実装 ―― ハードウェアとソフトウェアを使い分けて処理の高速化を実現

4．アーム軌道の補間

　図8に全体のブロック図を示します．

図8　多階層制御回路のブロック図　（クリックすると拡大します）

　最後に，ロボットの運動制御に使っているMacroData，MicroDataを用いた動作軌道生成によるパターン動作を行っているソース・コードを紹介します．MacroDataとは，到達点までの大まかな目標位置を示すデータ（ソフトウェアが生成）です．MicroDataとは，目標位置までの細かな軌道を補完するデータ（ハードウェアが配列で保持）です．これらのデータを組み合わせることで，上位層では大まかなデータを，下位層では細かなデータを扱うという階層構造を構築しました．到達点までの軌道を補完することによって，動作の始めと終わりはゆっくり，中間は素早くといったように，なめらかな動作を実現できます（図9）．

図9　階層的なデータ構造を用いて軌道を補完
目標までの軌道を補完することによって，動作の始めと終わりにはゆっくり，中間は素早くといったような，なめらかな動作を実現できる．

● 軌道補間のコーディング

　リスト3はMacroDataとMicroDataを用いた軌道補完Verilog HDL記述です．サーボモータの制御用信号を生成するために，ソフトウェア・レベルからhwModuleレベルにMacroData，MicroAddress，Startが入力されます．現在のサーボモータの回転角度を示すパルス幅データを保持するためのレジスタとして，PWMDATAを用意します．

リスト3　MacroData，MicroDataを用いた軌道補完のVerilog HDL記述　（クリックすると拡大します）

　PWMDATAは8ビットのレジスタであり，8個のモータを制御するため8×8ビットのレジスタとなり，それぞれ1個のサーボモータのPWMパルス幅を保持します．このデータを信号生成部に出力し，カウンタを用いて計測することで，出力するPWMパルスの幅を増減させられます．

　パターン動作で使用するMicroData（軌道データ）は，FPGA内部のメモリ・ブロックに保存されており，動作開始命令とともに入力するMicroAddressによって読み出し開始位置を指定してデータを読み出します．

　1回のMacroDataによる動作（Macro動作）では，目標位置であるMacroDataに到達するまでに動作軌道を補完するMicroDataを8回呼び出します．MicroDataを1回読み出すたびにPWMDATAに加減算して出力することで，モータの角度を指定された軌道にそって移動できます．

参考・引用＊文献
（1 ）工藤健慈，今中晴記，志賀裕介，関根優年；hw/sw混載システムにおけるhwObjectモデルとその制御手法，情報科学技術フォーラム 一般講演論文集 第1分冊 FT2002 C-3，pp.193-194，2002年9月．
（2 ）前川裕明，田中柳一，関根優年；多階層制御回路を用いた高速制御システム，電子情報通信学会 技術研究報告，CPSY研究会，Vol.107，No414,7-12，2008年1月．

まえかわ・ひろあき
東京農工大学大学院　工学府　
電気電子工学専攻　関根研究室（記事執筆時）