CN114397642A - Three-dimensional laser radar and IMU external reference calibration method based on graph optimization - Google Patents

Three-dimensional laser radar and IMU external reference calibration method based on graph optimization Download PDF

Info

Publication number
CN114397642A
CN114397642A CN202210038266.XA CN202210038266A CN114397642A CN 114397642 A CN114397642 A CN 114397642A CN 202210038266 A CN202210038266 A CN 202210038266A CN 114397642 A CN114397642 A CN 114397642A
Authority
CN
China
Prior art keywords
imu
laser radar
residual
residual error
optimization
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.)
Pending
Application number
CN202210038266.XA
Other languages
Chinese (zh)
Inventor
黄志清
张凡
李�杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing University of Technology
Original Assignee
Beijing University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing University of Technology filed Critical Beijing University of Technology
Priority to CN202210038266.XA priority Critical patent/CN114397642A/en
Publication of CN114397642A publication Critical patent/CN114397642A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating
    • 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
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Operations Research (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

The invention provides a three-dimensional laser radar and IMU external reference calibration method based on graph optimization, which relates to the technical field of multi-sensor calibration and comprises the following steps: acquiring measurement data of a laser radar and an IMU in equipment; pre-integrating the measurement data of the IMU, and calculating an IMU residual error; projecting the measurement data of the laser radar to a world coordinate system through an IMU coordinate system to obtain a point cloud picture, and calculating the distance residual error between the laser radar and corresponding characteristic lines and characteristic surfaces in the point cloud picture; obtaining an initial objective function and an initial optimization incremental equation based on the graph model, the IMU residual error and the distance residual error; setting a constant frame sliding window, acquiring an marginalized increment optimization equation, and calculating a marginalized residual error term; and obtaining a target function based on the graph model, the IMU residual error, the distance residual error and the marginalized residual error, and calculating external reference calibration of the laser radar and the IMU. The invention obtains the latest state of the sensor equipment in real time through the constant frame sliding window, and realizes the real-time calibration of external parameters.

Description

Three-dimensional laser radar and IMU external reference calibration method based on graph optimization
Technical Field
The invention relates to the technical field of multi-sensor calibration, in particular to a three-dimensional laser radar and IMU external reference calibration method based on graph optimization.
Background
Multi-sensor fusion is an important technology used by unmanned vehicles, mobile robots, and mapping instruments for accurate environmental perception. Because the current work requirement on the robot is higher and higher, the environment perception work of the robot cannot be accurately realized only by using a single sensor. Therefore, the multi-sensor fusion technology is concerned by many researchers at home and abroad; different sensor characteristics differ, for example, Inertial Measurement Units (IMUs) have high update rates, but sensor data is noisy and drifting; laser radar (LIDAR) has accurate depth perception capability, but is easy to have the problem of motion distortion in the motion process; therefore, the single sensor has the defect that the robot positioning result using the single sensor is not accurate enough. Therefore, through data fusion, the defects of the multiple sensors can be mutually compensated, and the whole sensing capability of the carrier is obviously improved.
For data fusion of the IMU and the laser radar, external reference calibration of the laser radar and the IMU is required to be carried out firstly, namely coordinate conversion between coordinate systems of the two sensors is calculated. As shown in fig. 2, when the robot is at position a, the point P observed by the radar output by the IMU will become P1, without external parameter calibration between sensors. With the robot at position B, the point P observed by the radar output by the IMU will become P2. Due to the inconsistency of the coordinate system, the point P generates two points P1 and P2. Therefore, only by obtaining the accurate coordinate transformation relation between the two sensors (namely calculating the conversion between the coordinate systems of the sensors), the robot can accurately detect the environment in an unknown environment through data fusion, and complete the self positioning and the process of constructing the surrounding map at the same time. Therefore, the sensor calibration is a technology with a great application prospect, has a strong research value, and is very necessary for the research of external reference calibration of the laser radar and the IMU sensor.
At present, some researchers have achieved certain results on the research of LIDAR-IMU external reference calibration, and one of the existing methods is realized by means of an additional sensor or manually setting a specific target, which is time-consuming and labor-consuming; the other method is a non-target external reference calibration method, which has the problems of high requirements on the plane characteristics of the experimental environment and high calculation intensity in the optimization process of the objective function. Meanwhile, LIDAR-IMU external references are generally calibrated off-line, requiring a professional to move the carrier into a fixed calibration environment, and when the mechanical configuration of the sensors of the carrier device changes slightly, the calibration work needs to be repeated, which is complex and laborious.
Disclosure of Invention
Aiming at the problems, the invention provides a three-dimensional laser radar and IMU external reference calibration method based on graph optimization, which realizes the quick and real-time three-dimensional laser radar and IMU external reference calibration.
In order to achieve the aim, the invention provides a three-dimensional laser radar and IMU external reference calibration method based on graph optimization, which comprises the following steps:
respectively acquiring measurement data of a laser radar and an IMU in sensor equipment;
performing IMU pre-integration on the measurement data of the IMU, acquiring a pose transformation estimation value of the IMU at the next moment according to the IMU pre-integration, and calculating an IMU residual error;
projecting the measurement data of the laser radar to an IMU coordinate system, and then projecting the measurement data to a world coordinate system to obtain a point cloud picture, and obtaining distance residual errors between the laser radar and corresponding characteristic lines and characteristic surfaces in the point cloud picture;
based on a graph model, obtaining an initial objective function and an initial optimization incremental equation through the IMU residual error and the distance residual error;
setting a constant frame sliding window, acquiring an marginalized increment optimization equation according to the initial optimization increment equation, and calculating a marginalized residual error term;
based on a graph model, obtaining a target function through the IMU residual error, the distance residual error and the marginalized residual error;
and minimizing the target function to obtain external reference calibration of the laser radar and the IMU.
As a further improvement of the present invention, obtaining an estimated value of pose transformation of the IMU at the next time according to the IMU pre-integration comprises:
constructing an IMU motion model;
based on the IMU motion model, acquiring an IMU pose state transformation equation of the laser radar point between two adjacent frames in a quaternion mode;
and calculating the pose transformation estimation value of the IMU at the next moment according to the IMU pose state transformation equation.
As a further improvement of the invention, the IMU residual error is calculated according to the pose transformation estimated value of the IMU at the next moment and the actual measurement value of the IMU at the next moment.
As a further improvement of the present invention, the obtaining of the distance residual error between the laser radar point and the corresponding characteristic line and characteristic plane in the point cloud chart includes:
extracting the features of the point cloud picture to obtain line feature points and surface feature points;
respectively constructing a line fitting the laser radar points to the line characteristic points in the point cloud picture and a distance equation to a surface fitting the surface characteristic points in the point cloud picture;
and minimizing the two distance equations to respectively obtain the distance residual between the point and the line and the distance residual between the point and the plane.
As a further improvement of the invention, feature extraction is carried out on the point cloud picture to obtain line feature points and surface feature points; the method comprises the following steps:
recording the space coordinate of the kth line laser radar point i in the laser radar coordinate system as
Figure BDA0003468984110000031
And (3) taking i as a set of nearby neighbors and recording the set as S, the formula of the curvature c is as follows:
Figure BDA0003468984110000032
and calculating the curvature of each laser radar point according to a curvature formula, and marking the points with the curvature larger than 0.1 as line characteristic points and the points with the curvature smaller than 0.1 as surface characteristic points.
As a further improvement of the present invention, the obtaining an initial objective function through the IMU residual and the distance residual based on the graph model includes:
based on the graph model, the lidar and IMU external parameters may be expressed as maximum likelihood estimates:
Figure BDA0003468984110000033
wherein the content of the first and second substances,
z represents a measured value;
f represents an initialization objective function;
according to the IMU residual error and the distance residual error, initializing an objective function and expressing as follows:
Figure BDA0003468984110000034
wherein the content of the first and second substances,
de. dp respectively represents the distance residual error between the laser radar point and the corresponding characteristic line and characteristic surface in the point cloud picture;
Figure BDA0003468984110000035
representing the IMU residual.
As a further improvement of the present invention, the obtaining an initial optimization increment equation comprises:
obtaining an incremental optimization equation by performing nonlinear optimization on most objective functions:
HδX＝b
wherein:
H＝ΣJTC-1J
b＝ΣJTC-1r
j represents the Jacobian equation of the residual state quantity;
c represents a covariance matrix of the states;
r represents the residual of the cost equation;
δ X represents the error state to be estimated;
updating the variable X to be optimized by solving the obtained error state delta X to be estimatedrAnd then:
Figure BDA0003468984110000041
wherein the content of the first and second substances,
o represents a small angle update of the quaternion.
As a further improvement of the invention, the constant frame sliding window is set, an marginalized increment optimization equation is obtained according to the initial optimization increment equation, and a marginalized residual error term is calculated; the method comprises the following steps:
by adopting an marginalization method, when a key frame enters the sliding window, and a constraint condition corresponding to the key frame to be discarded is kept, the marginalization increment optimization equation is expressed as:
Figure BDA0003468984110000042
obtaining an marginalized optimization equation by using a elimination method:
Figure BDA0003468984110000043
wherein the content of the first and second substances,
Xmrepresenting the amount of optimization that needs marginalization;
Xrrepresenting variables that need to be kept optimized;
calculating the variable X to be optimized before and after marginalization according to the initialized optimization equation and the marginalized optimization equationrAnd obtaining the marginalization error term.
As a further improvement of the present invention, the graph model is based on, and an objective function is obtained through the IMU residual, the distance residual, and the marginalized residual; the method comprises the following steps:
based on the graph model, the lidar and IMU external parameters may be expressed as maximum likelihood estimates:
Figure BDA0003468984110000044
wherein the content of the first and second substances,
z represents a measured value;
f (X) represents an objective function;
according to the IMU residual, distance residual and marginalized residual terms, the objective function is expressed as:
Figure BDA0003468984110000051
wherein the content of the first and second substances,
de. dp respectively represents the distance residual error between the laser radar point and the corresponding characteristic line and characteristic surface in the point cloud picture;
Figure BDA0003468984110000052
representing the IMU residual;
rprepresenting the marginalized residual terms.
As a further improvement of the invention, the IMU residual error, the distance residual error and the marginalized residual error items in the target function are minimized by adopting a Gauss-Newton method, and the laser radar and IMU external reference calibration is obtained.
Compared with the prior art, the invention has the beneficial effects that:
compared with the prior art, the method has the advantages that the latest state of the sensor equipment is acquired in real time by arranging the sliding window, so that the external reference of the laser radar and the IMU is calibrated in real time, and the method is simple, convenient and quick; meanwhile, in calibration calculation, a point cloud picture is generated by utilizing the relation between the laser radar coordinate system and the IMU coordinate system, the distance residual errors of the laser radar point opposite surface and the laser radar point opposite line are respectively obtained corresponding to the characteristic line and the special front surface in the point cloud picture, and the distance residual errors are used as constraint conditions of external reference calibration, so that the accuracy of the external reference calibration is improved.
In order to ensure the computational complexity in the optimization process, the invention sets the sliding window as a fixed frame, realizes the balance of computational efficiency and accuracy and simultaneously meets the requirement of real-time calibration.
When the method adopts the fixed frame sliding window, in order to avoid the influence of discarding state data, a marginalized residual item is obtained by adopting a marginalization method, and then the marginalized residual item is used as a constituent item of a target function, so that the accuracy of laser radar and IMU external parameter optimization is further improved.
Drawings
FIG. 1 is a flowchart of a three-dimensional laser radar and IMU external reference calibration method based on graph optimization according to an embodiment of the present invention;
FIG. 2 is a diagram illustrating differences in coordinate systems of sensors in a multi-sensor apparatus disclosed in the background of the invention;
FIG. 3 is a schematic diagram of a three-dimensional laser radar and IMU external reference calibration method based on graph optimization according to an embodiment of the present invention;
FIG. 4 is a schematic diagram illustrating a relationship between a lidar factor and an IMU factor in a graph model according to an embodiment of the present invention;
FIG. 5 is a cloud point diagram illustration according to an embodiment of the present disclosure;
FIG. 6 is a schematic diagram of a line feature point according to an embodiment of the present invention;
FIG. 7 is a schematic view of a surface feature point disclosed in one embodiment of the present invention;
FIG. 8 is a schematic illustration of the point-to-line and point-to-plane geometric constraints disclosed in one embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be obtained by a person skilled in the art without any inventive step based on the embodiments of the present invention, are within the scope of the present invention.
The invention is described in further detail below with reference to the attached drawing figures:
as shown in fig. 1 and 3, the method for calibrating the three-dimensional lidar and the IMU external reference based on graph optimization provided by the invention comprises the following steps:
s1, respectively acquiring measurement data of the laser radar and the IMU in the sensor equipment;
s2, performing IMU pre-integration on the measurement data of the IMU, acquiring a pose transformation estimation value of the IMU at the next moment according to the IMU pre-integration, and calculating an IMU residual error;
wherein, include:
constructing an IMU motion model;
based on an IMU motion model, acquiring an IMU pose state transformation equation of a laser radar point between two adjacent frames in a quaternion mode;
calculating a pose transformation estimated value of the IMU at the next moment according to the IMU pose state transformation equation;
and calculating the residual error of the IMU according to the pose transformation estimated value of the IMU at the next moment and the actual measurement value of the IMU at the next moment.
Specifically, the method comprises the following steps:
constructing an IMU motion model according to the angular velocity and the linear velocity measured by the IMU; setting letter b to represent IMU carrier coordinate system, letter W to represent world coordinate system, using
Figure BDA0003468984110000071
A rotation transformation matrix representing the world coordinate system to the IMU coordinate system may result in:
Figure BDA0003468984110000072
Figure BDA0003468984110000073
wherein the content of the first and second substances,
wg represents the gravity vector in the world coordinate system.
Respectively representing the acceleration at a certain moment and the angular velocity at a certain moment measured under an IMU coordinate system; a isb(t)，wbAnd (t) respectively representing the true values of the acceleration and the angular velocity under the IMU coordinate system, wherein the true values contain self deviation and interference of random noise.
ba(t)，bg(t) representing accelerometer bias and random noise of the accelerometer, respectively;
na(t)，ng(t) each represents a gyroInstrumental bias and random noise of the gyroscope.
And further obtaining an IMU equation of motion:
Figure BDA0003468984110000074
wherein p isW，vW，aWRespectively representing the position, the speed and the acceleration under a world coordinate system;
the IMU pose state between two lidar keyframes is expressed as follows by using a quaternion mode:
Figure BDA0003468984110000075
Figure BDA0003468984110000076
Figure BDA0003468984110000077
wherein:
Figure BDA0003468984110000078
the IMU pre-integration can avoid the problem of repeated integration, and a reference system needs to be converted into an IMU coordinate system from a world coordinate system during IMU pre-integration, namely, two sides of the formula are multiplied by the IMU coordinate system simultaneously
Figure BDA0003468984110000079
Obtaining the IMU pose state of the bk +1 moment under the IMU coordinate system:
Figure BDA0003468984110000081
Figure BDA0003468984110000082
Figure BDA0003468984110000083
wherein α, β and γ are the result of IMU pre-integration:
Figure BDA0003468984110000084
discretizing the IMU pre-integration result by using a median method to obtain:
Figure BDA0003468984110000085
Figure BDA0003468984110000086
Figure BDA0003468984110000087
wherein:
Figure BDA0003468984110000088
Figure BDA0003468984110000089
according to the IMU pose transformation estimation value obtained by calculation and an actual IMU measurement value at the next moment (for example, subtracting the IMU pose data of m +1 frames when the laser radar data arrives from the IMU pose information of m frames predicted by the IMU pose information of m frames when the laser radar arrives to obtain the pose residual error of the IMU at the moment of m frames of laser radar data), constraining the state between the two moments to obtain the IMU residual error:
Figure BDA00034689841100000810
s3, projecting the measurement data of the laser radar to an IMU coordinate system, projecting the measurement data to a world coordinate system to obtain a point cloud picture (shown in figure 5), and obtaining distance residual errors between the laser radar and corresponding characteristic lines and characteristic surfaces in the point cloud picture;
wherein, include:
extracting the features of the point cloud pictures to obtain line feature points and surface feature points (as shown in figures 6 and 7);
respectively constructing a line fitting from a laser radar point to a line characteristic point in a point cloud picture and a distance equation to a surface fitting from a surface characteristic point in the point cloud picture;
and minimizing two distance equations, and respectively obtaining the distance residual between the point and the line and the distance residual between the point and the plane.
Specifically, the method comprises the following steps:
s3.1, extracting the features of the laser radar point cloud, wherein the specific process of extracting the features comprises the following substeps:
(1) and (4) processing the point cloud distortion problem of the laser radar. The robot and other carriers are assumed to move at a constant speed in the working process, namely the linear speed and the angular speed of the laser radar are kept constant in the primary scanning process of the laser radar. Assuming that the scanning start time of the laser radar is the starting time of the laser radar to start scanning and the scanning end time of the laser radar to finish one scanning, recording the relative pose transformation of the laser radar at the time relative to the time, and then compensating the laser radar point cloud i according to the following formula:
Figure BDA0003468984110000091
(2) recording the point cloud after the point cloud distortion processing of the laser radar as T, recording a certain point in the point cloud T as i, recording a set of adjacent points taking the i as the center as S, recording the curvature of each point as c, and recording the space coordinate of the kth line laser radar point i in a laser radar coordinate system as T
Figure BDA0003468984110000092
The formula for the curvature is then:
Figure BDA0003468984110000093
(3) equally dividing the laser radar point cloud of each frame into six parts, calculating the curvature of the points in each region, sequentially ordering according to the curvature, marking the points with the curvature larger than 0.1 as line characteristic points, and marking the points with the curvature smaller than 0.1 as surface characteristic points.
S3.2, projecting the laser radar points to an IMU (inertial measurement Unit) by using coordinate transformation, projecting the characteristic point cloud of the current radar and the corresponding characteristic surface and characteristic line in the generated point cloud picture to perform radar characteristic matching based on point-to-point and point-to-line under a world coordinate system by using IMU pre-integration; finding a nearest point j of the point i in the local point cloud picture by using a nearest neighbor search algorithm (KDTree search method), and finding a next nearest point l around the j, so that edges of the point i in the picture formed by splicing the points are corresponding to the edges of the point i in the picture; the method for associating plane features comprises the following steps: similarly, the nearest point j in the graph formed by splicing the point clouds is found by i, next-nearest points l and m are found around j, and the (j, l, m) is called as the point i corresponding to the surface in the point cloud graph.
As shown in fig. 8, point-to-line and point-to-surface distance equations can be constructed according to the geometric constraint graphs of point-to-line and point-to-surface, and are used as the associated parameters of the IMU measurement data and the lidar measurement data. The laser radar point-to-line and point-to-plane distance residuals are then expressed as:
Figure BDA0003468984110000101
Figure BDA0003468984110000102
wherein i, j, v, w are corresponding characteristic association points,
Figure BDA0003468984110000103
is the coordinates of the line characteristic points of the (k + 1) th frame point cloud,
Figure BDA0003468984110000104
the coordinates of the surface feature points of the point cloud of the (k + 1) th frame are obtained.
S4, acquiring an initial objective function and an initial optimization incremental equation through IMU residual errors and distance residual errors based on a graph model;
wherein the content of the first and second substances,
based on the graph model, the lidar and IMU external parameters can be expressed as maximum likelihood estimates:
Figure BDA0003468984110000105
wherein the content of the first and second substances,
z represents a measured value;
f represents an initialization objective function;
according to the IMU residual error and the distance residual error, the initialization objective function is expressed as:
Figure BDA0003468984110000106
wherein the content of the first and second substances,
de. dp respectively represents the distance residual error between the laser radar point and the corresponding characteristic line and characteristic surface in the point cloud picture;
Figure BDA0003468984110000107
representing the IMU residual;
then, nonlinear optimization is carried out on most objective functions to obtain an incremental optimization equation:
HδX＝b
wherein:
Figure BDA0003468984110000108
Figure BDA0003468984110000109
j represents the Jacobian equation of the residual state quantity;
c represents a covariance matrix of the states;
r represents the residual of the cost equation;
δ X represents the error state to be estimated;
updating the variable X to be optimized by solving the obtained error state delta X to be estimatedrAnd then:
Figure BDA0003468984110000111
wherein the content of the first and second substances,
o represents a small angle update of the quaternion.
S5, setting a constant frame sliding window, acquiring an marginalized incremental optimization equation according to the initial optimization incremental equation, and calculating a marginalized residual error term;
wherein, include:
in the prior art, in the optimization process of an objective function, the calculation amount increases with the number of key frames and the scale of a state quantity to be optimized, and in order to ensure the calculation complexity in the optimization process, the optimization of the objective function is completed in a sliding window, and a constant frame number is maintained in the sliding window, for example: the state quantity of 11 frames is kept, and the balance of calculation efficiency and accuracy can be realized by setting the number of frames; after the new key frame arrives, an old key frame needs to be removed from the sliding window. And directly discarding the state quantity of the old frame may result in the loss of constraint information between other frames within the sliding window and the frame. With marginalization, constraint information corresponding to the discarded state quantity can be retained as an a priori residual term (marginalized residual term).
By adopting the marginalization method, when a key frame enters a sliding window, and a constraint condition corresponding to the key frame to be discarded is kept, the marginalization increment optimization equation is expressed as:
Figure BDA0003468984110000112
and (3) eliminating Yuan by using Schur:
Figure BDA0003468984110000113
Figure BDA0003468984110000114
obtaining an marginalized optimization equation after elimination of elements:
Figure BDA0003468984110000115
wherein the content of the first and second substances,
Xmrepresenting the amount of optimization that needs marginalization;
Xrrepresenting variables that need to be kept optimized;
since the marginalized optimization equation is obtained by elimination, no constraint information is actually lost in the equation, but more constraints are added to the retained optimization variables, and H is definedP，bpoComprises the following steps:
Figure BDA0003468984110000121
Figure BDA0003468984110000122
when the marginalization is carried out, the
Figure BDA0003468984110000123
Indicated as the subsequent state. In the next optimization, a new estimate of the subsequent state will be used
Figure BDA0003468984110000124
In the form of a new one which will be returned
Figure BDA0003468984110000125
Comprises the following steps:
Figure BDA0003468984110000126
furthermore, when marginalization occurs, bpoIs fixed by providing HpAnd bpCalculating the variable X to be optimized before and after marginalization according to the initialized optimization equation and the marginalized optimization equationrObtaining a marginalization error term; the marginalization error term can be written as:
Figure BDA0003468984110000127
s6, obtaining a target function through IMU residual error, distance residual error and marginalized residual error items based on the graph model;
wherein the content of the first and second substances,
as shown in fig. 4, the lidar and IMU extrinsic solution problem may be represented using a graph model; the graph structure is mainly composed of two parts: nodes and edges. In fig. 4, node C represents the outer reference of the lidar and IMU to be solved, where rotation is represented, translation is represented, black squares represent the lidar factor, black circles represent the IMU factor, and node In represents the pose and velocity of the IMU at time tn. The superscript n denotes the nth scan from the lidar and the subscript W denotes the world coordinate system. The calibration method mainly aims at estimating external parameters C of the laser radar and the IMU and information such as the IMU direction R, the position P, the speed V and the like of each laser radar scanning. X can be used to represent the state of the main estimate, i.e.:
Figure BDA0003468984110000128
the lidar and IMU external parameters may be expressed as maximum likelihood estimates, i.e.:
Figure BDA0003468984110000129
wherein the content of the first and second substances,
z represents a measured value;
f (X) represents an objective function;
the calibration method provided by the invention is added with a processing mechanism of controlling optimization quantity of a sliding window, and can involve the problem of discarding the prior residual error of a frame by marginalization processing, so that the external reference calibration problem can be solved by minimizing the point-to-surface distance and the point-to-line distance of the lidar residual error, the IMU measurement residual error and the marginalization prior residual error. Namely: solving the extrinsic parameters according to the IMU residual, distance residual and marginalized residual terms, so the objective function can be expressed as:
Figure BDA0003468984110000131
wherein the content of the first and second substances,
de. dp respectively represents the distance residual error between the laser radar point and the corresponding characteristic line and characteristic surface in the point cloud picture;
Figure BDA0003468984110000132
representing the IMU residual;
rprepresenting the marginalized residual terms.
And S7, minimizing the objective function, and obtaining external reference calibration of the laser radar and the IMU.
Wherein the content of the first and second substances,
and minimizing IMU residual errors, distance residual errors and marginalized residual error items in the target function by adopting a Gauss-Newton method to obtain the external parameter calibration of the laser radar and the IMU.
The invention has the advantages that:
compared with the prior art, the method has the advantages that the latest state of the sensor equipment is acquired in real time by arranging the sliding window, so that the external reference of the laser radar and the IMU is calibrated in real time, and the method is simple, convenient and quick; meanwhile, in calibration calculation, a point cloud picture is generated by utilizing the relation between the laser radar coordinate system and the IMU coordinate system, the distance residual errors of the laser radar point opposite surface and the laser radar point opposite line are respectively obtained corresponding to the characteristic line and the special front surface in the point cloud picture, and the distance residual errors are used as constraint conditions of external reference calibration, so that the accuracy of the external reference calibration is improved.
In order to ensure the computational complexity in the optimization process, the invention sets the sliding window as a fixed frame, realizes the balance of computational efficiency and accuracy and simultaneously meets the requirement of real-time calibration.
When the method adopts the fixed frame sliding window, in order to avoid the influence of discarding state data, a marginalized residual item is obtained by adopting a marginalization method, and then the marginalized residual item is used as a constituent item of a target function, so that the accuracy of laser radar and IMU external parameter optimization is further improved.
The above is only a preferred embodiment of the present invention, and is not intended to limit the present invention, and various modifications and changes will occur to those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (10)

1. A three-dimensional laser radar and IMU external reference calibration method based on graph optimization is characterized by comprising the following steps:
respectively acquiring measurement data of a laser radar and an IMU in sensor equipment;
performing IMU pre-integration on the measurement data of the IMU, acquiring a pose transformation estimation value of the IMU at the next moment according to the IMU pre-integration, and calculating an IMU residual error;
projecting the measurement data of the laser radar to an IMU coordinate system, and then projecting the measurement data to a world coordinate system to obtain a point cloud picture, and obtaining distance residual errors between the laser radar and corresponding characteristic lines and characteristic surfaces in the point cloud picture;
based on a graph model, obtaining an initial objective function and an initial optimization incremental equation through the IMU residual error and the distance residual error;
setting a constant frame sliding window, acquiring an marginalized increment optimization equation according to the initial optimization increment equation, and calculating a marginalized residual error term;
based on a graph model, obtaining a target function through the IMU residual error, the distance residual error and the marginalized residual error;
and minimizing the target function to obtain external reference calibration of the laser radar and the IMU.
2. The method of claim 1, wherein: obtaining an estimated value of pose transformation of the IMU at the next moment according to the IMU pre-integration, comprising:
constructing an IMU motion model;
based on the IMU motion model, acquiring an IMU pose state transformation equation of the laser radar point between two adjacent frames in a quaternion mode;
and calculating the pose transformation estimation value of the IMU at the next moment according to the IMU pose state transformation equation.
3. The method according to claim 1 or 2, characterized in that: and calculating an IMU residual error according to the pose transformation estimation value of the IMU at the next moment and the actual measurement value of the IMU at the next moment.
4. The method of claim 1, wherein: the obtaining of the distance residual error between the laser radar point and the corresponding characteristic line and characteristic surface in the point cloud picture includes:
extracting the features of the point cloud picture to obtain line feature points and surface feature points;
respectively constructing a line fitting the laser radar points to the line characteristic points in the point cloud picture and a distance equation to a surface fitting the surface characteristic points in the point cloud picture;
and minimizing the two distance equations to respectively obtain the distance residual between the point and the line and the distance residual between the point and the plane.
5. The method of claim 4, wherein: extracting the features of the point cloud picture to obtain line feature points and surface feature points; the method comprises the following steps:
recording the space coordinate of the kth line laser radar point i in the laser radar coordinate system as
Figure FDA0003468984100000021
And (3) taking i as a set of nearby neighbors and recording the set as S, the formula of the curvature c is as follows:
Figure FDA0003468984100000022
and calculating the curvature of each laser radar point according to a curvature formula, and marking the points with the curvature larger than 0.1 as line characteristic points and the points with the curvature smaller than 0.1 as surface characteristic points.
6. The method of claim 1, wherein obtaining an initial objective function from the IMU residual and the range residual based on the graph model comprises:
based on the graph model, the lidar and IMU external parameters may be expressed as maximum likelihood estimates:
Figure FDA0003468984100000023
wherein the content of the first and second substances,
z represents a measured value;
f represents an initialization objective function;
according to the IMU residual error and the distance residual error, initializing an objective function and expressing as follows:
Figure FDA0003468984100000024
wherein the content of the first and second substances,
de. dp respectively represents the distance residual error between the laser radar point and the corresponding characteristic line and characteristic surface in the point cloud picture;
Figure FDA0003468984100000025
representing the IMU residual.
7. The method of claim 1, wherein obtaining an initial optimization increment equation comprises:
obtaining an incremental optimization equation by performing nonlinear optimization on most objective functions:
HδX＝b
wherein:
H＝∑JTC-1J
b＝∑JTC-1r
j represents the Jacobian equation of the residual state quantity;
c represents a covariance matrix of the states;
r represents the residual of the cost equation;
δ X represents the error state to be estimated;
updating the variable X to be optimized by solving the obtained error state delta X to be estimatedrAnd then:
Figure FDA0003468984100000035
wherein the content of the first and second substances,
o denotes a small angle update of the quaternion.
8. The method according to claim 7, wherein the constant frame sliding window is set, the marginalized incremental optimization equation is obtained according to the initial optimization incremental equation, and the marginalized residual term is calculated; the method comprises the following steps:
by adopting an marginalization method, when a key frame enters the sliding window, and a constraint condition corresponding to the key frame to be discarded is kept, the marginalization increment optimization equation is expressed as:
Figure FDA0003468984100000031
obtaining an marginalized optimization equation by using a elimination method:
Figure FDA0003468984100000032
wherein the content of the first and second substances,
Xmrepresenting the amount of optimization that needs marginalization;
Xrrepresenting variables that need to be kept optimized;
calculating the variable X to be optimized before and after marginalization according to the initialized optimization equation and the marginalized optimization equationrAnd obtaining the marginalization error term.
9. The method of claim 1, wherein the objective function is obtained by the IMU residual, distance residual, and marginalized residual terms based on the graph model; the method comprises the following steps:
based on the graph model, the lidar and IMU external parameters may be expressed as maximum likelihood estimates:
Figure FDA0003468984100000033
wherein the content of the first and second substances,
z represents a measured value;
f (X) represents an objective function;
according to the IMU residual, distance residual and marginalized residual terms, the objective function is expressed as:
Figure FDA0003468984100000034
wherein the content of the first and second substances,
de. dp respectively represents the distance residual error between the laser radar point and the corresponding characteristic line and characteristic surface in the point cloud picture;
Figure FDA0003468984100000041
representing the IMU residual;
rprepresenting the marginalized residual terms.
10. The method according to claim 1 or 9, characterized in that: and minimizing IMU residual errors, distance residual errors and marginalized residual error items in the target function by adopting a Gauss-Newton method to obtain the laser radar and IMU external parameter calibration.
CN202210038266.XA 2022-01-13 2022-01-13 Three-dimensional laser radar and IMU external reference calibration method based on graph optimization Pending CN114397642A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210038266.XA CN114397642A (en) 2022-01-13 2022-01-13 Three-dimensional laser radar and IMU external reference calibration method based on graph optimization

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210038266.XA CN114397642A (en) 2022-01-13 2022-01-13 Three-dimensional laser radar and IMU external reference calibration method based on graph optimization

Publications (1)

Publication Number Publication Date
CN114397642A true CN114397642A (en) 2022-04-26

Family

ID=81231370

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210038266.XA Pending CN114397642A (en) 2022-01-13 2022-01-13 Three-dimensional laser radar and IMU external reference calibration method based on graph optimization

Country Status (1)

Country Link
CN (1) CN114397642A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114923453A (en) * 2022-05-26 2022-08-19 杭州海康机器人技术有限公司 Calibration method and device for external parameter of linear contourgraph and electronic equipment

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114923453A (en) * 2022-05-26 2022-08-19 杭州海康机器人技术有限公司 Calibration method and device for external parameter of linear contourgraph and electronic equipment
CN114923453B (en) * 2022-05-26 2024-03-05 杭州海康机器人股份有限公司 Calibration method and device for external parameters of linear profiler and electronic equipment

Similar Documents

Publication Publication Date Title
CN111207774B (en) Method and system for laser-IMU external reference calibration
CN112347840B (en) Vision sensor laser radar integrated unmanned aerial vehicle positioning and image building device and method
CN111795686B (en) Mobile robot positioning and mapping method
CN113781582B (en) Synchronous positioning and map creation method based on laser radar and inertial navigation combined calibration
CN107941217B (en) Robot positioning method, electronic equipment, storage medium and device
CN112815939B (en) Pose estimation method of mobile robot and computer readable storage medium
CN102914293A (en) Information processing apparatus and information processing method
CN111156997B (en) Vision/inertia combined navigation method based on camera internal parameter online calibration
CN111707261A (en) High-speed sensing and positioning method for micro unmanned aerial vehicle
JP2012026895A (en) Position attitude measurement device, position attitude measurement method, and program
CN115272596A (en) Multi-sensor fusion SLAM method oriented to monotonous texture-free large scene
CN111623773B (en) Target positioning method and device based on fisheye vision and inertial measurement
CN114526745A (en) Drawing establishing method and system for tightly-coupled laser radar and inertial odometer
CN115371665B (en) Mobile robot positioning method based on depth camera and inertial fusion
CN111890373A (en) Sensing and positioning method of vehicle-mounted mechanical arm
CN111780781A (en) Template matching vision and inertia combined odometer based on sliding window optimization
CN112179373A (en) Measuring method of visual odometer and visual odometer
CN114964276A (en) Dynamic vision SLAM method fusing inertial navigation
CN114397642A (en) Three-dimensional laser radar and IMU external reference calibration method based on graph optimization
CN108648214B (en) Visual SLAM algorithm and system based on width learning filtering
CN112147599A (en) Spline function-based continuous-time external parameter calibration method for 3D laser radar and inertial sensor
Hu et al. 1d-lrf aided visual-inertial odometry for high-altitude mav flight
CN113155152A (en) Camera and inertial sensor spatial relationship self-calibration method based on lie group filtering
CN111121818B (en) Calibration method for camera and two-dimensional code in unmanned vehicle
CN111637892A (en) Mobile robot positioning method based on combination of vision and inertial navigation

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination