阅读说明：本技术 一种基于磁传感器与轮式里程计融合定位方法 (Fusion positioning method based on magnetic sensor and wheel type odometer ) 是由 马芳武 史津竹 冯曙 葛林鹤 代凯 仲首任 吴量 单子桐 郭荣辉 于 2019-10-18 设计创作，主要内容包括：本发明公开了一种基于磁传感器与轮式里程计融合定位方法,通过对车辆CAN报文解析模块和磁场传感器进行同步采样匹配,构建同步匹配成功的车辆数据(车速数据、方向盘转角数据和地球磁场数据),然后基于该车辆数据,采用将车辆轮式里程计的定位估计值与磁场传感器的灰色预测值相融合方法,抑制了轮式里程计的累积误差,提高了航向角估计的精度,通过灰色预测方法的预测值建立扩展卡尔曼滤波的观测模型,有效抑制磁场传感器的磁场数据波动；优点是采用低成本的磁场传感器和车辆现有的速度传感器、转角传感器实现长时间的定位,在保证低成本的基础上,有效提高定位精度和鲁棒性。(The invention discloses a fusion positioning method based on a magnetic sensor and a wheel type odometer, which comprises the steps of carrying out synchronous sampling matching on a vehicle CAN message analysis module and the magnetic sensor, constructing vehicle data (vehicle speed data, steering wheel corner data and earth magnetic field data) which are successfully matched synchronously, then adopting a fusion method of a positioning estimation value of the vehicle wheel type odometer and a gray prediction value of the magnetic sensor based on the vehicle data, inhibiting the accumulated error of the wheel type odometer, improving the accuracy of course angle estimation, establishing an observation model of extended Kalman filtering through the prediction value of the gray prediction method, and effectively inhibiting the magnetic data fluctuation of the magnetic sensor; the method has the advantages that the long-time positioning is realized by adopting the low-cost magnetic field sensor and the existing speed sensor and the existing rotation angle sensor of the vehicle, and the positioning precision and the robustness are effectively improved on the basis of ensuring the low cost.)

1. A fusion positioning method based on a magnetic sensor and a wheel type odometer is characterized by comprising the following steps:

(1) recording the time interval between two adjacent samplings of the CAN message analysis module of the vehicle as △ t₁，△t₁The time interval between two adjacent samples of the magnetic field sensor is noted as △ t at 0.01s₂，△t₂0.05 s; establishing an array for storing earth magnetic field data, wherein the capacity of the array is 10, when the capacity is exceeded, the earth magnetic field data stored in the array are covered according to the sequence of storage time from morning to evening, the earth magnetic field data in the array are arranged from front to back according to the storage sequence, the earth magnetic field data stored later are arranged behind the earth magnetic field data stored earlier, the number of the earth magnetic field data stored in the array is recorded as a variable n, when in an initial state, the earth magnetic field data do not exist in the array, and at the moment, the earth magnetic field data stored in the array are 0, and the value of n is 0; designing a buffer memory for storing the time stamp of the sampling data acquired by the vehicle CAN message analysis module at each sampling, wherein the capacity of the buffer memory is 100, when the capacity of the buffer memory exceeds the capacity of the buffer memory, the stored time stamp in the buffer memory is covered according to the sequence of the storage time from morning to evening, the time stamps in the buffer memory are arranged from front to back according to the storage sequence, the later stored time stamp is arranged behind the earth magnetic field data stored in advance, the sampling data acquired by the vehicle CAN message analysis module at each sampling comprises vehicle speed data and steering wheel corner data, the time of the vehicle CAN message analysis module at each sampling is represented by UTC time, the time of the vehicle CAN message analysis module at each sampling is stored in the buffer memory as the time stamp of the sampling data acquired by the vehicle CAN message analysis module at the sampling time, and the magnetic field data are stored in the buffer memoryThe method comprises the steps that earth magnetic field data are obtained by a field sensor during sampling each time, and the sampling time of the magnetic field sensor each time is represented by UTC time;

(2) setting a variable of the number of times of successful matching of synchronous sampling of a vehicle CAN message analysis module and a magnetic field sensor, recording the variable as t, and carrying out initialization assignment on t, wherein t is set to be 0;

(3) simultaneously starting the vehicle CAN message analysis module and the magnetic field sensor, wherein the vehicle CAN message analysis module and the magnetic field sensor start to sample for the 1 st time simultaneously, and the sampling times of the vehicle CAN message analysis module and the magnetic field sensor are increased by 1 every subsequent sampling;

(4) recording the current sampling times of the magnetic field sensor as the ith time, and carrying out synchronous sampling matching on the vehicle CAN message analysis module and the vehicle-mounted monocular camera for the ith time, wherein the specific matching process is as follows:

4.1 recording the earth magnetic field data obtained by the first sampling of the magnetic field sensor as m_lThe time of the first sampling of the magnetic field sensor is recorded as t_l；

4.2 mixing of t_lMatching with all timestamps stored in the cache, searching for and matching with t_lIf the timestamp with the minimum difference is found, the first synchronous sampling matching is successful, the vehicle speed data and the steering wheel corner data corresponding to the found timestamp are obtained, the step (5) is carried out, if the timestamp with the minimum difference is not found, the current value of l is added with the value of 1, the value of l is updated, and the step (4) is repeated until the condition that the synchronous sampling matching is successful is met;

(5) the method comprises the following steps of firstly, adding 1 to the current value of t and updating the value of t, and then constructing vehicle data which is successfully sampled and matched for the t-th time in a synchronous mode, wherein the specific process is as follows:

5.1 recording the vehicle speed data successfully matched with the sampling synchronization at the t time as v_tThe steering wheel angle data is recorded as delta_ftThe earth's magnetic field data being denoted m^t；

5.2 assigning the vehicle speed data corresponding to the timestamp found after the current sampling synchronous matching is successful to v_tAssigning steering wheel angle data to delta_ft，m_lIs assigned to m^tThe construction of vehicle data successfully matched with the sampling synchronization at the t time is completed;

(6) m is to be^tSaving the last data in the array, counting the number of the earth magnetic field data in the array again, updating the value of n by adopting the counted number, and recording the current array as the last data in the array

(7) judging whether the current value of t is greater than or equal to 2, if so, entering the step (8), if not, adopting the current value of l plus 1 to update the value of l, and repeating the steps (4) to (6) until the condition of entering the step (8) is met;

(8) will be the current array

(9) using a one-time accumulation generation algorithm (1-AGO) to process the course angle data sequence

9.1 set an original gray sequence for storing n data, which is denoted as θ⁽⁰⁾Will theta⁽⁰⁾The ith data in (1) is recorded as theta⁽⁰⁾(i) I is 1,2, …, n, will

9.2 will theta⁽⁰⁾The sequence generated by the first accumulation is marked as theta⁽¹⁾，θ⁽¹⁾Expressed by formula (2):

θ⁽¹⁾＝(θ⁽¹⁾(1),θ⁽¹⁾(2),…,θ⁽¹⁾(n)) (2)

wherein, theta⁽¹⁾(i) Is theta⁽¹⁾The ith data in (1);

9.3 The formula (3) is theta⁽¹⁾And (4) assignment:

9.4 will theta⁽¹⁾Is denoted as z⁽¹⁾，z⁽¹⁾Expressed by formula (4):

z⁽¹⁾＝(z⁽¹⁾(1),z⁽¹⁾(2),…,z⁽¹⁾(n)) (4)

wherein z is⁽¹⁾(i) Is z⁽¹⁾The ith data in (1);

9.5 Using formula (5) vs. z⁽¹⁾And (4) assignment is carried out:

wherein, K is 2, …, n, β is an adjacent value to generate a weight coefficient, β is 0.5;

9.6 establishing a gray differential equation model, which is expressed by equation (6):

θ⁽⁰⁾(K)+az⁽¹⁾(K)＝b (6)

wherein, a is called a gray development coefficient, b is called a gray acting quantity, and a and b are parameters to be solved;

9.7 unfolding and arranging the formula (6) into a matrix vector form, and expressing the formula (7) as follows:

Y＝Bu (7)

wherein u, B and Y are respectively represented by formulas (8), (9) and (10), and specifically:

9.8 solving equation (7) by the least square method, and calculating parameters a and b:

in the formula (11), the superscript T represents the transposition of the matrix, and the superscript-1 represents the inverse operation of the matrix;

9.9 build a whitening model for GM (1,1), which is expressed as equation (12):

9.10 recording the predicted value of the one-time accumulation generating sequence successfully matched with the t-1 th sampling synchronization as the predicted value

in formula (13), e represents the base of the natural logarithm;

9.11 recording the predicted value of the one-time accumulation generating sequence successfully matched with the sampling synchronization of the t time as the predicted value

9.12 recording the predicted value of the original gray sequence successfully matched with the sampling synchronization of the t-th time as

9.13 recording the predicted value of the heading angle data of the vehicle successfully matched with the sampling synchronization at the t-th time as theta_tLet us order

(10) Recording the current positioning times as t ', making t ' equal to t-1, performing data fusion by adopting an extended Kalman filtering algorithm, and then performing the t ' th positioning of the vehicle, wherein the specific process comprises the following steps:

a. obtaining the kinematic motion trail generated by the vehicle kinematics through a vehicle kinematic track presumption algorithm, namely the currentVehicle position estimation coordinates at time of positioning

ds_t'＝v_t'-1·△t₁(17)

δ_ft'＝ω_t'·η (19)

wherein the content of the first and second substances,an abscissa estimated value representing the position of the vehicle in the vehicle coordinate system at the time of the t' -1 th positioning,

b. by using

wherein, when t' is equal to 1,

c. by v_t'-1And delta_ft'Constructing a control input vector at the current positioning, and marking the control input vector as B_t'：

d. Establishing a vehicle kinematic model with noise at the current positioning time, and recording a vector expression of the model as f (A)_t',B_t')：

Wherein, N (·) is a gaussian white noise generating function, N (0, Q) represents a gaussian white noise vector with dimension 3 × 1 generated by the gaussian white noise generating function, wherein 0 is a mean value of the gaussian white noise generating function, Q is a state propagation process covariance matrix of the gaussian white noise generating function, and the state propagation process covariance matrix Q is a matrix with dimension 3 × 3 generated by a random function, and is a fixed value after being generated;

e. will be currently located f (A)_t',B_t') With respect to the state vector A_t'The Jacobian matrix is denoted as F_t'，F_t'Expressed by equation (23):

f. the covariance matrix after state propagation is recorded asUsing equation (24) to the covariance matrix after state propagationUpdating:

wherein P represents the latest value of the state covariance matrix before current positioning, and superscript T represents the transposition of the matrix; when t' is 1, i.e. the initial time, P is initialized to an identity matrix of dimension 3 × 3, i.e.:

g. establishing a GPS observation model during current positioning:

wherein Z_t'Is the observation vector of the GPS observation model at the current location,

h. observing function when current location is carried out

i. recording the observation residual error of the GPS observation model in the current positioning as y_t'Calculating the observation residual y of the GPS observation model at the current positioning_t'：

j. The Kalman gain of the current positioning timeIs marked as K_t'Calculating the Kalman gain K at the time of current positioning_t'：

In the above formula, the first and second carbon atoms are,is its current latest value; the superscript "-1" represents the matrix inversion operation;

k. for state vector A_t'And updating the state covariance matrix P:

wherein I is an identity matrix with dimensions of 3x3,is its current latest value;

