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

Description

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

自立移動可能なロボットの自己位置を推定する技術が知られている（特許文献１参照）。特許文献１では、移動ロボットが移動可能な領域をセンサで検出した結果（周囲環境情報）を、移動ロボットを基準として予め定めた領域内に限定し、この限定した周囲環境情報と予め移動ロボットが保持している環境地図とを照合することにより自己位置を推定している。

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

ところで、車両の自己位置を推定するために、車両の車幅方向の両側に位置する白線を用いる場合がある。一般的に、双方の白線が同時に検出される場合、白線の検出位置には誤差が含まれている。特に、車両に対する白線の車幅方向の位置は、キャリブレーション誤差などで定常的にオフセットされている。このため、自己位置の推定結果が不安定になったり、或いは、自己位置の推定精度が低下したりする。

本発明は、上記課題に鑑みて成されたものであり、その目的は、相対位置の誤差が多いと推定される物標位置データを排除することにより自己位置の推定精度を向上させる自己位置推定方法及び自己位置推定装置を提供することである。

本発明の一態様に係わる自己位置推定方法は、移動体の周囲に存在する物標と移動体との相対位置を検出し、検出した相対位置を移動体の移動量だけ移動させた位置を物標位置データとして蓄積し、物標位置データの移動体との相対位置の信頼性に基づいて物標位置データを選択し、選択した物標位置データと、道路上又は道路周辺に存在する物標の位置情報を含む地図情報とを照合することにより、移動体の現在位置である自己位置を推定し、相対位置から得られる移動体から物標までの移動体の車幅方向の距離と、移動体が走行する走路から想定される移動体から物標までの移動体の車幅方向の想定距離との差が小さいほど、前記した信頼性が高いと判断する。
本発明の他の一態様に係わる自己位置推定方法は、移動体の周囲に存在する物標と移動体との相対位置を検出し、検出した相対位置を移動体の移動量だけ移動させた位置を物標位置データとして蓄積し、物標位置データの移動体との相対位置の信頼性に基づいて物標位置データを選択し、選択した物標位置データと、道路上又は道路周辺に存在する物標の位置情報を含む地図情報とを照合することにより、移動体の現在位置である自己位置を推定し、移動体が走行する走路を特定する走路境界を示す複数の物標位置データを近似した線と複数の物標位置データとの誤差が小さいほど、走路境界を示す物標位置データの移動体との相対位置の信頼性が高いと判断する。

本発明の一態様に係わる自己位置推定方法によれば、相対位置の誤差が多いと推定される物標位置データを排除できるので自己位置の推定精度を向上させることができる。

図１は、実施形態に係わる自己位置推定装置の構成の一例を示すブロック図である。 図２は、周囲センサ群１が車両Ｖに搭載された状態を示す斜視図である。 図３は、図１の自己位置推定装置を用いた自己位置推定方法の一例を示すフローチャートである。 図４は、自己位置推定を行う際に車両Ｖが走行する環境を示す斜視図である。 図５（ａ）〜図５（ｄ）は、図４に示す例において、時刻ｔ１〜ｔ４の間に物標位置検出部３１により検出された、車両座標系における縁石６１の位置７１及び白線６２、６３の物標位置データ７２、７３を示す図である。 図６は、図５（ａ）〜図５（ｄ）に示す例において、車両センサ群５による検出結果に基づいて算出された車両Ｖの移動量を積分した結果を示す図である。 図７は、図５及び図６に示す例において、オドメトリ座標系に変換された物標位置データを示す図である。 図８は、物標位置データ（７１ａ〜７１ｄ、７２ａ〜７２ｄ、７３ａ〜７３ｄ）から抽出される直線情報（Ｎ１、Ｎ２、Ｎ３）を示す概念図である 図９は、走路境界を示す物標位置データ（７２、７３）に対して近似した直線（Ｎ２、Ｎ３）を示す図である。 図１０は、車両Ｖが走行している走路を特定する走路境界を示す、直線近似が可能な走路境界（７２ｊ、７２ｋ、７３ｊ、７３ｋ）が検出されている様子を示す図である。 図１１は、車両Ｖが走行している走路を特定する走路境界を示す、曲線近似が可能な走路境界（７２ｍ、７２ｎ、７３ｍ、７３ｎ）が検出されている様子を示す図である。

