CN105758408A - Method and device for building local maps - Google Patents

Abstract

The invention discloses a method and a device for building local maps.The method includes steps of establishing local coordinate systems; recording pose information when snapshooting is carried out at every time; carrying out Kalman filtering on the basis of the snapshooting and the pose information; carrying out nearest neighbor matching on Kalman filter results; matching each point in first point sets with the nearest points in second point sets, computing coordinate transformation and reducing errors between the match points.Map information is scanned by the aid of laser light during the snapshooting, and pose includes relative local coordinate system offset and direction angles.According to the technical scheme, the method and the device have the technical advantages that the local coordinate systems are established, the snapshooting and the pose information are recorded, the follow-up filter matching steps are carried out, and accordingly errors generated during local map building can be reduced.

Description

Local map constructing method and device

Technical field

The present invention relates to map and gather field, particularly relate to a kind of for the local map constructing method in super large closed loop ground drawing method and device.

Background technology

In the through engineering approaches of unmanned vehicle (or other robot), practical process, how in the scope of a super large region of tens kilometers, even several thousand kilometers (several kilometers) major issue is, and for a long time driving process realizes to record while self position accurately through the map of environment.

Although the benefit of priori map is self-evident, but can obtain when priori map is not all.It is independent of priori map and can greatly expand the use scope of robot.Such as, range of application is from Development of cities to field.For the other type of robot needing navigator fix, such as unmanned underwater robot or unmanned vehicle, obtain priori map and be likely to very difficult or unrealistic.Therefore, possesses the ability building map in real time very useful for unmanned vehicle (or other robot).

Above-mentioned problem needs to solve in two aspects:

1. in local aspect (by the impact of the effective range of robot sensor and algorithm, be typically in rice/ten meter/hundred meter level other), it is necessary to solve the relatedness problem of the sensing data gathered on several positions of closing on over time and space.The sensing data of several different continuous print station acquisition is mated by certain mode, is the basis building local map.

2. in global level, it is necessary to a kind of method finding between current local data and the overall situation (whole map) data coupling.It is to say, when unmanned vehicle comes back to the three unities arrived before time, it is exactly some place of previously recorded mistake on map that unmanned vehicle is wanted to pick out current home environment.This function is most important for super large-scale independent navigation, because generally positioning the inertial navigator relied on or the mileage problem in respect of cumulative error, the more remote error of distance is more big.And some localization methods not affected by distance, such as global position system, there are again many restrictions.In this case, the error of accumulation between twice arrival is just had an opportunity to substantially eliminate when unmanned vehicle comes back to the place arrived before.This is also the closed loop problem in so-called navigation.

This method proposes the technology of local map structuring in a kind of global map matching technique, it is possible to be applied in global scope map match, and uses modification method to reduce the error existed in the map structuring of local, improves the accuracy of map.Solve the coupling in global scope.This technology may be used for the location in this method and the coupling of map structuring system, it is also used as other system needing to solve closed loop problem, a kind of (method relying on experience data base to improve unmanned vehicle location adaptability to changes) mentioned in such as another section.It addition, global map matching technique can act on the map of Real-time Collection, it is also possible to act on the map of priori.

Summary of the invention

For this reason, it may be necessary to provide one to be applied to local map constructing technology, solve the problem that map structuring error is too big.

For achieving the above object, inventor provide a kind of local map constructing method, comprise the steps,

Set up local coordinate system；

Recording posture information during each snapshot, described snapshot is laser scanning cartographic information, and described pose includes relative local coordinate system side-play amount and deflection；

Kalman filtering is carried out based on described snapshot and posture information；

The result of above-mentioned Kalman filtering is carried out arest neighbors coupling: mate the closest approach that first each point concentrated is concentrated to second point, calculating coordinate change, reduce the error between match point.

Specifically, described " carrying out Kalman filtering based on described snapshot and posture information " including:

Kalman filtering based on absolute pose and the Kalman filtering based on permanent delay pose；

Specifically, described calculating coordinate change, the error reduced between match point specifically includes step:

Calculating every pair of match point difference in normal direction by alignment error computing formula, described difference is designated as matching error,

Described matching error is carried out Lorentz correction；

It is used for generating the covariance leading matched transform recently relative to the Jacobian matrix of pose transformation parameter by the variance of matching error and matching error.

A kind of local map construction device, sets up module, snapshot logging modle, filtration module and arest neighbors matching module including coordinate,

Described coordinate sets up module for setting up local coordinate system；

Record posture information when described snapshot logging modle is for each snapshot, described snapshot is laser scanning cartographic information, and described pose includes relative local coordinate system side-play amount and deflection；

Described filtration module is for carrying out Kalman filtering based on described snapshot and posture information；

Described arest neighbors matching module is for carrying out arest neighbors coupling to the result of above-mentioned Kalman filtering: mates the closest approach that first each point concentrated is concentrated to second point, calculating coordinate change, reduces the error between match point.

Specifically,

Described filtration module is used for the Kalman filtering based on absolute pose and the Kalman filtering based on permanent delay pose；

Specifically, described arest neighbors matching module is specifically additionally operable to:

Calculating every pair of match point difference in normal direction by alignment error computing formula, described difference is designated as matching error,

Described matching error is carried out Lorentz correction；

It is used for generating the covariance leading matched transform recently relative to the Jacobian matrix of pose transformation parameter by the variance of matching error and matching error.

Being different from prior art, technique scheme is by setting up local coordinate system and recording snapshot and positional information, and follow-up filtering coupling step, has reached to reduce the technique effect of the error existed in the map structuring of local.

Accompanying drawing explanation

Fig. 1 is the position and posture evolution diagram described in the specific embodiment of the invention；

Fig. 2 is the Point matching schematic diagram based on surface normal described in the specific embodiment of the invention；

Fig. 3 is that the normal vector described in the specific embodiment of the invention calculates schematic diagram；

Fig. 4 is the histogram of surface normal in the map described in the specific embodiment of the invention；

Fig. 5 is the projection histogram of the Weight described in the specific embodiment of the invention；

Fig. 6 is the map rectangular histogram described in the specific embodiment of the invention and Entropy sequence comparison diagram；

Fig. 7 is the method for exhaustion global peak schematic diagram described in the specific embodiment of the invention；

