JP7289761B2 - 車両の自己位置推定装置、および、自己位置推定方法 - Google Patents

Description

本発明は、自車が走行する周囲環境構造と、地図情報に記録されている構造情報を照合し、自車が地図中のどこにいるのかを示す詳細位置を推定する、車両の自己位置推定装置および自己位置推定方法に関する。

従来、測位装置により検出された車両の大凡の現在位置を補正する技術として、車両に搭載されたカメラを用いて車両から交差点までの距離を求め、地図情報における車両の位置を特定することにより、車両の大凡の現在位置を補正する技術が提案されている。

例えば、特許文献１の車両位置判定方法では、晴天時や積雪時、高速走行時や左右走行車線などの走行条件に基づいて認識対象とする領域を変更することで、地物認識部の処理負荷を低減した自己位置推定技術について記載がある。

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

特許文献１では、積雪によって走行環境の形状が変わる場合や、圧雪路で路面標示が見えない道路が続く場合には、同文献の段落００２８や図２に示されるように、カメラが撮影した画像の上半分の領域に含まれる道路標識や案内標識を認識することで、車両の現在位置の推定精度を数ｍ程度の誤差まで補正することができる。

しかしながら、特許文献１の技術では、自己位置推定誤差を、自動運転に必要とされる数ｃｍ以下に抑制することができず、自動運転には利用できないという問題があった。

そこで、本発明では、積雪によって走行環境に存在する環境構造物が遮蔽される状況や、環境構造物の形状が変わる状況が継続する場合、また、圧雪路で路面標示が見えない状況が継続する場合であっても、自動運転の実行時に求められる高精度な自己位置推定が可能な自己位置推定装置を提供することを目的とする。

上記課題を解決するために、本発明の自己位置推定装置は、地図情報を記憶する地図データベースと、周囲環境の三次元点群データを取得する周囲環境観測センサと、に接続され、車両の自己位置を推定する自己位置推定装置であって、大凡の自己位置を推定する粗位置推定部と、詳細な自己位置を推定する三次元照合部と、を有し、前記三次元照合部は、前記地図情報から積雪時に観測可能な外形情報を抽出した三次元地図データを生成する照合用マップ抽出部と、前記周囲環境観測センサから前記三次元点群データを得るセンシング結果抽出部と、前記三次元地図データと前記三次元点群データを三次元照合することで、前記粗位置推定部が推定した自己位置を補正する位置補正部と、を有し、前記照合用マップ抽出部は、前記地図情報に含まれる外形情報から、前記周囲環境観測センサで観測した積雪高さ以上の外形情報を抽出することで、前記三次元地図データを生成するものとした。

本発明の自己位置推定装置によれば、積雪によって走行環境に存在する環境構造物が遮蔽されたり、環境構造物の形状が変わる状況が継続したり、また、圧雪路で路面標示が見えない状況が継続したりする場合にでも、自動運転の実行時に求められる高精度な自己位置推定が可能にとなる。

実施例１に係る自己位置推定装置のブロック図 実施例１の粗位置推定部で実施される処理のフローチャート 道路リンクに対して評価値を算出する場合の一例を説明する説明図 積雪時三次元照合部の機能ブロック図 照合用マップ抽出部の機能ブロック図 センシング結果抽出部の機能ブロック図 実施例２の粗位置推定部で実施されるフローチャート 実施例３の自己位置推定装置のブロック図。 積雪のない環境と積雪のある環境の例

以下、本発明に係る自己位置推定装置の実施例を、図面を用いて説明する。

図１は、本発明の実施例１に係る自己位置推定装置１０の構成とそれに接続されるセンサ等を説明するブロック図である。ここに示すように、自己位置推定装置１０は、車両挙動観測センサ１、絶対位置取得センサ２、地図データベース３、周囲環境観測センサ４、積雪判定部５、車両制御部６と接続されている。また、自己位置推定装置１０は、粗位置推定部１１、マップマッチング部１２、三次元照合部１３を備えており、三次元照合部１３は、汎用照合部１４と、積雪対応照合部１５と、を有している。なお、自己位置推定装置１０は、具体的には、ＣＰＵ等の演算装置、半導体メモリ等の主記憶装置、補助記憶装置、および、通信装置などのハードウェアを備えた計算機であり、主記憶装置にロードされたプログラムを演算装置が実行することで、粗位置推定部１１等の各機能を実現するが、以下では、このような計算機分野での周知技術を適宜省略しながら、各部の詳細を説明する。

車両挙動観測センサ１は、例えば加速度センサ、角速度センサ、角加速度センサ、車輪パルスセンサ、車速センサなどの自車の相対運動を観測するための１つ以上からなるセンサである。これらの車両挙動観測センサ１から得られたセンシング結果を時系列で累積することにより、自車の相対的な移動および姿勢変化を連続的に推定することが可能である。なお、センシング結果に混入する誤差やノイズも累積してしまうため、一般的に自車挙動観測センサ１による推定結果は徐々に精度が悪化する特性がある。

絶対位置取得センサ２は、例えばＧＮＳＳ（Global Navigation Satellite System：全球測位衛星システム）の受信装置、スードライド（擬似衛星）やビーコン（無線標識）などの電波の受信装置、環境側に設置したバーコードなどの可視マーカや磁気マーカの認識装置などの、自車の絶対位置を認識するための１つ以上からなるセンサである。これらの絶対位置取得センサ２から得られたセンシング結果から、その時点の自車位置および姿勢を推定することが可能である。なお、絶対位置取得センサ２による自車位置および姿勢の推定は、時間的または空間的な観点で離散的であり、例えばＧＮＳＳの測位間隔は１秒から５秒程度であることが多く、マーカ認識装置はマーカが設置されている地点近傍の場所に限定されている。

粗位置推定部１１では、車両挙動観測センサ１と絶対位置取得センサ２のセンシング結果を用いて、時々刻々と変化する大凡の自己位置（粗位置）を推定する。また、ここでの自己位置推定の初期値として、前時刻に三次元照合部１３で算出された自己位置推定結果を用いることができる。なお、粗位置推定部１１で推定する自己位置（粗位置）には、数ｍ程度の誤差がある可能性があるため、目的地への経路を案内するカーナビゲーション等には利用できるが、自動運転には利用できない精度のものである。

マップマッチング部１２では、地図データベース３からの地図情報と、粗位置推定部１１からの自己位置推定結果を照合することで、自己位置推定結果の精度改善をおこなう。例えば、自車が交差点を通過する状況を想定すると、自車は地図データベース３に格納されている交差点の形状に沿って走行しているはずであるので、粗位置推定部１１による自己位置推定結果が交差点形状に合わない場合には、自己位置推定結果に誤差があるものとし扱い、地図に合わせて自己位置を補正する。

周囲環境観測センサ４は、例えばステレオカメラ、単眼カメラ、ＬｉＤＡＲ（Laser Imaging Detection and Ranging：レーザによる画像検出と測距）、ミリ波、超音波ソナー、ＴＯＦカメラ（Time of Flight）などの自車周囲を観測するための１つ以上からなるセンサである。これらを用いて自車周囲の障害物や環境構造物の形状、模様、色、大きさ、あるいは三次元点群の集合などの情報を取得するとともに、自車に対する相対的な位置、姿勢を取得する。

積雪判定部５は、周囲環境観測センサ４からの情報を入力とし、観測している領域内に積雪があるか否かを判定する。この判定は公知の技術が複数提案されているため、それらを用いても良い。例えば、単眼カメラの場合、昼間は積雪表面で太陽光をよく反射するため、撮像時の明度が高くなり白色領域として観測できること、自動露光機能がある場合にはシャッタ時間が短くなること、といった特徴がある。そこで、これらを実験的に求めた閾値求めておき、所定の閾値よりも大きい輝度値をもつ画素数が撮像領域内に一定以上あり、かつ所定の閾値よりもシャッタ時間が短い場合に、積雪があると判定することができる。なお、この積雪判定部５は、専用のハードウェアを用意しても良いし、自己位置推定装置１０を実現するとのと同じ計算機により実現されるものであっても良い。

三次元照合部１３は、時々刻々と変化する自己位置を数ｃｍ程度の精度で推定するものであり、汎用照合部１４と、積雪対応照合部１５を有し、積雪判定部５の判定結果に応じて、使用する照合部を切り替える。例えば、判定結果が積雪なしの場合は、汎用照合部１４の使用が選択され、判定結果が積雪ありの場合は、積雪対応照合部１５の使用が選択される。なお、積雪が少ないと判断されたときなどには、両照合部を併用しても良い。

汎用照合部１４は、地図データベース３からの地図情報と、マップマッチング部１２からの自己位置推定結果と、周囲環境観測センサ４からの障害物や環境構造物の形状、模様、色、大きさ、あるいは三次元点群などの情報を取得する。そして、地図情報から、自己位置推定結果付近の三次元構造物を含む周辺地図を抽出する。次に、抽出された三次元構造物の位置や姿勢と、周囲環境観測センサ４で得られた障害物や環境構造物の位置や姿勢を比較し、照合することで地図データベース３に対する周囲環境観測センサ４の視点を算出する。この照合技術は公知の技術が複数提案されているため、それらを用いても良い。

例えば、ＩＣＰ（Iterative Closest Point）という技術がある。これは２つの三次元点群Ａ，Ｂを照合するときに、まず仮視点を仮定して点群を重ね合わせ、点群Ａのある点ａに対し、点群Ｂ中で最も近い点ｂを算出し、点ａと点ｂの二乗距離を算出する。この操作を点群Ａ中のすべての点に対しておこない、ペアとなった点間の距離を合計する。次に、任意の方向に仮視点を移動させ、同様にペアとなった点間の距離を合計する。合計距離が小さくなる方向に仮視点を移動させていくことで、最終的に最も点群Ａと点群Ｂが近くなる視点を求めることができる。このような技術を用いることにより、汎用照合部１４では、自己位置を数ｃｍ程度の精度で推定できる。

積雪対応照合部１５では、基本的に、汎用照合部１４と同等の処理が実施され、詳細な自己位置を推定する。ただし、積雪対応照合部１５は、積雪判定部５で積雪があると判定された場合に利用されるものであるため、積雪状況に特化した処理も実施される。この処理の詳細は後述する。

三次元照合部１３で算出され、更新された自己位置は、粗位置推定部１１の入力にフィードバックされ、次の時刻の初期の自己位置として利用することができる。

車両制御部６は、運転者の運転操作に対して、運転者の操作を要しない自動運転を含む運転支援を可能とするものであり、先行車両に対する追い越し、車線維持、高速道路自動合流等を含む適応走行制御、障害物への自動回避制御、一時停止などの運転支援制御を実施する。

以下、本実施例の、粗位置推定部１１、マップマッチング部１２、三次元照合部１３の詳細を順次説明する。

