特開 2024-103582 情報処理装置、制御方法、プログラム及び記憶媒体

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024103582

(43)【公開日】2024-08-01

(54)【発明の名称】情報処理装置、制御方法、プログラム及び記憶媒体

(51)【国際特許分類】

   G01C 21/30 20060101AFI20240725BHJP

【ＦＩ】

G01C21/30

【審査請求】有

【請求項の数】1

【出願形態】ＯＬ

(21)【出願番号】P 2024086471

(22)【出願日】2024-05-28

(62)【分割の表示】P 2023027424の分割

【原出願日】2020-12-01

(31)【優先権主張番号】P 2019218034

(32)【優先日】2019-12-02

(33)【優先権主張国・地域又は機関】JP

(71)【出願人】

【識別番号】000005016

【氏名又は名称】パイオニア株式会社

(74)【代理人】

【識別番号】100107331

【弁理士】

【氏名又は名称】中村 聡延

(72)【発明者】

【氏名】加藤 正浩

(57)【要約】

【課題】車両の姿勢を好適に推定可能な情報処理装置を提供する。
【解決手段】車載機１の制御部１５は、空間を区切った単位領域（ボクセル）ごとの物体の位置情報であるボクセルデータＶＤから、自車両の近傍に存在する複数のボクセルのボクセルデータＶＤを抽出する。そして、制御部１５は、抽出した複数のボクセルのボクセルデータＶＤに基づき求めた近似平面に対する法線ベクトルを算出する。そして、制御部１５は、自車両の向きと、法線ベクトルとに基づき、自車両のピッチ角又はロール角の少なくとも一方を算出する。
【選択図】図１５

【特許請求の範囲】

【請求項1】

空間を区切った単位領域ごとの物体の位置情報から、移動体の近傍に存在する複数の単位領域での物体の位置情報を抽出する抽出部と、
前記複数の単位領域での物体の位置情報に基づき求めた近似した平面に対する法線ベクトルを算出する法線ベクトル算出部と、
前記移動体の向きと、前記法線ベクトルとに基づき、前記移動体のピッチ角又はロール角の少なくとも一方を算出する角度算出部と、
を有する情報処理装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、車両の姿勢の推定技術に関する。

【背景技術】

【0002】

従来から、レーザスキャナなどの計測装置を用いて計測した周辺物体の形状データを、予め周辺物体の形状が記憶された地図情報と照合（マッチング）することで、車両の自己位置を推定する技術が知られている。例えば、特許文献１には、空間を所定の規則で分割したボクセル中における検出物が静止物か移動物かを判定し、静止物が存在するボクセルを対象として地図情報と計測データとのマッチングを行う自律移動システムが開示されている。また、特許文献２には、ボクセル毎の静止物体の平均ベクトルと共分散行列とを含むボクセルデータとライダが出力する点群データとの照合により自車位置推定を行うスキャンマッチング手法が開示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】国際公開ＷＯ２０１３／０７６８２９

【特許文献2】国際公開ＷＯ２０１８／２２１４５３

【発明の概要】

【発明が解決しようとする課題】

【0004】

一般に、車両は道路面に拘束されており、車両のピッチ角やロール角や上下方向の変化は、サスペンションによる変動はあるものの無視できる程度に小さい。よって、一般的なスキャンマッチングによる車両の自己位置推定では、車両の平面位置及び方位を推定すべきパラメータとして設定している。一方、勾配が大きい坂道や横断勾配を含む道路などの走行時では、車両の平面位置及び方位を推定するだけでは、ピッチ角やロール角の変化に対応できず、マッチングされるべきボクセルと計測データとの対応付けがされなかったり、間違った対応付けが行われてしまったりする。ここで、ピッチ角やロール角の変化に対応するため、ピッチ角やロール角を自己位置推定の推定パラメータとして加えた場合、推定パラメータの増加に起因して計算負荷が増加し、必要な周期での自己位置推定が安定的に終わらなくなるという問題が生じる。また、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）のデータから車両のピッチ角やロール角を求める方法が存在するが、一般的なＩＭＵでは感度誤差やオフセットがあるために、ピッチ角やロール角を正確に計算できないという問題もあった。

【0005】

本発明は、上記のような課題を解決するためになされたものであり、車両の姿勢を好適に推定可能な情報処理装置を提供することを主な目的とする。

【課題を解決するための手段】

【0006】

請求項に記載の発明は、物体の位置情報を有する地図情報から、移動体の近傍に存在する物体の位置情報を複数抽出する抽出部と、前記複数の物体の位置情報に基づき求めた近似した平面に対する法線ベクトルを算出する法線ベクトル算出部と、前記移動体の向きと、前記法線ベクトルとに基づき、前記移動体のピッチ角又はロール角の少なくとも一方を算出する角度算出部と、を有する情報処理装置であることを特徴とする。

【図面の簡単な説明】

【0007】

【図1】運転支援システムの概略構成図である。

【図2】車載機の機能的構成を示すブロック図である。

【図3】状態変数ベクトルを２次元直交座標で表した図である。

【図4】ボクセルデータの概略的なデータ構造の一例を示す。

【図5】ＮＤＴマッチング部の機能ブロックの一例である。

【図6】（Ａ）車両の位置と路面ボクセルとの対応関係を示す俯瞰図である。（Ｂ）路面平均ベクトルを明示した車両の側面図である。

【図7】車両のヨー角と進行方向ベクトルとの関係を示すｘｙ平面図である。

【図8】法線ベクトルと進行方向ベクトルのなす角と車両のピッチ角との関係示す図である。

【図9】車両のヨー角と横方向ベクトルとの関係を示すｘｙ平面図である。

【図10】法線ベクトルと横方向ベクトルのなす角と車両のロール角との関係示す図である。

【図11】（Ａ）平坦な路面を走行する車両及び道路の側面図である。（Ｂ）点群データに車両のピッチ角に基づく座標変換を行わない場合において、勾配が大きい路面を走行する車両及び道路の側面図である。

【図12】点群データに車両のピッチ角に基づく座標変換を行う場合において、勾配が大きい路面を走行する車両及び道路の側面図である。

【図13】車両の位置姿勢推定処理の手順を示すフローチャートの一例である。

【図14】車両高さ推定処理の手順を示すフローチャートの一例である。

【図15】車両のロール角・ピッチ角の推定処理の手順を示すフローチャートの一例である。

【図16】ピッチ角の推定を行い、推定したピッチ角に基づく点群データの座標変換を行った場合に得られた点群データ数、対応付け数、対応付け比率、評価値、及びピッチ角の遷移をそれぞれ示す。

【図17】ピッチ角の推定を行わない場合に得られた点群データ数、対応付け数、対応付け比率、評価値、及びピッチ角をそれぞれ示す。

【発明を実施するための形態】

【0008】

本発明の好適な実施形態によれば、情報処理装置は、空間を区切った単位領域ごとの物体の位置情報から、移動体の近傍に存在する複数の単位領域での物体の位置情報を抽出する抽出部と、前記複数の単位領域での物体の位置情報に基づき求めた近似した平面に対する法線ベクトルを算出する法線ベクトル算出部と、前記移動体の向きと、前記法線ベクトルとに基づき、前記移動体のピッチ角又はロール角の少なくとも一方を算出する角度算出部と、を有する。

【0009】

一般に、移動体は路面上を走行するため、移動体の近傍には路面が存在し、移動体の近傍に存在する複数の単位領域には路面が含まれることが推測される。また、移動体は路面に拘束されており、路面の傾きに応じた移動体のピッチ角及びロール角が生じる。以上を勘案し、この態様では、情報処理装置は、路面の位置情報と推測される複数の単位領域での物体の位置情報に基づき路面を近似した平面の法線ベクトルを算出する。これにより、情報処理装置は、算出した法線ベクトルと移動体の向きとの関係に基づき、移動体のピッチ角又はロール角の少なくとも一方を好適に算出することができる。

【0010】

上記情報処理装置の一態様では、前記角度算出部は、水平面上において前記移動体の進行方向の向きを示すベクトルと、前記法線ベクトルとの内積に基づき、前記ピッチ角を算出する。この態様により、情報処理装置は、移動体の水平面上での進行方向と近似平面の法線ベクトルとのなす角を算出し、移動体のピッチ角を好適に求めることができる。

【0011】

上記情報処理装置の他の一態様では、前記角度算出部は、水平面上において前記移動体の横方向の向きを示すベクトルと、前記法線ベクトルとの内積に基づき、前記ロール角を算出する。この態様により、情報処理装置は、移動体の水平面上での横方向と近似平面の法線ベクトルとのなす角を算出し、移動体のロール角を好適に求めることができる。

【0012】

上記情報処理装置の他の一態様では、情報処理装置は、前記単位領域ごとに、前記物体の位置情報と、前記移動体に設けられた計測部が出力する計測データを前記ピッチ角又はロール角の少なくとも一方に基づき座標変換したデータと、の照合を行うことで、前記移動体の位置推定を行う位置推定部をさらに有する。この態様では、情報処理装置は、物体の位置情報と、計測部が出力する計測データとの照合（マッチング）により移動体の位置推定を行う。このとき、情報処理装置は、算出したピッチ角又はロール角に基づき計測データを座標変換する。これにより、情報処理装置は、坂道や横断勾配が高い道路上での走行により車両のピッチ角又はロール角の変化が生じた場合であっても、被計測対象が実際に存在する単位領域と当該被計測対象の計測データとを的確に対応付けてマッチングを行うことができる。

【0013】

上記情報処理装置の他の一態様では、前記単位領域ごとの物体の位置情報は、前記単位領域ごとの当該物体の位置に関する平均ベクトルの情報を含み、前記法線ベクトル算出部は、前記複数の単位領域での各々の前記平均ベクトルの座標に基づき、前記法線ベクトルを算出する。この態様によれば、情報処理装置は、移動体が存在する路面の位置情報と推測される複数の単位領域での物体の位置情報に基づき、当該路面を近似した平面の法線ベクトルを好適に算出することができる。

【0014】

上記情報処理装置の他の一態様では、前記抽出部は、前記物体の位置情報が存在する単位領域のうち、前記移動体と平面位置が重なる第１単位領域及び当該第１単位領域と隣接する第２単位領域での前記物体の位置情報を抽出する。この態様によれば、情報処理装置は、路面の位置情報と推測される複数の単位領域での物体の位置情報を好適に抽出することができる。

【0015】

上記情報処理装置の他の一態様では、情報処理装置は、前記第１単位領域での前記物体の位置情報と、前記移動体の路面からの高さの情報とに基づき、前記移動体の基準位置からの高さを算出する高さ算出部をさらに備える。基準位置は、地図などで用いられる絶対座標系において基準となる位置であって、例えば標高０ｍの位置を示す。この態様によれば、情報処理装置は、移動体が存在すると推測される第１単位領域での物体（路面）の位置情報を用いて、移動体の基準位置からの高さを好適に算出することができる。好適な例では、前記高さ算出部は、前記第１単位領域での前記物体の位置情報と、前記移動体の路面からの高さの情報とに基づき算出した前記基準位置からの高さと、１時刻前に算出した前記基準位置からの高さとの差が所定値より大きい場合、１時刻前に算出した前記基準位置からの高さを、現時刻における前記基準位置からの高さとして設定するとよい。これにより、立体交差の道路やガードレールなどが近傍に存在する等に起因して、算出した移動体の高さに誤差が生じた場合に、誤差が生じた移動体の高さを現在の移動体の高さとして認識するのを好適に抑制する。

【0016】

上記情報処理装置の他の一態様では、前記物体の位置情報は、道路の路面を含む静止構造物の位置情報であり、前記抽出部は、前記単位領域ごとの物体の位置情報を含む地図データから、前記複数の単位領域での路面の位置情報を抽出する。この態様により、情報処理装置は、移動体が存在する路面の位置情報を地図データから好適に抽出し、ピッチ角又はロール角の算出に必要な近似平面の法線ベクトルを好適に算出することができる。

【0017】

本発明の他の好適な実施形態によれば、情報処理装置が実行する制御方法であって、空間を区切った単位領域ごとの物体の位置情報から、移動体の近傍に存在する複数の単位領域での物体の位置情報を抽出し、前記複数の単位領域での物体の位置情報に基づき求めた近似した平面に対する法線ベクトルを算出し、前記移動体の向きと、前記法線ベクトルとに基づき、前記移動体のピッチ角又はロール角の少なくとも一方を算出する。情報処理装置は、この制御方法を実行することで、移動体近傍の単位領域の位置情報に基づく近似平面の法線ベクトルと移動体の向きとの関係に基づき、移動体のピッチ角又はロール角の少なくとも一方を好適に算出することができる。

【0018】

本発明の他の好適な実施形態によれば、プログラムは、空間を区切った単位領域ごとの物体の位置情報から、移動体の近傍に存在する複数の単位領域での物体の位置情報を抽出する抽出部と、前記複数の単位領域での物体の位置情報に基づき求めた近似した平面に対する法線ベクトルを算出する法線ベクトル算出部と、前記移動体の向きと、前記法線ベクトルとに基づき、前記移動体のピッチ角又はロール角の少なくとも一方を算出する角度算出部としてコンピュータを機能させる。コンピュータは、このプログラムを実行することで、移動体近傍の単位領域の位置情報に基づく近似平面の法線ベクトルと移動体の向きとの関係に基づき、移動体のピッチ角又はロール角の少なくとも一方を好適に算出することができる。好適には、上記プログラムは、記憶媒体に記憶される。

【実施例0019】

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

【0020】

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

【0021】

車載機１は、ライダ２、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５と電気的に接続し、これらの出力に基づき、車載機１が設けられた車両の位置（「自車位置」とも呼ぶ。）の推定を行う。そして、車載機１は、自車位置の推定結果に基づき、設定された目的地への経路に沿って走行するように、車両の自動運転制御などを行う。車載機１は、ボクセルデータ「ＶＤ」を含む地図データベース（ＤＢ：ＤａｔａＢａｓｅ）１０を記憶する。ボクセルデータＶＤは、３次元空間の最小単位となる立方体（正規格子）を示すボクセルごとに静止構造物の位置情報等を記録したデータである。ボクセルデータＶＤは、各ボクセル内の静止構造物の計測された点群データを正規分布により表したデータを含み、後述するように、ＮＤＴ（Ｎｏｒｍａｌ Ｄｉｓｔｒｉｂｕｔｉｏｎｓ Ｔｒａｎｓｆｏｒｍ）を用いたスキャンマッチングに用いられる。また、車載機１は、ＮＤＴスキャンマッチングにより車両の平面上の位置及びヨー角を推定すると共に、ボクセルデータＶＤに基づき，車両の高さ位置と，ピッチ角及びロール角の少なくとも一方の推定を行う。

【0022】

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

【0023】

なお、運転支援システムは、ジャイロセンサ３に代えて、又はこれに加えて、３軸方向における計測車両の加速度及び角速度を計測する慣性計測装置（ＩＭＵ）を有してもよい。

【0024】

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

【0025】

インターフェース１１は、ライダ２、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５などのセンサから出力データを取得し、制御部１５へ供給する。また、インターフェース１１は、制御部１５が生成した車両の走行制御に関する信号を車両の電子制御装置（ＥＣＵ：Ｅｌｅｃｔｒｏｎｉｃ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）へ供給する。

【0026】

記憶部１２は、制御部１５が実行するプログラムや、制御部１５が所定の処理を実行するのに必要な情報を記憶する。本実施例では、記憶部１２は、ボクセルデータＶＤを含む地図ＤＢ１０を記憶する。なお、地図ＤＢ１０は、定期的に更新されてもよい。この場合、例えば、制御部１５は、通信部１３を介し、地図情報を管理するサーバ装置から、自車位置が属するエリアに関する部分地図情報を受信し、地図ＤＢ１０に反映させる。なお、記憶部１２は、地図ＤＢ１０を記憶しなくともよい。この場合、例えば、制御部１５は、通信部１３を介して、ボクセルデータＶＤを含む地図データを記憶するサーバ装置と通信を行うことで、自車位置推定処理等に必要な情報を必要なタイミングにより取得する。

【0027】

入力部１４は、ユーザが操作するためのボタン、タッチパネル、リモートコントローラ、音声入力装置等であり、経路探索のための目的地を指定する入力、自動運転のオン及びオフを指定する入力などを受け付ける。情報出力部１６は、例えば、制御部１５の制御に基づき出力を行うディスプレイやスピーカ等である。

【0028】

制御部１５は、プログラムを実行するＣＰＵなどを含み、車載機１の全体を制御する。本実施例では、制御部１５は、姿勢角算出部１７と、ＮＤＴマッチング部１８とを有する。制御部１５は、本発明における「抽出部」、「法線ベクトル算出部」、「角度算出部」、「位置推定部」、「高さ算出部」及びプログラムを実行する「コンピュータ」の一例である。

【0029】

姿勢角算出部１７は、ボクセルデータＶＤを参照し、車両のピッチ角又はロール角の少なくとも一方を算出する。ＮＤＴマッチング部１８は、ライダ２から出力される点群データと、当該点群データが属するボクセルに対応するボクセルデータＶＤとに基づき、ＮＤＴに基づくスキャンマッチング（ＮＤＴスキャンマッチング）を行うことで、自車位置の推定を行う。本実施例では、ＮＤＴマッチング部１８は、後述するように、ＮＤＴスキャンマッチングにおいて、車両の平面位置（即ち緯度及び経度により特定される水平面上の位置）とヨー角（即ち方位）とを推定パラメータとして設定する。

【0030】

（３）ＮＤＴスキャンマッチングに基づく位置推定
図３は、ＮＤＴマッチング部１８が推定すべき自車位置を２次元直交座標で表した図である。図３に示すように、ｘｙの２次元直交座標上で定義された平面での自車位置は、座標「（ｘ、ｙ）」、自車の方位（ヨー角）「ψ」により表される。ここでは、ヨー角ψは、車の進行方向とｘ軸とのなす角として定義されている。座標（ｘ、ｙ）は、例えば緯度及び経度の組合せに相当する絶対位置、あるいは所定地点を原点とした位置を示すワールド座標である。そして、ＮＤＴマッチング部１８は、これらのｘ、ｙ、ψを推定パラメータとする自車位置推定を行う。なお、車両のピッチ角及びロール角の推定方法については「（４）姿勢角の推定」のセクションで詳しく説明する。

【0031】

次に、ＮＤＴスキャンマッチングに用いるボクセルデータＶＤについて説明する。ボクセルデータＶＤは、各ボクセル内の静止構造物の計測された点群データを正規分布により表したデータを含む。

【0032】

図４は、ボクセルデータＶＤの概略的なデータ構造の一例を示す。ボクセルデータＶＤは、ボクセル内の点群を正規分布で表現する場合のパラメータの情報を含み、本実施例では、図４に示すように、ボクセルＩＤと、ボクセル座標と、平均ベクトルと、共分散行列とを含む。

【0033】

「ボクセル座標」は、各ボクセルの中心位置などの基準となる位置の絶対的な３次元座標を示す。なお、各ボクセルは、空間を格子状に分割した立方体であり、予め形状及び大きさが定められているため、ボクセル座標により各ボクセルの空間を特定することが可能である。ボクセル座標は、ボクセルＩＤとして用いられてもよい。

【0034】

「平均ベクトル」及び「共分散行列」は、対象のボクセル内での点群を正規分布で表現する場合のパラメータに相当する平均ベクトル及び共分散行列を示す。なお、任意のボクセル「ｎ」内の任意の点「ｉ」の座標を
Ｘ_ｎ（ｉ）＝［ｘ_ｎ（ｉ）、ｙ_ｎ（ｉ）、ｚ_ｎ（ｉ）］^Ｔ
と定義し、ボクセルｎ内での点群数を「Ｎ_ｎ」とすると、ボクセルｎでの平均ベクトル「μ_ｎ」及び共分散行列「Ｖ_ｎ」は、それぞれ以下の式（１）及び式（２）により表される。

【0035】

【数1】

【0036】

【数2】

【0037】

次に、ボクセルデータＶＤを用いたＮＤＴスキャンマッチングの概要について説明する。

【0038】

車両を想定したＮＤＴによるスキャンマッチングは、道路平面（ここではｘｙ座標とする）内の移動量及び車両の向きを要素とした推定パラメータ
Ｐ＝［ｔ_ｘ、ｔ_ｙ、ｔ_ψ］^Ｔ
を推定することとなる。ここで、「ｔ_ｘ」は、ｘ方向の移動量を示し、「ｔ_ｙ」は、ｙ方向の移動量を示し、「ｔ_ψ」は、ヨー角を示す。

【0039】

また、ライダ２により得られた点群データに対して、マッチングさせるべきボクセルとの対応付けを行い、対応するボクセルｎでの任意の点の座標を
Ｘ_Ｌ（ｊ）＝［ｘ_ｎ（ｊ）、ｙ_ｎ（ｊ）、ｚ_ｎ（ｊ）］^Ｔ
とすると、ボクセルｎでのＸ_Ｌ（ｊ）の平均値「Ｌ´_ｎ」は、以下の式（３）により表される。

【0040】

【数3】

そして、上述の推定パラメータＰを用い、平均値Ｌ´を座標変換すると、変換後の座標「Ｌ_ｎ」は、以下の式（４）により表される。

【0041】

【数4】

そして、車載機１は、地図ＤＢ１０と同一の座標系である絶対的な座標系（「ワールド座標系」とも呼ぶ。）に変換した点群データと、ボクセルデータＶＤに含まれる平均ベクトルμ_ｎと共分散行列Ｖ_ｎとを用い、ボクセルｎの評価関数値（「個別評価関数値」とも呼ぶ。）「Ｅ_ｎ」を算出する。この場合、車載機１は、以下の式（５）に基づき、ボクセルｎの個別評価関数値Ｅ_ｎを算出する。

【0042】

【数5】

