JP2020086588A - 移動経路生成装置、移動経路生成方法、および、コンピュータプログラム - Google Patents

Abstract

【課題】 移動経路生成装置において、自律移動体がより確実にかつ高精度に目的地に到達する技術を提供することを目的とする。【解決手段】 自律移動体の現在地から目的地までの移動経路を生成する移動経路生成装置であって、自律移動体の位置を推定するためのランドマークに関する情報が含まれる地図情報を記憶する地図データベースと、地図情報を用いて、自律移動体の現在地から目的地までの間においてランドマークを観測可能な可観測エリアを推定する可観測エリア地図生成部と、推定された可観測エリアを用いて移動経路を生成する移動経路生成部と、を備える。【選択図】 図１

Description

本発明は、移動経路生成装置、移動経路生成方法、および、コンピュータプログラムに関する。

従来、地図情報を用いて自律移動体の現在地から目的地までの移動経路を生成する移動経路生成装置が知られている。移動経路生成装置では、現在地から目的地までの移動にかかる時間やエネルギーなどの移動コストを基準として、最適な移動経路を生成する。例えば、特許文献１には、人間を乗せて移動する自律移動体の移動経路を生成する装置であって、自律移動体の移動距離に加え、自律移動体からの見通しの良し悪しと経験的に得た安心感の大きさとに基づいて、移動経路を生成する技術が開示されている。また、特許文献２には、自律移動体の移動経路上に複数のノードを設定し、現在地から当該複数のノードまで直接移動したときの移動コストを最小にする移動経路を順次選択する技術が開示されている。

特開２０１６−２１８８８０号公報 特開２００５−５０１０５号公報

特許文献１に記載の移動経路生成装置では、自律移動体に搭乗者がいることを想定しているため、搭乗者に安心感を与える移動経路を生成する。しかしながら、搭乗者の安心感を優先すると、自律移動体の位置を推定するための環境特徴に乏しい移動経路が設定される場合があり、自律移動体の現在地の推定誤差が大きくなるおそれがある。
また、特許文献２に記載の移動経路生成装置は、障害物や自身の現在地を推定するための環境認識手段を備えている。しかしながら、移動コストが最小となる移動経路上に、自身の現在地を推定するための環境特徴が乏しい空間が存在すると現在地の推定誤差が大きくなり、自身の現在地を見失って移動できなくなるおそれがある。また、自律移動体が目的地まで到達しても、目的地での誤差が大きいと自律移動体の次の動作に影響をおよぼすおそれがある。

本発明は、上述した課題を解決するためになされたものであり、自律移動体の現在地から目的地までの移動経路を生成する移動経路生成装置において、自律移動体が高精度に目的地に到達する技術を提供することを目的とする。

本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態として実現することが可能である。

（１）本発明の一形態によれば、自律移動体の現在地から目的地までの移動経路を生成する移動経路生成装置が提供される。移動経路生成装置は、前記自律移動体の位置を推定するためのランドマークに関する情報が含まれる地図情報を記憶する地図情報記憶部と、前記地図情報を用いて、前記自律移動体の現在地から目的地までの間において前記ランドマークを観測可能な可観測エリアを推定する可観測エリア推定部と、推定された前記可観測エリアを用いて前記移動経路を生成する移動経路生成部と、を備える。

この構成によれば、移動経路生成部は、可観測エリア推定部において推定された、ランドマークを観測可能な可観測エリアを用いて移動経路を生成する。自律移動体の移動経路が、可観測エリアを優先的に通るように生成されると、当該移動経路上を移動する自律移動体は、自身の現在地を推定するためのランドマークを確実に観測することができる。これにより、自律移動体の現在地の推定精度を高めることができるとともに、現在地を見失うことなく目的地に到達できるため目的地での誤差を小さくすることができる。したがって、自律移動体は、高精度に目的地に到達することができる。

（２）上記形態の移動経路生成装置は、さらに、前記自律移動体に配置され、前記自律移動体の外部環境を認識するための外部環境情報を取得する環境認識センサを備え、前記移動経路生成部は、前記環境認識センサが前記外部環境情報を取得する方向を、前記ランドマークを観察可能な方向に設定してもよい。この構成によれば、移動経路生成部は、移動経路の生成に加え、当該移動経路上を移動する自律移動体に配置されている環境認識センサが外部環境情報を取得する方向を、ランドマークを観察可能な方向に設定する。これにより、移動経路上を移動する自律移動体の環境認識センサがランドマークをより確実に検出することができるため、自律移動体の現在地の推定誤差をさらに小さくすることができる。

（３）上記形態の移動経路生成装置は、さらに、前記外部環境情報に含まれる前記ランドマークに関する情報を検出するランドマーク検出部と、前記ランドマーク検出部が検出したランドマークに関する情報と、前記地図情報に含まれるランドマークに関する情報とを照合して前記自律移動体の現在地を推定する現在地推定部と、を備え、前記移動経路生成部は、前記現在地推定部によって推定された現在地から目的地までの前記移動経路を生成してもよい。この構成によれば、移動経路の出発点である現在地の位置精度が向上するため、自律移動体は、より確実に、かつ、より高精度に目的地に到達することができる。

（４）上記形態の移動経路生成装置は、さらに、前記外部環境情報に含まれる移動障害物に関する情報を検出する障害物検出部と、検出された前記移動障害物に関する情報を用いて、前記移動障害物の位置変化を予測する障害物位置予測部と、を備え、前記可観測エリア推定部は、前記障害物位置予測部による前記移動障害物の位置変化予測を用いて、前記環境認識センサと前記ランドマークとの間に前記移動障害物が位置することによって前記環境認識センサが取得する前記外部環境情報に前記ランドマークに関する情報が含まれなくなる遮蔽エリアを推定し、前記可観測エリアから前記遮蔽エリアを除外してもよい。この構成によれば、可観測エリア推定部は、環境認識センサが取得する外部環境情報にランドマークに関する情報が含まれなくなる遮蔽エリアを、可観測エリアから除外するため、移動経路上を移動する自律移動体の環境認識センサがランドマークを検出できずに、現在地を見失うことを防止することができる。これにより、自律移動体は、より確実に目的地に到達することができる。

