CN104570147A

CN104570147A - 一种基于单目相机和主动结构光的障碍探测方法

Info

Publication number: CN104570147A
Application number: CN201410829101.XA
Authority: CN
Inventors: 毛晓艳; 邢琰; 刘祥; 滕宝毅; 贾永
Original assignee: Beijing Institute of Control Engineering
Current assignee: Beijing Institute of Control Engineering
Priority date: 2014-12-26
Filing date: 2014-12-26
Publication date: 2015-04-29
Anticipated expiration: 2034-12-26
Also published as: CN104570147B

Abstract

本发明公开了一种基于单目相机和主动结构光的障碍探测方法，实现步骤包括基准相机与主动结构光坐标标定、备份相机与基准相机坐标转换、激光图像处理、激光点三维恢复、障碍判断。本发明能够在阴影区域或者缺乏纹理特征区域进行环境感知；利用结构光的先验信息和单个相机配合进行快速三维恢复，计算量小，处理时间短，能够实现障碍物的快速识别和判断。

Description

一种基于单目相机和主动结构光的障碍探测方法

技术领域

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

背景技术

巡视探测器在行星工作阶段，需要控制巡视探测器在非结构复杂地面环境中实现长距离行驶，并安全到达预先指定的工作点，以完成特定的科学考察任务，并保障自身的安全和稳定工作。由于行星表面环境的不可预知性，巡视器必须具有环境感知的能力，在选择配备了立体相机作为环境感知的主要手段之后，考虑到行星表面阴影区完全无光照，巡视器进入阴影区或背对太阳时无法探测环境进行安全行走，所以考虑增加主动结构光，利用投射出的离散激光点进行阴影区内的障碍识别判断，保证行走安全。

针对相机和主动结构光的障碍探测方式，考虑冗余配置问题，设计了主动结构光分别和双目相机、左单目、右单目相机配合的三种工作方式。其中双目相机的方式类似立体视觉重建，而单目相机和主动结构光的配合使用，就需要用到结构光本身的信息，为此提出了激光点空间出射方程的高精度标定方法，在主动结构光加工完成后，在地面进行了离散激光出射线的标定，通过标定结果和相机拍摄的激光点图像实现了主动结构光和左相机或者右相机的激光点坐标解算，保证在任何一个相机失效的情况下，主动结构光探测仍能进行，提高了结构光探测的可靠性和安全性。

发明内容

本发明要解决的技术问题是：为克服现有技术的不足，提出一种基于单目相机和主动结构光的障碍探测方法，在基准相机失效的情况下，利用备份相机仍能实现障碍物的快速识别和判断。

本发明技术解决方案：

一种基于单目相机和主动结构光的障碍探测方法，具体步骤如下：

(1)基准相机与主动结构光坐标标定

在主动结构光的外部粘接基准镜作为外测基准建立基准镜下坐标系，同时将基准镜作为基准相机虚拟坐标系的外引基准，建立与主动结构光外测基准之间的关系，得到基准相机坐标系下主动结构光出射线的标定方程；假设主动结构光一共设计有N个激光出射点，标定得到的主动结构光的出射线方程在基准相机坐标系下的描述如下(A_0i,B_0i,C_0i)为直线上一点坐标，(p_i,q_i,r_i)为直线的矢量方向；用基准相机对激光点成像，得到基准图像；

(2)备份相机与基准相机坐标转换

可另设备份相机，备份相机到基准相机坐标系的转换矩阵为

R = [\begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix}],

平移矩阵为

T = [\begin{matrix} t_{x} \\ t_{y} \\ t_{z} \end{matrix}],

同时用备份相机对激光点成像，得到备份图像；

(3)激光图像处理

将步骤(1)及步骤(2)得到的基准图像和备份图像中灰度值大于灰度阈值的像素点分割出来，剔除伪激光点及由其他杂光在图像中引起的噪点；

根据从图像中提取出的激光点的能量中心，得到激光点能量中心在基准图像中的坐标和备份图像中的坐标，并确定图像中的激光点对应的编号；

(4)激光点三维恢复

结合相机参数，对步骤(3)中识别出的各激光点的坐标进行校正，消除其畸变误差，进行基准相机、备份相机与主动结构光的坐标解算，激光点在基准图像上三维坐标确定方法为：

A = [\begin{matrix} f_{1} & 0 & - x_{1 i} & 0 & 0 & 0 \\ 0 & f_{1} & y_{1 i} & 0 & 0 & 0 \\ 0 & 0 & 0 & q_{i} & - p_{i} & 0 \\ 0 & 0 & 0 & r_{i} & 0 & - p_{i} \\ x_{1 i} f_{1} & - y_{1 i} f_{1} & {f_{1}}^{2} & - x_{1 i} f_{1} & y_{1 i} f_{1} & - {f_{1}}^{2} \\ - p_{i} & - q_{i} & - r_{i} & p_{i} & q_{i} & r_{i} \end{matrix}]

其中f₁为基准相机焦距，(x1_i,y1_i)i＝1～N为基准图像中激光点坐标，

C = [\begin{matrix} 0 \\ 0 \\ q_{i} A_{0 i} - p_{i} B_{0 i} \\ r_{i} A_{0 i} - p_{i} C_{0 i} \\ 0 \\ 0 \end{matrix}]

M＝(A)^-1C

其中M＝(m_i)_6×1，(A)^-1表示对矩阵A进行逆运算

\{\begin{matrix} x_{p} (i) = (m_{1} + m_{4}) / 2000 \\ y_{p} (i) = (m_{2} + m_{5}) / 2000 \\ z_{p} (i) = (m_{3} + m_{6}) / 2000 \end{matrix}

得到激光点的三维坐标；

其中(x_p(i),y_p(i)_,z_p(i))i＝1～N为地面投射的主动结构光点在基准相机坐标系下的坐标；

激光点在基准图像上三维坐标确定方法为：

A = [\begin{matrix} r_{13} x_{2 i} + r_{11} f_{2} & - r_{23} x_{2 i} - r_{21} f_{2} & - r_{33} x_{2 i} - r_{31} f_{2} & 0 & 0 & 0 \\ r_{13} y_{2 i} + r_{12} f_{2} & - r_{23} y_{2 i} - r_{22} f_{2} & - r_{33} y_{2 i} - r_{32} f_{2} & 0 & 0 & 0 \\ 0 & 0 & 0 & q_{i} & - p_{i} & 0 \\ 0 & 0 & 0 & r_{i} & 0 & - p_{i} \\ - L_{1} & - L_{2} & - L_{3} & L_{1} & L_{2} & L_{3} \\ - p_{i} & - q_{i} & - r_{i} & p_{i} & q_{i} & r_{i} \end{matrix}]

其中：

f₂为备份相机焦距，(x2_i,y2_i)i＝1～N为备份图像中激光点的坐标，

L₁＝(r₂₃x_2i+r₂₁f₂)*(r₃₃y_2i+r₃₂f₂)-(r₃₃x_2i+r₃₁f₂)*(r₂₃y_2i+r₂₂f₂)

L₂＝-(r₃₃x_2i+r₃₁f₂)*(r₁₃y_2i+r₁₂f₂)+(r₁₃x_2i+r₁₁f₂)*(r₃₃y_2i+r₃₂f₂)

L₃＝-(r₁₃x_2i+r₁₁f₂)*(r₂₃y_2i+r₂₂f₂)+(r₂₃x_2i+r₂₁f₂)*(r₁₃y_2i+r₁₂f₂)

C = [\begin{matrix} r_{11} f_{2} t_{x} + r_{13} x_{2 i} t_{x} + r_{21} f_{2} t_{y} + r_{23} x_{2 i} t_{y} + r_{31} f_{2} t_{z} + r_{33} x_{2 i} t_{z} \\ r_{12} f_{2} t_{x} + r_{13} y_{2 i} t_{x} + r_{22} f_{2} t_{y} + r_{23} y_{2 i} t_{y} + r_{32} f_{2} t_{z} + r_{33} y_{2 i} t_{z} \\ q_{i} A_{0 i} - p_{i} B_{0 i} \\ r_{i} A_{0 i} - p_{i} C_{0 i} \\ 0 \\ 0 \end{matrix}]

M＝(A)^-1C

其中M＝(m_i)_6×1，(A)^-1表示对矩阵A进行逆运算

\{\begin{matrix} x_{p} (i) = (m_{1} + m_{4}) / 2000 \\ y_{p} (i) = (m_{2} + m_{5}) / 2000 \\ z_{p} (i) = (m_{3} + m_{6}) / 2000 \end{matrix}

得到激光点的三维坐标；

其中(x_p(i),y_p(i),z_p(i))i＝1～N为地面投射的主动结构光点在备份相机坐标系下的坐标。

(5)障碍判断

结合相机的安装及巡视器的姿态，得到由相机坐标系到水平坐标系的转换矩阵，然后进行平面拟合，得到各激光点到拟合平面的距离，进而判断激光投射区域是否存在障碍。

所述步骤(5)中相机坐标系的定义为：原点位于相机光心，Z轴沿相机的光轴向前，与图像平面垂直，X轴与Z轴垂直，指向相机系统的右侧，Y轴与X轴、Z轴满足右手定则。

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

(1)本发明能够在阴影区域或者缺乏纹理特征区域进行环境感知，利用结构光的先验信息和单个相机配合进行快速三维恢复，计算量小，处理时间短，能够实现障碍物的快速识别和判断；

(2)本发明基准相机及备份相机都可与主动结构光形成测量，作为补充备份，增加系统可靠性，可应用于巡视探测器或移动机器人的在线实时障碍探测；

(3)本发明提出了将主动结构光设备和相机测量的坐标系进行统一的标定方法，通过装订数据实现了结构光的直接坐标测量。

附图说明

图1为本发明的流程图；

图2为本发明的激光点构型示例图。

具体实施方式

下面结合附图及实施例对本发明进行详细说明。

以能够投射出图2所示激光点构型的激光点阵器为例，采用基准与备份两个相机(两相机放置方向可任意，本实施例采用左右放置方式)的测量方式说明本发明的具体实现过程，如图2所示，激光点阵器能够在巡视器前方投射出远近两排激光点，两排激光点的数量各为8个，第一排激光点的编号为1-8，第二排激光点的编号为9-16。

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

(1)基准相机与主动结构光坐标标定

(2)备份相机与基准相机坐标转换

可另设备份相机，其与基准相机在障碍探测上操作类似，与基准相机之间需要进行坐标转换，备份相机到基准相机坐标系的转换矩阵为

R = [\begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix}],

平移矩阵为

T = [\begin{matrix} t_{x} \\ t_{y} \\ t_{z} \end{matrix}],

同时用备份相机对激光点成像，得到备份图像；

参见专利《机器视觉中光学成像传感器安装方法》(授权号：ZL200710122908.X)，通过将光学成像传感器坐标系、V-STARS观测坐标系、经纬仪系统坐标系以及基准镜坐标系进行统一并引入相机标定参数的方法，将相机的坐标外引至基准镜上，然后利用经纬仪对基准相机的基准镜和主动结构光的基准镜进行瞄准，得到基准镜之间的平移和旋转关系，再根据相机到相机基准镜的关系换算得到主动结构光基准镜坐标系到相机坐标系的平移和旋转转换关系。同样，基准相机和备份相机的基准镜之间也进行瞄准，再根据基准相机到基准相机基准镜的关系和备份相机到备份相机基准镜的关系换算得到备份相机到基准相机的平移和旋转关系，基准相机坐标系和备份相机坐标系的转换关系如下：

(3)激光图像处理

对获得的基准图像和备份图像分别进行处理，设置灰度阈值T＝150，通过阈值分割的方法在图像上搜索激光点，将图像中灰度值大于灰度阈值150的像素点作为激光点，根据设定的激光点光斑面积范围阈值A_min＝10,A_max＝100，剔除光斑面积大于A_min、小于A_max的伪激光点；另外，根据激光点阵器与相机的相对位置关系，推算出激光点均分布在左右相机图像的下半部分，在进行激光点搜索时仅对基准图像和备份图像的下半部分进行搜索；

对于从基准图像和备份图像中提取出的激光点，采用质心法计算激光点的能量中心，得到激光点能量中心在基准图像和备份图像对中的像素坐标：

\{\begin{matrix} \overset{&OverBar;}{u_{i}} = \frac{\underset{(u, v) &Element; R}{Σ} I_{uv} \cdot u}{\underset{(u, v) &Element; R}{Σ} I_{uv}} \\ \overset{&OverBar;}{v_{i}} = \frac{\underset{(u, v) &Element; R}{Σ} I_{uv} \cdot v}{\underset{(u, v) &Element; R}{Σ} I_{uv}} \end{matrix}

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

根据激光点阵器所投射的激光点的构型如图2所示，对激光点进行归类排序，图像中上的点分为两排，第一排最左侧的为1号点，最右侧的为8号点。第二排最左侧的为9号点，最右侧的为16号点。通过排布得到左/右相机图像中的激光点对应编号。

(4)激光点三维恢复

A = [\begin{matrix} f_{1} & 0 & - x_{1 i} & 0 & 0 & 0 \\ 0 & f_{1} & y_{1 i} & 0 & 0 & 0 \\ 0 & 0 & 0 & q_{i} & - p_{i} & 0 \\ 0 & 0 & 0 & r_{i} & 0 & - p_{i} \\ x_{1 i} f_{1} & - y_{1 i} f_{1} & {f_{1}}^{2} & - x_{1 i} f_{1} & y_{1 i} f_{1} & - {f_{1}}^{2} \\ - p_{i} & - q_{i} & - r_{i} & p_{i} & q_{i} & r_{i} \end{matrix}]

C = [\begin{matrix} 0 \\ 0 \\ q_{i} A_{0 i} - p_{i} B_{0 i} \\ r_{i} A_{0 i} - p_{i} C_{0 i} \\ 0 \\ 0 \end{matrix}]

M＝(A)^-1C

其中M＝(m_i)_6×1，(A)^-1表示对矩阵A进行逆运算

\{\begin{matrix} x_{p} (i) = (m_{1} + m_{4}) / 2000 \\ y_{p} (i) = (m_{2} + m_{5}) / 2000 \\ z_{p} (i) = (m_{3} + m_{6}) / 2000 \end{matrix}

得到激光点的三维坐标；

其中(x_p(i),y_p(i),z_p(i))i＝1～N为地面投射的主动结构光点在基准相机坐标系下的坐标；

激光点在备份图像上三维坐标确定方法为：

A = [\begin{matrix} r_{13} x_{2 i} + r_{11} f_{2} & - r_{23} x_{2 i} - r_{21} f_{2} & - r_{33} x_{2 i} - r_{31} f_{2} & 0 & 0 & 0 \\ r_{13} y_{2 i} + r_{12} f_{2} & - r_{23} y_{2 i} - r_{22} f_{2} & - r_{33} y_{2 i} - r_{32} f_{2} & 0 & 0 & 0 \\ 0 & 0 & 0 & q_{i} & - p_{i} & 0 \\ 0 & 0 & 0 & r_{i} & 0 & - p_{i} \\ - L_{1} & - L_{2} & - L_{3} & L_{1} & L_{2} & L_{3} \\ - p_{i} & - q_{i} & - r_{i} & p_{i} & q_{i} & r_{i} \end{matrix}]

其中：

C = [\begin{matrix} r_{11} f_{2} t_{x} + r_{13} x_{2 i} t_{x} + r_{21} f_{2} t_{y} + r_{23} x_{2 i} t_{y} + r_{31} f_{2} t_{z} + r_{33} x_{2 i} t_{z} \\ r_{12} f_{2} t_{x} + r_{13} y_{2 i} t_{x} + r_{22} f_{2} t_{y} + r_{23} y_{2 i} t_{y} + r_{32} f_{2} t_{z} + r_{33} y_{2 i} t_{z} \\ q_{i} A_{0 i} - p_{i} B_{0 i} \\ r_{i} A_{0 i} - p_{i} C_{0 i} \\ 0 \\ 0 \end{matrix}]

M＝(A)^-1C

其中M＝(m_i)_6×1，(A)^-1表示对矩阵A进行逆运算

\{\begin{matrix} x_{p} (i) = (m_{1} + m_{4}) / 2000 \\ y_{p} (i) = (m_{2} + m_{5}) / 2000 \\ z_{p} (i) = (m_{3} + m_{6}) / 2000 \end{matrix}

得到激光点的三维坐标；

(5)障碍判断

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

{[x_{ri}, y_{ri}, z_{ri}]}^{T} = T_{c}^{r} {[x_{ci}, y_{ci}, z_{ci}]}^{T}, (i = 1,2, . . ., 16)

得到激光点在水平坐标系下的三维坐标(x_ri,y_ri,z_ri)(i＝1,2,...,16)，进行平面拟合其中(x_ci,y_ci,z_ci)(i＝1,2,...,16)为任意相机在相机坐标系下的三维坐标。

水平坐标系定义为：原点为相机原点在当前用来判断障碍的水平面的投影点，X轴指向东，Y轴指向下，Z轴指向北，三轴满足右手定则。

计算拟合平面的法向量与垂直向量的夹角θ_p；计算各激光点到拟合平面的距离，求得最小值Δh_min和最大值Δh_max，计算各激光点到拟合平面的高度差Δh_p＝|Δh_max-Δh_min|，根据巡视器的安全性能指标，设置地形坡度阈值P_safe和地形绝对高度阈值H_safe,根据激光点阵器的安装位置及激光点的构型，设置相对高度阈值H_rel,进行相对高度及绝对高度判断,进而可以判断激光点阵器的投射区域障碍的有无。

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

Claims

1.一种基于单目相机和主动结构光的障碍探测方法，其特征在于，具体步骤如下：

(1)基准相机与主动结构光坐标标定

(2)备份相机与基准相机坐标转换

可另设备份相机，备份相机到基准相机坐标系的转换矩阵为

R = [\begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix}],

平移矩阵为

T = [\begin{matrix} t_{x} \\ t_{y} \\ t_{z} \end{matrix}],

同时用备份相机对激光点成像，得到备份图像；

(3)激光图像处理

(4)激光点三维恢复

A = [\begin{matrix} f_{1} & 0 & - x_{1 i} & 0 & 0 & 0 \\ 0 & f_{1} & y_{1 i} & 0 & 0 & 0 \\ 0 & 0 & 0 & q_{i} & - p_{i} & 0 \\ 0 & 0 & 0 & r_{i} & 0 & - p_{i} \\ x_{1 i} f_{1} & - y_{1 i} f_{1} & {f_{1}}^{2} & - x_{1 i} f_{1} & y_{1 i} f_{1} & - {f_{1}}^{2} \\ - p_{i} & - q_{i} & - r_{i} & p_{i} & q_{i} & r_{i} \end{matrix}]

C = [\begin{matrix} 0 \\ 0 \\ q_{i} A_{0 i} - p_{i} B_{0 i} \\ r_{i} A_{0 i} - p_{i} C_{0 i} \\ 0 \\ 0 \end{matrix}]

M＝(A)^-1C

其中M＝(m_i)_6×1，(A)^-1表示对矩阵A进行逆运算

\{\begin{matrix} x_{p} (i) = (m_{1} + m_{4}) / 2000 \\ y_{p} (i) = (m_{2} + m_{5}) / 2000 \\ z_{p} (i) = (m_{3} + m_{6}) / 2000 \end{matrix}

得到激光点的三维坐标；

激光点在备份图像上三维坐标确定方法为：

A = [\begin{matrix} r_{13} x_{2 i} + r_{11} f_{2} & - r_{23} x_{2 i} - r_{21} f_{2} & - r_{33} x_{2 i} - r_{31} f_{2} & 0 & 0 & 0 \\ r_{13} y_{2 i} + r_{12} f_{2} & - f_{23} y_{2 i} - r_{22} f_{2} & - r_{33} y_{2 i} - r_{32} f_{2} & 0 & 0 & 0 \\ 0 & 0 & 0 & q_{i} & - p_{i} & 0 \\ 0 & 0 & 0 & r_{i} & 0 & - p_{i} \\ - L_{1} & {- L}_{2} & - L_{3} & L_{1} & L_{2} & L_{3} \\ - p_{i} & - q_{i} & - r_{i} & p_{i} & q_{i} & r_{i} \end{matrix}]

其中：

C = [\begin{matrix} r_{11} f_{2} t_{x} + r_{13} x_{2 i} t_{x} + r_{21} f_{2} t_{y} + r_{23} x_{2 i} t_{y} + r_{31} f_{2} t_{z} + r_{33} x_{2 i} t_{z} \\ r_{12} f_{2} t_{x} + r_{13} y_{2 i} t_{x} + r_{22} f_{2} t_{y} + r_{23} y_{2 i} t_{y} + r_{32} f_{2} t_{z} + r_{33} y_{2 i} t_{z} \\ q_{i} A_{0 i} - p_{i} B_{0 i} \\ r_{i} A_{0 i} - p_{i} C_{0 i} \\ 0 \\ 0 \end{matrix}]

M＝(A)^-1C

其中M＝(m_i)_6×1，(A)^-1表示对矩阵A进行逆运算

\{\begin{matrix} x_{p} (i) = (m_{1} + m_{4}) / 2000 \\ y_{p} (i) = (m_{2} + m_{5}) / 2000 \\ z_{p} (i) = (m_{3} + m_{6}) / 2000 \end{matrix}

得到激光点的三维坐标；

其中(x_p(i),y_p(i),z_p(i))i＝1～N为地面投射的主动结构光点在备份相机坐标系下的坐标；

(5)障碍判断

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