CN105180955A - Real-time precise positioning method and real-time precise positioning device of motor vehicles - Google Patents

Abstract

The invention provides a real-time precise positioning method and a real-time precise positioning device of motor vehicles. The method comprises the following steps: updating motorized displacement and updating measurement. The step of updating the motorized displacement comprises the following steps: taking a starting speed as an initial condition and carrying out integration on an inertial accelerated speed relative to time to obtain a system inertial speed; taking an initial position coordinate as an initial condition and carrying out integration to obtain an initial position; repeating the steps to obtain a continuous navigation coordinate system; and eliminating the deviation between the navigation coordinate system and a vehicle coordinate system to obtain posterior probability. The step of updating the measurement comprises the following steps: carrying out laser scanning on the environment to obtain reflection data; and calculating posterior probability distribution according to the posterior probability, the reflection data and map data information. According to the real-time precision positioning method and the real-time precision positioning device, the technical effects of high positioning precision on the motor vehicles, rapid updating and strong anti-interference capability are realized.

Description

Motor vehicle real-time and precise localization method and device

Technical field

The present invention relates to automotive field, particularly relate to a kind of motor vehicle real-time and precise localization method based on probability map and device.

Background technology

Motor vehicle accurately determines that in the urban environment of high complexity position is the extremely important link realizing automatic driving of motor vehicle, and the electronic chart particularly gathered in advance for main dependence realizes the automated driving system of environment sensing.

The method realizing in prior art locating has a variety of, and more common have:

1. rely on onboard sensor realization to locate completely:

A. based on GPS, or based on global location/inertial navigation emerging system (GPS/INS).

B. based on other sensor, as stereo camera, laser radar.

2. to rely on the informatization and network in vehicle and running environment to realize two-way communication.

Running environment also greatly impact locate the method taked, because the difficulty of location in various situations being not quite similar.Obviously, at dynamic environment (i.e. changing environment, road as busy city) in position will than the environment of static state (during as cross-country run, there is no the environment of what other vehicle) in position more difficult because the uncertainty that more dynamic disorder causes will be considered.

Common localization method is each has something to recommend him.But will realize the positioning precision of reliable and stable centimetre-sized, difficulty is still very large.And this is indispensable for realizing automatic Pilot in city.The deviation of tens centimetres often determines on crowded urban road, and whether motor vehicle can travel, holds walkway etc. by line ball.

Summary of the invention

For this reason, needing to provide a kind of can provide higher positioning precision (if sensor such as GPS/INS and laser radar can reach certain precision, then can reach 10 centimetre-sized), can academic environment, and provide the precision of location along with passage of time, motor vehicle localization method and the device of good anti-interference can be had to the change of the change of environment, particularly dynamic barrier.

For achieving the above object, inventor provides a kind of motor vehicle real-time and precise localization method, comprises the steps, kinematical displacement step of updating, measurement updaue step;

Wherein kinematical displacement step of updating comprises the steps:, using motor vehicle starting velocity as starting condition, motor vehicle inertial acceleration to be carried out integration for the time, obtains system inertia speed; System inertia position is obtained as starting condition integration using motor vehicle starting position coordinates; Repeat above-mentioned steps, obtain continuous print navigation coordinate system according to system inertia speed and system inertia position; Eliminate the deviation of described navigation coordinate system and vehicle coordinate system, obtain posterior probability;

Measurement updaue comprises the steps: to carry out laser scanning to environment, obtains reflectance data, calculates Posterior probability distribution according to posterior probability, reflectance data and map data information, judges to obtain vehicle position according to Posterior probability distribution.

Further, after measurement updaue, also comprise step, maximal possibility estimation is carried out to the result of Posterior probability distribution, select optimized coordinate to represent the estimated value of motor vehicle coordinate.

Particularly, optimized coordinate is selected to represent estimated value by following formula:

$\begin{array}{cc}x=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·x}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}& y=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·y}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}\end{array}$

Wherein P is Posterior probability distribution, and x, y are GPS azimuthal coordinates.

Particularly, the formula obtaining posterior probability described in is:

$\stackrel{\‾}{P}(x,y)=\mathrm{\η}\·\underset{i,j}{\Σ}P(i,j)\·\exp \left(-\frac{1}{2}{(i-x)}^2{(j-y)}^2/{\mathrm{\σ}}^2\right)$

Wherein represent and upgrade the posterior probability of rear vehicle position at (x, y) at kinematical displacement, η represents normaliztion constant, and x, y are GPS azimuthal coordinates, and i, j are cell coordinate.

Particularly, described according to posterior probability, reflectance data and map data information calculate Posterior probability distribution be specially:

P(x,y|z,m)＝η·P(z|x,y,m)·P(x,y)

Wherein P (x, y|z, m) is Posterior probability distribution, and z is reflectance data, and m is map data information, and x, y are GPS azimuthal coordinates, and η represents normaliztion constant.

A kind of motor vehicle real-time and precise locating device, comprises kinematical displacement update module, measurement updaue module;

Wherein kinematical displacement update module is used for using starting velocity as starting condition, carries out integration, obtain system inertia speed to inertial acceleration for the time; Also for obtaining inertial position using starting position coordinates as starting condition integration; Also for repeating above-mentioned steps, obtain continuous print navigation coordinate system; Eliminate the deviation of described navigation coordinate system and vehicle coordinate system, obtain posterior probability;

Measurement updaue module is used for carrying out laser scanning to environment, obtains reflectance data; Also for calculating Posterior probability distribution according to posterior probability, reflectance data and map data information, judge to obtain vehicle position according to Posterior probability distribution.

Further, also comprise maximal possibility estimation module, described maximal possibility estimation module is used for carrying out maximal possibility estimation to the result of Posterior probability distribution, selects optimized coordinate to represent the estimated value of motor vehicle coordinate.

Particularly, described maximal possibility estimation module selects optimized coordinate to represent estimated value by following formula:

$\begin{array}{cc}x=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·x}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}& y=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·y}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}\end{array}$

Wherein P is Posterior probability distribution, and x, y are GPS azimuthal coordinates.

Particularly, described kinematical displacement module for obtaining the formula of posterior probability is:

$\stackrel{\‾}{P}(x,y)=\mathrm{\η}\·\underset{i,j}{\Σ}P(i,j)\·\exp \left(-\frac{1}{2}{(i-x)}^2{(j-y)}^2/{\mathrm{\σ}}^2\right)$

Wherein represent and upgrade the posterior probability of rear vehicle position at (x, y) at kinematical displacement, η represents normaliztion constant, and x, y are GPS azimuthal coordinates, and i, j are cell coordinate.

Particularly, described measurement updaue module is used for being specially according to posterior probability, reflectance data and map data information calculating Posterior probability distribution:

P(x,y|z,m)＝η·P(z|x,y,m)·P(x,y)

Wherein P (x, y|z, m) is Posterior probability distribution, and z is reflectance data, and m is map data information, and x, y are GPS azimuthal coordinates, and η represents normaliztion constant.

Be different from prior art, technique scheme adopts the method for Bayesian inference, along with the mobile distribution constantly updating state variable of motor vehicle, reaches precision high, updating decision, the technique effect that antijamming capability is strong.

Accompanying drawing explanation

Fig. 1 is the motor vehicle real-time and precise localization method schematic flow sheet described in the specific embodiment of the invention;

Fig. 2 estimates schematic diagram for motor vehicle under the one-dimensional condition described in the specific embodiment of the invention moves with position probability distribution;

Fig. 3 is the motor vehicle measurement updaue effect schematic diagram described in the specific embodiment of the invention;

Fig. 4 is the motor vehicle real-time and precise locating device module diagram described in the specific embodiment of the invention.

Description of reference numerals:

400, kinematical displacement update module;

402, measurement updaue module;

404, maximal possibility estimation module.

Embodiment

By describe in detail technical scheme technology contents, structural attitude, realized object and effect, coordinate accompanying drawing to be explained in detail below in conjunction with specific embodiment.

Refer to Fig. 1, be a kind of motor vehicle real-time and precise localization method schematic flow sheet of the present invention, comprise the steps, the renewal of S1 kinematical displacement, S2 measurement updaue;

Wherein kinematical displacement upgrades and comprises the steps: that S101 is using starting velocity as starting condition, carries out integration, obtain system inertia speed to inertial acceleration for the time; S103 obtains inertial position using starting position coordinates as starting condition integration; Repeat above-mentioned steps S101, S103, obtain continuous print navigation coordinate system; S105 eliminates the deviation of described navigation coordinate system and vehicle coordinate system, obtains posterior probability; Measurement updaue S2 comprises the steps: that S201 carries out laser scanning to environment, obtains reflectance data; S203 calculates Posterior probability distribution according to posterior probability, reflectance data and map data information, and S205 judges to obtain vehicle position according to Posterior probability distribution.By said method, reach motor vehicle positioning precision high, updating decision, the technique effect that antijamming capability is strong.

In certain embodiments, test vehicle and be loaded with laser radar, GPS/INS sensor.GPS/INS can export global positioning coordinates, current angular velocity and acceleration (being called for short inertial acceleration both following) with very high frequency (such as, higher than 100 hertz).Laser radar is through calibration.And described map data information be gather in advance, pre-service and be stored in the map based on probability of the target running environment in truck-mounted computer.

Based on above-mentioned hypothesis, can the following probability model of component:

Motor vehicle state is at a time by a stochastic variable x _trepresent, i.e. state variable.State variable can comprise position coordinates, Eulerian angle (Eulerangles comprises pitch, roll, yaw) etc.

The probability distribution of state variable, represents the estimation of motor vehicle for self-position.

Adopt the way of Bayesian inference, along with the movement of motor vehicle, constantly update the probability distribution of state variable:

Observation that ο is new each time (scanning that such as laser radar one time 360 degree is complete), motor vehicle all can change estimation for self-position in conjunction with the electronic chart gathered in advance:

p(x|z,m)

Here z represents observation data, and m is electronic map data.The electronic chart gathered in advance is the map of a two dimension, gridding.Each unit in grid stores the reflective information that surface corresponding in environment is irradiated for laser radar.This information can be a vector as the absolute value of reflection strength, also can have more complicated structure: such as use a probability distribution (such as Gaussian distribution) to record the distribution of each unit laser intensity.

Each new maneuver of ο, also can change the estimation for self-position: p (x|u)

Here u represents the change of the position of being measured by motor vehicle sensor (as inertial navigator).Below with the change of an embodiment demonstration along with the probability distribution of the mobile status variable of motor vehicle:

Here please see Figure in the embodiment shown in 2, small machine people represents motor vehicle.Motor vehicle is only upper mobile in the direction (x-axis) of one dimension.Bel (x) i.e. motor vehicle, for the estimated value of the probability distribution of self-position, also can be expressed as p (x|z, m, u), assuming that at the beginning, motor vehicle does not have the information of oneself position any as shown in Figure 2 a, and therefore bel (x) is identical for the value of all x.

Next please see Figure 2b, motor vehicle is moved along to the first fan in front of the door, and Yishanmen observed by sensor.Owing to knowing position map having three fan doors and door in advance, so become shown in figure for estimation bel (x) of self-position.Present motor vehicle narrows down to several position for the estimation of self-position quickly: all places having door on map.Following again, motor vehicle continues a motor-driven segment distance forward, and the estimation that this section of motor-driven result motor vehicle distributes for self-position when having arrived the position shown in Fig. 2 c has also moved forward.It should be noted that the information for each position but declines to some extent, which reflects and motor-driven itself there is uncertainty (error of such as inertial navigator).Next please see Figure 2d again, motor vehicle sensor detects Yishanmen again.In conjunction with the information of kinematical displacement, motor vehicle has had very strong deflection (crest the highest under figure) for the distribution of self-position quickly.This is because specifically see Yishanmen again apart from rear for mobile one section on map, current position is exactly probably at the second fan in front of the door.

After understanding of basic conception hypothesis, be appreciated that namely basic step of the present invention is divided into two steps for the filtration (namely eliminating the process that noise cancellation signal infers real probability) of state variable: kinematical displacement upgrades.This step, according to the shift value of kinematical displacement sensor measurement, reduces the confidence for self-position valuation, with the error reflecting in instrument and motor vehicle motion exists.Measurement updaue (figure tri-).This step, by mating result and the electronic chart of observation, increases the confidence for self-position valuation.In some embodiment shown in Fig. 3, the measurement of laser radar represents with light and dark lines.Can find out in fig. 3 a, vehicle front both direction arrow, the deviation of the arrow on the arrow of measurement and map.In fig 3b through measurement updaue, two arrows overlap substantially.

In further embodiments, although position coordinates x, y are for being set to continuous print, because the error of sensor, motor vehicle controller itself, simultaneously in order to improve the efficiency of calculating, this method adopts histogram by interval for the individual continuous print one by one of continuous x, y segmentation.There is a fixing value in each interval, is the mean value of original successive value.Make filtrator in this way hereinafter referred to as histogram filtrator.Another kind of filtrator conventional in location is particle filter.But GPS can be limited in hunting zone in scope (rice) certain near estimation point by let us, and directly can calculate the probability of all coordinates (x, y) in 10*10 cell.In this case by x, even if the benefit that y coordinates table is shown as histogram filtrator is that the particle around physical location has disappearance, also can not affects us and effectively expressing is carried out to x, y coordinate.

Kinematical displacement upgrades

Kinematical displacement to be made exactly and upgrade the motion model first will setting up a close as far as possible reality.We can obtain a continuous print navigation coordinate system to use the data of sensor, and concrete step is as follows:

1) using starting velocity as starting condition, use correct kinematical equation, relative to the time, integration is carried out to inertial acceleration, just can obtain system inertia speed.

2) then using reference position coordinate as starting condition again integration just can obtain inertial position.

3) often obtain new data and just repeat above two steps, by the continuous integration to new inertial acceleration data, obtain a continuous print navigation coordinate system.

Our this navigation coordinate system by obtaining GPS and inertial acceleration data integrate is now devious with the coordinate system of self vehicle position.If can eliminate the deviation between this navigation coordinate system and self vehicle position coordinate system, we just can obtain global location more accurately.Our emphasis is just now, how to go this deviation of simulation estimate.Because inertial position obtains by carrying out integration to speed, we can be that random walk under the Gaussian noise of 0 goes accurately to simulate the side-play amount of inertial position relative to physical location by expectation value.Under this model, we upgrade with the probability of following formula to each cell:

$\stackrel{\‾}{P}(x,y)=\mathrm{\η}\·\underset{i,j}{\Σ}P(i,j)\·\exp \left(-\frac{1}{2}{(i-x)}^2{(j-y)}^2/{\mathrm{\σ}}^2\right)$

Wherein represent in mobile update rear vehicle position at (x, y) posterior probability, η represents normaliztion constant (normaliztion constant makes this function just equal 1 for the integration between a given zone for the constant contained by any interval of any nonnegative function).This parameter of σ is then used to the ratio describing side-play amount, and because the frequency upgraded is very high, this ratio is usually very little.Although in theory, this equation is the quadratic equation about element number, and namely about the biquadratic equation of search radius, operand is not large.But in practice, we can be optimized by the cell in detection range (x, a y) Liang Huosange unit completely.This is because we can upgrade with frequency high arbitrarily, and the ratio of skew is relatively very little, so in a short period of time, the skew of vehicle sensors is very limited.Explain from statistical angle, side-play amount probability is within the specific limits high, and concrete scope is much just sees the specific requirement to this probability.Give an example, inertial navigation coordinate system produced in 0.1 second the probability being greater than 45cm side-play amount be level off to 0 (although such jumping GPS is moved than compared with normal).Even if there is large skewing event, very fast renewal next time also can be corrected it return.Because the expected value of this side-play amount is roughly directly proportional to car speed, so the ratio that we are also directly proportional to car speed with goes to carry out mobile update.When vehicle translational speed is slow, we just can the corresponding frequency reducing to upgrade

Measurement updaue

The Part II of histogram filtrator is exactly measurement updaue, then constantly can estimate to upgrade to the orientation of vehicle with the reflectance data newly obtained by constantly carrying out new laser scanning to environment.Be on the increase along with by the measurement data of scanning circumstance, we also can be more and more accurate to the estimation of vehicle heading.This just embodies a kind of study to environment and adaptive faculty in fact.

In order to process the measurement data of continuous renewal, we, according to will the sensing data gridding of accumulation, set up a corresponding rolling grid.Often obtain a new laser reflectance data, we will process timely to it, it is encoded in this rolling network both deposited, upgrades corresponding cell.Through such process, we directly can compare the cell on the cell of sensing data and map, and can not receive the cell of a lot of response data to those, for example, tree or cell corresponding to larger dynamic disorder, undue weighting.

In certain embodiments, if z is our sensing data, m is map, x and y is possible GPS azimuthal coordinates, so utilizes bayesian criterion:

P(x,y|z,m)＝η·P(z|x,y,m)·P(x,y)

We can be by variance the Gaussian distribution inaccuracy of going simulating GPS/IMU orientation to estimate, so P (x, y) is exactly GPS Gauss and posterior probability in fact product

$P(x,y)=\mathrm{\η}\·\exp \left(\frac{x^2+y^2}{-2{\mathrm{\σ}}_{GPS}^2}\right)\·\stackrel{\‾}{P}(x,y)---\left(1\right)$

Next we need to calculate P (z|x, y, m), and namely when known (x, y) and m, sensing data equals the probability of z.

$P\left(z\right|x,y,m)={\Π}_{i,j}\exp {\left(\frac{-{(m_{r(i-x,j-y)}-z_{r(i,j)})}^2}{2{\left(m_{{\mathrm{\σ}}_{\left(i-x,j-y\right)}+z_{{\mathrm{\σ}}_{\left(i,j\right)}}}\right)}^2}\right)}^{\mathrm{\α}}---\left(2\right)$

In this formula above, m _σand z _σbe two two-dimentional arrays, represent the intensity level standard deviation of map unit and sensing data respectively.Give an example, on map m, what represent is with the global estimated position of vehicle for initial point, the intensity level standard deviation of past 0.45 meter, east and the cell toward 1.2 meters, north.Equally, we also use m _rand z _rrepresent average (reflection) intensity of map unit and sensing data respectively.

For same example, what represent is on map m, with the global estimated position of vehicle for initial point, and average (reflection) intensity of past 0.45 meter, east and the cell toward 1.2 meters, north.

Note, why this product will be raised to α (α >1) power by us, is the slight change because of the system calibration mistake considered due to possible or environment, between data not necessarily completely independently.

We have had (1) and (2) now, just can obtain final Posterior probability distribution:

$P\left(x,y|z,m\right)=\mathrm{\η}\·\underset{i,j}{\Π}\exp {\left(\frac{-{(m_{r(i-x,j-y)}-z_{r(i,j)})}^2}{2{\left(m_{{\mathrm{\σ}}_{\left(i-x,j-y\right)}+z_{{\mathrm{\σ}}_{\left(i,j\right)}}}\right)}^2}\right)}^{\mathrm{\α}}\·\exp \left(\frac{x^2+y^2}{-2{\mathrm{\σ}}_{GOS}^2}\right)\·\stackrel{\‾}{P}\left(x,y\right)$

According to Posterior probability distribution, P (x, y|z, m) can be made to get the x of maximal value, and namely y is judged as the coordinate of vehicle position.Although the variance of sensing data and map unit is all very little, we further can remove the combined standard deviation minimizing sensing data and map unit, in the hope of more powerful stability.In addition, conveniently calculate, we can go to calculate P (x, y|z, m) by the cell near the global estimated position of a use vehicle in several meters.This hunting zone can change flexibly according to the degree of accuracy of gps system, and that is, gps system is more accurate, and it is less that our hunting zone just can be determined.If instead the location of gps system is accurate not, hunting zone just can expand by we.

Maximal possibility estimation

In some further embodiment, after measurement updaue, also comprise step S3 maximal possibility estimation, comprise step S301 and maximal possibility estimation is carried out to the result of Posterior probability distribution, select optimized coordinate to represent the estimated value of motor vehicle coordinate.Particularly, after having had final Posterior distrbutionp, last step select exactly one possibility optimized (x, y) represent and our estimated value be namely finally output to the coordinate points of automobile navigation schedule module.Here also non-immediate gets max _x,yp (x, y) is just passable, because the maximal value distributed as a multimodal, and max _x,yp (x, y) is easy to jump between each peak value, causes us also can jump to the coordinate points of automobile navigation schedule module feedback.This is obviously unfavorable for realizing automobile navigation accurately.

In order to not only optimize coordinate estimated value from probability but also can ensure better continuity, we are selected by following formula:

$\begin{array}{cc}x=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·x}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}& y=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·y}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}\end{array}$

Two formula above, from definition, have been raised to α (α >1) power Posterior distrbutionp exactly and have then got its particle.Here ascending power be also due to P (x, y) not necessarily completely mutually independently.

This coordinate points estimated value (x, y) can be calculated with the frequency of 10 hertz and be outputted to automobile navigation schedule module.Such vehicle just can utilize this optimized azimuth estimation value to do path planning in global coordinate system.By maximal possibility estimation step, ensure that the continuity after coordinate optimizing, make this method have higher practical value.

Fig. 4 is a kind of motor vehicle real-time and precise locating device module diagram, and described device comprises kinematical displacement update module 400, measurement updaue module 402;

Wherein kinematical displacement update module 400 is for using starting velocity as starting condition, carries out integration to inertial acceleration for the time, obtains system inertia speed; Also for obtaining inertial position using starting position coordinates as starting condition integration; Also for repeating above-mentioned steps, obtain continuous print navigation coordinate system; Eliminate the deviation of described navigation coordinate system and vehicle coordinate system, obtain posterior probability;

Measurement updaue module 402, for carrying out laser scanning to environment, obtains reflectance data; Also for calculating Posterior probability distribution according to posterior probability, reflectance data and map data information, judge to obtain vehicle position according to Posterior probability distribution.By above-mentioned modular design, reach motor vehicle positioning precision high, updating decision, the technique effect that antijamming capability is strong.

In some further embodiment, also comprise maximal possibility estimation module 404, described maximal possibility estimation module represents estimated value for selecting optimized coordinate.

Particularly, described maximal possibility estimation module selects optimized coordinate to represent estimated value by following formula:

$\begin{array}{cc}x=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·x}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}& y=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·y}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}\end{array}$

Wherein P is Posterior probability distribution, and x, y are GPS azimuthal coordinates.Maximal possibility estimation module ensure that the continuity after coordinate optimizing, makes this method have higher practical value.

Particularly, described kinematical displacement module, for repeating above-mentioned steps, obtains continuous print navigation coordinate system; Eliminate the deviation of described navigation coordinate system and vehicle coordinate system, the formula obtaining posterior probability is:

$\stackrel{\‾}{P}(x,y)=\mathrm{\η}\·\underset{i,j}{\Σ}P(i,j)\·\exp \left(-\frac{1}{2}{(i-x)}^2{(j-y)}^2/{\mathrm{\σ}}^2\right)$

Wherein represent and upgrade the posterior probability of rear vehicle position at (x, y) at kinematical displacement, η represents normaliztion constant, and x, y are GPS azimuthal coordinates, and i, j are cell coordinate.Reach motor vehicle positioning precision better high, updating decision, the technique effect that antijamming capability is strong.

Particularly, described measurement updaue module is used for being specially according to posterior probability, reflectance data and map data information calculating Posterior probability distribution:

P(x,y|z,m)＝η·P(z|x,y,m)·P(x,y)

Wherein P (x, y|z, m) is Posterior probability distribution, and z is reflectance data, and m is map data information, and x, y are GPS azimuthal coordinates, and η represents normaliztion constant.Reach motor vehicle positioning precision better high, updating decision, the technique effect that antijamming capability is strong.

It should be noted that, in this article, the such as relational terms of first and second grades and so on is only used for an entity or operation to separate with another entity or operational zone, and not necessarily requires or imply the relation that there is any this reality between these entities or operation or sequentially.And, term " comprises ", " comprising " or its any other variant are intended to contain comprising of nonexcludability, thus make to comprise the process of a series of key element, method, article or terminal device and not only comprise those key elements, but also comprise other key elements clearly do not listed, or also comprise by the intrinsic key element of this process, method, article or terminal device.When not more restrictions, the key element limited by statement " comprising ... " or " comprising ... ", and be not precluded within process, method, article or the terminal device comprising described key element and also there is other key element.In addition, in this article, " be greater than ", " being less than ", " exceeding " etc. be interpreted as and do not comprise this number; " more than ", " below ", " within " etc. be interpreted as and comprise this number.

Those skilled in the art should understand, the various embodiments described above can be provided as method, device or computer program.These embodiments can adopt the form of complete hardware embodiment, completely software implementation or the embodiment in conjunction with software and hardware aspect.The hardware that all or part of step in the method that the various embodiments described above relate to can carry out instruction relevant by program has come, described program can be stored in the storage medium that computer equipment can read, for performing all or part of step described in the various embodiments described above method.Described computer equipment, includes but not limited to: personal computer, server, multi-purpose computer, special purpose computer, the network equipment, embedded device, programmable device, intelligent mobile terminal, intelligent home device, wearable intelligent equipment, vehicle intelligent equipment etc.; Described storage medium, includes but not limited to: the storage of RAM, ROM, magnetic disc, tape, CD, flash memory, USB flash disk, portable hard drive, storage card, memory stick, the webserver, network cloud storage etc.

The various embodiments described above describe with reference to the process flow diagram of method, equipment (system) and computer program according to embodiment and/or block scheme.Should understand can by the combination of the flow process in each flow process in computer program instructions realization flow figure and/or block scheme and/or square frame and process flow diagram and/or block scheme and/or square frame.These computer program instructions can being provided to the processor of computer equipment to produce a machine, making the instruction performed by the processor of computer equipment produce device for realizing the function of specifying in process flow diagram flow process or multiple flow process and/or block scheme square frame or multiple square frame.

These computer program instructions also can be stored in can in the computer equipment readable memory that works in a specific way of vectoring computer equipment, the instruction making to be stored in this computer equipment readable memory produces the manufacture comprising command device, and this command device realizes the function of specifying in process flow diagram flow process or multiple flow process and/or block scheme square frame or multiple square frame.

These computer program instructions also can be loaded on computer equipment, make to perform sequence of operations step on a computing device to produce computer implemented process, thus the instruction performed on a computing device is provided for the step realizing the function of specifying in process flow diagram flow process or multiple flow process and/or block scheme square frame or multiple square frame.

Although be described the various embodiments described above; but those skilled in the art are once obtain the basic creative concept of cicada; then can make other change and amendment to these embodiments; so the foregoing is only embodiments of the invention; not thereby scope of patent protection of the present invention is limited; every utilize instructions of the present invention and accompanying drawing content to do equivalent structure or equivalent flow process conversion; or be directly or indirectly used in other relevant technical fields, be all in like manner included within scope of patent protection of the present invention.

Claims (10)

1. a motor vehicle real-time and precise localization method, is characterized in that, comprises the steps, kinematical displacement step of updating, measurement updaue step;

Wherein kinematical displacement step of updating comprises the steps:, using motor vehicle starting velocity as starting condition, motor vehicle inertial acceleration to be carried out integration for the time, obtains system inertia speed; System inertia position is obtained as starting condition integration using motor vehicle starting position coordinates; Repeat above-mentioned steps, obtain continuous print navigation coordinate system according to system inertia speed and system inertia position; Eliminate the deviation of described navigation coordinate system and vehicle coordinate system, obtain posterior probability;

Measurement updaue comprises the steps: to carry out laser scanning to environment, obtains reflectance data, calculates Posterior probability distribution according to posterior probability, reflectance data and map data information, judges to obtain vehicle position according to Posterior probability distribution.

2. a kind of motor vehicle real-time and precise localization method according to claim 1, is characterized in that, also comprise step after measurement updaue, carry out maximal possibility estimation to the result of Posterior probability distribution, select optimized coordinate to represent the estimated value of motor vehicle coordinate.

3. a kind of motor vehicle real-time and precise localization method according to claim 2, is characterized in that, select optimized coordinate to represent estimated value by following formula:

$\begin{array}{cc}x=\frac{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}\·x}{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}}& y=\frac{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}\·y}{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}}\end{array}$

Wherein P is Posterior probability distribution, and x, y are GPS azimuthal coordinates.

4. a kind of motor vehicle real-time and precise localization method according to claim 1, is characterized in that, described in obtain posterior probability formula be:

$\stackrel{\‾}{P}(x,y)=\mathrm{\η}\·\underset{i,j}{\Σ}P(i,j)\·\exp (-\frac{1}{2}{\left(i-x\right)}^2{\left(j-y\right)}^2/{\mathrm{\σ}}^2)$

Wherein represent and upgrade the posterior probability of rear vehicle position at (x, y) at kinematical displacement, η represents normaliztion constant, and x, y are GPS azimuthal coordinates, and i, j are cell coordinate.

5. a kind of motor vehicle real-time and precise localization method according to claim 1, is characterized in that, described according to posterior probability, reflectance data and map data information calculate Posterior probability distribution be specially:

P(x,y|z,m)＝η·P(z|x,y,m)·P(x,y)

Wherein P (x, y|z, m) is Posterior probability distribution, and z is reflectance data, and m is map data information, and x, y are GPS azimuthal coordinates, and η represents normaliztion constant.

6. a motor vehicle real-time and precise locating device, is characterized in that, comprises kinematical displacement update module, measurement updaue module;

Wherein kinematical displacement update module is used for using starting velocity as starting condition, carries out integration, obtain system inertia speed to inertial acceleration for the time; Also for obtaining inertial position using starting position coordinates as starting condition integration; Also for repeating above-mentioned steps, obtain continuous print navigation coordinate system; Eliminate the deviation of described navigation coordinate system and vehicle coordinate system, obtain posterior probability;

Measurement updaue module is used for carrying out laser scanning to environment, obtains reflectance data; Also for calculating Posterior probability distribution according to posterior probability, reflectance data and map data information, judge to obtain vehicle position according to Posterior probability distribution.

7. a kind of motor vehicle real-time and precise locating device according to claim 6, it is characterized in that, also comprise maximal possibility estimation module, described maximal possibility estimation module is used for carrying out maximal possibility estimation to the result of Posterior probability distribution, selects optimized coordinate to represent the estimated value of motor vehicle coordinate.

8. a kind of motor vehicle real-time and precise locating device according to claim 7, it is characterized in that, described maximal possibility estimation module selects optimized coordinate to represent estimated value by following formula:

$\begin{array}{cc}x=\frac{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}\·x}{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}}& y=\frac{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}\·y}{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}}\end{array}$

Wherein P is Posterior probability distribution, and x, y are GPS azimuthal coordinates.

9. a kind of motor vehicle real-time and precise locating device according to claim 6, is characterized in that, described kinematical displacement module for obtaining the formula of posterior probability is:

$\stackrel{\‾}{P}(x,y)=\mathrm{\η}\·\underset{i,j}{\Σ}P(i,j)\·\exp (-\frac{1}{2}{\left(i-x\right)}^2{\left(j-y\right)}^2/{\mathrm{\σ}}^2)$

Wherein represent and upgrade the posterior probability of rear vehicle position at (x, y) at kinematical displacement, η represents normaliztion constant, and x, y are GPS azimuthal coordinates, and i, j are cell coordinate.

10. a kind of motor vehicle real-time and precise locating device according to claim 6, is characterized in that, described measurement updaue module is used for calculating Posterior probability distribution according to posterior probability, reflectance data and map data information and is specially:

P(x,y|z,m)＝η·P(z|x,y,m)·P(x,y)

Wherein P (x, y|z, m) is Posterior probability distribution, and z is reflectance data, and m is map data information, and x, y are GPS azimuthal coordinates, and η represents normaliztion constant.