Summary of the invention
The object of the invention is the above-mentioned deficiency overcoming prior art, and propose a kind of camera self-orientation method based on robot motion's parameter, the method mainly overcomes the shortcoming of slow, the flexible difference of traditional orientation method speed, can also reach the feature that matching rate is high, accuracy is high simultaneously.Realize the object of the invention technical scheme, comprise the following steps:
(1) utilize camera to remain unchanged relative to robot end's tool location, hand and eye calibrating is carried out to robot;
(2) trick relation and robot motion's parameter is utilized, by the position unification of robot in robot basis coordinates;
(3) position in basis coordinates system of robot is tied up to, to camera self-orientation according to camera coordinates;
(4) epipolar-line constraint coupling is adopted to realize the coupling of gauge point;
(5) feature based vector carries out ambiguity matching and correlation.
In step (2), camera is arranged on robot end and moves along with robot motion, and camera remains unchanged relative to the position of robot end's instrument; Basic ideas are that control moves to diverse location, make camera carry out imaging from different azimuth to calibrated reference space; Then
P
c1=CP
c2(1)
P
c1=XP
t1(2)
P
t1=DP
t2(3)
P
c2=XP
t2(4)
Obtained by formula (1) and (4)
P
c1=CXP
t2(5)
Obtained by formula (2) and (3)
P
c1=XDP
t2(6)
Simultaneous (5) obtains with (6)
CX=XD(7)
Mobile robot is to diverse location, and obtain multiple group type (7) equation of constraint, simultaneous can try to achieve X.
As shown in Fig. 3 (b), C
calfor scaling board coordinate system; C
cam1and C
cam2for camera coordinates system; C
tool1with C
tool2for tool coordinates system; Use scaling board to C
cam1and C
cam2camera Calibration on two positions calculates inside and outside parameter, obtains three matrixes; Robot motion's parameter can be read by controller again, therefore C
tool1and C
tool2be known parameters, can matrix D be obtained; The relative position X of tool coordinates system and camera coordinates system remains unchanged; 1 P is had at coordinate system C in hypothesis space
cam1, C
cam2, C
tool1and C
tool2in coordinate be respectively P
c1, P
c2, P
t1and P
t2.
In step (5), utilize the close matrix of self property structure of point set, point set is transformed into higher dimensional space from two-dimensional space and mates again; Suppose that two groups of points with ambiguity coupling are respectively I
i(i=1,2 ..., m) and J
j(j=1,2 ..., n), set up metric matrix:
H
ij=exp(-r
ij 2/2σ
2)
R in above formula
ijan I
iwith a J
jbetween distance, σ is calculated as follows:
Wherein M is number a little, carries out SVD decomposition to matrix H:
H
m=V
mD
mV
m T
Wherein V is the unit orthogonal matrix of m dimension; Obtain the m-dimensional space description that V matrix just obtains this point set, m column vector E of matrix V
ithe base of this m-dimensional space, m the row vector then F of V
ithe coordinate of this m point under this group m ties up base respectively; H can be obtained from first point set
1=V
1d
1v
1 t, can H be obtained from second point set equally
2=V
2d
2v
2 t, remember that two groups of points are described as F under new base respectively
i, 1and F
j, 2; If m with n is not identical, choose | an element before m-n|, set up trip current Z:
Z
ij=||F
i,1-F
j,2||
2
Decision criteria is: only have and work as Z
ijwhen being the least member of the i-th row and jth row in matrix Z, just think an I
iwith a J
jthat correct coupling is right.
The present invention compared with prior art tool has the following advantages:
1. this invention is without the need to pasting coded markings point, avoids using controlling filed and directed target, replaces hand labor, improve the speed of camera orientation, improve the automaticity of measurement with robot;
2. this invention overcomes the shortcoming of slow, the flexible difference of traditional orientation method speed;
3. this invention can reach the feature that matching rate is high, accuracy is high, and is applicable to the online precision measurement in intelligence manufacture.
Accompanying drawing explanation
Fig. 1: process flow diagram of the present invention.
Fig. 2: travelling shot is measured.(a) travelling shot instrumentation plan, (b) pin-hole imaging model.Collinearity condition equation describes picture point m
u, projected centre point O
c, and object space point M be in equation on same straight line.O
c-X
cy
cz
cfor camera coordinates system, O
w-X
wy
wz
wfor world coordinate system, o
0-uv is image coordinate system, o
1-xy is plane of delineation coordinate system.
Fig. 3: hand and eye calibrating.(a) hand and eye calibrating schematic diagram, (b) each coordinate system relative position.As shown in (b), A, B, C, D, X are 4 × 4 matrixes, and the relative orientation be used between expression two coordinate systems, can be analyzed to translation vector and rotation matrix, C
calfor scaling board coordinate system, C
cam1and C
cam2for camera coordinates system, C
tool1with C
tool2for tool coordinates system.Use scaling board to C
cam1and C
cam2camera Calibration on two positions calculates inside and outside parameter, obtains three matrixes.Robot motion's parameter can be read by controller again, therefore C
tool1and C
tool2be known parameters, can matrix D be obtained.The relative position X of tool coordinates system and camera coordinates system remains unchanged.1 P is had at coordinate system C in hypothesis space
cam1, C
cam2, C
tool1and C
tool2in coordinate be respectively P
c1, P
c2, P
t1and P
t2.
Fig. 4: Epipolar geometric constraint.A () pole geometric match, (b) ambiguity is mated.As shown in (a), two as plane be respectively I, I ', m and m ' is picture point corresponding to spatial point P, the photocentre that C and C ' is camera, and the line of photocentre is called baseline, baseline and intersection point e, the e ' be called limit as plane; Baseline CC ' is called polar plane with two projection ray CP, C ' plane that P is formed, polar plane and the intersection l as plane
m ', l
m' be called polar curve; M and m ' is corresponding point, l
m' for m picture planar I ' on polar curve, obvious m ' is at l
m' on.Equally, m is inevitable at polar curve l
m 'on.
Fig. 5: search for match point on corresponding polar curve.
Fig. 6: the correct coupling realizing point set, demonstrates the accuracy of the directed result of camera.
Fig. 7: the correct matching double points 5,6 and 7,8 that random selecting two is adjacent, its objective is that the architectural feature making point set is more obvious.Matching double points correct in addition participates in correcting, and can guarantee the accuracy corrected.If 5,6 and 7,8 still to mate after correcting, then think and correct successfully, otherwise think and correct unsuccessfully, again choose matching double points, until correct successfully.
Fig. 8: ambiguity matching and correlation result.
Embodiment
Process flow diagram of the present invention as shown in Figure 1, first carries out hand and eye calibrating to robot; Then trick relation and robot motion's parameter is utilized, by the unification of camera relative position in basis coordinates system of robot; Finally realize the self-orientation of camera.In addition, for eliminating the coupling ambiguity produced because polar plane is identical, a kind of ambiguity correct methods matching of feature based vector is also proposed.Below in conjunction with accompanying drawing, the specific implementation process of technical solution of the present invention is illustrated.
1. travelling shot measuring principle;
As shown in Fig. 2 (a), mobile camera carries out imaging from different azimuth to measured object, set up the restriction relation of different erect-position same place based on collinearity condition equation, eventually pass the three-dimensional information that bundle adjustment recovers gauge point, realize the measurement of key position.In order to improve Automatic survey, speed and flexibility, camera being fixed in robot, moving camera by planning robot's motion path.Gauge point is launched by laser projecting apparatus in addition, avoids contacting measured object, can realize 100% noncontacting measurement.
As shown in Fig. 2 (b), collinearity condition equation describes picture point m
u, projected centre point O
c, and object space point M be in equation on same straight line.O
c-X
cy
cz
cfor camera coordinates system, O
w-X
wy
wz
wfor world coordinate system, o
0-uv is image coordinate system, o
1-xy is plane of delineation coordinate system.It is easy to show that
Consider lens distortion, distortion correction need be carried out to picture point.
In formula, (x
u, y
u) be ideal image point coordinate, (x
d, y
d) be picpointed coordinate actual value, (X
w, Y
w, Z
w) be object space point coordinate under world coordinate system, (X
c, Y
c, Z
c) be the coordinate of object space point under camera coordinates system.F is focal length, K
1, K
2, K
3, P
1, P
2for distortion parameter, focal length and distortion parameter are the interior orientation parameter of camera, can demarcate in advance.R, T are respectively rotation matrix and the translation vector of camera, represent the orientation of camera in world coordinate system.From formula, the three-dimensional coordinate of gauge point be recovered, imaging must be carried out from different azimuth to multiple point, set up collinearity condition equation group.Therefore on different picture, the coupling of same tag point is a crucial step, and camera orientation is the prerequisite of coupling.
2. hand and eye calibrating;
Camera orientation based on robot motion's parameter is based upon on the basis of hand and eye calibrating and robot motion's parameter.As shown in Fig. 3 (a), camera is arranged on robot end and moves along with robot motion, and in the process of robot motion, camera remains unchanged relative to the position of robot end's instrument.The basic ideas of hand and eye calibrating are that control moves to diverse location, make camera carry out imaging from different azimuth to calibrated reference space, derive the mathematical relation of tool coordinates system and camera coordinates system.
As shown in Fig. 3 (b), camera remains unchanged relative to the position of robot end's instrument; Basic ideas are that control moves to diverse location, make camera carry out imaging from different azimuth to calibrated reference space; Then
P
c1=CP
c2(1)
P
c1=XP
t1(2)
P
t1=DP
t2(3)
P
c2=XP
t2(4)
Obtained by formula (1) and (4)
P
c1=CXP
t2(5)
Obtained by formula (2) and (3)
P
c1=XDP
t2(6)
Simultaneous (5) obtains with (6)
CX=XD(7)
Mobile robot is to diverse location, and obtain multiple group type (7) equation of constraint, simultaneous can try to achieve X.
3. camera is directed:
After obtaining trick relation, composition graphs 3 (a), in matrix X corresponding diagram
inverse matrix, namely
mobile robot during measurement, can read tool coordinates from controller and tie up to position basis coordinates system
then camera coordinates ties up to the position in basis coordinates system of robot and is
In the middle of basis coordinates system, the self-orientation of camera is realized thus, for epipolar-line constraint coupling is prepared by unified for the position of camera.
4. epipolar-line constraint coupling;
Described by the geometry of pole is the corresponding relation of picture point and its polar curve, and basis matrix is the Algebraic Expression of pole geometry.As shown in Fig. 4 (a), two as plane be respectively I, I ', m and m ' is picture point corresponding to spatial point P, the photocentre that C and C ' is camera, and the line of photocentre is called baseline, baseline and intersection point e, the e ' be called limit as plane.Baseline CC ' is called polar plane with two projection ray CP, C ' plane that P is formed, polar plane and the intersection l as plane
m ', l
m' be called polar curve.In figure, m and m ' is corresponding point, l
m' for m picture planar I ' on polar curve, obvious m ' is at l
m' on.Equally, m is inevitable at polar curve l
m 'on.Describing above-mentioned restriction relation with basis matrix F is
As long as calculate basis matrix just by calculating corresponding polar curve search match point.
Tying up to the position in robot basis coordinates for camera coordinates as Fig. 4 (a), H, H ', is the self-orientating result of camera, then the relative orientation H of two cameras
relfor
H
rel=H
-1H′
H
relcan be analyzed to
Then basis matrix is
F=K
T[t
rel]
xR
relK
-1
In above formula, K is the Intrinsic Matrix of camera, remains unchanged in the process measured.The limit restraint coupling that is calculated as of basis matrix provides the foundation.
The ambiguity matching and correlation of 5 feature based vectors;
As Fig. 4 (b), point P and N is positioned on same polar plane, and the corresponding polar curve of 2 is identical, on polar curve, therefore searches for match point there will be ambiguity coupling.Propose a kind of ambiguity correct methods matching of feature based vector herein, utilize the close matrix of self property structure of point set, point set is transformed into higher dimensional space from two-dimensional space and mates again.Suppose that two groups of points with ambiguity coupling are respectively I
i(i=1,2 ..., m) and J
j(j=1,2 ..., n), set up metric matrix
H
ij=exp(-r
ij 2/2σ
2)
R in above formula
ijan I
iwith a J
jbetween distance, σ is calculated as follows
Wherein M is number a little.SVD decomposition is carried out to matrix H
H
m=V
mD
mV
m T
Wherein V is the unit orthogonal matrix of m dimension.Obtain the m-dimensional space description that V matrix just obtains this point set, m column vector E of matrix V
ithe base of this m-dimensional space, m the row vector then F of V
ithe coordinate of this m point under this group m ties up base respectively.H can be obtained from first point set
1=V
1d
1v
1 t, can H be obtained from second point set equally
2=V
2d
2v
2 t, remember that two groups of points are described as F under new base respectively
i, 1and F
j, 2.If m with n is not identical, before choosing | m-n| element, set up trip current Z
Z
ij=F
i,1-F
j,2 2
Decision criteria is: only have and work as Z
ijwhen being the least member of the i-th row and jth row in matrix Z, just think an I
iwith a J
ithat correct coupling is right.The result that the result of correction and epipolar-line constraint mate is combined, completes ambiguity matching and correlation.
6. experimental system;
The system configuration adopted herein has: industrial robot (ABBIRB1410), laser plumbing device, industrial camera (VA-8MC coordinates AFNikon28mmf/2.8D camera lens), computing machine and scaling board (Ti-TIMESCG-150-H-15).
Hand and eye calibrating result is as following table:
7. the directed result of camera;
Read tool coordinates from controller and tie up to position in basis coordinates system of robot, in conjunction with hand and eye calibrating result, by camera position unification in basis coordinates system of robot, following table lists the directed result of wherein two cameras.
8. reference points matching result;
In conjunction with the directed result of camera shown in upper table, calculate the corresponding polar curve of left figure gauge point on right figure, as Fig. 5.Corresponding polar curve searches for match point, as Fig. 6, realizes the correct coupling of point set, demonstrate the accuracy of the directed result of camera.In addition the corresponding polar curve of Fig. 5 mid point 1 and point 2 all crosses point 3 and point 4, occurs that ambiguity is mated, need correct.
As Fig. 7, the matching result according to Fig. 6, the correct matching double points 5,6 and 7,8 that random selecting two is adjacent, its objective is that the architectural feature making point set is more obvious.Matching double points correct in addition participates in correcting, and can guarantee the accuracy corrected.If 5,6 and 7,8 still to mate after correcting, then think and correct successfully, otherwise think and correct unsuccessfully, again choose matching double points, until correct successfully.The coordinate of point set to be matched is as following table.
Trimming process is as follows, first calculates the σ parameter of point set (1,2,5,7) and (3,4,6,8), σ
1=657.6971, σ
2=1290.5428.Then computation measure matrix H
1with H
2
Calculate trip current Z afterwards
Finding in trip current in row, column is all minimum element, completes Point set matching, corrects result as Fig. 8.