WO2016199338A1 - 移動体の位置姿勢推定装置及び移動体の自律走行システム - Google Patents

Abstract

移動体の位置姿勢推定装置に於いて、前回の第１検出時点でのセンシングデータである環境地図に基づいて次の第２検出時点で可能となる位置姿勢を示す複数の運動候補を設定する。照合部（３０）は、第２検出時点でのセンシングデータと運動候補のそれぞれとを照合することにより最大評価値となる運動候補を推定し、環境地図が示す位置姿勢から最大評価値の運動候補が示す位置姿勢に至る位置姿勢変化量を特定する。この場合に、照合部を構成する複数の分割照合部（３０ａ～３０ｃ）は、互いに異なる第１変数を変化させる一方、他の変数である第２変数は固定しておき、最大評価値となる運動候補を推定する。

Description

移動体の位置姿勢推定装置及び移動体の自律走行システム 関連出願の相互参照

　本出願は、２０１５年６月１２日に出願された日本出願番号２０１５－１１９２９８号と、２０１５年１２月２４日に出願された日本出願番号２０１５－２５１４９９号に基づくもので、ここにその記載内容を援用する。

　本開示は、環境地図に基づいて設定した運動候補とセンシングデータとを照合することにより移動体の現在の位置姿勢を推測する移動体の位置姿勢推定装置及び移動体の自律走行システムに関する。

　車両やロボット等の移動体の現在の位置姿勢を推定する際には、一般的には、前回の検出時点からの移動体の位置姿勢変化量として位置（ｘ，ｙ）と姿勢θの３種類の変数（ｘ，ｙ，θ）を特定する必要がある。この３種類の変数の変化量を求めるには、外部環境に対する移動体の相対的な位置姿勢を例えば測距センサにより検出し、測距センサから取得したセンシングデータと前回の検出時点でのセンシングデータである環境地図との照合を実施する。

　照合方法としては、環境地図に基づいて次の検出時点での移動体の位置姿勢を示す運動候補を設定し、その運動候補とセンシングデータとを照合することで行われる。運動候補は次の検出時点で可能となる位置姿勢であり、環境地図を所定の（ｘ，ｙ）だけ並行移動させると共に所定のθだけ回転させて設定する。このような照合を運動候補の数だけ実施し、照合精度を示す評価値が最大となる運動候補を推定することで現在の位置姿勢を特定する。

ＪＰ　２００９－１０９２００　Ａ

　上記のような照合方法により最大評価値となる運動候補を推定する場合、運動候補が３種類の変数（ｘ，ｙ，θ）から構成されるため、環境地図に基づいて設定する運動候補の数が膨大となり、照合の処理量が大きいという課題がある。

　このような課題に対して、特許文献１では、環境地図を作成する対策と位置姿勢を推定する対策とを分離し、位置姿勢を推定する対策での処理量を減らす方法を提案している。また、環境地図の作成では、センシングデータに基づいて有効画素のみ抽出しているため、位置姿勢を推定する対策では、無効画素の判定を省略することで照合の処理量を低減している。

　しかしながら、特許文献１では、全ての運動候補と照合する必要があるため、照合の処理時間が膨大になる欠点がある。また、処理量に関しても、環境地図を作成する処理では、無効画素を判定する処理が存在するため、それらのデータ処理を含めた全体の処理量を大きく低減することはできない。さらに、環境地図の作成を外部のコンピュータで実施して通信により取得する構成なので、外部のコンピュータとの通信時間が発生し、この点でも処理時間を要する。

　本開示の目的は、環境地図に基づいて設定した運動候補とセンシングデータとを照合することにより移動体の現在の位置姿勢を推測する場合に、移動体の位置姿勢を推定するまでの処理時間を大幅に低減することができる移動体の位置姿勢推定装置及び移動体の自律走行システムを提供することにある。

　本開示の一つの観点によれば、移動体の位置姿勢推定装置は、検出部、記憶部、運動候補設定部、照合部、推定値出力部、とを備えるように構成される。検出部は、所定の連続する検出時点の各時点で外部環境に対する移動体の相対的な位置姿勢を示す複数の変数から構成されるセンシングデータを検出する。記憶部は、前回の検出時点である第１時点のセンシングデータを複数の変数から構成される第１時点に対応する環境地図として記憶する。運動候補設定部は、第１時点に対応する環境地図に基づいて、第１時点の次の検出の時点である第２時点で移動体が可能となる位置姿勢を示す、各々が複数の変数から構成される複数の運動候補を設定する。照合部は、第２時点でのセンシングデータと運動候補のそれぞれとを照合し、照合精度を示す評価値が最大である最大評価値となる最大評価運動候補を推定する。推定値出力部は、第１時点に対応する環境地図が示す位置姿勢から最大評価運動候補が示す位置姿勢に至る変化量を位置姿勢推定値として出力する。さらに、照合部は、複数の分割照合部から構成される。分割照合部の各々は、運動候補を構成する複数の変数の内、少なくとも一つを第１変数として選択し、その他全てを第２変数として選択し、ここで、分割照合部の各々が選択する第１変数は互いに異なるように選択されている。分割照合部の各々は、第２変数として所定の初期値を固定値として設定し変化させない状態で第１変数を変化させて評価値を求めることにより最大評価値となる第１変数を推定する。推定値出力部は、分割照合部がそれぞれ推定した最大評価値となる第１変数を位置姿勢推定値として出力する。

　上記の構成によれば、位置姿勢推定値を累積して合算することで車両の走行軌跡を特定することができる。

　本開示についての上記目的およびその他の目的、特徴や利点は、添付の図面を参照しながら下記の詳細な記述により、より明確になる。
第１実施形態における自律走行システムを示す機能ブロック図 測距センサ部を示す機能ブロック図 照合部の構成を示す機能ブロック図 外部環境に対する測距センサの位置姿勢を示す図 センシングデータと環境地図との関係を示す図 最大評価値となる運動候補を推定する第１実施形態における手順を示す図 最大評価値となる運動候補を推定する手順を示す比較例を示す図 ｘ1を中心とする正規分布を示す図 センシングデータと環境地図との照合手順を示す図 第２実施形態における照合部の構成を示す機能ブロック図 第３実施形態における照合部の構成を示す機能ブロック図 第４実施形態における照合部の構成を示す機能ブロック図 最大評価値となる運動候補を推定する手順を示す図（その１） 最大評価値となる運動候補を推定する手順を示す図（その２） 最大評価値となる運動候補を推定する手順を示す図（その３） バイナリサーチの手順を示す図 第５実施形態における照合部の構成を示す機能ブロック図 運動候補と評価値との関係を示す図 運動候補と評価値との関係を示す図 推定値を確定する方法を示すフローチャート 第６実施形態における照合部の構成を示す機能ブロック図 第７実施形態における照合部の構成を示す機能ブロック図 最大評価値となる運動候補を推定する手順を示す図（その１） 最大評価値となる運動候補を推定する手順を示す図（その２） 最大評価値となる運動候補を推定する手順を示す図（その３） 第８実施形態における最大評価値となる運動候補を推定する手順を示す図（その１） 第８実施形態における最大評価値となる運動候補を推定する手順を示す図（その２） 第８実施形態における最大評価値となる運動候補を推定する手順を示す図（その３） 第９実施形態における最大評価値となる運動候補を推定する手順を示す図（その１） 第９実施形態における最大評価値となる運動候補を推定する手順を示す図（その２） 第９実施形態における最大評価値となる運動候補を推定する手順を示す図（その３） 第１０実施形態における照合部の構成を示す機能ブロック図 第１１実施形態における照合部の構成を示す機能ブロック図 第１２実施形態における照合部の構成を示す機能ブロック図 運動候補と評価値との関係を示す図 評価値分布の立上りまたは立下りの傾斜角度を求めるための方法を示す図

　（第１実施形態）
　第１実施形態について図１から図８を参照して説明する。

　図１に示すように、車両には自律走行システム１が搭載されている。この車両は、搭載車、当該車、あるいは自車(Ｈｏｓｔ　Ｖｅｈｉｃｌｅ)とも言及される。自律走行システム１は、位置姿勢推定装置を構成する外部環境取得部２、走行環境認識部３及びインフラ情報保存部４と、自律走行装置を構成する行動選択部５、車両制御部６及び経路計画部７とから構成されている。尚、本開示において、自律走行システム１を構成する上記の各部は、後述するように、さらに分割されて構成される部、あるいは組合わさって構成される部として提供される。これらの部の各々は、(i)ハードウエアユニット（例えば、ＣＰＵ，ＲＡＭ，ＲＯＭ等を含むコンピュータ）と組み合わさったソフトウエアの部として、あるいは(ii)ハードウエア（例えば、集積回路、配線論理回路）の部として、関連する装置の機能を含みあるいは含まずに実現できる。さらに、ハードウエアの部は、マイクロコンピュータの内部に含まれることもできる。

　外部環境取得部２は、画像撮像部８、距離算出部９、速度検出部１０及び方向検出部１１から構成されている。

　画像撮像部８及び距離算出部９は、図２に示す測距センサ部１２（検出部に相当）の要部を構成するものである。画像撮像部８は発光部１３及び受光部１４から構成されており、車両の前方方向を中心として扇状の水平方向に首振り運動するようになっている。発光部１３はレーザ光をパルス状に照射し、受光部１４は、検出領域に位置する外部環境で反射した発光部１３からのレーザ光を受光する。距離算出部９は、発光部１３がレーザ光を照射してから受光部１４が受光するまでの時間差を計測し、その時間差に基づいて算出した外部環境までの距離を首振り角度に対応して距離画像メモリ１５に記憶する。距離画像メモリ１５は、距離と首振り角度とを対応付けた距離データを出力する。受光部１４による受光強度は、首振り角度に対応して強度画像メモリ１６に記憶される。強度画像メモリ１６は、受光強度と首振り角度とを対応付けた画像データを出力する。この画像データは、距離画像メモリ１５から出力される距離データの信頼性を判定するために参照される。

　速度検出部１０は、車両に搭載された速度センサから出力される速度信号に基づいて車両速度を検出する。方向検出部１１は、車両に搭載された角度センサから出力される角度信号に基づいて車両の旋回角度を検出する。

　インフラ情報保存部４は、地図情報保存部１７及び交通情報保存部１８から構成されている。地図情報保存部１７は、車両が走行可能な道路を示す地図情報を例えば地図情報センタから受信して保存する。交通情報保存部１８は、道路の混雑情報や事故情報等の交通情報を例えば交通情報センタから受信して保存する。尚、情報は、不可算名詞のみならず、可算名詞としても使用される。複数の情報は、複数の情報項目と同等である。

　走行環境認識部３は、自車位置推定部１９、走行可能領域認識部２０、障害物認識部２１及び道路標識認識部２２から構成されている。自車位置推定部１９は、本開示が適用される主要部であり、後述する。走行可能領域認識部２０は、画像データや距離データから得られた外部環境である静止物と自車位置推定部１９が推定した自車位置との間の距離を演算することで静止物情報を求め、その静止物情報に基づいて車両の走行可能領域である走行レーンを認識する。障害物認識部２１は、画像データや距離データや静止物情報から車両の周囲に位置する移動体を検出して追跡することにより、移動体の種別を推定したり移動速度を推定したりする。

　道路標識認識部２２は、画像データや地図データのような外部環境取得部２が取得した情報に基づいて道路標識を認識する。

　行動選択部５は、レーンキープ部２３、レーンチェンジ部２４及び障害物回避部２５から構成されている。レーンキープ部２３は、走行可能領域認識部２０が認識した走行レーンを走行したり、前方車両が走行している場合には当該前方車両に追随して走行したりする走行行動を実施する。レーンチェンジ部２４は、前方車両や後方車両への危険回避行動を実施する。障害物回避部２５は、レーン上に障害物が存在する場合等に危険を判断し、回避行動を実施する。

　経路計画部７はグローバル経路探索部２６からなり、目的地を設定すると共に、当該目的地に至る経路を設定する。つまり、地図情報保存部１７に保存されている地図情報と、交通情報保存部１８に保存されている交通情報とに基づいて、現在位置から目的地に至るまでの経路を設定する。目的地に至る経路は、所定間隔毎のグリッド毎に経路コストを計算し、経路コストの合計が最小となる経路を設定する。

　車両制御部６はアクチュエータ駆動部２７からなり、行動選択部５による結果を、アクセル、ブレーキ、ステアリング等に反映することで自律走行動作を実施する。

　さて、本開示は、走行環境認識部３を構成する自車位置推定部１９に関することから、以下、自車位置推定部１９について詳述する。

　自車位置推定部１９は、図３に示すように、位置データメモリ２８（記憶部に相当）、環境地図メモリ２９及び照合部３０（照合機とも言及される）から構成されている。照合部３０は運動候補設定部、運動候補設定機、推定値出力部、推定値出力機に相当する。更に、推定値出力機は、推定値提供機とも言及される。測距センサ部１２は、図４に示すように、車両の前方方向を基準として所定の検出領域（車両の前方方向に対して例えば－６０°～６０°）内に位置する外部環境に対する車両の相対的な位置姿勢を示すセンシングデータを位置データメモリ２８に記憶する。この場合、検出領域に位置する外部環境に対する車両の相対的な位置は（ｘ，ｙ）で示すことができる一方、外部環境に対する車両の相対的な姿勢は回転角θで表すことができることから、センシングデータは（ｘ，ｙ，θ）で表すことができる。このように位置データメモリ２８にセンシングデータが記憶される場合は、当該センシングデータは環境地図として環境地図メモリ２９にも記憶される。測距センサ部１２は、測距ディテクタとも言及される。

　照合部３０は、環境地図メモリ２９に記憶された環境地図（即ち、前回の検出時点である第１時点のセンシングデータ）に基づいて第1時点の次の検出時点である第２時点において可能となる位置姿勢を示す変数から構成される複数の運動候補（ｘ，ｙ，θ）を設定する。そして、次の検出時点である第２時点となった場合は、位置データメモリ２８に新たに記憶される第２時点でのセンシングデータと、第２時点において可能となると設定されたそれぞれの運動候補とを照合する。言い換えると、第１時点のセンシングデータから、次の第２時点での位置姿勢を運動候補として設定し、この運動候補を、第２時点で新たに得られたセンシングデータと照合することとなる。

　照合部３０が運動候補を設定するには、環境地図を構成する３種類の変数（ｘ，ｙ，θ）をそれぞれ変化させることにより運動候補を設定する。この運動候補は、次の検出時点で可能となる位置姿勢範囲で設定される。そして、今回の検出時点でのセンシングデータと運動候補とを照合し、環境地図が示す位置姿勢から最大評価値の運動候補が示す位置姿勢に至る位置姿勢変化量を位置姿勢推定値（以下、推定値）として特定する。つまり、図５に示す環境地図に推定値を合算するとセンシングデータとなることから、センシングデータと運動候補とを照合することで推定値を特定することが可能となる。

　さて、運動候補を空間座標の３軸に割り当てると、比較例の図６Ｂに丸印で示すように、運動候補を立体的に表すことができる。このように設定した運動候補の中にセンシングデータと一致または近似する最大評価値の運動候補が存在し、環境地図が示す位置姿勢から最大評価値の運動候補が示す位置姿勢に至る位置姿勢変化量が推定値ということになる。

　ところで、従来手法の推定値の推測方法では、センシングデータと、比較例の図６Ｂに丸印で示す全ての運動候補とを３次元的に照合していたことから、推定値を特定するまでの処理時間が長いという課題がある。

　また、次の検出時点での位置姿勢を推測し、推測した位置姿勢を中心として運動候補と照合するのが望ましいものの、本実施形態のように次の検出時点での位置姿勢を推定しない構成の場合は、環境地図を初期値として設定することになる。

　即ち、運動候補を構成する３種類の変数の内、ｘ以外の２種類の変数ｙ，θに関して、初期値としてｙ0＝０、θ0＝０°と設定してｙとθを固定すると、変数はｘのみとなる。ｘ＝ｘ1が最大評価値と仮定すると、ｘの運動候補は、ｘを横軸、評価値を縦軸で示すと、図７に実線で示すように、ｘ＝ｘ1の評価値が最大となり、ｘ＝ｘ1から離間するにしたがって評価値が減少する正規分布を示すようになる。

　ここで、運動候補の探索範囲に位置するように横軸に平行な第２横軸を想定すると、第２横軸上の運動候補もｘ＝ｘ1が最大評価値となる正規分布を示すようになる。これは、ｘ＝ｘ1を中心として評価値が正規分布を示していることから、第２横軸が評価値の正規分布領域を通過する場合には、図７に点線で示すように、第２横軸上の運動候補もｘ＝ｘ1が最大評価値となる正規分布を示すようになる。従って、初期値としてｙ＝ｙ0、θ＝θ0（第２変数）を設定することにより、本実施形態においては、図６Ａに示すように、最大評価値となる運動候補ｘ＝ｘ1（第１変数）を求めることができる。同様にして、初期値としてｘ＝ｘ0、θ＝θ0（第２変数）を設定することにより、最大評価値となる運動候補ｙ＝ｙ2（第１変数）を求めることができる。さらに、初期値として変数ｘ＝ｘ0，ｙ＝ｙ0（第２変数）を設定することにより、最大評価値となる運動候補としてθ＝θ3（第１変数）を求めることができる。そして、以上のようにしてそれぞれ求めた最大評価値となる運動候補（ｘ1，ｙ2，θ3）（最大評価値となる第１変数）を推定値（ｘ1，ｙ2，θ3）として特定することができる。

　このような推定方法を実現するために、照合部３０は、照合部１軸目３０ａ、照合部２軸目３０ｂ及び照合部３軸目３０ｃから構成されており、１次元的に運動候補と照合するようになっている。１軸目はｘに対応し、２軸目はｙに対応し、３軸目はθに対応する。ｘ，ｙに関しては、その単位は距離となる。θに関しては、その単位は角度（－６０°～＋６０°）となる。

　説明の簡略化のために、図８に示すように、各軸の運動候補が３個ずつ設定されているものとして説明する。

　照合部１軸目３０ａは、初期値としてｙ＝０、θ＝０°を設定することで、照合対象となる運動候補をｘのみを変数とする１次元として評価値を判定する。図８に示す例では、ｘ＝１、ｘ＝０、ｘ＝－１の評価値を判定している。この場合、ｘ＝０のときに最大評価値となることから、１軸上で最大評価値となる運動候補はｘ＝０であると推定する。

　照合部２軸目３０ｂは、ｘ＝０、θ＝０°を設定することで、照合対象となる運動候補をｙのみを変数とする１次元として評価値を判定する。図８に示す例では、ｙ＝１、ｙ＝０、ｙ＝－１の評価値を求めている。この場合、ｙ＝０のときに最大評価値となることから、２軸上で最大評価値となる運動候補はｙ＝０であると推定する。

　照合部３軸目３０ｃは、ｘ＝０、ｙ＝０を設定することで、照合対象となる運動候補をｚのみを変数とする１次元として評価値を判定する。図８に示す例では、θ＝９０°、θ＝０°、θ＝－９０°の評価値を判定している。この場合、θ＝－９０°のときに最大評価値となることから、３軸上で最大評価値となる運動候補はθ＝－９０°であると推定する。

　以上のようにして、各照合部１～３軸目（以下、分割照合部あるいは分割照合機とも言及される）３０ａ～３０ｃにより、最大評価値となる運動候補としてｘ＝０、ｙ＝０、θ＝－９０°を推定したことから、位置姿勢変化量を示す推定値として（０，０，－９０°）を特定する。

　要するに、各軸に対応した運動候補が図８に示すような場合、従来の手法では各軸の全ての運動候補を３次元的に組み合わせた数である２７（＝３×３×３）回だけセンシングデータと運動候補とを照合する必要があったが、本実施形態では、１次元的に組み合わせた９（＝３＋３＋３）回で済み、センシングデータと運動候補との照合回数を減少させて処理時間を大幅に低減することができる。この場合、従来の手法では、運動候補の数が照合回数となることから、運動候補が多くなる程、それだけ処理時間の低減効果が大きくなると言える。

　尚、最大評価値となる運動候補が複数存在した場合、その数が奇数個の場合は中央の値（例えば５個の場合は３番目）、偶数個の場合は中央側の値（例えば６個の場合は３番目か４番目）の運動候補を推定値とする。これは、評価値は最大評価値を中心として正規分布していることから、正規分布の中央に位置する運動候補となる程、最大評価値の信頼性が高くなるからである。

　そして、上述のような処理を繰り返すことにより車両の走行軌跡を特定することができるので、車両の出発位置における絶対的な位置姿勢（緯度、経度、進行方位）に基づいて現在の絶対的な位置姿勢（緯度、経度、進行方位）を特定することができる。

　このような実施形態によれば、次のような効果を奏することができる。

　照合部３０は、前回の検出時点でのセンシングデータである環境地図に基づいて次の検出時点で可能となる位置姿勢を示す複数の変数から構成される運動候補を３次元的に設定し、センシングデータと運動候補とを照合することにより最大評価値となる運動候補を推定し、環境地図が示す位置姿勢から最大評価値の運動候補が示す位置姿勢に至る位置姿勢変化量を示す推定値を特定する場合に、２種類の変数を固定して１種類の変数のみを変化させることで最大評価値となる運動候補を１次元的に推定するようにしたので、全ての運動候補と３次元的に照合する構成に比較して、最大評価値となる運動候補を推定するまでの処理時間を大幅に低減することができる。

　前回の検出時点でのセンシングデータである環境地図のみを記憶すれば良いことから、多数の環境地図を記憶する構成に比較して、環境地図メモリ２９の記憶容量を大幅に削減できる共に環境地図を記憶したり読出したりするのに要する時間を大幅に低減することができる。

　（第２実施形態）
　第２実施形態について図９を参照して説明する。第１実施形態では、分割照合部３０ａ～３０ｃにおいて、２種類の変数を固定し、１種類の変数の評価値を求めたが、本実施形態では、１種類の変数を固定し、２種類の変数の評価値を求めることを特徴とする。

　図９に示すように、第１実施形態の照合部１軸目３０ａ、照合部２軸目３０ｂに代えて、それらを統合した照合部１軸目・２軸目３０ｄ（分割照合部に相当）が設けられている。照合部１軸目・２軸目３０ｄは、運動候補との照合を２次元的に行うものである。

　即ち、２種類の変数（ｘ，ｙ）は車両の位置を２次元的に示すものであり、互いに関連するものであることから、１種類の変数毎に照合するのに比較して、２種類の変数（ｘ，ｙ）を組み合わせて２次元的に照合する方が推定値の精度が高くなると言える。

　そこで、初期値としてθ＝０°と設定し、ｘ，ｙの２種類の変数の評価値を求めるようにした。この場合、照合部１軸目・２軸目３０ｄが照合する運動候補は３×３の９個であることから、照合回数は９回となり、照合部３軸目３０ｃの照合回数の３回を加えると全体の照合回数は１２回となる。これにより、第１実施形態の照合回数よりも増加することになるものの、従来の照合回数に対しては低減することができる。

　このような実施形態によれば、運動候補を構成する変数の内、複数の変数の組み合せを考慮しないと推定値の精度が低下するような場合に、推定精度が低減することを抑制しながら、第１実施形態と同様に、推定値を特定するまでの処理時間を低減することができる。

　（第３実施形態）
　第３実施形態について図１０を参照して説明する。本実施形態は、照合部３０で特定した推定値を初期値として照合部３０に再設定することを特徴とする。

　初期値としては、次の検出時点での位置姿勢に極力近い値を設定することが重要である。つまり、初期値が次の検出時点での位置姿勢に近い程、正規分布の精度が高くなり、特定した推定値（ｘ1，ｙ2，θ3）の精度が高くなるからである。

　そこで、図１０に示すように、分割照合部３０ａ～３０ｃから出力される推定値を初期値として分割照合部３０ａ～３０ｃ再設定するようにした。この場合、分割照合部３０ａ～３０ｃは、環境地図メモリ２９に記憶されている環境地図を１回目の初期値として設定することで推定値を特定すると共に、初期値に対応する分割照合部３０ａ～３０ｃからの推定値を２回目の初期値として再設定するようになっている。

　このような実施形態によれば、初期値に対応する分割照合部３０ａ～３０ｃからの推定値を初期値として再設定するようにしたので、次の検出時点での位置姿勢により近い値を初期値として設定でき、分割照合部３０ａ～３０ｃによる推定値の精度を高めることができる。尚、初期値の再設定を２回以上行うようにしても良い。

　（第４実施形態）
　第４実施形態について図１１から図１３を参照して説明する。第１実施形態では、初期値として環境地図を設定したが、本実施形態では、カルマンフィルタ、パーティクルフィルタ等の時系列フィルタによる予測値を初期値として設定することを特徴とする。

　上述したように、初期値としては、次の検出時点での位置姿勢に極力近い値を設定することが重要である。

　そこで、本実施形態では、次の検出時点での位置姿勢を時系列フィルタで予測するようにした。尚、時系列フィルタについては特徴がないことから、その説明は省略する。

　図１１に示すように、自車位置推定部１９には、照合部３０の後段に時系列フィルタ部３１が設けられている。この時系列フィルタ部３１は、分割照合部３０ａ～３０ｃからの推定値を入力するように設けられており、入力した過去の推定値を時系列に記憶している。このような時系列的な推定値は統計情報であることから、その統計情報からノイズを除去することで車両の動きを精度良く予測（旋回時は加速度運動を想定）可能となる。つまり、図１２Ａ，図１２Ｂ，図１２Ｃに示すように、次の検出時点での位置姿勢を精度良く推定することができる。

　ここで、図１３に示すように、運動候補の探索範囲は、時系列フィルタ部３１による予測値を中心とした所定範囲に制限することが有効であることから、車両の加減速の限界値である９．８ｍ／ｓ²若しくは走行結果から求めた最大の変化幅に設定する。このように探索範囲を制限することにより、最大評価値となる運動候補を探索するまでの処理時間を低減することができる。

　さらに、最大評価値となる運動候補を求める場合、センシングデータを探索範囲の全ての運動候補と照合する必要はなく、最大評価値となる運動候補をバイナリサーチにより検出することで処理時間の一層の低減化を図ることができる。