（５）上記形態の移動経路生成装置において、前記移動経路生成部は、前記自律移動体から観測可能な前記ランドマークの数と、前記自律移動体から観測可能な視野内の前記ランドマークの分散状態と、所定の時間間隔での前記自律移動体からの複数回の観測において、共通に観測可能な前記ランドマークの割合と、所定の時間間隔での、前記環境認識センサが前記外部環境情報を取得する方向の変化量とのうち、少なくとも一つを用いて、前記移動経路を生成してもよい。この構成によれば、移動経路生成部は、ランドマークの認識のしやすさに関連するこれら４つの項目の少なくとも１つを用いて移動経路を生成することができるため、自律移動体は、より確実に、かつ、より高精度に目的地に到達することができる。

（６）上記形態の移動経路生成装置は、さらに、前記地図情報に含まれる前記ランドマークに関する情報を用いて、当該ランドマークの前記自律移動体の位置推定への貢献度を推定するランドマーク選定部を備え、前記可観測エリア推定部は、前記貢献度が高い前記ランドマークのみの可観測エリア、または、前記貢献度の重みを反映した可観測エリア、を推定してもよい。この構成によれば、ランドマーク選定部は、自律移動体の位置推定への貢献度の高さに応じてランドマークを選定し、可観測エリア推定部は、当該選定したランドマークの可観測エリアを推定する。これにより、可観測エリア推定部は、全てのランドマークについて可観測エリアを推定する代わりに、自律移動体の位置推定への貢献度の高いランドマークについての可観測エリアを推定するため、より短時間でかつ少ない労力で移動経路を生成することができる。したがって、移動経路生成装置における移動経路の生成処理の効率化や高速化を図ることができる。

なお、本発明は、種々の態様で実現することが可能であり、例えば、移動経路生成方法、移動経路生成システム、移動経路の生成をコンピュータに実行させるコンピュータプログラム、コンピュータプログラムを配布するためのサーバ装置、コンピュータプログラムを記憶した一時的でない記憶媒体等などの形態で実現することができる。

第１実施形態の移動経路生成装置の概略構成を示した説明図である。 第１実施形態の移動経路生成方法のフローチャートである。 第１実施形態で移動経路が生成される空間の模式図である。 第１実施形態で推定される可観測エリアの模式図である。 第１実施形態で生成される可観測エリア地図の模式図である。 第１実施形態で生成される移動経路を示す模式図である。 第１実施形態で設定される環境認識センサが向けられる方向を示す模式図である。 比較例の移動経路生成方法で生成される移動経路を示す模式図である。 第２実施形態の移動経路生成装置の概略構成を示した説明図である。 第２実施形態の移動経路生成方法のフローチャートである。 第２実施形態での移動障害物の移動軌跡を示す模式図である。 第２実施形態での別の移動障害物の移動軌跡を示す模式図である。 第２実施形態で生成される移動経路を示す模式図である。

＜第１実施形態＞
図１は、第１実施形態の移動経路生成装置の概略構成を示した説明図である。図２は、第１実施形態の移動経路生成方法のフローチャートである。図３は、第１実施形態で移動経路が生成される空間の模式図である。第１実施形態の移動経路生成装置１は、自律的に移動する自律移動体５の現在地から目的地までの移動経路を生成する装置である。移動経路生成装置１は、事前に取得している地図情報と、自律移動体５がオンタイムで取得する自律移動体５の外部環境を認識するための外部環境情報とを照合して、移動経路を生成する。自律移動体５としては、自身の外部環境を認識する手段を有する、自律走行型ロボットや自律飛行型ドローンなどが、例示される。なお、本実施形態では、移動経路生成装置１による２次元平面内での自律走行型ロボットの移動経路の生成を説明するが、移動経路生成装置１は、３次元空間内でのドローンの移動経路を生成してもよい。

移動経路生成装置１は、地図データベース１０と、環境認識センサ１１と、ランドマーク検出部１２と、現在地推定部１３と、可観測エリア地図生成部１４と、移動経路生成部１５と、を備える。本実施形態では、移動経路生成装置１は、自律移動体５に搭載されている。しかしながら、環境認識センサ１１を除く、地図データベース１０と、ランドマーク検出部１２と、現在地推定部１３と、可観測エリア地図生成部１４と、移動経路生成部１５と、の一部または全部は、自律移動体５の外部に設けられていてもよい。

地図データベース１０は、自律移動体５が移動する空間の地図情報であって、図２に示す移動経路生成方法によって、移動経路を生成する前に取得されているデータの集まりである。地図データベース１０には、図３に示す地図のように、自律移動体５の位置を推定するための複数のランドマーク４や自律移動体５の移動の障害となる障害物７の、位置や形状、移動の有無などの情報が記憶されている。ランドマーク４や障害物７は、画像特徴点、幾何学的特徴、物体など、様々なレベルの特徴に関連付けて記憶されることが可能である。本実施形態では、ランドマーク４や障害物７の情報は、ＳＩＦＴやＯＲＢといった画像特徴点に関連付けられており、例えば、ＯＲＢ−ＳＬＡＭを用いれば、ＯＲＢを特徴点とする地図情報を構築することができる。本実施形態では、地図データベース１０には、複数のランドマーク４や障害物７の３次元位置だけでなく、ランドマーク４や障害物７の特徴点を観測したときのカメラの位置や姿勢も合わせて記憶されている。

図３の地図では、自律移動体５を、菱形の図形で示している。すなわち、図３の地図では、菱形がある位置が、自律移動体５の現在地を示している。また、自律移動体５の目的地６を、６つの突起を有する星形で示している。
図３の地図では、ランドマーク４は、４つの突起を有する星形で示している。図３の地図では、ランドマーク４は、自律移動体５の現在地から近い順に、ランドマーク４ａからランドマーク４ｖまでの、２２個が示されている。また、障害物７は、矩形または楕円で示している。図３に示す模式図では、障害物７は、障害物７ａから障害物７ｋまでの、１１個示されている。移動経路生成装置１は、図３に示す地図をベースにして、自律移動体５の現在地から目的地６までの移動経路を生成する。

環境認識センサ１１は、自律移動体５に配置され、自律移動体５の外部環境情報を取得する機能を有している、例えば、カメラやレーザーレンジファインダなどである。本実施形態では、自律移動体５には、自律移動体５自身に固定されているカメラが搭載されており、自律移動体５の姿勢を変化させることで、カメラが外部環境情報を取得する方向を変更することが可能である。なお、環境認識センサ１１は、自律移動体５に対して相対移動可能な機構を介して自律移動体５に設けられ、自律移動体５の姿勢を変化させることなく環境認識センサ１１が外部環境情報を取得する方向を変更できるようにしてもよい。

ランドマーク検出部１２は、環境認識センサ１１に電気的に接続している。ランドマーク検出部１２は、環境認識センサ１１が取得した外部環境情報を処理し、当該外部環境情報から環境認識センサ１１が認識しているランドマーク４を検出する。

現在地推定部１３は、ランドマーク検出部１２と地図データベース１０とに電気的に接続している。現在地推定部１３は、ランドマーク検出部１２が検出したランドマークに関する情報と、地図データベース１０に記憶されている地図情報のランドマークに関する情報と、を照合し、自律移動体５の現在地と、環境認識センサ１１の姿勢と、を推定する。

可観測エリア地図生成部１４は、現在地推定部１３と地図データベース１０とに電気的に接続している。可観測エリア地図生成部１４は、地図データベース１０に記憶されている地図上においてランドマーク４を観測可能なエリアとしての可観測エリアを推定する。可観測エリア地図生成部１４は、図３に示すように地図中に複数のランドマーク４がある場合、それぞれのランドマーク４について可観測エリアをそれぞれ推定し、複数の可観測エリアを組み合わせた可観測エリア地図を生成する。可観測エリア地図生成部１４での可観測エリア地図の生成方法の詳細は、後述する。

移動経路生成部１５は、可観測エリア地図生成部１４に電気的に接続している。移動経路生成部１５は、可観測エリア地図生成部１４の可観測エリア地図を用いて、自律移動体５の移動経路を生成する。移動経路生成部１５での移動経路の生成方法の詳細は、後述する。

次に、移動経路生成装置１による自律移動体５の移動経路の生成方法について説明する。本実施形態の移動経路生成装置１における移動経路の生成方法は、自律移動体５の現在地を高精度に推定可能な場所を選択しつつ、目的地まで向かうことが可能な移動経路を生成する。

最初に、地図データベース１０は、自律移動体５が移動する地図情報を記憶する（ステップＳ１０）。地図データベース１０は、上述したように、複数のランドマーク４に関する情報や障害物７に関する情報を含む地図情報を記憶する。

次に、環境認識センサ１１は、外部環境情報を取得する（ステップＳ１１）。具体的には、環境認識センサ１１は、自律移動体５に搭載されているカメラによって自律移動体５の外部環境を撮影し、撮像結果を自律移動体５の外部環境情報として取得する。環境認識センサ１１は、撮像結果をランドマーク検出部１２に出力する。

次に、ランドマーク検出部１２は、外部環境情報からランドマーク４を検出する（ステップＳ１２）。具体的には、ランドマーク検出部１２は、環境認識センサ１１の撮像結果に写っているランドマーク４を検出する。ランドマーク検出部１２は、検出したランドマーク４を現在地推定部１３に出力する。

次に、現在地推定部１３は、検出したランドマーク４に関する情報と地図情報のランドマーク４に関する情報と照合し、自律移動体５の現在地を推定する（ステップＳ１３）。具体的には、現在地推定部１３は、ランドマーク検出部１２が検出したランドマーク４と同じランドマーク４を、地図情報内で探す。ランドマーク検出部１２が検出したランドマーク４と一致するランドマーク４を地図情報内で見つけた場合、当該ランドマーク４について、地図データベース１０に記憶されている当該ランドマーク４の位置や特徴点を観測したときのカメラの位置と姿勢とから、自律移動体５の位置と、環境認識センサ１１の姿勢とを推定する。

次に、現在地推定部１３は、目的地情報を取得する（ステップＳ１４）。具体的には、現在地推定部１３は、図示しない入力装置によって移動経路生成装置１に入力される自律移動体５の目的地６を記憶する。

次に、可観測エリア地図生成部１４は、地図情報を用いてランドマーク４の可観測エリア８を推定する（ステップＳ１５）。具体的には、可観測エリア地図生成部１４は、地図データベース１０から取得する図３のランドマーク４が分布している地図情報を用いて、複数のランドマーク４のそれぞれについて可観測エリア８を推定する。

図４は、本実施形態で推定される可観測エリア８の模式図である。地図データベース１０の地図情報に含まれるランドマーク４のそれぞれは、見える位置と方向とが、地図を生成するときに得られる情報から明らかとなっている。
例えば、画像特徴点でランドマーク４を記憶している場合、観測方向によってランドマーク４の見え方が変化するため、ランドマーク４は全方位から観測可能ではないが、ある程度の角度範囲であれば同じランドマーク４を観測可能であることがわかっている。また、地図を生成するときに使用されるカメラの解像度を考慮すると、ランドマーク４が観測できる距離も概算することが可能であることがわかっている。可観測エリア地図生成部１４は、図３の地図を生成するときに得られるこれらの情報から、図４に示す扇形状の可観測エリア８を、複数のランドマーク４のそれぞれについて推定する。

図４には、複数のランドマーク４のうち３つのランドマーク４ｄ、４ｆ、４ｇの可観測エリア８を示す。具体的には、ランドマーク４ｄでは、図４に示す扇形状の可観測エリア８ｄが推定される。同様に、ランドマーク４ｆでは、図４の扇形状の可観測エリア８ｆが推定され、ランドマーク４ｇでは、図４の扇形状の可観測エリア８ｇが推定される。
図４に示す推定された可観測エリア８の結果から、複数の可観測エリア８が互いに重なる領域に位置する自律移動体５は、少なくとも二つ以上のランドマーク４を観測することが可能であるといえる。例えば、上述の可観測エリア８ｄ、８ｆ、８ｇでは、図４に示す領域８１では、三つのランドマーク４ｄ、４ｆ、４ｇの可観測エリア８が重なっており、領域８１からは、三つのランドマーク４ｄ、４ｆ、４ｇが観測可能であるといえる。同様に、図４に示す領域８２ａからは、二つのランドマーク４ｄ、４ｆが観測可能であり、領域８２ｂからは、二つのランドマーク４ｆ、４ｇが観測可能であるといえる。また、図４に示す領域８３ａでは、一つのランドマーク４ｄが観測可能であり、領域８３ｂ、８３ｃのそれぞれでは、一つのランドマーク４ｆが観測可能であり、領域８３ｄでは、一つのランドマーク４ｄが観測可能であるといえる。

図５は、本実施形態で生成される可観測エリア地図の模式図である。可観測エリア地図生成部１４は、図５に示すように、地図データベース１０に記憶されている地図の自律移動体５が移動可能な領域に、グリッドを生成し、全てのランドマーク４について可観測エリア８を推定した結果を、当該グリッドに反映することが可能である。具体的には、グリッドごとに可観測エリア８が重なった数に応じて表示分けをし、可視化する。例えば、図５では、各ランドマーク４の可観測エリア８が重なった数が多いほど、グリッド内に表示されているドットの密度が高くなるように表示している。

可観測エリア地図生成部１４は、このようにして、自律移動体５からランドマーク４を観測可能な可観測エリア８を複数のランドマーク４のそれぞれについて推定し、複数の可観測エリア８を足し合わせた可観測エリア地図（図５）を生成する。可観測エリア地図生成部１４は、生成した可観測エリア地図を、移動経路生成部１５に出力する。

次に、移動経路生成部１５は、推定された可観測エリア８を用いて移動経路を生成するとともに環境認識センサ１１を向ける方向を設定する（ステップＳ１６）。具体的には、移動経路生成部１５は、可観測エリア地図生成部１４が生成した可観測エリア地図と、以下の式（１）に示す評価関数ｆとを用いて、自律移動体５の移動経路を生成し、環境認識センサ１１が外部環境情報を取得する方向を設定する。
ｆ（ｘ，ｙ，θ）
＝Ｆ(ｎ_t（ｘ，ｙ，θ），Ｄ_t（ｘ，ｙ，θ），Ｃ_t,t-1，１／Ｚ_t,t-1，１／Ｌ)
・・・（１）

ここで、式（１）に示す関数Ｆの変数ｎ_t（ｘ，ｙ，θ）は、地図内での位置座標が（ｘ，ｙ）であって、環境認識センサ１１の外部環境情報を取得する方向がθとなっている自律移動体５から観測可能なランドマーク４の数を示す。また、変数Ｄ_t（ｘ，ｙ，θ）は、地図内での位置座標が（ｘ，ｙ）であって、環境認識センサ１１の外部環境情報を取得する方向がθとなっている自律移動体５から観測可能な視野内でのランドマーク４の分散状態を示す。また、変数Ｃ_t,t-1は、時刻ｔと時刻（ｔ−１）との間での自律移動体５からの複数回の観測において、共通に観測可能なランドマーク４の割合を示す。変数Ｃ_t,t-1は、例えば、時刻ｔでの観測と時刻（ｔ−１）での観測とで共通に観測可能なランドマーク４の数を変数ｎ_t,t-1として表すと、以下の式（２）で表される。
Ｃ_t,t-1＝２×ｎ_t,t-1／(ｎ_t＋ｎ_t-1) ・・・（２）
また、変数Ｚ_t,t-1は、時刻ｔと時刻（ｔ−１）との間での、環境認識センサ１１が外部環境情報を取得する方向の変化量を示す。また、変数Ｌは、自律移動体５の現在地から目的地までの距離を示す。変数Ｚ_t,t-1と、変数Ｌとは、小さくなるほど自律移動体５の現在地の推定誤差が小さくなるため、式（１）では、これらの変数の逆数が、関数Ｆの変数として含まれている。

図６は、本実施形態で生成される移動経路を示す模式図である。自律移動体５の移動経路３０は、可観測エリア地図生成部１４が推定した可観測エリア８に含まれる領域であって、評価関数ｆが最大となる地図内での位置座標（ｘ、ｙ）を優先的に通過するように生成される。その生成結果を図６に示す。図６に示す移動経路３０は、評価関数ｆの値が大きいグリッドを通過するように生成されている。具体的には、移動経路３０は、図６に示すグリッドのうち評価関数ｆの値が大きいことを示すドットの密度が高いグリッドを通過するように、生成されている。このとき、例えば、自律移動体５の操舵輪が切れる角度など、自律移動体５の運動制約を考慮し、移動経路３０の曲線部分の最小内径が設定されてもよい。

また、本実施形態のように環境認識センサ１１が自律移動体５に固定されている場合、自律移動体５の運動制約によって、ランドマーク４の観察に不具合が生じる場合がある。そこで、本実施形態では、移動経路生成部１５は、移動経路３０の生成だけでなく、移動経路３０上での環境認識センサ１１が外部環境情報を取得する方向も設定する。

図７は、本実施形態で設定される環境認識センサ１１が外部環境情報を取得する方向を示す模式図である。図７には、図６に示した自律移動体５の移動経路３０を示してある。移動経路生成部１５は、移動経路３０上の各所における環境認識センサ１１が外部環境情報を取得する方向θが、評価関数ｆが最大となるように設定する。図７では、一例として、環境認識センサ１１が外部環境情報を取得する方向を、符号３１〜３６で示している。例えば、符号３１が示す環境認識センサ１１が外部環境情報を取得する方向には、例えば、ランドマーク４ｂ、４ｃが位置していることを示している。また、符号３２が示す環境認識センサ１１が外部環境情報を取得する方向には、例えば、ランドマーク４ｆ、４ｅが位置していることを示している。また、符号３３〜３６についても同様である。このように、本実施形態では、移動経路生成部１５は、自律移動体５の移動経路３０上において、評価関数ｆが最大となるように、環境認識センサ１１が外部環境情報を取得する方向を設定する。

以上説明した、本実施形態の移動経路生成装置１によれば、移動経路生成部１５は、可観測エリア地図生成部１４において推定された、ランドマーク４を観測可能な可観測エリア８を用いて移動経路３０を生成する。自律移動体５の移動経路３０は、可観測エリア８を優先的に通るように生成されるため、当該移動経路３０上を移動する自律移動体５は、現在地を推定するためのランドマーク４を確実に観測することができる。これにより、自律移動体５の推定される現在地の位置精度を高めることができるとともに、現在地を見失うことによって移動不能となることなく目的地６に到達できるため目的地６での誤差を小さくすることができる。したがって、自律移動体５は、より確実にかつ高精度に目的地６に到達することができる。

図８は、比較例の移動経路生成方法で生成される移動経路を示す模式図である。図８に示す地図は、本実施形態の地図データベース１０に記憶される地図と同じであって（図３参照）、比較例の移動経路生成方法で生成される移動経路４０は、自律移動体５の現在地から目的地６まで、最小コストとなるように生成されている。図８には、比較例の移動経路生成方法で生成される移動経路４０のうち、地点４１以降の予定されていた移動経路４０ａで示し、地点４１以降で実際に移動した移動経路４０を移動経路４０ｂで示す。

図８に示す移動経路４０では、移動経路４０上の地点４１において、自律移動体５の位置に誤差が生じ始めていることが分かる。これは、図５に示す可観測エリア地図から分かるように、ランドマーク４ｆの近傍では、ランドマーク４を認識しづらくなり、ランドマーク４を認識することによって現在地を推定する方法では、その精度に誤差が生じるためである。このため、比較例の移動経路生成方法で生成される移動経路４０では、地点４１を通過した後、移動予定であった移動経路４０ａを通らず、移動経路４０ｂを通ることとなり、図８に示すように、最終到達地にずれが生じるおそれがある。
一方、本実施形態では、ランドマーク４を確実に観測することができる移動経路３０を通るため、現在地の位置精度が高い状態のまま目的地６に到達できる。したがって、自律移動体５は、より確実にかつ高精度に目的地６に到達することができる。

また、本実施形態の移動経路生成装置１は、自律移動体５の外部環境を認識し、外部環境情報を取得する環境認識センサ１１を備えており、移動経路生成部１５は、移動経路３０上での環境認識センサ１１が外部環境情報を取得する方向を設定する。これにより、環境認識センサ１１がランドマーク４を確実に検出することができるため、自律移動体５の現在地の推定誤差をより小さくすることができる。

また、本実施形態の移動経路生成装置１は、ランドマーク検出部１２が検出したランドマーク４に関する情報と、地図情報に含まれるランドマーク４に関する情報とを照合し、自律移動体５の現在地を推定する現在地推定部１３を備える。移動経路生成部１５は、現在地推定部１３が推定する現在地から目的地６までの移動経路３０を生成する。これにより、移動経路３０の出発点である現在地の位置精度が向上するため、自律移動体５は、より高精度に目的地に到達することができる。

また、本実施形態の移動経路生成装置１によれば、移動経路生成部１５は、式（１）に示す評価関数ｆを用いて移動経路３０を生成する。評価関数ｆは、自律移動体５から観測可能なランドマーク４の数と、自律移動体５から観測可能な視野内のランドマーク４の分散状態と、所定の時間間隔での自律移動体５からの複数回の観測において共通に観測可能なランドマーク４の割合と、所定の時間間隔での環境認識センサ１１が外部環境情報を取得する方向の変化量と、を変数として含んでいる。この構成によれば、移動経路生成部１５は、さらにランドマーク４を観測しやすい移動経路３０を生成することができるため、自律移動体５は、より確実に、かつ、より高精度に目的地に到達することができる。

＜第２実施形態＞
図９は、第２実施形態の移動経路生成装置の概略構成を示した説明図である。第２実施形態の移動経路生成装置２は、第１実施形態の移動経路生成装置（図１）と比較すると、ランドマーク選定部１６と、障害物検出部１７と、障害物位置予測部１８と、を備える点が異なる。

ランドマーク選定部１６は、現在地推定部１３と、可観測エリア地図生成部１４と、地図データベース１０と、に電気的に接続している。ランドマーク選定部１６は、ランドマーク４における自律移動体５の現在地の推定に対する貢献度を推定し、貢献度が高いランドマーク４を選定する。
具体的には、ランドマーク選定部１６は、ランドマーク検出部１２が検出したランドマーク４と照合させるときの、観測可能範囲の広さと、安定性とを、地図データベース１０に記憶されている複数のランドマーク４のそれぞれに対して推定する。この複数のランドマーク４のそれぞれの観測可能範囲の広さと安定性との推定結果は、自律移動体５の現在地の推定に対する貢献度となる。ここで、ランドマーク４の安定性は、地図データベース１０に記憶されている地図を生成するときの異なる観測間の照合結果から評価する。ランドマーク選定部１６は、この推定結果を用いて、貢献度が高いランドマーク４を選定する。本実施形態のランドマーク選定部１６によるランドマーク４の選定では、自律移動体５の現在地の推定に対する貢献度が所定の値より高いランドマーク４と、当該貢献度が所定の値以下のランドマーク４とに２分割する。また、ランドマーク選定部１６は、複数の基準で層別する形にして、ランドマーク４の数を加味して利用するレベルを選択してもよい。

障害物検出部１７は、環境認識センサ１１に電気的に接続している。障害物検出部１７は、環境認識センサ１１で取得した外部環境情報に含まれる障害物のうち、移動する移動障害物を検出する。環境認識センサ１１がカメラである場合、例えば、パターン認識で人やロボットなど自律移動体５にとっての移動障害物を認識することが可能である。また、カメラがステレオカメラである場合、距離情報を取得することが可能であるため、時間経過に伴う障害物の位置変化から移動障害物を検出することが可能である。レーザーレンジファインダのようなレーザを用いたセンサについても同様である。障害物検出部１７は、カルマンフィルタなどのフィルタリングによって外部環境情報の時間変化を検出し、障害物が移動するものであるか否かを判定するとともに、移動障害物である場合、その位置や移動方向を推定する。

障害物位置予測部１８は、障害物検出部１７と、可観測エリア地図生成部１４と、に電気的に接続している。障害物位置予測部１８は、障害物検出部１７が検出した移動障害物について、移動障害物の移動軌跡を算出し、特定の時刻ｔにおける移動障害物の位置を推定する。

図１０は、本実施形態の移動経路生成方法のフローチャートである。次に、移動経路生成装置２による自律移動体５の移動経路の生成方法について説明する。

最初に、第１実施形態のステップＳ１０、Ｓ１１と同様に、地図データベース１０は、自律移動体５が移動する地図情報を記憶し（ステップＳ２０）、環境認識センサ１１は、外部環境情報を取得する（ステップＳ２１）。次に、第１実施形態のステップＳ１２、Ｓ１３と同様に、ランドマーク検出部１２は、外部環境情報からランドマーク４を検出し（ステップＳ２２）、現在地推定部１３は、検出したランドマークに関する情報と地図情報のランドマークに関する情報と照合し、自律移動体５の現在地を推定する（ステップＳ２３）。

次に、障害物検出部１７は、外部環境情報に含まれる移動障害物を検出する（ステップＳ２４）。具体的には、障害物検出部１７は、上述したように、環境認識センサ１１が出力する外部環境情報に含まれる障害物情報のうち移動している障害物を移動障害物として検出する。障害物検出部１７は、検出した移動障害物について、その位置や移動方向を、障害物情報を用いて推定する。障害物検出部１７は、移動障害物に関する情報を障害物位置予測部１８に出力する。

次に、障害物位置予測部１８は、時刻ｔにおける移動障害物の位置を予測する（ステップＳ２５）。具体的には、障害物位置予測部１８は、障害物検出部１７が出力する移動障害物に関する情報を用いて、移動障害物の移動軌跡を算出し、時刻ｔにおける位置を推定する。

図１１は、本実施形態での移動障害物の移動軌跡を示す模式図である。図１１（ａ）の地図では、移動障害物５１ａが、ランドマーク４ｇ、４ｉ、４ｊ、４ｋの近傍に位置している。移動障害物５１ａは、図１１（ａ）に示す移動軌跡５１ｂのように、楕円を描くように移動している。障害物位置予測部１８は、障害物検出部１７が出力する移動障害物５１ａに関する情報を用いて、この移動軌跡５１ｂを算出し、特定の時刻における位置を推定する。

図１２は、本実施形態での別の移動障害物の移動軌跡を示す模式図である。図１２（ａ）の地図では、移動障害物５２ａが、ランドマーク４ｄ、４ｅ、４ｆの近傍に位置している。移動障害物５２ａは、図１２（ａ）に示す移動軌跡５２ｂのように、一定区間を往復移動するように移動する。障害物位置予測部１８は、障害物検出部１７が出力する移動障害物５２ａに関する情報を用いて、この移動軌跡５２ｂを算出し、特定の時刻における位置を推定する。障害物位置予測部１８は、移動障害物５１ａ、５２ａの移動軌跡５１ｂ、５２ｂの情報を、可観測エリア地図生成部１４に出力する。
次に、第１実施形態のステップＳ１４と同様に、現在地推定部１３は、目的地情報を取得する（ステップＳ２６）。

次に、ランドマーク選定部１６は、ランドマーク４ごとに自律移動体５の現在地の推定に対する貢献度を推定する（ステップＳ２７）。具体的には、ランドマーク選定部１６は、上述したように、ランドマーク４ごとの観測可能範囲の広さと安定性とを推定し、自律移動体５の現在地の推定に対する貢献度が高いランドマーク４を選定する。ランドマーク選定部１６は、選定したランドマーク４を、可観測エリア地図生成部１４に出力する。

次に、可観測エリア地図生成部１４は、地図情報を用いてランドマーク４の可観測エリア８を推定する（ステップＳ２８）。具体的には、可観測エリア地図生成部１４は、地図データベース１０から取得するランドマーク４が分布している地図情報を用いて、ランドマーク選定部１６が選定したランドマーク４について可観測エリア８を推定する。

次に、可観測エリア地図生成部１４は、移動障害物５１ａ、５２ａの影響を考慮し、ランドマーク４の可観測エリア８を修正する（ステップＳ２９）。ステップＳ２９における可観測エリア地図生成部１４での処理を、図１１と図１２とを用いて具体的に説明する。

図１１（ａ）では、上述したように、移動障害物５１ａが、ランドマーク４ｇ、４ｉ、４ｊ、４ｋの近傍において楕円を描くように移動している。自律移動体５が図１１（ａ）に示す位置から環境認識センサ１１をランドマーク４ｉ、４ｊに向ける場合、移動障害物５１ａが環境認識センサ１１とランドマーク４ｉ、４ｊとの間に位置するため、環境認識センサ１１は、移動障害物５１ａに遮蔽されるランドマーク４ｉ、４ｊを認識することができない。このため、図１１（ａ）に示す位置の自律移動体５では、現在地の推定に誤差が生じるおそれがある。そこで、可観測エリア地図生成部１４は、障害物位置予測部１８が出力する移動障害物５１ａの移動軌跡５１ｂの情報から、ステップＳ２８において推定した可観測エリア８を修正する。この修正によって、図１１（ａ）に示すドットの密度の表示では、ランドマーク４ｉ、４ｊ近傍のグリッド内に表示されているドットの密度が低くなる（移動障害物５１ａがある本実施形態の可観測エリア地図の図１１（ａ）と、移動障害物がない第１実施形態の可観測エリア地図の図１１（ｂ）とを比較参照）。

また、図１２（ａ）では、上述したように、移動障害物５２ａが、ランドマーク４ｄ、４ｅ、４ｆの近傍において一定区間を往復移動するように移動する。自律移動体５が、移動障害物５２ａの移動軌跡５２ｂの近傍を通ると、自律移動体５と移動障害物５２ａとが接触するおそれがある。そこで、可観測エリア地図生成部１４は、障害物位置予測部１８が出力する移動障害物５２ａの移動軌跡５２ｂの情報から、ステップＳ２８において推定した可観測エリア８を修正する。この修正によって、図１２（ａ）に示すドットの密度の表示では、移動障害物５２ａの移動軌跡５２ｂ近傍のグリッド内のドットの表示がなくなり、可観測エリアから完全に除外される。（移動障害物５２ａがある本実施形態の可観測エリア地図の図１２（ａ）と、移動障害物がない第１実施形態の可観測エリア地図の図１１（ｂ）とを比較参照）。
可観測エリア地図生成部１４は、ステップＳ２９において修正した可観測エリア地図を、移動経路生成部１５に出力する。

次に、移動経路生成部１５は、推定された可観測エリア８を用いて移動経路を生成するとともに環境認識センサ１１を向ける方向を設定する（ステップＳ３０）。移動経路生成部１５は、可観測エリア地図生成部１４が出力したステップＳ２９において修正した可観測エリア地図と、式（１）に示す評価関数ｆとを用いて、自律移動体５の移動経路を生成し、環境認識センサ１１を向ける方向を設定する

図１３は、本実施形態で生成される移動経路を示す模式図である。図１３の地図には、図１１（ａ）と図１２（ａ）とに示す修正された可観測エリア地図を組み合わせた可観測エリア地図が示されている。ステップＳ３０では、移動経路生成部１５は、図１３に示す地図を用いて、自律移動体５の移動経路５０を生成する。図１３には、比較例として、移動障害物がない第１実施形態で生成される移動経路３０を点線で示している。移動経路５０は、修正された可観測エリア地図において、ドットの密度が高いグリッドを通るように生成されている。第１実施形態で生成される移動経路３０と比較すると、環境認識センサ１１が取得する外部環境情報にランドマーク４ｉ、４ｊに関する情報が含まれなくなる遮蔽エリアを形成する移動障害物５１ａの近傍や、衝突するおそれがある移動障害物５２ａの近傍を通らない移動経路となる。

以上説明した、本実施形態の移動経路生成装置２によれば、障害物位置予測部１８は、移動障害物５１ａ、５２ａの移動軌跡５１ｂ、５２ｂを算出し、特定の時刻における移動障害物の位置を推定する。可観測エリア地図生成部１４は、障害物位置予測部１８による移動障害物５１ａの位置の推定結果を用いて、環境認識センサ１１が取得する外部環境情報にランドマーク４ｉ、４ｊに関する情報が含まれなくなる遮蔽エリアを、可観測エリア８から除外し、可観測エリア８を修正する。これにより、移動経路５０上を移動する自律移動体５の環境認識センサ１１がランドマーク４を検出できずに現在地を見失うことを防止することができる。したがって、自律移動体５は、より確実に目的地に到達することができる。

また、本実施形態の移動経路生成装置２によれば、可観測エリア地図生成部１４は、障害物位置予測部１８による移動障害物５２ａの移動軌跡５２ｂを算出し、自律移動体５と移動障害物５２ａとの衝突を防止するように、可観測エリア８を修正する。これにより、自律移動体５は、移動障害物５２ａとの衝突による移動不能を防止することができるため、より確実に目的地に到達することができる。

また、本実施形態の移動経路生成装置２によれば、ランドマーク選定部１６は、自律移動体５の位置推定への貢献度の高さに応じてランドマーク４を選定する。可観測エリア地図生成部１４は、ランドマーク選定部１６が選定したランドマーク４の可観測エリア８のみから、可観測エリア地図を生成する。これにより、可観測エリア地図生成部１４は、全てのランドマーク４について可観測エリア８を推定する代わりに、自律移動体５の位置推定への貢献度の高いランドマーク４についての可観測エリアを推定するため、より短時間でかつ少ない労力で移動経路５０を生成することができる。したがって、移動経路生成装置２における移動経路５０の生成処理の効率化や高速化を図ることができる。

＜本実施形態の変形例＞
本発明は上記の実施形態に限られるものではなく、その要旨を逸脱しない範囲において種々の態様において実施することが可能であり、例えば次のような変形も可能である。

［変形例１］
上述の実施形態では、移動経路生成装置は、自律移動体に搭載されるとした。しかしながら、移動経路生成装置が置かれる場所はこれに限定されない。環境認識センサ１１だけが、自律移動体５に搭載され、ランドマーク検出部１２と、現在地推定部１３と、可観測エリア地図生成部１４と、移動経路生成部１５と、地図データベース１０とは、自律移動体の外に置かれていてもよい。この場合、環境認識センサ１１が取得する観測データを無線などによってランドマーク検出部１２に送信し、移動経路生成部１５で生成された移動経路を自律移動体５に送信することによって、自律移動体５が移動することとなる。

［変形例２］
上述の実施形態では、移動経路生成部１５は、環境認識センサ１１が外部環境情報を取得する方向を設定するとした。しかしながら、移動経路生成部１５は、移動経路だけを生成してもよい。例えば、環境認識センサ１１が、全周方向を観測可能なカメラの場合、環境認識センサ１１が外部環境情報を取得する方向を設定する必要はない。

［変形例３］
第２実施形態では、ランドマーク選定部１６は、自律移動体５の現在地の推定に対する貢献度が大きいランドマーク４を選定し、可観測エリア地図生成部１４は、当該貢献度が大きいランドマーク４の可観測エリア８のみで、可観測エリア地図を生成するとした。しかしながら、可観測エリア地図の生成方法は、これに限定されない。例えば、可観測エリア地図生成部１４は、全てのランドマーク４に対して、貢献度の重みを反映した可観測エリア８を推定し、全てのランドマーク４の可観測エリア８を、重みに応じて足し合わせる方法で可観測エリア地図を生成してもよい。

［変形例４］
上述の実施形態では、移動経路生成部１５は、可観測エリア地図と、複数の変数を含む式（１）に示す評価関数ｆとを用いて、移動経路を生成するとした。しかしながら、評価関数ｆの変数は、これに限定されない。上述した変数の少なくとも１つであってもよいし、例えば、自律移動体５の移動量の総計を変数としてもよい。また、別の評価関数と組み合わせて評価してもよい。また、移動経路生成部１５は、可観測エリアのみで移動経路を生成してもよい。

［変形例５］
上述の実施形態では、移動経路生成装置は、現在地から目的地までの移動経路を一度に生成するとした。しかしながら、現在地から目的地までの間の中間地を設定し、現在地から中間値までの移動経路を生成したのち、逐次的に目的地まで生成してもよい。

［変形例６］
上述の実施形態では、可観測エリア地図は、グリッド表現によって観測のしやすさを示しているが、グラフで表現したり、格子状ノードで表現したりしてもよい。

［変形例７］
第２実施形態では、移動経路生成装置２は、ランドマーク選定部１６と、障害物検出部１７と、障害物位置予測部１８と、を備えるとした。ランドマーク選定部１６だけでもよいし、障害物検出部１７と、障害物位置予測部１８との組み合わせだけでもよい。

以上、実施形態、変形例に基づき本態様について説明してきたが、上記した態様の実施の形態は、本態様の理解を容易にするためのものであり、本態様を限定するものではない。本態様は、その趣旨並びに特許請求の範囲を逸脱することなく、変更、改良され得るとともに、本態様にはその等価物が含まれる。また、その技術的特徴が本明細書中に必須なものとして説明されていなければ、適宜、削除することができる。

１、２…移動経路生成装置
４、４ｂ、４ｃ、４ｄ、４ｅ、４ｆ、４ｇ、４ｈ、４ｉ、４ｊ、４ｋ、４ｌ、４ｍ、４ｎ、４ｏ、４ｐ、４ｑ、４ｒ、４ｓ、４ｔ、４ｕ、４ｖ…ランドマーク
５…自律移動体
６…目的地
７、７ａ、７ｂ、７ｃ、７ｄ、７ｅ、７ｆ、７ｇ、７ｈ、７ｉ、７ｊ、７ｋ…障害物
８、８ｄ、８ｆ、８ｇ…可観測エリア
１０…地図データベース
１１…環境認識センサ
１２…ランドマーク検出部
１３…現在地推定部
１４…可観測エリア地図生成部
１５…移動経路生成部
１６…ランドマーク選定部
１７…障害物検出部
１８…障害物位置予測部
３０、５０…移動経路
４１…地点
５１ａ、５２ａ…移動障害物
５１ｂ、５２ｂ…移動軌跡
８１、８２ａ、８２ｂ、８３ａ、８３ｂ、８３ｃ、８３ｄ…領域

Claims (8)

1. 自律移動体の現在地から目的地までの移動経路を生成する移動経路生成装置であって、
   前記自律移動体の位置を推定するためのランドマークに関する情報が含まれる地図情報を記憶する地図情報記憶部と、
   前記地図情報を用いて、前記自律移動体の現在地から目的地までの間において前記ランドマークを観測可能な可観測エリアを推定する可観測エリア推定部と、
   推定された前記可観測エリアを用いて前記移動経路を生成する移動経路生成部と、を備える、
   移動経路生成装置。
2. 請求項１に記載の移動経路生成装置は、さらに、
   前記自律移動体に配置され、前記自律移動体の外部環境を認識するための外部環境情報を取得する環境認識センサを備え、
   前記移動経路生成部は、前記環境認識センサが前記外部環境情報を取得する方向を、前記ランドマークを観察可能な方向に設定する、
   移動経路生成装置。
3. 請求項２に記載の移動経路生成装置は、さらに、
   前記外部環境情報に含まれる前記ランドマークに関する情報を検出するランドマーク検出部と、
   前記ランドマーク検出部によって検出された前記ランドマークに関する情報と、前記地図情報に含まれる前記ランドマークに関する情報とを照合して前記自律移動体の現在地を推定する現在地推定部と、を備え、
   前記移動経路生成部は、前記現在地推定部によって推定された現在地から目的地までの前記移動経路を生成する、
   移動経路生成装置。
4. 請求項２または請求項３に記載の移動経路生成装置は、さらに、
   前記外部環境情報に含まれる移動障害物に関する情報を検出する障害物検出部と、
   検出された前記移動障害物に関する情報を用いて、前記移動障害物の位置変化を予測する障害物位置予測部と、を備え、
   前記可観測エリア推定部は、
   前記障害物位置予測部による前記移動障害物の位置変化予測を用いて、前記環境認識センサと前記ランドマークとの間に前記移動障害物が位置することによって、前記環境認識センサが取得する前記外部環境情報に前記ランドマークに関する情報が含まれなくなる遮蔽エリアを推定し、
   前記可観測エリアから前記遮蔽エリアを除外する、
   移動経路生成装置。
5. 請求項２から請求項４のいずれか一項に記載の移動経路生成装置であって、
   前記移動経路生成部は、
   前記自律移動体から観測可能な前記ランドマークの数と、
   前記自律移動体から観測可能な視野内の前記ランドマークの分散状態と、
   所定の時間間隔での前記自律移動体からの複数回の観測において、共通に観測可能な前記ランドマークの割合と、
   所定の時間間隔での、前記環境認識センサが前記外部環境情報を取得する方向の変化量と、のうち少なくとも一つを用いて、前記移動経路を生成する、
   移動経路生成装置。
6. 請求項１から請求項５のいずれか一項に記載の移動経路生成装置は、さらに、
   前記地図情報に含まれる前記ランドマークに関する情報を用いて、当該ランドマークの、前記自律移動体の位置推定への貢献度を推定するランドマーク選定部を備え、
   前記可観測エリア推定部は、前記貢献度が高い前記ランドマークのみの可観測エリア、または、前記貢献度の重みを反映した可観測エリア、を推定する、
   移動経路生成装置。
7. 自律移動体の現在地から目的地までの移動経路を生成する移動経路生成方法であって、
   前記自律移動体の位置を推定するためのランドマークに関する情報が含まれる地図情報を記憶する地図情報記憶工程と、
   前記地図情報を用いて、前記自律移動体の現在地から目的地までの間において前記ランドマークを観測可能な可観測エリアを推定する可観測エリア推定工程と、
   推定された前記可観測エリアを用いて前記移動経路を生成する移動経路生成工程と、を備える、
   移動経路生成方法。
8. 自律移動体の現在地から目的地までの移動経路の生成をコンピュータに実行させるコンピュータプログラムであって、
   前記自律移動体の位置を推定するためのランドマークに関する情報が含まれる地図情報を記憶する地図情報記憶機能と、
   前記地図情報を用いて、前記自律移動体の現在地から目的地までの間において前記ランドマークを観測可能な可観測エリアを推定する可観測エリア推定機能と、
   推定された前記可観測エリアを用いて前記移動経路を生成する移動経路生成機能と、を前記コンピュータに実行させる、
   コンピュータプログラム。