JP2010058603A

JP2010058603A - 倒立車輪型移動体、及びその制御方法

Info

Publication number: JP2010058603A
Application number: JP2008225008A
Authority: JP
Inventors: Yoshiyuki Senba; 快之仙波; Hideki Kajima; 日出輝梶間; Nobuyasu Tomokuni; 伸保友國; Motoki Kotake; 元基小竹; Minoru Kamata; 実鎌田
Original assignee: University of Tokyo NUC; Toyota Motor Corp
Current assignee: University of Tokyo NUC; Toyota Motor Corp
Priority date: 2008-09-02
Filing date: 2008-09-02
Publication date: 2010-03-18
Anticipated expiration: 2028-09-02
Also published as: JP5261081B2

Abstract

【課題】制御性及び走行性に優れた制御コントローラを有する倒立車輪型移動体、及びその制御方法を提供すること。
【解決手段】本発明に係る倒立車輪型移動体１００の制御部８０は、取得した傾斜角度及び傾斜角速度と目標傾斜角度及び目標傾斜角速度との姿勢偏差、第１の測定部により取得した駆動量と目標駆動量との車輪偏差、第２の測定部により取得した駆動量と目標駆動量とのスライダ偏差、及び第３の測定部により取得した駆動量と目標駆動量とのアーム偏差を算出し、当該算出した４つの偏差に所定のゲインを乗じて、車輪を駆動する車輪トルク指令、スライダ機構を駆動するスライダ力指令、及びアーム機構を駆動するアームトルク指令を算出する。
【選択図】図５

Description

本発明は倒立車輪型移動体、及びその制御方法に関する。

倒立二輪車両などの倒立車輪型移動体は、通常、左右の駆動輪を駆動して安定状態を維持するように重心位置を修正しつつ、移動を行なうように制御している。さらに、倒立状態を安定させるため、車輪上方に設けられた慣性体を駆動する構成が開示されている（特許文献１）。この倒立車輪型移動体では、走行中に、慣性体をスライド移動させている。これにより、重心位置が車軸の鉛直線上に速やかに移動するため、倒立を安定させることができる。
特開２００６−２０５８３９号公報

しかしながら、従来技術では、このような多自由度を有する倒立車輪型移動体の制御コントローラの設計は、各事由度の動作に着目して制御コントローラを設計し、設計したそれら複数の制御コントローラを結合することにより行われるものであった。このため、従来の制御コントローラでは、各自由度の動作が干渉しあい、動作計画を適切に調整しないと、良好な動作制御を実現することができないという問題があった。また、各自由度の干渉を意識しすぎることで、各制御コントローラの制御性能を十分に発揮することができなくなるという問題があった。

特許文献１記載の倒立車輪型移動体では、台車並進方向に関する車輪の制御を行う制御コントローラと、台車並進方向にスライド可能な制御アームの制御を行う制御コントローラとを有している。例えば、特許文献１記載の倒立車輪型移動体が加減速を行う場合には、加減速を行うための車輪のトルク指令値に合わせて制御アームの動作を適切に制御する必要がある。しかし、特許文献１記載の倒立車輪型移動体では、各制御コントローラは互いに独立しているため、車輪と制御アームとの制御を協調させて行わせるためのさらに他の演算器（ＣＰＵなど）が必要となる。

本発明は、多自由度を有する制御対象に対して良好な動作制御を実現可能な制御コントローラを設計し、制御性及び走行性に優れた当該制御コントローラを有する倒立車輪型移動体、及びその制御方法を提供することを目的とする。

本発明に係る倒立車輪型移動体は、車輪を回転可能に支持する車台と、前記車輪を回転駆動する第１の駆動部と、支持部材を介して前記車台に対して回動可能に支持された車体部と、前記車体部を駆動する第２の駆動部と、前記車体部の高さを変化させる第３の駆動部と、前記第１の駆動部、前記第２の駆動部、及び第３の駆動部を制御する制御部と、前記車体部の鉛直方向に対する傾斜角度及び傾斜角速度を取得する姿勢傾斜測定部と、前記第１の駆動部の駆動量を取得する第１の測定部と、前記第２の駆動部の駆動量を取得する第２の測定部と、前記第３の駆動部の駆動量を取得する第３の測定部と、を備える倒立車輪型移動体であって、前記制御部が、前記姿勢傾斜測定部により取得した前記傾斜角度及び傾斜角速度と目標傾斜角度及び目標傾斜角速度との姿勢偏差、前記第１の測定部により取得した駆動量と目標駆動量との第１の偏差、前記第２の測定部により取得した駆動量と目標駆動量との第２の偏差、及び前記第３の測定部により取得した駆動量と目標駆動量との第３の偏差を算出し、当該算出した４つの偏差に所定のゲインを乗じて、前記第１の駆動部を駆動する第１の制御量、前記第２の駆動部を駆動する第２の制御量、及び前記第３の駆動部を駆動する第３の制御量を算出するものである。

このように、第１の駆動部、第２の駆動部、及び第３の駆動部に関する制御を一つの制御コントローラにより実現することで、多自由度を有する倒立車輪型移動体に対して良好な動作制御を実現することができる。

また、前記第３の偏差に積分演算値を含むことで、第３の駆動部に関する重力補償を行うことができる。さらに、前記姿勢偏差に積分演算値を含むことで、姿勢偏差に関する定常偏差を解消することができる。また、前記制御部は、重力を含む加速度による非線形負荷を補償するフィードフォワード量を算出し、当該算出したフィードフォワード量を前記第１の制御量、前記第２の制御量、及び前記第３の制御量に加算することで、重力を含む非線形項の補償を行うことができる。

さらにまた、前記ゲインのゲインパターンを予め複数設定し、前記第３の測定部により取得した前記第３の駆動部の駆動量に応じて、前記ゲインパターンを切替えるようにしてもよい。このように、単一の制御コントローラであっても、影響度の大きな駆動量に応じてゲインのゲインパターンを切替えることで、制御性能の低下を抑制することができる。

また、前記第３の駆動部が前記車体部を支持しながら鉛直方向に伸縮するアーム機構を有しており、前記第３の測定部は、前記アーム機構の変化量を前記第３の駆動部の駆動量として取得すると好適である。

