JP6020729B2 - 車両位置姿勢角推定装置及び車両位置姿勢角推定方法 - Google Patents

Description

本発明は、パーティクルフィルタを用いて車両の位置及び姿勢角を推定する車両位置姿勢角推定装置及びその方法に関する。

パーティクルフィルタを用いて移動体の位置と姿勢角を算出する技術として、従来では特許文献１が開示されている。この技術では、オドメトリによって算出した移動体の位置と姿勢角の近傍に複数のパーティクルを散布する。そして、これらのパーティクルの中で移動体に搭載されたレーザセンサの測定値と最も合致したパーティクルを、この移動体の位置と姿勢角の真値として算出する。

このとき、パーティクルを散布する存在分布範囲は、パーティクルフィルタを用いて算出した位置と姿勢角がどれだけ補正されていたかを時々刻々算出し、この補正量の履歴に応じて設定されていた。

特開２０１０−２２４７５５号公報

しかしながら、上述した従来の技術では、車両の進行方向への誤差は考慮されているが、車幅方向への誤差については十分に考慮されていなかった。特に、車速が高いときに車両が旋回すると、車両には車幅方向への大きな移動が生じるので、車幅方向への誤差は大きくなるが、従来ではそのような時の対応は十分に行われていなかった。そのためパーティクルの存在分布範囲の設定に遅れが生じ、適正に設定することができないという問題点があった。

そこで、本発明は、上述した実情に鑑みて提案されたものであり、車両の速度が高くなって車両に車幅方向への移動が生じた場合でもパーティクルの存在分布範囲を適正に設定することのできる車両位置姿勢角推定装置及びその方法を提供することを目的とする。

上述した課題を解決するために、本発明の一態様に係る車両位置姿勢角推定装置及びその方法は、パーティクルフィルタを用いて所定範囲のパーティクルの存在分布範囲を設定する。そして、設定した存在分布範囲内にパーティクルを散布して車両の周囲環境を撮像した画像から車両の位置及び姿勢角を推定し、車両の速度が高くなると、設定した存在分布範囲を車両の車幅方向に広げる。

図１は、本発明の第１実施形態に係る車両位置姿勢角推定装置を備えた車両位置姿勢角推定システムの構成を示すブロック図である。 図２は、本発明の第１実施形態に係る車両位置姿勢角推定装置による車両の位置姿勢角推定処理の処理手順を示すフローチャートである。 図３は、定速円旋回中の車両の旋回中心と各タイヤの横すべり角を模式的に表した図である。 図４は、本発明の第１実施形態に係る車両位置姿勢角推定装置による速度に応じてパーティクルの存在分布範囲を車両の前後方向に広げる方法を説明するための図である。 図５は、本発明の第１実施形態に係る車両位置姿勢角推定装置による速度に応じてパーティクルの存在分布範囲を車両の車幅方向に広げる方法を説明するための図である。 図６は、本発明の第１実施形態に係る車両位置姿勢角推定装置による速度に応じてパーティクルの存在分布範囲を車両のヨー角方向に広げる方法を説明するための図である。 図７は、本発明の第２実施形態に係る車両位置姿勢角推定装置を備えた車両位置姿勢角推定システムの構成を示すブロック図である。 図８は、車両が転舵した際のタイヤ横力とヨーモーメントを説明するための図である。 図９は、本発明の第２実施形態に係る車両位置姿勢角推定装置によるヨーレートに応じてパーティクルの存在分布範囲を車両の車幅方向及びヨー角方向に広げる方法を説明するための図である。 図１０は、本発明の第３実施形態に係る車両位置姿勢角推定装置を備えた車両位置姿勢角推定システムの構成を示すブロック図である。 図１１は、本発明の第３実施形態に係る車両位置姿勢角推定装置によるステアリング操舵角に応じてパーティクルの存在分布範囲を車両の前後方向、車幅方向及びヨー角方向に広げる方法を説明するための図である。 図１２は、本発明に係る車両位置姿勢角推定装置によるパーティクルの散布方法を説明するための図である。

以下、本発明を適用した第１〜第３実施形態について図面を参照して説明する。

［第１実施形態］
［車両位置姿勢角推定システムの構成］
図１は、本実施形態に係る車両位置姿勢角推定装置を搭載した車両位置姿勢角推定システムの構成を示すブロック図である。図１に示すように、本実施形態に係る車両位置姿勢角推定システムは、ＥＣＵ１と、カメラ２と、三次元地図データベース３と、車両センサ群４とを備えている。

ここで、ＥＣＵ１は、ＲＯＭ、ＲＡＭ、演算回路等によって構成された電子制御ユニットであり、本実施形態に係る車両位置姿勢角推定装置１０を備えている。尚、ＥＣＵ１は他の制御に用いるＥＣＵと兼用してもよい。

カメラ２は、例えばＣＣＤ等の固体撮像素子を用いたものであり、本実施形態では、車両のフロントバンパーに光軸が水平で、尚且つ車両前方が撮像可能となるように設置されている。撮像された画像はＥＣＵ１へ送信される。

三次元地図データベース３は、例えば路面表示を含む周囲環境のエッジ等の三次元位置情報を記憶している。本実施形態では、少なくとも道路端部を示す区画線や縁石等の位置とその向きの三次元情報が記録されており、この他に白線、停止線、横断歩道、路面マーク等の路面表示の他に、建物等の構造物のエッジ情報も含まれている。尚、三次元地図データベース３のデータフォーマットとして、道路端部等の各地図情報は、エッジの集合体として定義されている。エッジが長い直線の場合には、例えば１ｍ毎に区切られるため、極端に長いエッジは存在しない。直線の場合には、各エッジは、直線の両端点を示す三次元位置情報を持っている。曲線の場合には、各エッジは、曲線の両端点と中央点を示す三次元位置情報を持っている。

