障害物回避機能を有する移動制御装置
【課題】計算負荷が低く、移動がスムーズであり環境の動的変化などノイズに強いロバストな移動動作が可能な制御装置を提供する。
【解決手段】障害物回避機能を有する移動制御装置１３は、移動体１と、この移動体１に設けられ、移動体進行方位を検出する検出手段１４−１７と、移動体１に設けられ、障害物の位置およびこの障害物までの実計測距離を計測し、この検出手段１４−１７が検出した移動体進行方位に対する障害物の方位角を出力する計測手段９−１２と、計測手段９−１２が出力した障害物の方位角の大きさに応じて、実計測距離を変化させて疑似距離を生成する疑似距離生成手段と、この疑似距離生成手段が生成した疑似距離に基づき、移動体１が障害物を回避するよう移動体１の動作を制御する制御手段と、備える。
【解決手段】障害物回避機能を有する移動制御装置１３は、移動体１と、この移動体１に設けられ、移動体進行方位を検出する検出手段１４−１７と、移動体１に設けられ、障害物の位置およびこの障害物までの実計測距離を計測し、この検出手段１４−１７が検出した移動体進行方位に対する障害物の方位角を出力する計測手段９−１２と、計測手段９−１２が出力した障害物の方位角の大きさに応じて、実計測距離を変化させて疑似距離を生成する疑似距離生成手段と、この疑似距離生成手段が生成した疑似距離に基づき、移動体１が障害物を回避するよう移動体１の動作を制御する制御手段と、備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は障害物回避機能を有する移動制御装置に関する。
【背景技術】
【０００２】
人間共存環境で活動するロボットなどの自律移動体の開発が盛んに行われている。コスト、消費電力、ＣＰＵリソースなどの観点から、このようなロボットに計算負荷の高いアルゴリズムのソフトウェアを実装することは現実的でない。また工場の中で移動する搬送車のように移動環境を完全に作り込むことは不可能であり、オブジェクトの配置ズレや他の移動物体などを許容した移動制御が必要となる。その一方でスムーズかつ素早い効率の良い移動動作をロボットが行えるようにする要求が高い。
【０００３】
移動体が障害物を回避しながら移動する技術に関しては、従来、障害物を回避しながら目標点に移動する経路の中から最短距離となるルートを見い出す移動体の経路生成法が知られている（特許文献１参照）。この経路生成法は、環境温度マップを作成し、環境温度マップ中に構築された障害物に関する各格子点における温度に基づき各格子点で探索しながら出発点と最終目標点とを結ぶ最短経路を作成し、移動体をこの最短経路に沿って最終目標点まで移動させている。
【０００４】
また、障害物の動きに応じて移動体の移動経路をその場で生成し、動的な障害物がある場合でも適切な回避行動を取れるようにした自走移動体の制御方法が知られている（特許文献２参照）。特許文献２に記載の方法は、障害物が存在する確率を表す確率ポテンンシャル場を生成し、この確率ポテンシャル場に目標位置に向かう勾配を付加し、この勾配が付された確率ポテンシャル場の傾きに基づき目標位置に向かう経路を探索し、自走移動体をこの経路に沿って移動させている。
【０００５】
また、与えられた領域を、できるだけ短時間でほぼ網羅的に走行できるようにし走行中のハンチングを防止可能とした自律走行ロボットも提案されている（特許文献３参照）。特許文献３に記載のロボットは、左右の障害物センサを設け、これらのセンサが検出した障害物までの距離に基づき検出距離が長い側に旋回動作を行うと共に、この旋回動作後の予定時間内に再び先の旋回動作とは逆方向に旋回する動作が必要になった場合、先の旋回時よりも旋回角を小さくするようにしている。
【特許文献１】特開２００１−１５４７０６号公報
【特許文献２】特開２００３−２４１８３６号公報
【特許文献３】特開平１０−２２８３１５号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、特許文献１に記載の経路生成法では、既知である地図を使用した経路計画をベースにした移動体の障害物の回避動作がノイズに非常に弱い。ノイズとは、地図に登録されていない障害物や、理想値（地図情報）と現実値とのズレ、及びデッドレコニング誤差である。また特許文献１に記載の技術では、位置が時々刻々と不規則に変化する移動体への制御の対応が難しい。
【０００７】
特許文献２に記載の自走移動体の制御方法は、移動体と障害物又は移動目標との間に仮想的な斥力あるいは引力などを生成する力場を定め、これらの力の合力から移動体の進行方向を決定するポテンシャル法である。この制御方法では、複雑な地形や多数の障害物に対する扱いが難しい。経路の精度を高めようとすると障害物を登録するためのグリッドマップのメッシュを細かくする必要があり、計算コストが爆発的に高くなる。
【０００８】
特許文献３に記載の自律走行ロボットはセンサ情報を移動指令に直結させる手法を用いたものである。この自律走行ロボットでは、きめ細かな動作指令をロボットへ与えることが難しく、ロボットは大まかで単調なぎこちない動作になりやすい。滑らかで効率の良い動作を実現することが難しい。
【０００９】
そこで、本発明は、上記の課題に鑑み、計算負荷が低く、移動がスムーズであり環境の動的変化などノイズに強いロバストな移動動作が可能な障害物回避機能を有する移動制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１０】
このような課題を解決するため、本発明の一態様によれば、移動体と、この移動体に設けられ、移動体進行方位を検出する検出手段と、前記移動体に設けられ、障害物の位置およびこの障害物までの実計測距離を計測し、この検出手段が検出した前記移動体進行方位に対する前記障害物の方位角を出力する計測手段と、この計測手段が出力した前記障害物の方位角の大きさに応じて、前記実計測距離を変化させて疑似距離を生成する疑似距離生成手段と、この疑似距離生成手段が生成した前記疑似距離に基づき、前記移動体が前記障害物を回避するよう前記移動体の動作を制御する制御手段と、備えることを特徴とする障害物回避機能を有する移動制御装置が提供される。
【００１１】
また、本発明の別の一態様によれば、移動空間に関する外部環境情報を保存し更新可能な記録手段と、この記録手段に記録された前記外部環境情報をもとに目的地点までの適切な経路を生成可能な経路生成手段と、この経路生成手段により生成された前記経路を辿るような速度出力指令値を生成する移動速度出力生成手段と、この移動速度出力生成手段が生成した前記速度出力指令値に対して、速度出力補正を行う請求項１〜１３に記載の疑似距離生成手段と、を備えることを特徴とする障害物回避機能を有する移動制御装置が提供される。
【発明の効果】
【００１２】
本発明によれば、計算負荷を大幅に低減しつつも不要な回避動作を抑制した滑らかな回避軌道を生成することが可能となる。また時々刻々と変化する動的環境にも対応した回避動作を実現できるようになる。
【発明を実施するための最良の形態】
【００１３】
以下、本発明の実施の形態に係る障害物回避機能を有する移動制御装置について、図１乃至図１８を参照しながら説明する。尚、各図において同一箇所については同一の符号を付すとともに、重複した説明は省略する。
【００１４】
本発明の実施の形態に係る障害物回避機能を有する移動制御装置は、独立二輪駆動型の移動ロボットに搭載される。図１（ａ）は移動ロボットの上面図であり、図１（ｂ）は図１（ａ）の矢印Ｊ方向から見たこの移動ロボットの正面図である。これらの図中で同じ符号を有する要素は互いに同じ要素を表す。
【００１５】
ロボット１は、基台２と、それぞれこの基台２の左右両側部に左右平行に同軸的に配置され互いに独立して駆動可能にされた一対の駆動輪３、４と、それぞれが基台２の下面に設けられ駆動輪３、４を回転させる一対のモータ５、６と、基台２の前方中央部に取り付けられた前方キャスタ７と、基台２の後方中央部に取り付けられた後方キャスタ８と、それぞれ基台２上面の四隅に設けられ被測定対象との間の距離を測定する４つのレーザレンジファインダ（ＬＲＦ）９−１２と、基台２に設けられモータ５、６の回転駆動制御を独立して行うコントローラユニット１３（本実施形態に係る障害物回避機能を有する移動制御装置）とを備える。コントローラユニット１３には、ＣＰＵ、ＲＯＭ及びＲＡＭが用いられる。
【００１６】
更にロボット１は、それぞれ駆動輪３、４のシャフトに設けられた一対の回転角度センサ１４、１５と、基台２の並進移動量を検出するための位置センサ１６と、基台２の垂直軸線周りの旋回角度を検出する姿勢センサ１７とを有する。回転角度センサ１４、１５にはポテンショメータやロータリーエンコーダが用いられる。位置センサ１６には慣性センサが用いられる。姿勢センサ１７には、垂直軸線周りの角速度を検出するジャイロセンサが用いられる。これらの回転角度センサ１４、１５の各センサ値と、位置センサ１６のセンサ値と、姿勢センサ１７のセンサ値とはいずれもコントローラユニット１３へ入力されるようになっている。
【００１７】
基台２は矩形状の平板である。モータ５、６は基台２に固定されており、各モータ軸を介して駆動輪３、４と連結されている。駆動輪３、４の各車輪面は互いに平行にされた状態でこれらの駆動輪３、４は正方向及び逆方向に回転自在である。駆動輪３、４と前方キャスタ７及び後方キャスタ８とによって基台２が床面に対して床面から車輪径で決まる高さで水平に保持される。コントローラユニット１３によるモータ５、６に対する駆動制御によって、駆動輪３の回転速度と駆動輪４の回転速度とは互いに同じにされ、あるいは異なるようにされている。モータ５、６が駆動制御されることによって、ロボット１の走行及び旋回が可能にされている。モータ５、６の各回転方向と各回転速度とが制御されることによって、ロボット１は床面内を移動可能になっている。
【００１８】
レーザレンジファインダ９−１２は平面走査型の距離計測手段である。レーザレンジファインダ９−１２はいずれも図示しないレーザ光源と、回転軸を有し外周面上に多面鏡を設けたレーザ光反射体と、レーザ光反射体を回転させる駆動モータと、レーザ光を変調する変調手段と、被測定対象によって反射されたレーザ光を受光する受光器と、電子回路部と、これらのレーザ光源等を収容する筐体とを有する。この駆動モータのモータ軸の回転角に応じて、筐体外部の走査空間を走査する走査光がレーザ光より生成されるようになっている。レーザ光源及び変調手段は障害物ないしは目標物体といった被測定対象に対してパルス変調したレーザ光を照射し、受光器は被測定対象により反射された反射光を受光し、電子回路部は変調光を送出した時間とこの反射光が戻ってきた時間とから、ロボット１と被測定対象物との間の距離の情報を出力する。
【００１９】
レーザレンジファインダ９は、基台２の前方に存在する物体までの距離とその物体からの反射光の反射強度とを、角度ステップ毎に計測し、反射光強度の計測結果から計算した距離を図示しないメモリに記憶する。このメモリには角度毎の距離値が記憶される。角度毎の距離値は更新可能及び補正可能である。レーザレンジファインダ１０−１２の構成もレーザレンジファインダ９の構成と同じである。レーザレンジファインダ９−１２はロボット１の周囲の全方位を死角なく走査可能に配置されている。
【００２０】
図２はレーザレンジファインダ９−１２の計測可能領域を示す図である。コントローラユニット１３がこれらのレーザレンジファインダ９−１２を制御することによって、レーザレンジファインダ９−１２はロボット１の周囲の空間を走査する。各レーザ光源からのレーザ光の送出方向は床面と平行である。また、これらのレーザレンジファインダ９−１２内の受光器の感度は略水平方向にされてある。基台２の四隅に設けられた各レーザレンジファインダ９−１２がそれぞれ上面視扇形のレーザレンジファインダ計測可能領域１８を走査する。
【００２１】
コントローラユニット１３は、４つのレーザレンジファインダ９−１２から出力される角度毎の距離値に基づき、床面上の障害物を含む外部環境の距離画像を生成する。コントローラユニット１３は、ロボット１の３６０°全方位に亘って障害物が存在しているかどうかを検出するようにしている。
【００２２】
また、コントローラユニット１３はモータ５、６の角速度及び角加速度を制御する。ロボット１の周囲に障害物が存在することをコントローラユニット１３が検知した場合、このコントローラユニット１３は後述するように障害物を回避するための演算処理を行う。コントローラユニット１３は、センサ類からの情報に基づきロボット１の並進移動量や旋回量を算出し、モータ５の角速度及び角加速度の目標値と、モータ６の角速度及び角加速度の目標値とを生成する。コントローラユニット１３は、生成したモータ５、６の各回転量の目標値から、これらのモータ５、６の図示しないドライバに対して電流制御を行うようにしている。
【００２３】
図３はコントローラユニット１３のブロック図である。コントローラユニット１３は、ロボット１が走行すべき経路を生成する経路生成部１９と、経路生成部１９からの出力に基づいて制御周期ごとに基台２のｘ、ｙ座標系での目標位置及び垂直軸線周りの目標回転位置を生成する目標軌道生成部２０と、目標軌道生成部２０からの出力及び基台２に取り付けられたセンサ類からのセンサ値を比較し、演算した結果を出力する演算部２１と、演算部２１から出力される演算結果の差分を埋めるように基台２の並進速度の目標値及び旋回速度の目標値を計算するロボット移動速度指令値生成部２２とを備える。
【００２４】
経路生成部１９は、後述する図４に示すような障害物マップを作成する。目標軌道生成部２０は、各制御周期でのロボット目標位置ｘr［ｍ］、ｙr［ｍ］及びロボット目標姿勢θ［ｒａｄ］を生成して出力する。演算部２１は、これらのｘr、ｙr及びθと、センサ類からの現在位置ｘ［ｍ］、ｙ［ｍ］とを比較する。ロボット移動速度指令値生成部２２は、演算部２１からの比較結果に基づき並進速度目標値Ｖr［ｍ／ｓ］及び旋回速度目標値ωr［ｍ／ｓ］を生成して出力する。
【００２５】
更にコントローラユニット１３は、ロボット移動速度指令値生成部２２からの出力を補正する障害物回避速度指令値補正部２３と、この障害物回避速度指令値補正部２３からの出力に基づいて各軸の回転速度の目標値を生成して出力する車軸回転速度指令値生成部２４と、車軸回転速度指令値生成部２４からの出力及び基台２のセンサ類からのセンサ値を比較演算した結果を出力する演算部２５と、演算部２５から出力される演算結果に基づいてモータ５、６へ与える電流の目標値を生成するモータ制御電流指令値生成部２６と、ロボット本体２７とを備える。ロボット本体２７は、駆動輪３、４及び回転角度センサ１４、１５、位置センサ１６及び姿勢センサ１７からなる。
【００２６】
障害物回避速度指令値補正部２３は、並進速度目標値Ｖrを補正して補正並進速度目標値Ｖr′［ｍ／ｓ］を出力する。障害物回避速度指令値補正部２３は、旋回速度目標値ωrを補正して補正旋回速度目標値ωr′［ｍ／ｓ］を出力する。車軸回転速度指令値生成部２４は各軸回転速度目標値φnr［ｒａｄ／ｓ］を生成して出力する。モータ制御電流指令値生成部２６は、各軸モータ電流指令値ｉnr［Ａ］を出力する。この各軸モータ電流指令値ｉnrは、モータドライバへの電圧の指令値Ｖn［Ｖ］にほぼ等しい。
【００２７】
ロボット１は移動体である。ロボット１は移動手段を有し、またこの移動手段を動作させる駆動手段を有する。移動手段は駆動輪３、４であり、駆動手段はモータ５、６である。コントローラユニット１３は本実施形態に係る障害物回避機能を有する移動制御装置である。障害物回避制御方法は、この移動体に関する制御手法である。コントローラユニット１３は目標地点を設定するという上位処理を行う。移動体は移動体自身の内部に持つ上位概念のルールに従い目標地点が設定される。移動体とは、目標地点へ到達するための並進速度指令値及び旋回速度指令値を生成するロボット１のような自律移動体を含む。あるいは、移動体とは、アシストカートなどのように外部入力を指令値としてこの指令値を実現するように駆動手段を動作させる半自律移動体なども含む。また移動体は移動体周囲の物体の配置状況を認知可能な認知手段を有しており、これにより移動体の周囲の全方位の障害物情報を知ることが可能となっている。
【００２８】
ここでは図１に示すような移動型ロボットを代表例として用いる。ロボット１は、モータ５、６により駆動される左右の駆動輪３、４を有する独立二輪駆動方式である。これら二輪の回転量で直進及び旋回の動作が定まる。またロボット１にはレーザレンジファインダ９−１２が図２のように死角なく複数例えば４個配置され、全方位の障害物までの距離をその方向とともに高精度で計測することが可能となっている。計測された方位角及び距離といった障害物情報は直ちにメモリ内で更新され、更新された障害物情報は制御に反映される。
【００２９】
図２ではレーザレンジファインダ９−１２が計測する距離方向は途中までしか描画されていないが、実際にはさらに遠方まで計測可能である。レーザレンジファインダ９−１２単体が計測可能な方位も特に図２の例に指定するものではなく、レーザレンジファインダ搭載個数も特に図２の数に限定するものではない。ロボット１の移動方法は本例の独立二輪駆動方式に限るものではなく、少なくともロボット１自身の移動方向と移動量とがホロノミック又はノンホロノミックの区別無く任意に指定できればよい。例えばステアリング方式や、４輪が駆動する全方向移動車、また二足歩行のような車輪以外の移動手段が用いられてもよい。
【００３０】
またセンサ系に関しても特に本例のレーザレンジファインダ方式に限るものではなく、配置位置が異なるものや、指向性のある超音波センサやＰＳＤ（Ｐｏｓｉｔｉｏｎ Ｓｅｎｓｉｔｉｖｅ Ｄｅｔｅｃｔｏｒ）センサを周囲に配置したものを用いてもよい。ＰＳＤセンサは、受光面を有しこの受光面に入射するビームスポットの位置に応じて差動的に変化する一対の出力信号を出力するＰＳＤ素子と、このＰＳＤ素子により検出された光位置から距離を演算する演算手段とからなる。また、これらのセンサ情報をロボット１の周囲の情報を予め記録した地図上に一度集約して、制御周期毎にこの地図より擬似的にロボット１の周囲の状況を読み出してもよい。さらにこの地図に環境カメラや他ロボットなど外部システムからも位置同期を取りつつ障害物配置をリアルタイムで登録し、削除するようにしてもよい。
【００３１】
本例でのコントローラユニット１３は図２のように配置されたレーザレンジファインダ９−１２のそれぞれから得られる距離情報をマップ上に統合する処理を行う。コントローラユニット１３は、既知である幾何配置情報をもとに一つの座標系で表される地図上で各距離情報を統合管理している。この障害物マップ情報より、ロボット１の指定方向の指定角度範囲内に存在する障害物のうちの最も近い障害物までの距離が得られるようになっている。
【００３２】
センサとしてのレーザレンジファインダ９−１２のサンプリングレートは、ロボット１自身の移動および他移動物体による外界の動的変化に対し相対的変化が連続的に捉えられる程度に十分高速なものが用意される。また指定角度最小範囲の分解能についても、規定の距離内に存在する外界障害物の相対的な位置変化が連続的に捉えられる程度の適切な分解能が設定されている。ここでの連続的とは、対象物の位置を計測するステップの後、観測対象物が計測した次ステップにおいて前回計測された位置より規定の距離範囲内に存在することを意味している。この分解能の値は用途に応じ変化するので、一意に定めるものではない。
【００３３】
図４はコントローラユニット１３が作成する障害物マップの一例を示す図である。同図中、上述した符号と同じ符号を有する要素はそれらと同じ要素を表す。２８から３２はいずれも障害物ないしは物体を表す。３３は移動体進行方位である。ロボット１から見て障害物２９の方位が計測指定方位である。これらの移動体進行方位及び計測指定方位が成す角度φは基準方位である正面方向に対する方位φである。５７は計測指定方向の最短距離に存在する障害物の位置を表す。
【００３４】
本例では障害物マップは、図４のようなロボット１を中心とした相対座標系において、計測指定方位として指定された方位φと、その方向の最短距離ｒとにより極座標表現される。障害物マップは、極座標以外の座標を用いた表記や、絶対座標などを用いても特に構わない。また本例では、方位φの基準方向はロボット正面としており、半時計回り方向を正としている。この基準方向や旋回方向に関してもロボット正面や半時計回り方向に特に限定されるものではない。
【００３５】
障害物マップ上での実計測距離と疑似距離との関係を、図５及び図６に示す。図５は移動体が前進する時の疑似距離の概念を示す図である。３４は障害物ないしは物体の実位置を表す。３５は障害物ないしは物体の疑似位置を表す。レーザレンジファインダ９はロボット１と、実障害物位置３４との間の距離を実測し実計測距離ｒを取得する。レーザレンジファインダ９は、移動体進行方位と計測指定方位とが成す角度θ1を取得する。コントローラユニット１３は、実計測距離ｒを疑似距離ｒ′に置き換える処理を行うことによって、障害物マップ上で、実障害物位置３４を疑似障害物位置３５へ置き換えるようにしている。置き換える処理とは、関数ｆを用いてｒ′＝ｆ（ｒ、θ1）を演算することを指す。ＣＰＵは、関数ｆの計算手順が記述されたプログラムをＲＯＭから読み込み、ＲＡＭを用いてこの計算手順を実行する。
【００３６】
また、ロボット１が後進する時の例を図６に示す。レーザレンジファインダ９はロボット１と、実障害物位置３４との間の距離を実測し実計測距離ｒを取得する。レーザレンジファインダ９は、移動体進行方位と計測指定方位とが成す角度θ2を取得する。この角度θ2の大きさは、図５の例の角度θ1よりも大きい。コントローラユニット１３は、ｒ′＝ｆ（ｒ、θ2）を演算することによって、障害物マップ上で、実障害物位置３４を疑似障害物位置３６へ置き換えるようにしている。
【００３７】
即ち本発明では図５に示すように、コントローラユニット１３は移動体進行方向と計測指定方向とが成す角度θ1の大きさに応じて計測指定方向の距離値が真の計測値（以下「真値」）から変化する疑似距離を用いる。本例での独立二輪駆動方式でロボット１が前進している場合、移動体進行方向と計測方位基準方向とが一致するので、この場合θ1＝φとなっている。ロボット１が後進する場合、移動体進行方向、計測指定方向それぞれの角度関係は図６のようになる。
【００３８】
さらに図７は角度θと疑似距離ｒn′との関係を説明するための図である。５８は疑似距離を表す。５９は真の距離である。本例では図７に示すように移動体進行方向と計測指定方向とが成す角度θnの大きさが大きくなるほど疑似距離ｒ′は真値ｒよりも大きな値を取るように、コントローラユニット１３は関数ｆの計算手順を実行する。角度θnが大きくなるほど、疑似距離が真値から乖離する量が大きくなる。
【００３９】
図８に同一地点での前進及び後進の進行方向の切り替わりにより周囲環境の障害物位置が変化する疑似障害物マップの概念図を示す。ここで図８（ａ）はロボット１が前進する時の疑似障害物マップの一例であり、図８（ｂ）は図８（ａ）のロボット１の地点と同じ地点でロボット１が後進する時の疑似障害物マップの一例である。また、図９にロボット１の姿勢変更後の移動により周囲環境の障害物位置が変化する疑似障害物マップの概念図を示す。図９（ａ）は姿勢を変更した後のロボット１が前進する時の疑似障害物マップの一例であり、図９（ｂ）は図９（ａ）のロボット１の地点と同じ地点での姿勢変更後のロボット１が後進する時の疑似障害物マップの一例である。３７から４０は障害物ないしは物体を表す。６０は疑似位置であり疑似障害物マップ上での障害物位置を表す。６１は真の位置である。
【００４０】
ここで疑似距離により形成されるロボット１の周囲環境の障害物位置を表示した疑似障害物マップ（以下、障害物マップ）は、図８（ａ）、図８（ｂ）で表されるように同一の地点であっても前進及び後進の動作の切り替わりで状況が一瞬のうちに変化するという特徴がある。また図９（ａ）、図９（ｂ）で表されるように、ロボット１が独立二輪駆動方式であっても、ロボット１がその場で旋回動作を行い、絶対座標系上でのロボット１自身の正面方向（以下「姿勢」と同義）を変化させた場合、その後の前後の移動時に得られる障害物マップはそれ以前のものとは異なるものとなる。
【００４１】
移動体は本例とは異なる移動方式を用いてもよい。移動体には全方向移動車が用いられてもよい。全方向移動車は、任意のタイミングで任意の方向に速度出力を生成可能な移動手段を持つ移動機構である。移動体として全方向移動車が用いられる場合の障害物マップを図１０に示す。図１０（ａ）から図１０（ｄ）には全方向移動車１Ａの同一姿勢状態での移動方向の切り替わりにより周囲環境の障害物位置が変化する疑似障害物マップが示されている。斜線付きの丸は疑似障害物マップ上での障害物位置を表す。白色の丸は真の位置を表す。
【００４２】
全方向移動車１Ａは、基台２Ａと、この基台２Ａの前後左右の４箇所に枢支された駆動輪４１と、この基台２Ａ上に設けられた図示しない本実施形態に係る障害物回避機能を有する移動制御装置とを有する。これら以外の符号で上述した符号と同じ符号を有する要素はそれらと同じ要素を表す。具体的には図１０（ａ）は全方向移動車１Ａの前進時の疑似障害物マップであり、図１０（ｂ）は同じ地点での後進時の疑似障害物マップである。図１０（ｃ）は姿勢変更後の全方向移動車１Ａの前進時の疑似障害物マップであり、図１０（ｄ）は同じ地点での姿勢変更後の全方向移動車１Ａの後進時の疑似障害物マップである。図１０で表されるように移動指令方向により障害物マップが表す周囲環境状況は、同一の地点で同一の姿勢を有していても異なる結果を出力することになる。全方向移動車１Ａの移動制御装置はロボット１のコントローラユニット１３と同じである。
【００４３】
以下、ロボット１に搭載されるコントローラユニット１３が行う疑似距離の生成処理について述べる。ここでは障害物計測方位角θに対応して、真値ｒを疑似距離ｒ′へと変換する『行為』を疑似距離『変換』と呼ぶことにする。本例では（式１）のような疑似距離変換を適用して疑似距離を生成している。
【数１】
【００４４】
ここでｌ，ｍ，ｎは任意の実数とし、ｋは０＜ｋ＜２の実数とし、コントローラユニット１３は（ｋ＝１，ｌ＝１，ｍ＝２，ｎ＝１）を一組として疑似距離を生成するようにしている。コントローラユニット１３は進行方向と同方向（即ち独立二輪駆動方式では正面方向）に位置する疑似距離の値については真の距離値と同じ値を出力する。コントローラユニット１３は、移動進行方向に対し真横方向に位置する疑似距離の値については真の距離値の２倍の距離を出力する。コントローラユニット１３は、進行方向（θが０度の方向）と真横方向（θが９０度の方向）との間の範囲内の所望の方向に位置する疑似距離の値については、（式１）に代入して得られる値を出力する。つまりこれらの２つの方向の間の遷移は余弦関数的に遷移していく。またコントローラユニット１３は、真横方向よりもθが大きい方向である後方は無限遠とすることで、真横方向よりも後方に位置する方向に存在する障害物を、障害物回避の考慮対象から除外するようにしている。
【００４５】
また、コントローラユニット１３は、正面方向の疑似距離の値を、真値と異なる値に設定してもよい。コントローラユニット１３は正面方向の疑似距離の値を真値から変える場合、ｌの値を変更する。コントローラユニット１３は、無限遠を除く最大値を変える場合、ｍの値を変更する。コントローラユニット１３は、正面方向について設定した値から最大値に至るまでの余弦関数の遷移形状を変化させる場合、ｎの値を変更すればよい。またコントローラユニット１３はｋの値を変更することで疑似距離が最大地点（無限遠を除く）となる角αの位置を真横から変えるようにもできる。ただし本例が提示する（式１）は一例にすぎず、コントローラユニット１３は進行方向背後の障害物距離を無限遠とはせずに有限値としてもよい。さらにはコントローラユニット１３は、他の関数を使用して疑似距離変換をデザインしても何ら問題はなく、これらは請求項に記載の本発明の範囲内である。
【００４６】
ロボット１は、上記疑似距離ｒ′を参照し、周囲環境が未だ考慮されていない段階で生成した移動動作指令を補正するようにしている。以下、本補正処理を障害物回避と呼び、補正処理された移動動作を障害物回避動作と呼ぶ。
【００４７】
ロボット１は障害物回避動作にあたり、ロボット１自身の周囲に存在する障害物についての複数の疑似距離を参照し、これらの疑似距離のうち最短の疑似距離を入力として使用するようにしている。図１１は回避対象の障害物の位置をコントローラユニット１３が決定する手法を説明するための図である。同図には、ロボット１の周囲に障害物（物体）４２〜４４が存在しており、コントローラユニット１３は、疑似障害物マップ上に複数の障害物位置を予め生成する。斜線付きの丸は障害物位置を表す。白色の丸は真の位置を表す。コントローラユニット１３は、これらの障害物位置の間で疑似距離を比較し、最短の疑似経路を有する障害物位置を検索する。複数の障害物位置のうちの４５を付された障害物位置をコントローラユニット１３が判断すると、コントローラユニット１３はこの障害物位置４５を回避対象障害物位置と決定する。即ち、図１１で表されるように、コントローラユニット１３は、最短疑似距離４６を有する方位，疑似距離が指す位置を回避対象障害物位置とする処理を行う。この地点を回避対象障害物と呼ぶこととする。
【００４８】
従来の障害物回避手法では、コントローラが回避軌道を生成するにあたり、コントローラは障害物までの距離とその方向との２成分を考慮しなくてはならなかった。しかし、本実施例による障害物回避制御手法では疑似距離にその方向成分によりもたらされる効果も含まれているので、特例的な処理を除き、マップ生成後のコントローラユニット１３の軌道生成作業ではコントローラユニット１３は基本的には疑似距離のみを参照すればよい。
【００４９】
本例で示すロボット１は疑似距離を入力として障害物回避のための速度出力方向と速度量との２成分を出力するようにしている。従来例のポテンシャル手法はコントローラが１つのポテンシャル関数から強度とその勾配とを求めこれらの強度及び勾配を速度量と速度出力方向とに当てはめたものである。これに対して、コントローラユニット１３は２成分の値を算出するにあたり、２つの独立した生成関数を使うようにしている。以下の説明では、これらの２つの独立した生成関数のそれぞれを速度方位ポテンシャル，速度量ポテンシャルと呼ぶこととする。なおここでの「ポテンシャル」という呼称は便宜上用いるものであり、一般的なポテンシャル関数のように導関数が特別な意味を持つわけではない。
【００５０】
速度方位ポテンシャルについて図１２から図１５を参照して説明する。図１２は速度方位ポテンシャルの一例を示す図である。横軸は疑似距離ｒ′であり、縦軸は並進速度出力許容方位角ξである。図１２で表されるような形状を有する速度方位ポテンシャルを、コントローラユニット１３は予め生成してコントローラユニット１３内のメモリ（記憶手段）に記憶しておく。コントローラユニット１３は予め横軸と縦軸との各値を取得する処理を実行し、速度方位ポテンシャルを生成しておく。
【００５１】
ここでの並進速度出力許容方位角について図１３及び図１４を参照して更に述べる。図１３は並進速度出力許容方向と回避対象障害物方向との関係を示す図である。回避対象障害物方向４７は回避対象障害物４８の存在する方向である。並進速度出力許容方向とは図１３の４９で表される。並進速度出力許容方向４９は、回避対象障害物方向４７に向けることが可能な移動速度出力の限界方向を指す。移動体基準位置５０（ここでは移動平面に投影したロボット重心とする）を頂点とし、回避対象障害物方向４７と並進速度出力許容方向４９とが成す角ξを並進速度出力許容方向角と呼ぶこととする。この並進速度出力許容方向４９は、基本的には回避対象障害物方向４７から移動速度出力方向に向けて展開される。
【００５２】
また、図１３の回避対象障害物方向４７と並進速度出力許容方向４９とが成す角ξは０からπ／２までの範囲を取りうる。コントローラユニット１３は、ξが０のとき任意の方向へ移動速度を出力するようにする。これに対し、ξの値が増加するに伴いコントローラユニット１３は回避対象障害物方向４７への移動速度出力を制限する。コントローラユニット１３が制限をかけるため、ξがπ／２になると、コントローラユニット１３は回避対象障害物方向４７に対し垂直方向もしくは回避対象障害物４８から遠ざかる方向にのみ速度出力を行うようにしている。
【００５３】
図１４は移動体の移動速度の出力方向の補正処理を説明するための図である。図１４のように移動速度出力方向５１が回避対象障害物方向５２と並進速度出力許容方向５３との間に存在する場合、ロボット１は移動速度出力方向５１を並進速度出力許容方向５３にまで補正変更するようにしている。
【００５４】
図１１の速度方位ポテンシャルより分かる通り、疑似距離ｒ′がある値ｒmax（第１閾値）以上である場合、コントローラユニット１３は並進速度出力許容方位角ξの値を０にする。コントローラユニット１３は速度出力方向については特別な制限をかけない。コントローラユニット１３により制限を受ける速度方位ポテンシャルの特性は、疑似距離ｒ′がｒmax以下になると徐々にξの値が増加し、疑似距離ｒ′がある値ｒmin（第２閾値）以下ではξの値はπ／２に飽和するような形状をとる。ここでｒmaxの値は「ロボット１がどれくらい回避せずに障害物方向にまで接近を許容するか」という回避開始距離を定義する値である。ｒminの値は「ロボット１が最悪どのくらいの距離まで障害物への接近を許容するか」という限界接近距離を定義する値である。
【００５５】
また、全方向移動車１Ａのように任意の時刻に任意の並進移動速度を生成可能な移動体については、コントローラユニット１３は補正処理した移動速度出力方向に自身の移動速度出力を設定する処理を行うだけでよい。移動体が本例のような独立二輪駆動型の場合この移動体の動作はノンホロノミックな移動拘束条件下での動作となるため、コントローラユニット１３は補正処理した移動速度出力方向に移動速度を生成することができない。このような場合、コントローラユニット１３は移動体の姿勢を変更するようにし、移動速度の出力方向が補正処理方向にまで向くようにロボット１を旋回動作させる。コントローラユニット１３は旋回のための車軸回転速度が生成されるように、回転動作補正処理を実行する。
【００５６】
またコントローラユニット１３が参照する速度方位ポテンシャルは、図１５のように疑似距離と並進速度出力許容方位角ξとの関係が移動体の移動速度に応じて変化するような特徴を有している。図１５は、移動体の移動速度に応じて異なるポテンシャル形状を有する速度方位ポテンシャルの一例を示す図である。この速度方位ポテンシャルはコントローラユニット１３のメモリに記憶されている。５４は移動体の移動速度が速い場合に適用される速度方位ポテンシャルである。５５は移動体の移動速度が遅い場合に適用される速度方位ポテンシャルである。５６は通常の速度方位ポテンシャルを表す。
【００５７】
速度方位ポテンシャル５４は、最小疑似距離ｒmin及び最大疑似距離ｒmaxがそれぞれ通常の速度方位ポテンシャルの最小疑似距離及び最大疑似距離よりも増加したポテンシャル形状を有する。速度方位ポテンシャル５５は、最小疑似距離ｒmin及び最大疑似距離ｒmaxがそれぞれ通常の速度方位ポテンシャルの最小疑似距離及び最大疑似距離よりも減少したポテンシャル形状を有する。コントローラユニット１３は予めこれらの速度方位ポテンシャル５４、５５を作成してメモリに設定しておく。これにより指定移動速度が速い場合、障害物に対しなるべく遠い位置から徐々に回避動作を行うことで安全性を高めることを可能とし、指定移動速度が遅い場合、安全性の観点からもある程度の障害物までの接近は許容され、その分指定軌道に沿うように移動させることが可能となる。また高速移動時及び低速移動時のそれぞれにおける最小疑似距離ｒmin，最大疑似距離ｒmaxの間の変化幅に限界値を設けておき、コントローラユニット１３はある程度の段階で飽和するような処理をかけるようにしてもよい。
【００５８】
また、移動速度出力方向が回避対象障害物方向と並進速度出力許容方向との間に存在する場合、コントローラユニット１３は原則として並進速度出力許容方向へ向けて移動速度出力方向を変更する。このため、移動速度出力が回避対象障害物方向へ向くような速度変更や姿勢変更は禁止される。移動速度出力方向が回避対象障害物方向と並進速度出力許容方向との間よりも外側に存在する場合でも、移動速度出力方向が並進速度出力許容方向の近傍に存在する場合、コントローラユニット１３は移動速度出力方向が回避対象障害物方向へ向くような速度変更や姿勢変更を禁止する。これにより並進速度出力許容方向を挟んでの移動速度出力や姿勢変更のチャタリング的な振動動作が抑制される。
【００５９】
以上は速度方位ポテンシャルについての説明である。以下、速度量ポテンシャルについて図１６を参照して説明する。図１６は速度量ポテンシャルの一例を示す図である。横軸は疑似距離である。縦軸は速度許容率γ（０≦γ≦１）である。速度許容率は、速度量ポテンシャルにより制限された状態で移動体が最終的に出力可能な並進速度の最大値を、障害物が無い状態で移動体が本来出力可能な並進速度の最大値で除して得た比率である。この比率は擬似距離および移動体自身の大きさに応じて定まる。移動体であるロボット１が最終的に出力可能な速度最大値は、この比率にロボット１が本来出力可能な速度最大値を乗じて得られる。ここで障害物距離により制限を受ける値は許容出力速度の『最大値』であり、出力速度が直接制限を受けるものではない。このため、速度出力の指令値が最終的に出力可能な速度最大値よりも小さい場合、何の補正も受けることなくそのまま指令された値の速度が出力される。また、速度出力の指令値が本来出力可能な速度最大値より小さい値であっても、最終的に出力可能な速度最大値よりも大きい場合、このときの出力は最終的に出力可能な速度最大値に制限される。
【００６０】
コントローラユニット１３は予め横軸と縦軸との各値を取得する処理を実行し、速度量ポテンシャルを生成しメモリに記憶しておく。ここでの速度許容率γは、（式２）のように表現される。
【数２】
【００６１】
速度許容率γは、許容最大移動速度量ｖmaxをｖmax′に変換するという演算上の効果がある。変換後の許容最大移動速度量ｖmax′を変換許容最大移動速度量と呼ぶこととする。移動体の並進移動目標速度量|ｖr|がこの変換許容最大移動速度量以下である場合、コントローラユニット１３は特別な処理を発生させない。並進移動目標速度量|ｖr|が変換許容最大移動速度量を超える場合、コントローラユニット１３はそのスカラ量がｖmax′になるまで並進移動目標速度を圧縮補正する。ここで補正後の並進移動目標速度を補正並進移動目標速度ｖr′と呼ぶこととし、（式３）のように表される。
【数３】
【００６２】
図１６から分かる通り、速度許容率γは疑似距離ｒ′が大きいときには１となる。疑似距離ｒ′が閾値ｒv max（第３閾値）以下では疑似距離ｒ′の減少にともない速度許容率γも減少傾向となり、疑似距離ｒ′が閾値ｒv min（第４閾値）以下では速度許容率γは０となる。この速度量ポテンシャルに基づくコントローラユニット１３の処理の内容は、移動体が障害物に対し衝突コースにない場合、即ち移動体が回避動作対象外の移動を行っている場合、コントローラユニット１３は最大速度制限をかけずに、任意の速度を出力させ続ける。回避対象障害物が移動体に近接する場合、コントローラユニット１３は最大速度に制限をかける。最終的にはコントローラユニット１３は、閾値ｒv minで表現されるある距離以内には移動体が接近しないよう並進速度の出力を完全に遮断するようにする。これにより障害物との衝突を防ぐことが可能となっている。また速度出力方向もしくは姿勢角をコントローラユニット１３が変更すると、それに応じて疑似距離も変化し、これにともない速度許容率γも変化することになるので、再び移動体は並進速度出力可能状態にされる。
【００６３】
本例では速度方位ポテンシャルおよび速度量ポテンシャルを直線で表現したが、これらは請求項に記載の本発明を実現するための一例にすぎない。三角関数やｒmin，ｒmaxを変曲点とするような高次関数など他の関数を使用して本例以外の形状のポテンシャル関数をデザインしても何ら問題はなく、これらを変形した発明は請求項に記載の本発明の範囲内である。
【００６４】
以上の処理をコントローラユニット１３が行うことにより、障害物の位置及び距離に応じて、移動体の並進移動速度量と、移動体の速度出力方向もしくは姿勢角速度とに補正が加えられる。結果として移動体の移動経路上に存在する障害物を回避するような速度出力を生成することが可能となる。
【００６５】
上述の構成により、コントローラユニット１３は、障害物回避速度補正処理を行う。図１７はコントローラユニット１３による障害物回避速度補正処理のフローチャートである。コントローラユニット１３は、モータ５、６を制御対象としてこれらのモータ５、６のモータシャフトの回転制御のサーボ周期毎に制御動作を実行する。
【００６６】
障害物の存在を考慮される前の処理であるステップＡ１において、コントローラユニット１３は、並進速度の指令値Ｖと、旋回速度の指令値ωとを入力される。ステップＡ２において、コントローラユニット１３は、並進速度指令値方向に関する最短疑似距離障害物を探索する。レーザレンジファインダ９−１２により障害物が検知されない間、コントローラユニット１３は、Ｙルートを通り、ステップＡ１の処理を行う。レーザレンジファインダ９−１２のいずれかにより障害物が検知されると、Ｎルートを通り、ステップＡ３において、コントローラユニット１３は、並進速度出力許容方向を算出する。
【００６７】
引き続き、ステップＡ４において、コントローラユニット１３は、並進速度指令値ｖの方向が回避対象障害物方向と並進速度出力許容方向との間に存在するかどうかを判定する。コントローラユニット１３は存在すると判定した場合、Ｙルートを通り、ステップＡ５において、現在の姿勢状態で並進速度出力許容方向に速度を出力可能であるかどうかを判定する。ステップＡ５において、出力可能であるとコントローラユニット１３が判定した場合、Ｙルートを通り、ステップＡ６において、コントローラユニット１３は、並進速度指令値ｖが並進速度許容方向になるよう速度値を並進速度指令値ｖ′に補正する。
【００６８】
続いて、ステップＡ７において、コントローラユニット１３は、補正後の並進速度指令値方向に関する最短疑似距離障害物を探索する。ステップＡ８において、コントローラユニット１３は、変換許容最大移動速度量を算出する。ステップＡ９において、コントローラユニット１３は、移動体の並進移動速度量が変換許容最大移動速度量よりも大きいかどうかを判定する。並進移動速度量が変換許容最大移動速度量よりも大きいとコントローラユニット１３が判定した場合、Ｙルートを通り、ステップＡ１０において、コントローラユニット１３は並進移動速度の圧縮補正を行い、ステップＡ１１において、コントローラユニット１３は圧縮補正した速度を出力する。ステップＡ９において、並進移動速度量が変換許容最大移動速度量よりも大きくはないとコントローラユニット１３が判定した場合、Ｎルートを通り、ステップＡ１１においてコントローラユニット１３はそのまま速度を出力する。
【００６９】
また、ステップＡ５において、現在の姿勢状態で並進速度出力許容方向に速度を出力可能ではないとコントローラユニット１３が判定した場合、Ｎルートを通り、ステップＡ１２において、コントローラユニット１３は、並進速度指令値ｖが並進速度許容方向を向くように旋回速度補正候補値ω′を生成する。この場合、並進速度指令値ｖの方位補正をコントローラユニット１３は行わない。続くステップＡ１３において、コントローラユニット１３は、旋回速度指令値ωと旋回速度補正候補値ω′とが同一の回転方向でありかつ|ω|＞|ω′|であるかどうかを判定する。ωとω′とが同一回転方向でありかつ|ω|＞|ω′|である場合、Ｙルートを通り、コントローラユニット１３は、ステップＡ７の処理を行う。また、ωとω′とが同一回転方向ではない場合あるいは|ω|が|ω′|以下である場合、Ｎルートを通り、ステップＡ１４において、コントローラユニット１３は、旋回速度指令を旋回速度補正候補値に変更し、ステップＡ７に進む。
【００７０】
このように、進行方向から障害物が無くなると、ロボット１は移動目標位置に向かって再び速度指令を生成しそれを出力するので目標へ復帰できるようになる。
【００７１】
本例で解説した障害物回避制御手法は、コントローラユニット１３に対しセンサ類からのセンサ入力が発生すると、コントローラユニット１３はこのセンサ入力に対し、直ちに反応して動作補正を行うことが可能な反射的な障害物回避動作と表現することができる。この反射的障害物回避動作を実現する本制御処理を、経路計画に基づく障害物回避移動制御処理の後に実行するように移動体を移動制御するようにもできる。
【００７２】
図１８は移動体に対する階層構造指令伝達型の移動制御方法を説明するためのフローチャートである。この移動体に実装された制御手段は、ステップＢ１において、障害物回避計画軌道を生成する。ステップＢ１における処理では、制御手段は図示しない地図データベースを参照した上で障害物回避計画軌道を生成する。ステップＢ２において、制御手段は、障害物は考慮せずに、並進速度及び旋回速度の出力値を計算する。引き続き、ステップＢ３において、制御手段は、障害物回避速度補正を行う。制御手段はこの障害物回避速度補正に、本例の手法を適用する。ステップＢ４において、制御手段は、補正速度を出力する。モータの回転数が計算されてから出力される。
【００７３】
換言すれば、特許文献１に記載の移動体の経路生成法のような経路計画に基づく障害物回避移動制御フローの下流に、障害物回避速度補正処理フローを配置する。制御手段はまず既知である地形情報に基づき効率的な経路を計画的に生成しつつ移動体に移動を行わせる。既知の情報通りに事が運んでいる間、制御手段は、本例の反射的障害物回避処理を表面化させることなく通常の移動動作指令をそのまま出力して計画通りの理想的な移動動作をおこなう。予期せぬ障害物が出現したときや、あるいは計画時の自己位置の認識誤差または地形データベース側の事前情報と実際の情報との誤差などにより、制御手段が生成した計画軌道では移動体が障害物に接触してしまうような状況に陥ってしまったときに、制御手段が本例の反射的障害物処理を発現させて障害物を回避する速度補正を適切に主処理に割り込ませるようにすることが可能となる。
【００７４】
本提案の障害物回避制御手法を用いることにより、外部環境の予期せぬ乱れに強い安定した移動を実現することが可能となる。安定した移動が実現することは、移動ロボットなどの活動の場を、環境の作り込みでは片付けることのできない一般的な環境下へと大きく広げることを可能にすると考えられる。さらに回避対象障害物を選定することや、移動体自身の回避動作用の指令値などを出力することが連続的な状態量として扱われている。このため、出力される回避動作なども滑らかで効率も良く、また最短疑似距離をもつ１つの回避対象障害物を選定した上で計算が行われるため制御装置にかかる計算負荷も非常に小さく、非常に実用的な手法である。
【００７５】
このように、コントローラユニット１３は、移動体の移動方向からの角の大きさに応じて距離が変化する疑似距離が支配する、障害物と移動体との相対的な姿勢関係により値が変化する異方特性を持った速度・姿勢ポテンシャルより移動状態を用意し、この移動状態の量に周囲障害物までの最短疑似距離を毎時与えることで得られる速度および姿勢により回避動作を実現する制御手法を定める。
【００７６】
このようにして、コントローラユニット１３によれば、計算負荷が低い移動体を得ることができる。また、移動体の移動がスムーズとなり環境の動的変化などノイズに強いロバストな移動動作が可能になる。
【００７７】
尚、本発明は上記実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。上記実施形態では、移動体進行方向と計測対象方向とが成す角度が大きくなるほど、コントローラユニット１３は真値よりも距離が長い疑似距離ｒ′を生成していたが、コントローラユニット１３は、同角度が大きくなるほど、真値よりも距離が短い疑似距離ｒ′を生成するようにもできる。例えばロボット本体の胴部の横断面の形状が真円でなく、断面形状中の右前方向の部位がロボット本体よりも外方へ突出して形成されている場合、この右前方向の部位は障害物に接触しやすい。あるいは移動体の筐体が構造的に弱い部位を有し、この構造的に弱い部位が障害物と接触するリスクを減らすように設計することがある。これらの障害物に接触しやすい右前方向の部位や、構造的に弱い部位を保護するためには、右前方向で最短となるようにする（正面方向から右前方向へ向かうにつれて一度疑似距離が小さくなる）処理をコントローラユニット１３が行うようにする。速度方位ポテンシャルや速度量ポテンシャルをメモリに記憶する際に、右前方向の部位や、構造的に弱い部位が存在する方位で疑似距離が小さくなるようなポテンシャル形状を有する速度方位ポテンシャルや速度量ポテンシャルをメモリに書き込むのである。コントローラユニット１３がこれらのポテンシャルをメモリから読み込むことによって、コントローラユニット１３は、移動体進行方向と計測対象方向とが成す角度が大きくなるほど、真値よりも距離が短い疑似距離ｒ′を生成することができる。
【００７８】
また、コントローラユニット１３は、障害物の方位角の大きさに応じて、実計測距離を変化させて疑似距離を生成する疑似距離生成手段として機能する。このコントローラユニット１３は、疑似距離に基づき、移動体が障害物を回避するよう移動体の動作を制御する制御手段として機能する。コントローラユニット１３は、回避対象決定手段、並進速度出力指令値補正処理手段、旋回速度出力指令値補正処理手段としても機能する。各手段はＣＰＵ、ＲＯＭ、ＲＡＭにより実現される。
【００７９】
また、上記実施形態に開示されている複数の構成要素の適宜な組み合わせにより、種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。本発明は、ロボットや全方向移動車のほかに、自動車などにも利用可能であり、自動車等の移動装置、移動制御装置及び制御手法に適用可能である。
【図面の簡単な説明】
【００８０】
【図１】（ａ）は本発明の実施の形態に係る障害物回避機能を有する移動制御装置が実装された移動体の上面図であり、（ｂ）は移動体の正面図である。
【図２】レーザレンジファインダの計測可能領域を示す図である。
【図３】本発明の実施の形態に係る障害物回避機能を有する移動制御装置のブロック図である。
【図４】本発明の実施の形態に係る障害物回避機能を有する移動制御装置が作成する障害物マップの一例を示す図である。
【図５】移動体が前進する時の疑似距離の概念を示す図である。
【図６】移動体が後進する時の疑似距離の概念を示す図である。
【図７】角度と疑似距離との関係を説明するための図である。
【図８】同一地点での前進及び後進の進行方向切り替わりにより周囲環境の障害物位置が変化する疑似障害物マップの概念図である。
【図９】移動体の姿勢変更後の移動により周囲環境の障害物位置が変化する疑似障害物マップの概念図である。
【図１０】移動体として全方向移動車が用いられる場合の障害物マップの一例を示す図である。
【図１１】回避対象の障害物の位置をコントローラユニットが決定する手法を説明するための図である。
【図１２】速度方位ポテンシャルの一例を示す図である。
【図１３】並進速度出力許容方向と回避対象障害物方向との関係を示す図である。
【図１４】移動体の移動速度の出力方向の補正処理を説明するための図である。
【図１５】移動体の移動速度に応じて異なるポテンシャル形状を有する速度方位ポテンシャルの一例を示す図である。
【図１６】速度量ポテンシャルの一例を示す図である。
【図１７】コントローラユニットによる障害物回避速度補正処理のフローチャートである。
【図１８】移動体に対する階層構造指令伝達型の移動制御方法を説明するためのフローチャートである。
【符号の説明】
【００８１】
１…ロボット（移動体）、１Ａ…全方向移動車（移動体）、２，２Ａ…基台、３，４…駆動輪、５，６…モータ、７…前方キャスタ、８…後方キャスタ、９−１２…レーザレンジファインダ（計測手段）、１３…コントローラユニット（障害物回避機能を有する移動制御装置）、１４，１５…回転角度センサ（検出手段）、１６…位置センサ（検出手段）、１７…姿勢センサ（検出手段）、１８…レーザレンジファインダ計測可能領域、１９…経路生成部、２０…目標軌道生成部、２１，２５…演算部、２２…ロボット移動速度指令値生成部、２３…障害物回避速度指令値補正部、２４…車軸回転速度指令値生成部、２６…モータ制御電流指令値生成部、２７…ロボット本体、２８〜３２，３７〜４０，４２〜４４…障害物ないしは物体、３３…移動体進行方位、３４…実位置、３５…疑似位置、３６…疑似障害物位置、４１…駆動輪、４５…障害物位置、４６…最短疑似距離、４７…回避対象障害物方向、４８…回避対象障害物、４９…並進速度出力許容方向、５０…移動体基準位置、５１…移動速度出力方向、５２…回避対象障害物方向、５３…並進速度出力許容方向、５４〜５６…速度方位ポテンシャル、５７…障害物位置、５８…疑似距離、５９…真の距離、６０…疑似位置、６１…真の位置。
【技術分野】
【０００１】
本発明は障害物回避機能を有する移動制御装置に関する。
【背景技術】
【０００２】
人間共存環境で活動するロボットなどの自律移動体の開発が盛んに行われている。コスト、消費電力、ＣＰＵリソースなどの観点から、このようなロボットに計算負荷の高いアルゴリズムのソフトウェアを実装することは現実的でない。また工場の中で移動する搬送車のように移動環境を完全に作り込むことは不可能であり、オブジェクトの配置ズレや他の移動物体などを許容した移動制御が必要となる。その一方でスムーズかつ素早い効率の良い移動動作をロボットが行えるようにする要求が高い。
【０００３】
移動体が障害物を回避しながら移動する技術に関しては、従来、障害物を回避しながら目標点に移動する経路の中から最短距離となるルートを見い出す移動体の経路生成法が知られている（特許文献１参照）。この経路生成法は、環境温度マップを作成し、環境温度マップ中に構築された障害物に関する各格子点における温度に基づき各格子点で探索しながら出発点と最終目標点とを結ぶ最短経路を作成し、移動体をこの最短経路に沿って最終目標点まで移動させている。
【０００４】
また、障害物の動きに応じて移動体の移動経路をその場で生成し、動的な障害物がある場合でも適切な回避行動を取れるようにした自走移動体の制御方法が知られている（特許文献２参照）。特許文献２に記載の方法は、障害物が存在する確率を表す確率ポテンンシャル場を生成し、この確率ポテンシャル場に目標位置に向かう勾配を付加し、この勾配が付された確率ポテンシャル場の傾きに基づき目標位置に向かう経路を探索し、自走移動体をこの経路に沿って移動させている。
【０００５】
また、与えられた領域を、できるだけ短時間でほぼ網羅的に走行できるようにし走行中のハンチングを防止可能とした自律走行ロボットも提案されている（特許文献３参照）。特許文献３に記載のロボットは、左右の障害物センサを設け、これらのセンサが検出した障害物までの距離に基づき検出距離が長い側に旋回動作を行うと共に、この旋回動作後の予定時間内に再び先の旋回動作とは逆方向に旋回する動作が必要になった場合、先の旋回時よりも旋回角を小さくするようにしている。
【特許文献１】特開２００１−１５４７０６号公報
【特許文献２】特開２００３−２４１８３６号公報
【特許文献３】特開平１０−２２８３１５号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、特許文献１に記載の経路生成法では、既知である地図を使用した経路計画をベースにした移動体の障害物の回避動作がノイズに非常に弱い。ノイズとは、地図に登録されていない障害物や、理想値（地図情報）と現実値とのズレ、及びデッドレコニング誤差である。また特許文献１に記載の技術では、位置が時々刻々と不規則に変化する移動体への制御の対応が難しい。
【０００７】
特許文献２に記載の自走移動体の制御方法は、移動体と障害物又は移動目標との間に仮想的な斥力あるいは引力などを生成する力場を定め、これらの力の合力から移動体の進行方向を決定するポテンシャル法である。この制御方法では、複雑な地形や多数の障害物に対する扱いが難しい。経路の精度を高めようとすると障害物を登録するためのグリッドマップのメッシュを細かくする必要があり、計算コストが爆発的に高くなる。
【０００８】
特許文献３に記載の自律走行ロボットはセンサ情報を移動指令に直結させる手法を用いたものである。この自律走行ロボットでは、きめ細かな動作指令をロボットへ与えることが難しく、ロボットは大まかで単調なぎこちない動作になりやすい。滑らかで効率の良い動作を実現することが難しい。
【０００９】
そこで、本発明は、上記の課題に鑑み、計算負荷が低く、移動がスムーズであり環境の動的変化などノイズに強いロバストな移動動作が可能な障害物回避機能を有する移動制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１０】
このような課題を解決するため、本発明の一態様によれば、移動体と、この移動体に設けられ、移動体進行方位を検出する検出手段と、前記移動体に設けられ、障害物の位置およびこの障害物までの実計測距離を計測し、この検出手段が検出した前記移動体進行方位に対する前記障害物の方位角を出力する計測手段と、この計測手段が出力した前記障害物の方位角の大きさに応じて、前記実計測距離を変化させて疑似距離を生成する疑似距離生成手段と、この疑似距離生成手段が生成した前記疑似距離に基づき、前記移動体が前記障害物を回避するよう前記移動体の動作を制御する制御手段と、備えることを特徴とする障害物回避機能を有する移動制御装置が提供される。
【００１１】
また、本発明の別の一態様によれば、移動空間に関する外部環境情報を保存し更新可能な記録手段と、この記録手段に記録された前記外部環境情報をもとに目的地点までの適切な経路を生成可能な経路生成手段と、この経路生成手段により生成された前記経路を辿るような速度出力指令値を生成する移動速度出力生成手段と、この移動速度出力生成手段が生成した前記速度出力指令値に対して、速度出力補正を行う請求項１〜１３に記載の疑似距離生成手段と、を備えることを特徴とする障害物回避機能を有する移動制御装置が提供される。
【発明の効果】
【００１２】
本発明によれば、計算負荷を大幅に低減しつつも不要な回避動作を抑制した滑らかな回避軌道を生成することが可能となる。また時々刻々と変化する動的環境にも対応した回避動作を実現できるようになる。
【発明を実施するための最良の形態】
【００１３】
以下、本発明の実施の形態に係る障害物回避機能を有する移動制御装置について、図１乃至図１８を参照しながら説明する。尚、各図において同一箇所については同一の符号を付すとともに、重複した説明は省略する。
【００１４】
本発明の実施の形態に係る障害物回避機能を有する移動制御装置は、独立二輪駆動型の移動ロボットに搭載される。図１（ａ）は移動ロボットの上面図であり、図１（ｂ）は図１（ａ）の矢印Ｊ方向から見たこの移動ロボットの正面図である。これらの図中で同じ符号を有する要素は互いに同じ要素を表す。
【００１５】
ロボット１は、基台２と、それぞれこの基台２の左右両側部に左右平行に同軸的に配置され互いに独立して駆動可能にされた一対の駆動輪３、４と、それぞれが基台２の下面に設けられ駆動輪３、４を回転させる一対のモータ５、６と、基台２の前方中央部に取り付けられた前方キャスタ７と、基台２の後方中央部に取り付けられた後方キャスタ８と、それぞれ基台２上面の四隅に設けられ被測定対象との間の距離を測定する４つのレーザレンジファインダ（ＬＲＦ）９−１２と、基台２に設けられモータ５、６の回転駆動制御を独立して行うコントローラユニット１３（本実施形態に係る障害物回避機能を有する移動制御装置）とを備える。コントローラユニット１３には、ＣＰＵ、ＲＯＭ及びＲＡＭが用いられる。
【００１６】
更にロボット１は、それぞれ駆動輪３、４のシャフトに設けられた一対の回転角度センサ１４、１５と、基台２の並進移動量を検出するための位置センサ１６と、基台２の垂直軸線周りの旋回角度を検出する姿勢センサ１７とを有する。回転角度センサ１４、１５にはポテンショメータやロータリーエンコーダが用いられる。位置センサ１６には慣性センサが用いられる。姿勢センサ１７には、垂直軸線周りの角速度を検出するジャイロセンサが用いられる。これらの回転角度センサ１４、１５の各センサ値と、位置センサ１６のセンサ値と、姿勢センサ１７のセンサ値とはいずれもコントローラユニット１３へ入力されるようになっている。
【００１７】
基台２は矩形状の平板である。モータ５、６は基台２に固定されており、各モータ軸を介して駆動輪３、４と連結されている。駆動輪３、４の各車輪面は互いに平行にされた状態でこれらの駆動輪３、４は正方向及び逆方向に回転自在である。駆動輪３、４と前方キャスタ７及び後方キャスタ８とによって基台２が床面に対して床面から車輪径で決まる高さで水平に保持される。コントローラユニット１３によるモータ５、６に対する駆動制御によって、駆動輪３の回転速度と駆動輪４の回転速度とは互いに同じにされ、あるいは異なるようにされている。モータ５、６が駆動制御されることによって、ロボット１の走行及び旋回が可能にされている。モータ５、６の各回転方向と各回転速度とが制御されることによって、ロボット１は床面内を移動可能になっている。
【００１８】
レーザレンジファインダ９−１２は平面走査型の距離計測手段である。レーザレンジファインダ９−１２はいずれも図示しないレーザ光源と、回転軸を有し外周面上に多面鏡を設けたレーザ光反射体と、レーザ光反射体を回転させる駆動モータと、レーザ光を変調する変調手段と、被測定対象によって反射されたレーザ光を受光する受光器と、電子回路部と、これらのレーザ光源等を収容する筐体とを有する。この駆動モータのモータ軸の回転角に応じて、筐体外部の走査空間を走査する走査光がレーザ光より生成されるようになっている。レーザ光源及び変調手段は障害物ないしは目標物体といった被測定対象に対してパルス変調したレーザ光を照射し、受光器は被測定対象により反射された反射光を受光し、電子回路部は変調光を送出した時間とこの反射光が戻ってきた時間とから、ロボット１と被測定対象物との間の距離の情報を出力する。
【００１９】
レーザレンジファインダ９は、基台２の前方に存在する物体までの距離とその物体からの反射光の反射強度とを、角度ステップ毎に計測し、反射光強度の計測結果から計算した距離を図示しないメモリに記憶する。このメモリには角度毎の距離値が記憶される。角度毎の距離値は更新可能及び補正可能である。レーザレンジファインダ１０−１２の構成もレーザレンジファインダ９の構成と同じである。レーザレンジファインダ９−１２はロボット１の周囲の全方位を死角なく走査可能に配置されている。
【００２０】
図２はレーザレンジファインダ９−１２の計測可能領域を示す図である。コントローラユニット１３がこれらのレーザレンジファインダ９−１２を制御することによって、レーザレンジファインダ９−１２はロボット１の周囲の空間を走査する。各レーザ光源からのレーザ光の送出方向は床面と平行である。また、これらのレーザレンジファインダ９−１２内の受光器の感度は略水平方向にされてある。基台２の四隅に設けられた各レーザレンジファインダ９−１２がそれぞれ上面視扇形のレーザレンジファインダ計測可能領域１８を走査する。
【００２１】
コントローラユニット１３は、４つのレーザレンジファインダ９−１２から出力される角度毎の距離値に基づき、床面上の障害物を含む外部環境の距離画像を生成する。コントローラユニット１３は、ロボット１の３６０°全方位に亘って障害物が存在しているかどうかを検出するようにしている。
【００２２】
また、コントローラユニット１３はモータ５、６の角速度及び角加速度を制御する。ロボット１の周囲に障害物が存在することをコントローラユニット１３が検知した場合、このコントローラユニット１３は後述するように障害物を回避するための演算処理を行う。コントローラユニット１３は、センサ類からの情報に基づきロボット１の並進移動量や旋回量を算出し、モータ５の角速度及び角加速度の目標値と、モータ６の角速度及び角加速度の目標値とを生成する。コントローラユニット１３は、生成したモータ５、６の各回転量の目標値から、これらのモータ５、６の図示しないドライバに対して電流制御を行うようにしている。
【００２３】
図３はコントローラユニット１３のブロック図である。コントローラユニット１３は、ロボット１が走行すべき経路を生成する経路生成部１９と、経路生成部１９からの出力に基づいて制御周期ごとに基台２のｘ、ｙ座標系での目標位置及び垂直軸線周りの目標回転位置を生成する目標軌道生成部２０と、目標軌道生成部２０からの出力及び基台２に取り付けられたセンサ類からのセンサ値を比較し、演算した結果を出力する演算部２１と、演算部２１から出力される演算結果の差分を埋めるように基台２の並進速度の目標値及び旋回速度の目標値を計算するロボット移動速度指令値生成部２２とを備える。
【００２４】
経路生成部１９は、後述する図４に示すような障害物マップを作成する。目標軌道生成部２０は、各制御周期でのロボット目標位置ｘr［ｍ］、ｙr［ｍ］及びロボット目標姿勢θ［ｒａｄ］を生成して出力する。演算部２１は、これらのｘr、ｙr及びθと、センサ類からの現在位置ｘ［ｍ］、ｙ［ｍ］とを比較する。ロボット移動速度指令値生成部２２は、演算部２１からの比較結果に基づき並進速度目標値Ｖr［ｍ／ｓ］及び旋回速度目標値ωr［ｍ／ｓ］を生成して出力する。
【００２５】
更にコントローラユニット１３は、ロボット移動速度指令値生成部２２からの出力を補正する障害物回避速度指令値補正部２３と、この障害物回避速度指令値補正部２３からの出力に基づいて各軸の回転速度の目標値を生成して出力する車軸回転速度指令値生成部２４と、車軸回転速度指令値生成部２４からの出力及び基台２のセンサ類からのセンサ値を比較演算した結果を出力する演算部２５と、演算部２５から出力される演算結果に基づいてモータ５、６へ与える電流の目標値を生成するモータ制御電流指令値生成部２６と、ロボット本体２７とを備える。ロボット本体２７は、駆動輪３、４及び回転角度センサ１４、１５、位置センサ１６及び姿勢センサ１７からなる。
【００２６】
障害物回避速度指令値補正部２３は、並進速度目標値Ｖrを補正して補正並進速度目標値Ｖr′［ｍ／ｓ］を出力する。障害物回避速度指令値補正部２３は、旋回速度目標値ωrを補正して補正旋回速度目標値ωr′［ｍ／ｓ］を出力する。車軸回転速度指令値生成部２４は各軸回転速度目標値φnr［ｒａｄ／ｓ］を生成して出力する。モータ制御電流指令値生成部２６は、各軸モータ電流指令値ｉnr［Ａ］を出力する。この各軸モータ電流指令値ｉnrは、モータドライバへの電圧の指令値Ｖn［Ｖ］にほぼ等しい。
【００２７】
ロボット１は移動体である。ロボット１は移動手段を有し、またこの移動手段を動作させる駆動手段を有する。移動手段は駆動輪３、４であり、駆動手段はモータ５、６である。コントローラユニット１３は本実施形態に係る障害物回避機能を有する移動制御装置である。障害物回避制御方法は、この移動体に関する制御手法である。コントローラユニット１３は目標地点を設定するという上位処理を行う。移動体は移動体自身の内部に持つ上位概念のルールに従い目標地点が設定される。移動体とは、目標地点へ到達するための並進速度指令値及び旋回速度指令値を生成するロボット１のような自律移動体を含む。あるいは、移動体とは、アシストカートなどのように外部入力を指令値としてこの指令値を実現するように駆動手段を動作させる半自律移動体なども含む。また移動体は移動体周囲の物体の配置状況を認知可能な認知手段を有しており、これにより移動体の周囲の全方位の障害物情報を知ることが可能となっている。
【００２８】
ここでは図１に示すような移動型ロボットを代表例として用いる。ロボット１は、モータ５、６により駆動される左右の駆動輪３、４を有する独立二輪駆動方式である。これら二輪の回転量で直進及び旋回の動作が定まる。またロボット１にはレーザレンジファインダ９−１２が図２のように死角なく複数例えば４個配置され、全方位の障害物までの距離をその方向とともに高精度で計測することが可能となっている。計測された方位角及び距離といった障害物情報は直ちにメモリ内で更新され、更新された障害物情報は制御に反映される。
【００２９】
図２ではレーザレンジファインダ９−１２が計測する距離方向は途中までしか描画されていないが、実際にはさらに遠方まで計測可能である。レーザレンジファインダ９−１２単体が計測可能な方位も特に図２の例に指定するものではなく、レーザレンジファインダ搭載個数も特に図２の数に限定するものではない。ロボット１の移動方法は本例の独立二輪駆動方式に限るものではなく、少なくともロボット１自身の移動方向と移動量とがホロノミック又はノンホロノミックの区別無く任意に指定できればよい。例えばステアリング方式や、４輪が駆動する全方向移動車、また二足歩行のような車輪以外の移動手段が用いられてもよい。
【００３０】
またセンサ系に関しても特に本例のレーザレンジファインダ方式に限るものではなく、配置位置が異なるものや、指向性のある超音波センサやＰＳＤ（Ｐｏｓｉｔｉｏｎ Ｓｅｎｓｉｔｉｖｅ Ｄｅｔｅｃｔｏｒ）センサを周囲に配置したものを用いてもよい。ＰＳＤセンサは、受光面を有しこの受光面に入射するビームスポットの位置に応じて差動的に変化する一対の出力信号を出力するＰＳＤ素子と、このＰＳＤ素子により検出された光位置から距離を演算する演算手段とからなる。また、これらのセンサ情報をロボット１の周囲の情報を予め記録した地図上に一度集約して、制御周期毎にこの地図より擬似的にロボット１の周囲の状況を読み出してもよい。さらにこの地図に環境カメラや他ロボットなど外部システムからも位置同期を取りつつ障害物配置をリアルタイムで登録し、削除するようにしてもよい。
【００３１】
本例でのコントローラユニット１３は図２のように配置されたレーザレンジファインダ９−１２のそれぞれから得られる距離情報をマップ上に統合する処理を行う。コントローラユニット１３は、既知である幾何配置情報をもとに一つの座標系で表される地図上で各距離情報を統合管理している。この障害物マップ情報より、ロボット１の指定方向の指定角度範囲内に存在する障害物のうちの最も近い障害物までの距離が得られるようになっている。
【００３２】
センサとしてのレーザレンジファインダ９−１２のサンプリングレートは、ロボット１自身の移動および他移動物体による外界の動的変化に対し相対的変化が連続的に捉えられる程度に十分高速なものが用意される。また指定角度最小範囲の分解能についても、規定の距離内に存在する外界障害物の相対的な位置変化が連続的に捉えられる程度の適切な分解能が設定されている。ここでの連続的とは、対象物の位置を計測するステップの後、観測対象物が計測した次ステップにおいて前回計測された位置より規定の距離範囲内に存在することを意味している。この分解能の値は用途に応じ変化するので、一意に定めるものではない。
【００３３】
図４はコントローラユニット１３が作成する障害物マップの一例を示す図である。同図中、上述した符号と同じ符号を有する要素はそれらと同じ要素を表す。２８から３２はいずれも障害物ないしは物体を表す。３３は移動体進行方位である。ロボット１から見て障害物２９の方位が計測指定方位である。これらの移動体進行方位及び計測指定方位が成す角度φは基準方位である正面方向に対する方位φである。５７は計測指定方向の最短距離に存在する障害物の位置を表す。
【００３４】
本例では障害物マップは、図４のようなロボット１を中心とした相対座標系において、計測指定方位として指定された方位φと、その方向の最短距離ｒとにより極座標表現される。障害物マップは、極座標以外の座標を用いた表記や、絶対座標などを用いても特に構わない。また本例では、方位φの基準方向はロボット正面としており、半時計回り方向を正としている。この基準方向や旋回方向に関してもロボット正面や半時計回り方向に特に限定されるものではない。
【００３５】
障害物マップ上での実計測距離と疑似距離との関係を、図５及び図６に示す。図５は移動体が前進する時の疑似距離の概念を示す図である。３４は障害物ないしは物体の実位置を表す。３５は障害物ないしは物体の疑似位置を表す。レーザレンジファインダ９はロボット１と、実障害物位置３４との間の距離を実測し実計測距離ｒを取得する。レーザレンジファインダ９は、移動体進行方位と計測指定方位とが成す角度θ1を取得する。コントローラユニット１３は、実計測距離ｒを疑似距離ｒ′に置き換える処理を行うことによって、障害物マップ上で、実障害物位置３４を疑似障害物位置３５へ置き換えるようにしている。置き換える処理とは、関数ｆを用いてｒ′＝ｆ（ｒ、θ1）を演算することを指す。ＣＰＵは、関数ｆの計算手順が記述されたプログラムをＲＯＭから読み込み、ＲＡＭを用いてこの計算手順を実行する。
【００３６】
また、ロボット１が後進する時の例を図６に示す。レーザレンジファインダ９はロボット１と、実障害物位置３４との間の距離を実測し実計測距離ｒを取得する。レーザレンジファインダ９は、移動体進行方位と計測指定方位とが成す角度θ2を取得する。この角度θ2の大きさは、図５の例の角度θ1よりも大きい。コントローラユニット１３は、ｒ′＝ｆ（ｒ、θ2）を演算することによって、障害物マップ上で、実障害物位置３４を疑似障害物位置３６へ置き換えるようにしている。
【００３７】
即ち本発明では図５に示すように、コントローラユニット１３は移動体進行方向と計測指定方向とが成す角度θ1の大きさに応じて計測指定方向の距離値が真の計測値（以下「真値」）から変化する疑似距離を用いる。本例での独立二輪駆動方式でロボット１が前進している場合、移動体進行方向と計測方位基準方向とが一致するので、この場合θ1＝φとなっている。ロボット１が後進する場合、移動体進行方向、計測指定方向それぞれの角度関係は図６のようになる。
【００３８】
さらに図７は角度θと疑似距離ｒn′との関係を説明するための図である。５８は疑似距離を表す。５９は真の距離である。本例では図７に示すように移動体進行方向と計測指定方向とが成す角度θnの大きさが大きくなるほど疑似距離ｒ′は真値ｒよりも大きな値を取るように、コントローラユニット１３は関数ｆの計算手順を実行する。角度θnが大きくなるほど、疑似距離が真値から乖離する量が大きくなる。
【００３９】
図８に同一地点での前進及び後進の進行方向の切り替わりにより周囲環境の障害物位置が変化する疑似障害物マップの概念図を示す。ここで図８（ａ）はロボット１が前進する時の疑似障害物マップの一例であり、図８（ｂ）は図８（ａ）のロボット１の地点と同じ地点でロボット１が後進する時の疑似障害物マップの一例である。また、図９にロボット１の姿勢変更後の移動により周囲環境の障害物位置が変化する疑似障害物マップの概念図を示す。図９（ａ）は姿勢を変更した後のロボット１が前進する時の疑似障害物マップの一例であり、図９（ｂ）は図９（ａ）のロボット１の地点と同じ地点での姿勢変更後のロボット１が後進する時の疑似障害物マップの一例である。３７から４０は障害物ないしは物体を表す。６０は疑似位置であり疑似障害物マップ上での障害物位置を表す。６１は真の位置である。
【００４０】
ここで疑似距離により形成されるロボット１の周囲環境の障害物位置を表示した疑似障害物マップ（以下、障害物マップ）は、図８（ａ）、図８（ｂ）で表されるように同一の地点であっても前進及び後進の動作の切り替わりで状況が一瞬のうちに変化するという特徴がある。また図９（ａ）、図９（ｂ）で表されるように、ロボット１が独立二輪駆動方式であっても、ロボット１がその場で旋回動作を行い、絶対座標系上でのロボット１自身の正面方向（以下「姿勢」と同義）を変化させた場合、その後の前後の移動時に得られる障害物マップはそれ以前のものとは異なるものとなる。
【００４１】
移動体は本例とは異なる移動方式を用いてもよい。移動体には全方向移動車が用いられてもよい。全方向移動車は、任意のタイミングで任意の方向に速度出力を生成可能な移動手段を持つ移動機構である。移動体として全方向移動車が用いられる場合の障害物マップを図１０に示す。図１０（ａ）から図１０（ｄ）には全方向移動車１Ａの同一姿勢状態での移動方向の切り替わりにより周囲環境の障害物位置が変化する疑似障害物マップが示されている。斜線付きの丸は疑似障害物マップ上での障害物位置を表す。白色の丸は真の位置を表す。
【００４２】
全方向移動車１Ａは、基台２Ａと、この基台２Ａの前後左右の４箇所に枢支された駆動輪４１と、この基台２Ａ上に設けられた図示しない本実施形態に係る障害物回避機能を有する移動制御装置とを有する。これら以外の符号で上述した符号と同じ符号を有する要素はそれらと同じ要素を表す。具体的には図１０（ａ）は全方向移動車１Ａの前進時の疑似障害物マップであり、図１０（ｂ）は同じ地点での後進時の疑似障害物マップである。図１０（ｃ）は姿勢変更後の全方向移動車１Ａの前進時の疑似障害物マップであり、図１０（ｄ）は同じ地点での姿勢変更後の全方向移動車１Ａの後進時の疑似障害物マップである。図１０で表されるように移動指令方向により障害物マップが表す周囲環境状況は、同一の地点で同一の姿勢を有していても異なる結果を出力することになる。全方向移動車１Ａの移動制御装置はロボット１のコントローラユニット１３と同じである。
【００４３】
以下、ロボット１に搭載されるコントローラユニット１３が行う疑似距離の生成処理について述べる。ここでは障害物計測方位角θに対応して、真値ｒを疑似距離ｒ′へと変換する『行為』を疑似距離『変換』と呼ぶことにする。本例では（式１）のような疑似距離変換を適用して疑似距離を生成している。
【数１】
【００４４】
ここでｌ，ｍ，ｎは任意の実数とし、ｋは０＜ｋ＜２の実数とし、コントローラユニット１３は（ｋ＝１，ｌ＝１，ｍ＝２，ｎ＝１）を一組として疑似距離を生成するようにしている。コントローラユニット１３は進行方向と同方向（即ち独立二輪駆動方式では正面方向）に位置する疑似距離の値については真の距離値と同じ値を出力する。コントローラユニット１３は、移動進行方向に対し真横方向に位置する疑似距離の値については真の距離値の２倍の距離を出力する。コントローラユニット１３は、進行方向（θが０度の方向）と真横方向（θが９０度の方向）との間の範囲内の所望の方向に位置する疑似距離の値については、（式１）に代入して得られる値を出力する。つまりこれらの２つの方向の間の遷移は余弦関数的に遷移していく。またコントローラユニット１３は、真横方向よりもθが大きい方向である後方は無限遠とすることで、真横方向よりも後方に位置する方向に存在する障害物を、障害物回避の考慮対象から除外するようにしている。
【００４５】
また、コントローラユニット１３は、正面方向の疑似距離の値を、真値と異なる値に設定してもよい。コントローラユニット１３は正面方向の疑似距離の値を真値から変える場合、ｌの値を変更する。コントローラユニット１３は、無限遠を除く最大値を変える場合、ｍの値を変更する。コントローラユニット１３は、正面方向について設定した値から最大値に至るまでの余弦関数の遷移形状を変化させる場合、ｎの値を変更すればよい。またコントローラユニット１３はｋの値を変更することで疑似距離が最大地点（無限遠を除く）となる角αの位置を真横から変えるようにもできる。ただし本例が提示する（式１）は一例にすぎず、コントローラユニット１３は進行方向背後の障害物距離を無限遠とはせずに有限値としてもよい。さらにはコントローラユニット１３は、他の関数を使用して疑似距離変換をデザインしても何ら問題はなく、これらは請求項に記載の本発明の範囲内である。
【００４６】
ロボット１は、上記疑似距離ｒ′を参照し、周囲環境が未だ考慮されていない段階で生成した移動動作指令を補正するようにしている。以下、本補正処理を障害物回避と呼び、補正処理された移動動作を障害物回避動作と呼ぶ。
【００４７】
ロボット１は障害物回避動作にあたり、ロボット１自身の周囲に存在する障害物についての複数の疑似距離を参照し、これらの疑似距離のうち最短の疑似距離を入力として使用するようにしている。図１１は回避対象の障害物の位置をコントローラユニット１３が決定する手法を説明するための図である。同図には、ロボット１の周囲に障害物（物体）４２〜４４が存在しており、コントローラユニット１３は、疑似障害物マップ上に複数の障害物位置を予め生成する。斜線付きの丸は障害物位置を表す。白色の丸は真の位置を表す。コントローラユニット１３は、これらの障害物位置の間で疑似距離を比較し、最短の疑似経路を有する障害物位置を検索する。複数の障害物位置のうちの４５を付された障害物位置をコントローラユニット１３が判断すると、コントローラユニット１３はこの障害物位置４５を回避対象障害物位置と決定する。即ち、図１１で表されるように、コントローラユニット１３は、最短疑似距離４６を有する方位，疑似距離が指す位置を回避対象障害物位置とする処理を行う。この地点を回避対象障害物と呼ぶこととする。
【００４８】
従来の障害物回避手法では、コントローラが回避軌道を生成するにあたり、コントローラは障害物までの距離とその方向との２成分を考慮しなくてはならなかった。しかし、本実施例による障害物回避制御手法では疑似距離にその方向成分によりもたらされる効果も含まれているので、特例的な処理を除き、マップ生成後のコントローラユニット１３の軌道生成作業ではコントローラユニット１３は基本的には疑似距離のみを参照すればよい。
【００４９】
本例で示すロボット１は疑似距離を入力として障害物回避のための速度出力方向と速度量との２成分を出力するようにしている。従来例のポテンシャル手法はコントローラが１つのポテンシャル関数から強度とその勾配とを求めこれらの強度及び勾配を速度量と速度出力方向とに当てはめたものである。これに対して、コントローラユニット１３は２成分の値を算出するにあたり、２つの独立した生成関数を使うようにしている。以下の説明では、これらの２つの独立した生成関数のそれぞれを速度方位ポテンシャル，速度量ポテンシャルと呼ぶこととする。なおここでの「ポテンシャル」という呼称は便宜上用いるものであり、一般的なポテンシャル関数のように導関数が特別な意味を持つわけではない。
【００５０】
速度方位ポテンシャルについて図１２から図１５を参照して説明する。図１２は速度方位ポテンシャルの一例を示す図である。横軸は疑似距離ｒ′であり、縦軸は並進速度出力許容方位角ξである。図１２で表されるような形状を有する速度方位ポテンシャルを、コントローラユニット１３は予め生成してコントローラユニット１３内のメモリ（記憶手段）に記憶しておく。コントローラユニット１３は予め横軸と縦軸との各値を取得する処理を実行し、速度方位ポテンシャルを生成しておく。
【００５１】
ここでの並進速度出力許容方位角について図１３及び図１４を参照して更に述べる。図１３は並進速度出力許容方向と回避対象障害物方向との関係を示す図である。回避対象障害物方向４７は回避対象障害物４８の存在する方向である。並進速度出力許容方向とは図１３の４９で表される。並進速度出力許容方向４９は、回避対象障害物方向４７に向けることが可能な移動速度出力の限界方向を指す。移動体基準位置５０（ここでは移動平面に投影したロボット重心とする）を頂点とし、回避対象障害物方向４７と並進速度出力許容方向４９とが成す角ξを並進速度出力許容方向角と呼ぶこととする。この並進速度出力許容方向４９は、基本的には回避対象障害物方向４７から移動速度出力方向に向けて展開される。
【００５２】
また、図１３の回避対象障害物方向４７と並進速度出力許容方向４９とが成す角ξは０からπ／２までの範囲を取りうる。コントローラユニット１３は、ξが０のとき任意の方向へ移動速度を出力するようにする。これに対し、ξの値が増加するに伴いコントローラユニット１３は回避対象障害物方向４７への移動速度出力を制限する。コントローラユニット１３が制限をかけるため、ξがπ／２になると、コントローラユニット１３は回避対象障害物方向４７に対し垂直方向もしくは回避対象障害物４８から遠ざかる方向にのみ速度出力を行うようにしている。
【００５３】
図１４は移動体の移動速度の出力方向の補正処理を説明するための図である。図１４のように移動速度出力方向５１が回避対象障害物方向５２と並進速度出力許容方向５３との間に存在する場合、ロボット１は移動速度出力方向５１を並進速度出力許容方向５３にまで補正変更するようにしている。
【００５４】
図１１の速度方位ポテンシャルより分かる通り、疑似距離ｒ′がある値ｒmax（第１閾値）以上である場合、コントローラユニット１３は並進速度出力許容方位角ξの値を０にする。コントローラユニット１３は速度出力方向については特別な制限をかけない。コントローラユニット１３により制限を受ける速度方位ポテンシャルの特性は、疑似距離ｒ′がｒmax以下になると徐々にξの値が増加し、疑似距離ｒ′がある値ｒmin（第２閾値）以下ではξの値はπ／２に飽和するような形状をとる。ここでｒmaxの値は「ロボット１がどれくらい回避せずに障害物方向にまで接近を許容するか」という回避開始距離を定義する値である。ｒminの値は「ロボット１が最悪どのくらいの距離まで障害物への接近を許容するか」という限界接近距離を定義する値である。
【００５５】
また、全方向移動車１Ａのように任意の時刻に任意の並進移動速度を生成可能な移動体については、コントローラユニット１３は補正処理した移動速度出力方向に自身の移動速度出力を設定する処理を行うだけでよい。移動体が本例のような独立二輪駆動型の場合この移動体の動作はノンホロノミックな移動拘束条件下での動作となるため、コントローラユニット１３は補正処理した移動速度出力方向に移動速度を生成することができない。このような場合、コントローラユニット１３は移動体の姿勢を変更するようにし、移動速度の出力方向が補正処理方向にまで向くようにロボット１を旋回動作させる。コントローラユニット１３は旋回のための車軸回転速度が生成されるように、回転動作補正処理を実行する。
【００５６】
またコントローラユニット１３が参照する速度方位ポテンシャルは、図１５のように疑似距離と並進速度出力許容方位角ξとの関係が移動体の移動速度に応じて変化するような特徴を有している。図１５は、移動体の移動速度に応じて異なるポテンシャル形状を有する速度方位ポテンシャルの一例を示す図である。この速度方位ポテンシャルはコントローラユニット１３のメモリに記憶されている。５４は移動体の移動速度が速い場合に適用される速度方位ポテンシャルである。５５は移動体の移動速度が遅い場合に適用される速度方位ポテンシャルである。５６は通常の速度方位ポテンシャルを表す。
【００５７】
速度方位ポテンシャル５４は、最小疑似距離ｒmin及び最大疑似距離ｒmaxがそれぞれ通常の速度方位ポテンシャルの最小疑似距離及び最大疑似距離よりも増加したポテンシャル形状を有する。速度方位ポテンシャル５５は、最小疑似距離ｒmin及び最大疑似距離ｒmaxがそれぞれ通常の速度方位ポテンシャルの最小疑似距離及び最大疑似距離よりも減少したポテンシャル形状を有する。コントローラユニット１３は予めこれらの速度方位ポテンシャル５４、５５を作成してメモリに設定しておく。これにより指定移動速度が速い場合、障害物に対しなるべく遠い位置から徐々に回避動作を行うことで安全性を高めることを可能とし、指定移動速度が遅い場合、安全性の観点からもある程度の障害物までの接近は許容され、その分指定軌道に沿うように移動させることが可能となる。また高速移動時及び低速移動時のそれぞれにおける最小疑似距離ｒmin，最大疑似距離ｒmaxの間の変化幅に限界値を設けておき、コントローラユニット１３はある程度の段階で飽和するような処理をかけるようにしてもよい。
【００５８】
また、移動速度出力方向が回避対象障害物方向と並進速度出力許容方向との間に存在する場合、コントローラユニット１３は原則として並進速度出力許容方向へ向けて移動速度出力方向を変更する。このため、移動速度出力が回避対象障害物方向へ向くような速度変更や姿勢変更は禁止される。移動速度出力方向が回避対象障害物方向と並進速度出力許容方向との間よりも外側に存在する場合でも、移動速度出力方向が並進速度出力許容方向の近傍に存在する場合、コントローラユニット１３は移動速度出力方向が回避対象障害物方向へ向くような速度変更や姿勢変更を禁止する。これにより並進速度出力許容方向を挟んでの移動速度出力や姿勢変更のチャタリング的な振動動作が抑制される。
【００５９】
以上は速度方位ポテンシャルについての説明である。以下、速度量ポテンシャルについて図１６を参照して説明する。図１６は速度量ポテンシャルの一例を示す図である。横軸は疑似距離である。縦軸は速度許容率γ（０≦γ≦１）である。速度許容率は、速度量ポテンシャルにより制限された状態で移動体が最終的に出力可能な並進速度の最大値を、障害物が無い状態で移動体が本来出力可能な並進速度の最大値で除して得た比率である。この比率は擬似距離および移動体自身の大きさに応じて定まる。移動体であるロボット１が最終的に出力可能な速度最大値は、この比率にロボット１が本来出力可能な速度最大値を乗じて得られる。ここで障害物距離により制限を受ける値は許容出力速度の『最大値』であり、出力速度が直接制限を受けるものではない。このため、速度出力の指令値が最終的に出力可能な速度最大値よりも小さい場合、何の補正も受けることなくそのまま指令された値の速度が出力される。また、速度出力の指令値が本来出力可能な速度最大値より小さい値であっても、最終的に出力可能な速度最大値よりも大きい場合、このときの出力は最終的に出力可能な速度最大値に制限される。
【００６０】
コントローラユニット１３は予め横軸と縦軸との各値を取得する処理を実行し、速度量ポテンシャルを生成しメモリに記憶しておく。ここでの速度許容率γは、（式２）のように表現される。
【数２】
【００６１】
速度許容率γは、許容最大移動速度量ｖmaxをｖmax′に変換するという演算上の効果がある。変換後の許容最大移動速度量ｖmax′を変換許容最大移動速度量と呼ぶこととする。移動体の並進移動目標速度量|ｖr|がこの変換許容最大移動速度量以下である場合、コントローラユニット１３は特別な処理を発生させない。並進移動目標速度量|ｖr|が変換許容最大移動速度量を超える場合、コントローラユニット１３はそのスカラ量がｖmax′になるまで並進移動目標速度を圧縮補正する。ここで補正後の並進移動目標速度を補正並進移動目標速度ｖr′と呼ぶこととし、（式３）のように表される。
【数３】
【００６２】
図１６から分かる通り、速度許容率γは疑似距離ｒ′が大きいときには１となる。疑似距離ｒ′が閾値ｒv max（第３閾値）以下では疑似距離ｒ′の減少にともない速度許容率γも減少傾向となり、疑似距離ｒ′が閾値ｒv min（第４閾値）以下では速度許容率γは０となる。この速度量ポテンシャルに基づくコントローラユニット１３の処理の内容は、移動体が障害物に対し衝突コースにない場合、即ち移動体が回避動作対象外の移動を行っている場合、コントローラユニット１３は最大速度制限をかけずに、任意の速度を出力させ続ける。回避対象障害物が移動体に近接する場合、コントローラユニット１３は最大速度に制限をかける。最終的にはコントローラユニット１３は、閾値ｒv minで表現されるある距離以内には移動体が接近しないよう並進速度の出力を完全に遮断するようにする。これにより障害物との衝突を防ぐことが可能となっている。また速度出力方向もしくは姿勢角をコントローラユニット１３が変更すると、それに応じて疑似距離も変化し、これにともない速度許容率γも変化することになるので、再び移動体は並進速度出力可能状態にされる。
【００６３】
本例では速度方位ポテンシャルおよび速度量ポテンシャルを直線で表現したが、これらは請求項に記載の本発明を実現するための一例にすぎない。三角関数やｒmin，ｒmaxを変曲点とするような高次関数など他の関数を使用して本例以外の形状のポテンシャル関数をデザインしても何ら問題はなく、これらを変形した発明は請求項に記載の本発明の範囲内である。
【００６４】
以上の処理をコントローラユニット１３が行うことにより、障害物の位置及び距離に応じて、移動体の並進移動速度量と、移動体の速度出力方向もしくは姿勢角速度とに補正が加えられる。結果として移動体の移動経路上に存在する障害物を回避するような速度出力を生成することが可能となる。
【００６５】
上述の構成により、コントローラユニット１３は、障害物回避速度補正処理を行う。図１７はコントローラユニット１３による障害物回避速度補正処理のフローチャートである。コントローラユニット１３は、モータ５、６を制御対象としてこれらのモータ５、６のモータシャフトの回転制御のサーボ周期毎に制御動作を実行する。
【００６６】
障害物の存在を考慮される前の処理であるステップＡ１において、コントローラユニット１３は、並進速度の指令値Ｖと、旋回速度の指令値ωとを入力される。ステップＡ２において、コントローラユニット１３は、並進速度指令値方向に関する最短疑似距離障害物を探索する。レーザレンジファインダ９−１２により障害物が検知されない間、コントローラユニット１３は、Ｙルートを通り、ステップＡ１の処理を行う。レーザレンジファインダ９−１２のいずれかにより障害物が検知されると、Ｎルートを通り、ステップＡ３において、コントローラユニット１３は、並進速度出力許容方向を算出する。
【００６７】
引き続き、ステップＡ４において、コントローラユニット１３は、並進速度指令値ｖの方向が回避対象障害物方向と並進速度出力許容方向との間に存在するかどうかを判定する。コントローラユニット１３は存在すると判定した場合、Ｙルートを通り、ステップＡ５において、現在の姿勢状態で並進速度出力許容方向に速度を出力可能であるかどうかを判定する。ステップＡ５において、出力可能であるとコントローラユニット１３が判定した場合、Ｙルートを通り、ステップＡ６において、コントローラユニット１３は、並進速度指令値ｖが並進速度許容方向になるよう速度値を並進速度指令値ｖ′に補正する。
【００６８】
続いて、ステップＡ７において、コントローラユニット１３は、補正後の並進速度指令値方向に関する最短疑似距離障害物を探索する。ステップＡ８において、コントローラユニット１３は、変換許容最大移動速度量を算出する。ステップＡ９において、コントローラユニット１３は、移動体の並進移動速度量が変換許容最大移動速度量よりも大きいかどうかを判定する。並進移動速度量が変換許容最大移動速度量よりも大きいとコントローラユニット１３が判定した場合、Ｙルートを通り、ステップＡ１０において、コントローラユニット１３は並進移動速度の圧縮補正を行い、ステップＡ１１において、コントローラユニット１３は圧縮補正した速度を出力する。ステップＡ９において、並進移動速度量が変換許容最大移動速度量よりも大きくはないとコントローラユニット１３が判定した場合、Ｎルートを通り、ステップＡ１１においてコントローラユニット１３はそのまま速度を出力する。
【００６９】
また、ステップＡ５において、現在の姿勢状態で並進速度出力許容方向に速度を出力可能ではないとコントローラユニット１３が判定した場合、Ｎルートを通り、ステップＡ１２において、コントローラユニット１３は、並進速度指令値ｖが並進速度許容方向を向くように旋回速度補正候補値ω′を生成する。この場合、並進速度指令値ｖの方位補正をコントローラユニット１３は行わない。続くステップＡ１３において、コントローラユニット１３は、旋回速度指令値ωと旋回速度補正候補値ω′とが同一の回転方向でありかつ|ω|＞|ω′|であるかどうかを判定する。ωとω′とが同一回転方向でありかつ|ω|＞|ω′|である場合、Ｙルートを通り、コントローラユニット１３は、ステップＡ７の処理を行う。また、ωとω′とが同一回転方向ではない場合あるいは|ω|が|ω′|以下である場合、Ｎルートを通り、ステップＡ１４において、コントローラユニット１３は、旋回速度指令を旋回速度補正候補値に変更し、ステップＡ７に進む。
【００７０】
このように、進行方向から障害物が無くなると、ロボット１は移動目標位置に向かって再び速度指令を生成しそれを出力するので目標へ復帰できるようになる。
【００７１】
本例で解説した障害物回避制御手法は、コントローラユニット１３に対しセンサ類からのセンサ入力が発生すると、コントローラユニット１３はこのセンサ入力に対し、直ちに反応して動作補正を行うことが可能な反射的な障害物回避動作と表現することができる。この反射的障害物回避動作を実現する本制御処理を、経路計画に基づく障害物回避移動制御処理の後に実行するように移動体を移動制御するようにもできる。
【００７２】
図１８は移動体に対する階層構造指令伝達型の移動制御方法を説明するためのフローチャートである。この移動体に実装された制御手段は、ステップＢ１において、障害物回避計画軌道を生成する。ステップＢ１における処理では、制御手段は図示しない地図データベースを参照した上で障害物回避計画軌道を生成する。ステップＢ２において、制御手段は、障害物は考慮せずに、並進速度及び旋回速度の出力値を計算する。引き続き、ステップＢ３において、制御手段は、障害物回避速度補正を行う。制御手段はこの障害物回避速度補正に、本例の手法を適用する。ステップＢ４において、制御手段は、補正速度を出力する。モータの回転数が計算されてから出力される。
【００７３】
換言すれば、特許文献１に記載の移動体の経路生成法のような経路計画に基づく障害物回避移動制御フローの下流に、障害物回避速度補正処理フローを配置する。制御手段はまず既知である地形情報に基づき効率的な経路を計画的に生成しつつ移動体に移動を行わせる。既知の情報通りに事が運んでいる間、制御手段は、本例の反射的障害物回避処理を表面化させることなく通常の移動動作指令をそのまま出力して計画通りの理想的な移動動作をおこなう。予期せぬ障害物が出現したときや、あるいは計画時の自己位置の認識誤差または地形データベース側の事前情報と実際の情報との誤差などにより、制御手段が生成した計画軌道では移動体が障害物に接触してしまうような状況に陥ってしまったときに、制御手段が本例の反射的障害物処理を発現させて障害物を回避する速度補正を適切に主処理に割り込ませるようにすることが可能となる。
【００７４】
本提案の障害物回避制御手法を用いることにより、外部環境の予期せぬ乱れに強い安定した移動を実現することが可能となる。安定した移動が実現することは、移動ロボットなどの活動の場を、環境の作り込みでは片付けることのできない一般的な環境下へと大きく広げることを可能にすると考えられる。さらに回避対象障害物を選定することや、移動体自身の回避動作用の指令値などを出力することが連続的な状態量として扱われている。このため、出力される回避動作なども滑らかで効率も良く、また最短疑似距離をもつ１つの回避対象障害物を選定した上で計算が行われるため制御装置にかかる計算負荷も非常に小さく、非常に実用的な手法である。
【００７５】
このように、コントローラユニット１３は、移動体の移動方向からの角の大きさに応じて距離が変化する疑似距離が支配する、障害物と移動体との相対的な姿勢関係により値が変化する異方特性を持った速度・姿勢ポテンシャルより移動状態を用意し、この移動状態の量に周囲障害物までの最短疑似距離を毎時与えることで得られる速度および姿勢により回避動作を実現する制御手法を定める。
【００７６】
このようにして、コントローラユニット１３によれば、計算負荷が低い移動体を得ることができる。また、移動体の移動がスムーズとなり環境の動的変化などノイズに強いロバストな移動動作が可能になる。
【００７７】
尚、本発明は上記実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。上記実施形態では、移動体進行方向と計測対象方向とが成す角度が大きくなるほど、コントローラユニット１３は真値よりも距離が長い疑似距離ｒ′を生成していたが、コントローラユニット１３は、同角度が大きくなるほど、真値よりも距離が短い疑似距離ｒ′を生成するようにもできる。例えばロボット本体の胴部の横断面の形状が真円でなく、断面形状中の右前方向の部位がロボット本体よりも外方へ突出して形成されている場合、この右前方向の部位は障害物に接触しやすい。あるいは移動体の筐体が構造的に弱い部位を有し、この構造的に弱い部位が障害物と接触するリスクを減らすように設計することがある。これらの障害物に接触しやすい右前方向の部位や、構造的に弱い部位を保護するためには、右前方向で最短となるようにする（正面方向から右前方向へ向かうにつれて一度疑似距離が小さくなる）処理をコントローラユニット１３が行うようにする。速度方位ポテンシャルや速度量ポテンシャルをメモリに記憶する際に、右前方向の部位や、構造的に弱い部位が存在する方位で疑似距離が小さくなるようなポテンシャル形状を有する速度方位ポテンシャルや速度量ポテンシャルをメモリに書き込むのである。コントローラユニット１３がこれらのポテンシャルをメモリから読み込むことによって、コントローラユニット１３は、移動体進行方向と計測対象方向とが成す角度が大きくなるほど、真値よりも距離が短い疑似距離ｒ′を生成することができる。
【００７８】
また、コントローラユニット１３は、障害物の方位角の大きさに応じて、実計測距離を変化させて疑似距離を生成する疑似距離生成手段として機能する。このコントローラユニット１３は、疑似距離に基づき、移動体が障害物を回避するよう移動体の動作を制御する制御手段として機能する。コントローラユニット１３は、回避対象決定手段、並進速度出力指令値補正処理手段、旋回速度出力指令値補正処理手段としても機能する。各手段はＣＰＵ、ＲＯＭ、ＲＡＭにより実現される。
【００７９】
また、上記実施形態に開示されている複数の構成要素の適宜な組み合わせにより、種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。本発明は、ロボットや全方向移動車のほかに、自動車などにも利用可能であり、自動車等の移動装置、移動制御装置及び制御手法に適用可能である。
【図面の簡単な説明】
【００８０】
【図１】（ａ）は本発明の実施の形態に係る障害物回避機能を有する移動制御装置が実装された移動体の上面図であり、（ｂ）は移動体の正面図である。
【図２】レーザレンジファインダの計測可能領域を示す図である。
【図３】本発明の実施の形態に係る障害物回避機能を有する移動制御装置のブロック図である。
【図４】本発明の実施の形態に係る障害物回避機能を有する移動制御装置が作成する障害物マップの一例を示す図である。
【図５】移動体が前進する時の疑似距離の概念を示す図である。
【図６】移動体が後進する時の疑似距離の概念を示す図である。
【図７】角度と疑似距離との関係を説明するための図である。
【図８】同一地点での前進及び後進の進行方向切り替わりにより周囲環境の障害物位置が変化する疑似障害物マップの概念図である。
【図９】移動体の姿勢変更後の移動により周囲環境の障害物位置が変化する疑似障害物マップの概念図である。
【図１０】移動体として全方向移動車が用いられる場合の障害物マップの一例を示す図である。
【図１１】回避対象の障害物の位置をコントローラユニットが決定する手法を説明するための図である。
【図１２】速度方位ポテンシャルの一例を示す図である。
【図１３】並進速度出力許容方向と回避対象障害物方向との関係を示す図である。
【図１４】移動体の移動速度の出力方向の補正処理を説明するための図である。
【図１５】移動体の移動速度に応じて異なるポテンシャル形状を有する速度方位ポテンシャルの一例を示す図である。
【図１６】速度量ポテンシャルの一例を示す図である。
【図１７】コントローラユニットによる障害物回避速度補正処理のフローチャートである。
【図１８】移動体に対する階層構造指令伝達型の移動制御方法を説明するためのフローチャートである。
【符号の説明】
【００８１】
１…ロボット（移動体）、１Ａ…全方向移動車（移動体）、２，２Ａ…基台、３，４…駆動輪、５，６…モータ、７…前方キャスタ、８…後方キャスタ、９−１２…レーザレンジファインダ（計測手段）、１３…コントローラユニット（障害物回避機能を有する移動制御装置）、１４，１５…回転角度センサ（検出手段）、１６…位置センサ（検出手段）、１７…姿勢センサ（検出手段）、１８…レーザレンジファインダ計測可能領域、１９…経路生成部、２０…目標軌道生成部、２１，２５…演算部、２２…ロボット移動速度指令値生成部、２３…障害物回避速度指令値補正部、２４…車軸回転速度指令値生成部、２６…モータ制御電流指令値生成部、２７…ロボット本体、２８〜３２，３７〜４０，４２〜４４…障害物ないしは物体、３３…移動体進行方位、３４…実位置、３５…疑似位置、３６…疑似障害物位置、４１…駆動輪、４５…障害物位置、４６…最短疑似距離、４７…回避対象障害物方向、４８…回避対象障害物、４９…並進速度出力許容方向、５０…移動体基準位置、５１…移動速度出力方向、５２…回避対象障害物方向、５３…並進速度出力許容方向、５４〜５６…速度方位ポテンシャル、５７…障害物位置、５８…疑似距離、５９…真の距離、６０…疑似位置、６１…真の位置。
【特許請求の範囲】
【請求項１】
移動体と、
この移動体に設けられ、移動体進行方位を検出する検出手段と、
前記移動体に設けられ、障害物の位置およびこの障害物までの実計測距離を計測し、この検出手段が検出した前記移動体進行方位に対する前記障害物の方位角を出力する計測手段と、
この計測手段が出力した前記障害物の方位角の大きさに応じて、前記実計測距離を変化させて疑似距離を生成する疑似距離生成手段と、
この疑似距離生成手段が生成した前記疑似距離に基づき、前記移動体が前記障害物を回避するよう前記移動体の動作を制御する制御手段と、
を備えることを特徴とする障害物回避機能を有する移動制御装置。
【請求項２】
前記疑似距離生成手段は、前記角度が大きくなるほど前記疑似距離を真値よりも長くすることを特徴とする請求項１記載の障害物回避機能を有する移動制御装置。
【請求項３】
前記疑似距離生成手段は、前記疑似距離を入力とし移動指令値を補正した補正移動指令値を出力することを特徴とする請求項１又は請求項２記載の障害物回避機能を有する移動制御装置。
【請求項４】
前記疑似距離生成手段は、前記移動体の周囲に存在する物体のうち、前記疑似距離が最短の計測点を有する物体の位置を回避対象障害物位置として決定する回避対象決定手段を備えることを特徴とする請求項１乃至３のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項５】
前記疑似距離生成手段は、
前記疑似距離および前記移動体自身の大きさに応じて前記移動体が取ることのできる並進速度出力許容方向の方位角と前記疑似距離との関係を定める速度方位ポテンシャル情報を記憶する記憶手段と、
前記移動体が出力する予定の並進移動速度の方向が回避対象障害物方向と前記並進速度出力許容方向との間に存在する場合、前記移動体の速度の出力の方向を、前記並進速度の出力が許容される方向にまで変更する並進速度出力指令値補正処理手段と、
を備えることを特徴とする請求項１乃至４のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項６】
前記疑似距離生成手段は、
前記疑似距離が大きくなるほど前記移動体が取ることのできる並進速度出力許容方向の方位角が０に近づき、前記疑似距離が小さくなるほど前記並進速度出力許容方向の方位角がπ／２に近づくようなポテンシャル形状を有する速度方位ポテンシャル情報を記憶する記憶手段を備えることを特徴とする請求項１乃至５のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項７】
前記疑似距離生成手段は、
前記疑似距離が第１閾値ｒmax以上である場合、前記移動体が取ることのできる並進速度出力許容方向の方位角が０となり、前記疑似距離が前記第１閾値ｒmax以下である場合、前記疑似距離の減少に伴い前記並進速度出力許容方向の方位角がπ／２に近づき、前記疑似距離が第２閾値ｒmin以下である場合、前記並進速度出力許容方向の方位角がπ／２となるようなポテンシャル形状を有する速度方位ポテンシャル情報を記憶する記憶手段を備えることを特徴とする請求項１乃至６のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項８】
前記記憶手段は、
前記移動体の移動速度が速いほど、前記移動体の移動速度が遅いときの疑似距離に比べて遠い擬似距離である段階から許容方位角を制限することを開始し、この移動速度が遅いときの疑似距離に比べて遠い擬似距離である段階でこの許容方位角が飽和状態に到達するよう、前記移動体の移動速度に応じて値が変化するようなポテンシャル形状を有する出力方位ポテンシャル情報を記憶することを特徴とする請求項５乃至７のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項９】
前記並進速度出力指令値補正処理手段により変更された補正並進移動速度出力指令方向に対し、前記移動体が現状の姿勢で速度を出力することが不可能である場合、この移動体が速度を出力可能になるまで前記移動体自身の姿勢を転回するよう出力予定旋回移動速度を変更して旋回速度を発生させる旋回速度出力指令値補正処理手段を更に備えることを特徴とする請求項５乃至８のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項１０】
前記並進移動速度を出力する方向が前記回避対象障害物方向と前記並進速度出力許容方向との間もしくは前記並進速度出力許容方向の近傍に存在する場合において、前記並進移動速度を出力する方向が前記回避対象障害物方向へ近づく旋回速度出力指令値の出力を許可しない旋回速度出力指令値補正処理手段を更に備えることを特徴とする請求項５乃至９のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項１１】
前記疑似距離生成手段は、
速度量ポテンシャルにより制限された状態で前記移動体が出力可能な並進速度の最大値を、前記障害物が無い状態で前記移動体が出力可能な並進速度の最大値で除した値を表す比率と、前記疑似距離との関係を定める速度量ポテンシャル情報を記憶する記憶手段と、
前記移動体の出力予定の並進移動速度量がその出力時に得られる並進速度最大値よりも大きい場合、並進速度出力指令値を並進速度最大値にまで変更する並進速度出力指令値補正手段と、
を備えることを特徴とする請求項１乃至１０のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項１２】
前記疑似距離生成手段は、
前記疑似距離が大きくなるほど、速度量ポテンシャルにより制限された状態で前記移動体が出力可能な並進速度の最大値を、前記障害物が無い状態で前記移動体が出力可能な並進速度の最大値で除した値を表す比率が１に近づき、前記疑似距離が小さくなるほどこの比率が０に近づくようなポテンシャル形状を有する速度量ポテンシャル情報を記憶する記憶手段を備えることを特徴とする請求項１乃至１１のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項１３】
前記疑似距離生成手段は、
前記疑似距離が第３閾値ｒv max以上である場合、速度量ポテンシャルにより制限された状態で前記移動体が出力可能な並進速度の最大値を、前記障害物が無い状態で前記移動体が出力可能な並進速度の最大値で除した値を表す比率が１となり、前記疑似距離が前記第３閾値ｒv max以下である場合、前記疑似距離の減少に伴い前記比率は０に近づき、前記疑似距離が第４閾値ｒv min以下である場合、前記比率が０となるようなポテンシャル形状を有する速度量ポテンシャル情報を記憶する記憶手段を備えることを特徴とする請求項１乃至１２のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項１４】
移動空間に関する外部環境情報を保存し更新可能な記録手段と、
この記録手段に記録された前記外部環境情報をもとに目的地点までの適切な経路を生成可能な経路生成手段と、
この経路生成手段により生成された前記経路を辿るような速度出力指令値を生成する移動速度出力生成手段と、
この移動速度出力生成手段が生成した前記速度出力指令値に対して、速度出力補正を行う請求項１乃至１３に記載の疑似距離生成手段と、
を備えることを特徴とする障害物回避機能を有する移動制御装置。
【請求項１】
移動体と、
この移動体に設けられ、移動体進行方位を検出する検出手段と、
前記移動体に設けられ、障害物の位置およびこの障害物までの実計測距離を計測し、この検出手段が検出した前記移動体進行方位に対する前記障害物の方位角を出力する計測手段と、
この計測手段が出力した前記障害物の方位角の大きさに応じて、前記実計測距離を変化させて疑似距離を生成する疑似距離生成手段と、
この疑似距離生成手段が生成した前記疑似距離に基づき、前記移動体が前記障害物を回避するよう前記移動体の動作を制御する制御手段と、
を備えることを特徴とする障害物回避機能を有する移動制御装置。
【請求項２】
前記疑似距離生成手段は、前記角度が大きくなるほど前記疑似距離を真値よりも長くすることを特徴とする請求項１記載の障害物回避機能を有する移動制御装置。
【請求項３】
前記疑似距離生成手段は、前記疑似距離を入力とし移動指令値を補正した補正移動指令値を出力することを特徴とする請求項１又は請求項２記載の障害物回避機能を有する移動制御装置。
【請求項４】
前記疑似距離生成手段は、前記移動体の周囲に存在する物体のうち、前記疑似距離が最短の計測点を有する物体の位置を回避対象障害物位置として決定する回避対象決定手段を備えることを特徴とする請求項１乃至３のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項５】
前記疑似距離生成手段は、
前記疑似距離および前記移動体自身の大きさに応じて前記移動体が取ることのできる並進速度出力許容方向の方位角と前記疑似距離との関係を定める速度方位ポテンシャル情報を記憶する記憶手段と、
前記移動体が出力する予定の並進移動速度の方向が回避対象障害物方向と前記並進速度出力許容方向との間に存在する場合、前記移動体の速度の出力の方向を、前記並進速度の出力が許容される方向にまで変更する並進速度出力指令値補正処理手段と、
を備えることを特徴とする請求項１乃至４のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項６】
前記疑似距離生成手段は、
前記疑似距離が大きくなるほど前記移動体が取ることのできる並進速度出力許容方向の方位角が０に近づき、前記疑似距離が小さくなるほど前記並進速度出力許容方向の方位角がπ／２に近づくようなポテンシャル形状を有する速度方位ポテンシャル情報を記憶する記憶手段を備えることを特徴とする請求項１乃至５のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項７】
前記疑似距離生成手段は、
前記疑似距離が第１閾値ｒmax以上である場合、前記移動体が取ることのできる並進速度出力許容方向の方位角が０となり、前記疑似距離が前記第１閾値ｒmax以下である場合、前記疑似距離の減少に伴い前記並進速度出力許容方向の方位角がπ／２に近づき、前記疑似距離が第２閾値ｒmin以下である場合、前記並進速度出力許容方向の方位角がπ／２となるようなポテンシャル形状を有する速度方位ポテンシャル情報を記憶する記憶手段を備えることを特徴とする請求項１乃至６のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項８】
前記記憶手段は、
前記移動体の移動速度が速いほど、前記移動体の移動速度が遅いときの疑似距離に比べて遠い擬似距離である段階から許容方位角を制限することを開始し、この移動速度が遅いときの疑似距離に比べて遠い擬似距離である段階でこの許容方位角が飽和状態に到達するよう、前記移動体の移動速度に応じて値が変化するようなポテンシャル形状を有する出力方位ポテンシャル情報を記憶することを特徴とする請求項５乃至７のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項９】
前記並進速度出力指令値補正処理手段により変更された補正並進移動速度出力指令方向に対し、前記移動体が現状の姿勢で速度を出力することが不可能である場合、この移動体が速度を出力可能になるまで前記移動体自身の姿勢を転回するよう出力予定旋回移動速度を変更して旋回速度を発生させる旋回速度出力指令値補正処理手段を更に備えることを特徴とする請求項５乃至８のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項１０】
前記並進移動速度を出力する方向が前記回避対象障害物方向と前記並進速度出力許容方向との間もしくは前記並進速度出力許容方向の近傍に存在する場合において、前記並進移動速度を出力する方向が前記回避対象障害物方向へ近づく旋回速度出力指令値の出力を許可しない旋回速度出力指令値補正処理手段を更に備えることを特徴とする請求項５乃至９のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項１１】
前記疑似距離生成手段は、
速度量ポテンシャルにより制限された状態で前記移動体が出力可能な並進速度の最大値を、前記障害物が無い状態で前記移動体が出力可能な並進速度の最大値で除した値を表す比率と、前記疑似距離との関係を定める速度量ポテンシャル情報を記憶する記憶手段と、
前記移動体の出力予定の並進移動速度量がその出力時に得られる並進速度最大値よりも大きい場合、並進速度出力指令値を並進速度最大値にまで変更する並進速度出力指令値補正手段と、
を備えることを特徴とする請求項１乃至１０のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項１２】
前記疑似距離生成手段は、
前記疑似距離が大きくなるほど、速度量ポテンシャルにより制限された状態で前記移動体が出力可能な並進速度の最大値を、前記障害物が無い状態で前記移動体が出力可能な並進速度の最大値で除した値を表す比率が１に近づき、前記疑似距離が小さくなるほどこの比率が０に近づくようなポテンシャル形状を有する速度量ポテンシャル情報を記憶する記憶手段を備えることを特徴とする請求項１乃至１１のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項１３】
前記疑似距離生成手段は、
前記疑似距離が第３閾値ｒv max以上である場合、速度量ポテンシャルにより制限された状態で前記移動体が出力可能な並進速度の最大値を、前記障害物が無い状態で前記移動体が出力可能な並進速度の最大値で除した値を表す比率が１となり、前記疑似距離が前記第３閾値ｒv max以下である場合、前記疑似距離の減少に伴い前記比率は０に近づき、前記疑似距離が第４閾値ｒv min以下である場合、前記比率が０となるようなポテンシャル形状を有する速度量ポテンシャル情報を記憶する記憶手段を備えることを特徴とする請求項１乃至１２のいずれかに記載の障害物回避機能を有する移動制御装置。
【請求項１４】
移動空間に関する外部環境情報を保存し更新可能な記録手段と、
この記録手段に記録された前記外部環境情報をもとに目的地点までの適切な経路を生成可能な経路生成手段と、
この経路生成手段により生成された前記経路を辿るような速度出力指令値を生成する移動速度出力生成手段と、
この移動速度出力生成手段が生成した前記速度出力指令値に対して、速度出力補正を行う請求項１乃至１３に記載の疑似距離生成手段と、
を備えることを特徴とする障害物回避機能を有する移動制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【公開番号】特開２０１０−１３４７４２（Ｐ２０１０−１３４７４２Ａ）
【公開日】平成２２年６月１７日（２０１０．６．１７）
【国際特許分類】
【出願番号】特願２００８−３１０７４４（Ｐ２００８−３１０７４４）
【出願日】平成２０年１２月５日（２００８．１２．５）
【国等の委託研究の成果に係る記載事項】（出願人による申告）国等の委託研究の成果に係る特許出願（平成１８年度独立行政法人新エネルギー・産業技術総合開発機構「戦略的先端ロボット要素技術開発プロジェクト、ロボット搬送システム（サービスロボット分野）、店舗応用を目指したロボット搬送システムの研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
【公開日】平成２２年６月１７日（２０１０．６．１７）
【国際特許分類】
【出願日】平成２０年１２月５日（２００８．１２．５）
【国等の委託研究の成果に係る記載事項】（出願人による申告）国等の委託研究の成果に係る特許出願（平成１８年度独立行政法人新エネルギー・産業技術総合開発機構「戦略的先端ロボット要素技術開発プロジェクト、ロボット搬送システム（サービスロボット分野）、店舗応用を目指したロボット搬送システムの研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
[ Back to top ]