(57)【要約】  精度の高い軌跡を算出することを目的とする。  移動体（Ｖ）が走行した軌跡を補正する管理部（２０）であって、車輪回転量測定部（１６）が取得した移動体（Ｖ）の軌跡データに、複数のノードを設定し、車輪回転量測定部（１６）が取得した移動体（Ｖ）の位置データを、ノードに対応付けるとともに、車輪回転量測定部（１６）とは別の他の測定手段が取得した移動体（Ｖ）の位置データを、ノードに対応付け、ノードが生起する可能性がある位置を確率で示し、ノードに対応付けられた位置データが生起する可能性がある位置を確率で示し、各確率を基に、ノードおよび位置データを変数として含む評価関数を算出する評価関数生成部（２１）と、評価関数を基に、各ノードが生起する確率が最も大きい軌跡を算出する軌跡最適化計算部（２２）と、を有することを特徴とする。
2012160630.tif 000002
【特許請求の範囲】
【請求項１】
移動体が走行した軌跡を補正する軌跡補正装置による軌跡補正方法であって、
前記軌跡補正装置は、
１の測定手段が取得した前記移動体の軌跡データに、複数のノードを設定し、
前記１の測定手段が取得した前記移動体の位置データを、前記ノードに対応付けるとともに、前記１の測定手段とは別の他の測定手段が取得した前記移動体の位置データを、前記ノードに対応付け、
前記ノードが生起する可能性がある位置を確率で示し、
前記ノードに対応付けられた位置データが生起する可能性がある位置を確率で示し、
各確率を基に、前記ノードおよび前記位置データを変数として含む評価関数を算出し、
前記評価関数を基に、各ノードが生起する確率が最も大きい軌跡を算出する
ことを特徴とする軌跡補正方法。
【請求項２】
前記確率は、確率密度関数で示されており、
前記評価関数は、尤度関数であり、
前記軌跡補正装置は、
最尤推定法によって、前記各ノードが生起する確率が最も大きい軌跡を算出する
ことを特徴とする請求の範囲第１項に記載の軌跡補正方法。
【請求項３】
前記軌跡補正装置は、
前記算出された軌跡の周囲に、構造物の形状データを貼付することにより、地図データを作成する
ことを特徴とする請求の範囲第１項または請求の範囲第２項に記載の軌跡補正方法。
【請求項４】
移動体が走行した軌跡を補正する軌跡補正装置であって、
１の測定手段が取得した前記移動体の軌跡データに、複数のノードを設定し、
前記１の測定手段が取得した前記移動体の位置データを、前記ノードに対応付けるとともに、前記１の測定手段とは別の他の測定手段が取得した前記移動体の位置データを、前記ノードに対応付け、
前記ノードが生起する可能性がある位置を確率で示し、
前記ノードに対応付けられた位置データが生起する可能性がある位置を確率で示し、
各確率を基に、前記ノードおよび前記位置データを変数として含む評価関数を算出する評価関数生成部と、
前記評価関数を基に、各ノードが生起する確率が最も大きい軌跡を算出する軌跡最適化計算部と、
を有することを特徴とする軌跡補正装置。
【請求項５】
請求の範囲第４項に記載の軌跡補正装置を搭載している
ことを特徴とする移動体装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の軌跡を補正する軌跡補正方法、軌跡補正装置および移動体装置の技術に関する。
【背景技術】
【０００２】
従来、移動体に搭載された測定装置（内界センサや外界センサ）によって測定されたデータに基づいて、実環境に対応した地図を生成しながら、自己位置を推定して目標経路に従って移動する自律移動システム（例えば、特許文献１，２を参照）が開示されている。【０００３】
特許文献１には、カメラを外界センサとして用いて、特定の形状をランドマーク（目印）として認識し、地図を生成しながら自律移動を行う環境同定装置、環境同定方法、プログラム、記録媒体およびロボット装置が開示されている。
特許文献２には、レーザスキャナを外界センサとして用いて、現在時刻に取得した周辺物体の形状データと、１つ前の時刻に現在とは異なる位置で取得した周辺物体の形状データとを逐次的にマッチング（重ね合わせ）することにより、周辺物体の形状データを測定した領域を拡大していくことで地図を生成する環境地図生成方法および移動ロボットが開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００４−１１０８０２号公報
【特許文献２】特開２００８−２７６３４８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
移動体が走行し、取得した軌跡において、測定誤差（ずれ）が発生する。そのため、従来技術のような逐次的な地図生成の手法は、作成する地図の領域が拡大するにつれて累積誤差が大きくなるという問題がある。特に、以前に訪れた地点を異なる道順で再度通過した際や、複数台の車両で地図を生成した際に、測定誤差によって同一の地点が地図上では異なる地点として記憶され、整合性が保てずに齟齬が発生する場合がある。その結果、地図の正確性が悪化し、移動体が自己位置や目標経路を見失い、移動の継続が困難になるなどの問題点がある。【０００６】
このような背景に鑑みて本発明がなされたのであり、本発明は、精度の高い軌跡を算出することを目的とする。
【課題を解決するための手段】
【０００７】
前記課題を解決するため、本発明は、移動体が走行した軌跡を補正する軌跡補正装置による軌跡補正方法であって、前記軌跡補正装置は、１の測定手段が取得した前記移動体の軌跡データに、複数のノードを設定し、前記１の測定手段が取得した前記移動体の位置データを、前記ノードに対応付けるとともに、前記１の測定手段とは別の他の測定手段が取得した前記移動体の位置データを、前記ノードに対応付け、前記ノードが生起する可能性がある位置を確率で示し、前記ノードに対応付けられた位置データが生起する可能性がある位置を確率で示し、各確率を基に、前記ノードおよび前記位置データを変数として含む評価関数を算出し、前記評価関数を基に、各ノードが生起する確率が最も大きい軌跡を算出することを特徴とする。
【発明の効果】
【０００８】
本発明によれば、精度の高い軌跡を算出することができる。
【図面の簡単な説明】
【０００９】
【図１】本実施形態に係る自律移動システムの構成例を示す図である。
【図２】本実施形態に係る自律移動システムの処理手順を示すフローチャートである。
【図３】周辺物体形状測定部による物体形状測定の説明を行うための図である（その１）。
【図４】周辺物体形状測定部による物体形状測定の説明を行うための図である（その２）。
【図５】移動体の走行軌跡の例を示す図である。
【図６】ノードとアークで構成される軌跡の例を示す図である。
【図７】本実施形態に係る各用語の定義を説明するための図である。
【図８】軌跡確定を説明するための図である。
【図９】修正された軌跡の例を示す図である。
【図１０】周辺物体形状マッチング部による対応関係と差分測定の誤差の確率分布を算出する処理の手順を示すフローチャートである。
【図１１】地理情報システムマッチング部による対応関係と差分測定の誤差の確率分布を算出する処理の手順を示すフローチャートである。
【図１２】ＧＮＳＳ測位部による対応関係と差分測定の誤差の確率分布を算出する処理の手順を示すフローチャートである。
【図１３】車輪回転量測定部による対応関係と差分測定の誤差の確率分布を算出する処理の手順を示すフローチャートである。
【図１４】本実施形態に係る自律走行システムの別の実施形態を示す図である（その１）。
【図１５】本実施形態に係る自律走行システムの別の実施形態を示す図である（その２）。
【発明を実施するための形態】
【００１０】
次に、本発明を実施するための形態（「実施形態」という）について、適宜図面を参照しながら詳細に説明する。
【００１１】
［システム構成］
図１は、本実施形態に係る自律移動システムの構成例を示す図である。
自律移動システム１は、地図作成に必要な各データを収集する車載部１０と、収集された各データを基に、軌跡補正を行う管理部２０とを有する。
車載部１０は、例えば、自律移動ロボットや乗物などの移動体装置である移動体Ｖに搭載されている。また、管理部２０は、例えば、オフィスビルなどの管理設備に設置されている。
車載部１０と、管理部２０とは、無線ネットワークなどを介して通信可能である。
【００１２】
（車載部）
車載部１０は、周辺物体形状測定部１１、周辺物体形状マッチング部１２、地理情報システムデータ取得部１３、地理情報システムマッチング部１４、ＧＮＳＳ（Global Navigation Satellite System）測位部１５、車輪回転量測定部１６、制御部１７などを有している。なお、周辺物体形状マッチング部１２、地理情報システムマッチング部１４、ＧＮＳＳ測位部１５、車輪回転量測定部１６をまとめて、測定手段と称することがある。
【００１３】
周辺物体形状測定部１１は、移動体Ｖの周辺に存在する物体（建物や街路樹や電柱などの道路構造物、人や他の移動体Ｖなど）の形状を測定する。周辺物体形状測定部１１として、例えば、レーザスキャナやステレオカメラやＴＯＦ（Time Of Flight）距離画像カメラなどを用いることができる。
【００１４】
周辺物体形状マッチング部１２は、現在位置において、移動体Ｖの周辺物体形状測定部１１が測定した周辺物体の形状データ（構造物の形状データ）と、異なる位置で測定した周辺物体の形状データをマッチング（重ね合わせ）することにより、移動体Ｖの２地点間の相対的な位置について、前時刻からの差分を算出する。マッチングには、例えば、論文「距離データ処理  −  複数距離画像からの形状モデル生成技術」（著者：増田  健，  岡谷（清水）郁子，  佐川  立昌、学会会議：Proc. of the 146th CVIM、開催年：２００４年）に記載の方法などを用いることができる。
【００１５】
地理情報システムデータ取得部１３は、都市内の道路形状や、建物や、街路樹や、電柱などの道路構造物の地図形状データを、地図データなどを有する地理情報システムから取得する部分である。地図形状データのデータ形式として、近年のカーナビゲーションシステムにおいて利用されている都市３次元モデルや、ＯＧＣ（Open Geospatial Consortium）により世界標準化されているＣｉｔｙＧＭＬ（Geography Markup Language）などを用いることができる。
【００１６】
地理情報システムマッチング部１４は、現在位置において、移動体Ｖの周辺物体形状測定部１１が測定した周辺物体の形状データと、地理情報システムデータ取得部１３が取得した現在位置付近の周辺物体の地図形状データとをマッチング（重ね合わせ）することにより、地理情報システムが有している地図に対する移動体Ｖの現在位置の相対的な位置について、前時刻からの差分を算出する。マッチングには、周辺物体形状マッチング部１２と同様の方法を用いることができる。
【００１７】
ＧＮＳＳ測位部１５は、ＧＰＳ（Global Posioning System）などの測位システムを用いて、例えば、平面直角座標系などの基準座標系に対する移動体Ｖの現在位置を算出する。
【００１８】
車輪回転量測定部１６は、車輪回転量を累積することで、前時刻における移動体Ｖの位置に対する現在位置の相対的な差分を算出する。算出には、ＩＭＵ（Inertial Measurement Unit）と呼ばれる慣性センサやジャイロセンサを用いて、論文「Gyrodometry : A New Method for Combining Data from Gyros and Odometry in Mobile Robots」（著者：Johann Borenstein, and Liqiang Feng、学会会議：Proc. of ICRA’96、開催年：１９９６年）に記載の方法などを用いることができる。
【００１９】
なお、車載部１０は、各部１１〜１６のすべてを有していなくてもよく、これらのうち、最低２つを有していればよい。
【００２０】
制御部１７は、各部１１〜１６を統括制御する。
【００２１】
各部１１〜１７は、ＲＯＭ（Read Only Memory）などに格納されたプログラムが、ＣＰＵ（Central Processing Unit）によって実行されることによって具現化する。
【００２２】
（管理部）
管理部２０は、評価関数生成部２１、軌跡最適化計算部２２、形状地図データ生成部２３を有している。
【００２３】
評価関数生成部２１は、各測定手段１１〜１６によって算出された移動体Ｖの前時刻に対する相対的な位置の差分（詳細は後記）を用いて、走行環境中を走行した際の移動体Ｖの軌跡に対して、それぞれの測定された相対的な位置の差分を変数とし、確率関数で表現した評価関数を生成する部分である。この評価関数を最適化したときの変数（つまり、相対的な位置の差）を求めることで、軌跡の誤差を訂正する。詳細は後記する。
【００２４】
軌跡最適化計算部２２は、評価関数生成部２１が生成した評価関数を、相対的な位置の差を変数として最適化する。これにより、軌跡最適化計算部２２は、累積誤差が発生している移動体Ｖの軌跡を、全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡に修正する。詳細に関しては後記する。
【００２５】
形状地図データ生成部２３は、軌跡最適化計算部２２によって修正された移動体Ｖの軌跡に対して周辺物体形状測定部１１が測定した周辺物体の形状データを貼付することで、累積誤差のない地図を生成する。
【００２６】
管理部２０は、ＰＣ（Personal Computer）などで実現され、各部２１〜２３はＲＯＭや、ＨＤＤ（Hard Disk Drive）に格納されたプログラムが、ＲＡＭ（Random Access Memory）に展開され、ＣＰＵによって実行されることによって具現化する。
【００２７】
［フローチャート］
次に、図１を参照しつつ、図２から図９に沿って本実施形態に係る自律移動システム１の具体的な処理内容について説明する。
図２は、本実施形態に係る自律移動システムの処理手順を示すフローチャートである。
自律移動システム１が図２に示すフローチャートの処理を実行することで正確な軌跡を算出し、さらにこの軌跡を基に地図を生成することで、移動体Ｖが自己位置や目標経路を見失うことなく目的地に到達することができる。
【００２８】
まず、移動体Ｖが対象となる走行環境を走行しつつ、周辺物体形状測定部１１が移動体Ｖの周辺に存在する物体（建物や街路樹や電柱などの道路構造物、人や他の移動体Ｖなど）の形状（物体形状）を測定する（Ｓ１０１）。なお、ここでの走行とは、移動体Ｖによる自律走行でもよいし、操縦者によって操縦されてもよい。
【００２９】
図３および図４を参照して、周辺物体形状測定部による物体形状測定の説明を行う。
図３における走行可能領域（道路）４１２を走行している移動体Ｖを中心とし、周辺物体形状測定部１１が、自身の測定範囲４０１に入る道路構造物４１１などの物体形状を測定する。
これは、例えば、周辺物体形状測定部１１がレーザを周囲に放射し、その反射によって周囲の物体の形状を測定する。
この際、人や他の移動体Ｖなどの移動体Ｖを地図に記憶してしまうことを避けるため、周辺物体形状測定部１１は、図４に示すように所定の高さの領域５０１の周辺物体の形状データを抽出してもよい。あるいは、周辺物体形状測定部１１は平面や円柱などの、特徴的な周辺物体の形状データを抽出するようにしてもよい。
【００３０】
そして、各測定手段１２，１４〜１６が、測定のデータを取得する（Ｓ１０２）。このとき、各測定手段１２，１４〜１６は管理部２０へ測定のデータを送信する。
次に、制御部１７が、測定のデータが所定量蓄積されたか否かを判定する（Ｓ１０３）。
ステップＳ１０３の結果、測定のデータが所定量蓄積されていない場合（Ｓ１０３→Ｎｏ）、制御部１７はステップＳ１０１へ処理を戻す。
ステップＳ１０３の結果、測定のデータが所定量蓄積されている場合（Ｓ１０３→Ｙｅｓ）、送信されたデータを基に作製される移動体Ｖが走行した軌跡に対して、評価関数生成部２１がノードとアークを生成する（Ｓ１０４）ことによって、グラフ構造を生成する。
【００３１】
ここで、図５および図６を参照して、評価関数生成部２１によるノードとアークの生成を説明する。図５において、図３と同様の要素については、同一の符号を付して説明を省略する。
走行軌跡ｔは移動体Ｖが実際に走行した際の走行軌跡の例を示す図である。
図５に示すように、移動体Ｖが実際の走行軌跡ｔに沿って走行しているものとする。走行軌跡ｔは、車輪回転量測定部１６による車輪の回転数から測定される距離を基に算出されるものである。なお、走行軌跡ｔは実際には閉じている。
【００３２】
図６は、ノードとアークの関係を説明するための図である。
図６に示すように、評価関数生成部２１は、走行軌跡ｔ（図５）を所定の長さ毎に区切り、区切った地点をノードｐとし、ノードｐ間を直線のアークｇで繋ぐことで、グラフ構造を生成し、軌跡Ｘを生成する。
この際、評価関数生成部２１は、車輪回転量測定部１６が車輪回転量を累積することで算出した移動体Ｖの位置を初期位置として、ノードｐとアークｇを生成する。
車輪回転量測定部１６の測定には累積誤差が発生するため、図６に示す評価関数生成部２１が生成したグラフ構造（軌跡Ｘ）には、図５に示す実際の環境での走行軌跡ｔとは測定誤差（ずれ）が発生してしまっている。自律移動システム１は、この測定誤差（ずれ）を修正して正確な地図を生成するために、図２に示す以降のフローチャートの処理手順（Ｓ１０５〜Ｓ１０９）を実行する。
【００３３】
ｉ番目のノードｐをノードｐｉと表現し、その位置をベクトルｘｉで表現することとする。例えば２次元の地図を生成する場合、ノードｐｉの位置であるベクトルｘｉは式（１）のように表現される。
【００３４】
ｘｉ＝（ｕｉ，ｖｉ，θｉ）  ・・・  （１）
【００３５】
ここで、ｕｉ，ｖｉは、例えば、世界座標であり、θｉは移動体Ｖの姿勢（向き）である。
このとき、ノードｐとアークｇで表される軌跡Ｘは、式（２）に示すようにｎ個のノードｐｉの位置を表すベクトルｘｉの集合として表現される。
【００３６】
Ｘ＝（ｘ１，・・・，ｘｎ）Ｔ  ・・・  （２）
【００３７】
ここで、ｎはノード数である。
【００３８】
次に、評価関数生成部２１によって、ステップＳ１０４で生成されたノードｐとアークｇから成るグラフ構造に対して、周辺物体形状マッチング部１２、地理情報システムマッチング部１４、ＧＮＳＳ測位部１５、車輪回転量測定部１６のそれぞれが、測定ｚと、ノードｐとの対応関係を算出し、取得した測定ｚと、ノードｐ間の差分測定の誤差の確率分布を算出する（Ｓ１０５）。
【００３９】
以下、図７を参照して、各測定手段による測定ｚと、ノードｐｉとの対応関係の算出、および、その確率的表現について説明する。
図７は、本実施形態に係る各用語の定義を説明するための図である。
まず、図７（ａ）に示すように、車輪回転量測定部１６が測定した位置として、ノードｐ１とノードｐ２を用いた例を示す。
ノードｐ１と、ノードｐ２とを結ぶ線をアークｇ１２と表現する。ノードｐ２は、測定誤差を含んでいるので、その測定誤差は正規分布に従うと仮定すると、真のノードｐ２は楕円ｙ１の範囲内に存在すると考えられる。ここで、ノードｐ２の測定誤差の分布（楕円分布ｙ１）を正規分布の精度行列Ωによって表現される。ここで、楕円ｙ１は、精度行列Ωが表す共分散楕円である。精度行列Ωは情報行列とも呼ばれ、正規分布の共分散行列の逆行列に相当する。具体的には、図７（ａ）における楕円ｙ１は、正規分布において、ノードｐ２を中心とし、その中心から標準偏差σの範囲を示している。
なお、小さな矢印ｑは、移動体Ｖの姿勢の向きを示すものであり、式（１）のθｉに相当する。
【００４０】
次に、図７（ｂ）を参照して、他の測定手段が測定した位置を車輪回転量測定部１６が測定したノードに対応付ける方法を示す。
図７（ｂ）に示すように、車輪回転量測定部１６以外の測定手段（例えば、ＧＮＳＳ測位部１５）がある位置を測定したとする。この位置を測定ｚ（ｍ１）（位置データ）と記載する。ここで、「ｍ１」とは、測定ｚを測定した測定手段（例えば、ＧＮＳＳ測位部１５）を示す識別番号である。つまり、測定ｚ（ｍ１）とは、測定手段ｍ１によって測定された測定ｚを意味する。
そして、測定ｚ（ｍ１）が、どのノードｐに対応付けられるのかを評価関数生成部２１が判定する。例えば、時刻などを基に、測定ｚ（ｍ１）がノードｐ２に対応すると評価関数生成部２１によって判定されたとすると、この測定ｚを測定ｚ２（ｍ１）と表現することとする。
ここで、ノードｐ１と測定ｚ２（ｍ１）とを結ぶ線を相対的な位置の差分の測定（以下、差分測定と称する）をＺ１２（ｍ１）と表現することとする。差分測定Ｚ１２（ｍ１）の成分は、測定ｚ２と、ノードｐ１との差で表現される。
【００４１】
また、測定ｚ２（ｍ１）の共分散楕円がノードｐ２と対応付けられている（言い換えれば、差分測定Ｚ１２（ｍ１）とアークｇ１２が対応付けられている）ことを対応関係ｃ１，２（ｍ１）と示すこととする。
このように、車輪回転量測定部１６によって測定されたノードｐは、その他の測定手段によって測定された測定ｚとの対応関係の初期値、つまり基準値となる。
【００４２】
測定ｚ２（ｍ１）も誤差を含んでいると考えられるので、測定ｚ２（ｍ１）は図７（ａ）と同様に精度行列Ωによる共分散楕円ｙ２を有している。
【００４３】
なお、図７（ｃ）に示すように、すべての測定ｚｉ（ｍ１）が、すべてのノードｐｉと対応付けられるわけではない。これは、各測定手段の測定間隔がまちまちであることに由来する。
図７（ｃ）のように、測定ｚ４（ｍ１）がノードｐ４と対応付けられ、ノードｐ２，ｐ３と対応付けられる測定ｚ（ｍ１）が得られなかった場合、この測定ｚ（ｍ１）は、測定ｚ４（ｍ１）となり、差分測定は、ノードｐ１に対する差分測定はＺ１４（ｍ１）となる。
【００４４】
なお、差分測定の差分元は、ユーザが任意に設定する。
例えば、図７（ｄ）に示すように、測定ｚと対応付けられているノードｐの所定個前のノードｐが差分元とされてもよい。図７（ｄ）の例では、２つ前のノードｐが差分元となっている。つまり、測定ｚ３（ｍ１）に対して生成される差分測定（Ｚ１３）の差分元は、測定ｚ３（ｍ１）と対応付けられているノードｐ３の２つ前のノードｐであるノードｐ１となっている。また、測定ｚ４（ｍ１）に対して生成される差分測定（Ｚ２４）の差分元は、測定ｚ４（ｍ１）と対応付けられているノードｐ４の２つ前のノードｐであるノードｐ２となっている。
【００４５】
差分測定の差分元の決定方法は、図７（ｄ）の方法に限らない。
例えば、図７（ｅ）に示すように、その前に測定ｚとが対応付けられたノードｐが差分元とされてもよい。図７（ｅ）に示されているように、測定ｚ１（ｍ１）とノードｐ１とが対応付けられ、測定ｚ３（ｍ１）とノードｐ３とが対応付けられ、測定ｚ４（ｍ１）とノードｐ４とが対応付けられているとする。ここで、測定ｚ３（ｍ１）に対して生成される差分測定（Ｚ１３）の差分元は、その前に測定ｚと対応付けられているノードｐ１となる。同様に、測定ｚ４（ｍ１）に対して生成される差分測定（Ｚ３４）の差分元は、その前に測定ｚと対応付けられているノードｐ３となる。そして、測定ｚ４（ｍ１）は、他の差分測定の差分元となる。
【００４６】
なお、車輪回転量測定部１６も測定手段の１つであるので、ノードｐも測定ｚとなる。例えば、図７（ａ）において、車輪回転量測定部１６の識別番号をｍ０とすると、ノードｐ１は測定ｚ１（ｍ０）となる。つまり、測定ｚ１（ｍ０）と、ノードｐ１とが対応付けられる（このとき、測定ｚ２（ｍ０）＝ノードｐ２）。この場合、測定ｚ１（ｍ０）と、ノードｐ１とが対応付けられているので、対応関係ｃ１，２（ｍ０）を定義することができる（図示せず）。
【００４７】
以上のように、各測定手段１２，１４〜１６は自身が測定した測定ｚｊ（ｍｋ）、差分測定Ｚｉｊ（ｍｋ）、対応関係ｃｉ，ｊ（ｍｋ）を算出する。ここで、ｉ，ｊ，ｋは整数であり、ｉ＜ｊである。
【００４８】
以下、差分測定の誤差の確率分布の生成について説明する。
図７で説明した定義を基に、評価関数生成部２１は軌跡Ｘを確定するための評価関数を算出し、この評価関数が最大となる各ノードｐの各位置ｘを算出することにより、最も尤もらしい軌跡Ｘｃを確定する。
図８は、軌跡確定を説明するための図である。なお、図８における軌跡は、図６における軌跡Ｘと同じものである。
図８では、ノードｐ０，ｐ１，ｐ２，・・・が算出されており、それに対応する測定ｚ０（ｍ１），ｚ１（ｍ１），ｚ２（ｍ１），ｚ５（ｍ１），・・・が算出されている（図８では（ｍ１）は省略）。ここで、差分元のノードは、対応関係が定義されたノードから３つ前のノードとしている。
なお、前記したようにノードｐ０，ｐ１，ｐ２，・・・も、それぞれ測定ｚ０（ｍ０），ｚ１（ｍ０），ｚ２（ｍ０）とすることができるが、煩雑になるのを避けるため、ここでは図示省略してある（「ｍ０」は車輪回転量測定部１６を示す識別番号）。
ここで、対応関係ｃｉ，ｊ（ｍｋ）（図８において図示せず）が定義されている条件下におけるすべてのノードｐの各位置ｘ∈Ｘの確率を確率密度関数ｐ（ｘ｜ｃｉ，ｊ（ｍｋ））で表すと、対応関係ｃｉ，ｊ（ｍｋ）は独立に生起すると見なされるため、位置ｘが生起する確率は、対応関係ｃｉ，ｊ（ｍｋ）が生起する条件におけるｘｊ（ノードｐｊの位置）が生起する確率の混合分布で表現され、その式は式（３）で示される確率密度関数ｐ（ｘ）で示される。ここで、ｘは式（２）で定義されるｘである。
【００４９】
【数１】
2012160630.tif 000003
【００５０】
ここで、＜ｉ，ｊ＞は、対応関係ｃｉ，ｊ（ｍｋ）が定義されるすべてのｉ，ｊの組み合わせの意味であり、Ｃは生じ得る対応関係ｃｉ，ｊ（ｍｋ）の集合であり、Ｍは、使用されているすべての測定手段の集合である。
式（３）が意味するところは、対応関係ｃｉ，ｊ（ｍｋ）が生じたときに軌跡Ｘが生起する確率である。
例えば、図８における測定手段の識別番号ｍ１に関する測定ｚｊ（ｍ１）を例にすると、ノードとの間で対応関係がとれているのは、｛ｚ０（ｍ１），ｚ１（ｍ１），ｚ２（ｍ１），ｚ５（ｍ１），ｚ６（ｍ１），ｚ９（ｍ１），ｚ１０（ｍ１），ｚ１３（ｍ１），ｚ１４（ｍ１）｝であるので、式（３）にて乗算される確率密度関数は、｛ｐ（ｘ｜ｃ１６，０（ｍ１）），ｐ（ｘ｜ｃ１４，１（ｍ１）），ｐ（ｘ｜ｃ１５，２（ｍ１）），ｐ（ｘ｜ｃ２，５（ｍ１）），ｐ（ｘ｜ｃ３，６（ｍ１）），ｐ（ｘ｜ｃ６，９（ｍ１）），ｐ（ｘ｜ｃ７，１０（ｍ１）），ｐ（ｘ｜ｃ１０，１３（ｍ１）），ｐ（ｘ｜ｃ１１，１４（ｍ１））｝となる。なお、図８ではｍ１に関する記載を省略している。
【００５１】
つまり、対応関係ｃｉ，ｊ（ｍｋ）を条件とした差分測定Ｚｉｊは独立に生起するため、各位置ｘが同時に生起する確率の確率密度関数は、式（３）に示すようにｐ（ｘｊ｜ｃｉ，ｊ（ｍｋ））をすべての測定手段による測定に関して乗算することで示される。
【００５２】
なお、前記したように車輪回転量測定部１６によるノードｐｊも測定ｚｊ（ｍ０）となるため、式（３）ではｐ（ｘｊ｜ｃｉ，ｊ（ｍ０））も乗算される。
【００５３】
評価関数生成部２１は、式（３）で示される確率密度関数ｐ（ｘ）を最大化することで、全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡Ｘｃを求める。ここで、軌跡Ｘは補正前の軌跡を示し、軌跡Ｘｃは補正後の軌跡を示している。つまり、式（３）で示される確率密度関数ｐ（ｘ）が最も大きいということは、各位置ｘｊが最も高い確率で生起することである。評価関数生成部２１は、このような各位置ｘを求め、求められた各位置ｘをノードとして結ぶことで全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡Ｘｃを求める。
【００５４】
以下、最尤推定法による軌跡Ｘｃの算出方法を説明する。
図７（ｂ）で説明したように、軌跡Ｘにおける各ノードｐの位置ｘが存在する確率的位置を、精度行列Ωの正規分布で表現すると、式（３）における確率密度関数ｐ（ｘ｜ｃｉ，ｊ（ｍｋ））は、以下の式（４）で示される。
【００５５】
【数２】
2012160630.tif 000004
【００５６】
ここでＮ（・）は、正規分布を表す確率密度関数である。具体的には、後記する各参考文献に記載の方法で、式（４）が生成される。この式（４）が、差分測定の誤差の確率分布となる。
【００５７】
次に、図２の説明へ戻り、周辺物体形状マッチング部１２、地理情報システムマッチング部１４、ＧＮＳＳ測位部１５、車輪回転量測定部１６のそれぞれによって生成された測定ｚとノードｐとの対応関係、および、差分測定の誤差の確率分布を基に、評価関数生成部２１が評価関数を生成する（Ｓ１０６）。
評価関数生成部２１は、評価関数Ｆ（ｘ）を以下のような手順で導出する。
まず、式（３）に式（４）を代入して正規分布の公式に従って展開すると、以下の式（５）が導かれる。
【００５８】
【数３】
2012160630.tif 000005
【００５９】
ここでηｉｊは正規化変数、ｄｉｊ（ｘ）はｉ番目のノードｐｉから見たｊ番目のノードｐｊに対応する測定ｚｊの相対的な位置の差分（差分測定Ｚｉｊ）を求めるための関数である。この関数はｘの関数となっている。また、Ωｉｊは、対応関係ｃｉ，ｊを有する測定ｚｊ（ｍｋ）における精度行列である。Ωｉｊは、後記する各参考文献に記載の技術などの手段で算出される。
式（５）に関して両辺の自然対数を求めると、式（６）が導かれる。
【００６０】
【数４】
2012160630.tif 000006
【００６１】
ここで「ｃｏｎｓｔ．」は定数である。式（３）に示した確率密度関数ｐ（ｘ）を最大化することは、式（６）に示した確率密度関数ｐ（ｘ）の自然対数（尤度関数）を最大化することと等価である。このため、式（６）から定数とマイナスの係数を除去することで、式（７）のように評価関数Ｆ（ｘ）を定式化できる。
【００６２】
【数５】
2012160630.tif 000007
【００６３】
なお、式（７）において、ｅｉｊは、以下の式（８）で示されるものである。
【００６４】
ｅｉｊ＝ｄｉｊ（ｘ）−Ｚｉｊ（ｍｋ）  ・・・  （８）
【００６５】
ここで、式（３）に示した確率密度関数ｐ（ｘ）を最大化することは、式（７）に示す評価関数Ｆ（ｘ）を最小化することと等価となる。よって、評価関数Ｆ（ｘ）を最小化することが、評価関数Ｆ（ｘ）を最適化することに相当する。
すなわち、図２のステップＳ１０６において、評価関数生成部２１は、周辺物体形状マッチング部１２、地理情報システムマッチング部１４、ＧＮＳＳ測位部１５、車輪回転量測定部１６によって取得された測定ｚｊ（ｍｋ）を基に、式（７）に示す評価関数Ｆ（ｘ）を算出する。
【００６６】
次に、制御部１７が移動体Ｖが対象とする走行環境中の走行を完了したか否か、すなわち、移動体Ｖの走行が終了したか否かを判定する（Ｓ１０７）ことで、周辺物体の形状データを収集し終えたか否かを判定する。
ステップＳ１０７の結果、走行終了していない場合（Ｓ１０７→Ｎｏ）、制御部１７は、ステップＳ１０１へ処理を戻す。
【００６７】
ステップＳ１０７の結果、走行終了している場合（Ｓ１０７→Ｙｅｓ）、軌跡最適化計算部２２が、式（７）に示される評価関数Ｆ（ｘ）を最適化することで、累積誤差が発生している移動体Ｖの軌跡Ｘを、全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡Ｘｃに修正する（Ｓ１０８）。
【００６８】
前記したように、式（３）の確率密度関数ｐ（ｘ）を最大化することは、式（７）および式（８）に示す評価関数Ｆ（ｘ）を最小化することと等価となる。すなわち、軌跡最適化計算部２２が、以下の式（９）によって、評価関数Ｆ（ｘ）を最小化するノードｐの位置ｘを求めることで、評価関数Ｆ（ｘ）を最適化を行う。これにより、軌跡最適化計算部２２は、車輪回転量測定部１６によって取得された軌跡Ｘ（図６）を、図９に示されるような全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡Ｘｃに修正する。なお、軌跡最適化計算部２２は、連立方程式解法や、非線形最適化法によって評価関数Ｆ（ｘ）を最小化する。
【００６９】
【数６】
2012160630.tif 000008
【００７０】
そして、形状地図データ生成部２３は、修正された移動体Ｖの軌跡Ｘｃに対して周辺物体形状測定部１１が測定した周辺物体の形状データを貼付することで、地図生成を行う（Ｓ１０９）。これにより、逐次的なマッチングにより自己位置を推定しながら地図を生成するのではなく、地図を生成する対象とする走行環境中における周辺物体の形状データをすべて収集した後に、測定データの全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡Ｘｃを推定し、この軌跡Ｘｃに対して測定した周辺物体の形状データを貼付することで正確な地図を生成することができる。
【００７１】
なお、本実施形態では、車輪回転量測定部１６による測定のデータからノードｐを生成したが、他の測定手段による測定のデータからノードｐを生成してもよい。つまり、他の測定手段による測定のデータを基準値としてもよい。
【００７２】
（各測位手段における対応関係および差分測定の誤差の確率分布の算出）
次に、図１０〜図１５を参照して、図２のステップＳ１０５における各測定手段の処理を説明する。
【００７３】
（周辺物体形状マッチング部）
図１０は、周辺物体形状マッチング部によるステップＳ１０５の処理の手順を示すフローチャートである。
周辺物体形状マッチング部１２は、ｉ番目のノードｐｉとｊ番目のノードｐｊに対して、ｉ番目のノードｐｉの地点で周辺物体形状測定部１１が測定した周辺物体の形状データと、ｊ番目のノードｐｊの地点で測定した周辺物体の形状データをマッチング（重ね合わせ）することにより、周辺物体形状マッチング部１２が取得した測定ｚと、ノードｐｉ，ｐｊとの対応関係を算出する。
【００７４】
周辺物体形状測定部１１は、現在対象としているｉ番目のノードｐｉから所定の閾値以内の距離にあるノードｐを探索し（Ｓ２０１）、対応するｊ番目のノードｐｊを決定する。
【００７５】
次に、周辺物体形状測定部１１は、ｉ番目のノードｐｉの地点で周辺物体形状測定部１１自身が測定した周辺物体の形状データと、ｊ番目のノードｐｊの地点で測定した周辺物体の形状データをマッチングする（Ｓ２０２）。このマッチングの結果が測定ｚとなる。マッチングには、例えば、「距離データ処理  −  複数距離画像からの形状モデル生成技術」（著者：増田  健，  岡谷（清水）郁子，  佐川  立昌、学会会議：Proc. of the 146th CVIM,開催年：２００４年）に記載の方法などを用いることができる。
【００７６】
次に、周辺物体形状測定部１１は、ステップＳ２０２でのマッチング結果を用いて、測定ｚｊにおける差分測定の誤差の確率分布（確率密度関数：式（４））を算出する（Ｓ２０３）。測定ｚｊにおける差分測定の誤差の確率分布は、例えば、論文「スキャンマッチングにおけるロボット姿勢の尤度分布計算」（著者：友納  正裕、学会会議：Ｐｒｏｃ．  ｏｆ  ＲＳＪ’１０、開催年：２０１０年）に記載の方法などを用いることができる。
【００７７】
そして、周辺物体形状測定部１１は、地図生成の対象である走行環境中のすべてのノードついて、ステップＳ２０１〜Ｓ２０３の処理が完了したか否かを判定する（Ｓ２０４）。
ステップＳ２０４の結果、完了していない場合（Ｓ２０４→Ｎｏ）、周辺物体形状測定部１１はステップＳ２０１へ処理を戻す。
ステップＳ２０４の結果、完了している場合（Ｓ２０４→Ｙｅｓ）、周辺物体形状測定部１１は処理を終了する。
【００７８】
（地理情報システムマッチング部）
図１１は、地理情報システムマッチング部によるステップＳ１０５の処理の手順を示すフローチャートである。
地理情報システムマッチング部１４は、地理情報システムデータ取得部１３が取得した地図形状データを基準として、対象とするノードｐで周辺物体形状測定部１１が測定した周辺物体の形状データをマッチング（重ね合わせ）することにより、地理情報システムが有する地図データに対するノードとの対応関係および差分測定の誤差の確率分布（確率密度関数：式（４））を算出する。
【００７９】
まず、地理情報システムマッチング部１４は、地理情報システムデータ取得部１３を介して現在対象としているノードｐｉから所定の閾値以内の距離にある地理情報システムのデータを探索する（Ｓ３０１）。
そして、地理情報システムマッチング部１４は、探索したデータを地理情報システムデータ取得部１３が取得した地図形状データを基準として、対象とするノードｐで周辺物体形状測定部１１が測定した周辺物体の形状データをマッチングする（Ｓ３０２）。このマッチングの結果が測定ｚとなる。マッチングには、図１０のステップＳ２０２の処理と同様の方法を用いることができる。
【００８０】
次に、地理情報システムマッチング部１４は、ステップＳ３０２でのマッチング結果を用いて、測定ｚとノードｐとの対応関係を算出し、差分測定の誤差の確率分布（確率密度関数：式（４））を算出する（Ｓ３０３）。差分測定の誤差の確率分布の算出は、図１０のステップＳ２０３と同様の方法を用いることができる。
【００８１】
（ＧＮＳＳ測位部）
図１２は、ＧＮＳＳ測位部によるステップＳ１０５の処理の手順を示すフローチャートである。
ＧＮＳＳ測位部１５は、測位システムを用いて、例えば、平面直角座標系などの基準座標系に対する対応関係ｃｉ，ｊを算出する。
【００８２】
まず、ＧＮＳＳ測位部１５は、現在対象としているノードｐの基準座標系に対する測定ｚを、ＧＮＳＳを用いて測位する（Ｓ４０１）。
そして、ＧＮＳＳ測位部１５は、ステップＳ４０１での測位結果(測定ｚ）を用いて、ノードｐに対する対応関係、および、測定ｚの誤差の確率分布を算出する（Ｓ４０２）。測定ｚの誤差の確率分布は、例えば、ＧＮＳＳで用いられる通信プロトコルであるＮＭＥＡ‐０１８３フォーマットのＧＳＴセンテンスの情報などを用いることができる。
【００８３】
（車輪回転量測定部）
図１３は、車輪回転量測定部によるステップＳ１０５の処理の手順を示すフローチャートである。
車輪回転量測定部１６は、車輪回転量を累積することで、移動体Ｖが１つ前の時刻にいたノードｐに対する現在位置のノードｐを測定ｚとしたときの対応関係ｃｉ，ｊを算出する。
【００８４】
まず、車輪回転量測定部１６は、移動体Ｖが１つ前の時刻にいたノードｐｉを基準として、現在位置のノードｐｊを測定ｚとし、その差分測定Ｚｉｊを、車輪回転量を累積することで測定する（Ｓ５０１）。この際に、ＩＭＵと呼ばれる慣性センサやジャイロセンサを用いて、論文「Gyrodometry : A New Method for Combining Data from Gyros and Odometry in Mobile Robots」（著者：Johann Borenstein, and Liqiang Feng、学会会議：Proc. of ICRA’96、開催年：１９９６年）に記載の方法などを用いることができる。
【００８５】
次に、車輪回転量測定部１６は、ステップＳ５０１での測定結果を用いて、ノードｐｊに対する測定ｚの対応関係ｃｉ，ｊと、差分測定Ｚｉｊの誤差の確率分布（確率密度関数：式（４））を算出する（Ｓ５０２）。測定ｚの誤差の確率密度分布の算出は、例えば、書籍「ビークル」（著者：金井  喜美雄  ほか、出版社：コロナ社、出版年：２００３年）に記載の方法などを用いることができる。
【００８６】
［別の実施形態］
【００８７】
図１４は、本実施形態に係る自律走行システムの別の実施形態を示す図である。
図１では、１台の移動体Ｖと、管理部２０とが通信を行っているが、図１４に示すように複数台の移動体Ｖ１，Ｖ２（Ｖ）に搭載された車載部１０が、管理部２０と通信を行うようにしてもよい。この場合、管理部２０は、各々の車載部１０から収集した測定のデータを基に、評価関数を生成し、軌跡Ｘｃを求める。
あるいは、各々の車載部１０が、各部１１〜１５を有していなくてもよい（ただし、車輪回転量測定部１６は、すべての車載部１０が有する必要がある）。この場合、管理部２０は、各々の車載部１０から収集した測定のデータを統合して、評価関数を生成し、軌跡Ｘｃを求めるようにしてもよい。
【００８８】
図１５は、本実施形態に係る自律走行システムの別の実施形態を示す図である。
図１５に示すように、移動体Ｖの車載部１０ａに図１の車載部１０および管理部２０の機能が設けられてもよい。
これにより、移動体Ｖａのみで軌跡の補正および地図の作成が可能となる。
【００８９】
［まとめ］
本実施形態によれば、複数の測定手段から得られたデータを基に、評価関数を生成し、その評価関数を最適化することにより、測定誤差を含む移動体Ｖの軌跡Ｘを、全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡Ｘｃを推定することができる。そして、その軌跡Ｘｃに、周辺物体の形状データを貼付することで、誤差が累積しない正確な地図の生成が可能となる。
これにより、このような地図を基に、移動体Ｖが自律移動することで、移動体Ｖは自己位置や目標経路を見失うことなく目的地に到達することができる。
【００９０】
つまり、本実施形態によれば、逐次的なマッチングにより自己位置を推定しながら地図を生成するのではなく、地図を生成する対象とする走行環境中の形状データをすべて収集した後に、測定データの全体にわたって累積誤差がなく整合性を保った移動体の軌跡を推定し、当該軌跡に対して測定した周辺物体の形状データを貼付することで正確な地図を生成し、自己位置や目標経路を見失うことなく目的地に到達する自律移動システムを実現する。
【符号の説明】
【００９１】
１      自律移動システム
１０，１０ａ  車載部
１１    周辺物体形状測定部
１２    周辺物体形状マッチング部
１３    地理情報システムデータ取得部
１４    地理情報システムマッチング部
１５    ＧＮＳＳ測位部
１６    車輪回転量測定部
１７    制御部
２０    管理部（軌跡補正装置）
２１    評価関数生成部
２２    軌跡最適化計算部
２３    形状地図データ生成部
Ｖ，Ｖ１，Ｖ２，Ｖａ  移動体（移動体装置）
【図１】
2012160630.tif 000009
【図２】
2012160630.tif 000010
【図３】
2012160630.tif 000011
【図４】
2012160630.tif 000012
【図５】
2012160630.tif 000013
【図６】
2012160630.tif 000014
【図７】
2012160630.tif 000015
【図８】
2012160630.tif 000016
【図９】
2012160630.tif 000017
【図１０】
2012160630.tif 000018
【図１１】
2012160630.tif 000019
【図１２】
2012160630.tif 000020
【図１３】
2012160630.tif 000021
【図１４】
2012160630.tif 000022
【図１５】
2012160630.tif 000023
【国際調査報告】
2012160630.tif 000024
2012160630.tif 000025
2012160630.tif 000026
2012160630.tif 000027