(57)【特許請求の範囲】
【請求項１】
地図において予め設定されているランドマークと、移動体に対して固定されている外界センサを通じて撮像された当該移動体の外界画像に含まれるランドマークとの対応付けに基づき、前記移動体の推定位置を外界推定位置として算出する外界推定要素と、
前記外界推定要素により算出された外界推定位置の履歴に対して、第１時定数により表現される減衰特性にしたがって高周波成分を減衰させるフィルタリングを実行する第１フィルタ要素を有し、外界推定位置の履歴に対する前記第１フィルタ要素によるフィルタリング処理結果に基づき、前記移動体の推定位置を前記移動体の動作制御基礎である第１推定位置として算出する第１推定要素と、
前記外界推定要素により算出された外界推定位置の履歴に対して、前記第１時定数よりも小さい第２時定数により表現される減衰特性にしたがって高周波成分を減衰させるフィルタリングを実行する第２フィルタ要素を有し、外界推定位置の履歴に対する前記第２フィルタ要素によるフィルタリング処理結果に基づき、前記外界推定要素により前記対応付けに際して基準として用いられる前記移動体の推定位置を第２推定位置として算出する第２推定要素と、を備えていることを特徴とする制御装置。
【請求項２】
請求項１記載の制御装置において、
前記移動体に搭載されている内界センサにより計測された前記移動体の姿勢および動作状態に基づき、前記移動体の推定位置を内界推定位置として算出する内界推定要素をさらに備え、
前記第１フィルタ要素は、前記内界推定要素により算出された内界推定位置の履歴に対して低周波成分を減衰させるフィルタリングをさらに実行し、
前記第１推定要素は、外界推定位置の履歴および内界推定位置の履歴のそれぞれに対する前記第１フィルタ要素によるフィルタリング処理結果に基づき、前記第１推定位置を算出することを特徴とする制御装置。
【請求項３】
請求項１記載の制御装置において、
前記移動体に搭載されている内界センサにより計測された前記移動体の姿勢および動作状態に基づき、前記移動体の推定位置を内界推定位置として算出する内界推定要素をさらに備え、
前記第２フィルタ要素は、前記内界推定要素により算出された内界推定位置の履歴に対して低周波成分を減衰させるフィルタリングをさらに実行し、
前記第２推定要素は、外界推定位置の履歴および内界推定位置の履歴のそれぞれに対する前記第２フィルタ要素によるフィルタリング処理結果に基づき、前記第２推定位置を算出することを特徴とする制御装置。
【請求項４】
請求項２または３記載の制御装置において、
前記第１フィルタ要素または前記第２フィルタ要素において、前記外界推定位置の履歴に対するフィルタリングおよび前記内界推定位置の履歴に対するフィルタリングのそれぞれを表わす伝達関数の和が１になるように当該伝達関数が設定されることを特徴とする制御装置。
【請求項５】
請求項２〜４のうちいずれか１つに記載の制御装置において、
前記第１フィルタ要素または前記第２フィルタ要素がローパスフィルタおよびハイパスフィルタにより構成されていることを特徴とする制御装置。
【請求項６】
請求項１記載の制御装置において、
前記第１推定要素および前記第２推定要素のうち少なくとも一方は、前記外界推定要素により算出される外界推定位置の履歴のばらつきの大きさに応じて前記第１時定数および前記第２時定数のうち少なくとも１つを動的に変更することを特徴とする制御装置。
【請求項７】
請求項６記載の制御装置において、
前記第１推定要素および前記第２推定要素のうち少なくとも一方は、前記外界センサを通じて計測された前記ランドマークの位置と前記第１推定位置または前記第２推定位置との間隔が閾値以上である場合、前記第１時定数および前記第２時定数のうち少なくとも１つを大きくすることを特徴とする制御装置。
【請求項８】
請求項６記載の制御装置において、
前記第１推定要素および前記第２推定要素のうち少なくとも一方は、前記外界画像における前記ランドマークの位置が当該外界画像の中心から所定の距離にある場合、前記第１時定数および前記第２時定数のうち少なくとも１つを大きくすることを特徴とする制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両等の移動体の制御装置に関する。
【背景技術】
【０００２】
外界センサおよび内界センサのそれぞれを通じて取得される情報をカルマンフィルタによって統合することにより、自己位置を推定する先行技術１が提案されている（特許文献１参照）。長周期で更新される外界センサによる前回時刻における自己位置の推定結果に対して、短周期で更新される内界センサによる当該前回時刻からの自己位置の相対変位量を加えることで、外界センサによる自己位置推定の遅れ時間を考慮して自己位置を推定する先行技術２が提案されている（特許文献２参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−１４９０８８号公報
【特許文献２】特開２００９−１９３０９７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、先行技術１によれば、外界センサによる処理の遅れ時間が考慮されていないため、当該外界センサを通じて取得された情報の処理量が多くなることで自己位置推定精度が低下する可能性がある。先行技術２によれば、外界センサによる自己位置推定結果にノイズが含まれている場合、外界センサによる自己位置推定結果に依存する最終的な自己位置の推定精度が低下する可能性がある。
【０００５】
そこで、本発明は、外界センサによる取得情報にノイズが混在した場合でも移動体の位置推定精度の向上を図りうる制御装置を提供することを解決課題とする。
【課題を解決するための手段】
【０００６】
本発明の制御装置は、地図において予め設定されているランドマークと、移動体に対して固定されている外界センサを通じて撮像された当該移動体の外界画像に含まれるランドマークとの対応付けに基づき、前記移動体の推定位置を外界推定位置として算出する外界推定要素と、前記外界推定要素により算出された外界推定位置の履歴に対して、第１時定数により表現される減衰特性にしたがって高周波成分を減衰させるフィルタリングを実行する第１フィルタ要素を有し、外界推定位置の履歴に対する前記第１フィルタ要素によるフィルタリング処理結果に基づき、前記移動体の推定位置を前記移動体の動作制御基礎である第１推定位置として算出する第１推定要素と、前記外界推定要素により算出された外界推定位置の履歴に対して、前記第１時定数よりも小さい第２時定数により表現される減衰特性にしたがって高周波成分を減衰させるフィルタリングを実行する第２フィルタ要素を有し、外界推定位置の履歴に対する前記第２フィルタ要素によるフィルタリング処理結果に基づき、前記外界推定要素により前記対応付けに際して基準として用いられる前記移動体の推定位置を第２推定位置として算出する第２推定要素と、を備えていることを特徴とする。
【０００７】
本発明の制御装置によれば、地図において予め設定されているランドマークと、外界センサを通じて撮像された移動体の外界画像に含まれるランドマークとの対応付けに基づき、外界推定位置（今回値）が算出される。第２フィルタ要素により外界推定位置の履歴（過去値）がフィルタリング処理された結果に基づいて第２推定位置が算出され、この第２推定位置が当該対応付けの基準として用いられる。このため、外界センサにより撮像された移動体の外界画像におけるランドマークの出現態様が不安定であって外界推定位置に高周波のノイズ（または外界センサを構成する撮像装置のひずみに起因するノイズ）が出現する場合でも、第１推定位置の確度（移動体の位置推定精度）の向上の観点から、第２推定位置が外界推定位置の適当な探索初期位置として決定される。
【０００８】
さらに、第２フィルタ要素によるフィルタリング処理の減衰特性を表わす第２時定数は第１時定数よりも小さいため、その分だけ広い周波数領域にわたり低周波成分を当該フィルタリング処理の結果に反映させることができる。このため、外界画像におけるランドマークの出現態様が不安定状態から安定状態に遷移する過程において、低周波領域で信頼性が高い外界センサによる取得情報を早期に第２フィルタ要素によるフィルタリング処理結果に反映させることができる。これにより、第１推定位置の確度（移動体の位置推定精度）の向上の観点から、第２推定位置が外界推定位置の適当な探索初期位置として決定される。
【０００９】
本発明の制御装置は、前記移動体に搭載されている内界センサにより計測された前記移動体の姿勢および動作状態に基づき、前記移動体の推定位置を内界推定位置として算出する内界推定要素をさらに備え、前記第１フィルタ要素は、前記内界推定要素により算出された内界推定位置の履歴に対して低周波成分を減衰させるフィルタリングをさらに実行し、前記第１推定要素は、外界推定位置の履歴および内界推定位置の履歴のそれぞれに対する前記第１フィルタ要素によるフィルタリング処理結果に基づき、前記第１推定位置を算出する。
【００１０】
当該構成の制御装置によれば、第１フィルタ要素により内界推定位置の履歴（過去値）に対して低周波成分を減衰させるフィルタリング処理が実行され、当該フィルタリング処理結果と、前記の外界推定位置の履歴（過去値）に対するフィルタリング処理結果とに基づいて第１推定位置が算出される。このため、内界センサによる計測結果にドリフト（低周波のノイズ）が含まれている場合でも、当該ドリフトの影響を軽減して第１推定位置の確度の向上が図られる。
【００１１】
本発明の制御装置は、前記移動体に搭載されている内界センサにより計測された前記移動体の姿勢および動作状態に基づき、前記移動体の推定位置を内界推定位置として算出する内界推定要素をさらに備え、前記第２フィルタ要素は、前記内界推定要素により算出された内界推定位置の履歴に対して低周波成分を減衰させるフィルタリングをさらに実行し、前記第２推定要素は、外界推定位置の履歴および内界推定位置の履歴のそれぞれに対する前記第２フィルタ要素によるフィルタリング処理結果に基づき、前記第２推定位置を算出する。
【００１２】
当該構成の制御装置によれば、第２フィルタ要素により内界推定位置の履歴（過去値）に対して低周波成分を減衰させるフィルタリング処理が実行され、当該フィルタリング処理結果と、前記の外界推定位置の履歴（過去値）に対するフィルタリング処理結果とに基づいて第２推定位置が算出される。このため、内界センサによる計測結果にドリフト（低周波のノイズ）が含まれている場合でも、当該ドリフトの影響を軽減して第２推定位置、ひいては第１推定位置の確度の向上が図られる。
【００１３】
前記第１フィルタ要素または前記第２フィルタ要素において、前記外界推定位置の履歴に対するフィルタリングおよび前記内界推定位置の履歴に対するフィルタリングのそれぞれを表わす伝達関数の和が１になるように当該伝達関数が設定される。
【００１４】
当該構成の制御装置によれば、外界推定位置の確度が高いにもかかわらず、外界推定位置の履歴のフィルタリング処理結果（低周波成分）のみならず、内界推定位置の履歴のフィルタリング処理結果（高周波成分）が過度に反映されることで第１推定位置または第２推定位置の確度が低下する事態が回避される。同様に、内界推定位置の確度が高いにもかかわらず、内界推定位置の履歴のフィルタリング処理結果（高周波成分）のみならず、外界推定位置の履歴のフィルタリング処理結果（低周波成分）が過度に反映されることで第１推定位置または第２推定位置の確度が低下する事態が回避される。これにより、第１推定位置または第２推定位置（および第１推定位置）の周波数依存性の低下が図られる。
【００１５】
本発明の制御装置において、前記第１フィルタ要素または前記第２フィルタ要素がローパスフィルタおよびハイパスフィルタにより構成されている。
【００１６】
当該構成の制御装置によれば、各フィルタ要素の構成の簡素化および各時定数の調節の簡易化が図られる。
【００１７】
本発明の制御装置において、前記第１推定要素および前記第２推定要素のうち少なくとも一方は、前記外界推定要素により算出される外界推定位置の履歴のばらつきの大きさに応じて前記第１時定数および前記第２時定数のうち少なくとも１つを動的に変更する。
【００１８】
当該構成の制御装置によれば、外界推定位置の履歴のばらつきが大きく、その確度が低い状況に応じて適応的に第１時定数または第２時定数が変更されることにより、当該ばらつきの影響を低減して第１推定位置または第２推定位置の確度の向上を図ることができる。
【００１９】
本発明の制御装置において、前記第１推定要素および前記第２推定要素のうち少なくとも一方は、前記外界センサを通じて計測された前記ランドマークの位置と前記第１推定位置または前記第２推定位置との間隔が閾値以上である場合、前記第１時定数および前記第２時定数のうち少なくとも１つを大きくする。
【００２０】
当該構成の制御装置によれば、移動体から比較的遠くに存在するランドマークを基準として外界推定位置が算出されることに由来する高周波のノイズの影響を軽減して第１推定位置または第２推定位置の確度の向上を図ることができる。
【００２１】
本発明の制御装置において、前記第１推定要素および前記第２推定要素のうち少なくとも一方は、前記外界画像における前記ランドマークの位置が当該外界画像の所定画素領域にある場合、前記第１時定数および前記第２時定数のうち少なくとも１つを大きくする。
【００２２】
当該構成の制御装置によれば、外界センサの特性（外界センサを構成する撮像装置のレンズのディストーションなど）に鑑みて、外界画像のうち信頼性が低い画素領域に存在するランドマークを基準として外界推定位置が算出されることに由来する高周波のノイズの影響を軽減して第１推定位置または第２推定位置の確度の向上を図ることができる。
【図面の簡単な説明】
【００２３】
【図１】本発明の一実施形態としての制御装置の構成説明図。
【図２】本発明の一実施形態としての制御装置の機能説明図。
【図３】図３Ａは検出マークおよび登録マークの対応付けに関する説明図。図３Ｂは当該対応付けに基づく外界推定位置の算出手法に関する説明図。
【図４】外界推定位置の算出遅れに関する説明図。
【図５】自己位置推定結果（実施例）に関する説明図。
【図６】自己位置推定結果（比較例）に関する説明図。
【図７】自己位置推定結果（他の実施例）に関する説明図。
【発明を実施するための形態】
【００２４】
（構成）
図１に示されている制御装置１は、コンピュータ（ＣＰＵ，ＲＯＭ，ＲＡＭ，Ｉ／Ｏ回路等により構成されている。）により構成されている。当該コンピュータは制御対象である車両などの移動体に搭載され、これにより当該移動体は自律的に移動することが可能に構成されている。当該コンピュータは制御対象に対して指令信号を付与する携帯端末またはサーバなどの外部機器を構成していてもよい。
【００２５】
制御装置１は、内界推定要素１１０と、外界推定要素１２０と、第１推定要素２１０と、第２推定要素２２０と、指令信号生成要素３００と、を備えている。内界推定要素１１０は、姿勢推定要素１１２および内界位置算出要素１１４を備えている。外界推定要素１２０は、ランドマーク相対位置算出要素１２２、ランドマーク絶対位置算出要素１２４および外界位置算出要素１２６を備えている。第１推定要素２１０は、第１ローパスフィルタ２１２、第１ハイパスフィルタ２１４、第１短期デッドレコニング要素２１６および第１推定位置算出要素２１８を備えている。第２推定要素２２０は、第２ローパスフィルタ２２２、第２ハイパスフィルタ２２４、第２短期デッドレコニング要素２２６および第２推定位置算出要素２２８を備えている。
【００２６】
第１ローパスフィルタ２１２および第１ハイパスフィルタ２１４が本発明の「第１フィルタ要素」を構成する。第１フィルタ要素は、カルマンフィルタにより構成されていてもよい。第２ローパスフィルタ２２２および第２ハイパスフィルタ２２４が本発明の「第２フィルタ要素」を構成する。第２フィルタ要素は、カルマンフィルタにより構成されていてもよい。
【００２７】
制御装置１の構成要素は後述する演算処理を実行するように構成されている。各要素が担当演算処理を実行するように「構成されている」とは、要素を構成するＣＰＵ等の演算処理装置が、ＲＯＭ、ＲＡＭ等のメモリ又は記録媒体から必要な情報に加えてソフトウェアを読み出し、当該情報に対して当該ソフトウェアにしたがって演算処理を実行するように「プログラムされている」または「デザイン（設計）されている」ことを意味する。各構成要素が共通のプロセッサ（演算処理装置）により構成されてもよく、各構成要素が相互通信可能な複数のプロセッサにより構成されてもよい。
【００２８】
（機能）
内界推定要素１１０により、内界センサＳ１により計測された移動体の姿勢および動作状態に基づき、当該移動体の推定位置が内界推定位置ｐ1として算出される（図２／ＳＴＥＰ１１０）具体的には、姿勢推定要素１１２が、移動体に搭載されている内界センサＳ１の１つであるＧジャイロセンサの出力信号に基づき、ストラップダウン法にしたがって絶対座標系（世界座標系）における移動体の１次推定姿勢θ＝（θx、θy、θz）を算出する。さらに内界位置算出要素１１４が、１次推定姿勢θ、２次推定姿勢θ’（後述）および移動体の駆動用アクチュエータＤの状態に応じた内界センサＳ１の１つであるアクチュエータセンサの出力信号に基づき、移動体の絶対座標系における位置を内界推定位置ｐ1＝（ｐ1x，ｐ1y，ｐ1z）として算出する。アクチュエータセンサは、たとえば移動体がクローラ式車両である場合、左右のクローラ速度を検出するセンサにより構成されている。
【００２９】
外界推定要素１２０により、地図において予め設定されているランドマークと、移動体に対して固定されている外界センサＳ２を通じて撮像された当該移動体の外界画像に含まれるランドマークとの対応付けに基づき、移動体の推定位置が外界推定位置ｐ2として算出する（図２／ＳＴＥＰ１２０）。具体的には、ランドマーク相対位置算出要素１２２が、移動体に対して固定されている外界センサＳ２としてのステレオカメラにより撮像された外界画像に含まれるランドマークの画像に基づき、移動体座標系（またはカメラ座標系）における当該ランドマークの３次元位置を算出する。移動体の通行帯の両脇に設けられているポール等の構造物のほか、通行帯の左右を画定するレーンマークなどがランドマークに該当する。ステレオカメラは、赤外光を照射し、ランドマークによる反射光を検知する方式のアクティブセンサであってもよく、ＣＣＤカメラ等のパッシブセンサであってもよい。
【００３０】
さらにランドマーク絶対位置算出要素１２４が、移動体座標系におけるランドマークの３次元位置の算出結果を、第２推定要素２２０により算出される第２推定位置ｑ^2および第２推定姿勢θ2（後述）および姿勢推定要素１１２により算出された１次推定姿勢θに基づいて座標変換することにより、絶対座標系におけるランドマークの３次元位置を算出する。
【００３１】
そして、外界位置算出要素１２６が、制御装置１を構成する記憶装置に予め登録されている絶対座標系におけるランドマークの３次元位置（ｘ、ｙおよびｚ座標値）と、絶対座標系におけるランドマークの３次元位置の算出結果（ｘ、ｙおよびｚ座標値）とを対応付けることにより、絶対座標系における移動体の３次元位置を外界推定位置ｐ2＝（ｐ2x，ｐ2y，ｐ2z）として算出するとともに、絶対座標系における移動体の姿勢を２次推定姿勢θ’＝（θx、θy、θ’z）として算出する。
【００３２】
具体的には、図３Ａに示されているように、登録されているランドマーク（登録ランドマーク）（○）のうち、位置が検出されたランドマーク（検出マーク）（●）を基準とする一定領域内に含まれる登録マークが当該検出マークと対応付けられる。そして、図３Ｂに示されているように、当該対応付けられた登録マークに検出マークを一致させるように移動体（移動体座標系）の位置および姿勢（方位角度またはｚ軸回りの角度）が補正されることで外界推定位置ｐ2および２次推定姿勢θ’が算出される。
【００３３】
第１推定要素２１０により、内界推定位置および外界推定位置の算出結果に基づき、移動体の推定位置が第１推定位置として算出される（図２／ＳＴＥＰ２１０）。具体的には、第１ローパスフィルタ２１２が、外界推定位置ｐ2および２次推定姿勢θ’のそれぞれの履歴に対して、第１時定数Ｔ1により表わされる減衰特性にしたがって高周波成分を減衰させるフィルタリングを実行する。第１ローパスフィルタ２１２は１次ローパスフィルタであり、その伝達関数ＧLPF1は式（１１）により表わされる。なお、第１ローパスフィルタ２１２は、２次以上のローパスフィルタであってもよい。
【００３４】
ＧLPF1＝１／（１＋Ｔ1ｓ）‥（１１）。
【００３５】
第１ハイパスフィルタ２１４が、内界推定位置ｐ1および１次推定姿勢θの履歴に対して、低周波成分を減衰させるフィルタリングを実行する。第１ハイパスフィルタ２１４の伝達関数ＧHPF1は式（１２）により表わされる。
【００３６】
ＧHPF1＝Ｔ1ｓ／（１＋Ｔ1ｓ）‥（１２）。
【００３７】
すなわち、第１ローパスフィルタ２１２の伝達関数ＧLPF1および第１ハイパスフィルタ２１４の伝達関数ＧHPF1の和が「１」に設計または調節されている。なお、当該和が１とは異なる値であってもよい。たとえば、第１ハイパスフィルタ２１４の伝達関数ＧHPF1が式（１２’）により表わされてもよい。
【００３８】
ＧHPF1＝Ｔ1ｓ／（１＋Ｔ1ｓ＋α（Ｔ1ｓ）2）（０＜α＜１）‥（１２’）。
【００３９】
第１短期デッドレコニング要素２１６が、内界推定位置ｐ1および１次推定姿勢θ（正確にはｚ軸回り成分のみθz）のそれぞれの変化速度ｖ1＝（ｖ1x，ｖ1y，ｖ1z）およびωz（ｚ成分）のそれぞれを一定期間にわたり積分または累積する。
【００４０】
そして、第１推定位置算出要素２１８が、第１ローパスフィルタ２１２、第１ハイパスフィルタ２１４および第１短期デッドレコニング要素２１６のそれぞれの出力を加算することにより、第１推定位置ｑ1＝（ｑ1x，ｑ1y，ｑ1z）および第１推定姿勢θ1＝（θ1x，θ1y，θ1z）を算出する。第１推定位置ｑ1は式（２１１）〜（２１３）により表わされる。第１推定姿勢θ1は式（２１４）〜（２１６）により表わされる。
【００４１】
0006397355.tif 000002
【００４２】
たとえば図４に示されているように、外界センサＳ２を通じて外界画像は周期Δ１（たとえば約２５［ｍｓ］）ごとに取得される一方、外界推定位置ｐ2はΔ１より長い周期Δ２（たとえば約５０［ｍｓ］）ごとに算出される。外界画像が取得されてから外界推定位置ｐ2が算出されるまでには１００〜４００［ｍｓ］程度の時間を要する。このため、内界推定位置ｐ1および１次推定姿勢θのそれぞれの変化速度の時刻ｔ’から時刻ｔまでの積分値（式（２１１）〜（２１３）および（２１６）の右辺第３項）により、この時間遅れの影響が補償された形で時刻ｔにおける第１推定位置ｑ1および第１推定姿勢θ1が算出される。これは後述する第２推定位置ｑ2および第２推定姿勢θ2の算出についても同様である。
【００４３】
第２推定要素２２０により、内界推定位置および外界推定位置の算出結果に基づき、移動体の推定位置が第２推定位置として算出される（図２／ＳＴＥＰ２２０）。具体的には、第２ローパスフィルタ２２２が、外界推定位置ｐ2および２次推定姿勢θ’のそれぞれの履歴に対して、第１時定数Ｔ1よりも小さい第２時定数Ｔ2により表わされる減衰特性にしたがって高周波成分を減衰させるフィルタリングを実行する。第２ローパスフィルタ２２２は１次ローパスフィルタであり、その伝達関数ＧLPF2は式（２１）により表わされる。なお、第２ローパスフィルタ２２２は、２次以上のローパスフィルタであってもよい。
【００４４】
ＧLPF2＝１／（１＋Ｔ2ｓ）‥（２１）。
【００４５】
第２ハイパスフィルタ２２４が、内界推定位置ｐ1および１次推定姿勢θの履歴に対して、低周波成分を減衰させるフィルタリングを実行する。第２ハイパスフィルタ２２４の伝達関数ＧHPF2は式（２２）により表わされる。
【００４６】
ＧHPF2＝Ｔ2ｓ／（１＋Ｔ2ｓ）‥（２２）。
【００４７】
すなわち、第２ローパスフィルタ２２２の伝達関数ＧLPF2および第２ハイパスフィルタ２２４の伝達関数ＧHPF2の和が「１」に設計または調節されている。なお、当該和が１とは異なる値であってもよい。たとえば、第２ハイパスフィルタ２２４の伝達関数ＧHPF2が式（２２’）により表わされてもよい。
【００４８】
ＧHPF2＝Ｔ2ｓ／（１＋Ｔ2ｓ＋β（Ｔ2ｓ）2）（０＜β＜１）‥（２２’）。
【００４９】
第２短期デッドレコニング要素２２６が、内界推定位置ｐ1および１次推定姿勢θのそれぞれの変化速度ｖ1＝（ｖ1x，ｖ1y，ｖ1z）およびωz（ｚ成分）のそれぞれを一定期間にわたり積分または累積する。
【００５０】
そして、第２推定位置算出要素２２８が、第２ローパスフィルタ２２２、第２ハイパスフィルタ２２４および第２短期デッドレコニング要素２２６のそれぞれの出力を加算することにより、第２推定位置ｑ2＝（ｑ2x，ｑ2y，ｑ2z）および第２推定姿勢θ2＝（θ2x，θ2y，θ2z）を算出する。第１推定位置ｑ1は式（２２１）〜（２２３）により表わされる。第２推定姿勢θ2は式（２２４）〜（２２６）により表わされる。
【００５１】
0006397355.tif 000003
【００５２】
指令信号生成要素３００により、第１推定要素２１０により算出された第１推定位置ｑ1および第１推定姿勢θ1に基づき、アクチュエータＤの指令信号が生成される（図２／ＳＴＥＰ３００）。これにより、たとえばランドマークにより左右の境界が画定される走行範囲から逸脱しないように移動体を走行させるようにアクチュエータＤの動作が指令信号にしたがって制御される。
【００５３】
（効果）
前記機能を発揮する制御装置１によれば、地図において予め設定されているランドマークと、外界センサＳ２を通じて撮像された移動体の外界画像に含まれるランドマークとの対応付けに基づき、外界推定位置ｐ2（ｔ”）（今回値）が算出される（図２／ＳＴＥＰ１２０、図４参照）。第２ローパスフィルタ２２２により外界推定位置ｐ2（ｔ’）（過去値）がフィルタリング処理された結果に基づいて第２推定位置ｑ2（ｔ”）が算出され、この第２推定位置ｑ2（ｔ”）が当該対応付けの基準として用いられる（式（２２１）〜（２２３）および図３Ａ参照）。このため、外界センサＳ２により撮像された移動体の外界画像におけるランドマークの出現態様が不安定であって外界推定位置ｐ2に高周波のノイズ（または外界センサＳ２を構成するカメラのひずみ（レンズのディストーションなど）に起因するノイズ）が出現する場合でも、第１推定位置ｑ1の確度（移動体の位置推定精度）の向上の観点から、第２推定位置ｑ2が外界推定位置ｐ2の適当な探索初期位置として決定される。
【００５４】
図５には、ｘ−ｙ平面において右上領域から左下領域まで移動体が移動した際における、第１推定位置ｑ1、第２推定位置ｑ2および外界推定位置ｐ2のそれぞれの算出結果が、実線、一点鎖線および二点鎖線のそれぞれによって示されている。破線で囲まれた領域において、外界センサＳ２により検出されたランドマークと登録ランドマークとの対応付け（図３Ａ参照）が不首尾になったため、外界推定位置ｐ2（二点鎖線参照）が不安定であって高周波のノイズが出現している。しかるに、移動体の実位置（真値）との誤差が非常に小さい第１推定位置ｑ1（実線参照）が算出されていることがわかる。
【００５５】
図６には、第２推定位置ｑ2が算出されず、ランドマーク絶対位置算出要素１２４により、ランドマークの絶対位置の算出基準として第１推定位置ｑ1（二点鎖線参照）の履歴がそのまま用いられた場合における第１推定位置ｑ1（実線参照）の算出結果が示されている。この場合、図６の破線で示されている領域において、外界センサＳ２により検出されたランドマークと登録ランドマークとの対応付けが不首尾になったため、外界推定位置ｐ2（二点鎖線参照）が不安定であって高周波のノイズが出現し、外界推定位置ｐ2が算出できなくなる。このため、内界センサＳ１による計測結果に含まれているドリフト（低周波のノイズ）が累積的に第１推定位置ｑ1（実線参照）に反映され、第１推定位置ｑ1が移動体の実位置（真値）から乖離していることがわかる。
【００５６】
さらに、第２ローパスフィルタ２２２の第２時定数Ｔ2は第１時定数Ｔ1よりも小さいため、その分だけ広い周波数領域にわたり低周波成分を当該フィルタリング処理の結果に反映させることができる。このため、外界画像におけるランドマークの出現態様が不安定状態から安定状態に遷移する過程（または外界画像におけるランドマークの出現態様が安定状態から不安定状態に遷移する過程）において、低周波領域で信頼性が高い外界センサＳ２による取得情報を早期に第２ローパスフィルタ２２２によるフィルタリング処理結果に反映させることができる。これにより、外界推定位置ｐ2の確度、ひいては第１推定位置ｑ1の確度のさらなる向上が図られる。
【００５７】
たとえば図５の破線で囲まれた領域において、前記のように第２推定位置ｑ2（一点鎖線参照）が不安定になっている。しかるに、その後移動体が左下領域に移動した際、外界推定位置ｐ2の安定化に伴って第２推定位置ｑ2が迅速に変化し、さらには移動体の実位置（真値）（破線参照）との誤差が非常に小さい第１推定位置ｑ1（実線参照）が算出されていることがわかる。
【００５８】
第１ハイパスフィルタ２１４および第２ハイパスフィルタ２２４のそれぞれにより内界推定位置ｐ1（ｔ’）（過去値）に対して低周波成分を減衰させるフィルタリング処理が実行され、当該フィルタリング処理結果と、外界推定位置ｐ2（ｔ’）（過去値）に対するフィルタリング処理結果とに基づいて第１推定位置ｑ1（ｔ）および第２推定位置ｑ2（ｔ）のそれぞれが算出される（式（２１１）〜（２１３）および式（２２１）〜（２２３）のそれぞれの右辺第１項および第２項参照）。このため、内界センサＳ１による計測結果にドリフト（低周波のノイズ）が含まれている場合でも、当該ドリフトの影響を軽減して第２推定位置ｑ2、ひいては第１推定位置ｑ1の確度の向上が図られる。
【００５９】
図７には、ｘ−ｙ平面において右から左まで移動体が移動した際における、第１推定位置ｑ1、内界推定位置ｐ1および外界推定位置ｐ2のそれぞれの算出結果ならびに移動体の実位置（真値）が、実線、一点鎖線、二点鎖線および破線のそれぞれによって示されている。内界センサＳ１による計測結果にドリフト（低周波のノイズ）が含まれているため、内界推定位置ｐ1（一点鎖線参照）が真値（破線参照）から徐々に大きくずれている。また、外界センサＳ２による計測結果に高周波のノイズが含まれているため、外界推定位置ｐ2（二点鎖線参照）が不安定になっている。しかるに、移動体の実位置（真値）との誤差が非常に小さい第１推定位置ｑ1（実線参照）が算出されていることがわかる。
【００６０】
（本発明の他の実施形態）
第１推定要素２１０および第２推定要素２２０のうち少なくとも一方が、外界推定要素１２０により算出される外界推定位置ｐ2の履歴のばらつきの大きさに応じて第１時定数Ｔ1および第２時定数Ｔ2のうち少なくとも１つを動的に変更してもよい。
【００６１】
当該構成の制御装置１によれば、外界推定位置ｐ2の履歴のばらつきが大きく、その確度が低い状況に応じて適応的に第１時定数Ｔ1または第２時定数Ｔ2が変更されることにより、当該ばらつきの影響を低減して第１推定位置ｑ1または第２推定位置ｑ2の確度の向上を図ることができる。
【００６２】
外界センサＳ２を通じて計測されたランドマークの位置と第１推定位置ｑ1または第２推定位置ｑ2との間隔が閾値以上である場合、第１時定数Ｔ1および第２時定数Ｔ2のうち少なくとも１つが大きくなるように調節されてもよい。この場合、移動体から比較的遠くに存在するランドマークを基準として外界推定位置ｐ2が算出されることにより、当該外界推定位置ｐ2の履歴に反映される高周波のノイズの影響を軽減して移動体の位置推定精度の向上を図ることができる。
【００６３】
外界画像におけるランドマークの位置が当該外界画像の所定画素領域に存在する場合、第１時定数Ｔ1および第２時定数Ｔ2のうち少なくとも１つが大きくなるように調節されてもよい。この場合、外界センサＳ２（ステレオカメラ）におけるレンズのディストーション等の特性に鑑みて外界画像のうち信頼性が低い画素領域に存在するランドマークを基準として外界推定位置が算出されることに由来する高周波のノイズの影響を軽減して移動体の位置推定精度の向上を図ることができる。
【符号の説明】
【００６４】
１‥制御装置、１１０‥内界推定要素、１２０‥外界推定要素、２１０‥第１推定要素、２１２、２１４‥フィルタ（第１フィルタ要素）、２２０‥第２推定要素、２２２、２２４‥フィルタ（第２フィルタ要素）、３００‥指令信号生成要素、Ｓ１‥内界センサ、Ｓ２‥外界センサ。
【図１】
0006397355.tif 000004
【図２】
0006397355.tif 000005
【図３】
0006397355.tif 000006
【図４】
0006397355.tif 000007
【図５】
0006397355.tif 000008
【図６】
0006397355.tif 000009
【図７】
0006397355.tif 000010