【0043】

そして、車載機１は、以下の式（６）により示される、マッチングの対象となる全てのボクセルを対象とした総合的な評価関数値（「総合評価関数値」とも呼ぶ。）「Ｅ（ｋ）」を算出する。

【0044】

【数6】

その後、車載機１は、ニュートン法などの任意の求根アルゴリズムにより総合評価関数値Ｅ（ｋ）が最大となるとなる推定パラメータＰを算出する。そして、車載機１は、デッドレコニングにより暫定的に算出した予測自車位置「Ｘ^－（ｋ）」に対し、推定パラメータＰを適用することで、以下の式（７）を用いて高精度な推定自車位置「Ｘ^＾（ｋ）」を算出する。

【0045】

【数7】

なお、ここでは、計算対象となる基準時刻（即ち現在時刻）「ｋ」の自車位置を示す状態変数ベクトルを、「Ｘ^－（ｋ）」または「Ｘ^＾（ｋ）」と表記している。

【0046】

図５は、ＮＤＴマッチング部１８の機能ブロックの一例である。図５に示すように、ＮＤＴマッチング部１８は、デッドレコニングブロック２１と、位置予測ブロック２２と、座標変換ブロック２３と、点群データ対応付けブロック２４と、位置補正ブロック２５とを有する。

【0047】

デッドレコニングブロック２１は、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５等の出力に基づく車両の移動速度と角速度を用い、前回時刻からの移動距離と方位変化を求める。位置予測ブロック２２は、直前の計測更新ステップで算出された時刻ｋ－１の推定自車位置Ｘ^＾（ｋ－１）に対し、求めた移動距離と方位変化を加えて、時刻ｋの予測自車位置Ｘ^－（ｋ）を算出する。

【0048】

座標変換ブロック２３は、ライダ２から出力される点群データを、地図ＤＢ１０と同一の座標系であるワールド座標系に変換する。この場合、座標変換ブロック２３は、時刻ｋで位置予測ブロック２２が出力する予測自車位置（即ち車両の平面位置及び方位）と、時刻ｋで姿勢角算出部１７が出力する車両の高さと姿勢角（ここではピッチ角又はロール角の少なくとも一方）とに基づき、時刻ｋでライダ２が出力する点群データの座標変換を行う。この座標変換の詳細については後述する。

【0049】

点群データ対応付けブロック２４は、座標変換ブロック２３が出力するワールド座標系の点群データと、同じワールド座標系で表されたボクセルデータＶＤとを照合することで、点群データとボクセルとの対応付けを行う。位置補正ブロック２５は、点群データと対応付けがなされた各ボクセルを対象として、式（５）に基づく個別評価関数値を算出し、式（６）に基づく総合評価関数値Ｅ（ｋ）が最大となるとなる推定パラメータＰを算出する。そして、位置補正ブロック２５は、式（７）に基づき、位置予測ブロック２２が出力する予測自車位置Ｘ^－（ｋ）に対し、時刻ｋで求めた推定パラメータＰを適用することで、推定自車位置Ｘ^＾（ｋ）を算出する。

【0050】

（４）姿勢角の算出
次に、ボクセルデータＶＤを用いた姿勢角算出部１７による車両の姿勢角であるピッチ角及びロール角の算出方法について説明する。

【0051】

（４－１）ピッチ角の算出
まず、姿勢角算出部１７による車両のピッチ角の算出方法について説明する。

【0052】

姿勢角算出部１７は、ボクセルデータＶＤを参照し、道路の路面に対応するボクセル（「路面ボクセル」とも呼ぶ。）から、ＮＤＴマッチング部１８が予測又は推定した自車平面位置ｘ、ｙ（即ち（ｘ^－、ｙ^－）又は（ｘ^＾、ｙ^＾））が位置する路面ボクセルを探索する。そして、姿勢角算出部１７は、自車平面位置ｘ、ｙが位置する路面ボクセル（「自車位置路面ボクセル」とも呼ぶ。）を含む「ｎ」個の周辺の路面ボクセルに対応するボクセルデータＶＤを取得する。「ｎ」は、３以上の任意の整数である。なお、自車位置路面ボクセルは、本発明における「第１単位領域」の一例であり、自車位置路面ボクセル以外のｎ－１個の周辺の路面ボクセルは、本発明における「第２単位領域」の一例である。

【0053】

図６（Ａ）は、車両の位置と路面ボクセルとの対応関係を示す俯瞰図である。図６（Ａ）では、自車平面位置ｘ、ｙ（ここでは（ｘ^＾、ｙ^＾））が位置する自車位置路面ボクセル「Ｖｏ５」と、自車位置路面ボクセルＶｏ５の前後左右の路面ボクセル「Ｖｏ１」～「Ｖｏ４」、「Ｖｏ６」～「Ｖｏ９」とが示されている。この例では「ｎ＝９」に設定されており、姿勢角算出部１７は、９個の路面ボクセルＶｏ１～Ｖｏ９に対応するボクセルデータＶＤを地図ＤＢ１０から取得する。

【0054】

次に、姿勢角算出部１７は、ｎ個の路面ボクセルの各々に対応するボクセルデータＶＤに含まれるボクセル座標及び平均ベクトルの情報に基づき、ｎ個の路面ボクセルの各々に含まれる路面のワールド座標系における平均ベクトル（「路面平均ベクトル」とも呼ぶ。）を算出する。

【0055】

図６（Ｂ）は、路面ボクセル毎の路面平均ベクトルを明示した車両の側面図である。図６（Ｂ）では、図６（Ａ）に示した路面ボクセルＶｏ４～Ｖｏ６の各々に対応する路面平均ベクトルの座標位置「Ｍ４」～「Ｍ６」が示されている。姿勢角算出部１７は、「ｎ＝９」であることから、９個の路面ボクセルＶｏ１～Ｖｏ９に対応するボクセルデータＶＤに基づき、９個の路面ボクセルＶｏ１～Ｖｏ９に対応する路面平均ベクトルを算出する。

【0056】

次に、姿勢角算出部１７は、路面を平面とみなし、路面を近似した平面の方程式を以下の式（８）により表す。

【0057】

【数8】

そして、姿勢角算出部１７は、算出したｎ個の路面平均ベクトル
（ｘ_１、ｙ_１、ｚ_１）、（ｘ_２、ｙ_２、ｚ_２）、…、（ｘ_ｎ、ｙ_ｎ、ｚ_ｎ）
を式（８）に代入することで、式（９）に示されるｎ個の式からなる連立方程式を得る。

【0058】

【数9】

ここで、式（９）を行列により表現すると、以下の式（１０）が得られる。

【0059】

【数10】

ここで、左辺のｎ×３型行列を「Ｃ」、同左辺の３×１型行列（ベクトル）を「ａ」、右辺のｎ×１型行列（ベクトル）を「ｂ」とすると、式（１０）は、以下の式（１１）により表される。

【0060】

【数11】

そして、式（１１）を変形すると、正規方程式として以下の式（１２）が得られる。

【0061】

【数12】

従って、姿勢角算出部１７は、ｎが３以上の場合には、以下の式（１３）に基づく最小２乗法により、式（８）に示す平面方程式の係数ベクトル「ａ＝［ａ、ｂ、ｃ］^Ｔ」を算出することができる。

【0062】

【数13】

また、式（８）の平面方程式を変形すると、以下の式（１４）が得られる。

【0063】

【数14】

式（１４）から、平面の法線ベクトル「Ｖｎ」は、以下の式（１５）により表される。

【0064】

【数15】

また、姿勢角算出部１７は、ｘｙ平面上での車両の向きベクトル「Ｖｘ」を、ＮＤＴマッチング部１８により予測又は推定されるヨー角に基づき決定する。図７は、車両のヨー角（ここではψ^＾とする）と進行方向ベクトルＶｘとの関係を示すｘｙ平面図である。進行方向ベクトルＶｘのｚ成分は０であり、かつ、ｘ成分及びｙ成分は、夫々、車両のヨー角の余弦及び正弦に比例することから、進行方向ベクトルＶｘは、以下の式（１６）により与えられる。

【0065】

【数16】

この場合、法線ベクトルＶｎと進行方向ベクトルＶｘのなす角「θ’」は、法線ベクトルＶｎと進行方向ベクトルＶｘとの内積計算を含む以下の式（１７）により示される。

【0066】

【数17】

そして、姿勢角算出部１７は、法線ベクトルＶｎと進行方向ベクトルＶｘのなす角θ’に基づき、車両のピッチ角「θ」を算出する。

【0067】

図８は、法線ベクトルＶｎと進行方向ベクトルＶｘのなす角θ’と車両のピッチ角θとの関係示す図である。図８に示すように、法線ベクトルＶｎと進行方向ベクトルＶｘのなす角θ’は、車両のピッチ角θよりも９０度（即ちπ／２）だけ大きい。また、車両は路面に拘束されていることから、車両の進行方向における路面の傾きは、車両のピッチ角θに等しい。よって、姿勢角算出部１７は、以下の式（１８）に基づき、ピッチ角θを算出する。

【0068】

【数18】

このように、姿勢角算出部１７は、路面ボクセルのボクセルデータＶＤに基づくｎ個の路面平均ベクトルから算出した平面方程式の係数ベクトルと、ＮＤＴマッチング部１８が予測又は推定したヨー角とに基づき、好適に車両のピッチ角を算出することができる。

【0069】

（４－２）ロール角の推定
姿勢角算出部１７は、ピッチ角の算出と同様、ボクセルデータＶＤに基づくｎ個の路面平均ベクトルに基づき、式（１５）に示される法線ベクトルＶｎを算出する。

【0070】

また、姿勢角算出部１７は、ｘｙ平面上での車両の横方向ベクトル「Ｖ_Ｙ」を、ＮＤＴマッチング部１８により予測又は推定されるヨー角に基づき決定する。図９は、車両のヨー角（ここではψ^＾とする）と横方向ベクトルＶ_Ｙとの関係を示すｘｙ平面図である。図９に示すように、横方向ベクトルＶ_Ｙは、ＮＤＴマッチング部１８により予測又は推定されるヨー角（ここではψ^＾とする）を、ｘｙ平面に沿って９０度（π／２）だけ回転させた方向に相当する。よって、横方向ベクトルＶ_Ｙは、以下の式（１９）により与えられる。

【0071】

【数19】

この場合、法線ベクトルＶｎと横方向ベクトルＶ_Ｙとのなす角「φ’」は、法線ベクトルＶｎと横方向ベクトルＶ_Ｙとの内積計算を含む以下の式（２０）により示される。

【0072】

【数20】

そして、姿勢角算出部１７は、法線ベクトルＶｎと横方向ベクトルＶ_Ｙのなす角φ’に基づき、車両のロール角「φ」を算出する。

【0073】

図１０は、法線ベクトルＶｎと横方向ベクトルＶ_Ｙのなす角φ’と車両のロール角φとの関係示す図である。図１０に示すように、法線ベクトルＶｎと横方向ベクトルＶ_Ｙのなす角φ’は、車両のロール角φよりも９０度（即ちπ／２）だけ大きい。また、車両は路面に拘束されていることから、車両の横方向における路面の傾きは、車両のロール角φに等しい。よって、姿勢角算出部１７は、以下の式（２１）に基づき、ロール角φを算出する。

【0074】

【数21】

このように、姿勢角算出部１７は、ボクセルデータＶＤに基づくｎ個の路面平均ベクトルから算出した平面方程式の係数ベクトルと、ＮＤＴマッチング部１８が予測又は推定したヨー角とに基づき、好適に車両のロール角を算出することができる。

【0075】

（５）点群データの座標変換
次に、姿勢角算出部１７が算出したピッチ角による点群データの座標変換について説明する。

【0076】

ＮＤＴマッチング部１８の座標変換ブロック２３は、姿勢角算出部１７が算出したピッチ角θを用いて、以下の式（２２）に示す回転行列「Ｒ_θ」を生成する。

【0077】

【数22】

ここで、ライダ２によりｎ個の３次元データを検出した場合、これらのｎ個の３次元データを行列により表した「Ｘ_Ｌ」は、以下の式（２３）により表される。

【0078】

【数23】

この場合、座標変換ブロック２３は、以下の式（２４）により、ピッチ角θに関する点群データの座標変換を行う。

【0079】

【数24】

その後、座標変換ブロック２３は、式（２４）のＸ_Ｌ’の各行に対応するｎ個の３次元データに対し、予測又は推定された３次元の自車位置（ｘ、ｙ、ｚ）の値を加算する。これにより、ｘ、ｙ、ｚの３次元位置についてもワールド座標系に変換された点群データが生成される。そして、座標変換ブロック２３は、ワールド座標系に変換された点群データを、点群データ対応付けブロック２４に供給する。

【0080】

次に、ピッチ角θとロール角φの両方を勘案した座標変換について説明する。座標変換ブロック２３は、姿勢角算出部１７が算出したピッチ角θに基づく回転行列Ｒ_θと、ロール角φに基づく回転行列「Ｒ_φ」とを用いて、ライダ２が出力するｎ個の３次元データを示す行列Ｘ_Ｌに乗じる回転行列「Ｒ」を、以下の式（２５）に基づき作成する。

【0081】

【数25】

この場合、位置予測ブロック２２は、以下の式（２６）により、ピッチ角θ及びロール角φに関する点群データのワールド座標系への座標変換を行う。

【0082】

【数26】

その後、座標変換ブロック２３は、式（２６）のＸ_Ｌ’に対しヨー角ψに基づく回転行列を同様に乗じることで、ヨー角ψに関する点群データのワールド座標系への座標変換を行う。また、座標変換ブロック２３は、座標変換後のＸ_Ｌ’の各行に対応するｎ個の３次元データに対し、予測又は推定した自車位置（ｘ、ｙ、ｚ）の値を加算することで、３次元位置に関するワールド座標系への座標変換を行う。そして、座標変換ブロック２３は、上述の処理によりワールド座標系に変換された点群データを、点群データ対応付けブロック２４に供給する。

【0083】

なお、座標変換ブロック２３は、ライダ２が計測した距離及びスキャン角度の組み合わせに基づくライダ２を基準とした３次元上の位置の各々を示す点群データを、車両座標系に変換する処理をさらに行ってもよい。車両座標系は、車両の進行方向と横方向を軸とした車両の座標系である。この場合、座標変換ブロック２３は、ライダ２の車両に対する設置位置及び設置角度の情報に基づき、ライダ２を基準とした座標系から車両座標系に点群データを変換し、車両座標系に変換した点群データを、さらに上述した方法によりワールド座標系に変換する。車両に設置されたライダが出力する点群データを車両座標系に変換する処理については、例えば、国際公開ＷＯ２０１９／１８８７４５などに開示されている。

【0084】

また、座標変換ブロック２３は、ピッチ角θとロール角φのうちロール角φのみを勘案した座標変換を行ってもよい。この場合、座標変換ブロック２３は、式（２５）に示される回転行列Ｒ_φを、ライダ２が出力するｎ個の３次元データを示す行列Ｘ_Ｌに乗じればよい。

【0085】

次に、上述の座標変換による効果について、図１１及び図１２を参照して補足説明する。

【0086】

図１１（Ａ）は、平坦な路面を走行する車両及び道路の側面図である。この場合、ワールド座標系の基準位置（例えば標高が０となる位置）からの車両高さ「ｚ^＾」は、ワールド座標系の基準位置から路面までの高さに、路面からの車両の高さ（「車両基準位置ｚ_０」とも呼ぶ。）を加算した高さとなる。そして、ライダ２が計測したある計測点「Ｐｔａｇ１」の車両からの高さ「ｄ」を車両高さｚ^＾に加算した「ｚ^＾＋ｄ」は、ワールド座標系における計測点Ｐｔａｇ１の実際の高さと一致する。よって、この場合、点群データ対応付けブロック２４が実行する対応付け処理では、対象の計測点Ｐｔａｇ１は、計測点Ｐｔａｇ１が実際に含まれるボクセル「Ｖ１」と対応付けが行われる。

【0087】

図１１（Ｂ）は、点群データに車両のピッチ角に基づく座標変換を行わない場合において、勾配が大きい路面を走行する車両及び道路の側面図である。この場合、路面勾配により車両にピッチ角が生じるため、点群データが上向きになる。よって、車両から高さｄの位置に存在する計測点「Ｐｔａｇ２」のワールド座標系での高さは、高さｄを車両高さｚ^＾に加算した「ｚ^＾＋ｄ」とはならない。その結果、点群データ対応付けブロック２４が実行する対応付け処理では、計測点Ｐｔａｇ２は、ワールド座標系での高さが「ｚ^＾＋ｄ」と算出されるため、実際に計測点Ｐｔａｇ２が存在するボクセル「Ｖ２」と対応付けが行われず、ボクセル「Ｖ２α」と誤った対応付けが行われる。

【0088】

図１２は、本実施例に基づき点群データに車両のピッチ角に基づく座標変換を行う場合に、勾配が大きい路面を走行する車両及び道路の側面図である。この場合、実施例に基づく算出手法により求めたピッチ角を用いた回転行列により、ライダ２が出力する点群データは、車両のピッチ角を考慮したワールド座標系への変換が行われる。従って、この場合、車両から高さｄの位置に存在する計測点「Ｐｔａｇ３」のワールド座標系での高さは、「ｚ^＾＋ｄ」となる。よって、点群データ対応付けブロック２４が実行する対応付け処理では、計測点Ｐｔａｇ３は、実際に計測点Ｐｔａｇ３が存在するボクセル「Ｖ３」（即ち正しいマッチング対象）と対応付けが行われることになり、ＮＤＴスキャンマッチングが好適に実行される。

【0089】

バンクなどの横断勾配が存在する道路を走行する場合においても、車載機１は、点群データに車両のロール角に基づく座標変換を行うことで、ライダ２が出力する点群データの各計測点を、正しいマッチング対象となるボクセルと適切に対応付けることができる。

【0090】

（６）処理フロー
次に、上述したピッチ角及びロール角の推定を含むＮＤＴマッチングによる車両の位置姿勢推定処理の具体的な処理フローについて、フローチャートを参照して説明する。

【0091】

（６－１）車両位置姿勢推定処理の概要
図１３は、車両の位置姿勢推定処理の手順を示すフローチャートの一例である。車載機１の姿勢角算出部１７及びＮＤＴマッチング部１８は、図１３に示すフローチャートの処理を、車両の位置及び姿勢の推定を行うべき所定の時間間隔毎に繰り返し実行する。なお、図１３の各ステップの右に表示されている記号は、各ステップで算出される要素を表す。

【0092】

まず、ＮＤＴマッチング部１８のデッドレコニングブロック２１は、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５等の出力に基づく車両の移動速度と角速度を用いて、前回時刻からの移動距離と方位変化を求める。これにより、ＮＤＴマッチング部１８の位置予測ブロック２２は、１時刻前（直前の処理時刻）に得られた推定自車位置ｘ^＾、ｙ^＾、ψ^＾から、現時刻の予測自車位置ｘ^－、ｙ^－、ψ^－を算出する（ステップＳ１１）。

【0093】

次に、姿勢角算出部１７は、車両高さ（例えば車両が位置する標高）の推定処理を行うことで、予測車両高さ「ｚ^－」を算出する（ステップＳ１２）。この処理については、図１４を参照して後述する。さらに、姿勢角算出部１７は、車両のロール角・ピッチ角の推定処理を行うことで、予測ロール角「φ^－」及び予測ピッチ角「θ^－」を算出する（ステップＳ１３）。この処理については、図１５を参照して後述する。

【0094】

そして、ＮＤＴマッチング部１８の座標変換ブロック２３は、ステップＳ１３で算出されたロール角・ピッチ角に基づく回転行列Ｒ（式（２５）参照）を生成する（ステップＳ１４）。そして、座標変換ブロック２３は、点群データをワールド座標系のデータに変換する（ステップＳ１５）。その後、ＮＤＴマッチング部１８（点群データ対応付けブロック２４及び位置補正ブロック２５）は、座標変換後の点群データとボクセルデータＶＤとを用いてＮＤＴマッチングを行い、現時刻での推定自車位置ｘ^＾、ｙ^＾、ψ^＾を算出する（ステップＳ１６）。その後、姿勢角算出部１７は、算出された現時刻での推定自車位置ｘ^＾、ｙ^＾、ψ^＾を用いて、再びステップＳ１２と同様の車両の高さ推定処理を行うことで、推定車両高さ「ｚ^＾」を算出する（ステップＳ１７）。この処理については、図１４を参照して後述する。さらに、姿勢角算出部１７は、算出された現時刻での推定自車位置ｘ^＾、ｙ^＾、ψ^＾と推定車両高さｚ^＾とを用いて、車両のロール角・ピッチ角の推定処理を行うことで、推定ロール角「φ^＾」及び推定ピッチ角「θ^＾」を算出する（ステップＳ１８）。この処理については、図１５を参照して後述する。

【0095】

（６－２）車両高さ推定処理
図１４は、図１３のステップＳ１２及びステップＳ１７で実行する車両高さ推定処理の手順を示すフローチャートの一例である。車両高さ推定処理では、姿勢角算出部１７は、予測又は推定された車両の平面位置に基づき自車位置路面ボクセルを決定すると共に、自車位置路面ボクセルの平均ベクトルのｚ座標に車両基準位置ｚ_０（車両の路面からの高さ）を加算した値を、求めるべき車両高さとして決定する。また、車載機１は、自車位置路面ボクセルとして信頼性が低い自車位置路面ボクセルのボクセルインデックスｉｄｚを「－１」に設定し、後述するロール角・ピッチ角の推定処理で用いないようにラベル付けを行う。

【0096】

まず、姿勢角算出部１７は、予測自車位置ｘ^－、ｙ^－又は推定自車位置ｘ^＾、ｙ^＾が含まれるボクセルのボクセルインデックス（ｉｄｘ、ｉｄｙ）を取得し、ボクセルインデックス（ｉｄｘ、ｉｄｙ）を有するボクセルを探索する（ステップＳ２１）。即ち、姿勢角算出部１７は、ボクセルデータＶＤを参照し、予測自車位置ｘ^－、ｙ^－又は推定自車位置ｘ^＾、ｙ^＾とｘｙ平面で同一位置となるボクセルを、高さ方向に探索する。なお、姿勢角算出部１７は、図１３のステップＳ１２の場合には、予測自車位置ｘ^－、ｙ^－が含まれるボクセルのボクセルインデックス（ｉｄｘ、ｉｄｙ）を有するボクセルを探索する。また、姿勢角算出部１７は、図１３のステップＳ１７の場合には、推定自車位置ｘ^＾、ｙ^＾が含まれるボクセルのボクセルインデックス（ｉｄｘ、ｉｄｙ）を有するボクセルを探索する。

【0097】

次に、姿勢角算出部１７は、ステップＳ２１において、１つ以上のボクセルが検出されたか否か判定する（ステップＳ２２）。そして、姿勢角算出部１７は、ボクセルが検出されなかった場合（ステップＳ２２；Ｎｏ）、１時刻前の予測車両高さｚ^－又は推定車両高さｚ^＾を、求めるべき予測車両高さｚ^－又は推定車両高さｚ^＾として定める（ステップＳ２７）。さらに、ステップＳ２７では、姿勢角算出部１７は、ボクセルインデックスｉｄｚが「－１」となる仮の自車位置路面ボクセルを定める。

【0098】

一方、ステップＳ２１において、１つ以上のボクセルが検出された場合（ステップＳ２２；Ｙｅｓ）、姿勢角算出部１７は、検出したボクセルを自車位置路面ボクセルの候補とし、各候補のｚ座標（当該候補の平均ベクトルのｚ座標）をボクセルデータＶＤから読み取り、当該各ｚ座標に車両基準位置ｚ_０を加算する（ステップＳ２３）。車両基準位置ｚ_０は、例えば記憶部１２に予め記憶されている。

【0099】

そして、姿勢角算出部１７は、各候補の平均ベクトルのｚ座標に車両基準位置ｚ_０を加算した値が、１時刻前の予測車両高さｚ^－又は推定車両高さｚ^＾に最も近い値となる自車位置路面ボクセルの候補を、自車位置路面ボクセルとして選択する（ステップＳ２４）。

【0100】

そして、姿勢角算出部１７は、選択した自車位置路面ボクセルの平均ベクトルのｚ座標に車両基準位置ｚ_０を加算した値と、１時刻前の予測車両高さｚ^－又は推定車両高さｚ^＾との差が所定値以下の場合（ステップＳ２５；Ｙｅｓ）、ステップＳ２６の処理を実行する。この場合、所定値は、例えば、１時刻前から現時刻までの時間間隔内において車両が坂道などで生じ得る高さの変動幅の上限に設定される。

【0101】

そして、姿勢角算出部１７は、自車位置路面ボクセルの平均ベクトルのｚ座標に車両基準位置ｚ_０を加算した値を、求めるべき予測車両高さｚ^－又は推定車両高さｚ^＾として定める（ステップＳ２６）。一方、姿勢角算出部１７は、上述の差が所定値より大きい場合（ステップＳ２５；Ｎｏ）、１時刻前の予測車両高さｚ^－又は推定車両高さｚ^＾を、求めるべき予測車両高さｚ^－又は推定車両高さｚ^＾として定める（ステップＳ２７）。さらに、ステップＳ２７では、姿勢角算出部１７は、ステップＳ２４で選択した自車位置路面ボクセルのボクセルインデックスｉｄｚを「－１」に設定する。

【0102】

（６－３）ロール角・ピッチ角推定処理
図１５は、図１３のステップＳ１３及びステップＳ１８で実行する車両のロール角・ピッチ角の推定処理の手順を示すフローチャートの一例である。なお、図１５では、車載機１は、一例として、車両のロール角及びピッチ角の両方を推定しているが、車両のロール角及びピッチ角のいずれか一方のみを推定する処理を行ってもよい。

【0103】

まず、姿勢角算出部１７は、直前に実行したステップＳ１２またはステップＳ１７の車両高さ推定処理で決定した自車位置路面ボクセルのボクセルインデックス（ｉｄｘ，ｉｄｙ，ｉｄｚ）を参照する（ステップＳ３１）。そして、姿勢角算出部１７は、自車位置路面ボクセルのボクセルインデックスｉｄｚが「－１」に設定されているか否か判定する（ステップＳ３２）。そして、姿勢角算出部１７は、自車位置路面ボクセルのボクセルインデックスｉｄｚが「－１」の場合（ステップＳ３２；Ｙｅｓ）、対象の自車位置路面ボクセルの信頼性が低い又は自車位置路面ボクセルが検出できなかったと判定する。よって、この場合、姿勢角算出部１７は、１時刻前に算出したロール角φ（φ^－若しくはφ^＾）又はピッチ角θ（θ^－若しくはθ^＾）を、求めるべき現時刻でのロール角φ（φ^－若しくはφ^＾）又はピッチ角θ（θ^－若しくはθ^＾）として定める（ステップＳ３８）。

【0104】

一方、姿勢角算出部１７は、自車位置路面ボクセルのボクセルインデックスｉｄｚが「－１」ではない場合（ステップＳ３２；Ｎｏ）、自車位置路面ボクセルを含む自車位置路面ボクセルを中心とした周辺のｎ個のボクセルのボクセルデータＶＤを取得する（ステップＳ３３）。例えば、姿勢角算出部１７は、自車位置路面ボクセルとボクセルインデックスｉｄｘ，ｉｄｙのうち少なくともいずれか１異なり、かつ、ボクセルインデックスｉｄｚが同一又は１異なるボクセルを、自車位置路面ボクセルと共に、上述のｎ個のボクセルとみなす。

【0105】

そして、姿勢角算出部１７は、上述のｎ個のボクセルに対し、それぞれのボクセルデータＶＤに含まれる平均ベクトルのｘ、ｙ、ｚ座標を読み取ることで、式（１０）に従い行列Ｃとベクトルｂを作成する（ステップＳ３４）。次に、姿勢角算出部１７は、式（１３）に基づき、最小２乗法により係数ベクトルａを算出し、式（１５）に示される法線ベクトルＶｎを特定する（ステップＳ３５）。そして、姿勢角算出部１７は、予測又は推定された車両のヨー角（方位）ψ（ψ^－若しくはψ^＾）を用いて、式（１６）に示される進行方向ベクトルＶｘ及び式（１９）により示される横方向ベクトルＶ_Ｙを算出する（ステップＳ３６）。その後、姿勢角算出部１７は、式（１７）に基づき法線ベクトルＶｎと進行方向ベクトルＶｘの内積を算出すると共に、式（２０）に基づき法線ベクトルＶｎと横方向ベクトルＶ_Ｙの内積を算出し、求めるべき現時刻でのロール角φ（φ^－若しくはφ^＾）又はピッチ角θ（θ^－若しくはθ^＾）を算出する（ステップＳ３７）。

【0106】

（７）実験例
出願人は、図１に示す構成と同様にライダを搭載した車両を、傾斜の大きい坂を含む道路上で走行させ、ＮＤＴスキャンマッチングに基づく自車位置推定を行い、必要なデータを収集した。なお、車両に搭載したライダは、２５ｍを最大測距距離とし、測距を行う水平角度を１２０度前後とした。

