JP2015212629A - 検出装置およびこの装置を具えたマニプレータの動作制御 - Google Patents

Abstract

【課題】撮像画像中にある対象物の３次元位置を演算する装置をマニプレータ先端に具え、ロボットコントローラがリアルタイムで操作して作業を実行するシステムの検出装置を提供する。【解決手段】光ビーム照射手段１と単眼撮像手段２と演算装置６にて構成される検出装置にて、装置取付基準面ＢＰから光ビーム照射点Ｓまでの距離Ｌを算出し、被写面４を表す数式により、被写面４上の撮像点である対象物中心の光ビーム照射点からの平面位置を演算し、ロボットコントローラはこの３次元位置情報を使ってマニプレータをリアルタイムで操作し作業するシステムであり、撮像画像から対象物を探索するには、あらかじめ、対象物を撮像画像特定区域において輝度差による要素にて再構成し、これを目標データとして登録し、距離Ｌを基準にして決定する大きさの探索ユニットを全画面に配置し、絶対差が最小となる探索ユニットに対象物があると判定することにより行う。【選択図】図１

Description

本発明は、ロボット等のマニプレータに装着された、１個の光ビーム照射装置と１台の単眼撮像カメラにより撮像した対象物の３次元位置を検出する検出装置、および、この装置を具えたマニプレータの操作をリアルタイムで制御する方法に関するものである。

近年ロボットの作業にビデオカメラ等による撮像画を利用し、作業対象物の識別や位置・姿勢情報を入手してロボットの動作を制御することが盛んに行われている。

しかし、対象物の３次元位置を得るためには複眼以上のカメラを使用するのが一般的であり、また、カメラは僅かにロボットのハンド周辺に装着されることもあるが、ロボットの外界もしくはロボットの本体やヘッド部に設置されることが多い。

単眼カメラの撮像画においては、斜視フレームを選択し固定することによって、画像に深さを与え疑似３次元とすることができ、また、正面からの画像であっても、カメラと撮像面の距離が既知ならば、３次元画像であり、これらの画像を処理することにより高速ピッキング作業等も行われている。

産業ロボットは、生産性の向上を目的とし、作業速度の向上と正確に繰り返すことが課されており、ロボットに作業を行わせるには、定まった方法で作業対象物を渡し、受け取ることが必要であるが、対象物の位置と姿勢を機械的に整備して渡すことがコスト的に難しいことがあり、この場合、撮像画像を利用することによりこれらの情報を入手して解決されていることが多い。

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

ビデオカメラの撮像画は連続画像であるが、前述のいずれも連続画像の1枚を切り出して処理してロボットの動作制御を行っており、連続画像の特性である時間的変化が利用されているとは言えない。

連続画像の時間変化とは、ロボット作業の品質に係るものであり、例えば作業対象物のピックアップの失敗の検知や、作業の完遂の監視といったものであり、産業ロボットが今後取り組む課題である。

連続画像の1枚を使用するのは、複眼カメラにおいて２次元の撮像画より３次元情報を入手するために、複雑な処理と相当の解析時間（０．４秒程度と言われている）を要するためであるが、単眼カメラにおいては、撮像フレーム（カメラ）を固定し、カメラと被写面の距離は既知とすることにより３次元情報が得られているのである。

複眼カメラでは撮像フレームの自由度があるが処理時間がかかり、単眼カメラでは撮像フレームは固定であるが処理時間が速く、これは被写面の距離検出の時間に関係している。

特許文献１においては、ハンドに取り付けられた単眼カメラと３本の光ビームを使用して被写面からカメラ焦点までの距離と被写面の向きをリアルタイムで検知する方法が記述されているが、機器の配置を検知数式が適用できるように調整するのが難しいこと、また、作業対象物の検知やハンドの動作制御については今後の課題となっている。

本発明では、マニプレータに取り付けられた１台の単眼のカメラとカメラ光軸と角度をなす１本の光ビームを使用し、撮像面の光ビーム照射点の位置より照射点と装置の取付基準面との距離を算出し、光ビーム照射点群の撮像面座標と基準面との距離をあらかじめ測定し、この照射点群を結ぶ直線を光ビームとして四角錐の仮想撮像空間に取り込み、光ビーム照射点と照射方向を同定して、光ビーム照射点を通り光ビームに直交する被写平面を表す数式を作成し、被写面上の撮像点の位置をこの数式を演算して求めるものである。

光ビーム照射点は、撮像面において、装置の取付基準面との距離に関係する一直線上の撮像点であり、検出が容易であり、近似式を用いることにより高速で距離が算出できるためリアルタイムでの画像処理が可能となる。

作業対象物は、撮像画像の輝度差によって特徴付けられる輝度差要素を抽出し、プロフイルをこの輝度差要素により定義し、要素群の図心を中心とし、その周りを等角度分割し、分割ゾーン内の要素の個数比率の平方根と平均距離の積を単位化してプロフイルデータとする。

プロフイルデータは原イメージデータを圧縮したものでありリアルタイム処理を容易にするものであると共に、輝度差を利用することで環境の照度に影響され難い性質を有するものであって、光ビーム照射点の輝度差を得るためにはむしろ環境照度が低いほうが望ましい面もある。

作業対象物の探索は、撮像画像全体を分割、これを再結合して探索ユニットを構成し、前記プロフイルデータを作成し、あらかじめ作成し登録された探索すべき作業対象物のプロフイルデータと比較し、全探索ユニットの内で差異が最小で許容値以内のものを選ぶことにより行い、作業対象物の位置は探索ユニットのプロフイルの中心とする。

なお、作業対象物の向き（回転）は、登録作業対象物のプロフイルデータと移動比較し、絶対値総和の差異が最小の角度（位相）とするが、その精度は、分割角の大きさが計算速度に反比例するため、リアルタイム処理を優先すれば制限される。

探索された作業対象物のプロフイルの中心（図心）を被写面上の撮像点の位置とすることにより、光ビーム照射点からの平面位置が計算され、作業対象物の３次元位置が求められる。

本発明の機器構成は、マニプレータに取付ける１台の単眼カメラと１本の光ビームより構成される装置と前記演算処理（作業対象物のプロフイルデータの生成と管理を含む作業対象物の探索や３次元位置の計算）の結果や画像を表示する演算装置より成る。

当該装置を取付けたマニプレータの操作は、ロボットコントローラによって行われ、本検出装置の演算装置とはＬＡＮ接続により相互通信するが、ロボットコントローラを主とし本検出装置の演算装置は従とする。

ロボットコントローラより作業対象物を設定し、探索を行う命令を本検出装置の演算装置に通信により伝えると、命令を受けた本演算装置は撮像画像を解析し、光ビーム照射点と装置取付基準面との距離、光ビーム照射点を原点とする作業対象物の中心の平面位置及び対象物の向きを、ロボットコントローラに返す。

ロボットコントローラは、返信された作業対象物の３次元位置とエンドエフェクタ等の作用点位置の関係を計算し、マニプレータの動作を制御する。

作業対象物の３次元位置はリアルタイム（１００msec程度）で更新できるので、状況をフィードバックしながら、ロボットコントローラはマニプレータの操作を行うことができる。

本装置の撮像カメラは動くマニプレータに設置されるため、解析に適用できる撮像画像は鮮明な画像が撮れる速度から静止での撮像になるが、ロボットの作業動作において作業対象物に接近する場合は低速になる機会がありこれを利用するか、意図的に撮像する機会をつくる必要がある。

本発明は、単眼カメラによる２次元画像を光ビーム照射の付加により３次元画像に変換し、作業対象物の画像はプロフイルデータに圧縮することにより、簡便で安価な３次元リアルタイム画像解析システムを提供する。

産業ロボットにおいては、従来、画像解析システムは、撮像カメラを固定（ロボットの外界または本体に設置）または半固定（旋回や首振りをするロボットヘッドに設置）して利用されてきたが、本発明の撮像カメラは動くマニプレータに設置し、作業対象物との距離、位置や向き（回転）をリアルタイムで検出する新しい検出装置である。

ロボット操作に本発明の検出装置を適用することにより、マニプレータと作業対象物との相対距離や位置を測定し、向きの検出を行いながら対象物にエンドエフェクタを作用させるといったロボットの新しい作業形態を提供することができる。

これは、ロボットのティーチングの過程にも共通するものであり、現在多くの労力を必要とする作業の省力化と確実性に貢献することが考えられる。

また、連続画像の時間的変化を利用することにより、エンドエフェクタと作業対象物の相対位置を監視することで対象物の把持の成否や、対象物基準画像との差異を比較することにより対象物のアライメントの良否や、差異の進行を検知することで作業進行状況の程度や良否といった、従来、対応が難しかったエンドエフェクタにおいて発生する不確実性の解消に貢献することが考えられる。

本発明の一実施例として、対象物の３次元位置検出装置の構成を示すものであり、光学系を前方より斜視した構成図である。 仮想撮像面上に写される光ビーム照射点の位置と、検出装置取付基準面から光ビーム照射点までの距離を幾何的に説明する図である。 撮像面、仮想撮像面と光ビーム照射点の幾何を示し、成立する数式を説明する図である。 参照被写面上の対象物図心の位置を、光ビーム直交面に投影する数学的処理を幾何的に示す図である。 本発明で考案した画像処理により、撮像画像から作成した作業対象物のプロフイルデータの一実施例を示す図である。 一実施例における前記登録プロフイルの距離、要素幅と要素高さを示す。 一実施例における前記登録プロフイルデータと距離の関係を示す。 撮像画面をブロック分割し対象物探索ユニットを探索単位とすることで、演算の高速化ができることを説明する図である。 本発明の根幹をなす光ビーム照射装置の斜視断面構造図であり、一実施例を示す。 移動型アームロボットのアーム先端に、本発明の検出装置を具えた作業ロ ボットの斜視図を一実施例として示す。 前記のアームロボット部分を取り出し、同検出装置を装着した状態の斜視図を一実施例として示す。 図１と図４の仮想撮像面２ｂ上の光ビーム照射点Ｓ’は、被写面上の光ビーム照射点Ｓと本来一致するが図が重なり判り難くなるのを避けるため意図的に離して描いている。

以下、本発明を実施する形態を図面に基づいて説明する。図１は、対象物の３次元位置検出装置の一実施例の構成を示すものであり、光学系を前方より斜視した構成図、図２は、仮想撮像面上に写される光ビーム照射点の位置と検出装置取付基準面から光ビーム照射点までの距離を示す図、図３は、撮像面、仮想撮像面と光ビーム照射点の幾何を示し、成立する数式を説明する図、図４は、参照被写面上の対象物図心の位置を、光ビーム直交面に投影する数学的処理を幾何的に示す図である。

図１および図３において、３次元位置検出装置は、フレーム３にて固持される光ビーム照射装置１と撮像カメラ２より成り、光ビーム照射装置から照射される１本の光ビームが照射方向ベクトルｎｂ→に対して、光ビームと撮像カメラ光軸とは交差する必要はないが、撮像カメラの光軸ＬＣを通る撮像水平面と角度α、同垂直面と角度βをなす配置にあり、撮像カメラの被写面４は、光ビーム照射点Ｓにて交わり、検出装置取付基準面ＢＰからの距離Ｌであり、光ビーム照射方向ベクトルに直交するものであり、演算装置６により、被写面に投影された作業対象物５の画像は、本発明にて考案したプロフイルデータに圧縮され、輝度差要素群の図心はカメラ仮想撮像面２ｂの座標（Ｘｉ，Ｙｉ）に演算され、光ビーム照射点の同座標（Ｘｓ，Ｙｓ）より、被写面上の光ビーム照射点を原点（０，０）とする作業対象物の中心の平面位置（ＤＸｎ，ＤＹｎ）が演算され、ロボットコントローラ７の出力命令を通信で受けた演算装置が、作業対象物のこれら３次元位置情報と向きθを、ロボットコントローラへ通信にて出力する検出装置である。

光ビーム照射装置１としては図９にこの一実施例を示すが、レーザーポインターに使用される光ビーム照射モジュール１ｃを光ビーム発生装置とし、これを内筒１ｂに内包・固着させ、フレーム３に固持された外筒１ａに弾性体の保持部材１ｄにて支持するが、光ビームの照射方向を微調整できる方向調整ボルト１ｅを外筒に具え、安全性を増し撮像面焼けを防止する減光板１ｆを内筒端部に具え、減光板には光ビームの大きさを決める絞穴１ｇを、
具えてなるものである。

撮像カメラ２としては、ＣＣＤ撮像素子等を撮像面２ａとし、演算装置６に撮像信号を入力できるカメラ（例えばＵＳＢ端子で接続するＷＥＢカメラ）であり、遠くや近くの被写体が鮮明に撮像できるオートフォーカス機能をもつことが望ましい。

演算装置６としては、ディスプレイ（映像表示装置）やＵＳＢ端子等の接続等の入出力インターフェイスを持ち、外部機器と通信ができ、ＲＯＭ等に記憶したプログラムに基づき一時記憶にＲＡＭ等を使用しながらＣＰＵ（中央演算装置）で演算処理を行うマイクロコンピュータを用いることができる。

ロボットコントローラ７としては、マニプレータを操作する装置であり通常マニプレータに付属しているものであるが、自ら演算装置を有し、演算装置６と通信を行って命令を発信し、結果を受信し、操作に反映するため通信機能を有することが必要である。

図２において、本発明の根幹をなす光ビーム照射による被写面までの距離を検知する機構を幾可的に説明するが、撮像カメラ２と撮像面２ａが成す四角錐と同仮想撮像面２ｂの成す四角錐は相似関係にあり、光ビーム照射装置１の取付基準面ＢＰから被写面４までの距離Ｌであり、光ビームは被写面４上の光ビーム照射点Ｓにて交わり、仮想撮像面との交点を光ビーム照射点とすると、撮像面は、通常、撮像画面の右方向をｘｓｃ、下方向をｙｓｃとし左上を原点（０，０）とする微小四角撮像素子のピクセル単位の座標が使用され、これをｍｍ単位の実サイズを有する仮想撮像面の座標軸をＸｓｃ、Ｙｓｃとし、光ビーム照射点の仮想撮像面上での座標（Ｘｓｃｓ，Ｙｓｃｓ）とし、撮像面上の座標（ｘｓｃｓ，ｙｓｃｓ）とする。

一例として、光ビーム照射点の距離をＬ１からＬ７の等間隔とし、各照射点と仮想撮像面を有する四角錐の頂点を結ぶ直線を伸縮し、仮想撮像面との交点群を作成すると、交点群は一直線上にあり、この直線の式を撮像面にて
ｙｓｃｓ＝ｙｓｃ０＋ａ０・ｘｓｃｓ
とすると、ｙｓｃ０は直線のｙｓｃ軸切片の値であり、ａ０は直線の傾き定数であって撮像カメラが光軸を中心にｘ軸から角度ｔａｎ^-1ａ０回転していることを示すものであり、これらは実際に撮像された画像を使用して求めることができる。

光ビーム照射点の仮想撮像面での座標Ｘｓｃｓは、光ビーム照射方向ベクトルｎｂ→を、撮像カメラの光軸を通るカメラ水平面に投影した直線ベクトルと撮像カメラの光軸のなす角度αに関係しているが、ピクセル単位である撮像面の座標ｘｓｃｓと距離Ｌは強い対数関係の相関性を有し、実際に撮像された画像を使用し作成する近似式を、一例として、
Ｌ＝１０^{ｆ（ｘｓｃｓ）}
とすると、ここで、
ｆ（ｘｓｃｓ）＝Σｂｉ・ｘｓｃｓ^ｉ （但しｉ＝０から４）
であり、この近似精度は、一実施例では、距離Ｌが１００から５００ｍｍにおいて最大で０．８６ｍｍ、平均で０．２６ｍｍであって、より高次の近似式にすれば精度は向上する。

なお、光ビーム照射方向が、検出装置取付基準面ＢＰと正に直交することは、取付基準面に対して平行移動する平面上において、光ビーム照射点の位置が不動であることによって担保される。

距離Ｌは、撮像面において光ビーム照射点が存在する直線上を探索し、光ビーム照射点を見出すことにより座標（ｘｓｃｓ，ｙｓｃｓ）が求められ、前記の近似式によって算出できるが、赤色光ビームの光ビーム照射点は、左右上下ピクセルの赤色輝度差と中央最大輝度に閾値を設けることにより安定して見出すことができる。

ここで、図３に示すように、光ビーム照射点Ｓの関係式を導出するために、撮像面２ａに撮像カメラ光軸ＬＣと交わる新たな座標を導入し、これを撮像面のｘｙ座標とし、撮像面の幅をｗピクセル、高さをｈピクセルとすると、ｘ軸とｙ軸はｘｓｃ軸とｙｓｃ軸に関して
ｘ＝ｘｓｃ−ｗ／２
ｙ＝ｙｓｃ−ｈ／２
となり、ｚ軸をカメラ光軸方向とすると、撮像面を有する四角錐の高さは焦点距離ｆピクセルであり、撮像面はｚ＝ｆとなり、撮像面と相似関係にある仮想撮像面２ｂにもＸＹ座標を導入する。

光ビームを撮像カメラの仮想撮像空間（四角錐）に特徴付ける３つの変数、角度α、角度β、距離Ｈ０を導入し、以下、これらを定義するが、角度αはカメラ光軸ＬＣを通るカメラ水平面へ投影した光ビーム投影直線とカメラ光軸のなす角度であり、角度βはカメラ光軸を通るカメラ垂直面へ投影した光ビーム投影直線とカメラ光軸のなす角度であり、距離（高さ）Ｈ０は同垂直面と光ビームの交点ＳＹの焦点からの距離であるが、光ビーム照射点Ｓは仮想撮像面２ｂ上にあり、検出装置取付面ＢＰからの距離はＬであり、その座標を（Ｘｉ，Ｙｉ）とし、カメラの撮像空間において、光ビームの同水平面投影直線のＺ軸の切片とＸ軸切片を結ぶ直線の長さをＬｉｈとすると、光軸上のＸＹ座標原点とＺ軸の切片を結ぶ直線の長さはＬｉｈ・ｃｏｓαであり
Ｘｉ＝Ｌｉｈ・ｓｉｎα
となり、交点ＳＹの同水平面からの距離をＹ０とすると
Ｙｉ＝Ｙ０＋Ｌｉｈ・ｃｏｓα・ｔａｎβ
ちなみに光ビーム照射点の直線式から
ｙｉ＋ｈ／２＝ｙｓｃ０＋ａ０・（ｘｉ＋ｗ／２）
ここで、ｘｉ＝０においては
ｙ０＝ｙｓｃ０＋（ａ０・ｗ＋ｈ）／２
であり、光ビーム照射点Ｓと交点ＳＹの光ビーム長さは
Ｌｉ＝Ｌｉｈ・√｛１＋（ｃｏｓα・ｔａｎβ）^２｝
となり、仮想撮像面の焦点からの距離をＨｉとすると
Ｈ０＝Ｈｉ-Ｌｉｈ・ｃｏｓα
検出装置取付面ＢＰから交点ＳＹまでの距離は
Ｌ０＝Ｌ-Ｌｉ
一方、距離Ｌ＝１０^{ｆ（ｘｉ）}の式を用いると
Ｌ０＝１０^ｆ（０）
となる。

角度αとβは、実際の撮像画像を用いて求めることができ、その方法は、角度αは、光ビームと直交する被写面４上にスケール直線部をＸ軸に平行に置き、被写面を検出装置取付基準面ＢＰからの距離ΔＬｉを平行移動させときのＸ＝０のスケール読値の変化量Δｍｘより
ｔａｎα＝Δｍｘ／ΔＬｉ／ｆβ
ここで、
ｆβ＝√｛１＋（ｃｏｓα・ｔａｎβ）^２｝
であり、例えば、α＝２４°とβ＝１°では、ｆβは１．０００１であり、小さいβにおいては
ｔａｎα＝Δｍｘ／ΔＬｉ
とすることができ、また、角度βは、被写面上にスケール直線部をＹ軸に平行に置き、被写面を同距離ΔＬｉ平行移動させときのＹ＝０のスケール読値の変化量Δｍｙより
ｔａｎβ＝Δｍｙ／（ΔＬｉ・ｃｏｓα）／ｆβ
であり、小さいβにおいては
ｔａｎβ＝Δｍｙ／（ΔＬｉ・ｃｏｓα）
とすることができる。

光ビーム照射点を撮像カメラ撮像空間に定めるには、交点ＳＹの高さＨ０とカメラ焦点距離ｆを同定する必要があり、Ｍ個の等間隔の光ビーム照射点の実際の撮像画像を使用して解析すると、撮像面上の光ビーム照射点座標（ｘｉ，ｙｉ）は撮像画像より読み出され検出装置取付面ＢＰから交点ＳＹまでの距離Ｌ０は既に求められており
Ｌｉ＝Ｌ−Ｌ０
Ｌｉｈ＝Ｌｉ／ｆβ
Ｘｉ＝Ｌｉｈ・ｓｉｎα
Ｈｉ＝Ｈ０＋Ｌｉｈ・ｃｏｓα
であるから、撮像面の撮像空間（四角錐）と仮想撮像面の仮想撮像空間の相似関係により
ｘｉ／ｆ＝Ｘｉ／Ｈｉ
であるので、各光ビーム照射点について、平方残差（Ｘｉ/Ｈｉ−ｘｉ／ｆ）^２を求め、照射点の個数をＭとするとこの平均総和の平方根
Ｏ（Ｈｉ，ｆ）＝√｛Σ（Ｘｉ/Ｈｉ−ｘｉ／ｆ）^２｝／√Ｍ （但しｉ＝１からＭ）
が最小となるＨｉ（即ちＨ０）とｆを決定することにより同定され、この精度は４桁程度で充分である。なお、焦点距離ｆが撮像カメラの仕様により既知であれば、交点ＳＹの高さＨ０のみの同定計算となる。

光ビーム照射方向ベクトルｎｂ→は、光ビーム長さＬｉのＸ、Ｙ、Ｚ方向成分よりＬｉｈ・ｓｉｎα、Ｌｉｈ・ｃｏｓα・ｔａｎβ、Ｌｉｈ・ｃｏｓαであり、単位ベクトルにすると
ｎｂ（ｓｉｎα／ｆβ、ｃｏｓα・ｔａｎβ／ｆβ，ｃｏｓα／ｆβ）→
となる。

光ビームに直交する被写面４の法線方向ベクトルはｎｂ→であり，焦点をＺ＝０とし仮想撮像面２ｂにＸ軸Ｙ軸を有する直交座標系において、被写面上の光ビーム照射点Ｓを通る平面の式は
ｎｂＸ（Ｘ−Ｘｉ）＋ｎｂＹ（Ｙ−Ｙｉ）＋ｎｂＺ（Ｚ−Ｈｉ）＝０
ここで
ｎｂＸ＝ｓｉｎα／ｆβ
ｎｂＹ＝ｃｏｓα・ｔａｎβ／ｆβ
ｎｂＺ＝ｃｏｓα／ｆβ
で表わされる。

撮像面に写された作業対象物の位置より被写面上の位置を決定するには、焦点ｏから撮像面２ａ上の点ｐ（ｘｐ，ｙｐ）を通る直線ｏｐと被斜面４との交点Ｐを求めることであり、直線ｏｐの方向ベクトルｏｐ（ｘｐ／ｏｐ，ｙｐ／ｏｐ，ｆ／ｏｐ）→より、ここで
ｏｐ＝√（ｘｐ^２＋ｙｐ^２＋ｆ^２）
であり、直線ｏｐの式は
Ｘ／（ｘｐ／ｏｐ）＝Ｙ／（ｙｐ／ｏｐ）＝Ｚ（ｆ／ｏｐ）
であり、
Ｘ＝（ｘｐ／ｏｐ）Ｔ
Ｙ＝（ｙｐ／ｏｐ）Ｔ
Ｚ＝（ｆ／ｏｐ）Ｔ
として、先に示した被写面上の光ビーム照射点Ｓを通る平面の式に代入すると
Ｔ＝ｎｂ→・ｏＰ→／ｎｂ→・ｏｐ→
ここで、ｎｂ→・ｏＳ→とｎｂ→・ｏｐ→は、ベクトルの内積であり成分で表わすと
ｎｂ→・ｏＰ→＝ｎｂＸ・Ｘｉ＋ｎｂＹ・Ｙｉ＋ｎｂＺ・Ｈｉ
ｎｂ→・ｏｐ→＝ｎｂＸ・ｘｐ／ｏｐ＋ｎｂＹ・ｙｐ／ｏｐ＋ｎｂＺ・ｆ／ｏｐ
となる。

被写面上の点Ｐは、作業対象物の投影図心を示すものであり、被写面の原点は、視認が可能な光ビーム照射点Ｓとし、被写面上の座標軸は、光ビームの廻りに任意に決めることができ、マニプレータの動作軸方向を選ぶのが実用上便利であるが、ここではデフォルト（初期設定）として撮像カメラの仮想撮像面の座標軸Ｘ、Ｙを用いて、被写面のＸｎ軸とＹｎ軸を決めると
Ｘｎ→＝Ｙ→×ｎｂ→
Ｙｎ→＝ｎｂ→×Ｘ→
ここで、×はベクトルの外積を表し、成分で表すと
Ｘｎ（ｎｂＺ／ＳＸ，０，−ｎｂＸ／ＳＸ）→
Ｙｎ（０，ｎｂＺ／ＳＹ，−ｎｂＹ／ＳＹ）→
ここで、
ＳＸ＝√（ｎｂＺ^２＋ｎｂＸ^２）
ＳＹ＝√（ｎｂＺ^２＋ｎｂＹ^２）
となる。

被写面上の点Ｓ（Ｘｓ，Ｙｓ，Ｚｓ）と点Ｐ（Ｘｐ，Ｙｐ，Ｚｐ）を結ぶ直線ＳＰのベクトルをＳＰ（Ｘｐ−Ｘｓ，Ｙｐ−Ｙｓ，Ｚｐ−Ｚｓ）→とすると、各座標軸成分はＳＰ→と各座標軸ベクトルＸｎ→、Ｙｎ→とｎｂ→の内積となり、Ｘｎ軸の成分は
ＳＰ→・Ｘｎ→＝（Ｘｐ−Ｘｓ）・ｎｂＺ／ＳＸ−（Ｚｐ−Ｚｓ）・ｎｂＸ／ＳＸ
Ｙｎ軸の成分は
ＳＰ→・Ｙｎ→＝（Ｙｐ−Ｙｓ）・ｎｂＺ／ＳＹ−（Ｚｐ−Ｚｓ）・ｎｂＹ／ＳＹ
Ｚｎ軸の成分は、ＳＰ→とｎｂ→が直交するので
ＳＰ→・ｎｂ→＝０
となる。

被写面は光ビームと直交するとは限らないので、図４に示すように、法線方向ベクトルｎｒ（ｎｒＸ，ｎｒＹ，ｎｒＺ）→を有し、光ビーム照射点Ｓを通る参照被写面４ａと称する平面の式は
ｎｒＸ（Ｘ−Ｘｉ）＋ｎｒＹ（Ｙ−Ｙｉ）＋ｎｒＺ（Ｚ−Ｈｉ）＝０
であり、撮像面２ａ上の点をｑ（ｘｑ，ｙｑ，ｚｑ）、仮想撮像面２ｂ上の点Ｑとすると
Ｔｒ＝ｎｒ→・ｏＱ→／ｎｒ→・ｏｑ→
Ｘｑ＝（ｘｑ／ｏｑ）Ｔｒ
Ｙｑ＝（ｙｑ／ｏｑ）Ｔｒ
Ｚｑ＝（ｆ／ｏｑ）Ｔｒ
となる。

参照被写面４ａ上の点Ｓと点Ｑを結ぶ直線ベクトルＳＱ→は、光ビーム直交面の座標軸Ｘｎ、Ｙｎ、Ｚｎの成分で表す方が、マニプレータの操作上便利であり、各座標軸成分はＳＱ→と各座標軸ベクトルＸｎ→、Ｙｎ→とｎｂ→の内積であり、Ｘｎ軸成分は
ＳＱ→・Ｘｎ→＝（Ｘｑ−Ｘｓ）・ｎｂＺ／ＳＸ−（Ｚｑ−Ｚｓ）・ｎｂＸ／ＳＸ
Ｙｎ軸の成分は
ＳＱ→・Ｙｎ→＝（Ｙｑ−Ｙｓ）・ｎｂＺ／ＳＹ−（Ｚｑ−Ｚｓ）・ｎｂＹ／ＳＹ
Ｚｎ軸の成分は
ＳＱ→・ｎｂ→＝（Ｘｑ−Ｘｓ）・ｎｂＸ＋（Ｙｑ−Ｙｓ）・ｎｂＹ
＋（Ｚｑ−Ｚｓ）・ｎｂＺ
となる。

ここで、参照被写面４ａを仮想撮像面２ｂとすれば、撮像画像が焦点からの距離Ｈｉの正面画像となるので、撮像カメラの撮像面をマニプレータ取付基準面に平行に、光ビーム照射方向を撮像カメラ光軸と角度を成す、撮像カメラ基準の検出装置にすることができる。

この３次元位置検出装置とマニプレータを使用して、被写面の向き（法線方向）を求める手順と方法を示すと、
（１）ロボットコントローラが現在位置の被写面までの距離検出を命令
（２）本演算装置からロボットコントローラに現在位置の被写面までの距離Ｌ００を返信
（３）ロボットコントローラがロボット動作Ｘ方向にＸ０１移動し、距離検出を命令
（４）本演算装置からロボットコントローラに距離Ｌ０１を返信
（５）ロボットコントローラがロボット動作Ｘ方向に−Ｘ０１、動作Ｙ方向にＹ０１移動
した後、距離検出を命令
（６）本演算装置からロボットコントローラに距離Ｌ０２を返信
により、コントローラの演算装置にて、動作Ｘ方向に傾き（Ｌ０１−Ｌ００）／Ｘ０１、動作Ｙ方向に傾き（Ｌ０２−Ｌ００）／Ｙ０１があることが計算される。マニプレータの検出装置取付基準面を、これら傾きを解消するように操作すれば、被写面に平行な姿勢となる。また、検出装置の傾きの検出精度は、一実施例では、１／１００より小さい。

ロボット動作方向を、この検出装置に登録する手順と方法は、
（１）被写面上に円板等の対象物を置き、検出装置を被写面に平行な姿勢にする
（２）検出装置に対象物撮像画像の図心（ｘ００，ｙ００）を記憶
（３）ロボット動作Ｘ方向にＸ０２移動
（４）対象物撮像の図心（ｘ０１，ｙ０１）を記憶
により、ロボット動作Ｘ方向ベクトル（ｘ０１−ｘ００，ｙ０１−ｙ００）→が求められる。ロボット座標系で使われる＋Ｚ方向を手前側にすると、ロボット動作Ｙ方向はこれより反時計回りに９０°の方向となるが、ここで、ロボット座標系と撮像カメラ座標系では、Ｙ方向とＺ方向が逆向きになっていることが多いので注意を要する。

撮像画像より作業対象物を抽出しプロフイルおよびプロフイルデータを作成する方法を説明するが、撮像面は水平（ｘｓｃ軸）方向と垂直（ｙｓｃ軸）方向にピクセルという画素を有し、そのサイズは例えば（８００ｘ６００）のように表し、カラー画像では光の３原色（赤、緑、青）についてそれぞれ輝度値を有するが、この発明では白黒の輝度値（グレースケール）を用いて、水平３ピクセルの左右および垂直３ピクセルの上下の輝度差が、閾値Ｂｒ以上である３ピクセルを、一つの輝度差要素として抽出し、撮像面座標上に記憶する。

作業対象物の撮像された部分を、全画面から長方形で切り取り、水平輝度差要素は垂直方向に逐次、水平方向に３ピクセル毎に、また、垂直輝度差要素は水平方向に逐次、垂直方向に３ピクセル毎に探索し、見出された輝度差要素は個々に座標値を記憶し要素の総個数Ｎｅを対象物のプロフイルとして記憶する。ここで、３ピクセルの両端の輝度差を用いるのは、同じ輝度差であれば、隣接するピクセルよりも要素数が多く対象物が捉えやすいこと、および、探索が３ピクセル毎で２ピクセル毎より１．５倍の速度になるためである。

なお、輝度差閾値Ｂｒと総要素数Ｎｅの関係は、一実施例によると、閾値１６から４０までは、はぼ反比例しているが、５０以上になると要素数が加速度的に減少しており、高い閾値は撮像画像の特徴を捉えているが全体を表すものではなく、プロフイルとしての閾値は画像全体を表しかつ要素数が少ないことが望まれるが、表１に一実施例を示す。

輝度差要素の探索と同時に、要素の水平座標（ｘｓｃｉ）、垂直座標（ｙｓｃｉ）をそれぞれ加算すると、輝度差要素群の図心（ｘｓｃｃ，ｙｓｃｃ）は、探索終了時の総要素数Ｎｅより
ｘｓｃｃ＝Σｘｓｃｉ／Ｎｅ （但しｉ＝１からＮｅ）
ｙｓｃｃ＝Σｙｓｃｉ／Ｎｅ （但しｉ＝１からＮｅ）
となる。

撮像画像のプロフイルは、図心の周りを等角度でＪ分割し、輝度差要素を、図心からの相対座標（ｘａｉ，ｙａｉ）によって該当する分割角度ゾーンに配置し、角度ゾーン番号θで表されるゾーン毎に要素の総個数Ｎθｊと平均半径ｒθｊを演算して作成するが、これらを式で表すと
ｘａｉ＝ｘｓｃｉ−ｘｓｃｃ
ｙａｉ＝ｙｓｃｉ−ｙｓｃｃ
θ＝ｆθ（ｘａｉ，ｙａｉ）
ｓｕｍｒθｊ＝Σ（ｘａｉ^２＋ｙａｉ^２） （但しｉ＝１からＮθｊ、θ＝ｊ）
ｒθｊ＝√（ｓｕｍｒθｊ／Ｎθｊ）
であり、ここで、ｆθ関数は、座標（ｘａｉ，ｙａｉ）に、角度ゾーン番号θをあらかじめ記載したテンプレートを用いて、θを読み出すことを表す。

平均半径ｒθｊは正しく形状を表しているが、プロフイルデータｒθｐｊとしては、特徴付け（探索比較の差分増加）を行うために、角度ゾーンの要素数の比率の平方根を乗じ、単位化（データの総和が１）を行ってパーセント（％）で表し、これを式にて表すと
ｒθｍｏｄｊ＝ｒθｊ√（Ｎθｊ／Ｎｅ）
ｓｕｍｒθｍｏｄ＝Σｒθｍｏｄｊ （但しｊ＝１からＪ）
ｒθｐｊ＝ｒθｍｏｄｊ／ｓｕｍｒθｍｏｄ・１００
となる。

作業対象物を単独で撮像したプロフイルデータｒθｐｊと背景に何らかの画像が写り撮像されたプロフイルデータでは差異が大きくなり（劣化するという）、対象物ではない部分を撮像したプロフイルデータと判別が難しいことがあるが、同一角度ゾーンの個々の要素半径ｒθｊｉと平均半径ｒθｍｊの平均絶対差を用いて、要素半径の分布値Δｒθｊを演算することにより、単独で撮像した分布値に近い（由来する）ものかを判別することができるので、この分布値Δｒθｊをプロフイルデータに加え、これを式で表すと
ｒθｊｉ＝√（ｘａｉ^２＋ｙａｉ^２） （但しθ＝ｊ）
ｒθｍｊ＝（Σｒθｊｉ）／Ｎθｊ （但しｉ＝１からＮθｊ）
Δｒθｊ＝（Σ｜ｒθｊｉ−ｒθｍｊ｜）／ｒθｍｊ／Ｎθｊ
（但しｉ＝１からＮθｊ）
となり、プロフイルデータｒθｐｊの劣化が大きいとき真偽の判別に使用する。

プロフイルデータｒθｐｊの一実施例を図５に示す。−印で示す水平輝度差要素と｜印の垂直輝度差要素が対象物の輪郭を表し、＋印の図心の周りを３６分割した○印で示す平均半径ｒθｊは輪郭上にあって正しく形状を表わしており、◎印で示すプロフイルデータｒθｐｊは特徴的な形になっているのが判る。この例では、輪郭の一部が欠けており対象物全体を捉えるには、輝度差の閾値を小さくし輝度差要素数を増加させる必要がある。なお、この例のプロフイルは、距離１４３ｍｍ、輝度差４８、輝度差要素数７７２、要素幅９３ピクセル、要素高２５３ピクセルであった。

撮像全画面の中より作業対象物を探すときは、対象物の大きさと形状を基に行うが、大きさは対象物の距離に、形状は輝度差閾値による要素数に関係しており、本発明においては、撮像画像中に即座に距離が見出されるので、距離に関するプロフイルおよびプロフイルデータを登録し、撮像画像の一部分である探索ユニットより抽出されるデータと比較することにより容易に対象物を見出すことができる。

一実施例として、表２に対象物距離と輝度差・要素数を、図６に距離とサイズを、図７に距離とプロフイルデータｒθｐｊを示すが、対象物距離が検出されると図６により対象物のサイズが計算され、対象物の向きは不明なので大きい方の高さを対象物サイズとするが、抽出する撮像画像のサイズが決定される。

比較のベースとなる図７のプロフイルデータをみると、距離２３７から４７９における絶対差の総和は数％以下であり距離にかかわらずほぼ同じであり、距離１６９のプロフイルデータは一部に欠けがあり他の距離とは少し形が異なり改良の余地があるが、絶対差の総和は１０％程度であり登録データとしては有効である。

ここで、撮像全画面の画像より対象物を探す手順と方法を示すと、
（１）対象物を登録
（２）撮像画像から被写面までの距離Ｌを検出
（３）近い距離の登録データｒθｐｊＴを選択し
（４）探索ユニットのサイズを決定
（５）探索輝度差要素数を設定、例えば、登録要素数の０．８５から１．２倍
（６）撮像画面全体に探索ユニットを配置
（７）探索ユニットの輝度差要素数を計算
（８）設定要素数の範囲内であれば、プロフイルデータｒθｐｊＳを作成
（９）絶対差の総和Σ｜ｒθｐｊＳ−ｒθｐｊＴ｜が最小となる位相を計算
（１０）この絶対差総和の小さい順に探索ユニットを整列
（１１）最小の絶対差総和が、例えば２０％より、小さければ探索対象物と判別
（１２）大きければ、プロフイルデータの分布値ΔｒθｊＳを計算し真偽を判別
により行う。

この検出装置がリアルタイムで動作するには、撮像画面に探索ユニットをいかに配置し、高速で探索するかが重要となり、このひとつの方法を図８に示すが、全画面８００ｘ６００ピクセルを、２４ｘ２４ピクセルのブロックに分割し、ここで、ブロック位置（ｂｗ，ｂｈ）、ブロック番号ｋとし、ブロックの特性値Ｂｘｋとすると、水平方向は３３ブロックに分割されるので
ｋ＝３３ｂｈ＋ｂｗ
探索ユニットの水平・垂直ブロック数Ｍｂとすると、水平部分の総和ｓｕｍＷＢｘｋは
ｓｕｍＷＢｘｋ＝ΣＢｘｋ （但しｋ＝ｋからｋ＋Ｍｂ−１）
探索ユニットの特性値の総和ｓｕｍＢｘｋは、垂直部分がｋ、ｋ＋３３、ｋ＋３３×２、・・、ｋ＋３３（Ｍｂ−１）であるので
ｓｕｍＢｘｋ＝ΣｓｕｍＷＢｘｎ （但しｍ＝０からＭｂ−１、ｎ＝ｋ＋３３ｍ）
となる。

この方法が高速演算できるのは、ｓｕｍＷＢｘｋをｓｕｍＷＢｘ（ｋ）で表すと右隣はｓｕｍＷＢｘ（ｋ＋１）＝ｓｕｍＷＢｘ（ｋ）＋Ｂｘ（ｋ＋Ｍｂ）−Ｂｘ（ｋ）
ｓｕｍＢｘｋをｓｕｍＢｘ（ｋ）で表すと直下は
ｓｕｍＢｘ（ｋ＋３３）＝ｓｕｍＢｘ（ｋ）＋ｓｕｍＷＢｘ（ｋ＋３３Ｍｂ）
−ｓｕｍＷＢｘ（ｋ）
であり、既計算値に隣接特性値を加減算することにより順次計算できるからである。

登録データで使用した撮像画面の区域と登録データの図心を固定すると、登録プロフイルデータと撮像プロフイルデータの絶対値差総和Σ｜ＲθｐｊＳ−ＲθｐｊＴ｜は作業の進行過程の値を表し、この最終形の値を与えることにより作業完了を検知することができるので、ロボット作業の過程の監視や作業前の対象物の姿勢の良否の判定といったロボット作業の品質を管理向上することが可能となる。

ここで、プロフイルデータＲθｐｊは、特徴付けられたｒθｍｏｄｊに替り、形状を正しく表す平均半径ｒθｊを使用し、
ｓｕｍｒθ＝Σｒθｊ （但しｊ＝１からＪ）
Ｒθｐｊ＝ｒθｊ／ｓｕｍｒθ・１００
とし、領域固定のプロフイルデータＲθｐｊと称する。

前記の方法を使って、エンドエフェクタの駆動機構の位置検知センサとし、駆動機に始動や停止の命令を出力することができるので、人が目視確認しながら道具を使うような作業も可能となる。

本発明の一実施例として、図１０に対象物の３次元位置検出装置を具えた移動型アームロボットの斜視図を、図１１にアームロボットの部分を取り出した斜視図を示す。

移動型アームロボットは、移動台車９ａの前方中央に本発明の対象物の３次元位置検出装置を具えたアームロボットを据え、移動台車の後方左右に独立して回転する駆動車輪９ｂを、前方下左右には自在キャスターを配して任意の２次元方向に前進と後退することができるものであり、後方中央部には動力源である蓄電池とロボットコントローラ、本発明の検出装置用演算装置、ＬＡＮ通信設備やその他の制御器を収納する設備空間のカバー９ｃがあり、移動台車の中央左右は荷物置き場９ｅとなっており、荷物置き場の両側面と後方には台車カバー９ｄを配して荷崩れを防止し、アームロボットは移動台車前方の地上または両側より物体をピックし荷物置き場に収納することができる。

移動台車の操作は、２つの形態によって行われ、ひとつは撮像画像の目印やマークを検出し追跡する走行モードであり、もうひとつはアームロボットの作動範囲外に対象物がある場合に作動範囲内になるようにする接近モードであり、共に、独立して回転する左右の駆動車輪を制御して行う。

移動台車においてオドメータ機能は常備されるが、長距離の走行で誤差が蓄積されるので要所地点での位置確認が必要となるが、本発明の対象物の３次元位置検出装置をこの用途に供することができる。

アームロボットは６軸の多関節を持ち、その軸組は、肩ヨー−肩ピッチ−肘ピッチ−手首ヨー−手首ピッチ−手首ロールであり、手首３軸は直交しており、３次元位置検出装置は、フレーム３の取付基準面を５自由度ある手首ロール軸の駆動機構８ｈの背面に密着させ、手首ロール軸と光ビーム照射方向が平行になるようにして、この照射方向が任意の向きにできるようにすることを図っている。

アームの制御は、検知した対象物とエンドエフェクタ作用点との相対距離、平面位置と向きのデータを本発明の検出装置から得ることによって、行われる。

アームロボットの構造は、アーム台座８ａのフランジ上に肩ヨー駆動機構８ｂを、肩ヨー駆動機構の出力端にＬ形の肩ヨーボデイ８ｃを、肩ヨーボデイの端部に肩ピッチ駆動機構８ｄを、肩ピッチ駆動機構の出力端にフォーク形の肩ピッチボデイ８ｅを、肩ピッチボデイのフォークの片側に肘ピッチ駆動機構８ｌを、肘ピッチ駆動機構の出力端に肘ピッチボデイ８ｆを、肘ピッチボデイの内部に手首ピッチ駆動機構を、手首ピッチ駆動機構の出力端にオフセットした形の手首ヨーボデイ８ｇを、手首ヨーボデイの端部に手首ピッチ駆動機構８ｋを、手首ピッチ駆動機構の出力端にＬ形の手首ピッチボデイ８ｊを、手首ピッチボデイの片端に手首ロール駆動機構８ｈを、手首ロール駆動機構の出力端にハンド（エンドエフェクタ）８ｉを配している。

また、この一実施例では、駆動車輪を含む全駆動機構の動力は８０Ｗ以下の電動機を使用し人との共存ができるように配慮している。

前記移動型アームロボットは、いちごの収穫用に考案されたものであり、畝間を走行して左右の畝に成るいちごを、撮像画像より探索し、発見したいちごの３次元位置と向きを検出し、撮像画像の色ヒストグラムで熟れ具合を判定したうえでロボットコントローラへ出力、ロボットコントローラはエンドエフェクタの摘果具でいちごを受け、摘果し、集果場（荷物置き場）に収納するものである。移動型アームロボットは、いちごに限らず果樹の収穫用として広く供されることが考えられる。

従来の産業ロボットが動作環境を固定（決定）して使用されるのに対して、移動型アームロボットは環境に対応して作業するものであり、作業対象物の検知に始まり、相対距離と位置関係、対象物の向きをリアルタイムで検出することが必要であり、従来の技術では大掛かりな装備を余儀なくされていたが、本発明の検出装置により装備が大幅に簡素化される。

本発明の検出装置は、産業ロボットに従来使用されてきた撮像画像の処理に替るものではないが、取組むべき課題とされるエンドエフェクタにおける不確実性（作業ミスの検知、作業の監視、事前の状態の良否、作業完了の検知等）に対するひとつの解となる。

撮像画像の区域と図心を固定したプロフイルデータの時間的変化を監視することにより、位置センサとして使用し、エンドエフェクタに装備される駆動動力を制御することが可能であり、人が目視確認しながら道具を使うように利用することで、作業品質を確保することができる。

移動型アームロボットに示される、従来の技術では大掛かりな装備を要するまたは解決が難しかった環境対応形の作業に本発明を適用することができ、これには多くの用途が見込まれ、例えば、危険な区域において無線ＬＡＮを使用し、自由なアングルで撮像された画像を人が遠隔でリアルタイムで見ながら、画中の対象物を人が指定して、エンドエフェクタを自動で対象物に作用させることも可能である。

１ 光ビーム照射装置
１ａ 外筒
１ｂ 内筒
１ｃ 光ビーム照射モジュール
１ｄ 弾性体の保持部材
１ｅ 光ビーム照射方向調整ボルト
１ｆ 減光板
１ｇ 光ビーム絞穴
１ｈ 光ビーム
２ 撮像カメラ
３ フレーム
４ 被写面
４ａ 参照被写面
５ 作業対象物
６ 演算装置
７ ロボットコントローラ
８ａ アームロボット台座
８ｂ 肩ヨー駆動機構
８ｃ 肩ヨーボデイ
８ｄ 肩ピッチ駆動機構
８ｅ 肩ピッチボデイ
８ｆ 肘ピッチボデイ
８ｇ 手首ピッチボデイ
８ｈ 手首ロール駆動機構
８ｉ ハンド（エンドエフェクタ）
８ｊ 手首ピッチボデイ
８ｋ 手首ピッチ駆動機構
８ｌ 肘ピッチ駆動機構
９ａ 移動台車
９ｂ 駆動車輪
９ｃ 蓄電池、コントローラ、演算機、通信設備等を収納する設備空間カバー
９ｄ 台車カバー
９ｅ 荷物置き場
ＢＰ 撮像画３次元位置検出装置の取付基準面
ｆ カメラ焦点距離（ピクセル）
Ｈ０ 光ビームがカメラ光軸上の垂直面を通る照射点のカメラ焦点からの距離
Ｌ 撮像画３次元位置検出装置の取付基準面から被写面上の光ビーム照射点までの距離
Ｌ０ カメラ光軸を通る垂直面上の光ビーム照射点と検出装置の取付基準面との距離
ＬＣ カメラ光軸
ｏ カメラ焦点
Ｐ 被写面上の対象物中心点
Ｑ 参照被写面上の対象物中心点
Ｓ、Ｓ１、Ｓ７ 被写面上の光ビーム照射点
Ｓ´、Ｓ１´、Ｓ７´ 仮想撮像面上の光ビーム照射点
ＳＹ 光ビームがカメラ光軸上の垂直面を通る点
Ｙｓｃ０ 仮想撮像面上の光ビーム照射点群を結ぶ直線のＹｓｃ軸切片
α カメラ光軸を通る水平面上に光ビームを投影した直線の傾き角
β カメラ光軸を通る垂直面上に光ビームを投影した直線の傾き角

撮像面に写された作業対象物の位置より被写面上の位置を決定するには、焦点ｏから撮像面２ａ上の点ｐ（ｘｐ，ｙｐ）を通る直線ｏｐと被斜面４との交点Ｐを求めることであり、直線ｏｐの方向ベクトルｏｐ（ｘｐ／ｏｐ，ｙｐ／ｏｐ，ｆ／ｏｐ）→より、ここで
ｏｐ＝√（ｘｐ^２＋ｙｐ^２＋ｆ^２）
であり、直線ｏｐの式は
Ｘ／（ｘｐ／ｏｐ）＝Ｙ／（ｙｐ／ｏｐ）＝Ｚ（ｆ／ｏｐ）
であり、
Ｘ＝（ｘｐ／ｏｐ）Ｔ
Ｙ＝（ｙｐ／ｏｐ）Ｔ
Ｚ＝（ｆ／ｏｐ）Ｔ
として、先に示した被写面上の光ビーム照射点Ｓを通る平面の式に代入すると
Ｔ＝ｎｂ→・ｏＳ→／ｎｂ→・ｏｐ→
ここで、ｎｂ→・ｏＳ→とｎｂ→・ｏｐ→は、ベクトルの内積であり成分で表わすと
ｎｂ→・ｏＳ→＝ｎｂＸ・Ｘｉ＋ｎｂＹ・Ｙｉ＋ｎｂＺ・Ｈｉ
ｎｂ→・ｏｐ→＝ｎｂＸ・ｘｐ／ｏｐ＋ｎｂＹ・ｙｐ／ｏｐ＋ｎｂＺ・ｆ／ｏｐ
となる。

