CN102385060A - Arrangement and method used for two-dimensional and three-dimensional exact position and directional determination - Google Patents

Abstract

The invention relates to an arrangement and a method used for two-dimensional and three-dimensional exact position and directional determination, which are a method and equipment for determining inaccurate global positioning system (GPS) samples in one group of GPS samples according to the following steps: (a) obtaining GPS samples obtained when a global positioning system carried by a vehicle processes along a track; (b) on the basis of the GPS samples, obtaining a first estimation of the track; (c) obtaining a second estimation of the track at least on the basis of measurement made when an inertia measurement unit carried by the vehicle processes along the track; (d) comparing the first estimation with the second estimation; (e) building a position on which the comparison value of the first estimation and the second estimation is showed to be higher the variation of a preset threshold value; (f) if the position can not be built, continuing by (j), and otherwise continuing by (g); (g) removing the GPS sample relevant to the position with high variation as an inaccurate GPS sample so as to form one group of residual GPS samples; (h) based on the residual GPS samples, calculating the first estimation of the track again, and calculating the second estimation again; (i) repeating (d) to (h); and (j) ending the steps.

Description

Be used for two and three dimensions exact position and directed layout and the method for confirming

Divide an application

Patented claim of the present invention is to be on November 6th, 2006 applying date, and application number is 200680056664.3, and denomination of invention dividing an application for the application for a patent for invention case of " being used for two and three dimensions exact position and directed layout and the method for confirming ".

Technical field

The present invention relates to two and three dimensions exact position and directed layout and the method for confirming.

Background technology

Position determining means is well known in the prior art.Position determining means is applied to (for example) vehicles, for example vehicle, steamer and aircraft more and more now.For this purpose, these vehicles can comprise different measuring units, like GPS (GPS), Inertial Measurement Unit (IMU) and distance measuring instrument (DMI).

When advancing, the output data of these a little measuring units is used for calculating the position and/or the orientation of the vehicles by processor.Depend on application, the measurement that online or off-line uses these measuring units to make.

Generally need position and orientation survey as far as possible accurately be provided from 3 measuring unit: GPS, IMU and DMI.Should solve many problems for this purpose, for example the drift (or other displacement) in the output signal of noise in multipath problem, the measuring-signal and IMU.Under the present art, Kalman (Kalman) wave filter is widely used in compensation from the drift in the output signal of IMU and compensate other effect.And other statistical method like moving average technology and the filtering of Gauss (Gaussian) white noise, can be used for removing many noises so that signal becomes pure from signal.

Yet, some filtering techniques and the averaging of removing Gaussian noise effectively to through shifted signal with poor efficiency work, said mux--out signal exhibits is to measuring the long period constant displacement of series (as in GPS measures).

The 5th, 311, No. 195 and the 5th, 948, No. 043 United States Patent (USP) disclose has other sensor and the gps system that is used to discern non-Kalman's member that the inaccurate GPS of considered not measures.

Summary of the invention

Target of the present invention provides a kind of position and confirms that with directed system and position confirm method with directed, at least one in the position that is based on the measurement of carrying out on the vehicles that move along track with improvement and directed the calculating.

For this reason, the present invention provides a kind of computing machine layout as being advocated in the autonomous device technical scheme.

And, the present invention relates to a kind of method as being advocated in the independent solution technical scheme.

The present invention provides very accurate and reliable method to come from one group of GPS sample when its track moves, collecting at the vehicles, to remove inaccurate GPS sample.Through using equipment or the method for being advocated, inaccurate GPS sample is eliminated with nonlinear way.

In one embodiment, but carry out, that is, after all measurement data of collecting from track, carry out about position and directed calculating off-line.This (for example) is real in so-called mobile mapping system (MMS system); Wherein come assembling position and directional data through the vehicles of advancing along road net; Said position and directional data are used to produce 2D and/or 3D road-map after a while; Or catching the geodata that can in the 3D of a zone (like the road in the city) demonstration, use, said 3D shows and also shows along the facade of the buildings of said road.Can use other field of the present invention is that wherein the MMS system can provide important support in create in road asset detail data storehouse.Can use other sensor equally; As be used for the sensor of the geographical radar of conduct of road surface management, based on the roughness coefficient measuring unit of laser, be widely used in the stroboscope that crack detection uses and strengthen at a high speed vertical camera, and be used for that object is confirmed, the laser scanner of 3D view application etc.

Position that records in this embodiment and directional data need not to use in real time but only to the processed offline device ability that is different under real-time condition the correction mechanism that is possible correction mechanism of carrying out are provided in the fact of using afterwards.That off-line method of the present invention provides inspection as all GPS samples of during advancing along road net, collecting even select more accurately, thus inaccurate GPS sample discerned, and consider that no longer these inaccurate GPS samples calculate the track of being advanced.As the track that calculates in this way has higher accuracy, because it no longer receives to influence through the skew gps signal.

Therefore; The present invention provides result accurately; Its very cost implement effectively and can in offline environment, use; In said offline environment, the location/orientation measurement is from the data computation of for example having collected along the vehicles of road driving with smooth fashion (therefore, for example not moved suddenly by what race vehicles was made).

In offline environment, the present invention can be used for producing the more responsive of track and confirms, and the slightly more accurate net result of therefore final generation.This is because in off-line embodiment, but the application of shape wave filter, and said shaped filters be the overall situation, recurrence and adaptive, and can be with the GPS sample classification for accurately or inaccurate.

In one embodiment, the present invention provides the selection that the drift (or other displacement) of the output signal of the position that becomes along with the time that is used for calculating the vehicles and directed Inertial Measurement Unit (IMU) is calculated.This accomplishes through using the GPS sample, and with the drift of confirming in this way of post-equalization IMU signal.To use through drift-corrected IMU signal subsequently and act on calculating location and directed main basis, and the GPS sample is mainly used in drift correction.The number that is used for this drift-compensated GPS sample can be up to 25%.

Description of drawings

To graphicly come explained in detail the present invention referring to some, saidly graphicly setly be used to explain the present invention rather than limit its scope that scope of the present invention is defined by additional claims and equivalent embodiment thereof.

In graphic:

Fig. 1 shows prior art position and the orientation survey system in the vehicles;

Fig. 2 a shows local traffic tool coordinates system;

Fig. 2 b shows so-called wgs coordinate system;

The illustrative example that Fig. 3 demonstrating computer is arranged;

Fig. 4 is the synoptic diagram that is used to explain the multipath problem;

Fig. 5 shows the figure of the output signal of gps system and Inertial Measurement Unit (IMU);

Fig. 6 displaying is used to explain process flow diagram of the present invention;

Fig. 7 a shows the successive relay trip that is used for from the method for one group of inaccurate GPS sample of GPS sample removal to Fig. 7 d;

Fig. 8 a, Fig. 8 b and Fig. 8 c show the curve that is used for illustrating the drift (with other displacement) that can how to estimate the IMU signal;

Fig. 9 a and Fig. 9 b show the track that can how come approximate representation road traffic instrument to be advanced through clothoid;

Figure 10 a, Figure 10 b and Figure 10 c show the curve that is used for illustrating the drift (with other displacement) that can how to estimate IMU pitching signal;

Figure 11 shows how to define slope with respect to the local gravity vector.

Embodiment

Fig. 1 shows the vehicles that possess existing pin-point accuracy position determining means.Fig. 1 shows the vehicles 1, and it possesses a plurality of wheels 2.And the vehicles 1 possess the pin-point accuracy position determining means.As shown in Figure 1, position determining means comprises following assembly:

