JP6989819B2 - データ補正装置、データ補正方法、および、データ補正プログラム - Google Patents

Description

本発明は、データを補正する装置、方法、および、プログラムに関する。

レーザーセンサを用いて、物体までの距離の測定や、物体の形状の測定が行われることがある。ここで、レーザーセンサには、スキャン方式のセンサと、一括照射方式のセンサがある。スキャン方式を採用するセンサでは時系列走査が行われるため、同時刻にスキャン範囲全体の測距はできない。一括照射でローリングシャッター方式の受光素子を用いて、測距や距離画像の生成が行われる場合も、スキャン方式と同様に、同時刻のスキャン範囲全体の測距はできない。（例えば、特許文献１など）。類似例として、ローリングシャッター方式の画像センサは同時刻にスキャン範囲全体の測距を行わないので、１フレームを取得する際に測距点によって露光時刻が異なる。従って、１フレームの撮像期間中に被写体の位置が変動すると、実際の被写体の形状とは異なる形状の撮像結果が得られてしまう。また、ローリングシャッター方式の画像センサが移動した場合にも、実際の被写体の形状とは異なる形状の撮像結果が得られてしまう。このため、画像センサの画像平面におけるピクセル位置と座標スペースにおける対応光線との間の関係に基づく校正データを用いて画像を修正する技術も提案されている（例えば、特許文献２など）。

一方、一括照射でグローバルシャッター方式を採用したセンサは同時刻にスキャン範囲全体の測距を行うので、グローバルシャッター方式のセンサを用いたシステムでは、被写体が移動しても、被写体の実際の形状を測距結果として得ることができる。例えば、周囲光が強い場合にも、周囲光による過剰な電荷を排除しながら信号光電荷を集積するグローバルシャッター方式のセンサも提案されている（例えば、非特許文献１など）。

特許第６０２５０８１号 特開２０１２－２５３７７８号公報

橋本裕介 外４名、「周囲光消去型距離画像センサ」、パナソニック電工技報、Vol.57第2号、P.4-9

グローバルシャッター方式のセンサを用いる場合、被写体が移動物体であっても、移動物体の全体を同一時刻に測距できるが、センサと移動物体の間に距離がある場合などは、照射光が拡散するために反射光が弱くなり、正確な測定が困難になる。しかし、照射光を一定以上強くすると、作業者の目に悪影響を及ぼしてしまうおそれがある。一方、スキャン方式のセンサを用いる場合、測距点あたりの照射光をグローバルシャッター方式のセンサよりも強くすることができるので、遠くにある物体も測距できる。しかし、スキャン方式のセンサでは時系列走査が行われるので、移動物体を測距すると、被写体の形状と異なる測距結果が得られてしまう。

本発明は、１つの側面では、時系列走査により取得した移動物体の測距結果を正しく補正することを目的とする。

ある１つの態様にかかるセンサデータ補正装置は、特定部、選択部、推定部、補正処理部を備える。特定部は、測距対象を時系列走査することにより測距する測距センサでの複数回の測距結果から選択された複数の測距点の各々に対して、前記測距センサより解像度の高いカメラによる前記測距対象物の複数回の撮像結果から、当該測距点に対応する画素を特定する。選択部は、前記複数回の測距結果のうちの第１の測距結果に含まれる測距点から補正対象として注目する第１の測距点を選択すると共に、前記第１の測距点の前記測距対象中の位置と同じ位置を測距した可能性のある第２の測距点を、前記複数回の測距結果のうちの前記第１の測距結果とは異なる第２の測距結果から選択する。推定部は、前記複数の測距点の各々に対応付けられた画素の特徴量を用いて、前記複数の測距点のうち前記測距対象物上の同じ表面位置の測距結果を推定することにより、前記測距対象物の移動軌跡を推定する。前記推定部は、前記第１の測距点に対応付けられた画素の特徴量と、前記第２の測距点に対応付けられた画素の特徴量の差が閾値を下回ると、前記移動軌跡は前記第１の測距点と前記第２の測距点を通ると推定する。補正処理部は、前記移動軌跡を用いて、前記測距センサでの前記測距対象物の測距結果を、特定の時刻での測距結果に補正する。

時系列走査により取得した移動物体の測距結果を正しく補正できる。

実施形態にかかるデータ補正方法の例を説明する図である。 データ補正装置の構成の例を説明する図である。 データ補正装置のハードウェア構成の例を説明する図である。 測距データおよび撮像データの例を説明する図である。 センサでの測距点とカメラによる撮像結果の関係の例を説明する図である。 測距点に対応する画素の特定例を説明する図である。 移動軌跡の例と補正処理の例を説明する図である。 第１の実施形態にかかる処理の例を説明するフローチャートである。 第１の実施形態にかかる処理の例を説明するフローチャートである。 フレーム数を制限する場合の処理の例を説明する図である。 遷移モデルの例を説明する図である。 第４の実施形態にかかるデータ補正装置の構成の例を説明する図である。 小平面の検索処理の例を説明する図である。 補間処理に使用するデータの選択方法の例を説明する図である。 第４の実施形態にかかる処理の例を説明するフローチャートである。

図１は、実施形態にかかるデータ補正方法の例を説明する図である。図１にかかる処理は、時系列走査型の測距センサと、カメラとにより生成されたデータを取得可能なデータ補正装置によって行われる。以下の説明では、カメラは、グローバルシャッター方式を採用している場合を例とするが、カメラでも、ローリングシャッター方式が採用されても良い。データ補正処理装置と測距センサとカメラとは、一つの装置内にともに搭載されてもよい。

データ補正装置は、時系列走査型の測距センサと、測距センサよりも解像度の高いカメラの各々で測距対象を複数回にわたって撮像する（ステップＳ１）。例えば、データ補正装置は、図１の例Ｅ中に示す期間Ｐ１とＰ２において、時系列走査型の測距センサで測距対象を測距したとする。以下、期間Ｐ１にフレームＦｓ１が測距され、期間Ｐ２にフレームＦｓ２が測距されたとする。さらに、データ補正装置は、カメラを用いて、図１の例Ｅ中に示す時刻Ｔ１とＴ２に測距対象を撮像したとする。以下、時刻Ｔ１にフレームＦｃ１が撮像され、期間Ｐ２にフレームＦｃ２が撮像されたとする。

その後、データ補正装置は、測距センサでの複数の測距結果から、複数の測距点を選択する（ステップＳ２）。例えば、データ補正装置は、フレームＦｓ１から測距点Ｐｏ１～Ｐｏ３、フレームＦｓ２から測距点Ｐｏ４を選択したとする。また、測距対象のうちで測距点Ｐｏ１～Ｐｏ３のいずれで観測された位置も、測距対象の移動により、期間Ｐ２には測距点Ｐｏ４として観測され得る範囲内であるとする。

ステップＳ３において、データ補正装置は、カメラでの複数の撮像結果から、ステップＳ２で選択した測距点に対応する画素を選択する。例えば、データ補正装置は、測距点Ｐｏ１に対応する画素としてフレームＦｃ１から画素Ｐｉ１を選択したとする。同様に、データ補正装置は、測距点Ｐｏ２に対応する画素としてフレームＦｃ１から画素Ｐｉ２を選択し、測距点Ｐｏ３に対応する画素としてフレームＦｃ１から画素Ｐｉ３を選択したとする。さらに、データ補正装置は、測距点Ｐｏ４に対応する画素としてフレームＦｃ２から画素Ｐｉ４を選択したとする。

次に、データ補正装置は、ステップＳ３で特定した画素（Ｐｉ１～Ｐｉ４）の特徴量を用いて、ステップＳ２で選択した測距点（Ｐｏ１～Ｐｏ４）のうちで、測距対象物上の同じ表面位置を測定した結果を推定する（ステップＳ４）。ここで使用される特徴量は、画素の輝度やＲＧＢ値など、画素に対応付けられた値であって測距対象の移動によって変動しない任意の要素である。例えば、特徴量として、画素のＲＧＢ値が使用されたとする。例えば、画素Ｐｉ１と画素Ｐｉ４のいずれも、ＲＧＢ値が（Ｒ，Ｇ，Ｂ）＝（０，０，０）であり、画素Ｐｉ２が（Ｒ，Ｇ，Ｂ）＝（０，０，２５５）、画素Ｐｉ３が（Ｒ，Ｇ，Ｂ）＝（２５５，０，０）であったとする。この場合、画素Ｐｉ１と画素Ｐｉ４は、ＲＧＢ値が一致しているので、測距対象のうちで時刻Ｔ１において画素Ｐｉ１として撮像された位置が、時刻Ｔ２において画素Ｐｉ４として撮像されたと推測することができる。画素Ｐｉ１は測距センサによる測距点Ｐｏ１に対応付けられており、画素Ｐｉ４は測距センサによる測距点Ｐｏ４に対応付けられている。このため、データ補正装置は、期間Ｐ１中での測距点Ｐｏ１と期間Ｐ２中での測距点Ｐｏ４が、測距対象のうちの同じ位置の測距結果であると推定する。

データ補正装置は、測距対象の同じ位置を測定した結果として推定した測距点の組合せから、測距対象の移動軌跡を推定する（ステップＳ５）。図１の例では、期間Ｐ１中の測距点Ｐｏ１の測距時刻に測距点Ｐｏ１に位置している点が、期間Ｐ２中の測距点Ｐｏ４の測距時刻に測距点Ｐｏ４の位置に移動したときに得られる時空間の移動軌跡を、測距対象の移動軌跡とする。

データ補正装置は、得られた移動軌跡を用いて、測距センサでの測距結果を、特定の時刻の測距結果に補正する（ステップＳ６）。例えば、データ補正装置は、フレームＦｓ１中の測距点Ｐｏ４の測距結果とステップＳ５で得られた移動軌跡に基づいて、測距点Ｐｏ４として測距された位置を、時刻Ｔ２などの他の時刻に測距された場合の位置に変換することにより、補正データを生成する。

このように、実施形態にかかるデータ補正方法によると、測距センサで得られた複数の測距点のうち、測距対象の同じ点に対する測距結果を、高解像度のカメラによって得られた画像を用いて特定することができる。このため、測距センサを用いた複数のフレームを撮像している間の測距対象物の移動の軌跡が推定でき、推定された軌跡を用いて、測距対象物の移動による歪みを正確に修正することができる。このため、時系列走査により取得した移動物体の測距結果が正しく補正される。

＜装置構成＞
図２は、データ補正装置２０の構成の例を説明する図である。データ補正装置２０は、３Ｄセンサ２１、カメラ２２と通信し、処理部３０、記憶部６０を備える。３Ｄセンサ２１とカメラ２２は、いずれも、測距対象物１０を測距および撮像可能な位置に設定されているものとする。以下の説明では、測距対象物１０は、移動や回転をする物体であるとする。３Ｄセンサ２１は、スキャン方式（時系列走査方式）での測距処理が可能なセンサである。以下の説明では、カメラ２２は、グローバルシャッター方式を採用しており、３Ｄセンサ２１よりも解像度が良いカメラであるとする。以下、各フレームの座標が３Ｄセンサ２１とカメラ２２のいずれを基準としているかを分かりやすくするために、３Ｄセンサ２１での測距に使用する座標系を「センサ座標系」、カメラ２２での撮像に使用する座標系を「カメラ座標系」と記載することがある。

記憶部６０は、処理プログラム６１、設定情報６３、取得データ７０を保持し、オプションとして遷移モデル６２を保持する。処理プログラム６１は、データ補正装置２０が備えるプロセッサ１０１（図３）に、データ補正装置２０としての動作処理を行わせるためのプログラムである。遷移モデル６２は、測距対象物１０の測距中に測距対象物１０に対して予測される位置の遷移のパターンを表わしたモデルである。遷移モデル６２は、測距対象物１０の動きが予測可能な場合に、記憶部６０に保持される。

設定情報６３は、３Ｄセンサ２１やカメラ２２の設置の際に決定される情報である。設定情報６３には、３Ｄセンサ２１とカメラ２２の相対位置の違いの調整に使用される情報が含まれる。取得データ７０は、３Ｄセンサ２１やカメラ２２での測距および撮像によって得られた情報であり、３Ｄデータ７１、カメラ画像データ７２、フレーム取得時刻情報７３を含む。３Ｄデータ７１は、３Ｄセンサ２１での測距によって得られるデータである。カメラ画像データ７２はカメラ２２での撮像によって得られるデータである。フレーム取得時刻情報７３は、３Ｄセンサ２１やカメラ２２によるフレームの測距および撮像が開始された時刻が、フレームの識別情報に対応付けられた情報である。

処理部３０は、３Ｄセンサ通信部３１、カメラ通信部３２、制御部３３、補正データ生成部４０を備える。補正データ生成部４０は、探索範囲決定部４２、推定部４３、補正処理部４４、組合せ候補抽出部５０を有し、オプションとして、背景除去部４１を有する。組合せ候補抽出部５０は、選択部５１、変換処理部５２、特定部５３を有する。３Ｄセンサ通信部３１は、３Ｄセンサ２１との間で通信することにより、３Ｄデータ７１やフレーム取得時刻情報７３を取得する。カメラ通信部３２は、カメラ２２との間で通信することにより、カメラ画像データ７２やフレーム取得時刻情報７３を取得する。制御部３３は、３Ｄセンサ２１での測距結果を補正する際の基準の時刻の設定を行う。なお、制御部３３は、適宜、ネットワーク接続装置１０９（図３）を介して他の装置との通信を行うことにより、３Ｄセンサ２１での測距結果を補正する際の基準の時刻の情報を取得できる。

背景除去部４１は、測距対象物１０が含まれていない状態においての、３Ｄセンサ２１での測距結果を背景データとして予め保持している。背景除去部４１は、３Ｄデータ７１やカメラ画像データ７２として記録されているデータと背景データの差分を、測距対象物１０の測距および撮像結果として抽出する。

探索範囲決定部４２は、測距対象物１０の特定の位置の測距結果である可能性がある範囲を、探索範囲として決定する。ここで、探索範囲は、測距対象物１０自身の移動や回転などによって、測距対象物１０のある位置の測距結果として得られた測距点が複数のフレーム間で変動する範囲である。探索範囲決定部４２は、探索範囲を決定する際に、遷移モデル６２やフレームのデータ取得間隔などの設定情報６３を使用することができる。

選択部５１は、３Ｄセンサ２１での複数回の測距結果から複数の測距点を選択する。背景除去部４１によって探索範囲が決定されている場合、選択部５１は、探索範囲内の複数の測距点を選択する。変換処理部５２は、適宜、設定情報６３を用いて、３Ｄセンサ２１での測距結果とカメラ２２での撮像結果の座標系を変換する。特定部５３は、選択部５１が選択した測距点の各々に対して、その測距点に対応する画素を、カメラ２２で測距対象物１０を複数回にわたって撮像した結果から特定する。特定部５３の処理の詳細については後述する。

推定部４３は、特定部５３などの処理によって、複数の測距点の各々に対応付けられた画素の特徴量を用いて、複数の測距点のうち測距対象物１０上の同じ表面位置の測距結果を推定することにより、測距対象物１０の移動軌跡を推定する。例えば、推定部４３は、複数の測距点のうち、対応付けられた画素のＲＧＢ値や輝度などの値が所定の範囲内に含まれている測距点同士は、測距対象物１０上の同じ表面位置の測距結果である可能性があると推定できる。補正処理部４４は、推定部４３で得られた測距対象物１０の移動軌跡の推定結果を用いて、３Ｄデータ７１を補正することにより、測距点の各々が所定の時刻に測距された場合に得られる測距データを生成する。

図３は、データ補正装置２０のハードウェア構成の例を示す。データ補正装置２０は、プロセッサ１０１、メモリ１０２、入力装置１０３、バス１０５を備える。さらにオプションとして、データ補正装置２０は、出力装置１０４、外部記憶装置１０６、媒体駆動装置１０７、ネットワーク接続装置１０９を備えても良い。データ補正装置２０は、例えば、入力装置１０３を備えているコンピュータか、入力装置１０３を接続可能なコンピュータなどで実現されることがある。

プロセッサ１０１は、任意の処理回路であり、例えば、Central Processing Unit（ＣＰＵ）とすることができる。プロセッサ１０１は、処理部３０として動作する。なお、プロセッサ１０１は、例えば、メモリ１０２や外部記憶装置１０６に記憶されたプログラムを実行することができる。メモリ１０２は、プロセッサ１０１の動作により得られたデータや、プロセッサ１０１の処理に用いられるデータも、適宜、記憶する。メモリ１０２や外部記憶装置１０６は、データ補正装置２０において、記憶部６０として動作する。入力装置１０３は、３Ｄセンサ２１やカメラ２２である。さらに、入力装置１０３として、データ補正装置２０は、ボタン、キーボードやマウス等を備えていても良い。

ネットワーク接続装置１０９は、ネットワーク１１０を介した他の装置との通信に使用される。出力装置１０４は、ディスプレイなどとして実現される。バス１０５は、プロセッサ１０１、メモリ１０２、入力装置１０３、出力装置１０４、外部記憶装置１０６、媒体駆動装置１０７、ネットワーク接続装置１０９の間を相互にデータの受け渡しが行えるように接続する。外部記憶装置１０６は、プログラムやデータなどを格納し、格納している情報を、適宜、プロセッサ１０１などに提供する。媒体駆動装置１０７は、メモリ１０２や外部記憶装置１０６のデータを可搬記憶媒体１０８に出力することができ、また、可搬記憶媒体１０８からプログラムやデータ等を読み出すことができる。ここで、可搬記憶媒体１０８は、フロッピイディスク、Magneto-Optical（ＭＯ）ディスク、Compact Disc Recordable（ＣＤ－Ｒ）やDigital Versatile Disk Recordable（ＤＶＤ－Ｒ）を含む、持ち運びが可能な任意の記憶媒体とすることができる。

＜第１の実施形態＞
以下、第１の実施形態を、測距データおよび撮像データの取得、測距点に対応する画素の特定、移動軌跡の推定と測距データの補正に分けて説明する。

（１）測距データおよび撮像データの取得
図４は、測距データおよび撮像データの例を説明する図である。図４のＥ１は、３Ｄセンサ２１とカメラ２２の各々で測距対象物１０を測距および撮像する様子の例である。Ｅ１では、３Ｄセンサ２１から測距される範囲をＡ、カメラ２２から撮像される範囲をＢとして示す。３Ｄセンサ２１によって時系列走査方式で測距対象物１０が測距されるので、複数の測距点についての情報が３Ｄセンサ２１から処理部３０に入力される。同様に、カメラ２２が撮像した結果得られた画像は、カメラ２２から処理部３０に入力される。

図４のＥ２は、３Ｄセンサ２１およびカメラ２２での測距および撮像のタイミングの例を示している。３Ｄセンサ２１では、時刻ｔＳｆ（ｉ－１）から始まる期間Ｐ（ｉ－１）にわたって行われる複数回の測距処理によって、（ｉ－１）番目のフレームが得られている。同様に、時刻ｔＳｆｉから始まる期間Ｐｉにわたって行われる複数回の測距処理によって、ｉ番目のフレームが得られ、時刻ｔＳｆ（ｉ＋１）から始まる期間Ｐ（ｉ＋１）での測距処理によって、（ｉ＋１）番目のフレームが得られる。

このように測距された（ｉ－１）番目～（ｉ＋１）番目のフレームに含まれている測距点の情報は、３Ｄセンサ２１から３Ｄセンサ通信部３１に入力される。３Ｄセンサ通信部３１は、得られた情報を３Ｄデータ７１として記憶部６０に記録する。さらに、３Ｄセンサ通信部３１は、各フレームの取得開始時刻を、フレームの番号と対応付けてフレーム取得時刻情報７３に記録する。

図４のＥ２に示す例では、カメラ２２による撮像処理では、グローバルシャッター方式が用いられるので、所定の時刻での１回の撮像処理により、１つのフレームが得られる。Ｅ２に示す例では、時刻ｔＣｆ（ｊ－２）に（ｊ―２）番目のフレームが撮像され、時刻ｔＣｆ（ｊ－１）に（ｊ―１）番目のフレームが撮像される。さらに、時刻ｔＣｆｊにｊ番目のフレーム、時刻ｔＣｆ（ｊ＋１）に（ｊ＋１）番目のフレーム、時刻ｔＣｆ（ｊ＋２）に（ｊ＋２）番目のフレームが撮像される。

撮像された（ｊ－２）番目～（ｊ＋２）番目のフレームの画像情報は、カメラ２２からカメラ通信部３２に入力される。カメラ通信部３２は、得られた情報をカメラ画像データ７２として記憶部６０に記録するとともに、カメラ２２で撮像された各フレームの取得時刻と、各フレームがカメラ２２で撮像されていることを、フレームの番号と対応付けてフレーム取得時刻情報７３に記録する。

図４のＥ３は、３Ｄセンサ２１とカメラ２２の各々から得られた情報と、各情報が取得されるタイミングの例を示す。カメラ２２での撮像により得られた画素のデータを四角、３Ｄセンサ２１による撮像で得られた測距点を黒丸で示す。なお、Ｅ３中に示す四角形の模様は、各画素の特徴量に対応しており、同じ模様の四角形同士は特徴量がほぼ同じものとする。Ｅ３に示すように、カメラ２２での撮像結果に含まれる各画素の情報は、グローバルシャッター方式による撮像が行われる時刻にまとめて生成される。一方、３Ｄセンサ２１ではスキャン方式が使用されるので、３Ｄセンサ２１での測距結果に含まれる測距点は、その測距点についての測距処理が行われた時刻での測距結果になる。このため、測距点の得られる時刻は、フレームの測距にかかる期間の範囲内で分散されている。図４のＥ３では、各フレームの測距にかかる期間を矢印で示している。

図４のＥ３では、縦軸は、変換処理後の座標系での空間位置であり、横軸は測距点や画素の取得された時刻である。なお、Ｅ３の例では、空間位置は、ＸＹＺ座標で表わされているものとする。また、Ｅ３に示す情報は、図を見やすくするために、３Ｄセンサ２１での測距点と、カメラ２２で得られた撮像画像中の画素の座標系が、変換処理部５２によって合わせたときに得られる情報として示している。

（２）測距点に対応する画素の特定
図５は、センサでの測距点とカメラによる撮像結果の関係の例を説明する図である。以下の説明では、各フレームが３Ｄセンサ２１とカメラ２２のいずれで撮像されたかを分かりやすくするために、３Ｄセンサ２１で得られた測距結果を「センサフレーム」と記載することがある。同様に、カメラ２２で得られた撮像結果を「カメラフレーム」と記載することがある。図５の例では、測距対象物１０中の特定の位置の測距結果のＸＹＺ座標値が２つのセンサフレーム間で変動する範囲として、閾値Ｔｈに示す範囲が、探索範囲決定部４２によって予め求められているとする。選択部５１は、探索範囲決定部４２から閾値Ｔｈを取得する。

さらに、選択部５１は、処理対象として選択したセンサフレーム中の測距点と、処理対象のセンサフレームの１つ前に測距されたセンサフレーム中の測距点であり、かつ、閾値Ｔｈの範囲内に含まれる測距点を選択する。図５中のグラフＧ１では、時刻ｔＳｆｉから開始する期間Ｐｉに測距されたｉ番目のセンサフレームのうち、時刻ｔｍに得られた測距点ｄ１に着目したとする。選択部５１は、（ｉ－１）番目のセンサフレーム中の測距点のうち、測距点ｄ１から閾値Ｔｈの範囲に含まれる測距点を選択する。図５の例では、（ｉ－１）番目のセンサフレームには、測距点ｄ２～ｄ５の４つの測距点が含まれている。測距点ｄ２～ｄ５のうち、測距点ｄ３～ｄ５は、測距点ｄ１から閾値Ｔｈの範囲に含まれているとする。すると、選択部５１は、ｉ番目のセンサフレームから測距点ｄ１を選択すると共に、（ｉ－１）番目のセンサフレームから測距点ｄ３～ｄ５を選択する。

選択部５１は、ｉ番目のセンサフレームから選択した測距点ｄ１を終点として、（ｉ－１）番目のセンサフレームから選択した測距点を始点とするＸＹＺｔベクトルを求める。例えば、選択部５１は、測距点ｄ３が始点で測距点ｄ１を終点とした場合として、ベクトルｖ２を計算する。同様に、選択部５１は、測距点ｄ４が始点で測距点ｄ１を終点とした場合としてベクトルｖ３、測距点ｄ５が始点で測距点ｄ１を終点とした場合としてベクトルｖ４を計算する。一方、測距点ｄ２は測距点ｄ１から閾値Ｔｈの範囲内に含まれていないので、選択部５１は、測距点ｄ２を選択せず、測距点ｄ２から測距点ｄ１に至るベクトル（ベクトルｖ１）も計算しない。

次に、特定部５３は、選択部５１が選択した各測距点に対応付ける画素を、カメラ画像データ７２から特定する。このとき、特定部５３は、選択部５１で求められたベクトルの各々について、そのベクトルが測距対象物１０の移動の軌跡に対応している場合の各測距点に対応するカメラフレーム中の画素を特定する。以下、グラフＧ１中のベクトルｖ４が測距対象物１０の軌跡を表わすベクトルであるという仮定に基づいた、測距点ｄ１に対応するカメラフレーム中の画素を求める場合の例を説明する。

特定部５３は、（ｉ－１）番目のセンサフレームの測距時刻からｉ番目のセンサフレームの測距時刻までの期間に行われたカメラ２２での撮像時刻のうち、時刻ｔｍに近い時刻を、フレーム取得時刻情報７３を用いて特定する。図４のＥ２に示すように、時刻ｔＣｆｊが時刻ｔｍに最も近いカメラ２２の撮像時刻となる。特定部５３は、図５のグラフＧ１に示すように、ＸＹＺｔ四次元空間において、選択したベクトルｖ４の時刻ｔＣｆｊに通過する点Ａを求める。なお、ここで、点ＡはグラフＧ１を用いて求められているので、センサ座標系に従ってＸＹＺ座標が求められている。すなわち、３Ｄセンサ２１から点Ａに至るＸＹＺ空間ベクトルは、測距対象物１０の移動軌跡がベクトルｖ４の通りである場合に、時刻ｔｍに測距点ｄ１として観測された測距対象物１０中の位置が、時刻ｔＣｆｊに観測される座標のセンサ座標系での表記となる。そこで、特定部５３は、変換処理部５２に点Ａのセンサ座標系での座標を通知して、点Ａの座標をセンサ座標系からカメラ座標系に変換することを要求する。

変換処理部５２は、３Ｄセンサ２１とカメラ２２の相対位置の違いを用いて、点Ａをセンサ座標系からカメラ座標系に変換する。なお、３Ｄセンサ２１とカメラ２２の相対位置の違いを計算するために使用される情報は、設定情報６３として、予め、記憶部６０に保持されている。設定情報６３の例として、例えば、３Ｄセンサ２１のスキャン方向ベクトル、３Ｄセンサ２１での測距時刻の計算用の情報、カメラ２２の入射光ベクトル、３Ｄセンサ２１とカメラ２２の相対的な位置角度の情報などが含まれる。３Ｄセンサ２１のスキャン方向ベクトルは、３Ｄセンサ２１で測距されるフレームの測距点の各々が、センサ座標系の原点からどちらの方向にあるかを示すベクトルの情報である。３Ｄセンサ２１での測距時刻の計算用の情報は、フレームの先頭の測定時刻を基準とした場合の各測距点の測定時刻までの時間や、フレーム間の測距時間間隔などである。カメラ２２の入射光ベクトルは、カメラ２２で撮像されたフレーム中の画素が、カメラ座標系の原点からどちらの方向にあるかを示すベクトルの情報である。３Ｄセンサ２１とカメラ２２の相対的な位置角度の情報は、カメラ座標系の原点とセンサ座標系の原点の相対的な位置角度の情報である。変換処理部５２は、設定情報６３に含まれている情報を用いて、任意の既知の方法により、座標系の変換処理を行う。変換処理部５２での座標の変換処理は、図５の右側の図に示すように、カメラ２２での時刻ｔＣｆｊでの撮像結果Ｂへの、３Ｄセンサ２１から点Ａに至るＸＹＺ空間ベクトルの写像を求めることに相当する。以下の説明では、点Ａに至るＸＹＺ空間ベクトルを撮像結果Ｂに写像することにより、画素α１が求められたとする。

すると、特定部５３は、変換処理部５２での処理結果に基づいて、測距対象物１０の移動軌跡がベクトルｖ４の通りである場合に測距点ｄ１に対応する画素として、画素α１を特定する。特定部５３は、特定した画素α１の特徴量を取得する。以下の説明では、特徴量として、画素のＲＧＢ値が用いられる場合を例とする。例えば、図５を参照しながら説明した処理により、特定部５３は、以下のような情報を取得できる。

測距対象物１０の移動の軌跡の予測：ベクトルｖ４
測距点ｄ１に対応する画素 ：画素α１
画素α１のＲＧＢ値 ：（Ｒ，Ｇ，Ｂ）＝（ｒ１，ｇ１，ｂ１）

特定部５３は、測距点ｄ１について、測距対象物１０の移動軌跡の候補として他に予測されたベクトルを採用した場合についても、図５を参照しながら説明した処理と同様の処理により、対応する画素をカメラフレーム中から特定する。さらに、特定部５３は、測距点ｄ３については、測距対象物１０の移動の軌跡の予測がベクトルｖ２であると仮定した上で、対応する画素をカメラフレーム中から特定する。同様に、特定部５３は、測距対象物１０の移動の軌跡の予測がベクトルｖ３であると仮定した上で測距点ｄ４に対応する画素を求め、測距対象物１０の移動の軌跡の予測がベクトルｖ４であると仮定した上で測距点ｄ５に対応する画素を求める。

図６は、測距点に対応する画素の特定例を説明する図である。図６中の黒丸は、対応付けられた時刻に３Ｄセンサ２１で得られた測距点である。一方、図６中の四角形は、対応付けられた時刻に撮像されたカメラフレーム中の画素である。図６では、各画素のＲＧＢ値の差を分かりやすくするために、四角形の模様は画素のＲＧＢ値に対応付けている。図６では、複数の測距点の各々に対応付けられた画素のＲＧＢ値の差が所定の閾値以下である場合に、同じ模様の四角形で表わしている。ここで、閾値の大きさは、同じ位置の撮像結果として許容できる程度のＲＧＢ値の差分であり、実装に応じて任意に設定されるものとする。例えば、画素α１を表わす四角形と画素α２を表わす四角形は、いずれも横線模様であるので、画素α１と画素α２ではＲＧＢ値の差が閾値以下であると判断できる。一方、画素γ１を表わす四角形は白抜きの四角形であるが、画素γ２を表わす四角形には斜線が入っているので、画素γ１と画素γ２ではＲＧＢ値が異なると判断できる。

以下、測距対象物１０の移動軌跡をベクトルｖ３と仮定した場合、測距点ｄ１に対応する画素として画素β１が得られたとする。さらに、測距対象物１０の移動軌跡をベクトルｖ２と仮定した場合、測距点ｄ１に対応する画素として画素γ１が得られたとする。なお、図６においても、図５を参照しながら説明した通り、測距点ｄ１について、測距対象物１０の移動軌跡をベクトルｖ４と仮定した場合、測距点ｄ１に対応する画素が画素α１であると求められているとする。

さらに、測距対象物１０の移動軌跡をベクトルｖ２と仮定した場合、測距点ｄ３に対応する画素として画素γ２が得られたとする。同様に、測距対象物１０の移動軌跡をベクトルｖ３と仮定したときに測距点ｄ４に対応する画素として画素β２が得られ、測距対象物１０の移動軌跡をベクトルｖ４と仮定したときに測距点ｄ５に対応する画素として画素α２が得られたとする。

（３）移動軌跡の推定と画像の補正
以下、図６に示すとおりに画素が特定された場合について、推定部４３で行われる移動軌跡の推定処理の例を、図６を参照しながら説明する。

推定部４３は、特定部５３での特定結果と特定された画素のＲＧＢ値を用いて、測距対象物１０の移動軌跡を表わしているベクトルを推定する。ここで、同じベクトルに対応付けられた画素同士や測距点同士は、そのベクトルに従って測距対象物１０が移動している場合に、測距対象物１０の表面上の同じ位置をカメラ２２で撮像した結果に相当する。このため、測距対象物１０の実際の移動軌跡を表わしているベクトルに対応付けられた画素同士では、ＲＧＢ値はほとんど変動しないと予測できる。そこで、推定部４３は、同じベクトルに対応付けられている画素同士のＲＧＢ値の変動量が誤差範囲に含まれる場合に、画素の特定に使用されたベクトルが測距対象物１０の移動軌跡を表わしていると推定する。一方、同じベクトルに対応付けられている画素同士のＲＧＢ値の変動量が誤差範囲に含まれない場合は、画素の特定に使用されたベクトルが測距対象物１０の移動軌跡を表わしていないと推定する。

例えば、測距対象物１０の移動軌跡がベクトルｖ２の通りであると仮定した場合、測距点ｄ１に対応付けて画素γ１が特定され、測距点ｄ３に対応付けて画素γ２が特定されている。そこで、推定部４３は、画素γ１と画素γ２のＲＧＢ値を比較する。図６の例では、画素γ１と画素γ２ではＲＧＢ値が異なっているので、推定部４３は、ベクトルｖ２は、測距対象物１０の移動軌跡ではないと推定する。

次に、測距対象物１０の移動軌跡をベクトルｖ３と仮定した場合について、推定部４３が処理を行うとする。測距対象物１０の移動軌跡をベクトルｖ３と仮定した場合、測距点ｄ１に対応付けて画素β１が特定され、測距点ｄ４に対応付けて画素β２が特定されている。そこで、推定部４３は、画素β１と画素β２のＲＧＢ値を比較する。図６の例では、画素β１の四角形は白抜きであるが、画素β２の四角形はグレーであるため、画素β１と画素β２ではＲＧＢ値が異なっている。このため、推定部４３は、ベクトルｖ３は、測距対象物１０の移動軌跡ではないと推定する。

さらに、測距対象物１０の移動軌跡をベクトルｖ４と仮定した場合について、推定部４３が処理を行うとする。測距対象物１０の移動軌跡をベクトルｖ４と仮定した場合、測距点ｄ１に対応付けて画素α１が特定され、測距点ｄ５に対応付けて画素α２が特定されている。そこで、推定部４３は、画素α１と画素α２のＲＧＢ値を比較する。図６の例では、画素α１と画素α２の四角形はいずれも同じ模様であるので、画素α１と画素α２ではＲＧＢ値がほぼ同じであると判定される。このため、推定部４３は、ベクトルｖ４が、測距対象物１０の移動軌跡であると推定する。

図７のＥ１１は、図６を参照しながら説明した処理によって推定された移動軌跡の例を示している。移動軌跡Ｌは、図６のベクトルｖ４を含み、測距対象物１０中の同じ位置を撮像した画素として推定された画素α１と画素α２の各々のＸＹＺｔ４次元空間での座標を結ぶ直線状となっている。

図７のＥ１２を参照しながら、移動軌跡Ｌを用いた測距結果の補正の例を説明する。補正処理部４４は、測距データの補正の基準とする時刻を、制御部３３から取得する。この例では、ｉ番目のセンサフレームの測距開始時刻である時刻ｔＳｆｉに合わせて測距データを補正することが要求されているとする。また、推定部４３によって移動軌跡Ｌが推定されたことから、測距対象物１０のうち、測距点ｄ１として測距された位置は、移動軌跡Ｌに沿って移動していると考えられる。そこで、補正処理部４４は、測距点ｄ１を移動軌跡Ｌの直線方程式で、時刻＝ｔＳｆｉの値を取る点ｃｄ１に置き換える。

補正対象のセンサフレームに含まれている他の測距点（測距点ｄ６、ｄ７）についても図５～図７を参照しながら説明した処理と同様の処理を行うことにより、各測距点を補正できる。補正処理部４４は、補正によって得られた結果をプロットすることにより、ｉ番目のセンサフレーム中に含まれている測距点を、時刻ｔＳｆｉにグローバルシャッター方式を用いた測距を行った場合と同様に補正したセンサフレームを生成する。

図８Ａと図８Ｂは、第１の実施形態にかかる処理の例を説明するフローチャートである。なお、図８Ａと図８Ｂにおいて、ステップＳ１１とＳ１２は並行して行われる処理であるが、実装に応じて、ステップＳ１１とＳ１２の順序は任意に変更され得る。また、図８Ａと図８Ｂでは、変数ｎ、変数ｍと定数Ｎを使用する。定数Ｎは、選択したセンサフレーム内で補正処理の対象とする測距点（注目測距点）の総数である。変数ｎと変数ｍは、処理対象とした注目測距点の数を計数するために使用される。図８Ａと図８Ｂでは、背景除去部４１を保持しているデータ補正装置２０で行われる処理の例を説明するが、背景除去部４１を保持していないデータ補正装置２０は、ステップＳ１３を省略する。

まず、３Ｄセンサ２１が測距対象物１０を測距することによりセンサフレームを生成すると、３Ｄセンサ通信部３１は、３Ｄセンサ２１からセンサフレームを受信すると共に、３Ｄデータ７１として記憶部６０に記憶する（ステップＳ１１）。同様に、カメラ２２での測距対象物１０の撮像によりカメラフレームが生成されると、カメラ通信部３２は、カメラフレームをカメラ２２から受信すると共に、カメラ画像データ７２として記憶部６０に記憶する（ステップＳ１２）。

背景除去部４１は、予め、背景測距フレームデータを記憶しているとする。ここで、背景測距フレームデータは、測距対象物１０が無い場合の測距対象物１０の背景を、３Ｄセンサ２１で測距したときに得られるセンサフレームのデータである。背景除去部４１は、背景測距フレームデータと処理対象のセンサフレームとの差分から、測距対象物１０が測距されている領域を抽出する（ステップＳ１３）。選択部５１は、制御部３３から補正目標とする時刻を取得し、補正目標の時刻から処理対象のセンサフレームデータを選択する（ステップＳ１４）。

選択部５１は、選択したセンサフレーム内の注目測距点を選択する（ステップＳ１５）。なお、ステップＳ１５において、注目測距点として選択される測距点の数は任意である。例えば、図７の例Ｅ１２では、測距点ｄ１、ｄ６、ｄ７が注目測距点として選択されている。探索範囲決定部４２は、注目測距点の各々に対して、その注目測距点で測距した測距対象物１０中の表面上の位置の測距結果がセンサフレーム間で変動する範囲（探索範囲）を決定する（ステップＳ１６）。ここで、探索範囲は、空間的な範囲と時間的な範囲を含む。例えば、図４～図７を参照した説明では、空間的な範囲は図５のＴｈで示した範囲であり、時間的な範囲は注目測距点の含まれるセンサフレームよりも１つ前に測距されたセンサフレームの測距期間である。選択部５１は、変数ｎおよび変数ｍを１に設定する（ステップＳ１７）。

選択部５１は、ｎ番目の注目測距点について、探索範囲内での測距点の組み合わせを抽出し、組合せごとに測距点を結んだときに得られるＸＹＺｔ４次元空間のベクトルを算出する（ステップＳ１８）。ここで、個々のＸＹＺｔ４次元空間のベクトルは、測距対象物１０の移動軌跡の候補である。特定部５３は、処理対象のカメラフレームのデータをカメラ画像データ７２から選択する（ステップＳ１９）。次に、特定部５３は、ｎ番目の注目測距点を含むＸＹＺｔ４次元ベクトルの各々について、ベクトルの始点か終点となっている測距点に対応する画素を２つ以上のカメラフレームから特定する（ステップＳ２０）。特定部５３は、変数ｎが定数Ｎ以上であるかを判定する（ステップＳ２１）。変数ｎが定数Ｎ未満である場合、全ての注目測距点について対応する画素の検索処理が終わっていないので、特定部５３は、変数ｎを１つインクリメントしてステップＳ１８に戻る（ステップＳ２１でＮｏ、ステップＳ２２）。

一方、変数ｎが定数Ｎ以上である場合、全ての注目測距点についての対応する画素の検索処理が終わっている（ステップＳ２１でＹｅｓ）。このため、移動軌跡の候補から実際の移動軌跡を推定するための処理が行われる。

推定部４３は、ｍ番目の注目測距点を用いて求められたＸＹＺｔ４次元空間のベクトルの始点と終点に対応付けられた画素を評価関数で評価することにより、最も評価の良いベクトルを選択する（ステップＳ２３）。ここで、評価関数は、処理対象となっているＸＹＺｔ４次元空間のベクトルを用いて選択された画素が測距対象物１０の表面上の同じ位置を撮像しているかの判定が可能な任意の関数や任意の評価基準であって良い。例えば、図６や図７を用いて説明した例では、ベクトルの始点と終点に対応付けられた画素の画素値の差が小さいほど評価関数の値が良くなるように設定されている。補正処理部４４は、ステップＳ２３で選択されたベクトルから、ｍ番目の注目測距点についての補正目標時刻における座標を算出し、出力する（ステップＳ２４）。次に、補正処理部４４は、変数ｍが定数Ｎ以上であるかを判定する（ステップＳ２５）。変数ｍが定数Ｎ未満である場合、全ての注目測距点についての補正処理が終わっていないので、補正処理部４４は、変数ｍを１つインクリメントしてステップＳ２３に戻る（ステップＳ２５でＮｏ、ステップＳ２６）。

一方、変数ｍが定数Ｎ以上である場合、全ての注目測距点についての補正処理が終わっている（ステップＳ２５でＹｅｓ）。そこで、制御部３３は、センサフレームの処理が完了したかを判定する（ステップＳ２７）。センサフレームの処理が完了していない場合、ステップＳ１５以降の処理が繰り返される（ステップＳ２７でＮｏ）。センサフレームの処理が完了した場合、制御部３３は、補正処理を終了する（ステップＳ２７でＹｅｓ）。

なお、以上の説明では、注目測距点を含むセンサフレームと、その１つ前の期間に測距されたセンサフレームを用いた処理が行われる場合を例としたが、ＸＹＺｔ４次元ベクトルの生成に使用されるセンサフレームは実装に応じて変更され得る。

このように、第１の実施形態にかかるデータ補正方法によると、３Ｄセンサ２１で得られた複数の測距点のうち、測距対象の同じ点に対する測距結果を、高解像度のカメラ２２によって得られた画像を用いて特定できる。従って、データ補正装置２０は、３Ｄセンサ２１を用いた複数のフレームの撮像している間の測距対象の移動の軌跡を推定でき、推定した軌跡を用いて、測距対象の移動による歪みを正確に修正することができる。このため、データ補正装置２０では、時系列走査により取得した移動物体の測距結果を正しく補正できる。

