CN103673882B - A kind of three dimensional space coordinate autonomous tracing in intelligent vehicle and data acquisition unit thereof - Google Patents

Abstract

The invention discloses a kind of three dimensional space coordinate autonomous tracing in intelligent vehicle and data acquisition unit thereof, data acquisition unit of the present invention guides laser and mounting plane spike plate and imageing sensor on tested object by arranging, make to guide laser to form three spike points at plane spike plate, thereby can record the two dimensional motion track of described each spike point on plane spike plate that includes tested object three-dimensional motion attitude information; Three dimensional space coordinate autonomous tracing in intelligent vehicle of the present invention calculates by the two dimensional motion track that above-mentioned data acquisition unit is obtained, and can in one-shot measurement, draw the rotation hypercomplex number of tested object in motion process and x, y axle translational movement simultaneously. The present invention has advantages of far measuring distance, measuring speed is fast, certainty of measurement is high.

Description

Three-dimensional space coordinate tracing method and data acquisition device thereof

Technical Field

The invention relates to a data acquisition device for three-dimensional space coordinate tracing, and also relates to a three-dimensional space coordinate tracing method using the device.

Background

Current target tracking methods include both independent and dependent. In the surveying and mapping field, a non-independent method for distance measurement, positioning and tracking by means of an external reference is generally adopted, for example, GPS, satellite orbit measurement and theodolite total station topographic surveying and mapping are adopted, and the three-dimensional position of a measured target can be measured. Generally, an independent measurement method of inertial navigation is adopted in the use of an airplane and a submarine to obtain a quaternion of three-dimensional rotation of a measured target.

The prior art is often lack of necessary technical means in the technical field of middle and high speed ms-level, middle and long distance hundred-meter-level and high-precision mm-level surveying and mapping and occasions needing rotating postures and relative offset at the same time.

Disclosure of Invention

The technical problem to be solved by the invention is as follows: a data acquisition device for three-dimensional space coordinate tracing is provided, which can record the two-dimensional motion track of each tracing point on a planar tracing plate, wherein the tracing point contains the three-dimensional motion attitude information of a tested object.

The invention further aims to solve the technical problems that: the three-dimensional space coordinate tracing method using the data acquisition device has the characteristics of long measurement distance, high measurement speed and high measurement precision.

The technical scheme adopted by the invention is as follows:

a data acquisition device for three-dimensional space coordinate tracking, comprising: the data acquisition device comprises a guiding laser, a plane tracing plate and an image sensor; the utility model discloses a three-dimensional movement tracing device, including the plane tracer board, the plane tracer board is installed on the measured object to be measured, it is equipped with three laser beams that are parallel to each other and not coplane to guide the laser instrument, and guide the laser instrument to install on fixed position and point to fixed direction, wherein, the plane at first laser beam and second laser beam place is mutually perpendicular with the plane at first laser beam and third laser beam place, the plane tracer board is installed on the measured object to form respectively with horizontal plane, vertical plane and not be 0 and not be 90 contained angle, first to third laser beam form first to third tracer point on the plane tracer board in proper order, image sensor installs on the measured object for note down contain measured object three-dimensional movement attitude information each tracer point is at the two-dimensional movement orbit on plane tracer board.

As an improvement of the invention, the plane of the first laser beam and the second laser beam is parallel to the horizontal plane, and the plane of the first laser beam and the third laser beam is parallel to the vertical plane.

In order to maximize the overall utility of the sensitivity and measurement range of the data acquisition device, as a preferred embodiment of the present invention, as an improvement of the present invention, the planar tracer plate forms an angle of 45 ° with the horizontal plane and the vertical plane, respectively.

As a modification of the invention, the directing laser is further provided with one or more laser beams.

As an improvement of the invention, the data acquisition device further comprises a bracket; the guiding laser is installed on a fixed position through the support, and the support can adjust the direction of the guiding laser.

A three-dimensional space coordinate tracing method applying the data acquisition device comprises the following steps:

the method comprises the following steps:

measuring the tested object by using the data acquisition device to acquire a two-dimensional motion track of the first to third tracing points on the planar tracing plate in the motion process of the tested object;

step two:

equivalently decomposing the rotation motion of the plane tracing plate from the moment before the motion to the moment after the motion into first equivalent rotation and second equivalent rotation;

wherein, the first equivalent rotation is as follows: taking the first tracing point at the moment before the movement as an equivalent rotation point, rotating the planar tracing plate at the moment before the movement through a first equivalent rotation angle around a first equivalent rotation axis to a first middle plane so that the normal of the first middle plane is parallel to the normal of the planar tracing plate at the moment after the movement, wherein the first equivalent rotation axis is the normal common perpendicular line of the planar tracing plate passing through the equivalent rotation point at the moment before and after the movement, the first equivalent rotation angle is equal to the normal included angle of the planar tracing plate at the moment before and after the movement, and the position of the first tracing point before and after the first equivalent rotation is unchanged;

the second equivalent rotation is: rotating the planar tracing plate located in the first middle plane around the second equivalent rotation axis through a second equivalent rotation angle to a second middle plane, wherein the second equivalent rotation axis is a normal line of the first middle plane passing through the equivalent rotation point, the second equivalent rotation angle is equal to a space included angle between a connecting line of the first tracing point and the second tracing point after the first equivalent rotation and a connecting line of the second tracing point after the movement, and is also equal to a space included angle between a connecting line of the first tracing point and the third tracing point after the first equivalent rotation and a connecting line of the third tracing point after the movement, so that the connecting line of the first tracing point and the second tracing point after the second equivalent rotation is parallel to the connecting line at the moment after the movement, the connecting line of the first tracing point and the third tracing point after the second equivalent rotation is parallel to the connecting line at the moment after the movement, and the position of the first tracing point before and after the second equivalent rotation is unchanged;

