阅读说明：本技术 一种基于差分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.)

一种基于差分GPS和车辆运动学模型的轨迹录制方法

技术领域

本发明涉及一种车辆轨迹录制方法，尤其是涉及一种基于差分GPS和车辆运动学模型的轨迹录制方法。

背景技术

实现车辆寻迹功能是无人驾驶车辆的首要任务之一，目前在室外实现寻迹功能的定位模块主要以差分GPS、视觉SLAM和激光SLAM为主。视觉SLAM和激光SLAM首先通过地图构建模块对车辆周围环境进行地图构建与轨迹保存，然后通过定位模块按照保存的地图进行车辆定位，通过车辆跟随模块实现车辆的寻迹。但是视觉SLAM和激光SLAM受环境动态目标的影响较大，鲁棒性较差，成本较高。

采用差分GPS方法实现寻迹是首要方法，在无遮挡和信号较好的环境下定位精度可达厘米级，可以满足无人驾驶寻迹的要求。但是当GPS处于障碍物较多(如立交桥)处时，GPS显示存在较多误差较大的漂移点，极端情况下还会出现丢失定位坐标数据的情况，录制的轨迹出现较大的噪声点、不平滑。有研究提出对GPS轨迹进行插值优化处理，该方法可以解决GPS轨迹不平滑的问题，但是难以解决轨迹丢失问题，然而无人驾驶车辆实现寻迹要求GPS录制的轨迹不仅要平滑，更重要的是轨迹要连续。

发明内容

本发明所要解决的技术问题是提供一种在具有平滑轨迹的基础上，不存在轨迹丢失，轨迹录制连续的基于差分GPS和车辆运动学模型的轨迹录制方法。

本发明解决上述技术问题所采用的技术方案为：一种基于差分GPS和车辆运动学模型的轨迹录制方法，包括以下步骤：

(1)设定当前时刻变量t；

(2)对当前t进行初始化赋值，令t＝1；

(3)进行当前时刻t车辆轨迹录制，具体过程为：

3.1通过差分GPS模块获取当前时刻t车辆所处位置的GPS经纬度数据、卫星数量信息和PDOP(Position Dilution of Precision)位置精度因子信息，通过整车CAN解析模块获取当前时刻t车辆的车速v_t和方向盘转角ω_t，所述的差分GPS模块采用双天线GPS和由千寻位置提供的差分RTK服务；

3.2将当前时刻t获取的GPS经纬度数据进行坐标变换，得到当前时刻t车辆坐标系下的车辆位置坐标

和航向角

其中

表示当前时刻t车辆坐标系下的车辆位置的横坐标，

表示当前时刻t车辆坐标系下的车辆位置的纵坐标；

3.3通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，即当前时刻t车辆位置估计坐标

和航向角估计值

表示当前时刻t车辆坐标系下的车辆位置的横坐标估计值，

表示当前时刻t车辆坐标系下的车辆位置的纵坐标估计值；其中车辆运动学航迹推测算法公式如下所示：

ds_t＝v_t-1·dt (1.2)

δ_ft＝ω_t·η (1.4)

其中，

表示t-1时刻车辆坐标系下的车辆位置的横坐标估计值，

表示t-1时刻车辆坐标系下的车辆位置的纵坐标估计值，表示t-1时刻车辆航向角估计值，ds_t表示从t-1时刻到当前时刻t车辆行驶的距离，dθ_t表示当前时刻t相对于t-1时刻车辆航向角变化量，v_t-1为t-1时刻车辆的车速，dt为整车CAN解析模块的采样周期，L为车辆轴距，δ_ft为当前时刻t车辆前轮转角，η为车辆的角传动比；当t＝1时，

v_t-1＝0，sin表示正弦函数，cos表示余弦函数，tan表示正切函数；

3.4根据当前时刻t获取的卫星数量信息来判断差分GPS模块在当前时刻t获取的车辆GPS经纬度数据是否存在丢失，如果卫星数量信息小于4个，则判定丢失，此时将当前时刻t车辆位置估计坐标

和航向角估计值

作为当前时刻t车辆最终的运动轨迹，进行轨迹保存；如果卫星数量信息大于等于4个，则判定未丢失，此时再当前时刻t获取的PDOP(Position Dilution of Precision)位置精度因子来判断GPS经纬度数据的噪声大小，若PDOP小于3，则判定GPS经纬度数据噪声较小，则将当前时刻t得到的车辆位置坐标

和航向角作为当前时刻t车辆最终的运动轨迹，进行轨迹保存，若PDOP大于等于3，则判定GPS经纬度数据噪声较大，此时采用扩展卡尔曼滤波算法先进行数据融合后再进行轨迹录制，具体过程为：

a、采用

和

构建当前时刻t的状态向量，将该状态向量记为A_t，采用式(1.5)对A_t进行初始化赋值：

其中，在t＝1时的初始时刻，

b、采用v_t-1和δ_ft构建当前时刻t的控制输入向量，将其记为B_t：

其中，在t＝1时的初始时刻，v_t-1＝0；

c、建立当前时刻t带噪声的车辆运动学模型，将其向量表达式记为f(A_t,B_t)：

其中，N(·)为高斯白噪声生成函数，N(0,Q)表示采用高斯白噪声生成函数生成的维度为3×1的高斯白噪声向量，其中，0为高斯白噪声生成函数的均值，Q为高斯白噪声生成函数的状态传播过程协方差矩阵，状态传播过程协方差矩阵Q为采用随机函数生成的维度为3x3的矩阵，生成后为固定值；

d、将当前时刻t时f(A_t,B_t)关于状态向量A_t的雅克比矩阵记为F_t，F_t采用式(1.8)表示为：

e、将状态传播后的协方差矩阵记为

采用公式(1.9)对状态传播后的协方差矩阵进行更新：

其中P表示当前时刻t之前状态协方差矩阵的最新值，上标T表示矩阵的转置；当t＝1时，即初始时刻，P初始化为维度为3×3的单位矩阵，即：

f、建立当前时刻t的GPS观测模型：

其中Z_t为当前时刻t的GPS观测模型的观测向量，

为当前时刻t的GPS观测模型的观测函数；N(·)为高斯白噪声生成函数，N(0,R)表示采用高斯白噪声生成函数生成的高斯白噪声向量，N(0,R)的维度为3×1，其中，0为高斯白噪声生成函数的均值，R为观测协方差矩阵，观测协方差矩阵R的维度为3×3，观测协方差矩阵R为：

g、将当前时刻t观测函数

关于状态向量A_t的雅克比矩阵记为H_t，H_t采用式(1.14)表示为：

h、当前时刻t的GPS观测模型的观测残差记为y_t，计算当前时刻t的GPS观测模型的观测残差y_t：

i、将当前时刻t卡尔曼增益记为K_t，计算当前时刻t卡尔曼增益K_t：

上式中，

的取值为其当前最新值；上标“-1”表示矩阵逆运算；

j、对状态向量A_t和状态协方差矩阵P进行更新：

其中，I为维度为3×3的单位矩阵，

的取值为其当前最新值；

k、将(x′_t，y′_t)作为当前时刻t车辆最终位置坐标，将θ′_t作为当前时刻t车辆最终航向角，采用当前时刻t车辆最终位置坐标(x′_t，y′_t)和当前时刻t车辆最终航向角θ′_t构成当前时刻t车辆最终的运动轨迹，进行轨迹保存，

(4)采用t的当前值加1的值更新t，返回步骤(3)进行下一时刻的轨迹录制。

与现有技术相比，本发明的优点在于通过差分GPS模块获取当前时刻t车辆所处位置的GPS经纬度数据、卫星数量信息和PDOP(Position Dilution of Precision)位置精度因子信息，通过整车CAN解析模块获取当前时刻t车辆的车速v_t和方向盘转角ω_t，差分GPS模块采用双天线GPS和由千寻位置提供的差分RTK服务，将当前时刻t获取的GPS经纬度数据进行坐标变换，得到当前时刻t车辆坐标系下的车辆位置坐标