＜第２の実施形態＞
図９は、フレーム数を制限する場合の処理の例を説明する図である。図９に示す例では、使用するセンサフレームとカメラフレームの数をそれぞれ２フレームずつに限定した場合の例を説明する。なお、図９においても、黒丸は、対応付けられた時刻に３Ｄセンサ２１で得られた測距点であり、四角形は、対応付けられた時刻に撮像されたカメラフレーム中の画素である。また、図９でも、各画素のＲＧＢ値の差を分かりやすくするために、四角形の模様を画素のＲＧＢ値に対応付けている。

（１）センサフレームの測距開始時刻に応じてカメラフレームを選択するケース
図９のＥ２１は、処理対象のセンサフレームの測距期間の開始時刻に最も近い時刻に撮像されたカメラフレームを、センサフレームと共に使用する場合の例を示している。Ｅ２１には、３Ｄセンサ２１によって（ｉ－１）番目～（ｉ＋１）番目のフレームの測距結果が示されている。また、（ｉ－１）番目のセンサフレームの測距開始時刻は時刻ｔＳｆ（ｉ－１）で、ｉ番目のセンサフレームの測距開始時刻は時刻ｔＳｆｉ、（ｉ＋１）番目のセンサフレームの測距開始時刻は時刻ｔＳｆ（ｉ＋１）であるとする。

さらにＥ２１の例では、（ｉ－１）番目～（ｉ＋１）番目のセンサフレームが測距される間に、（ｊ－２）番目～（ｊ＋２）番目のカメラフレームが撮像されている。（ｊ－２）番目のカメラフレームの撮像時刻は時刻ｔＣｆ（ｊ－２）、（ｊ－１）番目のカメラフレームの撮像時刻は時刻ｔＣｆ（ｊ－１）、ｊ番目のフレームの撮像時刻は時刻ｔＣｆｊであるとする。同様に、（ｊ＋１）番目のカメラフレームの撮像時刻は時刻ｔＣｆ（ｊ＋１）であり、（ｊ＋２）番目のカメラフレームの撮像時刻は時刻ｔＣｆ（ｊ＋２）であるとする。

Ｅ２１のケースでは、選択部５１は、ｉ番目のセンサフレーム中の測距点から注目測距点を選択し、（ｉ－１）番目のセンサフレームから、注目測距点と組み合わせてベクトルを求める測距点を選択したとする。すると、特定部５３は、処理対象のセンサフレーム中の測距点に対応する画素を、処理対象のセンサフレームの測距開始時刻に最も近い時刻に撮像されたカメラフレームから選択する。例えば、ｉ番目のセンサフレームの測距開始時刻（ｔＳｆｉ）に最も近い時刻に撮像されたカメラフレームは、ｊ番目のカメラフレームである。また、（ｉ―１）番目のセンサフレームの測距開始時刻（ｔＳｆ（ｉ－１））に最も近い時刻に撮像されたカメラフレームは、（ｊ―２）番目のカメラフレームである。そこで、特定部５３は、ｉ番目のセンサフレーム中の測距点に対応する画素を、ｊ番目のカメラフレーム中の画素から選択する。さらに、特定部５３は、（ｉ－１）番目のセンサフレーム中の測距点に対応する画素を、（ｊ－２）番目のカメラフレーム中の画素から選択する。Ｅ２１では、処理に使用されるセンサフレームの測距開始時刻とカメラフレームの撮像時刻を楕円で囲んでいる。なお、特定部５３での画素の選択の際の処理は、第１の実施形態と同様である。さらに、選択部５１や特定部５３での処理により、測距点に対応する画素の特定が行われた後の処理も、第１の実施形態と同様である。

Ｅ２１に示すように、センサフレームの測距開始時刻に応じて使用するカメラフレームが決定される場合、特定部５３は、センサフレーム中で測距開始時刻以降に測距された測距点については観測時刻に関係なくカメラフレームを選択できる。このため、特定部５３の処理負担が小さいという利点がある。

（２）測距点ごとの測距時刻に応じてカメラフレームを選択するケース
図９のＥ２２を参照しながら、測距時刻に応じてカメラフレームを選択する場合の例を説明する。なお、Ｅ２２においても、センサフレームの測距とカメラフレームの撮像のタイミングは、Ｅ２１と同じであるとする。

Ｅ２２では、選択部５１は、ｉ番目のセンサフレーム中の測距点ｄ１を注目測距点として選択し、（ｉ－１）番目のセンサフレームから、注目測距点と組み合わせてベクトルを求める測距点を選択したとする。この場合、測距点ｄ１の測距時刻に最も近い時刻に撮像されたカメラフレームはｊ番目のカメラフレームであるので、特定部５３は、測距点ｄ１に対応付ける画素をｊ番目のカメラフレームから選択する。

さらに、測距点ｄ１と組み合わせてＸＹＺｔ４次元ベクトルを求める際に使用する測距点として、測距点ｄ３と測距点ｄ５が選択されたとする。すると、特定部５３は、各測距点の測距時刻に最も近い時刻に撮像されたカメラフレームから、画素を特定する。例えば、測距点ｄ３の測距時刻に最も近い時刻に撮像されたカメラフレームは、（ｊ－２）番目のカメラフレームである。そこで、特定部５３は、（ｊ－２）番目のカメラフレームから測距点ｄ３に対応付ける画素を選択する。一方、測距点ｄ５の測距時刻に最も近い時刻に撮像されたカメラフレームは、（ｊ－１）番目のカメラフレームである。そこで、特定部５３は、（ｊ－１）番目のカメラフレームから測距点ｄ５に対応付ける画素を選択する。測距点に対応する画素の選択状況を、Ｅ２２中の楕円で示す。

以上の説明では、１つの注目測距点とベクトルの計算のために組み合わせる測距点の間でカメラフレームが異なる場合を説明したが、注目測距点の間で画素との対応付けに使用されるカメラフレームが異なっても良い。例えば、測距点ｄ７が２つ目の注目測距点として選択されたとする。この場合、測距点ｄ７の測距時刻に最も近い時刻に撮像されたカメラフレームは、（ｊ＋１）番目のカメラフレームである。従って、測距点ｄ７が注目測距点として使用される場合、特定部５３は、測距点ｄ７に対応する画素を（ｊ＋１）番目のカメラフレームから特定する。以後の処理は、Ｅ２２のケースでも、第１の実施形態と同様である。

Ｅ２２に示すように、測距点ごとの測距時刻に応じて使用するカメラフレームが決定される場合、測距点が測距された状況に最も近い状況下で撮像されたカメラフレーム中の画素を、測距点に対応付けることができる。このため、Ｅ２１のように、センサフレーム毎にカメラフレームが選択される場合に比べて、正確な補正を行うことができるという利点がある。

なお、第２の実施形態においても、注目測距点を含むセンサフレームと、その１つ前の期間に測距されたセンサフレームを用いた処理が行われる場合を例としたが、ＸＹＺｔ４次元ベクトルの生成に使用されるセンサフレームは実装に応じて変更され得る。例えば、注目測距点を含むセンサフレームと、その１つ後の期間に測距されたセンサフレームを用いた処理が行われても良い。

＜第３の実施形態＞
第３の実施形態では、データ補正装置２０が測距対象物の遷移モデル６２を記憶しており、探索範囲決定部４２が遷移モデル６２に基づいて、ＸＹＺｔ４次元ベクトルを求めるために使用する測距点の検索範囲を決定する場合について説明する。

（１）物体の空間位置移動の遷移モデル６２が線形モデルの場合
遷移モデル６２は、測距対象物１０の各位置が線形に４次元空間を遷移したと仮定するモデル（線形モデル）であってもよい。例えば、測距対象物１０の移動方向が変異する速度より、３Ｄセンサ２１やカメラ２２で取得するフレームのフレームレートが十分速い場合には、フレーム間では撮像対象物１０の移動は線形とみなせる。この場合、探索範囲決定部４２は、線形モデルに基づいて、各注目測距点について、１センサフレーム前に推定したＸＹＺｔ４次元空間のベクトルが継続された場合の時空間位置を基準として、測距対象物１０の同じ表面上の位置の測距結果である可能性のある他のフレーム中の測距点の検索範囲を決定する。なお、探索範囲決定部４２で測距点の検索範囲が決定された後の処理は、第１の実施形態と同様である。

（２）曲線近似される遷移モデル６２を用いるケース
図１０は、遷移モデル６２の例を説明する図である。図１０のＥ３１では、過去の測距対象物１０の移動予測結果に基づいた曲線近似の遷移モデル６２の例を実線で示す。Ｅ３１の実線は、（ｉ－２）番目からｉ番目のセンサフレームを、カメラフレームと対応付ける処理を行うことによって各注目測距点に対して得られた、測距対象物１０の移動軌跡である。Ｅ３１の実線として示している時空間遷移曲線は、測距対象物１０の移動の性質などにより、破線で示すような３通りの移動パターンを示す可能性があるとする。すると、（ｉ＋１）番目以降のセンサフレームを用いた処理に使用する遷移モデル６２として、Ｅ３１の実線と、Ｅ３１中の３パターンの破線を組み合わせた情報が使用される。この場合、探索範囲決定部４２は、ｉ番目のセンサフレーム中の注目測距点と（ｉ＋１）番目のセンサフレーム中の測距点を用いたＸＹＺｔ４次元ベクトルの計算に用いる測距点の候補の検索範囲を、遷移モデル６２により、Ｅ３１中のＲに示すように決定できる。

（３）測距対象物１０の移動方向が既知であるか限定されている場合
測距対象物１０の移動方向が既知であるか限定されている場合、測距対象物１０の移動方向に対する情報を遷移モデル６２として含めることができる。すると、探索範囲決定部４２は、遷移モデル６２に基づいて、測距対象物１０の移動方向から除外される方向には広がりを持たない探索範囲を決定できる。

例えば、測距対象物１０がＥ３２の左側の図に示すように移動するとする。なお、Ｅ３２でのＸＹＺ座標系は、Ｅ３２の右側の図に示すとおりであるとする。この場合、測距対象物１０はＹ軸方向には移動しないことになる。従って、Ｙ軸方向については、現在の空間位置からの変動が発生しない。すると、遷移モデル６２には、測距対象物１０がＹ軸方向に移動しないことを表わす情報が含められる。一方、探索範囲決定部４２は、Ｙ軸方向には広がりを持たない探索範囲を決定することができる。

他にも、測距対象物１０の移動のパターンに応じて同様に、遷移モデル６２が設定され得る。例えば、遷移モデル６２は、直前の移動ベクトルに応じた制限を設けていても良い。また、測距対象物１０と地面の相対的な位置関係が既知であり、測距対象物１０が地面に接地していない場合、重力加速度から移動ベクトルを推定するように、遷移モデル６２が設定されても良い。なお、第３の実施形態のいずれのパターンが用いられる場合でも、測距点の検索範囲が決定された後の処理は、第１の実施形態と同様であるとする。

第３の実施形態で説明したように遷移モデル６２が設定されていると、探索範囲決定部４２は、探索範囲を限定することができる。このため、選択部５１や特定部５３での処理負荷が軽減される。

＜第４の実施形態＞
第１～第３の実施形態にかかる処理を行ってセンサフレームを補正すると、センサフレームに含まれている測距点の数や密度は、測距によって得られたセンサフレーム以下になる。これは、対応付けの可能な測距点の組合せが得られなかった場合や、評価値が許容範囲未満の場合に、補正した測距点が得られなくなるためである。第４の実施形態では、第１～第３の実施形態と同様の処理により補正処理を行った上で、さらに、カメラフレーム中の情報を用いて、測距点の推定値を生成すると共に、測距点の推定値を含めた補正３Ｄデータを生成する場合について説明する。

図１１は、第４の実施形態にかかるデータ補正装置２００の構成の例を説明する図である。データ補正装置２００は、３Ｄセンサ２１、カメラ２２、処理部２１０、記憶部２５０を備える。処理部２１０は、小平面領域抽出部２２０、小平面方程式算出部２３０、点群データ追加部２４０を備え、さらに、３Ｄセンサ通信部３１、カメラ通信部３２、制御部３３、補正データ生成部４０も備える。記憶部２５０は、処理プログラム６１、取得データ７０、設定情報６３を保持し、さらに、処理部２１０の処理で得られた補正３Ｄデータ２５１を格納する。また、データ補正装置２００でも、データ補正装置２０と同様に、オプションとして遷移モデル６２を保持する。

小平面領域抽出部２２０は、オプティカルフロー生成部２２１、スーパーピクセル生成部２２２、動き判定部２２３を有する。オプティカルフロー生成部２２１は、複数のカメラフレームのデータと各カメラフレームの撮像時刻の情報を用いてオプティカルフローを計算する。スーパーピクセル生成部２２２は、処理対象となっているカメラフレームについて、スーパーピクセルを生成する。以下の説明では、スーパーピクセルは、類似した特徴量を有する画素の集まりであるとする。動き判定部２２３は、オプティカルフローの計算結果を用いて、スーパーピクセルごとにそのスーパーピクセルに撮像されている対象が動いているかを判定する。

小平面方程式算出部２３０は、点群抽出部２３１と算出部２３２を有する。点群抽出部２３１は、補正データ生成部４０によって補正された補正済みの測距点のうち、スーパーピクセル中の画素に対応付けられる測距点を抽出する。算出部２３２は、点群抽出部２３１が抽出した測距点の座標を用いてスーパーピクセルの平面方程式を求める。

点群データ追加部２４０は、座標変換部２４１とデータ追加処理部２４２を有する。座標変換部２４１は、得られた平面方程式を用いて、スーパーピクセル中の各画素をＸＹＺ３次元座標に変換する。データ追加処理部２４２は、座標変換部２４１が計算した座標を、補正データ生成部４０で補正された３Ｄデータ７１に追加することにより、補正３Ｄデータ２５１を生成する。

データ補正装置２００においても、３Ｄセンサ２１、カメラ２２、３Ｄセンサ通信部３１、カメラ通信部３２、制御部３３、補正データ生成部４０の処理は、第１～第３の実施形態と同様である。また、処理プログラム６１、取得データ７０、遷移モデル６２、設定情報６３も、データ補正装置２０に保持されている情報と同様である。

なお、データ補正装置２００のハードウェア構成も図３に示すとおりである。処理部２１０はプロセッサ１０１によって実現される。さらに、メモリ１０２と外部記憶装置１０６は、記憶部２５０として動作する。

図１２は、小平面の検索処理の例を説明する図である。図１２のＣｆｊは、ｊ番目のカメラフレームである。オプティカルフロー生成部２２１は、ｊ番目のカメラフレームと、（ｊ－１）番目のカメラフレームを用いて、オプティカルフローを計算する。なお、オプティカルフローの計算方法は任意であり、例えば、勾配法やＬｕｃａｓ－Ｋａｎａｄｅ法などが用いられ得る。オプティカルフローの計算により、Ｅ４１に示すような結果が得られたとする。Ｅ４１では、オプティカルフローとして得られたベクトルのうちのいくつかを図示している。オプティカルフロー生成部２２１の処理により、領域Ｒ２に含まれる画素の各々について、オプティカルフローが得られたとする。

一方、スーパーピクセル生成部２２２は、ｊ番目のカメラフレーム（Ｃｆｊ）に含まれている画素の特徴量を用いて、スーパーピクセルを生成する。ここで、スーパーピクセルは、類似した特徴量を有する画素の集まりを特定可能な任意の方法で生成される。図１２の例では、Ｅ４２に示すようにスーパーピクセルが得られたとする。以下の処理では、スーパーピクセル生成部２２２によって計算されたスーパーピクセルの各々が、１つの小平面として扱われる。

図１３は、補間処理に使用するデータの選択方法の例を説明する図である。動き判定部２２３は、オプティカルフロー生成部２２１とスーパーピクセル生成部２２２の処理結果を用いて、オプティカルフローが得られた画素を１つ以上含むスーパーピクセルを特定する。図１３のＥ５１の例では、Ｒ２に示す四角形の領域中に含まれているスーパーピクセルの各々が、オプティカルフローが求められた画素を１つ以上含んでいるとする。動き判定部２２３は、オプティカルフローが求められた画素を１つ以上含むスーパーピクセルの各々について、動きのあるスーパーピクセルであると判定する。ここで、各スーパーピクセルは小平面として扱われる。従って、動きがあると判定されたスーパーピクセルの各々は、（ｊ－１）番目のカメラフレームの撮像時刻からｊ番目のカメラフレームの撮像時刻までの間の期間で移動している小平面として特定されていることになる。また、動きがあると判定されたスーパーピクセルは、補間処理に使用するデータとして選択される。

一方、制御部３３は、ｊ番目のカメラフレームを用いて小平面の特定が行われたことから、補正データ生成部４０での補正処理の基準とする時刻（補正目標時刻）を、ｊ番目のカメラフレームの撮像時刻（ｔＣｊ）に決定する。制御部３３は、決定した補正目標時刻を補正データ生成部４０に通知する。

補正データ生成部４０は、ｊ番目のカメラフレームの撮像時刻（ｔＣｊ）を補正目標時刻にすることを制御部３３から要求されると、３Ｄセンサ２１での撮像により得られた測距点の補正処理を行う。従って、この例において補正データ生成部４０によって得られた補正後の測距点の各々は、時刻ｔＣｊでの各測距点のＸＹＺ座標であるといえる。なお、補正データ生成部４０での測距点の補正処理は、第１～第３の実施形態のいずれかで説明した処理と同様である。図１３のＥ５２に、補正後のセンサフレーム中の測距点のＸＹＺ座標でのイメージを白抜きの十字で示す。なお、ｊ番目のカメラフレームの撮像時刻から最も近い時刻から撮像されたセンサフレーム中の測距点のＸＹＺ座標のイメージをＥ５２の白い三角形で示す。さらに、ｊ番目のカメラフレームの撮像時刻から最も近い時刻から撮像されたセンサフレームよりも１つ前の撮像期間で撮像されたセンサフレーム中の測距点のＸＹＺ座標のイメージをＥ５２の白丸で示す。

補正データ生成部４０は、Ｅ５２に示すように、ｊ番目のカメラフレームの撮像時刻（ｔＣｊ）に合わせた補正後の測距点のＸＹＺ座標の情報を、点群抽出部２３１に出力する。点群抽出部２３１は、補正データ生成部４０から取得した各測距点のＸＹＺ座標の情報を用いて、各測距点がいずれのスーパーピクセルに含まれているかを判定する。点群抽出部２３１は、動き判定部２２３によって動きがあると判定されているスーパーピクセルについて、そのスーパーピクセルに対応付けられた測距点が３点以上あるかを判定する。補正データ生成部４０から通知された３点以上の測距点を含むスーパーピクセルでは、通知された測距点の全てを通る平面がスーパーピクセルの小平面と一致することになる。そこで、算出部２３２は、動きがあると判定されているスーパーピクセルについて、そのスーパーピクセルに含まれる３点以上の測距点を通る平面の方程式を求める。ここで、平面の方程式の求め方は任意であり、例えば、ＲＡＮＳＡＣ（Random sample consensus）が用いられても良い。

座標変換部２４１は、算出部２３２での処理により得られた平面方程式と、カメラ２２のカメラパラメータを用いて、射影変換を行うことにより、スーパーピクセル中の各画素のＸＹＺ座標を求める。例えば、スーパーピクセル中の画素の座標（ｘ，ｙ，１）を、平面Ｚ＝ａＸ＋ｂＹ＋ｃ上に変換する場合、式（１）に従って、射影変換が行われる。

ここで、Ｈは式（２）で表わされ、カメラパラメータは既知でありＰで表わされるとする。

なお、式（３）において、Ｃは平面を表わす。

データ追加処理部２４２は、座標変換部２４１で得られた各画素のＸＹＺ座標の値を、ｊ番目のカメラフレームの撮像時刻（ｔＣｊ）での測距点を補間するデータとして、補正データ生成部４０で得られた補正結果に追加する。データ追加処理部２４２は、追加処理後の補正結果を、補正３Ｄデータ２５１として記憶部２５０に格納する。

平面方程式の計算から、補正データ生成部４０での補正結果に対する測距点の補間データの追加までの処理は、３点以上の補正後の測距点が通過しており、かつ、動きがある領域として特定された全てのスーパーピクセルに対して行われる。

図１４は、第４の実施形態にかかる処理の例を説明するフローチャートである。図１４の説明では、スーパーピクセルの生成や３次元座標の計算に用いられるカメラフレームのことを、処理対象のカメラフレームと記載する。また、図１４では、処理対象としたスーパーピクセルの数の計数のために、変数ｘを用いる。

まず、３Ｄセンサ２１がセンサフレームを生成すると、３Ｄセンサ通信部３１は、３Ｄセンサ２１からセンサフレームを受信すると共に、３Ｄデータ７１として記憶部６０に記憶する（ステップＳ３１）。同様に、カメラ２２でカメラフレームが生成されると、カメラ通信部３２は、カメラフレームをカメラ２２から受信すると共に、カメラ画像データ７２として記憶部６０に記憶する（ステップＳ３２）。補正データ生成部４０は、ステップＳ３１とＳ３２で得られたフレームのデータを用いて、センサフレーム中の測距点の補正データを生成する（ステップＳ３３）。なお、ステップＳ３３において、補正データ生成部４０は、処理対象のカメラフレームの撮像時刻に合わせた補正データを生成するものとする。補正データ生成部４０での処理の詳細は、図８Ａと図８Ｂを用いて説明したステップＳ１３～Ｓ２７と同様である。

オプティカルフロー生成部２２１は、処理対象のカメラフレームと他のカメラフレームを用いて、処理対象のカメラフレーム中の画素の各々について、オプティカルフローを計算する（ステップＳ３４）。スーパーピクセル生成部２２２は、処理対象のカメラフレームからスーパーピクセルを生成する（ステップＳ３５）。動き判定部２２３は、変数ｘを１に設定する（ステップＳ３６）。その後、動き判定部２２３は、ｘ番目のスーパーピクセルに動きがあるかを判定する（ステップＳ３７）。このとき、動き判定部２２３は、ｘ番目のスーパーフレーム中のいずれかの画素についてオプティカルフローが求められている場合、ｘ番目のスーパーピクセルに動きがあると判定する。

ｘ番目のスーパーピクセルに動きがある場合、点群抽出部２３１は、補正データからｘ番目のスーパーピクセル内の画素に対応付けられる測距点を抽出する（ステップＳ３７でＹｅｓ、ステップＳ３８）。算出部２３２は、抽出した測距点の３次元座標（ＸＹＺ座標）を用いて、ｘ番目のスーパーピクセルの平面方程式を算出する（ステップＳ３９）。座標変換部２４１は、平面方程式とカメラパラメータを用いて、ｘ番目のスーパーピクセルの各画素で撮像した位置の３次元座標を求める（ステップＳ４０）。データ追加処理部２４２は、得られた３次元座標を補正データに追加する（ステップＳ４１）。データ追加処理部２４２は、全てのスーパーピクセルの処理が完了しているかを判定する（ステップＳ４２）。このとき、データ追加処理部２４２は、変数ｘがステップＳ３５で生成されたスーパーピクセルの総数に達しているかによって、全てのスーパーピクセルの処理が完了しているかを判定する。変数ｘがスーパーピクセルの総数未満の場合、データ追加処理部２４２は、全てのスーパーピクセルの処理が完了していないと判定する（ステップＳ４２でＮｏ）。すると、データ追加処理部２４２は、変数ｘを１つインクリメントしてステップＳ３７に戻る（ステップＳ４３）。

一方、変数ｘがスーパーピクセルの総数以上の場合、データ追加処理部２４２は、全てのスーパーピクセルの処理が完了したと判定して処理を終了する（ステップＳ４２でＹｅｓ）。

ステップＳ３７において、ｘ番目のスーパーフレーム中のいずれの画素についてもオプティカルフローが求められていない場合、動き判定部２２３は、ｘ番目のスーパーピクセルに動きがないと判定する（ステップＳ３７でＮｏ）。この場合、ｘ番目のスーパーフレームを用いた補間処理は行われず、ステップＳ４２以降の処理が行われる。

このように、第４の実施形態によると、カメラフレーム中の情報を用いて、測距点の間を補間するデータを生成することができる。従って、３Ｄセンサ２１を用いて得られた測距点の密度が低い場合であっても、第４の実施形態による補間処理を用いて、データの取得密度を高めることができる。

＜その他＞
なお、実施形態は上記に限られるものではなく、様々に変形可能である。以下にその例をいくつか述べる。

以上の説明では、３Ｄデータ７１で得られた測距点をカメラ系座標に合わせた場合を例としていたが、実装に応じて、変換処理部５２は、カメラ画像データ７２で得られた各画素をセンサ座標系に合わせて変換しても良い。

予め、測距対象物１０の最大の移動速度が分かっている場合、データ補正装置２０やデータ補正装置２００は、最大移動速度を用いて、固定的に測距点の探索範囲を決定しても良い。

以上の説明では、分かりやすくするためにグローバルシャッター方式のカメラ２２を用いた場合を例として説明したが、データ補正装置２０やデータ補正装置２００は、ローリングシャッター方式のカメラを使用しても良い。この場合、カメラと３Ｄセンサ２１でのデータ取得時刻が既知であるか、計算可能であれば、カメラの撮像結果中の画素間での撮像位置が一致しているかを判定した上で、以上の説明と同様の処理を行うことにより、３Ｄセンサ２１での測距点を補正できる。

第１の実施形態では、複数の測距点の各々に対応付けられた画素のＲＧＢ値の差が所定以下であるかを、測距点の対象物の表面上の位置が互いに一致しているかの判定基準に用いた例を説明したが、これは評価関数の一例に過ぎない。評価関数として、例えば、測距点の各々に対応付けられた画素の周辺の複数の画素でのＲＧＢ値や輝度の平均値を求め、平均値同士の差が小さいほど、良好な結果となる評価関数が用いられても良い。この場合、評価関数で得られた値が良好なほど、測距点の対象物の表面上の位置が互いに一致している確率が高いと判定されるものとする。

さらに、測距点の対象物の表面上の位置が互いに一致している確率が高いほど値が大きくなるように設定されている評価関数での評価結果が、ある注目測距点について、いずれの測距点と組み合せても所定値以下の場合、その注目測距点についてのデータを破棄しても良い。さらに、ある注目測距点について複数のフレーム間でカメラ画像中での位置の変動が、所定量よりも大きい場合、推定部４３は、３次元空間中でも位置の変動が大きいので、補正に適していないと判定してデータを破棄しても良い。さらに、推定部４３は、画素値が極端に大きい値や小さい値が測距点に対応付けられている場合に、正常な撮像が行われていないと判定して注目測距点のデータを破棄するように変形されても良い。このような処理により、破棄されたデータ量が増えた場合には、第４の実施形態を用いて、カメラ２２での撮像結果から３Ｄセンサ２１での測距点の補間処理を行うことができる。

上述の第１～第４の実施形態を含む実施形態に関し、さらに以下の付記を開示する。
（付記１）
測距対象を時系列走査することにより測距する測距センサと、
前記測距センサより解像度の高いカメラと、
前記測距センサでの複数回の測距結果から選択された複数の測距点の各々に対して、前記カメラでの前記測距対象の複数回の撮像結果から、当該測距点に対応する画素を特定する特定部と、
前記複数の測距点の各々に対応付けられた画素の特徴量を用いて、前記複数の測距点のうち前記測距対象中の同じ位置の測距結果を推定することにより、前記測距対象の移動軌跡を推定する推定部と、
前記移動軌跡を用いて、前記測距センサでの前記測距対象の測距結果を、特定の時刻での測距結果に補正する補正処理部
を備えることを特徴とするセンサデータ補正装置。
（付記２）
第１の測距結果に含まれる測距点から補正対象として注目する第１の測距点を選択すると共に、前記第１の測距点の測距対象中の位置と同じ位置を測距した可能性のある第２の測距点を、前記複数回の測距結果のうちの前記第１の測距結果とは異なる第２の測距結果から選択する選択部
をさらに備え、
前記推定部は、前記第１の測距点に対応付けられた画素の特徴量と、前記第２の測距点に対応付けられた画素の特徴量の差が閾値を下回ると、前記移動軌跡は前記第１の測距点と前記第２の測距点を通ると推定する
ことを特徴とする付記１に記載のセンサデータ補正装置。
（付記３）
前記特定部は、
前記カメラでの前記測距対象の撮像により得られた第１のカメラフレーム中から、前記第１の測距点と前記第２の測距点を結ぶ空間ベクトルと前記第１のカメラフレームの撮像時刻を用いて、前記第１の測距点に対応する画素を特定し、
前記カメラでの前記測距対象の撮像により得られ、前記第１のカメラフレームとは異なる第２のカメラフレーム中から、前記空間ベクトルと前記第２のカメラフレームの撮像時刻を用いて、前記第２の測距点に対応する画素を特定する
ことを特徴とする付記２に記載のセンサデータ補正装置。
（付記４）
前記測距対象の移動のパターンを記録した遷移モデルを記憶する記憶部と、
前記遷移モデルを用いて、前記第１の測距点の測距対象中の位置と同じ位置を測距した可能性のある測距点の存在する存在範囲を決定する決定部
をさらに備え、
前記選択部は、前記存在範囲に含まれる測距点から前記第２の測距点を選択する
ことを特徴とする付記２または３に記載のセンサデータ補正装置。
（付記５）
前記補正処理部が前記測距センサでの測距結果を前記第１のカメラフレームの撮像時刻を基準として補正する場合、前記第１のカメラフレーム中の画素の特徴量が所定範囲内に含まれている１つ以上の領域を生成する生成部と、
前記１つ以上の領域のうち補正後の測距点を含む領域を平面とみなしたときの当該平面を表わす平面方程式を、前記補正後の測距点の座標を用いて算出する算出部と、
前記平面方程式から計算した前記領域中の画素の座標を、前記補正処理部による補正結果を補間するデータとして、前記補正結果に追加する追加処理部
をさらに備えることを特徴とする付記３または４に記載のセンサデータ補正装置。
（付記６）
測距対象を時系列走査することにより測距する測距センサと、前記測距センサより解像度の高いカメラを備えるセンサデータ補正装置が、
前記測距センサでの複数回の測距結果から選択した複数の測距点の各々に対して、前記カメラでの前記測距対象の複数回の撮像結果から、当該測距点に対応する画素を特定し、
前記複数の測距点の各々に対応付けられた画素の特徴量を用いて、前記複数の測距点のうち前記測距対象中の同じ位置の測距結果を推定することにより、前記測距対象の移動軌跡を推定し、
前記移動軌跡を用いて、前記測距センサでの前記測距対象の測距結果を、特定の時刻での測距結果に補正する
ことを特徴とするセンサデータ補正方法。
（付記７）
前記センサデータ補正装置が、
第１の測距結果に含まれる測距点から補正対象として注目する第１の測距点を選択し、
前記第１の測距点の前記測距対象中の位置と同じ位置を測距した可能性のある第２の測距点を、前記複数回の測距結果のうちの前記第１の測距結果とは異なる第２の測距結果から選択し、
前記第１の測距点に対応付けられた画素の特徴量と、前記第２の測距点に対応付けられた画素の特徴量の差が閾値を下回ると、前記移動軌跡は前記第１の測距点と前記第２の測距点を通ると推定する
ことを特徴とする付記６に記載のセンサデータ補正方法。
（付記８）
前記センサデータ補正装置は、
前記カメラでの前記測距対象の撮像により得られた第１のカメラフレーム中から、前記第１の測距点と前記第２の測距点を結ぶ空間ベクトルと前記第１のカメラフレームの撮像時刻を用いて、前記第１の測距点に対応する画素を特定し、
前記カメラでの前記測距対象の撮像により得られ、前記第１のカメラフレームとは異なる第２のカメラフレーム中から、前記空間ベクトルと前記第２のカメラフレームの撮像時刻を用いて、前記第２の測距点に対応する画素を特定する
ことを特徴とする付記７に記載のセンサデータ補正方法。
（付記９）
前記センサデータ補正装置は、
前記測距対象の移動のパターンを記録した遷移モデルを用いて、前記第１の測距点の前記測距対象中の位置と同じ位置を測距した可能性のある測距点の存在する存在範囲を決定し、
前記存在範囲に含まれる測距点から前記第２の測距点を選択する
ことを特徴とする付記７または８に記載のセンサデータ補正方法。
（付記１０）
前記センサデータ補正装置は、
前記測距センサでの測距結果を前記第１のカメラフレームの撮像時刻を基準として補正する場合、前記第１のカメラフレーム中の画素の特徴量が所定範囲内に含まれている１つ以上の領域を生成し、
前記１つ以上の領域のうち補正後の測距点を含む領域を平面とみなしたときの当該平面を表わす平面方程式を、前記補正後の測距点の座標を用いて算出し、
前記平面方程式から計算した前記領域中の画素の座標を、前記補正による補正結果を補間するデータとして、前記補正結果に追加する
処理をさらに行うことを特徴とする付記８または９に記載のセンサデータ補正方法。
（付記１１）
測距対象を時系列走査することにより測距する測距センサと、前記測距センサより解像度の高いカメラを備えるセンサデータ補正装置に、
前記測距センサでの複数回の測距結果から選択した複数の測距点の各々に対して、前記カメラでの前記測距対象の複数回の撮像結果から、当該測距点に対応する画素を特定し、
前記複数の測距点の各々に対応付けられた画素の特徴量を用いて、前記複数の測距点のうち前記測距対象中の同じ位置の測距結果を推定することにより、前記測距対象の移動軌跡を推定し、
前記移動軌跡を用いて、前記測距センサでの前記測距対象の測距結果を、特定の時刻での測距結果に補正する
処理を行わせることを特徴とするセンサデータ補正プログラム。
（付記１２）
前記センサデータ補正装置に、
第１の測距結果に含まれる測距点から補正対象として注目する第１の測距点を選択し、
前記第１の測距点の前記測距対象中の位置と同じ位置を測距した可能性のある第２の測距点を、前記複数回の測距結果のうちの前記第１の測距結果とは異なる第２の測距結果から選択し、
前記第１の測距点に対応付けられた画素の特徴量と、前記第２の測距点に対応付けられた画素の特徴量の差が閾値を下回ると、前記移動軌跡は前記第１の測距点と前記第２の測距点を通ると推定する
処理を行わせることを特徴とする付記１１に記載のセンサデータ補正プログラム。
（付記１３）
前記センサデータ補正装置に、
前記カメラでの前記測距対象の撮像により得られた第１のカメラフレーム中から、前記第１の測距点と前記第２の測距点を結ぶ空間ベクトルと前記第１のカメラフレームの撮像時刻を用いて、前記第１の測距点に対応する画素を特定し、
前記カメラでの前記測距対象の撮像により得られ、前記第１のカメラフレームとは異なる第２のカメラフレーム中から、前記空間ベクトルと前記第２のカメラフレームの撮像時刻を用いて、前記第２の測距点に対応する画素を特定する
処理を行わせることを特徴とする付記１２に記載のセンサデータ補正プログラム。
（付記１４）
前記センサデータ補正装置に、
前記測距対象の移動のパターンを記録した遷移モデルを用いて、前記第１の測距点の前記測距対象中の位置と同じ位置を測距した可能性のある測距点の存在する存在範囲を決定し、
前記存在範囲に含まれる測距点から前記第２の測距点を選択する
処理を行わせることを特徴とする付記１２または１３に記載のセンサデータ補正プログラム。
（付記１５）
前記センサデータ補正装置に、
前記測距センサでの測距結果を前記第１のカメラフレームの撮像時刻を基準として補正する場合、前記第１のカメラフレーム中の画素の特徴量が所定範囲内に含まれている１つ以上の領域を生成し、
前記１つ以上の領域のうち補正後の測距点を含む領域を平面とみなしたときの当該平面を表わす平面方程式を、前記補正後の測距点の座標を用いて算出し、
前記平面方程式から計算した前記領域中の画素の座標を、前記補正による補正結果を補間するデータとして、前記補正結果に追加する
処理を行わせることを特徴とする付記１３または１４に記載のセンサデータ補正プログラム。

１０ 測距対象物
２０、２００ データ補正装置
２１ ３Ｄセンサ
２２ カメラ
３０、２１０ 処理部
３１ ３Ｄセンサ通信部
３２ カメラ通信部
３３ 制御部
４０ 補正データ生成部
４１ 背景除去部
４２ 探索範囲決定部
４３ 推定部
４４ 補正処理部
５０ 組合せ候補抽出部
５１ 選択部
５２ 変換処理部
５３ 特定部
６０ 記憶部
６１ 処理プログラム
６２ 遷移モデル
６３ 設定情報
７０ 取得データ
７１ ３Ｄデータ
７２ カメラ画像データ
７３ フレーム取得時刻情報
１０１ プロセッサ
１０２ メモリ
１０３ 入力装置
１０４ 出力装置
１０５ バス
１０６ 外部記憶装置
１０７ 媒体駆動装置
１０８ 可搬記憶媒体
１０９ ネットワーク接続装置
１１０ ネットワーク
２２０ 小平面領域抽出部
２２１ オプティカルフロー生成部
２２２ スーパーピクセル生成部
２２３ 動き判定部
２３０ 小平面方程式算出部
２３１ 点群抽出部
２３２ 算出部
２４０ 点群データ追加部
２４１ 座標変換部
２４２ データ追加処理部
２５１ 補正３Ｄデータ

Claims (6)

1. 測距対象を時系列走査することにより測距する測距センサによる複数回の測距結果から選択された複数の測距点の各々に対して、前記測距センサより解像度の高いカメラによる前記測距対象の複数回の撮像結果から、当該測距点に対応する画素を特定する特定部と、
   前記複数回の測距結果のうちの第１の測距結果に含まれる測距点から補正対象として注目する第１の測距点を選択すると共に、前記第１の測距点の前記測距対象中の位置と同じ位置を測距した可能性のある第２の測距点を、前記複数回の測距結果のうちの前記第１の測距結果とは異なる第２の測距結果から選択する選択部と、
   前記複数の測距点の各々に対応付けられた画素の特徴量を用いて、前記複数の測距点のうち前記測距対象中の同じ位置の測距結果を推定することにより、前記測距対象の移動軌跡を推定する推定部であって、前記第１の測距点に対応付けられた画素の特徴量と、前記第２の測距点に対応付けられた画素の特徴量の差が閾値を下回ると、前記移動軌跡は前記第１の測距点と前記第２の測距点を通ると推定する前記推定部と、
   前記移動軌跡を用いて、前記測距センサでの前記測距対象の測距結果を、特定の時刻での測距結果に補正する補正処理部
   を備えることを特徴とするセンサデータ補正装置。
2. 前記特定部は、
   前記カメラでの前記測距対象の撮像により得られた第１のカメラフレーム中から、前記第１の測距点と前記第２の測距点を結ぶ空間ベクトルと前記第１のカメラフレームの撮像時刻を用いて、前記第１の測距点に対応する画素を特定し、
   前記カメラでの前記測距対象の撮像により得られ、前記第１のカメラフレームとは異なる第２のカメラフレーム中から、前記空間ベクトルと前記第２のカメラフレームの撮像時刻を用いて、前記第２の測距点に対応する画素を特定する
   ことを特徴とする請求項１に記載のセンサデータ補正装置。
3. 前記測距対象の移動のパターンを記録した遷移モデルを記憶する記憶部と、
   前記遷移モデルを用いて、前記第１の測距点の前記測距対象中の位置と同じ位置を測距した可能性のある測距点の存在する存在範囲を決定する決定部
   をさらに備え、
   前記選択部は、前記存在範囲に含まれる測距点から前記第２の測距点を選択する
   ことを特徴とする請求項１または２に記載のセンサデータ補正装置。
4. 前記補正処理部が前記測距センサでの測距結果を前記第１のカメラフレームの撮像時刻を基準として補正する場合、前記第１のカメラフレーム中の画素の特徴量が所定範囲内に含まれている１つ以上の領域を生成する生成部と、
   前記１つ以上の領域のうち補正後の測距点を含む領域を平面とみなしたときの当該平面を表わす平面方程式を、前記補正後の測距点の座標を用いて算出する算出部と、
   前記平面方程式から計算した前記領域中の画素の座標を、前記補正処理部による補正結果を補間するデータとして、前記補正結果に追加する追加処理部
   をさらに備えることを特徴とする請求項２または３に記載のセンサデータ補正装置。
5. 測距対象を時系列走査することにより測距する測距センサと、前記測距センサより解像度の高いカメラとからのデータを取得可能なセンサデータ補正装置が、
   前記測距センサでの複数回の測距結果から選択した複数の測距点の各々に対して、前記カメラでの前記測距対象の複数回の撮像結果から、当該測距点に対応する画素を特定し、
   前記複数回の測距結果のうちの第１の測距結果に含まれる測距点から補正対象として注目する第１の測距点を選択すると共に、前記第１の測距点の前記測距対象中の位置と同じ位置を測距した可能性のある第２の測距点を、前記複数回の測距結果のうちの前記第１の測距結果とは異なる第２の測距結果から選択し、
   前記複数の測距点の各々に対応付けられた画素の特徴量を用いて、前記複数の測距点のうち前記測距対象中の同じ位置の測距結果を推定することにより、前記測距対象の移動軌跡を推定し、
   前記移動軌跡の推定において、前記第１の測距点に対応付けられた画素の特徴量と、前記第２の測距点に対応付けられた画素の特徴量の差が閾値を下回ると、前記移動軌跡は前記第１の測距点と前記第２の測距点を通ると推定し、
   前記移動軌跡を用いて、前記測距センサでの前記測距対象の測距結果を、特定の時刻での測距結果に補正する
   ことを特徴とするセンサデータ補正方法。
6. 測距対象を時系列走査することにより測距する測距センサと、前記測距センサより解像度の高いカメラとからのデータを取得可能なセンサデータ補正装置に、
   前記測距センサでの複数回の測距結果から選択した複数の測距点の各々に対して、前記カメラでの前記測距対象の複数回の撮像結果から、当該測距点に対応する画素を特定し、
   前記複数回の測距結果のうちの第１の測距結果に含まれる測距点から補正対象として注目する第１の測距点を選択すると共に、前記第１の測距点の前記測距対象中の位置と同じ位置を測距した可能性のある第２の測距点を、前記複数回の測距結果のうちの前記第１の測距結果とは異なる第２の測距結果から選択し、
   前記複数の測距点の各々に対応付けられた画素の特徴量を用いて、前記複数の測距点のうち前記測距対象中の同じ位置の測距結果を推定することにより、前記測距対象の移動軌跡を推定し、
   前記移動軌跡の推定において、前記第１の測距点に対応付けられた画素の特徴量と、前記第２の測距点に対応付けられた画素の特徴量の差が閾値を下回ると、前記移動軌跡は前記第１の測距点と前記第２の測距点を通ると推定し、
   前記移動軌跡を用いて、前記測距センサでの前記測距対象の測距結果を、特定の時刻での測距結果に補正する
   処理を行わせることを特徴とするセンサデータ補正プログラム。

Images