技术摘要：
本发明公开了一种基于三维点云地图定位的智能车纯追踪循迹方法，包括将智能车的激光雷达和组合导航系统进行时间同步及频率校准，并通过组合导航系统确定智能车的平面坐标；获取智能车的轨迹点和点云地图；确定智能车的定位点和偏航角；确定智能车的循迹目标；输出控制 全部
背景技术：
路径追踪(跟踪)方法是指载体能够自主地产生相对平滑的控制指令，从而使自身 按照预设路径安全平稳行驶的一种控制方法，是自动驾驶的关键技术之一。 目前常用的跟踪算法主要分为两类：基于控制理论以及基于几何学控制。常见的 基于控制理论的方法有：PID控制、模糊控制、模型预测控制等，此类方法需要对载体的运动 进行建模与分析，往往需要很强的理论基础，通常需要非常精确的模型才能达到理想效果， 其关键参数的调参对整个系统的稳定性影响巨大。基于几何学的控制以其实现简单，易于 编程，物理意义明显在该领域内广受欢迎，其中Pure Pursit是一种经典而有效的方法，目 前在自动驾驶上往往和组合导航系统结合应用，通过GPS记录轨迹点以及确定自身位姿，通 过一段圆弧来拟合自身位置和目标点，通过单车模型来确定相应的汽车转角。但是在地下 停车场、树荫遮挡、高楼林立等GPS信号不佳或缺失的环境下无法正确地获取到当前车身的 位姿，进而导致追踪方法的失效。
技术实现要素：
： 本发明的目的是提供一种基于三维点云地图定位的智能车纯追踪循迹方法，以弥 补现有技术的不足。 为达到上述目的，本发明采取的具体技术方案为： 一种基于三维点云地图定位的智能车纯追踪循迹方法，包括以下步骤： S1：将智能车的激光雷达和组合导航系统进行时间同步及频率校准，并通过组合 导航系统确定智能车的平面坐标； S2：获取智能车的轨迹点和点云地图； S3：确定智能车的定位点P_local和偏航角heading； S4：确定智能车的循迹目标点P_goal； S5：输出控制指令：在获取定位点P_local、偏航角heading、目标点P_goal后，通过 纯跟踪理论获取单车模型下的前轮转角，并对方向盘转角和车轮转角进行标定得到相关 map图，从而得到方向盘控制信号，实现追踪循迹。 进一步的，所述S1中，进行时间同步及频率校准，以确保激光雷达和组合导航系统 数据获取的时间一致；组合导航系统中GPS采用载波相位差分技术(Real-time kinematic， 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- 4 CN 111578957 A 说 明 书 2/5 页 GPS_Y0)。 进一步的，所述S2中：首先通过组合导航系统确定智能车车身起始位置坐标系与 全局坐标系之间的变换关系；接着利用激光雷达获取汽车周围环境点云信息，采取LeGO- LOAM SLAM方法建立周围环境地图，并在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，由几何关系可得车轮转角 整理获取车轮转角 后续在旋转方向盘转角的同时对车轮转角进行记录，完成方向盘转角 和车轮转角之间的标定。即可获取汽车在输出某车轮转角时方向盘转角的大小。 本发明的优点和技术效果： 本发明针对当前智能驾驶中Pure Pursit方法在缺失GPS信号时无法工作的情况， 提出了一种基于三维点云地图定位的智能车纯追踪循迹方法，利用车载激光雷达和组合导 航系统对行驶过程中的周围环境进行建图，同时在地图上记录行驶轨迹点，从而实现在GPS 信号丢失情况下的Pure Pursit循迹功能。本发明利用车载激光雷达与预先建立的点云地 图匹配来确定汽车位姿，通过跟踪点云地图中的目标点实现循迹算法，本发明能够在GPS信 号不佳或缺失的环境下工作，鲁棒性强，算法新颖，能够适用于绝大部分场景。 5 CN 111578957 A 说 明 书 3/5 页 附图说明 图1为数据预处理流程图； 图2为点云地图及轨迹点获取流程图； 图3为智能车定位流程图； 图4为循迹目标点确定的流程图； 图5为控制指令输出流程图； 图6为Pure Pursit算法原理图； 图7为设备数据流示意图。
本发明公开了一种基于三维点云地图定位的智能车纯追踪循迹方法，包括将智能车的激光雷达和组合导航系统进行时间同步及频率校准，并通过组合导航系统确定智能车的平面坐标；获取智能车的轨迹点和点云地图；确定智能车的定位点和偏航角；确定智能车的循迹目标；输出控制 全部
背景技术：
路径追踪(跟踪)方法是指载体能够自主地产生相对平滑的控制指令，从而使自身 按照预设路径安全平稳行驶的一种控制方法，是自动驾驶的关键技术之一。 目前常用的跟踪算法主要分为两类：基于控制理论以及基于几何学控制。常见的 基于控制理论的方法有：PID控制、模糊控制、模型预测控制等，此类方法需要对载体的运动 进行建模与分析，往往需要很强的理论基础，通常需要非常精确的模型才能达到理想效果， 其关键参数的调参对整个系统的稳定性影响巨大。基于几何学的控制以其实现简单，易于 编程，物理意义明显在该领域内广受欢迎，其中Pure Pursit是一种经典而有效的方法，目 前在自动驾驶上往往和组合导航系统结合应用，通过GPS记录轨迹点以及确定自身位姿，通 过一段圆弧来拟合自身位置和目标点，通过单车模型来确定相应的汽车转角。但是在地下 停车场、树荫遮挡、高楼林立等GPS信号不佳或缺失的环境下无法正确地获取到当前车身的 位姿，进而导致追踪方法的失效。
技术实现要素：
： 本发明的目的是提供一种基于三维点云地图定位的智能车纯追踪循迹方法，以弥 补现有技术的不足。 为达到上述目的，本发明采取的具体技术方案为： 一种基于三维点云地图定位的智能车纯追踪循迹方法，包括以下步骤： S1：将智能车的激光雷达和组合导航系统进行时间同步及频率校准，并通过组合 导航系统确定智能车的平面坐标； S2：获取智能车的轨迹点和点云地图； S3：确定智能车的定位点P_local和偏航角heading； S4：确定智能车的循迹目标点P_goal； S5：输出控制指令：在获取定位点P_local、偏航角heading、目标点P_goal后，通过 纯跟踪理论获取单车模型下的前轮转角，并对方向盘转角和车轮转角进行标定得到相关 map图，从而得到方向盘控制信号，实现追踪循迹。 进一步的，所述S1中，进行时间同步及频率校准，以确保激光雷达和组合导航系统 数据获取的时间一致；组合导航系统中GPS采用载波相位差分技术(Real-time kinematic， 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- 4 CN 111578957 A 说 明 书 2/5 页 GPS_Y0)。 进一步的，所述S2中：首先通过组合导航系统确定智能车车身起始位置坐标系与 全局坐标系之间的变换关系；接着利用激光雷达获取汽车周围环境点云信息，采取LeGO- LOAM SLAM方法建立周围环境地图，并在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，由几何关系可得车轮转角 整理获取车轮转角 后续在旋转方向盘转角的同时对车轮转角进行记录，完成方向盘转角 和车轮转角之间的标定。即可获取汽车在输出某车轮转角时方向盘转角的大小。 本发明的优点和技术效果： 本发明针对当前智能驾驶中Pure Pursit方法在缺失GPS信号时无法工作的情况， 提出了一种基于三维点云地图定位的智能车纯追踪循迹方法，利用车载激光雷达和组合导 航系统对行驶过程中的周围环境进行建图，同时在地图上记录行驶轨迹点，从而实现在GPS 信号丢失情况下的Pure Pursit循迹功能。本发明利用车载激光雷达与预先建立的点云地 图匹配来确定汽车位姿，通过跟踪点云地图中的目标点实现循迹算法，本发明能够在GPS信 号不佳或缺失的环境下工作，鲁棒性强，算法新颖，能够适用于绝大部分场景。 5 CN 111578957 A 说 明 书 3/5 页 附图说明 图1为数据预处理流程图； 图2为点云地图及轨迹点获取流程图； 图3为智能车定位流程图； 图4为循迹目标点确定的流程图； 图5为控制指令输出流程图； 图6为Pure Pursit算法原理图； 图7为设备数据流示意图。
