CN113063414A - Vehicle dynamics pre-integration construction method for visual inertia SLAM - Google Patents

Vehicle dynamics pre-integration construction method for visual inertia SLAM Download PDF

Info

Publication number
CN113063414A
CN113063414A CN202110329172.3A CN202110329172A CN113063414A CN 113063414 A CN113063414 A CN 113063414A CN 202110329172 A CN202110329172 A CN 202110329172A CN 113063414 A CN113063414 A CN 113063414A
Authority
CN
China
Prior art keywords
vehicle
integration
dynamics
follows
dynamics pre
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202110329172.3A
Other languages
Chinese (zh)
Inventor
张培志
余卓平
王晓
蒋屹晨
史戈松
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Intelligent New Energy Vehicle Technology Innovation Platform Co ltd
Original Assignee
Shanghai Intelligent New Energy Vehicle Technology Innovation Platform Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Intelligent New Energy Vehicle Technology Innovation Platform Co ltd filed Critical Shanghai Intelligent New Energy Vehicle Technology Innovation Platform Co ltd
Priority to CN202110329172.3A priority Critical patent/CN113063414A/en
Publication of CN113063414A publication Critical patent/CN113063414A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manufacturing & Machinery (AREA)
  • Automation & Control Theory (AREA)
  • Steering Control In Accordance With Driving Conditions (AREA)

Abstract

The invention relates to a vehicle dynamics pre-integration construction method for visual inertia SLAM, which specifically comprises the following steps: s1, obtaining vehicle information of the target vehicle, and establishing a two-degree-of-freedom single-rail dynamic model of the target vehicle according to the vehicle information; s2, obtaining a steering wheel corner and a vehicle speed corresponding to a vehicle chassis of the target vehicle, and establishing a system observation model according to the dynamic model; s3, establishing a vehicle displacement dynamics pre-integral and a vehicle rotation dynamics pre-integral through Euler integration according to a system observation model; s4, separating the speed noise item and the angular speed noise item in the vehicle displacement dynamics pre-integration and the rotation dynamics pre-integration, establishing a vehicle dynamics integration error propagation model according to the separation result, and calculating to obtain the vehicle dynamics pre-integration of the target vehicle through the vehicle dynamics integration error propagation model. Compared with the prior art, the method has the advantages of improving the positioning precision and stability of the visual inertia SLAM system through vehicle dynamics pre-integration and the like.

Description

