CN108731679A - 移动机器人环境特征定位方法 - Google Patents

Abstract

本发明公开了一种移动机器人环境特征定位方法，该方法通过激光传感器需定期提取环境特征，并构建多维数据空间，在构建多维数据空间的基础上，基于假设检验理论确定环境点特征；通过确定的点特征估计线段特征范围，结合概率模型方法估计线段特征，运用最小二乘法对估计的线段特征进行拟合，得到准确的线段特征。本发明通过航迹推算用于粗略定位，地图匹配定期对航迹推算更新机器人位姿以减小累积误差，地图匹配侧重于对环境中的点特征与线段特征进行准确提取，由此通过提取环境特征匹配已知地图，实现精确定位。

Description

移动机器人环境特征定位方法

技术领域

本发明属于机器人的技术领域，特别涉及机器人移动的定位方法。

背景技术

机器人是自动执行工作的机器装置。它既可以接受人类指挥，又可以运行预先编排的程序，也可以根据以人工智能技术制定的原则纲领行动。它的任务是协助或取代人类工作的工作，例如生产业、建筑业，或是危险的工作。随时技术水平的提高，机器人的智能化越来越高，已经逐步进入到人们的日常工作和生活领域。

为了使机器人能够准确的运动，机器人应该首先知道自己所处的位置，即机器人对自身进行准确的定位。

如专利申请201610982663.7所公开的一种室内机器人定位方法及装置，包括：获取机器人采集地面上射频识别标签得到的标签信息；根据标签信息中的位置坐标，计算机器人的位置坐标；获取机器人所在室内不同参考位置接收的无线个域网的信号强度指示值的分布图；对机器人接收的无线个域网的信号强度指示值进行修正，得到修正值，计算修正值与分布图上不同参考位置对应的指示值的匹配概率，根据匹配概率确定机器人的辅助定位位置坐标；根据机器人的位置坐标和机器人的辅助定位位置坐标确定机器人的实际位置坐标。本发明中，当在多层楼层定位时，不需要在每层都设置无线个域网节点，减少了无线个域网子节点的个数，降低了成本，避免了每层之间信号干扰，使得定位准确。

如专利申请201611230784.2所公开的一种基于多传感器融合的移动机器人室内定位方法，使用基于航迹推算的里程计位置数据补偿激光匹配定位中无法识别相似环境的情况。本发明不仅将基于惯性单元和里程计的传感器定位数据进行融合，也将激光雷达匹配定位作为参考指标，不仅减小了里程计的累积误差，还弥补了短距离激光雷达在单一特征环境中无法进行匹配定位的缺陷，为机器人的地图绘制和导航提供更为准确的定位数据。

然而，上述方法都需要多种设备或工具相配合，才能实现机器人的定位，定位复杂，而且误差大，难以做到准确可靠。

发明内容

针对上述问题，本发明的目的在于提供一种移动机器人环境特征定位方法，该方法采用航迹推算并结合地图匹配的定位方法进行信息融合，可以实现较小的定位误差，定位准确可靠。

为实现上述目的，本发明的技术方案为：

一种移动机器人环境特征定位方法，其特征在于该方法通过激光传感器需定期提取环境特征，并构建多维数据空间，在构建多维数据空间的基础上，基于假设检验理论确定环境点特征；通过确定的点特征估计线段特征范围，结合概率模型方法估计线段特征，运用最小二乘法对估计的线段特征进行拟合，得到准确的线段特征。

进一步，所述方法首先通过高斯降采样的方法构建多维数据空间，在低维数据空间上，采用极值点搜索与假设检验的方法进行点特征提取；二维激光传感器在平面上按一定的角度分辨率进行距离扫描，扫描数据的极坐标表示为：SK＝(ρk,θk),K＝1…N,，其中N为扫描数据个数，原始数据带有噪声且服从高斯白噪声独立分布，即ρk～N(μρk,σ²)，θk～N(μθk,σ²θ)；二维激光传感器获得的原始数据具有噪声，按前述方法对数据进行预处理，然后对处理完的数据进行环境聚类，将激光传感器可能扫描到的同一物体扫描点聚集为一类；环境聚类后对采样点做降采样处理，构建多维数据空间。

