CN104567812A

CN104567812A - Method and device for measuring spatial position

Info

Publication number: CN104567812A
Application number: CN201310475942.0A
Authority: CN
Inventors: 王锴磊; 冯伟利; 崔桂利
Original assignee: China Academy of Launch Vehicle Technology CALT; Beijing Aerospace Institute for Metrology and Measurement Technology
Current assignee: China Academy of Launch Vehicle Technology CALT; Beijing Aerospace Institute for Metrology and Measurement Technology
Priority date: 2013-10-12
Filing date: 2013-10-12
Publication date: 2015-04-29

Abstract

The invention relates to a space position measurement method and device, aiming to establish a portable and rapid space position measurement method by using the principle of optical imaging and photography, so as to realize the rapid measurement of space position under static and dynamic conditions. The present invention comprises the following steps: step 1, establish more than 3 target points whose coordinates are known in the ground rectangular coordinate system; Calculate the equation group to get the relationship between the camera measurement coordinate system and the ground Cartesian coordinate system, and complete the positioning. The establishment of the photographic positioning method of the present invention fundamentally solves the technical problem of high-precision measurement of spatial positions, not only satisfying the fast and accurate measurement under static conditions, but also possessing the ability of fast and accurate positioning under dynamic conditions.

Description

Spatial position measurement method and device

技术领域technical field

本发明属于空间位置测量技术领域，具体涉及一种空间位置测量方法及装置。The invention belongs to the technical field of spatial position measurement, and in particular relates to a spatial position measurement method and device.

背景技术Background technique

目前空间位置的测量方法，主要有以下几种：通过全站仪实现空间位置坐标的静态测量；通过经纬仪和基准尺实现坐标的测量；通过GPS实现空间位置坐标的静态及动态测量；通过惯性系统实现空间位置的静动态测量。At present, the measurement methods of spatial position mainly include the following types: the static measurement of spatial position coordinates by total station; the measurement of coordinates by theodolite and reference ruler; the static and dynamic measurement of spatial position coordinates by GPS; the inertial system Realize static and dynamic measurement of spatial position.

全站仪测量位置坐标是基于全站仪的测距原理，通过全站仪瞄准基准坐标点，测量基准坐标点距全站仪坐标系原点的距离，同时测量基准坐标点在全站仪坐标系下的方位和俯仰角度，由此计算全站仪原点在基准点所在坐标系下的坐标，实现空间坐标的静态测量。由于全站仪的是工作建立在水平基准上对靶标进行瞄准，因此，不能实现动态的位置测量。The position coordinates of the total station are based on the principle of distance measurement of the total station. The total station is aimed at the reference coordinate point, and the distance between the reference coordinate point and the origin of the total station coordinate system is measured. At the same time, the reference coordinate point is measured in the total station coordinate system. The azimuth and pitch angle below are used to calculate the coordinates of the origin of the total station in the coordinate system where the reference point is located, and realize the static measurement of space coordinates. Since the work of the total station is based on aiming at the target on a horizontal datum, dynamic position measurement cannot be realized.

通过经纬仪及基准尺进行坐标测量时，首先将两台经纬仪对瞄，然后分别瞄准已知长度的基准尺的靶标点，由此建立一个虚拟的坐标系，最后由两台经纬仪分别瞄准被测目标点，测量被测目标点在虚拟坐标系下的坐标，实现空间坐标的静态测量。同样经纬仪的工作也是建立在水平基准上进行瞄准测量的，所以无法实现动态测量。When measuring coordinates with a theodolite and a standard ruler, first aim the two theodolites at each other, and then aim at the target point of the standard ruler with a known length, thereby establishing a virtual coordinate system, and finally aim the two theodolites at the measured target point, measure the coordinates of the measured target point in the virtual coordinate system, and realize the static measurement of space coordinates. Similarly, the work of the theodolite is also based on the horizontal datum for aiming measurement, so dynamic measurement cannot be realized.

全球定位系统（GPS）空间位置测量是基于接收GPS卫星信号进行解算的。GPS接收机根据接收到的GPS卫星信号识别GPS卫星位置、时差等信息解算GPS接收机当前的空间位置信息。可以实现空间位置信息的静态测量和动态测量。GPS空间位置测量对GPS卫星信号依赖度高，受气象因素等影响严重，且具有被限制使用的风险，不能实现全自主工作。Global Positioning System (GPS) spatial position measurement is based on receiving GPS satellite signals for solution. The GPS receiver identifies the GPS satellite position, time difference and other information according to the received GPS satellite signal to solve the current spatial position information of the GPS receiver. Static measurement and dynamic measurement of spatial position information can be realized. GPS spatial position measurement is highly dependent on GPS satellite signals, is seriously affected by meteorological factors, and has the risk of being restricted in use, so it cannot achieve fully autonomous work.

惯性系统的空间位置测量是基于测量某一指向的加速度，经积分解算当前的位置信息。可以实现静态及动态的测量，但是由于陀螺的漂移，其测量精度较差。The spatial position measurement of the inertial system is based on measuring the acceleration of a certain direction, and calculating the current position information through integration. Static and dynamic measurement can be realized, but due to the drift of the gyroscope, its measurement accuracy is poor.

上述测量方法，由于测量原理的关系，主要限于静态条件下的测量，动态测量精度较低。Due to the relationship of the measurement principle, the above measurement methods are mainly limited to the measurement under static conditions, and the dynamic measurement accuracy is low.

