PRIMER-V2

バランス制御は古典的なＰＩＤ制御を基本とする。ただ早く安定点に収束させるには各項（比例項、微分項、積分項）のパラメタまでも変数として関数化する必要がある。またカーブの切り方にもいろいろな方式があり、更にロボット制御そのものも複雑なのでそれらを全て実験orシミュレーションすることは殆ど不可能に近い。（今は感に頼りいい加減に設定している）これを将来的には本ＨＰ「創発モデル」の項で紹介する技術（学習・自己組織化）を利用し、合理的かつ柔軟な動作を自己組織的に実現したい（今は単なる夢）。その「前段階の実験機」という位置付け。

写真1．市販の二足歩行ロボット（近藤科学　KHR-3HV）を改造

表1.ロボットの仕様（改造後）

改造した箇所

【足首】

写真２．ロール軸を削除した足首部分

・足首が長すぎ、ペダルを漕ぐ際、不自然に見えましたので足首ロール軸部分を削除しました。
・ソールは、甲が少し長めの仕様に変更。これにより、発進時、爪先立ちからペダルに足を乗せるまでの時間を短縮。また甲が長い分、機構上ペダルの回転半径を大きく取れ、ペダル踏み込み時のトルクをアップします。

【膝関節部】

写真３．外側にずらした膝関節部分

・膝関節を外側に移動。股を広げずに足裏を横方向に対し平行にできるため、ペダルを回す際の駆動モーションをスムースでシンプルなものにすることができます。

【制御ボード】

写真４．バックパックに搭載された制御ボード

・制御基板はSH7125をコアとする自作基板に交換。これでC言語による、独自プログラムの組み込みが可能になり、処理能力もアップしました。

・自転車は、市販品がありませんでしたので、ホームセンタや100円ショップなどで集めた材料を利用。ほぼ素材レベルから自作したものです。フレーム部は角パイプと削り出しの継ぎ手によって組み上げました。

写真５．ロボットのサイズに合わせて自作した小型自転車

表2．自転車の仕様

【ギア部】

写真６．チェーンの代わりにラジコンカーのプリーとベルトを利用

・小型自転車を製作するにあたって最初に悩んだのが適合するギアとチェーンの入手です。色々調べましたがそのまま使えるものが無かったので、ラジコンカー用のプリーとドライブベルトで代用することにしました。

【ペダル】

写真７．ペダルと足裏部分

・ペダルは写真の様に丸棒の状態になっています。・ロボットの足には、半円形の溝とテーパが付いていてペダルと嵌合します。

【ハンドル】

写真８．ハンドル部分

・ハンドルは6mmの鉄棒を曲げただけのもので、手を固定するための特別な機構はありません。ロボットの腕を外側にスライドさせれば、ハンドルから手が外れます。

【車輪】

写真９．タイヤは100円ショップにあったゴム管を利用

・ホイール部分は5mmのアクリル板を使用。そこにゴム管（100円ショップで入手したエクササイズ用のもの）を接着、外れないよう中心にワイヤを通し、締め付けてあります。

制御基板は、KHR-3HVのキットに添付（RCB-4HV）されていますが、プログラムをC言語で記述したい等の事情で、自作の制御基板に乗せ換えることにしました。自作基板の概要は以下の通りです。

図１．ロボットの制御回路ブロック図

①マイコン：AKI-7125マイコンボードを使用、 SH7125（32bit、RISCマイコン、50MHz）搭載、ROM:128KB、RAM:8KB
②絶対角生成回路：レートジャイロの出力（角速度）をOPアンプ(MC6484)で積分し、高精度な絶対角を生成。ただし、パラメータを動的に変更したい、などの都合で、自転車ロボットでは使用せず、角速度の積分はソフト的に行うことにしました。
③加速度センサ：KXM52-1050モジュール使用（3軸）
④RS232Cインタフェエース：AMD3202AN使用、外部PCとの通信用。
⑤リセット回路：M51967B使用、スタートと電源監視
⑥角速度センサインタフェース:レートジャイロ（現在TAG-201使用）からの出力をマイコンのアナログポートに入力。
⑦無線モジュール：KRR-1モジュール使用(ADバンド）
⑧圧力センサインタフェース：ロボット足裏の圧力センサ用ですが、自転車ロボットでは使用していません。
⑨シリアルサーボインタフェース：サーボ（KRS-2552HV）信号をマルチドロップ（後述）でマイコンのシリアルポートに接続。
⑩３端子レギュレータ：TAG4805SをアナログICの+5V
電源に、XC6202をロジックICの+5V電源に使用、Ni-MHバッテリー(10.8V,800mAh)から電源供給。

ソフトウェアは、RenesasのHEW環境を利用し、Ｃ言語で作成しました。図2ににサーボ制御廻りを中心としたゼネラルフローを示します。

図2．ゼネラルフロー

[メインルーチン]
串刺し式に、先ずは絶対角算出処理（後述）、続いて無線モジュールで受信したコマンドの解析処理、最後にロボットの状態毎に分岐したフェーズ別処理を行います。これら一連の処理は、タイマ割込みのタイミングに合わせ、10msの周期で実行されます。

[タイマ割込み]
10ms周期で内部割込みを発生させ、この割込みをメイン処理に通知するためのフラグを立てます。続いてサーボ指示角が記録された１８個のレジスタ（各サーボに対応）を順次読み出し、サーボコマンド形式に変換した後、送信用リングバッファに書き込みます。最後に送信割込みを有効にして、コマンドの送信を待ちます。

[送信割込み０]
各サーボに115.2Kbpsの速度でリングバッファ内のコマンドデータを送信します。送信割込み０は右側サーボ用、送信割込み2（表記省略）を左側サーボ用に振分けることで全サーボへの送信完了時間を短縮し、送信ポートのドライブ付加を軽減しています。

[受信割込み０]
右側サーボからのレスポンスデータ（コマンド送信後）を受信します。左側サーボのレスポンスは受信割込み2(表記省略)で受信します。

[受信割込み１]
無線モジュールからのデータ（2.4Kbps）を受信し、データは受信用リングバッファに格納されます。内容の解析はメインルーチンで行ないます。

[TE割込み０]
送信キャラクタの最終ビットが送信され、その後、送信キャラクタがセットされない場合、本割込みが発生します。この割込みタイミングで送信ポートを入力に設定することでサーボからのレスポンス信号との衝突を回避します。（後述）

図3．バランス制御の流れ

図３は、自転車が傾いた際に、ロボットがハンドル操作によって車体を立て直すまでの大まかな制御の流れを示したものです。先ず、車体が傾いた際①、ロボット側は、ジャイロセンサの出力を積分して求めた絶対角※によって検知②します。その傾き角から車体を垂直に戻すための適切なハンドル角をPID制御③の手法で求めます。次はその角度にハンドルを切った際の手の位置④を計算し、続いて実際にその位置に手を移動させるための各関節のサーボ駆動量を逆運動学計算⑤※によって求めます。以上の過程を経てロボットの手が動き⑥、傾きに応じた適切な角度ハンドルを切る⑦ことで車体の立て直し⑧を行ないます。。（※部は次項で詳述）

走行中、車体がどの程度傾いているか、その角度（垂線に対する左右方向の傾き角）を精度良く算出することは自転車ロボットを安定に走行させるための重要な条件です。

リスト１．絶対角の算出

絶対角の算出処理はメイン処理の中にあり10ｍｓ周期で実行します。処理内容は、ジャイロセンサの出力をＡＤ変換した値（角速度）が格納されるデータレジスタ(ADD.ADDR0)を右に6bit分シフト（ＳＨマイコンの場合、下位6bitは無効）し、その値から静止時のジャイロ基準レベル(base_RLは、予めイニシャルルーチンで計測)を引くことで、オフセット０の角速度に変換します①。次にノイズ等による誤差（本ロボットの場合、１程度）以上の角速度の変化があったときのみ①の角速度（J0）を加算してゆく（積分する）ことで絶対角(RL_ANGLE_I)を求めます②。更に、温度ドリフトなどで基準レベルが変化した場合を考慮し、基準レベル補正レジスタ(jyrBaseAdj_RL)③④を介してbase_RLが基準レベルの変化に緩やかに追従できるようにしています⑤。最後にRL_ANGLE_I の符号を反転し、更に1/4することで最終的な絶対角(RL_ANGL)に変換します⑥。尚、RL_ANGLは30・degの倍率に設定されており、これはサーボ(KRS-2552HV)から読み出せるポジションデータと同じ単位となります。

写真12．ハンドルを切る場面（動画「二足歩行ロボットを自転車に乗せてみた」より）

自転車の場合、自動車などとは違って、行きたい方向にハンドルを切るだけでは目的の方向に曲がることができません。曲がるためには、例えば右に曲がるには先ず、右側に重心を移動させる必要があります。具体的には、反対方向（左）に一旦ハンドルを切り車体を右に傾けた後（右側に重心移動）、そのままでは右に倒れてしまいますので、今度は逆に右にハンドルを切り、傾いた状態でバランスを取ります。以上の操作で方向を変えることはできますが、このままの体勢では円を描いて回ったままになってしまいますので、目的の方向に車体の向きが変わったら、今度はやや内側にハンドルを切って、車体を垂直に戻すことで直進状態に復帰させます。

図６．足の現在位置からΘ進めた位置を駆動目標にする

ペダルは単純に固定モーションで回すだけですと、路面抵抗や慣性力などの影響でペダルの回転速度が変化した際、駆動目標と、実際の足位置との位相関係が狂い、ペダルが回らなくなったり、最悪サーボを壊してしまう可能性があります。こういったことが起こらないよう、サーボのポジション取得機能を利用して、現在の足位置を割出し（順運動学計算により）、そこからΘ進んだ位置を駆動目標にしています。このように、実際のペダル位置から駆動目標をΘ進めることでスムーズにペダルを漕ぐことができ、また、この進み角Θの調整によって、自転車の走行速度を変えることもできます。

図７．運動エネルギーを機体の横振動に変換

自転車にはプレーキが付いていないので、止まる時は地面に足を着けて停止します。一見ただ足を伸ばしているだけのように見えますが、実は図１のように脚を少し前方に突き出し、且つ股関節サーボの駆動トルクを小さく（電流制限）しています。こうすることで、停止の際、地面と接触した側が持ち上がり、反対側に傾き、また、反対側でも同じことが起きて、結果、車体が横に振動します。これにより、進行方向の運動エネルギーをロボットの横振動に変換し、股関節の負担軽減と、トルク負けによる転倒を防止ししています。

マイコンのシリアルポートから直接、サーボを制御する際に、考慮しなければならないサーボ信号（レスポンス信号）とマイコンシリアルポート出力との衝突回避に関して、現在のところ細かな回避手法が公開されていないようですので、その一例として本ロボットで用いた技術を紹介したいと思います。

図８．シリアルサーボ制御部分

シリアルサーボは、受信信号と送信信号が共通な半二重通信方式になっていて、複数のサーボをマルチドロップにより１本の信号線で制御します。図８は、公開されたKONDO KCB-1の回路図を参考に設計した本ロボットのシリアルサーボ制御部分です。サーボ制御用のシリアルポートは、KCB-1と同様、SIポート0と２を使用していて、RXD(受信ポート)とTXD(送信ポート)は抵抗を介して１線に纏め、直接、サーボに接続しています。ここで注意しなければならないのは、TXD0,2はオープンドレインではないということです。従って、ワイヤードオア的に使用する場合、マイコン側がコマンド送信後、サーボからのレスポンス受信待ちとなったとき、出力の衝突を避け、マイコン側送信ポートをハイインピーダンス状態にする必要があります（出力ハイレベル許容電流IOH=2mA）。具体的にはリスト４のようにコマンドの最終ビットが送信されたタイミングでＴＥ割込みを発生させ、送信ポートを入力（ハイインピーダンス状態）に設定し、サーボのレスポンス信号との衝突を回避します。

リスト４．割込みルーチン

[送信割込み]
リングバッファに送信データがある場合①は、TDR(トランスミットデータレジスタ)に送信データをセット②し、 PA1ポートをTXD0ポート（出力）に設定③します。また、送信すべきデータが無くなり、コマンド送信終了となった場合④は、送信割込みを禁止⑤し、 TE（Trancemit End）割込みを有効⑥にします。

[TE割込み]
先ずTE割込みを禁止⑦し、TXD0を入出力ポートに変更⑧、最後に入力ポートに設定⑨することでサーボから出力されるレスポンス信号との衝突を回避します。

シリアルサーボをマイコンから直接制御する際、出力の衝突回避を考慮していないケースは多いのではないかと思われます。そうした場合、マイコンポートのハイレベル許容電流(IOH=2mA)に抵触しマイコンを劣化させる可能性がありますので、以上の例を参考に確認して頂ければと思います。

自転車に乗る二足歩行ロボット