环境特征提取流程，包括如下步骤：

步骤1，机器人启动，对数据进行预处理；

步骤2，环境聚类，即将激光传感器可能扫描到的同一物体扫描点聚集为一类；

步骤3，激光传感器进行激光扫描，并计算扫描点的统计量；

步骤4，进行特征搜索；

步骤5，分别进行点判断和线段判断；

步骤6，点提取、线段提取，并将不是点也不是线段的特征归入其它环境特征。

更进一步，在第n维数据空间下，每个采样点所在的环境均由其左右邻域的若干点决定，算法拟对每个采样点左右邻域内的点进行检测，边界由单侧邻域决定，左右邻域的环境分为四种：线段、曲线、点及复杂环境，算法拟对左右邻域内的点做线段估计，然后采用假设检验的估计环境特征。

更进一步，对于采样点的左右邻域内，与第i个采样点较近的点可能为冗余点，与第i个采样点较远的点可能为无关点，每个点对第i个采样点的影响各不相同，因此需要定义对采样点影响的概率函数，概率函数应该服从与距离相关的正态分布，通过概率函数获得

接受域H₀：

拒绝域H₁：

满足拒绝域的点的左邻域或右邻域为可能存在点特征的区域，其他区域可能为线段特征。

更进一步，对点特征进行提取，点特征表示为P:[ρ,θ]^T，其中ρ为点特征极径，θ为ρ的倾角，点特征分布在左右邻域统计量相对较大的区域，由此确定点特征的位置；点特征位置确定后，通过检验极值点邻域内点的分布情况，判断是否为点特征。

更进一步，对线段特征进行提取，线段特征表示为：L：其中d为激光传感器所在直角坐标系原点到线段的距离，为d的倾斜角，p_s为线段的起点，pl为线段的终点。

更进一步，所述线段特征，通过带权值的约束最小二乘法的线段表示形式如下：

其中S，E分别为线段的两个端点，将上式改写为矩阵形式：

ε²＝J^TSJ

其中S＝D^TD,

由于α²+β²＝1，引入拉格朗日乘子λ，得：

ε²＝J^TSJ-λ(J^TCJ-1)

其中满足J^TCJ＝1，令得：

SJ-λCJ＝0

上式中S为正定矩阵，广义特征向量J对应最小的特征值λ，当α与β确定以后，求得：

最终求出线段特征d。

更进一步，得出线段特征后，还需要进行线段融合；融合时，输入与输出的关系通过最小化隐含的关系方程F(I,O)给出，其中I为输入向量，O为输出向量。

更进一步，由于相互连接的线段L_i与L_i+1的分别服从与如果两条连接的线段属于同一线段，则

根据u检验有：

接受域H₀：L_i与L_i+1为同一条线段；选择统计量为

其中μ_o＝0，n＝1；

对于给定的显著水平α，在H0成立的条件下，满足等式的临界值为如果则拒绝H0；反之接受H0，对两段线段包含的采样点重新进行特征提取。

进一步，线段提取完成后，则进行位姿更新；机器人在运动过程中从实际环境中提取线段特征与点特征，利用这些特征与已知地图匹配；首先需要对线段特征两两求取交点，并与环境中的点特征一起进行拟合。

更进一步，点特征表示为p:[ρ,θ]^T,其中ρ为点特征极径，θ为ρ的倾角，将点特征转换为机器人直角坐标系Guv下为[ρcosθ，ρsinθ]；通过里程计模型将点特征与线段特征的交点坐标转换到世界坐标系Oxy下，表示为p_i＝[x_i,y_i,1]，p_i表示实际激光传感器测得的测量值，真实值从标有特征的已知地图上通过最近点寻得并表示为p_Oi；测量值与真实值两个点集间存在位姿变换矩阵，求得两个点集间的位姿变换矩阵T；里程计模型下，t时刻机器人在世界坐标系Oxy下位姿为运用位姿变换矩阵T对机器人位姿进行更新，更新后的位姿为

本发明所实现的移动机器人环境特征定位方法，航迹推算用于粗略定位,地图匹配定期对航迹推算更新机器人位姿以减小累积误差,地图匹配侧重于对环境中的点特征与线段特征进行准确提取，由此通过提取环境特征匹配已知地图，实现精确定位。

附图说明

图1为本发明所实施的局部路径规划流程图。

图2为本发明所实施的三角形角平分线示意图。

图3为本发明所实时的角平分线插值示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

为匹配已知地图，减小移动机器人里程计模型产生的累积误差，激光传感器需定期提取环境特征，实现精确定位,所以环境特征提取是室内移动机器人自定位的核心问题。环境特征是指能够反映大范围环境的结构化特征，主要有直线段、角、点、垂直线等，对应实例分别为墙、墙角、凸角、门等特征对应,稳定的环境特征的成功提取对于室内移动机器人的自定位具有重要的意义。

由于利用多个点特征与线段特征便可对机器人进行准确定位，因此需要对环境中的点特征与线段特征进行检测。本发明提出一种室内环境特征提取方法，在构建多维数据空间的基础上，基于假设检验理论确定环境点特征。通过确定的点特征估计线段特征范围，结合概率模型方法估计线段特征，运用最小二乘法对估计的线段特征进行拟合，得到准确的线段特征。

本发明首先通过高斯降采样的方法构建多维数据空间，在低维数据空间上，采用极值点搜索与假设检验的方法进行点特征提取。二维激光传感器在平面上按一定的角度分辨率进行距离扫描，扫描数据的极坐标表示为：SK＝(ρk,θk),K＝1…N,，其中N为扫描数据个数，原始数据带有噪声且服从高斯白噪声独立分布，即ρk～N(μρk,σ²)，θk～N(μθk,σ²θ)。二维激光传感器获得的原始数据具有噪声，按前述方法对数据进行预处理，表示不变。由于室内物体大部分是相互分开的，对处理完的数据进行环境聚类，将激光传感器可能扫描到的同一物体扫描点聚集为一类。由于降采样后环境的重要特征不变，因此环境聚类后对采样点做降采样处理，构建多维数据空间，其目的是：

(1)容易从的环境中获得出有用特征；

(2)降低计算开销；

(3)抑制噪声。

环境特征提取流程图如图1所示，其包括如下步骤：

步骤1，机器人启动，对数据进行预处理；

步骤2，环境聚类，即将激光传感器可能扫描到的同一物体扫描点聚集为一类；

步骤3，激光传感器进行激光扫描，并计算扫描点的统计量；

步骤4，进行特征搜索；

步骤5，分别进行点判断和线段判断；

步骤6，点提取、线段提取，并将不是点也不是线段的特征归入其它环境特征。

在第n维数据空间下，每个采样点所在的环境均由其左右邻域的若干点决定，算法拟对每个采样点左右邻域内的点进行检测，边界由单侧邻域决定，左右邻域的环境分为四种：线段、曲线、点及复杂环境，算法拟对左右邻域内的点做线段估计，然后采用假设检验的估计环境特征。

首先推导三角形角平分线的一个性质。设三角形ABC中，∠A平分线AD交BC于D点，AB＝c，AC＝b，BC＝a，BD＝e，CD＝f，AD＝g，∠A＝α，∠C＝γ，则a＝e+f，如图2所示。

由三角形角平分线分对边线段之比等于相应邻边之比，并利用合比性质，得

求得

在三角形ABC和三角形ACD中，分别运用余弦定理，得

g²＝b²+f²-2bfcosγ (5)

此外

Cosα+1＝【cos²(α/2)-sin²(α/2)】+【cos²(α/2)+sin²(α/2)】(6)＝2cos²(α/2)

将式(2)、(3)、(4)和(6)代入式(5)，求得

如图3所示，ρi为第i个采样点的极径，为第i个采样点左邻域或右邻域内第j个采样点极径的估计值，ρi±2j为第i个采样点左邻域或右邻域内第2j个采样点的极径。

在极坐标下，依据上述三角形角平分线性质，可列如下方程：

在线段环境下，是ρ_i±j的无偏估计，所以有 所以在第i个采样点左右邻域内采样点的极径是独立分布的。

根据X²假设检验有：

接受域H₀：第i个采样点为线段上的点；

拒绝域H₁：第i个采样点为点特征或者其他环境。

在第i个采样点左右邻域内，扫描点满足的关系为总体的一个样本，且有：

其中为样本方差，n为第i个采样点左邻域或右邻域内点的个数。

样本均值则为：

在第i个采样点的左右邻域内，与第i个采样点较近的点可能为冗余点，与第i个采样点较远的点可能为无关点。每个点对第i个采样点的影响各不相同，因此需要定义对采样点影响的概率函数，概率函数应该服从与距离相关的正态分布。所以有：

其中ρ_i±j为第j个采样点对第i个采样点的影响概率，d_i±j为第j个采样点到第i个采样点的距离。μd为对第i个采样点影响最大的距离。σd为对分布的影响，μd与σd可以人为给定。

样本方差：

选择统计量：

因为是的无偏估计，所以在H₀成立的条件下，近似为1。对于固定的邻域(采样点个数为n)，等式(14)近似等于n-1。

所以接受域H₀：

拒绝域H1:

满足式(16)的点的左邻域或右邻域为可能存在点特征的区域，其他区域可能为线段特征。

点特征提取：

点特征表示为P:[ρ,θ]^T，其中ρ为点特征极径，θ为ρ的倾角，点特征分布在左右邻域统计量相对较大的区域，由此确定点特征的位置。

点特征位置确定后，需要验证该点是否为点特征。可以通过检验极值点邻域内点的分布情况，判断是否为点特征。激光传感器扫描数据服从高斯白噪声独立分布若极值点P_i在直线上，则在其小邻域内可认为根据x²假设检验有：

因此，拒绝域H₁上的点为点特征。

由上可知，在极值点ρ_i的小邻域内，激光传感器扫描到的极径值{ρ_i-j，ρ_i-j+1，...，ρ_i,ρ_i+j-1,ρ_i+j}为总体的一个样本，则样本方差与样本均值满足：

其中n＝2j+1。

样本均值为：

样本方差为：

选择统计量为：

因为是的无偏估计，所以在H₀成立的条件下，近似为1。对于固定的邻域(采样点个数为n_ρ)，等式(21)近似n-1。所以拒绝域H₁为：

满足式(22)的极值点为点特征，这类极值点在环境中为突出的点，讲这类点作为点特征，得到P：[ρ,θ]^T。

线段特征提取：

根据统计量可知，线段特征表示为：L：其中d为激光传感器所在直角坐标系原点到线段的距离，为d的倾斜角，p_s为线段的起点，pl为线段的终点。线段在直角坐标系下的方程为：

其中d>0，x_i＝ρ_icosθ_i,y_i＝ρ_i ^sinθ_i。

上部分中，激光传感器的扫描点已经通过概率统计方法划分为若干个部分，每个部分都可能是线段特征。带权值的约束最小二乘法的线段表示形式如下：

其中S，E分别为线段的两个端点。将上式改写为矩阵形式：

ε²＝J^TSJ (25)

其中S＝D^TD,

由于α²+β²＝1，引入拉格朗日乘子λ，得：

ε²＝J^TSJ-λ(J^TCJ-1) (26)

其中满足J^TCJ＝1。令得：

SJ-λCJ＝0 (27)

上式中S为正定矩阵，广义特征向量J对应最小的特征值λ。当α与β确定以后，可以求得：

最终求出线段特征d。

得出线段特征后，还需要进行线段融合。环境中的噪声有可能将一条线段分割成两条线段，因此需要检验两条线段是否属于同一线段，进而对线段特征进行融合。融合时，输入与输出的关系不是通过明确的关系方程给出，而是通过最小化隐含的关系方程F(I,O)给出，其中I为输入向量，O为输出向量。根据这个理论，∑_I为输入向量的协方差矩阵，∑_o为输出向量的协方差矩阵。则有：

其中，令：

其中，I＝[x_i,y_i]^T，O为则：T

上式中：

由于由误差传递公式可知：

其中，则：

代入式(29)求出的协方差∑_o。由于相互连接的线段L_i与L_i+1的分别服从与如果两条连接的线段属于同一线段，则

根据u检验有：

接受域H₀：L_i与L_i+1为同一条线段。选择统计量为

其中μ_o＝0，n＝1。

对于给定的显著水平α，在H0成立的条件下，满足等式的临界值为如果则拒绝H0；反之接受H0，对两段线段包含的采样点重新进行特征提取。

线段提取完成后，则进行位姿更新。机器人在运动过程中从实际环境中提取线段特征与点特征，需要利用这些特征与已知地图匹配，以更新机器人位姿，减小里程计累积误差。首先需要对线段特征两两求取交点，并与环境中的点特征一起进行拟合。由于环境中线段特征较点特征更为稳定，因此直线交点的权重要比环境中点特征的权重要大。点特征表示为p:[ρ,θ]^T,其中ρ为点特征极径，θ为ρ的倾角。将点特征转换为机器人直角坐标系Guv下为[ρcosθ，ρsinθ]。通过里程计模型将点特征与线段特征的交点坐标转换到世界坐标系Oxy下，表示为p_i＝[x_i,y_i,1]。p_i表示实际激光传感器测得的测量值，真实值从标有特征的已知地图上通过最近点寻得并表示为p_Oi。测量值与真实值两个点集间存在位姿变换矩阵，表示为：

为了匹配测量值与真实值两个点集，利用最小二乘法理论列出下式

通过求偏导数使目标函数E(T)最小，求得两个点集间的位姿变换矩阵T。里程计模型下，t时刻机器人在世界坐标系Oxy下位姿为运用位姿变换矩阵T对机器人位姿进行更新，更新后的位姿为

由此，本发明是于假设检验的室内环境特征提取定位方法，该方法在构建多维数据空间的基础上，基于假设检验理论确定环境点特征。通过确定的点特征估计线段特征范围，结合概率模型方法估计线段特征，运用最小二乘法对估计的线段特征进行拟合，得到准确的线段特征，从而使机器人通过提取环境特征匹配已知地图，实现精确定位。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种移动机器人环境特征定位方法，其特征在于该方法通过激光传感器需定期提取环境特征，并构建多维数据空间，在构建多维数据空间的基础上，基于假设检验理论确定环境点特征；通过确定的点特征估计线段特征范围，结合概率模型方法估计线段特征，运用最小二乘法对估计的线段特征进行拟合，得到准确的线段特征。

2.如权利要求1所述的移动机器人环境特征定位方法，其特征在于所述方法首先通过高斯降采样的方法构建多维数据空间，在低维数据空间上，采用极值点搜索与假设检验的方法进行点特征提取；激光传感器在平面上按一定的角度分辨率进行距离扫描，扫描数据的极坐标表示为：SK＝(ρk,θk),K＝1…N,，其中N为扫描数据个数，原始数据带有噪声且服从高斯白噪声独立分布，即ρk～N(μρk,σ²)，θk～N(μθk,σ²θ)；激光传感器获得的原始数据具有噪声，按前述方法对数据进行预处理，然后对处理完的数据进行环境聚类，将激光传感器可能扫描到的同一物体扫描点聚集为一类；环境聚类后对采样点做降采样处理，构建多维数据空间。

