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

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

Info

Publication number
CN113324538B
CN113324538B CN202110501861.8A CN202110501861A CN113324538B CN 113324538 B CN113324538 B CN 113324538B CN 202110501861 A CN202110501861 A CN 202110501861A CN 113324538 B CN113324538 B CN 113324538B
Authority
CN
China
Prior art keywords
pose
coordinate system
initial
camera
laser
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110501861.8A
Other languages
Chinese (zh)
Other versions
CN113324538A (en
Inventor
赵汝进
朱自发
马跃博
颜坤
刘恩海
易晋辉
曾思康
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Institute of Optics and Electronics of CAS
Original Assignee
Institute of Optics and Electronics of CAS
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Institute of Optics and Electronics of CAS filed Critical Institute of Optics and Electronics of CAS
Priority to CN202110501861.8A priority Critical patent/CN113324538B/en
Publication of CN113324538A publication Critical patent/CN113324538A/en
Application granted granted Critical
Publication of CN113324538B publication Critical patent/CN113324538B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/04Interpretation of pictures
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C3/00Measuring distances in line of sight; Optical rangefinders

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了一种合作目标远距离高精度六自由度位姿测量方法,包括如下步骤:(1)获取合作目标初始六自由度位姿和初始深度值;(2)计算激光光斑圆方程;(3)求取初始位置与相机光心两个三维空间点所确定空间直线的方程;(4)求取光斑圆与空间直线的交点,矫正初始位姿;(5)利用非线性优化方法对矫正结果进行进一步优化。该方法借助激光测距仪所提供的准确深度值来矫正单目视觉位姿测量视线方向上的较大误差,同时能够消除由激光测距仪激光光斑弥散而引入的干扰误差。

Figure 202110501861

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.

Figure 202110501861

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

目标高精度六自由度位姿测量有助于实现精确高效的机器操作,因此在自主机器人、精密加工和航天器对接等领域得到了广泛应用。单目视觉是一种非接触式的六自由度姿态测量方法,具有硬件复杂度低、精度高、测量范围广等优点,是目前最常用的六自由度姿态测量方法之一。然而,由于模型自身的限制,沿相机光轴的测量精度通常要比其他两个方向的测量精度低很多,根据赵连军等人《单目三点位置测量精度分析》(参见《光学精密工程》,2014年22卷(5)),沿相机光轴的测量精度只有其他两个方向的二十分之一。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.

根据晁志超等人的《单目摄像机-激光测距传感器位姿测量系统》(参见《光学学报》,2011年31卷(3)85-91页),利用了激光测距仪所提供的深度值直接替换了单目视觉位姿测量结果中视线方向的位移值,没有考虑激光光斑弥散所带来的的误差,因此它的矫正效果有限,只能达到毫米级精度。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:

步骤(1)、获取合作目标图像,根据PnP算法解算合作目标初始位姿Ti=[Ri|ti],其中,R表示姿态信息,t表示位移信息,下标i代表这是初始结果;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;

所述步骤(1)的具体过程为:The concrete process of described step (1) is:

步骤(11)、利用相机成像获取合作目标图像,然后基于灰度重心法提取合作目标特征成像中心点亚像素坐标

Figure BDA0003056718050000021
并进行合作目标上的3D合作特征点Pi T(i=1,2,3,4)与对应的图片上的2D成像点
Figure BDA0003056718050000022
的匹配;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
Figure BDA0003056718050000021
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
Figure BDA0003056718050000022
match;

步骤(12)、基于PnP算法,利用步骤(11)中的4对2D-3D匹配点进行位姿解算,得到合作目标坐标系相对于相机光心坐标系的位姿转换关系Ti=[Ri|ti]。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 ].

步骤(2)、利用激光测距仪测量准确深度信息DaStep (2), use a laser rangefinder to measure the accurate depth information Da .

步骤(3)、利用步骤(2)中的准确深度信息Da矫正步骤(1)中的初始位姿Ti=[Ri|ti],获取矫正后的位姿Ta=[Ra|ta],其中,下标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;

所述步骤(3)的具体过程为:The concrete process of described step (3) is:

步骤(31)、在相机坐标系中,通过相机所测初始位置坐标

Figure BDA0003056718050000023
与相机光心坐标系原点O=[0 0 0]T可以确定一条直线LOT,其方程为
Figure BDA0003056718050000024
其中,上标i代表这是初始结果,{l,m,n}表示直线的参数;Step (31), in the camera coordinate system, the initial position coordinates measured by the camera
Figure BDA0003056718050000023
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
Figure BDA0003056718050000024
Among them, the superscript i represents that this is the initial result, and {l,m,n} represents the parameters of the straight line;

步骤(32)、激光测距仪坐标系与相机光心坐标系的转换关系已事先标定,可以用一个空间三维点和一个空间方向向量表示: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:

OLO=[xL,yL,zL]T O LO = [x L , y L , z L ] T

nLO=[nxL,nyL,nzL]T n LO =[n xL ,n yL ,n zL ] T

其中,xL、yL、zL分别为激光测距仪坐标系原点位置在相机坐标系X,Y,Z方向的坐标值,nxL、nyL、nzL从相机坐标系转到激光测距仪坐标系的欧拉旋转角。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.

由于激光末端具有一定的弥散,所以激光光斑不是一个精确的点,而是有了一定的尺寸,不失一般性,我们假设激光光斑弥散成了一个半径为r的圆。由于我们已知激光测距仪坐标系在相机坐标系下的表示以及激光测距仪的所测的距离值,因此可以求出激光光斑面中心点的坐标P=[xr,yr,zr]T,进而通过以上条件求解出弥散圆的方程: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:

Figure BDA0003056718050000025
Figure BDA0003056718050000025

步骤(33)、求步骤(31)中确定的直线LOT与本步骤(32)中所确定的圆面的交点,所得的交点坐标即为矫正后的合作目标位置。联合方程如下所示: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:

Figure BDA0003056718050000031
Figure BDA0003056718050000031

解此方程将得到矫正后的合作目标位置坐标

Figure BDA0003056718050000032
其中,上标a代表这是矫正后的结果;用ta更新Ti=[Ri|ti]中的位移分量,得到初步矫正后的位姿Ta=[Ra|ta]。Solving this equation will get the corrected cooperative target position coordinates
Figure BDA0003056718050000032
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 ].

步骤(4)、利用非线性优化方法对通过步骤(3)中得到的矫正后的位姿Ta=[Ra|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:

Figure BDA0003056718050000033
Figure BDA0003056718050000033

其中,

Figure BDA0003056718050000034
为图片上2D成像点,pro(Pi T)表示空间合作目标上特征点的重投影函数,λ是权重因子,一般设为5,
Figure BDA0003056718050000035
表示矫正后的Z方向位移值,
Figure BDA0003056718050000036
表示初始的Z方向位移值,最小化上式,将得到一个更加精确的位姿T′a=[R′a|t′a]。in,
Figure BDA0003056718050000034
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,
Figure BDA0003056718050000035
represents the corrected Z-direction displacement value,
Figure BDA0003056718050000036
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)本发明解决现有单目位姿测量方法中视线方向上误差较大的问题;(1) the present invention solves the problem of large error in the line of sight direction in the existing monocular pose measurement method;

(2)本发明消除了现有的相机与激光测距仪融合方法中因激光光斑弥散而引入的误差;(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)本发明具有极高的精度,计算量小,稳定性高。(3) The present invention has extremely high precision, small calculation amount and high stability.

附图说明Description of drawings

图1为本发明一种合作目标远距离高精度六自由度位姿测量方法具体流程图;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;

图2为本发明的靶标坐标设计图;Fig. 2 is the target coordinate design drawing of the present invention;

图3为本发明的合作目标特征在相机平面上的成像点;Fig. 3 is the imaging point of the cooperative target feature of the present invention on the camera plane;

图4为本发明的融合方法示意图。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.

本发明一种合作目标远距离高精度六自由度位姿测量方法具体流程图如图1所示,设测试距离为10米。合作目标设计见图2,经三坐标仪标定后的六个LED的准确坐标如表1所示。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.

表1六个LED的准确坐标Table 1 The exact coordinates of the six LEDs

Figure BDA0003056718050000041
Figure BDA0003056718050000041

本发明具体步骤如下:The concrete steps of the present invention are as follows:

步骤(1)、获取合作目标图像,根据PnP算法解算合作目标初始位姿Ti=[Ri|ti],所述步骤(1)的具体过程为: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:

步骤(11)、相机成像,然后基于灰度重心法提取合作目标特征成像中心点亚像素坐标

Figure BDA0003056718050000042
(图3为某次实验中合作目标特征所成的像,表2为成像点的亚像素图像坐标),并进行合作目标上的3D合作特征点Pi T(i=1,2,3,4)与对应的图片上2D成像点
Figure BDA0003056718050000043
的匹配;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
Figure BDA0003056718050000042
(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
Figure BDA0003056718050000043
match;

表2成像点的亚像素坐标Table 2 Subpixel coordinates of imaging points

Figure BDA0003056718050000044
Figure BDA0003056718050000044

步骤(12)、基于PnP算法,利用步骤(11)中的4对2D-3D匹配点进行位姿解算,得到合作目标坐标系相对于相机光心坐标系的位姿转换关系Ti=[Ri|ti]。表3为此次测量所得的初始位姿。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.

表3测量所得的初始位姿Table 3. Initial poses measured

Figure BDA0003056718050000051
Figure BDA0003056718050000051

步骤(2)、利用激光测距仪测量准确深度信息DaStep (2), use a laser rangefinder to measure the accurate depth information Da .

步骤(3)、利用步骤(2)中的准确深度信息Da矫正步骤(1)中的初始位姿Ti=[Ri|ti],获取矫正后的准确位姿Ta=[Ra|ta],所述步骤(3)的具体过程为: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:

步骤(31)、在相机坐标系中,通过相机所测初始位置坐标

Figure BDA0003056718050000052
与相机光心坐标系原点O=[0 0 0]T可以确定一条直线LOT,其方程为
Figure BDA0003056718050000053
Step (31), in the camera coordinate system, the initial position coordinates measured by the camera
Figure BDA0003056718050000052
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
Figure BDA0003056718050000053

步骤(32)、激光测距仪坐标系与相机光心坐标系的转换关系已事先标定,可以用一个空间三维点和一个空间方向向量表示: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:

OLO=[xL,yL,zL]T O LO = [x L , y L , z L ] T

nLO=[nxL,nyL,nzL]T n LO =[n xL ,n yL ,n zL ] T

其中,xL、yL、zL分别为激光测距仪坐标系原点位置在相机坐标系X,Y,Z方向的坐标值,nxL、nyL、nzL从相机坐标系转到激光测距仪坐标系的欧拉旋转角。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.

由于激光末端具有一定的弥散,所以激光光斑不是一个精确的点,而是有了一定的尺寸,不失一般性,假设激光光斑弥散成了一个半径为r的圆。由于已知测距仪坐标系在相机坐标系下的表示以及激光测距仪的所测的距离长度,因此可以求出激光光斑中心点的坐标P=(xr,yr,zr),进而通过以上条件求解出弥散圆的方程: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:

Figure BDA0003056718050000054
Figure BDA0003056718050000054

步骤(33)、求步骤(31)中确定的直线LOT与步骤(32)中所确定的圆面的交点,所得的交点坐标即为矫正后的合作目标位置坐标。联合方程如下所示: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:

Figure BDA0003056718050000061
Figure BDA0003056718050000061

解此方程将得到矫正后的合作目标位置坐标

Figure BDA0003056718050000062
用ta更新Ti=[Ri|ti]中的位移分量,得到初步矫正后的位姿Ta=[Ra|ta]。表4为对此次测量值进行矫正所得的位姿。Solving this equation will get the corrected cooperative target position coordinates
Figure BDA0003056718050000062
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.

表4对测量值进行矫正所得的位姿Table 4. The poses obtained by correcting the measured values

Figure BDA0003056718050000063
Figure BDA0003056718050000063

步骤(4)、利用非线性优化方法对通过步骤(3)中得到的矫正后的位姿Ta=[Ra|ta]进行进一步优化提升精度,具体优化模型为: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:

Figure BDA0003056718050000064
Figure BDA0003056718050000064

最小化上式,将得到一个更加精确的位姿T′a=[R′a|t′a]。表5为对此次矫正结果进行进一步优化所得的位姿,利用AT460激光测距仪测量的位姿(可以视为真实值)以及此次的测量误差。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.

表5对矫正结果进行进一步优化所得的位姿Table 5. The poses obtained by further optimizing the correction results

Figure BDA0003056718050000065
Figure BDA0003056718050000065

本发明未详述部分属于本技术领域的公知技术。以上所述仅为本发明的具体实例而已,并不用于以限制本发明,凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,均应包含在本发明的保护范围之内。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.一种合作目标远距离高精度六自由度位姿测量方法,其特征在于,包括如下步骤:1. a cooperative target long-distance high-precision six-degree-of-freedom pose measurement method, is characterized in that, comprises the steps: 步骤(1)、获取合作目标图像,根据PnP算法解算合作目标初始位姿Ti=[Ri|ti],其中,R表示姿态信息,t表示位移信息,下标i代表这是初始结果;所述步骤(1)的具体过程为: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: 步骤(11)、利用相机成像获取合作目标图像,然后基于灰度重心法提取合作目标特征成像中心点亚像素坐标
Figure FDA0003747152800000011
并进行2D-3D点的匹配;
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
Figure FDA0003747152800000011
And perform 2D-3D point matching;
步骤(12)、基于PnP算法,利用步骤(11)中的4对2D-3D匹配点进行位姿解算,得到合作目标的初始位姿Ti=[Ri|ti];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; 步骤(2)、利用激光测距仪测量准确深度信息DaStep (2), utilize laser range finder to measure accurate depth information D a ; 步骤(3)、利用步骤(2)中的准确深度信息Da矫正步骤(1)中的初始位姿Ti=[Ri|ti],获取矫正后的位姿Ta=[Ra|ta],其中,下标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; 所述步骤(3)的具体过程为:The concrete process of described step (3) is: 步骤(31)、在相机坐标系中,通过相机所测初始位置坐标
Figure FDA0003747152800000012
与相机光心坐标系原点O=[0 0 0]T可以确定一条直线LOT,其方程为
Figure FDA0003747152800000013
其中,上标i代表这是初始结果,{l,m,n}表示直线的参数;
Step (31), in the camera coordinate system, the initial position coordinates measured by the camera
Figure FDA0003747152800000012
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
Figure FDA0003747152800000013
Among them, the superscript i represents that this is the initial result, and {l,m,n} represents the parameters of the straight line;
步骤(32)、激光测距仪坐标系与相机光心坐标系的转换关系已事先标定,可以用一个空间三维点和一个空间方向向量表示: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: OLO=[xL,yL,zL]T O LO = [x L , y L , z L ] T nLO=[nxL,nyL,nzL]T n LO =[n xL ,n yL ,n zL ] T 其中,xL、yL、zL分别为激光测距仪坐标系原点位置在相机坐标系X,Y,Z方向的坐标值,nxL、nyL、nzL为相机坐标系转到激光测距仪坐标系的欧拉旋转角;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; 假设激光光斑弥散成了一个半径为r的圆,已知激光光线在相机坐标系下的表示以及激光测距仪的所测的距离值,求出激光光斑面中心点坐标P=[xr,yr,zr]T,进而通过以上条件求解出弥散圆的方程: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:
Figure FDA0003747152800000021
Figure FDA0003747152800000021
其中,x,y,z分别表示空间圆上点在相机坐标系X,Y,Z方向的坐标值;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; 步骤(33)、求步骤(31)中确定的直线LOT与步骤(32)中所确定的圆面的交点,所得的相交点即为矫正后的合作目标位置
Figure FDA0003747152800000022
用ta更新Ti=[Ri|ti]中的位移分量,得到初步矫正后的位姿Ta=[Ra|ta];
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
Figure FDA0003747152800000022
Update the displacement component in Ti =[R i |t i ] with ta to obtain the initial corrected pose Ta = [R a |t a ];
步骤(4)、利用非线性优化方法对通过步骤(3)中得到的矫正后的位姿Ta=[Ra|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:
Figure FDA0003747152800000023
Figure FDA0003747152800000023
其中,
Figure FDA0003747152800000024
为图片上2D成像点,pro(Pi T)表示空间合作目标上特征点的重投影函数,λ是权重因子,设为5,
Figure FDA0003747152800000025
表示矫正后的Z方向位移值,
Figure FDA0003747152800000026
表示初始的Z方向位移值,最小化上式,将得到一个更加精确的位姿T′a=[R′a|t′a]。
in,
Figure FDA0003747152800000024
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,
Figure FDA0003747152800000025
represents the corrected Z-direction displacement value,
Figure FDA0003747152800000026
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.
CN202110501861.8A 2021-05-08 2021-05-08 A long-distance high-precision six-degree-of-freedom pose measurement method for cooperative targets Active CN113324538B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110501861.8A CN113324538B (en) 2021-05-08 2021-05-08 A long-distance high-precision six-degree-of-freedom pose measurement method for cooperative targets

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110501861.8A CN113324538B (en) 2021-05-08 2021-05-08 A long-distance high-precision six-degree-of-freedom pose measurement method for cooperative targets

Publications (2)

Publication Number Publication Date
CN113324538A CN113324538A (en) 2021-08-31
CN113324538B true CN113324538B (en) 2022-10-21

Family

ID=77415054

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110501861.8A Active CN113324538B (en) 2021-05-08 2021-05-08 A long-distance high-precision six-degree-of-freedom pose measurement method for cooperative targets

Country Status (1)

Country Link
CN (1) CN113324538B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114545426B (en) * 2022-01-20 2025-06-17 原力聚合(重庆)机器人科技有限公司 Positioning method, device, mobile robot and computer readable medium
CN117284499B (en) * 2023-11-24 2024-01-19 北京航空航天大学 Monocular vision-laser-based pose measurement method for spatial unfolding mechanism

Citations (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102063718A (en) * 2010-12-24 2011-05-18 江南大学 Field calibration and precision measurement method for spot laser measuring system
CN103837084A (en) * 2014-02-18 2014-06-04 浙江华东工程安全技术有限公司 Three-direction displacement measurement method based on laser speckle imaging technology
CN104111071A (en) * 2014-07-10 2014-10-22 上海宇航系统工程研究所 High-precision position posture calculating method based on laser ranging and camera visual fusion
CN104880178A (en) * 2015-06-01 2015-09-02 中国科学院光电技术研究所 Monocular vision pose measuring method based on tetrahedral side length and volume weighting constraint
JP6046852B1 (en) * 2016-07-06 2016-12-21 株式会社エクサ Shape analysis program
CN106441151A (en) * 2016-09-30 2017-02-22 中国科学院光电技术研究所 Measuring system for three-dimensional target Euclidean space reconstruction based on vision and active optical fusion
CN109448055A (en) * 2018-09-20 2019-03-08 中国科学院光电研究院 Monocular vision attitude determination method and system
CN110178045A (en) * 2016-11-17 2019-08-27 特里纳米克斯股份有限公司 Detector at least one object of optical detection
CN111121722A (en) * 2019-12-13 2020-05-08 南京理工大学 Binocular three-dimensional imaging method combining laser dot matrix and polarization vision
CN111199564A (en) * 2019-12-23 2020-05-26 中国科学院光电研究院 Indoor positioning method and device of intelligent mobile terminal and electronic equipment
CN111210468A (en) * 2018-11-22 2020-05-29 中移(杭州)信息技术有限公司 Image depth information acquisition method and device
CN111220126A (en) * 2019-11-19 2020-06-02 中国科学院光电技术研究所 Space object pose measurement method based on point features and monocular camera
CN111815712A (en) * 2020-06-24 2020-10-23 中国地质大学(武汉) A high-precision camera-single laser joint calibration method
CN111965807A (en) * 2020-09-18 2020-11-20 安徽科技学院 Optical aiming system, camera module and electronic equipment
CN111964694A (en) * 2020-08-19 2020-11-20 天津大学 A laser rangefinder calibration method for three-dimensional measurement
CN112229323A (en) * 2020-09-29 2021-01-15 华南农业大学 Six degrees of freedom measurement method of checkerboard cooperation target based on monocular vision of mobile phone and its application

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE112005000738T5 (en) * 2004-03-29 2007-04-26 Evolution Robotics, Inc., Pasadena Method and device for determining position using reflected light sources
JP4644540B2 (en) * 2005-06-28 2011-03-02 富士通株式会社 Imaging device
US7855778B2 (en) * 2007-04-27 2010-12-21 Robert Bosch Company Limited Method and apparatus for locating and measuring the distance to a target
US9222771B2 (en) * 2011-10-17 2015-12-29 Kla-Tencor Corp. Acquisition of information for a construction site
TWI534451B (en) * 2012-04-27 2016-05-21 鴻海精密工業股份有限公司 3d laser location apparatus

Patent Citations (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102063718A (en) * 2010-12-24 2011-05-18 江南大学 Field calibration and precision measurement method for spot laser measuring system
CN103837084A (en) * 2014-02-18 2014-06-04 浙江华东工程安全技术有限公司 Three-direction displacement measurement method based on laser speckle imaging technology
CN104111071A (en) * 2014-07-10 2014-10-22 上海宇航系统工程研究所 High-precision position posture calculating method based on laser ranging and camera visual fusion
CN104880178A (en) * 2015-06-01 2015-09-02 中国科学院光电技术研究所 Monocular vision pose measuring method based on tetrahedral side length and volume weighting constraint
JP6046852B1 (en) * 2016-07-06 2016-12-21 株式会社エクサ Shape analysis program
CN106441151A (en) * 2016-09-30 2017-02-22 中国科学院光电技术研究所 Measuring system for three-dimensional target Euclidean space reconstruction based on vision and active optical fusion
CN110178045A (en) * 2016-11-17 2019-08-27 特里纳米克斯股份有限公司 Detector at least one object of optical detection
CN109448055A (en) * 2018-09-20 2019-03-08 中国科学院光电研究院 Monocular vision attitude determination method and system
CN111210468A (en) * 2018-11-22 2020-05-29 中移(杭州)信息技术有限公司 Image depth information acquisition method and device
CN111220126A (en) * 2019-11-19 2020-06-02 中国科学院光电技术研究所 Space object pose measurement method based on point features and monocular camera
CN111121722A (en) * 2019-12-13 2020-05-08 南京理工大学 Binocular three-dimensional imaging method combining laser dot matrix and polarization vision
CN111199564A (en) * 2019-12-23 2020-05-26 中国科学院光电研究院 Indoor positioning method and device of intelligent mobile terminal and electronic equipment
CN111815712A (en) * 2020-06-24 2020-10-23 中国地质大学(武汉) A high-precision camera-single laser joint calibration method
CN111964694A (en) * 2020-08-19 2020-11-20 天津大学 A laser rangefinder calibration method for three-dimensional measurement
CN111965807A (en) * 2020-09-18 2020-11-20 安徽科技学院 Optical aiming system, camera module and electronic equipment
CN112229323A (en) * 2020-09-29 2021-01-15 华南农业大学 Six degrees of freedom measurement method of checkerboard cooperation target based on monocular vision of mobile phone and its application

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
Calibration of a 3D laser rangefinder and a camera based on optimization solution;Yi An等;《JOURNAL OF INDUSTRIAL AND MANAGEMENT OPTIMIZATION》;20210228;第427-445页 *
Lidar-Monocular Visual Odometry;Johannes Graeter等;《2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)》;20190107;第7872-7879页 *
Mono-camera based calibration method for two-axes LRF measurement system;Langming Zhou等;《SPIE》;20190814;第1-7页 *
单目摄像机-激光测距传感器位姿测量系统;晁志超等;《光学学报》;20110315;第31卷(第3期);第1-7页 *
基于主被动融合的非合作目标重建与位姿测量研究;张壮;《中国优秀博硕士学位论文全文数据库(博士) 基础科学辑》;20190815;第A005-69页 *
基于多传感器融合的航天器间位姿参数估计;冯春等;《红外与激光工程》;20150525;第1616-1622页 *

Also Published As

Publication number Publication date
CN113324538A (en) 2021-08-31

Similar Documents

Publication Publication Date Title
CN108362266B (en) A method and system for assisted monocular vision measurement based on EKF laser ranging
CN105091744B (en) The apparatus for detecting position and posture and method of a kind of view-based access control model sensor and laser range finder
CN105157680B (en) A kind of vision measurement system that global characteristics are combined with local feature and measurement method
CN110006402A (en) A visual measurement system and measurement method for relative pose of large component butt assembly
CN109087355B (en) Monocular camera pose measuring device and method based on iterative updating
CN108198219B (en) Error compensation method for camera calibration parameters for photogrammetry
CN108648242B (en) Two-camera calibration method and device without public view field based on assistance of laser range finder
CN113324538B (en) A long-distance high-precision six-degree-of-freedom pose measurement method for cooperative targets
CN114396894B (en) Pipeline straightness detection device and method based on laser ranging and monocular vision
CN107246866A (en) A kind of high-precision six-freedom degree measuring system and method
CN106157322B (en) A method of camera installation position calibration based on plane mirror
CN110030926B (en) Calibration Method of Laser Beam Spatial Pose
CN109737913A (en) A laser tracking attitude angle measurement system and method
CN108692661A (en) Portable three-dimensional measuring system based on Inertial Measurement Unit and its measurement method
CN109212497A (en) A kind of measurement of space six degree of freedom vehicle radar antenna pose deviation and interconnection method
CN104154931B (en) Optical machine positioning survey method of intersection survey system
CN102096918B (en) Calibration method of parameters of camera for rendezvous and docking
CN114413789A (en) Pipeline inner wall three-dimensional vision measurement data splicing device and method
CN108458710B (en) Pose measurement method
CN108180829B (en) A method for measuring the spatial pointing of objects with parallel line features
CN110211175B (en) Method for calibrating space pose of collimated laser beam
Stevenson et al. Robot aerobics: four easy steps to a more flexible calibration
CN110260817A (en) Complex-curved deviation based on virtual mark point measures method for self-locating
CN110455277B (en) High-precision attitude measurement device and method based on data fusion of Internet of things
CN117419640A (en) Space point three-dimensional coordinate measuring method based on binocular stereoscopic vision

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant