JP2024016186A - 推定装置、制御方法、プログラム及び記憶媒体 - Google Patents

Abstract

【課題】対象物に対する距離を計測する計測部の移動体に対する姿勢を好適に推定する。
【解決手段】移動体が加減速して走行しているときの、計測部に設けられた加速度検出部の検出結果に基づいて、移動体に対する計測部の姿勢を推定する。
【選択図】図１２

Description

本発明は、計測部の姿勢を推定する技術に関する。

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

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

レーダやカメラなどの計測部から得られるデータは、計測部を基準とした座標系の値であり、車両に対する計測部の姿勢等に依存したデータとなっているため、車両を基準とした座標系の値に変換する必要がある。従って、計測部の姿勢にずれが生じた場合には、そのずれを的確に検知して計測部のデータに反映させる必要がある。

本発明は、上記のような課題を解決するためになされたものであり、対象物に対する距離を計測する計測部の移動体に対する姿勢を好適に推定可能な推定装置を提供することを主な目的とする。

請求項に記載の発明は、対象物に対する距離を計測する計測部の移動体に対する姿勢を推定する推定装置であって、前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定部を有する。

また、請求項に記載の発明は、対象物に対する距離を計測する計測部の移動体に対する姿勢を推定する推定装置が実行する制御方法であって、前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定工程を有する。

また、請求項に記載の発明は、対象物に対する距離を計測する計測部の移動体に対する姿勢を推定するコンピュータが実行するプログラムであって、前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定部として前記コンピュータを機能させる。

運転支援システムの概略構成図である。 車載機の機能的構成を示すブロック図である。 ２次元座標により表された車両座標系とライダ座標系との関係を示す図である。 ３次元座標により表された車両座標系とライダ座標系との関係を示す図である。 車両座標系及びライダ座標系における重力加速度のベクトルを示した図である。 ライダのｚ方向位置の変化前後において平坦な路面を車両が走行中のときにライダにより計測された路面のｚ方向の計測値を示した図である。 上り坂の始点の前後を走行中の車両のライダのｚ方向の計測値の大きさを示す図である。 車両の走行時に得られる計測値及び車体ピッチ角の時間変化を示すグラフである。 バンプの前後を走行中の車両のライダのｚ方向の計測値の大きさを示す図である。 車両の走行時に得られる計測値及び車体ピッチレートの時間変化を示すグラフである。 旋回中の車両に生じる作用を概略的に示した図である。 ライダの出力を補正する処理の手順を示すフローチャートの一例である。

本発明の好適な実施形態によれば、対象物に対する距離を計測する計測部の移動体に対する姿勢を推定する推定装置であって、前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定部を有する。推定装置は、この態様により、車両に対するヨー方向における計測部の姿勢を好適に推定することができる。なお、「加減速して走行している」態様とは、加速して走行している態様、及び、減速して走行している態様のいずれの態様も含む。

上記推定装置の一態様では、前記推定部は、前記移動体が所定速度により走行または停止しているときの前記加速度検出部が出力する加速度データに基づいて、前記計測部のロール方向及びピッチ方向の姿勢を推定し、前記移動体が加減速して走行しているときの前記加速度検出部が出力する加速度データ並びに前記推定されたロール方向及びピッチ方向の姿勢に基づいて、前記計測部のヨー方向の姿勢を推定する。この態様により、推定装置は、移動体が所定速度により走行または停止しているときには車両に対するロール方向及びピッチ方向における計測部の姿勢を好適に推定し、移動体が加減速して走行しているときには車両に対するヨー方向における計測部の姿勢を好適に推定することができる。

上記推定装置の他の一態様では、前記推定部は、推定した前記計測部の姿勢と、記憶部に記憶された前記計測部の姿勢とに基づき、前記姿勢の変化量を推定する。これにより、推定装置は、記憶部に記憶された標準の姿勢に対する現在の姿勢の変化量を好適に推定することができる。

上記推定装置の他の一態様では、前記推定部は、高さ方向における路面の位置を示す前記計測部の計測データに基づいて、前記計測部の前記高さ方向の位置を推定する。この態様により、推定装置は、計測部の高さ方向における位置を好適に推定することができる。

上記推定装置の他の一態様では、前記推定部は、勾配が変化する道路地点が前記計測部により計測されたときの前記道路地点と前記移動体との距離に基づいて、前記移動体の前後方向における前記計測部の位置を推定する。この態様により、推定装置は、移動体の前後方向における計測部の位置を好適に推定することができる。

上記推定装置の他の一態様では、前記推定部は、前記移動体のピッチ方向の傾きを検出する傾き検出部が出力するデータの変化と、前記計測部が出力する計測データの変化との時間差に基づき、前記距離を算出する。この態様により、推定装置は、勾配が変化する道路地点が計測部により計測されたときの道路地点と移動体との距離を好適に算出し、移動体の前後方向における計測部の位置を推定に用いることができる。

上記推定装置の他の一態様では、前記推定部は、前記移動体の旋回中において前記加速度検出部が出力する前記移動体の左右方向の加速度データと、前記移動体に搭載された加速度センサが出力する前記移動体の左右方向の加速度データと、前記移動体に搭載されたジャイロセンサが出力する前記移動体のヨーレートとに基づいて、前記移動体の左右方向における前記計測部の位置を推定する。この態様により、推定装置は、移動体の左右方向における計測部の位置を好適に推定することができる。

上記推定装置の他の一態様では、前記推定部は、推定した前記計測部の位置と、記憶部に記憶された前記計測部の位置とに基づき、前記位置の変化量を推定する。これにより、推定装置は、記憶部に記憶された計測部の標準の位置に対する現在の位置の変化量を好適に推定することができる。

上記推定装置の他の一態様では、前記変化量に基づき、前記計測部が出力する計測データを補正する補正部をさらに備える。推定装置は、この態様により、計測部の姿勢や位置のずれが生じた場合であっても、ずれの影響が生じないように計測部の計測データを補正することができる。

上記推定装置の他の一態様では、前記変化量が所定量以上である場合、前記計測部が出力する計測データに基づく処理を停止する停止制御部をさらに備える。この態様により、推定装置は、姿勢や位置のずれが大きい計測部の計測データを用いることによる当該計測データを用いる種々の処理の精度が低下するのを確実に抑制することができる。

本発明の他の好適な実施形態によれば、対象物に対する距離を計測する計測部の移動体に対する姿勢を推定する推定装置が実行する制御方法であって、前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定工程を有する。推定装置は、この制御方法を用いることで、車両に対するヨー方向における計測部の姿勢を好適に推定することができる。

本発明の他の好適な実施形態によれば、対象物に対する距離を計測する計測部の移動体に対する姿勢を推定するコンピュータが実行するプログラムであって、前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定部として前記コンピュータを機能させる。コンピュータは、このプログラムを実行することで、車両に対するヨー方向における計測部の姿勢を好適に推定することができる。好適には、上記プログラムは、記憶媒体に記憶される。

以下、図面を参照して本発明の好適な各実施例について説明する。

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

車載機１は、ライダ２、ジャイロセンサ３、車体用加速度センサ４、及びライダ用加速度センサ５と電気的に接続し、これらの出力データを取得する。また、道路データ及び道路付近に設けられた地物に関する地物情報などを記憶した地図データベース（ＤＢ：ＤａｔａＢａｓｅ）１０を記憶している。そして、車載機１は、上述の出力データ及び地図ＤＢ１０に基づき、車両の位置（「自車位置」とも呼ぶ。）の推定を行い、自車位置の推定結果に基づいて自動運転制御などの車両の運転支援に関する制御などを行う。また、車載機１は、ライダ２、ジャイロセンサ３、車体用加速度センサ４、及びライダ用加速度センサ５の出力に基づいて、ライダ２の姿勢及び位置の推定を行う。そして、車載機１は、この推定結果に基づいて、ライダ２が出力する点群データの各計測値を補正する処理などを行う。車載機１は、本発明における「推定装置」の一例である。

ライダ２は、水平方向および垂直方向の所定の角度範囲に対してパルスレーザを出射することで、外界に存在する物体までの距離を離散的に測定し、当該物体の位置を示す３次元の点群情報を生成する。この場合、ライダ２は、照射方向を変えながらレーザ光を照射する照射部と、照射したレーザ光の反射光（散乱光）を受光する受光部と、受光部が出力する受光信号に基づくスキャンデータを出力する出力部とを有する。スキャンデータは、受光部が受光したレーザ光に対応する照射方向と、上述の受光信号に基づき特定される当該レーザ光のその照射方向での物体までの距離とに基づき生成され、車載機１へ供給される。本実施例では、一例として、ライダ２は、車両のフロント部分とリア部分とにそれぞれ設けられている。ライダ２は、本発明における「計測部」の一例である。

ジャイロセンサ３は、車両に設けられ、車体のヨーレートに相当する出力信号を車載機１へ供給する。車体用加速度センサ４は、車両に設けられた３軸加速度センサであり、車体の進行方向、側面方向、高さ方向に相当する３軸の加速度データに相当する検出信号を車載機１へ供給する。ジャイロセンサ３及び車体用加速度センサ４は、本発明における「傾き検出部」の一例である。ライダ用加速度センサ５は、各ライダ２に設けられた３軸加速度センサであり、設置されたライダ２の３軸の加速度データに相当する検出信号をそれぞれ車載機１へ供給する。ライダ用加速度センサ５は、本発明における「加速度検出部」の一例である。

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

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

記憶部１２は、制御部１５が実行するプログラムや、制御部１５が所定の処理を実行するのに必要な情報を記憶する。本実施例では、記憶部１２は、地図ＤＢ１０と、ライダ設置情報ＩＬとを有する。ライダ設置情報ＩＬは、ある基準時（例えばライダ２のアライメント調整直後などの姿勢・位置ずれが生じていない時）における各ライダ２の相対的な３次元位置と姿勢に関する情報である。本実施例では、ライダ２等の姿勢を、ロール角、ピッチ角、ヨー角（即ちオイラー角）により表すものとする。ライダ設置情報ＩＬは、上述の基準時において実測された位置及び姿勢に関する情報であってもよく、後述するライダ２の位置及び姿勢の推定処理により車載機１によって推定されたライダ２の位置及び姿勢に関する情報であってもよい。

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

制御部１５は、プログラムを実行するＣＰＵなどを含み、車載機１の全体を制御する。制御部１５は、インターフェース１１から供給される各センサの出力信号及び地図ＤＢ１０に基づき、自車位置の推定を行い、自車位置の推定結果に基づいて自動運転制御を含む車両の運転支援に関する制御などを行う。このとき、制御部１５は、ライダ２の出力データを用いる場合には、ライダ２が出力する計測データを、ライダ設置情報ＩＬに記録されたライダ２の姿勢及び位置を基準として、ライダ２を基準とした座標系から車両を基準とした座標系に変換する。さらに、本実施例では、制御部１５は、車両に対するライダ２の現在（即ち処理基準時）の位置及び姿勢を推定することで、ライダ設置情報ＩＬに記録された位置及び姿勢に対する変化量を算出し、当該変化量に基づきライダ２が出力する計測データを補正する。これにより、制御部１５は、ライダ２の位置又は姿勢にずれが生じた場合であっても、当該ずれの影響を受けないようにライダ２が出力する計測データを補正する。制御部１５は、本発明における「推定部」、「補正部」、「停止制御部」及びプログラムを実行する「コンピュータ」の一例である。

［ライダの位置及び姿勢推定］
次に、ライダ２の位置及び姿勢の推定方法について説明する。車載機１は、以下に示す処理を、ライダ２ごとに実行する。

（１）座標系の変換
ライダ２により取得される３次元点群データの各計測点が示す３次元座標は、ライダ２の位置及び姿勢を基準とした座標系（「ライダ座標系」とも呼ぶ。）で表されており、車両の位置及び姿勢を基準とした座標系（「車両座標系」とも呼ぶ。）に変換する必要がある。ここでは、まず、ライダ座標系と車両座標系との変換について説明する。

図３は、２次元座標により表された車両座標系とライダ座標系との関係を示す図である。ここでは、車両座標系は、車両の中心を原点とし、車両の進行方向に沿った座標軸「ｘ_ｂ」と車両の側面方向に沿った座標軸「ｙ_ｂ」を有する。また、ライダ座標系は、ライダ２の正面方向（矢印Ａ２参照）に沿った座標軸「ｘ_Ｌ」とライダ２の側面方向に沿った座標軸「ｙ_Ｌ」を有する。

ここで、車両座標系に対するライダ２のヨー角を「Ｌ_ψ０」、ライダ２の位置を［Ｌ_ｘ０、Ｌ_ｙ０］^Ｔとした場合、車両座標系から見た時刻「ｋ」の計測点［ｘ_ｂ（ｋ）、ｙ_ｂ（ｋ）］^Ｔは、回転行列「Ｃ_ψ０」を用いた以下の式（１）によりライダ座標系の座標［ｘ_Ｌ（ｋ）、ｙ_Ｌ（ｋ）］^Ｔへ変換される。

一方、ライダ座標系から車両座標系への変換は、回転行列の逆行列（転置行列）を用いればよい。よって、ライダ座標系で取得した時刻ｋの計測点［ｘ_Ｌ（ｋ）、ｙ_Ｌ（ｋ）］^Ｔは、以下の式（２）により車両座標系の座標［ｘ_ｂ（ｋ）、ｙ_ｂ（ｋ）］^Ｔに変換することが可能である。

図４は、３次元座標により表された車両座標系とライダ座標系との関係を示す図である。ここでは、座標軸ｘ_ｂ、ｙ_ｂに垂直な座標軸を「ｚ_ｂ」、座標軸ｘ_Ｌ、ｙ_Ｌに垂直な座標軸を「ｚ_Ｌ」とする。

車両座標系に対するライダ２のロール角を「Ｌ_φ０」、ピッチ角を「Ｌ_θ０」、ヨー角を「Ｌ_ψ０」とし、ライダ２の座標軸ｘ_ｂにおける位置が「Ｌ_ｘ０」、座標軸ｙ_ｂにおける位置が「Ｌ_ｙ０」、座標軸ｚ_ｂにおける位置が「Ｌ_ｚ０」とした場合、車両座標系から見た時刻「ｋ」の計測点［ｘ_ｂ０（ｋ）、ｙ_ｂ０（ｋ）、ｚ_ｂ０（ｋ）］^Ｔは、ロール、ピッチ、ヨーに対応する各回転行列「Ｃ_φ０」、「Ｃ_θ０」、「Ｃ_ψ０」により表される方向余弦行列「Ｃ_０」を用いた以下の式（３）により、ライダ座標系の座標［ｘ_Ｌ０（ｋ）、ｙ_Ｌ０（ｋ）、ｚ_Ｌ０（ｋ）］^Ｔへ変換される。

一方、ライダ座標系から車両座標系への変換は、方向余弦行列の逆行列（転置行列）を用いればよい。よって、ライダ座標系で取得した時刻ｋの計測点［ｘ_Ｌ０（ｋ）、ｙ_Ｌ０（ｋ）、ｚ_Ｌ０（ｋ）］^Ｔは、以下の式（４）により車両座標系の座標［ｘ_ｂ０（ｋ）、ｙ_ｂ０（ｋ）、ｚ_ｂ０（ｋ）］^Ｔに変換することが可能である。

なお、以後では、車両座標系における各座標軸ｘ_ｂ、ｙ_ｂ、ｚ_ｂに沿った方向を、それぞれ単に「ｘ方向」、「ｙ方向」、「ｚ方向」とも呼ぶ。

（２）ロール角及びピッチ角の推定
次に、ライダ２のロール角Ｌ_φ０及びピッチ角Ｌ_θ０の推定方法について説明する。以下に説明するように、車載機１は、対象のライダ２に設置されたライダ用加速度センサ５から得られる３軸の加速度出力値に基づき、ライダ２のロール角Ｌ_φ０及びピッチ角Ｌ_θ０を推定する。以後では、説明便宜上、ライダ用加速度センサ５は、ライダ座標系の３軸の加速度を計測するものとする。

車両が水平な場所で停車中あるいは一定速度で走行中の場合、車両座標系における加速度は、ｚ方向の重力加速度ｇのみである。図５（Ａ）は、車両座標系における重力加速度ｇのベクトルを示した図であり、図５（Ｂ）は、ライダ座標系における重力加速度ｇのベクトルを示した図である。よって，ライダ用加速度センサ５のライダ座標系の出力値［α_ｘ,α_ｙ,α_ｚ］^Ｔは以下の式（５）が成り立つ。

ここで、式（５）のα_ｙ,α_ｚを用いて「α_ｙ／α_ｚ」を計算すると、以下の式（６）が得られる。

よって、ライダ２のロール角Ｌ_φ０は、重力加速度ｇを用いない以下の式（７）により表される。

また、式（５）のα_ｙ,α_ｚを用いて「α_ｙ ^２＋α_ｚ ^２」を計算すると、以下の式（８）が得られ、さらに式（８）と式（５）のα_ｘを用いて重力加速度ｇを消去すると、以下の式（９）が得られる。

よって、ライダ２のピッチ角Ｌ_θ０は、重力加速度ｇを用いない以下の式（１０）により表される。

以上により、車載機１は、水平な場所で、停止あるいは一定速度で走行しているときに、式（７）及び式（１０）を参照することで、ライダ用加速度センサ５の出力値に基づき、ライダ２のロール角Ｌ_φ０及びピッチ角Ｌ_θ０を算出することができる。なお、車載機１は、現在位置が水平な場所か否かを、ジャイロセンサ３又は車体用加速度センサ４の出力に基づき判定してもよく、車両の現在位置に相当する道路の道路データの傾斜角度に関する情報を地図ＤＢ１０から参照することで判定してもよい。また、車載機１は、車両が停止あるいは一定速度で走行しているか否かを、車体用加速度センサ４の出力に基づき判定してもよく、図示しない車速センサの出力に基づき判定してもよい。

（３）ヨー角の推定
次に、ライダ２のヨー角Ｌ_ψ０の推定方法について説明する。以下に説明するように、車載機１は、算出したライダ２のロール角Ｌ_φ０及びピッチ角Ｌ_θ０を用いて、車両が直進道路を加速中又は減速中に、ライダ用加速度センサ５から得られる３軸の加速度出力値に基づき、ヨー角Ｌ_ψ０を推定する。

車両が直進道路を加速度「α」により加速中又は減速中の場合、車両座標系において、ｘ方向において加速度αが発生すると共に、ｚ方向において重力加速度ｇが発生するため、ライダ用加速度センサ５のライダ座標系の出力値［α_ｘ,α_ｙ,α_ｚ］^Ｔについて、以下の式（１１）が成り立つ。

ここで、加速度αと重力加速度ｇを消去するため、以下に説明する演算を行う。

まず、式（１１）のα_ｙ,α_ｚを用いると、それぞれ以下の式（１２）、式（１３）が成立する。

そして、式（１２）を式（１３）により減算すると、以下の式（１４）が得られる。

同様に、式（１１）のα_ｙ,α_ｚを用いると、それぞれ以下の式（１５）、式（１６）が成立する。

そして、式（１５）と式（１６）とを加算すると、以下の式（１７）が得られる。

さらに、式（１７）にｓｉｎＬ_θ０を乗じた式と、式（１１）のα_ｘにｃｏｓＬ_θ０を乗じた式とを加算すると、以下の式（１８）が得られる。

そして、式（１４）を式（１８）により割ると、以下の式（１９）が得られる。

よって、ライダ２のヨー角Ｌ_ψ０は、加速度α及び重力加速度ｇを用いない以下の式（２０）により表される。

以上により、車載機１は、直進道路を加減速中に得られたライダ用加速度センサ５の出力値に基づき、式（２０）を参照することで、ライダ２のヨー角Ｌ_ψ０を算出することができる。なお、車載機１は、車両が直進道路を走行中か否かを、車体用加速度センサ４の出力に基づき判定してもよく、現在位置に相当する道路の道路データを地図ＤＢ１０から参照することで判定してもよい。また、車載機１は、車両が加減速中であるか否かを、車体用加速度センサ４の出力に基づき判定してもよく、図示しない車速センサの出力に基づき判定してもよい。

（４）姿勢の変化量の算出
次に、ライダ設置情報ＩＬの生成時から現在時刻である処理基準時点までのライダ２のピッチ角、ロール角、ヨー角の変化量の算出について補足説明する。以下では、ライダ設置情報ＩＬに記録された（即ちライダ２の姿勢・位置ずれが生じてない初期時の）ライダ２のピッチ角、ロール角、ヨー角を、それぞれ、「Ｌ_φ０」、「Ｌ_θ０」、「Ｌ_ψ０」とする。

何らかの影響により、以下の式（２１）に示されるように、ライダ２のロール角が「ΔＬ_φ」、ピッチ角が「ΔＬ_θ」、ヨー角が「ΔＬ_ψ」だけライダ設置情報ＩＬの生成時からそれぞれ変化し、処理基準時点のロール角が「Ｌ_φ」、ピッチ角が「Ｌ_θ」、ピッチ角が「Ｌ_ψ」になったとする。

同様に、以下の式（２２）に示されるように、ライダ２の座標軸ｘ_ｂにおける位置が「ΔＬ_ｘ０」、座標軸ｙ_ｂにおける位置が「ΔＬ_ｙ０」、座標軸ｚ_ｂにおける位置が「ΔＬ_ｚ０」だけライダ設置情報ＩＬの生成時からそれぞれ変化し、処理基準時点の座標軸ｘ_ｂにおける位置が「Ｌ_ｘ」、座標軸ｙ_ｂにおける位置が「Ｌ_ｙ」、座標軸ｚ_ｂにおける位置が「Ｌ_ｚ」になったとする。

また、ライダ２の上述の姿勢及び位置の変化に起因して、ライダ２から得られる時刻ｋの計測点が［ｘ_Ｌ０（ｋ）、ｙ_Ｌ０（ｋ）、ｚ_Ｌ０（ｋ）］^Ｔから［ｘ_Ｌ（ｋ）、ｙ_Ｌ（ｋ）、ｚ_Ｌ（ｋ）］^Ｔになったとする。この場合、時刻ｋの計測点の車両座標系からライダ座標系への変換は、式（２３）のようになる。

ここで、式（２３）は、式（３）と同じ形式であるため、式（７）、式（１０）、式（２０）と同様の式を用いて、ライダ２のロール角Ｌ_φ、ピッチ角Ｌ_θ、ヨー角Ｌ_ψを算出することができる。よって、車載機１は、これらのロール角Ｌ_φ、ピッチ角Ｌ_θ、ヨー角Ｌ_ψと、ライダ設置情報ＩＬに記録されたロール角Ｌ_φ０、ピッチ角Ｌ_θ０、ヨー角Ｌ_ψ０との差をそれぞれ算出することで、ロール角の変化量ΔＬ_φ、ピッチ角の変化量ΔＬ_θ、ヨー角の変化量ΔＬ_ψを好適に算出することができる。なお、車載機１は、ライダ２の車両への取付け（即ちアライメント調整）後からライダ２の姿勢の推定処理が最初に実行可能なタイミングで式（７）、式（１０）、式（２０）に基づき推定したライダ２のロール角Ｌ_φ０、ピッチ角Ｌ_θ０、ヨー角Ｌ_ψ０を、ライダ設置情報ＩＬとして記憶してもよい。

（５）ｚ方向の位置変化量の算出
次に、ｚ方向におけるライダ２の位置変化量の算出方法について説明する。車載機１は、平坦道路で車両が停止中又は一定速度走行中に、道路面を計測したライダ２の計測点のｚ方向の値に基づき、ｚ方向におけるライダ２の位置変化量「ΔＬ_ｚ」を算出する。

処理基準時点での車両座標系の時刻ｋの計測点の座標［ｘ_ｂ（ｋ）、ｙ_ｂ（ｋ）、ｚ_ｂ（ｋ）］^Ｔは、式（４）により示される座標［ｘ_ｂ０（ｋ）、ｙ_ｂ０（ｋ）、ｚ_ｂ０（ｋ）］^Ｔと同様、方向余弦行列「Ｃ」を用いて、以下の式（２４）により表される。

ここで、処理基準時点のロール角Ｌ_φ、ピッチ角Ｌ_θ、ピッチ角Ｌ_ψが正確に求められている場合、「Ｃ^－１［ｘ_Ｌ（ｋ）、ｙ_Ｌ（ｋ）、ｚ_Ｌ（ｋ）］^Ｔ」は、車両座標系の値に正しく変換されている。よって、平坦な路面で車両が停車中あるいは一定速度で走行している時は、車両のピッチ変動が少ないため、道路面を照射したライダ２のz方向の計測値は、ライダ２の搭載位置から道路面までの高さ（即ち位置Ｌ_ｚ）となる。

図６（Ａ）は、ライダ２の位置Ｌ_ｚの変化前において平坦な路面を車両が走行中のときにライダ２により計測された路面のｚ方向の計測値ｚ_ｂ０（ｋ）を示し、図６（Ｂ）は、ライダ２の位置Ｌ_ｚの変化後において平坦な路面を車両が走行中のときにライダ２により計測された路面のｚ方向の計測値ｚ_ｂ（ｋ）を示す。図６（Ａ）、（Ｂ）に示すように、計測値ｚ_ｂ０（ｋ）、計測値ｚ_ｂ（ｋ）は、それぞれ、ライダ２のｚ方向の位置Ｌ_ｚ０、Ｌ_ｚとそれぞれ同一長となっている。

よって、式（４）で計算される道路面の計測値ｚ_ｂ０（ｋ）と、姿勢変化後の道路面の計測値ｚ_ｂ（ｋ）との差分は、ｚ方向の変化量ΔＬ_ｚと等しくなることがわかる。この場合に用いる計測値ｚ_ｂ０（ｋ）及び計測値ｚ_ｂ（ｋ）は、複数の走査ラインでの平均及び時間平均であることが望ましい。

以上を勘案し、車載機１は、以下の式（２５）に基づき、変化量ΔＬ_ｚを算出する。

従って、車載機１は、ライダ２の初期位置Ｌ_ｚ０の計測後に、平坦な路面で車両が停車中あるいは一定速度で走行している時の路面の計測値ｚ_ｂ０（ｋ）の平均を算出し、ライダ２の初期位置Ｌ_ｚ０と共にライダ設置情報ＩＬに記録しておく。これにより、車載機１は、ライダ設置情報ＩＬを参照することで、処理基準時の位置Ｌ_ｚ及び変化量ΔＬ_ｚを好適に算出することができる。

なお、車載機１は、初期位置Ｌ_ｚ０の計測時と処理基準時とでの車両のサスペンションのストローク量（即ち伸び切り位置からの沈み量）をそれぞれ推定し、推定したストローク量の差に基づき、z方向の変化量ΔＬ_ｚを補正してもよい。この場合、車載機１は、車両のサスペンションに設けられたストロークセンサ等に基づき、サスペンションのストローク量を計測してもよく、車両の搭乗人数に基づき、サスペンションのストローク量を推定してもよい。これにより、より正確に変化量ΔＬ_ｚを算出することが可能である。

（６）ｘ方向の位置変化量の算出
次に、ｘ方向におけるライダ２の位置変化量の算出方法について説明する。車載機１は、坂道の始点又は終点付近において、あるいは路面上のバンプを通過するときにおいて、ライダ２のｚ方向計測値の変化とジャイロセンサ３から得られるピッチレートの変化との時間差に基づき、ｘ方向の位置変化量Ｌ_ｘを算出する。

図７（Ａ）～図７（Ｇ）は、上り坂の始点５０の前後を走行中の車両のライダ２の特定のスキャンラインのｚ方向の計測値ｚ_ｂ（ｋ）の大きさを線分５１～５７により表した図である。また、図８（Ａ）は、図７（Ａ）～図７（Ｇ）に示す車両の走行時に計測される計測値ｚ_ｂ（ｋ）の時間変化を示すグラフであり、図８（Ｂ）は、図８（Ａ）と同一期間における車体ピッチ角（ピッチレートの積分値）の時間変化を示すグラフである。なお、図８（Ａ）及び図８（Ｂ）における番号５１～５７は、図７（Ａ）～図７（Ｇ）の線分５１～５７が示す計測値ｚ_ｂ（ｋ）に対応する位置をそれぞれ指し示している。

上り坂や下り坂などの勾配路面の始まりや終わりの直前では、道路面を計測点とするｚ方向のライダ２の計測値は、坂の始点又は終点付近において変化する。図７の例では、図８（Ａ）に示すように、ライダ２が始点５０（即ち勾配の変化点）を照射した時刻「ｔ１」（図７（Ｂ）参照）から計測値ｚ_ｂ（ｋ）が徐々に変化し、車体の前輪が始点５０に差し掛かる時刻「ｔ２」（図７（Ｄ）参照）において計測値ｚ_ｂ（ｋ）が最小となる。その後、徐々に計測値ｚ_ｂ（ｋ）は大きくなり、後輪が始点５０に差し掛かるとき（図７（Ｆ）参照）の計測値ｚ_ｂ（ｋ）は平坦な路面を走行するときの計測値ｚ_ｂ（ｋ）と同じになる。

以上を勘案し、車載機１は、計測値ｚ_ｂ（ｋ）が減少し始める時刻ｔ１（図７（Ｂ）参照）から計測値ｚ_ｂ（ｋ）が最小となる時刻ｔ２（図７（Ｄ）参照）までの時間間隔「Δｔ」を算出する。

ここで、時間間隔Δｔは、図７（Ａ）に示す距離ｄを車両が走行するのに要する時間に相当し、距離ｄは、特定のスキャンラインにおいて坂道の始点５０を検出したときの当該始点５０と車両の前輪までの距離に相当する。時間間隔Δｔは、本発明における「時間差」の一例である。

そして、車載機１は、以下の式（２６）に示すように、車速パルスなどから車両の走行速度「ｖ」を求めて時間間隔Δtを乗じることで、距離ｄを算出する。

なお、図８（Ｂ）に示すように、車載機１は、車体の前輪が勾配の変化点（図７では始点５０）に差し掛かる時刻ｔ２を、車体又はライダ２に搭載されたジャイロセンサ３により計測したピッチ角（図８（Ｂ）では車体ピッチ角）により判定することもできる。

また、車載機１は、距離ｄを、路面上のバンプを通過したときにも同様に計測することが可能である。図９（Ａ）～図９（Ｇ）は、バンプ６０の前後を走行中の車両のライダ２のｚ方向の計測値ｚ_ｂ（ｋ）の大きさを線分６１～６６により表した図である。また、図１０（Ａ）は、図９（Ａ）～図９（Ｇ）に示す車両の走行時に得られる計測値ｚ_ｂ（ｋ）の時間変化を示すグラフであり、図１０（Ｂ）は、図１０（Ａ）と同一期間における車体ピッチレートの時間変化を示すグラフである。なお、図１０（Ａ）及び図１０（Ｂ）における番号６１～６６は、図９（Ａ）～図９（Ｇ）の線分６１～６６が示す計測値ｚ_ｂ（ｋ）が対応する位置をそれぞれ指し示している。この場合、図１０（Ａ）に示すように、車両のライダ２のｚ方向の計測値ｚ_ｂ（ｋ）は、バンプ６０を照射する時刻「ｔ３」（図９（Ｂ）参照）において計測値ｚ_ｂ（ｋ）が一時的に減少し、車体の前輪がバンプ６０を超える時刻「ｔ４」（図９（Ｄ）参照）において計測値ｚ_ｂ（ｋ）一時的に増加する。従って、車載機１は、計測値ｚ_ｂ（ｋ）が一時的に下がった時刻ｔ３（図９（Ｂ）参照）から計測値ｚ_ｂ（ｋ）が一時的に大きくなった時刻ｔ４（図９（Ｄ）参照）までの時間間隔を、時間間隔Δｔとして算出する。これによっても、車載機１は、式（２６）に基づき距離ｄを好適に算出することができる。なお、図１０（Ｂ）に示すように、車載機１は、車体の前輪がバンプ６０を超える時刻ｔ４を、車体又はライダ２に搭載されたジャイロセンサ３により計測したピッチレート（図１０（Ｂ）では車体ピッチレート）により判定することもできる。

