基于三维点云地图定位的智能车纯追踪循迹方法技术

本发明专利技术公开了一种基于三维点云地图定位的智能车纯追踪循迹方法，包括将智能车的激光雷达和组合导航系统进行时间同步及频率校准，并通过组合导航系统确定智能车的平面坐标；获取智能车的轨迹点和点云地图；确定智能车的定位点和偏航角；确定智能车的循迹目标；输出控制指令，从而得到方向盘控制信号，实现追踪循迹。本发明专利技术利用车载激光雷达与预先建立的点云地图匹配来确定汽车位姿，通过跟踪点云地图中的目标点实现循迹算法，本发明专利技术能够在GPS信号不佳或缺失的环境下工作，鲁棒性强，算法新颖，能够适用于绝大部分场景。

【技术实现步骤摘要】
基于三维点云地图定位的智能车纯追踪循迹方法
本专利技术涉及汽车智能驾驶
，具体涉及一种基于三位点云地图的智能车纯追踪循迹方法。
技术介绍
路径追踪(跟踪)方法是指载体能够自主地产生相对平滑的控制指令，从而使自身按照预设路径安全平稳行驶的一种控制方法，是自动驾驶的关键技术之一。目前常用的跟踪算法主要分为两类：基于控制理论以及基于几何学控制。常见的基于控制理论的方法有：PID控制、模糊控制、模型预测控制等，此类方法需要对载体的运动进行建模与分析，往往需要很强的理论基础，通常需要非常精确的模型才能达到理想效果，其关键参数的调参对整个系统的稳定性影响巨大。基于几何学的控制以其实现简单，易于编程，物理意义明显在该领域内广受欢迎，其中PurePursit是一种经典而有效的方法，目前在自动驾驶上往往和组合导航系统结合应用，通过GPS记录轨迹点以及确定自身位姿，通过一段圆弧来拟合自身位置和目标点，通过单车模型来确定相应的汽车转角。但是在地下停车场、树荫遮挡、高楼林立等GPS信号不佳或缺失的环境下无法正确地获取到当前车身的位姿，进而导致追踪方法的失效。
技术实现思路
：本专利技术的目的是提供一种基于三维点云地图定位的智能车纯追踪循迹方法，以弥补现有技术的不足。为达到上述目的，本专利技术采取的具体技术方案为：一种基于三维点云地图定位的智能车纯追踪循迹方法，包括以下步骤：S1：将智能车的激光雷达和组合导航系统进行时间同步及频率校准，并通过组合导航系统确定智能车的平面坐标；S2：获取智能车的轨迹点和点云地图；S3：确定智能车的定位点P_local和偏航角heading；S4：确定智能车的循迹目标点P_goal；S5：输出控制指令：在获取定位点P_local、偏航角heading、目标点P_goal后，通过纯跟踪理论获取单车模型下的前轮转角，并对方向盘转角和车轮转角进行标定得到相关map图，从而得到方向盘控制信号，实现追踪循迹。进一步的，所述S1中，进行时间同步及频率校准，以确保激光雷达和组合导航系统数据获取的时间一致；组合导航系统中GPS采用载波相位差分技术(Real-timekinematic，RTK)，能够实现厘米级别的定位精度；由于GPS直接输出的是经纬度信息，所以需要通过高斯投影至平面坐标系，得到平面坐标(GPS_X，GPS_Y)；以起始点为地图原点建立全局坐标系，将起始的平面坐标记为GPS_0(GPS_X0，GPS_Y0)，将后续获得的平面坐标记为GPS_i(GPS_Xi，GPS_Yi),则后续坐标点在全局坐标系表示为GPS_i0(GPS_Xi-GPS_X0，GPS_Yi-GPS_Y0)。进一步的，所述S2中：首先通过组合导航系统确定智能车车身起始位置坐标系与全局坐标系之间的变换关系；接着利用激光雷达获取汽车周围环境点云信息，采取LeGO-LOAMSLAM方法建立周围环境地图，并在LeGO-LOAM方法中的帧间匹配部分采用GPS_i0作为帧间匹配初值；依次通过帧间匹配，帧与地图匹配，回环检测步骤后，获得行驶路径周围环境点云地图，其中点云地图包括关键帧点云P_i、关键帧与全局坐标系之间的位姿变换关系T_i，位姿变换关系T_i包括一个旋转矩阵R_i和一个位移向量t_i(x，y，z)，取t_i中的(x，y)作为轨迹点。进一步的，所述S3中：智能车通过激光雷达实时获取周围环境点云P_i，通过组合导航系统实时获取自身在全局坐标系下的坐标GPS_t0，首先在轨迹点序列中搜索离GPS_t0最近的轨迹点，通过该轨迹点的临近关键帧点云及其对应的位姿变换关系建立局部地图M_local，如果组合导航系统信号良好则以其获取的航向角以及位置信息作为初值，否则采用上一时刻的定位信息作为初值；利用传统的ICP方法匹配P_i和M_local，当误差函数小于阈值时，则认为匹配成功，输出对应的位姿变换关系T_t作为当前时刻智能车在全局坐标系下的精确位姿，当误差大于阈值时，则认为匹配失败，智能车无法正确定位。进一步的，所述S4中：在获取到当前智能车在全局坐标系下的精确位姿后，提取位姿中的对应的定位点P_local以及偏航角heading，并通过组合导航系统中的惯性测量单元获得车速信息；利用车速确定预瞄距离L_pre，在轨迹点序列中搜索离定位点P_local最近的轨迹点P_near，计算轨迹点P_near与定位点P_local的距离以及P_near后续n个轨迹点之间的折线距离L_z，直至折线距离L_z和超过预瞄距离L_pre，取最后一个计算的轨迹点为目标点P_goal。进一步的，所述S5中：基于S3中获得的定位点P_local、偏航角heading以及S4中获得的目标点P_goal，设定位点和目标点之间的距离为L，通过几何关系获取的车身与L的夹角为b，通过正弦定理得到转弯半径由此可得转弯曲率设前轴和后轴之间的距离为ld，由几何关系可得车轮转角整理获取车轮转角后续在旋转方向盘转角的同时对车轮转角进行记录，完成方向盘转角和车轮转角之间的标定。即可获取汽车在输出某车轮转角时方向盘转角的大小。本专利技术的优点和技术效果：本专利技术针对当前智能驾驶中PurePursit方法在缺失GPS信号时无法工作的情况，提出了一种基于三维点云地图定位的智能车纯追踪循迹方法，利用车载激光雷达和组合导航系统对行驶过程中的周围环境进行建图，同时在地图上记录行驶轨迹点，从而实现在GPS信号丢失情况下的PurePursit循迹功能。本专利技术利用车载激光雷达与预先建立的点云地图匹配来确定汽车位姿，通过跟踪点云地图中的目标点实现循迹算法，本专利技术能够在GPS信号不佳或缺失的环境下工作，鲁棒性强，算法新颖，能够适用于绝大部分场景。附图说明图1为数据预处理流程图；图2为点云地图及轨迹点获取流程图；图3为智能车定位流程图；图4为循迹目标点确定的流程图；图5为控制指令输出流程图；图6为PurePursit算法原理图；图7为设备数据流示意图。具体实施方式以下通过具体实施例并结合附图对本专利技术进一步解释和说明。实施例：一种基于三维点云地图定位的智能车纯追踪循迹方法，本方法中的硬件设备构成如图7所示，包括以下步骤：S1：如图1所示，该步骤进行数据预处理：将智能车的激光雷达和组合导航系统进行时间同步及频率校准，并通过组合导航系统确定智能车的平面坐标；进行时间同步及频率校准，以确保激光雷达和组合导航系统数据获取的时间一致；组合导航系统由GPS和IMU组成，组合导航系统中GPS采用载波相位差分技术(Real-timekinematic，RTK)，能够实现厘米级别的定位精度；由于GPS直接输出的为世界大地坐标系(即WGS-84)坐标，包括经度纬度等，而在实际应用中需要通过高斯投影转化为平面坐标系，所以需要通过高斯投影至平面坐标系，得到平面坐标(GPS_X，GPS_Y)；以起始点为地图原点建立全局坐标系，将起始的平面坐本文档来自技高网...

【技术保护点】
1.一种基于三维点云地图定位的智能车纯追踪循迹方法，其特征在于，包括以下步骤：/nS1：将智能车的激光雷达和组合导航系统进行时间同步及频率校准，并通过组合导航系统确定智能车的平面坐标；/nS2：获取智能车的轨迹点和点云地图；/nS3：确定智能车的定位点P_local和偏航角heading；/nS4：确定智能车的循迹目标点P_goal；/nS5：输出控制指令：在获取定位点P_local、偏航角heading、目标点P_goal后，通过纯跟踪理论获取单车模型下的前轮转角，并对方向盘转角和车轮转角进行标定得到相关map图，从而得到方向盘控制信号，实现追踪循迹。/n

【技术特征摘要】
1.一种基于三维点云地图定位的智能车纯追踪循迹方法，其特征在于，包括以下步骤：
S1：将智能车的激光雷达和组合导航系统进行时间同步及频率校准，并通过组合导航系统确定智能车的平面坐标；
S2：获取智能车的轨迹点和点云地图；
S3：确定智能车的定位点P_local和偏航角heading；
S4：确定智能车的循迹目标点P_goal；
S5：输出控制指令：在获取定位点P_local、偏航角heading、目标点P_goal后，通过纯跟踪理论获取单车模型下的前轮转角，并对方向盘转角和车轮转角进行标定得到相关map图，从而得到方向盘控制信号，实现追踪循迹。


2.如权利要求1所述的追踪循迹方法，其特征在于，所述S1中，进行时间同步及频率校准，组合导航系统中GPS采用载波相位差分技术，通过高斯投影至平面坐标系，得到平面坐标(GPS_X，GPS_Y)；以起始点为地图原点建立全局坐标系，将起始的平面坐标记为GPS_0(GPS_X0，GPS_Y0)，将后续获得的平面坐标记为GPS_i(GPS_Xi，GPS_Yi),则后续坐标点在全局坐标系表示为GPS_i0(GPS_Xi-GPS_X0，GPS_Yi-GPS_Y0)。


3.如权利要求1所述的追踪循迹方法，其特征在于，所述S2中：首先通过组合导航系统确定智能车车身起始位置坐标系与全局坐标系之间的变换关系；接着利用激光雷达获取汽车周围环境点云信息，采取LeGO-LOAMSLAM方法建立周围环境地图，并在LeGO-LOAM方法中的帧间匹配部分采用GPS_i0作为帧间匹配初值；依次通过帧间匹配，帧与地图匹配，回环检测步骤后，获得行驶路径周围环境点云地图，其中点云地图包括关键帧点云P_i、关键帧与全局坐标系之间的位姿变换关系T_i，位姿变换关系T_i包括一个旋转矩阵R_i和一个位移向量t_i(x，y，z)，取t_i中的(x，y)作为轨迹...

【专利技术属性】
技术研发人员：郭杰龙，魏宪，张剑锋，兰海，俞辉，李杰，邵东恒，汤璇，
申请(专利权)人：泉州装备制造研究所，
类型：发明
国别省市：福建;35