l, mixing (x'_t，y′_t) As the final position coordinates of the vehicle at the time of current positioning, will be θ'_tAs the final course angle of the vehicle during the current positioning, the positioning for the t' th time is completed;

(11) and (5) adding 1 to the current value of l and updating the value of l, and returning to the step (4) for next positioning.

Technical Field

The invention relates to a positioning method, in particular to a fusion positioning method based on a magnetic sensor and a wheel type odometer.

Background

The realization of centimeter-level positioning requirement of the vehicle is the key of autonomous navigation of the unmanned vehicle, and the existing vehicle positioning methods mainly comprise a positioning method based on a wheel type odometer, a positioning method based on a visual SLAM, a positioning method based on a laser SLAM and a positioning method based on a high-precision differential GPS. The positioning method based on the visual SLAM and the positioning method based on the laser SLAM are greatly influenced by the environment dynamic target, and have poor robustness and high cost. The high-precision differential GPS positioning method has higher cost and is mainly used for off-line map making of the unmanned vehicle. The position calculation of the vehicle can be realized through the existing speed sensor and the existing corner sensor on the vehicle based on the wheel type odometer positioning method, the price is low, the accumulated error along with the time is large, and the long-time effective work is not performed.

The multi-sensor fusion is an effective method for effectively improving the positioning precision and the robustness. The magnetic sensor is low in cost, can obtain global course information, is not influenced by illumination conditions and surrounding dynamic targets, and has a wide application prospect, but the magnetic sensor is easily interfered by surrounding magnetic fields, and noise fluctuation exists in the obtained magnetic field data.

Therefore, the fusion positioning method based on the magnetic sensor and the wheel type odometer is designed, the positioning precision and the robustness are effectively improved on the basis of ensuring low cost, and the fusion positioning method has important significance for the development of a vehicle positioning technology.

Disclosure of Invention

The invention aims to solve the technical problem of providing a fusion positioning method based on a magnetic sensor and a wheel type odometer, which fuses a positioning estimation value of the wheel type odometer of a vehicle and a grey prediction value of a magnetic sensor, inhibits the accumulated error of the wheel type odometer, improves the accuracy of course estimation, establishes an observation model of extended Kalman filtering according to the prediction value of the grey prediction method, effectively inhibits the fluctuation of magnetic field data of the magnetic sensor, adopts the low-cost magnetic sensor, the existing speed sensor and the existing corner sensor of the vehicle to realize long-time positioning, and effectively improves the positioning accuracy and the robustness on the basis of ensuring low cost.

The technical scheme adopted by the invention for solving the technical problems is as follows: a fusion positioning method based on a magnetic sensor and a wheel type odometer comprises the following steps:

(1) recording the time interval between two adjacent samplings of the CAN message analysis module of the vehicle as △ t₁，△t₁The time interval between two adjacent samples of the magnetic field sensor is noted as △ t at 0.01s₂，△t₂0.05 s; establishing an array for storing earth magnetic field data, wherein the capacity of the array is 10, when the capacity is exceeded, the earth magnetic field data stored in the array are covered according to the sequence of storage time from morning to evening, the earth magnetic field data in the array are arranged from front to back according to the storage sequence, the earth magnetic field data stored later are arranged behind the earth magnetic field data stored earlier, the number of the earth magnetic field data stored in the array is recorded as a variable n, when in an initial state, the earth magnetic field data do not exist in the array, and at the moment, the earth magnetic field data stored in the array are 0, and the value of n is 0; designing a buffer memory for storing the time stamp of the sampling data acquired by the vehicle CAN message analysis module in each sampling, wherein the capacity of the buffer memory is 100, when the capacity is exceeded, the stored time stamp in the buffer memory is covered according to the sequence of the storage time from morning to evening, the time stamps in the buffer memory are arranged from front to back according to the storage sequence, the later-stored time stamp is arranged behind the earlier-stored earth magnetic field data, and each time of the vehicle CAN message analysis module is arrangedThe sampling data obtained by secondary sampling comprises vehicle speed data and steering wheel corner data, the time of each sampling of the vehicle CAN message analysis module is represented by UTC time, the time of each sampling of the vehicle CAN message analysis module is stored in the cache as the timestamp of the sampling data obtained by the secondary sampling, the earth magnetic field data is obtained by the magnetic field sensor at each sampling, and the time of each sampling of the magnetic field sensor is represented by UTC time;

(2) setting a variable of the number of times of successful matching of synchronous sampling of a vehicle CAN message analysis module and a magnetic field sensor, recording the variable as t, and carrying out initialization assignment on t, wherein t is set to be 0;

(3) simultaneously starting the vehicle CAN message analysis module and the magnetic field sensor, wherein the vehicle CAN message analysis module and the magnetic field sensor start to sample for the 1 st time simultaneously, and the sampling times of the vehicle CAN message analysis module and the magnetic field sensor are increased by 1 every subsequent sampling;

(4) recording the current sampling times of the magnetic field sensor as the ith time, and carrying out synchronous sampling matching on the vehicle CAN message analysis module and the vehicle-mounted monocular camera for the ith time, wherein the specific matching process is as follows:

4.1 recording the earth magnetic field data obtained by the first sampling of the magnetic field sensor as m_lThe time of the first sampling of the magnetic field sensor is recorded as t_l；

4.2 mixing of t_lMatching with all timestamps stored in the cache, searching for and matching with t_lIf the timestamp with the minimum difference is found, the first synchronous sampling matching is successful, the vehicle speed data and the steering wheel corner data corresponding to the found timestamp are obtained, the step (5) is carried out, if the timestamp with the minimum difference is not found, the current value of l is added with the value of 1, the value of l is updated, and the step (4) is repeated until the condition that the synchronous sampling matching is successful is met;

(5) the method comprises the following steps of firstly, adding 1 to the current value of t and updating the value of t, and then constructing vehicle data which is successfully sampled and matched for the t-th time in a synchronous mode, wherein the specific process is as follows:

5.1 mixingAnd recording the vehicle speed data successfully matched with the sampling synchronization at the t time as v_tThe steering wheel angle data is recorded as delta_ftThe earth's magnetic field data being denoted m^t；

5.2 assigning the vehicle speed data corresponding to the timestamp found after the current sampling synchronous matching is successful to v_tAssigning steering wheel angle data to delta_ft，m_lIs assigned to m^tThe construction of vehicle data successfully matched with the sampling synchronization at the t time is completed;

(6) m is to be^tSaving the last data in the array, counting the number of the earth magnetic field data in the array again, updating the value of n by adopting the counted number, and recording the current array as the last data in the array

Wherein

J is the jth earth magnetic field data in the current array, and j is 1,2, …, n;

(7) judging whether the current value of t is greater than or equal to 2, if so, entering the step (8), if not, adopting the current value of l plus 1 to update the value of l, and repeating the steps (4) to (6) until the condition of entering the step (8) is met;

(8) will be the current array

The medium n earth magnetic field data are respectively converted by a geomagnetic coordinate conversion method to obtain a heading angle data sequence of the vehicle

Wherein

For earth magnetic field data

The heading angle data obtained by the conversion is converted,

the j-th course angle data in the course angle data sequence;

(9) using a one-time accumulation generation algorithm (1-AGO) to process the course angle data sequence

Processing to obtain a gray sequence, which comprises the following steps:

9.1 set an original gray sequence for storing n data, which is denoted as θ⁽⁰⁾Will theta⁽⁰⁾The ith data in (1) is recorded as theta⁽⁰⁾(i) I is 1,2, …, n, will

Is given by θ⁽⁰⁾(i) And obtaining an original gray sequence:

9.2 will theta⁽⁰⁾The sequence generated by the first accumulation is marked as theta⁽¹⁾，θ⁽¹⁾Expressed by formula (2):

θ⁽¹⁾＝(θ⁽¹⁾(1),θ⁽¹⁾(2),…,θ⁽¹⁾(n)) (2)

wherein, theta⁽¹⁾(i) Is theta⁽¹⁾The ith data in (1);

9.3 The formula (3) is theta⁽¹⁾And (4) assignment:

9.4 will theta⁽¹⁾Is denoted as z⁽¹⁾，z⁽¹⁾Expressed by formula (4):

z⁽¹⁾＝(z⁽¹⁾(1),z⁽¹⁾(2),…,z⁽¹⁾(n)) (4)

wherein z is⁽¹⁾(i) Is z⁽¹⁾The ith data in (1);

9.5 Using formula (5) vs. z⁽¹⁾And (4) assignment is carried out:

wherein, K is 2, …, n, β is an adjacent value to generate a weight coefficient, β is 0.5;

9.6 establishing a gray differential equation model, which is expressed by equation (6):

θ⁽⁰⁾(K)+az⁽¹⁾(K)＝b (6)

wherein, a is called a gray development coefficient, b is called a gray acting quantity, and a and b are parameters to be solved;

9.7 unfolding and arranging the formula (6) into a matrix vector form, and expressing the formula (7) as follows:

Y＝Bu (7)

wherein u, B and Y are respectively represented by formulas (8), (9) and (10), and specifically:

9.8 solving equation (7) by the least square method, and calculating parameters a and b:

in the formula (11), the superscript T represents the transposition of the matrix, and the superscript-1 represents the inverse operation of the matrix;

9.9 build a whitening model for GM (1,1), which is expressed as equation (12):

9.10 recording the predicted value of the one-time accumulation generating sequence successfully matched with the t-1 th sampling synchronization as the predicted value

Expressed by formula (13):

in formula (13), e represents the base of the natural logarithm;

9.11 recording the predicted value of the one-time accumulation generating sequence successfully matched with the sampling synchronization of the t time as the predicted value

Expressed by equation (14):

9.12 recording the predicted value of the original gray sequence successfully matched with the sampling synchronization of the t-th time asExpressed by formula (15):

9.13 recording the predicted value of the heading angle data of the vehicle successfully matched with the sampling synchronization at the t-th time as theta_tLet us order

(10) Recording the current positioning times as t ', making t ' equal to t-1, performing data fusion by adopting an extended Kalman filtering algorithm, and then performing the t ' th positioning of the vehicle, wherein the specific process comprises the following steps:

a. obtaining a motion trail generated by vehicle kinematics through a vehicle kinematics track presumption algorithm, namely vehicle position estimated coordinates in current positioningAnd course angle estimate

An abscissa estimated value representing the position of the vehicle in the vehicle coordinate system at the time of the current positioning,a vertical coordinate estimation value representing a vehicle position in a vehicle coordinate system at the time of current positioning; the vehicle kinematic trajectory presumption algorithm formula is as follows:

ds_t'＝v_t'-1·△t₁(17)

δ_ft'＝ω_t'·η (19)

wherein the content of the first and second substances,

an abscissa estimated value representing the position of the vehicle in the vehicle coordinate system at the time of the t' -1 th positioning,

an estimated value of the ordinate indicating the position of the vehicle in the vehicle coordinate system at the time of the t' -1 th positioning,

represents the vehicle heading angle estimate at the t' -1 th position, ds_t'Represents the distance traveled by the vehicle from the time of the t '-1 th positioning to the time of the t' th positioning，dθ_t'Represents the variation of the vehicle heading angle at the t 'th positioning relative to the variation of the vehicle heading angle at the t' -1 th positioning, v_t'-1The vehicle speed at the t' -1 th positioning, L the vehicle wheel base, delta_ft'The turning angle of the front wheel of the vehicle at the current positioning is η the angular transmission ratio of the vehicle, η is calibrated by the existing mature experimental method according to the type of the vehicle, when t' is 1,

v_t'-1sin denotes a sine function, cos denotes a cosine function, and tan denotes a tangent function;

b. by using

And

the constructed state vector, denoted A_t'By using the formula (20) to A_t'Carrying out initialization assignment:

wherein, when t' is equal to 1,

c. by v_t'-1And delta_ft'Constructing a control input vector at the current positioning, and marking the control input vector as B_t'：

d. Establishing a vehicle kinematic model with noise at the current positioning time, and recording a vector expression of the model as f (A)_t',B_t')：

