梁 锋、黄政华
（上汽通用五菱汽车股份有限公司，柳州 545007）
近年来，随着物联网技术、无人驾驶、感知控制算法及传感器不断发展成熟，无人驾驶技术不断涌入物流、公共交通、出租车和餐饮配送等行业，逐渐展现出智能移动机器人替换传统人工的新兴景象[1]。为达到物流行业无人配送目的，本文对牵引车进行改造升级，如增加感知定位传感器、电子制动、驻车、转向控制器以及大算力的智驾控制器等，通过CAN 总线，智驾控制系统与车辆控制系统进行交互。升级改造后的车辆，通过智驾感知传感器，车辆可自行感知周围环境和车辆当前位置、位姿，通过循迹算法，自动控制车辆转向、驱动电机工作，按既定点云地图上的路线前往指定位置，最终实现无人物流配送功能[2]。
在整个智驾系统，车辆的感知定位及循迹算法均依赖于提前采集和创建的高精度地图数据。地图创建的可靠性，直接影响车辆运行时的匹配准确性，进而决定车辆能否完成智驾功能[3]。
无人驾驶的快速发展离不开控制器和智能传感器的不断发展，只有丰富的环境感知数据和高算力的控制器，才能支撑整个智驾系统所有算法模块均在毫秒级内完成相应感知定位和规控任务[4]。本文硬件系统主要包括车辆载体、智驾控制器、组合惯导和3D 激光雷达（图1）。
图1 无人驾驶硬件系统架构图
其中，车辆本体选用线控底盘技术较为成熟的E300 作为载体，激光雷达选用速腾聚创的16 线激光雷达，组合惯导选用导远INS570D 组合惯导，控制器采用米文Apex Xavier Ⅱ（图2）。控制器和传感器安装在车顶，硬件之间通过网线或者CAN 总线连接。
图2 硬件组装效果图
由于生产制造工艺、物流运输、使用环境变化以及组装过程影响，精密仪器的实际使用直接输出值与理论值均存在一定偏差，为此，在组合惯导、激光雷达等传感器使用之前，均需对所有传感器进行标定。组合惯导标定参考导远官方标定手册标定即可，而激光雷达标定，需借助开源“IMU+Lidar”联合标定软件完成标定工作，最终得到激光雷达与惯导间旋转平移关系矩阵以及载体当前的横滚角、俯仰角和偏航角，为建图时的坐标统一提供转换支撑。
为了将所有精力集中到功能模块的开发上，提高开发效率，本系统软件设计基于Ubuntu18.04 操作系统，通讯架构成熟稳定；
分布式的ROS（Robot Operating System，ROS）中间框架，利用ROS 通讯机制，实现各个算法模块间的数据通讯。实时定位与建图的实现主要需要激光点云、GPS 和IMU 数据[5]。
激光雷达采用UDP 协议通讯，通过网线与控制器连接，集成对应型号的雷达驱动后，可从系统发布的话题中获取激光雷达的实时点云数据。如速腾16 线的雷达RS_LIDAR_16 对应输出的话题为/rslidar_points。对于GPS 位置、IMU 数据，通过集成CAN 驱动，把解析后的CAN 报文转换成ROS 系统Subscribe/Publish 通讯模式，发布到系统中，供其他功能模块订阅使用。本系统软件架构设计如图3 所示。
图3 建图软件系统架构图
系统主要由数据转换、imu 预积分、点云映射、特征提取和地图数据融合优化等几个模块组成。数据转换模块car_driver从CAN 总线获取imu、gps 数据，通过CAN 协议解析，转换成ROS 通讯话题后发布到系统中，供其他算法模块订阅使用。imu预积分模块imuPreintegration 订阅/imu 数据进行预积分后，发布imu 实时里程数据。点云映射模块imageProjection 主要用于激光点云数据处理，发布矫正后的点云数据信息。特征提取模块featureExtraction 主要是提取有效点云的特征数据。地图优化融合模块订阅点云信息及gps 数据进行计算融合，并发布雷达里程计数据，最后将关键帧数据集合发布到地图保存服务，通过PCL点云库保存成我们需要的pcd 文件（图4）。
图4 3D 点云图
3.1 数据转换模块
整个驾控系统中，与车辆CAN 总线进行数据、命令交互的模块是car_driver。该模块的一个功能是通过集成第三方CAN 动态库，将特定波特率的CAN 数据读取到驾控系统，并根据协议解析指定帧头的帧数据，获取到我们需要的实时数据；
同时，将数据按ROS 系统通讯架构，发布到驾控系统中，如imu 数据/imu，gps 数据/gps/fix。car_driver 的另一个功能就是解析需要下发到车辆控制执行器的的话题，将话题转换成CAN 报文，发布到CAN 总线（图5）。
图5 数据模块
3.2 imu 预积分模块
预积分模块主要由两部分构成：一部分是订阅及处理imu 数据，另一部分是订阅及处理激光里程计数据。
imu 处理过程主要是订阅实时imu 数据，通过雷达与imu的外参矩阵把imu 数据转换到雷达坐标系后保存到消息队列中。同时，利用一个全局标志，控制系统是否继续将imu 数据添加到积分器中进行预积分，并结合上一帧优化后的数据完成当前状态的预测，发布当前里程计状态数据。
文中雷达数据频率为10 Hz，imu 数据频率是200 Hz，在雷达里程计数据还未完成优化并预测最新位置时，默认imu 数据直接添加到积分器中进行预积分。当完成当前状态优化后，记录优化后的位置、状态及偏差值。优化的过程主要是借助GTSAM 因子图库，将所有约束都添加到因子图中，执行优化操作（图6）。
图6 imu 预积分模块
3.3 图像映射模块
图像映射模块主要功能是处理激光点云数据，通过订阅imu数据以及imu 里程计数据，根据激光点云当前帧的起止时间，对imu 及imu 里程计数据计算起止时间的状态变化量。
点云处理过程是遍历当前点云帧数据，利用每个点的位置（x、y、z 值）计算出所有点与原点的的距离，过滤部分有问题或距离不符合需求的点云。同时，通过imu 数据计算得到的旋转矩阵和imu 里程计数据计算平移矩阵，将所有过滤后的点云都转换到第一个激光点的坐标系下，进行运动补偿。最后将具有距离的所有激光点云都统一保存到点云信息结构中，发布经过矫正及过滤后的云信息（图7）。
图7 图像映射模块
3.4 特征提取模块
特征提取模块主要是处理经过运动畸变矫正、过滤无效点云后的点云数据。首先是订阅去畸变后的有效点云信息，通过计算点云曲率，并以曲率为基础再次过滤掉与激光线曲率或者被遮挡的点。之后，再对剩余的点云进行分割提取平面和角点，保存到点云信息中。最后发布角点、面点点云信息（图8）。
图8 特征提取模块
3.5 地图优化模块
地图优化模块主要功能是根据当前帧和当前帧时空上较近的全局地图匹配，获取到当前帧一个初步的位姿。最终通过融合lidar 里程计的因子、GPS 因子和闭环因子，获得一个准确的位姿，同时更新所有历史关键帧的位姿，最后把优化后的数据集发布出去（图9）。
图9 地图优化模块
要完成当前帧到附近全局地图的匹配并获取到较好的位姿，首先要根据当前关键帧位置搜索最近的地图关键帧集合，提取角点、平面点等特征，加入局部地图。其次，对当前帧角点、平面点进行降采样。最后，执行迭代优化，即对关键帧特征点进行匹配，并将匹配上了的点集都加入同一集合。同时构建高斯牛顿方程，迭代优化并同步更新当前位姿，从而完成帧到地图的匹配过程，获取当前较好的位姿[6]。
在获取到当前帧位姿后，计算与上一关键帧的变化量，以确定是否添加当前帧为关键帧。同时，将约束条件都加到因子图优化中，获取当前帧优化后姿态数据并执行更新操作。最后将优化后的数据集向外发布，并利用点云库保存全局点云地图，即可完成数据集保存及地图创建。
本文基于车载驾控硬件及建图算法，研究无人物流车的多传感器融合的实时定位与建图技术，实现高精度点云地图创建。在整个系统运行中，由于点云数据量大且有实时性的要求，对控制器配置要求较高，配置过低，有出现卡滞或者宕机可能。在点云关键帧与地图配准过程中，需要较多特征点进行比对，故针对空旷且周围缺少固定特征的场景，定位结果容易有偏差。为解决该问题，可在此基础上增加视觉传感器，融合图像数据，使得提取到的关键特征数据有所增加，提高系统冗余性与稳定性。
猜你喜欢 里程计关键帧位姿 室内退化场景下UWB双基站辅助LiDAR里程计的定位方法导航定位学报(2022年5期)2022-10-13一种单目相机/三轴陀螺仪/里程计紧组合导航算法中国惯性技术学报(2019年1期)2019-05-21基于模板特征点提取的立体视觉里程计实现方法传感器与微系统(2018年7期)2018-08-29基于改进关键帧选择的RGB-D SLAM算法大连理工大学学报(2017年4期)2017-08-07基于共面直线迭代加权最小二乘的相机位姿估计光学精密工程(2016年5期)2016-11-07基于CAD模型的单目六自由度位姿测量光学精密工程(2016年4期)2016-11-07基于相关系数的道路监控视频关键帧提取算法重庆交通大学学报(自然科学版)(2016年1期)2016-05-25大角度斜置激光惯组与里程计组合导航方法导航定位与授时(2016年6期)2016-03-16小型四旋翼飞行器位姿建模及其仿真湖北工业大学学报(2016年5期)2016-02-27基于聚散熵及运动目标检测的监控视频关键帧提取西北工业大学学报(2015年3期)2015-12-14