[发明专利]一种基于差分GPS和车辆运动学模型的轨迹录制方法

权利要求书

1.一种基于差分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.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)进行下一时刻的轨迹录制。