CN103090863A - Method for measuring posture and height of dynamic platform - Google Patents

Abstract

The invention provides a method for measuring posture and height of a dynamic platform. According to the technical scheme, the method comprises the following steps of: obtaining pavement scanning lines by utilizing two two-dimensional laser scanners which are orthogonally mounted and perform vertically downward scanning; estimating an expression equation of the scanning lines by using an RANSAC (random sample consensus algorithm), and obtaining a pavement plane equation by utilizing the expression equation of the two scanning lines; and solving the posture and height of the platform by taking the pavement plane equation as reference. The equipment is simple and is low in cost. The measurement result is not influenced by the acceleration of the motion platform, drift is avoided, and the measurement accuracy is high.

Description

A kind of dynamic platform attitude and height measurement method

Technical field

The invention belongs to the photoelectric monitoring technical field, relate to a kind of attitude and height measurement method, especially a kind of dynamic platform attitude and height measurement method further relate to a kind of method of utilizing orthogonal two-dimensional laser scanning principle to carry out the dynamic platform attitude on the road surface and highly measure.

Background technology

Measure at present the main inertial measurement system that uses based on gyro and accelerometer during the high-precision real of dynamic platform attitude, and normal and GPS(Global Position System, GPS) combination is to guarantee long measuring accuracy, but the setup time before this type systematic work is longer, expensive, be unsuitable for using in some real-time measurement systems.Attitude measurement technology based on GPS is the hot fields of current research, but satellite-signal in some cases possibly can't normally receive, and attitude measurement accuracy is subject to the base length of receiver.Obliquity sensor is by measuring the component of acceleration of gravity on its sensitive axes, calculate sensor attitude angle with respect to the horizontal plane, be mainly used in the attitude measurement of carrier under static condition, only hour precision is higher in attitude of carrier angle pace of change, and under dynamic condition, the quick variation of the attitude angle of carrier platform will cause measuring accuracy significantly to descend.

Dynamic platform is in practical work process, due to reasons such as motion, mechanical vibration, platform (or mounting bracket) deformation, its attitude and highly all can changing, the attitude of the sensor of installing on platform and highly also can thereupon changing, this variation is all in real time to measure and to compensate in many high precision measuring systems.Such as, the monocular vision positioning system just needs to measure in real time shooting attitude and the height of camera, and this is one of gordian technique that guarantees bearing accuracy.

In view of above analysis, be necessary to further investigate the attitude and the height measurement method that are different from classic method and the higher dynamic platform of cost performance.

Summary of the invention

The technical problem to be solved in the present invention is: a kind of attitude and height measurement method of dynamic platform are provided, and Real-time Measuring measures attitude and the height change that dynamic platform causes due to reasons such as motion, mechanical vibration, platform (or mounting bracket) deformation.The method algorithm is simple, equipment cost is comparatively cheap, survey frequency and the precision of platform stance and height are higher.

Technical scheme of the present invention is: based on the platform stance of orthogonal two-dimensional laser scanning and the basic ideas of height measurement method be: the two dimensional laser scanning instrument of being installed, being scanned straight down by two quadratures obtains the road surface sweep trace; Use the RANSAC algorithm to estimate the expression formula equation of sweep trace, utilize the expression formula equation of two sweep traces to obtain the road plane equation; Resolve attitude and the height of platform take the road plane equation as reference.

Concrete technical scheme of the present invention is as follows:

Two two dimensional laser scanning instrument are fixed on attitude and platform highly to be measured, and two scanners are all with straight down mode synchronous scanning road surface, and the plane of scanning motion of two scanners is mutually orthogonal.Set up beam scanner system coordinate system X _S-scannerInitial point is the plane of scanning motion of two scanners and the intersection point of plane, platform place three dimensions, X-axis is parallel to plane, platform place but perpendicular to the platform working direction, Y-axis is parallel to plane, platform place and opposite with the platform working direction, Z axis is perpendicular to plane, platform place and upwards be forward, and X-axis, Y-axis and Z axis consist of right-handed coordinate system.Implement following step:

The first step, image data.

Platform moves on the road surface, when namely platform is kept in motion.Two scanner S ₁And S ₂Synchronous scanning, image data.If S ₁And S ₂At a time T scans respectively and obtains a sweep trace, makes it be respectively L ₁And L ₂Every sweep trace is comprised of n analyzing spot, L ₁At beam scanner system coordinate system X _S-scannerIn the coordinate of i analyzing spot be

(i=1,2 ..., n), L ₂At beam scanner system coordinate system X _S-scannerIn the coordinate of i analyzing spot be

(i=1,2 ..., n).

Second step calculates T sweep trace L constantly ₁And L ₂The expression formula equation.

Utilize sweep trace L ₁And L ₂The analyzing spot coordinate that comprises uses the RANSAC algorithm to estimate T L constantly ₁And L ₂The expression formula equation, be respectively:

$\left\{\begin{array}{ccc}{L}_1:& a_{1}x+b_{1}z+{\mathrm{<figure-callout\; id="1"\; label="c"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">c</figure-callout>}}_1=0,& y=0\\ L_2:& a_{2}x+b_{2}z+{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">c</figure-callout>}}_2=0,& x=0\end{array}\right.$ (formula one)

In following formula, (x, y, z) is illustrated in beam scanner system coordinate system X _S-scannerThe coordinate of middle any point.

In the 3rd step, calculate the road plane equation:

By sweep trace L ₁And L ₂Straight-line equation, obtain constantly road plane equation of T, as shown in formula two:

$A(x-\frac{b_1{\mathrm{<figure-callout\; id="1"\; label="t"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">t</figure-callout>}}_1}{2})+B(y-\frac{b_2{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">t</figure-callout>}}_2}{2})+C(z+\frac{{\mathrm{<figure-callout\; id="1"\; label="c"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">c</figure-callout>}}_1/b_1+{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">c</figure-callout>}}_2/b_2+a_1{\mathrm{<figure-callout\; id="1"\; label="t"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">t</figure-callout>}}_1+a_2{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">t</figure-callout>}}_2}{2})=0$ (formula two)

Wherein:

$\left\{\begin{array}{ccc}A=a_1{b}_2& & \\ B=a_2{\mathrm{<figure-callout\; id="1"\; label="b"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">b</figure-callout>}}_1,& {\mathrm{<figure-callout\; id="1"\; label="t"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">t</figure-callout>}}_1=\frac{\frac{a_1{b}_2}{b_1}(c_2{b}_1-c_1{b}_2)}{a_1^2{\mathrm{<figure-callout\; id="2"\; label="b"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">b</figure-callout>}}_2^2+a_2^2{\mathrm{<figure-callout\; id="1"\; label="b"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">b</figure-callout>}}_1^2+{\mathrm{<figure-callout\; id="1"\; label="b"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">b</figure-callout>}}_1^2{\mathrm{<figure-callout\; id="2"\; label="b"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">b</figure-callout>}}_2^2},& {\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">t</figure-callout>}}_2=\frac{\frac{a_2{b}_1}{b_2}(c_2{b}_1-c_1{b}_2)}{a_1^2{\mathrm{<figure-callout\; id="2"\; label="b"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">b</figure-callout>}}_2^2+a_2^2{b}_1^2+{\mathrm{<figure-callout\; id="1"\; label="b"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">b</figure-callout>}}_1^2{\mathrm{<figure-callout\; id="2"\; label="b"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">b</figure-callout>}}_2^2}\\ C=b_1{b}_2& & \end{array}\right.$ (formula three)

In the 4th step, resolve platform stance angle and height.

T platform stance angle is constantly calculated by following formula:

$\left\{\begin{array}{c}\mathrm{\&phi;}=a\tan (B/C)\\ \mathrm{\&theta;}=a\sin \left[\mathrm{sign}(-\frac{A}{C})\sqrt{\frac{1}{1+{(B/A)}^2+{(C/A)}^2}}\right]\end{array}\right.$ (formula four)

Wherein φ and θ are respectively the angle of pitch and the roll angle of platform.

On T moment platform, the height value H of tested point M is calculated by following formula:

$H=\frac{|A(x_M-\frac{b_1{\mathrm{<figure-callout\; id="1"\; label="t"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">t</figure-callout>}}_1}{2})+B(y_M-\frac{b_2{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">t</figure-callout>}}_2}{2})+C(z_M+\frac{{\mathrm{<figure-callout\; id="1"\; label="c"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">c</figure-callout>}}_1/b_1+{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">c</figure-callout>}}_2/b_2+a_1{\mathrm{<figure-callout\; id="1"\; label="t"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">t</figure-callout>}}_1+a_2{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA00002804187500021.png,HDA00002804187500022.png"\; state="\{\{state\}\}">t</figure-callout>}}_2}{2})|}{\sqrt{A^2+B^2+C^2}}$ (formula five)

Wherein, (x _M, _M, z _M) be the coordinate of tested point M in the beam scanner system coordinate system, measure with surveying instrument in advance.

Utilize said method can measure the attitude of any time platform and the platform height of tested point of anticipating of taking up an official post.

The invention has the beneficial effects as follows: utilize two scanners sweep trace of acquisition in real time, can measure in real time height and the attitude of dynamic platform.With respect to traditional attitude measurement method, the equipment of use is simple, and cost is low.Measurement result is not subjected to the impact of motion platform self-acceleration, not drift, and measuring accuracy is high.

Description of drawings

Fig. 1 is the principle schematic of dynamic platform attitude and height measurement method;

Fig. 2 is concrete implementing procedure figure of the present invention;

Fig. 3 is for adopting the comparison diagram of platform angle of pitch measurement result of the present invention and ADU sensor measurement;

Fig. 4 is for adopting the comparison diagram of platform roll angle measurement result of the present invention and ADU sensor measurement;

Fig. 5 is for adopting the real-time height measurement results of platform of the present invention.

Embodiment

Below with reference to Figure of description, the present invention is described in further details.

Fig. 1 is the principle schematic of dynamic platform attitude and height measurement method, expresses definition and the equipment mounting means of beam scanner system coordinate system, and the X-Y-Z coordinate is beam scanner system coordinate system X _S-scanerInitial point is the plane of scanning motion of two scanners and the intersection point O of plane, platform place three dimensions, X-axis is parallel to plane, platform place but perpendicular to the platform working direction, Y-axis is parallel to plane, platform place and opposite with the platform working direction, Z axis is perpendicular to plane, platform place and upwards be forward, and X-axis, Y-axis and Z axis consist of right-handed coordinate system; With two two dimensional laser scanning instrument S ₁And S ₂Be fixed on attitude and platform highly to be measured, two scanners are all with straight down mode synchronous scanning road surface, and the plane of scanning motion of two scanners is mutually orthogonal.As tested point, the m point is the subpoint of point of fixity M on the road surface with certain point of fixity M of being positioned at arbitrary height on platform, and obviously line segment Mm is perpendicular to the road surface, and its length H is height to be measured.T is by scanner S at any time ₁And S ₂Obtain two road surface sweep traces, utilize the RANSAC algorithm to implement straight line to two road surface sweep traces and estimate, obtain respectively corresponding straight line L ₁And L ₂The equation expression formula, the introduction of RANSAC algorithm principle referring to the 338th page of " mathematical method in computer vision " book (Science Press publish, Wu Fuchao work) to 344 pages.Due to L ₁And L ₂For being positioned at the straight line on the road surface, simultaneous L ₁And L ₂Straight-line equation obtain the constantly expression formula equation of road plane under the X-Y-Z coordinate system of T.After obtaining T road plane equation constantly, take the normal direction of road plane as with reference to obtaining T platform stance constantly, calculate T camera heights constantly by calculating the M point to the distance of road plane.

Fig. 2 has provided the concrete process flow diagram of implementing of the present invention.The first step, image data; Second step calculates T sweep trace L constantly ₁And L ₂The expression formula equation; In the 3rd step, calculate the road plane equation; In the 4th step, resolve platform stance angle and tested point height.

Adopt the specific embodiment of the present invention to test, verified that the present invention measures the precision of motion platform attitude and height.The major equipment that uses in specific embodiment is described as follows: the computing machine that is used for the data processing mainly is configured to: CPU is Intel Core2Duo Processor2.8GHz, uses 4GB DDR2 dual access memory; The two dimensional laser scanning instrument adopts is the LMS291-S05 type laser scanner of German SICK company; What attitude angle transducer adopted is the ADU5600 sensor (hereinafter to be referred as the ADU sensor) that StarNet's space reaches company, and survey frequency is 100Hz, and dynamic measurement precision is 1 °.

Measuring accuracy based on the attitude sensor of accelerometer is subjected to the impact of vehicle acceleration and deceleration larger, and this method is take road plane as reference data measuring table attitude, so on principle, the vehicle acceleration and deceleration can not affect its attitude measurement result.For verifying this conclusion, adopt ADU sensor, method and method provided by the invention to compare in experiment.Vehicle accelerates and retarded motion repeatedly on the road surface, during experience turn for four times, ADU sensor and scanner synchro measure, the platform stance of acquisition and tested point height measurement results are respectively as Fig. 3, Fig. 4 and shown in Figure 5.As shown in Figure 3, dotted line represents the platform angle of pitch that the ADU sensor records, and solid line represents the platform angle of pitch that adopts this method to record; The vehicle acceleration and deceleration are larger on the impact of ADU sensor measurement, larger skew appears in the vehicle angle of pitch that obtains, curve still has the upwards trend of drift after vehicle stop, and the vehicle angle of pitch that this method obtains fluctuation is less, and this is consistent with the experiment condition of Vehicle Driving Cycle in the road surface.because vehicle can occur significantly rolling in the turning process fast, as shown in Figure 4, dotted line represents the platform roll angle that the ADU sensor records, solid line represents the platform roll angle that adopts this method to record, two kinds of methods have all detected the roll angle variation that vehicle roll causes, four crest correspondences that occur in curve four turning motions of vehicle, know by inference by convention: vehicle is when swinging, its roll angle should fluctuate centered by 0 °, as shown in Figure 4, the curve that this paper method obtains and this convention are coincide better, and in the second half section, obvious whole drift error has appearred by the curve that the ADU sensor obtains.Fig. 5 is height Real-time Measuring discharge curve in the motion platform traveling process, the initial antenna height of motion platform is that 1.45m(is the height of some point of fixity on platform), because carrier vehicle in traveling process accelerates, slows down and turns, podium level constantly changes, so podium level real-time change and fluctuating up and down around 1.45m as shown in Figure 5.

Claims (1)

1. a dynamic platform attitude and height measurement method, it is characterized in that, two two dimensional laser scanning instrument are fixed on attitude and platform highly to be measured, and two scanners are all with straight down mode synchronous scanning road surface, and the plane of scanning motion of two scanners is mutually orthogonal; Set up beam scanner system coordinate system X _S-scannerInitial point is the plane of scanning motion of two scanners and the intersection point of plane, platform place three dimensions, X-axis is parallel to plane, platform place but perpendicular to the platform working direction, Y-axis is parallel to plane, platform place and opposite with the platform working direction, Z axis is perpendicular to plane, platform place and upwards be forward, and X-axis, Y-axis and Z axis consist of right-handed coordinate system; Implement following step:

The first step, image data:

When platform moves on the road surface, two scanner S ₁And S ₂Synchronous scanning, image data; If S ₁And S ₂At a time T scans respectively and obtains a sweep trace, makes it be respectively L ₁And L ₂Every sweep trace is comprised of n analyzing spot, L ₁At beam scanner system coordinate system X _S-scannerIn the coordinate of i analyzing spot be

(i=1,2 ..., n), L ₂At beam scanner system coordinate system X _S-scannerIn the coordinate of i analyzing spot be (i=1,2 ..., n);

Second step calculates T sweep trace L constantly ₁And L ₂The expression formula equation:

Utilize sweep trace L ₁And L ₂The analyzing spot coordinate that comprises uses the RANSAC algorithm to estimate T L constantly ₁And L ₂The expression formula equation, be respectively:

$\left\{\begin{array}{ccc}{L}_1:& a_{1}x+b_{1}z+c_1=0,& y=0\\ L_2:& a_{2}x+b_{2}z+c_2=0,& x=0\end{array}\right.$ Formula one

In following formula, (x, y, z) is illustrated in beam scanner system coordinate system X _S-scannerThe coordinate of middle any point;

In the 3rd step, calculate the road plane equation:

By sweep trace L ₁And L ₂Straight-line equation, obtain constantly road plane equation of T, as shown in formula two:

$A(x-\frac{b_1{t}_1}{2})+B(y-\frac{b_2{t}_2}{2})+C(z+\frac{c_1/b_1+c_2/b_2+a_1{t}_1+a_2{t}_2}{2})=0$ Formula two

Wherein:

$\left\{\begin{array}{ccc}A=a_1{b}_2& & \\ B=a_2{b}_1,& t_1=\frac{\frac{a_1{b}_2}{b_1}(c_2{b}_1-c_1{b}_2)}{a_1^2{b}_2^2+a_2^2{b}_1^2+b_1^2{b}_2^2},& t_2=\frac{\frac{a_2{b}_1}{b_2}(c_2{b}_1-c_1{b}_2)}{a_1^2{b}_2^2+a_2^2{b}_1^2+b_1^2{b}_2^2}\\ C=b_1{b}_2& & \end{array}\right.$ Formula three

In the 4th step, resolve platform stance angle and height:

T platform stance angle is constantly calculated by following formula:

$\left\{\begin{array}{c}\mathrm{\&phi;}=a\tan (B/C)\\ \mathrm{\&theta;}=a\sin \left[\mathrm{sign}(-\frac{A}{C})\sqrt{\frac{1}{1+{(B/A)}^2+{(C/A)}^2}}\right]\end{array}\right.$ Formula four

Wherein φ and θ are respectively the angle of pitch and the roll angle of platform;

On T moment platform, the height value H of tested point M is calculated by following formula:

$H=\frac{|A(x_M-\frac{b_1{t}_1}{2})+B(y_M-\frac{b_2{t}_2}{2})+C(z_M+\frac{c_1/b_1+c_2/b_2+a_1{t}_1+a_2{t}_2}{2})|}{\sqrt{A^2+B^2+C^2}}$ Formula five

Wherein, (x _M, y _M, z _M) be the coordinate of tested point M in the beam scanner system coordinate system.

Images