(57)【要約】      （修正有）【課題】コスト低減をしつつ高い安全性が実現できる移動体制御装置、その制御方法、及び、制御プログラムを提供する。【解決手段】移動体制御装置１０は、回転検出手段１６により検出された各駆動車輪の回転情報に基いて駆動系に内在する摩擦及び路面と各駆動車輪との間に生じる各駆動車輪の摩擦トルクを算出する摩擦トルク算出手段１８と、各駆動車輪の慣性トルクと駆動トルク算出手段１７により算出された駆動トルクと摩擦トルク算出手段により算出された摩擦トルクと各駆動車輪の運動方程式とに基いて移動体に付加された外力により駆動車輪に生じる外力トルクを算出し、算出した駆動車輪の外力トルクと駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式とを用いて移動体の重心位置に作用する外力を算出する外力算出手段１９と、算出された外力に応じて移動体の移動を制御する移動体制御手段１１と、を備える。【選択図】図３
2014044625.tif 000002
【特許請求の範囲】
【請求項１】
複数の駆動車輪を駆動させて移動を行う移動体を制御する移動体制御装置であって、
前記各駆動車輪の回転情報を検出する回転検出手段と、
前記各駆動車輪を駆動するための駆動トルクを算出する駆動トルク算出手段と、
前記回転検出手段により検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出する摩擦トルク算出手段と、
前記各駆動車輪の慣性トルクと、前記駆動トルク算出手段により算出された駆動トルクと、前記摩擦トルク算出手段により算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力により前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出する外力算出手段と、
前記外力算出手段により算出された外力に応じて、前記移動体の移動を制御する移動体制御手段と、
を備えることを特徴とする移動体制御装置。
【請求項２】
請求項１記載の移動体制御装置であって、
前記外力算出手段は、前記駆動車輪の外力トルクと、移動体の重心位置に作用する外力との関係を示すヤコビ転置行列の疑似逆行列と、を用いて前記移動体の重心位置に作用する外力を算出することを特徴とする移動体制御装置。
【請求項３】
請求項２記載の移動体制御装置であって、
前記外力算出手段は、下記式を用いて前記移動体の重心位置に作用する外力ｆＢハットを算出することを特徴とする移動体制御装置。
【数１】
2014044625.tif 000010
但し、上記式において、ｒは前記各駆動車輪の半径であり、τｅハットは前記駆動車輪の外力トルクであり、一対の駆動車輪間の距離を２ｌとする。
【請求項４】
請求項１乃至３のうちいずれか１項記載の移動体制御装置であって、
前記摩擦トルク算出手段は、前記回転検出手段により検出された前記各駆動車輪の回転角速度に基いて、前記各駆動車輪の粘性摩擦トルク及びクーロン摩擦トルクを夫々算出し、該算出した各駆動車輪の粘性摩擦トルク及びクーロン摩擦トルクを加算して前記各駆動車輪の摩擦トルクを算出することを特徴とする移動体制御装置。
【請求項５】
請求項１乃至４のうちいずれか１項記載の移動体制御装置であって、
前記移動体の傾斜角を検出する傾斜角検出手段を更に備え、
前記外力算出手段は、前記傾斜角検出手段により検出された移動体の傾斜角に基いて、前記移動体に作用する斜面方向の重力成分を算出し、前記算出した移動体の重心位置に作用する外力から前記算出した重力成分を減算することを特徴とする移動体制御装置。
【請求項６】
請求項１乃至５のうちいずれか１項記載の移動体制御装置であって、
前記移動体は一対の駆動車輪を駆動する一対の駆動手段を備えており、
前記移動体制御手段は、前記前記外力算出手段により算出された外力の増加を抑制するように前記各駆動手段を制御することを特徴とする移動体制御装置。
【請求項７】
複数の駆動車輪を駆動させて移動を行う移動体を制御する移動体制御装置の制御方法であって、
前記各駆動車輪の回転情報を検出するステップと、
前記各駆動車輪を駆動するための駆動トルクを算出するステップと、
前記検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出するステップと、
前記各駆動車輪の慣性トルクと、前記算出された駆動トルクと、前記算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力によりに前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出するステップと、
前記算出された外力に応じて、前記移動体の移動を制御するステップと、
を含むことを特徴とする移動体制御装置の制御方法。
【請求項８】
複数の駆動車輪を駆動させて移動を行う移動体を制御する移動体制御装置の制御プログラムであって、
前記各駆動車輪の回転情報を検出する処理と、
前記各駆動車輪を駆動するための駆動トルクを算出する処理と、
前記検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出する処理と、
前記各駆動車輪の慣性トルクと、前記算出された駆動トルクと、前記算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力により前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出する処理と、
前記算出された外力に応じて、前記移動体の移動を制御する処理と、
を含むことを特徴とする移動体制御装置の制御プログラム。
【請求項９】
移動体に設けられた複数の駆動車輪の回転情報を検出する回転検出手段と、
前記各駆動車輪を駆動するための駆動トルクを算出する駆動トルク算出手段と、
前記回転検出手段により検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出する摩擦トルク算出手段と、
前記各駆動車輪の慣性トルクと、前記駆動トルク算出手段により算出された駆動トルクと、前記摩擦トルク算出手段により算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力により前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出する外力算出手段と、
を備えることを特徴とする外力推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体に作用する外力を推定し、移動体を最適に制御する移動体制御装置、その制御方法、及び制御プログラムに関するものである。
【背景技術】
【０００２】
近年、環境内に存在する障害物等を回避しつつ、自律的に移動する移動体の開発が行われている。このような移動体においては、仮に、障害物などに接触した場合にその接触により生じる外力を推定し、その衝撃を緩和するなどの高い安全性の制御を行うことが望まれる。
【０００３】
例えば、トルクセンサなどによる人の入力と環境からの外力に対して、夫々コンプライアンス制御を行う走行車の制御方法が知られている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００４−０４９５２３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、上記特許文献１に示す制御方法においては、例えば、環境内に存在する障害物等と接触したときに生じる外力を推定するための、複数のセンサなどが必要となりコスト増加に繋がる虞がある。
【０００６】
本発明は、このような問題点を解決するためになされたものであり、コスト低減をしつつ高い安全性が実現できる移動体制御装置、その制御方法、及び制御プログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００７】
上記目的を達成するための本発明の一態様は、複数の駆動車輪を駆動させて移動を行う移動体を制御する移動体制御装置であって、前記各駆動車輪の回転情報を検出する回転検出手段と、前記各駆動車輪を駆動するための駆動トルクを算出する駆動トルク算出手段と、前記回転検出手段により検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出する摩擦トルク算出手段と、前記各駆動車輪の慣性トルクと、前記駆動トルク算出手段により算出された駆動トルクと、前記摩擦トルク算出手段により算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力により前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出する外力算出手段と、前記外力算出手段により算出された外力に応じて、前記移動体の移動を制御する移動体制御手段と、を備えることを特徴とする移動体制御装置である。
この一態様において、前記外力算出手段は、前記駆動車輪の外力トルクと、移動体の重心位置に作用する外力との関係を示すヤコビ転置行列の疑似逆行列と、を用いて前記移動体の重心位置に作用する外力を算出することを特徴とする移動体制御装置。
この一態様において、前記外力算出手段は、下記（５）式を用いて前記移動体の重心位置に作用する外力ｆＢハットを算出してもよい。但し、上記式において、ｒは前記各駆動車輪の半径であり、τｅハットは前記駆動車輪の外力トルクであり、一対の駆動車輪間の距離を２ｌとする。
この一態様において、前記摩擦トルク算出手段は、前記回転検出手段により検出された前記各駆動車輪の回転角速度に基いて、前記各駆動車輪の粘性摩擦トルク及びクーロン摩擦トルクを夫々算出し、該算出した各駆動車輪の粘性摩擦トルク及びクーロン摩擦トルクを加算して前記各駆動車輪の摩擦トルクを算出してもよい。
この一態様において、前記移動体の傾斜角を検出する傾斜角検出手段を更に備え、前記外力算出手段は、前記傾斜角検出手段により検出された移動体の傾斜角に基いて、前記移動体に作用する斜面方向の重力成分を算出し、前記算出した移動体の重心位置に作用する外力から前記算出した重力成分を減算してもよい。
この一態様において、前記移動体は一対の駆動車輪を駆動する一対の駆動手段を備えており、前記移動体制御手段は、前記前記外力算出手段により算出された外力の増加を抑制するように前記各駆動手段を制御してもよい。
他方、上記目的を達成するための本発明の一態様は、複数の駆動車輪を駆動させて移動を行う移動体を制御する移動体制御装置の制御方法であって、前記各駆動車輪の回転情報を検出するステップと、前記各駆動車輪を駆動するための駆動トルクを算出するステップと、前記検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出するステップと、前記各駆動車輪の慣性トルクと、前記算出された駆動トルクと、前記算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力によりに前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出するステップと、前記算出された外力に応じて、前記移動体の移動を制御するステップと、を含むことを特徴とする移動体制御装置の制御方法であってもよい。
また、上記目的を達成するための本発明の一態様は、複数の駆動車輪を駆動させて移動を行う移動体を制御する移動体制御装置の制御プログラムであって、前記各駆動車輪の回転情報を検出する処理と、前記各駆動車輪を駆動するための駆動トルクを算出する処理と、
前記検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出する処理と、前記各駆動車輪の慣性トルクと、前記算出された駆動トルクと、前記算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力により前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出する処理と、前記算出された外力に応じて、前記移動体の移動を制御する処理と、を含むことを特徴とする移動体制御装置の制御プログラムであってもよい。
さらに、上記目的を達成するための本発明の一態様は、移動体に設けられた複数の駆動車輪の回転情報を検出する回転検出手段と、前記各駆動車輪を駆動するための駆動トルクを算出する駆動トルク算出手段と、前記回転検出手段により検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出する摩擦トルク算出手段と、前記各駆動車輪の慣性トルクと、前記駆動トルク算出手段により算出された駆動トルクと、前記摩擦トルク算出手段により算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力により前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出する外力算出手段と、を備えることを特徴とする外力推定装置であってもよい。
【発明の効果】
【０００８】
本発明によれば、コスト低減をしつつ高い安全性が実現できる移動体制御装置、その制御方法、及び制御プログラムを提供することができる。
【図面の簡単な説明】
【０００９】
【図１】本発明の一実施の形態に係る移動体の概略を示す斜視図である。
【図２】本発明の一実施の形態に係る移動体の概略的なシステム構成を示すブロック図である。
【図３】本発明の一実施の形態に係る移動体制御装置の概略的構成を示すブロック図である。
【図４】移動体を上方から見たときの移動体の座標系を示す図である。
【図５】移動体が傾斜面上に存在する場合の影響について説明するための図である。
【図６】本発明の一実施の形態に係る移動体制御装置の外力推定フローの一例を示すフローチャートである。
【図７】移動体の右前部に２回、所定の外力を付加したときの、移動体制御装置の外力算出部により算出された外力推定値の変化を示す図である。
【図８】移動体の右前部に２回、所定の外力を付加したときの、移動体座標系における移動体の重心位置の軌跡を示す図である。
【図９】移動体の右前部に２回、所定の外力を付加したときの、各駆動車輪の回転角速度の変化を示す図である。
【発明を実施するための形態】
【００１０】
実施の形態
以下、図面を参照して本発明の実施の形態について説明する。
本実施の形態に係る移動体制御装置１０は、例えば、図１に示すような移動体１に搭載され、その移動を制御することができる。本実施の形態に係る移動体１は、例えば、移動体本体２と、移動体本体２に回転可能に設けられた左右一対の駆動車輪３Ｌ、３Ｒ及び前後一対の従動車輪４Ｆ、４Ｂと、を備える移動型ロボットとして構成されている。
【００１１】
図２は、本実施の形態に係る移動体の概略的なシステム構成を示すブロック図である。移動体１は、各駆動車輪３Ｌ、３Ｒを駆動する一対のモータ５Ｌ、５Ｒと、各モータ５Ｌ、５Ｒを駆動する一対のモータドライバ６Ｌ、６Ｒと、各モータ５Ｌ、５Ｒに電力を供給するバッテリ７と、バッテリ７と各モータドライバ６Ｌ、６Ｒとの間に設けられる絶縁パワースイッチ８と、バッテリ７から供給される直流電圧を変換するＤＣ−ＤＣコンバータ９と、各モータドライバ６Ｌ、６Ｒの駆動を制御する移動体制御装置１０と、移動体制御装置１０から出力されるデジタル信号をアナログ信号に変換するＤＡ変換器１２と、各駆動車輪３Ｌ、３Ｒの回転角度を検出する一対のエンコーダ１３Ｌ、１３Ｒと、各エンコーダ１３Ｌ、１３Ｒから出力される信号をカウントするカウンタ１４と、移動体本体２の姿勢を検出する姿勢センサ１５と、を備えている。
【００１２】
各モータ５Ｌ、５Ｒは、駆動手段の一具体例であり、例えば、各駆動軸が減速機などを介して各駆動車輪３Ｌ、３Ｒに夫々連結されている。各モータ５Ｌ、５Ｒは、各モータドライバ６Ｌ、６Ｒから出力される駆動信号に応じて、夫々独立して各駆動車輪３Ｌ、３Ｒを回転駆動することができる。これにより、移動体１は前後進、左右旋回、停止、加減速などの任意の走行を行うことができる。
【００１３】
各モータドライバ６Ｌ、６Ｒは、移動体制御装置１０からＤＡ変換器１２を介して入力される制御電圧信号に応じた駆動電流信号を、各モータ５Ｌ、５Ｒに対して出力する。各モータ５Ｌ、５Ｒは、各モータドライバ６Ｌ、６Ｒから出力される駆動電流信号を用いて回転駆動し、各駆動車輪３Ｌ、３Ｒを回転させる。
【００１４】
バッテリ７は、例えばリチウムイオンバッテリなどが用いられており、一定の電力を蓄電することができる。バッテリ７は、絶縁パワースイッチ８、及び各モータドライバ６Ｌ、６Ｒを介して各モータ５Ｌ、５Ｒに電力を供給し、或いは、ＤＣ−ＤＣコンバータ９を介してカウンタ１４及び移動体制御装置１０に対して電力を供給する。
【００１５】
絶縁パワースイッチ８は、例えば、ユーザがオンオフ切替え可能なスイッチであり、オン状態になるとバッテリ７から出力される電力を各モータ５Ｌ、５Ｒに供給可能な状態となる。これにより、移動体１は移動可能な状態となる。
【００１６】
ＤＣ−ＤＣコンバータ９は、バッテリ７から出力される直流電圧を変換しカウンタ１４及び移動体制御装置１０に夫々出力する。
【００１７】
移動体制御部１０は、例えば、制御処理、演算処理等と行うＣＰＵ（Central Processing Unit）１０ａ、ＣＰＵ１０ａによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ１０ｂ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）１０ｃ、などからなるマイクロコンピュータを、中心にしてハードウェア構成されている。ＣＰＵ１０ａ、メモリ１０ｂ、及びインターフェイス部１０ｃは、データバス１０ｄなどを介して相互に接続されている。
【００１８】
移動体制御装置１０は、ＤＣ−ＤＣコンバータ９、ＤＡ変換器１２、及びカウンタ１４と、例えばＵＳＢ接続されている。移動体制御装置１０は、例えば、カウンタ１４から出力される信号と姿勢センサ１５から出力される姿勢とに応じて生成した制御信号を、ＤＡ変換器１２、各モータドライバ６Ｌ、６Ｒを介して各モータ５Ｌ、５Ｒに対して出力することでフィードバック制御を行う。
【００１９】
ＤＡ変換器１２は、移動体制御装置１０から出力されたデジタルの制御信号をアナログの制御信号に変換して、各モータドライバ６Ｌ、６Ｒあるいは絶縁パワースイッチ８に出力する。
【００２０】
各エンコーダ１３Ｌ、１３Ｒは、例えば、各モータ５Ｌ、５Ｒの駆動軸の回転をパルス状信号としてカウンタ１４に対して出力する。カウンタ１４は、各エンコーダ１３Ｌ、１３Ｒから出力されるパルス状信号をカウントし、そのカウント値を移動体制御装置１０に対して出力する。移動体制御装置１０は、カウンタ１４から出力されるカウント値に基いて、各駆動車輪３Ｌ、３Ｒの回転角度、回転角速度、回転角加速度を算出することができる。
【００２１】
姿勢センサ１５は、傾斜角検出手段の一具体例であり、例えば、ジャイロセンサや加速度センサなどから構成されており、移動体１の傾斜角度などの姿勢を検出することができる。
【００２２】
次に、上述のように構成された移動体を制御する移動体制御装置について詳細に説明する。図３は、本実施の形態に係る移動体制御装置の概略的構成を示すブロック図である。本実施の形態に係る移動体制御装置１０は、各モータドライバ６Ｌ、６Ｒを介して各モータ５Ｌ、５Ｒの制御を行う移動体制御部１１と、各駆動車輪３Ｌ、３Ｒの回転角度を検出する回転角度検出部１６と、各駆動車輪３Ｌ、３Ｒの駆動トルクを算出する駆動トルク算出部１７と、路面と各駆動車輪３Ｌ、３Ｒとの間に生じる摩擦トルクを算出する摩擦トルク算出部１８と、移動体１に対して作用する外力を算出する外力算出部１９と、を備えている。
【００２３】
回転角度検出部１６は、回転検出手段の一具体例であり、例えば、上述した各エンコーダ１３Ｌ、１３Ｒとカウンタ１４とを用いて各駆動車輪３Ｌ、３Ｒの回転角度、回転角速度、回転角加速度を検出する。
【００２４】
ここで上記移動体１の座標系について考える。なお、移動体１が移動する空間を水平な平面上と仮定する。図４は、移動体を上方から見たときの移動体の座標系を示す図である。絶対座標系の原点から見た移動体１の座標をｐＢ＝［ｐＢｘ、ＰＢｙ、ＰＢθ］Ｔとする。ｐＢｘ及びｐＢｙは移動体１の重心位置を中心とする移動体１の位置を示しており、ｐＢθは移動体１の向き（角度）を示している。また、移動体１の左右駆動車輪３Ｌ、３Ｒの間隔を２ｌとし、各駆動車輪３Ｌ、３Ｒの半径をｒとし、各駆動車輪３Ｌ、３Ｒの回転角度をθＷ＝［θＬ、θＲ］Ｔとする。さらに、移動体１の進行方向をＸ軸とする移動体１の座標系を想定する。
【００２５】
上記のように想定した移動体１の座標系において、移動体１に作用する外力をｆＢ＝［ｆＢｘ、ｆＢｙ、τＢ］Ｔとすると、絶対座標系ではｆＢＡ＝Ｒ（ｐＢθ）ｆＢと表わすことができる。なお、Ｒ（ｐＢθ）はｐＢθだけ回転させる回転変換行列を表わしており、下記（１）式のように表現できる。但し、下記（１）式において、ＳｐＢθ＝ｓｉｎ（ｐＢθ）、ＣｐＢθ＝ｃｏｓ（ｐＢθ）とする。
【数１】
2014044625.tif 000003
【００２６】
各駆動車輪３Ｌ、３Ｒが路面上を滑ることなく回転すると仮定する。この場合、左右駆動車輪３Ｌ、３Ｒの回転角速度（θＬの１階微分値）、（θＲの１階微分値）を用いて、移動体の速度（ｐＢｘの１階微分値）、（ｐＢｙの１階微分値）、及び姿勢の角速度（傾斜角速度）（ｐＢθの１階微分値）を求める為の下記（２）式が成立する。
【数２】
2014044625.tif 000004
【００２７】
ところで、例えばデッドレコニング（Dead-Reckoning:ＤＲ）方式（又はオドメトリ）において、移動体の速度及び姿勢の角速度を積分することで移動体の自己位置を算出することができる。一方で、デッドレコニング方式を単に用いて自己位置を算出した場合、移動体の速度及び姿勢の角速度の誤差も積分されるため、最終的に算出された自己位置は絶対位置情報として精度上の問題が生じ得る。特に、移動体の旋回時には各駆動車輪と路面との間に滑りが生じるため、上記誤差はさらに大きなものとなる。
【００２８】
これに対し、上記（２）式における行列ＪＢはヤコビ行列を表わしている。そこで、このヤコビ行列ＪＢの転置行列ＪＢＴを用いて、移動体１の重心位置に作用する外力ｆＢｘ、ｆＢｙ、τＢと左右駆動車輪３Ｌ、３ＲのトルクτＬ、τＲとの関係を示す下記（３）式を得ることができる。
【数３】
2014044625.tif 000005
【００２９】
上記のように最終的に得られた（３）式は移動体１の向きｐＢθを含まない。このため、移動体１の座標系において、移動体１の重心位置に作用する外力ｆＢｘ、ｆＢｙ、τＢと左右駆動車輪３Ｌ、３ＲのトルクτＬ、τＲとは、移動体１の向きｐＢθと無関係であることが分かる。すなわち、車輪トルクにおいて外力による外力トルクを推定できれば、上記（３）式を用いて移動体１の重心位置に作用する外力ｆＢｘ、ｆＢｙ、τＢを逆算することができる。
【００３０】
なお、この算出過程で、移動体１の向きｐＢθの影響を受けない（３）式を用いることから、デッドレコニング方式による積分誤差の影響を受けることもないため、高精度に移動体１の重心位置に作用する外力を求めることができる。但し、上記ヤコビ行列ＪＢは正方でないため、後述の如く疑似逆行列を用いることになる。
【００３１】
ここで、各駆動車輪３Ｌ、３Ｒと路面との間の摩擦力と、路面から各駆動車輪３Ｌ、３Ｒに対し作用する外力と、を加味した各駆動車輪３Ｌ、３Ｒの運動方程式は、下記（４）式のように表わすことができる。
【数４】
2014044625.tif 000006
【００３２】
なお、上記（４）式において、ＩＬ及びＩＲはそれぞれ各駆動車輪３Ｌ、３Ｒの等価慣性モーメント係数を示し、ＢＬ及びＢＲはそれぞれ各駆動車輪３Ｌ、３Ｒの粘性摩擦係数を示し、ＥＬ及びＥＲはそれぞれ各駆動車輪３Ｌ、３Ｒのクーロン摩擦係数を示している。
【００３３】
摩擦トルク算出部１８は、摩擦トルク算出手段の一具体例であり、回転角度検出部１６により検出された各駆動車輪３Ｌ、３Ｒの回転角速度（θＬの１階微分値）及び（θＲの１階微分値）を用いて、上記（４）式における摩擦項である粘性摩擦トルクＢＬ・（θＬの１階微分値）、ＢＲ・（θＲの１階微分値）及びクーロン摩擦トルクＥＬｓｇｎ（θＬの１階微分値）、ＥＲｓｇｎ（θＲの１階微分値）を夫々算出する。
【００３４】
なお、本実施の形態において、簡略化のため粘性摩擦及びクーロン摩擦を用いてモデル化した上記（４）式を示している。一方、例えば、減速機等の駆動系の特性により、各駆動車輪３Ｌ、３Ｒの正回転と逆回転で摩擦特性が異なる場合も存在する。その場合、実測などによって求めた特性を用いてもよい。
【００３５】
駆動トルク算出部１８は、駆動トルク算出手段の一具体例であり、例えば、各モータドライバ６Ｌ、６Ｒに入力される制御電圧信号あるいは各モータドライバ６Ｌ、６Ｒから各モータ５Ｌ、５Ｒに出力される制御電流信号に基いて、上記（４）式における各駆動車輪３Ｌ、３Ｒを駆動する駆動トルクτＬ、τＲを夫々算出する。なお、駆動トルク算出部１８は、各モータ５Ｌ、５Ｒ又は各駆動車輪５Ｌ、５Ｒに取付けられたトルクセンサを用いて各駆動車輪５Ｌ、５Ｒを駆動する駆動トルクτＬ、τＲを算出してもよい。
【００３６】
また、上記（４）式において、τＬｅ及びτＲｅはそれぞれ路面から各駆動車輪３Ｌ、３Ｒに作用する外力トルクを示している。なお、この外力トルクには、各モータ５Ｌ、５Ｒなどから伝達される駆動力によって生じる外力や移動体１が障害物等に衝突したときに生じる外力も含まれる。
【００３７】
外力算出部１９は、外力算出手段の一具体例であり、上記（４）式を用いて外力トルクの推定値（以下、外力トルク推定値）τＬｅハット及びτＲｅハットを夫々算出する。
【００３８】
ここで、外力算出部１９は、まず、回転角度検出部１６により検出された各駆動車輪５Ｌ、５Ｒの回転角加速度（θＬの２階微分値）及び（θＲの２階微分値）を用いて、各駆動車輪５Ｌ、５Ｒの等価慣性トルクＩＬ・（θＬの２階微分値）、ＩＲ・（θＲの２階微分値）を算出する。そして、外力算出部１９は、算出した各駆動車輪５Ｌ、５Ｒの等価慣性トルクＩＬ・（θＬの２階微分値）、ＩＲ・（θＲの２階微分値）と、摩擦トルク算出部１８により算出された粘性摩擦トルクＢＬ・（θＬの１階微分値）、ＢＲ・（θＲの１階微分値）及びクーロン摩擦トルクＥＬｓｇｎ（θＬの１階微分値）、ＥＲｓｇｎ（θＲの１階微分値）と、駆動トルク算出部１７により算出された駆動トルクτＬ、τＲと、に基いて、上記（４）式を用いて、外力トルク推定値τＬｅハット及びτＲｅハットを夫々算出する。なお、移動体１の加速度が低い状態のとき、外力算出部１９は、上記（４）式において、各駆動車輪５Ｌ、５Ｒの等価慣性トルクＩＬ・（θＬの２階微分値）、ＩＲ・（θＲの２階微分値）を含めずに（０として）、外力トルク推定値τＬｅハット及びτＲｅハットを夫々算出してもよい。
【００３９】
さらに、上記（３）式を用いて、移動体１の重心位置に作用する外力の推定値を算出するための下記（５）式を導出することができる。
【数５】
2014044625.tif 000007
なお、上記（５）式において、（・）＋は疑似逆行列を示しており、例えば、行列Ａの疑似逆行列はＡＴ（ＡＡＴ）−１となる。
【００４０】
ここで、上記（５）式において、外力のｙ成分ｆＢｙは常時０となる。この外力のｙ成分ｆＢｙは、各駆動車輪３Ｌ、３Ｒに対して横方向に作用する力成分であり、各駆動車輪３Ｌ、３Ｒの回転トルクに影響しないため、０となる。
【００４１】
外力算出部１９は、上記算出した外力トルク推定値τＬｅハット及びτＲｅハットと、上記（５）式とに基いて、移動体１の重心位置に作用する外力の推定値（以下、外力推定値）ｆＢハットを算出する。以上のようにして、移動体１の重心位置に作用する外力を、その外力を計測するための特別なセンサを用いること無く、移動体１の既存のセンサだけで算出することができる。したがって、そのセンサにかかるコストを低減できる。
【００４２】
次に、移動体１が傾斜面上に存在する場合について説明する。この場合、水平面上に存在する場合と比較して、移動体１にはさらに鉛直下向きの重力Ｍｇが外力の一部として作用することとなる（図５）。外力算出部１９は、例えば、姿勢センサ１５により検出された加速度情報に基いて重力加速度ｇの斜面方向成分（重力加速度ｇを斜面に投影したベクトル成分）ａを算出する。そして、外力算出部１９は、上記のように算出した外力推定値から、外力に含まれる重力成分Ｍａを減算する。このようにして、移動体１が水平面上に存在する場合だけでなく、斜面上に存在する場合でも、物体の接触に伴う正味の外力を、その外力を検出するための特別なセンサを用いることなく算出することができる。
【００４３】
移動体制御部１１は、例えば、外力算出部１９により算出された外力推定値を抑制するように移動体１のインピーダンス制御を行う。具体的には、移動体制御部１１は、移動体１の速度目標値（Ｐｄの１階微分値）を、例えば、下記（６）式に示す仮想インピーダンスモデルに基いて算出する。なお、下記（６）式において、Ｍｄは移動体１の仮想質量であり、Ｄｄは移動体１の仮想の粘性摩擦であり、Ｐｄは移動体１の目標位置ベクトルであり、ｆＢは、外力算出部１９により算出される移動体１に作用する外力ベクトル（外力推定値）であり、Ｖｃは仮想コンベアの速度ベクトルである。
【数６】
2014044625.tif 000008
【００４４】
そして、移動体制御部１１は、算出した移動体１の速度目標値（Ｐｄの１階微分値）に基いて、下記（７）式を用いて各駆動車輪３Ｌ、３Ｒの速度目標値（θＷｄの１階微分値）を算出する。
【数７】
2014044625.tif 000009
【００４５】
移動体制御部１１は、各駆動車輪３Ｌ、３Ｒの回転角速度が上記算出した各駆動車輪３Ｌ、３Ｒの速度目標値（θＷｄの１階微分値）となるように、各モータ５Ｌ、５Ｒのフィードバック制御を行う。これにより、移動体１に付加される外力の増加を緩和するように移動体１を高精度に制御することができ、高い安全性が実現できる。
【００４６】
次に、本実施の形態に係る移動体制御装置による外力推定フローについて説明する。図６は、本実施の形態に係る移動体制御装置の外力推定フローの一例を示すフローチャートである。
【００４７】
まず、ＤＡ変換器１２及び各モータドライバ６Ｌ、６Ｒの初期化が行われる（ステップＳ１０１）。次に、ＤＡ変換器１２から各モータドライバ６Ｌ、６Ｒへ、その中立点に相当する電圧を出力する（ステップＳ１０２）。さらに絶縁パワースイッチ８をオン状態とすることで、バッテリ７より各モータドライバ６Ｌ、６Ｒに対して電力が供給され、各モータドライバ６Ｌ、６Ｒがオン状態となる（ステップＳ１０３）。
【００４８】
各エンコーダ１３Ｌ、１３Ｒは、各駆動車輪３Ｌ、３Ｒが回転するとその回転に応じたパルス状信号をカウンタ１４に出力する（ステップＳ１０４）。カウンタ１４は、そのパルス状信号のカウント値を回転角度検出部１６に出力する。
【００４９】
回転角度検出部１６は、カウンタ１４から出力されるカウント値を各駆動車輪３Ｌ、３Ｒの回転角度、回転角速度、回転角加速度に変換する（ステップＳ１０５）。
【００５０】
外力算出部１９は、各駆動車輪３Ｌ、３Ｒの等価慣性モーメントと、摩擦トルク算出部により算出された粘性摩擦トルク及びクーロン摩擦トルクと、駆動トルク算出部により算出された駆動トルクと、に基いて、上記（４）式を用いて、外力トルク推定値を算出する（ステップＳ１０６）。
【００５１】
外力算出部１９は、上記算出した外力トルク推定値と、上記（５）式と、に基いて、移動体１の重心位置に作用する外力推定値を算出する（ステップＳ１０７）。外力算出部１９は、本外力推定処理を終了するか否かを判定し（ステップＳ１０８）、終了しない場合、上記（ステップＳ１０５）の処理に戻る。
【００５２】
次に本実施の形態に係る移動体制御装置によるシミュレーション結果について説明する。本シミュレーションにおいて、移動体１の中央部、右前部、及び左前部を夫々２回手を触れて所定の外力を付加している。図７は、移動体の右前部に２回、所定の外力を付加したときの、移動体制御装置の外力算出部により算出された外力推定値の変化を示す図である。図８は、移動体の右前部に２回、所定の外力を付加したときの、移動体座標系における移動体の重心位置（ｘＢ、ｙＢ）の軌跡を示す図である。図９は、移動体の右前部に２回、所定の外力を付加したときの、各駆動車輪の回転角速度の変化を示す図である。
【００５３】
図７に示すように、最初の１秒間は移動体１の加速の影響で外力推定値が大きくなっている。しかしながら、その後、２〜４秒間、及び７〜１０秒間の一定速移動中において、外力推定値は略０となっている。一方、上記外力を付加した４〜７秒間、及び１０〜１３秒間においては、外力推定値が負方向に大きくなっている。このとき、図９に示すように、いずれの場合も、その外力付加時の衝撃はほとんど感じられず、移動体１はスムーズに停止できていることが分かる。
【００５４】
また、図８に示すように、移動体制御装置１０はｘＢ方向に付加された外力を推定し、移動体１の直進を停止させ、外力の付加が無くなる移動体１の直進を再開させている。これは、移動体制御装置１０が移動体１に付加された外力を的確に推定し、移動体１を停止させているためである。
【００５５】
以上、本実施の形態に係る移動体制御装置１０において、移動体１の重心位置に作用する外力を、その外力を計測するための特別なセンサを用いること無く、移動体１の既存のセンサだけで算出することができる。したがって、コスト低減に繋がる。さらに、移動体制御装置１０は、上述のようにして算出した外力を緩和するように移動体１を高精度に制御することで、移動体１の安全性を向上させることができる。すなわち、コスト低減をしつつ高い安全性が実現できる。
【００５６】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【００５７】
また、本発明は、例えば、上記図６に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
【００５８】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【００５９】
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００６０】
１    移動体
２    移動体本体
３Ｌ、３Ｒ    駆動車輪
４Ｆ、４Ｂ    従動車輪
５Ｌ、５Ｒ    モータ
６Ｌ、６Ｒモータドライバ
７    バッテリ
８    絶縁パワースイッチ
９    ＤＣ−ＤＣコンバータ
１０    移動体制御装置
１１    移動体制御部
１２    ＤＡ変換器
１３Ｌ、１３Ｒ    エンコーダ
１４    カウンタ
１５    姿勢センサ
１６    回転角度検出部
１７    駆動トルク算出部
１８    摩擦トルク算出部
１９    外力算出部
【図１】
2014044625.tif 000011
【図２】
2014044625.tif 000012
【図３】
2014044625.tif 000013
【図４】
2014044625.tif 000014
【図５】
2014044625.tif 000015
【図６】
2014044625.tif 000016
【図７】
2014044625.tif 000017
【図８】
2014044625.tif 000018
【図９】
2014044625.tif 000019