CN100449444C - Method for moving robot simultanously positioning and map structuring at unknown environment - Google Patents

Abstract

A method for simultaneously carrying out positioning and map-making by mobile robot at unknown environment includes setting-up current local occupation lattice map and local section map according to data obtained by distance-measurement transducer on mobile robot, estimating current period pose of mobile robot based on certain data and some set-up map, setting-up obtained local section map and local occupation lattice map according to pose estimation of current period mobile robot and updating global occupation lattice map and global section map.

Description

The mobile robot locatees the method with map structuring simultaneously in circumstances not known

Technical field

The present invention relates to a kind of mobile robot location and the method and system of map structuring simultaneously in circumstances not known, they can be used for the mobile robot the setting out more arbitrarily of circumstances not known, estimate robot pose and constructing environment map according to the perception data of distance measuring sensor and boat position supposition sensor.

Background technology

Along with the expansion gradually of mobile robot's range of application, how to make robot autonomous cognitive circumstances not known become a research focus in the robot and artificial intelligence field in recent years.One of mode of robot autonomous cognitive circumstances not known is exactly model or the map that makes up circumstances not known, promptly by robot according to the sensor measurement information independence make up the spatial model of its place environment or map (reach referring to " S.Thrun.Robotic mapping:Asurvey.In G.Lakemeyer and B.Nebel; editors; Exploring Artificial Intelligence in theNew Millenium.Morgan Kaufmann; 2002. " " and Chen Weidong; Zhang Fei. mobile robot's synchronous self-align and map building progress. control theory and application; 22 (3): 455-460,2005. ").By the environmental map that structure obtains, robot can carry out mission planning, path planning and carry out various operations.

All poses in the early stage circumstances not known map structuring hypothesis robot moving process can accurately be obtained by boat position supposition sensor, method according to all kinds of environmental maps of distance measuring sensor information architecture has been proposed thus, comprise based on boolean Bayes filtering and make up the method (referring to " A.Elfes.Sonar-basedreal-world mapping and navigation.IEEE Journal of Robotics and Automation; RA-3 (3): 249-265; 1987. ") take grating map and based on the method (referring to " K.S.Chong; L.Kleeman.Mobile-robot map building from an advanced sonar arrayand accurate odometry.International Journal of Robotics Research; 18 (1): 20-36,1999. ") of Kalman filtering construction feature map.

Yet the robot posture information that boat position supposition sensor is obtained not is what determine, and it exists statistics and goes up relevant measuring error on the contrary, and promptly error can be along with time integral.But traditional robot pose is estimated, as Markov localization method and Monte Carlo localization method, always suppose that map is known (referring to " D.Fox; W.Burgard; and S.Thrun.Markov localization for mobile robots in dynamicenvironments.Journal of Artificial Intelligence Research; 11:391-427; 1999. " and " S.Thrun; D.Fox, W.Burgard and F.Dellaert.Robust Monte Carlo localization formobile robots.Artificial Intelligence, 101:99-141,2000. ").

As seen, in the circumstances not known map structuring, because being used for the sensor measurement data of perception external environmental information and self mobile message, robot all exists inevitable measurement noise, make robot location and environmental map all have uncertainty in making up, and these uncertainties interrelated, influence each other.When joining the current information that observes in the constructed map according to the robot pose, the uncertainty of robot location can be brought in the constructed map, make constructed map deviation occur; When estimating the robot pose according to current observation with the matching relationship that makes up map, current observation and the uncertainty that has made up in the map can make the robot location deviation occur again.Therefore the circumstances not known map structuring must solve robot location and map structuring problem simultaneously.

There are EKF method, expectation value maximization method, particle filter method in existing location simultaneously with map constructing method.The location is made up of a series of continental embankments with the middle map of map constructing method (referring to " J.A.Castellanos; et al.The SPmap:A probabilistic framework for simultaneouslocalization and map building.IEEE Transactions on Robotics and Automation; 15 (5): 948-953; 1999. ") in the time of based on EKF, and algorithm is being safeguarded the posterior probability of robot pose and all landmark location.These class methods can be implemented in the line map structuring, but owing to adopted the hypothesis of Gaussian noise, make it require continental embankment to be distinguished mutually fully, and the sum of errors matching error of the disappearance of continental embankment, landmark recognition all will cause the failure of algorithm.Based on expectation value maximized simultaneously the location with map constructing method (referring to " W.Burgard; et al.Sonar-based mapping of large-scale mobile robotenvironments using EM.in Proc.of the International Conference on MachineLearning; Slovenia; 1999; pp.67-76. ") by constantly repeating the accuracy that two steps progressively improve map, one is that another is to calculate most probable map according to the pose expectation value according to the posterior probability of given map calculation robot pose.This method can be avoided because landmark recognition sum of errors matching error causes the map structuring failure, but because it carries out climbing method in the space of all maps, thereby can't realize online map structuring.Location and map constructing method are (referring to " M.Montemerlo and S.Thrun.Simultaneous localization and mapping with unknowndata association using FastSLAM.in Proceedings of the 2003 IEEE InternationalConference on Robotics﹠amp in the time of based on particle filter; Automation, Taipei, Taiunn, 2003, pp.1985-1991. ") be that particle filter and EKF are combined, it adopts particle filter to estimate the pose of robot, and each particle is represented a possible robot path; safeguarding a map of being determined by this path simultaneously, and the EKF method is adopted in the estimation of landmark location in the map.This method has solved location and the uncertain interactional problem of map structuring by many hypothesis, but owing to safeguard a corresponding map for each particle, needs to consume bigger storage space and computational resource.

