JP2020085798A - 三次元位置検出装置、三次元位置検出システム、及び三次元位置検出方法 - Google Patents

Abstract

【課題】対象物の正確な三次元位置を検出すること。【解決手段】開示の技術の一態様に係る三次元位置検出装置は、所定の回転軸周りに回転する回転機構部と、前記回転軸上に配置され、前記回転機構部による回転角度毎で、ＬＩＤＡＲ（Light Detection And Ranging）法により対象物の第１の三次元位置を検出するＬＩＤＡＲ検出部と、前記回転軸と交差する方向に前記回転軸から離れて配置され、前記回転機構部により回転されながら、前記対象物の画像を撮像する撮像部と、前記回転機構部による前記回転角度毎で撮像された２以上の前記画像に基づいて前記対象物の第２の三次元位置を検出する画像検出部と、前記第１の三次元位置と、前記第２の三次元位置との比較により取得される前記対象物の三次元位置情報を出力する三次元位置出力部と、を有する。【選択図】図４

Description

本願は、三次元位置検出装置、三次元位置検出システム、及び三次元位置検出方法に関する。

従来、発光素子等から対象物に光を投光した投光タイミングと、対象物からの反射光を受光した受光タイミングとの時間差から、対象物までの距離を測定するＴＯＦ（Time of Flight）法が知られている。

ＴＯＦ法として、レーザ光源から射出されたレーザ光を回転ミラーで走査し、対象物で反射又は散乱された光を、再度回転ミラーを介して受光素子で検出することで、所望の範囲の対象物の有無や、対象物の三次元位置を取得する走査型ＬＩＤＡＲ（Light Detection and Ranging）装置が航空機や鉄道、車載等で広く使用されている。

また、走査型ＬＩＤＡＲ装置として、受光素子の出力電流に基づく電圧信号（受光信号）に、オフセット量が時間的に変化するオフセット信号を加算することにより、三次元位置等の対象物に関する情報の検出精度を向上させる装置が開示されている（例えば、特許文献１参照）。

しかし特許文献１の装置では、太陽光や回路の電磁波ノイズ等に起因した受光信号のノイズにより、検出される対象物の三次元位置情報に正確でないものが含まれる場合があった。

本発明は、上記の点に鑑みてなされたものであって、対象物の正確な三次元位置を検出することを課題とする。

開示の技術の一態様に係る三次元位置検出装置は、所定の回転軸周りに回転する回転機構部と、前記回転軸上に配置され、前記回転機構部による回転角度毎で、ＬＩＤＡＲ（Light Detection And Ranging）法により対象物の第１の三次元位置を検出するＬＩＤＡＲ検出部と、前記回転軸と交差する方向に前記回転軸から離れて配置され、前記回転機構部により回転されながら、前記対象物の画像を撮像する撮像部と、前記回転機構部による前記回転角度毎で撮像された２以上の前記画像に基づいて前記対象物の第２の三次元位置を検出する画像検出部と、前記第１の三次元位置と、前記第２の三次元位置との比較により取得される前記対象物の三次元位置情報を出力する三次元位置出力部と、を有する。

本発明の一実施形態によれば、対象物の正確な三次元位置を検出することができる。

第１の実施形態に係る三次元位置検出装置の構成の一例を説明する図であり、（ａ）は斜視図であり、（ｂ）は上面図であり、（ｃ）は側面図である。 第１の実施形態に係るＬＩＤＡＲ検出部の構成の一例を説明するブロック図である。 第１の実施形態に係る処理部のハードウェア構成の一例を説明するブロック図である。 第１の実施形態に係る処理部の機能構成の一例を説明するブロック図である。 第１の実施形態に係る全方位カメラの撮像画像の一例を説明する図である。 ＬＩＤＡＲ検出部と全方位カメラの位置合わせ処理の一例を説明する図である。 第１及び第２の三次元位置情報の座標空間を合わせる処理の一例を説明する図である。 三次元位置比較部による処理の一例を説明する図である。 第１の実施形態に係る三次元位置検出装置の動作の一例を示すフローチャートである。 第１の実施形態に係る三次元位置検出装置による検出結果の一例を示す図である。 第２の実施形態に係る三次元位置検出システムによる検出方法の一例を説明する図である。 第２の実施形態に係る三次元位置検出システムの機能構成の一例を説明するブロック図である。 第２の実施形態に係る三次元位置検出システムの動作の一例を示すフローチャートである。

以下、図面を参照して発明を実施するための形態について説明する。なお、各図面において、同一構成の部分には同一符号を付し、重複した説明を省略する場合がある。

［第１の実施形態］
＜三次元位置検出装置の構成＞
図１は、本実施形態に係る三次元位置検出装置の構成の一例を説明する図である。（ａ）は斜視図であり、（ｂ）は上面図であり、（ｃ）は側面図である。

図１に示すように、三次元位置検出装置１は、回転ステージ２と、回転ステージ２上に配置されたＬＩＤＡＲ（Light Detection And Ranging）検出部３と、ＬＩＤＡＲ検出部３の筐体上に配置された全方位カメラ４とを有する。

回転ステージ２は、回転機構部の一例であり、回転ステージ２上に搭載されたＬＩＤＡＲ検出部３及び全方位カメラ４を、Ａ軸（所定の回転軸の一例）周りに回転させることができる。

ＬＩＤＡＲ検出部３は、回転ステージ２のＡ軸上に固定され、回転ステージ２の回転に伴ってＡ軸周りに検出方向を変化させながら、各検出方向に存在する対象物の三次元位置を検出することができる。

ここで、ＬＩＤＡＲ検出部３は、自機から検出方向にある対象物までの距離を検出する走査型レーザレーダである。ＬＩＤＡＲ検出部３は、対象物に向けて走査光を投光し、走査光を投光してから、走査光が対象物に反射（散乱）された反射光を受光するまでの時間である光飛行時間に基づいて、自機から対象物までの距離を検出することができる。

図１（ａ）に示す破線の矢印３１０は、走査光の走査方向を示し、３１１は走査光を示している。例えば走査光のうちのレーザ光３１１ａの反射光から、レーザ光３１１ａの方向にある対象物の距離を検出することができ、またレーザ光３１１ｂの反射光から、レーザ光３１１ｂの方向にある対象物の距離を検出することができる。

なお、図１（ａ）に示すＬＩＤＡＲ検出部３は、Ａ軸に平行な方向（Ｙ方向）にレーザ光を走査し、Ａ軸と直交する方向にはレーザ光を広げて投光することで、直交２方向の走査範囲にある対象物にレーザ光を照射する１軸走査方式のＬＩＤＡＲ装置である。

但し、これに限定されるものではなく、ＬＩＤＡＲ検出部３を、直交２方向に走査する２軸走査方式のＬＩＤＡＲ装置で構成してもよい。２軸走査方式にすることで、Ａ軸と直交する方向においても、対象物に照射するレーザ光を集光することができるため、反射光の光強度を大きくでき、距離検出の精度を向上させることができる。このような直交２方向に走査する構成は、「直交する２軸方向に光を走査する光走査部」の一例である。

このＬＩＤＡＲ検出部３の構成の詳細については、別途、図２を用いて説明する。

一方、全方位カメラ４は、「撮像部」の一例であり、３６０度の全方位の画像である全方位画像を１台で一度に撮像できるカメラである。全方位カメラ４は、図１（ｃ）に示すように、ＬＩＤＡＲ検出部３の筐体上に配置され、回転ステージ２の回転に伴ってＡ軸周りに撮像方向（角度）を変化させながら、対象物の画像を撮像することができる。

また、全方位カメラ４は、Ａ軸と交差する方向においてＡ軸から離れた位置に配置されているため、回転ステージ２の回転に伴い、角度だけでなく位置も変化させることができる。これにより、全方位カメラ４は、回転ステージ２の回転に伴い、視差のある画像を撮像することができる。

なお、全方位カメラ４は全方位の画像を撮像できるため、全方位カメラ４の備える撮像レンズの光軸の向きは、必ずしもＬＩＤＡＲ検出部３の検出方向と合わせなくてもよく、任意の方向に向けて全方位カメラ４を配置してもよい。但し、図１では分かりやすいように、撮像レンズの光軸方向とＬＩＤＡＲ検出部３の検出方向とが合うように示している。

また、図中のＹ方向はＡ軸と平行の方向を示し、Ｚθ方向は回転ステージ２の回転に伴ってＡ軸周りに変化するＬＩＤＡＲ検出部３の検出方向及び全方位カメラ４の撮像方向を示している。以降の図で示すＹ方向、Ｚθ方向においても同様である。

＜ＬＩＤＡＲ検出部の構成＞
次に、三次元位置検出装置１の備えるＬＩＤＡＲ検出部３の構成について説明する。図２は、ＬＩＤＡＲ検出部の構成の一例を説明するブロック図である。

図２に示すように、ＬＩＤＡＲ検出部３は、投光系３１と、受光光学系３３と、検出系３４と、時間計測部３４５と、同期系３５と、測定制御部３４６と、三次元位置検出部３４７とを有する。

投光系３１は、光源としてのＬＤ（Laser Diode）と、ＬＤ駆動部３１２と、投光光学系３２とを有する。ＬＤは、ＬＤ駆動部３１２から出力される駆動電流に応じてパルス状のレーザ光を出力する半導体素子であり、一例として、端面発光レーザ等である。

ＬＤ駆動部３１２は、測定制御部３４６からのＬＤ駆動信号に応じてパルス状の駆動電流を出力する回路であり、駆動電流を蓄積するコンデンサ、コンデンサとＬＤ２１との導通／非導通を切り換えるトランジスタ、電源等から構成される。

投光光学系３２は、ＬＤ２１から出力されたレーザ光を調光する光学系であり、レーザ光を平行化させるカップリングレンズ、レーザ光の進行方向を変化させる偏向器としての回転ミラー等から構成される。投光光学系３２から出力されたパルス状のレーザ光が走査光となる。

受光光学系３３は、走査範囲に投光された走査光が対象物に反射された反射光を受光するための光学系であり、集光レンズ、平行化レンズ等から構成される。

検出系３４は、反射光を光電変換し、光飛行時間を算出するための電気信号を生成する電気回路である。検出系３４は、時間測定ＰＤ（Photodiode）３４２及びＰＤ出力検出部３４３を含んでいる。

時間測定ＰＤ３４２は、反射光の光量に応じた電流（検出電流）を出力するフォトダイオードである。ＰＤ出力検出部３４３は、時間測定ＰＤ３１からの検出電流に応じた電圧（検出電圧）を生成するＩ／Ｖ変換回路等から構成される。

同期系３５は、走査光を光電変換し、走査光の投光タイミングを調整するための同期信号を生成する電気回路である。同期系３５は、同期検知ＰＤ３５４及びＰＤ出力検出部３５６を含んでいる。同期検知ＰＤ３５４は、走査光の光量に応じた電流を出力するフォトダイオードである。ＰＤ出力検出部３５６は、同期検知ＰＤ３５４からの電流に応じた電圧を利用して同期信号を生成する回路である。

時間計測部３４５は、検出系３４により生成された電気信号（検出電圧等）及び測定制御部３４６により生成されたＬＤ駆動信号に基づいて光飛行時間を計測する回路であり、例えばプログラムにより制御されるＣＰＵ（Central Processing Unit）、適宜なＩＣ（Integrated Circuit）等により構成される。

時間計測部３４５は、ＰＤ出力検出部３５６からの検出信号（ＰＤ出力検出部３５６での受光信号の検出タイミング）に基づいて、時間計測用ＰＤ３４２での受光タイミングを求め、受光タイミングとＬＤ駆動信号の立ち上がりタイミングとに基づいて対象物までの往復時間を計測する。そして、時間計測部３４５は、対象物までの往復時間を、時間計測結果として測定制御部３４６に出力する。

測定制御部３４６は、時間計測部３４５からの時間計測結果を距離に変換することで、対象物までの往復距離を算出し、往復距離の１／２を距離データとして三次元位置検出部３４７に出力する。

三次元位置検出部３４７は、測定制御部３４６からの１走査又は複数の走査で取得された複数の距離データに基づいて、対象物が存在する三次元位置を検出し、三次元位置情報を測定制御部３４６に出力する。測定制御部３４６は、三次元位置検出部３４７からの三次元位置情報を処理部１００に転送する。ここで、ＬＩＤＡＲ検出部３による三次元位置は、「第１の三次元位置」の一例であり、以下では第１の三次元位置という。

また、測定制御部３４６は、処理部１００からの測定制御信号（例えば、測定開始信号や測定停止信号等）を受けて、測定の開始や停止を行なうことができる。

なお、ＬＩＤＡＲ検出部３には、特開２０１７−１６１３７７号公報に記載されたもの等を適用することができるため、ここでは、さらに詳細な説明は省略する。

＜処理部のハードウェア構成＞
次に、三次元位置検出装置１の備える処理部１００のハードウェア構成について説明する。図３は、処理部のハードウェア構成の一例を説明するブロック図である。

処理部１００は、ＣＰＵ（Central Processing Unit）１０１と、ＲＯＭ（Read Only Memory）１０２と、ＲＡＭ（Random Access Memory）１０３と、ＳＳＤ（Solid State Drive）１０４と、入出力Ｉ／Ｆ（Interface）１０５とを有している。それぞれはシステムバスＢで相互に接続されている。

ＣＰＵ１０１は、ＲＯＭ１０２やＳＳＤ１０４等の記憶装置からプログラムやデータをＲＡＭ１０３上に読み出し、処理を実行することで、処理部１００全体の制御や、後述する機能を実現する演算装置である。なお、ＣＰＵ１０１の有する機能の一部、又は全部を、ＡＳＩＣ（application specific integrated circuit）やＦＰＧＡ（Field-Programmable Gate Array）といったハードウェアにより実現させてもよい。

ＲＯＭ１０２は、電源を切ってもプログラムやデータを保持することが可能な不揮発性の半導体メモリ（記憶装置）である。ＲＯＭ１０２には、処理部１００の起動時に実行されるＢＩＯＳ（Basic Input／Output System）、ＯＳ設定、及びネットワーク設定等のプログラムやデータが格納されている。ＲＡＭ１０３は、プログラムやデータを一時保持する揮発性の半導体メモリ（記憶装置）である。

ＳＳＤ１０４は、処理部１００による処理を実行するプログラムや各種データが記憶された不揮発性メモリである。なお、ＳＳＤはＨＤＤ（Hard Disk Drive）であってもよい。

入出力Ｉ／Ｆ１０５は、ＰＣ（Personal Computer）や映像機器等の外部機器と接続するためのインターフェースである。

＜処理部の機能構成＞
次に、処理部１００の機能構成について説明する。図４は、処理部の機能構成の一例を説明するブロック図である。

図４に示すように、処理部１００は、回転制御部１１１と、ＬＩＤＡＲ制御部１１２と、ステレオ検出部１１３と、座標空間合わせ部１１４と、三次元位置比較部１１５と、三次元位置出力部１１６とを有している。

回転制御部１１１は、回転ステージ２に電気的に接続され、回転ステージ２の回転を制御する機能を有する。回転制御部１１１は、制御信号に応じて駆動電圧を出力する電気回路等により実現することができる。

ＬＩＤＡＲ制御部１１２は、ＬＩＤＡＲ検出部３に電気的に接続され、測定制御部３４６に測定制御信号を出力して、測定の開始や停止を制御することができる。

ステレオ検出部１１３は、全方位カメラ４に電気的に接続され、Ａ軸周りの回転角度毎での全方位カメラ４の撮像画像を入力することができる。上述したように、これらの画像には視差が含まれるため、ステレオ検出部１１３は、ステレオマッチングにより検出した視差に基づき三次元位置を検出し、三次元位置情報をＲＡＭ１０３等に記憶させることができる。

ここで、ステレオ検出部１１３の検出した三次元位置は、「第２の三次元位置」の一例であり、以下では第２の三次元位置という。またステレオ検出部１１３は「画像検出部」の一例である。

なお、ステレオマッチングには、ブロックマッチング法やセミグローバルマッチング法等の公知の技術を適用できるため、ここでは、詳細な説明は省略する。

また、全方位カメラ４で撮像した画像に基づき、ステレオマッチングに代えて、複数枚の画像から対象の形状を復元するＳＦＭ（Structure From Motion）法を適用して、対象物の三次元位置を検出してもよい。ＳＦＭ法も公知のものを適用可能であるため、詳細な説明は省略する。

座標空間合わせ部１１４は、ＬＩＤＡＲ検出部３に電気的に接続され、ＬＩＤＡＲ検出部３による第１の三次元位置情報を入力し、また、ステレオ検出部１１３による第２の三次元位置情報を入力して、第１及び第２の三次元位置情報の座標空間を合わせる処理を実行することができる。また、座標空間合わせ部１１４は、座標空間を合わせた第１及び第２の三次元位置情報をＲＡＭ１０３等に記憶させることができる。

三次元位置比較部１１５は、座標空間が合わせられた第１及び第２の三次元位置情報を比較し、第１の三次元位置情報から正確であると推定される三次元位置情報を選択して、三次元位置出力部１１６に出力する機能を有する。

三次元位置出力部１１６は、三次元位置比較部１１５から入力した三次元位置情報を出力することができる。

ここで、図５は、本実施形態に係る全方位カメラの撮像画像の一例を説明する図である。図５において、回転ステージ２は太い矢印５３で示した方向に回転し、回転に伴ってＡ軸周りに全方位カメラ４の撮像方向Ｚθは変化する。回転ステージ２の所定の回転角度毎で、全方位カメラ４により対象物の画像が撮像される。図５の画像５２ａ〜５２ｄは、それぞれ所定の回転角度毎で、全方位カメラ４により撮像される画像の一例である。

上述したように、全方位カメラ４は、Ａ軸と交差する方向にＡ軸から離れて配置されているため、図５に示すように円を描くように回転し、画像５２ａ〜５２ｄには、回転半径と回転角度にて決定される視差が含まれる。ステレオ検出部１１３は、この視差を利用して、ステレオマッチングにより対象物の三次元位置を検出することができる。

次に、座標空間合わせ部１１４による処理について説明する。

ここで、第１の三次元位置は、ＬＩＤＡＲ検出部３の配置位置を基準とした三次元位置であり、第２の三次元位置は、全方位カメラ４の配置位置を基準とした三次元位置である。上述したように、回転ステージ２上で、ＬＩＤＡＲ検出部３はＡ軸上に配置され、全方位カメラ４はＡ軸と交差する方向にＡ軸から離れて配置されている。

従って、第１の三次元位置と第２の三次元位置は基準位置が異なり、座標空間が異なっている。そのため、座標空間合わせ部１１４は、第２の三次元位置情報の座標空間を、第１の三次元位置情報の座標空間に合わせる処理を実行する。

図６は、ＬＩＤＡＲ検出部と全方位カメラの位置合わせ処理の一例を説明する図である。なお、この処理は、座標空間合わせ部１１４が実行する処理の１つである。

図６において、Ａ軸に交差する方向での全方位カメラ４のＡ軸からの距離をｔとし、また、Ｙ方向におけるＬＩＤＡＲ検出部３の中心位置と全方位カメラ４の光軸との距離をｈとする。

一方、回転ステージ２の回転原点におけるＬＩＤＡＲ検出部３の検出方向及び全方位カメラ４の撮像方向をＺ０とし、回転ステージ２の回転角度がθの時のＬＩＤＡＲ検出部３の検出方向及び全方位カメラ４の撮像方向をＺθとする。

これらの関係からＬＩＤＡＲ検出部３の中心に対する全方位カメラ４の光軸位置は次の（１）式で表すことができる。

また、図７は、第１及び第２の三次元位置情報の座標空間を合わせる処理の一例を説明する図である。なお、この処理も座標空間合わせ部１１４が実行する処理の１つである、
図７において、全方位カメラ４の撮像画像５３ａの座標をｕ、ｖとすると、全方位カメラ４による第２の三次元位置の座標空間（ｘ、ｙ、ｚ）は、次の（２）式で表すことができる。

ここで、（２）式において、ｕ０、ｖ０は撮像画像の中心座標を表し、例えば撮像画像の画素数が１９２０×１０８０画素の場合、（ｕ０、ｖ０）＝（９６０、５４０）となる。またｆは全方位カメラ４の焦点距離である。

（１）式及び（２）式と、回転ステージ２による回転角度θから、次の（３）式を用いて、第２の三次元位置情報の座標空間を変換し、第１の三次元位置情報の座標空間に合わせることができる。

なお、（３）式におけるＴは転置行列を示している。

次に、図８は、三次元位置比較部による処理の一例を説明する図である。

ここで、ＬＩＤＡＲ検出部３により検出される第１の三次元位置は、太陽光等に起因したショットノイズで距離を誤検出する場合がある。つまり、正確な距離を検出できるが、正確でない距離が含まれる場合もある。

そこで、三次元位置比較部１１５は、第１の三次元位置情報と、座標空間を合わせた後の第２の三次元位置情報とを比較し、第２の三次元位置情報に距離検出値が近い第１の三次元位置情報を、正確であると推定される三次元位置情報として選択する。

図８において、第１の三次元位置情報８１１には、第２の三次元位置情報８２１が近い距離検出値として存在する。一方、第１の三次元位置情報８１２には、第２の三次元位置情報に近い距離検出値が存在しない。

そのため、三次元位置比較部１１５は、第１の三次元位置情報８１１のみを、正確であると推定される三次元位置情報として選択する。近い距離検出値があるか否かの判定では、一例として、予め定められた閾値を用い、第１の三次元位置情報と第２の三次元位置情報の差が閾値を下回る場合に、近い距離検出値が存在すると判定することができる。

＜三次元位置検出装置の動作＞
次に、本実施形態に係る三次元位置検出装置１の動作について説明する。

図９は、本実施形態に係る三次元位置検出装置の動作の一例を示すフローチャートである。

先ず、ステップＳ９１において、ＬＩＤＡＲ検出部３は、回転ステージ２の所定の回転角度（回転原点等）で第１の三次元位置を検出する。検出された第１の三次元位置情報は、ＲＡＭ１０３等に出力され、記憶される。

続いて、ステップＳ９２において、全方位カメラ４は、対象物が含まれる画像を撮像する。撮像された画像情報は、ＲＡＭ１０３等に出力され、記憶される。

続いて、ステップＳ９３において、回転制御部１１１は、予め定めた全ての回転角度で第１の三次元位置の検出、及び画像の撮像が実行されたかを判定する。

ステップＳ９３において、全ての回転角度で第１の三次元位置の検出、及び画像の撮像が実行されていないと判定された場合は（ステップＳ９３、Ｎｏ）、ステップＳ９４において、回転制御部１１１は予め定めた所定の回転角度で回転ステージ２を回転させる。その後、ステップＳ９１に戻る。

一方、ステップＳ９３において、全ての回転角度で第１の三次元位置の検出、及び画像の撮像が実行されたと判定された場合は（ステップＳ９３、Ｙｅｓ）、ステップＳ９５において、ステレオ検出部１１３は、回転ステージ２による回転角度毎で、２以上の撮像画像を用いてステレオマッチング処理を実行し、第２の三次元位置を検出する。検出された第２の三次元位置情報は、ＲＡＭ１０３等に出力され、記憶される。

なお、この場合、ステレオ検出部１１３は、回転ステージ２による所定の回転角度において、ＬＩＤＡＲ検出部３により検出された第１の三次元位置情報が２個以上ある場合に限って、ステレオマッチング処理を実行するようにしてもよい。

第２の三次元位置情報は、第１の三次元位置情報から正確であると推定されるものを選択するために用いられるが、所定の回転角度での第１の三次元位置情報が１個のみの場合は、正確である場合が多い。従って、この場合にはステレオマッチング処理の実行を省略することで、演算の処理負荷を低減させるとともに、処理時間を削減することができる。

図９に戻って説明を続けると、ステップＳ９６において、座標空間合わせ部１１４は、ＲＡＭ１０３等に記憶された第１及び第２の三次元位置情報を読み出し、第２の三次元位置情報の座標空間を、第１の三次元位置情報の座標空間に合わせる処理を実行する。回転ステージ２による所定の回転角度毎で、第１及び第２の三次元位置が検出されるため、座標空間合わせ部１１４は、所定の回転角度毎で、座標空間を合わせる処理を実行し、処理結果を三次元位置比較部１１５に出力する。

続いて、ステップＳ９７において、三次元位置比較部１１５は、入力した第１及び第２の三次元位置情報を比較し、正確であると推定される第１の三次元位置情報を選択する処理を実行する。この場合も、ステップＳ９７と同様に、三次元位置比較部１１５は、回転ステージ２による所定の回転角度毎で、三次元位置の比較処理を実行し、処理結果を三次元位置出力部１１６に出力する。

続いて、ステップＳ９８において、三次元位置出力部１１６は、三次元位置比較部１１５から正確であると推定された三次元位置情報を入力し、表示装置やＰＣ（Perspnal Computer）等の外部装置に出力する。

このようにして、三次元位置検出装置１は、三次元位置情報を取得し、出力することができる。図１０は、三次元位置検出装置１による検出結果の一例を示す図である。図１０は、対象物までの距離を各画素の輝度値に置き換えて表示した距離画像である。このようにして三次元位置を検出することができる。

＜三次元位置検出装置の効果＞
以上説明してきたように、本実施形態では、回転ステージ２で回転させながら検出した第１及び第２の三次元位置情報に基づき、正確であると推定された三次元位置情報を検出する。これにより、ショットノイズに起因した正確でない三次元位置情報を、第２の三次元位置情報との比較により、第１の三次元位置情報から除去することができる。そして、対象物の正確な三次元位置を検出することができる。

また、本実施形態によれば、ＬＩＤＡＲ検出部３の誤検出を低減するために、複数回の検出を行ったり、検出値の後処理を行ったりしなくてよい。追加の機能を搭載することがないため、三次元位置検出装置１のコストの低減を図ることができる。

［第２の実施形態］
次に、第２の実施形態に係る三次元位置検出システムについて説明する。なお、既に説明した実施形態と同一の構成部についての説明は省略する。

図１０に三次元位置の検出結果の一例を示したが、図１０に１１０で示した箇所のように、三次元位置検出装置１からみて対象物の裏側は、ＬＩＤＡＲ検出部３から投光される光は届かず、また全方位カメラ４でも撮像することができない場合がある。そのため、このような対象物の裏側は死角となり、三次元位置を検出することができない場合がある。

そこで、本実施形態に係る三次元位置検出システムでは、三次元位置検出装置１の位置を変化させながら、三次元位置検出装置１により複数回の検出を行い、検出結果を合成することで、死角のない三次元位置検出を実現している。

図１１は、本実施形態に係る三次元位置検出システムによる検出方法の一例を説明する図である。図１１に示すように、三次元位置検出装置１_Ｐ１は、第１の位置に設置された三次元位置検出装置１を示し、三次元位置検出装置１_Ｐ２は、第１の位置とは異なる第２の位置に設置された三次元位置検出装置１を示している。なお、三次元位置検出装置１の第１の位置から第２の位置への移動は、直動ステージで行うが、図１１では直動ステージの図示を省略している。

本実施形態に係る三次元位置検出システム１ａは、このように位置を変えながら三次元位置検出装置１による複数回の検出を実行することができる。

＜三次元位置検出システムの構成＞
図１２は、本実施形態に係る三次元位置検出システムの機能構成の一例を示すブロック図である。

図１２に示すように、三次元位置検出システム１ａは、直動ステージ５と、処理部１００ａとを有している。また、処理部１００ａは、三次元位置出力部１１６ａと、位置制御部１２１と、撮像位置取得部１２２と、ＬＩＤＡＲ位置角度取得部１２３と、三次元位置合成部１２４と、合成三次元位置出力部１２５とを有している。

三次元位置出力部１１６ａは、三次元位置比較部１１５から入力した、回転角度毎での三次元位置情報を、ＲＡＭ１０３等に出力し、記憶させることができる。

直動ステージ５は、位置可変部の一例であり、三次元位置検出装置１を搭載したテーブルを移動させることで、三次元位置検出装置１の位置を変化させることができる。なお、直動ステージ５による移動方向の軸数は、１軸や２軸等、何れの数であってもよい。

位置制御部１２１は、直動ステージ５に電気的に接続され、直動ステージ５による三次元位置検出装置１の位置を制御する機能を有する。位置制御部１２１は、制御信号に応じて直動ステージ５に駆動電圧を出力する電気回路等により実現することができる。

撮像位置取得部１２２は、直動ステージ５が変化させた位置毎で全方位カメラ４が撮像した画像に基づき、ＳＦＭ法により全方位カメラ４の位置情報を取得し、取得した位置情報をＬＩＤＡＲ位置角度取得部１２３に出力する機能を有する。

ＳＦＭ法は、上述したように、複数枚のカメラ画像から、個々のカメラの配置位置や三次元空間を推定するための画像処理アルゴリズムである。ＳＦＭ法のアルゴリズムが実装された演算装置は、各画像の特徴点を探索し、画像同士の特徴点の類似度と位置関係をマッピングする処理を実行する。そして、特徴点が最も適切に当てはまる位置を推定することで、個々のカメラの相対位置を決定し、また、個々のカメラの位置関係から特徴点の三次元位置を決定することができる。なお、ＳＦＭ法自体は公知の技術を適用できるため、ここでは詳細な説明は省略する。

一方、ＬＩＤＡＲ位置角度取得部１２３は、入力した全方位カメラ４の位置情報に基づき、ＬＩＤＡＲ検出部３の位置及び角度の情報を取得し、三次元位置合成部１２４に出力する機能を有する。

より詳しくは、ＬＩＤＡＲ位置角度取得部１２３は、先ず、全方位カメラ４の位置情報に基づいて、三次元位置検出装置１の位置（三次元位置検出装置１の装置中心が配置された位置）を特定する。そして、既知である三次元位置検出装置１の装置中心に対するＬＩＤＡＲ検出部３の位置の情報を用いて、ＬＩＤＡＲ検出部３の位置及び角度情報を取得することができる。

三次元位置合成部１２４は、ＬＩＤＡＲ検出部３の位置及び設置角度情報に基づき、三次元位置検出装置１により検出され、ＲＡＭ１０３等に記憶された三次元位置を合成する処理を実行し、合成三次元位置出力部１２５に出力する機能を有する。

合成三次元位置出力部１２５は、入力した三次元位置を表示装置やＰＣ等の外部装置に出力することができる。

＜三次元位置検出システムの動作＞
図１３は、本実施形態に係る三次元位置検出システムの動作の一例を示すフローチャートである。

図１３のステップＳ１３１〜Ｓ１３７の処理は、図９のステップＳ９１〜Ｓ９７の処理と同じであるため、説明を省略する。

ステップＳ１３８において、三次元位置出力部１１６ａは、三次元位置比較部１１５から入力した、回転角度毎での三次元位置情報を、ＲＡＭ１０３等に出力し、記憶させる。

続いて、ステップＳ１３９において、位置制御部１２１は、予め定められた全ての位置で、三次元位置検出装置１による検出が行われたか否かを判定する。

ステップＳ１３９において、全ての位置で三次元位置検出装置１による検出が行われていないと判定された場合は（ステップＳ１３９、Ｎｏ）、ステップＳ１４０において、位置制御部１２１は、直動ステージ５を所定の移動量だけ変化させ、三次元位置検出装置１の位置を変化させる。その後ステップＳ１３１に戻る。

一方、ステップＳ１３９において、全ての位置で三次元位置検出装置１による検出が行われたと判定された場合は（ステップＳ１３９、Ｙｅｓ）、ステップＳ１４１において、撮像位置取得部１２２は、直動ステージ５により変化させた位置毎で全方位カメラ４により撮像され、ＲＡＭ１０３等に記憶された画像に基づき、ＳＦＭ法により全方位カメラ４の位置情報を取得する処理を実行する。そして、取得した全方位カメラ４の位置情報をＬＩＤＡＲ位置角度取得部１２３に出力する。

続いて、ステップＳ１４２において、ＬＩＤＡＲ位置角度取得部１２３は、入力した全方位カメラ４の位置情報に基づき、ＬＩＤＡＲ検出部３の位置及び角度情報を取得し、三次元位置合成部１２４に出力する。

続いて、ステップＳ１４３において、三次元位置合成部１２４は、ＲＡＭ１０３等から位置毎での三次元位置情報を読み出し、ＬＩＤＡＲ検出部３の位置及び角度情報に基づいて、読み出した三次元位置情報を合成する。そして、合成された三次元位置情報を合成三次元位置出力部１２５に出力する。

続いて、ステップＳ１４４において、合成三次元位置出力部１２５は、入力した三次元位置情報を表示装置やＰＣ等の外部装置に出力する。

このようにして、三次元位置検出システム１ａは、変化させた位置毎での三次元位置情報を合成した三次元位置情報を取得し、出力することができる。

＜三次元位置検出システムの効果＞
以上説明してきたように、本実施形態では、直動ステージ５で位置を変化させながら三次元位置検出装置１により検出した三次元位置情報に基づき、合成した三次元位置情報を検出する。これにより、死角のない対象物の正確な三次元位置を検出することができる。

複数の箇所で検出された三次元位置情報を合成する比較例として、例えば、三次元位置情報をメッシュ構造化し、メッシュ同士を比較して構造の近い場所を探して合成する手法や、加速度センサ等から三次元位置情報を検出した箇所のずれを把握し、ずれを解消するように合成する手法等がある。

しかし、メッシュ構造化する手法は、三次元位置情報のデータ間隔が細かい（解像度が高い）とメッシュ化が困難になる場合や、室内空間等の広い空間ではメッシュ化が困難になる場合がある。また、加速度センサ等を用いる手法は、三次元位置検出システムの構成に加速度センサ等を追加する必要があり、システム構成が複雑になり、またコストが増大する場合がある。

本実施形態では、全方位カメラ４で撮像した画像に基づき三次元位置情報を合成するため、高精度、簡単、かつ低コストで、合成した三次元位置情報を取得することができる。

なお、これ以外の効果は、第１の実施形態で説明したものと同様である。

尚、本発明は、具体的に開示された上記の実施形態に限定されるものではなく、特許請求の範囲から逸脱することなく、種々の変形や変更が可能である。

また、本実施形態では、三次元位置検出方法を含む。例えば、三次元位置検出方法は、所定の回転軸周りに回転する回転工程と、前記回転軸上に配置され、前記回転工程による回転角度毎で、ＬＩＤＡＲ法により対象物の第１の三次元位置を検出する工程と、前記回転軸と交差する方向に前記回転軸から離れて配置され、前記回転工程により回転されながら、前記対象物の画像を撮像する工程と、前記回転工程による前記回転角度毎で撮像された２以上の前記画像に基づいて前記対象物の第２の三次元位置を検出する工程と、前記第１の三次元位置と、前記第２の三次元位置との比較により取得される前記対象物の三次元位置情報を出力する工程と、を含む。このような三次元位置検出方法により、上述の三次元位置検出装置と同様の効果を得ることができる。

また上記で説明した実施形態の各機能は、一又は複数の処理回路によって実現することが可能である。ここで、本明細書における「処理回路」とは、電子回路により実装されるプロセッサのようにソフトウェアによって各機能を実行するようプログラミングされたプロセッサや、上記で説明した各機能を実行するよう設計されたASIC(Application Specific Integrated Circuit)、DSP（digital signal processor）、FPGA（field programmable gate array）や従来の回路モジュール等のデバイスを含むものとする。

１ 三次元位置検出装置
１ａ 三次元位置検出システム
２ 回転ステージ
３ ＬＩＤＡＲ検出部
４ 全方位カメラ
５ 直動ステージ
３１ 投光系
３１２ ＬＤ駆動部
３２ 投光光学系
３３ 受光光学系
３４ 検出系
３４２ 時間計測用ＰＤ
３４３ ＰＤ出力検出部
３４５ 時間計測部
３４６ 測定制御部
３４７ 三次元位置検出部
３５ 同期系
３５４ 同期検知用ＰＤ
３５６ ＰＤ出力検出部
５１ 対象物
５２ 撮像画像
１００ 処理部
１０１ ＣＰＵ
１０２ ＲＯＭ
１０３ ＲＡＭ
１０４ ＳＳＤ
１０５ 入出力Ｉ／Ｆ
１１１ 回転制御部
１１２ ＬＩＤＡＲ制御部
１１３ ステレオ検出部
１１４ 座標空間合わせ部
１１５ 三次元位置比較部
１１６、１１６ａ 三次元位置出力部
１２１ 位置制御部
１２２ 撮像位置取得部
１２３ ＬＩＤＡＲ位置角度取得部
１２４ 三次元位置合成部
１２５ 合成三次元位置出力部
Ｂ システムバス

特開２０１７−１６１３７７号公報

Claims (7)

1. 所定の回転軸周りに回転する回転機構部と、
   前記回転軸上に配置され、前記回転機構部による回転角度毎で、ＬＩＤＡＲ（Light Detection And Ranging）法により対象物の第１の三次元位置を検出するＬＩＤＡＲ検出部と、
   前記回転軸と交差する方向に前記回転軸から離れて配置され、前記回転機構部により回転されながら、前記対象物の画像を撮像する撮像部と、
   前記回転機構部による前記回転角度毎で撮像された２以上の前記画像に基づいて前記対象物の第２の三次元位置を検出する画像検出部と、
   前記第１の三次元位置と、前記第２の三次元位置との比較により取得される前記対象物の三次元位置情報を出力する三次元位置出力部と、を有する
   三次元位置検出装置。
2. 前記ＬＩＤＡＲ検出部は、直交する２軸方向に光を走査する光走査部を有する
   請求項１に記載の三次元位置検出装置。
3. 前記画像検出部は、前記２以上の前記画像の視差に基づき、前記第２の三次元位置を検出する
   請求項１、又は２に記載の三次元位置検出装置。
4. 前記撮像部は、全方位画像を撮像する全方位カメラを含む
   請求項１乃至３の何れか１項に記載の三次元位置検出装置。
5. 前記画像検出部は、前記回転機構部による所定の回転角度において、前記ＬＩＤＡＲ検出部による前記第１の三次元位置情報が２個以上ある場合に、前記２以上の前記画像の視差に基づき、前記第２の三次元位置情報を検出する
   請求項３、又は４に記載の三次元位置検出装置。
6. 請求項１乃至５の何れか１項に記載の三次元位置検出装置を搭載し、前記三次元位置検出装置の位置を変化させる位置可変部と、
   前記位置可変部により変化させた位置毎で、前記撮像部により撮像された前記画像に基づき、ＳＦＭ（Structure From Motion）法により前記撮像部の位置を取得する撮像位置取得部と、
   前記回転軸と、前記撮像部との位置関係に基づき、前記ＬＩＤＡＲ検出部の設置された位置及び角度を取得するＬＩＤＡＲ位置角度取得部と、
   前記ＬＩＤＡＲ検出部の前記位置及び前記角度に基づき、合成された前記三次元位置情報を出力する合成三次元位置出力部と、を有する
   三次元位置検出システム。
7. 所定の回転軸周りに回転する回転工程と、
   前記回転軸上に配置され、前記回転工程による回転角度毎で、ＬＩＤＡＲ法により対象物の第１の三次元位置を検出する工程と、
   前記回転軸と交差する方向に前記回転軸から離れて配置され、前記回転工程により回転されながら、前記対象物の画像を撮像する工程と、
   前記回転工程による前記回転角度毎で撮像された２以上の前記画像に基づいて前記対象物の第２の三次元位置を検出する工程と、
   前記第１の三次元位置と、前記第２の三次元位置との比較により取得される前記対象物の三次元位置情報を出力する工程と、を含む
   三次元位置検出方法。