JP2007148595A - 移動体 - Google Patents

Abstract

【課題】障害物検出手段の障害物検出可能領域が狭い場合でも，確実に障害物を避けながら目標位置に到達でき，かつ使用者が事前の設定によってその挙動を制限できるような移動体を提供する。
【解決手段】未知障害物４０１を回避しながら目標位置８０１に到る軌道を自動的に生成するにあたって、目標位置８０１と現在位置１０１との直線軌道上８０２であって、障害物検出可能領域１２１内の暫定目標位置９０１を逐次生成し、これを通過していくように制御するとともに、暫定目標位置９０１までの間に障害物４０１が検出されれば、障害物検出可能領域１２１内であって、障害物４０１近傍に暫定目標位置９０２を生成し、これを通過するように回避軌道を生成するようにした
【選択図】図９

Description

本発明は、指令された目標位置に障害物を避けながら自動的に移動する移動体に関する。

指令された目標位置に向かって途中にある障害物を観測し避けながら自動的に移動体を移動せしめるために，従来の移動体には以下のような構成としたものがあった。
・障害物のまわりにポテンシャル関数を設定し，その値と空間に対する勾配とを利用して得る軌道上に移動体を駆動制御するもの(例えば，特許文献１)。以下、技術[１]とする。
・目標位置までの軌道を所定ステップ距離ごとに分割した各点における障害物との接触有無を調べ，接触可能性がある場合は障害物のない領域に回避軌道を逐次生成，その上を通るように移動体を駆動制御していくもの（例えば，特許文献２）。以下、技術[２]とする。
・未知障害物を検出したときにはそれが検出されなくなるまで回避指令を発生して障害物を回避せしめ，然る後に目標位置までの経路を再計算するもの(例えば,特許文献３)。以下、技術[３]とする。

技術[１]における基本的な考え方を，特許文献１に開示されている図１２によって説明する。図１２は障害物周りに設定されたポテンシャル関数を３次元的に描いたものであり，図において，１２０１は移動体の位置，１２０２は障害物，１２０３は移動体が移動する移動平面，１２０４は移動平面１２０３上に設定されたポテンシャル関数，１２０５は平面１２０３においてポテンシャル関数１２０４の値が等しい点を結んだポテンシャル関数の等高線である。ここではポテンシャル関数１２０４は障害物１２０２から遠いほど大きな値をとり，障害物１２０２に近いほど小さな値をとるようになっている。したがって障害物１２０２周辺の任意の点においてポテンシャル関数１２０４の勾配を上るように移動体を駆動制御すれば移動体は障害物から遠ざかる向きに移動し，また等高線１２０５上を通るように駆動制御すれば移動体は障害物１２０２の周りを迂回するように移動する。また移動体位置１２０１におけるポテンシャル関数１２０４の値があらかじめ設定した値を下回らないように移動体を駆動制御すれば，移動体が障害物１２０２に接触することはない。このように適切なポテンシャル関数を設定することによって，障害物を回避する軌道を自動的に生成できるのである。

技術[２]を特許文献２に基づき,図１３を用いて説明する。図１３は障害物を回避しつつ目標位置に向かう経路計画の概念説明図であり，図中１３０１は移動体，１３０２は障害物，１３０３は障害物１３０２を仮想的に膨張させた仮想障害物，１３０４は目標位置を表し，ａ０，ａ１，ａ２，・・・，ｂ１，ｂ２，・・・，ｃ１，ｃ２，・・・，ｄ１，ｄ２，・・・は回避軌道探索のために１辺の長さがＬの正方形区画の格子点に設定される点列である。なお，障害物の位置・形状・大きさに関する情報は地形センサによって得られるものとしている。まず移動体の出発点と目標位置との間を直線で結んで第１直線経路を設定する。次に第１直線経路上に所定のステップ距離Ｌ毎に目標位置に向かって点列ａ０，ａ１，ａ２，・・・を設定する。なお,特許文献２では，ステップ距離Ｌは移動体の車両長に相当するものが望ましい，とされている。前記正方形区画は点列ａ０，ａ１，ａ２，・・・をその格子点に含むように設定する。設定した点列が障害物にかかった場合は回避の必要があるため，第１回避軌道を探索する。図１３では点ａ３に続く点ｂ１が仮想障害物１３０３にかかっているため，第１回避軌道の探索が必要となる。第１回避軌道の候補は，はじめ第１直線経路に対して障害物の広がりが少ない方向に設定する。すなわち，第１直線経路に対して障害物中心Ｇの反対側に位置する前記正方形区画の格子点に第１回避軌道の候補ｂ２，ｂ３，ｂ４，ａ４を設定する。もしこの第１回避軌道の候補がすべて障害物にかかっていた場合は，第１直線経路に対して障害物中心Ｇと同じ側に位置する前記正方形区画の格子点にｃ１，ｃ２，ｃ３，ｃ４を設定し，第１回避軌道の候補とする。図１３では点ａ４が障害物にかかっていないため，これを第１回避軌道として設定する。続いてａ４から目標位置までを直線で結び，これを第２直線経路とする。第２直線経路上にステップ距離Ｌ毎に点列ｄ１，ｄ２，・・・を設定し，以後は同様な繰り返しによって目標位置に到るまでの回避軌道が探索される。

技術[３]を特許文献３に基づき,図１５を用いて説明する。ここでは，移動体は道の障害物に関する情報を含まない環境地図を記憶しており，既知障害物を避けつつ目標位置まで移動する経路を計画する手段を有すると仮定されている。図１５において１５０１は移動体初期位置，１５０２は目標位置，１５０３は既知障害物，１５０４は記憶している環境地図に基づく計画経路，１５０５は未知障害物，１５０６は回避軌道，１５０７は回避後の移動体位置，１５０８は再計画経路である。移動体は，最初に事前に記憶していた環境地図中の既知障害物１５０３を回避しながら目標位置１５０２に到達するための計画経路１５０４を生成し，移動を開始する，途中で未知障害物１５０５が検出されると，前記未知障害物１５０５が移動体に搭載されている障害物検出手段で検出されなくなるまで回避動作を行う。そのときの経路が回避軌道１５０６である。未知障害物１５０５が検出されなくなると回避動作を終了し，回避後の移動体位置１５０７から目標位置１５０２に到る再計画経路１５０８を生成し再度移動を開始する。これを移動体が目標位置１５０２に到達するまで繰り返す。未知障害物が検出されている間は回避動作を持続するため，検出された障害物を確実に避けることができる。
特開平０５−２９７９４４号公報（第６頁，図２） 特開平０７−６４６３３号公報（第６頁，図１） 特開２００４−４２１４８号公報（第１２頁，図１）

技術[１]は，適切なポテンシャル関数を設定するには障害物の位置・形状・大きさを考慮せねばならず，あらかじめ正確な環境地図を保持している必要があった。一般に移動体に搭載される障害物検出手段は計測可能な情報に制限があることが多いため，未知障害物が存在した場合はポテンシャル関数設定のための情報が十分に得られない可能性が高い。また，広大な環境で移動体を使用する場合や環境の形状が複雑な場合は設定するポテンシャル関数も複雑になって必要な記憶容量や計算量が増大するという問題もあった。更に，ポテンシャル関数の勾配が０となる点では移動体が使用者の意に反して停止してしまい，目標位置に向かう軌道に復帰できなくなるという問題もあった。この問題は特許文献２などでも指摘されている。

技術[２]も，回避軌道の探索のためには障害物の全体形状が既知である必要があるため，実現が困難であるという問題があった。また，探索の結果得られた回避軌道や直線経路が移動体に搭載された障害物検出手段の検出可能領域外を通る場合は，移動途中に障害物を検知できずに障害物に接触してしまう恐れもあった。あるいは，障害物が非常に大きい場合などには得られる回避軌道が著しく初期位置や目標位置から外れ，使用者の意図しない位置にまで到達する危険性もあった。目標位置までの経路探索が終了してから移動を開始するため，移動中に新たな障害物が出現した場合には衝突の危険が生じ，既存の障害物が消滅した場合には必要以上に経路が長くなってしまうという問題もあった。

技術[３]は未知障害物の存在を考慮しているが，障害物検出手段で未知障害物が検出されなくなるまで回避動作を持続するため，障害物検出手段の検出可能領域が広い場合は結果的に得られる移動ロボットの軌道が必要以上に長くなる可能性があった。逆に障害物検出手段の障害物検出可能領域が狭い場合は障害物が近くに存在していても回避動作を終了してしまうため，移動ロボットが障害物に衝突する恐れがあった。

技術[３]の問題点の例を図１０に示して説明する。図１０は障害物検出可能領域が広い場合の例であって，１０１は移動体，１２１は障害物検出可能領域，８０１は目標位置，４０２は障害物，８１０は生成された回避軌道であり，１０１’と１２１’は１０１の位置から回避軌道８１０に沿って一定時間移動した後の移動体および障害物検出可能領域である。技術[3]では障害物検出可能領域から障害物が出るまで，すなわち移動体が１０１の位置から１０１’の位置に到るまで，回避動作を持続する。よって目標位置８０１に到る軌道は８１０のようになり，移動体は無用に長い距離を走行して目標位置に到達するのである。

また,上記技術の他の問題点を図１１に例を示して説明する。図１１において４０２−１および４０２−２は障害物である。この例では障害物４０２−２が障害物検出可能領域外にあるため，回避軌道８１０は障害物４０２−２を回避できていない。障害物検出可能領域を考慮せずに生成された回避軌道に沿って移動すると，移動体が障害物に衝突する恐れが非常に高いのである。

従来の移動体は，障害回避軌道の決定にあたって障害物検出手段の検出可能領域が考慮されておらず，障害物検出可能領域が狭い場合には上に示したような問題がどうしても発生する。障害物検出可能領域を広くとるには，移動体にあらかじめ精細な環境地図を記憶させておくか，複数の障害物検出手段を搭載したり，特に広範囲を監視できるような特殊な障害物検出手段を用いたりする必要があり，装備が大掛かりになるという問題があった。とりわけ移動体が前後・左右に自在に移動可能な全方向移動体であった場合は，全方位をくまなく監視する全方位障害物検出手段を搭載するなどの措置が必要であった。また，従来の技術では移動体が障害回避軌道を自動的に生成することに主眼が置かれており，使用者が状況に応じてその挙動を調整することについては全く考慮されていなかった。

本発明はこのような問題点に鑑みてなされたものであり、障害物検出手段の障害物検出可能領域が狭い場合でも，大規模な環境地図を必要とせず，できるだけ単純な演算によって未知障害物が存在する環境中を確実に障害物を避けながら目標位置に到達でき，なおかつ使用者が事前の設定によってその挙動を制限できるような移動体を提供することを目的とする。

本発明によるところの移動体は，上記問題を解決するため次のように構成する。

請求項１記載の発明は、回転を含めた全方位へ移動体を駆動できる移動手段と、前記移動体の移動空間における位置・姿勢を検出する位置・姿勢検出手段と、前記移動体からのある一定の領域に存在する障害物を検出する障害物検出手段と、前記障害物が検出されたときは、前記位置・姿勢の情報と前記障害物の情報とから、前記障害物を避ける回避軌道を逐次生成し、前記移動手段に目標位置までの移動指令を出力する制御手段と、を有する自律移動型の移動体において、前記制御手段は、現在位置から前記目標位置までの直線軌道上であって、前記障害物検出手段の検出可能領域内に設定する暫定目標位置を逐次生成して通過していくよう前記移動体を制御するとともに、前記直線軌道上に前記障害物が検出されたときは、該障害物近傍であって、現在位置における前記障害物検出手段の検出可能領域内に設定する暫定目標位置を通過する前記回避軌道を適時生成して前記移動体を制御することを特徴とした移動体とするものである。
請求項２記載の発明は、前記制御手段は、前記移動体が前記回避軌道上にあるときも、前記移動体の現在位置から目標位置に向かう直線あるいは曲線が、前記障害物検出手段の検出可能領域内に入るような前記回避軌道を随時生成することを特徴とする請求項１記載の移動体とするものである。
請求項３記載の発明は、前記制御手段は、前記障害物検出手段の検出可能領域よりも狭く設定した障害物認知距離内において検出された前記障害物のみを、前記回避軌道の回避対象とすることを特徴とする請求項１記載の移動体とするものである。
請求項４記載の発明は、前記障害物認知距離は、前記移動体の使用者が自由に設定できることを特徴とする請求項３記載の移動体とするものである。
請求項５記載の発明は、前記障害物認知距離は、前記移動手段が最高速度で運転されているときの前記移動体の制動距離よりも大きな値に設定されており、前記使用者が前記障害物認知距離の設定値を前記制動距離よりも小さく設定した場合は、前記制御手段に更に備えたモニタに、設定値不正信号を送出して前記使用者に通知するとともに、前記設定値を前記制動距離に変更することを特徴とする請求項４記載の移動体とするものである。
請求項６記載の発明は、前記制御手段は、前記暫定目標位置から前記目標位置までの距離を算出し、該距離が所定の閾値距離以上になれば、前記移動体を停止させることを特徴とした請求項１記載の移動体とするものである。
請求項７記載の発明は、前記閾値距離は、前記使用者が自由に設定できることを特徴とする請求項６記載の移動体とするものである。
請求項８記載の発明は、前記閾値距離の設定値が、前記使用者によって前記初期位置から前記目標位置までの距離よりも小さな値に設定された場合は、前記制御手段に更に備えたモニタに、設定値不正信号を送出して前記使用者に通知することを特徴とする請求項７記載の移動体とするものである。
請求項９記載の発明は、前記制御手段は、前記移動体の現在位置から前記目標位置までの線分と、前記現在位置から前記暫定目標位置までの線分と、のなす角度が予め設定した閾値角以上になれば、前記移動体を停止させることを特徴とした請求項１記載の移動体とするものである。
請求項１０記載の発明は、前記閾値角は、前記使用者が自由に設定できることを特徴とする請求項９記載の移動体とするものである。
請求項１１記載の発明は、前記暫定目標位置は、前記移動体の現在位置における前記移動体の制動距離よりも遠い位置に設定されることを特徴とする請求項１記載の移動体とするものである。
請求項１２記載の発明は、前記制御装置は、前記暫定目標位置を設定することができなかったとき、前記制御手段に更に備えたモニタに、通行不能信号を送出して前記使用者に通知するとともに、前記移動体を停止させることを特徴とした請求項１記載の移動体とするものである。
請求項１３記載の発明は、前記障害物検出手段は、水平面内のある一定の角度範囲をスキャンして前記障害物の位置を検知するレーザーレンジファインダを用いることを特徴とする請求項１記載の移動体とするものである。
請求項１４記載の発明は、前記障害物検出手段は、前記レーザーレンジファインダと、前記移動体の正面の各側面に並設された複数の超音波センサと、からなることを特徴とした請求項１記載の移動体とするものである。
請求項１５記載の発明は、前記移動手段は、少なくとも３つのサーボモータと、該サーボモータの各々の出力軸に接続されたオムニホイルとから構成されていることを特徴とする請求項１記載の移動体とするものである。

請求項１に記載の発明によると，前記障害物検出手段の検出可能領域が限定されている場合でも，確実に障害物を検出することができ，移動体走行時の安全性を著しく向上させることができる。また，前記障害物検出手段の検出可能領域は必ずしも広範な領域である必要がなくなるため，移動体の製造コスト低減にも寄与する。

請求項２に記載の発明によれば，前記障害物検出手段の検出可能領域が限定されている場合でも，特に障害物が動的に変化するような環境において，移動体が目標位置に到る軌道をできるだけ短くできる。

請求項３に記載の発明によれば，前記障害物認知距離よりも遠い位置にある障害物を考慮することなく回避軌道を探索することによって計算量を削減できる。

請求項４に記載の発明によれば，移動体を使用する環境に応じて前記障害物認知距離を変更することによって，移動体の障害回避性能と計算量とのバランスを調整することができる。

請求項５に記載の発明によれば，前記障害物認知距離を不適切に短く設定してしまった場合でも，移動体が障害物に接触する事態を未然に防ぎ，使用者の不注意を警告することができる。

請求項６〜１０に記載の発明によれば，目標位置に対する回避軌道の乖離を比較的単純な演算結果によって表現・通知できるとともに、使用者が移動体の監視基準を自由に設定できる。また、閾値設定が不正であることを使用者に通知し，閾値の再設定を促すことができる。

請求項１１,１２に記載の発明によれば、確実に停止できる暫定目標位置を設定でき、使用者は移動体が回避軌道を生成できなかったことを知ることができ，これに迅速に対処できる。

請求項１３,１４に記載の発明によれば，実用的な障害物検出可能領域を有する障害物検出手段を構成できる。

請求項１５に記載の発明によれば、移動手段にサーボモータとオムニホイルを使用することで、回転・全方位の移動が簡単に実現できる。

以下、本発明の実施の形態について図を参照して説明する。

図１，図２は、本発明の移動体の側視図および下視図である。図において、１０１は移動体，１０２は移動体１０１に固定された障害物検出手段であって，本実施例ではレーザーレンジファインダである。レーザーレンジファインダとは，水平面内の一定角度範囲をレーザーポイント光でスキャンし，発光から受光までの時間を計測することで障害物の表面までの距離と向きを計測するスキャン型センサである。また移動体１０１の側面には，障害物検出手段として図示しない超音波センサが並べてある。超音波センサは環境に向けて超音波を発射し，障害物からの反射波を検出するまでの時間から障害物までの距離とおおよその向きを計測するセンサである。一般に超音波センサで検出可能な障害物までの距離はレーザーレンジファインダよりも小さく，障害物の向きも正確には分からない。本実施例では，超音波センサは主にレーザーレンジファインダ１０２の死角に接近した障害物を検出するための補助的な役割を担う。１０３−１から１０３−３は移動体の駆動力発生手段であって，本実施例では減速器付きサーボモータである。１０４−１から１０４−３はオムニホイル，１０５は絶対座標系のｘ軸，１０６は絶対座標系のｙ軸，１０７は絶対座標系のｚ軸，１０８は絶対座標系における移動体のｘ軸に対する角度θ，１１０はオムニホイル１０４の半径ｒ，１１１は移動体１０１の中心からオムニホイル１０４の接地点までの距離Ｒである。１２１はレーザーレンジファインダ１０２および図示しない超音波センサの障害物検出可能領域である。矢印１３０は移動体１０１の正面方向で，レーザーレンジファインダ１０２の正面方向に等しく，障害物検出可能領域の中心である。以下では矢印１３０の向きを単に正面方向と記述し，移動体の姿勢は正面方向とｘ軸とのなす角とし，前記θに相当する。なお，図１および図２はθ＝０の場合を描いたものである。移動体１０１の動作制御は，図示しない制御回路により行う。

図３は本実施例における障害物検出可能領域１２１を移動体１０１の上視図に重ねて示したものである。図を簡単化するため，移動体１０１は外形のみを描いてある。レーザーレンジファインダ１０２の障害物検出可能領域は半径ｄ_{ｒａｎｇｅ}，内角φ_{ｒａｎｇｅ}の扇形で，扇形の中心から移動体正面方向に向かう直線について対称になっている。図中の破線３０１は移動体１０１に外接し，かつ障害物検出可能領域１２１の内向き頂点に接する補助直線であって，補助直線３０１の移動体正面方向１３０に対する傾き角度３０２をφ_ｔｈとする。角度３０２は，移動体の設計図上で超音波センサとレーザーレンジファインダ１０２の障害物検出可能範囲を図示し，補助直線３０１を描くことで求めることができる。移動体正面方向に対し，移動体の並進移動の向きがφ_ｔｈよりも小さければ，移動体１０１は障害物検出可能領域１２１から出ることがない。逆に移動体の並進移動の向きがφ_ｔｈよりも大きいと，移動体１０１の一部が障害物検出可能領域１２１以外の部分を通るため，検出できなかった障害物に接触する可能性が生じるのである。したがってφ_ｔｈをあらかじめ求めておき，移動体正面方向に対する移動体１０１の並進移動方向がφ_ｔｈを超えないように監視する必要がある。
３１１はレーザーレンジファインダ１０２の計測基準点から移動体外縁部までの最大距離で，移動体１０１を円とみなしたときの半径に相当し，ｄ_ｖｃｌと表現する。

図４はレーザーレンジファインダ１０２の出力から得られる障害物情報の例である。４０１−１から４０１−４の斜線領域は障害物，実線４０２は検出された障害物の表面である。障害物４０１−３のように検出可能領域外にある障害物や，４０１−４のように他の障害物の陰にある障害物は検出されない。出力値は移動体正面方向に対する角度φ_ｏｂｓとその向きに存在する障害物までの距離ｄ_ｏｂｓの対であり，ｄ_ｏｂｓ（φ_ｏｂｓ）のように角度φ_ｏｂｓの関数として表現できる。

サーボモータ１０３およびオムニホイル１０３は，移動体１０１の中心周りにその回転軸が互いに１２０度をなすように配置されている。サーボモータの回転速度と移動体の移動速度には次の運動学的関係式が成り立つことが知られている。

式中ω_ｉ（ｉ＝１，２，３）はモータ１０３−ｉの回転速度，ｖ＝(ｖ_ｘ，ｖ_ｙ，ω)^Ｔは移動体１０１の絶対座標Ｘ，Ｙ，θの時間変化率すなわち移動体の移動速度（速度，回転速度），Ｇ_ｒはモータ１０３に付属の減速器の減速比である。数１によって所望の移動速度に対して発生すべきモータ回転速度が計算でき，数２によってエンコーダを通して観測したモータ回転速度から移動体移動速度を計算できる。移動体の位置・姿勢ｐ＝(Ｘ，Ｙ，θ)^Ｔはｖを逐次積分していくことで求めることができる。

上記数１,数２から分かるように，移動体１０１はモータの回転速度を適切に設定することによって前後・左右・回転の動作を自在に制御可能である。したがって障害物検出可能領域１２１以外の方向にも常に移動が可能であることに注意せねばならない。すなわち，生成される回避軌道が不適切であった場合，検出できなかった障害物に移動体が接触する事態が起こり得るのである。これは移動体の並進移動の向きが前記φ_ｔｈよりも大きい場合に相当する。また移動体が高速で移動する場合，障害物を検出してから減速を開始しても障害物との接触が避けられないという事態も起こり得る。しかし，本発明の適用により，これを防止することができるのである。

次いで，図５により移動体１０１の制御回路の構成を説明する。図において５０１はパーソナルコンピュータ，５０２はサーボアンプ，５０３はキーボード，５０４はモニタ，５１１はモータ回転速度指令信号，５１２はモータ回転速度フィードバック信号，５１３は障害物情報，５１４は設定値信号，５１５は通知信号である。パーソナルコンピュータ５０１はサーボアンプ５０２からモータ回転速度フィードバック信号５１２を，レーザーレンジファインダ１０２と超音波センサから障害物検出可能領域内の障害物情報５１３を，キーボード５０３から設定値信号５１４を，それぞれ取得し，移動体のとるべき回避軌道を計算し，得られた回避軌道に応じて必要なモータ回転速度指令信号５１１を計算し，サーボアンプ５０２に出力する。設定値信号５１４は，パーソナルコンピュータ５０１内部の処理で使用する障害物認知距離閾値ｄ_ｔｈおよび動作許容半径ｄ_{ｌｉｍｉｔ}であって，キーボード５０３であらかじめ使用者が設定する。またパーソナルコンピュータ５０１は，処理中に使用者へ通知すべき事態が発生した場合，モニタ５０３に通知信号５１５を出力してその旨を通知する。

図６はパーソナルコンピュータ５０１内部の処理を示したブロック図である。
図中６０１は運動学計算処理，６０２は積分処理，６０３は目標位置・姿勢記憶手段，６０４は回避経路生成処理，６０５は移動体目標速度計算処理，６０６は逆運動学計算処理，６０７は閾値処理，６０８は設定値補正処理，６１１は目標位置・姿勢信号ｐ_ｄ＝(Ｘ_ｄ，Ｙ_ｄ，θ_ｄ)^Ｔ，６１２は移動体速度フィードバック信号ｖ＝(ｖ_ｘ，ｖ_ｙ，ω)^Ｔ，６１３は移動体位置・姿勢フィードバック信号ｐ＝(Ｘ，Ｙ，θ)^Ｔ，６１４は位置・姿勢指令信号，６１５は移動体目標速度信号ｖ_ｄ＝(ｖ_ｘｄ，ｖ_ｙｄ，ω_ｄ)^Ｔである。
運動学計算処理６０１は，数２を用いてモータ回転速度フィードバック信号５１２から移動体速度フィードバック信号６１２を計算する。この際使用する移動体の姿勢θには，後述の移動体位置フィードバック信号６１３を用いる。
積分処理６０２は得られた移動体速度フィードバック信号６１２を積分し，移動体位置・姿勢フィードバック信号６１３を得る。
目標位置・姿勢記憶手段６０３は移動体の目標位置・姿勢をあらかじめに記憶させておくためのもので，目標位置・姿勢信号６１１として順次読み出せるようにしておく。
回避軌道生成処理６０４では障害物情報５１３と目標位置・姿勢信号６１１と移動体位置・姿勢フィードバック信号６１２とを用い，回避経路として位置・姿勢指令信号６１４を出力する。
移動体目標速度計算処理６０５は，移動体位置・姿勢フィードバック信号６１３の位置・姿勢指令信号６１４に対する偏差，および移動体速度フィードバック信号６１２，を用いて，移動体位置・姿勢を位置・姿勢指令信号６１４に一致させるように移動体目標速度信号６１５を加減し出力する。
逆運動学計算処理６０６では数１を用いて移動体目標速度信号６１５からモータ回転速度指令信号５１１を計算する。この際使用する移動体の姿勢θには，移動体位置・姿勢フィードバック信号６１３を用いる。
閾値処理６０７は，障害物情報５１３から設定された障害物認知距離閾値ｄ_ｔｈよりも遠くにある障害物情報を切り捨てる処理である。すなわち，ｄ_ｏｂｓ（φ_ｏｂｓ）ｄ_ｔｈ＞である全ての方向φ_ｏｂｓについては，障害物の有無を考慮しない。この結果，レーザーレンジファインダ１０２の障害物検出可能領域は半径ｄ_ｔｈ，中心角φ_{ｒａｎｇｅ}の扇形となる。遠くの障害物に関する情報を切り捨てることによって計算量を削減することができる。更にｄ_ｔｈは障害物の回避を開始する最長の障害物までの距離であるから，生成される回避軌道の形状をｄ_ｔｈによって調整することもできる。
設定値補正処理６０８は，設定値５１４の値が不正でないかを確認し，不正な値であった場合は設定値不正を通知する通知信号５１５を出力するとともに適正な値に置き換える。距離閾値ｄ_ｔｈについては，移動体１０１が最高速度で走行しているときに最大加速度で減速して停止するまでに走行する距離である最大制動距離ｄ_ｓｔｏｐと前記距離閾値ｄ_ｔｈとを比較し，ｄ_ｓｔｏｐ＞ｄ_ｔｈである場合は設定値不正通知信号を発生し，ｄ_ｔｈ＝ｄ_ｓｔｏｐとする。またｄ_ｔｈ＞ｄ_{ｒａｎｇｅ}である場合は設定値不正通知信号を発生するとともにｄ_ｔｈ＝ｄ_{ｒａｎｇｅ}とする。ｄ_ｓｔｏｐ≦ｄ_ｔｈ≦ｄ_{ｒａｎｇｅ}である場合はｄ_ｔｈをそのまま用いる。
動作許容半径ｄ_{ｌｉｍｉｔ}については，移動体位置・姿勢フィードバック信号６１３にもとづく移動体位置と目標位置との距離と比較し，ｄ_{ｌｉｍｉｔ}＜（（Ｘ−Ｘｄ）^２＋（Ｙ−Ｙｄ）^２）^１／２であった場合は設定値不正通知信号を発生し，ｄ_{ｌｉｍｉｔ}＝（（Ｘ−Ｘｄ）^２＋（Ｙ−Ｙｄ）^２）^１／２とする。ｄ_{ｌｉｍｉｔ}≧（（Ｘ−Ｘｄ）^２＋（Ｙ−Ｙｄ）^２）^１／２であった場合はｄ_{ｌｉｍｉｔ}をそのまま用いる。

前記回避軌道生成処理６０４における回避軌道生成までの流れを，図７を用いて説明する。生成中の回避軌道は暫定目標位置・姿勢ｐ_ｄｔｍｐ＝(ｘ_ｄｔｍｐ，ｙ_ｄｔｍｐ，θ_ｄｔｍｐ)^Ｔとして表現し，暫定目標位置・姿勢を抽出して位置・姿勢指令６１４として出力するものとする。

ステップＳ０では処理１を行う。処理１とは，入力された目標位置・姿勢信号６１１を前記暫定目標位置・姿勢信号として新たに設定する処理である。

ステップＳ１ではループ処理を開始する。

ステップＳ２では判断１を実行し，論理が真であればステップＳ３へ，偽であればステップＳ５へ進む。判断１とは，移動体位置・姿勢フィードバック信号６１３の該ステップにおける暫定目標位置・姿勢信号に対する偏差が十分小さいか否かの判断である。具体的には，目標位置・姿勢に対する誤差ノルム閾値ｃ_１を定めておき，論理式数３の真偽を調べる。
|ｐ−ｐ_ｄｔｍｐ| ≦ｃ_１ ・・・（数３）

ステップＳ３では判断２を実行し，論理が真であれば目標位置へ到達したものとして移動を完了し，偽であればステップＳ４に進む。判断２とは，該ステップの暫定目標位置・姿勢信号が目標位置・姿勢信号６１１に一致しているか否かの判断である。具体的には，論理式数４の真偽を調べる。
ｐ_ｄｔｍｐ＝ｐ_ｄ ・・・（数４）

ステップＳ４では処理１を実行し，ステップＳ５に進む。

ステップＳ５では判断３を実行し，論理が真であればステップＳ７へ，偽であればステップＳ６へ進む。判断３とは，移動体位置・姿勢フィードバック信号６１３に基づく移動体の現在位置と該ステップの暫定目標位置とを結ぶ線分である通過予定軌道の長さが，前記距離閾値ｄ_ｔｈから余有距離ｄ_{ｍａｒｇｉｎ}を引いた距離以下であるか否かの判断である。具体的な論理式は数５である。ここでｄ_{ｍａｒｇｉｎ}はｄ_ｔｈよりも小さく，前記ｄ_ｖｃｌよりも大きな値である。
|ｐ−ｐ_dtmp|≦ｄ_th−ｄ_margin ・・・（数５）

ステップＳ６では処理２を実行し，ステップＳ７に進む。処理２とは，前記通過予定軌道上に前記移動体の現在位置からｄ_ｔｈ−ｄ_{ｍａｒｇｉｎ}だけ離れた点をとり，もって新たな暫定目標位置とし，前記通過予定軌道の向きφ_pathを新たな暫定目標姿勢とする処理である。ここでφ_pathは移動体正面方向１３０を基準として表現するものとする。

ステップＳ７では判断４を実行し，論理が真であればステップＳ８へ，偽であればステップＳ１１へ進む。判断４とは，前記通過予定軌道の向きφ_pathに移動体が直進した場合に通過する領域である通過予定領域が障害物検出可能領域１２１内部に含まれるか否かの判断である。具体的には，前記φ_ｔｈを用いた次の論理式を用いる。
｜φ_path｜＜φ_th ・・・（数６）
本実施例では判断４に前記φ_ｔｈと論理式数６を用いたが，移動体の通過予定領域が障害物検出可能領域に含まれることが確実に分かればよいのであって，実施の形態を限定する必要はない。

ステップＳ８では判断５を実行し，論理が真であればステップＳ９へ，偽であればステップＳ１０へ進む。判断５とは，前記通過予定領域上に障害物が存在するか否かの判断である。具体的には，得られた点列ｄ_ｏｂｓ（φ_ｏｂｓ）と前記通過予定軌道との距離が全てのφ_ｏｂｓについてｄ_{ｍａｒｇｉｎ}を上回っているか否かを調べる。そのために，
ｄ_obs(φ_obs) ≦((Ｘ−Ｘ_dtmp)²＋(Ｙ−Ｙ_dtmp)²)^1/2＋ｄ_margin ・・・（数７）
を満足する全てのφ_ｏｂｓに対し
ｄ_obs(φ_obs)ｓｉｎ(φ_path−φ_obs)＞ｄ_margin ・・・（数８）
が成り立っておれば判断５は真，それ以外の場合は偽とする。数７は現在の移動体位置から暫定目標位置までの距離にｄ_{ｍａｒｇｉｎ}を加えた距離よりも近い障害物を選別するものである。数８の意味は図８で説明する。図中８０１が暫定目標位置，破線線分８０２が通過予定軌道，破線線分８０３は障害物表面の点から前記通過予定軌道に下ろした垂線である。図から線分８０３の長さはｄ_obs(φ_obs)ｓｉｎ(φ_path−φ_obs)であることが分かる。したがって数８を満足する障害物上の点は移動体の通過予定領域外にあることが保証される。

ステップＳ９では処理３を実行してステップＳ１０に進む。処理３とは，検出されている障害物に接触しない位置に暫定目標位置・姿勢を新たに設定し直す処理であり，φ_pathの向きにできるだけ近く，かつ通過予定領域に障害物が存在しないような新たな暫定目標位置を探索する。探索の結果はステップＳ１０に渡す。

ステップＳ１０では判断６を実行する。判断６とは，前段のステップＳ９における処理３によって新しい暫定目標位置の設定に失敗したか否かの判断である。真であれば回避軌道なしとしてステップＳ１４に進み，ループを抜ける。偽であればステップＳ１５に進む。

ステップＳ１１では処理４を実行し，ステップＳ１７に進む。処理４とは，暫定目標位置に移動体位置・姿勢フィードバック信号６１３に基づく移動体の現在位置を，暫定目標姿勢に(θ＋φ_path)を，それぞれ設定する処理である。この処理は，前記通過予定軌道が障害物検出可能領域の中に入るように修正するものである。

ステップＳ１２では，上記処理を経て得られた暫定目標位置・姿勢を該ステップの位置・姿勢指令信号６１４としてそのまま出力する。

ステップＳ１３は該ステップの終端であり，次のステップＳ１に戻る。

ステップＳ１４では通行不能信号を出力してループを抜け，移動体１０１を停止させる。

ステップＳ１５では判断７を実行し，論理が真であればステップＳ１６へ，偽であればステップＳ１２へ進む。判断７とは，新しい暫定目標位置・姿勢が目標位置・姿勢信号６１１と大きく乖離しているか否かの判断である。具体的には，新しい暫定目標位置と目標位置・姿勢信号６１１にもとづく目標位置との距離を計算し，動作許容半径ｄ_{ｌｉｍｉｔ}を上回っているか否かを調べる。すなわち
|ｐ_ｄ−ｐ_ｄｔｍｐ| ＞ｄ_{ｌｉｍｉｔ} ・・・（数９）
の論理式の真偽を調べる。

ステップＳ１６では回避軌道規格外信号を出力してループを抜け，移動体１０１を停止させる。

ステップＳ１７では位置・指令信号６１４を，位置・姿勢フィードバック信号６１３にもとづく移動体位置および得られた暫定目標姿勢で構成し出力する。位置・指令信号６１４は，移動体１０１の位置は固定し姿勢だけを変更する指令となる。

本実施例における移動体１０１の動作例を図９により説明する。図９は回避のための暫定目標位置を設定し移動するまでの一連の流れを示したものである。図９(a)では目標位置８０１に向かうときの通過予定軌道８０２が障害物検出可能領域１２１の外部にあるため，途中の障害物４０１が検出できていない。まずステップＳ０により目標位置８０１が暫定目標位置として設定されるが，目標位置までの距離がｄ_th−ｄ_marginより大きいためステップＳ５からＳ６によって暫定目標位置が点９０１に変更される。続いて暫定目標位置９０１に対する通過予定軌道の向きφ_pathが｜φ_path｜＜φ_thを満足していないため，ステップＳ７からステップ１１に進み，φ_pathの向きが位置・姿勢指令として出力される。移動体１０１は出力された暫定目標姿勢に向かって回転し，(b)の状態に移る。(b)では障害物検出可能領域１２１内部に障害物４０１が存在するため，ステップＳ８からステップＳ９に進み，回避のための暫定目標位置９０２が(c)のように設定される。暫定目標位置９０２に対する通過予定軌道の向きφ_pathも｜φ_path｜＜φ_thを満足していないため，ステップＳ７からステップ１１に進み，φ_pathの向きが位置・姿勢指令として出力される。移動体１０１が回転すると(d)の状態となる。暫定目標位置９０２に到る通過予定領域上に障害物がないことが確認できるので，ステップＳ８における判断結果は偽となって，暫定目標位置９０２が位置・姿勢指令として出力される。移動体１０１が暫定目標位置９０２に到達すると，ステップＳ２の判断が真となってステップＳ４に進み，目標位置８０１が新たな暫定目標位置として再設定される。
以上のように，移動を開始する前に通過予定領域が障害物検出可能領域に含まれるような動作を行わせることによって，確実に障害物を検出できるのである。

本実施例の判断７で新しい暫定目標位置・姿勢と目標位置・姿勢信号６１１との乖離を調べるにあたっては，両者の距離|ｐ_ｄ−ｐ_ｄｔｍｐ|を用いた。が，現在位置から目標位置に到る線分と現在位置から暫定目標位置に到る線分とのなす角などを用いて両者の乖離を調べてもよい。あるいは，移動体初期位置を記憶しておき，移動体初期位置と目標位置とを結ぶ線分と暫定目標位置との距離を調べるようにしてもよい。

本発明が従来技術と異なるのは，未知障害物を回避しながら目標位置に到る軌道を自動的に生成するにあたって障害物検出手段の障害物検出可能領域を明確に考慮して確実に障害物を検出できるようにした点である。また，回避軌道探索の範囲に一定の制限を設け，回避軌道が想定外の領域にまで到ることを防げるようにした点も従来技術とは異なっている。また，逐次更新される障害物検出手段の出力を用いるため，時間とともに変化するような環境中でも効果を発揮することができる。

障害物検出可能領域が狭い場合でも障害物を確実に検出できるため，構造上あるいは予算上の制約によって広域を監視できるセンサを備えることができない多くの移動ロボットに適用可能である。例えば建物内を行き来する荷物搬送ロボットや未知の環境を走行する探索ロボット，デザイン上の制約が大きいペットロボットなどにも用いることができる。

本発明の第１実施例を示す移動体の側視図 本発明の第１実施例を示す移動体の下視図 本発明の第1実施例における障害物検出可能領域を示した図 レーザーレンジファインダから得られる障害物情報の例を示した図 本発明の第1実施例を示す移動体の制御回路構成図 本発明の第1実施例を示す移動体のパーソナルコンピュータ内部の処理を示したブロック図 本発明の第1実施例を示す移動体の回避軌道生成までの流れ図 本発明の第1実施例を示す移動体の通過予定領域上に障害物が存在するか否かの判断法を説明するための図 本発明の第1実施例を示す移動体の動作例 従来の技術[3]の問題点を示した図 従来の技術[1][2][3]の問題点を示した図 従来の技術[1]を示した図 従来の技術[2]を示した図 従来の技術[3]を示した図

符号の説明

