JP2023054314A - 情報処理装置、制御方法、プログラム及び記憶媒体 - Google Patents

Abstract

【課題】車両の姿勢を好適に推定可能な情報処理装置を提供する。【解決手段】車載機１の制御部１５は、地物の位置情報を含む地図データであるランドマークデータＬＤから、車両の近傍に存在する、路面上に描かれた地物の複数の位置情報を取得する。そして、制御部１５は、取得した複数の位置情報に基づき求めた近似平面に対する法線ベクトルを算出する。そして、制御部１５は、自車両の向きと、法線ベクトルとに基づき、自車両のピッチ角又はロール角の少なくとも一方を算出する。【選択図】図１５

Description

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

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

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

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

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

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

運転支援システムの概略構成図である。 車載機の機能的構成を示すブロック図である。 地図ＤＢ（ＤａｔａＢａｓｅ）の概略的なデータ構造の一例を示す。 状態変数ベクトルを２次元直交座標で表した図である。 予測ステップと計測更新ステップとの概略的な関係を示す図である。 自車位置推定部の機能ブロックの一例である。 （Ａ）区画線の位置情報を明示した道路上の車両の俯瞰図である。（Ｂ）区画線の位置情報を明示した道路上の車両の側面図である。 車両のヨー角と進行方向ベクトルとの関係を示すｘｙ平面図である。 法線ベクトルと進行方向ベクトルのなす角と車両のピッチ角との関係を示す図である。 車両のヨー角と横方向ベクトルとの関係を示すｘｙ平面図である。 法線ベクトルと横方向ベクトルのなす角と車両のロール角との関係を示す図である。 （Ａ）平坦な路面を走行する車両及び道路の側面図である。（Ｂ）点群データに車両のピッチ角に基づく座標変換を行わない場合において、勾配が大きい路面を走行する車両及び道路の側面図である。 点群データに車両のピッチ角に基づく座標変換を行う場合において、勾配が大きい路面を走行する車両及び道路の側面図である。 車両の位置姿勢推定処理の手順を示すフローチャートの一例である。 車両のロール角・ピッチ角の推定処理の手順を示すフローチャートの一例である。

本発明の好適な実施形態によれば、情報処理装置は、地物の位置情報を含む地図データから、移動体の近傍に存在する、路面上に描かれた地物の複数の位置情報を取得する取得部と、前記複数の位置情報に基づき求めた近似した平面に対する法線ベクトルを算出する法線ベクトル算出部と、前記移動体の車両の向きと、前記法線ベクトルとに基づき、前記移動体のピッチ角又はロール角の少なくとも一方を算出する角度算出部と、を有する。

一般に、移動体は路面に拘束されており、路面の傾きに応じた移動体のピッチ角及びロール角が生じる。従って、この態様では、情報処理装置は、路面上に描かれた地物の位置情報に基づき路面を近似した平面の法線ベクトルを算出する。これにより、情報処理装置は、算出した法線ベクトルと移動体の向きとの関係に基づき、移動体のピッチ角又はロール角の少なくとも一方を好適に算出することができる。

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

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

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

上記情報処理装置の他の一態様では、前記地図データには、複数の地物の位置情報が登録されており、前記位置推定部は、前記移動体の周辺の地物が前記複数の地物に含まれる場合、当該周辺の地物の前記座標変換したデータが含まれると予測される予測範囲を前記周辺の地物の位置情報に基づき設定し、当該予測範囲に含まれる前記データと、前記周辺の地物の位置情報との照合を行うことで、前記移動体の位置推定を行う。情報処理装置は、算出したピッチ角又はロール角に基づき計測データを座標変換することで、ピッチ角又はロール角が生じる路面上を車両が走行する場合でも、計測対象の地物の計測データを、当該地物の位置情報に基づき設定した予測範囲内に含めることができる。よって、情報処理装置は、坂道や横断勾配が高い道路上での走行により車両のピッチ角又はロール角の変化が生じた場合であっても、計測部により得られる地物の計測データを用いて好適に移動体の位置推定を行うことができる。

上記情報処理装置の他の一態様では、情報処理装置は、前記近似した平面の方程式に基づき算出される当該平面の基準位置からの高さと、前記移動体の路面からの高さの情報とに基づき、前記移動体の基準位置からの高さを算出する高さ算出部をさらに備える。基準位置は、地図などで用いられる絶対座標系において基準となる位置であって、例えば標高０ｍの位置を示す。この態様によれば、情報処理装置は、ピッチ角又はロール角の算出に用いる平面の方程式に基づき、移動体の基準位置からの高さを好適に算出することができる。

本発明の他の好適な実施形態によれば、情報処理装置が実行する制御方法であって、地物の位置情報を含む地図データから、移動体の近傍に存在する、路面上に描かれた地物の複数の位置情報を取得し、前記複数の位置情報に基づき求めた近似した平面に対する法線ベクトルを算出し、前記移動体の車両の向きと、前記法線ベクトルとに基づき、前記移動体のピッチ角又はロール角の少なくとも一方を算出する。情報処理装置は、この制御方法を実行することで、路面上に描かれた地物の複数の位置情報に基づく近似平面の法線ベクトルと移動体の向きとの関係に基づき、移動体のピッチ角又はロール角の少なくとも一方を好適に算出することができる。

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

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

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

車載機１は、ライダ２、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５と電気的に接続し、これらの出力に基づき、車載機１が設けられた車両の位置（「自車位置」とも呼ぶ。）の推定を行う。そして、車載機１は、自車位置の推定結果に基づき、設定された目的地への経路に沿って走行するように、車両の自動運転制御などを行う。車載機１は、道路付近に設けられた目印となる地物や区画線などのランドマークに関する情報（「ランドマークデータＬＤ」と呼ぶ。）及び道路データ等を含む地図データベース（ＤＢ：ＤａｔａＢａｓｅ）１０を記憶する。そして、車載機１は、このランドマークデータＬＤが示すランドマークの位置とライダ２の計測結果とを照合させて自車位置の推定を行う。

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

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

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

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

記憶部１２は、制御部１５が実行するプログラムや、制御部１５が所定の処理を実行するのに必要な情報を記憶する。本実施例では、記憶部１２は、ランドマークデータＬＤを含む地図ＤＢ１０を記憶する。図３は、地図ＤＢ１０のデータ構造の一例を示す。図３に示すように、地図ＤＢ１０は、施設情報、道路データ、及びランドマークデータＬＤを含む。

ランドマークデータＬＤは、ランドマークとなる地物ごとに当該地物に関する情報が関連付けられた情報であり、ここでは、ランドマークのインデックスに相当するランドマークＩＤと、位置情報と、種別情報と、サイズ情報とを含む。位置情報は、緯度及び経度（及び標高）等により表わされたランドマークの絶対的な位置を示す。なお、ランドマークが区画線（路面上に描かれた地物）の場合、対応する位置情報として、区画線の離散的な位置を示す座標データが少なくとも含まれている。種別情報は、ランドマークの種別を示す情報であり、少なくとも対象のランドマークが区画線であるか否かを識別可能な情報である。サイズ情報は、ランドマークの大きさを表す情報であり、例えば、ランドマークの縦又は／及び横の長さ（幅）を示す情報であってもよく、ランドマークに形成された面の面積を示す情報であってもよい。

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

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

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

姿勢角算出部１７は、ランドマークデータＬＤを参照し、車両のピッチ角又はロール角の少なくとも一方を算出する。自車位置推定部１８は、インターフェース１１から供給される各センサの出力信号及び地図ＤＢ１０に基づき、車載機１が載置された車両の位置（自車位置）の推定を行う。本実施例では、自車位置推定部１８は、後述するように、車両の平面位置（即ち緯度及び経度により特定される水平面上の位置）とヨー角（即ち方位）とを推定パラメータとして設定する。

（３）自車位置推定の概要
まず、自車位置推定部１８による自車位置の推定処理の概要について説明する。

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

本実施例では、一例として、自車位置推定部１８は、拡張カルマンフィルタを用いた自車位置推定を行う。以下では、拡張カルマンフィルタを用いた自車位置推定について簡略的に説明する。

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

図５は、予測ステップと計測更新ステップとの概略的な関係を示す図である。図６は、自車位置推定部１８の機能ブロックの一例を示す。図６に示すように、自車位置推定部１８は、デッドレコニングブロック２１と、位置予測ブロック２２と、座標変換ブロック２３と、ランドマーク探索・抽出ブロック２４と、位置補正ブロック２５とを有する。

図５に示すように、自車位置推定部１８は、予測ステップと計測更新ステップとを繰り返すことで、自車位置を示す状態変数ベクトル「Ｘ」の推定値の算出及び更新を逐次的に実行する。なお、図５では、計算対象となる基準時刻（即ち現在時刻）「ｔ」の状態変数ベクトルを、「Ｘ^－（ｔ）」または「Ｘ＾（ｔ）」と表記している（「状態変数ベクトルＸ（ｔ）＝（ｘ（ｔ）、ｙ（ｔ）、ψ（ｔ））^Ｔ」と表記する）。ここで、予測ステップで推定された暫定的な推定値（予測値）には当該予測値を表す文字の上に「－」を付し、計測更新ステップで更新された，より精度の高い推定値には当該値を表す文字の上に「＾」を付す。

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

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

そして、自車位置推定部１８のランドマーク探索・抽出ブロック２４は、地図ＤＢ１０に登録された計測対象のランドマークの位置ベクトルと、座標変換ブロック２３によりワールド座標系に変換されたライダ２の点群データとの対応付けを行う。この場合、ランドマーク探索・抽出ブロック２４は、ランドマークデータＬＤと、位置予測ブロック２２が出力する予測自車位置と、に基づき、ライダ２による検出範囲内でのランドマークの存否を判定する。そして、ランドマーク探索・抽出ブロック２４は、検出可能なランドマークがランドマークデータＬＤに登録されている場合には、当該ランドマークを検出する範囲を定める予測ウィンドウ「Ｗｐ」を設定する。この場合、ランドマーク探索・抽出ブロック２４は、座標変換されたライダ２の点群データの座標（ワールド座標）空間において、ランドマークデータＬＤに基づく対象のランドマークの位置情報を中心とする所定サイズの予測ウィンドウＷｐを設定する。予測ウィンドウＷｐは、例えば立方体であってもよい。そして、ランドマーク探索・抽出ブロック２４は、予測ウィンドウＷｐ内において、所定の閾値以上の反射率となる高反射率の点群データの計測点の有無を判定する。そして、ランドマーク探索・抽出ブロック２４は、このような計測点が存在する場合に、上述の対応付けができたと判定し、対応付けができたランドマークのライダ２による計測値「Ｚ（ｔ）」と、予測自車位置Ｘ^－（ｔ）及び地図ＤＢ１０に基づくランドマークの位置を示すベクトル値である計測予測値「Ｚ^－（ｔ）」とを決定する。この場合、ランドマーク探索・抽出ブロック２４は、予測ウィンドウＷｐ内において点群データから抽出した計測点が示す位置の代表位置（例えば座標値の平均に基づく位置）を、対象のランドマークの計測値Ｚ（ｔ）として算出する。

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

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

なお、自車位置推定部１８は、複数の地物に対し、地図ＤＢ１０に登録された地物の位置ベクトルとライダ２のスキャンデータとの対応付けができた場合、選定した任意の一組の計測予測値及び計測値等に基づき計測更新ステップを行ってもよく、対応付けができた全ての計測予測値及び計測値等に基づき計測更新ステップを複数回行ってもよい。なお、複数の計測予測値及び計測値等を用いる場合には、自車位置推定部１８は、ライダ２から遠い地物ほどライダ計測精度が悪化することを勘案し、ライダ２と地物との距離が長いほど、当該地物に関する重み付けを小さくするとよい。

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

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

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

姿勢角算出部１７は、ランドマークデータＬＤを参照し、自車位置推定部１８が予測又は推定した自車平面位置ｘ、ｙ（即ち（ｘ^－、ｙ^－）又は（ｘ^＾、ｙ^＾））の周辺に存在する区画線（白線）を探索する。そして、姿勢角算出部１７は、探索した区画線に対応するランドマークデータＬＤを参照し、当該区画線の「ｎ」個のワールド座標系の位置情報を取得する。なお、ランドマークが区画線の場合、ランドマークデータＬＤには、位置情報として、例えば数メートル間隔で、区画線の離散的な位置（離散点）を示す緯度経度高度の座標データが含まれている。姿勢角算出部１７が取得する区画線のｎ個のワールド座標系の位置情報は、「路面上に描かれた地物の複数の位置情報」の一例である。

図７（Ａ）は、区画線６０～６２の位置情報を明示した道路上の車両の俯瞰図である。図７（Ｂ）は、区画線６２の位置情報を明示した道路上の車両の側面図である。図７（Ａ）では、区画線６０～６２が存在する道路を車両が走行している。そして、区画線６０には、ランドマークデータＬＤに登録された位置情報が示す位置Ｐ１～Ｐ３が存在し、区画線６１には、ランドマークデータＬＤに登録された位置情報が示す位置Ｐ４～Ｐ６が存在し、区画線６２には、ランドマークデータＬＤに登録された位置情報が示す位置Ｐ７～Ｐ１４が存在する。

この場合、姿勢角算出部１７は、ランドマークデータＬＤを参照し、自車位置推定部１８が予測又は推定した自車平面位置ｘ、ｙ（ここでは（ｘ^＾、ｙ^＾））から所定距離以内に区画線６０～６２が存在することを認識する。そして、姿勢角算出部１７は、当該区画線６０～６２に対応するランドマークデータＬＤを参照することで、区画線６０～６２上の計１４個（即ちｎ＝１４）の位置Ｐ１～Ｐ１４のワールド座標系の位置情報を取得する。

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

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

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

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

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

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

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

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

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

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

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

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

このように、姿勢角算出部１７は、区画線のランドマークデータＬＤに基づくｎ個の区画線位置ベクトルから算出した平面方程式の係数ベクトルと、自車位置推定部１８が予測又は推定したヨー角とに基づき、好適に車両のピッチ角を算出することができる。
なお、式（７）で求めた係数ベクトル「ａ＝［ａ、ｂ、ｃ］^Ｔ」と自車平面位置ｘ、ｙを式（２）に代入することで、車両の高さｚ^－若しくはｚ^＾を求めることができる。

（４－２）ロール角の推定
姿勢角算出部１７は、ピッチ角の算出と同様、ランドマークデータＬＤに基づくｎ個の区画線位置ベクトルに基づき、式（９）に示される法線ベクトルＶｎを算出する。

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

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

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

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

このように、姿勢角算出部１７は、ランドマークデータＬＤに基づくｎ個の区画線位置ベクトルから算出した平面方程式の係数ベクトルと、自車位置推定部１８が予測又は推定したヨー角とに基づき、好適に車両のロール角を算出することができる。

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

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

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

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

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

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

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

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

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

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

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

図１２（Ａ）は、平坦な路面を走行する車両及び道路の側面図である。この場合、自車位置推定部１８は、ランドマークデータＬＤを参照し、現在位置周辺に存在する標識「Ｌ１」をランドマークとして抽出した場合、当該標識Ｌ１の位置情報から標識Ｌ１の基準位置からの高さ（例えば標高）「ｈ」を取得する。そして、自車位置推定部１８は、この場合、予測又は推定された車両高さｚ（ここでは推定車両高さｚ^＾とする）と標識Ｌ１との差「ｈ－ｚ^＾」だけ車両より高い位置に予測ウィンドウＷｐを設定する。そして、自車位置推定部１８は、予測ウィンドウＷｐの中にある反射強度の高いライダ２の点群データの計測点の平均値等に基づき計測値Ｚ（ｔ）を算出し、式（１）に基づく自車位置推定を行う。

図１２（Ｂ）は、点群データに車両のピッチ角に基づく座標変換を行わない場合において、勾配が大きい路面を走行する車両及び道路の側面図である。図１２（Ｂ）では、路面勾配により車両にピッチ角が生じるため、点群データが上向きになる。よって、自車位置推定部１８は、車両高さｚ^＾とランドマークデータＬＤに基づく標識Ｌ１の高さとの差「ｈ－ｚ^＾」だけ車両より高い位置を中心とする予測ウィンドウＷｐを設定した場合、予測ウィンドウＷｐに標識Ｌ１が含まれなくなる。よって、自車位置推定部１８は、この場合、標識Ｌ１に対応するライダ２の点群データの計測点を検出できず、標識Ｌ１をランドマークとする自車位置推定処理を実行することができない。

図１３は、本実施例に基づき点群データに車両のピッチ角に基づく座標変換を行う場合に、勾配が大きい路面を走行する車両及び道路の側面図である。この場合、実施例に基づく算出手法により求めたピッチ角を用いた回転行列により、ライダ２が出力する点群データは、車両のピッチ角を考慮したワールド座標系への変換が行われる。従って、この場合、車両から点群データの各計測点までの高さは、ワールド座標系での高さとなる。よって、この場合、自車位置推定部１８は、車両高さｚ^＾と標識Ｌ１の高さとの差「ｈ－ｚ^＾」だけ車両より高い位置を中心とする予測ウィンドウＷｐを設定することで、ライダ２が出力する点群データから、標識Ｌ１に対応する計測点を抽出することができる。

バンクなどの横断勾配が存在する道路を走行する場合においても、車載機１は、点群データに車両のロール角に基づく座標変換を行うことで、設定した予測ウィンドウＷｐに基づき、計測対象となるランドマークの計測点を点群データから好適に抽出することができる。

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

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

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

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

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

