CN116338719A - Laser radar-inertia-vehicle fusion positioning method based on B spline function - Google Patents
Laser radar-inertia-vehicle fusion positioning method based on B spline function Download PDFInfo
- Publication number
- CN116338719A CN116338719A CN202310183664.5A CN202310183664A CN116338719A CN 116338719 A CN116338719 A CN 116338719A CN 202310183664 A CN202310183664 A CN 202310183664A CN 116338719 A CN116338719 A CN 116338719A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- imu
- spline function
- laser radar
- spline
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 40
- 230000004927 fusion Effects 0.000 title claims abstract description 22
- 238000005259 measurement Methods 0.000 claims abstract description 50
- 230000001133 acceleration Effects 0.000 claims abstract description 38
- 238000012937 correction Methods 0.000 claims abstract description 32
- 230000010354 integration Effects 0.000 claims abstract description 21
- 238000012545 processing Methods 0.000 claims abstract description 14
- 238000005070 sampling Methods 0.000 claims description 18
- 238000005457 optimization Methods 0.000 claims description 17
- 238000009795 derivation Methods 0.000 claims description 13
- 239000013598 vector Substances 0.000 claims description 13
- 239000011159 matrix material Substances 0.000 claims description 12
- 230000008859 change Effects 0.000 claims description 7
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 238000004519 manufacturing process Methods 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 2
- 230000017105 transposition Effects 0.000 claims description 2
- 230000008569 process Effects 0.000 description 5
- 230000015556 catabolic process Effects 0.000 description 4
- 238000006731 degradation reaction Methods 0.000 description 4
- 230000004807 localization Effects 0.000 description 3
- 230000003321 amplification Effects 0.000 description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000003199 nucleic acid amplification method Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
-
- 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
- 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
- G01S7/4972—Alignment of sensor
-
- 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)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
Abstract
The invention discloses a laser radar-inertia-vehicle fusion positioning method based on a B spline function. The method comprises the following steps: modeling the motion trail of the vehicle by using a B spline function to obtain an IMU motion trail; processing readings of a wheel speed meter and a rotation angle sensor of the vehicle through a dynamics model to obtain vehicle dynamics measurement; performing discrete integration on IMU data and vehicle dynamics measurement between adjacent control points of the B spline function, and initializing newly added spline control points; adding a lateral acceleration constraint of the vehicle to the vehicle trajectory estimate; and optimizing the estimated vehicle track by utilizing the point cloud data, the IMU data and the vehicle dynamics measurement data, and estimating correction factors of vehicle dynamics model parameters on line. According to the invention, the multi-sensor fusion positioning is performed based on the B spline function, so that the robustness and the accuracy of unmanned vehicle positioning are improved, and the influence of factors such as vehicle load, driving road surface and the like on vehicle dynamics measurement values is reduced.
Description
Technical Field
The invention relates to the field of robot navigation and positioning, in particular to a laser radar-inertia-vehicle fusion positioning method based on a B spline function.
Background
The simultaneous localization and mapping (SLAM, simultaneous Localization and Mapping) technique is a key technique in mobile robots, which can estimate the current pose information of the carrier using the information of the local measurement sensors mounted without depending on external signals, and simultaneously construct a map model of the perimeter environment. Currently, vision and lidar are two kinds of external sensors mainly used in SLAM technology, and compared with vision sensors, lidar is widely used in practical applications because it can obtain high-precision distance information and is not affected by environmental light and texture. However, lidar relies strongly on structural feature information of the environment, resulting in degradation in tunnels, open land and the like.
To overcome the above-mentioned drawbacks of lidar, some studies have fused other types of sensors, such as IMUs, wheel speed meters, etc., to the lidar for positioning. When the existing method utilizes information such as an IMU, a wheel speed meter, a rotation angle sensor and the like, the method of obtaining relative change between frames by integration is adopted for fusion, original acquired data is not used, noise of the original data is easily amplified in the integration process, and therefore fusion positioning accuracy is affected. In addition, the phenomenon that the instantaneous speed direction deviates from the direction of the head of the vehicle due to the lateral force between the tire and the ground during the running of the vehicle, and the phenomenon commonly occurring during the running of the vehicle cannot be accurately described by the kinematic model and the wheel speed meter. And the parameters of the vehicle model deviate from the preset calibration values due to factors such as load, ground conditions and the like in the running process of the vehicle, so that measurement errors are introduced.
Therefore, in order to reduce the influence of the phenomenon on fusion positioning, the invention provides a laser radar-inertia-vehicle fusion positioning method based on a B spline function, and the transverse acceleration constraint of the vehicle is introduced into the positioning problem for the first time, so that the method is used for describing the motion characteristics of the vehicle and providing priori information for positioning, and the accuracy and the robustness of autonomous positioning of the vehicle in a complex environment are improved.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides a laser radar-inertia-vehicle fusion positioning method based on a B spline function, which utilizes a vehicle dynamics model to process wheel speed meter and corner sensor data, effectively considers the cornering phenomenon of a vehicle, can directly fuse the IMU and the original data of vehicle dynamics measurement, avoids noise amplification caused by integration, corrects the drift of vehicle model parameters in the running process on line, introduces the lateral acceleration constraint of the vehicle, and ensures that the vehicle realizes robust high-precision autonomous positioning in complex and various running environments.
The aim of the invention is realized by the following technical scheme: a laser radar-inertia-vehicle fusion positioning method based on a B spline function comprises the following steps:
s1: collecting laser radar real-time scanning point cloud, IMU data, wheel speed meter readings and corner sensor readings; vehicle trajectory estimation is performed: modeling the position p and the gesture R of the vehicle-mounted IMU by using two B spline functions respectively to obtain position, gesture and speed data of the vehicle-mounted IMU at any time, and further obtain an IMU motion track;
s2: processing the wheel speed meter readings and the corner sensor readings in the step S1 by using a vehicle dynamics model to obtain vehicle dynamics measurement, optimizing correction factor vectors on line, and correcting parameters of the vehicle dynamics model;
s3: in the time period from the last control point before track updating to the newly added control point, the IMU data in S1 is subjected to discrete integration processing to obtain a pose set corresponding to each IMU sampling moment in the time period
Speed->
A collection;
discrete integration processing is carried out on the vehicle dynamics measurement in the S2, so that pose sets corresponding to each moment of obtaining the vehicle dynamics measurement in the time period are obtained
Taking all pose sets and velocity sets at discrete moments as measured values, constraining the poses and the velocities obtained by sampling the B spline function at the discrete moments, and constructing a first optimization problem for initializing a control point newly added into the B spline;
s4: adding vehicle lateral acceleration constraint in vehicle track estimation, and calculating a lateral acceleration constraint residual error;
s5: matching each frame of laser radar point cloud obtained in the step S1 with line features and surface features extracted from an environment map, and calculating laser radar point cloud residual errors; according to the IMU data in the S1, calculating an IMU residual error; calculating a vehicle dynamics measurement residual according to the vehicle dynamics measurement in S2;
s6: and constructing a second optimization problem through the point cloud residual, the IMU residual, the vehicle dynamics measurement residual and the lateral acceleration constraint residual, taking the control point newly added with the B spline after the initialization of the S3 as an initial value of the optimization problem, and simultaneously optimizing the spline function control point in the S1 and the correction factor vector in the S2 to obtain a vehicle real-time pose track and a vehicle model parameter correction value for positioning.
Further, in the step S1, two B-spline functions are used to model the position p and the pose R of the vehicle-mounted IMU, specifically:
R(t)＝λ(t)d θ
p(t)＝λ(t)d t
wherein R (t) is a B-spline function representing the gesture, and p (t) is a B-spline function representing the position; lambda (t) is spline basis function, d θ D is spline parameter related to control point of the gesture B spline function t Spline parameters related to the control points of the position B spline function; the angular velocity and the angular acceleration are obtained by first-order and second-order derivation of the B-spline function representing the gesture:
the linear velocity and the linear acceleration are obtained by conducting first-order and second-order derivation on the B-spline function representing the position:
the above describes the sampled values of the B-spline function of attitude, angular velocity, angular acceleration, position, linear velocity, linear acceleration at time t.
Further, the S2 specifically is: obtaining the slip angle alpha and the yaw angle change rate of the vehicle from the wheel speed meter readings and the rotation angle sensor readings by using a dynamics model of the vehicle
The linear speed and the angular speed taking the mass center of the vehicle as the origin of a coordinate system are obtained through the sliding angle and the change rate of the yaw angle:
adding a vehicle dynamics model parameter correction factor vector [ c ] to be optimized v c α c ω ]Then there is a vehicle dynamics measure of:
wherein C is r And C f Lateral deflection rigidity of front wheel and rear wheel of vehicle respectively r And l f The distances from the center of the rear axle and the center of the front axle to the center of mass of the vehicle, l=l r +l f The method comprises the steps of carrying out a first treatment on the surface of the m is the mass of the vehicle;
the method comprises the steps that (1) a wheel speed meter of a vehicle is read, and delta is the read of a vehicle rotation angle sensor; />
For the linear speed part of the vehicle dynamics measurement, +.>
An angular velocity component measured for vehicle dynamics; c v C is a correction factor for the magnitude of the linear velocity α C is a correction factor for the slip angle of the vehicle ω Is a correction factor for the magnitude of the angular velocity of the vehicle.
Further, the step S3 specifically includes: discrete integration processing is carried out on IMU data to obtain pose sets corresponding to each IMU sampling moment in the time period
Speed set +.>
Performing discrete integration processing on the vehicle dynamics measurement to obtain a pose set corresponding to each vehicle dynamics measurement obtaining moment in the time period>
The pose and the speed at discrete time obtained by recursion are used as measured values, and a first optimization problem objective function is constructed as follows:
wherein phi is new For a newly added control point,
pose obtained by discrete integration of IMU, < ->
The velocity obtained for the IMU discrete integration; />
Measuring discrete integral for vehicle dynamics to obtain pose; />
For the acquisition time of IMU data, +.>
The time of acquisition of the vehicle dynamics measurement; />
The upper corner mark T represents transposition for external parameters from the vehicle-mounted IMU coordinate system to the vehicle mass center coordinate system.
Further, in the step S4, a vehicle lateral acceleration constraint is added to the vehicle track estimation, and the calculation of the vehicle lateral acceleration constraint residual is specifically:
wherein u= [0 1 0]Is a selection matrix, a V (t s ) The IMU coordinate system is converted into the linear acceleration of the vehicle mass center coordinate system after the second-order derivation is carried out on the IMU motion track,
for the three-axis angular velocity reading of the IMU gyroscope, v V (t s ) Line converted from IMU coordinate system to vehicle mass center coordinate system after first-order derivation of IMU motion trackSpeed [ (] ∧ For the operation of vector conversion into its corresponding anti-symmetric matrix, [ ·] z To take the operation of the corresponding shaft angle conversion rate of the z-axis; t is t s The set track sampling time is consistent with the acquisition time of the IMU.
Further, the vehicle dynamics measurement residual in S5 is specifically: the method comprises the steps of constructing a laser radar point cloud residual, directly constructing an IMU residual by utilizing original IMU triaxial acceleration and triaxial angular velocity measurement and first and second derivatives of an IMU motion track, and directly constructing a vehicle dynamics residual by utilizing vehicle dynamics measurement and the first derivatives of the estimated track, wherein the formula is as follows:
wherein e L 、e I 、e V The laser radar point cloud residual error, the IMU residual error and the vehicle dynamics residual error are respectively;
for converting from IMU track in world coordinate system to laser radar track in world coordinate system by external reference +.>
For the sampling time of the kth sampling point of the frame laser radar, pi is a projection equation from a plane characteristic point to a plane and from an edge characteristic point to a straight line; />
Is IMU track under world coordinate system, t I For the acquisition time of IMU data omega B (t I ) Obtaining the angular velocity under the IMU coordinate system after deriving the IMU track in first order, < +.>
For three-axis angular velocity measurement of IMU, b a And b g Respectively the bias of the IMU accelerometer and the gyroscope; v v (t V ),ω V (t V ) Respectively obtaining linear velocity and angular velocity, t under a vehicle mass center coordinate system obtained through external parameters after first-order derivation of the track V Time of acquisition for vehicle dynamics measurement, c v For measuring the correction factor of the linear velocity magnitude with respect to the dynamics of the vehicle c α For measuring correction factors of slip angle with respect to vehicle dynamics c ω The correction factor for the magnitude of the angular velocity is measured with respect to the dynamics of the vehicle.
Further, the objective function of the second optimization problem in S6 is:
wherein the optimization variable is a correction factor vector c= [ c ] v c α c ω ]Active control point set Φ= { Φ of track i ,…,φ i+k-1 K is the order of the B-spline function; e, e L 、e I 、e V 、e M The laser radar point cloud residual error, the IMU residual error, the vehicle dynamics residual error and the vehicle transverse acceleration constraint residual error are respectively obtained; sigma and method for producing the same L 、Σ I 、Σ V Respectively a laser radar point cloud residual error, an IMU residual error and a weight matrix of vehicle dynamics residual error,
the sigma-delta norm corresponding to the sigma weight matrix.
Compared with the prior art, the invention has the beneficial effects that: according to the invention, the data of the wheel speed meter and the rotation angle sensor are processed by adopting a vehicle dynamics model, the cornering phenomenon of the vehicle is considered, and more accurate dynamics measurement values are obtained; modeling the track by using a B spline function, and directly fusing the IMU and the original data of vehicle dynamics measurement by using the property that the track can be sampled in continuous time and is smooth and conductive, so that noise amplification caused by integration is avoided to influence positioning accuracy; the drift of the vehicle model parameters in the running process is corrected on line, so that the influence of the changes of factors such as vehicle load, running road surface and the like on the vehicle dynamics measured value is reduced; and the lateral acceleration constraint of the vehicle is introduced, priori information is provided for track estimation of the vehicle, and the robustness and accuracy of autonomous positioning of the vehicle are effectively improved.
Drawings
FIG. 1 is an overall flow chart of the B-spline function-based lidar-inertial-vehicle fusion positioning method of the present invention.
Detailed Description
The embodiment of the invention provides a laser radar-inertia-vehicle fusion positioning method based on a B spline function, which is used for robust high-precision positioning of a vehicle in a complex scene.
In order to make the technical solution of the present invention better understood by those skilled in the art, the following description of the technical solution of the present invention will be made clearly and completely by referring to the accompanying drawings of the embodiments of the present invention. It will be apparent that the described embodiments are merely some, but not all embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, shall fall within the scope of the invention.
The invention provides a laser radar-inertia-vehicle fusion positioning method based on a B spline function, which is further described in detail below with reference to the embodiment in order to make the purposes, technical schemes and advantages of the invention more clear. FIG. 1 is a flow chart of a method for laser radar-inertial-vehicle fusion localization based on B-spline functions, the method comprising.
S1: the system comprises a vehicle, wherein a laser radar, an IMU, a wheel speed meter and a corner sensor are arranged on the vehicle, and the laser radar real-time scanning point cloud, IMU data, wheel speed meter readings and corner sensor readings are collected; vehicle trajectory estimation is performed: and modeling the position p and the gesture R of the vehicle-mounted IMU by using two B spline functions respectively to obtain position, gesture and speed data of the vehicle-mounted IMU at any time, so as to obtain an IMU motion track, wherein the IMU motion track and control points of the B spline functions are updated on line in real time.
The modeling of the position p and the posture R of the vehicle-mounted IMU is carried out by utilizing two B spline functions respectively, and specifically comprises the following steps: setting the sampling time interval of the control point of the B spline function (such as 0.1s, the specific time interval can be adjusted according to the actual situation),
R(t)＝λ(t)d θ
p(t)＝λ(t)d t
wherein R (t) is a B-spline function representing the gesture, and p (t) is a B-spline function representing the position; lambda (t) is spline basis function, d θ D is spline parameter related to control point of the gesture B spline function t Is a spline parameter associated with the location B spline function control point. The angular velocity and the angular acceleration are obtained by first-order and second-order derivation of the B-spline function representing the gesture:
the linear velocity and the linear acceleration are obtained by conducting first-order and second-order derivation on the B-spline function representing the position:
the above describes the sampled values of the B-spline function of attitude, angular velocity, angular acceleration, position, linear velocity, linear acceleration at time t.
S2: processing the wheel speed meter readings and the rotation angle sensor readings in the step S1 by using a vehicle dynamics model to obtain the slip angle alpha and the yaw angle change rate of the vehicle from the wheel speed meter readings and the rotation angle sensor readings
Wherein C is r And C f Lateral deflection rigidity of front wheel and rear wheel of vehicle respectively r And l f The distances from the center of the rear axle and the center of the front axle to the center of mass of the vehicle, l=l r +l f The method comprises the steps of carrying out a first treatment on the surface of the m is the mass of the vehicle;
for the wheel speed meter readings of the vehicle, delta is the readings of the rotation angle sensor of the vehicle. And obtaining vehicle dynamics measurement by the change rate of the slip angle and the yaw angle, namely, linear speed and angular speed taking the mass center of the vehicle as the origin of a coordinate system:
adding a vehicle dynamics model parameter correction factor [ c ] to be optimized to a vehicle dynamics measurement v c α c ω ]Then there is a vehicle dynamics measure of:
wherein, the liquid crystal display device comprises a liquid crystal display device,
for the linear speed part of the vehicle dynamics measurement, +.>
An angular velocity component measured for vehicle dynamics; c v C is a correction factor for the magnitude of the linear velocity α C is a correction factor for the slip angle of the vehicle ω Is a correction factor for the magnitude of the angular velocity of the vehicle.
S3: initializing a newly added B spline control point: in the time period from the last control point before track updating to the newly added control point, the IMU data in S1 is subjected to discrete integration processing to obtain a pose set corresponding to each IMU sampling moment in the time period
Speed->
A collection;
discrete integration processing is carried out on the vehicle dynamics measurement in the S2, so that pose sets corresponding to each moment of obtaining the vehicle dynamics measurement in the time period are obtained
Using all pose sets and speed sets of the discrete time as measured values, and constraining the positions obtained by sampling the B spline function at the discrete timePose and speed, constructing a first optimization problem for initializing a control point newly added with a B spline;
the first optimization problem objective function is constructed as follows:
wherein phi is new For a newly added control point,
pose obtained by discrete integration of IMU, < ->
The velocity obtained for the IMU discrete integration; />
Measuring discrete integral for vehicle dynamics to obtain pose; />
For the acquisition time of IMU data, +.>
The time of acquisition of the vehicle dynamics measurement; />
Is an external parameter between the vehicle-mounted IMU coordinate system and the vehicle mass center coordinate system.
Step (4): adding a vehicle lateral acceleration constraint in vehicle track estimation, and calculating a vehicle lateral acceleration constraint residual as follows:
wherein u= [0 1 0]Is a selection matrix, a V (t s ) Line converted from IMU coordinate system to vehicle mass center coordinate system after second-order derivation of trackThe acceleration rate of the vehicle is calculated,
for the three-axis angular velocity reading of the IMU gyroscope, v V (t s ) The linear velocity of the trajectory is converted from the IMU coordinate system to the vehicle mass center coordinate system after first order derivation] ∧ For the operation of vector conversion into its corresponding anti-symmetric matrix, [ ·] z To take the operation of the corresponding shaft angle conversion rate of the z-axis; t is t s The set track sampling time is set to be consistent with the acquisition time of the IMU in the invention.
Step (5): extracting plane characteristic points and edge characteristic points of a laser radar point cloud, constructing a laser radar point cloud residual, directly constructing an IMU residual by utilizing the original IMU triaxial acceleration and triaxial angular velocity measurement and first and second derivatives of an estimated track, and directly constructing a vehicle dynamics residual by utilizing the vehicle dynamics measurement and the first derivative of the estimated track:
wherein e L 、e I 、e V The laser radar point cloud residual error, the IMU residual error and the vehicle dynamics measurement residual error are respectively obtained;
for converting from IMU track in world coordinate system to laser radar track in world coordinate system by external reference +.>
Sampling the kth sampling point of the frame laser radarAnd the time pi is a projection equation from the plane characteristic point to the plane and from the edge characteristic point to the straight line. />
Is IMU track under world coordinate system, t I For the acquisition time of IMU data omega B (t I ) Obtaining the angular velocity under the IMU coordinate system after deriving the IMU track in first order, < +.>
For three-axis angular velocity measurement of IMU, b a And b g The bias of IMU accelerometer and gyroscope, respectively. v V (t V ),ω V (t V ) Respectively obtaining linear velocity and angular velocity, t under a vehicle mass center coordinate system obtained through external parameters after first-order derivation of the track V Time of acquisition for vehicle dynamics measurement, c v For measuring the correction factor of the linear velocity magnitude with respect to the dynamics of the vehicle c α For measuring correction factors of slip angle with respect to vehicle dynamics c ω The correction factor for the magnitude of the angular velocity is measured with respect to the dynamics of the vehicle.
Step (6): constructing an optimization problem according to a point cloud residual, an IMU residual, a vehicle dynamics residual and a lateral acceleration constraint residual, and simultaneously optimizing a track spline function control point and a correction factor vector by using a Levenberg-Marquardt algorithm, wherein the objective function of the optimization problem is as follows:
wherein the optimization variable is a correction factor vector c= [ c ] v c α c ω ]Active control point set Φ= { Φ of track i ,…,φ i+k-1 K is the order of the B-spline function; e, e L 、e I 、e V 、e M And the laser radar point cloud residual error, the IMU residual error, the vehicle dynamics residual error and the vehicle transverse acceleration constraint residual error are respectively obtained. Sigma and method for producing the same L 、Σ I 、Σ V Respectively laser radar point cloudsResidual, IMU residual, weight matrix of vehicle dynamics residual,
the sigma-delta norm corresponding to the sigma weight matrix.
In order to further illustrate that the invention can effectively improve the positioning accuracy and robustness of the vehicle, the invention is used for carrying out positioning track experiments in an outdoor environment and a tunnel degradation environment, the positioning results of the invention and a comparison algorithm are shown in the table 1 and the table two, wherein the root mean square error (RMSE, root mean square error) of an absolute track error (ATE, absolute trajectory error) of positioning is compared in the outdoor environment, and the smaller the value is, the higher the positioning accuracy is; the loop-back error of the positioning is compared in the tunnel degradation environment, and the smaller the error is, the higher the positioning accuracy is. As can be seen from Table 1, the effectiveness of each innovative module in the present invention, and compared with other methods, the present invention can obtain high-precision positioning results. As can be seen from table 2, the positioning of the present invention in a tunnel degradation environment is more robust than other methods.
TABLE 1
TABLE 2
| Method | The invention is that LIO- | SAM |
| Loop error (m) | 1.798 | 7.28 |
The above embodiments are only preferred embodiments of the present invention, and are used for illustrating the technical solution of the present invention, but not limiting the protection scope of the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, it should be understood that modifications and substitutions may be made to the technical solutions described in the foregoing embodiments or to some of the technical features thereof without departing from the spirit and scope of the claims and their equivalents, and therefore, these modifications and substitutions are within the scope of the technical solutions.
Claims (7)
1. A laser radar-inertia-vehicle fusion positioning method based on a B spline function is characterized by comprising the following steps:
s1: collecting laser radar real-time scanning point cloud, IMU data, wheel speed meter readings and corner sensor readings; vehicle trajectory estimation is performed: modeling the position p and the gesture R of the vehicle-mounted IMU by using two B spline functions respectively to obtain position, gesture and speed data of the vehicle-mounted IMU at any time, and further obtain an IMU motion track;
s2: processing the wheel speed meter readings and the corner sensor readings in the step S1 by using a vehicle dynamics model to obtain vehicle dynamics measurement, optimizing correction factor vectors on line, and correcting parameters of the vehicle dynamics model;
s3: in the time period from the last control point before track updating to the newly added control point, the IMU data in S1 is subjected to discrete integration processing to obtain a pose set corresponding to each IMU sampling moment in the time period
Speed->
A collection;
by performing discrete integration processing on the vehicle dynamics measurement in S2, each obtained vehicle in the time period is obtainedPose set corresponding to moment of dynamic measurement
Taking all pose sets and velocity sets at discrete moments as measured values, constraining the poses and the velocities obtained by sampling the B spline function at the discrete moments, and constructing a first optimization problem for initializing a control point newly added into the B spline;
s4: adding vehicle lateral acceleration constraint in vehicle track estimation, and calculating a lateral acceleration constraint residual error;
s5: matching each frame of laser radar point cloud obtained in the step S1 with line features and surface features extracted from an environment map, and calculating laser radar point cloud residual errors; according to the IMU data in the S1, calculating an IMU residual error; calculating a vehicle dynamics measurement residual according to the vehicle dynamics measurement in S2;
s6: and constructing a second optimization problem through the point cloud residual, the IMU residual, the vehicle dynamics measurement residual and the lateral acceleration constraint residual, taking the control point newly added with the B spline after the initialization of the S3 as an initial value of the optimization problem, and simultaneously optimizing the spline function control point in the S1 and the correction factor vector in the S2 to obtain a vehicle real-time pose track and a vehicle model parameter correction value for positioning.
2. The method for positioning a laser radar-inertia-vehicle fusion based on a B-spline function according to claim 1, wherein in S1, two B-spline functions are used to model the position p and the pose R of the vehicle-mounted IMU, specifically:
R(t)＝λ(t)d θ
p(t)＝λ(t)d t
wherein R (t) is a B-spline function representing the gesture, and p (t) is a B-spline function representing the position; lambda (t) is spline basis function, d θ D is spline parameter related to control point of the gesture B spline function t Spline parameters related to the control points of the position B spline function; deriving the angular velocity and the angular acceleration of the B-spline function representing the gesture by first order and second order：
The linear velocity and the linear acceleration are obtained by conducting first-order and second-order derivation on the B-spline function representing the position:
the above describes the sampled values of the B-spline function of attitude, angular velocity, angular acceleration, position, linear velocity, linear acceleration at time t.
3. The method for positioning a laser radar-inertia-vehicle fusion based on a B-spline function according to claim 1, wherein S2 is specifically: obtaining the slip angle alpha and the yaw angle change rate of the vehicle from the wheel speed meter readings and the rotation angle sensor readings by using a dynamics model of the vehicle
The linear speed and the angular speed taking the mass center of the vehicle as the origin of a coordinate system are obtained through the sliding angle and the change rate of the yaw angle:
adding a vehicle dynamics model parameter correction factor vector [ c ] to be optimized v c α c ω ]Then there is a vehicle dynamics measure of:
wherein C is r And C f Lateral deflection rigidity of front wheel and rear wheel of vehicle respectively r And l f The distances from the center of the rear axle and the center of the front axle to the center of mass of the vehicle, l=l r +l f The method comprises the steps of carrying out a first treatment on the surface of the m is the mass of the vehicle;
the method comprises the steps that (1) a wheel speed meter of a vehicle is read, and delta is the read of a vehicle rotation angle sensor; />
For the linear speed part of the vehicle dynamics measurement, +.>
An angular velocity component measured for vehicle dynamics; c v C is a correction factor for the magnitude of the linear velocity α C is a correction factor for the slip angle of the vehicle ω Is a correction factor for the magnitude of the angular velocity of the vehicle.
4.The method for positioning a laser radar-inertia-vehicle fusion based on a B-spline function according to claim 1, wherein the step S3 is specifically: discrete integration processing is carried out on IMU data to obtain pose sets corresponding to each IMU sampling moment in the time period
Speed set +.>
Performing discrete integration processing on the vehicle dynamics measurement to obtain a pose set corresponding to each vehicle dynamics measurement obtaining moment in the time period>
The pose and the speed at discrete time obtained by recursion are used as measured values, and a first optimization problem objective function is constructed as follows:
wherein phi is new For a newly added control point,
pose obtained by discrete integration of IMU, < ->
The velocity obtained for the IMU discrete integration; />
Measuring discrete integral for vehicle dynamics to obtain pose; />
For the acquisition time of IMU data, +.>
The time of acquisition of the vehicle dynamics measurement; />
The upper corner mark T represents transposition for external parameters from the vehicle-mounted IMU coordinate system to the vehicle mass center coordinate system.
5. The method for positioning a laser radar-inertia-vehicle fusion based on a B-spline function according to claim 1, wherein in S4, a vehicle lateral acceleration constraint is added in the vehicle trajectory estimation, and the calculation of a vehicle lateral acceleration constraint residual is specifically:
wherein u= [0 1 0]Is a selection matrix, a V (t s ) The IMU coordinate system is converted into the linear acceleration of the vehicle mass center coordinate system after the second-order derivation is carried out on the IMU motion track,
for the three-axis angular velocity reading of the IMU gyroscope, v V (t s ) The linear velocity of the IMU coordinate system converted to the vehicle mass center coordinate system after the IMU motion track is first-order derived, [] ∧ For the operation of vector conversion into its corresponding anti-symmetric matrix, [ ·] z To take the operation of the corresponding shaft angle conversion rate of the z-axis; t is t s The set track sampling time is consistent with the acquisition time of the IMU.
6. The method for positioning a laser radar-inertia-vehicle fusion based on a B-spline function according to claim 1, wherein the vehicle dynamics measurement residual in S5 is specifically: the method comprises the steps of constructing a laser radar point cloud residual, directly constructing an IMU residual by utilizing original IMU triaxial acceleration and triaxial angular velocity measurement and first and second derivatives of an IMU motion track, and directly constructing a vehicle dynamics residual by utilizing vehicle dynamics measurement and the first derivatives of the estimated track, wherein the formula is as follows:
wherein e L 、e I 、e V The laser radar point cloud residual error, the IMU residual error and the vehicle dynamics residual error are respectively;
for converting from IMU track in world coordinate system to laser radar track in world coordinate system by external reference +.>
For the sampling time of the kth sampling point of the frame laser radar, pi is a projection equation from a plane characteristic point to a plane and from an edge characteristic point to a straight line; />
Is IMU track under world coordinate system, t I For the acquisition time of IMU data omega B (t I ) Obtaining the angular velocity under the IMU coordinate system after deriving the IMU track in first order, < +.>
For three-axis angular velocity measurement of IMU, b a And b g Respectively the bias of the IMU accelerometer and the gyroscope; v V (t V ),ω V (t V ) Respectively obtaining linear velocity and angular velocity, t under a vehicle mass center coordinate system obtained through external parameters after first-order derivation of the track V Time of acquisition for vehicle dynamics measurement, c v For measuring the correction factor of the linear velocity magnitude with respect to the dynamics of the vehicle c α For measuring correction factors of slip angle with respect to vehicle dynamics c ω The correction factor for the magnitude of the angular velocity is measured with respect to the dynamics of the vehicle.
7. The method for positioning a fusion of a lidar and an inertia-vehicle based on a B-spline function according to claim 1, wherein the objective function of the second optimization problem in S6 is:
wherein the optimization variable is a correction factor vector c= [ c ] v c α c ω ]Active control point set Φ= { Φ of track i ,…,φ i+k-1 K is the order of the B-spline function; e, e L 、e I 、e V 、e M The laser radar point cloud residual error, the IMU residual error, the vehicle dynamics residual error and the vehicle transverse acceleration constraint residual error are respectively obtained; sigma and method for producing the same L 、Σ I 、Σ V Respectively a laser radar point cloud residual error, an IMU residual error and a weight matrix of vehicle dynamics residual error,
the sigma-delta norm corresponding to the sigma weight matrix.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310183664.5A CN116338719A (en) | 2023-03-01 | 2023-03-01 | Laser radar-inertia-vehicle fusion positioning method based on B spline function |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310183664.5A CN116338719A (en) | 2023-03-01 | 2023-03-01 | Laser radar-inertia-vehicle fusion positioning method based on B spline function |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116338719A true CN116338719A (en) | 2023-06-27 |
Family
ID=86893913
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310183664.5A Pending CN116338719A (en) | 2023-03-01 | 2023-03-01 | Laser radar-inertia-vehicle fusion positioning method based on B spline function |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116338719A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116576850A (en) * | 2023-07-12 | 2023-08-11 | 北京集度科技有限公司 | Pose determining method and device, computer equipment and storage medium |
-
2023
- 2023-03-01 CN CN202310183664.5A patent/CN116338719A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116576850A (en) * | 2023-07-12 | 2023-08-11 | 北京集度科技有限公司 | Pose determining method and device, computer equipment and storage medium |
| CN116576850B (en) * | 2023-07-12 | 2023-10-20 | 北京集度科技有限公司 | Pose determining method and device, computer equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111426318B (en) | Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering | |
| CN108731670B (en) | Inertial/visual odometer integrated navigation positioning method based on measurement model optimization | |
| CN110501024B (en) | Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system | |
| CN107941217B (en) | Robot positioning method, electronic equipment, storage medium and device | |
| CN113311411B (en) | Laser radar point cloud motion distortion correction method for mobile robot | |
| CN102538781B (en) | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method | |
| CN109916431B (en) | Wheel encoder calibration algorithm for four-wheel mobile robot | |
| CN110887481B (en) | Carrier dynamic attitude estimation method based on MEMS inertial sensor | |
| CN110307836B (en) | Accurate positioning method for welt cleaning of unmanned cleaning vehicle | |
| CN113819914A (en) | Map construction method and device | |
| CN113063429B (en) | Self-adaptive vehicle-mounted integrated navigation positioning method | |
| CN111238535B (en) | IMU error online calibration method based on factor graph | |
| CN110926460A (en) | Uwb positioning abnormal value processing method based on IMU | |
| CN109870173A (en) | A kind of track correct method of the submarine pipeline inertial navigation system based on checkpoint | |
| CN107063305B (en) | Method for correcting downhill suspended rear wheel odometer error by using inertial navigation and pressure sensors | |
| CN109612460B (en) | Plumb line deviation measuring method based on static correction | |
| CN111238471B (en) | Sideslip angle estimation method and estimator suitable for agricultural machine linear navigation | |
| CN112683267B (en) | Vehicle-mounted attitude estimation method with GNSS velocity vector assistance | |
| CN111189474A (en) | Autonomous calibration method of MARG sensor based on MEMS | |
| CN112562077A (en) | Pedestrian indoor positioning method integrating PDR and prior map | |
| CN111189454A (en) | Unmanned vehicle SLAM navigation method based on rank Kalman filtering | |
| CN111307114B (en) | Water surface ship horizontal attitude measurement method based on motion reference unit | |
| CN116338719A (en) | Laser radar-inertia-vehicle fusion positioning method based on B spline function | |
| Chen et al. | 3D LiDAR-GPS/IMU calibration based on hand-eye calibration model for unmanned vehicle | |
| JP2014240266A (en) | Sensor drift amount estimation device and program |
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 |