さらにまた、前記第２の駆動部が前記車体部を前後にスライドさせるスライド機構を有し、前記第３の駆動部が前記車体部を支持しながら鉛直方向に伸縮するアーム機構を有し、前記制御部は、前記算出した第１の制御量としての車輪トルク指令を前記車輪に出力し、前記算出した第２の制御量としてのスライド力指令を前記スライド機構に出力し、前記算出した第３の制御量としてのアームトルク指令を前記アーム機構に出力すると好適である。

本発明に係る倒立車輪型移動体の制御方法は、車輪を回転可能に支持する車台と、前記車輪を回転駆動する第１の駆動部と、支持部材を介して前記車台に対して回動可能に支持された車体部と、前記車体部を駆動する第２の駆動部と、前記車体部の高さを変化させる第３の駆動部と、を備える倒立車輪型移動体の制御方法であって、前記車体部の鉛直方向に対する傾斜角度及び傾斜角速度を取得し、前記第１の駆動部の駆動量、前記第２の駆動部の駆動量、及び前記第３の駆動部の駆動量を取得し、前記取得した前記傾斜角度及び傾斜角速度と目標傾斜角度及び目標傾斜角速度との姿勢偏差、前記取得した駆動量と目標駆動量との第１の偏差、前記取得した駆動量と目標駆動量との第２の偏差、及び前記取得した駆動量と目標駆動量との第３の偏差を算出し、当該算出した４つの偏差に所定のゲインを乗じて、前記第１の駆動部を駆動する第１の制御量、前記第２の駆動部を駆動する第２の制御量、及び前記第３の駆動部を駆動する第３の制御量を算出するものである。

本発明によれば、多自由度を有する制御対象に対して良好な動作制御を実現可能な倒立車輪型移動体、及びその制御方法を提供することができる。

発明の実施の形態１．
本実施の形態にかかる移動体は倒立振子制御によって移動する倒立車輪型移動体である。移動体は、地面に接地した車輪を駆動することによって、所定の位置まで移動する。さらに、ジャイロセンサ等からの出力に応じて車輪を駆動することによって、倒立状態を維持することができる。また、移動体は、倒立状態を維持したまま、操作者が操作する操作量に応じて移動する。

図１及び図２を用いて、本実施の形態にかかる移動体１００の構成について説明する。図１は移動体１００の構成を模式的に示す側面図であり、図２は移動体１００の構成を模式的に示す正面図である。

図２に示されるように、移動体１００は、倒立車輪型の移動体（走行体）であり、右駆動輪１８と、左駆動輪２０と、右スイングアーム１７と、左スイングアーム１９と、車体１２と、を備えている。車体１２は、右駆動輪１８、及び左駆動輪２０の上方に配置された移動体１００の上体部の一部である。ここで、移動体１００の進行方向（図２の紙面と垂直方向）を前後方向とし、水平面において前後方向に垂直な方向を左右方向（横方向）とする。よって、図２は、進行方向前側から移動体１００を見た図であり、図１は、左側から移動体１００を見た図である。

走行時において、右スイングアーム１７、及び左スイングアーム１９は、車高を調整する。さらに、一方、又は両方のスイングアームを駆動して、車体１２の地面に対する左右の傾斜角度を調整する。例えば、水平な地面を走行中に、右駆動輪１８のみが段差に乗り上げたり、地面が右上がりの傾斜面に変わったりしたとする。この場合、右駆動輪１８が左駆動輪２０よりも高くなる。このため、右スイングアーム１７の関節を駆動して右駆動輪１８を車体１２の方向により近づけるようにする。これにより、右駆動輪１８が高くなった分を吸収でき、横方向（左右方向）において車体１２を水平にすることができる。即ち、右スイングアーム１７及び左スイングアーム１９が、第３の駆動部に相当する。

右スイングアーム１７側端側には右マウント２６が固定され、車軸３０を介して右駆動輪１８を回転可能に支持する。右駆動輪１８は、車軸３０を介して右輪駆動モータ３４の回転軸Ｃ１に固定されている。右輪駆動モータ３４は、右マウント２６内に固定され、車輪用駆動部（アクチュエータ）として機能する。

左スイングアーム１９の側端側には左マウント２８が固定され、車軸３２を介して左駆動輪２０を回転可能に支持する。左駆動輪２０は、車軸３２を介して左輪駆動モータ３６の回転軸Ｃ２に固定されている。左輪駆動モータ３６は、左マウント２８内に固定され、車輪用駆動部（アクチュエータ）として機能する。右駆動輪１８と左駆動輪２０は、地面と接地し、略同軸上で回転する一対の車輪である。

右駆動輪１８と左駆動輪２０が、回転することによって、移動体１００が移動する。また、右輪駆動モータ３４と左輪駆動モータ３６が、車輪を駆動させる駆動輪モータとなる。即ち、右輪駆動モータ３４と左輪駆動モータ３６が、第１の駆動部に相当する。右マウント２６と左マウント２８が、左右の駆動輪を回転可能に支持する車台となる。

右輪駆動モータ３４及び左輪駆動モータ３６（以下、モータ３４、３６と称することもある。）は例えば、サーボモータである。尚、車輪用アクチュエータは、電気的なモータに限らず、空圧、油圧を使用したアクチュエータでもよい。なお、以下の説明において、右駆動輪１８と左駆動輪２０をまとめて、駆動輪と呼ぶこともある。

また、右マウント２６は、右輪エンコーダ５２を備えている。右輪エンコーダ５２は、右駆動輪１８の回転量としての回転角を検出する。左マウント２８は、左輪エンコーダ５４を備えている。左輪エンコーダ５４は、左駆動輪２０の回転量としての回転角を検出する。

右スイングアーム１７は、右上リンク２１と、右スイング軸６２と、右スイングアーム駆動モータ６０を有している。左スイングアーム１９は、左上リンク２２と、左スイング軸６６と、左スイングアーム駆動モータ６４を有している。車体１２の下部には、右上リンク２１及び左上リンク２２が固定されている。右上リンク２１には、右スイングアーム駆動モータ６０が固定され、右スイング軸６２を介して、回転軸Ｃ４回りに、右スイングアーム１７を駆動する。左スイング軸６６には、左スイングアーム駆動モータ６４が固定され、左スイング軸６６を介して、回転軸Ｃ５周りに左スイングアーム１９を駆動する。このように、右スイングアーム１７には、回転軸Ｃ４回りに回転する回転関節が設けられ、左スイングアーム１９には、回転軸Ｃ５回りに回転する回転関節が設けられている。右スイングアーム１７及び左スイングアーム１９（以下、スイングアーム１７、１９と称することもある。）に設けられた関節をスイングアーム関節とする。

車体１２には、搭乗席駆動モータ７０、ラックアンドピニオン７２、ジャイロセンサ４８、及び搭乗席７４が取り付けられている。また、車体１２には、対向して、右上リンク２１及び左上リンク２２が取り付けられている。

車体１２の中央近傍には、ラックアンドピニオン７２が設けられている。ラックアンドピニオンのラックは、前後方向に沿って設けられている。ラックアンドピニオン７２によって、搭乗席７４が支持されている。即ち、搭乗席７４は、ラックアンドピニオン７２を介して車体１２に取り付けられている。搭乗席７４は、搭乗者が座ることができる椅子の形状を有する。なお、ラックアンドピニオン７２の代わりにボールネジなどを用いてスライドさせてもよい。

車体１２の上部には、搭乗席駆動モータ７０が固定されている。搭乗席７４と搭乗席駆動モータ７０は、ラックアンドピニオン７２によって連結されている。搭乗席駆動モータ７０は、回転軸Ｃ３回りに回転する。これにより、ラックアンドピニオン７２のピニオンに回転力が加えられる。搭乗席駆動モータ７０の回転運動は、ラックアンドピニオン７２によって、直線運動に変換される。すなわち、搭乗席駆動モータ７０を駆動すると、車体１２に対する搭乗席７４の位置が前後にスライドする。即ち、搭乗席駆動モータ７０が第２の駆動部に相当する。このとき、搭乗席７４と搭乗者又は搭乗物との合成重心位置が、車体１２に対して前後に変化する。なお、車体１２に対して、搭乗席７４と搭乗者又は搭乗物との合成重心位置を変化させる手段としては、スライド機構の他に、回転軸機構、旋回機構などで実現することも可能である。また、搭乗席駆動モータ７０の動力をギアやベルトやプーリなどを介して、搭乗席７４に伝達してもよい。ここで、搭乗席駆動モータ７０によって前後に移動する構成全体を車体部７７とする。車体部７７には、搭乗席７４や操作モジュール４６等が含まれる。もちろん、車体１２を駆動するアクチュエータを備える場合は、車体部７７に車体１２も含まれる。また、搭乗席駆動モータ７０には、スライド位置を計測するためのエンコーダ（図示せず）が設けられている。

回転軸Ｃ３は回転軸Ｃ１及びＣ２と平行であり、回転軸Ｃ１及びＣ２の上方に位置する。回転軸Ｃ３と回転軸Ｃ１との間に右スイングアーム１７が設けられ、回転軸Ｃ３と回転軸Ｃ２との間に左スイングアーム１９が設けられている。右スイングアーム駆動モータ６０は、右スイングアーム１７を回転軸Ｃ４回りに回転させ、左スイングアーム駆動モータ６４は、左スイングアーム１９を回転軸Ｃ５回りに回転させる。通常の走行時には、回転軸Ｃ１〜回転軸Ｃ５は水平になっている。

さらに、移動体１００には、転倒を防止するために、２つの補助輪５１が設けられている。補助輪５１は、補助輪支持ブロック５５に対して回転可能に支持されている。そして、補助輪支持ブロック５５は車体１２に取り付けられている。ここでは、一方の補助輪５１は、駆動輪の前方側に配置され、他方の補助輪５１は駆動輪の後方側に配置されている。補助輪５１は、従動輪であり、移動体１００の移動にしたがって回転する。

一方の補助輪５１の回転軸は、回転軸Ｃ１、Ｃ２よりも前側上方にあり、他方の補助輪５１の回転軸は、回転軸Ｃ１，Ｃ２よりも後側上方にある。すなわち、補助輪５１の一方は、駆動輪の車軸よりも前方に配置され、他方は、駆動輪の車軸よりも後方に配置される。これにより、移動体１００が前後に転倒するのを防止することができる。なお、補助輪以外の転倒防止部材によって、転倒を防止してもよい。例えば、前後方向に突出したストッパなどで転倒を防止することができる。

車体１２には、バッテリーモジュール４４と、センサ５８が収納されている。センサ５８は、例えば、光学式の障害物検知センサであり、移動体１００の前方に障害物を検知すると、検知信号を出力する。また、センサ５８は、障害物センサ以外のセンサであってもよい。例えば、センサ５８として、加速度センサを用いることも可能である。もちろん、センサ５８として、２以上のセンサが用いられていてもよい。センサ５８は移動体１００の状態に応じて変化する変化量を検出する。バッテリーモジュール４４は、センサ５８、ジャイロセンサ４８、右輪駆動モータ３４、左輪駆動モータ３６、右スイングアーム駆動モータ６０、左スイングアーム駆動モータ６４、搭乗席駆動モータ７０、及び制御部８０等に対して電力を供給する。

車体１２上には、ジャイロセンサ４８が設けられている。ジャイロセンサ４８は、車体１２の傾斜角に対する角速度を検出する。ここで、車体１２の傾斜角は、移動体１００の重心位置が車軸３０、３２の鉛直上方に伸びる軸からの傾斜度合いであり、例えば移動体１００の進行方向前方に車体１２が傾斜している場合を「正」とし、移動体１００の進行方向後方に車体１２が傾斜している場合を「負」として表わす。したがって、車体１２が水平になっている状態では、傾斜角度が０°になる。そして、通常の走行時には、傾斜角度の制御目標値が０°なっている。この制御目標値に追従するように、フィードバック制御されている。また、前後方向における傾斜角度を移動体１００の姿勢の傾斜角度とする。

また、進行方向の前後方向に加えて、左右方向の傾斜角速度はロール、ピッチ、ヨーの３軸のジャイロセンサ４８を用いて測定される。このように、ジャイロセンサ４８は、車体１２の傾斜角の変化を、車体１２の傾斜角速度として測定する。もちろん、ジャイロセンサ４８は他の箇所に取り付けられていてもよい。ジャイロセンサ４８で測定された傾斜角速度は、移動体１００の姿勢の変化に応じて変化する。即ち、傾斜角速度は、車軸の位置に対する車体１２の重心位置に応じて変化する変化量である。従って、外乱などによって、姿勢の傾斜角度が急激に変化すると、傾斜角速度の値が大きくなる。

搭乗席７４の側面には、操作モジュール４６が設けられている。操作モジュール４６には、操作レバー（図示せず）及びブレーキレバー（図示せず）が設けられている。操作レバーは、搭乗者が移動体１００の走行速度や走行方向を調整するための操作部材である、搭乗者は、操作レバーの操作量を調整することによって移動体１００の移動速度を調整することができる。また、搭乗者は、操作レバーの操作方向を調整することによって移動体１００の移動方向を指定することができる。移動体１００は、操作レバーに加えられた操作に応じて、前進、停止、後退、左折、右折、左旋回、右旋回することができる。搭乗者がブレーキレバーを倒すことによって、移動体１００を制動することができる。移動体１００の進行方向は、水平面内において、車軸３０、３２と垂直な方向になる。また、操作モジュール４６には、制御モードを切換えるスイッチが設けられている。

さらに、搭乗席７４の背もたれ部分には、制御部８０が実装されている。制御部８０は、搭乗者が操作モジュール４６に対して行なった操作に追従して、右輪駆動モータ３４及び左輪駆動モータ３６を制御し、移動体１００の走行（移動）を制御する。制御部８０は、操作モジュールでの操作に応じて、右輪駆動モータ３４及び左輪駆動モータ３６を制御する。これにより、操作モジュール４６での操作に応じた加速度、速度指令値で右輪駆動モータ３４及び左輪駆動モータ３６が駆動する。

制御部８０は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、通信用のインターフェースなどを有し、移動体１００の各種動作を制御する。そして、この制御部８０は、例えばＲＯＭに格納された制御プログラムに従って各種の制御を実行する。制御部８０は、操作モジュール４６での操作に応じて、所望の加速度、及び目標速度になるように、また、移動体１００が倒立を維持するように、ロバスト制御、状態フィードバック制御、ＰＩＤ制御などの周知のフィードバック制御により、右輪駆動モータ３４及び左輪駆動モータ３６を制御する。これにより、移動体１００が、操作モジュール４６での操作に応じて加減速しながら走行する。

すなわち、操作モジュール４６は、搭乗者の操作によって与えられた操作量を取得し、この操作量を操作信号として、制御部８０に出力する。そして、制御部８０は、操作信号に基づいて、移動体１００の目標加速度や、目標速度を算出し、これに追従するように、移動体１００をフィードバック制御する。これにより、移動体１００を倒立させつつ、移動させることができる。

また、制御部８０は、右輪駆動モータ３４、左輪駆動モータ３６、右スイングアーム駆動モータ６０、左スイングアーム駆動モータ６４、及び搭乗席駆動モータ７０を制御する。ここで、搭乗席駆動モータ７０が右輪駆動モータ３４及び左輪駆動モータ３６と協調して動作するよう、制御部８０が制御を行う。すなわち、倒立を安定させるように、駆動輪を回転駆動するとともに搭乗席７４をスライド移動させる。これにより、車体１２の傾斜角度が小さくなり、倒立を安定させることができる。このようにして、搭乗席駆動モータ７０が右スイングアーム駆動モータ６０、左スイングアーム駆動モータ６４、及び搭乗席駆動モータ７０と協調して動作する。

以下、制御部８０による制御目標値の算出方法について具体的に説明する。図３は、以下の説明において使用する変数の定義を表す図である。図３Ａは、本実施の形態にかかる移動体の構成を概念的に示す図であり、長さに関する変数の定義を表す図である。図３Ｂは、本実施の形態にかかる移動体の構成を概念的に示す図であり、角度に関する変数の定義を表す図である。

図３Ａに示すように、搭乗席駆動モータ７０によりスライドする搭乗席７４に関して、搭乗席７４の重心をｍ_ｐとし、車体１２に設けた原点Ｏからの重心ｍ_ｐの位置を、ｘ軸方向のスライダ位置Ｐ_ｇｘ、ｙ軸方向のスライダ位置Ｐ_ｇｙ、ｚ軸方向のスライダ位置Ｐ_ｇｚとして示す。また、右スイングアーム１７及び左スイングアーム１９を原点Ｏからの距離ｌ_ｗに設け、その重心ｍ_Ａは、スイングアームの端部から距離ｌ_ａに位置するものとする。また、車体１２の重心をｍ_Ｂとし、原点Ｏからのその重心ｍ_Ｂの位置をｘ軸方向の距離Ｂ_ｇｘ、ｙ軸方向の距離Ｂ_ｇｙ、ｚ軸方向の距離Ｂ_ｇｚとして示す。また、右駆動輪１８及び左駆動輪２０の半径をＲ_Ｔとし、その重心ｍ_ｔを、スイングアームの重心ｍ_Ａから距離ｌ_Ａに設ける。

図３Ｂに示すように、鉛直方向に対する車体１２の傾斜角をθ_ｂｙ、水平方向に対する右スイングアーム１７及び左スイングアーム１９の回転角度をそれぞれθ_ａｒ及びθ_ａｌ、右駆動輪１８及び左駆動輪２０の回転角度をそれぞれθ_ｔｒ及びθ_ｔｌとして示す。

図３に示した移動体は、右駆動輪１８及び左駆動輪２０の２軸、右スイングアーム１７及び左スイングアーム１９の２軸、及び搭乗席駆動モータ７０のスライド機構の１軸からなる合計で５軸の自由度を有する、３次元５軸モデルである。図３に示したモデルについて、以下の数１に示す５つの釣り合いの式を得ることができる。数１は、図３のモデルに関する逆動力学を示す。尚、τ_ｔｒは右駆動輪１８のトルク、τ_ｔｌは左駆動輪２０のトルク、τ_ａｒは右スイングアーム１７のトルク、τ_ａｌは左スイングアーム１９のトルク、Ｆ_ｐは搭乗席駆動モータ７０のスライド機構に作用するスライダ力を示す。変数上のドット（・）は、時間に関する一階微分であることを示し、ダブルドット（・・）は、時間に関する二階微分であることを示す。即ち、θ_ｂｙ ^（・）は車体１２の傾斜角速度、θ_ａｒ ^（・）は右スイングアーム１７の回転角速度、θ_ａｌ ^（・）は左スイングアーム１９の回転角速度、θ_ｔｒ ^（・）は右駆動輪１８の回転角速度、θ_ｔｌ ^（・）は左駆動輪２０の回転角速度、Ｐ_ｇｘ ^（・）はスライダの移動速度を示す。θ_ｂｙ ^（・・）は車体１２の傾斜角加速度、θ_ａｒ ^（・・）は右スイングアーム１７の回転角加速度、θ_ａｌ ^（・・）は左スイングアーム１９の回転角加速度、θ_ｔｒ ^（・・）は右駆動輪１８の回転角加速度、θ_ｔｌ ^（・・）は左駆動輪２０の回転角加速度、Ｐ_ｇｘ ^（・・）はスライダの移動加速度を示す。

尚、数１におけるｆ_ｉｊ及びｇ_ｉは、以下の数２により示される関数ｆ及びｇを用いて計算される。関数ｆは、状態量に基づいて、係数ｆ_ｉｊを計算する。関数ｇは、状態量に基づいて、重力を含めた各非線形項ｇ_ｉを計算する。

数１に示す車体１２の傾斜角θ_ｂｙ、とスライダ位置Ｐ_ｇｘを線形化し、擬似逆行列Ｈ_１及び行列Ｈ_２を用いることで、数１を以下の数３に示す行列形式により表現することができる。

ここで、ｇ´を関数ｇに線形化する。以下の数４から、θ_ｂｙとＰ_ｘ０の比例項を含めた関数とする（θ_ｂｙ０とＰ_ｘ０は、それぞれ線形化原点を示す）。また、擬似逆行列Ｈ_１及び行列Ｈ_２は以下の数５により示され、行列中の各要素ｈ_ｉｊは、数１を変形することにより計算することができる。

図４に示した移動体のモデルを、以下の数６の状態方程式により表す。

ここで、状態変数ｘ、制御入力ｕ、行列Ａ、及び行列Ｂを、以下の数７に示す。尚、Ｉ_θａｌは左スイングアーム１９の回転角度θ_ａｌの時間積分値、Ｉ_θａｒは右スイングアーム１７の回転角度θ_ａｒの時間積分値、Ｉ_θｂｙは車体１２の傾斜角θ_ｂｙの時間積分値をそれぞれ示す。また、行列Ａ及び行列Ｂの要素のうち、値を表示していない要素の値は０である。

本実施の形態では、１３個の状態量（Ｉ_θａｌ、Ｉ_θａｒ、Ｉ_θｂｙ、θ_ａｌ、θ_ａｒ、θ_ｂｙ、Ｐ_ｇｘ、θ_ｔｌ ^（・）、θ_ｔｒ ^（・）、θ_ａｌ ^（・）、θ_ａｒ ^（・）、θ_ｂｙ ^（・）、Ｐ_ｇｘ ^（・））を用いて、多自由度を有する移動体の制御系であって、その拡大系を生成する。ここで、状態量に積分値を追加することで、以下のような効果を得ることができる。即ち、スイングアームの回転角度に関する時間積分値Ｉ_θａｌ及びＩ_θａｒを追加することで、スイングアームの重力補償が可能になる。また、車体１２の傾斜角に関する時間積分値Ｉ_θｂｙを追加することで、傾斜角について生じる定常偏差を解消することができる。尚、ここでは、傾斜角の時間積分値Ｉ_θｂｙを０に収束させやすくするために、スライダ位置に関する時間積分値を設けていない。

次いで、数６及び数７に示したシステムの状態方程式に基づいて、その制御系を設計する。ここでは、以下の数８に示すように、関数ｌｑｒを用いて、線形フィードバックコントローラのフィードバックゲインＫ_Ｌを設計する。フィードバックゲインＫ_Ｌは５×１３の行列であり、上述した１３個の状態量に対してゲイン行列Ｋ_Ｌを乗算することで、制御入力ｕを算出する。制御入力ｕは、右駆動輪１８のトルク、左駆動輪２０のトルク、右スイングアーム１７のトルク、左スイングアーム１９のトルク、搭乗席駆動モータ７０のスライド機構に作用するスライダ力から構成される。尚、関数ｌｑｒは、最適レギュレータによるフィードバックゲインＫ_Ｌを設計する関数である。即ち、最適レギュレータは、２次形式の評価関数を最小にする状態フィードバックゲイン行列Ｋを定める方法である。Ａ及びＢは数７に示した行列である。Ｑ及びＲは評価関数に用いられる正定行列であり、シミュレーションにより定める。また、状態フィードバックゲインＫ_Ｌの設計方法は最適レギュレータに限定されず、極配置法などの公知の制御理論により設計するものとしてもよい。

さらに、本実施の形態に係る制御系では、以下の数９に示すように、制御入力ｕに対して非線形補償項ｇ´を追加することで、重力を含む加速度による非線形負荷を補償する。即ち、フィードフォワード的にｇを与えることで、移動体の動作を滑らかにすることができる。尚、ｘ_ｒｅｆは状態変数ｘの目標値、ｘ_ｎｏｗは状態変数ｘの測定値を示す。ｅ_ｘは状態量の偏差を示す。

また、本実施の形態に係る制御系では、フィードバックゲインのゲインパターンを予め複数設定し、影響度の大きな駆動量に応じてフィードバックゲインを切替えることで、制御性能の低下を抑制することができる。これにより、単一の制御コントローラであっても、影響度の大きな駆動量に応じてゲインのゲインパターンを切替えることで、制御性能の低下を抑制することができる。