calculating a normalized rotation quaternion of the first equivalent rotation of the plane tracing plate according to the two-dimensional motion track obtained in the first step;

step three:

calculating a normalized rotation quaternion of the second equivalent rotation of the planar tracing plate;

step four:

and multiplying the rotation quaternion of the first equivalent rotation and the second equivalent rotation of the plane tracing plate calculated in the second step and the third step, and calculating to obtain the rotation quaternion from the plane tracing plate at the moment before the movement to the second middle plane, wherein the calculation result is the rotation quaternion of the measured object from the moment before the movement to the moment after the movement.

As an improvement of the present invention, the three-dimensional space coordinate tracking method further includes:

step five:

taking the direction from the first laser beam to the second laser beam as an x axis and the direction from the first laser beam to the third laser beam as a y axis, and equivalently decomposing the translational motion of the planar tracing plate from the time before the motion to the time after the motion into the translational motion along the x axis and the y axis and the translational motion along the direction of the first laser beam; and calculating the translation amounts of the tested object from the moment before the movement to the moment after the movement along the x axis and the y axis respectively.

As an embodiment of the present invention, in the first step, from the two-dimensional motion trajectory, the following are obtained:

1) the three-dimensional space equation parameters of the three laser beams are as follows:

three-dimensional equation of first laser beam $\left\{\begin{array}{c}x=0;\\ y=0;\end{array}\right.......1.1)$

Three-dimensional equation of second laser beam $\left\{\begin{array}{c}x=Lx;\\ y=0;\end{array}\right.......1.2)$

Three-dimensional equation of third laser beam $\left\{\begin{array}{c}x=0;\\ y=Ly;\end{array}\right.......1.3)$

Wherein Lx is the distance between the first and second laser beams, and Ly is the distance between the first and third laser beams;

2) the moment before the movement of the measured object is the initial moment of the movement process, and the two-dimensional coordinates of the first to third tracing points on the planar tracing plate are as follows in sequence:

Po0＝(Po0(1),Po0(2))；

Px0＝(Px0(1),Px0(2))；

Py0＝(Py0(1),Py0(2))；

the post-movement time of the measured object is any time except the initial time of the movement process, and the two-dimensional coordinates of the first to third tracing points on the planar tracing plate sequentially are as follows:

Pon＝(Pon(1),Pon(2))；

Pxn＝(Pxn(1),Pxn(2))；

Pyn＝(Pyn(1),Pyn(2))；

in the second step, the step of calculating the normalized rotation quaternion of the first equivalent rotation of the planar tracing plate is as follows:

1) calculating the time before the movement, wherein the distance between the first tracing point and the second tracing point is as follows:

$Lx0=\sqrt{{(Px0\left(1\right)-Po0\left(1\right))}^2+{(Px0\left(2\right)-Po0\left(2\right))}^2}......2.1)$

calculating the distance between the first tracing point and the third tracing point at the moment before the movement:

$Ly0=\sqrt{{(Py0\left(1\right)-Po0\left(1\right))}^2+{(Py0\left(2\right)-Po0\left(2\right))}^2}......2.2)$

calculating the time before the movement, wherein the point normal equation coefficient of the plane tracing plate is as follows:

m0k＝[-1/(tan(arcsin(Lx/Lx0)))-1/(tan(asin(Ly/Ly0)))1]……2.3)

2) after the first equivalent rotation is calculated, the distance between the first tracing point and the second tracing point is as follows:

$Lxn=\sqrt{{(Pxn\left(1\right)-Pol\left(1\right))}^2+{(Pxn\left(2\right)-Pon\left(2\right))}^2}......2.4)$

calculating the distance between the first tracing point and the third tracing point after the first equivalent rotation:

$Lyn=\sqrt{{(Pyn\left(1\right)-Pol\left(1\right))}^2+{(Pyn\left(2\right)-Pon\left(2\right))}^2}......2.5)$

after the first equivalent rotation is calculated, the point normal equation coefficient of the plane tracer plate is as follows:

mnk＝[-1/(tan(arcsin(Lx/Lxn)))-1/(tan(asin(Ly/Lyn)))1]……2.6)

3) defining the equivalent rotation point as a three-dimensional coordinate origin, thereby calculating:

at the moment before the movement and after the second equivalent rotation, the three-dimensional coordinates of the first tracing point are both:

Pow0＝Pow1＝(0,0,0)；

at the moment before the movement, the three-dimensional coordinates of the second tracing point and the third tracing point are as follows in sequence:

Pxw0＝(Lx，0，-Lx*m0k(1)/m0k(3))……2.7)

Pyw0＝(0,Ly,-Ly*m0k(2)/m0k(3))……2.8)

after the second equivalent rotation, the three-dimensional coordinates of the second tracing point and the third tracing point are sequentially as follows:

Pxwn＝(Lx，0，-Lx*mnk(1)/mnk(3))……2.9)

Pywn＝(0,Ly,-Ly*mnk(2)/mnk(3))……2.10)

4) obtaining a three-dimensional vector of the first equivalent rotation axis as:

$Ai1=mnk\⊗m0k......3.1)$

5) obtaining the rotation angle of the first equivalent rotation as follows:

ai1＝-arccos((m0k·mnk)/(|m0k|*|mnk|))……3.2)

6) calculating the unnormalized (sum of square quaternion is not 1) rotation quaternion of the first equivalent rotation of the plane tracing plate as:

ci1t＝[cos(ai1/2)Ai1*sin(ai1/2)]；……3.3)