● GPS (GPS) unit, it is connected to antenna 8, and through arrange with a plurality of satellite SLi (i=1,2,3 ...) communicate by letter and also come the calculating location signal according to the signal that receives from satellite SLi.Gps system also provides the course data (that is the direction of, advancing) and the speed of the vehicles 1.Gps system can be differential GPS (DGPS) system, and it provides the accuracy (except that possible white noise) of 1 ∑/1 meter.Gps system is connected to microprocessor μ P, and said microprocessor is through arranging to handle GPS output signal.

● distance measuring instrument (DMI).This instrument is a mileometer of measuring the distance that the vehicles 1 are advanced through the one or more rotation number in the sensing wheel 2.Therefore, DMI will measure the rotation number of one in the rear wheel, because rear wheel does not rotate with respect to the main body of the vehicles.DMI can 10Hz or the above sampling frequency operation of 10Hz.DMI also is connected to microprocessor μ P, considers the distance measured like DMI when the output signal of handling from the GPS unit to allow microprocessor μ P.

● Inertial Measurement Unit (IMU).This IMU can be embodied as: 3 gyro units, and it is through arranging to measure rotary acceleration; And three accelerometers, it is through arranging to measure translational acceleration along 3 orthogonal directionss.IMU/ gyroscope and accelerometer can 200Hz the sampling frequency operation.IMU also is connected to microprocessor μ P, considers the measurement of IMU when the output signal of handling from the GPS unit to allow microprocessor μ P.Observe in some embodiments of the invention, 1 or 2 IMU or 1 gyroscope can be enough with the DMI that is used for supporting the 2D location.

● DMI, IMU and gps system are connected to microprocessor μ P.Microprocessor μ P be used for confirming these three measuring systems any one make the time of measurement: microprocessor clock can be used for said purpose.Can improve the linearity of said time through the PPS signal (the accurate positioning service of PPS=) that receives from gps signal.Based on the signal that receives from DMI, IMU and gps system; Microprocessor μ P can confirm the suitable shows signal that will on the monitor 4 of the vehicles 1, show, thereby informs the direction at the position at driver's vehicles place and the place of maybe the vehicles just advancing.

During system as shown in Figure 1 can be applicable to so-called " mobile mapping system ", " mobile mapping system " (for example) was through collecting geodata with one or more camera 9 pictures taken that are installed on the vehicles 11.And, can one or more laser scanners (not shown among Fig. 1) be installed to the vehicles 1.Camera (and optional laser scanner) is connected to microprocessor μ P.Camera picture and other sensing data of possibility can be collected together with all DMI, IMU and gps data, for the processed offline in future.The Another Application of the system of Fig. 1 can be in the field of vehicle navigation system.Now, in vehicle navigation system, shows signal is usually directed to the position on two dimension (2D) map.

Fig. 2 a shows can obtain from three measuring unit GPS, DMI and IMU shown in Figure 1 for which position signalling.Fig. 2 a shows that microprocessor μ P is through arranging to calculate 6 different parameters: with respect to 3 distance parameter x, y, z and 3 parameter ω of initial point _x, ω _yAnd ω _zThe initial point that note that coordinate system is not fixed, but defines with respect to the vehicles (for example, with respect to the IMU system).3 parameter ω _x, ω _yAnd ω _zExpression changes speed (or rotational speed) around the angle of x axle, y axle and z axle respectively respectively.The z direction overlaps with the direction of local gravity vector.In many situations, track will have slope with respect to gravity vector.Therefore, the x direction is not orthogonal to the local gravity vector.This coordinate system uses with measuring the computing system that also can combine the off-line use.Note that the orientation that the vehicles center on the y axle is commonly called pitching, and be commonly called rolling, and the course is the orientation around the z axle around the orientation of x axle.Therefore, following equality is set up:

Pitching=∫ ω _xDt

Rolling=∫ ω _yDt

Course=∫ ω _zDt

Show employed coordinate system among Fig. 2 b.Hereinafter will explain processor 11 and can how in geographic coordinate, to confirm latitude, longitude and height above sea level and the free angle of the vehicles 1.Geographic coordinate system (GEO) is defined as and makes its X axle in the equatorial plane of the earth, but fixes along with the rotation of the earth, makes it pass Greenwich meridian (0 ° of warp).The Z axle of said coordinate system is parallel to the turning axle of the earth, and the Y axle of said coordinate system completion dextrorotation orthogonal set (Y=Z * X).In order to do like this, how explaination is confirmed the position and the orientation of the vehicles, surveying instrument GPS, IMU and the DMI of the wherein said vehicles as the local moving coordinate system that defines by the local gravity vector in.

For this reason, the vehicles 1 are arranged in the some place with a certain lat/lon, wherein its x axle is parallel to the course of the vehicles 1, and the z axle is parallel to the local gravity vector in the said on earth point.

In general, the application's case can be applicable to the vehicles as shown in Figure 1.Yet the application's case can be applied to any other equally and possess the vehicles of wheel to move along earth boundary locus, for example motorbus, truck, train or tramcar etc.And; ((it maybe be because multipath or other be former thereby squint from actual position to remove inaccurate GPS sample to the database of the GPS sample that influenced by non-Gaussian noise for the embodiment of the method for explaining like hereinafter; Promptly; Non-through modeled tropospheric propagation delay)) be equally applicable to system's (for example ship) and air line (helicopter and aircraft) based on water, as long as said system disposition has GPS and inertial positioning system.

One of applicable field of the present invention relates to the 3D rendering that produces the buildings in the avenue.In order to produce this a little images, can use mobile mapping system (MMS), it is driven by the driver and passes the street of being paid close attention to.Fig. 1 shows this MMS system, explains like preceding text.Microprocessor μ P makes its time or distance with rule come pictures taken at interval or with specific course change through arranging with control camera 9.Use for some, camera 9 be with its can take the building block in the city facade picture mode and aim at environment.These facade pictures are arranged by independent computing machine at time point place (therefore, off-line) after a while and are handled.Fig. 3 shows this off-line layout 10.

Observe, one or more cameras that said picture possibly alternatively be loaded with through the air traffic instrument are taken.

And; Observe; MMS system (or air line) also can comprise one or more laser scanners, and the laser sample of its collection (for example) buildings is used said laser sample in the process of process that picture is mapped to the buildings facade or identification road sign.

In Fig. 3, provide the overview of computing machine layout 10, said computing machine arranges that 10 can be used for carrying out according to calculated off-line of the present invention.Computing machine arranges that 10 comprise the processor 11 that is used to carry out arithmetical operation.

Processor 11 is connected to a plurality of memory assemblies, comprises hard disk 12, ROM (read-only memory) (ROM) 13, Electrically Erasable Read Only Memory (EEPROM) 14 and random-access memory (ram) 15.All these type of memory may not be provided.And these memory assemblies need not to be physically located near processor 11 places, but can be positioned at away from processor 11 places.

Processor 11 is also connected to the member that is used for coming through the user input instruction, data etc., like keyboard 16 and mouse 17.The those skilled in the art also can be provided known other input links such as for example touch screen, tracking ball and/or speech convertor.

The reading unit 19 that is connected to processor 11 is provided.Reading unit 19 is through arranging with from like data carrier reading of data such as floppy disk 20 or CDROM21 and maybe data be written on the said data carrier.Other data carrier can be tape, DVD, CD-R, DVD-R, memory stick etc., and is known like the those skilled in the art.

Processor 11 is also connected to and is used for the printer 23 of print-out on paper, and is connected to display 18, for example the display of known any other type of monitor or LCD (LCD) screen or those skilled in the art.Processor 11 can be connected to loudspeaker 29.

Processor 11 can be connected to communication network 27 by means of I/O member 25, for example PSTN (PSTN), Local Area Network, wide area network (WAN), the Internet etc.Processor 11 can be through arranging to communicate by letter with other communication arrangement through network 27.

Data carrier 20,21 can comprise the computer program that is data and instruction type, and said data are arranged so that execution ability according to the method for the invention to be provided to processor with the instruction warp.Yet this computer program is alternately downloaded via communication network 27.

Processor 11 can be embodied as independently system; Or be embodied as the processor of a plurality of parallel work-flows, its each through arranging to carry out subtask than computation machine program; Or be embodied as one or more primary processors with plurality of sub processor.Functional part of the present invention can even be carried out by the teleprocessing unit of communicating by letter with processor 11 through network 27.

Observe, layout shown in Figure 3 also can be used for the microprocessor in the vehicles 1, although in the vehicles 1, need not use like illustrated all component, like loudspeaker 29 and printer 23.

In one embodiment, processor 11 is through arranging receiving as by camera 9 picture shot, and with in one (for example, hard disk 12) of said picture-storage in its storer.Hard disk 12 is also stored so-called " floor area " of taking the building block of facade pictures from it.These floor areas comprise the 2D data about building block position on earth.In the storer 12 to 15 one stored a program, and said program can be made up facade pictures and floor area by processor 11 operations and instruction processing unit, makes correct facade pictures be associated with correct building block.The data that so obtain are stored for being used for (for example) vehicle navigation system after a while, with show to the driver his skipper the 3D view in street.For this reason, these data can be stored on the DVD.

The present invention is not to being moved so that the computer program that facade pictures is associated with floor area by processor 11.The computer program of any prior art (or still leaved for development) all can be used for carrying out this operation in situation of the present invention, or it can be accomplished down the auxiliary of those skilled in the art.

The present invention relates to the position and the directional data that are associated with (for example) facade pictures.More generally, the present invention relates to the data collected by a certain (some) measuring units, wherein said data are related with on earth position and oriented phase, and this position is in other data of measurement, to measure with orientation.The data that record can (for example) relate to the something that is different from facade pictures fully; As be used for the road side sign of numerical map or be used to search for or survey the edaphic condition of the natural energy resources (oil, rock gas etc.) of soil, or mobile phone or other RF signal strength measurement.Therefore, application of the present invention is not limited to be used to collect the sensor type of the relevant data in a certain on earth position on the vehicles 1.Yet for simplicity, hereinafter will use the instance of collecting facade pictures sometimes when explaination is of the present invention.

As it will be apparent to those skilled in the art that; The 3D position data that relates to each facade pictures must be measured by the MMS system as far as possible exactly; With permission processor 11 the facade pictures data correctly are registered to other data available, for example buildings floor area data.These position datas of facade pictures are directly relevant with the position and the directional data of the moment MMS system of taking each picture at camera 9.Therefore, should know the position and the orientation of MMS system when taking facade pictures as far as possible exactly.When taking these pictures, the MMS system should (for example) have 1 to 1.5 meter or better accuracy about its position on x, y and z, and about 0.1 ° or better accuracy of course, pitching and the rolling of its angular orientation.

Known like the those skilled in the art, prior art systems uses the GPS of system shown in Figure 1 partly to derive position data x, y and possibly also have z.Yet the those skilled in the art knows that also the accuracy of gps system data is because demote like multipath error equal error.Fig. 4 shows the multipath problem in a schematic way.

Fig. 4 shows that MMS drives on the street between two building block BB1 and the BB2.Satellite (not shown) is to earth transmission location signal.If the direct and unbroken transmission path between MMS system and the satellite is with available, the MMS system will be via the signal path SP1 receiving position signal along the straight line between MMS system and the satellite so.Yet building block BB1 has stopped transmission path SP1.Yet as shown in the figure, the MMS system receives like the position signalling by another building block BB2 reflection from satellite really.Yet SP2's said position signalling advances along another path, thereby causes the inaccurate position measurement of the concern satellite such as depending on.

In real case, will often receive via a plurality of paths by the MMS system from the position signalling of satellite.In many cases, there is direct-path SP1 and via a plurality of paths facing to the reflections of building block and trees etc.This reflection sources (for example) is formed by the truck from MMS system next door process.For example with Kalman filter, suitable statistical computation, along with average etc. the form of asking of past of time provides the prior art solution to solve the multipath problem.Other multipath error source can relate to thunder cloud or other ionospheric reflection.It is detailed that the instance of the multipath error of mentioning here is not intended to.Yet, especially in application, on public way truck, surrounding buildings thing, sign and the mobile vehicles, produce local configuration complicated and that change fast based on land.In this case, the standard method that the multipath of observing based on the long period is confirmed is failed probably.

To explain now as can how to be used to judge that by at least one position of collecting among two IMU of other system and the DMI (Fig. 1) and orientation survey data which GPS measured value is likely accurately, and which GPS measured value inaccurate probably (with respect to required accuracy) and not being taken in any position calculation (for example by processor 11 (Fig. 3) execution) after a while is considered.Hereinafter, will be at first with reference to the 2D situation (that is, do not consider height z and do not consider pitching and rolling, and only consider x, the situation in y and course) explain the mode of accomplishing this judgement.Only after it, just will provide the 3D instance.

Calculate 2D position and orientation

Fig. 5 shows four subsequent region A1, A2, A3, A4 in a schematic way, and wherein the MMS system advances along road.Fig. 5 is the schematic plan (not drawn on scale) of road surface, and how its indicating traffic instrument 1 will advance according to different measuring source.Fig. 5 show the MMS system as with arrow A R indicated direction on advancing.The real trace that dotted line indication MMS system among Fig. 5 is advanced.

In all four regional A1, A2, A3, A4, the GPS sample has been collected so that during the shooting of continuous facade pictures, measure the 2D position and the orientation of MMS system by the MMS system.With G (i) (i=1,2,3 ..., I) indicate the track according to the GPS sample of being advanced.Per second can exist 5 or 10 or even more GPS sample, yet the invention is not restricted to this numeral.

Fig. 5 shows the GPS sample with very schematic mode.That is be continuous step, with the GPS samples show.In fact, each step is an instance, wherein has sample time between the successive sample.

And the IMU system has measured the track that the MMS system is advanced.The track that the IMU system is calculated is indicated with solid line.Recalling IMU is relative position and orientation survey device, and needs an absolute reference that it is placed with respect to track.In this example, locate, pure IMU position at random is placed near the real trace in the beginning (left side) of track.Indicate this solid line more and more to depart from real trace along with during the moving of past of time in the MMS system.This is that drift or other unregulated displacement in the calculating of being made owing to the IMU system causes.This displacement/drift is only relative measurement to be provided and not to be provided the fact of absolute measurement to cause by IMU.Error by drift causes can be explained as follows.

Indicated like preceding text, the IMU system provides the data that can therefrom infer following parameter:

● around the rotational speed of x axle, y axle and z axle, i.e. d ω _x/ dt, d ω _y/ dt, d ω _z(Fig. 2 a) for/dt;

● a _x, a _yAnd a _z, i.e. acceleration on x, y and z direction respectively.

In order to calculate position and the orientation in the 2D space, use the acceleration a on x and y direction _xAnd a _yAnd the rotational speed that centers on the z axle, i.e. ω _zFrom these acceleration, calculating location x and y and course as follows:

x＝∫∫a _x·dt ²

y＝∫∫a _y·dt ²

Course=∫ d ω _z/ dt

Usually, the past from these acceleration calculation x, y position and course along with the time is not accurate enough, and this is that said error is accumulated according to double integrator subsequently owing to represent in the output signal that the drift of error causes.Whenever this drift of 1km of advancing can be approximately 1m usually.Hereinafter, will use a technical term " drift " refer to displacement like other kind that causes with the drift of the said kind of these equalities explaination and owing to the cumulative errors in the output signal of IMU system.

At last, the some tracing among Fig. 5 shows that measuring sample G (i) and time based on GPS true course goes up local distance and IMU between the approaching GPS detector and measure and measure with DMI and track that the MMS system of estimation is advanced.

As visible from Fig. 5, in regional A4, gps system is showed the deviation of estimating between track and the GPS output that increases gradually.This possibly be by as the error of all kinds that it will be apparent to those skilled in the art that cause.

And, Fig. 5 show the GPS measured value between regional A1 and A2 and the transition position between A2 and the A3 show that the unexpected displacement in its output (note that Fig. 5 only shows some GPS samples.In fact, the number of the GPS sample in the zone (like regional A2) maybe be much bigger).These a little displacements suddenly can be the indication of multipath error.Yet, can not declare simply that GPS measured value among regional A2 and the A4 is because multipath error and inaccurate.Same possible situation is that GPS sample among regional A1 and the A3 is owing to the multipath error and inaccurate.In addition, can not with inference mode depend in these zones as by the IMU systematic survey to x and the absolute value of y because the unknown that said absolute value is showed as preceding text are explained drifts about, and a certain absolute reference of needs.Yet can use the IMU output data to find out inaccurate and which the GPS sample of which GPS sample is accurately.Then, can remove inaccurate GPS sample, make one group of accurate GPS sample remain.This organizes reliable GPS sample and can be used for again subsequently promptly estimating the drift in the IMU output data and from the IMU measured value, remove said drift that for IMU sets up absolute position and calibration IMU output data totally the position is definite accurately thereby draw.This will come explained in detail to Fig. 7 d and Fig. 8 a to Fig. 8 c referring to Fig. 6, Fig. 7 a hereinafter.

Fig. 6 shows the process flow diagram of the action that can carry out according to the present invention.These actions are carried out by processor 11, and to solve among the regional A2 multipath problem and the deviation shown in the regional A4 both.

In action 62, the time-varying course that processor 11 obtains like the vehicles 1 that recorded by gps system.Here said course is called in " GPS course ".Fig. 8 a shows that this GPS course is at the instance of cycle t0 during the t3.Note that the GPS measurement receives The noise, change indicated like high frequency less in the curve.

Processor 11 comprises x, the y position of the vehicles 1 from other data that gps system obtains, as moves in 88 indicated.Can derive the speed of the vehicles 1 from these x, y position, as move in 78 indicated.As known from prior art, all these measurement data are uncorrelated on statistics.

In action 64, the rotational speed ω that processor 11 acquisitions as the track of being advanced along the MMS system by the IMU system record _z, and the course of the vehicles 1 of calculating like the integrating meter of explaining through preceding text is provided.

In action 72, the time-varying value of wheel angle of the MMS system that processor 11 acquisitions as the track of being advanced along the MMS system by the DMI system record.

In order to calculate position and the orientation in the 2D plane, the vehicles 1 that in most of the cases have greater than the quality of 1000kg are regarded as being equivalent to low-pass filter.Therefore, suppose the quick change that when the vehicles 1 move, will can not observe in the track that the vehicles 1 are advanced.And, suppose that the vehicles 1 are equivalent to fixed frame.

DMI sensor (for example) is installed to the rear wheel of the vehicles 1.Therefore, DMI positional fluctuation with respect to the barycenter of the vehicles 1 in the 2D plane is minimum, and can the fluctuation in its output be regarded as amplitude than the fluctuation in the gps signal little the white noise of Duoing.Therefore, in calculating, can ignore fluctuation in the DMI output.

According to vehicles dynamics simulation, well-known to be vehicles bodies proportional with respect to the acceleration of the vectorial angle of local gravity and the vehicles.This is real under " normally " condition; Said " normally " condition i.e. (for example) vehicles is not involved in accident, and the driver is forwards upwards with mode (non-strong driving style) very stably drive a conveyance (with the brake of same ratio factor, quicken).

Under these supposition, following equality is effective:

" MC track "=" V reference locus "+" MC vibration " (1)

" MC track "=" V reference locus "+" MC vibration " (1)

Wherein:

● the track that the barycenter of MC track=vehicles 1 is advanced;

● the reference locus of V reference locus=advanced like the vehicles 1 of deriving from gps system;

● the local difference between the reference locus that the MC vibration=vehicles 1 are advanced and the barycenter of the vehicles 1; The amplitude of MC vibration is expected in the scope of 0.1m, and can be regarded as the white noise of confirming to track.

Because vehicles bodies can be regarded as fixed frame, so the shared orientation (that is, have identical rolling, pitching and course) identical with the barycenter of the vehicles 1 of having a few of vehicles bodies.Therefore, can the position of the vehicles 1 be confirmed as the 3D motion vector of position of the barycenter of the vehicles 1.

In one embodiment, be not to use the center of the center of the vehicles 1, but the position of gps antenna 8 is used as the center in the local coordinate system as local coordinate system.The advantage of this way is need not to confirm barycenter, and it makes and calculates facility.

Now, action below processor 11 is carried out.

In action 70, local linear regression is carried out in the course of 11 pairs of the processors as the vehicles 1 of being derived by the IMU system, so that obtain along the level and smooth local mean values of warp in the course of track.In this process, processor 11 uses a plurality of GPS samples (after the non-Gaussian noise removal in said GPS sample).This provides accurate " truly " course that possibility is lower than the precision of 0.1 degree that has of the vehicles 1.The output of action 70 is time-varying courses of the vehicles 1.

In action 74, processor 11 is carried out the calibration of separating to rotating like the wheel angle that is provided by DMI.This reproduces the distance that said wheel is advanced.Action 74 result be based on that MMS system that DMI measures advanced in time and the estimation that displacement leaves.Said distance is called as " DMI distance " here.

In action 80, be used for dynamically calibrating this DMI distance by processor 11 like the speed that records by gps system.Therefore in action 80, processor 11 produces time-varying estimation " truly " travel distance.

Observe, dynamically calibrate the DMI measured value more accurate than obtaining said value usually aspect definite " truly " travel distance, because the DMI system receives the influence of inertia distortion less from the IMU system with the GPS velocity measurement.

Action 80 output and 70 the output of moving are input to next action that is called as " curve calculation " 84.In this curve calculation action 84, use following equality:

INS_Trajectory2D＝Traj_true2D+IMU_drift2D+IMU_noise (2)

Wherein:

● the curve that INS_Trajectory2D=is advanced from the vehicles 1 that the meter that inertial navigation system INS is done is calculated, said INS comprises IMU, DMI and GPS unit.This is the output of action 84;

● the vehicles reference locus in the Traj_true2D=2D plane, that is, and the track that the vehicles 1 are truly advanced.

● the drift that the IMU_Drift2D=IMU systematic survey causes.In practice, " IMU_Drift-2D " will be the function that slowly changed along with the past of time of 2D;

● the noise in the IMU_noise=IMU systematic survey.

INS_Trajectory2D in the equality (2) is corresponding to " estimation track " among Fig. 5.

On less distance, following equality is effective:

INS_Trajectory2D=Traj_true2D+IMU_drift2D+IMU_noise (little) (3)

Traj_true-2D＝GPS_meas2D-GPS_noise-GPS_multipath (4)

Wherein:

● the record position signalling of GPS_meas2D=as deriving from gps system

● GPS_noise=among the GPS_meas of gps system as under the situation that does not have the multipath error with the noise that exists.Note that and from the GPS sample any Gaussian noise to be reduced to minimum.Can use any known method that carries out this way.Yet, except that Gaussian noise, possibly still leave other noise.

● the error that causes owing to the multipath problem among the GPS_multipath=GPS_meas.

Now, can derive from equality (3) and (4):

IMU_drift2D＝INS_Trajectory2D-IMU_noise-

[GPS_meas2D-GPS_noise-GPS_mulipath] (5)

Now, rewrite equality (2) with following form:

Traj_ture2D_approx＝INS_Trajectory2D-IMU_drifi2D_approx (6)

Wherein all parameters have and identical meaning in equality (2), and " approx " that add refers to the parameter with approximate value.Equality (6) is the equality that is associated with action 86.And equality (5) can be used for that the drift (IMU_drift2D) that is caused by the IMU system is made first and estimates.That is, estimate that IMU_drift2D is substantially equal to:

IMU_drifi2D≈[IMU_meas2D-GPS_meas-2D] (5a)

Wherein:

IMU_meas2D=is like the course of the vehicles that recorded by the IMU system.

This can explain to Fig. 8 c referring to Fig. 8 a as follows.Fig. 8 a shows the course like the vehicles 1 that recorded by gps system.Possibly still have (Gauss) noise on the signal shown in Fig. 8 a.Fig. 8 b shows identical course, but derives from the IMU system.The course of deriving from the IMU system has drift with respect to true course.

Fig. 8 c shows from the curve of Fig. 8 b and deducts the curve that the curve of Fig. 8 a obtains.The gained curve is the cardinal principle straight line that has skew with respect to initial point.The angle of said curve is corresponding to the single order and the fundamental component of time-varying drift in the IMU system.Still show a certain (Gauss) noise that causes by gps system on the said curve.Yet said noise can be easily through leaching from the known any method of prior art.Remaining error will be owing to the multipath that is identified of explaining like hereinafter causes.

Certainly, when the gps signal that records because multipath problem and non-Gaussian noise and when still containing error, these errors will be also in the curve of Fig. 8 c it is thus clear that.Therefore, have only in the gps signal all errors that cause owing to multipath and non-Gaussian noise all to be removed, just can reproduce suitable estimation the drift in the IMU system like the method for explaining to Fig. 8 c referring to Fig. 8 a.Therefore, when in action 86, using, the result is still very inaccurate.And, in case increasing inaccurate GPS sample is removed from the GPS measured value, just can iterative manner use said method.This will explain hereinafter.

Equality (5) and (6) are made up obtain:

Traj_true2D_approx＝Traj_meas2D-EQ

[GPS_meas2D-GPS_noise-GPS_multipath-IMU_meas2D] (7)

Wherein EQ [...] refers to the time series equilibrium of parameter, that is, and and in the moving average of confirming between [...] on predetermined number (the for example 100) sample (or any other low-pass filter).

Observe, can all these 2D signals be regarded as on x and y direction, having the parameter time series of component.Therefore, orientation problem is resolved into a series of 1 dimension time series equalization problem.

As visible from equality (7), present problem is in order to calculate EQ [...], need to know GPS_noise and GPS_multipath-IMU_meas2D.If can from the GPS measured value, leach these two parameters, equality will work preferably so.This can accomplish in following iterative process.

At first, as move indication in 92, processor 11 is used the local variance wave filters, makes its test is whether:

|GPS_meas2D-Traj_true2D_approx|＞Threshold (8)

Wherein GPS_meas2D is formed by all GPS samples.

This " Threshold " (for example) equals the mean square difference to the variance measurement of GPS set GPS.Can calculate local variance to some (for example 20 to 10) GPS ranges of the sample.

Fig. 7 a shows to Fig. 7 d how this variance filter device can work when being applied to instance shown in Figure 5.

Fig. 7 a shows the some curves corresponding to curve shown in Figure 5 (if not having actual path).First curve is showed piece shape signal, the vehicles 1 that its expression is seen from vertical view advanced according to a plurality of continuous GPS sample G (1), G (2), G (3) ... Track.As visible from Fig. 7 a, in existence between GPS sample G (i) and the G (i+1), between G (i+5) and the G (i+6) and between G (j+6) and the G (j+7) than great transition.And, exist to start from the transition that changes gradually that G (j+1) locates.Bigger transition possibly cause owing to the multipath error.Yet, only based on the GPS sample, can't judge which GPS sample and measure relevantly owing to mistake that noise/the multipath problem causes that and which sample is correct.

Second curve among Fig. 7 a is a solid line, and it refers to as from IMU measured value a _xAnd a _yThe track that calculates.This IMU line has drift (common every 1km is 1m), but definite drift value is unknown.

The 3rd curve among Fig. 7 a is a dotted line, and it is the curve that newly calculates of action 86 (that is the equalities of preceding text (6)).

Now, in action 92, processor 11 is carried out the variance filter devices, that is, processor 11 with as the mode explained referring to equality (8) dotted line of GPS line and Fig. 7 a is compared.As can be obvious from Fig. 7 a, the said zone of relatively having reproduced with higher variance.Note that in this description term " variance " refers to poor between two difference rather than continuous samples in the same signal between the unlike signal.Indicate with the dot-dash circle in Fig. 7 a in these zones.Therefore, at first, this has passed on sample G (j+1) to have so high variance to G (j+5), so that it should not use in the calculating of the track that the vehicles 1 are advanced again.Secondly, this has passed on the sample of the transition position between G (i) and the G (i+1) to have too high variance.Therefore, should not re-use sample G (i+1) yet.For sample G (i+5) also is like this.Therefore, the result who uses the variance wave filter does not re-use the sample relevant with noise (that is, G (j+1) is to G (j+5)), and the GPS sample (that is, G (i+1) and G (i+5)) of the transition position that causes owing to the multipath error.

Here; Suppose with approximate trajectory Traj_true-2D_approx relatively be fair relatively; Its any drifting problem that too much not caused by the IMU system influences, and can in the less time cycle of for example several contiguous GPS samples, be left in the basket because the influence of the drift in the IMU signal is enough little.

In action 90, processor 11 is removed the GPS samples that the variance filter device found (its can be kept in the storer but no longer be considered) now from said group of GPS sample.This indicates with cross in Fig. 7 b.This has reproduced one group of new GPS sample.

Observe in one embodiment, after a while action place in time, which processor 11 can seek is sample best in the removed GPS sample, and it is added in the said group of accurate GPS sample once more.This spends 11 some times of processor possibly, but also can improve the approximate of track.Especially after having removed many GPS samples, add these a little GPS samples to said group once more with the recurrence mode and can significantly reduce the space that has no the GPS sample.This can make said method more accurate.

In action 94, the new GPS sample of this group is used for the track Traj_ture2D_approx that the vehicles 1 are advanced is done the estimation that makes new advances.The new track of estimating is indicated with dot-and-dash line in Fig. 7 b.And, estimate the drift that the IMU data cause according to equality (5a) once more.

In action 96, processor 11 is used " shaped filters " and is detected said group of GPS sample that remains in the GPS sample after action 92, and it is got rid of from calculate track Traj_true-2D_approx.In this " shaped filters ", applicable equations (8) once more, but subsequently equality (8) is applied to from the track that newly calculates of action 94 and the threshold value of sensitivity more.

This action will be reproduced the new GPS sample of showing higher variance: in Fig. 7 b, this will identify should remove GPS sample G (i+2) and G (i+4).Processor 11 is from removing these GPS samples G (i+2) and G (i+4) said group of remaining GPS sample after action 94.

Shown in Fig. 7 c, in action 98, whether processor 11 inspections find this to have the GPS sample of higher variance a bit in action 96.If find, processor is to move 100 continuation so.If do not find, processor 11 is to move 102 continuation so.

In action 100, processor 11 reuses the track Traj_true2D_approx (in Fig. 7 c, indicating with dot-and-dash line) that remaining GPS sample calculates new estimation.Through using the track Traj_true2D_approx of this new estimation, calculate the drift that causes as by the IMU system according to equality (5a) once more.

Therefore, because the test in the action 98 as long as finding the GPS sample with higher variance with iterative manner among the new track Traj_true2D_approx that estimates, is just removed the GPS sample with this higher variance.If such as the action 98 in the institute inspection, do not find this GPS sample that has higher variance a bit again, so processor with move 102 continuation.In action 102, processor 11 combines the estimation track Traj_true2D_approx of gained and the course that becomes (and therefore becoming with the position on track) in time of the vehicles 1.

Therefore, after the action of processor 11 executeds process flow diagram shown in Figure 6, know the track that the MMS system is advanced in the 2D plane.That is, processor 11 has calculated time-varying x, y and course.

Note that process of the present invention utilized the measurement sequence before the optimum estimate of finding track.In using in real time, measure sequence and only must consider measured value in the past.Though the present invention can be through revising with in being applied to use in real time suitably, fully benefit mainly is when iteration allows to handle to the point in past and with reference to the point in future of any specified point, to realize.From this reason, the present invention is regarded as and is primarily aimed at applied off-line.

Remarks and replacement scheme

Make following observation.

The method of the drift in the estimation track that causes owing to the drift in the output signal of IMU system like the estimation of explaining to Fig. 8 c referring to Fig. 8 a also can be used for the available orientation after action 66 of direct estimation IMU system and exports the drift in the signal.For this reason, the phasing signal from the IMU system deducts the time-varying phasing signal that after removing all inaccurate GPS samples, can use from gps system.This is similar to the signal of signal shown in Fig. 8 c with reproduction, and it shows the time-varying drift in the IMU signal.

Under some situations, the multipath problem possibly relate to a large amount of GPS samples.Subsequently, the extra-regional GPS sample at multipath is influenced by multipath also maybe.Therefore, processor 11 also can be removed at the more extra-regional GPS measured values of multipath, for example before said zone with said zone after 20 GPS samples.

In order suitably to detect " point " shape difference on every side in the GPS curve, " shaped filters " must seek to compromise between following compromise requirement:

1. wave filter considered point as much as possible (in travel distance)

2. the wave filter brachmorphy shape that also should detect in the GPS curve is unusual.

Can be used for setting up this wave filter from the known several different methods of prior art.

The window sample size that is used for variance filter device and shaped filters can be variable, and calculates to remove the influence that the vehicles stop by means of institute's travel distance (for example on two time orientations, being not less than 100m (or any other value)) of measuring according to DMI.

As move 84, action 86, action 94 and move in 100 indicatedly, processor 11 calculates the estimation curve of the track that the MMS systems are advanced.This can be according to accomplishing from the known any method of prior art.Yet, in one embodiment, the method for using as explaining referring to Fig. 9 a and Fig. 9 b.

Fig. 9 a shows the track Tture that the MMS system is advanced.Yet, provide time-varying output of calculating the action 70 in course only to be the form of a finite population sample (IMU system per second provides (for example) 200 samples, and the gps system per second provides 5 samples).Therefore, should be from time-varying these directed samples and real trace from coming approximate representation MMS system to be advanced as institute's computed range from action 80 output.The very common method of known this approximate track of calculating is showed with arrow Tappr1 in this technology.Arrow Tappr1 indication is based on the calculating of the linear interpolation between the calculation level P of institute in succession on the track.Yet as visible from Fig. 9 a, this only provides first approximation, and it possibly not be very accurate when driving in curve in the MMS system.

Calculating the alternative method of the approximate track between the successive point P indicates with track Tappr2 in Fig. 9 a.Track Tappr2 itself is crooked, and is the basis with interior the inserting of being calculated between the successive point on the track based on the clothoid part.Fig. 9 b shows the nemaline definition of rondo.That is, when curve has to the distance R of point of fixity R0 and said distance R reduces with the amount corresponding to 1/R along curve towards point of fixity R0 travel distance D the time, said curve is called convoluted shape.Therefore, in Fig. 9 b, distance R 1 is reduced to R2=R1-a/R after the distance B of having advanced, and after the another distance B of having advanced, is reduced to R3=R2-a/R2.Employed definite curvature of circling round part depends on the amount of the curvature of the track that calculates to the track before the some P between two successive point P.

Be based on following understanding according to the solution of Fig. 9 a and Fig. 9 b: when the driver rotate its bearing circle with vehicle driving in curve or when come out in a turning, the most natural mode of carrying out this way is to carry out this way with the mode that the vehicles are followed the convoluted shape curve.And the curve in the road is asked to the convoluted shape design now legally, because this curve shape allows the driver to drive through said curve with the highest possible speed.If not like this, the driver possibly only drive through curve to avoid dangerous situation with limited speed so.

Input to calculating still is course and distance.Sample carries out said calculating one by one.Through using the method for explaining referring to Fig. 9 a and Fig. 9 b, the estimation of the real trace that has realized more accurately the MMS system being advanced than art methods estimation in the cards.

The result is based on the estimation track of the serial connection of a plurality of curve Tappr2.

Calculate around orientation, slope, local gravity and the z level of x axle and y axle

Preceding text have been explained and can how to have been used " shaped filters " and remove inaccurate GPS sample from a series of GPS samples (comprising the inaccurate GPS sample that causes owing to the multipath error).And, through comparing, can eliminate the drift that causes owing to the IMU system with iterative manner pairing approximation track and the track of confirming from the GPS sample.Through doing like this, showed that the accurate of track that can confirm in the 2D world is similar to.Yet real world is 3D, and uses based on the great majority of the measurement of (for example) MMS system shown in Figure 1 and to need the 3D data equally.These 3D data comprise orientation and the z level around x axle and y axle along the MMS system of the road of advancing.Yet another important parameter can be the ramp angle with respect to the local gravity vector.Note that the local gravity vector need not to overlap with the vector that points to ground ball center.This is especially true under the situation of mountain range, and wherein the quality on mountain range causes gravity vector from pointing to the vectorial partial deviations of ground ball center.Difference can be up to 2%.For (for example) truck, how earth center vector points to so unimportant, but the local gravity vector how to point to be important.Only the decision of local gravity vector puts on the gravity of truck.This receives publicity in the truck industry gradually.

The GPS sensor does not provide the local gravity vector.The GPS sensor only provides the data about the vector that points to ground ball center.

In principle, the IMU sensor can be in measure local gravity vector under the rest configuration.Under dynamic case, IMU measures the stack of gravity and inertial force.Therefore under dynamic case, when attempting measuring gravity vector, need from the IMU reading, remove all dynamic force.

Basically, the method that goes out mentioned herein is following:

● the IMU system provides the measured value that adds all translational accelerations about gravity under dynamic case, because the MMS system goes when collecting data;

Yet ●, DMI systematic survey speed, and therefore processor 11 can derive the translational acceleration of MMS system on the x direction (note that the x direction is not the x direction of fixing, but with respect to the IMU system in the MMS system of being installed in along the direction that moves of road; Similarly, y direction part is defined as perpendicular to the x direction and in road surface);

● therefore, processor 11 can be removed the translational acceleration of deriving from the DMI system from the measured value that the IMU system is obtained, thereby reproduces gravity vector via the time-varying pitching of MMS system and rolling.

To explain this with mathematical way now.

After setting up accurate course data and calibration DMI system, processor 11 can be derived the point-device dynamic acceleration that influences accelerometer readings.These acceleration can be described by following equality:

${\stackrel{\→}{a}}_x=d{\stackrel{\→}{v}}_x/\mathrm{dt}---\left(9\right)$

${\stackrel{\→}{a}}_y={\stackrel{\→}{\mathrm{\ω}}}_z{\×\stackrel{\→}{v}}_x---\left(10\right)$

Wherein:

equals the acceleration on the direct of travel of MMS system;

is the speed of MMS system on the x direction that derives from the DMI system;

equals the acceleration that records on perpendicular to the direct of travel of MMS system and the direction perpendicular to the local gravity vector;

is the flight path rate of turn from the MMS of 2D method derivation mentioned above;

Remove horizontal acceleration, and in addition, can derive normal acceleration from following equality:

${\stackrel{\→}{a}}_z={\stackrel{\→}{\mathrm{\ω}}}_y{\×\stackrel{\→}{v}}_x---\left(11\right)$

Wherein:

equals the acceleration on the direction opposite with local gravity vector;

equals the pitching vectorial with respect to local gravity of MMS system.

This removes the chance of the fundamental component of dynamic acceleration for preprocessor 11.Produce " being similar to static " reading in this way:

${\stackrel{\→}{a}}_{\mathrm{STx}}={\stackrel{\→}{a}}_{\mathrm{IMUX}}-{\stackrel{\→}{a}}_x---\left(12\right)$

${\stackrel{\→}{a}}_{\mathrm{STy}}={\stackrel{\→}{a}}_{\mathrm{IMUy}}-{\stackrel{\→}{a}}_y---\left(13\right)$

${\stackrel{\→}{a}}_{\mathrm{STz}}={\stackrel{\→}{a}}_{\mathrm{IMUz}}-{\stackrel{\→}{a}}_z---\left(14\right)$

Wherein:

equals the acceleration that the IMU system records on the x axle

equals static (dynamically the having removed) acceleration on the x axle

equals the acceleration that the IMU system records on the y axle

equals static (dynamically the having removed) acceleration on the y axle

equals the acceleration that the IMU system records on the z axle

equals static (dynamically the having removed) acceleration on the z axle

These nothings that " are similar to " value static and will will be the local gravity reading estimate partially, yet still have the white noise that some are caused by the vibration of MMS system when the movement on roads.Along with the past of time, the mean value of this noise component will equal zero.Therefore, can use to ask on average to come from signal and eliminate this white noise.

Now, can obtain pitching and rolling from following equality:

$\sin \left({\mathrm{pitch}}_{\mathrm{acc}}\right)=\frac{a_{\mathrm{<figure-callout\; id="2"\; label="STx\; a\; STx"\; filenames="HSA00000555462400011.png,HSA00000555462400042.png"\; state="\{\{state\}\}">STx</figure-callout>}}}{\sqrt{a_{\mathrm{STx}}^{}}}---\left(15\right)$

$\sin \left({\mathrm{roll}}_{\mathrm{acc}}\right)=\frac{a_{\mathrm{<figure-callout\; id="2"\; label="STy\; a\; STx"\; filenames="HSA00000555462400011.png,HSA00000555462400042.png"\; state="\{\{state\}\}">STy</figure-callout>}}}{\sqrt{a_{\mathrm{STx}}^{}}}---\left(16\right)$

Pitch from equality (15) and equality (16) derivation _AccAnd roll _AccValue be based on pitching and the no inclined to one side approximate value of rolling of orientation of the IMU system of acceleration measurement.Yet, because said value is to derive from the data with noise, so said value will also be the data with noise.

In order to estimate the directed pitching of IMU and the actual value of rolling, processor 11 can use like preceding text in the method for 2D described in partly in a similar manner.How preceding text can remove drift in the estimation of vehicles course if having been explained.Now, processor 11 uses pitch respectively _AccAnd roll _AccData as a reference, and calculate the IMU parameter respectively

With

In drift.

Figure 10 a shows to time window t0 to the approximate pitching value pitch of the gained of t3 _AccIn instance, pitch _AccRise at moment t1 place, and reduce at moment t2 place.The signal of being showed has the noise that causes owing to the MMS system vibration during advancing in road.

Now, processor 11 can be derived pitching and the rolling directed actual value of IMU system with respect to gravity vector.In the description hereinafter, these values are with being called as pitch respectively _TrueAnd roll _True

The acceleration analysis that processor 11 is made through the IMU system obtains the pitching value around the y axle.Through double integrator, the pitching pitch that is calculated is derived in processor 11 acceleration analysis from then on _IMUShow the result among Figure 10 b.Pitching pitch as shown in the figure, as to calculate _IMUDo not have noise, but it has the drift that is caused by double integrator.

Then, processor 11 deducts the curve shown in Figure 10 a from the curve shown in Figure 10 b.This produces the curve shown in Figure 10 c.Curve shown in Figure 10 c should show that the drift in the measured pitching of IMU system has some noises that add it to.Therefore, (for example) asked averaging or local linear regression through use, and processor 11 is removed noise from the signal shown in Figure 10 c, thereby reproduces the drift from the pitching that IMU measures.Now, processor 11 can compensate the drift since the pitching that measures of IMU system.This provides pitching pitch accurately _True

Now, processor 11 also can be as with respect to pitching pitch _True(map 10a is to Figure 10 c) and the explaination the identical mode of mode calculate rolling roll accurately _True

Slope calculations

In principle, but static IMU sensor measure local gravity vector that is to say, in case know pitching and rolling, just know the local gravity vector with respect to local gravity vector.In the part of preceding text, presented about how the method for measure local gravity vector under dynamic case.From these reading slope calculations, need to consider the extra dynamic perfromance of the vehicles for further.For instance, if car has relatively short length, or the IMU system accurately is not installed to vehicles bodies, and the local gravity vector that records so possibly be accurately, but the angle on slope maybe be still very inaccurate.This certainly can be through measuring with very long car (or truck), and the IMU system accurately is installed to the vehicles avoids, but that is unpractical.And this can be through being avoided with the measurement of IMU sensor at car just in motion, but measure the too much influence that possibly suffer the drift in the IMU output.

Figure 11 displaying is used to calculate some correlation parameters with respect to the slope of gravity.Suppose that the MMS system is in the some P place on the slope.The MMS system goes with speed

; Said speed is recorded by the DMI system (please be remembered; The x direction is with respect to the moving coordinate system that is defined by MMS itself, thus the x direction along the slope itself).Purpose from explaination; Relatively large deviation with the direction of ball center from sensing ground shows that in most of the cases, this deviation will be not more than 2% to local gravity vector

.Travel speed

can be decomposed into the local gravity vector in the direction of the "vertical" Speed

and perpendicular to this vertical velocity

speed (hereinafter, a vertical speed is not shown in Figure 11).Therefore, following equality is set up:

slope_angle＝arcsin(v _z/v _x) (17)

Vertical speed v _zAlso can from the measurement that the IMU stationary system is made, derive said IMU stationary system measure local gravity vector

Direction on vertical static acceleration Following equality is set up:

${\stackrel{\&RightArrow;}{v}}_z=\&Integral;{\stackrel{\&RightArrow;}{a}}_{\mathrm{STz}}\&CenterDot;\mathrm{dt}---\left(18\right)$

The result of this equality shows a certain drift, but can on small distance, use well.Can be with the vertical speed v that derives from equality (18) _zValue substitution equality (17) in.And, can be with from the known travel speed of DMI system

substitution equality (17).

In other words, through obtaining travel speed from the DMI measured value From about normal acceleration

The static measured value of IMU calculate vertical speed v _z, and applicable equations (17), processor 11 calculates with respect to the local gravity vector

Ramp angle.

In first replacement scheme, processor 11 uses following equality to calculate ramp angle:

v _z＝v _x·sin(pitch _true-k·a _x) (19)

Wherein: pitch _True=pitching, it can obtain from the calculating that preceding text are explained;

a _xAcceleration on the=x direction, it can obtain from the DMI system;

The k=constant.The value of k can find through test, and depends on the quality spring performance of MMS system.

Observe, equality (19) with from well known in the prior art the same.Yet; New content is to understand this equality to confirm the ramp angle with respect to local gravity vector

, but and processor 11 so this slope of easy for calculation.

In second replacement scheme, both come slope calculations processor 11 use equalities (17) and equality (19), and do not use the IMU measurement to obtain vertical speed v _zUse equality (19) to come from the value that obtains through equality (18), to remove drift (or other error), and reproduce vertical speed value v through error compensation _{Z, err-comp}Therefore, can avoid confirming vertical speed v from the IMU system _zIn drift.Therefore, the method provides to processor 11 and has calculated with respect to the local gravity vector

Vertical speed v _zOther type, that is, and with v _{Z, err-comp}Form.In case calculate vertical speed value v in this way through error compensation _{Z, err-comp}, processor 11 just uses equality (17) to calculate ramp angle, wherein in equality, uses v _{Z, err-comp}Replace v _z

Although slope calculating is rendered as based on equality (17) here; But in general, the calculating of the slope vectorial with respect to local gravity can and wherein recognize that the measurement of IMU system is any equivalent equality made from respect to the local gravity vector based on the measured value that wherein uses DMI system and IMU system.

And, observe, the slope that calculates with respect to the local gravity vector is regarded as separate inventions, it is different from any invention in the 2D plane, for example removes inaccurate GPS sample.In case have one group of suitable accurate GPS sample, just can the described mode of preceding text come slope calculations.This group GPS sample need not to obtain with the mode of being explained in this document.

Calculate the z level

Now, processor 11 can use following equality easily to calculate the time-varying relative z level of MMS system:

z(t)＝∫v _x(t)·sin(slope_angle(t)) (20)

Wherein: z (t) is the relative z level on the direction of local gravity vector

.

This relative z level can be used for calculating absolute z level by processor 11, the z level that is higher than the sea level that promptly on the z direction opposite with the center position of the earth, records.For this reason, processor 11 can be through arranging that it is used to make each relative z level value to be displaced to absolute sea level value to derive shift vector.This shift vector can (for example) be obtained by processor 11 in the following manner: the 2D method that preceding text are explained has been identified as all good GPS z _GPS(t) reading is asked on average, thereby obtains mean value, and from then on mean value deducts the corresponding mean value of z (t) value relatively.

Claims (7)

1. a computing machine is arranged; It is used to calculate vehicles ramp angle slope_angle with respect to local gravity vector when track is advanced; Said computing machine arranges and comprises processor (11), said processor (11) through arrange with storer (12; 13; 14; 15) communication, said storer (12; 13; 14; 15) storage can be by the computer program that comprises instruction and data of said processor (11) operation; The said vehicles comprise distance measuring unit (DMI) and Inertial Measurement Unit (IMU); The xy plane is defined by x axle and y axle part; Said x axle is along said track, and said y axle is perpendicular to said x axle and perpendicular to said local gravity vector, and said processor (11) calculates said ramp angle through arranging with the measurement that both make from said distance measuring unit (DMI) and said Inertial Measurement Unit (IMU).

2. computing machine according to claim 1 arranges that wherein said processor is through arranging to calculate said ramp angle slope_angle according to following equality:

slope_angle＝arcsin(v _z/v _x) (17)

Wherein:

v _zSpeed on the=direction opposite with said local gravity vector;

Speed on the said x direction of

=derive from said distance measuring unit (DMI).

3. computing machine according to claim 2 arranges that wherein said processor is through arranging to calculate v from following equality _z:

${\stackrel{\&RightArrow;}{v}}_z=\&Integral;{\stackrel{\&RightArrow;}{a}}_{\mathrm{STz}}\&CenterDot;\mathrm{dt}---\left(18\right)$

Wherein: ${\stackrel{\&RightArrow;}{a}}_{\mathrm{STz}}={\stackrel{\&RightArrow;}{a}}_{\mathrm{IMUz}}-{\stackrel{\&RightArrow;}{a}}_z---\left(14\right)$

Wherein: the z directional acceleration that

=said Inertial Measurement Unit (IMU) records;

Acceleration on the z direction opposite of

=calculate through following equality with said local gravity vector:

${\stackrel{\&RightArrow;}{a}}_z={\stackrel{\&RightArrow;}{\mathrm{\&omega;}}}_y\&times;{\stackrel{\&RightArrow;}{v}}_x---\left(11\right)$

Wherein:

equals the pitching of the said vehicles with respect to said local gravity vector.

4. computing machine according to claim 2 arranges that wherein said processor is through arranging to calculate v from following equality _z:

v _z＝v _x·sin(pitch _true-k·a _x) (19)

Wherein:

Pitch _True=said the vehicles are with respect to the true pitching of said local gravity vector;

a _xAcceleration on the=x direction that derives from said distance measuring unit;

The k=constant.

5. computing machine according to claim 3 arranges, wherein said processor (11) through arrange with:

Calculate:

v _z＝v _x·sin(pitch _true-k·a _x) (19)

Wherein:

Pitch _True=said the vehicles are with respect to the true pitching of said local gravity vector;

a _xAcceleration on the=x direction that derives from said distance measuring unit;

The k=constant;

Use the v of equality (19) from obtaining through equality (18) _zThe middle error of removing, thus vertical speed value v reproduced through error compensation _{Z, err_comp}

With said vertical speed v through error compensation _{Z, err_comp}Replace the v in the equality (17) _z

6. arrange according to arbitrary described computing machine among the claim 2-5 that wherein said processor (11) is through arranging to calculate the horizontal z of z (t) on the direction opposite with said local gravity vector

from following equality:

z(t)＝∫v _x(t)·sin(slope_angle(t)) (20)。

The calculating vehicles when track is advanced with respect to the method for the ramp angle slope_angle of local gravity vector

; The said vehicles comprise distance measuring unit (DMI) and Inertial Measurement Unit (IMU); The xy plane is defined by x axle and y axle part; Said x axle is along said track; And said y axle is perpendicular to said x axle and perpendicular to said local gravity vector, and said method comprises that the measurement that both make from said distance measuring unit (DMI) and said Inertial Measurement Unit (IMU) calculates said ramp angle.

Images