Wherein, N (·) is a gaussian white noise generating function, N (0, Q) represents a gaussian white noise vector with dimension 3 × 1 generated by the gaussian white noise generating function, wherein 0 is a mean value of the gaussian white noise generating function, Q is a state propagation process covariance matrix of the gaussian white noise generating function, and the state propagation process covariance matrix Q is a matrix with dimension 3 × 3 generated by a random function, and is a fixed value after being generated;

e. will be currently located f (A)_t',B_t') With respect to the state vector A_t'The Jacobian matrix is denoted as F_t'，F_t'Expressed by equation (23):

f. the covariance matrix after state propagation is recorded as

Using equation (24) to the covariance matrix after state propagation

Updating:

wherein P represents the latest value of the state covariance matrix before current positioning, and superscript T represents the transposition of the matrix; when t' is 1, i.e. the initial time, P is initialized to an identity matrix of dimension 3 × 3, i.e.:

g. establishing a GPS observation model during current positioning:

wherein Z_t'Is the observation vector of the GPS observation model at the current location,

the observation function of the GPS observation model at the current positioning time; n (-) is a Gaussian white noise generating function, N (0, R) represents a Gaussian white noise vector generated by the Gaussian white noise generating function, and the dimensionality of N (0, R) is 3 multiplied by 1, wherein 0 is the mean value of the Gaussian white noise generating function, R is an observation covariance matrix, the dimensionality of the observation covariance matrix R is 3 multiplied by 3, and the observation covariance matrix R is:

h. observing function when current location is carried out

With respect to the state vector A_t'The Jacobian matrix is recorded as H_t'，H_t'Expressed by equation (29):

i. recording the observation residual error of the GPS observation model in the current positioning as y_t'Calculating the observation residual y of the GPS observation model at the current positioning_t'：

j. Let the Kalman gain at the time of current positioning be K_t'Calculating the Kalman gain K at the time of current positioning_t'：

In the above formula, the first and second carbon atoms are,is its current latest value; the superscript "-1" represents the matrix inversion operation;

k. for state vector A_t'And updating the state covariance matrix P:

wherein I is an identity matrix with dimensions of 3x3,

is its current latest value;

l, mixing (x'_t，y′_t) As the final position coordinates of the vehicle at the time of current positioning, will be θ'_tAs the final course angle of the vehicle during the current positioning, the positioning for the t' th time is completed;

(11) and (5) adding 1 to the current value of l and updating the value of l, and returning to the step (4) for next positioning.

Compared with the prior art, the method has the advantages that vehicle data (vehicle speed data, steering wheel corner data and earth magnetic field data) which are successfully matched in a synchronous mode are constructed by synchronously sampling and matching the vehicle CAN message analysis module and the magnetic field sensor, then based on the vehicle data, a method of fusing a positioning estimation value of the vehicle wheel type odometer and a gray prediction value of the magnetic field sensor is adopted, accumulated errors of the wheel type odometer are restrained, the course angle estimation precision is improved, an observation model of extended Kalman filtering is established through a prediction value of the gray prediction method, the magnetic field data fluctuation of the magnetic field sensor is effectively restrained, long-time positioning is achieved by adopting the low-cost magnetic field sensor, the existing speed sensor and the existing corner sensor of the vehicle, and the positioning precision and robustness are effectively improved on the basis of ensuring low cost.

Detailed Description

The present invention will be described in further detail with reference to examples.