JP5772373B2 - 倒立型移動体制御装置、及びその制御方法 - Google Patents

Description

本発明は、倒立状態を維持しつつ所望の走行を行う倒立型移動体を制御する倒立型移動体制御装置、及びその制御方法に関するものである。

プラットホーム上に搭乗者が搭乗し、その搭乗者の操作に応じて、倒立状態を維持しつつ所望の走行を行う倒立型移動体が知られている（例えば、特許文献１参照）。

特表２００３−５０２００２号公報

上記特許文献１に示す倒立型移動体においては、例えば、高速走行中に、搭乗者が無理な加速操作を行った場合、その倒立状態の維持が困難となることがある。この場合、搭乗者に対して意図しない力が掛かり、倒立型移動体が急停車するなどの問題が生じている。

本発明は、このような問題点を解決するためになされたものであり、倒立型移動体の安全性を向上させることができる倒立型移動体制御装置、及びその制御方法を提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、倒立型移動体の状態量を検出する状態検出手段と、前記倒立型移動体の車輪を駆動する駆動手段と、搭乗者の操作情報が入力され、該操作情報に応じた操作指令値を算出する指令入力手段と、前記状態検出手段により検出された前記倒立型移動体の状態量に基づいて、搭乗者の姿勢状態を推定する状態推定手段と、前記状態検出手段により検出された前記倒立型移動体の状態量と、前記駆動手段に対するトルク指令値と、に基づいて、前記倒立型移動体の走行状態を判定する判定手段と、前記状態推定手段により推定された前記搭乗者の姿勢状態と、前記判定手段により判定された前記倒立型移動体の走行状態と、に基づいて、該走行状態毎に、前記状態量、前記姿勢状態及び前記トルク指令値を調整する際の優先順位を設定し、該優先順位に応じて制御パラメータの重みを算出する重み演算手段と、前記判定手段により判定された前記倒立型移動体の走行状態と、前記指令入力手段により算出された操作指令値と、に基づいて、前記判定手段により判定された前記走行状態毎に、前記状態検出手段により検出される状態量の目標値となる指令値を算出する指令演算手段と、前記指令入力手段により算出された操作指令値と、前記指令演算手段により算出された前記指令値と、前記重み演算手段により算出された制御パラメータの重みと、に基づいて、前記トルク指令値を算出するトルク指令演算手段と、を備える、ことを特徴とする倒立型移動体制御装置である。この一態様によれば、倒立型移動体の安全性を向上させることができる。

この一態様において、前記状態量検出手段は、前記車輪の角度、角速度、角加速度、前記搭乗者が搭乗するプラットホームの姿勢角度、姿勢角速度、及び姿勢角加速度のうち少なくとも１つを検出してもよい。

この一態様において、前記状態推定手段は、前記状態検出手段により検出された前記車輪の角度と、搭乗者が搭乗するプラットホームの姿勢角度と、に基づいて、前記搭乗者の姿勢角度を推定してもよい。

この一態様において、前記判定手段は、前記倒立型移動体の走行状態を、前記状態検出手段により検出された前記車輪の速度の絶対値が所定速度以下となり、かつ、前記トルク指令値の絶対値が前記駆動手段の最大トルクより小さくなるケース１と、前記状態検出手段により検出された前記車輪の速度の絶対値が所定速度以下となり、かつ、前記トルク指令値の絶対値が前記駆動手段の最大トルクに等しくなるケース２と、前記状態検出手段により検出された前記車輪の速度の絶対値が所定速度より大きくなり、かつ、前記トルク指令値の絶対値が前記駆動手段の最大トルクより小さな値に設定された所定トルクよりも小さくなるケース３と、前記状態検出手段により検出された前記車輪の速度の絶対値が所定速度より大きくなり、かつ、前記トルク指令値の絶対値が前記所定トルク以上となるケース４と、に判定してもよい。これにより、倒立型移動体の走行状態を判定し、その走行状態に最適な動作制御を実施できる。

この一態様において、前記判定手段により前記倒立型移動体の走行状態が前記ケース１又はケース３と判定されたとき、前記重み演算手段及び前記指令演算手段は、前記トルク指令値が前記駆動手段の最大トルクを超えないことを第１優先とし、前記搭乗者の姿勢角度及び姿勢角速度の絶対値を減少させることを第２優先とし、前記搭乗者が搭乗するプラットホームの姿勢角度及び姿勢角速度の絶対値を減少させることを第３優先となるように、前記制御パラメータの重み及び前記指令値を夫々算出し、前記判定手段により前記倒立型移動体の走行状態が前記ケース２と判定されたとき、前記重み演算手段は、前記制御パラメータの重みを算出せず、前記トルク指令演算手段は前記トルク指令値を０に設定し、前記判定手段により前記倒立型移動体の走行状態が前記ケース４と判定されたとき、前記重み演算手段及び前記指令演算手段は、前記トルク指令値が前記駆動手段の最大トルクを超えないことを第１優先とし、前記搭乗者の姿勢角速度の絶対値を減少させることを第２優先とし、前記搭乗者の姿勢角度の絶対値を減少させることを第３優先とし、前記車輪の速度の絶対値を減少させることを第４優先とし、前記プラットホームの姿勢角度の絶対値を減少させることを第５優先となるように、前記制御パラメータの重み及び前記指令値を夫々算出してもよい。これにより、倒立型移動体の走行状態に最適な制御パラメータ及び指令値を設定できる。

この一態様において、前記判定手段により前記倒立型移動体の走行状態が前記ケース１乃至４と判定されたとき、前記重み演算手段は、下記（１７）式乃至（１９）式及び（２２）式乃至（２４）式を用いて前記制御パラメータの重みＱ、Ｒ、Ｈを算出してもよい。これにより、倒立型移動体の走行状態に最適な制御パラメータを設定できる。

この一態様において、前記判定手段により前記倒立型移動体の走行状態が前記ケース１乃至４と判定されたとき、前記指令演算手段は、下記（２０）式及び（２５）式を用いて、前記指令値ｒを算出してもよい。これにより、倒立型移動体の走行状態に最適な指令値を設定できる。

この一態様において、前記トルク指令演算手段は、下記（１４）式及び（１５）式を用いて前記トルク指令値Ｔ_ｍを算出してもよい。これにより、倒立型移動体の走行状態に最適なトルク指令値を設定できる。

他方、上記目的を達成するための本発明の一態様は、倒立型移動体の状態量を検出するステップと、搭乗者の操作情報が入力され、該操作情報に応じた操作指令値を算出するステップと、前記検出された倒立型移動体の状態量に基づいて、搭乗者の姿勢状態を推定するステップと、前記検出された倒立型移動体の状態量と、車輪を駆動する駆動手段に対するトルク指令値と、に基づいて、前記倒立型移動体の走行状態を判定するステップと、前記推定された搭乗者の姿勢状態と、前記判定された倒立型移動体の走行状態と、に基づいて、該走行状態毎に、前記状態量、前記姿勢状態及び前記トルク指令値を調整する際の優先順位を設定し、該優先順位に応じて制御パラメータの重みを算出するステップと、前記判定された倒立型移動体の走行状態と、前記算出された操作指令値と、に基づいて、前記判定された走行状態毎に、前記検出される状態量の目標値となる指令値を算出するステップと、前記算出された操作指令値と、前記算出された指令値と、前記算出された制御パラメータの重みと、に基づいて、前記トルク指令値を算出するステップと、を含む、ことを特徴とする倒立型移動体制御装置の制御方法であってもよい。この一態様によれば、倒立型移動体の安全性を向上させることができる。

また、上記目的を達成するための本発明の一態様は、搭乗者の操作情報が入力され、該操作情報に応じた操作指令値を算出する処理と、検出された倒立型移動体の状態量に基づいて、搭乗者の姿勢状態を推定する処理と、前記検出された倒立型移動体の状態量と、車輪を駆動する駆動手段に対するトルク指令値と、に基づいて、前記倒立型移動体の走行状態を判定する処理と、前記推定された搭乗者の姿勢状態と、前記判定された倒立型移動体の走行状態と、に基づいて、該走行状態毎に、前記状態量、前記姿勢状態及び前記トルク指令値を調整する際の優先順位を設定し、該優先順位に応じて制御パラメータの重みを算出する処理と、前記判定された倒立型移動体の走行状態と、前記算出された操作指令値と、に基づいて、前記判定された走行状態毎に、前記検出される状態量の目標値となる指令値を算出する処理と、前記算出された操作指令値と、前記算出された指令値と、前記算出された制御パラメータの重みと、に基づいて、前記トルク指令値を算出する処理と、をコンピュータに実行させることを特徴とするプログラムであってもよい。この一態様によれば、倒立型移動体の安全性を向上させることができる。

本発明によれば、倒立型移動体の安全性を向上させることができる倒立型移動体制御装置、及びその制御方法を提供することができる。

本発明の一実施の形態に係る倒立型移動体制御装置の概略的なシステム構成を示すブロック図である。 本発明の一実施の形態に係る倒立型移動体制御装置のシミュレーションにおける搭乗者の姿勢角度の時間変化を示す図である。 本発明の一実施の形態に係る倒立型移動体制御装置のシミュレーションにおける搭乗者の姿勢角加速度の時間変化を示す図である。 本発明の一実施の形態に係る倒立型移動体の概略的構成を示す斜視図である。

以下、図面を参照して本発明の実施の形態について説明する。
本発明の一実施の形態に係る倒立型移動体制御装置１３０は、図４に示すような倒立型移動体１２０の走行を制御する装置である。本実施の形態に係る倒立型移動体１２０は、搭乗者が搭乗するプラットホーム４２１と、プラットホーム４２１の両側に回転可能に設けられた一対の第１及び第２駆動車輪４１０と、プラットホーム４２１に前後左右方向へ揺動可能に設けられ搭乗者が把持し操作を行うハンドル４２２と、を備えている。倒立型移動体１２０は、搭乗者の走行操作（ハンドル４２２の操作、プラットホーム４２１上での体重移動、ハンドル４２２やプラットホーム４２１に設けられたスイッチ操作、など）に応じて倒立状態を維持しつつ、所望の走行を行うように構成されている。

図１は、本実施の形態に係る倒立型移動体制御装置の概略的なシステム構成を示すブロック図である。本実施の形態に係る倒立型移動体制御装置１３０は、第１及び第２駆動車輪４１０を駆動する一対の第１及び第２モータ１２１と、搭乗者により操作情報が入力される指令入力器１２２と、倒立型移動体１２０の状態量を状態検出値として検出する状態検出器１２３と、搭乗者の姿勢角度を推定する状態推定器１３１と、倒立型移動体１２０の走行状態を判定するケース判定器１３２と、制御パラメータの重みを算出する重み演算器１３３と、トルク指令演算器１３５に対する指令値を算出する指令演算器１３４と、第１及び第２モータ１２１に対するトルク指令値を算出するトルク指令演算器１３５と、を備えている。

なお、プラットホーム４２１には、一対の第１及び第２駆動車輪４１０が設けられる構成であるが、これに限らず、例えば、１つ或いは３つ以上の駆動車輪、さらに従動輪が組み合わされる構成であってもよい。

また、倒立型移動体制御装置１３０は、例えば、ＡＳＩＣ（Application Specific Integrated Circuit）、プログラマブル電子システムなどにより実現することもできる。

第１及び第２モータ１２１は、駆動手段の一具体例であり、プラットホーム４２１に内蔵され、減速機などを介して第１及び第２駆動車輪４１０の駆動軸に連結されている。第１及び第２モータ１２１は、トルク指令演算器１３５からのトルク指令値に応じて、第１及び第２駆動車輪４１０を独立して夫々回転駆動させ、倒立型移動体１２０の倒立状態を維持しつつ所望の進行方向及び速度で走行させる。

指令入力器１２２は、指令入力手段の一具体例であり、搭乗者によるプラットホーム４２１、ハンドル４２２、スイッチなどの操作を介して操作情報が入力される。指令入力器１２２は、入力された操作情報に応じた所望の走行を行うための操作指令値（第１及び第２駆動車輪４１０の角度指令値、プラットホーム４２１の姿勢角度の指令値、搭乗者の姿勢角度の指令値など）を算出する。指令入力器１２２は、算出した操作指令値を指令演算器１３４及びトルク指令演算器１３５に対して出力する。これにより、搭乗者は、例えば、ハンドル４２２などを操作することで、指令入力器１２２を介して、倒立型移動体１２０を所望の移動速度及び方向に走行させることができる。

状態検出器１２３は、状態検出手段の一具体例であり、倒立型移動体１２０に設けられた回転センサや姿勢センサなどの検出センサを用いて、倒立型移動体１２０の状態量を検出する。

例えば、状態検出器１２３は、プラットホーム４２１に設けられた姿勢センサ（ジャイロセンサ、加速度センサなど）を用いて、プラットホーム４２１の姿勢角度（傾斜角度）を検出する。また、状態検出器１２３は、第１及び第２駆動車輪４１０に設けられた回転センサ（ポテンショメータなど）を用いて、第１及び第２駆動車輪４１０の角度、角速度、及び角加速度を検出する。状態検出器１２３には状態推定器１３１及びケース判定器１３２が接続されており、状態検出器１２３は、検出した状態量を状態検出値として状態推定器１３１及びケース判定器１３２に対して出力する。

状態推定器１３１は、状態推定手段の一具体例であり、状態検出器１２３から出力された倒立型移動体１２０の状態検出値（第１及び第２駆動車輪４１０の角速度、プラットホーム４２１の姿勢角度など）に基づいて、搭乗者の姿勢角度（傾斜角度）などの状態量を推定し、状態推定値として出力する。状態推定器１３１には重み演算器１３３が接続されており、状態推定器１３１は、推定した状態量を重み演算器１３３に対して出力する。

ケース判定器１３２は、判定手段の一具体例であり、状態検出器１２３から出力された倒立型移動体１２０の状態検出値（第１及び第２駆動車輪４１０の角速度、プラットホーム４２１の姿勢角度など）と、トルク指令演算器１３５から出力されたトルク指令値と、に基づいて、倒立型移動体１２０の走行状態を判定する。なお、倒立型移動体１２０の走行状態は、例えば、第１及び第２駆動車輪４１０の角速度などの複数の条件の組合せにより分類されている。ケース判定器１３２には、重み演算器１３３及び指令演算器１３４が接続されており、ケース判定器１３２は判定結果を重み演算器１３３及び指令演算器１３４に対して出力する。

重み演算器１３３は、重み演算手段の一具体例であり、状態推定器１３１からの出力された状態推定値と、ケース判定器１３２からの判定結果と、に基づいて、ケース判定器１３２が出力するケース毎のトルク指令値を決定するための制御パラメータの重みを算出する。重み演算器１３３にはトルク指令演算器１３５が接続されており、重み演算器１３３は算出した重みをトルク指令演算器１３５に対して出力する。

指令演算器１３４は、指令演算手段の一具体例であり、ケース判定器１３２からの判定結果に基づいて、ケース判定器１３２が出力するケース毎に、状態検出器１２３により検出される状態量の目標値となる指令値を算出する。指令演算器１３４にはトルク指令演算器１３５が接続されており、指令演算器１３４は算出した指令値をトルク指令演算器１３５に対して出力する。

トルク指令演算器１３５は、トルク指令演算手段の一具体例であり、重み演算器１３３から出力される重みと、指令演算器１３４から出力される指令値と、に基づいて、第１及び第２モータ１２１が発生するモータトルクを決めるトルク指令値を算出する。トルク指令演算器１３５には、駆動回路１３６を介して第１及び第２モータ１２１が接続されており、トルク指令演算器１３５は、算出したトルク指令値を、駆動回路１３６を介して第１及び第２モータ１２１に対して出力する。

次に、倒立型移動体制御装置１３０の動作原理を導出し、その機能を詳細に説明する。
まず、倒立型移動体１２０の運動方程式は、下記（１）式乃至（３）式として導出できる。