Vehicle dynamics pre-integration construction method for visual inertia SLAM
Technical Field
The invention relates to the technical field of automatic driving automobile positioning, in particular to a vehicle dynamics pre-integration construction method for visual inertia SLAM.
Background
The positioning technology is one of the core technologies of an automatic driving automobile, and the visual inertia SLAM is a main positioning means applied to indoor/underground and other scenes without GPS signals. The automatic driving automobile is often under the working condition of uniform speed, the problem of inconspicuous system scale can be caused due to insufficient excitation of the inertial navigation accelerometer, and the measurement value of the inertial navigation has a zero drift phenomenon, so that the estimation precision of the system can be influenced.
Considering that the information such as the steering wheel angle, the vehicle speed and the like of the automatic driving automobile is accurately known and CAN be conveniently measured from the CAN bus, the method CAN be used for carrying out fusion positioning with visual information and inertial navigation information by utilizing the characteristics that the method does not need excitation and has no zero drift, so that the positioning precision and stability of the system are improved, and how to utilize the vehicle chassis information and construct the motion integral between key image frames based on a vehicle dynamic model becomes the problem to be solved firstly.
Disclosure of Invention
The invention aims to overcome the defects that the excitation of an inertial navigation accelerometer in the prior art is insufficient, so that the system scale is not considerable, and the measured value of inertial navigation has a zero drift phenomenon, and provides a vehicle dynamics pre-integration construction method for a visual inertia SLAM.
The purpose of the invention can be realized by the following technical scheme:
a vehicle dynamics pre-integration construction method for visual inertia SLAM specifically comprises the following steps:
s1, obtaining vehicle information of the target vehicle, and establishing a two-degree-of-freedom single-rail dynamic model of the target vehicle according to the vehicle information;
s2, obtaining a steering wheel corner and a vehicle speed corresponding to a vehicle chassis of the target vehicle, and establishing a system observation model according to the dynamic model;
s3, establishing a vehicle displacement dynamics pre-integral and a vehicle rotation dynamics pre-integral through Euler integration according to a system observation model;
s4, separating the speed noise item and the angular speed noise item in the vehicle displacement dynamics pre-integration and the rotation dynamics pre-integration, establishing a vehicle dynamics integration error propagation model according to the separation result, and calculating the vehicle dynamics pre-integration of the target vehicle through the vehicle dynamics integration error propagation model.
The formula of the dynamic model of the two-degree-of-freedom single track of the target vehicle is specifically as follows:
Figure BDA0002995760690000021
wherein k isfFor front axle yaw stiffness, krFor rear wheel cornering stiffness, /)fIs the distance of the center of mass to the front axis,/rIs the distance from the center of mass to the rear axle, beta is the side slip angle of the center of mass, alpha is the front wheel rotation angle, omegarAs yaw rate, IzThe vehicle is subjected to right multiplication and right multiplication of the lie algebra around the z axis, v is the vehicle speed, v isxIn order to be the longitudinal vehicle speed,
Figure BDA0002995760690000022
the first derivative of lateral vehicle speed with respect to time.
Further, when the centroid slip angle is smaller than a set threshold, the longitudinal vehicle speed is used as the vehicle speed of the target vehicle in the dynamic model, and at this time, the dynamic model is specifically as follows:
Figure BDA0002995760690000023
wherein, the mass center slip angle satisfies d beta/dt equals 0, and the yaw angular velocity satisfies d omegar/dt＝0。
The formula of the system observation model is specifically as follows:
Figure BDA0002995760690000024
Figure BDA0002995760690000025
wherein the content of the first and second substances,
Figure BDA0002995760690000026
is a three-dimensional space yaw-rate vector,
Figure BDA0002995760690000027
is a three-dimensional space velocity vector and is,
Figure BDA0002995760690000028
is a measure of the speed of the center of mass, betatIs the centroid slip angle at time t,
Figure BDA0002995760690000029
is a measure of yaw rate.
Further, the calculation formulas of the measured values of the centroid slip angle and the yaw rate at the time t are as follows:
Figure BDA00029957606900000210
Figure BDA0002995760690000031
wherein alpha istAnd the steering angle of the front wheel at the time t is calculated by dividing the steering wheel angle corresponding to the chassis of the vehicle by the steering transmission ratio, and K is a stability factor.
The vehicle displacement dynamics pre-integration and the vehicle rotation dynamics pre-integration are specifically as follows:
Figure BDA0002995760690000032
Figure BDA0002995760690000033
wherein the content of the first and second substances,
Figure BDA0002995760690000034
is a pre-integration of the vehicle displacement dynamics,
Figure BDA0002995760690000035
is a pre-integration of the vehicle's rotational dynamics,
Figure BDA0002995760690000036
rotation matrix from vehicle coordinate system to world coordinate system for time t, nvFor velocity vector noise, nωIs angular velocity vector noise.
Further, the vehicle displacement dynamics pre-integration and the vehicle rotation dynamics pre-integration are obtained by separating optimization variables and measurement values through discrete chassis motion data integration, and are specifically as follows:
Figure BDA0002995760690000037
Figure BDA0002995760690000038
wherein the content of the first and second substances,
Figure BDA0002995760690000039
for the displacement from the vehicle coordinate system to the world coordinate system,
Figure BDA00029957606900000310
is a rotation matrix from the vehicle coordinate system to the world coordinate system at the corresponding time of the image frame.
Further, the state recursion of the discrete form chassis motion data integration is as follows:
Figure BDA00029957606900000311
Figure BDA00029957606900000312
the discrete chassis motion data integral is calculated by the chassis motion data integral through an Euler integral algorithm, and is specifically as follows:
Figure BDA00029957606900000313
Figure BDA00029957606900000314
wherein, Δ t is the sampling time interval of the chassis motion data, Δ tijAnd j-i-1 chassis motion data between two image frames.
Further, the chassis motion data integral is obtained by calculation through a system observation model, and the corresponding continuous form state recursion is specifically as follows:
Figure BDA0002995760690000041
Figure BDA0002995760690000042
wherein the subscript v represents a vehicle coordinate system, the superscript w represents a world coordinate system, vkRepresenting the vehicle coordinate system corresponding to the image frame sequence k.
The vehicle displacement dynamics pre-integration and the vehicle rotation dynamics pre-integration separate a velocity noise term and an angular velocity noise term as follows:
Figure BDA0002995760690000043
Figure BDA0002995760690000044
the formula of the vehicle dynamics integral error propagation model is specifically as follows:
Figure BDA0002995760690000045
wherein, δ αijError states, δ γ, for vehicle displacement dynamics pre-integrationijIs the error state of the vehicle rotational dynamics pre-integration, I is the identity matrix,
Figure BDA0002995760690000046
as a rotation matrix of the propagation process, JrIs a lie algebra right-multiplicative jacobian.
Compared with the prior art, the invention has the following beneficial effects:
the method is based on a vehicle dynamics model, makes full use of the information such as steering wheel turning angle and vehicle speed accurately known by an automatic driving automobile chassis, constructs motion integral between key image frames, constructs a vehicle dynamics integral error propagation model through vehicle displacement dynamics pre-integration and vehicle rotation dynamics pre-integration, and obtains the vehicle dynamics pre-integration of a target vehicle through calculation of the vehicle dynamics integral error propagation model, so that the problems that the scale of a visual inertia SLAM system is not observable under the working condition of a vehicle at a constant speed and the inertial navigation measured value has zero drift due to the fact that inertial navigation is only relied on are solved, and the positioning accuracy and the positioning stability of the visual inertia SLAM system are improved.
Drawings
FIG. 1 is a schematic flow diagram of the present invention;
FIG. 2 is a schematic structural diagram of a two-degree-of-freedom single-rail dynamic model according to the present invention.
Detailed Description
The invention is described in detail below with reference to the figures and specific embodiments. The present embodiment is implemented on the premise of the technical solution of the present invention, and a detailed implementation manner and a specific operation process are given, but the scope of the present invention is not limited to the following embodiments.
Examples
As shown in fig. 1, a vehicle dynamics pre-integration construction method for a visual inertia SLAM specifically includes the following steps:
s1, obtaining vehicle information of the target vehicle, and establishing a two-degree-of-freedom single-rail dynamic model of the target vehicle according to the vehicle information;
s2, obtaining a steering wheel corner and a vehicle speed corresponding to a vehicle chassis of the target vehicle, and establishing a system observation model according to the dynamic model;
s3, establishing a vehicle displacement dynamics pre-integral and a vehicle rotation dynamics pre-integral through Euler integration according to a system observation model;
s4, separating the speed noise item and the angular speed noise item in the vehicle displacement dynamics pre-integration and the rotation dynamics pre-integration, establishing a vehicle dynamics integration error propagation model according to the separation result, and calculating to obtain the vehicle dynamics pre-integration of the target vehicle through the vehicle dynamics integration error propagation model.
As shown in fig. 2, in this embodiment, the mass of the vehicle is concentrated at the center of mass, the adhesion force between the tire and the ground is directly transmitted through the front wheel and the rear wheel, while the influence of the suspension is ignored, the pitch angle, the roll angle and the vertical displacement of the vehicle are all zero, and the force acting on the center of mass of the vehicle does not change the distribution of the tire load. In addition, under the working condition of medium and low speed, the tire cornering characteristic is in a linear range. The target vehicle is simplified into a two-degree-of-freedom monorail model supported on the ground by front and rear lateral elastic tires and having lateral and yaw motions, which is specifically shown as follows:
Figure BDA0002995760690000051
wherein k isfFor front axle yaw stiffness, krFor rear wheel cornering stiffness, /)fIs the distance of the center of mass to the front axis,/rIs the distance from the center of mass to the rear axle, beta is the side slip angle of the center of mass, alpha is the front wheel rotation angle, omegarAs yaw rate, IzThe vehicle is subjected to right multiplication and right multiplication of the lie algebra around the z axis, v is the vehicle speed, v isxIn order to be the longitudinal vehicle speed,
Figure BDA0002995760690000052
the first derivative of lateral vehicle speed with respect to time.
When the centroid slip angle is smaller than a set threshold value, the longitudinal speed is taken as the speed of the target vehicle in the dynamic model, and the dynamic model is as follows:
Figure BDA0002995760690000061
wherein, the mass center slip angle satisfies d beta/dt equals 0, and the yaw angular velocity satisfies d omegar/dt＝0。
The formula of the system observation model is specifically as follows:
Figure BDA0002995760690000062
Figure BDA0002995760690000063
wherein the content of the first and second substances,
Figure BDA0002995760690000064
is a three-dimensional space yaw-rate vector,
Figure BDA0002995760690000065
is a three-dimensional space velocity vector and is,
Figure BDA0002995760690000066
is a measure of the speed of the center of mass, betatIs the centroid slip angle at time t,
Figure BDA0002995760690000067
is a measure of yaw rate.
the calculation formula of the measured values of the centroid slip angle and the yaw rate at time t is as follows:
Figure BDA0002995760690000068
Figure BDA0002995760690000069
wherein alpha istAnd the steering angle of the front wheel at the time t is calculated by dividing the steering wheel angle corresponding to the chassis of the vehicle by the steering transmission ratio, and K is a stability factor.
The vehicle displacement dynamics pre-integration and the vehicle rotation dynamics pre-integration are specifically as follows:
Figure BDA00029957606900000610
Figure BDA00029957606900000611
wherein the content of the first and second substances,
Figure BDA00029957606900000612
is a pre-integration of the vehicle displacement dynamics,
Figure BDA00029957606900000613
is a pre-integration of the vehicle's rotational dynamics,
Figure BDA00029957606900000614
rotation matrix from vehicle coordinate system to world coordinate system for time t, nvFor velocity vector noise, nωIs angular velocity vector noise.
The vehicle displacement dynamics pre-integration and the vehicle rotation dynamics pre-integration are obtained by separating optimization variables from measured values through discrete chassis motion data integration, and are specifically as follows:
Figure BDA00029957606900000615
Figure BDA00029957606900000616
wherein the content of the first and second substances,
Figure BDA0002995760690000071
for the displacement from the vehicle coordinate system to the world coordinate system,
Figure BDA0002995760690000072
is a rotation matrix from the vehicle coordinate system to the world coordinate system at the corresponding time of the image frame.
The state recursion of the discrete form chassis motion data integration is as follows:
Figure BDA0002995760690000073
Figure BDA0002995760690000074
the discrete chassis motion data integral is calculated by the chassis motion data integral through an Euler integral algorithm, and is specifically as follows:
Figure BDA0002995760690000075
Figure BDA0002995760690000076
wherein Δ t is chassis motion dataSampling time interval, Δ tijAnd j-i-1 chassis motion data between two image frames.
The chassis motion data integral is obtained by calculation through a system observation model, and the corresponding continuous form state recursion is specifically as follows:
Figure BDA0002995760690000077
Figure BDA0002995760690000078
wherein the subscript v represents a vehicle coordinate system, the superscript w represents a world coordinate system, vkRepresenting the vehicle coordinate system corresponding to the image frame sequence k.
The vehicle displacement dynamics pre-integration and the vehicle rotation dynamics pre-integration separate the velocity noise term and the angular velocity noise term as follows:
Figure BDA0002995760690000079
Figure BDA00029957606900000710
for the vehicle rotational dynamics pre-integration, the following is specifically satisfied:
Figure BDA0002995760690000081
wherein the content of the first and second substances,
Figure BDA0002995760690000082
the method is a vehicle rotation dynamics pre-integration of a real state containing noise, is directly obtained from chassis motion data, and meets the condition that Exp (phi + delta phi) is approximately equal to Exp (phi) Exp (J) when delta phi is a high-order small quantityr(φ)δφ)。
Is rotatedError state of kinetic integration δ γijThe expression is as follows:
Figure BDA0002995760690000083
when delta phi is a high-order small quantity, the condition is satisfied
Figure BDA0002995760690000084
At the same time due to
Figure BDA0002995760690000085
Is very small, can obtain
Figure BDA0002995760690000086
Taking the logarithm of both sides of the above formula at the same time, the following formula is obtained:
Figure BDA0002995760690000087
wherein, n is given in the present embodimentωObey a zero mean gaussian distribution, so the error state δ γijThe first order approximation of (d) also follows a zero mean gaussian distribution.
Calculating to obtain delta gammaijThe recurrence formula of (c) is as follows:
Figure BDA0002995760690000088
for the vehicle rotational dynamics pre-integration, the following is specifically satisfied:
Figure BDA0002995760690000091
when φ is a high-order small quantity, Exp (φ) ≈ I + φ ^ is satisfied, so that the following formula is satisfied:
Figure BDA0002995760690000092
in this embodiment, high order small quantities are ignored
Figure BDA0002995760690000093
According to the property a ^ b ^ a, satisfy:
Figure BDA0002995760690000094
obtaining the error state delta alpha of the displacement dynamics integral quantityijThe expression is as follows:
Figure BDA0002995760690000095
wherein, n is given in the present embodimentvObeying a zero mean Gaussian distribution, δ γi,tThe approximation follows a Gaussian distribution, so the error state δ αijAlso approximately obey a gaussian distribution.
Delta alpha is obtained by calculationijThe recurrence formula of (c) is as follows:
Figure BDA0002995760690000096
the formula of the vehicle dynamics integral error propagation model is specifically as follows:
Figure BDA0002995760690000097
wherein, δ αijError states, δ γ, for vehicle displacement dynamics pre-integrationijIs the error state of the vehicle rotational dynamics pre-integration, I is the identity matrix,
Figure BDA0002995760690000098
as a rotation matrix of the propagation process, JrIs a lie algebra right-multiplicative jacobian.
In addition, it should be noted that the specific embodiments described in the present specification may have different names, and the above descriptions in the present specification are only illustrations of the structures of the present invention. All equivalent or simple changes in the structure, characteristics and principles of the invention are included in the protection scope of the invention. Various modifications or additions may be made to the described embodiments or methods may be similarly employed by those skilled in the art without departing from the scope of the invention as defined in the appending claims.

