CN102261033A - Inertial measurement unit (IMU) based motion compensation algorithm of vehicular pavement detection system - Google Patents

Abstract

The invention relates to an inertial measurement unit (IMU) based motion compensation algorithm of a vehicular pavement detection system. The motion compensation algorithm is characterized by utilizing motion information to which an IMU is sensitive to carry out a real-time strapdown algorithm to obtain vehicle body motion information; fusing the vehicle body motion information with global positioning system (GPS) information to obtain real-time and accurate motion information; during measuring the pavement, utilizing another purely inertial strapdown algorithm to obtain elevation information and superposing the elevation information and information measured by a laser sensor to obtain a divergent curve containing pavement fluctuation information; filtering the divergent curve with a high pass digital filter to obtain true pavement fluctuation information; and carrying out statistics on the true pavement fluctuation information according to the international roughness index (IRI) standard to obtain the pavement flatness result. The motion compensation algorithm has the advantage of high motion compensation precision and can be used for improving the pavement flatness detection precision of the vehicular pavement detection system.

Description

A kind of fortune of the vehicle-mounted pavement detection system based on Inertial Measurement Unit is mended algorithm

Technical field

The fortune that the present invention relates to a kind of vehicle-mounted pavement detection system based on Inertial Measurement Unit is mended algorithm, can be used for the true fluctuating on auxiliary laser stadia surveying road surface, calculates the planeness on road surface.

Background technology

The pavement of road planeness is the principal element that influences the road traveling quality, is one of most important index of road surface functional performance, is one of leading indicator of check urban road and speedway construction quality.

The at present domestic method that is used to detect this index mainly contains: 3m ruler measuring method, continous way smoothness measuring equipment detection method, vehicular bump-integrator detection method.This several method is widely used, but shortcoming is obvious.Wherein, 3m ruler certainty of measurement is low, detection efficiency is low, representativeness is poor and need bow when detecting bend over, workload is big; The instrument mechanical performance of continous way smoothness measuring equipment is bigger to the influence of the precision of data, test speed for actual road speed faster the large tracts of land of high-grade highway detect still restricted; The true section that the vehicular bump-integrator time stability is poor, transformational is poor, can not provide the road surface in recent years, along with to the improving constantly and the continuous development of pavement management system (PMS) of highway quality of service requirement, surface evenness fast, accurately method of testing just becomes one of problem of paying close attention to the most in the highway construction.

Vehicle-mounted laser profiler measuring method is one of state-of-the-art in the world at present road surface measuring method, and this measuring method has fast, reliable, low, the easy enforcement of cost, automaticity advantages of higher.But the fluctuating of jolting of vehicle self has caused great kinematic error when measuring owing to the method, and it is relatively poor to cause single laser profiler to measure road surface fluctuating precision.Inertial measurement system is a kind of high-precision Motion Measuring Equipment, can measure body movement and jolt, thereby the measurement result of laser profiler is compensated.Measurement result was dispersed in time when but inertial measuring unit used separately, and measurement result has tangible step when being used in combination with GPS.

Summary of the invention

The problem that technology of the present invention solves is: overcome the deficiencies in the prior art, provide a kind of fortune of the vehicle-mounted pavement detection system based on Inertial Measurement Unit to mend algorithm, this method has realized inertia measurement data and the effective method that merges of laser profiler measurement result, realize compensation that car body in the laser profiler measuring process is jolted, improved the certainty of measurement of vehicle-mounted pavement detection system.

Technical solution of the present invention is: a kind of fortune of the vehicle-mounted pavement detection system based on Inertial Measurement Unit is mended algorithm, and its characteristics are to comprise the following steps:

(1) the Strapdown Inertial Units measuring unit carries out initial alignment, determines the initial orientation angle of Strapdown Inertial Units measuring unit

Initial pitching angle theta ₀With initial roll angle γ ₀

(2) the Strapdown Inertial Units measuring unit is gathered three-axis gyroscope data and three axis accelerometer data in real time, carries out compensation data, obtains three axis angular rates and 3-axis acceleration;

(3) Strapdown Inertial Units measuring unit startup strapdown resolves algorithm, and three axis angular rates that utilization obtains and 3-axis acceleration carry out strapdown and resolve attitude, position and the speed of real-time update quick-connecting inertia measurement unit;

(4) strapdown resolves the result and GPS information is carried out the information fusion filtering operation, calculate attitude error, site error and velocity error, and attitude information, positional information and velocity information that strapdown resolves in the algorithm revised, zero inclined to one side value with inertia device feeds back in the compensation data algorithm simultaneously, helps to improve the precision of compensation data;

(5) when vehicle enters highway section to be measured, adopt double strapdown to resolve algorithm, just utilize this constantly attitude, position and the velocity calculated result in information fusion algorithm start new strapdown and resolve process and carry out pure inertia and resolve as initial value;

(6) strapdown in the test segment is resolved the road surface fluctuating information stack that the pure inertia elevation information that obtains and laser sensor record, stack result is carried out the high-pass digital computing, promptly obtain road surface fluctuating information and pavement texture information.

(7) obtain the surface evenness result according to international planeness IRI canonical statistics.

Principle of the present invention is: the laser pavement profiler is in measuring process, comprise the measure error that bigger body movement brings in the measurement result, the true fluctuating and the texture that can't reflect the road surface need to use Inertial Measurement Unit that the measurement result of laser pavement profiler is compensated.And when using Inertial Measurement Unit that laser pavement profiler measurement result is compensated, if using Inertial Measurement Unit can introduce measurement result separately disperses, if with Inertial Measurement Unit and GPS is used in combination and can introduce the jump of measurement result.The long-term accuracy of considering Inertial Measurement Unit and GPS information fusion is higher, therefore the position that the result of Inertial Measurement Unit and GPS information fusion is resolved as pure inertia strapdown, attitude, the speed initial value, use pure inertia strapdown to resolve the measurement body movement in the measuring process, overcome the information fusion result jump has been arranged, the shortcoming that pure inertia measurement result disperses, simultaneously, using high-pass digital filter that the elevation of Inertial Measurement Unit is resolved the result carries out filtering and handles, the low frequency aberration that body movement is caused carries out filtering, finally obtain high accuracy, level and smooth inertia measurement result, this result is compensated laser pavement profiler measurement result, finally obtain fluctuating of real road surface and texture information, thereby calculate the planeness information on road surface.

Basic principle is as follows:

Desirable elevation design formulas is as follows:

$h_r=\∫(f_b-C_n^b\·g_n)\·\mathrm{dt}$

Wherein, h _rBe the elevation information that measures, f _bFor elevation adds the responsive specific force of meter,

Be attitude matrix, g _nBe desirable local gravitational acceleration;

Singly add in the meter scheme in tradition, elevation measurement design formulas is as follows:

h _s＝∫(f _b-g _n+Δf)·dt

Wherein, h _sBe the actual measurement elevation, Δ f is zero an inclined to one side error of accelerometer;

Therefore, error in measurement is:

$h_{\mathrm{error}}=h_s-h_r=\∫(C_n^b-I)\·g_n;$

By following formula as can be known, vehicle attitude

Change big more, the elevation error in measurement h that is produced _ErrorBig more, simultaneously, because zero existence of error partially of accelerometer, the elevation measuring value can be dispersed gradually along with the time.

In this patent scheme, adopted complete inertial navigation to resolve technology, this technology can measure complete movable information, has overcome aforementioned " puppet " elevation variation that the attitude variation brings in the meter scheme that singly adds; Adopted the pure-inertial guidance of measurement period to resolve technology, this technology can overcome the filtering error that Kalman filtering causes and proofread and correct the error sudden change that brings, and makes the elevation measurement information of this patent scheme overcome the elevation step that conventional Kalman filtering process is brought; Adopted the high-pass filtering technology, the error that this technology can the pure inertia of filtering be resolved in the elevation information is dispersed, and keeps actual measurements information wherein.

The present invention's advantage compared with prior art is: the present invention is used for the kinematic error compensation that road vehicle is measured with inertial navigation technology, make that the measurement information precision is higher, adopted the pure-inertial guidance technology of local time's section, overcome the error sudden change that the error correction of common inertial navigation Kalman filtering brings, adopted the high-pass filtering technology, filtering the vertical error of the pure inertia section of resolving disperse.Therefore, this patent is compared with traditional scheme, has the advantage of precision height, data smoothing.

Description of drawings

Fig. 1 is the structured flowchart based on the vehicle-mounted pavement detection system of Inertial Measurement Unit and laser displacement sensor;

Fig. 2 is the flow chart that resolves of information fusion algorithm of the present invention.

The specific embodiment

As shown in Figure 1, the present invention mainly is made up of Inertial Measurement Unit 1, GPS module 2, mileage gauge 3, laser displacement sensor 4, data acquisition board 5 and PC104 computer 6, Inertial Measurement Unit 1 is made up of three gyroscopes 11, three accelerometers 12 etc., is used for the angular velocity and the acceleration of measuring vehicle; GPS module 2 is made up of GPS receiver 21 and antenna 22, is used for the position and the speed of measuring vehicle; Laser displacement sensor 4 measuring vehicle are to the distance on road surface; The operating range of mileage gauge 3 measuring vehicle, and generation pulse signal trigger data acquisition plate 5 carries out data acquisition; Data acquisition board 5 is gathered the output signal of three gyroscopes 11, three accelerometers 12 and laser displacement sensor 41, sends to PC104 computer 6 by usb bus after the digitlization; PC104 computer 6 receives the raw information that data acquisition board 5 sends over, the error compensation, the inertial navigation that carry out three gyroscopes 11, three accelerometers 12 then successively resolve and inertia/GPS information fusion filtering, with the information fusion of inertia/GPS information fusion result and laser displacement sensor, calculate surface evenness information at last.

As shown in Figure 2, concrete grammar of the present invention is as follows:

(1) the Strapdown Inertial Units measuring unit carries out initial alignment, determines the initial orientation angle of Strapdown Inertial Units measuring unit

Initial pitching angle theta ₀With initial roll angle γ ₀, design formulas is as follows:

Gather gyro and accelerometer data 5-10 minute, ask the average g of sampled value _x, g _y, g _z, a _x, a _y, a _z(wherein, g _x, g _y, g _zBe three gyrostatic responsive values, a _x, a _y, a _zBe the sensitivity value of three accelerometers) utilize g _x, g _y, g _z, a _x, a _y, a _zAsk the initial attitude angle

θ ₀, γ ₀, design formulas is as follows:

$C_b^n=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

$a^{\′}=\sqrt{{{\mathrm{ax}}_x}^2+{a_y}^2+{a_z}^2},$ $w^{\′}=\sqrt{{g_x}^2+{g_y}^2+{g_z}^2},$ $H_{g\·a}=\frac{g_x\·a_x+g_y\·a_y+g_z\·a_z}{a^{\′}},$ $E=\sqrt{w^{\′2}-{H_{g\·a}}^2}$

$T_{31}=\frac{a_x}{a^{\&prime;}},$ $T_{32}=\frac{a_y}{a^{\&prime;}},$ $T_{33}=\frac{a_z}{a^{\&prime;}}---<1>$

$T_{21}=\frac{(g_x-H_{g\&CenterDot;a}\&CenterDot;T_{31})}{E},$ $T_{22}=\frac{(g_y-H_{g\&CenterDot;a}\&CenterDot;T_{32})}{E},$ $T_{23}=\frac{(g_z-H_{g\&CenterDot;a}\&CenterDot;T_{33})}{E}---<2>$

T ₁₁＝T ₂₂·T ₃₃-T ₂₃·T ₃₂，T ₁₂＝T ₂₃·T ₃₁-T ₂₁·T ₃₃，T ₁₃＝T ₁₁·T ₃₂-T ₂₂·T ₃₁ <3>

Wherein,

Be system's attitude matrix, T _IjFor In the element of the capable j of i row, a ', w ', H _Ga, E is intermediate variable;

It is as follows to get attitude angle:

θ ₀＝sin ^-1(T ₂₃) <5>

${\mathrm{\&gamma;}}_0={\tan }^{-1}(-\frac{T_{31}}{T_{33}})---<6>$

(2) gather gyroscope and accelerometer in the measuring process in real time, carry out strapdown and resolve renewal, design formulas is as follows:

Utilize the sampling of gyroscope and accelerometer to calculate angle increment Δ θ _x, Δ θ _y, Δ θ _z, utilize angle increment to upgrade hypercomplex number and attitude matrix:

$q(n+1)=\{(1-\frac{{\left({\mathrm{\&Delta;\&theta;}}_0\right)}^2}{8}+\frac{{\left({\mathrm{\&Delta;\&theta;}}_0\right)}^4}{384})I+(\frac{1}{2}-\frac{{\left({\mathrm{\&Delta;\&theta;}}_0\right)}^2}{48})\left(\mathrm{\&Delta;\&theta;}\right)\}q\left(n\right)---<7>$

Wherein, q (n) is hypercomplex number value of resolving of putting n computing time, and angle increment Δ θ formula is as follows:

$\mathrm{\&Delta;\&theta;}=\left[\begin{array}{cccc}0& -\mathrm{\&Delta;}{\mathrm{\&theta;}}_x& -\mathrm{\&Delta;}{\mathrm{\&theta;}}_y& -\mathrm{\&Delta;}{\mathrm{\&theta;}}_z\\ {\mathrm{\&Delta;\&theta;}}_x& 0& {\mathrm{\&Delta;\&theta;}}_z& {-\mathrm{\&Delta;\&theta;}}_y\\ {\mathrm{\&Delta;\&theta;}}_y& -\mathrm{\&Delta;}{\mathrm{\&theta;}}_z& 0& {\mathrm{\&Delta;\&theta;}}_x\\ {\mathrm{\&Delta;\&theta;}}_z& {\mathrm{\&Delta;\&theta;}}_y& -{\mathrm{\&Delta;\&theta;}}_x& 0\end{array}\right]---<8>$

${\mathrm{\&Delta;\&theta;}}_0=\sqrt{{\mathrm{\&Delta;\&theta;}}_x^2+{\mathrm{\&Delta;\&theta;}}_y^2+{\mathrm{\&Delta;\&theta;}}_z^2}---<9>$

Attitude matrix

More new formula as follows:

$C_b^n=\left[\begin{array}{ccc}{q}_0^2+{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000058815640000011.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000058815640000011.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000058815640000011.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-q_2^2+q_3^2\end{array}\right]---<10>$

Utilize acceleration renewal speed and position

$C_e^n=\left[\begin{array}{ccc}-\sin \left(\mathrm{\&lambda;}\right)& \cos \left(\mathrm{\&lambda;}\right)& 0\\ -\sin \left(L\right)\&CenterDot;\cos \left(\mathrm{\&lambda;}\right)& -\sin \left(L\right)\&CenterDot;\sin \left(\mathrm{\&lambda;}\right)& \cos \left(L\right)\\ \cos \left(L\right)\&CenterDot;\cos \left(\mathrm{\&lambda;}\right)& \cos \left(L\right)\&CenterDot;\sin \left(\mathrm{\&lambda;}\right)& \sin \left(L\right)\end{array}\right]---<11>$

${\mathrm{\&omega;}}_{\mathrm{ie}}^n=C_e^n\&CenterDot;\left[\begin{array}{c}0\\ 0\\ 15.04\end{array}\right],$ ${\mathrm{\&omega;}}_{\mathrm{en}}^n=\left[\begin{array}{ccc}0& -1/R& 0\\ 1/R& 0& 0\\ \frac{C_e^n\left(\mathrm{3,3}\right)}{R\&CenterDot;C_e^n\left(\mathrm{2,3}\right)}& 0& 0\end{array}\right]\&CenterDot;V_n---<12>$

$f_n=C_b^n\&CenterDot;f_b---<13>$

${\stackrel{\&CenterDot;}{V}}_n=f_n+({2\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n)\&times;V_n-g_n---<14>$

$V_n(n+1)=V_n\left(n\right)+{\stackrel{\&CenterDot;}{V}}_n\&CenterDot;\mathrm{\&Delta;t}---<15>$

$\left[\begin{array}{c}L(n+1)\\ \mathrm{\&lambda;}(n+1)\\ h(n+1)\end{array}\right]=\left[\begin{array}{c}L\left(n\right)\\ \mathrm{\&lambda;}\left(n\right)\\ h\left(n\right)\end{array}\right]+\frac{V_n\left(n\right)\&CenterDot;\mathrm{\&Delta;t}}{R}---<16>$

In the above formula, λ is a longitude, and L is a latitude, V _nBe the speed under the navigation system,

Be the transition matrix of navigation system with respect to earth system,

Be the projection of rotational-angular velocity of the earth under navigation system,

The vehicle that brings for movement velocity is with respect to the motion angular velocity of the earth, f _bFor Inertial Measurement Unit measures the specific force that obtains, f _nBe the projection of specific force under navigation system, Be attitude matrix, R is an earth radius.

The strapdown attitude algorithm finishes.

(3) strapdown is resolved result and gps data and carry out information fusion by the information fusion wave filter, and utilize the inertia device zero inclined to one side estimated result that merges that the data backoff algorithm is carried out feedback compensation, design formulas is as follows:

State equation:

$\stackrel{\&CenterDot;}{X}=\mathrm{FX}+\mathrm{GW}---<17>$

Wherein, X is a system state vector, and W is the system noise vector, and F is system's transfer matrix, and G is the noise transition matrix:

X＝[φ _x?φ _y?φ _z?δV _n(x)?δV _n(y)?δV _n(z)?δL?δλ?δh?ε _x?ε _y?ε _z?σ _x?σ _y?σ _z] ^T

<18>

$W={\left[\begin{array}{cccccc}{W}_{{\mathrm{\&epsiv;}}_x}& W_{{\mathrm{\&epsiv;}}_y}& W_{{\mathrm{\&epsiv;}}_z}& W_{{\mathrm{\&sigma;}}_x}& W_{{\mathrm{\&sigma;}}_x}& W_{{\mathrm{\&sigma;}}_x}\end{array}\right]}^T---<19>$

$G=\left[\begin{array}{ccc}{C}_b^n& 0_{3\&times;3}& 0_{3\&times;9}\\ 0_{3\&times;3}& C_b^n& 0_{3\&times;9}\\ 0_{9\&times;3}& 0_{9\&times;3}& 0_{9\&times;9}\end{array}\right]---<20>$

The measurement equation of system:

Z＝HX+η <21>

Wherein: Z is a measurement vector, and H is an observing matrix, and η is a measurement noise:

Z＝[δV _n(x)?δV _n(y)δV _n(z)δL?δλ?δh] ^T <22>

$H=\left[\begin{array}{cccc}{0}_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;6}\\ 0_{3\&times;3}& 0_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;6}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;6}\end{array}\right]---<23>$

$\mathrm{\&eta;}={\left[\begin{array}{cccccc}{\mathrm{\&eta;}}_{V_n\left(x\right)}& {\mathrm{\&eta;}}_{V_n\left(y\right)}& {\mathrm{\&eta;}}_{V_n\left(z\right)}& {\mathrm{\&eta;}}_L& {\mathrm{\&eta;}}_{\mathrm{\&lambda;}}& {\mathrm{\&eta;}}_h\end{array}\right]}^T---<24>$

The layout of information fusion filtering rudimentary algorithm, the flow chart of this algorithm be as shown in Figure 2:

State one-step prediction equation

${\stackrel{\mathrm{\&Lambda;}}{X}}_{k/k-1}={\mathrm{\&phi;}}_{k,k-1}{\stackrel{\mathrm{\&Lambda;}}{X}}_{k-1}---<25>$

The State Estimation accounting equation

${\stackrel{\mathrm{\&Lambda;}}{X}}_k={\stackrel{\mathrm{\&Lambda;}}{X}}_{k/k-1}+K_K(Z_k-H_k{\stackrel{\mathrm{\&Lambda;}}{X}}_{k/k-1})---<26>$

Filtering increment equation

${\stackrel{\mathrm{\&Lambda;}}{K}}_k={\stackrel{\mathrm{\&Lambda;}}{P}}_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---<27>$

One-step prediction mean square error equation

