CN115683109A - 基于cuda和三维栅格地图的视觉动态障碍物检测方法 - Google Patents

Abstract

本发明属于计算机视觉技术领域，具体涉及一种基于CUDA和三维栅格地图的视觉动态障碍物检测方法。相比于一些传统的动态检测方法在存在快速移动物体情况下难以兼顾实时性与准确性，本发明方法能够在三维场景中出现快速移动物体的情况下进行有效的快速准确的识别，使无人机路径规划过程快速且稳定。本发明方法在实际应用中不需要为无人机配备多个视觉相机提供多视角图像，仅根据单一视角图像便可识别动态障碍物。本发明设计的动态检测方法可以提供多个动态障碍物的质心及速度信息，为后续路径规划算法的在线处理提供原始数据。

Description

基于CUDA和三维栅格地图的视觉动态障碍物检测方法

技术领域

本发明属于计算机视觉技术领域，具体涉及一种基于CUDA和三维栅格地图的视觉动态障碍物检测方法。

背景技术

近年来，由于计算机视觉在机器人导航规划、图像处理、SLAM等领域中的广泛应用，关于计算机视觉的研究得到了学术界和工业界极大的关注。在机器人路径规划任务中，需要机器人通过视觉方法感知周围环境，对环境进行实时建图、躲避静态以及动态障碍物。而由于动态障碍物的不可预测性以及检测动态障碍物算法实时性或者准确性差的特点，如今的大多路径规划算法难以应对动态障碍物场景。因此，通过设计新的动态检测方法使得机器人可以快速准确地感知动态障碍物位置并进行躲避是十分重要的。

发明内容

有鉴于此，本发明提供了一种基于CUDA和三维栅格地图的视觉动态障碍物检测方法，能够在配备NVIDIA GPU的嵌入式设备上进行实时处理，识别出多个动态障碍物的位置与速度；通过本发明的动态检测方法，可以有效的应用于无人机路径规划中，保证路径规划的稳定性与安全性。

实现本发明的技术方案如下：

基于CUDA和三维栅格地图的视觉动态障碍物检测方法，该检测方法是指检测每帧时刻动态障碍物的质心、瞬时速度和轮廓，包括以下步骤：

步骤一，根据相机位姿及深度信息，利用GPU并行化处理能力对相机中的第一帧图像实时构建三维栅格地图，得到第一帧图像的三维栅格地图；

步骤二，根据步骤一得到的第一帧图像的三维栅格地图，对第二帧图像进行并行化动态障碍物检测，得到第二帧图像的检测结果(包括动态障碍物的轮廓)，然后对第二帧图像的检测结果进行后处理得到第二帧图像中动态障碍物的个数，并获得第二帧图像中每个动态障碍物的质心三维坐标；

步骤三，根据相机位姿及深度信息，利用GPU并行化处理能力对相机中的第二帧图像实时构建三维栅格地图，得到第二帧图像的三维栅格地图；

步骤四，根据步骤三得到的第二帧图像的三维栅格地图，对第三帧图像进行并行化动态障碍物检测，得到第三帧图像的检测结果，然后对第三帧图像的检测结果进行后处理得到第三帧图像中动态障碍物的个数，并获得第三帧图像中每个动态障碍物的质心三维坐标；

步骤五，根据步骤四得到的第三帧图像中动态障碍物的质心三维坐标与步骤二得到的第二帧图像中动态障碍物的质心三维坐标计算动态障碍物第三帧时刻的瞬时速度；

步骤六，根据相机位姿及深度信息，利用GPU并行化处理能力对相机中的第三帧图像实时构建三维栅格地图，得到第三帧图像的三维栅格地图；

步骤七，根据步骤六得到的第三帧图像的三维栅格地图，对第四帧图像进行并行化动态障碍物检测，得到第四帧图像的检测结果，然后对第四帧图像的检测结果进行后处理得到第四帧图像中动态障碍物的个数，并获得第四帧图像中每个动态障碍物的质心三维坐标；

步骤八，根据步骤七得到的第四帧图像中动态障碍物的质心三维坐标与步骤五得到的第三帧图像中动态障碍物的质心三维坐标计算动态障碍物第四帧时刻的瞬时速度；

…

以此类推，得到每帧时刻每个动态障碍物的质心和瞬时速度。

所述的第一步中，在三维栅格地图构建中，对以相机为中心的周围三维空间区域以小的正方体(边长为0.2m)栅格为单位进行空间划分，得到体素栅格，用来表示三维栅格地图；

为了能够利用CUDA的大量线程进行并行化处理，需要设计基于CUDA的并行化算法，具体来说，对空间区域进行二维切片，已知相机模型，将体素栅格投影到相机二维平面，对应到每个像素上，体素栅格在相机二维平面上的投影为：

其中，V_x,y,z代表体素栅格中心坐标，i为投影像素的横坐标，j为投影像素的纵坐标，p为相机模型，f代表函数，

代表投影深度，通过比较投影深度与测量深度，得到当前体素栅格的状态是被占用还是空闲，具体可以分为以下四种情况：

情况1：投影像素超出图像边界，说明该体素栅格没有被观测到，所以占据概率不会更新；

情况2：投影深度大于测量深度，说明该体素栅格在一个障碍物后面，所以占据概率不会更新；

情况3：投影深度小于测量深度，说明该体素栅格在障碍物前面，所以占据概率降低；

情况4：投影深度与测量深度接近，说明该体素栅格属于障碍物，所以占据概率增加；

所述的步骤二中，在根据第一帧图像的三维栅格地图，对第二帧图像进行并行化动态障碍物检测的准备工作中，首先进行显存预分配以及CPU向GPU的数据传输，传输当前帧的深度图并将深度图由二维形式转为一维。根据输入图像的分辨率，分配相应线程进行图像像素点二维到三维的反投影得到空间三维坐标，存储在全局内存中，对于图像噪声进行额外的归零处理。二维图像到三维空间反投影公式如下：

其中，f_x，f_y，c_x，c_y由相机内参矩阵获得，在相机出厂后固定，该算法中假定已获取内参矩阵。u为像素横坐标，v为像素纵坐标，Z为由深度图像获取的每个像素点的深度，X和Y分别代表反投影到三维空间后的横纵坐标。

在并行化动态障碍物检测过程中，重新分配线程，分配大小应按照空间区域大小以及体素栅格大小决定。随后根据上一帧位姿得到上一帧的空间区域范围，对上一帧的空间区域进行切片化处理得到三维体素栅格，对三维体素栅格进行筛选，得到仅是上一帧被判断为障碍物的三维体素栅格，随后对当前帧进行三维到二维的投影，得到投影过后的像素坐标，三维空间到二维图像的投影公式如下：

其中，f_x，f_y，c_x，c_y由相机内参矩阵获得，在相机出厂后固定，该算法中假定已获取内参矩阵。Z为由深度图像获取的每个像素点的深度，X和Y分别代表三维空间点的横纵坐标，u为像素横坐标，v为像素纵坐标。

投影过程中对超出图像边界的部分进行去除。随后对在该三维体素栅格投影到的二维像素坐标附近设定搜索窗口，在搜索窗口内以遍历的方式寻找像素，计算该像素反投影到三维空间后的坐标与该格点的三维坐标之间欧式距离的平方，得到最小误差距离的平方。

其中，x、y、z分别代表空间格点的三维坐标，而

代表像素反投影得到的三维坐标，d为欧式距离的平方。

接着，根据事先设定的距离阈值，对小于该阈值的点设定为静态点，而大于该阈值的点设定为动态点。其中，通过对每个像素设定状态锁，解决三维空间格点切片投影的过程中出现不同切片对同一像素的动静态点区分错乱的情况，具体为在某三维体素栅格投影到的像素被识别为静态后，通过设定状态锁使其不会再后续的判断中被重新识别为动态，保证算法执行的稳定性与有效性。

在并行化动态障碍物检测完成后，将得到的动态点由设备端数据传输回主机端，以二值化的图像形式进行显示，其中静态点以黑色表示，动态点以白色表示。

在对第二帧图像的检测结果进行后处理的过程中，对并行化处理后的原始图像进行图像处理中的开操作，随后对得到的处理后的图像进行连通域判断，初步得到多个动态障碍物各自的面积大小以及质心坐标，通过设定阈值，如果识别出的动态障碍物面积小于等于500像素将被视为噪声处理，对于面积大于500像素的动态障碍物，经过坐标转换得到世界坐标系下的质心坐标。

所述的步骤三中，流程与步骤一完全相同，进行三维栅格地图构建。

所述的步骤四中，并行化动态障碍物检测以及质心三维坐标计算的流程与步骤二完全相同，可以得到当前帧的质心坐标。

所述的步骤五中，在计算瞬时速度的准备过程中，由于图像噪声的影响，即使经过了图像开操作以及设计的阈值进行过噪声的处理，但某些动态障碍物的识别仍有可能是错误的。为了更为准确的识别，随后进行前后帧间多个动态障碍物的匹配，对于无法成功匹配的动态障碍物，算法并不视为有效的动态障碍物。首先得到当前帧识别出的可能存在的多个动态障碍物，可以得到它们在世界坐标系下的三维质心坐标，对于实际情况只有1个动态障碍物的情况，由于图像处理的误差，可能会识别出超过一个动态障碍物，通过设定质心距离阈值对近距离的多个动态物体进行删除，只保留其中的一个作为实际的识别出的动态物体。

在计算瞬时速度的过程中，首先对所有当前帧识别出的动态物体，与上一帧所识别出的动态物体进行匹配，其中考虑了多种可能出现的情况，包括当前帧与上一帧检测到的动态物体数量不一致等的处理。整个匹配过程通过设定距离阈值，对前后帧动态物体的质心距离加以判断，并对成功匹配的动态物体根据质心坐标的距离以及相邻两帧图像的相隔时间计算瞬时速度。

在得到实时的三维栅格地图后，根据上一帧的占据栅格信息以及上一帧相机位姿、当前帧的位姿及深度信息，通过对比空间点三维坐标和像素点反投影到三维空间的坐标的误差判断动态点。整个过程利用CUDA进行并行化处理，使得算法能够高效的完成。

有益效果

(1)相比于一些传统的动态检测方法在存在快速移动物体情况下难以兼顾实时性与准确性，本发明方法能够在三维场景中出现快速移动物体的情况下进行有效的快速准确的识别，使无人机路径规划过程快速且稳定。

(2)本发明方法在实际应用中不需要为无人机配备多个视觉相机提供多视角图像，仅根据单一视角图像便可识别动态障碍物。

(3)本发明设计的动态检测方法可以提供多个动态障碍物的质心及速度信息，为后续路径规划算法的在线处理提供原始数据。

附图说明

图1为GPU建立三维栅格地图效果图；

图2为去噪之前的GPU动态检测效果图；

图3为去噪之后的GPU动态检测效果图；

图4为可视化后的最终动态检测效果图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

下面给出GPU动态检测算法的详细说明：

首先在接收到图像的前十帧，进行预建图处理，该过程按照步骤一中GPU建图算法处理，保存当前帧的位姿，且不进行任何动态检测操作。

随后对每一帧接收到的位姿及深度信息，结合保存的上一帧位姿，先进行当前帧的GPU动态检测，再进行当前帧的GPU建图。动态检测中，首先进行显存预分配以及CPU向GPU的数据传输，传输当前帧的深度图并将深度图由二维形式转为一维。

根据输入图像的分辨率，分配相应线程进行图像像素点二维到三维的反投影得到空间三维坐标，存储在全局内存中，对于图像噪声进行额外的归零处理。二维图像到三维空间反投影公式如下：

其中，f_x，f_y，c_x，c_y由相机内参矩阵获得，在相机出厂后固定，该算法中假定已获取内参矩阵。u为像素横坐标，v为像素纵坐标，Z为由深度图像获取的每个像素点的深度，X和Y分别代表反投影到三维空间后的横纵坐标。

并行化反投影完成之后重新分配线程，分配大小应按照空间区域事先划分的格点数量进行分配，随后进行并行化动态检测算法，具体步骤如下。

首先根据上一帧位姿得到上一帧的空间区域格点，对上一帧的空间区域进行切片化处理，并将上一帧的切片化的空间区域格点向当前帧进行投影，可以得到投影过后的像素坐标，该投影过程与二维到三维的反投影所用公式相同，只是未知量的不同。投影过程中对超出图像边界的部分进行去除。

对空间格点进行筛选，得到仅是上一帧被判断为障碍物的格点进入下一环节。

在该格点投影到的二维像素坐标附近设定搜索窗口，在窗口内遍历寻找像素，对比该像素反投影到三维后的坐标与该格点的三维坐标之间欧式距离的平方，得到最小误差距离的平方。

其中，x、y、z分别代表空间格点的三维坐标，而

代表像素反投影得到的三维坐标，d为欧式距离的平方。

接着，根据事先设定的距离阈值，对小于该阈值的点设定为静态点，而大于该阈值的点设定为动态点。其中，通过对每个像素设定状态锁，解决三维空间格点切片投影的过程中出现不同切片对同一像素的动静态点区分错乱的情况，具体为在某一切片投影过程中某像素被识别为静态点后，通过设定状态锁使其不会再后续的判断中被重新识别为动态，保证算法执行的稳定性与有效性。至此，并行化动态检测部分结束。

在并行化处理后，将得到的动态点由设备端数据传输回主机端，以二值化的图像形式进行显示，其中静态点以黑色表示，动态点以白色表示。为了更好的执行后续的动态障碍物识别，对并行化处理后的原始图像进行图像处理中的开操作，随后对得到的处理后的图像进行连通域判断，初步得到多个动态障碍物各自的面积大小以及质心坐标，通过设定阈值，如果识别出的动态障碍物面积过小将被视为噪声处理，不会进入到下一环节，随后经过坐标转换后得到世界坐标系下的质心坐标。

由于图像噪声的影响，即使经过了图像开操作以及设计的阈值进行过噪声的处理，但某些动态障碍物的识别仍有可能是错误的。为了更为准确的识别，随后进行前后帧间多个动态障碍物的匹配，对于无法成功匹配的动态障碍物，算法并不视为有效的动态障碍物。下面详细说明具体的匹配方法。

首先得到当前帧识别出的可能存在的多个动态障碍物，可以得到它们在世界坐标系下的三维质心坐标，对于实际情况只有1个动态障碍物的情况，由于图像处理的误差，可能会识别出超过一个动态障碍物，通过设定质心距离阈值对近距离的多个动态物体进行处理，只保留其中的一个作为实际的识别出的动态物体。

随后对所有当前帧识别出的动态物体，与上一帧所识别出的动态物体进行匹配，其中考虑了多种可能出现的情况，包括当前帧与上一帧检测到的动态物体数量不一致等的处理。整个匹配过程通过设定距离阈值，对前后帧动态物体的质心距离加以判断，并对成功匹配的动态物体计算瞬时相对相机的速度。

接着，我们对提出的GPU动态检测方法进行了实际场景实验。我们进行了多种测试环境下的实际实验：多种测试环境下的实验用于验证动态物体的检出效果同时计算程序处理一帧图像的总耗时，算法的测试环境为个人电脑，实际运行环境为Jetson NX嵌入式设备。所有实验中，算法输入的图像分辨率为640x480。

图2为在行人快速行走的室内环境下检测出的动态物体，其中，白色为动态点，黑色为静态点。该图是在进行滤波之前的效果图，可以看出，存在一些离散的点，这样会对后续动态物体匹配带来干扰；图3为进行噪声去除后的检测效果图，可以看出，图2中的离散点被完全滤除，方便后续的动态物体匹配。经过测算，程序包括建图加上动态检测的总耗时为10ms左右,如果出现动态障碍物，总耗时大约在20ms左右，即50Hz，完全能够满足实时性要求。

一种基于CUDA和三维栅格地图的视觉动态障碍物检测方法，包括：

根据实时生成的三维栅格地图对每帧图像中出现的动态点进行并行化检测。

在检测出动态点后，进行滤除噪声等后处理操作，分离出多个动态障碍物，

对多个动态障碍物进行前后帧匹配，得到三维质心坐标及相对速度。

本发明考虑在三维空间中，针对在视觉传感器约束下的实时建图与动态检测问题，提出了一种基于CUDA和视觉三维栅格地图构建的动态障碍物检测方法。该方法在获知相机位姿以及图像深度信息的前提下，实时地生成三维栅格地图，并根据此地图进行动态障碍物的检测，整个过程并行化处理，可显著提升实时性。步骤1：根据相机位姿及深度信息，利用GPU并行化处理能力对每帧图像进行实时建图。步骤2：根据上一帧的建图信息，对当前帧进行并行化动态障碍物检测，随后对并行化检测结果进行后处理得到分离出的动态物体个数及质心三维坐标，并根据前后帧检测结果计算物体相对速度。

实施例

本发明提供了基于CUDA和三维栅格地图的视觉动态障碍物检测方法，其详细实施方式如下：

步骤一、根据相机位姿及深度信息，利用GPU并行化处理能力对每帧图像进行实时建图。

具体过程如下：

本发明考虑无人机在复杂场景下的三维空间中飞行。根据机载电脑Jetson NX搭载的t265提供的无人机位姿以及D435i获取的深度图像对无人机接收到的每帧图像进行实时建图。算法所建立的三维栅格地图可以为无人机提供三维空间障碍物信息，使得无人机路径规划算法可以有效的运行。

在三维栅格地图构建中，对以相机为中心的周围三维空间区域以小的正方体栅格为单位进行空间划分，得到体素栅格，用来表示三维栅格地图。为了能够利用CUDA的大量线程进行并行化处理，需要设计基于CUDA的并行化算法。具体来说，对空间区域进行二维切片，由于已知相机模型，所以可以将其投影到相机二维平面，对应到每个像素上。公式化表示如下：

其中，V_x,y,z代表体素栅格中心坐标，

代表投影深度，w代表测量深度。通过比较投影深度与测量深度，可以得到当前体素的状态是被占用还是空闲。具体可以分为以下四种情况

情况1：投影像素超出图像边界，说明该体素没有被观测到，所以占据概率不会更新。

情况2：投影深度大于测量深度，说明该体素在一个障碍物后面，所以占据概率不会更新。

情况3：投影深度小于测量深度，说明该体素在障碍物前面，所以占据概率降低。

情况4：投影深度与测量深度相等，说明该体素属于障碍物，所以占据概率增加。

图1所示为在rviz中显示的GPU建立三维栅格地图的效果，实际测试场地为走廊环境，图中的彩色点为实际的障碍物位置，可以明显看出来是一个走廊环境。测试过程中对每帧图像的建图耗时进行了计算，总共约5ms，即200Hz。

步骤二、根据上一帧的建图信息，对当前帧进行并行化动态障碍物检测，随后对并行化检测结果进行后处理得到分离出的动态物体个数及质心三维坐标，并根据前后帧检测结果计算物体相对速度。

在得到实时的三维栅格地图后，根据上一帧的占据栅格信息以及上一帧相机位姿、当前帧的位姿及深度信息，通过对比空间点三维坐标和像素点反投影到三维空间的坐标的误差判断动态点。整个过程利用CUDA进行并行化处理，使得算法能够高效的完成。

下面给出GPU动态检测算法的详细说明：

首先在接收到图像的前十帧，进行预建图处理，该过程按照步骤一中GPU建图算法处理，保存当前帧的位姿，且不进行任何动态检测操作。

随后对每一帧接收到的位姿及深度信息，结合保存的上一帧位姿，先进行当前帧的GPU动态检测，再进行当前帧的GPU建图。动态检测中，首先进行显存预分配以及CPU向GPU的数据传输，传输当前帧的深度图并将深度图由二维形式转为一维。

根据输入图像的分辨率，分配相应线程进行图像像素点二维到三维的反投影得到空间三维坐标，存储在全局内存中，对于图像噪声进行额外的归零处理。二维图像到三维空间反投影公式如下：

其中，f_x，f_y，c_x，c_y由相机内参矩阵获得，在相机出厂后固定，该算法中假定已获取内参矩阵。u为像素横坐标，v为像素纵坐标，Z为由深度图像获取的每个像素点的深度，X和Y分别代表反投影到三维空间后的横纵坐标。

并行化反投影完成之后重新分配线程，分配大小应按照空间区域事先划分的格点数量进行分配，随后进行并行化动态检测算法，具体步骤如下。

首先根据上一帧位姿得到上一帧的空间区域格点，对上一帧的空间区域进行切片化处理，并将上一帧的切片化的空间区域格点向当前帧进行投影，可以得到投影过后的像素坐标，该投影过程与二维到三维的反投影所用公式相同，只是未知量的不同。投影过程中对超出图像边界的部分进行去除。

对空间格点进行筛选，得到仅是上一帧被判断为障碍物的格点进入下一环节。

在该格点投影到的二维像素坐标附近设定搜索窗口，在窗口内遍历寻找像素，对比该像素反投影到三维后的坐标与该格点的三维坐标之间欧式距离的平方，得到最小误差距离的平方。

其中，x、y、z分别代表空间格点的三维坐标，而

代表像素反投影得到的三维坐标，d为欧式距离的平方。

接着，根据事先设定的距离阈值，对小于该阈值的点设定为静态点，而大于该阈值的点设定为动态点。其中，通过对每个像素设定状态锁，解决三维空间格点切片投影的过程中出现不同切片对同一像素的动静态点区分错乱的情况，具体为在某一切片投影过程中某像素被识别为静态点后，通过设定状态锁使其不会再后续的判断中被重新识别为动态，保证算法执行的稳定性与有效性。至此，并行化动态检测部分结束。

在并行化处理后，将得到的动态点由设备端数据传输回主机端，以二值化的图像形式进行显示，其中静态点以黑色表示，动态点以白色表示。为了更好的执行后续的动态障碍物识别，对并行化处理后的原始图像进行图像处理中的开操作，随后对得到的处理后的图像进行连通域判断，初步得到多个动态障碍物各自的面积大小以及质心坐标，通过设定阈值，如果识别出的动态障碍物面积过小将被视为噪声处理，不会进入到下一环节，随后经过坐标转换后得到世界坐标系下的质心坐标。

由于图像噪声的影响，即使经过了图像开操作以及设计的阈值进行过噪声的处理，但某些动态障碍物的识别仍有可能是错误的。为了更为准确的识别，随后进行前后帧间多个动态障碍物的匹配，对于无法成功匹配的动态障碍物，算法并不视为有效的动态障碍物。下面详细说明具体的匹配方法。

首先得到当前帧识别出的可能存在的多个动态障碍物，可以得到它们在世界坐标系下的三维质心坐标，对于实际情况只有1个动态障碍物的情况，由于图像处理的误差，可能会识别出超过一个动态障碍物，通过设定质心距离阈值对近距离的多个动态物体进行处理，只保留其中的一个作为实际的识别出的动态物体。

随后对所有当前帧识别出的动态物体，与上一帧所识别出的动态物体进行匹配，其中考虑了多种可能出现的情况，包括当前帧与上一帧检测到的动态物体数量不一致等的处理。整个匹配过程通过设定距离阈值，对前后帧动态物体的质心距离加以判断，并对成功匹配的动态物体计算瞬时相对相机的速度。

接着，我们对提出的GPU动态检测方法进行了实际场景实验。我们进行了多种测试环境下的实际实验：多种测试环境下的实验用于验证动态物体的检出效果同时计算程序处理一帧图像的总耗时，算法的测试环境为个人电脑，实际运行环境为Jetson NX嵌入式设备。所有实验中，算法输入的图像分辨率为640x480。

图2为在行人快速行走的室内环境下检测出的动态物体，其中，白色为动态点，黑色为静态点。该图是在进行滤除噪声之前的效果图，可以看出，存在一些离散的点，这样会对后续动态物体匹配带来干扰；图3为进行噪声去除后的检测效果图，可以看出，图2中的离散点被完全滤除，方便后续的动态物体匹配。经过测算，程序包括建图加上动态检测的总耗时为10ms左右,如果出现动态障碍物，总耗时大约在20ms左右，即50Hz，完全能够满足实时性要求。

图4为可视化显示后的结果，其中图片右侧的白色圆点为算法给出的动态物体质心，实际图片中的动态物体为手，可以看出基本准确。

通过真实场景实验验证，可以说明，使用这种基于CUDA和三维栅格地图的视觉动态障碍物检测方法，能够兼顾准确性与实时性，可以在嵌入式设备中实时运行，可以作为无人机路径规划、导航避障等上层任务的基础。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.基于CUDA和三维栅格地图的视觉动态障碍物检测方法，其特征在于：

该检测方法是指检测每帧时刻动态障碍物的质心、瞬时速度和轮廓，包括以下步骤：

步骤一，根据相机位姿及深度信息，利用GPU并行化处理能力对相机中的第一帧图像实时构建三维栅格地图，得到第一帧图像的三维栅格地图；

步骤二，根据步骤一得到的第一帧图像的三维栅格地图，对第二帧图像进行并行化动态障碍物检测，得到第二帧图像的检测结果，然后对第二帧图像的检测结果进行后处理得到第二帧图像中动态障碍物的个数，并获得第二帧图像中每个动态障碍物的质心三维坐标；

步骤三，根据相机位姿及深度信息，利用GPU并行化处理能力对相机中的第二帧图像实时构建三维栅格地图，得到第二帧图像的三维栅格地图；

步骤四，根据步骤三得到的第二帧图像的三维栅格地图，对第三帧图像进行并行化动态障碍物检测，得到第三帧图像的检测结果，然后对第三帧图像的检测结果进行后处理得到第三帧图像中动态障碍物的个数，并获得第三帧图像中每个动态障碍物的质心三维坐标；

步骤五，根据步骤四得到的第三帧图像中动态障碍物的质心三维坐标与步骤二得到的第二帧图像中动态障碍物的质心三维坐标计算动态障碍物第三帧时刻的瞬时速度；

步骤六，根据相机位姿及深度信息，利用GPU并行化处理能力对相机中的第三帧图像实时构建三维栅格地图，得到第三帧图像的三维栅格地图；

步骤七，根据步骤六得到的第三帧图像的三维栅格地图，对第四帧图像进行并行化动态障碍物检测，得到第四帧图像的检测结果，然后对第四帧图像的检测结果进行后处理得到第四帧图像中动态障碍物的个数，并获得第四帧图像中每个动态障碍物的质心三维坐标；

步骤八，根据步骤七得到的第四帧图像中动态障碍物的质心三维坐标与步骤五得到的第三帧图像中动态障碍物的质心三维坐标计算动态障碍物第四帧时刻的瞬时速度；

…

以此类推，得到每帧时刻每个动态障碍物的质心和瞬时速度。

2.根据权利要求1所述的基于CUDA和三维栅格地图的视觉动态障碍物检测方法，其特征在于：

所述的第一步中，在三维栅格地图构建中，对以相机为中心的周围三维空间区域以小的正方体栅格为单位进行空间划分，得到体素栅格，用来表示三维栅格地图；

对空间区域进行二维切片，已知相机模型，将体素栅格投影到相机二维平面，对应到每个像素上，体素栅格在相机二维平面上的投影为：

其中，V_x,y,z代表体素栅格中心坐标，i为投影像素的横坐标，j为投影像素的纵坐标，p为相机模型，f代表函数，

代表投影深度，通过比较投影深度与测量深度，得到当前体素栅格的状态是被占用还是空闲。

3.根据权利要求2所述的基于CUDA和三维栅格地图的视觉动态障碍物检测方法，其特征在于：

所述的当前体素栅格的状态是被占用还是空闲具体分为以下四种情况：

情况1：投影像素超出图像边界，说明该体素栅格没有被观测到，所以占据概率不会更新；

情况2：投影深度大于测量深度，说明该体素栅格在一个障碍物后面，所以占据概率不会更新；

情况3：投影深度小于测量深度，说明该体素栅格在障碍物前面，所以占据概率降低；

情况4：投影深度与测量深度接近，说明该体素栅格属于障碍物，所以占据概率增加。

4.根据权利要求2所述的基于CUDA和三维栅格地图的视觉动态障碍物检测方法，其特征在于：

所述的步骤二中，在根据第一帧图像的三维栅格地图，对第二帧图像进行并行化动态障碍物检测的准备工作中，首先进行显存预分配以及CPU向GPU的数据传输，传输当前帧的深度图并将深度图由二维形式转为一维，根据输入图像的分辨率，分配相应线程进行图像像素点二维到三维的反投影得到空间三维坐标，存储在全局内存中，对于图像噪声进行额外的归零处理，二维图像到三维空间反投影公式如下：

其中，f_x，f_y，c_x，c_y由相机内参矩阵获得，在相机出厂后固定，该算法中假定已获取内参矩阵。u为像素横坐标，v为像素纵坐标，Z为由深度图像获取的每个像素点的深度，X和Y分别代表反投影到三维空间后的横纵坐标。

5.根据权利要求4所述的基于CUDA和三维栅格地图的视觉动态障碍物检测方法，其特征在于：

在并行化动态障碍物检测过程中，重新分配线程，分配大小应按照空间区域大小以及体素栅格大小决定，随后根据上一帧位姿得到上一帧的空间区域范围，对上一帧的空间区域进行切片化处理得到三维体素栅格，对三维体素栅格进行筛选，得到仅是上一帧被判断为障碍物的三维体素栅格，随后对当前帧进行三维到二维的投影，得到投影过后的像素坐标，三维空间到二维图像的投影公式如下：

其中，f_x，f_y，c_x，c_y由相机内参矩阵获得，在相机出厂后固定，该算法中假定已获取内参矩阵。Z为由深度图像获取的每个像素点的深度，X和Y分别代表三维空间点的横纵坐标，u为像素横坐标，v为像素纵坐标。

6.根据权利要求5所述的基于CUDA和三维栅格地图的视觉动态障碍物检测方法，其特征在于：

投影过程中对超出图像边界的部分进行去除，随后对在该三维体素栅格投影到的二维像素坐标附近设定搜索窗口，在搜索窗口内以遍历的方式寻找像素，计算该像素反投影到三维空间后的坐标与该格点的三维坐标之间欧式距离的平方，得到最小误差距离的平方；

其中，x、y、z分别代表空间格点的三维坐标，而

代表像素反投影得到的三维坐标，d为欧式距离的平方。

7.根据权利要求6所述的基于CUDA和三维栅格地图的视觉动态障碍物检测方法，其特征在于：

根据事先设定的距离阈值，对小于该阈值的点设定为静态点，而大于该阈值的点设定为动态点；

在并行化动态障碍物检测完成后，将得到的动态点由设备端数据传输回主机端，以二值化的图像形式进行显示。

8.根据权利要求7所述的基于CUDA和三维栅格地图的视觉动态障碍物检测方法，其特征在于：

在对第二帧图像的检测结果进行后处理的过程中，对并行化处理后的原始图像进行图像处理中的开操作，随后对得到的处理后的图像进行连通域判断，初步得到多个动态障碍物各自的面积大小以及质心坐标，通过设定阈值，如果识别出的动态障碍物面积小于等于500像素将被视为噪声处理，对于面积大于500像素的动态障碍物，经过坐标转换得到世界坐标系下的质心坐标。

9.根据权利要求8所述的基于CUDA和三维栅格地图的视觉动态障碍物检测方法，其特征在于：

所述的步骤五中，进行前后帧间多个动态障碍物的匹配，对于无法成功匹配的动态障碍物，算法并不视为有效的动态障碍物，首先得到当前帧识别出的可能存在的多个动态障碍物，得到它们在世界坐标系下的三维质心坐标，对于实际情况只有1个动态障碍物的情况，通过设定质心距离阈值对近距离的多个动态物体进行删除，只保留其中的一个作为实际的识别出的动态物体。

10.根据权利要求9所述的基于CUDA和三维栅格地图的视觉动态障碍物检测方法，其特征在于：

在计算瞬时速度的过程中，首先对所有当前帧识别出的动态物体，与上一帧所识别出的动态物体进行匹配，包括当前帧与上一帧检测到的动态物体数量不一致等的处理，整个匹配过程通过设定距离阈值，对前后帧动态物体的质心距离加以判断，并对成功匹配的动态物体根据质心坐标的距离以及相邻两帧图像的相隔时间计算瞬时速度。

Images