Claims (10)

1. A vehicle dynamics pre-integration construction method for visual inertia SLAM is characterized by comprising the following steps:
s1, obtaining vehicle information of the target vehicle, and establishing a two-degree-of-freedom single-rail dynamic model of the target vehicle according to the vehicle information;
s2, obtaining a steering wheel corner and a vehicle speed corresponding to a vehicle chassis of the target vehicle, and establishing a system observation model according to the dynamic model;
s3, establishing a vehicle displacement dynamics pre-integral and a vehicle rotation dynamics pre-integral through Euler integration according to a system observation model;
s4, separating the speed noise item and the angular speed noise item in the vehicle displacement dynamics pre-integration and the rotation dynamics pre-integration, establishing a vehicle dynamics integration error propagation model according to the separation result, and calculating the vehicle dynamics pre-integration of the target vehicle through the vehicle dynamics integration error propagation model.
2. The method of claim 1, wherein the formula of the dynamic model of the two-degree-of-freedom single-rail of the target vehicle is specifically as follows:
Figure FDA0002995760680000011
wherein k isfFor front axle yaw stiffness, krFor rear wheel cornering stiffness, /)fIs the distance of the center of mass to the front axis,/rIs the distance from the center of mass to the rear axle, beta is the side slip angle of the center of mass, alpha is the front wheel rotation angle, omegarAs yaw rate, IzThe vehicle is subjected to right multiplication and right multiplication of the lie algebra around the z axis, v is the vehicle speed, v isxIn order to be the longitudinal vehicle speed,
Figure FDA0002995760680000012
the first derivative of lateral vehicle speed with respect to time.
3. The vehicle dynamics pre-integration construction method for the visual inertia SLAM as claimed in claim 2, wherein when the centroid slip angle is smaller than a set threshold, the longitudinal vehicle speed is taken as the vehicle speed of the target vehicle in the dynamics model, and the dynamics model is as follows:
Figure FDA0002995760680000021
wherein, the mass center slip angle satisfies d beta/dt equals 0, and the yaw angular velocity satisfies d omegar/dt＝0。
4. The method of claim 3, wherein the system observation model is specifically formulated as follows:
Figure FDA0002995760680000022
Figure FDA0002995760680000023
wherein the content of the first and second substances,
Figure FDA0002995760680000024
is a three-dimensional space yaw-rate vector,
Figure FDA0002995760680000025
is a three-dimensional space velocity vector and is,
Figure FDA0002995760680000026
is a measure of the speed of the center of mass, betatIs the centroid slip angle at time t,
Figure FDA0002995760680000027
is a measure of yaw rate.
5. The vehicle dynamics pre-integration construction method for visual inertia SLAM according to claim 4, wherein the measured values of centroid yaw angle and yaw rate at time t are calculated as follows:
Figure FDA0002995760680000028
Figure FDA0002995760680000029
wherein alpha istK is the stability factor for the front wheel angle at time t.
6. The vehicle dynamics pre-integration construction method for the visual inertia SLAM according to claim 4, wherein the vehicle displacement dynamics pre-integration and the vehicle rotation dynamics pre-integration are specifically as follows:
Figure FDA00029957606800000210
Figure FDA00029957606800000211
wherein the content of the first and second substances,
Figure FDA00029957606800000212
is a pre-integration of the vehicle displacement dynamics,
Figure FDA00029957606800000213
is a pre-integration of the vehicle's rotational dynamics,
Figure FDA00029957606800000214
rotation matrix from vehicle coordinate system to world coordinate system for time t, nvFor velocity vector noise, nωIs angular velocity vector noise.
7. The vehicle dynamics pre-integration construction method for the visual inertia SLAM as claimed in claim 6, wherein the vehicle displacement dynamics pre-integration and the vehicle rotation dynamics pre-integration are obtained by separating optimization variables and measurement values through discrete chassis motion data integration, specifically as follows:
Figure FDA0002995760680000031
Figure FDA0002995760680000032
wherein the content of the first and second substances,
Figure FDA0002995760680000033
for the displacement from the vehicle coordinate system to the world coordinate system,
Figure FDA0002995760680000034
from the vehicle coordinate system to the corresponding time instant of the image frameA rotation matrix of the world coordinate system.
8. The vehicle dynamics pre-integration construction method for visual inertial SLAM of claim 7, wherein the state recursion of the discrete form chassis motion data integration is as follows:
Figure FDA0002995760680000035
Figure FDA0002995760680000036
the discrete chassis motion data integral is calculated by the chassis motion data integral through an Euler integral algorithm, and is specifically as follows:
Figure FDA0002995760680000037
Figure FDA0002995760680000038
where Δ t is the sampling time interval of the chassis motion data.
9. The vehicle dynamics pre-integration construction method for the visual inertia SLAM according to the claim 8, wherein the chassis motion data integration is calculated by a system observation model, and the corresponding continuous form state recursion is specifically as follows:
Figure FDA0002995760680000039
Figure FDA00029957606800000310
wherein the subscript v represents a vehicle coordinate system, the superscript w represents a world coordinate system, vkRepresenting the vehicle coordinate system corresponding to the image frame sequence k.
10. The vehicle dynamics pre-integration construction method for visual inertial SLAM of claim 8, wherein the vehicle displacement dynamics pre-integration and vehicle rotation dynamics pre-integration separate velocity noise term and angular velocity noise term as follows:
Figure FDA00029957606800000311
Figure FDA00029957606800000312
the formula of the vehicle dynamics integral error propagation model is specifically as follows:
Figure FDA0002995760680000041
wherein, δ αijError states, δ γ, for vehicle displacement dynamics pre-integrationijIs the error state of the vehicle rotational dynamics pre-integration, I is the identity matrix,
Figure FDA0002995760680000042
as a rotation matrix of the propagation process, JrIs a lie algebra right-multiplicative jacobian.
CN202110329172.3A 2021-03-27 2021-03-27 Vehicle dynamics pre-integration construction method for visual inertia SLAM Pending CN113063414A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110329172.3A CN113063414A (en) 2021-03-27 2021-03-27 Vehicle dynamics pre-integration construction method for visual inertia SLAM

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110329172.3A CN113063414A (en) 2021-03-27 2021-03-27 Vehicle dynamics pre-integration construction method for visual inertia SLAM