和航向角

其中

表示当前时刻t车辆坐标系下的车辆位置的横坐标，表示当前时刻t车辆坐标系下的车辆位置的纵坐标，通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，即当前时刻t车辆位置估计坐标

和航向角估计值

根据当前时刻t获取的卫星数量信息来判断差分GPS模块在当前时刻t获取的车辆GPS经纬度数据是否存在丢失，如果卫星数量信息小于4个，则判定丢失，此时将当前时刻t车辆位置估计坐标

和航向角估计值

作为当前时刻t车辆最终的运动轨迹，进行轨迹保存；如果卫星数量信息大于等于4个，则判定未丢失，此时再当前时刻t获取的PDOP(Position Dilution of Precision)位置精度因子来判断GPS经纬度数据的噪声大小，若PDOP小于3，则判定GPS经纬度数据噪声较小，则将当前时刻t得到的车辆位置坐标

和航向角

作为当前时刻t车辆最终的运动轨迹，进行轨迹保存，若PDOP大于等于3，则判定GPS经纬度数据噪声较大，此时采用扩展卡尔曼滤波算法先进行数据融合后再进行轨迹录制，本发明通过运动学模型实现车辆的航迹推测定位，有效利用了车辆的运动信息进行定位，在GPS经纬度数据丢失情况下，通过运动学模型航迹推测实现车辆局部定位和轨迹生成，针对差分GPS模块出现噪声点的情况，通过扩展卡尔曼滤波算法实现运动学模型的轨迹与差分GPS模块轨迹融合，提高轨迹的平滑性和轨迹生成精度，由此本发明的轨迹录制方法在具有平滑轨迹的基础上，不存在轨迹丢失，轨迹录制连续。

具体实施方式

以下结合实施例对本发明作进一步详细描述。

实施例：一种基于差分GPS和车辆运动学模型的轨迹录制方法，包括以下步骤：

(1)设定当前时刻变量t；

(2)对当前t进行初始化赋值，令t＝1；

(3)进行当前时刻t车辆轨迹录制，具体过程为：

3.1通过差分GPS模块获取当前时刻t车辆所处位置的GPS经纬度数据、卫星数量信息和PDOP(Position Dilution of Precision)位置精度因子信息，通过整车CAN解析模块获取当前时刻t车辆的车速v_t和方向盘转角ω_t，差分GPS模块采用双天线GPS和由千寻位置提供的差分RTK服务；

3.2将当前时刻t获取的GPS经纬度数据进行坐标变换，得到当前时刻t车辆坐标系下的车辆位置坐标和航向角

其中

表示当前时刻t车辆坐标系下的车辆位置的横坐标，

表示当前时刻t车辆坐标系下的车辆位置的纵坐标；

3.3通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，即当前时刻t车辆位置估计坐标

和航向角估计值

表示当前时刻t车辆坐标系下的车辆位置的横坐标估计值，表示当前时刻t车辆坐标系下的车辆位置的纵坐标估计值；其中车辆运动学航迹推测算法公式如下所示：

ds_t＝v_t-1·dt (1.5)

δ_ft＝ω_t·η (1.4)

其中，

表示t-1时刻车辆坐标系下的车辆位置的横坐标估计值，

表示t-1时刻车辆坐标系下的车辆位置的纵坐标估计值，

表示t-1时刻车辆航向角估计值，ds_t表示从t-1时刻到当前时刻t车辆行驶的距离，dθ_t表示当前时刻t相对于t-1时刻车辆航向角变化量，v_t-1为t-1时刻车辆的车速，dt为整车CAN解析模块的采样周期，L为车辆轴距，δ_ft为当前时刻t车辆前轮转角，η为车辆的角传动比，η预先根据车辆型号通过现有成熟的实验方法标定得到；当t＝1时，

v_t-1＝0，sin表示正弦函数，cos表示余弦函数，tan表示正切函数；

3.4根据当前时刻t获取的卫星数量信息来判断差分GPS模块在当前时刻t获取的车辆GPS经纬度数据是否存在丢失，如果卫星数量信息小于4个，则判定丢失，此时将当前时刻t车辆位置估计坐标

和航向角估计值

作为当前时刻t车辆最终的运动轨迹，进行轨迹保存；如果卫星数量信息大于等于4个，则判定未丢失，此时再当前时刻t获取的PDOP(Position Dilution of Precision)位置精度因子来判断GPS经纬度数据的噪声大小，若PDOP小于3，则判定GPS经纬度数据噪声较小，则将当前时刻t得到的车辆位置坐标

和航向角

作为当前时刻t车辆最终的运动轨迹，进行轨迹保存，若PDOP大于等于3，则判定GPS经纬度数据噪声较大，此时采用扩展卡尔曼滤波算法先进行数据融合后再进行轨迹录制，具体过程为：

a、采用和

构建当前时刻t的状态向量，将该状态向量记为A_t，采用式(1.5)对A_t进行初始化赋值：

其中，在t＝1时的初始时刻，

b、采用v_t-1和δ_ft构建当前时刻t的控制输入向量，将其记为B_t：

其中，在t＝1时的初始时刻，v_t-1＝0；

c、建立当前时刻t带噪声的车辆运动学模型，将其向量表达式记为f(A_t,B_t)：

其中，N(·)为高斯白噪声生成函数，N(0,Q)表示采用高斯白噪声生成函数生成的维度为3×1的高斯白噪声向量，其中，0为高斯白噪声生成函数的均值，Q为高斯白噪声生成函数的状态传播过程协方差矩阵，状态传播过程协方差矩阵Q为采用随机函数生成的维度为3x3的矩阵，生成后为固定值；

d、将当前时刻t时f(A_t,B_t)关于状态向量A_t的雅克比矩阵记为F_t，F_t采用式(1.8)表示为：

e、将状态传播后的协方差矩阵记为

采用公式(1.8)对状态传播后的协方差矩阵

进行更新：

其中P表示当前时刻t之前状态协方差矩阵的最新值，上标T表示矩阵的转置；当t＝1时，即初始时刻，P初始化为维度为3×3的单位矩阵，即：

f、建立当前时刻t的GPS观测模型：

其中Z_t为当前时刻t的GPS观测模型的观测向量，

为当前时刻t的GPS观测模型的观测函数；N(·)为高斯白噪声生成函数，N(0,R)表示采用高斯白噪声生成函数生成的高斯白噪声向量，N(0,R)的维度为3×1，其中，0为高斯白噪声生成函数的均值，R为观测协方差矩阵，观测协方差矩阵R的维度为3×3，观测协方差矩阵R为：

g、将当前时刻t观测函数

关于状态向量A_t的雅克比矩阵记为H_t，H_t采用式(1.14)表示为：

h、当前时刻t的GPS观测模型的观测残差记为y_t，计算当前时刻t的GPS观测模型的观测残差y_t：

i、将当前时刻t卡尔曼增益记为K_t，计算当前时刻t卡尔曼增益K_t：

上式中，

的取值为其当前最新值；上标“-1”表示矩阵逆运算；

j、对状态向量A_t和状态协方差矩阵P进行更新：

其中，I为维度为3×3的单位矩阵，

的取值为其当前最新值；

k、将(x′_t，y′_t)作为当前时刻t车辆最终位置坐标，将θ′_t作为当前时刻t车辆最终航向角，采用当前时刻t车辆最终位置坐标(x′_t，y′_t)和当前时刻t车辆最终航向角θ_t'构成当前时刻t车辆最终的运动轨迹，进行轨迹保存，

(4)采用t的当前值加1的值更新t，返回步骤(3)进行下一时刻的轨迹录制。