　具体的には、時系列フィルタ予測値が図１３に示すａであった場合、ａの右側か左側のいずれかに最大評価値となる運動候補が存在することになる。ａの右側に最大評価値となる運動候補が存在すると仮定し、時系列フィルタ予測値から所定値だけ加算したｂで示す運動候補を評価する。この場合、評価値が減少することから、最大評価値となる運動候補はａの右側には存在せず、左側に存在すると判断し、ａから所定値だけ減算したｃで示す運動候補を評価する。この場合、評価値が上昇することから、最大評価値はさらに左側に存在する可能性があると判断し、さらに所定値だけ減算したｄで示す運動候補を評価する。評価値がさらに上昇することから、最大評価値となる運動候補はａの左側に存在する可能性があると判断し、さらに所定値だけ減算したｅで示す運動候補を評価する。今度は評価値が減少することから、最大評価値となる運動候補はｅよりも右側からｃまでの範囲に存在すると判断し、まず、ｃとｄの中間となるｆで示す運動候補を評価する。すると、ｄの評価値よりも低下することから、最大評価値となる運動候補はｄとｅとの中間に存在すると判断し、ｄとｅとの中間となるｇで示す運動候補を評価する。

　以上のようにして運動候補の探索範囲を徐々に制限し、探索範囲が所定の限界値以下となった場合、若しくは探索を指定回数実行した場合に最大評価値となる運動候補を探索して推定値として出力する。

　このような実施形態によれば、時系列フィルタ部３１による予測値を初期値として設定するようにしたので、推定値の精度を高めることができる。

　運動候補の探索範囲を時系列フィルタ部３１による予測値を中心とした所定範囲に制限すると共に最大評価値となる運動候補をバイナリサーチにより検出するようにしたので、最大評価値となる運動候補を探索するまでの処理時間を一層低減することができる。

　（第５実施形態）
　第５実施形態について図１４～１７を参照して説明する。実際の走行シーンでは、アクセル制御やステアリング操作により自車の動きが変動し、周辺環境にある静止物の配置も動的に変動するため、その変動に対応するのが望ましい。しかし、上記の第３及び第４実施形態では、分割照合部３０ａ～３０ｃに対して推定値を初期値として再設定するループ回数は固定されており、変動に対して適切に対応できないことが予測される。

　そこで、本実施形態は、分割照合部３０ａ～３０ｃに対して推定値を初期値として再設定するループ回数を変動可能としたことを特徴とし、そのための対策として、図１４に示すように再設定制御部３２を設けた。この再設定制御部３２は、分割照合部３０ａ～３０ｃから推定値及びその評価値を入力し、それらに基づいて初期値を再設定するか否かを判断し、初期値を再設定すると判断した場合は、初期値を分割照合部３０ａ～３０ｃに再設定するものである。

　初期値を再設定するか否かの判断方法について説明する。図１５に示すような運動候補に対する評価値分布が得られた場合、軸毎に収束回数は異なるものの、初期値の再設定を繰り返すことにより図中に丸数字で示す順番で真値に向かって収束するものと考えられる。

　ここで、図１６に示すように、初期値を再設定した結果、得られた評価値の変動幅（図１６中に示すΔ評価値）が所定値内となれば、最大評価値となる運動候補であることを確定することができる。そこで、図１７に示すように、分割照合部３０ａ～３０ｃにおいて、評価値の変動幅が所定値を上回る場合は初期値を再設定し、全ての軸において評価値の変動幅が所定値内となった場合は、初期値の再設定を終了して推定値を最終的に確定する。

　このような実施形態によれば、最大評価値となる運動候補を特定するまで初期値を再設定するループ回数を変動可能としたので、初期値の再設定するループ回数が固定している構成に比較して、周囲環境の変動に対して動的に対応可能となり、推定値の精度を常に高めることができる。

　（第６実施形態）
　第６実施形態について図１８を参照して説明する。本実施形態は、車両制御ＥＣＵ３３からの位置姿勢情報に基づいて初期値を設定することを特徴とする。

　図１８に示すように、照合部３０は、車両制御ＥＣＵ３３（情報出力部、制御部に相当）から車両速度及び回転角に基づく位置姿勢情報が入力するようになっており、入力した位置姿勢情報を初期値として設定するようになっている。尚、情報出力部は、情報出力機、情報提供部、情報提供機とも言及される。

　即ち、照合部３０は、車両制御ＥＣＵ３３からの位置姿勢情報に基づいて次の検出時点でのｘ方向位置ｘ＝ｘ0、ｙ方向位置ｙ＝ｙ0、回転角θ＝θ0を、分割照合部３０ａ～３０ｃに初期値として設定した状態で最大評価値となる運動候補を推測する。

　このような実施形態によれば、車両制御ＥＣＵ３３からの位置姿勢情報に基づいて初期値を適切に設定することができるので、推定値の精度を高めることができる。

　（第７実施形態）
　第７実施形態について図１９及び図２０Ａ，図２０Ｂ、図２０Ｃを参照して説明する。第１実施形態では、分割照合部３０ａ～３０ｃを並列動作するように構成したが、本実施形態では、分割照合部３０ａ～３０ｃを直列動作するように構成したことを特徴とする。

　図１９に示すように、照合部１軸目３０ａは、位置データメモリ２８からのセンシングデータと環境地図メモリ２９からの環境地図とを入力する。照合部２軸目３０ｂは、位置データメモリ２８からのセンシングデータと環境地図メモリ２９からの環境地図と照合部１軸目３０ａからの１軸目推定値とを入力する。照合部３軸目３０ｃは、位置データメモリ２８からのセンシングデータと環境地図メモリ２９からの環境地図と照合部２軸目３０ｂからの２軸目推定値とを入力する。この場合、分割照合部３０ａ～３０ｃにおいて、環境地図メモリ２９からの環境地図と前段側からの推定値との両方が入力する場合には、初期値として前段側からの推定値を設定するようになっている。これは、前段側からの推定値の方が環境地図よりも精度が高いからである。

　図２０Ａに示すように、照合部１軸目３０ａは、初期値として環境地図によるｙ＝ｙ0とθ＝θ0とを設定してｙとθの運動候補を固定し、ｘのみを変化させて最大評価値となる運動候補としてｘ＝ｘ1を推定する。

　次に図２０Ｂに示すように、照合部２軸目３０ｂは、初期値として環境地図によるθ＝θ0と照合部１軸目３０ａからの１軸目推定値ｘ1とを設定してｘとθの運動候補を固定し、ｙのみを変化させて最大評価値となる運動候補としてｙ＝ｙ2を推定する。

　最後に図２０Ｃに示すように、照合部３軸目３０ｃは、初期値として照合部１軸目３０ａからの１軸目推定値ｘ＝ｘ1と照合部２軸目３０ｂからの２軸目推定値ｙ＝ｙ2とを設定してｘとｙの運動候補を固定し、θのみを変化させて最大評価値となる運動候補としてθ＝θ3を推定する。