上述した３次元モデルにおいて、上下移動（スイングアームによる車体の高さの変化）による影響と、前後移動（スライド機構の移動による、搭乗席のスライド変化）による影響を、姿勢加速度のグラフから判断する。本実施の形態に係る移動体では、上下移動による影響度が特に大きなものであるため、以下では、スイングアームのアーム角度に応じて制御コントローラのフィードバックゲインを切替える構成について説明する。

図４は、駆動輪に対して一定の車輪トルクが与えられている場合に、スイングアームの回転角度の変化が車体の傾斜角加速度に与える影響を示す概念図である。図４に示すように、スイングアームのアーム角度が小さな場合には車体の重心位置が低くなるため、移動体の重心周りの慣性モーメントが小さいものとなる。このため、同一の車輪トルクによりその移動を制御しようとすると応答性は高くなる。即ち、移動体の姿勢を所定の状態へと維持するために必要な車輪トルクは、スイングアームのアーム角度が大きな場合に比べて減少する。一方で、スイングアームのアーム角度が大きな場合には車体の重心位置が高くなるため、移動体の重心周りの慣性モーメントが大きい。このため、同一の車輪トルクに対して応答性は低くなる。従って、スイングアームの回転角度の増加に伴って車体の重心位置が変化し、これにより、姿勢制御に関する車輪トルクの影響が無視できなくなる。このため、姿勢制御をスイングアームの回転角度に応じて変更する。

本実施の形態に係る移動体では、特に、左右のスイングアームの回転角度の差（ロール方向の姿勢のずれ）が、倒立制御に影響を与える。このため、制御部８０は、各スイングアームの回転角度に応じて、フィードバックゲインを切替えることにより、倒立制御の制御性能の低下を抑制する。図５は、各スイングアームの回転角度と、制御コントローラのフィードバックゲインとの関係を示すゲインマップである。各スイングアームの回転角度の可動範囲を例えば０ｄｅｇから７０ｄｅｇとした場合に、各スイングアームの回転角度を所定の単位で複数段階の領域に分割する。そして、分割した各スイングアームの回転角度の各段階に対して、上述した手法によりフィードバックゲインＫ_Ｌを算出する。図においては、１０ｄｅｇ単位でスイングアームの回転角度を分割し、８段階のスイングアームの回転角度に対するフィードバックゲインＫ_Ｌを算出している。さらに、各段階の間のフィードバックゲインＫ_Ｌを、算出した各段階に対するフィードバックゲインＫ_Ｌの値から線形補間することにより算出している。即ち、ゲインマップは、このようにして算出した複数のフィードバックゲインのゲインパターンから構成される。制御部８０は、検出した各スイングアームの回転角度の組合せに応じてフィードバックゲインを切替え、切替えたフィードバックゲインを用いて制御量を算出する。

尚、ゲインマップを参照して、フィードバックゲインを切替える実施例としては、スイングアームの回転角度に応じて切替える場合に限定されない。図６は、スイングアームの回転角度以外の駆動量に応じてフィードバックゲインを切替える実施例を示す図である。図６Ａは及び図６Ｂは進行方向前方から移動体を示す概念図であり、図６Ｃは進行方向側方から移動体を示す概念図である。例えば、図６Ａに示すように、駆動輪の幅の変化に応じて、フィードバックゲインを切替えるようにしてもよい。また、図６Ｂに示すように、例えば搭乗席を上下に変化させる上下移動機構の変化に応じて、フィードバックゲインを切替えるようにしてもよい。また、図６Ｃに示すように、前後移動機構（スライド機構）の変化に応じて、フィードバックゲインを切替えるようにしてもよい。

次に、上記の制御を行う移動体の制御系の構成について図７を用いて説明する。図７は、制御部８０を含む制御系の構成を示すブロック図である。制御部８０は、上述したようにして設計した制御コントローラとしての協調制御部９１、指令値生成部９２、及び非線形補償部９３を備えている。移動体１００には、各モータを駆動制御するアンプが設けられている。ここで、右輪駆動モータ３４、左輪駆動モータ３６、右スイングアーム駆動モータ６０、左スイングアーム駆動モータ６４、搭乗席駆動モータ７０に設けられているアンプをそれぞれ、アンプ３４ａ、アンプ３６ａ、アンプ６０ａ、アンプ６４ａ、アンプ７０ａとする。各アンプは、協調制御部９１からの制御信号に基づいて動作する。

センサ類８３は、移動体１００に設けられている各種センサを示すものであり、例えば、ジャイロセンサ４８、右輪エンコーダ５２、左輪エンコーダ５４、センサ５８等を含んでいる。即ち、センサ類８３は、姿勢傾斜測定部、第１の測定部、第２の測定部、及び第３の測定部を含む。

指令値生成部９２は、倒立制御計算を行い、各駆動部を駆動する制御目標値を算出する。そして、減算器により制御目標値と現在値との偏差が求められる。尚、現在値は、例えば、センサ類８３からの出力に基づいて算出することができる。そして、協調制御部９２は、この偏差に上述したフィードバックゲインＫ_Ｌを乗じて、フィードバック制御を行う。倒立制御計算では、例えば、車体１２の傾斜角度及び傾斜角速度などを制御目標値として算出する。車体１２の傾斜角速度は、ジャイロセンサ４８で測定される。そして、傾斜角速度を積分することによって、車体１２の傾斜角度が求まる。例えば、倒立走行時には、姿勢の目標傾斜角度が０°になるようにフィードバック制御する。また、その場で停止させる場合は、目標傾斜角速度が０になるようにフィードバック制御する。

より具体的には、指令値生成部９２は、目標傾斜角度及び目標傾斜角速度、目標車輪回転角度及び目標車輪回転角速度、目標スライド位置及び目標スライド速度、目標スイングアーム角度及び目標スイングアーム速度を生成する。減算器を用いて、センサ類８３の検出値から取得した傾斜角度及び傾斜角速度と目標傾斜角度及び目標傾斜角速度との姿勢偏差、センサ類８３の検出値から取得した車輪回転角度及び車輪回転角速度と目標車輪回転角度及び目標車輪回転角速度との第１の偏差、センサ類８３の検出値から取得したスライド位置及びスライド速度と目標スライド位置及び目標スライド速度との第２の偏差、センサ類８３の検出値から取得したスイングアーム角度及びスイングアーム速度と目標スイングアーム角度及び目標スイングアーム速度との第３の偏差を算出する。