${\stackrel{\mathrm{\&Lambda;}}{P}}_{k/k-1}={\mathrm{\&phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k-1}^T---<28>$

Estimate the mean square error equation

${\stackrel{\mathrm{\&Lambda;}}{P}}_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T.---\left(29\right)$

(4) in test segment, adopt double strapdown to resolve algorithm, just start new strapdown and resolve algorithm, only carry out more new Algorithm of strapdown, do not carry out information fusion filtering with GPS;

(5) second strapdown in the test segment resolved the road surface fluctuating information stack that the elevation information that obtains and laser sensor record, stack result is carried out the high-pass digital computing, promptly obtain road surface fluctuating information and pavement texture information, design formulas is as follows:

h＝h _laser+h _imu <30>

$h_n^{\&prime;}=\underset{i=1}{\overset{k-1}{\mathrm{\&Sigma;}}}{h}_{n-i}^{\&prime;}\&CenterDot;s_i+\underset{i=0}{\overset{k-1}{\mathrm{\&Sigma;}}}{h}_{n-i}\&CenterDot;r_i---<31>$

Wherein, h ' ₁, h ' ₂, h ' ₃... h ' _nBe the information that truly rises and falls of resulting road surface after the filtering, h _LaserBe the road surface fluctuating information that laser sensor records, h _ImuBe the elevation information that obtains in the Inertial Measurement Unit strapdown algorithm two, h ₁, h ₂, h ₃... h _nBe h _LaserWith h _ImuThe algebraical sum time series, k is a filter order, s ₁, s ₂, s ₃... s _kBe digital filter feedback factor, r ₁, r ₂, r ₃... r _kBe the digital filter filter factor;

(6) obtain the surface evenness result according to international planeness IRI canonical statistics, design formulas is as follows:

Calculate variable

${\stackrel{\&CenterDot;}{Z}}_M=S_{11}{\stackrel{\&CenterDot;}{z}}_M+S_{12}{\stackrel{\&CenterDot;\&CenterDot;}{z}}_M+S_{13}{\stackrel{\&CenterDot;}{z}}_m+S_{14}{\stackrel{\&CenterDot;\&CenterDot;}{z}}_m+P_1\stackrel{\&CenterDot;}{h}$

${\stackrel{\&CenterDot;\&CenterDot;}{Z}}_M=S_{21}{\stackrel{\&CenterDot;}{z}}_M+S_{22}{\stackrel{\&CenterDot;\&CenterDot;}{z}}_M+S_{23}{\stackrel{\&CenterDot;}{z}}_m+S_{24}{\stackrel{\&CenterDot;\&CenterDot;}{z}}_m+P_2\stackrel{\&CenterDot;}{h}$

${\stackrel{\&CenterDot;}{Z}}_m=S_{31}{\stackrel{\&CenterDot;}{z}}_M+S_{32}{\stackrel{\&CenterDot;\&CenterDot;}{z}}_M+S_{33}{\stackrel{\&CenterDot;}{z}}_m+S_{34}{\stackrel{\&CenterDot;\&CenterDot;}{z}}_m+P_3\stackrel{\&CenterDot;}{h}$

${\stackrel{\&CenterDot;\&CenterDot;}{Z}}_m=S_{41}{\stackrel{\&CenterDot;}{z}}_M+S_{42}{\stackrel{\&CenterDot;\&CenterDot;}{z}}_M+S_{43}{\stackrel{\&CenterDot;}{z}}_m+S_{44}{\stackrel{\&CenterDot;\&CenterDot;}{z}}_m+P_4\stackrel{\&CenterDot;}{h}---<32>$

Wherein, S _Mn(m=1,2,3,4; N=1,2,3,4) and P _l(l=1,2,3,4) are the international norm coefficient matrixes according to the 250mm specification;

h _iBe the i road surface fluctuating value constantly that step (7) calculates, dx is the interval between the measuring point;

Calculate each measuring point and adjust the gradient:

${\mathrm{RS}}_k=|{\stackrel{\&CenterDot;}{Z}}_m-{\stackrel{\&CenterDot;}{Z}}_M|---<33>$

Calculate the international roughness index IRI on road surface:

$\mathrm{IRI}=\frac{1}{n-1}\underset{k=2}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{RS}}_k---<34>$

