JP5913001B2

JP5913001B2 - 移動体制御装置、その制御方法、及び制御プログラム

Info

Publication number: JP5913001B2
Application number: JP2012187416A
Authority: JP
Inventors: 直行武居; 藤本　英雄; 英雄藤本; 山口　宇唯; 宇唯山口
Original assignee: Nagoya Institute of Technology NUC; Toyota Motor Corp
Current assignee: Nagoya Institute of Technology NUC; Toyota Motor Corp
Priority date: 2012-08-28
Filing date: 2012-08-28
Publication date: 2016-04-27
Anticipated expiration: 2032-08-28
Also published as: JP2014044625A

Description

本発明は、移動体に作用する外力を推定し、移動体を最適に制御する移動体制御装置、その制御方法、及び制御プログラムに関するものである。

近年、環境内に存在する障害物等を回避しつつ、自律的に移動する移動体の開発が行われている。このような移動体においては、仮に、障害物などに接触した場合にその接触により生じる外力を推定し、その衝撃を緩和するなどの高い安全性の制御を行うことが望まれる。

例えば、トルクセンサなどによる人の入力と環境からの外力に対して、夫々コンプライアンス制御を行う走行車の制御方法が知られている（例えば、特許文献１参照）。

特開２００４−０４９５２３号公報

しかしながら、上記特許文献１に示す制御方法においては、例えば、環境内に存在する障害物等と接触したときに生じる外力を推定するための、複数のセンサなどが必要となりコスト増加に繋がる虞がある。

本発明は、このような問題点を解決するためになされたものであり、コスト低減をしつつ高い安全性が実現できる移動体制御装置、その制御方法、及び制御プログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、複数の駆動車輪を駆動させて移動を行う移動体を制御する移動体制御装置であって、前記各駆動車輪の回転情報を検出する回転検出手段と、前記各駆動車輪を駆動するための駆動トルクを算出する駆動トルク算出手段と、前記回転検出手段により検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出する摩擦トルク算出手段と、前記各駆動車輪の慣性トルクと、前記駆動トルク算出手段により算出された駆動トルクと、前記摩擦トルク算出手段により算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力により前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出する外力算出手段と、前記外力算出手段により算出された外力に応じて、前記移動体の移動を制御する移動体制御手段と、を備えることを特徴とする移動体制御装置である。
この一態様において、前記外力算出手段は、前記駆動車輪の外力トルクと、移動体の重心位置に作用する外力との関係を示すヤコビ転置行列の疑似逆行列と、を用いて前記移動体の重心位置に作用する外力を算出することを特徴とする移動体制御装置。
この一態様において、前記外力算出手段は、下記（５）式を用いて前記移動体の重心位置に作用する外力ｆ_Ｂハットを算出してもよい。但し、上記式において、ｒは前記各駆動車輪の半径であり、τ_ｅハットは前記駆動車輪の外力トルクであり、一対の駆動車輪間の距離を２ｌとする。
この一態様において、前記摩擦トルク算出手段は、前記回転検出手段により検出された前記各駆動車輪の回転角速度に基いて、前記各駆動車輪の粘性摩擦トルク及びクーロン摩擦トルクを夫々算出し、該算出した各駆動車輪の粘性摩擦トルク及びクーロン摩擦トルクを加算して前記各駆動車輪の摩擦トルクを算出してもよい。
この一態様において、前記移動体の傾斜角を検出する傾斜角検出手段を更に備え、前記外力算出手段は、前記傾斜角検出手段により検出された移動体の傾斜角に基いて、前記移動体に作用する斜面方向の重力成分を算出し、前記算出した移動体の重心位置に作用する外力から前記算出した重力成分を減算してもよい。
この一態様において、前記移動体は一対の駆動車輪を駆動する一対の駆動手段を備えており、前記移動体制御手段は、前記前記外力算出手段により算出された外力の増加を抑制するように前記各駆動手段を制御してもよい。
他方、上記目的を達成するための本発明の一態様は、複数の駆動車輪を駆動させて移動を行う移動体を制御する移動体制御装置の制御方法であって、前記各駆動車輪の回転情報を検出するステップと、前記各駆動車輪を駆動するための駆動トルクを算出するステップと、前記検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出するステップと、前記各駆動車輪の慣性トルクと、前記算出された駆動トルクと、前記算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力によりに前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出するステップと、前記算出された外力に応じて、前記移動体の移動を制御するステップと、を含むことを特徴とする移動体制御装置の制御方法であってもよい。
また、上記目的を達成するための本発明の一態様は、複数の駆動車輪を駆動させて移動を行う移動体を制御する移動体制御装置の制御プログラムであって、前記各駆動車輪の回転情報を検出する処理と、前記各駆動車輪を駆動するための駆動トルクを算出する処理と、
前記検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出する処理と、前記各駆動車輪の慣性トルクと、前記算出された駆動トルクと、前記算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力により前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出する処理と、前記算出された外力に応じて、前記移動体の移動を制御する処理と、を含むことを特徴とする移動体制御装置の制御プログラムであってもよい。
さらに、上記目的を達成するための本発明の一態様は、移動体に設けられた複数の駆動車輪の回転情報を検出する回転検出手段と、前記各駆動車輪を駆動するための駆動トルクを算出する駆動トルク算出手段と、前記回転検出手段により検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出する摩擦トルク算出手段と、前記各駆動車輪の慣性トルクと、前記駆動トルク算出手段により算出された駆動トルクと、前記摩擦トルク算出手段により算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力により前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出する外力算出手段と、を備えることを特徴とする外力推定装置であってもよい。

本発明によれば、コスト低減をしつつ高い安全性が実現できる移動体制御装置、その制御方法、及び制御プログラムを提供することができる。

本発明の一実施の形態に係る移動体の概略を示す斜視図である。本発明の一実施の形態に係る移動体の概略的なシステム構成を示すブロック図である。本発明の一実施の形態に係る移動体制御装置の概略的構成を示すブロック図である。移動体を上方から見たときの移動体の座標系を示す図である。移動体が傾斜面上に存在する場合の影響について説明するための図である。本発明の一実施の形態に係る移動体制御装置の外力推定フローの一例を示すフローチャートである。移動体の右前部に２回、所定の外力を付加したときの、移動体制御装置の外力算出部により算出された外力推定値の変化を示す図である。移動体の右前部に２回、所定の外力を付加したときの、移動体座標系における移動体の重心位置の軌跡を示す図である。移動体の右前部に２回、所定の外力を付加したときの、各駆動車輪の回転角速度の変化を示す図である。

実施の形態
以下、図面を参照して本発明の実施の形態について説明する。
本実施の形態に係る移動体制御装置１０は、例えば、図１に示すような移動体１に搭載され、その移動を制御することができる。本実施の形態に係る移動体１は、例えば、移動体本体２と、移動体本体２に回転可能に設けられた左右一対の駆動車輪３Ｌ、３Ｒ及び前後一対の従動車輪４Ｆ、４Ｂと、を備える移動型ロボットとして構成されている。

図２は、本実施の形態に係る移動体の概略的なシステム構成を示すブロック図である。移動体１は、各駆動車輪３Ｌ、３Ｒを駆動する一対のモータ５Ｌ、５Ｒと、各モータ５Ｌ、５Ｒを駆動する一対のモータドライバ６Ｌ、６Ｒと、各モータ５Ｌ、５Ｒに電力を供給するバッテリ７と、バッテリ７と各モータドライバ６Ｌ、６Ｒとの間に設けられる絶縁パワースイッチ８と、バッテリ７から供給される直流電圧を変換するＤＣ−ＤＣコンバータ９と、各モータドライバ６Ｌ、６Ｒの駆動を制御する移動体制御装置１０と、移動体制御装置１０から出力されるデジタル信号をアナログ信号に変換するＤＡ変換器１２と、各駆動車輪３Ｌ、３Ｒの回転角度を検出する一対のエンコーダ１３Ｌ、１３Ｒと、各エンコーダ１３Ｌ、１３Ｒから出力される信号をカウントするカウンタ１４と、移動体本体２の姿勢を検出する姿勢センサ１５と、を備えている。

各モータ５Ｌ、５Ｒは、駆動手段の一具体例であり、例えば、各駆動軸が減速機などを介して各駆動車輪３Ｌ、３Ｒに夫々連結されている。各モータ５Ｌ、５Ｒは、各モータドライバ６Ｌ、６Ｒから出力される駆動信号に応じて、夫々独立して各駆動車輪３Ｌ、３Ｒを回転駆動することができる。これにより、移動体１は前後進、左右旋回、停止、加減速などの任意の走行を行うことができる。

各モータドライバ６Ｌ、６Ｒは、移動体制御装置１０からＤＡ変換器１２を介して入力される制御電圧信号に応じた駆動電流信号を、各モータ５Ｌ、５Ｒに対して出力する。各モータ５Ｌ、５Ｒは、各モータドライバ６Ｌ、６Ｒから出力される駆動電流信号を用いて回転駆動し、各駆動車輪３Ｌ、３Ｒを回転させる。

バッテリ７は、例えばリチウムイオンバッテリなどが用いられており、一定の電力を蓄電することができる。バッテリ７は、絶縁パワースイッチ８、及び各モータドライバ６Ｌ、６Ｒを介して各モータ５Ｌ、５Ｒに電力を供給し、或いは、ＤＣ−ＤＣコンバータ９を介してカウンタ１４及び移動体制御装置１０に対して電力を供給する。

絶縁パワースイッチ８は、例えば、ユーザがオンオフ切替え可能なスイッチであり、オン状態になるとバッテリ７から出力される電力を各モータ５Ｌ、５Ｒに供給可能な状態となる。これにより、移動体１は移動可能な状態となる。

ＤＣ−ＤＣコンバータ９は、バッテリ７から出力される直流電圧を変換しカウンタ１４及び移動体制御装置１０に夫々出力する。

移動体制御部１０は、例えば、制御処理、演算処理等と行うＣＰＵ（Central Processing Unit）１０ａ、ＣＰＵ１０ａによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ１０ｂ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）１０ｃ、などからなるマイクロコンピュータを、中心にしてハードウェア構成されている。ＣＰＵ１０ａ、メモリ１０ｂ、及びインターフェイス部１０ｃは、データバス１０ｄなどを介して相互に接続されている。

移動体制御装置１０は、ＤＣ−ＤＣコンバータ９、ＤＡ変換器１２、及びカウンタ１４と、例えばＵＳＢ接続されている。移動体制御装置１０は、例えば、カウンタ１４から出力される信号と姿勢センサ１５から出力される姿勢とに応じて生成した制御信号を、ＤＡ変換器１２、各モータドライバ６Ｌ、６Ｒを介して各モータ５Ｌ、５Ｒに対して出力することでフィードバック制御を行う。

ＤＡ変換器１２は、移動体制御装置１０から出力されたデジタルの制御信号をアナログの制御信号に変換して、各モータドライバ６Ｌ、６Ｒあるいは絶縁パワースイッチ８に出力する。

各エンコーダ１３Ｌ、１３Ｒは、例えば、各モータ５Ｌ、５Ｒの駆動軸の回転をパルス状信号としてカウンタ１４に対して出力する。カウンタ１４は、各エンコーダ１３Ｌ、１３Ｒから出力されるパルス状信号をカウントし、そのカウント値を移動体制御装置１０に対して出力する。移動体制御装置１０は、カウンタ１４から出力されるカウント値に基いて、各駆動車輪３Ｌ、３Ｒの回転角度、回転角速度、回転角加速度を算出することができる。

姿勢センサ１５は、傾斜角検出手段の一具体例であり、例えば、ジャイロセンサや加速度センサなどから構成されており、移動体１の傾斜角度などの姿勢を検出することができる。

次に、上述のように構成された移動体を制御する移動体制御装置について詳細に説明する。図３は、本実施の形態に係る移動体制御装置の概略的構成を示すブロック図である。本実施の形態に係る移動体制御装置１０は、各モータドライバ６Ｌ、６Ｒを介して各モータ５Ｌ、５Ｒの制御を行う移動体制御部１１と、各駆動車輪３Ｌ、３Ｒの回転角度を検出する回転角度検出部１６と、各駆動車輪３Ｌ、３Ｒの駆動トルクを算出する駆動トルク算出部１７と、路面と各駆動車輪３Ｌ、３Ｒとの間に生じる摩擦トルクを算出する摩擦トルク算出部１８と、移動体１に対して作用する外力を算出する外力算出部１９と、を備えている。

回転角度検出部１６は、回転検出手段の一具体例であり、例えば、上述した各エンコーダ１３Ｌ、１３Ｒとカウンタ１４とを用いて各駆動車輪３Ｌ、３Ｒの回転角度、回転角速度、回転角加速度を検出する。

ここで上記移動体１の座標系について考える。なお、移動体１が移動する空間を水平な平面上と仮定する。図４は、移動体を上方から見たときの移動体の座標系を示す図である。絶対座標系の原点から見た移動体１の座標をｐ_Ｂ＝［ｐ_Ｂｘ、Ｐ_Ｂｙ、Ｐ_Ｂθ］^Ｔとする。ｐ_Ｂｘ及びｐ_Ｂｙは移動体１の重心位置を中心とする移動体１の位置を示しており、ｐ_Ｂθは移動体１の向き（角度）を示している。また、移動体１の左右駆動車輪３Ｌ、３Ｒの間隔を２ｌとし、各駆動車輪３Ｌ、３Ｒの半径をｒとし、各駆動車輪３Ｌ、３Ｒの回転角度をθ_Ｗ＝［θ_Ｌ、θ_Ｒ］^Ｔとする。さらに、移動体１の進行方向をＸ軸とする移動体１の座標系を想定する。

上記のように想定した移動体１の座標系において、移動体１に作用する外力をｆ_Ｂ＝［ｆ_Ｂｘ、ｆ_Ｂｙ、τ_Ｂ］^Ｔとすると、絶対座標系ではｆ_Ｂ ^Ａ＝Ｒ（ｐ_Ｂθ）ｆ_Ｂと表わすことができる。なお、Ｒ（ｐ_Ｂθ）はｐ_Ｂθだけ回転させる回転変換行列を表わしており、下記（１）式のように表現できる。但し、下記（１）式において、Ｓｐ_Ｂθ＝ｓｉｎ（ｐ_Ｂθ）、Ｃｐ_Ｂθ＝ｃｏｓ（ｐ_Ｂθ）とする。

各駆動車輪３Ｌ、３Ｒが路面上を滑ることなく回転すると仮定する。この場合、左右駆動車輪３Ｌ、３Ｒの回転角速度（θ_Ｌの１階微分値）、（θ_Ｒの１階微分値）を用いて、移動体の速度（ｐ_Ｂｘの１階微分値）、（ｐ_Ｂｙの１階微分値）、及び姿勢の角速度（傾斜角速度）（ｐ_Ｂθの１階微分値）を求める為の下記（２）式が成立する。

ところで、例えばデッドレコニング（Dead-Reckoning:ＤＲ）方式（又はオドメトリ）において、移動体の速度及び姿勢の角速度を積分することで移動体の自己位置を算出することができる。一方で、デッドレコニング方式を単に用いて自己位置を算出した場合、移動体の速度及び姿勢の角速度の誤差も積分されるため、最終的に算出された自己位置は絶対位置情報として精度上の問題が生じ得る。特に、移動体の旋回時には各駆動車輪と路面との間に滑りが生じるため、上記誤差はさらに大きなものとなる。

これに対し、上記（２）式における行列Ｊ_Ｂはヤコビ行列を表わしている。そこで、このヤコビ行列Ｊ_Ｂの転置行列Ｊ_Ｂ ^Ｔを用いて、移動体１の重心位置に作用する外力ｆ_Ｂｘ、ｆ_Ｂｙ、τ_Ｂと左右駆動車輪３Ｌ、３Ｒのトルクτ_Ｌ、τ_Ｒとの関係を示す下記（３）式を得ることができる。

上記のように最終的に得られた（３）式は移動体１の向きｐ_Ｂθを含まない。このため、移動体１の座標系において、移動体１の重心位置に作用する外力ｆ_Ｂｘ、ｆ_Ｂｙ、τ_Ｂと左右駆動車輪３Ｌ、３Ｒのトルクτ_Ｌ、τ_Ｒとは、移動体１の向きｐ_Ｂθと無関係であることが分かる。すなわち、車輪トルクにおいて外力による外力トルクを推定できれば、上記（３）式を用いて移動体１の重心位置に作用する外力ｆ_Ｂｘ、ｆ_Ｂｙ、τ_Ｂを逆算することができる。

なお、この算出過程で、移動体１の向きｐ_Ｂθの影響を受けない（３）式を用いることから、デッドレコニング方式による積分誤差の影響を受けることもないため、高精度に移動体１の重心位置に作用する外力を求めることができる。但し、上記ヤコビ行列Ｊ_Ｂは正方でないため、後述の如く疑似逆行列を用いることになる。

ここで、各駆動車輪３Ｌ、３Ｒと路面との間の摩擦力と、路面から各駆動車輪３Ｌ、３Ｒに対し作用する外力と、を加味した各駆動車輪３Ｌ、３Ｒの運動方程式は、下記（４）式のように表わすことができる。

なお、上記（４）式において、Ｉ_Ｌ及びＩ_Ｒはそれぞれ各駆動車輪３Ｌ、３Ｒの等価慣性モーメント係数を示し、Ｂ_Ｌ及びＢ_Ｒはそれぞれ各駆動車輪３Ｌ、３Ｒの粘性摩擦係数を示し、Ｅ_Ｌ及びＥ_Ｒはそれぞれ各駆動車輪３Ｌ、３Ｒのクーロン摩擦係数を示している。

摩擦トルク算出部１８は、摩擦トルク算出手段の一具体例であり、回転角度検出部１６により検出された各駆動車輪３Ｌ、３Ｒの回転角速度（θ_Ｌの１階微分値）及び（θ_Ｒの１階微分値）を用いて、上記（４）式における摩擦項である粘性摩擦トルクＢ_Ｌ・（θ_Ｌの１階微分値）、Ｂ_Ｒ・（θ_Ｒの１階微分値）及びクーロン摩擦トルクＥ_Ｌｓｇｎ（θ_Ｌの１階微分値）、Ｅ_Ｒｓｇｎ（θ_Ｒの１階微分値）を夫々算出する。

なお、本実施の形態において、簡略化のため粘性摩擦及びクーロン摩擦を用いてモデル化した上記（４）式を示している。一方、例えば、減速機等の駆動系の特性により、各駆動車輪３Ｌ、３Ｒの正回転と逆回転で摩擦特性が異なる場合も存在する。その場合、実測などによって求めた特性を用いてもよい。

駆動トルク算出部１８は、駆動トルク算出手段の一具体例であり、例えば、各モータドライバ６Ｌ、６Ｒに入力される制御電圧信号あるいは各モータドライバ６Ｌ、６Ｒから各モータ５Ｌ、５Ｒに出力される制御電流信号に基いて、上記（４）式における各駆動車輪３Ｌ、３Ｒを駆動する駆動トルクτ_Ｌ、τ_Ｒを夫々算出する。なお、駆動トルク算出部１８は、各モータ５Ｌ、５Ｒ又は各駆動車輪５Ｌ、５Ｒに取付けられたトルクセンサを用いて各駆動車輪５Ｌ、５Ｒを駆動する駆動トルクτ_Ｌ、τ_Ｒを算出してもよい。

また、上記（４）式において、τ_Ｌｅ及びτ_Ｒｅはそれぞれ路面から各駆動車輪３Ｌ、３Ｒに作用する外力トルクを示している。なお、この外力トルクには、各モータ５Ｌ、５Ｒなどから伝達される駆動力によって生じる外力や移動体１が障害物等に衝突したときに生じる外力も含まれる。

外力算出部１９は、外力算出手段の一具体例であり、上記（４）式を用いて外力トルクの推定値（以下、外力トルク推定値）τ_Ｌｅハット及びτ_Ｒｅハットを夫々算出する。

ここで、外力算出部１９は、まず、回転角度検出部１６により検出された各駆動車輪５Ｌ、５Ｒの回転角加速度（θ_Ｌの２階微分値）及び（θ_Ｒの２階微分値）を用いて、各駆動車輪５Ｌ、５Ｒの等価慣性トルクＩ_Ｌ・（θ_Ｌの２階微分値）、Ｉ_Ｒ・（θ_Ｒの２階微分値）を算出する。そして、外力算出部１９は、算出した各駆動車輪５Ｌ、５Ｒの等価慣性トルクＩ_Ｌ・（θ_Ｌの２階微分値）、Ｉ_Ｒ・（θ_Ｒの２階微分値）と、摩擦トルク算出部１８により算出された粘性摩擦トルクＢ_Ｌ・（θ_Ｌの１階微分値）、Ｂ_Ｒ・（θ_Ｒの１階微分値）及びクーロン摩擦トルクＥ_Ｌｓｇｎ（θ_Ｌの１階微分値）、Ｅ_Ｒｓｇｎ（θ_Ｒの１階微分値）と、駆動トルク算出部１７により算出された駆動トルクτ_Ｌ、τ_Ｒと、に基いて、上記（４）式を用いて、外力トルク推定値τ_Ｌｅハット及びτ_Ｒｅハットを夫々算出する。なお、移動体１の加速度が低い状態のとき、外力算出部１９は、上記（４）式において、各駆動車輪５Ｌ、５Ｒの等価慣性トルクＩ_Ｌ・（θ_Ｌの２階微分値）、Ｉ_Ｒ・（θ_Ｒの２階微分値）を含めずに（０として）、外力トルク推定値τ_Ｌｅハット及びτ_Ｒｅハットを夫々算出してもよい。

さらに、上記（３）式を用いて、移動体１の重心位置に作用する外力の推定値を算出するための下記（５）式を導出することができる。

なお、上記（５）式において、（・）^＋は疑似逆行列を示しており、例えば、行列Ａの疑似逆行列はＡ^Ｔ（ＡＡ^Ｔ）^−１となる。

ここで、上記（５）式において、外力のｙ成分ｆ_Ｂｙは常時０となる。この外力のｙ成分ｆ_Ｂｙは、各駆動車輪３Ｌ、３Ｒに対して横方向に作用する力成分であり、各駆動車輪３Ｌ、３Ｒの回転トルクに影響しないため、０となる。

外力算出部１９は、上記算出した外力トルク推定値τ_Ｌｅハット及びτ_Ｒｅハットと、上記（５）式とに基いて、移動体１の重心位置に作用する外力の推定値（以下、外力推定値）ｆ_Ｂハットを算出する。以上のようにして、移動体１の重心位置に作用する外力を、その外力を計測するための特別なセンサを用いること無く、移動体１の既存のセンサだけで算出することができる。したがって、そのセンサにかかるコストを低減できる。

次に、移動体１が傾斜面上に存在する場合について説明する。この場合、水平面上に存在する場合と比較して、移動体１にはさらに鉛直下向きの重力Ｍｇが外力の一部として作用することとなる（図５）。外力算出部１９は、例えば、姿勢センサ１５により検出された加速度情報に基いて重力加速度ｇの斜面方向成分（重力加速度ｇを斜面に投影したベクトル成分）ａを算出する。そして、外力算出部１９は、上記のように算出した外力推定値から、外力に含まれる重力成分Ｍａを減算する。このようにして、移動体１が水平面上に存在する場合だけでなく、斜面上に存在する場合でも、物体の接触に伴う正味の外力を、その外力を検出するための特別なセンサを用いることなく算出することができる。

移動体制御部１１は、例えば、外力算出部１９により算出された外力推定値を抑制するように移動体１のインピーダンス制御を行う。具体的には、移動体制御部１１は、移動体１の速度目標値（Ｐ_ｄの１階微分値）を、例えば、下記（６）式に示す仮想インピーダンスモデルに基いて算出する。なお、下記（６）式において、Ｍ_ｄは移動体１の仮想質量であり、Ｄ_ｄは移動体１の仮想の粘性摩擦であり、Ｐ_ｄは移動体１の目標位置ベクトルであり、ｆ_Ｂは、外力算出部１９により算出される移動体１に作用する外力ベクトル（外力推定値）であり、Ｖ_ｃは仮想コンベアの速度ベクトルである。

そして、移動体制御部１１は、算出した移動体１の速度目標値（Ｐ_ｄの１階微分値）に基いて、下記（７）式を用いて各駆動車輪３Ｌ、３Ｒの速度目標値（θ_Ｗｄの１階微分値）を算出する。

移動体制御部１１は、各駆動車輪３Ｌ、３Ｒの回転角速度が上記算出した各駆動車輪３Ｌ、３Ｒの速度目標値（θ_Ｗｄの１階微分値）となるように、各モータ５Ｌ、５Ｒのフィードバック制御を行う。これにより、移動体１に付加される外力の増加を緩和するように移動体１を高精度に制御することができ、高い安全性が実現できる。

次に、本実施の形態に係る移動体制御装置による外力推定フローについて説明する。図６は、本実施の形態に係る移動体制御装置の外力推定フローの一例を示すフローチャートである。

まず、ＤＡ変換器１２及び各モータドライバ６Ｌ、６Ｒの初期化が行われる（ステップＳ１０１）。次に、ＤＡ変換器１２から各モータドライバ６Ｌ、６Ｒへ、その中立点に相当する電圧を出力する（ステップＳ１０２）。さらに絶縁パワースイッチ８をオン状態とすることで、バッテリ７より各モータドライバ６Ｌ、６Ｒに対して電力が供給され、各モータドライバ６Ｌ、６Ｒがオン状態となる（ステップＳ１０３）。

各エンコーダ１３Ｌ、１３Ｒは、各駆動車輪３Ｌ、３Ｒが回転するとその回転に応じたパルス状信号をカウンタ１４に出力する（ステップＳ１０４）。カウンタ１４は、そのパルス状信号のカウント値を回転角度検出部１６に出力する。

回転角度検出部１６は、カウンタ１４から出力されるカウント値を各駆動車輪３Ｌ、３Ｒの回転角度、回転角速度、回転角加速度に変換する（ステップＳ１０５）。

外力算出部１９は、各駆動車輪３Ｌ、３Ｒの等価慣性モーメントと、摩擦トルク算出部により算出された粘性摩擦トルク及びクーロン摩擦トルクと、駆動トルク算出部により算出された駆動トルクと、に基いて、上記（４）式を用いて、外力トルク推定値を算出する（ステップＳ１０６）。

外力算出部１９は、上記算出した外力トルク推定値と、上記（５）式と、に基いて、移動体１の重心位置に作用する外力推定値を算出する（ステップＳ１０７）。外力算出部１９は、本外力推定処理を終了するか否かを判定し（ステップＳ１０８）、終了しない場合、上記（ステップＳ１０５）の処理に戻る。

次に本実施の形態に係る移動体制御装置によるシミュレーション結果について説明する。本シミュレーションにおいて、移動体１の中央部、右前部、及び左前部を夫々２回手を触れて所定の外力を付加している。図７は、移動体の右前部に２回、所定の外力を付加したときの、移動体制御装置の外力算出部により算出された外力推定値の変化を示す図である。図８は、移動体の右前部に２回、所定の外力を付加したときの、移動体座標系における移動体の重心位置（ｘ_Ｂ、ｙ_Ｂ）の軌跡を示す図である。図９は、移動体の右前部に２回、所定の外力を付加したときの、各駆動車輪の回転角速度の変化を示す図である。

図７に示すように、最初の１秒間は移動体１の加速の影響で外力推定値が大きくなっている。しかしながら、その後、２〜４秒間、及び７〜１０秒間の一定速移動中において、外力推定値は略０となっている。一方、上記外力を付加した４〜７秒間、及び１０〜１３秒間においては、外力推定値が負方向に大きくなっている。このとき、図９に示すように、いずれの場合も、その外力付加時の衝撃はほとんど感じられず、移動体１はスムーズに停止できていることが分かる。

また、図８に示すように、移動体制御装置１０はｘ_Ｂ方向に付加された外力を推定し、移動体１の直進を停止させ、外力の付加が無くなる移動体１の直進を再開させている。これは、移動体制御装置１０が移動体１に付加された外力を的確に推定し、移動体１を停止させているためである。

以上、本実施の形態に係る移動体制御装置１０において、移動体１の重心位置に作用する外力を、その外力を計測するための特別なセンサを用いること無く、移動体１の既存のセンサだけで算出することができる。したがって、コスト低減に繋がる。さらに、移動体制御装置１０は、上述のようにして算出した外力を緩和するように移動体１を高精度に制御することで、移動体１の安全性を向上させることができる。すなわち、コスト低減をしつつ高い安全性が実現できる。

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

また、本発明は、例えば、上記図６に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

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

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

１移動体
２移動体本体
３Ｌ、３Ｒ駆動車輪
４Ｆ、４Ｂ従動車輪
５Ｌ、５Ｒモータ
６Ｌ、６Ｒモータドライバ
７バッテリ
８絶縁パワースイッチ
９ＤＣ−ＤＣコンバータ
１０移動体制御装置
１１移動体制御部
１２ＤＡ変換器
１３Ｌ、１３Ｒエンコーダ
１４カウンタ
１５姿勢センサ
１６回転角度検出部
１７駆動トルク算出部
１８摩擦トルク算出部
１９外力算出部

Claims

複数の駆動車輪を駆動させて移動を行う移動体を制御する移動体制御装置であって、
前記各駆動車輪の回転情報を検出する回転検出手段と、
前記各駆動車輪を駆動するための駆動トルクを算出する駆動トルク算出手段と、
前記回転検出手段により検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出する摩擦トルク算出手段と、
前記各駆動車輪の慣性トルクと、前記駆動トルク算出手段により算出された駆動トルクと、前記摩擦トルク算出手段により算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力により前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出する外力算出手段と、
前記外力算出手段により算出された外力に応じて、前記移動体の移動を制御する移動体制御手段と、
を備えることを特徴とする移動体制御装置。
請求項１記載の移動体制御装置であって、
前記外力算出手段は、前記駆動車輪の外力トルクと、移動体の重心位置に作用する外力との関係を示すヤコビ転置行列の疑似逆行列と、を用いて前記移動体の重心位置に作用する外力を算出することを特徴とする移動体制御装置。
請求項２記載の移動体制御装置であって、
前記外力算出手段は、下記式を用いて前記移動体の重心位置に作用する外力ｆ_Ｂハットを算出することを特徴とする移動体制御装置。

但し、上記式において、ｒは前記各駆動車輪の半径であり、τ_ｅハットは前記駆動車輪の外力トルクであり、一対の駆動車輪間の距離を２ｌとする。
請求項１乃至３のうちいずれか１項記載の移動体制御装置であって、
前記摩擦トルク算出手段は、前記回転検出手段により検出された前記各駆動車輪の回転角速度に基いて、前記各駆動車輪の粘性摩擦トルク及びクーロン摩擦トルクを夫々算出し、該算出した各駆動車輪の粘性摩擦トルク及びクーロン摩擦トルクを加算して前記各駆動車輪の摩擦トルクを算出することを特徴とする移動体制御装置。
請求項１乃至４のうちいずれか１項記載の移動体制御装置であって、
前記移動体の傾斜角を検出する傾斜角検出手段を更に備え、
前記外力算出手段は、前記傾斜角検出手段により検出された移動体の傾斜角に基いて、前記移動体に作用する斜面方向の重力成分を算出し、前記算出した移動体の重心位置に作用する外力から前記算出した重力成分を減算することを特徴とする移動体制御装置。
請求項１乃至５のうちいずれか１項記載の移動体制御装置であって、
前記移動体は一対の駆動車輪を駆動する一対の駆動手段を備えており、
前記移動体制御手段は、前記前記外力算出手段により算出された外力の増加を抑制するように前記各駆動手段を制御することを特徴とする移動体制御装置。
複数の駆動車輪を駆動させて移動を行う移動体を制御する移動体制御装置の制御方法であって、
前記各駆動車輪の回転情報を検出するステップと、
前記各駆動車輪を駆動するための駆動トルクを算出するステップと、
前記検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出するステップと、
前記各駆動車輪の慣性トルクと、前記算出された駆動トルクと、前記算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力によりに前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出するステップと、
前記算出された外力に応じて、前記移動体の移動を制御するステップと、
を含むことを特徴とする移動体制御装置の制御方法。
複数の駆動車輪を駆動させて移動を行う移動体を制御する移動体制御装置の制御プログラムであって、
前記各駆動車輪の回転情報を検出する処理と、
前記各駆動車輪を駆動するための駆動トルクを算出する処理と、
前記検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出する処理と、
前記各駆動車輪の慣性トルクと、前記算出された駆動トルクと、前記算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力により前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出する処理と、
前記算出された外力に応じて、前記移動体の移動を制御する処理と、
を含むことを特徴とする移動体制御装置の制御プログラム。
移動体に設けられた複数の駆動車輪の回転情報を検出する回転検出手段と、
前記各駆動車輪を駆動するための駆動トルクを算出する駆動トルク算出手段と、
前記回転検出手段により検出された各駆動車輪の回転情報に基いて、駆動系に内在する摩擦及び路面と前記各駆動車輪との間に生じる該各駆動車輪の摩擦トルクを算出する摩擦トルク算出手段と、
前記各駆動車輪の慣性トルクと、前記駆動トルク算出手段により算出された駆動トルクと、前記摩擦トルク算出手段により算出された摩擦トルクと、前記各駆動車輪の運動方程式と、に基いて前記移動体に付加された外力により前記駆動車輪に生じる外力トルクを算出し、該算出した駆動車輪の外力トルクと、前記駆動車輪の外力トルクと移動体の重心位置に作用する外力との関係を示す式と、を用いて前記移動体の重心位置に作用する外力を算出する外力算出手段と、
を備えることを特徴とする外力推定装置。