JP7215448B2 - 検出器の姿勢・位置検出装置 - Google Patents

Description

本開示は、車両に搭載して用いられる検出器の姿勢および位置を検出する技術に関する。

車両に搭載されている検出器から適切な検出結果を得るために、種類および分解能が異なる２つの検出器の検出結果を融合させて、各検出器の姿勢および位置等の較正パラメーターを決定する技術が提案されている（例えば、特許文献１）。

国際公開第２０１７－１２２５２９号

しかしながら、特許文献１に記載の技術では、２つの検出器の種類が異なるので、各検出器の検出範囲が重複しない場合や、検出範囲が重複する場合であっても重複範囲が小さい場合がある。このような場合、較正パラメーターの決定に利用可能な検出結果が減少し、較正パラメーターを精度よく決定できないおそれがある。したがって、車両に搭載されている検出器の姿勢および位置の少なくともいずれか一方を精度よく検出する技術が望まれている。

本開示は、以下の態様として実現することが可能である。

本開示の一実施形態によれば、車両（１００）に搭載されて用いられる検出器（５０）の姿勢・位置検出装置（１０）が提供される。この姿勢・位置検出装置は、前記検出器から照射された照射光の照射範囲内の対象物までの距離を示す距離画像（ＫＩ）と、環境光の受光強度を示す背景光画像（ＨＩ）であって、解像度が前記距離画像の解像度以上である背景光画像と、を異なるタイミングにて前記検出器から取得する画像取得部（２１）と、前記距離画像と前記背景光画像とを用いて、前記検出器の姿勢・位置を検出する姿勢・位置検出部（２３）と、を備える。

この形態の検出器の姿勢・位置検出装置によれば、距離画像と、解像度が距離画像の解像度以上である背景光画像とを検出器から取得し、距離画像と背景光画像とを用いて、検出器の姿勢・位置を検出するので、検出器の検出データのみを用いて検出器の姿勢・位置を検出できる。このため、種類の異なる２つの検出器の検出データを用いて検出器の姿勢・位置を検出する構成に比べて、検出器の姿勢・位置を精度よく検出できる。

本開示は、種々の形態で実現することも可能である。例えば、検出器の姿勢・位置検出方法、検出器の姿勢・位置検出プログラムまたは当該プログラムを記録するコンピュータ読み取り可能記録媒体としても実現可能である。

本開示の一実施形態としての検出器の姿勢・位置検出システムの概略構成を示す説明図。 検出器の姿勢・位置検出システムの概略構成を示す説明図。 ターゲットの一例を示す説明図。 姿勢・位置検出装置の概略構成を示すブロック図。 受光素子の構成を示す説明図。 背景光画像の一例を模式的に示す説明図。 距離画像の一例を模式的に示す説明図。 姿勢・位置検出処理の処理手順を示すフローチャート。 対応画素および周囲画素を示す説明図。 第２実施形態の姿勢・位置検出処理の処理手順を示すフローチャート。 第２実施形態のターゲットの一例を示す説明図。 第３実施形態の検出器の姿勢・位置検出システムの概略構成を示す説明図。 第３実施形態の姿勢・位置検出処理の処理手順を示すフローチャート。 ターゲットの他の例を示す説明図。 ターゲットの他の例を示す説明図。 ターゲットの他の例を示す説明図。 ターゲットの他の例を示す説明図。

第１実施形態：
図１および図２に示すように、車両１００に搭載されている検出器５０の姿勢・位置検出システム５００は、複数のターゲットＴＧと、姿勢・位置検出装置１０と、測量機器３００とを備える。複数のターゲットＴＧは、検出器５０の姿勢および位置の少なくともいずれか一方の検出に用いられる物理的な標的であり、車両１００の周囲に配置されている。姿勢・位置検出システム５００では、姿勢・位置検出装置１０および測量機器３００がそれぞれ、ターゲットＴＧ上における１つ又は複数の特徴点により決定されるターゲットＴＧの３次元空間における座標位置を検出し、両座標位置のずれ量を求めることにより、後述の基準座標軸に対する検出器５０の姿勢・位置を検出する。本明細書において、姿勢・位置の記載は、姿勢および位置の少なくともいずれか一方を意味する。

図２に示すように、複数のターゲットＴＧは、予め定められた検出用パターンＫＰが表面に描かれた平面板である。ターゲットＴＧにおける車両１００と対面する面には、市松模様の検出用パターンＫＰが表されている。図３に示すように、検出用パターンＫＰは、格子状の複数のパターンにより構成され、黒色のパターンＰｔ１と白色のパターンＰｔ２とが交互に配列されている。各パターンＰｔ１およびＰｔ２との交点には、特徴点Ｆｐを示す記号が表されている。黒色のパターンＰｔ１と白色のパターンＰｔ２とが隣接している、換言すると、輝度が異なるパターンＰｔ１およびＰｔ２が隣接していることにより、後述の背景光画像におけるエッジ検出が容易となり、背景光画像上の特徴点Ｆｐの画素位置を精度よく検出できる。

姿勢・位置検出装置１０は、後述の姿勢・位置検出処理を実行することにより、検出器５０の姿勢・位置を検出する。具体的には、姿勢・位置検出装置１０は、検出器５０から取得された検出データ、具体的には、後述の背景光画像および距離画像を用いてターゲットＴＧ上の特徴点Ｆｐの距離を算出し、算出された距離を３次元位置に変換する。そして、姿勢・位置検出装置１０は、変換された３次元位置と測量機器３００により取得される３次元位置とを比較することによって、後述の基準座標軸に対する検出器５０の姿勢・位置を検出する。

姿勢・位置検出装置１０は、測量機器３００により取得される車両１００の位置を用いて、検出器５０の姿勢・位置の検出の基準とする座標軸（以下、「基準座標軸」と呼ぶ）を決定する。また、姿勢・位置検出装置１０は、測量機器３００により取得されるターゲットＴＧ上の特徴点Ｆｐの位置を用いて、基準座標軸に対するターゲットＴＧの特徴点Ｆｐの３次元位置（以下、「基準位置」と呼ぶ）を決定する。具体的には、姿勢・位置検出装置１０は、測量機器３００を用いて、車両１００の４つの車輪の位置および向きを検出して、基準座標軸の向きを決定する。姿勢・位置検出装置１０は、測量機器３００を用いて、各ターゲットＴＧの特徴点Ｆｐを計測し、基準座標軸に対するターゲットＴＧ上の特徴点Ｆｐの３次元位置（Ｘ、Ｙ、Ｚ）を検出する。姿勢・位置検出装置１０は、測量機器３００により検出された特徴点Ｆｐの３次元位置を基準位置とする。本実施形態において、測量機器３００は、例えば、トータルステーションである。なお、基準座標軸の原点位置は、車両１００の諸元表や、測量機器３００により取得される車両１００前方のエンブレムの位置に基づき、車両１００の前方先端にオフセットされる。基準座標軸の原点位置は、車両１００の前方先端に限らず、例えば、車両１００の後輪軸中心の位置としてもよい。

検出器５０は、車両１００の前方の屋根上に搭載され、照射光を照射して対象物からの反射光を受光する。検出器５０は、対象物からの照射光の反射光に加えて、反射光以外の光、例えば、日光、街灯の光、他車両の前照灯の光や、これらの光が対象物に反射した光などの環境光（以下、「背景光」とも呼ぶ）を受光する。検出器５０は、受光した光から背景光を除いた光を対象物からの反射光として特定し、照射光を照射してから反射光を受光するまでの時間、すなわち、光の飛行時間ＴＯＦ（Time of Flight）を特定する。検出器５０は、かかる飛行時間ＴＯＦを対象物までの距離として算出する。したがって、姿勢・位置検出装置１０は、照射光の反射光の受光強度を各画素の画素値とする反射強度画像と、背景光の受光強度を各画素の画素値とする背景光画像と、対象物までの距離を表す距離画像とを取得し、これら３つの画像を利用して、対象物を検出する。本実施形態において、検出器５０は、ライダー（ＬｉＤＡＲ：Light Detection And Ranging）である。なお、検出器５０は、例えば、車両１００のフロントグリル、フロントウィンドウ、フロントバンパー、リアウィンドウ、リアバンパー、フロントフェンダー、リアフェンダーに配置されていてもよく、複数備えられていてもよい。

図１および図２では図示を省略しているが、車両１００は、車両１００の運転支援または自動運転を実行するための制御装置や、検出器５０から入力される検出データを処理するデータ処理装置等を備えていてもよい。また、車両１００に姿勢・位置検出装置１０が備えられていてもよい。

図４に示すように、姿勢・位置検出装置１０は、ＣＰＵ２０と、メモリ３０と、入出力インターフェース１１とを備えるパーソナルコンピュータにより構成されている。ＣＰＵ２０、メモリ３０、および入出力インターフェース１１は、バス１５を介して双方向に通信可能に接続されている。メモリ３０は、ＲＯＭ、ＲＡＭ、およびＥＥＰＲＯＭを含む。入出力インターフェース１１には、検出器５０が備える発光制御部５１および受光制御部５５とそれぞれ制御信号線を介して接続されている。

発光制御部５１は、姿勢・位置検出装置１０から入力される発光指令信号に応じたタイミングで照射光を照射するよう発光素子５２を駆動する。発光素子５２は、例えば、赤外レーザダイオードであり、照射光として赤外レーザ光を出射する。発光素子５２は１つでもよく複数であってもよい。発光素子５２により照射された照射光は、検出器５０の照射範囲内に存在する対象物により反射される。対象物により反射された反射光及び背景光を含む光は、受光素子５６により受光される。

受光制御部５５は、姿勢・位置検出装置１０から入力される受光指令信号に応じて受光素子５６に入射された入射光の受光値を画素ごとに取得して、背景光画像ＨＩ、距離画像ＫＩおよび反射強度画像ＲＩを出力する。本実施形態において、「背景光画像ＨＩ」とは、発光素子５２から照射光が射出されていない状態において受光素子５６が受光した光量や受光強度を示す輝度を画素値とする画像を意味する。背景光画像ＨＩは、例えば、発光制御部５１が発光素子５２を駆動する直前に取得することができる。「距離画像ＫＩ」とは、反射光によって特定される対象物の少なくとも一部が存在する位置を示す検出点（検出点群）に対応する距離を画素値とする画像を意味する。「反射強度画像ＲＩ」とは、発光素子５２が照射光を射出して対象物により反射された反射光が受光素子５６に受光された光量や受光強度を示す輝度を画素値とする画像を意味する。

図５に示すように、受光素子５６は、受光面５７に複数の受光要素５８が面状に二次元配列された受光素子アレイである。受光要素５８は、例えば、シングルフォトンアバランシェダイオード（ＳＰＡＤ：Single Photon Avalanche Diode）により構成される。受光要素５８としては、ＰＩＮフォトダイオードやＡＰＤ等の他の種類の受光素子により構成されていてもよい。受光制御部５５は、受光面５７に配列された複数の受光要素５８を、水平方向の数Ｈ及び垂直方向の数Ｖの受光要素５８の集まりを一画素として、二次元配列された複数の画素Ｐｓに区分し、各画素Ｐｓに含まれる［Ｈ×Ｖ］個の受光要素５８の受光結果を、それぞれ、各画素Ｐｓの画素値として出力する。Ｈ及びＶはそれぞれ１以上の整数である。１つの画素Ｐｓを構成する受光要素５８の数［Ｈ×Ｖ］を「画素サイズ」とも呼ぶ。この画素サイズが小さいほど、照射光の反射光を含む光を検出する画素の分解能（deg）は高く、取得される画像の解像度は高くなる。

１つの画素Ｐｓの画素サイズ、すなわち、受光要素５８の数［Ｈ×Ｖ］は、受光制御部５５により予め設定されており、本実施形態では、背景光画像ＨＩの解像度が、距離画像ＫＩおよび反射強度画像ＲＩの解像度よりも高くなるように設定されている。具体的には、図５の破線枠で示す画素Ｐｓ＿ｈは、Ｈ＝Ｖ＝２の画素サイズに設定された、背景光画像ＨＩの一画素を示し、一点鎖線枠で示す画素Ｐｓ＿ｋは、Ｈ＝４、Ｖ＝６の画素サイズに設定された、距離画像ＫＩおよび反射強度画像ＲＩの一画素を示している。したがって、画素Ｐｓ＿ｈの分解能は、画素Ｐｓ＿ｋの分解能より高い。このような画素サイズで各画素Ｐｓに含まれる［Ｈ×Ｖ］個の受光要素５８の受光値を、それぞれ、各画素Ｐｓの画素値とした画像が出力される。したがって、距離画像ＫＩの解像度に比べて高い解像度の背景光画像ＨＩが出力される。

図６には、ターゲットＴＧに対する背景光画像ＨＩを示し、図７には、ターゲットＴＧに対する距離画像ＫＩを示している。背景光画像ＨＩの画素Ｇ１は、画素Ｐｓ＿ｈに含まれる４つの受光要素５８により構成されている。距離画像ＫＩの画素Ｇ２は、画素Ｐｓ＿ｋに含まれる６つの受光要素５８により構成されている。したがって、背景光画像ＨＩにおける水平方向の画素数Ｒは、距離画像ＫＩにおける水平方向の画素数Ｍよりも大きく、背景光画像ＨＩにおける垂直方向の画素数Ｑは、距離画像ＫＩにおける垂直方向の画素数Ｎよりも大きい。また、画素Ｐｓ＿ｋ及びＰｓ＿ｈの画素サイズは一例であって、これに限定されるものではなく、０＜Ｐｓ＿ｈ＜Ｐｓ＿ｋを満たす他の任意の値を設定できる。

図４に示すように、ＣＰＵ２０は、メモリ３０に記憶されたプログラムを展開して実行することにより、画像取得部２１、姿勢・位置検出部２３および較正部２５として機能する。

画像取得部２１は、検出器５０から背景光画像ＨＩ、距離画像ＫＩおよび反射強度画像ＲＩを取得する。姿勢・位置検出部２３は、背景光画像ＨＩおよび距離画像ＫＩを用いて、検出された特徴点Ｆｐの３次元位置と基準位置とを比較することによって、基準座標軸に対する検出器５０の姿勢・位置を検出する。較正部２５は、姿勢・位置検出部２３によって検出された検出器５０の姿勢・位置と、予め定められた基準とする検出器５０の姿勢・位置とのずれ量を相殺するように、検出器５０の姿勢・位置を較正する。

図８に示す検出器の姿勢・位置検出処理は、ＣＰＵ２０が、メモリ３０に予め格納されている図示しない姿勢・位置検出プログラムを実行することにより実行される。姿勢・位置検出処理は、例えば、出荷前の車両１００の検査ラインにおいて、検出器５０の姿勢・位置を調整するために実行される。

姿勢・位置検出部２３は、基準位置を取得する（ステップＳ１０５）。具体的には、姿勢・位置検出部２３は、測量機器３００を用いてターゲットＴＧ上の特徴点Ｆｐを検出することにより、基準位置を取得する。なお、姿勢・位置検出部２３は、取得された基準位置をメモリ３０に格納してもよい。

画像取得部２１は、検出器５０から背景光画像ＨＩを取得する（ステップＳ１１０）。画像取得部２１は、検出器５０から距離画像ＫＩを取得する（ステップＳ１１５）。

