CN105573318A - 基于概率分析的环境构建方法 - Google Patents

Abstract

本发明涉及一种基于概率分析的环境构建方法，属于机器人或无人车技术领域。本发明通过一种基于概率分析的环境构建方法利用激光雷达提供的点云数据，对机器人当前的未知外部环境进行统计分析，将激光雷达采集的点云数据转换成可通行、未知以及障碍物等三类状态的地形分类地图，从而实现准确地构建环境，提高了环境构建的精度，为机器人自主导航时的障碍规避提供了可靠保障。

Description

基于概率分析的环境构建方法

技术领域

本发明涉及机器人或无人车技术领域，具体涉及一种基于概率分析的环境构建方法

背景技术

环境构建主要是指机器人对外部未知环境进行自主识别与理解，构建或恢复真实的环境信息，是机器人进行自主导航所必需的关键技术，具有重要的研究意义与应用价值。由于激光雷达测距与定位精度高、性价比高等特点，利用激光雷达构建未知的环境成为重要的研究内容。在获取激光雷达点云数据后，传统的环境建模方法是通过带高斯噪声概率误差模型补偿位置误差，从不精确的数据中恢复精确的环境模型，并假定下一时刻，同一目标会被激光雷达重复扫描。

而实际中，由于点云数据的有限性，恢复的环境模型是不精确的也不会出现点云数据重叠。因此，如何依据有限的点云数据，准确地构建环境成为亟待解决的技术问题。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是:如何依据有限的点云数据，准确地构建环境。

(二)技术方案

为了解决上述技术问题，本发明提供了一种基于概率分析的环境构建方法，包括以下步骤：

S1、建立机器人的位姿误差模型，获取激光雷达的点云数据；

S2、利用步骤S1建立的位姿误差模型，确定障碍物满足的概率统计测试；

S3、通过二维栅格单元及其邻域进行障碍物测试，生成地形分类地图，从而实现环境构建。

(三)有益效果

本发明通过一种基于概率分析的环境构建方法利用激光雷达提供的点云数据，对机器人当前的未知外部环境进行统计分析，将激光雷达采集的点云数据转换成可通行、未知以及障碍物等三类状态的地形分类地图，从而实现准确地构建环境，提高了环境构建的精度，为机器人自主导航时的障碍规避提供了可靠保障。

具体实施方式

为使本发明的目的、内容、和优点更加清楚，下面结合实施例，对本发明的具体实施方式作进一步详细描述。

本发明实施例提供的一种基于概率分析的环境地图构建方法，包括以下步骤：

步骤S1：建立机器人的位姿误差模型，获取激光雷达点云数据；

为了补偿因机器人横滚角、俯仰角导致的分类误差，以一阶马尔科夫模型作为机器人的位姿误差模型，该模型如式(1)所示：

其中，(x_t，y_t，z_t)表示t时刻机器人的位置；(φ_t，θ_t，φ_t)表示t时刻机器人的姿态角；w_t表示随时间变化、服从正态分布：N(w_t-1，R)的高斯噪声；v_t表示瞬间的、服从正态分布：N(0，Q)的高斯噪声；R和Q表示时不变的噪声协方差矩阵，分别用式(2)与式(3)表示：

根据观测数据得到准确的机器人位姿信息。

定义激光雷达在t时刻第i个元素的观测数据为观测距离为观测角为则激光雷达的点云数据通过式(4)计算得到:

其中，T_t表示t时刻由机器人姿态角构成的旋转矩阵；x_s与T_s表示激光雷达相对于机器人局部坐标系的偏移量和方向角。

步骤S2：确定基于概率统计的障碍物测试；

利用步骤S1建立的一阶马尔科夫模型，将障碍物测试转化为基于概率统计的问题。给定两点(X_t1 ⁱ，Y_t1 ⁱ，Z_t1 ⁱ)和t2>t1，高度差分的概率分布如式(5)所示：

表示正态分布，表示均值，表示方差。

其中，表示值Z_tk ^i(j)的雅克比矩阵(k＝1,2)，上标i(j)表示i或j。假定观测角β_t1 ⁱ＝β_t2 ^j并忽略激光雷达的偏移量x_s，则用式(6)近似表示。

合并式(5)和式(6)，得式(7)：

因此，基于概率统计的障碍物测试表示为：

其中，H表示障碍物高度阈值；λ表示误差概率阈值，并令λ＝0.03。则上式(8)可表示为如式(9)所示：

由上式(9)可知，障碍物测试的概率随着时间差分的增加逐渐降低。因此，障碍物测试的判断依据主要集中在激光雷达点云数据中邻近的两点。

步骤S3：通过二维栅格单元及其邻域进行障碍物测试，生成地形分类地图，从而实现环境构建。

二维栅格地图包含了环境的有用信息。当分析一组新的点云数据时，基于概率分析的方法需要相对应的2维栅格单元及其领域，其中，每个栅格单元存储一个相对较大的值Z_max和一个相对较小的值Z_min。在栅格单元中当观测到一个新的数据时，分别与其领域中的Z_max和Z_min比较，如果一个满足步骤2障碍物测试，则标记该栅格单元状态为障碍物；同时，在栅格单元中，当观测值Z小于该Z_min或大于Z_max时，则分别用观测值Z替代。假定栅格单元初始状态为未知状态，当有新的观测数据时，改变栅格单元及其邻域的状态为可通行状态，如果栅格单元满足障碍物测试，则改变栅格单元及其邻域状态为障碍物，若不满足，仍为可通行状态，没有观测数据的栅格单元为初始的未知状态，进而生成包含未知、可通行、障碍物三种状态的地形分类地图。

通过以上步骤，基于概率分析的环境构建方法将激光雷达观测到的点云数据转换为包含可通行、未知以及障碍物等三种状态的栅格地图，为机器人自主导航时的障碍规避提供可靠保障。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (5)

1.一种基于概率分析的环境构建方法，其特征在于，包括以下步骤：

S1、建立机器人的位姿误差模型，获取激光雷达的点云数据；

S2、利用步骤S1建立的位姿误差模型，确定障碍物满足的概率统计测试；

S3、通过二维栅格单元及其邻域进行障碍物测试，生成地形分类地图，从而实现环境构建。

2.如权利要求1所述的方法，其特征在于，步骤S1中，以一阶马尔科夫模型作为机器人的位姿误差模型，即式(1)：

其中，(x_t，y_t，z_t)表示t时刻机器人的位置；(φ_t，θ_t，φ_t)表示t时刻机器人的姿态角；w_t表示随时间变化、服从正态分布：N(w_t-1，R)的高斯噪声；v_t表示瞬间的、服从正态分布：N(0，Q)的高斯噪声；R和Q表示时不变的噪声协方差矩阵，分别用式(2)与式(3)表示：

根据观测数据得到准确的机器人位姿信息。

定义激光雷达在t时刻第i个元素的观测数据为观测距离为观测角为则激光雷达的点云数据通过式(4)计算得到:

$\left[\begin{array}{c}{X}_t^i\\ Y_t^i\\ Z_t^i\end{array}\right]=r_t^i{T}_t{T}_s\left[\begin{array}{c}{\mathrm{cos\β}}_t^i\\ {\mathrm{sin\β}}_t^i\\ 0\end{array}\right]+\left[\begin{array}{c}{x}_t\\ y_t\\ z_t\end{array}\right]+T_t{x}_s---\left(4\right)$

其中，T_t表示t时刻由机器人姿态角构成的旋转矩阵；x_s与T_s表示激光雷达相对于机器人局部坐标系的偏移量和方向角。

3.如权利要求2所述的方法，其特征在于，步骤S2具体为：

给定两点(X_t1 ⁱ，Y_t1 ⁱ，Z_t1 ⁱ)和t2>t1，高度差分的概率分布如式(5)所示：

$|{Z_{t1}^i}^{*}-{Z_{t2}^j}^{*}|~N\left(\right|Z_{t1}^i-Z_{t2}^j|,{\left({\▿}_{t1}^i\right(Z\left)\right)}^T(2Q+\left(t_2-t_1\right)R){\▿}_{t2}^j\left(Z\right))---\left(5\right)$

$N\left(\right|Z_{t1}^i-Z_{t2}^j|,{\left({\▿}_{t1}^i\right(Z\left)\right)}^T(2Q+\left(t_2-t_1\right)R){\▿}_{t2}^j\left(Z\right))$ 表示正态分布，表示均值， ${\left({\▿}_{t1}^i\right(Z\left)\right)}^T(2Q+(t_2-t_1\left)R\right){\▿}_{t2}^j\left(Z\right)$ 表示方差；

其中，▽_tk ^i(j)(Z)表示值Z_tk ^i(j)的雅克比矩阵(k＝1,2)，上标i(j)表示i或j。假定观测角β_t1 ⁱ＝β_t2 ^j，并忽略激光雷达的偏移量x_s，则▽_tk ^i(j)(Z)用式(6)近似表示；

${\▿}_{tk}^{i\left(j\right)}\left(Z\right)\≈{\left[\begin{array}{cccccc}0& 0& 1& r_{tk}^{i\left(j\right)}{\mathrm{cos\β}}_{tk}^{i\left(j\right)}& r_{tk}^{i\left(j\right)}{\mathrm{sin\β}}_{tk}^{i\left(j\right)}& 0\end{array}\right]}^T---\left(6\right)$

合并式(5)和式(6)，得式(7)：

则基于概率统计的障碍物测试表示为：

$p\left(\right|{Z_{t1}^i}^{*}-{Z_{t2}^j}^{*}|>H)>\mathrm{\λ}---\left(8\right)$

其中，H表示障碍物高度阈值；λ表示误差概率阈值，并令λ＝0.03。则上式(8)可表示为式(9)：

4.如权利要求3所述的方法，其特征在于，步骤S3具体为：假定二维栅格单元初始状态为未知状态，当有新的观测数据时，改变二维栅格单元及其邻域的状态为可通行状态，如果二维栅格单元满足步骤S2的障碍物测试，则改变二维栅格单元及其邻域状态为障碍物，若不满足，仍为可通行状态，没有观测数据的二维栅格单元为初始的未知状态，进而生成包含未知、可通行、障碍物三种状态的地形分类地图。

5.如权利要求4所述的方法，其特征在于，判断是否满足步骤S2的障碍物测试具体为：二维栅格单元的每个栅格单元存储一个相对较大的值Z_max和一个相对较小的值Z_min，在栅格单元中当观测到一个新的数据时，分别与其领域中的Z_max和Z_min比较，如果一个满足步骤S2的障碍物测试，则标记该栅格单元状态为障碍物；同时，在栅格单元中，当观测值Z小于Z_min或大于Z_max时，则用观测值Z替代Z_min或Z_max。