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

说明书

本发明公开了了一种基于差分GPS和车辆运动学模型的轨迹录制方法，通过差分GPS模块获取当前时刻t车辆所处位置的GPS经纬度数据、卫星数量信息和PDOP位置精度因子信息，通过整车CAN解析模块获取当前时刻t车辆的车速v_t和方向盘转角ω_t，通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，根据当前时刻t获取的卫星数量信息来判断差分GPS模块在当前时刻t获取的车辆GPS经纬度数据是否存在丢失，再未丢失时判定GPS经纬度数据噪声，当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)