基于双目立体视觉伺服的智能车室内弯道控制

0 引言

智能车环境感知系统的一个重要任务就是实时地提供车辆在行使过程中的位置信息。机器视觉因其有信号探测范围宽、获取信息完整、符合人类认知习惯、维护成本低、不产生环境污染等多优点，已在智能车环境感知系统中广泛采用。机器视觉车辆环境感知系统中的主要任务是完成道路及目标的识别和跟踪，为智能车行使提供必须的本车位置信息和周围环境距离信息。

对于现实情况下的道路，一般可以将其分为两大类，即：结构化道路和非结构化道路。结构化道路上有明显的道路标记，且这些标记具有较强的几何特征，道路路面主要是由这些标记界分确定的，如高速公路上道路中间黄色的连续标志线或白色的间断标志线以及两旁白色的连续标志线；非结构化道路上没有明显的道路标记，在二维图像中道路路面与非路面主要依靠纹理与色彩而区分的，如没有标记的水泥路、野外土路或石板路等。

弯道图像包含丰富的道路信息和环境信息，解释了道路周围场景。弯道检测是从道路图像中检测出弯曲车道线的边界，这也是对弯道理解的基础。建立弯道模型；提取车道线像素点；拟合车道线模型属于目前较常采用的认知方法，并在特定的结构化道路体现出较好的检测效果。文献[6]介绍了弯道检测在车道偏离预警、弯道限速以及弯道防碰撞预警等领域的应用情况，并提出了弯道检测应该建立三维车道线模型，提高适用性。

均采用的Hough变换求出车道线直线方程，从而确定对应直线段上的最低点和最高点，然后根据相应准则判断曲线道路的弯曲方向，最后分段拟合车道线的直线段和曲线段实现车道线的二维重建。

弯道检测不仅需要识别出道路边界线，还需要判断道路弯曲方向，确定转弯的曲率半径。常用的车道检测方法可分为2大类：基于道路特征和基于道路模型的方法。目前国外主要常用基于道路模型的方法，即将弯道检测转化为各种曲线模型中数学参数的求解问题。省略弯道曲线模型建立和数学参数的复杂求解过程，本文采用立体视觉感知环境的三维信息，利用它的视差原理对所获取室内道路周边环境图像中角点特征的位置恢复其三维信息来判断车体的弯道转向和偏航角度。建立了视觉信息直接控制车体驱动偏离角与偏离距离视觉伺服控制系统，初步采用了BP控制策略，利用Simulink仿真环境实现了针对未知弯道曲率的智能车转弯控制运动。

1 室内道路环境信息的视觉感知

1.1 立体视觉系统模型

采用针孔成像模型将图像中任何点的投影位置与实际点的物理位置建立连线关系，摄像机光心O与空间P点间的连线OP与图像平面的交点即为图像投影的位置(u,v)。用齐次坐标和矩阵表示上述透视投影关系为：

本文采用的双目平行相机的模型如图1所示，C1与C2摄像机的焦距相等，各内部参数也相等，而且两个相机的光轴互相平行，x轴互相重合，y轴互相平行，因此，将第一个摄像机沿x轴平移一段距离b后与第二个摄像机完全重合。假设C1坐标系为O1 x1 y1 z1，C2坐标系为O2 x2 y2 z2，则在上述摄像机配置下，若任何空间点P的坐标在C1坐标系下为(x1),y1,z1，在C2坐标系下为(x1)-b,y1,z1。由中心摄影的比例关系可得：

其中(u1,v1)、(u2,v2)分别为P1与P2的图像坐标。由P1与P2的图像坐标(u2,v2)、(u2,v2)可求出空间点P的三维坐标(x1,y1,z1)。

1.2 室内环境特征向量的提取与匹配

由于噪声、光照变化、遮挡和透视畸变等因素的影响，空间同一点投影到两个摄像机的图像平面上形成的对应点的特性可能不同，对在一幅图像中的一个特征点或者一小块子图像，在另一幅图像中可能存在好几个相似的候选匹配。因此需要另外的信息或者约束作为辅助判据，以便能得到惟一准确的匹配。最近邻法是一种有效的为每个特征点寻找匹配点的方法。最近邻点被定义为与特征点的不变描述子向量之间的欧氏距离最短的点。[page]

假定参考图像P的SIFT特征点集合为：FP ={FP (1),FP (2),-,FP (m)}，m为图像P的特征点的个数；待匹配图像Q的特征点的个数为N，SIFT特征点集合为FQ ={FQ (1)，FQ (2)，-,FQ (m)}。当在建立参考图像中的特征点和待匹配图像的特征筛选对应匹配关系时，应按照图2所示的算法进行对称性测试，只有当两个匹配集中的对应点完全一致时，才视为有效匹配。

SIFT特征向量生成后，利用特征点向量的欧式距离来作为两幅图像中特征点的相似性判定度量。最近邻法是一种有效的为每个特征点寻找匹配点的方法。

最近邻点被定义为与特征点的不变描述子向量之间的欧氏距离最短的点。对特征点集合FP中的每个点，逐一计算其与特征点集合FQ中每个点的距离，得到特征点之间的距离集合D。将距离集合D中的元素进行排序，得到最近邻距离dmin和次近邻距离dn–nim。SIFT算法通过判断最近邻和次近邻距离的比值：

来区分正确匹配对和错误匹配对。

对于正确的匹配对，其最近邻距离dmin要远远小于次近邻距离dn-nim，即DistanceRatio-1；而错误的匹配对，由于特征空间的维数很高，其最近邻距离dmin与次近邻距离dn-nim差距不大，即DistanceRatio≈1。所以可取一个距离比阈值Threh∈(0,1)来区分正确匹配对和错误匹配对。

1.3 运动估计

进行特征点立体匹配和跟踪匹配如图2所示，对当前时刻t获得图像对(ImgL1和ImgL2)进行SIFT特征匹配之后获得道路环境匹配特征点Ni个，再对相邻时刻t+1获得的Ni+1个特征点计算欧式距离，寻找t与t+1时刻获取图像中相同的特征点，得到一系列匹配点在车体运动前后的三维坐标。

本文实验环境是在室内，在假定室内环境为理想的水平面的基础上，同一特征点三维坐标中y坐标保持基本不变，即车体运动参数仅在x-z坐标平面内发生变化。

图3中点P(x1,y1,z1)为所提取环境特征点，在t时刻立体匹配后得到其在摄像头坐标系下的三维坐标，由于摄像头固定安装在小车上，因此通过坐标系的旋转与平移变换可将点P的三维坐标转换到小车坐标系ΣBt下表示。同理，t+1时刻所获取的图像中相同点P的(u,v)二维坐标发生变化，经坐标转换后可得到小车坐标系ΣBt+1下表示。

小车t时刻位于位置A时，特征点P在坐标系ΣBt为参考位置的三维坐标是[x1],y1,z1T，当经过t+1时刻后移动到空间位置B时，同一特征点P在以B为参考位置的三维坐标为[x]t+1,yt+1,zt+1T，所以得到：

当这些位置满足3个及以上相同的匹配特征点时，小车所在坐标系间的平移与旋转向量就可以通过以上方程获得。

2 车体的转弯控制

2.1 车体定位参数计算

环境特征点投影到车体坐标系下表示后，即为所有的环境坐标均统一在世界坐标系下的表示。如图4所示，其中小车前进方向为zw轴，xw水平垂直于zw，方向向右，其交点为原点Ow。直线为lmid为道路中间线，lmid与zw的夹角为φ，即为车辆的偏航角。点Ow到左车道线lL的距离为DL，到右车道线lR的距离为DR，可行驶区域的道路宽度为w = DL+DR。

2.2 弯道控制策略

2.2.1 控制策略

机器人的底层运动控制是非常重要的一个环节，机器人运动控制的好坏直接决定了能否有效执行决策意图，准确无误地沿着预定轨迹行进，完成导航任务。传统的定位控制中，PI、PID是广泛采用的控制方法，这些方法比较成熟，但却具有一定的局限性。对被控系统的参数变化比较敏感，难以克服系统中非线性因素的影响。本文采用BP神经网络控制器在行使过程中经学习对网络进行训练，以适应当前的环境路况来调整车的左右轮速控制量。智能车左右轮速的大小，和车体偏离道路中间线lmid的距离d和车运动时方向偏离中间线的角度φ有关。小车控制输入为d,φ，输出为VL,VR。[page]

基于双目立体视觉获取得到的三维信息，即为经左右图像SIFT特征点匹配，在摄像头坐标系中获取前景环境中各关键点的坐标信息。选取关键点中与机器人基坐标系原点高度靠近的点，并且该点的个数需大于3，之后，采用最小二乘法拟合可得行驶区域的边界直线，即确定了车体定位参数中左、右车道线lL、lR。直线与图像右边界存在交点，即可判断出弯道转向。同时，拟合确定的左车道线与图像下边缘的交点Pl0为第一个控制点；以图像右边界与拟合直线的交点处为第二个控制点Pln。图5表示为前景环境中对特征点的匹配结果图和判断可行驶区域。

对序列图像进行实时处理时，由于摄像机采集系统速度为30帧/s，车速在不超过33cm/s的情况下，采集一帧图像智能车向前行驶约小于1cm，连续采集的两帧图像中所判断的车道左右车道线和弯道方向偏差不会太大。

2.2.2 控制仿真实验

实验采用BP神经网络作为控制对偏离角φ进行仿真实验。在Simulink环境下搭建的系统模型中BP神经网络的搭建如图6所示。U为经由视觉信息得到的偏航角φ作为系统输入量，输出Y为小车质心速度V。小车左、右轮速可在判断弯道转向的前提下，由两轮速差VD经计算得出。若如图5所示，弯道转向向右：

利用Simulink库中Signal Builder产生变化的Signal仿真实际环境中视觉系统得到的当前车体的偏移量。

小车经左右轮差速转弯运动中质心位置实际偏转角度作为网络的输入，经训练学习反馈回系统的控制输入端。图7为控制仿真结果图，由图示可知该方法基本完成了对信号的控制跟踪，由此说明了利用双目立体视觉所获得的信息，并采用BP网络自学习对道路的变化可适用于不同弯道的道路，避免了传统PID控制方法因弯道曲率变化使得小车转弯控制失败。

3 结论

借助机器视觉系统判断小车可行驶区域，同时还可清晰地判断出弯曲线路的走向，避免了弯道方向的复杂判别方法，且该方法普遍适用于不同的道路环境，特别是非结构化的道路环境，也增强了算法的实用性和鲁棒性。但目前对双目摄像机进行精确匹配和标定仍然是个技术难题；神经网络训练方法在选取训练样本数据和精确定位车辆位置方面也比较困难，这将是进一步研究的主要关键问题。