JP6848531B2 - 物標位置推定方法及び物標位置推定装置 - Google Patents

Description

本発明は、物標位置推定方法及び物標位置推定装置に関する。

従来より、カーナビゲーション装置などに格納される地図データを補正する方法が知られている。特許文献１に記載された発明は、ＧＰＳを用いた走行軌跡データと、地図データとを比較し、走行軌跡データに対する地図データのずれを補正する。これにより、地図上の物標の位置などが補正され、補正された物標の位置を用いて、車両の自己位置を算出している。

特開２００７−３１０１９８号公報

しかしながら、特許文献１に記載された発明のように走行軌跡データを用いる場合において、例えばＧＰＳでは建物が密集している場所で誤差が多く含まれるため、地図上の物標の位置を精度よく補正できないおそれがある。

本発明は、上記問題に鑑みて成されたものであり、その目的は、地図上の物標の位置を精度よく推定できる物標位置推定方法及び物標位置推定装置を提供することである。

本発明の一態様に係る物標位置推定方法は、車両から物標までの距離及び方向を地図上における複数の車両の位置から取得し、距離及び方向を取得した時の車両のトラジェクトリを算出し、トラジェクトリと距離及び方向とを用いて地図上の物標の位置を算出し、物標の位置の分散が所定値より大きい場合にトラジェクトリを補正し、補正したトラジェクトリを用いて物標の位置を算出する。

本発明によれば、物標の位置を精度よく推定できる。

図１は、本発明の実施形態に係る物標位置推定装置の構成図である。 図２は、本発明の実施形態に係る走行軌跡モデルを説明する図である。 図３は、本発明の実施形態に係るトラジェクトリを用いた物標の位置の算出方法を説明する図である。 図４は、本発明の実施形態に係る補正後のトラジェクトリを用いた物標の位置の算出方法を説明する図である。 図５は、本発明の実施形態に係る補正後のトラジェクトリを用いた物標の位置の算出方法を説明する図である。 図６は、本発明の実施形態に係る物標位置推定装置の動作例を説明するフローチャートである。

以下、本発明の実施形態について、図面を参照して説明する。図面の記載において同一部分には同一符号を付して説明を省略する。

図１を参照して、本実施形態に係る物標位置推定装置１について説明する。図１に示すように、物標位置推定装置１は、カメラ２と、レーザレンジファインダー３と、ＧＰＳ受信機４と、ジャイロセンサ５と、地図データベース６と、コントローラ１０と、通信機７を備える。

カメラ２は、ＣＣＤ（ｃｈａｒｇｅ−ｃｏｕｐｌｅｄ ｄｅｖｉｃｅ）やＣＭＯＳ（ｃｏｍｐｌｅｍｅｎｔａｒｙ ｍｅｔａｌ ｏｘｉｄｅ ｓｅｍｉｃｏｎｄｕｃｔｏｒ）などの撮像素子を有する。カメラ２は、自車両に搭載され、自車両の周囲を撮影する。カメラ２は、画像処理機能を有しており、撮影した画像から白線や道路端、物標などを検出する。物標とは、道路や歩道に設けられる物体であり、例えば信号機や電柱、交通標識などである。カメラ２は、検出したデータをコントローラ１０に出力する。

レーザレンジファインダー３は、自車両の周囲の物標を検出するセンサである。詳しくは、レーザレンジファインダー３は、レーザ光をある角度範囲内で走査し、その時の反射光を受光して、レーザ発射時点と反射光の受光時点との時間差を検出する。レーザレンジファインダー３は、自車両に対する物標の相対距離や方向などを検出し、検出したデータをコントローラ１０に出力する。レーザレンジファインダー３は、ボンネット、バンパー、ナンバープレート、ヘッドライト、サイドミラーなどの周辺に設置される。

ＧＰＳ受信機４は、人工衛星からの電波を受信することにより、地上における自車両の現在地を検出する。ＧＰＳ受信機４は、検出したデータをコントローラ１０に出力する。

ジャイロセンサ５は、自車両の重心の鉛直軸周りのヨーレート（回転角速度）を検出し、検出したデータをコントローラ１０に出力する。

