WO2019189098A1

WO2019189098A1 - 自己位置推定装置、自己位置推定方法、プログラム及び記憶媒体

Info

Publication number: WO2019189098A1
Application number: PCT/JP2019/012721
Authority: WO
Inventors: 加藤　正浩; 岩井　智昭; 多史藤谷
Original assignee: パイオニア株式会社
Priority date: 2018-03-28
Filing date: 2019-03-26
Publication date: 2019-10-03
Also published as: EP3779510A1; US11822009B2; JPWO2019189098A1; EP3779510A4; JP2021181995A; JP6923750B2; US20210025981A1; JP2023118751A

Abstract

自己位置推定装置は、移動体に搭載され、移動体の予測位置を取得する。また、自己位置推定装置は、地図情報から取得した区画線の端点の情報に基づいて得られた区画線の端点の予測位置と、移動体に搭載された計測部が光を所定方向に走査することで計測された区画線の端点の計測位置との差分値を算出し、差分値に係数を乗じた値で予測位置を修正して前記移動体の自己位置を推定する。そして、自己位置推定装置は、区画線の端点を検出した位置における前記計測部の走査位置の間隔に基づいて係数を補正する。

Description

自己位置推定装置、自己位置推定方法、プログラム及び記憶媒体

　本発明は、自己位置推定技術に関する。

　従来から、車両の進行先に設置される地物をレーダやカメラを用いて検出し、その検出結果に基づいて自車位置を校正する技術が知られている。例えば、特許文献１には、計測センサの出力と、予め地図上に登録された地物の位置情報とを照合させることで自己位置を推定する技術が開示されている。また、特許文献２には、カルマンフィルタを用いた自車位置推定技術が開示されている。

特開２０１３－２５７７４２号公報特開２０１７－７２４２２号公報

　特許文献２に示されるベイズ手法に基づく自己位置推定では、白線、縁石、ガードレールなどの連続的に設けられた構造物を計測する場合、自車から見て横方向の距離は計測可能であるが、進行方向への連続性に起因して進行方向における距離を的確に特定できないという問題がある。また、破線形状の区画線の場合には、検出される区画線の前後方向の長さが変化するため、同様に進行方向における位置を的確に特定できない。

　本発明は、上記のような課題を解決するためになされたものであり、白線などの区画線を計測対象とし、横方向だけでなく進行方向の位置推定を高精度に行うことが可能な自己位置推定装置を提供することを主な目的とする。

　請求項１に記載の発明は、移動体に搭載される自己位置推定装置であって、前記移動体の予測位置を取得する取得手段と、地図情報から取得した区画線の端点の情報に基づいて得られた前記区画線の端点の予測位置と、前記移動体に搭載された計測部が光を所定方向に走査することで計測された前記区画線の端点の計測位置との差分値を算出する差分値算出手段と、前記差分値に係数を乗じた値で前記移動体の予測位置を修正して前記移動体の自己位置を推定する推定手段と、を備え、前記推定手段は、前記区画線の端点を検出した位置における前記計測部の走査位置の間隔に基づいて、前記係数を補正する。

　請求項６に記載の発明は、移動体に搭載される自己位置推定装置により実行される自己位置推定方法であって、前記移動体の予測位置を取得する取得工程と、地図情報から取得した区画線の端点の情報に基づいて得られた前記区画線の端点の予測位置と、前記移動体に搭載された計測部が光を所定方向に走査することで計測された前記区画線の端点の計測位置との差分値を算出する差分算出工程と、前記差分値に係数を乗じた値で前記移動体の予測位置を修正して前記移動体の自己位置を推定する推定工程と、を備え、前記推定工程は、前記区画線の端点を検出した位置における前記計測部の走査位置の間隔に基づいて、前記係数を補正する。

　請求項７に記載の発明は、移動体に搭載され、コンピュータを備える自己位置推定装置により実行されるプログラムであって、前記移動体の予測位置を取得する取得手段、地図情報から取得した区画線の端点の情報に基づいて得られた前記区画線の端点の予測位置と、前記移動体に搭載された計測部が光を所定方向に走査することで計測された前記区画線の端点の計測位置との差分値を算出する差分値算出手段、前記差分値に係数を乗じた値で前記移動体の予測位置を修正して前記移動体の自己位置を推定する推定手段、として前記コンピュータを機能させ、前記推定手段は、前記区画線の端点を検出した位置における前記計測部の走査位置の間隔に基づいて、前記係数を補正する。

運転支援システムの概略構成図である。車載機の機能的構成を示すブロック図である。状態変数ベクトルを２次元直交座標で表した図である。予測ステップと計測更新ステップとの概略的な関係を示す図である。自車位置推定部の機能ブロックを示す。ライダ計測値をデカルト座標系へ変換する方法を示す。車両に搭載されたライダにより白線を検出する方法を示す。ウィンドウ内における複数のスキャンラインの位置を示す。白線の始まり部分の端点の位置を検出する方法を示す。白線の終わり部分の端点の位置を検出する方法を示す。スキャンラインの途中で反射強度が変化した場合の端点の位置の決定方法を示す。白線の端点のランドマーク予測値を算出する方法を示す。カルマンゲインの補正係数を示す。計測雑音の補正係数を示す。自車位置推定処理のフローチャートである。

　本発明の１つの好適な実施形態は、移動体に搭載される自己位置推定装置であって、前記移動体の予測位置を取得する取得手段と、地図情報から取得した区画線の端点の情報に基づいて得られた前記区画線の端点の予測位置と、前記移動体に搭載された計測部が光を所定方向に走査することで計測された前記区画線の端点の計測位置との差分値を算出する差分値算出手段と、前記差分値に係数を乗じた値で前記移動体の予測位置を修正して前記移動体の自己位置を推定する推定手段と、を備え、前記推定手段は、前記区画線の端点を検出した位置における前記計測部の走査位置の間隔に基づいて、前記係数を補正する。