ロボット動作方向を、この検出装置に登録する手順と方法は、
（１）被写面上に円板等の対象物を置き、検出装置の取付基準面を被写面に平行な姿勢にする
（２）検出装置に対象物撮像画像の図心（ｘ００，ｙ００）を記憶
（３）ロボット動作Ｘ方向にＸ０２移動
（４）対象物撮像の図心（ｘ０１，ｙ０１）を記憶
により、ロボット動作Ｘ方向ベクトル（ｘ０１−ｘ００，ｙ０１−ｙ００）→が求められる。
ロボット座標系で使われる＋Ｚ方向を手前側にすると、ロボット動作Ｙ方向はこれより反時計回りに９０°の方向となるが、ここで、ロボット座標系と撮像カメラ座標系では、Ｙ方向とＺ方向が逆向きになっていることが多いので注意を要する。

平均半径ｒθｊは正しく形状を表しているが、プロフイルデータｒθｐｊとしては、特徴付けを行うために、角度ゾーンの半径二乗和ｓｕｍｒθｊの平方根をその総和Σ√ｓｕｍｒθｊで除して単位化（データの総和が１）を行ってパーセント（％）で表し、これを式にて表すと
ｒθｐｊ＝√ｓｕｍｒθｊ／（Σ√ｓｕｍｒθｊ）・１００ （但しｊ＝１からＪ）
となる。

一実施例として、表２に対象物距離と輝度差・要素数を、図６に距離とサイズを、図７に距離とプロフイルデータｒθｐｊを示すが、対象物距離が検出されると図６から近い距離の縦横大きさを選び距離補間または補外して対象物のサイズが計算され、対象物の向きは不明なので大きい方の高さを対象物サイズとするが、抽出する撮像画像のサイズが決定される。

ここで、プロフイルデータＲθｐｊは、特徴付けられたｒθｐｊに替り、形状を正しく表す平均半径ｒθｊを使用し、
Ｒθｐｊ＝ｒθｊ
とし、領域固定のプロフイルデータＲθｐｊと称する。

Claims (9)

1. １台の単眼の撮像手段と１個の光ビーム照射手段と演算装置を持つ装置において、
   撮像手段の光軸を通る撮像水平面または撮像垂直面に投影した光ビームと撮像手段の光軸は交差角を有するものであって、
   光ビーム照射方向に直交する被写面上の光ビーム照射点の撮像画像における位置より、あらかじめ測定し較正した近似式を演算して、装置取付基準面と光ビーム照射点の距離を検出する、
   検出装置。
2. 請求項１記載の装置において、被写面上の点の位置を、光ビーム照射点と被写面の法線方向をもって定義される平面を表す数式により演算するものであって、
   被写面の法線方向は光ビーム照射方向であって、
   撮像水平面または垂直面の光ビーム投影直線の傾きを、被写面までの距離の変化量と撮像垂直面または水平面が指す撮像水平面上または垂直面上のスケール読み値変化量、
   によって光ビーム照射方向を、
   実際に撮像された複数の前記光ビーム照射点を撮像手段の仮想撮像面を有する四角錐の仮想撮像空間に配置し、撮像手段の焦点距離と光ビーム照射点が前記垂直面を通る位置の焦点からの距離を、撮像面を含む撮像空間と仮想撮像面を含む仮想撮像空間の相似性の差異を最小化すること、
   によってレーザ光照射点群の位置を、
   決定することにより、
   被写面上の撮像点の位置を、光ビーム照射点を原点とし、別途定義される平面座標系の各軸成分とする、
   検出装置。
3. 請求項１記載の装置において、請求項２記載の被写面を、任意の法線方向を有する参照被写面とし、
   撮像手段の撮像面上の任意の点を参照被写面上に投影し、
   投影点の位置を、光ビーム照射点を原点とし、光ビーム照射方向を１軸とする前記別途定義された平面座標系の各軸成分とする、
   検出装置。
4. 請求項１記載の被写面および請求項２記載の参照被写面に投影される作業対象物であって、
   この対象物を、特定区域の撮像画像の輝度差により抽出される要素で再構成し、
   第１のプロフイルデータを、要素群の図心を中心とし、その周りを等角度分割し、分割域内の要素の個数比率の平方根と平均距離の積を単位化したもの、
   第２のプロフイルデータを、分割域内の要素半径と平均半径の差の絶対値総和を平均半径と要素数で除したもの、
   プロフイルを、撮像時の前記装置取付基準面と光ビーム照射点の距離、輝度差閾値、輝度差要素群の縦横の大きさ、図心位置、輝度差要素数、
   により定義する方法。
5. 請求項４記載の作業対象物にあって、撮像画像中においてこれを探索する方法として、
   請求項１記載の方法により光ビーム照射点の距離を算出し、
   この距離を基準にして、登録された対象物のプロフイルの中から最も近い距離のプロフイルとプロフイルデータを選び、登録プロフイルの縦横大きさより探索ユニットの大きさを決定し、
   撮像全画面に配置した探索ユニットの前記第１プロフイルデータの中で、選択した登録第１プロフイルデータとの絶対差総和が最小の探索ユニットに、対象物が存在すると判定し、
   対象物の中心をこの探索ユニットの輝度差要素群の図心とし、前記撮像面の任意の点をこの図心とする請求項２または請求項３記載の方法による作業対象物の位置、
   検出方法。
6. 請求項５記載の絶対差総和が最小の探索ユニットにあって、
   登録前記第１プロフイルデータとの絶対差総和が大きい場合は、前記第２プロフイルデータの比較を追加することによって真偽の判定を行う、
   検出方法。
7. マニプレータの先端に、前記全請求項に記載の検出装置を具え、本演算装置とロボットコントローラはＬＡＮ接続するものであって、
   通信にてロボットコントローラから本演算装置に、作業対象物の設定と探索命令を出力することにより、
   本演算装置は、装置取付基準面から作業対象物との距離、被写面上の位置と対象物の向きをロボットコントローラに返信することにより、
   ロボットコントローラはマニプレータをリアルタイムで操作する、
   動作制御方法。
8. 請求項４記載の作業対象物にあって、
   定義に使用された撮像画像の特定区域を固定し、登録プロフイルの図心をもって撮像画像の輝度差要素の中心として固定することにより、
   撮像画像より作成された領域固定プロフイルデータと登録の領域固定プロフイルデータとの絶対差より、
   作業前の状態の良否、作業の進行程度、作業の終了と作業失敗の有無等の作業品質に係る事項を検知する、撮像画像中の対象物と登録対象物との差分、
   検出方法。
9. 請求項７記載の作業品質に係る事項を検知することにより、エンドエフェクタの駆動動力を制御する位置を検出し、ロボットコントローラが動力を制御して作業を実行する、
   動作制御方法。

Images