一种服务于无人机自主导航的简化环境建模方法与流程

本发明属于机器人自动化领域，尤其涉及一种服务于无人机自主导航的简化环境建模方法。

背景技术：

无人机是无人驾驶飞机的简称，近年来广泛的应用需求带来了无人机领域的蓬勃发展，其应用包括搜寻与营救、智能交通、三维重建和电力巡线等。根据无人机的自动化程度可以将其分为半自主无人机和全自主无人机，前者要求操作人员辅助执行任务，后者完全依靠无人机携带的传感器在线感知环境并自主导航和执行决策。半自主无人机的应用容易受到环境和任务的限制，对于危险性较大的应用场景，现场操作人员将面临生命危险，另外远程操作容易受到通信故障和网络延迟的影响，因此全自主无人机将是未来该领域研究的重要趋势。全自主无人机在线导航的基础组件是构建环境地图，根据传感器探测数据建模周围障碍物与自由空间，从而服务于路径规划和躲避障碍等任务。

传统的地图构建方法主要是基于占据栅格地图：将地图划分为相等大小的正方形或立方体格子，并计算每个格子是障碍物或自由空间的概率。由于无人机的存储和计算资源有限，随着探测区域面积的增加，无人机将面临较大的存储负担。简化环境建模方法不仅可以降低存储压力，提高地图搜索速度，还能为无人机在线导航等任务提供直观的环境感知信息。并且简化的环境模型可以减少无人机共享感知数据等任务的通信负担和传输延时。

目前已有的简化环境模型中，应用于自动驾驶和地面机器人的方法主要针对二维场景，无法直接应用于无人机自主导航任务。现有的三维环境简化建模方法仍然依赖栅格地图，首先将传感器数据转化为三维栅格地图，进一步提取障碍物的多边形轮廓从而构建简化环境模型。该方法依赖于栅格尺寸的大小，且障碍物边缘容易受到量化的影响，建模精度降低。

技术实现要素：

本发明针对现有技术的不足，提供一种服务于无人机自主导航的简化环境建模方法。

本发明的技术方案为一种服务于无人机自主导航的简化环境建模方法，包括以下步骤：

步骤1，获取深度图像并统计分析每列深度图像数据的概率密度分布，根据峰值宽度分析障碍物粗糙程度，并计算深度图像上每列障碍物的位置；

步骤2，将深度图像上提取的障碍物映射到三维空间中，称为障碍物垂直条带，借助深度图像的索引序号对障碍物垂直条带进行快速空间聚类；

步骤3，搜索聚类内部障碍物垂直条带之间的缝隙，若缝隙大于无人机可穿行尺寸，则根据缝隙位置分割聚类，反之将缝隙填充；

步骤4，根据步骤3中得到的聚类结果拟合障碍物轮廓平面，构建由多层多边形棱柱面组成的简化环境模型。

而且，步骤1中，设深度图像中第u列像素的概率密度分布为fu(x)，x为核密度估计结果中的像素深度值，每列深度图像中提取的障碍物的近似深度记为其中为障碍物的近似深度，i用于表示当前列从图像顶部开始第i个障碍物，k为当前列障碍物数目；采用自适应大小的滑动窗口提取障碍物，

滑动窗口宽度δx为

其中和分表为核密度估计结果的峰值宽度两侧的深度值，α为峰值宽度系数，下标表示x^±与峰值的值关系，x^±表示左右两边满足条件的x值；

设定滑动窗口的高度为

其中，huav为无人机高度，fx为相机的焦距，β为误差系数。

而且，步骤2中，设障碍物垂直条带为其中xc,yc分别为障碍物垂直条带x轴、y轴的坐标值，为障碍物垂直条带的z轴下端和上端的坐标值，相机坐标系中x轴正方向朝右，y轴正方向朝前，z轴正方向超上，其中u表示深度图像的列坐标，i用于标识深度图像第u列从上计算第i个障碍物；

通过借助二维深度图像的列索引序号，避免空间中邻域搜索的复杂计算，包括从深度图像第一列开始创建第一个类c1，类采用队列存储结构，每一个聚类用队列上标l表示当前队列的存储数量，下表qn表示队列序号，将第一列第一个障碍物垂直条带填入第一个队列中，依次计算下一个障碍物垂直条带与已有聚类的队列末端的垂直条带之间的欧几里得距离d，若距离d小于无人机宽度wuav，将该列垂直条带填入相应的聚类队列末端，反之则创建新的聚类，从而实现障碍物垂直条带的快速空间聚类。

而且，步骤3中，根据每个障碍物垂直条带上的缝隙是其相对整个墙面的高度范围的补集，计算缝隙宽度，当缝隙宽度大于无人机宽度时，保留该缝隙，并从该缝隙处分割聚类，最终同一聚类内部障碍物垂直柱子高度相等，否则将缝隙填补；多个障碍物垂直条带的缝隙有效高度为相邻障碍物垂直条带高度补集的交集。

而且，采用二分法对聚类内部的障碍物条带进行搜索，若该障碍物垂直条带的补集高度小于无人机高度，则将该障碍物垂直条带的高度缝隙填补，并从该处分割墙体为两部分，对分割后的墙面重复上述搜索分割步骤，直到墙面宽度小于无人机宽度时停止搜索。

而且，步骤4中，设多层多边形棱柱面组成的简化障碍物模型为由多个空间邻接且垂直地面的矩形组成，障碍物为o∈r³，r³为三维空间，其中θ为简化障碍物模型，满足即简化障碍物模型包含障碍物本身，πr为组成障碍物简化模型的矩形平面，下标r表示矩形的序号，描述障碍物的矩形数量为r，

每个矩形平面由四个顶点组成，从正视方向看分别为矩形的左下角，左上角、右下角和右上角坐标，由于该矩形面垂直于地面，和在水平面上的投影坐标值相同，因此矩形πr简化表达为从而较少存储量，其中矩形的两个对角顶点和其中xa,ya,zmin为点的x,y,z轴的坐标值，其中xb,yb,zmax为点的x,y,z轴的坐标值，矩形参数η为np为该平面拟合的障碍物垂直条带的数量，分别为属于该平面的障碍物垂直条带的x轴坐标和x轴坐标的平均值，分别为属于该平面的障碍物垂直条带的x轴坐标与y轴坐标之积的平均值和x轴坐标平方的平均值，存储拟合平面的参数，便于合并相邻近似平行的平面。

而且，步骤4中，所述的拟合障碍物轮廓平面，实现方式为，步骤3得到的障碍物垂直条带聚类中，相同聚类内部的障碍物垂直条带高度相同，从俯视方向看障碍物轮廓的提取视为带有高度数值的线段提取过程；

采用分割与合并的线段提取思想，拟合聚类内部的障碍物垂直条带，得到拟合平面l，利用点到平面距离的计算方法，计算每个障碍物垂直条带中心到平面l的距离，距离的最大值记为最大拟合误差εmax，如果最大拟合误差εmax小于阈值εl，则该聚类可以用一个拟合平面表示，如果最大拟合误差εmax大于等于阈值εl，将聚类从最大拟合误差处分割开，继续对分割开的两个部分执行上述分割流程，直到剩余点数小于预设数值nl时，停止分割。

而且，为了进一步减少与无人机导航无关的环境细节信息，将分割与合并的线段提取结果中分割点数小于nl的部分进行凹陷填补。

本发明将无人机携带的深度传感器数据转化为由棱柱面组成的简化环境模型，采用概率密度估计的方法提取深度图像上的障碍物并计算其位置信息，根据无人机的安全飞行尺寸消除与自主导航无关的环境细节信息，用尽可能少的数据量描述环境模型，同时保证较高的建模精度，为无人机自主导航和感知信息共享提供服务。

附图说明

图1是本发明实施例的原理示意图；

图2是本发明实施例的流程图；

图3是本发明实施例的简化环境模型的平面表达参数的示意图；

图4是本发明实施例的每列深度图像的概率密度估计结果和深度图像的关系示意图；

图5是本发明实施例的自适应滑动窗口提取一列深度图像中的障碍物示意图；

图6是本发明实施例的障碍物高度缝隙过滤说明图；

图7是本发明实施例的障碍物轮廓拟合和凹陷填补的流程示意图；

图8是本发明实施例的仿真环境中简化环境建模的结果图；

图9是本发明实施例的仿真环境数据集存储量与栅格地图简化环境模型存储量的对比结果图；

图10是本发明实施例的仿真环境数据集存储量与栅格地图简化环境模型精度的对比结果图；

图11是本发明实施例的真实世界数据和仿真数据运算效率与列采样率关系图。

具体实施方式

为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。

所述实施例的示例在附图中示出，下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

下文的公开提供了许多不同的实施例或例子用来实现本发明的不同结构。为了简化本发明的公开，下文中对特定例子的部件和设置进行描述。它们仅仅为示例，并且目的不在于限制本发明。此外，本发明可以在不同例子中重复参考数字和/或字母。这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施例和/或设置之间的关系。

本发明提出，服务于无人机自主导航的环境地图不需要保留所有的环境细节信息，例如无人机无法穿行的缝隙和粗糙的障碍物表面。将无人机携带的传感器探测数据转化为障碍物几何模型同时，过滤与自主导航等任务无关的环境细节信息，在保持无人机可通行自由空间不变的情况下，用尽可能少的数据量建模环境，从而减少无人机的存储负担和环境感知数据共享的通信负担。

本发明方法使用的建模系统由深度传感器、嵌入式处理平台和无人机组成，其中深度传感器用于获取无人机周围的环境信息，无人机机身搭载嵌入式处理平台用于处理传感器数据，由此组成简化环境建模系统。无人机型号为经纬matrice100，嵌入式处理平台型号为jetsontx2，深度传感器型号为zed；

下面结合图1至图11介绍本发明的实施方式。

图1为本实施例的原理示意图。无人机携带深度视觉传感器，搭载嵌入式平台实时处理深度传感器的探测数据。根据环境中障碍物的特点构建包含障碍物的多层多边形棱柱模型，把深度图像转换为棱柱表面世界。无人机通过gps或slam获得自身的位置，通过图像坐标系和世界坐标系的变换关系，将视野中探测到的障碍物拟合为平面并转为到三维世界中。

图2所示为本实施例的流程说明图，首先将双目视觉传感器获得的左右彩色图像转为灰度图，通过视差匹配算法得到深度图像，采用概率密度估计的方法提取深度图像上的障碍物并计算其位置，将得到的每列障碍物映射到三维空间中，即障碍物垂直条带，进一步过滤与无人机导航无关的环境细节信息，拟合障碍物垂直条带得到简化的障碍物平面轮廓，进而服务于无人机自主导航和感知信息共享。

参见图2，本发明实施例提供的一种服务于无人机自主导航的简化环境建模方法，具体包含以下步骤：

步骤1：获取深度图像并统计分析每列深度图像数据的概率密度分布，根据峰值宽度分析障碍物粗糙程度，并计算深度图像上每列障碍物的位置；

实施例中，步骤1中所述的深度传感器数据记为x(u,v),u＝1,2,...,m；v＝1,2,...,n，其中m＝640为深度图像的总列数，n＝480为深度图像的总行数，x(u,v)为每个像素(u,v)的深度数值，单位为米，其中x(u,v)∈[xmin,xmax]，表示传感器的探测范围，深度值下限xmin＝0.5，深度值上限xmax＝30，单位为米，超过范围的像素不处理；

步骤1中所述深度图像中第u列像素的概率密度分布为fu(x)：

其中，为核函数，x为核密度估计结果中的像素深度值，xv为第v行的像素深度值，h为核函数宽度，下标u表示深度图像的列序号，

实施例采用的核函数为：

其中x与fu(x)中的x含义相同，其中ω为均值，σ为标准差，核密度估计的结果反映了某一深度下存在障碍物，将fu(x)中低于λho部分消除，过滤噪声和地面微弱的起伏，其中λho表示最小物体高度对应的阈值，具体实施时可根据感知的物体分辨率设定。每列深度图像中提取的障碍物的近似深度记为其中为障碍物的近似深度，其中下标μ代表fu(x)的峰值中心，i用于表示当前列从图像顶部开始第i个障碍物，其中k为当前列障碍物数目，为了抵抗深度传感器的噪声，采用自适应大小的滑动窗口提取障碍物，滑动窗口宽度δx为

其中和分表为核密度估计结果的峰值宽度两侧的深度值，α为峰值宽度系数，下标表示x^±与峰值的值关系，x^±表示左右两边满足条件的x值，深度传感器的误差特性通常与距离的平方成正比，因此设定滑动窗口的高度为

其中huav为无人机高度，huav＝1.5，单位为米，fx为相机的焦距，取值2.0毫米，β为误差系数，β＝0.01，εe为静态误差，若滑动窗口内像素深度值的平均值在阈值范围内，则内部像素属于深度为的障碍物，反之，窗口中在阈值范围内的最外端像素设置为物体端点；

根据核密度估计峰值的宽度判断物体的粗糙程度，若物体粗糙度小于等于阈值用该物体深度的平均值表示物体距离，若物体粗糙度大于阈值，则用物体凸出最近点表示物体距离表述为：

其中minx表示该段物体的最小深度值；

步骤1中所述的深度图像上提取的障碍物位置表示为：其中和分别表示在深度图像上该列障碍物的上下坐标，uⁱ表示图像列数，上标i为当前列从上计算第i个障碍物；

每列深度图像中没有被提取的剩余像素主要为倾斜角度小或深度分布过于分散的物体，为了建立统一障碍物模型，本发明将剩余像素按固定深度距离δd范围进行离散分割，δd根据无人机的大小设定，深度值在[q×δd，(q+1)×δd]之间的像素划分为一段，表示障碍物的距离在该深度区间内，其中q为正整数，剔除像素数量小于np的分段，np可以设为3。

图4所示为本发明实施例的每列深度图像的概率密度估计结果，采用概率密度估计的方法分析深度图像的每列数据，当某个深度下存在障碍物时，则该列相应深度位置的概率密度估计结果将大于某一阈值，图中浅色透明斜平面表示最小物体高度，过滤由于传感器噪声或轻微的地面起伏产生的微小峰值。上方的曲线为某一列深度图像的概率密度估计结果，峰值的宽度δx反应了物体的粗糙程度。

图5所示为本发明实施例的自适应滑动窗口提取深度图像中障碍物的示意图，图中中间部分为某一列深度像素值，滑动窗口的大小由步骤1中概率密度估计结果的峰值宽度δx决定，窗口高度δv由无人机安全飞行高度决定，过滤由于传感器噪声或物体自身的凹陷缝隙导致的深度值缺失或大幅度跳变，下方是当前列深度值的概率密度估计结果，其中三个峰值中心分别为图中o0(0,m)为当前列最底部的像素坐标，oh(0,vc)为校正的相机中心的行坐标。

步骤2：将深度图像上提取的障碍物映射到三维空间中，称为障碍物垂直条带，借助深度图像的索引序号对障碍物垂直条带进行快速空间聚类；

步骤2中所述的障碍物垂直条带为其中xc,yc分别为障碍物垂直条带x轴、y轴的坐标值，为障碍物垂直条带的z轴下端和上端的坐标值，相机坐标系中x轴正方向朝右，y轴正方向朝前，z轴正方向超上，其中u表示深度图像的列坐标，i用于标识深度图像第u列从上计算第i个障碍物；

障碍物在图像坐标系中和相机坐标系的转换关系为：

uc、vc表示相机标定的横坐标中心和纵坐标中心，zc表示障碍物竖直方向的坐标值；

步骤2中所述的聚类为cj，j∈[1,t]，t为聚类的数目，本发明提出一种快速空间聚类方法：通过借助二维深度图像的列索引序号u，避免空间中邻域搜索的复杂计算，从深度图像第一列开始创建第一个类c1，类采用队列存储结构，每一个聚类用队列上标l表示当前队列的存储数量，下表qn表示队列序号，将第一列第一个障碍物垂直条带填入第一个队列中，依次计算下一个障碍物垂直条带与已有聚类的队列末端的垂直条带之间的欧几里得距离d，若距离d小于无人机宽度wuav，将该列垂直条带填入相应的聚类队列末端，反之则创建新的聚类，从而实现障碍物垂直条带的快速空间聚类。

步骤3：搜索聚类内部障碍物垂直条带之间的缝隙，若缝隙大于无人机可穿行尺寸，则根据缝隙位置分割聚类，反之将缝隙填充；

步骤3中所述的缝隙记为g(w,h)，其中w为缝隙宽度，h为缝隙高度；

步骤3中所述的聚类为步骤2中的聚类结果cj，同一个聚类的障碍物垂直条带在空间上具有连续性，可以视为一个具有一定厚度的“障碍物墙”，在墙面上寻找与无人机导航无关的缝隙，通过消除这些冗余的环境细节信息进一步减少存储，该障碍物墙面的高度范围为分别表示聚类cj的障碍物垂直条带竖直方向上的最大值和最小值，障碍物垂直条带的高度简化表示为表示第i个障碍物垂直条带在竖直方向的底部坐标值和顶部坐标值，每个障碍物垂直条带上的缝隙是其相对整个墙面的高度范围的补集，由两段组成：

表示通过求最大最小高度的补集得到的缝隙高度范围，其中cj为聚类序号；因为障碍物垂直条带上下都不一定是最值，那么上下端都存在缝隙，为了不失一般性，用两段表示，表示两段补集的z轴方向的底部值和顶部的值；m为补集的上下两段序号，如果补集的高度小于无人机高度，则直接填补，如果补集高度大于无人机高度，则继续向聚类内部相邻障碍物垂直条带搜索，计算缝隙宽度，当缝隙宽度大于无人机宽度时，保留该缝隙，并从该缝隙处分割聚类，最终同一聚类内部障碍物垂直柱子高度相等，否则将缝隙填补；

多个障碍物垂直条带的缝隙有效高度为相邻障碍物垂直条带高度补集的交集：

为了加快缝隙搜寻的速度，采用二分法对聚类cj内部的障碍物条带进行搜索，先计算索引号为的障碍物垂直条带，|cj|为聚类cj中障碍物垂直条带的数量，为该聚类近似中间的索引序号，若该障碍物垂直条带的补集高度小于无人机高度，则将该障碍物垂直条带的高度缝隙填补，并从该处分割墙体为两部分，对分割后的墙面重复上述搜索分割步骤，直到墙面宽度小于无人机宽度时停止搜索；

最终得到的障碍物垂直条带的聚类，属于同一类的障碍物垂直条带具有相同高度。

图6所示为本发明的障碍物高度缝隙过滤说明图，其中圆柱体表示障碍物垂直条带，该图中所有障碍物垂直条带聚为一类，可以视为具有一定厚度的障碍物墙面，墙面的高度最大值为zmax，最小值为zmin，由于传感器噪声和物体凹陷缝隙等因素导致障碍物垂直条带之间存在高度上的缝隙，如果这些缝隙宽度w2或高度h2小于无人机尺寸，则可以填补这些与导航等任务无关的缝隙区域，如果缝隙宽度w1和高度h1都大于无人机尺寸，则保留该缝隙的高度交集部分，并根据该缝隙的位置分割整个聚类，得到分割后的聚类内部的障碍物垂直条带高度相同。

步骤4：根据步骤3中得到的聚类结果拟合障碍物轮廓平面，构建由多层多边形棱柱面组成的简化环境模型；

图3所示为本发明实施例的简化环境模型示意，左图为墙面的简化结果，采用垂直于地面的矩形简化障碍物墙面，四个顶点表示物体的轮廓位置。

实施例中，多层多边形棱柱面组成的简化障碍物模型为由多个空间邻接且垂直地面的矩形组成，障碍物为o∈r³，r³为三维空间，其中θ为简化障碍物模型，满足即简化障碍物模型包含障碍物本身，πr为组成障碍物简化模型的矩形平面，下标r表示矩形的序号，描述障碍物的矩形数量为r。每个矩形平面由四个顶点组成，从正视方向看分别为矩形的左下角，左上角、右下角和右上角坐标，由于该矩形面垂直于地面，和在水平面上的投影坐标值相同，因此矩形πr简化表达为从而较少存储量，其中矩形的两个对角顶点和其中xa,ya,zmin为点的x,y,z轴的坐标值，其中xb,yb,zmax为点的x,y,z轴的坐标值，矩形参数η为np为该平面拟合的障碍物垂直条带的数量，分别为属于该平面的障碍物垂直条带的x轴坐标和x轴坐标的平均值，分别为属于该平面的障碍物垂直条带的x轴坐标与y轴坐标之积的平均值和x轴坐标平方的平均值，存储拟合平面的参数，便于合并相邻近似平行的平面；

为了明确自由空间在平面πi的哪儿一侧，定义方向，从俯视角度看，沿着到方向，平面右侧区域为自由空间，左侧区域为障碍物或未知区域；

作为优选，步骤4中所述的拟合障碍物轮廓平面方法为：

步骤3得到的障碍物垂直条带聚类中，相同聚类内部的障碍物垂直条带高度相同，从俯视方向看障碍物轮廓的提取可以视为带有高度数值的线段提取过程；

采用分割与合并的线段提取思想，拟合该聚类内部的障碍物垂直条带，得到拟合平面l，利用点到平面距离的计算方法，计算每个障碍物垂直条带中心到平面l的距离，距离的最大值记为最大拟合误差εmax，如果最大拟合误差εmax小于阈值εl，则该聚类可以用一个拟合平面表示，如果最大拟合误差εmax大于等于阈值εl，将聚类从最大拟合误差处分割开，继续对分割开的两个部分执行上述分割流程，直到剩余点数小于nl时，停止分割，实施例中nl＝3；分割后计算相邻平面的角度，角度小于θ0的时合并两个线段，εl根据传感器噪声设为0.1米，θ0通常设为5到10度，最大拟合误差的阈值εl根据障碍物模型的分辨率设定为；

线段合并后新的平面参数ηnew为：其中η1、η2分别为两个待合并的平面的参数，np1、np2分别为两个平面拟合的障碍物垂直条带数量；

为了进一步减少与无人机导航无关的环境细节信息，本发明将分割与合并的线段提取结果中分割点数小于nl的部分进行凹陷填补；

凹陷填补通过计算凹陷区域的直径判断是否填补该区域，由于同一聚类内部障碍物垂直条带的高度相同，根据水平面上的二维向量的叉乘结果可以判断该区域是否为凹陷区域，设相邻的三个障碍物垂直条带为{p^e-1,p^e,p^e+1}，上标表示障碍物垂直条带的索引序号，如果满足条件：

则剔除障碍物垂直条带p^e，迭代执行上述操作直到没有障碍物垂直条带被剔除，其中表示障碍物垂直条带p^e-1和p^e的xy平面上连接后形成的向量，表示障碍物垂直条带p^e+1和p^e的xy平面上连接后形成的向量，||p^e-1,p^e+1||表示障碍物垂直条带p^e-1和p^e之间在xy平面上的投影距离，其中wuav为无人机宽度。

图7所示为本发明的障碍物轮廓拟合和凹陷填补的流程示意图，图中上方的黑色实心圆圈为俯视方向的障碍物垂直条带，下方带有a、b、c…h字母的圆圈和弧形连线分别表示障碍物垂直条带和他们的相邻关系，相邻的三个障碍物垂直条带用椭圆形表示，图中θ表示相邻三个障碍物垂直条带的夹角，d表示两端障碍物垂直条带的距离，如果他们凹凸变化较大，分割与合并的线段提取算法无法拟合轮廓线段，根据相邻三个障碍物垂直条带之间的凹凸关系和凹陷区域大小进一步消除无关环境细节，从上向下看每一行减少的圆圈表示被消除的障碍物垂直条带，迭代填补凹陷缝隙得到最终简化环境模型。

具体实施时，以上流程可采用软件方式实现自动运行。运行本发明的硬件装置也应当在保护范围内。

图8所示为本发明实施例的无人机搭载深度传感器探测数据的简化环境建模结果图，无人机携带深度视觉传感器，搭载嵌入式平台实时处理深度传感器的探测数据。根据环境中障碍物的特点构建包含障碍物的多层多边形棱柱模型，把深度图像转换为棱柱表面世界。其中大于无人机安全尺寸的可通行区域被保留，小于无人机安全尺寸的区域被填补。

图9所示为发明实施例的仿真环境数据集存储量与栅格地图简化环境模型存储量的对比结果图，图(a)为仿真数据集存储量对比结果，图(b)为真实数据集存储量对比结果。本发明的简化环境模型数据量相比于栅格地图简化环境模型的数据量大多数情况下更小，由于栅格的简化环境模型依赖于栅格的分辨率大小，栅格的分辨率越高，消耗的存储量越大，分辨率越低，同时由于量化导致的建模误差越大。对比不同栅格分辨率与本发明算法的存储数量，栅格大小分别为0.1米、0.2米、0.4米和0.8米，左图中虚线椭圆部分本算法的存储量大于分辨率低的栅格简化环境建模算法，本算法在存储和精度上综合占优。

图10所示为发明实施例的仿真环境数据集存储量与栅格地图简化环境模型精度的对比结果图，图(a)为仿真数据集不加噪声情况下建模精度对比，图(b)为仿真数据集加入不同幅度随机噪声的建模精度对比。因为本算法分析一列深度图像中障碍物的概率密度分布特点估计障碍物位置，相比于不同分辨率的栅格简化环境建模算法具有更高的建模精度，并且具有更好的抗造性能。

图11所示为本发明实施例的真实世界数据和仿真数据运算效率与列采样率关系图，图(a)为仿真数据采样间隔与运行时间关系图，图(b)为采样间隔与运行时间关系图。算法主要包含两大部分，障碍物垂直条带提取和平面拟合，算法的运行效率随着列采样间隔的增加而提高，在不降采样的情况下，算法处理640*480的深度图像运行效率为50hz左右，满足实时性需求。

本文中所描述的具体实施案例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施案例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。