【0107】

図１６（Ａ）～（Ｅ）は、本実施例に基づきピッチ角の推定を行い、推定したピッチ角に基づく点群データの座標変換を行った場合に上述の道路上で得られた点群データ数、対応付け数、対応付け比率、評価値、及びピッチ角の遷移をそれぞれ示す。また、図１７（Ａ）～（Ｅ）は、ピッチ角の推定を行わない場合に上述の道路上で得られた点群データ数、対応付け数、対応付け比率、評価値、及びピッチ角をそれぞれ示す。

【0108】

ここで、「点群データ数」は、１つの処理時刻毎にライダから得られる点群データの計測点の数を示し、「対応付け数」は、点群データ数のうちボクセルとの対応付けができた計測点の数を示す。また、「対応付け比率」は、点群データ数に対する対応付け数の比率を示し、「評価値」は、ＮＤＴスキャンマッチングにおいて定めた推定パラメータに対応する総合評価関数値を示す。「ピッチ角」は、車両が推定した車両のピッチ角を示す。なお、図１７（Ａ）～（Ｅ）の場合にはピッチ角の推定を行っていないため、図１７（Ｅ）に示すピッチ角は、常時０度となっている。また、図１６（Ｂ）～（Ｅ）における破線矢印５８の期間及び図１７（Ｂ）～（Ｅ）における破線矢印５９の期間は、周辺にボクセルデータＶＤが存在しない道路を車両が走行時の期間であるため、ＮＤＴスキャンマッチングが実施されなかった箇所である。

【0109】

図１６（Ｅ）において、破線枠６０及び破線枠６１により示される期間では、ピッチ角の絶対値が一時的に大きくなっており、車両が坂道を走行している期間に対応する。このような坂道の走行期間においても、図１６（Ｃ）に示されるように、対応付け比率は、ピッチ角を考慮した点群データの座標変換により、対応付け数の低下が抑制され、他の平坦な道路の走行期間と同等の対応付け比率となっている。

【0110】

一方、ピッチ角の推定及びピッチ角に基づく点群データの座標変換を行わない場合、上述の坂道の走行期間に対応する期間に対応する対応付け比率は、図１７（Ｃ）の破線枠７０及び破線枠７１に示されるように、一時的に顕著に低下している。なお、対応付け数及び対応付け比率の低下が生じている場合には、ＮＤＴスキャンマッチングのロバスト性が低下し、推定される自車位置の信頼性が低いことが推測される。

【0111】

このように、本実施例によれば、ピッチ角の推定及びピッチ角に基づく点群データの座標変換を行うことで、坂道においても点群データの対応付け数及び対応付け比率の低下を好適に抑制し、ＮＤＴスキャンマッチングによる自車位置推定のロバスト性を好適に向上させることができる。同様に、ロール角の推定及びロール角に基づく点群データの座標変換を行うことで、横断勾配が高い道路においても点群データの対応付け数及び対応付け比率の低下を好適に抑制し、ＮＤＴスキャンマッチングによる自車位置推定のロバスト性を好適に向上させることができる。

【0112】

以上説明したように、本実施例に係る車載機１の制御部１５は、空間を区切った単位領域（ボクセル）ごとの物体の位置情報であるボクセルデータＶＤから、自車両の近傍に存在する複数のボクセルのボクセルデータＶＤを抽出する。そして、制御部１５は、抽出した複数のボクセルのボクセルデータＶＤに基づき求めた近似平面に対する法線ベクトルを算出する。そして、制御部１５は、自車両の向きと、法線ベクトルとに基づき、自車両のピッチ角又はロール角の少なくとも一方を算出する。この態様により、車載機１は、ボクセルデータＶＤに基づき、ピッチ角又はロール角の少なくとも一方を高精度に算出することができる。

【0113】

（８）変形例
以下、上述の実施例に好適な変形例について説明する。以下の変形例は、組み合わせてこれらの実施例に適用してもよい。

【0114】

（変形例１）
図１３のフローチャート等では、車載機１は、図１４に示す車両の高さ推定処理を行うことで、車両の高さ（予測車両高さｚ^－又は推定車両高さｚ^＾）を算出した。しかし、車両の高さの算出方法は、これに限定されない。

【0115】

例えば、車載機１は、ＮＤＴスキャンマッチングに基づく自車位置推定において、車両の高さをさらに推定パラメータとして加えてもよい。この場合、車載機１は、図３に示す座標（ｘ、ｙ）及びヨー角ψに加えて、ｘ軸及びｙ軸に垂直なｚ軸の座標を勘案した４変数（ｘ、ｙ、ｚ、ψ）を自車位置の状態変数とした自車位置推定を行う。この態様によっても、車載機１は、車両の高さを好適に推定することができる。

【0116】

（変形例２）
車載機１は、ＮＤＴスキャンマッチングを行わない場合においても、実施例に基づく車両のピッチ角又はロール角の少なくともいずれかの推定を行ってもよい。

【0117】

この場合、例えば、車載機１は、図１３において、ステップＳ１１～ステップＳ１３を繰り返し実行することで、ボクセルデータＶＤに基づき車両のピッチ角又はロール角の少なくともいずれかの推定を繰り返し行う。この場合であっても、車載機１は、推定したピッチ角又は／及びロール角を、ライダ２等の外界センサの出力データの座標変換又は坂道（バンク）検出処理などの種々の応用に使用することが可能である。

【0118】

（変形例３）
図１に示す運転支援システムの構成は一例であり、本発明が適用可能な運転支援システムの構成は図１に示す構成に限定されない。例えば、運転支援システムは、車載機１を有する代わりに、車両の電子制御装置が車載機１の姿勢角算出部１７及びＮＤＴマッチング部１８の処理を実行してもよい。この場合、地図ＤＢ１０は、例えば車両内の記憶部又は車両とデータ通信を行うサーバ装置に記憶され、車両の電子制御装置は、この地図ＤＢ１０を参照することで、ロール角又は／及びピッチ角の推定及びＮＤＴスキャンマッチングに基づく自車位置推定などを実行する。

【0119】

（変形例４）
ボクセルデータＶＤは、図３に示すように、平均ベクトルと共分散行列とを含むデータ構造に限定されない。例えば、ボクセルデータＶＤは、平均ベクトルと共分散行列を算出する際に用いられる計測整備車両が計測した点群データをそのまま含んでいてもよい。この場合、車載機１は、例えば、ボクセルデータＶＤを参照し、ボクセルごとに平均ベクトルを算出することで、式（１０）に示される行列Ｃを生成する。

【0120】

また、本実施例は、ＮＤＴによるスキャンマッチングに限定されず、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）などの他のスキャンマッチングを適用してもよい。この場合であっても、実施例と同様、車載機１は、実施例に基づく姿勢角（ピッチ角又は／及びロール角）の算出を行うと共に、任意のスキャンマッチングにより平面位置及び方位に関する自車位置推定を行う。

【0121】

以上、実施形態を参照して本願発明を説明したが、本願発明は上記実施形態に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。すなわち、本願発明は、請求の範囲を含む全開示、技術的思想にしたがって当業者であればなし得るであろう各種変形、修正を含むことは勿論である。また、引用した上記の特許文献等の各開示は、本書に引用をもって繰り込むものとする。

【符号の説明】

【0122】

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

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】