(57)【要約】      （修正有）【課題】観測データの質に応じ、計算量をリソースと精度のバランスを取り調整できる自己位置推定装置などを提供する。【解決手段】複数の物体の相対座標情報を取得する手段４０１と、取得の中から１つを選択する手段４０２と、選択された相対座標情報と物体の絶対座標情報を格納したデータベースとに基づき、選択された物体に対応する候補をデータベースから複数検索し、検索された候補の相対座標と選択された物体の相対座標との差分をそれぞれ算出する手段４０３と、複数の差分のうち最も小さい差分の候補が選択された物体に該当し、候補の絶対座標情報と所定のアルゴリズムを用いて自己位置を推定する手段４０４と、複数の差分を記録する手段４０５と、記録された複数の差分のうち最小のものから順に所定の個数までの差分の平均値が所定の閾値より小さいか判断する手段４０７と、小さい場合選択を中止し、推定された自己位置を出力する手段４０７とを備える。【選択図】図４
2015152991.tif 000002
【特許請求の範囲】
【請求項１】
自車両の位置を前記自車両の周辺に存在する物体の位置に基づいて推定する自己位置推定装置であって、
前記自車両の位置に対する複数の前記物体のそれぞれの相対位置の相対座標情報をセンサーを介して取得する取得手段と、
取得された前記複数の物体のそれぞれの相対座標情報から１つの物体の相対座標情報を選択する選択手段と、
選択された前記相対座標情報を所定の手法で変換された絶対座標情報と、あらかじめ前記複数の物体のそれぞれの絶対位置の絶対座標情報を格納したデータベースとに基づいて、前記選択された物体に対応する候補を前記データベースから複数検索し、前記検索された複数の候補の絶対座標を前記自車両の位置に対する相対座標に変換し、変換された前記相対座標と前記選択された物体の相対座標との差分をそれぞれ算出する算出手段と、
算出された前記複数の差分のうち、最も小さい差分の前記候補が前記選択された物体に該当するものとして、前記候補の絶対座標情報と所定のアルゴリズムとを用いて前記自車両の自己位置を推定する推定手段と、
算出された前記複数の差分を記録する記録手段と、
記録された前記複数の差分のうち、差分の小さい順に数えて最小のものから所定の個数までの差分の平均値が所定の閾値より小さいか否かを判断する判断手段と、
前記平均値が前記所定の閾値より小さいと判断された場合に、前記選択手段による相対座標情報の選択を中止し、前記推定手段によって推定された前記自車両の自己位置を出力する出力手段とを、
備える自己位置推定装置。
【請求項２】
前記物体の相対位置の相対座標を変数とする関数であって、前記物体が所定の物体であるという信頼性の度合いを示す信頼度関数を備え、
前記選択手段は、前記信頼度関数に基づいて、信頼度の高い順に前記物体の相対座標情報を選択する請求項１に記載の自己位置推定装置。
【請求項３】
前記信頼度関数は所定の時間間隔ごとに更新される請求項２に記載の自己位置推定装置。
【請求項４】
前記取得手段は、複数のセンサーを介して前記複数の物体のそれぞれの相対位置の相対座標情報を取得し、
前記選択手段は、１つのセンサーで前記自己位置を推定することができない場合に、他のセンサーを介して取得された前記複数の物体のそれぞれの相対座標情報から１つの物体の相対座標情報を選択する請求項１から３のいずれか１つに記載の自己位置推定装置。
【請求項５】
前記複数のセンサーそれぞれに信頼性の度合いを示す信頼度を設け、
前記選択手段は、信頼度の高いセンサー順に、センサーを介して取得された前記複数の物体のそれぞれの相対座標情報から１つの物体の相対座標情報を選択する請求項４に記載の自己位置推定装置。
【請求項６】
前記自車両の周辺に位置する、前記物体の相対位置の相対座標情報を取得するためのセンサーを備える他車両と通信するための通信手段を備え、
前記推定手段は、前記他車両による前記他車両の自己位置の推定結果を前記通信手段を介して受信し、受信した前記推定結果を用いて前記自車両の自己位置を推定する請求項１から５のいずれか１つに記載の自己位置推定装置。
【請求項７】
前記他車両に信頼性の度合いを示す信頼度を設け、前記信頼度は前記他車両が有するセンサーに設けられた信頼性の度合いを示す信頼度に依存した関数で定義された請求項６に記載の自己位置推定装置。
【請求項８】
自車両の位置を前記自車両の周辺に存在する物体の位置に基づいて推定する自己位置推定方法であって、
前記自車両の位置に対する複数の前記物体のそれぞれの相対位置の相対座標情報をセンサーを介して取得する取得ステップと、
取得された前記複数の物体のそれぞれの相対座標情報から１つの物体の相対座標情報を選択する選択ステップと、
選択された前記相対座標情報を所定の手法で変換された絶対座標情報と、あらかじめ前記複数の物体のそれぞれの絶対位置の絶対座標情報を格納したデータベースとに基づいて、前記選択された物体に対応する候補を前記データベースから複数検索し、前記検索された複数の候補の絶対座標を前記自車両の位置に対する相対座標に変換し、変換された前記相対座標と前記選択された物体の相対座標との差分をそれぞれ算出する算出ステップと、
算出された前記複数の差分のうち、最も小さい差分の前記候補が前記選択された物体に該当するものとして、前記候補の絶対座標情報と所定のアルゴリズムとを用いて前記自車両の自己位置を推定する推定ステップと、
算出された前記複数の差分を記録する記録ステップと、
記録された前記複数の差分のうち、差分の小さい順に数えて最小のものから所定の個数までの差分の平均値が所定の閾値より小さいか否かを判断する判断ステップと、
前記平均値が前記所定の閾値より小さいと判断された場合に、前記選択ステップによる相対座標情報の選択を中止し、前記推定ステップによって推定された前記自車両の自己位置を出力する出力ステップとを、
有する自己位置推定方法。
【請求項９】
前記物体の相対位置の相対座標を変数とする関数であって、前記物体が所定の物体であるという信頼性の度合いを示す信頼度関数を備え、
前記選択ステップでは、前記信頼度関数に基づいて、信頼度の高い順に前記物体の相対座標情報を選択する請求項８に記載の自己位置推定方法。
【請求項１０】
前記信頼度関数は所定の時間間隔ごとに更新される請求項９に記載の自己位置推定方法。
【請求項１１】
前記取得ステップでは、複数のセンサーを介して前記複数の物体のそれぞれの相対位置の相対座標情報を取得し、
前記選択ステップでは、１つのセンサーで前記自己位置を推定することができない場合に、他のセンサーを介して取得された前記複数の物体のそれぞれの相対座標情報から１つの物体の相対座標情報を選択する請求項８から１０のいずれか１つに記載の自己位置推定方法。
【請求項１２】
前記複数のセンサーそれぞれに信頼性の度合いを示す信頼度を設け、
前記選択ステップでは、信頼度の高いセンサー順に、センサーを介して取得された前記複数の物体のそれぞれの相対座標情報から１つの物体の相対座標情報を選択する請求項１１に記載の自己位置推定方法。
【請求項１３】
前記自車両の周辺に位置する、前記物体の相対位置の相対座標情報を取得するためのセンサーを備える他車両と通信する通信ステップを有し、
前記推定ステップでは、前記他車両による前記他車両の自己位置の推定結果を前記通信ステップで受信し、受信した前記推定結果を用いて前記自車両の自己位置を推定する請求項８から１２のいずれか１つに記載の自己位置推定方法。
【請求項１４】
前記他車両に信頼性の度合いを示す信頼度を設け、前記信頼度は前記他車両が有するセンサーに設けられた信頼性の度合いを示す信頼度に依存した関数で定義された請求項１３に記載の自己位置推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自車両の位置を自車両の周辺に存在する物体の位置に基づいて推定する自己位置推定装置及び自己位置推定方法に関する。
【背景技術】
【０００２】
従来からSLAM（Simultaneous Localization And Mapping）という技術が存在する。SLAMとは、ある移動体がセンサーを使い、自己位置と周辺の観測された物体の位置とを推定するものである。この推定には、移動体の自己内部センサーによって得られる、移動体がいかなる速度でどの方向に向いているかの情報と、周囲の静止物と自己との相対座標からなる観測情報とが用いられ、自己の位置の絶対座標及び周囲の静止物の絶対座標の両方が更新されながら計算される。
【０００３】
ここでは、移動体として自動車を一例として述べる。自動車は、自らの内部センサーによってある一定の時間間隔ごとに、速度とハンドルの旋回角度とをノイズによる誤差を含みながらも情報として取得することができる。これは「オドメトリー」と呼ばれる。さらに、自動車に装備されている周辺を観測するセンサー、すなわちミリ波レーダー、レーザー装置、カメラなどにより、周辺の物体の自車からの相対位置が観測できる。周辺の物体は一般に「ランドマーク」と呼ばれる。
【０００４】
ここでは、図１１ａに示すように、周辺の観測センサーとして自動車（自車）の前方に取り付けられたミリ波レーダーを用いた場合を考える。ミリ波レーダーは、不図示の送信部からミリ波を照射し、周辺の物体から反射して戻ってきた波面を不図示の受信部で受信する。これにより、図１１ｂに示すように、自車は反射点（照射されたミリ波などを反射させた物体）の自車からの相対距離ｒと方向角度θとを数値として取得することができる。これにより、自車は、自車前方のある角度の領域をスキャンし、周辺にどのような物体があるか観測することができる。
【０００５】
内部センサーにより得られる「オドメトリー」と観測により得られる「ランドマーク」の情報にはノイズが含まれるため、単純にお互いの座標の足し算や引き算で自車の正確な絶対座標、自己位置は求めることができない。そのため、SLAMアルゴリズムでは、拡張カルマンフィルター（EKF：Extended Kalman Filter）を使い、推測した絶対座標を何度も更新しながら、正解値に近づけていく。このSLAMとミリ波レーダーを利用した技術として下記の特許文献１の「無人搬送車」がある。無人搬送車は、周囲の物体を観測してその物体の座標を特定し、かつ自己の位置を正確に推測して自動で移動していくものである。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２０１３−４５２９８号公報（要約）
【非特許文献】
【０００７】
【非特許文献１】「確率ロボティクス」著者  セバスチャン＝サーン  ISBN-10: 4839924015  ISBN-13: 978-4839924010  7章186ページ「計測の尤度」
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ここで、図１１ｂに示すように、実際、ミリ波レーダーを装備した自動車の自己位置をSLAMアルゴリズムの技術を適応させて計算した場合を考える。ミリ波レーダーが捉える周辺の物体とは、例えばガードレールや標識であり、ある一つのセンサー（例えば、ミリ波レーダー）は毎時間２０ミリ秒あたり２個から３０個ほどの反射点群を観測できる。図１２から図１４には、観測された反射点群から一つ一つサンプリングして自己位置推定をする計算処理フローが示されている。
【０００９】
図１２には、ある一つのセンサーにおいて観測された反射点群から一つ一つの反射点をサンプリングしていかに自己位置推定計算を行うかのフローが示されている。図１２のステップＳ１２０１には、反射点は自車からの角度θと自車からの距離ｒとからなる相対円座標で取得されることが示されている。ステップＳ１２０３のサンプリングされた反射点の処理の詳細が図１３に示されている。図１３のフローで使われるEKFアルゴリズムの詳細が図１４に示されている。
【００１０】
図１４のステップＳ１４０１において、観測された反射点データは自車からの距離ｒと角度θからなる相対円座標であるため、まずこれを相対XY直交座標に変換する。次に、ステップＳ１４０２において、自車の位置、すなわち現在推測されている自己位置の絶対座標（自車自己位置絶対座標）を足して、反射点の相対座標を絶対座標に変換する。ステップＳ１４０３において、観測された反射点が絶対座標に変換されたことで、自車があらかじめ有する反射点データベース（以下、単にデータベースとも言う）の反射点が絶対座標として記録されているため、データベースのどの反射点に近いのか比較することができる。すなわち、データベースから検索し、距離が近い反射点を“複数”候補として求める。この時点で一番近いものを検索し、それが観測された反射点と同一のものであると結論づけられない理由は、観測された反射点座標の値には誤差が含まれているため、複数の候補に絞ることしかできない。
【００１１】
ステップＳ１４０４において、これらのデータベース上の候補の反射点群に対して、それぞれが正しいと仮定して、仮に自車からの相対円座標として観測された場合、どのような座標になるかを計算する（すなわち、相対座標を計算し、更に相対円座標に変換する）。ステップＳ１４０５において、データベース上の候補の反射点で相対円座標として観測されていた場合の座標と、実際に観測された反射点の相対円座標との差分を計算する。両者は円座標であるから、差分は（ｄｒ、ｄθ）となる。EKFアルゴリズムでは、単純にこの差分の２乗の和の値が小さいほど、差分が小さいというようなユークリッド距離で測ることはできない。前述のように、観測された反射点の円座標には誤差が含まれており、誤差は計測誤差と観測誤差とからなり、その誤差に応じたrとθの特有の誤差分布を考慮して、はじめて差分が大きいか判断できる。
【００１２】
EKFアルゴリズムでは、ｄｒ、ｄθの誤差分布を考慮した尤度関数Sを求めることができる。差分が大きいかどうかは、この尤度Sと差分である点（ｄｒ、ｄθ）とのマハラノビス距離（Mahalanobis Distance）mdを計算することで判断できる。値mdが最も小さい候補の反射点が、観測された反射点に該当するものであると判断される。ここまで述べた、観測された反射点がどのデータベースの反射点に対応するのかを計算する一連の作業は、一般的に「データアソシエーション（Data Association）」と呼ばれる。
【００１３】
ステップＳ１４０６において、該当するデータベース上の反射点が決まれば、その反射点の座標から、EKFアルゴリズムによりその時点の自車自己位置を計算し、現在推測されている自車自己位置を更新してより正確な値に近づけることができる。
【００１４】
しかし、以上に述べた図１４の処理をするのには時間がかかる。とりわけ、ステップＳ１４０３のデータベースからの候補となる近辺の反射点の検索が最も機器への負担が大きく、遅延、メモリ負荷、消費電力増大となってしまう。これは、データベースのサイズが大きくなるほど問題となることが予想される。
【００１５】
なお、上記非特許文献１の既存のEKF-Loc onlyアルゴリズムでは、反射点を“すべて”サンプリングし、尤度を計算することで正しいデータアソシエーションを行うことに関しては述べられている。しかし、計算量を減らすために、反射点を如何に何個だけサンプリングすれば十分であるかという視点では述べられていない。システムの設計の観点からは、より多くの反射点をサンプリングすることで自己位置推定の精度を上げることが可能であるという点と、計算処理負荷を最小限に抑えたいという点とのバランスを考える必要がある。現状では、観測された反射点のうち、事前に経験的に決めた個数のみをサンプリングし計算処理するということが考えられる。
【００１６】
図１１ｃに示すように、実際の道路状況下は、ガードレール周辺には一時的に駐車する駐車車両や工事などによる一時的な障害物が存在し、それらから反射される反射点のデータを観測してしまうことが多々ある。ミリ波レーダーは、単体では観測された反射点がガードレールなどの本来の反射点から来たものなのか、あるいは障害物から反射されてきた偽物の反射点なのか、カメラの画像などの他の手段でも使わない限り判断することができない。結局のところ、障害物が存在しない場合は、反射点を２、３個サンプリングすれば十分に自己位置が正確に推定できる場合もあれば、障害物が存在する場合は、障害物から来た偽の反射点も含めて、全体で１０個以上サンプリングしなければ、十分な自己位置が推定できない場合もあり得るなど、サンプリングすべき反射点の数は道路状況に応じて著しく大きく変わる。
【００１７】
統計学的におけるサンプリング数の最適化に関して、サンプルの数が増えるほど信頼区間が狭まることが算出できるが、これはサンプリングの標本の数が少ない、例えば１０個ほどの場合は正しくなく、またそれらはガウス分布のノイズを持つことが仮定された話であり、道路に障害物が存在するなどの特殊な状況を考慮していない。さらに、今後、計算に使えるサンプリングの対象は、センサーの数が複数になった場合や、自動車の例では車-車間通信、路-車間通信などが実現した場合、大量の観測データのうちどれだけをサンプリングして計算処理をし、どれだけ以上はサンプリングせず計算負荷を最小限にとどめるのか、周辺の環境に応じて自動で判断できる機能が必要とされている。
【００１８】
本発明は、上記の問題点に鑑み、観測されたデータの質に応じて、計算処理する量をリソースと精度のバランスを取って調整できる自己位置推定装置などを提供することを目的とする。
【課題を解決するための手段】
【００１９】
上記目的を達成するために、本発明によれば、自車両の位置を前記自車両の周辺に存在する物体の位置に基づいて推定する自己位置推定装置であって、前記自車両の位置に対する複数の前記物体のそれぞれの相対位置の相対座標情報（後述する相対円座標に相当）をセンサーを介して取得する取得手段と、取得された前記複数の物体のそれぞれの相対座標情報から１つの物体の相対座標情報を選択する選択手段と、選択された前記相対座標情報を所定の手法で変換された絶対座標情報と、あらかじめ前記複数の物体のそれぞれの絶対位置の絶対座標情報を格納したデータベースとに基づいて、前記選択された物体に対応する候補を前記データベースから複数検索し、前記検索された複数の候補の絶対座標を前記自車両の位置に対する相対座標に変換し、変換された前記相対座標と前記選択された物体の相対座標との差分mdをそれぞれ算出する算出手段と、算出された前記複数の差分のうち、最も小さい差分の前記候補が前記選択された物体に該当するものとして、前記候補の絶対座標情報と所定のアルゴリズム（例えば、EKFアルゴリズム）とを用いて前記自車両の自己位置を推定する推定手段と、算出された前記複数の差分を記録する記録手段と、記録された前記複数の差分のうち、差分の小さい順に数えて最小のものから所定の個数（例えば、後述するＮ個）までの差分の平均値が所定の閾値より小さいか否かを判断する判断手段と、前記平均値が前記所定の閾値（Tmd）より小さいと判断された場合に、前記選択手段による相対座標情報の選択を中止し、前記推定手段によって推定された前記自車両の自己位置を出力する出力手段とを備える自己位置推定装置が提供される。この構成により、観測されたデータの質に応じて、計算処理する量をリソースと精度のバランスを取って調整できる。
【００２０】
また、本発明の自己位置推定装置において、前記物体の相対位置の相対座標を変数とする関数であって、前記物体が所定の物体（上述したガードレールや標識など）であるという信頼性の度合いを示す信頼度関数を備え、前記選択手段が、前記信頼度関数に基づいて、信頼度の高い順に前記物体の相対座標情報を選択することは、本発明の好ましい態様である。この構成により、信頼度の高い順に選択（サンプリング）できるため、効率よく、少ないサンプリング数で質のよい反射点を計算処理できる。
【００２１】
また、本発明の自己位置推定装置において、前記信頼度関数が所定の時間間隔ごとに更新されることは、本発明の好ましい態様である。この構成により、時間の経過とともに変化する周囲の環境に対応することができる。
【００２２】
また、本発明の自己位置推定装置において、前記取得手段が複数のセンサーを介して前記複数の物体のそれぞれの相対位置の相対座標情報を取得し、前記選択手段が、１つのセンサーで前記自己位置を推定することができない場合に、他のセンサーを介して取得された前記複数の物体のそれぞれの相対座標情報から１つの物体の相対座標情報を選択することは、本発明の好ましい態様である。この構成により、１つのセンサーで自己位置の推定を行うことができない場合でも複数のセンサーで自己位置の推定を行うことができる。
【００２３】
また、本発明の自己位置推定装置において、前記複数のセンサーそれぞれに信頼性の度合いを示す信頼度を設け、前記選択手段が、信頼度の高いセンサー順に、センサーを介して取得された前記複数の物体のそれぞれの相対座標情報から１つの物体の相対座標情報を選択することは、本発明の好ましい態様である。この構成により、効率よく、質のよい反射点を計算処理できる。
【００２４】
また、本発明の自己位置推定装置において、前記自車両の周辺に位置する、前記物体の相対位置の相対座標情報を取得するためのセンサーを備える他車両と通信するための通信手段を備え、前記推定手段が、前記他車両による前記他車両の自己位置の推定結果を前記通信手段を介して受信し、受信した前記推定結果を用いて前記自車両の自己位置を推定することは、本発明の好ましい態様である。この構成により、自車両のセンサーから十分信頼できる反射点をサンプリングできない場合でも自己位置の推定を行うことができる。
【００２５】
また、本発明の自己位置推定装置において、前記他車両に信頼性の度合いを示す信頼度を設け、前記信頼度は前記他車両が有するセンサーに設けられた信頼性の度合いを示す信頼度に依存した関数で定義されたことは、本発明の好ましい態様である。この構成により、効率よく、自己位置の推定を行うことができる。
【００２６】
また、本発明によれば、自車両の位置を前記自車両の周辺に存在する物体の位置に基づいて推定する自己位置推定方法であって、前記自車両の位置に対する複数の前記物体のそれぞれの相対位置の相対座標情報をセンサーを介して取得する取得ステップと、取得された前記複数の物体のそれぞれの相対座標情報から１つの物体の相対座標情報を選択する選択ステップと、選択された前記相対座標情報を所定の手法で変換された絶対座標情報と、あらかじめ前記複数の物体のそれぞれの絶対位置の絶対座標情報を格納したデータベースとに基づいて、前記選択された物体に対応する候補を前記データベースから複数検索し、前記検索された複数の候補の絶対座標を前記自車両の位置に対する相対座標に変換し、変換された前記相対座標と前記選択された物体の相対座標との差分をそれぞれ算出する算出ステップと、算出された前記複数の差分のうち、最も小さい差分の前記候補が前記選択された物体に該当するものとして、前記候補の絶対座標情報と所定のアルゴリズムとを用いて前記自車両の自己位置を推定する推定ステップと、算出された前記複数の差分を記録する記録ステップと、記録された前記複数の差分のうち、差分の小さい順に数えて最小のものから所定の個数までの差分の平均値が所定の閾値より小さいか否かを判断する判断ステップと、前記平均値が前記所定の閾値より小さいと判断された場合に、前記選択ステップによる相対座標情報の選択を中止し、前記推定ステップによって推定された前記自車両の自己位置を出力する出力ステップとを有する自己位置推定方法が提供される。この構成により、観測されたデータの質に応じて、計算処理する量をリソースと精度のバランスを取って調整できる。
【００２７】
また、本発明の自己位置推定方法において、前記物体の相対位置の相対座標を変数とする関数であって、前記物体が所定の物体であるという信頼性の度合いを示す信頼度関数を備え、前記選択ステップで、前記信頼度関数に基づいて、信頼度の高い順に前記物体の相対座標情報を選択することは、本発明の好ましい態様である。この構成により、信頼度の高い順に選択（サンプリング）できるため、効率よく、少ないサンプリング数で質のよい反射点を計算処理できる。
【００２８】
また、本発明の自己位置推定方法において、前記信頼度関数が所定の時間間隔ごとに更新されることは、本発明の好ましい態様である。この構成により、時間の経過とともに変化する周囲の環境に対応することができる。
【００２９】
また、本発明の自己位置推定方法において、前記取得ステップで、複数のセンサーを介して前記複数の物体のそれぞれの相対位置の相対座標情報を取得し、前記選択ステップで、１つのセンサーで前記自己位置を推定することができない場合に、他のセンサーを介して取得された前記複数の物体のそれぞれの相対座標情報から１つの物体の相対座標情報を選択することは、本発明の好ましい態様である。この構成により、１つのセンサーで自己位置の推定を行うことができない場合でも複数のセンサーで自己位置の推定を行うことができる。
【００３０】
また、本発明の自己位置推定方法において、前記複数のセンサーそれぞれに信頼性の度合いを示す信頼度を設け、前記選択ステップで、信頼度の高いセンサー順に、センサーを介して取得された前記複数の物体のそれぞれの相対座標情報から１つの物体の相対座標情報を選択することは、本発明の好ましい態様である。この構成により、効率よく、質のよい反射点を計算処理できる。
【００３１】
また、本発明の自己位置推定方法において、前記自車両の周辺に位置する、前記物体の相対位置の相対座標情報を取得するためのセンサーを備える他車両と通信する通信ステップを有し、前記推定ステップで、前記他車両による前記他車両の自己位置の推定結果を前記通信ステップで受信し、受信した前記推定結果を用いて前記自車両の自己位置を推定することは、本発明の好ましい態様である。この構成により、自車両のセンサーから十分信頼できる反射点をサンプリングできない場合でも自己位置の推定を行うことができる。
【００３２】
また、本発明の自己位置推定方法において、前記他車両に信頼性の度合いを示す信頼度を設け、前記信頼度は前記他車両が有するセンサーに設けられた信頼性の度合いを示す信頼度に依存した関数で定義されたことは、本発明の好ましい態様である。この構成により、効率よく、自己位置の推定を行うことができる。
【発明の効果】
【００３３】
本発明の自己位置推定装置及び自己位置推定方法は、上記構成を有し、観測されたデータの質に応じて、計算処理する量をリソースと精度のバランスを取って調整できる。
【図面の簡単な説明】
【００３４】
【図１】本発明の第１の実施の形態における、ある一つのセンサーにおいて観測された反射点群からいかに反射点をサンプリングして自己位置推定計算を行うかのフローの一例を示すフローチャートである。
【図２】本発明の第１の実施の形態におけるサンプリングをすべきかこれ以上はすべきでないかの判断をする処理フローの一例を示すフローチャートである。
【図３ａ】図２に示す処理フロー（アルゴリズム）を使った実際の実験結果の一例を示す図であり、横軸に時間フレーム（１フレームあたり20ミリ秒）、縦軸にEKF  Loc  Onlyのアルゴリズムにより計算された位置推定と正解位置との差分を計算誤差（自己位置推定誤差）として示した図である。
【図３ｂ】図２に示す処理フロー（アルゴリズム）を使った実際の実験結果の一例を示す図であり、サンプリング数８個における、横軸に時間フレーム、縦軸に時間フレームごとの観測された反射点群のmd値の平均を示した図である。
【図３ｃ】図２に示す処理フロー（アルゴリズム）を使った実際の実験結果の一例を示す図であり、自動調整の場合における横軸に時間フレーム、縦軸に時間フレームごとのサンプリングした反射点の数を示した図である。
【図４】本発明の第１から第３の実施の形態に係る自己位置推定装置の構成の一例を示す構成図である。
【図５ａ】本発明の第２の実施の形態における実施の方法を説明するための図が示され、ある道路状況下での自車と反射点と駐車車両とを示す図である。
【図５ｂ】本発明の第２の実施の形態における実施の方法を説明するための図が示され、横軸に反射点のmd値、縦軸に反射点の信頼度を示す関数を示す図である。
【図５ｃ】本発明の第２の実施の形態における実施の方法を説明するための図が示され、横軸に自車のセンサーから反射点までの方角θ、縦軸に反射点の信頼度を示す関数を示す図である。
【図６】本発明の第２の実施の形態における処理フローの一例を示すフローチャートである。
【図７ａ】本発明の第３の実施の形態における、ある１台の自車に複数のセンサーが装備されている場合の道路状況を示す図である。
【図７ｂ】本発明の第３の実施の形態における全体の構成図の一例を示す図である。
【図８】本発明の第３の実施の形態における処理フローの一例を示すフローチャートである。
【図９ａ】本発明の第３の実施の形態における、複数の自動車にセンサーが装備されている場合の道路状況の一例を示す図である。
【図９ｂ】本発明の第３の実施の形態における他の全体の構成図の一例を示す図である。
【図１０】本発明の第３の実施の形態における処理フローの一例を示すフローチャートである。
【図１１ａ】従来技術を説明するための図である。
【図１１ｂ】従来技術を説明するための図である。
【図１１ｃ】従来技術を説明するための図である。
【図１２】従来技術におけるある一つのセンサーにおいて観測された反射点群からいかに一つ一つの反射点をサンプリングして自己位置推定計算を行うかのフローの一例を示すフローチャートである。
【図１３】図１２のステップＳ１２０３におけるサンプリングされた反射点の処理の詳細を示す図である。
【図１４】図１３で使われるEKFアルゴリズムの詳細フローの一例を示すフローチャートである。
【発明を実施するための形態】
【００３５】
＜第１の実施の形態＞
上記問題を踏まえ、本発明では、逐次反射点をサンプリングしていき、図１４のステップＳ１４０５で求められたmd値を使って、観測された反射点の何個分を計算するのかを判断する。md値は、観測された反射点とそれに本来該当するデータベースの反射点との“ズレ”であると考える。その詳細について以下に述べる。本発明の第１の実施の形態を図１及び図２を用いて説明する。なお、図２のステップＳ２０１は上述した図１４に示すEKFアルゴリズムの計算を行うステップである。
【００３６】
従来の方法と異なり、図１では、反射点の計算後、これ以上サンプリングの必要がないというフラグが立っている場合（サンプリングを中止するリクエストが出ている場合）、それ以上はサンプリングせず、中断リクエストが送られ（ステップＳ１０７）、それに基づいてサンプリングが中止され、その時点での自己位置の推定結果が出力され（ステップＳ１０８）、終了する。一方、フラグがない場合は、データリクエストとして、他のサンプリングデータがないか（反射点群のすべてをサンプリングしたか）更に検索される（ステップＳ１０５）。なければ他のセンサーについて探すことになる（ステップＳ１０６）。
【００３７】
図２には、具体的に本発明における、サンプリングをすべきかこれ以上はすべきでないかの判断をするアルゴリズム（処理フロー）が記載されている。ステップＳ２０２において、それまでのすべてのサンプリングされた反射点のmd値を不図示の記録領域（例えば、ハードディスクやメモリなど）に記録する。ステップＳ２０３において、それまでサンプリングされた反射点のうち、md値の最も小さい反射点をＮ個選び、それらのmd値の平均を計算する。その平均が事前に決められた閾値Tmdより小さければ「十分にズレの小さい反射点がＮ個分取得できた」と判断し、これ以上のサンプリングは終了し（ステップＳ２０５）、次の時間フレームに移ると判断する。一方、その平均が閾値Tmdより小さくなければ次の反射点群の１つを選択する（ステップＳ２０４）。なお、サンプリングされた数がＮ個に満たない場合はサンプリングを続ける。
【００３８】
図３ａ〜図３ｃには、図２に示すアルゴリズムを使った実際の実験結果が示されている。ここでの実験は、ミリ波レーダーを自車の前方に取り付け、高速道路を走行した際にガードレールなどの周辺の反射点を検知し、事前に作成されたデータベースの反射点と比較したもので、EKF  Loc  OnlyアルゴリズムでＮ=３個、Tmd＝0.001として自己位置推定を行った結果である。データは20ミリ秒ごとに観測されている。
【００３９】
図３ａは、横軸に時間フレーム（１フレームあたり20ミリ秒）、縦軸にEKF  Loc  Onlyのアルゴリズムにより計算された位置推定と正解位置との差分を計算誤差（自己位置推定誤差）として示したものである。図３ａには複数のグラフがあり、それぞれのグラフは、毎時間フレームごとに何個の反射点をEKF  Loc  Onlyのアルゴリズムでサンプリングし、計算処理したかを示したものである。図３ａでは、横軸の時間フレーム700付近で、自車の速度などを含む異常なオドメトリー情報が計測され、計算誤差が大きくなっている状態が示されている。すなわち、このオドメトリーの異常値を打ち消すほどの多くの反射点をサンプリングして精度よく計算処理しなければ、推定された自己位置が正解と大きくずれていってしまう状況を示している。
【００４０】
サンプリングする反射点が８個以上であれば、時間フレーム700あたり以降で誤差が少なくなり、自己位置推定が正確に行われていることがわかる。その一方、サンプリングする反射点が７個以下の場合、オドメトリーの異常値に影響され、推定位置と実際の位置が回復せずに更に大きくなっていき、SLAMのアルゴリズムが破たんしていく様子が分かる。さらに本発明の自動でサンプリング数を決定する場合、自動調整の場合は８個以上の場合と同等の精度で計算できることが示されている。
【００４１】
図３ｂは、サンプリング数８個の場合における、横軸に時間フレーム、縦軸に時間フレームごとの観測された反射点群のmd値の平均を示したものである。md値は、データベースの反射点と観測された反射点とのズレを意味することから、時間フレーム700あたりで異常な反射点を計算していることがわかる。この値は、逐次サンプリングした際に計算してわかることから、サンプリングして計算に使われた反射点が妥当なものなのか、すなわち図１１ｂのような状況の場合なのか、図１１ｃの駐車車両などのノイズがある状況の場合なのか推測する手がかりとなる。
【００４２】
理想的なサンプリングの個数とは、図３ａ、図３ｂに示すように、反射点群のmd値の平均値が低いうちは良質な反射点が得られているとし、サンプリングする反射点の数が少なくても問題ない。それにより計算の負荷を抑えることができる。一方、時間フレーム700あたりに示されるようにmd値の平均値が高くなると、サンプリングされた反射点の質が悪く、多くの反射点をサンプリングしなければいけないとする。そのためには、毎時間フレームごとに、少なくともＮ個の反射点のmd値の平均がある閾値より低い状態の観測がされれば、それ以上はサンプリングをしないという方針を提案する。
【００４３】
図３ｃは、本発明の自動調整の場合における、横軸に時間フレーム、縦軸に時間フレームごとのサンプリングした反射点の数を示したものである。従来例のサンプリング反射点数が固定の場合と異なり、通常は反射点少なく、反射点が異常と判断された場合は多くの反射点をサンプリングする。反射点数の時間フレームあたりの平均は６個であった。自己位置推定の誤差も図３ａに示されるように、毎時間フレームごとのサンプリング数８個以上のグループと同じ、それ以上の精度で自己位置推定ができた。
【００４４】
これにより、第１の実施の形態により、反射点の質がよいとされた場合は、処理する反射点の数を低減させることでＣＰＵの負荷や消費電力を抑え、かつ、反射点が異常な場合は多くの反射点をサンプリングし、正確な値を得ようとする機能が達成された。
【００４５】
ここでは、本発明の実施の形態に係る自己位置推定装置の一例について図４を用いて説明する。図４に示すように、自己位置推定装置４００は、取得部４０１、選択部４０２、算出部４０３、推定部４０４、記録部４０５、判断部４０６、出力部４０７、通信部４０８から構成されている。取得部４０１は、自車両の位置に対する複数の物体の相対位置の相対座標情報（相対円座標）をセンサー（ミリ波レーダーなど）を介して取得するものである。選択部４０２は、取得された複数の物体の相対座標情報から１つの物体の相対座標情報を選択するものである。
【００４６】
算出部４０３は、選択された相対座標情報と、あらかじめ物体の絶対位置の絶対座標情報を格納したデータベースとに基づいて、選択された物体に対応する候補をデータベースから複数検索し、検索された候補の絶対座標を自車両の位置に対する相対座標に変換し、変換された相対座標と選択された物体の相対座標との差分をそれぞれ算出するものである。推定部４０４は、算出された複数の差分のうち、最も小さい差分の候補が選択された物体に該当するものとして、候補の絶対座標情報と所定のアルゴリズム（EKFアルゴリズム）とを用いて自車両の自己位置を推定するものである。
【００４７】
記録部４０５は、算出された複数の差分を不図示の記録領域（例えば、ハードディスクやメモリなど）に記録するものである。判断部４０６は、記録された複数の差分のうち、差分の小さい順に数えて最小のものから所定の個数までの差分の平均値が所定の閾値より小さいか否かを判断するものである。出力部４０７は、平均値が所定の閾値より小さいと判断された場合に、選択部４０２による相対座標情報の選択を中止し、推定部４０４によって推定された自車両の自己位置を不図示のディスプレイなどに出力するものである。通信部４０８は、自車両の周辺に位置する、物体の相対位置の相対座標情報を取得するためのセンサーを備える他車両と通信するためのものである。
【００４８】
＜第２の実施の形態＞
第２の実施の形態では、第１の実施の形態を拡張したものについて述べる。第１の実施の形態では、図１のステップＳ１０２に示すように、観測された反射点群からランダムに一つ一つの反射点をサンプリングしていた。しかし、図１１ｃで示したような誤った反射点となる駐車車両はある一定の大きさを持って空間領域にまたがっているため、ある方角で悪いサンプルが得られた場合は、次のサンプリングではその周辺からの反射点は避けるべきである。
【００４９】
これを実現するために、自車のセンサーから反射点までの方角θと、その方向からの反射点の信頼度を表す関数を提案する。過去のサンプリングの結果は信頼度の関数に反映され、次のサンプリングは、信頼度の高い座標にある反射点からサンプリングすることで効率よく、少ないサンプリング数で質の良い反射点を計算処理することができる。
【００５０】
図５ａ〜図５ｃには、第２の実施の形態における実施の方法を説明するための図が示されている。図５ａには、ある道路状況下での自車と反射点と駐車車両とが示されている。図５ｂには、横軸に反射点のmd値、縦軸に反射点の信頼度を示す関数が示されている。図５ｃには、横軸に自車のセンサーから反射点までの方角θ、縦軸に反射点の信頼度を示す関数が示されている。図５ａ〜図５ｃでは、例として３つの反射点P1、P2、P3が示されている。反射点P1、P2はガードレールから反射された良質な反射点である。反射点P3は駐車車両から反射されたノイズとしての反射点である。
【００５１】
サンプリングして計算を行った反射点のmd値から、図５ｂに示される、事前に定義される反射のmd値と信頼度との関係の関数を使って信頼度を計算する。md値とは予想される観測データからの“ズレ”であるため、md値が小さいほど信頼度が高くなるような関数が望ましい。図５ｂに示す例では単純な直線の場合が考えられている。
【００５２】
さらに、図５ｃに示すように、信頼度と観測される反射点の角度との関数が作成される。反射点の角度θが中心で高さが信頼度で表されるガウス関数をそれぞれ足し合わせていく（P1、P2、P3におけるガウス関数を足す）ことで、ガウス混合分布として一つの信頼度の関数が作成される。信頼度が負の場合には下に窪んだガウスとなり、信頼度が正の場合には上に凸となったガウスとなる。以下の数式１はｎ個の観測された反射点を反映させた場合の信頼度の式を示す。
【００５３】
信頼度（θ）＝Con１×ガウスP１（σ, θ-θ１）＋Con2×ガウスP２（σ, θ-θ２）＋・・・
Conn×ガウスPn（σ, θ-θn）  ・・・・・（数式１）
【００５４】
ここで、角度θ１から観測された反射点P1の図５ｂに示す関数から出される信頼度がCon１であり、角度θ２から観測された反射点P2の図５ｂに示す関数から出される信頼度がCon２である。σは事前に経験則で決められた定数であり、ガウス関数の幅の形状を決定するものである。信頼度が０は未評価を意味し、初期の値となる。したがって、図５ｃに示す関数は初期では値０の関数となる。次のサンプリングは、この更新された図５ｃの関数を使い、反射点群のそれぞれの信頼度を計算し、信頼度の高いものからサンプリングされる。
【００５５】
さらに効率よくサンプリングする方法として、図５ａに示す駐車車両などの障害物は、20ミリ秒ごとの観測データの中で、ある程度の観測時間幅にもまたがって存在することに着目する。すなわち、図５ｃに示す信頼度の関数は、次の時間フレームでもそのまま使用する。時間の経過とともに周囲の環境も変わることから、毎時間フレームごとに信頼度の関数に１より低いある定数である消失定数を掛けることで、未評価状態である０に収縮していく処理を行う。
【００５６】
図６は第２の実施の形態における処理フローを示す。第１の実施の形態における図１との差分は、図６のステップＳ６０１、ステップＳ６０３、ステップＳ６０５である。図１の処理に比べて処理が増えるが、全体の繰り返しサンプリング数を減らすことができる。ステップＳ６０１では、図５ｃに示す信頼度の関数（反射点信頼度関数）に消失定数を掛ける。ステップＳ６０２では、センサーから観測された反射点群の相対円座標（ｒ、θ）データを取得する。ステップＳ６０３では、観測された反射点群すべてのθから図５ｃに示す関数から信頼度を求め、信頼度の高い反射点からサンプリングする。
【００５７】
ステップＳ６０４では自己位置推定計算を行う。そして、計算された反射点を図５ｂに示す関数を使い、md値から信頼度に変換し、図５ｃに示す信頼度の関数にガウス成分として足し合わせ、信頼度の関数を更新する（計算された反射点を信頼度関数に反映させる）。ステップＳ６０５では、サンプリング中止のリクエストが出ているかを判断する。ステップＳ６０６では、該当するセンサーで観測されたすべての反射点をサンプリングしたかを判断する。ステップＳ６０７では、サンプリングがまだ十分でないため、他のセンサーがあれば検索する。ステップＳ６０８ではサンプリング中止のリクエストが出ているため、サンプリングを終了する。
【００５８】
＜第３の実施の形態＞
第１、第２の実施の形態では、センサー１つ、自車１台に関してのみ述べてきた。第３の実施の形態では、１台の自車に複数のセンサーがある場合、さらに自動車が複数台ある場合に拡張した例を述べる。ある１台の自車に複数のセンサーが装備されている場合の道路状況を図７ａに、全体の構成図を図７ｂに、その場合の処理フローを図８に示す。
【００５９】
また、複数の自動車にセンサーが装備されている場合の道路状況を図９ａに、全体の構成図を図９ｂに、その場合の処理フローを図１０に示す。センサーが複数の場合も、センサーが一つの場合を拡張したものとして扱う。すなわち、各センサーに信頼度を設ける。ある一つのセンサーにおけるサンプリングされた反射点群の信頼度を足し合わせることにより、センサー一つとしての信頼度を計算できる。それを以下の数式２で示す。あるセンサーのＮ個の反射点の信頼度conを足し合わせることで、センサー全体としての信頼度を計算する。
【００６０】
センサーの信頼度  ＝  Σconk (k=1 to N)  ・・・（数式２）
【００６１】
このようにセンサーに信頼度を設けることで、どのセンサーから観測点をサンプリングするかはセンサーの信頼度の高い順に選ばれ、自己位置推定を行う。その処理フローの図を図８に示す。
【００６２】
同様に、複数の車両における場合でも、複数のセンサーがある場合を拡張したものとして扱う。すなわち、各車両にも信頼度を設ける。ある一つの車両において、その車両がもつセンサー群の上述の信頼度を足し合わせることにより、車両一つとしての信頼度を計算できる。それを以下の数式３で示す。ある車両が持つＭ個のセンサーの信頼度を足し合わせることで、車両全体としての信頼度を計算する。
【００６３】
車両の信頼度  ＝  Σセンサーの信頼度k (k=1 to M)  ・・・（数式３）
【００６４】
自車のセンサーから十分信頼できる反射点をサンプリングできない場合は、高い信頼度を持つ他車をサンプリングし、観測データを車-車通信などにより、他車が算出した他車の自己位置推定値を読み取り、その他車との相対距離から自車の自己位置推定を行うことができる。その処理フロー図を図１０に示す。
【００６５】
なお、上述した本発明の各実施の形態では、模式的なブロック図を用いて説明を行ったが、これらのブロック図に記載されている各ブロックは、ハードウェア及び／又はソフトウェアによって実現可能である。また、フローチャートによって表されている動作は、ハードウェア及び／又は各処理を定めたプログラムをＣＰＵ（Central Processing Unit：中央演算処理装置）によって実行させることによって実現可能である。
【産業上の利用可能性】
【００６６】
本発明に係る自己位置推定装置及び自己位置推定方法は、観測されたデータの質に応じて、計算処理する量をリソースと精度のバランスを取って調整できるため、自車両の位置を自車両の周辺に存在する物体の位置に基づいて推定する自己位置推定装置などに有用である。
【符号の説明】
【００６７】
４００  自己位置推定装置
４０１  取得部（取得手段）
４０２  選択部（選択手段）
４０３  算出部（算出手段）
４０４  推定部（推定手段）
４０５  記録部（記録手段）
４０６  判断部（判断手段）
４０７  出力部（出力手段）
４０８  通信部（通信手段）
【図１】
2015152991.tif 000003
【図２】
2015152991.tif 000004
【図３ａ】
2015152991.tif 000005
【図３ｂ】
2015152991.tif 000006
【図３ｃ】
2015152991.tif 000007
【図４】
2015152991.tif 000008
【図５ａ】
2015152991.tif 000009
【図５ｂ】
2015152991.tif 000010
【図５ｃ】
2015152991.tif 000011
【図６】
2015152991.tif 000012
【図７ａ】
2015152991.tif 000013
【図７ｂ】
2015152991.tif 000014
【図８】
2015152991.tif 000015
【図９ａ】
2015152991.tif 000016
【図９ｂ】
2015152991.tif 000017
【図１０】
2015152991.tif 000018
【図１１ａ】
2015152991.tif 000019
【図１１ｂ】
2015152991.tif 000020
【図１１ｃ】
2015152991.tif 000021
【図１２】
2015152991.tif 000022
【図１３】
2015152991.tif 000023
【図１４】
2015152991.tif 000024