JP2019061603A - 情報処理装置、移動装置、および方法、並びにプログラム - Google Patents

Abstract

【課題】自己位置を算出する複数の自己位置算出器の算出自己位置に基づいて最終的に１つの装置位置情報を取得可能とした構成を実現する。【解決手段】自己位置を算出する複数の自己位置算出器と、複数の自己位置算出器の算出自己位置を統合して１つの最終自己位置を算出する自己位置統合部を有する。自己位置統合部は、複数の自己位置算出器対応の算出自己位置を、各算出器のセンサ位置を考慮して標準自己位置に変換し、複数の標準自己位置から最終自己位置を算出する。自己位置統合部は、移動装置の外部環境、複数の自己位置算出器の利用センサの故障情報、リソースの利用状況等の環境情報に応じて、最終自己位置を算出する。【選択図】図１３

Description

本開示は、情報処理装置、移動装置、および方法、並びにプログラムに関する。さらに詳細には、複数のセンサの検出情報を利用した移動体の移動処理を実現させる情報処理装置、移動装置、および方法、並びにプログラムに関する。

近年、例えば自動運転車やロボット等、自律型移動装置の開発が盛んに行われている。
このような自動運転車やロボット等の移動装置が所定経路（パス）に従って移動するためには、自装置の位置や姿勢を正確に把握することが必要である。

自装置の位置や姿勢を算出する機器、いわゆる自己位置算出器には様々な種類がある。
例えば、ＧＰＳと、ＩＭＵ（慣性計測装置：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）を組み合わせた構成や、カメラ撮影画像の特徴点情報から自己位置算出を行うＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）を利用した構成が知られている。

これらの自己位置算出器は、それぞれ異なるアルゴリズムを適用して、自己位置を算出、あるいは自己位置と姿勢を算出する。
しかし、これらの様々な種類の自己位置算出器は、環境によって精度が大きく変化してしまうという問題がある。
例えば、ＳＬＡＭは、カメラ撮影画像を利用した処理を行うため、夜間や激しい雨の中のように明瞭な画像を撮影しにくい環境では、算出する位置精度が低下してしまう。

また、例えば高層ビルが多い環境等、ＧＰＳ衛星からのデータの届きにくい環境では、ＧＰＳ利用システムによって算出される位置精度が低下する。
さらに、例えば、自己位置算出器を構成するセンサの故障が発生すると、そのセンサに依存した自己位置算出器は正常に機能しなくなる。

なお、自己位置算出器を利用して位置を確認しながら移動する移動体の構成を開示した従来技術として、例えば特許文献１（特開２０１４−１９１６８９号公報）がある。
この特許文献１は、利用対象が特定の移動体に限定されることのない汎用性の高いユニット化した位置検出装置を開示している。
しかし、このようなユニット化した位置検出装置であっても、１つの位置検出アルゴリズムを適用している以上、環境に応じて検出精度が大きく変化してしまうという問題がある。

特開２０１４−１９１６８９号公報

本開示は、例えば上記問題点に鑑みてなされたものであり、様々な環境の変化が発生した場合でも、高精度な自己位置算出を可能とした情報処理装置、移動装置、および方法、並びにプログラムを提供することを目的とする。

本開示の第１の側面は、
自己位置を算出する複数の自己位置算出器と、
前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出する自己位置統合部を有し、
前記自己位置統合部は、
前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する情報処理装置にある。

さらに、本開示の第２の側面は、
自己位置を算出する複数の自己位置算出器と、
前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出する自己位置統合部と、
自己位置統合部の算出した最終自己位置を利用して移動装置の行動を決定する計画部と、
前記計画部の決定した行動に従って移動装置の動作を制御する動作制御部を有し、
前記自己位置統合部は、
前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する移動装置にある。

さらに、本開示の第３の側面は、
情報処理装置において実行する情報処理方法であり、
複数の自己位置算出器の各々が、自己位置を算出する複数の自己位置算出ステップと、
自己位置統合部が、前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出する自己位置統合ステップを有し、
前記自己位置統合ステップは、
前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出するステップである情報処理方法にある。

さらに、本開示の第４の側面は、
移動装置において実行する移動装置制御方法であり、
複数の自己位置算出器の各々が、自己位置を算出する複数の自己位置算出ステップと、
自己位置統合部が、前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出する自己位置統合ステップと、
計画部が、自己位置統合部の算出した最終自己位置を利用して移動装置の行動を決定する計画ステップと、
動作制御部が、前記計画部の決定した行動に従って移動装置の動作を制御する動作制御ステップを有し、
前記自己位置統合ステップは、
前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する移動装置制御方法にある。

さらに、本開示の第５の側面は、
情報処理装置において情報処理を実行させるプログラムであり、
複数の自己位置算出器の各々に、自己位置を算出させる複数の自己位置算出ステップと、
自己位置統合部に、前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出させる自己位置統合ステップを実行させ、
前記自己位置統合ステップにおいて、
前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する処理を実行させるプログラムにある。

さらに、本開示の第６の側面は、
移動装置において移動装置制御処理を実行させるプログラムであり、
複数の自己位置算出器の各々に、自己位置を算出させる複数の自己位置算出ステップと、
自己位置統合部に、前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出させる自己位置統合ステップと、
計画部に、自己位置統合部の算出した最終自己位置を利用して移動装置の行動を決定させる計画ステップと、
動作制御部に、前記計画部の決定した行動に従って移動装置の動作を制御させる動作制御ステップを実行させ、
前記自己位置統合ステップにおいて、
前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する処理を実行させるプログラムにある。

なお、本開示のプログラムは、例えば、様々なプログラム・コードを実行可能な情報処理装置やコンピュータ・システムに対して、コンピュータ可読な形式で提供する記憶媒体、通信媒体によって提供可能なプログラムである。このようなプログラムをコンピュータ可読な形式で提供することにより、情報処理装置やコンピュータ・システム上でプログラムに応じた処理が実現される。

本開示のさらに他の目的、特徴や利点は、後述する本開示の実施例や添付する図面に基づくより詳細な説明によって明らかになるであろう。なお、本明細書においてシステムとは、複数の装置の論理的集合構成であり、各構成の装置が同一筐体内にあるものには限らない。

本開示の一実施例の構成によれば、自己位置を算出する複数の自己位置算出器の算出自己位置に基づいて最終的に１つの装置位置情報を取得可能とした構成が実現される。
具体的には、例えば、自己位置を算出する複数の自己位置算出器と、複数の自己位置算出器の算出自己位置を統合して１つの最終自己位置を算出する自己位置統合部を有する。自己位置統合部は、複数の自己位置算出器対応の算出自己位置を、各算出器のセンサ位置を考慮して標準自己位置に変換し、複数の標準自己位置から最終自己位置を算出する。自己位置統合部は、移動装置の外部環境、複数の自己位置算出器の利用センサの故障情報、リソースの利用状況等の環境情報に応じて、最終自己位置を算出する。
本構成により、自己位置を算出する複数の自己位置算出器の算出自己位置に基づいて最終的に１つの装置位置情報を取得可能とした構成が実現される。
なお、本明細書に記載された効果はあくまで例示であって限定されるものではなく、また付加的な効果があってもよい。

移動装置の自己位置を算出する処理に利用する自己位置算出器と座標系について説明する図である。 移動装置に対する複数の自己位置算出器の装着例について説明する図である。 相対位置ツリーの一例について説明する図である。 相対位置ツリーを利用した処理を行う装置の一構成例を示す図である。 相対位置ツリーを利用した処理を行う装置の一構成例を示す図である。 相対位置ツリー適用構成において、複数の異なるアルゴリズムによる自己位置算出器を利用する場合の問題点について説明する図である。 本開示の処理において利用する相対位置ツリーの構成例を示す図である。 最下位ノードとして追加した自己位置算出器原点ノードの意味について説明する図である。 リンクに対応する相対位置情報の具体例について説明する図である。 相対位置ツリー更新処理の具体例について説明する図である。 本開示の処理を適用した相対位置ツリー更新処理の一般的な例について説明する図である。 相対位置ツリーの自己位置原点と装置原点の２ノード間のデータ更新処理について説明する図である。 自己位置統合部の実行する処理について説明する図である。 自己位置算出器Ｐに対応する標準自己位置Ｐの算出例について説明する図である。 自己位置算出器Ｐに対応する標準自己位置Ｐの算出例について説明する図である。 自己位置算出器Ｐに対応する標準自己位置Ｐの算出例について説明する図である。 複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を選択してツリー更新に適用する標準自己位置を決定する処理について説明する図である。 複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を生成する処理について説明する図である。 移動装置の実行する処理のシーケンスについて説明するフローチャートを示す図である。 移動装置の実行する処理のシーケンスについて説明するフローチャートを示す図である。 移動装置に装着可能な移動体制御システムの一例である車両制御システムの構成例について説明する図である。 情報処理装置のハードウェア構成例について説明する図である。

以下、図面を参照しながら本開示の情報処理装置、移動装置、および方法、並びにプログラムの詳細について説明する。なお、説明は以下の項目に従って行なう。
１．自己位置算出処理に利用する自己位置算出器と座標系について
２．相対位置ツリーについて
３．複数の異なる自己位置算出器を利用して様々な環境下で高精度な自己位置算出を可能とした構成について
４．移動装置の実行する処理のシーケンスについて
５．移動装置の構成例について
６．情報処理装置の構成例について
７．本開示の構成のまとめ

［１．自己位置算出処理に利用する自己位置算出器と座標系について］
まず、図１以下を参照して、本開示の処理、すなわち移動装置の自己位置を算出する処理に利用する自己位置算出器と座標系について説明する。

図１には地図を示している。地図の中央部には、予め規定された移動経路（パス）に沿って移動する移動装置１０を示している。
移動装置１０は、図１に示す起点Ｓから終点Ｅまで、予め規定された移動経路（パス）に沿って移動する。

なお、以下の実施例では、移動装置１０の一例として、移動装置１０が自動車（車両）である例を説明するが、本開示の処理は、自動車以外の様々な移動装置において利用可能である。
例えば、ロボット（歩行型、走行型）や、ドローン等の飛行体、あるいは船舶、潜水艦等の水上、水中を移動する装置等、様々な移動装置に適用可能である。

移動装置１０は、複数の異なる構成を持つ自己位置算出器を備えている。具体的には、例えば、以下の構成を持つ自己位置算出器である。
（１）ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）、あるいはＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）からの受信信号と、ＩＭＵ（慣性計測装置：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）を組み合わせた自己位置算出器。
（２）カメラ撮影画像に基づいて自己位置推定を行うＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）を利用した自己位置算出器。

（３）車輪回転数およびステアリング角から自己位置推定を行うオドメトリ（車輪オドメトリ）を適用した自己位置算出器。
（４）パルス状のレーザ光を用いて周囲情報を取得するライダー（ＬｉＤＡＲ：Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ，Ｌａｓｅｒ Ｉｍａｇｉｎｇ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）やソナーの観測結果と高精度３次元地図とのマッチングにより自己位置を推定するＮＤＴ（Ｎｏｒｍａｌ Ｄｉｓｔｒｉｂｕｔｉｏｎｓ Ｔｒａｎｓｆｏｒｍ）を用いた自己位置算出器。

上記（１）〜（４）の自己位置算出器は、それぞれ異なるアルゴリズムにより自己位置を推定する機器である。
なお、上述した（１）〜（４）の自己位置算出器は代表的な自己位置算出器の例を説明したものであり、本開示の処理では、これら（１）〜（４）の機器に限らず、その他の様々な自己位置算出器を利用することが可能である。
図１に示す移動装置１０は、例えば、これら（１）〜（４）の自己位置算出器、あるいはその他の自己位置算出器の少なくとも２つ以上の異なる自己位置算出器を備えている。
なお、自己位置算出器による算出情報は、移動装置１０の位置情報、または位置情報と姿勢情報の組み合わせのいずれかである。

また、ＳＬＡＭ等のようにカメラ撮影画像による位置推定を行う場合、利用するカメラは、一般的な可視光カメラの他、ＴｏＦ（Ｔｉｍｅ Ｏｆ Ｆｌｉｇｈｔ）カメラ、ステレオカメラ、単眼カメラ、赤外線カメラ等が利用可能である。

本開示の処理を適用した自己位置算出処理では、複数の座標系と相対位置ツリーを利用した処理を行う。
図１に示す地図には、以下の３つの座標系を示している。
（１）地図座標系
（２）自己位置座標系
（３）装置座標系
以下、これらの座標系について説明する。

（１）地図座標系
地図座標系は、地図上に設定した点を原点（地図原点）とした座標系である。
図１に示す地図原点２１が地図座標の原点（Ｘａ，Ｙａ，Ｚａ）＝（０，０，０）である。
地図原点２１から右方向に向かう軸が地図座標系のＸ軸であり、Ｘａ軸として示している。
地図原点２１から上方向に向かう軸が地図座標系のＹ軸であり、Ｙａ軸として示している。
なお、図には、Ｘ軸、Ｙ軸のみを示しているが、Ｚ軸も存在し、Ｚ軸は、紙面に垂直に上向きに設定される。
このように、地図座標系は、地図に設定された固定点を地図原点とした座標系である。

（２）自己位置座標系
自己位置座標系は、移動装置１０の移動経路の一点、例えば図に示す起点Ｓを原点（自己位置原点）とした座標系である。
図１に示す自己位置原点２２が自己位置座標の原点（Ｘｂ，Ｙｂ，Ｚｂ）＝（０，０，０）である。
自己位置原点２２から右方向に向かう軸が自己位置座標系のＸ軸であり、Ｘｂ軸として示している。
自己位置原点２２から上方向に向かう軸が自己位置座標系のＹ軸であり、Ｙｂ軸として示している。
なお、図には、Ｘ軸、Ｙ軸のみを示しているが、Ｚ軸も存在し、Ｚ軸は、紙面に垂直に上向きに設定される。
このように、自己位置座標系は、移動装置１０の移動経路の一点、例えば図に示す起点Ｓを原点（自己位置原点）とした座標系である。

（３）装置座標系
装置座標系は、移動装置１０内の一点、例えば図に示す移動装置１０に示す装置原点２３を原点とした座標系である。
図１に示す装置原点２３が装置座標の原点（Ｘｃ，Ｙｃ，Ｚｃ）＝（０，０，０）である。
装置原点２３から右方向に向かう軸が装置座標系のＸ軸であり、Ｘｃ軸として示している。
装置原点２３から上方向に向かう軸が装置座標系のＹ軸であり、Ｙｃ軸として示している。
なお、図には、Ｘ軸、Ｙ軸のみを示しているが、Ｚ軸も存在し、Ｚ軸は、紙面に垂直に上向きに設定される。
このように、装置座標系は、移動装置１０内の一点を原点（装置原点）とした座標系である。

本開示の自己位置算出処理では、例えば、これら３種類の座標系を利用した処理を行う。
次に、図２を参照して、移動装置１０に対する複数の自己位置算出器の装着例について説明する。

図２に示すように、移動装置１０には、複数の自己位置算出器が装着される。
図２に示す例は、以下の３つの自己位置算出器を装着した例である。
自己位置算出器Ｐ３１、
自己位置算出器Ｑ３２、
自己位置算出器Ｒ３３、
これらの３つの自己位置算出器が移動装置１０の異なる場所に装着されている。

自己位置算出器Ｐ３１は、例えば、カメラ撮影画像に基づいて自己位置推定を行うＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）を利用した自己位置算出器である。

自己位置算出器Ｑ３２は、例えば、車輪回転数およびステアリング角から自己位置推定を行うオドメトリ（車輪オドメトリ）を適用した自己位置算出器である。

自己位置算出器Ｒ３３、例えば、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）、あるいはＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）からの受信信号と、ＩＭＵ（慣性計測装置：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）を組み合わせた自己位置算出器である。

これら３つの自己位置算出器は、それぞれのセンサの装着場所の位置を算出する。
しかし、これら３つの自己位置算出器の移動装置１０に対する装着位置はそれぞれ異なる位置である。
装置座標系（Ｘｃ，Ｙｃ，Ｚｃ）における各自己位置算出器の装着位置は、以下の通りである。
自己位置算出器Ｐ３１の装着位置は、（Ｘｃ，Ｙｃ，Ｚｃ）＝（Ｐｘ，Ｐｙ，Ｐｚ）である。
自己位置算出器Ｑ３２の装着位置は、（Ｘｃ，Ｙｃ，Ｚｃ）＝（Ｑｘ，Ｑｙ，Ｑｚ）である。
自己位置算出器Ｒ３３の装着位置は、（Ｘｃ，Ｙｃ，Ｚｃ）＝（Ｒｘ，Ｒｙ，Ｒｚ）である。

従って、これら３つの自己位置算出器の算出する位置情報には、各算出器の装着位置に応じたずれが発生する。さらに、それぞれの自己位置算出器の実行する位置算出アルゴリズムも異なっているため、その算出アルゴリズムの差異に基づく差分も発生することになる。
従って、複数の異なる自己位置算出器の算出する位置情報を利用して、移動装置１０の１つの最終的な位置情報を算出するためには、複数の異なる自己位置算出器の算出位置情報を統合する処理を行うことが必要となる。

［２．相対位置ツリーについて］
本開示の処理では、複数の異なる自己位置算出器の算出位置情報を統合する処理を行うために、複数の異なる座標系の関係や、座標原点とオブジェクトの位置関係等を定義した相対位置ツリーを用いる。
以下、この相対位置ツリーについて説明する。

先に図１を参照して説明した移動装置１０の位置を算出するためには、複数の相対位置関係を管理する必要がある。例えば、
図１を参照して説明した地図原点２１と、装置原点２３の相対位置、
装置原点２３と、図２を参照して説明した自己位置算出器またはその利用センサの相対位置、さらに、
移動装置１０やセンサと、移動装置１０の障害物となり得る人、標識、交通信号などと相対位置、
これら様々な異なる座標系の相対位置関係や、座標原点とオブジェクトの相対位置関係を把握することが必要となる。

相対位置関係は、例えば２つの座標系または物体（オブジェクト）の相対的な位置（または位置と姿勢）の関係である。
なお、以下において、相対位置関係は相対位置とも言う。

相対位置関係または相対位置の一例は、例えば１つの座標系の原点位置と、実在する物体（オブジェクト）の三次元位置と姿勢との対応関係情報である。
なお、１つの座標系の原点を基準とした相対位置関係と、その逆関係、すなわち座標系の原点ではない方を基準とした相対位置とは、相互変換可能であり、ある１つの相対位置を取得することと、その相対位置の逆関係を取得することは同義である。

複数の異なる相対位置の組み合わせを取得することで、その相対位置の組み合わせに基づいて、新たな相対位置を取得することが可能である。
例えば、
（ａ）装置原点と自己位置算出器（センサ）との相対位置、
（ｂ）自己位置算出器（センサ）と人との相対位置、
これらの２種類の相対位置を取得できれば、
（ｃ）装置原点と人の相対位置、
を算出することができる。

また、同じ相対位置を違う複数の相対位置の組み合わせで取得することも可能である。
例えば、
（Ｐａ）地図原点と自己位置算出器Ｐ（カメラセンサ）との相対位置、
（Ｐｂ）装置原点と自己位置算出器Ｐ（カメラセンサ）との相対位置、
これらの２つの異なる相対位置に基づいて、
（Ｐｃ）地図原点と装置原点との相対位置、
を算出することができる。

また、
（Ｒａ）地図原点と自己位置算出器Ｒ（ＧＰＳアンテナ）との相対位置、
（Ｒｂ）装置原点と自己位置算出器Ｒ（ＧＰＳアンテナ）との相対位置、
これらの２つの異なる相対位置に基づいて、
（Ｒｃ）地図原点と装置原点との相対位置、
を算出することができる。

しかし、上記の
自己位置算出器Ｐ（カメラセンサ）を用いて算出した「（Ｐｃ）地図原点と装置原点との相対位置」と、
自己位置算出器Ｒ（ＧＰＳアンテナ）を用いて算出した「（Ｒｃ）地図原点と装置原点との相対位置」、
これら２つの相対位置は、本来、同じ相対位置でなければならないが、各自己位置算出器の位置算出アルゴリズムや、センサ装着位置の違い等に起因して異なる値となる場合がある。

このように、利用する自己位置算出器によって異なる相対位置が算出されてしまうと、利用した自己位置算出器によって算出される移動装置１０の自己位置が異なるものになってしまうという問題が発生する。
このような問題を解決するために、「相対位置ツリー」が利用される。

図３を参照して相対位置ツリーの一例について説明する。
図３（１）に示すように、相対位置ツリーはノードをリンクで接続したツリー構造を有する。
相対位置ツリーは、例えば自律型移動を行う移動装置の記憶部に格納される。
ノードを接続するリンクは、そのリンクによって接続された２つのノード間の相対位置情報を記録情報として保持することを意味する。すなわち、例えばリンク接続されたツリー上部の親ノードに対するツリー下部の子ノードの相対位置が記録情報として記憶部に格納されている。

図３（１）は、以下の２つの相対位置をツリー構造に設定した相対位置ツリーである。
（ａ）地図原点と信号との相対位置、
（ｂ）地図原点と装置原点との相対位置、
例えば、図１に示す地図原点２１、信号１２、装置原点２３との間の相対位置を設定した相対位置ツリーである。

図３（１）に示す相対位置ツリーのリンク（ａ）は、地図原点２１と信号１２との相対位置情報が、この相対位置ツリーの記録情報に含まれていること、すなわち相対位置ツリーを格納した記憶部に格納されていることを意味し、様々なモジュール、例えば移動装置の経路決定モジュール等が、様々なタイミングにおいて記憶部から取得可能であることを意味する。

なお、この（ａ）の相対位置情報は、具体的には、例えば、地図原点２１の位置を示す３次元座標情報と、信号１２の位置を示す３次元座標情報と信号１２の姿勢情報（３軸姿勢情報）との対応データによって構成される。
なお、地図原点２１の位置を示す３次元座標情報と、信号１２の位置を示す３次元座標情報は、同じ座標系、例えば地図座標系を用いた情報である。

また、リンク（ｂ）は、地図原点と装置原点との相対位置情報が記録情報として含まれ、取得可能であることを意味する。
このリンク（ｂ）の相対位置情報は、具体的には、例えば、地図原点２１の位置を示す３次元座標情報と、装置原点２３の位置を示す３次元座標情報との対応データによって構成される。
なお、地図原点２１の位置を示す３次元座標情報と、装置原点２３の位置を示す３次元座標情報は、同じ座標系、例えば地図座標系を用いた情報である。

図３（２）は、図３（１）に示す相対位置ツリーを用いた一つの処理例を示す図である。
（ａ）地図原点と信号との相対位置、
（ｂ）地図原点と装置原点との相対位置、
これら（ａ），（ｂ）２つの相対位置が規定された相対位置ツリーを利用することで、
（ｃ）装置原点と信号との相対位置、
を算出することができる。
なお、相対位置ツリー構造はオープンソースのロボティクスフレームワークであるＲＯＳ（Ｒｏｂｏｔ Ｏｐｅｒａｔｉｎｇ Ｓｙｓｔｅｍ）などで採用されている。

相対位置ツリーの格納情報、すなわち、例えば、ある座標系の原点とあるオブジェクトとの相対位置は、逐次、変化するため、逐次、更新する必要がある。例えば移動装置１０の移動に伴い、移動装置１０に装着された自己位置算出器（センサ）と地図原点との相対位置は、逐次、変化し、更新の必要がある。

相対位置ツリーを利用した処理、具体的には自己位置算出処理等の処理を、相対位置ツリーを利用して行う場合、
相対位置ツリーの更新処理を実行するモジュール、すなわち、相対位置ツリー更新モジュールが必要となる。

図４は、相対位置ツリーを利用した処理を行う装置の一構成例を示す図である。
図４に示す装置は以下の構成要素を有する。
相対位置ツリーの更新処理を実行する相対位置ツリー更新モジュール４１，４２、
相対位置ツリーを格納した記憶部４３、
記憶部４３に格納された相対位置ツリーを利用して様々な相対位置情報を取得する相対位置利用モジュール４４〜４６。

相対位置ツリー更新モジュール４１，４２は、例えば、地図情報の解析を行うマップ解析部や、自己位置算出器等によって構成される。

相対位置ツリー更新モジュール１（マップ解析部）４１は、地図から得られる情報、例えば信号の位置情報等に基づいて、地図原点と信号との相対位置を取得して、記憶部４３に格納された相対位置ツリーの更新処理を行う。

また、相対位置ツリー更新モジュール２（自己位置算出器）４２は、自己位置算出器の算出した自己位置情報等に基づいて、地図原点と装置原点との相対位置を取得して、記憶部４３に格納された相対位置ツリーの更新処理を行う。

これらの相対位置ツリー更新モジュールのツリー更新処理により、記憶部４３に格納された相対位置ツリーは、常に最新の情報に更新される。
記憶部４３に格納された相対位置ツリーは、様々な相対位置利用モジュール４４〜４６によって読み出され、各座標系の原点と物（オブジェクト）間の相対位置や、移動装置と、障害物間の相対位置情報等が取得され、利用される。相対位置情報の利用態様は、例えば、先に図３（２）を参照して説明した処理等である。

相対位置利用モジュール４４〜４６は、例えば、移動装置１０の進行経路を決定するルート計画部、行動計画部、自動動作計画部、運転制御部等である。具体的には、例えば、相対位置の算出対象である障害物を避けた安全な進行経路（パス）を決定する処理を行うモジュール等によって構成される。

図４に示す構成において説明したように、相対位置ツリー更新モジュールとして、自己位置算出器が利用される。
自己位置算出器は、先に説明したように、様々な種類がある。すなわち、例えば以下のような機器である。
（１）ＧＰＳ、あるいはＧＮＳＳと、ＩＭＵを組み合わせた自己位置算出器、
（２）ＳＬＡＭを利用した自己位置算出器。
（３）オドメトリ（車輪オドメトリ）を適用した自己位置算出器。
（４）ライダー（ＬｉＤＡＲ）やソナーを用いた自己位置算出器。

しかし、これらは、環境によって精度が大きく変化してしまうという問題がある。
例えば、ＳＬＡＭは、カメラ撮影画像を適用した処理を行うため、夜や激しい雨の中のような環境等、周囲の明瞭な画像を撮影しにくい環境では、算出する位置精度が低下してしまう。
また、例えば高層ビルが多い環境等、ＧＰＳ衛星からのデータの届きにくい環境では、ＧＰＳ利用システムによって算出される位置精度が低下する。

このように、自己位置算出器は環境の変化や違いによって可用性と性能が変わる。すべての環境で高精度な位置情報を算出可能な自己位置算出器は存在しない。
また、センサの故障によって、そのセンサに依存した自己位置算出器は正常に機能しなくなる。

１つの装置、例えば移動装置１０に、複数の異なる自己位置算出器を装着することで、様々な環境において高精度な位置情報を取得可能な構成、すなわちロバスト性の高い構成を実現することができる。

しかし、複数の異なる自己位置算出器を用いて、記憶部に格納された相対位置ツリーを更新する処理を行おうとすると、複数の異なる自己位置算出器の各々が、相対位置ツリー上の同一ノード間の相対位置として、異なる相対位置情報を出力する競合が発生し、相対位置ツリーの正常な更新処理ができなくなる場合がある。
図５を参照して、この問題について説明する。

図５は、図４と同様、相対位置ツリーを利用した処理を行う装置の一構成例を示す図である。
図５に示す装置は、図４と同様、以下の構成要素を有する。
相対位置ツリーの更新処理を実行する相対位置ツリー更新モジュール４７，４８、
相対位置ツリーを格納した記憶部４３、
記憶部４３に格納された相対位置ツリーを利用して様々な相対位置情報を取得する相対位置利用モジュール４４〜４６、

図５に示す構成において、相対位置ツリー更新モジュール４７，４８の各々は、異なるアルゴリズムＰ，Ｑによって自己位置算出を行う２つの自己位置算出器Ｐ，Ｑによって構成される。
その他の構成は、図４を参照して説明した構成と同様の構成である。

図５に示す構成において、相対位置ツリー更新モジュールＰ（自己位置算出器Ｐ）４７は、アルゴリズムＰを利用した位置算出を行う自己位置算出器であり、算出した位置情報に基づいて、地図原点と自己位置原点と装置原点との相対位置を取得して、記憶部４３に格納された相対位置ツリーの更新処理を行うための更新情報を生成する。
生成する更新情報は、
ツリー構成情報Ｐ＝地図原点と自己位置原点と装置原点との相対位置
である。

一方、相対位置ツリー更新モジュールＱ（自己位置算出器Ｑ）４８は、アルゴリズムＱを利用した位置算出を行う自己位置算出器であり、算出した位置情報に基づいて、地図原点と自己位置原点と装置原点との相対位置を取得して、記憶部４３に格納された相対位置ツリーの更新処理を行うための更新情報を生成する。
生成する更新情報は、
ツリー構成情報Ｑ＝地図原点と自己位置原点と装置原点との相対位置
である。

ここで、相対位置ツリー更新モジュールＰ（自己位置算出器Ｐ）４７の生成した更新情報、すなわち、
ツリー構成情報Ｐ＝地図原点と自己位置原点と装置原点との相対位置
と、
相対位置ツリー更新モジュールＱ（自己位置算出器Ｑ）４８の生成した更新情報、すなわち、
ツリー構成情報Ｑ＝地図原点と自己位置原点と装置原点との相対位置
これら２つの更新情報は相対位置ツリーの同一ノード間の相対位置情報である。
すなわち、２つの相対位置ツリー更新モジュールが同じ更新情報を生成する更新情報の競合が発生する。

これら２つの更新情報が一致し、全く同一のデータから構成されていれば、記憶部４３に格納された相対位置ツリーをその共通の更新情報で更新することができる。
しかし、２つの相対位置ツリー更新モジュールＰ（自己位置算出器Ｐ）４７と相対位置ツリー更新モジュールＱ（自己位置算出器Ｑ）４８は、それぞれ異なるアルゴリズムを適用した位置情報算出処理を行うモジュールであり、さらに、位置算出用のセンサ取り付け位置も異なっている。
従って、これら２つのモジュールが算出した情報は一致せず、差が発生する場合がある。

このような場合、どちらか一方の自己位置算出器の算出情報を適用して記憶部４３に格納された相対位置ツリーを更新してしまうと、他方の自己位置算出器の算出した位置情報との不整合が発生する。
このような不整合が発生すると、相対位置利用モジュールにおける相対位置を利用した処理にも実際の相対位置と誤差が生じ、移動装置の自己位置の認識が正しく行われない可能性がある。

このように、複数の異なる自己位置算出器を相対位置ツリー更新モジュールとして利用すると、それぞれの算出器の算出値にずれが発生するという問題が生じる。
そのため、相対位置ツリーを適用する構成では、複数の異なるアルゴリズムによる自己位置算出器を利用する構成を適用することが困難となるという問題がある。
なお、異なるアルゴリズムを適用した複数の自己位置算出器を利用した構成に限らず、同一のアルゴリズムを適用した複数の自己位置算出器を利用した場合であっても、各自己位置算出器の取り付け位置の違いや、各自己位置算出器の測定精度の違い、測定誤差等により、それぞれの算出器の算出値にずれが発生するという問題が生じる。

［３．複数の異なる自己位置算出器を利用して様々な環境下で高精度な自己位置算出を可能とした構成について］
次に、上述した問題を解決した構成、すなわち、相対位置ツリーを適用した構成において、複数の異なる自己位置算出器を利用して様々な環境下で高精度な自己位置算出を可能とした構成について説明する

まず、図６を参照して、相対位置ツリー適用構成において、複数の自己位置算出器を利用する場合の問題点について整理して説明する。
なお、以下の実施例では、複数の異なるアルゴリズムを適用した自己位置算出器を利用した構成について説明するが、本開示の処理は、複数の異なるアルゴリズムを適用した自己位置算出器を利用する構成に限らず、同じアルゴリズムを適用した複数の自己位置算出器を利用する構成にも適用可能である。

図６の中央に示すツリー、すなわち、地図原点５１、自己位置原点５２、装置原点５３、カメラ５４、車輪中心５５、これらの５つのノードから構成されるツリー構成が、移動装置内の記憶部に格納されている相対位置ツリーであるとする。
この相対位置ツリーにおいてノード間に接続リンクが設定されている場合、そのノード間の相対位置情報が記憶部に格納されている。

相対位置情報は、例えば移動装置の移動等に伴い、逐次、更新することが必要となる。
図６に示す構成では、以下の２つの相対位置更新モジュールを示している。
相対位置ツリー更新モジュールＰ（自己位置算出器Ｐ）５６、
相対位置ツリー更新モジュールＱ（自己位置算出器Ｑ）５７、
これら２つのモジュールである。

相対位置ツリー更新モジュールＰ（自己位置算出器Ｐ）５６は、例えばＳＬＡＭによって構成される相対位置算出器であり、相対位置ツリーの最下位ノードとして設定されているカメラ５４の撮影画像に基づいて自己位置を算出する。
算出した自己位置に基づいて、相対位置ツリーの更新情報、すなわち、図６に示すツリー構成情報Ｐを生成して、相対位置ツリーの一部のリンクに相当する相対位置情報の更新処理を行おうとする。
具体的には、図６に示すように、ツリー構成情報Ｐは、自己位置原点と装置原点のノード間の相対位置情報の更新情報によって構成される。

一方、相対位置ツリー更新モジュールＱ（自己位置算出器Ｑ）５７は、例えばオドメトリによって構成される相対位置算出器であり、相対位置ツリーの最下位ノードとして設定されている車輪中心５５に取り付けられたセンサの取得する計測情報、すなわち車輪の回転や方向（ステアリング角度）の計測情報を利用して自己位置を算出する。
算出した自己位置に基づいて、相対位置ツリーの更新情報、すなわち、図６に示すツリー構成情報Ｑを生成して、相対位置ツリーの一部のリンクに相当する相対位置情報の更新処理を行おうとする。
具体的には、図６に示すように、ツリー構成情報Ｑは、自己位置原点と装置原点のノード間の相対位置情報の更新情報によって構成される。

このように、
相対位置ツリー更新モジュールＰ（自己位置算出器Ｐ）５６、
相対位置ツリー更新モジュールＱ（自己位置算出器Ｑ）５７、
これら２つのモジュールは、いずれも、同じノード間の相対位置情報を更新情報として生成する。

しかし、これらの２つの相対位置情報は、異なる位置に取り付けられたセンサを用い、さらに異なるアルゴリズムを適用して算出された情報であり、多くの場合、一致することのない相対位置情報となる。

すなわち、相対位置ツリー更新モジュールＰ（自己位置算出器Ｐ）５６は、位置算出のためのセンサとして、相対位置ツリーの最下位ノードとして設定されているカメラ５４を用い、このカメラの撮影画像に基づいて自己位置を算出する。
カメラ装着位置は、図２を参照して説明した例と同様、車上部の中央である。
相対位置ツリー更新モジュールＰ（自己位置算出器Ｐ）５６は、ＳＬＡＭに従ったアルゴリズムを適用してカメラ５４の位置を、装置原点として算出してしまう。

同様に、相対位置ツリー更新モジュールＱ（自己位置算出器Ｑ）５７は、位置算出のためのセンサとして、相対位置ツリーの最下位ノードとして設定されている車輪中心５５に取り付けられたセンサによる車輪の回転や方向の計測情報５５を用いて自己位置を算出する。
この場合のセンサ装着位置は、図２を参照して説明した例と同様、車輪の中心である。
相対位置ツリー更新モジュールＱ（自己位置算出器Ｑ）５７は、オドメトリを用いた位置算出アルゴリズムを適用して車輪中心５５の位置を、装置原点として算出してしまう。

このように、
相対位置ツリー更新モジュールＰ（自己位置算出器Ｐ）５６、
相対位置ツリー更新モジュールＱ（自己位置算出器Ｑ）５７、
これら２つのモジュールは、それぞれ異なる位置に取り付けられたセンサ（カメラ、車輪中心部の回転＆方向計測器）の情報に基づいて、異なるアルゴリズムを適用して装置原点の位置算出を行っており、結果として、各モジュールの算出したツリー構成情報（更新情報）が一致せず競合してしまうことになり、相対位置ツリーの更新処理ができなくなる。

次に、図７以下を参照して、上記問題点を解決した構成について説明する。
図７は、本開示の処理において利用する相対位置ツリーの構成例を示す図である。
図７に示す相対位置ツリーは、地図原点７１、自己位置原点７２、装置原点７３、カメラ７４、車輪中心７５、自己位置算出器Ｐ原点７６、自己位置算出器Ｑ原点７７、これらの７つのノードによって構成される。ノード間の接続リンクは、リンク設定ノード間の相対位置情報が記憶部に格納されていることを意味する。
このツリーが、移動装置内の記憶部に格納されている相対位置ツリーとなる。

図７に示す相対位置ツリーの構成ノード中、最下位ノードを除いた部分、すなわち、地図原点７１、自己位置原点７２、装置原点７３、カメラ７４、車輪中心７５、これらの５つのノードとリンク設定は、先に図６を参照して説明した従来型の相対位置ツリーと同様の構成である。
本開示の処理において利用する相対位置ツリーは、この従来型の相対位置ツリーにさらに、最下位ノードとして、自己位置算出器Ｐ原点７６、自己位置算出器Ｑ原点７７、これらの２つのノードを追加した構成である。

最下位ノードの１つである自己位置算出器Ｐ原点７６は、その上位ノードであるカメラ７４をセンサとして利用して自己位置を算出する自己位置算出器Ｐの原点位置を位置情報として持つノードである。
自己位置算出器Ｐは、例えば、カメラ７４の撮影画像に基づいてＳＬＡＭアルゴリズムにより自己位置算出を実行する自己位置算出器である。

また、もう１つの最下位ノードである自己位置算出器Ｑ原点７７は、その上位ノードである車輪中心７５に装着された車輪回転＆方向計測装置等をセンサとして利用して自己位置を算出する自己位置算出器Ｑの原点位置を位置情報として持つノードである。
自己位置算出器Ｑは、例えば、車輪中心７５に装着された車輪回転＆方向計測装置の計測結果に基づいてオドメトリアルゴリズムにより自己位置算出を実行する自己位置算出器である。

最下位ノードとして追加した自己位置算出器Ｐ原点７６、自己位置算出器Ｑ原点７７、これらの２つのノードの意味について、図８を参照して説明する。

「自己位置算出器原点」は、自己位置算出器が自己位置を算出する場合に、原点（基準点）とする位置である。誤差を考慮しなければ、自己位置算出器原点は地球などのグローバル座標系に対して静止位置にある。

例えば図８に示すように、移動装置１０が、時間Ｔ０に、起点位置Ｓから出発して、移動を開始して、時間Ｔ１において現在位置Ｃに移動したものとする。
図８には、この時の自己位置算出器Ｐ原点と、自己位置算出器Ｑ原点の例を示している。

例えば、図８に示すように、自己位置算出器Ｐ原点は、起点位置Ｓにおける移動装置の自己位置算出器Ｐのセンサ位置に相当するカメラ位置とする。
また、自己位置算出器Ｑ原点は、起点位置Ｓにおける移動装置の自己位置算出器Ｑのセンサ位置に相当する車輪中心位置とする。

図８に示す例は、自己位置算出器Ｐ原点と、自己位置算出器Ｑ原点の一例であるが、自己位置算出器原点は、地球などのグローバル座標系に対して静止位置にある一つの基準となる点として設定する。
このような自己位置算出器原点を設定することで、その後、移動装置１０が移動した場合、各自己位置算出器の移動状態、すなわち、現在の自己位置算出器の位置と自己位置算出器原点との相対位置を正確に取得することが可能となる。

図７に示す本開示の処理に適用する相対位置ツリーの左下側の２つのノード、すなわち、カメラ７４と、自己位置算出器Ｐ原点７６間のリンクは、これら２つのノード間の相対位置情報に相当する。
このリンクに対応する相対位置情報の具体例について、図９を参照して説明する。

図９には、図８を参照して説明したと同様、移動装置１０が、時間Ｔ０に、起点位置Ｓから出発して、移動を開始して、時間Ｔ１において現在位置Ｃに移動した状態を示している。
図９の起点位置Ｓの移動装置１０の自己位置算出器Ｐのセンサであるカメラの位置が自己位置算出器Ｐ原点である。
この原点位置を（Ｘｐ，Ｙｐ）＝（０，０）とする。
なお、本例では、説明を簡略化するため、移動装置１０はＺ軸方向（垂直方向）には移動しないものとして説明する。

移動装置１０の移動に伴い、自己位置算出器Ｐのセンサであるカメラの位置も移動する。時間Ｔ１において現在位置Ｃに移動した状態において、カメラの位置は図に示すように、座標位置（Ｘｐｃ，Ｙｐｃ）にある。

図９左側には、移動装置１０の記憶部に格納される相対位置ツリーの一部を示している。
自己位置算出器Ｐのセンサであるカメラのノードであるカメラ７４と、自己位置算出器Ｐ原点７６のノードとのリンク接続構成である。

自己位置算出器Ｐ原点７６は、起点位置Ｓの移動装置１０の自己位置算出器Ｐのセンサであるカメラの位置に相当し、カメラ７４は、現在位置Ｃに移動した移動装置１０のカメラ位置、すなわち座標位置（Ｘｐｃ，ＹＰｃ）に相当する。

自己位置算出器Ｐのセンサであるカメラのノードであるカメラ７４と、自己位置算出器Ｐ原点７６のノードとのリンクは、カメラ７４の位置に対する自己位置算出器Ｐ原点７６の相対位置情報が記憶部の格納データであることを意味する。

この相対位置情報は、図に示すように、起点位置Ｓの自己位置算出器Ｐ原点７６の位置と、現在位置Ｃの移動装置１０のカメラ位置、すなわち座標位置（Ｘｐｃ，ＹＰｃ）との差分に相当する。
すなわち、図９の左側の２つのノード間のリンク部に示す（−Ｘｐｃ，−Ｙｐｃ，０）がカメラ７４に対する自己位置算出器Ｐ原点の相対位置情報として記憶部に記録し、かつ更新すべきデータである。
この記録、更新処理を行うのが、相対位置ツリー更新モジュールとして機能する自己位置算出器Ｐ自身である。

すなわち、自己位置算出器の各々は、各自己位置算出器に対応するセンサの現在位置と、その自己位置算出器原点との差分（＝相対位置）を逐次、算出して、自己位置算出器原点ノードと、その自己位置算出器の利用センサノードとを結ぶリンクに相当する相対位置を算出して、相対位置ツリーの更新処理を行う。

この相対位置ツリー更新処理の具体例について、図１０を参照して説明する。
図１０の中央部には、先に図７を参照して説明した本開示の処理において利用する相対位置ツリーを示している。
すなわち、地図原点７１、自己位置原点７２、装置原点７３、カメラ７４、車輪中心７５、自己位置算出器Ｐ原点７６、自己位置算出器Ｑ原点７７、これらの７つのノードによって構成される相対位置ツリーである。

図１０には、２つの相対位置ツリー更新モジュールを示している。
相対位置ツリー更新モジュールＰ，７８は、自己位置算出器Ｐに相当する。
また、相対位置ツリー更新モジュールＱ，７９は、自己位置算出器Ｑに相当する。

相対位置ツリー更新モジュールＰ（自己位置算出器Ｐ）７８は、図８、図９を参照して説明した移動装置１０の上部中央に備え付けられたカメラ（センサＰ）によって撮影された画像に基づいて自己位置（＝センサＰの位置）を算出する例えばＳＬＡＭアルゴリズムに基づく自己位置算出器である。

一方、相対位置ツリー更新モジュールＱ（自己位置算出器Ｑ）７９は、図８を参照して説明した移動装置１０の車輪中心に備え付けられた回転＆方向検出器（センサＱ）によって取得された情報に基づいて自己位置（＝センサＱの位置）を算出する例えばオドメトリアルゴリズムに基づく自己位置算出器である。

図１０に示すように、これらの自己位置算出器Ｐ，Ｑは、いずれも相対位置ツリー更新モジュール７８，７９として、記憶部に格納された相対位置ツリーの一部を更新する。

図１０に示すように、相対位置ツリー更新モジュールＰ（自己位置算出器Ｐ）７８は、自己位置算出器Ｐに対応するカメラの現在位置と、自己位置算出器Ｐ原点との差分（＝相対位置）を逐次、算出して、相対位置ツリーのカメラ７４と自己位置算出器Ｐ原点７６との各ノードを結ぶリンクに相当する相対位置を算出して、相対位置ツリーの更新処理を行う。

また、相対位置ツリー更新モジュールＱ（自己位置算出器Ｑ）７９は、自己位置算出器Ｑのセンサ位置に対応する車輪中心の現在位置と、自己位置算出器Ｑ原点との差分（＝相対位置）を逐次、算出して、相対位置ツリーの車輪中心７５と自己位置算出器Ｑ原点７７との各ノードを結ぶリンクに相当する相対位置を算出して、相対位置ツリーの更新処理を行う。

このように、複数の相対位置ツリー更新モジュール（自己位置算出器）の各々は、各モジュールである自己位置算出器の利用するセンサの位置対応のノードと自己位置算出器原点との接続構成のみを対象とした相対位置ツリー更新処理を行う。従って、先に図５を参照して説明したようなデータ競合の問題が発生しない。

本開示の処理を適用した相対位置ツリー更新処理の一般的な例について、図１１を参照して説明する。
図１１には、以下の２つの相対位置ツリー更新モジュールを示している。
相対位置ツリー更新モジュールＰ，７８は、自己位置算出器Ｐに相当し、自己位置算出器センサＰを利用して、アルゴリズムＰを適用した自己位置算出処理を行う。
相対位置ツリー更新モジュールＱ，７９は、自己位置算出器Ｑに相当し、自己位置算出器センサＱを利用して、アルゴリズムＱを適用した自己位置算出処理を行う。

記憶部８２には、相対位置ツリーが格納されている。これは、例えば、先に図７を参照して説明した相対位置ツリーである。

相対位置ツリー更新モジュールＰ，７８は、記憶部８２に格納された相対位置ツリーの一部、すなわち、
自己位置算出器Ｐセンサと自己位置算出器Ｐ原点とのノード接続構成のみを対象とした相対位置ツリー更新処理を行う。
一方、相対位置ツリー更新モジュールＱ，７９は、記憶部８２に格納された相対位置ツリーの一部、すなわち、
自己位置算出器Ｑセンサと自己位置算出器Ｑ原点とのノード接続構成のみを対象とした相対位置ツリー更新処理を行う。

このように、複数の相対位置ツリー更新モジュール（自己位置算出器）の各々は、各モジュールである自己位置算出器の利用するセンサの位置対応のノードと自己位置算出器原点との接続構成のみを対象とした相対位置ツリー更新処理を行う。従って、先に図５を参照して説明したようなデータ競合の問題が発生しない。
なお、図１１に示す例では、相対位置ツリー更新モジュールを２つとした例を示しているが、３以上の設定でも同様の処理が可能であり、データ競合を発生させることなく、相対位置ツリーの更新処理を行うことができる。

しかし、図１０、図１１を参照して説明した相対位置ツリーの更新処理は、相対位置ツリーの下位ノードのみの更新処理である。
相対位置ツリーの更新処理は、さらに上位ノードについても実行する必要がある。

図１２を参照して相対位置ツリーの自己位置原点７２と装置原点７３の２ノード間のデータ更新処理について説明する。
なお、装置原点７３と、その下位ノードのセンサノード（カメラ７４、車輪中心７５）間の相対位置は、変更されないので更新処理が不要である。

図１２に示すように、相対位置ツリーの自己位置原点７２と装置原点７３の２ノード間のデータ更新処理は、自己位置統合部８０が実行する。
自己位置統合部８０は、移動装置１０内に設けられた処理部である。
自己位置統合部８０の実行する処理について、図１３以下を参照して説明する。

図１２には、自己位置統合部８０の実行する処理をステップＳ１１ａ〜Ｓ１３として、処理順に示している。

まず、自己位置統合部８０は、ステップＳ１１ａにおいて、記憶部８２に格納された相対位置ツリーを読み出す。
読出し対象データは、図１３に示すように、
装置原点７３、カメラ７４、車輪中心７５、自己位置算出器Ｐ原点７６、自己位置算出器Ｑ原点７７、これらのノードによって構成されるデータ、すなわち、各ノード間の相対位置情報を含むデータである。
図には、各ノードを結ぶリンクａ〜ｄを示しているが、自己位置統合部８０は、これらのリンク対応の相対位置情報を記憶部８２から取得する。

自己位置統合部８０は、さらに、ステップＳ１１ｂにおいて、状況分析部８３から、環境情報を入力する。
この状況分析部８３は、移動装置１０の一構成要素であり、例えば移動装置１０の外部の明るさ、視界等の環境、さらに各センサの動作状態等を分析し、この分析結果を自己位置統合部８０に入力する。

先に説明したように、本開示の処理において、移動装置１０には複数の異なるアルゴリズムによる自己位置を算出する自己位置算出器が装着されている。
しかし、これらの自己位置算出器の算出する位置情報は、環境によって精度が大きく変化してしまうという問題がある。
例えば、ＳＬＡＭは、カメラ撮影画像を適用した処理を行うため、夜や激しい雨の中のような環境等、周囲の明瞭な画像を撮影しにくい環境では、算出する位置精度が低下してしまう。
また、例えば高層ビルが多い環境等、ＧＰＳ衛星からのデータの届きにくい環境では、ＧＰＳ利用システムによって算出される位置精度が低下する。
なお、前述したように、本開示の処理は、異なるアルゴリズムを適用した複数の自己位置算出器を利用した構成に限らず、同一のアルゴリズムを適用した複数の自己位置算出器を利用した構成においても適用可能である。同一のアルゴリズムを適用した複数の自己位置算出器を利用した構成においても、各自己位置算出器の取り付け位置の違いや、各自己位置算出器の測定精度の違い、測定誤差等により、それぞれの算出器の算出値にずれが発生する場合がある。