Fig. 8 is the exhaustive relevant diagrammatic series of views described in the specific embodiment of the invention；

Fig. 9 is the figure relevant with Entropy sequence of the rectangular histogram described in the specific embodiment of the invention；

Figure 10 is recipient's performance characteristic (ROC) curve chart described in the specific embodiment of the invention；

Figure 11 is the local map constructing method flow chart described in the specific embodiment of the invention；

Figure 12 is the local map construction device module map described in the specific embodiment of the invention；

Figure 13 is the global closed loop map-matching method flow chart described in the specific embodiment of the invention；

Figure 14 is the global closed loop map matching means module map described in the specific embodiment of the invention；

Figure 15 is the coupling schematic diagram of the local map that the time point different, discrete described in the specific embodiment of the invention obtains.

Description of reference numerals:

1200, coordinate sets up module；

1202, snapshot logging modle；

1204, filtration module；

1206, arest neighbors matching module；

1400, map acquisition module；

1402, histogram calculation module；

1404, correlation calculations module；

1406, side-play amount computing module；

1408, synthesis module；

1410, Entropy sequence computing module；

1412, exhaustive computations module；

1414, optimal threshold determines module.

Detailed description of the invention

By describing the technology contents of technical scheme, structural feature in detail, being realized purpose and effect, below in conjunction with specific embodiment and coordinate accompanying drawing to be explained in detail.

1. general thought

In SLAM (simultaneouslocalizationandmapping), reliable data association technique is particularly critical for the environmental map setting up large scale.Data association technique has the requirement of two aspects: (1) local aspect represents the internal ring building nomography；(2) global level represents newly-established map area and previously established map area repetition degree and closed loop.Setting up by the iterative scans matching technique of Shandong nation of local map, and in conjunction with EKF, state comprises the pose of current pose and periodic samples before and sampling interval from current time.Fixing sampling reduces the growth of error in location and result map.Mating for global map, we enhance existing histogram intersection corresponding technology, the Entropy sequence of introducing projection histogram and detailed correlating method in order to for the reliable coupling in unstructured moving grids.Map registration information before this ensure that closed loop and being not based on.Environmental map can be divided into: the map of feature based, situational map, and visual field map.

2. the local map based on laser scanning is set up

First introduce a kind of local map method for building up below, just can set up local map merely with laser scanning sequence and estimate the state of robot.The motion of robot can be estimated by continuous print laser scanning, and in order to ensure the overlapping region of scanning, the frequency of laser scanning is sufficiently high relative to the speed of robot.The method uses a kind of EKF (EKF), utilizes robot " snapshot " of pose in the time before is not grown to strengthen the status information that robot is current.

The scanning of current location compares with the snapshot of posture information before and updates current state and estimate current pose and speed.Utilize arest neighbors iteration (ICP) matching algorithm of Shandong nation to realize the comparison of scanning room.In this way, local map is incremented setting up of formula and reaches maximum until state vector dimension, and now, current map has been set up, and begins setting up new local map.The closed loop problem being discussed below is exactly determine the spatial relationship between the map of local.The foundation of local discussed herein map.

2.1 preliminary definitions

One frame scan is produced one group of directive point set in laser single pass, and this organizes the directive point set pose relative to sensor.The expression of point set is also at sensor coordinate system.

We distinguish the map of two kinds of aspects: local map and global map.Local map is produced by scanning sequence one group limited, and the pose scanned each time is expressed in a single local coordinate system.Therefore as shown in figure 11, a kind of local map constructing method, start from step S1100 and set up local coordinate system.Global map is the set of local map, and each local coordinate system transforms in a unified global coordinate system.Such as a local map can comprise the scanning of ten meters of scopes, and global map comprises builds all of local map in figure overall process.

Coordinate transform from coordinate system b to coordinate system a is designated as T_a ^b.One coordinate system can measure the time or map reference is quoted.

One Atlas figure is the network of a local map coordinates system, and each limit of network represents estimation and the uncertainty of coordinate transform between the map of overlapping local.Scheming from Atlas, a global map can utilize border conversion to produce by calculating the global change of each node coordinate system unified relative to.

2.2 based on the Kalman filtering of pose snapshot

EKF (EKF) can be used to maintain state and the covariance of all poses in the map of local.Wave filter does not utilize direct map feature, it is not required that from laser scanning data, extract feature.This method also needs the data sampling carrying out map to have numerous embodiments, for instance sampling vehicle, automatic driving car etc., illustrates for sampling robot in certain embodiments.Also including step S1102, record posture information during each snapshot, described snapshot is laser scanning cartographic information, and described pose includes relative local coordinate system side-play amount and deflection.In the particular embodiment, each robot pose preserved comprises a snapshot, is the initial data of corresponding timestamp acquisition, because of referred to herein as pose snapshot Kalman filtering (PSKF).

The pose T at local map m moment t of robot_m ^tIt is expressed as the transfer amount x of local coordinate system_t, y_tWith deflection θ_t.The state vector X of PSKF_mIt is pose and the pose t in selected moment before of current time t₀,t₁,...,t_NRow put:

$X_m=\left[\begin{array}{c}\left(\begin{array}{c}{x}_t\\ y_t\\ {\mathrm{\θ}}_t\end{array}\right)\\ \left(\begin{array}{c}{x}_{t_N}\\ y_{t_N}\\ {\mathrm{\θ}}_{t_N}\end{array}\right)\\ .\\ .\\ .\\ \left(\begin{array}{c}{x}_{t_1}\\ y_{t_1}\\ {\mathrm{\θ}}_{t_1}\end{array}\right)\\ \left(\begin{array}{c}{x}_{t_0}\\ y_{t_0}\\ {\mathrm{\θ}}_{t_0}\end{array}\right)\end{array}\right]\≡\left[\begin{array}{c}{T}_m^t\\ T_m^{t_N}\\ .\\ .\\ .\\ T_m^{t_1}\\ T_m^{t_0}\end{array}\right]$

Also carry out step S1104 subsequently, carry out Kalman filtering based on snapshot and posture information.Time initial, PSKF state only comprises the initial pose of robot.Laser visual angle current in distance and angle threshold.Not having attitude to impinge upon distance soon and angle meets the threshold value from present laser visual angle, new pose joins in state vector.These pose snapshots are referred to as absolute moment (AT) pose, because they are the robot states relative to particular moment.The prediction step of PSKF only updates the current pose of robot.State vector adds new pose just before current pose updates.In some documents, it is referred to as " delaying state " or the SLAM of " based on track " EKF state vector retains the absolute pose in moment in the past.

The limited size of map in the quantity of absolute position snapshot, the therefore full-size of restriction state vector sum covariance matrix.The dimension maximum due to PSKF state vector is defined, and therefore required in each time cycle calculating time is also defined.

The prediction steps of PSKF uses a model processing noise estimated based on vehicle kinematics and present speed.Such as, if vehicle is nonholonomic system, we can utilize the speed being perpendicular to wheel to be approximately zero.Or, if the data of speedometer can obtain, they can be used to replace velocity estimation.

For each time step, the scanning of current time t and the scanning preserved are (at moment t_i) mate (utilizing ICP) continuously.If having obvious overlap in current scanning and the scanning room preserved, ICP algorithm the transfer matrix T determined_ti ^tIt is the observation of the two coordinate system relative pose.The relative status variable used in filter measurement model z=h (X) is to update PSKF state and covariance matrix

Z=h (X),

$\begin{array}{c}{T}_{t_i}^t=\⊕T_m^{t_i}\⊕T_m^t\\ =T_{t_i}^m\⊕T_m^t\end{array},$

Two symbols are counter transference matrix and composite operator respectively.Relative pose T_ti ^tMeasure covariance with it to be obtained by the calculating of iterative scans matching algorithm.It should be noted that the information of renewal corrected pose current in PSKF state and be all before pose, and Current Scan and scanning overlap associated therewith be also detected.

2.2.1 the pose snapshot of permanent delay step-length

We utilize extra position and posture to enhance PSKF state model, and extra state model is used to reduce the error building in figure and increases.In further embodiments, except the Kalman filtering based on the absolute position and posture in distance interval, the wave filter of enhancing also comprises the position and posture from current pose in permanent delay (FL) interval: detailed description below introduction is based on Kalman's recorded broadcast method of permanent delay pose

$X_m=\left[\begin{array}{c}{T}_m^t\\ T_m^{(t-1)}\\ T_m^{(t-2)}\\ T_m^{t_N}\\ .\\ .\\ .\\ T_m^{t_1}\\ T_m^{t_0}\end{array}\right]---\left(1\right)$

FL pose snapshot be before n time scanning time state (n is typically smaller than equal to 3), when updating each time, they are replaced by n time nearest scanning.The update mode of FL pose is same as the update mode of AT pose.It should be noted that FL pose indexes relative to current pose in equation (1), and AT pose is absolute indices.

The degree of overlapping when degree of overlapping of scanning is greater than with nearest AT state when usual Current Scan and FL state.Thus improve the quality that transfer matrix updates.Wave filter can include in FL state more more newly arrive reduce build in figure error.Embodiment shown in Fig. 1 describes typical PSKF snapshot and utilizes the state evolution of single FL pose.As it can be seen, FL state introduces extra measurement between continuous pose, the degree of overlapping between continuous pose is big.

Additionally, the velocity estimation based on current pose and nearest FL pose relative motion also uses (employing noise model) in the prediction of wave filter walks.This velocity estimation is more accurate than using nearest AT pose, it is possible to make acceleration and turning that filter process is big.

2.3 arest neighbors coupling (ICP)

Some further embodiment also includes the step S1106 result to above-mentioned Kalman filtering and carries out arest neighbors coupling.ICP is a simple algorithm, to some cloud its two groups unknown.In general thought, each iteration of algorithm has two steps.First step, associate at first and concentrate the closest approach that each point is concentrated to second point to find match point.Second step, finds coordinate transform, reduces the error between match point.The two step repeats until convergence or maximum iterations.

The false code of iterative algorithm is as follows:

Wherein S_aAnd S_bIt is laser scanning data, T_a ^bIt is the relative Coordinate Conversion in laser scanning center, χ_a ^bIt is two scanning element collection corresponding point.∑_abIt it is the covariance of final calibration.

2.3.1 surface normal

In different scanning, surface is not be sampled identical point accurately, the Euclidean distance between point be not sweep span from mechanism of properly evaluating.In a preferred embodiment, calculating every pair of match point difference in normal direction by alignment error computing formula, described difference is designated as matching error.Specifically, it is possible to use a more accurate surface error is expressed: the component of vector is to organize the deviation (as shown in Figure 2) scanning element surface normal direction from current scan point to another.So do not need point interpolation, also reduce the deviation that heterogeneous surface produces.By experience, it has therefore proved that use this error mechanism to improve the convergence rate of ICP algorithm.Normal direction can also be used in association step by restricting the match point with similar normal orientation.Due to effective match surface rather than point, this algorithm is named some arest neighbors iteration (ICP) with it, not as crying surface arest neighbors iteration (ICS).

The normal direction on the surface of each scanning element is similar to and o'clock obtains from a surface up-sampling.Given continuous print point A, B and C, the normal direction of some B is line segmentWithThe meansigma methods (as shown in Figure 3) of normal direction.Continuity hypothesis is false at the place of blocking or scanning boundary.Therefore, if the distance of fruit dot A and some B or some B and some C is more than a threshold value, only short-term is used for determining normal direction.If two distances are all very big, such as detecting a tubule, the normal direction of this point is simply calculated as this point and points to the direction of scanning center.

2.3.2 the renewal of transfer matrix

In each ICP iteration, after scanning of a surface Point matching completes, the transfer matrix T being before directed at_a ^bIt is updated.Renewal process is to minimize the current alignment error associating point set.Alignment error formula calculates each match point difference in normal orientation

$E_{align}=\underset{(a,b)\∈{\mathrm{\μ}}_{ab}}{\Σ}{\left(n_a^T\right(p_a-T_a^b{p}_b\left)\right)}^2---\left(2\right)$

P_aAnd P_bIt is surface sweeping point vector, n_aIt is the normal vector of an a, T_a ^bIt is in alignment with transfer matrix, E_alignBe in alignment with error and.This measurement mechanism is similar to the method for Lu and Milios.

Target determines that transfer matrix so that alignment error is minimum.Due to rotation amount, optimal transformation does not have linear solution.On the contrary, error equation is linearized near the initial estimation of transfer matrix can be in the hope of relative to, the linear solution of such a optimum.Alignment error can be written as the form of vector:

$E_{align}=h{\left(T_a^b\right)}^{T}h\left(T_a^b\right)---\left(3\right)$

Every a line of vector equation h () is in equation (2) summation.Linearizing error can be expressed as first order Taylor exhibition formula:

$h\left(T_a^b\right)\≈h\left(T_{a0}^b\right)+H\left[\begin{array}{c}dx\\ dy\\ d\mathrm{\θ}\end{array}\right]---\left(4\right)$

H is the h Jacobian matrix about x, y and θ.

Asking equation (3) about the differential of unknown quantity (x, y, θ), what utilize in formula (4) is approximate, and to make expression formula be zero, it is possible to be transformed into a linear system, it is possible to try to achieve optimum linearity solution:

$0=2H^T(H\left[\begin{array}{c}hx\\ dy\\ \mathrm{\ω}\end{array}\right]+h(T_{a0}^b\left)\right)$

$\left[\begin{array}{c}dx\\ dy\\ d\mathrm{\θ}\end{array}\right]=-{\left(H^{T}H\right)}^{-1}{H}^{T}h\left(T_{a0}^b\right)---\left(5\right)$

Each behavior of H

$H_i={\left[\begin{array}{c}-n_x\\ -n_y\\ {\left[\begin{array}{c}{n}_x\\ n_y\end{array}\right]}^T\left[\begin{array}{cc}\sin \mathrm{\θ}& \cos \mathrm{\θ}\\ -\cos \mathrm{\θ}& \sin \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{p}_{bx}\\ p_{by}\end{array}\right]\end{array}\right]}^T$

To more be newly added in transformation matrix parameter, and linearization procedure iteration will be until restraining.So far the coordinate transform of matrix is completed.It should be noted that this solves each correspondence and only comprises a constraint, and put correspondence and comprise two constraints.This difference prevents the deviation in scanning element location.

2.3.3 the exterior point weight of Shandong nation

Further carry out in example, also carry out step, described matching error is carried out Lorentz correction；

In order to alleviate the impact of (Non-overlapping Domain of scanning, mobile object and/or ground point) exterior point in data, matching error utilizes Lorentzian to revise, and reduces greatly weight when mistake becomes.Lorentzian weight is equal to hypothesis Cauchy error distribution.

Alignment error equation is corrected for Lorentzian equation:

$\mathrm{\ρ}\left(x\right)=\log ({\stackrel{\‾}{r}}^2+x^2)---\left(6\right)$

$E_{robust}=\underset{i=1}{\overset{N}{\Σ}}\mathrm{\ρ}\left(h_i\right(T_a^b\left)\right)---\left(7\right)$

It is defined as exterior point soft-threshold.

The parameter minimizing error can be zero by asking the differential of error equation assigned result.Equation (7) and (3) is used to only difference is that each row is added by LorentzianEach item can regard the weight based on initial error each surface corresponding point as.Weight can form a diagonal matrix W, and the minimum variance solution of Weight is:

$0=H^{T}W(H\left[\begin{array}{c}dx\\ dy\\ d\mathrm{\θ}\end{array}\right]+h(T_{a0}^b\left)\right)$

$\left[\begin{array}{c}dx\\ dy\\ d\mathrm{\θ}\end{array}\right]=-{\left(H^{T}WH\right)}^{-1}{H}^{T}Wh\left(T_{a0}^b\right)$

After convergence, the final weight of each corresponding point is used for initializing weight in scan matching subsequently, and total weight of many couplings is used to determine the exterior point in environment, and these exterior points will be left in the basket in process subsequently.

2.3.4 alignment transfer covariance

Further, the variance by matching error and the matching error Jacobian matrix relative to pose transformation parameter is further comprised the steps of: for generating the covariance leading matched transform recently.When the renewal of last ICS transfer matrix, it is determined that the covariance of scan matching conversion.The corresponding variance being used to determine equalization point error with alignment error in final surface.Then, the variance of point tolerance and registration error are used for generating the covariance of alignment transfer matrix relative to the Jacobian matrix of transfer transformation parameter.

The variance of surface point matching errorFor the sample variance of sum term in formula (2):

${\mathrm{\σ}}_s^2=\frac{1}{N-1}\underset{i=1}{\overset{N}{\Σ}}{h}_i{\left(T_a^b\right)}^2=\frac{1}{N-1}{E}_{align}---\left(9\right)$

N is the match point quantity on surface.

Transfer matrix parameter [xy θ]^TThe covariance that covariance is (formula (5)) final updating, it is assumed that $E\[{\mathrm{hh}}^T\]={\mathrm{\σ}}_S^{2}I:$

$\begin{array}{c}{\Σ}_{ab}=E\left[\begin{array}{cc}\left[\begin{array}{c}dx\\ dy\\ d\mathrm{\θ}\end{array}\right]& \left[\begin{array}{ccc}dx& dy& d\mathrm{\θ}\end{array}\right]\end{array}\right]\\ ={\left(H^{T}H\right)}^{-1}{H}^{T}E\left({\mathrm{hh}}^T\right)H{\left(H^{T}H\right)}^{-1}\\ ={\mathrm{\σ}}_s^2{\left(H^{T}H\right)}^{-1}\end{array}---\left(10\right)$

Covariance matrix is used for describing the measurement noise variance when formula (1) updates PSKF.The covariance that transfer matrix updates depends on the scanning element quantity of coupling and the mean error of each coupling and the geometry of scanning.Such as, when the normal direction of two-dimensional scan point can not support a two-dimensional space, covariance is ill-condition matrix.This situation is obvious especially in long corridor environment, now only has two parallel planar walls and is observed.In this case, Kalman filter can not reduce its uncertainty along the direction in corridor.

3. map match

Last point describes, after when one, initial value gives effectively, and the registration between laser scanning, and based on the method that laser registration sets up local controlled map (a series of laser scanning of registration).This part considers (local between map) related question under a bigger yardstick.This contributes to detection closed loop and iterates through from different directions with vicinal judgement.Being usually present a bigger uncertainty between the initial matching between map, map not mates under many circumstances.Accordingly, it would be desirable to extra step determines whether map has overlap, and overlap is determined an initial conjecture.This initial conjecture can be supplied to ICS algorithm and ensure to rapidly converge to global minima.Proposing a kind of global map closed loop matching process in some embodiment shown in Figure 13, including step S1300, obtain local map, described local map comprises local coordinate system information and the point map cloud information scanned；Described local map can be obtained by the mode described in second section, it is also possible to extracts from specific chart database partly, does not all affect the realization of this method,

3.1 Histogram Matchings

In order to quickly to its two width map, every width local map needs that the feature of its significance has a compact expression.This expression can be used to distinguish different local maps, and if the transfer matrix that can be used in determining between map have overlap.In the present embodiment, also include step S1302 and obtain direction histogram according to the normal direction distribution frequency at the point map cloud midpoint scanned；

The point map cloud scanned is projected to from discrete weighted direction according to rectangular projection and obtains projection histogram line by step S1304；

The concrete construction method of direction histogram introduced below and projection histogram.Our map match expresses the projection histogram of direction histogram and a series of Weight comprising a scanning normal direction, projection histogram by rectangular projection by scanning element from discrete direction projection to line.Strong association between rectangular histogram can be used to derive potential map match: first direction histogram may be used to determine offset, is then associated with by projection histogram on required direction and determines translational component.In order to improve the quality of coupling and this method is generalized to non-structured outdoor environment, the technology of some enhancings can be added in existing histogram matching.

When being left out translational component, direction histogram is used for calculating the rotation amount between the map of local.In order to set up rectangular histogram, unit circle is discretized as series of identical magnitude unit, and the frequency (as shown in Figure 4) of each unit surface normal in map represents the value of that unit corresponding in rectangular histogram.The peak value of direction histogram represents main surface direction, the most significantly ought have large-area flat surface in laser video Yezhong.Generally, histogrammic cell size should with map in scanning noise and definitiveness match.Experience is an angle part is that 5.625 degree (360 degree/64) are best suitable for industrial building, city, the street in suburb, and residential block.One map example and its direction histogram are as shown in Figure 5.

The projection histogram of Weight is used for determining the translational component between the map of local, once their rotational component is determined.Each projection histogram H_m(θ_p, generation d) is by each scanning element (x in rectangular chart m_i,y_i) it is θ to an inclination angle_pStraight line on, produce a skew for d_iRectangular histogram relative to the center d of unit, the normal direction on their surface of dot product

d_i=x_icosθ_p+y_isinθ_p,

$H_m({\mathrm{\&theta;}}_p,d)=\underset{\left|\right|d_i-d\left|\right|<\frac{\mathrm{\&Delta;}}{2}}{\&Sigma;}{n}_{x_i}{\mathrm{cos\&theta;}}_p+n_{y_i}{\mathrm{sin\&theta;}}_p$

Δ is histogrammic cell size.

The dynamic range of projection histogram is by each point its normal vector of weighting.The weight of the point that surface direction is parallel to projection line is weakened, and therefore rectangular histogram will not be produced fuzzy by they.Surface is perpendicular to the point of projection line and is endowed bigger weight.Additionally, due to weight can be negative, the contribution distinguishing scanning element according to the direction of scanning element normal direction is feasible (such as Fig. 5).Which increase dynamic range and the significance of projection histogram.When projection line is parallel to metope, the accumulation contribution of long wall will not desalinate fine structure.Otherwise, the metope being perpendicular to projection line will not match with the metope of equidirectional.

The direction of each projection line and the final quantity producing projection histogram are determined by the quantity of angular unit in direction histogram.For projection histogram, the size of offset units should be sufficiently small so that the CONSTRUCTED SPECIFICATION in environment can capture, but can not be so small that and be account for dominating in unit by noise.Empirically, the size of unit environment above-mentioned in 1m is to literary composition is suitable for.

The projection histogram of one complete Weight is as shown in Figure 6.Every string in figure represent one to its in angular histogram unit along special angle θ_pProjection histogram.

3.2 rectangular histogram dependencys

In further embodiment, also carry out step step S1306 and calculate rectangular histogram dependency, the first local map and second locally map that Rapid matching is close.The purpose of Histogram Matching algorithm is quickly to determine whether that local map is to matching, if it has, the transfer matrix calculated between coupling pair.The angle that general idea is that of this method is determined between two width local maps offsets by the histogrammic dependency of calculated direction.Then, the given angle skew calculated, the position offset being perpendicular to deviation angle direction can be determined by the dependency of calculating projection histogram.

Will seeing, determining that the Histogram Matching of angle is highly suitable for structurized environment, this environment can obtain significant result for ground smooth in map in direction histogram.For unstructured moving grids, it is necessary to technology, an introduction in saving 3.3 more reliably.

3.2.1 direction histogram dependency

This trifle and next little successively described embodiment also include step S1308 and calculates offset according to the direction histogram that the first local map and the second local map are corresponding；

Translational offsets amount is calculated according to the projection histogram that the first local map and the second local map are corresponding；

S1310 is according to offset and translation side-play amount synthesis the first local map and second locally map；

Repeat the above steps, until global map builds complete.

Specifically, the first step of map match is to calculate the histogrammic dependency of direction in map-making to determine possible rotational offset.In order to hide the impact on border, dependency by justifying convolutional calculation, rectangular histogram normalization in its not Luo Beini crow this norm (Frobeniusnorms), now histogrammic auto-correlation will obtain maximum is 1.The position at dependency peak represents the estimated value of rotational offset between map.Due to noise, imperfect overlap, and periodically, other local maximum can also indicate offset veritably；Therefore, calculating subsequently will consider multi-peak.

3.2.2 projection histogram dependency

The peak value of direction histogram dependency imply that the candidate value of two width local one angular registration of map；Therefore, to each candidate offset amount, the offset given with each peak value, for projection line, calculates the projection histogram dependency of two width local maps.Further, the rotational offset θ of a given candidate_oIf the angle of projection line is θ in the first width map_p, the projection line angle in the second width map is θ_p+θ_o, it is such as H for map a projection histogram_a(θ_p, d), the projection histogram of corresponding map b is H_b(θ_p+θ_o,d)。

Translational movement computational methods as follows.From the first width local map, (projection straight line have rotated θ with the second width map to select two vertical projective histogram (needing two is to solve amount of alignment in the two directions)_o) in corresponding rectangular histogram do correlation operation calculate translational movement.(generally corresponding is that in direction histogram, peak is maximum) that can be preferably selected in two projection histograms entropy by rule of thumb minimum, another is its amount of quadrature, because this selection improves the reliability of coupling.The peak value of projection histogram dependency represents the translational offsets amount estimated when a given candidate angle side-play amount.Shown in the example of projection histogram dependency such as Figure 15 (d) and (e).Offset for calculating dependency is based on the peak value in Figure 15 (c).

Between map coordinates system, translation vector obtains t by solving linear system_xAnd t_y:

$\left(\begin{array}{cc}{\mathrm{cos\&theta;}}_p& -{\mathrm{sin\&theta;}}_p\\ {\mathrm{sin\&theta;}}_p& {\mathrm{cos\&theta;}}_p\end{array}\right)\left(\begin{array}{c}{t}_x\\ t_y\end{array}\right)=\left(\begin{array}{c}{d}_{{\mathrm{\&theta;}}_p}\\ d_{{\mathrm{\&theta;}}_p+\frac{\mathrm{\&pi;}}{2}}\end{array}\right)$

θ_pIt is first projection line angle relative to x-axis,It is the side-play amount of first projection histogram correlation maximum peak value,Obtained by second projection histogram correlation calculations.The precision of transfer matrix depends on the size of histogram unit.Although registration is very coarse, such as Figure 15 (f), precision is enough satisfied converges on, for the iterative algorithm described in 2.3 joints, the coupling that precision is high.Figure 15 shows the coupling of the local map that two pairs obtain at time point different, discrete.A () (b) two secondary local map, point represents scanning element, and solid line represents the track of vehicle.C the entropy of the secondary map of () two is along with the change of angle, and the dependency of two secondary maps.D () (e) is used for the translational offsets amount to coupling marking and dependency graph box projection histogram and dependency graph.F two width local maps that () mates according to extreme value, the effect of fusion.

3.3 Entropy sequence

In some further embodiment, after obtaining projection histogram, also include step, try to achieve Entropy sequence according to the normalization probability distribution of projection histogram, described Entropy sequence comprises the entropy metrical information of each Angles Projections line, calculates offset according to described entropy metrical information.It can be seen that direction histogram is not very reliable for unstructured moving grids.Many typical outdoor environment scenes lack even curface and causes that concordance direction histogram lacks main peak.

Substitute accordingly, as one and use the histogrammic method in direction, use based on projection histogram H (θ_p, d) a series of measurement ε (θ of entropy_p) to calculate offset relatively reliable.Intuitively, entropy represents the histogrammic uniformity, and uniform entropy of histogram is maximum, and when rectangular histogram only has a unit non-zero, entropy is minimum.Therefore, entropy effectively measures existence and the acutance thereof at peak, and peak changes along with the angle of projection line and changes.Variance is used for describing histogrammic extensibility, and unlike entropy, it can not capture " ambiguity " (this is common in the application) in multimodal situation, and the border for map is sensitive.

For each projection histogram in a given map can normalization in probability distribution, entropy is calculated by this probability distribution.Entropy sequence comprise from each angle be θ_pThe entropy of projection line is measured:

$\hat{H}({\mathrm{\&theta;}}_p,h)=\frac{\left|H({\mathrm{\&theta;}}_p,d)\right|}{{\mathrm{\&Sigma;}}_d\left|H({\mathrm{\&theta;}}_p,d)\right|},$

$E\left({\mathrm{\&theta;}}_p\right)=-\underset{d}{\&Sigma;}\hat{H}({\mathrm{\&theta;}}_p,d)log\left(\hat{H}\right({\mathrm{\&theta;}}_p,d\left)\right)$

The absolute value of each histogram unit is needs, should be weight and is likely to be negative value.

Before coupling, first entropy becomes a big peak (passing through exponentiation), then negates, displacement, and normalization makes sequence be similar to a direction histogram:

$\mathrm{\&epsiv;}\left({\mathrm{\&theta;}}_p\right)=-\frac{2^{E\left({\mathrm{\&theta;}}_p\right)}-{\max }_{{\mathrm{\&theta;}}_p}\left(2^{E\left({\mathrm{\&theta;}}_p\right)}\right)}{\sqrt{{\&Sigma;}_{{\mathrm{\&theta;}}_p}{(2^{E\left({\mathrm{\&theta;}}_p\right)}-{\max }_{{\mathrm{\&theta;}}_p}(2^{E\left({\mathrm{\&theta;}}_p\right)}\left)\right)}^2}}$

Owing to the differential seat angle of two projection straight line is 180 degree, containing identical some distribution, the Entropy sequence repetition period is 180 degree.Therefore, each peak of Entropy sequence dependency produces two angle skews, differs 180 degree.This ambiguousness, is often solved by subsequence step, utilize when projection histogram associate, falseness skew have low accordingly.

Fig. 6 shows and contrasts the showing in structuring and unstructured moving grids of direction histogram and Entropy sequence.The map used in figure is obtained by the data in 2.4 joints, and vehicle-mounted the two laser radar has travelled 1140m.Fig. 6 (a) and Fig. 6 (b) shows example map, and a structurized environment comprises body of wall and building, and a non-structured environment comprises natural objects such as trees.

Direction histogram and the Entropy sequence of two width maps is given at Fig. 6 (c) and 6 (d).Identical peak position is had, although Entropy sequence peak is more wider for structurized map, rectangular histogram and Entropy sequence.At this moment the ordinary circumstance of structured environment, the metrical information of Entropy sequence is not so abundant.Conversely, for destructuring map, direction histogram lacks obvious peak, and has noise dominant, and Entropy sequence information is abundanter, has strong signal and peak clearly.

Shown in the auto-correlation of signal such as Fig. 6 (e) and 6 (f).Auto-correlation shows that we are expected under ideal conditions, and the coupling of map is very perfect.It will be seen that have similar autocorrelative peak value (although the peak of angular histogram is higher) for structuring map, angular histogram and Entropy sequence, for destructuring map, only Entropy sequence has peak clearly.This example illustrates rectangular histogram in unstructured moving grids and can arrive very much so that can not be mated reliably by noise, now must use Entropy sequence.

3.4 methods of exhaustion

In further embodiments, further, also include step, correlation calculations is done according to the projection histogram that the first local map and the second local map are corresponding, and record the global peak obtained in correlation calculations, the global peak value with identical offset is averaged, and arranges in order, calculate offset.

When direction histogram and Entropy sequence correlation registration, noise is excessive or do not have reliable peak, it is possible to use a kind of method of exhaustion, is based only upon projection histogram to determine side-play amount.In this approach, in piece image, all of projection histogram does correlation calculations with each projection histogram in another piece image, and records the global peak (as shown in Figure 7) in correlation calculations.The maximum tried to achieve in all rectangular histogram dependencys, is averaged the maximum with identical rotational offset, and arranges in order, with side-play amount for index, so we term it exhaustive correlated series (as shown in Figure 8).Local peaking in this sequence determines candidate's rotational offset (be similar in Fig. 9 rectangular histogram relevant with Entropy sequence in peak) of two width images.Method described by 3.2 parts can determine translational movement.

The method of exhaustion calculates complicated than direction histogram or Entropy sequence correlation technique, is therefore regarded as one's last shift, when other two kinds of methods do not find the coupling with quality.When being directly realized by, the computation complexity of enumerative technique is O (n⁴), and traditional correlation technique time complexity is O (n²). but, correlation calculations can at Fourier product calculation, and therefore the time complexity of enumerative technique can be optimized for O (n³Logn). main amount of calculation is calculate the inverse transformation of fast Fourier transform (FFT), because the FFTs of each projection histogram only needs to calculate once.In reality, it is possible to find that the method for exhaustion seldom needs to use, the therefore overall time of running will not be subject to very big impact.

3.5 matching measurements

Other further embodiments, also include step, determine the optimal threshold of tolerance according to the peak value in rectangular histogram dependency or signal to noise ratio.Our map-matching algorithm can calculate the transfer amount between two width maps, but we only focus on the coupling with high confidence level, and such coupling map overlapping accuracy is high.Although the coupling of mistake is generally excluded in ICS step, but in restriction map matching process, the quantity of erroneous matching can save amount of calculation.Therefore, it is necessary to the quality of match that assessment is from rectangular histogram or Entropy sequence correlation calculations.Quality of match can have many degrees metering method, such as utilizes the peak value in four dependencys (direction histogram, Entropy sequence, and two projection histograms), or utilizes the signal to noise ratio of dependency sequence.These values can be combined into a quality metric method, as by summation or connection multiplication.Signal to noise ratio (SNR) can be defined as:

$SNR(a,b)=\frac{min{\left(max\right(a/\left|\right|a\left|\right|),max((b/|\left|b\right|\left|\right)\left)\right)}^2}{2(1-\mathrm{\&eta;}(a,b))},$

Wherein, (a b) is the cross-correlation of signal a and b to η.

By calculating recipient's performance characteristic (ROC) curve (such as Figure 10) of mass data atlas, it is known that the dependency of these atlases, four different measures are distinguished correct matching capacity and is estimated.For specific test or tolerance, ROC curve is a probability curve with detection changes of threshold, the probability of detection false-alarm.Here the tolerance considered be correlation peak and, the product of correlation peak, sum of signal to noise ratio, and the product of signal to noise ratio (for each situation, sue for peace or product includes direction histogram, Entropy sequence, the dependency of the projection histogram pair of selection).ROC curve is also used to determine the optimum threshold value of tolerance, and this causes high detection probability, P (D), and low false-alarm probability, P (FA).The tolerance with optimum performance is cross-correlation summation (such as Figure 10 (a)).Following work will study other tolerance, it is contemplated that to the significant property of peak response.

For the high-quality tolerance of acceptable, its threshold value selects to be by rule of thumb.To each threshold value, the estimation of detection probability P (D) and false-alarm probability P (FA), by analyzing the data set in a large number with quality metric, when known correct and erroneous matching, as shown in Figure 10.Using these probability equations, we select the threshold value of the coefficient summation measure of crosscorrelation to be 3.4, and its detection probability is 0.51, and false alarm rate is lower than 0.01.In reality, so low verification and measurement ratio is acceptable, because losing the probability of multiple adjacent map match or at a fairly low, ((1-P (D))ⁿ).It is generally desirable to restriction false alarm frequency, by the functional cost analyzing the calculation cost performing extra scan matching with lack closed loop detection.Another strategy is based on the quality of matching measurement, keeps a priority sequence mated, in order to for further consideration, then, by a kind of at any time in the way of process potential coupling, based on available calculating resource.

All utilizations have the transfer amount that quality metric calculates, and mate registration by iterative scans subsequently and test (and improvement).Additionally, the coupling of any mistake come from truly ambiguous environment, utilize cycle proof procedure elimination (as described in Part IV).

In some embodiment shown in Figure 12, for one local map construction device, set up module 1200, snapshot logging modle 1202, filtration module 1204 and arest neighbors matching module 1206 including coordinate,

Described coordinate sets up module 1200 for setting up local coordinate system；

Record posture information when described snapshot logging modle 1202 is for each snapshot, described snapshot is laser scanning cartographic information, and described pose includes relative local coordinate system side-play amount and deflection；

Described filtration module 1204 is for carrying out Kalman filtering based on described snapshot and posture information；

Described arest neighbors matching module 1206 is for carrying out arest neighbors coupling to the result of above-mentioned Kalman filtering: mates the closest approach that first each point concentrated is concentrated to second point, calculating coordinate change, reduces the error between match point.

In some specific embodiment,

Described filtration module 1204 is for the Kalman filtering based on absolute pose and the Kalman filtering based on permanent delay pose；

In other specific embodiments, described arest neighbors matching module 1206 is specifically additionally operable to:

Calculating every pair of match point difference in normal direction by alignment error computing formula, described difference is designated as matching error,

Described matching error is carried out Lorentz correction；

It is used for generating the covariance leading matched transform recently relative to the Jacobian matrix of pose transformation parameter by the variance of matching error and matching error.

In the embodiment shown in fig. 14, for a kind of global map closed loop coalignment module map, including map acquisition module 1400, histogram calculation module 1402, correlation calculations module 1404, side-play amount computing module 1406, synthesis module 1408,

Described map acquisition module 1400 is used for obtaining local map, and described local map comprises local coordinate system information and the point map cloud information scanned；

Described histogram calculation module 1402 is for obtaining direction histogram according to the normal direction distribution frequency at the point map cloud midpoint scanned；

Described histogram calculation module 1402 is additionally operable to be projected to from discrete weighted direction by the point map cloud scanned according to rectangular projection obtain projection histogram line；

Described correlation calculations module 1404 is used for calculating rectangular histogram dependency, the first local map and second locally map that Rapid matching is close；

Described side-play amount computing module 1406 calculates offset for the direction histogram corresponding according to the first local map and the second local map；Translational offsets amount is calculated according to the projection histogram that the first local map and the second local map are corresponding；

Described synthesis module 1408 for synthesizing the first local map and the second local map according to offset and translation side-play amount.

Further carry out in example, also include Entropy sequence computing module 1410,

Described Entropy sequence computing module 1410 tries to achieve Entropy sequence for the normalization probability distribution according to projection histogram, and described Entropy sequence comprises the entropy metrical information of each Angles Projections line,

Described side-play amount computing module 1406 is additionally operable to calculate offset according to described entropy metrical information.

Further carry out in example, also include exhaustive computations module 1412, described exhaustive computations module does correlation calculations for the projection histogram corresponding according to the first local map and the second local map, and record the global peak obtained in correlation calculations, the global peak value with identical offset is averaged, and arrange in order, calculate offset.

Further carry out in example, also include optimal threshold and determine that module 1414, described optimal threshold determine that module for determining the optimal threshold of tolerance according to the peak value in rectangular histogram dependency or signal to noise ratio.

It should be noted that, in this article, the relational terms of such as first and second or the like is used merely to separate an entity or operation with another entity or operating space, and not necessarily requires or imply the relation that there is any this reality between these entities or operation or sequentially.And, term " includes ", " comprising " or its any other variant are intended to comprising of nonexcludability, so that include the process of a series of key element, method, article or terminal unit not only include those key elements, but also include other key elements being not expressly set out, or also include the key element intrinsic for this process, method, article or terminal unit.When there is no more restriction, statement " including ... " or " comprising ... " key element limited, it is not excluded that there is also other key element in including the process of described key element, method, article or terminal unit.Additionally, in this article, " more than ", " less than ", " exceeding " etc. be interpreted as not including this number；" more than ", " below ", " within " etc. be interpreted as including this number.

Those skilled in the art are it should be appreciated that the various embodiments described above can be provided as method, device or computer program.These embodiments can adopt the form of complete hardware embodiment, complete software implementation or the embodiment in conjunction with software and hardware aspect.All or part of step in the method that the various embodiments described above relate to can be completed by the hardware that program carrys out instruction relevant, 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, general 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 are that flow chart and/or block diagram with reference to the method according to embodiment, equipment (system) and computer program describe.It should be understood that can by the combination of the flow process in each flow process in computer program instructions flowchart and/or block diagram and/or square frame and flow chart and/or block diagram and/or square frame.These computer program instructions can be provided to produce a machine to the processor of computer equipment so that the instruction performed by the processor of computer equipment is produced for realizing the device of function specified in one flow process of flow chart or multiple flow process and/or one square frame of block diagram or multiple square frame.

These computer program instructions may be alternatively stored in the computer equipment readable memory that computer equipment can be guided to work in a specific way, the instruction making to be stored in this computer equipment readable memory produces to include the manufacture of command device, and this command device realizes the function specified in one flow process of flow chart or multiple flow process and/or one square frame of block diagram 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 that performs on a computing device provides for realizing the step of function specified in one flow process of flow chart or multiple flow process and/or one square frame of block diagram or multiple square frame.

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

Claims (6)

1. a local map constructing method, it is characterised in that comprise the steps,

Set up local coordinate system；

Recording posture information during each snapshot, described snapshot is laser scanning cartographic information, and described pose includes relative local coordinate system side-play amount and deflection；

Kalman filtering is carried out based on described snapshot and posture information；

The result of above-mentioned Kalman filtering is carried out arest neighbors coupling: mate the closest approach that first each point concentrated is concentrated to second point, calculating coordinate change, reduce the error between match point.

2. local according to claim 1 map constructing method, it is characterised in that described " carrying out Kalman filtering based on described snapshot and posture information " including:

Kalman filtering based on absolute pose and the Kalman filtering based on permanent delay pose.

3. local according to claim 1 map constructing method, it is characterised in that described calculating coordinate change, the error reduced between match point specifically includes step:

Calculating every pair of match point difference in normal direction by alignment error computing formula, described difference is designated as matching error,

Described matching error is carried out Lorentz correction；

It is used for generating the covariance leading matched transform recently relative to the Jacobian matrix of pose transformation parameter by the variance of matching error and matching error.

4. a local map construction device, it is characterised in that include coordinate and set up module, snapshot logging modle, filtration module and arest neighbors matching module,

Described coordinate sets up module for setting up local coordinate system；

Record posture information when described snapshot logging modle is for each snapshot, described snapshot is laser scanning cartographic information, and described pose includes relative local coordinate system side-play amount and deflection；

Described filtration module is for carrying out Kalman filtering based on described snapshot and posture information；

Described arest neighbors matching module is for carrying out arest neighbors coupling to the result of above-mentioned Kalman filtering: mates the closest approach that first each point concentrated is concentrated to second point, calculating coordinate change, reduces the error between match point.

5. local according to claim 4 map construction device, it is characterised in that

Described filtration module is used for the Kalman filtering based on absolute pose and the Kalman filtering based on permanent delay pose.

6. local according to claim 4 map construction device, it is characterised in that described arest neighbors matching module is specifically additionally operable to:

Calculating every pair of match point difference in normal direction by alignment error computing formula, described difference is designated as matching error,

Described matching error is carried out Lorentz correction；

It is used for generating the covariance leading matched transform recently relative to the Jacobian matrix of pose transformation parameter by the variance of matching error and matching error.