JP2903964B2 - Three-dimensional position and posture recognition method based on vision and three-dimensional position and posture recognition device based on vision - Google Patents
Three-dimensional position and posture recognition method based on vision and three-dimensional position and posture recognition device based on visionInfo
- Publication number
- JP2903964B2 JP2903964B2 JP24249693A JP24249693A JP2903964B2 JP 2903964 B2 JP2903964 B2 JP 2903964B2 JP 24249693 A JP24249693 A JP 24249693A JP 24249693 A JP24249693 A JP 24249693A JP 2903964 B2 JP2903964 B2 JP 2903964B2
- Authority
- JP
- Japan
- Prior art keywords
- marker
- orientation
- straight line
- direction vector
- unit
- 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.)
- Expired - Fee Related
Links
Landscapes
- Control Of Position Or Direction (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【0001】[0001]
【産業上の利用分野】本発明は、単眼視カメラにより撮
影した画像情報に基づいてその単眼視カメラの三次元位
置および姿勢を認識するようにした視覚に基く三次元位
置および姿勢の認識方法ならびに視覚に基く三次元位置
および姿勢の認識装置に関する。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method of recognizing a three-dimensional position and orientation based on vision, which recognizes the three-dimensional position and orientation of the monocular camera based on image information taken by the monocular camera. The present invention relates to a three-dimensional position and orientation recognition device based on vision.
【0002】[0002]
【従来の技術】近年、例えば、自律移動ロボット等のよ
うに、形状および絶対座標位置が既知である対象物体に
対して接近するように移動し、ロボットハンドの先端に
設けたマニピュレータにより対象物体を把持するといっ
た能動的操作を行うようにしたものがある。このような
自律移動ロボット等においては、CCDカメラなどの撮
像手段を搭載してその二次元的な画像情報に基いて対象
物体に対する自己の三次元的な相対位置を認識すること
が必要になる。この場合、位置認識を行うための装置に
おいては、高精度且つ迅速に計算を行って三次元的な位
置のデータを求めることが要求され、しかも安価に実現
できる構成が望まれている。2. Description of the Related Art In recent years, for example, an autonomous mobile robot or the like moves so as to approach a target object whose shape and absolute coordinate position are known, and moves the target object by a manipulator provided at the tip of a robot hand. In some cases, active operations such as grasping are performed. In such an autonomous mobile robot, it is necessary to mount imaging means such as a CCD camera and recognize its own three-dimensional relative position with respect to the target object based on the two-dimensional image information. In this case, in a device for performing position recognition, it is required to perform calculation with high accuracy and speed to obtain three-dimensional position data, and a configuration that can be realized at low cost is desired.
【0003】そこで、従来では、比較的簡単なものとし
て、例えば、特開平3−166072号公報あるいは特
開平3−166073号公報に示されるようなものが考
えられている。すなわち、これらにおいては、対象装置
に固定された形状および寸法が既知の特殊な幾何学形状
(マーカと呼ぶ)をカメラにより撮影し、そのマーカの
二次元的な画像情報に基いて、その重心位置を計算する
等の方法により対象装置に対するCCDカメラの相対的
な三次元の位置関係を求め、これに基づいてロボットハ
ンドを移動制御するようにしたものである。[0003] Conventionally, as a relatively simple device, for example, a device as disclosed in Japanese Patent Application Laid-Open No. Hei 3-166072 or Japanese Patent Application Laid-Open No. Hei 3-166073 has been considered. That is, in these, a special geometric shape (referred to as a marker) having a known shape and dimensions fixed to the target device is photographed by a camera, and the center of gravity is determined based on the two-dimensional image information of the marker. The relative three-dimensional positional relationship of the CCD camera with respect to the target device is obtained by a method such as calculating the following, and the movement of the robot hand is controlled based on this.
【0004】[0004]
【発明が解決しようとする課題】しかしながら、上述の
ような従来構成のものでは、CCDカメラによるマーカ
の撮影を、CCDカメラの光軸に対する垂直面内にマー
カ表示面が位置するようにCCDカメラを移動させて行
うようにしており、マーカの撮影画面がマーカ表示面の
二次元的な相対位置と1対1の対応関係となるようにす
る必要があるため、次のような実用上の不具合がある。However, in the above-described conventional configuration, the imaging of the marker by the CCD camera is performed such that the marker display surface is positioned in a plane perpendicular to the optical axis of the CCD camera. This is performed by moving the marker, and it is necessary to make the photographing screen of the marker have a one-to-one correspondence with the two-dimensional relative position of the marker display surface. is there.
【0005】すなわち、例えば、対象物を把持するため
のロボットハンドにカメラを取り付けていると、対象物
の近傍に配置されたマーカを垂直方向から撮影するため
にロボットハンドを適切な位置まで移動させる必要があ
るが、このとき、ロボットハンド先端のマニピュレータ
が物品の把持状態にある場合などにおいては、その移動
範囲に制約を受けることになる。[0005] That is, for example, when a camera is attached to a robot hand for gripping an object, the robot hand is moved to an appropriate position in order to vertically photograph a marker arranged near the object. However, at this time, when the manipulator at the tip of the robot hand is in a state of gripping an article, the movement range is restricted.
【0006】また、CCDカメラによりマーカを視野内
に入るように移動させるために、マーカの設置領域が狭
いと、ロボットハンドがその作業環境と接触して互いに
損傷を与える不具合が生ずるため、マーカの設置領域を
ロボットハンドが十分に移動可能となるような作業環境
にする必要があり、作業環境に大きく制約を与えること
になるという不具合がある。Further, when the marker is moved into the field of view by the CCD camera, if the area where the marker is installed is small, the robot hands come into contact with the working environment and cause damage to each other. It is necessary to set the installation area in a work environment in which the robot hand can move sufficiently, and there is a problem that the work environment is greatly restricted.
【0007】ところが、従来、このような不具合を解消
すべく、マーカから斜方向に離れた位置から撮影するこ
とにより、カメラの位置姿勢を高速且つ高精度でロバス
ト(ロバストとは、一般的に、処理や認識の結果がノイ
ズや環境条件の変動によってあまり影響を受けないこと
を定性的に示すことばとして用いられている。)に認識
できるものがなかった。However, conventionally, in order to solve such a problem, by photographing from a position obliquely away from the marker, the position and orientation of the camera can be robustly and quickly and accurately (robust is generally called “robustness”). It is used as a qualitative term to indicate that the results of processing and recognition are not significantly affected by noise or changes in environmental conditions.)
【0008】本発明は、上記事情に鑑みてなされたもの
で、その目的は、撮像手段により平面マーカを撮影して
三次元位置を認識するときに、平面マーカに対して撮像
手段を垂直方向に対向させることなく、斜方向から撮影
した画像情報に基づいて、簡単な二値化画像処理を実施
するだけで、高速且つ高精度でしかもロバストな位置姿
勢認識を行うことができるようにした視覚に基く三次元
位置および姿勢の認識方法およびその装置を提供するこ
とにある。SUMMARY OF THE INVENTION The present invention has been made in view of the above circumstances, and an object of the present invention is to make the image pickup means perpendicular to the plane marker when the three-dimensional position is recognized by photographing the plane marker by the image pickup means. Without facing each other, based on image information taken from an oblique direction, it is possible to perform high-speed, high-accuracy and robust position / posture recognition simply by performing simple binary image processing. It is an object of the present invention to provide a three-dimensional position and orientation recognition method and apparatus based on the three-dimensional position and orientation.
【0009】[0009]
【課題を解決するための手段】本発明の視覚に基く三次
元位置および姿勢の認識方法は、所定寸法を有し互いに
直交する2本の線分とそれら2本の線分にそれぞれ平行
な線分とを含んで囲まれた図形を背景部分に対して濃淡
情報あるいは色情報の変化により平面上に表示されたマ
ーカを備え、前記撮像手段からの画像データに基づいて
前記マーカの画像領域を選択すると共に、その選択され
た画像領域内の画像データの値の変化点を検出して輪郭
点位置を抽出する画像処理ステップと、この画像処理ス
テップにて抽出された前記輪郭点位置のデータに基づい
て、撮影画面上における前記マーカの直交する線分およ
びそれらに平行な線分をそれぞれ含む直線のパラメータ
を最小二乗推定法により計算する直線あてはめ処理ステ
ップと、前記マーカに表示された平行な線分に対応して
前記直線あてはめ処理ステップにて計算された直線の交
点である消失点のデータに基づいて前記マーカの単位方
向ベクトルを計算する単位方向ベクトル計算処理ステッ
プと、前記マーカの直交する線分の交点を原点としてそ
れら各線分の方向およびマーカ面に垂直でその原点を通
る法線方向を基準方向としたマーカ座標系を設定し、前
記単位方向ベクトルに基づいて前記マーカの位置姿勢を
示すパラメータを計算する位置姿勢計算処理ステップ
と、この位置姿勢計算処理ステップにおける前記撮像手
段の位置姿勢を示すパラメータ計算結果に基づいて、前
記撮像手段の基準座標系から見た前記マーカの位置姿勢
を表す座標変換行列を計算する座標変換行列計算処理ス
テップとを設けたところに特徴を有する。According to the present invention, there is provided a method for recognizing a three-dimensional position and orientation based on vision, which comprises two line segments having predetermined dimensions and orthogonal to each other, and lines respectively parallel to the two line segments. A marker which is displayed on a plane by a change in grayscale information or color information with respect to a background portion of the figure enclosed including the minutes, and selects an image area of the marker based on image data from the imaging means. An image processing step of detecting a change point of the value of the image data in the selected image area and extracting a contour point position, and based on the data of the contour point position extracted in the image processing step. A straight line fitting processing step of calculating parameters of straight lines each including a line segment orthogonal to the marker and a line segment parallel to the marker on a shooting screen by a least squares estimation method; A unit direction vector calculation processing step of calculating a unit direction vector of the marker based on vanishing point data which is an intersection of the straight lines calculated in the straight line fitting processing step corresponding to the parallel line segments displayed in A marker coordinate system is set with the intersection of the line segments orthogonal to the marker as the origin, and the direction of each line segment and the normal direction perpendicular to the marker plane and passing through the origin as the reference direction, based on the unit direction vector. A position and orientation calculation processing step of calculating a parameter indicating the position and orientation of the marker; and a parameter calculation result indicating the position and orientation of the imaging means in the position and orientation calculation processing step, as viewed from a reference coordinate system of the imaging means. And a coordinate transformation matrix calculation processing step of calculating a coordinate transformation matrix representing the position and orientation of the marker. .
【0010】また、前記マーカを、背景部分に対して濃
淡情報あるいは色情報の変化により表示された少なくと
も2個の長方形状あるいは正方形状をなす図形から構成
し、それらの図形を互いに一つの頂点で接すると共にそ
の頂点を含む隣接する辺同士が直角をなすように配置し
て構成すると良い。[0010] The marker is composed of at least two rectangular or square figures which are displayed by a change in shading information or color information with respect to the background part, and these figures are mutually represented by one vertex. It is good to arrange so that adjacent sides including the vertices that are in contact with each other and make an angle may form a right angle.
【0011】そして、前記直線あてはめ処理ステップに
おいては、前記画像処理ステップにて抽出された前記輪
郭点位置のデータに基づいて、前記マーカのひとつの線
分に対応して直線を複数本検出し、それらの直線の平均
化処理を行うことにより1本の直線を求めてその直線パ
ラメータ決定することが好ましい。In the line fitting processing step, a plurality of straight lines corresponding to one line segment of the marker are detected based on the data of the contour point position extracted in the image processing step, It is preferable to obtain one straight line by averaging those straight lines and determine the straight line parameters.
【0012】さらに、前記単位方向ベクトル計算処理ス
テップにおいては、複比不変の法則を利用して前記消失
点の候補値を3つ以上計算し、それらの候補値の2次元
座標データに基づいて前記単位方向ベクトルを計算する
ことが好ましい。Further, in the unit direction vector calculation processing step, three or more candidate values of the vanishing point are calculated using the law of invariance of cross ratio, and the candidate values are calculated based on the two-dimensional coordinate data of the candidate values. Preferably, a unit direction vector is calculated.
【0013】また、前記単位方向ベクトル計算処理ステ
ップにおいては、前記マーカの3本以上の平行な線分に
それぞれ対応して得られた直線について、それらの交点
に対する単位方向ベクトルをモーメント行列に対する二
次形式最小化問題として解くことにより前記単位方向ベ
クトルを計算すると良い。In the unit direction vector calculation processing step, for a straight line obtained corresponding to each of three or more parallel line segments of the marker, a unit direction vector with respect to an intersection point between the straight lines is quadratic with respect to a moment matrix. It is preferable to calculate the unit direction vector by solving as a formal minimization problem.
【0014】そして、本発明の視覚に基く三次元位置お
よび姿勢の認識装置は、所定寸法を有する少なくとも2
本の平行な線分からなる2組の平行線群が互いに直交す
るように配置された状態を背景部分に対して濃淡情報あ
るいは色情報の変化により平面上に表示されたマーカ
と、撮影画像の画像データを出力する撮像手段と、前記
マーカの形状寸法を示すマーカデータが記憶された記憶
手段と、前記撮像手段からの画像データに基づいて前記
マーカの画像領域を選択すると共に、その選択された画
像領域内の画像データの値の変化点を検出して輪郭点位
置を抽出する画像処理手段と、この画像処理手段により
抽出された前記輪郭点位置のデータに基づいて前記マー
カの直交する平行線群のそれぞれの線分を含む直線のパ
ラメータを最小二乗推定法により求めるようにした直線
あてはめ処理手段と、前記2組の平行線群の各線分を含
む直線の前記撮影画面上における交点である消失点のデ
ータに基づいて前記マーカの単位方向ベクトルを計算す
る単位方向ベクトル計算処理手段と、前記直線あてはめ
処理手段により求められた2本の直交する直線とその交
点を通る法線とを座標系の軸としたマーカ座標系を設定
し、このマーカ座標系と前記単位方向ベクトルに基づい
て前記マーカの位置姿勢を計算する位置姿勢計算処理手
段と、この位置姿勢計算処理手段による前記撮像手段の
位置姿勢を示すパラメータ計算結果に基づいて、前記撮
像手段の基準座標系から見た前記マーカの位置姿勢を表
す座標変換行列を計算する座標変換行列計算手段とを設
けて構成したところに特徴を有する。Further, the three-dimensional position and orientation recognition apparatus based on vision according to the present invention has at least two
A state in which two sets of parallel lines composed of two parallel line segments are arranged so as to be orthogonal to each other is a marker displayed on a plane by a change in density information or color information with respect to a background portion, and an image of a captured image. Imaging means for outputting data, storage means for storing marker data indicating the shape and dimensions of the marker, and selecting an image area of the marker based on image data from the imaging means, and selecting the selected image Image processing means for detecting a change point of the value of the image data in the area and extracting a contour point position, and a group of parallel lines orthogonal to the marker based on the data on the contour point position extracted by the image processing means Line fitting processing means for obtaining the parameters of the straight line including the respective line segments by the least squares estimation method, and the photographed image of the straight line including the respective line segments of the two sets of parallel lines. A unit direction vector calculation processing means for calculating a unit direction vector of the marker based on data of a vanishing point which is an intersection point above, and a method passing through two orthogonal straight lines obtained by the straight line fitting processing means and the intersection point thereof A position and orientation calculation processing means for setting a marker coordinate system having a line as an axis of a coordinate system and calculating the position and orientation of the marker based on the marker coordinate system and the unit direction vector; And a coordinate conversion matrix calculation unit that calculates a coordinate conversion matrix representing the position and orientation of the marker viewed from a reference coordinate system of the imaging unit based on a parameter calculation result indicating the position and orientation of the imaging unit. It has features.
【0015】また、前記マーカを、背景部分に対して濃
淡情報あるいは色情報の変化により表示された少なくと
も2個の長方形状あるいは正方形状をなす図形から構成
し、それらの図形を互いに一つの頂点で接すると共にそ
の頂点を含む隣接する辺同士が直角をなすように配置す
ることが好ましい。Further, the marker is constituted by at least two rectangular or square figures which are displayed by a change in shading information or color information with respect to a background portion, and these figures are mutually formed by one vertex. It is preferable to arrange so that adjacent sides that touch and include the vertex form a right angle.
【0016】そして、前記直線あてはめ処理手段を、前
記画像処理手段により抽出された前記輪郭点位置のデー
タに基づいて、前記マーカのひとつの線分に対応して直
線を複数本検出し、それらの直線の平均化処理を行うこ
とにより1本の直線を求めてその直線パラメータ決定す
るように構成すると良い。The straight line fitting processing means detects a plurality of straight lines corresponding to one line segment of the marker based on the data of the contour point position extracted by the image processing means. It is preferable that one straight line is obtained by averaging the straight lines, and the straight line parameters are determined.
【0017】さらに、前記単位方向ベクトル計算処理手
段を、複比不変の法則を利用して前記消失点の候補値を
3つ以上計算し、それらの候補値の2次元座標データに
基づいて前記単位方向ベクトルを計算するように構成す
ることができる。Further, the unit direction vector calculation processing means calculates three or more candidate values of the vanishing point using the law of invariance of cross ratio, and calculates the unit value based on the two-dimensional coordinate data of the candidate values. It can be configured to calculate a direction vector.
【0018】また、前記単位方向ベクトル計算処理手段
を、前記マーカの3本以上の平行な線分にそれぞれ対応
して得られた直線について、それらの交点に対する単位
方向ベクトルをモーメント行列に対する二次形式最小化
問題として解くことにより前記単位方向ベクトルを計算
するように構成することもできる。Further, the unit direction vector calculation processing means may calculate a unit direction vector for a straight line corresponding to each of three or more parallel line segments of the marker with respect to their intersection by a quadratic form for a moment matrix. The unit direction vector may be calculated by solving as a minimization problem.
【0019】[0019]
【作用】請求項1記載の視覚に基く三次元位置および姿
勢の認識方法によれば、マーカに対して三次元空間の斜
方向に位置する撮像手段によりそのマーカを撮影したと
きに、以下のように各処理ステップを実行することによ
り、マーカに対する撮像手段の三次元位置および姿勢を
認識することができるようになる。According to the method for recognizing a three-dimensional position and orientation based on vision according to the first aspect, when the marker is photographed by the imaging means positioned obliquely in the three-dimensional space with respect to the marker, By executing each processing step, it becomes possible to recognize the three-dimensional position and orientation of the imaging means with respect to the marker.
【0020】すなわち、まず、撮像手段により撮影され
たマーカの画像データは、画像処理ステップにおいて、
マーカの画像領域が選択されると共に、その選択された
画像領域内の画像データの値の変化点を検出して輪郭点
位置が抽出されるようになる。このとき、マーカの各線
分は濃淡情報あるいは色情報の変化で表示されてるの
で、線分の検出を線幅を持たない図形の輪郭線から求め
ることができるようになり、画像処理が簡単になると共
に、検出誤差を極力低減して輪郭点位置を精度良く抽出
することができる。That is, first, in the image processing step, the image data of the marker photographed by the image pickup means is obtained.
The image area of the marker is selected, and a change point of the value of the image data in the selected image area is detected to extract the contour point position. At this time, since each line segment of the marker is displayed as a change in shading information or color information, the detection of the line segment can be obtained from the outline of a figure having no line width, and image processing is simplified. At the same time, it is possible to extract the contour point position with high accuracy by minimizing the detection error.
【0021】次に、直線あてはめ処理ステップにおい
て、上述の画像処理ステップにて抽出された輪郭点位置
のデータに基づいてマーカの直交する線分およびそれら
に平行な線分をそれぞれ含む直線のパラメータが最小二
乗推定法により求められるようになる。Next, in the straight line fitting processing step, based on the data of the contour point position extracted in the above-described image processing step, the parameters of the straight line including the line segment orthogonal to the marker and the line segment parallel to the marker are calculated. It can be obtained by the least squares estimation method.
【0022】そして、単位方向ベクトル計算処理ステッ
プにおいては、マーカを撮像手段により斜方向から撮影
していることにより、マーカに表示された平行な線分に
対応して直線あてはめ処理ステップにて計算された直線
が無限遠で交差するため、その交点である消失点の2次
元座標が計算され、これから単位方向ベクトルが計算さ
れるようになる。In the unit direction vector calculation processing step, since the marker is photographed from an oblique direction by the imaging means, the marker is calculated in the straight line fitting processing step corresponding to the parallel line segment displayed on the marker. Since the straight lines intersect at infinity, the two-dimensional coordinates of the vanishing point, which is the intersection, are calculated, and the unit direction vector is calculated from this.
【0023】続いて、位置姿勢計算処理ステップにおい
て、マーカの直交する線分の交点を原点としてそれら各
線分の方向およびマーカ面に垂直でその原点を通る法線
方向を基準方向としたマーカ座標系が設定され、単位方
向ベクトル計算処理ステップにて求められた単位方向ベ
クトルに基づいてマーカの位置姿勢を示すパラメータが
計算されるようになる。Subsequently, in a position / orientation calculation processing step, a marker coordinate system with the intersection of the line segments orthogonal to the marker as the origin and the direction of each line segment and the normal direction perpendicular to the marker plane and passing through the origin as the reference direction. Is set, and a parameter indicating the position and orientation of the marker is calculated based on the unit direction vector obtained in the unit direction vector calculation processing step.
【0024】そして、座標変換行列計算処理ステップに
おいて、撮像手段の位置姿勢を示すパラメータ計算結果
に基づき、撮像手段の基準座標系から見たマーカの位置
姿勢を表す座標変換行列が計算されるようになる。In the coordinate transformation matrix calculation processing step, a coordinate transformation matrix representing the position and orientation of the marker viewed from the reference coordinate system of the imaging means is calculated based on the parameter calculation result indicating the position and orientation of the imaging means. Become.
【0025】この場合、撮像手段によりマーカを斜方向
から撮影することにより三次元位置および姿勢が認識で
きると共に、マーカの図形に対する簡単な計算処理を実
行するだけで良いので、高速且つ高精度で認識できると
共に、ロバストな処理結果を得ることができるようにな
る。In this case, the three-dimensional position and orientation can be recognized by photographing the marker obliquely by the image pickup means, and simple calculation processing for the figure of the marker only needs to be executed. And a robust processing result can be obtained.
【0026】請求項2記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、マーカを、2個以上の長方
形状あるいは正方形状をなす簡単な図形で表示している
ので、マーカの位置姿勢認識の計算処理を簡単化するこ
とができるようになると共に、例えば、図形を横方向に
並べるように配置することにより、撮像手段の撮影画面
に対してマーカの傾き角度が大きくなる場合でも、線分
の奥行き方向の距離変化を少なくして焦点ずれによる誤
差を低減して精度良く検出することができるようにな
る。According to the method for recognizing a three-dimensional position and orientation based on vision according to the second aspect of the present invention, the markers are displayed as two or more simple rectangular or square figures. In addition to simplifying the posture recognition calculation process, for example, by arranging the figures so as to be arranged in the horizontal direction, even when the inclination angle of the marker with respect to the shooting screen of the imaging unit becomes large, It is possible to reduce the distance change in the depth direction of the line segment, reduce errors due to defocus, and perform accurate detection.
【0027】請求項3記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、直線あてはめ処理ステップ
において、画像処理ステップで抽出された前記輪郭点位
置のデータに基づいて、マーカのひとつの線分に対応し
て直線を複数本検出し、それらの直線の平均化処理を行
うことにより1本の直線を求めてその直線パラメータを
決定するので、例えば、撮像手段により撮影されたマー
カが、焦点ずれにより輪郭線が直交する線分として検出
できない場合でも、それらの輪郭点位置から得られる複
数本の直線から平均化することにより、より正確な直線
として検出することができるようになる。According to a third aspect of the present invention, in the line fitting processing step, one of the markers is determined based on the data of the contour point position extracted in the image processing step. By detecting a plurality of straight lines corresponding to the line segments and averaging the straight lines to obtain a single straight line and determine the straight-line parameters, for example, a marker photographed by the imaging unit is Even when a contour cannot be detected as an orthogonal line segment due to defocus, it can be detected as a more accurate straight line by averaging a plurality of straight lines obtained from the contour point positions.
【0028】請求項4記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、単位方向ベクトル計算処理
ステップにおいて、複比不変の法則を利用して消失点の
候補値を3つ以上計算し、それらの候補値の2次元座標
データに基づいて単位方向ベクトルを計算するので、例
えば、平行な線分に対応して検出された直線に誤差が生
じていてそれらの交点から得られる消失点が複数存在す
る場合でも、より正確な単位方向ベクトルを検出するこ
とができるようになる。According to the third aspect of the present invention, in the unit direction vector calculation processing step, three or more vanishing point candidate values are calculated using the bi-invariance rule. Then, since the unit direction vector is calculated based on the two-dimensional coordinate data of those candidate values, for example, a vanishing point obtained from the intersection point of the straight line detected corresponding to the parallel line segment has an error. Is more accurate, it is possible to detect a more accurate unit direction vector.
【0029】請求項5記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、単位方向ベクトル計算処理
ステップにおいて、マーカの3本以上の平行な線分にそ
れぞれ対応して得られた直線について、それらの交点に
対する単位方向ベクトルをモーメント行列に対する二次
形式最小化問題として解くことにより計算するので、上
述と同様にして検出された直線に誤差が生じている場合
でも、より正確な単位方向ベクトルを検出することがで
きるようになる。According to the method for recognizing a three-dimensional position and orientation based on vision, in the unit direction vector calculation processing step, straight lines obtained respectively corresponding to three or more parallel line segments of the marker are provided. Is calculated by solving the unit direction vector for those intersections as a quadratic form minimization problem for the moment matrix. Therefore, even if an error occurs in the straight line detected in the same manner as described above, a more accurate unit direction can be obtained. Vectors can be detected.
【0030】請求項6記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、マーカに対して三次元空間
の斜方向に位置する撮像手段によりそのマーカを撮影し
たときに、以下のようしてマーカに対する撮像手段の三
次元位置および姿勢を認識することができるようにな
る。According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to the sixth aspect, when the marker is photographed by the imaging means positioned obliquely in the three-dimensional space with respect to the marker, As a result, it becomes possible to recognize the three-dimensional position and orientation of the imaging means with respect to the marker.
【0031】すなわち、撮像手段によりマーカを三次元
空間の斜方向から撮影したときに得られるマーカの画像
データに対して、画像処理手段により、マーカの画像領
域が選択されると共に、その選択された画像領域内の画
像データの値の変化点を検出して輪郭点位置を抽出す
る。このとき、マーカの各線分は濃淡情報あるいは色情
報の変化で表示されてるので、線分の検出を線幅を持た
ない図形の輪郭線から求めることができるようになり、
画像処理が簡単になると共に、検出誤差を極力低減して
輪郭点位置を精度良く抽出することができる。That is, with respect to the marker image data obtained when the marker is photographed in the oblique direction of the three-dimensional space by the image pickup means, the image processing means selects the marker image area and the selected image area. A change point of the value of the image data in the image area is detected, and a contour point position is extracted. At this time, since each line segment of the marker is displayed as a change in shading information or color information, the line segment can be detected from the outline of a figure having no line width.
Image processing is simplified, and a detection error can be reduced as much as possible to accurately extract the contour point position.
【0032】次に、直線あてはめ処理手段により、抽出
された輪郭点位置のデータに基づいてマーカの直交する
線分およびそれらに平行な線分をそれぞれ含む直線のパ
ラメータを最小二乗推定法により求める。次に、マーカ
を撮像手段により斜方向から撮影していることにより、
マーカに表示された平行な線分に対応して直線あてはめ
処理手段により計算された直線が無限遠位置では交差す
ることを利用して、単位方向ベクトル計算処理手段によ
り、その交点である消失点の2次元座標を計算し、これ
から単位方向ベクトルを計算するようになる。Next, based on the extracted contour point position data, the straight line fitting processing means obtains the parameters of the straight line including the line segment orthogonal to the marker and the line segment parallel thereto by the least squares estimation method. Next, since the marker is photographed from an oblique direction by the imaging means,
Utilizing that the straight line calculated by the straight line fitting processing means corresponding to the parallel line segment displayed on the marker intersects at the infinity position, the unit direction vector calculation processing means makes use of the intersection point of the vanishing point. The two-dimensional coordinates are calculated, and the unit direction vector is calculated from this.
【0033】続いて、位置姿勢計算処理手段により、マ
ーカの直交する線分の交点を原点としてそれら各線分の
方向およびマーカ面に垂直でその原点を通る法線方向を
基準方向としたマーカ座標系を設定し、単位方向ベクト
ル計算手段により求められた単位方向ベクトルに基づい
てマーカの位置姿勢を示すパラメータを計算する。そし
て、撮像手段の位置姿勢を示すパラメータ計算結果に基
づき、座標変換行列計算処理手段により、撮像手段の基
準座標系から見たマーカの位置姿勢を表す座標変換行列
を計算する。Subsequently, the position and orientation calculation processing means sets a marker coordinate system with the intersection of the line segments orthogonal to the marker as the origin, the direction of each line segment and the normal direction perpendicular to the marker plane and passing through the origin as the reference direction. Is set, and a parameter indicating the position and orientation of the marker is calculated based on the unit direction vector obtained by the unit direction vector calculation means. Then, based on the parameter calculation result indicating the position and orientation of the imaging unit, the coordinate conversion matrix calculation processing unit calculates a coordinate conversion matrix representing the position and orientation of the marker viewed from the reference coordinate system of the imaging unit.
【0034】この場合、撮像手段によりマーカを斜方向
から撮影することにより三次元位置および姿勢が認識で
きると共に、マーカの図形に対する簡単な計算処理を実
行するだけで良いので、高速且つ高精度で認識できると
共に、ロバストな処理結果を得ることができるようにな
る。In this case, the three-dimensional position and orientation can be recognized by photographing the marker obliquely by the imaging means, and simple calculation processing for the figure of the marker only needs to be performed. And a robust processing result can be obtained.
【0035】請求項7記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、マーカを、2個以上の長方
形状あるいは正方形状をなす簡単な図形で表示している
ので、マーカの位置姿勢認識の計算処理を簡単化するこ
とができるようになると共に、例えば、図形を横方向に
並べるように配置することにより、撮像手段の撮影画面
に対してマーカの傾き角度が大きくなる場合でも、線分
の奥行き方向の距離変化を少なくして焦点ずれによる誤
差を低減して精度良く検出することができるようにな
る。According to the apparatus for recognizing three-dimensional position and orientation based on vision according to the seventh aspect, the marker is displayed as two or more simple figures having a rectangular or square shape. In addition to simplifying the posture recognition calculation process, for example, by arranging the figures so as to be arranged in the horizontal direction, even when the inclination angle of the marker with respect to the shooting screen of the imaging unit becomes large, It is possible to reduce the distance change in the depth direction of the line segment, reduce errors due to defocus, and perform accurate detection.
【0036】請求項8記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、直線あてはめ処理手段によ
り、画像処理ステップで抽出された前記輪郭点位置のデ
ータに基づいて、マーカのひとつの線分に対応して直線
を複数本検出し、それらの直線の平均化処理を行うこと
により1本の直線を求めてその直線パラメータ決定する
ので、例えば、撮像手段により撮影されたマーカが、焦
点ずれにより輪郭線が直交する線分として検出できない
場合でも、それらの輪郭点位置から得られる複数本の直
線から平均化することにより、より正確な直線として検
出することができるようになる。According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to claim 8, one of the markers is determined by the straight line fitting processing means based on the data of the contour point position extracted in the image processing step. A plurality of straight lines are detected corresponding to the line segments, and one straight line is obtained by averaging the straight lines to determine the straight line parameters. Even when the contour cannot be detected as an orthogonal line segment due to the displacement, it can be detected as a more accurate straight line by averaging a plurality of straight lines obtained from the contour point positions.
【0037】請求項9記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、単位方向ベクトル計算処理
手段により、複比不変の法則を利用して消失点の候補値
を3つ以上計算し、それらの候補値の2次元座標データ
に基づいて単位方向ベクトルを計算するので、例えば、
平行な線分に対応して検出された直線に誤差が生じてい
てそれらの交点から得られる消失点が複数存在する場合
でも、より正確な単位方向ベクトルを検出することがで
きるようになる。According to the apparatus for recognizing three-dimensional position and orientation based on vision according to the ninth aspect, the unit direction vector calculation processing means calculates three or more vanishing point candidate values using the law of invariance of cross ratio. Then, since the unit direction vector is calculated based on the two-dimensional coordinate data of those candidate values, for example,
Even when there is an error in the straight line detected corresponding to the parallel line segment and there are a plurality of vanishing points obtained from the intersections, a more accurate unit direction vector can be detected.
【0038】請求項10記載の視覚に基く三次元位置お
よび姿勢の認識装置によれば、単位方向ベクトル計算処
理手段により、マーカの3本以上の平行な線分にそれぞ
れ対応して得られた直線について、それらの交点に対す
る単位方向ベクトルをモーメント行列に対する二次形式
最小化問題として解くことにより計算するので、上述と
同様にして検出された直線に誤差が生じている場合で
も、より正確な単位方向ベクトルを検出することができ
るようになる。According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to the tenth aspect, straight lines obtained by the unit direction vector calculation processing means corresponding to three or more parallel line segments of the marker, respectively. Is calculated by solving the unit direction vector for those intersections as a quadratic form minimization problem for the moment matrix. Therefore, even if an error occurs in the straight line detected in the same manner as described above, a more accurate unit direction can be obtained. Vectors can be detected.
【0039】[0039]
【実施例】以下、本発明を工場内などの軌道上を走行す
る自律移動ロボットに搭載する三次元位置認識装置に適
用した場合の第1の実施例について図1ないし図14を
参照しながら説明する。全体構成の概略を示す図2にお
いて、自律走行ロボット(図示せず)は、工場内などの
軌道上を走行して所定の作業位置で停止して、本体上部
に取り付けられたロボットハンド1により対象物を把持
したり載置したりするなどの作業を行うものであり、所
定の位置に、センサからの検出信号や外部情報などに基
づいて停止されるようになっている。DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS A first embodiment in which the present invention is applied to a three-dimensional position recognition apparatus mounted on an autonomous mobile robot running on a track in a factory or the like will be described below with reference to FIGS. I do. 2, an autonomous mobile robot (not shown) travels on a track in a factory or the like, stops at a predetermined work position, and is controlled by a robot hand 1 attached to an upper part of the main body. An operation such as grasping or placing an object is performed, and the operation is stopped at a predetermined position based on a detection signal from a sensor, external information, or the like.
【0040】ロボットハンド1は、自律走行ロボットの
本体上部に回動可能に保持されたアーム部2とこのアー
ム部2の先端に回動可能に設けられたヘッド部3から構
成され、このヘッド部3に対象物を把持するためのマニ
ピュレータ4が設けられている。そして、ロボットハン
ド1は、図示しない制御装置からの制御信号に応じてア
ーム部2,ヘッド部3およびマニピュレータ4が駆動制
御されるようになっている。The robot hand 1 comprises an arm 2 rotatably held at the upper part of the main body of the autonomous mobile robot and a head 3 rotatably provided at the tip of the arm 2. A manipulator 4 for gripping an object is provided at 3. In the robot hand 1, the drive of the arm 2, head 3 and manipulator 4 is controlled in accordance with a control signal from a control device (not shown).
【0041】この場合、アーム部2は、基端部2aにお
いて垂直方向の軸を回転軸として水平面内で回動可能と
され、中間部に設けられた関節2bにおいて水平方向の
軸を回転軸として垂直面内で回動可能とされている。ま
た、アーム部2の先端部に取り付けられたヘッド部3
は、水平方向の軸を回転軸として垂直面内で回動可能と
され、ヘッド部3の上下方向の軸を回転軸として回動可
能とされると共に、ヘッド部3の突出方向の軸を回転軸
として捩じる方向に回動可能とされている。In this case, the arm portion 2 is rotatable in a horizontal plane with a vertical axis as a rotation axis at the base end 2a, and a horizontal axis as a rotation axis at a joint 2b provided at an intermediate portion. It is rotatable in a vertical plane. Also, a head unit 3 attached to the tip of the arm unit 2
Is rotatable about a horizontal axis as a rotation axis in a vertical plane, is rotatable about a vertical axis of the head section 3 as a rotation axis, and is rotated about an axis in a protruding direction of the head section 3. The shaft is rotatable in a twisting direction.
【0042】また、このヘッド部3の上部には、マニピ
ュレータ4の動作に支障を来さない位置に撮像手段とし
ての小形のCCDカメラ5が取り付けられており、その
ヘッド部3と共に移動制御されるようになっている。そ
して、このCCDカメラ5により撮影された画像データ
に基づいて、後述するようにして、CCDカメラ5の位
置と姿勢が計算により求められると、その結果に応じて
制御装置によりロボットハンド1の動作が駆動制御され
るようになっている。A small CCD camera 5 as an image pickup means is mounted on the upper part of the head 3 at a position where the operation of the manipulator 4 is not hindered. It has become. When the position and orientation of the CCD camera 5 are obtained by calculation based on the image data captured by the CCD camera 5 as described later, the operation of the robot hand 1 is controlled by the control device according to the result. The drive is controlled.
【0043】マニピュレータ4により作業する位置に
は、対象物との位置関係があらかじめ決められた関係で
配置されたマーカ6が配設されている。このマーカ6
は、図3に示すように、例えば、白地の背景部分に黒色
で表示された2個の正方形部7および8を横に並べた状
態で配置された構成となっている。At the position where the manipulator 4 works, there is provided a marker 6 whose positional relationship with the object is arranged in a predetermined relationship. This marker 6
Has a configuration in which, for example, two square portions 7 and 8 displayed in black are arranged side by side on a white background portion as shown in FIG.
【0044】この場合、正方形部7および8は、ひとつ
の頂点で接する状態に配置され、正方形部7の4つの辺
a1,a2,a3,a4および正方形部8の4つの辺b
1,b2,b3,b4のうち、辺a1および辺b2と、
辺a2および辺b1とがそれぞれ一直線上に位置するよ
うに配置されている。そして、正方形部7および8が接
する点をマーカ座標系の原点Oとしている。なお、マー
カ6の大きさは、CCDカメラ5で撮影したときにその
撮影視野内に全体が十分な大きさで且つ歪みのない状態
で入るように設定されている。In this case, the square portions 7 and 8 are arranged so as to be in contact with one vertex, and four sides a1, a2, a3, a4 of the square portion 7 and four sides b of the square portion 8
1, b2, b3, and b4, sides a1 and b2;
The sides a2 and b1 are arranged so as to be located on a straight line. The point where the square portions 7 and 8 are in contact is defined as the origin O of the marker coordinate system. In addition, the size of the marker 6 is set so that when the image is taken by the CCD camera 5, the whole is within a field of view of the image taken with a sufficient size and without distortion.
【0045】さて、三次元位置姿勢認識装置9は、具体
的には、例えば全体の計算制御を実行させるコンピュー
タ等から構成されるもので、その内部の機能的なブロッ
ク構成は図1に示すように構成されている。すなわち、
図1において、三次元位置姿勢認識装置9の全体は、概
略構成として、画像処理手段としての画像処理部10,
直線あてはめ手段としての直線あてはめ部11,単位方
向ベクトル計算手段としての消失点計算部12,位置姿
勢計算手段としての位置姿勢計算部13,座標変換行列
計算手段としての座標変換行列計算部14および記憶手
段としてのメモリ15から構成されており、それぞれ
は、後述するフローチャートの流れに従って計算処理が
実行されるようになっている。The three-dimensional position / posture recognition device 9 is specifically composed of, for example, a computer for executing overall calculation control, and its internal functional block configuration is as shown in FIG. Is configured. That is,
In FIG. 1, the whole of a three-dimensional position and orientation recognition device 9 has, as a schematic configuration, an image processing unit 10 as image processing means,
A straight line fitting unit 11 as a straight line fitting unit, a vanishing point calculation unit 12 as a unit direction vector calculation unit, a position and orientation calculation unit 13 as a position and orientation calculation unit, a coordinate conversion matrix calculation unit 14 as a coordinate conversion matrix calculation unit, and storage It comprises a memory 15 as a means, and each executes a calculation process according to the flow of a flowchart described later.
【0046】そして、画像処理部10は、前処理計算部
16,サンプル点抽出部17から構成されている。前処
理計算部16は、前処理計算としてCCDカメラ5が撮
影したマーカ6の画像データを二値化したデジタル画像
データとし、マーカ6の存在する画像領域を切り出して
出力する。サンプル点抽出部17は、切り出された画像
領域のマーカ画像が存在する位置の座標データから、正
方形部7および8の輪郭点を抽出してサンプル点座標デ
ータとして出力する。The image processing section 10 includes a pre-processing calculation section 16 and a sample point extraction section 17. The pre-processing calculation section 16 converts the image data of the marker 6 captured by the CCD camera 5 into digital image data as binarized data as a pre-processing calculation, cuts out an image area where the marker 6 is present, and outputs it. The sample point extracting unit 17 extracts the contour points of the squares 7 and 8 from the coordinate data of the position where the marker image in the clipped image area exists, and outputs it as sample point coordinate data.
【0047】直線あてはめ部12は、最小二乗推定部1
8および直線パラメータ出力部19から構成されてお
り、最小二乗推定部18により、後述するようにしてサ
ンプル点座標データから撮影画面上における直線を検出
すると、直線パラメータ出力部19により、検出した直
線のパラメータが出力されるようになっている。The straight-line fitting unit 12 includes the least-squares estimating unit 1
8 and a straight line parameter output unit 19. When the least square estimation unit 18 detects a straight line on the photographing screen from the sample point coordinate data as described later, the straight line parameter output unit 19 Parameters are output.
【0048】消失点計算部12は、モーメント行列計算
部20,最小固有値計算部21,単位固有ベクトル計算
部22,最適ベクトル計算部23および消失点座標推定
部24から構成される。これらは、検出された直線のパ
ラメータから、後述するように、マーカ6の平行な線分
に対応する直線のパラメータに基づいて、それらの直線
が無限遠点で撮影画面上において交差する点である消失
点を計算するもので、ここでは二次形式最小化問題とし
てこれを解くようにしている。The vanishing point calculator 12 comprises a moment matrix calculator 20, a minimum eigenvalue calculator 21, a unit eigenvector calculator 22, an optimum vector calculator 23, and a vanishing point coordinate estimator 24. These are points at which the straight lines intersect at an infinity point on the shooting screen based on the detected straight line parameters based on the straight line parameters corresponding to the parallel line segments of the marker 6 as described later. The vanishing point is calculated, and is solved here as a quadratic form minimization problem.
【0049】位置姿勢計算部13は、焦点距離計算部2
5,姿勢計算部26およびマーカ原点位置計算部27か
ら構成される。これらは、求められた撮影画面上におけ
る消失点の二次元座標データとマーカ6の原点Oを基準
としたマーカ座標系とに基づいて、最適ベクトル,焦点
距離,姿勢ベクトルおよびマーカ原点位置を計算するよ
うになっている。The position and orientation calculator 13 includes a focal length calculator 2
5, a posture calculator 26 and a marker origin position calculator 27. These calculate an optimal vector, a focal length, a posture vector, and a marker origin position based on the obtained two-dimensional coordinate data of the vanishing point on the photographing screen and the marker coordinate system based on the origin O of the marker 6. It has become.
【0050】なお、メモリ15は、マーカ6の形状寸法
等を示すマーカデータが記憶されると共に、CCDカメ
ラ5の撮影画角やアスペクト比(画面の縦横の比率)な
どの、あらかじめわかっている情報が記憶されている。The memory 15 stores marker data indicating the shape and size of the marker 6 and the like, and also knows in advance information such as the angle of view of the CCD camera 5 and the aspect ratio (vertical and horizontal ratio of the screen). Is stored.
【0051】座標変換行列部14は、上述のようにして
求められたマーカ6の位置姿勢を表すデータから、CC
Dカメラ5の座標系を基準としたマーカ6の位置姿勢を
表す座標変換行列を計算して求めるようになっており、
その計算結果を、データ出力部28を介して自律移動ロ
ボットの動作を制御する制御装置等に与えるようになっ
ている。The coordinate transformation matrix unit 14 calculates the CC from the data representing the position and orientation of the marker 6 obtained as described above.
A coordinate transformation matrix representing the position and orientation of the marker 6 with respect to the coordinate system of the D camera 5 is calculated and obtained.
The calculation result is provided to a control device or the like for controlling the operation of the autonomous mobile robot via the data output unit 28.
【0052】次に、本実施例の作用について、図4ない
し図13をも参照し、以下、各処理ステップに対応して
順次説明する。まず、自律走行ロボットは、作業位置近
傍に近付くと、別途に設けられたセンサ等により所定の
停止領域内に停止され、その位置でCCDカメラ5によ
りマーカ6の撮影を行なうように、あらかじめティーチ
ングされている。この場合、CCDカメラ5は、図5に
示すように、マーカ6を正方形部7,8が横に並んだ状
態で斜め上方向から撮影するようになっており、これに
より、CCDカメラ5の撮影視野内にマーカ6の全体像
が歪みなく取り込まれるようになっている。Next, the operation of the present embodiment will be sequentially described with reference to FIGS. First, when approaching the vicinity of the work position, the autonomous mobile robot is stopped in a predetermined stop area by a separately provided sensor or the like, and is taught in advance so that the CCD camera 5 takes an image of the marker 6 at that position. ing. In this case, as shown in FIG. 5, the CCD camera 5 shoots the marker 6 obliquely from above with the square portions 7 and 8 arranged side by side. The whole image of the marker 6 is taken in the field of view without distortion.
【0053】そして、三次元位置認識装置8は、図4に
示す認識プログラムの流れを示すフローチャートにした
がって三次元位置姿勢の認識を行うための処理ステップ
を実行するようになる。Then, the three-dimensional position recognizing device 8 executes processing steps for recognizing the three-dimensional position and orientation in accordance with the flowchart of the recognition program shown in FIG.
【0054】(a)画像処理ステップ(S1〜S4) さて、まず、プログラムがスタートされると、各種の初
期値設定を実行した(ステップS1)後、前処理計算部
16により、CCDカメラ5で撮影されたマーカ6の画
像データを入力するようになる(ステップS2)。次
に、前処理計算部16は、ステップS3にて、前処理計
算を実行するようになる。この前処理計算においては、
CCDカメラ5から入力した画像データを所定のしきい
値で二値化することにより、明暗を示すアナログデータ
を「1」,「0」などの二値化されたデジタル画像デー
タに変換すると共に、マーカ6が撮影された画像領域を
切り出して出力するようになる。(A) Image Processing Steps (S1 to S4) First, when the program is started, various initial value settings are executed (step S1), and then the preprocessing calculation unit 16 uses the CCD camera 5 The photographed image data of the marker 6 is input (step S2). Next, the pre-processing calculation unit 16 executes the pre-processing calculation in step S3. In this preprocessing calculation,
By binarizing the image data input from the CCD camera 5 with a predetermined threshold value, the analog data representing light and dark is converted into binary digital image data such as "1" or "0", and The image area where the marker 6 is photographed is cut out and output.
【0055】これにより、マーカ6に黒色で描かれた正
方形部7および8に該当する位置のデジタル画像データ
と白地の背景部分に該当する位置のデジタル画像データ
とが、それぞれ「1」および「0」のいずれかのデジタ
ル画像データとして識別されるようになる。As a result, the digital image data at the positions corresponding to the square portions 7 and 8 drawn in black on the marker 6 and the digital image data at the positions corresponding to the white background portion are “1” and “0”, respectively. ] Is identified as any one of the digital image data.
【0056】この後、サンプル抽出部17は、ステップ
S4で、デジタル画像データに基づいて、マーカ6の正
方形部7および8の部分の辺a1〜a4,b1〜b4に
相当する輪郭線をサンプリングする。マーカ6の正方形
部7および8の各辺を通る直線は、図6に平面図で示す
ように、6本の直線Lm1〜Lm6がある。Thereafter, in step S4, the sample extracting section 17 samples the contour lines corresponding to the sides a1 to a4, b1 to b4 of the square portions 7 and 8 of the marker 6 based on the digital image data. . As shown in the plan view of FIG. 6, there are six straight lines Lm1 to Lm6 as straight lines passing through the sides of the square portions 7 and 8 of the marker 6.
【0057】サンプル抽出においては、これらの直線L
m1〜Lm6に対応する撮影画面上における直線L1〜
L6を検出するために、図7に示すように、撮影画面上
で横方向つまりx軸方向に走査してデジタル画像データ
値が変化する位置の画素の座標データを記憶していく。
これにより、撮影画面上における正方形部7,8の各辺
a1〜a4,b1〜b4上のサンプリング点を、撮影画
面上の二次元座標で示された例えばs個のサンプル抽出
点SPn[xn,yn]として得ることができる(ただ
しn=1,2,…,s)。In sample extraction, these straight lines L
Straight lines L1 to L1 on the shooting screen corresponding to m1 to Lm6
In order to detect L6, as shown in FIG. 7, scanning is performed in the horizontal direction, that is, in the x-axis direction, on the shooting screen, and the coordinate data of the pixel at the position where the digital image data value changes is stored.
Accordingly, the sampling points on the sides a1 to a4 and b1 to b4 of the square portions 7 and 8 on the shooting screen are converted into, for example, s sample extraction points SPn [xn, yn] (where n = 1, 2,..., s).
【0058】(b)直線あてはめ処理ステップ(S5〜
S9) 次に、ステップS5に進むと、直線あてはめ処理部11
の最小二乗推定部18および直線パラメータ出力部19
において、得られたs個のサンプル抽出点SPnの座標
データに基づいて、マーカ6の直線Lm1〜Lm6に対
応する撮影画面上における直線L1〜L6(図8参照)
を最小二乗推定法により計算し、得られた直線のパラメ
ータを出力するようになる。(B) Straight-line fitting processing steps (S5 to S5)
S9) Next, proceeding to step S5, the straight-line fitting processing unit 11
Least squares estimation unit 18 and straight line parameter output unit 19
In, based on the obtained coordinate data of the s sample extraction points SPn, straight lines L1 to L6 on the shooting screen corresponding to the straight lines Lm1 to Lm6 of the marker 6 (see FIG. 8)
Is calculated by the least squares estimation method, and the parameters of the obtained straight line are output.
【0059】この場合、最小二乗推定法を適用して計算
を実施するにあたって、本実施例においては、直線L1
〜L6の一般式を式(1)のようなヘッセの標準形で表
現し、この方程式にサンプル抽出点SPn[xn,y
n]の座標データを適用し、後述する(付録)に示した
ような導出過程により計算を実行して直線パラメータ
[θi,hi]を計算している。In this case, when performing the calculation by applying the least squares estimation method, in this embodiment, the straight line L1
To L6 are expressed in a Hessian standard form as shown in equation (1), and this equation includes a sampling point SPn [xn, y
n], and the calculation is executed by a derivation process as described in the appendix below to calculate the straight line parameters [θi, hi].
【0060】なお、(付録)に記載した導出過程及び以
下の探索過程における導出では、小野寺,金谷氏による
『計算射影幾何学によるカメラキャリブレーション』
(情報処理学会研究報告68−CV−1,1990−0
9)を参考にした。In the derivation process described in (Appendix) and the derivation in the following search process, “Camera calibration by computational projection geometry” by Onodera and Kanaya
(Information Processing Society of Japan Research Report 68-CV-1, 1990-0
9) was referred to.
【0061】[0061]
【数1】 (Equation 1)
【0062】なお、上述のようにして直線の検出を行う
場合に、例えば、CCDカメラ5による撮影画面がフォ
ーカスずれを起こしていることにより、マーカ6の画像
にボケが生ずる場合があると、図9に示すように、輪郭
線があいまいになっていわゆるグレーゾーンGLが生ず
ることがある。そして、本実施例においては、このよう
なフォーカスずれに対応して、ステップS6において、
直線Liが検出されるべき部分のグレーゾーンGLに対
応して複数本の直線Li1,Li2,…を検出してそれ
らの直線パラメータの平均化処理を行うことにより一つ
の直線Liのパラメータを決定するようにしている。When the straight line is detected as described above, for example, if the image taken by the CCD camera 5 is out of focus, the image of the marker 6 may be blurred. As shown in FIG. 9, the outline may become ambiguous and a so-called gray zone GL may occur. In this embodiment, in response to such a focus shift, in step S6,
By detecting a plurality of straight lines Li1, Li2,... Corresponding to the gray zone GL of the portion where the straight line Li is to be detected, and averaging the straight line parameters, the parameters of one straight line Li are determined. Like that.
【0063】そして、以上のようにして、例えば直線L
1についての計算が終了すると、ステップS7にてその
直線のパラメータ[θ1,h1]を求め、ステップS
8,S9を経て再びステップS5〜S7を実行して次の
直線L2について同様にして直線のパラメータ[θ2,
h2]を求め、以下、同様にして直線L3〜L6につい
ても直線のパラメータを計算して求めるようになる。As described above, for example, the straight line L
Upon completion of the calculation for step S1, the parameters [θ1, h1] of the straight line are obtained in step S7.
Steps S5 to S7 are executed again after steps S8 and S9, and the parameters of the straight line [θ2,
h2], and similarly, the parameters of the straight lines L3 to L6 are calculated and calculated.
【0064】(c)マーカ単位方向ベクトル計算処理ス
テップ(S10〜S17) 次に、CCDカメラ5により撮影しているマーカ6の姿
勢を示す単位方向ベクトルを上述のようにして得られた
6本の直線L1〜L6のパラメータ[θi,hi](i
=1〜6)のデータに基づいて計算する。(C) Marker unit direction vector calculation processing steps (S10 to S17) Next, the unit direction vectors indicating the orientation of the marker 6 being photographed by the CCD camera 5 are obtained from the six unit direction vectors obtained as described above. The parameters [θi, hi] (i
= 1 to 6).
【0065】この場合、マーカ6の面内の平行な直線L
m1〜Lm3の組および直線Lm4〜Lm6の組に対し
て、撮影画面上の対応する平行な直線L1〜L3の組お
よびL4〜L6の組においては、CCDカメラ5により
マーカ6を斜方向から撮影していることから、それぞれ
の平行な直線の組における無限遠に対応する点が撮像画
面上のある1点でそれぞれ交差するようになる。In this case, a parallel straight line L in the plane of the marker 6
With respect to the set of m1 to Lm3 and the set of parallel straight lines L1 to L3 and the set of L4 to L6 corresponding to the set of m1 to Lm3 and the set of straight lines Lm4 to Lm6, the marker 6 is photographed from the oblique direction by the CCD camera 5. Therefore, points corresponding to infinity in each set of parallel straight lines intersect at a certain point on the imaging screen.
【0066】このとき、それらの交点をそれぞれ消失点
V1,V2とすると、マーカ6の座標系を設定すること
により、そのマーカ座標原点Oと撮影画面上のその消失
点V1,V2との二次元座標データに基づいて幾何学的
性質を利用すると、CCDカメラ5の位置姿勢を一意的
に決めることができる。At this time, assuming that these intersections are vanishing points V1 and V2, respectively, by setting the coordinate system of the marker 6, a two-dimensional image of the marker coordinate origin O and the vanishing points V1 and V2 on the photographing screen is obtained. When the geometric properties are used based on the coordinate data, the position and orientation of the CCD camera 5 can be uniquely determined.
【0067】ところが、上述の各直線の組においてその
消失点V1あるいはV2を計算すると、一般に、図10
に示すように、複数個の消失点候補V1a,V1b,…
およびV2a,V2b,…が得られるので、これらの消
失点候補のデータから最適な消失点V1あるいはV2を
求めるべく、以下の2つの方法により別々に計算し、そ
れらの結果から最適な消失点V1,V2に対応するデー
タを計算するようにしている。However, when the vanishing point V1 or V2 is calculated for each set of the above-described straight lines, generally, FIG.
, A plurality of vanishing point candidates V1a, V1b,.
, And V2a, V2b,... Are obtained. In order to find the optimum vanishing point V1 or V2 from the data of these vanishing point candidates, the optimum vanishing point V1 or V2 is separately calculated by the following two methods, and the optimal vanishing point V1 is obtained from the results. , V2 are calculated.
【0068】すなわち、第1には(ア)複比不変の法則
を利用して計算する方法であり、第2には(イ)二次形
式最小化問題として計算する方法である。なお、これら
の計算方法を用いるにあたっては、あらかじめ実験等に
より得られたデータに基づいて、両者の計算方法の結果
をどのように合成して用いるかを指標αで指定されてお
り、ステップS10およびS13においてはその指標α
の値に応じて選択が分かれるようになっている。That is, the first is (a) a method of calculating using the law of invariability of the compound ratio, and the second is (a) a method of calculating as a quadratic form minimization problem. In using these calculation methods, based on data obtained in advance through experiments and the like, how to combine and use the results of the two calculation methods is designated by an index α, and steps S10 and S10 are used. In S13, the index α
The selection is divided according to the value of.
【0069】この場合、指標α値の設定は、あらかじめ
実験等により得られたデータに基づいて0≦α≦1の範
囲で決定されるようになっている。そして、指標αの値
は、後述する消失点V1,V2にそれぞれ対応する最適
ベクトルmo1,mo2を求める際の指標となるもの
で、次式で定義されるように、(ア)複比不変の法則に
基づいて求めた単位方向ベクトルm1′,m2′と、
(イ)二次形式最小化問題として求めた単位方向ベクト
ルm1,m2とを合成して最適ベクトルmo1,mo2
を求めるための算出式(2)の合成比率を設定している
ものである。In this case, the setting of the index α value is determined in the range of 0 ≦ α ≦ 1 based on data obtained in advance through experiments and the like. The value of the index α is used as an index when obtaining the optimum vectors mo1 and mo2 respectively corresponding to vanishing points V1 and V2, which will be described later. Unit direction vectors m1 ′ and m2 ′ obtained based on the law,
(A) Combining unit direction vectors m1 and m2 obtained as a quadratic form minimization problem to obtain optimal vectors mo1 and mo2
Is set for the combination ratio of the calculation formula (2).
【0070】そして、指標αの値が、α=1である場合
には単位方向ベクトルm1,m2のみを用い、α=0で
ある場合には単位方向ベクトルm1′,m2′のみを用
いて最適ベクトルmo1,mo2を求めることに相当し
ている。また、指標αの値が、0<α<1である場合に
は、両者を用いて式(2)から最適ベクトルmo1,m
o2を求めるようになっている。When the value of the index α is α = 1, only the unit direction vectors m1 and m2 are used, and when the value of α = 0, only the unit direction vectors m1 ′ and m2 ′ are used. This corresponds to obtaining the vectors mo1 and mo2. When the value of the index α is 0 <α <1, the optimal vectors mo1, m
o2 is to be obtained.
【0071】[0071]
【数2】 (Equation 2)
【0072】(ア)複比不変の法則を用いた計算過程 ここでは、指標αの値が「1」でない場合に対応してお
り、ステップS10にて「YES」と判断してステップ
S11,S12の計算ステップに進んだときに実行され
る。(A) Calculation process using the rule of invariance of compound ratio Here, the case where the value of the index α is not “1” corresponds to “1”, and “YES” is determined in step S10, and steps S11 and S12 are performed. Is executed when the calculation step is advanced.
【0073】複比不変の法則というのは、「直線から直
線への射影変換は、その直線上の相異なる3点がどのよ
うに写像されるかを指定すると、それで一意に決定され
る」という幾何学的な法則であり、これを利用すると、
例えば、図11に示すように、直線L上(マーカ6上の
直線Lm1〜Lm6に対応する)の点A,B,C,Dが
直線L′上(撮影画面上の直線L1〜L6に対応する)
の点A′,B′,C′,D′の各点に写像されるとする
と、これらの各点間で区切られる線分の長さの関係は、
式(3)のようになる。The law of cross ratio invariance is that "projection transformation from a straight line to a straight line is uniquely determined by specifying how three different points on the straight line are mapped". It is a geometric law, and using this,
For example, as shown in FIG. 11, points A, B, C and D on a straight line L (corresponding to the straight lines Lm1 to Lm6 on the marker 6) are on a straight line L '(corresponding to the straight lines L1 to L6 on the photographing screen). Do)
Are mapped to the points A ', B', C ', and D', the relationship between the lengths of the line segments demarcated between these points is as follows:
Equation (3) is obtained.
【0074】[0074]
【数3】 (Equation 3)
【0075】そこで、直線L上の線分ABおよび線分B
Cの長さ寸法を「1」とし、点Dnが点Aから距離nの
位置にあるとすると、式(3)の左辺は式(4)のよう
になり、このとき、直線L′上の線分A′Dn′の長さ
寸法をdnとおくと、式(3)の右辺は式(5)のよう
になる。式(3)の関係から式(4)と式(5)の各右
辺の値は等しいから、式(6)が得られる。Then, the line segments AB and B on the straight line L
Assuming that the length dimension of C is "1" and the point Dn is located at a distance n from the point A, the left side of the equation (3) is as shown in the equation (4). Assuming that the length of the line segment A'Dn 'is dn, the right side of Expression (3) becomes Expression (5). From the relationship of Expression (3), since the values on the right sides of Expression (4) and Expression (5) are equal, Expression (6) is obtained.
【0076】[0076]
【数4】 (Equation 4)
【0077】すると、式(6)を直線L′上の長さ寸法
dnについて解くと、式(7)が得られる。ここで、直
線Lの点Dnが無限遠にあるときを考えると、これに対
応する直線L′上の点Dn′は式(7)中のnが無限大
になるときの長さ寸法dnの値であるから、式(8)が
得られる。つまり、この長さ寸法の値dnが消失点に相
当する位置の距離になるのである。Then, when equation (6) is solved for the length dn on the straight line L ', equation (7) is obtained. Here, considering the case where the point Dn of the straight line L is at infinity, the corresponding point Dn 'on the straight line L' is defined by the length dn of n when the infinity in the equation (7) becomes infinite. Equation (8) is obtained because of the value. That is, the value dn of this length dimension is the distance of the position corresponding to the vanishing point.
【0078】[0078]
【数5】 (Equation 5)
【0079】そこで、本実施例における消失点V1,V
2を求めるために、図12に示すように、撮影画面上の
直線L1〜L6の各交点を特徴点P1〜P9として抽出
し、これらの各特徴点と求めるべき消失点V1,V2の
消失点座標(xv1,yvi),(xv2、yv2)と
の関係を式(8)に示した点A〜Cに対応させると表1
のように表すことができるので、上述の式(8)の関係
を用いて各座標値を求めれば良い。Therefore, the vanishing points V1 and V
12, each intersection of straight lines L1 to L6 on the photographing screen is extracted as feature points P1 to P9 as shown in FIG. 12, and these feature points and vanishing points V1 and V2 to be found are extracted. Table 1 shows that the relationship between the coordinates (xv1, yvi) and (xv2, yv2) corresponds to points A to C shown in Expression (8).
Thus, each coordinate value may be obtained using the relationship of the above equation (8).
【0080】[0080]
【表1】 [Table 1]
【0081】すなわち、式(8)の関係から、各直線L
1〜L6における各特徴点間の距離dv(t)(t=1
〜6)は、式(9)のように表せるから、消失点V1,
V2の各座標値xv1,yv1,xv2,yv2の値は
式(10)〜(13)のように表すことができる。That is, from the relationship of equation (8), each straight line L
1 to L6, the distance dv (t) between the feature points (t = 1
To 6) can be expressed as in equation (9), so that vanishing point V1,
The value of each coordinate value xv1, yv1, xv2, yv2 of V2 can be expressed as in equations (10) to (13).
【0082】[0082]
【数6】 (Equation 6)
【0083】そして、消失点V1,V2の撮影画面上で
の二次元座標は、上述のようにV1(xv1,yv
1),V2(xv2、yv2)としているので、CCD
カメラ5からこの消失点V1,V2に向かうベクトル
は、三次元空間における実際のマーカ6の各直線Lm1
〜Lm3の方向およびLm4〜Lm6の方向を示す単位
ベクトルとなる。The two-dimensional coordinates of the vanishing points V1 and V2 on the photographing screen are represented by V1 (xv1, yv
1), V2 (xv2, yv2), CCD
The vector from the camera 5 to the vanishing points V1 and V2 is the straight line Lm1 of the actual marker 6 in the three-dimensional space.
It is a unit vector indicating the directions of Lm3 and Lm4 to Lm6.
【0084】したがって、図13に示すように、マーカ
6の原点Oから消失点V1に向かう単位方向ベクトルを
m1′(x軸方向)、消失点V2に向かう単位方向ベク
トルをm2′(y軸方向)とすると、それぞれの単位方
向ベクトルm1′およびm2′は、CCDカメラ5から
撮影画面の中心点までの距離をf(焦点距離)とすると
式(14),(15)のように表すことができる。つま
り、ステップS12では、この式(14),(15)に
基づいて単位方向ベクトルm1′,m2′を計算するよ
うになっているのである。Therefore, as shown in FIG. 13, the unit direction vector from the origin O of the marker 6 toward the vanishing point V1 is m1 '(x-axis direction), and the unit direction vector toward the vanishing point V2 is m2' (y-axis direction). ), The unit direction vectors m1 'and m2' can be expressed as in equations (14) and (15), where f (focal length) is the distance from the CCD camera 5 to the center point of the shooting screen. it can. That is, in step S12, the unit direction vectors m1 'and m2' are calculated based on the equations (14) and (15).
【0085】[0085]
【数7】 (Equation 7)
【0086】(イ)二次形式最小化問題としての計算過
程 さて、指標αの値が「0」でない場合には、ステップS
13になると、ここで「YES」と判断してステップS
14〜S16においてこの計算過程が実行されるように
なる。(A) Calculation process as quadratic form minimization problem If the value of index α is not “0”, step S
When it becomes 13, it is determined "YES" here and step S
This calculation process is executed in 14 to S16.
【0087】まず、ステップS14においては、モーメ
ント行列計算部20により、マーカ6の各直線Lm1〜
Lm6に対して、単位方向ベクトルm1,m2を求める
べく、ステップS7にて計算した直線L1〜L6の直線
パラメータ[θi,hi](i=1〜6)に基づいて、
式(16),(17)で定義されるモーメント行列M
1,M2を計算する。First, in step S 14, the moment matrix calculator 20 calculates each straight line Lm 1 through Lm 1 of the marker 6.
In order to obtain the unit direction vectors m1 and m2 for Lm6, based on the straight line parameters [θi, hi] (i = 1 to 6) of the straight lines L1 to L6 calculated in step S7,
Moment matrix M defined by equations (16) and (17)
1, M2 is calculated.
【0088】この場合、三次元空間中の直線Lmiに対
して、CCDカメラ5の視点Pと直線Lmiを含む平面
の単位法線ベクトルを直線LmiのNベクトルniと定
義すると、直線Lmiが撮影画面上において直線Liと
して式(1)のように表現されているから、そのNベク
トルniは式(18)のようになる。ただし、式(1
8)におけるf′はCCDカメラ5の仮の焦点距離を示
すデータである。In this case, if a unit normal vector of a plane including the viewpoint P of the CCD camera 5 and the straight line Lmi is defined as an N vector ni of the straight line Lmi with respect to the straight line Lmi in the three-dimensional space, the straight line Lmi becomes Since the above is expressed as a straight line Li as in equation (1), its N vector ni is as in equation (18). However, equation (1)
F 'in 8) is data indicating the temporary focal length of the CCD camera 5.
【0089】この仮の焦点距離f′の値は、メモリ15
内に記憶されているデータで、本実施例においては、計
算精度の向上を図るために、CCDカメラ5の仕様書の
公称値あるいは既に算出した計算結果を用いるようにし
ている。The value of the provisional focal length f 'is stored in the memory 15
In this embodiment, the nominal value of the specification of the CCD camera 5 or the already calculated result is used in order to improve the calculation accuracy.
【0090】[0090]
【数8】 (Equation 8)
【0091】そして、以下のステップS15,16にお
いては、3本の直線の組(Lm1〜Lm3,Lm4〜L
m6)のそれぞれの組における直線の交点となる消失点
V1,V2のNベクトルm1,m2を、それぞれモーメ
ント行列M1,M2に対する二次形式最小化問題すなわ
ち、 mi={[m,Mim]]を最小にするm} として解き、m1,m2をM1,M2の最小固有値に対
する単位固有ベクトルとして計算するものである。よく
知られているように、二次形式[m,Mm](i=1,
2)を最小にする単位ベクトルm1,m2は、それぞれ
式(16),(17)のモーメント行列の最小固有値に
対する固有ベクトルである。In the following steps S15 and S16, a set of three straight lines (Lm1 to Lm3, Lm4 to Lm
m6), the N-vectors m1 and m2 of vanishing points V1 and V2, which are the intersections of the straight lines in each set, are converted to the quadratic form minimization problem for the moment matrices M1 and M2, that is, mi = {[m, Mim]]. It solves as m} to be minimized, and calculates m1 and m2 as unit eigenvectors for the minimum eigenvalues of M1 and M2. As is well known, the quadratic form [m, Mm] (i = 1,
Unit vectors m1 and m2 that minimize 2) are eigenvectors for the minimum eigenvalues of the moment matrices of equations (16) and (17), respectively.
【0092】すなわち、ステップS15においては、最
小固有値計算部21により、上述の最小固有値を計算
し、続くステップS16においては、単位固有ベクトル
計算部22により上述の単位固有ベクトルm1,m2を
計算する。That is, in step S15, the minimum eigenvalue is calculated by the minimum eigenvalue calculator 21, and in subsequent step S16, the unit eigenvectors m1 and m2 are calculated by the unit eigenvector calculator 22.
【0093】この場合、Nベクトルがmである点P(点
PのNベクトルとは、視点から空間座標系の点Pに向う
単位ベクトルとして定義される)がすべての直線Li上
にあれば、すべてのiに対して、内積(m,ni)=0
となるから、次式(19)を最小にする単位ベクトルを
考えると上述のように二次形式最小化問題に帰着するの
である。そして、以上の結果から、単位固有ベクトルm
1,m2が式(20),(21)のように求まる。な
お、式(20)、(21)中のfはCCDカメラ5の真
の焦点距離である。In this case, if the point P whose N vector is m (the N vector of the point P is defined as a unit vector from the viewpoint to the point P in the space coordinate system) is on all the straight lines Li, Dot product (m, ni) = 0 for all i
Therefore, considering a unit vector that minimizes the following equation (19), it results in the quadratic form minimization problem as described above. Then, from the above results, the unit eigenvector m
1 and m2 are obtained as in equations (20) and (21). Note that f in Expressions (20) and (21) is the true focal length of the CCD camera 5.
【0094】[0094]
【数9】 (Equation 9)
【0095】以上のようにして(ア)の複比不変の法則
に基づく単位方向ベクトルm1′,m2′と(イ)の二
次形式最小化問題として求めた単位固有ベクトルm1,
m2に基づいて、ステップS17において、最適ベクト
ル計算部23により、前述の式(2)にしたがって、指
標αの値に応じて最適ベクトルmo1,mo2を計算
し、これを単位方向ベクトルm1,m2として決定する
ようになる。As described above, the unit direction vectors m1 'and m2' based on the law of invariability of the cross ratio (a) and the unit eigenvectors m1 and m1 obtained as the quadratic form minimization problem (a)
Based on m2, in step S17, the optimum vector calculation unit 23 calculates the optimum vectors mo1 and mo2 according to the value of the index α according to the above-described equation (2), and sets these as unit direction vectors m1 and m2. Will decide.
【0096】(d)位置姿勢計算処理ステップ(S18
〜S20) 次に、ステップS18に進むと、焦点距離計算部25に
より、CCDカメラ5の真の焦点距離fを上述の結果に
基づいて計算するようになる。すなわち、単位方向ベク
トルm1,m2は、互いに直交しているから、その内積
をとると0になるから式(22)が成り立ち、これから
fについて解くと真の焦点距離fの値が式(23)のよ
うに得られる。(D) Position and orientation calculation processing step (S18)
-S20) Next, when the process proceeds to step S18, the focal length calculator 25 calculates the true focal length f of the CCD camera 5 based on the result described above. That is, since the unit direction vectors m1 and m2 are orthogonal to each other, the inner product becomes 0 when the inner product thereof is obtained, and the equation (22) is established. When f is solved from this, the value of the true focal length f is obtained by the equation (23) It is obtained as follows.
【0097】[0097]
【数10】 (Equation 10)
【0098】続いて、ステップS19に進むと、姿勢計
算部26により、図13に示したマーカ6の姿勢ベクト
ル[ms1,ms2,ms3]が計算される。この場
合、姿勢ベクトルms3は、姿勢ベクトルms1とms
2との外積として与えられるから、式(24)のように
なる。そして、姿勢ベクトルms1,ms2は、前述の
最適ベクトルmo1,mo2および真の焦点距離fの値
から式(25)のように計算することができる。Subsequently, when the flow proceeds to step S19, the posture calculating section 26 calculates the posture vector [ms1, ms2, ms3] of the marker 6 shown in FIG. In this case, the attitude vector ms3 is equal to the attitude vectors ms1 and ms.
Since it is given as an outer product of 2, it becomes as shown in equation (24). Then, the posture vectors ms1 and ms2 can be calculated from the above-described optimum vectors mo1 and mo2 and the value of the true focal length f as in Expression (25).
【0099】[0099]
【数11】 [Equation 11]
【0100】次に、ステップS20になると、マーカ原
点位置計算部27により、マーカ6の原点位置Oが計算
される。すなわち、まず、ステップS7にて求められた
6本の直線L1〜L6のパラメータと真の焦点距離fの
値に基づいて、各直線L1〜L6のNベクトルn1〜n
6を次式(26)にしたがって計算し、これらから、各
特徴点Pk(k=1〜9)のNベクトルmPkを式(2
7)にしたがって計算する。なお、点PのNベクトルm
Pは、CDDカメラ5の視点Vから点Pに向かう単位方
向ベクトルとして定義されるものである。Next, at step S20, the marker origin position calculator 27 calculates the origin position O of the marker 6. That is, first, based on the parameters of the six straight lines L1 to L6 obtained in step S7 and the value of the true focal length f, the N vectors n1 to n of the straight lines L1 to L6 are determined.
6 is calculated according to the following equation (26). From these, the N vector mPk of each feature point Pk (k = 1 to 9) is calculated according to the equation (2).
Calculate according to 7). Note that the N vector m of the point P
P is defined as a unit direction vector from the viewpoint V of the CDD camera 5 toward the point P.
【0101】そして、これらから、各点PkについてC
CDカメラ5の視点とマーカ6の原点Oとの間の距離を
もとめてそれらの平均値を計算することによりマーカ原
点位置までの距離Rを式(28)にしたがって計算す
る。Then, from these, C for each point Pk
The distance between the viewpoint of the CD camera 5 and the origin O of the marker 6 is calculated and the average value thereof is calculated to calculate the distance R to the marker origin position according to equation (28).
【0102】[0102]
【数12】 (Equation 12)
【0103】(e)座標変換行列計算処理ステップ(S
21) このようにして、マーカ6の原点位置姿勢が求まると、
ステップS21に進み、座標変換行列計算部14にて、
上述の結果に基づいてCCDカメラ5の空間座標系に相
対的な位置と向きを示す座標変換行列を、周知の計算方
法により回転変換行列および平行移動変換行列とに分け
た位置パラメータとして計算するようになる。(E) Coordinate transformation matrix calculation processing step (S
21) In this way, when the origin position and orientation of the marker 6 are obtained,
Proceeding to step S21, the coordinate transformation matrix calculation unit 14
Based on the above result, a coordinate conversion matrix indicating a position and an orientation relative to the spatial coordinate system of the CCD camera 5 is calculated as a position parameter divided into a rotation conversion matrix and a translation conversion matrix by a known calculation method. become.
【0104】このような本実施例によれば、マーカ6と
して、2個の簡単な正方形部7,8を白黒の二値化され
た情報で表示した構成としてこれをCCDカメラ5によ
り斜方向から撮影して三次元の位置姿勢を認識できるよ
うにしたので、マーカ6の線分を正方形部7,8のエッ
ジデータとして抽出することができるようになり、線幅
をもたない線分として精度良く検出することができ、し
かも、CCDカメラ5をマーカ6の正面まで移動させな
くとも検出することができるので、CCDカメラ5の撮
影位置の自由度を大きくしてロボットハンド1の移動範
囲の制約を低減すると共に、画像処理を簡単化しなが
ら、高速,高精度且つロバストにCCDカメラ5の位置
姿勢の認識を行うことができる。According to the present embodiment, as the marker 6, two simple square portions 7, 8 are displayed in black and white binarized information. Since the three-dimensional position and orientation can be recognized by photographing, the line segment of the marker 6 can be extracted as edge data of the square portions 7 and 8, and the line segment having no line width can be accurately extracted. Since the detection can be performed well and the CCD camera 5 can be detected without moving the CCD camera 5 to the front of the marker 6, the degree of freedom of the photographing position of the CCD camera 5 is increased and the movement range of the robot hand 1 is restricted. , And the position and orientation of the CCD camera 5 can be recognized at high speed, with high accuracy, and robustly while simplifying image processing.
【0105】また、本実施例によれば、直線あてはめ処
理部11において直線の検出を行う場合に、ステップS
6において、複数の直線を検出してそれらの平均をとる
ことにより1本の直線を決定するようにしたので、CC
Dカメラ5によるマーカ6の撮影画面が焦点ずれでグレ
ーゾーンGLが生じている場合においても、つねに、正
確な直線の検出処理を行うことができる。According to the present embodiment, when the straight-line fitting processing unit 11 detects a straight line, the process proceeds to step S
In FIG. 6, a single straight line is determined by detecting a plurality of straight lines and averaging them.
Even when the gray zone GL occurs due to the defocus of the imaging screen of the marker 6 by the D camera 5, an accurate straight line detection process can always be performed.
【0106】そして、本実施例においては、単位方向ベ
クトルの計算において、指標αの設定により、複比不変
の法則を利用した方法によって消失点V1,V2を求め
ると共に、二次形式最小化問題として消失点V1,V2
のNベクトルを求めることにより単位方向ベクトルの最
適ベクトルを計算するようにしているので、必要に応じ
て適切な単位方向ベクトルを計算することができる。In this embodiment, in the calculation of the unit direction vector, the vanishing points V1 and V2 are obtained by setting the index α by a method utilizing the law of invariance of the cross ratio, and the quadratic form minimization problem Vanishing point V1, V2
Since the optimum vector of the unit direction vector is calculated by obtaining the N vector of the above, an appropriate unit direction vector can be calculated as needed.
【0107】図15ないし図19は、本発明の第2ない
し第6の実施例を示すもので、いずれも、第1の実施例
におけるマーカ6の代わるマーカ29〜33を示してた
ものである。FIGS. 15 to 19 show the second to sixth embodiments of the present invention, and all show markers 29 to 33 instead of the markers 6 in the first embodiment. .
【0108】すなわち、図15に示す第2の実施例にお
いては、マーカ29を、第1の実施例で用いたマーカ6
の背景部分に対する正方形部7,8の白黒を反転させた
状態の正方形部34,35を表示して形成したもので、
これによっても第1の実施例と同様の作用効果を得るこ
とができるものである。That is, in the second embodiment shown in FIG. 15, the marker 29 is replaced with the marker 6 used in the first embodiment.
Are formed by displaying square portions 34 and 35 in a state where the black and white of the square portions 7 and 8 with respect to the background portion are inverted.
With this, the same operation and effect as those of the first embodiment can be obtained.
【0109】また、図16に示す第3の実施例において
は、マーカ30を、正方形部7、8に代えて2個の長方
形36,37を直角をなすようにハ字状に配置した状態
に形成したもので、図の方向からCCDカメラ5により
撮影するようにティーチングして位置姿勢の認識を行う
ようにするものである。In the third embodiment shown in FIG. 16, the marker 30 is replaced by squares 7 and 8 in which two rectangles 36 and 37 are arranged in a C shape so as to form a right angle. The teaching is performed so that the CCD camera 5 captures an image from the direction shown in the figure, and the position and orientation are recognized.
【0110】そして、この場合には、第1の実施例と同
様の効果が得られると共に、CCDカメラ5によりマー
カ29を撮影するときの傾斜角度が大きい場合でも、遠
方側に位置する長方形部36,37の辺a3,b3を撮
影画面上で比較的長い寸法のデータとして入力すること
ができるので、直線の検出精度を向上させることができ
るものである。In this case, the same effect as that of the first embodiment can be obtained, and even if the inclination angle when the marker 29 is photographed by the CCD camera 5 is large, the rectangular portion 36 located on the far side is obtained. , 37 can be input as data having relatively long dimensions on the photographing screen, so that the accuracy of detecting straight lines can be improved.
【0111】図17に示す第4の実施例においては、マ
ーカ31を、大きい正方形部38の内部に小さい正方形
部39を異なる濃淡情報で示すと共に、それらの正方形
部38および39の対角線が重なる位置に配置したもの
であり、このようなマーカ31を用いても第1の実施例
と同様の作用効果を得ることができる。In the fourth embodiment shown in FIG. 17, a marker 31 is represented by a small square 39 with different shading information inside a large square 38, and a position where the diagonal lines of the squares 38 and 39 overlap. The same operation and effect as in the first embodiment can be obtained by using such a marker 31.
【0112】図18に示す第5の実施例においては、マ
ーカ32を、マーカ6で使用した正方形部7,8に相当
する正方形部40,41を上下2段に配置して表示した
ものであり、図19に示す第6の実施例においては、マ
ーカ33を、3個の正方形部42〜44と直角三角形部
45とを互いに等間隔だけ離間した状態で配置して表示
したもので、これらのマーカ32,33によっても第1
の実施例と同様の作用効果を得ることができるものであ
る。In the fifth embodiment shown in FIG. 18, the marker 32 is displayed by arranging square portions 40 and 41 corresponding to the square portions 7 and 8 used in the marker 6 in two upper and lower stages. In the sixth embodiment shown in FIG. 19, the markers 33 are displayed by arranging three square portions 42 to 44 and a right-angled triangular portion 45 at equal intervals from each other. The first by the markers 32 and 33
The same operation and effect as those of the embodiment can be obtained.
【0113】なお、上記各実施例においては、マーカ
6,29〜33を白黒の濃淡情報により表示する構成の
場合について説明したが、これに限らず、色情報により
表示する構成とすることもできるものであり、この場合
には、カラー撮影可能なCCDカメラを用いると共に、
前処理計算部にてカラー信号を二値化するように構成す
れば良い。In each of the above-described embodiments, the case where the markers 6, 29 to 33 are displayed using black and white shading information has been described. However, the present invention is not limited to this, and it is also possible to display the markers 6, 29 to 33 using color information. In this case, in addition to using a CCD camera capable of taking color images,
The preprocessing calculation unit may be configured to binarize the color signal.
【0114】また、上記各実施例においては、本発明を
自律移動ロボットの三次元位置認識装置に適用した場合
について説明したが、ロボット自身は固定された状態と
され、作業対象がロボットの近傍に移動してくる構成の
三次元位置認識装置等に適用することもできるものであ
る。Further, in each of the above embodiments, the case where the present invention is applied to the three-dimensional position recognition device of the autonomous mobile robot has been described. However, the robot itself is fixed, and the work target is located near the robot. The present invention can also be applied to a moving three-dimensional position recognition device or the like.
【0115】(付録)最小二乗推定法による直線の検出 前述した直線あてはめ処理ステップにおける、最小二乗
推定法による直線の検出のための導出過程について説明
する。前述のように、直線の式をヘッセの標準形で示す
と、式(a)のように表され、N個の抽出サンプル点P
i(xi,yi)との距離は式(b)のようになる。し
たがって、式(c)のように定義した関数の値を最小に
するときのθおよびhの値を求めることにより式(a)
の直線の方程式が得られる。(Appendix) Detection of Straight Line by Least Square Estimation Method The derivation process for the detection of a straight line by the least square estimation method in the above-described straight line fitting processing step will be described. As described above, when the equation of a straight line is expressed in Hessian standard form, it is expressed as equation (a), and N extracted sample points P
The distance from i (xi, yi) is as shown in equation (b). Therefore, by obtaining the values of θ and h when minimizing the value of the function defined as in equation (c), equation (a) is obtained.
Is obtained.
【0116】[0116]
【数13】 (Equation 13)
【0117】ここで、以下の導出過程で用いる種々の記
号を次式(d)ないし(f)のように定義する。Here, various symbols used in the following derivation process are defined as in the following equations (d) to (f).
【0118】[0118]
【数14】 [Equation 14]
【0119】さて、式(c)をhで微分して「0」とお
くと、hは式(g)のように得られる。式(g)の結果
を式(c)に代入し、θで微分して「0」とおくと、θ
は式(h)のように得られる。By differentiating equation (c) with h and setting it to "0", h is obtained as in equation (g). Substituting the result of equation (g) into equation (c), differentiating by θ and setting it to “0” gives θ
Is obtained as in equation (h).
【0120】[0120]
【数15】 (Equation 15)
【0121】この場合、hおよびθの選択条件は、それ
ぞれ式(i)および式(j)とすれば良い。In this case, the conditions for selecting h and θ may be expressed by equations (i) and (j), respectively.
【0122】[0122]
【数16】 (Equation 16)
【0123】[0123]
【発明の効果】以上説明したように、本発明の視覚に基
く三次元位置および姿勢の認識方法ならびに視覚に基く
三次元位置および姿勢の認識装置によれば、以下のよう
な優れた効果を奏する。As described above, the method for recognizing three-dimensional position and orientation based on vision and the apparatus for recognizing three-dimensional position and orientation based on vision according to the present invention have the following excellent effects. .
【0124】すなわち、請求項1記載の視覚に基く三次
元位置および姿勢の認識方法によれば、平面上に所定寸
法を有し互いに直交する2本の線分とそれら2本の線分
にそれぞれ平行な線分とを含んで囲まれた図形を背景部
分に対して濃淡情報あるいは色情報の変化により表示し
たマーカを、三次元空間の斜方向に位置する撮像手段に
より撮影し、画像処理ステップにおいて輪郭点位置を抽
出し、直線あてはめ処理ステップにおいてマーカの線分
に対応する直線のパラメータが最小二乗推定法により求
め、単位方向ベクトル計算処理ステップにおいて単位方
向ベクトルを計算し、位置姿勢計算処理ステップにおい
て、マーカの位置姿勢を示すパラメータを計算して撮像
手段の基準座標系から見たマーカの位置姿勢を表す座標
変換行列を計算するようにしたので、マーカに表示され
た直交する線分を精度良く検出することができると共
に、単純な図形に対する画像処理を行うことから計算処
理が簡単になり、高速,高精度且つロバストに三次元位
置および姿勢が認識できるという優れた効果を奏する。That is, according to the method for recognizing a three-dimensional position and orientation based on vision, two line segments having a predetermined dimension on a plane and orthogonal to each other, and the two line segments are respectively provided. In the image processing step, a marker displaying a figure surrounded by parallel line segments and displaying a background portion by a change in density information or color information is photographed by imaging means positioned in an oblique direction of the three-dimensional space. The contour point position is extracted, and in a straight line fitting processing step, a parameter of a straight line corresponding to the line segment of the marker is obtained by a least squares estimation method, a unit direction vector is calculated in a unit direction vector calculation processing step, and a position and orientation calculation step is performed. Calculating a parameter indicating the position and orientation of the marker and calculating a coordinate transformation matrix representing the position and orientation of the marker as viewed from the reference coordinate system of the imaging means. As a result, the orthogonal line segment displayed on the marker can be accurately detected, and the image processing is performed on a simple figure, so that the calculation processing is simplified, and the three-dimensional processing is performed at high speed, with high accuracy, and robustly. It has an excellent effect that the position and the posture can be recognized.
【0125】請求項2記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、マーカを、2個以上の長方
形状あるいは正方形状をなす簡単な図形で表示している
ので、マーカの位置姿勢認識の計算処理を簡単化するこ
とができると共に、例えば、図形を横方向に並べるよう
に配置することにより、撮像手段の撮影画面に対してマ
ーカの傾き角度が大きくなる場合でも、線分の奥行き方
向の距離変化を少なくして焦点ずれによる誤差を低減し
て精度良く検出することができるようになるという優れ
た効果を奏する。According to the three-dimensional position and orientation recognition method based on vision, the markers are displayed as two or more simple figures having a rectangular or square shape. In addition to simplifying the calculation process of the posture recognition, for example, by arranging the figures so as to be arranged in the horizontal direction, even if the inclination angle of the marker with respect to the photographing screen of the imaging means becomes large, the line segment There is an excellent effect that it is possible to reduce an error due to defocus by reducing a change in distance in the depth direction and to perform accurate detection.
【0126】請求項3記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、直線あてはめ処理ステップ
において、画像処理ステップで抽出された前記輪郭点位
置のデータに基づいて、マーカのひとつの線分に対応し
て直線を複数本検出し、それらの直線の平均化処理を行
うことにより1本の直線を求めてその直線パラメータ決
定するので、例えば、撮像手段により撮影されたマーカ
が、焦点ずれにより輪郭線が直交する線分として検出で
きない場合でも、より正確な直線として検出することが
できるという優れた効果を奏する。According to the method for recognizing a three-dimensional position and orientation based on vision according to the third aspect, in the straight line fitting processing step, one of the markers is determined based on the data of the contour point position extracted in the image processing step. A plurality of straight lines are detected corresponding to the line segments, and one straight line is obtained by averaging the straight lines to determine the straight line parameters. Even when the outline cannot be detected as an orthogonal line segment due to the displacement, an excellent effect that an accurate straight line can be detected is obtained.
【0127】請求項4記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、単位方向ベクトル計算処理
ステップにおいて、複比不変の法則を利用して消失点の
候補値を3つ以上計算し、それらの候補値の2次元座標
データに基づいて単位方向ベクトルを計算するので、例
えば、平行な線分に対応して検出された直線に誤差が生
じていてそれらの交点から得られる消失点が複数存在す
る場合でも、より正確な単位方向ベクトルを検出するこ
とができるという優れた効果を奏する。According to the method for recognizing a three-dimensional position and orientation based on vision, in the unit direction vector calculation processing step, three or more vanishing point candidate values are calculated using the law of invariance of cross ratio. Then, since the unit direction vector is calculated based on the two-dimensional coordinate data of those candidate values, for example, a vanishing point obtained from the intersection point of the straight line detected corresponding to the parallel line segment has an error. In the case where there are a plurality of, there is an excellent effect that a more accurate unit direction vector can be detected.
【0128】請求項5記載の視覚に基く三次元位置およ
び姿勢の認識方法によれば、単位方向ベクトル計算処理
ステップにおいて、マーカの3本以上の平行な線分にそ
れぞれ対応して得られた直線について、それらの交点に
対する単位方向ベクトルをモーメント行列に対する二次
形式最小化問題として解くことにより計算するので、上
述と同様にして検出された直線に誤差が生じている場合
でも、より正確な単位方向ベクトルを検出することがで
きるようになるという優れた効果を奏する。According to the three-dimensional position and orientation recognition method based on vision, in the unit direction vector calculation processing step, straight lines obtained respectively corresponding to three or more parallel line segments of the marker. Is calculated by solving the unit direction vector for those intersections as a quadratic form minimization problem for the moment matrix. Therefore, even if an error occurs in the straight line detected in the same manner as described above, a more accurate unit direction can be obtained. This has an excellent effect that a vector can be detected.
【0129】また、請求項6記載の視覚に基く三次元位
置および姿勢の認識装置によれば、前記請求項1記載の
マーカを利用すると共に各処理ステップを実行する各手
段を設けて構成したので、マーカに表示された直交する
線分を精度良く検出することができると共に、単純な図
形に対する画像処理を行うことから計算処理が簡単にな
り、高速,高精度且つロバストに三次元位置および姿勢
が認識できるという優れた効果を奏する。According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to claim 6, since the marker according to claim 1 is used and each means for executing each processing step is provided. In addition to being able to accurately detect orthogonal line segments displayed on the marker and performing image processing on a simple figure, the calculation processing is simplified, and the three-dimensional position and orientation can be quickly, accurately, and robustly. It has an excellent effect that it can be recognized.
【0130】請求項7記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、マーカを、2個以上の長方
形状あるいは正方形状をなす簡単な図形で表示している
ので、マーカの位置姿勢認識の計算処理を簡単化するこ
とができると共に、例えば、図形を横方向に並べるよう
に配置することにより、撮像手段の撮影画面に対してマ
ーカの傾き角度が大きくなる場合でも、線分の奥行き方
向の距離変化を少なくして焦点ずれによる誤差を低減し
て精度良く検出することができるようになるという優れ
た効果を奏する。According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to the seventh aspect, the markers are displayed by two or more simple figures having a rectangular or square shape. In addition to simplifying the calculation process of the posture recognition, for example, by arranging the figures so as to be arranged in the horizontal direction, even if the inclination angle of the marker with respect to the photographing screen of the imaging means becomes large, the line segment There is an excellent effect that it is possible to reduce an error due to defocus by reducing a change in distance in the depth direction and to perform accurate detection.
【0131】請求項8記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、直線あてはめ処理手段によ
り、画像処理ステップで抽出された前記輪郭点位置のデ
ータに基づいて、マーカのひとつの線分に対応して直線
を複数本検出し、それらの直線の平均化処理を行うこと
により1本の直線を求めてその直線パラメータ決定する
ので、例えば、撮像手段により撮影されたマーカが、焦
点ずれにより輪郭線が直交する線分として検出できない
場合でも、それらの輪郭点位置から得られる複数本の直
線から平均化することにより、より正確な直線として検
出することができるという優れた効果を奏する。According to the three-dimensional position and orientation recognition apparatus based on vision, one of the markers is determined by the straight line fitting processing means based on the data of the contour point position extracted in the image processing step. A plurality of straight lines are detected corresponding to the line segments, and one straight line is obtained by averaging the straight lines to determine the straight line parameters. Even when the contour cannot be detected as an orthogonal line segment due to the displacement, an excellent effect is obtained in that it can be detected as a more accurate straight line by averaging a plurality of straight lines obtained from the contour point positions. .
【0132】請求項9記載の視覚に基く三次元位置およ
び姿勢の認識装置によれば、単位方向ベクトル計算処理
手段により、複比不変の法則を利用して消失点の候補値
を3つ以上計算し、それらの候補値の2次元座標データ
に基づいて単位方向ベクトルを計算するので、例えば、
平行な線分に対応して検出された直線に誤差が生じてい
てそれらの交点から得られる消失点が複数存在する場合
でも、より正確な単位方向ベクトルを検出することがで
きるという優れた効果を奏する。According to the apparatus for recognizing three-dimensional position and orientation based on vision according to the ninth aspect, the unit direction vector calculation processing means calculates three or more vanishing point candidate values using the law of invariance of cross ratio. Then, since the unit direction vector is calculated based on the two-dimensional coordinate data of those candidate values, for example,
Even if there is an error in the straight line detected corresponding to the parallel line segment and there are a plurality of vanishing points obtained from the intersections, an excellent effect that a more accurate unit direction vector can be detected can be obtained. Play.
【0133】請求項10記載の視覚に基く三次元位置お
よび姿勢の認識装置によれば、単位方向ベクトル計算処
理手段により、マーカの3本以上の平行な線分にそれぞ
れ対応して得られた直線について、それらの交点に対す
る単位方向ベクトルをモーメント行列に対する二次形式
最小化問題として解くことにより計算するので、上述と
同様にして検出された直線に誤差が生じている場合で
も、より正確な単位方向ベクトルを検出することができ
るという優れた効果を奏する。According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to the tenth aspect, straight lines obtained by the unit direction vector calculation processing means respectively corresponding to three or more parallel line segments of the marker. Is calculated by solving the unit direction vector for those intersections as a quadratic form minimization problem for the moment matrix. Therefore, even if an error occurs in the straight line detected in the same manner as described above, a more accurate unit direction can be obtained. This has an excellent effect that a vector can be detected.
【図面の簡単な説明】[Brief description of the drawings]
【図1】本発明の第1の実施例を示す全体のブロック構
成図FIG. 1 is an overall block diagram showing a first embodiment of the present invention.
【図2】ロボットハンドとマーカとの関係を示す図FIG. 2 is a diagram showing a relationship between a robot hand and a marker.
【図3】マーカの平面図FIG. 3 is a plan view of a marker.
【図4】認識プログラムの概略的なフローチャートFIG. 4 is a schematic flowchart of a recognition program.
【図5】CCDカメラによるマーカの撮影位置と姿勢FIG. 5 is a photographing position and posture of a marker by a CCD camera.
【図6】マーカの各線分と検出すべき直線とを示す説明
図FIG. 6 is an explanatory diagram showing each line segment of a marker and a straight line to be detected;
【図7】撮影画面上のサンプル抽出点の走査方向を示す
説明図FIG. 7 is an explanatory diagram showing a scanning direction of a sample extraction point on a shooting screen;
【図8】マーカの正方形部と検出すべき直線との関係を
示す説明図FIG. 8 is an explanatory diagram showing a relationship between a square portion of a marker and a straight line to be detected.
【図9】フォーカスずれを生じているときの補償を行う
場合の説明図FIG. 9 is an explanatory diagram of a case where compensation is performed when a focus shift has occurred;
【図10】3本以上の直線に対する消失点が複数発生す
る場合の説明図FIG. 10 is an explanatory diagram when a plurality of vanishing points occur for three or more straight lines.
【図11】複比不変の法則の説明図FIG. 11 is an explanatory diagram of the law of invariance of cross ratio.
【図12】撮影画面上の特徴点と直線との関係の説明図FIG. 12 is an explanatory diagram of a relationship between a feature point on a shooting screen and a straight line.
【図13】マーカの姿勢ベクトルと座標系の説明図FIG. 13 is an explanatory diagram of a marker posture vector and a coordinate system.
【図14】最適な消失点方向ベクトルの説明図FIG. 14 is an explanatory diagram of an optimal vanishing point direction vector.
【図15】本発明の第2の実施例を示す図3相当図FIG. 15 is a view corresponding to FIG. 3, showing a second embodiment of the present invention.
【図16】本発明の第3の実施例を示す図3相当図FIG. 16 is a view corresponding to FIG. 3, showing a third embodiment of the present invention.
【図17】本発明の第4の実施例を示す図3相当図FIG. 17 is a view corresponding to FIG. 3, showing a fourth embodiment of the present invention;
【図18】本発明の第5の実施例を示す図3相当図FIG. 18 is a view corresponding to FIG. 3, showing a fifth embodiment of the present invention;
【図19】本発明の第6の実施例を示す図3相当図FIG. 19 is a view corresponding to FIG. 3, showing a sixth embodiment of the present invention;
1はロボットハンド、2はアーム部、3はヘッド部、4
はマニピュレータ、5はCCDカメラ(撮像手段)、6
はマーカ、7,8は正方形部、9は三次元位置姿勢認識
装置、10は画像処理部(画像処理手段)、11は直線
あてはめ部(直線あてはめ手段)、12は消失点計算部
(単位方向ベクトル計算手段)、13は位置姿勢計算部
(位置姿勢計算手段)、14は座標変換行列計算部(座
標変換行列計算手段)、15はメモリ(記憶手段)、1
6は前処理計算部、17はサンプル点抽出部、18は最
小二乗推定部、19は直線パラメータ出力部、20はモ
ーメント行列計算部、21は最小固有値計算部、22は
単位固有ベクトル計算部、23は最適ベクトル計算部、
24は消失点座標推定部、25は焦点距離計算部、26
は位置姿勢計算部、27はマーカ原点計算部、28はデ
ータ出力部、29〜33はマーカである。1 is a robot hand, 2 is an arm, 3 is a head, 4
Is a manipulator, 5 is a CCD camera (imaging means), 6
Is a marker, 7 and 8 are square parts, 9 is a three-dimensional position and orientation recognition device, 10 is an image processing unit (image processing means), 11 is a straight line fitting unit (straight line fitting means), and 12 is a vanishing point calculation unit (unit direction) Vector calculation means), 13 is a position and orientation calculation unit (position and orientation calculation means), 14 is a coordinate transformation matrix calculation unit (coordinate transformation matrix calculation means), 15 is a memory (storage means), 1
6 is a preprocessing calculation unit, 17 is a sample point extraction unit, 18 is a least squares estimation unit, 19 is a linear parameter output unit, 20 is a moment matrix calculation unit, 21 is a minimum eigenvalue calculation unit, 22 is a unit eigenvector calculation unit, 23 Is the optimal vector calculator,
24 is a vanishing point coordinate estimating unit, 25 is a focal length calculating unit, 26
Is a position and orientation calculation unit, 27 is a marker origin calculation unit, 28 is a data output unit, and 29 to 33 are markers.
───────────────────────────────────────────────────── フロントページの続き (56)参考文献 特開 平6−243236(JP,A) 特開 平5−312521(JP,A) 特開 平5−126527(JP,A) 特開 平2−38804(JP,A) 特開 平1−242903(JP,A) 特開 昭63−133002(JP,A) 特開 平1−133183(JP,A) 特開 平5−2413(JP,A) 国際公開93/15376(WO,A1) (58)調査した分野(Int.Cl.6,DB名) G01B 11/00 - 11/30 102 B25J 3/00 - 3/04 B25J 9/10 - 9/22 B25J 13/00 - 13/08 B25J 19/02 - 19/06 G05B 19/18 - 19/46 G05D 3/00 - 3/20 G06T 1/00 - 9/20 ──────────────────────────────────────────────────続 き Continuation of the front page (56) References JP-A-6-243236 (JP, A) JP-A-5-321521 (JP, A) JP-A-5-126527 (JP, A) JP-A-2- 38804 (JP, A) JP-A-1-242903 (JP, A) JP-A-63-133002 (JP, A) JP-A-1-133183 (JP, A) JP-A-5-2413 (JP, A) WO 93/15376 (WO, A1) (58) Fields investigated (Int. Cl. 6 , DB name) G01B 11/00-11/30 102 B25J 3/00-3/04 B25J 9/10-9 / 22 B25J 13/00-13/08 B25J 19/02-19/06 G05B 19/18-19/46 G05D 3/00-3/20 G06T 1/00-9/20
Claims (10)
分とそれら2本の線分にそれぞれ平行な線分とを含んで
囲まれた図形を背景部分に対して濃淡情報あるいは色情
報の変化により平面上に表示されたマーカを備え、この
マーカを斜方向から撮像手段により撮影したときの画像
データに基づいて、以下(a)〜(e)の処理ステップ
を実行することを特徴とした視覚に基く三次元位置およ
び姿勢の認識方法。 (a)前記撮像手段からの画像データに基づいて前記マ
ーカの画像領域を選択すると共に、その選択された画像
領域内の画像データの値の変化点を検出して輪郭点位置
を抽出する画像処理ステップ、 (b)この画像処理ステップにて抽出された前記輪郭点
位置のデータに基づいて、撮影画面上における前記マー
カの直交する線分およびそれらに平行な線分をそれぞれ
含む直線のパラメータを最小二乗推定法により計算する
直線あてはめ処理ステップ、 (c)前記マーカに表示された平行な線分に対応して前
記直線あてはめ処理ステップにて計算された直線の交点
である消失点のデータに基づいて前記マーカの単位方向
ベクトルを計算する単位方向ベクトル計算処理ステッ
プ、 (d)前記マーカの直交する線分の交点を原点としてそ
れら各線分の方向およびマーカ面に垂直でその原点を通
る法線方向を基準方向としたマーカ座標系を設定し、前
記単位方向ベクトルに基づいて前記マーカの位置姿勢を
示すパラメータを計算する位置姿勢計算処理ステップ、 (e)この位置姿勢計算処理ステップにおける前記撮像
手段の位置姿勢を示すパラメータ計算結果に基づいて、
前記撮像手段の基準座標系から見た前記マーカの位置姿
勢を表す座標変換行列を計算する座標変換行列計算処理
ステップ。1. A figure surrounded by two line segments having a predetermined dimension and orthogonal to each other and a line segment parallel to each of the two line segments is represented by shading information or color information with respect to a background portion. Characterized by performing the following processing steps (a) to (e) based on image data obtained when the marker is photographed from an oblique direction by an image pickup means. 3D position and posture recognition method based on visual perception. (A) Image processing for selecting an image area of the marker based on image data from the imaging means, detecting a change point of a value of image data in the selected image area, and extracting a contour point position And (b) minimizing parameters of a straight line including a line segment orthogonal to the marker and a line segment parallel to the marker on the photographing screen based on the data of the contour point position extracted in the image processing step. A straight line fitting processing step calculated by a square estimation method; (c) based on vanishing point data which is an intersection of the straight lines calculated in the straight line fitting processing step corresponding to the parallel line segments displayed on the markers. A unit direction vector calculation processing step of calculating a unit direction vector of the marker; Position and orientation calculation processing step of setting a marker coordinate system with a direction normal to the marker plane and a normal direction passing through the origin as a reference direction, and calculating a parameter indicating the position and orientation of the marker based on the unit direction vector (E) based on a parameter calculation result indicating the position and orientation of the imaging means in the position and orientation calculation processing step,
A coordinate conversion matrix calculation processing step of calculating a coordinate conversion matrix representing the position and orientation of the marker as viewed from a reference coordinate system of the imaging means.
報あるいは色情報の変化により表示された少なくとも2
個の長方形状あるいは正方形状をなす図形から構成さ
れ、それらの図形は互いに一つの頂点で接すると共にそ
の頂点を含む隣接する辺同士が直角をなすように配置さ
れていることを特徴とする請求項1記載の視覚に基く三
次元位置および姿勢の認識方法。2. The method according to claim 1, wherein the marker includes at least two markers that are displayed based on a change in density information or color information with respect to a background portion.
A plurality of rectangular or square figures, which are arranged so that they contact at one vertex and adjacent sides including the vertex form a right angle. 3. The method for recognizing a three-dimensional position and posture based on vision according to 1.
は、前記画像処理ステップにて抽出された前記輪郭点位
置のデータに基づいて、前記マーカのひとつの線分に対
応して直線を複数本検出し、それらの直線の平均化処理
を行うことにより1本の直線を求めてその直線パラメー
タ決定することを特徴とする請求項1または請求項2記
載の視覚に基く三次元位置および姿勢の認識方法。3. In the straight line fitting processing step, a plurality of straight lines corresponding to one line segment of the marker are detected based on the data of the contour point position extracted in the image processing step, 3. A method for recognizing a three-dimensional position and orientation based on vision according to claim 1, wherein one straight line is obtained by averaging the straight lines and the straight line parameters are determined.
においては、複比不変の法則を利用して前記消失点の候
補値を3つ以上計算し、それらの候補値の2次元座標デ
ータに基づいて前記単位方向ベクトルを計算することを
特徴とする請求項2または3記載の視覚に基く三次元位
置および姿勢の認識方法。4. In the unit direction vector calculation processing step, three or more candidate values of the vanishing point are calculated using a law of invariance of cross ratio, and based on two-dimensional coordinate data of the candidate values. The method of recognizing a three-dimensional position and orientation based on vision according to claim 2 or 3, wherein a unit direction vector is calculated.
においては、前記マーカの3本以上の平行な線分にそれ
ぞれ対応して得られた直線について、それらの交点に対
する単位方向ベクトルをモーメント行列に対する二次形
式最小化問題として解くことにより前記単位方向ベクト
ルを計算することを特徴とする請求項2、3または4記
載の視覚に基く三次元位置および姿勢の認識方法。5. In the unit direction vector calculation processing step, for a straight line obtained corresponding to each of three or more parallel line segments of the marker, a unit direction vector with respect to an intersection point between the straight lines is quadratic with respect to a moment matrix. 5. The method according to claim 2, wherein the unit direction vector is calculated by solving the unit direction vector as a form minimization problem.
な線分からなる2組の平行線群が互いに直交するように
配置された状態を背景部分に対して濃淡情報あるいは色
情報の変化により平面上に表示されたマーカと、 撮影画像の画像データを出力する撮像手段と、 前記マーカの形状寸法を示すマーカデータが記憶された
記憶手段と、 前記撮像手段からの画像データに基づいて前記マーカの
画像領域を選択すると共に、その選択された画像領域内
の画像データの値の変化点を検出して輪郭点位置を抽出
する画像処理手段と、 この画像処理手段により抽出された前記輪郭点位置のデ
ータに基づいて前記マーカの直交する平行線群のそれぞ
れの線分を含む直線のパラメータを最小二乗推定法によ
り求めるようにした直線あてはめ処理手段と、 前記2組の平行線群の各線分を含む直線の前記撮影画面
上における交点である消失点のデータに基づいて単位方
向ベクトルを計算する単位方向ベクトル計算処理手段
と、 前記直線あてはめ処理手段により求められた2本の直交
する直線とその交点を通る法線とを座標系の軸としたマ
ーカ座標系を設定し、このマーカ座標系と前記単位方向
ベクトルに基づいて前記マーカの位置姿勢を計算する位
置姿勢計算処理手段と、 この位置姿勢計算処理手段による前記撮像手段の位置姿
勢を示すパラメータ計算結果に基づいて、前記撮像手段
の基準座標系から見た前記マーカの位置姿勢を表す座標
変換行列を計算する座標変換行列計算手段とを備えたこ
とを特徴とする視覚に基く三次元位置および姿勢の認識
装置。6. A state in which two parallel line groups each having at least two parallel line segments having a predetermined dimension are arranged so as to be orthogonal to each other on a plane due to a change in density information or color information with respect to a background portion. An imager that outputs image data of a captured image; a storage device that stores marker data indicating the shape and dimensions of the marker; an image of the marker based on image data from the imager Image processing means for selecting a region, detecting a change point of the value of the image data in the selected image region, and extracting a contour point position; data of the contour point position extracted by the image processing means A straight line fitting processing means for obtaining a parameter of a straight line including each line segment of the group of orthogonal parallel lines of the marker based on the least squares estimation method, A unit direction vector calculation processing unit that calculates a unit direction vector based on data of a vanishing point that is an intersection point on the shooting screen of a straight line including each line segment of the set of parallel line groups, A position and orientation for calculating a position and orientation of the marker based on the marker coordinate system and the unit direction vector, by setting a marker coordinate system using two orthogonal straight lines and a normal passing through the intersection point as axes of the coordinate system. Calculating a coordinate conversion matrix representing the position and orientation of the marker as viewed from a reference coordinate system of the imaging unit based on a parameter calculation result indicating the position and orientation of the imaging unit by the position and orientation calculation processing unit. An apparatus for recognizing a three-dimensional position and orientation based on vision, comprising: a coordinate transformation matrix calculation unit.
報あるいは色情報の変化により表示された少なくとも2
個の長方形状あるいは正方形状をなす図形から構成さ
れ、それらの図形は互いに一つの頂点で接すると共にそ
の頂点を含む隣接する辺同士が直角をなすように配置さ
れていることを特徴とする請求項6記載の視覚に基く三
次元位置および姿勢の認識装置。7. The image processing apparatus according to claim 1, wherein the marker is at least two markers displayed based on a change in density information or color information with respect to a background portion.
A plurality of rectangular or square figures, which are arranged so that they contact at one vertex and adjacent sides including the vertex form a right angle. 6. A three-dimensional position and orientation recognition apparatus based on vision according to 6.
処理手段により抽出された前記輪郭点位置のデータに基
づいて、前記マーカのひとつの線分に対応して直線を複
数本検出し、それらの直線の平均化処理を行うことによ
り1本の直線を求めてその直線パラメータ決定するよう
に構成されていることを特徴とする請求項6または請求
項7記載の視覚に基く三次元位置および姿勢の認識装
置。8. The straight line fitting processing means detects a plurality of straight lines corresponding to one line segment of the marker, based on the data of the contour point position extracted by the image processing means, 8. A three-dimensional position and orientation based on vision according to claim 6, wherein a straight line is obtained by performing a straight line averaging process, and the straight line parameters are determined. Recognition device.
複比不変の法則を利用して前記消失点の候補値を3つ以
上計算し、それらの候補値の2次元座標データに基づい
て前記単位方向ベクトルを計算するように構成されてい
ることを特徴とする請求項7または8記載の視覚に基く
三次元位置および姿勢の認識装置。9. The unit direction vector calculation processing means,
The apparatus is configured to calculate three or more candidate values of the vanishing point using the law of invariance of cross ratio, and calculate the unit direction vector based on two-dimensional coordinate data of the candidate values. 9. The three-dimensional position and orientation recognition device based on vision according to claim 7 or 8.
は、前記マーカの3本以上の平行な線分にそれぞれ対応
して得られた直線について、それらの交点に対する単位
方向ベクトルをモーメント行列に対する二次形式最小化
問題として解くことにより前記単位方向ベクトルを計算
するように構成されていることを特徴とする請求項7、
8または9記載の視覚に基く三次元位置および姿勢の認
識装置。10. The unit direction vector calculation processing means converts a unit direction vector with respect to an intersection point between straight lines obtained respectively corresponding to three or more parallel line segments of the marker into a quadratic form with respect to a moment matrix. The system according to claim 7, wherein the unit direction vector is calculated by solving as a minimization problem.
10. A three-dimensional position and orientation recognition apparatus based on vision according to 8 or 9.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP24249693A JP2903964B2 (en) | 1993-09-29 | 1993-09-29 | Three-dimensional position and posture recognition method based on vision and three-dimensional position and posture recognition device based on vision |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP24249693A JP2903964B2 (en) | 1993-09-29 | 1993-09-29 | Three-dimensional position and posture recognition method based on vision and three-dimensional position and posture recognition device based on vision |
Publications (2)
Publication Number | Publication Date |
---|---|
JPH0798214A JPH0798214A (en) | 1995-04-11 |
JP2903964B2 true JP2903964B2 (en) | 1999-06-14 |
Family
ID=17089959
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP24249693A Expired - Fee Related JP2903964B2 (en) | 1993-09-29 | 1993-09-29 | Three-dimensional position and posture recognition method based on vision and three-dimensional position and posture recognition device based on vision |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2903964B2 (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109458969A (en) * | 2018-12-18 | 2019-03-12 | 东莞市三姆森光电科技有限公司 | A kind of multi-axial Simultaneous investigating method of position closed-loop |
Families Citing this family (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
FR2760277B1 (en) | 1997-02-28 | 1999-03-26 | Commissariat Energie Atomique | METHOD AND DEVICE FOR LOCATING AN OBJECT IN SPACE |
US6727885B1 (en) | 1999-09-07 | 2004-04-27 | Nikon Corporation | Graphical user interface and position or attitude detector |
JP4095768B2 (en) * | 2000-11-24 | 2008-06-04 | 株式会社日立製作所 | Image processing method and non-contact image input apparatus using the same |
JP3793100B2 (en) | 2002-02-14 | 2006-07-05 | キヤノン株式会社 | Information processing method, apparatus, and recording medium |
JP3946711B2 (en) * | 2004-06-02 | 2007-07-18 | ファナック株式会社 | Robot system |
WO2007037227A1 (en) * | 2005-09-27 | 2007-04-05 | Tamura Corporation | Position information detection device, position information detection method, and position information detection program |
JP5079614B2 (en) * | 2008-07-15 | 2012-11-21 | Toa株式会社 | Camera parameter specifying apparatus and method, and program |
JP5233601B2 (en) * | 2008-11-07 | 2013-07-10 | セイコーエプソン株式会社 | Robot system, robot control apparatus, and robot control method |
CN102768545A (en) * | 2012-08-10 | 2012-11-07 | 江苏物联网研究发展中心 | Method for determining optimum tracking angles of photovoltaic tracking systems under standard equatorial coordinate system |
JP5988368B2 (en) * | 2012-09-28 | 2016-09-07 | Kddi株式会社 | Image processing apparatus and method |
JP6369933B2 (en) * | 2014-07-08 | 2018-08-08 | Kddi株式会社 | Information terminal device, method and program |
CN107671008A (en) * | 2017-11-13 | 2018-02-09 | 中国科学院合肥物质科学研究院 | A kind of part stream waterline automatic sorting boxing apparatus of view-based access control model |
CN110455272B (en) * | 2018-05-07 | 2023-05-12 | 苏州睿牛机器人技术有限公司 | Sensor system in a track following system |
JP7182981B2 (en) * | 2018-10-04 | 2022-12-05 | キヤノンメディカルシステムズ株式会社 | Shape measurement guide device, radiotherapy system and shape measurement guide program |
CN109682309B (en) * | 2018-12-21 | 2023-11-14 | 北京安视中电科技有限公司 | Intelligent measuring device and method for black crystal panel parameters based on machine vision |
JP6696102B1 (en) * | 2019-05-20 | 2020-05-20 | 東芝エレベータ株式会社 | Image processing device and marker |
CN114187588B (en) * | 2021-12-08 | 2023-01-24 | 贝壳技术有限公司 | Data processing method, device and storage medium |
-
1993
- 1993-09-29 JP JP24249693A patent/JP2903964B2/en not_active Expired - Fee Related
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109458969A (en) * | 2018-12-18 | 2019-03-12 | 东莞市三姆森光电科技有限公司 | A kind of multi-axial Simultaneous investigating method of position closed-loop |
CN109458969B (en) * | 2018-12-18 | 2020-12-11 | 东莞市三姆森光电科技有限公司 | Multi-axis synchronous measurement and control method of position full closed loop |
Also Published As
Publication number | Publication date |
---|---|
JPH0798214A (en) | 1995-04-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP2903964B2 (en) | Three-dimensional position and posture recognition method based on vision and three-dimensional position and posture recognition device based on vision | |
CN109308693B (en) | Single-binocular vision system for target detection and pose measurement constructed by one PTZ camera | |
CN109255813B (en) | Man-machine cooperation oriented hand-held object pose real-time detection method | |
JP3208953B2 (en) | Three-dimensional position and posture recognition method based on vision and three-dimensional position and posture recognition device based on vision | |
US9679385B2 (en) | Three-dimensional measurement apparatus and robot system | |
CN103558850B (en) | A kind of welding robot full-automatic movement self-calibration method of laser vision guiding | |
CN109676243A (en) | Weld distinguishing and tracking system and method based on dual laser structure light | |
Chen et al. | The autonomous detection and guiding of start welding position for arc welding robot | |
CN111476841B (en) | Point cloud and image-based identification and positioning method and system | |
CN107957246B (en) | binocular vision-based method for measuring geometric dimension of object on conveyor belt | |
CN108907526A (en) | A kind of weld image characteristic recognition method with high robust | |
CN114714356A (en) | Method for accurately detecting calibration error of hand eye of industrial robot based on binocular vision | |
JP2000094374A (en) | Picking device | |
JP4044454B2 (en) | Position measurement using zoom | |
CN114245091A (en) | Projection position correction method, projection positioning method, control device and robot | |
KR20170142379A (en) | Apparatus for detect dimensional welding line of welding robot using image processing | |
CN116160458B (en) | Multi-sensor fusion rapid positioning method, equipment and system for mobile robot | |
CN112288801A (en) | Four-in-one self-adaptive tracking shooting method and device applied to inspection robot | |
CN114842144A (en) | Binocular vision three-dimensional reconstruction method and system | |
JP2519445B2 (en) | Work line tracking method | |
JPH06258028A (en) | Method and system for visually recognizing three dimensional position and attitude | |
CN112419409A (en) | Pose estimation method based on real-time video | |
JP2001191285A (en) | Robot system and its usage | |
CN114252024B (en) | Single-measurement-module multi-working-mode workpiece three-dimensional measurement device and method | |
CN116091608B (en) | Positioning method and positioning device for underwater target, underwater equipment and storage medium |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
FPAY | Renewal fee payment (prs date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110326 Year of fee payment: 12 |
|
LAPS | Cancellation because of no payment of annual fees |