专利名称：一种适用于自动驾驶车导航的环境建模方法
技术领域：
本发明涉及到车辆导航领域，具体地说，特别涉及到一种适用于自动驾驶车导航的环境建模方法。
背景技术：
自动驾驶车导航技术广泛应用于安全辅助驾驶、外星球探索、国防自动化等领域。 自动驾驶车的导航需要解决的关键问题之一是如何建模车辆行驶的环境，辨识其中的景物。将环境信息转化为自动驾驶车智能避障、路径规划任务中可利用的参数化信息。该类参数化信息中以环境景物的轮廓方位的利用效率最高。为了得到这些信息，同类方法使用视觉、红外、超声波传感器。但这些传感器较容易受光照、浮尘等环境因素的影响，同时还需解决观测配准、校正方面的难题。相比之下，激光传感器具有全天候、精度高、装配简单的特点。但同类应用中大多从激光束中提取出所描述的景物中的点、线、面等几何基元作为建模环境的显著特征，这在真实的应用中缺乏普适性。经对现有技术文献的检索发现，Dissanayake等人在《IEEE Transactions on Robotics and Automation》2001，17 卷，2四_241 页上发表了 “A solution to the simultaneous localization and map building(SLAM)problem，，(〈〈iiL:||人禾口自动学 艮〉〉， 一种同时定位与地图构建方案)。该文介绍了一种基于毫米波雷达用于车辆导航的定位和环境建模方法。该方法存在的主要问题是使用环境中的少量点特征作为环境建模的主要依据，这不适用于特别复杂的环境，例如，很难将包含大量点、线、面特征的都市环境表征为有限个几何基元的集合；同时，该方法所构建的过于稀疏的环境模型对于车辆自主导航和智能避障的利用价值有限，很难在实际中应用。综上所述，针对现有技术的缺陷，特别需要一种适用于自动驾驶车导航的环境建模方法，以解决现有技术的不足。
发明内容
本发明针对上述现有技术的不足和缺陷，提供了一种适用于自动驾驶车导航的环境建模方法，解决了背景技术中存在的不足。本发明所解决的技术问题可以采用一种自动驾驶车导航的环境建模方法来实现， 包括如下步骤1)在自动驾驶车的前端装配激光传感器，当车辆行驶时激光扫描传感器开始按照固定频率采集激光束观测；2)测量激光传感器中心与车辆中心的空间距离；3)确定车辆当前采样时刻在地球中心坐标系下的位置。首次运行系统时，需借助其它传感器观测车辆在地球中心坐标系下的初始位置；4)预处理车辆行驶过程中在相邻两采样时刻所采集的两束激光束中返回的距离读数序列，具体步骤为
a.剔除两组激光束的读数序列中大于传感器极限观测距离的距离读数；b.基于激光传感器视角分辨率分别将每束激光束转换为一组以传感器中心坐标系下极坐标表征的点序列；c.将每组激光点序列的极坐标转换为传感器中心坐标系下的笛卡尔坐标；5)通过寻找步骤4)中两组相邻激光束中激光点的一一对应的关系，求取两个相邻时刻间车辆行驶的位移和航向角，具体步骤为a.构建一个链式条件随机场图模型，并将激光点序列及其关联关系分别映射为该链式条件随机场的节点和状态；b.提取两激光点序列间的几何特征作为条件随机场图模型的局部特征和配对特征；c.基于从标定数据学习得到的特征权重，通过图模型的概率推理求取两个相邻激光点序列中激光点之间一一对应的关系；d.通过优化所有关联激光点的距离累加和最小求取相邻时刻间车辆行驶的位移和航向角。6)将步骤幻中求得的车辆行驶的位移和航向角与前一采样时刻车辆的绝对位置合成出当前采样时刻车辆在地球中心坐标系下的绝对位置；7)将当前采样时刻传感器中心坐标系下的以笛卡尔座标表征的激光点序列与步骤5)中求取的车辆在地球中心坐标系下的绝对位置合成，求取激光点序列在地球中心坐标系下的坐标；8)根据步骤2)中测得的激光传感器中心与车辆中心的空间距离作为传感器中心坐标系原点与车辆中心坐标系原点的空间偏移，进一步修正步骤6)中的激光点序列在地球中心坐标系下的坐标，存储激光点序列的坐标，这些激光点坐标描绘了当前环境景物的距地面一定高度的横截面轮廓；9)运行步骤幻至步骤8)直到车辆导航结束；车辆行驶全程中的激光点序列建模了整个环境。有益效果与现有技术相比，本发明具有如下有益效果(1)本发明所提供方法无需像同类方法(见引证文献)那样提取环境中的几何基元作为特征，因此适用于城市街道等复杂环境；(2)通过配准激光传感器在相邻采样时刻的观测求取车辆的位移和航向角，是一种自包含的方案，可有效避免同类方法中所使用的基于星座系统(例如GPQ的方案在极端环境中信号丢失的问题；(3)通过概率图模型推理的方法处理激光束中的激光点序列，智能地利用并管理了景物轮廓的几何特征，因此对环境建模的精确度更高。
下面结合附图与实施案例进一步说明本发明。图1本发明的流程示意图；图2本发明所使用的三种坐标系的示意图3本发明相邻两时刻测量的两束激光束中激光点关联关系的示意图；图4本发明实施例中测试车辆行驶的轨迹与卫星图像匹配的效果图；图5本发明实施例中所建模的环境轮廓的效果图；图6本发明实施例中所建模的环境轮廓与卫星图像匹配的效果图。
具体实施例方式为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体图示，进一步阐述本发明。本实施例的测试环境为城市街道，测试中的车辆的整体车程为1410m，平均车速为 40km/ho如图1所示，本发明的实施例包括如下步骤第一步，在自动驾驶车的前端装配激光传感器。本实施例的实验中使用民用轿车作为测试车辆，使用SICK (施克)LMS 221为测试激光传感器，并安装于测试车辆的前端距地面1. 2m处，面向车辆向前行驶的方向。本实施例配置其视角分辨率为0. 5度，即每束激光束包含361个激光点，其有效量程为80m。LMS221使用直流24V供电，最大输入电流为 I-SA0数据的输出接口为RS-232或RS-422。本实施例中使用RS 232协议通过车载电脑接收并存储激光传感器数据。当车辆行驶时激光扫描传感器开始按照固定频率采集激光束观测；第二步，测量激光传感器中心与车辆中心的空间距离，图2所示，本发明所提供方法中共需要三种坐标系地球中心坐标系G，车辆中心坐标系V和传感器中心坐标系S。本实施例中，激光传感器安装于车辆前端，其与车辆中心的空间距离作为传感器中心坐标系S 在车辆中心坐标系V中的空间偏移，为xsv = 1. 62m, ysv = 0. 45m, rsv = 5° ；第三步，确定车辆当前采样时刻在地球中心坐标系下的位置。首次运行系统时，需借助其它传感器观测车辆在地球中心坐标系下的初始位置。为了获得车辆在初始状态的绝对位置，本实施例中测试车辆还装配了 GPS传感器，且GPS传感器的天线基本与车辆中心重合，因此，GPS测量值可认为是车辆中心坐标系原点在地球中心坐标系下的坐标，即车辆的绝对位置；第四步，预处理车辆行驶过程中在相邻两采样时刻所采集的两束激光束中返回的距离读数序列r，具体步骤为a.剔除两组激光束的读数序列中大于传感器极限距离的距离读数，本实施例中激光传感器测距的极限距离为80m，故剔除掉返回距离大于80m的读数；b.基于激光传感器视角分辨率分别将每束激光束转换为一组以传感器中心坐标系下极坐标表征的点序列。本实施例中，激光传感器视角分辨率为0. 5度，每束激光束包含 361个激光点，则初始激光点序列对应的极坐标中的偏角为φ = {φ” φ2, φ3，…，φ 360} = {0° ，0.5° ，1.0° ，...，180° }基于a中剔除激光点的序号，可得到对应不超限距离序列的偏角；c.将每组激光点序列的极坐标转换为传感器中心坐标系下的笛卡尔坐标。对于一个极坐标(Y，Φ)的激光点，它的笛卡尔坐标(X，y)可依据如下求得χ = rX cos Φ , y = rX sin Φ
第五步，通过寻找第四步求得的两组相邻激光束中激光点的一一对应的关系，求取两个相邻时刻间车辆行驶的位移和航向角，具体步骤为a.构建一个链式条件随机场图模型，并将激光点序列及其关联关系分别映射为该链式条件随机场的节点和状态；该步骤仅需运行一次即可。本实施例中所构建的链式条件随机场图模型只包含两种势函数局部势和配对势。其中将参考激光束和新激光束所包含激光点作为观测量ζ。参考激光束包含N个激光点{/丄f. I· ·■■■⑶， 它们各自的可能的关联状态映射为条件随机场图中的节点。新激光束包含M个激光点 {Llw. I:i..w. .... I^U。条件随机场图中的任一节点Xi都对应了 Μ+1种状态的似然。例如，Xi的状态1表示Xi所对应的参考激光束中的激光点 4ι与新激光束的激光点关联的似然。Xi状态2表示￡“与新激光束的激光点/.iL关联的似然，依次类推，状态M+1表示
在新激光束中找不到相关联的激光点的似然，即，化r是孤立点的似然。b.提取两激光点序列间的几何特征作为条件随机场图模型的局部特征和配对特征。本实施例中提取两束激光束中激光点之间的距离、角、测地线和辐射等几何特征作为局部特征，提取配对序列、配对孤立点和配对距离为配对特征；c.基于从标定数据学习得到的特征权重，通过图模型的概率推理求取两个相邻激光点序列中激光点之间一一对应的关系。本实施例中的特征最佳权重依据20组标定数据并使用最大伪似然(Maximum pseudo-likelihood, MPL)学习方法得到，激光点间的关联关系通过最大和(Max-sum)概率推理得到。如图3所示，相邻两束激光束建立了关联关系；d.通过优化所有关联激光点的距离累加和最小求取相邻时刻间车辆行驶的位移 T = [Tx, Ty]和航向角R。本实施例中，车辆的位移和航向角组成车辆在相邻两时刻间的相对运动RT = [R，T]，它可通过优化下式来求得闭合的解
权利要求
1.一种适用于自动驾驶车导航的环境建模方法，包括如下步骤1)在自动驾驶车的前端装配激光传感器，当车辆行驶时激光扫描传感器开始按照固定频率采集激光束观测；2)测量激光传感器中心与车辆中心的空间距离；3)确定车辆当前采样时刻在地球中心坐标系下的位置，首次运行系统时，需借助其它传感器观测车辆在地球中心坐标系下的初始位置；4)预处理车辆行驶过程中在相邻两采样时刻所采集的两束激光束中返回的距离读数序列；5)通过寻找步骤4)中两组相邻激光束中激光点的一一对应的关系，求取两个相邻时刻间车辆行驶的位移和航向角；6)将步骤幻中求得的车辆行驶的位移和航向角与前一采样时刻车辆的绝对位置合成出当前采样时刻车辆在地球中心坐标系下的绝对位置；7)将当前采样时刻传感器中心坐标系下的以笛卡尔座标表征的激光点序列与步骤5) 中求取的车辆在地球中心坐标系下的绝对位置合成，求取激光点序列在地球中心坐标系下的坐标；8)根据步骤2)中测得的激光传感器中心与车辆中心的空间距离作为传感器中心坐标系原点与车辆中心坐标系原点的空间偏移，进一步修正步骤6)中的激光点序列在地球中心坐标系下的坐标，存储激光点序列的坐标，这些激光点坐标描绘了当前环境景物的距地面一定高度的横截面轮廓；9)运行步骤幻至步骤8)直到车辆导航结束，车辆行驶全程中的激光点序列建模了整个环境。
2.根据权利要求1所述的环境建模方法，其特征在于所述步骤4)具体步骤为a.剔除两组激光束的读数序列中大于传感器极限观测距离的距离读数；b.基于激光传感器视角分辨率分别将每束激光束转换为一组以传感器中心坐标系下极坐标表征的点序列；c.将每组激光点序列的极坐标转换为传感器中心坐标系下的笛卡尔坐标。
3.根据权利要求1所述的环境建模方法，其特征在于所述步骤幻具体步骤为a.构建一个链式条件随机场图模型，并将激光点序列及其关联关系分别映射为该链式条件随机场的节点和状态；b.提取两激光点序列间的几何特征作为条件随机场图模型的局部特征和配对特征；c.基于从标定数据学习得到的特征权重，通过图模型的概率推理求取两个相邻激光点序列中激光点之间一一对应的关系；d.通过优化所有关联激光点的距离累加和最小求取相邻时刻间车辆行驶的位移和航向角。
全文摘要
本发明为一种适用于自动驾驶车导航的环境建模方法。自动驾驶车的导航需解决的关键问题之一是建模车辆行驶的环境，辨识其中的景物；将环境信息转化为自动驾驶车智能避障、路径规划可用的参数化信息。本发明采取在自动驾驶车的前端装配激光传感器；测量激光传感器中心与车辆中心的空间距离等一系列步骤；利用车辆行驶过程中采集的激光点序列建模整个环境。通过配准激光传感器在相邻采样时刻的观测求取车辆的位移和航向角，是一种自包含的方案，可有效避免同类方法中所使用的基于星座系统的方案在极端环境中信号丢失的问题；基于概率图模型推理的方法处理激光束中的激光点序列，智能地利用并管理了景物轮廓的几何特征，因此对环境建模的精确度更高。
文档编号G01C21/26GK102353379SQ20111018818
公开日2012年2月15日 申请日期2011年7月6日 优先权日2011年7月6日
发明者孙作雷, 曾连荪, 杨宁 申请人:上海海事大学