対象物との相対位置計測方法と装置
【課題】向きの変更と移動の少なくとも一方を行う動作体から、対象物との相対位置を高精度に計測できる手段を提供する。
【解決手段】動作体３に設置したレーザ距離センサにより、各被計測点の位置を動作体座標系で表わされた座標値を取得する。動作体３に設置した撮像装置により、対象物５が含まれる領域を撮像して画像を生成する。画像において、対象物５に取り付けられた指標の位置を特定し、この位置に基づいて、撮像時において対象物５の方向を特定する。距離計測時の動作体３の向きと位置に対する撮像時の動作体３の相対的な向きθと位置Δｘ，Δｙに従って、各被計測点の座標値を、距離計測時の動作体座標系の座標値から撮像時の動作体座標系の座標値に変換する。画像内の指標の位置に対応する、変換後の座標値を特定し、この座標値に基づいて、撮像時における動作体３から対象物５までの距離を求める。
【解決手段】動作体３に設置したレーザ距離センサにより、各被計測点の位置を動作体座標系で表わされた座標値を取得する。動作体３に設置した撮像装置により、対象物５が含まれる領域を撮像して画像を生成する。画像において、対象物５に取り付けられた指標の位置を特定し、この位置に基づいて、撮像時において対象物５の方向を特定する。距離計測時の動作体３の向きと位置に対する撮像時の動作体３の相対的な向きθと位置Δｘ，Δｙに従って、各被計測点の座標値を、距離計測時の動作体座標系の座標値から撮像時の動作体座標系の座標値に変換する。画像内の指標の位置に対応する、変換後の座標値を特定し、この座標値に基づいて、撮像時における動作体３から対象物５までの距離を求める。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、向きの変更と移動の少なくとも一方を行う動作体から見た対象物の方向と、動作体から対象物までの距離とを、動作体と対象物の相対位置として計測する対象物との相対位置計測方法と装置に関する。
【背景技術】
【０００２】
屋外のように変化に富んだ多様性のある環境において、例えばロボットである動作体が作業をする時に、ロボットが、対象物との相対位置を検出する必要がある場合がある。
【０００３】
その具体例を説明する。複数のロボットが、広範囲のエリアを分担して農作業や地雷撤去などの作業を行うことがある。この場合に、各ロボットは、他のロボットにより作業が済んだ範囲で作業を行わないようにする目的で、または、他のロボットとの衝突を避ける目的で、互いの相対位置を知る必要がある。
【０００４】
別の具体例を説明する。搬送ロボットが、荷物を持った状態で、人に追従して移動する自動制御を行うことにより、人の移動先まで荷物を搬送することがある。この場合には、搬送ロボットは、人を対象物として、人との相対位置を検出する必要がある。
【０００５】
対象物との相対位置を検出するために、対象物に取り付けた指標を検出している。対象物に取り付ける指標は、例えば、下記の特許文献１〜３に記載されている。
【０００６】
下記の特許文献１に記載された指標は、特定のパターンで輝度が変化する発光ダイオードである。この指標を用いる場合、指標が取り付けられた対象物を含む領域を撮像して得た画像から、発光ダイオードの発光パターンで輝度が変化する画素を特定する。特定した画素に基づいて、対象物の位置や方向を検出することができる。
【０００７】
下記の特許文献２に記載された指標は、直線や四角形や円弧などの幾何学的模様である。この指標を用いる場合、指標が取り付けられた対象物を含む領域を撮像して得た画像において、指標の幾何学的模様の位置を識別する。この位置に基づいて、対象物の位置や方向を検出することができる。
【０００８】
下記の特許文献３に記載された指標は、複数の特徴的な幾何学的模様である。この指標を用いる場合、指標が取り付けられた対象物を含む領域を撮像して得た画像において、指標の一部が他の物体に隠れていても、または、画像の背景部分に似た模様があっても、各幾何学的模様を、その既知の模様と照合することにより、指標における一部の幾何学的模様の位置を検出することができる。この位置に基づいて、対象物の位置や方向を検出することができる。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開２００９−３３３６６号公報
【特許文献２】特開２００７−３２３３号公報
【特許文献３】特開２０１１−２８４１７号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
しかし、画像に基づいて、対象物までの距離を計測する場合には、その計測精度が低くなることがある。対象物までの距離の計測は、画像における指標の寸法に基づいて行われる。しかし、画像における指標の寸法に誤差が生じることがある。特に、距離が大きい場合には、画像内において指標が小さくなるので、計測誤差が大きくなる。例えば、１００ｍ以上の距離から、６０度以上の視野で、縦と横の寸法が０．５ｍ程度の幾何学的模様（指標）を、１００万画素のカメラで撮像したとしても、画像において、幾何学的模様の縦または横の寸法が、５画素程度以下になる。この場合に、指標が有する実際の寸法と、画像における指標の寸法との関係から距離を計算すると、指標の縦または横の寸法に１画素の誤差があれば、距離の計測誤差は２０ｍにもなる。
【００１１】
そこで、本発明の目的は、向きの変更と移動の少なくとも一方を行う動作体から、対象物との相対位置を高精度に計測できる方法と装置を提供することにある。
【００１２】
上記目的を達成するため、本発明によると、向きの変更と移動の少なくとも一方を行う動作体から見た対象物の方向と、動作体から対象物までの距離とを、対象物との相対位置として計測する方法であって、
（Ａ）レーザ距離センサと撮像装置を動作体に設置し、
（Ｂ）レーザ距離センサにより、動作体に設定されている計測基点から各計測方向に存在する物体上の被計測点までの距離を計測し、各被計測点の位置を、動作体に固定された動作体座標系の座標値で表わし、
（Ｃ）撮像装置により、対象物が含まれる領域を撮像して画像を生成し、
（Ｄ）前記画像に基づいて、動作体から見た対象物の方向を取得する方向データ取得処理を行い、
（Ｅ）距離計測時と撮像時との間における動作体の向きと位置の変化量を求める動作変化量取得処理を行い、
（Ｆ）前記画像と前記座標値と前記変化量とに基づいて、撮像時における、動作体から対象物までの距離を取得する距離データ取得処理を行う、ことを特徴とする対象物との相対位置計測方法が提供される。
【００１３】
本発明の好ましい実施形態によると、
方向データ取得処理では、
（Ｄ１）撮像装置が撮像した画像において、対象物に取り付けられた指標の位置を特定し、
（Ｄ２）当該特定した位置に基づいて、撮像装置による撮像時において動作体から見た対象物の方向を特定し、
動作変化量取得処理では、
距離計測時の動作体の向きに対する撮像時の動作体の相対的な向きと、距離計測時の動作体の位置に対する撮像時の動作体の相対的な位置と、を前記変化量として求め、
距離データ取得処理では、
（Ｆ１）求めた相対的な向きと相対的な位置とに従って、前記各被計測点の座標値を、距離計測時の動作体座標系の座標値から撮像時の動作体座標系の座標値に変換し、
（Ｆ２）レーザ距離センサと撮像装置の配置関係に基づいて、前記（Ｄ１）で特定した前記指標の位置に対応する被計測点を、撮像時の動作体座標系において特定し、当該被測定点の位置を表わす前記変換した座標値を特定し、
（Ｆ３）当該特定した座標値に基づいて、撮像時における、動作体から対象物までの距離を求める。
【００１４】
また、上記目的を達成するため、本発明によると、向きの変更と移動の少なくとも一方を行う動作体から見た対象物の方向と、動作体から対象物までの距離とを、対象物との相対位置として計測する装置であって、
動作体に設置され、動作体に設定されている計測基点から各計測方向に存在する物体上の被計測点までの距離を計測し、各被計測点の位置を、動作体に固定された動作体座標系の座標値で表わすレーザ距離センサと、
動作体に設置され、対象物を含む領域を撮像して画像を生成する撮像装置と、
距離計測時と撮像時との間における動作体の向きと位置の変化量を求める動作変化量計測装置と、
前記画像に基づいて、動作体から見た対象物の方向を取得する方向データ取得処理を行い、前記画像と前記座標値と前記変化量とに基づいて、撮像時における、動作体から対象物までの距離を取得する距離データ取得処理を行うデータ処理装置と、を備える、ことを特徴とする対象物との相対位置計測装置が提供される。
【００１５】
上述した本発明によると、撮像した画像における対象物の指標の位置に基づいて、撮像時において動作体から見た対象物の方向を特定するので、対象物の方向を高精度に検出することができる。
対象物までの距離を求めるのに、レーザ距離センサを用いるので、対象物までの距離を高精度に計測することができる。
よって、動作体から見た対象物の方向と動作体から対象物までの距離とを、動作体と対象物の相対位置として高精度に取得できる。
【００１６】
また、前記画像と前記座標値と前記変化量とに基づいて、撮像時における、動作体から対象物までの距離を取得するので、撮像時における動作体と対象物の相対位置を得ることができる。
【図面の簡単な説明】
【００１７】
【図１】本発明の実施形態による対象物との相対位置計測装置を示すブロック図である。
【図２】動作体と対象物を示す簡略斜視図である。
【図３】動作体の向きと位置の変化を示す平面図である。
【図４】本発明の実施形態による対象物との相対位置計測方法を示すフローチャートである。
【図５】対象物の指標が光学特性部である場合の撮像装置の一例を示す。
【発明を実施するための形態】
【００１８】
本発明の好ましい実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００１９】
図１は、本発明の実施形態による対象物との相対位置計測装置１０を示すブロック図である。
対象物との相対位置計測装置１０は、動作体３から見た対象物５の方向と、動作体３から対象物５までの距離とを、動作体３と対象物５の相対位置として計測する。動作体３は、向きの変更と移動の少なくとも一方を行う。
【００２０】
図２は、動作体３と対象物５を示す簡略斜視図である。図２の例では、動作体３と対象物５は、回転駆動される車輪６により路面上を移動する自律移動ロボットである。また、図２の例では、動作体３と対象物５は、同じ構成を有する。すなわち、対象物５は、動作体３と同じ構成を有する別の動作体である。
【００２１】
対象物との相対位置計測装置１０は、動作体３に設けられる。なお、対象物５が、動作体３と同じ構成を有する別の動作体である場合には、対象物５にも、対象物との相対位置計測装置１０が設けられる。この場合、対象物（動作体）５の相対位置計測装置１０は、動作体５と動作体３との相対位置を計測する。
【００２２】
対象物との相対位置計測装置１０は、撮像装置７とレーザ距離センサ９と動作変化量計測装置１１とデータ処理装置１３とを備える。
【００２３】
撮像装置７は、動作体３に設置され、対象物５を含む領域を撮像して画像を生成する。
【００２４】
レーザ距離センサ９は、動作体３に設置される。レーザ距離センサ９は、動作体３に設定されている計測基点から各計測方向に存在する物体上の被計測点までの距離を計測し、各被計測点の位置を、動作体３に固定された動作体座標系の座標値で表わす。
詳しくは、レーザ距離センサ９は、計測基点から各計測方向にレーザ光を射出して当該レーザ光の反射光を検出することにより、該計測方向に存在する物体上の被計測点までの距離を計測する。また、レーザ距離センサ９は、各計測方向と、該方向上にある被計測点までの計測距離とに基づいて、各被計測点の位置を、動作体座標系の座標値で表わす。該座標値は、当該距離計測時の動作体座標系の座標値である。動作体座標系の原点は、計測基点であり、動作体座標系において、原点から見た各方向が、各計測方向である。ただし、レーザ距離センサ９で得られた様々な物体の座標値だけでは、どの座標値が対象物５の位置に相当するか、または、座標値のどの範囲に対象物５に相当する位置が含まれるかを判別することはできない。
【００２５】
レーザ距離センサ９は、例えば、図２のように、鉛直方向を向く走査軸Ｃ回りに光を走査することにより、水平な各計測方向にレーザ光を射出して当該各計測方向において被計測点までの距離を計測し、次いで、走査軸Ｃを鉛直軸から傾けて、走査軸Ｃ回りに光を走査することにより、水平方向から下方または上方に傾いた各計測方向にレーザ光を射出して当該各計測方向において被計測点までの距離を計測してよい。
【００２６】
動作体座標系は、動作体３と一体的に移動する。従って、動作体３が移動する空間に固定されている静止座標系に対して、動作体３が移動する場合に、動作体座標系は、動作体３と一体的に、前記静止座標系に対して移動する。
同様に、動作体３が前記静止座標系に対して向きを変えると、動作体座標系は、動作体３と一体的に、前記静止座標系に対して向きを変える。例えば、動作体座標系と静止座標系の各々が、互いに直交するｘ軸とｙ軸とｚ軸とを有する直交座標系であり、動作体３の向きが動作体座標系のｘ軸の向きと一致する場合には、動作体３の向きが、静止座標系のｘ軸の向きから静止座標系のｙ軸の向きに変化すると、動作体座標系のｘ軸も、静止座標系のｘ軸の向きから静止座標系のｙ軸の向きに変化する。
なお、本願において、動作体３の向きとは、動作体３に設定された第１固定点から、この第１固定点と間隔をおいて動作体３に設定されて第２固定点に向かう方向である。動作体３の向きは、動作体３の後方から前方へ向かう向きであってよい。
【００２７】
動作変化量計測装置１１は、距離計測時と撮像時との間における動作体３の向きと位置の変化量を求める。
詳しくは、動作変化量計測装置１１は、レーザ距離センサ９による距離計測時の動作体３の向きに対する、撮像装置７による撮像時の動作体３の相対的な向きと、距離計測時の動作体３の位置に対する撮像時の動作体３の相対的な位置と、を前記変化量として求める。なお、本実施形態では、レーザ距離センサ９により距離計測を行った後、撮像装置７により撮像を行っている。ただし、撮像装置７により撮像を行った後、レーザ距離センサ９により距離計測を行ってもよい。
【００２８】
動作変化量計測装置１１は、計測部１１ａと演算部１１ｂとを有する。
【００２９】
計測部１１ａは、距離計測時と撮像時との間の各時点および距離計測時と撮像時において、動作体３の向きと動作体３の移動速度を計測する。計測部１１ａは、例えば、動作体３が、路面上を転動することにより移動する車輪６を有する場合には、動作変化量計測装置１１は、車輪６の回転速度を計測し、回転速度から動作体３の移動速度を演算する。また、計測部１１ａは、動作体３に設置されたジャイロセンサを用いて、動作体３の向き（進行方向）を検出する。
【００３０】
演算部１１ｂは、計測した各時点における動作体３の向きと移動速度とに基づいて、距離計測時の動作体３の向きに対する、撮像時の動作体３の相対的な向きと、距離計測時の動作体３の位置に対する撮像時の動作体３の相対的な位置と、を求める。
【００３１】
データ処理装置１３は、撮像装置７により得た画像に基づいて、動作体３から見た対象物５の方向を取得する方向データ取得処理を行い、撮像装置７により得た前記画像とレーザ距離センサ９により得た前記座標値と動作変化量計測装置１１により得た前記変化量とに基づいて、撮像時における、動作体３から対象物５までの距離を取得する距離データ取得処理を行う。方向データ取得処理は、データ処理装置１３の方向データ取得処理部１３ａにより行われ、距離データ取得処理は、データ処理装置１３の距離データ取得処理部１３ｂにより行われる。
【００３２】
方向データ取得処理は、次の（ａ）（ｂ）からなる。
【００３３】
（ａ）撮像装置７が撮像した画像において、対象物５に取り付けられた指標の位置を特定する。
【００３４】
（ｂ）次いで、当該特定した位置に基づいて、撮像装置７による撮像時において動作体３から見た対象物５の方向を特定する。すなわち、撮像装置７（カメラ）の既知の向きと、撮像した画像の中心位置と、画像における指標の位置とに基づいて、撮像時に、動作体３から見た対象物５の方向を特定する。
【００３５】
指標は、図２の例では、対象物５の外周面５ａに、その周方向全体にわたって、連続してまたは間隔をおいて取り付けられている。外周面５ａは、鉛直軸回りの周方向に延びて１周している。なお、符号３ａは、指標が取り付けられる動作体３の外周面を示し、この外周面３ａも、鉛直軸回りの周方向に延びて１周している。
【００３６】
指標として、次のものを用いてよい。
一例では、指標は、特許文献１のように、特定のパターンで輝度が変化する発光ダイオードである。この場合、撮像装置７は、短い設定時間にわたる動画を画像として撮像し、画像において、特定のパターンで輝度が変化する画素の位置を、指標の位置として特定する。この場合、撮像時は、動画を構成する一連の画像のうちのいずれかの指定画像を撮像した時であってよく、当該指定画像を後述の処理（ｄ）で使用してよい。
別の例では、指標は、特許文献２、３に記載されたような幾何学的模様である。この場合、幾何学的模様と同じ模様を予め設定しておき、画像において、この設定模様と同じ模様の部分を、指標の位置として特定する。
【００３７】
また、対象物５において、指標の位置または指標と隣接する位置に、レーザ光を高反射率（例えば、８０％以上）で反射する反射シートを設けてもよい。これにより、レーザ距離センサ９で、指標の位置または指標と隣接する位置を被計測点として計測できなくなる確率が減る。
【００３８】
距離データ取得処理は、次の（ｃ）（ｄ）（ｅ）からなる。
【００３９】
（ｃ）動作変化量計測装置１１が求めた相対的な向きと相対的な位置とに従って、レーザ距離センサ９が距離計測時に計測した前記各被計測点の前記座標値を、撮像時における動作体座標系の座標値に座標変換する。
【００４０】
この座標変換の一例を説明する。動作体３が、水平な２次元平面である路面上を移動し、かつ、対象物５がこの路面上に存在し、動作体３と対象物５の位置を、互いに直交し水平方向を向くｘ軸とｙ軸を有する当該２次元平面の静止座標系Ｓ０で表わすとする。この場合に、図３（Ａ）に示すように、動作体３が、距離計測時に、位置Ｐ１において方向Ｄ１を向いており、その後の撮像時に、位置Ｐ２において方向Ｄ２を向いている場合には、次の［数１］により、レーザ距離センサ９が距離計測時に計測した前記各被計測点の前記座標値を、撮像時における動作体座標系の座標値に座標変換する。
【００４１】
【数１】
【００４２】
ここで、右辺のｘｉとｙｉは、レーザ距離センサ９が距離計測を行った時の動作体座標系における各被計測点の前記座標値である。左辺のｘｉとｙｉは、撮像装置７が撮像を行った時の動作体座標系における各被計測点の座標値である。右辺と左辺のｘｉとｙｉの添え字ｉは、被計測点の識別番号であり、被計測点がｎ個ある場合には、１〜ｎまでの値をとり、ｘｉとｙｉはｎ組だけ存在する。右辺のΔｘは、上述した２次元平面の静止座標系Ｓ０における位置Ｐ１のｘ座標と位置Ｐ２のｘ座標の差であり、右辺のΔｙは、上述した２次元平面の静止座標系Ｓ０における位置Ｐ１のｙ座標と位置Ｐ２のｙ座標の差である。また、右辺のθは、距離計測時の動作体３が向いている方向Ｄ１に対する、撮像時の動作体３が向いている方向を示す角度θである。
なお、動作体座標系は、この例では２次元座標系であるが、３次元座標系であってもよい。
【００４３】
（ｄ）レーザ距離センサ９と撮像装置７の配置関係に基づいて、前記（ａ）で特定した前記指標の位置に対応する被計測点を、撮像時の動作体座標系において特定し、当該被測定点の位置を、撮像時の動作体座標系において表わす前記変換した座標値を特定する。
距離計測時と撮像時が同じであるとすると、上述の各計測方向が、撮像装置７により撮像した画像のどの位置に一致するかは、レーザ距離センサ９と撮像装置７の配置関係から分かる。従って、この配置関係に基づいて、距離計測時と撮像時が同じである場合において、各計測方向と画像内の位置との対応関係を予め求めておく。その上で、（ｄ）の処理では、この対応関係に基づいて、撮像時の動作体座標系において、前記（ａ）で特定した前記指標の位置に対応する計測方向を特定し、当該計測方向の被計測点の位置を表わす前記変換した座標値を特定する。
その一例を図３（Ｂ）を用いて説明する。図３（Ｂ）は、図３（Ａ）の場合において、距離計測時の動作体座標系Ｓ１と、撮像時の動作体座標系Ｓ２と各被計測点との関係を示す。前記（ａ）で特定した前記指標の位置に対応する計測方向が、図３（Ｂ）の動作体座標系Ｓ２において方向Ｄｍであるとする。この場合、（ｄ）の処理では、座標系Ｓ２において、計測方向Ｄｍの被計測点の座標値を特定する。この座標値は、（ｃ）の処理で変換されたものである。
【００４４】
（ｅ）前記（ｄ）で特定した座標値に基づいて、撮像時点における動作体３から対象物５までの距離を得る。例えば、上述の［数１］で座標変換された被計測点の座標値ｘｉ，ｙｉのうち、前記（ｄ）で特定した座標値がｘｊ，ｙｊである場合には、（ｘｊ＋ｙｊ）１／２が、撮像時における動作体３から対象物５までの距離となる。
【００４５】
なお、距離計測時と撮像時との間に、対象物５の向きと位置が変化したとしても、この変化による誤差を無視してよい。動作体３から見た対象物５の方向の計測について、対象物５の移動により移動体３から見た対象物５の方向が変化することによる計測誤差は、動作体３の向きの変化による計測値の変化量と比べて無視できる。動作体３から対象物５までの距離の計測について、対象物５の移動速度により計測周期の間に移動できる距離が対象物５の大きさより小さくなる制限値（例えば、移動体５の大きさが０．５ｍであり、計測が１００ミリ秒間隔である場合には速度５ｍ毎秒の移動速度）よりも小さければ、他の指標（例えば、他の対象物５）との混同が発生しないため、対象物５の位置の変化による計測誤差を許容できる。
【００４６】
図４は、本発明の実施形態による対象物との相対位置計測方法を示すフローチャートである。
【００４７】
ステップＳ１において、レーザ距離センサ９と撮像装置７を動作体３に設置する。
【００４８】
ステップＳ２において、レーザ距離センサ９により、動作体３上の計測基点から各計測方向にレーザ光を射出しその反射光を受けて、レーザ光の射出時点と反射光の受光時点との時間差に基づいて、計測基点から、該計測方向に存在する物体上の被計測点までの距離を計測する。このように計測された各計測方向の距離に基づいて、各被計測点の位置を、レーザ距離センサ９により、距離計測時の動作体座標系の座標値で表わす。
【００４９】
ステップＳ３において、撮像装置７により、対象物５が含まれる領域を撮像して画像を生成する。
【００５０】
その一方で、ステップＳ４において、動作変化量計測装置１１により、ステップＳ２における距離計測時の動作体３の向きに対する、ステップＳ３における撮像時の動作体３の相対的な向きと、ステップＳ２における距離計測時の動作体３の位置に対する、ステップＳ３における撮像時の動作体３の相対的な位置と、を求める。
【００５１】
その後、ステップＳ５において方向データ取得処理を行うとともに、ステップＳ６において距離データ取得処理を行う。
【００５２】
ステップＳ５の方向データ取得処理は、ステップＳ５１とステップ５２とを有する。
ステップＳ５１では、撮像装置７が撮像した画像において、対象物５に取り付けられた指標の位置を特定する。
ステップＳ５２では、ステップＳ５１で特定した位置と、動作体３に固定された撮像装置７（カメラ）の向きに基づいて、撮像装置７による撮像時において動作体３から見た対象物５の方向を特定する。
【００５３】
ステップＳ６の距離データ取得処理は、ステップＳ６１とステップＳ６２とステップＳ６３とを有する。
【００５４】
ステップＳ６１では、ステップＳ４で求めた相対的な向きと相対的な位置とに従って、ステップＳ２の距離計測時に計測した前記各被計測点の前記座標値を、距離計測時における動作体座標系の座標値から、撮像時における動作体座標系の座標値に座標変換する。
【００５５】
ステップＳ６２では、レーザ距離センサ９と撮像装置７の配置関係に基づいて、ステップＳ５１で特定した前記指標の位置に対応する計測方向と被計測点を、撮像時の動作体座標系において特定し、撮像時の動作体座標系において当該被測定点の位置を表わすステップＳ６１で変換した座標値を特定する。
ステップＳ５１で特定した前記指標の位置に対応する被計測点は、撮像時の動作体座標系において、指標の位置に正確に一致する被計測点でなくてもよく、指標の位置に正確に一致する被計測点Ａから、指標若しくは対象物の寸法に相当する範囲内に位置する被計測点Ｂであってもよい。ただし、被計測点Ａの計測距離と被計測点Ｂの計測距離との差が、対象物の寸法（奥行）以内となるようにする。
【００５６】
ステップＳ６３では、ステップＳ６２で特定した座標値に基づいて、撮像時点における動作体３から対象物５までの距離を得る。
【００５７】
上述のように、ステップＳ５２で得た、動作体３から見た対象物５の方向と、ステップＳ６３で得た、動作体３から対象物５までの距離とを、動作体３と対象物５の位置関係として取得することができる。
【００５８】
上述した実施形態によると、撮像した画像における対象物の指標の位置に基づいて、撮像時において動作体３から見た対象物５の方向を特定するので、対象物５の方向を高精度に検出することができる。
対象物５までの距離を求めるのに、レーザ距離センサ９を用いるので、対象物５までの距離を高精度に計測することができる。
よって、動作体３から見た対象物５の方向と動作体３から対象物５までの距離とを、動作体３と対象物５の相対位置として高精度に取得できる。
【００５９】
また、距離計測時の動作体３の向きに対する、撮像時の動作体３の相対的な向きと、距離計測時の動作体３の位置に対する、撮像時の動作体３の相対的な位置とに従って、レーザ距離センサ９により取得した各被計測点の座標値を、距離計測時の動作体座標系の座標値から、撮像時の動作体座標系の座標値に変換するので、撮像時における対象物５までの距離を得ることができる。従って、撮像時における動作体３と対象物５の相対位置を得ることができる。
【００６０】
しかも、この相対位置を簡単な処理によって得ることができる。取得時が異なる距離データ（各被計測点の座標値）と画像データとを対応づけるために、画像データを、距離データの取得時に合わせて補正する場合、画像データの補正処理は複雑になる。これに対し、本実施形態では、取得時が異なる距離データと画像データとを対応づけるために、距離データを、画像データの取得時に合わせて補正する。この補正は、上述のように座標変換するだけで行えるので、補正処理が簡単である。
【００６１】
本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。例えば、以下のようにしてもよい。
【００６２】
指標は、第１の波長域の光を吸収し、かつ、第２の波長域の光を反射する光学特性部であってもよい。第１の波長域は、赤外線（好ましくは、近赤外線）の波長域であり、第２の波長域は、赤色光の波長域である。代わりに、第１の波長域は、紫外線（好ましくは、長波長紫外線）の波長域であり、第２の波長域は、青色光の波長域であってもよい。このような場合、以下の構成を採用する。
【００６３】
指標は、対象物５に取り付けられ第２の波長域の光を反射する反射層と、この反射層の表面に取り付けられ第１の波長域の光を吸収するフィルムとからなる。
【００６４】
撮像装置７は、図５に示すように、太陽光に直接または間接的に照らされる地球の屋外に位置する対象物３を含む領域を撮像する第１撮像部７ａと第２撮像部７ｂとを有する。また、撮像装置７は、第１光学系と第２光学系を有する。第１光学系は、ハーフミラー１５と第１フィルタ１７とレンズ１９から構成され、前記領域から進行してきた第１の波長域の光を第１撮像部７ａに入射させ、かつ、前記領域から進行してきた第２の波長域の光を吸収する。第２光学系は、ハーフミラー１５と第２フィルタ２１とレンズ２３から構成され、前記領域から進行してきた第２の波長域の光を第２撮像部７ｂに入射させる。
【００６５】
第１フィルタ１７は、第１の波長域の光を透過させ、かつ、第２の波長域の光を吸収する。第２フィルタ２１は、第２の波長域の光を透過させ、かつ、第１の波長域の光を吸収する。ハーフミラー１５は、対象物５を含む領域である撮像範囲からの光の一部を、第１撮像部７ａの撮像面へ向けて反射させ、撮像範囲からの光の一部を、第２撮像部７ｂの撮像面へ向けて透過させる。
【００６６】
上述のステップＳ３では、第１撮像部７ａにより、第１光学系を介して、前記領域を撮像して第１画像を生成するとともに、第２撮像部７ｂにより、第２光学系を介して、前記領域を撮像して第２画像を生成する。
【００６７】
上述のステップＳ５１では、第１画像の各画素について、該画素の輝度をＡとし、前記領域において該画素と同じ位置に相当する第２画像の画素の輝度をＢとし、輝度Ａに対する輝度Ｂの比率Ｂ／Ａを求め、第１画像または第２画像において、前記比率がしきい値より大きくなる位置を、第１画像または第２画像における前記光学特性部（指標）の位置として特定する。
【００６８】
前記比率に関して、例えば、地球上の自然界や一般的な人工物には、赤色の可視光線（第２の波長域の光）の反射率が赤外線（第１の波長域の光）の反射率よりも大幅に大きい物体はほとんど無いと考えられる。
これに対し、光学特性部は、赤外線に含まれる光を吸収し、赤色の可視光線に含まれる光を反射するので、光学特性部では、赤色の可視光線の反射率が赤外線の反射率よりも大幅に大きくなる。従って、前記比率Ｂ／Ａにより、光学特性部を、自然界の物体や一般的な人工物と区別して認識できる。
【００６９】
このような光学特性部を用いる場合に、他の点は、上述と同じであってよい。なお、他の光学的特性を有する光学特性部を指標としてもよい。
例えば、入射光を直線偏光または円偏光に変換して反射する光学的特性を有する光学特性部（光学系）を指標としてもよい。この場合、以下のようにする。入射した直線偏光または円偏光の光量を減らして撮像装置７の第１撮像部の撮像面へ導き、入射した直線偏光または円偏光の光量を減らすことなく撮像装置７の第２撮像部の撮像面へ導くように撮像装置７を構成する。指標を含む領域を撮像装置７が撮像して、第１撮像部により第１画像を生成し、第２撮像部により第２画像を生成する。第１画像の各画素について、該画素の輝度をＡとし、前記領域において該画素と同じ位置に相当する第２画像の画素の輝度をＢとし、輝度Ａに対する輝度Ｂの比率Ｂ／Ａを求め、この比率に基づいて、第１画像または第２画像において、光学特性部（指標）の位置を特定する。
【００７０】
上述したように、光学特性部により生成された光の状態（特定の波長域で光量が減少している状態や、偏光が生じている状態）を２つの撮像部を用いて検出することにより、光学特性部の位置を特定できる。
【００７１】
また、指標として、他の手段を用いてもよい。
【符号の説明】
【００７２】
３ 動作体、３ａ 動作体の外周面、５ 対象物、５ａ 対象物の外周面、６ 車輪、７ 撮像装置、７ａ 第１撮像部、７ｂ 第２撮像部、９ レーザ距離センサ、１０ 対象物との相対位置計測装置、１１ 動作変化量計測装置、１１ａ 計測部、１１ｂ 演算部、１３ データ処理装置、１５ ハーフミラー、１７ 第１フィルタ、１９ レンズ、２１ 第２フィルタ、２３ レンズ
【技術分野】
【０００１】
本発明は、向きの変更と移動の少なくとも一方を行う動作体から見た対象物の方向と、動作体から対象物までの距離とを、動作体と対象物の相対位置として計測する対象物との相対位置計測方法と装置に関する。
【背景技術】
【０００２】
屋外のように変化に富んだ多様性のある環境において、例えばロボットである動作体が作業をする時に、ロボットが、対象物との相対位置を検出する必要がある場合がある。
【０００３】
その具体例を説明する。複数のロボットが、広範囲のエリアを分担して農作業や地雷撤去などの作業を行うことがある。この場合に、各ロボットは、他のロボットにより作業が済んだ範囲で作業を行わないようにする目的で、または、他のロボットとの衝突を避ける目的で、互いの相対位置を知る必要がある。
【０００４】
別の具体例を説明する。搬送ロボットが、荷物を持った状態で、人に追従して移動する自動制御を行うことにより、人の移動先まで荷物を搬送することがある。この場合には、搬送ロボットは、人を対象物として、人との相対位置を検出する必要がある。
【０００５】
対象物との相対位置を検出するために、対象物に取り付けた指標を検出している。対象物に取り付ける指標は、例えば、下記の特許文献１〜３に記載されている。
【０００６】
下記の特許文献１に記載された指標は、特定のパターンで輝度が変化する発光ダイオードである。この指標を用いる場合、指標が取り付けられた対象物を含む領域を撮像して得た画像から、発光ダイオードの発光パターンで輝度が変化する画素を特定する。特定した画素に基づいて、対象物の位置や方向を検出することができる。
【０００７】
下記の特許文献２に記載された指標は、直線や四角形や円弧などの幾何学的模様である。この指標を用いる場合、指標が取り付けられた対象物を含む領域を撮像して得た画像において、指標の幾何学的模様の位置を識別する。この位置に基づいて、対象物の位置や方向を検出することができる。
【０００８】
下記の特許文献３に記載された指標は、複数の特徴的な幾何学的模様である。この指標を用いる場合、指標が取り付けられた対象物を含む領域を撮像して得た画像において、指標の一部が他の物体に隠れていても、または、画像の背景部分に似た模様があっても、各幾何学的模様を、その既知の模様と照合することにより、指標における一部の幾何学的模様の位置を検出することができる。この位置に基づいて、対象物の位置や方向を検出することができる。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開２００９−３３３６６号公報
【特許文献２】特開２００７−３２３３号公報
【特許文献３】特開２０１１−２８４１７号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
しかし、画像に基づいて、対象物までの距離を計測する場合には、その計測精度が低くなることがある。対象物までの距離の計測は、画像における指標の寸法に基づいて行われる。しかし、画像における指標の寸法に誤差が生じることがある。特に、距離が大きい場合には、画像内において指標が小さくなるので、計測誤差が大きくなる。例えば、１００ｍ以上の距離から、６０度以上の視野で、縦と横の寸法が０．５ｍ程度の幾何学的模様（指標）を、１００万画素のカメラで撮像したとしても、画像において、幾何学的模様の縦または横の寸法が、５画素程度以下になる。この場合に、指標が有する実際の寸法と、画像における指標の寸法との関係から距離を計算すると、指標の縦または横の寸法に１画素の誤差があれば、距離の計測誤差は２０ｍにもなる。
【００１１】
そこで、本発明の目的は、向きの変更と移動の少なくとも一方を行う動作体から、対象物との相対位置を高精度に計測できる方法と装置を提供することにある。
【００１２】
上記目的を達成するため、本発明によると、向きの変更と移動の少なくとも一方を行う動作体から見た対象物の方向と、動作体から対象物までの距離とを、対象物との相対位置として計測する方法であって、
（Ａ）レーザ距離センサと撮像装置を動作体に設置し、
（Ｂ）レーザ距離センサにより、動作体に設定されている計測基点から各計測方向に存在する物体上の被計測点までの距離を計測し、各被計測点の位置を、動作体に固定された動作体座標系の座標値で表わし、
（Ｃ）撮像装置により、対象物が含まれる領域を撮像して画像を生成し、
（Ｄ）前記画像に基づいて、動作体から見た対象物の方向を取得する方向データ取得処理を行い、
（Ｅ）距離計測時と撮像時との間における動作体の向きと位置の変化量を求める動作変化量取得処理を行い、
（Ｆ）前記画像と前記座標値と前記変化量とに基づいて、撮像時における、動作体から対象物までの距離を取得する距離データ取得処理を行う、ことを特徴とする対象物との相対位置計測方法が提供される。
【００１３】
本発明の好ましい実施形態によると、
方向データ取得処理では、
（Ｄ１）撮像装置が撮像した画像において、対象物に取り付けられた指標の位置を特定し、
（Ｄ２）当該特定した位置に基づいて、撮像装置による撮像時において動作体から見た対象物の方向を特定し、
動作変化量取得処理では、
距離計測時の動作体の向きに対する撮像時の動作体の相対的な向きと、距離計測時の動作体の位置に対する撮像時の動作体の相対的な位置と、を前記変化量として求め、
距離データ取得処理では、
（Ｆ１）求めた相対的な向きと相対的な位置とに従って、前記各被計測点の座標値を、距離計測時の動作体座標系の座標値から撮像時の動作体座標系の座標値に変換し、
（Ｆ２）レーザ距離センサと撮像装置の配置関係に基づいて、前記（Ｄ１）で特定した前記指標の位置に対応する被計測点を、撮像時の動作体座標系において特定し、当該被測定点の位置を表わす前記変換した座標値を特定し、
（Ｆ３）当該特定した座標値に基づいて、撮像時における、動作体から対象物までの距離を求める。
【００１４】
また、上記目的を達成するため、本発明によると、向きの変更と移動の少なくとも一方を行う動作体から見た対象物の方向と、動作体から対象物までの距離とを、対象物との相対位置として計測する装置であって、
動作体に設置され、動作体に設定されている計測基点から各計測方向に存在する物体上の被計測点までの距離を計測し、各被計測点の位置を、動作体に固定された動作体座標系の座標値で表わすレーザ距離センサと、
動作体に設置され、対象物を含む領域を撮像して画像を生成する撮像装置と、
距離計測時と撮像時との間における動作体の向きと位置の変化量を求める動作変化量計測装置と、
前記画像に基づいて、動作体から見た対象物の方向を取得する方向データ取得処理を行い、前記画像と前記座標値と前記変化量とに基づいて、撮像時における、動作体から対象物までの距離を取得する距離データ取得処理を行うデータ処理装置と、を備える、ことを特徴とする対象物との相対位置計測装置が提供される。
【００１５】
上述した本発明によると、撮像した画像における対象物の指標の位置に基づいて、撮像時において動作体から見た対象物の方向を特定するので、対象物の方向を高精度に検出することができる。
対象物までの距離を求めるのに、レーザ距離センサを用いるので、対象物までの距離を高精度に計測することができる。
よって、動作体から見た対象物の方向と動作体から対象物までの距離とを、動作体と対象物の相対位置として高精度に取得できる。
【００１６】
また、前記画像と前記座標値と前記変化量とに基づいて、撮像時における、動作体から対象物までの距離を取得するので、撮像時における動作体と対象物の相対位置を得ることができる。
【図面の簡単な説明】
【００１７】
【図１】本発明の実施形態による対象物との相対位置計測装置を示すブロック図である。
【図２】動作体と対象物を示す簡略斜視図である。
【図３】動作体の向きと位置の変化を示す平面図である。
【図４】本発明の実施形態による対象物との相対位置計測方法を示すフローチャートである。
【図５】対象物の指標が光学特性部である場合の撮像装置の一例を示す。
【発明を実施するための形態】
【００１８】
本発明の好ましい実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００１９】
図１は、本発明の実施形態による対象物との相対位置計測装置１０を示すブロック図である。
対象物との相対位置計測装置１０は、動作体３から見た対象物５の方向と、動作体３から対象物５までの距離とを、動作体３と対象物５の相対位置として計測する。動作体３は、向きの変更と移動の少なくとも一方を行う。
【００２０】
図２は、動作体３と対象物５を示す簡略斜視図である。図２の例では、動作体３と対象物５は、回転駆動される車輪６により路面上を移動する自律移動ロボットである。また、図２の例では、動作体３と対象物５は、同じ構成を有する。すなわち、対象物５は、動作体３と同じ構成を有する別の動作体である。
【００２１】
対象物との相対位置計測装置１０は、動作体３に設けられる。なお、対象物５が、動作体３と同じ構成を有する別の動作体である場合には、対象物５にも、対象物との相対位置計測装置１０が設けられる。この場合、対象物（動作体）５の相対位置計測装置１０は、動作体５と動作体３との相対位置を計測する。
【００２２】
対象物との相対位置計測装置１０は、撮像装置７とレーザ距離センサ９と動作変化量計測装置１１とデータ処理装置１３とを備える。
【００２３】
撮像装置７は、動作体３に設置され、対象物５を含む領域を撮像して画像を生成する。
【００２４】
レーザ距離センサ９は、動作体３に設置される。レーザ距離センサ９は、動作体３に設定されている計測基点から各計測方向に存在する物体上の被計測点までの距離を計測し、各被計測点の位置を、動作体３に固定された動作体座標系の座標値で表わす。
詳しくは、レーザ距離センサ９は、計測基点から各計測方向にレーザ光を射出して当該レーザ光の反射光を検出することにより、該計測方向に存在する物体上の被計測点までの距離を計測する。また、レーザ距離センサ９は、各計測方向と、該方向上にある被計測点までの計測距離とに基づいて、各被計測点の位置を、動作体座標系の座標値で表わす。該座標値は、当該距離計測時の動作体座標系の座標値である。動作体座標系の原点は、計測基点であり、動作体座標系において、原点から見た各方向が、各計測方向である。ただし、レーザ距離センサ９で得られた様々な物体の座標値だけでは、どの座標値が対象物５の位置に相当するか、または、座標値のどの範囲に対象物５に相当する位置が含まれるかを判別することはできない。
【００２５】
レーザ距離センサ９は、例えば、図２のように、鉛直方向を向く走査軸Ｃ回りに光を走査することにより、水平な各計測方向にレーザ光を射出して当該各計測方向において被計測点までの距離を計測し、次いで、走査軸Ｃを鉛直軸から傾けて、走査軸Ｃ回りに光を走査することにより、水平方向から下方または上方に傾いた各計測方向にレーザ光を射出して当該各計測方向において被計測点までの距離を計測してよい。
【００２６】
動作体座標系は、動作体３と一体的に移動する。従って、動作体３が移動する空間に固定されている静止座標系に対して、動作体３が移動する場合に、動作体座標系は、動作体３と一体的に、前記静止座標系に対して移動する。
同様に、動作体３が前記静止座標系に対して向きを変えると、動作体座標系は、動作体３と一体的に、前記静止座標系に対して向きを変える。例えば、動作体座標系と静止座標系の各々が、互いに直交するｘ軸とｙ軸とｚ軸とを有する直交座標系であり、動作体３の向きが動作体座標系のｘ軸の向きと一致する場合には、動作体３の向きが、静止座標系のｘ軸の向きから静止座標系のｙ軸の向きに変化すると、動作体座標系のｘ軸も、静止座標系のｘ軸の向きから静止座標系のｙ軸の向きに変化する。
なお、本願において、動作体３の向きとは、動作体３に設定された第１固定点から、この第１固定点と間隔をおいて動作体３に設定されて第２固定点に向かう方向である。動作体３の向きは、動作体３の後方から前方へ向かう向きであってよい。
【００２７】
動作変化量計測装置１１は、距離計測時と撮像時との間における動作体３の向きと位置の変化量を求める。
詳しくは、動作変化量計測装置１１は、レーザ距離センサ９による距離計測時の動作体３の向きに対する、撮像装置７による撮像時の動作体３の相対的な向きと、距離計測時の動作体３の位置に対する撮像時の動作体３の相対的な位置と、を前記変化量として求める。なお、本実施形態では、レーザ距離センサ９により距離計測を行った後、撮像装置７により撮像を行っている。ただし、撮像装置７により撮像を行った後、レーザ距離センサ９により距離計測を行ってもよい。
【００２８】
動作変化量計測装置１１は、計測部１１ａと演算部１１ｂとを有する。
【００２９】
計測部１１ａは、距離計測時と撮像時との間の各時点および距離計測時と撮像時において、動作体３の向きと動作体３の移動速度を計測する。計測部１１ａは、例えば、動作体３が、路面上を転動することにより移動する車輪６を有する場合には、動作変化量計測装置１１は、車輪６の回転速度を計測し、回転速度から動作体３の移動速度を演算する。また、計測部１１ａは、動作体３に設置されたジャイロセンサを用いて、動作体３の向き（進行方向）を検出する。
【００３０】
演算部１１ｂは、計測した各時点における動作体３の向きと移動速度とに基づいて、距離計測時の動作体３の向きに対する、撮像時の動作体３の相対的な向きと、距離計測時の動作体３の位置に対する撮像時の動作体３の相対的な位置と、を求める。
【００３１】
データ処理装置１３は、撮像装置７により得た画像に基づいて、動作体３から見た対象物５の方向を取得する方向データ取得処理を行い、撮像装置７により得た前記画像とレーザ距離センサ９により得た前記座標値と動作変化量計測装置１１により得た前記変化量とに基づいて、撮像時における、動作体３から対象物５までの距離を取得する距離データ取得処理を行う。方向データ取得処理は、データ処理装置１３の方向データ取得処理部１３ａにより行われ、距離データ取得処理は、データ処理装置１３の距離データ取得処理部１３ｂにより行われる。
【００３２】
方向データ取得処理は、次の（ａ）（ｂ）からなる。
【００３３】
（ａ）撮像装置７が撮像した画像において、対象物５に取り付けられた指標の位置を特定する。
【００３４】
（ｂ）次いで、当該特定した位置に基づいて、撮像装置７による撮像時において動作体３から見た対象物５の方向を特定する。すなわち、撮像装置７（カメラ）の既知の向きと、撮像した画像の中心位置と、画像における指標の位置とに基づいて、撮像時に、動作体３から見た対象物５の方向を特定する。
【００３５】
指標は、図２の例では、対象物５の外周面５ａに、その周方向全体にわたって、連続してまたは間隔をおいて取り付けられている。外周面５ａは、鉛直軸回りの周方向に延びて１周している。なお、符号３ａは、指標が取り付けられる動作体３の外周面を示し、この外周面３ａも、鉛直軸回りの周方向に延びて１周している。
【００３６】
指標として、次のものを用いてよい。
一例では、指標は、特許文献１のように、特定のパターンで輝度が変化する発光ダイオードである。この場合、撮像装置７は、短い設定時間にわたる動画を画像として撮像し、画像において、特定のパターンで輝度が変化する画素の位置を、指標の位置として特定する。この場合、撮像時は、動画を構成する一連の画像のうちのいずれかの指定画像を撮像した時であってよく、当該指定画像を後述の処理（ｄ）で使用してよい。
別の例では、指標は、特許文献２、３に記載されたような幾何学的模様である。この場合、幾何学的模様と同じ模様を予め設定しておき、画像において、この設定模様と同じ模様の部分を、指標の位置として特定する。
【００３７】
また、対象物５において、指標の位置または指標と隣接する位置に、レーザ光を高反射率（例えば、８０％以上）で反射する反射シートを設けてもよい。これにより、レーザ距離センサ９で、指標の位置または指標と隣接する位置を被計測点として計測できなくなる確率が減る。
【００３８】
距離データ取得処理は、次の（ｃ）（ｄ）（ｅ）からなる。
【００３９】
（ｃ）動作変化量計測装置１１が求めた相対的な向きと相対的な位置とに従って、レーザ距離センサ９が距離計測時に計測した前記各被計測点の前記座標値を、撮像時における動作体座標系の座標値に座標変換する。
【００４０】
この座標変換の一例を説明する。動作体３が、水平な２次元平面である路面上を移動し、かつ、対象物５がこの路面上に存在し、動作体３と対象物５の位置を、互いに直交し水平方向を向くｘ軸とｙ軸を有する当該２次元平面の静止座標系Ｓ０で表わすとする。この場合に、図３（Ａ）に示すように、動作体３が、距離計測時に、位置Ｐ１において方向Ｄ１を向いており、その後の撮像時に、位置Ｐ２において方向Ｄ２を向いている場合には、次の［数１］により、レーザ距離センサ９が距離計測時に計測した前記各被計測点の前記座標値を、撮像時における動作体座標系の座標値に座標変換する。
【００４１】
【数１】
【００４２】
ここで、右辺のｘｉとｙｉは、レーザ距離センサ９が距離計測を行った時の動作体座標系における各被計測点の前記座標値である。左辺のｘｉとｙｉは、撮像装置７が撮像を行った時の動作体座標系における各被計測点の座標値である。右辺と左辺のｘｉとｙｉの添え字ｉは、被計測点の識別番号であり、被計測点がｎ個ある場合には、１〜ｎまでの値をとり、ｘｉとｙｉはｎ組だけ存在する。右辺のΔｘは、上述した２次元平面の静止座標系Ｓ０における位置Ｐ１のｘ座標と位置Ｐ２のｘ座標の差であり、右辺のΔｙは、上述した２次元平面の静止座標系Ｓ０における位置Ｐ１のｙ座標と位置Ｐ２のｙ座標の差である。また、右辺のθは、距離計測時の動作体３が向いている方向Ｄ１に対する、撮像時の動作体３が向いている方向を示す角度θである。
なお、動作体座標系は、この例では２次元座標系であるが、３次元座標系であってもよい。
【００４３】
（ｄ）レーザ距離センサ９と撮像装置７の配置関係に基づいて、前記（ａ）で特定した前記指標の位置に対応する被計測点を、撮像時の動作体座標系において特定し、当該被測定点の位置を、撮像時の動作体座標系において表わす前記変換した座標値を特定する。
距離計測時と撮像時が同じであるとすると、上述の各計測方向が、撮像装置７により撮像した画像のどの位置に一致するかは、レーザ距離センサ９と撮像装置７の配置関係から分かる。従って、この配置関係に基づいて、距離計測時と撮像時が同じである場合において、各計測方向と画像内の位置との対応関係を予め求めておく。その上で、（ｄ）の処理では、この対応関係に基づいて、撮像時の動作体座標系において、前記（ａ）で特定した前記指標の位置に対応する計測方向を特定し、当該計測方向の被計測点の位置を表わす前記変換した座標値を特定する。
その一例を図３（Ｂ）を用いて説明する。図３（Ｂ）は、図３（Ａ）の場合において、距離計測時の動作体座標系Ｓ１と、撮像時の動作体座標系Ｓ２と各被計測点との関係を示す。前記（ａ）で特定した前記指標の位置に対応する計測方向が、図３（Ｂ）の動作体座標系Ｓ２において方向Ｄｍであるとする。この場合、（ｄ）の処理では、座標系Ｓ２において、計測方向Ｄｍの被計測点の座標値を特定する。この座標値は、（ｃ）の処理で変換されたものである。
【００４４】
（ｅ）前記（ｄ）で特定した座標値に基づいて、撮像時点における動作体３から対象物５までの距離を得る。例えば、上述の［数１］で座標変換された被計測点の座標値ｘｉ，ｙｉのうち、前記（ｄ）で特定した座標値がｘｊ，ｙｊである場合には、（ｘｊ＋ｙｊ）１／２が、撮像時における動作体３から対象物５までの距離となる。
【００４５】
なお、距離計測時と撮像時との間に、対象物５の向きと位置が変化したとしても、この変化による誤差を無視してよい。動作体３から見た対象物５の方向の計測について、対象物５の移動により移動体３から見た対象物５の方向が変化することによる計測誤差は、動作体３の向きの変化による計測値の変化量と比べて無視できる。動作体３から対象物５までの距離の計測について、対象物５の移動速度により計測周期の間に移動できる距離が対象物５の大きさより小さくなる制限値（例えば、移動体５の大きさが０．５ｍであり、計測が１００ミリ秒間隔である場合には速度５ｍ毎秒の移動速度）よりも小さければ、他の指標（例えば、他の対象物５）との混同が発生しないため、対象物５の位置の変化による計測誤差を許容できる。
【００４６】
図４は、本発明の実施形態による対象物との相対位置計測方法を示すフローチャートである。
【００４７】
ステップＳ１において、レーザ距離センサ９と撮像装置７を動作体３に設置する。
【００４８】
ステップＳ２において、レーザ距離センサ９により、動作体３上の計測基点から各計測方向にレーザ光を射出しその反射光を受けて、レーザ光の射出時点と反射光の受光時点との時間差に基づいて、計測基点から、該計測方向に存在する物体上の被計測点までの距離を計測する。このように計測された各計測方向の距離に基づいて、各被計測点の位置を、レーザ距離センサ９により、距離計測時の動作体座標系の座標値で表わす。
【００４９】
ステップＳ３において、撮像装置７により、対象物５が含まれる領域を撮像して画像を生成する。
【００５０】
その一方で、ステップＳ４において、動作変化量計測装置１１により、ステップＳ２における距離計測時の動作体３の向きに対する、ステップＳ３における撮像時の動作体３の相対的な向きと、ステップＳ２における距離計測時の動作体３の位置に対する、ステップＳ３における撮像時の動作体３の相対的な位置と、を求める。
【００５１】
その後、ステップＳ５において方向データ取得処理を行うとともに、ステップＳ６において距離データ取得処理を行う。
【００５２】
ステップＳ５の方向データ取得処理は、ステップＳ５１とステップ５２とを有する。
ステップＳ５１では、撮像装置７が撮像した画像において、対象物５に取り付けられた指標の位置を特定する。
ステップＳ５２では、ステップＳ５１で特定した位置と、動作体３に固定された撮像装置７（カメラ）の向きに基づいて、撮像装置７による撮像時において動作体３から見た対象物５の方向を特定する。
【００５３】
ステップＳ６の距離データ取得処理は、ステップＳ６１とステップＳ６２とステップＳ６３とを有する。
【００５４】
ステップＳ６１では、ステップＳ４で求めた相対的な向きと相対的な位置とに従って、ステップＳ２の距離計測時に計測した前記各被計測点の前記座標値を、距離計測時における動作体座標系の座標値から、撮像時における動作体座標系の座標値に座標変換する。
【００５５】
ステップＳ６２では、レーザ距離センサ９と撮像装置７の配置関係に基づいて、ステップＳ５１で特定した前記指標の位置に対応する計測方向と被計測点を、撮像時の動作体座標系において特定し、撮像時の動作体座標系において当該被測定点の位置を表わすステップＳ６１で変換した座標値を特定する。
ステップＳ５１で特定した前記指標の位置に対応する被計測点は、撮像時の動作体座標系において、指標の位置に正確に一致する被計測点でなくてもよく、指標の位置に正確に一致する被計測点Ａから、指標若しくは対象物の寸法に相当する範囲内に位置する被計測点Ｂであってもよい。ただし、被計測点Ａの計測距離と被計測点Ｂの計測距離との差が、対象物の寸法（奥行）以内となるようにする。
【００５６】
ステップＳ６３では、ステップＳ６２で特定した座標値に基づいて、撮像時点における動作体３から対象物５までの距離を得る。
【００５７】
上述のように、ステップＳ５２で得た、動作体３から見た対象物５の方向と、ステップＳ６３で得た、動作体３から対象物５までの距離とを、動作体３と対象物５の位置関係として取得することができる。
【００５８】
上述した実施形態によると、撮像した画像における対象物の指標の位置に基づいて、撮像時において動作体３から見た対象物５の方向を特定するので、対象物５の方向を高精度に検出することができる。
対象物５までの距離を求めるのに、レーザ距離センサ９を用いるので、対象物５までの距離を高精度に計測することができる。
よって、動作体３から見た対象物５の方向と動作体３から対象物５までの距離とを、動作体３と対象物５の相対位置として高精度に取得できる。
【００５９】
また、距離計測時の動作体３の向きに対する、撮像時の動作体３の相対的な向きと、距離計測時の動作体３の位置に対する、撮像時の動作体３の相対的な位置とに従って、レーザ距離センサ９により取得した各被計測点の座標値を、距離計測時の動作体座標系の座標値から、撮像時の動作体座標系の座標値に変換するので、撮像時における対象物５までの距離を得ることができる。従って、撮像時における動作体３と対象物５の相対位置を得ることができる。
【００６０】
しかも、この相対位置を簡単な処理によって得ることができる。取得時が異なる距離データ（各被計測点の座標値）と画像データとを対応づけるために、画像データを、距離データの取得時に合わせて補正する場合、画像データの補正処理は複雑になる。これに対し、本実施形態では、取得時が異なる距離データと画像データとを対応づけるために、距離データを、画像データの取得時に合わせて補正する。この補正は、上述のように座標変換するだけで行えるので、補正処理が簡単である。
【００６１】
本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。例えば、以下のようにしてもよい。
【００６２】
指標は、第１の波長域の光を吸収し、かつ、第２の波長域の光を反射する光学特性部であってもよい。第１の波長域は、赤外線（好ましくは、近赤外線）の波長域であり、第２の波長域は、赤色光の波長域である。代わりに、第１の波長域は、紫外線（好ましくは、長波長紫外線）の波長域であり、第２の波長域は、青色光の波長域であってもよい。このような場合、以下の構成を採用する。
【００６３】
指標は、対象物５に取り付けられ第２の波長域の光を反射する反射層と、この反射層の表面に取り付けられ第１の波長域の光を吸収するフィルムとからなる。
【００６４】
撮像装置７は、図５に示すように、太陽光に直接または間接的に照らされる地球の屋外に位置する対象物３を含む領域を撮像する第１撮像部７ａと第２撮像部７ｂとを有する。また、撮像装置７は、第１光学系と第２光学系を有する。第１光学系は、ハーフミラー１５と第１フィルタ１７とレンズ１９から構成され、前記領域から進行してきた第１の波長域の光を第１撮像部７ａに入射させ、かつ、前記領域から進行してきた第２の波長域の光を吸収する。第２光学系は、ハーフミラー１５と第２フィルタ２１とレンズ２３から構成され、前記領域から進行してきた第２の波長域の光を第２撮像部７ｂに入射させる。
【００６５】
第１フィルタ１７は、第１の波長域の光を透過させ、かつ、第２の波長域の光を吸収する。第２フィルタ２１は、第２の波長域の光を透過させ、かつ、第１の波長域の光を吸収する。ハーフミラー１５は、対象物５を含む領域である撮像範囲からの光の一部を、第１撮像部７ａの撮像面へ向けて反射させ、撮像範囲からの光の一部を、第２撮像部７ｂの撮像面へ向けて透過させる。
【００６６】
上述のステップＳ３では、第１撮像部７ａにより、第１光学系を介して、前記領域を撮像して第１画像を生成するとともに、第２撮像部７ｂにより、第２光学系を介して、前記領域を撮像して第２画像を生成する。
【００６７】
上述のステップＳ５１では、第１画像の各画素について、該画素の輝度をＡとし、前記領域において該画素と同じ位置に相当する第２画像の画素の輝度をＢとし、輝度Ａに対する輝度Ｂの比率Ｂ／Ａを求め、第１画像または第２画像において、前記比率がしきい値より大きくなる位置を、第１画像または第２画像における前記光学特性部（指標）の位置として特定する。
【００６８】
前記比率に関して、例えば、地球上の自然界や一般的な人工物には、赤色の可視光線（第２の波長域の光）の反射率が赤外線（第１の波長域の光）の反射率よりも大幅に大きい物体はほとんど無いと考えられる。
これに対し、光学特性部は、赤外線に含まれる光を吸収し、赤色の可視光線に含まれる光を反射するので、光学特性部では、赤色の可視光線の反射率が赤外線の反射率よりも大幅に大きくなる。従って、前記比率Ｂ／Ａにより、光学特性部を、自然界の物体や一般的な人工物と区別して認識できる。
【００６９】
このような光学特性部を用いる場合に、他の点は、上述と同じであってよい。なお、他の光学的特性を有する光学特性部を指標としてもよい。
例えば、入射光を直線偏光または円偏光に変換して反射する光学的特性を有する光学特性部（光学系）を指標としてもよい。この場合、以下のようにする。入射した直線偏光または円偏光の光量を減らして撮像装置７の第１撮像部の撮像面へ導き、入射した直線偏光または円偏光の光量を減らすことなく撮像装置７の第２撮像部の撮像面へ導くように撮像装置７を構成する。指標を含む領域を撮像装置７が撮像して、第１撮像部により第１画像を生成し、第２撮像部により第２画像を生成する。第１画像の各画素について、該画素の輝度をＡとし、前記領域において該画素と同じ位置に相当する第２画像の画素の輝度をＢとし、輝度Ａに対する輝度Ｂの比率Ｂ／Ａを求め、この比率に基づいて、第１画像または第２画像において、光学特性部（指標）の位置を特定する。
【００７０】
上述したように、光学特性部により生成された光の状態（特定の波長域で光量が減少している状態や、偏光が生じている状態）を２つの撮像部を用いて検出することにより、光学特性部の位置を特定できる。
【００７１】
また、指標として、他の手段を用いてもよい。
【符号の説明】
【００７２】
３ 動作体、３ａ 動作体の外周面、５ 対象物、５ａ 対象物の外周面、６ 車輪、７ 撮像装置、７ａ 第１撮像部、７ｂ 第２撮像部、９ レーザ距離センサ、１０ 対象物との相対位置計測装置、１１ 動作変化量計測装置、１１ａ 計測部、１１ｂ 演算部、１３ データ処理装置、１５ ハーフミラー、１７ 第１フィルタ、１９ レンズ、２１ 第２フィルタ、２３ レンズ
【特許請求の範囲】
【請求項１】
向きの変更と移動の少なくとも一方を行う動作体から見た対象物の方向と、動作体から対象物までの距離とを、対象物との相対位置として計測する方法であって、
（Ａ）レーザ距離センサと撮像装置を動作体に設置し、
（Ｂ）レーザ距離センサにより、動作体に設定されている計測基点から各計測方向に存在する物体上の被計測点までの距離を計測し、各被計測点の位置を、動作体に固定された動作体座標系の座標値で表わし、
（Ｃ）撮像装置により、対象物が含まれる領域を撮像して画像を生成し、
（Ｄ）前記画像に基づいて、動作体から見た対象物の方向を取得する方向データ取得処理を行い、
（Ｅ）距離計測時と撮像時との間における動作体の向きと位置の変化量を求める動作変化量取得処理を行い、
（Ｆ）前記画像と前記座標値と前記変化量とに基づいて、撮像時における、動作体から対象物までの距離を取得する距離データ取得処理を行う、ことを特徴とする対象物との相対位置計測方法。
【請求項２】
方向データ取得処理では、
（Ｄ１）撮像装置が撮像した画像において、対象物に取り付けられた指標の位置を特定し、
（Ｄ２）当該特定した位置に基づいて、撮像装置による撮像時において動作体から見た対象物の方向を特定し、
動作変化量取得処理では、
距離計測時の動作体の向きに対する撮像時の動作体の相対的な向きと、距離計測時の動作体の位置に対する撮像時の動作体の相対的な位置と、を前記変化量として求め、
距離データ取得処理では、
（Ｆ１）求めた相対的な向きと相対的な位置とに従って、前記各被計測点の座標値を、距離計測時の動作体座標系の座標値から撮像時の動作体座標系の座標値に変換し、
（Ｆ２）レーザ距離センサと撮像装置の配置関係に基づいて、前記（Ｄ１）で特定した前記指標の位置に対応する被計測点を、撮像時の動作体座標系において特定し、当該被測定点の位置を表わす前記変換した座標値を特定し、
（Ｆ３）当該特定した座標値に基づいて、撮像時における、動作体から対象物までの距離を求める、ことを特徴とする請求項１に記載の対象物との相対位置計測方法。
【請求項３】
向きの変更と移動の少なくとも一方を行う動作体から見た対象物の方向と、動作体から対象物までの距離とを、対象物との相対位置として計測する装置であって、
動作体に設置され、動作体に設定されている計測基点から各計測方向に存在する物体上の被計測点までの距離を計測し、各被計測点の位置を、動作体に固定された動作体座標系の座標値で表わすレーザ距離センサと、
動作体に設置され、対象物を含む領域を撮像して画像を生成する撮像装置と、
距離計測時と撮像時との間における動作体の向きと位置の変化量を求める動作変化量計測装置と、
前記画像に基づいて、動作体から見た対象物の方向を取得する方向データ取得処理を行い、前記画像と前記座標値と前記変化量とに基づいて、撮像時における、動作体から対象物までの距離を取得する距離データ取得処理を行うデータ処理装置と、を備える、ことを特徴とする対象物との相対位置計測装置。
【請求項１】
向きの変更と移動の少なくとも一方を行う動作体から見た対象物の方向と、動作体から対象物までの距離とを、対象物との相対位置として計測する方法であって、
（Ａ）レーザ距離センサと撮像装置を動作体に設置し、
（Ｂ）レーザ距離センサにより、動作体に設定されている計測基点から各計測方向に存在する物体上の被計測点までの距離を計測し、各被計測点の位置を、動作体に固定された動作体座標系の座標値で表わし、
（Ｃ）撮像装置により、対象物が含まれる領域を撮像して画像を生成し、
（Ｄ）前記画像に基づいて、動作体から見た対象物の方向を取得する方向データ取得処理を行い、
（Ｅ）距離計測時と撮像時との間における動作体の向きと位置の変化量を求める動作変化量取得処理を行い、
（Ｆ）前記画像と前記座標値と前記変化量とに基づいて、撮像時における、動作体から対象物までの距離を取得する距離データ取得処理を行う、ことを特徴とする対象物との相対位置計測方法。
【請求項２】
方向データ取得処理では、
（Ｄ１）撮像装置が撮像した画像において、対象物に取り付けられた指標の位置を特定し、
（Ｄ２）当該特定した位置に基づいて、撮像装置による撮像時において動作体から見た対象物の方向を特定し、
動作変化量取得処理では、
距離計測時の動作体の向きに対する撮像時の動作体の相対的な向きと、距離計測時の動作体の位置に対する撮像時の動作体の相対的な位置と、を前記変化量として求め、
距離データ取得処理では、
（Ｆ１）求めた相対的な向きと相対的な位置とに従って、前記各被計測点の座標値を、距離計測時の動作体座標系の座標値から撮像時の動作体座標系の座標値に変換し、
（Ｆ２）レーザ距離センサと撮像装置の配置関係に基づいて、前記（Ｄ１）で特定した前記指標の位置に対応する被計測点を、撮像時の動作体座標系において特定し、当該被測定点の位置を表わす前記変換した座標値を特定し、
（Ｆ３）当該特定した座標値に基づいて、撮像時における、動作体から対象物までの距離を求める、ことを特徴とする請求項１に記載の対象物との相対位置計測方法。
【請求項３】
向きの変更と移動の少なくとも一方を行う動作体から見た対象物の方向と、動作体から対象物までの距離とを、対象物との相対位置として計測する装置であって、
動作体に設置され、動作体に設定されている計測基点から各計測方向に存在する物体上の被計測点までの距離を計測し、各被計測点の位置を、動作体に固定された動作体座標系の座標値で表わすレーザ距離センサと、
動作体に設置され、対象物を含む領域を撮像して画像を生成する撮像装置と、
距離計測時と撮像時との間における動作体の向きと位置の変化量を求める動作変化量計測装置と、
前記画像に基づいて、動作体から見た対象物の方向を取得する方向データ取得処理を行い、前記画像と前記座標値と前記変化量とに基づいて、撮像時における、動作体から対象物までの距離を取得する距離データ取得処理を行うデータ処理装置と、を備える、ことを特徴とする対象物との相対位置計測装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１３−５７５４１（Ｐ２０１３−５７５４１Ａ）
【公開日】平成２５年３月２８日（２０１３．３．２８）
【国際特許分類】
【出願番号】特願２０１１−１９４８３４（Ｐ２０１１−１９４８３４）
【出願日】平成２３年９月７日（２０１１．９．７）
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【Ｆターム（参考）】
【公開日】平成２５年３月２８日（２０１３．３．２８）
【国際特許分類】
【出願日】平成２３年９月７日（２０１１．９．７）
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【Ｆターム（参考）】
[ Back to top ]