図面を参照して、実施形態を説明する。図面の記載において同一部分には同一符号を付して説明を省略する。

図１を参照して、本実施形態に係る自己位置推定装置の構成を説明する。本実施形態に係る自己位置推定装置は、周囲センサ群１と、処理装置３と、記憶装置４と、車両センサ群５とを備える。本実施形態に係る自己位置推定装置は、車両Ｖ（図２参照）に搭載され、車両Ｖの自己位置を推定する。

本実施形態では、推定する車両Ｖの自己位置として、東西方向（Ｘ軸方向）の位置（Ｘ座標［ｍ］）、南北方向（Ｙ軸方向）の位置（Ｙ座標［ｍ］）、姿勢角情報として車両の方位角θ（ヨー角［ｒａｄ］）の二次元平面上における合計３自由度の位置及び姿勢角を推定する。

周囲センサ群１は、例えば、複数のレーザレンジファインダ（ＬＲＦ）１０１、１０２と、複数のカメラ２０１、２０２とを備える。レーザレンジファインダ（ＬＲＦ）１０１、１０２は、それぞれ照射したレーザ光の物標による反射光を受光することにより物標までの距離及び方位を検出する。カメラ２０１、２０２は、車両Ｖの周囲を撮像し、画像処理可能なデジタル画像を取得する。このように、周囲センサ群１は、車両Ｖの周囲に存在する物標をそれぞれ検出する複数のセンサからなる。周囲センサ群１には、このほかに、ソナー、レーダーが含まれていてもよい。車両Ｖの周囲に存在する物標には、車両Ｖの周囲の走路上に存在する白線、縁石、中央分離帯、ガードレール、リフレクタ等の走路境界を示す物標、停止線、横断歩道、制限速度等の路面標示、標識、信号機、電柱など道路構造物が含まれる。

図２は、周囲センサ群１が車両Ｖに搭載された状態を図示した一例である。ＬＲＦ１０１、１０２は、例えば、車両Ｖの左右両側のフロントフェンダー近傍にそれぞれ搭載可能である。ＬＲＦ１０１、１０２は、例えば、車両Ｖの前後方向Ｄに沿う回転軸として、照射するレーザ光の軌跡が路面に対する垂直面をなすように所定の走査角（例えば９０°）でレーザ光を走査する。これにより、ＬＲＦ１０１、１０２は、車両Ｖの左右方向に存在する縁石等の物標を検出することができる。ＬＲＦ１０１、１０２は、逐次、検出した物標の形状を検出結果として処理装置３に出力する。

カメラ２０１、２０２は、例えば、車両Ｖの左右両側のドアミラーにそれぞれ搭載可能である。カメラ２０１、２０２は、例えば、ＣＣＤ、ＣＭＯＳ等の固体撮像素子により画像を撮影する。カメラ２０１、２０２は、例えば、車両Ｖの側方の路面を撮影する。カメラ２０１、２０２は、逐次、撮影した画像を処理装置３に出力する。

図１に戻り、記憶装置４は、道路上又は道路周辺に存在する物標の位置情報を含む地図情報４１を記憶する地図情報格納部である。記憶装置４は、半導体メモリ、磁気ディスク等から構成可能である。地図情報４１に記録される物標（ランドマーク）は、例えば、停止線、横断歩道、横断歩道予告、区画線等を示す道路標示や、縁石等の構造物の他、周囲センサ群１により検出可能な種々の設備等を含む。地図情報４１は、縁石等、実際は高さを有する物標であっても、二次元平面上の位置情報のみで記述される。地図情報４１において、縁石、白線等の位置情報は、両端点の二次元位置情報を有する直線情報の集合体で定義される。地図情報４１は、実環境の形状が曲線の場合、折れ線で近似された二次元平面上の直線情報として記述される。

車両センサ群５は、ＧＰＳ受信機５１、アクセルセンサ５２、ステアリングセンサ５３、ブレーキセンサ５４、車速センサ５５、加速度センサ５６、車輪速センサ５７、及びヨーレートセンサ等のその他のセンサを備える。各センサ５１〜５７は処理装置３に接続され、逐次、各種の検出結果を処理装置３に出力する。処理装置３は、車両センサ群５の各検出結果を用いて、地図情報４１における車両Ｖの位置を算出したり、単位時間における車両Ｖの移動量を示すオドメトリを算出したりすることができる。例えば、車両Ｖの移動量は、タイヤの回転数によるオドメトリ計測、ジャイロ、加速度センサを用いた慣性計測、ＧＮＳＳ（汎地球航法衛星システム）など衛星からの電波を受信する方法、さらに外界センサの計測値の変化から移動量の推定を行なうＳＬＡＭ（Simultaneous Localization and Mapping）などさまざまな方策が考えられるが、いずれの方法を用いても構わない。

処理装置３は、物標位置検出部３１と、移動量推定部３２と、物標位置蓄積部３３と、直線抽出部３４と、物標位置選択部３５と、自己位置推定部３６と、物標属性推定部３７とを有する。処理装置３は、例えば、中央演算処理装置（ＣＰＵ）、メモリ、及び入出力Ｉ／Ｆ等を備える集積回路であるマイクロコントローラにより構成可能である。この場合、マイクロコントローラに予めインストールされたコンピュータプログラムをＣＰＵが実行することにより、処理装置３を構成する複数の情報処理部（３１〜３７）が実現される。処理装置３を構成する各部は、一体のハードウェアから構成されてもよく、別個のハードウェアから構成されてもよい。マイクロコントローラは、例えば自動運転制御等の車両Ｖに関わる他の制御に用いる電子制御ユニット（ＥＣＵ）と兼用されてもよい。「自己位置推定回路」には、移動量推定部３２と、物標位置蓄積部３３と、直線抽出部３４と、物標位置選択部３５と、自己位置推定部３６と、物標属性推定部３７とが含まれる。

物標位置検出部３１は、ＬＲＦ１０１、１０２及びカメラ２０１、２０２の少なくともいずれかの検出結果に基づいて、車両Ｖの周囲に存在する物標と車両Ｖとの相対位置を検出する。物標位置検出部３１が検出する相対位置は、車両座標系における位置である。車両座標系は、例えば、車両Ｖの後輪車軸中心を原点とし、前方向をｘ軸の正方向、左方向をｙ軸の正方向、上方向をｚ軸の正方向とすればよい。また、ＬＲＦ１０１、１０２及びカメラ２０１、２０２の座標系（センサ座標系）から車両座標系への変換式は予め物標位置検出部３１に設定される。「物標検出センサ」には、車両センサ群５と物標位置検出部３１とが含まれる。

移動量推定部３２は、車両センサ群５が備える少なくともいずれかのセンサの検出結果情報に基づいて、単位時間における車両Ｖの移動量であるオドメトリを検出する。車両Ｖの移動量は、オドメトリ座標系における移動量として検出される。物標位置蓄積部３３は、物標位置検出部３１により検出された物標の相対位置を、移動量推定部３２により検出された車両Ｖの移動量だけ移動させた位置を、物標位置データとして処理装置３内の一次記憶装置または記憶装置４に蓄積する。

直線抽出部３４は、物標位置蓄積部３３により蓄積された物標位置データから直線情報を抽出する。物標属性推定部３７は、ＬＲＦ１０１、１０２及びカメラ２０１、２０２の少なくともいずれかの検出結果に基づいて、物標の属性を推定する。物標位置選択部３５は、物標位置データの車両との相対位置の信頼性に基づいて物標位置データを選択する。物標位置選択部３５は、直線抽出部３４により抽出された直線情報、及び物標属性推定部３７により推定された物標の属性に基づいて、物標位置データの車両Ｖとの相対位置の信頼性を判断する。自己位置推定部３６は、選択した物標位置データと、道路上又は道路周辺に存在する物標の位置情報を含む地図情報とを照合することにより、車両Ｖの現在位置である自己位置を推定する。

図３を参照して、図１の自己位置推定装置を用いた自己位置推定方法の一例を説明する。先ず、ステップＳ０１において、自己位置推定装置は、周囲センサ群１を用いて車両Ｖの周囲を計測する。

ステップＳ０３に進み、周囲センサ群１は、車両Ｖの周囲に存在する物標をそれぞれ検出する。ステップＳ０５に進み、物標位置検出部３１は、ＬＲＦ１０１、１０２及びカメラ２０１、２０２の少なくともいずれかの検出結果に基づいて、ＬＲＦ１０１、１０２及びカメラ２０１、２０２に対する物標の位置、即ち、センサ座標系における物標の相対位置を推定する。例えば、カメラ２０１、２０２の場合、画像内位置と距離の関係をあらかじめ計測しておいてもよい。或いは、モーション・ステレオ方式を活用することもできる。これに限らず、その他の既知の方法を用いてもよい。また他の距離情報を取得可能なセンサー（ソナー、ＬＲＦ、レーダー）などであれば、その値をそのまま活用すればよい。

図４は、自己位置推定を行う際に車両Ｖが走行する環境を図示した一例である。図４に示す例において、ＬＲＦ１０１から射出されたレーザ光は、縁石６１含む路面にライン６４のように照射される。物標位置検出部３１は、照射されたレーザ光の方向及び距離から形状の変化が大きい場所を縁石６１の位置として抽出し、センサ座標系における位置を検出する。ＬＲＦ１０１、１０２の鉛直下方向には常に路面があると仮定できるため、路面と高さを比較した場合に大きな変化ある点を抽出することで縁石６１の検出を行うことができる。

また、物標位置検出部３１は、カメラ２０１、２０２により撮像された画像の輝度情報から、車両Ｖの両側方に存在する白線６２、６３を検出する。例えば、物標位置検出部３１は、カメラ２０１、２０２により撮像されたグレースケール画像から、暗、明、暗と順に輝度が変化するパターンを検出することで、明部の中央を白線６２、６３として検出することができる。カメラ２０１、２０２及び路面の位置関係から、センサ座標系における白線６２、６３の位置を検出することができる。ステップＳ０５において検出されたセンサ座標系における位置は、高さ情報が除外され、以降、二次元データとして扱われる。

ステップＳ０７に進み、物標位置検出部３１は、予め設定した変換式を用いて、センサ座標系における物標の相対位置を、車両座標系における物標の相対位置へ変換する。

図５（ａ）〜図５（ｄ）は、図４に示す例において、時刻ｔ１〜ｔ４の間に物標位置検出部３１により検出された、車両座標系における縁石６１の位置７１及び白線６２、６３の物標位置データ７２、７３を示す図である。ｔ１が最も過去の時刻であり、ｔ４が最も新しい時刻である。

次に、ステップＳ０７において、移動量推定部３２は、車両センサ群５による検出結果に基づいて算出した車両Ｖの移動量を積分することにより、オドメトリ座標系における車両Ｖの位置を算出する。オドメトリ座標系は、例えば、自己位置推定装置に電源が投入された時点や処理がリセットされた時点の車両Ｖの位置を原点とし、車両Ｖの方位角を０°とすればよい。車両Ｖの移動量の積分は、オドメトリ座標系において行われる。

図６は、図５（ａ）〜図５（ｄ）に示す例において、車両センサ群５による検出結果に基づいて算出された車両Ｖの移動量（Ｍ１、Ｍ２、Ｍ３、Ｍ４）を積分した結果を示す図である。移動量には、二次元座標上における位置及び姿勢（θ：ヨー角）の変化が含まれる。このように、移動量推定部３２は、オドメトリ座標系における車両Ｖの位置（Ｘｏ、Ｙｏ）を算出する。

ステップＳ０７において、物標位置蓄積部３３は、物標位置検出部３１により検出された車両座標系における物標の相対位置を、移動量推定部３２により検出された車両Ｖの移動量だけ移動させた位置を、物標位置データとして蓄積する。

図７は、図５及び図６に示す例において、オドメトリ座標系に変換された物標位置データ（７１ａ〜７１ｄ、７２ａ〜７２ｄ、７３ａ〜７３ｄ）を示す図である。このように、物標位置蓄積部３３は、過去（ｔ１、ｔ２、ｔ３、・・・）に計測されたセンサ座標系における物標の位置を、車両Ｖの移動量（Ｍ１、Ｍ２、Ｍ３、Ｍ４）に基づいて、オドメトリ座標系における物標の位置へ変換し、これを物標位置データとして蓄積する。

ステップＳ０９に進み、物標位置選択部３５は、蓄積された複数の物標位置データから走路境界を示す物標位置データ（７１ａ〜７１ｄ、７２ａ〜７２ｄ、７３ａ〜７３ｄ）を抽出し、抽出された物標位置データ（７１ａ〜７１ｄ、７２ａ〜７２ｄ、７３ａ〜７３ｄ）に対して、車両Ｖとの相対位置の信頼性を計算する。

先ず、図８に示すように、直線抽出部３４は、物標位置蓄積部３３により蓄積された物標位置データ（７１ａ〜７１ｄ、７２ａ〜７２ｄ、７３ａ〜７３ｄ）から直線情報（Ｎ１、Ｎ２、Ｎ３）を抽出する。白線検出結果（物標位置データ）に対して線形近似を実施する。そして、物標位置選択部３５は、物標の相対位置から得られる車両Ｖから物標までの距離と車両Ｖから物標までの想定距離との差に応じて物標位置データの車両Ｖとの相対位置の信頼性を判断する。当該差が小さいほど、信頼性が高いと判断する。

例えば、直線抽出部３４は、図９に示す走路境界を示す物標位置データ（７２、７３）に対して直線（Ｎ２、Ｎ３）を近似する。物標位置選択部３５は、車両Ｖから直線（Ｎ２、Ｎ３）までの車幅方向の距離を計測する。走路幅が４ｍであり、車両Ｖが走路の中央を走行している場合、車両Ｖの中心から白線までの想定距離は２ｍである。走路に対する車両Ｖの位置のずれや、検出誤差等を含めて、車両Ｖから直線（Ｎ２、Ｎ３）までの距離と想定距離（２ｍ）との差の絶対値が１ｍ以上である場合、車両Ｖから直線（Ｎ２、Ｎ３）までの距離が不正確である可能性が高いと判断する。図９に示す例では、距離ＷＬと想定距離（２ｍ）との差の絶対値は１ｍ未満であるが、距離ＬＲと想定距離（２ｍ）との差の絶対値は１ｍ以上であった。よって、物標位置選択部３５は、右側の走路境界を示す物標位置データ７３の信頼性を低く評価し、左側の走路境界を示す物標位置データ７２の信頼性を高く評価する。物標位置選択部３５は、信頼性を低く評価した物標位置データ７３を排除し、信頼性を高く評価した物標位置データ７２のみを選択する。

上記した想定距離との差に基づく信頼性の判断手法は、白線、縁石などの走路境界に限らず、他の物標にも適用することができる。例えば、標識、信号機、電柱など道路構造物は路側帯に存在する。よって、走路幅から想定距離を設定することができ、車両Ｖから検出された道路構造物までの相対距離と想定距離の差を算出することができる。

物標位置選択部３５は、上記した想定距離との差に基づいて選択された物標位置データ７２に対して、更に、物標の属性に基づく信頼性を判断する。具体的には、物標位置選択部３５は、物標属性推定部３７により推定された物標の属性に基づいて、物標位置データ７２の信頼性を判断し、自己位置推定に用いる物標位置データを更に絞り込む。

例えば、同じ白線であっても、破線よりも実線の方が検出できる領域が大きいため、その相対位置の検出精度、即ち信頼性は高いと判断することができる。地図情報を参照することにより、検出された白線が実線であるか破線であるかを予め特定することができる。車両Ｖの一方側に位置する白線は実線であるのに対して、他方側に位置する白線が破線であることが判明した場合、検出誤差や車両Ｖからの距離が同程度であったとしても、一方側に位置する白線を示す物標位置データの信頼性が高いと判断する。そして、一方側に位置する白線を示す物標位置データを選択する。

白線の種類は、物標の属性の一例であって、それ以外の属性についても適用することができる。例えば、区画線の色について、黄色線より白色線のほうが検出しやすいので、白色線の信頼性を高く判断する。また、異なる物標同士で、信頼性を判断してもよい。例えば、停止線と横断歩道では、横断歩道の特徴箇所が停止線のそれよりも多いので、横断歩道の信頼性を高く判断する。

