CN109490926B - 一种基于双目相机和gnss的路径规划方法 - Google Patents

Abstract

本发明涉及图像处理及车辆导航领域，尤其涉及一种基于双目相机和GNSS的路径规划方法。其中，利用双目相机及GNSS实时获取路况信息和位置信息，结合地图信息与实时路径情况，利用像素评价机制将两者结合起来，实现当前位置与目标位置及当前路面信息的融合。

Description

一种基于双目相机和GNSS的路径规划方法

技术领域

本发明涉及图像处理及车辆导航领域，尤其涉及一种基于双目相机和GNSS的路径规划方法。

背景技术

随着计算机硬件计算能力的不断提升和图像处理技术的不断成熟，如今无人驾驶技术正逐渐成为一大热门产业，其中涉及到利用多种传感器的数据融合，从而引导车辆行驶的技术，一般涉及摄像头、激光雷达、毫米波雷达及GNSS系统等。其中利用双目相机，可以获取具有深度的图像，从而可以对前方障碍物情况及可通行状况进行判断；利用GNSS系统可以获得当前位置信息。

本发明基于机器视觉的通路检测技术及基于GNSS的路径规划技术，利用像素评价机制将两者结合起来，从而实现通路检测和路径规划的统一系统。

发明内容

本发明提供了一种基于双目相机和GNSS的路径规划方法，可应用于无人驾驶车辆。

本发明解决问题所提供的技术方案如下：一种基于双目相机和GNSS的路径规划方法，包括如下步骤：

(1)利用GNSS模块获取当前位置，根据目的地位置，规划初始路径；车辆行进中不断获取当前坐标(GNSS模块)、当前车辆方向及姿态(陀螺仪)，计算出当前方向与导航方向的偏差角度。

(2)双目相机采集前方场景，得到前方场景的彩色图像和深度图像，处理器根据彩色图像，利用区域生长的方法获得可通行区域，或者利用棒状像素检测法获得可通行区域，或者利用机器学习中的语义分割的方式学习得到可通行区域。

(3)障碍物检测，包括如下步骤：

根据相机参数，计算得到深度图像中的点[u,v]^T到世界坐标点[x_w,y_w,z_w]^T的变换公式:

u₀,v₀分别为深度图像的中心坐标。z_c表示相机坐标的z轴值，f为双目相机焦距；

利用该公式将深度图像转换为点云图，并获取图像中每一像素点的高度H，此时坐标系是以相机为基准的，对H进行判断，若H不在[-0.1-h～0.1-h]范围内，即视为障碍物；h为相机高度；

(4)对步骤2标记出可通行区域和步骤3标记出障碍物进行凸包检测，将凸点记为

m表示凸点的个数；将凹点记为

n表示凹点个数，对于可通行区域的第j行像素点有

分别代表当前行像素点的左右两端像素点，对于第j行中的像素点(x_ij,y_ij)(其中i和j分别代表行和列的标号)，利用以下计算公式求出T的评分：

其中α和β_p为评分参数。

利用以下公式求出O的评分：

其中γ_q为评分参数。

(5)对于步骤2获得的可通行区域以内的像素，设置一套评分体系，内容包括且不限于步骤1获得的偏差角度θ、距离可通行区域边缘的远近T、距离障碍物的远近O等。由以下公式可算出像素点(x_ij,y_ij)的总评分G：

G＝Aθ+BT+CO

其中A、B、C分别为θ、T、O的评分参数。

对于所有属于通行区域的像素进行评分得到像素的评分集合{G_ij}。

取每一行的所有像素取评分最高的像素

从而得到从远到近的最高评分像素合集{G_max}。将相邻行最高分像素点用平滑曲线相连，即可获得基于当前图像的最优路径规划。

进一步地，所述步骤2通过如下种子生长法获得可通行区域，包括如下步骤：

(2.1)双目相机采集前方场景，将采集得到的两个彩色图像传入处理器中处理获得深度图像；

(2.2)利用深度图像，使用种子生长算法，获得可通行的路径；具体实现如下：对于深度图像中所有的像素点(X,Y,Z)，随机选取3个点，构成以这3个点为顶角的一个三角形平面，平面方程为AX+BY+CZ+D＝0。如果

(n_th为阈值，取0.85～0.95)，说明该平面和水平面夹角较小，可作为地面的候选；否则跳出进行下一次随机取点。对可作为地面的候选平面，计算出深度图像中每一点(X_i,Y_i,Z_i)到该候选平面的距离

如果DPS_i＜T，(T为阈值，取20mm～100mm)，则将(X_i,Y_i,Z_i)归为该平面内点。取所有候选平面中内点最多的平面作为初始地面。

(2.3)对步骤2.1获得的彩色图像进行颜色空间变换，获取在HSV颜色空间中的彩色图HSV。对彩色图像提取Canny边缘，获取彩色图像的边缘图像Canny。

(2.4)对于初始地面中的像素点，其四邻域不全是地面的像素点设为种子点。将种子点都压入种子点堆栈进行四邻域生长。设定种子点可向四邻域生长的条件：①这个种子点没有遍历过；②这个种子点不属于边缘图像Canny的像素点。四邻域生长过程如下：

对种子点G，该点的色调值为h；它的其中一个四邻域像素点G_i，色调值为h_i；如G_i不属于边缘图像Canny的像素点，且G_i没有被遍历过，且|h-h_i|＜δ，其中δ是阈值(取2～5)，则G_i也视为可通行的地面，压入种子点堆栈。当遍历堆栈中所有种子点后，种子区域生长结束。初始地面经过种子区域生长，被扩展到了距离更长，范围更广的扩展地面，即为可通行区域。

进一步地，所述步骤3中，点云图的计算过程如下：

(3.1)世界坐标点M(Xw,Yw,Zw)映射到图像点m(u,v)的公式如下：

其中u,v为图像坐标系下的任意坐标点。u₀,v₀分别为图像的中心坐标。x_w,y_w,z_w表示世界坐标系下的三维坐标点。z_c表示相机坐标的z轴值，即目标到相机的距离。R,T分别为外参矩阵的3x3旋转矩阵和3x1平移矩阵。f为双目相机焦距；

由于世界坐标原点和相机原点是重合的，即没有旋转和平移，所以:

相机坐标系和世界坐标系的坐标原点重合，相机坐标和世界坐标下的同一个物体具有相同的深度，即z_c＝z_w，公式可进一步简化为：

从以上的变换矩阵公式，可以计算得到图像点[u,v]^T到世界坐标点[x_w,y_w,z_w]^T的变换公式:

进一步地，所述步骤4中α取值为1.0～1.5，β_p取值为0.3～0.7，γ_q取值为0.3～0.7。

进一步地，所述步骤5中，A的取值为1.5-2.5，B的取值为0.5-1.5，C的取值为0.5-1.5。

本发明的有益效果在于：

(1)利用双目相机及GNSS，实时获取路况信息和位置信息，结合地图信息与实时路径情况，实现当前位置与目标位置及当前路面信息的融合。

(2)精度较高，实现10米左右的通路检测。

(3)具有较好的实时性，可达到每秒10到15帧。

附图说明

图1为本发明结构示意图；

图2为本发明系统示意图；

图3为本发明系统流程图；

图4为前方场景的图像；

图5为前方场景的深度图像；

图6为障碍物检测结果图；

附图中标记及对应的零部件名称：1-外壳、2-双目相机、3-底盘、31-车前轮、32-车后轮、4-上位机、41-GNSS模块、42-陀螺仪、43-4G模块、44-下位机、5-电池、6-电机。

具体实施方式

为使本发明的目的、技术方案和优点更加清晰明了，下面结合实例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。

系统实例如图1所示，其包括GNSS模块、陀螺仪、网络通信模块、双目相机、电机模块、上位机、下位机、车轮。所述GNSS模块为GPS与北斗双模定位模块，与上位机相连，实时采集地理定位信息并发送至上位机；所述陀螺仪模块与上位机相连，将方向信息实时发送至上位机；所述网络通信模块为4G移动通信模块，与上位机相连，在有请求时进行网络通信；所述双目相机为双目相机，与上位机相连，进行对路况的实时采集并将获取的深度图像交由上位机处理；所述电机模块控制车轮前进后退及转向，与下位机相连；所述下位机模块可用Arduino开发板，与上位机相连，控制无人车的行驶；所述上位机为微型台式机，运行平台为Windows，负责对深度图像的实时处理、请求网络通信、实时获取GNSS数据和获取陀螺仪数据并分析处理；所述电池模块与上位机、下位机、电机相连，为其三者供电。

在车辆启动后，从上位机输入目的地址，上位机从GNSS模块中解析出当前地理坐标，将目的地和当前位置通过4G模块发送至云端服务器获取路径规划，而后开启通路检测算法，利用像素评价的方法计算出最佳可通行线路，借助电子罗盘传来的方向按照规划的路径向下位机传输行驶并避障的路线指引。

下面结合具体实例，对本发明作进一步说明。

一种基于双目相机和GNSS的路径规划方法，包括如下步骤：

(1)利用GNSS模块获取当前位置；根据目的地位置，规划初始路径；车辆行进中不断获取当前坐标，根据陀螺仪获取当前车辆方向及姿态，计算出当前方向与导航方向的偏差角度。

(2)双目相机采集前方场景，采集得到两个彩色图像，如图4所示，处理器根据彩色图像，利用区域生长的方法获得可通行区域，或者利用棒状像素检测法获得可通行区域，或者利用机器学习中的语义分割的方式学习得到可通行区域。本实施例中，采用如下所述的种子生长法来实现：

(2.1)双目相机采集前方场景，将采集得到的两个彩色图像传入处理器中处理获得深度图像，如图5所示；

(2.2)利用深度图像，使用种子生长算法，获得可通行的路径；具体实现如下：对于深度图像中所有的像素点(X,Y,Z)，随机选取3个点，构成以这3个点为顶角的一个三角形平面，平面方程为AX+BY+CZ+D＝0。如果

(n_th为阈值，取0.85～0.95)，说明该平面和水平面夹角较小，可作为地面的候选；否则跳出进行下一次随机取点。对可作为地面的候选平面，计算出深度图像中每一点(X_i,Y_i,Z_i)到该候选平面的距离

如果DPS_i＜T，(T为阈值，取20mm～100mm)，则将(X_i,Y_i,Z_i)归为该平面内点。取所有候选平面中内点最多的平面作为初始地面。

(2.3)对步骤2.1获得的彩色图像进行颜色空间变换，获取在HSV颜色空间中的彩色图HSV。对彩色图像提取Canny边缘，获取彩色图像的边缘图像Canny。

(2.4)对于初始地面中的像素点，其四邻域不全是地面的像素点设为种子点。将种子点都压入种子点堆栈进行四邻域生长。设定种子点可向四邻域生长的条件：①这个种子点没有遍历过；②这个种子点不属于边缘图像Canny的像素点。四邻域生长过程如下：

对种子点G，该点的色调值为h；它的其中一个四邻域像素点G_i，色调值为h_i；如G_i不属于边缘图像Canny的像素点，且G_i没有被遍历过，且|h-h_i|＜δ，其中δ是阈值(取2～5)，则G_i也视为可通行的地面，压入种子点堆栈。当遍历堆栈中所有种子点后，种子区域生长结束。初始地面经过种子区域生长，被扩展到了距离更长，范围更广的扩展地面，即为可通行区域。

(3)障碍物检测，包括如下步骤：

根据相机参数，计算得到图像点[u,v]^T到世界坐标点[x_w,y_w,z_w]^T的变换公式:

u₀,v₀分别为图像的中心坐标。z_c表示相机坐标的z轴值，f为双目相机焦距；

利用该公式将深度图像转换为点云图，并获取图像中每一像素点的高度H，此时坐标系是以相机为基准的，对H进行判断，若h不在[-0.1-h～0.1-h]范围内，即视为障碍物，如图6所示，分界线以上即为检测出的障碍物。

点云图的计算过程如下：

(3.1)世界坐标点M(Xw,Yw,Zw)映射到图像点m(u,v)的公式如下：

其中u,v为图像坐标系下的任意坐标点。u₀,v₀分别为图像的中心坐标。x_w,y_w,z_w表示世界坐标系下的三维坐标点。z_c表示相机坐标的z轴值，即目标到相机的距离。R,T分别为外参矩阵的3x3旋转矩阵和3x1平移矩阵。f为双目相机焦距；

由于世界坐标原点和相机原点是重合的，即没有旋转和平移，所以:

相机坐标系和世界坐标系的坐标原点重合，相机坐标和世界坐标下的同一个物体具有相同的深度，即z_c＝z_w，公式可进一步简化为：

从以上的变换矩阵公式，可以计算得到图像点[u,v]^T到世界坐标点[x_w,y_w,z_w]^T的变换公式:

(4)对步骤2标记出可通行区域和步骤3标记出障碍物进行凸包检测，将凸点记为

m表示凸点的个数；将凹点记为

n表示凹点个数，对于可通行区域的第j行像素点有

分别代表当前行像素点的左右两端像素点，对于第j行中的像素点(x_ij,y_ij)(其中i和j分别代表行和列的标号)，利用以下计算公式求出T的评分：

其中α和β_p为评分参数。本实施例中，α取值可以为1.0～1.5，β_p取值可以为0.3～0.7。

利用以下公式求出O的评分：

其中γ_q为评分参数，本实施例中，可以为0.3～0.7，

(5)对于步骤2获得的可通行区域以内的像素，设置一套评分体系，内容包括且不限于步骤1获得的偏差角度θ、距离可通行区域边缘的远近T、距离障碍物的远近O等。由以下公式可算出像素点(x_ij,y_ij)的总评分G：

G＝Aθ+BT+CO

其中A、B、C分别为θ、T、O的评分参数。本实施例中，A的取值可以为1.5-2.5，B的取值可以为0.5-1.5，C的取值可以为0.5-1.5

对于所有属于通行区域的像素进行评分得到像素的评分集合{G_ij}。

取每一行的所有像素取评分最高的像素

从而得到从远到近的最高评分像素合集{G_max}。将相邻行最高分像素点用平滑曲线相连，即可获得基于当前图像的最优路径规划。

Claims (1)

1.一种基于双目相机和GNSS的路径规划方法，其特征在于，包括如下步骤：

(1)利用GNSS模块获取当前位置，根据目的地位置，规划初始路径；车辆行进中不断获取当前坐标、当前车辆方向及姿态，计算出当前方向与导航方向的偏差角度；

(2)双目相机采集前方场景，得到前方场景的彩色图像和深度图像，处理器根据彩色图像，利用区域生长的方法获得可通行区域，包括如下步骤：

(2.1)双目相机采集前方场景，将采集得到的两个彩色图像传入处理器中处理获得深度图像；

(2.2)利用深度图像，使用种子生长算法，获得可通行的路径；具体实现如下：对于深度图像中所有的像素点(X,Y,Z)，随机选取3个点，构成以这3个点为顶角的一个三角形平面，平面方程为AX+BY+CZ+D＝0；如果

其中，n_th为阈值，取0.85～0.95，说明该平面和水平面夹角较小，可作为地面的候选；否则跳出进行下一次随机取点；对可作为地面的候选平面，计算出深度图像中每一点(X_i,Y_i,Z_i)到该候选平面的距离

如果DPS_i＜T，其中，T为阈值，取20mm～100mm，则将(X_i,Y_i,Z_i)归为该平面内点；取所有候选平面中内点最多的平面作为初始地面；

(2.3)对步骤(2.1)获得的彩色图像进行颜色空间变换，获取在HSV颜色空间中的彩色图HSV；对彩色图像提取Canny边缘，获取彩色图像的边缘图像Canny；

(2.4)对于初始地面中的像素点，其四邻域不全是地面的像素点设为种子点；将种子点都压入种子点堆栈进行四邻域生长；设定种子点可向四邻域生长的条件：①这个种子点没有遍历过；②这个种子点不属于边缘图像Canny的像素点；四邻域生长过程如下：

对种子点G，该点的色调值为h；它的其中一个四邻域像素点G_i，色调值为h_i；如G_i不属于边缘图像Canny的像素点，且G_i没有被遍历过，且|h-h_i|＜δ，其中δ是阈值，取2～5，则G_i也视为可通行的地面，压入种子点堆栈；当遍历堆栈中所有种子点后，种子区域生长结束；初始地面经过种子区域生长，被扩展到了距离更长，范围更广的扩展地面，即为可通行区域；

(3)障碍物检测，包括如下步骤：

根据相机参数，计算得到深度图像中的点[u,v]^T到世界坐标点[x_w,y_w,z_w]^T的变换公式:

u₀,v₀分别为深度图像的中心坐标；z_c表示相机坐标的z轴值，f为双目相机焦距；

利用该公式将深度图像转换为点云图，并获取图像中每一像素点的高度H，此时坐标系是以相机为基准的，对H进行判断，若H不在[-0.1-h～0.1-h]范围内，即视为障碍物；h为相机高度；

其中，点云图的计算过程如下：

(3.1)世界坐标点M(Xw,Yw,Zw)映射到图像点m(u,v)的公式如下：

其中u,v为图像坐标系下的任意坐标点；u₀,v₀分别为图像的中心坐标；x_w,y_w,z_w表示世界坐标系下的三维坐标点；z_c表示相机坐标的z轴值，即目标到相机的距离；R,T分别为外参矩阵的3x3旋转矩阵和3x1平移矩阵；f为双目相机焦距；

由于世界坐标原点和相机原点是重合的，即没有旋转和平移，所以:

相机坐标系和世界坐标系的坐标原点重合，相机坐标和世界坐标下的同一个物体具有相同的深度，即z_c＝z_w，公式可进一步简化为：

从以上的变换矩阵公式，可以计算得到图像点[u,v]^T到世界坐标点[x_w,y_w,z_w]^T的变换公式:

(4)对步骤(2)标记出可通行区域和步骤(3)标记出障碍物进行凸包检测，将凸点记为

m表示凸点的个数；将凹点记为

n表示凹点个数，对于可通行区域的第j行像素点有

分别代表当前行像素点的左右两端像素点，对于第j行中的像素点(x_ij,y_ij)(其中i和j分别代表行和列的标号)，利用以下计算公式求出T的评分：

其中α和β_p为评分参数，α取值为1.0～1.5，β_p取值为0.3～0.7；

利用以下公式求出O的评分：

其中γ_q为评分参数，γ_q取值为0.3～0.7；

(5)对于步骤(2)获得的可通行区域以内的像素，设置一套评分体系，内容包括步骤(1)获得的偏差角度θ、距离可通行区域边缘的远近T、距离障碍物的远近O；由以下公式可算出像素点(x_ij,y_ij)的总评分G：

G＝Aθ+BT+CO

其中A、B、C分别为θ、T、O的评分参数；其中，A的取值为1.5-2.5，B的取值为0.5-1.5，C的取值为0.5-1.5；

对于所有属于通行区域的像素进行评分得到像素的评分集合{G_ij}；

取每一行的所有像素取评分最高的像素G_jmax，从而得到从远到近的最高评分像素合集{G_max}；将相邻行最高分像素点用平滑曲线相连，即可获得基于当前图像的最优路径规划。

Images