CN111429524A - Online initialization and calibration method and system for camera and inertial measurement unit - Google Patents
Online initialization and calibration method and system for camera and inertial measurement unit Download PDFInfo
- Publication number
- CN111429524A CN111429524A CN202010197372.3A CN202010197372A CN111429524A CN 111429524 A CN111429524 A CN 111429524A CN 202010197372 A CN202010197372 A CN 202010197372A CN 111429524 A CN111429524 A CN 111429524A
- Authority
- CN
- China
- Prior art keywords
- measurement unit
- inertial measurement
- camera
- frame
- rotation angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- 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
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- 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
-
- 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
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Image Analysis (AREA)
Abstract
The invention provides a method and a system for initializing and calibrating a camera and an inertia measurement unit on line, which comprises the following steps: step M1: pure vision initialization, namely acquiring the pose of an initial key frame by utilizing the observed quantity of a camera and constructing an initial map; step M2: calibrating and initializing vision and inertia jointly, and finishing the initialization of an angular velocity null shift value of an inertia measurement unit and the calibration of a relative rotation angle and a translation amount of a camera and the inertia measurement unit based on the pose of an initial key frame and an initial map; step M3: in the process of synchronous positioning and mapping fused by the camera and the inertial measurement unit, the initialization of the angular velocity null shift value of the inertial measurement unit and the calibration of the relative rotation angle and translation amount of the camera and the inertial measurement unit are subjected to abnormal detection and updating. The invention can complete the calibration of the relative rotation angle and the relative translation amount of the camera and the inertial measurement unit in the combined initialization process without manual calibration tools such as a calibration plate and the like, thereby expanding the application range.
Description
Technical Field
The invention relates to the field of sensor fusion, in particular to a method and a system for initializing and calibrating a camera and an inertial measurement unit on line, and more particularly to a method and a system for jointly initializing and calibrating a monocular camera and the inertial measurement unit on line based on an information content window.
Background
The camera and the Inertial Measurement Unit (IMU) have complementary characteristics, and thus are widely used in fields such as navigation and three-dimensional reconstruction in combination. The camera can obtain rich information of the external environment, but is easy to lose effectiveness under the conditions of rapid movement, illumination change and the like. The IMU can obtain motion information inside the robot at high frequency and is not affected by the surrounding environment, thereby making up for the deficiency of the vision sensor. Meanwhile, the accumulated drift error of the IMU can be effectively corrected through loop detection and loop correction finished through visual matching.
Initialization is an indispensable link in an optimized S L AM system, the initialization process determines the initial values of variables to be estimated and parameters required by system operation, including the poses of initial frames, initial map points, scale factors, gravity vectors, IMU null shift values and the like.
The external parameters of the camera and the IMU comprise relative rotation angle and relative translation amount of the camera and the IMU. And determining external parameters of the vision sensor and the IMU as a precondition for fusion application of the camera and the IMU. At present, the calibration of the visual sensor and the IMU external parameters mainly depends on offline manual operation, and an operator needs to hold a calibration plate by hand and perform proper movement and rotation operation. The whole process is time and labor consuming and requires the operator to have relevant knowledge. There are several methods for on-line calibration. However, in the case of a degraded motion situation (the robot has a small acceleration and rotation), the amount of computation of these methods increases rapidly with time.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide a method and a system for initializing and calibrating a camera and an inertial measurement unit on line.
The invention provides an online initialization and calibration method for a camera and an inertial measurement unit, which comprises the following steps:
step M1: pure vision initialization, namely acquiring the pose of an initial key frame by utilizing the observed quantity of a camera and constructing an initial map;
step M2: calibrating and initializing vision and inertia jointly, and finishing initialization of an angular velocity null shift value of an inertia measurement unit and calibration of a relative rotation angle and a translation amount of a camera and the inertia measurement unit based on the pose of an initial key frame and an initial map;
step M3: in the process of synchronous positioning and mapping fused by the camera and the inertial measurement unit, the initialization of the angular velocity null shift value of the inertial measurement unit and the calibration of the relative rotation angle and translation amount of the camera and the inertial measurement unit are subjected to abnormal detection and updating.
Preferably, the step M1 includes:
step M1.1: according to the new key frame, searching a current frame and a historical frame which accord with the parallax angle and the matching point logarithm of the preset condition;
step M1.2: calculating the relative pose between the current frame and the historical frame by utilizing epipolar constraint;
step M1.3: according to the obtained relative pose between the current frame and the historical frame, obtaining the world coordinate system coordinates of map points observed by the current frame and the historical frame together through triangulation, and constructing an initial map;
step M1.4: and projecting points in the initial map onto the pixel plane of each key frame by using a perspective n-point method, and solving the pose of each key frame in a world coordinate system.
Preferably, the step M2 includes:
step M2.1: screening the observed quantities of the camera and the inertia measurement unit based on the information quantity, and screening out the observed quantities of the preset quantity which maximizes the total information quantity; initializing an angular velocity null shift value of the inertial measurement unit and calibrating a relative rotation angle between the camera and the inertial measurement unit by using the observed quantities of the camera and the inertial measurement unit obtained after screening until the relative rotation angle between the camera and the inertial measurement unit is judged to be converged;
step M2.2: the method comprises the steps that the null shift value of the acceleration of an inertial measurement unit is assumed to be zero, the observed quantities of a camera and the inertial measurement unit are screened based on information quantity, the observed quantities of a fixed quantity of maximized total information quantity are reserved, and a scale factor, a gravity vector and the relative translation quantity of the camera and the inertial measurement unit are roughly estimated;
step M2.3: screening the observed quantities of the camera and the inertial measurement unit based on the information quantity by using the direction of the roughly estimated gravity vector and introducing the gravity as a new constraint, reserving a fixed quantity of observed quantities maximizing the total information quantity, and finely estimating a scale factor, the gravity vector, the relative translation quantity of the camera and the inertial measurement unit and a null shift value of an accelerometer in the inertial measurement unit; when the scale factor and the relative translation amount of the camera and the inertial measurement unit are converged, the visual and inertial joint initialization and calibration are completed;
the information quantity is the observed quantity of the camera and the inertial measurement unit and comprises the relative rotation angle information quantity of the camera and the inertial measurement unit.
Preferably, said step M2.1 comprises:
step M2.1.1: obtaining the relative rotation angle of the inertia measurement unit between two adjacent frames by using the observed quantity of the inertia measurement unit;
obtaining the relative rotation angle of the inertial measurement unit between two adjacent frames by integrating the angular velocity of the inertial measurement unit
Wherein, Δ Ri,i+1Representing an inertial measurement unit angular velocity integral value;a Jacobian matrix representing a null shift value of the inertial measurement unit with respect to the angular velocity; bgRepresenting the zero drift value of the angular speed of the inertial measurement unit;
step M2.1.2: obtaining the relative rotation angle of the inertial measurement unit between two adjacent frames by using the observed quantity of the camera;
the poses of two adjacent frames of cameras under a world coordinate system are obtained through visual synchronous positioning and mappingObtaining the relative rotation angle of the inertial measurement unit by using the relative rotation angle of the camera and the inertial measurement unit
Wherein R iscbRepresenting a relative rotation angle of the camera and the inertial measurement unit;
step M2.1.3: minimizing an error of a relative rotation angle of the inertial measurement unit between two adjacent frames obtained by the observed quantity of the inertial measurement unit and a relative rotation angle of the inertial measurement unit between two adjacent frames obtained by the observed quantity of the camera to obtain an optimal estimation of an angular velocity null shift value of the inertial measurement unit, namely initializing the angular velocity null shift value of the inertial measurement unit;
obtaining the optimal estimation of the null shift value of the angular velocity of the inertial measurement unit by minimizing the error of the two relative rotation angles
Where N denotes the number of key frames, R2 denotes the relative rotation angle of the inertial measurement unit obtained from the camera observation, R1 denotes the relative rotation angle of the inertial measurement unit obtained by integration of the angular velocity of the inertial measurement unit, and biInertial measurement Unit representing the ith Key frame, bi+1Inertial measurement Unit representing the i +1 st Key frame, bgRepresenting inertial measurement unit angleA speed null shift value;
step M2.1.4: for any two adjacent key frames, the quantity of information about the relative rotation angle between the camera and the inertial measurement unit contained in the observed quantity is as follows:
iQi,i+1=trace((QL(qbi,bi+1)-QR(qci,ci+1))T·(QL(qbi,bi+1)-QR(qci,ci+1))) (4)
wherein Q isL(q) a left-hand matrix corresponding to the quaternion q; qR(q) a right-multiplication matrix corresponding to the quaternion q; q. q.sbi,bi+1A quaternion representing the relative rotation angle of the inertial measurement unit of the ith frame and the (i + 1) th frame; q. q.sci,ci+1A quaternion representing the relative rotation angle of the i frame and the i +1 frame camera;
setting the information content window size to NwinIf the total number of key frames is less than NwinThe latest key frame is added to the sequence for initialization and calibration. If the total number of key frames is greater than NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
step M2.1.5: constructing an equation set taking the relative rotation angle of the camera and the inertial measurement unit as unknown quantity by using the screened observed quantity, and solving the equation set by using a least square method to obtain the optimal estimation of the relative rotation angle of the camera and the inertial measurement unit expressed in the form of quaternionCalibrating the relative rotation angle between the camera and the inertial measurement unit;
for any two adjacent key frames, the relative rotation angle of the camera and the inertial measurement unit is an unknown quantity, and the following equation system is satisfied:
(QL(qbi,bi+1)-QR(qci,ci+1))·qbc=0 (5)
wherein Q isLA left-hand multiplication matrix, q, representing a quaternionbi,bi+1Representing the inertial measurement unit under the ith frame and the (i + 1) th frameQuaternion representation of the relative rotation angle, QRA right-hand multiplication matrix representing a quaternion, qci,ci+1Quaternion representation, q, representing the relative rotation angle of the camera in frame i and frame i +1bcA quaternion representation representing the relative rotation angle of the camera and the inertial measurement unit;
will (Q)L(qbi,bi+1)-QR(qci,ci+1) Is represented by Q)i,i+1Using the observed quantity Q after screeningi,i+1Forming an observation matrix Q, and establishing an equation:
Q·qbc=0 (6)
solving for optimal estimation of the relative rotation angle of the camera and the inertial measurement unit using least squares
Step M2.1.6: by judging the nearest NwincStandard deviation sigma of yaw angle, pitch angle and roll angle of Euler angle corresponding to relative rotation angle of individual camera and inertial measurement unityaw、σpitch、σrollWhether or not less than a threshold valueDetermining whether the calibration of the visual inertia rotation angle is finished; when the calibration is not completed, that is, the relative rotation angle between the camera and the inertial measurement unit is not converged, waiting for a new key frame to repeatedly execute steps M2.1.1 to M2.1.5 until the calibration of the visual inertial rotation angle is completed;
said step M2.2 comprises:
step M2.2.1: obtaining a rotation angle and a translation amount of the camera in a world coordinate system according to the camera observed quantity by utilizing a vision synchronous positioning and mapping algorithm; obtaining the relative translation amount and the relative linear velocity of two adjacent key frames of the inertia measurement unit according to the observed amount of the inertia measurement unit by using a pre-integration algorithm;
constructing the ith frame by the scale factor s, gravity directionAmount gwAnd the relative translation amount P of the camera and the inertial measurement unitcbThe equation of (c):
wherein, an acceleration zero drift value b of the inertial measurement unit is assumedaα is zeroiExpressing the coefficients before the scale factor in the equation, βiCoefficient before the initialized value of the gravity vector in the equation, gammaiExpressing the coefficients of the equations before the amount of visual inertial translation, ηiConstant term coefficients in the equation are expressed, and s represents a scale factor; gwAn initialization value representing a gravity vector; pcbRepresenting the visual inertial translation amount; rwcRepresenting the rotation angle of the camera in the world coordinate system; pwcThe translation amount of the camera in the world coordinate system;represents the translation amount of the i +1 th frame camera in the world coordinate system,represents the translation amount, dt, of the camera of the ith frame in the world coordinate systemi+1,i+2Representing the time difference between the (i + 1) th frame and the (i + 2) th frame,represents the translation amount, dt, of the i +2 th frame camera in the world coordinate systemi,i+1Representing the time difference between the ith frame and the (I + 1) th frame, I3×3The unit matrix is represented by a matrix of units,represents the rotation angle of the i +2 th frame camera in the world coordinate system,represents the rotation angle of the i +1 th frame camera in the world coordinate system,representing the rotation angle, R, of the i-th frame camera in the world coordinate systemcbRepresenting the relative rotation angle, Δ P, between the camera and the inertial measurement uniti,i+1Represents the relative translation, Δ P, of the inertial measurement units of frame i and frame i +1i+1,i+2Represents the relative translation, Deltav, of the inertial measurement units of frame i +1 and frame i +2i,i+1Representing the relative linear speeds of the inertial measurement units of the (i + 1) th frame and the (i + 2) th frame;
step M2.2.2: suppose Ai=[αiβiγi]The observation quantity formed by adjacent 3 key frames contains the information measurement quantity of unknown quantitySetting the information content window size to NwinWhen the total number of key frames is less than NwinAdding the latest key frame into the sequence for initialization and calibration; when the total number of the key frames is more than or equal to NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
step M2.2.3: suppose thatBi=ηiFor every 3 adjacent frames, there is equation Ai·x=Bi(ii) a Using information content after screeningEstablishing an information matrix A and a constant matrix B, and establishing an equation:
A·x=B (9)
solving optimal estimate s of scale factor using least squares*Optimal estimation of the gravity vector gw *And optimal estimation P of relative translation amount of camera and inertial measurement unitcb *:
Said step M2.3 comprises: introducing a gravity vector with a preset value as a constraint to obtain a scale factor s and a gravity vector gwInitial value of (1) and visual inertial translation amount PcbA calibration value of (1);
according to the obtained gravity vector gwObtaining the rotation angle of gravity:
wherein R isWIRepresenting the rotation angle of gravity in the world coordinate system,representation and gravity vectorAnd a fixed vector gIVertical unit vector, theta is expressed in relation to gravity vectorAnd a fixed vector gIAngle between them, gIRepresents a fixed vector; the gravity vector is expressed by the rotation angle and the gravity magnitude and is subjected to first-order Taylor expansion:
wherein θ represents an updated value of the gravity vector rotation angle; g represents the magnitude of gravity, θxThe component of the updated value representing the rotation angle of the gravity vector in the x-axis of the world coordinate system, θyA component of the updated value representing the rotation angle of the gravity vector in the y-axis of the world coordinate system;
obtaining the rotation angle R of the camera in the world coordinate system according to the camera observation quantity by using a vision synchronous positioning and mapping algorithmwcAnd the amount of translation Pwc(ii) a Obtaining the relative translation quantity delta P of two adjacent key frames of the inertia measurement unit according to the observed quantity of the inertia measurement unit by utilizing a pre-integration algorithmi,i+1And relative linear velocity Δ vi,i+1(ii) a Constructing the ith frame by a scale factor s and a gravity vector gwZero drift value b of acceleration of inertial measurement unitaAnd the relative translation amount P of the camera and the inertial measurement unitcbThe equation of (c):
wherein λ isiCoefficient before scale factor in the equation, muiCoefficient before the updated value of the rotation angle of the gravity vector in the equation, ωiThe coefficient before the acceleration zero drift value of the inertia measurement unit in the equation is expressed as upsiloniBefore the relative translation of the camera and the inertial measurement unit in the expression equationCoefficient of (p)iThe coefficients of the constant terms in the equation are expressed,jacobian matrix, dt, representing the relative linear velocity of the inertial measurement unit in frame i and frame i +2 compared to the null shift value of the acceleration of the inertial measurement unit12Representing the time difference, dt, between the ith frame and the (i + 1) th frame23Representing the time difference between the i +1 th frame and the i +2 th frame,a Jacobian matrix representing the relative translation of the inertial measurement unit between frame i +1 and frame i +2 compared to the inertial measurement unit acceleration null shift value,a Jacobian matrix, a small scale, representing the relative translation of the inertial measurement unit between frame i and frame i +1 compared to the null shift value of the acceleration of the inertial measurement unit[:,1:2]The representation takes only the first two columns of the matrix;
hypothesis Ci=[λiμiωiυi]The observation quantity formed by adjacent 3 key frames contains the information measurement quantity of unknown quantitySetting the information content window size to NwinIf the total number of key frames is less than NwinAdding the latest key frame into the sequence for initialization and calibration; when the total number of key frames is greater than NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
constructing equation set and solving by least square method
Suppose thatDi=ρiFor every 3 adjacent frames, there is equation Ci·y=Di(ii) a Benefit toAnd (3) constructing an observation matrix C and a constant matrix D by using the screened observed quantity, and constructing an equation:
C·y=D (14)
solving optimal estimate s of scale factor using least squares*Optimal estimation of the update value of the rotation angle of the gravity vector*Optimal estimation of acceleration null shift value of inertial measurement unitAnd optimal estimation P of relative translation amount of camera and inertial measurement unitcb *:
By judging the nearest NwincStandard deviation sigma corresponding to each dimension value of vision inertial translation quantityx、σy、σzWhether or not less than a threshold valueDetermining whether the calibration of the visual inertial translation amount is finished; by judging the nearest NwincStandard deviation sigma of individual scale factorssWhether or not it is less than threshold thrsTo determine whether visual inertia initialization is complete; and when the calibration of the vision inertia translation amount is not finished or the initialization of the vision inertia is not finished, waiting for a new key frame and then repeatedly executing the step M2.2 to the step M2.3 until the joint initialization and calibration of the vision and the inertia are finished.
Preferably, the step M3 includes: in the process of synchronous positioning and image building fused by the camera and the inertial measurement unit, judging whether the relative pose of the camera and the inertial measurement unit changes every other preset value period, and when the relative pose of the camera and the inertial measurement unit changes, initializing the angular velocity null shift value of the inertial measurement unit and recalibrating the relative rotation angle and translation quantity of the camera and the inertial measurement unit according to any two adjacent frames in the latest frame;
step M3.1: and judging whether the calibration value of the relative rotation angle of the camera and the inertia needs to be updated according to whether the consistency error between the observed quantity of the camera and the inertia measurement unit and the initial calibration value of the relative rotation angle of the camera and the inertia measurement unit exceeds a threshold value.
The invention provides an online initialization and calibration system for a camera and an inertial measurement unit, which comprises:
module M1: pure vision initialization, namely acquiring the pose of an initial key frame by utilizing the observed quantity of a camera and constructing an initial map;
module M2: calibrating and initializing vision and inertia jointly, and finishing initialization of an angular velocity null shift value of an inertia measurement unit and calibration of a relative rotation angle and a translation amount of a camera and the inertia measurement unit based on the pose of an initial key frame and an initial map;
module M3: in the process of synchronous positioning and mapping fused by the camera and the inertial measurement unit, the initialization of the angular velocity null shift value of the inertial measurement unit and the calibration of the relative rotation angle and translation amount of the camera and the inertial measurement unit are subjected to abnormal detection and updating.
Preferably, said module M1 comprises:
module M1.1: according to the new key frame, searching a current frame and a historical frame which accord with the parallax angle and the matching point logarithm of the preset condition;
module M1.2: calculating the relative pose between the current frame and the historical frame by utilizing epipolar constraint;
module M1.3: according to the obtained relative pose between the current frame and the historical frame, obtaining the world coordinate system coordinates of map points observed by the current frame and the historical frame together through triangulation, and constructing an initial map;
module M1.4: and projecting points in the initial map onto the pixel plane of each key frame by using a perspective n-point method, and solving the pose of each key frame in a world coordinate system.
Preferably, said module M2 comprises:
module M2.1: screening the observed quantities of the camera and the inertia measurement unit based on the information quantity, and screening out the observed quantities of the preset quantity which maximizes the total information quantity; initializing an angular velocity null shift value of the inertial measurement unit and calibrating a relative rotation angle between the camera and the inertial measurement unit by using the observed quantities of the camera and the inertial measurement unit obtained after screening until the relative rotation angle between the camera and the inertial measurement unit is judged to be converged;
module M2.2: the method comprises the steps that the null shift value of the acceleration of an inertial measurement unit is assumed to be zero, the observed quantities of a camera and the inertial measurement unit are screened based on information quantity, the observed quantities of a fixed quantity of maximized total information quantity are reserved, and a scale factor, a gravity vector and the relative translation quantity of the camera and the inertial measurement unit are roughly estimated;
module M2.3: screening the observed quantities of the camera and the inertial measurement unit based on the information quantity by using the direction of the roughly estimated gravity vector and introducing the gravity as a new constraint, reserving a fixed quantity of observed quantities maximizing the total information quantity, and finely estimating a scale factor, the gravity vector, the relative translation quantity of the camera and the inertial measurement unit and a null shift value of an accelerometer in the inertial measurement unit; when the scale factor and the relative translation amount of the camera and the inertial measurement unit are converged, the visual and inertial joint initialization and calibration are completed;
the information quantity is the observed quantity of the camera and the inertial measurement unit and comprises the relative rotation angle information quantity of the camera and the inertial measurement unit.
Preferably, said module M2.1 comprises:
module M2.1.1: obtaining the relative rotation angle of the inertia measurement unit between two adjacent frames by using the observed quantity of the inertia measurement unit;
obtaining the relative rotation angle of the inertial measurement unit between two adjacent frames by integrating the angular velocity of the inertial measurement unit
Wherein, Δ Ri,i+1Representing an inertial measurement unit angular velocity integral value;representing inertiaA Jacobian matrix of magnitude units with respect to zero drift values of angular velocity; bgRepresenting the zero drift value of the angular speed of the inertial measurement unit;
module M2.1.2: obtaining the relative rotation angle of the inertial measurement unit between two adjacent frames by using the observed quantity of the camera;
the poses of two adjacent frames of cameras under a world coordinate system are obtained through visual synchronous positioning and mappingObtaining the relative rotation angle of the inertial measurement unit by using the relative rotation angle of the camera and the inertial measurement unit
Wherein R iscbRepresenting a relative rotation angle of the camera and the inertial measurement unit;
module M2.1.3: minimizing an error of a relative rotation angle of the inertial measurement unit between two adjacent frames obtained by the observed quantity of the inertial measurement unit and a relative rotation angle of the inertial measurement unit between two adjacent frames obtained by the observed quantity of the camera to obtain an optimal estimation of an angular velocity null shift value of the inertial measurement unit, namely initializing the angular velocity null shift value of the inertial measurement unit;
obtaining the optimal estimation of the null shift value of the angular velocity of the inertial measurement unit by minimizing the error of the two relative rotation angles
Where N denotes the number of key frames, R2 denotes the relative rotation angle of the inertial measurement unit obtained from the camera observation, and R1 denotes the phase of the inertial measurement unit obtained by integrating the angular velocity of the inertial measurement unitFor the angle of rotation, biInertial measurement Unit representing the ith Key frame, bi+1Inertial measurement Unit representing the i +1 st Key frame, bgRepresenting the zero drift value of the angular speed of the inertial measurement unit;
module M2.1.4: for any two adjacent key frames, the quantity of information about the relative rotation angle between the camera and the inertial measurement unit contained in the observed quantity is as follows:
wherein Q isL(q) a left-hand matrix corresponding to the quaternion q; qR(q) a right-multiplication matrix corresponding to the quaternion q; q. q.sbi,bi+1A quaternion representing the relative rotation angle of the inertial measurement unit of the ith frame and the (i + 1) th frame; q. q.sci,ci+1A quaternion representing the relative rotation angle of the i frame and the i +1 frame camera;
setting the information content window size to NwinIf the total number of key frames is less than NwinThe latest key frame is added to the sequence for initialization and calibration. If the total number of key frames is greater than NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
module M2.1.5: constructing an equation set taking the relative rotation angle of the camera and the inertial measurement unit as unknown quantity by using the screened observed quantity, and solving the equation set by using a least square method to obtain the optimal estimation of the relative rotation angle of the camera and the inertial measurement unit expressed in the form of quaternionCalibrating the relative rotation angle between the camera and the inertial measurement unit;
for any two adjacent key frames, the relative rotation angle of the camera and the inertial measurement unit is an unknown quantity, and the following equation system is satisfied:
(QL(qbi,bi+1)-QR(qci,ci+1))·qbc=0 (5)
wherein,QLA left-hand multiplication matrix, q, representing a quaternionbi,bi+1Quaternion representation, Q, representing the relative rotation angle of the inertial measurement unit in frame i and frame i +1RA right-hand multiplication matrix representing a quaternion, qci,ci+1Quaternion representation, q, representing the relative rotation angle of the camera in frame i and frame i +1bcA quaternion representation representing the relative rotation angle of the camera and the inertial measurement unit;
will (Q)L(qbi,bi+1)-QR(qci,ci+1) Is represented by Q)i,i+1Using the observed quantity Q after screeningi,i+1Forming an observation matrix Q, and establishing an equation:
Q·qbc=0 (6)
solving for optimal estimation of the relative rotation angle of the camera and the inertial measurement unit using least squares
Module M2.1.6: by judging the nearest NwincStandard deviation sigma of yaw angle, pitch angle and roll angle of Euler angle corresponding to relative rotation angle of individual camera and inertial measurement unityaw、σpitch、σrollWhether or not less than a threshold valueDetermining whether the calibration of the visual inertia rotation angle is finished; when the calibration is not completed, that is, the relative rotation angle between the camera and the inertial measurement unit is not converged, wait for a new key frame to repeatedly trigger the execution of the modules M2.1.1 to M2.1.5 until the calibration of the visual inertial rotation angle is completed;
said module M2.2 comprises:
module M2.2.1: obtaining a rotation angle and a translation amount of the camera in a world coordinate system according to the camera observed quantity by utilizing a vision synchronous positioning and mapping algorithm; obtaining the relative translation amount and the relative linear velocity of two adjacent key frames of the inertia measurement unit according to the observed amount of the inertia measurement unit by using a pre-integration algorithm;
constructing the ith frame by a scale factor s and a gravity vector gwAnd the relative translation amount P of the camera and the inertial measurement unitcbThe equation of (c):
wherein, an acceleration zero drift value b of the inertial measurement unit is assumedaα is zeroiExpressing the coefficients before the scale factor in the equation, βiCoefficient before the initialized value of the gravity vector in the equation, gammaiExpressing the coefficients of the equations before the amount of visual inertial translation, ηiConstant term coefficients in the equation are expressed, and s represents a scale factor; gwAn initialization value representing a gravity vector; pcbRepresenting the visual inertial translation amount; rwcRepresenting the rotation angle of the camera in the world coordinate system; pwcThe translation amount of the camera in the world coordinate system;represents the translation amount of the i +1 th frame camera in the world coordinate system,represents the translation amount, dt, of the camera of the ith frame in the world coordinate systemi+1,i+2Representing the i +1 th frame and the i +2 th frameThe time difference between the two phases of the operation,represents the translation amount, dt, of the i +2 th frame camera in the world coordinate systemi,i+1Representing the time difference between the ith frame and the (I + 1) th frame, I3×3The unit matrix is represented by a matrix of units,represents the rotation angle of the i +2 th frame camera in the world coordinate system,represents the rotation angle of the i +1 th frame camera in the world coordinate system,representing the rotation angle, R, of the i-th frame camera in the world coordinate systemcbRepresenting the relative rotation angle, Δ P, between the camera and the inertial measurement uniti,i+1Represents the relative translation, Δ P, of the inertial measurement units of frame i and frame i +1i+1,i+2Represents the relative translation, Deltav, of the inertial measurement units of frame i +1 and frame i +2i,i+1Representing the relative linear speeds of the inertial measurement units of the (i + 1) th frame and the (i + 2) th frame;
module M2.2.2: suppose Ai=[αiβiγi]The observation quantity formed by adjacent 3 key frames contains the information measurement quantity of unknown quantitySetting the information content window size to NwinWhen the total number of key frames is less than NwinAdding the latest key frame into the sequence for initialization and calibration; when the total number of the key frames is more than or equal to NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
module M2.2.3: suppose thatBi=ηiFor every 3 adjacent frames, there is equation Ai·x=Bi(ii) a Constructing an information matrix A and a constant matrix B by using the screened information quantity, and constructing an equation:
A·x=B (9)
solving optimal estimate s of scale factor using least squares*Optimal estimation of the gravity vector gw *And optimal estimation P of relative translation amount of camera and inertial measurement unitcb *:
Said module M2.3 comprises: introducing a gravity vector with a preset value as a constraint to obtain a scale factor s and a gravity vector gwInitial value of (1) and visual inertial translation amount PcbA calibration value of (1);
according to the obtained gravity vector gwObtaining the rotation angle of gravity:
wherein R isWIRepresenting the rotation angle of gravity in the world coordinate system,representation and gravity vectorAnd a fixed vector gIVertical unit vector, theta is expressed in relation to gravity vectorAnd a fixed vector gIAngle between them, gIRepresents a fixed vector; the gravity vector is expressed by the rotation angle and the gravity magnitude and is subjected to first-order Taylor expansion:
wherein θ represents an updated value of the gravity vector rotation angle; g represents the magnitude of gravity, θxThe component of the updated value representing the rotation angle of the gravity vector in the x-axis of the world coordinate system, θyA component of the updated value representing the rotation angle of the gravity vector in the y-axis of the world coordinate system;
obtaining the rotation angle R of the camera in the world coordinate system according to the camera observation quantity by using a vision synchronous positioning and mapping algorithmwcAnd the amount of translation Pwc(ii) a Obtaining the relative translation quantity delta P of two adjacent key frames of the inertia measurement unit according to the observed quantity of the inertia measurement unit by utilizing a pre-integration algorithmi,i+1And relative linear velocity Δ vi,i+1(ii) a Constructing the ith frame by a scale factor s and a gravity vector gwZero drift value b of acceleration of inertial measurement unitaAnd the relative translation amount P of the camera and the inertial measurement unitcbThe equation of (c):
wherein λ isiCoefficient before scale factor in the equation, muiCoefficient before the updated value of the rotation angle of the gravity vector in the equation, ωiInertia measurement in expression equationCoefficient before zero drift of acceleration of the measuring unit, upsiloniThe coefficient, rho, of the relative translation between the camera and the inertial measurement unit in the equationiThe coefficients of the constant terms in the equation are expressed,jacobian matrix, dt, representing the relative linear velocity of the inertial measurement unit in frame i and frame i +2 compared to the null shift value of the acceleration of the inertial measurement unit12Representing the time difference, dt, between the ith frame and the (i + 1) th frame23Representing the time difference between the i +1 th frame and the i +2 th frame,a Jacobian matrix representing the relative translation of the inertial measurement unit between frame i +1 and frame i +2 compared to the inertial measurement unit acceleration null shift value,a Jacobian matrix, a small scale, representing the relative translation of the inertial measurement unit between frame i and frame i +1 compared to the null shift value of the acceleration of the inertial measurement unit[:,1:2]The representation takes only the first two columns of the matrix;
hypothesis Ci=[λiμiωiυi]The observation quantity formed by adjacent 3 key frames contains the information measurement quantity of unknown quantitySetting the information content window size to NwinIf the total number of key frames is less than NwinAdding the latest key frame into the sequence for initialization and calibration; when the total number of key frames is greater than NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
constructing equation set and solving by least square method
Suppose thatDi=ρiFor every 3 adjacent frames, there is equation Ci·y=Di(ii) a And (3) constructing an observation matrix C and a constant matrix D by using the screened observed quantity, and constructing an equation:
C·y=D (14)
solving optimal estimate s of scale factor using least squares*Optimal estimation of the update value of the rotation angle of the gravity vector*Optimal estimation of acceleration null shift value of inertial measurement unitAnd optimal estimation P of relative translation amount of camera and inertial measurement unitcb *:
By judging the nearest NwincStandard deviation sigma corresponding to each dimension value of vision inertial translation quantityx、σy、σzWhether or not less than a threshold valueDetermining whether the calibration of the visual inertial translation amount is finished; by judging the nearest NwincStandard deviation sigma of individual scale factorssWhether or not it is less than threshold thrsTo determine whether visual inertia initialization is complete; when the calibration of the vision inertia translation amount is not completed or the initialization of the vision inertia is not completed, the module M2.2 to the module M2.3 are repeatedly triggered to execute after waiting for a new key frame until the vision and inertia combined initialization and calibration are completed.
Preferably, said module M3 comprises: in the process of synchronous positioning and image building fused by the camera and the inertial measurement unit, judging whether the relative pose of the camera and the inertial measurement unit changes every other preset value period, and when the relative pose of the camera and the inertial measurement unit changes, initializing the angular velocity null shift value of the inertial measurement unit and recalibrating the relative rotation angle and translation quantity of the camera and the inertial measurement unit according to any two adjacent frames in the latest frame;
module M3.1: and judging whether the calibration value of the relative rotation angle of the camera and the inertia needs to be updated according to whether the consistency error between the observed quantity of the camera and the inertia measurement unit and the initial calibration value of the relative rotation angle of the camera and the inertia measurement unit exceeds a threshold value.
Compared with the prior art, the invention has the following beneficial effects:
1. according to the invention, calibration of the relative rotation angle and the relative translation amount of the camera and the inertial measurement unit can be completed in a combined initialization process without manual calibration tools such as a calibration plate, so that the application range is expanded;
2. the invention can autonomously judge whether recalibration is needed in the operation process, and can deal with the condition that the relative pose of the camera and the inertial measurement unit changes along with time;
3. the invention utilizes the information quantity as a measurement index, screens the observed quantity participating in initialization and calibration, and inhibits the continuous increase of the calculated quantity under the condition of motion degradation.
Drawings
Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:
FIG. 1 is a flow chart of pure visual initialization;
FIG. 2 is a flow chart of visual and inertial fusion initialization and calibration;
FIG. 3 is a flow chart of a screening algorithm based on an information content window;
FIG. 4 is a flowchart illustrating the calibration value abnormality determination and update process.
Detailed Description
The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.
The invention provides a method for united online initialization and calibration of a monocular camera and an inertial measurement unit, which is carried out by taking a key frame period as a period and comprises three stages of pure visual initialization, visual and inertial united calibration and initialization and calibration value abnormity detection and updating. In the pure visual initialization stage, a current frame and a historical frame pair which meet the parallax condition and the matching point logarithm condition are searched first. And then, initializing the pose of each key frame in a world coordinate system by using the matching point pair in the current frame and the historical frame pair, obtaining the coordinates of each map point in the world coordinate system, and constructing an initial map. In the visual and inertial combined calibration and initialization phase, first, a key frame for calibration and initialization is screened based on the information amount. And then, initializing an angular velocity null shift value of the inertial measurement unit and calibrating the relative rotation angle of the camera and the inertial measurement unit by using the observed quantity of the key frame left after screening until the relative rotation angle of the camera and the inertial measurement unit is judged to be converged. Then, assuming that the null shift value of the acceleration is zero, a rough estimation is performed on the scale factor, the gravity vector, and the relative translation amount of the camera and the inertial measurement unit. And then, utilizing the direction of the roughly estimated gravity vector and introducing the gravity as a new constraint to carry out fine estimation on the scale factor, the gravity vector, the relative translation amount of the camera and the inertial measurement unit and the null shift value of the accelerometer in the inertial measurement unit. And when the scale factor and the visual inertial translation amount are converged, the initialization and calibration are completed. And updating the calibration value in the running process of the system, judging whether the relative pose of the camera and the inertial measurement unit changes at fixed intervals, and re-calibrating the relative pose if the relative pose of the camera and the inertial measurement unit changes. Compared with other methods for initializing and calibrating the monocular camera and the inertial measurement unit, the algorithm can directly utilize information in the actual environment without an additional calibration board, can deal with the condition of calibration value change in the long-term operation process, and can deal with the problem of rapid increase of the calculation amount of initialization and calibration under the condition of insufficient body movement.
Aiming at the problem that the calculated amount rapidly increases along with the time under the condition of motion degradation in the online initialization and calibration of the existing camera and the inertial measurement unit, the invention provides a monocular camera and inertial measurement unit combined online initialization and calibration method based on an information amount window, and aims to inhibit the rapid increase of the calculated amount along with the time.
The invention provides an online initialization and calibration method for a camera and an inertial measurement unit, which comprises the following steps:
defining variables to be initialized:
T1,T2......TN: the poses of the first N frames of cameras in a world coordinate system;
s: monocular camera scale factors;
gw: a gravity vector in a world coordinate system;
bg: the angular velocity zero drift value of the inertial measurement unit;
ba: an acceleration zero drift value of an inertia measurement unit;
defining variables to be calibrated:
θ2={Rbc,Pbctherein of
Rbc: the relative rotation angle of the camera and the inertial measurement unit;
Pbc: the camera and the inertia measurement unit relatively translate;
step M1: as shown in fig. 1, pure visual initialization, obtaining the pose of an initial key frame by using the observed quantity of a camera and constructing an initial map; performing pure vision initialization is a precondition for pure vision synchronous mapping and positioning, and in the step M2 and the step M3, the pose of the camera under each key frame comes from vision synchronous positioning and mapping; each point is a point observed in the initial key frame, the determination of each point coordinate depends on the determination of the initial key frame position, and each frame position is the initial key frame position.
Step M2: as shown in fig. 2, vision and inertia are calibrated and initialized in a combined manner, and initialization of an angular velocity null shift value of an inertia measurement unit and calibration of a relative rotation angle and a translation amount of a camera and the inertia measurement unit are completed based on the pose of an initial key frame and an initial map;
step M3: as shown in fig. 4, in the process of synchronous positioning and mapping by fusing the camera and the inertial measurement unit, the initialization of the null shift value of the angular velocity of the inertial measurement unit and the calibration of the relative rotation angle and translation amount of the camera and the inertial measurement unit are performed for abnormality detection and updating. The detection and updating of the anomalies is carried out at a preset frequency during the whole operation of the system, after the initialization and the completion of the initial calibration of steps M1 and M2, with the aim of correcting the calibration values which may vary over time.
Specifically, the step M1 includes:
step M1.1: according to the new key frame, searching a current frame and a historical frame which accord with the parallax angle and the matching point logarithm of the preset condition;
step M1.2: calculating the relative pose between the current frame and the historical frame by utilizing epipolar constraint;
step M1.3: according to the obtained relative pose between the current frame and the historical frame, obtaining the world coordinate system coordinates of map points observed by the current frame and the historical frame together through triangulation, and constructing an initial map;
step M1.4: and projecting points in the initial map onto the pixel plane of each key frame by using a Perspective n-Point method (PnP), and solving the pose of each key frame in a world coordinate system.
Specifically, the step M2 includes:
step M2.1: screening the observed quantities of the camera and the inertia measurement unit based on the information quantity, and screening out the observed quantities of the preset quantity which maximizes the total information quantity; initializing an angular velocity null shift value of the inertial measurement unit and calibrating a relative rotation angle between the camera and the inertial measurement unit by using the observed quantities of the camera and the inertial measurement unit obtained after screening until the relative rotation angle between the camera and the inertial measurement unit is judged to be converged;
step M2.2: the method comprises the steps that the null shift value of the acceleration of an inertial measurement unit is assumed to be zero, the observed quantities of a camera and the inertial measurement unit are screened based on information quantity, the observed quantities of a fixed quantity of maximized total information quantity are reserved, and a scale factor, a gravity vector and the relative translation quantity of the camera and the inertial measurement unit are roughly estimated;
step M2.3: utilizing the direction of the roughly estimated gravity vector and introducing the gravity as a new constraint to carry out fine estimation on the scale factor, the gravity vector, the relative translation amount of the camera and the inertial measurement unit and the null shift value of an accelerometer in the inertial measurement unit; when the scale factor and the relative translation amount of the camera and the inertial measurement unit are converged, the visual and inertial joint initialization and calibration are completed;
the information quantity is the observed quantity of the camera and the inertial measurement unit and comprises the relative rotation angle information quantity of the camera and the inertial measurement unit.
In particular, said step M2.1 comprises:
step M2.1.1: obtaining the relative rotation angle of the inertia measurement unit between two adjacent frames by using the observed quantity of the inertia measurement unit;
obtaining the relative rotation angle of the inertial measurement unit between two adjacent frames by integrating the angular velocity of the inertial measurement unit
Wherein, Δ Ri,i+1Representing an inertial measurement unit angular velocity integral value;a Jacobian matrix representing a null shift value of the inertial measurement unit with respect to the angular velocity; bgRepresenting the zero drift value of the angular speed of the inertial measurement unit;
step M2.1.2: obtaining the relative rotation angle of the inertial measurement unit between two adjacent frames by using the observed quantity of the camera;
obtaining the poses of the cameras of the two adjacent frames in a world coordinate system through visual synchronous positioning and Mapping (S L AM: Simultaneous L localization and Mapping)Obtaining the relative rotation angle of the inertial measurement unit by using the relative rotation angle of the camera and the inertial measurement unit
Wherein R iscbRepresenting a relative rotation angle of the camera and the inertial measurement unit;
step M2.1.3: minimizing an error of a relative rotation angle of the inertial measurement unit between two adjacent frames obtained by the observed quantity of the inertial measurement unit and a relative rotation angle of the inertial measurement unit between two adjacent frames obtained by the observed quantity of the camera to obtain an optimal estimation of an angular velocity null shift value of the inertial measurement unit, namely initializing the angular velocity null shift value of the inertial measurement unit;
obtaining the optimal estimation of the null shift value of the angular velocity of the inertial measurement unit by minimizing the error of the two relative rotation angles
Where N denotes the number of key frames, R2 denotes the relative rotation angle of the inertial measurement unit obtained from the camera observation, R1 denotes the relative rotation angle of the inertial measurement unit obtained by integration of the angular velocity of the inertial measurement unit, and biInertial measurement Unit representing the ith Key frame, bi+1Inertial measurement Unit representing the i +1 st Key frame, bgRepresenting the zero drift value of the angular speed of the inertial measurement unit;
step M2.1.4: for any two adjacent key frames, the observed quantity comprises the information quantity measure of the relative rotation angle between the camera and the inertial measurement unitWherein
Wherein Q isL(q): a left multiplication matrix corresponding to the quaternion q; qR(q): a right multiplication matrix corresponding to the quaternion q; q. q.sbi,bi+1: a quaternion corresponding to the relative rotation angle of the inertial measurement unit of the ith frame and the (i + 1) th frame; q. q.sci,ci+1: a quaternion corresponding to the relative rotation angle of the camera of the i-th frame and the i + 1-th frame;
setting the information content window size to NwinIf the total number of key frames is less than NwinThe latest key frame is added to the sequence for initialization and calibration. If the total number of key frames is greater than NwinAnd comparing the current information quantity measurement with the information quantity measurement in the existing sequence, and rejecting the observed quantity with the minimum information quantity.
Step M2.1.5: constructing an equation set taking the relative rotation angle of the camera and the inertial measurement unit as unknown quantity by using the screened observed quantity, and solving the equation set by using a least square method to obtain the optimal estimation of the relative rotation angle of the camera and the inertial measurement unit expressed in the form of quaternionCalibrating the relative rotation angle between the camera and the inertial measurement unit;
for any two adjacent key frames, the relative rotation angle of the camera and the inertial measurement unit is an unknown quantity, and the following equation system is satisfied:
(QL(qbi,bi+1)-QR(qci,ci+1))·qbc=0 (4)
wherein Q isLA left-hand multiplication matrix, q, representing a quaternionbi,bi+1Quaternion representation, Q, representing the relative rotation angle of the inertial measurement unit in frame i and frame i +1RA right-hand multiplication matrix representing a quaternion, qci,ci+1Indicating relative rotation of the phase under the ith frame and the (i + 1) th frameQuaternion representation of the angle, qbcA quaternion representation representing the relative rotation angle of the camera and the inertial measurement unit;
will (Q)L(qbi,bi+1)-QR(qci,ci+1) Is represented by Q)i,i+1Using the observed quantity Q after screeningi,i+1Forming an observation matrix Q, and establishing an equation:
Q·qbc=0 (5)
solving for optimal estimation of the relative rotation angle of the camera and the inertial measurement unit using least squares
Step M2.1.6: by judging the nearest NwincStandard deviation sigma of yaw angle, pitch angle and roll angle of Euler angle corresponding to relative rotation angle of individual camera and inertial measurement unityaw、σpitch、σrollWhether or not less than a threshold valueDetermining whether the calibration of the visual inertia rotation angle is finished; when the calibration is not completed, that is, the relative rotation angle between the camera and the inertial measurement unit is not converged, waiting for a new key frame to repeatedly execute steps M2.1.1 to M2.1.5 until the calibration of the visual inertial rotation angle is completed;
said step M2.2 comprises:
step M2.2.1: obtaining a rotation angle and a translation amount of the camera in a world coordinate system according to the camera observed quantity by utilizing a vision synchronous positioning and mapping algorithm; obtaining the relative translation amount and the relative linear velocity of two adjacent key frames of the inertia measurement unit according to the observed amount of the inertia measurement unit by using a pre-integration algorithm;
constructing the ith frame by a scale factor s and a gravity vector gwAnd the relative translation amount P of the camera and the inertial measurement unitcbThe equation of (c):
wherein, an acceleration zero drift value b of the inertial measurement unit is assumedaα is zeroiExpressing the coefficients before the scale factor in the equation, βiCoefficient before the initialized value of the gravity vector in the equation, gammaiExpressing the coefficients of the equations before the amount of visual inertial translation, ηiConstant term coefficients in the equation are expressed, and s represents a scale factor; gwAn initialization value representing a gravity vector; pcbRepresenting the visual inertial translation amount; rwcRepresenting the rotation angle of the camera in the world coordinate system; pwcThe translation amount of the camera in the world coordinate system;represents the translation amount of the i +1 th frame camera in the world coordinate system,represents the translation amount, dt, of the camera of the ith frame in the world coordinate systemi+1,i+2Representing the time difference between the (i + 1) th frame and the (i + 2) th frame,represents the translation amount, dt, of the i +2 th frame camera in the world coordinate systemi,i+1Representing the time difference between the ith frame and the (i + 1) th frame,I3×3the unit matrix is represented by a matrix of units,represents the rotation angle of the i +2 th frame camera in the world coordinate system,represents the rotation angle of the i +1 th frame camera in the world coordinate system,representing the rotation angle, R, of the i-th frame camera in the world coordinate systemcbRepresenting the relative rotation angle, Δ P, between the camera and the inertial measurement uniti,i+1Represents the relative translation, Δ P, of the inertial measurement units of frame i and frame i +1i+1,i+2Represents the relative translation, Deltav, of the inertial measurement units of frame i +1 and frame i +2i,i+1Representing the relative linear speeds of the inertial measurement units of the (i + 1) th frame and the (i + 2) th frame;
as shown in fig. 3, step M2.2.2: suppose Ai=[αiβiγi]The observation quantity formed by adjacent 3 key frames contains the information measurement quantity of unknown quantitySetting the information content window size to NwinWhen the total number of key frames is less than NwinAdding the latest key frame into the sequence for initialization and calibration; when the total number of the key frames is more than or equal to NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
step M2.2.3: suppose thatBi=ηiFor every 3 adjacent frames, there is equation Ai·x=Bi(ii) a Constructing an information matrix A and a constant matrix B by using the screened information quantity, and constructing an equation:
A·x=B (8)
solving optimal estimate s of scale factor using least squares*Optimal estimation of the gravity vector gw *And optimal estimation P of relative translation amount of camera and inertial measurement unitcb *:
Said step M2.3 comprises: introducing a constraint of G-9.8 as the gravity vector size to obtain a scale factor s and a gravity vector GwInitial value of (1) and visual inertial translation amount PcbA calibration value of (1);
according to the obtained gravity vector gwObtaining the rotation angle of gravity:
wherein R isWIRepresenting the rotation angle of gravity in the world coordinate system,representation and gravity vectorAnd a fixed vector gIVertical unit vector, theta is expressed in relation to gravity vectorAnd a fixed vector gIAngle between them, gIRepresents a fixed vector [0,0, -1](ii) a The gravity vector is expressed by the rotation angle and the gravity magnitude and is subjected to first-order Taylor expansion:
wherein θ represents an updated value of the gravity vector rotation angle; g represents the magnitude of gravity, θxThe component of the updated value representing the rotation angle of the gravity vector in the x-axis of the world coordinate system, θyRepresenting the angle of rotation of the gravity vectorThe component of the new value under the y-axis of the world coordinate system;
obtaining the rotation angle R of the camera in the world coordinate system according to the camera observation quantity by using a vision synchronous positioning and mapping algorithmwcAnd the amount of translation Pwc(ii) a Obtaining the relative translation quantity delta P of two adjacent key frames of the inertia measurement unit according to the observed quantity of the inertia measurement unit by utilizing a pre-integration algorithmi,i+1And relative linear velocity Δ vi,i+1(ii) a Constructing the ith frame by a scale factor s and a gravity vector gwZero drift value b of acceleration of inertial measurement unitaAnd the relative translation amount P of the camera and the inertial measurement unitcbThe equation of (c):
wherein λ isiCoefficient before scale factor in the equation, muiCoefficient before the updated value of the rotation angle of the gravity vector in the equation, ωiThe coefficient before the acceleration zero drift value of the inertia measurement unit in the equation is expressed as upsiloniThe coefficient, rho, of the relative translation between the camera and the inertial measurement unit in the equationiThe coefficients of the constant terms in the equation are expressed,jacobian matrix, dt, representing the relative linear velocity of the inertial measurement unit in frame i and frame i +2 compared to the null shift value of the acceleration of the inertial measurement unit12Representing the time difference, dt, between the ith frame and the (i + 1) th frame23Representing the time difference between the i +1 th frame and the i +2 th frame,a Jacobian matrix representing the relative translation of the inertial measurement unit between frame i +1 and frame i +2 compared to the inertial measurement unit acceleration null shift value,a Jacobian matrix, a small scale, representing the relative translation of the inertial measurement unit between frame i and frame i +1 compared to the null shift value of the acceleration of the inertial measurement unit[:,1:2]The representation takes only the first two columns of the matrix;
hypothesis Ci=[λiμiωiυi]The observation quantity formed by adjacent 3 key frames contains the information measurement quantity of unknown quantitySetting the information content window size to NwinIf the total number of key frames is less than NwinAdding the latest key frame into the sequence for initialization and calibration; when the total number of key frames is greater than NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
constructing equation set and solving by least square method
Suppose thatDi=ρiFor every 3 adjacent frames, there is equation Ci·y=Di(ii) a And (3) constructing an observation matrix C and a constant matrix D by using the screened observed quantity, and constructing an equation:
C·y=D (13)
solving optimal estimate s of scale factor using least squares*Optimal estimation of the update value of the rotation angle of the gravity vector*Optimal estimation of acceleration null shift value of inertial measurement unitAnd optimal estimation P of relative translation amount of camera and inertial measurement unitcb *:
By judging the nearest NwincStandard deviation sigma corresponding to each dimension value of vision inertial translation quantityx、σy、σzWhether or not less than a threshold valueDetermining whether the calibration of the visual inertial translation amount is finished; by judging the nearest NwincStandard deviation sigma of individual scale factorssWhether or not it is less than threshold thrsTo determine whether visual inertia initialization is complete; and when the calibration of the vision inertia translation amount is not finished or the initialization of the vision inertia is not finished, waiting for a new key frame and then repeatedly executing the step M2.2 to the step M2.3 until the joint initialization and calibration of the vision and the inertia are finished.
Specifically, the step M3 includes: in the process of synchronous positioning and image building fused by the camera and the inertial measurement unit, judging whether the relative pose of the camera and the inertial measurement unit changes every other preset value period, and when the relative pose of the camera and the inertial measurement unit changes, initializing the angular velocity null shift value of the inertial measurement unit and recalibrating the relative rotation angle and translation quantity of the camera and the inertial measurement unit according to any two adjacent frames in the latest frame;
and judging whether the calibration value of the relative rotation angle of the camera and the inertia needs to be updated according to whether the consistency error between the observed quantity of the camera and the inertia measurement unit and the initial calibration value of the relative rotation angle of the camera and the inertia measurement unit exceeds a threshold value.
Step M3.1: and judging whether the calibration value of the relative rotation angle between the camera and the inertia needs to be updated or not.
With the latest NupdateTaking a frame as an observed quantity, calculating for any two adjacent frames i and i + 1:
Qi,i+1=(QL(qbi,bi+1)-QR(qci,ci+1))。
qbc: the relative rotation angle of the front camera and the inertial measurement unit is updated.
The calculation result is compared with a threshold value thrupdateRAnd (6) comparing. If it is greater than the threshold value thrupdateRThe relative rotation angle needs to be calibrated again. Step M3.2 is carried out; otherwise, turning to the step M3.3;
and M3.2, screening the observed quantity participating in calibration based on the information quantity. Calibrating the relative rotation angle between the camera and the inertial measurement unit by using the screened observed quantity;
for the latest NupdateCalculating the information metric contained in the observed quantity for any two adjacent frames in the frameSelecting N with the largest information metricwinAnd (6) measuring the observed quantity. Forming an observation matrix Q by using the observed quantities, and solving the relative rotation angle between the camera and the inertial measurement unit by using a least square method
And M3.3, judging whether the relative translation calibration values of the camera and the inertial measurement unit need to be updated.
With the latest NupdateTaking frames as observed quantities, and performing visual comparison by using the observed quantities of any three adjacent frames i and i +1 and i +2The inertial fusion S L AM algorithm obtains the rotation angle R of each frame camera in the world coordinate systemwcCalculating the relative translation amount pre-integral delta P and the relative linear velocity pre-integral delta v of two adjacent frames:
Pcbto update the relative translation of the front camera and the inertial measurement unit.
The calculation result is compared with a threshold value thrupdatePAnd (6) comparing. If it is greater than the threshold value thrupdatePThen the relative translation amount needs to be calibrated again, and the step M3.4 is carried out; otherwise, returning to the step M3.1;
step M3.4: and screening the observed quantity participating in calibration based on the information quantity. And calibrating the relative translation quantity of the camera and the inertial measurement unit by using the screened observed quantity.
For the latest NupdateCalculating the information metric contained in the observed quantity for any two adjacent frames in the frameSelecting N with the largest information metricwinAnd (6) measuring the observed quantity. The observation matrixes V and P are formed by the observation quantities, and the relative translation quantity of the camera and the inertial measurement unit is solved by a least square method
Information content window size N during frame screeningwin=12, convergence judgment parameter Nwinc=15, thrs0.02. Judging whether the window size N needs to be calibrated againupdate50, threshold size thrupdateR=0.2°,thrupdateP=0.1m。
The invention provides an online initialization and calibration system for a camera and an inertial measurement unit, which comprises:
defining variables to be initialized:
T1,T2......TN: the poses of the first N frames of cameras in a world coordinate system;
s: monocular camera scale factors;
gw: a gravity vector in a world coordinate system;
bg: the angular velocity zero drift value of the inertial measurement unit;
ba: an acceleration zero drift value of an inertia measurement unit;
defining variables to be calibrated:
θ2={Rbc,Pbctherein of
Rbc: the relative rotation angle of the camera and the inertial measurement unit;
Pbc: the camera and the inertia measurement unit relatively translate;
module M1: as shown in fig. 1, pure visual initialization, obtaining the pose of an initial key frame by using the observed quantity of a camera and constructing an initial map; the pure vision initialization is a precondition for pure vision synchronous mapping and positioning, and in a module M2 and a module M3, the poses of the cameras under each key frame come from vision synchronous positioning and mapping; each point is a point observed in the initial key frame, the determination of each point coordinate depends on the determination of the initial key frame position, and each frame position is the initial key frame position.
Module M2: as shown in fig. 2, vision and inertia are calibrated and initialized in a combined manner, and initialization of an angular velocity null shift value of an inertia measurement unit and calibration of a relative rotation angle and a translation amount of a camera and the inertia measurement unit are completed based on the pose of an initial key frame and an initial map;
module M3: as shown in fig. 4, in the process of synchronous positioning and mapping by fusing the camera and the inertial measurement unit, the initialization of the null shift value of the angular velocity of the inertial measurement unit and the calibration of the relative rotation angle and translation amount of the camera and the inertial measurement unit are performed for abnormality detection and updating. The detection and updating of the anomalies is carried out at a preset frequency during the whole system operation, after the initialization and the completion of the initial calibration of the module M1 and module M2, with the aim of correcting the calibration values which may vary over time.
Specifically, the module M1 includes:
module M1.1: according to the new key frame, searching a current frame and a historical frame which accord with the parallax angle and the matching point logarithm of the preset condition;
module M1.2: calculating the relative pose between the current frame and the historical frame by utilizing epipolar constraint;
module M1.3: according to the obtained relative pose between the current frame and the historical frame, obtaining the world coordinate system coordinates of map points observed by the current frame and the historical frame together through triangulation, and constructing an initial map;
module M1.4: and projecting points in the initial map onto the pixel plane of each key frame by using a Perspective n-Point method (PnP), and solving the pose of each key frame in a world coordinate system.
Specifically, the module M2 includes:
module M2.1: screening the observed quantities of the camera and the inertia measurement unit based on the information quantity, and screening out the observed quantities of the preset quantity which maximizes the total information quantity; initializing an angular velocity null shift value of the inertial measurement unit and calibrating a relative rotation angle between the camera and the inertial measurement unit by using the observed quantities of the camera and the inertial measurement unit obtained after screening until the relative rotation angle between the camera and the inertial measurement unit is judged to be converged;
module M2.2: the method comprises the steps that the null shift value of the acceleration of an inertial measurement unit is assumed to be zero, the observed quantities of a camera and the inertial measurement unit are screened based on information quantity, the observed quantities of a fixed quantity of maximized total information quantity are reserved, and a scale factor, a gravity vector and the relative translation quantity of the camera and the inertial measurement unit are roughly estimated;
module M2.3: utilizing the direction of the roughly estimated gravity vector and introducing the gravity as a new constraint to carry out fine estimation on the scale factor, the gravity vector, the relative translation amount of the camera and the inertial measurement unit and the null shift value of an accelerometer in the inertial measurement unit; when the scale factor and the relative translation amount of the camera and the inertial measurement unit are converged, the visual and inertial joint initialization and calibration are completed;
the information quantity is the observed quantity of the camera and the inertial measurement unit and comprises the relative rotation angle information quantity of the camera and the inertial measurement unit.
In particular, said module M2.1 comprises:
module M2.1.1: obtaining the relative rotation angle of the inertia measurement unit between two adjacent frames by using the observed quantity of the inertia measurement unit;
obtaining the relative rotation angle of the inertial measurement unit between two adjacent frames by integrating the angular velocity of the inertial measurement unit
Wherein, Δ Ri,i+1Representing an inertial measurement unit angular velocity integral value;a Jacobian matrix representing a null shift value of the inertial measurement unit with respect to the angular velocity; bgRepresenting the zero drift value of the angular speed of the inertial measurement unit;
module M2.1.2: obtaining the relative rotation angle of the inertial measurement unit between two adjacent frames by using the observed quantity of the camera;
obtaining the poses of the cameras of the two adjacent frames in a world coordinate system through visual synchronous positioning and Mapping (S L AM: Simultaneous L localization and Mapping)Obtaining the relative rotation angle of the inertial measurement unit by using the relative rotation angle of the camera and the inertial measurement unit
Wherein R iscbRepresenting a relative rotation angle of the camera and the inertial measurement unit;
module M2.1.3: minimizing an error of a relative rotation angle of the inertial measurement unit between two adjacent frames obtained by the observed quantity of the inertial measurement unit and a relative rotation angle of the inertial measurement unit between two adjacent frames obtained by the observed quantity of the camera to obtain an optimal estimation of an angular velocity null shift value of the inertial measurement unit, namely initializing the angular velocity null shift value of the inertial measurement unit;
obtaining the optimal estimation of the null shift value of the angular velocity of the inertial measurement unit by minimizing the error of the two relative rotation angles
Wherein N represents the number of key frames, R2 denotes a relative rotation angle of the inertial measurement unit obtained by camera observation, R1 denotes a relative rotation angle of the inertial measurement unit obtained by integration of an angular velocity of the inertial measurement unit, biInertial measurement Unit representing the ith Key frame, bi+1Inertial measurement Unit representing the i +1 st Key frame, bgRepresenting the zero drift value of the angular speed of the inertial measurement unit;
module M2.1.4: for any two adjacent key frames, the quantity of information contained in the observed quantity and related to the relative rotation angle of the camera and the inertial measurement unit is measured as iQi,i+1=trace((QL(qbi,bi+1)-QR(qci,ci+1))T·(QL(qbi,bi+1)-QR(qci,ci+1) ))) wherein
Wherein Q isL(q): a left multiplication matrix corresponding to the quaternion q; qR(q): a right multiplication matrix corresponding to the quaternion q; q. q.sbi,bi+1: a quaternion corresponding to the relative rotation angle of the inertial measurement unit of the ith frame and the (i + 1) th frame; q. q.sci,ci+1: a quaternion corresponding to the relative rotation angle of the camera of the i-th frame and the i + 1-th frame;
setting the information content window size to NwinIf the total number of key frames is less than NwinThe latest key frame is added to the sequence for initialization and calibration. If the total number of key frames is greater than NwinAnd comparing the current information quantity measurement with the information quantity measurement in the existing sequence, and rejecting the observed quantity with the minimum information quantity.
Module M2.1.5: constructing an equation set taking the relative rotation angle of the camera and the inertial measurement unit as unknown quantity by using the screened observed quantity, and solving the equation set by using a least square method to obtain the optimal estimation of the relative rotation angle of the camera and the inertial measurement unit expressed in the form of quaternionCalibrating the relative rotation angle between the camera and the inertial measurement unit;
for any two adjacent key frames, the relative rotation angle of the camera and the inertial measurement unit is an unknown quantity, and the following equation system is satisfied:
(QL(qbi,bi+1)-QR(qci,ci+1))·qbc=0 (4)
wherein Q isLA left-hand multiplication matrix, q, representing a quaternionbi,bi+1Quaternion representation, Q, representing the relative rotation angle of the inertial measurement unit in frame i and frame i +1RA right-hand multiplication matrix representing a quaternion, qci,ci+1Quaternion representation, q, representing the relative rotation angle of the camera in frame i and frame i +1bcA quaternion representation representing the relative rotation angle of the camera and the inertial measurement unit;
will (Q)L(qbi,bi+1)-QR(qci,ci+1) Is represented by Q)i,i+1Using the observed quantity Q after screeningi,i+1Forming an observation matrix Q, and establishing an equation:
Q·qbc=0 (5)
solving for optimal estimation of the relative rotation angle of the camera and the inertial measurement unit using least squares
Module M2.1.6: by judging the nearest NwincStandard deviation sigma of yaw angle, pitch angle and roll angle of Euler angle corresponding to relative rotation angle of individual camera and inertial measurement unityaw、σpitch、σrollWhether or not less than a threshold valueDetermining whether the calibration of the visual inertia rotation angle is finished; when the calibration is not completed, that is, the relative rotation angle between the camera and the inertial measurement unit is not converged, wait for a new key frame to repeatedly trigger the execution of the modules M2.1.1 to M2.1.5 until the calibration of the visual inertial rotation angle is completed;
said module M2.2 comprises:
module M2.2.1: obtaining a rotation angle and a translation amount of the camera in a world coordinate system according to the camera observed quantity by utilizing a vision synchronous positioning and mapping algorithm; obtaining the relative translation amount and the relative linear velocity of two adjacent key frames of the inertia measurement unit according to the observed amount of the inertia measurement unit by using a pre-integration algorithm;
constructing the ith frame by a scale factor s and a gravity vector gwAnd the relative translation amount P of the camera and the inertial measurement unitcbThe equation of (c):
wherein, an acceleration zero drift value b of the inertial measurement unit is assumedaα is zeroiExpressing the coefficients before the scale factor in the equation, βiCoefficient before the initialized value of the gravity vector in the equation, gammaiExpressing the coefficients of the equations before the amount of visual inertial translation, ηiConstant term coefficients in the equation are expressed, and s represents a scale factor; gwAn initialization value representing a gravity vector; pcbRepresenting the visual inertial translation amount; rwcRepresenting the rotation angle of the camera in the world coordinate system; pwcThe translation amount of the camera in the world coordinate system;represents the translation amount of the i +1 th frame camera in the world coordinate system,represents the translation amount, dt, of the camera of the ith frame in the world coordinate systemi+1,i+2Representing the time difference between the (i + 1) th frame and the (i + 2) th frame,represents the translation amount, dt, of the i +2 th frame camera in the world coordinate systemi,i+1Representing the time difference between the ith frame and the (I + 1) th frame, I3×3The unit matrix is represented by a matrix of units,represents the rotation angle of the i +2 th frame camera in the world coordinate system,represents the rotation angle of the i +1 th frame camera in the world coordinate system,representing the rotation angle, R, of the i-th frame camera in the world coordinate systemcbRepresenting the relative rotation angle, Δ P, between the camera and the inertial measurement uniti,i+1Represents the relative translation, Δ P, of the inertial measurement units of frame i and frame i +1i+1,i+2Represents the relative translation, Deltav, of the inertial measurement units of frame i +1 and frame i +2i,i+1Representing the relative linear speeds of the inertial measurement units of the (i + 1) th frame and the (i + 2) th frame;
as shown in fig. 3, module M2.2.2: suppose Ai=[αiβiγi]The observation quantity formed by adjacent 3 key frames contains the information measurement quantity of unknown quantitySetting the information content window size to NwinWhen the total number of key frames is less than NwinAdding the latest key frame into the sequence for initialization and calibration; when the total number of the key frames is more than or equal to NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
module M2.2.3: suppose thatBi=ηiFor every 3 adjacent frames, there is equation Ai·x=Bi(ii) a Constructing an information matrix A and a constant matrix B by using the screened information quantity, and constructing an equation:
A·x=B (8)
solving optimal estimate s of scale factor using least squares*Optimal estimation of the gravity vector gw *And optimal estimation P of relative translation amount of camera and inertial measurement unitcb *:
Said module M2.3 comprises: introducing a constraint of G-9.8 as the gravity vector size to obtain a scale factor s and a gravity vector GwInitial value of (1) and visual inertial translation amount PcbA calibration value of (1);
according to the obtained gravity vector gwObtaining the rotation angle of gravity:
wherein, R isWIRepresenting the rotation angle of gravity in the world coordinate system,representation and gravity vectorAnd a fixed vector gIVertical unit vector, theta is expressed in relation to gravity vectorAnd a fixed vector gIAngle between them, gIRepresents a fixed vector [0,0, -1](ii) a Using the gravity vector as the rotation angle and the gravity magnitudeRepresent and perform a first order Taylor expansion:
wherein θ represents an updated value of the gravity vector rotation angle; g represents the magnitude of gravity, θxThe component of the updated value representing the rotation angle of the gravity vector in the x-axis of the world coordinate system, θyA component of the updated value representing the rotation angle of the gravity vector in the y-axis of the world coordinate system;
obtaining the rotation angle R of the camera in the world coordinate system according to the camera observation quantity by using a vision synchronous positioning and mapping algorithmwcAnd the amount of translation Pwc(ii) a Obtaining the relative translation quantity delta P of two adjacent key frames of the inertia measurement unit according to the observed quantity of the inertia measurement unit by utilizing a pre-integration algorithmi,i+1And relative linear velocity Δ vi,i+1(ii) a Constructing the ith frame by a scale factor s and a gravity vector gwZero drift value b of acceleration of inertial measurement unitaAnd the relative translation amount P of the camera and the inertial measurement unitcbThe equation of (c):
wherein λ isiCoefficient before scale factor in the equation, muiCoefficient before the updated value of the rotation angle of the gravity vector in the equation, ωiThe coefficient before the acceleration zero drift value of the inertia measurement unit in the equation is expressed as upsiloniThe coefficient, rho, of the relative translation between the camera and the inertial measurement unit in the equationiThe coefficients of the constant terms in the equation are expressed,jacobian matrix, dt, representing the relative linear velocity of the inertial measurement unit in frame i and frame i +2 compared to the null shift value of the acceleration of the inertial measurement unit12Representing the time difference, dt, between the ith frame and the (i + 1) th frame23Representing the time difference between the i +1 th frame and the i +2 th frame,a Jacobian matrix representing the relative translation of the inertial measurement unit between frame i +1 and frame i +2 compared to the inertial measurement unit acceleration null shift value,a Jacobian matrix, a small scale, representing the relative translation of the inertial measurement unit between frame i and frame i +1 compared to the null shift value of the acceleration of the inertial measurement unit[:,1:2]The representation takes only the first two columns of the matrix;
hypothesis Ci=[λiμiωiυi]The observation quantity formed by adjacent 3 key frames contains the information measurement quantity of unknown quantitySetting the information content window size to NwinIf the total number of key frames is less than NwinAdding the latest key frame into the sequence for initialization and calibration; when the total number of key frames is greater than NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
constructing equation set and solving by least square method
Suppose thatDi=ρiFor every 3 adjacent frames, there is equation Ci·y=Di(ii) a And (3) constructing an observation matrix C and a constant matrix D by using the screened observed quantity, and constructing an equation:
C·y=D (13)
solving optimal estimate s of scale factor using least squares*Optimal estimation of the update value of the rotation angle of the gravity vector*Optimal estimation of acceleration null shift value of inertial measurement unitAnd optimal estimation P of relative translation amount of camera and inertial measurement unitcb *:
By judging the nearest NwincStandard deviation sigma corresponding to each dimension value of vision inertial translation quantityx、σy、σzWhether or not less than a threshold valueDetermining whether the calibration of the visual inertial translation amount is finished; by judging the nearest NwincStandard deviation sigma of individual scale factorssWhether or not it is less than threshold thrsTo determine whether visual inertia initialization is complete; when the calibration of the vision inertia translation amount is not completed or the initialization of the vision inertia is not completed, the module M2.2 to the module M2.3 are repeatedly triggered to execute after waiting for a new key frame until the vision and inertia combined initialization and calibration are completed.
Specifically, the module M3 includes: in the process of synchronous positioning and image building fused by the camera and the inertial measurement unit, judging whether the relative pose of the camera and the inertial measurement unit changes every other preset value period, and when the relative pose of the camera and the inertial measurement unit changes, initializing the angular velocity null shift value of the inertial measurement unit and recalibrating the relative rotation angle and translation quantity of the camera and the inertial measurement unit according to any two adjacent frames in the latest frame;
and judging whether the calibration value of the relative rotation angle of the camera and the inertia needs to be updated according to whether the consistency error between the observed quantity of the camera and the inertia measurement unit and the initial calibration value of the relative rotation angle of the camera and the inertia measurement unit exceeds a threshold value.
Module M3.1: and judging whether the calibration value of the relative rotation angle between the camera and the inertia needs to be updated or not.
With the latest NupdateTaking a frame as an observed quantity, calculating for any two adjacent frames i and i + 1:
Qi,i+1=(QL(qbi,bi+1)-QR(qci,ci+1))。
qbc: the relative rotation angle of the front camera and the inertial measurement unit is updated.
The calculation result is compared with a threshold value thrupdateRAnd (6) comparing. If it is greater than the threshold value thrupdateRThe relative rotation angle needs to be calibrated again. Rotating the module M3.2; otherwise, turning the module M3.3;
and a module M3.2 for screening the observed quantity participating in calibration based on the information quantity. Calibrating the relative rotation angle between the camera and the inertial measurement unit by using the screened observed quantity;
for the latest NupdateCalculating the information metric contained in the observed quantity for any two adjacent frames in the frameSelecting N with the largest information metricwinAnd (6) measuring the observed quantity. Forming an observation matrix Q by using the observed quantities, and solving the relative rotation angle between the camera and the inertial measurement unit by using a least square method
And a module M3.3 for judging whether the relative translation calibration values of the camera and the inertial measurement unit need to be updated.
With the latest NupdateTaking frames as observed quantities, and obtaining a rotation angle R of each frame camera in a world coordinate system by using the observed quantities of any three adjacent frames i and i +1 and i +2 through an S L AM algorithm of fusion of vision and inertiawcCalculating the relative translation amount pre-integral delta P and the relative linear velocity pre-integral delta v of two adjacent frames:
Pcbto update the relative translation of the front camera and the inertial measurement unit.
The calculation result is compared with a threshold value thrupdatePAnd (6) comparing. If it is greater than the threshold value thrupdatePThen the relative translation amount needs to be calibrated again, and the module M3.4 is rotated; otherwise, returning to the module M3.1;
module M3.4: and screening the observed quantity participating in calibration based on the information quantity. And calibrating the relative translation quantity of the camera and the inertial measurement unit by using the screened observed quantity.
For the latest NupdateCalculating the information metric contained in the observed quantity for any two adjacent frames in the frameSelecting N with the largest information metricwinAnd (6) measuring the observed quantity. These observations are used to form observation matrices V and P, using the best caseMethod for solving relative translation quantity of camera and inertial measurement unit by using small two multiplication
Information content window size N during frame screeningwin12, convergence judgment parameter Nwinc=15,thrPcb=0.01m,thrs0.02. Judging whether the window size N needs to be calibrated againupdate50, threshold size thrupdateR=0.2°,thrupdateP=0.1m。
Those skilled in the art will appreciate that, in addition to implementing the systems, apparatus, and various modules thereof provided by the present invention in purely computer readable program code, the same procedures can be implemented entirely by logically programming method steps such that the systems, apparatus, and various modules thereof are provided in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system, the device and the modules thereof provided by the present invention can be considered as a hardware component, and the modules included in the system, the device and the modules thereof for implementing various programs can also be considered as structures in the hardware component; modules for performing various functions may also be considered to be both software programs for performing the methods and structures within hardware components.
The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.
Claims (10)
1. A camera and inertial measurement unit online initialization and calibration method is characterized by comprising the following steps:
step M1: pure vision initialization, namely acquiring the pose of an initial key frame by utilizing the observed quantity of a camera and constructing an initial map;
step M2: calibrating and initializing vision and inertia jointly, and finishing initialization of an angular velocity null shift value of an inertia measurement unit and calibration of a relative rotation angle and a translation amount of a camera and the inertia measurement unit based on the pose of an initial key frame and an initial map;
step M3: in the process of synchronous positioning and mapping fused by the camera and the inertial measurement unit, the initialization of the angular velocity null shift value of the inertial measurement unit and the calibration of the relative rotation angle and translation amount of the camera and the inertial measurement unit are subjected to abnormal detection and updating.
2. The method for initializing and calibrating a camera and an inertial measurement unit online as claimed in claim 1, wherein the step M1 comprises:
step M1.1: according to the new key frame, searching a current frame and a historical frame which accord with the parallax angle and the matching point logarithm of the preset condition;
step M1.2: calculating the relative pose between the current frame and the historical frame by utilizing epipolar constraint;
step M1.3: according to the obtained relative pose between the current frame and the historical frame, obtaining the world coordinate system coordinates of map points observed by the current frame and the historical frame together through triangulation, and constructing an initial map;
step M1.4: and projecting points in the initial map onto the pixel plane of each key frame by using a perspective n-point method, and solving the pose of each key frame in a world coordinate system.
3. The method for initializing and calibrating a camera and an inertial measurement unit online as claimed in claim 1, wherein the step M2 comprises:
step M2.1: screening the observed quantities of the camera and the inertia measurement unit based on the information quantity, and screening out the observed quantities of the preset quantity which maximizes the total information quantity; initializing an angular velocity null shift value of the inertial measurement unit and calibrating a relative rotation angle between the camera and the inertial measurement unit by using the observed quantities of the camera and the inertial measurement unit obtained after screening until the relative rotation angle between the camera and the inertial measurement unit is judged to be converged;
step M2.2: the method comprises the steps that the null shift value of the acceleration of an inertial measurement unit is assumed to be zero, the observed quantities of a camera and the inertial measurement unit are screened based on information quantity, the observed quantities of a fixed quantity of maximized total information quantity are reserved, and a scale factor, a gravity vector and the relative translation quantity of the camera and the inertial measurement unit are roughly estimated;
step M2.3: screening the observed quantities of the camera and the inertial measurement unit based on the information quantity by using the direction of the roughly estimated gravity vector and introducing the gravity as a new constraint, reserving a fixed quantity of observed quantities maximizing the total information quantity, and finely estimating a scale factor, the gravity vector, the relative translation quantity of the camera and the inertial measurement unit and a null shift value of an accelerometer in the inertial measurement unit; when the scale factor and the relative translation amount of the camera and the inertial measurement unit are converged, the visual and inertial joint initialization and calibration are completed;
the information quantity is the observed quantity of the camera and the inertial measurement unit and comprises the relative rotation angle information quantity of the camera and the inertial measurement unit.
4. The method for initializing and calibrating a camera and an inertial measurement unit on line according to claim 3, wherein the step M2.1 comprises:
step M2.1.1: obtaining the relative rotation angle of the inertia measurement unit between two adjacent frames by using the observed quantity of the inertia measurement unit;
obtaining the relative rotation angle of the inertial measurement unit between two adjacent frames by integrating the angular velocity of the inertial measurement unit
Wherein, Δ Ri,i+1Representing an inertial measurement unit angular velocity integral value;a Jacobian matrix representing a null shift value of the inertial measurement unit with respect to the angular velocity; bgRepresenting the zero drift value of the angular speed of the inertial measurement unit;
step M2.1.2: obtaining the relative rotation angle of the inertial measurement unit between two adjacent frames by using the observed quantity of the camera;
the poses of two adjacent frames of cameras under a world coordinate system are obtained through visual synchronous positioning and mappingObtaining the relative rotation angle of the inertial measurement unit by using the relative rotation angle of the camera and the inertial measurement unit
Wherein R iscbRepresenting a relative rotation angle of the camera and the inertial measurement unit;
step M2.1.3: minimizing an error of a relative rotation angle of the inertial measurement unit between two adjacent frames obtained by the observed quantity of the inertial measurement unit and a relative rotation angle of the inertial measurement unit between two adjacent frames obtained by the observed quantity of the camera to obtain an optimal estimation of an angular velocity null shift value of the inertial measurement unit, namely initializing the angular velocity null shift value of the inertial measurement unit;
obtaining the optimal estimation of the null shift value of the angular velocity of the inertial measurement unit by minimizing the error of the two relative rotation angles
Where N denotes the number of key frames, R2 denotes the relative rotation angle of the inertial measurement unit obtained from the camera observation, R1 denotes the relative rotation angle of the inertial measurement unit obtained by integration of the angular velocity of the inertial measurement unit, and biInertial measurement Unit representing the ith Key frame, bi+1Inertial measurement Unit representing the i +1 st Key frame, bgRepresenting the zero drift value of the angular speed of the inertial measurement unit;
step M2.1.4: for any two adjacent key frames, the quantity of information about the relative rotation angle between the camera and the inertial measurement unit contained in the observed quantity is as follows:
iQi,i+1=trace((QL(qbi,bi+1)-QR(qci,ci+1))T·(QL(qbi,bi+1)-QR(qci,ci+1))) (4)
wherein Q isL(q) a left-hand matrix corresponding to the quaternion q; qR(q) a right-multiplication matrix corresponding to the quaternion q; q. q.sbi,bi+1A quaternion representing the relative rotation angle of the inertial measurement unit of the ith frame and the (i + 1) th frame; q. q.sci,ci+1A quaternion representing the relative rotation angle of the i frame and the i +1 frame camera;
setting the information content window size to NwinIf the total number of key frames is less than NwinThe latest key frame is added to the sequence for initialization and calibration. If the total number of key frames is greater than NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
step M2.1.5: constructing an equation set taking the relative rotation angle of the camera and the inertial measurement unit as unknown quantity by using the screened observed quantity, and solving the equation set by using a least square method to obtain the optimal estimation of the relative rotation angle of the camera and the inertial measurement unit expressed in the form of quaternionCalibrating the relative rotation angle between the camera and the inertial measurement unit;
for any two adjacent key frames, the relative rotation angle of the camera and the inertial measurement unit is an unknown quantity, and the following equation system is satisfied:
(QL(qbi,bi+1)-QR(qci,ci+1))·qbc=0 (5)
wherein Q isLA left-hand multiplication matrix, q, representing a quaternionbi,bi+1Quaternion representation, Q, representing the relative rotation angle of the inertial measurement unit in frame i and frame i +1RA right-hand multiplication matrix representing a quaternion, qci,ci+1Quaternion representation, q, representing the relative rotation angle of the camera in frame i and frame i +1bcA quaternion representation representing the relative rotation angle of the camera and the inertial measurement unit;
will (Q)L(qbi,bi+1)-QR(qci,ci+1) Is represented by Q)i,i+1Using the observed quantity Q after screeningi,i+1Forming an observation matrix Q, and establishing an equation:
Q·qbc=0 (6)
solving for optimal estimation of the relative rotation angle of the camera and the inertial measurement unit using least squares
Step M2.1.6: by judging the nearest NwincStandard deviation sigma of yaw angle, pitch angle and roll angle of Euler angle corresponding to relative rotation angle of individual camera and inertial measurement unityaw、σpitch、σrollWhether or not less than a threshold valueDetermining whether the calibration of the visual inertia rotation angle is finished; when the calibration is not completed, i.e. the camera and the inertia measurementIf the relative rotation angle of the quantity unit is not converged, waiting for a new key frame to repeatedly execute the steps M2.1.1 to M2.1.5 until the visual inertia rotation angle calibration is completed;
said step M2.2 comprises:
step M2.2.1: obtaining a rotation angle and a translation amount of the camera in a world coordinate system according to the camera observed quantity by utilizing a vision synchronous positioning and mapping algorithm; obtaining the relative translation amount and the relative linear velocity of two adjacent key frames of the inertia measurement unit according to the observed amount of the inertia measurement unit by using a pre-integration algorithm;
constructing the ith frame by a scale factor s and a gravity vector gwAnd the relative translation amount P of the camera and the inertial measurement unitcbThe equation of (c):
wherein, an acceleration zero drift value b of the inertial measurement unit is assumedaα is zeroiExpressing the coefficients before the scale factor in the equation, βiCoefficient before the initialized value of the gravity vector in the equation, gammaiExpressing the coefficients of the equations before the amount of visual inertial translation, ηiConstant term coefficients in the equation are expressed, and s represents a scale factor; gwAn initialization value representing a gravity vector; pcbRepresenting the visual inertial translation amount; rwcTo representThe rotation angle of the camera in the world coordinate system; pwcThe translation amount of the camera in the world coordinate system;represents the translation amount of the i +1 th frame camera in the world coordinate system,represents the translation amount, dt, of the camera of the ith frame in the world coordinate systemi+1,i+2Representing the time difference between the (i + 1) th frame and the (i + 2) th frame,represents the translation amount, dt, of the i +2 th frame camera in the world coordinate systemi,i+1Representing the time difference between the ith frame and the (I + 1) th frame, I3×3The unit matrix is represented by a matrix of units,represents the rotation angle of the i +2 th frame camera in the world coordinate system,represents the rotation angle of the i +1 th frame camera in the world coordinate system,representing the rotation angle, R, of the i-th frame camera in the world coordinate systemcbRepresenting the relative rotation angle, Δ P, between the camera and the inertial measurement uniti,i+1Represents the relative translation, Δ P, of the inertial measurement units of frame i and frame i +1i+1,i+2Represents the relative translation, Deltav, of the inertial measurement units of frame i +1 and frame i +2i,i+1Representing the relative linear speeds of the inertial measurement units of the (i + 1) th frame and the (i + 2) th frame;
step M2.2.2: suppose Ai=[αiβiγi]The observation quantity formed by adjacent 3 key frames contains the information measurement quantity of unknown quantitySetting the information content window size to NwinWhen the total number of key frames is less than NwinAdding the latest key frame into the sequence for initialization and calibration; when the total number of the key frames is more than or equal to NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
step M2.2.3: suppose thatBi=ηiFor every 3 adjacent frames, there is equation Ai·x=Bi(ii) a Constructing an information matrix A and a constant matrix B by using the screened information quantity, and constructing an equation:
A·x=B (9)
solving optimal estimate s of scale factor using least squares*Optimal estimation of the gravity vector gw *And optimal estimation P of relative translation amount of camera and inertial measurement unitcb *:
Said step M2.3 comprises: introducing a gravity vector with a preset value as a constraint to obtain a scale factor s and a gravity vector gwInitial value of (1) and visual inertial translation amount PcbA calibration value of (1);
wherein R isWIRepresenting the rotation angle of gravity in the world coordinate system,representation and gravity vectorAnd a fixed vector gIVertical unit vector, theta is expressed in relation to gravity vectorAnd a fixed vector gIAngle between them, gIRepresents a fixed vector; the gravity vector is expressed by the rotation angle and the gravity magnitude and is subjected to first-order Taylor expansion:
wherein θ represents an updated value of the gravity vector rotation angle; g represents the magnitude of gravity, θxThe component of the updated value representing the rotation angle of the gravity vector in the x-axis of the world coordinate system, θyA component of the updated value representing the rotation angle of the gravity vector in the y-axis of the world coordinate system;
obtaining the rotation angle R of the camera in the world coordinate system according to the camera observation quantity by using a vision synchronous positioning and mapping algorithmwcAnd the amount of translation Pwc(ii) a Obtaining the relative translation quantity delta P of two adjacent key frames of the inertia measurement unit according to the observed quantity of the inertia measurement unit by utilizing a pre-integration algorithmi,i+1And relative linear velocity Δ vi,i+1(ii) a Constructing the ith frame by a scale factor s and a gravity vector gwZero drift value b of acceleration of inertial measurement unitaAnd the relative translation amount P of the camera and the inertial measurement unitcbThe equation of (c):
wherein λ isiCoefficient before scale factor in the equation, muiCoefficient before the updated value of the rotation angle of the gravity vector in the equation, ωiThe coefficient before the acceleration zero drift value of the inertia measurement unit in the equation is expressed as upsiloniThe coefficient, rho, of the relative translation between the camera and the inertial measurement unit in the equationiThe coefficients of the constant terms in the equation are expressed,jacobian matrix, dt, representing the relative linear velocity of the inertial measurement unit in frame i and frame i +2 compared to the null shift value of the acceleration of the inertial measurement unit12Representing the time difference, dt, between the ith frame and the (i + 1) th frame23Representing the time difference between the i +1 th frame and the i +2 th frame,a Jacobian matrix representing the relative translation of the inertial measurement unit between frame i +1 and frame i +2 compared to the inertial measurement unit acceleration null shift value,a Jacobian matrix, a small scale, representing the relative translation of the inertial measurement unit between frame i and frame i +1 compared to the null shift value of the acceleration of the inertial measurement unit[:,1:2]Representing only momentsThe first two columns of the array;
hypothesis Ci=[λiμiωiυi]The observation quantity formed by adjacent 3 key frames contains the information measurement quantity of unknown quantitySetting the information content window size to NwinIf the total number of key frames is less than NwinAdding the latest key frame into the sequence for initialization and calibration; when the total number of key frames is greater than NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
constructing equation set and solving by least square method
Suppose thatDi=ρiFor every 3 adjacent frames, there is equation Ci·y=Di(ii) a And (3) constructing an observation matrix C and a constant matrix D by using the screened observed quantity, and constructing an equation:
C·y=D (14)
solving optimal estimate s of scale factor using least squares*Optimal estimation of the update value of the rotation angle of the gravity vector*Optimal estimation of acceleration null shift value of inertial measurement unitAnd optimal estimation P of relative translation amount of camera and inertial measurement unitcb *:
By judging the nearest NwincStandard deviation sigma corresponding to each dimension value of vision inertial translation quantityx、σy、σzWhether or not less than a threshold valueDetermining whether the calibration of the visual inertial translation amount is finished; by judging the nearest NwincStandard deviation sigma of individual scale factorssWhether or not it is less than threshold thrsTo determine whether visual inertia initialization is complete; and when the calibration of the vision inertia translation amount is not finished or the initialization of the vision inertia is not finished, waiting for a new key frame and then repeatedly executing the step M2.2 to the step M2.3 until the joint initialization and calibration of the vision and the inertia are finished.
5. The method for initializing and calibrating a camera and an inertial measurement unit online as claimed in claim 1, wherein the step M3 comprises: in the process of synchronous positioning and image building fused by the camera and the inertial measurement unit, judging whether the relative pose of the camera and the inertial measurement unit changes every other preset value period, and when the relative pose of the camera and the inertial measurement unit changes, initializing the angular velocity null shift value of the inertial measurement unit and recalibrating the relative rotation angle and translation quantity of the camera and the inertial measurement unit according to any two adjacent frames in the latest frame;
step M3.1: and judging whether the calibration value of the relative rotation angle of the camera and the inertia needs to be updated according to whether the consistency error between the observed quantity of the camera and the inertia measurement unit and the initial calibration value of the relative rotation angle of the camera and the inertia measurement unit exceeds a threshold value.
6. An online initialization and calibration system for a camera and an inertial measurement unit is characterized by comprising:
module M1: pure vision initialization, namely acquiring the pose of an initial key frame by utilizing the observed quantity of a camera and constructing an initial map;
module M2: calibrating and initializing vision and inertia jointly, and finishing initialization of an angular velocity null shift value of an inertia measurement unit and calibration of a relative rotation angle and a translation amount of a camera and the inertia measurement unit based on the pose of an initial key frame and an initial map;
module M3: in the process of synchronous positioning and mapping fused by the camera and the inertial measurement unit, the initialization of the angular velocity null shift value of the inertial measurement unit and the calibration of the relative rotation angle and translation amount of the camera and the inertial measurement unit are subjected to abnormal detection and updating.
7. The system for online initialization and calibration of a camera and an inertial measurement unit according to claim 6, wherein the module M1 comprises:
module M1.1: according to the new key frame, searching a current frame and a historical frame which accord with the parallax angle and the matching point logarithm of the preset condition;
module M1.2: calculating the relative pose between the current frame and the historical frame by utilizing epipolar constraint;
module M1.3: according to the obtained relative pose between the current frame and the historical frame, obtaining the world coordinate system coordinates of map points observed by the current frame and the historical frame together through triangulation, and constructing an initial map;
module M1.4: and projecting points in the initial map onto the pixel plane of each key frame by using a perspective n-point method, and solving the pose of each key frame in a world coordinate system.
8. The system for online initialization and calibration of a camera and an inertial measurement unit according to claim 6, wherein the module M2 comprises:
module M2.1: screening the observed quantities of the camera and the inertia measurement unit based on the information quantity, and screening out the observed quantities of the preset quantity which maximizes the total information quantity; initializing an angular velocity null shift value of the inertial measurement unit and calibrating a relative rotation angle between the camera and the inertial measurement unit by using the observed quantities of the camera and the inertial measurement unit obtained after screening until the relative rotation angle between the camera and the inertial measurement unit is judged to be converged;
module M2.2: the method comprises the steps that the null shift value of the acceleration of an inertial measurement unit is assumed to be zero, the observed quantities of a camera and the inertial measurement unit are screened based on information quantity, the observed quantities of a fixed quantity of maximized total information quantity are reserved, and a scale factor, a gravity vector and the relative translation quantity of the camera and the inertial measurement unit are roughly estimated;
module M2.3: screening the observed quantities of the camera and the inertial measurement unit based on the information quantity by using the direction of the roughly estimated gravity vector and introducing the gravity as a new constraint, reserving a fixed quantity of observed quantities maximizing the total information quantity, and finely estimating a scale factor, the gravity vector, the relative translation quantity of the camera and the inertial measurement unit and a null shift value of an accelerometer in the inertial measurement unit; when the scale factor and the relative translation amount of the camera and the inertial measurement unit are converged, the visual and inertial joint initialization and calibration are completed;
the information quantity is the observed quantity of the camera and the inertial measurement unit and comprises the relative rotation angle information quantity of the camera and the inertial measurement unit.
9. The system for on-line initialization and calibration of a camera and an inertial measurement unit according to claim 8, characterized in that said module M2.1 comprises:
module M2.1.1: obtaining the relative rotation angle of the inertia measurement unit between two adjacent frames by using the observed quantity of the inertia measurement unit;
obtaining the relative rotation angle of the inertial measurement unit between two adjacent frames by integrating the angular velocity of the inertial measurement unit
Wherein, Δ Ri,i+1Representing an inertial measurement unit angular velocity integral value;a Jacobian matrix representing a null shift value of the inertial measurement unit with respect to the angular velocity; bgRepresenting the zero drift value of the angular speed of the inertial measurement unit;
module M2.1.2: obtaining the relative rotation angle of the inertial measurement unit between two adjacent frames by using the observed quantity of the camera;
two adjacent images are obtained by visual synchronous positioning and mappingPose of frame camera under world coordinate systemObtaining the relative rotation angle of the inertial measurement unit by using the relative rotation angle of the camera and the inertial measurement unit
Wherein R iscbRepresenting a relative rotation angle of the camera and the inertial measurement unit;
module M2.1.3: minimizing an error of a relative rotation angle of the inertial measurement unit between two adjacent frames obtained by the observed quantity of the inertial measurement unit and a relative rotation angle of the inertial measurement unit between two adjacent frames obtained by the observed quantity of the camera to obtain an optimal estimation of an angular velocity null shift value of the inertial measurement unit, namely initializing the angular velocity null shift value of the inertial measurement unit;
obtaining the optimal estimation of the null shift value of the angular velocity of the inertial measurement unit by minimizing the error of the two relative rotation angles
Where N denotes the number of key frames, R2 denotes the relative rotation angle of the inertial measurement unit obtained from the camera observation, R1 denotes the relative rotation angle of the inertial measurement unit obtained by integration of the angular velocity of the inertial measurement unit, and biInertial measurement Unit representing the ith Key frame, bi+1Inertial measurement Unit representing the i +1 st Key frame, bgRepresenting the zero drift value of the angular speed of the inertial measurement unit;
module M2.1.4: for any two adjacent key frames, the quantity of information about the relative rotation angle between the camera and the inertial measurement unit contained in the observed quantity is as follows:
wherein Q isL(q) a left-hand matrix corresponding to the quaternion q; qR(q) a right-multiplication matrix corresponding to the quaternion q; q. q.sbi,bi+1A quaternion representing the relative rotation angle of the inertial measurement unit of the ith frame and the (i + 1) th frame; q. q.sci,ci+1A quaternion representing the relative rotation angle of the i frame and the i +1 frame camera;
setting the information content window size to NwinIf the total number of key frames is less than NwinThe latest key frame is added to the sequence for initialization and calibration. If the total number of key frames is greater than NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
module M2.1.5: constructing an equation set taking the relative rotation angle of the camera and the inertial measurement unit as unknown quantity by using the screened observed quantity, and solving the equation set by using a least square method to obtain the optimal estimation of the relative rotation angle of the camera and the inertial measurement unit expressed in the form of quaternionCalibrating the relative rotation angle between the camera and the inertial measurement unit;
for any two adjacent key frames, the relative rotation angle of the camera and the inertial measurement unit is an unknown quantity, and the following equation system is satisfied:
(QL(qbi,bi+1)-QR(qci,ci+1))·qbc=0 (5)
wherein Q isLA left-hand multiplication matrix, q, representing a quaternionbi,bi+1Quaternion representation, Q, representing the relative rotation angle of the inertial measurement unit in frame i and frame i +1RA right-hand multiplication matrix representing a quaternion, qci,ci+1Quaternary element representing relative rotation angles of camera in ith frame and (i + 1) th frameNumber representation, qbcA quaternion representation representing the relative rotation angle of the camera and the inertial measurement unit;
will (Q)L(qbi,bi+1)-QR(qci,ci+1) Is represented by Q)i,i+1Using the observed quantity Q after screeningi,i+1Forming an observation matrix Q, and establishing an equation:
Q·qbc=0 (6)
solving for optimal estimation of the relative rotation angle of the camera and the inertial measurement unit using least squares
Module M2.1.6: by judging the nearest NwincStandard deviation sigma of yaw angle, pitch angle and roll angle of Euler angle corresponding to relative rotation angle of individual camera and inertial measurement unityaw、σpitch、σrollWhether or not less than a threshold valueDetermining whether the calibration of the visual inertia rotation angle is finished; when the calibration is not completed, that is, the relative rotation angle between the camera and the inertial measurement unit is not converged, wait for a new key frame to repeatedly trigger the execution of the modules M2.1.1 to M2.1.5 until the calibration of the visual inertial rotation angle is completed;
said module M2.2 comprises:
module M2.2.1: obtaining a rotation angle and a translation amount of the camera in a world coordinate system according to the camera observed quantity by utilizing a vision synchronous positioning and mapping algorithm; obtaining the relative translation amount and the relative linear velocity of two adjacent key frames of the inertia measurement unit according to the observed amount of the inertia measurement unit by using a pre-integration algorithm;
constructing the ith frame by a scale factor s and a gravity vector gwAnd the relative translation amount P of the camera and the inertial measurement unitcbThe equation of (c):
wherein, an acceleration zero drift value b of the inertial measurement unit is assumedaα is zeroiExpressing the coefficients before the scale factor in the equation, βiCoefficient before the initialized value of the gravity vector in the equation, gammaiExpressing the coefficients of the equations before the amount of visual inertial translation, ηiConstant term coefficients in the equation are expressed, and s represents a scale factor; gwAn initialization value representing a gravity vector; pcbRepresenting the visual inertial translation amount; rwcRepresenting the rotation angle of the camera in the world coordinate system; pwcThe translation amount of the camera in the world coordinate system;represents the translation amount of the i +1 th frame camera in the world coordinate system,represents the translation amount, dt, of the camera of the ith frame in the world coordinate systemi+1,i+2Representing the time difference between the (i + 1) th frame and the (i + 2) th frame,denotes the (i + 2) thTranslation, dt, of frame camera in world coordinate systemi,i+1Representing the time difference between the ith frame and the (I + 1) th frame, I3×3The unit matrix is represented by a matrix of units,represents the rotation angle of the i +2 th frame camera in the world coordinate system,represents the rotation angle of the i +1 th frame camera in the world coordinate system,representing the rotation angle, R, of the i-th frame camera in the world coordinate systemcbRepresenting the relative rotation angle, Δ P, between the camera and the inertial measurement uniti,i+1Represents the relative translation, Δ P, of the inertial measurement units of frame i and frame i +1i+1,i+2Represents the relative translation, Deltav, of the inertial measurement units of frame i +1 and frame i +2i,i+1Representing the relative linear speeds of the inertial measurement units of the (i + 1) th frame and the (i + 2) th frame;
module M2.2.2: suppose Ai=[αiβiγi]The observation quantity formed by adjacent 3 key frames contains the information measurement quantity of unknown quantitySetting the information content window size to NwinWhen the total number of key frames is less than NwinAdding the latest key frame into the sequence for initialization and calibration; when the total number of the key frames is more than or equal to NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
module M2.2.3: suppose thatBi=ηiFor every 3 adjacent frames, there is equation Ai·x=Bi(ii) a After screeningThe information quantity of the system is used for constructing an information matrix A and a constant matrix B, and an equation is constructed:
A·x=B (9)
solving optimal estimate s of scale factor using least squares*Optimal estimation of the gravity vector gw *And optimal estimation P of relative translation amount of camera and inertial measurement unitcb *:
Said module M2.3 comprises: introducing a gravity vector with a preset value as a constraint to obtain a scale factor s and a gravity vector gwInitial value of (1) and visual inertial translation amount PcbA calibration value of (1);
according to the obtained gravity vector gwObtaining the rotation angle of gravity:
wherein R isWIRepresenting the rotation angle of gravity in the world coordinate system,representation and gravity vectorAnd a fixed vector gIVertical unit vector, theta is expressed in relation to gravity vectorAnd a fixed vector gIAngle between them, gIRepresents a fixed vector; the gravity vector is expressed by the rotation angle and the gravity magnitude and is subjected to first-order Taylor expansion:
wherein, theta tableAn updated value indicating the rotation angle of the gravity vector; g represents the magnitude of gravity, θxThe component of the updated value representing the rotation angle of the gravity vector in the x-axis of the world coordinate system, θyA component of the updated value representing the rotation angle of the gravity vector in the y-axis of the world coordinate system;
obtaining the rotation angle R of the camera in the world coordinate system according to the camera observation quantity by using a vision synchronous positioning and mapping algorithmwcAnd the amount of translation Pwc(ii) a Obtaining the relative translation quantity delta P of two adjacent key frames of the inertia measurement unit according to the observed quantity of the inertia measurement unit by utilizing a pre-integration algorithmi,i+1And relative linear velocity Δ vi,i+1(ii) a Constructing the ith frame by a scale factor s and a gravity vector gwZero drift value b of acceleration of inertial measurement unitaAnd the relative translation amount P of the camera and the inertial measurement unitcbThe equation of (c):
wherein λ isiCoefficient before scale factor in the equation, muiCoefficient before the updated value of the rotation angle of the gravity vector in the equation, ωiTo representCoefficient before acceleration zero drift value of inertia measurement unit in equation, upsiloniThe coefficient, rho, of the relative translation between the camera and the inertial measurement unit in the equationiThe coefficients of the constant terms in the equation are expressed,jacobian matrix, dt, representing the relative linear velocity of the inertial measurement unit in frame i and frame i +2 compared to the null shift value of the acceleration of the inertial measurement unit12Representing the time difference, dt, between the ith frame and the (i + 1) th frame23Representing the time difference between the i +1 th frame and the i +2 th frame,a Jacobian matrix representing the relative translation of the inertial measurement unit between frame i +1 and frame i +2 compared to the inertial measurement unit acceleration null shift value,a Jacobian matrix, a small scale, representing the relative translation of the inertial measurement unit between frame i and frame i +1 compared to the null shift value of the acceleration of the inertial measurement unit[:,1:2]The representation takes only the first two columns of the matrix;
hypothesis Ci=[λiμiωiυi]The observation quantity formed by adjacent 3 key frames contains the information measurement quantity of unknown quantitySetting the information content window size to NwinIf the total number of key frames is less than NwinAdding the latest key frame into the sequence for initialization and calibration; when the total number of key frames is greater than NwinComparing the current information quantity measurement with the information quantity measurement in the existing sequence, and removing the observed quantity with the minimum information quantity;
constructing equation set and solving by least square method
Suppose thatDi=ρiFor every 3 adjacent frames, there is equation Ci·y=Di(ii) a And (3) constructing an observation matrix C and a constant matrix D by using the screened observed quantity, and constructing an equation:
C·y=D (14)
solving optimal estimate s of scale factor using least squares*Optimal estimation of the update value of the rotation angle of the gravity vector*Optimal estimation of acceleration null shift value of inertial measurement unitAnd optimal estimation P of relative translation amount of camera and inertial measurement unitcb *:
By judging the nearest NwincStandard deviation sigma corresponding to each dimension value of vision inertial translation quantityx、σy、σzWhether or not less than a threshold valueDetermining whether the calibration of the visual inertial translation amount is finished; by judging the nearest NwincStandard deviation sigma of individual scale factorssWhether or not it is less than threshold thrsTo determine whether visual inertia initialization is complete; when the calibration of the vision inertia translation amount is not completed or the initialization of the vision inertia is not completed, the module M2.2 to the module M2.3 are repeatedly triggered to execute after waiting for a new key frame until the vision and inertia combined initialization and calibration are completed.
10. The system for online initialization and calibration of a camera and an inertial measurement unit according to claim 6, wherein the module M3 comprises: in the process of synchronous positioning and image building fused by the camera and the inertial measurement unit, judging whether the relative pose of the camera and the inertial measurement unit changes every other preset value period, and when the relative pose of the camera and the inertial measurement unit changes, initializing the angular velocity null shift value of the inertial measurement unit and recalibrating the relative rotation angle and translation quantity of the camera and the inertial measurement unit according to any two adjacent frames in the latest frame;
module M3.1: and judging whether the calibration value of the relative rotation angle of the camera and the inertia needs to be updated according to whether the consistency error between the observed quantity of the camera and the inertia measurement unit and the initial calibration value of the relative rotation angle of the camera and the inertia measurement unit exceeds a threshold value.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010197372.3A CN111429524B (en) | 2020-03-19 | 2020-03-19 | Online initialization and calibration method and system for camera and inertial measurement unit |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010197372.3A CN111429524B (en) | 2020-03-19 | 2020-03-19 | Online initialization and calibration method and system for camera and inertial measurement unit |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111429524A true CN111429524A (en) | 2020-07-17 |
CN111429524B CN111429524B (en) | 2023-04-18 |
Family
ID=71548171
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010197372.3A Active CN111429524B (en) | 2020-03-19 | 2020-03-19 | Online initialization and calibration method and system for camera and inertial measurement unit |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111429524B (en) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112229424A (en) * | 2020-11-16 | 2021-01-15 | 浙江商汤科技开发有限公司 | Parameter calibration method and device for visual inertial system, electronic equipment and medium |
CN112629565A (en) * | 2021-03-08 | 2021-04-09 | 中国人民解放军国防科技大学 | Method, device and equipment for calibrating rotation relation between camera and inertial measurement unit |
CN113313763A (en) * | 2021-05-26 | 2021-08-27 | 珠海深圳清华大学研究院创新中心 | Monocular camera pose optimization method and device based on neural network |
CN114018291A (en) * | 2021-11-08 | 2022-02-08 | 中国科学院空天信息创新研究院 | Calibration method and device for parameters of inertial measurement unit |
CN114199275A (en) * | 2020-09-18 | 2022-03-18 | 阿里巴巴集团控股有限公司 | Parameter determination method and device for sensor |
CN115451958A (en) * | 2022-11-10 | 2022-12-09 | 中国人民解放军国防科技大学 | Camera absolute attitude optimization method based on relative rotation angle |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108827315A (en) * | 2018-08-17 | 2018-11-16 | 华南理工大学 | Vision inertia odometer position and orientation estimation method and device based on manifold pre-integration |
CN110044354A (en) * | 2019-03-28 | 2019-07-23 | 东南大学 | A kind of binocular vision indoor positioning and build drawing method and device |
WO2019157925A1 (en) * | 2018-02-13 | 2019-08-22 | 视辰信息科技(上海)有限公司 | Visual-inertial odometry implementation method and system |
WO2019169540A1 (en) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人(深圳)有限公司 | Method for tightly-coupling visual slam, terminal and computer readable storage medium |
CN110375738A (en) * | 2019-06-21 | 2019-10-25 | 西安电子科技大学 | A kind of monocular merging Inertial Measurement Unit is synchronous to be positioned and builds figure pose calculation method |
CN110726406A (en) * | 2019-06-03 | 2020-01-24 | 北京建筑大学 | Improved nonlinear optimization monocular inertial navigation SLAM method |
-
2020
- 2020-03-19 CN CN202010197372.3A patent/CN111429524B/en active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2019157925A1 (en) * | 2018-02-13 | 2019-08-22 | 视辰信息科技(上海)有限公司 | Visual-inertial odometry implementation method and system |
WO2019169540A1 (en) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人(深圳)有限公司 | Method for tightly-coupling visual slam, terminal and computer readable storage medium |
CN108827315A (en) * | 2018-08-17 | 2018-11-16 | 华南理工大学 | Vision inertia odometer position and orientation estimation method and device based on manifold pre-integration |
CN110044354A (en) * | 2019-03-28 | 2019-07-23 | 东南大学 | A kind of binocular vision indoor positioning and build drawing method and device |
CN110726406A (en) * | 2019-06-03 | 2020-01-24 | 北京建筑大学 | Improved nonlinear optimization monocular inertial navigation SLAM method |
CN110375738A (en) * | 2019-06-21 | 2019-10-25 | 西安电子科技大学 | A kind of monocular merging Inertial Measurement Unit is synchronous to be positioned and builds figure pose calculation method |
Non-Patent Citations (1)
Title |
---|
王昱欣等: "软体机器人手眼视觉/形状混合控制", 机器人 * |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114199275A (en) * | 2020-09-18 | 2022-03-18 | 阿里巴巴集团控股有限公司 | Parameter determination method and device for sensor |
CN112229424A (en) * | 2020-11-16 | 2021-01-15 | 浙江商汤科技开发有限公司 | Parameter calibration method and device for visual inertial system, electronic equipment and medium |
CN112229424B (en) * | 2020-11-16 | 2022-04-22 | 浙江商汤科技开发有限公司 | Parameter calibration method and device for visual inertial system, electronic equipment and medium |
CN112629565A (en) * | 2021-03-08 | 2021-04-09 | 中国人民解放军国防科技大学 | Method, device and equipment for calibrating rotation relation between camera and inertial measurement unit |
CN112629565B (en) * | 2021-03-08 | 2021-06-01 | 中国人民解放军国防科技大学 | Method, device and equipment for calibrating rotation relation between camera and inertial measurement unit |
CN113313763A (en) * | 2021-05-26 | 2021-08-27 | 珠海深圳清华大学研究院创新中心 | Monocular camera pose optimization method and device based on neural network |
CN114018291A (en) * | 2021-11-08 | 2022-02-08 | 中国科学院空天信息创新研究院 | Calibration method and device for parameters of inertial measurement unit |
CN115451958A (en) * | 2022-11-10 | 2022-12-09 | 中国人民解放军国防科技大学 | Camera absolute attitude optimization method based on relative rotation angle |
CN115451958B (en) * | 2022-11-10 | 2023-02-03 | 中国人民解放军国防科技大学 | Camera absolute attitude optimization method based on relative rotation angle |
Also Published As
Publication number | Publication date |
---|---|
CN111429524B (en) | 2023-04-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111429524B (en) | Online initialization and calibration method and system for camera and inertial measurement unit | |
Chilian et al. | Multisensor data fusion for robust pose estimation of a six-legged walking robot | |
CN110081881B (en) | Carrier landing guiding method based on unmanned aerial vehicle multi-sensor information fusion technology | |
CN110986968B (en) | Method and device for real-time global optimization and error loop judgment in three-dimensional reconstruction | |
CN112113582A (en) | Time synchronization processing method, electronic device, and storage medium | |
CN112183171A (en) | Method and device for establishing beacon map based on visual beacon | |
Yang et al. | Online imu intrinsic calibration: Is it necessary? | |
CN112184824A (en) | Camera external parameter calibration method and device | |
CN113223161B (en) | Robust panoramic SLAM system and method based on IMU and wheel speed meter tight coupling | |
CN111707261A (en) | High-speed sensing and positioning method for micro unmanned aerial vehicle | |
CN111754579A (en) | Method and device for determining external parameters of multi-view camera | |
CN112347205A (en) | Method and device for updating error state of vehicle | |
CN112388635B (en) | Method, system and device for fusing sensing and space positioning of multiple sensors of robot | |
CN115540860A (en) | Multi-sensor fusion pose estimation algorithm | |
CN110986928A (en) | Real-time drift correction method for triaxial gyroscope of photoelectric pod | |
CN114310901A (en) | Coordinate system calibration method, apparatus, system and medium for robot | |
CN114217665A (en) | Camera and laser radar time synchronization method, device and storage medium | |
JP7145770B2 (en) | Inter-Vehicle Distance Measuring Device, Error Model Generating Device, Learning Model Generating Device, Methods and Programs Therefor | |
Liu et al. | High altitude monocular visual-inertial state estimation: Initialization and sensor fusion | |
CN108827287B (en) | Robust visual SLAM system in complex environment | |
JP2730457B2 (en) | Three-dimensional position and posture recognition method based on vision and three-dimensional position and posture recognition device based on vision | |
CN113436267A (en) | Visual inertial navigation calibration method and device, computer equipment and storage medium | |
CN114812601A (en) | State estimation method and device of visual inertial odometer and electronic equipment | |
CN115797490B (en) | Graph construction method and system based on laser vision fusion | |
CN112712107B (en) | Optimization-based vision and laser SLAM fusion positioning method |
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 |