CN101196999A

CN101196999A - A Target Model and Positioning Method Based on Straight Lines

Info

Publication number: CN101196999A
Application number: CNA2006101346375A
Authority: CN
Inventors: 朱枫; 秦丽娟; 郝颖明; 周静; 欧锦军; 付双飞
Original assignee: Shenyang Institute of Automation of CAS
Current assignee: Shenyang Institute of Automation of CAS
Priority date: 2006-12-08
Filing date: 2006-12-08
Publication date: 2008-06-11

Abstract

The present invention discloses a target model and a positioning method for three-dimensional object positioning. The positioning using a tetrahedral pyramid model can obtain accurate depth and posture information of the three-dimensional object. The four sides and the bottom surface of the tetrahedral pyramid target model are black and white. The tetrahedral pyramid target model contains twelve straight lines. The tetrahedral pyramid target model is placed with the square bottom surface rotated 45 degrees around the axis. In combination with the model, a positioning algorithm is designed. For any three straight lines intersecting at two points on the tetrahedral pyramid target model (wherein: the distances between the two intersection points and the optical center of the camera are not equal), a positioning mathematical model is established based on the angle information between the straight lines and the length information of the straight lines. The model is solved based on geometric meaning and combined with a step-length acceleration method. The present invention has the advantages of easy image processing, strong anti-occlusion ability, free line selection, high positioning accuracy, simple positioning algorithm and strong real-time performance.

Description

A Target Model and Positioning Method Based on Straight Lines

技术领域 technical field

本发明涉及图像处理技术，具体地说是一种基于直线的目标模型及定位方法。The invention relates to image processing technology, in particular to a line-based target model and positioning method.

背景技术 Background technique

可以获得位姿信息的视觉方法有许多，典型的有双目或多目立方体视觉，基于模型的单目视觉等。基于模型的单目视觉是指仅利用一台摄像机拍摄单张照片来进行定位工作。因其仅需一台视觉传感器，所以该方法结构简单，同时还避免了立体视觉中的视场小，立体匹配困难等问题。其前提条件是物体的几何模型已知。There are many visual methods that can obtain pose information, typically binocular or multi-eye cube vision, model-based monocular vision, etc. Model-based monocular vision refers to using only one camera to take a single photo for localization. Because only one vision sensor is needed, the method has a simple structure and avoids problems such as small field of view and difficulty in stereo matching in stereo vision. The prerequisite is that the geometric model of the object is known.

当前基于模型单目视觉定位的模型特征分为点、直线与曲线等几类。相对来说，对基于点特征的单目视觉定位方法研究较多，而基于直线特征的单目视觉定位方法现在研究的还比较少。而且，在某些特定的环境中，直线特征定位方法比点特征定位方法具有一定的优势。直线特征的优势表现在以下几方面：(1)自然环境的图像包含很多的直线特征。(2)在图像上直线特征比点特征的提取精度更高。(3)直线特征抗遮挡能力比较强。同时相对于更高级的几何特征(曲线、椭圆等)，直线特征也具有优势，具体表现在以下几方面：(1)在周围自然环境的图像中，直线比其他的高级几何特征更常见，同时也更容易提取。(2)直线的数学表达式更简单，处理起来效率更高。因此综合来看，采用直线特征进行视觉定位在某些方面具有其它特征所不具有的优势，在实现高精度、实时自主定位方面有着广泛的应用前景。The current model features based on model monocular vision positioning are divided into points, straight lines and curves. Relatively speaking, there are many studies on the monocular vision positioning method based on point features, while the monocular vision positioning method based on straight line features is still less studied. Moreover, in some specific environments, the linear feature location method has certain advantages over the point feature location method. The advantages of straight line features are shown in the following aspects: (1) The image of the natural environment contains many straight line features. (2) The extraction accuracy of straight line features is higher than that of point features on the image. (3) The anti-occlusion ability of straight line features is relatively strong. At the same time, compared with more advanced geometric features (curves, ellipses, etc.), straight line features also have advantages, specifically in the following aspects: (1) In the images of the surrounding natural environment, straight lines are more common than other advanced geometric features, and at the same time Also easier to extract. (2) The mathematical expression of the straight line is simpler and the processing efficiency is higher. Therefore, in general, the use of linear features for visual positioning has advantages over other features in some aspects, and has broad application prospects in realizing high-precision, real-time autonomous positioning.

直线特征定位研究集中在三方面：(1)基于直线的合作目标设计；(2)直线定位算法；(3)迭代求解方法。The research of line feature location focuses on three aspects: (1) Cooperative object design based on line; (2) Line location algorithm; (3) Iterative solution method.

(1)基于直线的合作目标设计(1) Cooperative goal design based on straight line

目前，合作目标的设计多是基于点的设计，基于直线的合作目标设计研究很少。直线在图像处理方面具有优势，因此基于直线的合作目标设计在实际应用中具有一定的应用价值。At present, the design of cooperative goals is mostly based on points, and there are few researches on the design of cooperative goals based on straight lines. Lines have advantages in image processing, so the design of cooperative objects based on lines has certain application value in practical applications.

(2)直线定位算法(2) Linear positioning algorithm

目前，理论上研究最多的是利用三线定位的问题，即perspective-three-line，以下简称(P3L)问题。对于P3L问题，大部分学者通过图像直线和摄像机光心构成的解释平面的法向量和物体直线垂直来建立数学模型。这种方法要求确定物体位姿的三条直线中至少有两条不平行且不和光心共面，进而建立由三条直线构成的三个非线性方程，其数学模型可以描述如下：假设摄像机坐标系和物体坐标系之间的旋转矩阵为R，对空间直线L_i，已知其在物体坐标系下的方向矢量为经过旋转变换到摄像机坐标系下，有 $S_{i} = R {\overset{&RightArrow;}{n}}_{i} .$ 由数学模型得到关于旋转矩阵R的关系式为： ${\overset{&RightArrow;}{N}}_{i} \cdot R {\overset{&RightArrow;}{n}}_{i} = 0,$ 因此只要通过三条直线的投影方程，就能通过解方程组得到R矩阵的三个参量，即可以求得R矩阵。这种方法有效地解决了使用直线特征如何进行视觉定位的问题，其中的不足之处是非线性方程组复杂，定位误差较大。At present, the most studied in theory is the problem of using three-line positioning, that is, the perspective-three-line, hereinafter referred to as (P3L) problem. For the P3L problem, most scholars establish a mathematical model through the normal vector of the interpretation plane formed by the image line and the optical center of the camera, which is perpendicular to the object line. This method requires at least two of the three straight lines to determine the object's pose to be non-parallel and not coplanar with the optical center, and then to establish three nonlinear equations composed of three straight lines. The mathematical model can be described as follows: Assume that the camera coordinate system and The rotation matrix between the object coordinate systems is R, and for the space straight line L _i , its direction vector in the object coordinate system is known to be After being rotated and transformed into the camera coordinate system, there is $S_{i} = R {\overset{&Right Arrow;}{no}}_{i} .$ The relational expression about the rotation matrix R obtained from the mathematical model is: ${\overset{&Right Arrow;}{N}}_{i} \cdot R {\overset{&Right Arrow;}{no}}_{i} = 0,$ Therefore, as long as the projection equations of three straight lines are used, the three parameters of the R matrix can be obtained by solving the equation system, that is, the R matrix can be obtained. This method effectively solves the problem of how to use straight line features for visual positioning. The disadvantage is that the nonlinear equations are complex and the positioning error is large.

(3)求解方法(3) Solution method

目前，求解的方法主要有两种，一种是解析解，一种是数值解。对于解析解(闭式解)方法，Dhome(文献1：M.Dhome，M.Richetic，J.-T.Lapreste，and G.Rives，“Determination of the attitude of 3-D objects from a singleperspective view，”IEEE Trans.Pattern Anal.Machine Intell，vol.11，no.12，pp.1266-1278，1989)和Chen(文献2：H.Chen，“Pose determination from lineto plane correspondences：existence solution and closed form solutions，”IEEE Trans.Pattern Anal.Machine Intell，vol 13，no.6pp.530-541.)由空间任意三条线通过建立特殊的模型坐标系推导出一个八次多项式，这个八次多项式可以由闭式解的方法来确定物体的位姿。Radu Horaud(文献3：Horaud.R.Conio.B.Leboullcux O and Lacolle，B，“An analytic solution for theperspective 4-point Problem，”Computer Vision Graphics and imageprocessing，vol 47，no.1，pp.33-44，1989)对于非共面的三条直线得到一个四次多项式，最后也可以由迭代的方法通过闭式解的方式确定物体的位姿。解析解方法的弱点是多解问题，定位误差大。许多学者提出来各种不同的迭代方法来解决解析解的多解问题，也就是数值解方法。对于数值解方法，Yuan(文献4：J S-C Yuan.“A general photogrammetric method fordetermining object position and orientation，”IEEE Transactions on Roboticsand Automation，vol.5，no.2，pp.129～142)建议把旋转矩阵R参数从T参数中分离出来，集中计算旋转矩阵R参数。旋转矩阵R是通过正交矩阵来表示，解是通过六个二次多项式的公共根来表示，这个公共根通过牛顿迭代梯度法得到，然而作者注意到在使用牛顿迭代梯度法时会出现局部最优解。Lowe(文献5：D Lowe.“Three-dimensional Object Recognition from SingleTwo-dimensional Images，”Artificial Intelligence，vol 31，pp.355～395，1987.)使用牛顿迭代法估计物体相对于摄像机的方向和位置参数，和Yuan(4)的方法一样，Lowe注意到牛顿迭代法的一些问题，并且在以后的文章中他研究了怎样处理初值和稳定性问题。Liu(文献6：Y Liu，T S Huang，O DFaugeras.“Determination of camera location from 2-D to 3-D line and pointcorrespondences，”IEEE Trans.Pattern Anal.Machine Intell，vol 12，no 1，pp.28～37，1990)使用交替迭代法来求解视觉参数，旋转矩阵以欧拉角来表示，并且作者把误差功能函数线性化，他们注意到当三个欧拉角比30度小的时候这种方法工作起来效果好。Thai Quynh Phong(文献7：Thai quynhphong，“Object Pose from 2-D to 3-D Point and Line Correspondences，”International Joural of Computer Vision，vol 15，pp.225-243，1995)使用最优区域优化方法求解。Stephane(文献9：Stephane Christy and Radu Horaud，“Iterative Pose Computation from Line Correspondences，”Computer Visionand Image Understanding，vol 73，no.1，pp.137-144，1999)使用接近于真值的初值来进行迭代，使用迭代算法对弱透视投影和平行透视投影下的位姿进行估计，并通过讨论中间过程中几何矩阵的秩，对目标上直线的布置提出了一些约束。这些方法可以应用在三条以上直线定位的情况。然而，数值解方法在优化过程中容易出现局部极小值，并不能保证解的唯一性；且计算量大，迭代时间长，不适合应用在实时系统中。At present, there are mainly two methods of solving, one is analytical solution and the other is numerical solution. For the analytical solution (closed solution) method, Dhome (Document 1: M.Dhome, M.Richetic, J.-T.Lapreste, and G.Rives, "Determination of the attitude of 3-D objects from a single perspective view, "IEEE Trans. Pattern Anal. Machine Intell, vol.11, no.12, pp.1266-1278, 1989) and Chen (Document 2: H. Chen, "Pose determination from line to plane correspondences: existence solution and closed form solutions ,"IEEE Trans.Pattern Anal.Machine Intell, vol 13, no.6pp.530-541.) An octagonal polynomial is derived from any three lines in space by establishing a special model coordinate system, and this octagonal polynomial can be expressed by the closed form solution to determine the pose of an object. Radu Horaud (Document 3: Horaud.R.Conio.B.Leboullkux O and Lacolle, B, "An analytic solution for the perspective 4-point Problem," Computer Vision Graphics and imageprocessing, vol 47, no.1, pp.33- 44, 1989) to obtain a quartic polynomial for three non-coplanar straight lines, and finally the pose of the object can also be determined by an iterative method through a closed-form solution. The weakness of the analytical solution method is the multi-solution problem and the large positioning error. Many scholars have proposed various iterative methods to solve the multi-solution problem of the analytical solution, that is, the numerical solution method. For the numerical solution method, Yuan (Document 4: J S-C Yuan. "A general photogrammetric method for determining object position and orientation," IEEE Transactions on Robotics and Automation, vol.5, no.2, pp.129~142) suggested that the rotation matrix The R parameter is separated from the T parameter, and the rotation matrix R parameter is calculated centrally. The rotation matrix R is represented by an orthogonal matrix, and the solution is represented by the common root of six quadratic polynomials. This common root is obtained by the Newton iterative gradient method. However, the author noticed that there will be a local optimum when using the Newton iterative gradient method. untie. Lowe (Document 5: D Lowe. "Three-dimensional Object Recognition from SingleTwo-dimensional Images," Artificial Intelligence, vol 31, pp.355-395, 1987.) uses the Newton iterative method to estimate the direction and position parameters of the object relative to the camera , like Yuan(4)'s method, Lowe noticed some problems with Newton's iterative method, and in later papers he studied how to deal with initial value and stability problems. Liu (Document 6: Y Liu, T S Huang, O DFaugeras. "Determination of camera location from 2-D to 3-D line and point correspondences," IEEE Trans. Pattern Anal. Machine Intell, vol 12, no 1, pp. 28～37, 1990) used the alternate iterative method to solve the visual parameters, the rotation matrix was represented by Euler angles, and the authors linearized the error function function, they noticed that when the three Euler angles were smaller than 30 degrees, this method works well. Thai Quynh Phong (Document 7: Thai quynhphong, "Object Pose from 2-D to 3-D Point and Line Correspondences," International Journal of Computer Vision, vol 15, pp.225-243, 1995) uses the optimal area optimization method solve. Stephane (Document 9: Stephane Christy and Radu Horaud, "Iterative Pose Computation from Line Correspondences," Computer Vision and Image Understanding, vol 73, no.1, pp.137-144, 1999) uses an initial value close to the true value to perform Iterative, an iterative algorithm is used to estimate the pose under weak perspective projection and parallel perspective projection, and by discussing the rank of the geometric matrix in the intermediate process, some constraints are put on the arrangement of lines on the target. These methods can be applied in the case of positioning more than three straight lines. However, the numerical solution method is prone to local minima during the optimization process, and cannot guarantee the uniqueness of the solution; and it has a large amount of calculation and a long iteration time, so it is not suitable for application in real-time systems.

发明内容 Contents of the invention

为了克服上述不足，本发明的目的是提供一种基于直线的目标模型，这个模型图像处理方便、选线更加自由、抗遮挡能力强、定位精度高。并针对模型上的直线设计了一种定位方法。这种定位方法计算精度高、实时性强。In order to overcome the above disadvantages, the object of the present invention is to provide a line-based object model, which is convenient for image processing, more free for line selection, strong in anti-occlusion ability, and high in positioning accuracy. And a positioning method is designed for the straight line on the model. This positioning method has high calculation accuracy and strong real-time performance.