7) carrying out normalized processing on the formula 3.3), so that the square sum of quaternions is 1, and obtaining the normalized rotation quaternion of the first equivalent rotation of the plane tracing plate as follows:

ci1＝[cos(ai1/2)Ai1*sin(ai1/2)*(kfq)]；……3.4)

wherein kfq ═ 0.5 ((1-ci1t (1) ^2)/sum (ci1t (2:4) ^ 2));

8) after the first equivalent rotation is calculated, the three-dimensional coordinates of the second tracing point and the third tracing point are sequentially as follows:

$\mathrm{Rxw}1=C_i^1\mathrm{Pxw}0;......4.1)$

$\mathrm{Ryw}1=C_i^1\mathrm{Pyw}0;......4.2)$

wherein, $C_i^1=ci1;$

in the third step, the step of calculating the normalized rotation quaternion of the second equivalent rotation of the planar tracing plate is as follows:

1) the three-dimensional vector of the second equivalent rotating shaft is the same as the normal expression of the first middle plane and is the expression of 2.6);

2) and calculating the rotation angle of the second equivalent rotation as follows:

a12＝-arccos((Pxw1·Pxwn)/(|Pxw1|*|Pxwn|))；……5.1)

3) calculating the denormalized rotation quaternion of the second equivalent rotation of the planar tracing Board (Board) as:

c12t＝[cos(a12/2)mnk*sin(a12/2)]；……5.3)

4) normalizing the formula 5.3) to make the square sum of quaternions be 1, and obtaining the normalized rotation quaternion of the second equivalent rotation of the planar tracing plate as follows:

c12＝[cos(a12/2)mnk*sin(a12/2)*(kfq2)]……5.4)

wherein kfq2 ═ ((1-c12t (1) ^2)/sum (c12t (2:4) ^2)) ^ 0.5;

in the fourth step, the rotation quaternion from the plane tracing plate at the moment before the movement to the second middle plane is calculated as:

$m=C_i^1{C}_1^2=ci1c12;......6)$

in the fifth step, the steps of calculating the translation amounts of the object from the pre-movement time to the post-movement time along the x-axis and the y-axis are as follows:

1) calculating the slope of a first motion track, namely the slope of a two-dimensional moving track of the first tracing point on the planar tracing plate when the planar tracing plate is translated along the x axis:

kx＝(Pxn(2)-Pon(2))/(Pxn(1)-Pon(1))；……7.1)

calculating the slope of a second motion track, namely the slope of a two-dimensional moving track of the first tracing point on the planar tracing plate when the planar tracing plate is translated along the y axis:

ky＝(Pyn(2)-Pon(2))/(Pyn(1)-Pon(1))；……7.2)

2) obtaining equation coefficients of the first motion trail:

klxn＝[kx-10]；……7.3)

obtaining equation coefficients of the second motion trail:

klyn＝[ky-10]；……7.4)

3) calculating equation coefficients of a first straight line, wherein the first straight line passes through a first tracing point at the moment before the movement and is parallel to the first movement track, and the first straight line and the second movement track intersect at a second intersection point:

klxno＝[kx-1Po0(2)-kx*Po0(1)]；……8.1)

calculating equation coefficients of a second straight line, wherein the second straight line passes through the first tracing point at the moment before the movement and is parallel to the second movement track, and the second straight line and the first movement track intersect at a first intersection point:

klyno＝[ky-1Po0(2)-ky*Po0(1)]；……8.2)

and solving the coordinates of the first intersection point and the second intersection point according to the formulas 8.1) and 8.2) and the formulas 7.3) and 7.4), thereby calculating the length from the first intersection point to a first tracing point at the moment after the movement:

dx0＝norm(px)；……8.3)

and calculating the length from the second intersection point to the first tracing point at the moment after the movement:

dy0＝norm(py)；……8.4)

finally, calculating the translation amount of the measured object along the x axis from the moment before the movement to the moment after the movement according to the formulas 8.3) and 2.6), and judging the translation direction of the measured object on the x axis according to the following formula:

af＝atan(mnk(3)/mnk(2))；

dy＝dy*sin(af)；

calculating the translation amount of the measured object along the y axis from the moment before the movement to the moment after the movement according to the formulas 8.4) and 2.6), and judging the translation direction of the measured object on the y axis according to the following formula:

af＝atan(mnk(3)/mnk(1))；

dx＝dx*sin(af)；

compared with the prior art, the invention has the following beneficial effects:

firstly, the data acquisition device is provided with the guide laser, and the planar tracing plate and the image sensor are arranged on the measured object, so that the guide laser forms three tracing points on the planar tracing plate, and a two-dimensional motion track of each tracing point on the planar tracing plate, which contains three-dimensional motion posture information of the measured object, can be recorded;

secondly, the three-dimensional space coordinate tracing method can simultaneously obtain the rotation quaternion and the x-axis and y-axis translation of the measured object in the motion process in one measurement by calculating the two-dimensional motion track obtained by the data acquisition device;

thirdly, the invention has wide application range and can be widely used in various attitude tracking occasions with the detection distance of 0-1 km, the resolution precision of the translation distance of 0.05-50 mm and the resolution precision of the rotation angle of more than 0.01 ℃;

fourthly, compared with the inertial navigation of independent measurement, the method has the advantage that the problem of drift completely does not exist, namely the measurement result is irrelevant to the duration time of measurement;

therefore, the invention has the advantages of long measuring distance, high measuring speed and high measuring precision.

Drawings

The invention is described in further detail below with reference to the following figures and specific examples:

FIG. 1 is a schematic view of a data acquisition device of the present invention;

FIG. 2 is a schematic diagram of the positions of three tracing points on a planar tracing plate at a pre-movement time and a post-movement time;

FIG. 3 is a schematic diagram of the position of the planar tracer panel at a pre-exercise time and a post-exercise time;

FIG. 4 is a schematic view of the position of the planar tracer plate after a first equivalent rotation;

FIG. 5 is a schematic view of the position of the planar tracer plate after a second equivalent rotation;

FIG. 6 is a schematic view of the position of a planar tracer plate after equivalent translation;

FIG. 7 is a schematic diagram of the calculation of the x and y axis translation amounts in step five;

FIG. 8 is a second schematic diagram illustrating the calculation of the x and y axis translation in step five;

in the figure, Board _ 0-the planar tracer plate at the moment before the movement, Board _ 1-the planar tracer plate after the first equivalent rotation, Board _ 2-the planar tracer plate after the second equivalent rotation, Board _ 3-the planar tracer plate after the equivalent translation, Board _ 4-the planar tracer plate at the moment after the movement, L _ 0-the normal of the planar tracer plate at the moment before the movement, L _ 1-the normal of the planar tracer plate after the first equivalent rotation, L _ 2-the normal of the planar tracer plate after the second equivalent rotation, L _ 3-the normal of the planar tracer plate after the equivalent translation, and L _ 4-the normal of the planar tracer plate at the moment after the movement.

Detailed Description

As shown in FIG. 1, the data acquisition device for three-dimensional space coordinate tracking of the invention comprises a guiding laser, a plane tracing Board Board and an image sensor; the guiding Laser is provided with three Laser beams Laser _ o, Laser _ x and Laser _ y which are parallel to each other and are not coplanar, the guiding Laser is installed on a fixed position and points to a fixed direction, in order to enable the continuous measurement range of the data acquisition device to be larger, the fixed point of the guiding Laser is approximately pointed to the moving direction of the measured object, wherein the plane where the first Laser beam Laser _ o and the second Laser beam Laser _ x are located is parallel to a horizontal plane, the plane where the first Laser beam Laser _ o and the third Laser beam Laser _ y are located is parallel to a vertical plane, so that the plane where the first Laser beam Laser _ o and the second Laser beam Laser _ x are located is perpendicular to the plane where the first Laser beam Laser _ o and the third Laser beam Laser _ y are located, the plane tracing plate Board is installed on the measured object and forms included angles alpha and beta which are not 0 degrees and not 90 degrees with the horizontal plane and the vertical plane respectively, when the two included angles alpha and beta are 45 degrees, the sensitivity of the data acquisition device and the comprehensive practicability of the measurement range are the highest, the first to third Laser beams Laser _ o, Laser _ x and Laser _ y sequentially form first to third trace points Po, Px and Py on the planar trace Board, so that when the tested object generates three-dimensional motion posture change relative to the rotation of the x-axis, the y-axis and the z-axis and the translation of the x-axis and the y-axis, the three tracing points can generate corresponding motion track changes on the plane tracing Board, the planar tracing Board has a proper area to ensure that all laser beams can form tracing points on the planar tracing Board within a certain distance of the directional movement of the tested object, the image sensor is arranged on the tested object and used for recording the two-dimensional motion track of each tracing point on the plane tracing Board, wherein the two-dimensional motion track contains the three-dimensional motion posture information of the tested object.

The three-dimensional space coordinate tracing method is realized based on the following principle:

1 coordinate system rigid rotation.

The basic principle is as follows: see pages 295 of Qin Yongyuan from inertial navigation, the original text is as follows:

$\left\{\begin{array}{c}{q}_0=cos\frac{\mathrm{\θ}}{2}\\ q_1=\sin \frac{\mathrm{\θ}}{2}\\ q_2=\sin \frac{\mathrm{\θ}}{2}\\ q_3=\sin \frac{\mathrm{\θ}}{2}\end{array}\right\}$

with q₀、q₁、q₂、q₃Constructing a quaternion:

$Q=q_0+q_1{i}_0+q_2{j}_0+q_3{k}_0=\cos \frac{\mathrm{\θ}}{2}+\left({\mathrm{li}}_0+{\mathrm{mj}}_0+{\mathrm{nk}}_0\right)\sin \frac{\mathrm{\θ}}{2}=\cos \frac{\mathrm{\θ}}{2}+u^R\sin \frac{\mathrm{\θ}}{2}$

quaternionThe fixed point rotation of the rigid body is described, namely when only the angular position of the b system relative to the a system is concerned, the b system can be considered to be formed by one-time equivalent rotation of the R system without an intermediate process, and Q contains all information of the equivalent rotation: u. of^RTheta is the rotation axis and the rotation direction, and theta is the angle of rotation.

The coordinate system is represented herein as a rigid body, i.e., the relative position and unit length between the axes remains unchanged regardless of rotation.

According to the theory and the practical application of the scheme, the method comprises the following steps:

there is a point in real space, under some orthogonal reference frame, its coordinates are marked as:

$r^1=\left[\begin{array}{c}{\mathrm{rx}}_1\\ {\mathrm{ry}}_1\\ {\mathrm{rz}}_1\end{array}\right];$

and under another orthonormal reference system with the same origin, the coordinates thereof are marked as:

$r^2=\left[\begin{array}{c}{\mathrm{rx}}_2\\ {\mathrm{ry}}_2\\ {\mathrm{rz}}_2\end{array}\right];$

the two frames of reference are according to: by "u^RTheta is the angle of rotation "coincident" with the axis of rotation and the direction of rotation.

Definition of $C_2^1=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3-q_1{q}_2\right)\\ 2\left(q_1{q}_2-q_0{q}_3\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3-q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

Then there is $r^1=\begin{array}{cc}{C}_2^1& r^2\end{array}$

2 coordinate system translation.

When the coordinate system is only translated, the point point offset of the origin can be independently decomposed into two non-orthogonal projections of x and y in the absolute coordinate system without affecting each other. And is linear with the slope of the line intersecting the x-0 plane and the y-0 plane of the tracer plate. The slope of the intersection line can be calculated and the translation of the origin under the absolute coordinate system can be obtained according to the equation of the tracing plate.

Therefore, in the three-dimensional space coordinate tracing method of the present invention, the one-time equivalent rotation of the planar tracing Board from the initial time before the movement of the object to the arbitrary time after the movement of the object can be decomposed into the first equivalent rotation and the second equivalent rotation without intermediate process, wherein the first equivalent rotation is: hinge-like rotation about an axis perpendicular to the two normals at two normal angles; the second equivalent rotation is: gyro-like rotation around the post-rotation normal rotation angle difference in order to make the pre-and post-rotation coordinate systems completely coincide. And sequentially multiplying quaternions which are equivalently rotated twice in the front and the back to obtain quaternion of rotation transformation of the new coordinate system and the old coordinate system.

As shown in fig. 1 to 8, the three-dimensional space coordinate tracking method using the data acquisition device of the present invention includes the following steps:

the method comprises the following steps:

measuring the object to be tested by using the data acquisition device to acquire the two-dimensional motion tracks of the first to third tracing points Po, Px and Py on the planar tracing Board Board during the motion of the object to be tested, thereby obtaining:

1) three-dimensional space equation parameters of the three laser beams:

three-dimensional equation of first Laser beam Laser _ o $\left\{\begin{array}{c}x=0;\\ y=0;\end{array}\right.......1.1)$

Three-dimensional equation of second Laser beam Laser _ x $\left\{\begin{array}{c}x=Lx;\\ y=0;\end{array}\right.......1.2)$

Three-dimensional equation of third Laser beam Laser _ y $\left\{\begin{array}{c}x=0;\\ y=Ly;\end{array}\right.......1.3)$

Wherein Lx is a distance between the first and second Laser beams Laser _ o and Laser _ x, and Ly is a distance between the first and third Laser beams Laser _ o and Laser _ y, which are determined by the guidance Laser of the data acquisition device at the time of production, manufacture and installation;

2) the moment before the movement of the tested object is the initial moment of the movement process, and the two-dimensional coordinates of the first to third tracing points Po, Px and Py on the planar tracing Board are as follows in sequence:

Po0＝(Po0(1),Po0(2))；

Px0＝(Px0(1),Px0(2))；

Py0＝(Py0(1),Py0(2))；

the post-movement time of the object to be measured, that is, any time except the initial time of the movement process, that is, the time to be measured in the method, two-dimensional coordinates of the first to third tracing points Po, Px and Py on the planar tracing Board are:

Pon＝(Pon(1),Pon(2))；

Pxn＝(Pxn(1),Pxn(2))；

Pyn＝(Pyn(1),Pyn(2))；

in order to ensure the measurement accuracy, the deflection angle of the three-dimensional rotation of the measured object in space is within plus or minus 45 degrees;

step two:

equivalently decomposing the rotation motion of the plane tracer Board from the moment before the motion to the moment after the motion into first equivalent rotation and second equivalent rotation;

wherein, referring to fig. 4, the first equivalent rotation is: taking the first tracing point Po at the moment before the movement as an equivalent rotation point, and rotating the plane tracing Board Board at the moment before the movement around a first equivalent rotation axis by a first equivalent rotation angle to reach a first middle plane, so that the normal of the first middle plane is parallel to the normal of the plane tracing Board at the moment after the movement, wherein the first equivalent rotation axis is the normal common perpendicular line of the plane tracing Board Board passing through the equivalent rotation point at the moment before and after the movement, the first equivalent rotation angle is equal to the normal included angle of the plane tracing Board at the moment before and after the movement, and the first tracing point Po is unchanged at the position before and after the first equivalent rotation;

referring to fig. 5, the second equivalent rotation is: rotating the planar tracing Board located at the first middle plane through a second equivalent rotation angle to a second middle plane, wherein the second equivalent rotation axis is a normal line of the first middle plane passing through the equivalent rotation point, the second equivalent rotation angle is equal to a space included angle between a connecting line of the first tracing point Po and the second tracing point Px after the first equivalent rotation and a connecting line of the moving moment, and is also equal to a space included angle between a connecting line of the first tracing point Po and the third tracing point Py after the first equivalent rotation and a connecting line of the moving moment, so that the connecting line of the first tracing point Po and the second tracing point Px after the second equivalent rotation is parallel to the connecting line at the moment after the movement, a connecting line of the first tracing point Po and the third tracing point Py after the second equivalent rotation is parallel to a connecting line of the moment after the movement, and the position of the first tracing point Po before and after the second equivalent rotation is unchanged;

so that:

1) calculating the time before the movement, wherein the distance between the first tracing point Po and the second tracing point Px is as follows:

$Lx0=\sqrt{{(Px0\left(1\right)-Po0\left(1\right))}^2+{(Px0\left(2\right)-Po0\left(2\right))}^2}......2.1)$

calculating the distance between the first tracing point Po and the third tracing point Py at the moment before the movement:

$Ly0=\sqrt{{(Py0\left(1\right)-Po0\left(1\right))}^2+{(Py0\left(2\right)-Po0\left(2\right))}^2}......2.2)$

calculating the time before movement, wherein the point normal equation coefficient of the plane tracer Board is as follows:

m0k＝[-1/(tan(arcsin(Lx/Lx0)))-1/(tan(asin(Ly/Ly0)))1]……2.3)

2) after the first equivalent rotation is calculated, the distance between the first tracing point Po and the second tracing point Px is:

$Lxn=\sqrt{{(Pxn\left(1\right)-Po1\left(1\right))}^2+{(Pxn\left(2\right)-Pon\left(2\right))}^2}......2.4)$

after calculating the first equivalent rotation, the distance between the first tracing point Po and the third tracing point Py is:

$Lyn=\sqrt{{(Pyn\left(1\right)-Po1\left(1\right))}^2+{(Pyn\left(2\right)-Pon\left(2\right))}^2}......2.5)$

after the first equivalent rotation is calculated, the point-normal equation coefficient of the plane tracer Board is as follows:

mnk＝[-1/(tan(arcsin(Lx/Lxn)))-1/(tan(asin(Ly/Lyn)))1]……2.6)

description of the drawings: because the plane where the plane tracer Board Board is located can be expressed as a dot-normal equation:

a (x-x0) + B (y-y0) + C (z-z0) ═ 0, where the first tracer point Po is defined as the three-dimensional origin. As the origin of the three-dimensional space, x0, y0, z0 are all 0, and this point is also on the planar tracer Board, satisfying the equation of the tracer Board plane. Then there is an equation representing the plane with the plane equation coefficient [ ABC ] in this text on the tracer plate;

3) defining the equivalent rotation point as a three-dimensional coordinate origin, thereby calculating:

at the moment before the movement and after the second equivalent rotation, the three-dimensional coordinates of the first tracing point Po are:

Pow0＝Pow1＝0,0,0；

at the moment before the movement, the three-dimensional coordinates of the second tracing point Px and the third tracing point Py sequentially are:

Pxw0＝(Lx，0，-Lx*m0k(1)/m0k(3))……2.7)

Pyw0＝(0,Ly,-Ly*m0k(2)/m0k(3))……2.8)

after the second equivalent rotation, the three-dimensional coordinates of the second tracing point Px and the third tracing point Py are:

Pxwn＝(Lx，0，-Lx*mnk(1)/mnk(3))……2.9)

Pywn＝(0,Ly,-Ly*mnk(2)/mnk(3))……2.10)

4) obtaining a three-dimensional vector of the first equivalent rotation axis as:

$Ai1=mnk\⊗m0k......3.1)$

5) obtaining the rotation angle of the first equivalent rotation as follows:

ai1＝-arccos((m0k·mnk)/(|m0k|*|mnk|))……3.2)

6) calculating the unnormalized (quaternion square sum is not 1) rotation quaternion of the first equivalent rotation of the plane tracer Board as:

ci1t＝[cos(ai1/2)Ai1*sin(ai1/2)]；……3.3)

7) normalizing the formula 3.3) to make the square sum of quaternions be 1, and obtaining the normalized rotation quaternion of the first equivalent rotation of the planar tracing Board as follows:

ci1＝[cos(ai1/2)Ai1*sin(ai1/2)*(kfq)]；……3.4)

wherein kfq ═ 0.5 ((1-ci1t (1) ^2)/sum (ci1t (2:4) ^ 2));

8) after the first equivalent rotation is calculated, the three-dimensional coordinates of the second and third tracing points Px and Py are:

$Pxwe1=C_i^{1}Pxw0;......4.1)$

$Pyw1=C_i^{1}Pyw0;......4.2)$

wherein, $C_i^1=ci1;$

step three:

calculating a rotation quaternion of the second equivalent rotation of the planar tracing Board, including:

1) the three-dimensional vector of the second equivalent rotating shaft is the same as the normal expression of the first middle plane and is the expression of 2.6);

2) and calculating the rotation angle of the second equivalent rotation as follows:

a12＝-arccos((Pxw1·Pxwn)/(|Pxw1|*|Pxwn|))；……5.1)

in addition, the rotation angle of the second equivalent rotation can be calculated according to the following formula:

a12＝-arccos((Pyw1·Pywn)/(|Pyw0|*|Pywn|))；……5.2)

in order to improve the calculation accuracy, the calculation results of the 5.1) formula and the 5.2) formula may be averaged to be used as the rotation angle of the second equivalent rotation;

3) calculating the unnormalized rotation quaternion of the second equivalent rotation of the plane tracing Board as follows:

c12t＝[cos(a12/2)mnk*sin(a12/2)]；……5.3)

4) normalizing the formula 5.3) to make the square sum of quaternions be 1, and obtaining the normalized rotation quaternion of the second equivalent rotation of the planar tracing Board as follows:

c12＝[cos(a12/2)mnk*sin(a12/2)*(kfq2)]……5.4)

wherein kfq2 ═ ((1-c12t (1) ^2)/sum (c12t (2:4) ^2)) ^ 0.5;

step four:

multiplying the rotation quaternion of the first equivalent rotation and the second equivalent rotation of the plane tracing Board, and calculating to obtain a rotation quaternion from the plane tracing Board at the moment before the movement to the second middle plane:

$m=C_i^1{C}_1^2=ci1c12;......6)$

the calculation result is the rotation quaternion of the tested object from the moment before the movement to the moment after the movement.

Step five:

referring to fig. 6 to 8, with the pointing directions of the first Laser beam Laser _ o to the second Laser beam Laser _ x as an x-axis and the pointing directions of the first Laser beam Laser _ o to the third Laser beam Laser _ y as a y-axis, the translational motion of the planar tracer Board from the time before the movement to the time after the movement is equivalently decomposed into the translational motion along the x-axis and the y-axis and the translational motion along the pointing direction of the first Laser beam Laser _ o; so that:

1) calculating the slope of a first motion track Pon to Pxn, namely the slope of a two-dimensional movement track of the first tracing point Po on the planar tracing Board Board when the planar tracing Board Board is translated along the x-axis:

kx＝(Pxn(2)-Pon(2))/(Pxn(1)-Pon(1))；……7.1)

calculating the slope of the second motion trajectory Pon to Pyn, namely the slope of the two-dimensional movement trajectory of the first tracing point Po on the planar tracing Board Board when the planar tracing Board Board is translated along the y-axis:

ky＝(Pyn(2)-Pon(2))/(Pyn(1)-Pon(1))；……7.2)

2) obtaining equation coefficients of the first motion trajectory Pon to Pxn:

klxn＝[kx-10]；……7.3)

obtaining equation coefficients of the second motion trajectories Pon to Pyn:

klyn＝[ky-10]；……7.4)

description of the drawings: when the planar Board and the object under test move along the x-axis, the first trace point Po moves along a line connecting the first trace point Po and the second trace point Px (hereinafter referred to as a Pon-Pxn straight line) in the Board coordinate system. From this, the two-dimensional slope of the line Pon to Pxn can be calculated and combined with Pon to form a two-dimensional point-slope equation. The point-skew equation can be converted into a general equation of 0; this equation is represented herein by the three subscript array [ ABC ].

Here the slope is kx;

[ABC]＝[kx-10]；

when the planar Board and the object under test move along the y-axis, the first trace point Po moves along a line connecting the first trace point Po and the third trace point Py in the Board coordinate system (hereinafter referred to as a Pon-Pyn straight line). From this, the two-dimensional slope of the line Pon to Pyn can be calculated and combined with Pon to form a two-dimensional point-slope equation. The point-skew equation can be converted into a general equation of 0; this equation is represented herein by the three subscript array [ ABC ].

Here the slope is ky;

[ABC]＝[ky-10]；

3) calculating equation coefficients of first straight lines Po0 to py passing through first trace points Po0 at the pre-motion time and parallel to first motion trajectories Pon to Pxn, which intersect with second motion trajectories Pon to Pyn at second intersection points py:

klxno＝[kx-1Po0(2)-kx*Po0(1)]；……8.1)

calculating equation coefficients of second straight lines Po0 to px, where the second straight lines Po0 to px pass through the first tracking point Po0 at the pre-motion time and are parallel to the second motion trajectories Pon to Pyn, and intersect the first motion trajectories Pon to Pxn at a first intersection point px:

klyno＝[ky-1Po0(2)-ky*Po0(1)]；……8.2)

the coordinates of the first intersection point px and the second intersection point py are solved according to the equations 8.1), 8.2), 7.3), and 7.4), and the length of the first intersection point px to the first tracing point Pon at the post-exercise time is calculated by:

dx＝norm(px)；……8.3)

and calculating the length from the second intersection point py to the first tracing point Pon at the moment after the movement:

dy＝norm(py)；……8.4)

finally, calculating the translation amount of the measured object along the x axis from the moment before the movement to the moment after the movement according to the formulas 8.3) and 2.6), and judging the translation direction of the measured object on the x axis according to the following formula:

af＝atan(mnk(3)/mnk(2))；

dy＝dy*sin(af)；

ifpy(2)<0；dy＝-dy；……9.1)

calculating the translation amount of the measured object along the y axis from the moment before the movement to the moment after the movement according to the formulas 8.4) and 2.6), and judging the translation direction of the measured object on the y axis according to the following formula:

af＝atan(mnk(3)/mnk(1))；

dx＝dx*sin(af)；

ifpx(1)<0；dx＝-dx；……9.2)

description of the drawings: the tracer plate translates in three dimensions, x and y, which are orthogonal in space, and the Po point translates on the tracer plate along two non-orthogonal straight lines Pon to Pxn and Pon to Pyn.

And the translation of the Po point is decomposed according to the vectors along the directions of PonPxn and PonPyn to obtain the distance of the translation of the Po point on the tracing plate along the directions of PonPxn and PonPyn.

The coordinate px of the intersection point of the straight line passing through Po0 and PonPyn and PonPxn is obtained by the following procedure.

A(1,:)＝klxn(1:2)；

A(2,:)＝klyno(1:2)；

B＝-[klxn(3)；klyno(3)]；

px＝A\B；

The coordinate of the intersection py of the straight line passing through Po0 and PonPxn and PonPyn is obtained by the following procedure.

A(1,:)＝klyn(1:2)；

A(2,:)＝klxno(1:2)；

B＝-[klyn(3)；klxno(3)]；

py＝A\B。

Finally, translation of the subject along the z-axis can be achieved by known measurement methods.

In addition, the guiding laser of the data acquisition device can be additionally provided with one or more than one laser beams, three of the four or more than four laser beams are extracted and respectively calculated by the three-dimensional space coordinate tracing method, and the measurement accuracy of the rotation quaternion and the x-axis and y-axis translation of the measured object obtained by the invention can be improved in a repeated measurement mode.

The guide laser can also be arranged on the fixed position through the bracket, when the tested object moves to the position which deviates from the guide laser, namely a tracing point is nearly beyond the plane tracing Board, the tested object can be stopped, the direction of the guide laser is adjusted through the bracket, each tracing point returns to the proper position of the plane tracing Board again, the azimuth angle and the pitch angle of the direction of the guide laser are recorded through the bracket, then the tested object is started again, the tested object is continuously measured, and the continuous tracing of the tested object is realized.

As an application case of the device, the device can be used on attitude tracking of a tunnel section outline measuring device based on vision measurement, a laser and a longitude and latitude adjusting bracket are guided to be arranged on a track, and a tracing plate is arranged on the tunnel section outline measuring device based on vision measurement. And the device moves along with the detection vehicle device, and the rotation angle of the detection vehicle along the three axes of x, y and z and the translation displacement along the axes of x and y are obtained through the device.

The present invention is not limited to the above embodiments, and various other equivalent modifications, substitutions or alterations can be made without departing from the basic technical concept of the invention according to the common technical knowledge and conventional means in the field. For example, it is only required that the planes of the first and second Laser beams Laser _ o and Laser _ x are perpendicular to the planes of the first and third Laser beams Laser _ o and Laser _ y, and not necessarily parallel to the horizontal and vertical planes.

Claims (7)

1. A data acquisition device for three-dimensional space coordinate tracking, comprising: the data acquisition device comprises a guiding laser, a plane tracing Board (Board) and an image sensor; the directing Laser is provided with three Laser beams (Laser _ o, Laser _ x and Laser _ y) which are parallel to each other and are not coplanar, the directing Laser is arranged on a fixed position and points to a fixed direction, wherein the plane of the first Laser beam (Laser _ o) and the second Laser beam (Laser _ x) is perpendicular to the plane of the first Laser beam (Laser _ o) and the third Laser beam (Laser _ y), the planar tracking plate (Board) is mounted on the object under test, and forms included angles (alpha and beta) which are not 0 degrees and not 90 degrees with the horizontal plane and the vertical plane respectively, the first to third Laser beams (Laser _ o, Laser _ x and Laser _ y) sequentially form first to third tracing points (Po, Px and Py) on a planar tracing Board (Board), the image sensor is arranged on the tested object and used for recording the two-dimensional motion track of each tracing point on a plane tracing Board (Board) containing the three-dimensional motion posture information of the tested object.

2. The data acquisition device of claim 1, wherein: the planes of the first Laser beam (Laser _ o) and the second Laser beam (Laser _ x) are parallel to the horizontal plane, and the planes of the first Laser beam (Laser _ o) and the third Laser beam (Laser _ y) are parallel to the vertical plane.

3. The data acquisition device of claim 2, wherein: the included angles (alpha and beta) formed by the plane tracer Board (Board) and the horizontal plane and the vertical plane are 45 degrees respectively.

4. A data acquisition device according to any one of claims 1 to 3, characterized in that: the guiding laser is also provided with one or more laser beams.

5. The data acquisition device of claim 4, wherein: the data acquisition device also comprises a bracket; the guiding laser is installed on a fixed position through the support, and the support can adjust the direction of the guiding laser.

6. A three-dimensional space coordinate tracking method using the data acquisition device of any one of claims 1 to 5, comprising the steps of:

the method comprises the following steps:

measuring the object under test with the data acquisition device according to any one of claims 1 to 5 to obtain a two-dimensional motion trajectory of the first to third tracking points (Po, Px, and Py) on the planar tracking plate (Board) during the motion of the object under test;

step two:

equivalently decomposing the rotation motion of the plane tracer Board (Board) from the moment before the motion to the moment after the motion into a first equivalent rotation and a second equivalent rotation;

wherein, the first equivalent rotation is as follows: taking the first tracing point (Po) at the moment before the movement as an equivalent rotation point, rotating the planar tracing plate (Board) at the moment before the movement by a first equivalent rotation angle around a first equivalent rotation axis to a first middle plane, so that the normal of the first middle plane is parallel to the normal of the planar tracing plate (Board) at the moment after the movement, wherein the first equivalent rotation axis is the common normal of the planar tracing plate (Board) at the moment before the movement and at the moment after the movement and passes through the equivalent rotation point, and the position of the first tracing point (Po) is unchanged before and after the first equivalent rotation;

the second equivalent rotation is: rotating the plane tracing plate (Board) positioned on the first middle plane around a second equivalent rotation axis through a second equivalent rotation angle to reach a second middle plane, wherein the second equivalent rotation axis is a normal of the first middle plane passing through the equivalent rotation point, so that a connecting line of the first tracing point (Po) and the second tracing point (Px) after the second equivalent rotation is parallel to a connecting line at the moment after the movement, a connecting line of the first tracing point (Po) and the third tracing point (Py) after the second equivalent rotation is parallel to the connecting line at the moment after the movement, and the position of the first tracing point (Po) is unchanged before and after the second equivalent rotation;

calculating a normalized rotation quaternion of the first equivalent rotation of the plane tracing Board (Board) according to the two-dimensional motion track obtained in the first step;

step three:

calculating a normalized rotation quaternion of a second equivalent rotation of the planar tracing Board (Board);

step four:

and (3) multiplying the rotation quaternion of the first equivalent rotation and the second equivalent rotation of the plane tracing Board (Board) calculated in the second step and the third step, and calculating to obtain the rotation quaternion from the plane tracing Board (Board) to the second middle plane at the moment before the movement, wherein the calculation result is the rotation quaternion from the moment before the movement to the moment after the movement of the object to be measured.

7. The method of claim 6, wherein: the three-dimensional space coordinate tracking method further comprises the following steps:

step five:

equivalently decomposing the translational motion of the planar tracking plate (Board) from the moment before the movement to the moment after the movement into a translational motion along the x-axis and the y-axis and a translational motion along the first Laser beam (Laser _ o) with the pointing direction of the first Laser beam (Laser _ o) to the second Laser beam (Laser _ x) as the x-axis and the pointing direction of the first Laser beam (Laser _ o) to the third Laser beam (Laser _ y) as the y-axis; and calculating the translation amounts of the tested object from the moment before the movement to the moment after the movement along the x axis and the y axis respectively.