運転支援装置
【課題】ＧＰＳ非受信区間において、ステレオカメラやＬＲＦなどの距離を計測する距離センサを用いて加速度センサのバイアスを補正する際に、車両の加速度，姿勢の影響により距離センサにノイズが生じ、精度良くバイアスを推定することが難しい。
【解決手段】レーザーレンジファインダやステレオカメラなどの距離センサを用いて、車両周辺に存在する物体表面の複数の点までの距離を計測し、距離センサによる計測分解能が所定の精度以上となる計測範囲を定める最大計測範囲決定手段と、最大計測範囲内で計測した点から特徴点を求める特徴点検出手段と、距離の変動から車両の挙動を逆算する車両並進・回転角計算手段から得られる自車の挙動と、加速度センサの計測値から加速度センサのパラメータを推定するセンサパラメータ推定手段により、加速度センサのバイアスを精度良く推定する。
【解決手段】レーザーレンジファインダやステレオカメラなどの距離センサを用いて、車両周辺に存在する物体表面の複数の点までの距離を計測し、距離センサによる計測分解能が所定の精度以上となる計測範囲を定める最大計測範囲決定手段と、最大計測範囲内で計測した点から特徴点を求める特徴点検出手段と、距離の変動から車両の挙動を逆算する車両並進・回転角計算手段から得られる自車の挙動と、加速度センサの計測値から加速度センサのパラメータを推定するセンサパラメータ推定手段により、加速度センサのバイアスを精度良く推定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両制御をするシステムに係り、車両の運転を支援する装置に関する。
【背景技術】
【０００２】
車両の位置推定に用いられる推測航法と呼ばれる手法では、ジャイロや加速度センサなどの慣性センサによる計測値から算出される、車両の進行方向と速度を順次加算することで車両の位置を推定する。この推測航法では、徐々に位置の推定誤差が蓄積されてゆくため、ジャイロや加速度センサの計測精度が非常に重要となり、これら慣性センサのキャリブレーションや推定位置の補正が必要である。
【０００３】
従来、ＧＰＳや車両の移動量を直接計測するセンサからの計測値を用いて、加速度センサのバイアスやジャイロのバイアス、あるいはスケールファクタのキャリブレーションを行う方法があった（特許文献１，特許文献２，特許文献６）。しかし、ＧＰＳによるセンサのキャリブレーションは、屋内やマルチパス多発地域などのようにＧＰＳ信号が取れない場合やＧＰＳによる測位精度が悪くなる場合などには使用できなくなってしまう。このような状況下では、ＧＰＳに代わり、レーザーレンジファインダ（ＬＲＦ）やステレオカメラなど、周辺の物体までの距離を計測する距離センサを用いてジャイロの補正を行うことがある。特許文献３にはＬＲＦやステレオカメラを用いた車両挙動の推定方法が記載されており、また、特許文献４にはカメラにより白線の特徴点を抽出してジャイロを補正する方法が記載されている。また、特許文献５にはステレオカメラにより、現在位置周辺の物体から直接、自車位置を補正する技術が記載されている。
【０００４】
これらの技術を用いることで、ＬＲＦやステレオカメラなどの距離センサにより、位置の補正やジャイロといったセンサのバイアス，スケールファクタを補正することが可能となる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００３−７５１７２号公報
【特許文献２】特開２００８−１７５７２１号公報
【特許文献３】特開２００６−１６０１１６号公報
【特許文献４】特開２００８−８２９３２号公報
【特許文献５】特開２０００−９７７１４号公報
【特許文献６】特開２００９−２８１７９９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかし、カメラやＬＲＦ等の距離センサを用いて、ジャイロの補正と同様の方法で加速度センサを補正しようとしても、車両加速度や車体の姿勢の傾きなどが加速度センサの値に影響する上に、車体のピッチ角やロール角といった姿勢変化がカメラやＬＲＦの計測精度に大きく影響するため、加速度センサのバイアスを精度良く補正することができない。
【課題を解決するための手段】
【０００７】
本願発明では、車両の周辺に存在する物体までの距離を計測する距離センサと、距離センサで計測した物体までの距離の変動から車両の挙動を求める車両並進・回転角計算手段と、車両並進・回転角計算手段から得られた自車の挙動と加速度センサの計測値とから加速度センサのパラメータを推定するセンサパラメータ推定手段とにより、加速度センサのバイアスを推定する。
【発明の効果】
【０００８】
距離センサの値から加速度センサのバイアスを高精度に推定することができる。
【図面の簡単な説明】
【０００９】
【図１】本発明による運転支援装置を備えた車両制御システムの構成を示す図。
【図２】距離時系列情報のデータ構成を示す図。
【図３】最大計測範囲を決定する処理のフローチャート。
【図４】距離センサパラメータのデータ構造を示す図。
【図５】距離センサパラメータを説明する図。
【図６】距離センサパラメータを説明する図。
【図７】最大計測範囲絞り込みを説明する図。
【図８】最大計測範囲絞り込みを説明する図。
【図９】道路形状による最大計測範囲の絞り込みを説明する図。
【図１０】距離センサによる計測を説明する図。
【図１１】並進距離・回転角を算出する処理のフローチャート。
【図１２】特徴点系列の対応付けを行う処理のフローチャート。
【図１３】並進距離・回転角の算出を説明する図。
【発明を実施するための形態】
【００１０】
以下、図を参照しながら、本願発明を実施するための形態について説明する。
【００１１】
図１に、本発明を用いた車両用の運転支援装置を含む車両制御システムの概略を示す。自位置から対象物までの距離を計測するＬＲＦやステレオカメラなどの距離センサ１０２からは、運転支援装置１０１に距離時系列情報が入力される。距離時系列情報のデータ構成を図２に示す。距離センサ１０２からの距離時系列情報は、距離を計測した点（計測対象点）の数である計測点数２０２と、計測点数分の計測対象点の位置２０３である。これが入力された際の、運転支援装置１０１内の内部時計による時刻が、距離を計測した時刻を表す計測時刻２０１として記録される。計測対象点の位置２０３は、距離センサ１０２内で定められた座標系で表され、以下の説明では（ｘ軸の位置，ｙ軸の位置，ｚ軸の位置）の３次元座標系で表されるものとする。この計測対象点の位置の表示は（計測対象点までの距離，計測対象点の方向）という極座標系で現すこともでき、本質的に同じ内容を示す。
【００１２】
以下、運転支援装置１０１内の処理を説明する。
【００１３】
運転支援装置１０１では、最大計測範囲決定手段１１０により、距離センサ１０２で計測した範囲の内、特徴点検出の対象とする最大計測範囲を設定して、この範囲の中で距離センサ１０２が計測した計測点から特徴点を検出する。図３を用いて最大計測範囲決定手段１１０における最大計測範囲の決定処理について説明する。この最大計測範囲決定手段１１０は、距離時系列情報の計測時刻２０１が更新される毎に起動する。
【００１４】
まず、ステップＳ３０１にて、運転支援装置１０１は距離センサパラメータ記憶手段１０３から距離センサパラメータのデータを取得する。この距離センサパラメータ記憶手段１０３に記憶されている距離センサパラメータのデータ構造を図４に示す。また、主なパラメータの概念図を図５，図６に示す。距離センサパラメータ記憶手段１０３には、距離センサパラメータが変更されたかどうか変更の有無を示すパラメータ変更フラグ４０１と、大きく分けて、取り付け角，取り付け位置，計測精度分解能，計測範囲の各パラメータが記憶されている。距離センサ１０２の取り付け角は、車体を水平にした場合を基準としたロール角４０２とピッチ角４０３、および車軸に対するヨー角４０４が保存されている。また、距離センサの取り付け位置として、車体軸に対する横ずれ量４０５と、地面からの高さ４０６が保存されている。更に使用する距離センサの計測精度分解能について奥行方向４０７，横方向４０８，高さ方向４０９の値が保存されている。同様に使用する距離センサの計測範囲について奥行方向４１０，横方向４１１，高さ方向４１２の値が保存されている。距離センサパラメータの初期値は、全て事前に距離センサ１０２の各スペックから求めた値、および計測した取り付け位置，取り付け角の値を保存しておくものとする。
【００１５】
次にステップＳ３０２にて、距離センサパラメータ記憶手段１０３から取得した距離センサパラメータのパラメータ変更フラグ４０１の値から、パラメータの値が変更されたかどうかを判断する。パラメータの値が変更されている場合、車両に対する距離センサ１０２の取り付け角や距離センサのスペック値から決められる初期最大計測範囲も変化するため、改めて初期最大計測範囲を算出するために、ステップＳ３０３へ進む。パラメータの値が変更されていない場合は、初期最大計測範囲を改めて算出する必要が無いため、ステップＳ３０５へ進む。
【００１６】
ステップＳ３０３では、ステップＳ３０１で取得した距離センサパラメータから取り付け角や取り付け位置，計測範囲の値から対象とする距離センサ１０２による距離の計測が可能な範囲を算出する。計測可能範囲は以下の（式１）で求めることができる。
【００１７】
【数１】
【００１８】
ここで、ｄは計測可能範囲（奥行方向）、ｗは計測可能範囲（横方向）、ｈは計測可能範囲（高さ方向）、Ｈｖは取り付け位置（地面からの高さ）、φは取り付け角（ピッチ角）、Ｄは計測可能範囲（奥行方向）、Ｗは計測範囲（横方向）、Ｈは計測範囲（高さ方向）を表す。
【００１９】
次にステップＳ３０４では、ステップＳ３０１で取得した距離センサパラメータの計測精度分解能から、計測可能範囲内で計測精度を保つことが可能な範囲を算出する。この計測精度を保つことが可能な範囲は、図７，図８に示すように、距離センサによる計測範囲の内側で、奥行方向，横方向，高さ方向の計測精度分解能が計測精度の閾値以内となる領域であり、その領域を初期最大計測範囲として記憶する。計測精度分解能は、車両からある距離だけ離れた地点における、計測点の間隔を現しており、奥行き，横方向，高さ方向のそれぞれに値を持つ。計測精度を保つためには、（計測点の間隔）≦（計測対象となる物体の大きさ）という条件を満たすことが必要となるため、最大計測範囲は以下のように表すことができる。
【００２０】
【数２】
【００２１】
ここでｄmax，ｗmax，ｈmaxはそれぞれ最大計測範囲（奥行方向），最大計測範囲（横方向），最大計測範囲（高さ方向）、ｒｗ，ｒｈ，ｒｄはそれぞれ距離Ｌだけ離れたときの分解能（横方向），分解能（高さ方向），分解能（奥行方向）、Ｏは計測対象物体の最小寸法を表す。
【００２２】
こうして求めた最大計測範囲を初期最大計測範囲として記憶する。
【００２３】
次にステップＳ３０５にて、自車が走行する周辺の地図データを地図データベース１０４から取得し、ステップＳ３０６にて、自車が走行する道路を含む地図データに道路形状データが存在するかを判断する。ここでは道路形状データを、曲率データ，勾配データなどであるものとする。道路形状データが存在する場合は、ステップＳ３０７へ進み、無い場合は、初期最大計測範囲を最大計測範囲として記憶し処理を終了する。
【００２４】
次にステップＳ３０７にて、ステップＳ３０４にて計算した初期最大計測範囲内における、道路形状データの変化値が閾値以上となる点があるかを判断する。道路形状データの変化値は、自車周辺のある点における道路形状データの値と、その先にある点の道路形状データの値との差分である。初期最大計測範囲内の道路形状データの変化値が閾値以下である場合は、初期最大計測範囲を最大計測範囲として記憶して処理を終了する。また、道路形状データの変化値が閾値以上である場合は、閾値以上となる点を道路形状の変局点とし、この変局点までの距離（横，高さ，奥行き）を記憶して、ステップＳ３０８へ進む。
【００２５】
ステップＳ３０８では、道路形状の変局点まで計測範囲を絞り、最大計測範囲として記憶する。距離センサが計測する対象物は、道路形状が変化している場所にある場合は、道路形状の変局点の前後でその見え方が変わる可能性が非常に高く、誤検知や誤差の原因となることが多い。そのため、自車から見て、道路形状の変局点より先の物体は観測しないようにする。図９に示すように、道路形状の変局点により絞り込んだ計測範囲が最大計測範囲として記憶され、処理を終了する。
【００２６】
特徴点検出手段１１１では、設定された最大計測範囲内の特徴点を検出する。距離センサ１０２から入力された距離時系列情報を用いて、距離センサ１０２が時刻ｔにおいて計測した車両周辺の物体表面の各計測点との相対位置関係から、物体の特徴点となる計測点の系列（特徴点系列）を検出する。図１０に特徴点系列を検出する概念図を示す。ある時刻ｔにおいて、計測された複数の計測点の位置を、距離センサ内の座標系におけるｘｙ平面，ｙｚ平面，ｚｘ平面へそれぞれ射影する。射影された各平面において、複数点を一つの単位とした最小二乗法により、その点列がなすベクトルを算出する。ここでベクトル算出対象とする複数点は、射影された各平面上で所定数の近接する点、あるいは所定距離内の点の集合としても良い。そして、隣り合うベクトルの成す角度がある一定以上である場合、特徴点となる部位であると認識し、両方のベクトルを構成する点列を全て特徴点系列として、計測点の計測時刻とともに特徴点初期位置保存手段１１２へ記憶する。この処理を距離センサ１０２で対象との距離を計測する毎に、全ての最大計測範囲内の計測点に対して行う。
【００２７】
次に、車両並進・回転角計算手段１１３にて、車両の並進変化と回転変化を計算する。車両並進・回転角計算手段１１３は、図１１に示すフローチャートの処理により車両の並進や回転角を計算する。まず、ステップＳ１１０１にて、特徴点初期位置保存手段１１２に、ある時間前の特徴点系列が存在するか判断する。存在しない場合は、並進・回転角を計算せずに処理を終了する。
【００２８】
ある時間前の特徴点系列が存在する場合は、ステップＳ１１０２に進み、現在の特徴点系列と比較して、互いの特徴点系列を対応付ける。特徴点系列の対応付けは、ある時間前の特徴点系列と現在の特徴点系列の間で各特徴点系列間の相関係数ρを計算し、相関が最も高い特徴点系列同士が対応付く特徴点系列であるとする。相関係数ρは次の（式３）を用いて求められる。
【００２９】
【数３】
【００３０】
ここで、ｘは現在の特徴点系列（ベクトル）、ｙはある時間前の特徴点系列（ベクトル）、Ｅ（ ）は期待値を表すものとする。
【００３１】
この特徴点系列の対応付けを行うステップＳ１１０２における処理のフローチャートを図１２に示す。まずステップＳ１２０１で、ある時間前の特徴点系列と現在の特徴点系列との間で特徴点系列同士の相関係数ρを求める。そしてステップＳ１２０２にて全ての特徴点系列の組み合わせで相関係数が算出されているかを判断する。もし、相関係数が算出されていない特徴点系列の組み合わせが残っていた場合は、ステップＳ１２０１に戻り、相関係数が算出されていない特徴点系列の組み合わせについて相関係数を求める処理を繰り返す。
【００３２】
次にステップＳ１２０３で、まだ対応付けがなされていない特徴点系列の組み合わせの中から相関係数が最も高い特徴点系列の組み合わせを選択する。次にステップＳ１２０４にて、対応付けられた最も高い特徴点系列の相関係数の値がある閾値より大きいかを判断する。閾値以下であれば、それ以上の対応付けられる特徴点系列の組み合わせはないとして、処理を終了する。閾値以上であれば、ステップＳ１２０５にて選択した特徴点系列の組み合わせを対応付けする。次にステップＳ１２０６にて、現在の特徴点系列とある時間前の特徴点系列の少なくとも一方の、全ての特徴点系列について対応付けがされているかを確認する。現在の特徴点系列もしくはある時間前の特徴点系列の少なくとも一方の、全ての特徴点系列について特徴点系列が対応付けられていれば、全ての特徴点系列が対応付けられたとして処理を終了する。まだ残っている特徴点系列があればステップＳ１２０３に戻り、次に相関係数の値が大きな特徴点系列の組み合わせを選んで処理を繰り返す。
【００３３】
図１１のフローチャートに戻り、上記の特徴点系列の対応付け処理を経て、決定された特徴点系列の組み合わせについて、それぞれ並進距離と回転角を求める（Ｓ１１０３）。
【００３４】
図１３を用いて並進距離と回転角を求める処理について説明する。まず、図１３（ａ）に示すように、対応付けがされた現在の特徴点系列とある時間前の特徴点系列について、現在の特徴点系列を構成する計測点をそれぞれの平面に投影した投影点の重心点と、同様に求めたある時間前の特徴点系列の重心点を一致させた際の重心点の移動量を記憶する。次に、図１３（ｂ）に示すように、現在の特徴点系列をその重心を中心として回転させ、ある時間前の特徴点系列と最も形状が近い回転角を最小二乗法により算出し、この回転角を記憶する。次に、図１３（ｃ）に示すように、前記回転角で回転させた現在の特徴点系列とある時間前の特徴点系列の各計測点の射影点間の差を最小にするような平行移動量を最小二乗法で求め、前記重心点の移動量と足し合わせて、並進距離を求める。
【００３５】
そしてステップＳ１１０４にて、上記並進距離と回転角を求める処理を全ての特徴点系列の組み合わせについて行ったかを判断し、全ての組み合わせで処理が完了している場合はステップＳ１１０５へ進む。操作が完了していない場合はステップＳ１１０３に戻り、未処理の組み合わせについて並進距離と回転角を求める処理を繰り返す。
【００３６】
次にステップＳ１１０５にて、各特徴点系列の組み合わせについて求めた並進距離，回転角、それぞれの平均値を求め、ステップＳ１１０６で、この平均値を車両の並進距離，回転角として設定する。
【００３７】
運転支援装置１０１では、ジャイロ１０５や加速度センサ１０６からの出力を定期的に受け、センサ積算手段１１４にて、ジャイロ１０５の出力値を積算して回転角データへ変換し、加速度センサ１０６の出力値を積算して並進距離データへ変換する。なお、ジャイロ１０５や加速度センサ１０６の出力周期と距離センサ１０２の出力周期は必ずしも一致する必要はない。
【００３８】
加速度センサ１０６の出力値には重力加速度も影響しているため、例えば特許文献１の技術と同様の技術を用いるなどして車両の姿勢を計算して、車両の姿勢による重力加速度の影響を取り除いて、速度データへ変換し、速度データを積算することで並進距離データへ変換する。
【００３９】
センサ出力値から回転角データ，並進距離データが算出されれば、次に、センサパラメータ推定手段１１５にて、ジャイロ１０５および加速度センサ１０６のパラメータを推定する。距離センサ１０２の出力データを用いて車両並進・回転角計算手段１１３により計算された車両の並進距離および回転角と、センサ積算手段１１４にて算出された並進距離データおよび回転角データとを比較することで速度誤差，角度誤差が計算できる。計算された速度誤差および角度誤差を現在時刻と、並進距離データおよび回転角データの比較を行った時刻差で除算すれば加速度および角速度の誤差が算出される。この誤差が時間当たりの加速度誤差および角速度誤差である。加速度誤差には、加速度センサのノイズと加速度センサのバイアスが影響しているため、加速度センサのノイズがホワイトノイズであると仮定して、カルマンフィルタにより加速度センサ１０６のバイアスを推定することができる。
【００４０】
以上のようにして、センサパラメータ推定手段１１５にて推定されたバイアス値を用いてセンサ積算手段１１４で求めた距離変位，角度変位を補正し、位置推定手段１１６にて位置を推定する。位置の推定にはカルマンフィルタを用いて、位置の誤差共分散を同時に求める。推定された位置および誤差共分散は、運転支援装置１０１から車両制御装置１０７へ出力される。
【００４１】
車両制御装置１０７は、運転支援装置１０１から出力される誤差共分散が大きくなり、自車位置の存在範囲が広がってきたことを検知して、自車位置の確からしさが低下したと判断し、車両速度を適切な速度に減速するような制御を行う。
【符号の説明】
【００４２】
１０１ 運転支援装置
１０２ 距離センサ
１０６ 加速度センサ
１０７ 車両制御装置
１１０ 最大計測範囲決定手段
１１５ センサパラメータ推定手段
１１６ 位置推定手段
【技術分野】
【０００１】
本発明は、車両制御をするシステムに係り、車両の運転を支援する装置に関する。
【背景技術】
【０００２】
車両の位置推定に用いられる推測航法と呼ばれる手法では、ジャイロや加速度センサなどの慣性センサによる計測値から算出される、車両の進行方向と速度を順次加算することで車両の位置を推定する。この推測航法では、徐々に位置の推定誤差が蓄積されてゆくため、ジャイロや加速度センサの計測精度が非常に重要となり、これら慣性センサのキャリブレーションや推定位置の補正が必要である。
【０００３】
従来、ＧＰＳや車両の移動量を直接計測するセンサからの計測値を用いて、加速度センサのバイアスやジャイロのバイアス、あるいはスケールファクタのキャリブレーションを行う方法があった（特許文献１，特許文献２，特許文献６）。しかし、ＧＰＳによるセンサのキャリブレーションは、屋内やマルチパス多発地域などのようにＧＰＳ信号が取れない場合やＧＰＳによる測位精度が悪くなる場合などには使用できなくなってしまう。このような状況下では、ＧＰＳに代わり、レーザーレンジファインダ（ＬＲＦ）やステレオカメラなど、周辺の物体までの距離を計測する距離センサを用いてジャイロの補正を行うことがある。特許文献３にはＬＲＦやステレオカメラを用いた車両挙動の推定方法が記載されており、また、特許文献４にはカメラにより白線の特徴点を抽出してジャイロを補正する方法が記載されている。また、特許文献５にはステレオカメラにより、現在位置周辺の物体から直接、自車位置を補正する技術が記載されている。
【０００４】
これらの技術を用いることで、ＬＲＦやステレオカメラなどの距離センサにより、位置の補正やジャイロといったセンサのバイアス，スケールファクタを補正することが可能となる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００３−７５１７２号公報
【特許文献２】特開２００８−１７５７２１号公報
【特許文献３】特開２００６−１６０１１６号公報
【特許文献４】特開２００８−８２９３２号公報
【特許文献５】特開２０００−９７７１４号公報
【特許文献６】特開２００９−２８１７９９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかし、カメラやＬＲＦ等の距離センサを用いて、ジャイロの補正と同様の方法で加速度センサを補正しようとしても、車両加速度や車体の姿勢の傾きなどが加速度センサの値に影響する上に、車体のピッチ角やロール角といった姿勢変化がカメラやＬＲＦの計測精度に大きく影響するため、加速度センサのバイアスを精度良く補正することができない。
【課題を解決するための手段】
【０００７】
本願発明では、車両の周辺に存在する物体までの距離を計測する距離センサと、距離センサで計測した物体までの距離の変動から車両の挙動を求める車両並進・回転角計算手段と、車両並進・回転角計算手段から得られた自車の挙動と加速度センサの計測値とから加速度センサのパラメータを推定するセンサパラメータ推定手段とにより、加速度センサのバイアスを推定する。
【発明の効果】
【０００８】
距離センサの値から加速度センサのバイアスを高精度に推定することができる。
【図面の簡単な説明】
【０００９】
【図１】本発明による運転支援装置を備えた車両制御システムの構成を示す図。
【図２】距離時系列情報のデータ構成を示す図。
【図３】最大計測範囲を決定する処理のフローチャート。
【図４】距離センサパラメータのデータ構造を示す図。
【図５】距離センサパラメータを説明する図。
【図６】距離センサパラメータを説明する図。
【図７】最大計測範囲絞り込みを説明する図。
【図８】最大計測範囲絞り込みを説明する図。
【図９】道路形状による最大計測範囲の絞り込みを説明する図。
【図１０】距離センサによる計測を説明する図。
【図１１】並進距離・回転角を算出する処理のフローチャート。
【図１２】特徴点系列の対応付けを行う処理のフローチャート。
【図１３】並進距離・回転角の算出を説明する図。
【発明を実施するための形態】
【００１０】
以下、図を参照しながら、本願発明を実施するための形態について説明する。
【００１１】
図１に、本発明を用いた車両用の運転支援装置を含む車両制御システムの概略を示す。自位置から対象物までの距離を計測するＬＲＦやステレオカメラなどの距離センサ１０２からは、運転支援装置１０１に距離時系列情報が入力される。距離時系列情報のデータ構成を図２に示す。距離センサ１０２からの距離時系列情報は、距離を計測した点（計測対象点）の数である計測点数２０２と、計測点数分の計測対象点の位置２０３である。これが入力された際の、運転支援装置１０１内の内部時計による時刻が、距離を計測した時刻を表す計測時刻２０１として記録される。計測対象点の位置２０３は、距離センサ１０２内で定められた座標系で表され、以下の説明では（ｘ軸の位置，ｙ軸の位置，ｚ軸の位置）の３次元座標系で表されるものとする。この計測対象点の位置の表示は（計測対象点までの距離，計測対象点の方向）という極座標系で現すこともでき、本質的に同じ内容を示す。
【００１２】
以下、運転支援装置１０１内の処理を説明する。
【００１３】
運転支援装置１０１では、最大計測範囲決定手段１１０により、距離センサ１０２で計測した範囲の内、特徴点検出の対象とする最大計測範囲を設定して、この範囲の中で距離センサ１０２が計測した計測点から特徴点を検出する。図３を用いて最大計測範囲決定手段１１０における最大計測範囲の決定処理について説明する。この最大計測範囲決定手段１１０は、距離時系列情報の計測時刻２０１が更新される毎に起動する。
【００１４】
まず、ステップＳ３０１にて、運転支援装置１０１は距離センサパラメータ記憶手段１０３から距離センサパラメータのデータを取得する。この距離センサパラメータ記憶手段１０３に記憶されている距離センサパラメータのデータ構造を図４に示す。また、主なパラメータの概念図を図５，図６に示す。距離センサパラメータ記憶手段１０３には、距離センサパラメータが変更されたかどうか変更の有無を示すパラメータ変更フラグ４０１と、大きく分けて、取り付け角，取り付け位置，計測精度分解能，計測範囲の各パラメータが記憶されている。距離センサ１０２の取り付け角は、車体を水平にした場合を基準としたロール角４０２とピッチ角４０３、および車軸に対するヨー角４０４が保存されている。また、距離センサの取り付け位置として、車体軸に対する横ずれ量４０５と、地面からの高さ４０６が保存されている。更に使用する距離センサの計測精度分解能について奥行方向４０７，横方向４０８，高さ方向４０９の値が保存されている。同様に使用する距離センサの計測範囲について奥行方向４１０，横方向４１１，高さ方向４１２の値が保存されている。距離センサパラメータの初期値は、全て事前に距離センサ１０２の各スペックから求めた値、および計測した取り付け位置，取り付け角の値を保存しておくものとする。
【００１５】
次にステップＳ３０２にて、距離センサパラメータ記憶手段１０３から取得した距離センサパラメータのパラメータ変更フラグ４０１の値から、パラメータの値が変更されたかどうかを判断する。パラメータの値が変更されている場合、車両に対する距離センサ１０２の取り付け角や距離センサのスペック値から決められる初期最大計測範囲も変化するため、改めて初期最大計測範囲を算出するために、ステップＳ３０３へ進む。パラメータの値が変更されていない場合は、初期最大計測範囲を改めて算出する必要が無いため、ステップＳ３０５へ進む。
【００１６】
ステップＳ３０３では、ステップＳ３０１で取得した距離センサパラメータから取り付け角や取り付け位置，計測範囲の値から対象とする距離センサ１０２による距離の計測が可能な範囲を算出する。計測可能範囲は以下の（式１）で求めることができる。
【００１７】
【数１】
【００１８】
ここで、ｄは計測可能範囲（奥行方向）、ｗは計測可能範囲（横方向）、ｈは計測可能範囲（高さ方向）、Ｈｖは取り付け位置（地面からの高さ）、φは取り付け角（ピッチ角）、Ｄは計測可能範囲（奥行方向）、Ｗは計測範囲（横方向）、Ｈは計測範囲（高さ方向）を表す。
【００１９】
次にステップＳ３０４では、ステップＳ３０１で取得した距離センサパラメータの計測精度分解能から、計測可能範囲内で計測精度を保つことが可能な範囲を算出する。この計測精度を保つことが可能な範囲は、図７，図８に示すように、距離センサによる計測範囲の内側で、奥行方向，横方向，高さ方向の計測精度分解能が計測精度の閾値以内となる領域であり、その領域を初期最大計測範囲として記憶する。計測精度分解能は、車両からある距離だけ離れた地点における、計測点の間隔を現しており、奥行き，横方向，高さ方向のそれぞれに値を持つ。計測精度を保つためには、（計測点の間隔）≦（計測対象となる物体の大きさ）という条件を満たすことが必要となるため、最大計測範囲は以下のように表すことができる。
【００２０】
【数２】
【００２１】
ここでｄmax，ｗmax，ｈmaxはそれぞれ最大計測範囲（奥行方向），最大計測範囲（横方向），最大計測範囲（高さ方向）、ｒｗ，ｒｈ，ｒｄはそれぞれ距離Ｌだけ離れたときの分解能（横方向），分解能（高さ方向），分解能（奥行方向）、Ｏは計測対象物体の最小寸法を表す。
【００２２】
こうして求めた最大計測範囲を初期最大計測範囲として記憶する。
【００２３】
次にステップＳ３０５にて、自車が走行する周辺の地図データを地図データベース１０４から取得し、ステップＳ３０６にて、自車が走行する道路を含む地図データに道路形状データが存在するかを判断する。ここでは道路形状データを、曲率データ，勾配データなどであるものとする。道路形状データが存在する場合は、ステップＳ３０７へ進み、無い場合は、初期最大計測範囲を最大計測範囲として記憶し処理を終了する。
【００２４】
次にステップＳ３０７にて、ステップＳ３０４にて計算した初期最大計測範囲内における、道路形状データの変化値が閾値以上となる点があるかを判断する。道路形状データの変化値は、自車周辺のある点における道路形状データの値と、その先にある点の道路形状データの値との差分である。初期最大計測範囲内の道路形状データの変化値が閾値以下である場合は、初期最大計測範囲を最大計測範囲として記憶して処理を終了する。また、道路形状データの変化値が閾値以上である場合は、閾値以上となる点を道路形状の変局点とし、この変局点までの距離（横，高さ，奥行き）を記憶して、ステップＳ３０８へ進む。
【００２５】
ステップＳ３０８では、道路形状の変局点まで計測範囲を絞り、最大計測範囲として記憶する。距離センサが計測する対象物は、道路形状が変化している場所にある場合は、道路形状の変局点の前後でその見え方が変わる可能性が非常に高く、誤検知や誤差の原因となることが多い。そのため、自車から見て、道路形状の変局点より先の物体は観測しないようにする。図９に示すように、道路形状の変局点により絞り込んだ計測範囲が最大計測範囲として記憶され、処理を終了する。
【００２６】
特徴点検出手段１１１では、設定された最大計測範囲内の特徴点を検出する。距離センサ１０２から入力された距離時系列情報を用いて、距離センサ１０２が時刻ｔにおいて計測した車両周辺の物体表面の各計測点との相対位置関係から、物体の特徴点となる計測点の系列（特徴点系列）を検出する。図１０に特徴点系列を検出する概念図を示す。ある時刻ｔにおいて、計測された複数の計測点の位置を、距離センサ内の座標系におけるｘｙ平面，ｙｚ平面，ｚｘ平面へそれぞれ射影する。射影された各平面において、複数点を一つの単位とした最小二乗法により、その点列がなすベクトルを算出する。ここでベクトル算出対象とする複数点は、射影された各平面上で所定数の近接する点、あるいは所定距離内の点の集合としても良い。そして、隣り合うベクトルの成す角度がある一定以上である場合、特徴点となる部位であると認識し、両方のベクトルを構成する点列を全て特徴点系列として、計測点の計測時刻とともに特徴点初期位置保存手段１１２へ記憶する。この処理を距離センサ１０２で対象との距離を計測する毎に、全ての最大計測範囲内の計測点に対して行う。
【００２７】
次に、車両並進・回転角計算手段１１３にて、車両の並進変化と回転変化を計算する。車両並進・回転角計算手段１１３は、図１１に示すフローチャートの処理により車両の並進や回転角を計算する。まず、ステップＳ１１０１にて、特徴点初期位置保存手段１１２に、ある時間前の特徴点系列が存在するか判断する。存在しない場合は、並進・回転角を計算せずに処理を終了する。
【００２８】
ある時間前の特徴点系列が存在する場合は、ステップＳ１１０２に進み、現在の特徴点系列と比較して、互いの特徴点系列を対応付ける。特徴点系列の対応付けは、ある時間前の特徴点系列と現在の特徴点系列の間で各特徴点系列間の相関係数ρを計算し、相関が最も高い特徴点系列同士が対応付く特徴点系列であるとする。相関係数ρは次の（式３）を用いて求められる。
【００２９】
【数３】
【００３０】
ここで、ｘは現在の特徴点系列（ベクトル）、ｙはある時間前の特徴点系列（ベクトル）、Ｅ（ ）は期待値を表すものとする。
【００３１】
この特徴点系列の対応付けを行うステップＳ１１０２における処理のフローチャートを図１２に示す。まずステップＳ１２０１で、ある時間前の特徴点系列と現在の特徴点系列との間で特徴点系列同士の相関係数ρを求める。そしてステップＳ１２０２にて全ての特徴点系列の組み合わせで相関係数が算出されているかを判断する。もし、相関係数が算出されていない特徴点系列の組み合わせが残っていた場合は、ステップＳ１２０１に戻り、相関係数が算出されていない特徴点系列の組み合わせについて相関係数を求める処理を繰り返す。
【００３２】
次にステップＳ１２０３で、まだ対応付けがなされていない特徴点系列の組み合わせの中から相関係数が最も高い特徴点系列の組み合わせを選択する。次にステップＳ１２０４にて、対応付けられた最も高い特徴点系列の相関係数の値がある閾値より大きいかを判断する。閾値以下であれば、それ以上の対応付けられる特徴点系列の組み合わせはないとして、処理を終了する。閾値以上であれば、ステップＳ１２０５にて選択した特徴点系列の組み合わせを対応付けする。次にステップＳ１２０６にて、現在の特徴点系列とある時間前の特徴点系列の少なくとも一方の、全ての特徴点系列について対応付けがされているかを確認する。現在の特徴点系列もしくはある時間前の特徴点系列の少なくとも一方の、全ての特徴点系列について特徴点系列が対応付けられていれば、全ての特徴点系列が対応付けられたとして処理を終了する。まだ残っている特徴点系列があればステップＳ１２０３に戻り、次に相関係数の値が大きな特徴点系列の組み合わせを選んで処理を繰り返す。
【００３３】
図１１のフローチャートに戻り、上記の特徴点系列の対応付け処理を経て、決定された特徴点系列の組み合わせについて、それぞれ並進距離と回転角を求める（Ｓ１１０３）。
【００３４】
図１３を用いて並進距離と回転角を求める処理について説明する。まず、図１３（ａ）に示すように、対応付けがされた現在の特徴点系列とある時間前の特徴点系列について、現在の特徴点系列を構成する計測点をそれぞれの平面に投影した投影点の重心点と、同様に求めたある時間前の特徴点系列の重心点を一致させた際の重心点の移動量を記憶する。次に、図１３（ｂ）に示すように、現在の特徴点系列をその重心を中心として回転させ、ある時間前の特徴点系列と最も形状が近い回転角を最小二乗法により算出し、この回転角を記憶する。次に、図１３（ｃ）に示すように、前記回転角で回転させた現在の特徴点系列とある時間前の特徴点系列の各計測点の射影点間の差を最小にするような平行移動量を最小二乗法で求め、前記重心点の移動量と足し合わせて、並進距離を求める。
【００３５】
そしてステップＳ１１０４にて、上記並進距離と回転角を求める処理を全ての特徴点系列の組み合わせについて行ったかを判断し、全ての組み合わせで処理が完了している場合はステップＳ１１０５へ進む。操作が完了していない場合はステップＳ１１０３に戻り、未処理の組み合わせについて並進距離と回転角を求める処理を繰り返す。
【００３６】
次にステップＳ１１０５にて、各特徴点系列の組み合わせについて求めた並進距離，回転角、それぞれの平均値を求め、ステップＳ１１０６で、この平均値を車両の並進距離，回転角として設定する。
【００３７】
運転支援装置１０１では、ジャイロ１０５や加速度センサ１０６からの出力を定期的に受け、センサ積算手段１１４にて、ジャイロ１０５の出力値を積算して回転角データへ変換し、加速度センサ１０６の出力値を積算して並進距離データへ変換する。なお、ジャイロ１０５や加速度センサ１０６の出力周期と距離センサ１０２の出力周期は必ずしも一致する必要はない。
【００３８】
加速度センサ１０６の出力値には重力加速度も影響しているため、例えば特許文献１の技術と同様の技術を用いるなどして車両の姿勢を計算して、車両の姿勢による重力加速度の影響を取り除いて、速度データへ変換し、速度データを積算することで並進距離データへ変換する。
【００３９】
センサ出力値から回転角データ，並進距離データが算出されれば、次に、センサパラメータ推定手段１１５にて、ジャイロ１０５および加速度センサ１０６のパラメータを推定する。距離センサ１０２の出力データを用いて車両並進・回転角計算手段１１３により計算された車両の並進距離および回転角と、センサ積算手段１１４にて算出された並進距離データおよび回転角データとを比較することで速度誤差，角度誤差が計算できる。計算された速度誤差および角度誤差を現在時刻と、並進距離データおよび回転角データの比較を行った時刻差で除算すれば加速度および角速度の誤差が算出される。この誤差が時間当たりの加速度誤差および角速度誤差である。加速度誤差には、加速度センサのノイズと加速度センサのバイアスが影響しているため、加速度センサのノイズがホワイトノイズであると仮定して、カルマンフィルタにより加速度センサ１０６のバイアスを推定することができる。
【００４０】
以上のようにして、センサパラメータ推定手段１１５にて推定されたバイアス値を用いてセンサ積算手段１１４で求めた距離変位，角度変位を補正し、位置推定手段１１６にて位置を推定する。位置の推定にはカルマンフィルタを用いて、位置の誤差共分散を同時に求める。推定された位置および誤差共分散は、運転支援装置１０１から車両制御装置１０７へ出力される。
【００４１】
車両制御装置１０７は、運転支援装置１０１から出力される誤差共分散が大きくなり、自車位置の存在範囲が広がってきたことを検知して、自車位置の確からしさが低下したと判断し、車両速度を適切な速度に減速するような制御を行う。
【符号の説明】
【００４２】
１０１ 運転支援装置
１０２ 距離センサ
１０６ 加速度センサ
１０７ 車両制御装置
１１０ 最大計測範囲決定手段
１１５ センサパラメータ推定手段
１１６ 位置推定手段
【特許請求の範囲】
【請求項１】
車両の姿勢または移動速度の変化を計測するセンサと、車両周辺の物体までの距離を計測する距離計測手段を備え、特定した車両位置を出力する運転支援装置において、
前記距離計測手段の特性情報を記憶した記憶手段と、
地形情報を記憶した地図データベースと、
前記記憶手段の前記特性情報と既に計測されている現在位置周辺の地形情報に基づき、前記距離計測手段による計測データ処理対象範囲を定める範囲決定手段と、
前記範囲決定手段による計測データ処理対象範囲における物体までの距離結果から特徴点を検出する特徴点検出手段と、
既に得られている特徴点と現在得られている特徴点を対応させて、特徴点の位置と向きの変化から車両挙動を求める車両並進・回転角計算手段と、
前記センサの計測値と前記車両挙動から前記センサのパラメータを推定するセンサパラメータ推定手段と、
推定されたセンサパラメータを用いて補正された位置を算出する位置推定手段と
を有することを特徴とする運転支援装置。
【請求項２】
請求項１に記載の運転支援装置において、
前記距離計測手段の特性情報は、少なくとも当該距離計測手段の車体への取り付け角度とを含み、
前記範囲決定手段は、前記特性情報に基づき前記距離計測手段における計測精度が所定の精度より良い範囲を計測データ処理対象範囲とすることを特徴とする運転支援装置。
【請求項３】
請求項２に記載の運転支援装置において、
前記範囲決定手段は、前記地図データベースから取得した現在位置周辺の地形情報に基づき、勾配の変化を含む路面の形状変化が所定の値より小さい範囲を計測データ処理対象範囲とすることを特徴とする運転支援装置。
【請求項４】
請求項１ないし３に記載の運転支援装置において、前記位置推定手段による車両位置の確からしさを基に減速制御を行うことを特徴とする運転支援装置。
【請求項５】
車両の姿勢または移動速度の変化を計測するセンサと、車両周辺の物体までの距離を計測する距離計測手段を備え、特定した車両位置を出力する運転支援方法において、
記憶手段に記憶された前記距離計測手段の特性情報と地図データベースに記憶された既に計測されている現在位置周辺の地形情報に基づき、前記距離計測手段による計測データ処理対象範囲を定める範囲決定処理と、
当該範囲決定処理による計測データ処理対象範囲内における物体までの距離結果から特徴点を検出する特徴点検出処理と、
既に得られている特徴点と現在得られている特徴点を対応させて、特徴点の位置と向きの変化から車両挙動を求める車両並進・回転角計算処理と、
前記センサの計測値と前記車両挙動から前記センサのパラメータを推定するセンサパラメータ推定処理と、
推定されたセンサパラメータを用いて補正された位置を算出する位置推定処理と
を有することを特徴とする運転支援方法。
【請求項６】
請求項５に記載の運転支援方法において、
前記距離計測手段の特性情報は、少なくとも当該距離計測手段の車体への取り付け角度とを含み、
前記範囲決定処理では、前記特性情報に基づき前記距離計測手段における計測精度が所定の精度より良い範囲を計測データ処理対象範囲とすることを特徴とする運転支援方法。
【請求項７】
請求項６に記載の運転支援方法において、
前記範囲決定方法では、前記地図データベースから取得した現在位置周辺の地形情報に基づき、勾配の変化を含む路面の形状変化が所定の値より小さい範囲を計測データ処理対象範囲とすることを特徴とする運転支援方法。
【請求項１】
車両の姿勢または移動速度の変化を計測するセンサと、車両周辺の物体までの距離を計測する距離計測手段を備え、特定した車両位置を出力する運転支援装置において、
前記距離計測手段の特性情報を記憶した記憶手段と、
地形情報を記憶した地図データベースと、
前記記憶手段の前記特性情報と既に計測されている現在位置周辺の地形情報に基づき、前記距離計測手段による計測データ処理対象範囲を定める範囲決定手段と、
前記範囲決定手段による計測データ処理対象範囲における物体までの距離結果から特徴点を検出する特徴点検出手段と、
既に得られている特徴点と現在得られている特徴点を対応させて、特徴点の位置と向きの変化から車両挙動を求める車両並進・回転角計算手段と、
前記センサの計測値と前記車両挙動から前記センサのパラメータを推定するセンサパラメータ推定手段と、
推定されたセンサパラメータを用いて補正された位置を算出する位置推定手段と
を有することを特徴とする運転支援装置。
【請求項２】
請求項１に記載の運転支援装置において、
前記距離計測手段の特性情報は、少なくとも当該距離計測手段の車体への取り付け角度とを含み、
前記範囲決定手段は、前記特性情報に基づき前記距離計測手段における計測精度が所定の精度より良い範囲を計測データ処理対象範囲とすることを特徴とする運転支援装置。
【請求項３】
請求項２に記載の運転支援装置において、
前記範囲決定手段は、前記地図データベースから取得した現在位置周辺の地形情報に基づき、勾配の変化を含む路面の形状変化が所定の値より小さい範囲を計測データ処理対象範囲とすることを特徴とする運転支援装置。
【請求項４】
請求項１ないし３に記載の運転支援装置において、前記位置推定手段による車両位置の確からしさを基に減速制御を行うことを特徴とする運転支援装置。
【請求項５】
車両の姿勢または移動速度の変化を計測するセンサと、車両周辺の物体までの距離を計測する距離計測手段を備え、特定した車両位置を出力する運転支援方法において、
記憶手段に記憶された前記距離計測手段の特性情報と地図データベースに記憶された既に計測されている現在位置周辺の地形情報に基づき、前記距離計測手段による計測データ処理対象範囲を定める範囲決定処理と、
当該範囲決定処理による計測データ処理対象範囲内における物体までの距離結果から特徴点を検出する特徴点検出処理と、
既に得られている特徴点と現在得られている特徴点を対応させて、特徴点の位置と向きの変化から車両挙動を求める車両並進・回転角計算処理と、
前記センサの計測値と前記車両挙動から前記センサのパラメータを推定するセンサパラメータ推定処理と、
推定されたセンサパラメータを用いて補正された位置を算出する位置推定処理と
を有することを特徴とする運転支援方法。
【請求項６】
請求項５に記載の運転支援方法において、
前記距離計測手段の特性情報は、少なくとも当該距離計測手段の車体への取り付け角度とを含み、
前記範囲決定処理では、前記特性情報に基づき前記距離計測手段における計測精度が所定の精度より良い範囲を計測データ処理対象範囲とすることを特徴とする運転支援方法。
【請求項７】
請求項６に記載の運転支援方法において、
前記範囲決定方法では、前記地図データベースから取得した現在位置周辺の地形情報に基づき、勾配の変化を含む路面の形状変化が所定の値より小さい範囲を計測データ処理対象範囲とすることを特徴とする運転支援方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【公開番号】特開２０１２−１３３６９６（Ｐ２０１２−１３３６９６Ａ）
【公開日】平成２４年７月１２日（２０１２．７．１２）
【国際特許分類】
【出願番号】特願２０１０−２８６９２４（Ｐ２０１０−２８６９２４）
【出願日】平成２２年１２月２４日（２０１０．１２．２４）
【出願人】（０００００５１０８）株式会社日立製作所 (27,607)
【Ｆターム（参考）】
【公開日】平成２４年７月１２日（２０１２．７．１２）
【国際特許分類】
【出願日】平成２２年１２月２４日（２０１０．１２．２４）
【出願人】（０００００５１０８）株式会社日立製作所 (27,607)
【Ｆターム（参考）】
[ Back to top ]