移動体制御装置、その制御方法及びプログラム
【課題】倒立型移動体の安全性を向上させる。
【解決手段】移動体制御装置１１０は、倒立型移動体の状態量を検出する状態検出手段１２３と、検出した状態量と駆動車輪４１０に生じる外乱トルク推定値と搭乗者の走行操作に応じ駆動車輪まわりに生じる操作トルク推定値と駆動車輪の駆動トルクとに基づき倒立型移動体の状態量及び搭乗者の姿勢角度のうち少なくとも１つを算出する状態推定手段１３１と、算出した状態量及び搭乗者の姿勢角度のうち少なくとも１つと駆動車輪の駆動トルクとに基づき外乱トルク推定値を算出する外乱トルク推定手段１３２と、算出した状態量及び搭乗者の姿勢角度のうち少なくとも１つに基づき操作トルク推定値を算出する操作トルク推定手段１３３と、外乱トルク推定値と操作トルク推定値と駆動車輪の駆動トルクとに基づき駆動車輪の外乱トルク及び操作トルクの組合せが不安定状態であるか否かを判定する状態判定手段と、を備える。
【解決手段】移動体制御装置１１０は、倒立型移動体の状態量を検出する状態検出手段１２３と、検出した状態量と駆動車輪４１０に生じる外乱トルク推定値と搭乗者の走行操作に応じ駆動車輪まわりに生じる操作トルク推定値と駆動車輪の駆動トルクとに基づき倒立型移動体の状態量及び搭乗者の姿勢角度のうち少なくとも１つを算出する状態推定手段１３１と、算出した状態量及び搭乗者の姿勢角度のうち少なくとも１つと駆動車輪の駆動トルクとに基づき外乱トルク推定値を算出する外乱トルク推定手段１３２と、算出した状態量及び搭乗者の姿勢角度のうち少なくとも１つに基づき操作トルク推定値を算出する操作トルク推定手段１３３と、外乱トルク推定値と操作トルク推定値と駆動車輪の駆動トルクとに基づき駆動車輪の外乱トルク及び操作トルクの組合せが不安定状態であるか否かを判定する状態判定手段と、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、搭乗者の走行操作に応じて倒立状態を維持しつつ所望の走行を行う倒立型移動体の走行を安定的に制御する移動体制御装置、その制御方法及びプログラムに関するものである。
【背景技術】
【０００２】
倒立状態を維持しつつ、搭乗者が搭乗するプラットホームの傾斜に応じて、所望の走行を行う倒立型移動体が知られている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００３−５０２００２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ここで、倒立状態を維持して走行する倒立型移動体は、例えば、凹凸路面を走行する場合に、その凹凸により駆動車輪に外乱トルクが生じるため走行が不安定となる。また、搭乗者によるプラットホームの傾斜操作により、その傾斜操作に応じて駆動車輪には操作トルクが生じている。
【０００５】
しかしながら、上記特許文献１に示す倒立型移動体においては、上記路面などからの外乱トルクと走行操作に応じた操作トルクとが組み合わさり不安定性が増す状態があり、そのような状態を、十分に考慮した安定的な制御を行っているとは言えず、安全性の面で問題が生じる虞がある。
【０００６】
本発明は、このような問題点を解決するためになされたものであり、倒立型移動体の安全性を向上させることができる移動体制御装置、その制御方法及びプログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００７】
上記目的を達成するための本発明の一態様は、倒立状態を維持して所望の走行を行う倒立型移動体を制御する移動体制御装置であって、前記倒立型移動体の状態量を検出する状態検出手段と、前記状態検出手段により検出された状態量と、前記倒立型移動体の駆動車輪に生じる外乱によるトルクの推定値である外乱トルク推定値と、搭乗者の走行操作に応じて前記駆動車輪まわりに生じるトルクの推定値である操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つを算出する状態推定手段と、前記状態推定手段により算出された前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つと、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記外乱トルク推定値を算出する外乱トルク推定手段と、前記状態推定手段により算出された前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つに基づいて、前記操作トルク推定値を算出する操作トルク推定手段と、前記外乱トルク推定手段により算出された前記外乱トルク推定値と、前記操作トルク推定手段により算出された前記操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記駆動車輪の外乱トルク及び操作トルクの組合せが前記倒立型移動体を不安定にする不安定状態であるか否かを判定する状態判定手段と、を備えることを特徴とする移動体制御装置である。
この一態様において、前記状態判定手段による判定結果と、前記状態推定手段により算出された前記倒立型移動体の状態量と、前記外乱トルク推定手段により算出された外乱トルク推定値と、に基づいて前記駆動車輪の駆動トルクを算出し、該算出した駆動トルクに基づいて前記駆動車輪の駆動を制御する制御手段を更に備えていてもよい。
この一態様において、前記制御手段は、前記状態判定手段により前記不安定状態であると判定されたとき、前記駆動車輪の駆動トルクがモーダル座標変換された前記状態量に制御ゲインを乗算した値から前記外乱トルク推定値を減算した値となるように、前記制御を行ってもよい。
この一態様において、前記状態判定手段により前記不安定状態であると判定されたとき、その旨をユーザに対して通知する警告手段を更に備えていてもよい。
この一態様において、前記状態判定手段は、モーダル座標変換された前記状態量と、前記駆動車輪を駆動する駆動トルクと、前記外乱トルク推定手段により算出された外乱トルク推定値と、前記操作トルク推定手段により算出された操作トルク推定値と、に基づいた値が所定閾値よりも大きいとき、前記不安定状態であると判定してもよい。
この一態様において、前記状態検出手段は、前記駆動車輪の回転情報、及び搭乗者が搭乗するプラットホームの姿勢角度情報のうち少なくとも一方を検出してもよい。
この一態様において、前記状態推定手段は、前記駆動車輪の駆動トルクと、前記外乱トルク推定手段により算出された外乱トルク推定値と、前記操作トルク推定手段により算出された操作トルク推定値と、前記状態量から該状態量の推定値を減算しゲインを乗算した演算結果と、に基づいて、前記倒立型移動体及び搭乗者の状態量を算出してもよい。
この一態様において、前記状態推定手段は、前記駆動車輪の角度、搭乗者が搭乗するプラットホームの姿勢角度、及び搭乗者の姿勢角度のうち少なくとも１つを算出してもよい。
この一態様において、前記状態推定手段は、下記（１２）式を用いて倒立型移動体及び搭乗者の状態量を推定してもよい。
この一態様において、前記外乱トルク推定手段は、下記（１３）式を用いて前記外乱トルク推定値を算出してもよい。
この一態様において、前記操作トルク推定手段は、下記（１４）式を用いて前記操作トルク推定値を算出してもよい。
この一態様において、前記状態判定手段は、下記（３１）式、（３２）式、（４０）式及び（４１）式のうちいずれか１つが成立したとき、前記不安定状態であると判定してもよい。
この一態様において、前記制御手段は、前記状態判定手段により倒立型移動体が前記不安定状態であると判定されたとき、前記駆動車輪の駆動トルクが下記（３５）式で算出されるトルクＴｍとなるように、前記制御を行ってもよい。
他方、上記目的を達成するための本発明の一態様は、倒立状態を維持して所望の走行を行う倒立型移動体を制御する移動体制御装置の制御方法であって、前記倒立型移動体の状態量を検出するステップと、前記検出された状態量と、前記倒立型移動体の駆動車輪に生じる外乱によるトルクの推定値である外乱トルク推定値と、搭乗者の走行操作に応じて前記駆動車輪まわりに生じるトルクの推定値である操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つを算出するステップと、前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つと、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記外乱トルク推定値を算出するステップと、前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つに基づいて、前記操作トルク推定値を算出するステップと、前記算出された外乱トルク推定値と、前記算出された操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記駆動車輪の外乱トルク及び操作トルクの組合せが前記倒立型移動体を不安定にする不安定状態であるか否かを判定するステップと、を含むことを特徴とする移動体制御装置の制御方法であってもよい。
この一態様において、前記判定結果と、前記検出された倒立型移動体の状態量と、前記算出された外乱トルク推定値と、に基づいて前記駆動車輪の駆動トルクを算出し、該算出した駆動トルクに基づいて前記駆動車輪の駆動を制御するステップを更に含んでいてもよい。
この一態様において、前記不安定状態であると判定されたとき、その旨をユーザに対して通知するステップを更に含んでいてもよい。
さらに、上記目的を達成するための本発明の一態様は、倒立状態を維持して所望の走行を行う倒立型移動体を制御する移動体制御装置のプログラムであって、検出された前記倒立型移動体の状態量と、前記倒立型移動体の駆動車輪に生じる外乱によるトルクの推定値である外乱トルク推定値と、搭乗者の走行操作に応じて前記駆動車輪まわりに生じるトルクの推定値である操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つを算出する処理と、前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つと、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記外乱トルク推定値を算出する処理と、前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つに基づいて、前記操作トルク推定値を算出する処理と、前記算出された外乱トルク推定値と、前記算出された操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記駆動車輪の外乱トルク及び操作トルクの組合せが前記倒立型移動体を不安定にする不安定状態であるか否かを判定する処理と、をコンピュータに実行させることを特徴とする移動体制御装置のプログラムであってもよい。
【発明の効果】
【０００８】
本発明によれば、倒立型移動体の安全性を向上させることができる移動体制御装置、その制御方法及びプログラムを提供することができる。
【図面の簡単な説明】
【０００９】
【図１】本発明の一実施の形態に係る移動体制御装置の概略的なシステム構成を示すブロック図である。
【図２】本発明の一実施の形態に係る移動体制御装置による制御処理フローの一例を示すフローチャートである。
【図３】本発明の一実施の形態に係る移動体制御装置のシミュレーションにおける搭乗者の姿勢角度の時間変化の一例を示す図である。
【図４】本発明の一実施の形態に係る移動体制御装置のシミュレーションにおける外乱トルクの時間変化の一例を示す図である。
【図５】本発明の一実施の形態に係る倒立型移動体の概略的構成を示す斜視図である。
【発明を実施するための形態】
【００１０】
以下、図面を参照して本発明の実施の形態について説明する。本発明の一実施の形態に係る移動体制御装置１００は、図５に示すような制御対象である倒立型移動体１２０の走行を安定的に制御する装置である。本実施の形態に係る倒立型移動体１２０は、搭乗者が搭乗するプラットホーム４２１と、プラットホーム４２１の両側に回転可能に設けられた左右一対の駆動車輪４１０と、一対の駆動車輪４１０を駆動する一対のモータ１２１（図１）と、プラットホーム４２１に揺動可能に設けられ搭乗者が把持し操作を行うハンドル４２２と、を備えている。
【００１１】
倒立型移動体１２０は、搭乗者による走行操作（例えば、プラットホーム４２１やハンドル４２２の傾斜操作、指令入力器（不図示）への操作情報（移動速度、移動方向など）の入力）に応じて、倒立状態を維持しつつ、所望の走行を行うように構成されている。例えば、プラットホーム４２１を前方へ傾斜させると倒立型移動体１２０が前進し、プラットホーム４２１を後方へ傾斜させると倒立型移動体１２０が後進するように構成されている。なお、本実施の形態において、上記倒立型移動体１２０の構成は一例である。例えば、倒立型移動体１２０は一対の駆動車輪４１０を有する構成であるが、これに限らず、一輪、又は３輪以上有する構成であってもよく任意の構成が適用可能である。
【００１２】
図１は、本実施の形態に係る移動体制御装置の概略的なシステム構成を示すブロック図である。本実施の形態に係る移動体制御装置１１０は、倒立型移動体１２０の状態量を検出する状態検出器１２３と、状態検出器１２３により検出された倒立型移動体１２０の状態量に基づいて、外乱トルク及び操作トルクの推定を行う推定装置１３０と、状態検出器１２３により検出された状態量と推定装置１３０により推定された外乱トルク推定値及び操作トルク推定値とに基づいて、各モータ１２１を制御する制御装置１４０と、を備えている。
【００１３】
各モータ１２１は、各駆動車輪４１０の駆動軸に減速機構などを介して連結されており、制御装置１４０からの制御信号（モータ電流）に応じて、各駆動車輪４１０を独立して回転駆動する。これにより、倒立型移動体１２０は、搭乗者の走行操作に応じて、倒立状態を維持しつつ、前後進、左右旋回、加減速、停止などの所望の走行を行うことができる。
【００１４】
状態検出器１２３は、状態検出手段の一具体例であり、各駆動車輪４１０の回転情報やプラットホーム４２１の姿勢角度情報など倒立型移動体１２０の状態量を検出する。状態検出器１２３は、例えば、各駆動車輪４１０に設けられた回転センサなどを用いて各駆動車輪４１０の回転角度、回転角速度、回転角加速度を検出し、プラットホーム４２１やハンドル４２２に設けられた角度センサなどを用いてプラットホーム４２１やハンドル４２２の姿勢角度を検出できる。
【００１５】
状態検出器１２３には推定装置１３０及び制御装置１４０が接続されており、検出した状態量を状態検出値として推定装置１３０及び制御装置１４０に対して出力する。
【００１６】
推定装置１３０は、状態検出器１２３からの倒立型移動体１２０の状態量と制御装置１４０からのモータトルク（駆動トルク）と、に基づいて、後述の外部環境によって各駆動車輪４１０に生じる外乱トルクや搭乗者の操作によって各駆動車輪４１０に生じる操作トルクを推定する。推定装置１３０は、状態推定器１３１と、外乱トルク推定器１３２と、操作トルク推定器１３３と、を有している。
【００１７】
状態推定器１３１は、状態推定手段の一具体例であり、状態検出器１２３からの状態検出値と、外乱トルク推定器１３２からの外乱トルク推定値と、操作トルク推定器１３３からの操作トルク推定値と、安定制御器１４２からの各モータ１２１のモータトルクと、に基づいて、例えば、搭乗者の姿勢角度など、状態検出器１２３で検出できない倒立型移動体１２０及び搭乗者の状態量の推定値を算出する。
【００１８】
状態推定器１３１には外乱トルク推定器１３２及び操作トルク推定器１３３が接続されており、算出した倒立型移動体１２０及び搭乗者の状態量の推定値を、状態推定値として外乱トルク推定器１３２及び操作トルク推定器１３３に対して出力する。
【００１９】
なお、状態推定器１３１は、予め設定されたパラメータ（例えば、後述の如く、外乱トルク推定値及び操作トルク推定値を算出するのに必要なパラメータ）のうち状態検出器１２３からの入力されない（状態検出器１２３が検出できない）パラメータ（例えば、搭乗者の姿勢角度）を自動的に算出し、それ以外は状態検出器１２３からの状態量を、夫々、状態推定値として出力してもよい。
【００２０】
外乱トルク推定器１３２は、外乱トルク推定手段の一具体例であり、状態推定器１３１からの状態推定値と、安定制御器１４２からの各モータ１２１のモータトルクと、に基づいて、倒立型移動体１２０の走行中に凹凸路面など外部環境からの入力により各駆動車輪４１０に生じる外乱トルクを算出する。外乱トルク推定器１３２には、制御装置１４０及び状態推定器１３１が接続されており、算出した外乱トルクを外乱トルク推定値として、制御装置１４０及び状態推定器１３１に対して出力する。
【００２１】
操作トルク推定器１３３は、操作トルク推定手段の一具体例であり、状態推定器１３１からの状態推定値に基づいて、搭乗者による走行操作に応じて各駆動車輪４１０まわりに生じる操作トルクを算出する。操作トルク推定器１３３には制御装置１４０及び状態推定器１３１が接続されており、算出した操作トルクを操作トルク推定値として、制御装置１４０及び状態推定器１３１に対して出力する。
【００２２】
制御装置１４０は、推定装置１３０からの倒立型移動体１２０の外乱トルク推定値及び操作トルク推定値と、状態推定器１３１からの状態推定値と、に基づいて、各モータ１２１を駆動制御する。制御装置１４０は、状態判定器１４１と、安定制御器１４２と、を有している。
【００２３】
状態判定器１４１は、状態判定手段の一具体例であり、推定装置１３０の外乱トルク推定器１３２からの外乱トルク推定値と、操作トルク推定器１３３からの操作トルク推定値と、安定制御器１４２からの各モータ１２１のモータトルクと、に基づいて、その外乱トルクと操作トルクとの組合せが倒立型移動体１２０を不安定にする不安定状態になっているか否かを判定する。状態判定器１４１には安定制御器１４２が接続されており、その判定結果を状態判定結果として安定制御器１４２に対して出力する。
【００２４】
なお、状態判定器１４１は不安定と判定したとき、例えば、警告装置（警告手段の一具体例）１４３などを用いて、ユーザに対して警告を行う。警告装置１４３としては、例えば、警告音を出力するスピーカ、警告灯を点灯／点滅させるライト、警告表示を行う表示装置、プラットホーム４２１やハンドル４２２などに警告振動を発生させる振動装置、第３者に無線など介して通知する通信装置、などが含まれる。
【００２５】
安定制御器１４２は、制御手段の一具体例であり、状態推定器１３１からの状態推定値と、状態判定器１４１からの状態判定結果と、外乱トルク推定器１３２からの外乱トルク推定値と、に基づいて、外乱トルクと操作トルクとの組合せが不安定状態になっていると判断した場合に、倒立型移動体１２０が安定走行を継続できるような各モータ１２１のトルクを算出し、算出したモータ１２１のモータトルクを状態推定器１３１、外乱トルク推定器１３２、及び状態判定器１４１に対して出力し、算出したモータ１２１のトルクに対応するモータ電流を、駆動回路などを介して各モータ１２１に対して出力する。各モータ１２１は、安全制御器１４２からのモータ電流に応じて、各駆動車輪４１０を回転駆動する。これにより、倒立型移動体１２０は、外乱トルクと操作トルクとの組合せが不安定状態になっている場合でも、安定的な走行を継続できる。
【００２６】
ここで、推定装置１３０および制御装置１４０は、例えば、ＡＳＩＣ（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）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【産業上の利用可能性】
【００８０】
本発明は、例えば、自動車、電動車椅子、航空機、船舶、鉄道、重機などの分野において、倒立状態を維持しつつ人の操作に応じて移動する倒立型移動体および、配管検査ロボット、遠隔手術ロボット、遠隔介護ロボットなど人が遠隔操作するサービスロボット全般に広く適用できる。
【符号の説明】
【００８１】
１１０ 移動体制御装置
１２０ 倒立型移動体
１２１ モータ
１２３ 状態検出器
１３０ 推定装置
１３１ 状態推定器
１３２ 外乱トルク推定器
１３３ 操作トルク推定器
１４０ 制御装置
１４１ 状態判定器
１４２ 安定制御器
４１０ 駆動車輪
４２１ プラットホーム
４２２ ハンドル
【技術分野】
【０００１】
本発明は、搭乗者の走行操作に応じて倒立状態を維持しつつ所望の走行を行う倒立型移動体の走行を安定的に制御する移動体制御装置、その制御方法及びプログラムに関するものである。
【背景技術】
【０００２】
倒立状態を維持しつつ、搭乗者が搭乗するプラットホームの傾斜に応じて、所望の走行を行う倒立型移動体が知られている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００３−５０２００２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ここで、倒立状態を維持して走行する倒立型移動体は、例えば、凹凸路面を走行する場合に、その凹凸により駆動車輪に外乱トルクが生じるため走行が不安定となる。また、搭乗者によるプラットホームの傾斜操作により、その傾斜操作に応じて駆動車輪には操作トルクが生じている。
【０００５】
しかしながら、上記特許文献１に示す倒立型移動体においては、上記路面などからの外乱トルクと走行操作に応じた操作トルクとが組み合わさり不安定性が増す状態があり、そのような状態を、十分に考慮した安定的な制御を行っているとは言えず、安全性の面で問題が生じる虞がある。
【０００６】
本発明は、このような問題点を解決するためになされたものであり、倒立型移動体の安全性を向上させることができる移動体制御装置、その制御方法及びプログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００７】
上記目的を達成するための本発明の一態様は、倒立状態を維持して所望の走行を行う倒立型移動体を制御する移動体制御装置であって、前記倒立型移動体の状態量を検出する状態検出手段と、前記状態検出手段により検出された状態量と、前記倒立型移動体の駆動車輪に生じる外乱によるトルクの推定値である外乱トルク推定値と、搭乗者の走行操作に応じて前記駆動車輪まわりに生じるトルクの推定値である操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つを算出する状態推定手段と、前記状態推定手段により算出された前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つと、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記外乱トルク推定値を算出する外乱トルク推定手段と、前記状態推定手段により算出された前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つに基づいて、前記操作トルク推定値を算出する操作トルク推定手段と、前記外乱トルク推定手段により算出された前記外乱トルク推定値と、前記操作トルク推定手段により算出された前記操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記駆動車輪の外乱トルク及び操作トルクの組合せが前記倒立型移動体を不安定にする不安定状態であるか否かを判定する状態判定手段と、を備えることを特徴とする移動体制御装置である。
この一態様において、前記状態判定手段による判定結果と、前記状態推定手段により算出された前記倒立型移動体の状態量と、前記外乱トルク推定手段により算出された外乱トルク推定値と、に基づいて前記駆動車輪の駆動トルクを算出し、該算出した駆動トルクに基づいて前記駆動車輪の駆動を制御する制御手段を更に備えていてもよい。
この一態様において、前記制御手段は、前記状態判定手段により前記不安定状態であると判定されたとき、前記駆動車輪の駆動トルクがモーダル座標変換された前記状態量に制御ゲインを乗算した値から前記外乱トルク推定値を減算した値となるように、前記制御を行ってもよい。
この一態様において、前記状態判定手段により前記不安定状態であると判定されたとき、その旨をユーザに対して通知する警告手段を更に備えていてもよい。
この一態様において、前記状態判定手段は、モーダル座標変換された前記状態量と、前記駆動車輪を駆動する駆動トルクと、前記外乱トルク推定手段により算出された外乱トルク推定値と、前記操作トルク推定手段により算出された操作トルク推定値と、に基づいた値が所定閾値よりも大きいとき、前記不安定状態であると判定してもよい。
この一態様において、前記状態検出手段は、前記駆動車輪の回転情報、及び搭乗者が搭乗するプラットホームの姿勢角度情報のうち少なくとも一方を検出してもよい。
この一態様において、前記状態推定手段は、前記駆動車輪の駆動トルクと、前記外乱トルク推定手段により算出された外乱トルク推定値と、前記操作トルク推定手段により算出された操作トルク推定値と、前記状態量から該状態量の推定値を減算しゲインを乗算した演算結果と、に基づいて、前記倒立型移動体及び搭乗者の状態量を算出してもよい。
この一態様において、前記状態推定手段は、前記駆動車輪の角度、搭乗者が搭乗するプラットホームの姿勢角度、及び搭乗者の姿勢角度のうち少なくとも１つを算出してもよい。
この一態様において、前記状態推定手段は、下記（１２）式を用いて倒立型移動体及び搭乗者の状態量を推定してもよい。
この一態様において、前記外乱トルク推定手段は、下記（１３）式を用いて前記外乱トルク推定値を算出してもよい。
この一態様において、前記操作トルク推定手段は、下記（１４）式を用いて前記操作トルク推定値を算出してもよい。
この一態様において、前記状態判定手段は、下記（３１）式、（３２）式、（４０）式及び（４１）式のうちいずれか１つが成立したとき、前記不安定状態であると判定してもよい。
この一態様において、前記制御手段は、前記状態判定手段により倒立型移動体が前記不安定状態であると判定されたとき、前記駆動車輪の駆動トルクが下記（３５）式で算出されるトルクＴｍとなるように、前記制御を行ってもよい。
他方、上記目的を達成するための本発明の一態様は、倒立状態を維持して所望の走行を行う倒立型移動体を制御する移動体制御装置の制御方法であって、前記倒立型移動体の状態量を検出するステップと、前記検出された状態量と、前記倒立型移動体の駆動車輪に生じる外乱によるトルクの推定値である外乱トルク推定値と、搭乗者の走行操作に応じて前記駆動車輪まわりに生じるトルクの推定値である操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つを算出するステップと、前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つと、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記外乱トルク推定値を算出するステップと、前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つに基づいて、前記操作トルク推定値を算出するステップと、前記算出された外乱トルク推定値と、前記算出された操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記駆動車輪の外乱トルク及び操作トルクの組合せが前記倒立型移動体を不安定にする不安定状態であるか否かを判定するステップと、を含むことを特徴とする移動体制御装置の制御方法であってもよい。
この一態様において、前記判定結果と、前記検出された倒立型移動体の状態量と、前記算出された外乱トルク推定値と、に基づいて前記駆動車輪の駆動トルクを算出し、該算出した駆動トルクに基づいて前記駆動車輪の駆動を制御するステップを更に含んでいてもよい。
この一態様において、前記不安定状態であると判定されたとき、その旨をユーザに対して通知するステップを更に含んでいてもよい。
さらに、上記目的を達成するための本発明の一態様は、倒立状態を維持して所望の走行を行う倒立型移動体を制御する移動体制御装置のプログラムであって、検出された前記倒立型移動体の状態量と、前記倒立型移動体の駆動車輪に生じる外乱によるトルクの推定値である外乱トルク推定値と、搭乗者の走行操作に応じて前記駆動車輪まわりに生じるトルクの推定値である操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つを算出する処理と、前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つと、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記外乱トルク推定値を算出する処理と、前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つに基づいて、前記操作トルク推定値を算出する処理と、前記算出された外乱トルク推定値と、前記算出された操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記駆動車輪の外乱トルク及び操作トルクの組合せが前記倒立型移動体を不安定にする不安定状態であるか否かを判定する処理と、をコンピュータに実行させることを特徴とする移動体制御装置のプログラムであってもよい。
【発明の効果】
【０００８】
本発明によれば、倒立型移動体の安全性を向上させることができる移動体制御装置、その制御方法及びプログラムを提供することができる。
【図面の簡単な説明】
【０００９】
【図１】本発明の一実施の形態に係る移動体制御装置の概略的なシステム構成を示すブロック図である。
【図２】本発明の一実施の形態に係る移動体制御装置による制御処理フローの一例を示すフローチャートである。
【図３】本発明の一実施の形態に係る移動体制御装置のシミュレーションにおける搭乗者の姿勢角度の時間変化の一例を示す図である。
【図４】本発明の一実施の形態に係る移動体制御装置のシミュレーションにおける外乱トルクの時間変化の一例を示す図である。
【図５】本発明の一実施の形態に係る倒立型移動体の概略的構成を示す斜視図である。
【発明を実施するための形態】
【００１０】
以下、図面を参照して本発明の実施の形態について説明する。本発明の一実施の形態に係る移動体制御装置１００は、図５に示すような制御対象である倒立型移動体１２０の走行を安定的に制御する装置である。本実施の形態に係る倒立型移動体１２０は、搭乗者が搭乗するプラットホーム４２１と、プラットホーム４２１の両側に回転可能に設けられた左右一対の駆動車輪４１０と、一対の駆動車輪４１０を駆動する一対のモータ１２１（図１）と、プラットホーム４２１に揺動可能に設けられ搭乗者が把持し操作を行うハンドル４２２と、を備えている。
【００１１】
倒立型移動体１２０は、搭乗者による走行操作（例えば、プラットホーム４２１やハンドル４２２の傾斜操作、指令入力器（不図示）への操作情報（移動速度、移動方向など）の入力）に応じて、倒立状態を維持しつつ、所望の走行を行うように構成されている。例えば、プラットホーム４２１を前方へ傾斜させると倒立型移動体１２０が前進し、プラットホーム４２１を後方へ傾斜させると倒立型移動体１２０が後進するように構成されている。なお、本実施の形態において、上記倒立型移動体１２０の構成は一例である。例えば、倒立型移動体１２０は一対の駆動車輪４１０を有する構成であるが、これに限らず、一輪、又は３輪以上有する構成であってもよく任意の構成が適用可能である。
【００１２】
図１は、本実施の形態に係る移動体制御装置の概略的なシステム構成を示すブロック図である。本実施の形態に係る移動体制御装置１１０は、倒立型移動体１２０の状態量を検出する状態検出器１２３と、状態検出器１２３により検出された倒立型移動体１２０の状態量に基づいて、外乱トルク及び操作トルクの推定を行う推定装置１３０と、状態検出器１２３により検出された状態量と推定装置１３０により推定された外乱トルク推定値及び操作トルク推定値とに基づいて、各モータ１２１を制御する制御装置１４０と、を備えている。
【００１３】
各モータ１２１は、各駆動車輪４１０の駆動軸に減速機構などを介して連結されており、制御装置１４０からの制御信号（モータ電流）に応じて、各駆動車輪４１０を独立して回転駆動する。これにより、倒立型移動体１２０は、搭乗者の走行操作に応じて、倒立状態を維持しつつ、前後進、左右旋回、加減速、停止などの所望の走行を行うことができる。
【００１４】
状態検出器１２３は、状態検出手段の一具体例であり、各駆動車輪４１０の回転情報やプラットホーム４２１の姿勢角度情報など倒立型移動体１２０の状態量を検出する。状態検出器１２３は、例えば、各駆動車輪４１０に設けられた回転センサなどを用いて各駆動車輪４１０の回転角度、回転角速度、回転角加速度を検出し、プラットホーム４２１やハンドル４２２に設けられた角度センサなどを用いてプラットホーム４２１やハンドル４２２の姿勢角度を検出できる。
【００１５】
状態検出器１２３には推定装置１３０及び制御装置１４０が接続されており、検出した状態量を状態検出値として推定装置１３０及び制御装置１４０に対して出力する。
【００１６】
推定装置１３０は、状態検出器１２３からの倒立型移動体１２０の状態量と制御装置１４０からのモータトルク（駆動トルク）と、に基づいて、後述の外部環境によって各駆動車輪４１０に生じる外乱トルクや搭乗者の操作によって各駆動車輪４１０に生じる操作トルクを推定する。推定装置１３０は、状態推定器１３１と、外乱トルク推定器１３２と、操作トルク推定器１３３と、を有している。
【００１７】
状態推定器１３１は、状態推定手段の一具体例であり、状態検出器１２３からの状態検出値と、外乱トルク推定器１３２からの外乱トルク推定値と、操作トルク推定器１３３からの操作トルク推定値と、安定制御器１４２からの各モータ１２１のモータトルクと、に基づいて、例えば、搭乗者の姿勢角度など、状態検出器１２３で検出できない倒立型移動体１２０及び搭乗者の状態量の推定値を算出する。
【００１８】
状態推定器１３１には外乱トルク推定器１３２及び操作トルク推定器１３３が接続されており、算出した倒立型移動体１２０及び搭乗者の状態量の推定値を、状態推定値として外乱トルク推定器１３２及び操作トルク推定器１３３に対して出力する。
【００１９】
なお、状態推定器１３１は、予め設定されたパラメータ（例えば、後述の如く、外乱トルク推定値及び操作トルク推定値を算出するのに必要なパラメータ）のうち状態検出器１２３からの入力されない（状態検出器１２３が検出できない）パラメータ（例えば、搭乗者の姿勢角度）を自動的に算出し、それ以外は状態検出器１２３からの状態量を、夫々、状態推定値として出力してもよい。
【００２０】
外乱トルク推定器１３２は、外乱トルク推定手段の一具体例であり、状態推定器１３１からの状態推定値と、安定制御器１４２からの各モータ１２１のモータトルクと、に基づいて、倒立型移動体１２０の走行中に凹凸路面など外部環境からの入力により各駆動車輪４１０に生じる外乱トルクを算出する。外乱トルク推定器１３２には、制御装置１４０及び状態推定器１３１が接続されており、算出した外乱トルクを外乱トルク推定値として、制御装置１４０及び状態推定器１３１に対して出力する。
【００２１】
操作トルク推定器１３３は、操作トルク推定手段の一具体例であり、状態推定器１３１からの状態推定値に基づいて、搭乗者による走行操作に応じて各駆動車輪４１０まわりに生じる操作トルクを算出する。操作トルク推定器１３３には制御装置１４０及び状態推定器１３１が接続されており、算出した操作トルクを操作トルク推定値として、制御装置１４０及び状態推定器１３１に対して出力する。
【００２２】
制御装置１４０は、推定装置１３０からの倒立型移動体１２０の外乱トルク推定値及び操作トルク推定値と、状態推定器１３１からの状態推定値と、に基づいて、各モータ１２１を駆動制御する。制御装置１４０は、状態判定器１４１と、安定制御器１４２と、を有している。
【００２３】
状態判定器１４１は、状態判定手段の一具体例であり、推定装置１３０の外乱トルク推定器１３２からの外乱トルク推定値と、操作トルク推定器１３３からの操作トルク推定値と、安定制御器１４２からの各モータ１２１のモータトルクと、に基づいて、その外乱トルクと操作トルクとの組合せが倒立型移動体１２０を不安定にする不安定状態になっているか否かを判定する。状態判定器１４１には安定制御器１４２が接続されており、その判定結果を状態判定結果として安定制御器１４２に対して出力する。
【００２４】
なお、状態判定器１４１は不安定と判定したとき、例えば、警告装置（警告手段の一具体例）１４３などを用いて、ユーザに対して警告を行う。警告装置１４３としては、例えば、警告音を出力するスピーカ、警告灯を点灯／点滅させるライト、警告表示を行う表示装置、プラットホーム４２１やハンドル４２２などに警告振動を発生させる振動装置、第３者に無線など介して通知する通信装置、などが含まれる。
【００２５】
安定制御器１４２は、制御手段の一具体例であり、状態推定器１３１からの状態推定値と、状態判定器１４１からの状態判定結果と、外乱トルク推定器１３２からの外乱トルク推定値と、に基づいて、外乱トルクと操作トルクとの組合せが不安定状態になっていると判断した場合に、倒立型移動体１２０が安定走行を継続できるような各モータ１２１のトルクを算出し、算出したモータ１２１のモータトルクを状態推定器１３１、外乱トルク推定器１３２、及び状態判定器１４１に対して出力し、算出したモータ１２１のトルクに対応するモータ電流を、駆動回路などを介して各モータ１２１に対して出力する。各モータ１２１は、安全制御器１４２からのモータ電流に応じて、各駆動車輪４１０を回転駆動する。これにより、倒立型移動体１２０は、外乱トルクと操作トルクとの組合せが不安定状態になっている場合でも、安定的な走行を継続できる。
【００２６】
ここで、推定装置１３０および制御装置１４０は、例えば、ＡＳＩＣ（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）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【産業上の利用可能性】
【００８０】
本発明は、例えば、自動車、電動車椅子、航空機、船舶、鉄道、重機などの分野において、倒立状態を維持しつつ人の操作に応じて移動する倒立型移動体および、配管検査ロボット、遠隔手術ロボット、遠隔介護ロボットなど人が遠隔操作するサービスロボット全般に広く適用できる。
【符号の説明】
【００８１】
１１０ 移動体制御装置
１２０ 倒立型移動体
１２１ モータ
１２３ 状態検出器
１３０ 推定装置
１３１ 状態推定器
１３２ 外乱トルク推定器
１３３ 操作トルク推定器
１４０ 制御装置
１４１ 状態判定器
１４２ 安定制御器
４１０ 駆動車輪
４２１ プラットホーム
４２２ ハンドル
【特許請求の範囲】
【請求項１】
倒立状態を維持して所望の走行を行う倒立型移動体を制御する移動体制御装置であって、
前記倒立型移動体の状態量を検出する状態検出手段と、
前記状態検出手段により検出された状態量と、前記倒立型移動体の駆動車輪に生じる外乱によるトルクの推定値である外乱トルク推定値と、搭乗者の走行操作に応じて前記駆動車輪まわりに生じるトルクの推定値である操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つを算出する状態推定手段と、
前記状態推定手段により算出された前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つと、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記外乱トルク推定値を算出する外乱トルク推定手段と、
前記状態推定手段により算出された前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つに基づいて、前記操作トルク推定値を算出する操作トルク推定手段と、
前記外乱トルク推定手段により算出された前記外乱トルク推定値と、前記操作トルク推定手段により算出された前記操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記駆動車輪の外乱トルク及び操作トルクの組合せが前記倒立型移動体を不安定にする不安定状態であるか否かを判定する状態判定手段と、
を備えることを特徴とする移動体制御装置。
【請求項２】
請求項１記載の移動体制御装置であって、
前記状態判定手段による判定結果と、前記状態推定手段により算出された前記倒立型移動体の状態量と、前記外乱トルク推定手段により算出された外乱トルク推定値と、に基づいて前記駆動車輪の駆動トルクを算出し、該算出した駆動トルクに基づいて前記駆動車輪の駆動を制御する制御手段を更に備える、ことを特徴とする移動体制御装置。
【請求項３】
請求項２記載の移動体制御装置であって、
前記制御手段は、前記状態判定手段により前記不安定状態であると判定されたとき、前記駆動車輪の駆動トルクがモーダル座標変換された前記状態量に制御ゲインを乗算した値から前記外乱トルク推定値を減算した値となるように、前記制御を行う、ことを特徴とする移動体制御装置。
【請求項４】
請求項１乃至３のうちいずれか１項記載の移動体制御装置であって、
前記状態判定手段により前記不安定状態であると判定されたとき、その旨をユーザに対して通知する警告手段を更に備える、ことを特徴とする移動体制御装置。
【請求項５】
請求項１乃至４のうちいずれか１項記載の移動体制御装置であって、
前記状態判定手段は、モーダル座標変換された前記状態量と、前記駆動車輪を駆動する駆動トルクと、前記外乱トルク推定手段により算出された外乱トルク推定値と、前記操作トルク推定手段により算出された操作トルク推定値と、に基づいた値が所定閾値よりも大きいとき、前記不安定状態であると判定する、ことを特徴とする移動体制御装置。
【請求項６】
請求項１乃至５のうちいずれか１項記載の移動体制御装置であって、
前記状態検出手段は、前記駆動車輪の回転情報、及び搭乗者が搭乗するプラットホームの姿勢角度情報のうち少なくとも一方を検出する、ことを特徴とする移動体制御装置。
【請求項７】
請求項１乃至６のうちいずれか１項記載の移動体制御装置であって、
前記状態推定手段は、前記駆動車輪の駆動トルクと、前記外乱トルク推定手段により算出された外乱トルク推定値と、前記操作トルク推定手段により算出された操作トルク推定値と、前記状態量から該状態量の推定値を減算しゲインを乗算した演算結果と、に基づいて、前記倒立型移動体及び搭乗者の状態量を算出する、ことを特徴とする移動体制御装置。
【請求項８】
請求項１乃至７のうちいずれか１項記載の移動体制御装置であって、
前記状態推定手段は、前記駆動車輪の角度、搭乗者が搭乗するプラットホームの姿勢角度、及び搭乗者の姿勢角度のうち少なくとも１つを算出する、ことを特徴とする移動体制御装置。
【請求項９】
請求項１乃至８のうちいずれか１項記載の移動体制御装置であって、
前記状態推定手段は、下記式を用いて倒立型移動体及び搭乗者の状態量を推定する、ことを特徴とする移動体制御装置。
【数１】
【請求項１０】
請求項１乃至９のうちいずれか１項記載の移動体制御装置であって、
前記外乱トルク推定手段は、下記式を用いて前記外乱トルク推定値を算出する、ことを特徴とする移動体制御装置。
【数２】
【数３】
ただし、上記式において、Ａは前記倒立型移動体の状態方程式のシステム行列、Ｌは前記状態推定手段のゲイン行列、Ｑは正定値行列とする。
【請求項１１】
請求項１乃至１０のうちいずれか１項記載の移動体制御装置であって、
前記操作トルク推定手段は、下記式を用いて前記操作トルク推定値を算出する、ことを特徴とする移動体制御装置。
【数４】
【請求項１２】
請求項１乃至１１のうちいずれか１項記載の移動体制御装置であって、
前記状態判定手段は、下記式のうちいずれか１つが成立したとき、前記不安定状態であると判定する、ことを特徴とする移動体制御装置。
【数５】
【請求項１３】
請求項１乃至１２のうちいずれか１項記載の移動体制御装置であって、
前記制御手段は、前記状態判定手段により倒立型移動体が前記不安定状態であると判定されたとき、前記駆動車輪の駆動トルクが下記式で算出されるトルクＴｍとなるように、前記制御を行う、ことを特徴とする移動体制御装置。
【数６】
【請求項１４】
倒立状態を維持して所望の走行を行う倒立型移動体を制御する移動体制御装置の制御方法であって、
前記倒立型移動体の状態量を検出するステップと、
前記検出された状態量と、前記倒立型移動体の駆動車輪に生じる外乱によるトルクの推定値である外乱トルク推定値と、搭乗者の走行操作に応じて前記駆動車輪まわりに生じるトルクの推定値である操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つを算出するステップと、
前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つと、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記外乱トルク推定値を算出するステップと、
前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つに基づいて、前記操作トルク推定値を算出するステップと、
前記算出された外乱トルク推定値と、前記算出された操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記駆動車輪の外乱トルク及び操作トルクの組合せが前記倒立型移動体を不安定にする不安定状態であるか否かを判定するステップと、
を含むことを特徴とする移動体制御装置の制御方法。
【請求項１５】
請求項１４記載の移動体制御装置の制御方法であって、
前記判定結果と、前記算出された倒立型移動体の状態量と、前記算出された外乱トルク推定値と、に基づいて前記駆動車輪の駆動トルクを算出し、該算出した駆動トルクに基づいて前記駆動車輪の駆動を制御するステップを更に含む、ことを特徴とする移動体制御装置の制御方法。
【請求項１６】
請求項１４又は１５記載の移動体制御装置の制御方法であって、
前記不安定状態であると判定されたとき、その旨をユーザに対して通知するステップを更に含む、ことを特徴とする移動体制御装置の制御方法。
【請求項１７】
倒立状態を維持して所望の走行を行う倒立型移動体を制御する移動体制御装置のプログラムであって、
検出された前記倒立型移動体の状態量と、前記倒立型移動体の駆動車輪に生じる外乱によるトルクの推定値である外乱トルク推定値と、搭乗者の走行操作に応じて前記駆動車輪まわりに生じるトルクの推定値である操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つを算出する処理と、
前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つと、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記外乱トルク推定値を算出する処理と、
前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つに基づいて、前記操作トルク推定値を算出する処理と、
前記算出された外乱トルク推定値と、前記算出された操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記駆動車輪の外乱トルク及び操作トルクの組合せが前記倒立型移動体を不安定にする不安定状態であるか否かを判定する処理と、
をコンピュータに実行させることを特徴とする移動体制御装置のプログラム。
【請求項１】
倒立状態を維持して所望の走行を行う倒立型移動体を制御する移動体制御装置であって、
前記倒立型移動体の状態量を検出する状態検出手段と、
前記状態検出手段により検出された状態量と、前記倒立型移動体の駆動車輪に生じる外乱によるトルクの推定値である外乱トルク推定値と、搭乗者の走行操作に応じて前記駆動車輪まわりに生じるトルクの推定値である操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つを算出する状態推定手段と、
前記状態推定手段により算出された前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つと、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記外乱トルク推定値を算出する外乱トルク推定手段と、
前記状態推定手段により算出された前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つに基づいて、前記操作トルク推定値を算出する操作トルク推定手段と、
前記外乱トルク推定手段により算出された前記外乱トルク推定値と、前記操作トルク推定手段により算出された前記操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記駆動車輪の外乱トルク及び操作トルクの組合せが前記倒立型移動体を不安定にする不安定状態であるか否かを判定する状態判定手段と、
を備えることを特徴とする移動体制御装置。
【請求項２】
請求項１記載の移動体制御装置であって、
前記状態判定手段による判定結果と、前記状態推定手段により算出された前記倒立型移動体の状態量と、前記外乱トルク推定手段により算出された外乱トルク推定値と、に基づいて前記駆動車輪の駆動トルクを算出し、該算出した駆動トルクに基づいて前記駆動車輪の駆動を制御する制御手段を更に備える、ことを特徴とする移動体制御装置。
【請求項３】
請求項２記載の移動体制御装置であって、
前記制御手段は、前記状態判定手段により前記不安定状態であると判定されたとき、前記駆動車輪の駆動トルクがモーダル座標変換された前記状態量に制御ゲインを乗算した値から前記外乱トルク推定値を減算した値となるように、前記制御を行う、ことを特徴とする移動体制御装置。
【請求項４】
請求項１乃至３のうちいずれか１項記載の移動体制御装置であって、
前記状態判定手段により前記不安定状態であると判定されたとき、その旨をユーザに対して通知する警告手段を更に備える、ことを特徴とする移動体制御装置。
【請求項５】
請求項１乃至４のうちいずれか１項記載の移動体制御装置であって、
前記状態判定手段は、モーダル座標変換された前記状態量と、前記駆動車輪を駆動する駆動トルクと、前記外乱トルク推定手段により算出された外乱トルク推定値と、前記操作トルク推定手段により算出された操作トルク推定値と、に基づいた値が所定閾値よりも大きいとき、前記不安定状態であると判定する、ことを特徴とする移動体制御装置。
【請求項６】
請求項１乃至５のうちいずれか１項記載の移動体制御装置であって、
前記状態検出手段は、前記駆動車輪の回転情報、及び搭乗者が搭乗するプラットホームの姿勢角度情報のうち少なくとも一方を検出する、ことを特徴とする移動体制御装置。
【請求項７】
請求項１乃至６のうちいずれか１項記載の移動体制御装置であって、
前記状態推定手段は、前記駆動車輪の駆動トルクと、前記外乱トルク推定手段により算出された外乱トルク推定値と、前記操作トルク推定手段により算出された操作トルク推定値と、前記状態量から該状態量の推定値を減算しゲインを乗算した演算結果と、に基づいて、前記倒立型移動体及び搭乗者の状態量を算出する、ことを特徴とする移動体制御装置。
【請求項８】
請求項１乃至７のうちいずれか１項記載の移動体制御装置であって、
前記状態推定手段は、前記駆動車輪の角度、搭乗者が搭乗するプラットホームの姿勢角度、及び搭乗者の姿勢角度のうち少なくとも１つを算出する、ことを特徴とする移動体制御装置。
【請求項９】
請求項１乃至８のうちいずれか１項記載の移動体制御装置であって、
前記状態推定手段は、下記式を用いて倒立型移動体及び搭乗者の状態量を推定する、ことを特徴とする移動体制御装置。
【数１】
【請求項１０】
請求項１乃至９のうちいずれか１項記載の移動体制御装置であって、
前記外乱トルク推定手段は、下記式を用いて前記外乱トルク推定値を算出する、ことを特徴とする移動体制御装置。
【数２】
【数３】
ただし、上記式において、Ａは前記倒立型移動体の状態方程式のシステム行列、Ｌは前記状態推定手段のゲイン行列、Ｑは正定値行列とする。
【請求項１１】
請求項１乃至１０のうちいずれか１項記載の移動体制御装置であって、
前記操作トルク推定手段は、下記式を用いて前記操作トルク推定値を算出する、ことを特徴とする移動体制御装置。
【数４】
【請求項１２】
請求項１乃至１１のうちいずれか１項記載の移動体制御装置であって、
前記状態判定手段は、下記式のうちいずれか１つが成立したとき、前記不安定状態であると判定する、ことを特徴とする移動体制御装置。
【数５】
【請求項１３】
請求項１乃至１２のうちいずれか１項記載の移動体制御装置であって、
前記制御手段は、前記状態判定手段により倒立型移動体が前記不安定状態であると判定されたとき、前記駆動車輪の駆動トルクが下記式で算出されるトルクＴｍとなるように、前記制御を行う、ことを特徴とする移動体制御装置。
【数６】
【請求項１４】
倒立状態を維持して所望の走行を行う倒立型移動体を制御する移動体制御装置の制御方法であって、
前記倒立型移動体の状態量を検出するステップと、
前記検出された状態量と、前記倒立型移動体の駆動車輪に生じる外乱によるトルクの推定値である外乱トルク推定値と、搭乗者の走行操作に応じて前記駆動車輪まわりに生じるトルクの推定値である操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つを算出するステップと、
前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つと、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記外乱トルク推定値を算出するステップと、
前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つに基づいて、前記操作トルク推定値を算出するステップと、
前記算出された外乱トルク推定値と、前記算出された操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記駆動車輪の外乱トルク及び操作トルクの組合せが前記倒立型移動体を不安定にする不安定状態であるか否かを判定するステップと、
を含むことを特徴とする移動体制御装置の制御方法。
【請求項１５】
請求項１４記載の移動体制御装置の制御方法であって、
前記判定結果と、前記算出された倒立型移動体の状態量と、前記算出された外乱トルク推定値と、に基づいて前記駆動車輪の駆動トルクを算出し、該算出した駆動トルクに基づいて前記駆動車輪の駆動を制御するステップを更に含む、ことを特徴とする移動体制御装置の制御方法。
【請求項１６】
請求項１４又は１５記載の移動体制御装置の制御方法であって、
前記不安定状態であると判定されたとき、その旨をユーザに対して通知するステップを更に含む、ことを特徴とする移動体制御装置の制御方法。
【請求項１７】
倒立状態を維持して所望の走行を行う倒立型移動体を制御する移動体制御装置のプログラムであって、
検出された前記倒立型移動体の状態量と、前記倒立型移動体の駆動車輪に生じる外乱によるトルクの推定値である外乱トルク推定値と、搭乗者の走行操作に応じて前記駆動車輪まわりに生じるトルクの推定値である操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記倒立型移動体及び搭乗者の状態量のうち少なくとも１つを算出する処理と、
前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つと、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記外乱トルク推定値を算出する処理と、
前記算出された倒立型移動体及び搭乗者の状態量のうち少なくとも１つに基づいて、前記操作トルク推定値を算出する処理と、
前記算出された外乱トルク推定値と、前記算出された操作トルク推定値と、前記駆動車輪を駆動する駆動トルクと、に基づいて、前記駆動車輪の外乱トルク及び操作トルクの組合せが前記倒立型移動体を不安定にする不安定状態であるか否かを判定する処理と、
をコンピュータに実行させることを特徴とする移動体制御装置のプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１３−８６５５７（Ｐ２０１３−８６５５７Ａ）
【公開日】平成２５年５月１３日（２０１３．５．１３）
【国際特許分類】
【出願番号】特願２０１１−２２６１５２（Ｐ２０１１−２２６１５２）
【出願日】平成２３年１０月１３日（２０１１．１０．１３）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２５年５月１３日（２０１３．５．１３）
【国際特許分類】
【出願日】平成２３年１０月１３日（２０１１．１０．１３）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]