JP2008309671A - Object recognition method and device - Google Patents
Object recognition method and device Download PDFInfo
- Publication number
- JP2008309671A JP2008309671A JP2007158282A JP2007158282A JP2008309671A JP 2008309671 A JP2008309671 A JP 2008309671A JP 2007158282 A JP2007158282 A JP 2007158282A JP 2007158282 A JP2007158282 A JP 2007158282A JP 2008309671 A JP2008309671 A JP 2008309671A
- Authority
- JP
- Japan
- Prior art keywords
- voxel
- measurement
- error distribution
- error
- new
- 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.)
- Granted
Links
Images
Landscapes
- Length Measuring Devices By Optical Means (AREA)
Abstract
Description
本発明は、被計測物たる物体の形状、計測地点から計測して得られた被計測物の位置、姿勢を認識する方法と装置に関する。 The present invention relates to a method and an apparatus for recognizing the shape and the position and orientation of an object to be measured obtained from a measurement point.
従来は、複数のビジョンセンサで撮影し、第1〜n画像から物体の二次元特徴を抽出し、その対応付けで物体の二次元特徴の三次元情報を加え、物体の三次元位置・姿勢を推定している(例えば、特許文献1参照)。
ただし、位置・姿勢の信頼度評価値は、二次元画像処理のエッジ抽出精度に依存するため、高い精度は得られない。また、信頼性評価に使用する特徴量を事前に設定しておく必要がある。
Conventionally, the two-dimensional features of the object are extracted from the first to n images by photographing with a plurality of vision sensors, and the three-dimensional information of the two-dimensional feature of the object is added by the association, and the three-dimensional position / orientation of the object is determined. It is estimated (see, for example, Patent Document 1).
However, since the reliability evaluation value of the position / orientation depends on the edge extraction accuracy of the two-dimensional image processing, high accuracy cannot be obtained. In addition, it is necessary to set the feature amount used for reliability evaluation in advance.
また、距離センサによって得られた距離データから、物体の特徴的な部位(例えば、円柱、楕円)の大まかな位置を求め、これに従ってビジョンセンサを物体に正対させた後に、二次元処理により特徴部位を検出し、物体の精密な位置・姿勢を求めるものもある。
ただし、この場合は、距離データだけで物体の位置・姿勢を特定することはできず、二段構えであるためコストと時間がかかる。また、検出できる物体の形状的な制約が大きい。さらに、形状検査に適用するには精度が不十分である。
Also, from the distance data obtained by the distance sensor, the rough position of the characteristic part of the object (for example, a cylinder or an ellipse) is obtained, and the vision sensor is directly opposed to the object according to this, and then the characteristic is obtained by two-dimensional processing. Some of them detect parts and determine the precise position and orientation of an object.
However, in this case, the position / orientation of the object cannot be specified only by the distance data, and costs and time are required because it is a two-stage structure. In addition, there are great restrictions on the shape of the detectable object. Furthermore, the accuracy is insufficient for application to shape inspection.
また、単眼ビジョンセンサで、事前に物体の見え方を十分教示しておくことで、現在の見え方から物体の位置・姿勢を推定するものもある。
ただし、物体が一部隠れているような場合に対応が困難である。また、ビジョンセンサを用いるため、照明条件に依存する。さらに、大量の画像を予め教示しておく必要がる。また、形状検査に適用するには精度が不十分である。
However, it is difficult to cope with a case where an object is partially hidden. Moreover, since a vision sensor is used, it depends on illumination conditions. Furthermore, it is necessary to teach a large amount of images in advance. In addition, the accuracy is insufficient for application to shape inspection.
ビジョンセンサを用いてワーク(被計測物)の位置・姿勢を認識する場合、ワークの背景情報や外光等の照明条件の変化に影響を受け易い。そこで、背景の単一化や照明条件の安定化により計測環境を整える必要があるが、これら環境の整備は必ずしも可能ではない。これに対して、レーザ光をワークに投影して距離データを取得するレーザスキャナ等は、背景の影響を受けず、照明条件の変化に対してロバストであり、環境の整備を行うことなく、ワークの形状を計測することができる。ただし、レーザスキャナを用いた場合の欠点として、レーザ光のワーク表面に対する入射角度が浅い場合や、レーザ光が直接反射するような場合等に、計測結果に欠損や誤差が発生する点が挙げられる。加えて、レーザスキャナは三角測量方式とTOF(Time of Flight)方式に大別できるが、三角測量方式のレーザスキャナは、計測可能な距離の範囲が狭いため、単一視点からの計測ではワークの一部しか計測できないという問題がある。また、TOF方式のレーザスキャナは、一般に三角測量方式に比べて計測範囲は広いが距離計測精度が低いと言う問題を持つ。 When recognizing the position / orientation of a workpiece (object to be measured) using a vision sensor, it is easily affected by changes in illumination conditions such as background information of the workpiece and external light. Therefore, it is necessary to prepare a measurement environment by unifying the background and stabilizing the lighting conditions, but it is not always possible to prepare these environments. On the other hand, a laser scanner or the like that projects distance data by projecting laser light onto a workpiece is not affected by the background and is robust against changes in lighting conditions. Can be measured. However, a disadvantage of using a laser scanner is that the measurement result may be deficient or error when the incident angle of the laser beam on the workpiece surface is shallow or when the laser beam is directly reflected. . In addition, laser scanners can be broadly divided into triangulation methods and TOF (Time of Flight) methods, but triangulation laser scanners have a narrow range of measurable distances. There is a problem that only a part can be measured. In addition, the TOF laser scanner generally has a problem that the distance measurement accuracy is low although the measurement range is wider than the triangulation method.
三次元レーザレーダのような距離センサを用いる場合、計測される三次元形状上の被計測点は、横方向及び垂直方向に離散した点群となる。この点群の間隔は、計測点からの距離が例えば50mの場合、被計測点の間隔は、例えば横方向で約315mm、垂直方向で約525mmに達する。 When a distance sensor such as a three-dimensional laser radar is used, the measured points on the three-dimensional shape to be measured are point groups that are discrete in the horizontal direction and the vertical direction. For example, when the distance from the measurement point is 50 m, for example, the distance between the point groups reaches about 315 mm in the horizontal direction and about 525 mm in the vertical direction.
また、複数の計測位置から静止している三次元形状を計測する場合、三次元レーザレーダのような距離センサでは、計測位置毎に、被計測点の位置は通常異なる。
さらに、このような距離センサは、一般に測定距離に例えば約20cm前後の誤差を有する。
従って、三次元レーザレーダのような距離センサを用いる場合、以下の制約条件A〜Cがある。
条件A:計測データに点数が少ない(例えば、1フレーム=166×50点)
条件B:計測データに誤差を含む(例えば、測定距離に約20cm前後)
条件C:計測データは同じ計測点を計るとは限らない。
すなわち、得られる距離データは横方向及び垂直方向に離散した点群であり、計測毎に位置が相違するため対応点がなく、測定距離に比較的大きな誤差を含んでいる。
Also, when measuring a stationary three-dimensional shape from a plurality of measurement positions, the position of the measurement point is usually different for each measurement position in a distance sensor such as a three-dimensional laser radar.
Further, such a distance sensor generally has an error of about 20 cm, for example, in the measurement distance.
Therefore, when using a distance sensor such as a three-dimensional laser radar, there are the following constraints A to C.
Condition A: The measurement data has a small number of points (for example, 1 frame = 166 × 50 points)
Condition B: The measurement data includes an error (for example, the measurement distance is about 20 cm)
Condition C: The measurement data does not always measure the same measurement point.
That is, the obtained distance data is a group of points that are discrete in the horizontal direction and the vertical direction, and since the position is different for each measurement, there is no corresponding point, and the measurement distance includes a relatively large error.
このような距離データを「ICP(Iterative Closest Point)アルゴリズム」で処理する場合、以下の問題点がある。
ICPアルゴリズムは、対応点が既知でない場合の位置合わせ手段の1つであり、先の計測データに対するその後の計測データの最も近傍の点を求め、回転・並進を行いながらその距離の総和が最小になる状態を一致状態となるように解を求めるものである。
(1)誤差の蓄積
ICPアルゴリズムは、2つの距離データの重ね合わせ手段であり、先のデータとその後のデータとの比較を繰り返し、その差分を積分しても、対応点がほとんどないため、誤差が蓄積してしまう。
(2)計算量が多い
ICPアルゴリズムは、繰り返し計算であるため、計算量が膨大となる。すなわち、ICPアルゴリズムは、計測データの各データ点に対応するモデルデータを探索する必要があるため、モデルデータ点数および計測データ点数が増加すると計算量が増大する。具体的には、モデルデータの点数をM、計測データの点数をNとした場合、例えば全探索時の計算オーダは、O(M×N)となる。
(3)計測点が少ない場合を扱えない
ICPアルゴリズムは、密な距離データを対象としているため、離散した点群であり、空間的に疎である場合は、誤った結果に収束してしまう。
When such distance data is processed by the “ICP (Iterative Closest Point) algorithm”, there are the following problems.
The ICP algorithm is one of the alignment means when the corresponding points are not known. The nearest point of the subsequent measurement data with respect to the previous measurement data is obtained, and the sum of the distances is minimized while performing rotation / translation. The solution is obtained so that the state becomes the matching state.
(1) Accumulation of error The ICP algorithm is a means for superimposing two distance data, and even if the comparison between the previous data and the subsequent data is repeated and the difference is integrated, there are almost no corresponding points. Will accumulate.
(2) Large amount of calculation Since the ICP algorithm is iterative calculation, the amount of calculation becomes enormous. That is, the ICP algorithm needs to search for model data corresponding to each data point of the measurement data, so that the amount of calculation increases as the number of model data points and the number of measurement data points increase. Specifically, when the number of model data is M and the number of measurement data is N, for example, the calculation order at the time of full search is O (M × N).
(3) Cannot handle the case where the number of measurement points is small Since the ICP algorithm is intended for dense distance data, it is a discrete point cloud, and if it is spatially sparse, it converges to an incorrect result.
そのため、このような距離センサを用いた位置同定には、以下の要件を満たす必要がある。
(1)メモリ効率の良い周囲環境のためのデータ構造
逐次得られる計測データを全て保存する方法では無尽蔵にメモリを必要とする。よって、周囲環境を計測した結果を効率良く格納するデータ構造が必要である。
(2)計測データの点数不足および誤差に対する安定性
計測データの点数が少ない場合、および、誤差を含む場合であっても、できるだけ位置同定精度が低下しない必要がある。
(3)位置同定のための計算の効率化
周囲環境を計測して獲得した環境情報と計測データの比較により、自己位置同定を行うが、この比較処理に多くの計算を必要とする。
Therefore, it is necessary to satisfy the following requirements for position identification using such a distance sensor.
(1) Data structure for ambient environment with good memory efficiency The method of storing all the measurement data obtained sequentially requires an infinite amount of memory. Therefore, a data structure for efficiently storing the result of measuring the surrounding environment is necessary.
(2) Insufficient number of points in measurement data and stability against errors Even when the number of points in measurement data is small and includes errors, position identification accuracy should not be reduced as much as possible.
(3) Efficient calculation for position identification Self-position identification is performed by comparing measured information with environmental information obtained by measuring the surrounding environment, but this comparison process requires a lot of calculations.
本発明は上述した問題点を解決するために創案されたものである。すなわち、位置・姿勢が未知のワークをレーザスキャナ等の距離センサにより複数の視点から計測し、計測結果の欠損部分を別視点のデータで充填しながら、なおかつ精度良く統合することができる物体認識の方法と装置を提供する。計測範囲が狭いという三角測量方式の欠点は、複数視点からのデータをつなぎ合わせながら統合することで解決する。また、距離計測が低精度というTOF方式の欠点も、複数回の計測を統計的手法により統合することで解決可能となる。 The present invention has been developed to solve the above-described problems. In other words, object recognition that can measure workpieces with unknown positions and orientations from multiple viewpoints using a distance sensor such as a laser scanner, and integrate the missing parts of the measurement results with data from different viewpoints while still integrating with high accuracy. Methods and apparatus are provided. The shortcoming of the triangulation method, which has a narrow measurement range, can be solved by combining data from multiple viewpoints. Further, the disadvantage of the TOF method that the distance measurement is low accuracy can be solved by integrating a plurality of measurements by a statistical method.
本発明の第1の特徴は、物体の位置及び姿勢を認識する物体認識方法であって、
(1)新たな計測位置から計測して得られた三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
(2)前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
(3)前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶する第1マッチングステップとを実施し、
(4−1)先の計測位置から計測して得られた前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
(4−2)先の計測位置から計測して得られた前記三次元形状のデータが存在する場合には、
(4−2−1)先の計測位置に対する環境モデルに対し、(A)新たな計測データおよび誤差分布を回転及び並進させ、または(B)新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値が最小になるように位置合わせする精密合わせステップと、
(4−2−2)前記精密合わせステップにおける回転量及び並進量から自己位置を同定する自己位置同定ステップとを実施し、
(5)更に複数の計測位置から計測された複数の計測データおよび複数の誤差分布を統合する統合ステップと、
(6)統合して得られた計測データおよび誤差分布に対し、モデルデータによって表される被計測物を回転および並進させ、前記計測データと前記モデルデータを構成する要素との距離に関する評価値が最小になるようにする位置合わせをする第2マッチングステップとを実施する、ことにある。
モデルデータの表現方法としては、例えば、多面体、自由曲面、ボクセルおよび代表点と誤差分布の集合などが挙げられる。
距離に関する評価値は、当該距離の総和、当該距離の平均値、当該距離の2乗の総和、または、当該距離の最大値であってもよく、他の適切な評価値であってもよい。
「座標値に対応するボクセル」とは、その座標値によって特定される点が内部に存在するボクセルという意味である。
「代表点」とは、統計処理の結果得られる、誤差分布の中心や最尤推定値等である。
「代表点とその誤差分布」を設定する方法、「誤差分布間の距離」を求める方法については後述する。
A first feature of the present invention is an object recognition method for recognizing the position and orientation of an object,
(1) a data input step of inputting coordinate values on a three-dimensional shape obtained by measurement from a new measurement position to a computer;
(2) A model construction step of constructing an environment model that divides the spatial region in which the three-dimensional shape exists into a plurality of voxels made of rectangular parallelepipeds whose boundary surfaces are orthogonal to each other, and stores each voxel position;
(3) performing a first matching step of setting and storing a representative point and its error distribution inside the voxel corresponding to the coordinate value;
(4-1) When there is no data of the three-dimensional shape obtained by measuring from the previous measurement position, the new measurement position is identified as the self-position,
(4-2) When there is data of the three-dimensional shape obtained by measuring from the previous measurement position,
(4-2-1) With respect to the environmental model for the previous measurement position, (A) rotate and translate the new measurement data and error distribution, or (B) rotate and translate the environment model for the new measurement position, A precision alignment step for aligning the evaluation values regarding the distance between adjacent error distributions to a minimum;
(4-2-2) performing a self-position identification step of identifying the self-position from the rotation amount and the translation amount in the precision alignment step;
(5) Further, an integration step for integrating a plurality of measurement data and a plurality of error distributions measured from a plurality of measurement positions;
(6) With respect to the measurement data and error distribution obtained by the integration, the measurement object represented by the model data is rotated and translated, and an evaluation value related to the distance between the measurement data and the elements constituting the model data is obtained. And performing a second matching step that aligns to minimize.
Examples of model data expression methods include polyhedrons, free-form surfaces, voxels, representative points, and a set of error distributions.
The evaluation value related to the distance may be a sum of the distances, an average value of the distances, a sum of squares of the distances, a maximum value of the distances, or another appropriate evaluation value.
“Voxel corresponding to a coordinate value” means a voxel in which a point specified by the coordinate value exists.
The “representative point” is the center of error distribution, maximum likelihood estimated value, or the like obtained as a result of statistical processing.
A method for setting the “representative point and its error distribution” and a method for obtaining the “distance between error distributions” will be described later.
前記第2マッチングステップにおいて、計測データとモデルデータを構成する要素との距離を用いて、計測データとモデルデータの差異を判断することにより、物体の検査を可能とする、としても良い。
また、前記第1マッチングステップにおいて、ボクセルの内部に代表点とその誤差分布に加え、ボクセル内に物体の存在確率を表す確率値を設定し、記憶する、としても良い。
In the second matching step, the object may be inspected by determining the difference between the measurement data and the model data using the distance between the measurement data and the elements constituting the model data.
Further, in the first matching step, in addition to the representative point and its error distribution inside the voxel, a probability value representing the existence probability of the object may be set and stored in the voxel.
前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、
(1)新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離の総和が最小になるように、または
(2)新たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボクセル間の距離の総和が最小になるように、
位置合わせする粗合わせステップを有する、としても良い。
Before the precision alignment step, for the environmental model for the previous measurement position,
(1) Rotate and translate the new measurement data and error distribution so that the sum of the distance between the measurement data and the error distribution and the voxels having the representative points that are close to each other is minimized, or (2) New measurement position Rotate and translate the environment model for, so that the sum of the distances between voxels with representative points is minimized.
It is good also as having the rough alignment step which aligns.
前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、
(1)新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値の総和が最大になるように、または
(2)新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差の総和が最小になるように、
位置合わせする粗合わせステップを有する、としても良い。
Before the precision alignment step, for the environmental model for the previous measurement position,
(1) Rotate and translate new measurement data and error distribution so that the sum of the probability values of voxels having representative points close to the measurement data and error distribution is maximized, or (2) for a new measurement position Rotate and translate the environment model so that the sum of the difference in probability values of neighboring voxels is minimized.
It is good also as having the rough alignment step which aligns.
前記データ入力ステップの後に、
(1)現在の計測位置を過去の計測位置の変化から推定し、または
(2)現在の計測位置を取得可能なセンサにより取得し、または
(3)計測データの距離値だけでなく反射強度値を利用し、
照合する範囲を限定する探索範囲限定ステップを有する、としても良い。
After the data input step,
(1) The current measurement position is estimated from a change in the past measurement position, or (2) the current measurement position is acquired by a sensor capable of acquiring, or (3) the reflection intensity value as well as the distance value of the measurement data Use
It is good also as having the search range limitation step which limits the range to collate.
前記自己位置同定ステップにおいて、先の計測位置における位置及び姿勢から新たな計測位置の6自由度位置を同定する、としても良い。 In the self-position identification step, a 6-degree-of-freedom position of a new measurement position may be identified from the position and orientation at the previous measurement position.
前記精密合わせステップにおいて、誤差分布が交差する場合を同一計測点とし、その場合の距離値に分布の一致度から求めた重みを掛け合わせて誤差分布間の距離を算出する、としても良い。
分布の一致度から重みを求める方法については、後述する。
In the fine matching step, the case where the error distributions intersect may be set as the same measurement point, and the distance between the error distributions may be calculated by multiplying the distance value in that case by the weight obtained from the degree of coincidence of the distributions.
A method for obtaining the weight from the degree of coincidence of the distribution will be described later.
前記モデル構築ステップにおいて、最大のボクセルを必要最小限の分解能に相当する大きさに設定し、かつ単一のボクセル内に複数の被計測点が存在する場合に、単一のボクセル内に単一の被計測点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、としても良い。
「必要最小限の分解能に相当する大きさ」とは、計測対象を包含することができる大きさである。
In the model building step, when a maximum voxel is set to a size corresponding to the minimum necessary resolution and there are a plurality of measurement points in a single voxel, a single voxel is single. The voxel may be further divided into a plurality of voxels hierarchically so that only the measured point exists.
The “size corresponding to the necessary minimum resolution” is a size that can include the measurement object.
前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定もしくは消去する、としても良い。
A model update step of updating the environmental model after the self-position identification step, and in the model update step, search for a voxel corresponding to a coordinate value of a newly input measurement point;
Assuming that no object exists between the origin and the point to be measured, the representative point and the error distribution in the voxel located between them may be reset or deleted.
前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する、としても良い。
A model update step of updating the environmental model after the self-position identification step, and in the model update step, search for a voxel corresponding to a coordinate value of a newly input measurement point;
When there is no representative point in the voxel, the coordinate value and the error distribution may be set as the coordinate value and the error distribution of the representative point.
前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、としても良い。
2つの誤差分布から新たな誤差分布と新たな代表点を再設定する方法については、後述する。
A model update step of updating the environmental model after the self-position identification step, and in the model update step, search for a voxel corresponding to a coordinate value of a newly input measurement point;
When there is a representative point already set in the voxel, the newly acquired error distribution is compared with the already set error distribution in the voxel,
If the error distributions overlap each other, reset a new error distribution and a new representative point from both error distributions,
When the error distributions do not overlap with each other, the voxel may be further divided into a plurality of voxels hierarchically so that only a single representative point exists within a single voxel.
A method of resetting a new error distribution and a new representative point from the two error distributions will be described later.
前記自己位置同定ステップにおいて、自己位置の同定とともに、自己位置の誤差分布を同定し、現在の自己位置と誤差分布および同定した自己位置と誤差分布とからカルマンフィルタによって、自己位置と誤差分布を補正する、としても良い。
自己位置の誤差分布を同定する方法については、後述する。
In the self-position identification step, self-position error distribution is identified together with self-position identification, and the self-position and error distribution are corrected by a Kalman filter from the current self-position and error distribution and the identified self-position and error distribution. , Or as good.
A method for identifying the self-position error distribution will be described later.
前記モデル更新ステップにおいて、前記新たに取得した誤差分布と前記既に設定したボクセル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボクセル内へ移動したとき、
該他のボクセル内に代表点がない場合に、該新たな誤差分布と該新たな代表点を該他のボクセルの内部に設定し、
該他のボクセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボクセル内の誤差分布とを比較し、(A)誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、(B)誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、としても良い。
In the model update step, the newly acquired error distribution is compared with the already set error distribution in the voxel, and when the error distributions overlap each other, a new error distribution and a new representative point are obtained from both error distributions. As a result of resetting, when a new representative point moves into another voxel,
If there is no representative point in the other voxel, the new error distribution and the new representative point are set inside the other voxel,
When there is a representative point already set in the other voxel, the new error distribution is compared with the already set error distribution in the other voxel, and (A) when the error distributions overlap each other, From both error distributions, or from both error distributions and the coordinate values of the representative points already set in the voxel and the newly inputted measurement point, a new error distribution and a new representative point are reset, (B) When the error distributions do not overlap with each other, the voxel may be further divided into a plurality of voxels hierarchically so that only a single representative point exists within a single voxel.
前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、
該モデル更新ステップにおいて、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する、としても良い。
A model update step of updating the environmental model after the self-location identification step;
In the model updating step, a new representative point and error distribution are obtained by a Kalman filter from the newly inputted coordinate value of the measured point and its error distribution, and the representative point in the already set voxel and its error distribution. You can reset it.
前記精密合わせステップにおいて、前記近接する誤差分布間の距離に関する評価値が最小になるように位置合わせする代わりに、前記近接する誤差分布間に基づく最尤推定値によって定めた一致度に関する評価値が最大となるように、先の計測位置に対する環境モデルに対し、
(1)新たな計測データおよび誤差分布を回転及び並進させ、または
(2)新たな計測位置に対する環境モデルを回転及び並進させて位置合わせする、としても良い。
In the precise alignment step, instead of performing alignment so that the evaluation value related to the distance between the adjacent error distributions is minimized, the evaluation value related to the degree of coincidence determined by the maximum likelihood estimation value based on the adjacent error distributions is obtained. To maximize the environmental model for the previous measurement position,
(1) The new measurement data and error distribution may be rotated and translated, or (2) the environmental model for the new measurement position may be rotated and translated for alignment.
前記一致度に関する評価値の算出式は前記[数1]の式で表わされる。 The formula for calculating the evaluation value related to the degree of coincidence is expressed by the formula [Equation 1].
前記[数1]の式において、計測点jと環境モデル上の代表点iとが対応付けられているとし、当該計測点jなる計測データが得られる確率をEM(i、j)としており、ω(j)は、環境モデルの中に計測点jと対応付けられる代表点が存在する場合は1、それ以外の場合は0としている。
「計測点jと環境モデル上の代表点iとが対応付けられている」とは、計測点jに対応するボクセルにすでに代表点が存在する場合をいう。
In the equation [Equation 1], it is assumed that the measurement point j is associated with the representative point i on the environmental model, and the probability that the measurement data corresponding to the measurement point j is obtained is EM (i, j). ω (j) is 1 when there is a representative point associated with the measurement point j in the environmental model, and 0 otherwise.
“The measurement point j is associated with the representative point i on the environmental model” means a case where a representative point already exists in the voxel corresponding to the measurement point j.
前記統合ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
(1)誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定し、
(2)誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことが好ましい。
In the integration step, search for the voxel corresponding to the coordinate value of the newly input measurement point,
When there is a representative point already set in the voxel, the newly acquired error distribution is compared with the already set error distribution in the voxel,
(1) When error distributions overlap each other, a new error distribution and a new representative point are reset from both error distributions,
(2) When the error distributions do not overlap each other, it is preferable to further divide the voxel into hierarchical voxels so that only a single representative point exists within a single voxel. .
前記統合ステップにおいて、両誤差分布から新たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボクセル内へ移動したとき、
(1)該他のボクセル内に代表点がない場合に、該新たな誤差分布と該新たな代表点を該他のボクセルの内部に設定し、
(2)該他のボクセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボクセル内の誤差分布とを比較し、(A)誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、(B)誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことが好ましい。
In the integration step, as a result of resetting a new error distribution and a new representative point from both error distributions, when a new representative point moves into another voxel,
(1) If there is no representative point in the other voxel, the new error distribution and the new representative point are set inside the other voxel,
(2) When there is a representative point already set in the other voxel, the new error distribution is compared with the error distribution in the already set other voxel, and (A) the error distributions overlap each other. In this case, a new error distribution and a new representative point are reset from both error distributions, or from the coordinate values of the representative points already set in both error distributions and voxels and the newly input measurement point, (B) When the error distributions do not overlap with each other, it is preferable to further divide the voxel into a plurality of hierarchical voxels so that only a single representative point exists in a single voxel. .
前記評価値は、前記計測データと、前記計測データに近接する各平面との距離の総和である、ことが好ましい。 The evaluation value is preferably a sum of distances between the measurement data and each plane close to the measurement data.
上記本発明の方法と装置によれば、三次元形状の存在する空間領域を、複数のボクセルに分割し、各ボクセル位置を記憶するので、計測対象物が大きい場合であっても、データ量をボクセル数に比例する小さいデータサイズに抑えることができる。
また、座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するので、ボクセルの分解能以上の情報を表現することができる。
従って、本発明のデータ構造により、複数視点の計測データを固定サイズに統合できる。
According to the method and apparatus of the present invention, a spatial region in which a three-dimensional shape exists is divided into a plurality of voxels and each voxel position is stored, so even if the measurement object is large, the amount of data can be reduced. It can be suppressed to a small data size proportional to the number of voxels.
Further, since the representative point and its error distribution are set and stored in the voxel corresponding to the coordinate value, information exceeding the resolution of the voxel can be expressed.
Therefore, measurement data of a plurality of viewpoints can be integrated into a fixed size by the data structure of the present invention.
また、ボクセルの内部に物体の存在確率を表す確率値を設定し、保存することによって、誤差分布が代表点の属するボクセルよりも広がっている場合においても、各ボクセルにおける物体の存在有無を代表点が属するボクセルを見つけ、その誤差分布から再計算させることなく、当該ボクセルの確率値だけで容易に判断できるので、検索時間を抑えることができる。
また、現在の計測位置を過去の計測位置の変化から推定し、もしくは現在の計測位置を取得可能なセンサにより取得し、もしくは計測データの距離値だけでなく反射強度を利用し、照合する範囲を制限するので、検索時間を抑えることができる。
Moreover, even if the error distribution is wider than the voxel to which the representative point belongs by setting and saving a probability value representing the existence probability of the object inside the voxel, the representative point indicates whether or not the object exists in each voxel. Since it is possible to easily determine only the probability value of the relevant voxel without finding the voxel to which it belongs and recalculating it from its error distribution, the search time can be reduced.
In addition, the current measurement position can be estimated from changes in the past measurement position, or acquired using a sensor that can acquire the current measurement position, or using the reflection intensity as well as the distance value of the measurement data, Because it is limited, search time can be reduced.
また、粗合わせステップにおいて、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離に関する評価値(例えば、当該距離の総和)が最小になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボクセル間の距離に関する評価値(例えば、当該距離の総和)が最小になるように、
もしくは新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する評価値(例えば、当該確率値の総和)が最大になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値(例えば、当該確率値の差の総和)が最小になるように、位置合わせするので、誤差の蓄積を防ぎながら、短時間に代表点を有するボクセル同士の位置合わせができる。
In addition, in the rough alignment step, new measurement data and error distribution are rotated and translated with respect to the environmental model for the previous measurement position, and the evaluation value related to the distance between the measurement data and the error distribution and the distance between the voxels having representative points close to each other. (E.g., the sum of the distances) is minimized, or the environment model for the new measurement position is rotated and translated, and the evaluation value (e.g., the sum of the distances) regarding the distance between voxels having representative points is minimized. So that
Alternatively, rotate and translate the new measurement data and error distribution so that the evaluation value related to the probability value of the voxel having a representative point close to the measurement data and error distribution (for example, the sum of the probability values) is maximized, or Rotate and translate the environment model for the new measurement position and align it so that the evaluation value (for example, the sum of the difference of the probability values) of adjacent voxels has the minimum value. Voxels having representative points can be aligned in a short time while preventing accumulation.
また、先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値(例えば、当該確率値の差の総和)が最小になるように、位置合わせする場合には、物体が存在しない情報も加味して位置合わせするので、精度の向上が図れる。 In addition, the environmental model for the new measurement position is rotated and translated with respect to the previous environment model for the measurement position, and the evaluation value (for example, the sum of the difference of the probability values) regarding the difference in probability values of adjacent voxels is minimized. As described above, in the case of positioning, since the positioning is performed in consideration of information in which no object exists, the accuracy can be improved.
次いで、精密合わせステップにおいて、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値(例えば、当該距離の総和)が最小になるように位置合わせするので、誤差分布同士の精密な位置合わせを短時間にできる。
従って、本発明による複数視点からのデータの統合処理により、誤差の蓄積を防ぎながら、高精度な形状取得を可能にできる。
Next, in the precision alignment step, the new measurement data and error distribution are rotated and translated with respect to the environmental model for the previous measurement position, or the environmental model for the new measurement position is rotated and translated, and the error distribution between adjacent error distributions is changed. Since the alignment is performed so that the evaluation value related to the distance (for example, the sum of the distances) is minimized, accurate alignment between the error distributions can be performed in a short time.
Therefore, the integration processing of data from a plurality of viewpoints according to the present invention enables highly accurate shape acquisition while preventing error accumulation.
また、本発明のデータ構造は、ボクセル構造を拡張した構造をしているため、データサイズは点群に比べて小さく済ませることができる。すなわち、本発明で提案するデータ構造はボクセル内に1つの代表点を保存するため、計測点に対応するモデル点を探索する計算オーダを1とすることができるため、全体での計算オーダをO(N)に減少することができる。
従って、周囲環境と計測データの位置合わせ(ICPアルゴリズム)を行う際に探索対象となるデータ量が減少することから、計算を効率化できる。
In addition, since the data structure of the present invention has an expanded voxel structure, the data size can be made smaller than that of the point group. That is, since the data structure proposed in the present invention stores one representative point in the voxel, the calculation order for searching for the model point corresponding to the measurement point can be set to 1, so that the total calculation order is O (N) can be reduced.
Accordingly, the amount of data to be searched for when positioning the surrounding environment and measurement data (ICP algorithm) is reduced, so that the calculation can be made more efficient.
さらに、従来のICPアルゴリズムは疎データに対して誤った結果を出力するが、本発明の環境モデルは、ボクセル内に代表点と誤差分布を持っているため、疎データに対応した位置合わせが可能である。 Furthermore, the conventional ICP algorithm outputs incorrect results for sparse data, but the environment model of the present invention has a representative point and error distribution in the voxel, so alignment corresponding to sparse data is possible. It is.
また、現在の自己位置と誤差分布および同定した自己位置と誤差分布とからカルマンフィルタによって、自己位置と誤差分布を補正することによって、より自己位置の精度を向上させることができる。 Further, by correcting the self position and the error distribution by the Kalman filter from the current self position and the error distribution and the identified self position and the error distribution, the self position accuracy can be further improved.
従って、本発明の方法と装置によれば、誤差を含むデータを正確な情報に補正する機能を有すると共に、長時間の計測に対して高精度に収束する。なおかつ、位置同定処理は、ボクセルの各ボクセルに対応する代表点とその誤差分布を計測点で更新する処理であるため計算量が小さく、演算は周囲ボクセルへの影響を与えずボクセル内で閉じている。よって、高速な処理が可能である。また、計測データはボクセルに逐次統合可能であり、その結果得られた周囲環境情報のメモリサイズは固定サイズを上回ることはない。 Therefore, according to the method and apparatus of the present invention, it has a function of correcting data including errors into accurate information, and converges with high accuracy for long-time measurement. In addition, the position identification process is a process of updating the representative point corresponding to each voxel and its error distribution at the measurement point, so the calculation amount is small, and the calculation is closed in the voxel without affecting the surrounding voxels. Yes. Therefore, high-speed processing is possible. Moreover, the measurement data can be sequentially integrated into the voxels, and the memory size of the surrounding environment information obtained as a result does not exceed the fixed size.
さらに、前記モデル更新ステップにおいて、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定するので、より真値に近い形状を得ることができる。
特に、カルマンフィルタを用いたモデル更新ステップを繰り返すことで、誤差を含むデータであってもカルマンフィルタの効果により真値に収束した高精度な形状が得られる。
Further, in the model update step, a new representative point and error distribution are obtained by a Kalman filter from the newly inputted coordinate value of the measured point and its error distribution, and the representative point and its error distribution in the already set voxel. Since it is acquired and reset, a shape closer to the true value can be obtained.
In particular, by repeating the model update step using the Kalman filter, a highly accurate shape converged to a true value can be obtained by the effect of the Kalman filter even for data including errors.
また、前記精密合わせステップにおいて、前記近接する誤差分布間の距離に関する評価値(例えば、当該距離の総和)が最小になるように位置合わせする代わりに、前記近接する誤差分布間に基づく最尤推定値によって定めた一致度に関する評価値(例えば、当該一致度の総乗)が最大となるように、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させて位置合わせすることで、環境モデル、計測データの双方の誤差を考慮して位置合わせすることができる。 Further, in the fine alignment step, maximum likelihood estimation based on the adjacent error distributions is performed instead of performing alignment so that an evaluation value (for example, the sum of the distances) between the adjacent error distributions is minimized. Rotate and translate new measurement data and error distribution with respect to the environmental model for the previous measurement position so that the evaluation value (for example, the total power of the coincidence) determined by the value is maximized, or By aligning the environment model with respect to the new measurement position by rotating and translating, it is possible to perform the alignment in consideration of errors in both the environment model and the measurement data.
また、前記出力ステップにおいて、前記ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する際に、該計測値の信頼性または精度を示す指標を、該ボクセルの内部の誤差分布の大きさ出力装置に出力するため、計測装置を使用する際に、信頼性の低い計測値をアプリケーションの内容に応じて使用者が取捨選択することができるようになる。 Further, in the outputting step, when outputting the position of the representative point of the voxel to the output device as a measured value of a three-dimensional shape, an index indicating the reliability or accuracy of the measured value is used as an error distribution inside the voxel. Therefore, when using the measuring device, the user can select a measurement value with low reliability according to the contents of the application.
また、前記出力ステップにおいて、前記ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力するときに、該ボクセルの内部の誤差分布の大きさが所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値を出力装置に出力しないようにすることによって、計測装置を使用する際に、そもそも信頼性の高い計測値のみを扱うことができるようになるため、扱うデータ量を削減することや、信頼性の向上につながる。 In the output step, when the position of the representative point of the voxel is output to the output device as a three-dimensional shape measurement value, the error distribution inside the voxel is larger than a predetermined reference value. Since the measurement value of the voxel is not output to the output device on the assumption that the reliability or accuracy of the measurement value is lower than a predetermined reference, the measurement device is highly reliable in the first place. Since only values can be handled, the amount of data to be handled is reduced and reliability is improved.
また、前記データ入力ステップにおいて、距離センサを用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得し、特に異なる方向から距離データを得ることによって、分布形状が異なる誤差分布の距離データを統合することが可能となり、精度を向上することができる。 Further, in the data input step, using a distance sensor, coordinate values on a three-dimensional shape are sequentially acquired while moving the origin as distance data having an arbitrary measurement position as the origin, and distance data is obtained particularly from different directions. This makes it possible to integrate distance data of error distributions having different distribution shapes, and improve accuracy.
また、前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、前記ボクセル内の代表点および誤差分布の少なくともいずれかが新たに設定される、または再設定される、または該ボクセルを更に分割して階層的に複数のボクセルに分割される場合のみ、前記出力ステップにおいて、当該ボクセルの代表点の位置、誤差分布およびボクセルの位置の少なくともいずれかを三次元形状の計測値として出力装置に出力することによって、距離センサから新たに得られた被計測点によって影響を受けたボクセル内の代表点等の値が出力される。このため、使用者は従来と同様の運用を想定しながら、距離センサで得られる元々の計測値があたかもより精度の高い計測値に置き換わったように、利用することができるようになる。このように、より精度の高い三次元形状計測が可能になる。 In addition, after the matching step, there is a model update step for updating the environmental model, and in the model update step, a voxel corresponding to the coordinate value of the newly input measurement point is searched, and the inside of the voxel Only in the case where at least one of the representative point and the error distribution is newly set or reset, or when the voxel is further divided and divided into a plurality of voxels hierarchically, in the output step, the voxel By outputting at least one of the representative point position, error distribution, and voxel position to the output device as a three-dimensional shape measurement value, the inside of the voxel affected by the measured point newly obtained from the distance sensor A value such as a representative point is output. For this reason, the user can use it as if the original measurement value obtained by the distance sensor was replaced with a measurement value with higher accuracy while assuming the same operation as the conventional one. In this way, more accurate three-dimensional shape measurement is possible.
また、前記出力ステップにおいて、距離センサの位置から距離センサが位置計測可能な範囲の環境モデル内のボクセルの代表点の位置を三次元形状の計測値として出力装置に出力することによって、従来の距離センサの計測値の分解能が粗い場合でも、あたかも精度良くかつ分解能が高い距離センサのように利用することが可能となる。このように、より精度の高い三次元形状計測が可能になる。
本発明のその他の目的及び有利な特徴は、添付図面を参照した以下の説明から明らかになろう。
In the output step, the position of the representative point of the voxel in the environmental model within the range in which the distance sensor can measure the position from the position of the distance sensor is output to the output device as a three-dimensional shape measurement value. Even when the resolution of the measurement value of the sensor is rough, it can be used as if it were a distance sensor with high accuracy and high resolution. In this way, more accurate three-dimensional shape measurement is possible.
Other objects and advantageous features of the present invention will become apparent from the following description with reference to the accompanying drawings.
以下本発明の好ましい実施形態について、図面を参照して説明する。なお、各図において、共通する部分には同一の符号を付し、重複した説明を省略する。 Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. In each figure, common portions are denoted by the same reference numerals, and redundant description is omitted.
図1は、本発明の物体認識装置の全体構成図である。図1に示すように、本発明の物体認識装置40は、計測対象物(ワーク)までの距離を計測する3Dセンサ41、物体認識処理を実行する物体認識処理部42、被計測対象物の三次元形状を表すデータ(例えば、CADデータ)を記憶するモデルデータベース部43を具備する。物体認識処理部42において認識された物体の位置や姿勢を、ロボット制御部45へ出力し、ロボット制御部45からロボット46へ制御信号を出力し、ハンド47を動かして計測対象物を把持したりすることができる。
本発明において、自己位置とは、計測位置を意味し、例えば、3Dセンサ41の外界における6自由度の位置と姿勢を意味する。
図示しないが、本発明において、距離センサ(3Dセンサ)以外のオドメータ、カメラ、GPS、姿勢センサを、必要に応じてオプションとして用いても良い。以下、距離センサを用いた例を説明する。
FIG. 1 is an overall configuration diagram of an object recognition apparatus according to the present invention. As shown in FIG. 1, an
In the present invention, the self-position means a measurement position, for example, a position and orientation of 6 degrees of freedom in the outside world of the
Although not shown, in the present invention, an odometer, a camera, a GPS, and an attitude sensor other than the distance sensor (3D sensor) may be optionally used as necessary. Hereinafter, an example using a distance sensor will be described.
図2は、距離センサの一例としての三次元レーザレーダの構成図である。この図に示すように、三次元レーザレーダ10は、レーダヘッド12と制御器20から構成される。レーザダイオード13から発振されたパルスレーザ光1は、投光レンズ14で平行光2に整形され、ミラー18a,18bと回転・揺動するポリゴンミラー15で二次元方向に走査され、測定対象物に照射される。測定対象物から反射されたパルスレーザ光3は、ポリゴンミラー15を介して受光レンズ16で集光され、光検出器17で電気信号に変換される。
FIG. 2 is a configuration diagram of a three-dimensional laser radar as an example of a distance sensor. As shown in this figure, the three-
制御器20内の時間間隔カウンタ21は、レーザダイオード13のパルス発振タイミングと同期したスタートパルス4と、光検出器17から出力されたストップパルス5の時間間隔を計測する。信号処理ボード22は、反射光が検出された時点の時間間隔t、ポリゴンミラーの回転角度θ、揺動角度φを極座標データ(r,θ,φ)として出力する。
rは計測位置(レーダヘッド設置位置)を原点とする距離であり、r=c×t/2 の式で求められる。ここでcは光速である。
判定処理ユニット23は、信号処理ボードからの極座標データを、レーダヘッド設置位置を原点とした三次元空間データ(x,y,z)へ変換して、検出処理を行うようになっている。なおこの図で24はドライブユニットである。
The
r is a distance with the measurement position (radar head installation position) as the origin, and is obtained by the equation r = c × t / 2. Here, c is the speed of light.
The
上述した三次元レーザレーダ10の計測範囲は、例えば、水平画角60°、垂直画角30°、最大測定距離50mである。また、位置検出精度は、例えば約20cmである。
また、計測データを各画素に対して奥行き方向の距離値を持った距離画像で表示する場合、1フレームの計測点数を、横方向166点、スキャン方向50点とすると、1フレームに166×50=8300点が表示される。この場合にフレームレートは、例えば約2フレーム/秒である。
The measurement range of the three-
Further, when displaying the measurement data as a distance image having a distance value in the depth direction for each pixel, if the number of measurement points in one frame is 166 points in the horizontal direction and 50 points in the scan direction, 166 × 50 per frame. = 8300 points are displayed. In this case, the frame rate is, for example, about 2 frames / second.
この三次元レーザレーダ10で計測される三次元形状上の被計測点は、横方向にΔθ×r、垂直方向にΔφ×r、互いに離散した点群となる。例えば、Δθ=60/166×π/180=6.3×10−3ラジアン、Δφ=30/50×π/180=10.5×10−3ラジアン、r=50mの場合、最も近接する場合でも、被計測点の間隔は、横方向で約315mm、垂直方向で約525mmとなる。
The points to be measured on the three-dimensional shape measured by the three-
本発明では、距離センサとして、例えば、上述した三次元レーザレーダ10を用いる。しかし、距離センサはこれに限定されず、視差を利用した距離センサ、その他の周知の距離センサを用いることができる。
In the present invention, for example, the above-described three-
図3,図4は、距離センサで計測された極座標データと誤差の関係を示す図である。
図3に示すように、任意の計測位置を原点とする極座標値(r,θ,φ)を計測結果として計測する。距離センサによる計測結果には、図に示すような誤差分布が通常存在する。
この誤差分布は、誤差分布のrs,θs,φsでの存在確率をP(rs,θs,φs)とした場合、誤差分布は計測の軸r,θ,φ方向に正規分布しているとし、例えば[数2]の式で表すことができる。ここで、r,θ,φはセンサからの計測値、σr,σθ,σφは標準偏差、Aは規格化定数である。
図4に示すように、誤差分布は、通常r方向に長い切頭円錐形(左図)に内包される分布であるが、遠方においてaとbの差は小さい。従って、この誤差分布を直方体に包含される楕円体として安全サイドに近似することができる。
3 and 4 are diagrams illustrating the relationship between polar coordinate data measured by the distance sensor and errors.
As shown in FIG. 3, polar coordinate values (r, θ, φ) with an arbitrary measurement position as the origin are measured as measurement results. An error distribution as shown in the figure normally exists in the measurement result by the distance sensor.
This error distribution is normalized in the directions of measurement axes r, θ, and φ when the existence probability of the error distribution at r s , θ s , and φ s is P (r s , θ s , φ s ). For example, it can be expressed by the equation [Equation 2]. Here, r, θ, and φ are measured values from the sensor, σ r , σ θ , and σ φ are standard deviations, and A is a normalization constant.
As shown in FIG. 4, the error distribution is a distribution that is normally included in a truncated cone shape (left figure) that is long in the r direction, but the difference between a and b is small in the distance. Therefore, this error distribution can be approximated to the safe side as an ellipsoid included in a rectangular parallelepiped.
図5は、物体認識処理部の構成の一例を示す。この図に示すように、物体認識処理部42は、内部記憶装置34、中央処理装置35を備える。物体認識処理部42は、データ入力装置32および外部記憶装置33からデータを受け取り、ロボット制御部45へデータを送る。
FIG. 5 shows an example of the configuration of the object recognition processing unit. As shown in this figure, the object
データ入力装置32は、上述した距離センサを有し、三次元形状上の座標値を物体認識処理部42に入力する。また、例えばゴニオメータ、オドメータ等を併用して、距離センサの位置・姿勢や移動距離も入力するのがよい。なお、データ入力装置32は、キーボード等の通常の入力手段も有するのがよい。
外部記憶装置33は、ハードディスク、フロッピー(登録商標)ディスク、磁気テープ、コンパクトディスク等である。外部記憶装置33は、環境モデルのサイズが大きく後述する内部記憶装置34に入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の全体を保持できない場合には、環境モデルの一部範囲または全体範囲に対する、入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の一部または全体を記憶し、かつ本発明の方法を実行するためのプログラムを記憶する。また、外部記憶装置33は、後述するCADモデルデータを記憶する。
内部記憶装置34は、例えばRAM,ROM等であり、環境モデルの一部範囲または全体範囲に対する、入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の一部または全体を保管し、かつ演算情報を保管する。
中央処理装置35(CPU)は、モデル構築装置、マッチング装置、粗合わせと精密合わせの位置合わせ装置、モデル更新装置として機能し、演算や入出力等を集中的に処理し、内部記憶装置34と共に、プログラムを実行する。モデル構築装置は、後述のモデル構築ステップを行う装置であり、マッチング装置は、後述の環境モデルと点群とのマッチングステップおよび計測データとCADモデルデータとのマッチングステップを行う装置であり、位置合わせ装置は、後述の粗合わステップと精密合わせステップを行う装置であり、モデル更新装置は、後述のモデル更新ステップを行う装置である。
ロボット制御部45は、物体認識処理部42から受け取る被計測物の位置や姿勢に基づいてロボットのアームやハンドの動きを制御する。
The
The external storage device 33 is a hard disk, a floppy (registered trademark) disk, a magnetic tape, a compact disk, or the like. When the size of the environment model is large and the external storage device 33 cannot hold the coordinate values on the three-dimensional shape, the voxel position, the representative point, and the entire error distribution inputted to the
The
The central processing unit 35 (CPU) functions as a model construction device, a matching device, an alignment device for rough alignment and fine alignment, and a model update device. Run the program. The model building device is a device that performs a model building step described later, and the matching device is a device that performs a matching step between an environmental model and a point cloud described later and a matching step between measurement data and CAD model data. The apparatus is an apparatus that performs a rough alignment step and a fine alignment step, which will be described later, and the model update apparatus is an apparatus that performs a model update step, which will be described later.
The
上述した本発明の装置は、上述した距離センサと通常のPC(コンピュータ)を組み合わせたものでもよく、或いは、全体を一体にした装置であってもよい。また、自走可能な装置内に一体的に組み込んでもよい。 The above-described apparatus of the present invention may be a combination of the above-described distance sensor and a normal PC (computer), or may be an apparatus in which the entirety is integrated. Moreover, you may integrate in the apparatus which can be self-propelled.
図6は、本発明の方法を示すフローチャートである。図6に示すように、データ入力ステップS1、データ補正ステップS2、探索範囲限定ステップS3、モデル構築ステップS4、環境モデルと点群とのマッチングステップS5、自己位置同定ステップS7、S10、粗合わせステップS8、精密合わせステップS9、モデル更新ステップS11及び計測データとワークのモデルデータとのマッチングステップS12を有する。
なお、S4は初めて計測データが得られたときにだけ実施し、それ以外のS1〜S3及びS5〜S12は、計測データが得られる毎に実施する。
FIG. 6 is a flowchart illustrating the method of the present invention. As shown in FIG. 6, a data input step S1, a data correction step S2, a search range limiting step S3, a model construction step S4, a matching step S5 between an environmental model and a point group, a self-position identification step S7, S10, and a rough matching step S8, precise alignment step S9, model update step S11, and measurement data and workpiece model data matching step S12.
In addition, S4 is implemented only when measurement data is obtained for the first time, and other S1-S3 and S5-S12 are implemented every time measurement data is obtained.
データ入力ステップS1では、新たな計測位置から距離センサを用いて計測した三次元形状上の座標値をコンピュータの記憶装置に入力する。また、例えばゴニオメータ、オドメータ等を併用して、距離センサの位置・姿勢や移動距離も入力するのがよい。
なお、このデータ入力ステップS1において、三次元レーザレーダ10を用いて、三次元形状上の座標値を、任意の計測位置を原点とする距離データとして、原点を移動しながら順次取得するのがよい。
In the data input step S1, the coordinate value on the three-dimensional shape measured using the distance sensor from the new measurement position is input to the storage device of the computer. In addition, for example, a goniometer, an odometer, or the like may be used together to input the position / posture and movement distance of the distance sensor.
In this data input step S1, it is preferable to use the three-
距離センサとして三次元レーザレーダ10を用いた場合、三次元形状上の座標値は、任意の計測位置を原点とする距離データであり、極座標値(r,θ,φ)で表される。また、各座標値の誤差分布は、極座標値(r,θ,φ)から演算で求めるか、予め別の入力手段(例えばキーボード)で入力する。
When the three-
データ補正ステップS2では、距離データの補正処理を行い、距離データの精度を向上させる。また、極座標データとオドメータのデータから、任意の固定位置を原点とした三次元空間データ(x,y,z)へ変換してもよい。
距離データの補正処理では、孤立点の除去、統計的処理、等を行う。孤立点は、周囲の点から孤立して存在する点であり、計測データは複数の近接する点で構成されることから、孤立点は誤計測と仮定して除去することができる。統計的処理は、計測データが含む誤差分布を考慮して、複数回の計測を統計処理(例えば平均値等)することで、距離の補正を行う。
さらに、対象とする三次元形状が、直線近似又は平面近似できる場合にはこれらを行うのがよい。
In the data correction step S2, distance data correction processing is performed to improve the accuracy of the distance data. Alternatively, the polar coordinate data and the odometer data may be converted into three-dimensional space data (x, y, z) having an arbitrary fixed position as the origin.
In the distance data correction processing, isolated point removal, statistical processing, and the like are performed. An isolated point is a point that is isolated from surrounding points, and the measurement data is composed of a plurality of adjacent points. Therefore, the isolated point can be removed on the assumption of an erroneous measurement. The statistical process corrects the distance by statistically processing a plurality of measurements (for example, an average value) in consideration of the error distribution included in the measurement data.
Furthermore, when the target three-dimensional shape can be linearly approximated or planarly approximated, these should be performed.
探索範囲限定ステップS3では、距離センサの探索範囲を限定する。
探索範囲を限定せずに環境モデルに対する計測データのマッチング処理を行うと、複数の解(被計測点)が得られる可能性がある。そこで、(1)現在のセンサ位置を、過去のセンサ位置とその過去のセンサ位置からの変化量から推定し、センサ位置推定結果の近傍を探索する、(2)オドメータを用いてセンサ位置を推定し、探索範囲を限定する、(3)距離データのうち、距離値だけでなく、反射強度値も利用して探索結果を絞り込む、等を実施する。
In the search range limiting step S3, the search range of the distance sensor is limited.
If the measurement data is matched with the environmental model without limiting the search range, a plurality of solutions (measurement points) may be obtained. Therefore, (1) the current sensor position is estimated from the past sensor position and the amount of change from the past sensor position, and the vicinity of the sensor position estimation result is searched. (2) The sensor position is estimated using an odometer. The search range is limited. (3) Of the distance data, not only the distance value but also the reflection intensity value is used to narrow down the search result.
図7は、ボクセルの分割に八分木を用いた場合のモデル構築ステップの模式図である。
モデル構築ステップS4では、この図に示すように、三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセル6に分割し、各ボクセル位置を記憶する環境モデルを構築する。
ボクセル6の形状は、各辺の長さが等しい立方体でも、各辺の長さが異なる直方体でもよい。
また、ボクセル6の各辺の長さは、最大のボクセル6を必要最小限の分解能に相当する大きさに設定するのがよい。以下、最大のボクセル6をレベル1のボクセルと呼ぶ。
また、単一のボクセル内に複数の被計測点が存在する場合には、単一のボクセル内に単一の被計測点のみが存在するように、例えば八分木を選んだ場合は、ボクセルを更に八分割して階層的に複数のボクセルに分割する。以下、最大のボクセル6の八分割を1回実施した空間領域をレベル2のボクセル、k回実施した空間領域をレベルk+1のボクセルと呼ぶ。
FIG. 7 is a schematic diagram of model building steps when an octree is used for voxel division.
In the model construction step S4, as shown in this figure, an environment model is constructed in which a spatial region in which a three-dimensional shape exists is divided into a plurality of
The shape of the
The length of each side of the
In addition, when there are multiple measured points within a single voxel, for example, when an octree is selected so that only a single measured point exists within a single voxel, the voxel Is further divided into eight voxels hierarchically. Hereinafter, a spatial region in which the
図8は、構築された環境モデルの模式図である。
マッチングステップS5では、この図に示すように、三次元形状上の座標値に対応するボクセル6の内部に代表点7とその誤差分布8を設定し記憶する。末端のボクセルは計測値の代表点を1つだけ持つことができる。各ボクセルが計測値の代表点とその誤差分布を持つことで、物体の形状を表す。また、物体の存在確率を表す確率値をボクセルに持たせることもできる。
FIG. 8 is a schematic diagram of the constructed environmental model.
In the matching step S5, as shown in this figure, the
マッチングステップS5において、代表点の絶対位置は次の[数3]の式で与えられる。ここで、(x,y,z)は代表点のボクセルでの相対座標、Sx,Sy,Szはレベル1でのボクセルの一辺の大きさ、nx(k),ny(k),nz(k)はレベルkでのボクセルの番地、Lは求める代表点が存在するレベルである。 In the matching step S5, the absolute position of the representative point is given by the following [Equation 3]. Here, (x, y, z) is the relative coordinates of the voxels of the representative points, Sx, Sy, Sz is the side of the voxel at level 1 size, n x (k), n y (k), n z (k) is the address of the voxel at level k, and L is the level at which the representative point to be found exists.
図9,図10は、本発明におけるボクセルデータのデータ構造を示す図である。
この図において、図9は、各ボクセルデータのメモリレイアウト例である。この図において、矢印はデータへのリンクを表し、値としてはデータへのポインタを保持する。
図10は、レベル2(1,1,0)のボクセルが代表点を持つ場合の例を示している。なおこの図において、nullは空集合を表す。
9 and 10 are diagrams showing the data structure of voxel data in the present invention.
In this figure, FIG. 9 is a memory layout example of each voxel data. In this figure, an arrow represents a link to data, and a pointer to the data is held as a value.
FIG. 10 shows an example in which a voxel of level 2 (1, 1, 0) has a representative point. In this figure, null represents an empty set.
上述したデータ構造の環境モデルは、以下の特徴を有する。
(1)内容:空間を複数の小直方体(ボクセル)で分割して各ボクセルに計測点の代表点と誤差分布を保持する。
(2)精度:ボクセル毎に持つ計測点の代表値相当である。
(3)存在:物体の存在の有無を表現できる。
(4)データ量:ボクセルの個数に比例してメモリを必要とするが、サイズ固定である。
(5)点群からの変換:適しており、計算量は少ない。
(6)アクセス速度:シンプルな構造をしているため、要素へのアクセスが高速である。
The environmental model of the data structure described above has the following characteristics.
(1) Contents: A space is divided into a plurality of small rectangular parallelepipeds (voxels), and a representative point of measurement points and an error distribution are held in each voxel.
(2) Accuracy: Equivalent to the representative value of the measurement points for each voxel.
(3) Presence: The presence or absence of an object can be expressed.
(4) Data amount: A memory is required in proportion to the number of voxels, but the size is fixed.
(5) Conversion from a point cloud: Suitable and less computational complexity.
(6) Access speed: Since the structure is simple, access to elements is fast.
またこの特徴から、上述した環境モデルは、以下の効果A〜Cをすべて満たしている。
効果A:誤差を考慮した表現が可能である。
効果B:必要なメモリ量と計算量が一定量以下である。
効果C:物体の存在だけでなく、物体が存在しないことを表せる。
Moreover, from this characteristic, the environmental model mentioned above satisfy | fills all the following effects AC.
Effect A: It is possible to express in consideration of errors.
Effect B: Necessary memory amount and calculation amount are below a certain amount.
Effect C: Not only the presence of an object but also the absence of an object can be expressed.
図6のステップS6で、先の計測位置から計測した同一の三次元形状に関するデータの有無をチェックする。このチェックで、先の計測位置から計測した同一の三次元形状に関するデータが存在しない場合には、新たな計測位置を自己位置(センサ位置)と同定する。
この同定は、逐次移動する移動体の初期位置において、既知の6自由度位置(例えばグローバル座標系の原点)で実施することが好ましい。また、この同定において、新たな計測位置の位置(3自由度)と姿勢(3自由度)を含む6自由度位置を同定することが好ましい。
In step S6 of FIG. 6, the presence or absence of data relating to the same three-dimensional shape measured from the previous measurement position is checked. In this check, when there is no data regarding the same three-dimensional shape measured from the previous measurement position, the new measurement position is identified as the self position (sensor position).
This identification is preferably performed at a known six-degree-of-freedom position (for example, the origin of the global coordinate system) at the initial position of the moving body that sequentially moves. In this identification, it is preferable to identify a 6-degree-of-freedom position including a position (3 degrees of freedom) and a posture (3 degrees of freedom) of a new measurement position.
また、ステップS6のチェックで、先の計測位置から計測した同一の三次元形状に関するデータが存在する場合には、粗合わせステップS8と精密合わせステップS9を、マッチングステップS5の後に実施する。
図11は、粗合わせステップS8と精密合わせステップS9のデータ処理フロー図であり、図12は粗合わせステップS8の模式図、図13は精密合わせステップS9の模式図である。
If there is data relating to the same three-dimensional shape measured from the previous measurement position in the check in step S6, the rough matching step S8 and the fine matching step S9 are performed after the matching step S5.
FIG. 11 is a data processing flowchart of the rough matching step S8 and the fine matching step S9, FIG. 12 is a schematic diagram of the rough matching step S8, and FIG. 13 is a schematic diagram of the fine matching step S9.
図11の粗合わせステップS8では、図12に示すように、先の計測位置に対する環境モデルに対し、
(1−1)新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルとの距離に関する評価値(例えば、当該距離の総和)が最小になるように、もしくは
(1−2)新たな計測位置に対する環境モデル(新たな計測データから作成したボクセル)を回転及び並進させ、近接する代表点を有するボクセル間の距離に関する評価値(例えば、当該距離の総和)が最小になるように、もしくは、
(2−1)新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する評価値(例えば、当該確率値の総和)が最大になるように、もしくは
(2−2)新たな計測位置に対する環境モデル(新たな計測データから作成したボクセルであって各ボクセルは確率値を有する)を回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値(例えば、当該確率値の差の総和)が最小になるように、位置合わせする。
In the rough matching step S8 of FIG. 11, as shown in FIG.
(1-1) Rotate and translate the new measurement data and error distribution so that the evaluation value (for example, the sum of the distances) regarding the distance between the measurement data and the error distribution and the voxel having a representative point close to the measurement data and the error distribution is minimized. Or (1-2) rotating and translating an environmental model (a voxel created from new measurement data) for a new measurement position, and an evaluation value (for example, the distance between the voxels having adjacent representative points) (Sum) is minimized, or
(2-1) Rotate and translate the new measurement data and error distribution, and the evaluation value (for example, the sum of the probability values) regarding the probability value of the voxel having a representative point close to the measurement data and error distribution is maximized. Or (2-2) rotating and translating an environmental model (a voxel created from new measurement data and each voxel has a probability value) for a new measurement position, and the probability value of the adjacent voxel has Alignment is performed so that an evaluation value related to the difference (for example, the sum of the differences of the probability values) is minimized.
粗合わせステップS8における位置合わせは、環境モデルと計測データをボクセル空間上で表現すること、もしくは環境モデルはボクセル空間上で、計測データは代表点と誤差分布表現することで実施する。現在の計測データが位置(x,y,z)、姿勢(θ,φ,ψ)での計測であったとして、計測データをワールド座標に変換して環境モデルとの一致度を算出する。
一致度の算出には、例えば最短距離法を用いることができる。最短距離法を用いた場合のボクセル間の距離は、2つのボクセル空間をx(1)、x(2)、ボクセルの総数をI、ボクセルの値をxi (n)とすると、[数4]の式で定義できる。
計測データの最適な位置・姿勢は、位置(x,y,z)、姿勢(θ,φ,ψ)を変化させることによってεを最小にする最小自乗法によって算出できる。
The alignment in the rough alignment step S8 is performed by expressing the environment model and measurement data on the voxel space, or by expressing the environment model on the voxel space and the measurement data by representative points and error distribution. Assuming that the current measurement data is measurement at position (x, y, z) and orientation (θ, φ, ψ), the measurement data is converted into world coordinates, and the degree of coincidence with the environment model is calculated.
For example, the shortest distance method can be used to calculate the degree of coincidence. When the shortest distance method is used, the distance between voxels is expressed as follows, assuming that the two voxel spaces are x (1) and x (2) , the total number of voxels is I, and the value of voxels is x i (n). ] Expression.
The optimum position and orientation of the measurement data can be calculated by the least square method that minimizes ε by changing the position (x, y, z) and the orientation (θ, φ, ψ).
また、一致度として、例えば環境モデルと計測データの両ボクセルにおいて、近接する両ボクセルの持つ確率値の差に関する評価値(例えば、当該確率値の差の総和)を用いることができる。この場合は一致度を最小にするように、計測データの最適な位置・姿勢を変化させる。
また、環境モデルはボクセル空間上で、計測データは代表値と誤差分布表現した場合には、計測データの代表値、および誤差分布が近接する環境モデルのボクセルの確率値に関する評価値(例えば、当該確率値の総和)を用いることができる。この場合は一致度を最大にするように、計測データの最適な位置・姿勢を変化させる。
As the degree of coincidence, for example, in both voxels of the environmental model and the measurement data, an evaluation value (for example, the sum of the difference between the probability values) regarding the difference between the probability values of the adjacent voxels can be used. In this case, the optimum position / posture of the measurement data is changed so as to minimize the degree of coincidence.
In addition, when the environmental model is represented in the voxel space and the measurement data is expressed as a representative value and an error distribution, the representative value of the measurement data and an evaluation value related to the probability value of the environmental model in which the error distribution is close (for example, (Sum of probability values) can be used. In this case, the optimum position / posture of the measurement data is changed so as to maximize the degree of coincidence.
図11の精密合わせステップS9では、図13に示すように、先の計測位置に対する環境モデルに対し、
(1)新たな計測データおよび誤差分布を回転及び並進させ、もしくは
(2)新たな計測位置に対する環境モデルを回転及び並進させ、
近接する誤差分布間の距離に関する評価値(例えば、当該距離の総和)が最小になるように位置合わせする。
精密合わせステップS9における環境モデルと計測データの精密合わせ位置合わせには、点群と点群の位置合わせが可能なICPアルゴリズムに誤差分布を考慮した手法を利用する。位置合わせの初期値には、粗い位置合わせにより得られた位置・姿勢を利用する。
ICPアルゴリズムに利用する誤差分布間の距離の算出には、例えば誤差分布が交差する場合を同一計測点と考え、その場合の距離値に分布の一致度から求めた重みを掛け合わせて算出する。分布の一致には例えばマハラノビス距離のような距離尺度を利用できる。
この場合の環境モデルと計測データの距離は、環境モデルデータをpMi、環境モデルデータの誤差分布をΣMi、計測データをPDi、計測データの誤差分布をΣDi、誤差分布の合成関数をw、計測データに対応する環境モデルデータの個数をNとすると、[数5]の式で定義できる。ここで、Tは転置を表す。誤差分布の合成関数wは、分布の一致度から求めた重みである。
計測データの最適な位置・姿勢は、計測データを計測した位置(x,y,z) 、姿勢(θ,φ,ψ)を変化させてPDiを移動することによりεを最小にする最小自乗法によって算出できる。
また、自己位置・姿勢の同定とともに、自己位置の誤差分布を同定し、現在の自己位置と誤差分布および同定した自己位置と誤差分布とからカルマンフィルタによって、自己位置と誤差分布を補正する。
In the precision alignment step S9 of FIG. 11, as shown in FIG. 13, with respect to the environmental model for the previous measurement position,
(1) Rotate and translate new measurement data and error distribution, or (2) Rotate and translate environment model for new measurement position,
Alignment is performed so that an evaluation value (for example, the sum of the distances) related to the distance between adjacent error distributions is minimized.
For precise alignment of the environmental model and measurement data in the precision alignment step S9, a technique that considers an error distribution is used for an ICP algorithm that can align the point cloud and the point cloud. For the initial value of alignment, the position / posture obtained by rough alignment is used.
In calculating the distance between error distributions used in the ICP algorithm, for example, the case where the error distributions intersect is considered as the same measurement point, and the distance value in that case is multiplied by the weight obtained from the degree of coincidence of the distributions. For example, a distance scale such as Mahalanobis distance can be used to match the distributions.
The distance between the environmental model and the measurement data in this case is p Mi for the environmental model data, Σ Mi for the error distribution of the environmental model data, P Di for the measurement data, Σ Di for the error distribution of the measurement data, and a composite function of the error distribution. If the number of environmental model data corresponding to w and measurement data is N, it can be defined by the equation [Equation 5]. Here, T represents transposition. The error distribution composition function w is a weight obtained from the distribution coincidence.
The optimum position / orientation of the measurement data is the smallest self that minimizes ε by moving P Di by changing the position (x, y, z) and orientation (θ, φ, ψ) where the measurement data is measured. It can be calculated by multiplication.
In addition to the self-position / posture identification, the self-position error distribution is identified, and the self-position and error distribution are corrected by the Kalman filter from the current self-position and error distribution and the identified self-position and error distribution.
さらに図6のモデル更新ステップS11は、自己位置同定ステップS10の後に実施し、モデル構築ステップS4で構築した環境モデルを更新する。 Further, the model update step S11 in FIG. 6 is performed after the self-position identification step S10, and updates the environment model constructed in the model construction step S4.
図14は、モデル更新ステップS11におけるデータ処理フロー図である。この図に示すように、ステップS21で新たに入力された被計測点の座標値に対応するボクセルを探索し、ステップS22で該当するボクセル内に代表点がない(ボクセルが空である)場合には、ステップS23で新たに入力された被計測点の座標値と誤差分布を代表点の座標値と誤差分布として設定(新規に登録)する。
また、このステップS23において、新しい計測位置(原点)と被計測点の間には、原理的に物体が存在しないはずである。従って新しい計測位置(原点)と被計測点の間に位置するボクセル内の代表点と誤差分布を再設定、もしくは消去する。
FIG. 14 is a data processing flowchart in the model update step S11. As shown in this figure, a voxel corresponding to the coordinate value of the point to be measured newly input in step S21 is searched, and in step S22, there is no representative point in the corresponding voxel (the voxel is empty). Sets (newly registers) the coordinate value and error distribution of the measured point newly input in step S23 as the coordinate value and error distribution of the representative point.
In step S23, in principle, no object should exist between the new measurement position (origin) and the point to be measured. Therefore, the representative point and error distribution in the voxel located between the new measurement position (origin) and the measurement point are reset or deleted.
図15は、該当するボクセル内に既に設定した代表点がある場合の模式図である。
図14のステップS22で該当するボクセル内に既に設定した代表点がある場合には、ステップS24で新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較する(すなわち異なる点か同一点かを判断する)。
この比較で、誤差分布が互いに重複する場合(図15の(A))には、ステップS25で両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定する(すなわち誤差分布を合成する)。
またこの比較で、誤差分布が互いに重複しない場合(図15の(B))には、ステップS26、S27で単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボクセルに分割し新規に登録する。
分割と合成の基準は、例えば誤差分布の一致度から判断する。誤差分布の一致度には例えば、マハラノビス距離のような距離尺度を利用できる。また、2つの誤差分布に基づき、両者が同一点を表しているかを統計的検定によって判定してもよい。
FIG. 15 is a schematic diagram in the case where there is a representative point already set in the corresponding voxel.
If there is a representative point already set in the corresponding voxel in step S22 of FIG. 14, the error distribution newly acquired in step S24 is compared with the error distribution in the already set voxel (that is, whether different points are the same). Judge whether it is one point).
In this comparison, if the error distributions overlap each other ((A) in FIG. 15), in step S25, both error distributions or the representative points already set in the both error distributions and voxels are newly input. A new error distribution and a new representative point are reset from the coordinate value of the measurement point (that is, the error distribution is synthesized).
In this comparison, if the error distributions do not overlap with each other (FIG. 15B), the voxels are further reduced so that only a single representative point exists in a single voxel in steps S26 and S27. Dividing into 8 voxels hierarchically and newly registering.
The criteria for division and synthesis are determined from, for example, the degree of coincidence of error distributions. For example, a distance scale such as Mahalanobis distance can be used for the degree of coincidence of error distributions. Further, based on the two error distributions, it may be determined by a statistical test whether both represent the same point.
ステップS25で両誤差分布から新たな誤差分布と新たな誤差分布の中心を再設定した結果、新たな代表点が他のボクセル内へ移動したとき(即ち、ステップS28で、Yes)、ステップS22へ戻り、上述の処理を繰り返す。
なお、図16は、ステップS25で両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな誤差分布の中心を再設定した結果、新たな代表点が他のボクセル内へ移動する場合を示している。
As a result of resetting the new error distribution and the center of the new error distribution from both error distributions in step S25, when a new representative point moves into another voxel (that is, Yes in step S28), the process proceeds to step S22. Return and repeat the above process.
Note that FIG. 16 shows a new error distribution and a new error from both error distributions in step S25, or from both error distributions and the coordinate values of the representative points already set in the voxels and the newly inputted measured points. As a result of resetting the center of the distribution, a case where a new representative point moves into another voxel is shown.
ボクセル内に物体の存在確率を表す確率値を設定する場合には、モデル更新ステップS11において、ボクセル内の代表点と誤差分布の新規登録、もしくは再設定、もしくは消去、もしくは分割後に新規登録の処理に応じて、ボクセル内の確率値も統計処理によって、新規登録、もしくは再設定、もしくは消去、もしくは分割後に新規登録を行う。 When a probability value representing the existence probability of an object is set in a voxel, new registration processing is performed after newly registering, resetting, deleting, or dividing representative points and error distributions in the voxel in the model update step S11. Accordingly, the probability value in the voxel is also newly registered, reset, erased, or newly registered after division by statistical processing.
図17は、誤差分布が互いに重複する場合(図15の(A))の別の模式図である。ステップS25において、2つの代表点と誤差分布を合成して新たな代表点を誤差分布を設定する手段として、カルマンフィルタを用いることができる。例えば、二次元の場合に、この図に示すように、2つの代表点をそれぞれx(1),x’(2)、2つの誤差分布をΣ(1)、Σ’(2)とし、これを合成した代表点をx(2)、誤差分布をΣ(2)とすると、代表点x(2)と誤差分布Σ(2)を算出する模式図は図17のようになる。 FIG. 17 is another schematic diagram when the error distributions overlap each other (FIG. 15A). In step S25, a Kalman filter can be used as means for combining the two representative points and the error distribution to set an error distribution for the new representative point. For example, in the case of two dimensions, as shown in this figure, the two representative points are x (1) and x ′ (2), the two error distributions are Σ (1) and Σ ′ (2), respectively. 17 is a schematic diagram for calculating the representative point x (2) and the error distribution Σ (2), where x (2) and the error distribution are Σ (2).
図6のマッチングステップS12では、計測データは、平均値m(i)、誤差共分散行列Sm(i)の正規分布に従うものとする。
図22は、計測データとCADモデルとのマッチングを説明するための二次元模式図である。この図に示すように、被計測物(例えば、ワーク)のCADモデルは、例えば三角形パッチで表現し、その頂点をw(j)で表す。CADモデルの位置をt、姿勢をR、とするとき、三角形パッチの頂点の座標はR・w(j)+tとなる。なお、tは3要素の並進ベクトル、Rは3×3の行列とする。
In the matching step S12 of FIG. 6, it is assumed that the measurement data follows a normal distribution of an average value m (i) and an error covariance matrix Sm (i).
FIG. 22 is a two-dimensional schematic diagram for explaining matching between measurement data and a CAD model. As shown in this figure, a CAD model of an object to be measured (for example, a workpiece) is represented by, for example, a triangular patch, and its vertex is represented by w (j). When the position of the CAD model is t and the posture is R, the coordinates of the vertices of the triangular patch are R · w (j) + t. Note that t is a three-element translation vector, and R is a 3 × 3 matrix.
図23は、計測データとCADモデルとの対応を説明するための図である。三角形パッチw(j)の添え字jは、m(i)に対して全ての三角形パッチの頂点のうち最近傍となるものを選択した結果とする。
図24は、計測データとCADモデルとの一致度を説明するための図である。計測データとCADモデルの一致度E(R,t)は、マハラノビス距離を用いて図25に示す式によって定義できる。計測データから算出される被計測物の位置と姿勢は、E(R,t)を最小とするR,tにより与えられる。
FIG. 23 is a diagram for explaining the correspondence between the measurement data and the CAD model. The subscript j of the triangle patch w (j) is the result of selecting the nearest vertex among the vertices of all triangle patches for m (i).
FIG. 24 is a diagram for explaining the degree of coincidence between the measurement data and the CAD model. The degree of coincidence E (R, t) between the measurement data and the CAD model can be defined by the equation shown in FIG. 25 using the Mahalanobis distance. The position and orientation of the measurement object calculated from the measurement data are given by R, t that minimizes E (R, t).
算出された被計測物の位置tと姿勢Rに関するデータは、ロボット制御部45などに出力される。ロボット制御部45などに出力された被計測物の位置tと姿勢Rに関するデータは、ロボット46が被計測物をつかむために使用される。
また、被計測物の位置tと姿勢Rが算出できると、計測データの各誤差分布とCADモデルデータとが一致しない部分の位置やそのずれ量が算出できる。ずれ量が大きい場合、被計測物の表面に欠損部があることなどが分かる。つまり、被計測物の表面形状の検査が可能となる。
Data on the calculated position t and posture R of the measurement object is output to the
If the position t and orientation R of the object to be measured can be calculated, the position of the portion where the error distribution of the measurement data does not match the CAD model data and the amount of deviation thereof can be calculated. When the amount of deviation is large, it can be seen that there is a defect on the surface of the object to be measured. That is, the surface shape of the measurement object can be inspected.
モデル更新ステップS11において、新たに入力された被計測点の座標値に対応するボクセルを探索し、前記ボクセル内の代表点および誤差分布の少なくともいずれかが新たに設定される、または再設定される、または該ボクセルを更に分割して階層的に複数のボクセルに分割される場合、マッチングステップS12において、当該ボクセルの代表点の位置を三次元形状の計測値としても良い。 In the model update step S11, a voxel corresponding to the coordinate value of the newly input measurement point is searched, and at least one of the representative point and the error distribution in the voxel is newly set or reset. Alternatively, when the voxel is further divided into a plurality of hierarchically divided voxels, the position of the representative point of the voxel may be a three-dimensional shape measurement value in the matching step S12.
マッチングステップS12において、距離センサの位置・姿勢が得られる場合は、その位置から見える範囲の環境モデル内のボクセルの代表点の位置を三次元形状の計測値としてもよい。距離センサの位置から見える範囲とは、距離センサの位置から距離センサが位置計測可能な範囲であり、距離センサの位置から距離センサが位置計測可能な角度範囲(視野)と、距離センサの位置から距離センサが位置計測可能な距離範囲とを含む。 In the matching step S12, when the position / posture of the distance sensor is obtained, the position of the representative point of the voxel in the environmental model in the range visible from the position may be used as the measurement value of the three-dimensional shape. The range that can be seen from the position of the distance sensor is the range in which the distance sensor can measure the position from the position of the distance sensor, the angle range (field of view) that the distance sensor can measure from the position of the distance sensor, and the position of the distance sensor Distance range in which the distance sensor can measure the position.
図6に示した処理の手順は、新たな計測位置において、新しい計測データが得られる度に、処理を繰り返し行い、内部記憶装置34および外部記憶装置33の少なくともいずれかに結果を格納する。処理を高速化するためには、内部記憶装置34に容量が許す限り結果を格納することが好ましい。
The process procedure shown in FIG. 6 is repeated every time new measurement data is obtained at a new measurement position, and the result is stored in at least one of the
上述した本発明の方法と装置によれば、三次元形状の存在する空間領域を、複数のボクセル6に分割し、各ボクセル位置を記憶するので、計測対象物が大きい場合であっても、データ量をボクセル数に比例する小さいデータサイズに抑えることができる。
また、座標値に対応するボクセル6の内部に代表点7とその誤差分布8を設定し記憶するので、ボクセルの分解能以上の情報を表現することができる。
According to the above-described method and apparatus of the present invention, a spatial region in which a three-dimensional shape exists is divided into a plurality of
In addition, since the
また、ボクセルの内部に物体の存在確率を表す確率値を設定し、保存することによって、誤差分布が代表点の属するボクセルよりも広がっている場合においても、各ボクセルにおける物体の存在有無を代表点が属するボクセルを見つけ、その誤差分布から再計算させることなく、当該ボクセルの確率値だけで容易に判断できるので、検索時間を抑えることができる。
また、現在の計測位置を、
(1)「過去の計測位置」と「過去の計測位置からの予定されている変化量」に基づいて推定し、
(2)現在の計測位置を取得可能なセンサにより取得し、または
(3)計測データの距離値だけでなく反射強度を利用し、照合する範囲を制限するので、検索時間を抑えることができる。
Moreover, even if the error distribution is wider than the voxel to which the representative point belongs by setting and saving a probability value representing the existence probability of the object inside the voxel, the representative point indicates whether or not the object exists in each voxel. Since it is possible to easily determine only the probability value of the relevant voxel without finding the voxel to which it belongs and recalculating it from its error distribution, the search time can be reduced.
Also, the current measurement position
(1) Estimate based on “past measurement position” and “planned amount of change from past measurement position”,
(2) The current measurement position can be acquired by a sensor that can be acquired, or (3) not only the distance value of the measurement data but also the reflection intensity is used to limit the collation range, so that the search time can be reduced.
また、粗合わせステップS8において、先の計測位置に対する環境モデルに対し、
(1−1)新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離に関する評価値(例えば、当該距離の総和)が最小になるように、もしくは
(1−2)新たな計測位置に対する環境モデル(新たな計測データから作成したボクセル)を回転及び並進させ、近接する代表点を有するボクセル間の距離に関する評価値(例えば、当該距離の総和)が最小になるように、もしくは
(2−1)新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する評価値(例えば、当該確率値の総和)が最大になるように、もしくは
(2−2)新たな計測位置に対する環境モデル(新たな計測データから作成したボクセルであって各ボクセルは確率値を有する)を回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値(例えば、当該確率値の差の総和)が最小になるように、位置合わせするので、誤差の蓄積を防ぎながら、短時間に代表点を有するボクセル同士の位置合わせができる。
Further, in the rough matching step S8, for the environmental model for the previous measurement position,
(1-1) Rotating and translating new measurement data and error distribution, and the evaluation value (for example, the sum of the distances) regarding the distance between the measurement data and the error distribution and the voxel having a representative point close to it is minimized. Or (1-2) rotating and translating an environmental model (a voxel created from new measurement data) for a new measurement position, and an evaluation value (for example, the distance) between voxels having adjacent representative points (2-1) New measurement data and error distribution are rotated and translated, and an evaluation value related to the probability value of a voxel having a representative point close to the measurement data and error distribution (for example, Or (2-2) an environmental model for a new measurement position (created from new measurement data). Rotate and translate voxels, each voxel has a probability value), and align so that the evaluation value (for example, the sum of the difference of the probability values) of the probability values of neighboring voxels is minimized Therefore, it is possible to align voxels having representative points in a short time while preventing error accumulation.
また、先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値(例えば、当該確率値の差の総和)が最小になるように、位置合わせする場合には、物体が存在しない情報も加味して位置合わせするので、精度の向上が図れる。 In addition, the environmental model for the new measurement position is rotated and translated with respect to the previous environment model for the measurement position, and the evaluation value (for example, the sum of the difference of the probability values) regarding the difference in probability values of adjacent voxels is minimized. As described above, in the case of positioning, since the positioning is performed in consideration of information in which no object exists, the accuracy can be improved.
次いで、精密合わせステップS7において、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値(例えば、当該距離の総和)が最小になるように位置合わせするので、誤差分布同士の精密な位置合わせを短時間にできる。
従って、本発明による複数視点からのデータの統合処理により、誤差の蓄積を防ぎながら、高精度な形状取得を可能にできる。
Next, in the precision alignment step S7, new measurement data and error distribution are rotated and translated with respect to the environment model for the previous measurement position, or the environment model for the new measurement position is rotated and translated, and the error distribution between adjacent error distributions is calculated. Since the alignment is performed so that the evaluation value (for example, the total sum of the distances) regarding the distance between the error distributions is minimized, accurate alignment between the error distributions can be performed in a short time.
Therefore, the integration processing of data from a plurality of viewpoints according to the present invention enables highly accurate shape acquisition while preventing error accumulation.
また、本発明では、ボクセル構造を拡張した構造をしているため、データサイズは点群に比べて小さく済ませることができる。従って、周囲環境と計測データの位置合わせ(ICPアルゴリズム)を行う際に探索対象となるデータ量が減少することから、計算を効率化できる。 In the present invention, since the voxel structure is expanded, the data size can be made smaller than that of the point group. Accordingly, the amount of data to be searched for when positioning the surrounding environment and measurement data (ICP algorithm) is reduced, so that the calculation can be made more efficient.
さらに、従来のICPアルゴリズムは疎データに対して誤った結果を出力するが、本発明の環境モデルは、ボクセル内に代表点と誤差分布を持っているため、疎データに対応した位置合わせが可能である。 Furthermore, the conventional ICP algorithm outputs incorrect results for sparse data, but the environment model of the present invention has a representative point and error distribution in the voxel, so alignment corresponding to sparse data is possible. It is.
また、モデル構築ステップS4において、最大のボクセル6を必要最小限の分解能に相当する大きさに設定し、かつ単一のボクセル6内に複数の被計測点が存在する場合に、単一のボクセル内に単一の被計測点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボクセルに分割するので、データ量を小さいデータサイズに抑えると同時に、分割後のボクセルと代表点を用いて解像度を更に高めることができる。
In the model construction step S4, when the
特に、三次元形状上の同じ位置に関する複数の座標値を、複数の計測位置を原点とする距離データとして取得し、該距離データの座標値を、前記代表点の座標値とし、距離データの座標値の計測誤差を代表点の誤差分布とすることにより、正確な座標値と誤差分布を用いて複数回の計測を統計的に統合することができ、一層の精度向上が可能となる。 In particular, a plurality of coordinate values related to the same position on the three-dimensional shape are acquired as distance data having a plurality of measurement positions as origins, and the coordinate value of the distance data is used as the coordinate value of the representative point, and the coordinates of the distance data are obtained. By using the measurement error of the value as the error distribution of the representative point, a plurality of measurements can be statistically integrated using the accurate coordinate value and the error distribution, and the accuracy can be further improved.
図18は、複数の計測位置を原点とする距離データを統合することによって、代表点の誤差分布が縮小し、代表点の精度が向上する様子を示している。このように異なる計測位置(即ち、距離センサである三次元計測器の位置)を原点として得られた距離データは誤差分布の向きも異なるので、これらの距離データを環境モデルを介して逐次統合することによって、代表点の誤差分布が縮小し、代表点の位置精度が向上する。なお、図18において、三次元計測後の図はコップの2次元断面を表わした模式図であり、三次元計測後の図の破線はコップの実際表面を表わしている。 FIG. 18 shows how the error distribution of the representative points is reduced and the accuracy of the representative points is improved by integrating the distance data having a plurality of measurement positions as the origin. Since distance data obtained using different measurement positions (that is, positions of a three-dimensional measuring instrument as a distance sensor) in this way also have different error distribution directions, these distance data are sequentially integrated via an environmental model. As a result, the error distribution of the representative point is reduced and the position accuracy of the representative point is improved. In FIG. 18, the figure after the three-dimensional measurement is a schematic diagram showing the two-dimensional cross section of the cup, and the broken line in the figure after the three-dimensional measurement shows the actual surface of the cup.
また、自己位置の同定とともに、自己位置の誤差分布を同定し、現在の自己位置と誤差分布および同定した自己位置と誤差分布とからカルマンフィルタによって、自己位置と誤差分布を補正することによって、一層の精度向上が可能となる。 In addition to the self-position identification, the self-position error distribution is identified, and the self-position and error distribution are corrected by the Kalman filter from the current self-position and error distribution and the identified self-position and error distribution. Accuracy can be improved.
また、原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定、もしくは消去することにより、誤った計測データの影響を除去することができる。 Also, assuming that there is no object between the origin and the point to be measured, the influence of erroneous measurement data can be removed by resetting or deleting the representative point and error distribution in the voxel located between them. it can.
また、新たに入力された被計測点の座標値に対応するボクセルを探索し、該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定することにより、代表点の座標値と誤差分布を容易に設定できる。 Also, a voxel corresponding to the coordinate value of the newly input measurement point is searched, and when there is no representative point in the voxel, the coordinate value and the error distribution are set as the coordinate value and the error distribution of the representative point. Thus, the coordinate value and error distribution of the representative point can be easily set.
更に、前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボクセルに分割する、ことにより、誤差の蓄積を回避しながら高精度な形状に収束させることができる。
Further, when there is a representative point already set in the voxel, the newly acquired error distribution is compared with the already set error distribution in the voxel,
If the error distributions overlap each other, either a new error distribution and a new representative from both error distributions, or from the coordinate values of both the error distribution and the representative point already set in the voxel and the newly input measured point. Reset the point,
When the error distributions do not overlap each other, the voxel is further divided into eight voxels and divided into a plurality of voxels hierarchically so that only a single representative point exists in a single voxel. It is possible to converge to a highly accurate shape while avoiding accumulation.
従って、本発明の方法と装置によれば、誤差を含むデータを正確な情報に補正する機能を有すると共に、複数回の計測に対して高精度に収束する。なおかつ、位置同定処理は、ボクセルの各ボクセルに対応する代表点とその誤差分布を計測点で更新する処理であるため計算量が小さく、演算は周囲ボクセルへの影響を与えずボクセル内で閉じている。よって、高速な処理が可能である。また、計測データはボクセルに逐次統合可能であり、その結果得られた周囲環境情報のメモリサイズは固定サイズを上回ることはない。具体的には、レベル1のボクセル個数をN、分割最大レベルをLmaxとするとき、メモリサイズは、各ボクセルに必要とするメモリサイズに、高々、図26に示す式を乗じた値となるため、これは固定サイズを上回ることはない。
Therefore, according to the method and apparatus of the present invention, it has a function of correcting data including an error into accurate information and converges with high accuracy for a plurality of measurements. In addition, the position identification process is a process of updating the representative point corresponding to each voxel and its error distribution at the measurement point, so the calculation amount is small, and the calculation is closed in the voxel without affecting the surrounding voxels. Yes. Therefore, high-speed processing is possible. Moreover, the measurement data can be sequentially integrated into the voxels, and the memory size of the surrounding environment information obtained as a result does not exceed the fixed size. Specifically, when the number of voxels of
カルマンフィルタを用いたモデル更新ステップについて、詳しく説明する。 The model update step using the Kalman filter will be described in detail.
カルマンフィルタを用いたモデル更新ステップの場合には、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する。 In the case of the model update step using the Kalman filter, a new representative point is obtained by the Kalman filter from the newly input coordinate value of the measurement point and its error distribution, and the representative point in the already set voxel and its error distribution. And get the error distribution and reset.
各モデル点群の位置m(i)を状態量とし、距離センサの計測点の位置を基に、モデルを次の[数6]で表現する。なお、本実施例では、m(i)は、ボクセル内部の代表点である(以下、同様)。 The model is expressed by the following [Equation 6] based on the position m (i) of each model point group as the state quantity and based on the position of the measurement point of the distance sensor. In this embodiment, m (i) is a representative point inside the voxel (the same applies hereinafter).
[数6]において、
L(j)は、距離センサによる計測位置である。例えば、L(j)は、距離センサのセンサ座標系において三次元LRF(レーザレンジファインダ)の計測点j(j=1,...,N)の位置L(j)=(xL(j),yL(j),zL(j))tである。ここで、tは転置行列を示す(以下、同様)。
hm(Rr,tr,m(i))は、L(j)に対する観測系モデルである。
Rrは、距離センサを搭載した移動体(例えば移動ロボット)のワールド座標系に対する姿勢を表す回転行列Rr=R(θx,θy,θz)である。なお、θx,θy,θzは、それぞれx軸、y軸、z軸周りの回転角を示す(以下、同様)。
trは、上記移動体のワールド座標系に対する位置を表す並進ベクトルtr=(x,y,z)である。
vL(i)は、距離センサの計測値L(j)に加わる観測ノイズである。
Rsは、センサ座標系の移動体座標系に対する回転行列Rs=R(θx,θy,θz)である。
tsは、センサ座標系の移動体座標系に対する位置を表す併進ベクトルts=(x,y,z)である。
In [Equation 6],
L (j) is a measurement position by the distance sensor. For example, L (j) is a position L (j) = (x L (j) of a measurement point j (j = 1,..., N) of a three-dimensional LRF (laser range finder) in the sensor coordinate system of the distance sensor. ), Y L (j), z L (j)) t . Here, t represents a transposed matrix (the same applies hereinafter).
h m (R r , tr , m (i)) is an observation system model for L (j).
R r is a rotation matrix R r = R (θx, θy, θz) representing the attitude of a mobile body (for example, a mobile robot) equipped with a distance sensor with respect to the world coordinate system. Note that θx, θy, and θz indicate rotation angles around the x-axis, y-axis, and z-axis, respectively (hereinafter the same).
tr is a translation vector tr = (x, y, z) representing the position of the moving body with respect to the world coordinate system.
v L (i) is observation noise added to the measurement value L (j) of the distance sensor.
R s is a rotation matrix Rs = R (θx, θy, θz) with respect to the moving body coordinate system of the sensor coordinate system.
t s is a translation vector t s = (x, y, z) representing the position of the sensor coordinate system with respect to the moving object coordinate system.
距離センサによる計測点群と、環境モデル点群上の点i(即ち、代表点)を対応づける。この対応付けが行われたモデル点群上の点iに対して[数7]の式により更新を行う。なお、距離センサによる計測点群と対応付けが行われたモデル点群上の代表点m(i)に対してのみ次の[数7]の式により更新を行ってよい。 The measurement point group by the distance sensor is associated with the point i (that is, the representative point) on the environmental model point group. The point i on the model point group to which this association is performed is updated by the equation [7]. It should be noted that only the representative point m (i) on the model point group associated with the measurement point group by the distance sensor may be updated by the following equation (7).
[数7]の式において、
添え字kは、離散時刻kでの値であることを表す。
mk(i)について、m’k(i)はmk(i)の更新値(事後推定値)を示し、mk,k−1(i)はm’k−1(i)に基づいたmk(i)の予測値(事前推定値)を示す。なお、環境(測定対象物)は静止しているので、mk,k-1(i)=m’k-1(i)である。
Σmk(i)は、ボクセル内部の代表点mk(i)の誤差共分散行列(即ち、上述の誤差分布)である。また、Σmk(i)について、 Σ’mk(i)はΣmk(i)の更新値(事後推定値)を示し、Σmk,k−1(i)はΣ’mk−1(i)に基づいたΣmk(i)の予測値(事前推定値)を示す。センサ座標系において三次元LRFの計測点j(j=1,…,N)の位置をL(j)で表し、その誤差共分散行列をΣL(j)で表す。ここでNは、三次元LRFで得られた計測点の総数である。三次元LRFの誤差モデルとして計測距離に関係ない一定の正規分布を仮定する。センサ座標系のx軸方向にレーザを照射する場合の誤差共分散行列をΣSとする。レーザの照射方向に応じて誤差分布も姿勢を変える。ΣL(j)は、基準の方向に対するレーザ照射方向を回転行列RL(j)を用いてΣL(j)=RL(j)ΣSRL t(j)と表される。計測点jのワールド座標系における位置z(j)、およびその誤差共分散行列Σz(j)は、それぞれz(j)=Rr(RsL(j)+ts)+tr、Σz(j)=RrRsΣL(j)Rs tRr tと表すことができる。
Kmk(i) は、 mk(i)に対するカルマンゲインである。
hmk(Rrk,trk,mk,k−1(i))は、Lk(j)、i=pk(j)に対する観測系モデルである。i=pk(j)は、計測点jに対応付けられた環境地図(即ち、環境モデル)上の点である。
Hmkは、Lk(j)、i=pk(j)に対する観測系モデルのヤコビアン行列であり、次の[数8]の式で表わされる。
In the formula of [Formula 7],
The subscript k represents a value at a discrete time k.
For m k (i), m ′ k (i) indicates the updated value (post-mortem value) of m k (i), and m k, k−1 (i) is based on m ′ k−1 (i). In addition, a predicted value (preliminary estimated value) of m k (i) is shown. Since the environment (measurement object) is stationary, m k, k−1 (i) = m ′ k−1 (i).
Σ mk (i) is an error covariance matrix (that is, the above-described error distribution) of the representative point m k (i) inside the voxel. Further, for Σ mk (i), Σ ′ mk (i) indicates an updated value (post-mortem estimate) of Σ mk (i), and Σ mk, k−1 (i) indicates Σ ′ mk−1 (i). The predicted value (preliminary estimated value) of Σ mk (i) based on In the sensor coordinate system, the position of the measurement point j (j = 1,..., N) of the three-dimensional LRF is represented by L (j), and its error covariance matrix is represented by Σ L (j). Here, N is the total number of measurement points obtained by the three-dimensional LRF. Assuming a constant normal distribution regardless of the measurement distance as an error model for 3D LRF. The error covariance matrix in the case of irradiating a laser beam to the x-axis direction of the sensor coordinate system and sigma S. The error distribution also changes its posture according to the laser irradiation direction. Σ L (j) is expressed as Σ L (j) = R L (j) Σ S R L t (j) using the rotation matrix R L (j) with respect to the reference direction. The position z (j) of the measurement point j in the world coordinate system and its error covariance matrix Σ z (j) are z (j) = R r (R s L (j) + t s ) + t r , Σ z (j) = it can be represented as R r R s Σ L (j ) R s t R r t.
K mk (i) is a Kalman gain for m k (i).
h mk (R rk , t rk , m k, k−1 (i)) is an observation system model for L k (j) and i = p k (j). i = p k (j) is a point on the environment map (ie, environment model) associated with the measurement point j.
H mk is the Jacobian matrix of the observation system model for L k (j) and i = p k (j), and is expressed by the following [Equation 8].
カルマンフィルタの更新過程によって、環境地図のモデル点群の各点(ボクセルの代表点)の位置と誤差共分散行列の更新値m’k(i)、Σ’mk(i)が得られた段階で、環境モデルの更新を以下の手順で行う。
(1)これら更新値m’k(i)、Σ’mk(i)を、新たな代表点、誤差分布として再設定する。
(2)上述(1)の結果、代表点の位置が別のボクセル内に移動した場合、移動先のボクセルが代表点を保持していないときは、移動後の代表点とその誤差共分散行列を移動先のボクセルに保持させ、移動元のボクセルからは代表点等を取り除く。移動先のボクセルが既に代表点を保持しているときには、2つの代表点において、これらの両誤差分布が重複するかを判断する(上述のS24における判断と同様)。その後の処理は、図14のS24以降の処理と同じであってよい。
(3)モデル点群上の代表点m(i)と対応付けが行われなかった距離センサによる計測点について、当該計測点が含まれるボクセルが代表点を持たない場合は、計測点とその誤差分布をそのボクセルの代表点と誤差分布として追加し保持する。もし、ボクセル内に既に代表点が存在する場合には、ボクセル内にある対応付けが行われなかった他の複数の計測点を含め、既存の代表点と各計測点とが全て異なるボクセルに含まれるように、ボクセルを分割した上で分割後のボクセルに代表点等を継承させる。
When the Kalman filter is updated, the position of each point (representative point of voxel) of the model point cloud of the environmental map and the updated values m ′ k (i) and Σ ′ mk (i) of the error covariance matrix are obtained. The environmental model is updated according to the following procedure.
(1) These updated values m ′ k (i) and Σ ′ mk (i) are reset as new representative points and error distributions.
(2) As a result of the above (1), when the position of the representative point moves into another voxel, if the destination voxel does not hold the representative point, the moved representative point and its error covariance matrix Is held in the destination voxel, and representative points and the like are removed from the source voxel. When the destination voxel already holds a representative point, it is determined whether these two error distributions overlap at the two representative points (similar to the determination in S24 described above). The subsequent processing may be the same as the processing after S24 in FIG.
(3) For a measurement point by a distance sensor that has not been associated with the representative point m (i) on the model point group, if the voxel including the measurement point does not have a representative point, the measurement point and its error The distribution is added and retained as a representative point of the voxel and an error distribution. If a representative point already exists in the voxel, the existing representative point and each measurement point are all included in different voxels, including other measurement points that were not associated in the voxel. As shown, the voxel is divided, and the divided voxels are made to inherit the representative points and the like.
上述のカルマンフィルタを用いたモデル更新ステップを繰り返すことで、序々にボクセル内の誤差共分散行列(即ち、誤差分布)の範囲が小さくなるとともに、ボクセルが分割され易くなる。ボクセルが分割されることによって、初期ボクセルのサイズ以下の変化も表現することが可能となる。
図19は、カルマンフィルタを用いたモデル更新ステップにより得られた結果を示す。図20は図19の一部拡大図である。これら図において、初期のボクセルの1辺の長さを100cmとし、再分割数を6分割まで許している。対象が存在している領域では、ボクセルの再分割を繰り返した結果、計測対象を精度良く表現している。対象が存在しない領域ではボクセルの再分割は行われず、必要十分なデータ量で環境を表現できることがわかる。また、各ボクセル内の代表点の誤差分布も小さく、環境地図を高精度で表現できている。このように、誤差を含むデータであってもカルマンフィルタの効果により、真値に収束した結果が得られる。さらに、この方法では計測データ数を増加させることによって標準偏差が小さくなり、精度のさらなる向上が期待できる。
By repeating the model update step using the above-described Kalman filter, the range of the error covariance matrix (that is, error distribution) in the voxel is gradually reduced, and the voxel is easily divided. By dividing the voxel, it is possible to express a change below the size of the initial voxel.
FIG. 19 shows the result obtained by the model update step using the Kalman filter. FIG. 20 is a partially enlarged view of FIG. In these figures, the length of one side of the initial voxel is 100 cm, and the number of subdivisions is allowed up to six. In the region where the target exists, the measurement target is expressed with high accuracy as a result of repeated subdivision of the voxels. It can be seen that the voxel is not subdivided in the area where there is no target, and the environment can be expressed with the necessary and sufficient data amount. In addition, the error distribution of the representative points in each voxel is small, and the environment map can be expressed with high accuracy. As described above, even if data includes an error, a result converged to a true value can be obtained due to the effect of the Kalman filter. Further, in this method, the standard deviation is reduced by increasing the number of measurement data, and further improvement in accuracy can be expected.
また、測定対象物の位置・姿勢は固定しているため、更新を測定対象物の位置・姿勢と独立して行うことができる。なお、距離センサによる計測点群と対応付けが行われたモデル点群上の代表点m(i)に対してのみ、上述のカルマンフィルタによる更新を行うことで、大幅な計算コストの削減が可能になる。 Further, since the position / orientation of the measurement object is fixed, the update can be performed independently of the position / orientation of the measurement object. Note that only the representative point m (i) on the model point group associated with the measurement point group by the distance sensor is updated by the above-described Kalman filter, so that the calculation cost can be greatly reduced. Become.
前記精密合わせステップにおいて、前記近接する誤差分布間の距離の総和が最小になるように位置合わせする代わりに、前記近接する誤差分布間に基づく最尤推定値によって定めた一致度に関する評価値(例えば、当該一致度の総乗)が最大となるように、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させて位置合わせしてもよい。
この場合について詳しく説明する。
In the fine alignment step, instead of performing alignment so that the sum of the distances between the adjacent error distributions is minimized, an evaluation value (for example, an agreement value determined by a maximum likelihood estimation value based on the adjacent error distributions (for example, , Rotate and translate new measurement data and error distribution with respect to the environment model for the previous measurement position, or rotate and translate the environment model for the new measurement position so that the matching power is the maximum) May be aligned.
This case will be described in detail.
環境地図(環境モデル)であるモデル点群、およびセンサの計測点群双方に誤差モデルを考慮しているため、前記一致度に関する評価値(例えば、当該一致度の総乗)の算出式(以下、評価関数と言う)にも双方の誤差を取り入れることが可能である。本実施例の場合、単に最も近い点を対応付けするのではなく、尤度の概念を取り入れることによって、評価関数に環境地図上の誤差分布、および計測点の誤差分布を加味し、現時点での環境地図において、各計測点の出現確率が最大となる場合に評価関数も最大となるように定める。
具体的には、計測点jに対応付けられた環境地図上の点i=p(j)の位置が、平均値(代表点)m(i)、誤差共分散行列Σm(i)の正規分布に従うと仮定した上で、三次元LRFで計測した結果、L(j)なる計測データが得られる確率値Pr(L(j)|m(i),Σm(i))を点iと点jとの評価関数EM(i,j)とし、その総乗が最大となるように次の[数9]の式で評価関数を定める。[数9]の式によって誤差分布間に基づく最尤推定値によって定めた一致度を算出することができる。
Since an error model is taken into account for both the model point cloud that is an environmental map (environment model) and the measurement point cloud of the sensor, a calculation formula for the evaluation value related to the matching degree (for example, the sum of the matching degree) It is also possible to incorporate both errors into the evaluation function. In the case of the present embodiment, instead of simply associating the closest point, by incorporating the concept of likelihood, the error distribution on the environmental map and the error distribution of the measurement point are added to the evaluation function, and In the environmental map, the evaluation function is determined to be maximized when the appearance probability of each measurement point is maximized.
Specifically, the position of the point i = p (j) on the environment map associated with the measurement point j is the normal value of the average value (representative point) m (i) and the error covariance matrix Σ m (i). Assuming that the distribution follows, the probability value Pr (L (j) | m (i), Σ m (i)) from which the measurement data L (j) is obtained as a result of measurement by the three-dimensional LRF is expressed as a point i. The evaluation function EM (i, j) with respect to the point j is set, and the evaluation function is determined by the following equation [Equation 9] so that the sum of the functions is maximum. The degree of coincidence determined by the maximum likelihood estimation value based on the error distribution can be calculated by the equation [Equation 9].
ただし、ω(j)は、モデル点群の中に計測点jと対応付けられる点が存在する場合は1、それ以外の場合は0とする。
ここで、Pr(L(j)|q)を環境地図の点がqの位置にある場合にL(j)なる計測データが得られる確率値を表すものとし、Pr(q|m(i),Σm(i))を平均値m(i)、誤差共分散行列Σm(i)の正規分布に従うと仮定した上で環境地図の点がqの位置にある確率値を表すものとすると、[数10]の式が成り立つ。
However, ω (j) is 1 when there is a point associated with the measurement point j in the model point group, and 0 otherwise.
Here, Pr (L (j) | q) represents a probability value for obtaining measurement data L (j) when the point on the environment map is at the position q, and Pr (q | m (i) , Σ m (i)) is assumed to follow the normal distribution of mean value m (i) and error covariance matrix Σ m (i), and represents the probability value that the point of the environmental map is at the position of q. , [Equation 10] holds.
Pr(q|m(i),Σm(i))は正規分布を仮定すると、次の[数11]の式となる。 Pr (q | m (i), Σ m (i)) is expressed by the following [Equation 11] assuming a normal distribution.
一方、Pr(L(j)|q)は、L(j)をz(j)で置き換えて、次の[数12]の式で近似することができる。
ここで、zk(j)は、距離センサを搭載した移動体、例えば移動ロボットの位置tr、姿勢Rrに依存している。実際は、三次元LRFのセンサ座標系の中心から見たqの向きと計測点L(j)の向きとは図21に示すように異なるため、誤差共分散行列Σz(j)もqの向きに合わせて回転変換する必要があるが、対応付けられた環境地図上の点iから大きく離れたところにあるqの存在確率は低いため、十分な精度で近似できると考えられる。よって、Pr(L(j)|m(i),Σm(i))は、次の[数13]の式で表すことができる。 Here, z k (j) depends on the position t r and posture R r of a mobile body equipped with a distance sensor, for example, a mobile robot. Actually, since the direction of q viewed from the center of the sensor coordinate system of the three-dimensional LRF and the direction of the measurement point L (j) are different as shown in FIG. 21, the error covariance matrix Σ z (j) is also the direction of q. However, since the existence probability of q located far away from the point i on the associated environmental map is low, it can be approximated with sufficient accuracy. Therefore, Pr (L (j) | m (i), Σ m (i)) can be expressed by the following equation [Equation 13].
簡単な計算によって、次の[数14]の式を得る。 By the simple calculation, the following equation (14) is obtained.
ただし、α(j),β(j)は次の[数15]の式で表すことができる。 However, α (j) and β (j) can be expressed by the following equation (15).
従って、モデル点群の点iと計測点群の点jとの対応付けの一致度を表す評価関数EM(i,j)は、平均値m(i)、誤差共分散行列Σm(i)+Σz(j)の正規分布において、z(j)が得られる確率値に近似できる。この評価関数を用いることによって、環境地図、計測データ双方の誤差を考慮した対応付けが可能となる。 Accordingly, the evaluation function EM (i, j) representing the degree of matching between the model point cloud point i and the measurement point cloud point j has an average value m (i) and an error covariance matrix Σ m (i). In the normal distribution of + Σ z (j), it can be approximated to a probability value for obtaining z (j). By using this evaluation function, it is possible to associate the environment map and the measurement data in consideration of errors.
計測点と環境地図(即ち、環境モデル)との対応付けについて補足説明をする。上記実施例では、誤差分布を考慮した統計的な評価関数を用いるため、評価関数の値を求めなければ対応点を定めることができない。そこで、環境地図上のモデル点群の中で対応付けする候補を予め絞り込み、その候補の中から評価関数の値を基に対応点を求める。具体的には、以下のように定めることができる。
(1)対象とする計測点jの誤差共分散行列ΣL(j)の範囲(例えば標準偏差の3倍の範囲)と交わる最上位のボクセルとそのボクセルに隣接している最上位のボクセルを求め、下層のボクセルも含めこれらのボクセル内に存在する代表点を対応点の候補とする。ボクセルが階層構造となっているため、この候補点の探索には計算コストはほとんどかからない。このとき、候補となる代表点がない場合には、対応点がないものとみなす。隣接するボクセルも候補に加える理由は、ボクセル内の代表点の位置によっては、誤差共分散行列の範囲が隣接するボクセルまではみ出すことがあるからである。
(2)候補となるボクセルの代表点iと誤差共分散行列を用いて、評価関数EM(i,j)の値を求める。
(3)評価関数EM(i,j)の値が最も大きい代表点iを対応点とする。ただし、最も大きい評価関数の値がある閾値未満の場合には、対応点がないものとみなす。
本実施例では、対応付けの評価関数EM(i,j)として、尤度に基づいた式を採用しており、対応点の有無に関して統計的に明確な判断尺度があるため、対応点が存在しないと考えられる場合においても無理に対応付けを行うようなことはない。なお、対応点がない場合には、対象となる計測点はこれまで未計測の部分に相当する点であると解釈し、環境地図に追加する。
Supplementary explanation will be given for the association between the measurement points and the environment map (that is, the environment model). In the above embodiment, since a statistical evaluation function considering the error distribution is used, the corresponding point cannot be determined unless the value of the evaluation function is obtained. Therefore, candidates to be associated in the model point group on the environment map are narrowed down in advance, and corresponding points are obtained from the candidates based on the value of the evaluation function. Specifically, it can be determined as follows.
(1) The highest voxel that intersects the range of the error covariance matrix Σ L (j) of the target measurement point j (for example, a range of three times the standard deviation) and the highest voxel adjacent to the voxel The representative points existing in these voxels including the lower-layer voxels are used as candidates for corresponding points. Since the voxels have a hierarchical structure, the search for this candidate point requires little computational cost. At this time, if there is no candidate representative point, it is considered that there is no corresponding point. The reason why adjacent voxels are also added to the candidates is that the range of the error covariance matrix may extend to adjacent voxels depending on the position of the representative point in the voxel.
(2) The value of the evaluation function EM (i, j) is obtained using the representative point i of the candidate voxel and the error covariance matrix.
(3) The representative point i having the largest value of the evaluation function EM (i, j) is taken as the corresponding point. However, when the value of the largest evaluation function is less than a certain threshold value, it is considered that there is no corresponding point.
In this embodiment, as an evaluation function EM (i, j) for association, an expression based on likelihood is adopted, and there is a statistically clear determination scale regarding the presence or absence of corresponding points, so there is a corresponding point. Even in the case where it is considered not to be performed, the association is not forcibly performed. If there is no corresponding point, the target measurement point is interpreted as a point corresponding to an unmeasured part so far and added to the environment map.
実施形態として、三次元形状の計測について説明したが、二次元形状を三次元形状の特別な場合として見ることにより、二次元形状も同様に計測できる。 Although the measurement of the three-dimensional shape has been described as an embodiment, the two-dimensional shape can be similarly measured by looking at the two-dimensional shape as a special case of the three-dimensional shape.
なお、本発明は、上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々に変更することができることは勿論である。 In addition, this invention is not limited to embodiment mentioned above, Of course, it can change variously in the range which does not deviate from the summary of this invention.
40 物体認識装置
41 3Dセンサ
42 物体認識処理部
43 モデルデータベース部
45 ロボット制御部
46 ロボット
40
Claims (21)
新たな計測位置から計測して得られた三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶する第1マッチングステップとを実施し、
先の計測位置から計測して得られた前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
先の計測位置から計測して得られた前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、
新たな計測データおよび誤差分布を回転及び並進させ、または
新たな計測位置に対する環境モデルを回転及び並進させ、
近接する誤差分布間の距離に関する評価値が最小になるように位置合わせする精密合わせステップと、
前記精密合わせステップにおける回転量及び並進量から自己位置を同定する自己位置同定ステップとを実施し、
更に複数の計測位置から計測された複数の計測データおよび複数の誤差分布を統合する統合ステップと、
統合して得られた計測データおよび誤差分布に対し、モデルデータによって表される被計測物を回転および並進させ、前記計測データと前記モデルデータを構成する要素との距離に関する評価値が最小になるようにする位置合わせをする第2マッチングステップとを実施する、ことを特徴とする物体認識方法。 An object recognition method for recognizing the position and orientation of an object,
A data input step for inputting a coordinate value on a three-dimensional shape obtained by measuring from a new measurement position to a computer;
A model construction step of dividing the spatial region where the three-dimensional shape exists into a plurality of voxels composed of rectangular parallelepipeds whose boundary surfaces are orthogonal to each other, and constructing an environment model that stores each voxel position;
Performing a first matching step of setting and storing a representative point and its error distribution inside the voxel corresponding to the coordinate value;
When there is no data of the three-dimensional shape obtained by measuring from the previous measurement position, the new measurement position is identified as the self-position,
When there is data of the three-dimensional shape obtained by measuring from the previous measurement position,
For the environmental model for the previous measurement position,
Rotate and translate new measurement data and error distribution, or rotate and translate environment model for new measurement position,
A precision alignment step for aligning the evaluation values regarding the distance between adjacent error distributions to a minimum;
Performing a self-position identification step of identifying a self-position from the rotation amount and the translation amount in the precision alignment step;
Furthermore, an integration step for integrating a plurality of measurement data and a plurality of error distributions measured from a plurality of measurement positions;
For the measurement data and error distribution obtained by the integration, the measurement object represented by the model data is rotated and translated, and the evaluation value related to the distance between the measurement data and the elements constituting the model data is minimized. And a second matching step for performing positioning so as to achieve the object recognition method.
新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離の総和が最小になるように、または
新たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボクセル間の距離の総和が最小になるように、
位置合わせする粗合わせステップを有する、ことを特徴とする請求項1に記載の物体認識方法。 Before the precision alignment step, for the environmental model for the previous measurement position,
Rotate and translate the new measurement data and error distribution so that the total sum of the distance between the measurement data and the error distribution and the voxels having the representative points that are close to each other is minimized, or the environment model for the new measurement position is rotated and translated. Translate so that the sum of the distances between voxels with representative points is minimized,
The object recognition method according to claim 1, further comprising a rough alignment step of aligning.
新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値の総和が最大になるように、または
新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差の総和が最小になるように、
位置合わせする粗合わせステップを有する、ことを特徴とする請求項1に記載の物体認識方法。 Before the precision alignment step, for the environmental model for the previous measurement position,
Rotate and translate the new measurement data and error distribution so that the sum of the probability values of voxels with representative points close to the measurement data and error distribution is maximized, or rotate and translate the environment model for the new measurement position So that the sum of the difference in probability values of neighboring voxels is minimized,
The object recognition method according to claim 1, further comprising a rough alignment step of aligning.
現在の計測位置を過去の計測位置の変化から推定し、または
現在の計測位置を取得可能なセンサにより取得し、または
計測データの距離値だけでなく反射強度値を利用し、
照合する範囲を限定する探索範囲限定ステップを有する、ことを特徴とする請求項1に記載の物体認識方法。 After the data input step,
Estimate the current measurement position from past measurement position changes, or acquire the current measurement position with a sensor that can acquire it, or use the reflection intensity value as well as the distance value of the measurement data,
The object recognition method according to claim 1, further comprising a search range limiting step for limiting a range to be collated.
原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定もしくは消去する、ことを特徴とする請求項1に記載の物体認識方法。 A model update step of updating the environmental model after the self-position identification step, and in the model update step, search for a voxel corresponding to a coordinate value of a newly input measurement point;
2. The object recognition method according to claim 1, wherein an object distribution between the origin and the point to be measured is assumed to be absent, and the representative point and the error distribution in the voxel located between them are reset or deleted.
該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する、ことを特徴とする請求項1に記載の物体認識方法。 A model update step of updating the environmental model after the self-position identification step, and in the model update step, search for a voxel corresponding to a coordinate value of a newly input measurement point;
2. The object recognition method according to claim 1, wherein when there is no representative point in the voxel, the coordinate value and the error distribution are set as the coordinate value and the error distribution of the representative point.
前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことを特徴とする請求項1に記載の物体認識方法。 A model update step of updating the environmental model after the self-position identification step, and in the model update step, search for a voxel corresponding to a coordinate value of a newly input measurement point;
When there is a representative point already set in the voxel, the newly acquired error distribution is compared with the already set error distribution in the voxel,
If the error distributions overlap each other, reset a new error distribution and a new representative point from both error distributions,
The voxel is further divided into a plurality of voxels hierarchically so that only a single representative point exists in a single voxel when error distributions do not overlap each other. Item 2. The object recognition method according to Item 1.
該他のボクセル内に代表点がない場合に、該新たな誤差分布と該新たな代表点を該他のボクセルの内部に設定し、
該他のボクセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボクセル内の誤差分布とを比較し、(A)誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、(B)誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことを特徴とする請求項1に記載の自己位置同定方法。 In the model update step, the newly acquired error distribution is compared with the already set error distribution in the voxel, and when the error distributions overlap each other, a new error distribution and a new representative point are obtained from both error distributions. As a result of resetting, when a new representative point moves into another voxel,
If there is no representative point in the other voxel, the new error distribution and the new representative point are set inside the other voxel,
When there is a representative point already set in the other voxel, the new error distribution is compared with the already set error distribution in the other voxel, and (A) when the error distributions overlap each other, From both error distributions, or from both error distributions and the coordinate values of the representative points already set in the voxel and the newly inputted measurement point, a new error distribution and a new representative point are reset, (B) The voxel is further divided into a plurality of voxels hierarchically so that only a single representative point exists in a single voxel when error distributions do not overlap each other. Item 2. The self-position identification method according to Item 1.
該モデル更新ステップにおいて、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する、ことを特徴とする請求項1に記載の物体認識方法。 A model update step of updating the environmental model after the self-location identification step;
In the model updating step, a new representative point and error distribution are obtained by a Kalman filter from the newly inputted coordinate value of the measured point and its error distribution, and the representative point in the already set voxel and its error distribution. The object recognition method according to claim 1, wherein the object recognition method is reset.
新たな計測データおよび誤差分布を回転及び並進させ、または
新たな計測位置に対する環境モデルを回転及び並進させて位置合わせする、ことを特徴とする請求項1に記載の物体認識方法。 In the precise alignment step, instead of performing alignment so that the evaluation value related to the distance between the adjacent error distributions is minimized, the evaluation value related to the degree of coincidence determined by the maximum likelihood estimation value based on the adjacent error distributions is obtained. To maximize the environmental model for the previous measurement position,
The object recognition method according to claim 1, wherein the new measurement data and the error distribution are rotated and translated, or the environmental model for the new measurement position is rotated and translated for alignment.
前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことを特徴とする請求項1に記載の物体認識方法。 In the integration step, search for the voxel corresponding to the coordinate value of the newly input measurement point,
When there is a representative point already set in the voxel, the newly acquired error distribution is compared with the already set error distribution in the voxel,
If the error distributions overlap each other, reset a new error distribution and a new representative point from both error distributions,
The voxel is further divided into a plurality of voxels hierarchically so that only a single representative point exists in a single voxel when error distributions do not overlap each other. Item 2. The object recognition method according to Item 1.
該他のボクセル内に代表点がない場合に、該新たな誤差分布と該新たな代表点を該他のボクセルの内部に設定し、
該他のボクセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボクセル内の誤差分布とを比較し、(A)誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、(B)誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことを特徴とする請求項18に記載の物体認識方法。 In the integration step, as a result of resetting a new error distribution and a new representative point from both error distributions, when a new representative point moves into another voxel,
If there is no representative point in the other voxel, the new error distribution and the new representative point are set inside the other voxel,
When there is a representative point already set in the other voxel, the new error distribution is compared with the already set error distribution in the other voxel, and (A) when the error distributions overlap each other, From both error distributions, or from both error distributions and the coordinate values of the representative points already set in the voxel and the newly inputted measurement point, a new error distribution and a new representative point are reset, (B) The voxel is further divided into a plurality of voxels hierarchically so that only a single representative point exists in a single voxel when error distributions do not overlap each other. Item 18. The object recognition method according to Item 18.
新たな計測位置から計測して得られた三次元形状上の座標値をコンピュータに入力するデータ入力部と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築部と、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶する第1マッチング部と、
先の計測位置から計測して得られた前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
先の計測位置から計測して得られた前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、(A)新たな計測データおよび誤差分布を回転及び並進させ、または(B)新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値が最小になるように位置合わせし、位置合わせに必要な回転量及び並進量から自己位置を同定する自己位置同定部と、
複数の計測位置から計測された複数の計測データおよび複数の誤差分布を統合する統合部と、
統合して得られた計測データおよび誤差分布に対し、モデルデータによって多面体として表される被計測物を回転および並進させ、前記計測データと前記多面体を構成する平面との距離に関する評価値が最小になるようにする位置合わせをする第2マッチング部とを具備する、ことを特徴とする物体認識装置。 An object recognition device for recognizing the position and orientation of an object,
A data input unit for inputting coordinate values on a three-dimensional shape obtained by measuring from a new measurement position to a computer;
A model construction unit that divides the spatial region in which the three-dimensional shape exists into a plurality of voxels made of rectangular parallelepipeds whose boundary surfaces are orthogonal to each other, and constructs an environment model that stores each voxel position;
A first matching unit that sets and stores a representative point and its error distribution inside a voxel corresponding to the coordinate value;
When there is no data of the three-dimensional shape obtained by measuring from the previous measurement position, the new measurement position is identified as the self-position,
When there is data of the three-dimensional shape obtained by measuring from the previous measurement position,
The distance between adjacent error distributions by (A) rotating and translating new measurement data and error distribution with respect to the environmental model for the previous measurement position, or (B) rotating and translating environmental model for the new measurement position. A self-position identifying unit that aligns the evaluation value with respect to the minimum and identifies the self-position from the rotation amount and the translation amount necessary for the alignment,
An integration unit that integrates a plurality of measurement data and a plurality of error distributions measured from a plurality of measurement positions;
For the measurement data and error distribution obtained by the integration, the measurement object represented as a polyhedron by the model data is rotated and translated, and the evaluation value related to the distance between the measurement data and the plane constituting the polyhedron is minimized. An object recognition apparatus comprising: a second matching unit configured to perform alignment.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2007158282A JP5380792B2 (en) | 2007-06-15 | 2007-06-15 | Object recognition method and apparatus |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2007158282A JP5380792B2 (en) | 2007-06-15 | 2007-06-15 | Object recognition method and apparatus |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2008309671A true JP2008309671A (en) | 2008-12-25 |
JP5380792B2 JP5380792B2 (en) | 2014-01-08 |
Family
ID=40237402
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2007158282A Expired - Fee Related JP5380792B2 (en) | 2007-06-15 | 2007-06-15 | Object recognition method and apparatus |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5380792B2 (en) |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010256209A (en) * | 2009-04-27 | 2010-11-11 | Ihi Corp | Device and method for recognizing baglike workpiece |
JP2013068417A (en) * | 2011-09-20 | 2013-04-18 | Dainippon Screen Mfg Co Ltd | Three-dimensional position/posture recognition device, industrial robot, three-dimensional position/posture recognition method, program and storing medium |
JP2014095553A (en) * | 2012-11-07 | 2014-05-22 | Nippon Telegr & Teleph Corp <Ntt> | Camera pause estimation device and camera pause estimation program |
WO2016092749A1 (en) * | 2014-12-09 | 2016-06-16 | Canon Kabushiki Kaisha | Information processing apparatus, information processing method, and program |
US10350752B2 (en) | 2017-03-13 | 2019-07-16 | Fanuc Corporation | Robot system, measurement data processing device and measurement data processing method for picking out workpiece using measurement data corrected by means of machine learning |
DE112017006839T5 (en) | 2017-01-16 | 2019-09-26 | Fujitsu Limited | Information processing apparatus, database creation apparatus, method, program and storage medium |
WO2019230285A1 (en) * | 2018-05-29 | 2019-12-05 | オムロン株式会社 | Simulation device, simulation method, and program |
JP2020085523A (en) * | 2018-11-19 | 2020-06-04 | 株式会社Ihi | Monitoring system and processor |
JP2021032716A (en) * | 2019-08-26 | 2021-03-01 | 株式会社トプコン | Survey data processor, survey data processing method, and program for survey data processing |
GB2596439A (en) * | 2019-03-14 | 2021-12-29 | Tokyo Seimitsu Co Ltd | Three-dimensional measuring system, and three-dimensional measuring method |
WO2022180790A1 (en) * | 2021-02-26 | 2022-09-01 | パイオニア株式会社 | Information processing device, control method, program, and storage medium |
CN117115487A (en) * | 2023-10-23 | 2023-11-24 | 睿励科学仪器(上海)有限公司 | Template matching method, template matching system and storage medium |
Citations (21)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0618221A (en) * | 1991-03-11 | 1994-01-25 | Agency Of Ind Science & Technol | Integration of multi-viewpoint distance data |
JPH09231370A (en) * | 1996-02-21 | 1997-09-05 | Canon Inc | Picture information input device |
JPH09229648A (en) * | 1996-02-21 | 1997-09-05 | Canon Inc | Input/output method and device for image information |
JP2000113193A (en) * | 1998-10-08 | 2000-04-21 | Minolta Co Ltd | Synthesizing method for multi-viewpoint three- dimensional data and recording medium |
JP2001022939A (en) * | 1999-07-12 | 2001-01-26 | Nippon Telegr & Teleph Corp <Ntt> | Method and device for extracting three-dimensional information and recording medium with three- dimensional information extraction program recorded therein |
JP2001236522A (en) * | 1999-12-17 | 2001-08-31 | Canon Inc | Image processor |
JP2002304626A (en) * | 2001-02-02 | 2002-10-18 | Matsushita Electric Ind Co Ltd | Data classifying device and body recognizing device |
JP2003296755A (en) * | 2001-11-27 | 2003-10-17 | Samsung Electronics Co Ltd | Node structure for representing three-dimensional object based on depth image |
JP2004005373A (en) * | 2001-11-27 | 2004-01-08 | Samsung Electronics Co Ltd | Node structure for representing three-dimensional object based on depth image |
JP2004132944A (en) * | 2002-08-16 | 2004-04-30 | Foundation For The Promotion Of Industrial Science | System and method for evaluating manufacturing error and program for the same |
JP2004521423A (en) * | 2001-03-12 | 2004-07-15 | コーニンクレッカ フィリップス エレクトロニクス エヌ ヴィ | Generation of 3D representation from many images using octree |
JP2005037379A (en) * | 2003-06-30 | 2005-02-10 | Sanyo Electric Co Ltd | Three-dimensional modeling method and device |
WO2007069726A1 (en) * | 2005-12-16 | 2007-06-21 | Ihi Corporation | Self-position identifying method and device, and three-dimensional shape measurement method and device |
WO2007069724A1 (en) * | 2005-12-16 | 2007-06-21 | Ihi Corporation | Three-dimensional shape data aligning method and device |
WO2007069721A1 (en) * | 2005-12-16 | 2007-06-21 | Ihi Corporation | Three-dimensional shape data storing/displaying method and device, and three-dimensional shape measuring method and device |
JP2008309603A (en) * | 2007-06-14 | 2008-12-25 | Ihi Corp | Fluorescent flaw detection method and its device |
JP2008309533A (en) * | 2007-06-13 | 2008-12-25 | Ihi Corp | Method and apparatus for measuring vehicle shape |
JP2009128075A (en) * | 2007-11-20 | 2009-06-11 | Ihi Corp | Object recognition method |
JP2009129189A (en) * | 2007-11-22 | 2009-06-11 | Ihi Corp | Object recognition method |
JP2009128097A (en) * | 2007-11-21 | 2009-06-11 | Ihi Corp | Method and device for recognizing flash or deficiency, and deflashing method and device |
JP2009128192A (en) * | 2007-11-22 | 2009-06-11 | Ihi Corp | Object recognition device and robot device |
-
2007
- 2007-06-15 JP JP2007158282A patent/JP5380792B2/en not_active Expired - Fee Related
Patent Citations (21)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0618221A (en) * | 1991-03-11 | 1994-01-25 | Agency Of Ind Science & Technol | Integration of multi-viewpoint distance data |
JPH09231370A (en) * | 1996-02-21 | 1997-09-05 | Canon Inc | Picture information input device |
JPH09229648A (en) * | 1996-02-21 | 1997-09-05 | Canon Inc | Input/output method and device for image information |
JP2000113193A (en) * | 1998-10-08 | 2000-04-21 | Minolta Co Ltd | Synthesizing method for multi-viewpoint three- dimensional data and recording medium |
JP2001022939A (en) * | 1999-07-12 | 2001-01-26 | Nippon Telegr & Teleph Corp <Ntt> | Method and device for extracting three-dimensional information and recording medium with three- dimensional information extraction program recorded therein |
JP2001236522A (en) * | 1999-12-17 | 2001-08-31 | Canon Inc | Image processor |
JP2002304626A (en) * | 2001-02-02 | 2002-10-18 | Matsushita Electric Ind Co Ltd | Data classifying device and body recognizing device |
JP2004521423A (en) * | 2001-03-12 | 2004-07-15 | コーニンクレッカ フィリップス エレクトロニクス エヌ ヴィ | Generation of 3D representation from many images using octree |
JP2004005373A (en) * | 2001-11-27 | 2004-01-08 | Samsung Electronics Co Ltd | Node structure for representing three-dimensional object based on depth image |
JP2003296755A (en) * | 2001-11-27 | 2003-10-17 | Samsung Electronics Co Ltd | Node structure for representing three-dimensional object based on depth image |
JP2004132944A (en) * | 2002-08-16 | 2004-04-30 | Foundation For The Promotion Of Industrial Science | System and method for evaluating manufacturing error and program for the same |
JP2005037379A (en) * | 2003-06-30 | 2005-02-10 | Sanyo Electric Co Ltd | Three-dimensional modeling method and device |
WO2007069721A1 (en) * | 2005-12-16 | 2007-06-21 | Ihi Corporation | Three-dimensional shape data storing/displaying method and device, and three-dimensional shape measuring method and device |
WO2007069724A1 (en) * | 2005-12-16 | 2007-06-21 | Ihi Corporation | Three-dimensional shape data aligning method and device |
WO2007069726A1 (en) * | 2005-12-16 | 2007-06-21 | Ihi Corporation | Self-position identifying method and device, and three-dimensional shape measurement method and device |
JP2008309533A (en) * | 2007-06-13 | 2008-12-25 | Ihi Corp | Method and apparatus for measuring vehicle shape |
JP2008309603A (en) * | 2007-06-14 | 2008-12-25 | Ihi Corp | Fluorescent flaw detection method and its device |
JP2009128075A (en) * | 2007-11-20 | 2009-06-11 | Ihi Corp | Object recognition method |
JP2009128097A (en) * | 2007-11-21 | 2009-06-11 | Ihi Corp | Method and device for recognizing flash or deficiency, and deflashing method and device |
JP2009129189A (en) * | 2007-11-22 | 2009-06-11 | Ihi Corp | Object recognition method |
JP2009128192A (en) * | 2007-11-22 | 2009-06-11 | Ihi Corp | Object recognition device and robot device |
Cited By (23)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010256209A (en) * | 2009-04-27 | 2010-11-11 | Ihi Corp | Device and method for recognizing baglike workpiece |
JP2013068417A (en) * | 2011-09-20 | 2013-04-18 | Dainippon Screen Mfg Co Ltd | Three-dimensional position/posture recognition device, industrial robot, three-dimensional position/posture recognition method, program and storing medium |
JP2014095553A (en) * | 2012-11-07 | 2014-05-22 | Nippon Telegr & Teleph Corp <Ntt> | Camera pause estimation device and camera pause estimation program |
WO2016092749A1 (en) * | 2014-12-09 | 2016-06-16 | Canon Kabushiki Kaisha | Information processing apparatus, information processing method, and program |
JP2016109630A (en) * | 2014-12-09 | 2016-06-20 | キヤノン株式会社 | Information processing apparatus, information processing method, and program |
US10335963B2 (en) | 2014-12-09 | 2019-07-02 | Canon Kabushiki Kaisha | Information processing apparatus, information processing method, and program |
DE112017006839T5 (en) | 2017-01-16 | 2019-09-26 | Fujitsu Limited | Information processing apparatus, database creation apparatus, method, program and storage medium |
US11506755B2 (en) | 2017-01-16 | 2022-11-22 | Fujitsu Limited | Recording medium recording information processing program, information processing apparatus, and information processing method |
US10350752B2 (en) | 2017-03-13 | 2019-07-16 | Fanuc Corporation | Robot system, measurement data processing device and measurement data processing method for picking out workpiece using measurement data corrected by means of machine learning |
DE102018001781B4 (en) | 2017-03-13 | 2019-07-18 | Fanuc Corporation | A robot system, measurement data processing apparatus, and measurement data processing method for picking up a workpiece using measurement data corrected by machine learning |
JP2019207153A (en) * | 2018-05-29 | 2019-12-05 | オムロン株式会社 | Simulation device, simulation method, and program |
JP7099052B2 (en) | 2018-05-29 | 2022-07-12 | オムロン株式会社 | Simulation equipment, simulation methods and programs |
WO2019230285A1 (en) * | 2018-05-29 | 2019-12-05 | オムロン株式会社 | Simulation device, simulation method, and program |
JP2020085523A (en) * | 2018-11-19 | 2020-06-04 | 株式会社Ihi | Monitoring system and processor |
JP7205181B2 (en) | 2018-11-19 | 2023-01-17 | 株式会社Ihi | Monitoring system and processing equipment |
GB2596439A (en) * | 2019-03-14 | 2021-12-29 | Tokyo Seimitsu Co Ltd | Three-dimensional measuring system, and three-dimensional measuring method |
US11365959B2 (en) | 2019-03-14 | 2022-06-21 | Tokyo Seimitsu Co., Ltd. | Three-dimensional measuring system, and three-dimensional measuring method |
GB2596439B (en) * | 2019-03-14 | 2023-04-19 | Tokyo Seimitsu Co Ltd | Three-dimensional measuring system, and three-dimensional measuring method |
JP2021032716A (en) * | 2019-08-26 | 2021-03-01 | 株式会社トプコン | Survey data processor, survey data processing method, and program for survey data processing |
JP7300930B2 (en) | 2019-08-26 | 2023-06-30 | 株式会社トプコン | Survey data processing device, survey data processing method and program for survey data processing |
WO2022180790A1 (en) * | 2021-02-26 | 2022-09-01 | パイオニア株式会社 | Information processing device, control method, program, and storage medium |
CN117115487A (en) * | 2023-10-23 | 2023-11-24 | 睿励科学仪器(上海)有限公司 | Template matching method, template matching system and storage medium |
CN117115487B (en) * | 2023-10-23 | 2024-03-08 | 睿励科学仪器(上海)有限公司 | Template matching method, template matching system and storage medium |
Also Published As
Publication number | Publication date |
---|---|
JP5380792B2 (en) | 2014-01-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP4650752B2 (en) | Self-position identification method and apparatus and three-dimensional shape measurement method and apparatus | |
JP5380792B2 (en) | Object recognition method and apparatus | |
JP4650751B2 (en) | Method and apparatus for aligning 3D shape data | |
JP4650750B2 (en) | 3D shape data storage and display method and apparatus, and 3D shape measurement method and apparatus | |
US11568559B2 (en) | Localization of a surveying instrument | |
JP5303873B2 (en) | Vehicle shape measuring method and apparatus | |
US8792726B2 (en) | Geometric feature extracting device, geometric feature extracting method, storage medium, three-dimensional measurement apparatus, and object recognition apparatus | |
CN113052908B (en) | Mobile robot pose estimation algorithm based on multi-sensor data fusion | |
JP5759161B2 (en) | Object recognition device, object recognition method, learning device, learning method, program, and information processing system | |
JP6736257B2 (en) | Information processing device, information processing method, and program | |
EP3078935A1 (en) | Method and device for real-time mapping and localization | |
US10288425B2 (en) | Generation of map data | |
US11867818B2 (en) | Capturing environmental scans using landmarks based on semantic features | |
JP5246468B2 (en) | Deburring or defect recognition method and apparatus, and deburring method and apparatus | |
JP6673504B2 (en) | Information processing device, database generation device, method, program, and storage medium | |
US20230316567A1 (en) | Localization of a surveying instrument | |
KR102130687B1 (en) | System for information fusion among multiple sensor platforms | |
JP7464134B2 (en) | Shape modeling device and shape modeling method | |
US11669988B1 (en) | System and method for three-dimensional box segmentation and measurement | |
Hu et al. | Efficient Visual-Inertial navigation with point-plane map | |
Mützel et al. | Geometric features for robust registration of point clouds | |
Kotthäuser et al. | Triangulation-based plane extraction for 3D point clouds | |
Forsman et al. | Feature based registration of range images for mapping of natural outdoor environments | |
Akca et al. | Fast quality control of 3D city models | |
Sinivaara | Simultaneous Localisation and Mapping Using Autonomous Target Detection and Recognition |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20100426 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20130521 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20130719 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20130903 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20130916 |
|
R151 | Written notification of patent or utility model registration |
Ref document number: 5380792 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
LAPS | Cancellation because of no payment of annual fees |