CN103278170B - Based on mobile robot's cascade map creating method that remarkable scene point detects - Google Patents

Abstract

The present invention relates to Mobile Robotics Navigation technical field.The invention discloses a kind of mobile robot's cascade map creating method detected based on remarkable scene point.Comprise the steps: 1) view data that gathers according to sensors for mobile robots, the natural scene road sign that the remarkable scene of on-line checkingi is corresponding, generates the topological node in global map; 2) mobile robot's pose and local grid sub-map is upgraded; 3) create Global Topological map structure using remarkable scene point as topological node, on the basis of robot trajectory's occlusion detection, introducing weighted scanning matching method and relaxation method are optimized topological structure, guarantee the global coherency of topological map.This invention is applicable to all kinds of mobile robot and carries out autonomous path planning and navigation application in the indoor comprising the regions such as multiple room, corridor on a large scale environment.

Description

Based on mobile robot's cascade map creating method that remarkable scene point detects

Technical field

The present invention relates to Mobile Robotics Navigation technical field, particularly relate to a kind of mobile robot's cascade map creating method detected based on remarkable scene point.

Background technology

Intelligent robot is the important prerequisite that robot realizes autonomous intelligence behavior to the deep layer understanding of its work space environment and utilization.Mobile robot only obtains environmental map, just can carry out path planning, track following, Global localization etc., thus arrive at the destination smoothly.At present, the autonomous map creating method that mobile robot commonly uses mainly is divided into following three classes:

1, navigation is independently explored in the environment by robot, by laser sensor scanning circumstance information, the sensors such as odometer create grating map (see " GrisettiG.ImprovedTechniquesforGridMappingwithRao-Blackw ellizedParticleFilters.IEEETransactiononRobotics; 2007; 23 (1): 34-46 "), such map is easily safeguarded and for location Calculation, but the accuracy of metric depends on the Uncertainty Management degree of odometer precision and distance measuring sensor, and the data volume of storage and maintenance is large.

2, robot is in the environment according to certain regular independent navigation, topology equivalence is carried out by the sensor such as sonar and odometer within a period of time, and mobile route connection is become topological structure, form topological map (see " B.Kuipers.LocalMetricalandGlobalTopologicalMapsintheHybr idSpatialSemanticHierarchy.IEEEInternationalConferenceon RoboticsandAutomation; 2004,4845-4851 ").Topological map focuses on the quantitative description of object on spacetime coordinates, and advantage is accurate and states brief, but cannot provide the accurate description of environment geological information.

3, robot motion under operating personnel control, the laser sensor of band The Cloud Terrace or stereo camera is utilized to obtain depth image, the three dimensional depth point cloud map (see " P.Henry.RGB-Dmapping:UsingKinect-styledepthcamerasforden se3Dmodelingofindoorenvironments; TheInternationalJournalofRoboticsResearch, 2012 ") of environment is rebuild by a cloud.Three-dimensional point cloud map can describe obstacle height information, but constructive process calculated amount is huge, and computation burden heavy in environment is on a large scale difficult to meet requirement of real time.

Although tradition can under robot self pose be coupled uncertain condition with environmental characteristic with map environment modeling method based on location while grid, solve the joint probability estimation problem of online grating map creating, but along with the raising of the increase of environment scale, circumstance complication degree and non-intellectual, there is environment and describe the problem too simple, counting yield is low in the method, constrains the deep layer understanding ability of robot to environment.How from the characteristic of human intelligible physical environment scene, not by artificial landmark means, utilize the multi-sensor information fusion such as the vehicle-mounted vision of robot, range finding, set up efficient environment descriptive model, and create problem in solution synchronous robot location with Online Map, have great importance.

When needing the mixed information such as road sign, place, grid, environmental sensor pose describing large-scale complex environment, the environmental model of mixed form should be set up, such as topology/grid mixing map, grid/feature mixing map etc. (see " Z.Lin.Recognition-basedIndoorTopologicalNavigationUsingR obustInvariantFeatures.IEEE/RSJInternationalConferenceon IntelligentRobotsandSystems.2005,3975-3980 ").While under this type of environment describes, robot localization and map building (SLAM) method generally follow stratification (Hierarchical) basic ideas, also referred to as son ground drawing method.General this kind of method needs to rely on certain map partition principle, distance measuring sensor is utilized to set up some separate sub-maps, the shared information antithetical phrase map recycled between sub-map carries out splicing and merges (see " J.Tard ó s.Robustmappingandlocalizationinindoorenvironmentsusings onardata; TheInternationalJournalofRoboticsResearch; 2002,21 (4): 311-330. ").The key difficulties of stratification SLAM method comprises topological correlation between the simultaneous localization and mapping in grid sub-map, the auto Segmentation of sub-map, sub-map, and the problem such as global level global coherency, be the key determining map building result precision and reliability.

Simultaneous localization and mapping (SLAM) in grid sub-map generally adopts following several method: EKF (ExtendedKalmanFilter, EKF), Rao-Blackwellized particle filter (Rao-BlackwellizedParticleFilter, RBPF), sparse Extended information filter (SparseExtendedInformationFilter, SEIF).The well-known shortcoming of EKF is, when estimated value and actual value depart from larger, carries out the mode that first-order linear is similar to can introduce larger linearized stability to nonlinear model.It is more effective that RBPF algorithm sets up problem at process complex environment map; but because each particle correspondence saves a complete environmental map; so it is very high to use RBPF to carry out the computation complexity of simple grating map creating under fairly large environment, be comparatively difficult to carry out.Also there is shortcoming in sparse Extended information filter (SEIF), the estimation average and the variance computation complexity that information vector and information matrix are converted to state vector very high in actual applications at every turn, is also comparatively difficult to implement in extensive environment.

The auto Segmentation of sub-map is one of difficult point of cascade map building.Current each seed map method is substantially all to make the sub-map of robot auto Segmentation by identification artificial landmark.Also method is had to utilize prior expert along training scene characteristic storehouse, but robot can not be realized to critical path target automatic acquisition (see " SchleicherD.Real-timehierarchicalstereoVisualSLAMinlarge-scaleenvironments.RoboticsandAutonomousSystems; 2010,58:991-1002. ").Also have certain methods environment for use Geometrical change information identification natural scene road sign, realize sub-map auto Segmentation, such as, point of crossing etc. between Voronoi unit, but the method result can obtain a large amount of road sign, and have lost the brief advantage of topological structure.

The natural scene of view-based access control model conspicuousness is detected as sub-map auto Segmentation and provides a kind of resolving ideas, also becomes the important means that mobile robot understands environment in circumstances not known.It walks around image object test problems, extracts the road sign of significant scene description as topological map, also brings new problem simultaneously, namely how to select significant scene to take into account storage demand and road sign matching problem.Some researchists, under human vision pays close attention to the inspiration of mechanism (VisualAttention), propose the multiple human perceptual model detected for interesting image regions.Its ultimate principle is, when biosome observes surrounding environment, often because notice to concentrate on certain or some scenery as outstanding object by behavior object demand or local scene clue selectively, various environment can effectively be distinguished and represented to such scenery as road sign.Newly there is the BayesianSurprise model of the unusual change of a kind of scene visual feature in recent years, it compares vision significance (Saliency) and Shannon entropy has more dominance energy (see " A.Ranganathan.BayesianSurpriseandLandmarkDetection; IEEEInternationalConferenceonRoboticsandAutomation, 2009 ").BayesianSurprise model achieves initial success application in fields such as the monitoring of video anomalous event, natural landmark detections, present its capacity of orientation to scene marked change.But at present, the angle not yet having bibliographical information to detect from BayesianSurprise conspicuousness discusses cascade environmental map model and establishment problem thereof.

In addition, how solving topological correlation between sub-map, and guarantee the global coherency of topological node, is another difficult point of cascade map building problem.In grid/topological cascade map, if the complexity in order to reduce cascade map calculation and storage, and ignoring the correlativity between road sign and global level, the inconsistency of global map can be introduced, cause the topological structure that creates out cannot be closed into ring.

Look into newly through patent retrieval, the people such as Xiong Rong have applied for No. 200610053690.2nd, Chinese invention patent, and name is called " method of mobile robot's simultaneous localization and mapping in circumstances not known ".Disclose the method for a kind of mobile robot simultaneous localization and mapping in circumstances not known, utilize the data that distance measuring sensor obtains, build local line segment characteristics map and grating map, utilize current robot pose estimated result to carry out coordinate transform to local map, thus upgrade overall line segment characteristics map and overall grating map.The method is based on extended Kalman filter, not high enough for the robustness compared with complex environment.In addition, the method creates environment geometry line segment characteristics map simultaneously and occupies grating map two kinds of maps, but not adopts stratification cascade map strategy.Under extensive environment, because robot localization easily occurs mistake, this can cause the two kinds of maps created to occur obvious stitching error; In addition, if to the complete grating map of extensive creating environments one, can cause huge calculated amount and memory space, actual being difficult to meets.

The people such as Wen Feng have applied for No. 201110376468.7th, Chinese invention patent, and name is called " a kind of method realizing mobile robot's simultaneous localization and mapping ".According to boat position, the method infers that mileage takes into account road sign observation data, adopt " strong tracking filfer " to improve the precision of robot localization and map building.But the map that this inventive method creates belongs to characteristics map, but not grid or topological map, robot path planning and navigation can not be directly used in.In addition, the method is also only for indoor small-scale environment and the artificial Landmarks arranged, and this also all limits the range of application of the method.

Beam the people such as to turn to and has applied for No. 200710019784.2nd, Chinese invention patent, and name is called " a kind of mobile robot map-building system and map creating method ".A kind of mobile robot map-building system and method based on wireless sensor network of this disclosure of the invention.The wireless sensor network data that the method utilization is formed creates the environmental map containing multiple information.This belongs to a kind of robot map creating method by external sensor, however due to wireless sensor node measuring accuracy lower, can only carry out rough location to robot, the environmental map created does not reach the demand of precision navigation yet.In addition, the method needs wireless sensor network node cloth to be spread in guarded region, and the method under the indoor environment not possessing this condition is also inapplicable.

Summary of the invention

How technical matters: for two subject matters existed in cascade map building, namely select significant scene to take into account storage demand and road sign matching problem, realize sub-map auto Segmentation; How to solve topological correlation between sub-map, and guarantee the global coherency of topological node; The present invention proposes a kind of mobile robot's cascade map creating method based on remarkable scene point.

Technical scheme: the mobile robot's cascade map creating method detected based on remarkable scene point, described method comprises:

According to the view data that the sensor of mobile robot gathers, carry out online significantly scene point and detect, generate Global Topological node;

Upgraded mobile robot's pose and local grid sub-map: according to laser sensor data, dead reckoning sensors, a upper cycle is to the pose estimation of mobile robot and in earlier stage build the local grid sub-map obtained, estimate the local pose of current period mobile robot, and upgrade grid sub-map local coordinate frame;

Create Global Topological map structure using remarkable scene point as topological node, on the basis of robot trajectory's occlusion detection, introducing weighted scanning matching method and relaxation method are optimized topological structure, revise the consistance of Global Topological map.

Wherein, remarkable scene point detection method comprises scene image feature extraction and scene significance calculates, and step is as follows:

1) guiding the sampling of SURF feature to gather marking area by vision noticing mechanism, rejecting a large amount of without being beneficial to the information characterizing Site characterization;

2) cluster is carried out by the SURF proper vector set of K-mean clustering algorithm to image, and adopt the space word bag model structure scene appearance features merging spatial relationship to describe, scene image is described as a kind of vision word histogram merging spatial relationship;

3) utilize the foundation of vision word histogram based on the place MultivariatePolya model of this feature interpretation, and by calculation expectation BayesianSurprise threshold value, judge whether the place residing for current robot is remarkable scene point.

The accurate description that barrier occupies the composite character such as grid, scene location feature is comprised in described local grid sub-map; Local grid sub-map adopt with current period position of mobile robot be initial point, the coordinate system that is X-axis with current period mobile robot positive dirction;

While described local grid sub-map, robot localization and map building adopt Rao-Blackwellized particle filter algorithm, create first Global Topological node after completing initial alignment, and the grating map of current establishment is saved as sub-map corresponding to described first Global Topological node.

Described Global Topological map adopts to be Global Topological node with remarkable scene point, to be connected to the graph structure that adjacent topological node formed with fillet; Each node occupies grid sub-map with a local and is associated, the transformation relation of adjacent sub-map reference framework that every bar limit is corresponding.

Described Global Topological map building step is:

1) robot acquisition vision sensor data, and carry out scene significance detection, in Global Topological map, new topological node is created when new remarkable scene point being detected, using the end pose of robot in a upper node as the true origin of new sub-map, the i.e. basis coordinates of new node, adds historical data by the feature at current field sight spot in addition;

2) iteration uses couplingsummation formula to carry out On-line Estimation and renewal to robot global position variance, the pedestal mark transformation relation between adjacent node is attached on internodal fillet, realizes relative position transmission between node and calculates;

3) topological node that each establishment is new, all mates with the historical data of topological node, thus judges whether mobile robot reaches the place accessed again; The similarity distance matching method of scene image SURF characteristic model is wherein adopted to calculate the similarity probability in current scene place and historic scenery place;

4) after detecting that bot access location trajectory is closed, to the order of connection of the topological node created by each node, the relative displacement converted quantity between the corresponding sub-map basis coordinates of weighted scanning matching method calculating adjacent node is adopted.

Described global coherency modification method is, mutual relationship between the node obtained by robot sensor observed quantity is regarded as a kind of position constraint between node, and employing relaxation method solves to meet and retrains and the topological organization structure of optimum, and step is as follows:

1) for certain node i, the estimated position of node described in each neighbors j position calculation utilizing described node and variance;

2) again according to the estimator about node i that all neighbors obtain, weighted mean obtains the new coordinate about i;

3) repeat above step, until in whole map all nodes position successively twice iteration medial error and be less than certain given threshold value, or when iteration exceedes certain total degree, terminate this relaxed algorithm process.

Preferably, described sensor comprises monocular cam and laser range finder.The data that described laser range finder obtains are that laser to scan in the environment obtained on barrier each relative to the Distance geometry angle of mobile robot in range finding height 35cm plane, within the scope of 0 ° ~ 180 °, every 1 ° of resolution obtains a laser beam data, amounts to 181 laser beam.

The present invention adopts technique scheme, has following beneficial effect:

1, for the fairly large complex environment residing for mobile work robot, establish the two-layer cascade map structure comprising local grid sub-map and Global Topological map, compare conventional cartographic model, effectively take into account the accurate description of grid sub-map to Environment Obstacles thing, and these two advantages of brief statement of relative position relation between topological map map.For barrier, the equal unknown situation of mobile robot self pose, achieve the two-layer cascade map comprising local grid sub-map and Global Topological map to create online, and solve robot pose uncertainty in map building process intercouple with map uncertainty associate, interactional problem.

2, in the natural scene situation of prosthetic road sign thing, the segmentation being used as between sub-map by the remarkable scene point in robot autonomous testing environment is proposed.Introduce human visual attention model, adopt a kind of natural landmark detection method based on remarkable scenario B ayesianSurprise, in large-scale complex indoor environment, there is lower loss and false drop rate, thus solve the sub-map auto Segmentation key issue in environment cascade map building.

3, in Global Topological map building process, relative coordinate relation between dynamic push operator node, and on the basis of robot trajectory's occlusion detection, introducing weighted scanning matching method and the topological structure of relaxation method to global map are optimized, and guarantee its global coherency.Storage space shared by the final cascade map created by robot and computational resource less, the map that can realize fairly large complicated circumstances not known creates online, and the cascade map obtained is suitable for all kinds of robot and carries out path planning and navigation application.

Accompanying drawing explanation

Fig. 1 is the initial alignment of the embodiment of the present invention and first Global Topological node process flow diagram;

Fig. 2 is the environment Mixed cascading cartographic model structural representation of the embodiment of the present invention, comprises local grid sub-map and Global Topological map;

Fig. 3 is the coordinate conversion relation schematic diagram between two Global Topological nodes of the embodiment of the present invention;

Fig. 4 is the remarkable scene point detection method process flow diagram of the embodiment of the present invention; The sBoW latent structure guided comprising image acquisition, SURF feature extraction, vision attention, the remarkable scene calculated based on apparent Surprise judge;

Fig. 5 a, 5b, 5c are that the environmental visual of remarkable scene point detection method typical scene in the actual environment comprising multiple regions such as (a) corridor, (c) office, (b) laboratory of the embodiment of the present invention notes figure and SURF feature extraction schematic diagram; Wherein the little figure in the left side of each subgraph shows the intensity of visual fixations, and the circle in the little figure in right side is the scene SURF characteristic pattern under vision attention guides;

Fig. 6 is the coordinate conversion relation schematic diagram between the local grid sub-map constructive process of the embodiment of the present invention and corresponding Global Topological node;

Fig. 7 is the visioning procedure figure of the integrated environment cascade map of the embodiment of the present invention;

Fig. 8 is that the present invention robot in example environment creates and the two-layer grid be spliced/topological cascade map.Detect when again accessing certain road sign in Global Topological, carry out the global coherency optimization of topological map, the Global Topological node line segment in figure is connected; In figure, curve is robot localization results trace, shows the kinematic roadmap of robot in map building process;

The some local grid sub-map of Fig. 9 for creating in example shown in Fig. 8.

Embodiment

Below in conjunction with specific embodiment, illustrate the present invention further, these embodiments should be understood only be not used in for illustration of the present invention and limit the scope of the invention, after having read the present invention, the amendment of those skilled in the art to the various equivalent form of value of the present invention has all fallen within the application's claims limited range.

The method step of the embodiment of the present invention is:

According to the view data of mobile robot's vehicle-mounted monocular camera collection, the natural scene road sign that the remarkable scene of on-line checkingi is corresponding, generates the topological node in global map;

According to range laser sensing data, dead reckoning speedometer sensor, a upper cycle is to the pose estimation of mobile robot and in earlier stage build the local grid sub-map obtained, estimate the local pose of current period mobile robot, and upgrade grid sub-map local coordinate frame;

Create Global Topological map structure using remarkable scene point as topological node, on the basis of robot trajectory's occlusion detection, introducing weighted scanning matching method and relaxation method are optimized topological structure, guarantee the global coherency of topological map.

Specific implementation is:

The initial alignment and the first Global Topological node that are illustrated in figure 1 the present embodiment create.First mobile robot starts (S1) to any direction from the optional position in circumstances not known, initial motion one segment distance in current room, gather laser sensor data (S2), and robot localization and map creating method carry out initial alignment (S3) while adopting in the grid sub-map of local in motion process, now create the sub-map that obtains using as the sub-map corresponding to first node in Global Topological map, now the range of movement of robot is only limitted in the room at initial place.When particle assembly converges in certain limit not yet, judge that initial alignment does not complete (S4), now circulate repeatedly until initial alignment completes; If initial alignment completes, then obtain the current pose under local coordinate system of robot, then robot continues mobile, and each region such as room and corridor of can coming in and going out, meanwhile carry out the cascade map building (S5) detected based on remarkable scene point.

In examples of implementation, the data that the vehicle-mounted range laser sensor of robot obtains are that laser to scan in the environment obtained on barrier each relative to the Distance geometry angle of mobile robot in range finding height 35cm plane, within the scope of 0 ° ~ 180 °, every 1 ° of resolution obtains a laser beam data, amounts to 181 laser beam.The vehicle-mounted vision sensor of robot is PTZCCD camera, gathers RGB image resolution ratio 320*240.

Preferably, in the present embodiment, environment cascade map structure is suitable for describing the floor indoor environment that such as hospital, office, laboratory homalographic are comparatively large, comprise the area of space such as corridor, multiple rooms, there is the double-layer structure of local grid sub-map and Global Topological map, as shown in Figure 2.Wherein:

1) contain the accurate description that barrier occupies the composite character such as grid, scene location feature in the grid sub-map of local, be suitable for mobile robot and use vehicle-mounted distance measuring sensor to locate accurately under the sub-map frame in local.Local occupy grid sub-map adopt with current period position of mobile robot be initial point, the coordinate system that is X-axis with current period mobile robot positive dirction;

Sub-map M contains the current pose of robot , Probabilistic Cell P, n place road sign position coordinates .These variablees are all set up relative to robot local loop coordinate system B, and basis coordinates can be taken as the coordinate of sub-map building initial time robot.Coordinate conversion relation such as between adjacent sub-map can be designated as , represent the relative coordinate converted quantity between node 2 and node 1.Every sub-map is created by corresponding process respectively, and only use the odometer in this process and other sensor reading during establishment, sub-map reference is also relative to the pose of robot at sub-map building initial time.Therefore each sub-map relatively between safeguard relatively independently, and not yet to correct through the overall situation.Crucial place is designated as FP={fp ₁..., fp _m, natural scene is characterized as , wherein it is the SURF Feature Descriptor of scene.

$M={(x_R^B,P,x_{F_1}^B,\·\·\·{,x}_{F_n}^B)}^T---\left(1\right)$

2) Global Topological map adopts and is Global Topological node with remarkable scene point, is connected to the graph structure that adjacent topological node formed with fillet.Each node occupies grid sub-map with a local and is associated, the transformation relation of adjacent sub-map reference framework that every bar limit is corresponding.

Each topological node i and sub-map M _ibe associated, world coordinates is designated as [x _i, y _i, θ _i].When node creates, this value is initialized as current odometer reading.The position variance of node i is designated as v _i, the variance yields being initialized as previous node adds compares 2% of previous node forward travel distance.In robot advance process, when establishment or when again visiting a node, this node and previous node limit is connected.Relative metric position relationship between topological node i with node j is designated as , as shown in Figure 3, it has variance v _ij, be taken as 0.02d _ij.

Fig. 4 is the remarkable scene point detection method process flow diagram of the present embodiment, the monocular vision sensor vehicle-mounted according to mobile robot, and the remarkable scene point of testing environment in robot kinematics, comprises scene image feature extraction and scene significance calculates, and step is as follows:

1) vision noticing mechanism introducing simulating human vision guides SURF feature to sample and gathers marking area, rejecting a large amount of without being beneficial to the information characterizing Site characterization, improving image processing efficiency;

To input scene image, sample respectively and set up the monochromatic gaussian pyramid (s of 5 grades of yardsticks ₀..., s ₄), 5 grades of yardstick CIELAB color space gaussian pyramids , and 4 grades of yardstick difference of Gaussian pyramid (DoG ₁..., DoG ₄).Then to every layer of pyramid decomposition be intensity (I), color (C) and direction (O) three passages extract characteristic pattern respectively, and adopt normalization to operate by waiting weights to add up after process, obtain vision attention figure:

Wherein on-center and off-center is the centripetal operator of cellula visualis in simulation human eye retina and centrifugal operator respectively.The sampling of SURF feature is carried out higher than the pixel region of certain threshold value for significance probability in marking area, thus the scope of feature extraction is limited in marking area, in order to avoid introduce noise spot scene key feature to without symbolical meanings, usually from the background such as body of wall, ground.

2) cluster is carried out by the SURF proper vector set of K-mean clustering algorithm to image, and adopt the space word bag model (SpatialBag-of-Word merging spatial relationship, sBoW) construct scene appearance features to describe, scene image is described as a kind of vision word histogram merging spatial relationship;

Carry out cluster by the SURF proper vector set of K-mean clustering algorithm to image, acquire k cluster centre as trunk vision word.Each topography block in image is approximately trunk vision word and represents, as the formula (3), by SURF proper vector s by characteristic quantification process employing arest neighbors classification method _ibe quantified as vision word w _irepresent.

$w_i=\underset{j}{\arg \min }\left(\right||s_i-v_j{\left|\right|}^2)---\left(3\right)$

Due to cluster centre substantial amounts, in order to accelerate quantification speed further, KD-Tree can be set up to cluster centre, can significantly accelerate nearest neighbor search speed.After characteristic quantification, add up vision word frequency of occurrence in every width image, structure generates vision word histogram, thus reduces the complexity of iamge description.

The spatial relation description of further introducing vision word.If image vision word histogram vectors is designated as X=(n ₁, n ₂..n _w), by each vision word relative to feature set set geometric center O=(x ₀, y ₀) distance and angle two features its space distribution is described, and set up histogram respectively.

A) distance component: the Euclidean distance (L calculating each unique point and geometric center ₁, L ₂..., L _w), get intermediate value as unit length L, other length is divided into 0 ~ L with reference to the ratio of L _maxfour interior intervals;

B) angle component: formed angle theta with each unique point nearest neighbor point anticlockwise with it.Any selection unique point is as starting point F ₀, other point with counterclockwise from the inside to the outside number consecutively for F ₁, F ₂..., F _w-1.Calculate with angle theta _i, i=1,2 ..., W-1, and be quantified as 0 ° ~ θ _maxfive interior intervals.

So obtain the scene visual appearance features vector shown in (4) formula, W dimensional vector { n wherein _wthe vision word statistics frequency that the word statistics in visual vocabulary table is formed, meet , rear Q dimensional vector { p _qit is the vision word spatial relationship statistics frequency.

a＝[n ₁,n ₂,...,n _W,p ₁,n ₂,...,p _Q](4)

3) foundation of vision word histogram is utilized based on the place MultivariatePolya(MP of this feature interpretation) model, and by calculation expectation BayesianSurprise threshold value, judge whether the place residing for current robot is remarkable scene point.

If current location model of place is M, there is prior probability distribution p (M).BayesianSurprise be defined as when online obtain observed quantity z time, for the KL distance between the Posterior distrbutionp p (M|z) of model M and prior distribution, that is:

$S\left(z\right)={\∫}_{M}p\left(M\right)\log \frac{p\left(M\right)}{p\left(M\right|z)}---\left(5\right)$

If the BayesianSurprise of robot current scene observation exceedes certain Surprise threshold value, then think that current location has enough notable features.Surprise threshold value adopts a kind of MonteCarlo method to carry out approximate treatment expectation value herein, is called and expects Surprise.Namely sample N number of observed quantity z from current ground point model p (M) _1:Nso, expect that Surprise is just calculated as the average of Surprise value corresponding to these samples:

$E\left(z\right)=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}S\left(z_i\right)---\left(6\right)$

If the observation A={a} of certain road sign given, wherein vectorial a as the formula (4).If w vision word occurrence number is n _wprobability be θ _wso vectorial a meets the multinomial distribution that parameter is n and θ, wherein θ=[θ ₁, θ ₂..., θ _w].Consider the specific θ value of one and θ _s=[θ _s1..., θ _sW], it obeys multinomial distribution:

$p\left(a\right|{\mathrm{\θ}}_s)=\frac{n!}{n_1!n_2!...n_W!}{\mathrm{\θ}}_{s1}^{n_1}{\mathrm{\θ}}_{s2}^{n_2}...{\mathrm{\θ}}_{\mathrm{sW}}^{n_w}---\left(7\right)$

Introduce parameter alpha _s=[α _s1, α _s2..., α _sW], represent for θ _s, to w kind vision word observation α _swlearn that the probability that this word occurs is θ for-1 time later _sw, then θ _sobey with α _sfor the Dirichlet of condition distributes, that is:

$p\left({\mathrm{\θ}}_s\right|{\mathrm{\α}}_s)=\frac{\mathrm{\Γ}\left({\mathrm{\Σ}}_{w=1}^W{\mathrm{\α}}_{\mathrm{sw}}\right)}{{\mathrm{\Π}}_{w=1}^W\mathrm{\Γ}\left({\mathrm{\α}}_{\mathrm{sw}}\right)}{\mathrm{\θ}}_{s1}^{{\mathrm{\α}}_{s1}-1}{\mathrm{\θ}}_{s2}^{{\mathrm{\α}}_{s2}-1}...{\mathrm{\θ}}_{\mathrm{sW}}^{{\mathrm{\α}}_{\mathrm{sW}}-1}---\left(8\right)$

Wherein Γ () is Gamma function.

Note α=Σ _wα _w, place model of place p (α | A) bayes rule can be used to be calculated as:

p(α|A)∝p(A|α)p(α)(9)

Likelihood score p (A| α) expansion that histogram is observed, so above formula is rewritten as:

$p\left(\mathrm{\α}\right|A)\∝p\left(\mathrm{\α}\right)\underset{\mathrm{\θ}}{\∫}p\left(\mathrm{\θ}\right|\mathrm{\α})\underset{a\∈A}{\∏}p\left(a\right|\mathrm{\θ})---\left(10\right)$

Above formula is called the MultivariatePolya(MP of place scene) model.Formula (7) (8) are substituted into the integration item of (10) formula, priori p (α) is taken as and is uniformly distributed, then place model of place is calculated as:

$p\left(\mathrm{\α}\right|A)\∝\frac{n!}{{\mathrm{\Π}}_{w=1}^W{n}_w!}\frac{\mathrm{\Γ}\left(\mathrm{\α}\right)}{\mathrm{\Γ}(n+\mathrm{\α})}\underset{w=1}{\overset{W}{\mathrm{\Π}}}\frac{\mathrm{\Γ}(n_w+{\mathrm{\α}}_w)}{\mathrm{\Γ}\left({\mathrm{\α}}_w\right)}---\left(11\right)$

The maximal possibility estimation of Dirichelet profile parameter can be obtained by sample training.Therefore for the some typical scenes (corridor, office, laboratory etc.) in environment, given a certain amount of sample image also uses iterative gradient decline optimization to learn to obtain α, thus obtains the priori MP model of scene.In order to calculate the expectation Surprise shown in (6) formula, utilize this MP model first according to (8) formula sampling θ _s, then according to (7) formula sampling a, that obtain is exactly observation sample z, as shown in the dotted line frame in Fig. 4.Fig. 5 utilizes the remarkable scene point detection method in invention, and in the actual environment comprising multiple regions such as (a) corridor, (c) office, (b) laboratory, the environmental visual of typical scene notes figure and SURF feature extraction example; Wherein the little figure in the left side of each subgraph shows the intensity of visual fixations, circle in the little figure in right side is the scene SURF feature under vision attention guides, the feature extraction scope of scene is limited in marking area by visible the method, in order to avoid introduce the noise spot (usually from the background such as body of wall, ground) scene key feature to without symbolical meanings.

According to the vehicle-mounted range finding of robot, speedometer sensor data, build the current environment cascade map observed: it comprises the local structure of grid sub-map and the structure of Global Topological map.While wherein in the grid sub-map of local, robot localization and map building adopt Rao-Blackwellized particle filter algorithm, create first Global Topological node, and the grating map of current establishment is saved as corresponding sub-map after completing initial alignment.Fig. 7 has been initial alignment and after creating first Global Topological node, the visioning procedure of integrated environment cascade map, and entire cascaded map building step is:

1) robot continues long distance motion in the environment, and period passes through each room and corridor area (step S71); Gather laser sensor data (step S72), according to dead reckoning speedometer sensor, a upper cycle is to the pose estimation of mobile robot and in earlier stage build the local grid sub-map obtained, still adopt Rao-Blackwellized particle filter algorithm, estimate the local pose of current period mobile robot, and upgrade grid sub-map local coordinate frame (step S73);

2) robot acquisition vision sensor data (step S74), and carry out scene significance detection (step S75), in Global Topological map, new topological node (step S76) is created when new remarkable scene point being detected, using the end pose of robot in a upper node as the true origin of new sub-map, the i.e. basis coordinates of new node, adds historical data by the feature at current field sight spot in addition;

3) iteration uses couplingsummation formula to carry out On-line Estimation and renewal to robot global position variance, pedestal mark transformation relation between adjacent node is attached on internodal fillet, thus realizes relative position transmission calculating (step S77) between node;

If first node base coordinate is [0,0,0], as global coordinate system reference origin.As new node fp _i+1during establishment, robot is at a upper node fp _iin pose just as new node fp _i+1true origin, namely .For realizing relative position transmission between node, iteration uses CouplingSummation formula meter to robot global position variance carry out On-line Estimation and renewal.Note for robot global coordinate system pose, usually select the basis coordinates initial point of first node as global coordinate system initial point, therefore also can be denoted as .If the corresponding place FP of new node i(is fp _i) create time the relative fp of robot _ilocal pose for and variance be respectively with .CouplingSummation formula utilizes the relative coordinate relation of two adjacent nodes to calculate relative coordinate relation between non-conterminous node.First utilize with calculate

$x_{\mathrm{robot}}^G=f(x_{{\mathrm{fp}}_i}^G,x_{\mathrm{robot}}^{{\mathrm{fp}}_i})=\left[\begin{array}{c}{x}_{{\mathrm{fp}}_i}^G+x_{\mathrm{robot}}^{{\mathrm{fp}}_i}\cos {\mathrm{\θ}}_{{\mathrm{fp}}_i}^G-y_{\mathrm{robot}}^{{\mathrm{fp}}_i}\sin {\mathrm{\θ}}_{{\mathrm{fp}}_i}^G\\ y_{{\mathrm{fp}}_i}^G+x_{\mathrm{robot}}^{{\mathrm{fp}}_i}{\sin \mathrm{\θ}}_{{\mathrm{fp}}_i}^G+y_{\mathrm{robot}}^{{\mathrm{fp}}_i}{\cos \mathrm{\θ}}_{{\mathrm{fp}}_i}^G\\ {\mathrm{\θ}}_{{\mathrm{fp}}_i}^G+{\mathrm{\θ}}_{\mathrm{robot}}^{{\mathrm{fp}}_i}\end{array}\right],---\left(12\right)$

$P_{\mathrm{robot}}^G={\▿f}_{x_{{\mathrm{fp}}_i}^G}{P}_{{\mathrm{fp}}_i}^G{{\▿f}_{x_{{\mathrm{fp}}_i}^G}}^T+{\▿f}_{x_{\mathrm{robot}}^{{\mathrm{fp}}_i}}{P}_{\mathrm{robot}}^{{\mathrm{fp}}_i}{{\▿f}_{x_{\mathrm{robot}}^{{\mathrm{fp}}_i}}}^T.---\left(13\right)$

Wherein:

${\▿f}_{x_{{\mathrm{fp}}_i}^G}=\frac{{\∂x}_{\mathrm{robot}}^G}{{\∂x}_{{\mathrm{fp}}_i}^G}=\left[\begin{array}{ccc}1& 0& {-x}_{\mathrm{robot}}^{{\mathrm{fp}}_i}{\cos \mathrm{\θ}}_{{\mathrm{fp}}_i}^G-y_{\mathrm{robot}}^{{\mathrm{fp}}_i}{\sin \mathrm{\θ}}_{{\mathrm{fp}}_i}^G\\ 0& 1& x_{\mathrm{robot}}^{{\mathrm{fp}}_i}\sin {\mathrm{\θ}}_{{\mathrm{fp}}_i}^G-y_{\mathrm{robot}}^{{\mathrm{fp}}_i}\cos {\mathrm{\θ}}_{{\mathrm{fp}}_i}^G\\ 0& 0& 1\end{array}\right],---\left(14\right)$

${\▿f}_{x_{\mathrm{robot}}^{{\mathrm{fp}}_i}}=\frac{{\∂x}_{\mathrm{robot}}^G}{{\∂x}_{\mathrm{robot}}^{{\mathrm{fp}}_i}}=\left[\begin{array}{ccc}{\cos \mathrm{\θ}}_{{\mathrm{fp}}_i}^G& {-\sin \mathrm{\θ}}_{{\mathrm{fp}}_i}^G& 0\\ {\sin \mathrm{\θ}}_{{\mathrm{fp}}_i}^G& {\cos \mathrm{\θ}}_{{\mathrm{fp}}_i}^G& 0\\ 0& 0& 1\end{array}\right],---\left(15\right)$

with robot and fp respectively _icorresponding overall absolute coordinates.Then, in order to obtain present node fp _ioverall variance , adopt CouplingSummation formula similarly, utilize with calculate

$P_{{\mathrm{fp}}_i}^G=\frac{{\∂x}_{{\mathrm{fp}}_i}^G}{{\∂x}_{{\mathrm{fp}}_{i-1}}^G}{P}_{{\mathrm{fp}}_{i-1}}^G{\left(\frac{{\∂x}_{{\mathrm{fp}}_i}^G}{{\∂x}_{{\mathrm{fp}}_{i-1}}^G}\right)}^T+\frac{{\∂x}_{{\mathrm{fp}}_i}^G}{{\∂x}_{{\mathrm{fp}}_i}^{{\mathrm{fp}}_{i-1}}}{P}_{{\mathrm{fp}}_i}^{{\mathrm{fp}}_{i-1}}{\left(\frac{{\∂x}_{{\mathrm{fp}}_i}^G}{{\∂x}_{{\mathrm{fp}}_i}^{{\mathrm{fp}}_{i-1}}}\right)}^T.---\left(16\right)$

(16) formula is used to calculate iteratively until .For calculating need utilize with , now have , and be known as [0,0,0], therefore can obtain

4) topological node that each establishment is new, all mates with the historical data of topological node, thus judges whether mobile robot reaches the place (step S78) accessed again; The similarity distance matching method of scene image SURF characteristic model is wherein adopted to calculate the similarity probability in current scene place and historic scenery place;

Robot multi-view image SURF characteristic matching strategy investigates two scene fp _aand fp _bsURF visual feature vector, its descriptor is respectively with , the SURF feature wherein detected is respectively m _aand m _bindividual, namely ${\stackrel{\→}{\mathrm{\δ}}}_A=\{{\stackrel{\→}{\mathrm{\δ}}}_A^1,...,{\stackrel{\→}{\mathrm{\δ}}}_A^{m_A}\},{\stackrel{\→}{\mathrm{\δ}}}_B=\{{\stackrel{\→}{\mathrm{\δ}}}_B^1,...,{\stackrel{\→}{\mathrm{\δ}}}_B^{m_B}\}$ 。First matching process calculates with between Euclidean distance dist (fp _a, fp _b), then adopt RANSAC method to remove impure point and can enough agree with point in model to retain, thus calculate likelihood probability tolerance.

5) after detecting that bot access location trajectory is closed, to the order of connection of the topological node created by each node, the relative displacement converted quantity (step S79) between the corresponding sub-map basis coordinates of weighted scanning matching method calculating adjacent node is adopted.

Before circuit closed, feature F is at sub-map M _jbasis coordinates system B _jwith sub-map M _ibasis coordinates system B _iunder there is different local location coordinates, and these two sub-maps are separate.And after circuit closed, sub-map M _iits basis coordinates system B is retained in continuation _ioutside also will introduce B _jrelative to B _iestimation, namely and sub-map M _jalso B is preserved _jrelative to B _j-1estimation, namely .This just makes to set up new connection between node j-1 and node i.Adopt the grid sub-map of weighted scanning coupling (WeightedScanMatching, WSM) method to adjacent node i and node i+1 correspondence to mate, calculate d _{i, i+1}, θ _{i, i+1}with , WSM method has higher computing velocity and precision for laser scanning data.After completing the converted quantity calculated between adjacent sub-map basis coordinates, carry out global map consistent correction.

Owing to there is the error of calculation in scan matching, after establishment completes all sub-maps, the global coherency of high-rise topological node is difficult to ensure, shows as only according to the local relative position relation (d between each node _ij, θ _ijwith ) global map that recovers exists comparatively big error.For this reason robot being detected again after access node, in upper level node, carry out the global coherency correction (step S710) of topological map.

The implementation method of global coherency correction is: mutual relationship between the node obtained by robot sensor observed quantity is regarded as a kind of position constraint between node, and employing relaxation method (Relaxation) solves to meet and retrains and the topological organization structure of optimum.Fillet between node can be regarded as and be similar to " spring ", the target of this relaxed algorithm makes the energy of all " spring " in whole map reach minimum exactly.For each node i, relaxed algorithm often walks iteration and comprises the following steps:

1) for certain node i, utilize its each neighbors j position to calculate estimated position and the variance of this node:

(x _i′) _j＝x _j+d _jicos(θ _ji+θ _j),(17)

(y _i′) _j＝y _j+d _jisin(θ _ji+θ _j),(18)

And the variance (v of node i _i') _j=v _j+ v _ji.

2) again according to the estimator about i that all neighbors obtain, weighted mean obtains the new coordinate about i:

$v_i=\frac{n_i}{{\mathrm{\Σ}}_j\frac{1}{{\left(v_i^{\′}\right)}_j}},---\left(20\right)$

Wherein n _iit is the neighbors number of node i.

$x_i=\frac{1}{n_i}\underset{j}{\mathrm{\Σ}}\frac{{\left(x_i^{\′}\right)}_j{v}_i}{{\left(v_i^{\′}\right)}_j},---\left(21\right)$

$y_i=\frac{1}{n_i}\underset{j}{\mathrm{\Σ}}\frac{{\left(y_i^{\′}\right)}_j{v}_i}{{\left(v_i^{\′}\right)}_j},---\left(22\right)$

${\mathrm{\θ}}_i=\arctan ({\mathrm{\Σ}}_j\frac{\sin {\left({\mathrm{\θ}}_i^{\′}\right)}_j}{{\left(v_i^{\′}\right)}_j}/{\mathrm{\Σ}}_j\frac{\cos {\left({\mathrm{\θ}}_i^{\′}\right)}_j}{{\left(v_i^{\′}\right)}_j}).---\left(23\right)$

When algorithm is initial, first node location is designated as [0,0,0], above two steps constantly iteration carry out, until in whole map all nodes position successively twice iteration medial error and be less than certain given threshold value, or when iteration exceedes certain total degree, terminate this relaxed algorithm process.

Claims (5)

1., based on mobile robot's cascade map creating method that remarkable scene point detects, it is characterized in that, described method comprises:

According to the view data that the vision sensor of mobile robot gathers, carry out online significantly scene point and detect, generate Global Topological node; Described remarkable scene point detection method comprises scene image feature extraction and scene significance calculates, and step is as follows:

1) guiding the sampling of SURF feature to gather marking area by vision noticing mechanism, rejecting a large amount of without being beneficial to the information characterizing Site characterization;

2) cluster is carried out by the SURF proper vector set of K-mean clustering algorithm to image, and adopt the space word bag model structure scene appearance features merging spatial relationship to describe, scene image is described as a kind of vision word histogram merging spatial relationship;

3) utilize the foundation of vision word histogram based on the place MultivariatePolya model of this feature interpretation, and by calculation expectation BayesianSurprise threshold value, judge whether the place residing for current robot is remarkable scene point;

Upgraded mobile robot's pose and local grid sub-map: according to laser sensor data, dead reckoning sensors, a upper cycle is to the pose estimation of mobile robot and in earlier stage build the local grid sub-map obtained, estimate the local pose of current period mobile robot, and upgrade grid sub-map local coordinate frame;

Create Global Topological map structure using remarkable scene point as topological node, on the basis of robot trajectory's occlusion detection, introducing weighted scanning matching method and relaxation method are optimized topological structure, revise the consistance of Global Topological map; Described Global Topological map adopts to be Global Topological node with remarkable scene point, to be connected to the graph structure that adjacent topological node formed with fillet; Each node occupies grid sub-map with a local and is associated, the transformation relation of adjacent sub-map reference framework that every bar limit is corresponding, and foundation step is:

1) robot acquisition vision sensor data, and carry out scene significance detection, in Global Topological map, new topological node is created when new remarkable scene point being detected, using the end pose of robot in a upper node as the true origin of new sub-map, the i.e. basis coordinates of new node, adds historical data by the feature at current field sight spot in addition;

2) iteration uses couplingsummation formula to carry out On-line Estimation and renewal to robot global position variance, the pedestal mark transformation relation between adjacent node is attached on internodal fillet, realizes relative position transmission between node and calculates;

3) topological node that each establishment is new, all mates with the historical data of topological node, thus judges whether mobile robot reaches the place accessed again; The similarity distance matching method of scene image SURF characteristic model is wherein adopted to calculate the similarity probability in current scene place and historic scenery place;

4) after detecting that bot access location trajectory is closed, to the order of connection of the topological node created by each node, the relative displacement converted quantity between the corresponding sub-map basis coordinates of weighted scanning matching method calculating adjacent node is adopted.

2. the mobile robot's cascade map creating method detected based on remarkable scene point according to claim 1, is characterized in that: comprise the accurate description that barrier occupies grid, these two composite characters of scene location feature in described local grid sub-map; Local grid sub-map adopt with current period position of mobile robot be initial point, the coordinate system that is X-axis with current period mobile robot positive dirction;

While described local grid sub-map, robot localization and map building adopt Rao-Blackwellized particle filter algorithm, create first Global Topological node after completing initial alignment, and the grating map of current establishment is saved as sub-map corresponding to described first Global Topological node.

3. the mobile robot's cascade map creating method detected based on remarkable scene point according to claim 1, it is characterized in that: described global coherency modification method is, mutual relationship between the node obtained by robot sensor observed quantity is regarded as a kind of position constraint between node, employing relaxation method solves to meet and retrains and the topological organization structure of optimum, and step is as follows:

1) for certain node i, the estimated position of node described in each neighbors j position calculation utilizing described node and variance;

2) again according to the estimator about node i that all neighbors obtain, weighted mean obtains the new coordinate about i;

3) repeat above step, until in whole map all nodes position successively twice iteration medial error and be less than certain given threshold value, or when iteration exceedes certain total degree, terminate this relaxed algorithm process.

4. the mobile robot's cascade map creating method detected based on remarkable scene point according to claim 1, it is characterized in that: described vision sensor is monocular cam, described laser sensor is laser range finder.

5. the mobile robot's cascade map creating method detected based on remarkable scene point according to claim 4, it is characterized in that: the data that described laser range finder obtains are that laser to scan in the environment obtained on barrier each relative to the Distance geometry angle of mobile robot in range finding height 35cm plane, within the scope of 0 °-180 °, every 1 ° of resolution obtains a laser beam data, amounts to 181 laser beam.