CN107741743A - Improved figure optimizes SLAM methods - Google Patents
Improved figure optimizes SLAM methods Download PDFInfo
- Publication number
- CN107741743A CN107741743A CN201711075649.XA CN201711075649A CN107741743A CN 107741743 A CN107741743 A CN 107741743A CN 201711075649 A CN201711075649 A CN 201711075649A CN 107741743 A CN107741743 A CN 107741743A
- Authority
- CN
- China
- Prior art keywords
- agv
- environment
- slam
- optimization
- laser
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Withdrawn
Links
- 238000000034 method Methods 0.000 title claims abstract description 23
- 238000005457 optimization Methods 0.000 claims abstract description 38
- 238000001514 detection method Methods 0.000 claims abstract description 15
- 241001269238 Data Species 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 3
- 238000011161 development Methods 0.000 claims description 2
- 239000000203 mixture Substances 0.000 abstract description 6
- 230000004888 barrier function Effects 0.000 abstract description 4
- 238000013316 zoning Methods 0.000 abstract 1
- 239000000243 solution Substances 0.000 description 20
- 230000006870 function Effects 0.000 description 14
- 239000000835 fiber Substances 0.000 description 9
- 229920000728 polyester Polymers 0.000 description 9
- 238000010586 diagram Methods 0.000 description 7
- 230000006872 improvement Effects 0.000 description 6
- 238000012552 review Methods 0.000 description 4
- 230000009466 transformation Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 3
- 230000004807 localization Effects 0.000 description 3
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 2
- WHXSMMKQMYFTQS-UHFFFAOYSA-N Lithium Chemical compound [Li] WHXSMMKQMYFTQS-UHFFFAOYSA-N 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 2
- 238000009795 derivation Methods 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 229910052744 lithium Inorganic materials 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000015654 memory Effects 0.000 description 2
- 238000000844 transformation Methods 0.000 description 2
- 230000008901 benefit Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 230000006378 damage Effects 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 235000013399 edible fruits Nutrition 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 239000004744 fabric Substances 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000002347 injection Methods 0.000 description 1
- 239000007924 injection Substances 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The positioning precision of traditional laser triangulation location algorithm is high, but being susceptible to the influence that barrier blocks causes AGV not position；The method that figure optimization SLAM algorithms are patterned with front end composition and rear end optimization ensure that the precision and accuracy of structure map, and the method detected with closed loop realizes the integrality of composition.But this feature in garage is less, feature is single, in the symmetrical environment of feature height so that the function of closed loop detection can not realize that, so as to cause have many overlapping places above the map established, this map can not provide navigator fix well.This paper presents laser triangulation algorithm is combined into the method optimized with SLAM algorithms, with the reflector being previously disposed in environment, overall situation is divided into numerous subenvironment and carries out independent numbering to these subenvironments.It can also be positioned inside subenvironment with laser triangulation positioning, so by after the Field Number of environment zoning, each region that can detection AGV is reached, then the last signal for being formed figure optimization SLAM closed loop detections of the different numbering in different regions, the precision for the map being built such that are just higher.
Description
Technical field
The present invention relates to improvement figure optimization SLAM to build the method for figure, and the application positioned in intelligent garage, is related to
The technical fields such as intelligent control, sensor fusion, robot, navigation and positioning.
Background technology
AGV positioning at present mainly uses laser triangulation navigator fix, magnetic nail navigator fix, magnetic stripe navigator fix, pulse
Line navigator fix, Quick Response Code positioning, laser SLAM navigator fixs, vision guided navigation positioning, the methods of mixed positioning.
Arrangement reflector in advance is needed in the environment using laser triangulation location algorithm, and at least need to know three it is reflective
The position of plate can just calculate AGV posture information, although the positioning precision of laser navigation localization method is very high, easily by
To blocking for other barriers, so as to cause positioning inaccurate or the function of positioning can not be realized.
The technology of magnetic stripe positioning is very ripe, and the positioning precision of the positioning of magnetic nail is high.But magnetic stripe positioning needs to lay
Magnetic stripe and later stage also need to safeguard, so that cost increase；And magnetic nail positioning needs to bury magnetic nail in ground, to ground
Destructiveness is big, and later maintenance workload is excessive.
Vision SLAM also in development, is also not applied in actual scene at present, and vision SLAM positioning needs
Want substantial amounts of characteristic point.The power of illumination, characteristic point number vision SLAM can all be produced serious influence, cause positioning not
Phenomenon that is accurate or can not positioning.But vision SLAM advantage is only to need camera, low cost.
Laser SLAM needs a laser range finder, does not have any destruction to ground environment.Surrounding environment when building figure
Feature be all finally plotted to by the extracting method extraction of correlation in map, and can also be realized in real time by online composition
The function of composition and real-time update.But in this high degree of symmetry in garage, feature is single and few environment in, the SLAM of foundation
Map is likely to result in AGV locations of mistake or can not be properly positioned AGV position.Still further aspect, due to garage overall situation
The characteristics of so that can not be realized during figure is built closed loop cause map many overlapping places appearing above for establishing with
As in the navigation that can not be applied to AGV, but after using this paper innovatory algorithm, above the problem of can be resolved.
Mixed positioning is usually the methods of positioning using magnetic nail with laser triangulation positioning or magnetic stripe with laser triangulation, anti-
In the case that tabula rasa is blocked by barrier, AGV is switched to follows closely positioning or magnetic stripe positioning with magnetic.This kind of method solves instead very well
The situation that tabula rasa can not position after being blocked, but not only workload is big for laying magnetic stripe or injection magnetic nail on the ground, and
And safeguard more difficult, and there is higher requirement to whole system.
The content of the invention
The present invention is proposed laser figure optimization SLAM and laser triangulation algorithm primarily directed to the deficiency of above-mentioned background technology
The method blended, the closed loop detection function for so realizing figure optimization SLAM cause the map accuracy established and precision all
Religion is high.And navigator fix when run into the situation that reflector blocked by barrier, the map established can be used to swash with current
The data that light observes are matched and then positioned；When causing positioning inaccurate due to garage symmetrical environment as SLAM
Wait, first can estimate AGV regions with odometer, calculate AGV pose with triangle polyester fibre algorithm inside zonule；When
, now can be with the information content of odometer come the current of rough estimate AGV when SLAM and triangle polyester fibre are all simultaneously inaccurate
Pose, AGV reached can with SLAM or triangle polyester fibre to calculate pose when, then by now pose carry out again
Estimation.
The present invention is that the function of realizing above-mentioned algorithm will adopt the following technical scheme that.
To realize that our schemes of above-mentioned algorithm need to use following equipment：Four motorized wheels omnidirectional drives AGV, PC mono-
Platform (needs to install linux system, 4G internal memories, i7 processors, four cores, hard disk size 500G), SICK151 laser radars (point
Resolution is 0.5 °, scan frequency 50Hz), remote-control handle or keyboard, several reflectors, odometer.Whole experiment porch figure
Such as Fig. 1.
Four motorized wheels omnidirectional AGV includes：Vehicle frame, four movable motors, four steering motors, four lifting motors,
Four walking drivers, four rotating drivers, four lift actuators, dynamic lithium battery, battery charger, reductor, connect
Nearly switch, emergency switch, gyroscope, laser radar, ultrasonic sensor.
Triangle polyester fibre algorithm
In environment according to a number of reflector of certain regular arrangement or reflecting pole, and these reflectors
Coordinate is stored in advance in inside computer, and its schematic diagram is as shown in Figure 2.When AGV is run in the environment, by not disconnecting
The information reflected by three or more than 3 known location reflectors, it is possible to determine AGV position.Principle therein
Figure is as shown in Figure 3.
Assuming that the position of three reflectors is (x1, y1) respectively, (x2, y2), (x3, y3), which is measured, to be reviewed between plate and laser
Angle be θ 1, θ 2, θ 3, make A=θ 1- θ 2, B=θ 1- θ 3, circle O12 represents the circle by dolly V, R1 and R2, and O13 represents warp
Dolly V, R1 and R3 circle is crossed, then central coordinate of circle O12 and O13 can be expressed as：
(x12,y12)=(x1+P1,y1+Q1) (1)
(x13,y13)=(x1+P2,y2+Q2) (2)
Have in formula (1) and formula (2)：
The coordinate of plate is reviewed, it is known that then can be in the hope of L12, L13 value, what E1 was represented is the angle between L12 and x-axis,
What E3 was represented is the angle between L12 and L13.
By equation (1) and (2), O12 and O13 can be represented as follows by us：
By the way that to solving an equation (5) and equation (6), we can calculate the pose of dolly：
(xv,yv)=[x1+k(Q2-Q1),y1-k(P2-P1)] (7)
Have in formula (7)：
Improved figure optimized algorithm merges with triangle polyester fibre algorithm：
Figure optimization SLAM theory is that the pose of robot and its observation data are expressed as to the form of graph structure, then not
The disconnected configuration for removing optimization figure, solution of the final optimal figure configuration i.e. as SLAM problems.
The essence of SLAM problems is that the states such as robot pose and environment are estimated using sensor, is optimized based on figure
The general framework of method by general figure Optimization Framework as shown in figure 4, be divided into front-end and back-end.Front end is responsible for building graph structure,
It is responsible for optimization figure configuration in rear end
The model structure of figure is illustrated shown in Fig. 5.Ci is represented to touch a pose node, kept to the side between two nodes to link.
During figure is built, associating between the Data Datas such as c1 and c2, c2 and c3 can be established according to the data of sensor,
The constraint that can be established between two nodes (c1 and cn) of closed loop.
Closed loop detection is the important ring in figure optimization SLAM, because it can significantly improve differing for structure map
Cause property, and the detection of laser closed loop is difficult to match correct closed loop in the scene of dullness, particularly in this symmetry in garage
Closed loop is likely to correctly to match in environment higher, feature is similar.If the high environment of symmetry is divided into different
Region, then can carries out closed loop detection with different regions.SLAM methods after improvement need in advance cloth in the environment
Put some and review plate, environment is then divided into many regions with triangle polyester fibre algorithm and these regions are numbered.
Build map when add in the environment review plate and when AGV X or Y-direction occur 5m it is mobile when, I
Just demarcate a region, whole environment is then divided into each different region, and inside each different region
The arrangement of reflector is different, can thus overcome the characteristics of can not correctly matching closed loop, Orientation on map is utilized in AGV
Can be according to the reflector inside region come coarse localization AGV position, then further according to laser data when inaccurate
Matching map so as to being accurately positioned AGV position.The SLAM of figure optimization after improvement framework is as shown in Figure 6.
Figure optimization front end is established：
Restriction relation of the pose between some is mainly established in the foundation of front end, and this has come mainly by laser scanning matching
The relative pose that the problem of being matched into, laser scanning mainly passes through between the frame laser data of general solution two converts, in scan matching
In, in order to try to achieve the pose of robot, it may be considered that with Probability model modeling, understood using bayesian probability model.
p(ci|ci-1,u,m,z)∝p(z|ci,m)p(ci|ci-1,u) (10)
Wherein, the current pose to be asked of ci is represented robot, what ci-1 was represented is previous moment robot pose, m tables
What is shown is the known map that above some frame laser datas are formed, and what z was represented is laser observations data to be matched, and what u was represented is
Control input amount in motion model.
Probabilistic model in formula (10) has been broken down into the product of observation model and motion model.Motion model is defeated by controlling
Enter or odometer obtains AGV poses ci prior probability, observation model is to ask to ask sight under conditions of given map and AGV pose
Survey data z likelihood value.Observation model is complex, and observation model is reduced to：
What zj was represented in formula (11) is j-th of direction measurement in laser scanner, and above formula is carried out to ask logarithm to obtain
Arrive：
In order to improve matching speed, the method that employs Olson, the different look-up table of structure Resolutions, to high score
The thin look-up table progress of resolution is down-sampled to obtain the thick look-up table of low resolution, and the value of low resolution grid takes corresponding high-resolution
In maximum, under basic step enters：
Each point on first in hunting zone uses the look-up table of low resolution, it is assumed that the value at i-th of grid
Maximum, maximum are represented with Li.
Secondly, to the grid i in above in high-resolution look-up table to each grid be ranked up, it is assumed that wherein most
Big value is Hi.Want to contaminate Hi<Li, because can too high estimation using low resolution look-up table.
Then, low resolution grid value is found out again and is more than Hi grid, and grid is counted with high-resolution look-up table
Calculate, if it find that there is higher value, then replace renewal Hi.
Finally, it is required in grid position corresponding to the maximum Hi in whole high resolution search region.
Closed loop detects：
Build during figure, when AGV X-direction or Y-direction is there occurs during 5m change, be considered as a new area
Domain, the region is numbered and the relation reviewed between plate inside posting field, then, by these transformations to the overall situation
In coordinate system.AGV by region detection and is reviewed to be considered as generating when plate matches to obtain the ID of repetition and closed in the zone
Ring signal, this closed signal is passed to the function of closed loop detection progress closed loop in SLAM.
Figure optimization rear end optimization：
The relation on node and side is established in front end data accumulation, rear end just needs to carry out the configuration of figure optimization.If c=
(c1, c2.....cn) is the node pose of the optimization required for us, and robot has because in the plane：
For the constraint between ith and jth node therein, i.e. two node Relative Transformation relations have：
Wherein Ri is the cosine matrix related to θ i, is expressed as：
The error function on the side of two node compositions, that is, observed quantity zij and desired valueError can be expressed as：
Then the overall error function error obtained is：
Ω ij are the covariance matrixes between ci and cj in formula.The target that we optimize is exactly to find optimal pose matching
So that global error function is minimum.
For this optimization problem, the numerical solution of above formula is tried to achieve using Gaussian weighting marks method.Iterative strategy is exactly will be by mistake
Difference function linearizes near initialization estimate, asks the optimal solution of linear system to carry out approximate evaluation, is gradually connect by iteration
The optimal solution of nearly nonlinear system
And then we can obtain：
Above formula is further converted to：
In view of Jij, Hij's, bij is openness, and eij is only relevant with node ci and cj, therefore has：
After carrying out derivation to formula (20), optimization problem finally translates into solution：
H Δs c=-b (25)
New approximate solution after solution is：
Each iteration decomposes solution linear equation, Ran Houyong after initial point obtains H and b, using sparse Cholesky
Δ c updates c*, as shown in Figure 7 the step of solution.Final experimental result is as shown in Figure 8 and Figure 9.
Brief description of the drawings
Fig. 1 is the platform figure of integral experiment
Fig. 2 is reflector schematic layout diagram in environment
Fig. 3 is laser triangulation location algorithm schematic diagram
Fig. 4 is standard drawing optimization SLAM frame diagrams
Fig. 5 is the modular concept figure of figure
Fig. 6 is figure optimization SLAM frame diagrams after improving
Fig. 7 is allocation optimum resolution principle figure
Fig. 8 is that standard drawing optimization SLAM builds figure result figure
Fig. 9 is that improvement figure optimization SLAM builds figure result figure
Embodiment
The present invention is that the function of realizing above-mentioned algorithm will use following implementation process.
To realize that our schemes of above-mentioned algorithm need to use following equipment：Four motorized wheels omnidirectional drives AGV, PC mono-
(needing to install linux system, 4G internal memories, i7 processors, four cores, hard disk size 500G), SICK151 laser radars (are differentiated
Rate is 0.5 °, scan frequency 50Hz), remote-control handle or keyboard, several reflectors, odometer.Whole experiment porch figure is such as
Fig. 1.
Four motorized wheels omnidirectional AGV includes：Vehicle frame, four movable motors, four steering motors, four lifting motors, four
Walking driver, four rotating drivers, four lift actuators, dynamic lithium battery, battery charger, reductor, close to opening
Pass, emergency switch, gyroscope, laser radar, ultrasonic sensor.
Triangle polyester fibre algorithm
In environment according to a number of reflector of certain regular arrangement or reflecting pole, and the coordinate of these reflectors
It is stored in advance in inside computer, its schematic diagram is as shown in Figure 2.When AGV is run in the environment, by constantly receiving three
The information that individual or more than 3 known location reflectors reflect, it is possible to determine AGV position.Schematic diagram therein is such as
Shown in Fig. 3.
Assuming that the position of three reflectors is (x1, y1) respectively, (x2, y2), (x3, y3) measures the folder reviewed between plate and laser
Angle is θ 1, θ 2, θ 3, makes A=θ 1- θ 2, B=θ 1- θ 3, and circle O12 represents the circle by dolly V, R1 and R2, and O13 is represented through too small
Car V, R1 and R3 circle, then central coordinate of circle O12 and O13 can be expressed as：
(x12,y12)=(x1+P1,y1+Q1) (1)
(x13,y13)=(x1+P2,y2+Q2) (2)
Have in formula (1) and formula (2)：
The coordinate of plate is reviewed, it is known that then can be in the hope of L12, L13 value, what E1 was represented is the angle between L12 and x-axis, E3 tables
What is shown is the angle between L12 and L13.
By equation (1) and (2), O12 and O13 can be represented as follows by us：
By the way that to solving an equation (5) and equation (6), we can calculate the pose of dolly：
(xv,yv)=[x1+k(Q2-Q1),y1-k(P2-P1)](7)
Have in formula (7)：
Improved figure optimized algorithm merges with triangle polyester fibre algorithm：
Figure optimization SLAM theory is that the pose of robot and its observation data are expressed as to the form of graph structure, is then constantly gone
Optimize the configuration of figure, solution of the final optimal figure configuration i.e. as SLAM problems.
The essence of SLAM problems is that the states such as robot pose and environment are estimated using sensor, based on figure optimization method
General framework as shown in figure 4, in general figure Optimization Framework is divided into front-end and back-end.It is responsible for building graph structure, rear end in front end
It is responsible for optimization figure configuration
The model structure of figure is illustrated shown in Fig. 5.Ci is represented to touch a pose node, kept to the side between two nodes to link.In structure
During building figure, associating between the Data Datas such as c1 and c2, c2 and c3 can be established according to the data of sensor, can also
The constraint established between two nodes (c1 and cn) of closed loop.
Closed loop detection is the important ring in figure optimization SLAM, because it can significantly improve the inconsistent of structure map
Property, and laser closed loop detection dullness scene in be difficult to match correct closed loop, particularly this symmetry in garage compared with
Closed loop is likely to correctly to match in environment high, feature is similar.If the high environment of symmetry is divided into different areas
Domain, then can carries out closed loop detection with different regions.SLAM methods after improvement need to arrange in the environment in advance
Some review plate, and environment then is divided into many regions with triangle polyester fibre algorithm and these regions are numbered.In structure
Added in the environment when building map review plate and when AGV X or Y-direction occur 5m it is mobile when, we
With regard to demarcating a region, whole environment is then divided into each different region, and it is anti-inside each different region
The arrangement of tabula rasa is different, can thus overcome the characteristics of can not correctly matching closed loop, AGV using Orientation on map not
Can be according to the reflector inside region come coarse localization AGV position, then further according to laser data when quasi-
Map is matched so as to be accurately positioned AGV position.The SLAM of figure optimization after improvement framework is as shown in Figure 6.
Figure optimization front end is established：
Restriction relation of the pose between some is mainly established in the foundation of front end, and this is completed mainly by laser scanning matching, is swashed
The relative pose that the problem of optical scanning matches mainly passes through between the frame laser data of general solution two converts, and in scan matching, is
Try to achieve the pose of robot, it may be considered that with Probability model modeling, understood using bayesian probability model.
p(ci|ci-1,u,m,z)∝p(z|ci,m)p(ci|ci-1,u) (10)
Wherein, the pose to be asked of the robot that ci is represented currently, what ci-1 was represented is previous moment robot pose, what m was represented
It is the known map that above some frame laser datas are formed, what z was represented is laser observations data to be matched, and what u was represented is motion
Control input amount in model.
Probabilistic model in formula (10) has been broken down into the product of observation model and motion model.Motion model by control input or
Person's odometer obtains AGV poses ci prior probability, and observation model is to ask to seek observation number under conditions of given map and AGV pose
According to z likelihood value.Observation model is complex, and observation model is reduced to：
What zj was represented in formula (11) is j-th of direction measurement in laser scanner, and above formula is carried out to ask logarithm to obtain：
In order to improve matching speed, the method that employs Olson, the different look-up table of structure Resolutions, to high-resolution
Thin look-up table carry out it is down-sampled obtain the thick look-up table of low resolution, the value of low resolution grid is taken in corresponding high-resolution
Maximum, under basic step enters：
Each point on first in hunting zone uses the look-up table of low resolution, it is assumed that and value is maximum at i-th of grid,
Maximum is represented with Li.
Secondly, to the grid i in above in high-resolution look-up table to each grid be ranked up, it is assumed that wherein maximum
For Hi.Want to contaminate Hi<Li, because can too high estimation using low resolution look-up table.
Then, low resolution grid value is found out again and is more than Hi grid, and grid is calculated with high-resolution look-up table, such as
Fruit finds there is higher value, then replaces renewal Hi.
Finally, it is required in grid position corresponding to the maximum Hi in whole high resolution search region.
Closed loop detects：
Build during figure, when AGV X-direction or Y-direction is there occurs during 5m change, be considered as a new region,
The region is numbered and the relation reviewed between plate inside posting field, then, these transformations sat to the overall situation
In mark system.AGV by region detection and is reviewed when plate matches to obtain the ID of repetition in the zone and is considered as generating closed loop
Signal, this closed signal is passed to the function of closed loop detection progress closed loop in SLAM.
Figure optimization rear end optimization：
The relation on node and side is established in front end data accumulation, rear end just needs to carry out the configuration of figure optimization.If c=(c1,
C2.....cn) be optimization required for us node pose, robot has because in the plane：
For the constraint between ith and jth node therein, i.e. two node Relative Transformation relations have：
Wherein Ri is the cosine matrix related to θ i, is expressed as：
The error function on the side of two node compositions, that is, observed quantity zij and desired valueError can be expressed as：
Then the overall error function error obtained is：
Ω ij are the covariance matrixes between ci and cj in formula.The target that we optimize is exactly to find optimal pose matching to cause
Global error function is minimum.
For this optimization problem, the numerical solution of above formula is tried to achieve using Gaussian weighting marks method.Iterative strategy is exactly by error letter
Number linearizes near initialization estimate, asks the optimal solution of linear system to carry out approximate evaluation, is moved closer to by iteration non-
The optimal solution of linear system
And then we can obtain：
Above formula is further converted to：
In view of Jij, Hij's, bij is openness, and eij is only relevant with node ci and cj, therefore has：
After carrying out derivation to formula (20), optimization problem finally translates into solution：
H Δs c=-b (25)
New approximate solution after solution is：
Each iteration decomposes solution linear equation after initial point obtains H and b, using sparse Cholesky, then with Δ c more
New c*, it is as shown in Figure 7 the step of solution.Final experimental result is as shown in Figure 8 and Figure 9.
Claims (1)
1. improved figure optimizes SLAM methods, it is characterised in that has merged laser triangulation algorithm and traditional figure optimization SLAM side
Method, key step are as follows：
1) according to the characteristics of environment, certain reflector is arranged in the environment；
2) AGV is allowed to be walked along figure place is built, establishing one using laser triangulation algorithm includes reflective plate features, straight line and turning
Characteristic pattern, and according to the change on X and Y-direction, region is subjected to independent numbering；
3) in the characteristic pattern that detection is established each region whether have it is identical, if, it is necessary to reflector is adjusted, then
Return to step 2) until the characteristics map for ensureing each zonule inside overall situation it is different, if each region is only
One is put into step 4)；
4) ROS node manager is opened, AGV driver is opened and returns to the program of AGV encoder datas；
5) configuration file of laser and the driver of laser are opened, it is ensured that the data of laser correctly return；
6) remote-control handle or keyboard are opened, and runs corresponding program on AGV, it is ensured that it is whole that AGV traversals can be remotely controlled
Individual environment；
7) the figure optimization SLAM programs after operational development, remote control AGV are traveled through to whole environment and are returned to AGV starting point；
8) terminate.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711075649.XA CN107741743A (en) | 2017-11-06 | 2017-11-06 | Improved figure optimizes SLAM methods |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711075649.XA CN107741743A (en) | 2017-11-06 | 2017-11-06 | Improved figure optimizes SLAM methods |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107741743A true CN107741743A (en) | 2018-02-27 |
Family
ID=61234083
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711075649.XA Withdrawn CN107741743A (en) | 2017-11-06 | 2017-11-06 | Improved figure optimizes SLAM methods |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107741743A (en) |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108981701A (en) * | 2018-06-14 | 2018-12-11 | 广东易凌科技股份有限公司 | A kind of indoor positioning and air navigation aid based on laser SLAM |
| CN109129468A (en) * | 2018-07-27 | 2019-01-04 | 广东工业大学 | A kind of mobile robot based on MYRIO platform |
| CN109631919A (en) * | 2018-12-28 | 2019-04-16 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of hybrid navigation map constructing method for merging reflector and occupying grid |
| CN109959937A (en) * | 2019-03-12 | 2019-07-02 | 广州高新兴机器人有限公司 | Localization method, storage medium and electronic equipment of the gallery environment based on laser radar |
| CN110530399A (en) * | 2019-09-06 | 2019-12-03 | 苏州寻迹智行机器人技术有限公司 | The wheel spacing modification method of two-wheel differential mobile robot odometer calibration |
| CN110749901A (en) * | 2019-10-12 | 2020-02-04 | 劢微机器人科技(深圳)有限公司 | Autonomous mobile robot, map splicing method and device thereof, and readable storage medium |
| CN111580530A (en) * | 2020-06-16 | 2020-08-25 | 福勤智能科技（昆山）有限公司 | Positioning method, positioning device, autonomous mobile equipment and medium |
| CN112099509A (en) * | 2020-09-24 | 2020-12-18 | 杭州海康机器人技术有限公司 | Map optimization method and device and robot |
| CN112147637A (en) * | 2019-06-28 | 2020-12-29 | 杭州海康机器人技术有限公司 | Robot repositioning method and device |
| CN112180940A (en) * | 2020-10-16 | 2021-01-05 | 三一机器人科技有限公司 | Mapping method and device for reverse positioning and vehicle operation method and device |
| CN112241002A (en) * | 2020-10-11 | 2021-01-19 | 西北工业大学 | Novel robust closed-loop detection method based on Karto SLAM |
| CN112629522A (en) * | 2020-12-31 | 2021-04-09 | 山东大学 | AGV positioning method and system with reflector and laser SLAM integrated |
| CN113514843A (en) * | 2021-07-09 | 2021-10-19 | 深圳华芯信息技术股份有限公司 | Multi-subgraph laser radar positioning method and system and terminal |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106153048A (en) * | 2016-08-11 | 2016-11-23 | 广东技术师范学院 | A kind of robot chamber inner position based on multisensor and Mapping System |
| CN106272423A (en) * | 2016-08-31 | 2017-01-04 | 哈尔滨工业大学深圳研究生院 | A kind of multirobot for large scale environment works in coordination with the method for drawing and location |
| CN106843222A (en) * | 2017-03-13 | 2017-06-13 | 苏州艾吉威机器人有限公司 | A kind of laser navigation AGV system of local paving reflecting plate |
-
2017
- 2017-11-06 CN CN201711075649.XA patent/CN107741743A/en not_active Withdrawn
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106153048A (en) * | 2016-08-11 | 2016-11-23 | 广东技术师范学院 | A kind of robot chamber inner position based on multisensor and Mapping System |
| CN106272423A (en) * | 2016-08-31 | 2017-01-04 | 哈尔滨工业大学深圳研究生院 | A kind of multirobot for large scale environment works in coordination with the method for drawing and location |
| CN106843222A (en) * | 2017-03-13 | 2017-06-13 | 苏州艾吉威机器人有限公司 | A kind of laser navigation AGV system of local paving reflecting plate |
Cited By (22)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108981701B (en) * | 2018-06-14 | 2022-05-10 | 广东易凌科技股份有限公司 | Indoor positioning and navigation method based on laser SLAM |
| CN108981701A (en) * | 2018-06-14 | 2018-12-11 | 广东易凌科技股份有限公司 | A kind of indoor positioning and air navigation aid based on laser SLAM |
| CN109129468A (en) * | 2018-07-27 | 2019-01-04 | 广东工业大学 | A kind of mobile robot based on MYRIO platform |
| CN109129468B (en) * | 2018-07-27 | 2021-03-12 | 广东工业大学 | Mobile robot based on MYRIO platform |
| CN109631919A (en) * | 2018-12-28 | 2019-04-16 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of hybrid navigation map constructing method for merging reflector and occupying grid |
| CN109631919B (en) * | 2018-12-28 | 2022-09-30 | 芜湖哈特机器人产业技术研究院有限公司 | Hybrid navigation map construction method integrating reflector and occupied grid |
| CN109959937B (en) * | 2019-03-12 | 2021-07-27 | 广州高新兴机器人有限公司 | Laser radar-based positioning method for corridor environment, storage medium and electronic equipment |
| CN109959937A (en) * | 2019-03-12 | 2019-07-02 | 广州高新兴机器人有限公司 | Localization method, storage medium and electronic equipment of the gallery environment based on laser radar |
| CN112147637A (en) * | 2019-06-28 | 2020-12-29 | 杭州海康机器人技术有限公司 | Robot repositioning method and device |
| CN110530399A (en) * | 2019-09-06 | 2019-12-03 | 苏州寻迹智行机器人技术有限公司 | The wheel spacing modification method of two-wheel differential mobile robot odometer calibration |
| CN110749901A (en) * | 2019-10-12 | 2020-02-04 | 劢微机器人科技(深圳)有限公司 | Autonomous mobile robot, map splicing method and device thereof, and readable storage medium |
| CN110749901B (en) * | 2019-10-12 | 2022-03-18 | 劢微机器人科技(深圳)有限公司 | Autonomous mobile robot, map splicing method and device thereof, and readable storage medium |
| CN111580530B (en) * | 2020-06-16 | 2021-10-26 | 福勤智能科技（昆山）有限公司 | Positioning method, positioning device, autonomous mobile equipment and medium |
| CN111580530A (en) * | 2020-06-16 | 2020-08-25 | 福勤智能科技（昆山）有限公司 | Positioning method, positioning device, autonomous mobile equipment and medium |
| CN112099509A (en) * | 2020-09-24 | 2020-12-18 | 杭州海康机器人技术有限公司 | Map optimization method and device and robot |
| CN112099509B (en) * | 2020-09-24 | 2024-05-28 | 杭州海康机器人股份有限公司 | Map optimization method and device and robot |
| CN112241002A (en) * | 2020-10-11 | 2021-01-19 | 西北工业大学 | Novel robust closed-loop detection method based on Karto SLAM |
| CN112241002B (en) * | 2020-10-11 | 2022-10-18 | 西北工业大学 | Novel robust closed-loop detection method based on Karto SLAM |
| CN112180940A (en) * | 2020-10-16 | 2021-01-05 | 三一机器人科技有限公司 | Mapping method and device for reverse positioning and vehicle operation method and device |
| CN112629522A (en) * | 2020-12-31 | 2021-04-09 | 山东大学 | AGV positioning method and system with reflector and laser SLAM integrated |
| CN112629522B (en) * | 2020-12-31 | 2023-04-11 | 山东大学 | AGV positioning method and system with reflector and laser SLAM integrated |
| CN113514843A (en) * | 2021-07-09 | 2021-10-19 | 深圳华芯信息技术股份有限公司 | Multi-subgraph laser radar positioning method and system and terminal |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107741743A (en) | Improved figure optimizes SLAM methods | |
| Akai et al. | Robust localization using 3D NDT scan matching with experimentally determined uncertainty and road marker matching | |
| Pfrunder et al. | Real-time autonomous ground vehicle navigation in heterogeneous environments using a 3D LiDAR | |
| Schuster et al. | Landmark based radar SLAM using graph optimization | |
| Kummerle et al. | Autonomous driving in a multi-level parking structure | |
| CN105865449B (en) | Hybrid positioning method of mobile robot based on laser and vision | |
| CN104914865A (en) | Transformer station inspection tour robot positioning navigation system and method | |
| Konolige et al. | Mapping, navigation, and learning for off‐road traversal | |
| CN104236548A (en) | Indoor autonomous navigation method for micro unmanned aerial vehicle | |
| US20220270268A1 (en) | Mapping and localization system for autonomous vehicles | |
| Usher et al. | Visual servoing of a car-like vehicle-an application of omnidirectional vision | |
| Mueller et al. | GIS-based topological robot localization through LIDAR crossroad detection | |
| Adachi et al. | Visual navigation using a webcam based on semantic segmentation for indoor robots | |
| CN114998276B (en) | Robot dynamic obstacle real-time detection method based on three-dimensional point cloud | |
| Wu et al. | Infrastructure-free hierarchical mobile robot global localization in repetitive environments | |
| Niewola et al. | PSD–probabilistic algorithm for mobile robot 6D localization without natural and artificial landmarks based on 2.5 D map and a new type of laser scanner in GPS-denied scenarios | |
| Takeuchi et al. | Robust localization method based on free-space observation model using 3D-map | |
| Kodagoda et al. | IMMPDAF approach for road-boundary tracking | |
| Ahn et al. | A particle filter localization method using 2D laser sensor measurements and road features for autonomous vehicle | |
| Guo et al. | Occupancy grid based urban localization using weighted point cloud | |
| Huang et al. | Probabilistic lane estimation for autonomous driving using basis curves | |
| Song et al. | Critical rays self-adaptive particle filtering SLAM | |
| Nie et al. | A survey of extrinsic parameters calibration techniques for autonomous devices | |
| Zeng et al. | An Indoor 2D LiDAR SLAM and Localization Method Based on Artificial Landmark Assistance | |
| Yu et al. | Road curbs detection based on laser radar |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| CB02 | Change of applicant information | ||
| CB02 | Change of applicant information |
Address after: 518000 15 / F, block a, building 6, Shenzhen International Innovation Valley, Dashi Road, Nanshan District, Shenzhen City, Guangdong Province Applicant after: SHENZHEN JINGZHI MACHINE Co.,Ltd. Address before: 518000 Beek science and technology building, 9 research road, Nanshan District hi tech Development Zone, Guangdong, Shenzhen, 1101-B Applicant before: SHENZHEN JINGZHI MACHINE Co.,Ltd. |
|
| WW01 | Invention patent application withdrawn after publication | ||
| WW01 | Invention patent application withdrawn after publication |
Application publication date: 20180227 |