１０１ 移動体
１０２ レーザーレンジファインダ
１０３−１，１０３−２，１０３−３ サーボモータ
１０４−１，１０４−２，１０４−３ オムニホイル
１０５ 絶対座標系のｘ軸
１０６ 絶対座標系のｙ軸
１０７ 絶対座標系のｚ軸
１０８ 絶対座標系における移動体のｘ軸に対する角度θ
１１０ オムニホイル１０４の半径ｒ
１１１ 移動体１０１の中心からオムニホイル１０４の接地点までの距離Ｒ
１２１，１２１’ 障害物検出可能領域
１３０ 移動体１０１の正面方向
３０１ 補助直線
３０２ 補助直線３０１の移動体正面方向１３０に対する傾き角度φ_ｔｈ
４０１，４０１−１，４０１−２，４０１−３ 障害物
４０２ 検出された障害物の表面
５０１ パーソナルコンピュータ
５０２ サーボアンプ
５０３ キーボード
５０４ モニタ
５１１ モータ回転速度指令信号
５１２ モータ回転速度フィードバック信号
５１３ 障害物情報
５１４ 設定値信号
５１５ 通知信号
６０１ 運動学計算処理
６０２ 積分処理
６０３ 目標位置・姿勢記憶手段
６０４ 回避経路生成処理
６０５ 移動体目標速度計算処理
６０６ 逆運動学計算処理
６０７ 閾値処理
６０８ 設定値補正処理
６１１ 目標位置・姿勢信号ｐ_ｄ
６１２ 移動体速度フィードバック信号ｖ
６１３ 移動体位置・姿勢フィードバック信号ｐ
６１４ 位置・姿勢指令信号
６１５ 移動体目標速度信号ｖ_ｄ
８０１ 暫定目標位置
８０２ 通過予定軌道
８０３ 障害物表面の点から前記通過予定軌道に下ろした垂線
９０１，９０２ 暫定目標位置
１２０１ 移動体の位置
１２０２ 障害物
１２０３ 移動体が移動する移動平面
１２０４ ポテンシャル関数
１２０５ ポテンシャル関数の等高線
１３０１ 移動体
１３０２ 障害物
１３０３ 仮想障害物
１３０４ 目標位置
１５０１ 移動体初期位置
１５０２ 目標位置
１５０３ 既知障害物
１５０４ 計画経路
１５０５ 未知障害物
１５０６ 回避軌道
１５０７ 回避後の移動体位置
１５０８ 再計画経路

Claims (15)

1. 回転を含めた全方位へ移動体を駆動できる移動手段と、
   前記移動体の移動空間における位置・姿勢を検出する位置・姿勢検出手段と、
   前記移動体からのある一定の領域に存在する障害物を検出する障害物検出手段と、
   前記障害物が検出されたときは、前記位置・姿勢の情報と前記障害物の情報とから、前記障害物を避ける回避軌道を逐次生成し、前記移動手段に目標位置までの移動指令を出力する制御手段と、を有する自律移動型の移動体において、
   前記制御手段は、現在位置から前記目標位置までの直線軌道上であって、前記障害物検出手段の検出可能領域内に設定する暫定目標位置を逐次生成して通過していくよう前記移動体を制御するとともに、前記直線軌道上に前記障害物が検出されたときは、該障害物近傍であって、現在位置における前記障害物検出手段の検出可能領域内に設定する暫定目標位置を通過する前記回避軌道を適時生成して前記移動体を制御することを特徴とした移動体。
2. 前記制御手段は、前記移動体が前記回避軌道上にあるときも、前記移動体の現在位置から目標位置に向かう直線あるいは曲線が、前記障害物検出手段の検出可能領域内に入るような前記回避軌道を随時生成することを特徴とする請求項１記載の移動体。
3. 前記制御手段は、前記障害物検出手段の検出可能領域よりも狭く設定した障害物認知距離内において検出された前記障害物のみを、前記回避軌道の回避対象とすることを特徴とする請求項１記載の移動体。
4. 前記障害物認知距離は、前記移動体の使用者が自由に設定できることを特徴とする請求項３記載の移動体。
5. 前記障害物認知距離は、前記移動手段が最高速度で運転されているときの前記移動体の制動距離よりも大きな値に設定されており、前記使用者が前記障害物認知距離の設定値を前記制動距離よりも小さく設定した場合は、前記制御手段に更に備えたモニタに、設定値不正信号を送出して前記使用者に通知するとともに、前記設定値を前記制動距離に変更することを特徴とする請求項４記載の移動体。
6. 前記制御手段は、前記暫定目標位置から前記目標位置までの距離を算出し、該距離が所定の閾値距離以上になれば、前記移動体を停止させることを特徴とした請求項１記載の移動体。
7. 前記閾値距離は、前記使用者が自由に設定できることを特徴とする請求項６記載の移動体。
8. 前記閾値距離の設定値が、前記使用者によって前記初期位置から前記目標位置までの距離よりも小さな値に設定された場合は、前記制御手段に更に備えたモニタに、設定値不正信号を送出して前記使用者に通知することを特徴とする請求項７記載の移動体。
9. 前記制御手段は、前記移動体の現在位置から前記目標位置までの線分と、前記現在位置から前記暫定目標位置までの線分と、のなす角度が予め設定した閾値角以上になれば、前記移動体を停止させることを特徴とした請求項１記載の移動体。
10. 前記閾値角は、前記使用者が自由に設定できることを特徴とする請求項９記載の移動体。
11. 前記暫定目標位置は、前記移動体の現在位置における前記移動体の制動距離よりも遠い位置に設定されることを特徴とする請求項１記載の移動体。
12. 前記制御装置は、前記暫定目標位置を設定することができなかったとき、前記制御手段に更に備えたモニタに、通行不能信号を送出して前記使用者に通知するとともに、前記移動体を停止させることを特徴とした請求項１記載の移動体。
13. 前記障害物検出手段は、水平面内のある一定の角度範囲をスキャンして前記障害物の位置を検知するレーザーレンジファインダを用いることを特徴とする請求項１記載の移動体。
14. 前記障害物検出手段は、前記レーザーレンジファインダと、前記移動体の正面の各側面に並設された複数の超音波センサと、からなることを特徴とした請求項１記載の移動体。
15. 前記移動手段は、少なくとも３つのサーボモータと、該サーボモータの各々の出力軸に接続されたオムニホイルとから構成されていることを特徴とする請求項１記載の移動体。