次に、距離ｄから位置変化量ΔＬ_ｘを算出する方法について説明する。車載機１は、ライダ２の位置が変化する前の距離「ｄ_０」を記憶しておき，以下の式（２７）に示すように、距離ｄとの差分を取ることで、ライダ２のｘ方向の位置変化量ΔＬ_ｘを算出することができる。

この場合、例えば、車載機１は、ライダ２の初期位置Ｌ_ｘ０の計測後、最初に坂道の始点又は終点付近を通過したとき、あるいは路面上のバンプを通過したときに距離ｄ_０を算出し、ライダ２の初期位置Ｌ_ｘ０と共にライダ設置情報ＩＬに記録しておく。これにより、車載機１は、ライダ設置情報ＩＬを参照することで、式（２７）に基づき、処理基準時の位置Ｌ_ｘ及び変化量ΔＬ_ｘを好適に算出することができる。

（７）ｙ方向の位置変化量の算出
次に、ｙ方向におけるライダ２の位置変化量の算出方法について説明する。車載機１は、車両の旋回中において、ライダ用加速度センサ５のｙ方向出力値と、車体用加速度センサ４のｙ方向出力値と、ジャイロセンサ３の出力値から、ｙ方向のライダ２の位置Ｌ_ｙを算出する。

図１１は、旋回中の車両に生じる作用を概略的に示した図である。一般に，旋回中の車両重心点の速度「Ｖ_Ｇ」と向心加速度「α_Ｇ」は、以下の式（２８）及び式（２９）で与えられる。

ここで，式（２８）の「ｒ_Ｇ」は旋回中心点から車両重心点までの距離を示している。また，速度Ｖ_Ｇと向心加速度α_Ｇの向きは直交している。剛体であれば，角速度はどの場所でも同じであるため，重心点から［Ａ_ｘ、Ａ_ｙ］^Ｔ離れた位置に設定した車両座標原点のＡ点の速度「Ｖ_Ａ」と向心加速度「α_Ａ」は，以下の式（３０）及び式（３１）で与えられる。

図１１より，向心加速度α_Ａと車両横方向成分「α_Ａｙ」との関係は，以下の式（３２）に示される関係となる。

よって、車両横方向成分α_Ａｙは、以下の式（３３）により表される。

この式（３３）の左辺の車両横方向成分α_Ａｙは、車両のＡ点の位置に加速度センサを搭載すると、ｙ軸方向の出力として計測できる。ここで、Ａ点から［Ｌ_ｘ、Ｌ_ｙ］^Ｔ離れた位置にライダ２が搭載されているとすると、ライダ２の位置での車両横方向成分「α_{（Ｌ＋Ａ）ｙ}」に関し、式（３３）と同様の以下の式（３４）が成立し、この式（３４）と式（３３）を用いることで、さらに以下の式（３５）が得られる。

よって、位置Ｌ_ｙは、以下の式（３６）により表される。

式（３６）は，車両に搭載した車両用加速度センサ４とライダ用加速度センサ５の出力の差分を車両のヨーレートの２乗で割れば、それが車両座標系原点に対するライダ２のｙ方向の位置になることを示している。従って、車載機１は、式（３６）を計算することで、ライダ２の位置Ｌ_ｙを把握することができる。また、車載機１は、ライダ設置情報ＩＬに記憶された初期位置Ｌ_ｙ０を参照することで、変化量ΔＬ_ｙを算出することもできる。

［処理フロー］
図１２は、ライダ２の出力を補正する処理の手順を示すフローチャートの一例である。車載機１は、図１２に示す処理を、所定のタイミングにおいて繰り返し実行する。

まず、車載機１は、水平な道路で車両が停止中又は一定速度走行中に、ライダ用加速度センサ５の出力値からライダ２のロール角の変化量ΔＬ_φ及びピッチ角の変化量ΔＬ_θを算出する（ステップＳ１０１）。この場合、車載機１は、式（７）及び（１０）と同等の式に基づきライダ２のロール角Ｌ_φ、ピッチ角Ｌ_θを算出し、ライダ設置情報ＩＬに記録されたロール角Ｌ_φ０、ピッチ角Ｌ_θ０との差を、変化量ΔＬ_φ、ΔＬ_θとして算出する。

次に、車載機１は、車両が直進道路を加減速中に、ライダ用加速度センサ５の出力値からライダ２のヨー角の変化量ΔＬ_ψを算出する（ステップＳ１０２）。この場合、車載機１は、式（２０）と同等の式に基づきライダ２のヨー角Ｌ_ψを算出し、ライダ設置情報ＩＬに記録されたヨー角Ｌ_ψ０との差を、変化量ΔＬ_ψとして算出する。

次に、車載機１は、車両が平坦道路で停止中又は一定速度走行中に、道路面を照射するライダ２のｚ方向の計測値から、ｚ方向の位置変化量ΔＬ_ｚを算出する（ステップＳ１０３）。この場合、車載機１は、平坦な路面で車両が停車中あるいは一定速度で走行している時の路面の計測値ｚ_ｂ（ｋ）の平均を算出し、ライダ設置情報ＩＬに記録された同条件での路面の計測値ｚ_ｂ０（ｋ）の平均との差分をとることで、変化量ΔＬｚを算出する（式（２５）参照）。

次に、車載機１は、坂道の始点又は終点付近において、あるいは路面上のバンプを通過するときにおいて、ライダ２のｚ方向計測値の変化とジャイロセンサの出力値の変化との時間間隔Δｔを算出することで、ｘ方向の位置変化量ΔＬ_ｘを算出する（ステップＳ１０４）。この場合、車載機１は、時間間隔Δｔに車両の走行速度ｖを乗じることで距離ｄを算出し、あらかじめライダ設置情報ＩＬに記憶された距離ｄ_０との差分を、変化量ΔＬ_ｚとして算出する（式（２６）参照）。