尚、スイングアームに関する制御量を算出するための偏差には、積分演算値を含むものとしてもよい。即ち、センサ類８３の検出値から取得したスイングアーム角度と目標スイングアーム角度との偏差を積分して算出する積分値を、協調制御部９２に対して更に入力するものとしてもよい。これにより、スイングアームに関する重力補償を行うことができる。また、姿勢偏差には、積分演算値を含むものとしてもよい。即ち、センサ類８３の検出値から取得した傾斜角と目標傾斜角との偏差を積分して算出する積分値を、協調制御部９２に対して更に入力するものとしてもよい。これにより、スイングアームに関する重力補償を行うことができる。これにより、姿勢偏差に関する定常偏差を解消することができる。

協調制御部９２は、上述したようにして算出した各偏差にフィードバックゲインを乗じて、各駆動部を駆動する制御量としてのスライド力、右スイングアームトルク、左スイングアームトルク、右車輪トルク、左車輪トルクを算出する。協調制御部９２は、算出したこれらの制御量を制御信号として出力し、右輪駆動モータ３４、左輪駆動モータ３６、右スイングアーム駆動モータ６０、左スイングアーム駆動モータ６４、及び搭乗席駆動モータ７０を協調して制御する。

より具体的には、協調制御部９２は制御信号を出力して、倒立状態を維持して安定して走行するように右輪駆動モータ３４及び左輪駆動モータ３６を駆動する。協調制御部９２は、右輪駆動モータ３４及び左輪駆動モータ３６に対する車輪トルクを指令値として出力する。即ち、モータ３４、３６のアンプ３４ａ、３６ａに車輪トルクに応じた制御信号を出力する。制御部８０からの制御信号は、アンプ３４ａ、３６ａを介して、右輪駆動モータ３４及び左輪駆動モータ３６に入力され、右輪駆動モータ３４及び左輪駆動モータ３６が駆動する。

また、協調制御部９２は制御信号を出力して、スイングアームが車体部を支持しながら鉛直方向に伸縮するようにスイングアーム関節６７を駆動する。協調制御部９２は、右輪駆動モータ３４及び左輪駆動モータ３６に対する右スイングアームトルク及び左スイングアームトルクを指令値として出力する。これにより、補助輪５１が接地している接地状態と、離地している離地状態とを切換えることができる。また、傾斜面を走行するときは、ジャイロセンサ４８などの出力に基づいて、制御信号を出力する。これにより、傾斜面の角度が吸収され、車体１２が水平になる。制御部８０からの制御信号は、アンプ６０ａ、６４ａを介して、右スイングアーム駆動モータ６０、左スイングアーム駆動モータ６４に入力され、右スイングアーム駆動モータ６０、左スイングアーム駆動モータ６４が駆動する。

また、協調制御部９２は制御信号を出力して、スライドが車体部を前後にスライドさせるように搭乗席駆動モータ７０を駆動する。協調制御部９２は、搭乗席駆動モータ７０のアンプ７０ａに対する力を指令値として出力する。制御部８０からの制御信号は、アンプ７０ａを介して、搭乗席駆動モータ７０に入力され、搭乗席駆動モータ７０が駆動する。

尚、駆動輪７８の回転速度は、右輪エンコーダ５２、及び左輪エンコーダ５４の出力によって、求めることができる。スイングアームの位置、スイングアームの速度は、スイングアームの各関節に設けられているエンコーダの出力によって求めることができる。搭乗席７４のスライドの位置、スライダの速度は、搭乗席駆動モータ７０に設けられているエンコーダの出力により求めることができる。また、スライド機構６８のスライド力は、搭乗席駆動モータ７０の回転トルクにより求めることができる。

また、協調制御部９２は、予め複数設定したフィードバックゲインのゲインパターンから、センサ類８３の検出値から取得したスイングアーム角度に応じて、ゲインパターンを切替える。これにより、単一の制御コントローラであっても、影響度の大きな駆動量に応じてゲインのゲインパターンを切替えることで、制御性能の低下を抑制することができる。

非線形補償部９３は、重力を含む加速度による非線形負荷を補償するフィードフォワード量を算出する。算出したフィードフォワード量を、加算器により、協調制御部９２で算出した制御量に加算する。これにより、摩擦を含む非線形項の補償を行うことができる。

以上説明したように、協調制御部９２により車輪とスイングアームとスライドに関する制御を実現することで、多自由度を有する倒立車輪型移動体に対しても良好な動作制御を実現することができる。

尚、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

本発明の実施の形態にかかる移動体の構成を示す側面図である。本発明の実施の形態にかかる移動体の構成を示す正面図である。本発明の実施の形態にかかる移動体による制御目標値の算出方法について説明するための図である。本発明の実施の形態にかかる移動体のモデルを示す図である。本発明の実施の形態にかかるゲインマップの一例を示す図である。スイングアームの回転角度以外の駆動量に応じてフィードバックゲインを切替える実施例を示す図である。本発明の実施の形態にかかる移動体の制御系の構成を示すブロック図である。

符号の説明

１２車体、１７右スイングアーム、１９左スイングアーム、
１８右駆動輪、２０左駆動輪、２１右上リンク、２２左上リンク、
２６右マウント、２８左マウント、
３０車軸、３２車軸、３４右輪駆動モータ、３６左輪駆動モータ、
４１本体部、４２操作レバー、４３操作角センサ、４４バッテリーモジュール、
４６操作モジュール、４８ジャイロセンサ、５１補助輪、
５２右輪エンコーダ、５４左輪エンコーダ、５５補助輪支持ブロック、
５８センサ、６０右スイングアーム駆動モータ、６２右スイング軸
６４左スイングアーム駆動モータ、６６左スイング軸
６７スイングアーム関節、６８スライド機構、
７０搭乗席駆動モータ、７２ラックアンドピニオン、７４搭乗席、
７６上体部、７７車体部、７８駆動輪、
８０制御部、８３センサ類、
９１協調制御部、９２指令値生成部、９３非線形補償部、
１００移動体、

