火星探査ローバー用の走行制御を自作ロボに搭載（その２）

パート1 では、アクチュエータの機械的出力（速度、位置など）が制御プログラムによって要求される値と一致することを保証するために広く使用されている、PID制御の背後にある理論について説明した。また、複雑な理論を基本的なマイクロコントローラで実行できる比較的シンプルな算術で表す方法についても紹介している。今回は、サーボモータで駆動する2輪バギーロボットの実際のコードを見ていく。ここでは、Microchip製の16ビットdsPIC33デジタル信号コントローラ（DSP拡張機能を備えたPIC24マイクロコントローラ）を使用しているが、実際の問題のほとんどは他のマイクロコントローラを使用したプロジェクトについても適用することが可能だ。

ホイール回転データのキャプチャ

ホイールの回転速度を感知するデバイスは、2個の部品（放射状に32個のスロットが付いたホイール本体と、スロットの位置に合わせてシャーシに固定された赤外線反射モジュール）で構成される。 （図.1）。この動作原理はシンプルだ。

センサモジュールはホイールのスロット（隙間）に向かって並んで配置された赤外線(IR) LEDとフォトダイオードで構成される。ホイールが回転すると、ビームが隙間を通り抜ける。そのため、フォトダイオードはビームを認識しないが、スポークの部分はビームがフォトダイオードに反射される。その結果パルス列が生成され、このパルスの繰り返し率はホイールの回転速度に比例する。この回転速度を計測する手段が必要となるのだが、幸い、最新のマイクロコントローラはこの用途で使用する「入力キャプチャ」ユニットと呼ばれるハードウェアを搭載している（図.2）。

原理上、センサで直接動作するカウンタを使用して速度を計測する。一定の間隔でカウンタをサンプリングし、その都度値をクリアすると速度が求められる。問題は、低速時にはサンプリング間隔が長すぎて重要なカウントを捕捉できないため、速度が分からなくなるということだ。この問題に対しては、高速クロックでカウンタをインクリメントしてサンプリングしたり、センサのパルスを使用して値を「キャプチャ」したりすることで対処する。これによって、通常の方法の代わりにパルス周期を計測することができる。dsPICインプットキャプチャ（IC）ユニットは、このことに必要なハードウェアをすべて備えており、動作させるのに必要なのは数行のアセンブラコードだけだ。しかし、残念なことに複雑な部分もある。使用するチップごとに2つの独立したICユニットを使用できるが、タイマの1つは別の機能で使う必要がある。これに対しての解決策は、残りのタイマをICユニット内の他のハードウェアと共有することだ。この場合、2つのICユニットを両方ともタイマ3を使うように設定する。このタイマは0000からFFFFまで継続してカウントし、オーバーフローすると0000に戻ることを繰り返すものだ。ただし、妥協点もあるが、2つのユニットがそれぞれ異なるタイミングでサンプリングを行うので、サンプリング期間が始まるたびにカウンタをリスタートすることができない（図.3）。

このランプ波形は、タイマーカウントが0000からFFFFまで上昇し、それを繰り返すことを表している。下に示されているのは、回転センサからのパルス波形で、各立ち上がりでタイマーカウントを「キャプチャ」するものだ。入力キャプチャハードウェアは、この「スナップショット」をFIFOバッファに保存すると同時に、プロセッサへの割り込みを生成する。

速度の計算

割り込みサービスコードは、直前の割り込みのカウント値を取得し、その値を最新の値から引き、センサのパルス周期を表す数値を取得する。この図では、時刻T1、T2時点でスナップショットカウントC1、C2が取得されている。C2 – C1は間隔T2 – T1に対応している。これは簡単だが、ホイールが先に回転を始めた場合の動作について、2つ問題がある。

• 最初のセンサ割り込みが発生したときに有効な「前のカウント」値がないため、最初に計算された速度は正しくない値となる。
• タイマが0000～FFFFをカウントする期間をセンサのパルス周期より長くできないため、最低速度制限がある。また、最高速度制限もあるが、こちらが問題になることはおそらくないだろう。

どちらの問題も、ロボットが動き始めてから計測される最初の数回の速度は無効な値になる可能性が高いことを意味する。これについては、モータを起動した後、センサ値が静止状態から一定の速度の範囲に入る予定時刻まで「開ループ」制御を行い、PID制御コードで最初の値を無視すること以外にできることはほとんどない。図.3を見直すとさらに問題があることがわかるが、これは対応可能である。T3時点で計算された速度について考えよう。T2～T3の間でカウンタがオーバーフローしているため、C3 – C2では誤った値が算出される。幸いなことに、この状況はソフトウェアで検出でき、C3がC2より小さい場合は、補正係数が適用される。通常は、最新の値がC1とC2のように前の値より大きくなる。

実際のコード例

dsPICプロセッサの基本的な割り込みサービスルーチンで、スピードと3つの「生」のP、I、D値をスケーリングせずに計算する。以下のリスト1では、K係数が指定されている。

これは、独自の組み込み制御プログラミング言語FORTHdsPICに組み込まれている2つの回転センサチャンネルの1つの割り込みルーチンだ。dsPICアセンブラ「ベアメタル」コードに存在するにもかかわらず、どのようにして元の複雑なPID方程式を数回の加算と減算で表せるかが簡単に分かる。このコードは、センサパルスが生じるたびに実行され、生のPID値を計算し、メインプログラムで処理する変数に保存するものだ。このメインプログラムについては、このシリーズのパート3で詳しく取り上げるが、変数を受け取ってモータ制御値の計算も行っている。また、この割り込みルーチンには、重要なナビゲーションタスクを実行する1行のコードが含まれており、そのコードは走行距離を計測するものである。

走行距離の計測

ホイール回転センサが提供するものは速度情報（回転速度計測）だけではない。パルスをカウントすることで、移動した距離も計測できる（走行距離計測）。それが、リスト1の「inc ODOMTR」の行で行われている処理だ。自動車やオートバイを運転したことがある人なら、走行距離計を知っているだろう。そう、移動距離を数値で示すダッシュボード上の装置だ。ロボットでは、これはモビリティ制御で重要な役割を果たすことができる。この例の場合、センサの割り込み（各スロットのホイール回転に対応し、合計32個で1周）が発生するたびにODOMTR変数がインクリメントされる。1スロット期間における移動距離を計算するには、ホイール外周πDを32で割って求められる。Dはホイールの直径だ。このParallaxエンコーダのホイールは直径が66mmのため、算出される走行距離は6.4mmとなる。また、このロボットでは速度が異なるステアリングを使用しているため、直進するには各ホイールの速度データが、正確に回転するには移動距離データが必要だ。これらのことについては、パート3で解説する。

直交エンコーダ

上述のシンプルな回転センサには、かなりはっきりした制限が1つある。それはホイールの回転方向が分からないとういものだ。そのため、別のフォトセンサを追加し、回転方向の情報を検出できるようにする必要がある。追加するセンサは、1つ目のセンサと出力パルス波形が正確に90°ずれるように配置する（図4）。

これらの信号をデコードするのに必要なものは、シンプルなD型フリップフロップだけだ。モータ制御用に設計された最新のマイクロコントローラは通常、1組のQEチャンネル入力と必要なオンチップハードウェアを装備している。惑星探査ローバーは後退できる必要があるのだが、私のロボットは「リバースギア」を必要としないためシンプルなタコメータで十分だ。

次回: パート3

PIDアルゴリズムのモータ駆動と「推測航法」によるロボット操作について解説する。また、最後には起伏の多い地形でモータの負荷が変わるとどうなるかという現実的な問題についても説明する。火星探査車キュリオシティのモビリティ制御システムの設計フローがどのようにホイールを損傷させたか、ご確認を。ロケット科学者でもすべてを考慮することはできなかったのだ！

もし何かに行き詰ってしまった場合は、是非Twitterで私の投稿をフォローしてほしい。最新のエレクトロニクスと関連技術、ロボットについて注目した投稿のリツイート、宇宙探査などの興味深い記事等をリンクしている。