　上記の自己位置推定装置は、移動体に搭載され、移動体の予測位置を取得する。また、自己位置推定装置は、地図情報から取得した区画線の端点の情報に基づいて得られた区画線の端点の予測位置と、移動体に搭載された計測部が光を所定方向に走査することで計測された区画線の端点の計測位置との差分値を算出し、差分値に係数を乗じた値で移動体の予測位置を修正して移動体の自己位置を推定する。そして、自己位置推定装置は、区画線の端点を検出した位置における計測部の走査位置の間隔に基づいて係数を補正する。この自己位置推定装置によれば、区画線の端点の位置を利用して、移動体の進行方向における自己位置の推定精度を高めることができる。好適な例では、係数はカルマンゲインである。

　上記の自己位置推定装置の一態様では、前記推定手段は、前記走査位置の間隔の逆数により、前記係数を補正する。他の一態様では、前記推定手段は、前記走査位置の間隔と、当該走査位置の間隔のうち前記計測部により計測された前記走査位置の最短の間隔との比に基づき、前記係数を補正する。この態様では、走査位置の間隔が小さいほど、差分値が自己位置の推定に大きく反映されるように係数が補正される。

　上記の自己位置推定装置の他の一態様では、前記計測部は、前記移動体の位置を基準とした所定位置に規定されるウィンドウ内に存在する区画線を検出し、前記推定手段は、前記区画線上に存在する走査線数と、前記ウィンドウのサイズに基づいて予め決められた走査線数との比により、前記係数を補正する。この態様では、検出された区画線上に存在する走査線数が多いほど、差分値が自己位置の推定に大きく反映されるように係数が補正される。

　上記の自己位置推定装置の他の一態様では、前記計測部は、相互に隣接する、区画線上に存在する走査線と区画線上に存在しない走査線との中間点を前記区画線の端点として検出する。これにより、区画線上に存在する走査線と区画線上に存在しない走査線の位置から、区画線の端点を決定することができる。

　本発明の他の好適な実施形態は、移動体に搭載される自己位置推定装置により実行される自己位置推定方法であって、前記移動体の予測位置を取得する取得工程と、地図情報から取得した区画線の端点の情報に基づいて得られた前記区画線の端点の予測位置と、前記移動体に搭載された計測部が光を所定方向に走査することで計測された前記区画線の端点の計測位置との差分値を算出する差分算出工程と、前記差分値に係数を乗じた値で前記移動体の予測位置を修正して前記移動体の自己位置を推定する推定工程と、を備え、前記推定工程は、前記区画線の端点を検出した位置における前記計測部の走査位置の間隔に基づいて、前記係数を補正する。この自己位置推定方法によれば、区画線の端点の位置を利用して、移動体の進行方向における自己位置の推定精度を高めることができる。

　本発明の他の好適な実施形態は、移動体に搭載され、コンピュータを備える自己位置推定装置により実行されるプログラムであって、前記移動体の予測位置を取得する取得手段、地図情報から取得した区画線の端点の情報に基づいて得られた前記区画線の端点の予測位置と、前記移動体に搭載された計測部が光を所定方向に走査することで計測された前記区画線の端点の計測位置との差分値を算出する差分値算出手段、前記差分値に係数を乗じた値で前記移動体の予測位置を修正して前記移動体の自己位置を推定する推定手段、として前記コンピュータを機能させ、前記推定手段は、前記区画線の端点を検出した位置における前記計測部の走査位置の間隔に基づいて、前記係数を補正する。このプログラムをコンピュータで実行することにより、上記の自己位置推定装置を実現することができる。このプログラムは、記憶媒体に記憶して取り扱うことができる。

　以下、図面を参照して本発明の好適な実施例について説明する。なお、任意の記号の上に「＾」または「－」が付された文字を、本明細書では便宜上、「Ａ^＾」または「Ａ^－」（「Ａ」は任意の文字）と表す。
　［運転支援システム］

　図１は、本実施例に係る運転支援システムの概略構成図である。図１に示す運転支援システムは、車両に搭載され、車両の運転支援に関する制御を行う車載機１と、ライダ（Ｌｉｄａｒ：Ｌｉｇｈｔ　Ｄｅｔｅｃｔｉｏｎ　ａｎｄ　Ｒａｎｇｉｎｇ、または、Ｌａｓｅｒ　Ｉｌｌｕｍｉｎａｔｅｄ　Ｄｅｔｅｃｔｉｏｎ　Ａｎｄ　Ｒａｎｇｉｎｇ）２と、ジャイロセンサ３と、車速センサ４と、ＧＰＳ受信機５とを有する。

　車載機１は、ライダ２、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５と電気的に接続し、これらの出力に基づき、車載機１が搭載される車両の位置（「自車位置」とも呼ぶ。）の推定を行う。そして、車載機１は、自車位置の推定結果に基づき、設定された目的地への経路に沿って走行するように、車両の自動運転制御などを行う。車載機１は、道路データ及び道路付近に設けられた目印となる地物や区画線に関する情報が登録された地図データベース（ＤＢ：ＤａｔａＢａｓｅ）１０を記憶する。上述の目印となる地物は、例えば、道路脇に周期的に並んでいるキロポスト、１００ｍポスト、デリニエータ、交通インフラ設備（例えば標識、方面看板、信号）、電柱、街灯などの地物である。そして、車載機１は、この地図ＤＢ１０に基づき、ライダ２等の出力と照合させて自車位置の推定を行う。車載機１は、本発明における「自己位置推定装置」の一例である。

　ライダ２は、水平方向および垂直方向の所定の角度範囲に対してパルスレーザを出射することで、外界に存在する物体までの距離を離散的に測定し、当該物体の位置を示す３次元の点群情報を生成する。この場合、ライダ２は、照射方向を変えながらレーザ光を照射する照射部と、照射したレーザ光の反射光（散乱光）を受光する受光部と、受光部が出力する受光信号に基づくスキャンデータを出力する出力部とを有する。本実施例では、ライダ２が出射するレーザの照射範囲には、少なくとも道路の路面が含まれている。スキャンデータは、受光部が受光したレーザ光に対応する照射方向と、上述の受光信号に基づき特定される当該レーザ光のその照射方向における物体までの距離とに基づき生成される。一般的に、対象物までの距離が近いほどライダの距離測定値の精度は高く、距離が遠いほど精度は低い。ライダ２、ジャイロセンサ３、車速センサ４、ＧＰＳ受信機５は、それぞれ、出力データを車載機１へ供給する。ライダ２は、本発明における「計測部」の一例である。

　図２は、車載機１の機能的構成を示すブロック図である。車載機１は、主に、インターフェース１１と、記憶部１２と、入力部１４と、制御部１５と、情報出力部１６と、を有する。これらの各要素は、バスラインを介して相互に接続されている。

　インターフェース１１は、ライダ２、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５などのセンサから出力データを取得し、制御部１５へ供給する。

　記憶部１２は、制御部１５が実行するプログラムや、制御部１５が所定の処理を実行するのに必要な情報を記憶する。本実施例では、記憶部１２は、区画線情報及び地物情報を含む地図ＤＢ１０を記憶する。ここで、区画線情報は、各道路に設けられた区画線（白線）に関する情報であり、区画線ごとに、区画線の離散的な位置を示す座標情報が含まれている。なお、区画線情報は、道路ごとの道路データに組み込まれた情報であってもよい。さらに本実施例では、破線の区画線については、その端点の位置を示す座標情報が含まれている。地物情報は、区画線以外の地物に関する情報であり、ここでは、地物ごとに、地物のインデックスに相当する地物ＩＤと、緯度及び経度（及び標高）等により表わされた地物の絶対的な位置を示す位置情報とが少なくとも関連付けられている。なお、地図ＤＢ１０は、定期的に更新されてもよい。この場合、例えば、制御部１５は、図示しない通信部を介し、地図情報を管理するサーバ装置から、自車位置が属するエリアに関する部分地図情報を受信し、地図ＤＢ１０に反映させる。

　入力部１４は、ユーザが操作するためのボタン、タッチパネル、リモートコントローラ、音声入力装置等である。情報出力部１６は、例えば、制御部１５の制御に基づき出力を行うディスプレイやスピーカ等である。

　制御部１５は、プログラムを実行するＣＰＵなどを含み、車載機１の全体を制御する。本実施例では、制御部１５は、自車位置推定部１７を有する。制御部１５は、本発明における取得手段、差分値算出手段、推定手段の一例である。

　自車位置推定部１７は、ランドマークに対するライダ２による距離及び角度の計測値と、地図ＤＢ１０から抽出したランドマークの位置情報とに基づき、ジャイロセンサ３、車速センサ４、及び／又はＧＰＳ受信機５の出力データから推定した自車位置を補正する。本実施例では、一例として、自車位置推定部１７は、ベイズ推定に基づく状態推定手法に基づき、ジャイロセンサ３、車速センサ４等の出力データから自車位置を予測する予測ステップと、直前の予測ステップで算出した自車位置の予測値を補正する計測更新ステップとを交互に実行する。これらのステップで用いる状態推定フィルタは、ベイズ推定を行うように開発された様々のフィルタが利用可能であり、例えば、拡張カルマンフィルタ、アンセンテッドカルマンフィルタ、パーティクルフィルタなどが該当する。このように、ベイズ推定に基づく位置推定は、種々の方法が提案されている。

　以下では、拡張カルマンフィルタを用いた自車位置推定について簡略的に説明する。後述するように、本実施例では、自車位置推定部１７は、地物又は区画線のいずれを対象として自車位置推定を行う場合にも、拡張カルマンフィルタを用いて統一的に自車位置推定処理を行う。

　図３は、状態変数ベクトルｘを２次元直交座標で表した図である。図３に示すように、ｘｙの２次元直交座標上で定義された平面での自車位置は、座標「（ｘ、ｙ）」、自車の方位「Ψ」により表される。ここでは、方位Ψは、車の進行方向とｘ軸とのなす角として定義されている。座標（ｘ、ｙ）は、例えば緯度及び経度の組合せに相当する、ある基準位置を原点とした座標系における絶対位置を示す。

　図４は、予測ステップと計測更新ステップとの概略的な関係を示す図である。また、図５は、自車位置推定部１７の機能ブロックの一例を示す。図４に示すように、予測ステップと計測更新ステップとを繰り返すことで、自車位置を示す状態変数ベクトル「Ｘ」の推定値の算出及び更新を逐次的に実行する。また、図５に示すように、自車位置推定部１７は、予測ステップを実行する位置予測部２１と、計測更新ステップを実行する位置推定部２２とを有する。位置予測部２１は、デッドレコニングブロック２３及び位置予測ブロック２４を含み、位置推定部２２は、ランドマーク探索・抽出部２５及び位置補正ブロック２６を含む。なお、図４では、計算対象となる基準時刻（即ち現在時刻）「ｔ」の状態変数ベクトルを、「Ｘ^－（ｔ）」または「Ｘ^＾（ｔ）」と表記している（「状態変数ベクトルＸ（ｔ）＝（ｘ（ｔ）、ｙ（ｔ）、Ψ（ｔ））^Ｔ」と表記する）。ここで、予測ステップで推定された暫定的な推定値（予測値）には当該予測値を表す文字の上に「^－」を付し、計測更新ステップで更新された，より精度の高い推定値には当該値を表す文字の上に「^＾」を付す。

　予測ステップでは、自車位置推定部１７のデッドレコニングブロック２３は、車両の移動速度「ｖ」と角速度「ω」（これらをまとめて「制御値ｕ（ｔ）＝（ｖ（ｔ）、ω（ｔ））^Ｔ」と表記する。）を用い、前回時刻からの移動距離と方位変化を求める。自車位置推定部１７の位置予測ブロック２４は、直前の計測更新ステップで算出された時刻ｔ－１の状態変数ベクトルＸ^＾（ｔ－１）に対し、求めた移動距離と方位変化を加えて、時刻ｔの自車位置の予測値（「予測自車位置」とも呼ぶ。）Ｘ^－（ｔ）を算出する。また、これと同時に、予測自車位置Ｘ^－（ｔ）の誤差分布に相当する共分散行列「Ｐ^－（ｔ）」を、直前の計測更新ステップで算出された時刻ｔ－１での共分散行列「Ｐ^＾（ｔ－１）」から算出する。

　計測更新ステップでは、自車位置推定部１７のランドマーク探索・抽出部２５は、地図ＤＢ１０に登録されたランドマークの位置ベクトルとライダ２のスキャンデータとの対応付けを行う。そして、自車位置推定部１７のランドマーク探索・抽出部２５は、この対応付けができた場合に、対応付けができたランドマークのライダ２による計測値（「ランドマーク計測値」と呼ぶ。）「Ｚ（ｔ）」と、予測自車位置Ｘ^－（ｔ）及び地図ＤＢ１０に登録されたランドマークの位置ベクトルを用いてライダ２による計測処理をモデル化して求めたランドマークの計測推定値（「ランドマーク予測値」と呼ぶ。）「Ｚ^－（ｔ）」とをそれぞれ取得する。ランドマーク計測値Ｚ（ｔ）は、時刻ｔにライダ２が計測したランドマークの距離及びスキャン角度から、車両の進行方向と横方向を軸とした成分に変換した車両のボディ座標系における２次元ベクトルである。そして、自車位置推定部１７の位置補正ブロック２６は、以下の式（１）に示すように、ランドマーク計測値Ｚ（ｔ）とランドマーク予測値Ｚ^－（ｔ）との差分値を算出する。

　そして、自車位置推定部１７の位置補正ブロック２６は、以下の式（２）に示すように、ランドマーク計測値Ｚ（ｔ）とランドマーク予測値Ｚ^－（ｔ）との差分値にカルマンゲイン「Ｋ（ｔ）」を乗算し、これを予測自車位置Ｘ^－（ｔ）に加えることで、更新された状態変数ベクトル（「推定自車位置」とも呼ぶ。）Ｘ^＾（ｔ）を算出する。

　また、計測更新ステップでは、自車位置推定部１７の位置補正ブロック２６は、予測ステップと同様、推定自車位置Ｘ^＾（ｔ）の誤差分布に相当する共分散行列Ｐ^＾（ｔ）（単にＰ（ｔ）とも表記する）を共分散行列Ｐ^－（ｔ）から求める。カルマンゲインＫ（ｔ）等のパラメータについては、例えば拡張カルマンフィルタを用いた公知の自己位置推定技術と同様に算出することが可能である。

　このように、予測ステップと計測更新ステップが繰り返し実施され、予測自車位置Ｘ^－（ｔ）と推定自車位置Ｘ^＾（ｔ）が逐次的に計算されることにより、もっとも確からしい自車位置が計算される。

　［区画線を用いた自車位置推定］
　次に、本実施例の特徴である、区画線を用いた自車位置推定について説明する。本実施例は、ランドマークとして区画線を用いて自車位置推定を行う。なお、以下の説明では、区画線として白線を用いた例を説明するが、黄色の区画線にも同様に適用することもできる。

（１）ライダによる白線の計測
　まず、ライダによる白線の計測方法について説明する。
　（ライダの計測値）
　いま、図６に示すように、車両の自車位置を原点とし、車両の進行方向をｘ軸にとったデカルト座標系（以下、「車両座標系」と呼ぶ。）を考える。ライダによる計測対象までの計測値は、水平角度α、垂直角度β、距離ｒを含む。これらをデカルト座標系に変換すると、ライダによる計測値は以下の式により示される。

　（白線の検出）
　図７は、車両に搭載されたライダ２により白線を検出する方法を示す。白線は再帰性反射材が塗布されているため、反射強度が強い。よって、車載機１は、車両の前方の路面上における白線を検出しやすい位置にウィンドウを設け、その中の反射強度の高い部分を検出して白線と認識する。

　具体的には、図７に示すように、車両の前方の左右に設けられたライダ２から、車両の前方の路面に向けて出射光ＥＬが出射される。車両前方の所定位置には、仮想的なウィンドウＷが規定される。車両の中心Ｏを基準としたウィンドウＷの位置は予め決められている。即ち、車両の中心ＯからウィンドウＷまでの水平距離Ｌ１、車両の中心Ｏから左右のウィンドウＷまでの横方向距離Ｌ２、及び、ウィンドウＷの長さＬ３などは予め決められている。車載機１は、ライダ２から出力されるスキャンデータのうち、ウィンドウＷ内に属するスキャンデータを抽出し、それらの反射強度が所定の閾値より高い場合に、白線を検出したと判定する。

　（ライン間隔）
　ライダ２として多層式のライダを用いることにより、ウィンドウＷ内には複数のスキャンラインｓが形成される。図８は、ウィンドウＷ内における複数のスキャンラインｓの位置を示す。ウィンドウＷ内の複数のスキャンラインｓは、車両から遠いほどその間隔（以下、「ライン間隔」と呼ぶ。）が長くなる。図８の右上部は、ウィンドウＷ内のスキャンラインを上方から見た平面図を示す。ウィンドウＷ内にスキャンラインｓ_１～ｓ_１０が形成されており、各スキャンラインのライン間隔ｄ_１～ｄ_９が示されている。車両から遠いスキャンラインほどライン間隔ｄが長くなる。また、図８の右下部は、ウィンドウＷ内のスキャンラインを側方から見た図を示す。同様に、車両から遠いスキャンラインほどライン間隔ｄが長くなる。

　任意のスキャンラインの番号を「ｉ」とし、ｉ番目のスキャンラインの水平角度をα（ｉ）、垂直角度をβ（ｉ）とし、車両からの距離をｒ（ｉ）とすると、ｉ番目のスキャンラインと（ｉ＋１）番目のスキャンラインとのライン間隔ｄ（ｉ）は以下の式（４）により求められる。同様に、（ｉ＋１）番目のスキャンラインと（ｉ＋２）番目のスキャンラインのライン間隔ｄ（ｉ＋１）は以下の式（５）により求められ、（ｉ＋２）番目のスキャンラインと（ｉ＋３）番目のスキャンラインのライン間隔ｄ（ｉ＋２）は以下の式（６）により求められる。

　こうして、ライダ２のスキャンデータに基づいて、ウィンドウＷ内の複数のスキャンラインｓのライン間隔ｄを求めることができる。

　（白線の端点位置の計測）
　次に、ライダにより白線の端点位置を計測する方法について説明する。図９は、白線の始まり部分の端点（白線の開始点）の位置を計測する方法を示す。ウィンドウＷ内にはｓ_１～ｓ_１０の１０本のスキャンラインが形成されている。前述のように、車載機１は、ライダにより得られるスキャンデータの反射強度に基づいて白線を検出する。いま、例１に示すように、スキャンラインｓ_２とｓ_３の間でスキャンデータの反射強度が変化したものとする。即ち、スキャンラインｓ_２の反射強度が所定の閾値以上になり、スキャンラインｓ_３の反射強度は所定の閾値未満であるとする。この場合、白線の端点は、スキャンラインｓ_２とｓ_３の間に存在することがわかるが、その正確な位置まではわからない。そこで、車載機１は、スキャンラインｓ_２とｓ_３の中点を白線の端点とみなす。

　同様に、例２に示すようにスキャンラインｓ_４とｓ_５の間でスキャンデータの反射強度が変化した場合、車載機１はスキャンラインｓ_４とｓ_５の中点を白線の端点とみなす。また、例３に示すようにスキャンラインｓ_７とｓ_８の間でスキャンデータの反射強度が変化した場合、車載機１はスキャンラインｓ_７とｓ_８の中点を白線の端点とみなす。

　白線の終わり部分の端点位置を検出する方法も基本的に同様である。図１０は、白線の終わり部分の端点（白線の終了点）の位置を計測する方法を示す。例４に示すように、スキャンラインｓ_２とｓ_３の間でスキャンデータの反射強度が変化したものとする。即ち、スキャンラインｓ_２の反射強度が所定の閾値未満になり、スキャンラインｓ_３の反射強度は所定の閾値以上であるとする。この場合、車載機１は、スキャンラインｓ_２とｓ_３の中点を白線の端点とみなす。

　同様に、例５に示すようにスキャンラインｓ_４とｓ_５の間でスキャンデータの反射強度が変化した場合、車載機１はスキャンラインｓ_４とｓ_５の中点を白線の端点とみなす。また、例６に示すようにスキャンラインｓ_７とｓ_８の間でスキャンデータの反射強度が変化した場合、車載機１はスキャンラインｓ_７とｓ_８の中点を白線の端点とみなす。

　いま、ｉ番目のスキャンラインｓ_ｉと、（ｉ＋１）番目のスキャンラインｓ_ｉ＋１との間でスキャンデータの反射強度が変化したものとすると、車両の進行方向における白線の端点の位置を示すライダの計測値Ｌ_ｘ（ｔ）は、車両の進行方向におけるｉ番目のスキャンラインｓ_ｉの位置ｒ_ｘ（ｉ）と（ｉ＋１）番目のスキャンラインｓ_ｉ＋１の位置ｒ_ｘ（ｉ＋１）とを用いて以下の式で算出される。

　ライダ２による白線の計測値は、以下の式で与えられる。

　なお、図９及び図１０の例では、反射強度が等しいスキャンデータの集合として１つのスキャンラインを検出できた場合を示しているが、１つのスキャンラインの途中でスキャンデータの反射強度が変化する場合がある。この場合、そのスキャンラインが白線の端点位置なのか、本当は白線の端点位置ではないが汚れやかすれ等によってそのように検出されてしまったのかを区別することが難しい。

　図１１は、スキャンラインの途中でスキャンデータの反射強度が変化した場合の例を示す。例７や例８では、スキャンラインｓ_３の途中でスキャンデータの反射強度が変化しているため、スキャンラインｓ_３を白線の端点と断定することは難しい。そこで、スキャンラインの途中でスキャンデータの反射強度が変化した場合は、車載機１は、白線として正しく検出されたスキャンラインと、白線として全く検出できなかったスキャンラインとの中点を白線の端点位置とみなす。図１１の例７では、車載機１は、白線として正しく検出されたスキャンラインｓ_２と、白線として全く検出できなかったスキャンラインｓ_４との中点を白線の端点位置とみなす。同様に、例８では、車載機１は、白線として正しく検出されたスキャンラインｓ_４と、白線として全く検出できなかったスキャンラインｓ_２との中点を白線の端点位置とみなす。

