JP6747157B2 - 自己位置推定方法及び自己位置推定装置 - Google Patents

Description

本発明は、自己位置推定方法及び自己位置推定装置に関する。

車両等の移動体の自己位置を推定する技術として、移動体を基準として予め定めた領域内に存在する周囲の環境情報を限定し、限定した環境情報を環境地図と照合して、移動体の自己位置を推定する技術が知られている（特許文献１参照）。特許文献１に記載された技術では、移動体が車両である場合、左右輪の回転角と回転角速度に応じて車両の移動量を求める方法（オドメトリ）により自己位置が推定される。

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

しかしながら、特許文献１に記載された技術では、雨天時のカーブ等で車両のタイヤがスリップしたタイヤスリップ区間においては、車両の移動量の検出値に誤差が発生する。このため、タイヤスリップ区間の通過位置以前に検出した物標位置に誤差が発生することとなり、車両の自己位置の推定精度が低下するおそれがある。

上記問題点に鑑み、本発明は、移動体の自己位置を推定する際に、移動体のタイヤスリップによって移動量の検出誤差が発生した場合でも、自己位置の推定精度の低下を抑制することができる自己位置推定方法及び自己位置推定装置を提供することを目的とする。

本発明の一態様によれば、移動体の周囲に存在する物標の移動体に対する相対位置を検出し、検出された物標の移動体に対する相対位置を移動体の移動量だけ移動させた物標位置データを蓄積し、蓄積された物標位置データと、地図上に存在する物標の物標位置情報を含む地図情報とを照合することにより、移動体の自己位置を推定する。この際、移動体の移動路においてタイヤスリップ区間を検出し、蓄積された物標位置データのうち、タイヤスリップ区間以外の区間で蓄積された物標位置データを選択し、選択された物標位置データと地図情報とを照合することにより、移動体の自己位置を推定することを特徴とする自己位置推定方法及び自己位置推定装置が提供される。

本発明によれば、移動体の自己位置を推定する際に、移動体のタイヤスリップによって移動量の検出誤差が発生した場合でも、自己位置の推定精度の低下を抑制することができる自己位置推定方法及び自己位置推定装置を提供することができる。

本発明の実施形態に係る自己位置推定装置の一例を示すブロック図である。 本発明の実施形態に係る自己位置推定回路の一例を示すブロック図である。 本発明の実施形態に係る自車両の走行シーンを示す概略図である。 比較例に係る自車両のタイヤスリップ時の物標位置データの蓄積処理を説明するための概略図である。 比較例に係る自車両のタイヤスリップ時の自己位置の推定処理を説明するための概略図である。 本発明の実施形態に係る自車両のタイヤスリップ時の物標位置データの蓄積処理を説明するための概略図である。 本発明の実施形態に係る自車両のタイヤスリップ時の自己位置の推定処理を説明するための概略図である。 本発明の実施形態に係る自己位置推定方法の一例を説明するためのフローチャートである。 本発明の実施形態に係るスリップ区間判定処理の一例を説明するためのフローチャートである。

以下において、図面を参照して、本発明の実施形態を説明する。以下の図面の記載において、同一又は類似の部分には同一又は類似の符号を貼付している。但し、図面は模式的なものであり、厚みと平面寸法との関係、厚みの比率等は現実のものとは異なることに留意すべきである。また、図面相互間においても互いの寸法の関係や比率が異なる部分が含まれていることは勿論である。また、以下に示す本発明の実施形態は、本発明の技術的思想を具体化するための装置や方法を例示するものであって、本発明の技術的思想は、構成部品の材質、形状、構造、配置等を下記のものに特定するものではない。本発明の技術的思想は、特許請求の範囲に記載された請求項が規定する技術的範囲内において、種々の変更を加えることができる。

＜自己位置推定装置＞
本発明の実施形態に係る自己位置推定装置は、図１に示すように、車両（以下、「自車両」という）ＭＣに搭載可能である。本発明の実施形態に係る自己位置推定装置は、自己位置推定回路１、物標センサ２、走行情報センサ５及び運転支援システム９を備える。

物標センサ２は、自車両ＭＣの前方等の自車両ＭＣの周囲に存在する物標を検出する。ここで、物標とは、例えば、自車両ＭＣが走行する走行路面Ｒ０上の線（車線区分線や停止線等）や、路肩の縁石、ガードレール、標識等である。物標センサ２は、例えば、広角カメラ等の撮像装置３や、レーザーレンジファインダ（ＬＲＦ）等の距離測定装置４を備える。撮像装置３は、自車両ＭＣの車室内に搭載され、自車両ＭＣの前方領域を撮像する。距離測定装置４は、自車両ＭＣの車室外に搭載され、自車両ＭＣの前方領域に電磁波を照射し、その反射波を検出する。距離測定装置４は、自車両ＭＣのボンネット、バンパー、ナンバープレート、ヘッドライト又はサイドミラー周辺に搭載可能である。

走行情報センサ５は、自車両ＭＣの移動量を推定するために、自車両ＭＣの走行情報を検出する。走行情報センサ５は、例えば、車輪速センサ６、ジャイロセンサ７、操舵角センサ８を備える。車輪速センサ６は、自車両ＭＣの車輪Ｗの回転に応じて発生させた車輪速パルスを自車両ＭＣの走行情報として検出する。操舵角センサ８は、例えば、ステアリングホイール（図示せず）を回転可能に支持するステアリングコラム（図示せず）に設けられ、操舵操作子であるステアリングホイールの回転角度（操舵操作量）である操舵角を自車両ＭＣの走行情報として検出する。ジャイロセンサ７は、自車両ＭＣに発生するヨーレートを自車両ＭＣの走行情報として検出する。

自己位置推定回路１及び運転支援システム９のそれぞれは、例えば中央演算処理装置（ＣＰＵ）、主記憶装置、入出力装置、入出力インターフェイス、データバス等を含むコンピュータやコンピュータに等価な半導体集積回路等で構成することができる。自己位置推定回路１及び運転支援システム９のそれぞれを半導体集積回路で構成する場合は、フィールド・プログラマブル・ゲート・アレイ（ＦＰＧＡ）等のプログラマブル・ロジック・デバイス（ＰＬＤ）等の半導体集積回路を利用してもよく、汎用の半導体集積回路中に設定される機能的な論理回路や論理ブロック等でも構わない。自己位置推定回路１及び運転支援システム９のそれぞれが有するメモリは、半導体記憶装置、磁気記憶装置又は光学記憶装置等で構成でき、レジスタ、キャッシュメモリ等が含まれる。

自己位置推定回路１は、タイヤスリップの無い路面の走行においては、自車両ＭＣの左右輪Ｗの回転角と回転角速度に応じて自車両ＭＣの移動距離（移動量）と移動方向を求める方法（以下において「オドメトリ」と称する。）によって自車両ＭＣの自己位置を推定する。自己位置推定回路１は、図２に示すように、物標位置検出部１１、移動量推定部１２、物標位置蓄積部１３、タイヤスリップ検出部１４、物標選択部１５、地図情報取得部１６及び自己位置推定部１７を備える。

物標位置検出部１１は、物標センサ２による自車両ＭＣの周囲に存在する物標の検出結果に基づいて、自車両ＭＣの周囲に存在する物標と自車両ＭＣとの相対位置を検出する。物標位置検出部１１は、検出した物標と自車両ＭＣとの相対位置の情報を含む相対位置信号を物標位置蓄積部１３へ出力する。

移動量推定部１２は、車輪速センサ６等により検出される自車両ＭＣの左右輪Ｗの回転角と回転角速度を用いて、前回の処理周期からの自車両ＭＣの移動量ΔＰ及び移動方向を推定する。移動量推定部１２は、推定した自車両ＭＣの移動量ΔＰ及び移動方向の情報を含む移動量信号を、物標位置蓄積部１３へ出力する。

物標位置蓄積部１３は、物標位置検出部１１から出力された相対位置信号を入力データとして、自車両ＭＣの周囲に存在する物標と自車両ＭＣの相対位置を蓄積する。更に、物標位置蓄積部１３は、移動量推定部１２から出力された移動量信号を入力データとして、前回の処理周期までの過去に蓄積した物標と自車両ＭＣの相対位置のデータを、現在までの経過時間と、移動量信号が含む移動量ΔＰを用いて、現在の自車両ＭＣに対する相対位置へ補正し、物標位置データとして蓄積する。即ち、物標位置蓄積部１３は、過去に蓄積した物標の相対位置のデータを、移動量推定部１２が推定した現在までの経過時間の移動量ΔＰだけ自車両ＭＣの移動方向と逆方向に移動させた物標位置データを蓄積する。

物標位置蓄積部１３は、前回の処理周期までに既に物標位置データを蓄積している場合には、蓄積している物標位置データを、移動量信号が含む移動量ΔＰを用いて更新する。物標位置データの更新は、既に蓄積している物標位置データが含む相対位置を、移動量信号が含む移動量ΔＰ分だけ相対移動させる。物標位置蓄積部１３は、移動量ΔＰ分だけ相対移動させた相対位置を、蓄積している物標位置データに上書きして、物標位置データの更新を行う。

地図情報取得部１６は、地図上に存在する物標の位置情報を含む地図情報を取得する。例えば、地図情報取得部１６は、カーナビゲーションシステムや地図データベース等で構成できる。なお、地図情報取得部１６は、無線通信（路車間通信又は車車間通信でも可）等の通信システムを介して外部から地図情報を取得してもよい。この場合、地図情報取得部１６は、定期的に最新の地図情報を入手して、保有する地図情報を更新してもよい。地図情報取得部１６は、自車両ＭＣが実際に走行した走路を、地図情報として蓄積してもよい。

自己位置推定部１７は、タイヤスリップの無い路面の走行においては、物標位置蓄積部１３により蓄積された物標位置データを、地図情報取得部１６が取得した地図上に存在する物標の位置情報と照合（マッチング）することにより、自車両ＭＣの位置及び姿勢を自己位置として推定する。

運転支援システム９は、自己位置推定部１７により推定された自車両ＭＣの自己位置を用いて、必要に応じて、警報等による自車両ＭＣの乗員に対する情報提示や、自車両ＭＣの制動等の運転支援を行う。

次に、図３〜図７を参照して、本発明の実施形態に係る自己位置推定装置を用いたオドメトリによる自己位置推定方法の一例を説明する。図３に示すように、走路境界線に区切られたＳ字カーブを含む車線があり、自車両ＭＣの進行方向（紙面の上方）に向かって右側の走路境界線上に物標Ａ１〜Ａ７が配置されている。物標Ａ１〜Ａ７は、白線や縁石、標識等である。

低μ路面やカーブ等ではタイヤスリップが発生する場合がある。図３の走行軌跡Ｒ１，Ｒ２，Ｒ３は、自車両ＭＣが通常路面を走行中にタイヤスリップ区間Ｌ１に進入し、路面のタイヤスリップが発生した場合に、オドメトリによって、自車両ＭＣが走行したことを推定した走行軌跡である。これに対して、走行軌跡Ｒ１，Ｒ４，Ｒ５は、自車両ＭＣが通常路面を走行中にタイヤスリップ区間Ｌ１に進入し、その後、通常路面へ戻った実際の走行軌跡を示している。自車両ＭＣが走行軌跡Ｒ１，Ｒ４，Ｒ５に沿って実際に走行する場合には、物標位置検出部１１が、自車両ＭＣの周囲に存在する物標Ａ１〜Ａ７の相対位置を検出する。移動量推定部１２が、自車両ＭＣの移動量ΔＰ及び移動方向を推定する。物標位置蓄積部１３が、物標位置検出部１１により検出された物標Ａ１〜Ａ７の相対位置を、移動量推定部１２により推定された自車両ＭＣの移動方向とは逆方向に、自車両ＭＣの移動量ΔＰだけ物標Ａ１〜Ａ７の相対位置を移動して物標位置データＢ１〜Ｂ７として蓄積し、更新していく。

図３に示すように、低μ路面やカーブ等ではタイヤスリップが発生する場合、タイヤスリップ区間Ｌ１の開始位置（始点）Ｐ１からタイヤスリップ区間Ｌ１の通過位置（終点）Ｐ２にかけて、オドメトリを作成した走行軌跡Ｒ１，Ｒ２，Ｒ３と実際の走行軌跡Ｒ１，Ｒ４，Ｒ５には位置誤差Ｘａ，Ｘｂ，Ｘｃが発生する。

図４に示すように、物標センサ２により検出された物標Ａ１〜Ａ７と自車両ＭＣの相対位置は、実際の走行軌跡Ｒ１，Ｒ４，Ｒ５に沿った自車両ＭＣに対する相対位置である。一方、オドメトリにより蓄積された物標位置データＢ１〜Ｂ７は、走行軌跡Ｒ１，Ｒ２，Ｒ３に沿って走行し図４の破線で示す自車両ＭＣの位置に到達したものとして推測されている。このため、タイヤスリップ区間Ｌ１を通過前の区間で蓄積された物標位置データＢ１，Ｂ２と、タイヤスリップ区間Ｌ１を通過後の区間で蓄積された物標位置データＢ６，Ｂ７では誤差は発生しないが、タイヤスリップ区間Ｌ１を通過中に蓄積された物標位置データＢ３〜Ｂ５では誤差が発生する。

このような状態で自己位置推定を行うと、タイヤスリップ区間Ｌ１を通過後に位置決めされた場合は正しい解を推定することになるが、タイヤスリップ区間Ｌ１を通過中はタイヤスリップによる距離差を含んだ誤った解であるので、例えば、最小誤差でマッチングを行った場合には、推定誤差が小さくならない場合がある。例えば、タイヤスリップ区間Ｌ１を通過して車線変更して隣の車線に進入するとき等、隣の車線のレーンや縁石等の物標を用いて自己位置推定を行いたいが、タイヤスリップ区間Ｌ１を通過後の物標を蓄積するまではしばらくはタイヤスリップ区間Ｌ１を通過前の物標の方のデータ数が多いため、車線変更先の隣の車線の物標ではなく、通過前の横位置誤差を含む物標が、マッチングで支配的に作用することになり、推定誤差が大きくなってしまうことがある。

また、図５に示すように、物標位置データＢ１〜Ｂ７の軌跡は、地図情報に含まれる物標位置情報Ｃ１〜Ｃ７の軌跡に対して乖離した形状となり、例えばタイヤスリップ区間Ｌ１の開始前の区間で蓄積された物標位置データＢ１，Ｂ２と地図情報に含まれる物標位置情報Ｃ１，Ｃ２との差Ｘ１，Ｘ２に対して、タイヤスリップ区間Ｌ１の通過後の区間で蓄積された物標位置データＢ６，Ｂ７と、地図情報に含まれる物標位置情報Ｃ６，Ｃ７との差Ｘ６，Ｘ７が大きくなる。このため、図５の領域Ｒａに示すように、誤差のないタイヤスリップ区間Ｌ１の開始前の区間で蓄積された物標位置データＢ１，Ｂ２を優先して地図情報に含まれる物標位置情報Ｃ１，Ｃ２とマッチングさせた場合と、図５の領域Ｒｂに示すように、タイヤスリップ区間Ｌ１の通過後の区間で蓄積された物標位置データＢ６，Ｂ７を優先して地図情報に含まれる物標位置情報Ｃ６，Ｃ７とマッチングさせた場合とで、推定される自己位置が二つ存在し、唯一の解に定まらない。その結果、タイヤスリップ区間Ｌ１の開始前に位置決めされたり、タイヤスリップ区間Ｌ１の通過後に位置決めされたり、不安定となる可能性がある。

そこで、本発明の実施形態においては、タイヤスリップ区間Ｌ１に起因した自己位置の推定精度の低下を抑制するために、図１に示したタイヤスリップ検出部１４及び物標選択部１５を備えている。タイヤスリップ検出部１４は、自車両ＭＣが走行中の走行路が、タイヤスリップ区間Ｌ１であるか否かを判定し、判定結果を含む判定結果信号を、物標選択部１５へ出力する。

タイヤスリップ区間Ｌ１の判定方法としては、例えば、車輪速センサ６により検出される自車両ＭＣの４輪の車輪速データの変化量から路面摩擦係数を推定する。そして、推定した路面摩擦係数が所定の閾値（例えば０．５）以上か否かを判定することにより、タイヤスリップ区間Ｌ１であるか否かを判定することができる。即ち、路面摩擦係数が所定の閾値以上の場合に、タイヤスリップ区間Ｌ１を通過中であると判定する。一方、路面摩擦係数が所定の閾値未満の場合に、タイヤスリップ区間Ｌ１を通過中ではないと判定する。

タイヤスリップ区間Ｌ１の判定方法は路面摩擦係数を用いる方法に限定されず、種々の方法が採用可能である。例えば、操舵角センサ８により検出される操舵角、ジャイロセンサ７により検出されるヨーレート等の車両挙動に関する検出値の相対関係から、タイヤスリップ区間Ｌ１であるか否かを判定することもできる。或いは、撮像装置３等から得られる画像による解析、３Ｄ点群のフロー、オドメトリ等から路面状態を推定することでタイヤスリップ区間Ｌ１を判定してもよい。

タイヤスリップ検出部１４は、タイヤスリップが発生を検出してタイヤスリップ区間Ｌ１の開始位置（始点）Ｐ１を推定する。タイヤスリップ検出部１４は、タイヤスリップの収まりを検出してタイヤスリップ区間Ｌ１の通過位置（終点）Ｐ２を推定する。

物標選択部１５は、タイヤスリップ検出部１４によりタイヤスリップ区間Ｌ１が検出された場合に、物標位置蓄積部１３が蓄積している物標位置データから、タイヤスリップ区間Ｌ１以外の区間で蓄積された物標位置データを、タイヤスリップ時の自己位置推定用の物標位置データとして選択する。物標選択部１５は、例えば図４に示すように、物標位置蓄積部１３が蓄積している物標位置データＢ１〜Ｂ７から、タイヤスリップ区間Ｌ１の開始前に蓄積した物標位置データＢ１，Ｂ２と、タイヤスリップ区間Ｌ１を通過中に蓄積された物標位置データＢ３〜Ｂ５を除外し、図６に示すように、タイヤスリップ区間Ｌ１を通過後の区間で蓄積された物標位置データＢ６，Ｂ７を選択する。なお、ここでは２点の物標位置データＢ６，Ｂ７を選択する場合を例示するが、選択する物標位置データ数は限定されることなく、マッチングにより自己位置を推定できる数であればよい。

タイヤスリップ区間Ｌ１の通過直後は、少なくとも自車両ＭＣの周囲に存在する物標位置データ及び自車両ＭＣの現在位置からタイヤスリップ区間Ｌ１の通過位置までの間に存在する物標位置データを選択する。自車両ＭＣの現在位置の周囲の物標位置データ及び、タイヤスリップ区間Ｌ１を通過後で、かつ、経過時間が比較的に、新しく蓄積された物標を優先して選択して自己位置推定に用いる。例えば、自車両ＭＣの現在位置の周囲は、２０ｍ程度で予め設定することができる。自車両ＭＣの現在位置の周囲の物標位置データは、自車両ＭＣの移動量検出の誤差が少ないので、位置精度が高い傾向にある。特に、道路境界であるレーンや縁石の位置データは走路内の横位置の精度が高い。

図６に示すように、物標選択部１５によりタイヤスリップ時の自己位置推定用の物標位置データＢ６，Ｂ７が選択された場合には、自己位置推定部１７は、図７に示すように、物標選択部１５により選択された物標位置データＢ６，Ｂ７を、地図情報取得部１６が取得した地図上に存在する物標の位置情報Ｃ６，Ｃ７と照合することにより、自車両ＭＣの現在位置Ｐ３及び姿勢を自己位置として推定する。

＜自己位置推定方法＞
次に、図８のフローチャートを参照しながら、本発明の実施形態に係る自己位置推定方法の一例を説明する。図８のフローチャートの手順は所定の処理周期で繰り返し実行され、各処理周期において物標位置蓄積部１３が物標位置データを蓄積していく。

ステップＳ１１において、物標位置検出部１１が、物標センサ２の検出結果から、自車両ＭＣの周辺に存在する物標と自車両ＭＣの相対位置を検出する。ステップＳ１２において、移動量推定部１２が、車輪速センサ６等により検出される左右輪Ｗの回転角及び回転角速度に応じて、自車両ＭＣの移動方向及び移動量を推定する。

ステップＳ１３において、物標位置蓄積部１３は、物標位置検出部１１により検出された自車両ＭＣの周辺に存在する物標と自車両ＭＣの相対位置を蓄積する。物標位置蓄積部１３は、過去に蓄積された自車両ＭＣの周辺に存在する物標と自車両ＭＣの相対位置を、過去に蓄積されたときから現在までの経過時間と、移動量推定部１２により推定された移動量を用いて、自車両ＭＣの現在位置に対する物標の相対位置に補正し、物標位置データとして蓄積する。

ステップＳ１４において、タイヤスリップ検出部１４は、タイヤスリップ区間を検出して、自車両ＭＣがタイヤスリップ区間を通過したか否かを判定する。自車両ＭＣがタイヤスリップ区間を通過していないと判定された場合、ステップＳ１６に移行する。一方、ステップＳ１４において自車両ＭＣがタイヤスリップ区間を通過したと判定された場合、ステップＳ１５に移行する。

ここで、図８のステップＳ１４のタイヤスリップ区間の通過判定処理の詳細の一例を、図９のフローチャートを参照しながら説明する。

ステップＳ２１において、タイヤスリップ検出部１４が、直前の処理周期において自車両ＭＣがタイヤスリップ区間を通過中であるか否かを判定する。自車両ＭＣがタイヤスリップ区間を通過中ではないと判定された場合、ステップＳ２２に移行する。

ステップＳ２２において、例えば路面摩擦係数が所定の閾値（例えば０．５）以上か否かを判定することにより、自車両ＭＣがタイヤスリップ区間に進入したか否かを判定する。路面摩擦係数が所定の閾値以上であり、タイヤスリップ区間に進入していないと判定された場合には、自車両ＭＣがタイヤスリップ区間を通過していないため、図８のステップＳ１６へ移行する。

一方、ステップＳ２２において、路面摩擦係数が所定の閾値未満であり、自車両ＭＣがタイヤスリップ区間に進入したと判定された場合には、ステップＳ２３に移行し、今回の処理周期における自車両ＭＣの位置をタイヤスリップ区間の開始位置として推定する。この場合、自車両ＭＣがタイヤスリップ区間を通過していないため、図８のステップＳ１６に移行する。

ステップＳ２１において、前回の処理周期において自車両ＭＣがタイヤスリップ区間を通過中であると判定された場合、ステップＳ２４に移行する。ステップＳ２４において、例えば路面摩擦係数が所定の閾値（例えば０．５）以上か否かを判定することにより、タイヤスリップ区間を通過したか否かを判定する。なお、ステップＳ２４で用いる所定の閾値と、ステップＳ２２で用いる所定の閾値とは、同じ値であってもよく、異なる値であってもよい。

ステップＳ２４において、路面摩擦係数が所定の閾値以上であり、タイヤスリップ区間を通過したと判定された場合、今回の処理周期における自車両ＭＣの位置をタイヤスリップ区間の通過位置として推定し、図８のステップＳ１５へ移行する。一方、ステップＳ２４において、路面摩擦係数が所定の閾値未満であり、タイヤスリップ区間を通過中であると判定された場合、自車両ＭＣがタイヤスリップ区間を通過していないため、図８のステップＳ１６に移行する。

図８のステップＳ１５において、物標選択部１５は、蓄積している物標位置データのうち、タイヤスリップ区間を通過前の物標位置データを除外する。或いは、データ容量を抑制する必要がある場合には、物標選択部１５は、蓄積している物標位置データから、タイヤスリップ区間を通過前の物標位置データを削除する。

ステップＳ１６において、地図情報取得部１６は、地図上に存在する物標の物標位置情報を含む地図情報を取得する。ステップＳ１７において、自己位置推定部１７は、地図情報取得部１６により取得された地図情報と、物標位置蓄積部１３により蓄積された物標位置データとを照合することにより、地図上の自車両ＭＣの位置及び姿勢を自己位置として推定する。ステップＳ１５を経由してステップＳ１７に移行した場合には、自己位置推定部１７は、タイヤスリップ区間以外の区間で蓄積された物標位置データを用いて地図上の自車両ＭＣの位置及び姿勢を自己位置として推定する。

＜自己位置推定プログラム＞
なお、本発明の実施形態に係る自己位置推定プログラムは、図８に示した自己位置推定方法の一連の処理を、図１に示した自己位置推定装置を構成する自己位置推定回路１等のコンピュータに実行させることができる。本発明の実施形態に係る自己位置推定プログラムは、例えば自己位置推定回路１のメモリ等に格納可能である。

以上説明したように、本発明の実施形態によれば、タイヤスリップ区間内の物標位置データを除外して、タイヤスリップ区間以外の区間から蓄積された物標位置データを選択する。そして、選択された物標位置データを地図情報と照合することにより、自車両ＭＣの自己位置を推定する。これにより、タイヤスリップによって発生する物標の位置の計測誤差による自己位置の推定精度低下を抑制することができる。例えば、タイヤスリップ区間を通過後に車線変更して隣接車線に進入するとき等、車線変更先の隣の車線のレーンや縁石等の正確な物標位置で自己位置推定が行われるため、自己位置を精度良く推定できる。

更に、タイヤスリップ区間を通過後の区間で蓄積された物標位置データを選択して、選択された物標位置データを用いて自車両ＭＣの自己位置を推定する。これにより、タイヤスリップによって移動量の検出誤差が発生しても、タイヤスリップ区間を通過前の距離差を含む物標に悪影響を及ぼされることなく、自己位置を精度良く推定できる。

更に、自車両ＭＣの現在位置の周囲の蓄積された物標位置データを選択して、選択された物標位置データを用いて自車両ＭＣの自己位置を推定する。これにより、自己位置の推定精度が高まる物標位置データを適切に選択することができる。

（第１の変形例）
本発明の実施形態の第１の変形例として、物標選択部１５が、タイヤスリップ検出部１４により検出されたタイヤスリップ区間が所定区間長以上の場合に、タイヤスリップ時の自己位置推定用の物標位置データを選択する場合を説明する。カーブや雨天時に路面が低μになった場合等のスリップが連続的に起きてタイヤスリップ区間が長くなるほど、自己位置推定に及ぼす影響は大きくなる。一方、自車両ＭＣが水たまりを通過するときや、自車両ＭＣのタイヤが石を踏んだとき等にスリップが瞬間的に起きてタイヤスリップ区間が比較的短い場合には、自己位置推定に及ぼす影響は比較的小さい。

そこで、物標選択部１５は、タイヤスリップ検出部１４が検出したタイヤスリップ区間が所定区間長以上か否かを判定する。所定区間長は適宜設定可能である。例えば、物標選択部１５は、タイヤスリップ検出部１４が検出したタイヤスリップ区間の通過時間が所定時間（例えば１秒）以上か否かを判定する。そして、タイヤスリップ区間の通過時間が所定時間（例えば１秒）以上と判定された場合に、物標選択部１５は、タイヤスリップ時の自己位置推定用の物標位置データを選択する。一方、タイヤスリップ区間の通過時間が所定時間（例えば１秒）未満と判定された場合に、物標選択部１５は、タイヤスリップ時の自己位置推定用の物標位置データを選択しない。つまり、タイヤスリップ時の物標位置データの選択処理を行わずに、通常処理によって物標位置データを用いて自己位置推定を行う。なお、通常処理の中で、他の条件で、物標位置データを選択することはできる。

第１の変形例によれば、タイヤスリップ区間が所定区間長（例えば１秒）以上の場合に、タイヤスリップ区間以外の区間から蓄積された物標位置データを選択することにより、自己位置推定に影響を及ぼすタイヤスリップ区間で蓄積された物標位置データを適切に除外して、自己位置を推定できる。一方、タイヤスリップ区間が所定区間長（例えば１秒）未満の場合には物標位置データを選択する処理を行わないことにより、計算負荷を軽減できる。

（第２の変形例）
本発明の実施形態の第２の変形例として、タイヤスリップ区間を通過後の物標位置データを利用して、タイヤスリップ区間の開始前の区間で蓄積された物標位置データを補正する場合を説明する。

自己位置推定部１７は、例えば図７に示すように、物標選択部１５により選択されたタイヤスリップ区間Ｌ１を通過後の物標位置データＢ６，Ｂ７を用いて、地図上の自車両ＭＣの現在の自己位置Ｐ３を推定する。自己位置推定部１７は、タイヤスリップ区間Ｌ１の開始前の区間で蓄積された物標位置データＢ１，Ｂ２を用いて推定した地図上の自車両ＭＣの過去の自己位置（ここでは例えば、自車両ＭＣの過去の自己位置を、タイヤスリップ区間Ｌ１の開始位置Ｐ１とする）を自己位置推定回路１のメモリから読み出す。自車両ＭＣの過去の自己位置Ｐ３は、以前の処理周期において自己位置推定部１７により推定され、蓄積されているものとする。自己位置推定部１７は、自車両ＭＣの現在の自己位置Ｐ３と、自車両ＭＣの過去の自己位置Ｐ１との相対位置を算出する。

自己位置推定部１７は、自車両ＭＣの現在の自己位置Ｐ３と、自車両ＭＣの過去の自己位置Ｐ１との相対位置から、タイヤスリップ区間Ｌ１の開始前の物標位置データＢ１，Ｂ２の地図上の位置を補正する。これによって、タイヤスリップ区間Ｌ１の開始前の交差点や停止線等、走路と直交した物標位置データを再利用することができ、自己位置推定の精度を向上することができる。

第２の変形例によれば、自己位置推定部１７が、タイヤスリップ区間Ｌ１の開始前の区間から蓄積された物標位置データＢ１，Ｂ２から推定した自己位置Ｐ１と、タイヤスリップ区間Ｌ１を通過後の区間から蓄積された物標位置データＢ６，Ｂ７から推定した自己位置Ｐ３との相対位置に基づいて、タイヤスリップ区間Ｌ１の開始前の区間の蓄積された物標位置データＢ１，Ｂ２を補正する。そして、自己位置推定部１７が、タイヤスリップ区間Ｌ１を通過後の区間から蓄積された物標位置データＢ６，Ｂ７に加えて、補正した物標位置データＢ１，Ｂ２を用いて自己位置を推定する。これにより、自己位置推定の精度を向上することができる。

（その他の実施形態）
上記のように、本発明は実施形態によって記載したが、この開示の一部をなす論述及び図面は本発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施形態、実施例及び運用技術が明らかとなろう。

例えば、物標選択部１５は、位置推定の高精度化や処理時間短縮のため、物標位置データの蓄積時又は選択時においてそれぞれ、タイヤスリップ区間の通過後に検出された物標位置データのうち、自車両ＭＣの走行方向に対して角度が大きいものを優先的に残すようにし、他の物標位置データを除外してもよい。或いは、物標選択部１５は、自車両ＭＣに対して所定の距離以上離間した物標位置データを除外してもよい。所定の距離は、タイヤスリップが大きく変化を繰り返すこと等による大きな測定誤差に起因する位置推定誤差に応じて、調整可能である。

また、本発明の実施形態においては、自己位置推定装置を自車両ＭＣに搭載した場合を例示したが、例えば、車輪を有するロボット等の車両以外の移動体にも自己位置推定装置を適用可能である。

本発明はここでは記載していない様々な実施形態等を含むことは勿論である。したがって、本発明の技術的範囲は上記の説明から妥当な特許請求の範囲に係る発明特定事項によってのみ定められるものである。

１…自己位置推定回路
２…物標センサ
３…撮像装置
４…距離測定装置
５…走行情報センサ
６…車輪速センサ
７…ジャイロセンサ
８…操舵角センサ
９…運転支援システム
１１…物標位置検出部
１２…移動量推定部
１３…物標位置蓄積部
１４…タイヤスリップ検出部
１５…物標選択部
１６…地図情報取得部
１７…自己位置推定部

Claims (6)

1. 移動体に搭載され、前記移動体の周囲に存在する物標の移動体に対する相対位置を検出する物標センサと、
   前記検出された物標の移動体に対する相対位置を前記移動体の移動量だけ移動させた物標位置のデータを、前記移動体を移動させながら蓄積していくことにより複数の物標位置のデータを蓄積し、前記蓄積された複数の物標位置のデータと、地図上に存在する物標の物標位置情報を含む地図情報とを照合することにより、前記移動体の自己位置を推定する自己位置推定回路とを用いた自己位置推定方法において、
   前記移動体の移動路においてタイヤスリップ区間を検出するステップと、
   前記蓄積された複数の物標位置のデータのうち、前記タイヤスリップ区間以外の区間で蓄積された物標位置のデータを選択するステップと、
   前記選択された物標位置のデータと前記地図情報とを照合することにより、前記移動体の自己位置を推定するステップ
   とを含むことを特徴とする自己位置推定方法。
2. 前記物標位置のデータを選択するステップは、前記タイヤスリップ区間を通過後の区間で蓄積された物標位置のデータを選択することを特徴とする請求項１に記載の自己位置推定方法。
3. 前記自己位置を推定するステップは、
   前記選択されたタイヤスリップ区間を通過後の区間で蓄積された物標位置のデータから推定した自己位置と、前記タイヤスリップ区間の開始前の区間で蓄積された物標位置のデータから推定した自己位置との相対位置から、前記タイヤスリップ区間の開始前の区間で蓄積された物標位置のデータを補正し、
   前記選択された物標位置のデータと、前記補正した物標位置のデータとを用いて、前記移動体の自己位置を推定する
   ことを特徴とする請求項２に記載の自己位置推定方法。
4. 前記物標位置のデータを選択するステップは、前記タイヤスリップ区間が所定区間長以上の場合に、前記物標位置のデータを選択することを特徴とする請求項１〜３のいずれか１項に記載の自己位置推定方法。
5. 前記物標位置のデータを選択するステップは、前記移動体の現在位置の周囲に存在する物標位置のデータを選択することを特徴とする請求項１〜４のいずれか１項に記載の自己位置推定方法。
6. 移動体に搭載され、前記移動体の周囲に存在する物標の移動体に対する相対位置を検出する物標センサと、
   前記検出された物標の移動体に対する相対位置を前記移動体の移動量だけ移動させた物標位置のデータを、前記移動体を移動させながら蓄積していくことにより複数の物標位置のデータを蓄積し、前記蓄積された複数の物標位置のデータと、地図上に存在する物標の物標位置情報を含む地図情報とを照合することにより、前記移動体の自己位置を推定する自己位置推定回路とを備え、
   前記自己位置推定回路が、前記移動体の移動路においてタイヤスリップ区間を検出し、
   前記蓄積された複数の物標位置のデータのうち、前記タイヤスリップ区間以外の区間で蓄積された物標位置のデータを選択し、前記選択された物標位置のデータと前記地図情報とを照合することにより、前記移動体の自己位置を推定することを特徴とする自己位置推定装置。

Images