　このような実施形態によれば、分割照合部３０ａ～３０ｃにおいて、環境地図メモリ２９からの環境地図と前段側からの推定値との両方が入力する場合は、前段側からの推定値を初期値として設定するようにしたので、環境地図を初期値として設定する場合に比較して、推定値の精度を高めることができる。

　（第８実施形態）
　第８実施形態について図２１Ａ，図２１Ｂ，図２１Ｃを参照して説明する。本実施形態は、第７実施形態で示す照合部３０に対して、環境地図メモリ２９からの環境地図に代えて時系列フィルタ部３１による予測値が入力することを特徴とする。この場合、分割照合部３０ａ～３０ｃにおいて、時系列フィルタ部３１による予測値と前段側からの推定値との両方が入力する場合は、初期値として前段側からの推定値を設定するようになっている。これは、前段側からの推定値の方が時系列フィルタ部３１による予測値よりも精度が高いからである。

　図２１Ａに示すように、照合部１軸目３０ａは、初期値として時系列フィルタ部３１による予測値ｙ＝ｙ0、θ＝θ0を設定してｙとθの運動候補を固定し、ｘのみを変化させて最大評価値となる運動候補としてｘ＝ｘ1を推定する。

　次に図２１Ｂに示すように、照合部２軸目３０ｂは、初期値として時系列フィルタ部３１による予測値θ＝θ0と照合部１軸目３０ａからの１軸目推定値ｘ＝ｘ1とを設定してｘとθの運動候補を固定し、ｙのみを変化させて最大評価値となる運動候補としてｙ＝ｙ2を推定する。

　最後に図２１Ｃに示すように、照合部３軸目３０ｃは、初期値として照合部１軸目３０ａからの１軸目推定値ｘ＝ｘ1と照合部２軸目３０ｂからの２軸目推定値ｙ＝ｙ2を設定してｘとｙの運動候補を固定し、θのみを変化させて最大評価値となる運動候補としてθ＝θ3を推定する。

　このような実施形態によれば、分割照合部３０ａ～３０ｃにおいて、時系列フィルタ部３１による予測値と前段側からの推定値とが入力する場合は、前段側からの推定値を初期値として設定するようにしたので、時系列フィルタ部３１による予測値を初期値として設定する構成に比較して、推定値の精度を高めることができる。

　（第９実施形態）
　第９実施形態について図２２Ａ、図２２Ｂ、図２２Ｃを参照して説明する。本実施形態は、第７実施形態で示す照合部３０には、環境地図メモリ２９からの環境地図に代えて車両制御ＥＣＵ３３から位置姿勢情報が入力することを特徴とする。この場合、分割照合部３０ａ～３０ｃにおいて、車両制御ＥＣＵ３３からの位置姿勢情報と前段側からの推定値との両方が入力する場合は、初期値として前段側からの推定値を設定するようになっている。これは、前段側からの推定値の方が車両制御ＥＣＵ３３からの位置姿勢情報よりも精度が高いからである。

　図２２Ａに示すように、照合部１軸目３０ａは、初期値として車両制御ＥＣＵ３３から得られる車両のｙ方向の値ｙ＝ｙ0と回転角の値θ＝θ0を設定してｙとθの運動候補を固定し、ｘのみを変化させて最大評価値となる運動候補としてｘ＝ｘ1を推定する。

　次に図２２Ｂに示すように、照合部２軸目３０ｂは、初期値として車両制御ＥＣＵ３３から得られる回転角の値θ＝θ0と照合部１軸目３０ａからの１軸目推定値ｘ＝ｘ1とを設定してｘとθの運動候補を固定し、ｙのみを変化させて最大評価値となる運動候補としてｙ＝ｙ2を推定する。

　最後に図２２Ｃに示すように、照合部３軸目３０ｃは、初期値として照合部１軸目３０ａからの１軸目推定値ｘ＝ｘ1と照合部２軸目３０ｂからの２軸目推定値ｙ＝ｙ2を設定してｘとｙの運動候補を固定し、θのみを変化させて最大評価値となる運動候補としてθ＝θ3を推定する。

　このような実施形態によれば、分割照合部３０ａ～３０ｃにおいて、車両制御ＥＣＵ３３からの位置姿勢情報と前段側からの推定値との両方が入力する場合は、前段側からの推定値を初期値として設定するようにしたので、車両制御ＥＣＵ３３からの位置姿勢情報を初期値として設定する構成に比較して、推定値の精度を高めることができる。

　（第１０実施形態）
　第１０実施形態について図２３を参照して説明する。本実施形態は、直列動作する分割照合部３０ａ～３０ｃの照合順序が互いに異なる照合部３０を複数設け、それらを並列動作するように構成したことを特徴とする。

　第７実施形態のように分割照合部３０ａ～３０ｃを直列動作させることで分割照合部３０ａ～３０ｃに照合順序を設定した場合、前段側からの推定値（初期値）の精度が低い場合には後段側の照合結果に影響を及ぼすことが考えられる。

　そこで、本実施形態では、図２３に示すように、分割照合部３０ａ～３０ｃが直列動作するように設定された第１照合部３０Ａと、当該第１照合部の分割照合部３０ａ～３０ｃとは照合順序が異なる第２照合部３０Ｂとを設け、第１照合部３０Ａと第２照合部３０Ｂとを並列動作するように構成した。

　即ち、第２照合部３０Ｂとして、第１照合部３０Ａの分割照合部３０ａ～３０ｃの照合順序と異なるように、照合部３軸目３０ｃ、照合部１軸目３０ａ、照合部２軸目３０ｂの順で照合するようにした。選択部３４は、第１照合部３０Ａからの推定値と第２照合部３０Ｂからの推定値とを変数毎に比較し、評価値の大きい方の推定値を変数に対応した推定値として特定する。

　このような実施形態によれば、第１照合部３０Ａ及び第２照合部３０Ｂにおいて分割照合部３０ａ～３０ｃの照合順序が互いに異なるように設定し、それらを並列動作するように構成したので、一の照合部３０Ａ，３０Ｂのみで推定値を特定する構成に比較して、推定値の精度を高めることができる。

　（第１１実施形態）
　第１１実施形態について図２４を参照して説明する。本実施形態は、時系列フィルタ部３１による予測値を構成する変数の精度の高い順番に応じて分割照合部３０ａ～３０ｃの照合順序を設定したことを特徴とする。

　時系列フィルタ部３１による予測値は、精度が比較的高いもののロバスト性が低いことから、予測値が一旦大きく外れると、それ以後の予測値に大きな影響を及ぼす。このため、時系列フィルタ部３１による予測値が本来の予測値から大きく外れたことを判定する必要がある。