（６－２）車両高さ・ロール角・ピッチ角推定処理
図１５は、図１４のステップＳ１２及びステップＳ１６で実行する車両高さ・ロール角・ピッチ角推定処理の手順を示すフローチャートの一例である。

まず、姿勢角算出部１７は、ランドマークデータＬＤを参照し、予測自車位置ｘ^－、ｙ^－又は推定自車位置ｘ^＾、ｙ^＾の周辺に存在する区画線の位置情報を抽出する（ステップＳ２１）。例えば、姿勢角算出部１７は、予測自車位置ｘ^－、ｙ^－又は推定自車位置ｘ^＾、ｙ^＾から所定距離以内の位置を示す座標データを含む区画線のランドマークデータＬＤを地図ＤＢ１０から抽出する。

次に、姿勢角算出部１７は、ステップＳ２１で抽出した区画線の座標データが３個未満であるか否か判定する（ステップＳ２２）。そして、姿勢角算出部１７は、ステップＳ２１で抽出した区画線の座標データが３個未満である場合（ステップＳ２２；Ｙｅｓ）、１時刻前の予測車両高さｚ^－又は推定車両高さｚ^＾を、求めるべき予測車両高さｚ^－又は推定車両高さｚ^＾として定める（ステップＳ２８）。さらに、ステップＳ２８では、姿勢角算出部１７は、１時刻前に算出したロール角φ（φ^－若しくはφ^＾）又はピッチ角θ（θ^－若しくはθ^＾）を、求めるべき現時刻でのロール角φ（φ^－若しくはφ^＾）又はピッチ角θ（θ^－若しくはθ^＾）として定める。

一方、ステップＳ２１で抽出した区画線の座標データの数ｎが３個以上（ｎ≧３）である場合（ステップＳ２２；Ｎｏ）、姿勢角算出部１７は、ワールド座標系のｎ個の区画線の座標データから、式（４）及び式（５）に従い行列Ｃとベクトルｂを作成する（ステップＳ２３）。次に、姿勢角算出部１７は、式（７）に基づき、最小２乗法により係数ベクトルａを算出し、予測自車位置ｘ^－、ｙ^－又は推定自車位置ｘ^＾、ｙ^＾を式（２）の平面方程式に代入することで、現時刻での予測車両高さｚ^－又は推定車両高さｚ^＾を算出する（ステップＳ２４）。

次に、姿勢角算出部１７は、算出した係数ベクトルａから、式（９）に示される法線ベクトルＶｎを特定する（ステップＳ２５）。そして、姿勢角算出部１７は、予測又は推定された車両のヨー角（方位）ψ（ψ^－若しくはψ^＾）を用いて、式（１０）に示される進行方向ベクトルＶｘ及び式（１３）により示される横方向ベクトルＶ_Ｙを算出する（ステップＳ２６）。その後、姿勢角算出部１７は、式（１１）に基づき法線ベクトルＶｎと進行方向ベクトルＶｘの内積を算出すると共に、式（１４）に基づき法線ベクトルＶｎと横方向ベクトルＶ_Ｙの内積を算出し、求めるべき現時刻でのロール角φ（φ^－若しくはφ^＾）又はピッチ角θ（θ^－若しくはθ^＾）を算出する（ステップＳ２７）。

以上説明したように、本実施例に係る車載機１の制御部１５は、地物の位置情報を含む地図データであるランドマークデータＬＤから、車両の近傍に存在する、路面上に描かれた地物の複数の位置情報を取得する。そして、制御部１５は、取得した複数の位置情報に基づき求めた近似平面に対する法線ベクトルを算出する。そして、制御部１５は、自車両の向きと、法線ベクトルとに基づき、自車両のピッチ角又はロール角の少なくとも一方を算出する。この態様により、車載機１は、ランドマークデータＬＤに基づき、ピッチ角又はロール角の少なくとも一方を高精度に算出することができる。

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

（変形例１）
車載機１は、式（１）に基づく自車位置推定を行わない場合においても、実施例に基づく車両のピッチ角又はロール角の少なくともいずれかの推定を行ってもよい。

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

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

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

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

Claims (1)

1. 地物の位置情報を含む地図データから、移動体の近傍に存在する地物の複数の位置情報を取得する取得部と、
   前記複数の位置情報に基づき求めた近似した平面に対する法線ベクトルを算出する法線ベクトル算出部と、
   前記移動体の向きと、前記法線ベクトルとに基づき、前記移動体のピッチ角又はロール角の少なくとも一方を算出する角度算出部と、
   を有する情報処理装置。

Images