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.
部品の組立工程や加工工程では、多関節ロボットを用いて部品を他の部品に組み付けたり、加工したりする。部品のサイズが極めて小さい場合や、部品に精度の高い加工をする場合には、部品や工具を把持するハンドの位置を高精度、例えば数十μｍ以下の精度で制御しなければならないことがある。このために、多関節ロボットに姿勢検出装置を設け、ハンドの姿勢を検出しながらフィードバック制御してハンドを所望の位置に移動させている。 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.
ここで、ハンドに取り付けたターゲットマークを利用してハンドの位置を認識する場合、ターゲットには、凹面鏡と、凹面鏡の周囲に配置された４つのマークとが使用される。４つのマークは、カメラの光軸に垂直で、かつ凹面鏡の曲率中心から光軸方向にオフセットさせた仮想平面上に配置されている。このタイプの姿勢検出装置では、ターゲットマークを照明する光源が１つ設けられている。 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.
１つの光源から出力された光は、凹面鏡で反射される。光源からの光のうち、凹面鏡の曲率中心を通って凹面鏡に入射する光は、凹面鏡に反射され、再び曲率中心を通ってカメラに入射する。凹面鏡における反射光は、曲率中心を通過した光のみがカメラで撮像されるので、カメラの画像には凹面鏡に起因して撮像された輝点が含まれる。このために、輝点の位置から凹面鏡の曲率中心の位置を特定できる。このとき、凹面鏡の曲率中心の位置を基準座標とする。さらに、凹面鏡の曲率中心の位置と、マークの配置は既知であるため、画像中でマークに起因する輝点の配置から、基準座標に対するそれぞれの輝点の相対的な位置を特定できる。これによって、カメラの光軸をＺ軸と、Ｚ軸に直交するＸ軸及びＹ軸で表される３次元空間内での位置を検出できる。 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.
ここで、Ｚ軸方向の位置の変化を検出するためには、撮像画像の輝点の位置の変化量を算出する必要があるが、凹面鏡で反射させた光を撮像して得られる輝点の位置の変化は、実際の距離の変化に比べて縮小される。従って、Ｚ軸方向の位置を精度良く検出することが困難であった。 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.
また、凹面鏡を用いたロボットハンドの姿勢検出処理では、３次元空間における位置と、Ｚ軸回りの回転角度であるθｚと、Ｘ軸回りの回転角度であるθｘと、Ｙ軸回りの回転角度であるθｙの６種類のデータを精度良く取得することは困難であった。例えば、６種類のデータを取得するためには、角度が異なる位置にカメラを２つ配置し、２つのカメラで取得した２つの画像を用いて画像処理を行う必要がある。このために、装置構成が複雑化、及び大型化すると共に、コストアップの原因となっていた。
この発明は、このような事情に鑑みてなされたものであり、移動体の位置を簡単な構成で精度良く検出できるようにすることを目的とする。
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.
実施形態の一観点によれば、凸面ミラーと、前記凸面ミラーの周囲に配置された複数の反射部材とを有する移動体に光源からの光を照射し、前記凸面ミラーと、複数の前記反射部材のそれぞれにおける反射光を撮像装置で撮像し、前記光源の光の反射像を含む画像を作成する工程と、前記凸面ミラー上の反射点と前記反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第１の角度を算出する工程と、前記反射点と凸面ミラーの曲率中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第２の角度と、前記光源と前記撮像装置の光軸との間の距離と、前記第１の角度とを用い、前記光源から光の前記反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する工程と、前記撮像装置の光軸を中心とする仮想円上に配置された前記光源からの光が前記凸面ミラーで反射することで撮像された複数の前記反射像を結ぶ仮想直線の交点から複数の前記反射像の中心を算出する工程と、複数の前記反射部材における反射光を前記撮像装置で撮影して得られた複数の反射像のうち、複数の前記反射像の中心を基準にして対向して配置される２つの前記反射像を結ぶ仮想直線の傾きを算出し、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度を算出する工程と、前記移動体の姿勢を変化させながら、前記凸面ミラーの前記反射点までの距離と複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度とを算出し、前記凸面ミラーの前記反射点までの距離と、複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度と、目標とする距離と位置と角度を有する基準位置との偏差がゼロになる姿勢に前記移動体を移動させる指令値を算出する工程と、を含むことを特徴とする移動体の姿勢検出方法が提供される。
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.
また、実施形態の別の観点によれば、移動体に取り付けられた凸面ミラーと、前記移動体に取り付けられ、前記凸面ミラーの周囲に配置された複数の反射部材と、前記凸面ミラー及び複数の前記反射部材で反射した光を受光する撮像装置と、前記撮像装置の光軸を囲むように配置され、前記凸面ミラー及び拡散反射部材を照明する光源と、前記凸面ミラー上の反射点と前記反射点で反射した光を前記撮像装置で撮像した反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第１の角度を算出し、前記反射点と凸面ミラーの曲率中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第２の角度と、前記光源と前記撮像装置の光軸との間の距離とを用い、前記光源から光の前記第１の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出し、前記撮像装置の光軸を中心とする仮想円上に配置された前記光源からの光が前記凸面ミラーで反射することで撮像された複数の前記反射像を結ぶ仮想直線の交点から複数の前記反射像の中心を算出し、複数の前記反射部材における反射光を前記撮像装置で撮影して得られた複数の反射像のうち、複数の前記反射像の中心を基準にして対向して配置される２つの前記反射像を結ぶ仮想直線の傾きを算出し、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度を算出し、前記移動体の姿勢を変化させながら、前記凸面ミラーの前記反射点までの距離と複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度とを算出し、前記凸面ミラーの前記反射点までの距離と、複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度と、目標とする距離と位置と角度を有する基準位置との偏差がゼロになる姿勢に前記移動体を移動させる指令値を算出する処理部と、を含むことを特徴とする移動体の姿勢検出装置が提供される。
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.
最初に、実施形態の原理について説明する。
図１及び図２に姿勢検出系の概略構成を示すように、姿勢検出装置１は、姿勢検出の目的対象となる移動体に取り付けられるターゲット部材１０を有する。移動体は、例えば、多関節ロボットのハンドなどの移動体の先端部分である。ターゲット部材１０自体が移動体であっても良い。また、姿勢とは、ターゲット部材１０の３次元空間における位置と、直交する３軸のそれぞれの軸回り回転角度を含むものとする。
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
ターゲット部材１０は、中央に第１のターゲットマークとなる凸面ミラー１１を有する。凸面ミラー１１は、曲率中心がｍ、曲率半径がＲである。さらに、ターゲット部材１０は、凸面ミラー１１の周囲に第２のターゲットマークとなる４つの拡散反射面１２Ａ，１２Ｂ，１２Ｃ，１２Ｄ（拡散反射部材）が凸面ミラー１１の外周に沿って配置されている。各拡散反射面１２Ａ〜１２Ｄは、四角形で、凸面ミラー１１の外周と同心の仮想円の円周方向に等間隔に配置されている。
The
また、姿勢検出装置１は、ターゲット部材１０から所定距離だけ上方に離れた位置に、カメラ１３（撮像装置）と、４つの点光源ＬＳ１，ＬＳ２，ＬＳ３，ＬＳ４を有する。カメラ１３は、光軸Ｃ１が凸面ミラー１１の曲率中心ｍを通り、かつ凸面ミラー１１及び４つの拡散反射面１２Ａ〜１２Ｄを撮像可能な向きに配置されている。
In addition, the
４つの点光源ＬＳ１〜ＬＳ４は、カメラ１３の光軸Ｃ１を中心とする仮想円の円周上に等間隔で配置されている。カメラ１３の光軸Ｃ１を挟んで配置される２つの点光源ＬＳ１，ＬＳ３を結ぶ仮想線の中心は、カメラ１３の光軸Ｃ１に一致するように配置されている。同様に、カメラ１３の光軸Ｃ１を挟んで配置される２つの点光源ＬＳ２，ＬＳ４を結ぶ仮想線の中心は、カメラ１３の光軸Ｃ１に一致するように配置されている。このような点光源ＬＳ１〜ＬＳ４の配置の一例として、各点光源ＬＳ１〜ＬＳ４は、ターゲット部材１０の各拡散反射面１２Ａ〜１２Ｄの上方に１つずつ配置される。点光源ＬＳ１〜ＬＳ４と、拡散反射面１２Ａ〜１２Ｄの数と配置は、同じ数及び光軸Ｃ１回りに同じ配置であることが好ましいが、両者の数又は配置を異ならせることもできる。
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
ここで、ターゲット部材１０は、例えば、点光源ＬＳ１〜ＬＳ４の光を反射しない材料から製造され、凸面ミラー１１及び４つの拡散反射面１２Ａ〜１２Ｄは、点光源ＬＳ１〜ＬＳ４の光を反射する材料、例えばガラスや石英を用いて製造されている。
Here, the
図２に示すように、カメラ１３は、撮像レンズ２１と、撮像面２２を有する。撮像面２２は、例えば、ＣＣＤ（Charge Coupled Device）が用いられる。カメラ１３で撮像したターゲット部材１０の画像の一例を図３に示す。画像２５には、画像の中心で直交する２つの直線を有し、これら直線がＸ軸と、Ｙ軸になっている。Ｘ軸及びＹ軸に直交するＺ軸であり、図２においてＺ軸はカメラ１３の光軸と一致している。
As shown in FIG. 2, the
画像２５には、円形の４つの第１の反射像ｑ１，ｑ２，ｑ３，ｑ４と、四角形の第２の反射像ｓ１，ｓ２，ｓ３，ｓ４が含まれる。４つの第１の反射像ｑ１〜ｑ４は、４つの点光源ＬＳ１〜ＬＳ４が凸面ミラー１１で反射されることによって形成された像であり、互いの距離が狭い。４つの第２の反射像ｓ１〜ｓ４、点光源ＬＳ１〜ＬＳ４の光が反射することによって形成されることから、典型的には円形になる。
The
また、４つの第２の反射像ｓ１〜ｓ４は、４つの拡散反射面１２Ａ〜１２Ｄで反射されることによって形成された像であり、互いの距離が広い。４つの第２の反射像ｓ１〜ｓ４は、拡散反射面１２Ａ〜１２Ｄの形状に従って、典型的に四角形状になる。 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.
次に、姿勢検出方法について説明する。姿勢検出の際には、最初に初期化処理を行う。さらに、移動体の姿勢を変化させながら、ターゲット部材１０を用いて姿勢検出を行い、検出結果を利用してフィードバック制御することによって、ターゲット部材１０が取り付けられた移動体を基準位置に導く。以下に、各処理の詳細について説明する。
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
最初に、図４のフローチャートを参照して、姿勢位置検出処理の初期化処理について説明する。
ステップＳ１０１で、ターゲット部材１０を初期化位置に移動させる。ターゲット部材１０が多関節ロボットのロボットハンドに取り付けられている場合には、ロボットハンドを初期位置に移動させる。
次に、ステップＳ１０２では、カメラ１３でターゲットマークを撮像する。具体的には、４つの点光源ＬＳ１〜ＬＳ４を点灯させてターゲット部材１０に光を照射し、凸面ミラー１１と４つの拡散反射面１２Ａ〜１２Ｄのそれぞれにおける各点光源ＬＳ１〜ＬＳ４の反射光をカメラ１３で撮像する。
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
この後、ステップＳ１０３では、Ｚ軸位置検出処理を行い、Ｚ軸上の位置を示すＺ軸位置値を算出する。Ｚ軸位置検出処理の詳細は、後に説明する。続いて、ステップＳ１０４で、カメラ１３の倍率βｓを算出する。図２に示す例において倍率βｓは、Ｆ／Ｚで算出できる。ここで、Ｆは、カメラ１３の撮像レンズ２１の主点ｎから撮像面２２までの距離であり、既知の値である。Ｚは、ステップＳ１０３で算出される値が用いられる。そして、ステップＳ１０５で、拡散反射面１２Ｂと拡散反射面１２Ｄの間の距離である拡散反射面間距離Ｒｘを２つの第２の反射像ｓ２，ｓ４の間の距離として算出する。また、拡散反射面１２Ａと拡散反射面１２Ｃの間の距離である拡散反射面間距離Ｒｙを２つの第２の反射像ｓ１，ｓ３の間の距離として算出する。図３の画像２５に示すように、対向する２点間の距離Ｒｘ、Ｒｙは、
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.
次に、図４のステップＳ１０３のＺ軸位置検出処理の詳細について、図５のフローチャートを参照して説明する。Ｚ軸位置検出処理は、カメラ１３が凸面ミラー１１における反射光を撮像したときに得られる４つの第１の反射像ｑ１〜ｑ４の１つ１つに着目して実施される。
最初のステップＳ２０１では、図３の画像２５内における４つの第１の反射像ｑ１〜ｑ４の第１の中心ｇ１（ｘ，ｙ）を算出する。第１の中心ｇ１（ｘ，ｙ）は、４つの第１の反射像ｑ１〜ｑ４の重心でもある。
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
具体的には、図６Ａ示す第１の反射像ｑ１と第１の反射像ｑ３を結ぶ仮想線Ｌ１３の式を求める。仮想線Ｌ１３は、
Ｙ１＝（ｑ１（ｙ）−ｑ３（ｙ））／（ｑ１（ｘ）−ｑ３（ｘ））×Ｘ１＋ｂ１
になる。ここで、Ｘ１とＹ１は、それぞれＸ軸上の座標と、Ｙ軸上の座標を示す。ｂ１は、仮想線Ｌ１３がＹ軸と交差する点のＹ座標に相当する。
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.
同様に、第１の反射像ｑ２と第１の反射像ｑ４を結ぶ仮想線Ｌ２４の式を求める。仮想線Ｌ２４は、
Ｙ２＝（ｑ２（ｙ）−ｑ４（ｙ））／（ｑ２（ｘ）−ｑ４（ｘ））×Ｘ２＋ｂ２
になる。ここで、Ｘ２とＹ２は、それぞれＸ軸上の座標と、Ｙ軸上の座標を示す。ｂ２は、仮想線Ｌ２４がＹ軸と交差する点のＹ座標に相当する。
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.
ここで、仮想線Ｌ１３と仮想線Ｌ２４とは、交差、より具体的には直交しており、これら２つの仮想線Ｌ１３、Ｌ２４の交点が４つの第１の反射像ｑ１〜ｑ４の第１の中心ｇ１（ｘ，ｙ）になる。ここで、カメラ１３の撮像系の座標と点光源ＬＳ１〜ＬＳ４との間にＺθ方向の角度オフセットを設けない場合には、仮想線Ｌ１３がＸ軸と平行なり、仮想線Ｌ２４がＹ軸と平行になるので、第１の中心ｇ１（ｑ２（ｘ），ｑ１（ｙ））になる。
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
続く図５のステップＳ２０２からステップＳ２０４は、４つの第１の反射像ｑ１〜ｑ４を１ずつ順番に抽出することによって実施される。まず、ステップＳ２０２で、角度αｎ（第１の角度）を算出する。ここで、図７に示すように、αｎは、カメラ１３の撮像レンズ２１の主点ｎを通過し、凸面ミラー１１の反射点ｐｎを結ぶ線分と、カメラ１３の光軸とのなす角度である。例えば、ｎ＝１のときは、点光源ＬＳ１の光が凸面ミラー１１で反射されたポイント、即ち反射点ｐ１と、反射点ｐ１における反射光が撮像レンズ２１を通って撮像面２２で撮像された第１の反射像ｑ１とを用いて計算される。
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
具体的には、図７を参照し、レンズの主点ｎを通り、凸面ミラー１１の反射点ｐｎを結ぶ線分とカメラ１３の光軸をなす角度αｎを算出する。角度αｎは、
αｎ＝ｔａｎ−１（（ｅｎ＋ｊｎ）／Ｆ）
と表せる。ここで、（ｅｎ＋ｊｎ）は、凸面ミラー１１からの第１の反射像ｑｎとカメラ１３の光軸の中心からの距離である。その内訳は、ｅｎが第１の中心ｇ１から第１の反射像ｑｎまでの距離であり、ｊｎが光軸ｃ１と凸面ミラー１１の中心とのずれ量である。例えば、点光源ＬＳ１又は点光源ＬＳ３を用いて角度α１又は角度α３を算出する場合には、（ｅｎ＋ｊｎ）は、ｘ軸の値が用いられる。一方、点光源ＬＳ２又は点光源ＬＳ４を用いて角度α２又は角度α４を算出する場合には、（ｅｎ＋ｊｎ）は、ｙ軸の値が用いられる。また、Ｆは、撮像レンズ２１の主点ｎから撮像面２２までの距離である。
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
さらに、図５のステップＳ２０３で、距離Ｚｎを算出する。距離Ｚｎは、撮像レンズ２１の主点ｎから凸面ミラー１１の反射点までの距離であって、光軸に沿った長さである。例えば、ｎ＝１のときは、点光源ＬＳ１の反射点ｐ１、反射像ｑ１を用いて距離Ｚ１が算出される。
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
具体的には、最初に、図７に示す点光源ＬＳｎから反射点ｐｎに入射する光の入射角φ´を求める。入射角φ´は、
φ´＝ｔａｎ−１（（Ｌ−ｈ）／Ｚｎ）―γ （１）
で表せる。ここで、Ｌは、カメラ１３の光軸から点光源ＬＳｎまでの距離である。ｈは、カメラ１３の光軸から反射点ｐ１までの距離で、
ｈ＝Ｚｎ×ｔａｎ（α） （２）
である。また、γは、凸面ミラー１１の曲率中心ｍと反射点ｐｎを結ぶ直線と、カメラ１３の光軸とがなす角度（第２の角度）である。
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
ここで、角度γは、
γ＝ｔａｎ−１（ｈ−ｈ０）／Ｒ） （３）
で表せる。ｈ０は、カメラ１３の光軸からの凸面ミラー１１の曲率中心ｍまでのオフセット量である。オフセット量は、点光源ＬＳ１又は点光源ＬＳ３を用いて角度γを算出する場合には、Ｘ軸に沿った長さが用いられる。一方、点光源ＬＳ２又は点光源ＬＳ４を用いて角度γを算出する場合には、Ｙ軸に沿った長さが用いられる。
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
続いて、点光源ＬＳｎからの光が反射点ｐｎにおいて反射したときに生じる反射光の出射角φを求める。出射角φは、
φ＝α＋γ （４）
で表せる。ここで、入射角度φ´と出射角φは、一致するので、（１）〜（４）の式を用いることによって、Ｚ１を算出できる。
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).
この後、図５のステップＳ２０４の判定処理を実行することによって、ステップＳ２０２からステップＳ２０４を繰り返し、残りの３つの点光源ＬＳ２〜ＬＳ４の反射点ｐ２，ｐ３，ｐ４に対するそれぞれの距離Ｚ２、Ｚ３、Ｚ４を算出する。そして、４つの点光源ＬＳ１〜ＬＳ４について算出した４つの距離Ｚ１〜Ｚ４は、本来であれば同一の値であるべきなので、ステップＳ２０５で、４つの距離Ｚ１〜Ｚ４の平均値を算出し、これによって得られた値をＺ軸位置とする。具体的には、ｎ＝２のとき、点光源ＬＳ２におけるＺ２が算出される。さらに、ｎ＝３，４のとき、それぞれ点光源ＬＳ３、ＬＳ４に対するＺ３、Ｚ４が算出される。そして、Ｚｎを４つの点光源ＬＳ１〜ＬＳ４及びその反射点ｐ１〜ｐ４、第１の反射像ｑ１〜ｑ４を用いて算出したら、Ｚ軸位置値を求める。Ｚ軸位置値は、Ｚ＝（Ｚ１＋Ｚ２＋Ｚ３＋Ｚ４）／４の式によって求められる。 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.
ここまでの処理で、初期状態のデータが得られたので、次に、図８に示すフローチャートに従って、移動体の姿勢を変化させながら、移動体をフィードバック制御する。
まず、ステップＳ３０１で、ターゲット部材１０の位置を移動させたときの姿勢位置検出処理を実行する。姿勢位置検出処理の詳細は、後に説明する。続いて、ステップＳ３０２で姿勢偏差検出処理を行い、目標となる基準位置情報と現在の姿勢位置情報との偏差を算出する。そして、ステップＳ３０３の判定処理で、基準位置情報と現在の姿勢位置情報の偏差がゼロであれば、姿勢制御処理を終了する。その一方で、偏差がゼロでなければ、誤差分だけ姿勢移動制御を実施して移動体の姿勢を変化させた後、ステップＳ３０１に戻る。
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
さらに、ステップＳ３０１の姿勢位置検出処理の詳細について、図９のフローチャートを参照して説明する。最初に、ステップＳ４０１で、ターゲットマークを撮像する。具体的には、４つの点光源ＬＳ１〜ＬＳ４を点灯させ、凸面ミラー１１と４つの拡散反射面１２Ａ〜１２Ｄのそれぞれにおける反射光をカメラ１３で撮像する。
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
続くステップＳ４０２で、Ｚ軸位置検出処理を実行する。ここでの処理は、図５と同様であり、移動後のターゲット部材１０のＺ軸位置値が算出される。さらに、ステップＳＳ４０３で、ＸＹ軸位置検出処理を実行し、移動後のターゲット部材１０のＸ軸上の位置値と、Ｙ軸上の位置値を算出する。また、４つの拡散反射面１２Ａ〜１４Ｄに対する各第２の反射像ｓ１〜ｓ４の第２の中心（重心）ｇ２´（ｘ，ｙ）、及び移動後の倍率βｍを算出する。この後、ステップＳ４０４において、Ｚ−θ軸位置検出処理を実行し、Ｚ−θ軸位置値を算出する。Ｚ−θ軸位置値とは、Ｚ軸回りの回転角度である。そして、ステップＳ４０５で、Ｘ−θ軸、Ｙ−θ軸の位置検出処理を実行し、Ｘ−θ軸位置値と、Ｙ−θ軸位置値とを算出する。Ｘ−θ軸位置値と、Ｙ−θ軸位置値は、それぞれＸ軸回りの回転角度と、Ｙ軸回りの回転角度である。
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
以下に、ステップＳ４０３のＸ軸位置値とＹ軸位置値の具体的な算出処理の具体例について説明する。
まず、図６Ｂに、移動後のターゲット部材１０からの反射光を撮像した画像２５Ａを示す。画像２５Ａ中において、ｑ１´〜ｑ４´は、点光源ＬＳ１〜ＬＳ４からの光が移動後の凸面ミラー１１にそれぞれ反射されることによって形成された反射像である。また、ｓ１´〜ｓ４´は、点光源ＬＳ１〜ＬＳ４からの光が移動後の移動後の拡散反射面１２Ａ〜１２Ｄにそれぞれ反射されることによって形成された反射像である。
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
最初に、図６Ｂに示す第２の反射像ｓ１´と第２の反射像ｓ３´を結ぶ仮想線Ｊ１３の式を求める。仮想線Ｊ１３は、
Ｙ３＝（ｓ１´（ｙ）−ｓ３´（ｙ））／（ｓ１´（ｘ）−ｓ３´（ｘ））×Ｘ１＋ｂ３
になる。ここで、Ｘ３とＹ３は、それぞれＸ軸上の座標と、Ｙ軸上の座標を示す。ｂ３は、仮想線Ｊ１３がＹ軸と交差する点のＹ座標に相当する。
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.
同様に、第２の反射像ｓ２´と第２の反射像ｓ４´を結ぶ仮想線Ｊ２４の式を求める。仮想線Ｊ２４は、
Ｙ４＝（ｓ２´（ｙ）−ｓ４´（ｙ））／（ｓ２´（ｘ）−ｓ４´（ｘ））×Ｘ２＋ｂ４
と表せる。ここで、Ｘ４とＹ４は、それぞれＸ軸上の座標と、Ｙ軸上の座標を示す。ｂ４は、仮想線Ｊ２４がＹ軸と交差する点のＹ座標に相当する。仮想線Ｊ１３と仮想線Ｊ２４とは交差しており、これら２つの仮想線Ｊ１３、Ｊ２４の交点が４つの第２の反射像ｓ１´〜ｓ４´の第２の中心ｇ２´（ｘ，ｙ）になる。
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.
また、図１０に示すように、倍率βｍは、βｍ＝Ｍ／Ｎ＝Ｆ／Ｚから算出できる。この結果、ターゲット部材１０上の実際の中心ｇｒ´２のＸ軸位置値は、ｇ２´（ｘ）／βｍで算出できる。同様に、中心のＹ軸位置値は、ｇ２´（ｙ）／βｍで算出できる。そして、座標（ｇ２´（ｘ）／βｍ，ｇ２´（ｙ）／βｍ）がターゲット部材１０のＸＹ平面における実位置となる。
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
次に、図９のステップＳ４０４のＺ−θ軸の位置検出処理の具体例について説明する。
まず、第２の反射像ｓ１´と第２の反射像ｓ３´を結ぶ仮想線Ｊ１３と、カメラ１３のＸ軸のなす角度をＺθ１´とする。同様に、第２の反射像ｓ２´と第２の反射像ｓ４´を結ぶ仮想線Ｊ２４と、カメラ１３のＹ軸のなす角度をＺθ２´とする。このとき、
Ｚθ１´＝ｔａｎ−1（（ｓ３´（ｙ）−ｓ１´（ｙ））／（ｓ３´（ｘ）−ｓ１´（ｘ）））
Ｚθ２´＝ｔａｎ−1（（ｓ４´（ｙ）−ｓ２´（ｙ））／（ｓ４´（ｘ）−ｓ２´（ｘ）））
と表せる。そして、Ｚθ１´とＺθ２´の平均値、即ち（Ｚθ１´＋Ｚθ２´）／２を算出すれば、Ｚ軸回りの回転角度であるＺ−θ値Ｚθ´が得られる。
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.
次に、図９のステップＳ４０５のターゲット部材１０のＸ−θ軸と、Ｙ−θ軸の検出処理の具体例について説明する。
最初に、初期状態の倍率βｓを基準にして、ターゲット部材１０の姿勢が変化したときの拡散反射面１２Ａ，１２Ｃの第２の反射像ｓ１，ｓ３の２点間の距離Ｒｙ´と、拡散反射面１２Ｂ，１２Ｄの第２の反射像ｓ２，ｓ４の２点間の距離Ｒｘ´をそれぞれ算出する。そして、回転方向は、凸面ミラー１１の第１の反射像ｑ１´〜ｑ４´の第１の中心ｇ１´と、拡散反射面１２Ａ〜１２Ｄの第２の反射像ｓ１´〜ｓ４´の第２の中心ｇ２´の位置関係から決定する。
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
ターゲット部材１０の初期状態における拡散反射面１２Ａ〜１２Ｄの第２の反射像ｓ１〜ｓ４の２点間の距離Ｒｘ，Ｒｙは、既に図４のステップＳ１０５の処理で算出されている。また、初期状態における倍率βｓもステップＳ１０４において算出されている。このために、これらの値は、既に算出したものを用いる。
次に、ターゲット部材１０の姿勢を変化させた後の拡散反射面１２Ａ〜１２Ｄの第２の反射像ｓ１´〜ｓ４´の２点間の距離は、
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
で算出できる。また、移動後の倍率βｍは、Ｆ／Ｚｍで算出される。ここで、Ｚｍは、図５と同様の処理によって算出される。 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.
次に、Ｘ軸回りの回転角度であるＸ−θ値Ｘθ´と、Ｙ軸回りの回転角度であるＹ−θ値Ｙθ´を算出する。図１１Ａに模式的に示すように、初期状態の拡散反射面１２Ｂ，１２Ｄが、移動後には、拡散反射面１２Ｂ´，１２Ｄ´になるので、
Ｘθ´＝ｃｏｓ−１(Ｒｘ´／βｍ)／（Ｒｘ／βｓ））
となる。ここで、Ｒｘ／βｓは、画像２５上の第２の反射像ｓ２，ｓ４の２点間の距離Ｒｘを倍率βｓで割り算したもので、初期位置における拡散反射面１２Ｂ，１２Ｄ間の実距離である。同様に、Ｒｘ´／βｍは、画像２５上の第２の反射像ｓ２´，ｓ４´の２点間の距離Ｒｘ´を倍率βｍで割り算したもので、移動後の拡散反射面１２Ｂ，１２Ｄ間の実距離である。
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
同様に、図１１Ｂに模式的に示すように、初期状態の拡散反射面１２Ａ，１２Ｃが、移動後には、拡散反射面１２Ａ´，１２Ｃ´になるので、
Ｙθ＝ｃｏｓ−１(Ｒｙ´／βｍ)／（Ｒｙ／βｓ））
となる。ここで、Ｒｙ／βｓは、画像２５上の第２の反射像ｓ１，ｓ３の２点間の距離Ｒｙを倍率βｓで割り算したもので、初期位置における拡散反射面１２Ａ，１２Ｃ間の実距離である。同様に、Ｒｙ´／βｍは、画像２５上の第２の反射像ｓ１´，ｓ３´の２点間の距離Ｒｙ´を倍率βｍで割り算したもので、移動後の拡散反射面１２Ａ，１２Ｃ間の実距離である。
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
さらに、ターゲット部材１０の回転方向を判定する。ターゲット部材１０の回転方向は、Ｘ−θ軸においては、ｇ１´（ｙ）＞ｇ２´（ｙ）のときは、拡散反射面１２Ｄ側がカメラ１３より遠くなる方向に回転している判定する。また、ｇ１´（ｙ）＜ｇ２´（ｙ）のときは、拡散反射面１２Ｂ側がカメラ１３より遠くなる方向に回転している判定する。そして、ｇ１´（ｙ）＝ｇ２´（ｙ）のときは、Ｘ−θ軸の傾きはゼロであると判定する。
Furthermore, the rotation direction of the
同様に、Ｙ−θ軸においては、ｇ１´（ｘ）＞ｇ２´（ｘ）のときは、拡散反射面１２Ｃ側がカメラ１３より遠くなる方向に回転している判定する。また、ｇ１´（ｘ）＜ｇ２´（ｘ）のときは、拡散反射面１２Ａ側がカメラ１３より遠くなる方向に回転している判定する。そして、ｇ１´（ｘ）＝ｇ２´（ｘ）のときは、Ｙ−θ軸の傾きはゼロであると判定する。
Similarly, on the Y-θ axis, when g1 ′ (x)> g2 ′ (x), it is determined that the diffuse
このようにして、姿勢移動後のＸＹＺの３軸方向の位置値と、各軸回りの回転角度を算出することによって、ターゲット部材１０の現在の姿勢、即ちＸＹＺの３次元空間における位置値と、Ｚ−θ軸の回転角度Ｚθと、Ｘ−θ軸の回転角度Ｘθ´と、Ｙ−θ軸の回転角度Ｙθ´を特定することができる。カメラ１３の位置は予め決められているので、カメラ１３を用いて検出したターゲット部材１０の現在の姿勢が、所望の位置及び回転角度を示す基準位置に一致するようにフィードバック制御すれば、ターゲット部材１０の位置を精度良く制御できる。
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
ここで、ターゲット部材１０に凸面ミラー１１を用いることによる効果について、図１２及び図１３を参照して説明する。図１２は、凸面ミラー１１を用いた場合の撮像面２２上での反射像ｑ１，ｑ３間の距離の変化を示している。一方、図１３は、平板形の反射板を用いた場合の反射像ｑ１，ｑ３間の距離の変化を示している。図１２と図１３において、撮像装置１３の構成と、撮像装置１３に対する点光源ＬＳ１，ＬＳ３の配置は、同一になっている。
Here, the effect of using the
図１２（ａ）に示す姿勢形地装置１では、凸面ミラー１１と、撮像装置１３の撮像レンズ２１との間の距離（Ｚ軸位置）がＺａ１のとき、撮像面２２上で光軸Ｃ１を通る反射像ｑ１，ｑ３間の距離はＷ１になっている。ここで、点光源ＬＳ１，ＬＳ３と、それぞれの反射点ｓ１，ｓ３を結ぶ仮想線と、反射点ｓ１，ｓ３とレンズ２１の主点ｎを結ぶ仮想線とがなす角度はθ１とする。
In the
これに対して、図１２（ｂ）に示すように、凸面ミラー１１と、撮像装置１３の撮像レンズ２１との間の距離をＺａ２まで近づけると、凸面ミラー１１上の反射点ｐ１，ｐ３が離れ、撮像面２２上での反射像ｑ１，ｑ３間の距離は、Ｗ１より大きいＷ２となる。
On the other hand, as shown in FIG. 12B, when the distance between the
一方、図１３（ａ）に示すように、比較例に係る姿勢検出装置１０１では、反射板１０２と、撮像装置１３の撮像レンズ２１との間の距離（Ｚ軸位置）がＺａ１のときに、撮像面２２上で光軸Ｃ１を通る反射像ｑ１，ｑ３間の距離がｗ１になるように構成されている。ここで、点光源ＬＳ１，ＬＳ３と、それぞれの反射板１０２上の反射点ｓ１１，ｓ３３を結ぶ仮想線と、反射点ｓ１１，ｓ３３とレンズ２１の主点ｎを結ぶ仮想線とがなす角度はθ２とする。
On the other hand, as shown in FIG. 13A, in the
これに対して、図１３（ｂ）に示すように、反射板１０２と、撮像装置１３の撮像レンズ２１との間の距離をＺａ２まで近づけると、撮像面２２上での反射像ｑ１，ｑ３間の距離は、ｗ１より大きいｗ３になる。これは、反射板１０２上での反射点ｐ１，ｐ３の位置は変化しないが、反射板１０２から撮像面２２までの距離が短くなるためである。
On the other hand, as shown in FIG. 13B, when the distance between the reflecting plate 102 and the
ここで、比較例における距離ｗ３は、図１２（ｂ）に示す姿勢検出装置１における反射像ｑ１，ｑ３間の距離ｗ２より小さい。これは、図１２に示す姿勢検出装置１は、凸面ミラー１１を用いているので、角度θ１がＺ軸位置の変化に応じて変化し易く、その変化量が比較例における角度φのＺ軸位置の変化に対する変化量より大きくなるためである。このことから、姿勢検出装置１は、Ｚ方向に同じ距離だけ移動したときの反射点ｐ１，ｐ３間の距離（又は光軸Ｃ１から反射点ｐ１，ｐ３までの距離）の変化量を大きくすることができる。
Here, the distance w3 in the comparative example is smaller than the distance w2 between the reflected images q1 and q3 in the
さらに、このようなＺ軸位置の変化に伴う反射像間の距離の変化の一例を図１４に示す。
図１４は、横軸がＺ軸位置、即ちターゲット部材１０とカメラ１３の撮像レンズ２１の間の距離を示す。横軸は、反射点ｐ１と反射点ｐ３の間の距離又は反射点ｐ２と反射点ｐ４の間の距離である。さらに、横軸は、撮像面２２における第１の反射像ｑ１と第１の反射像ｑ３の間の距離又は第１の反射像ｑ２と第１の反射像ｑ４の間の距離も示している。
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
ラインＥ１は、ミラーに平面鏡を用いた比較例のターゲット部材とカメラ１３の撮像レンズ２１の間の距離の変化を示している。比較例のターゲット部材とカメラ１３の撮像レンズ２１の間の距離を変化させても、平面ミラー上の反射点ｓ１〜ｓ４間の距離はあまり変化しない。これは、比較例のターゲット部材のミラーが平面であるためである。これに伴って、ラインＥ２に示すように、撮像面２２における第１の反射像ｑ１〜ｑ４の変化も小さくなる。
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
これに対して、ラインＥ３に示すように、実施形態のターゲット部材１０では凸面ミラー１１を用いたので、ターゲット部材１０とカメラ１３の撮像レンズ２１の間の距離の変化に応じて、凸面ミラー１１上の反射点間の距離が大きく変化する。これに伴って、ラインＥ４に示すように、撮像面２２における第１の反射像ｑ１〜ｑ４の位置の変化が比較例に比べて大きくなる。
On the other hand, as shown by line E3, since the
従って、凸面ミラー１１を使用して取得した反射像ｑ１，ｑ３の位置変化からＺ軸位置を算出することによって、比較例に比べてＺ軸位置の算出精度が向上する。
Therefore, by calculating the Z-axis position from the positional changes of the reflected images q1 and q3 acquired using the
ここで、カメラ１３を基準として、ターゲット部材１０の位置と距離が予め分かっている場合には、点光源ＬＳ１〜ＬＳ４を最小で２個にすることができる。また、ターゲットマークが方向を規定できるような形状を有する場合、ターゲット部材１０の位置と距離が予め分かっていれば、拡散反射面１２Ａ〜１２Ｄを最小で２個にすることができる。
Here, when the position and distance of the
（実施例）
以下に、実施例として、姿勢検出装置１を多関節ロボットに搭載し、かつ点光源の変わりにリング状光源を用い、４つの拡散反射面の代わりにリング状拡散反射面を用いた部品組立装置について説明する。
図１５に示すように、部品組立装置５１は、部品を把持して搬送する多関節ロボット５２と、多関節ロボット５２のハンド５３の姿勢を検出する姿勢検出装置１を有する。
多関節ロボット５２は、ロボット制御部５５からの信号を受けて、ハンド５３の姿勢を制御するように構成されている。ハンド５３の姿勢は、ＸＹＺの３軸に平行な方向及び、３軸回りに回転可能である。
(Example)
Hereinafter, as an embodiment, a component assembly apparatus in which the
As shown in FIG. 15, the
The articulated
姿勢検出装置１は、ハンド５３にターゲット部材１０が取り付けられており、リング状光源６１を制御する照明制御部６２と、カメラ１３を制御するカメラ制御部６３と、カメラ１３で撮像した画像を処理して姿勢検出する検知処理部６４と、ハンド５３の姿勢と基準位置情報６５との偏差を算出する偏差算出部６６とを有する。偏差算出部６６は、偏差の計算結果をロボット制御部５５に出力するように構成されている。
The
図１６及び図１７に示すように、姿勢検出装置１は、ハンド５３の所定位置に取り付けられたターゲット部材１０を有する。ターゲット部材１０の中央には、第１のターゲットマークとなる凸面ミラー１１を有し、凸面ミラー１１の周囲に第２のターゲットマークとなる楕円形状を有するリング状拡散反射面７１（リング状拡散反射部材）が配置されている。リング状拡散反射面７１は、リング状光源６１の光を反射する材料、例えばガラスや石英を用いて製造されている。
As shown in FIGS. 16 and 17, the
ここで、リング状拡散反射面７１の短軸ａの長さは、例えば、ａ＝（凸面ミラー１１の外径）＋（リング状拡散反射面７１のリング幅の１／２）である。また、リング状拡散反射面７１の長径ｂは、ｂ≧ｃｏｓ（θｘｙ）×ａ＋（リング状拡散反射面７１のリング幅の１／２）である。ここで、θｘｙは、姿勢検出装置１におけるＸ−θ軸又はＹ−θ軸の最大検出角度である。
Here, the length of the minor axis a of the ring-shaped diffuse
また、ターゲット部材１０から離れた所定の位置には、カメラ１３と、リング状光源６１とが配置されている。カメラ１３は、撮像レンズ２１がターゲット部材１０に向かうように、かつ光軸が凸面ミラー１１の曲率中心ｍに一致するように配置されている。リング状光源６１は、全周にわたって発光可能に構成されており、中心がカメラ１３の光軸と一致する。
In addition, a
ここで、図１８Ａにカメラ１３で撮像したターゲット部材１０の画像の一例を示す。画像７５には、直交するラインで示されるＸＹ軸に加えて、リング状光源６１の光が凸面ミラー１１で反射されることによって形成された円形の反射像８１と、リング状拡散反射面７１における楕円形の反射像８２とが含まれる。
Here, FIG. 18A shows an example of an image of the
次に、実施例における姿勢検出方法について説明する。姿勢検出の際には、最初に初期化処理を行う。さらに、移動体であるハンド５３の姿勢を変化させながら、ターゲット部材１０を用いて姿勢検出を行い、検出結果を利用して多関節ロボット５２をフィードバック制御することによって、ハンド５３を基準位置に導く。以下に、各処理の詳細について説明する。
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
最初に、図１９のフローチャートを参照して、部品組付装置５１の姿勢位置検出処理の初期化処理について説明する。
ステップＳ１０１では、ロボット制御部５５が、ターゲット部材１０が初期化位置に配置されるように多関節ロボット５２を駆動させる。
続くステップＳ１０２では、カメラ１３でターゲットマークを撮像する。具体的には、照明制御部６２がリング状光源６１を点灯させ、ターゲット部材１０に光を照射する。さらに、カメラ制御部６３の指令に基づいて、凸面ミラー１１とリング状拡散反射面７１のそれぞれにおけるリング状光源６１の反射光をカメラ１３で撮像する。カメラ１３で撮像した画像は、検知処理部６４に送信される。
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
続く、ステップＳ１０２Ａでは、検知処理部６４が反射像８１，８２の画像処理を行い、画像７５に含まれる反射像８１からデータ解析用の４点の第１の反射像ｑ１〜ｑ４を抽出する。データ解析用の第１の反射像ｑ１〜ｑ４は、反射像８１の周方向の等間隔に配置される４つの点を抽出することが好ましい。同様に、画像７５の反射像８２からデータ解析用の４点の第２の反射像ｓ１〜Ｓ４を抽出する。データ解析用の第２の反射像ｓ１〜ｓ４は、反射像８２の周方向の等間隔に配置される４つの点を抽出することが好ましい。さらに、画像７５中の反射像８１の第１の中心ｇ１（ｘ，ｙ）と、反射像８２の第２の中心ｇ２（ｘ，ｙ）と、Ｚ−θ軸位置とをそれぞれ算出する。
Subsequently, in step S102A, the
具体的には、図１８Ａに示す凸面ミラー１１の反射像８１は、円形状になっているので、
（Ｘｉ−ｇ１（ｘ））２＋（Ｙｉ−ｇ１（ｙ））２＝ｒ２ （５）
と表すことができる。この式に、反射像８１を構成するｎ個（ｉ＝１〜ｎ）の座標値（Ｘｉ，Ｙｉ）から、円の近似値を例えば最小二乗法を用いて算出し、第１の中心ｇ１（ｘ，ｙ）と、円の半径ｒを求める。さらに、図１８Ｂに示すように、反射像８１を構成する画素から特徴的な４点を抽出し、データ解析用の４つの第１の反射像ｑ１〜ｑ４を決定する。
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
また、図１８Ａに示すように、リング状拡散反射面７１の反射像８２は、楕円形であるので、
Further, as shown in FIG. 18A, the
と表すことができる。この式に、反射像８２を構成するｎ個（ｉ＝１〜ｎ）の座標値（Ｘｉ，Ｙｉ）から、円の近似値を、例えば最小二乗法を用いて算出し、第２の中心ｇ２（ｘ，ｙ）と、Ｘｒ，Ｙｒ，θを求める。さらに、図１８Ｂに示すように、反射像８２を構成する画素から特徴的な４点を抽出し、データ解析用の４つの第２の反射像ｓ１〜ｓ４を決定する。ここで、距離Ｒｘと距離Ｒｙは、楕円の長軸又は短軸に相当するので、ＸｒとＹｒをそれぞれ２倍した値になる。
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
また、Ｚ―θ軸位置の算出処理では、第２の中心ｇ２を基準にして対向して配置される２つの第２の反射像ｓ１，ｓ３を結ぶ仮想線Ｊ１３のＸ軸に対する傾きであるＺθ１を算出する。さらに、対向して配置される２つの第２の反射像ｓ２，ｓ４を結ぶ仮想線Ｊ２４のＹ軸に対する傾きであるＺθ２を算出する。そして、Ｚθ１とＺθ２の平均を算出すると、Ｚ−θ軸位置値が得られる。 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.
また、図１９のステップＳ１０３で、Ｚ軸位置検出処理を行い、Ｚ軸上の位置を示すＺ軸位置値を算出する。Ｚ軸位置検出処理は、図５と同様である。即ち、反射像８１の第１の中心ｇ１（ｘ，ｙ）を算出し、第１の反射像ｑ１〜ｑ４のそれぞれについて角度αｎを算出し、式（１）〜（４）を用いて、Ｚ１〜Ｚ４を算出する。さらに、Ｚ１〜Ｚ４の平均値を計算し、これをＺ軸位置値とする。
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
続いて、ステップＳ１０４で、カメラ１３の倍率βｓを算出する。倍率βｓは、図１７に示すＦ／Ｚで算出できる。
そして、ステップＳ１０５で、リング状拡散反射面７１によって形成された楕円形の反射像８２の距離Ｒｘと距離Ｒｙをそれぞれ反射像８２の長軸と短軸である。ここまでの処理で、初期状態のデータが得られた。初期状態のデータは、検知処理部の不図示のメモリに記憶される。
Subsequently, in step S104, the magnification βs of the
In step S105, the distance Rx and the distance Ry of the
次に、図８に示すフローチャートに従って、ロボットハンドの姿勢制御を開始する。即ち、ステップＳ３０１で、ロボット制御部５５の指令に基づいてターゲット部材１０の位置を移動させたときの姿勢位置検出処理を姿勢検出装置１で実行する。続いて、ステップＳ３０２で偏差算出部６６が姿勢偏差検出処理を行い、目標となる基準位置情報６５と現在の姿勢位置情報とを比較する。基準位置情報６５とは、例えば、カメラ１３の位置を基準として、ハンド５３が所望する位置に精度良く配置された状態におけるターゲット部材１０の姿勢を特定する情報である。そして、ステップＳ３０３の判定処理で、偏差算出部６６が基準位置情報と現在の姿勢位置情報との偏差がゼロになったと判定すれば、姿勢制御処理を終了する。その一方で、偏差がゼロでなければ、ロボット制御部５５が偏差を打ち消すような指令値を算出して多関節ロボット５２を駆動させ、ステップＳ３０１に戻る。
Next, the posture control of the robot hand is started according to the flowchart shown in FIG. That is, in step S301, the
さらに、ステップＳ３０１の姿勢位置検出処理の詳細について図２０のフローチャートを主に参照して説明する。
最初に、ステップＳ４０１で、ターゲットマークを撮像する。具体的には、リング状光源６１を点灯させ、凸面ミラー１１とリング状拡散反射面７１のそれぞれにおける反射光をカメラ１３で撮像する。
続くステップＳ４０１Ａで、検知処理部６４が反射像８１，８２を画像処理する。ここでの処理は、図１９のステップＳ１０２Ａと同様である。また、ステップＳ４０２で、検知処理部６４がＺ軸位置検出処理を実行する。ここでの処理は、図５と同様の処理である。
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
さらに、ステップＳ４０３で、ＸＹ軸位置検出処理を実行し、ターゲット部材１０のＸ軸上の位置と、Ｙ軸上の位置を算出する。また、各第２の反射像ｓ１〜ｓ４の中心（重心）の座標ｇ２´（ｘ，ｙ）、及び倍率βｍを算出する。この後、ステップＳ４０４において、Ｚ−θ軸位置検出処理を実行し、Ｚ−θ軸位置値を算出する。そして、ステップＳ４０５で、Ｘ−θ軸、Ｙ−θ軸の位置検出処理を実行し、Ｘ−θ軸位置値と、Ｙ−θ軸位置値とを算出する。
In step S403, an XY axis position detection process is executed to calculate the position of the
ここで、移動後に図２１に示すような画像８５が得られた場合、リング状光源６１の凸面ミラー１１における反射像８１からデータ解析用の第１の反射像ｑ１´〜ｑ４´が抽出される。同様に、リング状光源６１のリング状拡散反射面７１における反射像８２からデータ解析用の第２の反射像ｓ１´〜ｓ４´が抽出される。そして、移動後の中心ｇ１´、ｇ２´は、式（５）、（６）から算出される。ターゲット部材１０のＸＹ座標は、（ｇ２´（ｘ）／βｍ，ｇ２´（ｙ）／βｍ）になる。
Here, when an
Ｚ軸位置は、Ｚ＝（Ｚ１＋Ｚ２＋Ｚ３＋Ｚ４）／４で算出される。Ｚ１〜Ｚ４は、式（１）〜式（４）で算出できる。Ｚ−θ軸の回転角度は、（Ｚθ１＋Ｚθ２）／２であり、Ｚθ１．Ｚθ２は、
Ｚθ１＝ｔａｎ−1（（ｓ３´（ｙ）−ｓ１´（ｙ））／（ｓ３´（ｘ）−ｓ１´（ｘ）））
Ｚθ２＝ｔａｎ−1（（ｓ４´（ｙ）−ｓ２´（ｙ））／（ｓ４´（ｘ）−ｓ２´（ｘ）））
から算出される。
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
図１１Ａに示すように、Ｘθは、Ｙθ＝ｃｏｓ−１(Ｒｘ´／βｍ)／（Ｒｘ／βｓ））になる。図１１Ｂに示すように、Ｙθは、Ｙθ＝ｃｏｓ−１(Ｒｙ´／βｍ)／（Ｒｙ／βｓ））になる。Ｘ−θ軸の回転方向と、Ｙ−θ軸の回転方向は、前記と同様に、ｇ１´とｇ２´の位置関係から決定する。 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.
この実施例では、リング状光源６１からの光を凸面ミラー１１とリンク状拡散反射面７１で反射するようにしたので、カメラ１３で２つのリング状の反射像８１，８２が得られる。例えば、リング状光源６１やターゲット部材１０に汚れがあったり、姿勢検出装置１の光路上に障害物があったりした場合で、反射像８１，８２の１／３程度が欠落した場合であっても、近似計算によって円又は楕円を補完することができるので、ターゲット部材１０と、ターゲット部材１０が取り付けられたハンド５３の姿勢を、精度を低下させることなく検出できる。従って、図１５に示す部品組付装置５１において、ハンド５３で把持した部品９１を高い位置精度で他の部品９２に組み付けることができる。また、ハンド５３に工具を取り付けて他の部品９２を加工する場合には、高い精度の加工が可能になる。姿勢検出装置１の用途は、部品組付装置５１に限定されず、部品の搬送装置などに用いても良い。
In this embodiment, since the light from the ring-shaped
ここで、リング状光源６１を円形にし、リング状拡散反射面７１を楕円形状にしたので、図１８Ａや図２０に示す反射像８１と反射像８２の区別が容易に付けられる。しかしながら、反射像８１より反射像８２が必ず大きくなるように配置される場合には、リング状光源６１とリング状拡散反射面７１とを同じ形状にしても良い。また、リング状光源６１、リング状拡散反射面７１の少なくとも一方は、一部が切り欠かれたＣ形状にすることができる。さらに、リング状光源６１、リング状拡散反射面７１の少なくとも一方は、２つ以上に分割された円弧又は楕円弧でも良い。
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.
以下に、前記の実施の形態の特徴を付記する。
（付記１） 光源からの光を移動体に照射し、前記移動体に取り付けられた凸面ミラーにおける反射光を撮像装置で撮像し、前記光源の光の第１の反射像を含む画像を作成する工程と、前記凸面ミラー上の反射点と前記第１の反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第１の角度を算出する工程と、前記第１の反射点と凸面ミラーの中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第２の角度と、前記光源と前記撮像装置の光軸との間の距離と、前記第１の角度とを用い、前記光源から光の前記第１の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する工程と、を含むことを特徴とする移動体の姿勢検出方法。
（付記２） 複数の前記第１の反射像のそれぞれを用いて算出した、前記撮像装置から前記凸面ミラーの前記反射点までの距離の平均値を算出する工程を含むことを特徴とする付記１に記載の移動体の姿勢検出方法。
（付記３） 前記光源からの光を移動体の前記凸面ミラーの周囲に配置された複数の拡散反射部材に照射し、それぞれの反射光を前記撮像装置で撮像し、複数の第２の反射像を取得し、前記複数の第２の反射像の中心から、前記複数の前記第２の反射像の中心である第２の中心を算出する工程と、前記第２の中心を挟んで配置される２つの前記第２の反射像を結ぶ仮想線と、前記撮像装置の光軸に垂直な軸とがなす角度から前記撮像装置の光軸に平行な軸回りの前記移動体の回転角度を算出する工程と、を含むことを特徴とする付記１又は付記２に記載の移動体の姿勢検出方法。
（付記４） 前記移動体を移動させた前後のそれぞれのタイミングで、前記第２の中心を通り、所定の２つの前記第２の反射像を結ぶ仮想線の長さを算出し、前記移動体の移動前後での前記仮想線の長さ変化から、前記撮像装置の光軸に直交する軸線回りの回転角度を算出する工程をさらに有する付記３に記載の移動体の姿勢検出装置。
（付記５） 前記移動体は、多関節ロボットの先端に取り付けられたハンドであり、前記ハンドの姿勢の現在値と、予め定められた前記ハンドの基準位置との偏差を算出し、偏差がゼロになるように前記多関節ロボットを駆動させる工程を有することを特徴とする付記１乃至付記４のいずれか一項に記載の部品組立方法。
（付記６） 移動体に取り付けられた凸面ミラーと、前記凸面ミラーで反射した光を受光する撮像装置と、前記撮像装置の光軸を囲むように配置され、前記凸面ミラー及び前記拡散反射部材を照明する光源と、前記凸面ミラー上の反射点と前記第１の反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第１の角度を算出し、前記第１の反射点と凸面ミラーの中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第２の角度と、前記光源と前記撮像装置の光軸との間の距離とを用い、前記光源から光の前記第１の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する処理部と、を含むことを特徴とする移動体の姿勢検出装置。
（付記７） 前記光源は、円環形状を有する付記６に記載の移動体の姿勢検出装置。
（付記８） 前記光源は、４つ以上の点光源である付記１に記載の移動体の姿勢検出装置。
（付記９） 付記６乃至付記８のいずれか一項に記載の姿勢検出装置と、先端にハンドが設けられた多関節ロボットと、を含み、前記移動体は、多関節ロボットの先端に取り付けられた部品組立用の前記ハンドであることを特徴とする部品組立装置。
（付記１０） 前記処理部は、前記姿勢検出装置で検出した前記ハンドの姿勢と、予め定められた前記ハンドの基準位置との偏差を算出する偏差算出部を有し、前記偏差がゼロになるように前記ロボットを駆動させるように構成したことを特徴とする付記９に記載の部品組立装置。
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
１ 姿勢検出装置
１１ 凸面ミラー
１２Ａ〜１２Ｄ 拡散反射面（拡散反射部材）
１３ 撮像装置
５１ 部品組付装置
５２ 多関節ロボット
５３ ハンド（移動体）
６１ リング状光源
６４ 検知処理部（処理部）
７１ リング状拡散反射面（拡散反射部材）
Ｃ１ 光軸
ＬＳ１〜ＬＳ４ 光源
ｑ１〜ｑ４ 第１の反射像
ｓ１〜ｓ４ 第２の反射像
ｇ１ 第１の中心
ｇ２ 第２の中心
Ｘθ，Ｙθ，Ｚθ 回転角度
α 第１の角度
γ 第２の角度
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)
前記凸面ミラー上の反射点と前記反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第１の角度を算出する工程と、
前記反射点と凸面ミラーの曲率中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第２の角度と、前記光源と前記撮像装置の光軸との間の距離と、前記第１の角度とを用い、前記光源から光の前記反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する工程と、
前記撮像装置の光軸を中心とする仮想円上に配置された前記光源からの光が前記凸面ミラーで反射することで撮像された複数の前記反射像を結ぶ仮想線の交点から複数の前記反射像の中心を算出する工程と、
複数の前記反射部材における反射光を前記撮像装置で撮影して得られた複数の反射像のうち、複数の前記反射像の中心を基準にして対向して配置される２つの前記反射像を結ぶ仮想直線の傾きを算出し、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度を算出する工程と、
前記移動体の姿勢を変化させながら、前記凸面ミラーの前記反射点までの距離と複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度とを算出し、前記凸面ミラーの前記反射点までの距離と、複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度と、目標とする距離と位置と角度を有する基準位置との偏差がゼロになる姿勢に前記移動体を移動させる指令値を算出する工程と、
を含むことを特徴とする移動体の姿勢検出方法。 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:
前記移動体に取り付けられ、前記凸面ミラーの周囲に配置された複数の反射部材と、
前記凸面ミラー及び複数の前記反射部材で反射した光を受光する撮像装置と、
前記撮像装置の光軸を囲むように配置され、前記凸面ミラー及び拡散反射部材を照明する光源と、
前記凸面ミラー上の反射点と前記反射点で反射した光を前記撮像装置で撮像した反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第１の角度を算出し、前記反射点と凸面ミラーの曲率中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第２の角度と、前記光源と前記撮像装置の光軸との間の距離とを用い、前記光源から光の前記第１の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出し、前記撮像装置の光軸を中心とする仮想円上に配置された前記光源からの光が前記凸面ミラーで反射することで撮像された複数の前記反射像を結ぶ仮想直線の交点から複数の前記反射像の中心を算出し、複数の前記反射部材における反射光を前記撮像装置で撮影して得られた複数の反射像のうち、複数の前記反射像の中心を基準にして対向して配置される２つの前記反射像を結ぶ仮想直線の傾きを算出し、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度を算出し、前記移動体の姿勢を変化させながら、前記凸面ミラーの前記反射点までの距離と複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度とを算出し、前記凸面ミラーの前記反射点までの距離と、複数の前記反射像の中心の位置と、前記撮像装置の光軸に直交する軸線と前記仮想直線とがなす角度と、目標とする距離と位置と角度を有する基準位置との偏差がゼロになる姿勢に前記移動体を移動させる指令値を算出する処理部と、
を含むことを特徴とする移動体の姿勢検出装置。 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 | ＴｅｃｈＳｈａｒｅ株式会社 | 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 |