　いま、（ｉ－１）番目のスキャンラインｓ_ｉー１と、（ｉ＋１）番目のスキャンラインｓ_ｉ＋１との間に位置するｉ番目のスキャンラインｓ_ｉの途中でスキャンデータの反射強度が変化したものとすると、車両の進行方向における白線の端点の位置を示すライダの計測値Ｌ_ｘ（ｔ）は、車両の進行方向における（ｉ－１）番目のスキャンラインｓ_ｉ－１の位置ｒ_ｘ（ｉ－１）と（ｉ＋１）番目のスキャンラインｓ_ｉ＋１の位置ｒ_ｘ（ｉ＋１）とを用いて以下の式（９）で算出される。また、その時のライン間隔ｄ（ｔ）は、式（１０）で算出される。

　以上より、白線の端点が検出できた場合、車両の進行方向の計測値Ｌ_ｘ（ｔ）は、式（７）又は式（９）で与えられる値となる。白線の端点が検出できなかった場合、車両の進行方向の計測値Ｌ_ｘ（ｔ）は無しとなる。一方、車両の横方向の計測値Ｌ_ｙ（ｔ）は、ウィンドウＷ内で検出された複数のスキャンラインの中心点の平均値となる。こうして、白線の検出結果に応じて、ライダによる白線の計測値が求められる。

（２）自車位置推定方法
　次に、実施例による自車位置推定方法について説明する。
　（ランドマーク予測値の算出）
　まず、地図データを用いて、白線の端点のランドマーク予測値を算出する方法を説明する。図１２は、白線の端点のランドマーク予測値を算出する方法を示す。図１２において、ワールド座標系に車両Ｖが存在し、その車両Ｖの中心を原点として車両座標系が規定される。地図ＤＢ１０に記憶されている白線端点ｋの座標を（ｍ_ｘ（ｋ），ｍ_ｙ（ｋ））とする。地図ＤＢ１０に記憶されている白線端点ｋの座標はワールド座標系における座標であり、車載機１はこれを車両座標系に座標変換する。具体的には、車両Ｖの推定自車位置をｘ^－（ｔ）、ｙ^－（ｔ）、推定自車方位をΨ^－（ｔ）とすると、ワールド座標系から車両座標系に座標変換する回転行列を用いて、白線端点ｋのランドマーク予測値は以下の式で表される。

　（カルマンゲインの修正）
　次に、白線の端点の検出状態に応じてカルマンゲインを修正する方法について説明する。図１３（Ａ）は、車載機１が白線の端点を検出した場合のカルマンゲインの補正係数を示す。自車位置推定に用いられるランドマーク計測値としては、車両の進行方向の計測値と、車両の横方向の計測値とがある。そこで、車両の進行方向の補正係数を進行方向補正係数ａ（ｔ）とし、車両の横方向の補正係数を横方向係数ｂ（ｔ）とする。

　ウィンドウＷ内に白線の端点を検出した場合、車載機１は、白線の端点の位置を進行方向の計測値として用いる。但し、前述したように、白線の端点位置の計測値は、反射強度が変化した前後の２つの隣接するスキャンラインの中点として求められるので、スキャンラインのライン間隔により、車両の進行方向の計測精度は異なる。即ち、ライン間隔が広いと誤差が大きく、ライン間隔が狭いと誤差は少ない。そこで、スキャンラインのライン間隔の長さを用いて、進行方向のカルマンゲインを修正する。具体的には、ライン間隔ｄ（ｔ）の逆数を進行方向係数ａ（ｔ）とする。即ち、進行方向係数ａ（ｔ）は、
　　ａ（ｔ）＝１／ｄ（ｔ）
と設定される。白線の端点が検出された場合、ライン間隔が狭い場合ほど、計測された白線の端点位置の精度は高いので、カルマンゲインが大きくなるような補正がなされる。

　車両の横方向の計測値は、ウィンドウＷ内で検出された複数のスキャンラインの中心点の平均値を用いる。ウィンドウＷ内に白線の端点が検出された場合、図１３（Ａ）に示すように、ウィンドウＷ内に存在するスキャンライン数をＮ_Ｍとし、ライダにより白線上で計測されたスキャンライン数をＮ_Ｌとすると、横方向係数ｂ（ｔ）は、
　　ｂ（ｔ）＝Ｎ_Ｌ（ｔ）／Ｎ_Ｍ
と設定される。白線の端点が検出された場合、白線上で検出されたスキャンライン数が多いほど、車両の横方向の計測精度は高いので、カルマンゲインが大きくなるような補正がなされる。

　図１３（Ｂ）は、車載機１が白線を検出したが、白線の端点を検出しない場合におけるカルマンゲインの補正係数を示す。白線を検出しない場合、車両の進行方向における計測値が得られないので、進行方向係数ａ（ｔ）は、
　　ａ（ｔ）＝０
と設定される。また、白線の端点を検出しない場合、ウィンドウＷ内の全てのスキャンラインの中心点が計測でき、車両の横方向の計測精度は高いため、横方向係数ｂ（ｔ）は、
　　ｂ（ｔ）＝１
と設定される。

　自車位置推定部１７は、こうして得られた進行方向係数ａ（ｔ）及び横方向係数ｂ（ｔ）を用いて、カルマンゲインを補正する。具体的には、自車位置推定部１７は、以下の式（１２）に示すカルマンゲインＫ（ｔ）に、進行方向係数ａ（ｔ）及び横方向係数ｂ（ｔ）を乗じて式（１３）に示す適応カルマンゲインＫ（ｔ）’を生成する。

　そして、自車位置推定部１７は、得られた適応カルマンゲインＫ（ｔ）’を式（２）におけるカルマンゲインＫ（ｔ）に適用し、以下の式（１４）により推定自車位置Ｘ＾（ｔ）を算出する。

　（計測雑音の修正）
　次に、白線の端点の検出状態に応じて計測雑音を修正する方法について説明する。図１４（Ａ）は、車載機１が白線の端点を検出した場合の計測雑音の補正係数を示す。自車位置推定に用いられるランドマーク計測値としては、車両の進行方向の計測値と、車両の横方向の計測値とがある。そこで、車両の進行方向の補正係数を進行方向補正係数ａ（ｔ）とし、車両の横方向の補正係数を横方向係数ｂ（ｔ）とする。

　ウィンドウＷ内に白線の端点を検出した場合、車載機１は、白線の端点の位置を進行方向の計測値として用いる。但し、前述したように、白線の端点位置の計測値は、反射強度が変化した前後の２つの隣接するスキャンラインの中点として求められるので、スキャンラインのライン間隔により、車両の進行方向の計測精度は異なる。即ち、ライン間隔が広いと誤差が大きく、ライン間隔が狭いと誤差は少ない。そこで、スキャンラインのライン間隔の長さを用いて、進行方向の計測雑音を修正する。具体的には、ライン間隔ｄ（ｔ）と、検出された複数のライン間隔のうち最短のライン間隔（図９におけるライン間隔ｄ_９）との比を進行方向係数ａ（ｔ）とする。即ち、進行方向係数ａ（ｔ）は、
　　ａ（ｔ）＝ｄ（ｔ）／ｄ_９
と設定される。白線の端点が検出された場合、ライン間隔が狭い場合ほど、計測された白線の端点位置の精度は高いので、計測雑音が小さくなるような補正がなされる。

　車両の横方向の計測値は、ウィンドウＷ内で検出された複数のスキャンラインの中心点の平均値を用いる。ウィンドウＷ内に白線の端点が検出された場合、図１４（Ａ）に示すように、ウィンドウＷ内に存在するスキャンライン数をＮ_Ｍとし、ライダにより白線上で計測されたスキャンライン数をＮ_Ｌとすると、横方向係数ｂ（ｔ）は、
　　ｂ（ｔ）＝Ｎ_Ｍ／Ｎ_Ｌ（ｔ）
と設定される。白線の端点が検出された場合、白線上で検出されたスキャンライン数が多いほど、車両の横方向の計測精度は高いので、計測雑音が小さくなるような補正がなされる。

　図１４（Ｂ）は、車載機１が白線を検出したが、白線の端点を検出しない場合における計測雑音の補正係数を示す。白線を検出しない場合、車両の進行方向における計測値が得られないので、進行方向係数ａ（ｔ）は、大きな値が設定される。例えば、
　　ａ（ｔ）＝１００００
と設定される。また、白線の端点を検出しない場合、ウィンドウＷ内の全てのスキャンラインの中心点が計測でき、車両の横方向の計測精度は高いため、横方向係数ｂ（ｔ）は、
　　ｂ（ｔ）＝１
と設定される。

　自車位置推定部１７は、こうして得られた進行方向係数ａ（ｔ）及び横方向係数ｂ（ｔ）を計測雑音に乗じて、適応的計測雑音を生成する。そして、自車位置推定部１７は、この適応的計測雑音を用いて、推定自車位置を求める。

　具体的に、基本的な計測雑音Ｒ（ｔ）は以下の式で与えられる。なお、対象とする白線計測に対して、σ_Ｌｘ ^２は車両座標系の進行方向における計測雑音であり、σ_Ｌy ^２は車両座標系の横方向における計測雑音である。

　よって、自車位置推定部１７は、進行方向係数ａ（ｔ）及び横方向係数ｂ（ｔ）を計測雑音に乗じて、以下の適応的計測雑音Ｒ（ｔ）’を得る。

　これを、カルマンゲインの式（１２）に代入して、以下の適応カルマンゲインＫ（ｔ）’を得る。

　そして、自車位置推定部１７は、得られた適応カルマンゲインＫ（ｔ）’を式（２）におけるカルマンゲインＫ（ｔ）に適用し、式（１４）により推定自車位置Ｘ＾（ｔ）を算出する。

　これにより、破線の端点情報を用いて進行方向の位置推定を行うことができ、かつ、白線の端点の検出精度及び検出ライン数に応じた適応的計測雑音による処理によって、白線の検出状態に応じた好適な処理を行うことができる。

（３）自車位置推定処理
　図１５は、自車位置推定部１７が実行する自車位置推定処理のフローチャートである。自車位置推定部１７は、図１５のフローチャートの処理を繰り返し実行する。

　まず、自車位置推定部１７は、車体速度と車両のヨー方向の角速度を検出したか否か判定する（ステップＳ１１）。例えば、自車位置推定部１７は、車速センサ４の出力に基づき車体速度を検出し、ジャイロセンサ３の出力に基づきヨー方向の角速度を検出する。自車位置推定部１７は、車体速度と車両のヨー方向の角速度を検出した場合（ステップＳ１１：Ｙｅｓ）、検出した車体速度と角速度を用いて、１時刻前の推定自車位置Ｘ^＾（ｔ－１）から予測自車位置Ｘ^－（ｔ）を算出する。さらに、自車位置推定部１７は、１時刻前の共分散行列から、現時刻での共分散行列を算出する（ステップＳ１２）。なお、自車位置推定部１７は、車体速度と車両のヨー方向の角速度を検出しなかった場合（ステップＳ１１：Ｎｏ）、１時刻前の推定自車位置Ｘ＾（ｔ－１）を予測自車位置Ｘ－（ｔ）とし、１時刻前の共分散行列を現時刻での共分散行列とする。

　次に、自車位置推定部１７は、自車位置推定に用いるランドマークとして、白線を検出したか否か判定する（ステップＳ１３）。具体的には、自車位置推定部１７は、図７に示すようにウィンドウＷを設定し、ライダ２のスキャンデータの反射強度に基づいて、ウィンドウＷ内に白線を検出したか否かを判定する。白線を検出しない場合（ステップＳ１３：Ｎｏ）、白線を利用した自車位置推定はできないので、処理は終了する。

　一方、白線を検出した場合（ステップＳ１３：Ｙｅｓ）、自車位置推定部１７は、地図ＤＢ１０内の地図データに基づいて白線のランドマーク予測値を算出するとともに、ライダ２による白線のランドマーク計測値を算出する（ステップＳ１４）。具体的には、自車位置推定部１７は、地図データに含まれる白線の端点の座標を車両座標系に座標変換して白線端点のランドマーク予測値を算出するとともに、ライダによる白線の計測値をランドマーク計測値とする。

　また、自車位置推定部１７は、図１３及び図１４で説明したように、白線の端点を検出できたか否かに応じて、進行方向係数ａ（ｔ）及び横方向係数ｂ（ｔ）を算出する（ステップＳ１５）。

　そして、自車位置推定部１７は、共分散行列を用いて上述の式（１０）に基づきカルマンゲインＫ（ｔ）を計算し、進行方向係数ａ（ｔ）及び横方向係数ｂ（ｔ）を用いて式（１３）又は式（１７）に示す適応カルマンゲインＫ（ｔ）’を生成し、式（１４）を用いて推定自車位置を算出する。さらに、自車位置推定部１７は、適応カルマンゲインＫ（ｔ）’を用いて、式（１８）に示すように共分散行列を更新する（ステップＳ１６）。

　以上説明したように、自車位置推定部１７は、白線の端点を検出できた場合、白線の端点をランドマークとしてカルマンフィルタに基づく自車位置推定処理を行う。また、白線の端点を検出した際のライダ２によるスキャンラインのライン間隔に基づいて、進行方向係数を算出してカルマンゲインを修正する。これにより、車両の進行方向においても高精度の自車位置推定が可能となる。

　［変形例］
　上記の実施例では、車載機１は、車両の前方の左右位置にウィンドウＷを設けて白線を検出しているが、車両の後部にも一対のライダ２が設けられている場合には、車両の後方の左右位置にもウィンドウＷを設けて白線を検出しても良い。

　１　車載機
　２　ライダ
　３　ジャイロセンサ
　４　車速センサ
　５　ＧＰＳ受信機
　１０　地図ＤＢ

Claims

　移動体に搭載される自己位置推定装置であって、
　前記移動体の予測位置を取得する取得手段と、
　地図情報から取得した区画線の端点の情報に基づいて得られた前記区画線の端点の予測位置と、前記移動体に搭載された計測部が光を所定方向に走査することで計測された前記区画線の端点の計測位置との差分値を算出する差分値算出手段と、
　前記差分値に係数を乗じた値で前記移動体の予測位置を修正して前記移動体の自己位置を推定する推定手段と、
　を備え、
　前記推定手段は、前記区画線の端点を検出した位置における前記計測部の走査位置の間隔に基づいて、前記係数を補正する自己位置推定装置。
　前記推定手段は、前記走査位置の間隔の逆数により、前記係数を補正する請求項１に記載の自己位置推定装置。
　前記推定手段は、前記走査位置の間隔と、当該走査位置の間隔のうち前記計測部により計測された前記走査位置の最短の間隔と、の比に基づき、前記係数を補正する請求項１に記載の自己位置推定装置。
　前記計測部は、前記移動体の位置を基準とした所定位置に規定されるウィンドウ内に存在する区画線を検出し、
　前記推定手段は、前記区画線上に存在する走査線数と、前記ウィンドウのサイズに基づいて予め決められた走査線数との比により、前記係数を補正する請求項１乃至３のいずれか一項に記載の自己位置推定装置。
　前記計測部は、相互に隣接する、区画線上に存在する走査線と区画線上に存在しない走査線との中間点を前記区画線の端点として検出する請求項１乃至４のいずれか一項に記載の自己位置推定装置。
　前記係数は、カルマンゲインである請求項１乃至５のいずれか一項に記載の自己位置推定装置。
　移動体に搭載される自己位置推定装置により実行される自己位置推定方法であって、
　前記移動体の予測位置を取得する取得工程と、
　地図情報から取得した区画線の端点の情報に基づいて得られた前記区画線の端点の予測位置と、前記移動体に搭載された計測部が光を所定方向に走査することで計測された前記区画線の端点の計測位置との差分値を算出する差分算出工程と、
　前記差分値に係数を乗じた値で前記移動体の予測位置を修正して前記移動体の自己位置を推定する推定工程と、
　を備え、
　前記推定工程は、前記区画線の端点を検出した位置における前記計測部の走査位置の間隔に基づいて、前記係数を補正する自己位置推定方法。
　移動体に搭載され、コンピュータを備える自己位置推定装置により実行されるプログラムであって、
　前記移動体の予測位置を取得する取得手段、
　地図情報から取得した区画線の端点の情報に基づいて得られた前記区画線の端点の予測位置と、前記移動体に搭載された計測部が光を所定方向に走査することで計測された前記区画線の端点の計測位置との差分値を算出する差分値算出手段、
　前記差分値に係数を乗じた値で前記移動体の予測位置を修正して前記移動体の自己位置を推定する推定手段、
　として前記コンピュータを機能させ、
　前記推定手段は、前記区画線の端点を検出した位置における前記計測部の走査位置の間隔に基づいて、前記係数を補正するプログラム。
　請求項８に記載のプログラムを記憶した記憶媒体。