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

假定参考图像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。