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

2． 腕の位置制御

● サーボモータの制御方法

　図4にロボット・アームを構成するサーボモータの接続方向やセンサの位置を示します．今回使用したサーボモータは近藤科学の「KRS2350ICS」です．このモータは0.7ms～2.3ms のPWMパルス信号（図5）を入力することで，0°～180°の範囲へ回転します．今回は20msに1回，PWMパルスを入力することで動作させます．入力するPWMパルスの最大幅は2.3msですので，1本の信号線に最大8個分の制御信号を時間多重化して出力できます．これにより40本という制限のあるhwModuleのGPIFを節約できます．

図4　ロボット・アームにおけるモータとセンサの位置

図5　サーボモータの制御信号
20msに1回，PWMパルスを入力することで動作させる．入力するPWMパルスの最大幅は2.3msで，1本の信号線に最大8個分の制御信号を時間多重化して出力できる．

● ロボット・アームの動作フロー

　動作の流れを図6に示します．起動後はスタート信号が入力されるまで待機し，スタート信号が入力された後は非同期リセットが入力されるまでPWMパルス信号を出力し続けます．スタート信号が入力されると，次のクロックでレジスタの初期化を行います．そして入力されたPWM DATAに最小幅の値を加算した値をrPWMDATACountに入力し，rCounterがrPWMDATACountと同じ値になるまでインクリメントしていき，信号を生成します．

図6　ロボット・アームの制御パルス生成のフロー

　1個のパルスが出力し終わった後に次のパルス用のrPWMDATACountを生成し，同じようにパルスを出力します．8個のPWMパルスを時間多重化して出力するので，8回の動作が完了したら1周期分の出力終了を知らせるoREFRESHを出力し，再びレジスタの初期化を行い次の出力を開始します．

● 腕の位置制御のコーディング

　サーボモータを制御するためのPWMパルス信号を生成するVerilog HDL記述をリスト1に示します．このコードはクロックに同期したステート・マシンによって動作します．rStateに保存されている値によって次のクロックで実行する処理をcase文によって記述しています．

リスト1　サーボモータを制御するためのPWMパルス信号の生成　（クリックすると拡大します）

　今回はスタート信号（iSTART）が入力されるまでは，制御信号を出力しないようにしてあります．これは電源を入れたとき，値がリセットされる前に突然動き出すのを防ぐためです．

　信号の変動範囲である1.6msを8ビットのデータ（rPWM DATA）として信号データを生成します．今回使用したモータは，PWMパルスの最小のパルス幅が0.7msなので，rPWMDATAの値に0.7ms分のカウント回数112を加算することで，生成するPWMパルス幅のデータ（rPWM DATA Count）とします．このデータを160kHzのクロックで動作するカウンタを用いて計測することで，0.7ms～2.3msのパルス信号を生成します．

　そして，1個目のPWMパルスの出力が終了した後に2個目を出力していき，最終的に8個のパルスを出力した後に，最後のパルスを知らせる信号（oRFRESH）を出力して1回分の出力を終了する処理を繰り返し行います．

3． 反射動作

　今回使用したhwModuleおよびhwObjectを用いたシステムでは，hwObjectの関数を呼び出すたびに10ms程度の遅延が発生するほか，PCIバスへのアクセスやhwModule上のLMにデータを書き込み，読み出す際の遅延が発生してしまいます．このため人間の脊髄反射のような高速な動作を，上位層であるソフトウェア・レベルを介して行った場合，上記の遅延に加えてソフトウェア上で動作しているほかのアプリケーションによって遅延が発生します．

　そこで，システムの下位にある末しょうFPGAレベルだけによってフィードバック制御を行うことで，これらの問題を解決することにしました．FPGAを用いて制御を行うため，モータやセンサの個数を増加させた場合でも並列に処理ができます．そのため処理時間の増加がほとんどないという利点があります．

● 動作のフローチャート

・　反射動作を生成するための反射動作用パラメータを用意
　反射動作を行うために，反射応答に使用するデータ（反射パラメータ）をあらかじめFPGA内部のメモリ・ブロックに保存し，反射真理値表を作成します．センサ入力から反射真理値表に該当する反射パラメータを読み出して利用することで制御信号を生成します．

・　反射動作の生成方法
　FPGAには125μsごとに加速度センサの値が入力されます．そのたびにセンサの値をアドレスとして反射真理値表から反射パラメータを読み出します．読み出した反射パラメータと反射判定パラメータ（8ビットのレジスタ）を比較し，反射動作を行うか否かの判定を行います．センサ・データの値によって呼び出された反射パラメータが判定用レジスタの値を超えた場合に危険と判断し，反射的に回避します（図7）．

図7　反射動作の制御フロー
FPGAには，125μsごとに加速度センサの値が入力される．そのたびに，センサの値をアドレスとして反射心理値表から反射パラメータを読み出す．

　制御信号の生成方法は，各センサに対応する反射真理値表から反射パラメータを取り出し，現在の位置を示すパルス幅を測定したデータに加減算を行い，パルス幅データを生成します．そのデータをパルス生成部に渡すことで，反射動作が実行されます．

● 反射動作のコーディング

　リスト2は，反射動作を行うモジュールのVerilog HDL記述です．このコードには反射真理値表を書き換える部分なども記述されていますが，ここでは実際に反射動作を行っている部分を紹介します．

リスト2 　反射動作のVerilog HDL記述 （クリックすると拡大します）

　ステートps2にてセンサの値を読み取ったことを知らせる信号が入力された後に，反射動作を行う状態（iMODE ==11）であった場合に反射動作を行うかの判定を行います（psH1）．

　psH1では反射真理値表のアドレスを設定し，読み出し状態にします．psH2では読み出した反射パラメータ（rShinritiData）と反射判定パラメータ（rHansya Hantei）を比較し，反射パラメータの値が大きいときに反射動作を開始します．

　そしてセンサ出力値の増減とパルス幅が増減した際にモータが回転する方向から，現在の位置を示すパルス幅の値（rPWMCountData）に加算するか減算するかを決定します．その結果を用いてrPWMCountDataとrShinriti Dataを加減算し，反射動作を行うパルス幅の値（rHansya Data）を算出します．この値を信号生成部に出力することで反射動作を行います．

　これによりセンサの値が入力されてから約1.6μsで，反射制御信号の出力を開始します．