車両センサ群４は、ＧＰＳ受信機４１と、アクセルセンサ４２と、ステアリングセンサ４３と、ブレーキセンサ４４と、車速センサ４５と、加速度センサ４６と、車輪速センサ４７と、ヨーレートセンサ４８とを備えている。車両センサ群４は、ＥＣＵ１に接続され、各センサ４１〜４８によって検出された各種の検出値をＥＣＵ１に供給する。ＥＣＵ１は、車両センサ群４の出力値を用いることによって、車両の概位置を算出したり、単位時間に車両が進んだ移動量を示すオドメトリを算出する。

車両位置姿勢角推定装置１０は、車両の周囲環境を撮像した画像と三次元地図データとのマッチングを行って車両の位置及び姿勢角を推定するための装置である。そして、特定のプログラムを実行することにより、エッジ画像算出部１２、オドメトリ算出部１４、車速検出部１５、パーティクル存在分布範囲設定部１６、パーティクル散布部１８、投影画像作成部２０、尤度算出部２２及び位置姿勢角推定部２４として動作する。

エッジ画像算出部１２は、カメラ２から車両の周囲環境を撮像した画像を取得し、この画像からエッジを検出してエッジ画像を算出する。カメラ２で撮像された画像には、自車両の位置及び姿勢角を推定するために必要な路面情報として、少なくとも道路端部を示す区画線や縁石が撮像されている。また、この他に白線、停止線、横断歩道、路面マーク等の路面表示が撮像されていてもよい。

オドメトリ算出部１４は、車両センサ群４から得られる各種センサ値を用いて、車両の単位時間当たりの移動量であるオドメトリを算出する。

車速検出部１５は、車速センサ４５で計測されたセンサ値を取得することによって、車両の速度を検出する。

パーティクル存在分布範囲設定部１６は、オドメトリ算出部１４で算出されたオドメトリの分だけ移動した位置と姿勢角の近傍に所定範囲のパーティクルの存在分布範囲を設定し、車両の走行状態に応じてパーティクルの存在分布範囲を補正する。具体的には、図１２に示すように、１ループ前に推定されていた車両Ｖ（ｔ１）の位置及び姿勢角のパーティクルＰと周囲のパーティクルＰ１〜Ｐ５をオドメトリ分だけ移動させ、パーティクルの存在分布範囲を設定して補正する。本実施形態では、車両の走行状態として速度が高くなると、パーティクルの存在分布範囲を車両の前後方向、車幅方向及びヨー角方向に広げる補正を行っている。

パーティクル散布部１８は、パーティクル存在分布範囲設定部１６によって設定されたパーティクルの存在分布範囲内にパーティクルを散布する。図１２に示すように、パーティクル散布部１８は、新たな車両Ｖ（ｔ２）の位置及び姿勢角を推定するためにパーティクルＰ１０〜１５を設定する。

投影画像作成部２０は、パーティクル散布部１８によって散布されたパーティクルのそれぞれについて投影画像を作成する。例えば、三次元地図データベース３に記憶されたエッジ等の三次元位置情報を、各パーティクルの位置及び姿勢角からカメラで撮像した画像となるように投影変換して投影画像を作成する。

尤度算出部２２は、投影画像作成部２０によって作成された投影画像とエッジ画像算出部１２によって算出されたエッジ画像とを比較して、パーティクルのそれぞれについて尤度を算出する。この尤度とは、各パーティクルの位置及び姿勢角が、どれぐらい実際の車両の位置及び姿勢角に対して尤もらしいかを示す指標であり、投影画像とエッジ画像との一致度が高いほど、尤度が高くなるように設定されている。

位置姿勢角推定部２４は、尤度算出部２２によって算出された尤度に基づいて車両の位置及び姿勢角を推定する。例えば、尤度が最も高いパーティクルの位置及び姿勢角を、車両の実際の位置及び姿勢角の推定結果として算出する。

尚、本実施形態では車両の６自由度の位置（前後方向，車幅方向，上下方向）及び姿勢角（ロール，ピッチ，ヨー）を求めている。しかし、例えばサスペンション等を備えていない工場等で使用される無人搬送車のように３自由度の位置（前後方向，横方向）及び姿勢角（ヨー）を推定する場合でも本技術を適用することは可能である。具体的に、このような車両では、上下方向の位置と姿勢角のロール及びピッチは固定となるので、予めこれらのパラメータを計測しておいて利用すればよい。

［車両の位置姿勢角推定処理の手順］
次に、本実施形態に係る車両の位置姿勢角推定処理の手順を図２のフローチャートを参照して説明する。尚、本実施形態では、車両の位置情報として前後方向、車幅方向、上下方向、姿勢角情報としてロール、ピッチ、ヨーの合計６自由度の位置と姿勢角を推定する。ただし、ロールは車両の前後方向を軸とした回転方向であり、ピッチは車両の車幅方向を軸とした回転方向であり、ヨーは車両の上下方向を軸とした回転方向である（図１２参照）。

また、以下で説明する車両の位置姿勢角推定処理は、例えば１００ｍｓｅｃ程度の間隔で連続的に行われる。

図２に示すように、まずステップＳ１１０において、エッジ画像算出部１２は、カメラ２の画像からエッジ画像を算出する。本実施形態におけるエッジとは、画素の輝度が鋭敏に変化している箇所を指している。エッジの検出方法としては、例えばＣａｎｎｙ法を用いることができる。この他にも微分エッジ検出など様々な手法を使用してもよい。

また、エッジ画像算出部１２は、カメラ２の画像からエッジの輝度変化の方向やエッジ近辺のカラーなどについても抽出することが望ましい。これにより、後述するステップＳ１６０及びステップＳ１７０において、三次元地図データベース３に記録されているこれらエッジ以外の情報についても利用して尤度を設定し、自車両の位置及び姿勢角を算出することが可能になる。

次に、ステップＳ１２０において、オドメトリ算出部１４は、車両センサ群４から得られるセンサ値に基づいて、１ループ前のステップＳ１２０で算出した時点から現在までの車両の移動量であるオドメトリを算出する。なお、プログラムを開始して最初のループの場合には、オドメトリをゼロとして算出する。

オドメトリ算出部１４は、オドメトリの算出方法として、車両運動を平面上に限定した上で、左右輪の車輪速センサ４７のエンコーダ値の差からヨー角方向の旋回量（旋回角）を算出する。そして、前後方向と車幅方向の移動量は、各車輪の車輪速センサ４７のエンコーダ値から平均の移動量を求め、この移動量に対してヨー角方向の旋回角の余弦と正弦を取ることで求めることができる。また、各車輪の車輪速とヨーレートから単位時間での移動量と回転量を算出してもよい。さらに、車輪速を車速やＧＰＳ受信機４１の測位値の差分で代用してもよく、ヨーレートを操舵角で代用してもよい。なお、オドメトリの算出方法は、様々な算出手法が考えられるが、オドメトリを算出できればどの手法を用いてもよい。

特に、車幅方向やヨー角方向の移動量をより正確に算出するために、例えば車速約１０ｋｍ／ｈ以下の極低速ではアッカーマンステアリングジオメトリ（自動車の運動と制御第３章、安部正人著、山海堂発行）に従ってオドメトリを求めてもよい。また、それ以上の車速ではタイヤの横滑り角を考慮できる線形２輪モデルの運動方程式（自動車の運動と制御第３章３．２．１節Ｐ．５６（３．１２）及び（３．１３）式、安部正人著、山海堂発行）を用いて算出するとなお良い。ただし、線形２輪モデルの運動方程式を用いる場合には予め各車輪のタイヤパラメータ（コーナーリングパラメータ）を測定しておく必要がある。

次に、ステップＳ１３０において、パーティクル存在分布範囲設定部１６は、１ループ前のステップＳ１７０で推定した各パーティクルの位置及び姿勢角を、今回のステップＳ１２０で算出したオドメトリの分だけ移動させる。ただし、プログラムを開始して初めてのループの場合には、前回の車両位置情報がないので、車両センサ群４に含まれるＧＰＳ受信機４１からのデータを初期位置情報とする。また、前回の停車時に最後に算出した車両位置及び姿勢角を記憶しておき、初期位置及び姿勢角情報としてもよい。

そして、オドメトリの分だけ移動させた車両の位置と姿勢角の近傍に車両の動特性や走行状態を考慮してパーティクルの存在分布範囲を設定する。このとき、本実施形態では、車両の走行状態を考慮して速度が高くなると、パーティクルの存在分布範囲を車両の前後方向、車幅方向及びヨー角方向に広げている。

ここで、図３を参照して、パーティクルの存在分布範囲を広げる理由を説明する。図３は、定速円旋回中の車両の旋回中心と各タイヤの横すべり角を模式的に表した図であり、図３（ａ）は極低速での定常円旋回の場合、図３（ｂ）は遠心力の働く定常円旋回の場合である。

図３（ａ）に示すように、極低速では車両に遠心力が働かないため、車両はタイヤの向きに沿って進み、車幅方向への移動は殆ど発生しないので、車幅方向へのオドメトリの誤差は発生しにくい。

しかし、図３（ｂ）に示すように、車速が高くなってくると、車両に遠心力が働くので、各車輪に横滑り角δがついて車両に車幅方向への運動が発生する。そのため、路面状態や移動体の個体差等が原因で生じるオドメトリの誤差が極低速のときと比較して発生しやすくなる。すなわち、車両モデルの特性として、車速が高くなると旋回中心Ｏｓが変化し、特に車両の前後方向の位置や車幅方向の位置、ヨー角方向の姿勢角に誤差が発生しやすくなる。

したがって、車速が高くなったときには、前後方向や車幅方向、ヨー角方向におけるパーティクルの存在分布範囲をオドメトリよりも拡大して広げなければ、誤差の発生によって適切なパーティクルの存在分布範囲を設定することができない。

そこで、本実施形態では、速度が高くなると、パーティクルの存在分布範囲を車両の前後方向、車幅方向及びヨー角方向に広げている。

以下、パーティクルの存在分布範囲を広げる方法について、図４〜６を参照して具体的に説明する。

まず、パーティクルの存在分布範囲を車両の前後方向に広げる場合について説明する。図４（ａ）に示すように車速検出部１５で検出された車速が、閾値Ｖｌｇｔｈ（ｋｍ／ｈ）未満では、オドメトリの分だけ移動させた位置から車両の前後方向に±Ｒｌｇ＿ｍｉｎ（ｍ）の範囲に存在分布範囲を設定する。そして、車速が閾値Ｖｌｇｔｈ以上になると、車両の前後方向に±Ｒｌｇ＿ｍａｘ（ｍ）の範囲に存在分布範囲を設定して存在分布範囲を広げている。

これは、図３で説明したように、車速が高くなると、車両の旋回中心が車両前方に移動するので、路面状態や移動体の個体差等が原因で生じる車両の前後方向への誤差が大きくなるためである。

また、図４（ｂ）に示すように、車速に応じて前後方向の存在分布範囲を連続的に変化させてもよい。

尚、本実施形態では、例えばＶｌｇｔｈ、Ｒｌｇ＿ｍｉｎ、Ｒｌｇ＿ｍａｘを、それぞれ２０［ｋｍ／ｈ］、０．５［ｍ］、１．０［ｍ］とする。ただし、Ｖｌｇｔｈは、車両の前後方向への誤差が大きくなる速度である。また、Ｒｌｇ＿ｍｉｎは、車両の前後方向への誤差が少ない低速でのパーティクルの存在分布範囲であり、予め実験やシミュレーションによって適正な値を求めて設定されている。Ｒｌｇ＿ｍａｘは、車両の前後方向への誤差が大きくなる高速でのパーティクルの存在分布範囲であり、予め実験やシミュレーションによって適正な値を求めて設定されている。一方、前後方向以外のパーティクルの存在分布範囲は以下のように設定する。すなわち、オドメトリの分だけ移動させた位置と姿勢角から車幅方向、上下方向、ロール、ピッチ、ヨーの各方向にそれぞれ±０．５［ｍ］、±０．１［ｍ］、±０．５［ｄｅｇ］、±０．５［ｄｅｇ］、±３．０［ｄｅｇ］とする。

次に、パーティクルの存在分布範囲を車両の車幅方向に広げる場合について説明する。図５（ａ）に示すように、車速が閾値Ｖｌｔｔｈ（ｋｍ／ｈ）未満では、オドメトリの分だけ移動させた位置から車両の車幅方向に±Ｒｌｔ＿ｍｉｎ（ｍ）の範囲に存在分布範囲を設定する。そして、車速が閾値Ｖｌｔｔｈ以上になると、車両の車幅方向に±Ｒｌｔ＿ｍａｘ（ｍ）の範囲に存在分布範囲を設定して存在分布範囲を広げている。

これは、図３で説明したように、車速が高くなると、転舵したときに極低速では現れなかった車両の車幅方向の運動が発生するので、路面状態や移動体の個体差等が原因で生じる車両の車幅方向への誤差が大きくなるためである。

また、図５（ｂ）に示すように、車速に応じて車幅方向の存在分布範囲を連続的に変化させてもよい。

尚、本実施形態では、例えばＶｌｔｔｈ、Ｒｌｔ＿ｍｉｎ、Ｒｌｔ＿ｍａｘを、それぞれ２０［ｋｍ／ｈ］、０．２［ｍ］、０．５［ｍ］とする。ただし、Ｖｌｔｔｈは、車両に遠心力が働いて各車輪に横滑り角がついて車両に車幅方向への運動が発生する速度である。また、Ｒｌｔ＿ｍｉｎは、車両に車幅方向への運動が発生しない低速でのパーティクルの存在分布範囲であり、予め実験やシミュレーションによって適正な値を求めて設定されている。Ｒｌｔ＿ｍａｘは、車両に車幅方向への運動が発生するような高速でのパーティクルの存在分布範囲であり、予め実験やシミュレーションによって適正な値を求めて設定されている。

次に、パーティクルの存在分布範囲をヨー角方向に広げる場合について説明する。図６（ａ）に示すように、車速が閾値Ｖｙｗｔｈ（ｋｍ／ｈ）未満では、オドメトリの分だけ移動させた姿勢角から車両のヨー角方向に±Ｒｙｗ＿ｍｉｎ（ｒａｄ）の範囲に存在分布範囲を設定する。そして、車速が閾値Ｖｙｗｔｈ以上になると、車両のヨー角方向に±Ｒｙｗ＿ｍａｘ（ｒａｄ）の範囲に存在分布範囲を設定して存在分布範囲を広げている。

これは、図３で説明したように、車速が高くなると、車両の旋回中心が車両前方に移動するので、路面状態や移動体の個体差等が原因で生じる車両のヨー角方向への誤差が大きくなるためである。

また、図６（ｂ）に示すように、車速に応じてヨー角方向の存在分布範囲を連続的に変化させてもよい。

尚、本実施形態では、例えばＶｙｗｔｈ、Ｒｙｗ＿ｍｉｎ、Ｒｙｗ＿ｍａｘを、それぞれ１０［ｋｍ／ｈ］、０．０２［ｒａｄ］、０．０５［ｒａｄ］とする。ただし、Ｖｙｗｔｈは、車両に遠心力が働いて各車輪に横滑り角がついて車両にヨー角方向への運動が発生する速度である。また、Ｒｙｗ＿ｍｉｎは、車両にヨー角方向への運動が発生しない低速でのパーティクルの存在分布範囲であり、予め実験やシミュレーションによって適正な値を求めて設定されている。Ｒｙｗ＿ｍａｘは、車両にヨー角方向への運動が発生するような高速でのパーティクルの存在分布範囲であり、予め実験やシミュレーションによって適正な値を求めて設定されている。

次に、ステップＳ１４０において、パーティクル散布部１８は、ステップＳ１３０で設定された存在分布範囲内にパーティクルを散布する。散布方法としては、パーティクルの位置及び姿勢角を規定する６自由度のパラメータに対して、それぞれステップＳ１３０で設定された範囲（上下限）の中で、乱数表等を用いてランダムに設定する。また、本実施形態では、パーティクルを常に５００個作成する。

尚、特許文献１で開示された技術を用いてパーティクルを散布してもよい。この場合には、ステップＳ１３０でオドメトリの分だけ移動させた車両の位置と姿勢角が、ステップＳ１７０で算出した位置と姿勢角によってどれだけ補正されたかを算出し、この補正量に応じてパーティクルを散布する平均と分布を設定して散布すればよい。このとき、パーティクルを散布する範囲はステップＳ１３０で設定された存在分布範囲とする。また、存在分布範囲についても特許文献１で開示された技術を用いて求め、ステップＳ１３０で設定された存在分布範囲とのＯＲをとってもよい。さらに、パーティクルを散布する数についても、特許文献１に開示された技術を用いてステップＳ１３０で設定された存在分布範囲に応じて動的に決定してもよい。

次に、ステップＳ１５０において、投影画像作成部２０は、ステップＳ１４０で散布された複数のパーティクルについてそれぞれ投影画像（仮想画像）を作成する。このとき、例えば三次元地図データベース３に記憶されたエッジ等の三次元位置情報を、それぞれの予測位置及び姿勢角候補でのカメラ画像に投影変換し、評価用の投影画像を作成する。この投影画像に投影された評価点群は、後述するステップＳ１６０において、ステップＳ１１０で算出したエッジ画像上のエッジと比較される。

また、投影変換では、カメラ２の位置を示す外部パラメータと、カメラ２の内部パラメータとが必要となる。外部パラメータは、車両からカメラ２までの相対位置を予め計測しておくことによって、予測位置及び姿勢角候補から算出すればよい。また内部パラメータは、予めキャリブレーションをしておけばよい。

なお、ステップＳ１１０においてカメラ画像からエッジの輝度変化の方向やエッジ近辺のカラー等について抽出している場合には、それらを用いて投影画像を作成することが望ましい。

次に、ステップＳ１６０において、尤度算出部２２は、ステップＳ１４０で散布した複数のパーティクルのそれぞれについて、ステップＳ１１０で算出したエッジ画像と、ステップＳ１５０で作成した投影画像とを比較する。そして、比較した結果に基づいて、予測位置及び姿勢角候補であるパーティクルごとに尤度を算出する。この尤度とは、それぞれの予測位置及び姿勢角候補が、どれぐらい実際の車両の位置及び姿勢角に対して尤もらしいかを示す指標である。尤度算出部２２は、投影画像とエッジ画像との一致度が高いほど、尤度が高くなるように設定する。この尤度の求め方の一例を以下で説明する。

まず、投影画像上において画素位置を特定し、この画素位置にエッジが存在するか否かを判定する。そして、投影画像と同じ画素位置となるエッジ画像上の位置にエッジが存在するか否かを判定し、エッジの有無が一致する場合には尤度ｌｉｋｅｌｉｈｏｏｄ（単位：なし）として１を設定し、一致しない場合には０を設定する。このような処理を全評価点に対して行い、その総和である一致評価点の数を尤度とする。そして、全ての予測位置及び姿勢角候補である各パーティクルについて尤度を算出したら、それぞれの尤度の合計値が１になるよう正規化する。尚、尤度の算出方法は、他にも多くの算出方法が考えられるので、それらの方法を用いてもよい。

次に、ステップＳ１７０において、位置姿勢角推定部２４は、ステップＳ１６０で算出した尤度情報を有する複数の予測位置及び姿勢角候補を用いて、最終的な車両の位置及び姿勢角を算出する。例えば、位置姿勢角推定部２４は、尤度が最も高い予測位置及び姿勢角候補を、車両の実際の位置及び姿勢角として算出する。また、各予測位置及び姿勢角候補の尤度を用いて、予測位置及び姿勢角の重み付き平均を求め、その値を最終的な車両の位置及び姿勢角としてもよい。こうして車両の位置と姿勢角の推定結果が算出されると、本実施形態に係る車両の位置姿勢角推定処理を終了する。

尚、本実施形態では、車載したカメラ２で撮影した画像と三次元地図データベース３とをマッチングして車両の位置と姿勢角を推定したが、特許文献１で開示されているようにレーザセンサの測定値を用いて車両の位置と姿勢角を推定してもよい。

この場合には、三次元地図データベース３は、例えばレーザセンサで車両からの距離や方位角の測定が可能な電柱などのポール状の障害物の位置情報、道路周辺の建物や塀等の構造物の位置や形状に関する情報を占有格子地図として備えているようにする。

この占有格子地図は、環境を格子状の細かいセルで区切り、各セルが障害物に占有されているか否かの占有確率を付加することで地図を表現する（確率ロボティクス・第９章、毎日コミュニケーションズ発行）。

そして、図２のフローチャートにおけるステップＳ１５０において、各パーティクルの位置及び姿勢角に車両がいた場合に、三次元地図データベース３に記憶された各障害物や構造物がどのような配置になるのかを算出し、占有格子地図に投影する。

その上で、図２のフローチャートにおけるステップＳ１６０では、各パーティクルごとに、三次元地図データベース３に記憶された各障害物や構造物によって占有されたセルを、占有格子地図上においてレーザセンサでいくつ検出できたかをカウントして尤度を求めるようにする。

このように、図２のフローチャートのステップＳ１５０及びＳ１６０の処理を変更するか、あるいは確率ロボティクス・第８章（毎日コミュニケーションズ発行）に記載されている方法を用いて位置や姿勢角を推定してもよい。

［第１実施形態の効果］
以上詳細に説明したように、本実施形態に係る車両位置姿勢角推定装置では、車両の速度が高くなると、パーティクルの存在分布範囲を車両の車幅方向に広げている。これにより、車両の速度が高くなって車両に車幅方向への移動が生じた場合でもパーティクルの存在分布範囲を適正に設定できるので、車両の位置及び姿勢角を正確に推定することができる。

また、本実施形態に係る車両位置姿勢角推定装置では、車両の速度が高くなると、パーティクルの存在分布範囲を車両のヨー角方向に広げている。これにより、車両の速度が高くなって車両にヨー角方向への移動が生じた場合でもパーティクルの存在分布範囲を適正に設定できるので、車両の位置及び姿勢角を正確に推定することができる。

［第２実施形態］
次に、本発明の第２実施形態に係る車両位置姿勢角推定装置について図面を参照して説明する。

［車両位置姿勢角推定システムの構成］
図７は、本実施形態に係る車両位置姿勢角推定装置を搭載した車両位置姿勢角推定システムの構成を示すブロック図である。図７に示すように、本実施形態に係る車両位置姿勢角推定装置１０は、ヨーレート検出部７５をさらに備えたことが第１実施形態と相違している。尚、その他の構成は第１実施形態と同一なので、同一の番号を付して詳細な説明は省略する。

ヨーレート検出部７５は、ヨーレートセンサ４８で計測されたセンサ値を取得することによって、車両のヨー角方向の変化率であるヨーレートを検出する。

［車両の位置姿勢角推定処理の手順］
本実施形態に係る車両の位置姿勢角推定処理は、図２のステップＳ１３０で実行されるパーティクルの存在分布範囲の設定方法が、第１実施形態と相違している。第１実施形態では、車両の速度に応じてパーティクルの存在分布範囲を設定していたが、本実施形態では、車両のヨーレートに応じてパーティクルの存在分布範囲を設定するようにしたことが相違している。

図２のステップＳ１３０において、パーティクル存在分布範囲設定部１６は、１ループ前の各パーティクルの位置及び姿勢角をオドメトリの分だけ移動させる。そして、移動させた車両の位置と姿勢角の近傍にパーティクルの存在分布範囲を設定し、本実施形態では車両のヨーレートが高くなると、パーティクルの存在分布範囲を車両の車幅方向及びヨー角方向に広げている。

ここで、図８を参照して、車両のヨーレートが高くなった場合にパーティクルの存在分布範囲を広げる理由を説明する。図８は、車両が転舵した際のタイヤ横力とヨーモーメントを示した図であり、図８（ａ）は直進走行した場合、図８（ｂ）は前輪を転舵した直後の場合、図８（ｃ）は定常円旋回の場合である。

図８（ａ）に示すように、車両のハンドルが中立位置に固定され、進行方向Ｍへ車両が直進しているときに、図３（ｂ）で説明したように各車輪に横すべり角δが付くような高い車速でハンドルを切って転舵する。そうすると、図８（ｂ）に示すように、転舵直後はまず前輪のみに横すべり角δがついて前輪にタイヤ横力Ｆｆが発生する。そして、このタイヤ横力ＦｆによってヨーモーメントＹｆが発生し、旋回が始まる。この状態ではヨー角方向の運動が主となるのでヨー角方向の姿勢角に誤差が出やすい状態となる。

そして、図８（ｂ）の転舵状態を維持すると、図８（ｃ）に示すように前輪のタイヤ横力ＦｆによるヨーモーメントＹｆと後輪のタイヤ横力ＦｒによるヨーモーメントＹｒとがつりあってヨーレートが増加しなくなり、定常円旋回状態となる。この状態では、車両のヨー角方向の運動に加えて、車幅方向の加速度が大きくなり、車幅方向の位置にも誤差が出やすい状態となる。

したがって、車両が転舵することによってヨーレートが高くなっているときには、車両の車幅方向及びヨー角方向におけるパーティクルの存在分布範囲を拡大して広げなければ、誤差の発生によって適切なパーティクルの存在分布範囲を設定することができない。

そこで、本実施形態では、ヨーレートが高くなると、パーティクルの存在分布範囲を車両の車幅方向及びヨー角方向に広げている。

以下、パーティクルの存在分布範囲を広げる方法について、図９を参照して具体的に説明する。

図９（ａ）に示すように、ヨーレート検出部７５で検出されたヨーレートが、閾値γｔｈ（ｒａｄ／ｓ）未満では、オドメトリの分だけ移動させた位置から車幅方向に±Ｒｌｔｒ＿ｍｉｎ（ｍ）の範囲に存在分布範囲を設定する。また、オドメトリの分だけ移動させた姿勢角からヨー角方向に±Ｒｙｗｒ＿ｍｉｎ（ｒａｄ）の範囲に存在分布範囲を設定する。そして、ヨーレートが閾値γｔｈ以上になると、車幅方向に±Ｒｌｔｒ＿ｍａｘ（ｍ）の範囲に、ヨー角方向には±Ｒｙｗｒ＿ｍａｘ（ｒａｄ）の範囲に存在分布範囲を設定して存在分布範囲を広げている。

これは、図８で説明したようにヨーレートや車幅方向の加速度が高くなると、各車輪に横すべり角が発生し、オドメトリによって算出した位置や姿勢角に誤差が発生しやすくなるためである。そこで、本実施形態では、路面状態や移動体の個体差等が原因で生じる車幅方向やヨー角方向の誤差が大きくなることを考慮して存在分布範囲を広げて設定している。

また、図９（ｂ）に示すように、ヨーレートに応じて存在分布範囲を連続的に変化させてもよい。

尚、本実施形態では、例えばγｔｈ，Ｒｌｔｒ＿ｍｉｎ，Ｒｌｔｒ＿ｍａｘ，Ｒｙｗｒ＿ｍｉｎ，Ｒｙｗｒ＿ｍａｘを、それぞれ０．１５［ｒａｄ／ｓ］、０．２［ｍ］、０．５［ｍ］、０．０２［ｒａｄ］、０．０５［ｒａｄ］とする。ただし、γｔｈは、車両の転舵によって車両のヨー角方向や車幅方向への運動が発生するヨーレートである。また、Ｒｌｔｒ＿ｍｉｎ、Ｒｙｗｒ＿ｍｉｎは、車両に車幅方向やヨー角方向への運動が発生しない低いヨーレートでのパーティクルの存在分布範囲であり、予め実験やシミュレーションによって適正な値を求めて設定されている。Ｒｌｔｒ＿ｍａｘ、Ｒｙｗｒ＿ｍａｘは、車両に車幅方向やヨー角方向への運動が発生するような高いヨーレートでのパーティクルの存在分布範囲であり、予め実験やシミュレーションによって適正な値を求めて設定されている。

さらに、本実施形態の方法と第１実施形態の方法とを併用してパーティクルの存在分布範囲を設定することも可能であり、その場合には各方向の上下限値のうち大きいほうの値を用いて存在分布範囲を設定すればよい。

また、本実施形態に係るパーティクル存在分布範囲設定部１６は、車両のヨーレートが高くなると、パーティクルの存在分布範囲を車両のヨー角方向に広げてから所定時間経過後に車両の車幅方向に広げるように制御する。例えば、ヨー角方向に広げてから時定数０．５［ｓ］の一次遅れを入れて車幅方向に広げるようにする。

これは、図８で説明したように、車両の車幅方向の位置はヨー角方向の姿勢角に対して遅れて変化するためである。

このように、ヨー角方向の存在分布範囲を広げた後に、遅れて車幅方向に広げるようにしたので、車両の車幅方向の位置がヨー角方向の姿勢角に対して遅れて変化することに対応させてパーティクルの存在分布範囲を適切に設定することができる。

尚、車両のヨー角方向の移動は、運転者のステアリング操舵に対してステアリング機構やタイヤ横力による遅れがある。そこで、ステアリング操舵に対してヨー角方向のパーティクルの存在分布範囲を広げるタイミングを遅らせてもよい。すなわち、運転者によってステアリングが操舵されると、例えば時定数０．２［ｓ］の一次遅れを入れてからヨー角方向のパーティクルの存在分布範囲を広げるようにしてもよい。

［第２実施形態の効果］
以上詳細に説明したように、本実施形態に係る車両位置姿勢角推定装置では、車両のヨーレートが高くなると、パーティクルの存在分布範囲を車両のヨー角方向に広げている。これにより、車両のヨーレートが高くなって車両にヨー角方向への移動が生じた場合でもパーティクルの存在分布範囲を適正に設定できるので、車両の位置及び姿勢角を正確に推定することができる。

また、本実施形態に係る車両位置姿勢角推定装置では、車両のヨーレートが高くなると、パーティクルの存在分布範囲を車両のヨー角方向に広げてから所定時間経過後に車両の車幅方向に広げている。これにより、車両の車幅方向の位置がヨー角方向の姿勢角に対して遅れて変化することに対応させてパーティクルの存在分布範囲を適切に設定することができる。

［第３実施形態］
次に、本発明の第３実施形態に係る車両位置姿勢角推定装置について図面を参照して説明する。

［車両位置姿勢角推定システムの構成］
図１０は、本実施形態に係る車両位置姿勢角推定装置を搭載した車両位置姿勢角推定システムの構成を示すブロック図である。図１０に示すように、本実施形態に係る車両位置姿勢角推定装置１０は、操舵角検出部１０５をさらに備えたことが第１実施形態と相違している。尚、その他の構成は第２実施形態と同一なので、同一の番号を付して詳細な説明は省略する。

操舵角検出部１０５は、ステアリングセンサ４３で計測されたセンサ値を取得することによって、車両の操舵角を検出する。

［車両の位置姿勢角推定処理の手順］
本実施形態に係る車両の位置姿勢角推定処理は、図２のステップＳ１３０で実行されるパーティクルの存在分布範囲の設定方法が、第１実施形態と相違している。第１実施形態では、車両の速度に応じてパーティクルの存在分布範囲を設定していたが、本実施形態では、車両のステアリング操舵角に応じてパーティクルの存在分布範囲を設定するようにしたことが相違している。

図２のステップＳ１３０において、パーティクル存在分布範囲設定部１６は、１ループ前の各パーティクルの位置及び姿勢角をオドメトリの分だけ移動させる。そして、移動させた車両の位置と姿勢角の近傍にパーティクルの存在分布範囲を設定し、車両のステアリング操舵角が大きくなると、パーティクルの存在分布範囲を車両の前後方向、車幅方向及びヨー角方向に広げている。

以下、パーティクルの存在分布範囲を広げる方法について、図１１を参照して具体的に説明する。

図１１（ａ）に示すように、操舵角検出部１０５で検出されたステアリング操舵角が、閾値θｔｈ未満では、オドメトリの分だけ移動させた位置から車両の前後方向に±Ｒｌｇｓ＿ｍｉｎ（ｍ）の範囲に、車幅方向に±Ｒｌｔｓ＿ｍｉｎ（ｍ）の範囲に存在分布範囲を設定する。また、オドメトリの分だけ移動させた姿勢角からヨー角方向に±Ｒｙｗｓ＿ｍｉｎ（ｒａｄ）の範囲に存在分布範囲を設定する。そして、ステアリング操舵角が閾値θｔｈ以上になると、車両の前後方向に±Ｒｌｇｓ＿ｍａｘ（ｍ）の範囲に、車幅方向に±Ｒｌｔｓ＿ｍａｘ（ｍ）の範囲に存在分布範囲を設定して存在分布範囲を広げている。また、ヨー角方向には±Ｒｙｗｓ＿ｍａｘ（ｒａｄ）の範囲に存在分布範囲を設定して存在分布範囲を広げている。

これは、図８で説明したようにヨーレートや車幅方向の加速度が高くなると、各車輪に横すべり角が発生して位置や姿勢角に誤差が発生するためで、このときステアリングが切られた状態であると、その誤差はさらに拡大する可能性が高くなるためである。

また、図１１（ｂ）に示すように、ステアリング操舵角に応じて存在分布範囲を連続的に変化させてもよい。

尚、本実施形態では、例えばθｔｈ、Ｒｌｇｓ＿ｍｉｎ、Ｒｌｔｓ＿ｍｉｎ、Ｒｙｗｓ＿ｍｉｎを、それぞれ３［ｒａｄ］、０．２［ｍ］、０．２［ｍ］、０．０２［ｒａｄ］とする。また、Ｒｌｇｓ＿ｍａｘ、Ｒｌｔｓ＿ｍａｘ、Ｒｙｗｓ＿ｍａｘを、それぞれ１．０［ｍ］、０．５［ｍ］、０．０５［ｒａｄ］とする。ただし、θｔｈは、車両の転舵によって車両のヨー角方向や車幅方向への運動が発生し、誤差が拡大するステアリング操舵角である。また、Ｒｌｇｓ＿ｍｉｎ、Ｒｌｔｓ＿ｍｉｎ、Ｒｙｗｓ＿ｍｉｎは、車両に車幅方向やヨー角方向への運動が発生しない低いステアリング操舵角でのパーティクルの存在分布範囲であり、予め実験やシミュレーションによって適正な値を求めて設定されている。Ｒｌｇｓ＿ｍａｘ、Ｒｌｔｓ＿ｍａｘ、Ｒｙｗｓ＿ｍａｘは、車両に車幅方向やヨー角方向への運動が発生するような高いステアリング操舵角でのパーティクルの存在分布範囲であり、予め実験やシミュレーションによって適正な値を求めて設定されている。

さらに、本実施形態の方法と第１及び第２実施形態の方法とを併用してパーティクルの存在分布範囲を設定することも可能であり、その場合には各方向の上下限値のうち最も大きい値を用いて存在分布範囲を設定すればよい。

［第３実施形態の効果］
以上詳細に説明したように、本実施形態に係る車両位置姿勢角推定装置では、車両の操舵角が大きくなると、存在分布範囲を車両の車幅方向とヨー角方向に広げている。これにより、操舵角が大きくなって車両に車幅方向やヨー角方向への移動が生じた場合でもパーティクルの存在分布範囲を適正に設定できるので、車両の位置及び姿勢角を正確に推定することができる。

なお、上述の実施形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施形態以外の形態であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計などに応じて種々の変更が可能であることは勿論である。

本出願は、２０１３年８月１日に出願された日本国特許願第２０１３−１６００７４号に基づく優先権を主張しており、この出願の内容が参照により本発明の明細書に組み込まれる。

１ ＥＣＵ
２ カメラ
３ 三次元地図データベース
４ 車両センサ群
１０ 車両位置姿勢角推定装置
１２ エッジ画像算出部
１４ オドメトリ算出部
１５ 車速検出部
１６ パーティクル存在分布範囲設定部
１８ パーティクル散布部
２０ 投影画像作成部
２２ 尤度算出部
２４ 位置姿勢角推定部
４１ ＧＰＳ受信機
４２ アクセルセンサ
４３ ステアリングセンサ
４４ ブレーキセンサ
４５ 車速センサ
４６ 加速度センサ
４７ 車輪速センサ
４８ ヨーレートセンサ
７５ ヨーレート検出部
１０５ 操舵角検出部

Claims (6)

1. パーティクルフィルタを用いて所定範囲のパーティクルの存在分布範囲を設定し、設定した存在分布範囲内にパーティクルを散布して、車両に設置されたカメラで前記車両の周囲環境を撮像した画像から前記車両の位置及び姿勢角を推定する車両位置姿勢角推定装置であって、
   前記車両は前後輪を有して前後方向に走行し、車幅方向に旋回する車両であり、
   前記車両の速度を検出する車速検出部と、
   前記車速検出部で検出された車両の速度が高くなると、前記存在分布範囲を前記車両の車幅方向に広げるパーティクル存在分布範囲設定部と
   を備えたことを特徴とする車両位置姿勢角推定装置。
2. 前記パーティクル存在分布範囲設定部は、前記車両の速度が高くなると、前記存在分布範囲を、前記車両の上下方向を軸とした回転方向であるヨー角方向に広げることを特徴とする請求項１に記載の車両位置姿勢角推定装置。
3. 前記車両のヨー角方向の変化率であるヨーレートを検出するヨーレート検出部をさらに備え、
   前記パーティクル存在分布範囲設定部は、前記ヨーレート検出部で検出されたヨーレートが高くなると、前記存在分布範囲を前記ヨー角方向に広げることを特徴とする請求項２に記載の車両位置姿勢角推定装置。
4. 前記パーティクル存在分布範囲設定部は、前記車両のヨーレートが高くなると、前記存在分布範囲を前記車両のヨー角方向に広げてから所定時間経過後に前記車両の車幅方向に広げることを特徴とする請求項３に記載の車両位置姿勢角推定装置。
5. 前記車両の操舵角を検出する操舵角検出部をさらに備え、
   前記パーティクル存在分布範囲設定部は、前記操舵角検出部で検出された操舵角が大きくなると、前記存在分布範囲を前記車両の車幅方向とヨー角方向に広げることを特徴とする請求項１〜４のいずれか１項に記載の車両位置姿勢角推定装置。
6. パーティクルフィルタを用いて所定範囲のパーティクルの存在分布範囲を設定し、設定した存在分布範囲内にパーティクルを散布して、車両に設置されたカメラで前記車両の周囲環境を撮像した画像から前記車両の位置及び姿勢角を推定する車両位置姿勢角推定装置の車両位置姿勢角推定方法であって、
   前記車両は前後輪を有して前後方向に走行し、車幅方向に旋回する車両であり、
   前記車両位置姿勢角推定装置は、
   前記車両の速度を検出し、
   前記車両の速度が高くなると、前記存在分布範囲を前記車両の車幅方向に広げることを特徴とする車両位置姿勢角推定方法。

Images