＜粗位置推定部１１＞
図２に粗位置推定部１１で実施される処理のフローチャートを示す。

前述したように、粗位置推定部１１では、車両挙動観測センサ１と絶対位置取得センサ２のセンシング結果から、時々刻々と変化する大凡の自己位置（粗位置）を推定する。車両挙動観測センサ１からは連続的に推定結果が得られるが、時間経過とともに誤差が累積する。一方、絶対位置取得センサ２は誤差が累積しないが、離散的にしか自己位置が得られない。このため、絶対位置取得センサ２の推定結果が得られ、かつ信頼できる状態である場合にのみ、絶対位置取得センサ２の出力に基づいて自己位置を更新し、そうでない場合には、車両挙動観測センサ１の出力に基づいて自己位置と方位を相対的に変化させるよう更新する。粗位置推定部１１での処理は、具体的には、次のステップからなる。

ステップＳ１：車両挙動観測センサ１、絶対位置取得センサ２からセンサ時刻を付加した上でセンサ出力値を受信する。センサ時刻は内部に実時間時計を持つセンサについてはその時刻を利用し、加速度センサや角速度センサのような内部に実時間時計を持たないセンサの場合は、センシング時刻とセンサ出力を受信した装置との間の伝送時間を加味したオフセット時間を考慮して、センサ出力を受信する装置で時刻を付与し、記憶または蓄積する。

ステップＳ２：絶対位置取得センサ２の出力は時間または空間の観点で離散的であるため、常に出力が得られているとは限らない。そこで、前回値から更新されたか否かを判定する必要がある。出力が更新されていない場合には、ステップＳ５にすすむ。更新された場合にはステップＳ３にすすむ。

ステップＳ３：絶対位置取得センサ２の出力は状況によって精度が悪化する場合がある。例えば、ＧＮＳＳ受信装置においては、測位衛星からの電波が届きにくいトンネル内や地下駐車場などでは精度良い位置情報が得られない場合がある。また、多くの高層ビルが建ち並ぶ市街地では、衛星電波が建物で反射および回折して受信されるマルチパス現象が起きることで誤った位置情報を得る場合がある。こうした状況を判断するために、測位ステータス、ＤＯＰ値（Dilution Of Precision：精度劣化度）や、誤差楕円半径、使用衛星数、速度精度、方位精度などの指標がある。これらの指標が所定の閾値より良好であることをもって、精度が一定以上であると判定する。所定の閾値よりも良好であること、とは、当該業務従事者であれば自明ではあるが、例えば、測位ステータスであれば２Ｄ測位、３Ｄ測位、RTK-FLOAT測位、RTK-FIX測位の順に良好さが増す。また、ＤＯＰ値と誤差楕円半径は値が小さいほど良好であり、使用衛星数は多いほど良好である。精度が一定以上でない場合には、ステップＳ５にすすむ。精度が一定以上の良好な状態であればステップＳ４にすすむ。なお、精度が一定以上であるか否かを判定する際、複数の指標値をＡＮＤ条件とＯＲ条件を適宜使用して結合してもよい。

ステップＳ４：絶対位置取得センサ２による最新の出力が良好な精度で得られている場合には、この出力結果を自己位置に反映させることで、車両挙動観測センサ１による累積誤差をリセットすることができる。自己位置への反映は、十分に信頼性が高い場合には、絶対位置取得センサ２の値をそのまま自己位置に設定してもよいが、カルマンフィルタやパーティクルフィルタを用いて安定化させた出力値を自己位置に設定してもよい。

ステップＳ５：絶対位置取得センサ２の出力が古い場合、もしくは出力精度が劣化している場合には、車両挙動観測センサ１の出力を加算することで、短時間に変化した自己位置を更新する。その際、車両挙動観測センサ１の出力をそのまま自己位置に加算してもよいが、カルマンフィルタやパーティクルフィルタを用いて安定化させてもよい。

ステップＳ６：絶対位置取得センサ２の出力が良好な精度で得られている場合には、自己位置推定の信頼度を高くなるように設定し、車両挙動観測センサ１の出力を累積するに従い、信頼度が徐々に低くなるように設定する。例えば、次の式１のように設定できる。

（信頼度）＝α÷（絶対位置取得センサの精度指標）
－β×（車両挙動観測センサによる自己位置更新回数） … （式１）
または、カルマンフィルタを用いる場合には共分散行列の項を、信頼度算出の式に加えてもよい。

具体的な数値例を挙げる。（絶対位置取得センサ２の精度指標）を出力周期１［Ｈｚ］のＧＮＳＳ受信機の誤差楕円半径とし、その値域が１～５０［ｍ］とすると、小さい値であるほど精度が良好となる。（車両挙動観測センサ１による自己位置更新回数）を出力周期２０［Ｈｚ］加速度センサと各加速度センサとする。また、α＝１０００、β＝２とする。この場合、ＧＮＳＳ受信機の出力周期に合わせて、２０～１０００の信頼度が設定され、その後１秒かけて徐々に信頼度が低下していく挙動となる。

＜マップマッチング部１２＞
次にマップマッチング部１２の処理の流れを説明する。

マップマッチング部１２では、粗位置推定部１１で推定された自己位置が、地図上のどの道路を走行しているかを算出し、道路から逸脱していない場合には道路上に乗るように自己位置を補正する。このマップマッチング方法は公知の方法が複数提案されている。以下に具体例を１つ示す。

まず、道路情報を地図データベース３から取得する。この道路情報は、道路ノード番号、ノード位置、道路リンク番号、道路リンクの幅員などのうち１つ以上を含む。

次に、粗位置推定部１１が推定した自己位置の周囲に存在する各道路リンクに対して、自己位置からの垂線をおろし、各道路リンクとの交点を道路リンク候補点として求める。さらに、垂線の長さと、道路リンク方位と自車方位θ_Ｖとの差を考慮して評価量を算出する。

図３は、道路リンクに対して評価値を算出する場合の一例を説明するための説明図である。例えば、自車の位置（自車位置Ｐ_Ｖ）および姿勢（自車方位θ_Ｖ）とし、自車位置から所定の距離内にある複数の道路リンクを、道路ノードＮ_Ａ１から道路ノードＮ_Ａ２への道路リンクＬ_Ａ、道路ノードＮ_Ｂ１から道路ノードＮ_Ｂ２への道路リンクＬ_Ｂとする。さらに、自車位置Ｐ_Ｖから道路リンクＬ_Ａに対して下ろした垂線の長さをＤ_Ａ、道路リンクとの交点をＰ_Ａとする。同様に、自車位置Ｐ_Ｖから道路リンクＬ_Ｂに対して下ろした垂線の長さをＤ_Ｂ、道路リンクとの交点をＰ_Ｂとする。そして、各道路リンク候補点Ｐ_ＡおよびＰ_Ｂに対して評価量を算出する。道路リンク方位θ_Ａ、θ_Ｂと、自車方位θ_Ｖの差を方位角誤差と呼ぶ。また、道路リンクまで距離Ｄ_Ａ、Ｄ_Ｂを位置誤差と呼ぶ。この方位角誤差と位置誤差に関して共分散誤差を計算し、これに基づいて各道路リンク候補点の評価量を計算する。この計算結果により、道路を走行している確率が最も高い評価量を持つ道路リンク候補点を第一道路リンク候補点とする。

次に、道路リンクの位置、方位及びそれらの分散と、幅員情報と、推定位置及び推定方位及びそれらの共分散とに基づいて、自車が道路を逸脱しているかどうかを判定する。逸脱していないと判定された場合には、自車位置Ｐ_Ｖを第一道路リンク候補点に近づけるように補正する。その際、第一道路リンク候補点の位置をそのまま自車位置Ｐ_Ｖに設定してもよいが、カルマンフィルタやパーティクルフィルタを用いて安定化させた出力値を自車位置Ｐ_Ｖに設定してもよい。

＜三次元照合部１３＞
三次元照合部１３は、前述したように、その内部に、汎用照合部１４と積雪対応照合部１５をもち、積雪判定部５からの結果に応じて、いずれか一方、もしくは両方を使用するかを切り替える。例えば、積雪判定部５の判定結果が所定の時間以上「積雪なし」の状態であれば、汎用照合部１４の結果のみ使用し、所定の時間以上「積雪あり」の状態であれば、積雪対応照合部１５の結果を使用する。積雪判定部５の結果が変動する場合は、部分的に積雪が残っており、除雪が不完全である道路状況であると予想されるため、その場合には汎用照合部１４と積雪対応照合部１５の両方を使用する。両方を使用する場合、照合結果として出力される、地図中の周囲環境観測センサ４の視点が複数出てくるため、重み付き平均を用いて統合する。なお、カルマンフィルタやパーティクルフィルタを用いて安定化させた出力値を用いてもよい。

＜積雪対応照合部１５＞
図４は、積雪対応照合部１５の機能ブロック図である。ここに示すように、積雪対応照合部１５は、外形情報抽出部１６、照合用マップ抽出部１７、センシング結果抽出部１８、センシング結果照合部１９、位置補正部２０を有する。

＜外形情報抽出部１６＞
外形情報抽出部１６は、マップマッチング部１２から出力される自己位置（図３参照）に基づき、自己位置近傍の所定の範囲に該当する地図データベース３に格納されている地図情報を読み込み、立体形状に係る外形情報に変換する。例えば、地図データベース３に格納されている地図情報は、道路自体の形状である舗装路面の領域形状や、ガードレールや防音壁の道路に対する設置位置、ガードレールや防音壁の高さ、制限速度・走行規制・道路案内などを表す標識が設置される棒状・Ｌ字状・跨線状などの形状をとる支柱の道路に対する形状種別や設置位置、標識支柱の高さ、標識看板の取付高さと形状、電柱の道路に対する設置位置と高さ、カーブミラーの道路に対する設置位置・形状・鏡部の取付高さ、横断歩道や走行路区分線などの路面標示の位置と形状、信号機の形状や取付高さ、などの１つ以上を含む。また、道路だけでなく、路外にあるビルや塀、樹木などの建造物や構造物の形状を含んでいてもよい。さらに、走行車両に搭載した外界センサで得られた３Ｄ形状や点群情報、テクスチャ情報などを、サーバに収集し、通信手段や記録メディアにより配布された情報を含んでいてもよい。外形情報抽出部１６は、これらの格納された地図情報を読み込み、含まれる物体の外形情報に変換する。

例えば、地図情報から、高さ２ｍで制限速度５０ｋｍ／ｈの交通標識の所在情報を読み込んだ場合、その情報を、路外１ｍの地点に太さ０．１ｍで長さ２ｍの円柱が存在するという外形情報、路外１ｍ高さ２ｍの同じ地点に半径０．６ｍの円盤状の板が存在するという外形情報、さらにその円盤には５０という文字が掲示されているという外形情報、といった３つの外形情報に変換し出力する。

また、この外形情報は、３Ｄ点群の座標値で出力する方法のほか、円柱や円盤の中心軸とその半径といったベクトル形式での出力方法や、表現する空間を一定サイズの直方体に区切って物体有無の情報を付加するボクセル形式での出力方法など、多くの出力形式がある。本実施例中では３Ｄ点群の座標値として出力することと仮定するが、本発明としてはこの形式に限定されない。

＜照合用マップ抽出部１７＞
図５に、照合用マップ抽出部１７のブロック図を示す。ここに示すように、照合用マップ抽出部１７は、支柱抽出部１７ａ、低位外形情報削除部１７ｂ、鉛直面抽出部１７ｃから構成される。

支柱抽出部１７ａは、外形情報抽出部１６から出力された外形情報から、速度制限標識や規制標識、注意標識などの標識が設置されている支柱に相当する外形情報や、自動車用や歩行者用、路面電車用などの信号機が設置されている支柱に相当する外形情報を抽出する。なお、標識や信号機は、道路交通上の重要度が高く、積雪時であっても最低限これらが見えるように除雪されていることが期待できる。このため、標識や信号機の支柱の外形情報を照合用マップとして抽出することで、センシング結果照合部１９における成功率が向上することが期待できる。

さらに、低位外形情報削除部１７ｂは、周囲環境観測センサ４で観測された積雪高さ、三次元照合部１３から過去に出力された地図中の視点の高さと車両に搭載時の取付高さの差分として表される積雪高さ、または、一般的な積雪高さの何れかに基づき、路面から一定の高さを除外した残りの外形情報（すなわち、積雪の上に出ていると思われる外形情報）を抽出してもよい。これにより、積雪時に観測されることのない外形情報を除外した照合用マップを抽出することが可能となる。

あるいは、低位外形情報削除部１７ｂは、ガードレールや防音壁など、一定の高さで連続的に存在する遮蔽物の向こう側にある外形情報について、路面から一定の高さを除外した残りの外形情報を抽出してもよい。これにより、遮蔽により観測されることのない外形情報を除外した照合用マップを抽出することが可能となる。

鉛直面抽出部１７ｃは、外形情報抽出部１６が出力した外形情報から、積雪時にも照合が可能な鉛直面に関する情報を抽出する。具体的には、水平に近い物体の面の上には雪が積もることが想定され、地図データベース３に格納されている形状情報と、積雪時の実際の環境での観測結果の形状とは異なる可能性がある。このため、外形情報抽出部１６が出力する外形情報のうち、鉛直方向から所定の角度閾値以内の鉛直に近い面を抽出することで、雪が積もる可能性が少なく、積雪時に見かけの形状が変わりにくい物体を抽出することができる。

以上で説明した支柱抽出部１７ａ等を用いることで、照合用マップ抽出部１７では、積雪時であっても照合可能な、支柱や壁面等の外形情報群からなる照合用マップを抽出する。

＜センシング結果抽出部１８＞
図６に、センシング結果抽出部１８のブロック図を示す。センシング結果抽出部１８は、周囲環境観測センサ４から得られた障害物や環境構造物の形状、模様、色、大きさなどの情報から、照合用マップ抽出部１７で抽出された外形情報に対応する可能性の高い形状情報を抽出する。なお、周囲環境観測センサ４で取得された形状情報は、既に積雪や他構造物に遮蔽されて対象の全ての部分が観測できていない可能性があるため、観測結果の下方を除外する必要はない。

図６に示すように、センシング結果抽出部１８は、支柱抽出部１８ａ、低位外形情報削除部１８ｂ、鉛直面抽出部１８ｃ、積雪領域除去部１８ｄ、移動体除去部１８ｅから構成される。

支柱抽出部１８ａは、周囲環境観測センサ４から出力されたセンシング結果から、速度制限標識や規制標識、注意標識などの標識が設置されている支柱や、自動車用や歩行者用、路面電車用などの信号機が設置されている支柱を抽出する。具体的には、周囲環境観測センサ４において、機械学習などを利用したパターン識別技術を用いることで、観測している対象がどの種別であるかを認識することが可能となる。また、ステレオカメラやＬｉＤＡＲなどの距離情報が認識できるセンサにおいては、距離情報を利用して観測対象物を背景物から抽出することができる。距離情報が取得できない単眼カメラであっても公知のSemantic Segmentation技術などを用いることで、信号機や標識の支柱の抽出は可能である。前述したように、標識と信号機は、道路交通上の重要度が高く、積雪時であっても最低限これらが見えるように除雪されていることが期待できる。このため、標識と信号機、およびそれらの支柱を抽出することで、センシング結果照合部１９における成功率が向上することが期待できる。

低位外形情報削除部１８ｂは、ガードレールや防音壁など、一定の高さで連続的に存在する遮蔽物の向こう側にある外形情報について、支柱上端から一定の高さを除外した残りの外形情報を抽出してもよい。これにより、遮蔽により観測されることのない外形情報を除外した照合用マップを抽出することが可能となる。

鉛直面抽出部１８ｃは、周囲環境観測センサ４から得られたセンシング結果から、積雪によって形状変化を受けにくい鉛直面を抽出する。具体的には、観測結果中の水平に近い面は積雪によって生成された面の可能性があるため、地図データベース３に格納されている形状情報と、観測結果の形状とは異なる可能性がある。このため、周囲環境観測センサ４から得られたセンシング結果のうち、鉛直方向から所定の角度閾値以内の鉛直に近い面を抽出することで、積雪によって生成された面である可能性が少ない物体を抽出することができる。これにより、積雪によって生成された照合対象の地図には含まれていないセンシング結果で誤って照合することを抑止し、センシング結果照合部１９における成功率が向上する効果が期待できる。

積雪領域除去部１８ｄは、周囲環境観測センサ４から得られたセンシング結果から、積雪によって生成された面を除去する。具体的には、公知のSemantic Segmentation技術などを用いて積雪領域からなる観測結果を除去してもよいし、ＬｉＤＡＲにおいてはレーザの反射率が所定の閾値よりも高い場合に積雪から得られた領域であると判定して除外してもよい。これにより、積雪によって生成された照合対象の地図には含まれていない面で誤って照合することを抑止し、センシング結果照合部１９における成功率が向上する効果が期待できる。

移動体除去部１８ｅは、周囲環境観測センサ４から得られたセンシング結果から、走行車両や歩行者などの移動体によって得られた観測情報を除去する。具体的には、複数回センシングをおこない、自車移動量を考慮したセンシング結果の差分を抽出することで、環境中の移動体を抽出して除去してもよいし、Deep Learningをはじめとした機械学習などを利用して車両や歩行者の領域を抽出し、対応する領域をセンシング結果から除去してもよい。これにより、照合対象の地図に含まれていない移動体からの観測結果で誤って照合することを抑止し、センシング結果照合部１９における成功率が向上する効果が期待できる。

なお、自車が水平な路面を走行していると記載したが、自車方位θ_Ｖは車両挙動観測センサ１として搭載されている加速度センサを用いて求めることも可能である。この場合、センシング結果抽出部１８に車両挙動観測センサ１の出力値を入力し、自車方位θ_Ｖを推定した上で、鉛直方向から所定の角度閾値以内の鉛直に近い面を抽出する際の角度計算を補正することができ、抽出の正確性が増す。なお、この車両挙動観測センサ１との接続は図３には図示していない。

＜センシング結果照合部１９＞
センシング結果照合部１９は、照合用マップ抽出部１７の出力と、センシング結果抽出部１８の出力を照合し、地図情報中の自車位置Ｐ_Ｖおよび自車方位θ_Ｖを算出する。この照合技術は公知の技術が複数提案されているため、それらを用いても良い。例えば、前述したＩＣＰという技術を用いてもよい。

センシング結果照合部１９では、形状情報の下部は遮蔽などの影響で、照合用マップ抽出部１７の出力とセンシング結果抽出部１８からの出力の、一方のみで観測されることが起こりうる。このため、地上高の高い部分で重みづけが強くなるように改良したＩＣＰを用いても良い。例えば、具体的には以下のようにすればよい。

２つの三次元点群Ａ，Ｂを照合するときに、まず仮視点を仮定して点群を重ね合わせ、点群Ａのある点ａに対し、点群Ｂ中で最も近い点ｂを算出し、点ａと点ｂの二乗距離Ｄ（ａ，ｂ）を算出する。この操作を点群Ａ中のすべての点に対しておこない、ペアとなった点間の距離を算出する。通常はこの距離をそのまま合計してマッチングの誤差評価値とするが、改良したＩＣＰでは路面からの点ａ，点ｂの高さをそれぞれＨａ，Ｈｂとし、単調増加する変換関数Ｔ（ｘ）とおいて、誤差評価値をΣ｛Ｄ（ａ，ｂ）×Ｔ（Ｈａ×Ｈｂ）｝として計算する。これにより、路面からの高さの高い点のずれが大きい場合には、路面からの高さが低い点のずれが大きい場合よりも、誤差評価値が大きくなる。

あとは通常のＩＣＰと同様に、任意の方向に仮視点を移動させ、同様にペアとなった点間の距離を算出し、誤差評価値を計算する。誤差評価値が小さくなる方向に仮視点を移動させていくことで、最終的に最も点群Ａと点群Ｂが近くなる視点を求めることができる。この視点位置は周囲環境観測センサ４のセンサ原点に対応しているため、周囲環境観測センサ４の搭載位置と車両原点との相対関係を用いて自車位置Ｐ_Ｖに変換する。なお、この相対関係は車両構築時に既知の情報となっている。

センシング結果照合部１９では、前述の方法で求めた自車位置Ｐ_Ｖと、誤差評価値、点群Ａと点群Ｂの各点が所定の距離内で対応付けが得られた点の数を一致面積数として、出力する。

＜位置補正部２０＞
位置補正部２０では、センシング結果照合部１９の出力から照合信頼度を計算し、自己位置を補正する。この照合信頼度は、誤差評価値が小さいほど良好であり、一致面積数が大きいほど良好になるよう設定する。例えば、照合信頼度は次の式２のように定義できる。

（照合信頼度）＝γ÷（誤差評価値）＋δ×（一致面積数） … 式２
照合信頼度に応じた重みをつけて、マップマッチング部１２で出力された自己位置を、センシング結果照合部１９で出力された自車位置Ｐ_Ｖに近づけるように補正して、車両制御部６に出力する自己位置を生成する。

なお、照合信頼度が十分に大きい所定の閾値以上であれば、車両制御部６に出力する自己位置をセンシング結果照合部１９で得られた自車位置Ｐ_Ｖで置き換えて出力してもよい。また、前述では重みづけで補正すると記載したが、カルマンフィルタやパーティクルフィルタを用いて安定化させてもよい。

以上のように、本実施例の自己位置推定装置１０によれば、地図とセンサから得られた情報から、それぞれ所定の時間および領域を抽出し、環境情報が記録された地図と、観測された周囲環境構造とで、照合可能な共通領域を抽出することで、積雪によって走行環境に存在する環境構造物が遮蔽されたり、あるいは環境構造物の形状が変わる状況が継続する場合や、圧雪路で路面標示が見えない状況が継続する場合でも、自己位置の詳細な推定が可能になる。

次に、図７のフローチャートを用いて、本発明の実施例２に係る自己位置推定装置１０を説明する。なお、実施例１との共通点は重複説明を省略する。

実施例１の粗位置推定部１１では、図２のフローチャートのステップＳ２以降の処理から明らかなように、絶対位置取得センサ２の出力が更新されたときに、絶対位置取得センサ２の出力に基づいて自己位置を更新したが、本実施例の粗位置推定部１１では、絶対位置取得センサ２の出力更新のタイミングに依存せずに自己位置を更新することで、処理負荷により処理周期が変動しても精度良く自己位置推定をおこなうことが可能になる。以下、実施例１から変更のあるステップのみを図７を用いて説明する。

例えば、ＧＮＳＳの受信装置が測位衛星電波を用いて測位を行う周期が処理負荷によって変動する場合や、センシング周期と自己位置推定を行う周期が異なる場合においては、各センサが出力した最終出力値をそのまま使うと、センサ間の時刻整合性がとれず、自己位置推定の位置精度が悪化する。そこで、本実施例では、センサ出力を蓄積しておき、自己位置推定をおこなうタイミングで、蓄積されたセンシング結果を利用する。

その場合、ステップＳ１のあとに、以下に述べるステップＳ１１、Ｓ１２を追加することで、自己位置推定をおこなうことが可能になる。

ステップＳ１１：自己位置推定の起動タイミングであるか否かを判定し、起動タイミングでなければ、ステップＳ１に進む。起動タイミングであればステップＳ１２に進む。

ステップＳ１２：ステップＳ１にて時刻を付与されたセンサ出力は過去のタイミングにおける出力値であるため、自己位置推定の計算時の出力値を補間、または過去値から予測して複数センサ間の時刻同期を取る必要がある。例えば、センサ毎にカルマンフィルタを用いたり、線形モデルや多項式モデルを用いたりして当該時刻のセンサ値を予測する。

このように、センサ出力時刻と非同期で自己位置推定を行う場合には、上記ステップを追加することで、精度よく自己位置推定をおこなうことが可能になる。また、処理負荷により処理周期が変動しても精度良く自己位置推定をおこなうことが可能になる。

次に、図８のブロック図を用いて、本発明の実施例３に係る自己位置推定装置１０を説明する。なお、上記の実施例との共通点は重複説明を省略する。

実施例１の積雪対応照合部１５では、積雪判定部５の出力に基づいて、照合用マップ抽出部１７やセンシング結果抽出部１８が所定の情報を抽出したが、本実施例の積雪対応照合部１５では、地図データベース３と周囲環境観測センサ４の照合残差、すなわち過去の地図情報と現在観測された環境情報との差異をフィードバックして活用することで、照合精度を向上し、自己位置推定精度を向上させるものである。

このため、本実施例では積雪判定部５の出力に加えて、周囲環境認識センサ４の出力を用いて、照合用マップ抽出部１７およびセンシング結果抽出部１８の出力を制御する。これにより、積雪状況により適合した照合用マップ抽出およびセンシング結果抽出ができるようになる。

図８において、センシング結果照合部１９は、前述した照合用マップ抽出部１７の出力と、センシング結果抽出部１８からの出力を照合し、地図情報中の自車位置Ｐ_Ｖおよび自車方位θ_Ｖを算出機能のほかに、照合されずに残った点群の情報から積雪状態を積雪状態記録部７へ出力する。

例えば、センシング結果照合部１９では、２つの三次元点群Ａ，Ｂを照合するときに、まず仮視点を仮定して点群を重ね合わせ、点群Ａのある点ａに対し、点群Ｂ中で最も近い点ｂを算出し、点ａと点ｂの二乗距離Ｄ（ａ，ｂ）を算出する。この操作を点群Ａ中のすべての点に対しておこない、ペアとなった点間の距離を算出する。通常はこの距離をそのまま合計してマッチングの誤差評価値とするが、改良したＩＣＰでは路面からの点ａ，点ｂの高さをそれぞれＨａ，Ｈｂとし、単調増加する変換関数Ｔ（ｘ）とおいて、誤差評価値をΣ｛Ｄ（ａ，ｂ）×Ｔ（Ｈａ×Ｈｂ）｝として計算する。これにより、路面からの高さの高い点のずれが大きい場合には、路面からの高さが低い点のずれが大きい場合よりも、誤差評価値が大きくなる。

通常のＩＣＰと同様に、任意の方向に仮視点を移動させ、同様にペアとなった点間の距離を算出し、誤差評価値を計算する。誤差評価値が小さくなる方向に仮視点を移動させていくことで、最終的に最も点群Ａと点群Ｂが近くなる視点を求めることができる。

この際、点ａと点ｂとの二乗距離Ｄ（ａ，ｂ）が所定の距離以内である場合には、点ａと点ｂとが対応付いているものとしてフラグをつけ、二乗距離Ｄ（ａ，ｂ）が所定の距離よりも大きい場合には、点ａと点ｂが対応づかなかったとしてフラグをつけない。こうすることで、最終的にどの点とも対応付けられない点を、フラグがつけられていない点として抽出できる。このどの点とも対応づけられていない点は、前述の変換関数Ｔ（ｘ）により、路面からの高さが低い点に多く抽出されることが期待できる。照合用マップ抽出部１７から出力された点群のうちで、どの点とも対応づけられていない点であり、かつ高さが低い点は、積雪などによりセンシングできない点であると見なせるため、これらの点の高さ情報をもとに積雪高さを推定することができる。

例えば、高さ２．０ｍのポールがあり、照合用マップ抽出部１７により上部から１．８ｍ分が点群として抽出されているとする。また、センシング結果抽出部１７から、当該領域に対応する高さ１．０ｍ分のポール状の点群が出力されているとすると、前述の変換関数Ｔ（ｘ）により、センシング結果の点群はマップから抽出された点群の上部にマッチングすることになる。仮に前述の二乗距離の閾値を０．１ｍとすると、下部の０．７ｍ分の点群がどの点とも対応づけられない点として抽出されることになる。このことから積雪高さが現在のシステムの想定よりも０．７ｍ分高いことが分かる。これを積雪状態記録部７に記録する。

また、センシング結果照合部１９では、積雪状態記録部７に記録されている積雪高さの差異を読み出し、所定の値よりも大きい場合には、照合用マップの点群の高さが低い点を削除することにより、抽出された照合用マップとセンシング結果との誤マッチングを抑制し、安定した自己位置推定をできるようにする。

なお、積雪高さという観点で、積雪状態記録部７への出力を述べたが、これに限定される必要はない。

図９に、積雪のない環境Ｅ１と、積雪後の環境Ｅ２の例を示す。この図を用いて、積雪状態記録部７へ出力する内容の別の例を説明する。

例えば、積雪のない環境Ｅ１において、路側帯と車道との区切りを示す路側帯白線Ｌ１と、車線区分線Ｌ２があるものとする。一般に、積雪時には除雪されるが、積雪量が多い場合には路肩に雪が残り、路側帯白線Ｌ１は雪に隠され、車線区分線２のみが可視状態となる場合がある。

センシング結果照合部１９では、一定時間走行中に、特定の種別の対象が、所定の照合回数以下でしか照合されない場合、その種別の対象がセンシングできない状態の道路であることを積雪状態記録部７に出力する。また、センシング結果照合部１９では、積雪状態記録部７から、照合できない種別の対象については、センシング結果抽出部１８からの出力から削除することで、抽出された照合用マップとセンシング結果との誤マッチングを抑制し、安定した自己位置推定をできるようにする。

なお、積雪状態記録部７に記録された情報は、一定時間後に削除して、当該種別の対象に関して再度照合を試行してもよい。

１ 車両挙動観測センサ
２ 絶対位置取得センサ
３ 地図データベース
４ 周囲環境観測センサ
５ 積雪判定部
６ 車両制御部
７ 積雪状態記録部
１０ 自己位置推定装置
１１ 粗位置推定部
１２ マップマッチング部
１３ 三次元照合部
１４ 汎用照合部
１５ 積雪対応照合部
１６ 外形情報抽出部
１７ 照合用マップ抽出部
１７ａ 支柱抽出部
１７ｂ 低位外形情報削除部
１７ｃ 鉛直面抽出部
１８ センシング結果抽出部
１８ａ 支柱抽出部
１８ｂ 低位外形情報削除部
１８ｃ 鉛直面抽出部
１８ｄ 積雪領域除去部
１８ｅ 移動体除去部
１９ センシング結果照合部
２０ 位置補正部

Claims (4)

1. 地図情報を記憶する地図データベースと、
   周囲環境の三次元点群データを取得する周囲環境観測センサと、に接続され、
   車両の自己位置を推定する自己位置推定装置であって、
   大凡の自己位置を推定する粗位置推定部と、
   詳細な自己位置を推定する三次元照合部と、を有し、
   前記三次元照合部は、
   前記地図情報から積雪時に観測可能な外形情報を抽出した三次元地図データを生成する照合用マップ抽出部と、
   前記周囲環境観測センサから前記三次元点群データを得るセンシング結果抽出部と、
   前記三次元地図データと前記三次元点群データを三次元照合することで、前記粗位置推定部が推定した自己位置を補正する位置補正部と、
   を有し、
   前記照合用マップ抽出部は、前記地図情報に含まれる外形情報から、前記周囲環境観測センサで観測した積雪高さ以上の外形情報を抽出することで、前記三次元地図データを生成することを特徴とする自己位置推定装置。
2. 請求項１に記載の自己位置推定装置において、
   さらに前記三次元照合の結果を記録する積雪状態記録部を有することを特徴とする自己位置推定装置。
3. 計算機が車両の自己位置を推定する自己位置推定方法であって、
   大凡の自己位置を推定する第一ステップと、
   地図情報から積雪時に観測可能な外形情報を抽出した三次元地図データを生成する第二ステップと、
   周囲環境観測センサから三次元点群データを得る第三ステップと、
   前記三次元地図データと前記三次元点群データを三次元照合することで、推定した大凡の自己位置を補正する第四ステップと、
   を有し、
   前記第二ステップでは、前記地図情報に含まれる外形情報から、前記周囲環境観測センサで観測した積雪高さ以上の外形情報を抽出することで、前記三次元地図データを生成することを特徴とする自己位置推定方法。
4. 請求項３に記載の自己位置推定方法において、
   さらに前記三次元照合の結果を記録する第五ステップを有することを特徴とする自己位置推定方法。

Images