为了实现上述目的，本发明的技术方案如下：In order to achieve the above object, the technical scheme of the present invention is as follows:

本发明基于直线的目标模型：为四棱锥模型，配设一放置底面；所述放置底面为四个分体正方形拼合成一个整体正方形结构，所述四个分体正方形在颜色上为黑白相间设置，底面上四个分体正方形的交线为位于底面中心、且相交的四条直线；所述四棱锥结构由四条底边和四条侧边共八条直线构成，四棱锥模型的四个侧面在颜色上为黑白相间设置；所述四棱锥模型绕整体正方形底面轴心旋转45度放置在底面上，四棱锥的侧面和底面的整体正方形在颜色上亦为黑白相间设置；The object model based on the straight line of the present invention is a quadrangular pyramid model equipped with a placement bottom surface; the placement bottom surface is composed of four split squares assembled into an overall square structure, and the color of the four split squares is arranged in black and white , the intersection lines of the four split squares on the bottom surface are four straight lines located at the center of the bottom surface and intersecting; It is set in black and white; the quadrangular pyramid model is rotated 45 degrees around the axis of the overall square bottom and placed on the bottom surface, and the overall square of the side and bottom of the quadrangular pyramid is also set in black and white in color;

其中：在所述四棱锥模型上的、十二条用于提取的直线中选取交于两点的三条直线构成三十二种布线组合的合作目标；交于两点的三条直线的两个交点与摄像机光心的距离不相等。Wherein: among the twelve straight lines used for extraction on the quadrangular pyramid model, three straight lines intersecting at two points are selected to constitute cooperation targets of thirty-two wiring combinations; two intersection points of the three straight lines intersecting at two points The distances from the optical center of the camera are not equal.

应用所述基于直线的目标模型的定位方法：以四棱锥模型上的十二条在黑白相间设置背景下的直线为图像摄取对象，从十二条直线的三十二种布线组合中选取一组交于两点的三条直线作为定位特征，根据直线之间的角度信息和直线的长度信息，在图像中提取出交于两点的三条直线的图像直线方程，基于几何意义并结合步长加速法求解定位数学模型，获取三维空间的合作目标位置与姿态；其中：交于两点的三条直线的两个交点与摄像机光心的距离不相等；Apply the positioning method based on the straight line target model: take the twelve straight lines on the quadrangular pyramid model under the background of black and white as the image capture object, and select a group from the thirty-two wiring combinations of the twelve straight lines The three straight lines intersecting at two points are used as positioning features. According to the angle information between the straight lines and the length information of the straight lines, the image straight line equation of the three straight lines intersecting at two points is extracted from the image. Based on the geometric meaning and combined with the step size acceleration method Solve the positioning mathematical model to obtain the position and attitude of the cooperative target in three-dimensional space; where: the distance between the two intersection points of the three straight lines intersecting at two points and the optical center of the camera is not equal;

所述定位数学模型包括：假设：空间直线L_i(i＝1，2，3)的单位方向向量是V_i(A_i，B_i，C_i)，三条直线的两个交点为第一交点p₂，第二交点p₃，其中第一交点p₂在图像上的投影为q₂(x₂，y₂，z₂)，第二交点p₃在图像上的投影为q₃(x₃，y₃，z₃)，第一交点p₂，第二交点p₃的坐标分别为(k₂x₂，k₂y₂，k₂z₂)，(k₃x₃，k₃y₃，k₃z₃)，其中k₂，k₃为待定系数；空间直线L_i在图像平面的投影为图像直线l_i，其直线方程为：a_ix_i+b_iy_i+c_i＝0，图像直线l_i上的任意一点为t_i(x_i，y_i，f)，图像直线l_i的方向向量为v_i(-b_i，a_i，0)；已知空间模型的几何约束为：第一交点p₂与摄像机光心的距离大于第二交点p₃与摄像机光心的距离；第一交点p₂和第二交点p₃的距离为a，式为：|p₂p₃|＝a；空间三条直线中第一条直线L₁和第二条直线L₂之间的夹角为α，第二条直线L₂和第三条直线L₃夹角为β，第一条直线L₁和第三条直线L₃之间的夹角为γ；由第一条直线L₁和第三条直线L₃之间的夹角为γ的约束条件，得到变量k₂的表达式f(k₂)＝0：其中变量k₂是第一交点p₂与摄像机光心o的距离和第一交点p₂在图像上的投影为q₂(x₂，y₂，z₂)与摄像机光心o的距离的比值；The positioning mathematical model includes: assumption: the unit direction vector of the space straight line L _i (i=1, 2, 3) is V _i (A _i , B _i , C _i ), and the two intersection points of the three straight lines are the first intersection points p ₂ , the second intersection point p ₃ , where the projection of the first intersection point p ₂ on the image is q ₂ (x ₂ , y ₂ , z ₂ ), and the projection of the second intersection point p ₃ on the image is q ₃ (x ₃ , y ₃ , z ₃ ), the coordinates of the first intersection point p ₂ and the second intersection point p ₃ are (k ₂ x ₂ , k ₂ y ₂ , k ₂ z ₂ ), (k ₃ x ₃ , k ₃ y ₃ , k ₃ z ₃ ), where k ₂ , k ₃ are undetermined coefficients; the projection of space line L _i on the image plane is image line l _i , and its line equation is: a _i x _i +b _i y _i + _ci = 0, any point on the image line l _i is t _i (xi _, y _i , f), and the direction vector of the image line l _i is v _i (-b _i , a _i , 0); the geometry of the space model is known The constraint is: the distance between the first intersection point p ₂ and the optical center of the camera is greater than the distance between the second intersection point p ₃ and the optical center of the camera; the distance between the first intersection point p ₂ and the second intersection point p ₃ is a, and the formula is: |p ₂ p ₃ |=a; Among the three straight lines in space, the included angle between the first straight line L ₁ and the second straight line L ₂ is α, the included angle between the second straight line L ₂ and the third straight line L ₃ is β, and the first The included angle between the first straight line L ₁ and the third straight line L ₃ is γ; from the constraint condition that the included angle between the first straight line L ₁ and the third straight line L ₃ is γ, the expression of the variable k ₂ is obtained Formula f(k ₂ )=0: where the variable k ₂ is the distance between the first intersection point p ₂ and the optical center o of the camera, and the projection of the first intersection point p ₂ on the image is q ₂ (x ₂ , y ₂ , z ₂ ) The ratio of the distance to the camera optical center o;

所述变量k₂，即第一交点p₂与摄像机光心o的距离和第一交点p₂在图像上的投影为q₂(x₂，y₂，z₂)与摄像机光心o的距离的比值的求解中基于几何意义的迭代方法是：在第一条直线L₁所在的解释平面S₁和第二条直线L₂所在的解释平面S₂之间的交线J₃上设定一个起始点p_2i，在第二条直线L₂所在的解释平面S₂和第三条直线L₃所在的解释平面S₃之间的交线J₄上找到与起始点p_2i距离为长度a的终止点p_3i；起始点p_2i，终止点p_3i连接构成第二条迭代直线L_2i；经过起始点p_2i在解释平面S₁上找到一条和第二条迭代直线L_2i成角度α的第一条迭代直线L_1i经过终止点p_3i点在解释平面S₃上找到一条和第二条迭代直线L_2i成角度β的第三条迭代直线L_3i；随着起始点p_2i与摄像机光心o之间距离的不断增大，第一条迭代直线L_1i和第三条迭代直线L_3i之间的夹角不断增大，当它们之间的夹角满足已知值γ，起始点p_2i迭代到正确位置，整个迭代过程结束。The variable k ₂ , that is, the distance between the first intersection point p ₂ and the camera optical center o and the projection of the first intersection point p ₂ on the image is the distance between q ₂ (x ₂ , y ₂ , z ₂ ) and the camera optical center o The iterative method based on the geometric meaning in the solution of the ratio is: set a line J ₃ on the intersection line J ₃ between the interpretation plane S 1 where the first straight line L ₁ is located and the interpretation plane S ₂ where the second straight line L ₂ is located. Starting point p _2i , on the intersection line J ₄ between the interpretation plane S ₂ where the second straight line L ₂ is located and the interpretation plane S ₃ where the third straight line L ₃ is located, find the distance a from the starting point p _2i The end point p _3i ; the start point p _2i , the end point p _3i are connected to form the second iterative straight line L _2i ; through the starting point p _2i , find a line on the interpretation plane S ₁ that forms an angle α with the second iterative straight line L _2i An iterative straight line L _1i passes through the end point p _3i to find a third iterative straight line L _3i which forms an angle β with the second iterative straight line L _2i on the interpretation plane S ₃ ; as the starting point p _2i and the optical center of the camera As the distance between o increases, the angle between the first iteration line L _1i and the third iteration line L _3i increases continuously. When the angle between them satisfies the known value γ, the starting point p _2i Iterate to the correct position, and the whole iteration process ends.

本发明原理：Principle of the present invention:

本发明采用四棱锥目标模型的四个侧面及放置底面涂上黑白相间的颜色，能够增加图像的对比度，更易于提取直线。同时，四棱锥合作目标上具有三十二种布线组合，可以根据定位场合的不同选取任意一组直线特征组合进行定位，合作目标的选线更加自由。合作目标具有三十二种布线组合使合作目标的抗遮挡能力增强。合作目标上具有三十二种布线组合，增加了信息的冗余，信息的冗余可以提高定位精度。四棱锥合作目标的摆放位置是以正方形底面绕四棱锥轴心旋转45度。经过大量的实验验证，这种摆放位置比其他摆放位置具有较高的定位精度。In the present invention, the four sides and the bottom surface of the quadrangular pyramid target model are painted with black and white colors, which can increase the contrast of the image and make it easier to extract straight lines. At the same time, there are 32 wiring combinations on the pyramid cooperation target, and any set of straight line feature combinations can be selected according to different positioning occasions for positioning, and the line selection of the cooperation target is more free. The cooperative target has thirty-two wiring combinations to enhance the anti-blocking ability of the cooperative target. There are thirty-two wiring combinations on the cooperation target, which increases the redundancy of information, which can improve the positioning accuracy. The placement position of the quadrangular pyramid cooperation target is that the square base rotates 45 degrees around the quadrangular pyramid axis. After a large number of experimental verifications, this placement position has higher positioning accuracy than other placement positions.

与现有技术相比，本发明更具有如下优点：Compared with the prior art, the present invention has the following advantages:

1.图像处理更容易。四棱锥的四个侧面黑白相间，增加了图像对比度，直线提取更方便。1. Image processing is easier. The four sides of the square pyramid are black and white, which increases the contrast of the image and makes it easier to extract straight lines.

2.抗遮挡能力强。直线特征本身比点特征具有更强的抗遮挡能力。同时，三十二种直线特征组合也增加了目标模型的抗遮挡能力。2. Strong anti-blocking ability. Line features are inherently more resistant to occlusion than point features. At the same time, thirty-two combinations of straight line features also increase the anti-occlusion ability of the target model.

3.选线更加自由。四棱锥模型上有三十二种直线特征组合，可以根据不同的场合需要，选取不同的直线组合定位，使得选线更加自由。3. Line selection is more free. There are thirty-two straight line feature combinations on the quadrangular pyramid model, and different straight line combinations can be selected for positioning according to the needs of different occasions, making line selection more free.

4.精度高。四棱锥模型上有三十二种直线特征组合，信息的冗余可以提高定位精度。4. High precision. There are thirty-two combinations of straight line features on the quadrangular pyramid model, and the redundancy of information can improve the positioning accuracy.

5.定位算法数学公式简单、实时性强。本发明使用具有特殊几何位置关系的直线组建立定位数学模型，直线之间的特殊位置关系能够简化求解算法。数学模型更简单，并且采用步长加速法基于几何意义搜索，迭代次数少，节省了计算时间，所以提高了系统的实时性。5. The mathematical formula of the positioning algorithm is simple and has strong real-time performance. The invention uses the straight line group with special geometric position relationship to establish the positioning mathematical model, and the special position relationship between the straight lines can simplify the solution algorithm. The mathematical model is simpler, and the step size acceleration method is used to search based on the geometric meaning, the number of iterations is small, and the calculation time is saved, so the real-time performance of the system is improved.

附图说明 Description of drawings

图1-1是本发明一个实施例的四棱锥模型设计的侧视图。Figure 1-1 is a side view of a quadrangular pyramid model design according to one embodiment of the present invention.

图1-2是四棱锥模型设计的俯视图。Figure 1-2 is a top view of the quadrangular pyramid model design.

图2是本发明四棱锥模型的实物图(如：四棱锥)。Fig. 2 is the actual figure of the quadrangular pyramid model of the present invention (such as: quadrangular pyramid).

图3本发明定位原理示意图(如：三条直线定位)。Fig. 3 is a schematic diagram of the positioning principle of the present invention (for example: three straight lines positioning).

图4是本发明求解方法示意图。Fig. 4 is a schematic diagram of the solution method of the present invention.

具体实施方式 Detailed ways

下面结合附图和实例对本发明作进一步详细说明。The present invention will be described in further detail below in conjunction with accompanying drawings and examples.

如图1-1、1-2所示，四棱锥模型的放置底面：为四个正方形拼合成一个整体正方形结构，这四个分体正方形在颜色上是黑白相间设置，底面上四个分体正方形的交线形成在底面中心相交的四条直线，正方形黑白相间的颜色使四条交线的图像处理更容易。四棱锥模型以正方形底面绕轴心旋转45度放置在底面上。As shown in Figure 1-1 and 1-2, the bottom surface of the quadrangular pyramid model: four squares are assembled into an overall square structure. The four split squares are set in black and white in color, and the four splits on the bottom surface The intersecting lines of the squares form four straight lines that intersect at the center of the base, and the alternate colors of the squares make image processing of the four intersecting lines easier. The quadrangular pyramid model is placed on the bottom surface with the square bottom surface rotated 45 degrees around the axis.

四棱锥模型：由四条底边和四条侧边共八条直线组成。四棱锥模型的四个侧面是黑白相间的颜色，使四条侧边的图像处理更简单。四棱锥的侧面和底面的正方形同时也构成黑白相间的颜色，使四棱锥模型四条底边的图像处理更方便。因此，在四棱锥模型上共有十二条便于提取的直线。从十二条直线中选取一组交于两点的三条直线(其中：两个交点与摄像机光心的距离不相等)，进行合作目标的位置与姿态计算。Pyramid model: It consists of eight straight lines with four bases and four sides. The four sides of the quadrangular pyramid model are black and white, which makes the image processing of the four sides easier. The squares on the sides and bottom of the quadrangular pyramid also form black and white colors, which makes the image processing of the four bottom edges of the quadrangular pyramid model more convenient. Therefore, there are twelve straight lines that are easy to extract on the quadrangular pyramid model. Select a group of three straight lines intersecting at two points from twelve straight lines (the distance between the two intersection points and the optical center of the camera is not equal), and calculate the position and attitude of the cooperative target.

