CN114690230A - Automatic driving vehicle navigation method based on visual inertia SLAM - Google Patents

Automatic driving vehicle navigation method based on visual inertia SLAM Download PDF

Info

Publication number
CN114690230A
CN114690230A CN202210480382.7A CN202210480382A CN114690230A CN 114690230 A CN114690230 A CN 114690230A CN 202210480382 A CN202210480382 A CN 202210480382A CN 114690230 A CN114690230 A CN 114690230A
Authority
CN
China
Prior art keywords
imu
representing
camera
dynamic target
bias
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
CN202210480382.7A
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.)
Guilin University of Electronic Technology
Original Assignee
Guilin University of Electronic Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guilin University of Electronic Technology filed Critical Guilin University of Electronic Technology
Priority to CN202210480382.7A priority Critical patent/CN114690230A/en
Publication of CN114690230A publication Critical patent/CN114690230A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
    • 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
    • G01C21/1656—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 with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C3/00—Measuring distances in line of sight; Optical rangefinders
    • G01C3/02—Details

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

The invention discloses an automatic driving vehicle navigation method based on visual inertia SLAM, in particular to a navigation method combining the visual inertia SLAM with a target detection network. The method aims to remove the influence of a dynamic target in a road scene and solve the problem that the navigation precision of an automatic driving vehicle is reduced in a dynamic environment. The method is realized by the following steps: the method comprises the following steps: the vehicle-mounted sensor collects images, acceleration and angular velocity, and then data are aligned; step two: detecting a dynamic target, and removing the influence of the dynamic target on pose estimation; step three: IMU pre-integration, and integration is not required to be carried out again after optimization and updating every time, so that the calculated amount is reduced; step four: initializing and calculating each parameter; step five: and calculating marginalized prior information, IMU measurement residual and visual observation residual by close coupling optimization. The invention is suitable for the visual inertial navigation system.

Description

