CN107272673A - SLAM rear ends track optimizing method based on pose chain model - Google Patents
SLAM rear ends track optimizing method based on pose chain model Download PDFInfo
- Publication number
- CN107272673A CN107272673A CN201710353614.1A CN201710353614A CN107272673A CN 107272673 A CN107272673 A CN 107272673A CN 201710353614 A CN201710353614 A CN 201710353614A CN 107272673 A CN107272673 A CN 107272673A
- Authority
- CN
- China
- Prior art keywords
- pose
- closed loop
- chain model
- optimization
- node
- 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.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
Abstract
The present invention relates to the technical field of full autonomous mobile robot, more particularly, to the SLAM rear ends track optimizing method based on pose chain model.SLAM is considered as the key for realizing real full autonomous mobile robot, it is broadly divided into front end pretreatment and rear end optimizes two parts, front end pretreatment is analysis integrated by being carried out to various sensing datas, initial estimation map and positional information are provided, rear end optimization is to describe initial estimation and probability constraintses using graph model, initial estimation is optimized with the method for optimization, that realizes higher precision builds figure and positioning.This algorithm has introduced the pose chain model for having strict sequential order relation between node, the problem of general SLAM rear ends optimisation technique irrealizable online real-time optimization being solved using the sparse characteristic of pose chain model, the required precision of application can be also met simultaneously, a kind of fast and effectively solution is provided to solve SLAM rear ends optimization problem.
Description
Technical field
The present invention relates to the technical field of full autonomous mobile robot, more particularly, to based on pose chain model
SLAM rear ends track optimizing method.
Background technology
SLAM (while positioning is with building figure) is considered as the key for realizing real full autonomous mobile robot, before being broadly divided into
End pretreatment and rear end optimize two parts, and by being carried out to various sensing datas, analysis integrated there is provided initial for front end pretreatment
Estimate map and positional information, rear end optimization is to describe initial estimation and probability constraintses using graph model, with the side of optimization
Method is optimized to initial estimation, and that realizes higher precision builds figure and positioning.General SLAM rear ends track optimizing algorithm is all
The data that front end is obtained extract a target letter to be optimized after all obtaining according to the factor graph or pose figure of foundation
Number, tries to achieve renewal step-length, this kind of algorithm belongs to offline optimization, and true using least-squares algorithm by nonlinear equation linearisation
The difference of positive SLAM systems and general three-dimensional reconstruction and location technology, which is that, to be obtained in real time when motion
The three-dimensional of surrounding environment builds figure and accurate location, so the processing scheme of offline optimization simply afterwards, for really application
For without too big value.
The content of the invention
The present invention is after overcoming at least one defect described in above-mentioned prior art there is provided the SLAM based on pose chain model
Track optimizing method is held, online real-time optimization is realized.
The technical scheme is that：The pose chain model for having strict sequential order relation between node is employed, is closed using sequential
System often detects a closed loop, just carries out a suboptimization to the track in closed loop, so avoids as other excellent algorithms
Must be when having gathered the processing lag issues just optimized after all data, it is possible to achieve on-line optimization.
Specifically, the SLAM rear ends track optimizing method based on pose chain model, wherein, comprise the following steps：
S1. the initial estimation and probability constraintses obtained by front end sets up complete pose chain model；
S2. there is strict sequential priority to the node that the closed loop portion of pose chain model is done in track optimizing, pose chain
Point, for last node in each closed loop, its pose conversion between closed loop start node is calculated, as whole
The overall error of track in individual closed loop, first does exploded by this error using interpolating function, is decomposed into the bar with closed loop inner edge
The local updating amount of number equal parts, then this local updating amount is assigned to using the multiplying in Lie group corresponding in closed loop
Bian Shang；
S3. two adjacent pose points are directed to again, and obtained local updating amount is distributed with algorithm and goes to update original two pose
The pose of point, calculates more accurate relative pose, optimization is completed inside closed loop, due to closed loop portion and non-closed loop portion
In have a node be it is public, with before and after this node optimization pose conversion as non-closed loop portion accumulative overall error simultaneously
Made to be assigned in a like fashion on each node of non-closed loop portion, complete the optimization of whole piece track.
This algorithm has introduced the pose chain model for having strict sequential order relation between node, it is believed that the closed loop detection module institute of front end
The ratio of precision initial estimation of acquisition is more credible.
The common trait of this algorithm and general rear end optimized algorithm is：Initially set up expression initial estimation and probability about
The graph model of beam, using graph model calculate initial estimation and the more accurate estimate that calculates otherwise it
Overall error, each several part of track is rationally distributed to using certain mathematical method by difference, and the present invention is different from general rear end rail
Mark optimized algorithm is characterised by：What is used is not the pose graph model that other algorithms are used, and selects more sparse pose
Chain model, pose chain model with cause this algorithm can solve the problem that rear end optimization huge amount of calculation caused by optimal speed it is slow
The problems such as, meanwhile, general track optimizing algorithm needs that overall error could be calculated after all data have been gathered, be all from
Line algorithm, and due to there is strict sequential relationship between the node in pose chain model, if run into closed loop can just start it is excellent
Change, this makes it possible on-line optimization.Therefore, it is possible to meet the on-line optimization demand being normally applied.
Further, more sparse pose chain model is employed.Algorithm is set up reaches one in current sensor accuracy
On the accurate basis of calibration, therefore excessive closed loop detection need not be done reduce track drift, compared to being traditionally used for rear end
Wanted for the pose graph model of track optimizing it is sparse a lot, solve general SLAM rear ends optimisation technique irrealizable online
The problem of real-time optimization, while can also meet the required precision of application, one kind is provided soon to solve SLAM rear ends optimization problem
Fast effective solution.
Compared with prior art, beneficial effect is：The beneficial effects of the invention are as follows employ between node to have strict sequential order pass
The pose chain model of system, on-line optimization is realized using sequential relationship, while utilizing the openness so that calculating speed of graph model
Have relative to general optimized algorithm and significantly lifted.In general it is difficult to take into account that speed and precision, which are, and this algorithm speed is carried
Sacrifice very little after rising in precision, it may be possible to meet application demand.
Brief description of the drawings
Fig. 1 is the pose chain model used in the present invention.
Fig. 2 is the initial estimation and real trace schematic diagram of odometer of the present invention.
Fig. 3 is that the present invention calculates local updating amount schematic diagram according to initial estimation and real trace.
Fig. 4 is the present invention according to local updating gauge point counting cloth renewal amount schematic diagram.
Embodiment
Accompanying drawing being given for example only property explanation, it is impossible to be interpreted as the limitation to this patent；It is attached in order to more preferably illustrate the present embodiment
Scheme some parts to have omission, zoom in or out, do not represent the size of actual product；To those skilled in the art,
Some known features and its explanation may be omitted and will be understood by accompanying drawing.Being given for example only property of position relationship described in accompanying drawing
Explanation, it is impossible to be interpreted as the limitation to this patent.
It is as shown in figure 1, present invention employs pose chain SLAM rear ends Optimized model, the characteristics of model：Between node
There are strict sequential relationship, graph model than sparse.
The pose of triangular representation robot, while representing there are two kinds of sides in the relative pose between connected pose, model：Even
Continuous side, connects t, t+1 moment pose；Closed loop side, connection t, t+k (k>1) dotted portion is closed loop side in moment pose, figure.
Fig. 2 is the initial estimation and real trace schematic diagram of odometer of the present invention.
This algorithm optimization is broadly divided into two parts, and closed loop portion is optimized first, and non-closed loop is directed to after the completion of optimization
The change that part optimizes front and rear pose with closed loop portion common point is optimized to non-closed loop portion, and two parts optimization uses phase
Same mode, is mainly carried out in three steps：
(1) local updating amount is calculated：Such as A in Fig. 3nWith DnBetween five black triangles be respectively five, left side side
Local updating amount.
(2) distributed update amount is calculated：Such as A in Fig. 4nWith DnBetween five black triangles correspond to the left side with dotted line
Five distributed update amounts when upper amount is five, the left side.
(3) relative pose between node to be optimized is recalculated so that AnConverge to Dn；
This algorithm is mainly characterized by：Often detect a closed loop and begin to optimization, this feature ensure that online excellent
The reduction on closed loop side causes graph model more sparse in the possibility of change, graph model so that calculating speed is improved significantly, and closes
Overall optimization precision is maintained to the feedback mechanism that non-closed loop portion is optimized again after the completion of loop section optimization.
Obviously, the above embodiment of the present invention is only intended to clearly illustrate example of the present invention, and is not pair
The restriction of embodiments of the present invention.For those of ordinary skill in the field, may be used also on the basis of the above description
To make other changes in different forms.There is no necessity and possibility to exhaust all the enbodiments.It is all this
Any modifications, equivalent substitutions and improvements made within the spirit and principle of invention etc., should be included in the claims in the present invention
Protection domain within.
Claims (2)
1. the SLAM rear ends track optimizing method based on pose chain model, it is characterised in that comprise the following steps：
S1. the initial estimation and probability constraintses obtained by front end sets up complete pose chain model；
S2. there is point of strict sequential priority to the node that the closed loop portion of pose chain model is done in track optimizing, pose chain,
For last node in each closed loop, its pose conversion between closed loop start node is calculated, as whole
The overall error of track in closed loop, first does exploded using interpolating function by this error, is decomposed into the bar number with closed loop inner edge
The local updating amount of equal parts, then this local updating amount using the multiplying in Lie group is assigned to corresponding side in closed loop
On；
S3. two adjacent pose points are directed to again, and obtained local updating amount is distributed with algorithm and goes to update original two poses point
Pose, calculates more accurate relative pose, optimization is completed inside closed loop, due to having in closed loop portion and non-closed loop portion
One node be it is public, with before and after this node optimization pose conversion as non-closed loop portion accumulative overall error and by its
Make to be assigned in a like fashion on each node of non-closed loop portion, complete the optimization of whole piece track.
2. the SLAM rear ends track optimizing method according to claim 1 based on pose chain model, it is characterised in that：It is described
Step S1 in, described pose chain model is sparse pose chain model.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710353614.1A CN107272673A (en) | 2017-05-18 | 2017-05-18 | SLAM rear ends track optimizing method based on pose chain model |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710353614.1A CN107272673A (en) | 2017-05-18 | 2017-05-18 | SLAM rear ends track optimizing method based on pose chain model |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107272673A true CN107272673A (en) | 2017-10-20 |
Family
ID=60063992
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710353614.1A Pending CN107272673A (en) | 2017-05-18 | 2017-05-18 | SLAM rear ends track optimizing method based on pose chain model |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107272673A (en) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108537287A (en) * | 2018-04-18 | 2018-09-14 | 北京航空航天大学 | Image closed loop detection method and device based on graph model |
| CN109343540A (en) * | 2018-11-30 | 2019-02-15 | 广东工业大学 | A kind of rear end SLAM track optimizing method based on winding detection |
| CN111258309A (en) * | 2020-01-15 | 2020-06-09 | 上海锵玫人工智能科技有限公司 | Fire extinguishing method for urban fire-fighting robot |
| CN111568305A (en) * | 2019-02-18 | 2020-08-25 | 北京奇虎科技有限公司 | Method and device for processing relocation of sweeping robot and electronic equipment |
| CN111708010A (en) * | 2019-03-01 | 2020-09-25 | 北京图森智途科技有限公司 | Mobile equipment positioning method, device and system and mobile equipment |
| CN111862163A (en) * | 2020-08-03 | 2020-10-30 | 湖北亿咖通科技有限公司 | Trajectory optimization method and device |
| CN112052300A (en) * | 2020-08-05 | 2020-12-08 | 浙江大华技术股份有限公司 | SLAM back-end processing method, device and computer readable storage medium |
| CN112925318A (en) * | 2021-01-25 | 2021-06-08 | 西南交通大学 | Calculation method applied to intelligent robot moving path |
| CN114286924A (en) * | 2019-09-03 | 2022-04-05 | 宝马汽车股份有限公司 | Method and device for determining vehicle trajectory |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104850615A (en) * | 2015-05-14 | 2015-08-19 | 西安电子科技大学 | G2o-based SLAM rear end optimization algorithm method |
| CN104851094A (en) * | 2015-05-14 | 2015-08-19 | 西安电子科技大学 | Improved method of RGB-D-based SLAM algorithm |
| 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 |
| WO2017060050A1 (en) * | 2015-10-09 | 2017-04-13 | Volkswagen Aktiengesellschaft | Fusion of position data by means of pose graph |
| CN106683169A (en) * | 2016-12-09 | 2017-05-17 | 华南理工大学 | Sparse local decomposition and restructuring algorithm of joint motion sensing |
-
2017
- 2017-05-18 CN CN201710353614.1A patent/CN107272673A/en active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104850615A (en) * | 2015-05-14 | 2015-08-19 | 西安电子科技大学 | G2o-based SLAM rear end optimization algorithm method |
| CN104851094A (en) * | 2015-05-14 | 2015-08-19 | 西安电子科技大学 | Improved method of RGB-D-based SLAM algorithm |
| WO2017060050A1 (en) * | 2015-10-09 | 2017-04-13 | Volkswagen Aktiengesellschaft | Fusion of position data by means of pose graph |
| 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 |
| CN106683169A (en) * | 2016-12-09 | 2017-05-17 | 华南理工大学 | Sparse local decomposition and restructuring algorithm of joint motion sensing |
Non-Patent Citations (1)
| Title |
|---|
| GIJS DUBBELMAN 等: "COP-SLAM: Closed-Form Online Pose-Chain Optimization for Visual SLAM", 《IEEE TRANSACTIONS ON ROBOTICS》 * |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108537287A (en) * | 2018-04-18 | 2018-09-14 | 北京航空航天大学 | Image closed loop detection method and device based on graph model |
| CN109343540A (en) * | 2018-11-30 | 2019-02-15 | 广东工业大学 | A kind of rear end SLAM track optimizing method based on winding detection |
| CN111568305A (en) * | 2019-02-18 | 2020-08-25 | 北京奇虎科技有限公司 | Method and device for processing relocation of sweeping robot and electronic equipment |
| CN111568305B (en) * | 2019-02-18 | 2023-02-17 | 北京奇虎科技有限公司 | Method and device for processing relocation of sweeping robot and electronic equipment |
| CN111708010B (en) * | 2019-03-01 | 2024-04-12 | 北京图森智途科技有限公司 | Mobile equipment positioning method, device and system and mobile equipment |
| CN111708010A (en) * | 2019-03-01 | 2020-09-25 | 北京图森智途科技有限公司 | Mobile equipment positioning method, device and system and mobile equipment |
| CN114286924A (en) * | 2019-09-03 | 2022-04-05 | 宝马汽车股份有限公司 | Method and device for determining vehicle trajectory |
| CN114286924B (en) * | 2019-09-03 | 2024-04-23 | 宝马汽车股份有限公司 | Method and device for determining a vehicle track |
| CN111258309A (en) * | 2020-01-15 | 2020-06-09 | 上海锵玫人工智能科技有限公司 | Fire extinguishing method for urban fire-fighting robot |
| CN111862163B (en) * | 2020-08-03 | 2021-07-23 | 湖北亿咖通科技有限公司 | Trajectory optimization method and device |
| CN111862163A (en) * | 2020-08-03 | 2020-10-30 | 湖北亿咖通科技有限公司 | Trajectory optimization method and device |
| CN112052300A (en) * | 2020-08-05 | 2020-12-08 | 浙江大华技术股份有限公司 | SLAM back-end processing method, device and computer readable storage medium |
| CN112925318A (en) * | 2021-01-25 | 2021-06-08 | 西南交通大学 | Calculation method applied to intelligent robot moving path |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107272673A (en) | SLAM rear ends track optimizing method based on pose chain model | |
| CN104215249B (en) | Smoothening method of driving track | |
| CN104166989B (en) | Rapid ICP method for two-dimensional laser radar point cloud matching | |
| CN106443622A (en) | Distributed target tracking method based on improved joint probability data association | |
| CN103759732B (en) | A kind of centralized multisensor multiple hypotheis tracking method of angle information auxiliary | |
| CN107704432A (en) | A kind of adaptive Interactive Multiple-Model method for tracking target of transition probability | |
| CN101819041A (en) | Self-evolution ANFIS and UKF combined GPS/MEMS-INS integrated positioning error dynamic forecasting method | |
| CN107978147B (en) | KNN algorithm-based traffic flow abnormal data bidirectional detection and restoration method | |
| CN109059907A (en) | Track data processing method, device, computer equipment and storage medium | |
| CN107622507B (en) | Air target tracking method based on deep learning | |
| CN103714045A (en) | Information fusion estimation method for asynchronous multi-rate non-uniform sampled observation data | |
| CN105137412A (en) | Accurate fitting method of line segment features in 2D laser radar distance image | |
| CN111127523A (en) | Multi-sensor GMPHD self-adaptive fusion method based on measurement iteration update | |
| CN102999696A (en) | Capacity information filtering-based pure direction tracking method of noise-related system | |
| CN102706345A (en) | Maneuvering target tracking method based on fading memory sequential detector | |
| CN104021285B (en) | A kind of interactive multi-model method for tracking target with optimal motion pattern switching parameter | |
| CN114386599B (en) | Method and device for training trajectory prediction model and trajectory planning | |
| CN106570536B (en) | A kind of positioning using TDOA aims of systems high precision tracking filtering method | |
| Hu et al. | IMM fusion estimation with multiple asynchronous sensors | |
| Qian et al. | An INS/DVL integrated navigation filtering method against complex underwater environment | |
| CN111340853B (en) | Multi-sensor GMPHD self-adaptive fusion method based on OSPA iteration | |
| CN104750998B (en) | Target tracking method of passive multi-sensor based on density filter | |
| Cao et al. | Multi-feature fusion tracking based on a new particle filter | |
| CN107966697B (en) | Moving target tracking method based on progressive unscented Kalman | |
| Li et al. | Performance analysis of deep learning supported Kalman filter |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20171020 |