A multi-vehicle cooperative navigation method based on factor graph optimization

Abstract

The application relates to a multi-vehicle cooperative navigation method based on factor graph optimization, which comprises the following steps: setting initial information of a vehicle, collecting and processing data information through a vehicle-mounted sensor; collecting vehicle acceleration and angular velocity information through an IMU, and performing pre-integration processing on the data; collecting vehicle speed information through a wheeled odometer; measuring the relative distance and plane angle information between vehicles through UWB, and then obtaining the observation between two vehicles; fusing the data information through a factor graph optimization method, obtaining a state compensation amount by solving a nonlinear least square solution, recalculating after compensating the system state amount, and finally performing multi-vehicle cooperative navigation according to the optimization value. Through the use of IMU, OD and UWB sensors and the use of a reasonable data processing mode and fusion method, the application can greatly prolong the long-time stable work of the system without the input of absolute navigation information, and realizes the positioning of the vehicle in passive autonomous navigation.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of vehicle positioning, in particular to a multi-vehicle cooperative navigation method based on factor graph optimization. BACKGROUND

[0002] At present, the most commonly used method for vehicle positioning is GNSS (Global Navigation Satellite System), but with the development of robot technology, such as the cargo robots in logistics warehouses and other vehicles working indoors, and other working environments such as underwater and dense forests where GNSS signals are blocked, GPS signals cannot be used, and only vehicle-mounted IMU (Inertial Motion Unit) can be used for positioning. The error will quickly accumulate over time, causing the system to diverge and making it impossible to use.

[0003] In addition, the accuracy of the navigation system is also affected by the filtering algorithm. Currently, KF (Kalman Filter) is mostly used in the navigation field to fuse the read data. KF assumes that the system is a first-order Markov process, and the current state is only related to the previous state, ignoring the previous state information. Moreover, KF is only applicable to linear systems, and EKF (Extended Kalman Filter) for non-linear systems will bring additional errors. In addition, KF is difficult to handle the asynchronous problem of multiple sensor data, and the filtering performance of KF depends on the quality of the prior assumptions about the covariance matrix (Q and P), which is not reliable in the case of insufficient system model knowledge.

[0004] It should be noted that the information disclosed in the above background section is only used to strengthen the understanding of the background of the present disclosure, and therefore can include information that does not constitute prior art known to those of ordinary skill in the art. SUMMARY

[0005] The purpose of the present application is to overcome the shortcomings of the prior art and provide a multi-vehicle cooperative navigation method based on factor graph optimization, which solves the problems existing in the prior art.

[0006] The purpose of the present application is achieved by the following technical solution: a multi-vehicle cooperative navigation method based on factor graph optimization, the multi-vehicle cooperative navigation method comprising:

[0007] S1, setting the attitude, speed, position and sensor line covariance matrix of the vehicle, collecting and processing data information through the vehicle-mounted sensor;

[0008] S2, collecting vehicle acceleration and angular velocity information through IMU, and performing pre-integration processing on the data obtained by IMU to obtain low-frequency data;

[0009] S3, collecting vehicle speed information through a wheeled odometer, and adjusting the odometer covariance matrix by judging whether the vehicle is turning;

[0010] S4, the relative distance between the vehicles is measured by UWB, and the plane angle information is obtained, and then the observation quantity between the two vehicles is obtained;

[0011] S5, all the collected data information is fused by the factor graph optimization method, and the state compensation quantity is obtained by solving the nonlinear least square solution, the system state quantity is compensated and recalculated until the state compensation quantity is less than the set threshold value, then the optimization is completed, and finally the multi-vehicle cooperative navigation is carried out according to the optimized value.

[0012] The attitude, speed, position and sensor line covariance matrix of the vehicle are set, and the data information collected and processed by the vehicle-mounted sensor specifically includes the following contents:

[0013] The system state quantity of each vehicle i is set as x i =[a,v,p,e,d] T , wherein a = [α, β, γ] represents the n system attitude, v = [V x , V y , V z ] represents the n system speed, p = [P x , P y , P z ] represents the position, e = [e x , e y , e z ] represents the three-axis gyroscope zero offset, and d = [d x , d y , d z ] represents the three-axis accelerometer zero offset.

[0014] The vehicle acceleration and angular velocity information is collected by the IMU, and the data obtained by the IMU is pre-integrated to obtain low-frequency data, which specifically includes the following contents:

[0015] The output of other sensors is set as one frame, and the accelerometer and gyroscope data f b , ω b obtained by the IMU are obtained by pre-integration, the attitude increment of two frames is calculated as Δa = [Δα, Δβ, Δγ], the speed increment is calculated as Δv = [ΔV x , ΔV y , ΔV z ], and the position increment is calculated as Δp = [ΔP x , ΔP y , ΔP z ], and the pre-integration factor of each vehicle i is obtained as:

[0016]

[0017] Wherein, k represents the frame number, and Ω represents the observation quantity covariance matrix.

[0018] The vehicle speed information is collected by the wheel odometer, and the odometer covariance matrix is adjusted by judging whether the vehicle is turning or not, which specifically includes the following contents:

[0019] The vehicle motion mileage is obtained by calculating the number of wheel rotations and the wheel radius, and the vehicle motion speed V in the b system is obtained by differentiating the mileage od ;

[0020] By combining the nonholonomic constraint, the direction finding and skyward velocity of the vehicle during the forward process are set to 0, and the vehicle speed is obtained as [0, V od , 0].

[0021] The odometer covariance matrix is adjusted by judging whether the vehicle is turning or not, and when straight, the covariance matrix is reduced, so that the system trusts the odometer data more, and when turning, the covariance matrix is increased; wherein the condition for judging whether the vehicle is turning or not is: when |ω y |>ω1 and |f y |>f1, turn, wherein ω y is the y-axis angular velocity measured by the IMU, f y is the y-axis acceleration measured by the IMU, i.e. the lateral angular velocity and lateral acceleration, ω1, f1 is the judgment threshold, and the covariance matrix is Ω od ;

[0022] The observation of each vehicle i is The odometer factor is wherein, represents the state of the i-th vehicle at time k.

[0023] The relative distance and plane angle information between the vehicles are measured by UWB, and the observation between the two vehicles is obtained, which includes:

[0024] The relative distance between the two vehicles is measured by UWB as L, and the plane angle is ρ, and the horizontal distance L x and the front and rear distance L y between the two vehicles are obtained by trigonometric functions, when the height difference between the two vehicles is 0, then the observation of the two vehicles i and j is The UWB factor is wherein, represents the relationship between the observation and the state quantity, and × represents the skew-symmetric matrix of the vector, is the rotation matrix from the n system to the b system of the vehicle carrying the UWB base station among the i, j vehicles;

[0025] When the height difference between the two vehicles is not 0, the observation of the two vehicles i and j is

[0026] The height difference between the two vehicles is determined as follows: when the i, j vehicle is or the height difference is determined as not being 0, and vice versa, wherein represents the lateral and upward angular velocity measured by the IMU of the i vehicle, W x , W y represents a threshold value.

[0027] The method for optimizing the factor graph comprises: the maximum a posteriori probability value of the factor graph at the k moment is The state corresponding to the minimum value of the maximum a posteriori probability value is found out by nonlinear least squares, and the optimization is completed.

[0028] The present application has the following advantages: a multi-vehicle cooperative navigation method based on factor graph optimization, by using IMU, OD and UWB sensors and using a reasonable data processing method and fusion method, the system can work stably for a long time without absolute navigation information input, and realize longer time passive autonomous navigation of vehicle positioning. BRIEF DESCRIPTION OF DRAWINGS

[0029] Figure 1 is a flowchart of the present application;

[0030] Figure 2 is a schematic diagram of UWB measurement of vehicle relative distance and plane angle. DETAILED DESCRIPTION

[0031] To make the purposes, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the accompanying drawings of the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments. The components of the embodiments of the present application described and shown in the accompanying drawings can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in conjunction with the accompanying drawings is not intended to limit the protection scope of the claimed present application, but only represents selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work are within the protection scope of the present application. The present application will be further described below in conjunction with the accompanying drawings.

[0032] ​The application relates to a multi-vehicle cooperative navigation method based on factor graph optimization, which uses a UWB (ultra-wideband) module to measure the relative distance and angle between vehicles, provides more information input for a navigation system, makes the system more accurate, and effectively eliminates errors. Vehicle speed information is read by an OD (wheel odometer) to further improve the navigation accuracy of the system, a data fusion method based on factor graph optimization is used, the fusion of data is related to all previous state quantities, and the asynchronous problem of multi-sensor data is easily solved, and prior assumptions of covariance matrices (Q and P) are not required.

[0033] As Figure 1 shown, specifically comprising the following contents:

[0034] S1, defining initial information of the vehicle, including attitude, speed, position, and sensor covariance matrix. Then data information is collected and processed through the vehicle-mounted sensor, wherein the system state quantity of each vehicle i is defined as x i =[a,v,p,e,d] T , a = [alpha, beta, gamma] represents the n system (local coordinate system) attitude, which is expressed by Euler angles; v = [V x , V y , V z ] represents the n system speed; p = [P x , P y , P z ] represents the position, the coordinate system origin is the starting point of the vehicle 1, and the xyz axis points to the east, north and sky; e = [e x , e y , e z ] represents the three-axis gyroscope zero offset; and d = [d x , d y , d z ] represents the three-axis accelerometer zero offset.

[0035] S2, collecting vehicle acceleration and angular velocity information through an IMU (inertial motion unit), since the IMU data acquisition frequency is too high, and the OD (wheel odometer) and UWB (ultra-wideband) acquisition frequencies are low, in order to reduce the real-time calculation pressure, the data obtained by the IMU is pre-integrated to obtain low-frequency data.

[0036] Taking the IMU output frequency of 200hz and the output frequency of other sensors of 2hz as an example, taking one output of other sensors as one frame, through pre-integration, the accelerometer and gyroscope data f b , omega b obtained by the IMU are obtained, the attitude increment Delta a = [Delta alpha, Delta beta, Delta gamma] and the speed increment Delta v = [Delta V x , Delta V y , Delta V z], position increment Δp = [ΔP x , ΔP y , ΔP z ], in the subsequent factor graph optimization, only the state quantity of each frame needs to be optimized.

[0037] The pre-integration factor of each vehicle i is obtained as where k represents the frame number, and Ω represents the observation covariance matrix.

[0038] S3, vehicle speed information is collected by OD (wheeled odometer), as follows, the vehicle movement mileage can be obtained by calculating the number of wheel rotations and the wheel radius, and the mileage is differentiated to obtain the running speed V od of the vehicle in the b system (carrier coordinate system). By combining NHC (nonholonomic constraint), i.e., defining that the vehicle has only forward speed in the forward process, the lateral and skyward speeds are 0, i.e., the vehicle speed is [0, V od , 0], it is easy to know that NHC is more accurate when the vehicle is straight, and when the vehicle is turning, there will be part of the direction-finding and skyward speed. Therefore, by judging whether the vehicle is turning or not, the odometer covariance matrix is adjusted, and when straight, the covariance matrix is reduced, so that the system trusts the odometer data more; when turning, vice versa.

[0039] The condition for judging whether the vehicle is turning or not is as follows: when |ω y | > ω1 and |f y | > f1, the vehicle is turning, where ω y is the y-axis angular velocity measured by the IMU, fy is the y-axis acceleration measured by the IMU, i.e., the lateral angular velocity and lateral acceleration, ω1 and f1 are judgment thresholds, and the covariance matrix is Ω od .

[0040] The observation of each vehicle i is The odometer factor is where where represents the state of the i-th vehicle at time k.

[0041] S4, the relative distance and angle information between vehicles are measured by UWB. Since the angle measuring device is expensive and large in size, the height of the vehicle group is basically consistent, so it is assumed that the vehicle height is consistent, as shown in Figure 2 , the three-dimensional problem is converted into a two-dimensional problem, so only one UWB measurement plane angle ρ is needed between two vehicles, effectively saving hardware devices. After measurement, the data is converted to L x , L y for subsequent calculation.

[0042] When vehicles are traveling on flat surfaces such as roads, assuming the height difference between two vehicles is 0, then the observations for each pair of vehicles i and j are: UWB factor is in This represents the relationship between the observed quantities and the state quantities, where × represents the antisymmetric matrix of the vector. For vehicles i and j carrying UWB base stations (UWB is divided into tags and base stations; depending on actual needs, tags are deployed in some vehicles and base stations are deployed in others), the vehicle rotation matrix is ​​from the n-system to the b-system.

[0043] When the vehicle operates under other conditions, i.e., the height difference is not zero, then we have h array is The first two lines; the judgment conditions are as follows:

[0044] When vehicles i and j or When the height difference is not zero, it is considered non-zero; otherwise, it is zero. W represents the lateral and upward angular velocities measured by the i-vehicle IMU. x W y Represents the threshold.

[0045] S5. After data acquisition is complete, all acquired information needs to be fused using factor graph optimization. This involves solving for the nonlinear least squares solution to obtain the state compensation amount, compensating the system state variables, and recalculating until the state compensation amount is less than a set threshold. This completes one optimization cycle. Details are as follows:

[0046] The maximum a posteriori probability of the factor graph at time k is Find the solution using nonlinear least squares. The state corresponding to the minimum value is now optimized.

[0047] The above description is merely a preferred embodiment of the present invention. It should be understood that the present invention is not limited to the forms disclosed herein and should not be construed as excluding other embodiments. It can be used in various other combinations, modifications, and environments, and can be altered within the scope of the concept described herein through the above teachings or related technologies or knowledge. Modifications and variations made by those skilled in the art that do not depart from the spirit and scope of the present invention should be within the protection scope of the appended claims.

Claims

1. A multi-vehicle cooperative navigation method based on factor graph optimization, characterized in that: The multi-vehicle cooperative navigation method includes: S1. Set the vehicle's attitude, speed, position, and sensor line covariance matrix, and collect and process data information through onboard sensors; S2. Vehicle acceleration and angular velocity information are collected through IMU, and the data obtained from IMU is pre-integrated to obtain low-frequency data; S3. Collect vehicle speed information through wheeled odometers and adjust the odometer covariance matrix by determining whether the vehicle is turning. S4. Measure the relative distance and planar angle between the vehicles using UWB, and then obtain the observational data between the two vehicles; S5. All collected data information is fused using the factor graph optimization method, and the state compensation amount is obtained by solving the nonlinear least squares solution. The system state variables are compensated and recalculated until the state compensation amount is less than the set threshold. Then the optimization is completed. Finally, multi-vehicle cooperative navigation is performed based on the optimized value.

2. The multi-vehicle cooperative navigation method based on factor graph optimization according to claim 1, characterized in that: The process of setting the vehicle's attitude, speed, position, and sensor line covariance matrix, and collecting and processing data information through onboard sensors, specifically includes the following: Set the system state variables of each vehicle i to... ,in Represents the attitude of the n-system. Represents the velocity of the n-system. Indicates location, This represents zero bias in a three-axis gyroscope. This indicates zero bias of the triaxial accelerometer.

3. The multi-vehicle cooperative navigation method based on factor graph optimization according to claim 2, characterized in that: The process of acquiring vehicle acceleration and angular velocity information via IMU and pre-integrating the data obtained from the IMU to obtain low-frequency data specifically includes the following: Other sensors are configured to output one frame at a time, and the accelerometer and gyroscope data obtained from the IMU are obtained through pre-integration. Calculate the pose increment between the two frames as follows: The speed increment is The position increment is The pre-integral factor for each vehicle i is obtained as follows: ； Where k represents the number of frames, Ω imu Let represent the IMU covariance matrix.

4. The multi-vehicle cooperative navigation method based on factor graph optimization according to claim 1, characterized in that: The process of collecting vehicle speed information via a wheeled odometer and adjusting the odometer covariance matrix based on whether the vehicle is turning specifically includes the following: The distance traveled by the vehicle is obtained by calculating the number of wheel rotations and the wheel radius. The speed of the vehicle in the b-frame is obtained by differentiating the distance. ; By incorporating non-holonomic constraints, and setting the vehicle's directional and axial velocities to 0 during its forward movement, the vehicle velocity is obtained as [0, V]. od ,0]; The odometer covariance matrix is ​​adjusted based on whether the vehicle is turning. When driving straight, the covariance matrix is ​​decreased to increase the system's trust in the odometer data; when turning, the covariance matrix is ​​increased. The condition for determining whether the vehicle is turning is: when... At that time, turning, among them f is the y-axis angular velocity measured by the IMU. y The y-axis acceleration measured by the IMU, i.e., lateral angular velocity and lateral acceleration. To determine the threshold, the covariance matrix is: ; Then the observations for each vehicle i are: The odometer factor is ,in, , , This represents the state of the i-th vehicle at time k.

5. A multi-vehicle cooperative navigation method based on factor graph optimization according to claim 4, characterized in that: The process of measuring the relative distance and planar angle information between vehicles via UWB to obtain the observational data between the two vehicles includes: The relative distance between the two vehicles is measured as L using UWB, and the plane angle is... And through the trigonometric function of the horizontal distance L between vehicles x and front-to-back distance L y When the height difference between the two vehicles is 0, the observations of vehicles i and j are as follows: UWB factor is ,in, This represents the relationship between observed quantities and state quantities. The antisymmetric matrix representing vectors, Let i and j be the rotation matrix from the n-system to the b-system for vehicles carrying UWB base stations; When the height difference between the two vehicles is not zero, the observations of vehicles i and j are: ; Determining the height difference between two vehicles includes: when vehicles i and j... When the height difference is not zero, it is considered non-zero; otherwise, it is zero. Represents the lateral and upward angular velocities measured by the IMU of vehicle i. Represents the threshold.

6. The multi-vehicle cooperative navigation method based on factor graph optimization according to claim 5, characterized in that: The method for optimizing the factor graph includes: the maximum posterior probability value of the factor graph at time k is... Find the maximum posterior probability value using nonlinear least squares. The state corresponding to the minimum value is now optimized.

Images