阅读说明：本技术 一种基于差分gps和车辆运动学模型的轨迹录制方法 (Track recording method based on differential GPS and vehicle kinematics model ) 是由 马芳武 史津竹 代凯 冯曙 葛林鹤 仲首任 吴量 单子桐 郭荣辉 于 2019-10-18 设计创作，主要内容包括：本发明公开了了一种基于差分GPS和车辆运动学模型的轨迹录制方法,通过差分GPS模块获取当前时刻t车辆所处位置的GPS经纬度数据、卫星数量信息和PDOP位置精度因子信息,通过整车CAN解析模块获取当前时刻t车辆的车速v<Sub>t</Sub>和方向盘转角ω<Sub>t</Sub>,通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹,根据当前时刻t获取的卫星数量信息来判断差分GPS模块在当前时刻t获取的车辆GPS经纬度数据是否存在丢失,再未丢失时判定GPS经纬度数据噪声,当GPS经纬度数据噪声较大时采用扩展卡尔曼滤波算法先进行数据融合后再进行轨迹录制；优点是在具有平滑轨迹的基础上,不存在轨迹丢失,轨迹录制连续。(The invention discloses a track recording method based on a differential GPS and a vehicle kinematic model, which comprises the steps of obtaining GPS longitude and latitude data, satellite quantity information and PDOP position accuracy factor information of a vehicle position at the current moment t through a differential GPS module, and obtaining the vehicle speed v of the vehicle at the current moment t through a vehicle CAN analysis module t And steering wheel angle omega t Obtaining a motion track generated by vehicle kinematics through a vehicle kinematics track presumption algorithm, judging whether vehicle GPS longitude and latitude data acquired by a differential GPS module at the current time t are lost or not according to satellite quantity information acquired at the current time t, judging GPS longitude and latitude data noise when the vehicle GPS longitude and latitude data are not lost, and performing track recording after data fusion by adopting an extended Kalman filtering algorithm when the GPS longitude and latitude data noise is high; the method has the advantages that on the basis of having smooth tracks, no track loss exists, and the tracks are recorded continuously.)

1. A track recording method based on a differential GPS and a vehicle kinematic model is characterized by comprising the following steps:

(1) setting a variable t at the current moment;

(2) carrying out initialization assignment on the current t, and enabling the t to be 1;

(3) recording the vehicle track at the current moment t, and the specific process is as follows:

3.1 obtaining GPS longitude and latitude data, satellite quantity information and PDOP (position Dilution of precision) position precision factor information of the position of the vehicle at the current moment t through a differential GPS module, and obtaining the vehicle speed v of the vehicle at the current moment t through a whole vehicle CAN analysis module_tAnd steering wheel angle omega_tThe differential GPS module adopts a dual-antenna GPS and a differential RTK service provided by a thousand-searching position;

3.2 carrying out coordinate transformation on the GPS longitude and latitude data acquired at the current moment t to obtain the vehicle position coordinate under the vehicle coordinate system at the current moment t

3.3 obtaining the motion trail generated by the vehicle kinematics through a vehicle kinematics track estimation algorithm, namely the vehicle position estimation coordinate at the current moment t

ds_t＝v_t-1·dt (1.2)

δ_ft＝ω_t·η (1.4)

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 time t-1,

3.4 judging whether the vehicle GPS longitude and latitude data acquired by the differential GPS module at the current moment t is lost or not according to the satellite number information acquired at the current moment t, if the satellite number information is less than 4, judging that the vehicle GPS longitude and latitude data are lost, and estimating the coordinate of the vehicle position at the current moment t at the moment

a. by using

wherein, at the initial moment when t is 1,

b. by v_t-1And delta_ftConstructing a control input vector of the current time t and recording the control input vector as B_t：

Wherein v is an initial time when t is 1_t-1＝0；

c. Establishing a vehicle kinematic model with noise at the current moment t, 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;

d. the current time t is compared with the time f (A)_t,B_t) With respect to the state vector A_tThe Jacobian matrix is denoted as F_t，F_tExpressed by the formula (1.8):

e. the covariance matrix after state propagation is recorded as

wherein P represents the latest value of the state covariance matrix before the current time T, 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.:

f. establishing a GPS observation model at the current time t:

wherein Z_tFor the observation vector of the GPS observation model at the current time t,

g. observing the current time t

h. the observation residual error of the GPS observation model at the current time t is recorded as y_tCalculating the observation residual y of the GPS observation model at the current time t_t：

i. Let the current time t Kalman gain be K_tCalculating the current time t Kalman gain K_t：

In the above formula, the first and second carbon atoms are,

j. for state vector A_tAnd updating the state covariance matrix P:

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

k. will be (x'_t，y′_t) As the final position coordinates of the vehicle at the current time t, theta'_tAnd adopting the final position coordinate (x ') of the vehicle at the current moment t as the final heading angle of the vehicle at the current moment t'_t，y′_t) And the final heading angle theta of the vehicle at the current moment t'_tForming the final motion track of the vehicle at the current moment t, storing the track,

(4) and (5) updating t by adding 1 to the current value of t, and returning to the step (3) to record the track at the next moment.

Technical Field

The invention relates to a vehicle track recording method, in particular to a track recording method based on a differential GPS and a vehicle kinematic model.

Background

The realization of the vehicle tracking function is one of the primary tasks of the unmanned vehicle, and the positioning module for realizing the tracking function outdoors mainly comprises a differential GPS, a visual SLAM and a laser SLAM. The visual SLAM and the laser SLAM firstly carry out map construction and track storage on the surrounding environment of the vehicle through a map construction module, then carry out vehicle positioning according to the stored map through a positioning module, and realize the tracking of the vehicle through a vehicle following module. However, the visual SLAM and the laser SLAM are greatly influenced by environmental dynamic targets, and have poor robustness and high cost.

The method for realizing the tracing by adopting the differential GPS method is a primary method, the positioning accuracy can reach centimeter level under the environment without shielding and with better signals, and the requirement of unmanned tracing can be met. However, when the GPS is located at a position with many obstacles (such as an overpass), the GPS displays that there are many drift points with large errors, and in extreme cases, the GPS loses positioning coordinate data, and the recorded track has large noise points and is not smooth. Research proposes that interpolation optimization processing is carried out on a GPS track, the method can solve the problem that the GPS track is not smooth, but is difficult to solve the problem of track loss, and the unmanned vehicle can realize track seeking and requires that the track recorded by the GPS is not only smooth, but more importantly, the track is continuous.

Disclosure of Invention

The invention aims to solve the technical problem of providing a track recording method based on a differential GPS and a vehicle kinematic model, which has no track loss and continuous track recording on the basis of smooth track.

The technical scheme adopted by the invention for solving the technical problems is as follows: a track recording method based on a differential GPS and a vehicle kinematic model comprises the following steps:

(1) setting a variable t at the current moment;

(2) carrying out initialization assignment on the current t, and enabling the t to be 1;

(3) recording the vehicle track at the current moment t, and the specific process is as follows:

3.1 obtaining GPS longitude and latitude data, satellite quantity information and PDOP (position Dilution of precision) position precision factor information of the position of the vehicle at the current moment t through a differential GPS module, and obtaining the vehicle speed v of the vehicle at the current moment t through a whole vehicle CAN analysis module_tAnd steering wheel angle omega_tThe differential GPS module adopts a dual-antenna GPS and a differential RTK service provided by a thousand-searching position;

3.2 carrying out coordinate transformation on the GPS longitude and latitude data acquired at the current moment t to obtain the vehicle position coordinate under the vehicle coordinate system at the current moment t

And course angle

Wherein

An abscissa indicating the vehicle position in the vehicle coordinate system at the present time t,

a vertical coordinate representing a vehicle position in a vehicle coordinate system at the current time t;

3.3 obtaining the motion trail generated by the vehicle kinematics through a vehicle kinematics track estimation algorithm, namely the vehicle position estimation coordinate at the current moment t

And course angle estimate

An abscissa estimated value representing the vehicle position in the vehicle coordinate system at the present time t,

a vertical coordinate estimated value representing a vehicle position in a vehicle coordinate system at the current time t; the vehicle kinematic trajectory presumption algorithm formula is as follows:

ds_t＝v_t-1·dt (1.2)

δ_ft＝ω_t·η (1.4)

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 time t-1,

an estimated value of the ordinate indicating the position of the vehicle in the vehicle coordinate system at the time t-1,representing the vehicle heading angle estimate, ds, at time t-1_tRepresenting the distance traveled by the vehicle from time t-1 to the present time t, d θ_tRepresents the variation of the vehicle course angle v at the current moment t relative to the t-1 moment_t-1The speed of the vehicle at the time t-1, dt is the sampling period of a CAN analysis module of the whole vehicle, L is the wheel base of the vehicle, and delta_ftAt the current moment t, the front wheel rotation angle of the vehicle is η, the angular transmission ratio of the vehicle is obtained, and when t is 1,

v_t-1sin denotes sine function, cos table, 0Cosine function, tan tangent function;

3.4 judging whether the vehicle GPS longitude and latitude data acquired by the differential GPS module at the current moment t is lost or not according to the satellite number information acquired at the current moment t, if the satellite number information is less than 4, judging that the vehicle GPS longitude and latitude data are lost, and estimating the coordinate of the vehicle position at the current moment t at the moment

And course angle estimate

The motion trajectory is used as the final motion trajectory of the vehicle at the current moment t, and the trajectory is stored; if the satellite quantity information is more than or equal to 4, judging that the satellite quantity information is not lost, judging the noise of the GPS longitude and latitude data by using a PDOP (position Dilution of precision) position precision factor acquired at the current time t, if the PDOP is less than 3, judging that the noise of the GPS longitude and latitude data is small, and judging that the noise of the vehicle position coordinate obtained at the current time t is small

And course angleAnd (3) performing track storage as the final motion track of the vehicle at the current moment t, if PDOP is more than or equal to 3, judging that the noise of the GPS longitude and latitude data is large, and performing track recording after data fusion by adopting an extended Kalman filtering algorithm, wherein the specific process comprises the following steps:

a. by using

And

constructing a state vector of the current time t, and recording the state vector as A_tBy using the formula (1.5) to A_tCarrying out initialization assignment:

wherein, at the initial moment when t is 1,

b. by v_t-1And delta_ftConstructing a control input vector of the current time t and recording the control input vector as B_t：

Wherein v is an initial time when t is 1_t-1＝0；

c. Establishing a vehicle kinematic model with noise at the current moment t, 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;

d. the current time t is compared with the time f (A)_t,B_t) With respect to the state vector A_tThe Jacobian matrix is denoted as F_t，F_tExpressed by the formula (1.8):

e. the covariance matrix after state propagation is recorded as

Using equation (1.9) to carry out covariance matrix after state propagationUpdating:

wherein P represents the latest value of the state covariance matrix before the current time T, 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.:

f. establishing a GPS observation model at the current time t:

wherein Z_tFor the observation vector of the GPS observation model at the current time t,

an observation function of the GPS observation model at the current time t; 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:

g. observing the current time t

With respect to the state vector A_tThe Jacobian matrix is recorded as H_t，H_tExpressed by the formula (1.14):

h. the observation residual error of the GPS observation model at the current time t is recorded as y_tCalculating the observation residual y of the GPS observation model at the current time t_t：

i. Let the current time t Kalman gain be K_tCalculating the current time t Kalman gain K_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;

j. for state vector A_tAnd updating the state covariance matrix P:

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

is its current latest value;

k. will be (x'_t，y′_t) As the final position coordinates of the vehicle at the current time t, theta'_tAs the current time tAnd (4) adopting the final position coordinate (x ') of the vehicle at the current moment t'_t，y′_t) And the final heading angle theta of the vehicle at the current moment t'_tForming the final motion track of the vehicle at the current moment t, storing the track,

(4) and (5) updating t by adding 1 to the current value of t, and returning to the step (3) to record the track at the next moment.

Compared with the prior art, the invention has the advantages that the GPS longitude and latitude data, the satellite quantity information and the PDOP (position distribution of precision) position precision factor information of the position of the vehicle at the current moment t are obtained through the differential GPS module, and the vehicle speed v of the vehicle at the current moment t is obtained through the whole vehicle CAN analysis module_tAnd steering wheel angle omega_tThe differential GPS module adopts a double-antenna GPS and a differential RTK service provided by a thousand-searching position to carry out coordinate transformation on GPS longitude and latitude data acquired at the current moment t to obtain a vehicle position coordinate under a vehicle coordinate system at the current moment t

And course angle

Wherein

An abscissa indicating the vehicle position in the vehicle coordinate system at the present time t,the vertical coordinate of the vehicle position under the vehicle coordinate system at the current moment t is represented, and the motion trail generated by the vehicle kinematics is obtained through a vehicle kinematics track estimation algorithm, namely the vehicle position estimation coordinate at the current moment t

And course angle estimate

Judging according to the satellite number information acquired at the current moment tWhether vehicle GPS longitude and latitude data acquired by the differential GPS module at the current moment t is lost or not is judged if the satellite number information is less than 4, and at the moment, the vehicle position estimation coordinate at the current moment t is used for estimating the coordinate

And course angle estimate

The motion trajectory is used as the final motion trajectory of the vehicle at the current moment t, and the trajectory is stored; if the satellite quantity information is more than or equal to 4, judging that the satellite quantity information is not lost, judging the noise of the GPS longitude and latitude data by using a PDOP (position Dilution of precision) position precision factor acquired at the current time t, if the PDOP is less than 3, judging that the noise of the GPS longitude and latitude data is small, and judging that the noise of the vehicle position coordinate obtained at the current time t is small

And course angle

The method is used for storing the track as the final motion track of the vehicle at the current moment t, if PDOP is more than or equal to 3, the GPS longitude and latitude data is judged to have larger noise, at the moment, an extended Kalman filtering algorithm is adopted for carrying out data fusion first and then carrying out track recording, the method realizes track presumption positioning of the vehicle through a kinematic model, effectively utilizes motion information of the vehicle for positioning, realizes local positioning and track generation of the vehicle through the kinematic model track presumption under the condition that the GPS longitude and latitude data are lost, realizes track fusion of the kinematic model and a differential GPS module track through the extended Kalman filtering algorithm aiming at the condition that a noise point occurs in a differential GPS module, and improves the smoothness and track generation precision of the track, so that the track recording method has no track loss and continuous track recording on the basis of smooth track.

Detailed Description

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