姿勢・位置検出部２３は、背景光画像ＨＩにおけるターゲットＴＧ上の特徴点Ｆｐの画素位置を検出する（ステップＳ１２０）。具体的には、姿勢・位置検出部２３は、例えば、Harrisのコーナー検出法を用いて、背景光画像ＨＩからターゲットＴＧのコーナー部分の画素を抽出する。

姿勢・位置検出部２３は、背景光画像ＨＩにおけるターゲットＴＧ上の特徴点Ｆｐの画素位置に対応する距離画像ＫＩにおける画素（以下、「対応画素」と呼ぶ）の位置を検出し、かかる対応画素の周囲の画素（以下、「周囲画素」と呼ぶ）を抽出する（ステップＳ１２５）。具体的には、姿勢・位置検出部２３は、まず、距離画像ＫＩから、特徴点Ｆｐ、例えば、特徴点Ｆｐ１の対応画素Ｇｔ（図９の破線に示す画素）を抽出する。次に、姿勢・位置検出部２３は、対応画素Ｇｔの周囲画素Ｇｓとして、一点鎖線に示す、対応画素Ｇｔの周囲を囲み、対応画素Ｇｔに隣接する画素Ｇｓ１、Ｇｓ２、Ｇｓ３、Ｇｓ４、Ｇｓ５、Ｇｓ６、Ｇｓ７およびＧｓ８を抽出する。同様にして、ターゲットＴＧ上の他の特徴点Ｆｐ（図９に示す例では、Ｆｐ２、Ｆｐ３およびＦｐ４）に対しても、対応画素Ｇｔおよび周囲画素Ｇｓが抽出される。なお、周囲画素Ｇｓの少なくとも１つ以上は、特徴点Ｆｐが含まれる同一のターゲットＴＧから抽出される。また、対応画素Ｇｔの抽出、換言すると、背景光画像ＨＩ上の画素位置と、距離画像ＫＩ上の画素位置との対応付けは、背景光画像ＨＩと距離画像ＫＩとが３次元空間上において重畳領域が同一であることから、画素Ｐｓの画素サイズと画素位置とを用いることで容易に求めることができる。以降の説明では、各特徴点を区別する場合、特徴点Ｆｐ１、Ｆｐ２等と呼び、各特徴点を区別しない場合には、総称して特徴点Ｆｐと呼ぶ。また、各周囲画素を区別する場合、周囲画素Ｇｓ１、Ｇｓ２等と呼び、各周囲画素を区別しない場合には、総称して周囲画素Ｇｓと呼ぶ。なお、図９においては、周囲画素Ｇｓは、対応画素Ｇｔに隣接する１画素まで（合計８画素）であるが、例えば、対応画素Ｇｔに隣接する２画素まで（合計２４画素）や、３画素まで（合計４８画素）としてもよい。

図８に示すように、姿勢・位置検出部２３は、周囲画素Ｇｓの各画素の距離を取得する（ステップＳ１３０）。具体的には、姿勢・位置検出部２３は、距離画像ＫＩ上の各周囲画素Ｇｓ１、Ｇｓ２、Ｇｓ３、Ｇｓ４、Ｇｓ５、Ｇｓ６、Ｇｓ７およびＧｓ８の画素値を取得する。取得された各周囲画素Ｇｓの距離は、本開示における「周囲点距離」に相当する。

姿勢・位置検出部２３は、取得された各周囲点距離を３次元座標に変換する（ステップＳ１３５）。具体的には、姿勢・位置検出部２３は、極座標を直交座標に変換する公知の座標変換処理を実行する。これにより、距離画像ＫＩ上の対応画素Ｇｔおよび周囲画素Ｇｓの３次元空間上の位置（以下、「周囲点３次元位置」とも呼ぶ）を取得することができる。

姿勢・位置検出部２３は、周囲画素Ｇｓの３次元位置を用いて、特徴点Ｆｐの３次元位置を算出する（ステップＳ１４０）。具体的には、姿勢・位置検出部２３は、ターゲットＴＧが平面であるので周囲画素ＧｓのうちターゲットＴＧから得られた画素の３次元位置を用いて補間処理を実行することによって、特徴点Ｆｐの３次元位置（以下、「対応点３次元位置」とも呼ぶ）を算出する。周囲画素ＧｓがターゲットＴＧから得られた画素であるか否かは、対応画素Ｇｔと各周囲画素Ｇｓとの３次元空間上の距離の差が予め定められた閾値である場合、周囲画素ＧｓがターゲットＴＧから得られた画素であると判定する。上述の閾値は、例えば、０．３メートルである。本実施形態では、補間処理を行うことによって対応点３次元位置を算出するので、特徴点Ｆｐの３次元位置を距離画像ＫＩ上の対応画素Ｇｔから算出される３次元位置を用いて算出する構成に比べて、特徴点Ｆｐの３次元位置を高精度（サブピクセル精度）に算出できる。なお、補間処理としては、例えば、ニアレストネイバー法、バイリニア法、バイキュービック法を用いることができる。

姿勢・位置検出部２３は、検出器５０の姿勢・位置を検出する（ステップＳ１４５）。具体的には、姿勢・位置検出部２３は、検出器５０により取得された複数の特徴点Ｆｐの３次元位置と、測量機器３００により取得された複数の基準位置とを、それぞれの３次元位置や勾配情報等を用いて、最近傍法やGlobal Nearest Neighbor法により対応付けし、４つ以上の対応点を用いて下記式（１）により、基準座標軸に対する検出器５０の姿勢および位置を算出する。
Ｘ’＝ＲＸ＋Ｔ・・・（１）
上記式（１）において、Ｒは姿勢であり、Ｔは位置である。姿勢Ｒは３×３行列であり、位置Ｔは３×１行列である。Ｘは検出器５０により取得された特徴点Ｆｐの３次元位置であり、Ｘ’は測量機器３００により取得された基準位置である。なお、特徴点Ｆｐの３次元位置と基準位置との対応付けは、例えば、ターゲットＴＧの隅に予めマーカーを付与しておき、かかるマーカーを検出し、該マーカーからの位置関係から算出されてもよい。

較正部２５は、検出器５０の姿勢・位置を較正する（ステップＳ１５０）。具体的には、較正部２５は、検出された姿勢・位置を用いて、検出器５０の物理的な姿勢・位置の調整、あるいは、検出された姿勢・位置と予め定められた基準 とする検出器５０の姿勢・位置との相違を補正値として検出器５０から出力される検出データを補正する、較正またはエーミングを実行する。

以上説明した第１実施形態に係る姿勢・位置検出装置１０によれば、対象物には、平面上に表された予め定められた検出用パターンＫＰを有するターゲットＴＧが含まれるので、ターゲットＴＧにおける特徴点Ｆｐの位置を精度よく検出できる。姿勢・位置検出部２３は、背景光画像ＨＩにおけるターゲットＴＧ上の特徴点Ｆｐの画素位置を検出し、距離画像ＫＩにおける特徴点Ｆｐの画素位置に対応する対応画素Ｇｔの３次元位置である対応点３次元位置を、距離画像ＫＩにおける対応画素Ｇｔの周囲画素Ｇｓの３次元位置である周囲点３次元位置を補間することによって算出し、算出された対応点３次元位置を用いて検出器５０の姿勢・位置を検出するので、距離画像ＫＩのみを用いて特徴点Ｆｐの位置を検出する構成に比べて、特徴点Ｆｐの３次元位置を精度よく算出できる。この結果、対応点３次元位置を精度よく算出できるので、検出器５０の姿勢・位置を精度よく検出できる。

第２実施形態：
第２実施形態に係る検出器５０の姿勢・位置検出システムは、姿勢・位置検出装置１０が車両１００に搭載されている点において、第１実施形態に係る検出器５０の姿勢・位置検出システム５００と異なる。第２実施形態の検出器５０の姿勢・位置検出システムのその他の構成は、第１実施形態と同様の構成であるので、同一の構成要素には同一の符号を付し、その詳細な説明を省略する。

第１実施形態の姿勢・位置検出処理は、車両１００の出荷時に検査ラインにおいて実行されていた。これに対して、第２実施形態の姿勢・位置検出処理は、出荷後の車両１００の走行中に実行される。このため、第２実施形態では、車両１００の走行環境に設けられている道路構造物をターゲットＴＧとして用いている。以下、具体的に説明する。

図１０に示す第２実施形態における姿勢・位置検出処理は、ステップＳ１０１およびステップＳ１０３を追加して実行する点と、ステップＳ１０５に代えてステップＳ１０５ａを実行する点とにおいて、第１実施形態の姿勢・位置検出処理と異なる。第２実施形態の姿勢・位置検出処理のその他の手順は、第１実施形態と同じであるので、同一の手順には同一の符号を付し、その詳細な説明を省略する。

姿勢・位置検出処理が開始されると、姿勢・位置検出部２３は、車両１００の現在位置の周辺に存在する道路構造物の位置を取得する（ステップＳ１０１）。具体的には、姿勢・位置検出部２３は、車両１００に搭載されている図示しない位置センサを用いて、車両１００の現在位置（自己位置）を取得する。現在位置は、車両１００の緯度および経度で表される。位置センサとしては、例えば、全地球航法衛星システム（ＧＮＳＳ）やジャイロセンサ等を用いることができる。なお、車両１００の現在位置は、無線基地局、例えば、移動体基地局や、道路に設置されている路車間通信装置を介して取得されてもよく、ＶＩＣＳ（Vehicle Information and Communication System/ＶＩＣＳは登録商標）から提供される交通情報を利用して取得されてもよい。次に、姿勢・位置検出部２３は、車両１００に搭載されている図示しないナビゲーション装置の地図情報を参照して、車両１００の現在位置の周辺に存在する道路構造物を取得する。本実施形態において、「道路構造物」とは、道路や、路肩に設けられている路側物、例えば、縁石、ガードレール、標識、信号機、走路区画線（レーンマーカー）、停止線、横断歩道等のうち、予め定められた種類の路側物を意味する。「予め定められた種類」とは、表面形状が平面状であることを意味し、例えば、止まれの標識や、一時停止の標識、信号機、走路区画線、停止線、横断歩道等、平面状に表された路側物が該当する。したがって、例えば、雪対策用のカバー付きの信号機や、表面形状が湾曲している信号機は、道路構造物には含まれない。また、本実施形態において、上述の「地図情報」は、高精度３次元地図を意味し、道路構造物の種別、例えば、標識の種類等を示す情報が含まれている。

姿勢・位置検出部２３は、取得された道路構造物をターゲットＴＧとして設定する（ステップＳ１０３）。例えば、図１１に示すターゲットＴＧ１は、「止まれ」の標識であり、検出用パターンＫＰ１を備えている。図１１では、検出用パターンＫＰ１上に特徴点Ｆｐ５およびＦｐ６を示す符号が表されているが、実際の検出用パターンＫＰ１、すなわち、道路構造物には、特徴点Ｆｐ５およびＦｐ６は表されていない。このため、姿勢・位置検出部２３は、「止まれ」の文字のエッジ部分を特徴点Ｆｐ５およびＦｐ６とみなして、背景光画像ＨＩおよび距離画像ＫＩにおける特徴点Ｆｐ５およびＦｐ６を検出することになる。なお、特徴点Ｆｐは、文字のエッジ部分に限らず、標識の角や中心の３次元位置としてもよい。

姿勢・位置検出部２３は、基準位置を取得する（ステップＳ１０５ａ）。具体的には、姿勢・位置検出部２３は、地図情報を用いて、車両１００の現在位置を原点とした特徴点Ｆｐに対応する３次元位置を取得し、かかる位置を基準位置とする。すなわち、第２実施形態における基準位置とは、高精度３次元地図から求められる特徴点の３次元位置である。ステップＳ１０５ａの実行後、上述のステップＳ１１０が実行される。

以上説明した第２実施形態に係る計測器の姿勢・位置検出装置１０によれば、ターゲットＴＧ１は、車両１００の現在位置の周辺に存在する予め定められた種類の道路構造物であり、姿勢・位置検出部２３は、道路構造物上の予め定められた特徴点を用いて、検出器５０の姿勢・位置を検出するので、車両１００が走行中であっても、検出器５０の姿勢・位置を検出できる。

第３実施形態：
図１２に示すように、第３実施形態に係る検出器５０の姿勢・位置検出システム５００ａは、測量機器３００を省略する点と、車両１００に代えて車両１００ａを備える点とにおいて、第１実施形態に係る検出器５０の姿勢・位置検出システム５００と異なる。検出器５０の姿勢・位置検出システム５００ａのその他の構成は、第１実施形態と同様の構成であるので、同一の構成要素には同一の符号を付し、その詳細な説明を省略する。

第３実施形態の車両１００ａは、検出器６０を備える。検出器６０は、例えば、撮像装置である。なお、検出器６０は、撮像装置に代えて、検出器５０とは異なるライダーであってもよい。検出器６０は、検出器６０の検出範囲と、検出器５０の検出範囲との少なくとも一部が重複するように、車両１００ａの前方の屋根上に搭載されている。このため、本実施形態では、検出器５０の検出範囲と、検出器６０の検出範囲との重複範囲（重畳領域）を利用して、検出器６０の姿勢・位置を基準とした座標軸に対する検出器５０の姿勢・位置のずれ量を検出することができる。

図１３に示す第３実施形態における姿勢・位置検出処理は、ステップＳ１０５に代えてステップＳ１０５ｂを実行する点において、第１実施形態の姿勢・位置検出処理と異なる。第３実施形態の姿勢・位置検出処理のその他の手順は、第１実施形態と同じであるので、同一の手順には同一の符号を付し、その詳細な説明を省略する。

姿勢・検出処理が開始されると、姿勢・位置検出部２３は、基準位置を取得する（ステップＳ１０５ｂ）。具体的には、姿勢・位置検出部２３は、検出器６０に基準位置を取得させる。検出器６０は、各ターゲットＴＧの撮像画像を取得し、基準座標軸の向きおよび位置を決定する。具体的には、検出器６０は、上述のHarrisのコーナー検出法を用いて、各ターゲットＴＧの撮像画像からターゲットＴＧのコーナー部分の画素を抽出して、特徴点Ｆｐの位置を基準位置として検出する。すなわち、第３実施形態における基準位置とは、検出器６０により取得される撮像画像から求められる特徴点の画素位置である。ステップＳ１０５ｂの実行後、上述のステップＳ１１０が実行される。

以上説明した第３実施形態に係る検出器５０の姿勢・位置検出装置１０によれば、上記第１実施形態と同様な効果を奏する。加えて、検出器６０に対する検出器５０の姿勢・位置を、精度よく検出できる。

その他の実施形態：
（１）上記各実施形態において、姿勢・位置検出装置１０は、較正部２５を備えていなくてもよい。すなわち、検出器５０の姿勢・位置が検出されるにとどまっていてもよい。

（２）上記各実施形態において、検出器５０は、ライダーであったが、ＴＯＦカメラであってもよい。

（３）上記各実施形態において、ターゲットＴＧ、ＴＧ１および検出用パターンＫＰ、ＫＰ１は、上述の例に限定されない。例えば、図１４に示すように、ターゲットＴＧ２は、複数のドットパターンＰｔ３が表された検出用パターンＫＰ２を備えていてもよい。この場合、ドットパターンＰｔ３の重心位置に特徴点Ｆｐ７が表されていてもよい。また、例えば、図１５に示すように、ターゲットＴＧ３は、複数のＡＲマーカーＰｔ４が表された検出用パターンＫＰ３を備えていてもよい。ＡＲマーカーＰｔ４は、例えば、ＡｒＵｃｏマーカーとしてもよい。この場合、ＡＲマーカーＰｔ４の４隅に特徴点Ｆｐ８が表されていてもよい。

また、例えば、図１６に示すように、ターゲットＴＧごとに検出用パターンＫＰの色を異ならせてもよい。具体的には、第１ターゲットＴＧ４は検出用パターンＫＰ４を備え、第２ターゲットＴＧ５は検出用パターンＫＰ５を備え、第３ターゲットＴＧ６は検出用パターンＫＰ６を備えている。各検出用パターンＫＰ４、ＫＰ５およびＫＰ６は、市松模様状のパターンであり、格子状の複数のパターンにより構成されている。検出用パターンＫＰ４、ＫＰ５およびＫＰ６における黒色のパターンＰｔ５ａ、Ｐｔ５ｂおよびＰｔ５ｃは、互いに色が異なり、検出用パターンＫＰ４のパターンＰｔ５ａの色は、検出用パターンＫＰ５のパターンＰｔ５ｂの色および検出用パターンＫＰ６のパターンＰｔ５ｃの色に比べて薄い。また、検出用パターンＫＰ５のパターンＰｔ５ｂの色は、検出用パターンＫＰ６のパターンＰｔ５ｃの色に比べて薄い。すなわち、各パターンＰｔ５ａ、Ｐｔ５ｂおよびＰｔ５ｃの輝度値は、パターンＰｔ５ａ、パターンＰｔ５ｂ、パターンＰｔ５ｃの順に大きい。

したがって、一つの背景光画像ＨＩ中に３つのターゲットＴＧ４、ＴＧ５およびＴＧ６が表されている場合であっても、背景光画像ＨＩの各画素値を、パターンＰｔ５ａ、Ｐｔ５ｂおよびＰｔ５ｃの各輝度値を閾値として区分することによって、各ターゲットＴＧ４、ＴＧ５およびＴＧ６における特徴点Ｆｐを区別して検出することができる。また、例えば、検出用パターンＫＰの色に代えて、または、色に加えて、検出用パターンＫＰの材質を異ならせてもよい。具体的には、検出用パターンＫＰ４、ＫＰ５およびＫＰ６がリフレクター機能を備え、検出用パターンＫＰ４、ＫＰ５およびＫＰ６ごとに反射率を異ならせてもよい。また、例えば、検出器５０の種類に応じた専用のターゲットＴＧを用いてもよい。具体的には、検出器５０がライダーにより構成される場合、ライダー用のリフレクター機能を有するターゲットＴＧを用いてもよい。検出器５０がカメラにより構成される場合、エッジ検出がより精度よく実行可能な色により構成される検出用パターンＫＰを備えるターゲットＴＧを用いてもよい。

（４）上記各実施形態において、ターゲットＴＧは、平面板であったが、立方体形状であってもよいし、平面板が凹凸状に組み合わされて構成される立方体形状であってもよい。具体的には、図１７に示すように、ターゲットＴＧ７は、大きさの異なる２つの立方体Ｒｓ１およびＲｓ２を交互に配列することにより構成される立方体である。ターゲットＴＧ７の面Ｓ１は、第１立方体Ｒｓ１の面Ｓ１ａと、第２立方体Ｒｓ２の面Ｓ１ｂとが交互に配列されることにより、凹凸形状となっている。この場合、面Ｓ１において凸面である面Ｓ１ａと凹面である面Ｓ１ｂとの交点を特徴点Ｆｐとすることができる。このような構成においても、凸面Ｓ１ａと凹面Ｓ１ｂとのエッジを抽出することで、特徴点Ｆｐの位置を検出できる。

（５）上記各実施形態において、背景光画像ＨＩの解像度は、距離画像ＫＩの解像度よりも高かったが、両画像の解像度は同じであってもよい。背景光画像ＨＩを用いて特徴点Ｆｐを検出することにより、より多くの物標をターゲットとすることができる。具体的には、距離画像ＫＩや反射強度画像ＲＩのみを用いる場合、距離の変化点や、ターゲットＴＧの材質の変化点を特徴点として検出する必要がある。これに対して、背景光画像ＨＩを用いる場合には、テクスチャが異なっていればよく、形状が平面の物体や、材質が均一の物体、例えば、標識、看板、路面、および道路にペイントされた標識等もターゲットとして採用できる。

本開示に記載の制御部等の各部及びその手法は、コンピュータプログラムにより具体化された一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリを構成することによって提供された専用コンピュータにより、実現されてもよい。あるいは、本開示に記載の制御部等の各部及びその手法は、一つ以上の専用ハードウエア論理回路によってプロセッサを構成することによって提供された専用コンピュータにより、実現されてもよい。もしくは、本開示に記載の制御部及びその手法は、一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリと一つ以上のハードウエア論理回路によって構成されたプロセッサとの組合せにより構成された一つ以上の専用コンピュータにより、実現されてもよい。また、コンピュータプログラムは、コンピュータにより実行されるインストラクションとして、コンピュータ読み取り可能な非遷移有形記録媒体に記憶されていてもよい。

以上、実施形態、変形例に基づき本開示について説明してきたが、上記した発明の実施の形態は、本開示の理解を容易にするためのものであり、本開示を限定するものではない。本開示は、その趣旨並びに特許請求の範囲を逸脱することなく、変更、改良され得ると共に、本開示にはその等価物が含まれる。たとえば、発明の概要の欄に記載した各形態中の技術的特徴に対応する実施形態、変形例中の技術的特徴は、上述の課題の一部又は全部を解決するために、あるいは、上述の効果の一部又は全部を達成するために、適宜、差し替えや、組み合わせを行うことが可能である。また、その技術的特徴が本明細書中に必須なものとして説明されていなければ、適宜、削除することが可能である。

ＨＩ…背景光画像、ＫＩ…距離画像、１０…姿勢・位置検出装置、２１…画像取得部、２３…姿勢・位置検出部、５０…検出器、１００，１００ａ…車両

Claims (11)

1. 車両（１００、１００ａ）に搭載されて用いられる検出器（５０）の姿勢・位置検出装置（１０）であって、
   前記検出器から照射された照射光の照射範囲内の対象物までの距離を示す距離画像（ＫＩ）と、環境光の受光強度を示す背景光画像（ＨＩ）であって、解像度が前記距離画像の解像度以上である背景光画像と、を異なるタイミングにて前記検出器から取得する画像取得部（２１）と、
   前記距離画像と前記背景光画像とを用いて、前記検出器の姿勢・位置を検出する姿勢・位置検出部（２３）と、
   を備える、検出器の姿勢・位置検出装置。
2. 請求項１に記載の検出器の姿勢・位置検出装置であって、
   前記背景光画像の解像度は、前記距離画像の解像度よりも高い、
   検出器の姿勢・位置検出装置。
3. 請求項１または請求項２に記載の検出器の姿勢・位置検出装置であって、
   前記対象物には、前記姿勢・位置の検出に用いられるターゲット（ＴＧ）であって、平面上に表された予め定められた検出用パターン（ＫＰ）を有するターゲットが含まれ、
   前記姿勢・位置検出部は、
   前記背景光画像における前記ターゲット上の特徴点（Ｆｐ）の画素位置を検出し、
   前記距離画像における前記特徴点の前記画素位置に対応する画素である対応画素（Ｇｔ）の画素値から対応点３次元位置を取得し、
   前記対応点３次元位置を用いて、前記検出器の姿勢・位置を検出する、
   を備える、検出器の姿勢・位置検出装置。
4. 請求項３に記載の検出器の姿勢・位置検出装置であって、
   前記姿勢・位置検出部は、
   前記距離画像における前記対応画素の周囲の画素である周囲画素（Ｇｓ）の各画素値から周囲点３次元位置を取得し、
   前記周囲点３次元位置を補間することによって前記対応点３次元位置を算出する、
   検出器の姿勢・位置検出装置。
5. 請求項３または請求項４に記載の検出器の姿勢・位置検出装置であって、
   前記ターゲットは、色と、材質とのうちの少なくとも一方が互いに異なる複数のターゲットを含む、
   検出器の姿勢・位置検出装置。
6. 請求項３から請求項５までのいずれか一項に記載の検出器の姿勢・位置検出装置であって、
   前記ターゲットは、前記車両の現在位置の周辺に存在する予め定められた種類の道路構造物であり、
   前記姿勢・位置検出部は、前記道路構造物上の予め定められた特徴点を用いて、前記検出器の姿勢・位置を検出する、
   検出器の姿勢・位置検出装置。
7. 請求項１から請求項６までのいずれか一項に記載の検出器の姿勢・位置検出装置であって、
   検出された前記検出器の姿勢・位置を用いて、前記検出器を較正する較正部（２５）を、さらに備える、
   検出器の姿勢・位置検出装置。
8. 請求項１から請求項７までのいずれか一項に記載の検出器の姿勢・位置検出装置であって、
   前記検出器は、
   前記照射光の反射光を含む入射光を受光可能な複数の受光素子を有し、
   前記受光素子（５６）は、受光要素（５８）の受光状態に応じた受光値を出力するシングルフォトンアバランシェダイオードである、
   検出器の姿勢・位置検出装置。
9. 請求項１から請求項８までのいずれか一項に記載の検出器の姿勢・位置検出装置であって、
   前記検出器は、ライダーである、
   検出器の姿勢・位置検出装置。
10. 車両（１００、１００ａ）に搭載されて用いられる検出器（５０）の姿勢・位置検出方法であって、
    前記検出器から照射された照射光の照射範囲内の対象物までの距離を示す距離画像（ＫＩ）と、環境光の受光強度を示す背景光画像（ＨＩ）であって、解像度が前記距離画像の解像度以上である背景光画像と、を異なるタイミングにて前記検出器から取得し、
    前記距離画像と前記背景光画像とを用いて、前記検出器の姿勢・位置を検出する、
    検出器の姿勢・位置検出方法。
11. 請求項１０に記載の検出器の姿勢・位置検出方法であって、
    前記背景光画像の解像度は、前記距離画像の解像度よりも高い、
    検出器の姿勢・位置検出方法。

Images