Chinese invention patent has proposed a multirobot cooperative system No. 00816243.3, finished drawing, self poisoning, function robot location and the function robot task of environment by one or more navigating robots and distribute, wherein navigating robot has adopted landmark recognition and has calculated algorithm when locating self.Because the factor of wheel slippage or imbalance calculates that the mistake of algorithm can be along with accumulated time.In order to remedy this error, this method combines landmark recognition when the location.But since it with the function robot in the system as continental embankment, make this method and be not suitable for the robot of single operation in circumstances not known, particularly when do not have continental embankment in the current observation, uncertainty greatly will appear in the robot location.

Environment identification device that Chinese invention patent is proposed for No. 200410100518.9 and method are not considered the uncertain problem of robot location mainly towards many planes environment (being stair).

Therefore be necessary to develop a kind of can realize robot pose estimation accurately and map structuring, again can the real-time online operation, to storage space with the computational resource requirement is little and the constructed map that obtains can provide detailed environmental information, location and map constructing method when being applicable to operation such as robot navigation.

Summary of the invention

The purpose of this invention is to provide a kind of mobile robot location and the method for map structuring simultaneously in circumstances not known.

The mobile robot locatees simultaneously in circumstances not known and the method for map structuring comprises the steps:

1) according on the mobile robot the data that obtain of the distance measuring sensor of installing, make up the current part that observes and take grating map and local line segment feature map;

2) infer that according to distance measuring sensor data, boat position a sensing data, the one-period of lasting estimate mobile robot's pose and make up the overall line segment characteristics map that obtains, estimation current period mobile robot's pose early stage;

3) estimate, make up the local line segment characteristics map and the part that obtain according to pose and take grating map, upgrade the overall situation and take grating map and overall line segment characteristics map the current period mobile robot.

The data that described distance measuring sensor obtains are each distance and angles with respect to the mobile robot on the barrier in the distance measuring sensor environment that scanning obtains on the range finding elevation plane.

The part takies grating map and local line segment feature map and adopts with current period mobile robot position and be initial point, be the coordinate system of X-axis with current period mobile robot positive dirction.

The overall situation takies grating map and overall line segment characteristics map and adopts with period 1 mobile robot position and be initial point, be the coordinate system of X-axis with period 1 mobile robot positive dirction.

According on the mobile robot the data that obtain of the distance measuring sensor of installing, make up the current part that observes and take grating map and local line segment feature map: it comprises that the part takies the structure of grating map and the structure of local line segment feature map, wherein the part takies grating map and makes up employing boolean Bayes filter method, and the step that local line segment characteristics map makes up is:

1), utilize the hough transform line fitting method to obtain the initial fitting line segment to the distance measuring sensor data;

2) to the initial fitting line segment carry out with the tropism judge, collinearity is judged and repeatability is judged, merges roughly the match point set of the line segment that conllinear in the same way overlaps;

3) for the line-fitting point set after merging, utilize least square fitting method to obtain accurate match line segment;

The step of estimation current period mobile robot's pose was to mobile robot's pose estimation and the overall line segment characteristics map that early stage, structure obtained according to distance measuring sensor data, a supposition sensing data that navigates, last one-period:

1) according to boat position supposition sensing data and last one-period mobile robot's pose was estimated, utilized the moveable robot movement model, estimate current period mobile robot's pose;

2) according to estimating pose, from overall line segment characteristics map, take out the feature that drops in the distance measuring sensor sweep limit, obtain to estimate current visible line segment feature;

3) will estimate that current visible line segment feature is transformed into the local map coordinates system from the global map coordinate system;

4) be harmonious with the best of estimating current visible segment feature by seeking the distance measuring sensor data, calculate the pose correction offset;

5) according to the current period mobile robot estimate pose and pose correction offset, calculate current period mobile robot's pose and estimate.

According to boat position supposition sensing data and last one-period mobile robot's pose was estimated, and utilized the moveable robot movement model, estimate current period mobile robot's pose: comprise the steps,

1) inferred according to the boat position of current period that sensing data and the boat position of last one-period inferred sensing data, calculate the approximate translational speed of mobile robot during this;

2) according to the mobile robot's that calculates approximate translational speed and the last one-period estimation pose to the mobile robot, the pose that calculates the current period mobile robot is estimated.

Be harmonious with the best of estimating current visible segment feature by seeking the distance measuring sensor data, calculate the pose correction offset: comprise the steps,

1) adopts point to the shortest matching process of line segment feature distance, seek distance measuring sensor data and the matching relationship of estimating current visible segment feature;

2) set about from the data point relation that is complementary with same line segment feature, above-mentioned matching relationship is judged, delete improper coupling;

3) whether be whether be match point five aspects of a certain local line segment feature carry out ratio quantize definition coupling weights at the turning if forming line segment, matched line segment length and matched data from confidence level, the coupling line segment of coupling line segment feature rise time, coupling line segment feature;

4) be that data point is that zero condition makes up the parameter model structure that compatible portion the best is harmonious to coupling line segment characteristic distance according to optimum matching, utilize weighted least-squares parameter estimation tagmeme appearance correction offset;

5) the distance measuring sensor data are done translation and rotation by the pose correction offset;

6) repeating step 1)-5), up to the residual error that satisfies the weighted least-squares parameter estimation less than the certain value or the stable condition of convergence.

Estimate, make up the local line segment characteristics map and the part that obtain according to pose and take grating map the current period mobile robot, upgrade the overall situation and take grating map and overall line segment characteristics map: comprise that the overall situation takies the grating map renewal and overall line segment characteristics map upgrades, wherein the overall situation takies the grating map renewal and adopts boolean Bayes filter method, and overall line segment characteristics map upgrades and comprises the steps:

1) for the line segment feature in the local line segment characteristics map, whether searching exists an overall line segment feature and this part line segment feature to satisfy same tropism, collinearity and repeatability requirement in overall line segment characteristics map;

2) if exist, then merge the line-fitting point, utilize the least-squares line fitting process again match obtain line segment, replace former overall line segment feature;

3) if there is no, then local line segment feature is joined in the overall line segment characteristics map.

4) utilize the overall situation to take the confidence level that grating map calculates overall line segment characteristics map middle conductor feature.

The mobile robot that the present invention proposes locatees simultaneously in circumstances not known and the method for map structuring has solved the uncertainty of robot pose in the circumstances not known map structuring and uncertain interrelated, the interactional problem of map structuring, shared storage space and computational resource are less, can be implemented in line circumstances not known map structuring, constructed overall line segment characteristics map and the overall situation of obtaining takies grating map and is applicable to that the types of applications robot carries out path planning and navigation.

Description of drawings

Fig. 1 implements mobile robot's software flow pattern of location and map structuring simultaneously in circumstances not known;

Fig. 2 takies the taper figure that in the grating map structure each barrier point data is constructed in the part;

Fig. 3 is an operational flowchart of estimating current period mobile robot pose in the inventive method;

Fig. 4 is the operational flowchart that calculates the pose correction offset in the inventive method;

Fig. 5 utilizes the part in the inventive method to take grating map construction method and the local example that takies grating map and local line segment feature map of the actual drafting of local line segment feature map constructing method;

Fig. 5 (a) is an example of distance measuring sensor acquired disturbance object point data;

Fig. 5 (b) utilizes the part in the inventive method to take the grating map construction method, data instance among Fig. 5 (a) is made up a part that obtains take grating map, wherein black is represented occupied zone, white expression clear area, grey colour specification uncertain region;

Fig. 5 (c) is the local line segment characteristics map construction method that utilizes in the inventive method, the data instance among Fig. 5 (a) is made up a local line segment characteristics map that obtains;

Fig. 6 utilizes the part in the inventive method to take grating map construction method and local another example that takies grating map and local line segment feature map of the actual drafting of local line segment feature map constructing method;

Fig. 6 (a) is another example of distance measuring sensor acquired disturbance object point data;

Fig. 6 (b) utilizes the part in the inventive method to take the grating map construction method, data instance among Fig. 6 (a) is made up a part that obtains take grating map, wherein black is represented occupied zone, white expression clear area, grey colour specification uncertain region;

Fig. 6 (c) is the local line segment characteristics map construction method that utilizes in the inventive method, the data instance among Fig. 6 (a) is made up a local line segment characteristics map that obtains;

Fig. 7 utilizes an example that calculates current period mobile robot pose in the inventive method;

Fig. 7 (a) is current period distance measuring sensor barrier point data of being obtained and an example of inferring the match condition of the current visible segment feature of expectation that sensing data obtained according to current period boat position, wherein black color dots is the barrier data point that the current period distance measuring sensor observes, and the black line is to infer the current visible line segment feature of expectation that sensing data acquires according to current period boat position;

Fig. 7 (b) is the barrier point data obtained of current period distance measuring sensor and the current period robot that calculates according to the inventive method estimates the example of the match condition of the current visible segment feature of expectation that pose obtains, wherein black color dots is the barrier data point that the current period distance measuring sensor observes, and the black line is to estimate the current visible line segment feature of expectation that pose acquires according to the current period robot that the inventive method calculates;

The example of the match condition of Fig. 7 (c) current visible segment feature of expectation that to be the barrier point data obtained of current distance measuring sensor obtained with the current period robot pose that calculates according to the inventive method, wherein black color dots is the barrier data point that the current period distance measuring sensor observes, and the black line is the current visible line segment feature of expectation that is acquired according to the current period robot pose that the inventive method calculates;

Fig. 8 is that an overall situation utilizing the inventive method to draw takies grating map and overall line segment characteristics map example;

Fig. 8 (a) takies the grating map example to a long 16.4m of being, a wide overall situation utilizing the inventive method to draw for the room of 7.9m.Wherein grey lines is robot location's line of boat position supposition sensor acquisition, and the black line is the robot location's line that utilizes the inventive method to calculate;

Fig. 8 (b) is an overall line segment characteristics map example that utilizes the inventive method to draw to room described in Fig. 8 (a).

Embodiment

Below with reference to accompanying drawings, describe according to the present invention robot examples of implementation of location and map structuring simultaneously in circumstances not known in detail.In these examples of implementation, robot can independently move, and the distance measuring sensor that is equipped with is laser range finder or sonar ranging instrument or stereo visual system or their combination, and the boat position that is equipped with infers that sensor is an odometer.

Fig. 1 is a software flow pattern of implementing the inventive method.Any one position from circumstances not known of mobile robot, start by any one direction.At first carry out sensor data acquisition (step S1), structure obtains the part and takies grating map and local line segment feature map (step S2), change by coordinate system, the part is taken the grating map data and local line segment feature map datum is transformed in the global map coordinate system, take grating map and overall line segment characteristics map (step S3) thereby obtain the initial overall situation.Robot begins to move (step S4) then, and certain hour stops at interval, carries out sensor data acquisition (step S5), finish location and map structuring (step S6) simultaneously, continue to move again, so circulation repeatedly, when robot is no longer mobile, withdraw from circulation.Wherein location and map structuring comprise step simultaneously: the part takies grating map and local line segment feature map structuring; The robot pose is estimated and is proofreaied and correct; The overall situation takies the renewal of grating map and overall line segment characteristics map.

In examples of implementation, distance measuring sensor obtain one group on the range finding elevation plane in the environment on the barrier each point with respect to the distance and the angle of robot.Be initial point, be that X-axis is constructed local map coordinates system that then detected each barrier point data is several to (r in the local map coordinates system with current mobile robot's positive dirction with current mobile robot position _i, α _i), i=1 ..., n, n represent to count, r _iThe expression point is to the distance of coordinate origin, α _iDenotation coordination is the line of former point-to-point and the angle of X-axis.The Cartesian coordinates of each obstacle object point is (x _i, y _i), x _i=r _iCos α _i, y _i=r _iSin α _i.

The part takies grating map and makes up employing boolean Bayes filter method.In these examples of implementation, to each barrier point data (r _i, α _i) at first make up conical region as shown in Figure 2, wherein d ₁=r _i-d, d ₂=r _i+ d, θ be adjacent obstacle object point and true origin line angle 1/2.For this obstacle object point, the occupied method for calculating probability of grid is in the zone 1

f＝1-E _r·E _α， (1)

E wherein _r=1-k _r(l/d ₁) ², E _α=1-k _α(β/θ) ², k _r, k _αBe constant coefficient, l is the distance of grid to conial vertex, and β is that grid is to the angle between conial vertex line and the conical centre's line.The occupied method for calculating probability of grid is in the zone 2

f＝O _r·O _α， (2)

O wherein _r=1-k _r((l-r _i)/d) ², O _α=1-k _α(β/θ) ². utilize coordinate transform, determine that each grid in the conical region takies the correspondence position in the grating map in the part, utilize formula

$\log \frac{p}{1-p}=\log \frac{p^{\′}}{1-p^{\′}}+\log \frac{f}{1-f}---\left(3\right)$

Calculate the local occupied probability that takies corresponding grid in the grating map, wherein p ' is the former occupied probability of this grid, and p is the occupied probability after the renewal of being asked.

In these examples of implementation, each line segment feature with [c, θ, l, P, gt, conf, FLine] six parametric descriptions, is designated as L.C wherein, θ is the parameter of line segment place straight line, straight-line equation is

xcosθ+ysinθ+c＝0. (4)

C is the distance of initial point to straight line, and θ is the normal of straight line and the angle of coordinate system x axle.L is a line segment length, P=(x _c, y _c) ^TBe line segment center position (T represents transposition).Gt is the timestamp attribute, the record line segment rise time.Conf is the confidence level attribute of line segment.FLine is a Boolean quantity, is used to represent whether line segment is the formation line segment at turning, if then be true, otherwise is false.

In the local line segment characteristics map of these examples of implementation makes up, at first hough transform fitting a straight line method is used in the barrier point data set that distance measuring sensor obtained and determined tentatively which bar line segment is those point data belong to.Be specially by certain dispersion and make up two dimension ballot grid, wherein one dimension is the discrete value of straight line parameter c, and one dimension is the discrete value of straight line parameter θ.For each obstacle object point (x _i, y _i), all there is one group of parameter to be (c _j, θ _j) straight line through this point, thereby be that corresponding grid is voted.By nose count, can determine that each obstacle object point belongs to the parametric description of which bar line segment and this line segment.

Yet because measuring error, the measured obstacle object point of distance measuring sensor is not a conllinear accurately, is difficult to select a suitable discrete grid block size in Hough changes yet, for one group should conllinear point tend to generate match line segment more than one.Therefore in the local line segment characteristics map of examples of implementation makes up, after the hough transform fitting a straight line, the line segment in the resultant line segment aggregate is carried out collinearity judgement in twos.As line segment L ₁With line segment L ₂When satisfying following collinearity judgment rule, think that two line segments are collinear, carry out the merging of some set on the line segment.

1) same tropism requires | θ ₁-θ ₂|≤Δ θ, wherein Δ θ is a threshold values;

2) collinearity requires ‖ c ₁|-| c ₂‖≤Δ C, $d=|x_{{\mathrm{<figure-callout\; id="1"\; label="c"\; filenames="C20061005369000221.png"\; state="\{\{state\}\}">c</figure-callout>}}_1}\cos {\mathrm{\&theta;}}_2+{\mathrm{<figure-callout\; id="1"\; label="y\; c"\; filenames="C20061005369000221.png"\; state="\{\{state\}\}">y</figure-callout>}}_{c_{}}\sin {\mathrm{\&theta;}}_2+c_2|\&le;\mathrm{\&Delta;D}$ , Δ C wherein, Δ D is a threshold values.Be initial point to the range difference of line segment place straight line less than threshold values Δ C, simultaneously line segment central point to the vertical range of another line segment less than threshold values Δ D.

3) heavy and property, requirement ${(x_{c_1}-x_{c_2})}^2+{(y_{c_1}-y_{c_2})}^2\&le;{({\mathrm{<figure-callout\; id="1"\; label="l"\; filenames="C20061005369000221.png"\; state="\{\{state\}\}">l</figure-callout>}}_1/2+l_2/2)}^2$ , promptly the Euclidean distance between the line segment central point is smaller or equal to line segment half-court sum.

Merge the line segment point set of roughly coincidence by the collinearity judgement after, utilize the least-squares line fitting process to ask for more accurate line segment parameter c, θ, l and P again.Parameter gt is the current time, parameter c onf=1.When the angle of line segment and another line segment about 90 degree and crossing intersection point on line segment the time, FLine is changed to very.

In these examples of implementation, mobile robot's pose was estimated and is made up the overall line segment characteristics map and the overall situation that obtain early stage to take grating map according to distance measuring sensor data, boat position supposition sensing data, last one-period, estimate current period mobile robot's pose, its step as shown in Figure 3.In these examples of implementation, be initial point, be X-axis structure global map coordinate system with mobile robot's inceptive direction with the initial pose of mobile robot.The boat position infers that sensing data and the estimation of mobile robot's pose are the data in the global map coordinate system.Thus, robot is (0,0,0) at the initial pose that the overall situation takies in grating map and the overall line segment characteristics map.

In step S61, according to boat position supposition sensing data and last one-period mobile robot's pose was estimated, utilize the moveable robot movement model, estimate current period mobile robot's pose.The note current period is t, and the t-1 cycle position of navigating infers that a sensor reading is $O_{t-1}={(x_{O_{t-1}},y_{O_{t-1}},z_{O_{t-1}})}^T$ , the t cycle position of navigating infers that a sensor reading is $O_t={(x_{O_t},y_{O_t},z_{O_t})}^T,$ T-1 cycle mobile robot's pose is estimated as X _T-1=(x _T-1, y _T-1, z _T-1) ^TCan suppose that the motion of robot is approximately uniform motion during the t-1 cycle to t cycle, its movement velocity is D=(dx, dy, d θ) ^T, dx wherein, dy is respectively x, the linear velocity of y direction, d θ is an angular velocity, its computing formula is as follows:

$\left\{\begin{array}{c}\mathrm{dx}=(x_{O_t}-x_{O_{t-1}})\cos {\mathrm{\&theta;}}_{O_{t-1}}+(y_{O_t}-y_{O_{t-1}})\sin {\mathrm{\&theta;}}_{O_{t-1}}\\ \mathrm{dy}=-(x_{O_t}-x_{O_{t-1}})\sin {\mathrm{\&theta;}}_{O_{t-1}}+(y_{O_t}-y_{O_{t-1}})\cos {\mathrm{\&theta;}}_{O_{t-1}}.\\ \mathrm{d\&theta;}={\mathrm{\&theta;}}_{O_t}-{\mathrm{\&theta;}}_{O_{t-1}}\end{array}\right.---\left(5\right)$

Pose according to this translational speed and t-1 cycle mobile robot is estimated, can obtain estimating of t cycle mobile robot pose $X_{t_0}={(x_{t_0},y_{t_0},z_{t_0})}^T,$ Its computing formula is as follows:

$\left\{\begin{array}{c}{x}_{t_0}=x_{t-1}+\mathrm{dx}\cos {\mathrm{\&theta;}}_{t-1}-\mathrm{dy}\sin {\mathrm{\&theta;}}_{t-1}\\ y_{t_0}=y_{t-1}+\mathrm{dx}\sin {\mathrm{\&theta;}}_{t-1}+\mathrm{dy}\cos {\mathrm{\&theta;}}_{t-1}.\\ {\mathrm{\&theta;}}_{t_0}={\mathrm{\&theta;}}_{t-1}+\mathrm{d\&theta;}\end{array}\right.---\left(6\right)$

In step S62, according to estimating pose $X_{t_0}={(x_{t_0},y_{t_0},z_{t_0})}^T$ From overall line segment characteristics map, take out the line segment feature that drops in the distance measuring sensor sweep limit, obtain to estimate current visible line segment feature.In these examples of implementation, the line segment feature that one of end points drops within the stadia surveying scope is the current visible line segment feature of expectation, and all estimate that current visible line segment feature constitutes set Γ.

In these examples of implementation, select local map coordinates system to be the coupling coordinate system, therefore will estimate that in step S63 current visible line segment feature is transformed into the local map coordinates system from the global map coordinate system, the line segment feature that is about to gather among the Γ is transformed in the local map coordinates system.Line segment among the note set Γ is L _W: { c _W, θ _W, l _W, P _W, gt _W, conf _W, the line segment that is transformed in the local map coordinates system is L _R: { c _R, θ _R, l _R, P _R, gt _R, conf _R, transfer equation is

$\left\{\begin{array}{c}\left[\begin{array}{c}{x}_{c_R}\\ y_{c_R}\end{array}\right]=\left[\begin{array}{cc}\cos {\mathrm{\&theta;}}_{t0}& \sin {\mathrm{\&theta;}}_{t0}\\ -\sin {\mathrm{\&theta;}}_{t0}& \cos {\mathrm{\&theta;}}_{t0}\end{array}\right]\left[\begin{array}{c}{x}_{c_W}-x_{t0}\\ y_{c_W}-y_{t0}\end{array}\right]\\ {\mathrm{\&theta;}}_R={\mathrm{\&theta;}}_W-{\mathrm{\&theta;}}_{t0}\\ c_R=-(x_{c_R}\cos {\mathrm{\&theta;}}_R+x_{c_R}\sin {\mathrm{\&theta;}}_R)\\ l_R=l_W,{\mathrm{gt}}_R={\mathrm{gt}}_W,\mathrm{con}{f}_R=\mathrm{con}{f}_W\end{array}\right.$

The line segment feature that is converted to constitutes set Γ '.

In step S64, be harmonious with the best of estimating current visible segment feature by seeking the distance measuring sensor data, calculate the pose correction offset.Fig. 4 has provided performing step.

In step S641, the barrier point data (x that records for each distance measuring sensor _i, y _i) at the nearest with it line segment feature of the middle searching of Γ '.In these examples of implementation, require the barrier data point to the distance of nearest line segment feature less than certain threshold values, if satisfy condition, just this line segment feature is called the coupling line segment of this data point, this data point is called match point.All match points constitute set V.

The matching way of step S641 causes improper coupling easily, and the improper coupling of this class will influence the degree of accuracy of the pose correction offset that is calculated, and need be removed.In step S642, carry out improper matching judgment, and the point that will be judged as improper coupling is deleted from set V.Examples of implementation of improper matching judgment are: remember that a certain coupling line segment is L _p, will gather among the V with L _pFor the coupling line segment match point by being linked in sequence clockwise or counterclockwise, be designated as the set Z, Z={v _i, i=1 ..., k}, v _i=(x _i, y _i) ^T, k is that coupling is counted.Utilize the least-squares line method that fits that the point among the Z is carried out line-fitting, the line segment that the note match obtains is L _qCalculation level arrives the distance and the d of match line segment,

$d=\underset{i=1}{\overset{k}{\mathrm{\&Sigma;}}}{d}_i=\underset{i=1}{\overset{k}{\mathrm{\&Sigma;}}}(x_i\cos {\mathrm{\&theta;}}_q+y_i\sin {\mathrm{\&theta;}}_q+c_q).---\left(8\right)$

If | θ _p-θ _q|＞threshold values, promptly match point fits the line segment that obtains with coupling line segment differential seat angle when big, thinks matching error, in set V among the deletion set Z have a few.If | θ _p-θ _q|≤threshold values, and d＞threshold values, think that then there is the mistake coupling in the part point among the Z.In order to remove these mistake couplings, each point among the Z is made the following judgment:

1) if v _iBe certain line segment L in the current local line segment characteristics map _oMatch point, then change (3), otherwise change (2);

2) in Z, get v _iPrevious match point v _I-1With a back match point v _I+1, ask v respectively _iWith the wire length and the angle of these two points, θ ₂=∠ v _iv _I+1, Δ θ ₁=| θ _p-θ ₁|, Δ θ ₂=| θ _p-θ ₂If |. d ₁Or d ₂Greater than certain value, and Δ θ ₁With Δ θ ₁, then be correct coupling all, otherwise be the mistake coupling less than definite value.

3) if local line segment L _oWith coupling line segment L _pSatisfying the collinearity requirement, then is correct coupling, otherwise is the mistake coupling.

Deletion is judged as the point of mistake coupling in set V.Obtain match point set V ' by step S42.

In step S643, to each match point v among the match point set V ' _i=(x _i, y _i) ^TDefinition coupling weight w _iThe weights computing formula is

$w_i=(t-{\mathrm{gt}}_p)/(t-\underset{i=1,\&CenterDot;\&CenterDot;\&CenterDot;,m}{\min }\left({\mathrm{gt}}_i\right))+{\mathrm{Fline}}_p+{\mathrm{conf}}_p+\mathrm{LineDot}\&times;\min (1,l_p/l_o)---\left(9\right)$

L in the formula _pBe v _iCorresponding coupling line segment feature, LineDot is a Boolean quantity, is used for expression point v _iWhether be the match point of certain line segment feature in the current local line segment characteristics map,, and remember that this part line segment is characterized as L if then be true _oFirst expression coupling line segment rise time in the formula, it was high more more then to mate weights, because line segment feature is along with the renewal of map is constantly revised; Second expression coupling weights when the coupling line segment is the formation line segment at turning are higher; The 3rd explanation matched line confidence level is high more, and the coupling weights are high more; The 4th expression is when match point is the match point of certain local line segment, its coupling weights are higher than the coupling weights of failing to be fitted to the match point in any local line segment, and when local line's segment length is less than or equal to the matched line segment length, its weights are higher than the weights of local line's segment length greater than the matched line segment length, and line segment will be corrected because the latter illustrates coupling.

Step S644 calculates pose correction offset Δ, Δ=(Δ _x, Δ _y, Δ _θ) ^TIn examples of implementation, make up the parameter model structure according to optimum matching earlier.The optimum matching situation is to each the match point v among the V ' _iAfter being rotated translation by pose correction offset Δ, each match point is 0 to the distance of corresponding matched line.Note v _i' be v _iBe rotated the point that obtains after the translation by Δ.

${v_i}^{\&prime;}=\left[\begin{array}{cc}\cos {\mathrm{\&Delta;}}_{\mathrm{\&theta;}}& -\sin {\mathrm{\&Delta;}}_{\mathrm{\&theta;}}\\ \sin {\mathrm{\&Delta;}}_{\mathrm{\&theta;}}& \cos {\mathrm{\&Delta;}}_{\mathrm{\&theta;}}\end{array}\right](v_i-\stackrel{\&OverBar;}{v})+\stackrel{\&OverBar;}{v}+\left[\begin{array}{c}{\mathrm{\&Delta;}}_x\\ {\mathrm{\&Delta;}}_y\end{array}\right]\&ap;{\mathrm{\&Delta;}}_{\mathrm{\&theta;}}\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right](v_i-\stackrel{\&OverBar;}{v})+v_i+\left[\begin{array}{c}{\mathrm{\&Delta;}}_x\\ {\mathrm{\&Delta;}}_y\end{array}\right].---\left(10\right)$

Wherein v is the center of gravity of being had a few among the coupling set V '.v _i' to v _iCorresponding matched line L _pDistance be d _i=r _i-u _iV _i', u _iBe L _pThe unit normal vector, r _iBe a bit any on the matched line and u _iDot product.Can get u by straight-line equation _i=[u _I1, u _I2]=[cos θ _p, sin θ _p], r _i=-c _p. then by optimal coupling condition d _i=0

Wherein

By total m point among the set V ', can get

Y＝ΦΔ. (13)

Wherein Y is by y _iM * 1 column vector of forming, Φ is

M * 3 matrixes of forming.Utilize weighted least-squares to estimate to calculate pose correction offset Δ,

Δ＝(Φ ^TWΦ) ^-1Φ ^TWY. (14)

Wherein W is m * m diagonal angle positive definite weighting matrix, W[i, i] be match point v _iThe coupling weight w _iCompatible portion will reach the best and be harmonious this moment, i.e. matching error covariance J _W=(Y-Φ Δ) ^TW (Y-Φ Δ) reaches minimum value.

Step S645 does translation and rotation with each barrier data point by the pose correction offset.And then execution in step S41, after step S41 is complete, restrain judgement.In these examples of implementation, the condition of convergence is that the residual error estimated of weighted least-squares is less than certain value or be stabilized on certain value.When the condition of convergence satisfied, gained pose correction offset was the pose correction offset of asking; When the condition of convergence does not satisfy, get back to step S42, so move in circles and satisfy up to the condition of convergence.

Execution in step S65 then is according to current period mobile robot's the pose of estimating $X_{t_0}={(x_{t_0},y_{t_0},z_{t_0})}^T$ With pose correction offset Δ=(Δ _x, Δ _y, Δ _θ) ^T, calculate current period mobile robot's pose and estimate X _t=(x _t, y _t, z _t) ^T, its computing formula is:

$\left\{\begin{array}{c}{x}_t=x_{t_0}+\mathrm{\&Delta;}x\cos {\mathrm{\&theta;}}_{t_0}-\mathrm{\&Delta;}y\sin {\mathrm{\&theta;}}_{t_0}\\ y_t=y_{t_0}+\mathrm{\&Delta;}x\sin {\mathrm{\&theta;}}_{t_0}+\mathrm{\&Delta;}y\cos {\mathrm{\&theta;}}_{t_0}.\\ {\mathrm{\&theta;}}_t={\mathrm{\&theta;}}_{t_0}+\mathrm{\&Delta;\&theta;}\end{array}\right.---\left(15\right)$

Take grating map according to the pose estimation of resulting robot, local line segment characteristics map and part, can upgrade the overall line segment characteristics map and the overall situation and take grating map.According to estimated robot pose, calculate local each grid that takies in the grating map and take pairing position in the grating map in the overall situation, utilize formula (3) to upgrade the occupied probability of this grid.Equally, according to estimated robot pose, each line segment feature in the local line segment characteristics map is transformed in the overall line segment characteristics map, and whether searching exists an overall line segment feature and this part line segment feature to satisfy same tropism, collinearity and repeatability requirement in overall line segment characteristics map.If exist, then merge the line-fitting point, utilize the least-squares line fitting process to recomputate the line segment feature attribute, replace former overall line segment feature, if there is no, then this line segment feature is joined in the overall line segment characteristics map.Utilize the overall situation to take the confidence level that grating map calculates each line segment feature at last, account form is to be positioned at the total length of the line segment feature length of occupied grid divided by this line segment feature.When with a low credibility during in certain value, this line segment feature of deletion in overall line segment characteristics map.

Claims (9)

1. the mobile robot method of location and map structuring simultaneously in circumstances not known is characterized in that, comprises the steps:

1) according on the mobile robot the data that obtain of the distance measuring sensor of installing, structure obtains the part and takies grating map and local line segment feature map, change by coordinate system, the part is taken the grating map data and local line segment feature map datum is transformed in the global map coordinate system, take grating map and overall line segment characteristics map thereby obtain the initial overall situation;

2) according on the mobile robot the data that obtain of the distance measuring sensor of installing, make up the current part that observes and take grating map and local line segment feature map;

3) infer that according to distance measuring sensor data, boat position a sensing data, the one-period of lasting estimate mobile robot's pose and make up the overall line segment characteristics map that obtains, estimation current period mobile robot's pose early stage;

4) estimate, make up the local line segment characteristics map and the part that obtain according to pose and take grating map, upgrade the overall situation and take grating map and overall line segment characteristics map the current period mobile robot;

5) repeating step 2)～step 4), obtain the overall situation and take grating map and overall line segment characteristics map.

2. a kind of mobile robot according to claim 1 locatees the method with map structuring simultaneously in circumstances not known, it is characterized in that the data that described distance measuring sensor obtains are each distance and angles with respect to the mobile robot on the barrier in the distance measuring sensor environment that scanning obtains on the range finding elevation plane.

3. a kind of mobile robot according to claim 1 locatees the method with map structuring simultaneously in circumstances not known, it is characterized in that described part takies grating map and local line segment feature map and adopts with current period mobile robot position and be initial point, be the coordinate system of X-axis with current period mobile robot positive dirction.

4. a kind of mobile robot according to claim 1 locatees the method with map structuring simultaneously in circumstances not known, it is characterized in that the described overall situation takies grating map and overall line segment characteristics map and adopts with period 1 mobile robot position and be initial point, be the coordinate system of X-axis with period 1 mobile robot positive dirction.

5. a kind of mobile robot according to claim 1 locatees the method with map structuring simultaneously in circumstances not known, it is characterized in that, described according on the mobile robot the data that obtain of the distance measuring sensor of installing, make up the current part that observes and take grating map and local line segment feature map: it comprises that the part takies the structure of grating map and the structure of local line segment feature map, wherein the part takies grating map and makes up employing boolean Bayes filter method, and the step that local line segment characteristics map makes up is:

1), utilize the hough transform line fitting method to obtain the initial fitting line segment to the distance measuring sensor data;

2) to the initial fitting line segment carry out with the tropism judge, collinearity is judged and repeatability is judged, merges roughly the match point set of the line segment that conllinear in the same way overlaps;

3) for the line-fitting point set after merging, utilize least square fitting method to obtain accurate match line segment

6. a kind of mobile robot according to claim 1 locatees the method with map structuring simultaneously in circumstances not known, it is characterized in that, it is described that the step of estimation current period mobile robot's pose was to mobile robot's pose estimation and the overall line segment characteristics map that early stage, structure obtained according to distance measuring sensor data, a supposition sensing data that navigates, last one-period:

1) according to boat position supposition sensing data and last one-period mobile robot's pose was estimated, utilized the moveable robot movement model, estimate current period mobile robot's pose;

2) according to estimating pose, from overall line segment characteristics map, take out the feature that drops in the distance measuring sensor sweep limit, obtain to estimate current visible line segment feature;

3) will estimate that current visible line segment feature is transformed into the local map coordinates system from the global map coordinate system;

4) be harmonious with the best of estimating current visible segment feature by seeking the distance measuring sensor data, calculate the pose correction offset;

5) according to the current period mobile robot estimate pose and pose correction offset, calculate current period mobile robot's pose and estimate.

7. a kind of mobile robot according to claim 6 locatees the method with map structuring simultaneously in circumstances not known, it is characterized in that, described according to position supposition sensing data and last one-period the pose estimation of navigating to the mobile robot, utilize the moveable robot movement model, estimate current period mobile robot's pose: comprise the steps

1) inferred according to the boat position of current period that sensing data and the boat position of last one-period inferred sensing data, calculate the approximate translational speed of mobile robot during this;

2) according to the mobile robot's that calculates approximate translational speed and the last one-period estimation pose to the mobile robot, the pose that calculates the current period mobile robot is estimated.

8. a kind of mobile robot according to claim 6 locatees the method with map structuring simultaneously in circumstances not known, it is characterized in that, described by seeking the distance measuring sensor data and estimating that the best of current visible segment feature is harmonious, and calculates the pose correction offset: comprise the steps

1) adopts point to the shortest matching process of line segment feature distance, seek distance measuring sensor data and the matching relationship of estimating current visible segment feature;

2) set about from the data point relation that is complementary with same line segment feature, above-mentioned matching relationship is judged, delete improper coupling;

3) whether be whether be match point five aspects of a certain local line segment feature carry out ratio quantize definition coupling weights at the turning if forming line segment, matched line segment length and matched data from confidence level, the coupling line segment of coupling line segment feature rise time, coupling line segment feature;

4) be that data point is that zero condition makes up the parameter model structure that compatible portion the best is harmonious to coupling line segment characteristic distance according to optimum matching, utilize weighted least-squares parameter estimation method to calculate the pose correction offset;

5) the distance measuring sensor data are done translation and rotation by the pose correction offset;

6) repeating step 1)-5), up to the residual error that satisfies the weighted least-squares parameter estimation less than the certain value or the stable condition of convergence.

9. a kind of mobile robot according to claim 1 locatees the method with map structuring simultaneously in circumstances not known, it is characterized in that, described basis is estimated, is made up the local line segment characteristics map and the part that obtain to current period mobile robot's pose and takies grating map, upgrade the overall situation and take grating map and overall line segment characteristics map: comprise that the overall situation takies the grating map renewal and overall line segment characteristics map upgrades, wherein the overall situation takies the grating map renewal and adopts boolean Bayes filter method, and overall line segment characteristics map upgrades and comprises the steps:

1) for the line segment feature in the local line segment characteristics map, whether searching exists an overall line segment feature and this part line segment feature to satisfy same tropism, collinearity and repeatability requirement in overall line segment characteristics map;

2) if exist, then merge the line-fitting point, utilize the least-squares line fitting process again match obtain line segment, replace former overall line segment feature;

3) if there is no, then local line segment feature is joined in the overall line segment characteristics map;

4) utilize the overall situation to take the confidence level that grating map calculates overall line segment characteristics map middle conductor feature.

Images