在这十二条直线中，交于两点的三条直线(其中：两个交点与摄像机光心的距离不相等)的组合共有三十二种。可以根据不同场合的需要，选取其中的任意一种直线组合进行定位。使用一台摄像机摄取该合作目标的图像，在图像中提取出交于两点的三条直线的图像直线方程，再由以下叙述的定位算法，就可以实现合作目标的位置与姿态计算。Among the twelve straight lines, there are thirty-two combinations of three straight lines intersecting at two points (wherein the distances between the two intersection points and the optical center of the camera are not equal). According to the needs of different occasions, any combination of straight lines can be selected for positioning. Use a camera to capture the image of the cooperation target, extract the image line equations of three straight lines intersecting two points in the image, and then use the positioning algorithm described below to realize the calculation of the position and attitude of the cooperation target.

下面介绍由任意一组交于两点的三条直线(其中：两个交点与摄像机光心的距离不相等)进行定位的方法，见图3，假设空间直线L_i(i＝1，2，3)的单位方向向量是V_i(A_i，B_i，C_i)。设三条直线的两个交点为第一交点p₂，第二交点p₃，其中第一交点p₂在图像上的投影为q₂(x₂，y₂，z₂)，第二交点p₃在图像上的投影为q₃(x₃，y₃，z₃)。第一交点p₂，第二交点p₃的坐标分别为(k₂x₂，k₂y₂，k₂z₂)，(k₃x₃，k₃y₃，k₃z₃)，其中k₂，k₃为待定系数。设空间直线L_i在图像平面的投影为图像直线l_i。其直线方程为：a_ix_i+b_iy_i+c_i＝0，那么图像直线l_i上的任意一点为t_i(x_i，y_i，f)，图像直线l_i的方向向量为v_i(-b_i，a_i，0)。已知空间模型的几何约束为：第一交点p₂与摄像机光心的距离大于第二交点p₃与摄像机光心的距离。第一交点p₂和第二交点p₃的距离为a，式为：|p₂p₃|＝a。空间三条直线中第一条直线L₁和第二条直线L₂之间的夹角为α，第二条直线L₂和第三条直线L₃夹角为β，第一条直线L₁和第三条直线L₃之间的夹角为γ。见图4。The following introduces the method of positioning by any set of three straight lines intersecting at two points (wherein: the distances between the two intersection points and the optical center of the camera are not equal), see Figure 3, assuming the space straight line L _i (i=1, 2, 3 )'s unit direction vector is V _i (A _i , B _i , C _i ). Let the two intersection points of three straight lines be the first intersection point p ₂ and the second intersection point p ₃ , where the projection of the first intersection point p ₂ on the image is q ₂ (x ₂ , y ₂ , z ₂ ), and the second intersection point p ₃ The projection onto the image is q ₃ (x ₃ , y ₃ , z ₃ ). The coordinates of the first intersection point p ₂ and the second intersection point p ₃ are (k ₂ x ₂ , k ₂ y ₂ , k ₂ z ₂ ), (k ₃ x ₃ , k ₃ y ₃ , k ₃ z ₃ ), respectively, where k ₂ and k ₃ are undetermined coefficients. Let the projection of the space straight line L _i on the image plane be the image straight line l _i . Its line equation is: a _i x _i +b _i y _i + _ci =0, then any point on the image line l _i is t _i (xi _, y _i , f), and the direction vector of the image line l _i is v _i (-b _i , a _i , 0). The geometric constraints of the known space model are: the distance between the first intersection point p ₂ and the optical center of the camera is greater than the distance between the second intersection point p ₃ and the optical center of the camera. The distance between the first intersection point p ₂ and the second intersection point p ₃ is a, and the formula is: |p ₂ p ₃ |=a. Among the three straight lines in space, the angle between the first straight line L ₁ and the second straight line L ₂ is α, the angle between the second straight line L ₂ and the third straight line L ₃ is β, and the first straight line L ₁ and The angle between the third straight line L ₃ is γ. See Figure 4.

垂直于解释平面的法向量N_i＝(N_i1，N_i2，N_i3)，可以利用图像直线上的一点t_i与摄像机光心o的连线的方向向量ot_i和图像直线的方向向量v_i的叉乘得到，已知：The normal vector N _i =(N _i1 , N _i2 , N _i3 ) perpendicular to the interpretation plane can use the direction vector ot i of the line connecting a point t _i on the image line to the optical center o of the camera _and the direction vector v of the image line The cross product of _i is obtained, known:

ot_i＝(x_i y_i f)，v_i＝(-b_i a_i 0)，则有N_i＝v_i×ot_i＝(a_if b_if c_i)ot _i ＝(xi _y _i f), v _i ＝(-b _i a _i 0), then N _i ＝v _i ×ot _i ＝(a _i f b _i f c _i )

对于第一条直线L₁：其向量的模为1，得到：For the first straight line L ₁ : the modulus of its vector is 1, we get:

${A A}_{11}^{22} + + {B B}_{11}^{22} + + {C C}_{11}^{22} = = 11 - - - - - - ((11))$

第一条直线L₁位于由图像直线l₁和光心所构成的解释平面上，得到：The first line L ₁ lies on the interpretation plane formed by the image line l ₁ and the optical center, resulting in:

A₁N₁₁+B₁N₁₂+C₁N₁₃＝0 (2)A ₁ N ₁₁ +B ₁ N ₁₂ +C ₁ N ₁₃ =0 (2)

由第一条直线L₁和第二条直线L₂之间的夹角为α约束条件，得到：The angle between the first straight line L ₁ and the second straight line L ₂ is the constraint condition α, and we get:

A₁A₂+B₁B₂+C₁C₂＝cos(α) (3)A ₁ A ₂ +B ₁ B ₂ +C ₁ C ₂ ＝cos(α) (3)

第一条直线L₁的方向向量A₁，B₁，C₁用第二条直线L₂的方向向量A₂，B₂，C₂表达出来：The direction vectors A ₁ _, B ₁ , C ₁ of the first straight line L 1 are expressed by the direction vectors A ₂ , B ₂ , C ₂ of the second straight line L ₂ :

${A A}_{11} = = \frac{- - ((mn mn + + pq pq)) &PlusMinus; &PlusMinus; \sqrt{{((mn mn + + pq pq))}^{22} - - (({n no}^{22} + + {q q}^{22} + + 11)) (({p p}^{22} + + {m m}^{22} - - 11))}}{(({n no}^{22} + + {q q}^{22} + + 11))}$

B₁＝m+nA₁ (4)B ₁ =m+nA ₁ (4)

C₁＝p+qA₁ C ₁ =p+qA ₁

其中： $m = \frac{\cos (α) N_{13}}{B_{2} N_{13} - N_{12} C_{2}},$ $n = \frac{N_{11} C_{2} - A_{2} N_{13}}{B_{2} N_{13} - N_{12} C_{2}},$ $p = \frac{- \cos (α) N_{12}}{B_{2} N_{13} - C_{2} N_{12}},$ $q = \frac{A_{2} N_{12} - B_{2} N_{11}}{B_{2} N_{13} - C_{2} N_{12}}$ in: $m = \frac{\cos (α) N_{13}}{B_{2} N_{13} - N_{12} C_{2}},$ $no = \frac{N_{11} C_{2} - A_{2} N_{13}}{B_{2} N_{13} - N_{12} C_{2}},$ $p = \frac{- \cos (α) N_{12}}{B_{2} N_{13} - C_{2} N_{12}},$ $q = \frac{A_{2} N_{12} - B_{2} N_{11}}{B_{2} N_{13} - C_{2} N_{12}}$

对于第二条直线L₂：第二条直线L₂的方向向量由第一交点p₂，第二交点p₃表示为：For the second straight line L ₂ : The direction vector of the second straight line L ₂ is represented by the first intersection point p ₂ and the second intersection point p ₃ as:

${L L}_{22} = = (\begin{matrix} {A A}_{22} \\ {B B}_{22} \\ {C C}_{22} \end{matrix}) = = (\begin{matrix} {k k}_{33} {x x}_{33} - - {k k}_{22} {x x}_{22} \\ {k k}_{33} {y the y}_{33} - - {k k}_{22} {y the y}_{22} \\ {k k}_{33} {z z}_{33} - - {k k}_{22} {z z}_{22} \end{matrix}) - - - - - - ((11))$

由约束条件：第一交点p₂和第二交点p₃的距离为a，式|p₂p₃|＝a，得到：From the constraints: the distance between the first intersection point p ₂ and the second intersection point p ₃ is a, and the formula |p ₂ p ₃ |=a, we get:

(k₃z₃-k₂z₂)²+(k₃y₃-k₂y₂)²+(k₃x₃-k₂x₂)²＝a² (6)(k ₃ z ₃ -k ₂ z ₂ ) ² +(k ₃ y ₃ -k ₂ y ₂ ) ² +(k ₃ x ₃ -k ₂ x ₂ ) ² ＝a ² (6)

整理(6)式得：After sorting (6), we get:

${k k}_{33} = = \frac{- - {f f}_{11} {k k}_{22} &PlusMinus; &PlusMinus; \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))}}{22 {f f}_{11}} - - - - - - ((77))$

${f f}_{11} = = {x x}_{22}^{22} + + {y the y}_{22}^{22} + + {z z}_{22}^{22}$

其中：f₂＝-2(x₁x₂+y₁y₂+z₁z₂)Where: f ₂ ＝-2(x ₁ x ₂ +y ₁ y ₂ +z ₁ z ₂ )

${f f}_{33} = = {x x}_{11}^{22} + + {y the y}_{11}^{22} + + {z z}_{11}^{22}$

由约束条件(第一交点p₂与摄像机光心的距离大于第二交点p₃与摄像机光心的距离)，所以等式(7)取负号，得到：Due to the constraints (the distance between the first intersection point p ₂ and the optical center of the camera is greater than the distance between the second intersection point p ₃ and the optical center of the camera), so the negative sign of equation (7) is obtained:

${k k}_{33} = = \frac{- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))}}{22 {f f}_{11}} - - - - - - ((88))$

把等式(8)代入等式(5)，等式(5)可以写成关于一个变量k₂的表达式，设为等式(9)：Substituting Equation (8) into Equation (5), Equation (5) can be written as an expression about a variable k ₂ , set as Equation (9):

${L L}_{22} = = (\begin{matrix} {A A}_{22} \\ {B B}_{22} \\ {C C}_{22} \end{matrix}) = = (\begin{matrix} {k k}_{33} {x x}_{33} - - {k k}_{22} {x x}_{22} \\ {k k}_{33} {y the y}_{33} - - {k k}_{22} {y the y}_{22} \\ {k k}_{33} {z z}_{33} - - {k k}_{22} {z z}_{22} \end{matrix}) = = (\begin{matrix} \frac{- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))}}{22 {f f}_{11}} {x x}_{33} - - {k k}_{22} {x x}_{22} \\ \frac{- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))}}{22 {f f}_{11}} {y the y}_{33} - - {k k}_{22} {y the y}_{22} \\ \frac{- - {f f}_{11} {k k}_{22} - - \sqrt{{(({g g}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))}}{22 {f f}_{11}} {z z}_{33} - - {k k}_{22} {z z}_{22} \end{matrix}) - - - - - - ((99))$

对于第三条直线L₃：其向量的模为1，得到：For the third straight line L ₃ : the modulus of its vector is 1, we get:

${A A}_{33}^{22} + + {B B}_{33}^{22} + + {C C}_{33}^{22} = = 11 - - - - - - ((1010))$

第三条直线L ₃ 位于由图像直线l₃和光心所构成的解释平面上，得到：The third straight line L ₃ lies on the interpretation plane constituted by the image straight line l ₃ and the optical center, and we get:

A₃N₃₁+B₃N₃₂+C₃N₃₃＝0 (11)A ₃ N ₃₁ +B ₃ N ₃₂ +C ₃ N ₃₃ =0 (11)

以及第三条直线L₃和第二条直线L₂之间的夹角为β约束条件，得到：And the angle between the third straight line L ₃ and the second straight line L ₂ is the β constraint condition, we get:

A₃A₂+B₃B₂+C₃C₂＝cos(β) (12)A ₃ A ₂ +B ₃ B ₂ +C ₃ C ₂ ＝ cos(β) (12)

第三条直线L₃的方向向量A₃，B₃，C₃用第二条直线L₂的方向向量A₂，B₂，C₂表达出来：The direction vectors A ₃ , B ₃ , and C ₃ of the third straight line L 3 are expressed by the direction vectors A ₂ , B ₂ , and C ₂ of _the second straight line L ₂ :

${A A}_{33} = = \frac{- - ((gh gh + + wl wl)) &PlusMinus; &PlusMinus; \sqrt{{((gh gh + + wl wl))}^{22} - - (({h h}^{22} + + {l l}^{22} + + 11)) (({w w}^{22} + + {g g}^{22} - - 11))}}{(({h h}^{22} + + {l l}^{22} + + 11))}$

B₃＝g+hA₃ (13)B ₃ =g+hA ₃ (13)

C₃＝w+lA₃ C ₃ =w+lA ₃

其中： $g = \frac{\cos (β) N_{33}}{B_{2} N_{33} - N_{32} C_{2}},$ $h = \frac{N_{31} C_{2} - A_{2} N_{33}}{B_{2} N_{33} - N_{32} C_{2}},$ $w = \frac{- \cos (β) N_{32}}{B_{2} N_{33} - C_{2} N_{32}},$ $l = \frac{A_{2} N_{32} - B_{2} N_{31}}{B_{2} N_{33} - C_{2} N_{32}}$ in: $g = \frac{\cos (β) N_{33}}{B_{2} N_{33} - N_{32} C_{2}},$ $h = \frac{N_{31} C_{2} - A_{2} N_{33}}{B_{2} N_{33} - N_{32} C_{2}},$ $w = \frac{- \cos (β) N_{32}}{B_{2} N_{33} - C_{2} N_{32}},$ $l = \frac{A_{2} N_{32} - B_{2} N_{31}}{B_{2} N_{33} - C_{2} N_{32}}$

把等式(9)代入等式(4)，则等式(4)可以写成关于一个变量k₂的表达式，设为等式(14)：Substituting equation (9) into equation (4), then equation (4) can be written as an expression about a variable k ₂ , set as equation (14):

B₁＝m+nA₁ (14)B ₁ =m+nA ₁ (14)

C₁＝p+qA₁ C ₁ =p+qA ₁

其中：in:

$m m = = \frac{22 {f f}_{11} cos cos ((α α)) {N N}_{1313}}{((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({k k}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {y the y}_{33} - - 22 {f f}_{11} {k k}_{22} {y the y}_{22})) {N N}_{1313} - - {N N}_{1212} ((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {z z}_{33} - - 22 {f f}_{11} {k k}_{22} {z z}_{22}))}$

$n no = = \frac{22 {f f}_{11} {N N}_{1111} {C C}_{22} - - ((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {x x}_{33} - - {k k}_{22} {x x}_{22})) {N N}_{1313}}{((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({k k}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {y the y}_{33} - - 22 {f f}_{11} {k k}_{22} {y the y}_{22})) {N N}_{1313} - - {N N}_{1212} ((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {z z}_{33} - - 22 {f f}_{11} {k k}_{22} {z z}_{22}))}$

$p p = = \frac{22 {f f}_{11} cos cos ((α α)) {N N}_{1212}}{((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({k k}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {y the y}_{33} - - 22 {f f}_{11} {k k}_{22} {y the y}_{22})) {N N}_{1313} - - {N N}_{1212} ((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {z z}_{33} - - 22 {f f}_{11} {k k}_{22} {z z}_{22}))}$

$q q = = \frac{((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {x x}_{33} - - 22 {f f}_{11} {k k}_{22} {x x}_{22})) {N N}_{1212} - - ((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {y the y}_{33} - - 22 {f f}_{11} {k k}_{22} {y the y}_{22})) {N N}_{1111}}{((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({k k}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {y the y}_{33} - - 22 {f f}_{11} {k k}_{22} {y the y}_{22})) {N N}_{1313} - - {N N}_{1212} ((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {z z}_{33} - - 22 {f f}_{11} {k k}_{22} {z z}_{22}))}$

把等式(9)代入等式(13)，则等式(13)可以写成关于一个变量k₂的表达式，设为等式(15)：Substituting equation (9) into equation (13), then equation (13) can be written as an expression about a variable k ₂ , set as equation (15):

B₃＝g+hA₃ (15)B ₃ =g+hA ₃ (15)

C₃＝w+lA₃ C ₃ =w+lA ₃

其中：in:

$g g = = \frac{22 {f f}_{11} cos cos ((β β)) {N N}_{3333}}{((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({k k}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {y the y}_{33} - - 22 {f f}_{11} {k k}_{22} {y the y}_{22})) {N N}_{3333} - - {N N}_{3232} ((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {z z}_{33} - - 22 {f f}_{11} {k k}_{22} {z z}_{22}))}$

$h h = = \frac{22 {f f}_{11} {N N}_{3131} {C C}_{22} - - ((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {x x}_{33} - - {k k}_{22} {x x}_{22})) {N N}_{3333}}{((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({k k}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {y the y}_{33} - - 22 {f f}_{11} {k k}_{22} {y the y}_{22})) {N N}_{3333} - - {N N}_{3232} ((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {z z}_{33} - - 22 {f f}_{11} {k k}_{22} {z z}_{22}))}$

$w w = = \frac{- - 22 {f f}_{11} cos cos ((β β)) {N N}_{3232}}{((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({k k}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {y the y}_{33} - - 22 {f f}_{11} {k k}_{22} {y the y}_{22})) {N N}_{3333} - - {N N}_{3232} ((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {z z}_{33} - - 22 {f f}_{11} {k k}_{22} {z z}_{22}))}$

$l l = = \frac{((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {x x}_{33} - - 22 {f f}_{11} {k k}_{22} {x x}_{22})) {N N}_{3232} - - ((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {y the y}_{33} - - 22 {f f}_{11} {k k}_{22} {y the y}_{22})) {N N}_{3131}}{((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({k k}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {y the y}_{33} - - 22 {f f}_{11} {k k}_{22} {y the y}_{22})) {N N}_{3333} - - {N N}_{3232} ((((- - {f f}_{11} {k k}_{22} - - \sqrt{{(({f f}_{22} {k k}_{22}))}^{22} - - 44 {f f}_{11} (({f f}_{33} {k k}_{22}^{22} - - {a a}^{22}))})) {z z}_{33} - - 22 {f f}_{11} {k k}_{22} {z z}_{22}))}$

由第一条直线L₁和第三条直线L₃之间的夹角为γ的约束条件，得到：From the constraint condition that the included angle between the first straight line L ₁ and the third straight line L ₃ is γ, we get:

A₁A₃+B₁B₃+C₁C₃＝cos(γ) (16)A ₁ A ₃ +B ₁ B ₃ +C ₁ C ₃ ＝ cos(γ) (16)

把等式(14)(15)代入等式(16)，得到关于一个变量k₂的表达式f(k₂)＝0。Substituting equations (14)(15) into equation (16), the expression f(k ₂ )=0 with respect to one variable k ₂ is obtained.

(2)迭代方法(2) Iterative method

针对变量k₂的求解，设计了一种基于几何意义和步长加速法的迭代方法，即第一交点p₂与摄像机光心o的距离和第一交点p₂在图像上的投影为q₂(x₂，y₂，z₂)的距离的比值的求解中基于几何意义的迭代方法如下：Aiming at solving the variable k ₂ , an iterative method based on geometric meaning and step size acceleration method is designed, that is, the distance between the first intersection point p ₂ and the optical center o of the camera and the projection of the first intersection point p ₂ on the image are q ₂ The iterative method based on geometric meaning in the solution of the distance ratio of (x ₂ , y ₂ , z ₂ ) is as follows:

见图4，在第一条直线L₁所在的解释平面S₁和第二条直线L₂所在的解释平面S₂之间的交线J₃上设定一个起始点p_2i，在第二条直线L₂所在的解释平面S₂和第三条直线L₃所在的解释平面S₃之间的交线J₄上找到与起始点p_2i距离为长度a的终止点p_3i。起始点p_2i，终止点p_3i连接构成第二条迭代直线L_2i。经过起始点p_2i在解释平面S₁上找到一条和第二条迭代直线L_2i成角度α的第一条迭代直线L_1i经过终止点p_3i在解释平面S₃上找到一条和第二条迭代直线L_2i成角度β的第三条迭代直线L_3i。随着起始点p_2i与摄像机光心o之间距离的不断增大，第一条迭代直线L_1i和第三条迭代直线L_3i之间的夹角不断增大，当它们之间的夹角满足已知值γ，起始点p_2i迭代到正确位置，整个迭代过程结束。See Figure 4, set a starting point p _2i on the intersection line J 3 between the interpretation plane S ₁ where the first straight line L ₁ is located and the interpretation plane S ₂ where the second straight line L ₂ is located, and set a starting point p _2i on the second line L 2 On the intersection line J ₄ between the interpretation plane S ₂ where the straight line L ₂ is located and the interpretation plane S ₃ where the third straight line L ₃ is located, an end point p _3i with a distance of length a from the starting point p _2i is found. The starting point p _2i and the ending point p _3i are connected to form the second iterative straight line L _2i . Go through the starting point p _2i and find the first iterative straight line L _1i that forms an angle α with the second iterative straight line L 2i on the interpretation plane S ₁ and find the first iterative straight line L _1i and the second iterative line on the interpretation plane S ₃ through the end point p _3i The third iterative straight line L _3i of the straight line L _2i forms an angle β. As the distance between the starting point p _2i and the optical center o of the camera increases, the angle between the first iterative straight line L _1i and the third iterative straight line L _3i keeps increasing, when the angle between them Satisfying the known value γ, the starting point p _2i iterates to the correct position, and the whole iterative process ends.

实例：已知摄像机的内参数矩阵为 $(\begin{matrix} 787.887 & 0 & 0 \\ 0 & 787.887 & 0 \\ 0 & 0 & 1 \end{matrix}),$ 摄像机的视场角是36×36度。四棱锥模型上一组交于两点的三条直线(其中：两个交点与摄像机光心的距离不相等)在图像平面的图像直线为l_i(i＝1，2，3)。三条图像直线l_i的方向向量分别为v₁(0.1970，0.1970，0)，v₂(-0.1970，0，0)，v₃(0，0.1970，0)。Example: The internal parameter matrix of the known camera is $(\begin{matrix} 787.887 & 0 & 0 \\ 0 & 787.887 & 0 \\ 0 & 0 & 1 \end{matrix}),$ The field of view of the camera is 36×36 degrees. On the quadrangular pyramid model, a group of three straight lines intersecting at two points (therefore: the distances between the two intersection points and the optical center of the camera are not equal) on the image plane is l _i (i=1, 2, 3). The direction vectors of the three image straight lines l _i are respectively v ₁ (0.1970, 0.1970, 0), v ₂ (-0.1970, 0, 0), and v ₃ (0, 0.1970, 0).

由以上叙述的定位算法可以计算出四棱锥合作目标正确的位置和姿态为T(0，0，850)，R(0，0，0)。因此，由任意一组交于两点的三条直线(其中：两个交点与摄像机光心的距离不相等)可以实现合作目标位置和姿态的计算。According to the positioning algorithm described above, the correct position and attitude of the quadrangular pyramid cooperative target can be calculated as T(0, 0, 850), R(0, 0, 0). Therefore, any group of three straight lines intersecting at two points (where the distances between the two intersecting points and the optical center of the camera are not equal) can realize the calculation of the position and attitude of the cooperative target.

总之，本发明设计了一种四棱锥目标模型，黑白相间的侧面及放置底面设计能够增加图像对比度，使直线提取更容易；四棱锥目标模型上有三十二组直线特征组合，增加了目标模型的抗遮挡能力，选线更加自由；同时也能增加信息量，提高定位精度。由特殊配置直线组的特殊位置关系建立定位数学模型，表达式简单，更易于求解；采用基于几何意义并结合步长加速法进行求解，巧妙地避免了非线性优化问题，避免了出现局部极小值，求解稳定，并提高了计算效率，使系统具有很高的实时性。In a word, the present invention has designed a kind of quadrangular pyramid object model, and the design of black and white side and placement bottom surface can increase image contrast, make straight line extraction easier; There are 32 groups of linear feature combinations on the quadrangular pyramid object model, which increases the target model The anti-occlusion ability makes the line selection more free; at the same time, it can also increase the amount of information and improve the positioning accuracy. The positioning mathematical model is established by the special position relationship of the special configuration line group, the expression is simple, and it is easier to solve; the solution is based on the geometric meaning and combined with the step size acceleration method, which skillfully avoids the nonlinear optimization problem and avoids the local minimum value, the solution is stable, and the calculation efficiency is improved, so that the system has high real-time performance.

Claims

1. A target model based on a straight line, characterized in that: it is a quadrangular pyramid model, equipped with a placement bottom surface; the placement bottom surface is four split squares assembled into an integral square structure, and the four split squares are The color is black and white, and the intersection lines of the four split squares on the bottom surface are four straight lines located in the center of the bottom surface and intersecting; the quadrangular pyramid structure is composed of eight straight lines with four bottom edges and four sides. The four sides are set in alternate black and white in color; the quadrangular pyramid model is placed on the bottom surface by rotating 45 degrees around the axis of the overall square bottom, and the overall square of the sides and bottom of the quadrangular pyramid is also set in black and white in color.

2. according to the described target model based on straight lines of claim 1, it is characterized in that: on the described quadrangular pyramid model, in twelve straight lines for extraction, choose three straight lines intersecting at two points to form thirty-two kinds of wiring Combined cooperation goals.

3. The object model based on straight lines according to claim 1, wherein the distances between the two intersection points of the three straight lines intersecting at two points and the optical center of the camera are not equal.

4. a method of positioning based on the straight-line target model described in claim 1, characterized in that: take twelve straight lines on the quadrangular pyramid model under the background of black and white settings as image capture objects, from twelve straight lines A set of three straight lines intersecting at two points is selected as a positioning feature from the thirty-two wiring combinations in the system. According to the angle information between the straight lines and the length information of the straight lines, the image line of the three straight lines intersecting at two points is extracted from the image. Equation, based on the geometric meaning and combined with the step size acceleration method to solve the positioning mathematical model, to obtain the position and attitude of the cooperative target in three-dimensional space.

5. The positioning method based on the straight line target model according to claim 4, wherein the distances between the two intersection points of the three straight lines intersecting at two points and the optical center of the camera are not equal.

6. According to the positioning method based on the straight line target model of claim 2, it is characterized in that: wherein the positioning mathematical model comprises: assumption: the unit direction vector of the space straight line L _i (i=1, 2, 3) is V _i ( A _i , B _i , C _i ), the two intersection points of the three straight lines are the first intersection point p ₂ and the second intersection point p ₃ , where the projection of the first intersection point p ₂ on the image is q ₂ (x ₂ , y ₂ , z ₂ ), the projection of the second intersection point p ₃ on the image is q ₃ (x ₃ , y ₃ , z ₃ ), the coordinates of the first intersection point p ₂ and the second intersection point p ₃ are (k ₂ x ₂ , k ₂ y ₂ , k ₂ z ₂ ), (k ₃ x ₃ , k ₃ y ₃ , k ₃ z ₃ ), where k ₂ , k ₃ are undetermined coefficients; the projection of space straight line L _i on the image plane is image straight line l _i , its line equation is: a _i x _i +b _i y _i + _ci =0, any point on the image line l _i is t _i (xi _, y _i , f), the direction vector of the image line l _i is v _i ( _-bi , a _i , 0); the geometric constraints of the known space model are: the distance between the first intersection point p ₂ and the optical center of the camera is greater than the distance between the second intersection point p ₃ and the optical center of the camera; the first intersection point The distance between p ₂ and the second intersection point p ₃ is a, and the formula is: |p ₂ p ₃ |=a; among the three straight lines in space, the angle between the first straight line L ₁ and the second straight line L ₂ is α, The included angle between the second straight line L ₂ and the third straight line L ₃ is β, and the included angle between the first straight line L ₁ and the third straight line L ₃ is γ; from the first straight line L ₁ and the third straight line The constraint condition that the included angle between the straight lines L ₃ is γ, the expression f(k ₂ )=0 of the variable k _x is obtained: where the variable k ₂ is the distance between the first intersection point p ₂ and the optical center o of the camera and the first intersection point The projection of p ₂ on the image is the ratio of q ₂ (x ₂ , y ₂ , z ₂ ) to the distance from the optical center o of the camera.

7. According to the positioning method based on the linear target model of claim 4, it is characterized in that: wherein the variable k ₂ is the distance between the first intersection point p ₂ and the optical center o of the camera and the projection of the first intersection point p ₂ on the image The iterative method based on geometric meaning in solving the ratio of the distance between q ₂ (x ₂ , y ₂ , z ₂ ) and the optical center o of the camera is: the interpretation plane S ₁ where the first line L ₁ is located and the second line Set a starting point p _2i on the intersection line J ₃ between the interpretation plane S ₂ where the line L ₂ is located, and set a starting point p _2i on the interpretation plane S 2 where the second line L ₂ is located and the interpretation plane S where the third line L ₃ is located ₃ on the intersection line J ₄ to find the end point p _3i with a distance of length a _from the start point p _2i ; the start point p _2i and the end point p _3i are connected to form the second iterative straight line L _2i ; Find a first iterative straight line L _1i that forms an angle α with the second iterative straight line L _2i on the interpretation plane S ₁ . After passing through the end point p _3i , find a third iterative straight line L _3i that forms an angle β with the second iterative straight line L _2i on the interpretation plane S ₃ ; as the distance between the starting point p _2i and the optical center o of the camera continues to increases, the angle between the first iterative straight line L _1i and the third iterative straight line L _3i continues to increase, when the angle between them satisfies the known value γ, the starting point p _2i iterates to the correct position, the entire The iteration process ends.