JP7764764B2 - センサ較正装置、および、センサ較正方法 - Google Patents

Description

本発明は、センサ較正装置、および、センサ較正方法に関する。

従来から、センサの姿勢や位置を較正するセンサ較正装置が知られている（例えば、特許文献１）。センサ較正装置は、センサの検出範囲内に基準を設定し、基準に対する物体の位置を計測することで、センサの位置姿勢パラメータを較正する。

特許５３１１３６５号公報

しかしながら、センサの設置環境によっては、基準に対する物体の位置を計測することが困難な場合がある。この場合、センサの位置姿勢パラメータを較正するためには、物体の位置に関する情報を多く取得する必要があり、時間がかかる。

本発明は、上述した課題を解決するためになされたものであり、センサ較正装置において、センサの位置と姿勢を簡便に較正することができる技術を提供することを目的とする。

本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態として実現することが可能である。

（１）本発明の一形態によれば、センサの位置および姿勢を較正するために用いられるセンサ較正装置が提供される。このセンサ較正装置は、基準直線を設定する基準直線設定部と、前記基準直線上に、三次元空間中を移動する物体の滞在の中心となる基準点を設定する基準点設定部と、前記センサの暫定的な位置および姿勢を表すパラメータとして予め設定されている位置姿勢パラメータと、三次元空間中の物体を検出した前記センサからの出力と、を用いて、前記物体の位置を計測する位置計測部と、前記位置計測部による、前記基準直線上を移動した前記物体の各時刻における位置計測結果を計測点集合として記憶する記憶部と、前記計測点集合における計測点の密度分布に基づいて、推定滞在点を算出する滞在点算出部と、前記記憶部に記憶されている前記計測点集合を用いて、前記計測点集合の近似直線を作成する直線近似部と、前記基準直線と、前記基準点と、前記近似直線と、前記推定滞在点とを用いて、前記位置姿勢パラメータを較正するパラメータ較正部と、を備え、前記パラメータ較正部は、前記推定滞在点が前記基準点に近づくように前記近似直線を並進させることで、位置パラメータを較正し、前記近似直線が前記基準直線に近づくように前記近似直線を回転させることで、姿勢パラメータを較正する。

この構成によれば、基準直線と基準直線上に設定される基準点とのそれぞれに対して、基準直線上を移動した物体の各時刻における位置計測結果から、近似直線を作成し、滞在推定点を算出する。パラメータ較正部は、基準直線に近づくように近似直線を回転させることで、センサの姿勢パラメータを較正することができる。また、滞在推定点は、物体の各時刻における位置計測結果を組み合わせた計測点集合における計測点の密度分布に基づいて算出されており、基準点を目印にして移動する物体の滞在の中心となっている。これにより、パラメータ較正部は、算出される推定滞在点が基準点に近づくように近似直線を並進させることで、位置パラメータを較正することができる。これにより、センサの検出範囲内で、基準点を滞在の中心として物体が基準直線上を移動することで、センサの位置姿勢パラメータを較正することができる。したがって、位置計測部による物体の位置計測結果が少なくてもセンサの位置姿勢パラメータを較正することができるため、短時間で位置姿勢パラメータを較正することができる。

（２）上記形態のセンサ較正装置において、前記基準点設定部は、複数の前記基準点を前記基準直線上に設定し、設定した複数の前記基準点のうち、前記物体が最も滞在する基準点を代表基準点に設定し、前記滞在点算出部は、前記計測点集合における計測点の密度分布に基づいて、複数の前記推定滞在点を算出し、複数の前記滞推定在点のうち、前記計測点集合における計測点の密度分布が最も高い推定滞在点を推定最長滞在点に設定し、前記パラメータ較正部は、前記推定最長滞在点が前記代表基準点に近づくように前記近似直線を並進させることで、位置パラメータを較正してもよい。この構成によれば、基準点設定部は、基準直線上に複数の基準点を設定するとき、物体が最も滞在する基準点を代表基準点に設定する。滞在点算出部は、計測点集合における計測点の密度分布に基づいて複数の推定滞在点を算出するとき、計測点の密度分布が最も高い推定滞在点を推定最長滞在点に設定する。これにより、複数の基準点が設定できる場合でも、代表基準点と推定最長滞在点とを設定することで、代表基準点と推定最長滞在点とが近づくように近似直線を並進させて、センサの位置パラメータを較正することができる。

（３）上記形態のセンサ較正装置において、前記直線近似部は、前記推定最長滞在点に対して主成分分析をおこなうことで、前記計測点集合の近似直線を作成してもよい。この構成によれば、近似直線は、推定最長滞在点に対して行われる主成分分析によって作成される。これにより、近似直線は、推定最長滞在点を通るため、近似直線を並進させることで代表基準点と推定最長滞在点とを重ねることができ、かつ、近似直線を回転させることで近似直線を基準直線に重ねることができる。したがって、センサの位置姿勢パラメータを確実に較正することができる。

（４）上記形態のセンサ較正装置において、前記基準点設定部は、複数の前記基準点を前記基準直線上に設定し、前記滞在点算出部は、前記基準点設定部が設定する前記基準点の数と同じ数の前記推定滞在点を算出し、前記パラメータ較正部は、複数の前記推定滞在点のそれぞれが、対応する複数の前記基準点のそれぞれに近づくように前記近似直線を並進させることで、位置パラメータを較正してもよい。この構成によれば、基準点設定部は、基準直線上に複数の基準点を設定する。滞在点算出部は、計測点集合における計測点の密度分布に基づいて複数の推定滞在点を算出する。パラメータ較正部は、複数の推定滞在点のそれぞれが複数の基準点のそれぞれに近くなるように、近似直線を並進させて、位置パラメータを較正する。これにより、１つの基準点に対して１つの推定滞在点を近づけるように近似直線を並進させる場合に比べ、センサの位置姿勢パラメータの較正精度を向上させることができる。

（５）上記形態のセンサ較正装置において、前記パラメータ較正部は、複数の前記基準点と、複数の前記基準点のそれぞれに対応する複数の前記推定滞在点との間の距離の絶対値の総和が最小となるように前記近似直線を並進させることで、位置パラメータを較正してもよい。この構成によれば、パラメータ較正部は、外れ値の影響を受けにくい最小絶対値法を用いて、位置パラメータを較正する。これにより、センサの位置パラメータの較正精度をさらに向上させることができる。

（６）上記形態のセンサ較正装置において、前記センサ較正装置は、異なった場所に設置されている２つのセンサのそれぞれに接続されており、前記基準直線設定部は、前記２つのセンサのそれぞれについて、互いに交差する基準直線を設定し、前記基準点設定部は、２つの前記基準直線のそれぞれに基準点を設定し、前記直線近似部は、交差する２つの前記基準直線と２つの前記基準直線の交点とから形成される基準折れ線における前記基準点と前記交点との間の距離に関する情報を取得し、前記基準点と前記交点との間の距離に関する情報を用いて、前記計測点集合の近似直線を作成してもよい。この構成によれば、直線近似部は、２つのセンサのそれぞれについて設定されている２本の基準直線から形成される基準折れ線を設定する。直線近似部は、２本の基準直線のそれぞれについて、２本の基準直線の交点から２本の基準直線上のそれぞれの基準点までの距離を取得し、基準点の位置と、基準点から交点までの距離とを用いて、センサの位置姿勢パラメータを較正する。これにより、近似直線を基準点と交点との２つの点で決定することができるため、近似直線の精度を向上させることができる。したがって、センサの位置姿勢パラメータの較正精度を向上させることができる。

