JP7226714B2

JP7226714B2 - 位置ずれ補正システム及びプログラム

Info

Publication number: JP7226714B2
Application number: JP2019100567A
Authority: JP
Inventors: 章弘渡邉; 晋也田口
Original assignee: Denso Corp; Toyota Central R&D Labs Inc
Current assignee: Denso Corp; Toyota Central R&D Labs Inc
Priority date: 2019-05-29
Filing date: 2019-05-29
Publication date: 2023-02-21
Anticipated expiration: 2039-05-29
Also published as: JP2020193907A

Description

本発明は、位置ずれ補正システム及びプログラムに関する。

従来から、自動運転等では、走行環境の地図に含まれる物標と、車載センサにより物標とを対応付け、地図に対する車両の位置を推定する技術が使われている。また、ＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓＬｏｃａｌｉｚａｔｉｏｎＡｎｄＭａｐｐｉｎｇ）のように、車両等の移動体搭載の周辺環境認識装置を用いて物標の位置を推定し、走行環境の地図を生成する技術がある（例えば、特許文献１）。

特開２０１８－０８１２５２号公報

しかし、ＳＬＡＭ等の技術により相対精度の高い物標の位置の推定は可能であるが、絶対位置に関しては、絶対位置が既知の物標を用いる必要があるため、絶対位置の精度が必ずしも十分ではない状況においては物標の位置の推定精度が落ちてしまう、という問題があった。

また、高精度な測位システムを用いて車両の絶対位置の精度を確保することが考えられるが、高コストになってしまう、という問題があった。

本発明は、上記問題点を解決するためになされたもので、車両の絶対位置の精度が必ずしも十分ではない状況であっても、低コスト、かつ、精度よく物標の絶対位置を推定することができる位置ずれ補正システム及びプログラムを提供することを目的とする。

上記の目的を達成するために、本発明の位置ずれ補正システムは、車両に搭載された第１測位装置により検出された前記車両の世界座標系の位置である第１車両位置を取得する第１取得部と、前記車両に搭載された第２測位装置により前記車両の位置の変化に基づいて検出された前記車両の世界座標系の位置である第２車両位置を取得する第２取得部と、前記車両に搭載されたセンサを用いて検出された前記車両の周辺の物標の車両座標系における位置を取得する物標検出部と、前記第１取得部が取得した前記第１車両位置と、前記第１車両位置の検出に対応して前記物標検出部により取得された物標の位置とに基づいて、前記物標の世界座標系における位置である第１物標位置を算出する第１物標位置算出部と、前記第２取得部が取得した前記第２車両位置と、前記第２車両位置の検出に対応して前記物標検出部により取得された物標の位置とに基づいて、前記物標の世界座標系における位置である第２物標位置を算出する第２物標位置算出部と、前記物標検出部によって検出された複数の物標の各々について、複数回の走行において前記第１物標位置算出部により算出された前記物標の前記第１物標位置や前記物標の大きさ等に基づいて、同一物標に対する第１物標群を生成し、前記第１物標群内の前記物標の前記第１物標位置のばらつきを求め、前記複数の物標のうち前記ばらつきが所定の第１閾値以下である物標を前記第１物標群から選択する第１物標群選択部と、前記第１物標群選択部により選択された前記第１物標群に含まれる物標の各々について、前記物標の前記第１物標位置の平均位置である第１平均位置を算出する第１物標群平均位置算出部と、前記第１物標群平均位置算出部で算出された前記第１平均位置に基づいて、同一の走行において前記センサを用いて検出された前記物標の各々について算出された前記第２物標位置の各々を補正する第２物標群位置補正部と、前記前記第２物標群位置補正部により補正された前記物標の前記第２物標位置の平均位置である第２平均位置を算出する第２物標群平均位置算出部と、前記第２物標群平均位置算出部で算出された第２平均位置を、前記第１物標群に含まれる物標各々の最終的な位置として出力する出力部と、を含んで構成されている。

また、本発明のプログラムは、コンピュータを、車両に搭載された第１測位装置により検出された前記車両の世界座標系の位置である第１車両位置を取得する第１取得部、前記車両に搭載された第２測位装置により前記車両の位置の変化に基づいて検出された前記車両の世界座標系の位置である第２車両位置を取得する第２取得部、前記車両に搭載されたセンサを用いて検出された前記車両の周辺の物標の車両座標系における位置を取得する物標検出部、前記第１取得部が取得した前記第１車両位置と、前記第１車両位置の検出に対応して前記物標検出部により取得された物標の位置とに基づいて、前記物標の世界座標系における位置である第１物標位置を算出する第１物標位置算出部、前記第２取得部が取得した前記第２車両位置と、前記第２車両位置の検出に対応して前記物標検出部により取得された物標の位置とに基づいて、前記物標の世界座標系における位置である第２物標位置を算出する第２物標位置算出部、
前記物標検出部によって検出された複数の物標の各々について、複数回の走行において前記第１物標位置算出部により算出された前記物標の前記第１物標位置や前記物標の大きさ等に基づいて、同一物標に対する第１物標群を生成し、前記第１物標群内の前記物標の前記第１物標位置のばらつきを求め、前記複数の物標のうち前記ばらつきが所定の第１閾値以下である物標を前記第１物標群から選択する第１物標群選択部、前記第１物標群選択部により選択された前記第１物標群に含まれる物標の各々について、前記物標の前記第１物標位置の平均位置である第１平均位置を算出する第１物標群平均位置算出部、前記第１物標群平均位置算出部で算出された前記第１平均位置に基づいて、同一の走行において前記センサを用いて検出された前記物標の各々について算出された前記第２物標位置の各々を補正する第２物標群位置補正部、前記前記第１物標群に含まれる物標の各々について、前記第２物標群位置補正部により補正された前記物標の前記第２物標位置の平均位置である第２平均位置を算出する第２物標群平均位置算出部、及び前記第２物標群平均位置算出部で算出された第２平均位置を、前記第１物標群に含まれる物標各々の最終的な位置として出力する出力部、として機能させるためのプログラムである。

本発明の補正システム及びプログラムによれば、第１取得部が、車両に搭載された第１測位装置により検出された当該車両の世界座標系の位置である第１車両位置を取得し、第２取得部が、当該車両に搭載された第２測位装置により当該車両の位置の変化に基づいて検出された当該車両の世界座標系の位置である第２車両位置を取得し、物標検出部が、当該車両に搭載されたセンサを用いて検出された当該車両の周辺の物標の車両座標系における位置を取得し、第１物標位置算出部が、第１取得部が取得した第１車両位置と、第１車両位置の検出に対応して物標検出部により取得された物標の位置とに基づいて、当該物標の世界座標系における位置である第１物標位置を算出し、第２物標位置算出部が、第２取得部が取得した第２車両位置と、第２車両位置の検出に対応して物標検出部により取得された物標の位置とに基づいて、当該物標の世界座標系における位置である第２物標位置を算出する。

そして、第１物標群選択部が、物標検出部によって検出された複数の物標の各々について、複数回の走行において第１物標位置算出部により算出された当該物標の第１物標位置や当該物標の大きさ等に基づいて、同一物標に対する第１物標群を生成し、第１物標群内の物標の第１物標位置のばらつきを求め、当該複数の物標のうち当該ばらつきが所定の第１閾値以下である物標の第１物標群を選択し、第１物標群平均位置算出部が、第１物標群選択部により選択された第１物標群に含まれる物標の各々について、当該物標の第１物標位置の平均位置である第１平均位置を算出し、第２物標群位置補正部が、第１物標群に含まれる物標の各々の第１平均位置に基づいて、同一の走行において当該センサを用いて検出された当該物標の各々について算出された第２物標位置の各々を補正し、第２物標群平均位置算出部が、第１物標群に含まれる物標の各々について、当該第２物標群位置補正部により補正された当該物標の第２物標位置の平均位置である第２平均位置を算出する。

このように、車両に搭載されたセンサを用いて検出された当該車両の周辺の物標の車両座標系における位置と、車両の世界座標系の位置である第１車両位置とに基づいて当該物標の世界座標系における位置である第１物標位置を算出し、当該物標の位置と、当該車両の位置の変化に基づいて検出された当該車両の世界座標系の位置である第２車両位置とに基づいて当該物標の世界座標系における位置である第２物標位置を算出し、検出された複数の物標の各々について、複数回の走行において算出された当該物標の第１物標位置や当該物標の大きさ等に基づいて、同一物標に対する第１物標群を生成し、第１物標群内の物標の第１物標位置のばらつきを求め、当該ばらつきが所定の第１閾値以下である物標の第１物標群を選択し、第１物標群に含まれる物標の各々について算出した当該物標の第１物標位置の平均位置である第１平均位置に基づいて、同一の走行において当該センサを用いて検出された当該物標の各々について算出された第２物標位置の各々を補正し、第１物標群に含まれる物標の各々について、補正された当該物標の第２物標位置の平均位置である第２平均位置を算出することにより、低コスト、かつ、精度よく物標の絶対位置を推定することができる。

また、本発明の位置ずれ補正システムの前記第２物標群位置補正部は、前記第１物標群に含まれる物標の前記第１平均位置と、同一の走行において前記センサを用いて検出された前記物標の各々の前記第２物標位置との対応付けを行い、前記物標の各々の前記第２物標位置を座標変換した結果と、前記物標の各々の前記第２物標位置と対応付けられた前記第１物標群に含まれる物標の前記第１平均位置の各々との位置ずれが最小となる第１座標変換行列を算出し、前記第１座標変換行列に基づいて、同一の走行において前記センサを用いて検出された前記物標の各々について算出された前記第２物標位置の各々を補正することができる。

また、本発明の位置ずれ補正システムの前記第２物標群位置補正部は、前記第１座標変換行列を算出し、前記第１座標変換行列に基づいて前記物標の前記第２物標位置を座標変換した結果と、前記物標の前記第２物標位置と対応付けられた前記第１物標群に含まれる物標の前記第１平均位置との位置ずれである残差が所定の第２閾値より大きい物標を、同一の走行において前記センサを用いて検出された前記物標から除外し、前記第１物標群に含まれる物標の前記第１平均位置と、同一の走行において前記センサを用いて検出された前記物標の各々の前記第２物標位置との対応付けを行い、前記位置ずれが最小となる前記第２座標変換行列を算出し、前記第２座標変換行列に基づいて、前記センサを用いて同時に検出された前記物標の各々について算出された前記第２物標位置の各々を補正することができる。

以上説明したように、本発明の位置ずれ補正システム及びプログラムによれば、低コスト、かつ、精度よく物標の絶対位置を推定することができる、という効果が得られる。

本発明の第１の実施の形態における位置ずれ補正装置の概略構成を示すブロック図である。本発明の第１の実施の形態における世界座標系における車両の位置・姿勢と車両座標系との関係を示すイメージ図である。本発明の第１の実施の形態における車両座標系における物標の位置の例を示すイメージ図である。本発明の第１の実施の形態における走行環境における物標の位置の真値の例を示すイメージ図である。本発明の第１の実施の形態における複数走行における第１物標位置の例を示すイメージ図である。本発明の第１の実施の形態における複数走行の第１物標位置の重ね合わせの例を示すイメージ図である。本発明の第１の実施の形態における各物標の第１物標位置の平均位置の例を示すイメージ図である。本発明の第１の実施の形態における複数走行の第１物標位置群と第１平均位置と真値との比較を表すイメージ図である。本発明の第１の実施の形態における第１物標群の選択の例を示すイメージ図である。本発明の第１の実施の形態における第１平均位置の例を示すイメージ図である。本発明の第１の実施の形態における物標の第１平均位置と真値との比較例を示すイメージ図である。本発明の第１の実施の形態における第１平均位置群への物標位置群２の位置合わせの例を示すイメージ図である。本発明の第１の実施の形態における複数走行の各物標の第２物標位置の集合である、物標位置群２の例を示すイメージ図である。本発明の第１の実施の形態における複数走行の物標位置群２の重ね合わせの例を示すイメージ図である。本発明の第１の実施の形態における第２平均位置の例を示すイメージ図である。本発明の第１の実施の形態における物標の第２平均位置と真値との比較例を示すイメージ図である。本発明の第１の実施の形態における物標位置収集処理ルーチンの内容を示すフローチャートである。本発明の第１の実施の形態における位置ずれ補正処理ルーチンの内容を示すフローチャートである。本発明の第２の実施の形態における位置ずれ補正システムの概略構成を示すブロック図である。本発明の第２の実施の形態における車載器の概略構成を示すブロック図である。本発明の第２の実施の形態における位置ずれ補正装置の概略構成を示すブロック図である。本発明の第２の実施の形態における位置あわせに用いる第２物標群の選択例を示すイメージ図である。本発明の第２の実施の形態における物標位置収集処理ルーチンの内容を示すフローチャートである。本発明の第２の実施の形態における位置ずれ補正処理ルーチンの内容を示すフローチャートである。

以下、図面を参照して本発明の実施の形態を詳細に説明する。
＜本発明の第１の実施の形態に係る位置ずれ補正システム１の構成＞
図１を用いて、本発明の実施の形態にかかる位置ずれ補正システム１について説明する。図１に示すように、本発明の実施の形態に係る位置ずれ補正システム１は、カメラ１０と、第１測位装置１１と、第２測位装置１２と、位置ずれ補正装置２０とを備えて構成される。

カメラ１０は、車両に搭載され、当該車両が走行している際に、当該車両の周辺を撮影する。例えば、カメラ１０はステレオカメラであり、車両の前方に搭載され、１秒毎に車両前方を撮影する（図３）。そして、カメラ１０は、撮影した画像を位置ずれ補正装置２０に渡す。

第１測位装置１１は、車両に搭載され、当該車両の世界座標系の位置である第１車両位置を測位する。具体的には、第１測位装置１１は、図２に示すように、測位衛星等を用いて得られる緯度・経度等を用いて、世界座標系（固定座標系）上で表現される車両の位置及び姿勢（方位）を測位する。以下では、第１測位装置１１により測位された車両の位置及び姿勢を第１車両位置と呼ぶこととする。

図２に示すように、世界座標系（Ｘｗ，Ｙｗ，Ｚｗ）は固定されているのに対し、車両座標系（Ｘ_ｖ，ｔ，Ｙ_ｖ，ｔ，Ｚ_ｖ，ｔ）は各時刻ｔの車両位置・姿勢により変化する。このため、第１車両位置は誤差を持つものの、測位衛星等の観測状況が良好な場所では、真値を中心に小さな誤差内に分布することが期待できる。一方、衛星の観測状況が不良な場所では、誤差が大きくなる。また、第１車両位置を時系列で見た場合、誤差の蓄積による影響はない。そして、第１測位装置１１は、測位した第１車両位置を、位置ずれ補正装置２０に渡す。

第２測位装置１２は、車両に搭載され、車両の位置の変化に基づいて検出された当該車両の世界座標系の位置である第２車両位置を測位する。具体的には、第２測位装置１２は、ジャイロセンサ等を用いて取得した車両の相対的な位置や姿勢の変化を積算することにより、相対的な位置・姿勢（車両軌跡）を算出する。第２車両位置の初期値として、ある時刻において第１測位装置１１により測位された第１車両位置を採用することで、車両の相対的な位置や姿勢の変化から、世界座標系上の車両の位置・姿勢を得ることができる。以下では、第２測位装置１２により算出された車両の世界座標系上の位置・姿勢を第２車両位置と呼ぶこととする。時系列で見た場合、第２車両位置は各時刻間の位置・姿勢の相対的な変化の誤差は小さいが、初期位置のずれ及び誤差の積算により、世界座標系における真値に対する誤差は大きくなる。そして、第２測位装置１２は、測位した第２車両位置を、位置ずれ補正装置２０に渡す。

位置ずれ補正装置２０は、走行環境に含まれる物標を検出し、検出された物標の世界座標系における位置情報を推定する。物標とは、路側の立体物（標識、信号等）や路面標示（区画線、横断歩道等）等の標識となるものである。

具体的には、位置ずれ補正装置２０は、ＣＰＵと、ＲＡＭと、後述する物標位置収集処理ルーチン及び位置ずれ補正処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。位置ずれ補正装置２０は、物標位置収集部２１０と、物標位置記憶部２２０と、位置ずれ補正部２３０と、出力部２４０とを備えて構成される。

物標位置収集部２１０は、車両の走行における、車両の周辺の物標の第１車両位置に基づく第１物標位置と、車両の周辺の物標の第２車両位置に基づく第２物標位置とを、複数回の走行について収集する。

具体的には、物標位置収集部２１０は、画像取得部２１１と、物標検出部２１２と、第１取得部２１３と、第１物標位置算出部２１４と、第２取得部２１５と、第２物標位置算出部２１６とを備えて構成される。

画像取得部２１１は、カメラ１０により撮影された画像を取得し、物標検出部２１２に渡す。

物標検出部２１２は、車両に搭載されたセンサを用いて検出された当該車両の周辺の物標の車両座標系における位置を取得する。具体的には、物標検出部２１２は、カメラ１０により撮影された画像を用いて、画像解析処理により、画像に写っている物標を検出し、当該物標の画像上の位置から、車両座標系における位置を検出する（図３の座標（Ｘｐ，Ｙｐ，Ｚｐ））。そして、物標検出部２１２は、検出した物標の車両座標系における位置を、第１物標位置算出部２１４及び第２物標位置算出部２１６に渡す。

第１取得部２１３は、車両に搭載された第１測位装置１１により検出された当該車両の第１車両位置を取得する。具体的には、第１取得部２１３は、第１測位装置１１により測位された、物標検出部２１２が検出した物標が撮影された時刻における第１車両位置を取得する。そして、第１取得部２１３は、取得した第１車両位置を、第１物標位置算出部２１４に渡す。

第１物標位置算出部２１４は、第１取得部２１３が取得した第１車両位置と、第１車両位置が測位された時刻に対応して物標検出部２１２により取得された物標の位置とに基づいて、当該物標の世界座標系における位置である第１物標位置を算出する。具体的には、第１物標位置算出部２１４は、車両座標系における物標の位置を、第１車両位置を用いて世界座標系に変換することにより、第１物標位置を算出する。そして、第１物標位置算出部２１４は、算出した第１物標位置を、物標位置記憶部２２０に格納する。

第２取得部２１５は、車両に搭載された第２測位装置１２により当該車両の第２車両位置を取得する。具体的には、第２取得部２１５は、第２測位装置１２により測位された物標検出部２１２が検出した物標が撮影された時刻における第２車両位置を取得する。そして、第２取得部２１５は、取得した物標の第２車両位置を、第２物標位置算出部２１６に渡す。

第２物標位置算出部２１６は、第２取得部２１５が取得した第２車両位置と、第２車両位置が測位された時刻に対応して物標検出部２１２により取得された物標の位置とに基づいて、当該物標の世界座標系における位置である第２物標位置を算出する。具体的には、第２物標位置算出部２１６は、車両座標系における物標の位置を、第２車両位置を用いて世界座標系に変換することにより、第２物標位置を算出する。そして、第２物標位置算出部２１６は、算出した第２物標位置を、物標位置記憶部２２０に格納する。

物標位置記憶部２２０には、ある走行環境での車両の同一走行において検出された物標の各々についての第１物標位置及び第２物標位置が、車両の走行毎に格納されている。具体的には、物標位置記憶部２２０には、車両の同一走行において検出された各物標の車両座標系の位置と、車両の同一走行における各時刻の第１車両位置及び第２車両位置とから算出した、各物標の第１物標位置及び第２物標位置が格納される。当該車両が、当該走行環境を複数回走行することにより、複数の走行の各々についての、各物標の第１物標位置及び第２物標位置が、物標位置記憶部２２０に蓄積される。

位置ずれ補正部２３０は、物標位置記憶部２２０に各物標について格納された複数の第１物標位置及び第２物標位置に基づいて、走行環境に含まれる複数の物標の世界座標系における位置を推定する。図４に、走行環境上に６つの物標が含まれる場合の、物標の世界座標系における位置の真値の例を示す。図４において、座標軸は世界座標系を、矢印（座標軸を除く）は車両が走行する走行経路を、各点は各物標の位置の真値を表す。すなわち、位置ずれ補正部２３０は、各物標の世界座標系における位置の真値を、物標位置収集部２１０により収集された第１物標位置及び第２物標位置を用いて推定する。

具体的には、位置ずれ補正部２３０は、第１物標群選択部２３１と、第１物標群平均位置算出部２３２と、第２物標群位置補正部２３３と、第２物標群平均位置算出部２３４とを備えて構成される。

図５に、４回の走行（走行１～走行４）において算出された各物標の第１物標位置の集合である、物標位置群１の例を示す。図６に、図５における各走行の物標位置群１を重ね合わせたもの示す。図５及び図６に示すように、各走行における物標位置群１には、観測のばらつきが生じる。測位衛星の状態に基づく第１車両位置の誤差と、車両に搭載されたセンサにより検出した物標の位置の差とが存在するためである。ここで、車両に搭載されたセンサにより検出した物標の位置誤差は、第１車両位置の誤差と比較して相対的に小さいものとする。誤差の大きい第１物標位置を含む物標を真値の推定に用いると、推定結果にも誤差が含まれてしまう。例えば図７に示すように各物標の第１物標位置の平均位置を採用すると、第１物標位置のばらつきが大きい物標が存在する場合には、図８に示すように真値との位置誤差が大きい物標が存在する。

そこで、第１物標群選択部２３１は、物標検出部２１２によって検出された複数の物標の各々について、複数回の走行において第１物標位置算出部２１４により算出された当該物標の第１物標位置や物標の大きさ等に基づいて、同一物標に対する第１物標群を生成し、第１物標群内の物標の第１物標位置のばらつきを求め、複数の物標のうちばらつきが所定の第１閾値以下である物標の第１物標群を選択する。具体的には、第１物標群選択部２３１は、図９に示すように、同一の物標に対する第１物標位置の分布を求め、ばらつき（分散）が所定の第１閾値以下である物標のみを選択して、第１物標群とする。図９において、白抜きの円が、第１物標位置のばらつきが、第１閾値以下でない物標の第１物標位置を示している。また、図９において、黒塗りの円が、第１物標位置のばらつきが、第１閾値以下となる物標の第１物標位置を示している。このように、観測誤差が大きい物標については、第１物標位置を用いると精度が下がるため、これを排除しておくことができる。そして、第１物標群選択部２３１は、第１物標群を、第１物標群平均位置算出部２３２に渡す。

