智能小车视觉导航方法
技术领域
本发明涉及一种智能小车，尤其涉及一种智能小车视觉导航方法，属于机器 人技术领域。
背景技术
工厂常使用小车进行工件、货物的搬运，现有技术的搬运小车通常通过红外 线、蓝牙等遥控器进行无线控制，实现小车的自动化工作。通过无线遥控对小车 的控制并未完全把用户解放出来，在小车运行过程中，用户仍需要实时通过遥控 器对小车下达指令。为了进一步实现小车运动的智能化，需要借助各类传感装置 使小车对当前路况和运动状态具有识别和判断功能，并进行自我决策，从而可以 使小车实现完全无人操作，并在工作过程中按规定路线运行，或者根据任务目标， 对环境识别后自行规划合理路线，完成智能化运行。随着运动机器人(智能小车) 的发展，其应用领域愈加广泛，目前运动机器人的导航方式主要有磁条导航，惯 性导航，激光导航等。磁条导航需要在机器人移动路径上铺设磁条，精度较低， 且突出地面的磁条易损坏；惯性导航随着时间的累积，惯性导航累积误差增大， 需辅助其他设备予以校正，且高精度的惯性导航器件成本较高；激光导航需要在 移动路径两边添加反光板，对反光板的安装精度要求较高，且对其他光源敏感， 不易于室外作业，成本较高。
视觉导航是目前移动机器人领域中的热点，基于前景图像纹理的视觉导航方 法，可以解决上述背景技术中提出的技术问题。基于前景图像纹理的视觉导航方 法不需对地面进行额外处理，适用范围广且方便，辅以地面的图片来标定，间歇 性校正累计误差，以达到高精度定位。
在智能小车运行过程中，有多个关键技术影响着运行状态，其中路径规划决 定着智能小车能否有效避开障碍物，规划出一条安全无碰的运行路径，从而高效 安全地实现智能小车工作任务。对前景视场进行三维重建则是路径规划的基础与 前提条件，为复杂环境下的路径规划提供先验知识，为实时在线规划提供环境信 息。智能小车控制系统采用光视觉导航，以双目摄像机作为传感器，小车前景路 径出现在双目摄像机的视场中，此时进行三维重建能够辅助双目视觉导航，使得 导航定位更加精确，提高智能小车运行准确率和避障性能。
小车前景视场三维重建主要分为图像采集预处理、立体匹配以及三维重建等 步骤，其中立体匹配是实现有效三维重建的关键技术。
由于基于双目摄像机在天气不佳、光线暗弱的环境下拍摄的图像存在照度低， 纹理弱等特点，现有立体匹配算法存在以下缺点：在低纹理区域不足以区分模糊 像素点，导致错误匹配；对于图像纹理不够充分而且存在噪声的情况下，低纹理 的待匹配点在正确的匹配上不能捕捉到正确纹理区域就容易导致错误的匹配结 果，导致重建点结果不平滑及边界不清晰。
发明内容
本发明的目的在于提供一种智能小车视觉导航方法，基于最小生成树结构的 立体匹配算法解决图像纹理弱，匹配精度不高以及匹配速度慢的问题，提供路径 规划所需要的环境信息，从而实现路径三维精确重建，为后续的路径规划与避障 提供必要的先验知识。
本发明的目的通过以下技术予以实现：
一种智能小车视觉导航方法，智能小车包括电源模块4、控制器5、车身6、 驱动轮7，导向轮8，舵机9、导航定位装置11、避障装置12，所述车身6底部 安装四个车轮，两个驱动轮7安装于后部，用于驱动小车行驶，两个导向轮8 安装于前部，用于转向；所述电源模块4为锂电池，用于提供电能；所述导航定 位装置11包括2个摄像机1和2个舵机9，所述舵机9布置于车身6前部，所 述摄像机1置于舵机9上，舵机9带动摄像机1旋转；所述避障装置12包括激 光测距仪2、超声波传感器3，所述激光测距仪2、超声波传感器3、摄像机1 将采集的信号输送至控制器5，所述控制器5对舵机9进行控制；当超声波传感 器检测到有障碍物时，小车停止运动，激光测距仪开始工作，在Δt的时间内， 发出两束激光进行测距，测得障碍物距离分别为L1和L2，则通过计算L1-L2的 值，即可判断障碍物是处于静止、朝向小车运动或是背离小车运动状态；通过公 式(L1-L2)/Δt计算出障碍物的运行速度，以此作为小车运动控制的依据，即当障 碍物靠近至一定距离L时，小车后退，避免碰撞；反之，则继续前进；小车避障 及转向行驶通过驱动轮的差速控制来实现，在行驶过程中，当障碍物出现在视野范围时，通过提取摄像机所拍摄的图像特征，判断出障碍物的位置信息并将其传 递至控制器，控制器输出控制信号，使靠近障碍一侧的驱动轮转速大于另一侧的 驱动轮转速，使小车转向进行避障，转过一定角度后，视野范围内检测不到障碍 物，控制器改变控制信号，使两驱动轮同步转动，小车恢复直线行驶状态；
前景视场分别在左右摄像机上成像，形成左图和右图，前景视场三维重建包 括以下步骤：
一、采集左右摄像机图像，对摄像机进行标定，得到摄像机内参数矩阵，畸 变系数矩阵，本征矩阵，基础矩阵，旋转矩阵以及平移矩阵；
二、对采集的图像，首先采用CENSUS变换区分水下图像对应的空气中图像 的弱纹理区域实现匹配代价计算，再采用基于最小生成树的立体匹配算法进行代 价聚合，接着采用WTA(WINNER-TAKES-ALL)算法计算得到代价最小的视差值，最 后采用亚像素精化对视差进行平滑优化，实现最终视差图的输出；
1.采用CENSUS变换区分图像的弱纹理区域实现匹配代价计算：
①将得到的图像左图转换为对应的灰度图，在左灰度图中任取一点，以该点 为中心取一个n×n个像素大小的窗口，将窗口中除中心点外的每一个点与窗口 的中心点进行比较，灰度值大于中心点像素灰度值即记为0，否则为1，从而得 到一个长度为8的二进制序列，称为CENSUS变换值，窗口内中心点像素的灰度 值则被CENSUS变换值替换：
CENSUS变换公式如下：
其中，p为n×n个像素大小窗口中的中心点，q为窗口内除中心点之外的 其余点，Np为以p为中心的n×n个像素大小窗口，B(p)表示经CENSUS变换后 得到的二进制序列，即为CENSUS变换值，ζ表示将窗口中除中心点外的每一个 点与窗口的中心点进行比较所得的结果按位连接，I(p)表示中心点像素灰度值， I(q)表示窗口内除中心像素点外其余点像素灰度值；
②移动窗口，遍历整个得到的空气中图像左图所对应灰度图中的像素点，完 成对左图的CENSUS变换；
③将得到的空气中图像右图转换为对应的灰度图，在右灰度图中任取一点， 以该点为中心取一个n×n个像素大小的窗口，将窗口中除中心点外的每一个点 与窗口的中心点进行比较，灰度值大于中心点像素灰度值即记为0，否则为1， 从而得到一个长度为8的二进制序列，称为CENSUS变换值，窗口内中心点像素 的灰度值则被CENSUS变换值替换：
CENSUS变换公式如下：
其中，p为n×n个像素大小窗口中的中心点，q为窗口内除中心点之外的 其余点，Np为以p为中心的n×n个像素大小窗口，B(p)表示经CENSUS变换后 得到的二进制序列，即为CENSUS变换值，ζ表示将窗口中除中心点外的每一个 点与窗口的中心点进行比较所得的结果按位连接，I(p)表示中心点像素灰度值， I(q)表示窗口内除中心像素点外其余点像素灰度值；
④移动窗口，遍历整个得到的空气中图像右图所对应灰度图中的像素点，完 成对右图的CENSUS变换；
⑤经过CENSUS变换后的图像使用汉明距离计算得到的左右两幅图像的相似 度，当左右两幅图像中两点视差为d时计算两点的汉明距离：将此两点的CENSUS 变换值逐位进行异或运算，然后计算结果为1的个数，即为此两点之间的汉明距 离，汉明距离计算公式为：
HH(p，d)＝Hamming(CCTL(p),CCTR(p，d)) (5)
其中，CCTL、CCTR分别为左右灰度图像经CENSUS变换后的图像，CCTL(p) 表示左灰度图中任意一点p，CCTR(p，d)表示右灰度图中与点p视差为d的点， Ham ming(CCTL(p),CCTR(p，d))表示将点CCTL(p)与点CCTR(p，d)的CENSUS 变换值逐位进行异或运算并计算结果为1的个数，HH(p，d)表示左右图中视 差为d的两点之间的汉明距离；
所以CENSUS变换的匹配代价计算模型为：
Cd(p，d)＝1-exp(-λHH(p，d)) (6)
其中，Cd(p，d)为在视差为d时点p的匹配代价，HH(p，d)为左右图中 视差为d的两点之间的汉明距离,λ为归一化常数，取为0.1；
2.采用基于最小生成树的立体匹配算法进行代价聚合：
①首先将图像的左图RGB图像表示成连通的无向图G(V,E)，其中V表示 图像中所有像素点，E表示连接相邻两像素点的边，边的权值即为相邻两像素 点的相似度测度。连接相邻两像素点e和k的边的权值采用三通道颜色及梯度 特征计算：
其中k是e的相邻点，Ii(e)，Ii(k)分别表示点e和点k的i通道值， i∈{R,G,B}，R表示红颜色通道，G表示绿颜色通道，B表示蓝颜色通道，
②设得到的图像的左图RGB图中有n个像素点，记为n个节点，最初先构 造一个只有n个节点，没有边的非连通图T＝{V,空}，图中每个节点自成一个 连通分量，将连接相邻两节点的所有边按照权值的大小进行升序排序；
③按照权值从小到大的顺序每次选择一条边，若该边的两个节点落在不同的 连通分量上，则将此边加入到最小生成树中；否则，则将此边舍去并且此后再也 不选用这条边；
④重复步骤③，直到具有n个节点的连通图G(V,E)筛选出n-1条边为止， 筛选出来的边和所有的节点构成此RGB图的最小生成树；
⑤将最小生成树中点u和点v两个节点之间的连接边的最小权重之和记为 D(u,v)，则两点之间的相似度s(u,v)为：
其中α为调节像素相似度的常数，设为0.1；
⑥根据最小生成树的树结构可得任意一点u在视差为d时的代价聚合值
其中s(u,v)表示最小生成树中点u和点v的相似度，Cd(u，d)表示在视差为 d时点u的匹配代价，v遍历图中除点u外的每个像素；
3.采用WTA(WINNER-TAKES-ALL)算法计算得到代价最小的视差值d(u)，表达 式如下：
其中，
4.采用亚像素精化对视差进行平滑优化：
①任意选取WTA算法结果中的一个值，记为d0，选择d_＝d0-1，d+＝d0+1， 其对应的代价聚合值已知；
②选取二次多项式如式(9)所示，根据d0、d_、d+f(d0)、f(d_)和f(d+) 计算出二次函数的参数a、b和c；
f(x)＝ax2+bx+c (11)
③根据公式(10)计算出二次函数值最小时对应的xmin值，为二次函数f(x) 的最小视差，即为亚像素值；
三、根据步骤一标定的参数进行三维重建。
本发明的目的还可以通过以下技术措施来进一步实现：
前述智能小车视觉导航方法，其中舵机9的型号为MG995。
前述智能小车视觉导航方法，其中激光测距仪2的型号为KLH-01T-20hz； 超声波传感器3的型号为HC-SR04。
前述智能小车视觉导航方法，其中控制器5是型号为LPC2148的单片机。
前述智能小车视觉导航方法，步骤三为；根据步骤一标定的参数运用点云库(Point Cloud Library，PCL)进行三维重建：
(1)先初始化一个深度图和一个PointCloud<PointT>类型的点云，用于存 储图像和点云。
(2)遍历深度图中的像素坐标，得到深度图像素区域内的单通道深度值。
(3)使用摄像机标定得到的内外部参数，计算出三维坐标，得到3D的PCL 点云点坐标。
(4)将原始图像中的每个像素点的RGB信息提取出，赋值给PCL点云中 的RGB颜色通道。
与现有技术相比，本发明的有益效果是：
(1)最小生成树可以在全图建立像素间的关系，使得像素间的关系一目了 然，不再受到窗口的约束，可以极大减少代价聚合所花费的时间，使得立体匹配 精度得到提高。
(2)本发明解决因传统局部立体匹配算法只考虑窗口邻域内像素值而忽略 其他像素对中心像素的影响而导致的误匹配率高的问题，同时相较于全局立体匹 配算法，改进的基于最小生成树的立体匹配算法，不需要进行迭代求精，能够大 幅度提高代价聚合的速度，所以能够在保证立体匹配算法准确度的同时保证算法 的实时性。
(3)光线暗弱情况下前景图像属于弱纹理区域，传统匹配算法对弱纹理区 域的效果差、误匹配率高，而CENSUS变换算法对光照、干扰具有较好的鲁棒性， 且保留了窗口中像素的位置特征，将CENSUS变化与树结构相结合，解决传统立 体匹配算法对弱纹理区域匹配误差率高的问题。
附图说明
图1是本发明的智能小车结构示意图；
图2是本发明的智能小车视觉导航方法流程图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明。
如图1所示，智能小车包括电源模块4、控制器5、车身6、驱动轮7，导 向轮8，舵机9、导航定位装置11、避障装置12，所述车身6底部安装四个车轮， 两个驱动轮7安装于后部，用于驱动小车行驶，两个导向轮8安装于前部，用于 转向；所述电源模块4为锂电池，用于提供电能；所述导航定位装置11包括2 个摄像机1和2个舵机9，所述舵机9布置于车身6前部，所述摄像机1置于舵 机9上，舵机9带动摄像机1旋转；所述避障装置12包括激光测距仪2、超声波传感器3，所述激光测距仪2、超声波传感器3、摄像机1将采集的信号输送 至控制器5，所述控制器5对舵机9进行控制。舵机9的型号为MG995，激光测 距仪2的型号为KLH-01T-20hz，超声波传感器3的型号为HC-SR04，控制器5 是型号为LPC2148的单片机。摄像机用于摄像识别当前周边路况，通过舵机固定 在车身上，通过控制舵机运动，即可实现对小车周边360°工况信息采集。
车身作为小车各部件的安装平台，同时可以提供承载货物的平台。两个驱动 轮置于车身底部的后端，用于驱动小车执行前进、后退及转向运动。两个导向轮 置于车身底部的前端，与车身通过旋转副相连，可绕旋转轴进行自由转动，用于 小车运动导向。当小车停止运动时，制动器固定住车轮，避免在外力干扰下或是 坡道停车时发生溜车现象。
激光测距仪2、超声波传感器3、摄像机1与控制器5相连，将采集的信号 输送至控制器5，超声波传感器用于障碍物探测，激光传感器用于对障碍物进行 测距。所述控制器为单片机，单片机接收来自导航定位装置、避障装置以及检测 到的环境信息以及反馈回的小车运动状态，对其判断处理后输出驱动信息，控制 小车的运动状态。
如图2所示，智能小车视觉导航方法包括以下步骤：
一、采集左右摄像机图像，对摄像机运用张氏平面标定法进行标定，得到摄 像机内参数矩阵，畸变系数矩阵，本征矩阵，基础矩阵，旋转矩阵以及平移矩阵， 为最后三维重建提供参数；
二、对图像采用改进的基于最小生成树结构的立体匹配方法，首先采用 CENSUS变换区分图像的弱纹理区域实现匹配代价计算，再采用基于最小生成树 的立体匹配算法进行代价聚合，接着采用WTA(WINNER-TAKES-ALL)算法计算得到 代价最小的视差值，最后采用亚像素精化对视差进行平滑优化，实现最终视差图 的输出；
(1)将图像左图转换为对应的灰度图，在左灰度图中任取一点，以该点为 中心取一个3×3个(或5×5)像素大小的窗口，将窗口中除中心点外的每一个 点与窗口的中心点进行比较，灰度值大于中心点像素灰度值即记为0，否则为1， 从而得到一个长度为8的二进制序列，称为CENSUS变换值，窗口内中心点像素 的灰度值则被CENSUS变换值替换：
CENSUS变换公式如下：
其中，p为3×3个像素大小窗口中的中心点，q为窗口内除中心点之外的 其余点，Np为以p为中心的3×3个像素大小窗口，B(p)表示经CENSUS变换后得 到的二进制序列，即为CENSUS变换值，ζ表示将窗口中除中心点外的每一个点 与窗口的中心点进行比较所得的结果按位连接，I(p)表示中心点像素灰度值， I(q)表示窗口内除中心像素点外其余点像素灰度值。
(2)移动窗口，遍历图像左图所对应灰度图中的像素点，完成对左图的 CENSUS变换。
(3)将图像右图转换为对应的灰度图，在右灰度图中任取一点，以该点为 中心取一个3×3个像素大小的窗口，将窗口中除中心点外的每一个点与窗口的 中心点进行比较，灰度值大于中心点像素灰度值即记为0，否则为1，从而得到 一个长度为8的二进制序列，称为CENSUS变换值，窗口内中心点像素的灰度值 则被CENSUS变换值替换：
CENSUS变换公式如下：
其中，p为3×3个像素大小窗口中的中心点，q为窗口内除中心点之外的 其余点，Np为以p为中心的3×3个像素大小窗口，B(p)表示经CENSUS变换后得 到的二进制序列，即为CENSUS变换值，ζ表示将窗口中除中心点外的每一个点 与窗口的中心点进行比较所得的结果按位连接，I(p)表示中心点像素灰度值， I(q)表示窗口内除中心像素点外其余点像素灰度值。
(4)移动窗口，遍历整个图像右图所对应灰度图中的像素点，完成对右图 的CENSUS变换。
(5)经过CENSUS变换后的图像使用汉明距离计算得到的左右两幅图像的相 似度。当左右两幅图像中两点视差为d时计算两点的汉明距离：将此两点的 CENSUS变换值逐位进行异或运算，然后计算结果为1的个数，即为此两点之间 的汉明距离。汉明距离计算公式为：
HH(p，d)＝Hamming(CCTL(p),CCTR(p，d)) (17)
其中，CCTL、CCTR分别左右灰度图像经CENSUS变换后的图像，CCTL(p)表 示左灰度图中任意一点p，CCTR(p，d)表示右灰度图中与点p视差为d的点， Hamming(CCTL(p),CCTR(p，d))表示将点CCTL(p)与点CCTR(p，d)的CENSUS 变换值逐位进行异或运算并计算结果为1的个数，HH(p，d)表示左右图中视 差为d的两点之间的汉明距离。
所以CENSUS变换的匹配代价计算模型为：
Cd(p，d)＝1-exp(-λHH(p，d))(18)
其中，Cd(p，d)为在视差为d时点p的匹配代价，HH(p，d)左右图中视 差为d的两点之间的汉明距离,λ为归一化常数，取为0.1。
(6)将图像的左图RGB图像表示成连通的无向图G(V,E)，其中V表示图 像中所有像素点，E表示连接相邻两像素点的边，边的权值即为相邻两像素点 的相似度测度。连接相邻两像素点e和k的边的权值采用三通道颜色及梯度特 征计算：
其中k是e的相邻点，Ii(e)，Ii(k)分别表示点e和点k的i通道值， i∈{R,G,B}，R表示红颜色通道，G表示绿颜色通道，B表示蓝颜色通道，
(7)设得到的图像的左图RGB图中有n个像素点，记为n个节点，最初先 构造一个只有n个节点，没有边的非连通图T＝{V,空}，图中每个节点自成一 个连通分量，将连接相邻两节点的所有边按照权值的大小进行升序排序。
(8)按照权值从小到大的顺序每次选择一条边，若该边的两个节点落在不 同的连通分量上，则将此边加入到最小生成树中；否则，则将此边舍去并且此后 再也不选用这条边。
(9)重复步骤(8)，直到具有n个节点的连通图G(V,E)筛选出n-1条边 为止，筛选出来的边和所有的节点构成此RGB图的最小生成树。
(10)将最小生成树中点u和点v两个节点之间的连接边的最小权重之和记 为D(u,v)，则两点之间的相似度s(u,v)为：
其中α为调节像素相似度的常数，设为0.1。
(11)根据最小生成树的树结构可得任意一点u在视差为d时的代价聚合 值
其中s(u,v)表示最小生成树中点u和点v的相似度，Cd(u，d)表示在视差为 d时点u的匹配代价，v遍历图中除点u外的每个像素。
(12)采用WTA(WINNER-TAKES-ALL)算法计算得到代价最小的视差值，采用 优胜者全选方式，选取对应匹配代价最小的视差作为最终的视差d(u)，表达式如 下：
其中，
(13)任意选取WTA算法结果中的一个值，记为d0，选择d_＝d0-1， d+＝d0+1，其对应的代价聚合值已知。
(14)选取二次多项式如式(23)所示，根据d0、d_、d+f(d0)、f(d_)和 f(d+)计算出二次函数的参数a、b和c。
f(x)＝ax2+bx+c(23)
(15)根据公式(24)计算出二次函数值最小时对应的xmin值，为二次函 数f(x)的最小视差，即为亚像素值。
三、根据步骤一标定的参数运用点云库(Point Cloud Library，PCL)进行三 维重建：
(1)先初始化一个深度图和一个PointCloud<PointT>类型的点云，用于存 储图像和点云。
(2)遍历深度图中的像素坐标，得到深度图像素区域内的单通道深度值。
(3)使用摄像机标定得到的内外部参数，计算出三维坐标，得到3D的PCL 点云点坐标。
(4)将原始图像中的每个像素点的RGB信息提取出，赋值给PCL点云中 的RGB颜色通道。
本发明基于最小生成树结构的立体匹配算法解决图像纹理弱，匹配精度不高 以及匹配速度慢的问题，提供路径规划所需要的环境信息，从而实现路径三维精 确重建，为后续的路径规划与避障提供必要的先验知识，取得良好的导航结果。
为了及时识别障碍物，以便及时避让，本发明还包括避障控制方法，当超声 波传感器检测到有障碍物时，小车停止运动，激光测距仪开始工作，在Δt的时 间内，发出两束激光进行测距，测得障碍物距离分别为L1和L2，则通过计算 L1-L2的值，即可判断障碍物是处于静止、朝向小车运动或是背离小车运动状态； 通过公式(L1-L2)/Δt计算出障碍物的运行速度，以此作为小车运动控制的依据， 即当障碍物靠近至一定距离L时，小车后退，避免碰撞；反之，则继续前进；
小车避障及转向行驶通过驱动轮的差速控制来实现，在行驶过程中，当障碍 物出现在视野范围时，通过提取相机所拍摄的图像特征，判断出障碍物的位置信 息并将其传递至控制器，控制器输出控制信号，使靠近障碍一侧的驱动轮转速大 于另一侧的驱动轮转速，使小车转向进行避障，转过一定角度后，视野范围内检 测不到障碍物，控制器改变控制信号，使两驱动轮同步转动，小车恢复直线行驶 状态。
除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变 换形成的技术方案，均落在本发明要求的保护范围内。
