CN102353379B - 一种适用于自动驾驶车导航的环境建模方法 - Google Patents

Abstract

本发明为一种适用于自动驾驶车导航的环境建模方法。自动驾驶车的导航需解决的关键问题之一是建模车辆行驶的环境，辨识其中的景物；将环境信息转化为自动驾驶车智能避障、路径规划可用的参数化信息。本发明采取在自动驾驶车的前端装配激光传感器；测量激光传感器中心与车辆中心的空间距离等一系列步骤；利用车辆行驶过程中采集的激光点序列建模整个环境。通过配准激光传感器在相邻采样时刻的观测求取车辆的位移和航向角，是一种自包含的方案，可有效避免同类方法中所使用的基于星座系统的方案在极端环境中信号丢失的问题；基于概率图模型推理的方法处理激光束中的激光点序列，智能地利用并管理了景物轮廓的几何特征，因此对环境建模的精确度更高。

Description

一种适用于自动驾驶车导航的环境建模方法

技术领域

本发明涉及到车辆导航领域，具体地说，特别涉及到一种适用于自动驾驶车导航的环境建模方法。

背景技术

自动驾驶车导航技术广泛应用于安全辅助驾驶、外星球探索、国防自动化等领域。自动驾驶车的导航需要解决的关键问题之一是如何建模车辆行驶的环境，辨识其中的景物。将环境信息转化为自动驾驶车智能避障、路径规划任务中可利用的参数化信息。该类参数化信息中以环境景物的轮廓方位的利用效率最高。为了得到这些信息，同类方法使用视觉、红外、超声波传感器。但这些传感器较容易受光照、浮尘等环境因素的影响，同时还需解决观测配准、校正方面的难题。相比之下，激光传感器具有全天候、精度高、装配简单的特点。但同类应用中大多从激光束中提取出所描述的景物中的点、线、面等几何基元作为建模环境的显著特征，这在真实的应用中缺乏普适性。

经对现有技术文献的检索发现，Dissanayake等人在《IEEE Transactions onRobotics and Automation》2001，17卷，229-241页上发表了“A solution tothe simultaneous localization and map building(SLAM)problem”(《机器人和自动化学报》，一种同时定位与地图构建方案)。该文介绍了一种基于毫米波雷达用于车辆导航的定位和环境建模方法。该方法存在的主要问题是：使用环境中的少量点特征作为环境建模的主要依据，这不适用于特别复杂的环境，例如，很难将包含大量点、线、面特征的都市环境表征为有限个几何基元的集合；同时，该方法所构建的过于稀疏的环境模型对于车辆自主导航和智能避障的利用价值有限，很难在实际中应用。

综上所述，针对现有技术的缺陷，特别需要一种适用于自动驾驶车导航的环境建模方法，以解决现有技术的不足。

发明内容

本发明针对上述现有技术的不足和缺陷，提供了一种适用于自动驾驶车导航的环境建模方法，解决了背景技术中存在的不足。

本发明所解决的技术问题可以采用一种自动驾驶车导航的环境建模方法来实现，包括如下步骤：

1)在自动驾驶车的前端装配激光传感器，当车辆行驶时激光扫描传感器开始按照固定频率采集激光束观测；

2)测量激光传感器中心与车辆中心的空间距离；

3)确定车辆当前采样时刻在地球中心坐标系下的位置。首次运行系统时，需借助其它传感器观测车辆在地球中心坐标系下的初始位置；

4)预处理车辆行驶过程中在相邻两采样时刻所采集的两束激光束中返回的距离读数序列，具体步骤为：

a.剔除两组激光束的读数序列中大于传感器极限观测距离的距离读数；

b.基于激光传感器视角分辨率分别将每束激光束转换为一组以传感器中心坐标系下极坐标表征的点序列；

c.将每组激光点序列的极坐标转换为传感器中心坐标系下的笛卡尔坐标；

5)通过寻找步骤4)中两组相邻激光束中激光点的一一对应的关系，求取两个相邻时刻间车辆行驶的位移和航向角，具体步骤为：