第１物標群平均位置算出部２３２は、第１物標群選択部２３１により選択された第１物標群に含まれる物標の各々について、当該物標の第１物標位置の平均位置である第１平均位置を算出する。具体的には第１物標群平均位置算出部２３２は、図１０に示すように第１物標群に含まれる物標の各々について、当該物標の第１物標位置（図１０の白抜きの円）の平均位置である第１平均位置（図１０の黒塗りの円の位置）を算出する。第１平均位置は単純な算術平均でもよいし、物標検出の結果に対して信頼度が付与されている場合は加重平均でもよい。算出した第１平均位置の集合を物標平均位置群１とする。図１１に示すように、図８と比較して、位置誤差の大きい物標が排除され、第１平均位置が真値に近いものとなり、誤差が小さくなっていることが分かる。

第２物標群位置補正部２３３は、第１物標群に含まれる物標の各々の第１平均位置と、同一の走行においてセンサを用いて検出された物標の各々の第２物標位置との対応付けを行い、当該物標の各々の第２物標位置を座標変換した結果と、当該物標の各々の第２物標位置と対応付けられた第１物標群に含まれる物標の第１平均位置の各々との位置ずれが最小となる第１座標変換行列を算出し、第１座標変換行列に基づいて、同一の走行においてセンサを用いて検出された当該物標の各々について算出された第２物標位置の各々を補正する。

具体的には、第２物標群位置補正部２３３は、まず、図１２に示すように、第１物標群平均位置算出部２３２により算出された物標平均位置群１に対して、当該第１物標群に含まれる物標の各々の第２物標位置である第２物標位置群を対応付ける。ここで、物標平均位置群１は、真値との誤差が小さいものである。図１３に、上述の４回の走行（走行１～走行４）において算出された各物標の第２物標位置の集合である、物標位置群２の例を示す。図１４に、図１３における各走行の物標位置群２を重ね合わせたもの示す。

次に、第２物標群位置補正部２３３は、対応付けの結果に基づいて、当該物標の各々の第２物標位置を座標変換した結果と、当該物標の各々の第２物標位置と対応付けられた第１物標群に含まれる物標の第１平均位置の各々との位置ずれが最小となる第１座標変換行列を算出する。第１座標変換行列は、回転・並進から構成される。第２物標群位置補正部２３３は、算出した第１座標変換行列により、第２物標位置群を変換することにより補正した補正第２物標位置群を算出する。なお、ある物標が物標検出部２１２により検出されていない場合には、第２物標群位置補正部２３３は、当該物標を位置合わせから除外する。そして、第２物標群位置補正部２３３は、補正第２物標位置群を第２物標群平均位置算出部２３４に渡す。

第２物標群平均位置算出部２３４は、第１物標群に含まれる物標の各々について、第２物標群位置補正部２３３により補正された当該物標の第２物標位置の平均位置である第２平均位置を算出する。具体的には、第２物標群平均位置算出部２３４は、第２物標群位置補正部２３３で算出した補正第２物標位置群に対して、第１物標群選択部２３１及び第１物標群平均位置算出部２３２と同様に、位置のばらつきが所定の第１閾値（第１閾値と異なる閾値を用いてもよい）以下の物標の集合である第２物標群を選択し、第２物標群に含まれる物標の各々について、補正された当該物標の第２物標位置の平均位置である第２平均位置を算出する（図１５）。図１６に、各物標の第２平均位置と、真値との比較を示す。図１６に示すように、各物標の第２平均位置は、各物標の真値にほぼ等しい結果が得られる。

出力部２４０は、第１物標群に含まれる物標の各々について、当該物標の第２平均位置を当該物標の最終的な位置として出力する。

＜本発明の第１の実施の形態に係る位置ずれ補正システム１の作用＞
次に、図１７を参照して、本実施形態の位置ずれ補正システム１の物標位置収集処理ルーチンについて説明する。物標位置収集処理ルーチンは、車両が走行する毎に、繰り返し実行される。

まず、ステップＳ１００において、画像取得部２１１は、カメラ１０により撮影された画像を取得する。

ステップＳ１１０において、物標検出部２１２は、上記ステップＳ１００により取得した画像を用いて検出された当該車両の周辺の物標の車両座標系における位置を取得する。

ステップＳ１２０において、第１取得部２１３は、車両に搭載された第１測位装置１１により検出された当該車両の第１車両位置を取得する。

ステップＳ１３０において、第１物標位置算出部２１４は、上記ステップＳ１２０により取得した第１車両位置と、第１車両位置が測位された時刻に対応して上記ステップＳ１１０により取得された物標の位置とに基づいて、当該物標の世界座標系における位置である第１物標位置を算出する。

ステップＳ１４０において、第１物標位置算出部２１４は、上記ステップＳ１３０により算出された第１物標位置を、物標位置記憶部２２０に格納する。

ステップＳ１５０において、第２取得部２１５は、車両に搭載された第２測位装置１２により当該車両の第２車両位置を取得する。

ステップＳ１６０において、第２物標位置算出部２１６は、上記ステップＳ１５０により取得した第２車両位置と、第２車両位置が測位された時刻に対応して上記ステップＳ１１０により取得された物標の位置とに基づいて、当該物標の世界座標系における位置である第２物標位置を算出する。

ステップＳ１７０において、第２物標位置算出部２１６は、上記ステップＳ１６０により算出された第２物標位置を、物標位置記憶部２２０に格納する。

ステップＳ１８０において、物標位置収集部２１０は、走行が終了したか否かを判定する。

走行が終了していない場合（ステップＳ１８０のＮＯ）、ステップＳ１９０において、所定時間待機し、ステップＳ１００に戻り、再度ステップＳ１００～Ｓ１７０の処理を繰り返す。

一方、走行が終了している場合（ステップＳ１８０のＹＥＳ）、物標位置収集処理ルーチンを終了する。上記ステップＳ１４０で物標位置記憶部２２０に格納された第１物標位置は、同一走行における第１物標位置として物標位置記憶部２２０に格納される。また、上記ステップＳ１７０で物標位置記憶部２２０に格納された第２物標位置は、同一走行における第２物標位置として物標位置記憶部２２０に格納される。

次に、図１８を参照して、本実施形態の位置ずれ補正システム１の位置ずれ補正処理ルーチンについて説明する。位置ずれ補正処理ルーチンは、上記物標位置収集処理ルーチンが、所定回数実行される毎に実行される。

まず、ステップＳ２００において、第１物標群選択部２３１は、上記物標位置収集処理ルーチンにより検出された複数の物標の各々について、第１物標群選択部２３１は、物標検出部２１２によって検出された複数の物標の各々について、複数回の走行において上記ステップＳ１４０により算出された当該物標の第１物標位置や物標の大きさ等に基づいて、同一物標に対する第１物標群を生成し、第１物標群内の物標の第１物標位置のばらつきを求め、複数の物標のうちばらつきが所定の第１閾値以下である物標の第１物標群を選択する。

ステップＳ２１０において、第１物標群平均位置算出部２３２は、上記ステップＳ２００により選択された第１物標群に含まれる物標の各々について、当該物標の第１物標位置の平均位置である第１平均位置を算出する。

ステップＳ２２０において、第２物標群位置補正部２３３は、第１物標群に含まれる物標の第１平均位置と、同一の走行においてセンサを用いて検出された当該物標の各々の第２物標位置との対応付けを行い、当該物標の各々の第２物標位置を座標変換した結果と、当該物標の各々の第２物標位置と対応付けられた第１物標群に含まれる物標の第１平均位置の各々との位置ずれが最小となる第１座標変換行列を算出し、第１座標変換行列に基づいて、同一の走行においてセンサを用いて検出された当該物標の各々について算出された第２物標位置の各々を補正する。

ステップＳ２３０において、第２物標群平均位置算出部２３４は、第１物標群に含まれる物標の各々について、上記ステップＳ２２０により補正された当該物標の第２物標位置の平均位置である第２平均位置を算出する。

ステップＳ２４０において、出力部２４０は、第１物標群に含まれる物標の各々について、当該物標の第２平均位置を当該物標の最終的な位置として出力する。

以上説明したように、本発明の第１の実施の形態に係る位置ずれ補正装置によれば車両に搭載されたセンサを用いて検出された当該車両の周辺の物標の車両座標系における位置と、車両の世界座標系の位置である第１車両位置とに基づいて当該物標の世界座標系における位置である第１物標位置を算出し、当該物標の位置と、当該車両の位置の変化に基づいて検出された当該車両の世界座標系の位置である第２車両位置とに基づいて当該物標の世界座標系における位置である第２物標位置を算出し、検出された複数の物標の各々について、複数回の走行において算出された当該物標の第１物標位置や当該物標の大きさ等に基づいて、同一物標に対する第１物標群を生成し、第１物標群内の物標の第１物標位置のばらつきを求め、当該ばらつきが所定の第１閾値以下である物標の第1物標群を選択し、第１物標群に含まれる物標の各々について算出した当該物標の第１物標位置の平均位置である第１平均位置に基づいて、同一の走行において当該センサを用いて検出された当該物標の各々について算出された第２物標位置の各々を補正し、第１物標群に含まれる物標の各々について、補正された当該物標の第２物標位置の平均位置である第２平均位置を算出することにより、低コスト、かつ、精度よく物標の絶対位置を推定することができる。

＜本発明の第２の実施の形態に係る位置ずれ補正システム２の構成＞
図１９を用いて、本発明の第２の実施の形態に係る位置ずれ補正システム２の構成について説明する。なお、第１の実施の形態に係る位置ずれ補正システム１と同様の構成については、同一の符号を付して詳細な説明は省略する。

第１の実施の形態に係る位置ずれ補正システム１では、１台の車両により走行環境を複数回走行することにより、第１物標位置及び第２物標位置を収集する構成とした。第２の実施の形態に係る位置ずれ補正システム２では、複数台の車両により走行環境を走行することにより、第１物標位置及び第２物標位置を収集し、ネットワークを介してサーバが位置ずれ補正を行う構成を例に説明する。

位置ずれ補正システム２は、カメラ１０、第１測位装置１１、及び第２測位装置１２を搭載した複数の車載器３０と、位置ずれ補正装置４０と、基地局５０と、ネットワーク６０とを備えて構成されている。

車載器３０は、ＣＰＵと、ＲＡＭと、後述する物標位置収集処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。

図２０に示すように、本発明の実施の形態に係る車載器３０は、物標位置収集部２１０と、送信部３００とを備えて構成される。

本実施の形態において、第１物標位置算出部２１４及び第２物標位置算出部２１６は、算出した第１物標位置及び第２物標位置を、送信部３００に渡す。

送信部３００は、基地局５０及びネットワーク６０を介して、位置ずれ補正装置４０に、第１物標位置及び第２物標位置を送信する。基地局５０はネットワーク６０に接続された無線基地局であり、ネットワーク６０は公衆網である。

位置ずれ補正装置４０は、ＣＰＵと、ＲＡＭと、後述する位置ずれ補正処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。

図２１に示すように、本発明の実施の形態に係る位置ずれ補正装置４０は、受信部４００と、第１物標群選択部２３１と、第１物標群平均位置算出部２３２と、第２物標群位置補正部４３３と、第２物標群平均位置算出部２３４と、出力部２４０とを備えて構成される。

受信部４００は、第１物標位置及び第２物標位置を受信し、例えばバッファ（図示しない）等を用いて保持しておく。受信部４００は、位置ずれ補正処理を開始する際に、保持している全ての第１物標位置及び第２物標位置を、第１物標群選択部２３１に渡す。