地図データベース６は、カーナビゲーション装置などに記憶されているデータベースであって、道路情報や施設情報など経路案内に必要となる各種データが記憶されている。地図データベース６は、高精度な地図であって、道路の車線数、道路境界線、物標などの情報が記憶されている。地図データベース６は、コントローラ１０の要求に応じて地図情報をコントローラ１０に出力する。なお、道路情報や物標情報などの各種データは必ずしも地図データベース６から取得するものに限定されず、自車両が備えるセンサにより取得するものでもよく、また車車間通信、路車間通信を用いて取得するようにしてもよい。例えば、道路情報や物標情報などの各種データがサーバ２０に記憶されている場合、コントローラ１０は、通信により随時これらのデータを取得することができる。また、コントローラ１０は、サーバ２０から定期的に最新の地図情報を入手して、保有する地図情報を更新することができる。なお、本実施形態においては、地図データベース６として第一地図（高精度地図、車両のリート案内に用いる地図）を備えており、第一地図を補足するものとして、第二地図を備えるようにしてもよい。この第二地図は、第一地図だけでは不完全な場合や、より詳細な地図情報が必要な場合に、車両や外部のサーバ２０で作成するようにしてもよい。

コントローラ１０は、カメラ２、レーザレンジファインダー３、ＧＰＳ受信機４、ジャイロセンサ５、地図データベース６から取得したデータを処理する回路であり、例えばＩＣ、ＬＳＩなどにより構成される。コントローラ１０は、これを機能的に捉えた場合、走行軌跡モデル作成部１１と、トラジェクトリ算出部１２と、物標位置算出部１３と、トラジェクトリ補正部１４と、物標位置補正部１５と、自己位置推定部１６とに分類することができる。

走行軌跡モデル作成部１１は、自車両の走行データを用いて走行軌跡モデルを作成する。走行軌跡モデルは、図２に示すように、所定区間（図２に示す１点鎖線で囲まれた区間）における走行軌跡上の複数の点Ｘｉ（ｉ＝１〜ｎ）で構成される。各点Ｘｉは、座標（ｘ，ｙ）と車両の向きθ（進行方向）から成る。走行軌跡モデル作成部１１は、自車両の走行軌跡から座標（ｘ，ｙ）と向きθを多数取得し、取得したデータを用いて各点Ｘｉの走行軌跡を学習し、走行軌跡を再現可能な走行軌跡モデルを作成する。なお、所定区間は、走行時間や走行シーンで区切られる区間である。走行時間は例えば３〜４秒である。また、走行シーンは例えば交差点の入り口から出口までのシーン、右折または左折シーンである。このように走行時間または走行シーンで区切る理由は、走行時間が長かったり走行シーンが複雑だったりすると、様々なデータが入り込むため走行軌跡モデルの精度が低下するおそれがあるからである。走行軌跡モデル作成部１１は、走行時間や走行シーンで区切られた区間の走行軌跡モデルを作成することで、高精度な走行軌跡モデルを生成することができる。なお、走行軌跡モデル作成部１１は、必ずしも自車両の走行データを用いる必要はなく、サーバ２０に集められた自車両以外の他車両の走行データを用いてもよく、走行データを使わずにあらかじめ作成されたモデルを用いてもよい。

具体的には、走行軌跡モデル作成部１１は、取得したデータから所定区間の各点Ｘｉ毎の平均位置を求める。次に、走行軌跡モデル作成部１１は、取得した走行データから分散共分散行列を計算し、主成分ベクトルを求める。このとき、走行軌跡モデル作成部１１は、主成分ベクトルの本数として走行軌跡を十分に表現できるだけの本数を求める。次に、走行軌跡モデル作成部１１は、主成分ベクトルを列に並べ、行列Ａを作成する。行列Ａは、軌跡制御係数である。走行軌跡モデル作成部１１は、モデル各点の平均位置と、行列Ａ及び行列Ａの制御パラメータであるqとを足し合わせて走行軌跡モデルＸを作成する。走行軌跡モデルＸは、式（１）、（２）を用いて次のように表される。

走行軌跡モデル作成部１１は、作成した走行軌跡モデルＸをサーバ２０に保存する。走行軌跡モデル作成部１１は、作成した走行軌跡モデルＸを自車両に保存するようにしてもよい。なお、走行軌跡モデルＸは、一つ生成されてもよく、複数生成されてもよい。また、所定区間は、５秒間、１０秒間など時間が区切ってもよく、動作で区切ってもよい。動作で区切る例は、交差点に進入してから出るまでの区間である。なお、走行軌跡モデル作成部１１は、車車間通信を使って他車両に走行軌跡モデルＸを保存するようにしてもよい。

トラジェクトリ算出部１２は、サーバ２０に記憶されている走行軌跡モデルＸを用いてトラジェクトリを算出する。トラジェクトリとは、走行軌跡モデルＸを用いて算出される自車両の走行軌跡である。トラジェクトリ算出部１２は、向きθと制御パラメータｑを所定値（例えば、０）で初期化し、トラジェクトリを算出する。また、制御パラメータｑについては、オドメトリの軌跡から逆算した値で初期化してもよい。自車両の走行軌跡は、オドメトリを使用して取得することも可能である。オドメトリとはタイヤの回転角と回転角速度に応じて移動体の移動距離と方向を求める方法である。しかし、オドメトリを使用した場合、タイヤスリップなどによる誤差を回避できない可能性があり、走行軌跡の推定精度が低下するおそれがある。本実施形態では走行軌跡モデルＸを用いたトラジェクトリを使用する。これにより、走行軌跡の推定精度が低下することを抑制することができる。なお、トラジェクトリ算出部１２は、物標を取得する前のトラジェクトリについて、オドメトリを用いて算出するようにしてもよい。また、トラジェクトリ算出部１２は、オドメトリから算出したトラジェクトリを用いて物標を取得するようにしてもよく、物標を取得した後のトラジェクトリを走行軌跡モデルを用いて算出するようにしてもよい。

物標位置算出部１３は、トラジェクトリ算出部１２によって算出されたトラジェクトリを用いて地図上の物標の位置を算出する。物標位置算出部１３は、トラジェクトリ上の各点から物標までの距離を取得し、この距離を取得した時の自車両の向きθを取得する。この各点とは、複数の位置であればよく、同じ位置であっても複数回、物標の距離及び向きを取得できればよい。物標位置算出部１３は、物標までの距離、及び向きθを用いて物標の位置を算出する。また、物標位置算出部１３は、トラジェクトリ上の各点から算出した物標の位置の分散を求める。分散が閾値よりも大きい場合、向きθとｑを調整し再度トラジェクトリを補正して、物標の位置を求め、分散を算出する。分散とは、散らばり具合を示す指標であり、物標位置算出部１３は一般的な手法を用いて分散を求めることができる。分散が閾値より大きい場合、平均値から遠く離れたデータが多く、散らばりが大きいことを意味する。一方、分散が閾値以下の場合、平均に近いデータが多く、散らばりが小さいことを意味する。換言すれば、分散が閾値以下の場合、物標位置算出部１３が物標の位置を精度よく推定できたことを意味する。一方、分散が閾値より大きい場合、物標位置算出部１３は物標の位置を精度よく推定できていないことを意味する。なお、閾値は、実験やシミュレーションを通じて求めてもよく、また任意に設定するようにしてもよい。

トラジェクトリ補正部１４は、物標位置算出部１３によって算出された物標の位置の分散に応じてトラジェクトリを補正する。詳しくは、トラジェクトリ補正部１４は、分散が閾値より大きい場合、向きθと制御パラメータｑを調整してトラジェクトリを補正する。また、トラジェクトリ補正部１４は、分散が閾値以下になるまで向きθと制御パラメータｑを繰り返し調整してトラジェクトリを補正する。なお、向きθと制御パラメータｑの調整方法は限定されないが、例えばガウスニュートン法を使用して繰り返し計算の中で徐々に真値に近づけていく方法を用いることができる。このトラジェクトリ補正部１４は、算出された物標の位置から、各点で取得した物標との距離及び向きを拘束条件として与え、走行軌跡モデルＸを用いて、トラジェクトリを補正することができる。これにより、物標との距離及び向きを取得した各点の位置に誤差が生じていたとしても（例えば、ＧＰＳ誤差やオドメトリ誤差）、トラジェクトリ補正部１４は、トラジェクトリを適切に算出することができるため、ひいては、車両の自己位置を正確に算出できるようになる。さらに、トラジェクトリ補正部１４は、この処理を繰り返すことで、さらにトラジェクトリを適切に算出することができる。

物標位置補正部１５は、トラジェクトリ補正部１４によって補正されたトラジェクトリを用いて地図上の物標の位置を補正する。そして、物標位置補正部１５は、補正後の物標の位置の分散を求める。また、物標位置補正部１５は、分散が閾値以下になるまでトラジェクトリ補正部１４によって補正されたトラジェクトリを用いて繰り返し補正後の物標の位置の分散を求める。そして、物標位置補正部１５は、分散が閾値以下になった場合、補正後の物標の位置を地図データベース６へリンクさせる。

自己位置推定部１６は、物標位置補正部１５によって地図データベース６へリンクされた物標の位置を用いて自己位置を推定する。本実施形態において地図上の物標の位置は精度よく推定されているため、自車両から物標までの距離と方向を用いることにより、自己位置推定部１６は自己位置を精度よく推定できる。具体的には、物標位置補正部１５が物標の位置を正しい位置に補正することができるため、自己位置推定部１６が物標を用いて車両の自己位置を推定する場合に、自己位置の推定精度が向上する。なお、物標を用いて車両の自己位置を推定する場合とは、物標の周辺を走行した場合に、物標までの距離と向きをセンサにより検出し、検出した距離と向き、及び取得していた物標の位置から、自車両の位置を推定することを意味する。また、自己位置推定部１６は物標の位置を用いることにより、例えば交差点内側のように自車周囲に白線がない場所においても、自己位置を精度よく推定できる。なお、自己位置推定部１６は、カメラ２によって検出された白線や道路端位置と、地図データベース６の白線や道路端位置とを比較して推定した自己位置と、物標を用いて推定した自己位置と照合して自己位置を推定してもよい。

通信機７は、サーバ２０と通信するための装置である。

次に、図３〜図５を参照してトラジェクトリを用いた物標の位置の算出方法及び補正方法を説明する。

図３において点線で示す座標はマップ座標系であり、実線で示す座標は車両座標系である。車両座標系におけるｙ軸は自車両の進行方向を示し、ｘ軸は進行方向に直交する横方向を示す。図３に示すトラジェクトリ２１は、初期のトラジェクトリである。トラジェクトリ算出部１２は、向きθと制御パラメータｑを所定値で初期化し、初期のトラジェクトリ２１を算出する。図３に示す座標（ｘ１，ｙ１）〜座標（ｘ５，ｙ５）は、トラジェクトリ２１上の自己位置である。図３に示す向きθ１〜θ５は、マップ座標系からのヨー角を示す。角度Φ１〜Φ５は、車両座標系から見た自車両進行方向に対する物標３０の方向を示す角度であり、距離Ｌ１〜Ｌ５は、車両座標系から見た自車両から物標３０までの距離である。なお、本実施形態においては、所定の座標系を用いて説明しているが、マップ座標系や車両座標系に限定される必要はなく、その他の座標系を用いてもよく、また、一つの座標系を用いるようにしてもよい。

図３に示すように、自車両は、座標（ｘ１，ｙ１）〜座標（ｘ５，ｙ５）の区間を走行中にカメラ２やレーザレンジファインダー３を用いて自車両から物標３０までの距離Ｌ１〜Ｌ５を計測する。自車両は、距離Ｌ１〜Ｌ５を計測した時刻と同時刻にジャイロセンサ５を用いて向きθ１〜θ５を検出する。図３に示す例では、異なる５つの走行地点から物標３０までの距離と方向を取得する様子を示しているが、５点に限定されない。なお、角度Φ１〜Φ５についてもジャイロセンサ５を用いて取得することができる。

ここで、トラジェクトリ２１上において、ｉ番目の時刻の自己位置をＸｉとすると、自己位置Ｘｉは、式（３）を用いて次のように表される。

物標３０の位置を（Ｃｘ_ｉ，Ｃｙ_ｉ）とすると、物標３０の位置（Ｃｘ_ｉ，Ｃｙ_ｉ）は、式（４）を用いて次のように表される。

式（５）における（Ｌｘ_ｉ，Ｌｙ_ｉ）は、ｉ番目の時刻における自車両から物標３０までの距離であり角度Φの要素を含む。式（５）に示すように物標位置算出部１３は、自車両から物標３０までの距離（Ｌｘ_ｉ，Ｌｙ_ｉ）を向きθ_ｉだけ回転させる。そして、物標位置算出部１３は、トラジェクトリ２１上の座標（ｘ_ｉ，ｙ_ｉ）に距離を加算し、座標（ｘ_ｉ，ｙ_ｉ）分平行移動してマップ座標系に変換することで各点毎に物標３０の位置を算出する。

次に、物標位置算出部１３は、算出した物標３０の位置の分散を求める。図３に示すように分散が閾値より大きい場合、トラジェクトリ補正部１４は、分散が閾値以下になるようにトラジェクトリ２１を補正する。トラジェクトリ２１の補正について図４，５を用いて説明する。

図４に示すように、トラジェクトリ補正部１４は、分散が閾値以下になるように向きθと制御パラメータｑを調整しトラジェクトリを補正する。図４に示すトラジェクトリ２２は、図３に示すトラジェクトリ２１を補正したトラジェクトリである。物標位置補正部１５は、補正後のトラジェクトリ２２を用いて物標３０の位置を再度算出し、物標３０の位置の分散を再度算出する。そして、図５に示すように分散が閾値以下になるまでトラジェクトリ補正部１４は、向きθと制御パラメータqを調整する。図５に示すトラジェクトリ２３は、図４に示すトラジェクトリ２２を補正したトラジェクトリである。このようにトラジェクトリ補正部１４と物標位置補正部１５は、分散が閾値以下になるまで繰り返し計算する。これによりトラジェクトリ補正部１４と物標位置補正部１５は、物標３０の位置を精度よく推定できる。そして、物標位置補正部１５は、分散が閾値以下になった場合、補正後の物標３０の位置を地図データベース６へリンクさせる。自己位置推定部１６は、物標位置補正部１５によって地図データベース６へリンクされた物標３０の位置を用いて自己位置を推定する。本実施形態において物標３０の位置は精度よく推定されているため、自車両から物標３０までの距離と方向を用いることにより、自己位置推定部１６は自己位置を精度よく推定できる。

次に、図６のフローチャートを参照して、物標位置推定装置１の一動作例について説明する。

ステップＳ１０１において、カメラ２及びレーザレンジファインダー３は、それぞれ異なる走行地点から物標３０までの距離Ｌ_ｉと向きθ_ｉを取得する。本実施形態において、ｉは時刻を示し、値は１〜５である。

ステップＳ１０２において、トラジェクトリ算出部１２は、向きθと制御パラメータｑを所定値で初期化する。

ステップＳ１０３において、トラジェクトリ算出部１２は、走行軌跡モデルＸを用いてトラジェクトリを算出する。詳しくは、トラジェクトリ算出部１２は、ステップＳ１０２で設定した初期値を走行軌跡モデルＸに入力し、初期のトラジェクトリ２１を算出する。走行軌跡モデルＸを用いてトラジェクトリを算出する理由は、オドメトリを使用したトラジェクトリ推定は、タイヤスリップなどによる誤差のため推定精度が低下するおそれがあるからである。

ステップＳ１０４において、物標位置算出部１３は、ステップＳ１０３で算出されたトラジェクトリ２１を用いて地図上の物標３０の位置を算出する。まず、物標位置算出部１３は、トラジェクトリ２１上の各点から物標３０までの距離Ｌ_ｉを取得し、この距離を取得した時の向きθ_ｉを取得する。そして、物標位置算出部１３は、物標３０までの距離Ｌ_ｉ、及び向きθ_ｉを用いて各点毎に物標３０の位置を算出する。

ステップＳ１０５において、物標位置算出部１３は、ステップＳ１０４で算出された物標３０の位置を用いて物標３０の位置の分散を算出する。分散とは、散らばり具合を示す指標である。よって、分散が閾値以下の場合、物標位置算出部１３は物標３０の位置を精度よく推定できたことを意味する。一方、分散が閾値より大きい場合、物標位置算出部１３は物標３０の位置を精度よく推定できていないことを意味する。分散が閾値より大きい場合（ステップＳ１０６でＹｅｓ）、処理はステップＳ１０７に進む。一方、分散が閾値以下の場合（ステップＳ１０６でＮｏ）、処理はステップＳ１０８に進む。

ステップＳ１０７において、トラジェクトリ算出部１２は、向きθと制御パラメータｑを調整してトラジェクトリ２１を補正する。以後、ステップＳ１０３〜ステップＳ１０７において、トラジェクトリ補正部１４は、分散が閾値以下になるまで向きθと制御パラメータｑを繰り返し調整してトラジェクトリを補正する。また、物標位置補正部１５は、分散が閾値以下になるまでトラジェクトリ補正部１４によって補正されたトラジェクトリを用いて繰り返し補正後の物標の位置の分散を求める。

ステップＳ１０８において、物標位置補正部１５は、分散が閾値以下になった場合、補正後の物標３０の位置を地図データベース６へリンクさせる。

ステップＳ１０９において、自己位置推定部１６は、ステップＳ１０８でリンクされた物標３０の位置を用いて自己位置を推定する。本実施形態において地図上の物標３０の位置は精度よく推定されているため、自車両から物標３０までの距離と方向を用いることにより、自己位置推定部１６は自己位置を精度よく推定できる。

以上説明したように、本実施形態に係る物標位置推定装置１によれば、以下の作用効果が得られる。

カメラ２またはレーザレンジファインダー３は、自車両から物標３０までの距離及び方向を地図上における複数の自車両の位置から取得する。トラジェクトリ算出部１２は、自車両から物標３０までの距離及び方向を取得した時の自車両のトラジェクトリを算出する。物標位置算出部１３は、トラジェクトリ上の位置と距離及び方向とを用いて地図上の物標３０の位置を算出し、物標３０の位置の分散を算出する。トラジェクトリ補正部１４は、分散が閾値以下になるまで向きθと制御パラメータｑを調整してトラジェクトリを補正する。物標位置補正部１５は、補正後のトラジェクトリを用いて物標３０の位置を補正する。トラジェクトリ補正部１４及び物標位置補正部１５は、分散が閾値以下になるまでこの補正処理を繰り返す。これにより、物標位置推定装置１は、物標３０の位置を精度よく推定できる。加えて、物標位置推定装置１は自己位置を精度よく算出することができるため、例えば、自動運転制御や運転支援制御を実行する上で、車両の位置を正確に制御できるようになるため、乗員に与える違和感を抑制した制御を実行することができるようになる。算出した自己位置を用いて自動運転制御を実行する場合、物標位置推定装置１は初めに車両が走行する走行軌跡を算出し、算出した走行軌跡に沿って走行するように車両のアクチュエータを制御して走行する。この時に、物標位置推定装置１は算出した自己位置を用いることで、走行軌跡を算出し、走行軌跡に沿って走行するように車両に対する制御を実行することができるようになる。

また、物標位置推定装置１は、自車両から物標３０までの距離及び方向を、複数の異なる位置から取得する。これにより、分散の算出に用いるデータが増えるため、物標位置推定装置１は、精度よく分散を算出することができる。

また、物標位置推定装置１は、物標３０を取得するよりも前にあらかじめ作成されたモデルにより、トラジェクトリを算出する。これにより、物標位置推定装置１は、物標３０を取得する時のシステムリソースを節約することができる。

また、トラジェクトリを算出するための走行軌跡モデルＸは、サーバ２０に記憶され、通信機７により取得される。これにより、システムリソースを節約することができる。

また、物標位置推定装置１は、物標位置補正部１５によって地図データベース６へリンクされた物標３０の位置を用いて自己位置を推定する。本実施形態において地図上の物標３０の位置は精度よく推定されているため、自車両から物標３０までの距離と方向を用いることにより、物標位置推定装置１は自己位置を精度よく推定できる。また、物標位置推定装置１は物標３０の位置を用いることにより、例えば交差点内側のように自車周囲に白線がない場所においても、自己位置を精度よく推定できる。また、オドメトリやＧＰＳを使用した自己位置推定は誤差を含む可能性があるが、物標位置推定装置１は物標３０の位置を用いることにより、自己位置を精度よく推定できる。

また、走行軌跡モデル作成部１１は走行軌跡モデルＸを作成する際、所定区間ごとに作成する。この所定区間は、走行時間や走行シーンで区切られる区間である。これにより、走行軌跡モデル作成部１１は、様々なデータの入り込みを防止でき、高精度な走行軌跡モデルＸを生成することができる。

上記のように、本発明の実施形態を記載したが、この開示の一部をなす論述及び図面はこの発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施の形態、実施例及び運用技術が明らかとなろう。