a.构建一个链式条件随机场图模型，并将激光点序列及其关联关系分别映射为该链式条件随机场的节点和状态；

b.提取两激光点序列间的几何特征作为条件随机场图模型的局部特征和配对特征；

c.基于从标定数据学习得到的特征权重，通过图模型的概率推理求取两个相邻激光点序列中激光点之间一一对应的关系；

d.通过优化所有关联激光点的距离累加和最小求取相邻时刻间车辆行驶的位移和航向角。

6)将步骤5)中求得的车辆行驶的位移和航向角与前一采样时刻车辆的绝对位置合成出当前采样时刻车辆在地球中心坐标系下的绝对位置；

7)将当前采样时刻传感器中心坐标系下的以笛卡尔座标表征的激光点序列与步骤5)中求取的车辆在地球中心坐标系下的绝对位置合成，求取激光点序列在地球中心坐标系下的坐标；

8)根据步骤2)中测得的激光传感器中心与车辆中心的空间距离作为传感器中心坐标系原点与车辆中心坐标系原点的空间偏移，进一步修正步骤6)中的激光点序列在地球中心坐标系下的坐标，存储激光点序列的坐标，这些激光点坐标描绘了当前环境景物的距地面一定高度的横截面轮廓；

9)运行步骤3)至步骤8)直到车辆导航结束；车辆行驶全程中的激光点序列建模了整个环境。

有益效果

与现有技术相比，本发明具有如下有益效果：

(1)本发明所提供方法无需像同类方法(见引证文献)那样提取环境中的几何基元作为特征，因此适用于城市街道等复杂环境；

(2)通过配准激光传感器在相邻采样时刻的观测求取车辆的位移和航向角，是一种自包含的方案，可有效避免同类方法中所使用的基于星座系统(例如：GPS)的方案在极端环境中信号丢失的问题；

(3)通过概率图模型推理的方法处理激光束中的激光点序列，智能地利用并管理了景物轮廓的几何特征，因此对环境建模的精确度更高。

附图说明

下面结合附图与实施案例进一步说明本发明。

图1本发明的流程示意图；

图2本发明所使用的三种坐标系的示意图；

图3本发明相邻两时刻测量的两束激光束中激光点关联关系的示意图；

图4本发明实施例中测试车辆行驶的轨迹与卫星图像匹配的效果图；

图5本发明实施例中所建模的环境轮廓的效果图；

图6本发明实施例中所建模的环境轮廓与卫星图像匹配的效果图。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体图示，进一步阐述本发明。

本实施例的测试环境为城市街道，测试中的车辆的整体车程为1410m，平均车速为40km/h。

如图1所示，本发明的实施例包括如下步骤：

第一步，在自动驾驶车的前端装配激光传感器。本实施例的实验中使用民用轿车作为测试车辆，使用SICK(施克)LMS 221为测试激光传感器，并安装于测试车辆的前端距地面1.2m处，面向车辆向前行驶的方向。本实施例配置其视角分辨率为0.5度，即每束激光束包含361个激光点，其有效量程为80m。LMS221使用直流24V供电，最大输入电流为1.8A。数据的输出接口为RS-232或RS-422。本实施例中使用RS 232协议通过车载电脑接收并存储激光传感器数据。当车辆行驶时激光扫描传感器开始按照固定频率采集激光束观测；

第二步，测量激光传感器中心与车辆中心的空间距离，图2所示，本发明所提供方法中共需要三种坐标系：地球中心坐标系G，车辆中心坐标系V和传感器中心坐标系S。本实施例中，激光传感器安装于车辆前端，其与车辆中心的空间距离作为传感器中心坐标系S在车辆中心坐标系V中的空间偏移，为x_sv＝1.62m，y_sv＝0.45m，r_sv＝5°；

第三步，确定车辆当前采样时刻在地球中心坐标系下的位置。首次运行系统时，需借助其它传感器观测车辆在地球中心坐标系下的初始位置。为了获得车辆在初始状态的绝对位置，本实施例中测试车辆还装配了GPS传感器，且GPS传感器的天线基本与车辆中心重合，因此，GPS测量值可认为是车辆中心坐标系原点在地球中心坐标系下的坐标，即车辆的绝对位置；

第四步，预处理车辆行驶过程中在相邻两采样时刻所采集的两束激光束中返回的距离读数序列r，具体步骤为：

a.剔除两组激光束的读数序列中大于传感器极限距离的距离读数，本实施例中激光传感器测距的极限距离为80m，故剔除掉返回距离大于80m的读数；

b.基于激光传感器视角分辨率分别将每束激光束转换为一组以传感器中心坐标系下极坐标表征的点序列。本实施例中，激光传感器视角分辨率为0.5度，每束激光束包含361个激光点，则初始激光点序列对应的极坐标中的偏角为：

φ＝{φ₁，φ₂，φ₃，…，φ₃₆₀}＝{0°，0.5°，1.0°，…，180°}

基于a中剔除激光点的序号，可得到对应不超限距离序列的偏角；

c.将每组激光点序列的极坐标转换为传感器中心坐标系下的笛卡尔坐标。

对于一个极坐标(γ，φ)的激光点，它的笛卡尔坐标(x，y)可依据如下求得：

x＝r×cosφ，y＝r×sinφ

第五步，通过寻找第四步求得的两组相邻激光束中激光点的一一对应的关系，求取两个相邻时刻间车辆行驶的位移和航向角，具体步骤为：

a.构建一个链式条件随机场图模型，并将激光点序列及其关联关系分别映射为该链式条件随机场的节点和状态；该步骤仅需运行一次即可。本实施例中所构建的链式条件随机场图模型只包含两种势函数：局部势和配对势。其中将参考激光束和新激光束所包含激光点作为观测量z。参考激光束包含N个激光点

它们各自的可能的关联状态映射为条件随机场图中的节点。新激光束包含M个激光点

条件随机场图中的任一节点x_i都对应了M+1种状态的似然。例如，x_i的状态1表示x_i所对应的参考激光束中的激光点与新激光束的激光点

关联的似然。x_i状态2表示与新激光束的激光点

关联的似然，依次类推，状态M+1表示

在新激光束中找不到相关联的激光点的似然，即，

是孤立点的似然。

b.提取两激光点序列间的几何特征作为条件随机场图模型的局部特征和配对特征。本实施例中提取两束激光束中激光点之间的距离、角、测地线和辐射等几何特征作为局部特征，提取配对序列、配对孤立点和配对距离为配对特征；

c.基于从标定数据学习得到的特征权重，通过图模型的概率推理求取两个相邻激光点序列中激光点之间一一对应的关系。本实施例中的特征最佳权重依据20组标定数据并使用最大伪似然(Maximum pseudo-likelihood，MPL)学习方法得到，激光点间的关联关系通过最大和(Max-sum)概率推理得到。如图3所示，相邻两束激光束建立了关联关系；

d.通过优化所有关联激光点的距离累加和最小求取相邻时刻间车辆行驶的位移T＝[T_x，T_y]和航向角R。本实施例中，车辆的位移和航向角组成车辆在相邻两时刻间的相对运动RT＝[R，T]，它可通过优化下式来求得闭合的解：

$\mathrm{RT}\←\arg \underset{R,T}{\min }\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left|\right|{\mathrm{RL}}_{\mathrm{new}}^{S_i}+T-L_{\mathrm{ref}}^i\left|\right|$

其中，S_i代表构建一条激光点关联配置时，为参考激光束中激光点

所对应节点选取的状态，它是新激光束中一个激光点的索引。

第六步，将第五步中求得的车辆行驶的相对运动RT与前一采样时刻车辆的绝对位置合成出当前采样时刻车辆在地球中心坐标系下的绝对位置；具体为：新时刻车辆所处的纬度

与参考时刻车辆所处的纬度

间的关系为：

新时刻车辆所处位置的经度λ_new与参考时刻车辆所处的经度λ_ref间的关系为：

其中，R_g为地球半径。

第七步，将当前采样时刻传感器中心坐标系下的以笛卡尔座标表征的激光点序列与第六步中求取的车辆在地球中心坐标系下的绝对位置合成，求取激光点序列在地球中心坐标系下的坐标；

第八步，根据第二步中测得的激光传感器中心与车辆中心的空间距离作为传感器中心坐标系原点与车辆中心坐标系原点的偏移量，进一步修正第七步中的激光点序列在地球中心坐标系下的坐标，存储激光点序列的坐标，这些激光点坐标描绘了当前环境景物的距地面一定高度的横截面轮廓；

第九步，循环运行第三步至第八步直到车辆导航结束。车辆行驶过程中的激光点序列建模了整个环境。

本实施例的测试车辆行驶过程中共测得激光束4092束，通过相邻的激光束两两配对的方式共得到4091组激光束对，测试车辆行驶的轨迹配准在卫星影像上的效果图示于图4。使用本发明所提供的方法建模的环境示于图5，它反应了测试车辆行驶的环境中距地面1.2m处的横截面轮廓。为了验证环境建模的效果，将本实施例所建模的环境轮廓与实地卫星图像匹配，效果图示于图6。由图可看出，本发明所提供方法所建模出的环境轮廓与真实景物有良好的吻合度。由于本发明所提供方法无需在环境中寻找显著的基元作为环境建模的特征，本发明所提供方法还可广泛应用于其它包含不规则景物的复杂环境，因此具有普适性。

以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都在要求保护的本发明范围内，本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (2)

1.一种适用于自动驾驶车导航的环境建模方法 ,包括如下步骤：

1）在自动驾驶车的前端装配激光扫描传感器，当车辆行驶时激光扫描传感器开始按照固定频率采集激光束观测；

2）测量激光扫描传感器中心与车辆中心的空间距离； 

3）确定车辆当前采样时刻在地球中心坐标系下的位置，首次运行系统时，需借助其它传感器观测车辆在地球中心坐标系下的初始位置；

4）预处理车辆行驶过程中在相邻两采样时刻所采集的两束激光束中返回的距离读数序列； 

5）通过寻找步骤4）中两组相邻激光束中激光点的一一对应的关系，求取两个相邻时刻间车辆行驶的位移和航向角；

所述步骤5)具体步骤为：

a．构建一个链式条件随机场图模型，并将激光点序列及其关联关系分别映射为该链式条件随机场的节点和状态；

b．提取两激光点序列间的几何特征作为条件随机场图模型的局部特征和配对特征；

c．基于从标定数据学习得到的特征权重，通过图模型的概率推理求取两个相邻激光点序列中激光点之间一一对应的关系；

d．通过优化所有关联激光点的距离累加和最小求取相邻时刻间车辆行驶的位移和航向角；

6）将步骤5）中求得的车辆行驶的位移和航向角与前一采样时刻车辆的绝对位置合成出当前采样时刻车辆在地球中心坐标系下的绝对位置；

7）将当前采样时刻传感器中心坐标系下的以笛卡尔座标表征的激光点序列与步骤5）中求取的车辆在地球中心坐标系下的绝对位置合成，求取激光点序列在地球中心坐标系下的坐标；

8）根据步骤2）中测得的激光扫描传感器中心与车辆中心的空间距离作为传感器中心坐标系原点与车辆中心坐标系原点的空间偏移，进一步修正步骤6）中的激光点序列在地球中心坐标系下的坐标，存储激光点序列的坐标，这些激光点坐标描绘了当前环境景物的距地面一定高度的横截面轮廓；

9）运行步骤3）至步骤8）直到车辆导航结束，车辆行驶全程中的激光点序列建模了整个环境。

2.根据权利要求1所述的环境建模方法，其特征在于：所述步骤4)具体步骤为：

a．剔除两组激光束的读数序列中大于所使用激光扫描传感器极限观测距离的距离读数；

b．基于激光扫描传感器视角分辨率分别将每束激光束转换为一组以传感器中心坐标系下极坐标表征的点序列；

c．将每组激光点序列的极坐标转换为传感器中心坐标系下的笛卡尔坐标。

Images