第２物標群位置補正部４３３は、まず、第２物標群位置補正部２３３と同様に、第１物標群に含まれる物標の各々の第１平均位置と、同一の走行においてセンサを用いて検出された物標の各々の第２物標位置との対応付けを行い、当該物標の各々の第２物標位置を座標変換した結果と、当該物標の各々の第２物標位置と対応付けられた第１物標群に含まれる物標の第１平均位置の各々との位置ずれが最小となる第１座標変換行列を算出し、第１座標変換行列に基づいて、同一の走行においてセンサを用いて検出された当該物標の各々について算出された第２物標位置の各々を座標変換する。

次に、第２物標群位置補正部４３３は、算出した第１座標変換行列に基づいて当該物標の当該第２物標位置を座標変換した結果と、当該物標の当該第２物標位置と対応付けられた第１物標群に含まれる物標の第１平均位置との位置ずれである残差が所定の第２閾値より大きい物標を、同一の走行においてカメラ１０を用いて検出された物標からなる同一走行物標群から除外する。具体的には、第２物標群位置補正部４３３は、図２１に示すように、第１座標変換行列による位置補正により、残差が第２閾値より大きい物標を、同一走行物標群から除外する。

次に、第２物標群位置補正部４３３は、第１物標群に含まれる物標の各々の第１平均位置と、残差が第２閾値より大きい物標が除外された、同一走行物標群に含まれる物標の各々の第２物標位置とを対応付けを行い、位置ずれが最小となる第２座標変換行列を算出し、第２座標変換行列に基づいて、同一走行物標群に含まれる物標の各々について算出された当該第２物標位置の各々を補正した補正第２物標位置群を算出する。第１座標変換行列による位置補正と比較し、残差が第２閾値より大きい物標が同一走行物標群から除外されていることから物標平均位置群１の絶対位置精度が向上している。このため、物標平均位置群２の絶対位置も向上することとなる。そして、第２物標群位置補正部４３３は、補正第２物標位置群を第２物標群平均位置算出部２３４に渡す。

＜本発明の第２の実施の形態に係る位置ずれ補正システム２の作用＞
図２３は、本発明の第２の実施の形態に係る物標位置収集処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る物標位置収集処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。物標位置収集処理ルーチンは、車両の各々において、当該車両が走行する毎に、繰り返し実行される。

ステップＳ３７０において、送信部３００は、基地局５０及びネットワーク６０を介して、位置ずれ補正装置４０に、第１物標位置及び第２物標位置を送信する。

図２４は、本発明の第２の実施の形態に係る位置ずれ補正処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る位置ずれ補正処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。

ステップＳ４７０において、受信部４００は、第１物標位置及び第２物標位置を受信する。

ステップＳ４８０において、受信部４００は、位置ずれ補正を開始するか否かを判定する。

位置ずれ補正を開始しない場合、ステップＳ４７０に戻り、受信部４００は、第１物標位置及び第２物標位置の受信を繰り返す。一方、位置ずれ補正を開始する場合、ステップＳ２００に進む。

ステップＳ５１０において、第２物標群位置補正部４３３は、第１座標変換行列に基づいて当該物標の当該第２物標位置を座標変換した結果と、当該物標の当該第２物標位置と対応付けられた第１物標群に含まれる物標の第１平均位置との位置ずれである残差が所定の第２閾値より大きい物標を、同一の走行においてセンサを用いて検出された物標からなる同一走行物標群から除外する。

ステップＳ５２０において、第２物標群位置補正部４３３は、第１物標群に含まれる物標の各々の第１平均位置と、上記ステップＳ５１０により除外された、同一走行物標群に含まれる物標の各々の第２物標位置とを対応付けを行い、位置ずれが最小となる第２座標変換行列を算出し、第２座標変換行列に基づいて、同一走行物標群に含まれる物標の各々について算出された当該第２物標位置の各々を補正する。

以上説明したように、本発明の第２の実施の形態に係る位置ずれ補正装置によれば、第１座標変換行列に基づいて当該物標の当該第２物標位置を座標変換した結果と、当該物標の当該第２物標位置と対応付けられた第１物標群に含まれる物標の第１平均位置との位置ずれである残差が所定の第２閾値より大きい物標を、同一の走行においてセンサを用いて検出された当該物標から除外し、第１物標群に含まれる物標の第１平均位置と、同一の走行においてセンサを用いて検出された当該物標の各々の第２物標位置との対応付けを行い、位置ずれが最小となる第２座標変換行列を算出し、第２座標変換行列に基づいて、当該センサを用いて同時に検出された当該物標の各々について算出された当該第２物標位置の各々を補正することにより、更に精度よく物標の位置を推定することができる。

なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。

例えば、上述の実施の形態では、車両を例に説明したが、これに限定されるものではなく、他の移動体を用いてもよい。また、車両と車両以外の移動体とを用いてもよい。

また、複数の車両により物標の位置を検出する場合、物標の密度が高くないものにより物標の位置の対応付けを予め行っておいてもよい。

また、上述の実施の形態では、物標検出部２１２は、カメラ１０によって撮影された画像を用いて走行環境中の物標を検出したが、これに限定されるものではない。例えば、Ｌｉｄａｒ等の他のセンサを用いて走行環境中の物標を検出する構成としてもよい。

また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。

１、２位置ずれ補正システム
１０カメラ
１１第１測位装置
１２第２測位装置
２０位置ずれ補正装置
３０車載器
４０位置ずれ補正装置
５０基地局
６０ネットワーク
２１０物標位置収集部
２１１画像取得部
２１２物標検出部
２１３第１取得部
２１４第１物標位置算出部
２１５第２取得部
２１６第２物標位置算出部
２２０物標位置記憶部
２３０位置ずれ補正部
２３１第１物標群選択部
２３２第１物標群平均位置算出部
２３３第２物標群位置補正部
２３４第２物標群平均位置算出部
２４０出力部
３００送信部
４００受信部
４３３第２物標群位置補正部

Claims

車両に搭載された第１測位装置により検出された前記車両の世界座標系の位置である第１車両位置を取得する第１取得部と、
前記車両に搭載された第２測位装置により前記車両の位置の変化に基づいて検出された前記車両の世界座標系の位置である第２車両位置を取得する第２取得部と、
前記車両に搭載されたセンサを用いて検出された前記車両の周辺の物標の車両座標系における位置を取得する物標検出部と、
前記第１取得部が取得した前記第１車両位置と、前記第１車両位置の検出に対応して前記物標検出部により取得された物標の位置とに基づいて、前記物標の世界座標系における位置である第１物標位置を算出する第１物標位置算出部と、
前記第２取得部が取得した前記第２車両位置と、前記第２車両位置の検出に対応して前記物標検出部により取得された物標の位置とに基づいて、前記物標の世界座標系における位置である第２物標位置を算出する第２物標位置算出部と、
前記物標検出部によって検出された複数の物標の各々について、複数回の走行において前記第１物標位置算出部により算出された前記物標の前記第１物標位置に基づいて、同一物標に対する第１物標群を生成し、前記第１物標群内の前記物標の前記第１物標位置のばらつきを求め、前記複数の物標のうち前記ばらつきが所定の第１閾値以下である物標を前記第１物標群から選択する第１物標群選択部と、
前記第１物標群選択部により選択された前記第１物標群に含まれる物標の各々について、前記物標の前記第１物標位置の平均位置である第１平均位置を算出する第１物標群平均位置算出部と、
前記第１物標群平均位置算出部で算出された前記第１平均位置に基づいて、同一の走行において前記センサを用いて検出された前記物標の各々について算出された前記第２物標位置の各々を補正する第２物標群位置補正部と、
前記第２物標群位置補正部により補正された前記物標の前記第２物標位置の平均位置である第２平均位置を算出する第２物標群平均位置算出部と、
前記第２物標群平均位置算出部で算出された第２平均位置を、前記第１物標群に含まれる物標各々の最終的な位置として出力する出力部と、
を含む位置ずれ補正システム。
前記第２物標群位置補正部は、前記第１物標群に含まれる物標の前記第１平均位置と、同一の走行において前記センサを用いて検出された前記物標の各々の前記第２物標位置との対応付けを行い、前記物標の各々の前記第２物標位置を座標変換した結果と、前記物標の各々の前記第２物標位置と対応付けられた前記第１物標群に含まれる物標の前記第１平均位置の各々との位置ずれが最小となる第１座標変換行列を算出し、前記第１座標変換行列に基づいて、同一の走行において前記センサを用いて検出された前記物標の各々について算出された前記第２物標位置の各々を補正する
請求項１記載の位置ずれ補正システム。
前記第２物標群位置補正部は、前記第１座標変換行列を算出し、前記第１座標変換行列に基づいて前記物標の前記第２物標位置を座標変換した結果と、前記物標の前記第２物標位置と対応付けられた前記第１物標群に含まれる物標の前記第１平均位置との位置ずれである残差が所定の第２閾値より大きい物標を、同一の走行において前記センサを用いて検出された前記物標から除外し、
前記第１物標群に含まれる物標の前記第１平均位置と、同一の走行において前記センサを用いて検出された前記物標の各々の前記第２物標位置との対応付けを行い、前記位置ずれが最小となる第２座標変換行列を算出し、前記第２座標変換行列に基づいて、前記センサを用いて同時に検出された前記物標の各々について算出された前記第２物標位置の各々を補正する
請求項２記載の位置ずれ補正システム。
コンピュータを、
車両に搭載された第１測位装置により検出された前記車両の世界座標系の位置である第１車両位置を取得する第１取得部、
前記車両に搭載された第２測位装置により前記車両の位置の変化に基づいて検出された前記車両の世界座標系の位置である第２車両位置を取得する第２取得部、
前記車両に搭載されたセンサを用いて検出された前記車両の周辺の物標の車両座標系における位置を取得する物標検出部、
前記第１取得部が取得した前記第１車両位置と、前記第１車両位置の検出に対応して前記物標検出部により取得された物標の位置とに基づいて、前記物標の世界座標系における位置である第１物標位置を算出する第１物標位置算出部、
前記第２取得部が取得した前記第２車両位置と、前記第２車両位置の検出に対応して前記物標検出部により取得された物標の位置とに基づいて、前記物標の世界座標系における位置である第２物標位置を算出する第２物標位置算出部、
前記物標検出部によって検出された複数の物標の各々について、複数回の走行において前記第１物標位置算出部により算出された前記物標の前記第１物標位置に基づいて、同一物標に対する第１物標群を生成し、前記第１物標群内の前記物標の前記第１物標位置のばらつきを求め、前記複数の物標のうち前記ばらつきが所定の第１閾値以下である物標を前記第１物標群から選択する第１物標群選択部、
前記第１物標群選択部により選択された前記第１物標群に含まれる物標の各々について、前記物標の前記第１物標位置の平均位置である第１平均位置を算出する第１物標群平均位置算出部、
前記第１物標群平均位置算出部で算出された前記第１平均位置に基づいて、同一の走行において前記センサを用いて検出された前記物標の各々について算出された前記第２物標位置の各々を補正する第２物標群位置補正部、
前記第２物標群位置補正部により補正された前記物標の前記第２物標位置の平均位置である第２平均位置を算出する第２物標群平均位置算出部、及び
前記第２物標群平均位置算出部で算出された第２平均位置を、前記第１物標群に含まれる物標各々の最終的な位置として出力する出力部、
として機能させるためのプログラム。