Automatic driving vehicle navigation method based on visual inertia SLAM
Technical Field
The invention relates to the technical field of automatic driving vehicle navigation, in particular to an automatic driving vehicle navigation method based on visual inertia SLAM.
Background
There are many problems and challenges facing the application of visual inertial SLAM to autonomous vehicles, and there are still some key technical issues to be solved. The performance of e.g. visual inertial SLAM depends to a large extent on the assumption that the surrounding visual features are static, so that the navigation accuracy of the system in a dynamic environment drops drastically. In order to improve the positioning and state estimation precision of the automatic driving vehicle in a dynamic environment, the invention provides a visual inertial navigation system combined with dynamic target detection, and provides theoretical guidance and a solution for applying the visual inertial SLAM to the automatic driving vehicle.
In order to solve the problems of reduced pose estimation precision, track drift and even navigation failure of the visual inertia SLAM in a dynamic environment, a plurality of excellent solutions are provided at home and abroad. The existing better solution method is to combine the neural network-semantic segmentation to remove the dynamic feature points, but the calculation amount is large, so that the real-time requirement cannot be met. Another solution is to use a random sample consensus algorithm to remove outliers, but when there are too many outliers in the scene, it can cause drift and even fail. The last solution is to detect moving objects with depth cameras and then eliminate the feature points on the objects, but the limited cameras also limit the application scenarios and onboard sensors. The above solutions still do not enable the visual inertial SLAM to be well applied to autonomous vehicles due to limitations in real-time, computational, sensors, etc.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides the automatic driving vehicle navigation method based on the visual inertia SLAM, and the navigation precision of the automatic driving vehicle is improved.
The invention is realized by adopting the following technical scheme:
the automatic driving vehicle navigation method based on visual inertia SLAM is realized by adopting the following steps:
firstly, a vehicle-mounted binocular camera acquires a gray image of a road in front, an IMU sensor acquires the acceleration and the angular velocity of a vehicle, and a master control aligns data;
training a dynamic target detection model, detecting a dynamic target in a road scene, extracting a target boundary box, and removing feature points in the boundary box;
eliminating noise and deviation of the IMU, performing pre-integration on IMU data, calculating an observed value of the IMU data, a covariance matrix and a Jacobian matrix of residual errors, and then correcting the deviation;
initializing and calculating an absolute scale, a gyroscope bias, an acceleration bias, a gravity acceleration and a speed of each IMU at the moment;
and step five, after initialization, executing tight coupling optimization based on a sliding window to obtain a state estimation value with higher precision and better robustness.
The invention has the following effective effects: firstly, the time calibration is carried out on the acquired data, and the pose estimation precision is improved; the invention pre-integrates IMU data, and reduces the calculation amount consumed during integration. Secondly, the invention detects the dynamic target in the road environment based on the dynamic target detection model proposed by the YOLOv5 network training. Compared with a model carried by YOLOv5, the model only detects dynamic targets such as automobiles, trucks, motorcycles, pedestrians and the like in a road scene, reduces the size of the model and accelerates the speed of recognizing the dynamic targets. The model is obtained by training the BD100K data set, and through a large amount of debugging and parameter optimization, compared with a model carried by YOLOv5, the dynamic target detection model reduces the model amount by about 70%, and the target detection speed is averagely improved by 63%. The dynamic target detection model trained by the invention can detect a dynamic target in a road environment, and after a target boundary frame is extracted by using the model, a characteristic point tracking module at the front end removes characteristic points in the target boundary frame, so that the aim of eliminating dynamic interference is fulfilled, and the navigation precision of the automatic driving vehicle is improved.
The effect of the invention is verified by the following experiment:
compared with the VINS-Fusion, the method has the functions of closed loop, reloading, high-precision map multiplexing and the like. The present invention has evaluated the proposed method in a common KITTI dataset and in the real world. To align the estimated trajectory with the ground truth, the present invention uses the method of horns's. The method evaluates the accuracy of navigation through the absolute pose error and the absolute track error. The absolute pose error and the absolute trajectory error are calculated by the EVO tool. The invention adopts the absolute track root mean square error to carry out quantitative evaluation. The absolute track error compares the absolute distance between the estimate and the translation component of the ground truth track. The absolute trajectory error at time step i is calculated as follows:
Figure BDA0003627372500000021
where T represents a transformation that aligns two tracks; g represents a real ground; e represents the estimated trajectory.
For a sequence of N poses, the absolute trajectory root mean square error of the absolute trajectory error is calculated as follows:
Figure BDA0003627372500000031
all values were obtained through five experiments considering the uncertainty of the system, and show the median result of the estimated trajectory accuracy. The present invention uses the KITTI data set to evaluate the proposed method, which is suitable for evaluating the performance of visual inertial SLAM in autonomous vehicles. In this dataset, a real ground is provided for sequences 00-10. In order to provide accurate ground truth, the lidar, camera and GPS are calibrated and synchronized. The KITTI data set is collected from various scene data in the real world, such as rural, highway and urban. The present invention primarily considers sequences 00 and 05-07 because there are moving vehicles and pedestrians in these urban sequences.
In real-world experiments, the present invention utilizes a homemade sensor suite to demonstrate the proposed method. The invention drives the automobile in an outdoor high dynamic environment and uses four different combinations for state estimation. In order to carry out error evaluation on the experiment, the control point is arranged through the RTK-GPS sensor and serves as a real ground point. To obtain the absolute error, the present invention aligns the estimated trajectory with the control point.
Experiments on KITTI data sets and the real world indicate that the method provided by the invention improves the positioning precision by about 40% in a large-scale dynamic urban environment compared with VINS-Fusion. The comparison experiment result shows that the method improves the accuracy and the robustness in a dynamic environment.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the embodiments or the prior art descriptions will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and other drawings can be obtained by those skilled in the art without creative efforts.
FIG. 1 is a schematic flow chart of steps one to five in the present invention.
FIG. 2 is the motion trajectory and dense point cloud of the present invention in a KITTI data set during the experiment.
FIG. 3 is a real world experimental set-up of the present invention in an experiment.
FIG. 4 is a plot of the root mean square error of the absolute track versus the absolute track error of the present invention and conventional methods.
Detailed Description
The present invention will be described in detail with reference to specific examples.
The automatic driving vehicle navigation method based on the visual inertia SLAM is realized by adopting the following steps, specifically the flow shown in figure 1:
firstly, a vehicle-mounted binocular camera acquires a gray image of a road in front, an IMU sensor acquires acceleration and angular velocity of a vehicle, and a master control aligns data.
And step two, training a dynamic target detection model, detecting a dynamic target in a road scene, extracting a target boundary box, and removing feature points in the boundary box.
And step three, eliminating the noise and deviation of the IMU, performing pre-integration on the IMU data, calculating an observed value of the IMU data, a covariance matrix and a Jacobian matrix of residual errors, and then correcting the deviation.
And step four, initializing and calculating the absolute scale, the gyroscope bias, the acceleration bias, the gravity acceleration and the speed of each IMU at the moment.
And step five, after initialization, executing tight coupling optimization based on a sliding window to obtain a state estimation value with higher precision and better robustness.
In the first step, the specific steps include:
(1) the vehicle-mounted binocular camera acquires left and right target gray level images at the frequency of 10 Hz;
(2) the vehicle-mounted IMU acquires the three-axis acceleration and the angular velocity of the vehicle body at the frequency of 100 Hz;
(3) the master control aligns data; due to the trigger delay, transmission delay, and unsynchronized clocks, the generated timestamps are not equal to the actual sampling time, resulting in time misalignment between the camera and the IMU. The alignment data is calculated as:
tIMU＝tcam+td；
wherein, tIMUAnd tcamTimestamps representing the IMU and the camera, respectively; t is tdRepresenting the time offset of an unknown constant.
In the second step, the concrete steps include:
(1) and training a dynamic target detection model. The self-trained neural network model can detect a specific dynamic target more accurately and quickly. The training images are passed to a neural network to create a custom trained detector. The present invention prepares a training set of BDDs 100K to train a dynamic object detection model. The dynamic target detection model was trained using the YOLOv5 network. Like other neural networks, the YOLO network also requires a large number of images to be labeled and sent into the model to train the parameters of the neural network. The dynamic objects detected by the dynamic object detection model include people, cars, motorcycles, buses, and trucks.
The accuracy and the running speed of the dynamic target detection model have decisive influence on the overall result. The invention does much work to improve the accuracy of the dynamic target detection model. In practical application, adding the dynamic target detection model to the visual inertia SLAM increases the calculation amount of the algorithm, but does not influence the real-time performance of the system.
(2) Lifting deviceAnd taking a bounding box. The dynamic object detection model provides classes and two-dimensional bounding boxes of dynamic objects. The dynamic object detection model requires three steps to obtain the bounding box of the dynamic object. First, the entire image is divided into equal sized grid cells, and the network runs on all combined cells simultaneously. Second, the network predicts the bounding box of each cell in the output feature map. Finally, in order to obtain the actual coordinate values of the predicted bounding box with respect to the original image, the algorithm will be (b)x,by,bw,bh) Divided by the size of the feature map and multiplied by the size of the original image. The calculation formula of the prediction bounding box is as follows:
Figure BDA0003627372500000051
wherein (b)x,by) Coordinates representing each grid cell at the prediction center; (b)w,bh) Representing the width and height of the bounding box; (c)x,cy) Representing the upper left coordinates of the grid cells in the feature map; p is a radical of formulawAnd phRespectively representing the width and height of the predicted bounding box.
In the third step, the concrete steps include:
(1) the IMU is noise and offset cancelled. The measured value is influenced by acceleration bias, gyroscope bias and noise, and the calculation formula for eliminating the noise and the bias is as follows:
Figure BDA0003627372500000052
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003627372500000053
and
Figure BDA0003627372500000054
random walk noise representing the accelerometer and gyroscope, respectively; a istAn ideal value representing the accelerometer measurements; omegatRepresenting an ideal value of a gyroscope measurement;
Figure BDA0003627372500000055
a rotation matrix representing an IMU coordinate system to a camera coordinate system; gwRepresenting the gravity vector. Acceleration noise naAnd gyroscope noise nwIs gaussian white noise and is expressed as:
Figure BDA0003627372500000056
(2) pre-integrating IMU measurement values; for two consecutive frames bkAnd bk+1The time interval t needs to be setk,tk+1]Integrating the inertial measurement into a local frame bkIn (1), the calculation formula is as follows:
Figure BDA0003627372500000061
wherein the content of the first and second substances,
Figure BDA0003627372500000062
representing a position pre-integration;
Figure BDA0003627372500000063
representing a velocity pre-integration;
Figure BDA0003627372500000064
representing a rotational pre-integration;
Figure BDA0003627372500000065
(3) and (5) correcting the deviation. If the predicted deviation varies little, it will be adjusted by a first order approximation of the deviation
Figure BDA0003627372500000066
And
Figure BDA0003627372500000067
the offset correction is calculated asThe following:
Figure BDA0003627372500000068
in the fourth step, the concrete steps include:
(1) the pose is converted from the camera frame into an IMU frame. The calculation formula is as follows:
Figure BDA0003627372500000069
wherein s is a proportional parameter to be solved;
Figure BDA00036273725000000610
and
Figure BDA00036273725000000611
representing the external reference between the camera and the IMU.
(2) Calibrating the gyroscope bias; initial calibration of the gyroscope bias is obtained by linearizing the IMU pre-integration with respect to the bias of the gyroscope and minimizing the cost function. The cost function is calculated by the formula:
Figure BDA00036273725000000612
wherein the content of the first and second substances,
Figure BDA00036273725000000613
indicating the index of all frames in the window.
(3) The velocity, gravity vector and metric scale are initialized. The velocity, gravity vector and metric vector are shown in the following equations:
Figure BDA0003627372500000071
wherein the content of the first and second substances,
Figure BDA0003627372500000072
representing the velocity of the IMU in the k-th image; g represents a gravity vector.
Two consecutive frames b in the windowkAnd bk+1The conversion formula of (1) is as follows:
Figure BDA0003627372500000073
the following linear model was obtained:
Figure BDA0003627372500000074
wherein the content of the first and second substances,
Figure BDA0003627372500000075
Δtkis the time interval between two successive frames.
By solving the linear least square problem, the velocity of each frame in the window is obtained, and the calculation formula is as follows:
Figure BDA0003627372500000076
in the fifth step, the concrete steps include:
(1) a cost function; for cameras and IMUs, the overall state that needs to be estimated is defined as:
Figure BDA0003627372500000077
wherein p represents an estimated position; r represents the estimated direction; x is the number ofcamRepresenting a camera-related state vector; x is the number ofimuRepresenting IMU-related state vectors; λ represents the depth of the feature; v represents a velocity vector; baRepresenting an acceleration bias; bgRepresenting the gyroscope bias.
After defining the state to be estimated, performing state estimation, wherein the state estimation is a maximum likelihood estimation problem and is defined as the following steps:
Figure BDA0003627372500000078
wherein S represents a measurement value set; x ═ x0,x1,…,xn}，
Figure BDA0003627372500000079
Figure BDA00036273725000000710
And
Figure BDA00036273725000000711
position vector and direction quaternion respectively;
Figure BDA00036273725000000712
the uncertainty representing the measured value is a gaussian distribution h (·).
The negative log-likelihood function of the above equation is:
Figure BDA0003627372500000081
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003627372500000082
Figure BDA0003627372500000083
information matrix representing a gaussian distribution
Figure BDA0003627372500000084
And
Figure BDA0003627372500000085
h (-) represents the sensor model.
The state estimate is then converted to a non-linear least squares problem. Solving the nonlinear least squares problem requires factoring the camera and IMU sensors.
(2) Camera and IMU sensor factors. The camera factor consists of the features of each frame of image. The residual calculation formula observed in the image t is:
Figure BDA0003627372500000086
wherein, picAnd
Figure BDA0003627372500000087
projection and back projection functions of the camera model, respectively; t represents a 4x4 homogeneous transformation matrix;
Figure BDA0003627372500000088
a transformation matrix representing the IMU center to camera center of the off-line calibration.
The IMU residual error calculation formula is:
Figure BDA0003627372500000089
wherein α represents a position; β represents a velocity; γ represents rotation;
Figure BDA00036273725000000810
negative value operation exclusively for non-linear rotation; g represents a gravity vector equal to 9.81.
(3) And (6) optimizing. And (3) after the camera factor and the IMU factor are obtained through the step (2), optimizing the nonlinear least square problem. The cost function is calculated by the formula:
Figure BDA00036273725000000811
wherein J represents each factor relative to the current state
Figure BDA00036273725000000812
A jacobian matrix.
After nonlinear approximation, the above equation yields a closed solution of δ χ. The derived calculation formula for the solution is as follows:
Figure BDA00036273725000000813
(4) and (5) marginating. To reduce the amount of computation, marginalization is introduced without losing useful information. Summarizing all marginalization factors by the above equation, resulting in a new H and b order of the rearranged states, the following relationship is obtained:
Figure BDA0003627372500000091
wherein the content of the first and second substances,
Figure BDA0003627372500000095
representing a set of states that are marginalized; xrRepresenting the remaining state set.
The formula for marginalization is as follows:
Figure BDA0003627372500000092
obtaining a new prior H of the residual state by the above formulapAnd bp. After prior information about the current state is obtained, a Bayesian rule is used to compute the posterior as the product of the likelihood and the prior:
Figure BDA0003627372500000093
then the state estimation becomes the maximum a posteriori problem, and the calculation formula of the maximum a posteriori problem is as follows:
Figure BDA0003627372500000094
fig. 2 is a sparse map and a motion trajectory constructed by the method proposed by the present invention. The device for collecting data set in real world of the present invention is shown in fig. 3, and fig. 3 includes a binocular depth camera (MYNTEYE-S1030), an Inertial Measurement Unit (IMU) and a GPS sensor (GPS-U-Blox). After the data set is obtained, the precision verification of the algorithm is carried out, the precision comparison result is shown in fig. 4, and the traditional algorithm in fig. 4 is VINS-Fusion.
While the invention has been described with reference to a preferred embodiment, it will be understood by those skilled in the art that various changes in form and detail may be made therein without departing from the spirit and scope of the invention as defined by the appended claims.

Claims (6)

1. An automatic driving vehicle navigation method based on visual inertia SLAM is characterized by comprising the following steps:
firstly, a vehicle-mounted binocular camera acquires a gray image of a road in front, an IMU sensor acquires the acceleration and the angular velocity of a vehicle, and a master control aligns data;
training a dynamic target detection model, detecting a dynamic target in a road scene, extracting a target boundary box, and removing feature points in the boundary box;
eliminating noise and deviation of the IMU, performing pre-integration on IMU data, calculating an observed value of the IMU data, a covariance matrix and a Jacobian matrix of residual errors, and then correcting the deviation;
initializing and calculating an absolute scale, a gyroscope bias, an acceleration bias, a gravity acceleration and a speed of each IMU at the moment;
and step five, after initialization, executing tight coupling optimization based on a sliding window to obtain a state estimation value with higher precision and better robustness.
2. The visual inertial SLAM-based autonomous vehicle navigation method of claim 1, wherein: in the first step, the specific steps include:
(1) the vehicle-mounted binocular camera collects left and right target gray level images at the frequency of 10 Hz;
(2) the vehicle-mounted IMU acquires the three-axis acceleration and the angular velocity of the vehicle body at the frequency of 100 Hz;
(3) the master control aligns data; due to trigger delay, transmission delay and an unsynchronized clock, the generated timestamp is not equal to the actual sampling time, resulting in time misalignment between the camera and the IMU; the alignment data is calculated as:
tIMU＝tcam+td；
wherein, tIMUAnd tcamTimestamps representing the IMU and the camera, respectively; t is tdRepresenting the time offset of an unknown constant.
3. The visual inertial SLAM-based autonomous vehicle navigation method of claim 1, wherein: in the second step, the concrete steps include:
(1) training a dynamic target detection model; the self-trained neural network model can detect a specific dynamic target more accurately and more quickly; the training images are passed to a neural network to create a custom trained detector; the invention prepares a BDD100K training set to train a dynamic target detection model; the dynamic target detection model is trained by using a YOLOv5 network; like other neural networks, the YOLO network also needs to label a large number of images and send them to the model to train the parameters of the neural network; the dynamic targets detected by the dynamic target detection model comprise people, automobiles, motorcycles, buses and trucks;
the accuracy and the running speed of the dynamic target detection model have decisive influence on the overall result; the invention does a lot of work to improve the accuracy of the dynamic target detection model; in practical application, adding the dynamic target detection model into the visual inertia SLAM can increase the calculation amount of the algorithm, but cannot influence the real-time performance of the system;
(2) extracting a bounding box; the dynamic target detection model provides a class and a two-dimensional bounding box of a dynamic target; the dynamic target detection model requires three steps to obtain the bounding box of the dynamic target; first, the entire image is divided into equal sized grid cells, and the network runs on all the combined cells simultaneously; second, the network predicts each cell in the output feature mapA bounding box; finally, in order to obtain the actual coordinate values of the predicted bounding box with respect to the original image, the algorithm will be (b)x,by,bw,bh) Dividing the size of the feature graph by the size of the original graph, and multiplying the size of the original graph by the size of the feature graph; the calculation formula of the prediction bounding box is as follows:
Figure FDA0003627372490000021
wherein (b)x,by) Coordinates representing each grid cell at the prediction center; (b)w,bh) Representing the width and height of the bounding box; (c)x,cy) Representing the upper left coordinates of the grid cells in the feature map; p is a radical ofwAnd phRespectively representing the width and height of the predicted bounding box.
4. The visual inertial SLAM-based autonomous vehicle navigation method of claim 1, wherein: in the third step, the concrete steps include:
(1) eliminating noise and bias of the IMU; the measured value is influenced by acceleration bias, gyroscope bias and noise, and the calculation formula for eliminating the noise and the bias is as follows:
Figure FDA0003627372490000022
wherein the content of the first and second substances,
Figure FDA0003627372490000023
and
Figure FDA0003627372490000024
random walk noise representing the accelerometer and gyroscope, respectively; a istAn ideal value representing the accelerometer measurements; omegatRepresenting an ideal value of a gyroscope measurement;
Figure FDA0003627372490000025
a rotation matrix representing an IMU coordinate system to a camera coordinate system; gwRepresenting a gravity vector; acceleration noise naAnd gyroscope noise nwIs gaussian white noise and is expressed as:
Figure FDA0003627372490000026
(2) pre-integrating IMU measurement values; for two consecutive frames bkAnd bk+1The time interval t needs to be setk,tk+1]Integrating the inertial measurement into a local frame bkIn (1), the calculation formula is as follows:
Figure FDA0003627372490000031
wherein the content of the first and second substances,
Figure FDA0003627372490000032
representing a position pre-integration;
Figure FDA0003627372490000033
representing a velocity pre-integration;
Figure FDA0003627372490000034
representing a rotational pre-integration;
Figure FDA0003627372490000035
(3) correcting deviation; if the predicted deviation varies little, it will be adjusted by a first order approximation of the deviation
Figure FDA0003627372490000036
And
Figure FDA0003627372490000037
the calculation formula is as follows:
Figure FDA0003627372490000038
5. the visual inertial SLAM-based autonomous vehicle navigation method of claim 1, wherein: in the fourth step, the concrete steps include:
(1) converting the pose from the camera frame into an IMU frame; the calculation formula is as follows:
Figure FDA0003627372490000039
wherein s is a proportional parameter to be solved;
Figure FDA00036273724900000310
and
Figure FDA00036273724900000311
representing external parameters between the camera and the IMU;
(2) calibrating the gyroscope bias; obtaining an initial calibration of the gyroscope bias by linearizing the IMU pre-integration for the bias with respect to the gyroscope and minimizing a cost function; the cost function is calculated by the formula:
Figure FDA00036273724900000312
wherein, B represents the index of all frames in the window;
(3) initializing a speed, a gravity vector and a measurement proportion; the velocity, gravity vector and vector of the metric ratio are shown in the following equations:
Figure FDA0003627372490000041
wherein the content of the first and second substances,
Figure FDA0003627372490000042
representing the velocity of the IMU in the k-th image; g represents a gravity vector;
two consecutive frames b in the windowkAnd bk+1The conversion formula of (c) is:
Figure FDA0003627372490000043
the following linear model was obtained:
Figure FDA0003627372490000044
wherein the content of the first and second substances,
Figure FDA0003627372490000045
Δtkis the time interval between two successive frames;
by solving the linear least square problem, the velocity of each frame in the window is obtained, and the calculation formula is as follows:
Figure FDA0003627372490000046
6. the visual inertial SLAM-based autonomous vehicle navigation method of claim 1, wherein: in the fifth step, the concrete steps include:
(1) a cost function; for cameras and IMUs, the overall state that needs to be estimated is defined as:
Figure FDA0003627372490000047
wherein p represents an estimated position; r represents the estimated direction; x is the number ofcamIndicating camera relevanceA state vector; x is the number ofimuRepresenting IMU-related state vectors; λ represents the depth of the feature; v represents a velocity vector; baRepresenting an acceleration bias; bgRepresenting a gyroscope bias;
after defining the state to be estimated, performing state estimation, wherein the state estimation is a maximum likelihood estimation problem and is defined as the following steps:
Figure FDA0003627372490000048
wherein S represents a measurement value set; x ═ x0,x1,…,xn}，
Figure FDA0003627372490000049
Figure FDA00036273724900000410
And
Figure FDA00036273724900000411
position vector and direction quaternion respectively;
Figure FDA0003627372490000051
the uncertainty representing the measurement is gaussian distributed h (·);
the negative log-likelihood function of the above equation is:
Figure FDA0003627372490000052
wherein the content of the first and second substances,
Figure FDA0003627372490000053
Figure FDA0003627372490000054
information matrix representing a gaussian distribution
Figure FDA0003627372490000055
And
Figure FDA0003627372490000056
h (-) represents the sensor model;
then, converting the state estimation into a nonlinear least square problem; solving the nonlinear least squares problem requires solving factors of the camera and the IMU sensor;
(2) camera and IMU sensor factors; the camera factor consists of the characteristics of each frame of image; the residual calculation formula observed in the image t is:
Figure FDA0003627372490000057
wherein, picAnd
Figure FDA0003627372490000058
projection and back projection functions of the camera model, respectively; t represents a 4x4 homogeneous transformation matrix;
Figure FDA0003627372490000059
a transformation matrix representing an offline calibrated IMU center to camera center;
the IMU residual error calculation formula is as follows:
Figure FDA00036273724900000510
wherein α represents a position; β represents a velocity; γ represents rotation;
Figure FDA00036273724900000511
negative value operation exclusively for non-linear rotation; g represents a gravity vector equal to 9.81;
(3) optimizing; after the camera factor and the IMU factor are obtained through the step (2), the nonlinear least square problem is optimized; the cost function is calculated by the formula:
Figure FDA00036273724900000512
wherein J represents each factor relative to the current state
Figure FDA00036273724900000513
A jacobian matrix of;
after nonlinear approximation, the above formula obtains a closed solution of δ χ; the derived calculation of the solution is as follows:
Figure FDA0003627372490000061
(4) marginalizing; in order to reduce the calculation amount, marginalization is introduced on the premise of not losing useful information; summarizing all marginalization factors by the above equation, resulting in a new H and b order of the rearranged states, the following relationship is obtained:
Figure FDA0003627372490000062
wherein, χmRepresenting a set of states that are marginalized; chi shaperRepresenting a set of remaining states;
the formula for marginalization is as follows:
Figure FDA0003627372490000063
obtaining a new prior H of the residual state by the above formulapAnd bp(ii) a After prior information about the current state is obtained, a Bayesian rule is used to compute the posterior as the product of the likelihood and the prior: p (χ | z) oc |. p (z |. χ) p (χ); then the state estimation becomes the maximum a posteriori problem, and the calculation formula of the maximum a posteriori problem is as follows:
Figure FDA0003627372490000064
CN202210480382.7A 2022-05-05 2022-05-05 Automatic driving vehicle navigation method based on visual inertia SLAM Pending CN114690230A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210480382.7A CN114690230A (en) 2022-05-05 2022-05-05 Automatic driving vehicle navigation method based on visual inertia SLAM

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210480382.7A CN114690230A (en) 2022-05-05 2022-05-05 Automatic driving vehicle navigation method based on visual inertia SLAM

Publications (1)

Publication Number Publication Date
CN114690230A true CN114690230A (en) 2022-07-01

Family

ID=82145617

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210480382.7A Pending CN114690230A (en) 2022-05-05 2022-05-05 Automatic driving vehicle navigation method based on visual inertia SLAM

Country Status (1)

Country Link
CN (1) CN114690230A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115688610A (en) * 2022-12-27 2023-02-03 泉州装备制造研究所 Wireless electromagnetic six-dimensional positioning method and system, storage medium and electronic equipment

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115688610A (en) * 2022-12-27 2023-02-03 泉州装备制造研究所 Wireless electromagnetic six-dimensional positioning method and system, storage medium and electronic equipment
CN115688610B (en) * 2022-12-27 2023-08-15 泉州装备制造研究所 Wireless electromagnetic six-dimensional positioning method, system, storage medium and electronic equipment

Similar Documents

Publication Publication Date Title
CN112347840B (en) Vision sensor laser radar integrated unmanned aerial vehicle positioning and image building device and method
CN108731670B (en) Inertial/visual odometer integrated navigation positioning method based on measurement model optimization
CN112083725B (en) Structure-shared multi-sensor fusion positioning system for automatic driving vehicle
CN110726406A (en) Improved nonlinear optimization monocular inertial navigation SLAM method
CN114526745A (en) Drawing establishing method and system for tightly-coupled laser radar and inertial odometer
CN113052908A (en) Mobile robot pose estimation method based on multi-sensor data fusion
CN114758504B (en) Online vehicle overspeed early warning method and system based on filtering correction
CN114019552A (en) Bayesian multi-sensor error constraint-based location reliability optimization method
CN113776519B (en) AGV vehicle mapping and autonomous navigation obstacle avoidance method under lightless dynamic open environment
CN113920198B (en) Coarse-to-fine multi-sensor fusion positioning method based on semantic edge alignment
CN115479602A (en) Visual inertial odometer method fusing event and distance
CN114690229A (en) GPS-fused mobile robot visual inertial navigation method
CN110929402A (en) Probabilistic terrain estimation method based on uncertain analysis
CN115540850A (en) Unmanned vehicle mapping method combining laser radar and acceleration sensor
CN114754768A (en) Visual inertial navigation method based on point-line fusion
CN114690230A (en) Automatic driving vehicle navigation method based on visual inertia SLAM
CN114608568A (en) Multi-sensor-based information instant fusion positioning method
Peng et al. Vehicle odometry with camera-lidar-IMU information fusion and factor-graph optimization
CN113759364A (en) Millimeter wave radar continuous positioning method and device based on laser map
CN112945233A (en) Global drift-free autonomous robot simultaneous positioning and map building method
CN112862818A (en) Underground parking lot vehicle positioning method combining inertial sensor and multi-fisheye camera
CN115218889A (en) Multi-sensor indoor positioning method based on dotted line feature fusion
CN114964276A (en) Dynamic vision SLAM method fusing inertial navigation
CN115453599A (en) Multi-sensor-cooperated pipeline robot accurate positioning method
Liu et al. 6-DOF motion estimation using optical flow based on dual cameras

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication