CN103941264B - 一种室内未知环境下使用激光雷达定位方法 - Google Patents

Abstract

本发明公开了一种室内未知环境下使用激光雷达定位方法，在传统激光雷达定位基础上，完成室内未知环境下的地图构建，并在构建地图的基础上进行线段和角点的特征匹配，提出一种混合特征匹配滤波模型，实现室内未知环境下的实时精确定位。该方法简单易行，避免室内环境对定位过程的不利影响，适合工程实用。

Description

一种室内未知环境下使用激光雷达定位方法

技术领域

本发明属于定位技术领域，特别是一种室内未知环境下使用激光雷达定位方法。

背景技术

以往的定位***主要解决的问题是在户外如何实时地实现载体的精确定位，但对于室内环境应用而言，这样的定位***不能满足实时定位的精确性要求。GPS定位因为在室内容易受到信号干扰以及接收不到卫星信号而无法精确定位；地磁定位因为容易受到地磁干扰而无法精确定位；惯导***定位因为随着时间发散而影响到定位精度；视觉导航定位***受环境光照等影响剧烈，定位精度低，且定位可靠性差。因此，当载体在室内未知环境下运动时，实现对其实时、精确的定位成为当前定位***发展的一个趋势。

发明内容

本发明要解决的技术问题是提供一种激光雷达定位方法，该方法可以排除室内环境对传统定位技术的限制，实现室内未知环境下的实时精确定位。

本发明公开的一种室内未知环境下使用激光雷达定位方法，包括以下步骤：

1)将激光雷达置于待定位的物体上；建立特征提取模型，提取室内环境下激光雷达扫描点里的线段和角点特征，并构建二维局部地图；

2)在所述步骤1)特征提取模型的基础上，以第一帧能够在局部坐标系下第二象限检测到直角的扫描点构建的地图作为全局地图，之后每一帧扫描点构建的局部地图与前一帧局部地图进行混合特征匹配，当发现新的线段特征和角点特征，就将特征加入到全局地图中，实现全局地图的实时更新；

3)在所述步骤2)的基础上，建立当前局部坐标系与全局坐标系的坐标转换关系，判断特征的空间变化，实现激光雷达的自定位，得到定位参数(T_x,T_y,θ)，(T_x,T_y)表示为载有激光雷达的载体在室内的位置，θ表示为载有激光雷达的载体在室内的相对偏移角度。

作为上述技术方案的进一步改进，步骤1)中所述的提取室内环境下激光雷达扫描点里的线段和角点特征的过程为：

11)设置自适应阈值γ＝λδ，λ为自适应因数，δ为激光雷达测量单位距离时的阈值；对激光雷达扫描点进行区域分块，去除跳变点；

12)进行线段分割，采用迭代的方式找出每个区域里离第一个点与最后一个点形成的线段距离最远的点，设置一个自适应阈值β，如果一个点到该线段的距离大于β，则为角点，得到角点位置信息(x,y)；

13)对线段分割得到的相邻角点之间的扫描点进行最小二乘法直线拟合，获得线段参数(a,b,rho,x_s,y_s,x_e,y_e)，其中，(a,b)为线段拟合参数；rho为线段所在的直线的斜率；(x_s,y_s),(x_e,y_e)为线段的起始点和终止点坐标。

作为上述技术方案的更进一步改进，步骤2)中所述的混合特征匹配的过程为：

21)以初始时刻构建的地图作为全局地图，构建t_c-1时刻与t_c时刻的局部地图，分别获得地图中的特征参数，包括线段参数和角点参数其中，i＝1,2,…,m,，m为地图的线段个数，j＝1,2,…k，k为地图的角点个数；

22)对t_c-1时刻与t_c时刻局部地图进行线段特征匹配；设定线段匹配判定条件：

条件1：判断线段在全局坐标系下与x轴的夹角：因为rho为线段斜率，可得到线段在局部坐标系下与x轴的夹角β_i

β_i＝arctan(rho_i) (1)

利用上一时刻t_c-1的定位参数推算t_c时刻激光雷达的转角，推算公式为：

然后获得线段在全局坐标系下与x轴的夹角β_gi：

θ_e为t_c时刻航向预估误差角；

设置阈值δ进行线段匹配，如果则认为该线段匹配；如果则判断第i+1条线段斜率，直到所有线段都判断结束，寻找出多余的线段为新增线段；

条件2：从激光雷达坐标系原点作线段的法线ρ，获得法线与线段的交点其中，i＝1,2,…,m,，m为地图的线段个数：

a_i，b_i分别表示第i条线段的线性拟合参数；

利用上一时刻t_c-1的定位参数推算这一时刻t_c的定位参数，再进行局部坐标系到全局坐标系的转换，得到当前时刻法线与线段的交点在全局坐标系位置

θ_e为t_c时刻航向预估误差角；为t_c时刻x,y方向位置预估误差量；θ为t_c-1时刻定位结果的相对角度；

设定阈值λ_p比较和两点之间的距离L_p

若L_p＜λ_p，则认为该条线段匹配，即为上一时刻第i条直线；若L_p＞λ_p，则再比较第i+1条，直到所有直线匹配完毕，多余的线段为新增线段，将其加入全局地图中，实现全局地图的实时更新；

当线段匹配同时满足条件1和条件2时，则认为该线段匹配；

23)对t_c-1时刻与t_c时刻局部地图进行角点特征匹配，判定t_c-1时刻与t_c时刻的角点匹配关系，设定阈值λ_j，比较和两点之间的距离L_j

若L_j＜λ_j，则认为该角点匹配，即为上一时刻的第j个角点；若L_j＞λ_j，则再比较第j+1个，直到所有角点匹配完毕，多余的角点即为新增角点，将其加入到全局地图中，实现全局地图的实时更新。

作为上述技术方案的再进一步改进，步骤3)所述的激光雷达自定位过程如下：

在载体初始位置时，即激光雷达第一帧扫描点，构建全局坐标系，此刻获得的激光雷达的位置和角度都为初始值此时测量的线段参数，角点参数都作为全局坐标系下的线段、角点参数，在载体运动过程中，实时地将采集到的新的特征加入到全局地图中，实现全局地图的实时更新，在进行全局地图更新的同时实现激光雷达自定位；t_c时刻激光雷达测得的局部地图建立局部坐标系，在线段匹配过程中，当t_c时刻激光雷达局部坐标系下线段l_i匹配全局坐标系下的线段l_gi，即可计算得出激光雷达在t_c-1时刻的旋转角度

在角点匹配过程中，当t_c时刻激光雷达局部坐标系下角点(x_j,y_j)匹配全局坐标系下的角点(x_j,y_j)_g

得到激光雷达定位参数(T_x,T_y,θ)，该定位参数即为待定位载体的定位参数，并实时地绘出载体的运动轨迹。

本发明公开的一种室内未知环境下使用激光雷达定位方法，在传统激光雷达定位基础上，完成室内未知环境下的地图构建，并在构建地图的基础上进行线段和角点的特征匹配，提出一种混合特征匹配滤波模型，实现室内未知环境下的实时精确定位。该方法简单易行，避免室内环境对定位过程的不利影响，适合工程实用。

附图说明

图1为本发明所使用的硬件结构示意图；

图2为本发明所述方法工作流程图；

图3为依照本发明的方法构建地图、定位轨迹的matlab仿真图；

图4为依照本发明的方法构建地图、定位轨迹以及定位数据实验结果图

具体实施方式

下面结合附图，对本发明提出的一种室内未知环境下使用激光雷达定位方法进行详细说明。

如图1所示，本发明公开的一种室内未知环境下使用激光雷达定位方法所使用的定位***，包括处理器和二维激光雷达，二维激光雷达通过USB与处理器连接，因为激光雷达数据量大，因此需要使用USB高速通道才能满足数据的实时、高速地采集。同时，在实时性比Windows好很多的Linux***下完成激光雷达数据采集和同步定位与地图构建的算法处理，并绘制运动轨迹。

如图2所示，本发明公开的一种室内未知环境下使用激光雷达定位方法，包括以下步骤：

1)将激光雷达置于待定位的载体上；建立特征提取模型，提取室内环境下激光雷达扫描点里的线段和角点特征，并构建二维局部地图：

11)设置自适应阈值γ＝λδ，λ为自适应因数，δ为激光雷达测量单位距离时的阈值；对激光雷达扫描点进行区域分块，去除跳变点；

12)进行线段分割，采用迭代的方式找出每个区域里离第一个点与最后一个点形成的线段距离最远的点，设置一个自适应阈值β，如果一个点到该线段的距离大于β，则为角点，得到角点位置信息(x,y)；

13)对线段分割得到的相邻角点之间的扫描点进行最小二乘法直线拟合，获得线段参数(a,b,rho,x_s,y_s,x_e,y_e)，其中，(a,b)为线段拟合参数；rho为线段所在的直线的斜率；(x_s,y_s),(x_e,y_e)为线段的起始点和终止点坐标。

2)在所述步骤1)特征提取模型的基础上，以第一帧能够在局部坐标系下第二象限检测到直角的扫描点构建的地图作为全局地图，之后每一帧扫描点构建的局部地图与前一帧局部地图进行混合特征匹配：

21)以初始时刻构建的地图作为全局地图，构建t_c-1时刻与t_c时刻的局部地图，t_c-1、t_c为相邻时刻时间，分别获得地图中的特征参数，包括线段参数和角点参数(x_j,y_j)_t，其中，i＝1,2,…,m,，m为地图的线段个数，j＝1,2,…k，k为地图的角点个数；

22)对t_c-1时刻与t_c时刻局部地图进行线段特征匹配；设定线段匹配判定条件：

条件1：判断线段在全局坐标系下与x轴的夹角：因为rho为线段斜率，可得到线段在局部坐标系下与x轴的夹角β_i

β_i＝arctan(rho_i) (1)

利用上一时刻t_c-1的定位参数推算t_c时刻激光雷达的转角，推算公式为：

然后获得线段在全局坐标系下与x轴的夹角β_gi：

θ_e为t_c时刻航向预估误差角；

设置阈值δ进行线段匹配，如果则认为该线段匹配；如果则判断第i+1条线段斜率，直到所有线段都判断结束，寻找出多余的线段为新增线段；

条件2：从激光雷达坐标系原点作线段的法线ρ，获得法线与线段的交点其中，i＝1,2,…,m,，m为地图的线段个数：

a_i，b_i分别表示第i条线段的线性拟合参数；

利用上一时刻t_c-1的定位参数推算这一时刻t_c的定位参数，再进行局部坐标系到全局坐标系的转换，得到当前时刻法线与线段的交点在全局坐标系位置

θ_e为t_c时刻航向预估误差角；为t_c时刻x,y方向位置预估误差量；θ为t_c-1时刻定位结果的相对角度；

设定阈值λ_p比较和两点之间的距离L_p

若L_p＜λ_p，则认为该条线段匹配，即为上一时刻第i条直线；若L_p＞λ_p，则再比较第i+1条，直到所有直线匹配完毕，多余的线段为新增线段，将其加入全局地图中，实现全局地图的实时更新；

当线段匹配同时满足条件1和条件2时，则认为该线段匹配；

23)对t_c-1时刻与t_c时刻局部地图进行角点特征匹配，判定t_c-1时刻与t_c时刻的角点匹配关系，设定阈值λ_j，比较和两点之间的距离L_j

若L_j＜λ_j，则认为该角点匹配，即为上一时刻的第j个角点；若L_j＞λ_j，则再比较第j+1个，直到所有角点匹配完毕，多余的角点即为新增角点，将其加入到全局地图中，实现全局地图的实时更新。

3)在所述步骤2)的基础上，建立当前局部坐标系与全局坐标系的坐标转换关系，判断特征的空间变化，实现激光雷达的自定位，得到定位参数(T_x,T_y,θ)：

在载体初始位置时，即激光雷达第一帧扫描点，构建全局坐标系，此刻获得的激光雷达的位置和角度都为初始值此时测量的线段参数，角点参数都作为全局坐标系下的线段、角点参数，在载体运动过程中，实时地将采集到的新的特征加入到全局地图中，实现全局地图的实时更新，在进行全局地图更新的同时实现激光雷达自定位；t_c时刻激光雷达测得的局部地图建立局部坐标系，在线段匹配过程中，当t_c时刻激光雷达局部坐标系下线段l_i匹配全局坐标系下的线段l_gi，即可计算得出激光雷达在t_c-1时刻的旋转角度

在角点匹配过程中，当t_c时刻激光雷达局部坐标系下角点(x_j,y_j)匹配全局坐标系下的角点(x_j,y_j)_g

得到激光雷达定位参数(T_x,T_y,θ)，该定位参数即为待定位载体的定位参数，并实时地绘出载体的运动轨迹。

对本发明所述方法进行仿真和验证：

从算法研究和技术应用的角度出发，引用软件和算法在两个平台上同时进行了开发，为了对本发明所述方法的算法的研究和仿真验证，建立了Windows下的Matlab验证***，对于实用化性能和应用测试在Linux下的Eclipse开发环境中进行。如图4所示，当载体载着激光雷达在室内运动时，在Windows下的Matlab验证仿真***上绘制出室内的结构化地图，图3左为原始数据地图，图3中为经过直线拟合后绘制的室内地图，图3右为在室内环境下载体的运动轨迹，即得到了载体的定位参数。

同时为对算法进行实用化性能和应用测试，搭建了室内模拟环境，让载体从起点绕室内一圈再回到起点，如图4所示，定位曲线在起点与终点位置的定位重合误差小于6.5cm，同时，实时性达到了100ms。混合匹配滤波算法由于同时利用了角点和线段特征信息作为观测量，因此，定位精度很高，实验结果与理论推断一致，验证了本发明所述方法的有效性和优越性。

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

Claims (2)

1.一种室内未知环境下使用激光雷达定位方法，其特征在于：包括以下步骤：

1)将激光雷达置于待定位的物体上；建立特征提取模型，提取室内环境下激光雷达扫描点里的线段和角点特征，并构建二维局部地图；

步骤1)中所述的提取室内环境下激光雷达扫描点里的线段和角点特征的过程为：

11)设置自适应阈值γ＝λδ，λ为自适应因数，δ为激光雷达测量单位距离时的阈值；对激光雷达扫描点进行区域分块，去除跳变点；

12)进行线段分割，采用迭代的方式找出每个区域里离第一个点与最后一个点形成的线段距离最远的点，设置一个自适应阈值β，如果一个点到该线段的距离大于β，则为角点，得到角点位置信息(x,y)；

13)对线段分割得到的相邻角点之间的扫描点进行最小二乘法直线拟合，获得线段参数(a,b,rho,x_s,y_s,x_e,y_e)，其中，(a,b)为线段拟合参数；rho为线段所在的直线的斜率；(x_s,y_s),(x_e,y_e)为线段的起始点和终止点坐标；

2)在所述步骤1)特征提取模型的基础上，以第一帧能够在局部坐标系下第二象限检测到直角的扫描点构建的地图作为全局地图，之后每一帧扫描点构建的局部地图与前一帧局部地图进行混合特征匹配，当发现新的线段特征和角点特征，就将特征加入到全局地图中，实现全局地图的实时更新；

步骤2)中所述的混合特征匹配的过程为：

21)以初始时刻构建的地图作为全局地图，构建t_c-1时刻与t_c时刻的局部地图，分别获得地图中的特征参数，包括线段参数和角点参数 其中，i＝1,2,…,m,m为地图的线段个数，j＝1,2,…k，k为地图的角点个数；

22)对t_c-1时刻与t_c时刻局部地图进行线段特征匹配；设定线段匹配判定条件：

条件1：判断线段在全局坐标系下与x轴的夹角：因为rho为线段所在的直线的斜率，可得到线段在局部坐标系下与x轴的夹角β_i

β_i＝arctan(rho_i) (1)

利用上一时刻t_c-1的定位参数推算t_c时刻激光雷达的转角，推算公式为：

然后获得线段在全局坐标系下与x轴的夹角β_gi：

θ_e为t_c时刻航向预估误差角；

设置阈值δ进行线段匹配，如果则认为该线段匹配；如果则判断第i+1条线段斜率，直到所有线段都判断结束，寻找出多余的线段为新增线段；

条件2：从激光雷达坐标系原点作线段的法线ρ，获得法线与线段的交点其中，i＝1,2,…,m,m为地图的线段个数：

a_i，b_i分别表示第i条线段的线性拟合参数；

利用上一时刻t_c-1的定位参数推算这一时刻t_c的定位参数，再进行局部坐标系到全局坐标系的转换，得到当前时刻法线与线段的交点在全局坐标系位置

θ_e为t_c时刻航向预估误差角；为t_c时刻x,y方向位置预估误差量；为t_c-1时激光雷达的旋转角度；

设定阈值λ_p比较和两点之间的距离L_p

若L_p＜λ_p，则认为该条线段匹配，即为上一时刻第i条直线；若L_p＞λ_p，则再比较第i+1条，直到所有直线匹配完毕，多余的线段为新增线段，将其加入全局地图中，实现全局地图的实时更新；

当线段匹配同时满足条件1和条件2时，则认为该线段匹配；

23)对t_c-1时刻与t_c时刻局部地图进行角点特征匹配，判定t_c-1时刻与t_c时刻的角点匹配关系，设定阈值λ_j，比较和两点之间的距离L_j

若L_j＜λ_j，则认为该角点匹配，即为上一时刻的第j个角点；若L_j＞λ_j，则再比较第j+1个，直到所有角点匹配完毕，多余的角点即为新增角点，将其加入到全局地图中，实现全局地图的实时更新；

3)在所述步骤2)的基础上，建立当前局部坐标系与全局坐标系的坐标转换关系，判断特征的空间变化，实现激光雷达的自定位，得到定位参数(T_x,T_y,θ)，(T_x,T_y)表示为载有激光雷达的载体在室内的位置，θ表示为载有激光雷达的载体在室内的相对偏移角度。

2.根据权利要求1所述的一种室内未知环境下使用激光雷达定位方法，其特征在于：步骤3)所述的激光雷达自定位过程如下：

在载体初始位置时，即激光雷达第一帧扫描点，构建全局坐标系，此刻获得的激光雷达的位置和角度都为初始值此时测量的线段参数，角点参数都作为全局坐标系下的线段、角点参数，在载体运动过程中，实时地将采集到的新的特征加入到全局地图中，实现全局地图的实时更新，在进行全局地图更新的同时实现激光雷达自定位；t_c时刻激光雷达测得的局部地图建立局部坐标系，在线段匹配过程中，当t_c时刻激光雷达局部坐标系下线段l_i匹配全局坐标系下的线段l_gi，即可计算得出激光雷达在t_c-1时刻的旋转角度

在角点匹配过程中，当t_c时刻激光雷达局部坐标系下角点(x_j,y_j)匹配全局坐标系下的角点(x_j,y_j)_g

得到激光雷达定位参数(T_x,T_y,θ)，该定位参数即为待定位载体的定位参数，并实时地绘出载体的运动轨迹。