基于核函数的实时地形估计方法，该方法所基于的硬件实现平 台是无人车平台，无人车平台上装备有64线3D激光雷达，INS-GPS组合导航单元等传感器。
[0046] 基于以上平台，典型工作流程首先需要进行系统初始化，包括：车辆底层初始化， 传感器参数初始化，车辆姿态位置初始化，地形矩阵建立，变尺度核函数模板建立；然后判 断地形估计任务是否结束，若是直接结束该方法，否则进入循环的系统工作流程，每个处理 周期需要包括以下几个步骤，如图1所示：
[0047] 一、获取实时激光点云数据和车辆当前状态相对于绝对空间的旋转平移矩阵；
[0048] 二、利用所述旋转平移矩阵对实时激光点云数据进行配准，并对配准后的点云数 据采用基于体素的降采样处理；
[0049] 三、遍历历史点云库中的每一点，采用点约束建立描述被估计区域地形的地形矩 阵 MAP ;
[0050] 四、利用实时点云数据对所述地形矩阵MAP进行点约束和光线约束，将此时获得 的地形矩阵MAP作为当前无人车周围的地形估计，输出该栅格地形图；
[0051] 五、将所述配准后的实时点云数据添加到历史点云库中，更新历史点云数据。
[0052] 下面具体介绍每个环节的工作过程：
[0053] -、获取实时激光点云数据和车辆当前状态相对于绝对空间的旋转平移矩阵。
[0054] 激光雷达通过发射激光束扫描周边环境采样得到激光点云数据PCn，其中每个点 云数据的格式为PCnj= (X，Y，Z)，其中X，Y，Z为该点相对于激光雷达的坐标值。从INS-GPS 获得无人车辆当前的位姿数据，所述位姿数据包括绝对位置（E，N)和当前姿态角（a，0， Y );然后结合上一帧车辆的位姿数据，能够得到无人车辆当前状态相对于绝对空间的旋转 平移矩阵Cn。
[0055] 二、配准和预处理实时点云数据。
[0056] 由于车体运动导致传感器倾斜，传感器也在跟随载体运动，要综合利用多帧数据， 由于每一帧点云数据皆为相对激光雷达的坐标值，因此需要对多帧点云数据进行坐标转 换，转换成相对于绝对空间的坐标值，实现点云配准，即利用旋转平移矩阵(；对PC n进行配 准PCna= PCn*Cn，经过配准后的PCna所属的XY平面平行于水平面，X，Y分别指向东向和北 向，坐标原点（经度，炜度）处于（E，N) = (0,0)。
[0057] 图4显示了单帧点云数据的分布特点（90度范围），由于其航向角分辨率是固定 值，而扫描距离不同，势必造成了单帧点云数据近处密度非常高，而远处分辨率较低；在距 离较远的区域内，径向分布密度比切向分布密度衰减的更快，即在距离较远时，点与点之间 的切向距离远远小于径向距离。
[0058] 本发明采用基于体素的降采样处理对配准后的点云进行预处理。将车体周围有点 云的三维空间平均分成尺寸为m个L*W*H的小块体素V，然后对每个体素包含的原点云进行 统计，再以每个体素中的点云数据的重心在近似代表该体素中的其他点云，这样该体素就 可以用一个重心点来表示。有效降低高密度点云区的点云数量，而不减少低密度区的点云 数量，能够在缩减点云规模的同时最大程度保持原点云信息，同时使得点云整体密度更加 平均，如图5所示。
[0059] 三、遍历历史点云库建立描述被估计区域地形的地形矩阵MAP。
[0060] 地形矩阵MAP是一个2M*2M的矩阵描述了被估计区域的地形，M为预设参数，矩阵 的行和列分别表示东向和北向方向距离，矩阵所表示的区域为以无人车为中心的边长（2M* 栅格分辨率）的正方形区域，无人车位置在矩阵中心（M，M)处，矩阵的每个元素的值表示地 形高度。具体的建立过程为：
[0061 ] 用QHj (xHj，yHj，zHj)表示历史点云库中的某点，MAP表示地形矩阵，P表示MAP中的 行列值，点约束关系可以表示为：
[0062]
【主权项】
1. 一种基于核函数的实时地形估计方法，其特征在于，具体过程为： 步骤一，获取实时点云数据和车辆当前状态相对于绝对空间的旋转平移矩阵； 步骤二，利用所述旋转平移矩阵对实时点云数据进行配准，并对配准后的点云数据采 用基于体素的降采样处理； 步骤三，遍历历史点云库中的每一点，采用点约束建立描述被估计区域地形的地形矩 阵MAP; 步骤四、利用实时点云数据对所述地形矩阵MP进行点约束和光线约束，将此时获得 的地形矩阵MP作为当前无人车周围的地形估计； 步骤五、将所述配准后的实时点云数据添加到历史点云库中，更新历史点云数据。
2. 根据权利要求1所述基于核函数的实时地形估计方法，其特征在于，所述点约束的 过程为： (1)选取被用于建立点约束的一个点I，判断Qm是否在地形矩阵MP的覆盖范围之 内，如果在，则根据点QHj的坐标（XHj，yHj)从地形矩阵MAP中找到离QHj最近的元素PHj; ⑵读取地形矩阵MP在PHj处的高度值MAP(PHj)，若IMAP(PHj) -zHjI>Zptostold，则对地 形矩阵MP的Piu点添加适合半径的径向基核函数模板，否则保持高度值MAP(PHp不变，其 中，zHj表示点QHj的高度值，Zpthrestold表示点约束高度差阈值； (3)按照步骤（1)-(2)的方式遍历所有被用于建立点约束的点，构建出被估计区域地 形的地形矩阵MAP。
3. 根据权利要求1所述基于核函数的实时地形估计方法，其特征在于，所述光线约束 的过程为：针对实时点云数据上的任意点，连接其与激光投射原点，计算连线上沿途地形矩 阵MP的高度是否高于该连线，若存在光线投射路径上的任意点Qscd= (xsc;i，ysc;i，Zsc;i)满足 Map(Pscd)-zsc;iI>ZltoslTOld，其中，Pscd表示从地形矩阵MP中找到离Qscd最近的元素，zsc;i表 示连线上点Qscd的高度值，ZlthMstold表示光线高度差阈值，则对地形矩阵MP的Psc;i点添加 适合半径的径向基核函数模板，否则保持地形矩阵MP不变。
4. 根据权利要求2或3所述基于核函数的实时地形估计方法，其特征在于，所述合适半 径的设定准则为： 第一，在地形锐利的边角处和有突兀特征的地区，采用半径较小的径向基函数模板，在 平坦区域采用半径较大的径向基函数模板； 第二，在点云较为密集的地方，采用半径较小的径向基函数模板，在点云较为稀疏的地 方，采用半径较大的径向基函数模板。
5. 根据权利要求1所述基于核函数的实时地形估计方法，其特征在于，所述更新历史 点云库过程为： a. 若更新次数n= 1，直接将配准后的第一帧数据作为历史点云库； b. 更新次数n>l时，将配准后的？乙与PC(n_1)a直接合并得到新的点云集PCntraipl = PC(n-l)a+PCna； C.对PCntejmpl进行尺寸范围裁剪，只保留无人车周围设定范围内的点云，得到PCntejmp2; d. 在PCntemp2随机删除一定比例的点云得到PCntemp3; e. 对PCntemp3进行基于体素的将采样处理，至此点云库的周期更新完成得到更新后的点 云库PCns。
【专利摘要】本发明提供一种基于核函数的实时地形估计方法，具体过程为：获取实时点云数据和车辆当前状态相对于绝对空间的旋转平移矩阵；利用所述旋转平移矩阵对实时点云数据进行配准，并对配准后的点云数据采用基于体素的降采样处理；遍历历史点云库中的每一点，采用点约束建立描述被估计区域地形的地形矩阵MAP；利用实时点云数据对所述地形矩阵MAP进行点约束和光线约束，将此时获得的地形矩阵MAP作为当前无人车周围的地形估计；将所述配准后的实时点云数据添加到历史点云库中，更新历史点云数据。该方法能适用于无人车高实时性、海量数据的地形估计，解决运动中盲区的补偿，并给出可以在线更新的显式估计结果。
【IPC分类】G06F19-00, G06T7-00, G01S17-89
【公开号】CN104820982
【申请号】CN201510199728
【发明人】杨毅, 李星河, 付梦印, 朱昊
【申请人】北京理工大学
【公开日】2015年8月5日
【申请日】2015年4月23日