（７）本発明の別の形態によれば、センサの位置および姿勢を較正するために用いられるセンサ較正方法が提供される。センサ較正方法は、基準直線を設定する工程と、前記基準直線上に、三次元空間中を移動する物体の滞在の中心となる基準点を設定する工程と、前記センサの暫定的な位置および姿勢を表すパラメータとして予め設定されている位置姿勢パラメータと、三次元空間中の物体を検出した前記センサからの出力と、を用いて、前記物体の位置を計測する工程と、前記位置計測部による、前記基準直線上を移動した前記物体の各時刻における位置計測結果を計測点集合として記憶する工程と、前記計測点集合における計測点の密度分布に基づいて、推定滞在点を算出する工程と、記憶されている前記計測点集合を用いて、前記計測点集合の近似直線を作成する工程と、前記基準直線と、前記基準点と、前記近似直線と、前記推定滞在点とを用いて、前記位置姿勢パラメータを較正する較正工程と、を備え、前記較正工程では、前記推定滞在点が前記基準点に近づくように前記近似直線を並進させることで、位置パラメータを較正し、前記近似直線が前記基準直線に近づくように前記近似直線を回転させることで、姿勢パラメータを較正する。この構成によれば、センサ較正方法では、基準直線に対して、基準直線上を移動した物体の各時刻における位置計測結果から、近似直線を作成する。センサ較正方法では、基準点に対して、物体の各時刻における位置計測結果を組み合わせた計測点集合における計測点の密度分布に基づく滞在推定点を算出する。これにより、センサの検出範囲内で、基準点を滞在の中心として、物体が基準直線上を移動するだけで、センサの位置姿勢パラメータを較正することができるため、短時間で位置姿勢パラメータを較正することができる。

なお、本発明は、種々の態様で実現することが可能であり、例えば、センサ較正装置を含むシステム、これら装置およびシステムの制御方法、これら装置およびシステムにおいてセンサ較正方法を実行させるコンピュータプログラム、そのコンピュータプログラムを配布するためのサーバ装置、そのコンピュータプログラムを記憶した一時的でない記憶媒体等の形態で実現することができる。

第１実施形態のセンサ較正装置の概略構成を示す模式図である。 センサ較正方法を説明するフローチャートである。 基準設定工程を説明する図である。 滞在点算出工程を説明する図である。 パラメータ較正工程を説明する第１の図である。 パラメータ較正工程を説明する第２の図である。 パラメータ較正工程を説明する第３の図である。 基準直線と近似直線との重ね方を説明する図である。 第１の比較例の近似直線の作成方法を説明する図である。 第２の比較例の近似直線の作成方法を説明する第１の図である。 第２の比較例の近似直線の作成方法を説明する第２の図である。 第１実施形態の近似直線の作成方法を説明する図である。 第２実施形態のセンサ較正方法を説明する図である。 第３実施形態のセンサ較正装置の概略構成を示す模式図である。 ２つの単眼カメラの位置関係を説明する図である。

＜第１実施形態＞
図１は、第１実施形態のセンサ較正装置１の概略構成を示す模式図である。本実施形態のセンサ較正装置１は、１か所に固定されて使用されるセンサを較正する装置である。センサ較正装置１は、例えば、イベントの開設などに伴って臨時的に設置されるセンサの位置を表す位置パラメータと姿勢を表す姿勢パラメータとを、現場において修正するときに用いられる。本実施形態のセンサ較正装置１は、センサとしての単眼カメラ５を１台較正する。単眼カメラ５は、例えば、車両が走行する道路の近傍に設置され、道路上を走行する車両や、道路脇の歩行者６（図１参照）などを連続的に撮像し、撮像した画像（多次元情報）を出力する。センサ較正装置１は、計測器１０と、較正処理器２０と、を備える。なお、本実施形態では、センサは、単眼カメラ５としたが、センサの種類は、これに限定されず、ステレオカメラ、レーダ、ＬｉＤＡＲ（レーザレーダ）、ソナーなどであってもよい。

計測器１０は、位置計測部１１と、基準設定部１２と、を備える。計測器１０は、単眼カメラ５と電気的に接続されており、単眼カメラ５によって撮像される画像が入力される。

位置計測部１１は、単眼カメラ５が撮像する画像に含まれる物体（歩行者６）の位置を計測する。位置計測部１１には、単眼カメラ５の暫定的な位置および姿勢を表すパラメータとして予め設定されている位置パラメータと姿勢パラメータとの両方を含む、暫定的な位置姿勢パラメータが入力されている。位置計測部１１は、この位置姿勢パラメータと、単眼カメラ５が撮像した画像から、移動している歩行者６の位置を計測する。位置計測部１１は、計測結果と、暫定的な位置姿勢パラメータと、を電気的に接続されている較正処理器２０に出力する。

基準設定部１２は、単眼カメラ５の出力情報である撮像した画像上に、基準直線と基準点を設定する。基準直線は、任意に設定可能な直線であって、具体的には、単眼カメラ５によって撮像された画像に含まれる、歩道上の視覚障害者誘導用ブロックの列や、歩道上を歩行者レーンと自転車レーンとに分ける白線など、直線状に配置されている物体を基準にして、画像上に設定される。基準点は、単眼カメラ５が撮像した画像において基準直線上に設定される点であり、歩行者６の滞在の中心として予め設定された対象物の位置を示す点である。具体的には、単眼カメラ５の画像に含まれる、例えば、バス停の標識が固定されている位置や、視覚障害者誘導用ブロックのうちの警告用のブロックが設置されている位置など、歩行者から見て、外観上認識しやすい物体が設置されている位置に対応する画像上に設定される。なお、基準直線の設定の基準となる対象物や、基準点の設定のために予め設定される対象物は、これらに限定されない。例えば、基準直線の設定の基準となる対象物は、道路上の車線を区別する白線や、車道と歩道とを区別する縁石などであってもよいし、基準点のために予め設定される対象物は、歩道上のベンチやゴミ箱などであってもよい。

較正処理器２０は、記憶部２１と、ＣＰＵ２２と、を備える。較正処理器２０は、位置計測部１１によって計測された物体の位置計測結果と、計測における暫定的な位置姿勢パラメータと、基準設定部１２によって設定された基準直線とを用いて、単眼カメラ５における位置姿勢パラメータを較正する。較正後の位置姿勢パラメータは、較正処理器２０によって、電気的に接続されている計測器１０の位置計測部１１に出力される。

記憶部２１は、ハードディスク、フラッシュメモリ、メモリカードなどで構成される。記憶部２１は、位置計測部１１が計測した、基準直線上を移動した物体の各時刻における位置計測結果を計測点集合として記憶する。また、記憶部２１は、ＣＰＵ２２における各種演算結果を記憶する。記憶部２１は、これらの記憶した情報をＣＰＵ２２に適宜出力する。

ＣＰＵ２２は、ＲＯＭに格納されているコンピュータプログラムをＲＡＭに展開して実行することにより、計測器１０も含むセンサ較正装置１の各部を制御する。そのほか、ＣＰＵ２２は、滞在点算出部２２ａと、直線近似部２２ｂと、パラメータ較正部２２ｃとして機能し、位置計測部１１が計測した移動する物体の位置計測結果から、推定滞在点を算出するとともに近似直線を作成し、推定滞在点と基準点との関係、および、近似直線と基準直線との関係から、位置姿勢パラメータを較正する。

滞在点算出部２２ａは、記憶部２１に記憶されている計測点集合における計測点の密度分布に基づいて、推定滞在点を算出する。具体的には、滞在点算出部２２ａは、記憶部２１に記憶されている計測点集合のうち、計測点の密度分布が比較的高い点を算出し、この点を推定滞在点に設定する。滞在点算出部２２ａの詳細な機能は、後述する。

直線近似部２２ｂは、記憶部２１に記憶されている、物体の位置計測結果の計測点集合を用いて、計測点集合の近似直線を作成する。直線近似部２２ｂの詳細な機能は、後述する。

パラメータ較正部２２ｃは、直線近似部２２ｂが作成した近似直線と、基準直線と、暫定的な位置姿勢パラメータと、を用いて、単眼カメラ５の位置姿勢パラメータを較正する。パラメータ較正部２２ｃの詳細な機能は、後述する。

図２は、センサ較正方法を説明するフローチャートである。次に、本実施形態のセンサ較正方法について説明する。本実施形態のセンサ較正方法は、例えば、単眼カメラ５を所定の場所に組み付けた際、単眼カメラ５の組み付け位置や単眼カメラ５の光軸の方向に影響する姿勢を確認し、単眼カメラ５の組み付けにおいてこれらを修正する作業（現地修正作業）として、センサ較正装置１によって行われる。このような現地修正作業は、誤検出（単眼カメラ５の検出可能範囲内に計測対象の物体が存在しないのに「存在する」とすること）や未検出（単眼カメラ５の検出可能範囲内に計測対象の物体が存在するのに「存在しない」とすること）を減らすために行われる。したがって、単眼カメラ５の組み付け作業後に、現場での即応可能な較正が必要となる。なお、単眼カメラ５の位置姿勢そのものの測量や、キャリブレーションボードによる較正では、この現地修正作業に対して即応できない。また、測位衛星受信やジャイロスコープなどの他のセンサによる位置姿勢の推定は、静止時の姿勢推定が適切に行えないことから、単眼カメラ５には適していない。

最初に、基準直線と基準点を設定する（基準設定工程：ステップＳ１）。ステップＳ１では、基準設定部１２は、現場に直線状に配置されている対象物を位置計測部１１の座標系における基準直線として設定し、設定される基準直線上の基準点を設定する。本実施形態では、基準設定部１２は、歩道上の視覚障害者誘導用ブロックの列７を基準直線とする（図１参照）。基準設定部１２が設定する基準点は、視覚障害者誘導用ブロックの列７に沿って移動する歩行者６にとって目印となるものが望ましい。本実施形態では、基準点は、視覚障害者誘導用ブロックの列７に沿って設置されている標識８が固定されている場所８１（図１参照）を含む図示しない複数の点とし、標識８が固定されている場所（以下、「標識の場所」という）８１を代表基準点とする。なお、基準直線とする対象、および、基準点とする対象は、これらに限定されない。

図３は、基準設定工程を説明する図である。図３には、単眼カメラ５が撮像する画像の一部を示している。基準設定部１２は、基準設定工程において、図３に示す画像の座標系のｘ軸に基準直線Ｌｓを設定し、基準直線Ｌｓ上に、標識の場所８１に対応する代表基準点Ｐｓ０を設定する。

次に、歩行者６の位置を計測する（位置計測工程：ステップＳ２）。ステップＳ２では、位置計測部１１は、あらかじめ設定されている単眼カメラ５の暫定的な位置姿勢パラメータと、単眼カメラ５の出力と、を用いて、視覚障害者誘導用ブロックの列７上を移動する歩行者６の位置を計測する。本実施形態では、歩行者６は、視覚障害者誘導用ブロックの列７上を歩くとき、代表基準点となっている標識の場所８１での滞在時間が他の位置での滞在時間に比べ長くなるように移動する。

ここで、本実施形態のセンサ較正方法における単眼カメラ５の暫定的な位置姿勢パラメータとは、ｘ軸、ｙ軸、ｚ軸のそれぞれの座標値ｓ_t（ベクトル表記）で表される位置パラメータ、並びに、方位角ａ_tおよび仰角ｅ_tで表される姿勢パラメータを指す。すなわち、位置計測部１１は、暫定的な位置パラメータｓ_t（ベクトル表記）と、姿勢パラメータｅ_t、ａ_tとが設定されている状態の単眼カメラ５が出力する歩行者６の位置を計測する。位置計測部１１は、歩行者６の位置計測結果を記憶部２１に出力する。なお、本文中に示す文字がベクトルである場合、対象文字の後に「（ベクトル表記）」と記載する。

次に、ステップＳ２において計測された、視覚障害者誘導用ブロックの列７上を移動した歩行者６の各時刻における位置計測結果を計測点集合として記憶させる（記憶工程：ステップＳ３）。記憶部２１は、位置計測部１１が出力する歩行者６の位置計測結果を、３次元空間での計測点集合として記憶する。また、記憶部２１は、基準設定部１２が設定した視覚障害者誘導用ブロックの列７の位置情報を、基準直線の位置情報として記憶する。

次に、ステップＳ３において記憶された歩行者６の位置計測結果から、推定滞在点を算出する（滞在点算出工程：ステップＳ４）。滞在点算出部２２ａは、記憶部２１に記憶されている計測点集合を用いて、歩行者６が比較的長く滞在した位置を推定滞在点として算出する。

図４は、滞在点算出工程を説明する図であって、計測点集合の一例を示す図である。図４は、記憶部２１に記憶された歩行者６の位置計測結果をプロットした２次元平面（ｘｙ平面）を示している。図４に示すｘｙ平面上の複数の丸印のそれぞれは、各時刻における歩行者６の位置計測結果を示しており、所定期間内の一定の時間間隔で計測された歩行者６の位置（計測点Ｍ）を示している。すなわち、記憶部２１には、歩行者６の各時刻における位置計測結果の集合（計測点集合Ｇｍ）が記憶されている。なお、図４には、説明の便宜上、計測点集合を２次元平面で示しており、基準設定工程において設定された基準直線Ｌｓと代表基準点Ｐｓ０が示されている。

ここで、ステップＳ４における推定滞在点の算出方法を説明する。本実施形態では、滞在点算出部２２ａは、計測点集合Ｇｍに含まれる計測点Ｍを距離が近い順に「クラスタ」と呼ばれるグループに集約する、公知の階層的クラスタリング最短距離法を用いて、推定滞在点を算出する。滞在点算出部２２ａは、クラスタ内最大距離が位置計測の分散を考慮した閾値、例えば、０．１ｍに達したときにクラスタリングを打ち切り、クラスタ内点数が多い順に基準点と同数になるまでクラスタを選択する。本実施形態では、各クラスタについて、クラスタ内各点の位置の中央値を推定滞在点Ｐｅとし、計測点集合Ｇｍにおいて選択された複数のクラスタのうち、計測点Ｍの分布密度が最も高い座標点を推定最長滞在点ＬＰｅとする。

次に、ステップＳ５において記憶部２１に記憶された計測点集合を用いて、計測点集合の近似直線を作成する（直線近似工程：ステップＳ５）。直線近似部２２ｂは、記憶部２１に記憶された歩行者６の位置計測結果の計測点集合を用いて、単眼カメラ５の位置パラメータｓ_t（ベクトル表記）と、姿勢パラメータｅ_t、ａ_tを較正するための近似直線を作成する。

直線近似工程では、直線近似部２２ｂは、最初に、主成分分析法とＲＡＮＳＡＣ法とを組み合わせた方法を用いて、近似直線候補を作成する。本実施形態では、直線近似部２２ｂは、近似直線が必ず推定最長滞在点を通過するように、複数の計測点の座標値の平均を用いる通常の主成分分析の代わりに、推定最長滞在点の座標値を用いて主成分分析を行い、近似直線候補を作成する。具体的には、複数の計測点のそれぞれの座標値行列を行列Ｐとし、推定最長滞在点の座標値を座標値ｐ０_t（ベクトル表記）とし、計測点の数をｎとする場合、以下の式（１）を固有値分解して、最大固有値と最大固有値に対応する第１固有ベクトルｆｅｖ（ベクトル表記）を求める。なお、式（１）に含まれるＴは、転置演算子である。

直線近似工程では、直線近似部２２ｂは、上述の主成分分析によって求めた最大固有値と第１固有ベクトルｆｅｖ（ベクトル表記）との組み合わせについて、最大固有値の平方根の大きさに基づいて、採用するか否かを判定する。直線近似部２２ｂは、最大固有値の平方根が、基準直線のうちの位置計測可能な範囲に含まれる線分長に基づいて設定する閾値以上である場合、作成した近似直線候補を採用し、閾値未満である場合、作成した近似直線候補を棄却する。具体的には、基準直線のうちの位置計測可能な範囲に含まれる線分長を線分長４ｍとし、閾値を、検出漏れを考慮した計測可能な範囲３．６ｍの半分の１．８ｍとした場合、１．８ｍ以上である場合には作成した近似直線候補を採用し、１．８ｍ未満である場合には作成した近似直線候補を棄却する。直線近似部２２ｂは、棄却されなかった回数が事前に設定した回数となるまで、上述の主成分分析による最大固有値と第１固有ベクトルｆｅｖ（ベクトル表記）との組み合わせの算出と、最大固有値の平方根の大きさに基づく判定を繰り返し行う。直線近似部２２ｂは、棄却されなかった回数が事前に設定した回数に達すると、棄却されなかった最大固有値と第１固有ベクトルｆｅｖ（ベクトル表記）との組み合わせのうち、近似誤差が最小となる直線を近似直線に選択する。直線近似部２２ｂは、選択された近似直線の最大固有値と第１固有ベクトルｆｅｖ（ベクトル表記）と、を記憶部２１に記憶させる。

図２に戻り、ステップＳ５の次に、ステップＳ１において設定された基準直線と、ステップＳ５において選定された近似直線とを用いて、単眼カメラ５の位置パラメータおよび姿勢パラメータを較正する（パラメータ較正工程：ステップＳ６）。本実施形態では、パラメータ較正部２２ｃは、姿勢パラメータａ、ｅを較正してから、位置パラメータｓを較正する。

図５は、パラメータ較正工程を説明する第１の図である。ここで、パラメータ較正工程での位置姿勢パラメータの較正方法について、説明する。図５（ａ）は、３次元空間中のｘｙ平面を示しており、複数の計測点Ｍのそれぞれをｘｙ平面に投影した投影点Ｍｘｙと、近似直線Ｌａをｘｙ平面に投影した投影近似直線Ｌａｘｙを示している。また、図５（ａ）には、ｘｙ平面における単眼カメラ５の暫定的な位置パラメータｓ_t（ベクトル表記）と、暫定的な姿勢を表す方位角ａ_tが示されている。ここでは、暫定的な方位角を、例えば、方位角ａ_t＝１５度と仮定する。図５（ｂ）は、３次元空間中のｘｚ平面を示しており、複数の計測点Ｍのそれぞれをｘｚ平面に投影した投影点Ｍｘｚと、近似直線Ｌａをｘｚ平面に投影した投影近似直線Ｌａｘｚを示している。また、図５（ｂ）には、ｘｚ平面における単眼カメラ５の暫定的な位置パラメータｓ_t（ベクトル表記）と、暫定的な姿勢を表す仰角ｅ_tが示されている。ここでは、暫定的な仰角を、例えば、仰角ｅ_t＝０度と仮定する。

基準直線Ｌｓは、ステップＳ１において、ｘ軸に設定されている（図５参照）。このとき、複数の計測点Ｍのそれぞれは、単眼カメラ５を中心とした座標系（単眼カメラ５が三次元空間の原点にあって、単眼カメラ５の正面の軸をｘ軸とする座標系であって、ｘ軸は、単眼カメラ５の光軸となる。）における点を、最初に、仰角ｅ_t、次に、方位角ａ_tで回転してから、単眼カメラ５の位置が座標値ｓ_t（ベクトル表記）となるように並進して、求められているとする。

また、ステップＳ５において、推定最長滞在点の座標値ｐ０_t（ベクトル表記）と、近似直線Ｌａの第１固有ベクトルｆｅｖ（ベクトル表記）は、記憶部２１に記憶されている。パラメータ較正部２２ｃは、これらの情報と、図５（ａ）、（ｂ）のそれぞれに示す２つの座標値ｐ０_t（ベクトル表記）と、座標値ｐ１_t（ベクトル表記）とを用いて、較正後の仰角ｅ_cおよび方位角ａ_cを導出する。ここで、座標値ｐ１_tは、座標値ｐ０_t（ベクトル表記）と近似直線Ｌａの第１固有ベクトルｆｅｖ（ベクトル表記）を用いて、次の式（２）で表される。
ここで、単眼カメラ５を中心とした座標系において、仰角ｅに応じて、原点を中心に計測点を回転させる回転行列を求める関数Ｅ（ｅ）、および、方位角ａに応じて、原点を中心に計測点を回転させる回転行列を求める関数Ａ（ａ）を導入する。仰角ｅは、ｙ軸（南から北への軸）を中心として、ｘ軸を南から見て反時計回りに回転させるため、式（３）のように定義される。

方位角ａは、ｚ軸（下から上への軸）を中心として、ｘ軸を上から見て反時計回りに回転させるため、式（４）のように定義される。

導出したい較正後の仰角ｅ_cおよび方位角ａ_cは、暫定的な位置姿勢パラメータの影響を打ち消したうえで、あらためて、仰角および方位角で回転したときに、投影近似直線Ｌａｘｙ、Ｌａｘｚが、基準直線Ｌｓであるｘ軸と平行になる仰角と方位角である。このとき、座標値ｐ０_t（ベクトル表記）の姿勢パラメータのみを較正した較正後の座標値ｐ０_p（ベクトル表記）と、座標値ｐ１_t（ベクトル表記）の姿勢パラメータのみを較正した較正後の座標値ｐ１_p（ベクトル表記）との差が、ｙ成分およびｚ成分について、同時に０となる。較正後の座標値ｐ０_p（ベクトル表記）は、式（５）で表され、較正後の座標値ｐ１_p（ベクトル表記）は、式（６）で表される。

較正後の仰角ｅ_cおよび方位角ａ_cを導出するには、以下に示す式（７）および式（８）からなる連立方程式を仰角差Δｅ、および、方位角ａ_cについて解く。式（７）は、ｙ成分に関する式であり、式（８）がｚ成分に関する式である。仰角差Δｅは（ｅ_c－ｅ_t）であり、仰角ｅ_tは、暫定仰角として既知の値であるため、仰角差Δｅが求まれば仰角ｅ_tを求めることができる。

式（７）および式（８）からなる連立方程式を解くと、式（９）および式（１０）に示すように、方位角ａ_c、および、仰角差Δｅの式が導出される。これらの式を用いて、較正後の方位角ａ_cおよび仰角ｅ_cを算出する。

図６は、パラメータ較正工程を説明する第２の図である。図６（ａ）、（ｂ）に示す図は、図５（ａ）（ｂ）のそれぞれに対応する図であって、式（９）および式（１０）を用いて算出された較正後の方位角ａ_cおよび仰角ｅ_cを用いて、投影近似直線Ｌａｘｙ、Ｌｘｘｚを修正したものである。図６に示すように、投影近似直線Ｌａｘｙ、Ｌａｘｚのそれぞれが、基準直線Ｌｓに平行となっている。具体的な数値で示すと、方位角ａは、１５度から６．１度となるように較正され、仰角ｅは、０度からマイナス３度に較正されている。

図７は、パラメータ較正工程を説明する第３の図である。次に、パラメータ較正部２２ｃは、位置パラメータを較正する。位置パラメータの較正では、姿勢パラメータのみが較正された較正後の座標値ｐ０_pのｙ成分と、ｚ成分とがそれぞれ０になるように、単眼カメラ５を並進させる。例えば、方位角ａのみが較正された図５（ａ）については、投影近似直線Ｌａｘｙをｙ軸のマイナスの方向に移動させて、基準直線Ｌｓと一致させる（図５（ａ）参照）。また、仰角ｅのみが構成された図６（ｂ）については、投影近似直線Ｌａｘｚをｚ軸のプラスの方向に移動させて、基準直線Ｌｓと一致させる（図７（ａ）参照）。これにより、近似直線Ｌａは、基準直線Ｌｓであるｘ軸と一致する。すなわち、並進後の単眼カメラ５の位置ｓ_cは、式（１１）で示すことができる。

図８は、本実施形態のセンサ較正方法における基準直線と近似直線との重ね方を説明する図である。ここで、上述のセンサ較正方法における位置姿勢パラメータの較正について、基準直線Ｌｓと、代表基準点Ｐｓ０と、推定最長滞在点ＬＰｅと、近似直線Ｌａ０との位置関係を用いて説明する。本実施形態のセンサ較正方法では、最初に、直線近似部２２ｂは、推定最長滞在点ＬＰｅを通る近似直線Ｌａ０を作成する。次に、パラメータ較正部２２ｃは、姿勢パラメータｅ_t、ａ_tを較正することで近似直線Ｌａ０を回転し（図８に示す矢印Ｒ１）、基準直線Ｌｓに平行な仮想直線Ｌａ１を作成する。次に、パラメータ較正部２２ｃは、仮想直線Ｌａ１と基準直線Ｌｓとが近づくように、仮想直線Ｌａ１を並進させて、仮想直線Ｌａ２を作成する（図８に示す矢印Ｔ１）。最後に、パラメータ較正部２２ｃは、仮想直線Ｌａ２上の推定最長滞在点ＬＰｅと代表基準点Ｐｓ０とが近づくように、仮想直線Ｌａ２を並進させる（図８に示す矢印Ｔ２）。このように、パラメータ較正部２２ｃは、近似直線Ｌａ０が基準直線Ｌｓに平行となるように近似直線Ｌａ０を回転させ、かつ、推定最長滞在点ＬＰｅが代表基準点Ｐｓ０に近づくように近似直線Ｌａ０を並進させることで、姿勢パラメータと位置パラメータとを較正する。なお、ここでの「近づける」は、代表基準点Ｐｓ０に推定最長滞在点ＬＰｅを近づけることで、推定最長滞在点ＬＰｅと代表基準点Ｐｓ０とを重ねる意味も含まれる。

パラメータ較正部２２ｃは、上述したように、ステップＳ６において、姿勢パラメータを較正してから、位置パラメータを較正する。パラメータ較正部２２ｃは、較正した位置姿勢パラメータを、計測器１０に向けて出力する。計測器１０では、入力された較正後の位置姿勢パラメータを用いて、これ以降の物体の位置測定を行う。

ここで、本実施形態のセンサ較正装置１の効果について、特に、直線近似部において作成される近似直線の精度に関して、複数の比較例との比較によって説明する。第１の比較例は、主成分分析法を用いた近似直線の作成例であり、第２の比較例は、主成分分析法とＲＡＮＳＡＣ法とを組み合わせた方法を用いた近似直線の作成例である。

図９は、第１の比較例での近似直線の作成方法を説明する図である。図９に示す丸印は、複数の計測点Ｍのそれぞれを示している。主成分分析法では、全ての計測点Ｍを用いて近似直線Ｌ０１を作成する。このため、近似直線Ｌ０１は、例えば、楕円Ｒ１０内の計測点Ｍｘのような外れ値の影響を受けため、傾きが大きくなりやすく、基準直線Ｌｓから乖離しやすい。

図１０は、第２の比較例での近似直線の作成方法を説明する第１の図である。第２の比較例では、最初に、第１ステップとして、全ての計測点Ｍのうちの一部をランダムに選択する。例えば、図１０に示すように、複数の太線丸印Ｍ２を選択する。この選択した計測点Ｍ２に対して、主成分分析を用いて暫定の近似直線Ｌ０２を求める。次に、第２ステップとして、選択されなかった計測点Ｍのうち、暫定の近似直線Ｌ０２との距離が閾値を超える点を外れ値とみなして、外れ値以外の点で近似直線と近似誤差を求める。

図１１は、第２の比較例での近似直線の作成方法を説明する第２の図である。第２の比較例では、第３のステップとして、第１のステップと第２のステップとを反復して、近似誤差が最小となる場合の直線を近似直線Ｌ１２とする。図１１に示すプロット点「×」は、第２ステップにおいて外れ値とみなした計測点Ｍである。近似直線Ｌ１２は、第１の比較例よりも傾きが大きくなり、基準直線Ｌｓからの乖離も大きくなっている。

図１２は、第１実施形態での近似直線の作成方法を説明する図である。図１４は、第１の比較例および第２の比較例で用いた計測点集合と同じ計測点集合を用いて、上述した近似直線の作成方法によって作成した近似直線Ｌ０３を示している。図１２に示すように、本実施形態によって作成した近似直線Ｌ０３は、第１の比較例の近似直線Ｌ０１に比べ傾きが小さく、基準直線Ｌｓに近い妥当な結果が得られることが明らかとなった。なお、図９と、図１１と、図１３とのそれぞれにおいて、符号Ｌ０１、Ｌ１２、Ｌ０３で示す実線は、それぞれの方法によって求められた近似直線の一部を表す線分であり、線分の長さは、各計測点Ｍを近似直線に正射影した点から求めた標準偏差に比例する。

以上説明した、本実施形態のセンサ較正装置１によれば、基準直線Ｌｓと基準直線Ｌｓ上に設定される代表基準点Ｐｓ０とのそれぞれに対して、基準直線Ｌｓ上を移動した歩行者６の各時刻における位置計測結果から、近似直線Ｌａを作成し、推定最長滞在点ＬＰｅを算出する。パラメータ較正部２２ｃは、基準直線Ｌｓに近づくように近似直線Ｌａを回転させることで、単眼カメラ５の姿勢パラメータａ、ｅを較正することができる。また、推定最長滞在点ＬＰｅは、歩行者６の各時刻における位置計測結果を組み合わせた計測点集合Ｇｍにおける計測点Ｍの密度分布に基づいて算出されており、代表基準点Ｐｓ０を目印にして移動する歩行者６の滞在の中心となっている。これにより、パラメータ較正部２２ｃは、算出される推定最長滞在点ＬＰｅが代表基準点Ｐｓ０に近づくように近似直線Ｌａを並進させることで、位置パラメータｓ_t（ベクトル表記）を較正することができる。これにより、単眼カメラ５の検出範囲内で、代表基準点Ｐｓ０を滞在の中心として、歩行者６が基準直線Ｌｓ上を移動することで、単眼カメラ５の位置姿勢パラメータを較正することができる。したがって、位置計測部１１による歩行者６の位置計測結果が少なくても単眼カメラ５の位置姿勢パラメータを較正することができるため、短時間で位置姿勢パラメータを較正することができる。

また、本実施形態のセンサ較正装置１によれば、基準設定部１２は、基準直線Ｌｓ上に複数の基準点を設定するとき、歩行者６が最も滞在する代表基準点Ｐｓ０を標識の場所８１に設定する。滞在点算出部２２ａは、計測点集合Ｇｍにおける計測点の密度分布に基づいて複数の推定滞在点Ｐｅを算出するとき、計測点の密度分布が最も高い推定滞在点Ｐｅを推定最長滞在点ＬＰｅに設定する。これにより、複数の基準点が設定できる場合でも、標識の場所８１と推定最長滞在点ＬＰｅとを設定することで、標識の場所８１と推定最長滞在点ＬＰｅとが近づくように近似直線Ｌａを並進させて、単眼カメラ５の位置パラメータを較正することができる。

また、本実施形態のセンサ較正装置１によれば、近似直線Ｌａは、推定最長滞在点ＬＰｅに対して行われる主成分分析によって作成される。これにより、近似直線Ｌａは、推定最長滞在点ＬＰｅを通るため、近似直線Ｌａを並進させることで標識の場所８１と推定最長滞在点ＬＰｅとを重ねることができ、かつ、近似直線Ｌａを回転させることで近似直線Ｌａを基準直線Ｌｓに重ねることができる。したがって、単眼カメラ５の位置姿勢パラメータを確実に較正することができる。

また、本実施形態のセンサ較正方法によれば、センサ較正方法では、基準直線Ｌｓに対して、基準直線Ｌｓ上を移動した歩行者６の各時刻における位置計測結果から、近似直線Ｌａを作成する。センサ較正方法では、代表基準点Ｐｓ０に対して、歩行者６の各時刻における位置計測結果を組み合わせた計測点集合Ｇｍにおける計測点の密度分布に基づく推定滞在点Ｐｅを算出する。これにより、単眼カメラ５の検出範囲内で、代表基準点Ｐｓ０を滞在の中心として、歩行者６が基準直線Ｌｓ上を移動するだけで、単眼カメラ５の位置姿勢パラメータを較正することができるため、短時間で位置姿勢パラメータを較正することができる。

＜第２実施形態＞
図１３は、第２実施形態のセンサ較正方法を説明する図である。第２実施形態のセンサ較正装置によるセンサ較正方法は、第１実施形態のセンサ較正方法（図１）と比較すると、滞在点算出工程で選択される複数の推定滞在点を用いて、センサの位置姿勢パラメータを較正する点が異なる。

第２実施形態のセンサ較正装置によるセンサ較正方法では、図１３に示すように、基準設定工程において、基準直線Ｌｓ上に、複数の基準点Ｐｓ１，Ｐｓ２，Ｐｓ３，Ｐｓ４を設定する。複数の基準点Ｐｓ１，Ｐｓ２，Ｐｓ３，Ｐｓ４は、第１実施形態と同様に、視覚障害者誘導用ブロックの列７に沿って移動する歩行者６にとって目印となるものが望ましく、視覚障害者誘導用ブロックの列７に含まれる警告ブロックや、道路上のマンホールなどであってもよい。位置計測工程では、視覚障害者誘導用ブロックの列７上を移動する歩行者６は、複数の基準点Ｐｓ１，Ｐｓ２，Ｐｓ３，Ｐｓ４のそれぞれにおいて、滞在時間が他の位置での滞在時間に比べ長くなるように移動する。

位置計測工程の次に、滞在点算出工程では、滞在点算出部２２ａは、記憶部２１に記憶されている計測点集合を用いて、歩行者６が比較的長く滞在した位置を推定滞在点として算出する。本実施形態では、基準設定工程において設定された基準点の数と同じ数のクラスタを、階層的クラスタリング最短距離法によって選択する。本実施形態では、各クラスタについて、クラスタの中央値を推定滞在点Ｐｅ１，Ｐｅ２，Ｐｅ３，Ｐｅ４とする。

パラメータ較正工程では、第１実施形態と同様に、姿勢パラメータを較正したのち、複数の基準点Ｐｓ１，Ｐｓ２，Ｐｓ３，Ｐｓ４のそれぞれに、推定滞在点Ｐｅ１，Ｐｅ２，Ｐｅ３，Ｐｅ４のそれぞれが近づくように、近似直線Ｌｐを並進させて、位置パラメータを較正する。本実施形態では、位置パラメータの較正は、最小二乗法よりも外れ値に強い最小絶対値法を用いて行う。具体的には、基準点Ｐｓ１，Ｐｓ２，Ｐｓ３，Ｐｓ４のそれぞれに対応する推定滞在点Ｐｅ１，Ｐｅ２，Ｐｅ３，Ｐｅ４のそれぞれと、基準点Ｐｓ１，Ｐｓ２，Ｐｓ３，Ｐｓ４との間の距離の絶対値の総和が最小となるように、近似直線Ｌｐを並進させる。

以上説明した、本実施形態のセンサ較正装置によれば、基準設定部１２は、基準直線Ｌｓ上に複数の基準点Ｐｓ１，Ｐｓ２，Ｐｓ３，Ｐｓ４を設定する。滞在点算出部２２ａは、計測点集合Ｇｍにおける計測点Ｍの密度分布に基づいて複数の推定滞在点Ｐｅ１，Ｐｅ２，Ｐｅ３，Ｐｅ４を算出する。パラメータ較正部２２ｃは、複数の推定滞在点Ｐｅ１，Ｐｅ２，Ｐｅ３，Ｐｅ４のそれぞれが複数の基準点Ｐｓ１，Ｐｓ２，Ｐｓ３，Ｐｓ４のそれぞれに近くなるように、近似直線Ｌａを並進させて、位置パラメータを較正する。これにより、１つの基準点に対して１つの推定滞在点を近づけて重ねるように近似直線を並進させる場合に比べ、単眼カメラ５の位置姿勢パラメータの較正精度を向上させることができる。

また、本実施形態のセンサ較正装置によれば、パラメータ較正部２２ｃは、外れ値の影響を受けにくい最小絶対値法を用いて、位置パラメータを較正する。これにより、単眼カメラ５の位置パラメータの較正精度をさらに向上させることができる。

＜第３実施形態＞
図１４は、第３実施形態のセンサ較正装置の概略構成を示す模式図である。第３実施形態のセンサ較正装置は、第１実施形態のセンサ較正装置（図１）と比較すると、２つの計測器を備えており、２つのセンサの位置姿勢パラメータを較正する点が異なる。

本実施形態のセンサ較正装置３は、２つの計測器１０ａ，１０ｂと、較正処理器３０と、を備える。本実施形態の２つの計測器１０ａ，１０ｂのそれぞれは、単眼カメラ５ａ，５ｂのそれぞれに接続されている。２つの計測器１０ａ，１０ｂと較正処理器３０とは、例えば、無線通信によって接続されている。較正処理器３０は、２つの計測器１０ａ，１０ｂのそれぞれの計測内容や設定内容を受信するとともに、較正後の位置姿勢パラメータを、２つの計測器１０ａ，１０ｂのそれぞれに出力することができる。

計測器１０ａは、位置計測部１１ａと、基準設定部１２ａと、を備える。計測器１０ａは、単眼カメラ５ａと電気的に接続されており、単眼カメラ５ａによって撮像される画像が入力される。位置計測部１１ａは、歩行者６ａの位置を計測する。基準設定部１２ａは、単眼カメラ５ａによって撮像された画像上に、基準直線７ａと基準直線７ａ上の基準点８１ａを設定する。計測器１０ｂは、位置計測部１１ｂと、基準設定部１２ｂと、を備える。計測器１０ｂは、単眼カメラ５ｂと電気的に接続されており、単眼カメラ５ｂによって撮像される画像が入力される。位置計測部１１ｂは、歩行者６ｂの位置を計測する。基準設定部１２ｂは、単眼カメラ５ｂによって撮像された画像上に、基準直線７ｂと基準直線７ｂ上の基準点８１ｂを設定する。

図１５は、２つの単眼カメラ５ａ、５ｂの位置関係を説明する図である。図１５には、例として、市街地９の地図を示している。センサ較正装置３によって構成される２つの単眼カメラ５ａ，５ｂは、例えば、市街地９の１つのブロック９ａにおいて、離れた場所に設置されている。本実施形態では、２つの単眼カメラ５ａ，５ｂのそれぞれに対応する２つの基準設定部１２ａ，１２ｂのそれぞれは、交差するように、基準直線７ａ，７ｂを設定する。ここで、基準直線７ａと基準直線７ｂとの交点を交点７ｃとする（図１５参照）。

較正処理器３０は、記憶部３１と、ＣＰＵ３２と、を備える。較正処理器３０は、計測器１０ａ，１０ｂのそれぞれによる２人の歩行者６ａ，６ｂの位置計測結果と、暫定的な位置姿勢パラメータと、基準設定部１２によって設定された基準直線７ａ，７ｂおよび基準点８１ａ，８１ｂとを用いて、単眼カメラ５ａ，５ｂの位置姿勢パラメータを較正する。

記憶部３１は、２つの位置計測部１１ａ，１１ｂのそれぞれが計測した、基準直線７ａ，７ｂ上を移動した歩行者６ａ，６ｂのそれぞれの各時刻における位置計測結果を計測点集合として記憶する。また、記憶部３１は、ＣＰＵ３２における各種演算結果を記憶し、これらの記憶した情報をＣＰＵ３２に適宜出力する。

ＣＰＵ３２は、滞在点算出部３２ａと、直線近似部３２ｂと、パラメータ較正部３２ｃとして機能し、２つの計測器１０ａ，１０ｂを含むセンサ較正装置３の各部を制御する。ＣＰＵ３２は、２つの位置計測部１１ａ，１１ｂが計測した歩行者６ａ，６ｂのそれぞれの位置計測結果から推定滞在点を算出するとともに近似直線を作成し、推定滞在点と基準点との関係、および、近似直線と基準直線との関係から、位置姿勢パラメータを較正する。

滞在点算出部３２ａは、第１実施形態の滞在点算出部２２ａと同様の機能を有している。滞在点算出部３２ａは、２つの位置計測部１１ａ，１１ｂのそれぞれによる位置計測結果である２つの計測点集合を用いて、推定滞在点を算出する。

直線近似部３２ｂは、候補作成部３２１ｂと、判定部３２２ｂと、選択部３２３ｂと、を備える。候補作成部３２１ｂは、２つの計測点集合のそれぞれについて、それぞれの基準点の座標値を用いて主成分分析を行い、近似直線候補を作成する。判定部３２２ｂは、候補作成部３２１ｂが作成した近似直線候補の組み合わせを採用するか否かを判定する。選択部３２３ｂは、判定部３２２ｂの判定によって採用された近似直線候補の組み合わせから、位置姿勢パラメータの較正に用いる近似直線の組み合わせを選択する。直線近似部３２ｂの詳細な機能は、後述する。

パラメータ較正部３２ｃは、第１実施形態のパラメータ較正部２２ｃと同様の機能を有している。パラメータ較正部３２ｅは、選択部３２ｄによって選択される近似直線と、基準直線と、暫定的な位置姿勢パラメータと、を用いて、位置姿勢パラメータを較正する。

次に、本実施形態のセンサ較正方法について説明する。本実施形態のセンサ較正方法では、最初に、基準設定部１２ａ，１２ｂは、２つの単眼カメラ５ａ，５ｂのそれぞれについて、基準直線７ａ，７ｂと、基準直線７ａ上の基準点８１ａとして標識８ａが固定されている場所８１ａと、基準直線７ｂ上の基準点８１ｂとして標識８ｂが固定されている場所８１ｂを設定する。本実施形態では、基準直線７ａと基準直線７ｂとは、図１５に示すように、交差するように設定される。単眼カメラ５ａの暫定的な位置姿勢パラメータは、基準直線７ａをｘ軸とし、基準点８１ａを原点とする三次元空間内に設定される。単眼カメラ５ｂの暫定的な位置姿勢パラメータは、基準直線７ｂをｘ軸とし、基準点８１ｂを原点とする三次元空間内に設定される。すなわち、基準直線７ａ，７ｂと基準点８１ａ，８１ｂは、異なる座標系で設定されている。

次に、位置計測部１１ａ，１１ｂは、基準直線７ａ，７ｂ上を移動する歩行者６ａ，６ｂのそれぞれの位置を計測し、記憶部３１は、歩行者６ａ，６ｂの各時刻における位置計測結果を計測点集合として記憶する。次に、滞在点算出部３２ａは、記憶部３１に記憶された歩行者６ａ，６ｂのそれぞれの位置計測結果から、それぞれの推定滞在点を第１実施形態と同様の方法によって算出する。

次に、候補作成部３２１ｂは、２つの単眼カメラ５ａ，５ｂのそれぞれの計測点集合のそれぞれについて、第１実施形態と同様に、基準点の座標値に対する主成分分析法とＲＡＮＳＡＣ法とを組み合わせた方法を用いて、計測点集合の近似直線候補を作成する。本実施形態では、候補作成部３２１ｂは、第１実施形態の直線近似工程と同様に、棄却されなかった回数が事前に設定した回数に達するまで、近似直線候補を算出する。候補作成部３２１ｂは、２つの単眼カメラ５ａ，５ｂのそれぞれに作成される近似直線候補を組み合わせた、近似直線候補の組み合わせを作成する。

次に、判定部３２２ｂは、最初に、基準直線７ａ，７ｂと基準点８１ａ，８１ｂとを共通の座標系、例えば、国土地理院地図の座標系に変換し、２つの基準直線７ａ，７ｂから形成される基準折れ線９ｃを設定する（図１５参照）。判定部３２２ｂは、基準折れ線９ｃを設定する際に、基準折れ線９ｃにおける２つの基準点８１ａ，８１ｂのそれぞれから基準直線７ａ，７ｂの交点７ｃまでの距離（以下、「計測距離」という）Ｄｍａ，Ｄｍｂに関する情報を取得する（図１５参照）。計測距離Ｄｍａ，Ｄｍｂは、例えば、ローラ距離計を用いて実際に計測した距離であってもよいし、地図から算出される距離であってもよい。

次に、判定部３２２ｂは、候補作成部３２１ｂによって作成された２つの単眼カメラ５ａ，５ｂのそれぞれの近似直線候補を基準折れ線９ｃと共通の座標系に変換し、変換後の推定滞在点と２つの近似直線候補の交点までの距離（以下、「推定距離」という）をそれぞれ算出する。判定部３２２ｂは、計測距離と推定距離との差の絶対値と、距離計測や基準折れ線を設定する手順を考慮した誤差（以下、「交点棄却閾値」という）とを比較する。判定部３２２ｂは、誤差が交点棄却閾値以下の場合、２つの近似直線候補の組み合わせを採用し、誤差が交点棄却閾値より大きい場合、２つの近似直線候補の組み合わせを棄却する。

例えば、候補作成部３２１ｂにおいて、単眼カメラ５ａにおける近似直線候補Ｌｐａ１（推定滞在点Ｐｅａ１）と、単眼カメラ５ｂにおける近似直線候補Ｌｐｂ１（推定滞在点Ｐｅｂ１）との組み合わせが作成されたとする。この近似直線候補の組み合わせ（Ｌｐａ１，Ｌｐｂ１）において、近似直線候補Ｌｐａ１と近似直線候補Ｌｐｂ１との交点から推定滞在点Ｐｅａ１までの距離を推定距離Ｄｅａとし、近似直線候補Ｌｐａ１と近似直線候補Ｌｐｂ１との交点から推定滞在点Ｐｅｂ１までの推定距離を推定距離Ｄｅｂとする。この場合、推定距離Ｄｅａと計測距離Ｄｍａとの差と、推定距離Ｄｅｂと計測距離Ｄｍｂとの差のいずれもが、交点棄却閾値以下である場合、近似直線候補の組み合わせ（Ｌｐａ１，Ｌｐｂ１）は採用される。一方、推定距離Ｄｅａが計測距離Ｄｍａとほぼ一致したとしても、推定距離Ｄｅｂと計測距離Ｄｍｂとの差が交点棄却閾値より大きい場合、近似直線候補の組み合わせ（Ｌａｐ１，Ｌｐｂ１）は、棄却される。なお、交点棄却閾値は、２つの計測距離がほとんど同じなら一律でもよい。また、交点棄却閾値は、計測距離に対する比率として設定してもよい。判定部３２２ｂでは、このようにして、候補作成部３２１ｂによって作成される複数の近似直線候補の組み合わせについて、採用または棄却の判定を行う。

次に、選択部３２３ｂは、判定部３２２ｂの判定によって採用された近似直線候補の組み合わせから、１つの組み合わせを選択する。本実施形態では、選択部３２３ｂは、採用された近似直線候補の組み合わせのうち、２つの近似直線候補のそれぞれの近似誤差の集約値が最小となる組合せを選択する。近似直線候補の誤差の集約値としては、単純平均を用いてもよいし、計測点数による加重平均を用いてもよい。

以上説明した、本実施形態のセンサ較正装置３によれば、直線近似部３２ｂは、２つの単眼カメラ５ａ，５ｂのそれぞれについて設定されている２本の基準直線７ａ，７ｂから形成される基準折れ線９ｃを設定する。直線近似部３２ｂは、２本の基準直線７ａ，７ｂのそれぞれについて、２本の基準直線７ａ，７ｂの交点７ｃから２本の基準直線７ａ，７ｂ上のそれぞれの基準点８１ａ，８１ｂまでの計測距離を取得する。また、直線近似部３２ｂは、推定滞在点と２つの近似直線候補の交点までの推定距離を取得し、基準点８１ａ，８１ｂの位置と、計測距離と、推定距離と、を用いて、単眼カメラ５ａ，５ｂの位置姿勢パラメータを較正する。これにより、基準直線７ａ，７ｂを基準点と交点との２つの点で決定することができるため、近似直線の精度を向上させることができる。したがって、センサの位置姿勢パラメータの較正精度を向上させることができる。

＜本実施形態の変形例＞
本発明は上記の実施形態に限られるものではなく、その要旨を逸脱しない範囲において種々の態様において実施することが可能であり、例えば次のような変形も可能である。

［変形例１］
上述の実施形態では、直線近似工程において、主成分分析法とＲＡＮＳＡＣ法とを組み合わせた方法を用いて、近似直線候補を作成するとした。しかしながら、近似直線候補の作成方法は、これに限定されない。主成分分析法のみで作成してもよい。

［変形例２］
第１実施形態では、パラメータ較正部２２ｃは、姿勢パラメータａ、ｅを較正してから、位置パラメータｓを較正するとした。しかしながら、単眼カメラ５の位置姿勢パラメータを較正する順番はこれに限定されない。近似直線を並進させて推定最長滞在点ＬＰｅと基準点Ｐｓとが重ねることで位置パラメータｓを較正してから、近似直線を回転させて近似直線Ｌａと基準直線Ｌｓとを重ねることで姿勢パラメータを較正してもよい。

［変形例３］
第１実施形態および第２実施形態では、センサ較正装置１は、１台の単眼カメラ５を較正するとした。しかしながら、「センサ」は、単眼カメラ５でなくてもよく、上述したように、ステレオカメラ、レーダ、ＬｉＤＡＲ（レーザレーダ）、ソナーなどであってもよい。２台の単眼カメラ５を構成する第３実施形態についても同様である。

［変形例４］
第２実施形態では、パラメータ較正部は、外れ値の影響を受けにくい最小絶対値法を用いて、複数の基準点に対して複数の滞在推定点を近づけることで、位置パラメータを較正するとした。パラメータ較正部による位置パラメータの較正方法は、これに限定されない。例えば、最小二乗法を用いて行ってもよい。

［変形例５］
第３実施形態では、選択部３２３ｂは、採用された近似直線候補の組み合わせのうち、２つの近似直線候補のそれぞれの近似誤差の集約値が最小となる組合せを選択するとした。しかしながら、選択部３２３ｂによる組み合わせの選択方法は、これに限定されない。例えば、計測点集合ごとに１つの近似直線候補を算出し、棄却されなければそれらの組み合わせを保持する。保持した組み合わせの数が事前に設定した回数に達したところで、近似誤差の集約値が最小となる組合せを選んでもよい。この方法では、棄却の条件によっては、近似直線の精度を向上させることができるため、近似直線候補の算出に必要な手間を少なくすることができる。

［変形例６］
第１実施形態および第２実施形態では、センサ較正装置１は、計測器１０と、較正処理器２０とを備えるとし、２つの機器から構成されるとした。しかしながら、センサ較正装置は、計測器と較正処理器とが一体となっていてもよいし、第３実施形態のように、異なる場所に設置されていてもよい。

以上、実施形態、変形例に基づき本態様について説明してきたが、上記した態様の実施の形態は、本態様の理解を容易にするためのものであり、本態様を限定するものではない。本態様は、その趣旨並びに特許請求の範囲を逸脱することなく、変更、改良され得るとともに、本態様にはその等価物が含まれる。また、その技術的特徴が本明細書中に必須なものとして説明されていなければ、適宜、削除することができる。

１，３…センサ較正装置
５，５ａ，５ｂ…単眼カメラ
６，６ａ，６ｂ…歩行者
１０，１０ａ，１０ｂ…計測器
１１，１１ａ，１１ｂ…位置計測部
１２，１２ａ，１２ｂ…基準設定部
２０，３０…較正処理器
２１，３１…記憶部
２２ａ，３２ａ…滞在点算出部
２２ｂ，３２ｂ…直線近似部
２２ｃ，３２ｃ…パラメータ較正部
Ｌａ…近似直線
Ｌｓ…基準直線
ＬＰｅ…推定最長滞在点
Ｐｓ０…代表基準点
Ｐｓ１，Ｐｓ２，Ｐｓ３，Ｐｓ４…基準点
Ｐｅ，Ｐｅ１，Ｐｅ２，Ｐｅ３，Ｐｅ４，Ｐｅａ１，Ｐｅｂ１…推定滞在点
ａ，ａ_c、ａ_t…方位角（姿勢パラメータ）
ｅ，ｅ_c、ｅ_t…仰角（姿勢パラメータ）
ｓ，ｓ_c、ｓ_t…座標値（位置パラメータ）

Claims (7)

1. センサの位置および姿勢を較正するために用いられるセンサ較正装置であって、
   基準直線を設定する基準直線設定部と、
   前記基準直線上に、三次元空間中を移動する物体の滞在の中心となる基準点を設定する基準点設定部と、
   前記センサの暫定的な位置および姿勢を表すパラメータとして予め設定されている位置姿勢パラメータと、三次元空間中の物体を検出した前記センサからの出力と、を用いて、前記物体の位置を計測する位置計測部と、
   前記位置計測部による、前記基準直線上を移動した前記物体の各時刻における位置計測結果を計測点集合として記憶する記憶部と、
   前記計測点集合における計測点の密度分布に基づいて、前記物体の推定滞在点を算出する滞在点算出部と、
   前記記憶部に記憶されている前記計測点集合を用いて、前記計測点集合の近似直線を作成する直線近似部と、
   前記基準直線と、前記基準点と、前記近似直線と、前記推定滞在点とを用いて、前記位置姿勢パラメータを較正するパラメータ較正部と、を備え、
   前記パラメータ較正部は、
   前記推定滞在点が前記基準点に近づくように前記近似直線を並進させることで、位置パラメータを較正し、
   前記近似直線が前記基準直線に近づくように前記近似直線を回転させることで、姿勢パラメータを較正する、
   センサ較正装置。
2. 請求項１に記載のセンサ較正装置であって、
   前記基準点設定部は、
   複数の前記基準点を前記基準直線上に設定し、
   設定した複数の前記基準点のうち、前記物体が最も滞在する基準点を代表基準点に設定し、
   前記滞在点算出部は、
   前記計測点集合における計測点の密度分布に基づいて、複数の前記推定滞在点を算出し、
   複数の前記推定滞在点のうち、前記計測点集合における計測点の密度分布が最も高い推定滞在点を推定最長滞在点に設定し、
   前記パラメータ較正部は、前記推定最長滞在点が前記代表基準点に近づくように前記近似直線を並進させることで、位置パラメータを較正する、
   センサ較正装置。
3. 請求項２に記載のセンサ較正装置であって、
   前記直線近似部は、前記推定最長滞在点に対して主成分分析をおこなうことで、前記計測点集合の近似直線を作成する、
   センサ較正装置。
4. 請求項１に記載のセンサ較正装置であって、
   前記基準点設定部は、複数の前記基準点を前記基準直線上に設定し、
   前記滞在点算出部は、前記基準点設定部が設定する前記基準点の数と同じ数の前記推定滞在点を算出し、
   前記パラメータ較正部は、複数の前記推定滞在点のそれぞれが、対応する複数の前記基準点のそれぞれに近づくように前記近似直線を並進させることで、位置パラメータを較正する、
   センサ較正装置。
5. 請求項４の記載のセンサ較正装置であって、
   前記パラメータ較正部は、複数の前記基準点と、複数の前記基準点のそれぞれに対応する複数の前記推定滞在点との間の距離の絶対値の総和が最小となるように前記近似直線を並進させることで、位置パラメータを較正する、
   センサ較正装置。
6. 請求項１から請求項５のいずれか一項に記載のセンサ較正装置であって、
   前記センサ較正装置は、異なった場所に設置されている２つのセンサのそれぞれに接続されており、
   前記基準直線設定部は、前記２つのセンサのそれぞれについて、互いに交差する基準直線を設定し、
   前記基準点設定部は、２つの前記基準直線のそれぞれに基準点を設定し、
   前記直線近似部は、
   交差する２つの前記基準直線と２つの前記基準直線の交点とから形成される基準折れ線における前記基準点と前記交点との間の距離に関する情報を取得し、
   前記基準点と前記交点との間の距離に関する情報を用いて、前記計測点集合の近似直線を作成する、
   センサ較正装置。
7. センサ較正装置を用いて、センサの位置および姿勢を較正するセンサ較正方法であって、
   基準直線を設定する工程と、
   前記基準直線上に、三次元空間中を移動する物体の滞在の中心となる基準点を設定する工程と、
   前記センサの暫定的な位置および姿勢を表すパラメータとして予め設定されている位置姿勢パラメータと、三次元空間中の物体を検出した前記センサからの出力と、を用いて、前記物体の位置を計測する工程と、
   前記物体の位置を計測する工程において計測した、前記基準直線上を移動した前記物体の各時刻における位置計測結果を計測点集合として記憶する工程と、
   前記計測点集合における計測点の密度分布に基づいて、前記物体の推定滞在点を算出する工程と、
   記憶されている前記計測点集合を用いて、前記計測点集合の近似直線を作成する工程と、
   前記基準直線と、前記基準点と、前記近似直線と、前記推定滞在点とを用いて、前記位置姿勢パラメータを較正する較正工程と、を備え、
   前記較正工程では、
   前記推定滞在点が前記基準点に近づくように前記近似直線を並進させることで、位置パラメータを較正し、
   前記近似直線が前記基準直線に近づくように前記近似直線を回転させることで、姿勢パラメータを較正する、
   センサ較正方法。