CN103604426A - Estimation method and apparatus for poses of mobile robot - Google Patents

Abstract

The invention provides an estimation method for the poses of a mobile robot. The method comprises the following steps carrying out scan preprocessing on data corresponding to a current scanning position and data corresponding to a reference scanning position so as to obtain to-be-processed current scanning data and to-be-processed reference scanning data; converting the current scanning position into a to-be-processed current scanning position in a polar coordinate system; converting data corresponding to the to-be-processed current scanning position in the polar coordinate system into data in a to-be-processed reference scanning data coordinate system and generating to-be-matched current scanning data; estimating a translation distance deviation value and a rotation angle deviation value of the current scanning position relative to the reference scanning position; and correcting data corresponding to the current scanning position by using the translation distance deviation value and the rotation angle deviation value and generating the estimated pose of the current scanning position. Thus, the estimation method for the poses of the mobile robot in the invention can meet the requirement of instantaneity of pose estimation.

Description

A kind of mobile robot's position and orientation estimation method and device

Technical field

The application relates to Robot Design field, particularly a kind of mobile robot's position and orientation estimation method and device.

Background technology

Along with the fast development of robot, mobile robot applies and gives birth to.Mobile robot's pose is estimated as priority research areas.

At present, conventionally adopt local laser scan matching method to estimate mobile robot's pose.Common local laser scan matching method comprises: ICP(Iterative Closest Point, iterative closest point) algorithm and IDC(Iterative Dual Correspondence, the iteration double base connecing based on iteration minimum variance is corresponding) algorithm.

The deficiency of ICP algorithm is that speed of convergence is slower, and especially, when environment presents curve shape, its speed of convergence when approaching local minimum is very slow.And the rotational component that the some correspondence of using closest approach rule to obtain comprises is less, therefore poor to the estimation of rotational component.

IDC algorithm improves on ICP algorithm basis, in IDC algorithm, not only uses closest approach rules selection corresponding point, goes back service range matched rule and selects corresponding point, chooses point that initial point distance is identical as corresponding point.These two regular uses have guaranteed that IDC algorithm can very accurately estimate rotational component and translational component, and speed of convergence is also obviously faster than ICP algorithm.But in actual applications, the calculated amount of IDC algorithm is still larger, speed of convergence still can not meet the requirement of real-time that mobile robot's pose is estimated.

Therefore the method for at present mobile robot's pose being estimated exists speed of convergence can not meet the shortcoming of the requirement of real-time of mobile robot's pose estimation.

Summary of the invention

For solving the problems of the technologies described above, the embodiment of the present application provides a kind of mobile robot's position and orientation estimation method and device, and to reach the object of the requirement of real-time that meets pose estimation, technical scheme is as follows:

Mobile robot's position and orientation estimation method, comprising:

Data corresponding to data corresponding to current scanning position and reference scan position are scanned to pre-service, obtain pending current scan-data and pending reference scan data, the data that described reference scan position is corresponding and pending reference scan data are the data under polar coordinate system;

Described current scanning position is converted to the pending current scanning position under polar coordinate system;

Described data corresponding to pending current scanning position under polar coordinate system are converted to the data under described pending reference scan data coordinate system, generate current scan-data to be matched;

According to described pending reference scan data and described current scan-data to be matched, estimate that current scanning position is with respect to translation distance deviate and the anglec of rotation deviate of reference scan position;

Use described translation distance deviate and described anglec of rotation deviate, data corresponding to described current scanning position are revised, generate the estimation pose of described current scanning position.

Preferably, the described pending reference scan data of described foundation and described current scan-data to be matched, estimate that current scanning position, with respect to the process of the translation distance deviate of reference scan position, comprising:

According to default polar coordinates deflection matched rule, in described pending reference scan data, periodically search for the to be matched scanning position corresponding with described current scanning position to be matched, according to corresponding data and current scan-data to be matched and the formula of scanning position to be matched searching at every turn calculate utmost point footpath residual sum of squares (RSS), wherein, described E (x _c, y _c) be utmost point footpath residual sum of squares (RSS), described w _ibe i the weight factor affecting for reducing bad coupling that scan-data is corresponding, described r _ribe i the included utmost point footpath of pending reference scan data, described r " _cibe i the included utmost point footpath of current scan-data to be matched, described r " _ciequal $\sqrt{{(r_{\mathrm{ci}}\cos ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+x_c)}^2+{(r_{\mathrm{ci}}\sin ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+y_c)}^2},$ Described x _c, y _c, θ _cform (x _c, y _c, θ _c) pose as current scanning position under current scan coordinate system, described φ _cifor polar angle, described r _cifor at polar angle φ _cion utmost point footpath, described r _ri-r " _ciequal linear formula ${\mathrm{\Δr}}_i\≈\frac{{\∂r}_{\mathrm{ci}}^{\′\′}}{{\∂x}_c}{\mathrm{\Δx}}_c+\frac{{\∂r}_{\mathrm{ci}}^{\′\′}}{{\∂y}_c}{\mathrm{\Δy}}_c=\cos \left({\mathrm{\φ}}_{\mathrm{ri}}\right){\mathrm{\Δx}}_c+\sin \left({\mathrm{\φ}}_{\mathrm{ri}}\right){\mathrm{\Δy}}_c,$ Described Δ x _cwith Δ y _cfor the translation distance deviate of current scanning position with respect to reference scan position, described neff is the number of the data that current scanning position is corresponding, described in for summing function;

Obtain data r ' corresponding to corresponding scanning position to be matched in the situation that of the residual sum of squares (RSS) minimum of described utmost point footpath _ri;

Calculate in the situation of described utmost point footpath residual sum of squares (RSS) minimum, the Jacobian matrix J of described utmost point footpath residual sum of squares (RSS), described J equals $\left[\begin{array}{cc}\frac{{\∂r}_{c1}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c1}^{\′\′}}{{\∂y}_c}\\ \frac{{\∂r}_{c2}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c2}^{\′\′}}{{\∂x}_c}\\ \·\·\·& \·\·\·\end{array}\right];$

Calculate weight diagonal matrix W corresponding in the situation of described utmost point footpath residual sum of squares (RSS) minimum, described w _ifor the element in described W, described w _iequal

described d _iequal the included utmost point footpath r of current scan-data to be matched " _cideduct described r ' _ridifference, described c is constant, how definition weight from 1 to 0 changes, described m determines that how weighting function changes fast;

By weighted least-squares method, according to data to be matched and formula corresponding in the situation that of the residual sum of squares (RSS) minimum of described utmost point footpath $\left[\begin{array}{c}{\mathrm{\Δx}}_c\\ {\mathrm{\Δy}}_c\end{array}\right]={\left(J^T\mathrm{WJ}\right)}^{-1}{J}^{T}W(r_c^{\′\′}-r_r),$ Calculate the translation distance deviate of current scanning position and reference scan position, described r " _cfor r " _civector, described r _rfor r _rivector.

Preferably, the described pending reference scan data of described foundation and described current scan-data to be matched, estimate that current scanning position, with respect to the process of the anglec of rotation deviate of reference scan position, comprising:

In the included polar angle adjacent area of described current scan-data to be matched, search for the utmost point footpath identical scan-data corresponding scanning position included with described current scan-data to be matched, and from described scanning position, select default number and the nearest scanning position of described current scanning position to be matched;

By described default number and nearest each the self-corresponding utmost point footpath residual error absolute average of scanning position of described current scanning position to be matched, parabola of fit equation, calculates minimum absolute average error;

By the horizontal ordinate of described minimum absolute average error, estimate anglec of rotation deviate.

Preferably, describedly data corresponding to data corresponding to current scanning position and reference scan position scanned to pretreated process comprise:

Data corresponding to data corresponding to current scanning position and reference scan position are removed to noise, about subtrahend certificate.

Preferably, described data corresponding to data corresponding to current scanning position and reference scan position are scanned to pretreated process, comprising:

Data corresponding to data corresponding to current scanning position and reference scan position are removed to noise, about subtrahend certificate, medium filtering and data block are cut apart.

Mobile robot's pose estimation unit, comprising:

The first pretreatment unit, for data corresponding to data corresponding to current scanning position and reference scan position are scanned to pre-service, obtain pending current scan-data and pending reference scan data, the data that described reference scan position is corresponding and pending reference scan data are the data under polar coordinate system;

The first converting unit, for being converted to the pending current scanning position under polar coordinate system by described current scanning position;

The second converting unit, for described data corresponding to pending current scanning position under polar coordinate system are converted to the data under described pending reference scan data coordinate system, generates current scan-data to be matched;

The first estimation unit, for according to described pending reference scan data and described current scan-data to be matched, estimates that current scanning position is with respect to translation distance deviate and the anglec of rotation deviate of reference scan position;

Amending unit, for using described translation distance deviate and described anglec of rotation deviate, revises data corresponding to described current scanning position, generates the estimation pose of described current scanning position.

Preferably, described the first estimation unit comprises:

The first computing unit, for the default polar coordinates deflection matched rule of foundation, in described pending reference scan data, periodically search for the to be matched scanning position corresponding with described current scanning position to be matched, according to corresponding data and current scan-data to be matched and the formula of scanning position to be matched searching at every turn

calculate utmost point footpath residual sum of squares (RSS), wherein, described E (x _c, y _c) be utmost point footpath residual sum of squares (RSS), described w _ibe i the weight factor affecting for reducing bad coupling that scan-data is corresponding, described r _ribe i the included utmost point footpath of pending reference scan data, described r " _cibe i the included utmost point footpath of current scan-data to be matched, described r " _ciequal $\sqrt{{(r_{\mathrm{ci}}\cos ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+x_c)}^2+{(r_{\mathrm{ci}}\sin ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+y_c)}^2},$ Described x _c, y _c, θ _cform (x _c, y _c, θ _c) pose as current scanning position under current scan coordinate system, described φ _cifor polar angle, described r _cifor at polar angle φ _cion utmost point footpath, described r _ri-r " _ciequal linear formula ${\mathrm{\Δr}}_i\≈\frac{{\∂r}_{\mathrm{ci}}^{\′\′}}{{\∂x}_c}{\mathrm{\Δx}}_c+\frac{{\∂r}_{\mathrm{ci}}^{\′\′}}{{\∂y}_c}{\mathrm{\Δy}}_c=\cos \left({\mathrm{\φ}}_{\mathrm{ri}}\right){\mathrm{\Δx}}_c+\sin \left({\mathrm{\φ}}_{\mathrm{ri}}\right){\mathrm{\Δy}}_c,$ Described Δ x _cwith Δ y _cfor the translation distance deviate of current scanning position with respect to reference scan position, described neff is the number of the data that current scanning position is corresponding, described in

for summing function;

Acquiring unit, for obtaining data r ' corresponding to corresponding scanning position to be matched the residual sum of squares (RSS) minimum of the described utmost point footpath in the situation that _ri;

The second computing unit, for calculating in the situation of described utmost point footpath residual sum of squares (RSS) minimum, the Jacobian matrix J of described utmost point footpath residual sum of squares (RSS), described J equals $\left[\begin{array}{cc}\frac{{\∂r}_{c1}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c1}^{\′\′}}{{\∂y}_c}\\ \frac{{\∂r}_{c2}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c2}^{\′\′}}{{\∂x}_c}\\ \·\·\·& \·\·\·\end{array}\right];$

The 3rd computing unit, for calculating weight diagonal matrix W corresponding in the situation of described utmost point footpath residual sum of squares (RSS) minimum, described w _ifor the element in described W, described w _iequal

described d _iequal the included utmost point footpath r of current scan-data to be matched " _cideduct described r ' _ridifference, described c is constant, how definition weight from 1 to 0 changes, described m determines that how weighting function changes fast;

The 4th computing unit, for passing through weighted least-squares method, according to data to be matched and formula corresponding in the situation that of the residual sum of squares (RSS) minimum of described utmost point footpath $\left[\begin{array}{c}{\mathrm{\Δx}}_c\\ {\mathrm{\Δy}}_c\end{array}\right]={\left(J^T\mathrm{WJ}\right)}^{-1}{J}^{T}W(r_c^{\′\′}-r_r),$ Calculate the translation distance deviate of current scanning position and reference scan position, described r " _cfor r " _civector, described r _rfor r _rivector.

Preferably, described the first estimation unit comprises:

Search unit, for in the included polar angle adjacent area of described current scan-data to be matched, search for the utmost point footpath identical scan-data corresponding scanning position included with described current scan-data to be matched, and from described scanning position, select default number and the nearest scanning position of described current scanning position to be matched;

The 5th computing unit, for by described default number and nearest each the self-corresponding utmost point footpath residual error absolute average of scanning position of described current scanning position to be matched, parabola of fit equation, calculates minimum absolute average error;

The second estimation unit, for by the horizontal ordinate of described minimum absolute average error, estimates anglec of rotation deviate.

Preferably, described the first pretreatment unit comprises:

The second pretreatment unit, for removing noise to data corresponding to data corresponding to current scanning position and reference scan position, about subtrahend certificate.

Preferably, described the first pretreatment unit comprises:

The 3rd pretreatment unit, for data corresponding to data corresponding to current scanning position and reference scan position are removed to noise, about subtrahend certificate, medium filtering and data block are cut apart.

Compared with prior art, the application's beneficial effect is:

In this application, because current scan-data to be matched and pending reference scan data are all the data under polar coordinate system, so translation distance deviate and anglec of rotation deviate all obtain according to current scan-data to be matched and pending reference scan data under polar coordinate system.

Due under polar coordinate system, calculate the algorithm the convergence speed of translation distance deviate and anglec of rotation deviate faster than IDC convergence of algorithm speed, and can meet the requirement of real-time that mobile robot's pose is estimated, therefore, mobile robot's position and orientation estimation method that the application provides, can meet the requirement of real-time that pose is estimated.

Accompanying drawing explanation

In order to be illustrated more clearly in the technical scheme in the embodiment of the present application, below the accompanying drawing of required use during embodiment is described is briefly described, apparently, accompanying drawing in the following describes is only some embodiment of the application, for those of ordinary skills, do not paying under the prerequisite of creative work, can also obtain according to these accompanying drawings other accompanying drawing.

Fig. 1 is a kind of process flow diagram of a kind of mobile robot's position and orientation estimation method of providing of the application;

Fig. 2 is each coordinate system schematic diagram of robot that the application provides;

Fig. 3 is a kind of sub-process figure of a kind of mobile robot's position and orientation estimation method of providing of the application;

Fig. 4 is the another kind of sub-process figure of a kind of mobile robot's position and orientation estimation method of providing of the application;

Fig. 5 is a kind of structural representation of a kind of mobile robot's pose estimation unit of providing of the application;

Fig. 6 is a kind of structural representation of a kind of the first estimation unit of providing of the application;

Fig. 7 is the another kind of structural representation of a kind of the first estimation unit of providing of the application.

Embodiment

Below in conjunction with the accompanying drawing in the embodiment of the present application, the technical scheme in the embodiment of the present application is clearly and completely described, obviously, described embodiment is only the application's part embodiment, rather than whole embodiment.Embodiment based in the application, those of ordinary skills are not making the every other embodiment obtaining under creative work prerequisite, all belong to the scope of the application's protection.

An embodiment

Refer to Fig. 1, a kind of process flow diagram that it shows a kind of mobile robot's position and orientation estimation method that the application provides, can comprise the following steps:

Step S11: data corresponding to data corresponding to current scanning position and reference scan position are scanned to pre-service, obtain pending current scan-data and pending reference scan data, the data that described reference scan position is corresponding and pending reference scan data are the data under polar coordinate system.

In the present embodiment, reference scan position is known location, and data corresponding to reference scan position are given data.

The data that current scanning position is corresponding are obtained by odometer.The data that current scanning position is corresponding

(x wherein _c, y _c, θ _c) for produce the robot pose of current scan-data under current scan coordinate system,

be described in current scan coordinate system n at direction φ _cithe distance r of upper measurement _ci,

in direction, according to counterclockwise increasing order, arrange.Reference scan data are: ${R=\{r_{\mathrm{ri}},{\mathrm{\φ}}_{\mathrm{ri}}\}}_{i=1}^n.$

In the present embodiment, the data corresponding to current scanning position scan pre-service, obtain pending current scan-data.The data corresponding to reference scan position scan pre-service, obtain pending reference scan data.Wherein, corresponding data and the pending reference scan data in reference scan position are the data under polar coordinate system.

In the present embodiment, data corresponding to data corresponding to current scanning position and reference scan position are scanned to pretreated process can be: data corresponding to data corresponding to current scanning position and reference scan position are removed to noise, about subtrahend certificate.

Certainly, data corresponding to data corresponding to current scanning position and reference scan position are scanned to pretreated process can also be: data corresponding to data corresponding to current scanning position and reference scan position are removed to noise, about subtrahend certificate, medium filtering and data block are cut apart.

Owing to always existing some to be not suitable for the point of coupling in laser scanning data, comprise: some points that represent mobile object: for example people, chair etc., mixing point: in the discontinuous place of distance, the laser scanning and ranging instrument measurement that usually dummy section in the middle of two objects produces some measured values, ultimate range: while there is not object in the effective range at laser scanning and ranging instrument, such reading will be returned to, some body surfaces (as smooth glass surface) are because reflection can not show laser spots, and their measuring distance is also maximum measuring distance.If distance measure is greater than this value, the distance between two so adjacent analyzing spots is also very large, therefore, cannot judge whether these two adjacent analyzing spots belong to an object.

In order to improve the confidence level of each laser scanning data, adopt fast electric-wave filter method to carry out filtering to these data.In order to remove the abnormal impact of scanning position, adopt median filter that data corresponding to a certain number scanning position are replaced by the intermediate value of n point around it.In the present embodiment, the value of n can be 5.

Carry out data block while cutting apart, for each frame range data, first laser scanning position is divided into different blocks.If the distance of continuous two scanning positions is less than a threshold value, these two scanning positions belong to same block.If the distance of continuous two scanning positions is greater than a threshold value, Frame just separates from this place.Finally a frame pitch is become to several blocks from Data Segmentation.The block table of cutting apart be shown Ri (i=1 ..., Q, wherein Q is the block counts of cutting apart), each block comprises Ni scanning position.Because the distribution of scanning position is not that generally, the scanning position density close to sensor is larger, and smaller away from the scanning position density of sensor uniformly.So carry out range data while cutting apart, application self-adapting becomes threshold segmentation method.For example, when certain scanning position is D from the distance of center sensor, segmentation threshold is chosen as d, and when scanning position is 3D from the distance of center sensor, threshold value is chosen as 3d.In addition, also can select other linearity or nonlinear function to define self-adaptation segmentation threshold.In a word, at different scanning positions, select different segmentation thresholds, can be consistent with actual environment characteristic model better in the hope of the block of cutting apart of range data.If the effective range finding of laser distance is 8 meters, and angular resolution is 1 degree, and the distance minimum between adjacent scanning position is: 2 * 8m * sin (0.5 °)=14cm.According to this value, can set suitable separation threshold value.

Scanning pre-service is conducive to remove the measuring error in scan-data, to coming from the measured value of same object, carries out cluster, improves precision and the robustness of scan matching algorithm.

Step S12: described current scanning position is converted to the pending current scanning position under polar coordinate system.

In the present embodiment, because current scanning position is not the position under polar coordinate system, therefore current scanning position need to be converted to the pending current scanning position under polar coordinate system, be about under coordinate system that current scanning position is converted to place, reference scan position.Concrete transfer process can be according to formula T ₁t _lt ₃=T ₂t _l, T wherein ₁to examine the anisotropy transform matrix that coordinate is tied to global coordinate system, T from machine ginseng ₂the current coordinate of Shi Cong robot is tied to the transformation matrix of global coordinate system, T ₃from current laser scanning coordinate, to be tied to the transformation matrix of reference laser scan coordinate system, T _lfrom laser scanning coordinate, to be tied to robot coordinate system's transformation matrix.From current scanning coordinate, be tied to the T that is converted to of reference scan coordinate system ₃=T _l ^-1t ₁ ^-1t ₂t _l.Each coordinate system schematic diagram of robot can be referring to Fig. 2, and shown in Fig. 2 is each coordinate system schematic diagram of robot that the application provides.

At T ₃, T _l, T ₁, T ₂in known situation, can calculate T ₃in current pose (x under the coordinate system of place, reference scan position _c, y _c, θ _c).

Step S13: described data corresponding to pending current scanning position under polar coordinate system are converted to the data under described pending reference scan data coordinate system, generate current scan-data to be matched.

In the present embodiment, data corresponding to pending current scanning position under polar coordinate system need to be converted to the data under pending reference scan data coordinate system.

Concrete, after conversion, the utmost point footpath r that current scan-data to be matched is included " _cican be expressed as $\sqrt{{(r_{\mathrm{ci}}\cos ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+x_c)}^2+{(r_{\mathrm{ci}}\sin ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+y_c)}^2},$ The polar angle φ that current scan-data to be matched is included " _rican be expressed as atan2 (r _cisin (θ _c+ φ _ci)+y _c, r _cicos (θ _c+ φ _ci)+x _c).

After conversion, for each direction φ " _ri, at least by a r " _ciwith the r from pending reference scan data _ricorresponding.

Step S14: according to described pending reference scan data and described current scan-data to be matched, estimate that current scanning position is with respect to translation distance deviate and the anglec of rotation deviate of reference scan position.

In the present embodiment, according to pending reference scan data and the to be matched current scan-data under pending reference scan data coordinate system under polar coordinate system, estimate current scanning position with respect to the translation distance deviate of reference scan position, be respectively Δ x _cwith Δ y _c.

In the present embodiment, according to pending reference scan data and the to be matched current scan-data under pending reference scan data coordinate system under polar coordinate system, can estimate that current scanning position is with respect to the anglec of rotation deviate of reference scan position, anglec of rotation deviate can be Δ θ _c.

Step S15: use described translation distance and the described anglec of rotation, data corresponding to described current scanning position are revised, generate the estimation pose of described current scanning position.

In the present embodiment, the translation distance that translation distance deviate is corresponding with reference scan position is added, obtain the estimation translation distance of current scanning position, the anglec of rotation that anglec of rotation deviate is corresponding with reference scan position is added, obtain the estimation anglec of rotation of current scanning position, calculate and estimate translation distance and estimate that the anglec of rotation calculates the estimation pose of current scanning position.

Another embodiment

In the present embodiment, what illustrate is according to described pending reference scan data and described current scan-data to be matched, estimate the detailed process of the translation distance between current scanning position and reference scan position, refer to Fig. 3, shown in Fig. 3 is a kind of sub-process figure of a kind of mobile robot's position and orientation estimation method of providing of the application, can comprise the following steps:

Step S31: according to default polar coordinates deflection matched rule, in described pending reference scan data, periodically search for the to be matched scanning position corresponding with described current scanning position to be matched, according to corresponding data and current scan-data to be matched and the formula of scanning position to be matched searching at every turn $E(x_c,y_c)=\underset{i=1}{\overset{\mathrm{neff}}{\mathrm{\Σ}}}{w}_i{(r_{\mathrm{ri}}-r_{\mathrm{ci}}^{\′\′})}^2,$ Calculate utmost point footpath residual sum of squares (RSS).

In the present embodiment, E (x _c, y _c) be utmost point footpath residual sum of squares (RSS), described w _ibe i the weight factor affecting for reducing bad coupling that scan-data is corresponding, described r _ribe i the included utmost point footpath of pending reference scan data, described r " _cibe i the included utmost point footpath of current scan-data to be matched, described r " _ciequal $\sqrt{{(r_{\mathrm{ci}}\cos ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+x_c)}^2+{(r_{\mathrm{ci}}\sin ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+y_c)}^2},$ Described x _c, y _c, θ _cform (x _c, y _c, θ _c) pose as current scanning position under current scan coordinate system, described φ _cifor polar angle, described r _cifor at polar angle φ _cion utmost point footpath, described r _ri-r " _ciequal linear formula ${\mathrm{\Δr}}_i\≈\frac{{\∂r}_{\mathrm{ci}}^{\′\′}}{{\∂x}_c}{\mathrm{\Δx}}_c+\frac{{\∂r}_{\mathrm{ci}}^{\′\′}}{{\∂y}_c}{\mathrm{\Δy}}_c=\cos \left({\mathrm{\φ}}_{\mathrm{ri}}\right){\mathrm{\Δx}}_c+\sin \left({\mathrm{\φ}}_{\mathrm{ri}}\right){\mathrm{\Δy}}_c,$ Described Δ x _cwith Δ y _cfor the translation distance deviate of current scanning position with respect to reference scan position, described neff is the number of the data that current scanning position is corresponding, described in

for summing function.

What default polar coordinates deflection matched rule was concrete is: rotate the included polar angle φ of current scan-data to be matched " _ri, the upper search of the deflection (being polar angle) of the correspondence scanning position nearest with current scanning position to be matched after each rotation.

According to default polar coordinates deflection matched rule, in pending reference scan data, periodically search for the to be matched scanning position corresponding with current scanning position to be matched, often search a scanning position to be matched, with regard to data and current scan-data to be matched and a formula that the scanning position to be matched searching is corresponding

calculate utmost point footpath residual sum of squares (RSS), until utmost point footpath residual sum of squares (RSS) is minimum.In the present embodiment, in the situation that the value of utmost point footpath residual sum of squares (RSS) is in preset range, can judge that utmost point footpath residual sum of squares (RSS) is minimum.

In the situation of utmost point footpath residual sum of squares (RSS) minimum, illustrate between pending reference scan data and current scan-data to be matched and realized successfully coupling, determined between reference scan position and current scanning position and changed relatively accurately.

Step S32: obtain data r ' corresponding to corresponding scanning position to be matched in the situation that of the residual sum of squares (RSS) minimum of described utmost point footpath _ri.

In the situation of utmost point footpath residual sum of squares (RSS) minimum, corresponding one group of data r ' _ri, data corresponding to corresponding scanning position to be matched in the situation for utmost point footpath residual sum of squares (RSS) minimum.From pending reference scan data, obtain r ' _ri.

Step S33: calculate in the situation of described utmost point footpath residual sum of squares (RSS) minimum, the Jacobian matrix J of described utmost point footpath residual sum of squares (RSS), described J equals $\left[\begin{array}{cc}\frac{{\∂r}_{c1}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c1}^{\′\′}}{{\∂y}_c}\\ \frac{{\∂r}_{c2}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c2}^{\′\′}}{{\∂x}_c}\\ \·\·\·& \·\·\·\end{array}\right];$

Step S34: calculate weight diagonal matrix W corresponding in the situation of described utmost point footpath residual sum of squares (RSS) minimum, described w _ifor the element in described W, described w _iequal

described d _iequal the included utmost point footpath r of current scan-data to be matched " _cideduct described r ' _ridifference, described c is constant, how definition weight from 1 to 0 changes, described m determines that how weighting function changes fast.

Step S35: by weighted least-squares method, according to data to be matched and formula corresponding in the situation that of the residual sum of squares (RSS) minimum of described utmost point footpath $\left[\begin{array}{c}{\mathrm{\Δx}}_c\\ {\mathrm{\Δy}}_c\end{array}\right]={\left(J^T\mathrm{WJ}\right)}^{-1}{J}^{T}W(r_c^{\′\′}-r_r),$ Calculate the translation distance deviate of current scanning position and reference scan position, described r " _cfor r " _civector, described r _rfor r _rivector.

Another embodiment

In the present embodiment, what illustrate is according to described pending reference scan data and described current scan-data to be matched, estimate that current scanning position is with respect to the detailed process of the anglec of rotation deviate of reference scan position, refer to Fig. 4, shown in Fig. 4 is the another kind of sub-process figure of a kind of mobile robot's position and orientation estimation method of providing of the application, can comprise the following steps:

Step S41: in the included polar angle adjacent area of described current scan-data to be matched, search for the utmost point footpath identical scan-data corresponding scanning position included with described current scan-data to be matched, and from described scanning position, select default number and the nearest scanning position of described current scanning position to be matched.

Step S42: by described default number and nearest each the self-corresponding utmost point footpath residual error absolute average of scanning position of described current scanning position to be matched, parabola of fit equation, calculates minimum absolute average error.

Now illustrate parabola of fit equation, calculate the detailed process of minimum absolute average error.For example, default number is 5, and 5 are respectively (2, e with the nearest scanning position of current scanning position to be matched _-2), (1, e _-1), (0, e ₀), (1, e ₁) and (2, e ₂), e _-2, e _-1, e ₀, e ₁and e ₂be respectively utmost point footpath residual error absolute average, parabolic equation is e=at ²+ bt+c, calculates parabolic equation e=at ²the minimum absolute average error of+bt+c is 0,

Step S43: by the horizontal ordinate of described minimum absolute average error, estimate anglec of rotation deviate.

Because minimum absolute average error is 0,

therefore 2am+b=0, calculates the horizontal ordinate of minimum absolute average error

With giving an example in step S42, to by the horizontal ordinate of described minimum absolute average error, estimate that the process of anglec of rotation deviate describes.In order to obtain a, b value, brings 5 known points into parabolic equation, obtains $\left[\begin{array}{c}a\\ b\\ c\end{array}\right]=\left[\begin{array}{ccccc}1/7& -1/14& -1/7& -1/14& 1/7\\ -1/5& -1/10& 0& 1/10& 1/5\\ -3/35& 12/35& 17/35& 12/35& -3/35\end{array}\right]\left[\begin{array}{c}{e}_{-2}\\ e_{-1}\\ e_0\\ e_1\\ e_2\end{array}\right]$ And then the horizontal ordinate that obtains minimum absolute average error is: $m=-\frac{b}{2a}=\frac{{14e}_{-2}+{7e}_{-1}-{7e}_1-{14e}_2}{{20e}_{-2}-{10e}_{-1}-{20e}_0-{10e}_1+{20e}_2},$ Suppose that at the correct position angle of polar angle 0 be Δ θ ₁, the distance between polar angle 0 and polar angle 1 is Δ φ, anglec of rotation deviate is so: Δ θ _c=Δ θ ₁+ m Δ φ.

For aforesaid each embodiment of the method, for simple description, therefore it is all expressed as to a series of combination of actions, but those skilled in the art should know, the application is not subject to the restriction of described sequence of movement, because according to the application, some step can adopt other orders or carry out simultaneously.Secondly, those skilled in the art also should know, the embodiment described in instructions all belongs to preferred embodiment, and related action and module might not be that the application is necessary.

Corresponding with above-described embodiment, the application also provides a kind of mobile robot's pose estimation unit, refer to Fig. 5, shown in Fig. 5 is a kind of structural representation of a kind of mobile robot's pose estimation unit of providing of the application, and mobile robot's pose estimation unit comprises: the first pretreatment unit 51, the first converting unit 52, the second converting unit 53, the first estimation unit 54 and amending unit 55.

The first pretreatment unit 51, for data corresponding to data corresponding to current scanning position and reference scan position are scanned to pre-service, obtain pending current scan-data and pending reference scan data, the data that described reference scan position is corresponding and pending reference scan data are the data under polar coordinate system.

In the present embodiment, the first pretreatment unit 51 can comprise: the second pretreatment unit, and for data corresponding to data corresponding to current scanning position and reference scan position being removed to noise, about subtrahend certificate.

Certainly, the first pretreatment unit 51 also can comprise: the 3rd pretreatment unit, and for data corresponding to data corresponding to current scanning position and reference scan position are removed to noise, about subtrahend certificate, medium filtering and data block are cut apart.

The first converting unit 52, for being converted to the pending current scanning position under polar coordinate system by described current scanning position.

The second converting unit 53, for described data corresponding to pending current scanning position under polar coordinate system are converted to the data under described pending reference scan data coordinate system, generates current scan-data to be matched.

The first estimation unit 54, for according to described pending reference scan data and described current scan-data to be matched, estimates that current scanning position is with respect to translation distance deviate and the anglec of rotation deviate of reference scan position.

In the present embodiment, the concrete structure of the first estimation unit 54 can be referring to Fig. 6, shown in Fig. 6 is a kind of structural representation of a kind of the first estimation unit of providing of the application, and the first estimation unit comprises: the first computing unit 61, acquiring unit 62, the second computing unit 63, the 3rd computing unit 64 and the 4th computing unit 65.

The first computing unit 61, for the default polar coordinates deflection matched rule of foundation, in described pending reference scan data, periodically search for the to be matched scanning position corresponding with described current scanning position to be matched, according to corresponding data and current scan-data to be matched and the formula of scanning position to be matched searching at every turn

calculate utmost point footpath residual sum of squares (RSS), wherein, described E (x _c, y _c) be utmost point footpath residual sum of squares (RSS), described w _ibe i the weight factor affecting for reducing bad coupling that scan-data is corresponding, described r _ribe i the included utmost point footpath of pending reference scan data, described r " _cibe i the included utmost point footpath of current scan-data to be matched, described r " _ciequal $\sqrt{{(r_{\mathrm{ci}}\cos ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+x_c)}^2+{(r_{\mathrm{ci}}\sin ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+y_c)}^2},$ Described x _c, y _c, θ _cform (x _c, y _c, θ _c) pose as current scanning position under current scan coordinate system, described φ _cifor polar angle, described r _cifor at polar angle φ _cion utmost point footpath, described r _ri-r " _ciequal linear formula ${\mathrm{\Δr}}_i\≈\frac{{\∂r}_{\mathrm{ci}}^{\′\′}}{{\∂x}_c}{\mathrm{\Δx}}_c+\frac{{\∂r}_{\mathrm{ci}}^{\′\′}}{{\∂y}_c}{\mathrm{\Δy}}_c=\cos \left({\mathrm{\φ}}_{\mathrm{ri}}\right){\mathrm{\Δx}}_c+\sin \left({\mathrm{\φ}}_{\mathrm{ri}}\right){\mathrm{\Δy}}_c,$ Described Δ x _cwith Δ y _cfor the translation distance deviate of current scanning position with respect to reference scan position, described neff is the number of the data that current scanning position is corresponding, described in

for summing function.

Acquiring unit 62, for obtaining data r ' corresponding to corresponding scanning position to be matched the residual sum of squares (RSS) minimum of the described utmost point footpath in the situation that _ri.

The second computing unit 63, for calculating in the situation of described utmost point footpath residual sum of squares (RSS) minimum, the Jacobian matrix J of described utmost point footpath residual sum of squares (RSS), described J equals $\left[\begin{array}{cc}\frac{{\∂r}_{c1}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c1}^{\′\′}}{{\∂y}_c}\\ \frac{{\∂r}_{c2}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c2}^{\′\′}}{{\∂x}_c}\\ \·\·\·& \·\·\·\end{array}\right];$

The 3rd computing unit 64, for calculating weight diagonal matrix W corresponding in the situation of described utmost point footpath residual sum of squares (RSS) minimum, described w _ifor the element in described W, described w _iequal

described d _iequal the included utmost point footpath r of current scan-data to be matched " _cideduct described r ' _ridifference, described c is constant, how definition weight from 1 to 0 changes, described m determines that how weighting function changes fast.

The 4th computing unit 65, for passing through weighted least-squares method, according to data to be matched and formula corresponding in the situation that of the residual sum of squares (RSS) minimum of described utmost point footpath $\left[\begin{array}{c}{\mathrm{\Δx}}_c\\ {\mathrm{\Δy}}_c\end{array}\right]={\left(J^T\mathrm{WJ}\right)}^{-1}{J}^{T}W(r_c^{\′\′}-r_r),$ Calculate the translation distance deviate of current scanning position and reference scan position, described r " _cfor r " _civector, described r _rfor r _rivector.

In the present embodiment, the another kind of concrete structure of the first estimation unit 54 can be referring to Fig. 7, shown in Fig. 7 is the another kind of structural representation of a kind of the first estimation unit of providing of the application, and the first estimation unit comprises: search unit 71, the 5th computing unit 72 and the second estimation unit 73.

Search unit 71, for in the included polar angle adjacent area of described current scan-data to be matched, search for the utmost point footpath identical scan-data corresponding scanning position included with described current scan-data to be matched, and from described scanning position, select default number and the nearest scanning position of described current scanning position to be matched.

The 5th computing unit 72, for by described default number and nearest each the self-corresponding utmost point footpath residual error absolute average of scanning position of described current scanning position to be matched, parabola of fit equation, calculates minimum absolute average error.

The second estimation unit 73, for by the horizontal ordinate of described minimum absolute average error, estimates anglec of rotation deviate.

Amending unit 55, for using described translation distance deviate and described anglec of rotation deviate, revises data corresponding to described current scanning position, generates the estimation pose of described current scanning position.

It should be noted that, each embodiment in this instructions all adopts the mode of going forward one by one to describe, and each embodiment stresses is the difference with other embodiment, between each embodiment identical similar part mutually referring to.For device class embodiment, because it is substantially similar to embodiment of the method, so description is fairly simple, relevant part is referring to the part explanation of embodiment of the method.

Finally, also it should be noted that, in this article, relational terms such as the first and second grades is only used for an entity or operation to separate with another entity or operational zone, and not necessarily requires or imply and between these entities or operation, have the relation of any this reality or sequentially.And, term " comprises ", " comprising " or its any other variant are intended to contain comprising of nonexcludability, thereby the process, method, article or the equipment that make to comprise a series of key elements not only comprise those key elements, but also comprise other key elements of clearly not listing, or be also included as the intrinsic key element of this process, method, article or equipment.The in the situation that of more restrictions not, the key element being limited by statement " comprising ... ", and be not precluded within process, method, article or the equipment that comprises described key element and also have other identical element.

A kind of mobile robot's position and orientation estimation method and the device that above the application are provided are described in detail, applied specific case herein the application's principle and embodiment are set forth, the explanation of above embodiment is just for helping to understand the application's method and core concept thereof; Meanwhile, for one of ordinary skill in the art, the thought according to the application, all will change in specific embodiments and applications, and in sum, this description should not be construed as the restriction to the application.

Claims (10)

1. mobile robot's position and orientation estimation method, is characterized in that, comprising:

Data corresponding to data corresponding to current scanning position and reference scan position are scanned to pre-service, obtain pending current scan-data and pending reference scan data, the data that described reference scan position is corresponding and pending reference scan data are the data under polar coordinate system;

Described current scanning position is converted to the pending current scanning position under polar coordinate system;

Described data corresponding to pending current scanning position under polar coordinate system are converted to the data under described pending reference scan data coordinate system, generate current scan-data to be matched;

According to described pending reference scan data and described current scan-data to be matched, estimate that current scanning position is with respect to translation distance deviate and the anglec of rotation deviate of reference scan position;

Use described translation distance deviate and described anglec of rotation deviate, data corresponding to described current scanning position are revised, generate the estimation pose of described current scanning position.

2. method according to claim 1, is characterized in that, the described pending reference scan data of described foundation and described current scan-data to be matched estimate that current scanning position, with respect to the process of the translation distance deviate of reference scan position, comprising:

According to default polar coordinates deflection matched rule, in described pending reference scan data, periodically search for the to be matched scanning position corresponding with described current scanning position to be matched, according to corresponding data and current scan-data to be matched and the formula of scanning position to be matched searching at every turn calculate utmost point footpath residual sum of squares (RSS), wherein, described E (x _c, y _c) be utmost point footpath residual sum of squares (RSS), described w _ibe i the weight factor affecting for reducing bad coupling that scan-data is corresponding, described r _ribe i the included utmost point footpath of pending reference scan data, described r " _cibe i the included utmost point footpath of current scan-data to be matched, described r " _ciequal $\sqrt{{(r_{\mathrm{ci}}\cos ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+x_c)}^2+{(r_{\mathrm{ci}}\sin ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+y_c)}^2},$ Described x _c, y _c, θ _cform (x _c, y _c, θ _c) pose as current scanning position under current scan coordinate system, described φ _cifor polar angle, described r _cifor at polar angle φ _cion utmost point footpath, described r _ri-r " _ciequal linear formula ${\mathrm{\Δr}}_i\≈\frac{{\∂r}_{\mathrm{ci}}^{\′\′}}{{\∂x}_c}{\mathrm{\Δx}}_c+\frac{{\∂r}_{\mathrm{ci}}^{\′\′}}{{\∂y}_c}{\mathrm{\Δy}}_c=\cos \left({\mathrm{\φ}}_{\mathrm{ri}}\right){\mathrm{\Δx}}_c+\sin \left({\mathrm{\φ}}_{\mathrm{ri}}\right){\mathrm{\Δy}}_c,$ Described Δ x _cwith Δ y _cfor the translation distance deviate of current scanning position with respect to reference scan position, described neff is the number of the data that current scanning position is corresponding, described in

for summing function;

Obtain data r corresponding to corresponding scanning position to be matched in the situation that of the residual sum of squares (RSS) minimum of described utmost point footpath " _ri;

Calculate in the situation of described utmost point footpath residual sum of squares (RSS) minimum, the Jacobian matrix J of described utmost point footpath residual sum of squares (RSS), described J equals $\left[\begin{array}{cc}\frac{{\∂r}_{c1}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c1}^{\′\′}}{{\∂y}_c}\\ \frac{{\∂r}_{c2}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c2}^{\′\′}}{{\∂x}_c}\\ \·\·\·& \·\·\·\end{array}\right];$

Calculate weight diagonal matrix W corresponding in the situation of described utmost point footpath residual sum of squares (RSS) minimum, described w _ifor the element in described W, described w _iequal

described d _iequal the included utmost point footpath r of current scan-data to be matched " _cideduct described r ' _ridifference, described c is constant, how definition weight from 1 to 0 changes, described m determines that how weighting function changes fast;

By weighted least-squares method, according to data to be matched and formula corresponding in the situation that of the residual sum of squares (RSS) minimum of described utmost point footpath $\left[\begin{array}{c}{\mathrm{\Δx}}_c\\ {\mathrm{\Δy}}_c\end{array}\right]={\left(J^T\mathrm{WJ}\right)}^{-1}{J}^{T}W(r_c^{\′\′}-r_r),$ Calculate the translation distance deviate of current scanning position and reference scan position, described r " _cfor r " _civector, described r _rfor r _rivector.

3. method according to claim 1, is characterized in that, the described pending reference scan data of described foundation and described current scan-data to be matched estimate that current scanning position, with respect to the process of the anglec of rotation deviate of reference scan position, comprising:

In the included polar angle adjacent area of described current scan-data to be matched, search for the utmost point footpath identical scan-data corresponding scanning position included with described current scan-data to be matched, and from described scanning position, select default number and the nearest scanning position of described current scanning position to be matched;

By described default number and nearest each the self-corresponding utmost point footpath residual error absolute average of scanning position of described current scanning position to be matched, parabola of fit equation, calculates minimum absolute average error;

By the horizontal ordinate of described minimum absolute average error, estimate anglec of rotation deviate.

4. method according to claim 1, is characterized in that, describedly data corresponding to data corresponding to current scanning position and reference scan position are scanned to pretreated process comprises:

Data corresponding to data corresponding to current scanning position and reference scan position are removed to noise, about subtrahend certificate.

5. method according to claim 1, is characterized in that, described data corresponding to data corresponding to current scanning position and reference scan position is scanned to pretreated process, comprising:

Data corresponding to data corresponding to current scanning position and reference scan position are removed to noise, about subtrahend certificate, medium filtering and data block are cut apart.

6. mobile robot's pose estimation unit, is characterized in that, comprising:

The first pretreatment unit, for data corresponding to data corresponding to current scanning position and reference scan position are scanned to pre-service, obtain pending current scan-data and pending reference scan data, the data that described reference scan position is corresponding and pending reference scan data are the data under polar coordinate system;

The first converting unit, for being converted to the pending current scanning position under polar coordinate system by described current scanning position;

The second converting unit, for described data corresponding to pending current scanning position under polar coordinate system are converted to the data under described pending reference scan data coordinate system, generates current scan-data to be matched;

The first estimation unit, for according to described pending reference scan data and described current scan-data to be matched, estimates that current scanning position is with respect to translation distance deviate and the anglec of rotation deviate of reference scan position;

Amending unit, for using described translation distance deviate and described anglec of rotation deviate, revises data corresponding to described current scanning position, generates the estimation pose of described current scanning position.

7. device according to claim 6, is characterized in that, described the first estimation unit comprises:

The first computing unit, for the default polar coordinates deflection matched rule of foundation, in described pending reference scan data, periodically search for the to be matched scanning position corresponding with described current scanning position to be matched, according to corresponding data and current scan-data to be matched and the formula of scanning position to be matched searching at every turn calculate utmost point footpath residual sum of squares (RSS), wherein, described E (x _c, y _c) be utmost point footpath residual sum of squares (RSS), described w _ibe i the weight factor affecting for reducing bad coupling that scan-data is corresponding, described r _ribe i the included utmost point footpath of pending reference scan data, described r " _cibe i the included utmost point footpath of current scan-data to be matched, described r " _ciequal $\sqrt{{(r_{\mathrm{ci}}\cos ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+x_c)}^2+{(r_{\mathrm{ci}}\sin ({\mathrm{\θ}}_c+{\mathrm{\φ}}_{\mathrm{ci}})+y_c)}^2},$ Described x _c, y _c, θ _cform (x _c, y _c, θ _c) pose as current scanning position under current scan coordinate system, described φ _cifor polar angle, described r _cifor at polar angle φ _cion utmost point footpath, described r _ri-r " _ciequal linear formula ${\mathrm{\Δr}}_i\≈\frac{{\∂r}_{\mathrm{ci}}^{\′\′}}{{\∂x}_c}{\mathrm{\Δx}}_c+\frac{{\∂r}_{\mathrm{ci}}^{\′\′}}{{\∂y}_c}{\mathrm{\Δy}}_c=\cos \left({\mathrm{\φ}}_{\mathrm{ri}}\right){\mathrm{\Δx}}_c+\sin \left({\mathrm{\φ}}_{\mathrm{ri}}\right){\mathrm{\Δy}}_c,$ Described Δ x _cwith Δ y _cfor the translation distance deviate of current scanning position with respect to reference scan position, described neff is the number of the data that current scanning position is corresponding, described in

for summing function;

Acquiring unit, for obtaining data r ' corresponding to corresponding scanning position to be matched the residual sum of squares (RSS) minimum of the described utmost point footpath in the situation that _ri;

The second computing unit, for calculating in the situation of described utmost point footpath residual sum of squares (RSS) minimum, the Jacobian matrix J of described utmost point footpath residual sum of squares (RSS), described J equals $\left[\begin{array}{cc}\frac{{\∂r}_{c1}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c1}^{\′\′}}{{\∂y}_c}\\ \frac{{\∂r}_{c2}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c2}^{\′\′}}{{\∂x}_c}\\ \·\·\·& \·\·\·\end{array}\right];$

The 3rd computing unit, for calculating weight diagonal matrix W corresponding in the situation of described utmost point footpath residual sum of squares (RSS) minimum, described w _ifor the element in described W, described w _iequal

described d _iequal the included utmost point footpath r of current scan-data to be matched " _cideduct described r ' _ridifference, described c is constant, how definition weight from 1 to 0 changes, described m determines that how weighting function changes fast;

The 4th computing unit, for passing through weighted least-squares method, according to data to be matched and formula corresponding in the situation that of the residual sum of squares (RSS) minimum of described utmost point footpath $\left[\begin{array}{c}{\mathrm{\Δx}}_c\\ {\mathrm{\Δy}}_c\end{array}\right]={\left(J^T\mathrm{WJ}\right)}^{-1}{J}^{T}W(r_c^{\′\′}-r_r),$ Calculate the translation distance deviate of current scanning position and reference scan position, described r " _cfor r " _civector, described r _rfor r _rivector.

8. device according to claim 6, is characterized in that, described the first estimation unit comprises:

Search unit, for in the included polar angle adjacent area of described current scan-data to be matched, search for the utmost point footpath identical scan-data corresponding scanning position included with described current scan-data to be matched, and from described scanning position, select default number and the nearest scanning position of described current scanning position to be matched;

The 5th computing unit, for by described default number and nearest each the self-corresponding utmost point footpath residual error absolute average of scanning position of described current scanning position to be matched, parabola of fit equation, calculates minimum absolute average error;

The second estimation unit, for by the horizontal ordinate of described minimum absolute average error, estimates anglec of rotation deviate.

9. device according to claim 6, is characterized in that, described the first pretreatment unit comprises:

The second pretreatment unit, for removing noise to data corresponding to data corresponding to current scanning position and reference scan position, about subtrahend certificate.

10. device according to claim 6, is characterized in that, described the first pretreatment unit comprises:

The 3rd pretreatment unit, for data corresponding to data corresponding to current scanning position and reference scan position are removed to noise, about subtrahend certificate, medium filtering and data block are cut apart.

Images