CN110398979A - 一种基于视觉与姿态融合的无人驾驶工程作业设备循迹方法及装置 - Google Patents

Abstract

本发明涉及一种基于视觉与姿态融合的无人驾驶工程作业设备循迹方法装置，可以通过视觉图像处理方法实现对工程作业设备航向及位置偏差的检测，同时通过在工程作业设备前后车体合适位置安装姿态传感器，在工程作业设备严重偏移碾压条带的情况下使用姿态传感器信息进行航向纠偏，直到条带重新回到视觉相机视野中就可以继续进行视觉循迹控制。相比现有技术通过差分GPS的循迹方案，本发明物理设备采用工业相机，不受卫星信号等影响，相比差分GPS降低了硬件成本，避免了因GPS信号不稳定带来的循迹控制不可靠问题。

Description

一种基于视觉与姿态融合的无人驾驶工程作业设备循迹方法 及装置

技术领域

本发明属于无人驾驶控制领域，涉及无人驾驶视觉循迹控制领域，具体是一种基于视觉与姿态融合的无人驾驶工程作业设备循迹方法及装置。

背景技术

随着无人驾驶技术的蓬勃发展，乘用车、重型卡车的无人驾驶研究与应用也在如火如荼的进行着。在汽车领域之外，工程作业设备也在往智能化的方向发展。工程作业设备广泛应用于筑路筑坝等施工建设，但恶劣的工作环境，长期重复枯燥的工程作业设备驾驶作业等导致工程作业设备驾驶员的工作效率降低，碾压施工质量得不到保障。此外，工程作业设备在工作过程中，需要开启震动碾压模式，这种激振严重危害工程作业设备驾驶员的身体健康。为了保护施工人员身体健康，保证施工质量，降低工程作业设备的作业成本，无人驾驶工程作业设备的研发具有十分重要的意义。

在工程作业设备工作过程中，往往需要将一个给定作业区域划分多个作业条带，并在各条带中进行往复碾压作业，碾压过程中需要保证碾压条带的覆盖率足够高，且碾压条带尽可能笔直。这就对无人驾驶工程作业设备的精确循迹提出了很高的要求。因此无人驾驶工程作业设备精确循迹技术的突破至关重要。现有技术里无人驾驶工程作业设备的循迹方法主要是通过差分天线GPS实现，详细方法是通过事先采集碾压作业区域的GPS点坐标，形成一系列期望轨迹点集。在工程作业设备作业过程中，只需要根据当前的GPS坐标去搜索工程作业设备前进方向上距离当前点最近的期望轨迹上的GPS轨迹点。然后计算横向位移偏差与航向偏差，进而通过设计控制算法实现车辆逼近期望轨迹的控制。但是GPS本身容易出现卫星信号接收不稳定，信号漂移，信号波动等问题，一旦在循迹过程中发生了上述问题，循迹就会失控，导致碾压作业失败，甚至出现安全问题。

发明内容

本发明的目的在于克服现有技术的不足之处，提供一种基于视觉与姿态融合的无人驾驶工程作业设备循迹方法，通过相机校准与畸变矫正，图像基本处理、特征颜色直方图提取、姿态融合、特征计算、循迹控制等方式实现基于视觉的无人驾驶工程作业设备循迹控制。

本发明解决其技术问题是采取以下技术方案实现的：

一种基于视觉与姿态融合的无人驾驶工程作业设备循迹方法，其特征在于：具体步骤如下：

(1)图像输入采集，用图像采集单元采集工程作业设备前、后的图像；

(2)相机参数校准与图像畸变矫正，将上一步采集的图像转化为平面图像，对图像的畸变进行修正；

(3)进行基于颜色空间与边缘特征的阈值化创建，对于修正后获得的图像进行处理，获得完整的碾压条带轨迹线的图像并创建为透视投影二值图像；

(4)应用透视变换纠正二值图像，将上一步骤获得的透视投影二值图像，变换为鸟瞰图形式二值图像；

(5)基于颜色直方图创建滑窗检测提取工程作业设备地面标线像素，根据上一步骤获得鸟瞰图形式二值图像中的碾压条带轨迹，获得左、右两侧碾压条带轨迹线的待选点；

(6)确定相对于采集图像中心的工程作业设备位置偏差与航向偏差，通过对左、右两侧待选点进行二次曲线拟合后，计算求得所需的位置偏差与航向偏差；

(7)对航向和位置偏差的数值滤波并输出，对于计算获得的位置偏差与航向偏差进行数值滤波后输出。

而且，在步骤(4)与步骤(5)之间，增加以下步骤：

判断条带特征是否严重偏离相机视野；

如果为判断为否，则直接进行步骤(5)；如果判断为是，则增加步骤，计算姿态传感器航向并进行航向纠偏，完成后再进行原步骤(5)。

而且，所述步骤(2)图像畸变矫正的具体方法为：

真实图像img与畸变图像imgD之间的关系为:img(U,V)＝imgD(Ud,Vd)，找出步骤(1)采集的原始图像对应所有的img(U,V)，进而结合相机的内部参数与外部参数实现相机的畸变矫正。

而且，所述步骤(3)具体步骤：采用HSL模型来描述图像，并根据HSL模型中的L通道来分割出图像中的地面条带白色轨迹线，得到一副分割后的结果记为MASK1；

对图像进行边缘检测，采用Sobel边缘提取算法选择横向或纵向的边缘进行提取，将进行边缘提取后的图像做二进制图的转化，即提取到边缘的像素点显示为白色，未提取到边缘的像素点显示为黑色(值为0)，并记此边缘提取分割得到的结果为MASK2；

综合考虑颜色空间分割得到的MASK1与边缘特征阈值化分割得到的MASK2，将两次的分割结果进行并集运算，叠加到一幅图上，得到两条被分割出来的完整的碾压条带轨迹线，此时图像为透视投影二值图像。

而且，所述步骤(4)的具体步骤为：

在透视投影二值图像中沿着左右轨迹线的边缘，选择一个梯形区域作为感兴趣区域，记录透视投影二值图像中选取覆盖轨迹线的梯形区域的四个顶点的坐标(src_points)；

新建一幅二值图，设置一个合适的长方形区域用于投影，令这个长方形的四个端点(dst_points)一一对应着透视投影二值图像中的四个顶点；

计算透视投影矩阵，进而得到透视变换后的图像，获得鸟瞰图形式二值图像。

而且，所述步骤(5)的具体步骤为：

确定左、右碾压条带轨迹线的大致位置，在鸟瞰图形式二值图像中对左右侧碾压条带轨迹线的点进行搜索；

找到左右车道线的大致位置，将这两个位置作为起始点；分别以两个起始点作为窗口的下边线中点，存储所有在方块中的白色点的横坐标，随后对存储的横坐标取均值，将该均值所在的列以及第一个窗口的上边缘所在的位置，作为下一个窗口的下边线中点，继续搜索；

以此往复，直到把所有的行都搜索完毕；

所有落在窗口中的白点，为左右侧碾压条带轨迹线的待选点，

随后将左右两侧待选点分别做二次曲线拟合，得到碾压条带轨迹线的曲线方程。

而且，所述步骤(6)的具体步骤为：

通过对左右两侧待选点进行二次曲线拟合后，设置两个列表分别存储左右两侧待选点，然后逐行对左右两侧待选点进行取中心计算，设置左侧任一待选点为(left_x,left_y)，同一行右侧待选点为(right_x,right_y)，对于同一行而言，若所检测出的中线上一点为(mid_x,mid_y)，则:

mid_x＝left_x+(right_x–left_x)/2，

mid_y＝left_y＝right_y

以此可以求得碾压条带中线上的所有待选点，并根据这些点拟合二次曲线，求得拟合的曲线参数，

对于尺寸为宽w，高h的图像，以图像左上角为(0,0)坐标系，选定预瞄点坐标(0.5w,0.25h)，在视觉检测的过程中，将预瞄点的纵坐标带入中线所拟合的二次曲线中，求得拟合曲线上的横坐标记为plot_x；

在计算位置偏差与航向偏差时，需要事先对距离进行标定，对于一张图像而言，x与y两个方向每个像素所代表的实际距离需要通过标定得到，分别记为xm_per_pix，ym_per_pix。

则图像上的x方向位置偏差offset的计算公式为：

位置偏差offset＝(plot_x–0.5w)*xm_per_pix

对于预瞄点方向的航向，通过计算预瞄点纵坐标下对应的碾压条带中线所拟合的二次曲线上的对应点(plot_x,0.25h)处的斜率，记为k1，取图像x方向为参考线，参考系斜率记为k2，其中k2＝0。这里记θ1为斜率分别为k1、k2的两条线之间的夹角，计算公式如下：

对tanθ1进行反正切计算之后再有弧度值转换为角度值，得到θ1的数值，预瞄点纵坐标下检测碾压条带的中线斜率与图像中线的夹角θ2，θ2为工程作业设备驾驶室与道路预瞄点偏差，计算公式：

θ2＝90°-θ1

但欲求的航向偏差为工程作业设备前振动轮与地面标线的航向偏差，记为θ3，将驾驶室与前振动轮的转向角度偏差记为θ4，其中θ4由装在工程作业设备前后的姿态传感器的航向数据求差得到。则欲求的偏向偏差计算方法如下：

θ3＝θ2-θ4

至此，求出所需的位置偏差与航向偏差。

而且，所述条带特征是否严重偏离相机视野的具体判断方法为：对鸟瞰图形式二值图像的长方形区域在x方向绘制直方图，对每一列的白色的点数量进行统计，以区域中线为界，若左右各存在一个明显的峰值分布，则说明条带特征仍保留在相机视野内；否则，说明条带特征已经偏离相机视野。

一种基于视觉与姿态融合的无人驾驶工程作业设备循迹装置，其特征在于：包括图像采集单元、图像处理单元、特征提取单元、循迹计算单元以及横向控制单元，图像采集单元、图像处理单元、特征提取单元、循迹计算单元以及横向控制单元依次顺序连接。

而且，所述图像处理单元与特征提取单元之间顺序连接有特征严重偏移判断单元以及航向纠偏单元；特征严重偏移判断单元的一个输出端直接连接特征提取单元。

本发明的优点和积极效果是：

1、本发明公开了一种基于视觉与姿态融合的无人驾驶工程作业设备循迹方法及装置，可以通过视觉图像处理方法实现对工程作业设备航向及位置偏差的检测，同时通过在工程作业设备前后车体合适位置安装姿态传感器，在工程作业设备严重偏移碾压条带的情况下使用姿态传感器信息进行航向纠偏，直到条带重新回到视觉相机视野中就可以继续进行视觉循迹控制。相比现有技术通过差分GPS的循迹方案，本发明物理设备采用工业相机，不受卫星信号等影响，相比差分GPS降低了硬件成本，避免了因GPS信号不稳定带来的循迹控制不可靠问题。

2、本发明利用视觉感知实现无人驾驶工程作业设备条带检测进而完成循迹，在条带严重偏移视野的情况下也可以通过姿态传感器航向信息实现纠偏，提高了系统的鲁棒性。通过视觉算法检测碾压区域条带特征，结合图像校准与滤波，可以实现稳定可靠且较高精度的检测。本发明从物理结构与算法原理方面相比现有技术都进行了创新，降低了工程研发的设备成本，避免了因传感器安装、卫星信号丢失、信号波动，GPS信号漂移等带来的运动控制不可靠问题，为无人驾驶工程开发带来了极大的方便。

附图说明

图1是本发明实施例一的检测方法流程示意图；

图2是本发明实施例二的检测方法流程示意图；

图3是本发明的检测装置结构示意图。

具体实施方式

下面结合附图并通过具体实施例对本发明作进一步详述，以下实施例只是描述性的，不是限定性的，不能以此限定本发明的保护范围。

实施例一：

一种基于视觉与姿态融合的无人驾驶工程作业设备循迹方法，具体步骤如下：

(1)图像输入采集，使用图像采集单元采集工程作业设备前、后的图像，完整清晰的显示出碾压区域中碾压条带轨迹线；

图像采集单元至少布置前后各一套，采用例如工业相机等作为图像采集单元的采集设备，采集设备稳定安装在工程作业设备车体上，以车体驾驶室前后位置正中为宜。由于工程作业设备的运动过程会带来震动，因此图像采集设备的安装需要考虑防震，可以采用防震硬质弹簧缓冲器或者设置防震橡胶垫片等方式进行防震处理。采集前，调整图像采集设备的采集角度，使有效视野内完整清晰显示出碾压区域中碾压条带轨迹线。

(2)相机参数校准与图像畸变矫正，将上一步采集的图像转化为图像平面，对图像的畸变进行修正；

图像采集单元(如相机等)的成像过程实质上是几个坐标系的转换，首先将空间中的点由“世界坐标系”转换到“相机坐标系”，然后再将其投影到成像平面，也即“图像物理坐标系”，最后再将成像平面上的数据转换到图像平面，也即“图像像素坐标系”。但是由于透镜制造精度以及组装工艺的偏差会引入畸变，导致原始图像的失真。其中镜头的畸变分为径向畸变和切向畸变两类。径向畸变是沿着透镜半径方向分布的畸变，产生原因是光线在远离透镜中心的地方比靠近中心的地方更加弯曲，这种畸变在普通廉价的镜头中表现更加明显。切向畸变是由于透镜本身与相机传感器平面(成像平面)或图像平面不平行而产生的，这种情况多是由于透镜被粘贴到镜头模组上的安装偏差导致。

使用有畸变的图像做条带轨迹视觉检测，检测结果的精度将会受到影响，因此进行图像处理的第一步工作就是去畸变。为了解决相机图像的畸变问题，相机校准标定技术应运而生。相机参数校准是通过对已知的形状进行拍照，通过计算该形状在真实世界中位置与在图像中位置的偏差量(畸变系数)，进而用这个偏差量去修正其他畸变图像的技术。

详述畸变矫正的原理：现有图像像素坐标系(uOv坐标系)下的无畸变坐标(U,V)，经过径向畸变和切向畸变后落在了uOv坐标系的(Ud,Vd)上。即就是说，真实图像img与畸变图像imgD之间的关系为:img(U,V)＝imgD(Ud,Vd)。

上一步我们得到的图像采集装置采集的原始图像，也就是产生畸变后的图像，要得到没有畸变的图像就要通过畸变模型推导其映射关系。真实图像img与畸变图像imgD之间的关系为:img(U,V)＝imgD(Ud,Vd)。可以通过这个关系，找出所有的img(U,V)。这个过程中，需要通过相机校准方法，如张正友标定法得到相机的内部参数，结合相机的安装位置得到相机的外部参数。进而结合相机的内部参数与外部参数实现相机的畸变矫正。

因为对于一幅图像而言，U和V都是整数，因为U和V代表组成图像的像素坐标位置。再从畸变后的图像imgD(Ud,Vd)中矫正为无畸变图像img(U,V)的过程中，会计算得出(Ud,Vd)往往不是整数的情况，因此需要进行插值法进行求解，例如最近邻插值与双线性内插法等均可实现。

(3)基于颜色空间与边缘特征进行阈值化创建二进制图像；

图像中的每个像素点都是由RGB(红绿蓝)三个颜色通道组成，其中R(red-红色)、G(green-绿色)、B(blue-蓝色)为了方便描述RGB颜色模型，在计算机中约束了每个通道由暗到亮的范围是0～255。当某个像素点的R通道数值为255，G和B通道数值为0时，实际表现出的颜色就是最亮的红色；当某个像素点的RGB三通道都为255时，所表示的是最亮的白色；当某个像素点的RGB三通道都为0时，就会显示最暗的黑色。在RGB颜色模型中，不会有比[255,255,255]的组合更亮的颜色了。

根据以上理论基础，一幅彩色图像，其实就是由三幅单通道的图像叠加。但是在图像处理中，通常情况下我们不会直接对RGB图像做处理，这主要是因为RGB与人类的视觉感知相差较远，

本实施例采用HSL(H色相、S饱和度、L亮度)模型来描述图像。并根据HSL模型中的L(亮度)通道来分割出图像中的地面条带白色轨迹线，得到一副分割后的结果记为MASK1。

同时由于所关注是地面条带白色轨迹线，因此对图像进行边缘检测是有必要的。一般常用Canny边缘检测算法获取图像待选边缘特征，但是，Canny边缘检测算法会将图像中各个方向、明暗交替位置的边缘都提取出来，因此，使用Canny边缘提取算法在处理碎石铺成的碾压区域路面时，会将各种碎石的轮廓也提取出来，这是我们不愿意看到的。因此我们选用Sobel边缘提取算法。Sobel相比于Canny的优秀之处在于，它可以选择横向或纵向的边缘进行提取。而我们关心的正是碾压条带轨迹在横向上的边缘突变。将进行边缘提取后的图像做二进制图的转化，即提取到边缘的像素点显示为白色(值为1)，未提取到边缘的像素点显示为黑色(值为0)，并记此边缘提取分割得到的结果为MASK2。

最后，综合考虑颜色空间分割得到的MASK1与边缘特征阈值化分割得到的MASK2，将两次的分割结果进行并集运算，叠加到一幅图上，就能得到两条被分割出来的完整的碾压条带轨迹线。此时图像为二值图，即只有碾压条带轨迹线为白色，其余像素都是黑色。

(4)应用透视变换来纠正二值图像；

在完成上述步骤后，我们得到了一副只包含期望轨迹特征的二值图像，在后续的处理过程中，为降低计算复杂度，需要定义一个感兴趣区域。很显然，我们的感兴趣区域就是工程作业设备正前方的这个碾压条带轨迹线。为了获取感兴趣区域，我们需要对工程作业设备正前方的碾压条带区域使用一种叫做透视变换的技术。所谓透视是图像成像时，物体距离摄像机越远，看起来越小的一种现象。在真实世界中，左右互相平行的条带轨迹线，会在图像的最远处交汇成一个点。这个现象就是“透视成像”的原理造成的。

透视变换的详细过程如下：首先新建一幅与原二值图同样大小的图，随后在原二值图中确定感兴趣区域。这里由于碾压条带两台轨迹线在真实世界中是平行线，但是在图像成像中则显示为一个梯形的两腰。因此我们沿着左右轨迹线的边缘，选择一个梯形区域作为感兴趣区域。因此在原二值化图像中选取覆盖轨迹线的梯形区域的四个顶点，记录这4个点的坐标，我们称这4个点为src_points。透视变换要做的就是将梯形区域还原为在现实世界中的一个长方形区域，我们新建一幅二值图，在其中设置一个合适的长方形区域用于投影，令这个长方形的4个端点一一对应着原图中的src_points，我们称新的这4个点为dst_points。得到src_points，dst_points后，就可以计算透视投影矩阵，进而得到透视变换后的图像。最终的透视投影结果就像“鸟瞰图”一样。

(5)基于颜色直方图创建滑窗检测提取工程作业设备地面标线像素；

在检测碾压条带轨迹前，需要粗定位碾压条带轨迹的位置。为了方便理解，这里引入一个概念——直方图。举例来说，我们所处理的图像的分辨率为1280*720，即720行，1280列。如果将每一列的白色的点数量进行统计，即可得到1280个值。将这1280个值绘制在一个坐标系中，横坐标为1-1280，纵坐标表示每列中白色点的数量，那么这幅图就是“直方图”。只要找到直方图左半边最大值所对应的列数，即为左侧碾压条带轨迹线所在的大致位置；找到直方图右半边最大值所对应的列数，即为右侧碾压条带轨迹线所在的大致位置。

确定了左右侧碾压条带轨迹线的大致位置后，使用一种叫做“滑动窗口”的技术，在图中对左右侧碾压条带轨迹线的点进行搜索。首先根据前面介绍的直方图方法，找到左右车道线的大致位置，将这两个大致位置作为起始点。定义一个矩形区域，称之为“窗口”，分别以两个起始点作为窗口的下边线中点，存储所有在方块中的白色点的横坐标。随后对存储的横坐标取均值，将该均值所在的列以及第一个“窗口”的上边缘所在的位置，作为下一个“窗口”的下边线中点，继续搜索。以此往复，直到把所有的行都搜索完毕。所有落在窗口中的白点，即为左右侧碾压条带轨迹线的待选点。随后将左右两侧待选点分别做二次曲线拟合，即可得到碾压条带轨迹线的曲线方程。

(6)确定相对于采集图像中心的工程作业设备位置偏差与航向偏差。

通过对左右两侧待选点进行二次曲线拟合后，设置两个列表分别存储左右两侧待选点。然后逐行对左右两侧待选点进行取中心计算。设置左侧任一待选点为(left_x,left_y)，同一行右侧待选点为(right_x,right_y)，对于同一行而言，若所检测出的中线上一点为(mid_x,mid_y)，则:

mid_x＝left_x+(right_x–left_x)/2，

mid_y＝left_y＝right_y

以此可以求得碾压条带中线上的所有待选点，并根据这些点拟合二次曲线，求得拟合的曲线参数。对于尺寸为宽w，高h的图像，以图像左上角为(0,0)坐标系，选定预瞄点坐标(0.5w,0.25h)，在视觉检测的过程中，将预瞄点的纵坐标带入中线所拟合的二次曲线中，即可求得拟合曲线上的横坐标记为plot_x。在计算位置偏差与航向偏差时，需要事先对距离进行标定，对于一张图像而言，x与y两个方向每个像素所代表的实际距离(以米为单位)需要通过标定得到。分别记为xm_per_pix，ym_per_pix。

则图像上的x方向位置偏差offset(假设相机位于车体的中轴线)的计算公式为：

位置偏差offset＝(plot_x–0.5w)*xm_per_pix

对于预瞄点方向的航向，通过计算预瞄点纵坐标下对应的碾压条带中线所拟合的二次曲线上的对应点(plot_x,0.25h)处的斜率，记为k1，取图像x方向为参考线，参考系斜率记为k2，其中k2＝0。这里记θ1为斜率分别为k1、k2的两条线之间的夹角，计算公式如下：

对tanθ1进行反正切计算之后再有弧度值转换为角度值即可得到θ1的数值，预瞄点纵坐标下检测碾压条带的中线斜率与图像中线的夹角θ2，θ2为工程作业设备驾驶室与道路预瞄点偏差，计算公式：

θ2＝90°-θ1

但欲求的航向偏差为工程作业设备前振动轮与地面标线的航向偏差，记为θ3，将驾驶室与前振动轮的转向角度偏差记为θ4，其中θ4由装在工程作业设备前后的姿态传感器的航向数据求差得到。则欲求的偏向偏差计算方法如下：

θ3＝θ2-θ4

至此，可以求出所需的位置偏差与航向偏差。

(7)对航向和位置偏差的数值滤波并输出。

在经过前述步骤的视觉检测后，可以得到航向偏差与位置偏差，但是视觉检测会受到光照等因素影响，可能带来输出输出值的波动，因此必须采用数值滤波的方式，才能保证输出的稳定性。本实施例采用了递推平均滤波法(又称滑动平均滤波法)来实现航向偏差与位置偏差的稳定输出。

递推平均滤波法的详细说明如下，将视觉感知持续输出的航向偏差与位置偏差分别视为连续的采样值，把连续取得的N个采样值看成一个队列，队列的长度固定为N，每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据(先进先出原则)，把队列中的N个数据进行算术平均运算，获得新的滤波结果即可使用。这种滤波方法的优点在于对周期性干扰有良好的抑制作用，平滑度高，适用于高频振荡的系统。

根据上述步骤的实现，本例可以实现基于视觉的无人驾驶工程作业设备碾压条带特征检测与航向偏差、位置偏差的稳定输出。

实施例二：

一种基于视觉与姿态融合的无人驾驶工程作业设备循迹方法，以实施例一为基础，进一步的，结合姿态传感器数据信息针对工程作业设备检测过程中出现的循迹条带成像严重偏移视野的情况进行纠偏处理，具体步骤为：

(1)图像输入采集。

(2)相机参数校准与图像畸变矫正。

(3)基于颜色空间与边缘特征进行阈值化创建二进制图像。

(4)应用透视变换来纠正二值图像。

(5)条带特征是否严重偏离相机视野。

在上一步的二值图像中，已经将ROI区域投影到一个长方形区域内，正常情况下，该长方形区域内应该左右分布两条白色线，期望情况下两条白线应该趋近于平行。因此，这里对该长方形区域在x方向绘制直方图，对每一列的白色的点数量进行统计，以区域中线为界，若左右各存在一个明显的峰值分布，则说明条带特征仍保留在相机视野内，则跳过步骤(6)直接进行步骤(7)；否则，就输出条带特征已经偏离相机视野，进行步骤(6)的纠偏过程。

(6)计算姿态传感器航向并进行航向纠偏；

本实施例中在工程作业设备的前后车体处都安装了一个姿态传感器，主要用于航向信息的获取，用于当工程作业设备行驶严重偏离地面条带时候进行基于航向角的纠偏。在工程作业设备的运动过程中，由于碾压条带是实现设置完成的，因此航向信息是确定的。本实施例将给定的航向信息作为先验信息，在执行每一个条带的碾压任务时，若出现偏离碾压条带时，就启用前后两台姿态传感器的航向信息分别与先验航向对比，通过控制算法控制工程作业设备进行航向纠偏，直到视觉重新可以检测到碾压条带时，停止使用姿态传感器的航向纠偏功能，继续进行视觉检测与循迹。

(7)基于颜色直方图创建滑窗检测提取工程作业设备地面标线像素。

(8)确定相对于采集图像中心的工程作业设备位置偏差与航向偏差。

(9)对航向和位置偏差的数值滤波并输出。

(10)基于航向和位置偏差进行横向控制。

在基于上述步骤后，可以得到航向偏差与位置偏差，将其作为控制量送入控制器中进行计算工程作业设备方向盘的转角量。无人驾驶工程作业设备的底层执行机构主要通过转向电机驱动工程作业设备方向盘的转动，通过纵向电机驱动前进后退推杆。这里所谓的横向控制就是指对方向盘的转向控制。通过建立工程作业设备运动学模型，设计控制算法，例如是PID(比例-积分-微分控制)及其变体算法或者是ADRC(自抗扰控制)算法等。这里的控制算法的输入为视觉与姿态融合给出的航向偏差与位置偏差，输出为方向盘的转角。控制算法输出量通过CAN通信送到转向电机控制器，进而通过转向电机控制器控制方向盘转动相应角度，实现横向控制。这里的横向控制方法仅作为一种实施案例供参考，为精确控制可以使用更优的控制策略。但控制器的输入量都是前述步骤所给出的航向偏差与位置偏差。

根据上述步骤的实现，本例可以实现基于视觉与姿态融合的无人驾驶工程作业设备循迹控制。

一种基于视觉与姿态融合的无人驾驶工程作业设备循迹装置，包括图像采集单元、图像处理单元、特征严重偏移判断单元、航向纠偏单元、特征提取单元、循迹计算单元以及横向控制单元，图像采集单元、图像处理单元、特征严重偏移判断单元、航向纠偏单元、特征提取单元、循迹计算单元以及横向控制单元依次顺序连接。

图像采集单元在无人驾驶工程作业设备前、后各安装一套，具体安装位置分别在工程作业设备驾驶室前、后正中部，图像采集单元主要用于提供进行视觉检测的原始数据来源。通过调整图像采集设备的采集角度，保证有效视野内完整清晰显示出碾压区域中碾压条带轨迹线为宜。

图像处理单元主要包括相机校准、图像畸变矫正、基于颜色特征与边缘特征提取的二值化处理、透视变换、直方图特征提取与滑窗检测轨迹线并拟合二次曲线，图像处理单元是整个装置的体系基础。

特征严重偏移判断单元主要通过对透视变换后的二值图进行判断，判断方法主要通过对每一列的白色的点数量进行统计，以区域中线为界，若左右各存在一个明显的峰值分布，则说明条带特征仍保留在相机视野内，否则，就输出条带特征已经偏离相机视野，继续执行后续纠偏单元。

航向纠偏单元主要用于当工程作业设备行驶严重偏离地面条带时候进行基于航向角的纠偏。航向纠偏单元将给定的航向信息作为先验信息，在执行每一个条带的碾压任务时，若出现偏离碾压条带时，就启用前后两台姿态传感器的航向信息分别与先验航向对比，通过控制算法控制工程作业设备进行航向纠偏，直到视觉重新可以检测到碾压条带时，停止使用姿态传感器的航向纠偏功能。

特征提取单元主要通过建立直方图进行特征检测，直方图以图像列数建立横坐标，纵坐标表示每列中白色点的数量。只要找到直方图左半边最大值所对应的列数，即为左侧碾压条带轨迹线所在的大致位置；找到直方图右半边最大值所对应的列数，即为右侧碾压条带轨迹线所在的大致位置。找到了条带大致位置后，就可以通过设置滑窗在图中对左右侧碾压条带轨迹线的点进行搜索。具体搜索步骤参加实施例一、二。最终得到左右侧碾压条带轨迹线的待选点。随后将左右两侧待选点分别做二次曲线拟合，即可得到碾压条带轨迹线的曲线方程。

循迹计算单元主要根据视觉检测到的特征进行拟合二次曲线，同时在计算位置偏差与航向偏差时，需要事先对像素距离与真实世界的距离进行标定，对于一张图像而言，x与y两个方向每个像素所代表的实际距离(以米为单位)需要通过标定得到。进而提过实施例一与实施例二中的计算方法可以得到待求的循迹所需的控制量——航向偏差与位置偏差，此外，对于视觉特征计算得到的航向偏差于位置偏差也要进行数值滤波，实现稳定输出。

横向控制单元主要基于航向偏差与位置偏差实现对方向盘的转向控制。通过建立工程作业设备运动学模型，设计控制算法，例如是PID(比例-积分-微分控制)及其变体算法或者是ADRC(自抗扰控制)算法等。控制算法输出量为工程作业设备方向盘转角，然后通过CAN通信送到转向电机控制器，进而通过转向电机控制器控制方向盘转动相应角度，实现横向控制。

最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。

Claims (10)

1.一种基于视觉与姿态融合的无人驾驶工程作业设备循迹方法，其特征在于：具体步骤如下：

(1)图像输入采集，用图像采集单元采集工程作业设备前、后的图像；

(2)相机参数校准与图像畸变矫正，将上一步采集的图像转化为平面图像，对图像的畸变进行修正；

(3)进行基于颜色空间与边缘特征的阈值化创建，对于修正后获得的图像进行处理，获得完整的碾压条带轨迹线的图像并创建为透视投影二值图像；

(4)应用透视变换纠正二值图像，将上一步骤获得的透视投影二值图像，变换为鸟瞰图形式二值图像；

(5)基于颜色直方图创建滑窗检测提取工程作业设备地面标线像素，根据上一步骤获得鸟瞰图形式二值图像中的碾压条带轨迹，获得左、右两侧碾压条带轨迹线的待选点；

(6)确定相对于采集图像中心的工程作业设备位置偏差与航向偏差，通过对左、右两侧待选点进行二次曲线拟合后，计算求得所需的位置偏差与航向偏差；

(7)对航向和位置偏差的数值滤波并输出，对于计算获得的位置偏差与航向偏差进行数值滤波后输出。

2.根据权利要求1所述的基于视觉与姿态融合的无人驾驶工程作业设备循迹方法，其特征在于：在步骤(4)与步骤(5)之间，增加以下步骤：

判断条带特征是否严重偏离相机视野；

如果为判断为否，则直接进行步骤(5)；如果判断为是，则增加步骤，计算姿态传感器航向并进行航向纠偏，完成后再进行原步骤(5)。

3.根据权利要求1所述的基于视觉与姿态融合的无人驾驶工程作业设备循迹方法，其特征在于：所述步骤(2)图像畸变矫正的具体方法为：

真实图像img与畸变图像imgD之间的关系为:img(U，V)＝imgD(Ud，Vd)，找出步骤(1)采集的原始图像对应所有的img(U，V)，进而结合相机的内部参数与外部参数实现相机的畸变矫正。

4.根据权利要求1所述的基于视觉与姿态融合的无人驾驶工程作业设备循迹方法，其特征在于：所述步骤(3)具体步骤：采用HSL模型来描述图像，并根据HSL模型中的L通道来分割出图像中的地面条带白色轨迹线，得到一副分割后的结果记为MASK1；

对图像进行边缘检测，采用Sobel边缘提取算法选择横向或纵向的边缘进行提取，将进行边缘提取后的图像做二进制图的转化，即提取到边缘的像素点显示为白色，未提取到边缘的像素点显示为黑色(值为0)，并记此边缘提取分割得到的结果为MASK2；

综合考虑颜色空间分割得到的MASK1与边缘特征阈值化分割得到的MASK2，将两次的分割结果进行并集运算，叠加到一幅图上，得到两条被分割出来的完整的碾压条带轨迹线，此时图像为透视投影二值图像。

5.根据权利要求1所述的基于视觉与姿态融合的无人驾驶工程作业设备循迹方法，其特征在于：所述步骤(4)的具体步骤为：

在透视投影二值图像中沿着左右轨迹线的边缘，选择一个梯形区域作为感兴趣区域，记录透视投影二值图像中选取覆盖轨迹线的梯形区域的四个顶点的坐标(src_points)；

新建一幅二值图，设置一个合适的长方形区域用于投影，令这个长方形的四个端点(dst_points)一一对应着透视投影二值图像中的四个顶点；

计算透视投影矩阵，进而得到透视变换后的图像，获得鸟瞰图形式二值图像。

6.根据权利要求1所述的基于视觉与姿态融合的无人驾驶工程作业设备循迹方法，其特征在于：所述步骤(5)的具体步骤为：

确定左、右碾压条带轨迹线的大致位置，在鸟瞰图形式二值图像中对左右侧碾压条带轨迹线的点进行搜索；

找到左右车道线的大致位置，将这两个位置作为起始点；分别以两个起始点作为窗口的下边线中点，存储所有在方块中的白色点的横坐标，随后对存储的横坐标取均值，将该均值所在的列以及第一个窗口的上边缘所在的位置，作为下一个窗口的下边线中点，继续搜索；

以此往复，直到把所有的行都搜索完毕；

所有落在窗口中的白点，为左右侧碾压条带轨迹线的待选点，

随后将左右两侧待选点分别做二次曲线拟合，得到碾压条带轨迹线的曲线方程。

7.根据权利要求1所述的基于视觉与姿态融合的无人驾驶工程作业设备循迹方法，其特征在于：所述步骤(6)的具体步骤为：

通过对左右两侧待选点进行二次曲线拟合后，设置两个列表分别存储左右两侧待选点，然后逐行对左右两侧待选点进行取中心计算，设置左侧任一待选点为(left_x，left_y)，同一行右侧待选点为(right_x，right_y)，对于同一行而言，若所检测出的中线上一点为(mid_x，mid_y)，则:

mid_x＝left_x+(right_x–left_x)/2，

mid_y＝left_y＝right_y

以此可以求得碾压条带中线上的所有待选点，并根据这些点拟合二次曲线，求得拟合的曲线参数，

对于尺寸为宽w，高h的图像，以图像左上角为(0，0)坐标系，选定预瞄点坐标(0.5w，0.25h)，在视觉检测的过程中，将预瞄点的纵坐标带入中线所拟合的二次曲线中，求得拟合曲线上的横坐标记为plot_x；

在计算位置偏差与航向偏差时，需要事先对距离进行标定，对于一张图像而言，x与y两个方向每个像素所代表的实际距离需要通过标定得到，分别记为xm_per_pix，ym_per_pix。

则图像上的x方向位置偏差offset的计算公式为：

位置偏差offset＝(plot_x-0.5w)*xm_per_pix

对于预瞄点方向的航向，通过计算预瞄点纵坐标下对应的碾压条带中线所拟合的二次曲线上的对应点(plot_x，0.25h)处的斜率，记为k1，取图像x方向为参考线，参考系斜率记为k2，其中k2＝0。这里记θ1为斜率分别为k1、k2的两条线之间的夹角，计算公式如下：

对tanθ1进行反正切计算之后再有弧度值转换为角度值，得到θ1的数值，预瞄点纵坐标下检测碾压条带的中线斜率与图像中线的夹角θ2，θ2为工程作业设备驾驶室与道路预瞄点偏差，计算公式：

θ2＝90°-θ1

但欲求的航向偏差为工程作业设备前振动轮与地面标线的航向偏差，记为θ3，将驾驶室与前振动轮的转向角度偏差记为θ4，其中θ4由装在工程作业设备前后的姿态传感器的航向数据求差得到。则欲求的偏向偏差计算方法如下：

θ3＝θ2-θ4

至此，求出所需的位置偏差与航向偏差。

8.根据权利要求2所述的基于视觉与姿态融合的无人驾驶工程作业设备循迹方法，其特征在于：所述条带特征是否严重偏离相机视野的具体判断方法为：对鸟瞰图形式二值图像的长方形区域在x方向绘制直方图，对每一列的白色的点数量进行统计，以区域中线为界，若左右各存在一个明显的峰值分布，则说明条带特征仍保留在相机视野内；否则，说明条带特征已经偏离相机视野。

9.一种基于视觉与姿态融合的无人驾驶工程作业设备循迹装置，其特征在于：包括图像采集单元、图像处理单元、特征提取单元、循迹计算单元以及横向控制单元，图像采集单元、图像处理单元、特征提取单元、循迹计算单元以及横向控制单元依次顺序连接。

10.根据权利要求9所述的基于视觉与姿态融合的无人驾驶工程作业设备循迹装置，其特征在于：所述图像处理单元与特征提取单元之间顺序连接有特征严重偏移判断单元以及航向纠偏单元；特征严重偏移判断单元的一个输出端直接连接特征提取单元。