本実施形態では、走行軌跡モデルＸを用いてトラジェクトリを算出したが、走行軌跡モデルＸの代わりに車両モデルを用いてもよい。車両モデルとは、車輪の径、車両の重心点から車軸までの距離、前輪及び後輪の位置関係から構成される。物標位置推定装置１は、車輪の回転速度から車両の速度を求め、さらに操舵角から横滑り角とヨー角速度を求める。物標位置推定装置１は、これらの要素から自己位置（ｘ，ｙ）を求めてトラジェクトリを算出する。そして物標位置推定装置１は、算出したトラジェクトリを用いて物標の位置を算出し、物標の位置の分散を算出する。その後、物標位置推定装置１は上述のように補正処理を繰り返す。これにより、物標位置推定装置１は物標の位置を精度よく推定できる。

本実施形態では、自車両の走行データを用いて走行軌跡モデルＸを作成したが、他車両の走行データも加えて走行軌跡モデルＸを作成してもよい。これにより、物標位置推定装置１は多くの道路に対応できる走行軌跡モデルＸを作成することができる。また、物標位置推定装置１は、他車両の走行データを加えることにより多くの走行軌跡モデルＸを作成することができる。

また、物標位置推定装置１は同一区間を繰り返し走行したデータを用いてトラジェクトリを補正してもよい。同一区間を繰り返し走行したデータを用いることにより、物標位置推定装置１は地図上の物標３０の位置を精度よく推定できる。

本発明は、自動運転機能を備える車両にも適用できる。

なお、上述の実施形態の各機能は、１または複数の処理回路により実装され得る。処理回路は、電気回路を含む処理装置等のプログラムされた処理装置を含む。処理回路は、また、実施形態に記載された機能を実行するようにアレンジされた特定用途向け集積回路（ＡＳＩＣ）や従来型の回路部品のような装置を含む。

１ 物標位置推定装置
２ カメラ
３ レーザレンジファインダー
４ ＧＰＳ受信機
５ ジャイロセンサ
６ 地図データベース
７ 通信機
１０ コントローラ
１１ 走行軌跡モデル作成部
１２ トラジェクトリ算出部
１３ 物標位置算出部
１４ トラジェクトリ補正部
１５ 物標位置補正部
１６ 自己位置推定部
２０ サーバ

Claims (9)

1. 車両から物標までの距離及び方向を地図上における複数の車両の位置から取得し、
   前記距離及び方向を取得した時の車両のトラジェクトリを算出し、
   前記トラジェクトリと前記距離及び方向とを用いて前記物標の位置を算出し、
   前記物標の位置の分散が所定値より大きい場合に前記トラジェクトリを補正し、
   補正した前記トラジェクトリを用いて前記物標の位置を算出することを特徴とする物標位置推定方法。
2. 前記車両から物標までの距離及び方向を、複数の異なる位置から取得することを特徴する請求項１に記載の物標位置推定方法。
3. 前記物標を取得するよりも前にあらかじめ作成されたモデルにより、前記トラジェクトリを算出することを特徴する請求項１または２に記載の物標位置推定方法。
4. 前記トラジェクトリを算出するモデルは、サーバに記憶され、通信機により取得されることを特徴する請求項３に記載の物標位置推定方法。
5. 前記モデルは、走行軌跡モデルであって、自車両及び他車両の走行データを用いて作成されることを特徴とする請求項３または４に記載の物標位置推定方法。
6. 前記走行軌跡モデルは、所定時間または走行シーンで区切られた区間において作成されることを及び特徴とする請求項５に記載の物標位置推定方法。
7. 前記物標を取得するために、繰り返し走行した時のデータを用いて前記トラジェクトリを補正することを特徴とする請求項１〜６のいずれか１項に記載の物標位置推定方法。
8. 前記物標の位置の分散が所定値以下の場合に、前記物標の位置に基づいて車両の位置を推定することを特徴とする請求項１〜７のいずれか１項に記載の物標位置推定方法。
9. 車両から物標までの距離及び方向を地図上における複数の車両の位置から取得するセンサと、
   前記距離及び方向を取得した時の車両のトラジェクトリを算出するコントローラと、を備え、
   前記コントローラは、前記トラジェクトリと前記距離及び方向とを用いて物標の位置を算出し、前記物標の位置の分散が所定値より大きい場合に前記トラジェクトリを補正し、補正した前記トラジェクトリを用いて前記物標の位置を算出することを特徴とする物標位置推定装置。

Images