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

　　来区分正确匹配对和错误匹配对。
　　对于正确的匹配对，其最近邻距离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。

　　基于双目立体视觉获取得到的三维信息，即为经左右图像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网络自学习对道路的变化可适用于不同弯道的道路，