Claims (4)

1. the fortune based on the vehicle-mounted pavement detection system of Inertial Measurement Unit is mended algorithm, it is characterized in that comprising the following steps:

(1) the Strapdown Inertial Units measuring unit carries out initial alignment, determines the initial orientation angle of Strapdown Inertial Units measuring unit

Initial pitching angle theta ₀With initial roll angle γ ₀

(2) the Strapdown Inertial Units measuring unit is gathered three-axis gyroscope data and three axis accelerometer data in real time, carries out compensation data, obtains three axis angular rates and 3-axis acceleration;

(3) Strapdown Inertial Units measuring unit startup strapdown resolves algorithm, and three axis angular rates that utilization obtains and 3-axis acceleration carry out strapdown and resolve attitude, position and the speed of real-time update quick-connecting inertia measurement unit;

(4) strapdown resolves the result and GPS information is carried out the information fusion filtering operation, calculate attitude error, site error and velocity error, and attitude information, positional information and velocity information that strapdown resolves in the algorithm revised, zero inclined to one side value with inertia device feeds back in the compensation data algorithm simultaneously, helps to improve the precision of compensation data;

(5) when vehicle enters highway section to be measured, adopt double strapdown to resolve algorithm, just utilize this constantly attitude, position and the velocity calculated result in information fusion algorithm start new strapdown and resolve process and carry out pure inertia and resolve as initial value;

(6) strapdown in the test segment is resolved the road surface fluctuating information stack that the pure inertia elevation information that obtains and laser sensor record, stack result is carried out the high-pass digital computing, promptly obtain road surface fluctuating information and pavement texture information;

(7) obtain the surface evenness result according to international planeness IRI canonical statistics.

2. the fortune based on the vehicle-mounted pavement detection system of Inertial Measurement Unit is mended algorithm, and it is characterized in that: the concrete steps of the described initial alignment of step (1) are:

(1) gathers gyro and accelerometer data 5-10 minute, ask the average g of sampled value _x, g _y, g _z, a _x, a _y, a _zWherein, g _x, g _y, g _zBe the responsive value of the angular velocity of the x axle of Inertial Measurement Unit, y axle, z axle, a _x, a _y, a _zAcceleration sensitive value for the x axle of Inertial Measurement Unit, y axle, z axle;

(2) utilize g _x, g _y, g _z, a _x, a _y, a _zAsk the initial attitude angle

θ ₀, γ ₀, design formulas is as follows:

$C_b^n=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

$a^{\&prime;}=\sqrt{{{\mathrm{ax}}_x}^2+{a_y}^2+{a_z}^2},$ $w^{\&prime;}=\sqrt{{g_x}^2+{g_y}^2+{g_z}^2},$ $H_{g\&CenterDot;a}=\frac{g_x\&CenterDot;a_x+g_y\&CenterDot;a_y+g_z\&CenterDot;a_z}{a^{\&prime;}},$ $E=\sqrt{w^{\&prime;2}-{H_{g\&CenterDot;a}}^2}$

$T_{31}=\frac{a_x}{a^{\&prime;}},$ $T_{32}=\frac{a_y}{a^{\&prime;}},$ $T_{33}=\frac{a_z}{a^{\&prime;}}$

$T_{21}=\frac{(g_x-H_{g\&CenterDot;a}\&CenterDot;T_{31})}{E},$ $T_{22}=\frac{(g_y-H_{g\&CenterDot;a}\&CenterDot;T_{32})}{E},$ $T_{23}=\frac{(g_z-H_{g\&CenterDot;a}\&CenterDot;T_{33})}{E}$

T ₁₁＝T ₂₂·T ₃₃-T ₂₃·T ₃₂，T ₁₂＝T ₂₃·T ₃₁-T ₂₁·T ₃₃，T ₁₃＝T ₁₁·T ₃₂-T ₂₂·T ₃₁

Wherein,

Be system's attitude matrix, T _IjFor

In the element of the capable j of i row, a ', w ', H _Ga, E is intermediate variable;

It is as follows to get attitude angle:

θ ₀＝sin ^-1(T ₂₃)

${\mathrm{\&gamma;}}_0={\tan }^{-1}(-\frac{T_{31}}{T_{33}}).$

3. the fortune based on the vehicle-mounted pavement detection system of Inertial Measurement Unit is mended algorithm, and it is characterized in that: the described information fusion wave filter of step (4) is:

State equation:

$\stackrel{\&CenterDot;}{X}=\mathrm{FX}+\mathrm{GW}$

Wherein, X is a system state vector, and W is the system noise vector, and F is system's transfer matrix, and G is the noise transition matrix:

X＝[φ _x?φ _y?φ _z?δV _n(x)?δV _n(y)?δV _n(z)?δL?δλ?δh?ε _x?ε _y?ε _z?σ _x?σ _y?σ _z] ^T

$W={\left[\begin{array}{cccccc}{W}_{{\mathrm{\&epsiv;}}_x}& W_{{\mathrm{\&epsiv;}}_y}& W_{{\mathrm{\&epsiv;}}_z}& W_{{\mathrm{\&sigma;}}_x}& W_{{\mathrm{\&sigma;}}_x}& W_{{\mathrm{\&sigma;}}_x}\end{array}\right]}^T$

$G=\left[\begin{array}{ccc}{C}_b^n& 0_{3\&times;3}& 0_{3\&times;9}\\ 0_{3\&times;3}& C_b^n& 0_{3\&times;9}\\ 0_{9\&times;3}& 0_{9\&times;3}& 0_{9\&times;9}\end{array}\right]$

The measurement equation of system:

Z＝HX+η

Wherein: Z is a measurement vector, and H is an observing matrix, and η is a measurement noise:

Z＝[δV _n(x)δV _n(y)δV _n(z)δL?δλδh] ^T

$H=\left[\begin{array}{cccc}{0}_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;6}\\ 0_{3\&times;3}& 0_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;6}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;6}\end{array}\right]$

$\mathrm{\&eta;}={\left[\begin{array}{cccccc}{\mathrm{\&eta;}}_{V_n\left(x\right)}& {\mathrm{\&eta;}}_{V_n\left(y\right)}& {\mathrm{\&eta;}}_{V_n\left(z\right)}& {\mathrm{\&eta;}}_L& {\mathrm{\&eta;}}_{\mathrm{\&lambda;}}& {\mathrm{\&eta;}}_h\end{array}\right]}^T$

The layout of information fusion filtering rudimentary algorithm is carried out according to Kalman filtering algorithm.

4. the fortune based on the vehicle-mounted pavement detection system of Inertial Measurement Unit is mended algorithm, and it is characterized in that: the described flow process of step (6) is:

At first with the ranging information h of laser pavement rangefinder _LaserElevation information h with the inertia metric data _ImuSuperpose, the elevation information h after the stack is:

h＝h _laser+h _imu

Because the elevation information h of inertia metric data _ImuIn comprise the drift information of real movable information and inertia, therefore, eliminated the ranging information h of laser pavement rangefinder after the stack _LaserIn movable information, but also introduced simultaneously inertia drift information.In order to eliminate the inertia drift information of low frequency, need be with the elevation information h after the stack by a high-pass digital filter, formula is as follows:

$h_n^{\&prime;}=\underset{i=1}{\overset{k-1}{\mathrm{\&Sigma;}}}{h}_{n-i}^{\&prime;}\&CenterDot;s_i+\underset{i=0}{\overset{k-1}{\mathrm{\&Sigma;}}}{h}_{n-i}\&CenterDot;r_i$

Wherein, h ' ₁, h ' ₂, h ' ₃... h ' _nBe the information that truly rises and falls of resulting road surface after the filtering, h _LaserBe the road surface fluctuating information that laser sensor records, h _ImuBe the elevation information that obtains in the Inertial Measurement Unit strapdown algorithm two, h ₁, h ₂, h ₃... h _nBe h _LaserWith h _ImuThe algebraical sum time series, k is a filter order, s ₁, s ₂, s ₃... s _kBe digital filter feedback factor, r ₁, r ₂, r ₃... r _kBe the digital filter filter factor.

Images