物標位置選択部３５は、上記した物標の属性に基づいて選択された物標位置データに対して、更に、物標位置データが連続して検出できた時間に基づいて、物標位置データの信頼性を判断する。

白線に限らず、一般的な環境に存在する物標は、経年劣化やオクルージョンなどの影響により、常に一定の信頼性をもって継続して検出できるとは限らない。また、複数のセンサが異なる方向をカバーするセンサーフュージョン方式を採用する場合では、常にある方向のみ検出が不確かになることも考えられる。そこで、白線や走路境界の情報については、その検出時間を合わせて評価し、ある一定時間（例えば１０秒）以上連続して検出された場合に限り、信頼性が高いと判断し、この物標位置データを選択する。

物標位置選択部３５は、上記した連続検出時間に基づいて選択された物標位置データに対して、更に、走路境界の物標位置データを線形近似したときの誤差分布に基づいて、物標位置データの信頼性を判断する。換言すれば、物標位置選択部３５は、直線抽出部３４により抽出された直線情報（近似直線）に基づいて、物標位置データの信頼性を判断し、自己位置推定に用いる物標位置データを更に絞り込む。

物標位置選択部３５は、車両Ｖが走行している走路を特定する走路境界（例えば、白線）を示す物標位置データとして、平行した複数の走路境界が検出されているか否かを判断する。そして、平行した複数の走路境界が検出されている場合、白線検出結果（物標位置データ）のうち、直線で近似できる範囲の物標位置データの信頼性を高く評価し、自己位置推定に用いる物標位置データとして選択する。例えば、図１０に示すように、車両Ｖが走行している走路を特定する、平行した複数の走路境界を示す物標位置データ（７２ｊ、７２ｋ、７３ｊ、７３ｋ）が検出されている。直線抽出部３４は、走路境界を示す物標位置データに対して線形近似を実施する。物標位置選択部３５は、物標位置データ（７２ｊ、７２ｋ、７３ｊ、７３ｋ）のうち、直線で近似できる範囲ＬＡに含まれる物標位置データ（７２ｊ、７３ｊ）を選択する。このときに、物標位置選択部３５は、車両Ｖを基準として、直線で近似できる範囲ＬＡを広げていく。例えば、近似線に対する物標位置データの最小距離が−１５ｃｍ以上１５ｃｍ以下である物標位置データの数が８０％以上となる区間を、直線で近似できる範囲ＬＡとして設定する。一方、直線で近似できる範囲ＬＡに含まれない物標位置データ（７２ｋ、７３ｋ）は排除する。

なお、図１１に示すように、車両Ｖが走行している走路がカーブ区間である場合、近似線は必ずしも直線とはならない。そこで、直線抽出部３４は、線形近似（直線近似）の代わりに曲線近似を実施する。物標位置選択部３５は、曲線（Ｎ２、Ｎ３）で近似できる範囲ＬＢに含まれる物標位置データ（７２ｍ、７３ｍ）の信頼性を高く評価し、自己位置推定に用いる物標位置データとして選択する。一方、曲線（Ｎ２、Ｎ３）で近似できる範囲ＬＢに含まれない物標位置データ（７２ｎ、７３ｎ）は排除する。

本実施形態では、物標位置選択部３５が、（１）車両Ｖから物標までの想定距離との差、（２）物標の属性、（３）連続検出時間、（４）走路境界を示す物標位置データを線形近似したときの誤差分布、の順番で、物標位置データの車両Ｖとの相対位置の信頼性を判断する例を示した。本発明はこれに限定されず、信頼性判断の順番は任意に入れ替えることが出来る。また、（１）〜（４）の一部の判断のみを実施することもできる。さらに、それぞれの信頼性判断を定量化して、総合評価してもよい。例えば、各信頼性判断で評価点を多段階で与えて、その評価点を加算して、総合評点を算出してもよい。これにより、検出された物標の信頼性を定量化した上で判断することができる。

次に、ステップＳ１３に進み、自己位置推定部３６は、物標位置選択部３５により選択された物標位置データと、地図情報４１における物標の位置とを照合する。換言すれば、地図情報４１における物標の位置と、物標位置選択部３５により信頼性が高いと判断された物標位置データとをマッチングする。

