CN113012196A - Positioning method based on information fusion of binocular camera and inertial navigation sensor - Google Patents

Positioning method based on information fusion of binocular camera and inertial navigation sensor Download PDF

Info

Publication number
CN113012196A
CN113012196A CN202110245725.7A CN202110245725A CN113012196A CN 113012196 A CN113012196 A CN 113012196A CN 202110245725 A CN202110245725 A CN 202110245725A CN 113012196 A CN113012196 A CN 113012196A
Authority
CN
China
Prior art keywords
line
frame
feature
positioning method
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.)
Granted
Application number
CN202110245725.7A
Other languages
Chinese (zh)
Other versions
CN113012196B (en
Inventor
李奎霖
魏武
曾锦秀
肖文煜
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202110245725.7A priority Critical patent/CN113012196B/en
Publication of CN113012196A publication Critical patent/CN113012196A/en
Application granted granted Critical
Publication of CN113012196B publication Critical patent/CN113012196B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving reference images or patches
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • 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/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/21—Design or setup of recognition systems or techniques; Extraction of features in feature space; Blind source separation
    • G06F18/211—Selection of the most significant subset of features
    • G06F18/2113—Selection of the most significant subset of features by ranking or filtering the set of features, e.g. using a measure of variance or of feature cross-correlation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Automation & Control Theory (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Multimedia (AREA)
  • Image Analysis (AREA)

Abstract

The invention discloses a positioning method based on information fusion of a binocular camera and an inertial navigation sensor, which comprises the following steps of: acquiring a peripheral image sequence of the robot; using a FAST feature point extraction algorithm for the obtained image frame, and introducing a quadtree to quickly screen feature points; for the obtained image frame, extracting line features by using an LSD line feature extraction algorithm, and calculating a line feature descriptor by using an LBD algorithm; performing tracking matching on FAST feature points extracted from the previous and next image frames to obtain an inter-frame estimation pose, re-projecting point and line features in the previous and next frames to the current frame, and performing tracking matching on the line features; confirming whether the current frame is a key frame, if so, acquiring IMU sensor data between a front frame and a rear frame, performing pre-integration on the IMU sensor data, and adding the current frame into a sliding window; and the paired dotted line characteristic constraints in the sliding window and IMU pre-integration tight coupling are combined to perform rear-end optimization, and more accurate inter-frame motion pose is output.

Description

Positioning method based on information fusion of binocular camera and inertial navigation sensor
Technical Field
The invention relates to the technical field of machine vision and positioning navigation, in particular to a positioning method based on information fusion of a binocular camera and an inertial navigation sensor.
Background
The intelligent robot senses the environmental information by means of abundant sensors, and uses the related environmental information for algorithm-level fusion to obtain comprehensive information about the self state and the surrounding environment of the robot, so as to be used as input of a control decision. The image information collected by the camera is rich, so that the repositioning precision can be improved, but the camera frequency is low, and the influence of illumination and the surface texture of an object is large; although the IMU can measure the pose of the robot at a high frequency in a short time, the IMU has serious drift and can continue to work only by the auxiliary correction of other sensors. Therefore, the multi-sensor fusion-based instant positioning algorithm comprehensively utilizes the advantages of various sensors, and can realize high-precision and high-robustness pose determination and map construction on the intelligent robot.
Although the existing visual positioning system mainly based on point features can achieve instant positioning mainly in a specific scene with rich textures, in an indoor environment, weak texture areas such as large-area white walls in corridor channels usually exist. And the light may change dramatically in the channel in a small area. These real-existing scene conditions will cause the visual positioning technology based on point features to fail to work properly. An Inertial Measurement Unit (IMU) integrates a gyroscope and a linear accelerometer component, so that the Inertial Measurement Unit (IMU) has strong adaptability to a large-scale motion state, can output angular velocity and linear acceleration at a high frequency, can acquire real-time motion pose transformation of a robot by integrating the angular velocity and the linear acceleration, but still has the problems of accumulated error and scale drift, so that visual information and IMU information have strong complementarity, visual pose constraint can correct IMU drift, and the IMU can provide absolute scale information for monocular vision. Nevertheless, monocular vision initialization is very demanding with respect to environment and camera motion, and initialization is prone to fail if there is little ambient texture information, little or too much camera motion.
Disclosure of Invention
The invention aims to provide a positioning method based on information fusion of a binocular camera and an inertial navigation sensor on the premise of saving cost and ensuring reliability, provides a mechanism for tightly coupling IMU data and visual point-line characteristics to perform indoor robot visual positioning, and realizes accurate and rapid pose resolving of a robot in a complex environment.
The invention is realized by at least one of the following technical schemes.
A positioning method based on information fusion of a binocular camera and an inertial navigation sensor comprises the following steps:
s1, acquiring a robot peripheral image sequence;
step S2, for the obtained image frame, using an improved FAST feature point extraction algorithm and introducing a quadtree to quickly screen feature points, so that the FAST feature points are uniformly distributed;
step S3, extracting line features of the image frame and calculating a line feature descriptor;
step S4, performing tracking matching on FAST feature points extracted from the previous and subsequent image frames to obtain an inter-frame estimation pose, re-projecting point and line features in the previous and subsequent frames to the current frame according to the estimation pose, and performing tracking matching on the line features;
step S5, determining whether the current frame is a key frame, if not, discarding the current frame, and temporarily storing the visual information and IMU pre-integration information to transmit to the next frame;
and step S6, adding the current frame into a sliding window, performing rear-end optimization by combining the paired dotted line feature constraint in the sliding window and IMU pre-integration tight coupling, and outputting the motion pose between frames.
Preferably, the improved FAST feature point extraction algorithm specifically includes: dividing grids according to the size of the image; traversing each grid, extracting FAST feature points from each grid, regarding the image as an N-ary tree root node, namely N equal divisions, if the number of the root node feature points is more than 1, performing iteration equal divisions on the node to generate four child nodes, wherein the iteration termination condition is that a certain child node only contains one feature point or does not contain the feature point; and finally, the selected characteristic point is the optimal.
Preferably, step S3 is to extract line features by using an LSD line feature extraction algorithm, and calculate line feature descriptors by using an LBD algorithm.
Preferably, the lsd (line Segment detector) line feature extraction algorithm specifically includes: the image size is reduced to a% of the original image through Gaussian sampling, the sawtooth effect existing in the original image is weakened on the premise that the image is not blurred, the pixel gradients of all points in the image are sequenced, and pixel points in the pixel gradient direction are combined to generate a linear support area.
Preferably, the lbd (line Band descriptor) algorithm is specifically: performing pixel gradient accumulation on the detected LSD line characteristics, constructing descriptors by accumulating gradient information associated with each line in the line characteristics, calculating the mean value and standard deviation of descriptor matrixes of each line, and matching line ends according to the distance of the descriptors on the basis of a nearest neighbor matching criterion;
the LBD descriptor is specifically: determining an LSD line segment strip area and a gradient direction; calculating gradient weight of each row of the strip; counting gradient histograms of each row to form a descriptor matrix; the mean and standard deviation were calculated and concatenated into an LBD descriptor.
Preferably, in step S4, the tracking matching of the feature points of the previous and subsequent frames is performed by a direct method based on a minimum luminosity error, and the luminosity error of the feature points and the surrounding pixel blocks is selected as a final error function; the gray value is obtained by a bilinear interpolation method, and the minimum luminosity error function is as follows:
Eu,v＝Iv[p`]-Iu[p]
where p is the previous image frame IuFAST feature point in (1), p' represents p in the current image frame IvThe back projected coordinates of (1).
Preferably, the tracking and matching of the line features is realized by minimizing a reprojection error:
Figure BDA0002964016540000041
wherein l ═ l1,l2]TIs a projected straight line,/1Representing the line feature to be matched in the previous frame, l2Representing line features to be matched in the subsequent frame, pT、qTAre the homogeneous coordinates of the two end points of the matching line segment.
Preferably, in step S5, the key frame selection policy is:
the current frame, which has been considered as a key frame N consecutive times, can track not less than h FAST point features and at least m line features, or the current frame shares 3/4 no more point features and line features in the reference key frame than the reference key frame.
Preferably, before adding the sliding window, the IMU sensor data between the front and rear frames of the key frame is pre-integrated.
Preferably, the objective function to be optimized in the sliding window is:
E＝‖rp-JpX‖2+∑‖rI(Zi,X)‖2+∑‖rf(Zf,X)‖2+∑‖rl(Zl,X)‖2
wherein X is the state quantity to be optimized, including the position, rotation, acceleration of IMU relative to the world coordinate system and the inverse depth of the characteristic point, etc. | rp-JpX‖2For residual constraint after marginalization, rpDenotes the a priori residual, JpJacobian, | r, representing a prior termI(Zi,X)‖2Is an IMU error, rI() Representing IMU residual, ZiMeasurement term, | r, representing pre-integrationf(ZfX) represents a point feature reprojection error, | rl(Zl,X)‖2Representing line feature reprojection error, rf() Representing point feature residual terms, rl() Represents the residual term of the line feature,Zfobservations representing line characteristics, ZlRepresents an observed quantity of line features.
The invention can well overcome the problem that the point characteristics are invalid in scenes such as weak texture corridors, light violent change and the like by a visual positioning technology combining the lead-in characteristics and the point characteristics. And by means of a mechanism of tightly coupling IMU data and visual point-line characteristics, the problem of autonomous positioning failure of pure vision under the conditions of rapid movement, rotation and the like can be solved, and the precision and the robustness of the robot instant positioning system are improved on the whole. The accumulated errors such as rotation, translation, scale drift and the like are obviously reduced, and the problems of unstable characteristic points and loss caused by a long corridor environment are effectively relieved.
Compared with the prior art, the invention has the beneficial effects that:
the invention can well overcome the problem that the point characteristics are invalid in scenes such as weak texture corridors, light violent change and the like by a visual positioning technology combining the lead-in characteristics and the point characteristics. And by means of a mechanism of tightly coupling IMU data and visual point-line characteristics, the problem of autonomous positioning failure of pure vision under the conditions of rapid movement, rotation and the like can be solved, and the precision and the robustness of the robot instant positioning system are improved on the whole. The accumulated errors such as rotation, translation, scale drift and the like are obviously reduced, and the problem of tracking loss caused by unstable feature points and weak textures is effectively solved.
Drawings
Fig. 1 is a positioning method based on information fusion of a binocular camera and an inertial navigation sensor according to the present embodiment;
fig. 2 is a result of actual environment point-line tracing in this embodiment.
Detailed Description
The invention is further described below with reference to the accompanying drawings. It should be noted that the following description gives detailed embodiments and specific operation procedures, and is intended to describe the present application in detail, but the scope of the present invention is not limited to this embodiment.
Reference will now be made in detail to the present preferred embodiments of the present invention, examples of which are illustrated in the accompanying drawings, wherein like reference numerals refer to like elements throughout.
In the description of the present invention, it should be understood that the orientation or positional relationship referred to in the description of the orientation, such as the upper, lower, front, rear, left, right, etc., is based on the orientation or positional relationship shown in the drawings, and is only for convenience of description and simplification of description, and does not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and thus, should not be construed as limiting the present invention.
In the description of the present invention, the meaning of a plurality of means is one or more, the meaning of a plurality of means is two or more, and larger, smaller, larger, etc. are understood as excluding the number, and larger, smaller, inner, etc. are understood as including the number. If the first and second are described for the purpose of distinguishing technical features, they are not to be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated or implicitly indicating the precedence of the technical features indicated.
In the description of the present invention, unless otherwise explicitly limited, terms such as arrangement, installation, connection and the like should be understood in a broad sense, and those skilled in the art can reasonably determine the specific meanings of the above terms in the present invention in combination with the specific contents of the technical solutions.
As shown in fig. 1 and 2, a flow chart of a positioning method based on information fusion of a binocular camera and an inertial navigation sensor includes the following steps:
s1, acquiring a robot peripheral image sequence;
in the embodiment, a trolley robot is used as a carrier for carrying out experiments, an STM32 single chip microcomputer is used as a bottom layer driving plate, a small foraging depth camera (the camera frequency is 30Hz, and the IMU frequency is 200Hz) is used as an external sensor, and a peripheral image sequence of the robot is acquired.
As another embodiment, the STM32 single chip microcomputer is replaced by an XMC4500 single chip microcomputer to serve as a bottom driving plate.
As another example, the small foraging depth camera is replaced with a Kinect camera.
Step S2, for the obtained image frame, using an improved FAST feature point extraction algorithm and introducing a quadtree to quickly screen feature points, so that the FAST feature points are uniformly distributed;
the extraction process of the original FAST feature points comprises the following steps: traversing each pixel point in the whole image area, investigating 16 pixel points on a circle with the pixel point as the center of a circle and 3 pixels as the radius, and if the difference between the gray value of 9 continuous points (or 12 continuous points) and the gray value of the current pixel point exceeds a preset threshold value t, regarding the points as FAST feature points, however, an original FAST feature point extraction algorithm cannot uniformly extract a certain number of feature points, so that the cluster aggregation of the feature points and the number are not easy to control, the effective image area for pose solving is small, and the camera pose solving is easy to cause large errors. Therefore, the strategy for extracting the feature points by the improved FAST feature point algorithm is as follows: dividing grids according to the size of the image, so that each grid is 15 pixel points long and 15 pixel points wide; and traversing each grid, and extracting FAST characteristic points on each grid. The image is regarded as a root node of a quadtree, namely 4 equal divisions (a binary tree can also be used here, namely halving, iterative equal divisions generate two child nodes, and the six equal divisions/eight equal divisions of the octree can be both used), if the number of the characteristic points of the root node is more than 1, the node is iterated and equally divided to generate 4 child nodes, and the iteration termination condition is that when a certain child node only contains one characteristic point or does not contain the characteristic point; and finally, the selected characteristic point is the optimal.
As another embodiment, the image division mesh size may be 30 pixels long and 30 pixels wide.
As another embodiment, the method for the quadtree to equally divide the grids can be replaced by a binary tree/a hexa-tree/an octree and the like.
Step S3, extracting line features of the acquired image frame by using an LSD (line Segment descriptor) line feature extraction algorithm, and calculating a line feature descriptor by using an LBD (line Band descriptor) algorithm;
the LSD algorithm line segment feature extraction process comprises the following steps: the size of the image is reduced to 80% of the original image through Gaussian sampling, and the sawtooth effect existing in the original image is weakened on the premise that the image is not blurred. The pixel gradients of each point in the image are ordered, and significant edge points tend to occur at points with larger gradient values. And combining the pixel points with the pixel gradient direction with a small difference to generate a linear support area.
The LBD (line Band descriptor) descriptor accumulates pixel gradient in LSD line characteristics, constructs descriptors by accumulating gradient information associated with each line in the line characteristics, calculates the mean value and standard deviation of descriptor matrixes of each line, and matches line ends according to the distance of the descriptors based on the nearest neighbor matching criterion.
The LBD descriptor is specifically: determining an LSD line segment strip area and a gradient direction; calculating gradient weight of each row of the strip; counting gradient histograms of each row to form a descriptor matrix; the mean and standard deviation were calculated and concatenated into an LBD descriptor.
And step S4, aiming at FAST feature points extracted from the previous and next image frames, tracking and matching the feature points by a direct method based on the minimized luminosity error to obtain an inter-frame estimated pose, then re-projecting the point and line features in the previous and next frames to the current frame by the estimated pose, and tracking and matching the line features by the minimized re-projection error.
The point-line feature matching process comprises the following steps: the feature matching of the front and rear frame points is carried out by a direct method based on the minimum photometric error, and in order to enhance the robustness of the system, the photometric error of the feature points and the pixel blocks around the feature points can be selected to be the minimum as a final error function.
Because the space point is represented by a floating point number, the projection pixel coordinate estimated on the second frame through pose transformation is also a floating point number, but the pixel coordinate is an integer, the invention adopts a bilinear interpolation method to solve the gray value, and the luminosity error function which needs to be minimized finally is as follows:
Eu,v＝Iv[p`]-Iu[p]
where p is the previous image frame IuFAST feature point in (1), p' represents p in the current image frame IvThe back projected coordinates of (1).
And line feature matching is achieved by minimizing the reprojection error:
Figure BDA0002964016540000081
wherein l ═ l1,l2]TIs a projected straight line,/1Representing the line feature to be matched in the previous frame, l2Representing line features to be matched in the subsequent frame, pT、qTIs the homogeneous coordinates of the two end points of the matching line segment, where the optimization does not use the distance between the two end points because the end points of the projected straight line may not be accurate due to factors such as image blur or occlusion, and therefore the directional distance along the projected straight line may not provide valid information, and it is reasonable to use the end point vertical directional distance.
Step S5, determining whether the current frame is a key frame, if not, discarding the current frame, and temporarily storing the visual information and IMU pre-integration information to transmit to the next frame; if so, acquiring IMU sensor data between the front frame and the rear frame, performing pre-integration on the IMU sensor data, and adding the current frame into a sliding window;
and performing back-end optimization by combining the feature point matched based on the direct method of the minimized photometric error in the sliding window and the line feature matched based on the minimized reprojection error and IMU pre-integration tight coupling, wherein the tight coupling refers to that intermediate data obtained by vision and IMU are processed by an optimization filter, the tight coupling needs to add the point line feature into a feature vector to be optimized together with IMU constraint, and more accurate inter-frame motion pose is output, and all inter-frame pose data are combined to form the motion path of the robot carrying the camera.
In order to ensure real-time performance, the back-end optimization needs to control the number of state quantities participating in optimization and keep the number of state quantities participating in graph optimization approximately unchanged, so that the invention adopts a sliding window method, but still needs to screen images participating in optimization to ensure that frames participating in optimization can provide abundant information without redundancy.
Wherein the key frame selection strategy is as follows: the current frame has not been considered as a key frame for 20 consecutive times; the current frame can track not less than 40 FAST point features and at least 15 line features. The total number of point features and line features in the current frame and the reference key frame does not exceed 3/4 for the reference key frame.
The method adopts a visual and IMU tight coupling mode to carry out back-end optimization, namely visual characteristic points are added into the back-end optimization, a visual residual error and an IMU residual error are combined into a combined optimization problem, and the solution of the whole optimization problem is the final state estimation. The back-end optimization uses a form of a sliding window, and an objective function to be optimized in the back-end sliding window is as follows:
E＝‖rp-JPX‖2+∑‖rI(Zi,X)‖2+∑‖rf(Zf,X)‖2+∑‖rl(Zl,X)‖2
wherein X is the state quantity to be optimized, including the position, rotation, acceleration of IMU relative to the world coordinate system and the inverse depth of the characteristic point, etc. | rp-JpX‖2For residual constraint after marginalization, rpDenotes the a priori residual, JpJacobian, | r, representing a prior termI(Zi,X)‖2Is an IMU error, rI() Representing IMU residual, ZiMeasurement term, | r, representing pre-integrationf(ZfX) represents a point feature reprojection error, | rl(Zl,X)‖2Representing line feature reprojection error, rf() Representing point feature residual terms, rl() Representing line feature residual terms, ZfObservations representing line characteristics, ZlRepresents an observed quantity of line features.
In order that those skilled in the art will better understand the disclosure, the invention will be described in further detail with reference to the accompanying drawings and specific embodiments. It is to be understood that the described embodiments are merely exemplary of the invention, and not restrictive of the full scope of the invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

Claims (10)

1. A positioning method based on information fusion of a binocular camera and an inertial navigation sensor is characterized by comprising the following steps:
s1, acquiring a robot peripheral image sequence;
step S2, for the obtained image frame, using an improved FAST feature point extraction algorithm and introducing a quadtree to quickly screen feature points, so that the FAST feature points are uniformly distributed;
step S3, extracting line features of the image frame and calculating a line feature descriptor;
step S4, performing tracking matching on FAST feature points extracted from the previous and subsequent image frames to obtain an inter-frame estimation pose, re-projecting point and line features in the previous and subsequent frames to the current frame according to the estimation pose, and performing tracking matching on the line features;
step S5, determining whether the current frame is a key frame, if not, discarding the current frame, and temporarily storing the visual information and IMU pre-integration information to transmit to the next frame;
and step S6, adding the current frame into a sliding window, performing rear-end optimization by combining the paired dotted line feature constraint in the sliding window and IMU pre-integration tight coupling, and outputting the motion pose between frames.
2. The positioning method based on information fusion of the binocular camera and the inertial navigation sensor according to claim 1, wherein the positioning method comprises the following steps: the improved FAST feature point extraction algorithm specifically comprises the following steps: dividing grids according to the size of the image; traversing each grid, extracting FAST feature points from each grid, regarding the image as an N-ary tree root node, namely N equal divisions, if the number of the root node feature points is more than 1, performing iteration equal divisions on the node to generate four child nodes, wherein the iteration termination condition is that a certain child node only contains one feature point or does not contain the feature point; and finally, the selected characteristic point is the optimal.
3. The positioning method based on information fusion of the binocular camera and the inertial navigation sensor according to claim 2, wherein the positioning method comprises the following steps: step S3 is to extract line features using the LSD line feature extraction algorithm and calculate line feature descriptors using the LBD algorithm.
4. The positioning method based on information fusion of the binocular camera and the inertial navigation sensor according to claim 3, wherein the positioning method comprises the following steps: the LSD (line Segment detector) line feature extraction algorithm specifically comprises the following steps: the image size is reduced to a% of the original image through Gaussian sampling, the sawtooth effect existing in the original image is weakened on the premise that the image is not blurred, the pixel gradients of all points in the image are sequenced, and pixel points in the pixel gradient direction are combined to generate a linear support area.
5. The positioning method based on information fusion of the binocular camera and the inertial navigation sensor according to claim 4, wherein the positioning method comprises the following steps: the LBD (line Band descriptor) algorithm specifically comprises the following steps: performing pixel gradient accumulation on the detected LSD line characteristics, constructing descriptors by accumulating gradient information associated with each line in the line characteristics, calculating the mean value and standard deviation of descriptor matrixes of each line, and matching line ends according to the distance of the descriptors on the basis of a nearest neighbor matching criterion;
the LBD descriptor is specifically: determining an LSD line segment strip area and a gradient direction; calculating gradient weight of each row of the strip; counting gradient histograms of each row to form a descriptor matrix; the mean and standard deviation were calculated and concatenated into an LBD descriptor.
6. The positioning method based on information fusion of the binocular camera and the inertial navigation sensor according to claim 5, wherein the positioning method comprises the following steps: in step S4, tracking and matching the feature points of the previous and subsequent frames is performed by a direct method based on a minimum luminosity error, and the luminosity error of the feature points and the pixel blocks around the feature points is selected as a final error function; the gray value is obtained by a bilinear interpolation method, and the minimum luminosity error function is as follows:
Eu，v＝Iv[p`]-Iu[p]
where p is the previous image frame IuFAST feature point in (1), p' represents p in the current image frame IvThe back projected coordinates of (1).
7. The positioning method based on information fusion of the binocular camera and the inertial navigation sensor according to claim 6, wherein the positioning method comprises the following steps: and the tracking matching of the line features is realized by minimizing the reprojection error:
Figure FDA0002964016530000031
wherein l ═ l1，l2]TIs a projected straight line,/1Representing the line feature to be matched in the previous frame, l2Representing line features to be matched in the subsequent frame, pT、qTAre the homogeneous coordinates of the two end points of the matching line segment.
8. The positioning method based on information fusion of the binocular camera and the inertial navigation sensor according to claim 7, wherein the positioning method comprises the following steps: in step S5, the key frame selection policy is:
the current frame, which has been considered as a key frame N consecutive times, can track not less than h FAST point features and at least m line features, or the current frame shares 3/4 no more point features and line features in the reference key frame than the reference key frame.
9. The positioning method based on information fusion of the binocular camera and the inertial navigation sensor according to claim 8, wherein the positioning method comprises the following steps: before adding the sliding window, IMU sensor data between the front frame and the rear frame of the key frame are pre-integrated.
10. The positioning method based on information fusion of the binocular camera and the inertial navigation sensor according to claim 9, wherein: the objective function to be optimized in the sliding window is:
E＝||rp-JpX||2+∑||rI(Zi，X)||2+∑||rf(Zf，X)||2+∑||rl(Zl，X)||2
wherein, X is the state quantity to be optimized, including the position, rotation, acceleration of IMU relative to the world coordinate system and the inverse depth of the characteristic point, and | rp-JpX||2For residual constraint after marginalization, rpDenotes the a priori residual, JpJacobian, r, representing a prior termI(Zi，X)||2Is an IMU error, rI() Representing IMU residual, ZiMeasurement term representing pre-integral, | | rf(ZfX) | | represents the point feature reprojection error, | | | rl(Zl，X)||2Representing line feature reprojection error, rf() Representing point feature residual terms, rl() Representing line feature residual terms, ZfObservations representing line characteristics, ZlRepresents an observed quantity of line features.
CN202110245725.7A 2021-03-05 2021-03-05 Positioning method based on information fusion of binocular camera and inertial navigation sensor Active CN113012196B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110245725.7A CN113012196B (en) 2021-03-05 2021-03-05 Positioning method based on information fusion of binocular camera and inertial navigation sensor

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110245725.7A CN113012196B (en) 2021-03-05 2021-03-05 Positioning method based on information fusion of binocular camera and inertial navigation sensor

Publications (2)

Publication Number Publication Date
CN113012196A true CN113012196A (en) 2021-06-22
CN113012196B CN113012196B (en) 2023-03-24

Family

ID=76406931

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110245725.7A Active CN113012196B (en) 2021-03-05 2021-03-05 Positioning method based on information fusion of binocular camera and inertial navigation sensor

Country Status (1)

Country Link
CN (1) CN113012196B (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114742891A (en) * 2022-03-30 2022-07-12 青岛虚拟现实研究院有限公司 Positioning system for VR display equipment
CN115578620A (en) * 2022-10-28 2023-01-06 北京理工大学 Point-line-surface multi-dimensional feature-visible light fusion slam method
CN116205947A (en) * 2023-01-03 2023-06-02 哈尔滨工业大学 Binocular-inertial fusion pose estimation method based on camera motion state, electronic equipment and storage medium

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109166149A (en) * 2018-08-13 2019-01-08 武汉大学 A kind of positioning and three-dimensional wire-frame method for reconstructing and system of fusion binocular camera and IMU
WO2019057179A1 (en) * 2017-09-22 2019-03-28 华为技术有限公司 Visual slam method and apparatus based on point and line characteristic
WO2019062291A1 (en) * 2017-09-29 2019-04-04 歌尔股份有限公司 Binocular vision positioning method, device, and system
CN109579840A (en) * 2018-10-25 2019-04-05 中国科学院上海微系统与信息技术研究所 A kind of close coupling binocular vision inertia SLAM method of dotted line Fusion Features
CN109934862A (en) * 2019-02-22 2019-06-25 上海大学 A kind of binocular vision SLAM method that dotted line feature combines
CN111222514A (en) * 2019-12-31 2020-06-02 西安航天华迅科技有限公司 Local map optimization method based on visual positioning
CN111583136A (en) * 2020-04-25 2020-08-25 华南理工大学 Method for simultaneously positioning and establishing image of autonomous mobile platform in rescue scene
CN112115980A (en) * 2020-08-25 2020-12-22 西北工业大学 Binocular vision odometer design method based on optical flow tracking and point line feature matching

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019057179A1 (en) * 2017-09-22 2019-03-28 华为技术有限公司 Visual slam method and apparatus based on point and line characteristic
WO2019062291A1 (en) * 2017-09-29 2019-04-04 歌尔股份有限公司 Binocular vision positioning method, device, and system
CN109166149A (en) * 2018-08-13 2019-01-08 武汉大学 A kind of positioning and three-dimensional wire-frame method for reconstructing and system of fusion binocular camera and IMU
CN109579840A (en) * 2018-10-25 2019-04-05 中国科学院上海微系统与信息技术研究所 A kind of close coupling binocular vision inertia SLAM method of dotted line Fusion Features
CN109934862A (en) * 2019-02-22 2019-06-25 上海大学 A kind of binocular vision SLAM method that dotted line feature combines
CN111222514A (en) * 2019-12-31 2020-06-02 西安航天华迅科技有限公司 Local map optimization method based on visual positioning
CN111583136A (en) * 2020-04-25 2020-08-25 华南理工大学 Method for simultaneously positioning and establishing image of autonomous mobile platform in rescue scene
CN112115980A (en) * 2020-08-25 2020-12-22 西北工业大学 Binocular vision odometer design method based on optical flow tracking and point line feature matching

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
李奎霖: "基于LDSO的机器人视觉定位与稠密建图的应用", 《微电子学与计算机》 *
魏武: "基于ORB-SLAM的室内机器人定位和三维稠密地图构建", 《计算机应用》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114742891A (en) * 2022-03-30 2022-07-12 青岛虚拟现实研究院有限公司 Positioning system for VR display equipment
CN115578620A (en) * 2022-10-28 2023-01-06 北京理工大学 Point-line-surface multi-dimensional feature-visible light fusion slam method
CN116205947A (en) * 2023-01-03 2023-06-02 哈尔滨工业大学 Binocular-inertial fusion pose estimation method based on camera motion state, electronic equipment and storage medium
CN116205947B (en) * 2023-01-03 2024-06-07 哈尔滨工业大学 Binocular-inertial fusion pose estimation method based on camera motion state, electronic equipment and storage medium

Also Published As

Publication number Publication date
CN113012196B (en) 2023-03-24

Similar Documents

Publication Publication Date Title
CN113012196B (en) Positioning method based on information fusion of binocular camera and inertial navigation sensor
Han et al. Deepvio: Self-supervised deep learning of monocular visual inertial odometry using 3d geometric constraints
CN108986037B (en) Monocular vision odometer positioning method and positioning system based on semi-direct method
CN110223348B (en) Robot scene self-adaptive pose estimation method based on RGB-D camera
CN111024066B (en) Unmanned aerial vehicle vision-inertia fusion indoor positioning method
CN110567469B (en) Visual positioning method and device, electronic equipment and system
CN112258600A (en) Simultaneous positioning and map construction method based on vision and laser radar
US9148650B2 (en) Real-time monocular visual odometry
CN107341814B (en) Four-rotor unmanned aerial vehicle monocular vision range measurement method based on sparse direct method
CN107167826B (en) Vehicle longitudinal positioning system and method based on variable grid image feature detection in automatic driving
CN103886107B (en) Robot localization and map structuring system based on ceiling image information
CN111210463A (en) Virtual wide-view visual odometer method and system based on feature point auxiliary matching
CN110807809A (en) Light-weight monocular vision positioning method based on point-line characteristics and depth filter
CN111882602B (en) Visual odometer implementation method based on ORB feature points and GMS matching filter
CN110599545B (en) Feature-based dense map construction system
CN109493385A (en) Autonomic positioning method in a kind of mobile robot room of combination scene point line feature
CN113223045A (en) Vision and IMU sensor fusion positioning system based on dynamic object semantic segmentation
CN113326769B (en) High-precision map generation method, device, equipment and storage medium
CN113532420A (en) Visual inertial odometer method integrating point-line characteristics
CN112541423A (en) Synchronous positioning and map construction method and system
CN114563000B (en) Indoor and outdoor SLAM method based on improved laser radar odometer
CN114812573A (en) Monocular visual feature fusion-based vehicle positioning method and readable storage medium
CN113345032A (en) Wide-angle camera large-distortion image based initial image construction method and system
CN112419411A (en) Method for realizing visual odometer based on convolutional neural network and optical flow characteristics
CN113155126A (en) Multi-machine cooperative target high-precision positioning system and method based on visual navigation

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