Claims

車輪を回転可能に支持する車台と、
前記車輪を回転駆動する第１の駆動部と、
支持部材を介して前記車台に対して回動可能に支持された車体部と、
前記車体部を駆動する第２の駆動部と、
前記車体部の高さを変化させる第３の駆動部と、
前記第１の駆動部、前記第２の駆動部、及び第３の駆動部を制御する制御部と、
前記車体部の鉛直方向に対する傾斜角度及び傾斜角速度を取得する姿勢傾斜測定部と、
前記第１の駆動部の駆動量を取得する第１の測定部と、
前記第２の駆動部の駆動量を取得する第２の測定部と、
前記第３の駆動部の駆動量を取得する第３の測定部と、を備える倒立車輪型移動体であって、
前記制御部が、
前記姿勢傾斜測定部により取得した前記傾斜角度及び傾斜角速度と目標傾斜角度及び目標傾斜角速度との姿勢偏差、前記第１の測定部により取得した駆動量と目標駆動量との第１の偏差、前記第２の測定部により取得した駆動量と目標駆動量との第２の偏差、及び前記第３の測定部により取得した駆動量と目標駆動量との第３の偏差を算出し、
当該算出した４つの偏差に所定のゲインを乗じて、前記第１の駆動部を駆動する第１の制御量、前記第２の駆動部を駆動する第２の制御量、及び前記第３の駆動部を駆動する第３の制御量を算出する
倒立車輪型移動体。
前記第３の偏差に積分演算値を含む
ことを特徴とする請求項１記載の倒立車輪型移動体。
前記姿勢偏差に積分演算値を含む
ことを特徴とする請求項１又は２記載の倒立車輪型移動体。
前記制御部は、
重力を含む加速度による非線形負荷を補償するフィードフォワード量を算出し、当該算出したフィードフォワード量を前記第１の制御量、前記第２の制御量、及び前記第３の制御量に加算する
ことを特徴とする請求項１乃至３いずれか１項記載の倒立車輪型移動体。
前記ゲインのゲインパターンを予め複数設定し、
前記第３の測定部により取得した前記第３の駆動部の駆動量に応じて、前記ゲインパターンを切替える
ことを特徴とする請求項１乃至４いずれか１項記載の倒立車輪型移動体。
前記第３の駆動部が前記車体部を支持しながら鉛直方向に伸縮するアーム機構を有しており、
前記第３の測定部は、前記アーム機構の変化量を前記第３の駆動部の駆動量として取得する
ことを特徴とする請求項１記載の倒立車輪型移動体。
前記第２の駆動部が前記車体部を前後にスライドさせるスライド機構を有し、
前記第３の駆動部が前記車体部を支持しながら鉛直方向に伸縮するアーム機構を有し、
前記制御部は、
前記算出した第１の制御量としての車輪トルク指令を前記車輪に出力し、
前記算出した第２の制御量としてのスライド力指令を前記スライド機構に出力し、
前記算出した第３の制御量としてのアームトルク指令を前記アーム機構に出力する
ことを特徴とする請求項１記載の倒立車輪型移動体。
車輪を回転可能に支持する車台と、
前記車輪を回転駆動する第１の駆動部と、
支持部材を介して前記車台に対して回動可能に支持された車体部と、
前記車体部を駆動する第２の駆動部と、
前記車体部の高さを変化させる第３の駆動部と、を備える倒立車輪型移動体の制御方法であって、
前記車体部の鉛直方向に対する傾斜角度及び傾斜角速度を取得し、
前記第１の駆動部の駆動量、前記第２の駆動部の駆動量、及び前記第３の駆動部の駆動量を取得し、
前記取得した前記傾斜角度及び傾斜角速度と目標傾斜角度及び目標傾斜角速度との姿勢偏差、前記取得した駆動量と目標駆動量との第１の偏差、前記取得した駆動量と目標駆動量との第２の偏差、及び前記取得した駆動量と目標駆動量との第３の偏差を算出し、
当該算出した４つの偏差に所定のゲインを乗じて、前記第１の駆動部を駆動する第１の制御量、前記第２の駆動部を駆動する第２の制御量、及び前記第３の駆動部を駆動する第３の制御量を算出する倒立車輪型移動体の制御方法。
前記第３の偏差に積分演算値を含む
ことを特徴とする請求項８記載の倒立車輪型移動体の制御方法。
前記姿勢偏差に積分演算値を含む
ことを特徴とする請求項８又は９記載の倒立車輪型移動体の制御方法。
重力を含む加速度による非線形負荷を補償するフィードフォワード量を算出し、当該算出したフィードフォワード量を前記第１の制御量、前記第２の制御量、及び前記第３の制御量に加算する
ことを特徴とする請求項８乃至１０いずれか１項記載の倒立車輪型移動体の制御方法。
前記ゲインのゲインパターンを予め複数設定し、
前記取得した前記第３の駆動部の駆動量に応じて、前記ゲインパターンを切替える
ことを特徴とする請求項８乃至１１いずれか１項記載の倒立車輪型移動体の制御方法。
前記第３の駆動部が前記車体部を支持しながら鉛直方向に伸縮するアーム機構を有しており、
前記アーム機構の変化量を前記第３の駆動部の駆動量として取得する
ことを特徴とする請求項８記載の倒立車輪型移動体の制御方法。
前記第２の駆動部が前記車体部を前後にスライドさせるスライド機構を有し、
前記第３の駆動部が前記車体部を支持しながら鉛直方向に伸縮するアーム機構を有し、
前記算出した第１の制御量としての車輪トルク指令を前記車輪に出力し、
前記算出した第２の制御量としてのスライド力指令を前記スライド機構に出力し、
前記算出した第３の制御量としてのアームトルク指令を前記アーム機構に出力する
ことを特徴とする請求項８記載の倒立車輪型移動体の制御方法。