CN113085896B - 一种现代有轨清洁车辅助自动驾驶系统及方法 - Google Patents

Abstract

本发明公开了一种现代有轨清洁车辅助自动驾驶系统及方法，该系统包括：用于采集图像数据和周围环境指示信息的深度相机，用于采集轨道及轨道周围环境边界点信息的激光雷达，用于采集物体三轴姿态角、角速率以及加速度的惯性测量单元，用于获取轨道位置以及走向的GPS模块，用于根据采集数据进行转向控制处理、基于障碍物的距离和方位进行检测障碍物的主控板，用于传动的传动控制机构，以及分别与深度相机、激光雷达、惯性测量单元、GPS模块、主控板连接的车载处理装置。本发明通过采用深度相机配合自动驾驶技术，在有轨电车轨道清洁上更加精准、快速，并且在夜间作业时，结合激光雷达和深度相机并不会受到低光照环境而产生太多的影响的技术效果。

Description

一种现代有轨清洁车辅助自动驾驶系统及方法

技术领域

本发明涉及辅助驾驶技术领域，尤其涉及一种现代有轨电车轨道清洁车的辅助驾驶系统及方法。

背景技术

目前，我国的现代有轨电车轨道清洁车在槽型轨清理维护方面，由于研究时间较短，相对于发达国家，我国相关技术还处于初级阶段，不是很成熟，基本还是通过人工手动对槽型轨进行清理的方法，只有极少数的企业自主研发并生产出了现代有轨电车槽型轨清洁设备，但耗费劳动量大、工作效率低，而且不能满足现代有轨电车发展对槽型轨高效清洁的要求。

其中，北京德高洁公司研制出一种轨道清洁车，该车主要由公路行驶与轨道行走系统、液压与动力系统、高压水射流与清扫系统吸尘除尘系统、自动化控制系统组成，该车有多种清洁模式和功能组件，但并未能实现清洁车的自主定位，循轨作业，自动避障等功能，且不能对清洁车整体运行状态进行有效的监测；德国斯沃林公司研制出轨道清洁车，其驱动装置采用Pickup公铁两用驱动装置，相对于前一代清洁车，性能更加先进噪音更小，清洁效果良好，但设计复杂、造价高昂，耗费清洁资源较多且只能用于工字轨，并不兼用于有轨电车槽形轨的清洁。

当前的辅助驾驶系统(ADAS)采用多传感器融合的方式来实现在驾车行驶过程中，即时感知周围人、车、路及交通环境，对静态和动态物体进行辨别，如车辆、行人以及交通标志标线等；虽然当前的辅助驾驶系统已经越来越成熟，但多用于奢侈的高档车辆和地铁上，且价格昂贵，还未普及到低成本车辆以及轨道清洁车上。

综上所述，具有辅助驾驶的现代有轨电车轨道清洁车目前还没有先例研究和成熟的商业化产品，现代有轨电车轨道清洁车辅助自动驾驶系统的发展前景相当可观。

发明内容

为了克服现有技术存在的缺陷与不足，本发明的第一目的在于提供一种现代有轨清洁车辅助自动驾驶系统，该系统结合深度相机和激光雷达并搭载于轨道清洁车，避免因驾驶员疲劳而导致的安全事故的发生，同时弥补当前GPS导航系统精度不足和当前辅助驾驶系统无法搭载于轨道清洁车的问题。

本发明的第二目的在于提供一种现代有轨清洁车辅助自动驾驶方法。

为了达到上述第一目的，本发明采用以下技术方案：

一种现代有轨清洁车辅助自动驾驶系统，包括：深度相机、激光雷达、惯性测量单元、GPS模块、主控板、传动控制机构、车载处理装置；

所述深度相机用于采集轨道及轨道周围环境图像数据和周围环境指示信息，所述激光雷达用于采集轨道及轨道周围环境边界点信息，所述惯性测量单元用于采集物体三轴姿态角、角速率以及加速度的，所述GPS模块用于获取轨道位置以及走向，所述主控板用于根据由深度相机、激光雷达、惯性测量单元、GPS模块获得的采集数据进行转向控制处理、基于障碍物的距离和方位进行检测障碍物，所述传动控制机构用于接收由主控板发送的转向控制指令进行传动；

所述车载处理装置分别与深度相机、激光雷达、惯性测量单元、GPS模块、主控板连接；所述主控板包括第一处理器、用于报警提示的警报器，所述第一处理器与警报器连接

作为优选的技术方案，所述车载处理装置设有第二处理器、与第二处理器连接的触摸屏，所述触摸屏用于为用户展示主控板处理结果、接收用户在屏幕的触摸操作并生成触碰指令，所述第二处理器用于基于PID闭环控制法计算导航参数并将导航参数发送至第一处理器、将所述触碰指令转发至所述主控板、将所述激光雷达采集的障碍物信息转发至所述主控板进行障碍物检测。

作为优选的技术方案，

所述第一处理器设有：调整模块、数据预处理模块、行驶路径拟合模块、轨道路线循迹模块、转向控制模块、障碍物检测模块；

所述调整模块用于初始化清洁车的位置，校准深度相机和激光雷达的初始化数据、位置，校准GPS模块，所述数据预处理模块用于坐标变换处理，所述行驶路径拟合模块，用于基于ROS对采集数据进行拟合清洁车的行驶路径和转向角度，所述轨道路线循迹模块用于基于深度相机采集到的图像采用循迹算法得到轨道路线，所述转向控制模块用于根据导航参数经过分析计算编码得到转向控制指令、将转向控制指令发送给传动控制机构进行传动，所述障碍物检测模块用于基于深度相机采集的深度图像判断障碍物大小和出现方位，配合激光雷达对障碍物的相对距离计算和判定，对障碍物进行精确定位，发送警报指令至警报器进行警报提示驾驶人员。

