一种基于激光雷达的全3D占据体元地形建模方法与流程

本发明属于野外作业机器人、无人驾驶等应用领域，具体来说，涉及一种基于激光雷达的全3D占据体元地形建模方法。

背景技术：

在室外环境，无人平台的自主导航应用领域，一个精确、高效且拥有鲁棒性的三维地形模型起着至关重要的作用，直接决定着整个系统的运行效率和整体的经济效益。与室内环境不同，野外环境呈现维度高和结构复杂的特征，而环境的大尺度特征则加剧了地形模型的实时性矛盾。

空间环境的感知是三维地形建模的首要问题。常见的感知方法有：双目相机，激光雷达，超声波测距。双目相机的工作过程中，需要复杂的图像处理、匹配的过程。这需要长时间的处理，不满足三维地形建模系统的实时性。超声波由于能量衰减的问题，不适用于室外的场合。而激光雷达可以直接得到环境的深度信息，并受环境因素的干扰较小，故采用激光雷达作为传感器。

三维地形建模方式是系统工作的核心，通常三维地形建模的方法有：点云图、高程地图、多层高程图、体素图。点云被认为是最简单的三维环境模型，其由传感器数据直接得到。每个点都表示障碍信息，而空间中没有被扫描的区域和空表区域均视为空白区域，因此不能有效的表示空间信息。同时随着扫描数据的增大，存储空间也线性的增长。此外，点云模型对机器人路径规划等任务作用不大。高程图是通过划分二维平面网络并为每个已知的网络单元设定一个高度值来对环境进行描述。这种方法会忽略一些有效的空洞信息，故不适用与机器人导航应用领域。多层高程图是在高程图的每个网络单元中，存放多个高度值以描述垂直表面。该地图模型的分辨率不能进行改变优化，此外其仅仅只能对三维空间的表面进行建模，不能反映整个三维空间，故不适用机器人导航。体素图是将三维空间划分为规则的体元，并根据体元的占据状态对空间进行建模。该方法最大的缺陷是计算和存储的效率比较低下。此外，该方法没有对建图的数据进行可靠性分析，因而在面向导航的应用中鲁棒性较差。

总的来说，上述常用的三维地形建模方法存在以下问题：1)没有考虑地形数据的多源不确定性，例如传感器的测量误差，传感器相对于机器人的方位不确定性，机器人运动的不确定性，这使得地图模型缺乏鲁棒性；2)空间的描述不够准确，如高程图属于2.5D的环境模型，多层高程图不能对空间的状态进行描述，属于全3D环境模型，不能为机器人提供可靠的地图信息；3)这些现有方法在构建地形模型时没有综合考虑精度和计算复杂度之间的平衡问题，这导致所建立的地图难以应用于实际。

技术实现要素：

本发明的目的是提出一种基于激光雷达的全3D占据体元地形建模方法，以解决基于不确定集表示的全3D占据体元地形建模方法中地形建模的不确定性问题、空间描述的准确性问题和三维地形环境建模的效率问题，实现野外地形环境的精准、高效和实时建模。

为解决上述技术问题，本发明采用一种基于激光雷达的全3D占据体元地形建模方法，该方法包括以下步骤：

步骤10)数据的获取与坐标转换：首先采用安装在移动机器人平台上的激光雷达设备获取周边环境的深度信息；其次采用坐标变换将激光测量数据转化为全局坐标系中的点云数据，并对测量过程中存在的多源不确定性，采用区间集员的方式进行融合和处理；

步骤20)建立体元：对于光线起点到终点的空隙区域，根据光线跟踪算法，计算其所经网格的入点和出点，并构建负体元表示空闲状态；对于终点位置区域，根据计算得到的区间边界信息，将其关联到若干网络，并构建相应正体元表示障碍物占据状态，从而将全空间划分为占据、空闲和未知三种状态表示；

步骤30)基于约束的体元更新：将步骤20)建立的正体元或负体元加入到相关联网格的现有体元链表中；然后根据体元链表所需满足的体元高度约束、不相交约束和间距约束三个约束进行更新处理；

步骤40)测算占据状态：对步骤30)更新后的体元占据状态的概率进行计算，判断占据的状态。

作为优选例：所述的步骤10)包括以下步骤：

步骤101)数据采集：采用安装在移动机器人平台上的激光雷达设备对周围环境进行扫描，获得地形感知的原始数据；

步骤102)数据的坐标变换：进行坐标变换，将原始数据转化为全局坐标系下的数据信息，根据式(1)进行从激光传感器到全局坐标的齐次变换：

^WTl＝^WTm^mTl 式(1)

其中，^WTl表示激光坐标系到全局坐标系的齐次坐标变换，^WTm表示移动机器人自身坐标系到全局坐标系的齐次坐标变换，^mTl表示移动机器人从激光坐标系到移动机器人自身坐标系的齐次坐标变换；

激光传感器获取的距离数据和扫描角度对应到激光坐标系的齐次坐标R(ρ,β)，如式(2)所示：

R(ρ,β)＝(ρ·sβ,ρ·cβ,0,1)^T 式(2)

其中，ρ表示激光传感器的测量值，即激光传感器到障碍物之间的距离，β表示激光的扫描角度，s表示sin函数的简称，c表示cos函数的简称；

根据式(3)得到激光数据在全局坐标系中的坐标：

x＝(x,y,z,1)^T＝^WTm·^mTl·R(ρ,β) 式(3)

其中，x表示激光数据在全局坐标系的坐标，x表示激光数据在全局坐标系x坐标轴的坐标值，y表示激光数据在全局坐标系y坐标轴的坐标值，z表示激光数据在全局坐标系z坐标轴的坐标值；

步骤103)区间集元估计：测量数据在全局坐标系中的位置，如式(4)所示：

其中，表示测量数据在全局坐标系三个坐标轴的坐标值，上标T表示转置，表示机器人坐标系相对于全局坐标系的位姿估计，α表示激光扫描平面与水平面夹角，rL表示某一时刻激光传感器的测量值，表示激光的扫描角度，且

依据式(5)确定测量数据在全局坐标系的定界区间：

其中，Ix表示测量数据在全局坐标系的定界区间，[x]表示测量数据在全局坐标系x轴的区间范围，[y]表示测量数据在全局坐标系y轴的区间范围，[z]表示测量数据在全局坐标系z轴的区间范围，[P]表示参数的变化范围，[rL]表示参数rL的变化范围，[α]表示参数α的变化范围，β表示激光的扫描角度。

作为优选例：所述的步骤20)具体包括：

步骤201)：负体元相关网络关联：

步骤202)：正体元相关网络关联：根据不确定度形成的激光终点立方体区域，得到一组被该区域覆盖的网络单元列表，确定与终点相关联的网络单元其中，表示激光终点所在的单元，表示相邻的激光终点单元，表示最后一个激光终点单元，对C_end中的每个网络单元ci,j创建一个与之对应的正体元vp，vp的顶面高度为激光数据在z轴方向的区间上界，vp的底面高度为激光数据在z轴方向的区间下界。

作为优选例：所述的步骤201)具体包括：

步骤2011)设激光数据在全局坐标系下的起点位置坐标xL以及终点位置坐标x′L，

将从xL到x′L的激光光线投影到xy平面并执行光线跟踪，得到一组被激光穿过的网格单元列表其中，表示激光起点所在的网络单元，表示激光经过的网络单元，表示激光终点所在的网络单元；

步骤2012)建立光线跟踪的模型，如下式所示：

xL+t*d

其中，xL表示激光的起点位置坐标，t表示变量，length表示光线长度，0≤t≤length，d表示射线的单位方向向量；

步骤2013)若和在同一网络单元，则将的光线进入高度值赋为激光起点位置高度值zL，即该数据点不再计算，并建立负体元，否则，进入步骤2014)；

步骤2014)根据起点和终点计算出射线的单位方向向量d，设d[0]为d在x轴方向的分量，d[1]为d在y轴方向的分量，d[2]为d在z轴方向的分量，tMax[0]表示x轴方向穿过下一个网络边界的t值，tMax[1]表示y轴方向穿过下一个网络边界的t值；

步骤2015)C_ray中每个网络单元ci,j对应垂直空间上光线进入的高度值为该网络单元的激光起点高度zL，即取tMax[0]和tMax[1]中的较小值，即tMax[dim]←min{tMax[0],tMax[1]}，tMax[dim]表示选取x、y轴主要方向的t值；则激光在该网络单元的射出高度为

步骤2016)以网络链表中的网络为底，以光线跟踪算法得到的高度为高，建立负体元，负体元顶面如公式(6)所示，底面如公式(7)所示：

其中，表示负体元的顶面高度；表示负体元的底面高度。

作为优选例：所述的步骤30)具体包括：

步骤301)：将体元插入体元链表：对网络单元创建的正体元或负体元，分别插入对应的体元链表中；

步骤302)：约束限制：根据激光数据创建新体元后，遍历所有被更新的体元链表，并满足以下三个约束条件：

高度约束：每个体元的高度大于或等于网格分辨率res；如果体元的高度小于res，则进行膨胀处理，使高度达到res。

不相交约束：同一体元链表中的任何两个体元不交叠；如果出现交叠，则进行体元融合；如果出现一个体元在另一个体元之内，则进行融合；

间距约束：同一体元链表中的任意两个体元的间距大于res；如果两个体元的间距小于或等于res，则进行融合。

作为优选例：所述的进行膨胀处理具体包括：依据式(8)或式(9)将任何高度小于res的体元vx替换为高度等于res的体元vx^*：

其中，表示被替换体元的顶面高度，表示被替换体元的底面高度，表示替换体元的顶面高度，表示替换体元的底面高度。

作为优选例：所述的不相交约束中，进行融合具体包括：依据式(10)和式(11)，将需要进行融合处理的体元vx^A和vx^B替换为新体元vx^*：

vx^*＝vx^A∪vx^B 式(10)

其中，表示融合后体元的质量，表示待融合体元A的质量，表示待融合体元B的质量。

作为优选例：所述的间距约束中，进行体元融合具体包括：在不满足间距约束的体元vx^A与vx^B之间创建一个新体元vx^GAP，以填补体元vx^A与vx^B间的空间，并将vx^GAP的密度初始化为1，然后将所述三个体元融合为一个新体元vx^*，如式(12)和式(13)所示：

vx^*＝vx^A∪vx^GAP∪vx^B 式(12)

其中，表示间隙体元的质量。

作为优选例：所述的步骤40)具体包括：

步骤401)计算体元占据概率：对于空间任意一点，其占据值根据该点所在的正体元和负体元的占据密度来计算，如式(14)所示：

其中，pO(x)表[0,示1]空间任意一点x的占据概率，pO(x)∈[0,1]，为包含点x的正元体的占据密度，为包含点x的负体元的占据密度，占据密度是单位体积内数据点的个数，unknown表示点x的占据状态未知。

步骤402)判断占据状态：将pO(x)与[0阈,1值]进行比较，如果pO(x)大[0于,则点x处于占据状态，如果pO(x)小[0于,1或]等于则点x处于空隙状态。

有益效果：本发明针对移动机器人三维地形环境建模方法中需要考虑的克服传感器自身误差以及位姿推算误差等不确定因素影响的鲁棒性、地形描述的准确性、地形模型的可更新性以及地形模型的存储效率等问题，提出一种基于激光雷达的全3D占据体元地形建模方法。本发明根据测量误差建立起体元模型，增强了地形模型的可靠性。在空间描述上，将空间划分为占据、空闲和未知状态，形成全3D地形模型，为机器人的导航提供可靠的环境信息。对于地形模型的在线更新，本发明提供一种可以随时插入新体元的体元链表描述方法，并通过定义占据质量与占据密度来应对环境模型的变化，从而保证地形模型的实时更新性。对于地形模型的存储，本发明将三维空间进行离散划分，以保证单位分辨率内的数据能够得到有效压缩，从而提高存储效率。本发明可满足路径规划、仿真建模、自主无人驾驶等不同领域的广泛需求，具有广阔的应用前景和良好的经济效益。

附图说明

图1为本发明实施例的流程图；

图2为本发明实施例中移动机器人坐标系模型示意图；

图3为本发明实施例中激光扫描极坐标变换示意图；

图4(a)为本发明实施例中正体元模型表示方法示意图；

图4(b)为本发明实施例中负体元模型表示方法示意图；

图5为本发明实施例中三维激光投影到xy平面的光线跟踪示意图；

图6为本发明实施例中最小体元约束施加示意图；

图7(a)为两个体元部分交叠的融合示意图；

图7(b)为一个体元完全位于另一个体元中的融合示意图；

图8为体元间距大小约束施加示意图。

具体实施方式

下面结合附图和具体实施例，对本发明做进一步说明。

如图1所示，本发明实施例的一种基于激光雷达的全3D占据体元地形建模方法，包括以下步骤：

步骤10)数据的获取与坐标转换：首先采用安装在移动机器人平台上的激光雷达设备获取周边环境的深度信息；其次采用坐标变换将激光测量数据转化为全局坐标系中的点云数据，并对测量过程中存在的多源不确定性，采用区间集员的方式进行融合和处理；

步骤20)建立体元：对于光线起点到终点的空隙区域，根据光线跟踪算法，计算其所经网格的入点和出点，并构建负体元表示空闲状态；对于终点位置区域，根据计算得到的区间边界信息，将其关联到若干网络，并构建相应正体元表示障碍物占据状态，从而将全空间划分为占据、空闲和未知三种状态表示；

步骤30)基于约束的体元更新：将步骤20)建立的正体元或负体元加入到相关联网格的现有体元链表中；然后根据体元链表所需满足的体元高度约束、不相交约束和间距约束三个约束进行更新处理；

步骤40)测算占据状态：对步骤30)更新后的体元占据状态的概率进行计算，判断占据的状态。

在上述实施例中，所述的步骤10)包括以下步骤

步骤101)数据采集：采用安装在移动机器人平台上的激光雷达设备对周围环境进行扫描，获得地形感知的原始数据。如图2所示，激光雷达设备为安装在移动机器人平台上向地面倾斜一定角度的二维SICK LMS激光雷达。图2中，OW是全局坐标系，XW,YW,ZW是全局坐标系的三个坐标轴；Om是机器人坐标系，Xm,Ym,Zm是机器人坐标系的三个坐标轴；Ol是激光器坐标系，Xl,Yl,Zl是激光器坐标系的三个坐标轴；α是激光器与水平面的夹角，r是激光扫描的距离。通过二维SICK LMS激光雷达获得地形感知的原始数据。

步骤102)数据的坐标变换：进行坐标变换，将原始数据转化为全局坐标系下的数据信息。为了将原始数据转化为全局坐标系下的数据信息，需要对激光数据进行坐标变换。如图2所示，从激光传感器到全局坐标的齐次变换为：

^WTl＝^WTm^mTl 式(1)

其中，^WTl表示激光坐标系到全局坐标系的齐次坐标变换，^WTm表示移动机器人自身坐标系到全局坐标系的齐次坐标变换，^mTl表示移动机器人从激光坐标系到移动机器人自身坐标系的齐次坐标变换。

如图3所示激光扫描平面上的测量，其中ρ表示激光传感器的测量值，即是激光传感器到障碍物之间的距离，β表示激光的扫描角度。激光传感器获取的距离数据和扫描角度对应到激光坐标系的齐次坐标R(ρ,β)，如式(2)所示：

R(ρ,β)＝(ρ·sβ,ρ·cβ,0,1)^T 式(2)

其中，s表示sin函数的简称，c表示cos函数的简称，

根据式(3)得到激光数据在全局坐标系中的坐标：

x＝(x,y,z,1)^T＝^WTm·^mTl·R(ρ,β) 式(3)

其中，x表示激光数据在全局坐标系中的坐标，x,y,z,表示激光数据在全局坐标系x、y、z坐标轴的坐标值。

步骤103)区间集元估计：测量数据在全局坐标系中的位置，如式(4)所示：

其中，表示测量数据在全局坐标系三个坐标轴的坐标值，上标T表示转置，表示机器人坐标系相对于全局坐标系的位姿估计，α表示激光扫描平面与水平面夹角，rL表示某一时刻激光传感器的测量值，表示激光的扫描角度，

依据式(6)确定测量数据在全局坐标系的定界区间：

其中，Ix表示测量数据在全局坐标系的定界区间，[x]表示测量数据在全局坐标系x轴的区间范围，[y]表示测量数据在全局坐标系y轴的区间范围，[z]表示测量数据在全局坐标系z轴的区间范围，[P]表示参数的变化范围，[rL]表示参数rL的变化范围，[α]表示参数α的变化范围，β表示激光的扫描角度。

在上述实施例中，所述的步骤20)具体包括：

步骤201)：负体元相关网络关联，具体包括步骤2011)—2026)：

步骤2011)设激光数据在全局坐标系下的起点位置坐标xL以及终点位置坐标x′L，如图5所示，将从xL到x′L的激光光线投影到xy平面并执行光线跟踪，得到一组被激光穿过的网格单元列表其中，表示激光起点所在的网络单元，表示激光经过的网络单元，表示激光终点所在的网络单元；

步骤2012)建立光线跟踪的模型，如下式所示：

xL+t*d

其中，xL表示激光的起点位置坐标，t表示变量，length表示光线长度，0≤t≤length，d表示射线的单位方向向量；

步骤2013)若和在同一网络单元，则将的光线进入高度值赋为激光起点位置高度值zL，即该数据点不再计算，并建立负体元，否则，进入步骤2014)；

步骤2014)根据起点和终点计算出射线的单位方向向量d，设d[0]为d在x轴方向的分量，d[1]为d在y轴方向的分量，d[2]为d在z轴方向的分量，tMax[0]表示x轴方向穿过下一个网络边界的t值，tMax[1]表示y轴方向穿过下一个网络边界的t值；

步骤2015)C_ray中每个网络单元ci,j对应垂直空间上光线进入的高度值为该网络单元的激光起点高度zL，即取tMax[0]和tMax[1]中的较小值，即tMax[dim]←min{tMax[0],tMax[1]}，tMax[dim]表示选取x，y轴主要方向的t值，即选取x轴、y轴方向中的较小值；则激光在该网络单元的射出高度为

步骤2016)以网络链表中的网络为底，以光线跟踪算法得到的高度为高，建立负体元，如图4b所示，负体元顶面如公式(6)所示，底面如公式(7)所示：

其中，表示负体元的顶面高度；表示负体元的底面高度。

步骤202)：正体元相关网络关联：根据不确定度形成的激光终点立方体区域，得到一组被该区域覆盖的网络单元列表，确定与终点相关联的网络单元其中，表示激光终点所在的单元，表示相邻的激光终点单元，表示最后一个激光终点单元，对C_end中的每个网络单元ci,j创建一个与之对应的正体元vp，如图4a所示，vp的顶面高度为激光数据在z轴方向的区间上界，vp的底面高度为激光数据在z轴方向的区间下界。

在上述实施例中，所述的步骤30)具体包括：

步骤301)：将体元插入体元链表：对网络单元创建的正体元或负体元，分别插入对应的体元链表中；

步骤302)：约束限制：根据激光数据创建新体元后，遍历所有被更新的体元链表，并满足以下三个约束条件：

高度约束：每个体元的高度大于或等于网格分辨率res；如图6所示，如果体元的高度小于res，则进行膨胀处理，使高度达到res。膨胀处理具体包括：依据式(8)或式(9)将任何高度小于res的体元vx替换为高度等于res的体元vx^*：

其中，表示被替换体元的顶面高度，表示被替换体元的底面高度，表示替换体元的顶面高度，表示替换体元的底面高度。

不相交约束：同一体元链表中的任何两个体元不交叠。如图7(a)所示，如果出现交叠，则进行体元融合；如图7(b)所示，如果出现一个体元在另一个体元之内，则进行融合。进行融合具体包括：依据式(10)和式(11)，将需要进行融合处理的体元vx^A和vx^B替换为新体元vx^*：

vx^*＝vx^A∪vx^B 式(10)

其中，表示融合后体元的质量，表示待融合体元A的质量，表示待融合体元B的质量。

间距约束：同一体元链表中的任意两个体元的间距大于res；如图8所示，如果两个体元的间距小于或等于res，则进行融合。进行体元融合具体包括：在不满足间距约束的体元vx^A与vx^B之间创建一个新体元vx^GAP，以填补体元vx^A与vx^B间的空间，并将vx^GAP的密度初始化为1，然后将所述三个体元融合为一个新体元vx^*，如式(12)和式(13)所示：

vx^*＝vx^A∪vx^GAP∪vx^B 式(12)

其中，表示间隙体元的质量。

在上述实施例中，所述的步骤40)具体包括：

步骤401)计算体元占据概率：对于空间任意一点，其占据值根据该点所在的正体元和负体元的占据密度来计算，如式(14)所示：

其中，pO(x)表[0,示1]空间任意一点x的占据概率，pO(x)∈[0,1]，为包含点x的正元体的占据密度，为包含点x的负体元的占据密度，占据密度是单位体积内数据点的个数，unknown表示点x的占据状态未知。

步骤402)判断占据状态：将pO(x)与[0阈,1值]进行比较，如果pO(x)大[0于,则点x处于占据状态，如果pO(x)小[0于,1或]等于则点x处于空隙状态。

应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。