(57)【要約】【課題】構造物から密集した特徴点が検出される場合であっても、精度良く移動体の位置を推定することができる移動体位置推定装置、及び移動体位置推定プログラムを得る。【解決手段】移動体位置推定装置２０は、移動体から画像を撮影する撮影部２１と、撮影された画像から複数の特徴点を検出する検出部２２と、検出部２２により検出された複数の特徴点を含む測定点群と、移動体の位置を示す移動体点と、を有する測定座標系を作成する作成部２３と、地図上の複数の場所の各々から事前に検出した複数の特徴点を含む地図座標系を取得する取得部２４と、測定座標系及び地図座標系の各々に含まれる複数の特徴点のうち、関連する特徴点同士を、密度に応じた重み値を付与した代表点に置換し、重み値と特徴点及び代表点との相互距離とから、測定座標系を地図座標系に整合させるための同次変換行列を導出し、地図上の移動体の位置を推定する推定部２５と、を備えている。【選択図】図３
2020125960.tif 000002
【特許請求の範囲】
【請求項１】
移動体から画像を撮影する撮影部と、
前記撮影部により撮影された画像から複数の特徴点を検出する検出部と、
前記検出部により検出された前記複数の特徴点を含む測定点群と、前記画像を撮影した際の前記移動体の位置を示す移動体点と、を有する測定座標系を作成する作成部と、
前記移動体が移動しうる地図上の複数の場所の各々から事前に検出した複数の特徴点を含む、地図点群を有する地図座標系を取得する取得部と、
前記測定点群に含まれる複数の特徴点のうち関連する複数の特徴点同士を、密度に応じた第一の重み値を付与した代表点に置換し、前記地図点群に含まれる複数の特徴点のうち関連する特徴点同士を、密度に応じた第二の重み値を付与した代表点に置換し、前記第一の重み値及び前記第二の重み値と、前記測定点群に含まれる特徴点及び代表点と前記地図点群に含まれる特徴点及び代表点との相互距離とから、前記測定座標系を前記地図座標系に整合させるための同次変換行列を導出し、導出した前記同次変換行列により前記測定座標系の前記移動体点を前記地図座標系に適用して、前記地図上の前記移動体の位置を推定する推定部と、
を備える移動体位置推定装置。
【請求項２】
前記推定部は、
前記測定点群及び前記地図点群間の任意の対応する特徴点の組又は代表点の組において、特徴点又は代表点が整合する前記同次変換行列を導出することを、導出した同次変換行列を測定点群及び前記地図点群間の対応する特徴点の組及び代表点の組に適用した時の特徴点間の距離及び代表点間の距離と前記第一の重み値及び前記第二の重み値とを用いた評価関数が所定の条件を満たすまで、繰り返す請求項１記載の移動体位置推定装置。
【請求項３】
前記推定部は、前記作成部で作成した前記測定座標系に含まれる特徴点と前記取得部から取得した前記地図座標系に含まれる特徴点との相互距離とから、前記測定座標系を前記地図座標系に整合させるための同次変換行列の初期値を導出し、導出した前記同次変換行列により前記測定座標系の前記移動体点を前記地図座標系に適用して、前記地図座標系上の前記移動体の初期位置を推定する、請求項１又は請求項２記載の移動体位置推定装置。
【請求項４】
前記推定部は、前記測定点群及び前記地図点群に含まれる特徴点及び代表点から、所定の閾値を超えた前記第一の重み値又は第二の重み値が付与された代表点を除いて、前記同次変換行列を導出する、請求項１から請求項３の何れか１項記載の移動体位置推定装置。
【請求項５】
前記測定座標系及び前記地図座標系は、２次元平面で表される座標系である、請求項１から請求項４の何れか１項に記載の移動体位置推定装置。
【請求項６】
移動体に搭載された撮影部により画像を撮影するステップと、
撮影部により撮影された画像から複数の特徴点を検出するステップと、
検出部により検出された前記複数の特徴点を含む測定点群と、前記画像を撮影した際の前記移動体の位置を示す移動体点と、を有する測定座標系を作成するステップと、
前記移動体が移動しうる地図上の複数の場所の各々から事前に検出した複数の特徴点を含む、地図点群を有する地図座標系を取得するステップと、
前記測定点群に含まれる複数の特徴点のうち関連する複数の特徴点同士を、密度に応じた第一の重み値を付与した代表点を置換し、前記地図点群に含まれる複数の特徴点のうち関連する特徴点同士を、密度に応じた第二の重み値を付与した代表点に置換し、前記第一の重み値及び前記第二の重み値と、前記測定点群に含まれる特徴点及び代表点と前記地図点群に含まれる特徴点及び代表点との相互距離とから、前記測定座標系を前記地図座標系に整合させるための同次変換行列を導出し、導出した前記同次変換行列により前記測定座標系の前記移動体点を前記地図座標系に適用して、前記地図上の前記移動体の位置を推定するステップと、
をコンピュータに実行させる移動体位置推定プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体位置推定装置、及び移動体位置推定プログラムに関する。
【背景技術】
【０００２】
移動体に搭載されたカメラ等の撮影部によって撮影された撮影画像に基づいて、３次元空間上における移動体の位置を推定する技術が提案されている。
【０００３】
例えば、移動体の位置を推定する技術として、撮影画像から取得した構造物の特徴を地図に登録し、運転時に取得した画像の構造物の特徴点と登録した地図の特徴点と、を照合して自車の位置を推定する技術がある（特許文献１参照）。この技術では、事前に走行した際の画像から構造物の特徴点を抽出し、その位置を推定して地図に登録し、運転時に画像から構造物の特徴点を抽出し、地図に登録された構造物の特徴点と照合することにより、移動体の位置情報を算出している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１７−１３８６６４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１に記載の技術では、樹木等の構造物では、特徴点が密集して検出されてしまう。この場合、撮影画像の樹木の特徴点と、地図上の樹木の特徴点との照合において局所解となってしまう場合があり、誤認識することがある。そのため、特許文献１の技術では、移動体の位置を必ずしも精度よく推定できるとは限らなかった。
【０００６】
本開示は、以上の事情を鑑みて成されたものであり、構造物から密集した特徴点が検出される場合であっても、精度良く移動体の位置を推定することができる移動体位置推定装置、及び移動体位置推定プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記目的を達成するために、請求項１に記載の移動体位置推定装置は、移動体から画像を撮影する撮影部と、撮影部により撮影された画像から複数の特徴点を検出する検出部と、検出部により検出された複数の特徴点を含む測定点群と、画像を撮影した際の移動体の位置を示す移動体点と、を有する測定座標系を作成する作成部と、移動体が移動しうる地図上の複数の場所の各々から事前に検出した複数の特徴点を含む、地図点群を有する地図座標系を取得する取得部と、測定点群に含まれる複数の特徴点のうち関連する複数の特徴点同士を、密度に応じた第一の重み値を付与した代表点に置換し、地図点群に含まれる複数の特徴点のうち関連する特徴点同士を、密度に応じた第二の重み値を付与した代表点に置換し、第一の重み値及び第二の重み値と、測定点群に含まれる特徴点及び代表点と地図点群に含まれる特徴点及び代表点との相互距離とから、測定座標系を地図座標系に整合させるための同次変換行列を導出し、導出した同次変換行列により測定座標系の移動体点を地図座標系に適用して、地図上の移動体の位置を推定する推定部と、を備えている。
【０００８】
また、請求項２に記載の発明は、請求項１に記載の発明において、推定部が、測定点群及び地図点群間の任意の対応する特徴点の組又は代表点の組において、特徴点又は代表点が整合する同次変換行列を導出することを、導出した同次変換行列を測定点群及び地図点群間の対応する特徴点の組及び代表点の組に適用した時の特徴点間の距離及び代表点間の距離と前記第一の重み値及び前記第二の重み値とを用いた評価関数が所定の条件を満たすまで、繰り返し処理を行う。
【０００９】
また、請求項３に記載の発明は、請求項１又は２の発明において、推定部が、作成部で作成した測定座標系に含まれる特徴点と取得部から取得した地図座標系に含まれる特徴点との相互距離とから、測定座標系を地図座標系に整合させるための同次変換行列の初期値を導出し、導出した同次変換行列により測定座標系の移動体点を地図座標系に適用して、地図座標系上の移動体の初期位置を推定する。
【００１０】
また、請求項４に記載の発明は、請求項１から３の何れか一項に記載の発明において、推定部が、代表点の重み値が閾値を超えた場合、前述の同次変換行列の導出に代えて、測定点群に含まれる特徴点及び重み値が閾値以下の代表点と前記地図点群に含まれる特徴点及び重み値が閾値以下の代表点との相互距離とから、前記測定座標系を前記地図座標系に整合させるための同次変換行列を導出する。
【００１１】
また、請求項５に記載の発明は、請求項１から４の何れか一項に記載の発明において、測定座標系及び地図座標系が、２次元平面に表される座標系である。
【００１２】
一方、上記目的を達成するために、請求項６に記載の移動体位置推定プログラムは、移動体に搭載された撮影部により画像を撮影するステップと、撮影部により撮影された画像から複数の特徴点を検出するステップと、検出部により検出された複数の特徴点を含む測定点群と、画像を撮影した際の移動体の位置を示す移動体点と、を有する測定座標系を作成する作成部と、移動体が移動しうる地図上の複数の場所の各々から事前に検出した複数の特徴点を含む、地図点群を有する地図座標系を取得するステップと、測定点群に含まれる複数の特徴点のうち関連する複数の特徴点同士を、密度に応じた第一の重み値を付与した代表点を置換し、地図点群に含まれる複数の特徴点のうち関連する特徴点同士を、密度に応じた第二の重み値を付与した代表点に置換し、第一の重み値及び第二の重み値と、測定点群に含まれる特徴点及び代表点と地図点群に含まれる特徴点及び代表点との相互距離とから、測定座標系を地図座標系に整合させるための同次変換行列を導出し、導出した同次変換行列により測定座標系の移動体点を地図座標系に適用して、地図上の移動体の位置を推定するステップと、をコンピュータに実行させる。
【発明の効果】
【００１３】
本開示によれば、構造物の特徴点が密集している場合であっても、精度良く移動体の位置を推定することができる。
【図面の簡単な説明】
【００１４】
【図１】本実施形態に係る車両の一例を示す側面図である。
【図２】本実施形態に係る移動体位置推定装置のハードウェア構成の一例を示すブロック図である。
【図３】本実施形態に係る移動体位置推定装置の機能的な構成の一例を示すブロック図である。
【図４】本実施形態に係る座標系の一例を示す図である。（ａ）は本実施形態に係る測定座標系の一例を模式的に示す図である。（ｂ）は、本実施形態に係る地図座標系の一例を模式的に示す図である。
【図５】（ａ）は本実施形態に係る構造物の特徴点の一例を模式的に示す図である。（ｂ）は本実施形態に係る構造物に配置された代表点の一例を模式に示す図である。
【図６】（ａ）は本実施形態に係るリサンプリング前の測定点群及び地図点群の特徴点の位置合わせの一例を模式的に示す図である。（ｂ）は本実施形態に係るリサンプリング後の測定点群及び地図点群の特徴点の位置合わせの一例を模式的に示す図である。
【図７】本実施形態に係る移動体位置推定処理の一例を示すフローチャートである。
【図８】本実施形態に係るリサンプリング処理の一例を示すフローチャートである。
【発明を実施するための形態】
【００１５】
以下、図面を参照して、本開示の技術を実施するための形態例を詳細に説明する。なお、本実施形態は、移動体が車両である場合を例示する。しかし、移動体は、車両に限定されない。移動体は、移動する物体であれば、飛行する物体であってもよい。以下、車両に搭載された移動体位置推定装置について詳細に説明する。
【００１６】
図１は、本実施形態に係る車両１０の一例を示す側面図である。図１に示すように、車両１０は、カメラ１１を備えている。
【００１７】
カメラ１１は、車両１０の後部のトランク等に設けられ、車両後方を撮影する。カメラ１１は、例えば、車幅方向の略中央部付近に設けられ、かつカメラ１１の光軸が水平方向より若干下側を向くように配置されている。なお、本実施形態では、カメラ１１を車両１０の後部に設ける形態を例示して説明する。しかし、これに限られず、環境等に応じて例えば前部又は側部に設けてもよい。また、本実施形態では、カメラ１１として単眼カメラを例示して説明する。しかし、これに限らず他の形態のカメラ、例えばステレオカメラ等であってもよい。
【００１８】
図２は、本実施形態に係る移動体位置推定装置２０のハードウェア構成の一例を示すブロック図である。図２に示すように、本実施形態に係る移動体位置推定装置２０は、車両１０に搭載され、カメラ１１、表示部１２、ＣＰＵ（Central Processing Unit）１３、ＲＡＭ（Random Access Memory）１４、ＲＯＭ（Read Only Memory）１５、及びストレージ１６を含んで構成されている。カメラ１１、表示部１２、ＣＰＵ１３、ＲＡＭ１４、ＲＯＭ１５、及びストレージ１６の各々はバス１７により相互に接続されている。
【００１９】
表示部１２は、カメラ１１が撮影した画像等を表示する。表示部１２は、画像を表示できる限りいかなる装置であってもよい。表示部１２は、例えば、液晶モニタ、ＣＲＴ（Cathode Ray Tube）モニタ、ＦＰＤ（Flat Panel Display）モニタ等である。
【００２０】
ＣＰＵ１３は、移動体位置推定装置２０の全体を統括、制御する。ＲＡＭ１４は、各種プログラムの実行時のワークエリアとして用いられるメモリである。ＲＯＭ１５は、本実施形態で用いる移動体の位置を推定する移動体位置推定プログラム１５Ａを含む各種プログラムやデータ等を記憶している。ＣＰＵ１３は、ＲＯＭ１５に記憶されたプログラムをＲＡＭ１４に展開して実行することにより、地図の生成や、移動体の位置の推定を行う。
【００２１】
ストレージ１６には、本実施形態に係る移動体の位置を推定する処理に用いる地図情報１６Ａ等を格納している。ストレージ１６は、一例としてＨＤＤ（Hard Disk Drive）、ＳＳＤ（Solid State Drive）、フラッシュメモリ等である。なお、ストレージ１６にはＣＰＵ１３によって生成した地図情報１６Ａの他に、移動体位置推定プログラム１５Ａ等を記憶してもよい。
【００２２】
なお、本実施形態では、カメラ１１及び移動体位置推定装置２０が車両１０に搭載されている形態について説明したが、これに限定されない。車両１０に搭載されたカメラ１１によって撮影された画像を、通信回線等を通して別の場所に設置されている移動体位置推定装置２０に送信して移動体の位置を推定する処理を行ってもよい。
【００２３】
次に、移動体値推定装置２０の機能構成について説明する。図３は、本実施形態に係る移動体位置推定装置２０の機能的な構成の一例を示すブロック図である。図４は、本実施形態に係る座標系の一例を示す図である。図４（ａ）は、測定座標系の一例を模式的に示す図であり、図４（ｂ）は、地図座標系の一例を模式的に示す図である。
【００２４】
図３に示すように、移動体位置推定装置２０は、撮影部２１、検出部２２、作成部２３、取得部２４、及び推定部２５を有する。ＣＰＵ１３が移動体位置推定プログラム１５Ａを実行することで、撮影部２１、検出部２２、作成部２３、取得部２４、及び推定部２５として機能する。
【００２５】
撮影部２１は、車両１０から画像を撮影する。具体的には、撮影部２１は、車両１０に搭載されたカメラ１１を制御して、カメラ１１により車両１０から画像を撮影する。
【００２６】
検出部２２は、撮影部２１により撮影された画像から複数の特徴点を検出する。検出部２２は、一例として、建物の凹凸に基づく陰影、壁面の模様等、撮影画像において輝度の濃淡差が所定値よりも大きい点を、特徴点として検出する。
【００２７】
作成部２３は、検出部２２により検出された複数の特徴点を含む測定点群と、画像を撮影した際の車両１０の位置を示す移動体点と、を有する測定座標系を作成する。測定座標系とは、例えば、図４（ａ）に示すような座標系Σｓであり、Ｘｓ軸、Ｙｓ軸、及びＺｓ軸の３次元により表される系である。
【００２８】
なお、本実施形態では、一例として、座標系は右手系の同次座標系を用いて表される形態について説明する。同次座標系とは、Ｘ軸、Ｙ軸、及びＺ軸の３次元ベクトルに１又は−1を付加して表す形式である。右手系の座標系の場合は、１を付加して（Ｘ、Ｙ、Ｚ、１）で表され、左手系の座標系の場合は、−1を付加して（Ｘ、Ｙ、Ｚ、−1）で表される。
【００２９】
取得部２４は、車両１０が移動しうる地図上の複数の場所の各々から事前に検出した複数の特徴点を含む、地図点群を有する地図座標系を取得する。取得部２４は、予めストレージ１６から地図座標系を取得してもよいし、車両１０の外部から通信により地図座標系を取得してもよい。地図座標系とは、例えば、図４（ｂ）に示すような座標系Σｍであり、Ｘｍ軸、Ｙｍ軸及びＺｍ軸の３次元により表される系である。地図座標系は、例えば、次のように作成される。
【００３０】
車両１０が走行しうる経路を別の車両又は車両１０が予め走行する。以下では、車両１０とは別の車両が走行する場合について、説明する。ここで、別の車両は、車両１０と同様にカメラを搭載している。車両に搭載されたカメラにより経路上の画像を撮像する。カメラが撮影した画像から特徴点の座標（３次元位置）と画像を撮影したカメラの位置を推定する。これらの推定は、例えばＶｉｓｕａｌ  ＳＬＡＭ（Simultaneous Localization and Mapping）を用いて行うことができる。
【００３１】
またオドメトリー法を適用してカメラにより撮影された画像から計測した車両の位置及び移動量、構造物までの距離及び大きさ等の情報が座標系に反映される。オドメトリー法とは、車両の車輪回転数や加速度等を計測するセンサーから取得した情報、及びカメラ１１により撮影された画像等を用いて、移動体の位置及び移動量、構造物までの距離及び大きさ等を計測する手法である。
【００３２】
すなわち、撮影した画像から、Ｖｉｓｕａｌ  ＳＬＡＭを用いて特徴点の３次元位置と撮影したカメラ１１の位置とが推定され、これらの推定された位置と推定された位置における特徴点を地図座標系に登録する。また、オドメトリー法を用いて地図座標系に構造物の大きさ及び車両から構造物までの距離等の情報が反映される。ストレージ１６に地図座標系を地図情報１６Ａとして予め記憶される。
【００３３】
なお、本実施形態では、地図座標系は、予め作成されてストレージ１６に記憶されている形態として説明した。しかし、これに限定されない。地図座標系は、地図情報１６Ａとしてストレージ１６に記憶された画像を用いて測定座標系を作成する際に同時に作成される形態としてもよい。
【００３４】
また、本実施形態では、特徴点の３次元位置と撮影したカメラ１１の位置との推定にＳＬＡＭを適用した例について説明した。しかし、これに限定されない。特徴点の検出方法として、ＳｆＭ（Structure from Motion）を適用してもよい。ＳｆＭは、カメラ１１で撮影した画像を用いて、周囲の構造物の幾何学的形状及びカメラ１１の移動の軌跡を同時に復元する技術である。
【００３５】
推定部２５は、測定点群に含まれる複数の特徴点のうち関連する複数の特徴点同士を密度に応じた第一の重み値を付与した代表点に置換する。推定部２５は、地図点群に含まれる複数の特徴点のうち関連する特徴点同士を、密度に応じた第二の重み値を付与した代表点に置換する。そして、推定部２５は、第一の重み値及び第二の重み値と、測定点群に含まれる特徴点及び代表点と地図点群に含まれる特徴点及び代表点との相互距離とから、測定座標系を地図座標系に整合させるための同次変換行列を導出する。推定部２５は、導出した同次変換行列を測定座標系の移動体点に適用して、地図座標系の移動体の位置を推定する。
【００３６】
次に、移動体位置推定装置２０の作用について説明する前に、移動体の位置を推定するための手法を説明する。本実施形態では、一例として、ＩＣＰ（Interactive Closest Point）法を適用して、車両１０の位置の推定を行う手法について説明する。
【００３７】
ＩＣＰ法とは、画像から取得した測定点群上のある点ｐについて、地図から取得した地図点群上のある点ｑに対する距離を求める処理を点群上の全ての点ｐ及び全て点ｑについて行い、移動体の移動位置を推定する手法である。ＩＣＰ法は、点群上の全ての点ｐ及び全て点ｑの距離を算出して距離の総和が最小となるｐ及びｑの組み合わせを求める（以下、「位置合わせ」という。）ことで、測定点群の点ｐと、点ｐに対応する地図点群の点ｑとを整合させるための同次変換行列を導出する。同次変換行列とは、点ｐに対して並進変換と回転変換を施すことで、点ｑにより近い座標になると推測される行列である。また、ＩＣＰ法は、位置合わせを行うことで同次変換行列及び距離の総和を求め、求めた同次変換行列を適用して位置合わせを行う処理を繰り返し行う。繰り返し求めた距離の総和の差分が所定の閾値を下回る収束条件を満たした場合、同次変換行列及び地図点群上における測定点群の点ｐの位置が確定する。
【００３８】
つまり、ＩＣＰ法は、画像から取得した測定点群及び地図から取得した地図点群の位置合わせと、同次変換行列及び距離の総和を導出する処理と、を繰り返し行うことで、地図座標系上における移動体点の位置を推定する処理の精度を上げる手法である。
【００３９】
なお、本実施形態では、関連する複数の特徴点同士を密度に応じた重み値を付与した代表点に置換する（以下、「リサンプリング」という。）処理を行い、測定座標系の特徴点及び代表点と、地図座標系の特徴点及び代表点との位置合わせを行う。更に、本実施形態では、一例として、測定座標系の特徴点及び代表点と、地図座標系の特徴点及び代表点との相互距離に、測定点群の代表点の密度及び地図点群の代表点の密度を適用して、同次変換行列及び距離の総和を導出する。
【００４０】
次に、図５を参照して、リサンプリング処理について説明する。図５は、リサンプリング処理前後の構造物の概念を示す図である。図５（ａ）は、本実施形態に係る構造物の特徴点を模式的に示す図である。図５（ｂ）は、本実施形態に係る構造物に配置された代表点を模式に示す図である。
【００４１】
図５（ａ）に示すように、特徴点が密集した構造物は、一例として、特徴点が隣接した状態で表される。図５（ｂ）に示すような代表点は、例えば、次のような処理を行うことで置換することができる。
【００４２】
推定部２４は、測定座標系及び地図座標系上から任意の特徴点を選択し、選択した特徴点に隣接する座標を参照する。参照した座標に特徴点が配置されている場合、推定部２４は、特徴点が配置されている座標を取得する。また、推定部２４は、参照した座標に特徴点が配置されていた場合、特徴点が配置されていた座標に隣接する座標に特徴点が配置されているか参照する処理を測定座標系及び地図座標系上の特徴点を全て抽出するまで行う。また、推定部２４は、取得した隣接する特徴点の座標を用いて、隣接する特徴点の代表点及び重み値を導出する。
【００４３】
なお、本実施形態では、一例として、隣接する特徴点の重心位置に代表点を導出する形態について説明する。また、本実施形態では、一例として、隣接する特徴点の密度を重み値として導出する形態について説明する。また、隣接する特徴点の重心位置Ｇｉ、及び隣接する特徴点の密度Ｄｉは次の数式によって求めることができる。
【００４４】
【数１】
2020125960.tif 000003
【００４５】
【数２】
2020125960.tif 000004
【００４６】
ここで、ｉは代表点ごとに付与される正の整数である。また、ｍは、特徴点ごとに付与される正の整数である。また、Ｎｉは、代表点に関連する特徴点の個数である。また、ｐｍは、測定点群上のｍ番目の特徴点の座標である。また、Ｋｉは、特徴点を算出する際に参照した座標の数である。
【００４７】
推定部２４は、算出した重心位置に代表点を配置し、リサンプリング後の測定座標系及びリサンプリング後の地図座標系を作成する。
【００４８】
図５（ａ）に示すような特徴点が密集する構造物に対してリサンプリング処理を行うことで、図５（ｂ）に示すように構造物の特徴点を簡略化して表示できる。
【００４９】
なお、本実施形態では、一例として、隣接している複数の特徴点を代表点に置換する形態について説明した。しかし、これに限定されない。所定の範囲に配置されている複数の特徴点を代表点に置換する形態としてもよい。また、本実施形態では、一例として、関連する複数の特徴点の重心位置を代表点の位置とする形態について説明した。しかし、これに限定されない。関連する複数の特徴点の任意の特徴点の位置を代表点としてもよいし、関連する複数の特徴点のうち最上部及び最下部に位置する特徴点の位置を代表点としてもよい。また、本実施形態では、重み値として、密度を用いた形態について説明した。しかし、これに限定されない。重み値として、関連する特徴点の個数を適用してもよい。
【００５０】
次に、図６を参照して、本実施形態に係る測定点群及び地図点群の位置合わせ処理について説明する。図６（ａ）は、本実施形態に係るリサンプリング前の測定点群及び地図点群の特徴点の位置合わせの一例を模式的に示す図である。また、図６（ｂ）は、本実施形態に係るリサンプリング後の測定点群及び地図点群の特徴点の位置合わせの一例を模式的に示す図である。
【００５１】
図６（ａ）に示すように特徴点が密集した構造物同士を位置合わせすると、誤った特徴点の組み合わせであっても距離の総和が最小となり、正常な組み合わせと誤認識してしまい、特徴点の位置合わせが精度よく行われないことがある。
【００５２】
本実施形態では図６（ｂ）に示すように、測定点群及び地図点群の密集する特徴点に代えて代表点を導入し、位置合わせ処理を行う。つまり、リサンプリング後の測定点群上のｉ個目の点ｐｉとリサンプリング後のｉ個目の地図点群上の点ｑｉとの組を（ｐｉ，ｑｉ）とし、点ｐｉ及び点ｑｉの距離に代表点の密度を掛けた値の総和が最小となる点ｐｉ及びｑｉの組み合わせを求める。なお、代表点の密度に点ｐｉ及びｑｉの距離を掛けた値の総和（以下、「距離の総和」という。）ｄは、次の数式によって求めることができる。
【００５３】
【数３】
2020125960.tif 000005
【数４】
2020125960.tif 000006
【００５４】
ここで、ｑは地図点群上のある１つの点であり、Ｑは、地図点群上の全ての点の集合である。また、Ｄｉ（ｐｉ）は、測定点群ｉ番目の代表点の密度、Ｄｉ（ｑｉ）は、地図点群上のｉ番目の代表点の密度である。また、ｐｉは、測定点群上のｉ番目の特徴点及び代表点であり、ｑｉは、地図点群上のｉ番目の特徴点及び代表点である。また、Ｍｉｃｐは、測定座標系から地図座標系に整合するための同次変換行列である。同次変換行列Ｍｉｃｐは、次の数式で表される。
【００５５】
【数５】
2020125960.tif 000007
【００５６】
ここで、Ｒｉｃｐは、回転変換を表す３×３行列である。また、ｔｉｃｐは、並進変換を表す３次元ベクトルである。なお、本実施形態では、同次変換行列Ｍｉｃｐを次の数式によって求めることができる。
【００５７】
【数６】
2020125960.tif 000008
【００５８】
ここで、Ｍは、予め設定された同次変換行列の初期値、又は繰り返し行われた処理うち、直近の処理で求められた同次変換行列である。
【００５９】
本実施形態では、次の数式で表される収束条件を満たす場合、測定点群及び地図点群の位置合わせを終了する。
【００６０】
【数７】
2020125960.tif 000009
【００６１】
なお、ｋは、測定点群及び地図点群の位置合わせを行った回数である。また、δは、予め決められた所定の閾値である。なお、本実施形態では、収束条件が所定の閾値を下回った場合、測定点群及び地図点群の位置合わせを終了する形態について説明したが、これに限定されない。位置合わせを所定の回数行った場合、処理を終了する形態としてもよいし、距離の総和が所定の値を下回った場合、位置合わせを終了する形態としてもよい。
【００６２】
次に、図７及び図８を参照して、本実施形態に係る移動体位置推定プログラムの作用について説明する。まず、図７は、本実施形態に係る移動体位置推定処理の一例を示すフローチャートである。ＣＰＵ１３が移動体位置推定プログラム１５Ａを実行することによって、図７に示す移動体位置推定処理が実行される。図７に示す移動体位置推定処理は、例えば、ユーザにより移動体位置推定装置２０に移動体位置推定処理の実行指示が入力された場合に実行される。
【００６３】
ステップＳ１００で、検出部２２は、撮影部２１で撮影した画像を取得する。
【００６４】
ステップＳ１０２で、検出部２２は、取得した画像からＦＡＳＴ等のコーナー検出アルゴリズムを用いて構造物の特徴点を検出する。
【００６５】
ステップＳ１０４で、作成部２３は、ＳＬＡＭを用いて検出部２２で検出した構造物の特徴点を座標系に配置した無次元測定点群を作成する。
【００６６】
ステップＳ１０６で、作成部２３は、オドメトリー法を用いて撮影部２１により撮影された画像から計測した車両１０の移動量、及び構造物までの距離及び大きさ等の情報を無次元測定点群に適用した有次元測定点群をさらに作成する。また、作成部２３は、作成した有次元測定点群及び移動体点を設定した測定座標系を作成する。また、取得部２４は、ストレージ１６に予め記憶していた地図座標系を取得する。
【００６７】
ステップＳ１０８で、推定部２５は、作成した測定座標系及び取得した地図座標系に隣接する特徴点を代表点に置換し、代表点に重み値を付与するリサンプリング処理を行う。
【００６８】
ステップＳ１１０で、推定部２５は、リサンプリング処理を施した測定座標系及び地図座標系を作成する。
【００６９】
ステップＳ１１２で、推定部２５は、リサンプリング処理を施した代表点の密度を取得する。
【００７０】
ステップＳ１１４で、推定部２５は、密度が所定の閾値を超えた代表点を特定する。ステップＳ１１６で、推定部２５は、特定した代表点に関連する特徴点を除いたリサンプリング前の測定座標系及び地図座標系を用いて位置合わせ処理を行い、距離の総和が最小となる特徴点及び代表点の組み合わせと、距離の総和の値とを算出する。
【００７１】
ステップＳ１１８で、推定部２５は、算出した特徴点及び代表点の組み合わせを用いて同次変換行列の初期値を導出する。ステップＳ１２０で、推定部２５は、リサンプリング前の測定座標系に含まれる移動体点に同次変換行列の初期値を適用して、車両１０の初期位置を推定する。
【００７２】
ステップＳ１２２で、推定部２５は、リサンプリング後の測定点群及び地図点群に含まれる特徴点及び代表点と、同次変換行列の初期値とを用いて位置合わせを行い、距離の総和が最小となる特徴点及び代表点の組み合わせと、距離の総和の値とを算出する。
【００７３】
ステップＳ１２４で、推定部２５は、算出した特徴点及び代表点の組み合わせを用いて同次変換行列を導出する。ステップＳ１２６で、推定部２５は、測定座標系に含まれる移動体点に同次変換行列を適用して、車両１０の位置を推定する。
【００７４】
ステップＳ１２８で、推定部２５は収束条件を満たしているか否かの判定を行う。ここで、収束条件を満たす場合（ステップＳ１２８：Ｙ）、ステップＳ１３０に移行する。ステップＳ１３０で、推定部２５は、推定した車両１０の位置を車両１０の現在位置としてみなす。一方、収束条件を満たさない場合（ステップＳ１２８：Ｎ）、ステップＳ１２２に移行する。
【００７５】
ステップＳ１３２で、推定部２５は、目的位置に到達し、撮影を終了するか否かの判定を行う。撮影を終了する場合（ステップＳ１３２：Ｙ）、移動体位置推定処理を終了する。一方、撮影を終了しない場合（ステップＳ１３２：Ｎ）、ステップＳ１００に移行する。
【００７６】
次に図８を参照して、本実施形態に係るリサンプリング処理の説明をする。図８は本実施形態に係るリサンプリング処理の一例を示すフローチャートである。図７に示すステップＳ１０６が終了した場合、一例として、図８に示すリサンプリング処理が実行される。
【００７７】
ステップＳ２００で、推定部２５は、測定座標系及び地図座標系を取得する。
【００７８】
ステップＳ２０２で、推定部２４は、測定座標系及び地図座標系の任意の特徴点を検出する。ステップＳ２０４で、推定部２４は、検出した特徴点の座標及び参照した座標をＲＡＭ１４に記憶する。
【００７９】
ステップＳ２０６で、推定部２４は、検出した特徴点に隣接する座標を参照して、特徴点が設定されているか否かの判定を行う。隣接する特徴点がない場合（ステップＳ２０６：Ｙ）、ステップＳ２０８に移行する。一方、隣接する特徴点がある場合（ステップＳ２０６：Ｎ）、ステップＳ２０２に移行して、検出した特徴点に隣接する特徴点を検出する。
【００８０】
ステップＳ２０８で、推定部２５は、記憶した特徴点の数及び参照した座標の数を算出する。ステップＳ２１０で、推定部２５は、記憶した特徴点の座標を用いて、関連する複数の特徴点の代表点として重心座標を算出する。
【００８１】
ステップＳ２１２で、推定部２５は、隣接する特徴点の数、及び参照した座標の数、を用いて重心位置に付与する密度を算出する。
【００８２】
ステップＳ２１４で、推定部２５は、測定座標系及び地図座標系上に設定されている全ての特徴点を検出したか否かの判定を行う。全ての特徴点を検出した場合（ステップＳ２１４：Ｙ）、ステップＳ２１６に移行する。一方、全ての特徴点を検出していない場合（ステップＳ２１４：Ｎ）、ステップＳ２０２に移行して、特徴点の検出を行う。
【００８３】
ステップＳ２１６で、推定部２５は、特徴点及び算出した密度を付与した重心位置を測定座標系及び地図座標系に設定し、リサンプリング処理を終了する。
【００８４】
以上説明したように、本実施形態によれば、特徴点が密集している構造物の特徴点の代表点及び密度を算出し、構造物の代表点及び密度を用いて、測定点群及び地図点群の位置合わせを行い、同次変換行列を導出することができる。従って、特徴点が密集した構造物があった場合でも、精度よく移動体の位置を推定することができる。
【００８５】
なお、本実施形態では、同次変換行列の初期値及び移動体の初期位置は、測定点群及び地図点群に含まれる特徴点及び密度が所定の閾値以下の代表点を位置合わせして導出する形態として説明したが、これに限定されない。所定の閾値を超えた代表点を用いて、同次変換行列の初期値及び移動体の初期位置を導出する形態としてもよいし、同次変換行列の初期値及び移動体の初期位置は予め初期値を設定する形態としてもよい。
【００８６】
また、本実施形態に係る測定座標系及び地図座標系は、三次元空間座標上に特徴点を配置する形態として、説明したがこれに限定されない。二次元平面座標上に特徴点を配置する形態としてもよい。
【００８７】
その他、上記実施形態で説明した移動体位置推定装置の構成は、一例であり、主旨を逸脱しない範囲内において状況に応じて変更してもよい。
【００８８】
また、上記実施形態で説明したプログラムの処理の流れも、一例であり、主旨を逸脱しない範囲内において不要なステップを削除したり、新たなステップを追加したり、処理順序を入れ替えたりしてもよい。
【００８９】
なお、上記各実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した移動体位置推定処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Field Programmable Gate Array）等の製造後に回路構成を変更可能なＰＬＤ（Programmable Logic Device）、及びＡＳＩＣ（Application Specific Integrated Circuit）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、リサンプリング処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。
また、上記各実施形態では、移動体位置推定処理、及びリサンプリング処理のプログラムがストレージ１６に予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ−ＲＯＭ（Compact Disk Read Only Memory）、ＤＶＤ−ＲＯＭ（Digital Versatile Disk Read Only Memory）、及びＵＳＢ（Universal Serial Bus）メモリ等の記録媒体に記録された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。
【符号の説明】
【００９０】
１０    車両
１１    カメラ
１２    表示部
１３    ＣＰＵ
１４    ＲＡＭ
１５    ＲＯＭ
１５Ａ  移動体位置推定プログラム
１６    ストレージ
１６Ａ  地図情報
２０    移動体位置推定装置
２１    撮影部
２２    検出部
２３    作成部
２４    取得部
２５    推定部
【図１】
2020125960.tif 000010
【図２】
2020125960.tif 000011
【図３】
2020125960.tif 000012
【図４】
2020125960.tif 000013
【図５】
2020125960.tif 000014
【図６】
2020125960.tif 000015
【図７】
2020125960.tif 000016
【図８】
2020125960.tif 000017