CN117584989B - Laser radar/IMU/vehicle kinematics constraint tight coupling SLAM system and algorithm - Google Patents
Laser radar/IMU/vehicle kinematics constraint tight coupling SLAM system and algorithm Download PDFInfo
- Publication number
- CN117584989B CN117584989B CN202311571021.4A CN202311571021A CN117584989B CN 117584989 B CN117584989 B CN 117584989B CN 202311571021 A CN202311571021 A CN 202311571021A CN 117584989 B CN117584989 B CN 117584989B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- imu
- point cloud
- time
- residual
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000010168 coupling process Methods 0.000 title claims abstract description 40
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 40
- 230000008878 coupling Effects 0.000 title claims abstract description 38
- 238000006073 displacement reaction Methods 0.000 claims abstract description 38
- 238000005457 optimization Methods 0.000 claims abstract description 38
- 239000011159 matrix material Substances 0.000 claims description 32
- 238000000034 method Methods 0.000 claims description 27
- 238000005259 measurement Methods 0.000 claims description 16
- 230000010354 integration Effects 0.000 claims description 11
- 238000013519 translation Methods 0.000 claims description 9
- 238000010276 construction Methods 0.000 claims description 8
- 230000003044 adaptive effect Effects 0.000 claims description 4
- 238000013507 mapping Methods 0.000 claims description 4
- 230000036544 posture Effects 0.000 claims description 4
- 238000007781 pre-processing Methods 0.000 claims description 4
- 230000021615 conjugation Effects 0.000 claims description 3
- 238000013459 approach Methods 0.000 abstract description 2
- 238000012545 processing Methods 0.000 description 13
- 238000010586 diagram Methods 0.000 description 5
- 238000011156 evaluation Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W2050/0001—Details of the control system
- B60W2050/0019—Control system elements or transfer functions
- B60W2050/0028—Mathematical models, e.g. for simulation
- B60W2050/0031—Mathematical model of the vehicle
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2520/00—Input parameters relating to overall vehicle dynamics
- B60W2520/28—Wheel speed
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10016—Video; Image sequence
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10032—Satellite or aerial image; Remote sensing
- G06T2207/10044—Radar image
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Geometry (AREA)
- Computer Graphics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Software Systems (AREA)
- Automation & Control Theory (AREA)
- Human Computer Interaction (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Navigation (AREA)
Abstract
The invention discloses a laser radar/IMU/vehicle kinematics constrained tight coupling SLAM system and algorithm, and belongs to the technical field of automatic driving. According to the invention, the IMU, the rear axle wheel speed and the front wheel steering angle are utilized to construct the vehicle kinematic constraint, and the displacement and posture constraint is respectively constructed through decoupling treatment of the displacement and posture information of the vehicle kinematic constraint, so that the accuracy of an optimization result is improved; and secondly, introducing self-adaptive coefficients according to the number of the characteristic points and the steering angle, and adjusting the weight of the vehicle kinematic constraint in real time. And finally, an IMU and a rear axle wheel speed are adopted to construct an odometer, so that an accurate initial value is provided for tight coupling optimization, and the situation of sinking into local optimum is effectively avoided. A more accurate location and map may be provided than with a single lidar approach.
Description
Technical Field
The invention belongs to the technical field of automatic driving, and particularly relates to a laser radar/IMU/vehicle kinematics constraint tight coupling SLAM system and algorithm.
Background
The automatic driving vehicle comprises three key modules of sensing, decision making and control. The positioning is a key perception technology of an automatic driving vehicle, and is an important precondition for realizing the functions of subsequent path planning, automatic navigation, obstacle avoidance and the like. In order to better solve the positioning problem of the automatic driving vehicle, the simultaneous positioning and map construction (simultaneous localization and map-ping, SLAM) is widely adopted at present to provide positioning information and an environment map for the vehicle. SLAM mainly uses laser radar or camera, relies on the abundant environment perception information of sensor to establish the environment map when estimating the position appearance. However, the SLAM scheme adopting a single sensor has the problems of poor precision and weak robustness, and is difficult to ensure the positioning requirement of the automatic driving vehicle in complex and changeable environments.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides a laser radar/IMU/vehicle kinematics constraint tight coupling SLAM system and algorithm.
A laser radar/IMU/vehicle kinematics constraint tight coupling SLAM system comprises an odometer module, a vehicle kinematics module, a laser radar module and a tight coupling optimization module;
The laser radar module takes a laser radar as a sensor, takes point clouds of the laser radar as data, takes corner points and plane points in radar point clouds of a current frame and corner points and plane points stored in a global point cloud map established before the current frame as input data, adopts a point-to-line and point-to-plane mode to construct point cloud residual errors, and takes the point cloud residual errors as one data input of the close-coupled optimization module;
the odometer module takes the angular speed of the IMU and the rear axle wheel speed of the vehicle as data sources, builds an odometer residual error, and takes the odometer residual error as one data input of the close-coupling optimization module;
The vehicle kinematics module takes the angular speed of the IMU, the rear axle wheel speed of the vehicle and the front wheel rotation angle of the vehicle as data sources, constructs vehicle kinematics residual error, and takes the vehicle kinematics residual error as data input of the tight coupling optimization module;
The tight coupling optimization module takes a point cloud residual error, an odometer residual error and a vehicle kinematics residual error as input data, converts the three residual errors into least square, takes the odometer residual error as an initial value of tight coupling optimization, and adopts a nonlinear optimization method to solve the least square; and registering the point cloud of the current frame to a global point cloud map by adopting the optimized system state quantity.
As a preferred embodiment of the invention, the laser radar module takes a laser radar as a sensor, takes point clouds of the laser radar as data, and processes the point cloud data of each frame in real time; firstly, preprocessing point cloud to remove invalid points; secondly, extracting characteristic points of the point cloud after the pretreatment is finished, and extracting angular points and plane points in the point cloud; then, carrying out point cloud residual construction, wherein the point cloud residual construction takes corner points and plane points in radar point cloud of a current frame and corner points and plane points stored in a global point cloud map established before the current frame as input data, and adopts a point-to-line and point-to-plane mode to construct point cloud residual; and finally, taking the constructed point cloud residual error as one data input of the tight coupling optimization module.
As a preferred embodiment of the present invention, the odometer module takes the angular velocity of the IMU and the rear axle wheel speed of the vehicle as data sources; firstly, pre-integrating to obtain the pose under an IMU coordinate system; then, constructing an odometer residual error; and finally, taking the odometer residual error as one data input of the close-coupled optimization module.
As a preferred embodiment of the present invention, the vehicle kinematics module uses the angular velocity of the IMU, the rear axle wheel speed of the vehicle and the front wheel rotation angle of the vehicle as data sources; firstly, calculating a speed vector and an angular speed vector under a vehicle coordinate system by using a vehicle kinematic model and taking the rear axle wheel speed and the front wheel rotation angle of a vehicle as data input; secondly, pre-integrating the velocity vector of the vehicle kinematic model and the angular velocity of the IMU to obtain a pose with six degrees of freedom, and independently pre-integrating the angular velocity vector of the vehicle kinematic model to obtain a pose with three degrees of freedom; then constructing a vehicle kinematic residual error by adopting the six-degree-of-freedom pose and the three-degree-of-freedom pose obtained by pre-integration; and finally, taking the vehicle kinematic residual error as one data input of the tight coupling optimization module.
As a preferred embodiment of the invention, the close coupling optimization module takes a point cloud residual error, an odometer residual error and a vehicle kinematics residual error as input data, converts the three residual errors into a least square, takes the odometer residual error as an initial value of the close coupling optimization, and adopts a nonlinear optimization method to solve the least square; and registering the point cloud of the current frame to a global point cloud map by adopting the optimized system state quantity.
Based on the system, the invention also claims a SLAM algorithm tightly coupled by the laser radar/IMU/vehicle kinematics constraint, which comprises the following steps:
(1) Constructing the laser radar/IMU/vehicle kinematic constraint tight coupling SLAM system;
(2) Pre-integrating the angular speed of the IMU and the speed polarity of the rear axle wheel speedometer to obtain the pose of the IMU in the coordinate system, and then constructing an odometer residual error;
(3) Firstly, calculating a speed vector and an angular speed vector under a vehicle coordinate system; secondly, pre-integrating the speed vector and the angular speed to obtain a pose with six degrees of freedom, and pre-integrating the angular speed vector to obtain a pose with three degrees of freedom; then constructing a vehicle kinematics residual error;
(4) Constructing point cloud residual constraints by using preprocessing and a characteristic method;
(5) And constructing a close coupling optimization problem by combining the odometer residual error, the vehicle kinematic residual error and the point cloud residual error, converting the close coupling optimization problem into a least square problem, taking the odometer residual error as an initial value of the close coupling optimization, and solving the least square problem by adopting a nonlinear optimization method.
As a preferred embodiment of the invention, the odometer module operates at a frequency of 100Hz and builds an odometer residual. The vehicle kinematics module operates at a frequency of 100Hz and builds vehicle kinematics constraints. The lidar module operates at a frequency of 5Hz, building a point cloud residual. And the solving frequency of the system state quantity is 5Hz, the system state quantity is optimized, and after each time of system state quantity solving is completed, the original point cloud data corresponding to the frame is registered into the global point cloud map by adopting the system state quantity after the solving is completed.
As a preferred embodiment of the present invention, the system state quantity is defined as follows:
wherein X k is a system state quantity at k times, the system state quantity at each time contains three data, Is a translation vector of the laser radar coordinate system under world coordinates,B ω represents zero offset of the IMU angular velocity for quaternion of the laser radar coordinate system in world coordinates.
As a preferred embodiment of the present invention, in the step (2), the odometer residual construction specifically includes:
S2-1: at the angular velocity of the IMU And rear axle wheel speed of vehicleAs the original data, simultaneously taking the noise existing in the original data into consideration, and establishing the angular velocity of the IMUIs a measurement model of (a):
Wherein the method comprises the steps of For the original angular velocity measurement of the IMU at time T, ω x is the angular velocity of the X-axis, ω y is the angular velocity of the Y-axis, ω z is the angular velocity of the Z-axis, T is the transpose operation, ω t is the true angular velocity of the IMU at time T,For the measurement zero offset of the IMU at the time t, n ω is the angular velocity noise of the IMU, and obeys zero-mean Gaussian noise;
Establishing the rear axle wheel speed Is a measurement model of (a):
For the rear axle wheel speed measurement at the moment t, v t is the real rear axle wheel speed value at the moment t, n v is the rear axle wheel speed noise and obeys zero-mean Gaussian noise;
S2-2: by passing through Will be of component omega z To X, Y, Z axes, so that a velocity vector V t at time t can be obtained:
Wherein Δt is the time interval from t to t+1;
S2-3: using velocity vector V t and the angular velocity of the IMU Odometer integration was performed:
the rotation integration formula is as follows:
Where exp () is an exponential mapping, For the rotation matrix at time t +1,A rotation matrix at the time t;
The pose integral formula is as follows:
Wherein the method comprises the steps of The displacement at time t+1, and P t I is the displacement at time t;
s2-4: building odometer residuals between system states k to k+1 frames The following are provided:
The above formula has a data time stamp alignment, the moment of system state, i.e., frame rate, denoted by k is a low frequency time, and the frequency of the odometer data is very high denoted by t, thus involving alignment of high frequency odometer data to low frequency system state data, Representing the multiplication of the quaternion, () * represents the conjugation of the quaternion,AndThe displacement of the odometer integral at the time points k+1 and k is expressed (displacement is formulaCalculated),AndThe pose of the odometer integral model at each of the aligned k+1 and k times is expressed (the pose is a formula)Calculated), while in quaternion form, R LI represents the rotation matrix of the IMU coordinate system to the radar coordinate system, q LI is in quaternion form of R LI,Is the displacement in the system state quantity at time k,Is the displacement in the system state quantity at time k +1,Is a quaternion in the system state quantity at time k,The quaternion in the system state quantity at the time k+1, b ω k is the IMU zero offset in the system state quantity at the time k, and b ω k+1 is the IMU zero offset in the system state quantity at the time k+1.
As a preferred embodiment of the present invention, in the step (3), the vehicle kinematic residual construction specifically includes:
S3-1: the front wheel rotation angle delta and the rear axle wheel speed are respectively passed through a vehicle linear two-degree-of-freedom model to obtain a three-dimensional angular velocity vector omega t 2DOF and a three-dimensional velocity vector V t 2DOF:
l f is the distance from the front axle to the vehicle coordinate system, l r is the distance from the rear axle to the vehicle coordinate system, α is the centroid slip angle of the vehicle linear two-degree-of-freedom model, and the calculation formula of α is as follows:
s3-2: according to the angular velocity of the IMU The three-dimensional velocity vector V t 2DOF is used for constructing pose constraint of six degrees of freedom together, and a rotation constraint formula of the six degrees of freedom pose constraint is as follows:
wherein R VI is a rotation matrix that converts the angular velocity measured under the IMU coordinate system to the vehicle coordinate system; is a rotation matrix under a coordinate system of the vehicle at the moment t, A rotation matrix in a vehicle coordinate system at the time t+1;
The displacement constraint formula of six-free pose constraint is as follows:
Where P t V is the displacement in the vehicle coordinate system at time t, Is the displacement under the coordinate system of the vehicle at the time t+1;
s3-3: constructing yaw angle constraint according to the three-dimensional angular velocity vector omega t 2DOF, wherein a yaw angle constraint formula is as follows:
Wherein the method comprises the steps of For the yaw rotation matrix in the vehicle coordinate system at time t,A yaw angle rotation matrix under a vehicle coordinate system at the moment t;
s3-4: vehicle motion constraint residual The following are provided:
The above formula has a data time stamp alignment, the moment of the system state, i.e. the frame rate, denoted k is a low frequency time, whereas the frequency of the vehicle kinematic data is denoted t very high, thus involving alignment of high frequency vehicle kinematic data to low frequency system state data, Is the displacement in the system state quantity at time k,Is the displacement in the system state quantity at time k +1,Is a quaternion in the system state quantity at time k,Is a quaternion in the system state quantity at time k +1,AndRepresenting the displacement of the six-degree-of-freedom pose integral model at the alignment k+1 and k times, respectively (displacement isCalculated),AndRepresenting the pose of the six-degree-of-freedom pose integration model aligned with k+1 and k times, respectively (pose is a formula)Calculated),AndRepresenting the attitude of the yaw angle integral model at the time of alignment k+1 and k (the attitude is a formula)Calculated) () z denotes that the constraint is built only on the yaw angle in rotation, and K is an adaptive adjustment coefficient, and is related to the steering angle of the vehicle and the number of the extracted characteristic points of the point cloud, and the specific calculation is as follows:
Where δ max is the maximum value of the front wheel rotation angle, n e、np is the number of corner points and plane points extracted from the current point cloud, and n emax、npmax is the maximum value of the number of corner points and plane points, respectively.
As a preferred embodiment of the present invention, in the step (4), constructing a point cloud residual, specifically includes:
s4-1: the point-to-line distance residual e corner is formulated as follows:
Wherein phi represents the line characteristic point set in the point cloud of the current frame, R, T is the rotation matrix and translation vector of the point cloud coordinate system of the current frame relative to the point cloud coordinate system of the map, and corresponds to the system state quantity AndP k、ph is the two nearest points in the map point cloud consistent with p i in line characteristics, and p i is the line characteristic point in the current frame point cloud, which is not on the same laser horizontal scanning line;
S4-2: the point-to-face distance residual e planar is formulated as follows:
Where ζ represents the set of face feature points in the current frame point cloud; r, T is the rotation matrix and translation vector of the current frame point cloud coordinate system relative to the map point cloud coordinate system, corresponding to the system state quantity AndP n、pm、pg is the nearest three points in the map point cloud consistent with p j in the plane characteristics, so that the three points are not on the same laser horizontal scanning line, and p j is the plane characteristic point in the current frame point cloud;
s4-3: point cloud residual The point cloud residual equation includes a point-to-line distance residual e corner and a point-to-plane distance residual e planar, as follows:
As a preferred embodiment of the present invention, in the step (5), the cost function of the least squares problem is:
As a preferred embodiment of the present invention, in the step (5), the least square problem is solved based on a nonlinear optimization LM method.
Compared with the prior art, the invention has the beneficial effects that:
According to the invention, the IMU, the rear axle wheel speed and the front wheel steering angle are utilized to construct the vehicle kinematic constraint, and the displacement and posture constraint is respectively constructed through decoupling treatment of the displacement and posture information of the vehicle kinematic constraint, so that the accuracy of an optimization result is improved; and introducing an adaptive coefficient according to the number of the characteristic points and the steering angle, and adjusting the weight of the vehicle kinematic constraint in real time. An IMU and a rear axle wheel speed are adopted to construct an odometer, so that an accurate initial value is provided for tight coupling optimization, and the situation that the IMU and the rear axle wheel speed are trapped in local optimum is effectively avoided. A more accurate location and map may be provided than with a single lidar approach.
Drawings
FIG. 1 is a block diagram of a lidar/IMU/vehicle kinematic constraint close-coupled SLAM system of the present invention.
FIG. 2 is a linear two-degree-of-freedom model of a vehicle used in the present invention, where v t is the measured speed of the rear axle wheel speed meter, δ is the front wheel angle, yO 1 x is the vehicle coordinate system, l f the distance of the front axle from the vehicle coordinate system, l r is the distance of the rear axle from the vehicle coordinate system, O is the instantaneous speed instant, v x is the speed component of the x-axis, v y is the speed component of the y-axis, v t is the rear axle wheel speed, and α is the centroid slip angle.
Fig. 3 is a diagram showing the relationship between different raw data and different system modules in the present embodiment.
Fig. 4 is a schematic diagram of the experimental platform according to this embodiment.
Fig. 5 is a satellite cloud image of the experimental scenario of the present embodiment.
FIG. 6 is a positioning track diagram of different algorithms in the present embodiment, in which GNSS is the positioning track of the inertial navigation system, and the GNSS has high accuracy and is used as a real track; the A_LOAM is an A_LOAM algorithm; leG _LOAM is LeG0_LOAM laser radar positioning algorithm; embodiment 1 is a positioning track of embodiment 1 of the present invention.
Detailed Description
For a better description of the objects, technical solutions and advantages of the present invention, the present invention will be further described with reference to the following specific examples.
Example 1
The laser radar/IMU/vehicle kinematics constraint tight coupling SLAM algorithm comprises the following steps:
the laser radar/IMU/vehicle kinematics constraint close-coupled SLAM system is composed of four modules (see figure 1), namely a laser radar module, an odometer module, a vehicle kinematics module and a close-coupled module. The raw data processed by the lidar/IMU/vehicle kinematics constrained tight coupling SLAM algorithm are raw point cloud data of the lidar, raw angular velocity of the IMU, rear axle wheel speed and front wheel rotation angle of the ackerman steering vehicle, as shown in figure 2, The wheel speed of a rear axle in the ackerman steering is shown, and delta is the front wheel corner of the ackerman steering. The wheel speed of the rear axle and the rotation angle of the front wheel are not provided with specific sensors, and different ackerman steering vehicles acquire the wheel speed of the rear wheel and the rotation angle of the front wheel in different modes.
The laser radar/IMU/vehicle kinematics constrained close-coupled SLAM algorithm involves different sensors, and the original data output frequency and processing frequency of the different sensors are different. The original point cloud output frequency of the laser radar is 5Hz, and the processing evaluation rate is 5Hz. The IMU has an original angular velocity output frequency of 100Hz and a processing frequency of 100Hz. The frequency of the wheel speed of the rear axle and the rotation angle of the front wheel is 300Hz, and the processing frequency is 100Hz. The state quantity of the system isMainly pose data and angular velocity zero offset of IMU, whereinIs a three-dimensional translation vector that is,Is a quaternion of a four-dimensional vector, b ω is the zero offset of the angular velocity of the IMU, is a three-dimensional vector, and comprises the zero offset of the angular velocity of the XYZ axes. The state quantity of the system is a time variable, and is related to a specific time, and in detail, the subscript of the state quantity X k of the system indicates the time k.
The main purpose of the laser radar/IMU/vehicle kinematics constraint tight coupling SLAM algorithm is to obtain the positioning of an experimental platform and an environment point cloud map by processing each frame of data in real time, wherein the positioning data are system state quantities, then the corresponding original point cloud data of the laser radar are registered to the same coordinate system according to the system state quantities at each moment to obtain the environment point cloud map, the environment point cloud map is the original point cloud data corresponding to the frame are needed to be added in the environment point cloud map every time the system state quantity at each moment is solved along with the continuous increase of the system state quantities. The algorithm runs in real time, the data processing thought between any two frames is the same, each frame of data is processed to obtain an increment pose, then a global pose under a world coordinate system is obtained in a recursive mode, and after each frame is processed, the original point cloud number of the laser radar corresponding to the frame is registered in a coordinate system through obtaining the global pose.
Fig. 3 details the relationship between different raw data and different modules. As shown in fig. 3, there are mainly three frequencies, the processing frequency of the system state quantity and the laser radar module is 5Hz, the processing evaluation rate of the odometer module is 100Hz, the processing frequency of the vehicle kinematic module is 100Hz, wherein the 100Hz processing frequency of the vehicle kinematic module is different from the 100Hz processing frequency of the odometer, the original data of the odometer module is the angular velocity of the IMU, the output frequency of the odometer module is 100Hz, so that the odometer module processes each frame of IMU data in real time, the original data of the vehicle kinematic module is the IMU angular velocity, the rear axle wheel speed and the front wheel angle, the output frequency of the IMU angular velocity is 100Hz, the output frequency of the rear axle wheel speed and the front wheel angle is 300Hz, and two different evaluation rates exist, therefore, before the processing of the vehicle kinematic module, a linear interpolation method needs to be adopted first, and the rear axle wheel speed and the front wheel angle are aligned to be 100Hz based on the 100Hz of the IMU angular velocity. Because the frequencies of the four modules are different, data synchronization is needed when solving the system state quantity, 100Hz data of the odometer module and the vehicle kinematics module are aligned to be 5Hz by taking the 5Hz frequency of the laser radar as a reference, and then the system state quantity at different moments is solved by the frequency of 5 Hz.
Based on a laser radar/IMU/vehicle kinematics constraint tight coupling SLAM algorithm, calculating to obtain a system state quantity of a specific frame, wherein the specific steps are as follows:
(1) The system state quantity of a specific frame is positioned as follows:
wherein X k is a system state quantity at k times, the system state quantity at each time contains three data, Is a translation vector of the laser radar coordinate system under world coordinates,B ω represents zero offset of the IMU angular velocity for quaternion of the laser radar coordinate system in world coordinates.
(2) And solving the state quantity of the system of one frame, namely, a point cloud residual error, an odometer residual error and a vehicle kinematics residual error, initializing if the point cloud data of the first frame is the point cloud data of the first frame, directly setting the position of the state quantity of the system to 0 without constructing the residual error, and registering the original point cloud data into a global point cloud map.
And 2.1, constructing a point cloud residual error. The laser radar module takes a laser radar as a sensor and takes point cloud of the laser radar as data; firstly, preprocessing point cloud to remove invalid points; secondly, extracting characteristic points of the point cloud after the pretreatment is finished, and extracting angular points and plane points in the point cloud; and then, carrying out point cloud residual construction, wherein the point cloud residual construction is a point cloud matching algorithm, the original point cloud is a set of angular points and planes in a global point cloud map established in real time, and the target point cloud is a set of angular points and planes extracted from the original point cloud of the current frame.
And (3) constructing a point cloud residual error:
point cloud residual constraints The point cloud residual constraint formula comprises a point-to-line distance residual e corner and a point-to-plane distance residual e planar, and is as follows:
the point-to-line distance residual e corner is formulated as follows:
Wherein phi represents the line characteristic point set in the point cloud of the current frame, R, T is the rotation matrix and translation vector of the point cloud coordinate system of the current frame relative to the point cloud coordinate system of the map, and corresponds to the system state quantity AndP k、ph is the nearest two points in the map point cloud consistent with p i in line characteristics, and p i is the line characteristic point in the current frame point cloud, which is not on the same laser horizontal scanning line.
The point-to-face distance residual e corner is formulated as follows:
Wherein phi represents the line characteristic point set in the point cloud of the current frame, R, T is the rotation matrix and translation vector of the point cloud coordinate system of the current frame relative to the point cloud coordinate system of the map, and corresponds to the system state quantity AndP k、ph is the nearest two points in the map point cloud consistent with p i in line characteristics, and p i is the line characteristic point in the current frame point cloud, which is not on the same laser horizontal scanning line.
And finally, taking the constructed point cloud residual as one data input of the tight coupling optimization module.
2.2 The odometer module takes the angular velocity of the IMU and the rear axle wheel speed of the vehicle as data sources. Firstly, pre-integrating to obtain the pose under an IMU coordinate system; the pre-integration steps are as follows:
At the angular velocity of the IMU And rear axle wheel speed of vehicleAs the original data, the noise of the original data is considered at the same time, and the establishmentIs a measurement model of (a):
Wherein the method comprises the steps of For the raw angular velocity measurement of the IMU at time t, omega t is the true angular velocity of the IMU at time t,For the measurement zero offset of the IMU at the time t, n ω is the angular velocity noise of the IMU, and obeys zero-mean Gaussian noise; establishment ofIs a measurement model of (a):
For the rear axle wheel speed measurement at time t, v t is the real rear axle wheel speed value at time t, b v is the measured zero offset of the IMU at time t, n ω is the angular velocity noise of the IMU, and the zero-mean Gaussian noise is obeyed.
Through the pass throughWill be of component omega z To X, Y, Z axes, thereby obtaining a velocity vector V t:
Where Δt is the time interval from t to t+1 and V t is the velocity vector at t.
Using velocity vector V t and the angular velocity of the IMUAnd (3) performing odometer integration, wherein an attitude integration formula is as follows:
Where exp () is an exponential mapping, For the rotation matrix at time t +1,Is the rotation matrix at time t.
The position integral formula is as follows:
Wherein the method comprises the steps of The displacement at time t+1 is referred to as P t I.
And then, according to the attitude integral and the position integral obtained by pre-integration, an odometer residual is built, wherein the built residual is the odometer residual between k and k+1 frames, and a specific residual formula is as follows:
Wherein the method comprises the steps of Representing the multiplication of the quaternion, () * represents the conjugation of the quaternion,AndThe displacement of the odometer integral at the time points k +1 and k are shown respectively,AndRespectively representing the attitudes of the odometer integral models at the time points of k+1 and k, simultaneously representing a quaternion form, R LI represents a rotation matrix from an IMU coordinate system to a radar coordinate system, q LI represents a quaternion form of R LI,Is the displacement in the system state quantity at time k,Is the displacement in the system state quantity at time k +1,Is a quaternion in the system state quantity at time k,The quaternion in the system state quantity at the time k+1, b ω k is the IMU zero offset in the system state quantity at the time k, and b ω k+1 is the IMU zero offset in the system state quantity at the time k+1.
And finally, taking the constructed odometer residual error as a data input of a close-coupled optimization module.
2.3, The vehicle kinematics module takes the angular speed of the IMU and the rear axle wheel speed of the vehicle as data sources;
Firstly, integrating to obtain the pose under the IMU coordinates, wherein the specific pre-integration is as follows: a linear two-degree-of-freedom model of the vehicle as shown in fig. 2 is adopted; the front wheel rotation angle and the rear axle wheel speed are respectively processed by a vehicle linear two-degree-of-freedom model to obtain a three-dimensional angular velocity vector omega t 2DOF and a three-dimensional velocity vector V t 2DOF, and the three-dimensional angular velocity vector omega t 2DOF has the following formula:
Where l f is the distance of the front axle to the vehicle coordinate system and l r is the distance of the rear axle to the vehicle coordinate system; delta is the front wheel corner.
The three-dimensional velocity vector V t 2DOF is formulated as follows:
where α is the centroid slip angle of the vehicle linear two-degree-of-freedom model.
According to the angular velocity of the IMUThe pose constraint of six degrees of freedom is constructed together with a three-dimensional speed vector V t 2DOF of a vehicle linear two-degree-of-freedom model; the rotation constraint formula of six-free pose constraint is as follows:
wherein R VI is a rotation matrix that converts the angular velocity measured under the IMU coordinate system to the vehicle coordinate system; is a rotation matrix under a coordinate system of the vehicle at the moment t, Is a rotation matrix in a vehicle coordinate system at time t+1.
The displacement constraint formula of six-free pose constraint is as follows:
Where P t V is the displacement in the vehicle coordinate system at time t, Is the displacement in the vehicle coordinate system at time t+1.
Constructing yaw angle constraint according to the angular speed of the linear two-degree-of-freedom model of the vehicle, wherein a yaw angle constraint formula is as follows:
Wherein the method comprises the steps of For the yaw rotation matrix in the vehicle coordinate system at time t,And the yaw angle rotation matrix is a yaw angle rotation matrix under a vehicle coordinate system at the moment t.
And then constructing a vehicle kinematics residual according to the integral result, wherein a specific residual formula is as follows:
Wherein the method comprises the steps of Is the displacement in the system state quantity at time k,Is the displacement in the system state quantity at time k +1,Is a quaternion in the system state quantity at time k,Is a quaternion in the system state quantity at time k +1,AndRespectively representing the displacement of the six-degree-of-freedom pose integral model aligned with the k+1 moment and the k moment,AndRespectively representing the postures of the six-degree-of-freedom pose integral models aligned with the k+1 moment and the k moment,AndRepresenting the attitude of the yaw integral model aligning the k+1 and K moments, () z represents that the constraint is built only on the yaw in rotation, and K is an adaptive adjustment coefficient, which is related to the steering angle of the vehicle and the number of extracted feature points of the point cloud, and is specifically calculated as follows:
Where δ is the front wheel rotation angle, δ max is the maximum value of the front wheel rotation angle, n e、np is the number of corner points and plane points extracted from the current point cloud, and n emax、npmax is the maximum value of the number of corner points and plane points, respectively.
(3) The tight coupling optimization module takes a point cloud residual error, an odometer residual error and a vehicle kinematics residual error as input data, and the three residual errors form a least square problem, and a cost function of the least square problem:
Taking the pre-integral of the odometer module as a solving initial value of the least square problem, and then adopting nonlinear optimization to carry out iterative solving, wherein the solving of the least square is the state quantity of the solving system.
The effect of the laser radar/IMU/vehicle kinematics constrained tight coupling SLAM system and algorithm of the invention is verified by adopting the experimental platform and experimental scene shown in FIG. 4. The platform is an autonomous automatic driving vehicle, the chassis simulates a real vehicle, an ackerman steering design is adopted, and a front wheel steering angle sensor and a rear axle wheel speed sensor are provided. The platform is provided with a binocular camera, a monocular camera, a 16-line laser radar, an inertial navigation system, an inertial measurement system IMU and an industrial personal computer, and all sensor data are accessed to the industrial personal computer and can be acquired through a topic communication mode of ROS. The experimental scene is shown in fig. 5, the full length is about 946 meters, the line in the figure represents the motion route of the data collected by the experimental platform, and the arrow represents the motion direction.
By comparing the A_LOAM algorithm, the LeG0_LOAM laser radar positioning algorithm and the laser radar/IMU/vehicle kinematic constraint tight coupling SLAM algorithm: during the experiment, the experimental platform is controlled to collect a circle of data around an experimental scene, and then an A_LOAM algorithm, a LeG0_LOAM laser radar positioning algorithm and the method are adopted for processing respectively. In order to effectively evaluate the effect of each method, an inertial navigation system is used to provide a true trajectory. The inertial navigation system adopts high-precision positioning service provided by thousands of seeking companies, and can realize real-time positioning with horizontal precision of 2cm and elevation precision of 5 cm. Fig. 6 is a positioning track diagram obtained by different methods, and it can be seen that, compared with the a_loam algorithm and the LeG0_loam laser radar positioning algorithm, the positioning track of the invention is closer to the real track, and the positioning accuracy is higher. Meanwhile, the laser radar/IMU/vehicle kinematics constraint tight coupling SLAM algorithm has higher application value, can improve the positioning of higher precision for an automatic driving vehicle, and promotes the development of automatic driving.
Finally, it should be noted that the above embodiments are only for illustrating the technical solution of the present invention and not for limiting the scope of the present invention, and although the present invention has been described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that the technical solution of the present invention may be modified or substituted equally without departing from the spirit and scope of the technical solution of the present invention.
Claims (5)
1. A lidar/IMU/vehicle kinematics constrained tightly coupled SLAM algorithm comprising the steps of:
(1) Constructing a laser radar/IMU/vehicle kinematics constraint tight coupling SLAM system; the laser radar/IMU/vehicle kinematics constraint tight coupling SLAM system comprises an odometer module, a vehicle kinematics module, a laser radar module and a tight coupling optimization module;
The laser radar module takes a laser radar as a sensor, takes point clouds of the laser radar as data, takes corner points and plane points in radar point clouds of a current frame and corner points and plane points stored in a global point cloud map established before the current frame as input data, adopts a point-to-line and point-to-plane mode to construct point cloud residual errors, and takes the point cloud residual errors as one data input of the close-coupled optimization module;
the odometer module takes the angular speed of the IMU and the rear axle wheel speed of the vehicle as data sources, builds an odometer residual error, and takes the odometer residual error as one data input of the close-coupling optimization module;
The vehicle kinematics module takes the angular speed of the IMU, the rear axle wheel speed of the vehicle and the front wheel rotation angle of the vehicle as data sources, constructs vehicle kinematics residual error, and takes the vehicle kinematics residual error as data input of the tight coupling optimization module;
The tight coupling optimization module takes a point cloud residual error, an odometer residual error and a vehicle kinematics residual error as input data, converts the three residual errors into least square, takes the odometer residual error as an initial value of tight coupling optimization, and adopts a nonlinear optimization method to solve the least square; registering the point cloud of the current frame to a global point cloud map by adopting the optimized system state quantity;
(2) Pre-integrating the angular speed of the IMU and the speed polarity of the rear axle wheel speedometer to obtain the pose of the IMU in the coordinate system, and then constructing an odometer residual error;
The method specifically comprises the following steps:
S2-1: at the angular velocity of the IMU And rear axle wheel speed of vehicleAs the original data, simultaneously taking the noise existing in the original data into consideration, and establishing the angular velocity of the IMUIs a measurement model of (a):
Wherein the method comprises the steps of For the original angular velocity measurement of the IMU at time T, ω x is the angular velocity of the X-axis, ω y is the angular velocity of the Y-axis, ω z is the angular velocity of the Z-axis, T is the transpose operation in the matrix, ω t is the true angular velocity of the IMU at time T,For the measurement zero offset of the IMU at the time t, n ω is the angular velocity noise of the IMU, and obeys zero-mean Gaussian noise;
Establishing the rear axle wheel speed Is a measurement model of (a):
For the rear axle wheel speed measurement at the moment t, v t is the real rear axle wheel speed value at the moment t, n v is the rear axle wheel speed noise and obeys zero-mean Gaussian noise;
S2-2: by passing through Will be of component omega z To X, Y, Z axes, so that a velocity vector V t at time t can be obtained:
Wherein Δt is the time interval from t to t+1;
S2-3: using velocity vector V t and the angular velocity of the IMU Odometer integration was performed:
the rotation integration formula is as follows:
Where exp () is an exponential mapping, For the rotation matrix at time t +1,A rotation matrix at the time t;
The pose integral formula is as follows:
Wherein the method comprises the steps of The displacement at time t+1, and P t I is the displacement at time t;
s2-4: building odometer residuals between system states k to k+1 frames The following are provided:
Where the odometer residuals have a data timestamp alignment, the moment in system state, denoted k, i.e. the frame rate, Representing the multiplication of the quaternion, () * represents the conjugation of the quaternion,AndThe displacement of the odometer integral at the time points k +1 and k are shown respectively,AndRespectively representing the attitudes of the odometer integral models at the time points of k+1 and k, simultaneously representing a quaternion form, R LI represents a rotation matrix from an IMU coordinate system to a radar coordinate system, q LI represents a quaternion form of R LI,Is the displacement in the system state quantity at time k,Is the displacement in the system state quantity at time k +1,Is a quaternion in the system state quantity at time k,B ω k is the quaternion in the system state quantity at the time k+1, b ω k is the IMU zero offset in the system state quantity at the time k, and b ω k+1 is the IMU zero offset in the system state quantity at the time k+1;
(3) Firstly, calculating a speed vector and an angular speed vector under a vehicle coordinate system; secondly, pre-integrating the speed vector and the angular speed to obtain a pose with six degrees of freedom, and pre-integrating the angular speed vector to obtain a pose with three degrees of freedom; then constructing a vehicle kinematics residual error;
(4) Constructing point cloud residual constraints by using preprocessing and a characteristic method;
(5) And constructing a close coupling optimization problem by combining the odometer residual error, the vehicle kinematic residual error and the point cloud residual error, converting the close coupling optimization problem into a least square problem, taking the odometer residual error as an initial value of the close coupling optimization, and solving the least square problem by adopting a nonlinear optimization method.
2. The lidar/IMU/vehicle kinematics constrained tightly coupled SLAM algorithm of claim 1, wherein in step (3), the vehicle kinematics residual construction specifically comprises:
S3-1: the front wheel rotation angle delta and the rear axle wheel speed are respectively passed through a vehicle linear two-degree-of-freedom model to obtain a three-dimensional angular velocity vector omega t 2DOF and a three-dimensional velocity vector V t 2DOF:
l f is the distance from the front axis to the vehicle coordinate system, l r is the distance from the rear axis to the vehicle coordinate system, alpha is the centroid slip angle of the vehicle linear two-degree-of-freedom model,
S3-2: according to the angular velocity of the IMUThe three-dimensional velocity vector V t 2DOF is used for constructing pose constraint of six degrees of freedom together, and a rotation constraint formula of the six degrees of freedom pose constraint is as follows:
wherein R VI is a rotation matrix that converts the angular velocity measured under the IMU coordinate system to the vehicle coordinate system; is a rotation matrix under a coordinate system of the vehicle at the moment t, A rotation matrix in a vehicle coordinate system at the time t+1;
The displacement constraint formula of six-free pose constraint is as follows:
Where P t V is the displacement in the vehicle coordinate system at time t, Is the displacement under the coordinate system of the vehicle at the time t+1;
s3-3: constructing yaw angle constraint according to the three-dimensional angular velocity vector omega t 2DOF, wherein a yaw angle constraint formula is as follows:
Wherein the method comprises the steps of For the yaw rotation matrix in the vehicle coordinate system at time t,A yaw angle rotation matrix under a vehicle coordinate system at the moment t;
s3-4: vehicle motion constraint residual The following are provided:
where the vehicle motion constraint residual equation has a data timestamp alignment, the time of the system state, denoted by k, i.e. the frame rate, Is the displacement in the system state quantity at time k,Is the displacement in the system state quantity at time k +1,Is a quaternion in the system state quantity at time k,Is a quaternion in the system state quantity at time k +1,AndRespectively representing the displacement of the six-degree-of-freedom pose integral model aligned with the k+1 moment and the k moment,AndRespectively representing the postures of the six-degree-of-freedom pose integral models aligned with the k+1 moment and the k moment,AndRepresenting the attitude of the yaw integral model aligning the k+1 and K moments, () z represents that the constraint is built only on the yaw in rotation, and K is an adaptive adjustment coefficient, which is related to the steering angle of the vehicle and the number of extracted feature points of the point cloud, and is specifically calculated as follows:
Where δ max is the maximum value of the front wheel rotation angle, n e、np is the number of corner points and plane points extracted from the current point cloud, and n emax、npmax is the maximum value of the number of corner points and plane points, respectively.
3. The lidar/IMU/vehicle kinematics constrained close-coupled SLAM algorithm of claim 1, wherein in step (4), constructing a point cloud residual specifically comprises:
s4-1: the point-to-line distance residual e corner is formulated as follows:
Wherein phi represents the line characteristic point set in the point cloud of the current frame, R, T is the rotation matrix and translation vector of the point cloud coordinate system of the current frame relative to the point cloud coordinate system of the map, and corresponds to the system state quantity AndP k、ph is the two nearest points in the map point cloud consistent with p i in line characteristics, and p i is the line characteristic point in the current frame point cloud, which is not on the same laser horizontal scanning line;
S4-2: the point-to-face distance residual e planar is formulated as follows:
Where ζ represents the set of face feature points in the current frame point cloud; r, T is the rotation matrix and translation vector of the current frame point cloud coordinate system relative to the map point cloud coordinate system, corresponding to the system state quantity AndP n、pm、pg is the nearest three points in the map point cloud consistent with p j in the plane characteristics, so that the three points are not on the same laser horizontal scanning line, and p j is the plane characteristic point in the current frame point cloud;
s4-3: point cloud residual Including a point-to-line distance residual e corner and a point-to-plane distance residual e planar, a point cloud residualThe formula is as follows:
4. The lidar/IMU/vehicle kinematics constrained tightly coupled SLAM algorithm of claim 1, wherein in step (5), the cost function of the least squares problem is:
5. the lidar/IMU/vehicle kinematics constrained tightly coupled SLAM algorithm of claim 1, wherein in step (5), a nonlinear optimization LM method is used to solve the least squares problem.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311571021.4A CN117584989B (en) | 2023-11-23 | 2023-11-23 | Laser radar/IMU/vehicle kinematics constraint tight coupling SLAM system and algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311571021.4A CN117584989B (en) | 2023-11-23 | 2023-11-23 | Laser radar/IMU/vehicle kinematics constraint tight coupling SLAM system and algorithm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117584989A CN117584989A (en) | 2024-02-23 |
| CN117584989B true CN117584989B (en) | 2024-07-19 |
Family
ID=89921358
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311571021.4A Active CN117584989B (en) | 2023-11-23 | 2023-11-23 | Laser radar/IMU/vehicle kinematics constraint tight coupling SLAM system and algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117584989B (en) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111707272A (en) * | 2020-06-28 | 2020-09-25 | 湖南大学 | Underground garage automatic driving laser positioning system |
| CN111739063A (en) * | 2020-06-23 | 2020-10-02 | 郑州大学 | Electric power inspection robot positioning method based on multi-sensor fusion |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111272165B (en) * | 2020-02-27 | 2020-10-30 | 清华大学 | Intelligent vehicle positioning method based on characteristic point calibration |
| US11254323B2 (en) * | 2020-03-04 | 2022-02-22 | Zoox, Inc. | Localization error monitoring |
| WO2021218620A1 (en) * | 2020-04-30 | 2021-11-04 | 上海商汤临港智能科技有限公司 | Map building method and apparatus, device and storage medium |
| CN112083726B (en) * | 2020-09-04 | 2021-11-23 | 湖南大学 | Park-oriented automatic driving double-filter fusion positioning system |
| CN114966734B (en) * | 2022-04-28 | 2024-11-08 | 华中科技大学 | Bidirectional depth vision inertial pose estimation method combined with multi-line laser radar |
-
2023
- 2023-11-23 CN CN202311571021.4A patent/CN117584989B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111739063A (en) * | 2020-06-23 | 2020-10-02 | 郑州大学 | Electric power inspection robot positioning method based on multi-sensor fusion |
| CN111707272A (en) * | 2020-06-28 | 2020-09-25 | 湖南大学 | Underground garage automatic driving laser positioning system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117584989A (en) | 2024-02-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113945206B (en) | Positioning method and device based on multi-sensor fusion | |
| CN112347840B (en) | Vision sensor laser radar integrated unmanned aerial vehicle positioning and image building device and method | |
| CN109991636B (en) | Map construction method and system based on GPS, IMU and binocular vision | |
| CN112083725B (en) | Structure-shared multi-sensor fusion positioning system for automatic driving vehicle | |
| CN111156998B (en) | Mobile robot positioning method based on RGB-D camera and IMU information fusion | |
| CN113819914A (en) | Map construction method and device | |
| CN106030430A (en) | Multi-sensor fusion for robust autonomous filght in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (MAV) | |
| CN113819905B (en) | Mileage metering method and device based on multi-sensor fusion | |
| Kang et al. | Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator | |
| CN115574816B (en) | Bionic vision multi-source information intelligent perception unmanned platform | |
| CN114111818B (en) | Universal vision SLAM method | |
| CN115272596A (en) | Multi-sensor fusion SLAM method oriented to monotonous texture-free large scene | |
| CN115218889A (en) | Multi-sensor indoor positioning method based on dotted line feature fusion | |
| CN114964276A (en) | Dynamic vision SLAM method fusing inertial navigation | |
| Chen et al. | 3D LiDAR-GPS/IMU calibration based on hand-eye calibration model for unmanned vehicle | |
| Karam et al. | Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping | |
| CN116429116A (en) | Robot positioning method and equipment | |
| CN117584989B (en) | Laser radar/IMU/vehicle kinematics constraint tight coupling SLAM system and algorithm | |
| CN115046543A (en) | Multi-sensor-based integrated navigation method and system | |
| CN110887487B (en) | Indoor synchronous positioning and mapping method | |
| Lei et al. | Visual localization strategy for indoor mobile robots in the complex environment | |
| CN112945233A (en) | Global drift-free autonomous robot simultaneous positioning and map building method | |
| CN115344033B (en) | Unmanned ship navigation and positioning method based on monocular camera/IMU/DVL tight coupling | |
| CN115077517A (en) | Low-speed unmanned vehicle positioning method and system based on fusion of stereo camera and IMU | |
| Xie et al. | Automatic docking optimization method of bed and chair based on multi-sensor information fusion |
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 |