CN108180917B - Top map construction method based on pose graph optimization - Google Patents
Top map construction method based on pose graph optimization Download PDFInfo
- Publication number
- CN108180917B CN108180917B CN201711495070.9A CN201711495070A CN108180917B CN 108180917 B CN108180917 B CN 108180917B CN 201711495070 A CN201711495070 A CN 201711495070A CN 108180917 B CN108180917 B CN 108180917B
- Authority
- CN
- China
- Prior art keywords
- landmark
- coordinate system
- pose
- map
- image
- 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.)
- Active
Links
Images
Classifications
-
- 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
- G01C21/32—Structuring or formatting of map data
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention is suitable for the technical field of robot positioning, and provides a topmark map construction method based on pose map optimization, which comprises the following steps: s1, constructing a pose graph of a landmark coordinate system based on the shot image; s2, calculating the pose of each landmark coordinate system in the map coordinate system based on the optimization of the pose graph; and S3, calculating the coordinates of each landmark point in the map coordinate system based on the pose of each landmark coordinate system in the map coordinate system. According to the topmark map construction method based on pose graph optimization, the rectangular coordinate systems existing in different topmarks are used as pose transformation of the same rectangular coordinate system, the coordinates of the landmark points are calculated through pose graph optimization, compared with the pose graph constructed based on the camera posture, the construction precision of the mobile robot indoor map based on the topmarks can be greatly improved, the calculation is simple, and the result is more stable.
Description
Technical Field
The invention belongs to the technical field of robot positioning, and provides a calibration map construction method based on pose map optimization.
Background
With the development of society and the advancement of technology, mobile robots are increasingly involved in human daily lives, such as cleaning robots in homes, transfer robots in factories, and meal delivery robots in restaurants. The mobile robot needs to know the position of the mobile robot accurately to realize all the functions, and a prerequisite for the real-time positioning of the mobile robot is to establish a map, which is the key of the navigation and other intelligent behaviors of the mobile robot. The sensors commonly used for mobile robot mapping and positioning include cameras, laser radars and the like. The hardware cost of the laser radar is high, and the laser radar is not beneficial to wide popularization and promotion of the mobile robot. The positioning hardware using the camera has low cost and high positioning precision, and the vision-based positioning method is widely applied to indoor positioning. The vision-based indoor positioning firstly constructs an accurate indoor map for calculating the posture of a camera under an absolute coordinate system and planning the moving path of the robot. The accurate geometric map can be constructed by artificial landmarks, and the map can also be constructed by using environmental landmarks. Although the method based on the environmental landmark has good universality and does not need to manually lay an additional mark, the method is complex in calculation and poor in practicability. The artificial road sign usually has obvious uniqueness on visual characteristics such as color, shape and the like, and can be easily identified by a computer vision method. Among them, there is a method of laying artificial road signs on the roof, the roof environment is single and easy to extract, and the visual field of the camera is not easy to be disturbed, and it is widely used in indoor positioning.
However, in the existing top mark map construction method, the positions of unknown road marks in a map are sequentially calculated according to known road marks, errors in the calculation process are accumulated and propagated backwards, and when the number of top marks is large, the map construction result is inaccurate, so that positioning failure is caused.
Disclosure of Invention
The embodiment of the invention provides a calibration map construction method based on pose graph optimization, and aims to solve the problems that when the positions of unknown road signs in a map are calculated in sequence based on known road signs, errors in the calculation process are accumulated and spread backwards, and when the number of the top signs is large, the map construction result is inaccurate in the existing top sign map construction method.
The invention is realized in such a way that a calibration map construction method based on pose graph optimization comprises the following steps:
s1, constructing a pose graph of a landmark coordinate system based on the shot image;
s2, calculating the pose of each landmark coordinate system in the map coordinate system based on the optimization of the pose graph;
and S3, calculating the coordinates of each landmark point in the map coordinate system based on the pose of each landmark coordinate system in the map coordinate system.
Further, the step S1 includes the following steps:
s11, extracting all road signs in the shot image;
s12, judging whether the current key frame set is an empty set, if not, executing step S13, if so, identifying whether an initial landmark exists in the landmark set in the current image, if so, setting the initial landmark as an initial frame and storing the initial landmark into the key frame set, entering step S13, if not, executing step S11,
s13, identifying known road signs and unknown road signs in the image, traversing all known road signs in the image, judging whether the current known road signs and other known road signs in the image are associated, if not, establishing the connection relation between the current known road signs and other known road signs in the image, traversing all unknown road signs in the image, establishing the connection relation between the current unknown road signs and all known road signs in the image, and storing the current unknown road signs as a key frame; the known signposts are signposts which are already included in the pose graph; the unknown landmarks refer to landmarks which are not included in the pose graph;
and S14, traversing all paths in the map, and establishing a pose graph of the landmark motion.
Further, the method for establishing the connection relationship between the road sign points comprises the following steps:
calculating the attitude R of the camera under the landmark coordinate system of the landmark n based on the image coordinate of the landmark point in the landmark n and the world coordinate in the current landmark coordinate systemn、tn；
Calculating the attitude R of the camera under the landmark coordinate system of the landmark m based on the image coordinate of the landmark point in the landmark m and the world coordinate in the current landmark coordinate systemm、tm；
The rotation matrix from the landmark coordinate system n to the landmark coordinate system m is
The translation vector is
Pose transformation from landmark coordinate system n to landmark coordinate system m to (t)nm(0),tnm(1),atan2(Rnm(1,0),Rnm(0,0))。
Further, the calculation method of the posture of the camera under the landmark s coordinate system is as follows:
using affine transformation equations
Calculate a rotation matrix
And translation vector
Then removing the photographic depth factor to obtain a rotation matrix R and a translation vector t,wherein the content of the first and second substances,
xifor the image coordinates x of known road marking points in the road markings si，XwIs the world coordinate, M, of a known landmark in a landmark scamAn internal reference matrix of the camera.
Further, based on formula Xw＝R*X′w+ t to calculate the coordinates of each landmark point in the map coordinate system, where R is the rotation matrix of the current landmark coordinate system, t is the translation vector, X 'of the current landmark coordinate system'wThe coordinates of the landmark points in the landmark coordinate system.
According to the topmark map construction method based on pose graph optimization, the rectangular coordinate systems existing in different topmarks are used as pose transformation of the same rectangular coordinate system, the coordinates of the landmark points are calculated through pose graph optimization, compared with the pose graph constructed based on the camera posture, the construction precision of the mobile robot indoor map based on the topmarks can be greatly improved, the calculation is simple, and the result is more stable.
Drawings
Fig. 1 is a flowchart of a calibration map construction method based on pose graph optimization according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
Fig. 1 is a flowchart of a pose graph optimization-based scaled map construction method according to an embodiment of the present invention, where the method includes the following steps:
s1, constructing a pose graph of a landmark coordinate system based on the shot image;
in the embodiment of the invention, the road signs are arranged on the roof of the robot movement area, and the cameras are arranged in parallel to the roof and are used for shooting the road signs on the roof;
step S1 of the present invention specifically includes the following steps:
s11, extracting all road signs in the shot image by using an image extraction algorithm;
s12, judging whether the current key frame set is an empty set, if not, executing step S13, if so, identifying whether an initial landmark exists in the landmark set in the current image, if so, setting the initial landmark as an initial frame and storing the initial landmark into the key frame set, entering step S13, if not, executing step S11,
and S13, identifying known signposts and unknown signposts in the image, traversing all known signposts in the image, judging whether the current known signposts and other known signposts in the image are associated, if not, establishing the connection relation between the current known signposts and other known signposts in the image, traversing all unknown signposts in the image, establishing the connection relation between the current unknown signposts and all known signposts in the image, and storing the current unknown signposts as a key frame, wherein the known signposts refer to signposts already included in the pose image, and the unknown signposts refer to signposts not included in the pose image.
In the embodiment of the present invention, the method for establishing the connection relationship between the landmark points specifically includes:
calculating the attitude R of the camera under the landmark coordinate system of the landmark n based on the image coordinate and the world coordinate of the landmark point in the landmark nn、tn；
Calculating the attitude R of the camera under the landmark coordinate system of the landmark m based on the image coordinate and the world coordinate of the landmark point in the landmark mm、tm；
The rotation matrix from the landmark coordinate system n to the landmark coordinate system m is
The translation vector is
Pose transformation from landmark coordinate system n to landmark coordinate system m to (t)nm(0),tnm(1),atan2(Rnm(1,0),Rnm(0,0))。
In the embodiment of the invention, the calculation method of the posture of the camera under the landmark s coordinate system is as follows:
using affine transformation equations
Calculate a rotation matrix
And translation vector
Then removing the photographic depth factor to obtain a rotation matrix R and a translation vector t, wherein,
xifor the image coordinates x of known road marking points in the road markings si，XwIs the world coordinate, M, of a known landmark in a landmark scamAn internal reference matrix of the camera.
And S14, traversing all paths in the map, and establishing a pose graph of the landmark motion.
In the embodiment of the present invention, the map is based on a map coordinate system, all landmarks are included in the map coordinate system, and a landmark coordinate system is generally selected as the map coordinate system.
S2, calculating the pose of each landmark coordinate system in the map coordinate system based on the optimization of the pose graph;
in the embodiment of the invention, the pose model graph is described by an equation, and the equation is expressed as follows:
wherein x iskThe k nodes are the position information of the kth node, and the k nodes are the origin of the kth landmark coordinate system; z is a radical ofkObserving the obtained position information for the kth node; e.g. of the typekIs xkAnd zkThe error between; omega is an information matrix and is a covariance matrixReversing;
the error term f (x) is expressed as follows:
to e of the kth edgek(xk) Performing a first order Taylor expansion:
j abovekIs ekWith respect to xkThe matrix form is a lower Jacobian matrix, and an objective function of the kth edge is further expanded by:
Fk(xk+Δx)＝ek(xk+Δx)TΩkek(xk+Δx)
≈(ek+JkΔx)TΩk(ek+JkΔx)
≈Ck+2bkΔx+ΔxTHkΔx
wherein C iskIs a constant term, 2bkIs a coefficient of a first order term, HkCoefficient of quadratic term, F of the objective functionkThe value of change is Δ Fk＝2bkΔx+ΔxTHkΔx
Order to
The problem is then transformed into a solution of a linear equation: hkΔx＝-bk；
Solving for globally optimal x*And (2) substituting the initial value into F (x) to carry out iterative calculation, and finally calculating to obtain the pose of each key frame, namely the pose of each road mark coordinate system in a map coordinate system.
And S3, calculating the coordinates of each landmark point in the map coordinate system based on the pose of each landmark coordinate system in the map coordinate system.
In the embodiment of the invention, the formula X is used as the basisw＝R*X′w+ t to calculate the coordinates of each landmark point in the map coordinate system, where R is the rotation matrix of the current landmark coordinate system, t is the translation vector, X 'of the current landmark coordinate system'wThe coordinates of the landmark points in the landmark coordinate system.
According to the topmark map construction method based on pose graph optimization, the rectangular coordinate systems existing in different topmarks are used as pose transformation of the same rectangular coordinate system, the coordinates of the landmark points are calculated through pose graph optimization, compared with the pose graph constructed based on the camera posture, the construction precision of the mobile robot indoor map based on the topmarks can be greatly improved, the calculation is simple, and the result is more stable.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents and improvements made within the spirit and principle of the present invention are intended to be included within the scope of the present invention.
Claims (3)
1. A topmark map construction method based on pose map optimization is characterized by comprising the following steps:
s1, constructing a pose graph of a landmark coordinate system based on the shot image;
s2, calculating the pose of each landmark coordinate system in the map coordinate system based on the optimization of the pose graph;
s3, calculating the coordinates of each landmark point in the map coordinate system based on the pose of each landmark coordinate system in the map coordinate system;
the step S1 includes the following steps:
s11, extracting all road signs in the shot image;
s12, judging whether the current key frame set is an empty set, if not, executing step S13, if so, identifying whether an initial landmark exists in the landmark set in the current image, if so, setting the initial landmark as an initial frame and storing the initial landmark into the key frame set, entering step S13, if not, executing step S11,
s13, identifying known road signs and unknown road signs in the image, traversing all known road signs in the image, judging whether the current known road signs and other known road signs in the image are associated, if not, establishing the connection relation between the current known road signs and other known road signs in the image, traversing all unknown road signs in the image, establishing the connection relation between the current unknown road signs and all known road signs in the image, and storing the current unknown road signs as a key frame; the known signposts are signposts which are already included in the pose graph; the unknown landmarks refer to landmarks which are not included in the pose graph;
s14, traversing all paths in the map, and establishing a pose graph of landmark motion;
the method for establishing the connection relation between the road mark points comprises the following steps:
calculating the attitude R of the camera under the landmark coordinate system of the landmark n based on the image coordinate of the landmark point in the landmark n and the world coordinate in the current landmark coordinate systemn、tn；
Calculating the attitude R of the camera under the landmark coordinate system of the landmark m based on the image coordinate of the landmark point in the landmark m and the world coordinate in the current landmark coordinate systemm、tm；
The rotation matrix from the n-coordinate system of the landmark to the m-coordinate system of the landmark is
The translation vector is
Pose transformation from landmark n-coordinate system to landmark m-coordinate system to (t)nm(0),tnm(1),atan2(Rnm(1,0),Rnm(0,0)) )。
2. The topmark map construction method based on pose graph optimization as claimed in claim 1, wherein the calculation method of the pose of the camera under the landmark s coordinate system is as follows:
using affine transformation equations
Calculate a rotation matrix
And translation vector
Then removing the photographic depth factor to obtain a rotation matrix R and a translation vector t, wherein,
xiis the image coordinate, X, of the known road marking point s in the road markingwFor the world coordinate, M, of a known landmark point s in the landmarkCAn internal reference matrix of the camera.
3. The pose graph optimization-based topmark map construction method according to claim 1, wherein the pose graph optimization-based topmark map construction method is based on a formula Xw＝R*X′w+ t to calculate the coordinates of each landmark point in the map coordinate system, where R is the rotation matrix of the current landmark coordinate system, t is the translation vector, X 'of the current landmark coordinate system'wThe coordinates of the landmark points in the landmark coordinate system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711495070.9A CN108180917B (en) | 2017-12-31 | 2017-12-31 | Top map construction method based on pose graph optimization |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711495070.9A CN108180917B (en) | 2017-12-31 | 2017-12-31 | Top map construction method based on pose graph optimization |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108180917A CN108180917A (en) | 2018-06-19 |
| CN108180917B true CN108180917B (en) | 2021-05-14 |
Family
ID=62549470
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711495070.9A Active CN108180917B (en) | 2017-12-31 | 2017-12-31 | Top map construction method based on pose graph optimization |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108180917B (en) |
Families Citing this family (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111256676B (en) * | 2018-11-30 | 2022-02-11 | 杭州海康机器人技术有限公司 | Mobile robot positioning method, device and computer readable storage medium |
| CN109613549B (en) * | 2018-12-28 | 2023-04-07 | 芜湖哈特机器人产业技术研究院有限公司 | Laser radar positioning method based on Kalman filtering |
| CN109613547B (en) * | 2018-12-28 | 2022-05-27 | 芜湖哈特机器人产业技术研究院有限公司 | Method for constructing occupied grid map based on reflector |
| CN109613550B (en) * | 2018-12-28 | 2023-04-07 | 芜湖哈特机器人产业技术研究院有限公司 | Laser radar map construction and positioning method based on reflector |
| CN109612468A (en) * | 2018-12-28 | 2019-04-12 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of top mark map structuring and robot localization method |
| CN110954114B (en) * | 2019-11-26 | 2021-11-23 | 苏州智加科技有限公司 | Method and device for generating electronic map, terminal and storage medium |
| CN111161412B (en) * | 2019-12-06 | 2024-02-09 | 苏州艾吉威机器人有限公司 | Three-dimensional laser mapping method and system |
| CN111553342B (en) * | 2020-04-01 | 2023-08-08 | 深圳一清创新科技有限公司 | Visual positioning method, visual positioning device, computer equipment and storage medium |
| CN112070068A (en) * | 2020-10-13 | 2020-12-11 | 上海美迪索科电子科技有限公司 | Map construction method, device, medium and equipment |
| CN113688678B (en) * | 2021-07-20 | 2024-04-12 | 深圳市普渡科技有限公司 | Road sign multi-ambiguity processing method, robot and storage medium |
| CN116197889A (en) * | 2021-11-30 | 2023-06-02 | 珠海一微半导体股份有限公司 | Positioning method of ceiling vision robot |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103292804A (en) * | 2013-05-27 | 2013-09-11 | 浙江大学 | Monocular natural vision landmark assisted mobile robot positioning method |
| CN103528568A (en) * | 2013-10-08 | 2014-01-22 | 北京理工大学 | Wireless channel based target pose image measuring method |
| CN103886107A (en) * | 2014-04-14 | 2014-06-25 | 苏州市华天雄信息科技有限公司 | Robot locating and map building system based on ceiling image information |
| CN106651990A (en) * | 2016-12-23 | 2017-05-10 | 芜湖哈特机器人产业技术研究院有限公司 | Indoor map construction method and indoor map-based indoor locating method |
| CN107063258A (en) * | 2017-03-07 | 2017-08-18 | 重庆邮电大学 | A kind of mobile robot indoor navigation method based on semantic information |
-
2017
- 2017-12-31 CN CN201711495070.9A patent/CN108180917B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103292804A (en) * | 2013-05-27 | 2013-09-11 | 浙江大学 | Monocular natural vision landmark assisted mobile robot positioning method |
| CN103528568A (en) * | 2013-10-08 | 2014-01-22 | 北京理工大学 | Wireless channel based target pose image measuring method |
| CN103886107A (en) * | 2014-04-14 | 2014-06-25 | 苏州市华天雄信息科技有限公司 | Robot locating and map building system based on ceiling image information |
| CN106651990A (en) * | 2016-12-23 | 2017-05-10 | 芜湖哈特机器人产业技术研究院有限公司 | Indoor map construction method and indoor map-based indoor locating method |
| CN107063258A (en) * | 2017-03-07 | 2017-08-18 | 重庆邮电大学 | A kind of mobile robot indoor navigation method based on semantic information |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108180917A (en) | 2018-06-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108180917B (en) | Top map construction method based on pose graph optimization | |
| CN108225327B (en) | Construction and positioning method of top mark map | |
| WO2021232470A1 (en) | Multi-sensor fusion-based slam method and system | |
| CN107967457B (en) | Site identification and relative positioning method and system adapting to visual characteristic change | |
| CN106679648B (en) | Visual inertia combination SLAM method based on genetic algorithm | |
| Nieto et al. | Recursive scan-matching SLAM | |
| CN108917759A (en) | Mobile robot pose correct algorithm based on multi-level map match | |
| CN109186606B (en) | Robot composition and navigation method based on SLAM and image information | |
| CN112667837A (en) | Automatic image data labeling method and device | |
| CN109544636A (en) | A kind of quick monocular vision odometer navigation locating method of fusion feature point method and direct method | |
| CN109613548B (en) | Laser radar road sign map construction method based on graph optimization | |
| EP2904417A1 (en) | Method of determining a position and orientation of a device associated with a capturing device for capturing at least one image | |
| Chen et al. | Vision-based autonomous vehicle guidance for indoor security patrolling by a SIFT-based vehicle-localization technique | |
| CN108151713A (en) | A kind of quick position and orientation estimation methods of monocular VO | |
| CN111998862A (en) | Dense binocular SLAM method based on BNN | |
| CN105096341A (en) | Mobile robot pose estimation method based on trifocal tensor and key frame strategy | |
| CN109612468A (en) | A kind of top mark map structuring and robot localization method | |
| CN110749308A (en) | SLAM-oriented outdoor positioning method using consumer-grade GPS and 2.5D building models | |
| CN113239072A (en) | Terminal equipment positioning method and related equipment thereof | |
| Lin et al. | A sparse visual odometry technique based on pose adjustment with keyframe matching | |
| Khurana et al. | Extrinsic calibration methods for laser range finder and camera: A systematic review | |
| Mariottini et al. | An accurate and robust visual-compass algorithm for robot-mounted omnidirectional cameras | |
| WO2022111723A1 (en) | Road edge detection method and robot | |
| CN107463871A (en) | A kind of point cloud matching method based on corner characteristics weighting | |
| Cipolla et al. | Image-based localization |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |