一种用于路面预瞄测距的融合Census-SAD算法

官锌强，孟 文，杨明亮，丁渭平

（1.西南交通大学机械工程学院，四川 成都 610031；2.先进驱动节能技术教育部工程研究中心，四川 成都 610031；3.轨道交通运维技术与装备四川省重点实验室，四川 成都 610031）

1 引言

减小车辆行驶过程中车身俯仰和侧倾的角度对提升汽车行驶安全性和舒适性有重要意义［1］。现阶段广泛应用的车身姿态控制技术是基于阻尼可调的半主动悬架轴间预瞄技术［2］或路面预瞄技术［3］。轴间预瞄技术利用前轮悬架受到的激励作为输入量预瞄后轮悬架，只能调整后悬架的阻尼［4］。路面预瞄技术多利用双目相机提前获取前方路面轮廓信息作为输入量预瞄前后悬架，即可调整前后悬架阻尼［5-6］。路面预瞄效果的关键在于测距算法的性能，而测距算法评价指标有三个：匹配准确率，算法实时性和测距准确度。

国外对于相关研究较为深入，例如利用基于半全局匹配的Dijkstra算法应用于“v-视差”来精确估算物体距离［7］；还有将路面表示为一般参数的B-样条曲线然后使用卡尔曼滤波器随时间跟踪路面参数［8］还原路面及物体信息，国内研究也在跟进，例如对路面进行逆透视变换，并将路面轮廓采取最小二乘法进行样条曲线参数化拟合［9］，亦能较好还原路面轮廓和物体的深度信息。

上述测距方法多过于侧重保证匹配精度，追求前方几乎所有路面的完全匹配，这样势必会产生较多无意义的计算时间，较大程度降低了算法的实时性；并且实验路面较为清晰且单一，未考虑到真实路面可能出现的破损或者减速带凸起以及光照差异较大等情况。针对上述原因，重点研究精度和实时性均较好的局部匹配算法，其中较为经典的算法主要有SAD算法［10］和Census变换算法［11］等，若单独使用上述两种匹配算法，则会因为各自缺点而导致适用场景较少。如果将SAD 算法与Census 算法改进融合，减少二者缺点对匹配结果的影响，则可大幅提升算法的鲁棒性。

2 基本原理

2.1 双目测距原理

双目测距主要是利用三角测量法［12］实现的，如图1所示。

图1 双目测距原理Fig.1 Principle of Binocular Ranging

令相机基线为B，点p到相机光心距离为Z，相机焦距为f，点p在左右成像面的对应点分别为p和p’，p与p’距离左右成像面的左边沿为xR和xT，且令视差为d，则有：

由图示关系可知，总有xR＞xT，故该约束可以一定程度上可以减少匹配算法的计算量。由上述已知量可求得深度Z为：

2.2 SAD原理

SAD（Sum of Absolute Differences，差的绝对值之和）原理，如图2所示。

图2 SAD算法原理Fig.2 Principle of SAD Algorithm

在右图像中选取一个点并以此为中心创建一个视差窗口D，然后在左图的相近位置创建一个相同大小的窗口，在左图的搜索范围内，创建的窗口的灰度值逐一与右图的窗口进行比较与计算。当结果最小时，即认为左图右图此时的两个窗口是匹配点对。计算公式如下：

式中：IL（x，y）—左图中像素点灰度值；IR（x-d，y）—右图中对应尾椎像素点灰度值。

2.3 Census变换原理

Census 变换本质是一种非参数的变换，并不关注像素具体灰度值，而是关注中心像素与周围像素的灰度值大小关系。通过比较窗口内中心点像素值与周围像素值的大小将窗口内的像素值转换成二进制比特串，变换规则为如果灰度值大于中心点则记为0，小于中心点记为1，变换公式如下：

式中：I（pi）—中心以外的像素点灰度值；I（p）—中心点像素灰度值。然后进行左右窗口的异或运算，结果中“1”的个数即为汉明距离，故汉明距离越小，则两点相似度越高。汉明距离C（u，v，d）表示如下：

式中：Ham—汉明规则；

Bl、Br—左右窗口像素形成的比特串。

3 算法改进及融合

3.1 SAD算法改进

由式（3）可知传统SAD 算法视差窗口内像素点的权重是相同的，但是在实际应用场景中，如果视差窗口较大，则越靠近窗口边缘的像素点跟中心点的相关性越小。若将视差窗口内的像素值的权重按正态分布重新分配，则可降低外围像素受干扰后对匹配结果的影响，像素权重分配公式如下：

式中：d—某像素距中心像素的距离；a—窗口半径。

在实际匹配过程中视差窗口大小一般在（5～21）左右，窗口半径为5和10时权重W与距离d的关系图，如图3（a）、图3（b）所示。

图3 窗口大小Fig.3 Window Size

故改进后的SAD计算公式如下：

式中：W（i）—该像素点的权重；P（x，y）—左右视差窗口内的像素灰度值。

3.2 Census变换改进

由前可知Census 变换在光照差异较大场景下匹配效果较好，但因其过于依赖中心像素点灰度值，若中心点像素受干扰则对结果影响较大。如果将边长大小为a的视差窗口以中心像素点为界分为左上，右上，左下，右下四个子窗口，并分别计算四个子窗口内的像素灰度均值σ，然后根据式（8）计算各子窗口的各像素点灰度值与平均值之差的绝对值之和：

式中：σ—子窗口内像素灰度均值；U—子窗口。然后取最小S值所在的子窗口的像素灰度均值替代原Census变换窗口的中心点像素灰度值I（u，v）：

式中：σ（Smin）—Smin所在的子窗口的像素灰度均值。然后得到新的比特串，并与另一幅图的比特串进行汉明距运算。

子窗口灰度均值替代法示例步骤如下，将（5*5）大小窗口分为左上，右上，左下，右下四个（3*3）大小的子窗口，如图4所示。

图4 子窗口灰度均值计算Fig.4 Calculation of Sub-Window Gray Mean

可以计算得到原始比特串为1110011100110001100011000。然后由式（8）计算可得右下子窗口的S值最小，为148，故取该子窗口的平均值σ=166替代大窗口原中心值136，则此时形成新的比特串为1111011110111101110011110，如果此时中心点像素值受到噪声干扰而改变，例如变成163，则未经改进Census变换计算的比特串数值会发生较大变化：1111011110110001110011110。如果采用上述改进Census变换，则计算可得受干扰后的窗口的中心像素值被替代为169，可以发现比特串仍为1111011110111101110011110，即在一定范围内保证了一定的抗干扰性。

3.3 改进算法融合

在光照差异场景下Census算法匹配效果较好，而在纹理丰富区域下SAD算法匹配效果较好，故将二者融合时需要考虑不同场景下二者算法的权重，并计算最终的相似度值。理想情况下汉明距离的取值范围为［0，a2］，而SSAD的取值范围则在［0，255*a2］。经验表明SCensus和SSAD数值差距较大，故分别将其除以一个经验值，使二者数值尽量相近，降低数值差距过大的影响。最后使用S型函数将其归一化。计算公式如下：

其中，S型函数表达式为：

式中：SCensus-SAD—融合Census-SAD 算法的匹配相似度值；SCensus—使用改进Census变换算法得出的汉明距离；SSAD—使用改进SAD算法得出的绝对值差的和；a—窗口边长大小。可以看到SCensus-SAD的取值范围在（0，1］区间内。

4 实验过程及结果评价

4.1 实验流程

实验流程如下，主要步骤为相机标定，图像对获取，校正、滤波等前处理，融合Census-SAD算法立体匹配并生成视差深度图，路面高度与物体深度数据获取，如图5所示。

图5 实验流程图Fig.5 Flow Chart of Experience

4.2 相机标定

相机标定［13-14］的作用是为了获取相机的内外参数，使点的坐标从世界坐标系转换到图像坐标系。使用MATLAB2017的Stereo-Camera Calibration Toolbox 对双目相机进行标定，得到的标定结果如下（保留五位数）：

左相机内参矩阵：

左相机畸变矩阵：

右相机内参矩阵：

右相机畸变矩阵：

4.3 实验结果对比分析

4.3.1 匹配率与实时性对比

实验硬件平台是基于一个双目相机和一台笔记本电脑，使用基于C/C++语言的OpenCV3图像处理库并在VS2017开发环境下进行实验。实验图像大小均为640*480。

本实验选取的场景为室外真实路面，主要选取四种典型路况：斜坡路面，减速带路面，路锥及破损路面。

实验左图已经灰度化处理，如图6（a）～图9（a）所示；传统SAD 匹配效果如图6（b）～图9（b）所示；改进后的融合Census-SAD算法（缩写为FC-SAD）匹配效果，如图6（c）～图9（c）所示。

图6 实验图Fig.6 Experimental Diagram

图7 实验图Fig.7 Experimental Diagram

图8 实验图Fig.8 Experimental Diagram

图9 实验图Fig.9 Experimental Diagram

需要说明的是视差图左边黑色区域是因为左右图像不完全重叠所致，匹配率将其计算在内，视差图中一条线框内依次表示斜坡轮廓，减速带，路锥，以及路面破损处的大致轮廓或位置。然后对比SAD算法与融合C-SAD算法的平均耗时与匹配率，如表1所示（保留四位有效数字）。

表1 算法匹配率与耗时Tab.1 Algorithm Matching Rate and Time Consumption

可以看到四种路况下改进后的融合Census-SAD 算法的匹配率比传统SAD算法大约高20%左右；但算法的实时性有所降低。一般情况下城市工况汽车行驶速度多数在60km/h，即16.66m/s以下，故最小预瞄距离大约为3.5m，而一般相机测距最远可达15m，显然改进后的算法仍然能够满足实时性要求。

4.3.2 测距优化

由4.3.1节可知融合Census-SAD算法的匹配率与实时性均能满足要求。测距结果对比实验可选取减速带和路锥两种物体上的相同位置点作为目标点，同时将相机摆放于不同位置来对验证测距精度。

不同位置下两种算法的测距精度对比，如表2所示（文中未表示出所有位置）。

表2 算法测距精度比较Tab.2 Comparison of Algorithms Ranging Accuracy

上表即为物体在（1.5～7）m距离时分别使用传统算法和改进算法测到的距离值，可以看到融合Census-SAD算法测距误差均小于SAD算法，且平均精度提升3.412%左右。

5 结论

针对传统SAD算法在匹配精度、测距精度均不能满足真实情况下路面预瞄的问题，提出一种改进的融合Census-SAD算法，经过真实道路场景实验对比可得出以下结论：

融合Census-SAD 算法匹配精度比传统SAD算法高20%左右，测距精度比传统SAD 算法高3%左右，实时性虽然较传统SAD算法有所下降但是仍能满足实际使用需求。

如何结合半主动悬架模型及控制策略进行预瞄效果的系统性研究是下一步工作的研究重点。