次に、車載機１は、車両の旋回中において、ライダ用加速度センサ５のｙ方向出力値と、車体用加速度センサ４のｙ方向出力値と、ジャイロセンサ３の出力値から、ｙ方向の位置変化量ΔＬ_ｙを算出する（ステップＳ１０５）。具体的には、車載機１は、式（３６）に基づき処理基準時の位置Ｌ_ｙを算出し、ライダ設置情報ＩＬに記憶された初期位置Ｌ_ｙ０との差分を、ｙ方向の位置変化量ΔＬ_ｙとして算出する。

次に、車載機１は、ステップＳ１０１～Ｓ１０５で算出した変化量ΔＬ_φ、ΔＬ_θ、ΔＬ_ψ、ΔＬ_ｘ、ΔＬ_ｙ、ΔＬ_ｚのうち、所定の閾値以上のものが存在するか否か判定する（ステップＳ１０６）。上述の閾値は、後述するステップＳ１０８でのライダ２の計測データの補正処理を行うことで引き続きライダ２の計測データを使用できるか否かを判定するための閾値であり、例えば予め実験等に基づき設定される。そして、車載機１は、ステップＳ１０１～Ｓ１０５で算出した変化量ΔＬ_φ、ΔＬ_θ、ΔＬ_ψ、ΔＬ_ｘ、ΔＬ_ｙ、ΔＬ_ｚのうち、所定の閾値以上のものが存在する場合（ステップＳ１０６；Ｙｅｓ）、対象のライダ２の出力データの使用（即ち障害物検知や自車位置推定等への利用）を中止し、対象のライダ２について再度のアライメント調整を行う必要がある旨の警告を情報出力部１６により出力する（ステップＳ１０７）。これにより、事故等により著しく姿勢・位置のずれが生じたライダ２の計測データを用いることによる安全性低下等を確実に抑制する。上述の閾値は、本発明における「所定量」の一例である。

一方、車載機１は、変化量ΔＬ_φ、ΔＬ_θ、ΔＬ_ψ、ΔＬ_ｘ、ΔＬ_ｙ、ΔＬ_ｚのうち、所定の閾値以上のものが存在しない場合（ステップＳ１０６；Ｎｏ）、これらの変化量に基づき、ライダ２が出力する点群データの各計測値を補正する（ステップＳ１０８）。この場合、車載機１は、例えば、各変化量の大きさごとの計測値の補正量を示すマップ等を記憶しておき、当該マップ等を参照することで、上述の計測値を補正する。また、変化量の所定の割合の値を計測値の補正量として計測値を補正してもよい。

以上説明したように、本実施例における車載機１は、対象物に対する距離を計測するライダ２の車両に対する姿勢を少なくとも推定するものであって、車両が加減速して走行しているときの、ライダ２に設けられたライダ用加速度センサ５の検出結果に基づいて、車両に対するライダ２の姿勢を推定する処理などを行う。これにより、車載機１は、ライダ２が出力する計測データを車両座標系に変換する処理を高精度に実行したり、ライダ２の使用可否の判定を行ったりすることができる。

［変形例］
以下、実施例に好適な変形例について説明する。以下の変形例は、組み合わせて実施例に適用してもよい。

（変形例１）
図１２のステップＳ１０８において、車載機１は、ステップＳ１０１～Ｓ１０５で算出した各変化量に基づきライダ２が出力する点群データの各計測値を補正する代わりに、ステップＳ１０１～Ｓ１０５で算出するライダ２の処理基準時の姿勢及び位置の各推定値に基づき、各計測値を車両座標系に変換してもよい。

この場合、車載機１は、ステップＳ１０１～Ｓ１０５で算出したロール角Ｌ_φ、ピッチ角Ｌ_θ、ヨー角Ｌ_ψ、ｘ方向位置Ｌ_ｘ、ｙ方向位置Ｌ_ｙ、ｚ方向位置Ｌ_ｚを用いて、式（２４）に基づき、ライダ２が出力する点群データの各計測値をライダ座標系から車体座標系に変換し、変換後のデータに基づいて、自車位置推定や自動運転制御などを実行してもよい。

他の例では、車載機１は、各ライダ２の姿勢及び位置を修正するためのアクチュエータなどの調整機構が各ライダ２に備わっている場合には、ステップＳ１０８の処理に代えて、ステップＳ１０１～Ｓ１０５で算出した各変化量の分だけライダ２の姿勢及び位置を修正するように調整機構を駆動させる制御を行ってもよい。

（変形例２）
図１に示す運転支援システムの構成は一例であり、本発明が適用可能な運転支援システムの構成は図１に示す構成に限定されない。例えば、運転支援システムは、車載機１を有する代わりに、車両の電子制御装置が図１２等に示す処理を実行してもよい。この場合、ライダ設置情報ＩＬは、例えば車両内の記憶部に記憶され、車両の電子制御装置は、ライダ２などの各種センサの出力データを受信可能に構成される。

１ 車載機
２ ライダ
３ ジャイロセンサ
４ 車体用加速度センサ
５ ライダ用加速度センサ
１０ 地図ＤＢ

Claims (1)

1. 対象物に対する距離を計測する計測部の移動体に対する姿勢を推定する推定装置であって、
   前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定部
   を有する推定装置。