JP6160062B2 - POSITION DETECTING METHOD OF MOBILE BODY, MOBILE BODY POSITION DETECTING DEVICE, AND PART ASSEMBLY - Google Patents
POSITION DETECTING METHOD OF MOBILE BODY, MOBILE BODY POSITION DETECTING DEVICE, AND PART ASSEMBLY Download PDFInfo
- Publication number
- JP6160062B2 JP6160062B2 JP2012238579A JP2012238579A JP6160062B2 JP 6160062 B2 JP6160062 B2 JP 6160062B2 JP 2012238579 A JP2012238579 A JP 2012238579A JP 2012238579 A JP2012238579 A JP 2012238579A JP 6160062 B2 JP6160062 B2 JP 6160062B2
- Authority
- JP
- Japan
- Prior art keywords
- reflection
- convex mirror
- imaging device
- reflected
- axis
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Length Measuring Devices By Optical Means (AREA)
- Manipulator (AREA)
Description
本発明は、移動体の姿勢検出方法及び移動体の姿勢検出装置並びに部品組立装置に関する。 The present invention relates to a mobile body posture detection method, a mobile body posture detection device, and a component assembly device.
部品の組立工程や加工工程では、多関節ロボットを用いて部品を他の部品に組み付けたり、加工したりする。部品のサイズが極めて小さい場合や、部品に精度の高い加工をする場合には、部品や工具を把持するハンドの位置を高精度、例えば数十μm以下の精度で制御しなければならないことがある。このために、多関節ロボットに姿勢検出装置を設け、ハンドの姿勢を検出しながらフィードバック制御してハンドを所望の位置に移動させている。 In a part assembly process or a machining process, a multi-joint robot is used to assemble or process a part to another part. When the size of a part is extremely small or when processing a part with high precision, the position of a hand that grips the part or tool may need to be controlled with high precision, for example, accuracy of several tens of micrometers or less. . For this purpose, an articulated robot is provided with a posture detection device, and the hand is moved to a desired position by feedback control while detecting the posture of the hand.
ハンドの姿勢を検出する方法としては、例えば、多関節ロボットのハンドの先端部をカメラで撮像し、ハンドの先端部の稜線などの特徴からハンドで把持した部品の姿勢を検出することがある。また、別の例では、ハンドにターゲットマークを取り付け、ターゲットマークの位置を、ハンドの位置として検出していた。 As a method for detecting the posture of the hand, for example, the tip of the hand of an articulated robot is imaged with a camera, and the posture of the component gripped by the hand is detected from features such as a ridge line of the tip of the hand. In another example, a target mark is attached to the hand, and the position of the target mark is detected as the position of the hand.
ここで、ハンドに取り付けたターゲットマークを利用してハンドの位置を認識する場合、ターゲットには、凹面鏡と、凹面鏡の周囲に配置された4つのマークとが使用される。4つのマークは、カメラの光軸に垂直で、かつ凹面鏡の曲率中心から光軸方向にオフセットさせた仮想平面上に配置されている。このタイプの姿勢検出装置では、ターゲットマークを照明する光源が1つ設けられている。 Here, when the position of the hand is recognized using the target mark attached to the hand, a concave mirror and four marks arranged around the concave mirror are used as the target. The four marks are arranged on a virtual plane perpendicular to the optical axis of the camera and offset in the optical axis direction from the center of curvature of the concave mirror. In this type of posture detection apparatus, one light source for illuminating the target mark is provided.
1つの光源から出力された光は、凹面鏡で反射される。光源からの光のうち、凹面鏡の曲率中心を通って凹面鏡に入射する光は、凹面鏡に反射され、再び曲率中心を通ってカメラに入射する。凹面鏡における反射光は、曲率中心を通過した光のみがカメラで撮像されるので、カメラの画像には凹面鏡に起因して撮像された輝点が含まれる。このために、輝点の位置から凹面鏡の曲率中心の位置を特定できる。このとき、凹面鏡の曲率中心の位置を基準座標とする。さらに、凹面鏡の曲率中心の位置と、マークの配置は既知であるため、画像中でマークに起因する輝点の配置から、基準座標に対するそれぞれの輝点の相対的な位置を特定できる。これによって、カメラの光軸をZ軸と、Z軸に直交するX軸及びY軸で表される3次元空間内での位置を検出できる。 The light output from one light source is reflected by the concave mirror. Of the light from the light source, light that enters the concave mirror through the center of curvature of the concave mirror is reflected by the concave mirror and enters the camera again through the center of curvature. Since only the light that has passed through the center of curvature is captured by the camera, the reflected light from the concave mirror includes a bright spot imaged due to the concave mirror. For this reason, the position of the center of curvature of the concave mirror can be specified from the position of the bright spot. At this time, the position of the center of curvature of the concave mirror is set as a reference coordinate. Further, since the position of the center of curvature of the concave mirror and the arrangement of the mark are known, the relative position of each bright spot with respect to the reference coordinates can be specified from the arrangement of the bright spot caused by the mark in the image. This makes it possible to detect the position in the three-dimensional space represented by the Z axis as the optical axis of the camera and the X and Y axes orthogonal to the Z axis.
ここで、Z軸方向の位置の変化を検出するためには、撮像画像の輝点の位置の変化量を算出する必要があるが、凹面鏡で反射させた光を撮像して得られる輝点の位置の変化は、実際の距離の変化に比べて縮小される。従って、Z軸方向の位置を精度良く検出することが困難であった。 Here, in order to detect a change in the position in the Z-axis direction, it is necessary to calculate the amount of change in the position of the bright spot in the captured image, but the bright spot obtained by imaging the light reflected by the concave mirror The change in position is reduced compared to the actual change in distance. Therefore, it has been difficult to accurately detect the position in the Z-axis direction.
また、凹面鏡を用いたロボットハンドの姿勢検出処理では、3次元空間における位置と、Z軸回りの回転角度であるθzと、X軸回りの回転角度であるθxと、Y軸回りの回転角度であるθyの6種類のデータを精度良く取得することは困難であった。例えば、6種類のデータを取得するためには、角度が異なる位置にカメラを2つ配置し、2つのカメラで取得した2つの画像を用いて画像処理を行う必要がある。このために、装置構成が複雑化、及び大型化すると共に、コストアップの原因となっていた。
この発明は、このような事情に鑑みてなされたものであり、移動体の位置を簡単な構成で精度良く検出できるようにすることを目的とする。
Also, in the robot hand posture detection process using the concave mirror, the position in the three-dimensional space, the rotation angle around the Z axis, θz, the rotation angle around the X axis, and the rotation angle around the Y axis. It was difficult to accurately obtain six types of data of a certain θy. For example, in order to acquire six types of data, it is necessary to arrange two cameras at positions with different angles and perform image processing using two images acquired by the two cameras. For this reason, the configuration of the apparatus is complicated and large, and the cost is increased.
This invention is made in view of such a situation, and it aims at enabling it to detect the position of a mobile body accurately with a simple structure.
実施形態の一観点によれば、凸面ミラーと、前記凸面ミラーの周囲に配置された複数の反射部材とを有する移動体に光源からの光を照射し、前記凸面ミラーと、複数の前記反射部材のそれぞれにおける反射光を撮像装置で撮像し、前記光源の光の反射像を含む画像を作成する工程と、前記凸面ミラー上の反射点と前記反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第1の角度を算出する工程と、前記反射点と凸面ミラーの曲率中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第2の角度と、前記光源と前記撮像装置の光軸との間の距離と、前記第1の角度とを用い、前記光源から光の前記反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する工程と、前記撮像装置の光軸を中心とする仮想円上に配置された前記光源からの光が前記凸面ミラーで反射することで撮像された複数の前記反射像を結ぶ仮想直線の交点から複数の前記反射像の中心を算出する工程と、複数の前記反射部材における反射光を前記撮像装置で撮影して得られた複数の反射像のうち、複数の前記反射像の中心を基準にして対向して配置される2つの前記反射像を結ぶ仮想直線の傾きを算出し、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度を算出する工程と、前記移動体の姿勢を変化させながら、前記凸面ミラーの前記反射点までの距離と複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度とを算出し、前記凸面ミラーの前記反射点までの距離と、複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度と、目標とする距離と位置と角度を有する基準位置との偏差がゼロになる姿勢に前記移動体を移動させる指令値を算出する工程と、を含むことを特徴とする移動体の姿勢検出方法が提供される。
According to one aspect of the embodiment, shines a convex mirror, the light from the light source to the moving body having a plurality of reflecting members disposed around the convex mirror irradiation, and the convex mirror, a plurality of the reflection the reflected light definitive to each member imaged by the imaging device, comprising the steps of creating an image including a reflection image of the light of the light source, through the reflected image and the reflection point on the convex mirror, the lens of the imaging device A step of calculating a first angle formed by a virtual line passing through the principal point and the optical axis of the imaging device, and a virtual line connecting the reflection point and the center of curvature of the convex mirror and the optical axis of the imaging device are formed. When the second angle, the distance between the light source and the optical axis of the imaging device, and the first angle are used, the incident angle and the reflection angle at the reflection point of light from the light source coincide with each other. The distance from the imaging device to the reflection point of the convex mirror A step of calculating the intersection of the virtual line connecting a plurality of the reflected image light is captured by the reflection by the convex mirror from the light source disposed on a virtual circle centered on the optical axis of the imaging device And calculating the centers of the plurality of reflection images from the plurality of reflection images obtained by photographing the reflected light from the plurality of reflection members with the imaging device. Calculating an inclination of an imaginary line connecting the two reflected images arranged opposite to each other , calculating an angle formed by an axis perpendicular to the optical axis of the imaging device and the imaginary line , While changing the posture, the distance to the reflection point of the convex mirror, the position of the center of the plurality of reflected images, and the angle formed by the axis line orthogonal to the optical axis of the imaging device and the virtual line are calculated. To the reflection point of the convex mirror Deviation the distance, the position of the center of the plurality of reflected images, and angle between the axis and the imaginary straight line perpendicular to the optical axis of the imaging device, a reference position having a position and angle and distance to the target of And a step of calculating a command value for moving the movable body to a posture where the zero becomes zero.
また、実施形態の別の観点によれば、移動体に取り付けられた凸面ミラーと、前記移動体に取り付けられ、前記凸面ミラーの周囲に配置された複数の反射部材と、前記凸面ミラー及び複数の前記反射部材で反射した光を受光する撮像装置と、前記撮像装置の光軸を囲むように配置され、前記凸面ミラー及び拡散反射部材を照明する光源と、前記凸面ミラー上の反射点と前記反射点で反射した光を前記撮像装置で撮像した反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第1の角度を算出し、前記反射点と凸面ミラーの曲率中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第2の角度と、前記光源と前記撮像装置の光軸との間の距離とを用い、前記光源から光の前記第1の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出し、前記撮像装置の光軸を中心とする仮想円上に配置された前記光源からの光が前記凸面ミラーで反射することで撮像された複数の前記反射像を結ぶ仮想直線の交点から複数の前記反射像の中心を算出し、複数の前記反射部材における反射光を前記撮像装置で撮影して得られた複数の反射像のうち、複数の前記反射像の中心を基準にして対向して配置される2つの前記反射像を結ぶ仮想直線の傾きを算出し、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度を算出し、前記移動体の姿勢を変化させながら、前記凸面ミラーの前記反射点までの距離と複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度とを算出し、前記凸面ミラーの前記反射点までの距離と、複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度と、目標とする距離と位置と角度を有する基準位置との偏差がゼロになる姿勢に前記移動体を移動させる指令値を算出する処理部と、を含むことを特徴とする移動体の姿勢検出装置が提供される。
According to another aspect of the embodiment, a convex mirror attached to a moving body, a plurality of reflecting members attached to the moving body and arranged around the convex mirror , the convex mirror, and a plurality of an imaging device for receiving the light reflected by the reflection member is arranged to surround the optical axis of the imaging device, the reflective light source for illuminating the convex mirror and the diffuse reflection member, the reflection point on the convex mirror A first angle formed by an imaginary line passing through a reflected image captured by the image capturing device and passing through a principal point of the lens of the image capturing device and an optical axis of the image capturing device; And a distance between the light source and the optical axis of the imaging device, and a second angle formed by an imaginary line connecting the curvature center of the convex mirror and the optical axis of the imaging device and a distance between the light source and the optical axis of the imaging device. Incident angle and reflection at the first reflection point Wherein calculating the distance to the reflection point of the convex mirror from the image pickup device, light the convex mirror from the light source disposed on a virtual circle centered on the optical axis of the image pickup apparatus when the bets are matched in calculating the centers of the reflection image from the intersection of the virtual line connecting a plurality of the reflection image captured by reflection, obtained by photographing the reflected light from the plurality of the reflecting member by the imaging device An inclination of an imaginary straight line connecting two reflection images arranged opposite to each other with reference to the center of the plurality of reflection images among a plurality of reflection images, and an axis line orthogonal to the optical axis of the imaging device; While calculating the angle formed by the virtual straight line and changing the posture of the moving body, the distance to the reflection point of the convex mirror, the center positions of the plurality of reflected images, and the optical axis of the imaging device An orthogonal axis and the virtual straight line Calculating the angle formed, and the distance to the reflection point of said convex mirror, the position of the center of the plurality of reflected images, and angle between the axis and the imaginary straight line perpendicular to the optical axis of the imaging device, And a processing unit that calculates a command value for moving the moving body to a posture in which a deviation between a target distance, a position, and a reference position having an angle is zero. Provided.
撮像装置と移動体との間の距離を光源の光を凸面ミラーで反射させることによって測定するようにしたので、距離の変化に伴って反射像の位置の変化が大きくなり易い。このために、反射像の位置を利用した距離の計算の精度が向上する。 Since the distance between the imaging device and the moving body is measured by reflecting the light from the light source with the convex mirror, the change in the position of the reflected image tends to increase with the change in the distance. For this reason, the accuracy of distance calculation using the position of the reflected image is improved.
発明の目的及び利点は、請求の範囲に具体的に記載された構成要素及び組み合わせによって実現され達成される。
前述の一般的な説明及び以下の詳細な説明は、典型例及び説明のためのものであって、本発明を限定するためのものではない。
The objects and advantages of the invention will be realized and attained by means of the elements and combinations particularly pointed out in the appended claims.
The foregoing general description and the following detailed description are exemplary and explanatory only and are not intended to limit the invention.
最初に、実施形態の原理について説明する。
図1及び図2に姿勢検出系の概略構成を示すように、姿勢検出装置1は、姿勢検出の目的対象となる移動体に取り付けられるターゲット部材10を有する。移動体は、例えば、多関節ロボットのハンドなどの移動体の先端部分である。ターゲット部材10自体が移動体であっても良い。また、姿勢とは、ターゲット部材10の3次元空間における位置と、直交する3軸のそれぞれの軸回り回転角度を含むものとする。
First, the principle of the embodiment will be described.
As shown in the schematic configuration of the posture detection system in FIGS. 1 and 2, the
ターゲット部材10は、中央に第1のターゲットマークとなる凸面ミラー11を有する。凸面ミラー11は、曲率中心がm、曲率半径がRである。さらに、ターゲット部材10は、凸面ミラー11の周囲に第2のターゲットマークとなる4つの拡散反射面12A,12B,12C,12D(拡散反射部材)が凸面ミラー11の外周に沿って配置されている。各拡散反射面12A〜12Dは、四角形で、凸面ミラー11の外周と同心の仮想円の円周方向に等間隔に配置されている。
The
また、姿勢検出装置1は、ターゲット部材10から所定距離だけ上方に離れた位置に、カメラ13(撮像装置)と、4つの点光源LS1,LS2,LS3,LS4を有する。カメラ13は、光軸C1が凸面ミラー11の曲率中心mを通り、かつ凸面ミラー11及び4つの拡散反射面12A〜12Dを撮像可能な向きに配置されている。
In addition, the
4つの点光源LS1〜LS4は、カメラ13の光軸C1を中心とする仮想円の円周上に等間隔で配置されている。カメラ13の光軸C1を挟んで配置される2つの点光源LS1,LS3を結ぶ仮想線の中心は、カメラ13の光軸C1に一致するように配置されている。同様に、カメラ13の光軸C1を挟んで配置される2つの点光源LS2,LS4を結ぶ仮想線の中心は、カメラ13の光軸C1に一致するように配置されている。このような点光源LS1〜LS4の配置の一例として、各点光源LS1〜LS4は、ターゲット部材10の各拡散反射面12A〜12Dの上方に1つずつ配置される。点光源LS1〜LS4と、拡散反射面12A〜12Dの数と配置は、同じ数及び光軸C1回りに同じ配置であることが好ましいが、両者の数又は配置を異ならせることもできる。
The four point light sources LS1 to LS4 are arranged at equal intervals on the circumference of a virtual circle centered on the optical axis C1 of the
ここで、ターゲット部材10は、例えば、点光源LS1〜LS4の光を反射しない材料から製造され、凸面ミラー11及び4つの拡散反射面12A〜12Dは、点光源LS1〜LS4の光を反射する材料、例えばガラスや石英を用いて製造されている。
Here, the
図2に示すように、カメラ13は、撮像レンズ21と、撮像面22を有する。撮像面22は、例えば、CCD(Charge Coupled Device)が用いられる。カメラ13で撮像したターゲット部材10の画像の一例を図3に示す。画像25には、画像の中心で直交する2つの直線を有し、これら直線がX軸と、Y軸になっている。X軸及びY軸に直交するZ軸であり、図2においてZ軸はカメラ13の光軸と一致している。
As shown in FIG. 2, the
画像25には、円形の4つの第1の反射像q1,q2,q3,q4と、四角形の第2の反射像s1,s2,s3,s4が含まれる。4つの第1の反射像q1〜q4は、4つの点光源LS1〜LS4が凸面ミラー11で反射されることによって形成された像であり、互いの距離が狭い。4つの第2の反射像s1〜s4、点光源LS1〜LS4の光が反射することによって形成されることから、典型的には円形になる。
The
また、4つの第2の反射像s1〜s4は、4つの拡散反射面12A〜12Dで反射されることによって形成された像であり、互いの距離が広い。4つの第2の反射像s1〜s4は、拡散反射面12A〜12Dの形状に従って、典型的に四角形状になる。 The four second reflection images s1 to s4 are images formed by being reflected by the four diffuse reflection surfaces 12A to 12D, and have a wide distance from each other. The four second reflected images s1 to s4 typically have a quadrangular shape according to the shape of the diffuse reflection surfaces 12A to 12D.
次に、姿勢検出方法について説明する。姿勢検出の際には、最初に初期化処理を行う。さらに、移動体の姿勢を変化させながら、ターゲット部材10を用いて姿勢検出を行い、検出結果を利用してフィードバック制御することによって、ターゲット部材10が取り付けられた移動体を基準位置に導く。以下に、各処理の詳細について説明する。
Next, the attitude detection method will be described. When detecting the posture, initialization processing is first performed. Further, the posture detection is performed using the
最初に、図4のフローチャートを参照して、姿勢位置検出処理の初期化処理について説明する。
ステップS101で、ターゲット部材10を初期化位置に移動させる。ターゲット部材10が多関節ロボットのロボットハンドに取り付けられている場合には、ロボットハンドを初期位置に移動させる。
次に、ステップS102では、カメラ13でターゲットマークを撮像する。具体的には、4つの点光源LS1〜LS4を点灯させてターゲット部材10に光を照射し、凸面ミラー11と4つの拡散反射面12A〜12Dのそれぞれにおける各点光源LS1〜LS4の反射光をカメラ13で撮像する。
First, the initialization process of the posture position detection process will be described with reference to the flowchart of FIG.
In step S101, the
Next, in step S102, the target mark is imaged by the
この後、ステップS103では、Z軸位置検出処理を行い、Z軸上の位置を示すZ軸位置値を算出する。Z軸位置検出処理の詳細は、後に説明する。続いて、ステップS104で、カメラ13の倍率βsを算出する。図2に示す例において倍率βsは、F/Zで算出できる。ここで、Fは、カメラ13の撮像レンズ21の主点nから撮像面22までの距離であり、既知の値である。Zは、ステップS103で算出される値が用いられる。そして、ステップS105で、拡散反射面12Bと拡散反射面12Dの間の距離である拡散反射面間距離Rxを2つの第2の反射像s2,s4の間の距離として算出する。また、拡散反射面12Aと拡散反射面12Cの間の距離である拡散反射面間距離Ryを2つの第2の反射像s1,s3の間の距離として算出する。図3の画像25に示すように、対向する2点間の距離Rx、Ryは、
Thereafter, in step S103, a Z-axis position detection process is performed to calculate a Z-axis position value indicating a position on the Z-axis. Details of the Z-axis position detection process will be described later. Subsequently, in step S104, the magnification βs of the
で算出される。ここまでの処理が実行したら、各データを不図示の記憶装置に記憶し、初期化処理を終了する。 Is calculated by When the processing so far is executed, each data is stored in a storage device (not shown), and the initialization processing is terminated.
次に、図4のステップS103のZ軸位置検出処理の詳細について、図5のフローチャートを参照して説明する。Z軸位置検出処理は、カメラ13が凸面ミラー11における反射光を撮像したときに得られる4つの第1の反射像q1〜q4の1つ1つに着目して実施される。
最初のステップS201では、図3の画像25内における4つの第1の反射像q1〜q4の第1の中心g1(x,y)を算出する。第1の中心g1(x,y)は、4つの第1の反射像q1〜q4の重心でもある。
Next, details of the Z-axis position detection process in step S103 of FIG. 4 will be described with reference to the flowchart of FIG. The Z-axis position detection process is performed by paying attention to each of the four first reflected images q1 to q4 obtained when the
In the first step S201, first centers g1 (x, y) of the four first reflected images q1 to q4 in the
具体的には、図6A示す第1の反射像q1と第1の反射像q3を結ぶ仮想線L13の式を求める。仮想線L13は、
Y1=(q1(y)−q3(y))/(q1(x)−q3(x))×X1+b1
になる。ここで、X1とY1は、それぞれX軸上の座標と、Y軸上の座標を示す。b1は、仮想線L13がY軸と交差する点のY座標に相当する。
Specifically, an equation of an imaginary line L13 connecting the first reflected image q1 and the first reflected image q3 shown in FIG. 6A is obtained. The virtual line L13 is
Y1 = (q1 (y) −q3 (y)) / (q1 (x) −q3 (x)) × X1 + b1
become. Here, X1 and Y1 indicate coordinates on the X axis and coordinates on the Y axis, respectively. b1 corresponds to the Y coordinate of the point where the virtual line L13 intersects the Y axis.
同様に、第1の反射像q2と第1の反射像q4を結ぶ仮想線L24の式を求める。仮想線L24は、
Y2=(q2(y)−q4(y))/(q2(x)−q4(x))×X2+b2
になる。ここで、X2とY2は、それぞれX軸上の座標と、Y軸上の座標を示す。b2は、仮想線L24がY軸と交差する点のY座標に相当する。
Similarly, an equation of an imaginary line L24 connecting the first reflected image q2 and the first reflected image q4 is obtained. The virtual line L24 is
Y2 = (q2 (y) -q4 (y)) / (q2 (x) -q4 (x)) * X2 + b2
become. Here, X2 and Y2 indicate coordinates on the X axis and coordinates on the Y axis, respectively. b2 corresponds to the Y coordinate of the point where the virtual line L24 intersects the Y axis.
ここで、仮想線L13と仮想線L24とは、交差、より具体的には直交しており、これら2つの仮想線L13、L24の交点が4つの第1の反射像q1〜q4の第1の中心g1(x,y)になる。ここで、カメラ13の撮像系の座標と点光源LS1〜LS4との間にZθ方向の角度オフセットを設けない場合には、仮想線L13がX軸と平行なり、仮想線L24がY軸と平行になるので、第1の中心g1(q2(x),q1(y))になる。
Here, the virtual line L13 and the virtual line L24 intersect, more specifically, are orthogonal, and the intersection of these two virtual lines L13 and L24 is the first of the four first reflected images q1 to q4. It becomes the center g1 (x, y). Here, when the angle offset in the Zθ direction is not provided between the coordinates of the imaging system of the
続く図5のステップS202からステップS204は、4つの第1の反射像q1〜q4を1ずつ順番に抽出することによって実施される。まず、ステップS202で、角度αn(第1の角度)を算出する。ここで、図7に示すように、αnは、カメラ13の撮像レンズ21の主点nを通過し、凸面ミラー11の反射点pnを結ぶ線分と、カメラ13の光軸とのなす角度である。例えば、n=1のときは、点光源LS1の光が凸面ミラー11で反射されたポイント、即ち反射点p1と、反射点p1における反射光が撮像レンズ21を通って撮像面22で撮像された第1の反射像q1とを用いて計算される。
The subsequent steps S202 to S204 in FIG. 5 are performed by sequentially extracting the four first reflected images q1 to q4 one by one. First, in step S202, an angle αn (first angle) is calculated. Here, as shown in FIG. 7, αn is an angle formed by a line segment passing through the principal point n of the
具体的には、図7を参照し、レンズの主点nを通り、凸面ミラー11の反射点pnを結ぶ線分とカメラ13の光軸をなす角度αnを算出する。角度αnは、
αn=tan−1((en+jn)/F)
と表せる。ここで、(en+jn)は、凸面ミラー11からの第1の反射像qnとカメラ13の光軸の中心からの距離である。その内訳は、enが第1の中心g1から第1の反射像qnまでの距離であり、jnが光軸c1と凸面ミラー11の中心とのずれ量である。例えば、点光源LS1又は点光源LS3を用いて角度α1又は角度α3を算出する場合には、(en+jn)は、x軸の値が用いられる。一方、点光源LS2又は点光源LS4を用いて角度α2又は角度α4を算出する場合には、(en+jn)は、y軸の値が用いられる。また、Fは、撮像レンズ21の主点nから撮像面22までの距離である。
Specifically, referring to FIG. 7, an angle αn that forms a line segment that passes through the principal point n of the lens and connects the reflection point pn of the
αn = tan −1 ((en + jn) / F)
It can be expressed. Here, (en + jn) is a distance from the first reflected image qn from the
さらに、図5のステップS203で、距離Znを算出する。距離Znは、撮像レンズ21の主点nから凸面ミラー11の反射点までの距離であって、光軸に沿った長さである。例えば、n=1のときは、点光源LS1の反射点p1、反射像q1を用いて距離Z1が算出される。
Further, in step S203 of FIG. 5, the distance Zn is calculated. The distance Zn is a distance from the principal point n of the
具体的には、最初に、図7に示す点光源LSnから反射点pnに入射する光の入射角φ´を求める。入射角φ´は、
φ´=tan−1((L−h)/Zn)―γ (1)
で表せる。ここで、Lは、カメラ13の光軸から点光源LSnまでの距離である。hは、カメラ13の光軸から反射点p1までの距離で、
h=Zn×tan(α) (2)
である。また、γは、凸面ミラー11の曲率中心mと反射点pnを結ぶ直線と、カメラ13の光軸とがなす角度(第2の角度)である。
Specifically, first, an incident angle φ ′ of light incident on the reflection point pn from the point light source LSn shown in FIG. 7 is obtained. The incident angle φ ′ is
φ ′ = tan −1 ((L−h) / Zn) −γ (1)
It can be expressed as Here, L is the distance from the optical axis of the
h = Zn × tan (α) (2)
It is. Further, γ is an angle (second angle) formed by a straight line connecting the center of curvature m of the
ここで、角度γは、
γ=tan−1(h−h0)/R) (3)
で表せる。h0は、カメラ13の光軸からの凸面ミラー11の曲率中心mまでのオフセット量である。オフセット量は、点光源LS1又は点光源LS3を用いて角度γを算出する場合には、X軸に沿った長さが用いられる。一方、点光源LS2又は点光源LS4を用いて角度γを算出する場合には、Y軸に沿った長さが用いられる。
Where the angle γ is
γ = tan −1 (h−h0) / R) (3)
It can be expressed as h0 is an offset amount from the optical axis of the
続いて、点光源LSnからの光が反射点pnにおいて反射したときに生じる反射光の出射角φを求める。出射角φは、
φ=α+γ (4)
で表せる。ここで、入射角度φ´と出射角φは、一致するので、(1)〜(4)の式を用いることによって、Z1を算出できる。
Subsequently, an emission angle φ of the reflected light generated when the light from the point light source LSn is reflected at the reflection point pn is obtained. The exit angle φ is
φ = α + γ (4)
It can be expressed as Here, since the incident angle φ ′ and the outgoing angle φ coincide with each other, Z1 can be calculated by using the equations (1) to (4).
この後、図5のステップS204の判定処理を実行することによって、ステップS202からステップS204を繰り返し、残りの3つの点光源LS2〜LS4の反射点p2,p3,p4に対するそれぞれの距離Z2、Z3、Z4を算出する。そして、4つの点光源LS1〜LS4について算出した4つの距離Z1〜Z4は、本来であれば同一の値であるべきなので、ステップS205で、4つの距離Z1〜Z4の平均値を算出し、これによって得られた値をZ軸位置とする。具体的には、n=2のとき、点光源LS2におけるZ2が算出される。さらに、n=3,4のとき、それぞれ点光源LS3、LS4に対するZ3、Z4が算出される。そして、Znを4つの点光源LS1〜LS4及びその反射点p1〜p4、第1の反射像q1〜q4を用いて算出したら、Z軸位置値を求める。Z軸位置値は、Z=(Z1+Z2+Z3+Z4)/4の式によって求められる。 Thereafter, by executing the determination process of step S204 in FIG. 5, steps S202 to S204 are repeated, and the respective distances Z2, Z3, and the reflection points p2, p3, and p4 of the remaining three point light sources LS2 to LS4, respectively. Z4 is calculated. Then, since the four distances Z1 to Z4 calculated for the four point light sources LS1 to LS4 should be the same value, the average value of the four distances Z1 to Z4 is calculated in step S205. The value obtained by is used as the Z-axis position. Specifically, when n = 2, Z2 in the point light source LS2 is calculated. Further, when n = 3 and 4, Z3 and Z4 are calculated for the point light sources LS3 and LS4, respectively. When Zn is calculated using the four point light sources LS1 to LS4, the reflection points p1 to p4, and the first reflection images q1 to q4, the Z-axis position value is obtained. The Z-axis position value is determined by the equation Z = (Z1 + Z2 + Z3 + Z4) / 4.
ここまでの処理で、初期状態のデータが得られたので、次に、図8に示すフローチャートに従って、移動体の姿勢を変化させながら、移動体をフィードバック制御する。
まず、ステップS301で、ターゲット部材10の位置を移動させたときの姿勢位置検出処理を実行する。姿勢位置検出処理の詳細は、後に説明する。続いて、ステップS302で姿勢偏差検出処理を行い、目標となる基準位置情報と現在の姿勢位置情報との偏差を算出する。そして、ステップS303の判定処理で、基準位置情報と現在の姿勢位置情報の偏差がゼロであれば、姿勢制御処理を終了する。その一方で、偏差がゼロでなければ、誤差分だけ姿勢移動制御を実施して移動体の姿勢を変化させた後、ステップS301に戻る。
Since the data of the initial state is obtained by the processing so far, next, the moving body is feedback-controlled while changing the posture of the moving body according to the flowchart shown in FIG.
First, in step S301, a posture position detection process when the position of the
さらに、ステップS301の姿勢位置検出処理の詳細について、図9のフローチャートを参照して説明する。最初に、ステップS401で、ターゲットマークを撮像する。具体的には、4つの点光源LS1〜LS4を点灯させ、凸面ミラー11と4つの拡散反射面12A〜12Dのそれぞれにおける反射光をカメラ13で撮像する。
Further, details of the posture position detection processing in step S301 will be described with reference to the flowchart of FIG. First, in step S401, a target mark is imaged. Specifically, the four point light sources LS <b> 1 to LS <b> 4 are turned on, and the reflected light from each of the
続くステップS402で、Z軸位置検出処理を実行する。ここでの処理は、図5と同様であり、移動後のターゲット部材10のZ軸位置値が算出される。さらに、ステップSS403で、XY軸位置検出処理を実行し、移動後のターゲット部材10のX軸上の位置値と、Y軸上の位置値を算出する。また、4つの拡散反射面12A〜14Dに対する各第2の反射像s1〜s4の第2の中心(重心)g2´(x,y)、及び移動後の倍率βmを算出する。この後、ステップS404において、Z−θ軸位置検出処理を実行し、Z−θ軸位置値を算出する。Z−θ軸位置値とは、Z軸回りの回転角度である。そして、ステップS405で、X−θ軸、Y−θ軸の位置検出処理を実行し、X−θ軸位置値と、Y−θ軸位置値とを算出する。X−θ軸位置値と、Y−θ軸位置値は、それぞれX軸回りの回転角度と、Y軸回りの回転角度である。
In subsequent step S402, a Z-axis position detection process is executed. The processing here is the same as in FIG. 5, and the Z-axis position value of the
以下に、ステップS403のX軸位置値とY軸位置値の具体的な算出処理の具体例について説明する。
まず、図6Bに、移動後のターゲット部材10からの反射光を撮像した画像25Aを示す。画像25A中において、q1´〜q4´は、点光源LS1〜LS4からの光が移動後の凸面ミラー11にそれぞれ反射されることによって形成された反射像である。また、s1´〜s4´は、点光源LS1〜LS4からの光が移動後の移動後の拡散反射面12A〜12Dにそれぞれ反射されることによって形成された反射像である。
A specific example of specific calculation processing of the X-axis position value and the Y-axis position value in step S403 will be described below.
First, FIG. 6B shows an
最初に、図6Bに示す第2の反射像s1´と第2の反射像s3´を結ぶ仮想線J13の式を求める。仮想線J13は、
Y3=(s1´(y)−s3´(y))/(s1´(x)−s3´(x))×X1+b3
になる。ここで、X3とY3は、それぞれX軸上の座標と、Y軸上の座標を示す。b3は、仮想線J13がY軸と交差する点のY座標に相当する。
First, an equation of an imaginary line J13 connecting the second reflected image s1 ′ and the second reflected image s3 ′ shown in FIG. 6B is obtained. The virtual line J13 is
Y3 = (s1 ′ (y) −s3 ′ (y)) / (s1 ′ (x) −s3 ′ (x)) × X1 + b3
become. Here, X3 and Y3 indicate coordinates on the X axis and coordinates on the Y axis, respectively. b3 corresponds to the Y coordinate of the point where the virtual line J13 intersects the Y axis.
同様に、第2の反射像s2´と第2の反射像s4´を結ぶ仮想線J24の式を求める。仮想線J24は、
Y4=(s2´(y)−s4´(y))/(s2´(x)−s4´(x))×X2+b4
と表せる。ここで、X4とY4は、それぞれX軸上の座標と、Y軸上の座標を示す。b4は、仮想線J24がY軸と交差する点のY座標に相当する。仮想線J13と仮想線J24とは交差しており、これら2つの仮想線J13、J24の交点が4つの第2の反射像s1´〜s4´の第2の中心g2´(x,y)になる。
Similarly, an equation of an imaginary line J24 connecting the second reflected image s2 ′ and the second reflected image s4 ′ is obtained. Virtual line J24 is
Y4 = (s2 ′ (y) −s4 ′ (y)) / (s2 ′ (x) −s4 ′ (x)) × X2 + b4
It can be expressed. Here, X4 and Y4 indicate the coordinates on the X axis and the coordinates on the Y axis, respectively. b4 corresponds to the Y coordinate of the point where the virtual line J24 intersects the Y axis. The virtual line J13 and the virtual line J24 intersect, and the intersection of these two virtual lines J13 and J24 is at the second center g2 ′ (x, y) of the four second reflected images s1 ′ to s4 ′. Become.
また、図10に示すように、倍率βmは、βm=M/N=F/Zから算出できる。この結果、ターゲット部材10上の実際の中心gr´2のX軸位置値は、g2´(x)/βmで算出できる。同様に、中心のY軸位置値は、g2´(y)/βmで算出できる。そして、座標(g2´(x)/βm,g2´(y)/βm)がターゲット部材10のXY平面における実位置となる。
Further, as shown in FIG. 10, the magnification βm can be calculated from βm = M / N = F / Z. As a result, the X-axis position value of the actual center gr′2 on the
次に、図9のステップS404のZ−θ軸の位置検出処理の具体例について説明する。
まず、第2の反射像s1´と第2の反射像s3´を結ぶ仮想線J13と、カメラ13のX軸のなす角度をZθ1´とする。同様に、第2の反射像s2´と第2の反射像s4´を結ぶ仮想線J24と、カメラ13のY軸のなす角度をZθ2´とする。このとき、
Zθ1´=tan−1((s3´(y)−s1´(y))/(s3´(x)−s1´(x)))
Zθ2´=tan−1((s4´(y)−s2´(y))/(s4´(x)−s2´(x)))
と表せる。そして、Zθ1´とZθ2´の平均値、即ち(Zθ1´+Zθ2´)/2を算出すれば、Z軸回りの回転角度であるZ−θ値Zθ´が得られる。
Next, a specific example of the Z-θ axis position detection process in step S404 in FIG. 9 will be described.
First, an angle formed by an imaginary line J13 connecting the second reflected image s1 ′ and the second reflected image s3 ′ and the X axis of the
Zθ1 ′ = tan −1 ((s3 ′ (y) −s1 ′ (y)) / (s3 ′ (x) −s1 ′ (x)))
Zθ2 ′ = tan −1 ((s4 ′ (y) −s2 ′ (y)) / (s4 ′ (x) −s2 ′ (x)))
It can be expressed. If an average value of Zθ1 ′ and Zθ2 ′, that is, (Zθ1 ′ + Zθ2 ′) / 2 is calculated, a Z−θ value Zθ ′ that is a rotation angle about the Z axis is obtained.
次に、図9のステップS405のターゲット部材10のX−θ軸と、Y−θ軸の検出処理の具体例について説明する。
最初に、初期状態の倍率βsを基準にして、ターゲット部材10の姿勢が変化したときの拡散反射面12A,12Cの第2の反射像s1,s3の2点間の距離Ry´と、拡散反射面12B,12Dの第2の反射像s2,s4の2点間の距離Rx´をそれぞれ算出する。そして、回転方向は、凸面ミラー11の第1の反射像q1´〜q4´の第1の中心g1´と、拡散反射面12A〜12Dの第2の反射像s1´〜s4´の第2の中心g2´の位置関係から決定する。
Next, a specific example of the X-θ axis and Y-θ axis detection processing of the
First, the distance Ry ′ between the two points of the second reflection images s1 and s3 of the diffuse reflection surfaces 12A and 12C when the posture of the
ターゲット部材10の初期状態における拡散反射面12A〜12Dの第2の反射像s1〜s4の2点間の距離Rx,Ryは、既に図4のステップS105の処理で算出されている。また、初期状態における倍率βsもステップS104において算出されている。このために、これらの値は、既に算出したものを用いる。
次に、ターゲット部材10の姿勢を変化させた後の拡散反射面12A〜12Dの第2の反射像s1´〜s4´の2点間の距離は、
The distances Rx and Ry between the two points of the second reflection images s1 to s4 of the diffuse reflection surfaces 12A to 12D in the initial state of the
Next, the distance between the two points of the second reflection images s1 ′ to s4 ′ of the diffuse reflection surfaces 12A to 12D after the posture of the
で算出できる。また、移動後の倍率βmは、F/Zmで算出される。ここで、Zmは、図5と同様の処理によって算出される。 It can be calculated by Further, the magnification βm after movement is calculated by F / Zm. Here, Zm is calculated by the same processing as in FIG.
次に、X軸回りの回転角度であるX−θ値Xθ´と、Y軸回りの回転角度であるY−θ値Yθ´を算出する。図11Aに模式的に示すように、初期状態の拡散反射面12B,12Dが、移動後には、拡散反射面12B´,12D´になるので、
Xθ´=cos−1(Rx´/βm)/(Rx/βs))
となる。ここで、Rx/βsは、画像25上の第2の反射像s2,s4の2点間の距離Rxを倍率βsで割り算したもので、初期位置における拡散反射面12B,12D間の実距離である。同様に、Rx´/βmは、画像25上の第2の反射像s2´,s4´の2点間の距離Rx´を倍率βmで割り算したもので、移動後の拡散反射面12B,12D間の実距離である。
Next, an X-θ value Xθ ′ that is a rotation angle around the X axis and a Y-θ value Yθ ′ that is a rotation angle around the Y axis are calculated. As schematically shown in FIG. 11A, the diffuse reflection surfaces 12B and 12D in the initial state become the diffusion reflection surfaces 12B ′ and 12D ′ after the movement.
Xθ ′ = cos −1 (Rx ′ / βm) / (Rx / βs))
It becomes. Here, Rx / βs is obtained by dividing the distance Rx between the two points of the second reflected images s2 and s4 on the
同様に、図11Bに模式的に示すように、初期状態の拡散反射面12A,12Cが、移動後には、拡散反射面12A´,12C´になるので、
Yθ=cos−1(Ry´/βm)/(Ry/βs))
となる。ここで、Ry/βsは、画像25上の第2の反射像s1,s3の2点間の距離Ryを倍率βsで割り算したもので、初期位置における拡散反射面12A,12C間の実距離である。同様に、Ry´/βmは、画像25上の第2の反射像s1´,s3´の2点間の距離Ry´を倍率βmで割り算したもので、移動後の拡散反射面12A,12C間の実距離である。
Similarly, as schematically shown in FIG. 11B, the diffuse reflection surfaces 12A and 12C in the initial state become the diffusion reflection surfaces 12A ′ and 12C ′ after the movement.
Yθ = cos −1 (Ry ′ / βm) / (Ry / βs))
It becomes. Here, Ry / βs is obtained by dividing the distance Ry between the two points of the second reflected images s1 and s3 on the
さらに、ターゲット部材10の回転方向を判定する。ターゲット部材10の回転方向は、X−θ軸においては、g1´(y)>g2´(y)のときは、拡散反射面12D側がカメラ13より遠くなる方向に回転している判定する。また、g1´(y)<g2´(y)のときは、拡散反射面12B側がカメラ13より遠くなる方向に回転している判定する。そして、g1´(y)=g2´(y)のときは、X−θ軸の傾きはゼロであると判定する。
Furthermore, the rotation direction of the
同様に、Y−θ軸においては、g1´(x)>g2´(x)のときは、拡散反射面12C側がカメラ13より遠くなる方向に回転している判定する。また、g1´(x)<g2´(x)のときは、拡散反射面12A側がカメラ13より遠くなる方向に回転している判定する。そして、g1´(x)=g2´(x)のときは、Y−θ軸の傾きはゼロであると判定する。
Similarly, on the Y-θ axis, when g1 ′ (x)> g2 ′ (x), it is determined that the diffuse
このようにして、姿勢移動後のXYZの3軸方向の位置値と、各軸回りの回転角度を算出することによって、ターゲット部材10の現在の姿勢、即ちXYZの3次元空間における位置値と、Z−θ軸の回転角度Zθと、X−θ軸の回転角度Xθ´と、Y−θ軸の回転角度Yθ´を特定することができる。カメラ13の位置は予め決められているので、カメラ13を用いて検出したターゲット部材10の現在の姿勢が、所望の位置及び回転角度を示す基準位置に一致するようにフィードバック制御すれば、ターゲット部材10の位置を精度良く制御できる。
In this way, by calculating the position values of the XYZ in the three-axis directions after the movement of the position and the rotation angle around each axis, the current position of the
ここで、ターゲット部材10に凸面ミラー11を用いることによる効果について、図12及び図13を参照して説明する。図12は、凸面ミラー11を用いた場合の撮像面22上での反射像q1,q3間の距離の変化を示している。一方、図13は、平板形の反射板を用いた場合の反射像q1,q3間の距離の変化を示している。図12と図13において、撮像装置13の構成と、撮像装置13に対する点光源LS1,LS3の配置は、同一になっている。
Here, the effect of using the
図12(a)に示す姿勢形地装置1では、凸面ミラー11と、撮像装置13の撮像レンズ21との間の距離(Z軸位置)がZa1のとき、撮像面22上で光軸C1を通る反射像q1,q3間の距離はW1になっている。ここで、点光源LS1,LS3と、それぞれの反射点s1,s3を結ぶ仮想線と、反射点s1,s3とレンズ21の主点nを結ぶ仮想線とがなす角度はθ1とする。
In the
これに対して、図12(b)に示すように、凸面ミラー11と、撮像装置13の撮像レンズ21との間の距離をZa2まで近づけると、凸面ミラー11上の反射点p1,p3が離れ、撮像面22上での反射像q1,q3間の距離は、W1より大きいW2となる。
On the other hand, as shown in FIG. 12B, when the distance between the
一方、図13(a)に示すように、比較例に係る姿勢検出装置101では、反射板102と、撮像装置13の撮像レンズ21との間の距離(Z軸位置)がZa1のときに、撮像面22上で光軸C1を通る反射像q1,q3間の距離がw1になるように構成されている。ここで、点光源LS1,LS3と、それぞれの反射板102上の反射点s11,s33を結ぶ仮想線と、反射点s11,s33とレンズ21の主点nを結ぶ仮想線とがなす角度はθ2とする。
On the other hand, as shown in FIG. 13A, in the
これに対して、図13(b)に示すように、反射板102と、撮像装置13の撮像レンズ21との間の距離をZa2まで近づけると、撮像面22上での反射像q1,q3間の距離は、w1より大きいw3になる。これは、反射板102上での反射点p1,p3の位置は変化しないが、反射板102から撮像面22までの距離が短くなるためである。
On the other hand, as shown in FIG. 13B, when the distance between the reflecting plate 102 and the
ここで、比較例における距離w3は、図12(b)に示す姿勢検出装置1における反射像q1,q3間の距離w2より小さい。これは、図12に示す姿勢検出装置1は、凸面ミラー11を用いているので、角度θ1がZ軸位置の変化に応じて変化し易く、その変化量が比較例における角度φのZ軸位置の変化に対する変化量より大きくなるためである。このことから、姿勢検出装置1は、Z方向に同じ距離だけ移動したときの反射点p1,p3間の距離(又は光軸C1から反射点p1,p3までの距離)の変化量を大きくすることができる。
Here, the distance w3 in the comparative example is smaller than the distance w2 between the reflected images q1 and q3 in the
さらに、このようなZ軸位置の変化に伴う反射像間の距離の変化の一例を図14に示す。
図14は、横軸がZ軸位置、即ちターゲット部材10とカメラ13の撮像レンズ21の間の距離を示す。横軸は、反射点p1と反射点p3の間の距離又は反射点p2と反射点p4の間の距離である。さらに、横軸は、撮像面22における第1の反射像q1と第1の反射像q3の間の距離又は第1の反射像q2と第1の反射像q4の間の距離も示している。
Furthermore, FIG. 14 shows an example of a change in the distance between the reflected images accompanying such a change in the Z-axis position.
In FIG. 14, the horizontal axis indicates the Z-axis position, that is, the distance between the
ラインE1は、ミラーに平面鏡を用いた比較例のターゲット部材とカメラ13の撮像レンズ21の間の距離の変化を示している。比較例のターゲット部材とカメラ13の撮像レンズ21の間の距離を変化させても、平面ミラー上の反射点s1〜s4間の距離はあまり変化しない。これは、比較例のターゲット部材のミラーが平面であるためである。これに伴って、ラインE2に示すように、撮像面22における第1の反射像q1〜q4の変化も小さくなる。
A line E <b> 1 indicates a change in the distance between the target member of the comparative example using a plane mirror as a mirror and the
これに対して、ラインE3に示すように、実施形態のターゲット部材10では凸面ミラー11を用いたので、ターゲット部材10とカメラ13の撮像レンズ21の間の距離の変化に応じて、凸面ミラー11上の反射点間の距離が大きく変化する。これに伴って、ラインE4に示すように、撮像面22における第1の反射像q1〜q4の位置の変化が比較例に比べて大きくなる。
On the other hand, as shown by line E3, since the
従って、凸面ミラー11を使用して取得した反射像q1,q3の位置変化からZ軸位置を算出することによって、比較例に比べてZ軸位置の算出精度が向上する。
Therefore, by calculating the Z-axis position from the positional changes of the reflected images q1 and q3 acquired using the
ここで、カメラ13を基準として、ターゲット部材10の位置と距離が予め分かっている場合には、点光源LS1〜LS4を最小で2個にすることができる。また、ターゲットマークが方向を規定できるような形状を有する場合、ターゲット部材10の位置と距離が予め分かっていれば、拡散反射面12A〜12Dを最小で2個にすることができる。
Here, when the position and distance of the
(実施例)
以下に、実施例として、姿勢検出装置1を多関節ロボットに搭載し、かつ点光源の変わりにリング状光源を用い、4つの拡散反射面の代わりにリング状拡散反射面を用いた部品組立装置について説明する。
図15に示すように、部品組立装置51は、部品を把持して搬送する多関節ロボット52と、多関節ロボット52のハンド53の姿勢を検出する姿勢検出装置1を有する。
多関節ロボット52は、ロボット制御部55からの信号を受けて、ハンド53の姿勢を制御するように構成されている。ハンド53の姿勢は、XYZの3軸に平行な方向及び、3軸回りに回転可能である。
(Example)
Hereinafter, as an embodiment, a component assembly apparatus in which the
As shown in FIG. 15, the
The articulated
姿勢検出装置1は、ハンド53にターゲット部材10が取り付けられており、リング状光源61を制御する照明制御部62と、カメラ13を制御するカメラ制御部63と、カメラ13で撮像した画像を処理して姿勢検出する検知処理部64と、ハンド53の姿勢と基準位置情報65との偏差を算出する偏差算出部66とを有する。偏差算出部66は、偏差の計算結果をロボット制御部55に出力するように構成されている。
The
図16及び図17に示すように、姿勢検出装置1は、ハンド53の所定位置に取り付けられたターゲット部材10を有する。ターゲット部材10の中央には、第1のターゲットマークとなる凸面ミラー11を有し、凸面ミラー11の周囲に第2のターゲットマークとなる楕円形状を有するリング状拡散反射面71(リング状拡散反射部材)が配置されている。リング状拡散反射面71は、リング状光源61の光を反射する材料、例えばガラスや石英を用いて製造されている。
As shown in FIGS. 16 and 17, the
ここで、リング状拡散反射面71の短軸aの長さは、例えば、a=(凸面ミラー11の外径)+(リング状拡散反射面71のリング幅の1/2)である。また、リング状拡散反射面71の長径bは、b≧cos(θxy)×a+(リング状拡散反射面71のリング幅の1/2)である。ここで、θxyは、姿勢検出装置1におけるX−θ軸又はY−θ軸の最大検出角度である。
Here, the length of the minor axis a of the ring-shaped diffuse
また、ターゲット部材10から離れた所定の位置には、カメラ13と、リング状光源61とが配置されている。カメラ13は、撮像レンズ21がターゲット部材10に向かうように、かつ光軸が凸面ミラー11の曲率中心mに一致するように配置されている。リング状光源61は、全周にわたって発光可能に構成されており、中心がカメラ13の光軸と一致する。
In addition, a
ここで、図18Aにカメラ13で撮像したターゲット部材10の画像の一例を示す。画像75には、直交するラインで示されるXY軸に加えて、リング状光源61の光が凸面ミラー11で反射されることによって形成された円形の反射像81と、リング状拡散反射面71における楕円形の反射像82とが含まれる。
Here, FIG. 18A shows an example of an image of the
次に、実施例における姿勢検出方法について説明する。姿勢検出の際には、最初に初期化処理を行う。さらに、移動体であるハンド53の姿勢を変化させながら、ターゲット部材10を用いて姿勢検出を行い、検出結果を利用して多関節ロボット52をフィードバック制御することによって、ハンド53を基準位置に導く。以下に、各処理の詳細について説明する。
Next, a posture detection method in the embodiment will be described. When detecting the posture, initialization processing is first performed. Furthermore, while changing the posture of the
最初に、図19のフローチャートを参照して、部品組付装置51の姿勢位置検出処理の初期化処理について説明する。
ステップS101では、ロボット制御部55が、ターゲット部材10が初期化位置に配置されるように多関節ロボット52を駆動させる。
続くステップS102では、カメラ13でターゲットマークを撮像する。具体的には、照明制御部62がリング状光源61を点灯させ、ターゲット部材10に光を照射する。さらに、カメラ制御部63の指令に基づいて、凸面ミラー11とリング状拡散反射面71のそれぞれにおけるリング状光源61の反射光をカメラ13で撮像する。カメラ13で撮像した画像は、検知処理部64に送信される。
First, the initialization process of the posture position detection process of the
In step S101, the robot control unit 55 drives the articulated
In the subsequent step S102, the target mark is imaged by the
続く、ステップS102Aでは、検知処理部64が反射像81,82の画像処理を行い、画像75に含まれる反射像81からデータ解析用の4点の第1の反射像q1〜q4を抽出する。データ解析用の第1の反射像q1〜q4は、反射像81の周方向の等間隔に配置される4つの点を抽出することが好ましい。同様に、画像75の反射像82からデータ解析用の4点の第2の反射像s1〜S4を抽出する。データ解析用の第2の反射像s1〜s4は、反射像82の周方向の等間隔に配置される4つの点を抽出することが好ましい。さらに、画像75中の反射像81の第1の中心g1(x,y)と、反射像82の第2の中心g2(x,y)と、Z−θ軸位置とをそれぞれ算出する。
Subsequently, in step S102A, the
具体的には、図18Aに示す凸面ミラー11の反射像81は、円形状になっているので、
(Xi−g1(x))2+(Yi−g1(y))2=r2 (5)
と表すことができる。この式に、反射像81を構成するn個(i=1〜n)の座標値(Xi,Yi)から、円の近似値を例えば最小二乗法を用いて算出し、第1の中心g1(x,y)と、円の半径rを求める。さらに、図18Bに示すように、反射像81を構成する画素から特徴的な4点を抽出し、データ解析用の4つの第1の反射像q1〜q4を決定する。
Specifically, since the reflected
(Xi−g1 (x)) 2 + (Yi−g1 (y)) 2 = r 2 (5)
It can be expressed as. In this equation, an approximate value of a circle is calculated from n (i = 1 to n) coordinate values (Xi, Yi) constituting the reflected
また、図18Aに示すように、リング状拡散反射面71の反射像82は、楕円形であるので、
Further, as shown in FIG. 18A, the
と表すことができる。この式に、反射像82を構成するn個(i=1〜n)の座標値(Xi,Yi)から、円の近似値を、例えば最小二乗法を用いて算出し、第2の中心g2(x,y)と、Xr,Yr,θを求める。さらに、図18Bに示すように、反射像82を構成する画素から特徴的な4点を抽出し、データ解析用の4つの第2の反射像s1〜s4を決定する。ここで、距離Rxと距離Ryは、楕円の長軸又は短軸に相当するので、XrとYrをそれぞれ2倍した値になる。
It can be expressed as. In this equation, an approximate value of a circle is calculated from n (i = 1 to n) coordinate values (Xi, Yi) constituting the reflected
また、Z―θ軸位置の算出処理では、第2の中心g2を基準にして対向して配置される2つの第2の反射像s1,s3を結ぶ仮想線J13のX軸に対する傾きであるZθ1を算出する。さらに、対向して配置される2つの第2の反射像s2,s4を結ぶ仮想線J24のY軸に対する傾きであるZθ2を算出する。そして、Zθ1とZθ2の平均を算出すると、Z−θ軸位置値が得られる。 In the calculation process of the Z-θ axis position, Zθ1 which is the inclination with respect to the X axis of the imaginary line J13 connecting the two second reflected images s1 and s3 arranged opposite to each other with respect to the second center g2. Is calculated. Further, Zθ2 that is an inclination with respect to the Y axis of the virtual line J24 connecting the two second reflection images s2 and s4 arranged to face each other is calculated. When the average of Zθ1 and Zθ2 is calculated, the Z-θ axis position value is obtained.
また、図19のステップS103で、Z軸位置検出処理を行い、Z軸上の位置を示すZ軸位置値を算出する。Z軸位置検出処理は、図5と同様である。即ち、反射像81の第1の中心g1(x,y)を算出し、第1の反射像q1〜q4のそれぞれについて角度αnを算出し、式(1)〜(4)を用いて、Z1〜Z4を算出する。さらに、Z1〜Z4の平均値を計算し、これをZ軸位置値とする。
In step S103 of FIG. 19, Z-axis position detection processing is performed to calculate a Z-axis position value indicating a position on the Z-axis. The Z-axis position detection process is the same as in FIG. That is, the first center g1 (x, y) of the reflected
続いて、ステップS104で、カメラ13の倍率βsを算出する。倍率βsは、図17に示すF/Zで算出できる。
そして、ステップS105で、リング状拡散反射面71によって形成された楕円形の反射像82の距離Rxと距離Ryをそれぞれ反射像82の長軸と短軸である。ここまでの処理で、初期状態のデータが得られた。初期状態のデータは、検知処理部の不図示のメモリに記憶される。
Subsequently, in step S104, the magnification βs of the
In step S105, the distance Rx and the distance Ry of the
次に、図8に示すフローチャートに従って、ロボットハンドの姿勢制御を開始する。即ち、ステップS301で、ロボット制御部55の指令に基づいてターゲット部材10の位置を移動させたときの姿勢位置検出処理を姿勢検出装置1で実行する。続いて、ステップS302で偏差算出部66が姿勢偏差検出処理を行い、目標となる基準位置情報65と現在の姿勢位置情報とを比較する。基準位置情報65とは、例えば、カメラ13の位置を基準として、ハンド53が所望する位置に精度良く配置された状態におけるターゲット部材10の姿勢を特定する情報である。そして、ステップS303の判定処理で、偏差算出部66が基準位置情報と現在の姿勢位置情報との偏差がゼロになったと判定すれば、姿勢制御処理を終了する。その一方で、偏差がゼロでなければ、ロボット制御部55が偏差を打ち消すような指令値を算出して多関節ロボット52を駆動させ、ステップS301に戻る。
Next, the posture control of the robot hand is started according to the flowchart shown in FIG. That is, in step S301, the
さらに、ステップS301の姿勢位置検出処理の詳細について図20のフローチャートを主に参照して説明する。
最初に、ステップS401で、ターゲットマークを撮像する。具体的には、リング状光源61を点灯させ、凸面ミラー11とリング状拡散反射面71のそれぞれにおける反射光をカメラ13で撮像する。
続くステップS401Aで、検知処理部64が反射像81,82を画像処理する。ここでの処理は、図19のステップS102Aと同様である。また、ステップS402で、検知処理部64がZ軸位置検出処理を実行する。ここでの処理は、図5と同様の処理である。
Further, details of the posture position detection processing in step S301 will be described with reference mainly to the flowchart in FIG.
First, in step S401, a target mark is imaged. Specifically, the ring-shaped
In subsequent step S401A, the
さらに、ステップS403で、XY軸位置検出処理を実行し、ターゲット部材10のX軸上の位置と、Y軸上の位置を算出する。また、各第2の反射像s1〜s4の中心(重心)の座標g2´(x,y)、及び倍率βmを算出する。この後、ステップS404において、Z−θ軸位置検出処理を実行し、Z−θ軸位置値を算出する。そして、ステップS405で、X−θ軸、Y−θ軸の位置検出処理を実行し、X−θ軸位置値と、Y−θ軸位置値とを算出する。
In step S403, an XY axis position detection process is executed to calculate the position of the
ここで、移動後に図21に示すような画像85が得られた場合、リング状光源61の凸面ミラー11における反射像81からデータ解析用の第1の反射像q1´〜q4´が抽出される。同様に、リング状光源61のリング状拡散反射面71における反射像82からデータ解析用の第2の反射像s1´〜s4´が抽出される。そして、移動後の中心g1´、g2´は、式(5)、(6)から算出される。ターゲット部材10のXY座標は、(g2´(x)/βm,g2´(y)/βm)になる。
Here, when an
Z軸位置は、Z=(Z1+Z2+Z3+Z4)/4で算出される。Z1〜Z4は、式(1)〜式(4)で算出できる。Z−θ軸の回転角度は、(Zθ1+Zθ2)/2であり、Zθ1.Zθ2は、
Zθ1=tan−1((s3´(y)−s1´(y))/(s3´(x)−s1´(x)))
Zθ2=tan−1((s4´(y)−s2´(y))/(s4´(x)−s2´(x)))
から算出される。
The Z-axis position is calculated by Z = (Z1 + Z2 + Z3 + Z4) / 4. Z1 to Z4 can be calculated by equations (1) to (4). The rotation angle of the Z-θ axis is (Zθ1 + Zθ2) / 2, and Zθ1. Zθ2 is
Zθ1 = tan −1 ((s3 ′ (y) −s1 ′ (y)) / (s3 ′ (x) −s1 ′ (x)))
Zθ2 = tan −1 ((s4 ′ (y) −s2 ′ (y)) / (s4 ′ (x) −s2 ′ (x)))
Is calculated from
図11Aに示すように、Xθは、Yθ=cos−1(Rx´/βm)/(Rx/βs))になる。図11Bに示すように、Yθは、Yθ=cos−1(Ry´/βm)/(Ry/βs))になる。X−θ軸の回転方向と、Y−θ軸の回転方向は、前記と同様に、g1´とg2´の位置関係から決定する。 As shown in FIG. 11A, Xθ is Yθ = cos −1 (Rx ′ / βm) / (Rx / βs)). As shown in FIG. 11B, Yθ becomes Yθ = cos −1 (Ry ′ / βm) / (Ry / βs)). The rotation direction of the X-θ axis and the rotation direction of the Y-θ axis are determined from the positional relationship between g1 ′ and g2 ′ as described above.
この実施例では、リング状光源61からの光を凸面ミラー11とリンク状拡散反射面71で反射するようにしたので、カメラ13で2つのリング状の反射像81,82が得られる。例えば、リング状光源61やターゲット部材10に汚れがあったり、姿勢検出装置1の光路上に障害物があったりした場合で、反射像81,82の1/3程度が欠落した場合であっても、近似計算によって円又は楕円を補完することができるので、ターゲット部材10と、ターゲット部材10が取り付けられたハンド53の姿勢を、精度を低下させることなく検出できる。従って、図15に示す部品組付装置51において、ハンド53で把持した部品91を高い位置精度で他の部品92に組み付けることができる。また、ハンド53に工具を取り付けて他の部品92を加工する場合には、高い精度の加工が可能になる。姿勢検出装置1の用途は、部品組付装置51に限定されず、部品の搬送装置などに用いても良い。
In this embodiment, since the light from the ring-shaped
ここで、リング状光源61を円形にし、リング状拡散反射面71を楕円形状にしたので、図18Aや図20に示す反射像81と反射像82の区別が容易に付けられる。しかしながら、反射像81より反射像82が必ず大きくなるように配置される場合には、リング状光源61とリング状拡散反射面71とを同じ形状にしても良い。また、リング状光源61、リング状拡散反射面71の少なくとも一方は、一部が切り欠かれたC形状にすることができる。さらに、リング状光源61、リング状拡散反射面71の少なくとも一方は、2つ以上に分割された円弧又は楕円弧でも良い。
Here, since the ring-shaped
ここで挙げた全ての例及び条件的表現は、発明者が技術促進に貢献した発明及び概念を読者が理解するのを助けるためのものであり、ここで具体的に挙げたそのような例及び条件に限定することなく解釈するものであり、また、明細書におけるそのような例の編成は本発明の優劣を示すこととは関係ない。本発明の実施形態を詳細に説明したが、本発明の精神及び範囲から逸脱することなく、それに対して種々の変更、置換及び変形を施すことができる。 All examples and conditional expressions given here are intended to help the reader understand the inventions and concepts that have contributed to the promotion of technology, and such examples and It is to be construed without being limited to the conditions, and the organization of such examples in the specification is not related to showing the superiority or inferiority of the invention. While embodiments of the present invention have been described in detail, various changes, substitutions and variations can be made thereto without departing from the spirit and scope of the present invention.
以下に、前記の実施の形態の特徴を付記する。
(付記1) 光源からの光を移動体に照射し、前記移動体に取り付けられた凸面ミラーにおける反射光を撮像装置で撮像し、前記光源の光の第1の反射像を含む画像を作成する工程と、前記凸面ミラー上の反射点と前記第1の反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第1の角度を算出する工程と、前記第1の反射点と凸面ミラーの中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第2の角度と、前記光源と前記撮像装置の光軸との間の距離と、前記第1の角度とを用い、前記光源から光の前記第1の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する工程と、を含むことを特徴とする移動体の姿勢検出方法。
(付記2) 複数の前記第1の反射像のそれぞれを用いて算出した、前記撮像装置から前記凸面ミラーの前記反射点までの距離の平均値を算出する工程を含むことを特徴とする付記1に記載の移動体の姿勢検出方法。
(付記3) 前記光源からの光を移動体の前記凸面ミラーの周囲に配置された複数の拡散反射部材に照射し、それぞれの反射光を前記撮像装置で撮像し、複数の第2の反射像を取得し、前記複数の第2の反射像の中心から、前記複数の前記第2の反射像の中心である第2の中心を算出する工程と、前記第2の中心を挟んで配置される2つの前記第2の反射像を結ぶ仮想線と、前記撮像装置の光軸に垂直な軸とがなす角度から前記撮像装置の光軸に平行な軸回りの前記移動体の回転角度を算出する工程と、を含むことを特徴とする付記1又は付記2に記載の移動体の姿勢検出方法。
(付記4) 前記移動体を移動させた前後のそれぞれのタイミングで、前記第2の中心を通り、所定の2つの前記第2の反射像を結ぶ仮想線の長さを算出し、前記移動体の移動前後での前記仮想線の長さ変化から、前記撮像装置の光軸に直交する軸線回りの回転角度を算出する工程をさらに有する付記3に記載の移動体の姿勢検出装置。
(付記5) 前記移動体は、多関節ロボットの先端に取り付けられたハンドであり、前記ハンドの姿勢の現在値と、予め定められた前記ハンドの基準位置との偏差を算出し、偏差がゼロになるように前記多関節ロボットを駆動させる工程を有することを特徴とする付記1乃至付記4のいずれか一項に記載の部品組立方法。
(付記6) 移動体に取り付けられた凸面ミラーと、前記凸面ミラーで反射した光を受光する撮像装置と、前記撮像装置の光軸を囲むように配置され、前記凸面ミラー及び前記拡散反射部材を照明する光源と、前記凸面ミラー上の反射点と前記第1の反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第1の角度を算出し、前記第1の反射点と凸面ミラーの中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第2の角度と、前記光源と前記撮像装置の光軸との間の距離とを用い、前記光源から光の前記第1の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する処理部と、を含むことを特徴とする移動体の姿勢検出装置。
(付記7) 前記光源は、円環形状を有する付記6に記載の移動体の姿勢検出装置。
(付記8) 前記光源は、4つ以上の点光源である付記1に記載の移動体の姿勢検出装置。
(付記9) 付記6乃至付記8のいずれか一項に記載の姿勢検出装置と、先端にハンドが設けられた多関節ロボットと、を含み、前記移動体は、多関節ロボットの先端に取り付けられた部品組立用の前記ハンドであることを特徴とする部品組立装置。
(付記10) 前記処理部は、前記姿勢検出装置で検出した前記ハンドの姿勢と、予め定められた前記ハンドの基準位置との偏差を算出する偏差算出部を有し、前記偏差がゼロになるように前記ロボットを駆動させるように構成したことを特徴とする付記9に記載の部品組立装置。
The features of the above embodiment will be added below.
(Additional remark 1) The light from a light source is irradiated to a moving body, the reflected light in the convex mirror attached to the said moving body is imaged with an imaging device, and the image containing the 1st reflected image of the light of the said light source is produced. A step of calculating a first angle formed by an imaginary line passing through a reflection point on the convex mirror and the first reflection image and passing through a principal point of the lens of the imaging device and an optical axis of the imaging device; A second angle formed by an imaginary line connecting the first reflection point and the center of the convex mirror and the optical axis of the imaging device, and a distance between the light source and the optical axis of the imaging device, Calculating a distance from the imaging device to the reflection point of the convex mirror when an incident angle and a reflection angle of light from the light source at the first reflection point coincide with each other using the first angle. And a posture detection method for a moving body.
(Additional remark 2) The process of calculating the average value of the distance from the said imaging device to the said reflective point of the said convex mirror calculated using each of several said 1st reflective images is included, The
(Additional remark 3) The light from the said light source is irradiated to the some diffuse reflection member arrange | positioned around the said convex mirror of a moving body, each reflected light is imaged with the said imaging device, and several 2nd reflected images And calculating a second center, which is the center of the plurality of second reflected images, from the centers of the plurality of second reflected images, and arranged with the second center interposed therebetween. A rotation angle of the movable body about an axis parallel to the optical axis of the imaging device is calculated from an angle formed by an imaginary line connecting the two second reflected images and an axis perpendicular to the optical axis of the imaging device. A method for detecting a posture of a moving object according to
(Supplementary Note 4) At each timing before and after the moving body is moved, the length of an imaginary line passing through the second center and connecting two predetermined second reflected images is calculated, and the moving body The posture detection device for a moving body according to
(Additional remark 5) The said mobile body is the hand attached to the front-end | tip of an articulated robot, The deviation of the present value of the said attitude | position of the hand and the reference position of the said hand determined in advance is calculated, and a deviation is zero. The component assembling method according to any one of
(Additional remark 6) It arrange | positions so that the convex mirror attached to the mobile body, the imaging device which receives the light reflected by the said convex mirror, and the optical axis of the said imaging device may be enclosed, and the said convex mirror and the said diffused reflection member are A first angle formed by an illuminating light source, a reflection point on the convex mirror and the first reflection image, and an imaginary line passing through the principal point of the lens of the imaging device and the optical axis of the imaging device is calculated. A second angle formed by an imaginary line connecting the first reflection point and the center of the convex mirror and the optical axis of the imaging device, and a distance between the light source and the optical axis of the imaging device. And a processing unit that calculates a distance from the imaging device to the reflection point of the convex mirror when an incident angle and a reflection angle of light from the light source at the first reflection point coincide with each other. A moving body posture detection device.
(Additional remark 7) The said light source is an attitude | position detection apparatus of the moving body of Additional remark 6 which has a ring shape.
(Additional remark 8) The said light source is an attitude | position detection apparatus of the moving body of
(Additional remark 9) The attitude | position detection apparatus as described in any one of additional remark 6 thru | or appendix 8, and the articulated robot by which the hand was provided in the front-end | tip, The said mobile body is attached to the front-end | tip of an articulated robot. A component assembling apparatus, wherein the component assembling device is the hand for assembling components.
(Additional remark 10) The said process part has a deviation calculation part which calculates the deviation of the attitude | position of the said hand detected with the said attitude | position detection apparatus, and the reference position of the said hand beforehand, and the said deviation becomes zero The component assembly apparatus according to
1 姿勢検出装置
11 凸面ミラー
12A〜12D 拡散反射面(拡散反射部材)
13 撮像装置
51 部品組付装置
52 多関節ロボット
53 ハンド(移動体)
61 リング状光源
64 検知処理部(処理部)
71 リング状拡散反射面(拡散反射部材)
C1 光軸
LS1〜LS4 光源
q1〜q4 第1の反射像
s1〜s4 第2の反射像
g1 第1の中心
g2 第2の中心
Xθ,Yθ,Zθ 回転角度
α 第1の角度
γ 第2の角度
DESCRIPTION OF
13
61 Ring-shaped
71 Ring-shaped diffuse reflective surface (diffuse reflective member)
C1 Optical axis LS1 to LS4 Light source q1 to q4 First reflected image s1 to s4 Second reflected image g1 First center g2 Second center Xθ, Yθ, Zθ Rotation angle α First angle γ Second angle
Claims (5)
前記凸面ミラー上の反射点と前記反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第1の角度を算出する工程と、
前記反射点と凸面ミラーの曲率中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第2の角度と、前記光源と前記撮像装置の光軸との間の距離と、前記第1の角度とを用い、前記光源から光の前記反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する工程と、
前記撮像装置の光軸を中心とする仮想円上に配置された前記光源からの光が前記凸面ミラーで反射することで撮像された複数の前記反射像を結ぶ仮想線の交点から複数の前記反射像の中心を算出する工程と、
複数の前記反射部材における反射光を前記撮像装置で撮影して得られた複数の反射像のうち、複数の前記反射像の中心を基準にして対向して配置される2つの前記反射像を結ぶ仮想直線の傾きを算出し、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度を算出する工程と、
前記移動体の姿勢を変化させながら、前記凸面ミラーの前記反射点までの距離と複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度とを算出し、前記凸面ミラーの前記反射点までの距離と、複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度と、目標とする距離と位置と角度を有する基準位置との偏差がゼロになる姿勢に前記移動体を移動させる指令値を算出する工程と、
を含むことを特徴とする移動体の姿勢検出方法。 Imaging and convex mirror, said shines irradiation light from the light source to the moving body having a plurality of reflecting members disposed around the convex mirror, said convex mirror, the reflected light definitive to each of the plurality of the reflecting member Imaging with an apparatus and creating an image including a reflected image of the light from the light source;
Calculating a first angle formed by an imaginary line passing through the reflection point on the convex mirror and the reflection image and passing through the principal point of the lens of the imaging device and the optical axis of the imaging device;
A second angle formed by an imaginary line connecting the reflection point and the center of curvature of the convex mirror and the optical axis of the imaging device, a distance between the light source and the optical axis of the imaging device, and the first Calculating a distance from the imaging device to the reflection point of the convex mirror when an incident angle and a reflection angle at the reflection point of light from the light source coincide with each other;
A plurality of reflections from intersections of virtual lines connecting a plurality of reflection images picked up by reflection of light from the light source arranged on a virtual circle centered on an optical axis of the imaging device by the convex mirror Calculating the center of the image;
Among the plurality of reflection images obtained by photographing the reflected light from the plurality of reflection members with the imaging device, the two reflection images arranged opposite to each other with the centers of the plurality of reflection images as a reference are connected. a step of calculating an angle formed by calculating the slope of a virtual straight line, the axis perpendicular to the optical axis of the imaging device and the virtual straight line,
While changing the posture of the moving body, the angle between the distance to the reflection point of the convex mirror, the positions of the centers of the plurality of reflected images, the axis perpendicular to the optical axis of the imaging device, and the virtual line And the distance from the convex mirror to the reflection point, the position of the center of the plurality of reflected images, the angle formed by the axis perpendicular to the optical axis of the imaging device and the virtual line, and the target Calculating a command value for moving the moving body to a posture in which a deviation between a reference position having a distance, a position, and an angle is zero;
A method of detecting a posture of a moving object, comprising:
前記移動体に取り付けられ、前記凸面ミラーの周囲に配置された複数の反射部材と、
前記凸面ミラー及び複数の前記反射部材で反射した光を受光する撮像装置と、
前記撮像装置の光軸を囲むように配置され、前記凸面ミラー及び拡散反射部材を照明する光源と、
前記凸面ミラー上の反射点と前記反射点で反射した光を前記撮像装置で撮像した反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第1の角度を算出し、前記反射点と凸面ミラーの曲率中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第2の角度と、前記光源と前記撮像装置の光軸との間の距離とを用い、前記光源から光の前記第1の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出し、前記撮像装置の光軸を中心とする仮想円上に配置された前記光源からの光が前記凸面ミラーで反射することで撮像された複数の前記反射像を結ぶ仮想直線の交点から複数の前記反射像の中心を算出し、複数の前記反射部材における反射光を前記撮像装置で撮影して得られた複数の反射像のうち、複数の前記反射像の中心を基準にして対向して配置される2つの前記反射像を結ぶ仮想直線の傾きを算出し、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度を算出し、前記移動体の姿勢を変化させながら、前記凸面ミラーの前記反射点までの距離と複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度とを算出し、前記凸面ミラーの前記反射点までの距離と、複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度と、目標とする距離と位置と角度を有する基準位置との偏差がゼロになる姿勢に前記移動体を移動させる指令値を算出する処理部と、
を含むことを特徴とする移動体の姿勢検出装置。 A convex mirror attached to the moving body;
A plurality of reflecting members attached to the movable body and disposed around the convex mirror;
An imaging device that receives light reflected by the convex mirror and the plurality of reflecting members ;
A light source disposed so as to surround the optical axis of the imaging device, and illuminating the convex mirror and the diffuse reflection member;
A reflection point on the convex mirror and a reflected image obtained by imaging the light reflected by the reflection point with the imaging device, a virtual line passing through a principal point of the lens of the imaging device, and an optical axis of the imaging device are formed. 1 between the second angle formed by the imaginary line connecting the reflection point and the center of curvature of the convex mirror and the optical axis of the imaging device, and between the light source and the optical axis of the imaging device. And calculating a distance from the imaging device to the reflection point of the convex mirror when an incident angle and a reflection angle of light from the light source at the first reflection point coincide with each other. Centers of the plurality of reflected images are obtained from intersections of virtual straight lines connecting the plurality of reflected images picked up by reflection of light from the light source arranged on a virtual circle centered on the optical axis by the convex mirror. the calculated, the image pickup light reflected at a plurality of said reflective member Calculating an inclination of an imaginary straight line connecting two reflection images arranged opposite to each other with reference to the center of the plurality of reflection images among a plurality of reflection images obtained by photographing at a position; Calculating the angle formed by the axis perpendicular to the optical axis and the virtual straight line, and changing the posture of the moving body, the distance to the reflection point of the convex mirror and the positions of the centers of the plurality of reflection images, Calculating an angle formed by an axis perpendicular to the optical axis of the imaging device and the virtual straight line, a distance to the reflection point of the convex mirror, a center position of the plurality of reflected images, and the imaging device Processing for calculating a command value for moving the moving body to a posture in which a deviation between an angle formed by an axis perpendicular to the optical axis and the virtual straight line and a target distance, position, and reference position having the angle is zero And
An apparatus for detecting a posture of a moving body, comprising:
先端にハンドが設けられた多関節ロボットと、
を含み、
前記移動体は、多関節ロボットの先端に取り付けられた部品組立用の前記ハンドであることを特徴とする部品組立装置。
The posture detection device according to claim 3 or 4,
An articulated robot with a hand at the tip;
Including
The component assembling apparatus, wherein the moving body is the hand for assembling components attached to a tip of an articulated robot.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2012238579A JP6160062B2 (en) | 2012-10-30 | 2012-10-30 | POSITION DETECTING METHOD OF MOBILE BODY, MOBILE BODY POSITION DETECTING DEVICE, AND PART ASSEMBLY |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2012238579A JP6160062B2 (en) | 2012-10-30 | 2012-10-30 | POSITION DETECTING METHOD OF MOBILE BODY, MOBILE BODY POSITION DETECTING DEVICE, AND PART ASSEMBLY |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2014089095A JP2014089095A (en) | 2014-05-15 |
JP6160062B2 true JP6160062B2 (en) | 2017-07-12 |
Family
ID=50791111
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2012238579A Active JP6160062B2 (en) | 2012-10-30 | 2012-10-30 | POSITION DETECTING METHOD OF MOBILE BODY, MOBILE BODY POSITION DETECTING DEVICE, AND PART ASSEMBLY |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP6160062B2 (en) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6446009B2 (en) * | 2016-09-28 | 2018-12-26 | 本田技研工業株式会社 | Position and orientation estimation method |
CN107270811A (en) * | 2017-06-22 | 2017-10-20 | 深圳市恒科通机器人有限公司 | Positioning test method and locating test device |
CN107297741B (en) * | 2017-08-09 | 2018-05-15 | 广州乾鼎电力科技发展有限公司 | Motion trail planning method and device based on machine vision |
US10776950B2 (en) | 2018-04-02 | 2020-09-15 | Quality Vision International Inc. | Alignment system for imaging sensors in multiple orientations |
JP7055883B2 (en) * | 2018-08-30 | 2022-04-18 | TechShare株式会社 | robot |
CN113188455B (en) * | 2020-12-19 | 2022-08-02 | 北京工业大学 | Object normal height calculation method based on robot terminal monocular vision |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH061173B2 (en) * | 1988-05-09 | 1994-01-05 | オムロン株式会社 | Curved property inspection device |
JP2564963B2 (en) * | 1990-03-29 | 1996-12-18 | 三菱電機株式会社 | Target and three-dimensional position and orientation measurement system using the target |
JPH05196415A (en) * | 1992-01-21 | 1993-08-06 | Sodick Co Ltd | Method and device for measuring displacement used for translation device |
JP2003117861A (en) * | 2001-10-15 | 2003-04-23 | Denso Corp | Position correcting system of robot |
-
2012
- 2012-10-30 JP JP2012238579A patent/JP6160062B2/en active Active
Also Published As
Publication number | Publication date |
---|---|
JP2014089095A (en) | 2014-05-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6160062B2 (en) | POSITION DETECTING METHOD OF MOBILE BODY, MOBILE BODY POSITION DETECTING DEVICE, AND PART ASSEMBLY | |
JP5290324B2 (en) | Method and system for accurately positioning at least one object in a final pose in space | |
US6549825B2 (en) | Alignment apparatus | |
JP5615416B2 (en) | Automatic measurement of dimensional data by laser tracker | |
CN108965690B (en) | Image processing system, image processing apparatus, and computer-readable storage medium | |
US11454498B2 (en) | Coordinate measuring system | |
CN104819707A (en) | Polyhedral active cursor target | |
CN105783880B (en) | A kind of monocular laser assisted bay section docking calculation | |
JP2004050356A (en) | Position and attitude sensor of movable structure | |
EP1985968B1 (en) | Noncontact measuring apparatus for interior surfaces of cylindrical objects based on using the autofocus function that comprises means for directing the probing light beam towards the inspected surface | |
CN112492192A (en) | Camera focus for ADAS | |
JP2014081279A (en) | Three-dimensional shape measuring apparatus and three-dimensional shape measuring method | |
JP2014069251A (en) | Working part control device, working robot, working part control method, and working part control program | |
JP5775533B2 (en) | System and method for picking up and placing a chip die | |
CN107727084B (en) | Method for automatically searching normal direction of cube mirror on satellite by robot high-precision measuring instrument | |
KR102457418B1 (en) | positioning device | |
CN112577423A (en) | Method for machine vision position location including in motion and application thereof | |
JP4520276B2 (en) | Measuring jig | |
JP6101037B2 (en) | Tracking laser device and measuring device | |
JP6129545B2 (en) | Spatial coordinate measuring apparatus and spatial coordinate measuring method | |
US10885368B2 (en) | Six-dimensional smart target | |
CN110969652B (en) | Shooting method and system based on mechanical arm monocular camera serving as binocular stereoscopic vision | |
JP2016177255A (en) | Multi-degree of freedom support mechanism, lens barrel, and optical apparatus | |
TWI776694B (en) | Automatic robot arm system and method of coordinating robot arm and computer vision thereof | |
CN110785624A (en) | Alignment system for multiple orientation imaging sensors |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20150706 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20160425 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20160510 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20160706 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20160726 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20160926 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20170228 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20170425 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20170516 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20170529 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 6160062 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |