CN113052908B - Mobile robot pose estimation algorithm based on multi-sensor data fusion - Google Patents

Mobile robot pose estimation algorithm based on multi-sensor data fusion Download PDF

Info

Publication number
CN113052908B
CN113052908B CN202110422808.9A CN202110422808A CN113052908B CN 113052908 B CN113052908 B CN 113052908B CN 202110422808 A CN202110422808 A CN 202110422808A CN 113052908 B CN113052908 B CN 113052908B
Authority
CN
China
Prior art keywords
laser
data
time
axis
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110422808.9A
Other languages
Chinese (zh)
Other versions
CN113052908A (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.)
Nanjing Tech University
Original Assignee
Nanjing Tech University
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 Nanjing Tech University filed Critical Nanjing Tech University
Priority to CN202110422808.9A priority Critical patent/CN113052908B/en
Publication of CN113052908A publication Critical patent/CN113052908A/en
Application granted granted Critical
Publication of CN113052908B publication Critical patent/CN113052908B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Computational Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Pure & Applied Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Computing Systems (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

The invention provides a mobile robot pose estimation algorithm based on multi-sensor data fusion, which comprises the following steps: step1: obtaining laser data, image data and IMU data; step2: laser is marked on a black calibration ruler, laser data and image data are calibrated in a combined mode, and a rotation matrix and a translation matrix of the laser data under a camera coordinate system are obtained; step3: acquiring image information and laser point cloud data between two adjacent key frames, projecting three-dimensional laser data onto an image through a rotation matrix and a translation matrix, and fusing the two to enable the image data to obtain depth information to form depth image data; step4: performing feature tracking and motion estimation on the image containing the depth information; step5: performing pre-integration processing on IMU information between two adjacent key frames; stsp6: and carrying out joint optimization on the IMU information and the depth image data to obtain a pose estimation result. Compared with the traditional calibration method, the method is simple to operate and high in calibration precision.

Description

Mobile robot pose estimation algorithm based on multi-sensor data fusion
Technical Field
The invention relates to the technical field of autonomous navigation, in particular to a mobile robot pose estimation algorithm based on multi-sensor fusion.
Background
In order to realize accurate positioning and navigation of the mobile robot in a complex environment and avoid the influence of factors such as ambient light change, scene rapid movement, object dynamics and the like on positioning and mapping, the SLAM scheme adopting multi-sensor fusion becomes a main research direction of SLAM technology of the future mobile robot. Vision sensors, laser sensors, and inertial navigation sensors have been widely used in intelligent robot navigation, and by fusing these data, more accurate pose and three-dimensional maps can be provided to the robot.
Disclosure of Invention
The utility model provides a mobile robot pose estimation algorithm based on multisensor data fusion, this document has designed the pose estimation algorithm based on multisensor data fusion, and laser and monocular camera jointly mark at first, compare in traditional calibration method, easy operation and calibration precision are higher. The visual information and the depth information are fused, and the pose estimation is carried out by carrying out joint optimization on the visual information and the depth information and the IMU information after feature extraction and matching.
In order to solve the technical problems, the invention adopts the following technical scheme:
a mobile robot pose estimation algorithm based on multi-sensor data fusion comprises the following steps:
step1: obtaining laser data fed back by a laser radar of the mobile robot, image data fed back by a monocular camera and IMU data fed back by an inertial navigation positioning device;
step2: laser is marked on a black calibration ruler, laser data and image data are calibrated in a combined mode, and a rotation matrix and a translation matrix of the laser data under a camera coordinate system are obtained;
step3: acquiring image information and laser point cloud data between two adjacent key frames, projecting three-dimensional laser data onto an image through a rotation matrix and a translation matrix, and fusing the two to enable the image data to obtain depth information to form depth image data;
step4: performing feature tracking and motion estimation on the image containing the depth information;
step5: performing pre-integration processing on IMU information between two adjacent key frames;
stsp6: and carrying out joint optimization on the IMU information and the depth image data to obtain a pose estimation result.
Preferably, the laser radar, the monocular camera and the inertial navigation positioning device in step1 are all arranged on the mobile robot,
the working frequency of the laser radar is 5-20Hz, and the detection angle in the horizontal direction is 360 degrees; the angle which can be detected in the vertical direction is 30 degrees, 15 degrees above and below respectively, and the scanning effective radius is 1 meter to 100 meters;
the inertial navigation system is composed of a magnetometer, a gyroscope and an accelerometer and is used for detecting and obtaining a triaxial rotation angle and triaxial acceleration, carrying out initial calibration on a direction angle and correcting drift on a course angle;
the resolution of the monocular camera is 2080×1552, and the highest operating frequency is 60Hz.
Further, step2 performs laser abnormal data rejection in advance, and the laser abnormal data rejection formula is as follows:
wherein t is 1 、t 2 、t 3 、t 4 Respectively represent different moments, and t 1 ＜t 2 ＜t 3 ＜t 4 ，l 1 Indicating that the laser ray is at t 1 Distance between time laser emission point and imaging point, l 2 Indicating that the laser ray is at t 2 Distance between time laser emission point and imaging point, l 3 Indicating that the laser ray is at t 3 Distance between time laser emission point and imaging point, l 4 Indicating that the laser ray is at t 4 Distance between time laser emission point and imaging point, c 1 Indicating that the laser ray is at t 1 Time sum t 2 Distance between two imaging points at moment, c 2 Indicating that the laser ray is at t 2 Time sum t 3 Distance between two imaging points at moment, c 3 Indicating that the laser ray is at t 3 Time sum t 4 The distance between the two imaging points at the moment, arccos is the inverse cosine in the inverse trigonometric function, v represents the angular velocity of the laser rotation,
(1) when meeting the publicIf the formula (1) does not satisfy the formula (2), rejecting the laser beam at t 2 Imaging points at time;
(2) when the formula (2) is satisfied and the formula (3) is not satisfied, rejecting the laser ray at t 3 Imaging points at time;
(3) when the formulas (1) and (3) are simultaneously satisfied, rejecting the laser ray at t 1 Time sum t 4 Imaging point of time.
Further, the joint calibration in Step2 includes the following steps:
step2.1: setting the coordinate P of laser data in a camera coordinate system C The calculation formula is as follows:
P C ＝RP L +t (4)
wherein P is C Representing the coordinates of the laser data in the camera coordinate system, is composed of three-dimensional vectors (x C ，y C ，z C ) T ，x C ，y C ，z C The coordinates of the laser data in the x-coordinate system, y-coordinate system, and z-coordinate system of the camera, respectively, R represents a rotation matrix, and is represented by a three-dimensional vector (θ x ，θ y ，θ z ) Constitution, θ x ，θ y ，θ z For rotation angles about three axes of the coordinate system x, y, z, respectively, t represents a translation matrix, represented by a three-dimensional vector (t x ，t y ，t z ) Constitution, t x ，t y ，t z The translation distances P in the three directions of the x, y and z axes of the coordinate system L Is the coordinates of the laser data in the laser's own coordinate system, which is defined by a three-dimensional vector (x L ，y l ，z L ) T Composition, x L ，y l ，z L Respectively representing the coordinates of the laser data in the x-axis, the y-axis and the z-axis of a laser coordinate system;
step2.2: introducing an internal parameter change matrix I of a camera, and establishing a connection between image data and laser data:
P＝IP C (5)
wherein P is the coordinates of the image data in the camera coordinate system, and is defined by three-dimensional vectors (u, v, l) T The composition, u, v, l, represents the image data in the x-axis of the camera coordinate systemCoordinates in the y axis and the z axis, and the camera internal parameter change matrix I is as follows:
wherein I is a camera internal parameter change matrix, f x 、f y 、f z 、u x And u y Are all the parameters of the transformation, and the transformation parameters,
step2.3: based on Gaussian Newton iteration method, solving optimal transformation parameters, and obtaining parameters of a rotation matrix R and a translation matrix t:
P＝f(Pc，x) (7)
where x is a transformation parameter matrix, formed by (f x ，f y ，u x ，u y ，θ x ，θ y ，θ z ，t x ，t y ，t z ) The composition f (Pc, x) represents a function composed of coordinates Pc and x of the laser data in the camera coordinate system, P i Representing the coordinates, pc, of the ith image data in the camera coordinate system i Representing the coordinates of the ith laser data in the camera coordinate system, x i Represents the transformation parameter matrix corresponding to the ith laser data,x represents the time of k i ，/>At time k, at +.>The first-order Taylor expansion is performed nearby, < >>For g (Pc, x) with respect to the first derivative of x, Δx k Representing a descent vector, +.>Representing vector P i Vector->The sum of squares of the straight line distances, arg min, represents the minimum of the solution parameters.
Further, step3, t j-1 From time to t j The projection of the moment uses the following formula:
ΔS j-1，j ＝R x R y R z Δx (11)
wherein the method comprises the steps of
Wherein DeltaR j-1，j Three-axis rotation speed, deltaS, of motion estimation between j-1 frames and j frames of images calculated for visual mileage j-1，j The calculated three-axis distance of motion estimation between the j-1 frame and the j frame images for the visual mileage is represented by Rx, ry and Rz, which are the calculated rotational speeds of the x, y and z axes for the visual mileage, roll, pitch, yaw are the attitude angles of the x, y and z axes for the j frame image time, cos (roll) represents the cosine of the attitude angle of the x axis for the j frame image time, sin (roll) represents the sine of the attitude angle of the x axis for the j frame image time, cos (pitch) represents the y axis for the j frame image timeSin (pitch) represents a sine value of the attitude angle of the y-axis at the j-frame image time, cos (yaw) represents a sine value of the attitude angle of the z-axis at the j-frame image time, sin (yaw) represents a sine value of the attitude angle of the z-axis at the j-frame image time, x j 、y j 、z j The displacement of the x-axis, the y-axis and the z-axis calculated by the visual mileage calculation respectively representing the moment of j frames of images, deltax represents a displacement matrix, T represents a time matrix, T j 、t j-1 Respectively representing the t and t-1 times.
Further, step4 includes the steps of:
step4.1: extracting feature key points from the image containing depth information, calculating descriptors, and finishing a feature extraction process;
step4.2: matching the extracted features to obtain feature matching results with fewer mismatching numbers;
step4.3: and calculating a motion transformation matrix by using the matched characteristic pairs.
Preferably, the descriptor is calculated in step4.1, and the hamming distance is used to represent the similarity between two feature points, and the smaller the distance is, the greater the similarity is.
Preferably, step4.3 adopts a random sample consistency algorithm to remove mismatching points, and purifies matched feature pairs.
Furthermore, step5 pre-integration processing comprises IMU modeling, a kinematic model, IMU pre-integration, a noise propagation model and a drift model, and IMU observation residual errors and covariances required in the optimization process are obtained through an IMU pre-integration algorithm.
Further, the step6 joint optimization includes the following steps:
step5.1, supposing that the visual observation value and the IMU observation value are mutually independent;
step5.2: applying a Bayes formula, and estimating the state quantity of the system by adopting the maximum posterior probability;
step5.3, under the assumption of zero-mean Gaussian noise distribution, converting the zero-mean Gaussian noise distribution into an optimization problem;
step5.4: the optimized objective function residual is composed of three parts, namely a sliding window initial value residual, an IMU observation residual and a visual observation residual.
The invention has the beneficial effects that:
the utility model provides a mobile robot pose estimation algorithm based on multisensor data fusion, this document has designed the pose estimation algorithm based on multisensor data fusion, and laser and monocular camera jointly mark at first, compare in traditional calibration method, easy operation and calibration precision are higher. The visual information and the depth information are fused, and the pose estimation is carried out by carrying out joint optimization on the visual information and the depth information and the IMU information after feature extraction and matching.
(1) The invention is based on multi-sensor data fusion, and the fusion of the data can provide more accurate pose and three-dimensional map for the robot;
(2) The invention creatively provides a laser abnormal data eliminating formula, and the eliminated data is more accurate.
(3) According to the invention, black calibration paper is used, and the combined calibration parameters of the laser and the camera are solved, so that the accurate calibration parameters of the three-dimensional laser and the monocular camera are obtained.
(4) The invention solves the optimal transformation parameters based on the Gaussian Newton iteration method, and obtains the parameters of the rotation matrix R and the translation matrix t.
(5) Based on the problem that the image feature points need to be fused with scale information, the three-dimensional laser data is projected onto the image by a joint calibration method, so that the actual three-dimensional coordinates of part of the image data are obtained.
(6) In the joint optimization link, the visual information and the IMU information are fused, and the pose of the camera is obtained through optimization. And optimizing the key frame state in the camera motion process by adopting a sliding window method.
(7) The invention performs pose tracking comparison with the visual inertial odometer system OKVIS through actual data measurement and calculation, and the algorithm is superior to the OKVIS system in positioning accuracy and can be applied in real time.
Drawings
Fig. 1 is a flowchart of an algorithm provided by the present invention.
Detailed Description
The mobile robot pose estimation algorithm based on multi-sensor data fusion is further described in detail below with reference to the accompanying drawings and a specific implementation method.
Example 1
As shown in fig. 1, a flow chart of a mobile robot pose estimation algorithm for multi-sensor data fusion mainly comprises the following steps:
step1: obtaining laser data fed back by a laser radar of the mobile robot, image data fed back by a monocular camera and IMU data fed back by an inertial navigation positioning device;
step2: laser is marked on a black calibration ruler, laser data and image data are calibrated in a combined mode, and a rotation matrix and a translation matrix of the laser data under a camera coordinate system are obtained;
step3: acquiring image information and laser point cloud data between two adjacent key frames, projecting three-dimensional laser data onto an image through a rotation matrix and a translation matrix, and fusing the two to enable the image data to obtain depth information to form depth image data; step4: performing feature tracking and motion estimation on the image containing the depth information;
step5: performing pre-integration processing on IMU information between two adjacent key frames;
stsp6: and carrying out joint optimization on the IMU information and the depth image data to obtain a pose estimation result.
Example 2
Example 2 differs from example 1 only in that: step was optimized.
Specifically, in step2, firstly, a monocular camera, a laser radar and an inertial navigation positioning device are mounted on a mobile robot platform, wherein the working frequency of the laser radar is 5-20Hz, and the angle which can be detected in the horizontal direction is 360 degrees; the angle which can be detected in the vertical direction is 30 degrees, 15 degrees above and below each, and the scanning effective radius is 1 meter to 100 meters. The inertial navigation system consists of a magnetometer, a gyroscope and an accelerometer, and can detect and obtain a triaxial rotation angle and triaxial acceleration, and perform initial calibration on a direction angle and correction drift on a course angle. The resolution of the monocular camera used was 2080×1552, with a maximum operating frequency of 60Hz.
To obtain accurate three-dimensional laser and monocular camera calibration parameters, accurate laser and visual data matching pairs are required, black calibration paper is used here, 54cm long and 39cm wide.
The joint calibration parameters of the laser and the camera are calculated, namely, a rotation matrix Rθ is needed to be calculated x ，θ y ，θ z ]And a translation matrix t [ t ] x ，t y ，t z ]So that the formula is established, wherein P L ＝(x L ，y l ，z L ) T Is the coordinate of the three-dimensional laser data under the coordinate system of the laser, P C ＝(x C ，y C ，z C ) T Is the coordinates of the laser data in the camera coordinate system, p= (u, v, l) T Is the coordinates of the three-dimensional laser spot in the camera coordinate system, such as equation (1), equation (2) and equation (3). The monocular camera used herein obtains the camera internal reference I matrix by a Zhengyou calibration method.
P C ＝RP L +t (14)
p＝IP C ＝I(RP L +t) (15)
According to the obtained corner pairs formed by the laser corner points and the image corner points, solving a rotation translation matrix between two coordinate systems by adopting a Gauss Newton method, and writing laser data and image data corner points into a homogeneous form P respectively for the convenience of calculation i ＝(x i ，y i ，z i ，1)、P i ＝(u i ，v i ,1). Conversion of laser data from laser coordinates to image coordinate System you parameter is x (f x ，f y ，u x ，u y ，θ x ，θ y ，θ z ，t x ，t y ，t z ) Wherein f x ，f y ，u x ，u y Is the internal reference of the camera, θ x ，θ y ，θ z To the rotation angle around three axes, t x ，t y ，t z Is the translation distance in three directions of the coordinate system. The transformation of the laser data from the laser coordinate system to the camera coordinate system can be represented by equation (4):
P＝f(Pc，x) (17)
where x is a transformation parameter matrix, formed by (f x ，f y ，u x ，u y ，θ x ，θ y ，θ z ，t x ，t y ，t z ) The composition f (Pc, x) represents a function composed of coordinates Pc and x of the laser data in the camera coordinate system, P i Representing the coordinates, pc, of the ith image data in the camera coordinate system i Representing the coordinates of the ith laser data in the camera coordinate system, x i Represents the transformation parameter matrix corresponding to the ith laser data,x represents the time of k i 。
The optimization solution is carried out by adopting a Gauss Newton iteration method, and at the moment k, at the momentThe first-order Taylor expansion is performed nearby, there is
Wherein the method comprises the steps ofFor the first derivative of g (Pc, x) with respect to x, a jacobian matrix is calculated. The goal here is to find the descent vector Deltax k Minimizing this formula as follows
At time k, at +.>The first-order Taylor expansion is performed nearby, < >>For g (Pc, x) with respect to the first derivative of x, Δx k Representing a descent vector, +.>Representing vector P i Vector->The sum of squares of the straight line distances, arg min, represents the minimum of the solution parameters
Example 3
Example 3 differs from example 2 only in that: step3 is specifically defined.
Specifically, in step3, image information and laser point cloud data between two adjacent key frames are acquired, three-dimensional laser data are projected onto an image, and the two are fused, so that the image data obtain depth information;
based on the problem that the image feature points need to be fused with scale information, three-dimensional laser data are projected onto an image through the joint calibration method, so that part of image data can obtain actual three-dimensional coordinates.
Since the camera selects the working frequency to be 60Hz and the laser working frequency to be 10Hz, the laser data time stamp is not necessarily completely aligned with the image data time stamp, the laser data is obtained at the moment t, and the laser data at the moment is supposed to be projected to the moment t j-1 On the image at time instant because from time instant t to time instant t j-1 Time of dayMovement has already occurred, so that a certain rotational-translational transformation of the laser light is required.
Assuming that the motion between the k-1 frame image and the k frame image needs to be solved, the visual odometer motion before the k-1 frame is calculated, and the scale information of the characteristic points on the k-1 frame image needs to be obtained at the moment, then laser at the latest moment t needs to be projected to the k-1 frame image, then certain rotation translation transformation needs to be carried out on the laser data, the transformation process is as follows, and the assumption is made that t j-1 From time to t j Is a linear model.
ΔS j-1，j ＝R x R y R z Δx (22)
Wherein the method comprises the steps of
Wherein DeltaR j-1，j Three-axis rotation speed (θ) of motion estimation between j-1 frames and j frames of images calculated for visual mileage x ，θ y ，θ z ) Roll, pitch, yaw is three-axis attitude angle, x of j frame image moment j 、y j 、z j For this time, the three-axis displacement calculated by the visual mileage at that time was calculated.
The internal reference of the gray level camera is known, then the combined calibration parameters of the three-dimensional laser and the monocular camera are obtained through calculation by the combined calibration method, and the three-dimensional laser is projected onto the two-dimensional gray level image.
Example 4
Example 4 differs from example 3 only in that a specific implementation is given for step4.
Specifically, in Step4, since feature extraction and matching are only one link in the visual odometer, if too long is taken on the link, the efficiency of the whole system is necessarily affected, so that the ORB algorithm with good performance and extremely obvious speed advantage is selected. The feature point extraction is carried out on the image by adopting the FAST algorithm, and in order to solve the problem of rotation invariance, the ORB uses a gray centroid method:
assuming that an image block with a feature point as a geometric center is B, the moment of the image block is defined as:
m pq ＝∑ x，y∈B x p y q I(x，y) p，q＝{0，1} (25)
wherein I (x, y) is the gray value of the point (x, y). The centroid of the image block is easily obtained by the moment of the image block:
connecting the feature point (geometric center O of the image block) with the centroid C to obtain a direction vectorThe direction of the feature points can be defined as follows:
the FAST corner has a direction by a gray centroid method. After the key points are obtained, the descriptors of the key points are calculated. The function of the descriptor is to match whether two feature points are the same feature point. The general descriptor is constructed by drawing a plurality of circles around the key points, and calculating the pixel points in the circles to obtain a vector. This vector can reflect the characteristics of the pixels surrounding this keypoint. I.e. it may describe the surrounding characteristics of this feature point at present. Because the same feature points are certainly similar around, a descriptor can be used to determine whether two feature points are identical.
For binary BRIEF descriptors, pass throughHamming distance is often used to represent the degree of similarity between two feature points, the smaller the distance, the greater the degree of similarity. Let a=a 1 a 2 ...a n And b=b 1 b 2 ...b n Is two binary strings, wherein a n And b n The hamming distances between a and B are described as:
for each feature point, only the hamming distances to all feature points of the matching image need be measured, and then the nearest neighbor is selected as its initial corresponding match. Setting a threshold valueWhen the distance between descriptors is greater than +.>When the match is deleted as a false match. Although the thresholding method is adopted to primarily filter out some mismatching, there are still some inaccurate matching, and the matching result needs to be purified by adopting a RANSAC (random sample consensus) algorithm. After the RANSAC algorithm further eliminates the mismatching, a large number of inaccurate matching is reduced, the matching accuracy is improved, and a more reliable basis is provided for the following pose estimation.
Example 5
Example 5 differs from example 1 only in that: one specific implementation is given for step5.
Step5, and for IMU information, pre-integration processing is performed. Firstly, carrying out mathematical modeling on IMU sensor data, and constructing an IMU measurement model formula to show by taking measurement white noise and random walk noise in the measurement process into consideration.
W represents the world coordinate system and B represents the IMU coordinate system. White noise obeys gaussian distribution, i.e.While the random walk noise carries out Brownian motion, which is differentiated into Gaussian distribution noise, i.e.>Wherein:
establishing a kinematic differential equation of the system:
in a visual odometer system, the original state quantity x of the system i Is constantly optimized and changed, so that the integration process needs to be carried out again, the process is called as a weighted integration process, and the part irrelevant to the state quantity before the system is separated, so that the following result is obtained:
wherein:
definition of IMU Pre-integral variable Y IMU ＝(ΔR ij ,Δv ij ,Δp ij ). Firstly, IMU pre-integration is carried out to obtain an IMU pre-integration variable Y IMU When the system estimates state R at time i i ,v i ，p i When changing, we calculate the estimated state R of the system at time j quickly by pre-integration j ,v j ，p j 。
Pre-integral variable Y as defined herein IMU ＝(ΔR ij ,Δv ij ,Δp ij ) Can be separated into noise-free partsAnd its corresponding noise->Definitions->For IMU observance, its corresponding noise +.>The covariance of (1) is the covariance Σ in the optimization objective function ij . Next, the IMU observables are analyzed>Corresponding residual->Covariance Σ ij
Example 6
Example 6 differs from example 1 only in that a specific implementation is given for step 6.
Specifically, in Step6, in the joint optimization link, the visual information and the IMU information are fused, and the pose of the camera is obtained through optimization. And optimizing the key frame state in the camera motion process by adopting a sliding window method, wherein the observation value of the system is divided into two parts, namely a vision measurement value and an IMU measurement value. Defining the visual measurement of the keyframe time i system as set C i . At C i The 3-dimensional characteristic point l observed by the middle camera is marked as z il . Defining all IMU observations located between keyframe I and keyframe j as set I ij . At the time of optimization, the observation value of the known system is Z k Solving state quantity X of system k . Assuming that the vision observation value and the IMU observation value are mutually independent, and the vision measurement at the moment i is only related to the state at the moment i, applying a Bayesian formula, estimating the state quantity of the system by adopting the maximum posterior probability, converting the maximum posterior distribution problem into an optimization problem under the assumption of zero-mean Gaussian noise distribution, and defining an optimization objective function as follows:
in the optimized objective function, the residual error consists of three parts, namely a sliding window initial value residual error r 0 IMU observation residualAnd visual observation residuals, which correspond to covariances Σ respectively 0 ，Σ ij ，Σ C . Based on the assumption that the visual observation value and the IMU observation value are mutually independent, the visual observation residual error and the IMU observation residual error are mutually independent, and the corresponding covariance is mutually independent.
Example 7
Example 7 differs from example 1 only in that: the step2 adds a culling formula for abnormal data.
Specifically, step2 performs laser abnormal data rejection in advance, and the laser abnormal data rejection formula is:
wherein t is 1 、t 2 、t 3 、t 4 Respectively represent different moments, and t 1 ＜t 2 ＜t 3 ＜t 4 ，l 1 Indicating that the laser ray is at t 1 Distance between time laser emission point and imaging point, l 2 Indicating that the laser ray is at t 2 Distance between time laser emission point and imaging point, l 3 Indicating that the laser ray is at t 3 Distance between time laser emission point and imaging point, l 4 Indicating that the laser ray is at t 4 Distance between time laser emission point and imaging point, c 1 Indicating that the laser ray is at t 1 Time sum t 2 Distance between two imaging points at moment, c 2 Indicating that the laser ray is at t 2 Time sum t 3 Distance between two imaging points at moment, c 3 Indicating that the laser ray is at t 3 Time sum t 4 The distance between the two imaging points at the moment, arccos is the inverse cosine in the inverse trigonometric function, v represents the angular velocity of the laser rotation,
(1) when the formula (1) is satisfied and the formula (2) is not satisfied, rejecting the laser ray at t 2 Imaging points at time;
(2) when the formula (2) is satisfied and the formula (3) is not satisfied, rejecting the laser ray at t 3 Imaging points at time;
(3) when the formulas (1) and (3) are simultaneously satisfied, rejecting the laser ray at t 1 Time sum t 4 Imaging point of time.
The pose estimation algorithm based on multi-sensor data fusion is designed, and firstly, laser and monocular camera are calibrated in a combined mode, and compared with a traditional calibration method, the pose estimation algorithm is simple to operate and high in calibration accuracy. The visual information and the depth information are fused, and the pose estimation is carried out by carrying out joint optimization on the visual information and the depth information and the IMU information after feature extraction and matching.
The algorithms modified herein were compared to the well-known visual odometer system okis for pose tracking, and experiments were performed using sequences in the EuRoC dataset, mh_05_difficult.
Table 1MH_05_difficult sequence experiment results
Where ATE represents the absolute track error, the difference between the real point and the estimated track point is described. PRE then represents the relative pose error, describing the error in the relative motion between frames. From the comparison table, the improved algorithm ATE, PRE and average rotation error are lower than that of the okrais system, the average time consumption is higher than that of the okrais system, and the average time consumption is lower than 30ms and is far lower than the interval of two images of the data set by 50ms, which indicates that the algorithm is superior to that of the okrais system in positioning accuracy and can be applied in real time.
In the description provided herein, numerous specific details are set forth. However, it is understood that embodiments of the invention may be practiced without these specific details. In some instances, well-known methods, structures and techniques have not been shown in detail in order not to obscure an understanding of this description.
While the invention has been described with respect to a limited number of embodiments, those skilled in the art, having benefit of the above description, will appreciate that other embodiments are contemplated within the scope of the invention as described herein. Furthermore, it should be noted that the language used in the specification has been principally selected for readability and instructional purposes, and may not have been selected to delineate or circumscribe the inventive subject matter. Accordingly, many modifications and variations will be apparent to those of ordinary skill in the art without departing from the scope and spirit thereof. The disclosure of the present invention is intended to be illustrative, but not limiting, of the scope of the invention.
The foregoing is only a preferred embodiment of the invention, it being noted that: it will be apparent to those skilled in the art that various modifications and adaptations can be made without departing from the principles of the present invention, and such modifications and adaptations are intended to be comprehended within the scope of the invention.

Claims (7)

1. The mobile robot pose estimation algorithm based on multi-sensor data fusion is characterized by comprising the following steps of:
step1: obtaining laser data fed back by a laser radar of the mobile robot, image data fed back by a monocular camera and IMU data fed back by an inertial navigation positioning device;
step2: laser is marked on a black calibration ruler, laser data and image data are calibrated in a combined mode, and a rotation matrix and a translation matrix of the laser data under a camera coordinate system are obtained;
step3: acquiring image information and laser point cloud data between two adjacent key frames, projecting three-dimensional laser data onto an image through a rotation matrix and a translation matrix, and fusing the two to enable the image data to obtain depth information to form depth image data;
step4: performing feature tracking and motion estimation on the image containing the depth information;
step5: performing pre-integration processing on IMU information between two adjacent key frames;
step6: performing joint optimization on the IMU information and the depth image data to obtain a pose estimation result;
wherein, joint calibration in Step2 includes the following steps:
step2.1: setting the coordinate P of laser data in a camera coordinate system C The calculation formula is as follows:
P C ＝RP L +t (4)
wherein P is C Representing the coordinates of the laser data in the camera coordinate system, is composed of three-dimensional vectors (x C ,y C ,z C ) T ，x C ,y C ,z C The laser data are respectively in the x coordinate system, the y coordinate system and the z coordinate of the cameraAnd the coordinates below the system, R represents a rotation matrix, which is represented by a three-dimensional vector (θ x ,θ y ,θ z ) Constitution, θ x ,θ y ,θ z For rotation angles about three axes of the coordinate system x, y, z, respectively, t represents a translation matrix, represented by a three-dimensional vector (t x ,t y ,t z ) Constitution, t x ,t y ,t z The translation distances P in the three directions of the x, y and z axes of the coordinate system L Is the coordinates of the laser data in the laser's own coordinate system, which is defined by a three-dimensional vector (x L ,y l ,z L ) T Composition, x L ,y l ,z L Respectively representing the coordinates of the laser data in the x-axis, the y-axis and the z-axis of a laser coordinate system;
step2.2: introducing an internal parameter change matrix I of a camera, and establishing a connection between image data and laser data:
P＝IP C (5)
wherein P is the coordinates of the image data in the camera coordinate system, and is defined by three-dimensional vectors (u, v, l) T The composition, u, v, l, represents the coordinates of the image data in the x-axis, y-axis and z-axis of the camera coordinate system, and the camera internal parameter change matrix I is:
wherein I is a camera internal parameter change matrix, f x 、f y 、f z 、u x And u y Are all the parameters of the transformation, and the transformation parameters,
step2.3: based on Gaussian Newton iteration method, solving optimal transformation parameters, and obtaining parameters of a rotation matrix R and a translation matrix t:
P＝f(Pc,x) (7)
where x is a transformation parameter matrix, formed by (f x ,f y ,u x ,u y ,θ x ,θ y ,θ z ,t x ,t y ,t z ) The composition f (Pc, x) represents a function composed of coordinates Pc and x of the laser data in the camera coordinate system, P i Representing the coordinates, pc, of the ith image data in the camera coordinate system i Representing the coordinates of the ith laser data in the camera coordinate system, x i Represents the transformation parameter matrix corresponding to the ith laser data,x represents the time of k i ，/>At time k, at +.>The first-order Taylor expansion is performed nearby, < >>For g (Pc, x) with respect to the first derivative of x, Δx k Representing a descent vector, +.>Representing vector P i Vector->The square sum of the straight line distances, arg min represents the minimum value of the solving parameters;
step3 t j-1 From time to t j The projection of the moment uses the following formula:
ΔS j-1,j ＝R x R y R z Δx (11)
wherein the method comprises the steps of
Wherein DeltaR j-1，j Three-axis rotation speed, deltaS, of motion estimation between j-1 frames and j frames of images calculated for visual mileage j-1，j The three-axis distance of motion estimation between the j-1 frame and the j frame images calculated for the visual mileage, rx, ry and Rz represent the rotational speeds of the x, y and z axes calculated for the visual mileage, roll, pitch, yaw are the attitude angles of the x, y and z axes at the j frame image time, cos (roll) represents the cosine value of the attitude angle of the x axis at the j frame image time, sin (roll) represents the sine value of the attitude angle of the x axis at the j frame image time, cos (pitch) represents the cosine value of the attitude angle of the y axis at the j frame image time, sin (pitch) represents the sine value of the attitude angle of the y axis at the j frame image time, cos (yaw) represents the cosine value of the attitude angle of the z axis at the j frame image time, sin (yaw) represents the sine value of the attitude angle of the z axis at the j frame image time, x j 、y j 、z j The displacement of the x-axis, the y-axis and the z-axis calculated by the visual mileage calculation respectively representing the moment of j frames of images, deltax represents a displacement matrix, T represents a time matrix, T j 、t j-1 Respectively representing the t and t-1 moments;
joint optimization in step6 includes the steps of:
step5.1, supposing that the visual observation value and the IMU observation value are mutually independent;
step5.2: applying a Bayes formula, and estimating the state quantity of the system by adopting the maximum posterior probability;
step5.3, under the assumption of zero-mean Gaussian noise distribution, converting the zero-mean Gaussian noise distribution into an optimization problem;
step5.4: the optimized objective function residual is composed of three parts, namely a sliding window initial value residual, an IMU observation residual and a visual observation residual.
2. The mobile robot pose estimation algorithm based on multi-sensor data fusion according to claim 1, wherein the laser radar, the monocular camera and the inertial navigation positioning device in step1 are all installed on the mobile robot,
the working frequency of the laser radar is 5-20Hz, and the detection angle in the horizontal direction is 360 degrees; the angle which can be detected in the vertical direction is 30 degrees, 15 degrees above and below respectively, and the scanning effective radius is 1 meter to 100 meters;
the inertial navigation system is composed of a magnetometer, a gyroscope and an accelerometer and is used for detecting and obtaining a triaxial rotation angle and triaxial acceleration, carrying out initial calibration on a direction angle and correcting drift on a course angle;
the resolution of the monocular camera is 2080×1552, and the highest operating frequency is 60Hz.
3. The mobile robot pose estimation algorithm based on multi-sensor data fusion according to claim 1, wherein Step2 performs laser abnormal data rejection in advance, and a laser abnormal data rejection formula is as follows:
wherein t is 1 、t 2 、t 3 、t 4 Respectively represent different moments, and t 1 ＜t 2 ＜t 3 ＜t 4 ，l 1 Indicating that the laser ray is at t 1 Distance between time laser emission point and imaging point, l 2 Indicating that the laser ray is at t 2 Distance between time laser emission point and imaging point, l 3 Indicating that the laser ray is at t 3 Distance between time laser emission point and imaging point, l 4 Indicating that the laser ray is at t 4 Distance between time laser emission point and imaging point, c 1 Indicating that the laser ray is at t 1 Time sum t 2 Distance between two imaging points at moment, c 2 Indicating that the laser ray is at t 2 Time sum t 3 Distance between two imaging points at moment, c 3 Indicating that the laser ray is at t 3 Time sum t 4 The distance between the two imaging points at the moment, arccos is the inverse cosine in the inverse trigonometric function, v represents the angular velocity of the laser rotation,
(1) when the formula (1) is satisfied and the formula (2) is not satisfied, rejecting the laser ray at t 2 Imaging points at time;
(2) when the formula (2) is satisfied and the formula (3) is not satisfied, rejecting the laser ray at t 3 Imaging points at time;
(3) when the formulas (1) and (3) are simultaneously satisfied, rejecting the laser ray at t 1 Time sum t 4 Imaging point of time.
4. The mobile robot pose estimation algorithm based on multi-sensor data fusion according to claim 1, wherein Step4 comprises the steps of:
step4.1: extracting feature key points from the image containing depth information, calculating descriptors, and finishing a feature extraction process;
step4.2: matching the extracted features to obtain feature matching results with fewer mismatching numbers;
step4.3: and calculating a motion transformation matrix by using the matched characteristic pairs.
5. The multi-sensor data fusion-based mobile robot pose estimation algorithm according to claim 4, wherein the descriptor is calculated in step4.1, and a hamming distance is used to represent the similarity between two feature points, and the smaller the distance is, the greater the similarity is.
6. The mobile robot pose estimation algorithm based on multi-sensor data fusion according to claim 4, wherein step4.3 adopts a random sample consistency algorithm to reject mismatching points, and the matched feature pairs are purified.
7. The mobile robot pose estimation algorithm based on multi-sensor data fusion according to claim 1, wherein step5 pre-integration processing comprises an IMU modeling part, a kinematic model part, an IMU pre-integration part, a noise propagation model part and a drift model part, and IMU observation residual errors and covariances required in an optimization process are obtained through the IMU pre-integration algorithm.
CN202110422808.9A 2021-04-16 2021-04-16 Mobile robot pose estimation algorithm based on multi-sensor data fusion Active CN113052908B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110422808.9A CN113052908B (en) 2021-04-16 2021-04-16 Mobile robot pose estimation algorithm based on multi-sensor data fusion

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110422808.9A CN113052908B (en) 2021-04-16 2021-04-16 Mobile robot pose estimation algorithm based on multi-sensor data fusion

Publications (2)

Publication Number Publication Date
CN113052908A CN113052908A (en) 2021-06-29
CN113052908B true CN113052908B (en) 2023-08-04

Family

ID=76519539

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110422808.9A Active CN113052908B (en) 2021-04-16 2021-04-16 Mobile robot pose estimation algorithm based on multi-sensor data fusion

Country Status (1)

Country Link
CN (1) CN113052908B (en)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113587904B (en) * 2021-07-29 2022-05-20 中国科学院西安光学精密机械研究所 Target attitude and position measurement method integrating machine vision and laser reference point information
CN113610149B (en) * 2021-08-05 2024-03-26 上海氢枫能源技术有限公司 Method and system for displaying pose of hydrogen compressor in real time
CN113721260B (en) * 2021-08-26 2023-12-12 南京邮电大学 Online combined calibration method for laser radar, binocular camera and inertial navigation
CN113902847B (en) * 2021-10-11 2024-04-16 岱悟智能科技(上海)有限公司 Monocular depth image pose optimization method based on three-dimensional feature constraint
CN114047766B (en) * 2021-11-22 2023-11-21 上海交通大学 Mobile robot data acquisition system and method for long-term application of indoor and outdoor scenes
CN114111681B (en) * 2021-11-24 2023-06-06 福建汉特云智能科技有限公司 Wheelbase calibration method and system for robot chassis
CN114998389A (en) * 2022-06-20 2022-09-02 珠海格力电器股份有限公司 Indoor positioning method
CN115655264A (en) * 2022-09-23 2023-01-31 智己汽车科技有限公司 Pose estimation method and device
CN115468560B (en) * 2022-11-03 2023-03-24 国网浙江省电力有限公司宁波供电公司 Quality inspection method, robot, device and medium based on multi-sensing information fusion

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105045263A (en) * 2015-07-06 2015-11-11 杭州南江机器人股份有限公司 Kinect-based robot self-positioning method
CN107564061A (en) * 2017-08-11 2018-01-09 浙江大学 A kind of binocular vision speedometer based on image gradient combined optimization calculates method
CN109141396A (en) * 2018-07-16 2019-01-04 南京航空航天大学 The UAV position and orientation estimation method that auxiliary information is merged with random sampling unification algorism
CN111047620A (en) * 2019-11-15 2020-04-21 广东工业大学 Unmanned aerial vehicle visual odometer method based on depth point-line characteristics
WO2020155616A1 (en) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 Digital retina-based photographing device positioning method
CN111595333A (en) * 2020-04-26 2020-08-28 武汉理工大学 Modularized unmanned vehicle positioning method and system based on visual inertial laser data fusion
CN111982114A (en) * 2020-07-30 2020-11-24 广东工业大学 Rescue robot for estimating three-dimensional pose by adopting IMU data fusion

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107747941B (en) * 2017-09-29 2020-05-15 歌尔股份有限公司 Binocular vision positioning method, device and system

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105045263A (en) * 2015-07-06 2015-11-11 杭州南江机器人股份有限公司 Kinect-based robot self-positioning method
CN107564061A (en) * 2017-08-11 2018-01-09 浙江大学 A kind of binocular vision speedometer based on image gradient combined optimization calculates method
CN109141396A (en) * 2018-07-16 2019-01-04 南京航空航天大学 The UAV position and orientation estimation method that auxiliary information is merged with random sampling unification algorism
WO2020155616A1 (en) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 Digital retina-based photographing device positioning method
CN111047620A (en) * 2019-11-15 2020-04-21 广东工业大学 Unmanned aerial vehicle visual odometer method based on depth point-line characteristics
CN111595333A (en) * 2020-04-26 2020-08-28 武汉理工大学 Modularized unmanned vehicle positioning method and system based on visual inertial laser data fusion
CN111982114A (en) * 2020-07-30 2020-11-24 广东工业大学 Rescue robot for estimating three-dimensional pose by adopting IMU data fusion

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
基于惯导与双目视觉融合的SLAM技术研究;杨梦佳;《中国优秀硕士学位论文全文数据库 信息科技辑 》;全文 *
点线特征融合的移动机器人视觉惯导里程计研究;王洁;《中国优秀硕士学位论文全文数据库 信息科技辑 》;全文 *
融合IMU信息的双目视觉SLAM研究;徐宽;《中国优秀硕士学位论文全文数据库 信息科技辑 》;全文 *

Also Published As

Publication number Publication date
CN113052908A (en) 2021-06-29

Similar Documents

Publication Publication Date Title
CN113052908B (en) Mobile robot pose estimation algorithm based on multi-sensor data fusion
CN111258313B (en) Multi-sensor fusion SLAM system and robot
US9990726B2 (en) Method of determining a position and orientation of a device associated with a capturing device for capturing at least one image
CN110044354A (en) A kind of binocular vision indoor positioning and build drawing method and device
CN113781582A (en) Synchronous positioning and map creating method based on laser radar and inertial navigation combined calibration
CN112815939B (en) Pose estimation method of mobile robot and computer readable storage medium
CN110081881A (en) It is a kind of based on unmanned plane multi-sensor information fusion technology warship bootstrap technique
CN104281148A (en) Mobile robot autonomous navigation method based on binocular stereoscopic vision
CN115371665B (en) Mobile robot positioning method based on depth camera and inertial fusion
CN110032201A (en) A method of the airborne visual gesture fusion of IMU based on Kalman filtering
CN111998862A (en) Dense binocular SLAM method based on BNN
Aufderheide et al. Towards real-time camera egomotion estimation and three-dimensional scene acquisition from monocular image streams
CN114608554A (en) Handheld SLAM equipment and robot instant positioning and mapping method
CN112179373A (en) Measuring method of visual odometer and visual odometer
CN116772844A (en) Navigation method of visual inertial indoor robot based on dynamic environment
CN115218889A (en) Multi-sensor indoor positioning method based on dotted line feature fusion
CN109785428A (en) A kind of handheld three-dimensional method for reconstructing based on polymorphic constrained Kalman filter
Bikmaev et al. Improving the accuracy of supporting mobile objects with the use of the algorithm of complex processing of signals with a monocular camera and LiDAR
Wu et al. AFLI-Calib: Robust LiDAR-IMU extrinsic self-calibration based on adaptive frame length LiDAR odometry
CN112731503A (en) Pose estimation method and system based on front-end tight coupling
De Marco et al. Position, velocity, attitude and accelerometer-bias estimation from imu and bearing measurements
CN116380079A (en) Underwater SLAM method for fusing front-view sonar and ORB-SLAM3
CN115930948A (en) Orchard robot fusion positioning method
CN115574816A (en) Bionic vision multi-source information intelligent perception unmanned platform
Hu et al. Efficient Visual-Inertial navigation with point-plane map

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