経路探索装置及び移動システム
【課題】安全かつ最短な経路を探索する。
【解決手段】取得部１１１は、周囲に存在する物体までの位置を示す測域データを取得する。地図データ生成部１１２は、取得部１１１により取得された測域データを二次元の座標空間にプロットすることで、２次元の地図データを生成する。特定部１１３は、地図データにおいて歩行者を示す点群を特定する。投票部１１４は、投票空間を生成し、生成した投票空間を構成する各格子に、特定部１１３により特定された歩行者を示す点群を構成する測域データを投票する。安全率算出部１１５は、投票部１１４による投票結果にしたがって、各格子の安全率を算出する。探索部１１６は、安全率が算出された投票空間において、スタート位置を示す格子とゴール位置を示す格子との距離が最短となり、かつ、安全率が最も高くなるような経路を所定のアルゴリズムを用いて探索する。
【解決手段】取得部１１１は、周囲に存在する物体までの位置を示す測域データを取得する。地図データ生成部１１２は、取得部１１１により取得された測域データを二次元の座標空間にプロットすることで、２次元の地図データを生成する。特定部１１３は、地図データにおいて歩行者を示す点群を特定する。投票部１１４は、投票空間を生成し、生成した投票空間を構成する各格子に、特定部１１３により特定された歩行者を示す点群を構成する測域データを投票する。安全率算出部１１５は、投票部１１４による投票結果にしたがって、各格子の安全率を算出する。探索部１１６は、安全率が算出された投票空間において、スタート位置を示す格子とゴール位置を示す格子との距離が最短となり、かつ、安全率が最も高くなるような経路を所定のアルゴリズムを用いて探索する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、スタート位置からゴール位置までの経路を探索する技術に関するものである。
【背景技術】
【０００２】
移動ロボットの自律移動には自己位置推定や地図構築が重要な要素となっている。しかしながら、屋外環境では気候等の影響により路面の状況が変化する等、環境が動的に変化しやすいため、移動時の環境状況に応じて目的地まで安全に走行できる経路探索手法が必要である。
【０００３】
環境状況に応じた自律移動手法として、画像情報からロボットが走行できる路面の領域を判別して路面上を走行する方法や、路面以外の障害物を検出して危険を回避する方法が知られている。また，環境の走行を繰り返すことで状況変化の多い領域、危険度の高い領域を学習し、安全に移動できる領域地図を生成する手法も提案されている。しかしながら、このような地図の生成には非常に労力を要するといった問題がある。
【０００４】
また、非特許文献１には、画像データから障害物と人間の位置情報を抽出することで、環境内を歩行している人間によって掃引された床面領域を移動可能領域とみなす地図を作成し、作成した地図を基に、経路計画を行う技術が開示されている。
【先行技術文献】
【非特許文献】
【０００５】
【非特許文献１】「移動ロボットによる歩行者観察に基づく地図作製と自律移動のための一手法」田中完爾、岡田伸宏廣、近藤英二、日本機械学会論文集、７０巻６９３号（２００４−５）、論文Ｎｏ．０３−０４３７
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、非特許文献１の手法では、ある床領域を掃引した人間の人数に関わらず、少なくとも１人の人間が掃引した床面領域は移動可能領域としてみなされているため、掃引した人間の人数に応じて床面領域がどの程度安全か否かを評価することができない。つまり、多くの人間が掃引した床面領域ほど安全である可能性が高いことが推定されるが、引用文献１の手法では、掃引した人間の人数を考慮に入れて床面領域の安全性の評価が行われていないため、より安全な経路を探索するためには更なる改善の余地がある。
【０００７】
本発明の目的は、安全かつ最短な経路を探索することができる経路探索装置及びその経路探索装置を備える移動システムを提供することである。
【課題を解決するための手段】
【０００８】
（１）本発明の一局面による経路探索装置は、周囲に存在する物体までの位置を示す測域データを取得する取得手段と、前記取得手段により取得された測域データを二次元の座標空間にプロットすることで、前記物体の位置を示す２次元の地図データを生成する地図データ生成手段と、前記地図データから周囲に存在する各物体を示す点群を抽出し、抽出した点群のうち所定の歩行者条件を満たしている点群を、歩行者を示す点群として特定する特定手段と、前記地図データが示す二次元の座標空間を格子状に区画することで投票空間を生成し、生成した投票空間を構成する各格子に、前記特定手段により特定された歩行者を示す点群を構成する各測域データを投票する投票手段と、前記投票手段による投票結果が高い格子ほどに、安全率が高くなるように各格子の安全率を算出する安全率算出手段と、前記安全率が算出された投票空間において、所定のスタート位置を示す格子と所定のゴール位置を示す格子との距離が最短となり、かつ、前記安全率が最も高くなるような経路を所定のアルゴリズムを用いて探索する探索手段とを備えることを特徴とする。
【０００９】
この構成によれば、周囲に存在する物体の位置を示す測域データが取得され、取得された測域データが二次元の座標空間にプロットされ、各物体を示す点群が抽出され、抽出された点群の中から歩行者を示す点群が特定される。そして、二次元の座標空間を格子状に区画することで得られる投票空間に、歩行者を示す点群を構成する測域データが投票され、投票結果が高い格子ほどに、安全率が高くなるように各格子の安全率が算出される。これにより、投票空間は、多くの歩行者が通った格子ほど高い安全率が算出された２次元の地図データとなる。そして、この投票空間を用いて、スタート位置を示す格子と所定のゴール位置とを示す格子との距離が最短となり、かつ、安全率が最も高くなるような経路が探索される。
【００１０】
これにより、多くの歩行者が通るために安全性が高く、かつ、スタート位置からゴール位置までの距離が最短となるような経路を探索することができる。よって、スタート位置からゴール位置まで自動運転する移動システム（ロボット）に本構成を採用すると、障害物との衝突を回避させると同時に、最短経路で移動体をゴール位置まで誘導させることができる。
【００１１】
（２）前記特定手段は、点群の移動速度及び形状を前記歩行者条件として採用することが好ましい。
【００１２】
この構成によれば、点群の移動速度及び形状が歩行者条件として採用されているため、歩行者を示す点群を精度良く特定することができる。
【００１３】
（３）前記歩行者条件における点群の形状は、点群を構成する点の個数及び点群の幅を含むことが好ましい。
【００１４】
この構成によれば、点群の個数及び幅が歩行者条件の形状として採用されているため、点群をより精度良く特定することができる。
【００１５】
（４）前記測域データは、地面からの高さが歩行者の膝下の高さとなるように水平方向に向けてレーザビームを照射することで測定されたものであり、前記投票手段は、歩行者を示す点群を構成する測域データが投票された格子を中心として当該格子の投票値を所定の二次元分布関数に従って変化させることで周囲の格子の投票値を設定することが好ましい。
【００１６】
この構成によれば、歩行者の胴体のサイズを考慮に入れた投票を行うことができる。また、二次元分布関数に従って投票値が変化されているため、投票値をなだらかに変化させることができる。
【００１７】
（５）前記特定手段は、前記歩行者条件を満たさない点群を障害物を示す点群として特定し、前記投票手段は、前記歩行者を示す点群の測域データについては投票値を正の値にして投票し、前記障害物を示す点群を構成する測域データについては投票値を負の値にして投票することが好ましい。
【００１８】
この構成によれば、障害物を示す点群と歩行者を示す点群とが明確に区別されるように安全率を算出することができる。
【００１９】
（６）前記探索手段は、Ａ＊アルゴリズムを用いて経路を探索することが好ましい。
【００２０】
この構成によれば、距離が最短となり、かつ、安全率の高い経路をより正確に探索することができる。
【００２１】
（７）ＧＰＳセンサと方位センサとを更に備え、前記安全率算出手段は、前記ＧＰＳセンサにより測定された自己の現在位置を示す位置データと前記方位センサにより測定された方位データとを基に、前記投票空間の座標系をグローバル座標系に変換し、前記探索手段は、前記ＧＰＳセンサにより測定された自己の現在位置を示す格子を前記投票空間に設定し、設定した現在位置を示す格子を前記スタート位置とすることで、前記経路を探索することが好ましい。
【００２２】
この構成によれば、投票空間の座標系をグローバル座標系で表すことができるため、ＧＰＳセンサにより測定された自己の現在位置をスタート位置としてゴール位置までの経路を探索することができる。また、グローバル座標系における位置データ（例えば、緯度、経度）を入力することで、ゴール位置の設定を行うことができる。また、ＧＰＳセンサ及び方位センサを備えているため、ＧＰＳセンサ及び方位センサから自己の現在位置及び進行方向が探索した経路からずれているか否かを検出することができる。
【発明の効果】
【００２３】
本発明によれば、多くの歩行者が通るために安全性が高く、かつ、スタート位置からゴール位置までの距離が最短となるような経路を探索することができる。
【図面の簡単な説明】
【００２４】
【図１】本発明の一実施の形態による移動システムの外観構成図を示している。
【図２】図１に示す移動システムの電気的な構成を示すブロック図である。
【図３】図１に示す移動システムの機能的な構成を示したブロック図である。
【図４】本発明の実施の形態による移動システムが経路を探索する際の動作を示すフローチャートである。
【図５】歩行者条件を示した表である。
【図６】（Ａ）は地図データ生成部により生成された地図データの一例を示す模式図であり、（Ｂ）は（Ａ）に示す地図データから歩行者を示す点群が抽出された地図データを示す模式図である。
【図７】（Ａ）は測域データがプロットされた地図データにおいて投票空間の分解能程度に分解能が低くされた地図データを示し、（Ｂ）は測域データが投票された投票空間を示している。
【図８】探索部により探索された経路の一例を示した模式図である。
【図９】探索部により探索された経路に沿って移動システムが移動する際の処理を示すフローチャートである。
【図１０】ナビゲーション実験によって得られた移動安全領域地図データと、探索した経路と、移動システムの走行経路とを示している。
【図１１】（Ａ）〜（Ｄ）は、走行中の移動システムの様子を示した図である。
【発明を実施するための形態】
【００２５】
以下、本発明の一実施の形態による移動システムについて説明する。図１は、本発明の一実施の形態による移動システムの外観構成図を示している。図１に示すように移動システムは、車椅子型の移動システムであって、測域センサ１、経路探索部２、方位センサ３、タイヤ部４、台５、フレーム６、操作部７、及びＧＰＳセンサ８を備えている。
【００２６】
測域センサ１は、例えばレーザレンジファインダにより構成され、移動システムの周囲に位置する物体までの距離及び方向を示す測域データを測定する。具体的には、測域センサ１は、地上から歩行者の膝下程度の高さの水平面上において、レーザビームの射出方向を扇状に変更してレーザビームをスキャンし、物体からの反射光を受光することで周囲に位置する物体までの測域データを測定する。ここで、測域センサ１は、例えば、水平面上において、真正面の方向を基準として時計回りの方向を正とすると、−９０度から＋９０度の１８０度の範囲でレーザビームをスキャンすることで測域データを測定する。
【００２７】
なお、測域データには、測域センサ１の真正面の方向を基準としたときの検出した物体の方向を示す方向データと、測域センサ１を基準としたときの物体までの距離を示す距離データとが含まれている。また、方向データとしては、レーザビームの照射角度が採用されている。
【００２８】
また、測域センサ１は、台５の前方に取り付けられているため、移動システムの前方において、数十メートル〜数百メートル（例えば最大８０メートル）を半径とする半円状の範囲内に位置する物体の測域データを測定することになる。なお、本実施の形態では、測域センサ１としては、例えば、ＳＩＣＫ（ジック）社製のＬＭＳ２００が採用されている。
【００２９】
経路探索部２は、コンピュータにより構成され、本実施の形態では、例えばノートブックパソコンが採用されている。方位センサ３は、例えば、３次元の磁気方位センサにより構成され、ヨー角の角度成分を、グローバル座標系の真北の方向と移動システムの真正面の方向との間の角度として測定し、測定した角度を方位データとして経路探索部２に出力する。本実施の形態では、方位センサ３としては、例えばHoneywell（ハネウエル）社製のTruePoint HMR3500が採用されている。
【００３０】
タイヤ部４は、フレーム６の下部に取り付けられた４つのタイヤにより構成され、移動システムを地面に沿って移動させる。なお、４つのタイヤのうち、後方に設けられた左右２つの半径の大きなタイヤには、それぞれ、左モータＭ１及び右モータＭ２が取り付けられ、両タイヤは、左右モータＭ１，Ｍ２からの駆動力を受けて回転する。なお、前方に設けられた半径の小さな２個のタイヤは補助輪である。
【００３１】
台５は、経路探索部２等のコンピュータを設置する台により構成されている。操作部７は、例えばジョイスティックにより構成され、移動システムをマニュアル運転する場合に使用される。
【００３２】
フレーム６は、例えば四角柱状の部材により構成され、車椅子の骨格部分を形成し、下部にはタイヤが取り付けられ、中央部には台５が取り付けられ、前方左手には操作部７が取り付けられている。
【００３３】
ＧＰＳセンサ８は、例えば、differentialＧＰＳセンサ（相対即位方式のＧＰＳセンサ）により構成され、フレーム６の前方左手側に立設された柱８１の上端に取り付けられ、移動システムのグローバル座標系における位置データを取得する。ここで、位置データは、緯度成分及び経度成分を含んでいる。本実施の形態では、ＧＰＳセンサ８としては、例えば、hemisphere社製のＡ１００が採用されている。
【００３４】
図２は、図１に示す移動システムの電気的な構成を示すブロック図である。図２に示すように、移動システムは、測域センサ１、経路探索部２、方位センサ３、操作部７、ＧＰＳセンサ８、左モータ駆動部９（タイヤ駆動手段の一例）、右モータ駆動部１０（タイヤ駆動手段の一例）、モータ制御部１１（タイヤ駆動手段の一例）、左モータＭ１（タイヤ駆動手段の一例）、及び右モータＭ２（タイヤ駆動手段の一例）を備えている。
【００３５】
経路探索部２は、入力装置２１、ＲＯＭ（リードオンリメモリ）２２、ＣＰＵ（中央演算処理装置）２３、ＲＡＭ（ランダムアクセスメモリ）２４、外部記憶装置２５、表示装置２６、記録媒体駆動装置２７、及び入出力インターフェイス（Ｉ／Ｆ）２９を備えている。入力装置２１、ＲＯＭ２２、ＣＰＵ２３、ＲＡＭ２４、外部記憶装置２５、表示装置２６、記録媒体駆動装置２７、及びＩ／Ｆ２９は、バスラインに接続され、このバスラインを介して種々のデータ等が入出され、ＣＰＵ２３の制御の下、種々の処理が実行される。
【００３６】
入力装置２１は、キーボード、マウス等から構成され、ユーザが種々のデータを入力するために使用される。ＲＯＭ２２には、ＢＩＯＳ（Basic Input/Output System）等のシステムプログラムが記憶される。外部記憶装置２５は、ハードディスクドライブやＳＳＤ（Solid State Drive）等から構成され、所定のＯＳ（Operating System）及びコンピュータを経路探索部２として機能させるための制御プログラムが記憶されている。ＣＰＵ２３は、外部記憶装置２５からＯＳ等を読み出し、各ブロックの動作を制御する。ＲＡＭ２４は、ＣＰＵ２３の作業領域等として用いられる。
【００３７】
表示装置２６は、液晶表示装置等から構成され、ＣＰＵ２３の制御の下に種々の画像を表示する。記録媒体駆動装置２７は、ＣＤ−ＲＯＭドライブ、フレキシブルディスクドライブ等から構成される。
【００３８】
なお、記録媒体２８は、ＣＤ−ＲＯＭ等のコンピュータ読み取り可能な記録媒体２８により構成され、コンピュータを経路探索部２として機能させるための制御プログラムを格納する。ユーザはこの記録媒体２８を記録媒体駆動装置２７に読み込ませることで、制御プログラムをコンピュータにインストールする。
【００３９】
Ｉ／Ｆ２９は、例えばＵＳＢ規格に準拠する複数のポートにより構成され、測域センサ１、方位センサ３、ＧＰＳセンサ８、及びモータ制御部１１が接続されている。モータ制御部１１には、左モータ駆動部９及び右モータ駆動部１０が接続されている。モータ制御部１１には、操作部７が接続されている。
【００４０】
モータ制御部１１は、経路探索部２から出力される指示信号にしたがって、左モータＭ１及び右モータＭ２のそれぞれが経路探索部２により指定された回転速度で回転するように左モータ駆動部９及び右モータ駆動部１０を制御する。左モータ駆動部９及び右モータ駆動部１０は、モータ制御部１１の制御のしたがって、左モータＭ１及び右モータＭ２のそれぞれを指定された回転速度で回転させる。
【００４１】
図３は、図１に示す移動システムの機能的な構成を示したブロック図である。図３に示すように移動システムは、取得部１１１（取得手段の一例）、地図データ生成部１１２（地図データ生成手段の一例）、特定部１１３（特定手段の一例）、投票部１１４（投票手段の一例）、安全率算出部１１５（安全率算出手段の一例）、探索部１１６（探索手段の一例）、移動制御部１１７（移動制御手段の一例）、地図データ記憶部１２１、及び移動安全領域地図データ記憶部１２２を備えている。
【００４２】
取得部１１１〜移動制御部１１７は、経路探索部２のＣＰＵ２３により構成され、地図データ記憶部１２１、及び移動安全領域地図データ記憶部１２２は、経路探索部２のＲＡＭ２４等により構成されている。
【００４３】
取得部１１１は、測域センサ１により測定された移動システムの周囲に存在する物体までの距離及び方向を示す測域データを取得する。また、取得部１１１は、ＧＰＳセンサ８により測定された移動システムのグローバル座標系における位置データを取得する。また、取得部１１１は、方位センサ３により測定された移動システムの方位データを取得する。また、取得部１１１は、入力装置２１を操作することでユーザにより入力されたゴール位置を示す位置データを取得する。ここで、ゴール位置を示す位置データとしては、グローバル座標系における緯度成分と緯度成分と含む位置データが採用される。
【００４４】
地図データ生成部１１２は、取得部１１１により取得された測域データを二次元の座標空間にプロットすることで、物体の位置を示す２次元の地図データを生成する。ここで、地図データ生成部１１２は、例えば以下のようにして、地図データを生成する。
【００４５】
まず、地表面を上側から見たときの座標平面であって、測域センサ１の真正面の方向が第１の座標軸として設定され、この第１の座標軸に直交する方向が第２の座標軸として設定された座標平面を２次元の座標空間として設定する。次に、設定した座標空間に、測域センサ１の位置を示す基準点をプロットする。次に、測域データに含まれる方向データからプロットするべき測域データの基準点からの向きを定め、測域データに含まれる距離データからプロットするべき測域データの基準点からの距離を定める。次に、定めた向きと距離とにより特定される座標空間の座標位置に測域データをプロットする。このような処理を、測域センサ１から出力する各測域データに対して実行し、地図データを生成する。
【００４６】
また、地図データ生成部１１２は、測域センサ１に一定の時間間隔（スキャン周期）を空けてレーザビームを複数回スキャンさせ、測域センサ１が１回スキャンする度に１枚の地図データを生成することで、複数枚の地図データを生成する。なお、生成された複数枚の地図データは地図データ記憶部１２１に記憶される。
【００４７】
特定部１１３は、地図データ生成部１１２により生成された各地図データにおいて、各物体を示す点群を抽出し、抽出した点群のうち所定の歩行者条件を満たしている点群を、歩行者を示す点群として特定する。具体的には、特定部１１３は、以下のようにして、歩行者を示す点群を特定する。まず、各地図データにおいて、一塊の点群、すなわち、連続して存在する点群を１つの物体を示す点群として抽出する。
【００４８】
次に、各地図データにおいて、幅及び構成点数が所定の歩行者条件を満たす点群を、歩行者の候補となる候補点群として特定する。次に、異なる地図データ間において、同一の物体を示す候補点群同士を対応付け、対応付けた候補点群の移動速度を算出する。次に、算出した移動速度が所定の歩行者条件を満たす点群を、歩行者を示す点群として特定する。
【００４９】
なお、候補点群の移動速度は、例えば下記のようにして求めればよい。まず、各地図データを、動画像を構成する１枚のフレームとしてとらえ、対応付けた候補点群を時系列に並べる。次に、時系列に並べた各候補点群の重心位置を特定する。次に、特定した重心位置のフレーム間における移動距離を求める。次に、求めた移動距離を測域センサ１によるスキャン周期で割ることで各候補点群のフレーム間での移動速度を求め、求めた移動速度の平均値を候補点群の移動速度として算出する。
【００５０】
なお、特定部１１３は、所定の歩行者条件を満たさなかった点群を障害物を示す点群として特定する。
【００５１】
投票部１１４は、地図データが示す二次元の座標空間を格子状に区画することで投票空間を生成し、生成した投票空間を構成する各格子に、特定部１１３により特定された歩行者を示す点群を構成する測域データを投票する。
【００５２】
ここで、投票部１１４は、歩行者を示す点群を構成する測域データが投票された格子を中心として当該格子の投票値を所定の二次元分布関数に従って変化させることで周囲の格子の投票値を設定し、設定した投票値で周囲の格子に投票する。ここで、二次元分布関数としては例えば二次元ガウス分布関数を採用することができる。
【００５３】
また、投票部１１４は、歩行者を示す点群の測域データについては投票値を正の値（例えば「＋１」）にして投票し、障害物を示す点群を構成する測域データについては投票値を負の値（例えば「−１」）にして投票する。これにより、安全率を算出するに際して、障害物と歩行者とを明確に区別することができる。
【００５４】
また、投票部１１４は、障害物を示す点群を構成する測域データについても歩行者を示す点群を構成する測域データと同様にして、測域データが投票された格子を中心として当該格子の投票値を二次元ガウス分布関数に従って変化させることで周囲の格子の投票値を設定する。
【００５５】
安全率算出部１１５は、投票部１１４による投票結果が高い格子ほどに、安全率が高くなるように各格子の安全率を算出する。具体的には、安全率算出部１１５は、投票部１１４が各格子に対して投票した投票値の和を求めることで各格子の安全率を算出し、移動安全領域地図データを生成する。なお、生成された移動安全領域地図データは移動安全領域地図データ記憶部１２２に記憶される。
【００５６】
また、安全率算出部１１５は、ＧＰＳセンサ８により測定された移動システムのグローバル座標系における位置と、方位センサ３により測定された移動システムの方位とから、二次元の座標空間に設定された第１及び第２の座標軸をグローバル座標系に変換することでプロットした各測域データのグローバル座標系における位置を求める。
【００５７】
探索部１１６は、安全率が算出された投票空間において、スタート位置を示す格子と予め定められたゴール位置を示す格子との距離が最短となり、かつ、安全率が最も高くなるような経路を所定のアルゴリズム（例えばＡ＊アルゴリズム）を用いて探索する。
【００５８】
なお、スタート位置としては、地図データ生成部１１２が地図データにプロットした基準点のグローバル座標系における位置データを採用することができる。
【００５９】
移動制御部１１７は、経路探索部２によって探索された経路にしたがって、移動システムが移動するように、モータ制御部１１に指示信号を出力し、左モータＭ１及び右モータＭ２を駆動させる。ここで、移動制御部１１７は、移動システムを右方向に旋回させる場合は、左モータＭ１の回転速度が右モータＭ２の回転速度より大きくなるように、モータ制御部１１に指示信号を出力する。一方、移動制御部１１７は、移動システムを左方向に旋回させる場合は、右モータＭ２の回転速度が左モータＭ１の回転速度よりも大きくなるように、モータ制御部１１に指示信号を出力する。
【００６０】
次に、本発明の実施の形態による移動システムの動作について説明する。図４は、本発明の実施の形態による移動システムが経路を探索する際の動作を示すフローチャートである。
【００６１】
まず、ステップＳ１において、取得部１１１は、ＧＰＳセンサ８により測定された移動システムの現在位置を示す位置データを取得すると共に、方位センサ３により測定された移動システムの方位を示す方位データを取得する。
【００６２】
次に、取得部１１１は、測域センサ１により測定された測域データを取得する（ステップＳ２）。ここで、測域センサ１は、上述したように、スキャン周期にしたがってレーザビームを複数回スキャンする。スキャン周期としては、歩行者の移動速度を求めるにあたり好適な値を採用することが好ましい。
【００６３】
次に、地図データ生成部１１２は、測域センサ１がレーザビームを１回スキャンする度に、測定した測域データを２次元の座標空間にプロットすることで、複数枚の地図データを生成する（ステップＳ３）。この場合、生成される地図データは、第１の座標軸が、例えば測域センサ１の真正面の方向に設定され、第２の座標軸が、例えば第１の座標軸と直交する方向に設定される。
【００６４】
次に、特定部１１３は、ステップＳ３で作成された複数枚の地図データを用いて、各地図データから移動システムの周囲に存在する各物体を示す点群を抽出し、抽出した点群が所定の歩行者条件を満たすか否かにより、歩行者を示す点群と障害物を示す点群とを特定する（ステップＳ４）。
【００６５】
図５は、歩行者条件を示した表である。図５に示すように、歩行者条件としては、点群の幅ｗ（ｃｍ）、構成点数Ｎ（個）、及び移動速度ｖ（ｃｍ／ｓｅｃ）が採用されている。本実施の形態では、幅ｗとして例えば、５≦ｗ≦６０が採用され、構成点数Ｎとして例えば１≦Ｎ≦３０が採用され、移動速度ｖとして例えばｖ≧１２が採用されている。
【００６６】
したがって、特定部１１３は、各地図データにおいて、まず、幅ｗ及び構成点数Ｎが図５に示す条件を満たす点群を歩行者を示す点群候補として特定する。そして、各地図データを動画像のフレームとみたてて、各フレームにおいて同一の歩行者を示す点群候補を対応付けて時系列に並べる。
【００６７】
そして、時系列に並べた点群候補の重心位置のフレーム間での移動距離を求め、移動距離をスキャン周期で割ることで、対応付けた点群候補のフレーム間での移動速度を求め、求めた移動速度の平均値を、対応付けた点群候補の移動速度として算出する。そして、算出した移動速度が図５に示す条件を満たす点群候補を、歩行者を示す点群として特定する。
【００６８】
なお、特定部１１３は、例えばあるフレームのある点群候補に対して最も近接する次のフレーム内の点群候補を同一物体を示す点群候補として対応付ければよい。
【００６９】
また、地図データから歩行者を示す点群を抽出する手法として、人の足首の高さをスキャンすることで得られる点群の幾何的特徴量を求めるＡｒｒａｓ達が開発した手法が知られている。したがって、このＡｒｒａｓ達が開発した手法を用いて歩行者を示す点群を特定してもよいし、Ａｒｒａｓ達が開発した手法と上述した歩行者を示す点群を特定する手法とを組み合わせることで歩行者を示す点群を特定してもよい。
【００７０】
なお、Ａｒｒａｓ達が開発した手法は、「Kai O.Arras，Oscar Martinez Mozos，Wolfman Burgard: “Using Boosted Features for the Detection of People in 2D Range Data”，IEEE International Conference on Robotics and Automation，pp.3402-3407. 2007.」に開示されている。
【００７１】
図６（Ａ）は地図データ生成部１１２により生成された地図データの一例を示す模式図であり、（Ｂ）は（Ａ）に示す地図データから歩行者を示す点群が抽出された地図データを示す模式図である。
【００７２】
図６（Ａ）に示すように、二次元の座標空間内において、複数の測域データがプロットされていることが分かる。そして、上述した歩行者を示す点群を特定する処理を実行した結果、図６（Ｂ）に示すように中央部の濃い黒丸で示す３つの点群が歩行者を示す点群として特定されていることが分かる。そして、それ以外の両端にプロットされた点群は、歩行者条件を満たしていないため、障害物として特定されていることが分かる。
【００７３】
次に、投票部１１４は、地図データが示す二次元の座標空間を格子状に区画することで投票空間を生成する（ステップＳ５）。
【００７４】
次に、投票部１１４は、ステップＳ３で生成した各地図データにおいて、歩行者を示す点群を構成する測域データについては、投票値を１として投票空間の各格子に投票すると共に、障害物を示す点群を構成する測域データについては、投票値を−１として投票空間の各格子に投票する（ステップＳ６）。
【００７５】
図７（Ａ）は投票空間の分解能程度に分解能が低くされた地図データを示し、（Ｂ）は測域データが投票された投票空間を示している。
【００７６】
図７（Ｂ）においては、グレーの色が濃くなるにつれて安全率が高いことを示し、黒色は安全率が負の値であることを示している。また、図７（Ｂ）においては、各格子の大きさは１０（ｃｍ）四方とされている。
【００７７】
図７（Ａ）において歩行者を示す測域データＤ１は、図７（Ｂ）において格子Ｋ１に位置するため、投票値が１とされて格子Ｋ１に投票されている。したがって、投票部１１４は、格子Ｋ１を中心として二次元ガウス分布関数を当てはめ、格子Ｋ１の投票値である１を、当てはめた二次元ガウス分布関数に従って変化させ、格子Ｋ１の周囲の格子の投票値を設定し、設定した投票値で周囲の格子に投票する。
【００７８】
ここで、二次元ガウス分布関数を当てはめるのは、歩行者の胴体の領域も安全であると考えたからである。つまり、測域センサ１は、地上から歩行者の膝下程度の高さの水平面上においてレーザビームを走査しているため、歩行者を示す点群のサイズは、歩行者の膝下周りのサイズを有することになる。したがって、二次元ガウス分布関数を当てはめて、投票する格子領域に広がりを持たせることで、歩行者の胴体のサイズを考慮に入れた投票を行うことができる。よって、二次元ガウス分布関数の半値幅としては、中心となる格子から人間の胴回りの半径程度（例えば２０ｃｍ）の領域内に含まれる格子が投票されるような値を採用することが好ましい。
【００７９】
例えば、図７（Ｂ）においては、格子Ｋ１を中心として格子Ｋ１を除く格子３個分の半径の円状の領域Ｒ１内に位置する各格子が投票されている。また、領域Ｒ１内の各格子の投票値は格子Ｋ１の投票値である１をガウス関数に従って変化させた値が設定されている。よって、領域Ｒ１において格子Ｋ１から離れる格子ほど投票値が低くなっている。
【００８０】
このようにして、投票部１１４は、ステップＳ３で作成した複数枚の地図データに含まれる各測域データを図７（Ｂ）に示す１枚の投票空間に投票していく。また、障害物を示す点群を構成する測域データについては、投票値を−１として投票を行う。そして、歩行者及び障害物を示す点群を構成する測域データについては、二次元ガウス分布関数に従って周囲の格子にも投票を行う。
【００８１】
次に、安全率算出部１１５は、各格子において、投票部１１４が投票した投票値の合計値を求め、求めた合計値を各格子の安全率として算出する（ステップＳ７）。例えば、図７（Ｂ）に示す格子Ｋ１においては、図７（Ａ）に示す測域データＤ１を投票する際に設定された投票値と測域データＤ１の周囲に位置する測域データＤ２を投票する際に設定された投票値とによって投票されることになる。そのため、これらの測域データＤ１が投票される際に格子Ｋ１に対して投票された投票値の合計値が格子Ｋ１の安全率として算出されることになる。
【００８２】
これにより、同じ領域を複数の歩行者が通過すると、その度に安全率が増加することになる。つまり、より多くの歩行者が通過する領域ほど、安全率が高い領域になる。なお、安全率の値には上限値を設けてもよい。
【００８３】
次に、安全率算出部１１５は、ステップＳ１にてＧＰＳセンサ８により測定された位置データと、方位センサ３により測定された方位データとを用いて投票空間に設定された第１及び第２の座標軸をグローバル座標系に変換する（ステップＳ８）。
【００８４】
具体的には、安全率算出部１１５は、ＧＰＳセンサ８により測定された位置データが、移動システムの現在位置に相当する基準点ＲＰ（図７（Ｂ）参照）を示し、かつ、第１の座標軸Ｚ１が経度方向を示し、第２の座標軸Ｚ２が緯度方向を示すように、位置データ及び方位データを用いて、元の移動安全領域地図データに設定されていた第１の座標軸及び第２座標軸をグローバル座標系に座標変換する。これにより、図７（Ｂ）に示す第１の座標軸Ｚ１は経度方向を向き、第２の座標軸Ｚ２は緯度方向を向くことになり、移動安全領域地図データ内の各格子はグローバル座標系、つまり経度成分及び緯度成分で表されることになる。
【００８５】
また、障害物を示す点群を構成する測域データＤ３は、格子Ｋ２に位置するため、格子Ｋ２に投票されることになるが、測域データＤ３は障害物を示す点群を構成する測域データであるため、格子Ｋ２の投票値は−１となる。また、障害物についても格子Ｋ２を中心として二次元ガウス分布関数が適用されるため、格子Ｋ２の周囲の格子にも投票される。なお、障害物について適用する二次元ガウス分布関数については、半値幅を歩行者について適用する二次元ガウス分布関数よりも小さくしてもよい。以上の処理によって、図７（Ｂ）に示すような移動安全領域地図データが生成される（ステップＳ９）。
【００８６】
次に、探索部１１６は、安全率算出部１１５により生成された移動安全領域地図データにスタート位置を示す格子とゴール位置を示す格子とを設定し、スタート位置を示す格子からゴール位置を示す格子までの安全かつ最短な経路を探索する（ステップＳ１０）。
【００８７】
図８は、探索部１１６により探索された経路の一例を示した模式図である。なお、図８においては、黒色の線が探索された経路を示している。探索部１１６は、スタート位置を示す格子ＳＫからゴール位置を示す格子ＧＫまでの総コストｆ（ｎ）を計算し、総コストｆ（ｎ）が最小となるような経路を探索する。なお、総コストｆ（ｎ）は式（１）で定義される。
【００８８】
【数１】
【００８９】
但し、ｈ（ｎ）はある格子ｎから格子ＧＫまでのコストの推定値であり、格子ｎから格子ＧＫまでの直線距離Ｄ（ｎ，ｇｏａｌ）により規定される。ｇ（ｎ）は格子ＳＫから格子ｎまでの各格子のコストを足し合わせたものである。各格子のコストは隣接する格子との直線距離Ｄ（ｉ，ｉ＋１）に比例し、格子ｉ＋１の安全率αｉ＋１に反比例する。そして、探索部１１６は、格子ＳＫからｆ（ｎ）が小さくなるように探索していき、格子ＧＫにたどり着いた時に得られる経路が最適経路となる。
【００９０】
具体的には、探索部１１６は、格子ＳＫを始めの注目格子として設定し、注目格子と注目格子に隣接する例えば８個の格子とのそれぞれのコストを算出し、算出したコストが最小となる格子を次の注目格子として設定する処理を繰り返すことで経路を探索する。
【００９１】
ここで、注目格子をＣＫ、隣接する８個の格子のうちいずれか１つの格子をＮＫとすると、注目格子ＣＫと格子ＮＫとのコストは、Ｄ（ＣＫ，ＮＫ）／αＮＫ＋Ｄ（ＮＫ，ＧＫ）により与えられる。そして、格子ＮＫのうちコストが最小となる格子が次の注目格子ＣＫとして設定される。なお、Ｄ（ＣＫ，ＮＫ）としては、例えば、注目格子ＣＫと格子ＮＫとの重心同士を結ぶ直線距離を採用することができる。Ｄ（ＮＫ，ＧＫ）としては、例えば、格子ＮＫと格子ＧＫとの重心同士を結ぶ直線距離を採用することができる。
【００９２】
また、注目格子ＣＫに隣接する８個の格子の全てをコストの算出対象とすることなく、８個の格子のうち、過去に設定された注目格子ＣＫにおいてコストの算出対象とされた格子については、コストの算出対象から除外するようにしてもよい。
【００９３】
これにより最適経路として図８に示すように、経路の距離も考慮した上で、安全率の高い領域を通過するような経路を探索することができる。また、より多くの歩行者が通過した領域が経路として選択されるため、複数の経路が存在する場合にも最適な経路を求めることができる。
【００９４】
次に、探索した経路にしたがって、移動システムが実際に移動する際の処理について説明する。移動システムは、ＧＰＳセンサ８と方位センサ３とによって、常に自己位置（ｘ，ｙ，θ）を推定している。但し、ｘは緯度成分、ｙは経度成分、θは移動システムの方位を示している。まず、移動システムを停止させた状態で測域センサ１による歩行者認識と移動安全領域地図データの生成を行う。そして、安全率が記録された格子が一定数を超えると、生成された移動安全領域地図データと自己位置とを基に、目的地までの経路を探索する。これらの処理については上述した。そして、移動制御部１１７は、推定した自己位置が探索した経路から外れないように移動システムを移動させる。
【００９５】
なお、生成した移動安全領域地図データの外にゴール位置が設定されることもある。この場合、経路探索部２は、生成済みの移動安全領域地図データ内において、ゴール位置に最も近い位置をサブゴール位置として設定し、設定したサブゴール位置までの経路を探索し、設定したサブゴール位置まで移動システムを移動させればよい。
【００９６】
そして、移動システムがサブゴール位置に到達すると、経路探索部２は、再び、移動安全領域地図データを生成し、生成した移動安全領域地図データ内に設定したゴール位置が存在する場合は、ゴール位置までの経路を探索し、移動システムをそのゴール位置まで移動させればよい。
【００９７】
一方、生成した移動安全領域地図データ内に設定したゴール位置が存在しない場合は、上述したようにサブゴール位置を設定し、サブゴール位置まで、移動システムを移動させればよい。そして、経路探索部２は、この処理を繰り返し実行することで、移動システムを目的とするゴール位置に到達させればよい。
【００９８】
図９は、探索部１１６により探索された経路に沿って移動システムが移動する際の処理を示すフローチャートである。まず、移動制御部１１７は、左モータ駆動部９及び右モータ駆動部１０に指示信号を出力し、左モータＭ１及び右モータＭ２の駆動を開始させる（ステップＳ２１）。
【００９９】
次に、取得部１１１は、ＧＰＳセンサ８から位置データを取得すると共に、方位センサ３から方位データを取得する（ステップＳ２２でＹＥＳ）。ここで、取得部１１１は、一定の時間間隔で位置データ及び方位データを取得する。一方、取得部１１１により位置データ及び方位データが取得されない場合（ステップＳ２２でＮＯ）、処理がステップＳ２５に進められる。
【０１００】
次に、移動制御部１１７は、移動システムの進行方向を変更させる必要があるか否かを判定する（ステップＳ２３）。この場合、移動制御部１１７は、取得部１１１により取得された位置データと方位データとから、移動システムの進行方向を特定し、特定した進行方向が探索された経路の方向からずれる場合は、進行方向が探索された経路の方向を向くように左右モータＭ１，Ｍ２の回転速度を設定するための指示信号をモータ制御部１１に出力する（ステップＳ２４）。
【０１０１】
一方、移動制御部１１７は、移動システムの進行方向が、探索された経路の方向からずれない場合（ステップＳ２３でＮＯ）、処理をステップＳ２５に進める。
【０１０２】
次に、移動制御部１１７は、取得部１１１により取得された位置データを用いて、移動システムがゴール位置に到着しているか否かを判定し、ゴール位置に到着していない場合（ステップＳ２５でＮＯ）、処理をステップＳ２２に戻し、ステップＳ２２〜Ｓ２５の処理を繰り返す。一方、移動制御部１１７は、移動システムがゴール位置に到着したと判定した場合（ステップＳ２５でＹＥＳ）、左モータＭ１及び右モータＭ２を停止させ（ステップＳ２６）、処理を終了する。
【０１０３】
次に、本移動システムの手法の有効性を検証するために、屋外環境で行われたナビゲーション実験について説明する。このナビゲーション実験では約５０（ｍ）先の地点をゴール位置として設定し、経路の途中に危険領域を想定した障害物領域を設けた。ナビゲーション実験を行った結果、歩行者を認識することによって、移動システムの移動安全領域地図データを生成することができた。図１０は、ナビゲーション実験によって得られた移動安全領域地図データと、探索した経路と、移動システムの走行経路とを示している。また、図１１（Ａ）〜（Ｄ）は、走行中の移動システムの様子を示した図である。なお、図１１（Ａ）〜（Ｄ）は、それぞれ、図１０に示す「１」，「２」，「３」，「４」の位置で移動システムを撮影したものである。
【０１０４】
図１０では、やや色の濃い（淡いグレー色の）線が安全率の高い領域を示し、上下両側に点在する線が障害物の領域を示し、線Ｌ１が探索された経路を示し、線Ｌ２が移動システムの走行経路を示し、×印はサブゴール位置を示している。
【０１０５】
図１０に示すように、移動ロボットが移動できる安全率の高い領域が示された移動安全領域地図データを生成することができ、生成した移動安全領域地図データをもとに経路探索を実行することで、測域センサ１では測定が困難な障害物が回避するように移動システムを走行させることができた。また、移動システムはサブゴール位置に停止して、移動安全領域地図データの拡大とナビゲーションを繰り返すことで、最終的に離れた場所にあるゴール位置までたどり着くことができた。
【０１０６】
このように、本移動システムでは、歩行者の軌跡情報を用いることで、動的な環境でも状況に応じて安全な経路で移動システムを移動させることができる。また、上記のナビゲーション実験からも分かるように、測域センサ１で測定が困難な障害物が存在する場合でも、歩行者の軌跡情報を用いることで自動的に移動システムが障害物を回避する経路を探索することができる。
【符号の説明】
【０１０７】
１ 測域センサ
２ 経路探索部
３ 方位センサ
８ ＧＰＳセンサ
９ 左モータ駆動部
１０ 右モータ駆動部
１１ モータ制御部
１１１ 取得部
１１２ 地図データ生成部
１１３ 特定部
１１４ 投票部
１１５ 安全率算出部
１１６ 探索部
１１７ 移動制御部
１２１ 地図データ記憶部
１２２ 移動安全領域地図データ記憶部
【技術分野】
【０００１】
本発明は、スタート位置からゴール位置までの経路を探索する技術に関するものである。
【背景技術】
【０００２】
移動ロボットの自律移動には自己位置推定や地図構築が重要な要素となっている。しかしながら、屋外環境では気候等の影響により路面の状況が変化する等、環境が動的に変化しやすいため、移動時の環境状況に応じて目的地まで安全に走行できる経路探索手法が必要である。
【０００３】
環境状況に応じた自律移動手法として、画像情報からロボットが走行できる路面の領域を判別して路面上を走行する方法や、路面以外の障害物を検出して危険を回避する方法が知られている。また，環境の走行を繰り返すことで状況変化の多い領域、危険度の高い領域を学習し、安全に移動できる領域地図を生成する手法も提案されている。しかしながら、このような地図の生成には非常に労力を要するといった問題がある。
【０００４】
また、非特許文献１には、画像データから障害物と人間の位置情報を抽出することで、環境内を歩行している人間によって掃引された床面領域を移動可能領域とみなす地図を作成し、作成した地図を基に、経路計画を行う技術が開示されている。
【先行技術文献】
【非特許文献】
【０００５】
【非特許文献１】「移動ロボットによる歩行者観察に基づく地図作製と自律移動のための一手法」田中完爾、岡田伸宏廣、近藤英二、日本機械学会論文集、７０巻６９３号（２００４−５）、論文Ｎｏ．０３−０４３７
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、非特許文献１の手法では、ある床領域を掃引した人間の人数に関わらず、少なくとも１人の人間が掃引した床面領域は移動可能領域としてみなされているため、掃引した人間の人数に応じて床面領域がどの程度安全か否かを評価することができない。つまり、多くの人間が掃引した床面領域ほど安全である可能性が高いことが推定されるが、引用文献１の手法では、掃引した人間の人数を考慮に入れて床面領域の安全性の評価が行われていないため、より安全な経路を探索するためには更なる改善の余地がある。
【０００７】
本発明の目的は、安全かつ最短な経路を探索することができる経路探索装置及びその経路探索装置を備える移動システムを提供することである。
【課題を解決するための手段】
【０００８】
（１）本発明の一局面による経路探索装置は、周囲に存在する物体までの位置を示す測域データを取得する取得手段と、前記取得手段により取得された測域データを二次元の座標空間にプロットすることで、前記物体の位置を示す２次元の地図データを生成する地図データ生成手段と、前記地図データから周囲に存在する各物体を示す点群を抽出し、抽出した点群のうち所定の歩行者条件を満たしている点群を、歩行者を示す点群として特定する特定手段と、前記地図データが示す二次元の座標空間を格子状に区画することで投票空間を生成し、生成した投票空間を構成する各格子に、前記特定手段により特定された歩行者を示す点群を構成する各測域データを投票する投票手段と、前記投票手段による投票結果が高い格子ほどに、安全率が高くなるように各格子の安全率を算出する安全率算出手段と、前記安全率が算出された投票空間において、所定のスタート位置を示す格子と所定のゴール位置を示す格子との距離が最短となり、かつ、前記安全率が最も高くなるような経路を所定のアルゴリズムを用いて探索する探索手段とを備えることを特徴とする。
【０００９】
この構成によれば、周囲に存在する物体の位置を示す測域データが取得され、取得された測域データが二次元の座標空間にプロットされ、各物体を示す点群が抽出され、抽出された点群の中から歩行者を示す点群が特定される。そして、二次元の座標空間を格子状に区画することで得られる投票空間に、歩行者を示す点群を構成する測域データが投票され、投票結果が高い格子ほどに、安全率が高くなるように各格子の安全率が算出される。これにより、投票空間は、多くの歩行者が通った格子ほど高い安全率が算出された２次元の地図データとなる。そして、この投票空間を用いて、スタート位置を示す格子と所定のゴール位置とを示す格子との距離が最短となり、かつ、安全率が最も高くなるような経路が探索される。
【００１０】
これにより、多くの歩行者が通るために安全性が高く、かつ、スタート位置からゴール位置までの距離が最短となるような経路を探索することができる。よって、スタート位置からゴール位置まで自動運転する移動システム（ロボット）に本構成を採用すると、障害物との衝突を回避させると同時に、最短経路で移動体をゴール位置まで誘導させることができる。
【００１１】
（２）前記特定手段は、点群の移動速度及び形状を前記歩行者条件として採用することが好ましい。
【００１２】
この構成によれば、点群の移動速度及び形状が歩行者条件として採用されているため、歩行者を示す点群を精度良く特定することができる。
【００１３】
（３）前記歩行者条件における点群の形状は、点群を構成する点の個数及び点群の幅を含むことが好ましい。
【００１４】
この構成によれば、点群の個数及び幅が歩行者条件の形状として採用されているため、点群をより精度良く特定することができる。
【００１５】
（４）前記測域データは、地面からの高さが歩行者の膝下の高さとなるように水平方向に向けてレーザビームを照射することで測定されたものであり、前記投票手段は、歩行者を示す点群を構成する測域データが投票された格子を中心として当該格子の投票値を所定の二次元分布関数に従って変化させることで周囲の格子の投票値を設定することが好ましい。
【００１６】
この構成によれば、歩行者の胴体のサイズを考慮に入れた投票を行うことができる。また、二次元分布関数に従って投票値が変化されているため、投票値をなだらかに変化させることができる。
【００１７】
（５）前記特定手段は、前記歩行者条件を満たさない点群を障害物を示す点群として特定し、前記投票手段は、前記歩行者を示す点群の測域データについては投票値を正の値にして投票し、前記障害物を示す点群を構成する測域データについては投票値を負の値にして投票することが好ましい。
【００１８】
この構成によれば、障害物を示す点群と歩行者を示す点群とが明確に区別されるように安全率を算出することができる。
【００１９】
（６）前記探索手段は、Ａ＊アルゴリズムを用いて経路を探索することが好ましい。
【００２０】
この構成によれば、距離が最短となり、かつ、安全率の高い経路をより正確に探索することができる。
【００２１】
（７）ＧＰＳセンサと方位センサとを更に備え、前記安全率算出手段は、前記ＧＰＳセンサにより測定された自己の現在位置を示す位置データと前記方位センサにより測定された方位データとを基に、前記投票空間の座標系をグローバル座標系に変換し、前記探索手段は、前記ＧＰＳセンサにより測定された自己の現在位置を示す格子を前記投票空間に設定し、設定した現在位置を示す格子を前記スタート位置とすることで、前記経路を探索することが好ましい。
【００２２】
この構成によれば、投票空間の座標系をグローバル座標系で表すことができるため、ＧＰＳセンサにより測定された自己の現在位置をスタート位置としてゴール位置までの経路を探索することができる。また、グローバル座標系における位置データ（例えば、緯度、経度）を入力することで、ゴール位置の設定を行うことができる。また、ＧＰＳセンサ及び方位センサを備えているため、ＧＰＳセンサ及び方位センサから自己の現在位置及び進行方向が探索した経路からずれているか否かを検出することができる。
【発明の効果】
【００２３】
本発明によれば、多くの歩行者が通るために安全性が高く、かつ、スタート位置からゴール位置までの距離が最短となるような経路を探索することができる。
【図面の簡単な説明】
【００２４】
【図１】本発明の一実施の形態による移動システムの外観構成図を示している。
【図２】図１に示す移動システムの電気的な構成を示すブロック図である。
【図３】図１に示す移動システムの機能的な構成を示したブロック図である。
【図４】本発明の実施の形態による移動システムが経路を探索する際の動作を示すフローチャートである。
【図５】歩行者条件を示した表である。
【図６】（Ａ）は地図データ生成部により生成された地図データの一例を示す模式図であり、（Ｂ）は（Ａ）に示す地図データから歩行者を示す点群が抽出された地図データを示す模式図である。
【図７】（Ａ）は測域データがプロットされた地図データにおいて投票空間の分解能程度に分解能が低くされた地図データを示し、（Ｂ）は測域データが投票された投票空間を示している。
【図８】探索部により探索された経路の一例を示した模式図である。
【図９】探索部により探索された経路に沿って移動システムが移動する際の処理を示すフローチャートである。
【図１０】ナビゲーション実験によって得られた移動安全領域地図データと、探索した経路と、移動システムの走行経路とを示している。
【図１１】（Ａ）〜（Ｄ）は、走行中の移動システムの様子を示した図である。
【発明を実施するための形態】
【００２５】
以下、本発明の一実施の形態による移動システムについて説明する。図１は、本発明の一実施の形態による移動システムの外観構成図を示している。図１に示すように移動システムは、車椅子型の移動システムであって、測域センサ１、経路探索部２、方位センサ３、タイヤ部４、台５、フレーム６、操作部７、及びＧＰＳセンサ８を備えている。
【００２６】
測域センサ１は、例えばレーザレンジファインダにより構成され、移動システムの周囲に位置する物体までの距離及び方向を示す測域データを測定する。具体的には、測域センサ１は、地上から歩行者の膝下程度の高さの水平面上において、レーザビームの射出方向を扇状に変更してレーザビームをスキャンし、物体からの反射光を受光することで周囲に位置する物体までの測域データを測定する。ここで、測域センサ１は、例えば、水平面上において、真正面の方向を基準として時計回りの方向を正とすると、−９０度から＋９０度の１８０度の範囲でレーザビームをスキャンすることで測域データを測定する。
【００２７】
なお、測域データには、測域センサ１の真正面の方向を基準としたときの検出した物体の方向を示す方向データと、測域センサ１を基準としたときの物体までの距離を示す距離データとが含まれている。また、方向データとしては、レーザビームの照射角度が採用されている。
【００２８】
また、測域センサ１は、台５の前方に取り付けられているため、移動システムの前方において、数十メートル〜数百メートル（例えば最大８０メートル）を半径とする半円状の範囲内に位置する物体の測域データを測定することになる。なお、本実施の形態では、測域センサ１としては、例えば、ＳＩＣＫ（ジック）社製のＬＭＳ２００が採用されている。
【００２９】
経路探索部２は、コンピュータにより構成され、本実施の形態では、例えばノートブックパソコンが採用されている。方位センサ３は、例えば、３次元の磁気方位センサにより構成され、ヨー角の角度成分を、グローバル座標系の真北の方向と移動システムの真正面の方向との間の角度として測定し、測定した角度を方位データとして経路探索部２に出力する。本実施の形態では、方位センサ３としては、例えばHoneywell（ハネウエル）社製のTruePoint HMR3500が採用されている。
【００３０】
タイヤ部４は、フレーム６の下部に取り付けられた４つのタイヤにより構成され、移動システムを地面に沿って移動させる。なお、４つのタイヤのうち、後方に設けられた左右２つの半径の大きなタイヤには、それぞれ、左モータＭ１及び右モータＭ２が取り付けられ、両タイヤは、左右モータＭ１，Ｍ２からの駆動力を受けて回転する。なお、前方に設けられた半径の小さな２個のタイヤは補助輪である。
【００３１】
台５は、経路探索部２等のコンピュータを設置する台により構成されている。操作部７は、例えばジョイスティックにより構成され、移動システムをマニュアル運転する場合に使用される。
【００３２】
フレーム６は、例えば四角柱状の部材により構成され、車椅子の骨格部分を形成し、下部にはタイヤが取り付けられ、中央部には台５が取り付けられ、前方左手には操作部７が取り付けられている。
【００３３】
ＧＰＳセンサ８は、例えば、differentialＧＰＳセンサ（相対即位方式のＧＰＳセンサ）により構成され、フレーム６の前方左手側に立設された柱８１の上端に取り付けられ、移動システムのグローバル座標系における位置データを取得する。ここで、位置データは、緯度成分及び経度成分を含んでいる。本実施の形態では、ＧＰＳセンサ８としては、例えば、hemisphere社製のＡ１００が採用されている。
【００３４】
図２は、図１に示す移動システムの電気的な構成を示すブロック図である。図２に示すように、移動システムは、測域センサ１、経路探索部２、方位センサ３、操作部７、ＧＰＳセンサ８、左モータ駆動部９（タイヤ駆動手段の一例）、右モータ駆動部１０（タイヤ駆動手段の一例）、モータ制御部１１（タイヤ駆動手段の一例）、左モータＭ１（タイヤ駆動手段の一例）、及び右モータＭ２（タイヤ駆動手段の一例）を備えている。
【００３５】
経路探索部２は、入力装置２１、ＲＯＭ（リードオンリメモリ）２２、ＣＰＵ（中央演算処理装置）２３、ＲＡＭ（ランダムアクセスメモリ）２４、外部記憶装置２５、表示装置２６、記録媒体駆動装置２７、及び入出力インターフェイス（Ｉ／Ｆ）２９を備えている。入力装置２１、ＲＯＭ２２、ＣＰＵ２３、ＲＡＭ２４、外部記憶装置２５、表示装置２６、記録媒体駆動装置２７、及びＩ／Ｆ２９は、バスラインに接続され、このバスラインを介して種々のデータ等が入出され、ＣＰＵ２３の制御の下、種々の処理が実行される。
【００３６】
入力装置２１は、キーボード、マウス等から構成され、ユーザが種々のデータを入力するために使用される。ＲＯＭ２２には、ＢＩＯＳ（Basic Input/Output System）等のシステムプログラムが記憶される。外部記憶装置２５は、ハードディスクドライブやＳＳＤ（Solid State Drive）等から構成され、所定のＯＳ（Operating System）及びコンピュータを経路探索部２として機能させるための制御プログラムが記憶されている。ＣＰＵ２３は、外部記憶装置２５からＯＳ等を読み出し、各ブロックの動作を制御する。ＲＡＭ２４は、ＣＰＵ２３の作業領域等として用いられる。
【００３７】
表示装置２６は、液晶表示装置等から構成され、ＣＰＵ２３の制御の下に種々の画像を表示する。記録媒体駆動装置２７は、ＣＤ−ＲＯＭドライブ、フレキシブルディスクドライブ等から構成される。
【００３８】
なお、記録媒体２８は、ＣＤ−ＲＯＭ等のコンピュータ読み取り可能な記録媒体２８により構成され、コンピュータを経路探索部２として機能させるための制御プログラムを格納する。ユーザはこの記録媒体２８を記録媒体駆動装置２７に読み込ませることで、制御プログラムをコンピュータにインストールする。
【００３９】
Ｉ／Ｆ２９は、例えばＵＳＢ規格に準拠する複数のポートにより構成され、測域センサ１、方位センサ３、ＧＰＳセンサ８、及びモータ制御部１１が接続されている。モータ制御部１１には、左モータ駆動部９及び右モータ駆動部１０が接続されている。モータ制御部１１には、操作部７が接続されている。
【００４０】
モータ制御部１１は、経路探索部２から出力される指示信号にしたがって、左モータＭ１及び右モータＭ２のそれぞれが経路探索部２により指定された回転速度で回転するように左モータ駆動部９及び右モータ駆動部１０を制御する。左モータ駆動部９及び右モータ駆動部１０は、モータ制御部１１の制御のしたがって、左モータＭ１及び右モータＭ２のそれぞれを指定された回転速度で回転させる。
【００４１】
図３は、図１に示す移動システムの機能的な構成を示したブロック図である。図３に示すように移動システムは、取得部１１１（取得手段の一例）、地図データ生成部１１２（地図データ生成手段の一例）、特定部１１３（特定手段の一例）、投票部１１４（投票手段の一例）、安全率算出部１１５（安全率算出手段の一例）、探索部１１６（探索手段の一例）、移動制御部１１７（移動制御手段の一例）、地図データ記憶部１２１、及び移動安全領域地図データ記憶部１２２を備えている。
【００４２】
取得部１１１〜移動制御部１１７は、経路探索部２のＣＰＵ２３により構成され、地図データ記憶部１２１、及び移動安全領域地図データ記憶部１２２は、経路探索部２のＲＡＭ２４等により構成されている。
【００４３】
取得部１１１は、測域センサ１により測定された移動システムの周囲に存在する物体までの距離及び方向を示す測域データを取得する。また、取得部１１１は、ＧＰＳセンサ８により測定された移動システムのグローバル座標系における位置データを取得する。また、取得部１１１は、方位センサ３により測定された移動システムの方位データを取得する。また、取得部１１１は、入力装置２１を操作することでユーザにより入力されたゴール位置を示す位置データを取得する。ここで、ゴール位置を示す位置データとしては、グローバル座標系における緯度成分と緯度成分と含む位置データが採用される。
【００４４】
地図データ生成部１１２は、取得部１１１により取得された測域データを二次元の座標空間にプロットすることで、物体の位置を示す２次元の地図データを生成する。ここで、地図データ生成部１１２は、例えば以下のようにして、地図データを生成する。
【００４５】
まず、地表面を上側から見たときの座標平面であって、測域センサ１の真正面の方向が第１の座標軸として設定され、この第１の座標軸に直交する方向が第２の座標軸として設定された座標平面を２次元の座標空間として設定する。次に、設定した座標空間に、測域センサ１の位置を示す基準点をプロットする。次に、測域データに含まれる方向データからプロットするべき測域データの基準点からの向きを定め、測域データに含まれる距離データからプロットするべき測域データの基準点からの距離を定める。次に、定めた向きと距離とにより特定される座標空間の座標位置に測域データをプロットする。このような処理を、測域センサ１から出力する各測域データに対して実行し、地図データを生成する。
【００４６】
また、地図データ生成部１１２は、測域センサ１に一定の時間間隔（スキャン周期）を空けてレーザビームを複数回スキャンさせ、測域センサ１が１回スキャンする度に１枚の地図データを生成することで、複数枚の地図データを生成する。なお、生成された複数枚の地図データは地図データ記憶部１２１に記憶される。
【００４７】
特定部１１３は、地図データ生成部１１２により生成された各地図データにおいて、各物体を示す点群を抽出し、抽出した点群のうち所定の歩行者条件を満たしている点群を、歩行者を示す点群として特定する。具体的には、特定部１１３は、以下のようにして、歩行者を示す点群を特定する。まず、各地図データにおいて、一塊の点群、すなわち、連続して存在する点群を１つの物体を示す点群として抽出する。
【００４８】
次に、各地図データにおいて、幅及び構成点数が所定の歩行者条件を満たす点群を、歩行者の候補となる候補点群として特定する。次に、異なる地図データ間において、同一の物体を示す候補点群同士を対応付け、対応付けた候補点群の移動速度を算出する。次に、算出した移動速度が所定の歩行者条件を満たす点群を、歩行者を示す点群として特定する。
【００４９】
なお、候補点群の移動速度は、例えば下記のようにして求めればよい。まず、各地図データを、動画像を構成する１枚のフレームとしてとらえ、対応付けた候補点群を時系列に並べる。次に、時系列に並べた各候補点群の重心位置を特定する。次に、特定した重心位置のフレーム間における移動距離を求める。次に、求めた移動距離を測域センサ１によるスキャン周期で割ることで各候補点群のフレーム間での移動速度を求め、求めた移動速度の平均値を候補点群の移動速度として算出する。
【００５０】
なお、特定部１１３は、所定の歩行者条件を満たさなかった点群を障害物を示す点群として特定する。
【００５１】
投票部１１４は、地図データが示す二次元の座標空間を格子状に区画することで投票空間を生成し、生成した投票空間を構成する各格子に、特定部１１３により特定された歩行者を示す点群を構成する測域データを投票する。
【００５２】
ここで、投票部１１４は、歩行者を示す点群を構成する測域データが投票された格子を中心として当該格子の投票値を所定の二次元分布関数に従って変化させることで周囲の格子の投票値を設定し、設定した投票値で周囲の格子に投票する。ここで、二次元分布関数としては例えば二次元ガウス分布関数を採用することができる。
【００５３】
また、投票部１１４は、歩行者を示す点群の測域データについては投票値を正の値（例えば「＋１」）にして投票し、障害物を示す点群を構成する測域データについては投票値を負の値（例えば「−１」）にして投票する。これにより、安全率を算出するに際して、障害物と歩行者とを明確に区別することができる。
【００５４】
また、投票部１１４は、障害物を示す点群を構成する測域データについても歩行者を示す点群を構成する測域データと同様にして、測域データが投票された格子を中心として当該格子の投票値を二次元ガウス分布関数に従って変化させることで周囲の格子の投票値を設定する。
【００５５】
安全率算出部１１５は、投票部１１４による投票結果が高い格子ほどに、安全率が高くなるように各格子の安全率を算出する。具体的には、安全率算出部１１５は、投票部１１４が各格子に対して投票した投票値の和を求めることで各格子の安全率を算出し、移動安全領域地図データを生成する。なお、生成された移動安全領域地図データは移動安全領域地図データ記憶部１２２に記憶される。
【００５６】
また、安全率算出部１１５は、ＧＰＳセンサ８により測定された移動システムのグローバル座標系における位置と、方位センサ３により測定された移動システムの方位とから、二次元の座標空間に設定された第１及び第２の座標軸をグローバル座標系に変換することでプロットした各測域データのグローバル座標系における位置を求める。
【００５７】
探索部１１６は、安全率が算出された投票空間において、スタート位置を示す格子と予め定められたゴール位置を示す格子との距離が最短となり、かつ、安全率が最も高くなるような経路を所定のアルゴリズム（例えばＡ＊アルゴリズム）を用いて探索する。
【００５８】
なお、スタート位置としては、地図データ生成部１１２が地図データにプロットした基準点のグローバル座標系における位置データを採用することができる。
【００５９】
移動制御部１１７は、経路探索部２によって探索された経路にしたがって、移動システムが移動するように、モータ制御部１１に指示信号を出力し、左モータＭ１及び右モータＭ２を駆動させる。ここで、移動制御部１１７は、移動システムを右方向に旋回させる場合は、左モータＭ１の回転速度が右モータＭ２の回転速度より大きくなるように、モータ制御部１１に指示信号を出力する。一方、移動制御部１１７は、移動システムを左方向に旋回させる場合は、右モータＭ２の回転速度が左モータＭ１の回転速度よりも大きくなるように、モータ制御部１１に指示信号を出力する。
【００６０】
次に、本発明の実施の形態による移動システムの動作について説明する。図４は、本発明の実施の形態による移動システムが経路を探索する際の動作を示すフローチャートである。
【００６１】
まず、ステップＳ１において、取得部１１１は、ＧＰＳセンサ８により測定された移動システムの現在位置を示す位置データを取得すると共に、方位センサ３により測定された移動システムの方位を示す方位データを取得する。
【００６２】
次に、取得部１１１は、測域センサ１により測定された測域データを取得する（ステップＳ２）。ここで、測域センサ１は、上述したように、スキャン周期にしたがってレーザビームを複数回スキャンする。スキャン周期としては、歩行者の移動速度を求めるにあたり好適な値を採用することが好ましい。
【００６３】
次に、地図データ生成部１１２は、測域センサ１がレーザビームを１回スキャンする度に、測定した測域データを２次元の座標空間にプロットすることで、複数枚の地図データを生成する（ステップＳ３）。この場合、生成される地図データは、第１の座標軸が、例えば測域センサ１の真正面の方向に設定され、第２の座標軸が、例えば第１の座標軸と直交する方向に設定される。
【００６４】
次に、特定部１１３は、ステップＳ３で作成された複数枚の地図データを用いて、各地図データから移動システムの周囲に存在する各物体を示す点群を抽出し、抽出した点群が所定の歩行者条件を満たすか否かにより、歩行者を示す点群と障害物を示す点群とを特定する（ステップＳ４）。
【００６５】
図５は、歩行者条件を示した表である。図５に示すように、歩行者条件としては、点群の幅ｗ（ｃｍ）、構成点数Ｎ（個）、及び移動速度ｖ（ｃｍ／ｓｅｃ）が採用されている。本実施の形態では、幅ｗとして例えば、５≦ｗ≦６０が採用され、構成点数Ｎとして例えば１≦Ｎ≦３０が採用され、移動速度ｖとして例えばｖ≧１２が採用されている。
【００６６】
したがって、特定部１１３は、各地図データにおいて、まず、幅ｗ及び構成点数Ｎが図５に示す条件を満たす点群を歩行者を示す点群候補として特定する。そして、各地図データを動画像のフレームとみたてて、各フレームにおいて同一の歩行者を示す点群候補を対応付けて時系列に並べる。
【００６７】
そして、時系列に並べた点群候補の重心位置のフレーム間での移動距離を求め、移動距離をスキャン周期で割ることで、対応付けた点群候補のフレーム間での移動速度を求め、求めた移動速度の平均値を、対応付けた点群候補の移動速度として算出する。そして、算出した移動速度が図５に示す条件を満たす点群候補を、歩行者を示す点群として特定する。
【００６８】
なお、特定部１１３は、例えばあるフレームのある点群候補に対して最も近接する次のフレーム内の点群候補を同一物体を示す点群候補として対応付ければよい。
【００６９】
また、地図データから歩行者を示す点群を抽出する手法として、人の足首の高さをスキャンすることで得られる点群の幾何的特徴量を求めるＡｒｒａｓ達が開発した手法が知られている。したがって、このＡｒｒａｓ達が開発した手法を用いて歩行者を示す点群を特定してもよいし、Ａｒｒａｓ達が開発した手法と上述した歩行者を示す点群を特定する手法とを組み合わせることで歩行者を示す点群を特定してもよい。
【００７０】
なお、Ａｒｒａｓ達が開発した手法は、「Kai O.Arras，Oscar Martinez Mozos，Wolfman Burgard: “Using Boosted Features for the Detection of People in 2D Range Data”，IEEE International Conference on Robotics and Automation，pp.3402-3407. 2007.」に開示されている。
【００７１】
図６（Ａ）は地図データ生成部１１２により生成された地図データの一例を示す模式図であり、（Ｂ）は（Ａ）に示す地図データから歩行者を示す点群が抽出された地図データを示す模式図である。
【００７２】
図６（Ａ）に示すように、二次元の座標空間内において、複数の測域データがプロットされていることが分かる。そして、上述した歩行者を示す点群を特定する処理を実行した結果、図６（Ｂ）に示すように中央部の濃い黒丸で示す３つの点群が歩行者を示す点群として特定されていることが分かる。そして、それ以外の両端にプロットされた点群は、歩行者条件を満たしていないため、障害物として特定されていることが分かる。
【００７３】
次に、投票部１１４は、地図データが示す二次元の座標空間を格子状に区画することで投票空間を生成する（ステップＳ５）。
【００７４】
次に、投票部１１４は、ステップＳ３で生成した各地図データにおいて、歩行者を示す点群を構成する測域データについては、投票値を１として投票空間の各格子に投票すると共に、障害物を示す点群を構成する測域データについては、投票値を−１として投票空間の各格子に投票する（ステップＳ６）。
【００７５】
図７（Ａ）は投票空間の分解能程度に分解能が低くされた地図データを示し、（Ｂ）は測域データが投票された投票空間を示している。
【００７６】
図７（Ｂ）においては、グレーの色が濃くなるにつれて安全率が高いことを示し、黒色は安全率が負の値であることを示している。また、図７（Ｂ）においては、各格子の大きさは１０（ｃｍ）四方とされている。
【００７７】
図７（Ａ）において歩行者を示す測域データＤ１は、図７（Ｂ）において格子Ｋ１に位置するため、投票値が１とされて格子Ｋ１に投票されている。したがって、投票部１１４は、格子Ｋ１を中心として二次元ガウス分布関数を当てはめ、格子Ｋ１の投票値である１を、当てはめた二次元ガウス分布関数に従って変化させ、格子Ｋ１の周囲の格子の投票値を設定し、設定した投票値で周囲の格子に投票する。
【００７８】
ここで、二次元ガウス分布関数を当てはめるのは、歩行者の胴体の領域も安全であると考えたからである。つまり、測域センサ１は、地上から歩行者の膝下程度の高さの水平面上においてレーザビームを走査しているため、歩行者を示す点群のサイズは、歩行者の膝下周りのサイズを有することになる。したがって、二次元ガウス分布関数を当てはめて、投票する格子領域に広がりを持たせることで、歩行者の胴体のサイズを考慮に入れた投票を行うことができる。よって、二次元ガウス分布関数の半値幅としては、中心となる格子から人間の胴回りの半径程度（例えば２０ｃｍ）の領域内に含まれる格子が投票されるような値を採用することが好ましい。
【００７９】
例えば、図７（Ｂ）においては、格子Ｋ１を中心として格子Ｋ１を除く格子３個分の半径の円状の領域Ｒ１内に位置する各格子が投票されている。また、領域Ｒ１内の各格子の投票値は格子Ｋ１の投票値である１をガウス関数に従って変化させた値が設定されている。よって、領域Ｒ１において格子Ｋ１から離れる格子ほど投票値が低くなっている。
【００８０】
このようにして、投票部１１４は、ステップＳ３で作成した複数枚の地図データに含まれる各測域データを図７（Ｂ）に示す１枚の投票空間に投票していく。また、障害物を示す点群を構成する測域データについては、投票値を−１として投票を行う。そして、歩行者及び障害物を示す点群を構成する測域データについては、二次元ガウス分布関数に従って周囲の格子にも投票を行う。
【００８１】
次に、安全率算出部１１５は、各格子において、投票部１１４が投票した投票値の合計値を求め、求めた合計値を各格子の安全率として算出する（ステップＳ７）。例えば、図７（Ｂ）に示す格子Ｋ１においては、図７（Ａ）に示す測域データＤ１を投票する際に設定された投票値と測域データＤ１の周囲に位置する測域データＤ２を投票する際に設定された投票値とによって投票されることになる。そのため、これらの測域データＤ１が投票される際に格子Ｋ１に対して投票された投票値の合計値が格子Ｋ１の安全率として算出されることになる。
【００８２】
これにより、同じ領域を複数の歩行者が通過すると、その度に安全率が増加することになる。つまり、より多くの歩行者が通過する領域ほど、安全率が高い領域になる。なお、安全率の値には上限値を設けてもよい。
【００８３】
次に、安全率算出部１１５は、ステップＳ１にてＧＰＳセンサ８により測定された位置データと、方位センサ３により測定された方位データとを用いて投票空間に設定された第１及び第２の座標軸をグローバル座標系に変換する（ステップＳ８）。
【００８４】
具体的には、安全率算出部１１５は、ＧＰＳセンサ８により測定された位置データが、移動システムの現在位置に相当する基準点ＲＰ（図７（Ｂ）参照）を示し、かつ、第１の座標軸Ｚ１が経度方向を示し、第２の座標軸Ｚ２が緯度方向を示すように、位置データ及び方位データを用いて、元の移動安全領域地図データに設定されていた第１の座標軸及び第２座標軸をグローバル座標系に座標変換する。これにより、図７（Ｂ）に示す第１の座標軸Ｚ１は経度方向を向き、第２の座標軸Ｚ２は緯度方向を向くことになり、移動安全領域地図データ内の各格子はグローバル座標系、つまり経度成分及び緯度成分で表されることになる。
【００８５】
また、障害物を示す点群を構成する測域データＤ３は、格子Ｋ２に位置するため、格子Ｋ２に投票されることになるが、測域データＤ３は障害物を示す点群を構成する測域データであるため、格子Ｋ２の投票値は−１となる。また、障害物についても格子Ｋ２を中心として二次元ガウス分布関数が適用されるため、格子Ｋ２の周囲の格子にも投票される。なお、障害物について適用する二次元ガウス分布関数については、半値幅を歩行者について適用する二次元ガウス分布関数よりも小さくしてもよい。以上の処理によって、図７（Ｂ）に示すような移動安全領域地図データが生成される（ステップＳ９）。
【００８６】
次に、探索部１１６は、安全率算出部１１５により生成された移動安全領域地図データにスタート位置を示す格子とゴール位置を示す格子とを設定し、スタート位置を示す格子からゴール位置を示す格子までの安全かつ最短な経路を探索する（ステップＳ１０）。
【００８７】
図８は、探索部１１６により探索された経路の一例を示した模式図である。なお、図８においては、黒色の線が探索された経路を示している。探索部１１６は、スタート位置を示す格子ＳＫからゴール位置を示す格子ＧＫまでの総コストｆ（ｎ）を計算し、総コストｆ（ｎ）が最小となるような経路を探索する。なお、総コストｆ（ｎ）は式（１）で定義される。
【００８８】
【数１】
【００８９】
但し、ｈ（ｎ）はある格子ｎから格子ＧＫまでのコストの推定値であり、格子ｎから格子ＧＫまでの直線距離Ｄ（ｎ，ｇｏａｌ）により規定される。ｇ（ｎ）は格子ＳＫから格子ｎまでの各格子のコストを足し合わせたものである。各格子のコストは隣接する格子との直線距離Ｄ（ｉ，ｉ＋１）に比例し、格子ｉ＋１の安全率αｉ＋１に反比例する。そして、探索部１１６は、格子ＳＫからｆ（ｎ）が小さくなるように探索していき、格子ＧＫにたどり着いた時に得られる経路が最適経路となる。
【００９０】
具体的には、探索部１１６は、格子ＳＫを始めの注目格子として設定し、注目格子と注目格子に隣接する例えば８個の格子とのそれぞれのコストを算出し、算出したコストが最小となる格子を次の注目格子として設定する処理を繰り返すことで経路を探索する。
【００９１】
ここで、注目格子をＣＫ、隣接する８個の格子のうちいずれか１つの格子をＮＫとすると、注目格子ＣＫと格子ＮＫとのコストは、Ｄ（ＣＫ，ＮＫ）／αＮＫ＋Ｄ（ＮＫ，ＧＫ）により与えられる。そして、格子ＮＫのうちコストが最小となる格子が次の注目格子ＣＫとして設定される。なお、Ｄ（ＣＫ，ＮＫ）としては、例えば、注目格子ＣＫと格子ＮＫとの重心同士を結ぶ直線距離を採用することができる。Ｄ（ＮＫ，ＧＫ）としては、例えば、格子ＮＫと格子ＧＫとの重心同士を結ぶ直線距離を採用することができる。
【００９２】
また、注目格子ＣＫに隣接する８個の格子の全てをコストの算出対象とすることなく、８個の格子のうち、過去に設定された注目格子ＣＫにおいてコストの算出対象とされた格子については、コストの算出対象から除外するようにしてもよい。
【００９３】
これにより最適経路として図８に示すように、経路の距離も考慮した上で、安全率の高い領域を通過するような経路を探索することができる。また、より多くの歩行者が通過した領域が経路として選択されるため、複数の経路が存在する場合にも最適な経路を求めることができる。
【００９４】
次に、探索した経路にしたがって、移動システムが実際に移動する際の処理について説明する。移動システムは、ＧＰＳセンサ８と方位センサ３とによって、常に自己位置（ｘ，ｙ，θ）を推定している。但し、ｘは緯度成分、ｙは経度成分、θは移動システムの方位を示している。まず、移動システムを停止させた状態で測域センサ１による歩行者認識と移動安全領域地図データの生成を行う。そして、安全率が記録された格子が一定数を超えると、生成された移動安全領域地図データと自己位置とを基に、目的地までの経路を探索する。これらの処理については上述した。そして、移動制御部１１７は、推定した自己位置が探索した経路から外れないように移動システムを移動させる。
【００９５】
なお、生成した移動安全領域地図データの外にゴール位置が設定されることもある。この場合、経路探索部２は、生成済みの移動安全領域地図データ内において、ゴール位置に最も近い位置をサブゴール位置として設定し、設定したサブゴール位置までの経路を探索し、設定したサブゴール位置まで移動システムを移動させればよい。
【００９６】
そして、移動システムがサブゴール位置に到達すると、経路探索部２は、再び、移動安全領域地図データを生成し、生成した移動安全領域地図データ内に設定したゴール位置が存在する場合は、ゴール位置までの経路を探索し、移動システムをそのゴール位置まで移動させればよい。
【００９７】
一方、生成した移動安全領域地図データ内に設定したゴール位置が存在しない場合は、上述したようにサブゴール位置を設定し、サブゴール位置まで、移動システムを移動させればよい。そして、経路探索部２は、この処理を繰り返し実行することで、移動システムを目的とするゴール位置に到達させればよい。
【００９８】
図９は、探索部１１６により探索された経路に沿って移動システムが移動する際の処理を示すフローチャートである。まず、移動制御部１１７は、左モータ駆動部９及び右モータ駆動部１０に指示信号を出力し、左モータＭ１及び右モータＭ２の駆動を開始させる（ステップＳ２１）。
【００９９】
次に、取得部１１１は、ＧＰＳセンサ８から位置データを取得すると共に、方位センサ３から方位データを取得する（ステップＳ２２でＹＥＳ）。ここで、取得部１１１は、一定の時間間隔で位置データ及び方位データを取得する。一方、取得部１１１により位置データ及び方位データが取得されない場合（ステップＳ２２でＮＯ）、処理がステップＳ２５に進められる。
【０１００】
次に、移動制御部１１７は、移動システムの進行方向を変更させる必要があるか否かを判定する（ステップＳ２３）。この場合、移動制御部１１７は、取得部１１１により取得された位置データと方位データとから、移動システムの進行方向を特定し、特定した進行方向が探索された経路の方向からずれる場合は、進行方向が探索された経路の方向を向くように左右モータＭ１，Ｍ２の回転速度を設定するための指示信号をモータ制御部１１に出力する（ステップＳ２４）。
【０１０１】
一方、移動制御部１１７は、移動システムの進行方向が、探索された経路の方向からずれない場合（ステップＳ２３でＮＯ）、処理をステップＳ２５に進める。
【０１０２】
次に、移動制御部１１７は、取得部１１１により取得された位置データを用いて、移動システムがゴール位置に到着しているか否かを判定し、ゴール位置に到着していない場合（ステップＳ２５でＮＯ）、処理をステップＳ２２に戻し、ステップＳ２２〜Ｓ２５の処理を繰り返す。一方、移動制御部１１７は、移動システムがゴール位置に到着したと判定した場合（ステップＳ２５でＹＥＳ）、左モータＭ１及び右モータＭ２を停止させ（ステップＳ２６）、処理を終了する。
【０１０３】
次に、本移動システムの手法の有効性を検証するために、屋外環境で行われたナビゲーション実験について説明する。このナビゲーション実験では約５０（ｍ）先の地点をゴール位置として設定し、経路の途中に危険領域を想定した障害物領域を設けた。ナビゲーション実験を行った結果、歩行者を認識することによって、移動システムの移動安全領域地図データを生成することができた。図１０は、ナビゲーション実験によって得られた移動安全領域地図データと、探索した経路と、移動システムの走行経路とを示している。また、図１１（Ａ）〜（Ｄ）は、走行中の移動システムの様子を示した図である。なお、図１１（Ａ）〜（Ｄ）は、それぞれ、図１０に示す「１」，「２」，「３」，「４」の位置で移動システムを撮影したものである。
【０１０４】
図１０では、やや色の濃い（淡いグレー色の）線が安全率の高い領域を示し、上下両側に点在する線が障害物の領域を示し、線Ｌ１が探索された経路を示し、線Ｌ２が移動システムの走行経路を示し、×印はサブゴール位置を示している。
【０１０５】
図１０に示すように、移動ロボットが移動できる安全率の高い領域が示された移動安全領域地図データを生成することができ、生成した移動安全領域地図データをもとに経路探索を実行することで、測域センサ１では測定が困難な障害物が回避するように移動システムを走行させることができた。また、移動システムはサブゴール位置に停止して、移動安全領域地図データの拡大とナビゲーションを繰り返すことで、最終的に離れた場所にあるゴール位置までたどり着くことができた。
【０１０６】
このように、本移動システムでは、歩行者の軌跡情報を用いることで、動的な環境でも状況に応じて安全な経路で移動システムを移動させることができる。また、上記のナビゲーション実験からも分かるように、測域センサ１で測定が困難な障害物が存在する場合でも、歩行者の軌跡情報を用いることで自動的に移動システムが障害物を回避する経路を探索することができる。
【符号の説明】
【０１０７】
１ 測域センサ
２ 経路探索部
３ 方位センサ
８ ＧＰＳセンサ
９ 左モータ駆動部
１０ 右モータ駆動部
１１ モータ制御部
１１１ 取得部
１１２ 地図データ生成部
１１３ 特定部
１１４ 投票部
１１５ 安全率算出部
１１６ 探索部
１１７ 移動制御部
１２１ 地図データ記憶部
１２２ 移動安全領域地図データ記憶部
【特許請求の範囲】
【請求項１】
周囲に存在する物体までの位置を示す測域データを取得する取得手段と、
前記取得手段により取得された測域データを二次元の座標空間にプロットすることで、前記物体の位置を示す２次元の地図データを生成する地図データ生成手段と、
前記地図データから周囲に存在する各物体を示す点群を抽出し、抽出した点群のうち所定の歩行者条件を満たしている点群を、歩行者を示す点群として特定する特定手段と、
前記地図データが示す二次元の座標空間を格子状に区画することで投票空間を生成し、生成した投票空間を構成する各格子に、前記特定手段により特定された歩行者を示す点群を構成する各測域データを投票する投票手段と、
前記投票手段による投票結果が高い格子ほどに、安全率が高くなるように各格子の安全率を算出する安全率算出手段と、
前記安全率が算出された投票空間において、所定のスタート位置を示す格子と所定のゴール位置を示す格子との距離が最短となり、かつ、前記安全率が最も高くなるような経路を所定のアルゴリズムを用いて探索する探索手段とを備えることを特徴とする経路探索装置。
【請求項２】
前記特定手段は、点群の移動速度及び形状を前記歩行者条件として採用することを特徴とする請求項１記載の経路探索装置。
【請求項３】
前記歩行者条件における点群の形状は、点群を構成する点の個数及び点群の幅を含むことを特徴とする請求項２記載の経路探索装置。
【請求項４】
前記測域データは、地面からの高さが歩行者の膝下の高さとなるように水平方向に向けてレーザビームを照射することで測定されたものであり、
前記投票手段は、歩行者を示す点群を構成する測域データが投票された格子を中心として当該格子の投票値を所定の二次元分布関数に従って変化させることで周囲の格子の投票値を設定することを特徴とする請求項１〜３のいずれかに記載の経路探索装置。
【請求項５】
前記特定手段は、前記歩行者条件を満たさない点群を障害物を示す点群として特定し、
前記投票手段は、前記歩行者を示す点群の測域データについては投票値を正の値にして投票し、前記障害物を示す点群を構成する測域データについては投票値を負の値にして投票することを特徴とする請求項４記載の経路探索装置。
【請求項６】
前記探索手段は、Ａ＊アルゴリズムを用いて経路を探索することを特徴とする請求項１〜５のいずれかに記載の経路探索装置。
【請求項７】
ＧＰＳセンサと方位センサとを更に備え、
前記安全率算出手段は、前記ＧＰＳセンサにより測定された自己の現在位置を示す位置データと前記方位センサにより測定された方位データとを基に、前記投票空間の座標系をグローバル座標系に変換し、
前記探索手段は、前記ＧＰＳセンサにより測定された自己の現在位置を示す格子を前記投票空間に設定し、設定した現在位置を示す格子を前記スタート位置とすることで、前記経路を探索することを特徴とする請求項１〜６のいずれかに記載の経路探索装置。
【請求項８】
請求項１〜７のいずれかに記載の経路探索装置と、
タイヤ部と、
前記タイヤ部を駆動するためのタイヤ駆動手段とを備え、
前記経路探索装置は、前記経路探索装置により探索された経路に沿って移動するように前記タイヤ駆動手段を駆動させることを特徴とする移動システム。
【請求項１】
周囲に存在する物体までの位置を示す測域データを取得する取得手段と、
前記取得手段により取得された測域データを二次元の座標空間にプロットすることで、前記物体の位置を示す２次元の地図データを生成する地図データ生成手段と、
前記地図データから周囲に存在する各物体を示す点群を抽出し、抽出した点群のうち所定の歩行者条件を満たしている点群を、歩行者を示す点群として特定する特定手段と、
前記地図データが示す二次元の座標空間を格子状に区画することで投票空間を生成し、生成した投票空間を構成する各格子に、前記特定手段により特定された歩行者を示す点群を構成する各測域データを投票する投票手段と、
前記投票手段による投票結果が高い格子ほどに、安全率が高くなるように各格子の安全率を算出する安全率算出手段と、
前記安全率が算出された投票空間において、所定のスタート位置を示す格子と所定のゴール位置を示す格子との距離が最短となり、かつ、前記安全率が最も高くなるような経路を所定のアルゴリズムを用いて探索する探索手段とを備えることを特徴とする経路探索装置。
【請求項２】
前記特定手段は、点群の移動速度及び形状を前記歩行者条件として採用することを特徴とする請求項１記載の経路探索装置。
【請求項３】
前記歩行者条件における点群の形状は、点群を構成する点の個数及び点群の幅を含むことを特徴とする請求項２記載の経路探索装置。
【請求項４】
前記測域データは、地面からの高さが歩行者の膝下の高さとなるように水平方向に向けてレーザビームを照射することで測定されたものであり、
前記投票手段は、歩行者を示す点群を構成する測域データが投票された格子を中心として当該格子の投票値を所定の二次元分布関数に従って変化させることで周囲の格子の投票値を設定することを特徴とする請求項１〜３のいずれかに記載の経路探索装置。
【請求項５】
前記特定手段は、前記歩行者条件を満たさない点群を障害物を示す点群として特定し、
前記投票手段は、前記歩行者を示す点群の測域データについては投票値を正の値にして投票し、前記障害物を示す点群を構成する測域データについては投票値を負の値にして投票することを特徴とする請求項４記載の経路探索装置。
【請求項６】
前記探索手段は、Ａ＊アルゴリズムを用いて経路を探索することを特徴とする請求項１〜５のいずれかに記載の経路探索装置。
【請求項７】
ＧＰＳセンサと方位センサとを更に備え、
前記安全率算出手段は、前記ＧＰＳセンサにより測定された自己の現在位置を示す位置データと前記方位センサにより測定された方位データとを基に、前記投票空間の座標系をグローバル座標系に変換し、
前記探索手段は、前記ＧＰＳセンサにより測定された自己の現在位置を示す格子を前記投票空間に設定し、設定した現在位置を示す格子を前記スタート位置とすることで、前記経路を探索することを特徴とする請求項１〜６のいずれかに記載の経路探索装置。
【請求項８】
請求項１〜７のいずれかに記載の経路探索装置と、
タイヤ部と、
前記タイヤ部を駆動するためのタイヤ駆動手段とを備え、
前記経路探索装置は、前記経路探索装置により探索された経路に沿って移動するように前記タイヤ駆動手段を駆動させることを特徴とする移動システム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２０１１−５９０４３（Ｐ２０１１−５９０４３Ａ）
【公開日】平成２３年３月２４日（２０１１．３．２４）
【国際特許分類】
【出願番号】特願２００９−２１１４６４（Ｐ２００９−２１１４６４）
【出願日】平成２１年９月１４日（２００９．９．１４）
【出願人】（５０４１４３４４１）国立大学法人 奈良先端科学技術大学院大学 (226)
【Ｆターム（参考）】
【公開日】平成２３年３月２４日（２０１１．３．２４）
【国際特許分類】
【出願日】平成２１年９月１４日（２００９．９．１４）
【出願人】（５０４１４３４４１）国立大学法人 奈良先端科学技術大学院大学 (226)
【Ｆターム（参考）】
[ Back to top ]