　このような事情から、時系列フィルタ部３１による予測値の精度を判定するために車両制御ＥＣＵ３３からの位置姿勢情報と比較するようにした。つまり、車両制御ＥＣＵ３３からの位置姿勢情報の精度は時系列フィルタ部３１による予測値よりも低いもののロバスト性は格段に高いことから、時系列フィルタ部３１による予測値と車両制御ＥＣＵ３３からの位置姿勢情報との差が大きくなる程、時系列フィルタ部３１による予測値の精度が低下したと推定することができる。

　そこで、本実施形態では、時系列フィルタ部３１による予測値と車両制御ＥＣＵ３３からの位置姿勢情報とを変数毎に比較し、その差が小さい順（精度の高い順）に変数の順番を決定し、その変数の順番に応じて分割照合部３０ａ～３０ｃの照合順序を設定するようにした。

　即ち、照合部１軸目３０ａが初期値として設定する予測値の精度が最も高く、照合部３軸目３０ｃが初期値として設定する予測値の精度が最も低いと仮定した場合、順序選択部３５は、図２４に示すように、照合部１軸目３０ａ、照合部２軸目３０ｂ、照合部３軸目３０ｃの照合順序となるように決定する。

　最初の照合部１軸目３０ａにおいて、時系列フィルタ部３１による予測値の精度が高い２軸目予測値及び３軸目予測値を初期値として設定した状態で１軸目推定値を特定する。つまり、１軸目予測値よりも２軸目予測値及び３軸目予測値の精度の方が高いことから、照合部１軸目３０ａにおいて、初期値として２軸目予測値及び３軸目予測値を設定するのである。

　次に照合部２軸目３０ｂにおいて、時系列フィルタ部３１による３軸目予測値と照合部１軸目３０ａからの１軸目推定値を初期値として設定した状態で２軸目推定値を特定する。

　最後に照合部３軸目３０ｃにおいて、初期値として照合部１軸目３０ａからの１軸目推定値及び照合部２軸目３０ｂからの２軸目推定値を設定した状態で３軸目推定値を特定する。

　このような実施形態によれば、時系列フィルタ部３１による予測値を構成する変数の精度の高い順番に応じて分割照合部３０ａ～３０ｃの照合順序を設定するようにしたので、推定値の精度を高めることができる。

　（第１２実施形態）
　第１２実施形態について図２５から図２７を参照して説明する。実際の走行シーンでは、アクセル制御やステアリング操作により自車の動きが変動し、周辺環境にある静止物の配置も動的に変動することから、その変動に対応するのが望ましい。しかし、上記の第７～１１実施形態では、分割照合部３０ａ～３０ｃの照合順序は固定されており、変動に対して適切に対応できないことが予測される。

　そこで、本実施形態は、分割照合部３０ａ～３０ｃの照合順序を変動可能とすることを特徴とし、そのための対策として、図２５に示すように順序制御部３６を設けた。この順序制御部３６は、分割照合部３０ａ～３０ｃから評価値と推定値とを入力し、それらに基づいて分割照合部３０ａ～３０ｃの照合順序を決定し、その照合順序を分割照合部３０ａ～３０ｃに指示するものである。

　照合順序を決定する方法について説明する。図２６に示すように、運動候補に対する評価値の分布を示す評価値分布を得られた場合、その評価値分布の立上りまたは立下りの傾斜角度は軸毎に異なる。この場合、傾斜角度が小さい程、最大評価値がノイズの影響を受けやすくなることから、最大評価値となる運動候補の精度が低いと言える。図２６に示す例では、ｘ軸の傾斜角度が最も小さいことから、最大評価値となる運動候補の精度が最も低くなる。このため、ｘ軸から最大評価値となる運動候補を特定した場合には後段に与える影響が大きくなることから、傾斜角度が最大であるｙ軸から順にθ軸、ｘ軸の順番で照合順序を設定するのが望ましい。

　ここで、傾斜角度を判定するには、図２７に示すように、最大評価値と所定の運動候補とを頂点とする直角三角形の斜辺の傾きで判定する。直角三角形の高さ寸法としては最大評価値の例えば１／２に設定すれば良いが、１／３や２／３等に設定しても良い。この場合、直角三角形の斜辺の傾きが大きくなる程、最大評価値の精度が高くなることから、斜辺の傾きが大きい順序となるように分割照合部３０ａ～３０ｃの照合順序を設定する。

　このような実施形態によれば、評価値分布の立上りまたは立下りの傾斜角度が大きい順に分割照合部３０ａ～３０ｃの照合順序を設定するようにしたので、後段側の分割照合部３０ａ～３０ｃに対して精度の高い初期値を設定することができ、推定値の精度を常に高めることができる。

　（その他の実施形態）
　本開示は、上記実施形態に限定されることなく、次のように変形または拡張できる。

　上記実施形態では、センシングデータ及び環境地図並びに運動候補を３種類の変数で構成したが、４種類以上の変数で構成するようにしても良い。その場合、変数の数をＮとすると、固定する変数（第２変数）は１～Ｎ－１の範囲となる。

　ＧＰＳによる位置姿勢情報の精度が極めて高い場合には、その位置姿勢情報に基づいて現在の絶対的な位置姿勢（緯度、経度、進行方位）を補正するようにしても良い。

　本開示は、実施例に準拠して記述されたが、本開示は当該実施例や構造に限定されるものではないと理解される。本開示は、様々な変形例や均等範囲内の変形をも包含する。加えて、様々な組み合わせや形態、さらには、それらに一要素のみ、それ以上、あるいはそれ以下、を含む他の組み合わせや形態をも、本開示の範疇や思想範囲に入るものである。

Claims (15)

1. 　所定の連続する検出時点の各時点で外部環境に対する移動体の相対的な位置姿勢を示す複数の変数から構成されるセンシングデータを検出する検出部（１２）と、
   　前回の検出時点である第１時点のセンシングデータを前記複数の変数から構成される前記第１時点に対応する環境地図として記憶する記憶部（２８）と、
   　前記第１時点に対応する環境地図に基づいて、前記第１時点の次の検出の時点である第２時点で前記移動体が可能となる位置姿勢を示す、各々が前記複数の変数から構成される複数の運動候補を設定する運動候補設定部（３０）と、
   　前記第２時点でのセンシングデータと、前記第１時点に対応する環境地図に基づいて設定された前記運動候補のそれぞれとを照合し、照合精度を示す評価値が最大である最大評価値となる最大評価運動候補を推定する照合部（３０）と、
   　前記第１時点に対応する環境地図が示す位置姿勢から前記最大評価運動候補が示す位置姿勢に至る変化量を位置姿勢推定値として出力する推定値出力部（３０）と、を備え、
   　前記照合部は、複数の分割照合部（３０ａ～３０ｄ）から構成され、
   　前記分割照合部の各々は、前記運動候補を構成する前記複数の変数の内、少なくとも一つを第１変数として選択し、その他全てを第２変数として選択し、ここで、前記分割照合部の各々が選択する前記第１変数は互いに異なるように選択されており、
   　前記分割照合部の各々は、前記第２変数として所定の初期値を固定値として設定し変化させない状態で前記第１変数を変化させて評価値を求めることにより最大評価値となる第１変数を推定し、
   　前記推定値出力部は、前記分割照合部がそれぞれ推定した最大評価値となる第１変数を前記位置姿勢推定値として出力する
   　移動体の位置姿勢推定装置。
2. 　前記分割照合部の各々は、前記初期値として前記環境地図を構成する第２変数を設定する
   　請求項１に記載の移動体の位置姿勢推定装置。
3. 　過去の時系列の統計情報に基づいて前記第２時点での位置姿勢を示す予測値を推定する時系列フィルタ部（３１）を備え、
   　前記分割照合部の各々は、前記初期値として前記予測値を構成する第２変数を設定する
   　請求項１または２に記載の移動体の位置姿勢推定装置。
4. 　前記第２時点での位置姿勢を示す位置姿勢情報を出力する情報出力部（３３）を備え、
   　前記分割照合部の各々は、前記初期値として前記位置姿勢情報が示す第２変数を設定する
   　請求項１または２に記載の移動体の位置姿勢推定装置。
5. 　前記分割照合部のそれぞれは、並列動作するように設けられている
   　請求項１から４のいずれか一項に記載の移動体の位置姿勢推定装置。
6. 　前記分割照合部の各々は、前記初期値として前記位置姿勢推定値を構成する第２変数を再設定する
   　請求項５に記載の移動体の位置姿勢推定装置。
7. 　前記分割照合部の各々が前記第２変数を再設定した場合における最大評価値と前回の最大評価値との差が所定値を上回る場合は、前記第２変数を再設定することを前記分割照合部の各々に指示する再設定制御部（３２）を備えた
   　請求項６に記載の移動体の位置姿勢推定装置。
8. 　前記分割照合部のそれぞれは、直列動作するように設けられ、
   　後段側の分割照合部は、前段側の分割照合部が推定した第１変数を前記初期値として設定する
   　請求項１から４のいずれか一項に記載の移動体の位置姿勢推定装置。
9. 　前記分割照合部の各々は、前記初期値として前段側の分割照合部が推定した第１変数及び前記環境地図の内のいずれか一方が入力する場合は当該一方を設定し、前段側の分割照合部が推定した第１変数及び前記環境地図の両方が入力する場合は当該第１変数を設定する
   　請求項８に記載の移動体の位置姿勢推定装置。
10. 　過去の時系列な統計情報に基づいて前記第２時点での位置姿勢を示す予測値を推定する時系列フィルタ部（３１）を備え、
    　前記分割照合部の各々は、前記初期値として前段側の分割照合部が推定した第１変数及び前記時系列フィルタ部が推定した予測値の内のいずれか一方が入力する場合は当該一方を設定し、前記第１変数及び前記予測値の両方が入力する場合は当該第１変数を設定する
    　請求項８に記載の移動体の位置姿勢推定装置。
11. 　前記第２時点での位置姿勢を示す位置姿勢情報を出力する情報出力部（３３）を備え、
    　前記分割照合部の各々は、前記初期値として前段側の分割照合部が推定した第１変数及び前記位置姿勢情報の内のいずれか一方が入力する場合は当該一方を設定し、前記第１変数及び前記位置姿勢情報の両方が入力する場合は当該第１変数を設定する
    　請求項８に記載の移動体の位置姿勢推定装置。
12. 　前記照合部と同等な少なくとも一つの別照合部を、更に、備え、
    　前記初期値として前記分割照合部の各々に設定される第２変数の順序が前記照合部と前記別照合部とで互いに異なるように設定され、
    　前記推定値出力部は、前記位置姿勢推定値の評価値を変数毎に比較し、一の変数において最大評価値となる変数を位置姿勢推定値として出力する
    　請求項８に記載の移動体の位置姿勢推定装置。
13. 　前記初期値として設定される過去の時系列な統計情報に基づいて前記第２時点での位置姿勢を示す予測値を推定する時系列フィルタ部（３１）と、
    　前記第２時点での位置姿勢を示す位置姿勢情報を出力する情報出力部（３３）と、
    　前記予測値と前記位置姿勢情報とを変数毎に比較し、その差が小さい変数が第２変数として設定された分割照合部から順に照合順序を選択する順序選択部（３５）と、
    　を備えた請求項８に記載の移動体の位置姿勢推定装置。
14. 　前記運動候補に対する前記評価値の分布の立上りまたは立下りの傾斜角度が大きい順に前記分割照合部の各々の照合順序を制御する順序制御部（３６）を備えた
    　請求項８に記載の移動体の位置姿勢推定装置。
15. 　所定の連続する検出時点の各時点で外部環境に対する移動体の相対的な位置姿勢を示す複数の変数から構成されるセンシングデータを検出する検出部（１２）と、
    　前回の検出時点である第１時点でのセンシングデータを前記複数の変数から構成される前記第１時点に対応する環境地図として記憶する記憶部（２８）と、
    　前記第１時点に対応する環境地図に基づいて、前記第１時点の次の検出時点である第２時点で前記移動体が可能となる位置姿勢を示す、各々が前記複数の変数から構成される複数の運動候補を設定する運動候補設定部（３０）と、
    　前記第２時点のセンシングデータと、前記第１時点に対応する環境地図に基づいて設定された前記運動候補のそれぞれとを照合し、照合精度を示す評価値が最大である最大評価値となる最大評価運動候補を推定する照合部（３０）と、
    　前記第１時点の環境地図が示す位置姿勢から前記最大評価運動候補が示す位置姿勢に至る変化量を位置姿勢推定値として出力する推定値出力部（３０）と、
    　前記位置姿勢推定値に基づいて現在位置を推測しながら前記移動体を移動制御する制御部（３３）と、を備え、
    　前記照合部は、複数の分割照合部（３０ａ～３０ｄ）から構成され、
    　前記分割照合部の各々は、前記運動候補を構成する前記複数の変数の内、少なくとも一つを第１変数として選択し、その他全てを第２変数として選択し、ここで、前記分割照合部の各々が選択する前記第１変数は互いに異なるように選択されており、
    　前記分割照合部の各々は、前記第２変数として所定の初期値を固定値として設定し変化させない状態で前記第１変数を変化させて評価値を求めることにより最大評価値となる第１変数を推定し、
    　前記推定値出力部は、前記分割照合部がそれぞれ推定した最大評価値となる第１変数を前記位置姿勢推定値として出力する
    　移動体の自立走行システム。

Images