Publications (1)

Publication Number Publication Date
CN113063414A true CN113063414A (en) 2021-07-02

Family

ID=76564134

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110329172.3A Pending CN113063414A (en) 2021-03-27 2021-03-27 Vehicle dynamics pre-integration construction method for visual inertia SLAM

Country Status (1)

Country Link
CN (1) CN113063414A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113968278A (en) * 2021-11-17 2022-01-25 广州文远知行科技有限公司 Vehicle steering wheel correction method and device, electronic equipment and storage medium
CN115406451A (en) * 2022-11-01 2022-11-29 联友智连科技有限公司 Vehicle positioning method, system, vehicle terminal and storage medium

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106649983A (en) * 2016-11-09 2017-05-10 吉林大学 Vehicle dynamics model modeling method used in unmanned vehicle high-velocity motion planning
US20180231385A1 (en) * 2016-10-25 2018-08-16 Massachusetts Institute Of Technology Inertial Odometry With Retroactive Sensor Calibration
CN111257853A (en) * 2020-01-10 2020-06-09 清华大学 Automatic driving system laser radar online calibration method based on IMU pre-integration
CN111547059A (en) * 2020-04-23 2020-08-18 上海大学 Distributed driving electric automobile inertia parameter estimation method
CN112083725A (en) * 2020-09-04 2020-12-15 湖南大学 Structure-shared multi-sensor fusion positioning system for automatic driving vehicle
CN112506195A (en) * 2020-12-02 2021-03-16 吉林大学 Vehicle autonomous positioning system and positioning method based on vision and chassis information

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180231385A1 (en) * 2016-10-25 2018-08-16 Massachusetts Institute Of Technology Inertial Odometry With Retroactive Sensor Calibration
CN106649983A (en) * 2016-11-09 2017-05-10 吉林大学 Vehicle dynamics model modeling method used in unmanned vehicle high-velocity motion planning
CN111257853A (en) * 2020-01-10 2020-06-09 清华大学 Automatic driving system laser radar online calibration method based on IMU pre-integration
CN111547059A (en) * 2020-04-23 2020-08-18 上海大学 Distributed driving electric automobile inertia parameter estimation method
CN112083725A (en) * 2020-09-04 2020-12-15 湖南大学 Structure-shared multi-sensor fusion positioning system for automatic driving vehicle
CN112506195A (en) * 2020-12-02 2021-03-16 吉林大学 Vehicle autonomous positioning system and positioning method based on vision and chassis information

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
RONG KANGY，等: "VINS-Vehicle: A Tightly-Coupled Vehicle Dynamics Extension to Visual-Inertial State Estimator", 《2019 IEEE INTELLIGENT TRANSPORTATION SYSTEMS CONFERENCE (ITSC)》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113968278A (en) * 2021-11-17 2022-01-25 广州文远知行科技有限公司 Vehicle steering wheel correction method and device, electronic equipment and storage medium
CN115406451A (en) * 2022-11-01 2022-11-29 联友智连科技有限公司 Vehicle positioning method, system, vehicle terminal and storage medium

Similar Documents

Publication Publication Date Title
CN108482379B (en) Wheel-hub motor driven vehicle coefficient of road adhesion and the synchronous real-time estimation system and method for road gradient
CN102529976B (en) Vehicle running state nonlinear robust estimation method based on sliding mode observer
Bevly et al. Integrating INS sensors with GPS velocity measurements for continuous estimation of vehicle sideslip and tire cornering stiffness
CN106568449B (en) A kind of GNSS/INS Combinated navigation method of auto model auxiliary and constraint based on MEMS
CN110341714B (en) Method for simultaneously estimating vehicle mass center slip angle and disturbance
CN108819950B (en) Vehicle speed estimation method and system of vehicle stability control system
CN111750897B (en) Yaw rate gyroscope deviation estimation method based on Longbeige observer
CN104061899A (en) Kalman filtering based method for estimating roll angle and pitching angle of vehicle
CN110884499B (en) Method and system for determining vehicle mass center slip angle
CN113063414A (en) Vehicle dynamics pre-integration construction method for visual inertia SLAM
CN111796522B (en) Vehicle state estimation method
CN103661398A (en) Vehicle non-steering left rear wheel linear speed estimation method based on sliding-mode observer
CN113247004A (en) Joint estimation method for vehicle mass and road transverse gradient
CN111688715B (en) Centroid slip angle observation method of four-wheel drive electric vehicle based on fusion technology
CN113682282A (en) Vehicle stability control method and system, vehicle and storage medium
CN111006884B (en) Method for measuring wheel axle slip angle and slip stiffness based on Fourier transform
CN109033017B (en) Vehicle roll angle and pitch angle estimation method under packet loss environment
CN111845755A (en) Method for estimating longitudinal speed of vehicle
CN115303265A (en) Vehicle obstacle avoidance control method and device and vehicle
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN111231976B (en) Vehicle state estimation method based on variable step length
CN111361571B (en) Method for simultaneously estimating vehicle mass center slip angle and disturbance based on GPI
CN108413923B (en) Vehicle roll angle and pitch angle estimation method based on robust hybrid filtering
Huang et al. Estimation of sideslip angle based on extended Kalman filter
CN113978476B (en) Wire-controlled automobile tire lateral force estimation method considering sensor data loss

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20210702