Summary of the invention
It will cause asking for path planning algorithm invalid search time-consuming and EMS memory occupation for the concave regions in grating map
Topic, the present invention provide a kind of map preprocess method for quickly excluding spill barrier region.This method is inspired by physical phenomenon,
It is not only restricted to barrier concrete shape, has the characteristics that intuitive, quick, realization is convenient.
A method of quickly filling up grating map middle concave region, comprising the following steps:
(1) it obtains and occupies grating map, select a gravity direction, be ordinarily selected to four direction up and down,
It is downward that following steps select gravity direction;
(2) traversal map, which finds a grid cell for possessing support force namely its underface, barrier grid cell,
And the grid cell is not barrier;
(3) start to carry out the expansion in horizontal direction in the grid cell or so, until filling up entire level course, Ye Jikuo
Opening up to the left and right sides is all barrier, persistently check in this process have above a certain grid barrier or certain
There is no the case where barrier below one grid, if it is present return step (2);
(4) it if this level course is the first layer that this is filled up, marks this to fill up layer and newly fills up region for one;
Otherwise the grid of this layer is filled up into region labeled as identical with next layer of grid, horizontal plane is gone up one layer, return step
(3);
(5) block creation association is filled up to adjacent, indicates this association structure using block contingency table;
(6) Exclude Lists L is establishedexc。
It is described that block creation association is filled up to adjacent, this association structure is indicated using block contingency table, comprising:
Block b is filled up by onenClassification sequence number be labeled as Tb(bn), following construction rules for block contingency table:
(5.1) apply for index list of the array space as block contingency table, the data item in array is one single-stranded
The head node of table, is abbreviated as B (n), and wherein n is index number, and wherein the data item of single linked list is to fill up the classification sequence number of block;
(5.2) block b is each filled up in tablenOccupy indexing units Tb(bn), as program is run, more blocks are added to
In concordance list, array space needs to redistribute;
(5.3) if one is being built block bnewIt is the adjacent block b established beforeoldAnd construct, then Tb(bnew) B can be become
(Tb(bold)) new node under head node.
It is described to establish Exclude Lists Lexc, comprising:
(1) an empty Exclude Lists L is establishedexc;
(2) block b will be filled up where starting point or target pointstartWith bendTb(bstart) and Tb(bend) it is added to Lexc,
Corresponding head node B (T is inquired according to block contingency tableb(bstart)) and B (Tb(bend)), and by their all subsequent knots
L is also added in pointexc;
(3) binding place is as head node, query block contingency table after will be all, and their rear binding place is added into row
Except list Lexc, this process is repeated, the node being to the last added is without rear binding place.
The beneficial effects of the present invention are:
(1) the phenomenon that abstract container is with fluid interaction power therein is flowed into, obtains one kind and fills up in grating map
Spill barrier region method.FC algorithm in the present invention is insensitive to barrier edge, as long as there are spills in map
Barrier can once be possessed filling up for preferable result.
(2) present invention is independent of the convex closure problem in computer graphics, be it is a kind of quickly, it is intuitive, facilitate application
Method.
(3) if starting point and target point are not present in a spill barrier, the grid in the spill barrier
It does not appear on optimal path.Path planning algorithm can be greatly reduced by filling up the spill barrier occupied in grating map
Invalid search time-consuming and EMS memory occupation.
(4) structure block contingency table and Exclude Lists may make carries out duplicate paths planning without repeating on the same map
Use FC algorithm.
Specific embodiment
The present invention is described further with reference to the accompanying drawing.
The present invention relates to robot path planning fields, and in particular to a kind of quickly to fill up grating map middle concave region
Method.
Path planning is one of the key problem of autonomous robot research, it is intended to the perception by robot to surrounding enviroment
Or by having cartographic information, cook up a secure path from starting point to target point.From the mesh of robot path planning
Global path planning and local paths planning can be divided into from the point of view of mark range.Wherein global path planning usually requires that robot initial
Known to pose, environmental information and target.And basic element of the map as path planning, directly affect Global motion planning algorithm
Runing time and planning quality.Common cartographic representation has grating map, topological map and semantically in robotics
Figure, wherein grating map is easy to establish, safeguard and can provide accurate measurement map.In many practical application scenes,
Such as SLAM, game and Mars exploration ramber, grating map is the most frequently used and most basic map view.But grating map
The redundancy for Precision Mapping is contained in measuring mapping, thus cannot effectively handle data.This is also resulted in
Path planning is carried out on grating map often to take considerable time.
A special situation is the invalid search to concave regions in path planning.Concave regions refer in map by room
Between, the irregular concave polygon such as office's compartment or corner be formed by region, if such region is present in starting
It puts and on the path of terminating point, will cause path planning algorithm a large amount of invalid search time and EMS memory occupation, even lead
The failure of boat.Thus by a kind of preprocessing means, to go out the type region very necessary for advance demand flag in grating map.
It will cause asking for path planning algorithm invalid search time-consuming and EMS memory occupation for the concave regions in grating map
Topic, the present invention provide a kind of map preprocess method for quickly excluding spill barrier region.This method is inspired by physical phenomenon,
It is not only restricted to barrier concrete shape, has the characteristics that intuitive, quick, realization is convenient.
To achieve the goals above, the present invention provides following schemes:
A kind of FC (Fill The Container) algorithm for quickly filling up concave regions.The algorithm is not an optimization
Scheme, it cannot be guaranteed to accomplish complete concave polygon completion, but it is insensitive to the shape of recess barrier, most of
In the case of can obtain relatively good effect.The phenomenon that abstracting container and flowing into fluid interaction power therein, is filled out
The step of mending the spill barrier region in grating map:
Step 1: acquisition occupies grating map, selectes a gravity direction, usually may be selected to be four up and down
Direction, following steps by gravity direction it is downward based on.
Step 2: traversal map, which finds a grid cell for possessing support force namely its underface, barrier grid list
Member, and the grid cell is not barrier.
Step 3: start to carry out the expansion in horizontal direction in the grid cell or so, until filling up entire level course,
Expanding to the left and right sides is all barrier, and persistently being checked whether there is above a certain grid in this process has barrier, or
Lower section does not have the case where barrier, if it happens, then jump back to step 2.
Step 4: if this level course is this first layer filled up, marking this to fill up floor and newly fill up area for one
Otherwise the grid of this layer is filled up region labeled as identical with next layer of grid by domain.Horizontal plane is gone up one layer, continues to walk
Rapid 3.
Several sunk areas are often had in real map, and each sunk area can fill up block by one or more and fill out
It mends.It fills up block and provides support force mutually in FC algorithm construction, this leads to only to clear up one in path planning and fills up block deficiency
It to go back original route, thus needs to fill up block creation association to adjacent, indicates this association structure using block contingency table.By one
Fill up block bnClassification sequence number be labeled as Tb(bn).Following construction rules for block contingency table:
Rule 1: applying for index list of the array space as block contingency table, and the data item in array is one single-stranded
The head node of table, is abbreviated as B (n), and wherein n is index number.Wherein the data item of single linked list is to fill up the classification sequence number of block.
Rule 2: block b is each filled up in tablenOccupy indexing units Tb(bn), as program is run, more blocks are added to
In concordance list, array space may need to redistribute.
Rule 3: if one is being built block bnewIt is the adjacent block b established beforeoldAnd construct, then Tb(bnew) B can be become
(Tb(bold)) new node under head node.
Execution route planning on the map for fill up block is had built up at one, then needs some additional processing work,
Because the starting point or target point that set at random are likely present in a certain fill up among block.Following steps are to stop filling out for path
Patch establishes an Exclude Lists LexcProcess:
Step 1: establishing an empty Exclude Lists Lexc。
Step 2: block b will be filled up where starting point or target pointstartWith bendTb(bstart) and Tb(bend) be added to
Lexc, corresponding head node B (T is inquired according to block contingency tableb(bstart)) and B (Tb(bend)), and will be after they all
L is also added after nodeexc。
Step 3: binding place is as head node, query block contingency table after will be all, and their rear binding place is added to
Enter Exclude Lists Lexc, this process is repeated, the node being to the last added is without rear binding place.
It, should not be by Exclude Lists L in path planning algorithm operationexcBlock is filled up represented by middle classification number as obstacle
Object, and other blocks not being acquired are then still used as barrier.
The present invention is described in detail below in conjunction with attached drawing, it is noted that described embodiment is only intended to just
In explaining the present invention, it is not construed as limiting the invention.
Unit relationship in setting two-dimensional grid map space is indicated that each grid cell is by cross by plane right-angle coordinate
Coordinate x and ordinate y identify its position.In addition, each grid has a value to indicate its classification type, this and conventional grid
Map is different, such as in two-value grating map, and 1 indicates obstacle, and 0 indicates free zone;Or in Probabilistic Cell map, grid
Numerical value indicate the grid occupy possibility.For occupying each grid in grating map, class declaration is as follows:
(1) if a grid is not occupied, classification type is 0, or is recorded as T (x, y)=0;
(2) if a grid is being occupied, classification type is 1 or T (x, y)=1;
(3) if a grid is in the filling up among region of FC algorithm, classification is more than or equal to 2, specific value meeting
It is related to the processing sequence of algorithm itself, usually, identical classification is had in the same grid for filling up region.
Several sunk areas are often had in real map, usually, each sunk area can be filled out by one or more
Patch is filled up.Such as in Fig. 5, gray area is one and fills up block.These fill up block or abbreviation block, are run in FC algorithm
Same stage creation.Each piece is made of several grid cells with same type, such alias can be used to identify
Each piece, and block b is filled up by onenCategory label be Tb(bn).With the operation of algorithm, more blocks are created, and they
Classification number can be also gradually increased from 2.
For convenience of subsequent processing, it is desirable that fill up between block and non-fully independent mutually, so being necessary for adjacent block creation
Association.This does not need the quad-tree structure in a similar multiresolution grating map, and concordance list is usually enough to handle this pass
Connection relationship, the concordance list are referred to as block contingency table.Block is each filled up in the table and occupies an indexing units, as program is run, more
More blocks are added in concordance list.Each indexing units are the head nodes an of single linked list, are abbreviated as B (type) namely rope
Draw type number node in table.If one is being built block bnewIt is the adjacent block b established beforeoldAnd construct, then Tb(bnew)
B (T can be becomeb(bold)) new node under head node.This structure is similar to Hash table, and only there is no hash functions to map.
The visible Fig. 1 of the structure of the table.
Following processes are the construction process of shown in FIG. 1 piece of contingency table:
Step 1: applying for index list of the array space as block contingency table, the data item in array is one single-stranded
The head node of table.Wherein No. 0 index entry and No. 1 index entry will not possess subsequent list structure, because being labeled as type=0
And it is to fill up block that the grid cell of type=1, which is not,.
Step 2: as FC algorithm is run, filling up block b0It is created, and Tb(b0)=2, addition index is in index list
2 unit, default fill up block bnIndex number be Tb(bn).Then fill up block b1It is created, and Tb(b1)=3, at this time b0With b1
EDGE CONTACT does not occur, thus successor node will not be become each other.
Step 3: filling up block b2It is created, and in b2Construction process in, with b0It is contacted.Because in FC algorithm
In support force decision process, the grid of the grid cell and all type >=1 of primary barrier namely type=1 all can be
It is current to determine that grid provides support force, this just cause constructed fill up block and neotectonics fill up the case where block is in contact.
At this point, a new single linked list node will be created, data item is arranged to Tb(b2)=4, and B (2) are set by the node
Successor node.Fill up block b2It does not contact, thus is existed only in the successor node of (2) B with other pieces.
Step 4: filling up block b3It is created, and in b3Construction process in, with b0And b1It is contacted.Construction two
Single linked list node, data item are arranged to Tb(b3)=5 respectively set them to the successor node of B (2) Yu B (3).
Step 5: continuing the addition of list item and the creation of chained list node, until the operation of FC algorithm finishes.
The specific mechanism of FC algorithm, comprising the following steps:
Step 1: acquisition occupies grating map M, generates an empty block contingency table B.It is 2 that current class type, which is arranged,.
Step 2: choosing a gravity direction, usually may be selected to be four direction up and down, or is diagonal plus four
Line direction.Following steps all by gravity direction it is downward based on.
Step 3: traversal removes all grid cells other than the grid of boundary one by one, and a block extended identifier is arranged
expb, and be set to true.Use the ordinate y record extension level level of active cell.
Step 4: such as fruit block extended identifier expbBe it is true, mean can to start at this time construction one it is new fill up block,
Or have been completed that the extension of one or many level courses is filled up, it can continue last round of to fill up block construction process.To each
Grid (x, y) trial is extended to construct and fill up block.Check whether grid is barrier below, and checks the grid cell
It whether is barrier.If meeting T (x, y-1)=1 and T (x, y) ≠ 1, situation as shown in Figure 2 then enters step 5, otherwise
Terminate filling, return step 3.
Step 5: the left end marker l and right end marker r extended using active cell abscissa x recording level layer, and attempt
Horizontal direction extension is carried out, one time level course spreading result is as shown in Figure 3.
Step 6: attempting to carry out left-hand extension.If left end is not barrier, i.e. T (l-1, level)=0, and block expands
Open up identifier expbIt is very, then level to be filled up into layer and extend a lattice to the left, i.e., set l-1 for l.It is then checked for the left end grid
The upper and lower whether be barrier, as shown in figure 4, lower layer's barrier or the shim previously constructed will be to work as front gate
Lattice unit provides support force, to prevent it lower repeatedly;The avoidable excessively filling of inspection upper layer barrier has been flooded can pass.Such as
Fruit meets T (l, level-1)=0 or T (l, level+1) ≠ 0, then terminates current left-hand extension, and block extension mark is arranged
Accord with expbIt is false.It is barrier or block extended identifier exp that the step, which is repeated, until expanding to left endbIt is false or arrival map
Until left margin.
Step 7: attempting to carry out dextrad extension.If right end is not barrier, i.e. T (r+1, level)=0, and block expands
Open up identifier expbIt is very, then level to be filled up into layer and extend a lattice to the right, i.e., set r+1 for r.It is then checked for the right end grid
The upper and lower whether be barrier, if meeting T (r, level-1)=0 or T (l, level+1) ≠ 0, then terminate work as
Block extended identifier exp is arranged to extension in front rightbIt is false.It is that barrier or block expand that the step, which is repeated, until expanding to right end
Open up identifier expbIt is false, or until arrival map right margin.
Step 8: such as fruit block extended identifier expbIt is false, and level ≠ y, then sets current class type to
type+1.Mean to have been built up when because meeting above-mentioned two condition complete one it is new fill up block, and upper one is filled up
Block bnTb(bn)=type.If being unsatisfactory for block extended identifier expbIt is vacation, then enters step 9.
Step 9: if meeting block extended identifier expbIt is true, it means that level course extends in step 5 and step 6
During be filled to spill barrier.Either one is filled up the bottom or other layers of block, wherein institute
There is grid that will enjoy identical classification number type.For all i ∈ (l, r+1), type is assigned to T (i, level).
Step 10: checking whether the extension of this level course is in contact with other blocks of filling up having been built up.For all i ∈
(l, r+1), if T (i, level) ≠ 1 andThen created at B (T (i, level-1))
One new node, and using type as its data item.In addition, check the left end of the horizontal extension layer and right end whether with it is other
Block contact.B if (T (l-1, level)) ≠ 1 or B (T (r+1, level)) ≠ 1, then in B (T (l-1, level)) or B (T
(r+1, level)) under create a new node, and using type as its data item.
Step 11: having finished on a level course extension at this time, enable level add 1 namely one layer of horizontal plane rise, return
Return step 4, and continue concave regions fill up filled up until one block completion, as shown in Figure 5.
Execution route planning on the map for fill up block is had built up at one, then needs some additional processing work,
Because the starting point or target point that set at random be likely present in it is a certain fill up among block, this treatment process is usually very simple
It is single, it is typically not required to map and carries out secondary treatment.Following steps are that the block of filling up in path is stopped to establish an Exclude Lists
LexcProcess:
Step 1: establishing an empty Exclude Lists Lexc。
Step 2: block b will be filled up where starting point or target pointstartWith bendTb(bstart) and Tb(bend) be added to
Lexc, corresponding head node B (T is inquired according to block contingency tableb(bstart)) and B (Tb(bend)), and will be after they all
L is also added after nodeexc。
Step 3: binding place is as head node, query block contingency table after will be all, and their rear binding place is added to
Enter Exclude Lists Lexc, this process is repeated, the node being to the last added is without rear binding place.
This process not will cause excessively complicated recursive call, because the classification number always of head node are less than thereafter
The classification number of binding place is established earlier because filling up block represented by head node.Due to block contingency table scale relative to
It is usually very little for map, so the time loss of this process is very low.In path planning algorithm operation, will not arrange
Except list LexcBlock is filled up represented by middle classification number as barrier, and other blocks not being acquired are then still used as barrier.