CN114440881A - Unmanned vehicle positioning method integrating multi-source sensor information - Google Patents

Abstract

The invention relates to an unmanned vehicle positioning method fusing multi-source sensor information, which comprises the following steps: step 1: acquiring GPS position information and IMU information of a vehicle to be controlled, and taking centimeter-level precision positioning obtained by combining the vehicle to be controlled with an inertial navigation system and a GPS signal as a true value; step 2: establishing a linear model; and step 3: acquiring an unmanned vehicle positioning algorithm for fusing multi-source sensor information under a linear model, and acquiring a fusion positioning result under the linear model; and 4, step 4: establishing a nonlinear model; and 5: acquiring an unmanned vehicle positioning algorithm for fusing multi-source sensor information under a kinematic model, and acquiring a fusion positioning result under a nonlinear model; step 6: establishing a bicycle model; and 7: compared with the prior art, the unmanned vehicle positioning method has the advantages of improving unmanned vehicle positioning accuracy, being high in applicability and the like.

Description

A method of unmanned vehicle localization based on fusion of multi-source sensor information

technical field

The invention relates to the technical field of automatic driving, in particular to an unmanned vehicle positioning method integrating multi-source sensor information.

Background technique

In recent years, with the growth of China's economy, cars have become more than just convenient means of transportation, and the intelligent networking of cars has become another demand of people. industry attention. Among them, the positioning of unmanned vehicles is the basic module of the research on the field of unmanned vehicles. The use of different sensing sensors requires different research methods. At present, the solution that simply uses GPS to complete the positioning work is characterized by all-weather and large coverage area. However, when the satellite signal is interfered, a large error will occur. In addition, the solution of simply using the IMU to complete the positioning work is because this technology is Relative positioning technology, its positioning error will accumulate more and more over time.

SUMMARY OF THE INVENTION

The purpose of the present invention is to provide an unmanned vehicle positioning method integrating multi-source sensor information in order to overcome the above-mentioned defects of the prior art.

The object of the present invention can be realized through the following technical solutions:

An unmanned vehicle positioning method integrating multi-source sensor information, the method includes the following steps:

Step 1: Obtain the GPS position information and IMU information of the vehicle to be controlled, and use the centimeter-level precision positioning obtained by the vehicle to be controlled in combination with the inertial navigation system and the GPS signal as the true value to verify the fusion positioning accuracy;

Step 2: Establish a linear model according to GPS location information and IMU information;

Step 3: Obtain the unmanned vehicle positioning algorithm fused with multi-source sensor information under the linear model, and obtain the fusion positioning result under the linear model;

Step 4: Build a nonlinear model according to GPS location information and IMU information;

Step 5: Obtain the unmanned vehicle positioning algorithm fused with multi-source sensor information under the nonlinear model, and obtain the fusion positioning result under the nonlinear model;

Step 6: Further deepen the nonlinearity of the nonlinear model and re-model to obtain a bicycle model;

Step 7: Obtain the unmanned vehicle positioning algorithm fused with multi-source sensor information under the bicycle model, and obtain the fusion positioning result under the bicycle model.

In the described step 1, the GPS information is collected through the GNSS antenna installed on the roof, and the IMU information is collected through the inertial navigation unit. The GPS information includes the latitude and longitude information of the vehicle, and the latitude and longitude information of the vehicle can be converted into the abscissa of the vehicle. and ordinate, the IMU information includes the triaxial angular velocity and acceleration of the vehicle.

In the described step 2, the linear model includes a constant velocity model and a constant acceleration model, wherein the expression of the state vector of the constant velocity model is:

Among them, x, y, θ and v are the abscissa, ordinate, vehicle heading angle and speed of the vehicle position at time t, respectively;

The expressions of the state equation and observation equation of the constant velocity model are:

为经过Δt时间后的状态向量，

为由GPS得到的观测值，x_gps为由GPS得到的车辆位置的横坐标，y_gps为由GPS得到的车辆位置的纵坐标；where Δt is the time interval,

is the state vector after Δt time,

is the observation value obtained by GPS, x _gps is the abscissa of the vehicle position obtained by GPS, and y _gps is the ordinate of the vehicle position obtained by GPS;

The expression for the state vector of the constant acceleration model is:

Among them, s and v are the displacement and velocity of the vehicle at time t, respectively;

The expressions of the state equation and observation equation of the constant acceleration model are:

为经过Δt时间后的状态向量，s(t)为t时刻车辆的位移，v(t)为t时刻车辆的速度，

为观测值，即GPS测量得到的位移；where a is the acceleration of the vehicle,

is the state vector after Δt time, s(t) is the displacement of the vehicle at time t, v(t) is the speed of the vehicle at time t,

is the observation value, that is, the displacement obtained by GPS measurement;

The constant acceleration model takes the values measured by the accelerometer as system input:

u(k)=a(k)

Among them, u(k) is the system input, and a(k) is the value measured by the accelerometer.

In the step 3, the method for locating the unmanned vehicle by fusing multi-source sensor information under the linear model is as follows:

Step 301: Obtain more accurate location information of the unmanned vehicle according to the Kalman filter algorithm. The specific process of the Kalman filter algorithm is divided into five links: state prediction, error covariance prediction, Kalman gain update, state update and error covariance update :

表示k时刻状态向量的预测值，

表示k时刻状态向量的最优估计值，

表示k时刻误差协方差的预测值，P_k表示k时刻误差协方差的最优估计值，K_k表示k时刻的卡尔曼增益，Q为过程噪声协方差矩阵，R为观测噪声协方差矩阵，u_k-1表示k-1时刻的系统输入，A表示系统的状态转移矩阵，H表示系统的观测矩阵，I表示单位矩阵，B为控制输入矩阵；in,

represents the predicted value of the state vector at time k,

represents the optimal estimate of the state vector at time k,

is the predicted value of the error covariance at time k, P _k is the optimal estimate of the error covariance at time k, K _k is the Kalman gain at time k, Q is the process noise covariance matrix, R is the observation noise covariance matrix, u _k-1 represents the system input at time k-1, A represents the state transition matrix of the system, H represents the observation matrix of the system, I represents the identity matrix, and B represents the control input matrix;

For the constant speed model, the expressions of the observation matrix H and the state transition matrix A are obtained according to the state equation and the observation equation of the constant speed model:

Among them, Δt is the time interval, and θ is the heading angle of the vehicle;

Initialize the covariance matrix P to the identity matrix, and initialize the process noise matrix Q and the observation noise matrix R:

For the constant acceleration model, the expressions of state transition matrix A, observation matrix H and control input matrix B are obtained according to the state equation, observation value and system input of the constant acceleration model:

H=[1 0]

Among them, Δt is the time interval;

The covariance matrix P is initialized as the identity matrix, and the process noise matrix Q and the observation noise matrix R are initialized as:

r = 1;

Step 302: Compare the obtained optimal estimated value with the true value, and calculate the variance before and after filtering.

In the described step 4, the nonlinear model is a kinematic model based on constant acceleration and angular velocity, and the expression of the state vector of the kinematic model is:

where x _k , y _k , ψ _k , ω _k , v _k and ak are the abscissa, ordinate, heading angle, angular velocity, velocity and acceleration of the vehicle at time _k , respectively;

The expressions of the state equation and observation equation of the kinematic model are:

为k-1时刻车辆的状态向量，v_k-1为k-1时刻车辆的速度，dt为时间间隔，

为观测值，x_g,k、y_g,k、a_a,k和ω_g,k分别为k时刻GPS观测得到的车辆横坐标、纵坐标、加速度计获得加速度以及陀螺仪测得的角速度.in,

is the state vector of the vehicle at time k-1, v _k-1 is the speed of the vehicle at time k-1, dt is the time interval,

are the observed values, x _g,k , y _g,k , a _a,k and ω _g,k are the abscissa and ordinate of the vehicle, the acceleration obtained by the accelerometer, and the angular velocity measured by the gyroscope at time k, respectively.

In the step 5, the process of obtaining the method for locating the unmanned vehicle by fusing multi-source sensor information under the kinematic model specifically includes the following steps:

Step 501: respectively adopt the extended Kalman algorithm, the unscented Kalman algorithm and the particle filter algorithm to perform fusion positioning;

Step 502: Comprehensively compare the three filtering algorithms, and calculate the RMS error to select the optimal positioning algorithm. The calculation formula of the RMS error is:

error _rms = |XX _est |

Among them, error _rms is the error after filtering, X is the true value, including the true value of the abscissa and ordinate of the vehicle, and _Xest is the best estimated value of the abscissa and ordinate of the vehicle after filtering;

Step 503: Select the corresponding filtering algorithm with the smallest RMS error, and obtain the fusion positioning result under the nonlinear model based on the filtering algorithm.

In the described step 501, the specific formula of the extended Kalman filter algorithm includes:

Among them, F _k-1 and H _k are Jacobian matrices respectively, uk is the system input at time _k , and x _k-1 is the state vector at time k-1;

The expressions of the Jacobian matrices F _k-1 and H _k are:

Among them, ψ _k-1 is the heading angle of the vehicle at time k-1, v _k-1 is the speed of the vehicle at time k-1, and dt is the time interval;

The specific filtering process of the unscented Kalman filter algorithm is as follows:

为真值，过程噪声矩阵Q、观测噪声矩阵R以及初始误差协方差矩阵P₀初始化的表达式为：Step 1a: Initialize the state vector x ₀ , the state estimation error covariance P ₀ , the process noise matrix Q and the observation noise matrix R, for the kinematic model, the initial value of the state vector

is the true value, the initialized expressions of the process noise matrix Q, the observation noise matrix R and the initial error covariance matrix P ₀ are:

Step 2a: According to the state vector x _k at the previous moment and the error covariance matrix P _k , the sampling point at the current moment is selected, and the weight is assigned;

Step 3a: Select 2n+1 sampling points, calculate the mean and covariance of all sampling points at the current moment through the state equation of the system, and complete the time update;

Step 4a: measure and update the sampling points through the observation equation of the system;

Step 5a: Calculate the Kalman gain through prediction and measurement update, and complete the filter update;

The particle filter algorithm uses the Monte Carlo method to solve the nonlinear filtering problem. The filtering process of the particle filter algorithm is as follows:

且将粒子的权值

统一设置为1/N；Step 1b: Initialize N particles

and the particle weights

Uniformly set to 1/N;

计算每个粒子的似然概率

将每个粒子的权值

更新为

并进行归一化处理，粒子的权值更新公式为：Step 2b: Update the particles

Calculate the likelihood probability for each particle

the weight of each particle

update to

And normalized, the particle weight update formula is:

为粒子的权值，

为粒子的似然概率，z_k为k时刻系统的观测值，

为k时刻第i个粒子观测方程的输出值，R_k为k时刻的观测噪声协方差矩阵；in,

is the weight of the particle,

is the likelihood probability of the particle, z _k is the observed value of the system at time k,

is the output value of the ith particle observation equation at time k, and R _k is the observation noise covariance matrix at time k;

进行重采样，计算有效粒子数N_eff，以此得到新的粒子群

且粒子的权值

为1/N；Step 3b: Right

Perform resampling and calculate the effective number of particles N _eff to obtain a new particle swarm

and the weight of the particle

is 1/N;

Step 4b: Calculate the filter state estimation and variance estimation at the current moment by using the weights and states of the resampled particles.

In the step 6, the relationship between the speed and the angular velocity in the bicycle model is:

为水平方向速度，

为垂直方向速度，

为航向角的角速度，v为质心车速，ψ为航向角大小，l_r与l_f分别为后悬长度与前悬长度，δ_f与δ_r分别为前轮偏角与后轮偏角；where β is the slip angle,

is the horizontal velocity,

is the vertical velocity,

is the angular velocity of the heading angle, v is the vehicle speed of the center of mass, ψ is the heading angle, l _r and l _f are the length of the rear overhang and the length of the front overhang, respectively, δ _f and δ _r are the front wheel declination angle and the rear wheel declination angle respectively;

The formula for calculating the slip angle β is:

Among them, l _r and l _f are the length of the rear overhang and the length of the front overhang, respectively, and δ _f and δ _r are the front wheel slip angle and the rear wheel slip angle, respectively;

The expression of the state vector of the bicycle model is:

Among them, x _k , y _k , ψ _k , r, b, N, δ _{r, k} and ω _k are the abscissa, ordinate, heading angle, wheel radius, vehicle wheelbase, gear ratio, front The wheel slip angle and wheel rotation angular velocity, wheel radius, vehicle wheelbase and gear ratio remain unchanged, and the front wheel slip angle and wheel rotation angular velocity of the vehicle are input to the system, and dt is the time interval;

The state equation of the bicycle model is:

The observed values for the bicycle model are:

Among them, x _g,k and y _g,k are the abscissa and ordinate of the vehicle observed by GPS at time k, respectively;

The observation equation of the bicycle model is:

In the step 7, the process of obtaining the unmanned vehicle positioning algorithm fused with multi-source sensor information under the bicycle model specifically includes the following steps:

Step 701: Based on the public data set, using the extended Kalman algorithm, the unscented Kalman algorithm and the particle filter algorithm to obtain the best estimated value of the vehicle under the bicycle model respectively;

Step 702: Perform error analysis on the coordinates and heading angle of the vehicle, and calculate the RMS error corresponding to the abscissa, ordinate and heading angle of the vehicle. The calculation formula of the RMS error is:

error _rms = |XX _est |

Among them, error _rms is the filtered error, X is the true value, including the true value of the abscissa and ordinate of the vehicle, and _Xest is the best estimated value of the abscissa, ordinate and heading angle of the vehicle after filtering, respectively ;

Step 703: Perform mean value processing on each RMS error to achieve normalization;

Step 704: Add the three normalized RMS errors of each filtering algorithm to form a score, evaluate the performance and effect of the three filtering algorithms by comparing the scores of the filtering algorithm, and then select the one with the smallest score as the score. optimal positioning algorithm;

Step 705: Obtain the fusion positioning result under the bicycle model based on the optimal positioning algorithm.

取真值，车轮半径r取0.425，车辆轴距b取0.8，齿轮比N取5，初始化过程噪声矩阵Q、观测噪声矩阵R以及初始误差协方差矩阵P₀，其表达式分别为：In the step 701, when the bicycle model is filtered by the filtering algorithm, the initial state vector

Take the true value, the wheel radius r is 0.425, the vehicle wheelbase b is 0.8, the gear ratio N is 5, the initialization process noise matrix Q, the observation noise matrix R and the initial error covariance matrix P ₀ are expressed as:

Compared with the prior art, the present invention has the following advantages:

1. The present invention obtains more accurate positioning results of unmanned vehicles by fusing GPS information and IMU information, and utilizes the complementarity of the two sensor information, thereby improving the positioning accuracy of automatic driving in a simple environment, and then controlling The technical effect of the safe driving of the vehicle;

2. The present invention achieves the technical effect of enhancing the applicability of the unmanned vehicle positioning method by selecting the optimal algorithm according to the nonlinear degree of the system to obtain the optimal positioning result.

Description of drawings

FIG. 1 is a diagram of a linear model used in a method for locating an unmanned vehicle based on fusion of multi-source sensor information proposed by the present invention.

FIG. 2 is an enlarged view of a fusion result for a linear model implemented by an unmanned vehicle positioning method for fusion of multi-source sensor information proposed by the present invention.

FIG. 3 is a diagram of a kinematics model used by an unmanned vehicle positioning method fused with multi-source sensor information proposed by the present invention.

FIG. 4 is an enlarged view of a fusion result for a kinematic model implemented by an unmanned vehicle positioning method that fuses multi-source sensor information according to the present invention.

FIG. 5 is a comparison diagram of filtering effects of three different filtering algorithms for kinematic models implemented by an unmanned vehicle positioning method based on fusion of multi-source sensor information proposed by the present invention.

FIG. 6 is a diagram of a bicycle model used in the method for locating an unmanned vehicle by fusing multi-source sensor information according to the present invention.

FIG. 7 is a result diagram of a fusion result for a bicycle model implemented by an unmanned vehicle positioning method that fuses multi-source sensor information according to the present invention.

FIG. 8 is an enlarged view of the comparison of filtering effects of three different filtering algorithms for a bicycle model implemented by an unmanned vehicle positioning method fused with multi-source sensor information according to the present invention.

FIG. 9 is a schematic flowchart of the method of the present invention.

Detailed ways

The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

Example

An unmanned vehicle positioning method integrating multi-source sensor information, the method includes the following steps:

Step 1: Obtain the GPS position information and IMU information of the vehicle to be controlled, and use the centimeter-level precision positioning obtained by the vehicle to be controlled in combination with the inertial navigation system and the GPS signal as the true value to verify the fusion positioning accuracy;

Step 2: Establish a linear model according to GPS location information and IMU information;

Step 3: Obtain the unmanned vehicle positioning method fused with multi-source sensor information according to the linear model, and obtain the fusion positioning result under the linear model;

Step 4: Build a nonlinear model according to GPS location information and IMU information;

Step 5: Obtain the positioning method of the unmanned vehicle fused with multi-source sensor information under the kinematic model according to the nonlinear model, and obtain the fusion positioning result under the nonlinear model;

Step 6: According to the nonlinear model, the nonlinear model is further deepened and remodeled to complete the establishment of the bicycle model;

Step 7: Obtain the unmanned vehicle positioning method fused with multi-source sensor information under the single-vehicle model according to the single-vehicle model, and obtain the fusion positioning result under the single-vehicle model.

In step 1, GPS information is collected through a GNSS antenna installed on the roof, and angular velocity and acceleration information is collected through an inertial navigation unit.

In step 2, the linear model includes a constant velocity model and a constant acceleration model.

In step 3, the unmanned vehicle localization method based on the fusion of multi-source sensor information under the linear model is as follows:

Taking the linear model, IMU information and GPS information as input, the GPS location information and IMU information are fused according to the Kalman filter algorithm to obtain more accurate location information of the unmanned vehicle, which is compared with the true value;

In step 4, the nonlinear model is a kinematic model of constant acceleration and angular velocity.

In step 5, the unmanned vehicle positioning method based on the fusion of multi-source sensor information under the kinematic model is as follows:

The extended Kalman filter algorithm, the unscented Kalman filter algorithm and the particle filter algorithm are respectively used to complete the fusion positioning, and comprehensive comparison is made to select the optimal positioning algorithm.

In step 6, the bicycle model has stronger nonlinearity than the kinematic model and is more similar to the real-life vehicle model.

In step 7, the method for locating the unmanned vehicle by fusing multi-source sensor information under the bicycle model is as follows:

According to the public data set, the extended Kalman filter algorithm, the unscented Kalman filter algorithm and the particle filter algorithm are used to analyze the influence of the nonlinear degree of the bicycle model on the accuracy of the fusion positioning algorithm of the unmanned vehicle, and obtain the optimal positioning algorithm.

In the present invention, data collection is performed on the vehicle to be controlled by GPS and IMU, linear system modeling is performed for the system, a Kalman filter algorithm is used for fusion to obtain a more accurate positioning result, nonlinear modeling is performed on the system and a real vehicle test is performed, Under the nonlinear system, the extended Kalman filter, unscented Kalman filter and particle filter are used to significantly improve the positioning accuracy of the unmanned vehicle, and the nonlinearity of the model is further expanded. The single vehicle model is used for further experimental simulation and the optimal filtering algorithm is selected. , the present invention fuses GPS information and IMU information, solves the technical problem of how to improve the positioning accuracy in automatic driving in a simple environment to control the driving of the vehicle, and can select the optimal solution in systems with different degrees of nonlinearity , the applicability is strong.

As shown in Figures 1 to 8, an unmanned vehicle positioning method integrating multi-source sensor information, the unmanned vehicle positioning method includes the following steps: obtaining GPS information and IMU information of the vehicle to be controlled, and obtaining the combined inertia of the vehicle to be controlled. The centimeter-level precision positioning obtained by the navigation system and the GPS signal is used as the true value to compare with the fusion positioning results, establish a linear constant acceleration model, and use the Kalman filter algorithm to realize the fusion of GPS position information and IMU position information, which is consistent with the true value. The kinematic model of constant acceleration and angular acceleration is established, and the extended Kalman filter algorithm, the unscented Kalman filter algorithm, and the particle filter algorithm are used to realize the fusion of GPS information and IMU information, and the RMS error is calculated. And judge the pros and cons of the three algorithms, further deepen the nonlinearity and build a bicycle model, complete the fusion of GPS information and IMU information through public data sets, and conduct error analysis for coordinate values and heading angles.

The invention can complement the advantages and disadvantages of GPS information and IMU information, and achieve a more accurate positioning effect of unmanned vehicles in a simple environment. For the positioning effect of unmanned vehicles, the extended Kalman filter algorithm is used in an environment with weak nonlinearity. When the degree of nonlinearity of the environment is strengthened, the unscented Kalman filter algorithm and particle filter algorithm are used to obtain better results. Therefore, the present invention aims at It can achieve good fusion positioning effect in environments with different degrees of nonlinearity, and has strong applicability.

The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person familiar with the technical field within the technical scope disclosed by the present invention can easily think of various equivalent modifications or Replacement, these modifications or replacements should all be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

Claims (10)

1. An unmanned vehicle positioning method fusing multi-source sensor information is characterized by comprising the following steps:

step 1: acquiring GPS position information and IMU information of a vehicle to be controlled, and taking centimeter-level precision positioning obtained by combining the vehicle to be controlled with an inertial navigation system and a GPS signal as a true value to verify the fused positioning precision;

step 2: establishing a linear model according to the GPS position information and the IMU information;

and step 3: acquiring an unmanned vehicle positioning algorithm for fusing multi-source sensor information under a linear model, and acquiring a fusion positioning result under the linear model;

and 4, step 4: establishing a nonlinear model according to the GPS position information and the IMU information;

and 5: acquiring an unmanned vehicle positioning algorithm for fusing multi-source sensor information under a nonlinear model, and acquiring a fusion positioning result under the nonlinear model;

step 6: further deepening nonlinearity of the nonlinear model and modeling again to obtain a bicycle model;

and 7: and acquiring an unmanned vehicle positioning algorithm for fusing multi-source sensor information under the single vehicle model, and obtaining a fusion positioning result under the single vehicle model.

2. The method as claimed in claim 1, wherein in step 1, GPS information is collected through a GNSS antenna installed on a roof of a vehicle, and IMU information is collected through an inertial navigation unit, wherein the GPS information includes longitude and latitude information of the vehicle, and the longitude and latitude information of the vehicle is converted into an abscissa and an ordinate of the vehicle, and the IMU information includes three-axis angular velocity and acceleration of the vehicle.

3. The method according to claim 1, wherein in step 2, the linear model comprises a constant velocity model and a constant acceleration model, and the expression of the state vector of the constant velocity model is as follows:

wherein x, y, theta and v are respectively an abscissa, an ordinate, a vehicle course angle and a speed of the vehicle position at the time t;

the expressions of the state equation and the observation equation of the constant speed model are respectively as follows:

wherein, Delta t is a time interval,

for the state vector after the time at has elapsed,

for observations obtained by GPS, x_gpsAs the abscissa, y, of the vehicle position obtained by GPS_gpsIs the ordinate of the vehicle position obtained by the GPS;

the expression of the state vector of the constant acceleration model is:

wherein s and v are respectively the displacement and the speed of the vehicle at the time t;

the expressions of the state equation and the observation equation of the constant acceleration model are respectively as follows:

wherein, a is the acceleration of the vehicle,

is a state vector after the time of delta t, s (t) is the displacement of the vehicle at the time of t, v (t) is the speed of the vehicle at the time of t,

the observed value is the displacement measured by the GPS;

the constant acceleration model takes the values measured by the accelerometer as system inputs:

u(k)＝a(k)

where u (k) is the system input and a (k) is the value measured by the accelerometer.

4. The unmanned aerial vehicle positioning method fused with multi-source sensor information according to claim 3, wherein in the step 3, the unmanned aerial vehicle positioning method fused with multi-source sensor information under a linear model specifically comprises:

step 301: obtaining more accurate unmanned vehicle position information according to a Kalman filtering algorithm, wherein the Kalman filtering algorithm comprises five steps of state prediction, error covariance prediction, Kalman gain updating, state updating and error covariance updating:

wherein ,

a predictor representing the state vector at time k,

represents the best estimate of the state vector at time k,

prediction value, P, representing the error covariance at time k_kOptimal estimate, K, representing the error covariance at time K_kRepresenting the Kalman gain at time k, Q being the process noise covariance matrix, R being the observation noise covariance matrix, u_k-1The system input at the moment of k-1 is represented, A represents a state transition matrix of the system, H represents an observation matrix of the system, I represents a unit matrix, and B is a control input matrix;

for the constant speed model, the expressions of an observation matrix H and a state transition matrix A obtained according to the state equation and the observation equation of the constant speed model are respectively as follows:

wherein, delta t is a time interval, and theta is a heading angle of the vehicle;

initializing a covariance matrix P into an identity matrix, and initializing a process noise matrix Q and an observation noise matrix R:

for the constant acceleration model, the expressions of a state transition matrix A, an observation matrix H and a control input matrix B obtained according to the state equation, the observation value and the system input of the constant acceleration model are respectively as follows:

H＝[1 0]

wherein Δ t is a time interval;

initializing the covariance matrix P as an identity matrix, and initializing a process noise matrix Q and an observation noise matrix R as follows:

R＝1；

step 302: and comparing the obtained optimal estimation value with the true value, and calculating the variance before and after filtering.

5. The method according to claim 1, wherein in step 4, the nonlinear model is a kinematic model based on constant acceleration and angular velocity, and the expression of the state vector of the kinematic model is as follows:

wherein ,x_k、y_k、ψ_k、ω_k、v_k and a_kRespectively an abscissa, an ordinate, a course angle, an angular velocity, a speed and an acceleration of the vehicle at the moment k;

the expressions of the state equation and the observation equation of the kinematic model are respectively as follows:

wherein ,

is the state vector of the vehicle at time k-1, v_k-1The speed of the vehicle at time k-1, dt is the time interval,

as an observed value, x_g,k、y_g,k、a_a,k and ω_g,kThe acceleration obtained by the accelerometer and the angular velocity measured by the gyroscope are respectively the abscissa and the ordinate of the vehicle observed by the GPS at the moment k.

6. The unmanned aerial vehicle positioning method fusing multi-source sensor information according to claim 1, wherein in the step 5, the process of obtaining the unmanned aerial vehicle positioning method fusing multi-source sensor information under the kinematic model specifically comprises the following steps:

step 501: performing fusion positioning by respectively adopting an extended Kalman algorithm, an unscented Kalman algorithm and a particle filter algorithm;

step 502: comprehensively comparing the three filtering algorithms, calculating RMS error, and selecting the optimal positioning algorithm, wherein the RMS error has the calculation formula:

error_rms＝|X-X_est|

wherein, error_rmsX is the true value of the filtered error and includes the true values of the abscissa and ordinate of the vehicle, X_estRespectively obtaining the optimal estimated values of the abscissa and the ordinate of the filtered vehicle;

step 503: and selecting a corresponding filtering algorithm with the minimum RMS error, and acquiring a fusion positioning result under the nonlinear model based on the filtering algorithm.

7. The unmanned aerial vehicle positioning method fusing multi-source sensor information according to claim 6, wherein in step 501, expanding a specific formula of a Kalman filtering algorithm comprises:

wherein ,F_k-1And H_kRespectively, the Jacobian matrix, u_kFor the system input at time k, x_k-1Is the state vector at the moment of k-1;

jacobian matrix F_k-1 and H_kAre respectively:

wherein ,ψ_k-1Is the heading angle, v, of the vehicle at time k-1_k-1The speed of the vehicle at time k-1, dt is the time interval;

the specific filtering process of the unscented Kalman filtering algorithm is as follows:

step 1 a: initialization state vector x₀State estimation error covariance P₀A process noise matrix Q and an observation noise matrix R, for a kinematic model, the initial values of the state vectors

Is true value, the process noise matrix Q, the observation noise matrix R and the initial error covariance matrix P₀The initialized expression is:

step 2 a: according to the state vector x at the previous moment_kSum error covariance matrix P_kSelecting a sampling point at the current moment and distributing weight;

step 3 a: selecting 2n +1 sampling points, calculating the mean value and covariance of all the sampling points at the current moment through a state equation of the system, and finishing time updating;

step 4 a: measuring and updating the sampling points through an observation equation of the system;

step 5 a: calculating Kalman gain through prediction and measurement updating, and finishing filtering updating;

the particle filter algorithm adopts a Monte Carlo method to solve the problem of nonlinear filtering, and the filtering process of the particle filter algorithm is as follows:

step 1 b: initializing N particles

And weight of the particles

Uniformly setting the ratio to 1/N;

and step 2 b: updating particles

Calculating likelihood probability of each particle

Weighting each particle

Is updated to

And carrying out normalization processing, wherein the weight value updating formula of the particles is as follows:

wherein ,

is the weight of the particle(s),

is the likelihood probability of a particle, z_kAs an observed value of the system at time k,

is the output value, R, of the ith particle observation equation at time k_kAn observed noise covariance matrix at time k;

and step 3 b: to pair

Resampling, and calculating effective particle number N_effThereby obtaining a new particle group

And the weight of the particles

Is 1/N;

and 4 b: and calculating the filtering state estimation and the variance estimation of the current moment by adopting the weight and the state of the resampled particles.

8. The unmanned aerial vehicle positioning method fused with multi-source sensor information as claimed in claim 1, wherein in step 6, the relation satisfied between the speed and the angular velocity in the single vehicle model is as follows:

wherein, beta is a slip angle,

the speed in the horizontal direction is the speed,

the speed in the vertical direction is the speed,

angular velocity as course angle, v as centroid speed, psi as course angle_rAnd l_fRespectively rear overhang length and front overhang length, delta_fAnd delta_rRespectively a front wheel deflection angle and a rear wheel deflection angle;

the slip angle β is calculated as:

wherein ,l_rAnd l_fRespectively rear overhang length and front overhang length, delta_fAnd delta_rRespectively a front wheel deflection angle and a rear wheel deflection angle;

the expression of the state vector of the bicycle model is:

wherein ,x_k、y_k、ψ_k、r、b、N、δ_r,k and ω_kRespectively an abscissa, an ordinate, a course angle, a wheel radius, a vehicle wheel base, a gear ratio, a front wheel deflection angle and a wheel rotation angular speed of the vehicle at the moment k, wherein the wheel radius, the vehicle wheel base and the gear ratio are kept unchanged, the front wheel deflection angle and the wheel rotation angular speed of the vehicle are used as system input, and dt is a time interval;

the equation of state of the bicycle model is as follows:

the observed values of the bicycle model are:

wherein ,x_g,k and y_g,kRespectively an abscissa and an ordinate of the vehicle observed by the GPS at the moment k;

the observation equation of the bicycle model is as follows:

9. the method according to claim 8, wherein the step 7 of obtaining the unmanned vehicle positioning algorithm fused with the multi-source sensor information under the single vehicle model specifically comprises the following steps:

step 701: respectively obtaining the optimal estimation values of the vehicle under the bicycle model by adopting an extended Kalman algorithm, an unscented Kalman algorithm and a particle filter algorithm based on a public data set;

step 702: and (3) carrying out error analysis aiming at the coordinates and the heading angle of the vehicle, and calculating RMS errors corresponding to the abscissa, the ordinate and the heading angle of the vehicle, wherein the RMS error has a calculation formula as follows:

error_rms＝|X-X_est|

wherein, error_rmsX is the true value of the filtered error, including the true values of the abscissa and ordinate of the vehicle, X_estRespectively obtaining the optimal estimated values of the abscissa, the ordinate and the course angle of the filtered vehicle;

step 703: carrying out mean value processing on each RMS error to realize normalization;

step 704: adding the three normalized RMS errors of each filtering algorithm to form a score, evaluating the performance and effect of the three filtering algorithms by comparing the scores of the filtering algorithms, and further selecting the score with the minimum value as the optimal positioning algorithm;

step 705: and obtaining a fusion positioning result under the bicycle model based on the optimal positioning algorithm.

10. The method of claim 9, wherein in step 701, initial state vectors are obtained when the single vehicle model is filtered by a filtering algorithm

Taking a true value, taking the wheel radius R to be 0.425, taking the vehicle wheel base b to be 0.8, taking the gear ratio N to be 5, and taking the initialization process noise matrix Q, the observation noise matrix R and the initial error covariance matrix P₀The expressions are respectively:

Images