基于概率分析的环境构建方法
【技术领域】
[0001] 本发明涉及机器人或无人车技术领域，具体涉及一种基于概率分析的环境构建方 法
【背景技术】
[0002] 环境构建主要是指机器人对外部未知环境进行自主识别与理解，构建或恢复真实 的环境信息，是机器人进行自主导航所必需的关键技术，具有重要的研究意义与应用价值。 由于激光雷达测距与定位精度高、性价比高等特点，利用激光雷达构建未知的环境成为重 要的研究内容。在获取激光雷达点云数据后，传统的环境建模方法是通过带高斯噪声概率 误差模型补偿位置误差，从不精确的数据中恢复精确的环境模型，并假定下一时刻，同一目 标会被激光雷达重复扫描。
[0003] 而实际中，由于点云数据的有限性，恢复的环境模型是不精确的也不会出现点云 数据重叠。因此，如何依据有限的点云数据，准确地构建环境成为亟待解决的技术问题。
【发明内容】
[0004] (一)要解决的技术问题
[0005] 本发明要解决的技术问题是:如何依据有限的点云数据，准确地构建环境。
[0006] (二)技术方案
[0007] 为了解决上述技术问题，本发明提供了一种基于概率分析的环境构建方法，包括 以下步骤：
[0008] S1、建立机器人的位姿误差模型，获取激光雷达的点云数据；
[0009] S2、利用步骤S1建立的位姿误差模型，确定障碍物满足的概率统计测试；
[0010] S3、通过二维栅格单元及其邻域进行障碍物测试，生成地形分类地图，从而实现环 境构建。
[0011](三)有益效果
[0012] 本发明通过一种基于概率分析的环境构建方法利用激光雷达提供的点云数据，对 机器人当前的未知外部环境进行统计分析，将激光雷达采集的点云数据转换成可通行、未 知以及障碍物等三类状态的地形分类地图，从而实现准确地构建环境，提高了环境构建的 精度，为机器人自主导航时的障碍规避提供了可靠保障。
【具体实施方式】
[0013] 为使本发明的目的、内容、和优点更加清楚，下面结合实施例，对本发明的具体实 施方式作进一步详细描述。
[0014] 本发明实施例提供的一种基于概率分析的环境地图构建方法，包括以下步骤：
[0015] 步骤S1:建立机器人的位姿误差模型，获取激光雷达点云数据；
[0016] 为了补偿因机器人横滚角、俯仰角导致的分类误差，以一阶马尔科夫模型作为机 器人的位姿误差模型，该模型如式(1)所示：
[0018] 其中，（Xt，yt，zt)表示t时刻机器人的位置；（(^，0(，(^)表示七时刻机器人的姿态 角;wt表示随时间变化、服从正态分布:Ν(^，R)的高斯噪声;vt表示瞬间的、服从正态分布： N(0，Q)的高斯噪声;R和Q表示时不变的噪声协方差矩阵，分别用式(2)与式(3)表示：
[0022]定义激光雷达在t时刻第i个元素的观测数据为〇*/，观测距离为rf'，观测角为 #/,则激光雷达的点云数据(X/, 17, Z/):通过式(4)计算得到：
[0024] 其中，Tt表示t时刻由机器人姿态角构成的旋转矩阵;^与1表示激光雷达相对于 机器人局部坐标系的偏移量和方向角。
[0025] 步骤S2:确定基于概率统计的障碍物测试；
[0026] 利用步骤S1建立的一阶马尔科夫模型，将障碍物测试转化为基于概率统计的问 题。给定两点（XuSYuSZu1)和0丨 2;_，1/, Z;/)，t2>tl，高度差分的概率分布如式（5)所 示：
[0028] 汉（|笔-私| (^ + ? -^)母％(幻）表示正态分布，-Z引 值，w))7 (2ρ+(/: 表示方差。
[0029] 其中，表示值ZtklW的雅克比矩阵(k=l，2)，上标i(j)表示i或j。假定观 测角?^ = ?^并忽略激光雷达的偏移量Xs，则V/^Z)用式(6)近似表示。
[0031]合并式(5)和式(6)，得式(7):
[0033]因此，基于概率统计的障碍物测试表示为：
[0035] 其中，Η表示障碍物高度阈值;λ表示误差概率阈值，并令λ = 〇 . 03。则上式(8)可表 示为如式(9)所示：
[0037]由上式(9)可知，障碍物测试的概率随着时间差分的增加逐渐降低。因此，障碍物 测试的判断依据主要集中在激光雷达点云数据中邻近的两点。
[0038]步骤S3:通过二维栅格单元及其邻域进行障碍物测试，生成地形分类地图，从而实 现环境构建。
[0039]二维栅格地图包含了环境的有用信息。当分析一组新的点云数据时，基于概率分 析的方法需要相对应的2维栅格单元及其领域，其中，每个栅格单元存储一个相对较大的值 Zmax和一个相对较小的值Zmin。在栅格单元中当观测到一个新的数据时，分别与其领域中的 Zmax和Zmin比较，如果一个满足步骤2障碍物测试，则标记该栅格单元状态为障碍物；同时，在 栅格单元中，当观测值Z小于该Z_或大于Z max时，则分别用观测值Z替代。假定栅格单元初始 状态为未知状态，当有新的观测数据时，改变栅格单元及其邻域的状态为可通行状态，如果 栅格单元满足障碍物测试，则改变栅格单元及其邻域状态为障碍物，若不满足，仍为可通行 状态，没有观测数据的栅格单元为初始的未知状态，进而生成包含未知、可通行、障碍物三 种状态的地形分类地图。
[0040] 通过以上步骤，基于概率分析的环境构建方法将激光雷达观测到的点云数据转换 为包含可通行、未知以及障碍物等三种状态的栅格地图，为机器人自主导航时的障碍规避 提供可靠保障。
[0041] 以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人 员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形 也应视为本发明的保护范围。
【主权项】
1. 一种基于概率分析的环境构建方法，其特征在于，包括W下步骤： 51、 建立机器人的位姿误差模型，获取激光雷达的点云数据； 52、 利用步骤S1建立的位姿误差模型，确定障碍物满足的概率统计测试； 53、 通过二维栅格单元及其邻域进行障碍物测试，生成地形分类地图，从而实现环境构 建。2. 如权利要求1所述的方法，其特征在于，步骤S1中，W-阶马尔科夫模型作为机器人 的位姿误差模型，即式(1):(1) 其中，（xt，yt，zt)表示t时刻机器人的位置；（Φ t，0t，Φ t)表示t时刻机器人的姿态角;wt 表示随时间变化、服从正态分布:N(wt-i，R)的高斯噪声;V康示瞬间的、服从正态分布:N(0， Q)的高斯噪声;R和Q表示时不变的噪声协方差矩阵，分别用式(2)与式(3)表示：根据观测数据，得到准确的机器人位姿信息。 定义激光雷达在t时刻第i个元素的观测数据为〇/，/的)，观测距离为r/，观测角为#/'， 则激光雷达的点云数据07，Γ/，通过式(4)计算得到：(句 其中，Tt表示t时刻由机器人姿态角构成的旋转矩阵；Xs与Ts表示激光雷达相对于机器 人局部坐标系的偏移量和方向角。3. 如权利要求2所述的方法，其特征在于，步骤S2具体为： 给定两点(Xtii，Ytii，2*11)和<又乂片/，名/)，t2〉tl，高度差分的概率分布如式(5)所 示：(5)良不正态分布：表不均值， (▽:;i(z))'口 ρ+的-/.I)巧)7/:(2)表示方差； 其中，▽tkiW(Z)表示值ZtkiW的雅克比矩阵化=1，2)，上标i(j)表示i或j。假定观测角 0tii = i3t2^并忽略激光雷达的偏移量xs，则▽tkiW(Z)用式(6)近似表示；其中，Η表示障碍物高度阔值;λ表示误差概率阔值，并令λ = 〇.03。则上式(8)可表示为 式(9):(9)4. 如权利要求3所述的方法，其特征在于，步骤S3具体为:假定二维栅格单元初始状态 为未知状态，当有新的观测数据时，改变二维栅格单元及其邻域的状态为可通行状态，如果 二维栅格单元满足步骤S2的障碍物测试，则改变二维栅格单元及其邻域状态为障碍物，若 不满足，仍为可通行状态，没有观测数据的二维栅格单元为初始的未知状态，进而生成包含 未知、可通行、障碍物Ξ种状态的地形分类地图。5. 如权利要求4所述的方法，其特征在于，判断是否满足步骤S2的障碍物测试具体为： 二维栅格单元的每个栅格单元存储一个相对较大的值Zmax和一个相对较小的值Zmin，在栅格 单元中当观测到一个新的数据时，分别与其领域中的Zmax和Zmin比较，如果一个满足步骤S2 的障碍物测试，则标记该栅格单元状态为障碍物；同时，在栅格单元中，当观测值Z小于Zmin 或大于Zmax时，则用观测值Z替代Zmin或Zmax。
【专利摘要】本发明涉及一种基于概率分析的环境构建方法，属于机器人或无人车技术领域。本发明通过一种基于概率分析的环境构建方法利用激光雷达提供的点云数据，对机器人当前的未知外部环境进行统计分析，将激光雷达采集的点云数据转换成可通行、未知以及障碍物等三类状态的地形分类地图，从而实现准确地构建环境，提高了环境构建的精度，为机器人自主导航时的障碍规避提供了可靠保障。
【IPC分类】G05D1/02
【公开号】CN105573318
【申请号】CN201510940000
【发明人】靳璐, 苏波, 康晓, 吴越
【申请人】中国北方车辆研究所
【公开日】2016年5月11日
【申请日】2015年12月15日