CN103884281B - 一种基于主动结构光的巡视器障碍探测方法 - Google Patents

Abstract

一种基于主动结构光的巡视器障碍探测方法，实现步骤包括环境感知、激光点提取、激光点快速匹配、激光点三维恢复、障碍判断。本发明能够在阴影区域或者缺乏纹理特征区域进行环境感知；利用结构光的先验信息进行快速匹配和三维恢复，计算量小，处理时间短，能够实现障碍物的快速识别和判断，可应用于巡视探测器或移动机器人的在线实时障碍探测。

Description

一种基于主动结构光的巡视器障碍探测方法

技术领域

本发明涉及一种障碍判断方法，适用于在月球、火星等外星球表面进行探测任务的巡视器的障碍识别，也可用于在野外恶劣环境进行作业的移动机器人的障碍识别。

背景技术

对于配备双目立体视觉系统进行探测任务的巡视器来说，一般情况下，巡视器利用视觉系统对其所处的环境采集图像，经过立体匹配处理获取环境的三维信息。但是，当探测目标处于阴影区域，或者探测区域缺乏足够的纹理特征时，相机无法对清晰成像，导致无法获取准确的三维信息，使得巡视器无法进行障碍识别，从而影响巡视器正常完成探测任务。

发明内容

本发明要解决的技术问题是：克服现有技术的不足，提出一种基于主动结构光的巡视器障碍探测的方法，该方法能够在阴影区域或者缺乏纹理特征区域进行环境感知，实现障碍物的快速识别和判断。

本发明技术解决方案：一种基于主动结构光的巡视器障碍探测方法，实现步骤如下：

(1)环境感知

在巡视器上安装能够投射出结构光的激光点阵器，根据巡视器所处的环境和激光点阵器的性能，设置相机积分时间，对巡视器的探测区域进行成像，获取含有一组激光点的左右相机图像对；对左右相机图像对进行分析，如果发现图像中激光点光斑不清晰，或者图像中有杂散光，则应该调整相机积分时间并重新成像，直到获取到激光点光斑清晰的图像；

（2）激光点提取

对步骤（1）中获得的左右相机图像对进行分析，设置灰度阈值T，将图像中灰度值大于灰度阈值T的像素点作为激光点，通过阈值分割的方法在图像上搜索激光点，根据设定的激光点光斑面积范围阈值[A_min,A_max]，剔除光斑面积大于A_min、小于A_max的伪激光点；另外，根据激光点阵器与相机的相对位置关系，推算出激光点在左右相机图像对中的分布范围，利用该先验知识，剔除由其他杂光在图像中引起的噪点；

对于从左右相机图像对中提取出的激光点，计算激光点的能量中心，得到激光点能量中心在左右相机图像对中的像素坐标，分别记为 $({\stackrel{\‾}{u}}_{\mathrm{Li}},{\stackrel{\‾}{v}}_{\mathrm{Li}})(i=\mathrm{1,2},...,N)$ 和 $({\stackrel{\‾}{u}}_{\mathrm{Ri}},{\stackrel{\‾}{v}}_{\mathrm{Ri}})(i=\mathrm{1,2},...,N),$ 其中，N为激光点阵器投射出的激光点的总个数；

（3）激光点快速匹配

根据激光点阵器所投射的激光点的构型，利用步骤（2）中得到的激光点像素坐标对激光点进行归类排序，得到激光点分别在左右相机图像对中对应的编号，从而完成激光点的快速匹配；

（4）激光点三维恢复

结合相机参数，对步骤（3）中匹配出的各激光点的像素坐标进行校正，并求得各激光点在左右相机图像中的视差值D_i(i=1,2,...,N)

根据立体视觉视差测距的原理，利用各激光点在左右相机图像中的视差值D_i(i=1,2,...,N)，计算各激光点在相机坐标系中的三维坐标(x_ci,y_ci,z_ci)，i=1,2,...,N；

结合相机的安装及巡视器的姿态，求得由相机坐标系到水平坐标系的转换矩阵对激光点三维坐标进行如下坐标转换：

${[x_{\mathrm{ri}},y_{\mathrm{ri}},z_{\mathrm{ri}}]}^T=T_c^r{[x_{\mathrm{ci}},y_{\mathrm{ci}},z_{\mathrm{ci}}]}^T---\left(1\right)$

从而得到激光点在水平坐标系下的三维坐标(x_ri,y_ri,z_ri)，i=1,2,...,N；

（5）障碍判断

利用步骤（4）得到的激光点在水平坐标系下的三维坐标(x_ri,y_ri,z_ri)进行平面拟合；

计算拟合平面的法向量与垂直向量的夹角θ_p，θ_p即表示激光点投射区域的地形坡度，计算各激光点到拟合平面的高度差Δh_p，Δh_p即表示激光点投射区域的高度落差；

根据巡视器的安全性能指标，设置地形坡度阈值P_safe和地形绝对高度阈值H_safe；根据激光点阵器的安装位置及激光点的构型，设置相对高度阈值H_rel；

采用如下流程对激光点投射区域及各激光点的投射位置进行障碍判断：

（51）进行相对高度判断：

若Δh_p≤H_rel，则执行步骤（52）；

否则，认为相对高度超限，执行步骤（53）；

（52）进行坡度判断：

若θ>P_safe，则认为坡度超限，激光点投射区域为斜坡障碍；

?否则，激光点投射区域为可安全通行区域；

（53）计算每个激光点相对于水平面的绝对高度h_ri=z_ri-H₀，z_ri为公式（1）中的激光点在水平坐标系下的Z坐标，H₀为巡视器质心相对于水平面的高度，对每个激光点进行如下判断：

若|h_ri|≤H_safe，则认为该激光点的投射位置安全；

否则，绝对高度超限，认为该激光点的投射位置为障碍。

所述步骤（4）中相机坐标系的定义为：原点位于左相机光心，Z_c轴沿左相机的光轴向前，与图像平面垂直，X_c轴与Z_c轴垂直，指向相机系统的右侧，Y_c轴与X_c轴、Z_c轴满足右手定则。

所述步骤（4）中水平坐标系的定义为：原点位于巡视器质心，X_r轴为巡视器前进方向在水平面上的投影，Y_r轴在水平面上与X_r轴垂直，指向巡视器的右侧，Z_r轴与X_r轴、Y_r轴满足右手定则。

所述步骤（4）中立体视觉视差测距的具体实现为：

如图2所示为双目立体视觉的成像原理示意图，图中的P_L和P_R为立体视觉系统相机对的左右两个成像平面。假设场景中的某一点p在左右图像中的像素坐标分别为(X_L,Y_L)和(X_R,Y_R)，由于两个成像平面在同一平面上，故Y_L=Y_R，则由三角几何关系有：

$\left\{\begin{array}{c}{X}_L=f\frac{x_c}{z_c}\\ X_R=f\frac{x_c-B}{z_c}\\ Y_L=Y_R=f\frac{y_c}{z_c}\end{array}\right.---\left(2\right)$

式中，f为相机的焦距，B为相机对的基线；

记点p在两个成像图像中的视差为D，根据视差的定义：D=X_L-X_R，则由公式（2）计算出点p在相机坐标系下的坐标(x_c,y_c,z_c)为：

$\left\{\begin{array}{c}{x}_c=\frac{B\·X_L}{D}\\ y_c=\frac{B\·Y_L}{D}\\ z_c=\frac{B\·f}{D}\end{array}\right.---\left(3\right).$

本发明与现有技术相比的优点在于：

（1）本发明提出一种双目立体视觉系统与主动光源相结合的障碍判断方法，能够在阴影区域或者缺乏纹理特征区域进行环境感知；

（2）本发明利用结构光的先验信息进行快速匹配和三维恢复，计算量小，处理时间短，能够实现障碍物的快速识别和判断，可应用于巡视探测器或移动机器人的在线实时障碍探测。

附图说明

图1为本发明的基于主动结构光的障碍探测流程图；

图2为本发明的立体视觉视差测距原理示意图；

图3为本发明的障碍判断流程图；

图4为本发明的激光点构型示例；

图5为本发明的激光点投射区域地形分析结果示意图。

具体实施方式

以能够投射出图4所示激光点构型的激光点阵器为例，说明本发明的具体实现过程。如图4所示，激光点阵器能够在巡视器前方投射出远近两排激光点，两排激光点的数量各为8个，第一排激光点的编号为1～8，第二排激光点的编号为9～16。

如图1所示，本发明的障碍探测方法的具体实现如下：

（1）环境感知

在巡视器上安装激光点阵器，根据巡视器所处的环境和激光点阵器的性能，设置相机积分时间20ms，对巡视器的探测区域进行成像，获取含有一组激光点的左右相机图像对；对左右相机图像对进行分析，如果发现图像中激光点光斑不清晰，或者图像中有杂散光，则应该调整相机积分时间并重新成像，直到获取到激光点光斑清晰的图像；

（2）激光点提取

对步骤（1）中获得的左右相机图像对进行分析，设置灰度阈值T=200，通过阈值分割的方法在图像上搜索激光点，将图像中灰度值大于灰度阈值200的像素点作为激光点，根据设定的激光点光斑面积范围阈值A_min=20,A_max=100，剔除光斑面积大于A_min、小于A_max的伪激光点；另外，根据激光点阵器与相机的相对位置关系，推算出激光点均分布在左右相机图像的下半部分，利用该先验知识，在进行激光点搜索时仅对左右相机图像的下半部分进行搜索；

对于从左右相机图像对中提取出的激光点，采用质心法计算激光点的能量中心，得到激光点能量中心在左右相机图像对中的像素坐标：

$\left\{\begin{array}{c}{\stackrel{\‾}{u}}_i=\frac{\underset{(u,v)\∈R}{\mathrm{\Σ}}{I}_{\mathrm{uv}}\·u}{\underset{(u,v)\∈R}{\mathrm{\Σ}}{I}_{\mathrm{uv}}}\\ {\stackrel{\‾}{v}}_i=\frac{\underset{(u,v)\∈R}{\mathrm{\Σ}}{I}_{\mathrm{uv}}\·v}{\underset{(u,v)\∈R}{\mathrm{\Σ}}{I}_{\mathrm{uv}}}\end{array}\right.$

上式中，R为激光点光斑所占图像区域，I_uv为激光点光斑中各像素点的灰度值，(u,v)为激光点光斑中各像素点的像素坐标；即为激光点能量中心在左右相机图像对中的像素坐标；

（3）激光点快速匹配

根据激光点阵器所投射的激光点的构型，利用步骤（2）中得到的激光点能量中心像素坐标对激光点进行归类排序，得到激光点分别在左右相机图像对中对应的编号，从而完成激光点的快速匹配；

匹配后的激光点在左右相机图像中的像素坐标和对应的编号分别如表1和表2所示；

表1左相机图像的激光点像素坐标及编号

表2右相机图像提取出的激光点像素坐标及编号

（4）激光点三维恢复

结合相机参数，对步骤（3）中匹配出的各激光点的像素坐标进行校正，并求得各激光点在左右相机图像中的视差值D_i(i=1,2,...,16)；

利用立体视觉视差测距的原理计算各激光点在相机坐标系中的三维坐标(x_ci,y_ci,z_ci)(i=1,2,...,16)；结合相机的安装及巡视器的姿态，求得由相机坐标系到水平坐标系的转换^矩阵对激光点三维坐标进行如下坐标转换：

[x_ri,y_ri,z_ri]^T=T_cr[x_ci,y_ci,z_ci]^T(i=1,2,...,16)

得到激光点在水平坐标系下的三维坐标(x_ri,y_ri,z_ri)(i=1,2,...,16)，如表3所示；

表3激光点在水平坐标系下的坐标

编号	xri(m)	yri(m)	zri(m)
				1	1.58963	-1.03441	0.35845
2	1.58862	-0.77287	0.34818
				3	1.58603	-0.51603	0.34769
4	1.56884	-0.25009	0.33428
				5	1.58579	0.00781	0.34536
6	1.57557	0.25624	0.34035
				7	1.57875	0.51776	0.34173
8	1.58170	0.77237	0.34553
				9	1.43372	-0.91346	0.35896
10	1.43295	-0.65878	0.36322
				11	1.40697	-0.38297	0.33725
12	1.41186	-0.12231	0.33882
				13	1.31993	0.11804	0.24028
14	1.41575	0.39028	0.34016
				15	1.42038	0.64925	0.34822
16	1.42289	0.90329	0.34765

（5）障碍判断

利用步骤（4）得到的激光点在水平坐标系下的三维坐标(x_ri,y_ri,z_ri)(i=1,2,...,16)进行平面拟合；

计算拟合平面的法向量与垂直向量的夹角θ_p=0.1304(rad)；计算各激光点到拟合平面的距离，求得最小值Δh_min和最大值Δh_max，计算各激光点到拟合平面的高度差：Δh_p=|Δh_max-Δh|_min=0.102(m)；θ_p即表示激光点投射区域的地形坡度，Δh_p即表示激光点投射区域的高度落差，如图5所示；

根据巡视器的安全性能指标，设置地形坡度阈值P_safe=0.35(rad)和地形绝对高度阈值H_safe=0.15(m)；根据激光点阵器的安装位置及激光点的构型，设置相对高度阈值H_rel=0.05(m)；

按照图3所示流程对激光点投射区域及各激光点的投射位置进行障碍判断：

（51）进行相对高度判断：由于Δh_p>H_rel，故相对高度超限；

（52）计算每个激光点相对于水平面的绝对高度h_ri=z_ri-H₀，H₀为巡视器质心相对于水平面的高度，取值为0.3(m)，激光点绝对高度计算结果如表4所示：

表4激光点绝对高度

编号	hri(m)
		1	0.05845
2	0.04818
		3	0.04769
4	0.03428
		5	0.04536
6	0.04035
		7	0.04173
8	0.04553
		9	0.05896
10	0.06322
		11	0.03725
12	0.03882
		13	-0.05972
14	0.04016
		15	0.04822
16	0.04765

由于每个激光点的绝对高度均满足：|h_ri|<H_safe，故认为激光点阵器的投射区域无障碍，巡视器可以安全通行。

本发明未详细阐述部分属于本领域公知技术。

Claims (4)

1.一种基于主动结构光的巡视器障碍探测方法，其特征在于实现步骤如下：

(1)环境感知

在巡视器上安装能够投射出结构光的激光点阵器，根据巡视器所处的环境和激光点阵器的性能，设置相机积分时间，对巡视器的探测区域进行成像，获取含有一组激光点的左右相机图像对；对左右相机图像对进行分析，如果发现图像中激光点光斑不清晰，或者图像中有杂散光，则应该调整相机积分时间并重新成像，直到获取到激光点光斑清晰的图像；

(2)激光点提取

对步骤(1)中获得的左右相机图像对进行分析，设置灰度阈值T，将图像中灰度值大于灰度阈值T的像素点作为激光点，通过阈值分割的方法在图像上搜索激光点，根据设定的激光点光斑面积范围阈值[A_min,A_max]，剔除光斑面积大于A_min、小于A_max的伪激光点；另外，根据激光点阵器与相机的相对位置关系，推算出激光点在左右相机图像对中的分布范围，利用该先验知识，剔除由其他杂光在图像中引起的噪点；

对于从左右相机图像对中提取出的激光点，计算激光点的能量中心，得到激光点能量中心在左右相机图像对中的像素坐标；

(3)激光点快速匹配

根据激光点阵器所投射的激光点的构型，利用步骤(2)中得到的激光点像素坐标对激光点进行归类排序，得到激光点分别在左右相机图像对中对应的编号，从而完成激光点的快速匹配；

(4)激光点三维恢复

结合相机参数，对步骤(3)中匹配出的各激光点的像素坐标进行校正，并求得各激光点在左右相机图像中的视差值D_i，式中，i＝1,2,...,N,N为激光点阵器投射出的激光点的总个数；

根据立体视觉视差测距的原理，利用各激光点在左右相机图像中的视差值D_i，计算各激光点在相机坐标系中的三维坐标(x_ci,y_ci,z_ci)，i＝1,2,...,N；

结合相机的安装及巡视器的姿态，求得由相机坐标系到水平坐标系的转换矩阵对激光点三维坐标进行如下坐标转换：

${\&lsqb;x_{ri},y_{ri},z_{ri}\&rsqb;}^T=T_c^r{\&lsqb;x_{ci},y_{ci},z_{ci}\&rsqb;}^T---\left(1\right)$

从而得到激光点在水平坐标系下的三维坐标(x_ri,y_ri,z_ri)，i＝1,2,...,N；

(5)障碍判断

利用步骤(4)得到的激光点在水平坐标系下的三维坐标(x_ri,y_ri,z_ri)进行平面拟合；

计算拟合平面的法向量与垂直向量的夹角θ_p，θ_p即表示激光点投射区域的地形坡度，计算各激光点到拟合平面的高度差Δh_p，Δh_p即表示激光点投射区域的高度落差；

根据巡视器的安全性能指标，设置地形坡度阈值P_safe和地形绝对高度阈值H_safe；根据激光点阵器的安装位置及激光点的构型，设置相对高度阈值H_rel；

采用如下流程对激光点投射区域及各激光点的投射位置进行障碍判断：

(51)进行相对高度判断：

若Δh_p≤H_rel，则执行步骤(52)；

否则，认为相对高度超限，执行步骤(53)；

(52)进行坡度判断：

若坡度θ>P_safe，则认为坡度超限，激光点投射区域为斜坡障碍；

否则，激光点投射区域为可安全通行区域；

(53)计算每个激光点相对于水平面的绝对高度h_ri＝z_ri-H₀，z_ri为公式(1)中的激光点在水平坐标系下的Z坐标，H₀为巡视器质心相对于水平面的高度，对每个激光点进行如下判断：

若|h_ri|≤H_safe，则认为该激光点的投射位置安全；

否则，绝对高度超限，认为该激光点的投射位置为障碍。

2.根据权利要求1所述的基于主动结构光的巡视器障碍探测方法，其特征在于：所述步骤(4)中相机坐标系的定义为：原点位于左相机光心，Z_c轴沿左相机的光轴向前，与图像平面垂直，X_c轴与Z_c轴垂直，指向相机系统的右侧，Y_c轴与X_c轴、Z_c轴满足右手定则。

3.根据权利要求1所述的基于主动结构光的巡视器障碍探测方法，其特征在于：所述步骤(4)中水平坐标系的定义为：原点位于巡视器质心，X_r轴为巡视器前进方向在水平面上的投影，Y_r轴在水平面上与X_r轴垂直，指向巡视器的右侧，Z_r轴与X_r轴、Y_r轴满足右手定则。

4.根据权利要求1所述的基于主动结构光的巡视器障碍探测方法，其特征在于：所述步骤(4)中立体视觉视差测距的具体实现为：

假设场景中的某一点p在左右图像中的像素坐标分别为(X_L,Y_L)和(X_R,Y_R)，由于两个成像平面在同一平面上，故Y_L＝Y_R，则由三角几何关系有：

$\left\{\begin{array}{c}{X}_L=f\frac{x_c}{z_c}\\ X_R=f\frac{x_c-B}{z_c}\\ Y_L=Y_R=f\frac{y_c}{z_c}\end{array}\right.---\left(2\right)$

式中，f为相机的焦距，B为相机对的基线；

记点p在两个成像图像中的视差为D，根据视差的定义：D＝X_L-X_R，则由公式(2)计算出点p在相机坐标系下的坐标(x_c,y_c,z_c)为：

$\left\{\begin{array}{c}{x}_c=\frac{B\&CenterDot;X_L}{D}\\ y_c=\frac{B\&CenterDot;Y_L}{D}\\ z_c=\frac{B\&CenterDot;f}{D}\end{array}\right.---\left(3\right).$