发明内容Contents of the invention

本发明的目的是提供一种能够实现便捷、快速的空间位置测量的空间位置测量方法及装置。以克服现有技术手段在空间位置测量上的缺陷，利用光学成像原理、通过摄影的方法建立一种便携、快速的空间位置测量方法，实现静态及动态条件下空间位置的快速测量。The object of the present invention is to provide a spatial position measurement method and device capable of realizing convenient and fast spatial position measurement. In order to overcome the shortcomings of the existing technical means in space position measurement, a portable and fast space position measurement method is established by using the principle of optical imaging and photography to realize the rapid measurement of space position under static and dynamic conditions.

本发明是这样实现的：一种空间位置测量方法，包含以下步骤：The present invention is achieved in that a kind of spatial position measurement method comprises the following steps:

步骤1、建立3个以上在地面直角坐标系中坐标已知的靶标点，分别为P₁（X₁，Y₁，Z₁），P₂（X₂，Y₂，Z₂），P3（X₃，Y₃，Z₃）……；Step 1. Establish more than 3 target points with known coordinates in the ground Cartesian coordinate system, respectively P ₁ (X ₁ , Y ₁ , Z ₁ ), P ₂ (X ₂ , Y ₂ , Z ₂ ), P3 ( X ₃ , Y ₃ , Z ₃ )...;

步骤2、拍摄包含至少3个靶标点的图像；Step 2, taking an image comprising at least 3 target points;

步骤3、以如下形式建立方程组；Step 3, set up a system of equations in the following form;

定义：O-XYZ为地面直角坐标系，S-X’Y’Z’为相机测量坐标系，S为相机的光心位置；S-UVF为像空间坐标系，o-uv为像平面坐标系；Definition: O-XYZ is the ground rectangular coordinate system, S-X'Y'Z' is the camera measurement coordinate system, S is the optical center position of the camera; S-UVF is the image space coordinate system, o-uv is the image plane coordinate system ;

S-X’Y’Z’的各个坐标轴的指向是按照绕X、Y、Z轴的旋转次序，对坐标系O-XYZ进行旋转得到，分别旋转ω,κ角度；The orientation of each coordinate axis of S-X'Y'Z' is obtained by rotating the coordinate system O-XYZ in the order of rotation around the X, Y, and Z axes, and rotating them separately ω, κ angle;

旋转矩阵R采用ω,κ表示为：The rotation matrix R uses ω, κ are expressed as:

在以下的方程组中用 $R = [\begin{matrix} a_{1}, b_{1}, c_{1} \\ a_{2}, b_{2}, c_{2} \\ a_{3}, b_{3}, c_{3} \end{matrix}],$ 的形式表示； In the following equations use $R = [\begin{matrix} a_{1}, b_{1}, c_{1} \\ a_{2}, b_{2}, c_{2} \\ a_{3}, b_{3}, c_{3} \end{matrix}],$ expressed in the form of;

S在O-XYZ中的坐标为(X_S,Y_S,Z_S)，对于任一物点P及其对应的像点p，物点P在O-XYZ中的坐标为(X,Y,Z)，物点P在S-X’Y’Z’中的坐标为(X-X_S,Y-Y_S,Z-Z_S)，物点P在S-UVF中的坐标为(X',Y',Z')，像点p在S-UVF中的坐标为(u,v,-f)；The coordinates of S in O-XYZ are (X _S , Y _S , Z _S ), for any object point P and its corresponding image point p, the coordinates of object point P in O-XYZ are (X, Y, Z), the coordinates of the object point P in S-X'Y'Z' are (XX _S , YY _S , ZZ _S ), the coordinates of the object point P in S-UVF are (X', Y', Z' ), the coordinates of the image point p in S-UVF are (u, v, -f);

其中，f为相机焦距；u、v为成像点在像平面坐标系o-uv下的坐标；Among them, f is the focal length of the camera; u and v are the coordinates of the imaging point in the image plane coordinate system o-uv;

像空间坐标系S-UVF由像平面坐标系o-uv扩展得到，U、V轴与u、v轴平行，像平面坐标系原点o，在S-UVF中的坐标为（0，0，-f）；The image space coordinate system S-UVF is obtained by extending the image plane coordinate system o-uv, the U and V axes are parallel to the u and v axes, the origin o of the image plane coordinate system, and the coordinates in S-UVF are (0, 0, - f);

得到如下方程组；The following equations are obtained;

$\begin{matrix} \{\begin{matrix} {u u}_{11} = = - - f f \frac{{a a}_{11} (({X x}_{11} - - {X x}_{s the s})) + + {b b}_{11} (({Y Y}_{11} - - {Y Y}_{s the s})) + + {c c}_{11} (({Z Z}_{11} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{11} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{11} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{11} - - {Z Z}_{s the s}))} \\ {v v}_{11} = = - - f f \frac{{a a}_{22} (({X x}_{11} - - {X x}_{s the s})) + + {b b}_{22} (({Y Y}_{11} - - {Y Y}_{s the s})) + + {c c}_{22} (({Z Z}_{11} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{11} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{11} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{11} - - {Z Z}_{s the s}))} \\ {u u}_{22} = = - - f f \frac{{a a}_{11} (({X x}_{22} - - {X x}_{s the s})) + + {b b}_{11} (({Y Y}_{22} - - {Y Y}_{s the s})) + + {c c}_{11} (({Z Z}_{22} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{22} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{22} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{22} - - {Z Z}_{s the s}))} \\ {v v}_{22} = = - - f f \frac{{a a}_{22} (({X x}_{22} - - {X x}_{s the s})) + + {b b}_{22} (({Y Y}_{22} - - {Y Y}_{s the s})) + + {c c}_{22} (({Z Z}_{22} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{22} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{22} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{22} - - {Z Z}_{s the s}))};; \\ {u u}_{33} = = - - f f \frac{{a a}_{11} (({X x}_{33} - - {X x}_{s the s})) + + {b b}_{11} (({Y Y}_{33} - - {Y Y}_{s the s})) + + {c c}_{11} (({Z Z}_{33} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{33} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{33} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{33} - - {Z Z}_{s the s}))} \\ {v v}_{33} = = - - f f \frac{{a a}_{22} (({X x}_{33} - - {X x}_{s the s})) + + {b b}_{22} (({Y Y}_{33} - - {Y Y}_{s the s})) + + {c c}_{22} (({Z Z}_{33} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{33} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{33} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{33} - - {Z Z}_{s the s}))} \end{matrix} \\ . . . . . . . . . . . . \end{matrix}$

其中，方程的数量为拍摄到的靶标点的数量的2倍；Wherein, the quantity of the equation is 2 times of the quantity of the target points photographed;

步骤4、解算方程组，算出未知量(X_S,Y_S,Z_S)、ω,κ；即可得到相机测量坐标系S-X’Y’Z’与地面直角坐标系O-XYZ的关系，完成定位。Step 4. Solve the equation system to calculate the unknown quantities (X _S , Y _S , Z _S ), ω, κ; the relationship between the camera measurement coordinate system S-X'Y'Z' and the ground Cartesian coordinate system O-XYZ can be obtained to complete the positioning.

如上所述的空间位置测量方法，其中，在步骤2中拍摄图像的时候，记录拍摄时刻，在步骤4中，输出定位信息的同时，输出时标信息，实现动态定位。In the spatial position measurement method described above, when the image is captured in step 2, the shooting time is recorded, and in step 4, time stamp information is output while positioning information is output, so as to realize dynamic positioning.

一种空间位置测量系统，其中，包含：3个以上坐标已知的靶标点，拍摄靶标点图像的相机，对相机拍摄到的图像进行处理、输出定位信息的运算设备；运算设备使用如权利要求1所述的方法进行定位运算。A spatial position measurement system, which includes: more than 3 target points with known coordinates, a camera for taking images of the target points, and a computing device for processing the images captured by the camera and outputting positioning information; the computing device is used as claimed in the claims The method described in 1 performs positioning operation.

如上所述的空间位置测量系统，其中，还包含记录相机拍摄图像时刻的时刻记录设备。The spatial position measurement system as described above further includes a time recording device for recording the time when the camera captures the image.

本发明的有益效果是能够对空间位置进行传递和精确测量，实现静态及动态条件下的高精度空间位置测量。The beneficial effect of the invention is that the space position can be transmitted and accurately measured, and the high-precision space position measurement under static and dynamic conditions can be realized.

本摄影定位方法建立在光学成像的基础上，通过三维到两维再到三维的坐标转换，整个过程依据目前高分辨率光学系统和高分辨率图像采集系统实现，无论静态或动态条件下均可实现高精度的空间位置测定。整个测量系统简单、便携。不需要进行繁杂的安装调试，整个测量过程可以在控制系统控制下自动完成，能够避免操作过程中的人为因素造成测量误差。This photographic positioning method is based on optical imaging, through three-dimensional to two-dimensional and then to three-dimensional coordinate conversion, the whole process is realized based on the current high-resolution optical system and high-resolution image acquisition system, regardless of static or dynamic conditions Realize high-precision spatial position measurement. The whole measurement system is simple and portable. There is no need for complicated installation and debugging, and the entire measurement process can be completed automatically under the control of the control system, which can avoid measurement errors caused by human factors in the operation process.

本发明所述摄影定位方法的建立，从根本上解决了空间位置高精度测量的技术难题，不仅满足静态条件下的快速、准确测量，同时具备动态条件下快速、准确定位的能力。The establishment of the photographic positioning method of the present invention fundamentally solves the technical problem of high-precision measurement of spatial positions, not only satisfying the fast and accurate measurement under static conditions, but also possessing the ability of fast and accurate positioning under dynamic conditions.

此外，还可以解决GPS定位系统、惯性定位系统静态及动态条件下的自主快速校准问题，大大提升我国在空间定位领域的技术水平。In addition, it can also solve the problem of independent rapid calibration of GPS positioning system and inertial positioning system under static and dynamic conditions, greatly improving my country's technical level in the field of space positioning.

附图说明Description of drawings

图1是摄影定位成像关系示意图；Fig. 1 is a schematic diagram of photographic positioning imaging relationship;

图2是本发明的一种空间位置测量装置的结构示意图。Fig. 2 is a structural schematic diagram of a space position measuring device of the present invention.

其中，P物点，p P的像点，O-XYZ地面直角坐标系，S-X’Y’Z’相机测量坐标系，S相机的光心位置，o-uv像平面坐标系，S-UVF从像平面坐标系扩展的像空间坐标系；Among them, P object point, p P image point, O-XYZ ground rectangular coordinate system, S-X'Y'Z' camera measurement coordinate system, optical center position of S camera, o-uv image plane coordinate system, S- The image space coordinate system that UVF extends from the image plane coordinate system;

P1、P2、P3靶标点。P1, P2, P3 target points.

具体实施方式Detailed ways

下面结合附图和实施例对本发明的一种空间位置测量方法和装置进行介绍。A spatial position measurement method and device of the present invention will be introduced below with reference to the drawings and embodiments.

首先，对于测量原理及具体实现方法进行介绍。First, the measurement principle and specific implementation methods are introduced.

本发明的原理是通过摄影的方式提取已知空间坐标的三维靶标点，在两维空间形成图像。根据光的直线传播原理，对于1个靶标点，能够建立两个直线方程。当一张图片拍摄到三个已知坐标的靶标点时，即可建立6个直线方程。六个方程中包含六个未知数（即x,y,z,ω,κ,分别为相机空间坐标系相对于靶标坐标系的3个平移量和3个旋转量，也表示了相机的3个空间坐标值以及相机朝向的3个方位角），通过解算，即可得到相机空间坐标系与靶标所在的空间坐标系之间的转换关系，也就得出相机光心在靶标所在的空间坐标系下的坐标。由此，实现对相机的摄影定位。The principle of the present invention is to extract three-dimensional target points with known spatial coordinates through photography, and form an image in two-dimensional space. According to the principle of straight-line propagation of light, two straight-line equations can be established for one target point. When a picture is taken to three target points with known coordinates, six straight line equations can be established. The six equations contain six unknowns (ie x, y, z, ω, κ, are respectively the 3 translations and 3 rotations of the camera space coordinate system relative to the target coordinate system, and also represent the 3 space coordinates of the camera and the 3 azimuths of the camera orientation), through the solution, The conversion relationship between the camera space coordinate system and the space coordinate system where the target is located can be obtained, and the coordinates of the optical center of the camera in the space coordinate system where the target is located can also be obtained. In this way, the photographic positioning of the camera is realized.

根据上述原理，摄影定位系统的定位建立在相机空间坐标系下。建立的六个直线方程可解算相机空间坐标系与靶标所在坐标系的关系。由此可见。摄影定位系统的测量不需要进行水平度等调整，只要拍摄到至少三个靶标点即可实现空间定位，实现快速的位置测量。According to the above principles, the positioning of the photography positioning system is established in the camera space coordinate system. The established six straight line equations can solve the relationship between the camera space coordinate system and the target coordinate system. It can be seen from this. The measurement of the photographic positioning system does not require adjustments such as leveling, as long as at least three target points are photographed, spatial positioning can be realized and rapid position measurement can be achieved.

如图1所示，P点为一物点，p为P的像点（位于o-uv平面），O-XYZ为地面直角坐标系，S-X’Y’Z’为相机测量坐标系，S为相机的光心位置，O-XYZ与S-X’Y’Z’的相应坐标轴互相平行；S-UVF为像空间坐标系，o-uv为像平面坐标系。As shown in Figure 1, point P is an object point, p is the image point of P (on the o-uv plane), O-XYZ is the ground Cartesian coordinate system, S-X'Y'Z' is the camera measurement coordinate system, S is the optical center position of the camera, and the corresponding coordinate axes of O-XYZ and S-X'Y'Z' are parallel to each other; S-UVF is the image space coordinate system, and o-uv is the image plane coordinate system.

图中各点在各个坐标系中具有如下的坐标：S在O-XYZ中的坐标为(X_S,Y_S,Z_S)，物点P在O-XYZ中的坐标为(X,Y,Z)，物点P在S-X’Y’Z’中的坐标为(X-X_S,Y-Y_S,Z-Z_S)，物点P在S-UVF中的坐标为(X',Y',Z')，像点p在S-UVF中的坐标为(u,v,-f)，因为S、P、p三点共线，故根据在S-UVF坐标系中的关系，有：Each point in the figure has the following coordinates in each coordinate system: the coordinates of S in O-XYZ are (X _S , Y _S , Z _S ), and the coordinates of object point P in O-XYZ are (X, Y, Z), the coordinates of the object point P in S-X'Y'Z' are (XX _S , YY _S , ZZ _S ), the coordinates of the object point P in S-UVF are (X', Y', Z' ), the coordinates of the image point p in the S-UVF are (u, v, -f), because the three points S, P, and p are collinear, so according to the relationship in the S-UVF coordinate system, there are:

$\frac{u u}{{X x}^{' '}} = = \frac{v v}{{Y Y}^{' '}} = = \frac{- - f f}{{Z Z}^{' '}} - - - - - - ((11))$

式⑴可进一步简化为：Formula (1) can be further simplified as:

$\{\begin{matrix} u u = = - - f f \frac{{X x}^{' '}}{{Z Z}^{' '}} \\ v v = = - - f f \frac{{Y Y}^{' '}}{{Z Z}^{' '}} \end{matrix} - - - - - - ((22))$

像点P点在S-XYZ中的坐标(X-X_S,Y-Y_S,Z-Z_S)与像点P在S-UVF中的坐标(X',Y',Z')的关系为：The relationship between the coordinates (XX _S , YY _S , ZZ _S ) of the image point P in S-XYZ and the coordinates (X', Y', Z') of the image point P in S-UVF is:

$[\begin{matrix} {X x}^{' '} \\ {Y Y}^{' '} \\ {Z Z}^{' '} \end{matrix}] = = R R [\begin{matrix} X x - - {X x}_{S S} \\ Y Y - - {Y Y}_{S S} \\ Z Z - - {Z Z}_{S S} \end{matrix}] - - - - - - ((33))$

其中 $R = [\begin{matrix} a_{1}, b_{1}, c_{1} \\ a_{2}, b_{2}, c_{2} \\ a_{3}, b_{3}, c_{3} \end{matrix}],$ 为旋转矩阵，是一正交矩阵，且in $R = [\begin{matrix} a_{1}, b_{1}, c_{1} \\ a_{2}, b_{2}, c_{2} \\ a_{3}, b_{3}, c_{3} \end{matrix}],$ is the rotation matrix, which is an orthogonal matrix, and

按照绕X、Y、Z轴的旋转次序，对坐标系S-XYZ进行旋转，分别旋转ω,κ角度，旋转矩阵R展开后，由ω,κ表示为：由此可知对应的a₁,a₂,a₃,b₁,b₂,b₃,c₁,c₂,c₃用三个旋转角的表示方法。Rotate the coordinate system S-XYZ according to the order of rotation around the X, Y, and Z axes, and rotate them separately ω, κ angle, after the rotation matrix R is expanded, by ω, κ are expressed as: It can be known that the corresponding a ₁ , a ₂ , a ₃ , b ₁ , b ₂ , b ₃ , c ₁ , c ₂ , c ₃ are represented by three rotation angles.

式中ω,κ为S-XYZ和S-UVF两坐标系间的旋转角，表明相机测量坐标系与像空间坐标系之间的旋转关系，亦称为相机的三个外方元素，即待求量。由式⑷可知，R的自由度为3，将⑶代入⑵可得：In the formula ω, κ are the rotation angles between the S-XYZ and S-UVF two coordinate systems, indicating the rotation relationship between the camera measurement coordinate system and the image space coordinate system, also known as the three external elements of the camera, that is, the quantity to be sought . It can be seen from formula (4) that the degree of freedom of R is 3, and substituting (3) into (2) can get:

$\{\begin{matrix} u u = = - - f f \frac{{a a}_{11} ((X x - - {X x}_{S S})) + + {b b}_{11} ((Y Y - - {Y Y}_{S S})) + + {c c}_{11} ((Z Z - - {Z Z}_{S S}))}{{a a}_{33} ((X x - - {X x}_{S S})) + + {b b}_{33} ((Y Y - - {Y Y}_{S S})) + + {c c}_{33} ((Z Z - - {Z Z}_{S S}))} \\ v v = = - - f f \frac{{a a}_{22} ((X x - - {X x}_{S S})) + + {b b}_{22} ((Y Y - - {Y Y}_{S S})) + + {c c}_{22} ((Z Z - - {Z Z}_{S S}))}{{a a}_{33} ((X x - - {X x}_{S S})) + + {b b}_{33} ((Y Y - - {Y Y}_{S S})) + + {c c}_{33} ((Z Z - - {Z Z}_{S S}))} \end{matrix} - - - - - - ((55))$

式⑸为成像基本方程，根据相机的结构，f为CCD相机的焦距，与像素当量、成像中心位置(像平面坐标系原点)等内参数信息通过精确标定得出。u、v为成像点在像平面坐标系o-uv下的坐标，其值为像素当量与成像点位置到o-uv坐标系原点的像素个数的乘积，例如，像素当量为每像素1微米，成像点位置坐标为（500,500）时，成像点u、v的值为500微米和500微米。Equation (5) is the basic imaging equation. According to the structure of the camera, f is the focal length of the CCD camera, which is obtained through accurate calibration of internal parameter information such as pixel equivalent and imaging center position (the origin of the image plane coordinate system). u and v are the coordinates of the imaging point in the image plane coordinate system o-uv, and its value is the product of the pixel equivalent and the number of pixels from the imaging point position to the origin of the o-uv coordinate system, for example, the pixel equivalent is 1 micron per pixel , when the position coordinates of the imaging point are (500,500), the values of the imaging points u and v are 500 microns and 500 microns.

根据上述分析，u、v、f为已知量，未知量为6个外方元素（ω,κ，X_S，Y_S，Z_S）。对于6个外方元素（ω,κ，X_S，Y_S，Z_S），当拍摄一个已知坐标点时，无法求取6个外方元素，也就无法确定S在O-XYZ中的坐标系下的坐标。要唯一确定S点的坐标，必须建立至少六个方程，为此，需要设置至少三个空间坐标已知的坐标点P1，P2，P3。According to the above analysis, u, v, f are known quantities, and the unknown quantities are 6 external elements ( ω, κ, X _S , Y _S , Z _S ). For the 6 outer elements ( ω, κ, X _S , Y _S , Z _S ), when shooting a known coordinate point, it is impossible to calculate the 6 outer elements, and it is impossible to determine the coordinates of S in the O-XYZ coordinate system. To uniquely determine the coordinates of point S, at least six equations must be established, and for this purpose, at least three coordinate points P1, P2, and P3 with known spatial coordinates need to be set.

图2即为摄影定位的测量装置示意图，首先建立三个在地面直角角标系O-XYZ中坐标已知的三个坐标点作为靶标点，分别为P1（X1，Y1，Z1），P2（X2，Y2，Z2），P3（X3，Y3，Z3）。根据式⑸建立成像基本方程。Figure 2 is a schematic diagram of the measurement device for photographic positioning. First, three coordinate points with known coordinates in the ground rectangular frame O-XYZ are established as target points, respectively P1 (X1, Y1, Z1), P2 ( X2, Y2, Z2), P3 (X3, Y3, Z3). According to formula (5), the basic imaging equation was established.

$\{\begin{matrix} {u u}_{11} = = - - f f \frac{{a a}_{11} (({X x}_{11} - - {X x}_{s the s})) + + {b b}_{11} (({Y Y}_{11} - - {Y Y}_{s the s})) + + {c c}_{11} (({Z Z}_{11} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{11} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{11} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{11} - - {Z Z}_{s the s}))} \\ {v v}_{11} = = - - f f \frac{{a a}_{22} (({X x}_{11} - - {X x}_{s the s})) + + {b b}_{22} (({Y Y}_{11} - - {Y Y}_{s the s})) + + {c c}_{22} (({Z Z}_{11} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{11} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{11} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{11} - - {Z Z}_{s the s}))} \\ {u u}_{22} = = - - f f \frac{{a a}_{11} (({X x}_{22} - - {X x}_{s the s})) + + {b b}_{11} (({Y Y}_{22} - - {Y Y}_{s the s})) + + {c c}_{11} (({Z Z}_{22} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{22} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{22} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{22} - - {Z Z}_{s the s}))} \\ {v v}_{22} = = - - f f \frac{{a a}_{22} (({X x}_{22} - - {X x}_{s the s})) + + {b b}_{22} (({Y Y}_{22} - - {Y Y}_{s the s})) + + {c c}_{22} (({Z Z}_{22} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{22} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{22} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{22} - - {Z Z}_{s the s}))} \\ {u u}_{33} = = - - f f \frac{{a a}_{11} (({X x}_{33} - - {X x}_{s the s})) + + {b b}_{11} (({Y Y}_{33} - - {Y Y}_{s the s})) + + {c c}_{11} (({Z Z}_{33} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{33} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{33} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{33} - - {Z Z}_{s the s}))} \\ {v v}_{33} = = - - f f \frac{{a a}_{22} (({X x}_{33} - - {X x}_{s the s})) + + {b b}_{22} (({Y Y}_{33} - - {Y Y}_{s the s})) + + {c c}_{22} (({Z Z}_{33} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{33} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{33} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{33} - - {Z Z}_{s the s}))} \end{matrix} - - - - - - ((66))$

根据式⑹即可解算光心位置S在地面直角坐标系下的坐标(X_S,Y_S,Z_S)，实现摄影定位。According to formula (6), the coordinates (X _S , Y _S , Z _S ) of the optical center position S in the ground Cartesian coordinate system can be solved to realize the photographic positioning.

在进行实际定位操作时，首先建立三个或三个以上的靶标点，供CCD相机拍摄使用。When performing actual positioning operations, firstly establish three or more target points for use by the CCD camera.

通过摄影定位系统的USB总线由键盘实现对控制系统的设置和已知坐标的输入，然后启动定位装置进行拍摄，记录拍摄图像。拍摄完成后启动图像处理程序对图像进行处理和运算，最后由显示系统显示当前的定位坐标。Through the USB bus of the photography positioning system, the setting of the control system and the input of known coordinates are realized through the keyboard, and then the positioning device is started to shoot and record the captured images. After the shooting is completed, the image processing program is started to process and calculate the image, and finally the display system displays the current positioning coordinates.

当CCD相机拍摄到多于三个靶标点时，可以得到冗余方程组，在进行方程求解时通过最小二乘法拟合进一步提高精度和可靠性。When more than three target points are captured by the CCD camera, redundant equations can be obtained, and the accuracy and reliability can be further improved by least squares fitting when solving the equations.

此外，在系统上添加时标装置，例如，此处添加同步脉冲输出装置，即可反映每次拍摄时刻，实现高精度的动态定位。In addition, adding a time scale device to the system, for example, adding a synchronous pulse output device here, can reflect each shooting moment and realize high-precision dynamic positioning.

以下，举例对于本发明所述方法的具体实施步骤进行说明。Hereinafter, specific implementation steps of the method of the present invention will be described with examples.

步骤一、建立坐标已知的3个靶标点，分别为P1（X1，Y1，Z1），P2（X2，Y2，Z2），P3（X3，Y3，Z3）。Step 1: Establish three target points with known coordinates, namely P1 (X1, Y1, Z1), P2 (X2, Y2, Z2), and P3 (X3, Y3, Z3).

此处，以3个靶标点为例进行说明，靶标点也可以多于3个。Here, three target points are taken as an example for illustration, but there may be more than three target points.

步骤二、拍摄靶标点的图像，根据已知的相机参数，分别得到3个靶标点在像空间坐标系S-UVF中的坐标(u,v,-f)。Step 2: Take images of the target points, and obtain the coordinates (u, v, -f) of the three target points in the image space coordinate system S-UVF according to the known camera parameters.

步骤三、建立方程组：Step 3: Create a system of equations:

$\{\begin{matrix} {u u}_{11} = = - - f f \frac{{a a}_{11} (({X x}_{11} - - {X x}_{s the s})) + + {b b}_{11} (({Y Y}_{11} - - {Y Y}_{s the s})) + + {c c}_{11} (({Z Z}_{11} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{11} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{11} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{11} - - {Z Z}_{s the s}))} \\ {v v}_{11} = = - - f f \frac{{a a}_{22} (({X x}_{11} - - {X x}_{s the s})) + + {b b}_{22} (({Y Y}_{11} - - {Y Y}_{s the s})) + + {c c}_{22} (({Z Z}_{11} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{11} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{11} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{11} - - {Z Z}_{s the s}))} \\ {u u}_{22} = = - - f f \frac{{a a}_{11} (({X x}_{22} - - {X x}_{s the s})) + + {b b}_{11} (({Y Y}_{22} - - {Y Y}_{s the s})) + + {c c}_{11} (({Z Z}_{22} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{22} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{22} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{22} - - {Z Z}_{s the s}))} \\ {v v}_{22} = = - - f f \frac{{a a}_{22} (({X x}_{22} - - {X x}_{s the s})) + + {b b}_{22} (({Y Y}_{22} - - {Y Y}_{s the s})) + + {c c}_{22} (({Z Z}_{22} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{22} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{22} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{22} - - {Z Z}_{s the s}))} \\ {u u}_{33} = = - - f f \frac{{a a}_{11} (({X x}_{33} - - {X x}_{s the s})) + + {b b}_{11} (({Y Y}_{33} - - {Y Y}_{s the s})) + + {c c}_{11} (({Z Z}_{33} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{33} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{33} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{33} - - {Z Z}_{s the s}))} \\ {v v}_{33} = = - - f f \frac{{a a}_{22} (({X x}_{33} - - {X x}_{s the s})) + + {b b}_{22} (({Y Y}_{33} - - {Y Y}_{s the s})) + + {c c}_{22} (({Z Z}_{33} - - {Z Z}_{s the s}))}{{a a}_{33} (({X x}_{33} - - {X x}_{s the s})) + + {b b}_{33} (({Y Y}_{33} - - {Y Y}_{s the s})) + + {c c}_{33} (({Z Z}_{33} - - {Z Z}_{s the s}))} \end{matrix}$

步骤四、解算方程组：Step 4. Solve the equation system:

该方程组具有六个（ω,κ，XS，YS，ZS）外方元素，也即六个未知数，六个方程六个未知数，即可进行求解，六个方程均含有一次项和三角函数，具体解算方式为公知常识，例如采用逐次逼近的方法进行求解。在本实施例中，直接使用MATLAB等工具软件中现有的函数进行解算。This system of equations has six ( ω, κ, XS, YS, ZS) external elements, that is, six unknowns, six equations and six unknowns, can be solved. The six equations all contain linear terms and trigonometric functions. The specific solution method is common knowledge , such as using the method of successive approximation to solve. In this embodiment, the existing functions in tool software such as MATLAB are directly used for calculation.

如图2所示，为了实现空间位置坐标的测量，搭建一套定位装置。包括CCD工业相机、测量控制系统和图像处理器。As shown in Figure 2, in order to realize the measurement of spatial position coordinates, a positioning device is built. Including CCD industrial camera, measurement control system and image processor.

装置包括已经标定内参数的CCD工业相机，CCD相机的内参数入焦距、像素当量、成像中心位置等信息均作为坐标解算的已知量参与运算，因此，在使用CCD相机前依照现有CCD相机内参数标定技术对其进行精确标定。The device includes a CCD industrial camera whose internal parameters have been calibrated. The internal parameters of the CCD camera, such as focal length, pixel equivalent, and imaging center position, are used as known quantities for coordinate solution to participate in the calculation. Therefore, before using the CCD camera, according to the existing CCD It is precisely calibrated by the camera intrinsic parameter calibration technology.

测量控制系统主要完成CCD相机的拍摄控制，同步脉冲的输出功能。The measurement control system mainly completes the shooting control of the CCD camera and the output function of the synchronous pulse.

图像采集软件系统完成CCD相机拍摄图像的采集和存储，为后续的图像处理软件系统提供原始拍摄图像。图像处理软件系统完成图像的预处理，提取各个拍摄到的靶标点的图像坐标，根据直线方程解算光心位置坐标。The image acquisition software system completes the acquisition and storage of the images captured by the CCD camera, and provides the original captured images for the subsequent image processing software system. The image processing software system completes the image preprocessing, extracts the image coordinates of each captured target point, and solves the optical center position coordinates according to the linear equation.

本技术方案基于光学成像原理，CCD相机在外触发模式下，由测量控制系统输出控制信号触发CCD相机对已知坐标的靶标点进行拍摄，同时测量控制系统输出对应每次拍摄的同步脉冲，实现拍摄时刻的控制和记录，克服了现有技术手段测量繁琐、动态性能差等缺陷。不仅可以实现静态条件下的定位，同时可以实现动态条件下的定位。This technical solution is based on the principle of optical imaging. When the CCD camera is in the external trigger mode, the measurement control system outputs a control signal to trigger the CCD camera to shoot the target point with known coordinates. At the same time, the measurement control system outputs a synchronization pulse corresponding to each shooting to achieve shooting. Time control and recording overcomes the shortcomings of existing technical means such as cumbersome measurement and poor dynamic performance. Not only positioning under static conditions can be realized, but also positioning under dynamic conditions can be realized.

其中，CCD工业相机已经标定了内参数，所述的内参数包括焦距、像素当量、成像中心位置等。CCD工业相机接收来自测量控制系统的控制信号，在该控制信号的控制下对三个或三个以上的已知坐标的靶标点进行拍摄，然后，CCD工业相机将图像信息发送给图像处理器。利用上述的方法，进行定位。Among them, the internal parameters of the CCD industrial camera have been calibrated, and the internal parameters include focal length, pixel equivalent, imaging center position, etc. The CCD industrial camera receives the control signal from the measurement control system, and under the control of the control signal, three or more target points with known coordinates are photographed, and then the CCD industrial camera sends the image information to the image processor. Use the above method for positioning.

Claims

1. a method for measuring spatial location, comprises following steps:

Step 1, set up more than 3 the target points that coordinate is known in rectangular coordinates system, be respectively P ₁(X ₁, Y ₁, Z ₁), P ₂(X ₂, Y ₂, Z ₂), P3(X ₃, Y ₃, Z ₃)

Step 2, shooting comprise the image of at least 3 target points;

Step 3, set up system of equations with following form;

Definition: O-XYZ is rectangular coordinates system, and S-X ' Y ' Z ' is camera surving coordinate system, and S is the photocentre position of camera; S-UVF is image space coordinate system, and o-uv is photo coordinate system;

The sensing of each coordinate axis of S-X ' Y ' Z ' is according to the rotation sequence around X, Y, Z axis, carries out rotation and obtains, rotate respectively coordinate system O-XYZ ω, κ angle;

Rotation matrix R adopts ω, κ are expressed as:

use in following system of equations

R = [\begin{matrix} a_{1}, b_{1}, c_{1} \\ a_{2}, b_{2}, c_{2} \\ a_{3}, b_{3}, c_{3} \end{matrix}],

form represent;

The coordinate of S in O-XYZ is (XS, YS, ZS), and for the picture point p of arbitrary object point P and correspondence thereof, the coordinate of object point P in O-XYZ is (X, Y, Z), and the coordinate of object point P in S-X ' Y ' Z ' is (X-X _s, Y-Y _s, Z-Z _s), the coordinate of object point P in S-UVF is (X', Y', Z'), and the coordinate of picture point p in S-UVF is (u, v ,-f);

Wherein, f is camera focus; U, v are the coordinate of imaging point under photo coordinate system o-uv;

Image space coordinate system S-UVF is expanded by photo coordinate system o-uv and obtains, and U, V axle is parallel with u, v axle, photo coordinate system initial point o, and the coordinate in S-UVF is (0,0 ,-f);

Obtain following system of equations;

\begin{matrix} \{\begin{matrix} u_{1} = - f \frac{a_{1} (X_{1} - X_{s}) + b_{1} (Y_{1} - Y_{s}) + c_{1} (Z_{1} - Z_{s})}{a_{3} (X_{1} - X_{s}) + b_{3} (Y_{1} - Y_{s}) + c_{3} (Z_{1} - Z_{s})} \\ v_{1} = - f \frac{a_{2} (X_{1} - X_{s}) + b_{2} (Y_{1} - Y_{s}) + c_{2} (Z_{1} - Z_{s})}{a_{3} (X_{1} - X_{s}) + b_{3} (Y_{1} - Y_{s}) + c_{3} (Z_{1} - Z_{s})} \\ u_{2} = - f \frac{a_{1} (X_{2} - X_{s}) + b_{1} (Y_{2} - Y_{s}) + c_{1} (Z_{2} - Z_{s})}{a_{3} (X_{2} - X_{s}) + b_{3} (Y_{2} - Y_{s}) + c_{3} (Z_{2} - Z_{s})} \\ v_{2} = - f \frac{a_{2} (X_{2} - X_{s}) + b_{2} (Y_{2} - Y_{s}) + c_{2} (Z_{2} - Z_{s})}{a_{3} (X_{2} - X_{s}) + b_{3} (Y_{2} - Y_{s}) + c_{3} (Z_{2} - Z_{s})}; \\ u_{3} = - f \frac{a_{1} (X_{3} - X_{s}) + b_{1} (Y_{3} - Y_{s}) + c_{1} (Z_{3} - Z_{s})}{a_{3} (X_{3} - X_{s}) + b_{3} (Y_{3} - Y_{s}) + c_{3} (Z_{3} - Z_{s})} \\ v_{3} = - f \frac{a_{2} (X_{3} - X_{s}) + b_{2} (Y_{3} - Y_{s}) + c_{2} (Z_{3} - Z_{s})}{a_{3} (X_{3} - X_{s}) + b_{3} (Y_{3} - Y_{s}) + c_{3} (Z_{3} - Z_{s})} \end{matrix} \\ . . . . . . \end{matrix}

Wherein, the quantity of equation is 2 times of the quantity of the target point photographed;

Step 4, resolve system of equations, calculate unknown quantity (X _s, Y _s, Z _s), ω, κ; Camera surving coordinate system S-X ' Y ' Z ' and the relation of rectangular coordinates system O-XYZ can be obtained, complete location.

2. method for measuring spatial location as claimed in claim 1, is characterized in that, when taking image in step 2, in the records photographing moment, in step 4, while exporting locating information, exports time scale information, realizes Kinematic Positioning.

3. a spatial position measuring system, is characterized in that, comprises: the target point that more than 3 coordinates are known, the camera of shooting target dot image, and the image photographed camera processes, exports the arithmetic facility of locating information; Arithmetic facility uses the method for claim 1 to position computing.

4. spatial position measuring system as claimed in claim 3, is characterized in that, also comprises the moment recording unit in record image shot by camera moment.