CN108398672B - 基于前倾2d激光雷达移动扫描的路面与障碍检测方法 - Google Patents

Abstract

基于前倾2D激光雷达移动扫描的路面与障碍检测方法，涉及城市环境中移动机器人的环境感知技术。包括一个坐标系定义与坐标转换的步骤、一个在雷达坐标系中进行扫描点分割的步骤和一个将线段划分为障碍物段和路面段的步骤。针对当前室外移动机器人自主导航中的道路区域和障碍物检测，及采用3D激光雷达带来的难以承受的价格昂贵问题，从传感器原始数据中提取线段，然后估计每一时刻扫描路面的高度和矢量；最后根据每一线段的平均高度和估计的扫描道路向量的线段的偏差，将线段划分为地面和障碍部分。该方法具有快速和稳定可靠特点，解决了必须采用3D激光雷达进行环境3D扫描的问题，为提高室外移动机器人导航的安全性。

Description

基于前倾2D激光雷达移动扫描的路面与障碍检测方法

技术领域

本发明涉及城市环境中移动机器人的环境感知技术，尤其是涉及基于前倾2D激光雷达 移动扫描的路面与障碍检测方法。

背景技术

环境感知，尤其是对道路路面和存在的障碍物进行检测，是实现服务机器人、自动驾驶 汽车、快递机器人以及智能轮椅自主、安全导航的关键技术之一。目前，环境检测传感器主 要采用摄像头、RGB-D深度相机和激光雷达等。

虽然基于摄像头的道路和障碍物检测方法具有操作功耗低、成本低、信息含量高的优点， 但当遇到复杂的阴影或恶劣的天气条件时，检测效果会大大降低，鲁棒性差；此外，相机无 法获得路面的高精度3D信息，需要与激光扫描雷达进行结合，实现多传感器融合的环境检测。 RGB-D深度相机可以同时获得图像和深度信息，但检测距离有限，易受阳光干扰，只适合于 室内环境。

激光雷达传感器由于其良好的距离分辨率、高精度、范围大和良好的方位估计能力等， 被广泛应用于智能车的环境及障碍物的检测。激光雷达传感器可分为二维激光雷达和三维激 光雷达。三维激光雷达可以获取障碍物和周围环境的丰富信息，但它的数据量大、复杂度高、 处理时间长，而且三维激光雷达价格昂贵，对于小型智能车(快递机器人、智能轮椅等)几乎 无法承受。与三维激光雷达相比，二维(2D)激光雷达结构简单，价格低，可以采用向前倾 斜安装的方法，随着机器人的运动，进行对地移动扫描，获得路面的3D信息，进而用于道路 和障碍物的检测。

参考文献：

[1]段志刚,李勇,王恩德,田建东,唐延东.基于光照不变图像的阴影图像道路及导航线提取算法[J]. 光学学报,36(12):1-13,2016.

[2]张文.基于多传感器融合的室内机器人自主导航方法研究[D].中国科学技术大学,2017.

[3]王肖,李克强,王建强,徐友春.基于三维激光雷达的智能车辆目标参数辨识[J].汽车工程,38(9): 1146-1152,2016.

[4]王超,王欢,赵春霞.基于形状先验和在线鉴别性分析的道路检测[J].北京理工大学学报,(12): 1257-1261,2014..

[5]黄如林,梁华为,陈佳佳,等.基于激光雷达的无人驾驶汽车动态障碍物检测、跟踪与识别方法[J]. 机器人,2016,38(4):437-443.

[6]段建民,任璐,王昶人,等.基于四线激光雷达的道路信息提取与目标检测[J].激光杂志,2017, 38(6):32-37.

[7]段建民,郑凯华,李龙杰,等.基于多层激光雷达的道路信息提取算法[J].控制工程,2016, 23(4):468-473.

[8]刘钊.无人车2D激光雷达结构化环境感知技术研究[D].国防科学技术大学,2013.

[9]俞奇奇,崔振山.一种基于2D激光雷达的扫描系统标定方法[J].激光与红外,2017(10).

发明内容

本发明针对室外移动机器人自主导航中的道路区域和障碍物检测，及采用3D激光雷达 带来的难以承受的价格昂贵问题，提供适用于小型无人车的基于前倾2D激光雷达移动扫描 的路面与障碍检测方法。

本发明包括以下步骤：

1)坐标系定义与坐标转换；

在步骤1)中，所述坐标系定义与坐标转换的具体方法可为：将2D激光雷达扫描测距仪 安装在移动机器人的正前方，向地面倾斜的角度为α，定义激光雷达的坐标系为 F_L(O_L,θ,l,X_L,Y_L)，O_L是激光发射源点，(θ，l)是扫描点的极坐标，θ为扫描角度，l为扫描距离，(X_L,Y_L)是扫描点在该坐标系下的笛卡尔坐标，扫描的开始角θ_min＝θ₁、结束角θ_max＝θ_N，扫 描角度θ_j，扫描距离l_j，角度分辨率Δθ＝θ_j-θ_j-1；机器人的坐标系定义为F_R(O_R,X_R,Y_R,Z_R)，O_R是机器人后轮与地面的接触点，(X_R,Y_R,Z_R)是扫描点在该坐标系下的笛卡尔坐标；世界坐标系 定义为F_W(O_W,X_W,Y_W,Z_W)，O_W是机器人初始时刻后轮与地面的接触点，(X_W,Y_W,Z_W)是扫描点在 该坐标系下的笛卡尔坐标；机器人的2D位姿

分别为时刻t_i机器人在世界 坐标系中的位置坐标和方向角；ΔH是激光安装的垂直高度，ΔX是激光相对于机器人坐标系 原点的水平位移；

定义2D激光雷达在时刻t_i的扫描数据帧为：

其中，(θ_ij,l_ij)为该时刻第j个障碍点在F_L中的极坐标，

为笛卡尔坐标，i表示时刻t_i； 障碍点从雷达坐标系到全局笛卡尔坐标的转化关系为：

其中，

表示第j障碍点在F_W中的全局坐标，从F_L到F_W旋转变换矩阵为：

2)在雷达坐标系的扫描分割；

在步骤2)中，所述在雷达坐标系的扫描分割的具体方法可为：在t_i时刻，从2D激光雷 达得到的一帧数据是极坐标形式，其对应的每一障碍点都有自己的序号(ij)，且在雷达坐标系 下都是二维的(θ_ij,l_ij)或

先在激光雷达坐标系下做扫描分割，分割结束后，再将所有 的线段根据它们端点的序号结合式(3)转换到世界坐标系下，具体步骤如下：

(1)断点检测

根据式(1)，对

的一个初始分割定义为：

S_iT＝{(θ_ik,l_ik),n_T＜k＜n_T+1},1＜T＜m (5)

该点云被分为m个部分，采用一个自适应的阈值：

其中，λ是一个辅助参数，σ_l是一个方差，用于表示扫描点

的随机性和l_ij的噪声；所述阈 值依赖于l_ij，比固定阈值更具灵活性，能用于多个场景下的断点检测；

初步的分割和断点检测定义为：

其中，p_ij为t_i时刻的第j个扫描点，||p_i(j+1)-p_ij||是两个连续点p_i(j+1)和p_ij之间的欧氏距离；

(2)直线提取

对于每个S_iT如包含的点少于一定数量(如8个)即去除，以去除分割出来的线段中存在 的噪点；接着采用IEPF算法再次对S_iT进行分割，做直线提取；d_max是IEPF算法中点到直线 的最大距离阈值，最后结合式(3)得到h条在世界坐标系下的线段：

其中，l_it为t_i时刻分割得到的第t条线段，s_it,e_it是线段l_it的起点和未点，在整个分割算法结束 后，一个完整的扫描点序列

变成了一对的t^th值的集合{(s_it,e_it)}，它们代表每条线段的起点 和未点；

对于每条线段，提取出它的特性：

其中，l_it的平均高度

l_it在F_W中的起点

l_it在F_W中的 终点

l_it在F_L中的起点

l_it在F_L中的终点

l_it的矢 量

l_it的长度

3)障碍物检测；

在步骤3)中，所述障碍物检测的具体方法可为：由于只用到机器人的2D位姿

整个人机器人相当于行驶在一个假定的水平面上，具体步骤如下：

(1)路面高度估计

在机器人的运动过程中，对每一帧扫描到的地面进行高度估算，因机器人起始在平坦的 道路上，第一帧的路面高度hight(t₀)取第一帧中间部分数据点的平均高度；对于t_i＞t₀，增加角 度范围，并对存在的障碍物点进行滤波，以获得更高精度的地面高度值：当前时刻数据帧内 的点的高度值

与上一时刻估算出来的路面高度进行比较，如果高度差大于一个阈值δ_th， 那么去除这个点；所有点的比较完后，将剩余点求平均高度值，即为对应的路面高度；

整体道路高度估计算法描述如表1所示：

表1

(2)路面矢量提取

机器人起始在无障碍路面上行驶，可选取第一帧里最长那段作为第一帧扫描到的路面的 路面矢量：

当t_i＞t₀，根据当前帧所获得的路面线段集RL_i，先进行线段滤波然后拟合出对应的路面向量；

RL_i中的线段有两个特性：方向角和长度，用于拟合路面向量的线段的方向角相对于所获 得的路面向量方向角的偏移量

对于长度，大于一个最小线长度L_min的线段用于路面向量估算；如果RL_i里面的线段的方向角 的偏移量大于一个最大阈值φ_max或者它的长度比L_min小，即去除，以过滤噪声线段；对于剩下 的线段，取出它们的端点于集合Θ，然后通过最小二乘法进行直线拟合，对于拟合的直线， 取两个端点的x坐标为x_s＝-1.5,x_e＝1.5，得到两个端点

将它们转换到F_W，即获得路面 向量

整体提取道路向量算法描述如表2所示：

表2

(3)障碍物提取

障碍物提取基于两个特性：每条分割出来的线段的平均高度和该线段相对于上一时刻提 取出来的路面向量的偏离程度；

在将扫描点分割成线段后，首先通过长度阈值l_min滤波；但高度值h_it大于阈值ξ_h的线段并 不都是障碍物段，还需要对线段与上一时刻的路面向量的偏离度进行考虑；用线段起点和末 点到上一路面向量的距离来表示它们之间的偏离度，只要这两个距离的任何一个超过一个最 小偏离值ξ_i,这条线段就被认为是一个障碍物段；

障碍物提取的算法如表3所示：

表3

在算法3中，OL_i,RL_i表示当前时刻的路面和障碍物线段集；ξ_i由下式表示：

其中，Δt＝t_i+1-t_i是机器人姿态和扫描数据采样的时间间隔，V_i是当前时刻机器人的速度，

表 示偏离量。

本发明从传感器原始数据中提取线段，然后估计每一时刻扫描路面的高度和矢量；最后 根据每一线段的平均高度和从前面估计的扫描道路向量的线段的偏差，将线段划分为地面和 障碍部分。

本发明涉及一种应用于快递机器人、智能轮椅等室外移动机器人自主、安全导航的环境 感知方法，主要包括一个坐标系定义与坐标转换的步骤、一个在雷达坐标系中进行扫描点分 割的步骤和一个将线段划分为障碍物段和路面段的步骤。针对当前室外移动机器人自主导航 中的道路区域和障碍物检测，及采用3D激光雷达带来的难以承受的价格昂贵问题，本发明 提供一种适用于小型无人车的基于前倾2D激光雷达移动扫描的路面与障碍检测方法。该方 法从传感器原始数据中提取线段，然后估计每一时刻扫描路面的高度和矢量；最后根据每一 线段的平均高度和估计的扫描道路向量的线段的偏差，将线段划分为地面和障碍部分。该方 法具有快速和稳定可靠特点，解决了必须采用3D激光雷达进行环境3D扫描的问题，为提高 室外移动机器人导航的安全性，提供了一种经济可靠和有精度保证的可行路面和障碍检测方 法。

附图说明

图1为机器人实际路面行驶扫描图。

图2为机器人在假定路面行驶扫描图。在图2中，标记A为机器人假定行驶路面，B为单帧激光扫描路面，C为整个激光扫描路面。

图3为坐标系定义和相关变量示意图。

图4为激光扫描坐标系示意图。

图5为IEPF算法提取直线示意图。在图5中，标记A为原始数据点，B为断点，C为提取的直线。

图6为实施例1的实验扫描原始数据。

图7为实施例1的实验处理结果图。

图8为实施例1的实验障碍物提取结果图。

具体实施方式

下面结合附图和实施例对本发明的具体实施方式进行说明。

本发明包括以下步骤：

1)坐标系定义与坐标转换：

将2D激光雷达扫描测距仪安装在移动机器人的正前方，向地面倾斜的角度为α，定义激 光雷达的坐标系为F_L(O_L,θ,l,X_L,Y_L)，O_L是激光发射源点，(θ，l)是扫描点的极坐标，θ为扫描 角度，l为扫描距离，(X_L,Y_L)是扫描点在该坐标系下的笛卡尔坐标，扫描的开始角θ_min＝θ₁、 结束角θ_max＝θ_N，扫描角度θ_j，扫描距离l_j，角度分辨率Δθ＝θ_j-θ_j-1；机器人的坐标系定义 为F_R(O_R,X_R,Y_R,Z_R)，O_R是机器人后轮与地面的接触点，(X_R,Y_R,Z_R)是扫描点在该坐标系下的 笛卡尔坐标；世界坐标系定义为F_W(O_W,X_W,Y_W,Z_W)，O_W是机器人初始时刻后轮与地面的接触点， (X_W,Y_W,Z_W)是扫描点在该坐标系下的笛卡尔坐标；机器人的2D位姿

分 别为时刻t_i机器人在世界坐标系中的位置坐标和方向角；ΔH是激光安装的垂直高度，ΔX是 激光相对于机器人坐标系原点的水平位移；

定义2D激光雷达在时刻t_i的扫描数据帧为：

其中，(θ_ij,l_ij)为第j障碍点在F_L中的极坐标，

为笛卡尔坐标，i表示时刻t_i。障碍点从 雷达坐标系到全局笛卡尔坐标的转化关系为：

其中，

表示第j障碍点在F_W中的全局坐标。从F_L到F_W旋转变换矩阵为：

以上坐标系定义方法的细节可参见图3，图4的图示说明。

2)在雷达坐标系的扫描分割

在t_i时刻，从2D激光雷达得到的一帧数据是极坐标形式，其对应的每一障碍点都有自己 的序号(ij)，且在雷达坐标系下都是二维的(θ_ij,l_ij)或

这比在世界坐标系下的三维坐标

处理速度快很多。因此，先在激光雷达坐标系下做扫描分割，分割结束后，再将所 有的线段根据它们端点的序号结合式(3)转换到世界坐标系下。

(1)断点检测

根据式(1)，对

的一个初始分割定义为：

S_iT＝{(θ_ik,l_ik),n_T＜k＜n_T+1},1＜T＜m (5)

该点云被分为m个部分。采用一个自适应的阈值：

其中，λ是一个辅助参数，σ_l是一个方差，用来表示扫描点

的随机性和l_ij的噪声。该阈值 依赖于l_ij，比固定阈值更具灵活性，能用于多个场景下的断点检测。

初步的分割和断点检测定义为：

其中，p_ij为t_i时刻的第j个扫描点，||p_i(j+1)-p_ij||是两个连续点p_i(j+1)和p_ij之间的欧氏距离；

(2)直线提取

对于每个S_iT如它包含的点少于一定数量(如8个)就将它去掉，以去除分割出来的线段中 存在的噪点；接下来采用IEPF算法再次对S_iT进行分割，做直线提取。d_max是IEPF算法中点 到直线的最大距离阈值。最后结合式(3)得到h条在世界坐标系下的线段

其中，l_it为t_i时刻分割得到的第t条线段，s_it,e_it是线段l_it的起点和未点。在整个分割算法结束 后，一个完整的扫描点序列

变成了一对的tt^h值的集合{(s_it,e_it)}，它们代表每条线段的起点 和未点。

对于每条线段，提取出它的特性：

其中，l_it的平均高度

l_it在F_W中的起点

l_it在F_W中的 终点

l_it在F_L中的起点

l_it在F_L中的终点

l_it的矢 量

l_it的长度

以上IEPF算法提取直线的细节可参见图5的图示说明。

3)障碍物检测方法

由于只用到机器人的2D位姿

整个人机器人就相当于行驶在一个假定的水平 面上，尽管它与实际的路面有些差异，但同样也能反映出路面和障碍物的真实情况。

(1)路面高度估计

在机器人的运动过程中，对每一帧扫描到的地面进行高度估算。因机器人一般起始在平 坦的道路上，第一帧的路面高度hight(t₀)取第一帧中间部分数据点的平均高度。对于t_i＞t₀，适 当增加角度范围，并对一些可能存在的障碍物点进行滤波以获得更高精度的地面高度值：当 前时刻数据帧内的点的高度值

与上一时刻估算出来的路面高度进行比较，如果高度差大 于一个阈值δ_th，那么去除这个点；所有点的比较完后，将剩余点求平均高度值，即为对应的 路面高度。

整体道路高度估计算法描述如表1所示。

(2)路面矢量提取

机器人起始在无障碍路面上行驶，可选取第一帧里最长那段作为第一帧扫描到的路面的 路面矢量：

当t_i＞t₀，根据当前帧所获得的路面线段集RL_i，先进行线段滤波然后拟合出对应的路面向量。

RL_i中的线段有两个特性：方向角和长度。用于拟合路面向量的线段的方向角相对于之前 时刻所获得的路面向量方向角的偏移量

不能太大：

对于长度，通常大于一个最小线长度L_min的线段更适合用于路面向量估算。

因此，如果RL_i里面的线段的方向角的偏移量大于一个最大阈值φ_max或者它的长度比L_min小，就将它移除掉，以过滤掉一些噪声线段，利于提高精确和处理速度。对于剩下的线段， 取出它们的端点于Θ，然后通过最小二乘法进行直线拟合，对于拟合出来的直线，取两个端 点的x坐标为x_s＝-1.5,x_e＝1.5，得到两个点

将它们转换到F_W，即可获得路面向量

整体提取道路向量算法描述如表2所示

(3)障碍物提取

障碍物提取主要是基于两个特性：每条分割出来的线段的平均高度和该线段相对于上一 时刻提取出来的路面向量的偏离程度。

在将扫描点分割成线段后，首先通过长度阈值l_min滤波；但高度值h_it大于阈值ξ_h的线段并 不都是障碍物段，还需要对线段与上一时刻的路面向量的偏离度进行考虑。本发明用线段起 点和未点到上一路面向量的距离来表示它们之间的偏离度，只要这两个距离的任何一个超过 了一个最小偏离值ξ_i,这条线段就被认为是一个障碍物段。

障碍物提取的算法如表3所示。

在算法3中，OL_i,RL_i表示当前时刻的路面和障碍物线段集；ξ_i由下式表示：

其中，Δt＝t_i+1-t_i是机器人姿态和扫描数据采样的时间间隔，V_i是当前时刻机器人的速度，

表示偏离量。

以下给出具体实施例。

本发明基于前倾2D激光雷达移动扫描的路面与障碍检测方法，用于城市室外环境的移 动机器人导航的道路和障碍物的检测，具体实施的操作过程如下。

1、以先锋机器人为实验平台，采用2D激光雷达扫描测距仪LMS111，安装在机器人的 正前方且向地面倾斜。各参数取值如表4所示：

表4

2、根据发明内容中的方法和算法，以笔记本电脑作为机器人的上位机(Windows系统和 VC++环境)，按照以下步骤进行编程实现。

2.1、定义激光雷达坐标系，机器人坐标系，世界坐标系(机器人起始点)；

2.2、在机器人的整个运动过程中，在每一采样时刻t_i，对获得的一帧扫描数据，在激光 雷达坐标系下做扫描分割，即：①使用式(5)～(7)进行断点检测，②去除分割出来的线段中存 在的噪点，③采用IEPF算法再次对S_iT进行分割做直线提取，④结合式(3)可以得到在世界坐 标系下的线段(式(8))，⑤对于每条线段，提取出它的特性(式(9))；

2.3、进行障碍物检测，将线段划分为障碍物段和路面段，即：①对每一帧扫描到的地面 进行高度估算，获得在整个移动扫描过程之后的地面高度数据(算法1)，②根据每一帧分 割出来的线段，提取出当前帧所对应的路面向量(算法2)，③依据每条分割出来的线段的 平均高度和该线段相对于上一时刻提取出来的路面向量的偏离程度，进行障碍物提取(算法 3)。

3、显示路面和障碍检测结果。

测试环境如下描述，在右下方是一个花坛，右前方停了两辆车，正前方有两个人和一个 箱子，左边有一些路障。

检测结果如图6～8所示，总共有约1000帧扫描数据，机器人的行驶速度为0.37m/s。图 6是原始扫描数据，图7是障碍物检测结果，灰色部分表示路面可通行区域，红色部分表示 障碍物区域，蓝色的机器人行驶的路径。图8是障碍物提取结果。其中1号对应的是场景中 的右下方的花坛，2号和3号对应于右前方的两辆汽车，5号6号是正前方的两个人，4号是 正前方的箱子，7，8，9，10号是左边的路障，从检测结果中，可以看到，在8号和9号路 障间少了一个，这是由于机器人在前进扫描的过程中，扫描线被4号箱子给挡住了，除此之外，该场景路面及两边的障碍物情况都被检测出来了。由于左边有一个路阶，使得左边区域较高，扫描出的结果呈现大量红色部分，然而这对机器人的前进行驶并不会产生影响。

Claims (1)

1.基于前倾2D激光雷达移动扫描的路面与障碍检测方法，其特征在于包括以下步骤：

1)坐标系定义与坐标转换，具体方法为：将2D激光雷达扫描测距仪安装在移动机器人的正前方，向地面倾斜的角度为α，定义激光雷达的坐标系为F_L(O_L,θ,l,X_L,Y_L)，O_L是激光发射源点，(θ，l)是扫描点的极坐标，θ为扫描角度，l为扫描距离，(X_L,Y_L)是扫描点在该坐标系下的笛卡尔坐标，扫描的开始角θ_min＝θ₁、结束角θ_max＝θ_N，扫描角度θ_j，扫描距离l_j，角度分辨率Δθ＝θ_j-θ_j-1；机器人的坐标系定义为F_R(O_R,X_R,Y_R,Z_R)，O_R是机器人后轮与地面的接触点，(X_R,Y_R,Z_R)是扫描点在该坐标系下的笛卡尔坐标；世界坐标系定义为F_W(O_W,X_W,Y_W,Z_W)，O_W是机器人初始时刻后轮与地面的接触点，(X_W,Y_W,Z_W)是扫描点在该坐标系下的笛卡尔坐标；机器人的2D位姿

分别为时刻t_i机器人在世界坐标系中的位置坐标和方向角；ΔH是激光安装的垂直高度，ΔX是激光相对于机器人坐标系原点的水平位移；

定义2D激光雷达在时刻t_i的扫描点云数据帧为：

其中，(θ_ij,l_ij)为该时刻第j个障碍点在F_L中的极坐标，

为笛卡尔坐标，i表示时刻t_i；障碍点从雷达坐标系到全局笛卡尔坐标的转化关系为：

其中，

表示第j障碍点在F_W中的全局坐标，从F_L到F_W旋转变换矩阵为：

2)在雷达坐标系的扫描分割，具体方法为：在t_i时刻，从2D激光雷达得到的一帧数据是极坐标形式，其对应的每一障碍点都有自己的序号(ij)，且在雷达坐标系下都是二维的(θ_ij,l_ij)或

先在激光雷达坐标系下做扫描分割，分割结束后，再将所有的线段根据它们端点的序号结合式(3)转换到世界坐标系下，具体步骤如下：

(1)断点检测

根据式(1)，对点云

的一个初始分割定义为：

S_iT＝{(θ_ik,l_ik),n_T＜k＜n_T+1},1＜T＜m (5)

该点云被分为m个部分，采用一个自适应的阈值：

其中，λ是一个辅助参数，σ_l是一个方差，用于表示扫描点

的随机性和l_ij的噪声；所述阈值依赖于l_ij，比固定阈值更具灵活性，能用于多个场景下的断点检测；

初步的分割和断点检测定义为：

其中，p_ij为t_i时刻的第j个扫描点，||p_i(j+1)-p_ij||是两个连续点p_i(j+1)和p_ij之间的欧氏距离；

(2)直线提取

对于每个S_iT如包含的点少于一定数量即去除，以去除分割出来的线段中存在的噪点；接着采用IEPF算法再次对S_iT进行分割，做直线提取；d_max是IEPF算法中点到直线的最大距离阈值，最后结合式(3)得到h条在世界坐标系下的线段：

其中，l_it为t_i时刻分割得到的第t条线段，s_it,e_it是线段l_it的起点和未点，在整个分割算法结束后，一个完整的扫描点序列

变成了一对的tt^h值的集合{(s_it,e_it)}，它们代表每条线段的起点和未点；

对于每条线段，提取出它的特性：

其中，l_it的平均高度

l_it在F_W中的起点

l_it在F_W中的终点

l_it在F_L中的起点

l_it在F_L中的终点

l_it的矢量

l_it的长度

3)障碍物检测：由于只用到机器人的2D位姿

整个人机器人相当于行驶在一个假定的水平面上，具体步骤如下：

(1)路面高度估计

在机器人的运动过程中，对每一帧扫描到的地面进行高度估算，因机器人起始在平坦的道路上，第一帧的路面高度hight(t₀)取第一帧中间部分数据点的平均高度；对于t_i＞t₀，增加角度范围，并对存在的障碍物点进行滤波，以获得更高精度的地面高度值：当前时刻数据帧内的点的高度值

与上一时刻估算出来的路面高度进行比较，如果高度差大于一个阈值δ_th，那么去除这个点；所有点的比较完后，将剩余点求平均高度值，即为对应的路面高度；

(2)路面矢量提取

机器人起始在无障碍路面上行驶，选取第一帧里最长那段作为第一帧扫描到的路面的路面矢量：

当t_i＞t₀，根据当前帧所获得的路面线段集RL_i，先进行线段滤波然后拟合出对应的路面向量；

RL_i中的线段有两个特性：方向角和长度，用于拟合路面向量的线段的方向角相对于所获得的路面向量方向角的偏移量θ_it：

对于长度，大于一个最小线长度L_min的线段用于路面向量估算；如果RL_i里面的线段的方向角的偏移量大于一个最大阈值φ_max或者它的长度比L_min小，即去除，以过滤噪声线段；对于剩下的线段，取出它们的端点于集合Θ，然后通过最小二乘法进行直线拟合，对于拟合的直线，取两个端点的x坐标为x_s＝-1.5,x_e＝1.5，得到两个端点

将它们转换到F_W，即获得路面向量

(3)障碍物提取

障碍物提取基于两个特性：每条分割出来的线段的平均高度和该线段相对于上一时刻提取出来的路面向量的偏离程度；

在将扫描点分割成线段后，首先通过长度阈值l_min滤波；但高度值h_it大于阈值ξ_h的线段并不都是障碍物段，还需要对线段与上一时刻的路面向量的偏离度进行考虑；用线段起点和末点到上一路面向量的距离来表示它们之间的偏离度，只要这两个距离的任何一个超过一个最小偏离值ξ_i,这条线段就被认为是一个障碍物段；

OL_i,RL_i表示当前时刻的路面和障碍物线段集；ξ_i由下式表示：

其中，Δt＝t_i+1-t_i是机器人姿态和扫描数据采样的时间间隔，V_i是当前时刻机器人的速度，

表示偏离量。

Images