ステップＳ１５に進み、自己位置推定部３６は、上記した物標の位置の照合（マップ・マッチング）により、車両Ｖの自己位置を推定する。すなわち、車両Ｖの東西方向の位置（Ｘ座標）と、南北方向の位置（Ｙ座標）と、方位角（ヨー角θ）とからなる合計３自由度の位置と姿勢角を推定する。地図上の位置を推定する手法として、既知の自己位置推定手法を用いればよい。ステップＳ１７に進み、自己位置推定部３６は、推定した車両Ｖの自己位置を出力する。

なお、ステップＳ１３における照合には、ＩＣＰ（Iterative Closest Point）アルゴリズムを用いることができる。このとき、自己位置推定部３６は、地図情報４１に含まれる物標の位置のうち、例えば区画線に対しては、その両端の端点を評価点としてマッチングする。また、車両Ｖ（周囲センサ群１）に近いほど物標位置データはオドメトリの誤差の影響を受けないので、自己位置推定部３６は、車両Ｖの近傍については直線補完して評価点の数を増加させ、車両Ｖの遠方については評価点の数を減少させることができる。

以上のように、本実施の形態によれば、以下の作用効果が得られる。

物標位置データの車両Ｖとの相対位置の信頼性に基づき、物標位置データを選択するので、相対位置の誤差が多いと推定される物標位置データが排除でき、自己位置の推定精度が向上する。

物標位置選択部３５は、車両Ｖから物標までの距離と想定距離との差が小さいほど、物標位置データの車両との相対位置の信頼性が高いと判断する。これにより、相対位置の誤差が大きいと推定される物標位置データを適切に排除できるので、自己位置の推定精度が向上する。

物標位置選択部３５は、物標位置データの車両Ｖとの相対位置の信頼性を、物標の属性に基づいて判断する。例えば、白線の実線と破線とを比較した場合、定常的に物標位置データを取得できる実線の方が破線に比べて、信頼性が高いと判断する。よって、相対位置の誤差の多いと推定される物標位置データを適切に判断できるので、自己位置の推定精度が向上する。

物標位置選択部３５は、物標位置データの車両Ｖとの相対位置の信頼性を、物標位置データが連続して検出できた時間に基づいて判断する。これにより、安定して正確な自己位置の推定を行なうことが可能となる。

物標位置選択部３５は、車両Ｖが走行する走路を特定する、平行した複数の走路境界を示す物標位置データが検出される場合、車両Ｖとの相対位置の信頼性が高い物標位置データを選択する。これにより、位置精度が高い走路境界が選択され、より自己位置推定精度が高くなる。

物標位置選択部３５は、走路境界を近似した時の近似線との誤差が小さいほど、走路境界を示す物標位置データの車両Ｖとの相対位置の信頼性が高いと判断する。これにより、検出位置の精度が高い走路境界が選択され、自己位置の推定精度が更に高くなる。

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

移動体には、陸上を移動する移動物体としての車両Ｖに限定されず、船舶、航空機、宇宙機、その他の移動物体が含まれる。

上述の各実施形態で示した各機能は、１又は複数の処理回路により実装され得る。処理回路は、電気回路を含む処理装置等のプログラムされた処理装置を含む。処理装置は、また、実施形態に記載された機能を実行するようにアレンジされた特定用途向け集積回路（ＡＳＩＣ）や従来型の回路部品のような装置を含む。

１ 周囲センサ群（物標検出センサ）
３１ 物標位置検出部（物標検出センサ）
３２ 移動量推定部（自己位置推定回路）
３３ 物標位置蓄積部（自己位置推定回路）
３４ 直線抽出部（自己位置推定回路）
３５ 物標位置選択部（自己位置推定回路）
３６ 自己位置推定部（自己位置推定回路）
３７ 物標属性推定部（自己位置推定回路）
４１ 地図情報
６１ 縁石（物標）
６２、６３ 白線（物標）
７２ｊ、７２ｋ、７２ｍ、７２ｎ 物標位置データ
７３ｊ、７３ｋ、７３ｎ、７３ｍ 物標位置データ
Ｍ１〜Ｍ４ 移動体の移動量
Ｎ１、Ｎ２、Ｎ３ 複数の物標位置データを近似した線
Ｖ 車両（移動体）

Claims (6)

1. 移動体に搭載され、前記移動体の周囲に存在する物標と前記移動体との相対位置を検出する物標検出センサと、
   前記相対位置を前記移動体の移動量だけ移動させた位置を物標位置データとして蓄積し、蓄積された前記物標位置データと、道路上又は道路周辺に存在する前記物標の位置情報を含む地図情報とを照合することにより、前記移動体の現在位置である自己位置を推定する自己位置推定回路と、を用いた自己位置推定方法において、
   前記物標位置データの前記移動体との相対位置の信頼性に基づいて、前記物標位置データの中から地図情報と照合する物標位置データを選択し、
   選択した物標位置データと前記地図情報とを照合することにより前記自己位置を推定し、
   前記相対位置から得られる前記移動体から前記物標までの前記移動体の車幅方向の距離と、前記移動体が走行する走路から想定される前記移動体から前記物標までの前記車幅方向の想定距離との差が小さいほど、前記信頼性が高いと判断する
   ことを特徴とする自己位置推定方法。
2. 移動体に搭載され、前記移動体の周囲に存在する物標と前記移動体との相対位置を検出する物標検出センサと、
   前記相対位置を前記移動体の移動量だけ移動させた位置を物標位置データとして蓄積し、蓄積された前記物標位置データと、道路上又は道路周辺に存在する前記物標の位置情報を含む地図情報とを照合することにより、前記移動体の現在位置である自己位置を推定する自己位置推定回路と、を用いた自己位置推定方法において、
   前記物標位置データの前記移動体との相対位置の信頼性に基づいて、前記物標位置データの中から地図情報と照合する物標位置データを選択し、
   選択した物標位置データと前記地図情報とを照合することにより前記自己位置を推定し、
   前記移動体が走行する走路を特定する走路境界を示す複数の物標位置データを近似した線と前記複数の物標位置データとの誤差が小さいほど、前記走路境界を示す物標位置データの移動体との相対位置の信頼性が高いと判断する
   ことを特徴とする自己位置推定方法。
3. 前記物標の属性に基づいて前記信頼性を判断することを特徴とする請求項１又は２に記載の自己位置推定方法。
4. 前記物標位置データが連続して検出できた時間に基づいて前記信頼性を判断することを特徴とする請求項１〜３のいずれか一項に記載の自己位置推定方法。
5. 移動体に搭載され、前記移動体の周囲に存在する物標と前記移動体との相対位置を検出する物標検出センサと、
   前記相対位置を前記移動体の移動量だけ移動させた位置を物標位置データとして蓄積し、前記物標位置データの前記移動体との相対位置の信頼性に基づいて、前記物標位置データの中から地図情報と照合する物標位置データを選択し、選択した物標位置データと、地図上に存在する前記物標の位置情報を含む地図情報とを照合することにより、前記移動体の現在位置である自己位置を推定する自己位置推定回路と、を備え、
   前記自己位置推定回路は、前記相対位置から得られる前記移動体から前記物標までの前記移動体の車幅方向の距離と、前記移動体が走行する走路から想定される前記移動体から前記物標までの前記車幅方向の想定距離との差が小さいほど、前記信頼性が高いと判断する
   ことを特徴とする自己位置推定装置。
6. 移動体に搭載され、前記移動体の周囲に存在する物標と前記移動体との相対位置を検出する物標検出センサと、
   前記相対位置を前記移動体の移動量だけ移動させた位置を物標位置データとして蓄積し、前記物標位置データの前記移動体との相対位置の信頼性に基づいて、前記物標位置データの中から地図情報と照合する物標位置データを選択し、選択した物標位置データと、地図上に存在する前記物標の位置情報を含む地図情報とを照合することにより、前記移動体の現在位置である自己位置を推定する自己位置推定回路と、を備え、
   前記自己位置推定回路は、前記移動体が走行する走路を特定する走路境界を示す複数の物標位置データを近似した線と前記複数の物標位置データとの誤差が小さいほど、前記走路境界を示す物標位置データの移動体との相対位置の信頼性が高いと判断する
   ことを特徴とする自己位置推定装置。

Images