3.如权利要求2所述的移动机器人环境特征定位方法，其特征在于环境特征提取流程，包括如下步骤：

步骤1，机器人启动，对数据进行预处理；

步骤2，环境聚类，即将激光传感器可能扫描到的同一物体扫描点聚集为一类；

步骤3，激光传感器进行激光扫描，并计算扫描点的统计量；

步骤4，进行特征搜索；

步骤5，分别进行点判断和线段判断；

步骤6，点提取、线段提取，并将不是点也不是线段的特征归入其它环境特征。

4.如权利要求3所述的移动机器人环境特征定位方法，其特征在于在第n维数据空间下，每个采样点所在的环境均由其左右邻域的若干点决定，算法拟对每个采样点左右邻域内的点进行检测，边界由单侧邻域决定，左右邻域的环境分为四种：线段、曲线、点及复杂环境，算法拟对左右邻域内的点做线段估计，然后采用假设检验的估计环境特征。

5.如权利要求4所述的移动机器人环境特征定位方法，其特征在于对于采样点的左右邻域内，定义对采样点影响的概率函数，概率函数应该服从与距离相关的正态分布，通过概率函数获得

接受域H₀：

拒绝域H₁：

满足拒绝域的点的左邻域或右邻域为可能存在点特征的区域，其他区域可能为线段特征。

6.如权利要求5所述的移动机器人环境特征定位方法，其特征在于对点特征进行提取，点特征表示为P:[ρ,θ]^T，其中ρ为点特征极径，θ为ρ的倾角，点特征分布在左右邻域统计量相对较大的区域，由此确定点特征的位置；点特征位置确定后，通过检验极值点邻域内点的分布情况，判断是否为点特征。

7.如权利要求4所述的移动机器人环境特征定位方法，其特征在于对线段特征进行提取，线段特征表示为：L：其中d为激光传感器所在直角坐标系原点到线段的距离，为d的倾斜角，p_s为线段的起点，p_l为线段的终点。

8.如权利要求7所述的移动机器人环境特征定位方法，其特征在于所述线段特征，通过带权值的约束最小二乘法的线段表示形式如下：

其中S，E分别为线段的两个端点，将上式改写为矩阵形式：

ε²＝J^TSJ

其中

由于α²+β²＝1，引入拉格朗日乘子λ，得：

ε²＝J^TSJ-λ(J^TCJ-1)

其中满足J^TCJ＝1，令得：

SJ-λCJ＝0

上式中S为正定矩阵，广义特征向量J对应最小的特征值λ，当α与β确定以后，求得：

最终求出线段特征d。

9.如权利要求8所述的移动机器人环境特征定位方法，其特征在于得出线段特征后，还需要进行线段融合；融合时，输入与输出的关系通过最小化隐含的关系方程F(I,O)给出，其中I为输入向量，O为输出向量；由于相互连接的线段L_i与L_i+1的分别服从与如果两条连接的线段属于同一线段，则

根据u检验有：

H₀：

接受域H₀：L_i与L_i+1为同一条线段；选择统计量为

其中μ_o＝0，n＝1；

对于给定的显著水平α，在H0成立的条件下，满足等式的临界值为如果则拒绝H0；反之接受H0，对两段线段包含的采样点重新进行特征提取。

10.如权利要求9所述的移动机器人环境特征定位方法，其特征在于线段提取完成后，则进行位姿更新；机器人在运动过程中从实际环境中提取线段特征与点特征，利用这些特征与已知地图匹配；首先需要对线段特征两两求取交点，并与环境中的点特征一起进行拟合。