ただし、上記（１）式乃至（３）式において、各記号は以下の通りである。
Ｊ_１：第１及び第２駆動車輪４１０の慣性モーメント[ｋｇ・ｍ^２]、
Ｊ_３：搭乗者の慣性モーメント[ｋｇ・ｍ^２]、
θ_１：第１及び第２駆動車輪４１０の角度［ｒａｄ］、
θ_２：プラットホーム４２１の姿勢角度（傾斜角度）［ｒａｄ］、
θ_３：搭乗者の姿勢角度［ｒａｄ］、
ｍ_１：第１及び第２駆動車輪４１０の質量[ｋｇ]、
ｍ_３：搭乗者の質量[ｋｇ]、
ｒ_１：第１及び第２駆動車輪４１０の半径[ｍ]、
ｌ_ｃ：搭乗者の足裏中心と重心の間の距離[ｍ]、
Ｔ_ｍ：トルク指令演算器１３５により算出されるトルク指令値（モータトルク）[Ｎ・ｍ]、
Ｔ_ｉｎ：搭乗者が入力するトルク（搭乗者の走行操作に応じたトルク）[Ｎ・ｍ]

倒立型移動体１２０の通常動作において、下記（４）式乃至（６）式が成立する。

上記（４）式乃至（６）式を用いて、上記（１）式乃至（３）式は、下記（７）式乃至（９）式のように線形化される。

ここで、状態検出器１２３により第１及び第２駆動車輪４１０の角度θ_１とプラットホーム４２１の姿勢角度θ_２とが検出されることを考慮して、上記（７）式乃至（９）式の状態空間表現は、下記（１０）式及び（１１）式として表わすことができる。

上記（１０）式及び（１１）式に対して、ローゼンブロック・ハウタス・ポポフテストを適用すると、可観測であることが分かるので、下記（１２）式に示す状態推定器を設計できる。

状態推定器１３１は、状態検出器１２３から出力された第１及び第２駆動車輪４１０の角度θ_１と、プラットホーム４２１の姿勢角度θ_２と、に基づいて、上記設計した（１２）式を用いて、搭乗者の姿勢角度θ_３を推定し、推定した搭乗者の姿勢角度θ_３を重み演算器１３３に対して出力する。

ここで、上述のローゼンブロック・ハウタス・ポポフテストにより、上記（１０）式は可制御であることが分かるので、以下、その制御設計について詳細に説明する。

まず、下記（１３）式に示す評価関数を導入する。

ただし、上記（１３）式において、各記号は以下の通りである。
Ｔ_ｒ：所望の応答時間[ｓ]、
Ｑ：倒立型移動体１２０の状態量に対する重み行列、
Ｒ：トルク指令値Ｔ_ｍに対する重み値、
Ｈ：所望の応答時間における状態量に対する重み行列
なお、上記重みＱ、Ｒ、Ｈの設計方法については、詳細に後述する。
上記（１３）式の評価関数を最小化するようなトルク指令値Ｔ_ｍを導出するために、下記（１４）式に示す微分リカッチ方程式を導入する。

ただし、上記（１４）式において、各記号は以下の通りである。
δ：安定度［ｒａｄ／ｓ］、
Ｉ：単位行列
Ａ：倒立型移動体１２０の状態方程式のシステム行列
ｂ：倒立型移動体１２０の状態方程式の入力行列
Ｐ：微分リカッチ方程式の解

上記（１４）式の解Ｐを用いて、トルク指令値Ｔ_ｍは下記（１５）式として導出できる。

ただし、上記（１５）式において、各記号は以下の通りである。
ｒ：指令演算器１３４から出力される指令値
θ_１ｒ：指令入力器１２２からの第１及び第２駆動車輪４１０の角度指令値（車輪角度の指令値）［ｒａｄ］、
θ_２ｒ：指令入力器１２２からのプラットホーム４２１の姿勢角度の指令値［ｒａｄ］、
θ_３ｒ：指令入力器１２２からの搭乗者の姿勢角度の指令値［ｒａｄ］
ｋ：制御ゲイン

上記（１５）式を用いて、上記（１０）式で定義される状態量ｘが少なくともδ［ｒａｄ／ｓ］以上の収束の速さで指令値ｒに収束する。トルク指令演算器１３５は、指令演算器１３４から出力された指令値ｒと、重み演算器１３３から出力された重みＱ、Ｒ、Ｈと、指令入力器１２２から出力された操作指令値θ_１ｒ、θ_２ｒ、θ_３ｒと、に基づいて、上記（１５）式を用いて、トルク指令値Ｔ_ｍを算出する。トルク指令演算器１３５は、算出したトルク指令値Ｔ_ｍを第１及び第２モータ１２１とケース判定器１３２に対して出力する。

ケース判定器１３２は、状態検出器１２３から出力された第１及び第２駆動車輪４１０の角度θ_１と、トルク指令演算器１３５から出力されたトルク指令値Ｔ_ｍと、に基づいて、倒立型移動体１２０の走行状態が下記（１６）式に示す４つのケース１〜４のうち、いずれに該当するかを判定する。ケース判定器１３２は、その判定結果（ケース１〜４）を重み演算器１３３及び指令演算器１３４に対して出力する。

ただし、上記（１６）式において、各記号は以下の通りである。
θ_１ｓの１階時間微分値：安全車輪速度（所定速度）（搭乗者が安全にプラットホーム４２１から降りることができる第１及び第２駆動車輪４１０の速度）［ｒａｄ／ｓ］、

Ｔ_ｍｌｉｍ：モータトルク上限値（第１及び第２モータ１２１が出力できる最大トルク）[Ｎ・ｍ]、
Ｔ_ｓ：安全モータトルク（所定トルク）[Ｎ・ｍ]（モータトルク上限値Ｔ_ｍｌｉｍより小さな絶対値となるモータトルクである。また、安全車輪速度を超える第１及び第２駆動車輪４１０の速度で倒立型移動体１２０が移動中に、第１及び第２モータ１２１のモータトルクがこの値を超えると、搭乗者を安全に降車させるまたは倒立型移動体１２０を安全に減速停車させるためのモータトルクの閾値である。）

重み演算器１３３は、状態推定器１３１から出力される搭乗者の姿勢角度と、ケース判定器１３２から出力される判定結果（ケース１〜４）と、に基づいて、その判定結果に応じた制御パラメータの重みＱ、Ｒ、Ｈを夫々算出する。同様に、指令演算器１３４は、指令入力器１２２から出力される操作指令値と、ケース判定器１３２から出力される判定結果と、に基づいて、その判定結果に応じた指令値ｒを夫々算出する。

（ケース１及びケース３の場合）
重み演算器１３３は、ケース判定器１３２から出力された判定結果が上記ケース１およびケース３である場合、重みＱ、Ｒ、Ｈを、下記（１７）式乃至（１９）式を用いて算出し、算出した重みＱ、Ｒ、Ｈをトルク指令演算器１３５に出力する。同様に、指令演算器１３４は、ケース判定器１３２から出力された判定結果が上記ケース１およびケース３である場合、指令値ｒを、下記（２０）式を用いて算出し、算出した指令値ｒをトルク指令演算器１３５に対して出力する。

ただし、上記（１７）式乃至（２０）式における各記号の意味は以下の通りである。
θ_２ｌｉｍ：プラットホーム４２１の姿勢角度の上限値［ｒａｄ］、
θ_３ｌｉｍ：搭乗者の姿勢角度の上限値［ｒａｄ］

上述の如く、重み演算器１３３は、上記（１７）式乃至（１９）式に基づいて重みＱ、Ｒ、Ｈを算出し、トルク指令演算器１３５は、その算出された重みＱ、Ｒ、Ｈを用いて、トルク指令値Ｔ_ｍを算出する。これにより、トルク指令値Ｔ_ｍがモータトルク上限値Ｔ_ｍｌｉｍを超えないことを第１優先とし、搭乗者の姿勢角度θ_３と搭乗者の姿勢角速度（θ_３の１階時間微分値）の絶対値を減少させることを第２優先とし、プラットホーム４２１の姿勢角度θ_２とプラットホーム４２１の姿勢角速度（θ_２の１階時間微分値）の絶対値を減少させることを第３優先として設定される。この優先順位で、所望の応答時間Ｔ_ｒまでの間、倒立型移動体１２０が制御される。

（ケース２の場合）
重み演算器１３３は、ケース判定器１３２から出力された判定結果がケース２の場合、重みＱ、Ｒ、Ｈを算出しない。この場合、トルク指令演算器１３５は、下記（２１）式に示すトルク指令値Ｔ_ｍを、第１及び第２モータ１２１とケース判定器１３２に対して出力する。

上記（２１）式のトルク指令値Ｔ_ｍによると、第１及び第２駆動車輪４１０の速度（θ_１の１階時間微分値）の絶対値が、安全車輪速度（θ_１ｓの１階時間微分値）の絶対値よりも小さく、且つトルク指令値Ｔ_ｍがモータトルク上限値Ｔ_ｍｌｉｍに達すると、トルク指令値Ｔ_ｍを０[Ｎ・ｍ]とし、搭乗者を進行方向へ安全に降車させることができる。

（ケース４の場合）
重み演算器１３３は、ケース判定器１３２から出力された判定結果が上記ケース４である場合、重みＱ、Ｒ、Ｈを、下記（２２）式乃至（２４）式を用いて算出し、算出した重みＱ、Ｒ、Ｈをトルク指令演算器１３５に対して出力する。同様に、指令演算器１３４は、ケース判定器１３２から出力された判定結果が上記ケース４である場合、指令値ｒを、下記（２５）式を用いて算出し、算出した指令値ｒをトルク指令演算器１３５に対して出力する。

ただし、上記（２２）式乃至（２５）式において、各記号は以下の通りである。
θ_１ｌｉｍの１階時間微分値：車輪速度上限値（第１及び第２駆動車輪４１０の速度上限値）［ｒａｄ／ｓ］、
θ_３ｓの２階時間微分値：安全搭乗者姿勢角加速度［ｒａｄ／ｓ^２］（搭乗者が倒立型移動体１２０に安全に乗車し続けることができる搭乗者の姿勢角加速度の上限値）
Ｔ：制御周期[ｓ]、
ｋ：時間のインデックス

上述の如く、重み演算器１３３は、上記（２２）式乃至（２５）式に基づいて重みＱ、Ｒ、Ｈを算出し、トルク指令演算器１３５は、その算出された重みＱ、Ｒ、Ｈを用いてトルク指令値Ｔ_ｍを算出する。これにより、トルク指令値Ｔ_ｍがモータトルク上限値Ｔ_ｍｌｉｍを超えないことを最優先とし、搭乗者の姿勢角速度（θ_３の１階時間微分値）の絶対値を減少させることを第２優先とし、搭乗者の姿勢角度θ_３の絶対値を減少させることを第３優先とし、第１及び第２駆動車輪４１０の速度（θ_１の１階時間微分値）の絶対値を減少させることを第４優先とし、プラットホーム４２１の姿勢角度θ_２の絶対値を減少させることを第５優先として設定される。この優先順位で、搭乗者の姿勢角加速度（θ_３の２階時間微分値）が安全搭乗者姿勢角加速度（θ_３ｓの２階時間微分値）を超えないように、所望の応答時間Ｔ_ｒまでの間、倒立型移動体１２０は制御される。

上述のように制御することで、倒立型移動体１２０で高速で走行する際に、搭乗者が無理な加速を強いた場合でも、安全に走行を継続できる範囲を超えた力が搭乗者に掛かることを防ぎ、且つ倒立型移動体１２０を安全な速度まで減速させることができる。

次に、本実施の形態に係る倒立型移動体制御装置１３０において行ったシミュレーション結果について、詳細に説明する。なお、本シミュレーションにおいて、各パラメータの値は、例えば、以下のように設定されている。

ｍ_１＝５[ｋｇ]、ｍ_３＝７０[ｋｇ]、Ｊ_１＝０．０２５[ｋｇ・ｍ^２]、Ｊ_３＝２５．２[ｋｇ・ｍ^２]、ｒ_１＝０．１[ｍ]、ｌ_ｃ＝０．８[ｍ]、ｇ＝９．８[ｍ／ｓ^２]、θ_２ｌｉｍ＝π／１６［ｒａｄ］、θ_３ｌｉｍ＝π／１６［ｒａｄ］、θ_１ｌｉｍの１階時間微分値＝２７．７［ｒａｄ／ｓ］、θ_２ｌｉｍの１階時間微分値＝０．１［ｒａｄ／ｓ］、θ_３ｌｉｍの１階時間微分値＝０．１［ｒａｄ／ｓ］、θ_１ｓの１階時間微分値＝２２．２［ｒａｄ／ｓ］、θ_３ｓの２階時間微分値＝９８［ｒａｄ／ｓ^２］、Ｔ_ｍｌｉｍ＝０．６３７[Ｎ・ｍ]、Ｔ_ｓ＝０．７・Ｔ_ｍｌｉｍ[Ｎ・ｍ]、Ｔ＝１×１０^−３[ｓ]、ｃ_１２＝０．５、ｃ_１３＝１、ｃ_４２＝０．２５、ｃ_４３＝０．７５、ｃ_４４＝０．５、ｃ_４６＝１、δ＝１×１０^−５［ｒａｄ／ｓ］、Ｔ_ｉｎ＝Ｔ_ｍｌｉｍ／２[Ｎ・ｍ]、θ_１ｉｃ（第１及び第２駆動車輪４１０の角度の初期値）＝０［ｒａｄ］、θ_２ｉｃ（プラットホーム４２１の姿勢角度の初期値）＝θ_２ｌｉｍ［ｒａｄ］、θ_３ｉｃ（搭乗者の姿勢角度の初期値）＝θ_３ｌｉｍ［ｒａｄ］、θ_１ｉｃの１階時間微分値＝θ_１ｓの１階時間微分値［ｒａｄ／ｓ］、θ_２ｉｃの１階時間微分値＝θ_２ｌｉｍの１階時間微分値［ｒａｄ／ｓ］、θ_３ｉｃの１階時間微分値＝θ_３ｌｉｍの１階時間微分値［ｒａｄ／ｓ］

本シミュレーションにおいて、車輪速度上限値（θ_１ｌｉｍの１階時間微分値）は、一般人がジョギングするときの速度で倒立型移動体１２０が走行させた場合の車輪速度である。また、プラットホーム４２１の姿勢角速度の上限値（θ_２ｌｉｍの１階時間微分値）、及び、搭乗者速度上限値（θ_３ｌｉｍの１階時間微分値）は、倒立型移動体１２０の通常動作中において起こりうる速度の上限値である。さらに、安全車輪速度（θ_１ｓの１階時間微分値）は、一般人が早歩きするときの速度で倒立型移動体１２０を走行させた場合の車輪速度である。安全搭乗者加速度（θ_３ｓの２階時間微分値）は、本シミュレーションで想定している搭乗者が受けても倒立型移動体１２０に搭乗し続けることができる加速度の上限値である。

本シミュレーションにおいて、搭乗者がモータトルク上限値Ｔ_ｍｌｉｍの半分にあたる大きなトルクを加え続けながら走行を続けた場合に、ケース判定器１３２が倒立型移動体１２０の走行状態を上述のケース４と判定した場合を想定している。

図２は、本シミュレーションにおける搭乗者の姿勢角度の時間変化を示す図である。図２において、実線（１）は、従来に係る倒立型移動体制御装置が最適制御を行った場合における搭乗者の姿勢角度の時間変化を示している。また、点線（２）は、本実施の形態に係る倒立型移動体制御装置１３０における搭乗者の姿勢角度の時間変化を示している。さらに、点線（３）は、搭乗者が転倒した場合の搭乗者の姿勢角度（約０．８［ｒａｄ］）を示している。すなわち、点線（３）より大きな搭乗者の姿勢角度は、搭乗者が転倒していることを表わしている。

図２に示すように、従来に係る倒立型移動体制御装置によると、０．１[ｓ]において搭乗者は転倒しているのに対し、本実施の形態に係る倒立型移動体制御装置１３０によると、搭乗者は転倒することなく継続して安全に搭乗していることが分かる。

図３は、本シミュレーションにおける搭乗者の姿勢角加速度の時間変化を示す図である。図３において、実線（１）は従来に係る倒立型移動体制御装置が最適制御を行った場合における搭乗者の姿勢角加速度の時間変化を示している。また、点線（２）は、本実施の形態に係る倒立型移動体制御装置１３０における搭乗者の姿勢角加速度の時間変化を示している。さらに、点線（３）は、安全搭乗者姿勢角加速度（θ_３ｓの２階時間微分値）を示している。

図３に示すように、従来に係る倒立型移動体制御装置によると安全搭乗者姿勢加速度を超える加速度が搭乗者にかかっているのに対し、本実施の形態に係る倒立型移動体制御装置１３０によると安全搭乗者姿勢加速度を下回った加速度しか搭乗者にかかっていないことが分かる。すなわち、本実施の形態に係る倒立型移動体制御装置１３０によれば、安全に走行を継続できる範囲を超えた力が搭乗者に掛からないことが分かる。さらに、０．１[ｓ]において、第１及び第２駆動車輪４１０の速度は安全車輪速度（θ_１ｓの１階微分値）まで減速されている。

以上、本実施の形態に係る倒立型移動体制御装置１３０によれば、倒立型移動体１２０が高速で走行する際に、搭乗者が無理な加速を強いた場合でも、安全に走行を継続できる範囲を超えた力が搭乗者に掛かることを防ぎ、且つ、倒立型移動体１２０を安全な速度まで減速させることができる。すなわち、倒立型移動体１２０の安全性を向上させることができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

また、上記一実施の形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、上記指令入力器１２２、状態推定器１３１、ケース判定器１３２、重み演算器１３３、指令演算器１３４、及びトルク指令演算器１３５、が行なう処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

本発明は、例えば、単数又は複数の車輪を備え、人が搭乗して倒立状態を維持しつつ所望の走行を行う、電動車椅子やサービスロボットなどに広く適用できる。

１２０ 倒立型移動体
１２１ 第１及び第２モータ
１２２ 指令入力器
１２３ 状態検出器
１３０ 倒立型移動体制御装置
１３１ 状態推定器
１３２ ケース判定器
１３３ 重み演算器
１３４ 指令演算器
１３５ トルク指令演算器
４１０ 第１及び第２駆動車輪
４２１ プラットホーム
４２２ ハンドル

Claims (10)

1. 倒立型移動体の状態量を検出する状態検出手段と、
   前記倒立型移動体の車輪を駆動する駆動手段と、
   搭乗者の操作情報が入力され、該操作情報に応じた操作指令値を算出する指令入力手段と、
   前記状態検出手段により検出された前記倒立型移動体の状態量に基づいて、搭乗者の姿勢状態を推定する状態推定手段と、
   前記状態検出手段により検出された前記倒立型移動体の状態量と、前記駆動手段に対するトルク指令値と、に基づいて、前記倒立型移動体の走行状態を判定する判定手段と、
   前記状態推定手段により推定された前記搭乗者の姿勢状態と、前記判定手段により判定された前記倒立型移動体の走行状態と、に基づいて、該走行状態毎に、前記状態量、前記姿勢状態及び前記トルク指令値を調整する際の制御パラメータの重みを算出する重み演算手段と、
   前記判定手段により判定された前記倒立型移動体の走行状態と、前記指令入力手段により算出された操作指令値と、に基づいて、前記判定手段により判定された前記走行状態毎に、前記状態検出手段により検出される状態量の目標値となる指令値を算出する指令演算手段と、
   前記指令入力手段により算出された操作指令値と、前記指令演算手段により算出された前記指令値と、前記重み演算手段により算出された制御パラメータの重みと、に基づいて、前記トルク指令値を算出するトルク指令演算手段と、
   を備える、ことを特徴とする倒立型移動体制御装置。
2. 請求項１記載の倒立型移動体制御装置であって、
   前記状態量検出手段は、前記車輪の角度、角速度、角加速度、前記搭乗者が搭乗するプラットホームの姿勢角度、姿勢角速度、及び姿勢角加速度のうち少なくとも１つを検出する、ことを特徴とする倒立型移動体制御装置。
3. 請求項１又は２記載の倒立型移動体制御装置であって、
   前記状態推定手段は、前記状態検出手段により検出された前記車輪の角度と、搭乗者が搭乗するプラットホームの姿勢角度と、に基づいて、前記搭乗者の姿勢角度を推定する、ことを特徴とする倒立型移動体制御装置。
4. 請求項１乃至３のうちいずれか１項記載の倒立型移動体制御装置であって、
   前記判定手段は、前記倒立型移動体の走行状態を、
   前記状態検出手段により検出された前記車輪の速度の絶対値が所定速度以下となり、かつ、前記トルク指令値の絶対値が前記駆動手段の最大トルクより小さくなるケース１と、
   前記状態検出手段により検出された前記車輪の速度の絶対値が所定速度以下となり、かつ、前記トルク指令値の絶対値が前記駆動手段の最大トルクに等しくなるケース２と、
   前記状態検出手段により検出された前記車輪の速度の絶対値が所定速度より大きくなり、かつ、前記トルク指令値の絶対値が前記駆動手段の最大トルクより小さな値に設定された所定トルクよりも小さくなるケース３と、
   前記状態検出手段により検出された前記車輪の速度の絶対値が所定速度より大きくなり、かつ、前記トルク指令値の絶対値が前記所定トルク以上となるケース４と、
   に判定する、ことを特徴とする倒立型移動体制御装置。
5. 請求項４記載の倒立型移動体制御装置であって、
   前記判定手段により前記倒立型移動体の走行状態が前記ケース１又はケース３と判定されたとき、前記重み演算手段及び前記指令演算手段は、前記トルク指令値が前記駆動手段の最大トルクを超えないことを第１優先とし、前記搭乗者の姿勢角度及び姿勢角速度の絶対値を減少させることを第２優先とし、前記搭乗者が搭乗するプラットホームの姿勢角度及び姿勢角速度の絶対値を減少させることを第３優先となるように、前記制御パラメータの重み及び前記指令値を夫々算出し、
   前記判定手段により前記倒立型移動体の走行状態が前記ケース２と判定されたとき、前記重み演算手段は、前記制御パラメータの重みを算出せず、前記トルク指令演算手段は前記トルク指令値を０に設定し、
   前記判定手段により前記倒立型移動体の走行状態が前記ケース４と判定されたとき、前記重み演算手段及び前記指令演算手段は、前記トルク指令値が前記駆動手段の最大トルクを超えないことを第１優先とし、前記搭乗者の姿勢角速度の絶対値を減少させることを第２優先とし、前記搭乗者の姿勢角度の絶対値を減少させることを第３優先とし、前記車輪の速度の絶対値を減少させることを第４優先とし、前記プラットホームの姿勢角度の絶対値を減少させることを第５優先となるように、前記制御パラメータの重み及び前記指令値を夫々算出する、
   ことを特徴とする倒立型移動体制御装置。
6. 請求項５記載の倒立型移動体制御装置であって、
   前記判定手段により前記倒立型移動体の走行状態が前記ケース１、３、及び４のうちのいずれかに判定されたとき、前記重み演算手段は、下記式を用いて前記制御パラメータの重みＱ、Ｒ、Ｈを算出する、ことを特徴とする倒立型移動体制御装置。
   ケース１及び３の場合：
   

   

   

   

   ケース４の場合：
   

   

   

   

   但し、上記式において、θ_１ｌｉｍを前記車輪の速度の上限値、θ_２ｌｉｍを前記プラットホームの姿勢角度の上限値、θ_３ｌｉｍを搭乗者の姿勢角度の上限値、Ｔ_ｍを前記トルク指令値、Ｔ_ｍｌｉｍを前記駆動手段のトルク上限値とする。
7. 請求項５又は６記載の倒立型移動体制御装置であって、
   前記判定手段により前記倒立型移動体の走行状態が前記ケース１、３、及び４のうちのいずれかに判定されたとき、前記指令演算手段は、下記式を用いて、前記指令値ｒを算出する、ことを特徴とする倒立型移動体制御装置。
   ケース１及び３の場合：
   

   

   ケース４の場合：
   

   

   但し、上記式において、θ_１ｒを前記車輪角度の指令値、θ_３ｒを搭乗者の姿勢角度の指令値、θ_３ｓを搭乗者が前記倒立型移動体に安全に乗車し続けることができる搭乗者の姿勢角加速度の上限値、Ｔを制御周期、ｋを時間のインデックス、とする。
8. 請求項１乃至７のうちいずれか１項記載の倒立型移動体制御装置であって、
   前記トルク指令演算手段は、下記式を用いて前記トルク指令値Ｔ_ｍを算出する、ことを特徴とする倒立型移動体制御装置。
   

   

   但し、上記式において、Ａを前記倒立型移動体の状態方程式のシステム行列、ｂを前記倒立型移動体の状態方程式の入力行列、Ｐを微分リカッチ方程式の解、δを安定度、Ｉを単位行列、ｋを制御ゲイン、ｒを前記指令演算手段により算出される指令値、θ_１ｒを前記指令入力手段からの車輪角度の指令値、θ_２ｒを前記指令入力手段からのプラットホームの角度指令値、θ_３ｒを前記指令入力手段からの搭乗者の姿勢角度の指令値、Ｑ、Ｒ及びＨを前記重み演算手段により算出される重み、Ｔを制御周期、とする。
9. 倒立型移動体の状態量を検出するステップと、
   搭乗者の操作情報が入力され、該操作情報に応じた操作指令値を算出するステップと、
   前記検出された倒立型移動体の状態量に基づいて、搭乗者の姿勢状態を推定するステップと、
   前記検出された倒立型移動体の状態量と、車輪を駆動する駆動手段に対するトルク指令値と、に基づいて、前記倒立型移動体の走行状態を判定するステップと、
   前記推定された搭乗者の姿勢状態と、前記判定された倒立型移動体の走行状態と、に基づいて、該走行状態毎に、前記状態量、前記姿勢状態及び前記トルク指令値を調整する際の制御パラメータの重みを算出するステップと、
   前記判定された倒立型移動体の走行状態と、前記算出された操作指令値と、に基づいて、前記判定された走行状態毎に、前記検出される状態量の目標値となる指令値を算出するステップと、
   前記算出された操作指令値と、前記算出された指令値と、前記算出された制御パラメータの重みと、に基づいて、前記トルク指令値を算出するステップと、
   を含む、ことを特徴とする倒立型移動体制御装置の制御方法。
10. 搭乗者の操作情報が入力され、該操作情報に応じた操作指令値を算出する処理と、
    検出された倒立型移動体の状態量に基づいて、搭乗者の姿勢状態を推定する処理と、
    前記検出された倒立型移動体の状態量と、車輪を駆動する駆動手段に対するトルク指令値と、に基づいて、前記倒立型移動体の走行状態を判定する処理と、
    前記推定された搭乗者の姿勢状態と、前記判定された倒立型移動体の走行状態と、に基づいて、該走行状態毎に、前記状態量、前記姿勢状態及び前記トルク指令値を調整する際の制御パラメータの重みを算出する処理と、
    前記判定された倒立型移動体の走行状態と、前記算出された操作指令値と、に基づいて、前記判定された走行状態毎に、前記検出される状態量の目標値となる指令値を算出する処理と、
    前記算出された操作指令値と、前記算出された指令値と、前記算出された制御パラメータの重みと、に基づいて、前記トルク指令値を算出する処理と、
    をコンピュータに実行させることを特徴とするプログラム。

Images