作为优选的技术方案，所述传动控制机构包括方向控制装置、设置在方向控制装置上的电机和用于驱动电机的电机驱动器。

作为优选的技术方案，所述深度相机、激光雷达安装在测试车辆的前方的前置扫盘盖板前沿，正对行驶方向。

为了达到上述第二目的，本发明采用以下技术方案：

一种现代有轨清洁车辅助自动驾驶方法，包括以下步骤：

初始化调整步骤：初始化清洁车的位置，校准深度相机和激光雷达的初始化数据和位置，校准GPS模块；

数据采集步骤：通过GPS模块获取轨道的走向，通过惯性测量单元采集三轴数据，通过深度相机采集轨道的RGB图像、深度图像以及周围环境指示信息，通过激光雷达采集轨道及轨道周围环境边界点信息；

数据预处理步骤：基于ROS对深度图像和边界点信息进行坐标变换形成以x，y，z为坐标的三维点云图像；

行驶路径拟合步骤：利用激光雷达和深度相机探测到的距离数据基于ROS配合深度相机、激光雷达、惯性测量单元和主控板，通过结合轨道的RGB图像、周围环境的深度图像、三轴数据以及周围环境指示信息并采用路径评价处理方法进行拟合清洁车的行驶路径和转向角度，调整车辆方向使图像中线向引导线方向靠近至重合；

轨道路线循迹步骤：基于RGB图像、深度图像、三轴数据结合循迹算法、栅格地图进行轨道路线的循迹，通过触摸屏显示所构建的轨道路线检查是否偏离轨道；

转向控制步骤：基于PID闭环控制法计算导航参数，发出转向控制指令给电机驱动器,根据导航参数经过分析计算编码得到转向控制指令、将转向控制指令发送给传动控制机构进行传动，进而实现无人驾驶的转向控制；

障碍物检测步骤：基于深度相机采集的深度图像判断障碍物大小和出现方位，配合激光雷达采用相位法对障碍物的相对距离计算和判定，基于探测到前方路径有较大障碍物、交通信号灯或清洁车已偏离预设路径进行判定第一警报触发条件或第二警报触发条件进行警报处理，基于栅格地图进行自行绕行障碍物；

所述第一警报触发条件具体为障碍物始终存在于清洁车的行驶范围内，且该障碍物距离超过前方预设距离阈值，当该障碍物距离超过前方预设距离阈值触发所述第一警报触发条件；

所述第二警报触发条件具体为基于GPS数据中车辆位置偏离预设路径的时间是否超过预设时间阈值，当超过预设时间阈值时触发第二警报触发条件。

作为优选的技术方案，所述基于ROS对深度图像和边界点信息进行坐标变换形成以x，y，z为坐标的三维点云数据，具体包括如下步骤：

通过Kinect深度相机获取RGB图像和深度图像，具体过程为通过Kinect深度相机基于深度摄像头发射的红外线获得深度数据，基于彩色摄像头获得彩色图像，对像素坐标叠加；

基于ROS-tf包对采集深度图像和RGB图像进行坐标变换，将深度图像坐标系统的u/v坐标转化到以Kinect深度相机为原点的x/y坐标，进而能够获得真实世界中物体表面任意点的X,Y,Z坐标，即得到任意点的位置数据，从而形成多个点的集合点云。

作为优选的技术方案，所述路径评价处理方法，具体包括以下步骤：

二值化处理步骤：对RGB图像通过二值化处理得到二值化图像；

距离评估步骤：根据深度相机采集到的深度图像得到深度图像中轨道距离车辆的距离范围；

路径调整步骤：将图像中间垂直方向的距离两侧边距离相等的线作为对截留后的轨道图像的中心线，基于openCV的cvMoments函数计算获得图像重心，图像重心与中心线下端的连线即为引导线，计算引导线与中心线之间的夹角，进而基于PID闭环控制法获得目标角度增量，基于目标角度增量进行调整行驶路径。

作为优选的技术方案，所述轨道路线循迹步骤，具体包括以下步骤；

采集步骤：通过深度相机基于RGB摄像头将采集得到的RGB图像；

色彩空间转换步骤：将RGB图像转换为HSV图像；

分割步骤：对HSV图像进行图像分割得到第一分割图像和第二分割图像，通过第一分割图像提取槽型轨道位置，对第一分割图像进行二值化处理得到轨道的轮廓，即找出轨道的中心线，通过计算第二分割图像的图形重心，将该图形重心作为引导线；

角速度调整步骤：进行中心线与引导线误差处理，即计算图像中的中心线到引导线的距离，将中心线到引导线的距离转换成目标角速度，基于ROS结合目标角速度调整电机进行控制方向盘；

返回到采集步骤，循环执行采集步骤、色彩空间转换步骤、分割步骤以及角速度调整步骤实现稳定的轨道跟随。

作为优选的技术方案，所述基于PID闭环控制法计算导航参数，具体为：选择Δω(角度增量)为控制的目标变量，Δω的基准是前一次闭环控制结尾的方向盘实际角度ω，即采用角度增量控制公式进行控制，所述角度增量控制公式为：

其中ω^*表示目标角度，ω表示当前角度，ω₀表示设定角度，δ表示预设角度差值阈值，K表示角度比例系数，当目标角度ω^*与当前角度ω之间的差值大于或等于预设角度差值阈值δ时，则令角度增量Δω等于设定角速度ω₀与闭环周期T的乘积；当目标角度ω^*与当前角度ω之间的差值小于预设角度差值阈值δ时，则令角度增量Δω等于目标角度ω^*与当前角度ω之间的差值乘以一个角度比例系数K，最终使得角度增量Δω趋向于0；

所述导航参数具体为车辆的角度增量。

本发明与现有技术相比，具有如下优点和有益效果：

(1)与现在最常见的人工清洁来对比，本发明采用了搭载智能辅助驾驶系统的技术方案，降低了工作人员的工作强度，解决了在槽型轨清洁时所消耗的人力资源和物理资源过多的技术问题，达到了清洁速度更快，清洁效果更好的技术效果，既在工作质量，工作效率上更优，同时也避免了驾驶人员因疲劳而导致的安全事故的发生。

(2)与现在最常见的搭载双目摄像机的槽型轨清洁车相比，本发明采用了深度相机和激光雷达，通过结合深度相机输出目标的三维坐标信息和激光雷达对环境的感知的技术方案，解决了传统的二目相机因为两枚都是RGB摄像头所以在暗光条件下表现不佳的技术问题，达到了有轨电车轨道清洁更加精准、快速，并且在夜间作业时，结合激光雷达和深度相机并不会受到低光照环境而产生太多的影响的技术效果，尽可能地减少了行进过程中多走或者少走时的人工调整。

(3)本发明采用了GPS和传感器相结合来感知周围环境的技术方案，达到了在微观层面更加的细致的技术效果，因为路径规划是即时演算的，所以一方面路径规划精度会比只采用GPS的路径规划高，另一方面本地进行计算也会更加的迅速，在遇到特殊情况的时候，例如，出现障碍物或坑洞时也可以从容应对。

附图说明

图1为本发明实施例1中现代有轨清洁车辅助自动驾驶系统的模块组成示意图；

图2为本发明实施例1中现代有轨清洁车辅助自动驾驶系统的信息传递示意图；

图3为本发明实施例1中现代有轨清洁车辅助自动驾驶系统侧视图；

图4为本发明实施例1中现代有轨清洁车辅助自动驾驶系统主视图；

图5为本发明实施例1中现代有轨清洁车辅助自动驾驶系统俯视图；

图6为本发明实施例1中ROS示例关系节点和节点图；

图7为本发明实施例2中现代有轨清洁车辅助自动驾驶方法的步骤流程图；

图8为本发明实施例2中三维点云图像处理的示意图；

图9为本发明实施例2中深度图像的示意图；

图10(a)为本发明实施例2中的RGB图像示意图；

图10(b)为本发明实施例2中的二值化图像示意图；

图10(c)为本发明实施例2中经过裁剪后的二值化图像示意图；

图10(d)为本发明实施例2中的中心线与引导线误差处理图像示意图；

图11为本发明实施例2中基于PID闭环控制法进行角度和电流控制的示意图；

其中，1-深度相机，2-激光雷达，3-惯性测量单元，4-GPS模块，5-中心线，6-引导线。

具体实施方式

在本公开的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本公开和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本公开的限制。

此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。同样，“一个”、“一”或者“该”等类似词语也不表示数量限制，而是表示存在至少一个。“包括”或者“包含”等类似的词语意指出现在该词前面的元素或者物件涵盖出现在该词后面列举的元素或者物件及其等同，而不排除其他元素或者物件。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电性的连接，不管是直接的还是间接的。

在本公开的描述中，需要说明的是，除非另有明确的规定和限定，否则术语“安装”、“相连”、“连接”应做广义理解。例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本公开中的具体含义。此外，下面所描述的本公开不同实施方式中所涉及的技术特征只要彼此之间未构成冲突就可以相互结合。

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例

实施例1

如图1所示，本实施例提供了一种现代有轨清洁车辅助自动驾驶系统，该系统基于ROS进行开发，该系统主要设有：用于采集轨道及轨道周围环境图像数据和周围环境指示信息的深度相机1，用于采集轨道及轨道周围环境边界点信息的激光雷达2，用于采集物体三轴姿态角、角速率以及加速度的惯性测量单元3(IMU)，用于获取轨道位置以及走向的GPS模块4，用于根据由深度相机1、激光雷达2、惯性测量单元3、GPS模块4获得的采集数据进行转向控制处理、基于障碍物的距离和方位进行检测障碍物的主控板，用于接收由主控板发送的转向控制指令进行传动的传动控制机构。其中周围环境指示信息包括车道线、交通信号灯等目标的状态和颜色；

在本实施例中，主控板包括第一处理器、与第一处理器连接的警报器；

在本实施例中，该系统还设有设置在清洁车内的车载处理装置，车载处理装置分别与深度相机1、激光雷达2、惯性测量单元3、GPS模块4、主控板连接，该车载处理装置作为PC上位机设有第二处理器、与第二处理器连接的触摸屏，其中触摸屏用于为用户展示主控板处理结果、接收用户在屏幕的触摸操作并生成触碰指令，第二处理器用于基于PID闭环控制法计算导航参数并将导航参数发送至第一处理器、将触碰指令转发至主控板、将激光雷达2采集的障碍物信息转发至主控板进行障碍物检测。实际应用时，当激光雷达2探测到周围存在障碍物时，会将障碍物距离清洁车辆的相对距离作为障碍物信息传输给第二处理器。

如图2所示，第一处理器设有：调整模块，用于初始化清洁车的位置，校准深度相机1和激光雷达2的初始化数据、位置，校准GPS模块4，数据预处理模块，用于坐标变换处理，行驶路径拟合模块，用于基于ROS对采集数据进行拟合清洁车的行驶路径和转向角度，轨道路线循迹模块，基于深度相机1采集到的图像采用循迹算法得到轨道路线，转向控制模块，用于根据导航参数经过分析计算编码得到转向控制指令、将转向控制指令发送给传动控制机构进行传动，进而实现无人驾驶的转向控制，以及障碍物检测模块，用于基于深度相机1采集的深度图像判断障碍物大小和出现方位，配合激光雷达2对障碍物的相对距离计算和判定，实现对障碍物进行精确定位，发送警报指令至警报器进行警报提示驾驶人员；

在本实施例中，主控板采用Raspberry Pi 3B，深度相机1采用Kinect深度相机，警报器采用警报灯。实际应用时，本实施例采用公铁两用洗扫车作为轨道清洁车进行实施测试。

此外，本实施例中的激光雷达2还可采用毫米波雷达达到相同的采集效果。

在本实施例中，传动控制机构包括方向控制装置、设置在方向控制装置上的电机和用于驱动电机的电机驱动器。实际应用时，方向控制装置为方向盘，电机根据实际轨道清洁车采用中型舵机，进而电机驱动器采用舵机驱动器，此外可根据实际情况采用不同的电机以及电机驱动器。

结合图3、图4、图5所示，在本实施例中，Kinect深度相机、激光雷达2安装在测试车辆的前方的前置扫盘盖板前沿，正对行驶方向；激光雷达2和惯性测量单元3通过USB线分别与车内的触摸屏连接，其中激光雷达2的安装高度为70cm，俯角为20度，惯性测量单元3安装在Kinect深度相机和激光雷达2的后方，其中射线实线表示激光雷达2的扫描范围，射线虚线表示Kinect深度相机的取景范围；GPS模块4设置于驾驶室内；中型舵机安装在方向盘上，分别与舵机驱动器和Raspberry Pi 3B连接，构成传动控制机构，Raspberry Pi 3B通过USB线连接至触摸屏；警报灯与Raspberry Pi 3B连接，并安装在驾驶室顶板上。

在本实施例中，ROS为机器人操作系统，它由很多独立的节点(Node)所构成，每一个独立的节点通过发布主题(Topic)或者订阅(Publisher or Subscriber)机制进行通信；例如在本实施例中，清洁车上的激光雷达2可以抽象为一个节点，在障碍物这个主题上他会连续不断的发布有关障碍物的数据(message)，如果清洁车ROS-move-base节点订阅了这个主题，就会一直连续不断的获取与障碍物的距离数据，进而当触发第一警报触发条件或第二警报触发条件时再通过警报器通知到驾驶员。

在本实施例中，主题的信息可以被多个节点订阅，且各节点之间也不会干扰，ROS通过这种方式将清洁车上面的各个传感器和上位机等节点连接在一起，形成一个有机的整体。

如图6所示，现代有轨清洁车辅助自动驾驶系统基于ROS布置节点得到节点图，其中，节点具体包括：深度相机节点、激光雷达节点、清洁路径设定节点、OpenCV节点、警报距离阈值节点、导航节点以及控制节点；

Kinect_Camera(Kinect深度相机节点)发布的主题为多个包含各种实时图像信息的主题，例如Camera/depth/Image_raw(原始深度图像)、Camera/RGB/Image_raw(原始RGB图像)；

LaserScan(激光雷达节点)发布的主题为多个包含障碍物的实时数据的主体，例如/Scan(扫描定位)，/Scan.angle_min(扫描起始角度)，/Scan.range_min(扫描最近距离)等；

清洁路径设定节点订阅的主题为GPS模块4发布的轨道路线，清洁路径设定节点发布的主题为清洁车行驶路径；

OpenCV节点订阅的主题为kinect深度相机的原始RGB图像，OpenCV节点发布的主题为即时清洁车的行径方向；

警报距离阈值节点订阅的主题包括kinect深度相机的深度图像、激光雷达节点的距离障碍物的距离以及清洁车是否偏离预设路径，警报距离阈值节点发布的主题包括驾驶室内警报灯和警报指令；

警报器节点订阅的主题包括警报指令；

导航节点订阅的主题包括清洁车的预定路线、即时的清洁车行驶方向，导航节点发布的主题为修正角度的信息；

控制节点move_base订阅的主题为修正角度的信息，控制节点move_base发布的主题包括方向盘舵机的驱动控制信息和角度调整信息。

实施例2

如图7所示，本实施例提供了一种现代有轨清洁车辅助自动驾驶方法，该方法包括以下步骤：

初始化调整步骤：初始化清洁车的位置，校准深度相机1和激光雷达2的初始化数据和位置，校准GPS模块4；

数据采集步骤：通过GPS模块4获取轨道的走向，通过惯性测量单元3采集三轴数据，通过深度相机1采集轨道的RGB图像、深度图像以及周围环境指示信息，通过激光雷达2采集轨道及轨道周围环境边界点信息。实际应用时，三轴数据包括基于惯性测量单元载体的前进方向、垂直于前进方向和垂直于地面作为三个轴的加速度数据、角速度数据，具体为该三个轴方向上的加速度和每两轴形成平面上的角速度；

数据预处理步骤：基于ROS对深度图像和边界点信息进行坐标变换形成以x，y，z为坐标的三维点云图像；

行驶路径拟合步骤：利用激光雷达2和深度相机1探测到的距离数据基于ROS配合深度相机1、激光雷达2、惯性测量单元3和主控板，通过结合轨道的RGB图像、周围环境的深度图像、三轴数据以及周围环境指示信息并采用路径评价处理方法进行拟合清洁车的行驶路径和转向角度，调整车辆方向使图像中线向引导线方向靠近至重合；

轨道路线循迹步骤：基于RGB图像、深度图像、三轴数据结合循迹算法、栅格地图进行轨道路线的循迹，通过触摸屏显示所构建的轨道路线，以便于驾驶人员检查是否偏离轨道；

转向控制步骤：基于PID闭环控制法计算导航参数，发出转向控制指令给电机驱动器,根据导航参数经过分析计算编码得到转向控制指令、将转向控制指令发送给传动控制机构进行传动，进而实现无人驾驶的转向控制，即电机驱动器通过蜗轮蜗杆与转向轴机械连接，使得电机驱动器驱动电机转动时连带蜗轮蜗杆转动转向轴进而完成对清洁车方向盘的转向控制；

障碍物检测步骤：基于深度相机1采集的深度图像判断障碍物大小和出现方位，配合激光雷达2采用相位法对障碍物的相对距离计算和判定，基于探测到前方路径有较大障碍物、交通信号灯或清洁车已偏离预设路径进行判定第一警报触发条件或第二警报触发条件进行警报处理，具体为当警报触发条件被触发时，通过主控板发送警报指令至控制警报灯闪烁，进而提醒驾驶人员，基于栅格地图进行自行绕行障碍物。实际应用时，通过对激光雷达2设定扫描角度和扫描距离，基于由激光雷达2扫描获得的边界点信息采用栅格法实时构建栅格地图，该实时构建的栅格地图用来判断障碍物的大小和方位，在清洁车遇到障碍物的场景下，当获取了障碍物大小、方位以及距离后，采用DWA动态窗口算法进行自行绕行障碍物，基于实时构建的栅格地图输出绕行的所有路径选择，即动态窗口，对所有的可行路径使用基于代价函数的路线评价算法，即可得到即时最佳的绕行障碍路线，基于该即时最佳的绕行障碍路线进行转向绕行；

在本实施例中，自行绕行障碍物的优先级大于轨道路线循迹，当清洁车遇到障碍物时，待绕行结束后，再执行轨道路线循迹。在本实施例中，通过激光雷达2采集轨道及轨道周围环境边界点信息，具体为通过控制激光雷达2左右转动，周期性的对前方预设距离阈值内的路面状况进行扫描，进而获取边界点信息；其中，前方预设距离阈值设置为6米。实际应用时，边界点信息具体为扫描范围内物体靠近雷达一侧的轮廓，边界点信息用于建立栅格地图，通过实时建立栅格地图，在进行辅助障碍物的检测和为自动绕行障碍物时提供依据，其中在自动绕行障碍物时，采用的DWA动态窗口算法主要依据实时建立的栅格地图。

在本示例中，周围环境指示信息包括车道线、交通信号灯等目标的状态和颜色；

在本实施例中，RGB图像由深度相机1通过RGB摄像头采集得到，深度图像由深度相机1通过深度摄像头采集得到；

在本实施例中，路径评价处理方法，具体包括以下步骤：

二值化处理步骤：对RGB图像通过二值化处理得到二值化图像。实际应用时，在一个处理周期当中，首先将深度相机采集到的RGB图像经过openCV中的cvThreshold函数进行处理，得到了二值化图像；

距离评估步骤：根据深度相机采集到的深度图像得到深度图像中轨道距离车辆的距离范围。实际应用时，轨道从图片下方一直延申至视线结点距离逐渐增加，根据每个地方的轨道曲折程度确定截留多远距离的轨道图像来进行后一步工作，弯道越多截留距离越多，反之则越少，本实例折中选择1/2；

路径调整步骤：将图像中间垂直方向的距离两侧边距离相等的线作为对截留后的轨道图像的中心线5，基于openCV的cvMoments函数计算获得图像重心，图像重心与中心线5下端的连线即为引导线6，计算引导线6与中心线5之间的夹角，进而基于PID闭环控制法获得目标角度增量，基于目标角度增量进行调整行驶路径。

在本实施例中，警报器采用警报灯，因此警报指令用于控制警报灯闪烁进行提示驾驶人员，此外警报器还可采用常用的警报设备，比如警报喇叭等，进行相适应的警报处理。

如图8所示，基于ROS对深度图像和边界点信息进行坐标变换形成以x，y，z为坐标的三维点云数据，具体包括如下步骤：

通过Kinect深度相机获取RGB图像和深度图像，具体过程为通过Kinect深度相机基于深度摄像头发射的红外线获得深度数据，基于彩色摄像头获得彩色图像，对像素坐标叠加。实际应用时，由于图像大小一致，因此可以将每个像素坐标叠加，其中叠加进行结合的目的不是图像，而是位置数据；

基于ROS-tf包对采集深度图像和RGB图像进行坐标变换，将深度图像坐标系统的u/v坐标转化到以Kinect深度相机为原点的x/y坐标，进而能够获得真实世界中物体表面任意点的X,Y,Z坐标，即得到任意点的位置数据，通过这样形成多个点的集合点云。实际应用时，RGB图像中每个像素点已经有了一个二维的坐标，即垂直于视线的平面内的(u，v)，深度数据即在上述的基础上，获得每个像素点在平行于视线方向上的坐标数据z，把z补到由(u，v)转换的(x，y)后面得到三维坐标，就是一个像素点的位置数据；

如图9所示，深度数据为深度图像坐标系的x，y，z坐标，其中z坐标即为物体上某点到镜头的距离，由于Kinect深度相机采集到原始深度坐标时把深度图像原点作为原点的图像坐标系统(u/v)，需借助ROS-tf包可以将深度图像坐标系统的u/v坐标通过坐标转换公式转化到以Kinect深度相机为原点的x/y坐标。

在本实施例中，坐标转换公式具体为：

上式中X，Y，Z即为以Kinect深度相机为原点的真实坐标系的X,Y,Z轴，U,V为深度图像的水平和数值坐标轴，U_X、V_Y分别表示水平像素个数、垂直像素个数，因而由

得到图像的中心坐标，f_XZ和f_YZ为Kinect深度相机的焦距。实际应用时，因为所采集到的图像的分辨率为800×600，所以摄像头中心坐标即为(400,300)，原坐标只有经过转换的才是真实世界的坐标，才可以用x，y，z的三轴数据去生成三维点云图像。

结合图10(a)至图10(d)所示，轨道路线循迹步骤，具体包括以下步骤；

采集步骤：通过深度相机1基于RGB摄像头将采集得到的RGB图像，具体为由USB_cam软件包取得原始数据并且转化为sensor_msgs/Image，然后将数据传送到TopicVideo0/Image；

色彩空间转换步骤：将RGB图像转换为HSV图像；

分割步骤：对HSV图像进行图像分割得到第一分割图像和第二分割图像，通过第一分割图像提取槽型轨道位置，对第一分割图像进行二值化处理得到轨道的轮廓，即找出轨道的中心线5，通过计算第二分割图像的图形重心，将该图形重心作为引导线6；

角速度调整步骤：进行中心线5与引导线6误差处理，即计算图像中的中心线5到引导线6的距离，将中心线5到引导线6的距离转换成目标角速度，基于ROS结合目标角速度调整电机进行控制方向盘；

返回到采集步骤，循环执行采集步骤、色彩空间转换步骤、分割步骤以及角速度调整步骤实现稳定的轨道跟随。

在本实施例中，色彩空间转换步骤，具体为：由ROS直接调用OpenCV视觉库的Package，使用OpenCV计算机视觉库进行处理，进而将RGB图像变为HSV图像，HSV把色彩空间分解为Hue(色调)，Saturation(饱和度)，Value(明度)；

在本实施例中，分割步骤，具体为：

从HSV图像中提取槽型轨道位置，根据H(色调)可以稳定的识别出某种颜色，根据色调检测图像中的像素点是否在轨道颜色为白色高明度范围内生成二值化图像，在二值化的图像中找到轨道的轮廓，在这里轮廓是算出中心线5区域的重要指标；

保留面积最大的轮廓，然后进行图像切割，保留

到

的高度处的图像，避免远处轨道的影响。实际应用时，根据深度图像中像素点的距离和效果来确定切除多远以外的图像，默认切除二分之一；

计算剩余图形重心，即切割后保留图像的重心，该重心的位置就是引导线6的方向；其中，由于轨道存在不同磨损程度和锈蚀程度，轨道亮色部分的明度存在一定差异，所以根据当地情况设置明度范围。

如图11所示，基于PID闭环控制法计算导航参数，具体为：选择Δω(角度增量)为控制的目标变量，Δω的基准是前一次闭环控制结尾的方向盘实际角度ω，即采用角度增量控制公式进行控制，其中角度增量控制公式为：

其中ω^*表示目标角度，ω表示当前角度，ω₀表示设定角度，δ表示预设角度差值阈值，K表示角度比例系数，当目标角度ω^*与当前角度ω之间的差值大于或等于预设角度差值阈值δ时，则令角度增量Δω等于设定角速度ω₀与闭环周期T的乘积；当目标角度ω^*与当前角度ω之间的差值小于预设角度差值阈值δ时，则令角度增量Δω等于目标角度ω^*与当前角度ω之间的差值乘以一个角度比例系数K，最终使得角度增量Δω趋向于0。

在本实施例中，导航参数具体为车辆的角度增量。实际应用时，角度增量需要舵机操作方向盘来弥补，即转化为舵机的角度增量，舵机的角度增量由舵机驱动器控制，第一处理器通过发出转向控制指令，舵机驱动器基于转向控制指令进行控制脉冲电流频率。

如图11所示，本实施例基于角度增量进行角度调节，通过将得到的角度增量归一化，由第二处理器计算得到目标角度对应的脉冲电流频率，主控板通过第一处理器将目标角度对应的脉冲电流频率与当前舵机脉冲电流频率对比，得到目标脉冲电流频率增量，主控板将目标脉冲电流频率增量作为转向控制指令，舵机驱动器基于转向控制指令对舵机调节电流，同时对舵机的当前电流频率实时监控，将监控到的即时电流作为反馈电流发送至主控板进行校验。

在本实施例中，由于无人驾驶的转向控制对于角度和角速度都有着严格的要求，所以选择Δω(角度增量)为控制的目标变量；

在本实施例中，障碍物检测步骤中主要依据两个方面的数据：

一方面是深度相机1传回来的深度图像，根据深度图像可以第一时间反应出障碍物的大小和出现方位，其中深度图像可以辅助激光雷达2进行对相对距离的计算和判定，进而达到更精确的定位检测；

另一方面是激光雷达2传输回的距离和方位信息，激光雷达2会以扇形来扫描车辆前方的区域。

在本实施例中，第一警报触发条件具体为障碍物始终存在于清洁车的行驶范围内，且该障碍物距离超过前方预设距离阈值；

设激光雷达2传回来的数据为(d,ω)，即第一警报触发条件可以表示为：

其中d为障碍物到激光雷达2的最远距离，D为前方预设距离阈值，ω为测量方向与行驶方向的夹角，L_max为轨道清洁车的车宽，公式中的长度单位采用mm。实际应用时，前方预设距离阈值设置为6000mm，激光雷达2扫描一圈，得到周边地形的轮廓，正常情况下车辆前方应该是扇形轮廓，当出现障碍物时则会出现一个扇形的凹坑。然后再判断障碍物的距离和方位信息，本实施例中的轨道清洁车的车宽为2170mm，则当d和ω符合第一警报触发条件时进行警报，则此时第一警报触发条件具体为：

在本实施例中，第二警报触发条件具体为基于GPS数据中车辆位置偏离预设路径的时间是否超过预设时间阈值；当超过预设时间阈值时进行触发警报和亮起警报灯。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (9)

1.一种现代有轨清洁车辅助自动驾驶系统，其特征在于，包括：深度相机、激光雷达、惯性测量单元、GPS模块、主控板、传动控制机构、车载处理装置；

所述深度相机用于采集轨道及轨道周围环境图像数据和周围环境指示信息，所述激光雷达用于采集轨道及轨道周围环境边界点信息，所述惯性测量单元用于采集物体三轴姿态角、角速率以及加速度的，所述GPS模块用于获取轨道位置以及走向，所述主控板用于根据由深度相机、激光雷达、惯性测量单元、GPS模块获得的采集数据进行转向控制处理、基于障碍物的距离和方位进行检测障碍物，所述传动控制机构用于接收由主控板发送的转向控制指令进行传动；

所述车载处理装置分别与深度相机、激光雷达、惯性测量单元、GPS模块、主控板连接；所述主控板包括第一处理器、用于报警提示的警报器，所述第一处理器与警报器连接；

所述第一处理器设有：调整模块、数据预处理模块、行驶路径拟合模块、轨道路线循迹模块、转向控制模块、障碍物检测模块；

所述调整模块用于初始化清洁车的位置，校准深度相机和激光雷达的初始化数据、位置，校准GPS模块；

所述数据预处理模块用于坐标变换处理，基于ROS对深度图像和边界点信息进行坐标变换形成以x，y，z为坐标的三维点云图像；

所述行驶路径拟合模块，用于基于ROS对采集数据进行拟合清洁车的行驶路径和转向角度，利用激光雷达和深度相机探测到的距离数据基于ROS配合深度相机、激光雷达、惯性测量单元和主控板，通过结合轨道的RGB图像、周围环境的深度图像、三轴数据以及周围环境指示信息并采用路径评价处理方法进行拟合清洁车的行驶路径和转向角度，调整车辆方向使图像中线向引导线方向靠近至重合；

所述轨道路线循迹模块用于基于深度相机采集到的图像采用循迹算法得到轨道路线，基于RGB图像、深度图像、三轴数据结合循迹算法、栅格地图进行轨道路线的循迹，通过触摸屏显示所构建的轨道路线检查是否偏离轨道；

所述转向控制模块用于根据导航参数经过分析计算编码得到转向控制指令、将转向控制指令发送给传动控制机构进行传动，基于PID闭环控制法计算导航参数，发出转向控制指令给电机驱动器,根据导航参数经过分析计算编码得到转向控制指令、将转向控制指令发送给传动控制机构进行传动，进而实现无人驾驶的转向控制；

所述障碍物检测模块用于基于深度相机采集的深度图像判断障碍物大小和出现方位，配合激光雷达对障碍物的相对距离计算和判定，对障碍物进行精确定位，发送警报指令至警报器进行警报提示驾驶人员；

基于探测到前方路径有较大障碍物、交通信号灯或清洁车已偏离预设路径进行判定第一警报触发条件或第二警报触发条件进行警报处理，基于栅格地图进行自行绕行障碍物；

所述第一警报触发条件具体为障碍物始终存在于清洁车的行驶范围内，且该障碍物距离超过前方预设距离阈值，当该障碍物距离超过前方预设距离阈值触发所述第一警报触发条件；

所述第二警报触发条件具体为基于GPS数据中车辆位置偏离预设路径的时间是否超过预设时间阈值，当超过预设时间阈值时触发第二警报触发条件。

2.根据权利要求1所述的现代有轨清洁车辅助自动驾驶系统，其特征在于，所述车载处理装置设有第二处理器、与第二处理器连接的触摸屏，所述触摸屏用于为用户展示主控板处理结果、接收用户在屏幕的触摸操作并生成触碰指令，所述第二处理器用于基于PID闭环控制法计算导航参数并将导航参数发送至第一处理器、将所述触碰指令转发至所述主控板、将所述激光雷达采集的障碍物信息转发至所述主控板进行障碍物检测。

3.根据权利要求1所述的现代有轨清洁车辅助自动驾驶系统，其特征在于，所述传动控制机构包括方向控制装置、设置在方向控制装置上的电机和用于驱动电机的电机驱动器。

4.根据权利要求1-3任一所述的现代有轨清洁车辅助自动驾驶系统，其特征在于，所述深度相机、激光雷达安装在测试车辆的前方的前置扫盘盖板前沿，正对行驶方向。

5.一种现代有轨清洁车辅助自动驾驶方法，其特征在于，包括以下步骤：

初始化调整步骤：初始化清洁车的位置，校准深度相机和激光雷达的初始化数据和位置，校准GPS模块；

数据采集步骤：通过GPS模块获取轨道的走向，通过惯性测量单元采集三轴数据，通过深度相机采集轨道的RGB图像、深度图像以及周围环境指示信息，通过激光雷达采集轨道及轨道周围环境边界点信息；

数据预处理步骤：基于ROS对深度图像和边界点信息进行坐标变换形成以x，y，z为坐标的三维点云图像；

行驶路径拟合步骤：利用激光雷达和深度相机探测到的距离数据基于ROS配合深度相机、激光雷达、惯性测量单元和主控板，通过结合轨道的RGB图像、周围环境的深度图像、三轴数据以及周围环境指示信息并采用路径评价处理方法进行拟合清洁车的行驶路径和转向角度，调整车辆方向使图像中线向引导线方向靠近至重合；

轨道路线循迹步骤：基于RGB图像、深度图像、三轴数据结合循迹算法、栅格地图进行轨道路线的循迹，通过触摸屏显示所构建的轨道路线检查是否偏离轨道；

转向控制步骤：基于PID闭环控制法计算导航参数，发出转向控制指令给电机驱动器,根据导航参数经过分析计算编码得到转向控制指令、将转向控制指令发送给传动控制机构进行传动，进而实现无人驾驶的转向控制；

障碍物检测步骤：基于深度相机采集的深度图像判断障碍物大小和出现方位，配合激光雷达采用相位法对障碍物的相对距离计算和判定，基于探测到前方路径有较大障碍物、交通信号灯或清洁车已偏离预设路径进行判定第一警报触发条件或第二警报触发条件进行警报处理，基于栅格地图进行自行绕行障碍物；

所述第一警报触发条件具体为障碍物始终存在于清洁车的行驶范围内，且该障碍物距离超过前方预设距离阈值，当该障碍物距离超过前方预设距离阈值触发所述第一警报触发条件；

所述第二警报触发条件具体为基于GPS数据中车辆位置偏离预设路径的时间是否超过预设时间阈值，当超过预设时间阈值时触发第二警报触发条件。

6.根据权利要求5所述的一种现代有轨清洁车辅助自动驾驶方法，其特征在于，所述基于ROS对深度图像和边界点信息进行坐标变换形成以x，y，z为坐标的三维点云数据，具体包括如下步骤：

通过Kinect深度相机获取RGB图像和深度图像，具体过程为通过Kinect深度相机基于深度摄像头发射的红外线获得深度数据，基于彩色摄像头获得彩色图像，对像素坐标叠加；

基于ROS-tf包对采集深度图像和RGB图像进行坐标变换，将深度图像坐标系统的u/v坐标转化到以Kinect深度相机为原点的x/y坐标，进而能够获得真实世界中物体表面任意点的X,Y,Z坐标，即得到任意点的位置数据，从而形成多个点的集合点云。

7.根据权利要求5所述的一种现代有轨清洁车辅助自动驾驶方法，其特征在于，所述路径评价处理方法，具体包括以下步骤：

二值化处理步骤：对RGB图像通过二值化处理得到二值化图像；

距离评估步骤：根据深度相机采集到的深度图像得到深度图像中轨道距离车辆的距离范围；

路径调整步骤：将图像中间垂直方向的距离两侧边距离相等的线作为对截留后的轨道图像的中心线，基于openCV的cvMoments函数计算获得图像重心，图像重心与中心线下端的连线即为引导线，计算引导线与中心线之间的夹角，进而基于PID闭环控制法获得目标角度增量，基于目标角度增量进行调整行驶路径。

8.根据权利要求5所述的一种现代有轨清洁车辅助自动驾驶方法，其特征在于，所述轨道路线循迹步骤，具体包括以下步骤；

采集步骤：通过深度相机基于RGB摄像头将采集得到的RGB图像；

色彩空间转换步骤：将RGB图像转换为HSV图像；

分割步骤：对HSV图像进行图像分割得到第一分割图像和第二分割图像，通过第一分割图像提取槽型轨道位置，对第一分割图像进行二值化处理得到轨道的轮廓，即找出轨道的中心线，通过计算第二分割图像的图形重心，将该图形重心作为引导线；

角速度调整步骤：进行中心线与引导线误差处理，即计算图像中的中心线到引导线的距离，将中心线到引导线的距离转换成目标角速度，基于ROS结合目标角速度调整电机进行控制方向盘；

返回到采集步骤，循环执行采集步骤、色彩空间转换步骤、分割步骤以及角速度调整步骤实现稳定的轨道跟随。

9.根据权利要求5所述的一种现代有轨清洁车辅助自动驾驶方法，其特征在于，所述基于PID闭环控制法计算导航参数，具体为：选择Δω(角度增量)为控制的目标变量，Δω的基准是前一次闭环控制结尾的方向盘实际角度ω，即采用角度增量控制公式进行控制，所述角度增量控制公式为：

其中ω^*表示目标角度，ω表示当前角度，ω₀表示设定角度，δ表示预设角度差值阈值，K表示角度比例系数，当目标角度ω^*与当前角度ω之间的差值大于或等于预设角度差值阈值δ时，则令角度增量Δω等于设定角速度ω₀与闭环周期T的乘积；当目标角度ω^*与当前角度ω之间的差值小于预设角度差值阈值δ时，则令角度增量Δω等于目标角度ω^*与当前角度ω之间的差值乘以一个角度比例系数K，最终使得角度增量Δω趋向于0；

所述导航参数具体为车辆的角度增量。

Images