一种基于TOF相机深度图和点云图的避障处理方法及系统技术方案

一种基于TOF相机深度图和点云图的避障处理方法及系统，主要包括如：(1)获取TOF相机固定帧率下的相关数据，包括深度值图像、幅度值图像；(2)对步骤1中获取的深度数据进行处理，实现基于深度图的车宽区域划分；(3)由深度图转换到点云图，对点云图进行地面滤除操作；(4)对处理后的深度图与点云图进行避障判断，输出信号；本发明专利技术原理通俗，操作简单，可以实现对不同障碍物的识别并进行判断和处理，适合固定场景下的工程应用。

【技术实现步骤摘要】
一种基于TOF相机深度图和点云图的避障处理方法及系统
本专利技术属于光学测量
，具体涉及一种基于TOF相机深度图和点云图的避障处理方法及系统。
技术介绍
TOF相机采用“时间飞行法”对物体进行测距，通过计算光在空中的飞行时间获取物体到相机的距离，并将距离信息保存在深度图中，所以可以针对深度图进行避障判断，根据不同的距离信息输出不同的避障信号。由于深度图是二维图像，很难解决地面信息造成的误触发现象，所以要将深度图转化成点云数据，对三维点云信息进行地面滤除，以消除地面信息的干扰。所以，基于TOF相机的避障判断要结合深度图和点云图进行，同时，在进行避障判断之前，要对深度图进行车宽区域划分，以消除车身以外区域的干扰，提高避障效率。对TOF相机采集到的深度图进行车宽区域划分，主要是根据深度图中像素的像元尺寸和相机焦距的关系去推算实际车身在深度图中所占的区域，以深度图的中心点为基准，求出车身区域对应的两列，然后在该区域内进行避障判断，以消除车身区域外的干扰，提高避障效率，此外，在避障判断中还要对车身区域进行渲染，方便用户进行区分。在避障功能中，地面信息会造成误触发现象，所以要将地面信息滤除，而对TOF相机的深度图进行避障，主要用于具有一定高度的物体，地面信息可以直接滤除而不会对避障结果产生影响，但如果物体自身的高度不够高，直接滤除地面就会把物体也滤除掉，从而无法进行避障处理，而深度图是二维图像，要进行地面滤除比较困难，所以，这也是本次专利技术中需要解决的问题。现有技术中，公开号为CN109343079A，公开日为2019年2月15日的中国专利技术专利申请《一种测距避障方法及避障装置》，公开了一种测距避障方法及避障装置，该避障装置包括TOF深度相机和移动载体，TOF深度相机安装在移动载体上，TOF深度相机包括光源、传感器模块和第一控制处理模块，移动载体包括第二控制处理模块，第一控制处理模块与光源、传感器模块和第二控制处理模块均电连接；光源用于提供光信号；传感器模块用于在光信号被障碍物反射后接收反射光信号，并将反射光信号转换为电信号以传输至第一控制处理模块；第一控制处理模块用于根据电信号得到探测结果；第二控制处理模块用于根据探测结果和移动载体的速度信息确定移动载体的避障措施。虽然上述文献的测距避障方法及避障装置具有很好的可靠性、稳定性和实时性，但是上述文献的避障方法要对场景中的所有区域进行避障判断，避障效率低，TOF相机只能放置在靠近障碍物的一端，放置位置不灵活。
技术实现思路
本专利技术的目的在于解决如何在避障判断之前对深度图进行车宽区域划分，消除车身以外区域的干扰，提高避障效率以及如何滤除地面的问题。本专利技术是通过以下技术方案解决上述技术问题的：一种基于TOF相机深度图和点云图的避障处理方法，包括如下步骤：步骤1：对TOF相机进行标定，固定相机的高度和角度，获取相机固定帧率下的实验数据，包括深度图、幅度图，TOF相机得到的数据会有所丢失，实验中要对获取到的深度图和幅度图进行裁剪，以统一分辨率，对裁剪后的深度图进行幅度值滤波，降低进行避障判断的噪声干扰；步骤2：对步骤1中的深度图进行车宽区域划分，滤除车宽以外的区域，车宽以外的区域不作为避障操作的考虑范围；所述的深度图进行车宽区域划分的方法为：1)选取经过幅度值滤波后的深度图中心点作为车宽区域划分的依据，求出车宽区域对应的列；深度图中心点的坐标为(r,c)，找出深度图中心点的物理距离为vehicle/2的两点p1和p2，p1和p2与深度图中心点同行不同列，分别位于深度图中心点的左右两侧，坐标为(r,c1)和(r,c2)；2)根据深度图中心点的深度值的取值范围，求出(r,c1)和(r,c2)之后，对深度图的遍历限制在c1列和c2列内，实现车宽区域的划分；其中vehicle为车宽，r为深度图中心点的行坐标，c表示深度图中心点的行坐标，c1和c2分别表示深度图中车身区域对应的左列和右列；步骤3：将步骤1中获取到的深度图转换成对应的点云图，利用生成的点云图进行地面移除处理，以消除地面干扰造成的避障信号误触发现象，实现针对客观物体的避障操作；步骤4：对步骤3中的点云图进行基于地面的障碍物检测，然后对步骤2中进行车宽区域划分后的深度图进行避障操作，输出相应的避障信号。选取经过幅度值滤波后的深度图中心点作为车宽区域划分的依据，求出车宽区域对应的列；根据深度图中心点的深度值的取值范围，求出(r,c1)和(r,c2)之后，对深度图的遍历限制在c1列和c2列内，实现车宽区域的划分；避障方法不需要对场景中的所有区域进行避障判断，只对车身内的区域进行避障判断，无关区域不做考虑，且TOF相机放置位置灵活，不需要放置在靠近障碍物的一端，提高了避障效率，可以实现对不同障碍物的识别并进行判断和处理，适合固定场景下的工程应用。作为本专利技术技术方案的进一步改进，所述的车宽区域对应的列的计算方法为：计算深度图中任意一点到深度图中心点的物理距离与焦距的比值tan(θ)如下：其中，dist为p1或p2点到深度图中心点p点的距离，depth为p1或p2点的深度值，center_depth为深度图中心点p的深度值，focal为相机的焦距，pixel为深度图的像元尺寸，dist与pixel的乘积表示p1或p2点到深度图中心点p的物理距离；p1或p2点到深度图中心点p点的距离的计算公式为：dist＝|c1(2)-c|(2)联立公式(1)和(2)，当p点的坐标为(r＝depth.row/2-1，c＝depth.col/2-1)时得到：depth.row表示深度图的行的值，depth.col表示深度图的列的值，c1(2)表示c1或c2的值。作为本专利技术技术方案的进一步改进，所述的深度图中心点的深度值的取值范围的计算方法为：由0≤c≤depth.col-1得到tan(θ)的取值范围如下：联立式(1)、(5)可得深度图中心点的深度值的取值范围如下：由(6)式可知，进行车身区域划分的深度图中心点的深度值的取值范围。作为本专利技术技术方案的进一步改进，根据所述的深度图中心点的深度值的取值范围，判断：1a)如果深度图中心点的深度值在所述的取值范围内，则可以直接由公式(1)至(4)求出(r,c1)和(r,c2)；1b)如果深度图中心点的深度值不在所述的取值范围内，则取深度图中心点周边的深度值在该取值范围内的像素点，通过公式(1)至(4)求出在取值范围内的中心深度值点，此时求出的深度图中心点的深度值不是实际的中心带点的深度值，而是为了方便求车宽区域而临时计算得到的，然后再求出(r,c1)和(r,c2)；1c)如果深度图中不包括位于取值范围内的点，无法对深度值进行车宽区域划分，此时直接进行避障判断。作为本专利技术技术方案的进一步改进，所述的步骤1中TOF相机得到的数据处理包括以下步骤：2a)对T本文档来自技高网...

【技术保护点】
1.一种基于TOF相机深度图和点云图的避障处理方法，其特征在于，包括如下步骤：/n步骤1：对TOF相机进行标定，固定相机的高度和角度，获取相机固定帧率下的实验数据，包括深度图、幅度图，TOF相机得到的数据会有所丢失，实验中要对获取到的深度图和幅度图进行裁剪，以统一分辨率，对裁剪后的深度图进行幅度值滤波，降低进行避障判断的噪声干扰；/n步骤2：对步骤1中的深度图进行车宽区域划分，滤除车宽以外的区域，车宽以外的区域不作为避障操作的考虑范围；/n所述的深度图进行车宽区域划分的方法为：/n1)选取经过幅度值滤波后的深度图中心点作为车宽区域划分的依据，求出车宽区域对应的列；深度图中心点的坐标为(r,c)，找出深度图中心点的物理距离为vehicle/2的两点p

【技术特征摘要】
1.一种基于TOF相机深度图和点云图的避障处理方法，其特征在于，包括如下步骤：
步骤1：对TOF相机进行标定，固定相机的高度和角度，获取相机固定帧率下的实验数据，包括深度图、幅度图，TOF相机得到的数据会有所丢失，实验中要对获取到的深度图和幅度图进行裁剪，以统一分辨率，对裁剪后的深度图进行幅度值滤波，降低进行避障判断的噪声干扰；
步骤2：对步骤1中的深度图进行车宽区域划分，滤除车宽以外的区域，车宽以外的区域不作为避障操作的考虑范围；
所述的深度图进行车宽区域划分的方法为：
1)选取经过幅度值滤波后的深度图中心点作为车宽区域划分的依据，求出车宽区域对应的列；深度图中心点的坐标为(r,c)，找出深度图中心点的物理距离为vehicle/2的两点p1和p2，p1和p2与深度图中心点同行不同列，分别位于深度图中心点的左右两侧，坐标为(r,c1)和(r,c2)；
2)根据深度图中心点的深度值的取值范围，求出(r,c1)和(r,c2)之后，对深度图的遍历限制在c1列和c2列内，实现车宽区域的划分；其中vehicle为车宽，r为深度图中心点的行坐标，c表示深度图中心点的行坐标，c1和c2分别表示深度图中车身区域对应的左列和右列；
步骤3：将步骤1中获取到的深度图转换成对应的点云图，利用生成的点云图进行地面移除处理，以消除地面干扰造成的避障信号误触发现象，实现针对客观物体的避障操作；
步骤4：对步骤3中的点云图进行基于地面的障碍物检测，然后对步骤2中进行车宽区域划分后的深度图进行避障操作，输出相应的避障信号。


2.根据权利要求1所述的一种基于TOF相机深度图和点云图的避障处理方法，其特征在于，所述的车宽区域对应的列的计算方法为：
计算深度图中任意一点到深度图中心点的物理距离与焦距的比值tan(θ)如下：



其中，dist为p1或p2点到深度图中心点p点的距离，depth为p1或p2点的深度值，center_depth为深度图中心点p的深度值，focal为相机的焦距，pixel为深度图的像元尺寸，dist与pixel的乘积表示p1或p2点到深度图中心点p的物理距离；
p1或p2点到深度图中心点p点的距离的计算公式为：
dist＝|c1(2)-c|(2)
联立公式(1)和(2)，当p点的坐标为(r＝depth.row/2-1，c＝depth.col/2-1)时得到：






depth.row表示深度图的行的值，depth.col表示深度图的列的值，c1(2)表示c1或c2的值。


3.根据权利要求2所述的一种基于TOF相机深度图和点云图的避障处理方法，其特征在于，所述的深度图中心点的深度值的取值范围的计算方法为：由0≤c≤depth.col-1得到tan(θ)的取值范围如下：



联立式(1)、(5)可得深度图中心点的深度值的取值范围如下：



由(6)式可知，进行车身区域划分的深度图中心点的深度值的取值范围。


4.根据权利要求3所述的一种基于TOF相机深度图和点云图的避障处理方法，其特征在于，根据所述的深度图中心点的深度值的取值范围，判断：
1a)如果深度图中心点的深度值在所述的取值范围内，则可以直接由公式(1)至(4)求出(r,c1)和(r,c2)；
1b)如果深度图中心点的深度值不在所述的取值范围内，则取深度图中心点周边的深度值在该取值范围内的像素点，通过公式(1)至(4)求出在取值范围内的中心深度值点，此时求出的深度图中心点的深度值不是实际的中心带点的深度值，而是为了方便求车宽区域而临时计算得到的，然后再求出(r,c1)和(r,c2)；
1c)如果深度图中不包括位于取值范围内的点，无法对深度值进行车宽区域划分，此时直接进行避障判断。


5.根据权利要求1所述的一种基于TOF相机深度图和点云图的避障处理方法，其特征在于，所述的步骤1中TOF相机得到的数据处理包括以下步骤：
2a)对TOF相机的深度图进行渲染，将不同的深度值区域渲染成不同颜色，同时保留客观物体的轮廓，方便直观显示；
2b)对TOF相机的幅度图进行渲染，调整幅度图的整体亮度，方便实时查看；
2c)将处理后的深度图进行车宽区域划分，并转换成相应的点云图数据以进行地面区域滤除，最后进行避障判断。


6.根据权利要求1所述的一种基于TOF相机深度图和点云图的避障处理方法，其特征在于，所述的步骤3中利用生成的点云图进行地面移除处理的方法包括以下步骤：
3a)预处理；将深度图像进行幅度值滤波，滤除部分噪点，幅度值滤波的阈值可以自由设置，设置过大会滤除物体细节，设置过小起不到明显的噪点滤除作用，所以幅度值阈值设置要合理，不宜过大或过小；
3b)深度图转点云图；深度图到点云图的转换，是球面坐标系到三维坐标的变换过程，遍历深度图中的所有像素点，对每个像素点求出它到中心深度点的距离，然后计算与相机焦距的比值，进而求得对应的点云z坐标值，进行wiggling校正计算公式为：



其中，z(r,c)、depth(r,c)分别为该像素坐标下点云距离值、深度图的距离值；
3c)地面滤除；根据步骤(3b)中生成的点云图，利用RANSAC算法检测地面并滤除，该算法名为随机采样一致性算法，表示从包含“局外点”的数据集中，以迭代的方式去估算某个数学模型的参数，最终得到具有概率性质的结果，如果想让所得结果更加精确，就必须增加迭代次数；在实验中，不同的参数分割得到的地面也不一样，参数设置越大，距离相机越远的点也会被认为是地面点，这样会导致滤除部分障碍物的点云数据，不利于避障检测；但参数设置过小会导致地面数据的滤除效果不明显，这样地面的误触发现象得不到解决，实验中通过固定相机的高度和角度来确定最佳参数的选择，如果相机的角度和高度发生改变，则最佳参数的设置也会发生改变；
3d)对进行地面滤除后的点云图和进行了车宽区域划分后的深度图进行避障检测，针对不同的距离区域输出不同的信号，输出信号有四种，分别为“安全”、“警告”、“减速”、“停止”，实验中会预先设置好三种输出信号分别对应的距离区间，停止信号的区间为[0，500mm]，减速区间为(500mm,1500mm],警告区间为(1500mm,3000mm]，大于3...

【专利技术属性】
技术研发人员：胡立松，瞿喜锋，金超，蔡勇，张瑞斌，于振中，李文兴，
申请(专利权)人：哈工大机器人合肥国际创新研究院，
类型：发明
国别省市：安徽;34