CN113155140A - Robot SLAM method and system used in outdoor characteristic sparse environment - Google Patents
Robot SLAM method and system used in outdoor characteristic sparse environment Download PDFInfo
- Publication number
- CN113155140A CN113155140A CN202110350778.5A CN202110350778A CN113155140A CN 113155140 A CN113155140 A CN 113155140A CN 202110350778 A CN202110350778 A CN 202110350778A CN 113155140 A CN113155140 A CN 113155140A
- Authority
- CN
- China
- Prior art keywords
- robot
- image
- time
- representing
- sparse
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 69
- 230000009466 transformation Effects 0.000 claims abstract description 68
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 45
- 230000010354 integration Effects 0.000 claims abstract description 35
- 230000000007 visual effect Effects 0.000 claims abstract description 33
- 238000005457 optimization Methods 0.000 claims abstract description 28
- 238000006073 displacement reaction Methods 0.000 claims abstract description 25
- 238000000605 extraction Methods 0.000 claims abstract description 17
- 230000008569 process Effects 0.000 claims abstract description 16
- 238000007781 pre-processing Methods 0.000 claims abstract description 13
- 238000009825 accumulation Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 72
- 238000004364 calculation method Methods 0.000 claims description 33
- 238000005259 measurement Methods 0.000 claims description 30
- 230000001133 acceleration Effects 0.000 claims description 20
- 239000013598 vector Substances 0.000 claims description 17
- 238000012545 processing Methods 0.000 claims description 14
- 230000004044 response Effects 0.000 claims description 14
- 230000000694 effects Effects 0.000 claims description 11
- 230000000903 blocking effect Effects 0.000 claims description 10
- 238000013519 translation Methods 0.000 claims description 8
- 238000003384 imaging method Methods 0.000 claims description 7
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 claims description 5
- 230000001186 cumulative effect Effects 0.000 claims description 5
- 238000013213 extrapolation Methods 0.000 claims description 5
- 230000017105 transposition Effects 0.000 claims description 4
- 238000005286 illumination Methods 0.000 abstract description 11
- 230000008859 change Effects 0.000 abstract description 7
- 230000004438 eyesight Effects 0.000 abstract description 7
- 238000000638 solvent extraction Methods 0.000 abstract 1
- 230000006870 function Effects 0.000 description 18
- 238000013507 mapping Methods 0.000 description 10
- 238000010586 diagram Methods 0.000 description 9
- 238000001514 detection method Methods 0.000 description 6
- 230000007613 environmental effect Effects 0.000 description 6
- 230000004393 visual impairment Effects 0.000 description 6
- 230000004048 modification Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 239000004576 sand Substances 0.000 description 3
- PEDCQBHIVMGVHV-UHFFFAOYSA-N Glycerine Chemical compound OCC(O)CO PEDCQBHIVMGVHV-UHFFFAOYSA-N 0.000 description 2
- 230000004313 glare Effects 0.000 description 2
- 239000011435 rock Substances 0.000 description 2
- 239000004575 stone Substances 0.000 description 2
- 230000001629 suppression Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000002567 autonomic effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/40—Extraction of image or video features
- G06V10/46—Descriptors for shape, contour or point-related descriptors, e.g. scale invariant feature transform [SIFT] or bags of words [BoW]; Salient regional features
- G06V10/462—Salient features, e.g. scale invariant feature transforms [SIFT]
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/40—Extraction of image or video features
- G06V10/50—Extraction of image or video features by performing operations within image blocks; by using histograms, e.g. histogram of oriented gradients [HoG]; by summing image-intensity values; Projection analysis
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Multimedia (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
Abstract
The invention provides a mobile robot vision inertia SLAM method and a system used in an outdoor characteristic sparse environment, which comprises the following steps: acquiring an environment image in an outdoor characteristic sparse environment, and preprocessing the environment image to obtain a preprocessed environment image; extracting sparse features of the preprocessed environment image through a partitioning SIFT feature extraction algorithm; estimating the inter-frame motion of the robot through the pre-integration component of the inertial unit IMU, estimating the inter-frame displacement of the matching point, judging the importance of sparse features through the displacement, and calculating to obtain a visual reprojection error; obtaining an IMU error according to the variance accumulation in the IMU pre-integration process; constructing a loss function according to the visual reprojection error and the IMU error; and (3) minimizing a loss function through a nonlinear optimization method, and solving the pose transformation and map point space coordinates of the robot. The invention can reduce the influence of the difficulties of severe illumination change, sparse characteristics and the like on the positioning performance and improve the autonomy of the mobile robot in a complex outdoor environment.
Description
Technical Field
The invention relates to a robot instant positioning and mapping algorithm, in particular to a robot SLAM method and a system used in an outdoor characteristic sparse environment, and more particularly to a robot visual inertia instant positioning and mapping algorithm used in the outdoor characteristic sparse environment.
Background
The problem of SLAM is generally directed to scenes such as rooms and streets with relatively closed environments and rich environmental features. In an outdoor characteristic sparse environment (such as a desert, a beach, a stone beach, a planet, a moon surface and the like), the real-time positioning and mapping problems of the robot relate to autonomous positioning, safety, task planning and the like of the robot in the scene, and particularly, the autonomous positioning capability of the robot is particularly critical under the condition of no GPS satellite signal (accurate absolute positioning cannot be carried out). IMU inertial measurement unit is often used for the autonomic relative positioning problem of robot, nevertheless can produce certain drift error, uses vision and inertial sensor information fusion, then can revise the drift error that is produced by IMU through the vision, and richer environmental information can be gathered to the camera simultaneously, and the robot of being convenient for realizes more complicated function. In conclusion, an algorithm for real-time positioning and mapping of a robot aiming at an outdoor characteristic sparse scene is urgently needed.
The publication number is CN110309834A (application number: 201910393343.1), which discloses an SLAM algorithm for outdoor off-line navigation, and a certain number of feature points are obtained through a local adaptive threshold adjustment algorithm, so that the problem of insufficient accuracy of feature point extraction under the condition of objective factors such as shadow, weak illumination, sudden noise and the like existing in an image of a field scene is solved. The method does not fundamentally solve the problems of difficult feature extraction and severe illumination change existing in outdoor feature sparse environments, and aims at reducing feature instability caused by feature detection threshold values in sparse environments, so that larger errors can be caused; and the description of the visual features is fuzzy by using the multi-scale detection method, and the weight of the image features in optimization cannot be accurately measured.
Patent document CN109166149A (application number: 201810917346.6) discloses a method and system for fusing binocular camera and IMU positioning and three-dimensional wireframe structure reconstruction. On the basis of binocular vision, the method utilizes a divide-and-conquer strategy to initialize and fuse inertial measurement information, implements tracking positioning and mapping, and can stably operate in indoor and outdoor environments and complex motion conditions. And on the basis of accurate positioning, reconstructing and iteratively optimizing a three-dimensional wire frame based on the pose optimized by the key frame. And matching the straight line segments through the local features and the space geometric constraint and back projecting the straight line segments into a three-dimensional space. The straight line segments are divided into different sets by angle and distance constraints. Based on the grouping results, fitting regions are determined and straight line segments are merged. And finally outputting the three-dimensional wire frame structure.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide a robot SLAM method and system used in an outdoor characteristic sparse environment.
The invention provides a robot SLAM method used in an outdoor characteristic sparse environment, which comprises the following steps:
step S1: acquiring an image in an outdoor characteristic sparse environment, and preprocessing the acquired image to obtain a preprocessed characteristic sparse environment image;
step S2: extracting sparse point features of the preprocessed feature sparse environment image through a blocking SIFT feature extraction algorithm;
step S3: judging the importance of the sparse features through the pre-integration quantity of the inertial unit IMU, and calculating to obtain a visual reprojection error;
step S4: obtaining an IMU error according to the variance accumulation in the IMU pre-integration process;
step S5: constructing a loss function according to the visual reprojection error and the IMU error;
step S6: and (3) minimizing a loss function through a nonlinear optimization method, and solving the pose transformation and map point space coordinates of the robot.
Preferably, the step S1 includes:
step S1.1: acquiring an image in an outdoor characteristic sparse environment, performing contrast stretching on a local image block by using a CLAHE local histogram equalization algorithm, eliminating a block boundary effect, and obtaining an image subjected to local equalization processing;
step S1.2: and stretching the contrast of the whole image by using a histogram equalization algorithm on the image subjected to the local equalization processing to obtain a preprocessed characteristic sparse environment image.
Preferably, the step S2 includes:
step S2.1: dividing the preprocessed feature sparse environment image into a preset number of small blocks, and extracting SIFT features from the preset number of small blocks respectively;
step S2.2: according to the extracted coordinates of the SIFT feature points under the whole image, performing quadtree distribution in the whole image until the number of the SIFT feature points in all the image blocks subjected to the quadtree distribution is not more than a preset value;
step S2.3: and calculating Harris response values for the SIFT feature points in all the image blocks, and reserving one SIFT feature point with the maximum Harris response value in each image block.
Preferably, the step S3 includes:
step S3.1: pre-integrating IMU data between front and back frames to describe pose transformation of the robot in time periods of the front and back frames;
step S3.2: according to pose transformation of the robot in time periods of front and rear frames and a camera imaging model, feature points in images of the front frame of the robot are projected to images of the rear frame, the pixel displacement distance of the feature points is calculated, and a corresponding image feature weight matrix is calculated according to the pixel displacement distance of the feature points.
Preferably, said step S3.1 comprises: using discrete IMU data pre-integration calculation to obtain the motion estimation between the previous frame and the next frame, wherein the calculation formula is as follows:
wherein w represents an estimated value of the angular velocity of the robot;actual measurement data representing the angular velocity of the robot at time k;indicating the deviation of the robot angular velocity measurement at time k,representing the cumulative amount of rotation of the robot from time i to time k; δ t represents the length of time from k to time k + 1;representing a right multiplication operation in a lie algebra; a represents an estimated value of robot acceleration;actual measurement values representing robot acceleration at time k;representing the deviation of the measured value of the robot acceleration at the moment k;representing the displacement predicted component of the robot from the time i to the time k;representing the robot speed pre-integral quantity at the k moment;representing a gaussian error in the acceleration measurement unit;representing a gaussian error in the angular velocity measurement unit; δ t2Representing the square of the length of time from time k to time k + 1.
Preferably, said step S3.2 comprises:
step S3.2.1: a characteristic point set P monitored at the k-1 momentk-1={Uk-1,Vk-1,Dk-1Matching with the characteristic points monitored at the moment k, wherein:
Uk-1={u1,u2,...,uN} (8)
Vk-1={v1,v2,...,vN} (9)
Dk-1={d1,d2,...,dN} (10)
wherein, Uk,Vk,DkA set of pixel coordinates and depth values representing the feature points at time k;
step S3.2.2: according to the position and posture estimation at the time k obtained by pre-integration, the matched feature points at the time k-1 are projected to a camera coordinate system at the time k, and the calculation formula is as follows:
Pc,k=Tc,k,k-1Pc,k-1 (11)
Tc,k,k-1=TcbTk,k-1Tbc (12)
Tk,k-1=Ik (13)
wherein, Pc,kRepresenting the estimated value of the coordinates of the robot at the moment k under the coordinate system of the matched feature point camera; t isc,k,k-1A transformation matrix representing a robot camera coordinate system from the time k-1 to the time k; i iskIs an attitude transformation estimate obtained by pre-integration; t iscbA transformation matrix representing a robot body coordinate system to a camera coordinate system; t isbcA transformation matrix representing a robot camera coordinate system to a body coordinate system; t isbcIncluding a rotation transformation matrix RbcAnd translating the transformed vector tbc;TcbThe same process is carried out; pc,k-1The coordinates of the characteristic point camera coordinate system at the moment of k-1 are obtained; t is tkRepresenting translation transformation vectors of the robot from the k-1 moment to the k moment; -tkRepresenting a translation transformation vector from the k moment to the k-1 moment;the robot rotation transformation matrix representing the time from k time to k-1 time is a rotation transformation matrix R from k-1 time to k timekThe inverse matrix of (d);
and obtaining according to the inverse extrapolation of the camera projection model:
wherein f isx、fy、cx、cyIs an intrinsic parameter of the camera;
step S3.2.3: converting the matching points projected to the k moment under the camera coordinate system into pixel coordinates, and calculating the offset between the front frame and the rear frame under the corresponding characteristic point pixel coordinate system, wherein the calculation formula is as follows:
Δ=(Uk,Vk)-(Uk-1,Vk-1) (18)
wherein K represents an internal reference matrix of the cameraDelta represents the offset of the corresponding characteristic point pixel coordinate system between the previous frame and the next frame;
step S3.2.4: calculating error loss weight of corresponding matching feature points according to the distance delta, and describing through the form of an information covariance matrix Hc:
Wherein λ represents a relative IMU weight adjustment parameter; f represents a weight calculation function, and the calculation formula is as follows:
wherein s represents a visual error relative weight distribution adjustment parameter;
step S3.2.5: by means of a covariance matrix HcInverting to obtain a weight WcA matrix;
preferably, the step S6 includes: solving pose transformation and map point space coordinates of the robot by a nonlinear optimization method;
wherein,representing the visual reprojection error of the jth matching point at the moment i; wcIs a weight matrix of visual reprojection error terms;representing the IMU error at time i;an IMU error weight representing time i;representing the reprojection deviation corresponding to the jth image characteristic point at the moment i;representing the weight corresponding to the reprojection deviation of the jth image feature point at the moment i;representing the reprojection deviation corresponding to the jth image characteristic point at the time i;andthe same, the transposition relation is formed; k represents the total number of time instants at which the error term is incorporated; f (k) represents a set of all image feature points at time k;a transpose of the IMU error vector representing time i;andin the same way, the first and second,the transposed form is written for matrix computation.
Preferably, the nonlinear optimization method in step S6 includes: levenberg-marquardt algorithm and gauss-newton method.
According to the invention, the robot SLAM system used in the outdoor characteristic sparse environment comprises:
module M1: acquiring an image in an outdoor characteristic sparse environment, and preprocessing the acquired image to obtain a preprocessed characteristic sparse environment image;
module M2: extracting sparse point features of the preprocessed feature sparse environment image through a blocking SIFT feature extraction algorithm;
module M3: judging the importance of the sparse features through the pre-integration quantity of the inertial unit IMU, and calculating to obtain a visual reprojection error;
module M4: obtaining an IMU error according to the variance accumulation in the IMU pre-integration process;
module M5: constructing a loss function according to the visual reprojection error and the IMU error;
module M6: and (3) minimizing a loss function through a nonlinear optimization method, and solving the pose transformation and map point space coordinates of the robot.
Preferably, said module M1 comprises:
module M1.1: acquiring an image in an outdoor characteristic sparse environment, performing contrast stretching on a local image block by using a CLAHE local histogram equalization algorithm, eliminating a block boundary effect, and obtaining an image subjected to local equalization processing;
module M1.2: stretching the contrast of the whole image by using a histogram equalization algorithm on the image subjected to the local equalization processing to obtain a preprocessed characteristic sparse environment image;
the module M2 includes:
module M2.1: dividing the preprocessed feature sparse environment image into a preset number of small blocks, and extracting SIFT features from the preset number of small blocks respectively;
module M2.2: according to the extracted coordinates of the SIFT feature points under the whole image, and performing quadtree distribution in the whole image until the number of the SIFT feature points in all the image blocks subjected to the quadtree distribution is not more than a preset value;
module M2.3: calculating Harris response values of the SIFT feature points in all the image blocks, and reserving a SIFT feature point with the maximum Harris response value in each image block;
the module M3 includes:
module M3.1: pre-integrating IMU data between front and back frames to describe pose transformation of the robot in time periods of the front and back frames;
module M3.2: according to pose transformation of the robot in time periods of front and rear frames and a camera imaging model, feature points in images of the front frame of the robot are projected to images of the rear frame, the pixel displacement distance of the feature points is calculated, and a corresponding image feature weight matrix is calculated according to the pixel displacement distance of the feature points.
Compared with the prior art, the invention has the following beneficial effects:
1. according to the robot instant positioning and mapping algorithm under the outdoor characteristic sparse environment, the influence of illumination with violent change on image characteristic extraction is effectively eliminated and weakened by designing an image preprocessing algorithm, more uniform and robust sparse characteristic points are effectively extracted by a blocking SIFT characteristic extraction algorithm, and finally the problem of information quantity provided by image characteristics in a wide-scale scene is considered, so that the weight occupied by visual loss in the pose and map point bundle optimization problem is optimized based on an IMU (inertial measurement unit) pre-integration theory;
2. the method fully utilizes the existing information in actual use, is simple in calculation, can complete the positioning of the robot and the construction of the sparse point map in real time, and has high operation efficiency;
3. the robot positioning and mapping algorithm provided by the invention has the advantages that the positioning capability of the robot in a characteristic sparse scene lacking GPS absolute positioning information is improved, accidents of the robot caused by positioning problems are avoided, related personnel can be effectively assisted to carry out task planning and other work, and the method is suitable for various fields of civil use, industry, military use and the like;
4. the visual inertia SLAM algorithm provided by the invention can improve the positioning capability of the robot in an outdoor characteristic sparse environment and reduce the influence of the system caused by the tracking failure problem caused by severe illumination change and difficult characteristic extraction;
5. the invention can be widely applied to the scenes such as deserts, sand beaches, stone beaches, planets, moon surfaces and the like, and has stronger popularization.
Drawings
Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:
FIG. 1 is a system framework diagram of the present invention;
FIG. 2 is a schematic diagram of block feature detection;
FIG. 3 is a schematic diagram of SIFT feature quadtree allocation and non-maximum suppression;
FIG. 4 is a schematic displacement diagram of a far and near feature point in a linear motion under a pixel coordinate system;
fig. 5 is a schematic displacement diagram of the far and near feature points in the rotation motion under the pixel coordinate system.
Detailed Description
The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.
Example 1
The invention aims at the outdoor characteristic sparse environment, provides a method for eliminating the influence of severe change of illumination conditions on scene image information by image preprocessing, obtains more robust and uniform characteristics by improving a characteristic extraction algorithm, optimizes a visual loss function by using a visual loss weight optimization algorithm based on IMU (inertial measurement unit) pre-integration on the globally extracted image characteristics and further obtains more accurate robot positioning and mapping results aiming at the problems existing in the current robot instant positioning and mapping algorithm.
According to the robot SLAM method for the outdoor characteristic sparse environment provided by the invention, as shown in figures 1 to 5, the method comprises the following steps:
according to the invention, the system needs to input the collected environment image data and the IMU inertial sensor measurement data of the robot, the visual image data is transmitted according to the frame rate of the camera, the IMU data between two frames is accumulated in the memory of the computer, and the IMU data is input together with the image data and is marked with the corresponding timestamp. After inputting the data, the algorithm will perform the following steps:
step S1: acquiring an image in an outdoor characteristic sparse environment, and preprocessing the acquired image to obtain a preprocessed characteristic sparse environment image;
step S2: extracting sparse point features of the preprocessed feature sparse environment image through a blocking SIFT feature extraction algorithm;
step S3: judging the importance of the sparse features through the pre-integration quantity of the inertial unit IMU, and calculating to obtain a visual reprojection error;
step S4: obtaining an IMU error according to the variance accumulation in the IMU pre-integration process; the IMU error weight term is transmitted through variance in the IMU pre-integration process, and the weight is obtained when the variance is larger;
step S5: constructing a loss function according to the visual reprojection error and the IMU error;
step S6: and (3) minimizing a loss function through a nonlinear optimization method, and solving the pose transformation and map point space coordinates of the robot.
Specifically, the step S1 includes:
because the characteristic sparse environment is generally influenced by the change of environmental illumination, even glare or sensor failure caused by overexposure of images can occur, the algorithm is required to have certain self-adaptive capacity aiming at the images, and the influence of the environmental illumination on the image characteristic extraction effect is eliminated or weakened as much as possible; in addition, most color features tend to be consistent in an outdoor feature sparse environment (such as sand, rock beach, planet surface and the like), the contrast is weak, and the contrast of scene images and the like needs to be improved, so that the extracted features are more stable and robust, and a targeted image preprocessing algorithm needs to be designed. According to the image requirement, the method is completed by the following steps:
step S1.1: acquiring an image in an outdoor characteristic sparse environment, performing contrast stretching on a local image block by using a CLAHE local histogram equalization algorithm, eliminating a block boundary effect, and obtaining an image subjected to local equalization processing; the method aims to eliminate the phenomenon that the image is locally over-bright and over-dark so as to obtain an image with higher local contrast;
step S1.2: and stretching the contrast of the whole image by using a histogram equalization algorithm on the image subjected to the local equalization processing, so that the brightness contrast of the image is higher, and a preprocessed characteristic sparse environment image is obtained.
After the image is processed, a sparse feature point detection algorithm is needed for the feature points to have higher uniformity and robustness. For example, when the land texture is uneven in a scene, the full-image features are often concentrated in image blocks with rich textures, and a part of information of the image is excessively depended on.
Specifically, according to the requirements of uniformity and robustness of the sparse feature points of the image, the step S2 includes:
step S2.1: dividing the preprocessed feature sparse environment image into a preset number of small blocks, and extracting SIFT features from the preset number of small blocks respectively; the purpose of extracting features in the small blocks is to ensure that each part of the image contains a certain number of feature points;
step S2.2: after extraction is finished, calculating the coordinate of each feature point under the whole image, and performing quadtree distribution in the whole image according to the extracted coordinate of the SIFT feature point under the whole image, wherein the distribution aims to solve the problem that local feature points are too concentrated;
because the length of the image is about 2 times of the width, in order to make the image blocks present a square shape as much as possible, the image is firstly divided into a left image block and a right image block, when the number of feature points in the image blocks is more than or equal to 4, the image is divided into four small image blocks in an average manner from left to right and from top to bottom, and as shown in fig. 3, the steps are repeated until the number of feature points in all the image blocks is not more than 4;
step S2.3: and calculating Harris response values for the SIFT feature points in all the image blocks, and reserving one SIFT feature point with the maximum Harris response value in each image block.
In consideration of wide feature point scale distribution in a feature sparse scene, under different motion states of the robot, the information amount brought to the pose estimation process by different feature points is very different, for example, a far point in linear motion hardly moves, and a near point can well reflect the motion of the robot. It is therefore desirable to increase the weight that the visual loss of significant feature points occupies in motion estimation.
Specifically, the step S3 includes:
step S3.1: pre-integrating IMU data between front and back frames to describe pose transformation of the robot in time periods of the front and back frames;
step S3.2: according to pose transformation of the robot in time periods of front and rear frames and a camera imaging model, feature points in images of the front frame of the robot are projected to images of the rear frame, the pixel displacement distance of the feature points is calculated, and a corresponding image feature weight matrix is calculated according to the pixel displacement distance of the feature points.
When the pose transformation of the robot is known, the information quantity provided by the feature points at different distances can be known, the motion condition of the robot is not known in advance, and the approximate motion of the robot is described by using the IMU pre-integration quantity.
In particular, said step S3.1 comprises: using discrete IMU data pre-integration calculation to obtain the motion estimation between the previous frame and the next frame, wherein the calculation formula is as follows:
wherein w represents an estimated value of the angular velocity of the robot;actual measurement data representing the angular velocity of the robot at time k;indicating the deviation of the robot angular velocity measurement at time k,representing the cumulative amount of rotation of the robot from time i to time k; δ t represents the length of time from k to time k + 1;representing a right multiplication operation in a lie algebra; a represents an estimated value of robot acceleration;actual measurement values representing robot acceleration at time k;representing the deviation of the measured value of the robot acceleration at the moment k;representing the displacement predicted component of the robot from the time i to the time k;representing the robot speed pre-integral quantity at the k moment;representing a gaussian error in the acceleration measurement unit;representing a gaussian error in the angular velocity measurement unit; δ t2Representing the square of the length of time from time k to time k + 1.
In particular, said step S3.2 comprises:
step S3.2.1: a characteristic point set P monitored at the k-1 momentk-1={Uk-1,Vk-1,Dk-1Matching with the characteristic points monitored at the moment k, wherein:
Uk-1={u1,u2,...,uN} (8)
Vk-1={v1,v2,...,vN} (9)
Dk-1={d1,d2,...,dN} (10)
wherein, Uk,Vk,DkA set of pixel coordinates and depth values representing the feature points at time k;
step S3.2.2: according to the position and posture estimation at the time k obtained by pre-integration, the matched feature points at the time k-1 are projected to a camera coordinate system at the time k, and the calculation formula is as follows:
Pc,k=Tc,k,k-1Pc,k-1 (11)
Tc,k,k-1=TcbTk,k-1Tbc (12)
Tk,k-1=Ik (13)
wherein, Pc,kRepresenting the estimated value of the coordinates of the robot at the moment k under the coordinate system of the matched feature point camera; t isc,k,k-1A transformation matrix representing a robot camera coordinate system from the time k-1 to the time k; i iskIs an attitude transformation estimate obtained by pre-integration; t iscbA transformation matrix representing a robot body coordinate system to a camera coordinate system; t isbcA transformation matrix representing a robot camera coordinate system to a body coordinate system; t isbcIncluding a rotation transformation matrix RbcAnd translating the transformed vector tbc;TcbThe same process is carried out; pc,k-1The coordinates of the characteristic point camera coordinate system at the moment of k-1 are obtained; t is tkRepresenting translation transformation vectors of the robot from the k-1 moment to the k moment; -tkRepresenting a translation transformation vector from the k moment to the k-1 moment;the robot rotation transformation matrix representing the time from k time to k-1 time is a rotation transformation matrix R from k-1 time to k timekThe inverse matrix of (d);
and obtaining according to the inverse extrapolation of the camera projection model:
wherein f isx、fy、cx、cyIs an intrinsic parameter of the camera;
step S3.2.3: converting the matching points projected to the k moment under the camera coordinate system into pixel coordinates, and calculating the offset between the front frame and the rear frame under the corresponding characteristic point pixel coordinate system, wherein the calculation formula is as follows:
Δ=(Uk,Vk)-(Uk-1,Vk-1) (18)
wherein K represents an internal reference matrix of the cameraDelta represents the offset of the corresponding characteristic point pixel coordinate system between the previous frame and the next frame;
step S3.2.4: calculating error loss weight of corresponding matching feature points according to the distance delta, and describing through the form of an information covariance matrix Hc:
Wherein λ represents a relative IMU weight adjustment parameter; f represents a weight calculation function, and the calculation formula is as follows:
wherein s represents a visual error relative weight distribution adjustment parameter;
step S3.2.5: by means of a covariance matrix HcInverting to obtain a weight WcA matrix;
specifically, the step S6 includes: solving pose transformation and map point space coordinates of the robot by a nonlinear optimization method;
wherein,representing the visual reprojection error of the jth matching point at the moment i; wcIs a weight matrix of visual reprojection error terms;representing the IMU error at time i;an IMU error weight representing time i;representing the reprojection deviation corresponding to the jth image characteristic point at the moment i;representing the weight corresponding to the reprojection deviation of the jth image feature point at the moment i;representing the reprojection deviation corresponding to the jth image characteristic point at the time i;andthe same, the transposition relation is formed; k represents the total number of time instants at which the error term is incorporated; f (k) represents a set of all image feature points at time k;a transpose of the IMU error vector representing time i;andthe same, writing the matrix calculation into a transposed form; and the optimal pose transformation and map point coordinates of the robot can be obtained by minimizing J.
Specifically, the nonlinear optimization method in step S6 includes: levenberg-marquardt algorithm and gauss-newton method. The optimization method can be selected and used according to actual conditions and effects.
According to the invention, the robot SLAM system used in the outdoor characteristic sparse environment comprises:
module M1: acquiring an image in an outdoor characteristic sparse environment, and preprocessing the acquired image to obtain a preprocessed characteristic sparse environment image;
module M2: extracting sparse point features of the preprocessed feature sparse environment image through a blocking SIFT feature extraction algorithm;
module M3: judging the importance of the sparse features through the pre-integration quantity of the inertial unit IMU, and calculating to obtain a visual reprojection error;
module M4: obtaining an IMU error according to the variance accumulation in the IMU pre-integration process; the IMU error weight term is transmitted through variance in the IMU pre-integration process, and the weight is obtained when the variance is larger;
module M5: constructing a loss function according to the visual reprojection error and the IMU error;
module M6: and (3) minimizing a loss function through a nonlinear optimization method, and solving the pose transformation and map point space coordinates of the robot.
Specifically, the module M1 includes:
because the characteristic sparse environment is generally influenced by the change of environmental illumination, even glare or sensor failure caused by overexposure of images can occur, the algorithm is required to have certain self-adaptive capacity aiming at the images, and the influence of the environmental illumination on the image characteristic extraction effect is eliminated or weakened as much as possible; in addition, most color features tend to be consistent in an outdoor feature sparse environment (such as sand, rock beach, planet surface and the like), the contrast is weak, and the contrast of scene images and the like needs to be improved, so that the extracted features are more stable and robust, and a targeted image preprocessing algorithm needs to be designed.
Module M1.1: acquiring an image in an outdoor characteristic sparse environment, performing contrast stretching on a local image block by using a CLAHE local histogram equalization algorithm, eliminating a block boundary effect, and obtaining an image subjected to local equalization processing; the method aims to eliminate the phenomenon that the image is locally over-bright and over-dark so as to obtain an image with higher local contrast;
module M1.2: and stretching the contrast of the whole image by using a histogram equalization algorithm on the image subjected to the local equalization processing, so that the brightness contrast of the image is higher, and a preprocessed characteristic sparse environment image is obtained.
After the image is processed, a sparse feature point detection algorithm is needed for the feature points to have higher uniformity and robustness. For example, when the land texture is uneven in a scene, the full-image features are often concentrated in image blocks with rich textures, and a part of information of the image is excessively depended on.
Specifically, according to the requirements of uniformity and robustness of the sparse feature points of the image, the module M2 includes:
module M2.1: dividing the preprocessed feature sparse environment image into a preset number of small blocks, and extracting SIFT features from the preset number of small blocks respectively; the purpose of extracting features in the small blocks is to ensure that each part of the image contains a certain number of feature points;
module M2.2: after extraction is finished, calculating the coordinate of each feature point under the whole image, and performing quadtree distribution in the whole image according to the extracted coordinate of the SIFT feature point under the whole image, wherein the distribution aims to solve the problem that local feature points are too concentrated;
because the length of the image is about 2 times of the width, in order to make the image blocks present a square shape as much as possible, the image is selected to be divided into a left image block and a right image block, when the number of feature points in the image blocks is greater than or equal to 4, the image is divided into four small image blocks in an average manner from left to right and from top to bottom, and as shown in fig. 3, the starting execution is repeated until the number of feature points in all the image blocks is not greater than 4;
module M2.3: and calculating Harris response values for the SIFT feature points in all the image blocks, and reserving one SIFT feature point with the maximum Harris response value in each image block.
In consideration of wide feature point scale distribution in a feature sparse scene, under different motion states of the robot, the information amount brought to the pose estimation process by different feature points is very different, for example, a far point in linear motion hardly moves, and a near point can well reflect the motion of the robot. It is therefore desirable to increase the weight that the visual loss of significant feature points occupies in motion estimation.
Specifically, the module M3 includes:
module M3.1: pre-integrating IMU data between front and back frames to describe pose transformation of the robot in time periods of the front and back frames;
module M3.2: according to pose transformation of the robot in time periods of front and rear frames and a camera imaging model, feature points in images of the front frame of the robot are projected to images of the rear frame, the pixel displacement distance of the feature points is calculated, and a corresponding image feature weight matrix is calculated according to the pixel displacement distance of the feature points.
When the pose transformation of the robot is known, the information quantity provided by the feature points at different distances can be known, the motion condition of the robot is not known in advance, and the approximate motion of the robot is described by using the IMU pre-integration quantity.
In particular, said module M3.1 comprises: using discrete IMU data pre-integration calculation to obtain the motion estimation between the previous frame and the next frame, wherein the calculation formula is as follows:
wherein w represents an estimated value of the angular velocity of the robot;actual measurement data representing the angular velocity of the robot at time k;indicating the deviation of the robot angular velocity measurement at time k,representing the cumulative amount of rotation of the robot from time i to time k; δ t represents the length of time from k to time k + 1;representing a right multiplication operation in a lie algebra; a represents an estimated value of robot acceleration;actual measurement values representing robot acceleration at time k;indicating k time machineDeviation of human acceleration measurements;representing the displacement predicted component of the robot from the time i to the time k;representing the robot speed pre-integral quantity at the k moment;representing a gaussian error in the acceleration measurement unit;representing a gaussian error in the angular velocity measurement unit; δ t2Representing the square of the length of time from time k to time k + 1.
In particular, said module M3.2 comprises:
module M3.2.1: a characteristic point set P monitored at the k-1 momentk-1={Uk-1,Vk-1,Dk-1Matching with the characteristic points monitored at the moment k, wherein:
Uk-1={u1,u2,...,uN} (8)
Vk-1={v1,v2,...,vN} (9)
Dk-1={d1,d2,...,dN} (10)
wherein, Uk,Vk,DkA set of pixel coordinates and depth values representing the feature points at time k;
module M3.2.2: according to the position and posture estimation at the time k obtained by pre-integration, the matched feature points at the time k-1 are projected to a camera coordinate system at the time k, and the calculation formula is as follows:
Pc,k=Tc,k,k-1Pc,k-1 (11)
Tc,k,k-1=TcbTk,k-1Tbc (12)
Tk,k-1=Ik (13)
wherein, Pc,kRepresenting the estimated value of the coordinates of the robot at the moment k under the coordinate system of the matched feature point camera; t isc,k,k-1A transformation matrix representing a robot camera coordinate system from the time k-1 to the time k; i iskIs an attitude transformation estimate obtained by pre-integration; t iscbA transformation matrix representing a robot body coordinate system to a camera coordinate system; t isbcA transformation matrix representing a robot camera coordinate system to a body coordinate system; t isbcIncluding a rotation transformation matrix RbcAnd translating the transformed vector tbc;TcbThe same process is carried out; pc,k-1The coordinates of the characteristic point camera coordinate system at the moment of k-1 are obtained; t is tkRepresenting translation transformation vectors of the robot from the k-1 moment to the k moment; -tkRepresenting a translation transformation vector from the k moment to the k-1 moment;the robot rotation transformation matrix representing the time from k time to k-1 time is a rotation transformation matrix R from k-1 time to k timekThe inverse matrix of (d);
and obtaining according to the inverse extrapolation of the camera projection model:
wherein f isx、fy、cx、cyIs an intrinsic parameter of the camera;
module M3.2.3: converting the matching points projected to the k moment under the camera coordinate system into pixel coordinates, and calculating the offset between the front frame and the rear frame under the corresponding characteristic point pixel coordinate system, wherein the calculation formula is as follows:
Δ=(Uk,Vk)-(Uk-1,Vk-1) (18)
wherein K represents an internal reference matrix of the cameraDelta represents the offset of the corresponding characteristic point pixel coordinate system between the previous frame and the next frame;
module M3.2.4: calculating error loss weight of corresponding matching feature points according to the distance delta, and describing through the form of an information covariance matrix Hc:
Wherein λ represents a relative IMU weight adjustment parameter; f represents a weight calculation function, and the calculation formula is as follows:
wherein s represents a visual error relative weight distribution adjustment parameter;
module M3.2.5: by means of a covariance matrix HcInverting to obtain a weight WcA matrix;
specifically, the module M6 includes: solving pose transformation and map point space coordinates of the robot by a nonlinear optimization method;
wherein,representing the visual reprojection error of the jth matching point at the moment i; wcIs a weight matrix of visual reprojection error terms;representing the IMU error at time i;an IMU error weight representing time i;representing the reprojection deviation corresponding to the jth image characteristic point at the moment i;representing the weight corresponding to the reprojection deviation of the jth image feature point at the moment i;representing the reprojection deviation corresponding to the jth image characteristic point at the time i;andthe same, the transposition relation is formed; k represents the total number of time instants at which the error term is incorporated; f (k) represents a set of all image feature points at time k;a transpose of the IMU error vector representing time i;andsame, write to for matrix calculationTransposed form; and the optimal pose transformation and map point coordinates of the robot can be obtained by minimizing J.
Specifically, the nonlinear optimization method in the module M6 includes: levenberg-marquardt algorithm and gauss-newton method. The optimization method can be selected and used according to actual conditions and effects.
Example 2
Example 2 is a modification of example 1
The positioning and mapping algorithm comprises (1) image preprocessing in an outdoor characteristic sparse environment; (2) extracting the blocking SIFT features; (3) visual loss weight optimization three parts based on IMU pre-integration are composed of the following steps:
step 1.1: a CLAHE local histogram equalization algorithm is used for the image in the outdoor characteristic sparse environment;
step 1.2: and using a histogram equalization algorithm for the image after the local equalization processing.
Step 2.1: dividing the image into small blocks, and extracting SIFT features in the small blocks respectively;
step 2.2: performing quadtree distribution on all the features, wherein a left image block and a right image block exist in an initial image, when the number of feature points in the image blocks is not less than 4, the image blocks are divided into four small image blocks in an average manner from top to bottom, and the steps are repeated until the number of the feature points in all the image blocks is not more than 4;
step 2.3: calculating Harris response values for the distributed characteristic points, and requiring that at most one characteristic point with the maximum response value can be reserved in each small image block;
step 3.1: estimating pose transformation of the robot in an inter-frame time period of two frames of images, specifically comprising:
the discrete IMU data is used for pre-integral calculation to obtain the motion estimation between the previous frame and the next frame, and the calculation formula is as follows:
in the formula: omega represents an estimate of the angular velocity of the robot,actual measurement data representing the angular velocity of the robot at time k,represents the deviation of the robot angular velocity measurement at time k (subject to a gaussian walking model),indicating the cumulative amount of rotation of the robot from time i to time i + k, deltat indicating the length of time from k to time k +1,represents the right multiplication operation in the lie algebra, a is the estimated value of the robot acceleration,representing the actual measurement of the robot acceleration at time k,represents the deviation of the robot acceleration measurement at time k (subject to a gaussian walking model),represents the predicted component of the displacement of the robot from the moment i to the moment i + k (by recursion calculation),indicating the robot speed (calculated by recursion) at time k.
Step 3.2: and projecting the feature points in the front frame image of the robot to the rear frame image according to the pose transformation relation and the camera imaging model, calculating the pixel displacement distance of the feature points, and finally calculating a corresponding image feature weight matrix according to the pixel displacement distance of the feature points. Specifically, the method comprises the following steps:
step 3.2.1: a characteristic point set P monitored at the k-1 momentk-1={Uk-1,Vk-1,Dk-1Matching the feature points monitored in the image at the time k, wherein:
Uk-1={u1,u2,…,uN},Vk-1={v1,v2,…,vN},Dk-1={d1,d2,…,dN} (8)
Uk、Vk、Dka set of pixel coordinates and depth values representing the feature points at time k.
Step 3.2.2: according to the position and posture estimation at the time k obtained by pre-integration, the matched feature points at the time k-1 are projected to a camera coordinate system at the time k, and the calculation formula is as follows:
Pc,k=Tc,k,k-1Pc,k-1 (9)
wherein:
Tc,k,k-1=TcbTk,k-1Tbc,Tk,k-1=Ik (10)
Pc,kan estimated value T representing coordinates of the robot at the moment k under a coordinate system of a camera matched with the feature pointsc,k,k-1Transformation matrix representing the coordinate system of the robot camera from time k-1 to time k, IkIs an attitude transformation estimate, T, obtained by pre-integrationcbTransformation matrix, T, representing the coordinate system of a robot to the coordinate system of a camerabcA transformation matrix, T, representing the robot camera coordinate system to the body coordinate systembcIncluding a rotation transformation matrix RbcAnd translating the transformed vector tbc,TcbThe same is true. Pc,k-1The coordinates of the feature points at the time k-1 in the camera coordinate system can be obtained by inverse extrapolation of the camera projection model:
wherein f isx、fy、cx、cyIs an intrinsic parameter of the camera.
Step 3.2.3: converting the matching points projected to the k moment under the camera coordinate system into pixel coordinates, and calculating the offset between the front frame and the rear frame under the corresponding characteristic point pixel coordinate system, wherein the calculation formula is as follows:
Δ=(Uk,Vk)-(Uk-1,Vk-1) (15)
and delta represents the offset of the corresponding characteristic point pixel coordinate system between the previous frame and the next frame.
Step 3.2.4: calculating error loss weight of corresponding matching feature points according to the distance delta, and describing through the form of an information covariance matrix Hc:
Wherein λ is a relative IMU weight adjustment parameter, f is a weight calculation function, and is a programmable function, where the form of f used here is as follows:
s is a visual error relative weight distribution adjustment parameter.
After the information covariance matrix is obtained through calculation, the covariance matrix H is usedcInverting to obtain a weight WcMatrix:
and 4, step 4: and constructing a vision and IMU error bundle set optimization problem according to the weight. The optimization objective function is:
whereinRepresenting the visual reprojection error, W, of the jth matching point at time icIs the weight matrix of the image error terms calculated in step 3,indicating the IMU error at time i,and an IMU error item information matrix representing the i moment. And solving the poses of the robot at k moments and the key points of the map through nonlinear optimization. The optimization method uses the Levenburg-Marquardt algorithm and the Gauss-Newton method. Specifically, the optimization method can be selected and used according to actual conditions and effects.
The invention will be further described in detail with reference to the accompanying drawings and implementation examples, and an algorithm framework of the invention is shown in a figure 1. Considering here a robot configuration using a binocular camera + IMU inertial sensor, the input data includes left and right eye camera images and acceleration, angular velocity information measured by the IMU sensor.
This example is illustrated using the ROS system to build a test environment. Through a subscription. In the preprocessing process, a CLAHE local histogram equalization algorithm is used, a threshold parameter is set to be 4.0, after illumination influence is eliminated, histogram equalization processing is carried out for the first time, then blocking SIFT feature detection is carried out on the preprocessed image, a schematic diagram of image blocking is shown in a diagram 2, and a schematic diagram of feature point quadtree allocation and non-maximum suppression is shown in a diagram 3.
And (3) obtaining approximate motion estimation between image frames according to IMU pre-integration, wherein the displacement motion of the far and near points of linear motion in pixel coordinates is shown in a graph 4, and the displacement motion of the far and near points of rotary motion in pixel coordinates is shown in a graph 5, so that the weight occupied by the far and near points in the bundle set optimization is calculated through the motion estimation and the spatial positions of the feature points. Here, the relative IMU weight adjustment parameter is set to 13, the visual error relative weight parameter is set to 0.5, and the visual information matrix is:
and constructing a vision and IMU combined optimization objective function through a vision information matrix and an IMU error matrix, and optimizing and solving by using a Levenberg-Marquardt algorithm. When setting the weight adjusting parameters, the adjustment needs to be performed according to the actually used scene, if the scene area is large, a large relative weight parameter is used, but correspondingly, the relative weight parameter of the IMU needs to be reduced, so that the weight of the entire IMU basically remains unchanged relative to the IMU.
Those skilled in the art will appreciate that, in addition to implementing the systems, apparatus, and various modules thereof provided by the present invention in purely computer readable program code, the same procedures can be implemented entirely by logically programming method steps such that the systems, apparatus, and various modules thereof are provided in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system, the device and the modules thereof provided by the present invention can be considered as a hardware component, and the modules included in the system, the device and the modules thereof for implementing various programs can also be considered as structures in the hardware component; modules for performing various functions may also be considered to be both software programs for performing the methods and structures within hardware components.
The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.
Claims (10)
1. A robot SLAM method for use in outdoor feature sparse environments, comprising:
step S1: acquiring an image in an outdoor characteristic sparse environment, and preprocessing the acquired image to obtain a preprocessed characteristic sparse environment image;
step S2: extracting sparse point features of the preprocessed feature sparse environment image through a blocking SIFT feature extraction algorithm;
step S3: judging the importance of the sparse features through the pre-integration quantity of the inertial unit IMU, and calculating to obtain a visual reprojection error;
step S4: obtaining an IMU error according to the variance accumulation in the IMU pre-integration process;
step S5: constructing a loss function according to the visual reprojection error and the IMU error;
step S6: and (3) minimizing a loss function through a nonlinear optimization method, and solving the pose transformation and map point space coordinates of the robot.
2. The robot SLAM method for outdoor sparse feature environment of claim 1, wherein said step S1 comprises:
step S1.1: acquiring an image in an outdoor characteristic sparse environment, performing contrast stretching on a local image block by using a CLAHE local histogram equalization algorithm, eliminating a block boundary effect, and obtaining an image subjected to local equalization processing;
step S1.2: and stretching the contrast of the whole image by using a histogram equalization algorithm on the image subjected to the local equalization processing to obtain a preprocessed characteristic sparse environment image.
3. The robot SLAM method for outdoor sparse feature environment of claim 1, wherein said step S2 comprises:
step S2.1: dividing the preprocessed feature sparse environment image into a preset number of small blocks, and extracting SIFT features from the preset number of small blocks respectively;
step S2.2: according to the extracted coordinates of the SIFT feature points under the whole image, performing quadtree distribution in the whole image until the number of the SIFT feature points in all the image blocks subjected to the quadtree distribution is not more than a preset value;
step S2.3: and calculating Harris response values for the SIFT feature points in all the image blocks, and reserving one SIFT feature point with the maximum Harris response value in each image block.
4. The robot SLAM method for outdoor sparse feature environment of claim 1, wherein said step S3 comprises:
step S3.1: pre-integrating IMU data between front and back frames to describe pose transformation of the robot in time periods of the front and back frames;
step S3.2: according to pose transformation of the robot in time periods of front and rear frames and a camera imaging model, feature points in images of the front frame of the robot are projected to images of the rear frame, the pixel displacement distance of the feature points is calculated, and a corresponding image feature weight matrix is calculated according to the pixel displacement distance of the feature points.
5. The robot SLAM method for use in outdoor feature sparse environments of claim 4, wherein said step S3.1 comprises: using discrete IMU data pre-integration calculation to obtain the motion estimation between the previous frame and the next frame, wherein the calculation formula is as follows:
wherein w represents an estimated value of the angular velocity of the robot;actual measurement data representing the angular velocity of the robot at time k;indicating the deviation of the robot angular velocity measurement at time k,representing the cumulative amount of rotation of the robot from time i to time k; δ t represents the length of time from k to time k + 1;representing a right multiplication operation in a lie algebra; a represents an estimated value of robot acceleration;actual measurement values representing robot acceleration at time k;representing the deviation of the measured value of the robot acceleration at the moment k;representing the displacement predicted component of the robot from the time i to the time k;representing the robot speed pre-integral quantity at the k moment;representing a gaussian error in the acceleration measurement unit;representing a gaussian error in the angular velocity measurement unit; δ t2Representing the square of the length of time from time k to time k + 1.
6. The robot SLAM method for use in outdoor feature sparse environments of claim 4, wherein said step S3.2 comprises:
step S3.2.1: a characteristic point set P monitored at the k-1 momentk-1={Uk-1,Vk-1,Dk-1Matching with the characteristic points monitored at the moment k, wherein:
Uk-1={u1,u2,...,uN} (8)
Vk-1={v1,v2,...,vN} (9)
Dk-1={d1,d2,...,dN} (10)
wherein, Uk,Vk,DkA set of pixel coordinates and depth values representing the feature points at time k;
step S3.2.2: according to the position and posture estimation at the time k obtained by pre-integration, the matched feature points at the time k-1 are projected to a camera coordinate system at the time k, and the calculation formula is as follows:
Pc,k=Tc,k,k-1Pc,k-1 (11)
Tc,k,k-1=TcbTk,k-1Tbc (12)
Tk,k-1=Ik (13)
wherein, Pc,kCoordinate under camera coordinate system of robot matching feature point for representing k timeAn estimated value of (d); t isc,k,k-1A transformation matrix representing a robot camera coordinate system from the time k-1 to the time k; i iskIs an attitude transformation estimate obtained by pre-integration; t iscbA transformation matrix representing a robot body coordinate system to a camera coordinate system; t isbcA transformation matrix representing a robot camera coordinate system to a body coordinate system; t isbcIncluding a rotation transformation matrix RbcAnd translating the transformed vector tbc;TcbThe same process is carried out; pc,k-1The coordinates of the characteristic point camera coordinate system at the moment of k-1 are obtained; t is tkRepresenting translation transformation vectors of the robot from the k-1 moment to the k moment; -tkRepresenting a translation transformation vector from the k moment to the k-1 moment;the robot rotation transformation matrix representing the time from k time to k-1 time is a rotation transformation matrix R from k-1 time to k timekThe inverse matrix of (d);
and obtaining according to the inverse extrapolation of the camera projection model:
wherein f isx、fy、cx、cyIs an intrinsic parameter of the camera;
step S3.2.3: converting the matching points projected to the k moment under the camera coordinate system into pixel coordinates, and calculating the offset between the front frame and the rear frame under the corresponding characteristic point pixel coordinate system, wherein the calculation formula is as follows:
Δ=(Uk,Vk)-(Uk-1,Vk-1) (18)
wherein K represents an internal reference matrix of the cameraDelta represents the offset of the corresponding characteristic point pixel coordinate system between the previous frame and the next frame;
step S3.2.4: calculating error loss weight of corresponding matching feature points according to the distance delta, and describing through the form of an information covariance matrix Hc:
Wherein λ represents a relative IMU weight adjustment parameter; f represents a weight calculation function, and the calculation formula is as follows:
wherein s represents a visual error relative weight distribution adjustment parameter;
step S3.2.5: by means of a covariance matrix HcInverting to obtain a weight WcA matrix;
7. the robot SLAM method for outdoor sparse feature environment of claim 1, wherein said step S6 comprises: solving pose transformation and map point space coordinates of the robot by a nonlinear optimization method;
wherein,representing the visual reprojection error of the jth matching point at the moment i; wcIs the weight of visionA weight matrix of projection error terms;representing the IMU error at time i;an IMU error weight representing time i;representing the reprojection deviation corresponding to the jth image characteristic point at the moment i;representing the weight corresponding to the reprojection deviation of the jth image feature point at the moment i;representing the reprojection deviation corresponding to the jth image characteristic point at the time i;andthe same, the transposition relation is formed; k represents the total number of time instants at which the error term is incorporated; f (k) represents a set of all image feature points at time k;a transpose of the IMU error vector representing time i;andsimilarly, a transposed form is written for matrix computation.
8. The robot SLAM method for outdoor sparse feature environments of claim 7, wherein the nonlinear optimization method in step S6 comprises: levenberg-marquardt algorithm and gauss-newton method.
9. A robotic SLAM system for use in outdoor feature sparse environments, comprising:
module M1: acquiring an image in an outdoor characteristic sparse environment, and preprocessing the acquired image to obtain a preprocessed characteristic sparse environment image;
module M2: extracting sparse point features of the preprocessed feature sparse environment image through a blocking SIFT feature extraction algorithm;
module M3: judging the importance of the sparse features through the pre-integration quantity of the inertial unit IMU, and calculating to obtain a visual reprojection error;
module M4: obtaining an IMU error according to the variance accumulation in the IMU pre-integration process;
module M5: constructing a loss function according to the visual reprojection error and the IMU error;
module M6: and (3) minimizing a loss function through a nonlinear optimization method, and solving the pose transformation and map point space coordinates of the robot.
10. The robotic SLAM system for use in outdoor feature sparse environments of claim 9, wherein the module M1 comprises:
module M1.1: acquiring an image in an outdoor characteristic sparse environment, performing contrast stretching on a local image block by using a CLAHE local histogram equalization algorithm, eliminating a block boundary effect, and obtaining an image subjected to local equalization processing;
module M1.2: stretching the contrast of the whole image by using a histogram equalization algorithm on the image subjected to the local equalization processing to obtain a preprocessed characteristic sparse environment image;
the module M2 includes:
module M2.1: dividing the preprocessed feature sparse environment image into a preset number of small blocks, and extracting SIFT features from the preset number of small blocks respectively;
module M2.2: according to the extracted coordinates of the SIFT feature points under the whole image, and performing quadtree distribution in the whole image until the number of the SIFT feature points in all the image blocks subjected to the quadtree distribution is not more than a preset value;
module M2.3: calculating Harris response values of the SIFT feature points in all the image blocks, and reserving a SIFT feature point with the maximum Harris response value in each image block;
the module M3 includes:
module M3.1: pre-integrating IMU data between front and back frames to describe pose transformation of the robot in time periods of the front and back frames;
module M3.2: according to pose transformation of the robot in time periods of front and rear frames and a camera imaging model, feature points in images of the front frame of the robot are projected to images of the rear frame, the pixel displacement distance of the feature points is calculated, and a corresponding image feature weight matrix is calculated according to the pixel displacement distance of the feature points.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110350778.5A CN113155140B (en) | 2021-03-31 | 2021-03-31 | Robot SLAM method and system used in outdoor characteristic sparse environment |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110350778.5A CN113155140B (en) | 2021-03-31 | 2021-03-31 | Robot SLAM method and system used in outdoor characteristic sparse environment |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113155140A true CN113155140A (en) | 2021-07-23 |
CN113155140B CN113155140B (en) | 2022-08-02 |
Family
ID=76885883
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110350778.5A Active CN113155140B (en) | 2021-03-31 | 2021-03-31 | Robot SLAM method and system used in outdoor characteristic sparse environment |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113155140B (en) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109993113A (en) * | 2019-03-29 | 2019-07-09 | 东北大学 | A kind of position and orientation estimation method based on the fusion of RGB-D and IMU information |
CN110009681A (en) * | 2019-03-25 | 2019-07-12 | 中国计量大学 | A kind of monocular vision odometer position and posture processing method based on IMU auxiliary |
CN110706248A (en) * | 2019-08-20 | 2020-01-17 | 广东工业大学 | Visual perception mapping algorithm based on SLAM and mobile robot |
CN111161337A (en) * | 2019-12-18 | 2020-05-15 | 南京理工大学 | Accompanying robot synchronous positioning and composition method in dynamic environment |
CN111288989A (en) * | 2020-02-25 | 2020-06-16 | 浙江大学 | Visual positioning method for small unmanned aerial vehicle |
CN111780754A (en) * | 2020-06-23 | 2020-10-16 | 南京航空航天大学 | Visual inertial odometer pose estimation method based on sparse direct method |
CN112240768A (en) * | 2020-09-10 | 2021-01-19 | 西安电子科技大学 | Visual inertial navigation fusion SLAM method based on Runge-Kutta4 improved pre-integration |
-
2021
- 2021-03-31 CN CN202110350778.5A patent/CN113155140B/en active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110009681A (en) * | 2019-03-25 | 2019-07-12 | 中国计量大学 | A kind of monocular vision odometer position and posture processing method based on IMU auxiliary |
CN109993113A (en) * | 2019-03-29 | 2019-07-09 | 东北大学 | A kind of position and orientation estimation method based on the fusion of RGB-D and IMU information |
CN110706248A (en) * | 2019-08-20 | 2020-01-17 | 广东工业大学 | Visual perception mapping algorithm based on SLAM and mobile robot |
CN111161337A (en) * | 2019-12-18 | 2020-05-15 | 南京理工大学 | Accompanying robot synchronous positioning and composition method in dynamic environment |
CN111288989A (en) * | 2020-02-25 | 2020-06-16 | 浙江大学 | Visual positioning method for small unmanned aerial vehicle |
CN111780754A (en) * | 2020-06-23 | 2020-10-16 | 南京航空航天大学 | Visual inertial odometer pose estimation method based on sparse direct method |
CN112240768A (en) * | 2020-09-10 | 2021-01-19 | 西安电子科技大学 | Visual inertial navigation fusion SLAM method based on Runge-Kutta4 improved pre-integration |
Non-Patent Citations (3)
Title |
---|
HONGLE XIE, ET AL: "Hierarchical Quadtree Feature Optical Flow Tracking Based Sparse Pose-Graph Visual-Inertial SLAM", 《2020 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA)》 * |
李建禹: "基于单目视觉与IMU结合的SLAM技术研究", 《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》 * |
盛淼: "基于双目视觉惯导的SLAM算法研究", 《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》 * |
Also Published As
Publication number | Publication date |
---|---|
CN113155140B (en) | 2022-08-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Concha et al. | Visual-inertial direct SLAM | |
CN111210463B (en) | Virtual wide-view visual odometer method and system based on feature point auxiliary matching | |
EP3679549B1 (en) | Visual-inertial odometry with an event camera | |
US10553026B2 (en) | Dense visual SLAM with probabilistic surfel map | |
Qin et al. | Vins-mono: A robust and versatile monocular visual-inertial state estimator | |
Zuo et al. | Visual-inertial localization with prior LiDAR map constraints | |
CN110726406A (en) | Improved nonlinear optimization monocular inertial navigation SLAM method | |
CN102722697B (en) | Unmanned aerial vehicle autonomous navigation landing visual target tracking method | |
JP2021518622A (en) | Self-location estimation, mapping, and network training | |
CN111932674A (en) | Optimization method of line laser vision inertial system | |
Chen et al. | A stereo visual-inertial SLAM approach for indoor mobile robots in unknown environments without occlusions | |
Alcantarilla et al. | Large-scale dense 3D reconstruction from stereo imagery | |
CN114494150A (en) | Design method of monocular vision odometer based on semi-direct method | |
Liu et al. | Real-time dense construction with deep multi-view stereo using camera and IMU sensors | |
Chen et al. | PGSR: Planar-based Gaussian Splatting for Efficient and High-Fidelity Surface Reconstruction | |
CN113155140B (en) | Robot SLAM method and system used in outdoor characteristic sparse environment | |
Cigla et al. | Gaussian mixture models for temporal depth fusion | |
Ling et al. | An iterated extended Kalman filter for 3D mapping via Kinect camera | |
Zieliński et al. | Keyframe-based dense mapping with the graph of view-dependent local maps | |
Belter et al. | Keyframe-Based local normal distribution transform occupancy maps for environment mapping | |
Ren | An improved binocular LSD_SLAM method for object localization | |
Benamar et al. | Gradient-Based time to contact on paracatadioptric camera | |
Zeng et al. | Fast and Robust Semi-Direct Monocular Visual-Inertial Odometry for UAV | |
CN117629241B (en) | Multi-camera visual inertial odometer optimization method based on continuous time model | |
Rosinol | 3D Spatial Perception with Real-Time Dense Metric-Semantic SLAM |
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 |