CN113324538B - A long-distance high-precision six-degree-of-freedom pose measurement method for cooperative targets - Google Patents

Abstract

The invention discloses a long-distance high-precision six-degree-of-freedom pose measurement method of a cooperative target, comprising the following steps: (1) obtaining an initial six-degree-of-freedom pose and an initial depth value of the cooperative target; (2) calculating a laser spot circle equation; (3) Obtain the equation of the space line determined by the initial position and the two three-dimensional space points of the camera optical center; (4) Obtain the intersection of the spot circle and the space line to correct the initial pose; (5) Use the nonlinear optimization method to The correction results are further optimized. The method uses the accurate depth value provided by the laser range finder to correct the large error of the monocular vision pose measurement in the direction of sight, and at the same time can eliminate the interference error introduced by the laser spot dispersion of the laser range finder.

Description

A long-distance high-precision six-degree-of-freedom pose measurement method for cooperative targets

technical field

The invention relates to the technical field of pose measurement based on multi-sensor fusion, in particular to a long-distance high-precision six-degree-of-freedom pose measurement method for a cooperative target.

Background technique

Targeted high-precision six-degree-of-freedom pose measurement helps to achieve precise and efficient machine operation, so it has been widely used in the fields of autonomous robots, precision machining, and spacecraft docking. Monocular vision is a non-contact 6-DOF attitude measurement method, which has the advantages of low hardware complexity, high precision, and wide measurement range. It is one of the most commonly used 6-DOF attitude measurement methods. However, due to the limitations of the model itself, the measurement accuracy along the optical axis of the camera is usually much lower than the measurement accuracy in the other two directions. 2014 Vol. 22 (5)), the measurement accuracy along the camera's optical axis is only one-twentieth of the other two directions.

According to Chao Zhichao et al.'s "Monocular Camera-Laser Ranging Sensor Pose Measurement System" (see "Acta Optics", 2011, Vol. 31 (3) pp. 85-91), the depth value provided by the laser range finder is used. It directly replaces the displacement value of the line-of-sight direction in the monocular vision pose measurement result, and does not consider the error caused by the dispersion of the laser spot, so its correction effect is limited and can only achieve millimeter-level accuracy.

SUMMARY OF THE INVENTION

Aiming at the problem of large measurement error along the optical axis of the camera in the existing monocular vision measurement, the present invention provides a long-distance high-precision six-degree-of-freedom pose measurement method for a cooperative target, which is a solution based on multi-sensor fusion. method. The method can use the accurate depth information obtained by the laser rangefinder to correct the initial pose measured by the camera, and at the same time can eliminate the measurement error caused by the dispersion of the laser spot. This method establishes the light spot dispersion circle, determines the initial measurement position and the straight line where the optical center point of the camera is located, and then converts the fusion problem of two sensors into a geometric problem of finding the intersection of a space circle and a space straight line. The obtained intersection point is the corrected measurement pose. After correction, the displacement measurement accuracy along the line-of-sight direction is higher than the other two directions.

The technical scheme adopted in the present invention is as follows: a long-distance high-precision six-degree-of-freedom pose measurement method for a cooperative target, comprising the following steps:

Step (1): Obtain the image of the cooperation target, and calculate the initial pose of the cooperation target according to the PnP algorithm T _i =[R _i |t _i ], where R represents the attitude information, t represents the displacement information, and the subscript i represents this is the initial position. result;

The concrete process of described step (1) is:

并进行合作目标上的3D合作特征点P_i ^T(i＝1,2,3,4)与对应的图片上的2D成像点

的匹配；Step (11), use camera imaging to obtain the image of the cooperation target, and then extract the sub-pixel coordinates of the center point of the characteristic imaging of the cooperation target based on the gray-scale centroid method

And carry out the 3D cooperation feature points P _i ^T (i=1, 2, 3, 4) on the cooperation target and the 2D imaging points on the corresponding pictures

match;

Step (12), based on the PnP algorithm, use the 4 pairs of 2D-3D matching points in step (11) to calculate the pose, and obtain the pose transformation relationship T _i =[ R _i |t _i ].

Step (2), use _{a laser rangefinder to measure the accurate depth information Da} .

Step (3), use the accurate depth information Da in step (2) to correct the initial pose Ti ₌ [R _i |t _i ] in step (1 ₎ , and obtain the corrected pose Ta ₌ [R _a |t _a ], where the subscript a represents the corrected result;

The concrete process of described step (3) is:

与相机光心坐标系原点O＝[0 0 0]^T可以确定一条直线L_OT，其方程为

其中，上标i代表这是初始结果，{l,m,n}表示直线的参数；Step (31), in the camera coordinate system, the initial position coordinates measured by the camera

A straight line L _OT can be determined with the origin of the camera optical center coordinate system O=[0 0 0] ^T , and its equation is

Among them, the superscript i represents that this is the initial result, and {l,m,n} represents the parameters of the straight line;

In step (32), the conversion relationship between the coordinate system of the laser rangefinder and the optical center coordinate system of the camera has been calibrated in advance, and can be represented by a three-dimensional point in space and a direction vector in space:

O _LO = [x _L , y _L , z _L ] ^T

n _LO =[n _xL ,n _yL ,n _zL ] ^T

Among them, x _L , y _L , and z _L are the coordinate values of the origin of the laser rangefinder coordinate system in the X, Y, and Z directions of the camera coordinate system, respectively, and n _xL , _nyL , and n _zL from the camera coordinate system to the laser measuring The Euler rotation angle of the tachymeter coordinate system.

Since the end of the laser has a certain dispersion, the laser spot is not a precise point, but has a certain size. Without loss of generality, we assume that the laser spot is dispersed into a circle with a radius of r. Since we know the representation of the laser rangefinder coordinate system in the camera coordinate system and the measured distance value of the laser rangefinder, the coordinates of the center point of the laser spot surface can be calculated P=[x _r , y _r , z _r ] ^T , and then solve the equation of the circle of confusion through the above conditions:

In step (33), find the intersection of the straight line L _OT determined in step (31) and the circular surface determined in this step (32), and the obtained intersection coordinates are the corrected cooperative target position. The joint equation looks like this:

其中，上标a代表这是矫正后的结果；用t_a更新T_i＝[R_i|t_i]中的位移分量，得到初步矫正后的位姿T_a＝[R_a|t_a]。Solving this equation will get the corrected cooperative target position coordinates

Wherein, the superscript a represents the corrected result; update the displacement component in T _{i =} [R _i |t _i ] with _ta to obtain the initially corrected pose Ta =[R _a | _ta ].

Step (4), use the nonlinear optimization method to further optimize the corrected pose T _a =[R _a | _ta ] obtained in step (3), and the specific optimization model is:

为图片上2D成像点，pro(P_i ^T)表示空间合作目标上特征点的重投影函数，λ是权重因子，一般设为5，

表示矫正后的Z方向位移值，

表示初始的Z方向位移值，最小化上式，将得到一个更加精确的位姿T′_a＝[R′_a|t′_a]。in,

is the 2D imaging point on the picture, pro(P _i ^T ) represents the reprojection function of the feature point on the spatial cooperation target, λ is the weight factor, generally set to 5,

represents the corrected Z-direction displacement value,

Represents the initial Z-direction displacement value, and by minimizing the above formula, a more accurate pose T' _a =[R' _a |t' _a ] will be obtained.

The advantages of the present invention compared with the prior art are:

(1) the present invention solves the problem of large error in the line of sight direction in the existing monocular pose measurement method;

(2) The present invention eliminates the error introduced by the dispersion of the laser spot in the existing fusion method of the camera and the laser rangefinder;

(3) The present invention has extremely high precision, small calculation amount and high stability.

Description of drawings

Fig. 1 is a kind of concrete flow chart of a kind of cooperative target long-distance high-precision six-degree-of-freedom pose measurement method of the present invention;

Fig. 2 is the target coordinate design drawing of the present invention;

Fig. 3 is the imaging point of the cooperative target feature of the present invention on the camera plane;

FIG. 4 is a schematic diagram of the fusion method of the present invention.

Detailed ways

The present invention will be further described in detail below with reference to specific embodiments.

A specific flow chart of a method for measuring a long-distance high-precision six-degree-of-freedom pose of a cooperative target of the present invention is shown in FIG. 1 , and the test distance is set to be 10 meters. The design of the cooperation target is shown in Figure 2, and the exact coordinates of the six LEDs after calibration by the three-coordinate instrument are shown in Table 1.

Table 1 The exact coordinates of the six LEDs

The concrete steps of the present invention are as follows:

In step (1), an image of the cooperation target is acquired, and the initial pose of the cooperation target is calculated according to the PnP algorithm T _i =[R _i |t _i ], and the specific process of the step (1) is:

(图3为某次实验中合作目标特征所成的像，表2为成像点的亚像素图像坐标)，并进行合作目标上的3D合作特征点P_i ^T(i＝1,2,3,4)与对应的图片上2D成像点

的匹配；Step (11), camera imaging, and then extract the sub-pixel coordinates of the center point of the cooperative target feature imaging based on the gray-scale centroid method

(Fig. 3 is the image formed by the characteristics of the cooperative target in a certain experiment, and Table 2 is the sub-pixel image coordinates of the imaging point), and carry out the 3D cooperative feature point P _i ^T (i=1,2,3, 4) 2D imaging point on the corresponding picture

match;

Table 2 Subpixel coordinates of imaging points

Step (12), based on the PnP algorithm, use the 4 pairs of 2D-3D matching points in step (11) to calculate the pose, and obtain the pose transformation relationship T _i =[ R _i |t _i ]. Table 3 shows the initial poses obtained from this measurement.

Table 3. Initial poses measured

Step (2), use _{a laser rangefinder to measure the accurate depth information Da} .

Step (3), use the accurate depth information Da in step (2) to correct the initial pose T _i =[R _i |t _i ] in step (1 ₎ , and obtain the corrected accurate pose T _a =[R _a | t _a ], the concrete process of described step (3) is:

与相机光心坐标系原点O＝[0 0 0]^T可以确定一条直线L_OT，其方程为

Step (31), in the camera coordinate system, the initial position coordinates measured by the camera

A straight line L _OT can be determined with the origin of the camera optical center coordinate system O=[0 0 0] ^T , and its equation is

In step (32), the conversion relationship between the coordinate system of the laser rangefinder and the optical center coordinate system of the camera has been calibrated in advance, and can be represented by a three-dimensional point in space and a direction vector in space:

O _LO = [x _L , y _L , z _L ] ^T

n _LO =[n _xL ,n _yL ,n _zL ] ^T

Among them, x _L , y _L , and z _L are the coordinate values of the origin of the laser rangefinder coordinate system in the X, Y, and Z directions of the camera coordinate system, respectively, and n _xL , _nyL , and n _zL from the camera coordinate system to the laser measuring The Euler rotation angle of the tachymeter coordinate system.

Since the end of the laser has a certain dispersion, the laser spot is not a precise point, but has a certain size. Without loss of generality, it is assumed that the laser spot is dispersed into a circle with a radius of r. Since the representation of the rangefinder coordinate system in the camera coordinate system and the measured distance length of the laser rangefinder are known, the coordinates P=(x _r , y _r , z _r ) of the center point of the laser spot can be obtained, And then through the above conditions to solve the equation of the circle of confusion:

In step (33), find the intersection of the straight line L _OT determined in step (31) and the circular surface determined in step (32), and the obtained intersection coordinates are the corrected cooperative target position coordinates. The joint equation looks like this:

用t_a更新T_i＝[R_i|t_i]中的位移分量，得到初步矫正后的位姿T_a＝[R_a|t_a]。表4为对此次测量值进行矫正所得的位姿。Solving this equation will get the corrected cooperative target position coordinates

The displacement component in Ti _{=[R i |t i ] is} updated with _ta to obtain the initially corrected pose Ta ₌ [R _a | _ta ]. Table 4 shows the poses obtained by correcting the measured values.

Table 4. The poses obtained by correcting the measured values

Step (4), use the nonlinear optimization method to further optimize the corrected pose Ta ₌ [R _a | _ta ] obtained in step (3) to improve the accuracy, and the specific optimization model is:

By minimizing the above formula, a more accurate pose T' _a =[R' _a |t' _a ] will be obtained. Table 5 shows the pose obtained by further optimizing the correction results, the pose measured by the AT460 laser rangefinder (can be regarded as the true value) and the measurement error this time.

Table 5. The poses obtained by further optimizing the correction results

The parts not described in detail in the present invention belong to the well-known technology in the technical field. The above descriptions are only specific examples of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements and improvements made within the spirit and principles of the present invention shall be included in the protection scope of the present invention. within.

Claims (1)

1. a cooperative target long-distance high-precision six-degree-of-freedom pose measurement method, is characterized in that, comprises the steps: Step (1): Obtain the image of the cooperation target, and calculate the initial pose of the cooperation target according to the PnP algorithm T _i =[R _i |t _i ], where R represents the attitude information, t represents the displacement information, and the subscript i represents this is the initial position. Result: The concrete process of described step (1) is: Step (11), use camera imaging to obtain the image of the cooperation target, and then extract the sub-pixel coordinates of the center point of the characteristic imaging of the cooperation target based on the gray-scale centroid method

And perform 2D-3D point matching; Step (12), based on the PnP algorithm, use the 4 pairs of 2D-3D matching points in step (11) to perform pose calculation to obtain the initial pose T _i =[R _i |t _i ] of the cooperative target; Step (2), utilize laser range finder to measure accurate depth information D _a ; Step (3), use the accurate depth information Da in step (2) to correct the initial pose Ti ₌ [R _i |t _i ] in step (1 ₎ , and obtain the corrected pose Ta ₌ [R _a |t _a ], where the subscript a represents the corrected result; The concrete process of described step (3) is: Step (31), in the camera coordinate system, the initial position coordinates measured by the camera

A straight line L _OT can be determined with the origin of the camera optical center coordinate system O=[0 0 0] ^T , and its equation is

Among them, the superscript i represents that this is the initial result, and {l,m,n} represents the parameters of the straight line; In step (32), the conversion relationship between the coordinate system of the laser rangefinder and the optical center coordinate system of the camera has been calibrated in advance, and can be represented by a three-dimensional point in space and a direction vector in space: O _LO = [x _L , y _L , z _L ] ^T n _LO =[n _xL ,n _yL ,n _zL ] ^T Among them, x _L , y _L , and z _L are the coordinate values of the origin of the laser rangefinder coordinate system in the X, Y, and Z directions of the camera coordinate system, respectively, and n _xL , _nyL , and n _zL are the camera coordinate system transfer to the laser measurement The Euler rotation angle of the tachymeter coordinate system; Assuming that the laser spot is dispersed into a circle with a radius of r, the representation of the laser light in the camera coordinate system and the measured distance value of the laser rangefinder are known, and the coordinates of the center point of the laser spot surface P=[x _r , y _r ,z _r ] ^T , and then solve the equation of the circle of confusion through the above conditions:

Among them, x, y, z respectively represent the coordinate values of the point on the space circle in the X, Y, Z directions of the camera coordinate system; Step (33), seek the intersection of the straight line L _OT determined in step (31) and the circle determined in step (32), the intersection of gained is the cooperation target position after correction

Update the displacement component in Ti _{=[R i |t i ] with ta} to obtain the initial corrected pose Ta ₌ [R _a |t _a ]; Step (4), use the nonlinear optimization method to further optimize the corrected pose T _a =[R _a | _ta ] obtained in step (3), and the specific optimization model is:

in,

is the 2D imaging point on the picture, pro(P _i ^T ) represents the reprojection function of the feature point on the spatial cooperation target, λ is the weight factor, set to 5,

represents the corrected Z-direction displacement value,

Represents the initial Z-direction displacement value, and by minimizing the above formula, a more accurate pose T' _a =[R' _a |t' _a ] will be obtained.

Images