このように、自己位置算出器は環境の変化や違いによって可用性と性能が変わる。すべての環境で高精度な位置情報を算出可能な自己位置算出器は存在しない。
また、センサの故障によって、そのセンサに依存した自己位置算出器は正常に機能しなくなる。
なお、環境情報には、移動装置の外部環境、複数の自己位置算出器の利用センサの故障情報、リソースの利用状況等が含まれる。
自己位置統合部８０は、移動装置の外部の様子やセンサ情報、リソース情報等を環境情報として入力して、これらの情報を参照して、相対位置ツリーの更新情報を生成する。

自己位置統合部８０は、ステップＳ１２ａにおいて、各自己位置算出器対応の標準自己位置の算出処理を行う。
標準自己位置とは、装置原点７３の位置に相当する。この装置原点７３の位置算出は、すなわち、自己位置原点と、装置原点との相対位置の算出処理にも相当する。
すなわち、図１３のステップＳ１３内に示す相対位置ツリーの一部構成である自己位置原点７２と、装置原点７３の各ノード間の相対位置情報（リンクＫ）を算出する処理に相当する。

このステップＳ１２ａの処理の具体例について、図１４を参照して説明する。
図１４に示す例は、自己位置算出器Ｐに対応する標準自己位置Ｐ，８８の算出例である。
なお、自己位置統合部８０は、ステップＳ１２ａにおいて、複数の自己位置算出器対応の複数の標準自己位置の算出処理を行う。
図１４に示す例は、その中の１つの自己位置算出器Ｐに対応する標準自己位置Ｐ，８８の算出例である。

図１４には、時間Ｔ０における起点位置Ｓ（出発地）の移動装置１０と、その後の時間Ｔ１における現在位置Ｃの移動装置１０を示している。
相対位置ツリーの更新処理は、逐次、実行されるが、図１４に示す例では、時間Ｔ１において移動装置１０が現在位置Ｃにいる時点において、自己位置算出器Ｐに対応する標準自己位置Ｐ，８８を算出して、算出値に基づいて相対位置ツリー更新処理を行う場合の例である。

自己位置統合部８０は、ステップＳ１２ａにおいて、自己位置算出器Ｐ対応の標準自己位置Ｐ，８８の算出処理を行う。標準自己位置とは、前述したように装置原点７３の位置に相当する。
図１４に示す例では、時間Ｔ１の装置原点７３（ｔ１）の位置である。
図１４に示す時間Ｔ１の現在位置Ｃの装置原点７３（ｔ１）の位置を算出すればよい。

図１４に示す現在位置Ｃの装置原点７３（ｔ１）の位置は、図１４に示す起点位置Ｓの移動装置１０の自己位置原点位置７２（ｔ０）との相対位置として算出することができる。
この相対位置は、時間Ｔ１における相対位置ツリー内のリンクＫによって示される相対位置である。これは、すなわち、図１３のステップＳ１３に示す相対位置ツリーの自己位置原点７２と、装置原点７３の各ノード間の相対位置情報（リンクＫ）に相当する。

なお、自己位置原点７２は、時間経過に従った移動装置１０の移動に伴って移動しない固定点であるが、装置原点７３は、移動装置１０の移動に伴い移動するので、相対位置ツリーの自己位置原点７２と、装置原点７３の各ノード間の相対位置情報（リンクＫ）は、時間経過に応じて、逐次、更新する必要がある。
図１４の起点位置Ｓにおける自己位置原点７２と、装置原点７３との接続線が時間Ｔ０におけるリンクＫ（ｔ０）に相当し、図１４の起点位置Ｓにおける自己位置原点７２と、現在位置Ｃの装置原点７３との接続線が時間Ｔ１におけるリンクＫ（ｔ１）となる。

図１４に示す時間Ｔ１の現在位置Ｃの移動装置１０のカメラ７４と、装置原点７３との相対位置は、図１３に示す記憶部８２から取得した相対位置ツリーのリンクａ対応の相対位置情報である。
図１４には、時間Ｔ１の相対位置情報としてリンクａ（ｔ１）として示している。

また、図１４に示す時間Ｔ１の現在位置Ｃの移動装置１０のカメラ７４と、時間Ｔ０の起点位置Ｓの移動装置１０のカメラ、すなわち自己位置算出器Ｐ原点７６との相対位置は、図１３に示す記憶部８２から取得した相対位置ツリーのリンクｂ対応の相対位置情報である。
図１４には、時間Ｔ１の相対位置情報としてリンクｂ（ｔ１）として示している。

なお、時間Ｔ０の起点位置Ｓ（出発地）における移動装置１０のカメラ位置に相当する自己位置算出器Ｐ原点７６と、自己位置原点７２との差分（相対位置）は、図に示すように、初期化処理時差分算出データ９０である。
この初期化処理時差分算出データ９０は、移動装置１０の初期化処理において算出してメモリに格納する。
すなわち、移動装置１０は、移動開始前に、自己位置算出器Ｐ原点７６と、自己位置原点７２との差分（相対位置）を計測してメモリに格納する処理を実行する。
具体的な処理シーケンスについては、後段で、図１９、図２０に示すフローチャートを参照して説明する。

図１３を参照して説明したステップＳ１２ａの処理において、自己位置統合部８０は、図１４に示す標準自己位置Ｐ，８８を算出する。
前述したように、標準自己位置Ｐ，８８は図１４に示す現在位置Ｃの装置原点７３（ｔ１）の位置に相当し、これは起点位置Ｓの移動装置１０の自己位置原点位置７２（ｔ０）との相対位置として算出することができる。
この相対位置は、時間Ｔ１における相対位置ツリー内のリンクＫ（ｔ１）によって示される相対位置である。

図１４から、理解されるように、
リンクＫ（ｔ１）と、リンクａ（ｔ１）と、リンクｂ（ｔ１）、および初期化処理時算出差分データ９０の４本の線は、閉じた四角形を構成している。
また、リンクａ（ｔ１）と、リンクｂ（ｔ１）、および初期化処理時算出差分データ９０の３本の線各々が結ぶ２つの頂点間の相対位置は既知である。
具体的には、以下の各頂点間の相対位置が既知である。
（１）リンクａ（ｔ１）が接続する頂点、すなわち、時間Ｔ１の現在位置Ｃの標準自己位置Ｐ，８８（＝装置原点７３（ｔ１））と、カメラ７４との相対位置、
（２）リンクｂ（ｔ１）が接続する頂点、すなわち、時間Ｔ１の現在位置Ｃにおけるメラ７４と、時間Ｔ０の起点位置Ｓの自己位置算出器Ｐ原点７６との相対位置、
（３）初期化処理時算出差分データ９０が接続する頂点、すなわち、時間Ｔ０の起点位置Ｓの自己位置算出器Ｐ原点７６と、自己位置原点７２との相対位置、
これら、の頂点間の相対位置が全て既知である。

従って、これらの相対位置関係から、リンクＫ（ｔ１）が接続する頂点、すなわち、時間Ｔ１の現在位置Ｃの標準自己位置Ｐ，８８（＝装置原点７３（ｔ１））と、時間Ｔ０の起点位置Ｓの自己位置原点７２との相対位置を算出することができる。

具体的には、
自己位置原点７２に対する標準自己位置Ｐ，８８（＝装置原点７３（ｔ１））の相対位置であるリンクＫ（ｔ１）は、以下の既知の３つの相対位置（相対位置１〜３）を加算することで算出できる。
（相対位置１）自己位置原点７２に対する自己位置算出器Ｐ原点７６との相対位置、
（相対位置２）自己位置算出器Ｐ原点７６に対するカメラ７４の相対位置、
（相対位置３）カメラ７４に対する標準自己位置Ｐ，８８（＝装置原点７３（ｔ１））の相対位置、

自己位置統合部８０は、上記３つの相対位置情報を加算することで、自己位置原点７２に対する標準自己位置Ｐ，８８（＝装置原点７３（ｔ１））の相対位置であるリンクＫ（ｔ１）を算出する。
このリンクＫ（ｔ１）で示される相対位置情報が、自己位置検出器Ｐ対応の標準自己位置Ｐ（ｔ１），８８、すなわち、現在位置Ｃにおける装置原点７３の位置を示す。
自己位置統合部８０は、この図１４を参照して説明した処理に従って、自己位置検出器Ｐ対応の標準自己位置Ｐ（ｔ１），８８を算出する。

なお、図１４を参照して説明した標準自己位置Ｐ（ｔ１），８８の算出処理例は一例であり、この他の処理も可能である。
図１５、図１６を参照して、異なる処理例について説明する。

まず、図１５に示す処理例について説明する。
図１５に示す処理と、図１４に示す処理例との差異は、図１４を参照して説明した初期化処理時算出差分データ９０を、２つの差分データに分割した点である。
図１５においては、以下の２つの差分データを初期化処理時算出差分データとしている。
（１）時間Ｔ０の起点位置Ｓの自己位置算出器Ｐ原点７６と、装置原点７３（ｔ０）との相対位置に相当する初期化処理時算出差分データ１，９１
（２）時間Ｔ０の起点位置Ｓの装置原点７３（ｔ０）と、自己位置原点７２との相対位置に相当する初期化処理時算出差分データ２，９２
これら２つの差分データの加算値は、図１４を参照して説明した初期化処理時算出差分データ９０に相当する。

この図１５に示す２つの差分データを初期化処理時に算出して、これらを利用して、標準自己位置Ｐ（ｔ１），８８を算出する構成としてもよい。

次に図１６に示す処理例について説明する。
図１６に示す処理例は、時間Ｔ０の起点位置Ｓにおいて、移動装置１０の自己位置原点７２と、装置原点７３（ｔ０）とを一致させて設定した例である。
この場合、図１６に示すように、
（１）時間Ｔ０の起点位置Ｓの自己位置算出器Ｐ原点７６と、自己位置原点７２（＝装置原点７３（ｔ０））との相対位置に相当する初期化処理時算出差分データ１，９１
この差分データのみを用いて標準自己位置Ｐ（ｔ１），８８を算出することが可能となる。
このように、標準自己位置Ｐ（ｔ１），８８の算出処理としては、様々な処理が可能となる。

なお、図１４〜図１６を参照して説明した処理は、自己位置検出器Ｐ対応の標準自己位置の算出処理であるが、自己位置統合部８０は、同様に、自己位置検出器Ｑ対応の標準自己位置Ｑも算出する。
標準自己位置Ｑの算出処理は、図１３に示す記憶部８２から取得した相対位置ツリーのリンクｃ，ｄ対応の相対位置情報を用いて行うことができる。

このようにして、自己位置統合部８０は、全ての自己位置検出器対応の標準自己位置を算出する。
自己位置統合部８０が算出する全ての自己位置検出器対応の標準自己位置は、すべて現在位置Ｃにおける装置原点７３の位置（自己位置原点７２との相対位置）であり、本来は、同じ位置情報でなければならない。

しかし、この標準自己位置は、それぞれ異なる自己位置算出器が、それぞれ異なる位置算出アルゴリズムに従って算出したものである。
例えば自己位置算出器Ｐは、ＳＬＡＭアルゴリズムに従った自己位置算出を実行し、自己位置算出器Ｑは、オドメトリアルゴリズムに従った自己位置算出を実行している。

これらの各アルゴリズムは、異なる処理であり、結果として、各自己位置算出器の算出する標準自己位置には差が生じる。
また、暗い環境では、カメラ撮影画像を用いたＳＬＡＭアルゴリズムに従った位置算出処理の精度が低下するといった、環境に応じた精度変化も発生する。
また、センサの故障等による精度低下も発生し得る。

これらの状況を考慮し、自己位置統合部８０は、ステップＳ１２ｂにおいて、ステップＳ１２ａで算出した複数の自己位置算出器対応の複数の標準自己位置に基づいて、最終的にツリー更新に適用するための標準自己位置、すなわち、リンクＫに相当する自己位置原点７２と装置原点７３との相対位置を算出する。

自己位置統合部８０が、ステップＳ１２ｂにおいて実行する処理、すなわち、最終的なツリー更新に適用する標準自己位置を決定する処理の処理態様は、様々な態様がある。
具体的には、例えば以下の（ａ）〜（ｃ）の３種類の処理態様がある。
（ａ）複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を選択してツリー更新に適用する標準自己位置を決定する処理。
（ｂ）複数の自己位置算出器対応の複数の標準自己位置を合成（フュージョン）して、ツリー更新に適用する標準自己位置を生成する処理。
（ｃ）上記、（ａ），（ｂ）の処理を状況に応じて切り替えて、ツリー更新に適用する標準自己位置を決定する処理。

以下、これら（ａ）〜（ｃ）の処理態様の具体例について、図１７、図１８を参照して説明する。

まず、図１７を参照して、
（ａ）複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を選択してツリー更新に適用する標準自己位置を決定する処理。
この処理（ａ）の具体例について説明する。
この処理（ａ）は、さらに、図１７に示すように（ａ１）〜（ａ４）の４つの処理態様に細分類することができる。
以下、これらの処理について説明する。

（ａ１）複数の自己位置算出器対応のセンサに応じて、予め設定した優先順に従って、１つの自己位置算出器対応の標準自己位置を選択する。
この処理例の具体例は、例えば以下の通りである。
（例１）ステレオカメラを搭載し、ステレオカメラの撮影画像に基づくＳＬＡＭを実行している場合、ＳＬＡＭ対応の標準自己位置を最優先に選択する。
（例２）センサとしてＬｉＤＡＲを搭載している場合は、ＮＤＴによって算出された標準自己位置を最優先に選択する。

（ａ２）移動装置の走行環境に応じて、１つの自己位置算出器対応の標準自己位置を選択する。
この処理の具体例は、例えば以下の通りである。
（例１）レーザ光の反射オブジェクトの少ない環境では、ＮＤＴによる位置検出精度が低下するので、ＮＤＴ以外の自己位置算出器対応の標準自己位置を選択する。
（例２）夜や特徴点の少ない環境ではカメラ撮影画像を用いるＳＬＡＭによる位置算出精度が低下するので、ＳＬＡＭ以外の自己位置算出器対応の標準自己位置を選択する。
（例３）タイヤのスリップ等が発生しやすい場所の場合、車輪オドメトリを適用した位置算出精度が低下するので、オドメトリ以外の自己位置算出器対応の標準自己位置を選択する。

（ａ３）計算リソースと精度に応じて、１つの自己位置算出器対応の標準自己位置を選択する。
この処理の具体例は、例えば以下の通りである。
（例１）省電力モードの場合、消費電力料の少ない車輪オドメトリを適用した自己位置算出器対応の標準自己位置を選択。なお、ＮＤＴは精度が高いが計算量が多く、消費電力が大きくなるので、省電力モードでは利用しない。

（ａ４）センサの故障検出に応じて、１つの自己位置算出器対応の標準自己位置を選択する。
この処理の具体例は、例えば以下の通りである。
（例１）通常は、カメラ撮影画像を用いたＳＬＡＭ対応の標準自己位置を選択するが、カメラ故障時は、車輪オドメトリ対応の標準自己位置を選択する。

次に、図１８を参照して、
（ｂ）複数の自己位置算出器対応の複数の標準自己位置を合成（フュージョン）して、ツリー更新に適用する標準自己位置を生成する処理。
（ｃ）上記、（ａ），（ｂ）の処理を状況に応じて切り替えて、ツリー更新に適用する標準自己位置を決定する処理。
これらの処理（ｂ），（ｃ）の具体例について説明する。
処理（ｂ）は、さらに、図１８に示すように（ｂ１）〜（ｂ２）の２つの処理態様に細分類することができる。
以下、これらの処理について説明する。

（ｂ１）カルマンフィルタによる確率統合を行う。
この処理の具体例は、以下の通りである。
（例１）ＳＬＡＭ対応の標準自己位置と、車輪オドメトリ対応の標準自己位置について、カルマンフィルタによる確率統合処理を行い、最終的な出力用の標準自己位置を算出する。

（ｂ２）比率統合を行う。
この処理の具体例は、以下の通りである。
（例１）ＳＬＡＭ対応の標準自己位置と、車輪オドメトリ対応の標準自己位置について、予め規定した比率で合成処理を行い、最終的な出力用の標準自己位置を算出する。

次に、
（ｃ）上記、（ａ），（ｂ）の処理を状況に応じて切り替えて、ツリー更新に適用する標準自己位置を決定する処理。
について説明する。
この処理は、
（ｃ１）算出器の標準自己位置から選択された１つの標準自己位置と合成（フュージョン）標準自己位置の切り替えを行う処理である。
具体例は、以下の通りである。

（例１）複数の標準自己位置の合成（フュージョン）処理によって算出した標準自己位置は環境ロバスト性が高いが、合成対象とした自己位置算出器のすべてが正常に機能していなければ、合成値は精度が低下する。
従って、各自己位置算出器の利用センサの故障が検出されていない場合は合成値を出力し、センサの故障が発生した場合は、故障していないセンサを利用した自己位置算出器対応の標準自己位置を選択して出力する。

（例２）複数の自己位置算出器対応の標準自己位置算出や、合成（フュージョン）処理には、計算リソースが多く必要となり、計算リソースに余裕がない場合は、合成処理を停止して、１つの自己位置算出器対応の標準自己位置を選択する。

このように、自己位置統合部８０は、図１３に示すステップＳ１２ｂにおいて、図１７、図１８を参照して説明した処理、すなわち、以下の（ａ）〜（ｃ）のいずれかの処理を実行して、複数の自己位置算出器対応の複数の標準自己位置から１つの、最終的に相対位置ツリーの更新処理に適用する標準自己位置を決定する。
（ａ）複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を選択してツリー更新に適用する標準自己位置を決定する処理。
（ｂ）複数の自己位置算出器対応の複数の標準自己位置を合成（フュージョン）して、ツリー更新に適用する標準自己位置を生成する処理。
（ｃ）上記、（ａ），（ｂ）の処理を状況に応じて切り替えて、ツリー更新に適用する標準自己位置を決定する処理。

次に、自己位置統合部８０は、図１３に示すステップＳ１３において、ステップＳ１２ｂで決定した相対位置ツリーの更新処理に適用する標準自己位置を利用して、記憶部８２に格納された相対位置ツリーの一部構成、すなわち、
自己位置原点７２と、装置原点７３とのノード接続構成の更新処理を行う。

なお、ステップＳ１２ｂにおいて算出した標準自己位置は、装置原点７３の位置情報、具体的には、自己位置原点７２の位置に対する装置原点７３の相対位置であり、図１３のステップＳ１３中のノード構成中に示すリンクＫに対応する相対位置情報である。
すなわち、ステップＳ１３では、ステップＳ１２ｂで決定した標準自己位置を、記憶部８２に格納された相対位置ツリーの自己位置原点７２と装置原点７３間のリンクＫ対応の相対位置情報として格納する。

これらの処理により記憶部８２に格納された相対位置ツリーは、問題なく更新処理が行われることになる。
なお、記憶部８２に格納された相対位置ツリーの更新処理は、移動装置１０の移動に伴い、逐次、定期的に実行され、常に、最新の移動装置１０の位置に対応したデータに書き換えられることになる。

この記憶部８２に格納された相対位置ツリーは、移動装置１０の相対位置ツリー利用モジュールによって利用される。
相対位置ツリー利用モジュールは、例えば移動装置１０の移動経路を決定する行動決定部などである。

行動決定部の決定した経路情報が駆動制御部に出力され、駆動制御部は、経路情報に従って移動装置１０を駆動する駆動制御情報を生成して生成した駆動制御情報を、走行部や歩行部、具体的には、アクセル、ブレーキ、ステアリング等によって構成される駆動部に出力して、決定した経路に従って移動装置１０を移動させる。

［４．移動装置の実行する処理のシーケンスについて］
次に、図１９、図２０に示すフローチャートを参照して、移動装置の実行する処理のシーケンスについて説明する。

図１９、図２０に示すフローチャートに従った処理は、例えば、移動装置のデータ処理部が、記憶部に格納されたプログラムに従って実行することが可能である。
データ処理部は例えばＣＰＵ等のプログラム実行機能を有するハードウェアを備えている。

なお、図１９、図２０に示すフローチャートに従った処理は、すべての処理を移動装置のデータ処理部の１つである自己位置統合部８０の処理として実行することも可能であり、自己位置統合部８０とその他のデータ処理部を利用した処理として実行してもよい。
以下、フローチャートの各ステップの処理について説明する。

（ステップＳ１０１）
まず、移動装置は、ステップＳ１０１において、移動装置の自己位置原点を設定する。
自己位置原点は、先に図１を参照して説明したように、例えば、移動装置の出発地である起点Ｓ等に設定される。
なお、図１の例は、自己位置原点の設定例の一例であり、この他の点、例えば、地図原点を自己位置原点として設定することも可能である。
ただし、自己位置原点は、移動装置の移動に伴って移動することのない固定点として設定することが必要である。

（ステップＳ１０２）
次に、ステップＳ１０２において、移動装置に装着された全ての自己位置算出器の初期化処理が完了したか否かを確認する。
移動装置には、様々な異なるアルゴリズムに従って自己位置を算出する複数の自己位置算出器が装着されている。

例えば以下のような自己位置算出器である。
（１）ＧＰＳ、あるいはＧＮＳＳと、ＩＭＵを組み合わせた自己位置算出器、
（２）ＳＬＡＭを利用した自己位置算出器。
（３）オドメトリ（車輪オドメトリ）を適用した自己位置算出器。
（４）ライダー（ＬｉＤＡＲ）やソナーを用いた自己位置算出器。

ステップＳ１０２では、移動装置に装着された全ての自己位置算出器の初期化処理が完了したか否かを判定する。
完了している場合は、ステップＳ１０６に進む。
未完了の機器がある場合は、ステップＳ１０３に進む。

（ステップＳ１０３）
ステップＳ１０２において、初期化処理の完了していない自己位置算出器があると判定された場合、初期化処理の完了していない自己位置算出器について、ステップＳ１０３〜Ｓ１０５の処理を実行して初期化処理を行う。

まず、ステップＳ１０３において、初期化処理の完了していない自己位置算出器の中から、１つの初期化処理対象の自己位置算出器を選択する。
この初期化処理対象の自己位置算出器を自己位置算出器Ａとする。

（ステップＳ１０４）
次に、ステップＳ１０４において、自己位置算出器Ａの原点とステップＳ１０１で設定した自己位置原点との差分をメモリに記録する。

自己位置算出器Ａの原点とは、例えば自己位置算出器Ａがカメラ撮影画像に基づく自己位置検出を行うカメラやＳＬＡＭ等であれば、画像を撮影するカメラの位置であり、自己位置算出器Ａが車輪の回転や方向等に基づく自己位置検出を行うオドメトリ等であれば、車輪中心位置である。

なお、この自己位置算出器の初期化処理は、移動装置が移動を開始する前に実行する。
この処理は、先に図１４を参照して説明した初期化処理時算出差分データ９０の算出処理に相当する。
例えば先に図１４を参照して説明した例では、起点位置Ｓ（出発地）において実行する。
図１４における自己位置算出器Ｐが初期化処理対象となる自己位置算出器である場合、ステップＳ１０４において算出する差分は、自己位置算出器Ｐ原点７６と、自己位置原点７２との差分に相当する。すなわち、自己位置算出器Ｐ原点７６と自己位置原点７２との相対位置である。

ステップＳ１０４では、このように初期化処理の済んでいない自己位置算出器Ａの原点とステップＳ１０１で設定した自己位置原点との差分、すなわち図１４を参照して説明した初期化処理時算出差分データ９０を算出してメモリに記録する。
なお、先に、図１４〜図１６を参照して説明したように、初期化処理時算出差分データには、いくつかのバリエーションがあり、図１４〜図１６のいずれかにおいて説明した初期化処理時算出差分データを算出してメモリに記録すればよい。

（ステップＳ１０５）
ステップＳ１０４の処理が完了すると、ステップＳ１０５において自己位置算出器Ａの初期化処理を完了し、ステップＳ１０２に戻り、初期化処理の完了していないその他の自己位置算出器について、ステップＳ１０３〜Ｓ１０５の処理を実行する。
ステップＳ１０２において、全ての自己位置算出器についての初期化処理が完了したと判定すると、ステップＳ１０６に進む。

（ステップＳ１０６）
ステップＳ１０６では、自己位置算出処理を終了するか否かを判定し、処理終了と判定した場合は処理を終了する。
自己位置算出処理を実行中の場合は、ステップＳ１０７に進む。

（ステップＳ１０７）
ステップＳ１０７において、自己位置統合部８０は、移動装置に装着した全ての自己位置算出器の算出した自己位置、すなわち現在の自己位置を取得する。

例えば、図１２、図１３を参照して説明した例では、
（Ｐ）カメラ撮影画像に基づくＳＬＡＭアルゴリズムを実行する自己位置算出器Ｐの算出した自己位置。
（Ｑ）車輪中心に装着した車輪回転＆方向検出器の検出情報に基づくオドメトリアルゴリズムを実行する自己位置算出器Ｑ。
自己位置統合部８０は、これらの複数の自己位置算出器各々の算出した複数の自己位置（現在値）を取得する。

（ステップＳ１０８）
次に、自己位置統合部８０は、ステップＳ１０８において、各自己位置算出器の算出した自己位置を、全て、標準自己位置（装置原点位置に相当）に変換する。
標準自己位置とは、移動装置の中心部、例えば装置原点の現在位置に相当する位置情報である。

すなわち、各自己位置算出器の算出した自己位置は、各自己位置算出器のセンサ位置、例えばカメラ位置や車輪中心位置等、各自己位置算出器の利用センサの位置であり、各自己位置算出器個々のセンサ位置となり、一致しない。

ステップＳ１０８では、これら各自己位置算出器の個々のセンサ位置である各自己位置算出器の算出自己位置を、移動装置の位置に相当する標準自己位置（装置原点位置に相当）に変換する。
この自己位置から標準自己位置への変換処理に際しては、各自己位置算出器のセンサ位置と装置原点との差分（相対位置）を考慮した処理を行う。

具体的には、例えば図１４の例では、自己位置算出器のセンサ位置と装置原点との差分（相対位置）は、リンクａに相当する。
リンクａの値は、起点位置Ｓにおいて実行した初期化処理、すなわち、ステップＳＳ１０３〜Ｓ１０５において実行した初期化処理において算出され、メモリに格納されている値である。

ステップＳ１０８では、このように、各自己位置算出器の個々のセンサ位置である各自己位置算出器の算出自己位置を、移動装置の位置に相当する標準自己位置（装置原点位置に相当）に変換する。
例えば、図１２〜図１４を参照して説明した例では、以下の２つの自己位置算出器の算出した２つの算出自己位置がある。
（Ｐ）カメラ撮影画像に基づくＳＬＡＭアルゴリズムを実行する自己位置算出器Ｐの算出した自己位置。
（Ｑ）車輪中心に装着した車輪回転＆方向検出器の検出情報に基づくオドメトリアルゴリズムを実行する自己位置算出器Ｑ。

ステップＳ１０８において、自己位置統合部８０は、これらの２つの自己位置算出器各々の算出した算出自己位置を、それぞれ標準自己位置に変換する。

これら複数の自己位置算出器の算出自己位置から得られる標準自己位置は、それぞれのセンサ位置と、装置原点（＝例えば車両中心）との差分を反映して算出される。
従って、全ての自己位置算出器の算出自己位置から得られる標準自己位置は一致する位置情報、すなわち、１つの装置原点（＝例えば車両中心）の位置情報が算出されるべきであるが、実際は、これらの値は一致することなく、各自己位置算出器対応の算出自己位置から求められる標準自己位置は一致しない値となる。

これは、各自己位置算出器が各々、異なるアルゴリズムで自己位置を算出しており、さらに、各自己位置算出器は、自己位置算出処理を実行する環境に応じて精度が大きく変わる可能性があるからである。
具体的には、夜や特徴点の少ない環境ではカメラ撮影画像を用いるＳＬＡＭによる位置算出精度が低下する。また、タイヤのスリップ等が発生しやすい場所の場合、車輪オドメトリを適用した位置算出精度が低下する。

このような状況から、各自己位置算出器の算出自己位置から得られる標準自己位置は、多くの場合、一致しない値となる。

（ステップＳ１０９）
ステップＳ１０８において、複数の自己位置算出器の算出自己位置の変換データである標準自己位置を算出すると、次に、自己位置統合部８０は、ステップＳ１０９において、最終的な出力情報、すなわち、相対位置ツリー更新情報として、１つの標準自己位置を含む出力情報の決定処理を行うため、環境情報を入力する。

この処理は、先に図１３を参照して説明したステップＳ１１ｂの処理に相当し、状況分析部８３から環境情報を入力する処理である。
この状況分析部８３は、移動装置１０の一構成要素であり、例えば移動装置１０の外部の明るさ、視界等の環境、さらに各センサの動作状態、リソース利用状況等を分析し、この分析結果を自己位置統合部８０に入力する。

先に説明したように、本開示の処理において、移動装置１０には複数の異なるアルゴリズムによる自己位置を算出する自己位置算出器が装着されている。
しかし、これらの自己位置算出器の算出する位置情報は、環境によって精度が大きく変化してしまうという問題がある。
例えば、ＳＬＡＭは、カメラ撮影画像を適用した処理を行うため、夜や激しい雨の中のような環境等、周囲の明瞭な画像を撮影しにくい環境では、算出する位置精度が低下してしまう。
また、例えば高層ビルが多い環境等、ＧＰＳ衛星からのデータの届きにくい環境では、ＧＰＳ利用システムによって算出される位置精度が低下する。

このように、自己位置算出器は環境の変化や違いによって可用性と性能が変わる。すべての環境で高精度な位置情報を算出可能な自己位置算出器は存在しない。
また、センサの故障によって、そのセンサに依存した自己位置算出器は正常に機能しなくなる。
自己位置統合部８０は、移動装置の外部の様子やセンサ情報、さらには、リソースの利用状況等を環境情報として入力して、これらの情報を参照して、相対位置ツリーの更新情報を生成する。

（ステップＳ１１０）
自己位置統合部８０は、ステップＳ１１０において、ステップＳ１０９で入力した環境情報に基づいて、標準自己位置（装置原点）の位置情報を含む相対位置ツリー更新情報の出力態様を決定する。

具体的な標準自己位置（装置原点）の位置情報の出力態様としては、３種類の態様がある。これらは、先に図１７、図１８を参照して説明した以下の３種類である。
（ａ）複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を選択してツリー更新に適用する標準自己位置を決定する処理。
（ｂ）複数の自己位置算出器対応の複数の標準自己位置を合成（フュージョン）して、ツリー更新に適用する標準自己位置を生成する処理。
（ｃ）上記、（ａ），（ｂ）の処理を状況に応じて切り替えて、ツリー更新に適用する標準自己位置を決定する処理。

自己位置統合部８０は、ステップＳ１１０において、上記（ａ）〜（ｃ）複数の標準自己位置出力態様のいずれの態様とするかを環境情報に基づいて決定する。
なお、図１７、図１８を参照して説明したように、上記（ａ），（ｂ）には、それぞれさらに複数の処理態様（（ａ１）〜（ａ４），（ｂ１）〜（ｂ２））があり、自己位置統合部８０は、入力した環境情報に基づいて、そのいずれの出力態様とするかについても決定する。

自己位置統合部８０が、環境情報に基づいて、
（ａ）複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を選択してツリー更新に適用する標準自己位置を決定する処理。
を実行することを決定した場合は、ステップＳ１１１の処理を実行する。

自己位置統合部８０が、環境情報に基づいて、
（ｂ）複数の自己位置算出器対応の複数の標準自己位置を合成（フュージョン）して、ツリー更新に適用する標準自己位置を生成する処理。
を実行することを決定した場合は、ステップＳ１１２の処理を実行する。

自己位置統合部８０が、環境情報に基づいて、
（ｃ）上記、（ａ），（ｂ）の処理を状況に応じて切り替えて、ツリー更新に適用する標準自己位置を決定する処理。
を実行することを決定した場合は、ステップＳ１１３〜Ｓ１１５の処理を実行する。

（ステップＳ１１１）
自己位置統合部８０が、環境情報に基づいて、
（ａ）複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を選択してツリー更新に適用する標準自己位置を決定する処理。
を実行することを決定した場合は、ステップＳ１１１の処理を実行する。

ステップＳ１１１では、複数の自己位置算出器の標準自己位置から、１つの標準自己位置を選択して、選択標準自己位置（＝装置原点位置）を出力する。すなわち、相対位置ツリー更新情報を出力して、記憶部に格納された相対位置ツリーの更新処理を行う。
具体的には、先に図１２、図１３を参照して説明した相対位置ツリーの更新処理であり、選択標準自己位置（＝装置原点位置）は、装置原点ノード７３の位置情報に相当する。
選択した１つの標準自己位置は、装置原点７３の位置情報、具体的には、自己位置原点７２の位置に対する装置原点７３の相対位置であり、図１３のステップＳ１３中のノード構成中に示すリンクＫに対応する相対位置情報である。
すなわち、ステップＳ１１１では、選択した１つの標準自己位置を、記憶部８２に格納された相対位置ツリーの自己位置原点７２と装置原点７３間のリンクＫ対応の相対位置情報として格納する。

なお、複数の自己位置算出器対応の標準自己位置から１つの標準自己位置を選択する処理は、先に図１７を参照して説明したように複数の処理（（ａ１）〜（ａ４））があり、自己位置統合部８０は、環境情報に応じて処理態様を決定して実行する。

（ステップＳ１１２）
一方、ステップＳ１１０において、自己位置統合部８０が、環境情報に基づいて、
（ｂ）複数の自己位置算出器対応の複数の標準自己位置を合成（フュージョン）して、ツリー更新に適用する標準自己位置を生成する処理。
を実行することを決定した場合は、ステップＳ１１２の処理を実行する。

ステップＳ１１２では、複数の自己位置算出器の標準自己位置を合成（フュージョン）して１つの標準自己位置を算出して出力する。すなわち、合成（フュージョン）標準自己位置を用いて相対位置ツリー更新処理を行う。
この場合、合成（フュージョン）標準自己位置（＝装置原点位置）は、装置原点ノード７３の位置情報に相当する。

合成（フュージョン）標準自己位置は、装置原点７３の位置情報、具体的には、自己位置原点７２の位置に対する装置原点７３の相対位置であり、図１３のステップＳ１３中のノード構成中に示すリンクＫに対応する相対位置情報である。
すなわち、ステップＳ１１２では、合成（フュージョン）標準自己位置を、記憶部８２に格納された相対位置ツリーの自己位置原点７２と装置原点７３間のリンクＫ対応の相対位置情報として格納する。

なお、複数の自己位置算出器対応の標準自己位置から１つの合成（フュージョン）標準自己位置を生成する処理には、先に図１８を参照して説明したように複数の処理（（ｂ１）〜（ｂ２））があり、自己位置統合部８０は、環境情報に応じて処理態様を決定して実行する。

（ステップＳ１１３）
さらに、ステップＳ１１０において、自己位置統合部８０が、環境情報に基づいて、
（ｃ）上記、（ａ），（ｂ）の処理を状況に応じて切り替えて、ツリー更新に適用する標準自己位置を決定する処理。
を実行することを決定した場合は、ステップＳ１１３〜Ｓ１１５の処理を実行する。

ステップＳ１１３では、自己位置統合部８０が、環境情報に基づいて、複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を選択する。
なお、複数の自己位置算出器対応の標準自己位置から１つの標準自己位置を選択する処理は、先に図１７を参照して説明したように複数の処理（（ａ１）〜（ａ４））があり、自己位置統合部８０は、環境情報に応じて処理態様を決定して実行する。

（ステップＳ１１４）
次に、ステップＳ１１４では、自己位置統合部８０が、複数の自己位置算出器対応の複数の標準自己位置の合成処理（フュージョン）を実行して１つの合成標準自己位置を算出する。
なお、複数の自己位置算出器対応の標準自己位置から１つの合成（フュージョン）標準自己位置を生成する処理には、先に図１８を参照して説明したように複数の処理（（ｂ１）〜（ｂ２））があり、自己位置統合部８０は、環境情報に応じて処理態様を決定して実行する。

（ステップＳ１１５）
次に、自己位置統合部８０は、ステップＳ１１３で選択した選択標準自己位置と、ステップＳ１１４で算出した合成標準自己位置を、環境情報に応じて切り替えて出力する。
出力情報は、相対位置ツリー更新情報である。

出力される標準自己位置は、装置原点７３の位置情報、具体的には、自己位置原点７２の位置に対する装置原点７３の相対位置であり、図１３のステップＳ１３中のノード構成中に示すリンクＫに対応する相対位置情報である。
すなわち、ステップＳ１１５では、選択標準自己位置、または合成（フュージョン）標準自己位置を、記憶部８２に格納された相対位置ツリーの自己位置原点７２と装置原点７３間のリンクＫ対応の相対位置情報として格納する。

なお、選択標準自己位置と、合成（フュージョン）標準自己位置との切り替えは、入力する環境情報の変化等に応じて実行する。
具体的には、例えば、先に図１８を参照して説明した（ｃ）の（例１）、（例２）のような処理態様となる。

ステップＳ１１１、またはステップＳ１１２、またはステップＳ１１３〜Ｓ１１５の処理のいずれかが終了すると、ステップＳ１０６に戻る。
ステップＳ１０６では、自己位置算出処理を終了するか否かを判定し、終了する場合は、処理を終了する。
自己位置算出処理を継続する場合は、ステップＳ１０７以下の処理をくりかえし実行する。

ステップＳ１０７以下では、複数の自己位置算出器によって新たに取得された自己位置情報を利用して処理が行われる。
この処理を繰り返すことで、記憶部に格納された相対位置ツリーは、常に最新の状態、すなわち、移動装置の移動位置に応じた位置情報を格納した状態に更新されることになる。

この記憶部に格納された最新の相対位置ツリーの情報は、先に図４を参照て説明したように様々な相対位置利用モジュールによって利用される。
相対位置利用モジュールは、例えば移動装置の移動経路（パス）を決定する行動計画部等である。例えば行動計画部は、記憶部に格納された最新の相対位置ツリーの情報を利用して自己位置を確認し、その後の進路を決定する処理などを実行する。

［５．移動装置の構成例について］
次に、図２１を参照して移動装置の構成例について説明する。
図２１は、上述した処理を実行する移動装置に装着可能な移動体制御システムの一例である車両制御システム１００の概略的な機能の構成例を示すブロック図である。

なお、以下、車両制御システム１００が設けられている車両を他の車両と区別する場合、自車又は自車両と称する。

車両制御システム１００は、入力部１０１、データ取得部１０２、通信部１０３、車内機器１０４、出力制御部１０５、出力部１０６、駆動系制御部１０７、駆動系システム１０８、ボディ系制御部１０９、ボディ系システム１１０、記憶部１１１、及び、自動運転制御部１１２を備える。入力部１０１、データ取得部１０２、通信部１０３、出力制御部１０５、駆動系制御部１０７、ボディ系制御部１０９、記憶部１１１、及び、自動運転制御部１１２は、通信ネットワーク１２１を介して、相互に接続されている。通信ネットワーク１２１は、例えば、ＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ Ａｒｅａ Ｎｅｔｗｏｒｋ）、ＬＩＮ（Ｌｏｃａｌ Ｉｎｔｅｒｃｏｎｎｅｃｔ Ｎｅｔｗｏｒｋ）、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、又は、ＦｌｅｘＲａｙ（登録商標）等の任意の規格に準拠した車載通信ネットワークやバス等からなる。なお、車両制御システム１００の各部は、通信ネットワーク１２１を介さずに、直接接続される場合もある。

なお、以下、車両制御システム１００の各部が、通信ネットワーク１２１を介して通信を行う場合、通信ネットワーク１２１の記載を省略するものとする。例えば、入力部１０１と自動運転制御部１１２が、通信ネットワーク１２１を介して通信を行う場合、単に入力部１０１と自動運転制御部１１２が通信を行うと記載する。

入力部１０１は、搭乗者が各種のデータや指示等の入力に用いる装置を備える。例えば、入力部１０１は、タッチパネル、ボタン、マイクロフォン、スイッチ、及び、レバー等の操作デバイス、並びに、音声やジェスチャ等により手動操作以外の方法で入力可能な操作デバイス等を備える。また、例えば、入力部１０１は、赤外線若しくはその他の電波を利用したリモートコントロール装置、又は、車両制御システム１００の操作に対応したモバイル機器若しくはウェアラブル機器等の外部接続機器であってもよい。入力部１０１は、搭乗者により入力されたデータや指示等に基づいて入力信号を生成し、車両制御システム１００の各部に供給する。

データ取得部１０２は、車両制御システム１００の処理に用いるデータを取得する各種のセンサ等を備え、取得したデータを、車両制御システム１００の各部に供給する。

例えば、データ取得部１０２は、自車の状態等を検出するための各種のセンサを備える。具体的には、例えば、データ取得部１０２は、ジャイロセンサ、加速度センサ、慣性計測装置（ＩＭＵ）、及び、アクセルペダルの操作量、ブレーキペダルの操作量、ステアリングホイールの操舵角、エンジン回転数、モータ回転数、若しくは、車輪の回転速度等を検出するためのセンサ等を備える。

また、例えば、データ取得部１０２は、自車の外部の情報を検出するための各種のセンサを備える。具体的には、例えば、データ取得部１０２は、ＴｏＦ（Ｔｉｍｅ Ｏｆ Ｆｌｉｇｈｔ）カメラ、ステレオカメラ、単眼カメラ、赤外線カメラ、及び、その他のカメラ等の撮像装置を備える。また、例えば、データ取得部１０２は、天候又は気象等を検出するための環境センサ、及び、自車の周囲の物体を検出するための周囲情報検出センサを備える。環境センサは、例えば、雨滴センサ、霧センサ、日照センサ、雪センサ等からなる。周囲情報検出センサは、例えば、超音波センサ、レーダ、ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ、Ｌａｓｅｒ Ｉｍａｇｉｎｇ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）、ソナー等からなる。

さらに、例えば、データ取得部１０２は、自車の現在位置を検出するための各種のセンサを備える。具体的には、例えば、データ取得部１０２は、ＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）衛星からのＧＮＳＳ信号を受信するＧＮＳＳ受信機等を備える。

また、例えば、データ取得部１０２は、車内の情報を検出するための各種のセンサを備える。具体的には、例えば、データ取得部１０２は、運転者を撮像する撮像装置、運転者の生体情報を検出する生体センサ、及び、車室内の音声を集音するマイクロフォン等を備える。生体センサは、例えば、座面又はステアリングホイール等に設けられ、座席に座っている搭乗者又はステアリングホイールを握っている運転者の生体情報を検出する。

また、例えば、データ取得部１０２は、記憶部からデータを取得し、車両制御システム１００の各部に供給する。例えばデータ取得部記憶部から自車の車体構造データを取得し、自己位置推定部などに提供する。

通信部１０３は、車内機器１０４、並びに、車外の様々な機器、サーバ、基地局等と通信を行い、車両制御システム１００の各部から供給されるデータを送信したり、受信したデータを車両制御システム１００の各部に供給したりする。なお、通信部１０３がサポートする通信プロトコルは、特に限定されるものではなく、また、通信部１０３が、複数の種類の通信プロトコルをサポートすることも可能である

例えば、通信部１０３は、無線ＬＡＮ、Ｂｌｕｅｔｏｏｔｈ（登録商標）、ＮＦＣ（Ｎｅａｒ Ｆｉｅｌｄ Ｃｏｍｍｕｎｉｃａｔｉｏｎ）、又は、ＷＵＳＢ（Ｗｉｒｅｌｅｓｓ ＵＳＢ）等により、車内機器１０４と無線通信を行う。また、例えば、通信部１０３は、図示しない接続端子（及び、必要であればケーブル）を介して、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）、ＨＤＭＩ（登録商標）（Ｈｉｇｈ−Ｄｅｆｉｎｉｔｉｏｎ Ｍｕｌｔｉｍｅｄｉａ Ｉｎｔｅｒｆａｃｅ）、又は、ＭＨＬ（Ｍｏｂｉｌｅ Ｈｉｇｈ−ｄｅｆｉｎｉｔｉｏｎ Ｌｉｎｋ）等により、車内機器１０４と有線通信を行う。

さらに、例えば、通信部１０３は、基地局又はアクセスポイントを介して、外部ネットワーク（例えば、インターネット、クラウドネットワーク又は事業者固有のネットワーク）上に存在する機器（例えば、アプリケーションサーバ又は制御サーバ）との通信を行う。また、例えば、通信部１０３は、Ｐ２Ｐ（Ｐｅｅｒ Ｔｏ Ｐｅｅｒ）技術を用いて、自車の近傍に存在する端末（例えば、歩行者若しくは店舗の端末、又は、ＭＴＣ（Ｍａｃｈｉｎｅ Ｔｙｐｅ Ｃｏｍｍｕｎｉｃａｔｉｏｎ）端末）との通信を行う。さらに、例えば、通信部１０３は、車車間（Ｖｅｈｉｃｌｅ ｔｏ Ｖｅｈｉｃｌｅ）通信、路車間（Ｖｅｈｉｃｌｅ ｔｏ Ｉｎｆｒａｓｔｒｕｃｔｕｒｅ）通信、自車と家との間（Ｖｅｈｉｃｌｅ ｔｏ Ｈｏｍｅ）の通信、及び、歩車間（Ｖｅｈｉｃｌｅ ｔｏ Ｐｅｄｅｓｔｒｉａｎ）通信等のＶ２Ｘ通信を行う。また、例えば、通信部１０３は、ビーコン受信部を備え、道路上に設置された無線局等から発信される電波あるいは電磁波を受信し、現在位置、渋滞、通行規制又は所要時間等の情報を取得する。

車内機器１０４は、例えば、搭乗者が有するモバイル機器若しくはウェアラブル機器、自車に搬入され若しくは取り付けられる情報機器、及び、任意の目的地までの経路探索を行うナビゲーション装置等を含む。

出力制御部１０５は、自車の搭乗者又は車外に対する各種の情報の出力を制御する。例えば、出力制御部１０５は、視覚情報（例えば、画像データ）及び聴覚情報（例えば、音声データ）のうちの少なくとも１つを含む出力信号を生成し、出力部１０６に供給することにより、出力部１０６からの視覚情報及び聴覚情報の出力を制御する。具体的には、例えば、出力制御部１０５は、データ取得部１０２の異なる撮像装置により撮像された画像データを合成して、俯瞰画像又はパノラマ画像等を生成し、生成した画像を含む出力信号を出力部１０６に供給する。また、例えば、出力制御部１０５は、衝突、接触、危険地帯への進入等の危険に対する警告音又は警告メッセージ等を含む音声データを生成し、生成した音声データを含む出力信号を出力部１０６に供給する。

出力部１０６は、自車の搭乗者又は車外に対して、視覚情報又は聴覚情報を出力することが可能な装置を備える。例えば、出力部１０６は、表示装置、インストルメントパネル、オーディオスピーカ、ヘッドホン、搭乗者が装着する眼鏡型ディスプレイ等のウェアラブルデバイス、プロジェクタ、ランプ等を備える。出力部１０６が備える表示装置は、通常のディスプレイを有する装置以外にも、例えば、ヘッドアップディスプレイ、透過型ディスプレイ、ＡＲ（Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ）表示機能を有する装置等の運転者の視野内に視覚情報を表示する装置であってもよい。

駆動系制御部１０７は、各種の制御信号を生成し、駆動系システム１０８に供給することにより、駆動系システム１０８の制御を行う。また、駆動系制御部１０７は、必要に応じて、駆動系システム１０８以外の各部に制御信号を供給し、駆動系システム１０８の制御状態の通知等を行う。

駆動系システム１０８は、自車の駆動系に関わる各種の装置を備える。例えば、駆動系システム１０８は、内燃機関又は駆動用モータ等の駆動力を発生させるための駆動力発生装置、駆動力を車輪に伝達するための駆動力伝達機構、舵角を調節するステアリング機構、制動力を発生させる制動装置、ＡＢＳ（Ａｎｔｉｌｏｃｋ Ｂｒａｋｅ Ｓｙｓｔｅｍ）、ＥＳＣ（Ｅｌｅｃｔｒｏｎｉｃ Ｓｔａｂｉｌｉｔｙ Ｃｏｎｔｒｏｌ）、並びに、電動パワーステアリング装置等を備える。

ボディ系制御部１０９は、各種の制御信号を生成し、ボディ系システム１１０に供給することにより、ボディ系システム１１０の制御を行う。また、ボディ系制御部１０９は、必要に応じて、ボディ系システム１１０以外の各部に制御信号を供給し、ボディ系システム１１０の制御状態の通知等を行う。

ボディ系システム１１０は、車体に装備されたボディ系の各種の装置を備える。例えば、ボディ系システム１１０は、キーレスエントリシステム、スマートキーシステム、パワーウィンドウ装置、パワーシート、ステアリングホイール、空調装置、及び、各種ランプ（例えば、ヘッドランプ、バックランプ、ブレーキランプ、ウィンカ、フォグランプ等）等を備える。

記憶部１１１は、例えば、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｃ Ｄｒｉｖｅ）等の磁気記憶デバイス、半導体記憶デバイス、光記憶デバイス、及び、光磁気記憶デバイス等を備える。記憶部１１１は、車両制御システム１００の各部が用いる各種プログラムやデータ等を記憶する。例えば、記憶部１１１は、ダイナミックマップ等の３次元の高精度地図、高精度地図より精度が低く、広いエリアをカバーするグローバルマップ、及び、自車の周囲の情報を含むローカルマップ等の地図データを記憶する。
記憶部１１１は、自車の車体構造データなども記憶する。例えば、自車原点から各センサの相対位置を記憶する。

自動運転制御部１１２は、自律走行又は運転支援等の自動運転に関する制御を行う。具体的には、例えば、自動運転制御部１１２は、自車の衝突回避あるいは衝撃緩和、車間距離に基づく追従走行、車速維持走行、自車の衝突警告、又は、自車のレーン逸脱警告等を含むＡＤＡＳ（Ａｄｖａｎｃｅｄ Ｄｒｉｖｅｒ Ａｓｓｉｓｔａｎｃｅ Ｓｙｓｔｅｍ）の機能実現を目的とした協調制御を行う。また、例えば、自動運転制御部１１２は、運転者の操作に拠らずに自律的に走行する自動運転等を目的とした協調制御を行う。自動運転制御部１１２は、検出部１３１、自己位置推定部１３２、状況分析部１３３、計画部１３４、及び、動作制御部１３５を備える。

検出部１３１は、自動運転の制御に必要な各種の情報の検出を行う。検出部１３１は、車外情報検出部１４１、車内情報検出部１４２、及び、車両状態検出部１４３を備える。

車外情報検出部１４１は、車両制御システム１００の各部からのデータ又は信号に基づいて、自車の外部の情報の検出処理を行う。例えば、車外情報検出部１４１は、自車の周囲の物体の検出処理、認識処理、及び、追跡処理、並びに、物体までの距離の検出処理を行う。検出対象となる物体には、例えば、車両、人、障害物、構造物、道路、信号機、交通標識、道路標示等が含まれる。また、例えば、車外情報検出部１４１は、自車の周囲の環境の検出処理を行う。検出対象となる周囲の環境には、例えば、天候、気温、湿度、明るさ、及び、路面の状態等が含まれる。車外情報検出部１４１は、検出処理の結果を示すデータを自己位置推定部１３２、状況分析部１３３のマップ解析部１５１、交通ルール認識部１５２、及び、状況認識部１５３、並びに、動作制御部１３５の緊急事態回避部１７１等に供給する。

車内情報検出部１４２は、車両制御システム１００の各部からのデータ又は信号に基づいて、車内の情報の検出処理を行う。例えば、車内情報検出部１４２は、運転者の認証処理及び認識処理、運転者の状態の検出処理、搭乗者の検出処理、及び、車内の環境の検出処理等を行う。検出対象となる運転者の状態には、例えば、体調、覚醒度、集中度、疲労度、視線方向等が含まれる。検出対象となる車内の環境には、例えば、気温、湿度、明るさ、臭い等が含まれる。車内情報検出部１４２は、検出処理の結果を示すデータを状況分析部１３３の状況認識部１５３、及び、動作制御部１３５の緊急事態回避部１７１等に供給する。

車両状態検出部１４３は、車両制御システム１００の各部からのデータ又は信号に基づいて、自車の状態の検出処理を行う。検出対象となる自車の状態には、例えば、速度、加速度、舵角、異常の有無及び内容、運転操作の状態、パワーシートの位置及び傾き、ドアロックの状態、並びに、その他の車載機器の状態等が含まれる。車両状態検出部１４３は、検出処理の結果を示すデータを自己位置推定部１３２、状況分析部１３３の状況認識部１５３、及び、動作制御部１３５の緊急事態回避部１７１等に供給する。

自己位置推定部１３２は、自車の自己位置を推定する。自己位置は自車の３次元空間における位置および姿勢を指す。自己位置推定部１３２は、自己位置算出部１８１、および、自己位置統合部１８３を備える。

自己位置算出部１８１は車両状態検出部１４３、車外情報検出部１４１、及び、状況分析部１３３の状況認識部１５３等の車両制御システム１００の各部からのデータ又は信号に基づいて、自車の位置及び姿勢等の推定処理を行う。自己位置算出部１８１は、一つ以上の自己位置算出器１８２を備える。

自己位置算出器１８２は車両状態検出部１４３、車外情報検出部１４１、及び、状況分析部１３３の状況認識部１５３等の車両制御システム１００の各部からのデータ又は信号に基づいて、自車の位置及び姿勢等の推定処理を行う事ができる。自己位置算出器１８２が出力する自己位置を算出器自己位置とする。
自己位置算出器は例えばＧＮＳＳ信号およびＩＭＵから自車の位置姿勢を推定する技術、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）技術、車輪回転数およびステアリング角から自車の位置姿勢を推定するオドメトリ（車輪オドメトリ）技術、ＬｉＤＡＲの観測結果と高精度３次元地図のマッチングによる自己位置同定技術ＮＤＴ（ｎｏｒｍａｌ ｄｉｓｔｒｉｂｕｔｉｏｎｓ ｔｒａｎｓｆｏｒｍ）などがある。
正常に動作している自己位置算出器の数は車外情報検出部、車両状態検出部または状況認識部からのデータまたは信号の種類によって設計時、起動時または実行時に増減することがある。例えばＬｉＤＡＲの入力が取得できるかどうかによってＮＤＴが正常に動作できるかどうかが変わる。
また、自己位置算出器１８２は、必要に応じて、自己位置の推定に用いるローカルマップ（以下、自己位置推定用マップと称する）を生成する。自己位置推定用マップは、例えば、ＳＬＡＭ等の技術を用いた高精度なマップとされる。自己位置算出器１８２は、自己位置推定用マップを記憶部１１１に記憶させる。

自己位置統合部１８２は一つ以上の自己位置算出器からの算出器自己位置を統合手法によって統合した結果の自己位置を出力する。自己位置統合部が出力する自己位置を統合自己位置とする。
自己位置統合部１８２は、状況分析部１３３から環境情報を入力する。例えば、移動装置の外部状況である明るさや視界等の外部状況や、各センサの稼働状況、故障状況、あるいはリソース利用状況等の環境情報を入力し、この環境情報に応じて決定した統合手法を適用して、１つの自己位置を算出する。
統合手法は複数の自己位置算出器の算出した自己位置を統合して統合自己位置を計算する手法である。例えば条件に応じて１つの自己位置算出器の算出した自己位置に基づいて算出した標準自己位置を選択する処理や、複数の自己位置算出器の算出した自己位置に基づいて算出した標準自己位置を合成（フュージョン）する処理などがある。詳細は、先に図１７、図１８を参照して説明した通りである。
自己位置統合部１８２は、統合自己位置を示すデータを状況分析部１３３のマップ解析部１５１、交通ルール認識部１５２、及び、状況認識部１５３等に供給する。

状況分析部１３３は、自車及び周囲の状況の分析処理を行う。状況分析部１３３は、マップ解析部１５１、交通ルール認識部１５２、状況認識部１５３、及び、状況予測部１５４を備える。

マップ解析部１５１は、自己位置推定部１３２及び車外情報検出部１４１等の車両制御システム１００の各部からのデータ又は信号を必要に応じて用いながら、記憶部１１１に記憶されている各種のマップの解析処理を行い、自動運転の処理に必要な情報を含むマップを構築する。マップ解析部１５１は、構築したマップを、交通ルール認識部１５２、状況認識部１５３、状況予測部１５４、並びに、計画部１３４のルート計画部１６１、行動計画部１６２、及び、動作計画部１６３等に供給する。

交通ルール認識部１５２は、自己位置推定部１３２、車外情報検出部１４１、及び、マップ解析部１５１等の車両制御システム１００の各部からのデータ又は信号に基づいて、自車の周囲の交通ルールの認識処理を行う。この認識処理により、例えば、自車の周囲の信号の位置及び状態、自車の周囲の交通規制の内容、並びに、走行可能な車線等が認識される。交通ルール認識部１５２は、認識処理の結果を示すデータを状況予測部１５４等に供給する。

状況認識部１５３は、自己位置推定部１３２、車外情報検出部１４１、車内情報検出部１４２、車両状態検出部１４３、及び、マップ解析部１５１等の車両制御システム１００の各部からのデータ又は信号に基づいて、自車に関する状況の認識処理を行う。例えば、状況認識部１５３は、自車の状況、自車の周囲の状況、及び、自車の運転者の状況等の認識処理を行う。また、状況認識部１５３は、必要に応じて、自車の周囲の状況の認識に用いるローカルマップ（以下、状況認識用マップと称する）を生成する。状況認識用マップは、例えば、占有格子地図（Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄ Ｍａｐ）とされる。

認識対象となる自車の状況には、例えば、自車の位置、姿勢、動き（例えば、速度、加速度、移動方向等）、並びに、異常の有無及び内容等が含まれる。認識対象となる自車の周囲の状況には、例えば、周囲の静止物体の種類及び位置、周囲の動物体の種類、位置及び動き（例えば、速度、加速度、移動方向等）、周囲の道路の構成及び路面の状態、並びに、周囲の天候、気温、湿度、及び、明るさ等が含まれる。認識対象となる運転者の状態には、例えば、体調、覚醒度、集中度、疲労度、視線の動き、並びに、運転操作等が含まれる。

状況認識部１５３は、認識処理の結果を示すデータ（必要に応じて、状況認識用マップを含む）を自己位置推定部１３２及び状況予測部１５４等に供給する。また、状況認識部１５３は、状況認識用マップを記憶部１１１に記憶させる。

状況予測部１５４は、マップ解析部１５１、交通ルール認識部１５２及び状況認識部１５３等の車両制御システム１００の各部からのデータ又は信号に基づいて、自車に関する状況の予測処理を行う。例えば、状況予測部１５４は、自車の状況、自車の周囲の状況、及び、運転者の状況等の予測処理を行う。

予測対象となる自車の状況には、例えば、自車の挙動、異常の発生、及び、走行可能距離等が含まれる。予測対象となる自車の周囲の状況には、例えば、自車の周囲の動物体の挙動、信号の状態の変化、及び、天候等の環境の変化等が含まれる。予測対象となる運転者の状況には、例えば、運転者の挙動及び体調等が含まれる。

状況予測部１５４は、予測処理の結果を示すデータを、交通ルール認識部１５２及び状況認識部１５３からのデータとともに、計画部１３４のルート計画部１６１、行動計画部１６２、及び、動作計画部１６３等に供給する。

ルート計画部１６１は、マップ解析部１５１及び状況予測部１５４等の車両制御システム１００の各部からのデータ又は信号に基づいて、目的地までのルートを計画する。例えば、ルート計画部１６１は、グローバルマップに基づいて、現在位置から指定された目的地までのルートを設定する。また、例えば、ルート計画部１６１は、渋滞、事故、通行規制、工事等の状況、及び、運転者の体調等に基づいて、適宜ルートを変更する。ルート計画部１６１は、計画したルートを示すデータを行動計画部１６２等に供給する。

行動計画部１６２は、マップ解析部１５１及び状況予測部１５４等の車両制御システム１００の各部からのデータ又は信号に基づいて、ルート計画部１６１により計画されたルートを計画された時間内で安全に走行するための自車の行動を計画する。例えば、行動計画部１６２は、発進、停止、進行方向（例えば、前進、後退、左折、右折、方向転換等）、走行車線、走行速度、及び、追い越し等の計画を行う。行動計画部１６２は、計画した自車の行動を示すデータを動作計画部１６３等に供給する

動作計画部１６３は、マップ解析部１５１及び状況予測部１５４等の車両制御システム１００の各部からのデータ又は信号に基づいて、行動計画部１６２により計画された行動を実現するための自車の動作を計画する。例えば、動作計画部１６３は、加速、減速、及び、走行軌道等の計画を行う。動作計画部１６３は、計画した自車の動作を示すデータを、動作制御部１３５の加減速制御部１７２及び方向制御部１７３等に供給する。

動作制御部１３５は、自車の動作の制御を行う。動作制御部１３５は、緊急事態回避部１７１、加減速制御部１７２、及び、方向制御部１７３を備える。

緊急事態回避部１７１は、車外情報検出部１４１、車内情報検出部１４２、及び、車両状態検出部１４３の検出結果に基づいて、衝突、接触、危険地帯への進入、運転者の異常、車両の異常等の緊急事態の検出処理を行う。緊急事態回避部１７１は、緊急事態の発生を検出した場合、急停車や急旋回等の緊急事態を回避するための自車の動作を計画する。緊急事態回避部１７１は、計画した自車の動作を示すデータを加減速制御部１７２及び方向制御部１７３等に供給する。

加減速制御部１７２は、動作計画部１６３又は緊急事態回避部１７１により計画された自車の動作を実現するための加減速制御を行う。例えば、加減速制御部１７２は、計画された加速、減速、又は、急停車を実現するための駆動力発生装置又は制動装置の制御目標値を演算し、演算した制御目標値を示す制御指令を駆動系制御部１０７に供給する。

方向制御部１７３は、動作計画部１６３又は緊急事態回避部１７１により計画された自車の動作を実現するための方向制御を行う。例えば、方向制御部１７３は、動作計画部１６３又は緊急事態回避部１７１により計画された走行軌道又は急旋回を実現するためのステアリング機構の制御目標値を演算し、演算した制御目標値を示す制御指令を駆動系制御部１０７に供給する。

［６．情報処理装置の構成例について］
図２１は、上述した処理を実行する移動装置内に装着可能な移動体制御システムの一例である車両制御システム１００の構成であるが、先に説明した実施例に従った処理は、例えば複数の自己位置算出器対応のセンサ、例えばカメラ等の様々なセンサの検出情報をＰＣ等の情報処理装置に入力してデータ処理を行い、相対位置ツリーの更新情報を生成して情報処理装置内の記憶部に格納された相対位置ツリーの更新を行う構成とすることも可能である。
この場合の情報処理装置の具体的なハードウェア構成例について、図２２を参照して説明する。

図２２は、一般的なＰＣ等の情報処理装置のハードウェア構成例を示す図である。
ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）３０１は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）３０２、または記憶部３０８に記憶されているプログラムに従って各種の処理を実行するデータ処理部として機能する。例えば、上述した実施例において説明したシーケンスに従った処理を実行する。ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）３０３には、ＣＰＵ３０１が実行するプログラムやデータなどが記憶される。これらのＣＰＵ３０１、ＲＯＭ３０２、およびＲＡＭ３０３は、バス３０４により相互に接続されている。

ＣＰＵ３０１はバス３０４を介して入出力インタフェース３０５に接続され、入出力インタフェース３０５には、各種スイッチ、キーボード、タッチパネル、マウス、マイクロフォン、さらに、センサ、カメラ、ＧＰＳ等の状況データ取得部などよりなる入力部３０６、ディスプレイ、スピーカなどよりなる出力部３０７が接続されている。
なお、入力部３０６には、センサ３２１からの入力情報も入力される。
また、出力部３０７は、移動装置の駆動部３２２に対する駆動情報も出力する。

ＣＰＵ３０１は、入力部３０６から入力される指令や状況データ等を入力し、各種の処理を実行し、処理結果を例えば出力部３０７に出力する。
入出力インタフェース３０５に接続されている記憶部３０８は、例えばハードディスク等からなり、ＣＰＵ３０１が実行するプログラムや各種のデータを記憶する。通信部３０９は、インターネットやローカルエリアネットワークなどのネットワークを介したデータ通信の送受信部として機能し、外部の装置と通信する。

入出力インタフェース３０５に接続されているドライブ１３０は、磁気ディスク、光ディスク、光磁気ディスク、あるいはメモリカード等の半導体メモリなどのリムーバブルメディア３１１を駆動し、データの記録あるいは読み取りを実行する。

［７．本開示の構成のまとめ］
以上、特定の実施例を参照しながら、本開示の実施例について詳解してきた。しかしながら、本開示の要旨を逸脱しない範囲で当業者が実施例の修正や代用を成し得ることは自明である。すなわち、例示という形態で本発明を開示してきたのであり、限定的に解釈されるべきではない。本開示の要旨を判断するためには、特許請求の範囲の欄を参酌すべきである。

なお、本明細書において開示した技術は、以下のような構成をとることができる。
（１） 自己位置を算出する複数の自己位置算出器と、
前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出する自己位置統合部を有し、
前記自己位置統合部は、
前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する情報処理装置。

（２） 前記自己位置統合部は、
環境情報に応じて、前記複数の標準自己位置から前記最終自己位置の算出態様を決定する（１）に記載の情報処理装置。

（３） 前記環境情報は、
前記最終自己位置を適用して決定する移動経路に従った移動を行う移動装置の外部環境、複数の自己位置算出器の利用センサの故障情報、リソースの利用状況の少なくともいずれかの情報を含む（２）に記載の情報処理装置。

（４） 前記自己位置統合部は、
環境情報に応じて、複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を選択し、選択した標準自己位置を前記最終自己位置とする（１）〜（３）いずれかに記載の情報処理装置。

（５） 前記自己位置統合部は、
環境情報に応じて、複数の自己位置算出器対応の複数の標準自己位置を合成して１つの標準自己位置を算出し、算出した合成標準自己位置を前記最終自己位置とする（１）〜（４）いずれかに記載の情報処理装置。

（６） 前記自己位置統合部は、
環境情報に応じて、複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を選択して選択標準自己位置を決定し、
さらに、環境情報に応じて、複数の自己位置算出器対応の複数の標準自己位置を合成して１つの合成標準自己位置を算出し、
さらに、環境情報に応じて、前記選択標準自己位置と、前記合成標準自己位置を切り替えて前記最終自己位置とする（１）〜（５）いずれかに記載の情報処理装置。

（７） 前記情報処理装置は、さらに、
複数の異なる定義の座標原点、またはオブジェクト位置から構成されるノード間の相対位置を記録した相対位置ツリーを格納した記憶部を有し、
前記自己位置統合部は、
前記前記最終自己位置を、前記相対位置ツリーの更新情報として算出する（１）〜（６）いずれかに記載の情報処理装置。

（８） 前記相対位置ツリーは、
前記複数の自己位置算出器を装着した移動装置の移動に伴って移動する自己位置算出器対応のセンサ位置情報を有する自己位置算出器対応センサノードと、前記移動装置の移動に伴って移動しない位置情報を有する自己位置算出器原点ノードを有し、各ノード間の相対位置をリンクデータとして有する相対位置ツリーである（７）に記載の情報処理装置。

（９） 前記相対位置ツリーは、
前記移動装置の装置原点位置を示す１つの装置原点ノードを有し、
前記複数の自己位置算出器各々に対応する複数の自己位置算出器対応センサノードの各々は、前記装置原点ノードとの相対位置を示すリンクによって接続された構成である（８）に記載の情報処理装置。

（１０） 前記自己位置統合部は、
前記最終自己位置を、前記相対位置ツリーに含まれる装置原点位置の更新情報として算出する（９）に記載の情報処理装置。

（１１） 自己位置を算出する複数の自己位置算出器と、
前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出する自己位置統合部と、
自己位置統合部の算出した最終自己位置を利用して移動装置の行動を決定する計画部と、
前記計画部の決定した行動に従って移動装置の動作を制御する動作制御部を有し、
前記自己位置統合部は、
前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する移動装置。

（１２） 前記自己位置統合部は、
環境情報に応じて、前記複数の標準自己位置から前記最終自己位置の算出態様を決定する（１１）に記載の移動装置。

（１３） 前記環境情報は、
前記最終自己位置を適用して決定する移動経路に従った移動を行う移動装置の外部環境、複数の自己位置算出器の利用センサの故障情報、リソースの利用状況の少なくともいずれかの情報を含む（１２）に記載の移動装置。

（１４） 前記自己位置統合部は、
環境情報に応じて、複数の自己位置算出器対応の複数の標準自己位置から選択した１つの選択標準自己位置、または複数の自己位置算出器対応の複数の標準自己位置を合成した合成標準自己位置のいずれかを前記最終自己位置とする（１１）〜（１３）いずれかに記載の移動装置。

（１５） 前記移動装置は、さらに、
複数の異なる定義の座標原点、またはオブジェクト位置から構成されるノード間の相対位置を記録した相対位置ツリーを格納した記憶部を有し、
前記自己位置統合部は、
前記前記最終自己位置を、前記相対位置ツリーの更新情報として算出する（１１）〜（１４）いずれかに記載の移動装置。

（１６） 前記相対位置ツリーは、
前記複数の自己位置算出器を装着した移動装置の移動に伴って移動する自己位置算出器対応のセンサ位置情報を有する自己位置算出器対応センサノードと、前記移動装置の移動に伴って移動しない位置情報を有する自己位置算出器原点ノードを有し、各ノード間の相対位置をリンクデータとして有する相対位置ツリーである（１５）に記載の移動装置。

（１７） 情報処理装置において実行する情報処理方法であり、
複数の自己位置算出器の各々が、自己位置を算出する複数の自己位置算出ステップと、
自己位置統合部が、前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出する自己位置統合ステップを有し、
前記自己位置統合ステップは、
前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出するステップである情報処理方法。

（１８） 移動装置において実行する移動装置制御方法であり、
複数の自己位置算出器の各々が、自己位置を算出する複数の自己位置算出ステップと、
自己位置統合部が、前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出する自己位置統合ステップと、
計画部が、自己位置統合部の算出した最終自己位置を利用して移動装置の行動を決定する計画ステップと、
動作制御部が、前記計画部の決定した行動に従って移動装置の動作を制御する動作制御ステップを有し、
前記自己位置統合ステップは、
前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する移動装置制御方法。

（１９） 情報処理装置において情報処理を実行させるプログラムであり、
複数の自己位置算出器の各々に、自己位置を算出させる複数の自己位置算出ステップと、
自己位置統合部に、前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出させる自己位置統合ステップを実行させ、
前記自己位置統合ステップにおいて、
前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する処理を実行させるプログラム。

（２０） 移動装置において移動装置制御処理を実行させるプログラムであり、
複数の自己位置算出器の各々に、自己位置を算出させる複数の自己位置算出ステップと、
自己位置統合部に、前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出させる自己位置統合ステップと、
計画部に、自己位置統合部の算出した最終自己位置を利用して移動装置の行動を決定させる計画ステップと、
動作制御部に、前記計画部の決定した行動に従って移動装置の動作を制御させる動作制御ステップを実行させ、
前記自己位置統合ステップにおいて、
前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する処理を実行させるプログラム。

また、明細書中において説明した一連の処理はハードウェア、またはソフトウェア、あるいは両者の複合構成によって実行することが可能である。ソフトウェアによる処理を実行する場合は、処理シーケンスを記録したプログラムを、専用のハードウェアに組み込まれたコンピュータ内のメモリにインストールして実行させるか、あるいは、各種処理が実行可能な汎用コンピュータにプログラムをインストールして実行させることが可能である。例えば、プログラムは記録媒体に予め記録しておくことができる。記録媒体からコンピュータにインストールする他、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、インターネットといったネットワークを介してプログラムを受信し、内蔵するハードディスク等の記録媒体にインストールすることができる。

なお、明細書に記載された各種の処理は、記載に従って時系列に実行されるのみならず、処理を実行する装置の処理能力あるいは必要に応じて並列的にあるいは個別に実行されてもよい。また、本明細書においてシステムとは、複数の装置の論理的集合構成であり、各構成の装置が同一筐体内にあるものには限らない。

以上、説明したように、本開示の一実施例の構成によれば、自己位置を算出する複数の自己位置算出器の算出自己位置に基づいて最終的に１つの装置位置情報を取得可能とした構成が実現される。
具体的には、例えば、自己位置を算出する複数の自己位置算出器と、複数の自己位置算出器の算出自己位置を統合して１つの最終自己位置を算出する自己位置統合部を有する。自己位置統合部は、複数の自己位置算出器対応の算出自己位置を、各算出器のセンサ位置を考慮して標準自己位置に変換し、複数の標準自己位置から最終自己位置を算出する。自己位置統合部は、移動装置の外部環境、複数の自己位置算出器の利用センサの故障情報、リソースの利用状況等の環境情報に応じて、最終自己位置を算出する。
本構成により、自己位置を算出する複数の自己位置算出器の算出自己位置に基づいて最終的に１つの装置位置情報を取得可能とした構成が実現される。

１０・・移動装置，２１・・地図原点，２２・・自己位置原点，２３・・装置原点，３１，３２，３３・・自己位置算出器，４１，４２・・相対位置ツリー更新モジュール，４３・・記憶部，４４，４５，４６・・相対位置ツリー利用モジュール，４７，４８・・相対位置ツリー更新モジュール，５１・・地図原点，５２・・自己位置原点，５３・・装置原点，５４・・カメラ，５５・・車輪中心，５６，５７・・相対位置ツリー更新モジュール，７１・・地図原点，７２・・自己位置原点，７３・・装置原点，７４・・カメラ，７５・・車輪中心，７６・・自己位置算出器Ｐ原点，７７・・自己位置算出器Ｑ原点，７８，７９・・相対位置ツリー更新モジュール，８０・・自己位置統合部，８２・・記憶部，８３・・状況分析部，１００・・車両制御システム，１０１・・入力部，１０２・・データ取得部，１０３・・通信部，１０４・・車内機器，１０５・・出力制御部，１０６・・出力部，１０７・・駆動系制御部，１０８・・駆動系システム，１０９・・ボディ系制御部，１１０・・ボディ系システム，１１１・・記憶部，１１２・・自動運転制御部，１２１・・通信ネットワーク，１３１・・検出部，１３２・・自己位置推定部，１３３・・状況分析部，１３４・・計画部，１３５・・動作制御部，１４１・・車外情報検出部，１４２・・車内情報検出部，１４３・・車両状態検出部，１５１・・マップ解析部，１５２・・交通ルール認識部，１５３・・状況認識部，１５４・・状況予測部，１６１・・ルート計画部，１６２・・行動計画部，１６３・・動作計画部，１７１・・緊急事態回避部，１７２・・加減速制御部，１７３・・方向制御部，１８１・・自己位置算出部，１８２・・自己位置算出器，１８３・・自己位置統合部，３０１・・ＣＰＵ，３０２・・ＲＯＭ，３０３・・ＲＡＭ，３０４・・バス，３０５・・入出力インタフェース，３０６・・入力部，３０７・・出力部，３０８・・記憶部，３０９・・通信部，３１０・・ドライブ，３１１・・リムーバブルメディア，３２１・・センサ，３２２・・駆動部

Claims (20)

1. 自己位置を算出する複数の自己位置算出器と、
   前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出する自己位置統合部を有し、
   前記自己位置統合部は、
   前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する情報処理装置。
2. 前記自己位置統合部は、
   環境情報に応じて、前記複数の標準自己位置から前記最終自己位置の算出態様を決定する請求項１に記載の情報処理装置。
3. 前記環境情報は、
   前記最終自己位置を適用して決定する移動経路に従った移動を行う移動装置の外部環境、複数の自己位置算出器の利用センサの故障情報、リソースの利用状況の少なくともいずれかの情報を含む請求項２に記載の情報処理装置。
4. 前記自己位置統合部は、
   環境情報に応じて、複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を選択し、選択した標準自己位置を前記最終自己位置とする請求項１に記載の情報処理装置。
5. 前記自己位置統合部は、
   環境情報に応じて、複数の自己位置算出器対応の複数の標準自己位置を合成して１つの標準自己位置を算出し、算出した合成標準自己位置を前記最終自己位置とする請求項１に記載の情報処理装置。
6. 前記自己位置統合部は、
   環境情報に応じて、複数の自己位置算出器対応の複数の標準自己位置から１つの標準自己位置を選択して選択標準自己位置を決定し、
   さらに、環境情報に応じて、複数の自己位置算出器対応の複数の標準自己位置を合成して１つの合成標準自己位置を算出し、
   さらに、環境情報に応じて、前記選択標準自己位置と、前記合成標準自己位置を切り替えて前記最終自己位置とする請求項１に記載の情報処理装置。
7. 前記情報処理装置は、さらに、
   複数の異なる定義の座標原点、およびオブジェクト位置との相対位置を記録した相対位置ツリーを格納した記憶部を有し、
   前記自己位置統合部は、
   前記前記最終自己位置を、前記相対位置ツリーの更新情報として算出する請求項１に記載の情報処理装置。
8. 前記相対位置ツリーは、
   前記複数の自己位置算出器を装着した移動装置の移動に伴って移動する自己位置算出器対応のセンサ位置情報を有する自己位置算出器対応センサノードと、前記移動装置の移動に伴って移動しない位置情報を有する自己位置算出器原点ノードを有し、各ノード間の相対位置をリンクデータとして有する相対位置ツリーである請求項７に記載の情報処理装置。
9. 前記相対位置ツリーは、
   前記移動装置の装置原点位置を示す１つの装置原点ノードを有し、
   前記複数の自己位置算出器各々に対応する複数の自己位置算出器対応センサノードの各々は、前記装置原点ノードとの相対位置を示すリンクによって接続された構成である請求項８に記載の情報処理装置。
10. 前記自己位置統合部は、
    前記最終自己位置を、前記相対位置ツリーに含まれる装置原点位置の更新情報として算出する請求項９に記載の情報処理装置。
11. 自己位置を算出する複数の自己位置算出器と、
    前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出する自己位置統合部と、
    自己位置統合部の算出した最終自己位置を利用して移動装置の行動を決定する計画部と、
    前記計画部の決定した行動に従って移動装置の動作を制御する動作制御部を有し、
    前記自己位置統合部は、
    前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する移動装置。
12. 前記自己位置統合部は、
    環境情報に応じて、前記複数の標準自己位置から前記最終自己位置の算出態様を決定する請求項１１に記載の移動装置。
13. 前記環境情報は、
    前記最終自己位置を適用して決定する移動経路に従った移動を行う移動装置の外部環境、複数の自己位置算出器の利用センサの故障情報、リソースの利用状況の少なくともいずれかの情報を含む請求項１２に記載の移動装置。
14. 前記自己位置統合部は、
    環境情報に応じて、複数の自己位置算出器対応の複数の標準自己位置から選択した１つの選択標準自己位置、または複数の自己位置算出器対応の複数の標準自己位置を合成した合成標準自己位置のいずれかを前記最終自己位置とする請求項１１に記載の移動装置。
15. 前記移動装置は、さらに、
    複数の異なる定義の座標原点、またはオブジェクト位置から構成されるノード間の相対位置を記録した相対位置ツリーを格納した記憶部を有し、
    前記自己位置統合部は、
    前記前記最終自己位置を、前記相対位置ツリーの更新情報として算出する請求項１１に記載の移動装置。
16. 前記相対位置ツリーは、
    前記複数の自己位置算出器を装着した移動装置の移動に伴って移動する自己位置算出器対応のセンサ位置情報を有する自己位置算出器対応センサノードと、前記移動装置の移動に伴って移動しない位置情報を有する自己位置算出器原点ノードを有し、各ノード間の相対位置をリンクデータとして有する相対位置ツリーである請求項１５に記載の移動装置。
17. 情報処理装置において実行する情報処理方法であり、
    複数の自己位置算出器の各々が、自己位置を算出する複数の自己位置算出ステップと、
    自己位置統合部が、前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出する自己位置統合ステップを有し、
    前記自己位置統合ステップは、
    前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出するステップである情報処理方法。
18. 移動装置において実行する移動装置制御方法であり、
    複数の自己位置算出器の各々が、自己位置を算出する複数の自己位置算出ステップと、
    自己位置統合部が、前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出する自己位置統合ステップと、
    計画部が、自己位置統合部の算出した最終自己位置を利用して移動装置の行動を決定する計画ステップと、
    動作制御部が、前記計画部の決定した行動に従って移動装置の動作を制御する動作制御ステップを有し、
    前記自己位置統合ステップは、
    前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する移動装置制御方法。
19. 情報処理装置において情報処理を実行させるプログラムであり、
    複数の自己位置算出器の各々に、自己位置を算出させる複数の自己位置算出ステップと、
    自己位置統合部に、前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出させる自己位置統合ステップを実行させ、
    前記自己位置統合ステップにおいて、
    前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する処理を実行させるプログラム。
20. 移動装置において移動装置制御処理を実行させるプログラムであり、
    複数の自己位置算出器の各々に、自己位置を算出させる複数の自己位置算出ステップと、
    自己位置統合部に、前記複数の自己位置算出器の算出した算出自己位置を統合して１つの最終自己位置を算出させる自己位置統合ステップと、
    計画部に、自己位置統合部の算出した最終自己位置を利用して移動装置の行動を決定させる計画ステップと、
    動作制御部に、前記計画部の決定した行動に従って移動装置の動作を制御させる動作制御ステップを実行させ、
    前記自己位置統合ステップにおいて、
    前記複数の自己位置算出器の算出した自己位置算出器対応の算出自己位置を、各自己位置算出器の利用センサのセンサ位置を考慮して標準自己位置に変換し、変換結果である複数の標準自己位置を利用して前記最終自己位置を算出する処理を実行させるプログラム。

Images