CN109737968A - 基于二维LiDAR和智能手机的室内融合定位方法 - Google Patents

Abstract

本发明提供了一种基于二维LiDAR和智能手机的室内融合定位方法，其包括如下步骤：从二维LiDAR点云数据中提取直线特征并进行点云处理；利用智能手机内置陀螺仪、加速度计和磁力计获取速度和航向变化数据，并对该数据进行修正；将角速度、加速度和地磁强度计算的采集人员行进速度和方向变化用于建立系统模型，利用提取的直线特征参数变化建立观测模型，基于卡尔曼滤波迭代估计位置。与传统PDR方法相比，本发明的方法能够有效削弱定位累积误差的影响，能够实现更精确地定位。

Description

基于二维LiDAR和智能手机的室内融合定位方法

技术领域

本发明属于室内外定位领域，具体涉及一种室内融合定位方法，特别是一种基于二维LiDAR和智能手机的室内融合定位方法。

背景技术

目前，北斗卫星导航系统在国民经济和社会发展多个领域被广泛应用，但由于卫星信号在室内区域受到遮挡和干扰，室内空间内的定位问题却没有得到解决。由于智能手机终端的普及，业界已经意识到以智能手机为载体解决室内定位问题。基于智能手机内置陀螺仪、加速度计和磁力计等传感器输出数据的行人航迹推算(Pedestrian DeadReckoning，PDR)方法具有不依赖额外设备而独立定位的特点，能够短时间内保持较高定位精度，衔接其他定位系统。然而，PDR这种方法具有误差累积的问题，长时间运行将导致定位结果严重偏离真实位置。

因此，需要不断地校准位置或者辅以更加精确的测量值来缓解PDR的误差累积问题。

发明内容

本发明的目的是针对以上要解决的技术问题，提供一种能够有效解决PDR误差累积问题的更加精确的室内定位方法。

为了实现以上目的，本发明提供了一种基于二维LiDAR和智能手机的室内融合定位方法，其包括如下步骤：

步骤1：从二维LiDAR点云数据中提取直线特征并进行点云处理；

步骤2：利用智能手机内置陀螺仪、加速度计和磁力计获取速度和航向变化数据，并对该数据进行修正；

步骤3：将角速度、加速度和地磁强度计算的采集人员行进速度和方向变化用于建立系统模型，利用提取的直线特征参数变化建立观测模型，基于卡尔曼滤波迭代估计位置。

优选地，所述点云处理包括断点检测、角点检测、直线特征参数估计和直线特征合并。

更优选地，对于断点检测，断点是激光雷达距离测量值中不连续的点，即相互距离超过设定阈值的前后扫描点。通常，环境中相同的特征都是连续的扫描点，因此，激光雷达距离测量值都应该是缓慢连续变化的。一旦相邻两束激光的距离测量值有显著的变化，则认为检测到断点，即

|r_i+1-r_i|≥r_b (1)

其中，r_i+1、r_i为相邻的两束激光距离测量值，r_b为距离阈值。断点被用来初步划分扫描点云，形成一系列点集。点集中点的数目少于设定阈值c₀或组成的直线长度小于设定阈值l₀将被舍弃。

更优选地，对于角点检测，针对断点检测形成的点集分别进行直线拟合，并检测点集中的点到拟合直线最大距离是否大于设定阈值d_p。如果是，进行角点检测。角点检测涉及待检点前后M个点，即如果待检验点与前M/2和后M/2个扫描点距离测量值差的和大于设定阈值r_c，则认为其为角点，即

检验出角点之后，将对应点集根据角点再次划分，如果点集中点数目少于设定阈值c₀或组成的直线长度小于设定阈值l₀将被舍弃。

更优选地，对于直线特征参数估计，针对划分好的点集，利用两个参数表征拟合直线。两个特征参数分别是拟合直线到载体坐标系原点的距离ρ，及其法线与载体坐标系X轴的夹角α。利用最小二乘法可以求得这两个参数，如下式所示，求得合适的参数使得点集内所有点到直线的垂直距离平方和最小。

J(α,ρ)＝min∑(ρ-x_icosα-y_isinα)² (3)

其中，x_i，y_i为第i个扫描点在载体坐标系中的坐标。令上式分别对两个参数求偏导，可求得

其中，S_xy是坐标x，y的协方差，表达式为S_yy为坐标y的方差，表达式为S_xx为坐标x的方差，表达式为 和表示拟合直线点集中的坐标平均值，N表示点的数目。

更优选地，对于直线特征合并，在求得所有直线特征参数之后，通过比较这些参数，如果不同直线的参数之差小于设定阈值，即

其中，ρ_i和ρ_j分别表示第i和j条直线到载体坐标系原点的距离，ρ_th表示直线到载体坐标系原点距离之差的阈值。α_i和α_j分别表示第i和j条直线法线与载体坐标系X轴的夹角，α_th表示直线法线与载体坐标系X轴的夹角之差的阈值。将对应点集合并，并计算合并之后新的直线特征参数。迭代合并直线，直到直线特征不再变化。

优选地，所述步骤3中，所述系统模型如下：

状态变量设为

其中，v表示速度，表示航向变化值。

在载体坐标系中构建模型：

其中，和分别表示速度和航向的动态噪声。表示由角速度、加速度和磁强度计算的航向。s_k表示采集者行走速度，通过计算步长和步频估计，

其中，K为与采集者有关的常数，a_max,k和a_min,k分别为当前步行加速度的最大值和最小值，Freq_stride,k和Freq_LiDAR,k分别表示步频和LiDAR扫描频率。

优选地，所述步骤3中，所述观测模型如下：

观测量设为从二维雷达点云提取的直线特征相邻时刻的变化值，假设有n对直线特征可以用于提取观测量，即

Z_k＝[Δρ_k,1 … Δρ_k,n Δα_k,1 … Δα_k,n]^T (9)

其中，直线特征变化值由下式计算

根据几何关系，观测模型可表示为

其中，和分别表示观测值的动态噪声。

由于观测方程是非线性方程组，利用扩展卡尔曼滤波理论，线性化上式得到观测矩阵，

根据卡尔曼滤波理论迭代估计速度和航向改变值。若已知采集的起始位置和航向，便能计算采集者在每次滤波时刻的位置，即

本发明提出从二维LiDAR点云数据中获取室内环境直线特征，融合智能手机内置陀螺仪、加速度计和磁力计等传感器数据估计采集人员轨迹，与传统PDR方法相比，本发明的方法能够有效削弱定位累积误差的影响，能够实现更精确地定位。

附图说明

图1为根据本发明的基于二维LiDAR和智能手机的室内融合定位方法的工作流程图。

图2是载体坐标系中的直线特征。

图3是根据本发明的基于二维LiDAR和智能手机的室内融合定位方法的定位结果。

具体实施方式

下面结合具体实施例进一步阐述本发明。应理解，这些实施例仅用于说明本发明而不用于限制本发明的应用范围。如未特别指出，实施例中涉及但未详细描述细节参数的操作为本领域技术人员所熟知的。

实施例

本发明的基于二维LiDAR和智能手机的室内融合定位方法的工作流程图如图1所示，其包括以下步骤：

步骤1：从二维LiDAR点云数据中提取直线特征并进行点云处理；

步骤2：利用智能手机内置陀螺仪、加速度计和磁力计获取速度和航向变化数据，必要时对该数据进行修正；

步骤3：将角速度、加速度和地磁强度计算的采集人员行进速度和方向变化用于建立系统模型，利用提取的直线特征参数变化建立观测模型，基于卡尔曼滤波迭代估计位置。

具体步骤如下：

1、原始数据采集与预处理

本发明针对的是采集人员利用智能手机和二维LiDAR装置在室内空间进行定位的场景。在采集数据之前首先确定起始时刻载体坐标系与全局坐标系的关系，获取坐标转换矩阵。校准智能手机和LiDAR采集设备的时间。利用校准的设备采集惯性和地磁数据、二维LiDAR点云数据。

在获取原始数据后，首先统一惯性和地磁数据的采样率，再以采集时刻为标记，关联惯性、地磁数据序列，剔除残缺数据。

2、二维LiDAR点云处理

点云处理主要是从二维点云数据中提取直线特征，为后续的位置估计提供数据。首先，以扫描周期(即激光扫描一周360度)为单位划分点云，然后针对每个扫描周期内的点云进行处理。点云处理包括断点检测、角点检测、直线特征参数估计和直线特征合并。

(1)断点检测

断点是激光雷达距离测量值中不连续的点，即相互距离超过设定阈值的前后扫描点。通常，环境中相同的特征都是连续的扫描点，因此，激光雷达距离测量值都应该是缓慢连续变化的。一旦相邻两束激光的距离测量值有显著的变化，则认为检测到断点，即

|r_i+1-r_i|≥r_b (1)

其中，r_i+1、r_i为相邻的两束激光距离测量值，r_b为距离阈值。断点被用来初步划分扫描点云，形成一系列点集。点集中点的数目少于设定阈值c₀或组成的直线长度小于设定阈值l₀将被舍弃。

(2)角点检测

针对断点检测形成的点集分别进行直线拟合，并检测点集中的点到拟合直线最大距离是否大于设定阈值d_p。如果是，进行角点检测。角点检测涉及待检点前后M个点，即如果待检验点与前M/2和后M/2个扫描点距离测量值差的和大于设定阈值r_c，则认为其为角点，即

检验出角点之后，将对应点集根据角点再次划分，如果点集中点数目少于设定阈值c₀或组成的直线长度小于设定阈值l₀将被舍弃。

(3)直线特征参数估计

针对划分好的点集，利用两个参数表征拟合直线。如图2所示，两个特征参数分别是拟合直线到载体坐标系原点的距离ρ，及其法线与载体坐标系X轴的夹角α。利用最小二乘法可以求得这两个参数，如下式所示，求得合适的参数使得点集内所有点到直线的垂直距离平方和最小。

J(α,ρ)＝min∑(ρ-x_icosα-y_isinα)² (3)

其中，x_i，y_i为第i个扫描点在载体坐标系中的坐标。令上式分别对两个参数求偏导，可求得

其中，S_xy是坐标x，y的协方差，表达式为S_yy为坐标y的方差，表达式为S_xx为坐标x的方差，表达式为 和表示拟合直线点集中的坐标平均值，N表示点的数目。

(4)直线特征合并

在求得所有直线特征参数之后，通过比较这些参数，如果不同直线的参数之差小于设定阈值，即

其中，ρ_i和ρ_j分别表示第i和j条直线到载体坐标系原点的距离，ρ_th表示直线到载体坐标系原点距离之差的阈值。α_i和α_j分别表示第i和j条直线法线与载体坐标系X轴的夹角，α_th表示直线法线与载体坐标系X轴的夹角之差的阈值。

将对应点集合并，并计算合并之后新的直线特征参数。迭代合并直线，直到直线特征不再变化。

3.3位置估计

融合手机内置三轴陀螺仪、三轴加速计、三轴磁力计等MEMS传感器输出的角速度、加速度和地磁强度以及从上一步提取的直线特征参数估计位置。具体地，将角速度、加速度和地磁强度计算的采集人员行进速度和方向变化用于建立系统模型，利用提取的直线特征参数变化建立观测模型，基于卡尔曼滤波迭代估计位置。由于LiDAR采样频率与智能手机MEMS传感器采样频率存在差异，本发明以较低的LiDAR采样频率为基准，降低由MEMS传输数据估计的速度和方向的输出频率。具体模型设计如下：

(1)系统模型

状态变量设为

其中，v表示速度，表示航向变化值。在载体坐标系中构建模型

其中，和分别表示速度和航向的动态噪声。表示由角速度、加速度和磁强度计算的航向。s_k表示采集者行走速度，通过计算步长和步频估计，

其中，K为与采集者有关的常数，a_max,k和a_min,k分别为当前步行加速度的最大值和最小值，Freq_stride,k和Freq_LiDAR,k分别表示步频和LiDAR扫描频率。

(2)观测模型

观测量设为从二维雷达点云提取的直线特征相邻时刻的变化值，假设有n对直线特征可以用于提取观测量，即

Z_k＝[Δρ_k,1 … Δρ_k,n Δα_k,1 … Δα_k,n]^T (9)

其中，直线特征变化值由下式计算

根据几何关系，观测模型可表示为

其中，和分别表示观测值的动态噪声。

由于观测方程是非线性方程组，利用扩展卡尔曼滤波理论，线性化上式得到观测矩阵，

根据卡尔曼滤波理论迭代估计速度和航向改变值。

若已知采集的起始位置和航向，便能计算采集者在每次滤波时刻的位置，即

定位效果测试

为了测试发明方法的定位效果，在某办公楼的走廊进行了连续定位实验，并与传统PDR方法进行对比。如图3所示，测试场地长48米，宽15米。测试人员手持终端设备沿着真实轨迹从起点匀速走到终点，轨迹长38.5米。从图中可以看到，利用PDR方法估计的位置在测试人员行走十多米便开始显著偏离真实轨迹，而本发明方法的结果则贴近真实轨迹，直到测试终点才出现小幅度偏差。

测试结果表明，与传统PDR方法相比，本发明方法有效地削弱了定位累积误差的影响，能够实现更精确地定位。

Claims (8)

1.一种基于二维LiDAR和智能手机的室内融合定位方法，其特征在于包括如下步骤：

步骤1：从二维LiDAR点云数据中提取直线特征并进行点云处理；

步骤2：利用智能手机内置陀螺仪、加速度计和磁力计获取速度和航向变化数据，并对该数据进行修正；

步骤3：将角速度、加速度和地磁强度计算的采集人员行进速度和方向变化用于建立系统模型，利用提取的直线特征参数变化建立观测模型，基于卡尔曼滤波迭代估计位置。

2.根据权利要求1所述的方法，其特征在于，所述点云处理包括断点检测、角点检测、直线特征参数估计和直线特征合并。

3.根据权利要求2所述的方法，其特征在于，所述断点检测为：一旦相邻两束激光的距离测量值有显著的变化，则认为检测到断点，即

|r_i+1-r_i|≥r_b (1)

其中，r_i+1、r_i为相邻的两束激光距离测量值，r_b为距离阈值。

4.根据权利要求2所述的方法，其特征在于，所述角点检测涉及待检点前后M个点，即如果待检验点与前M/2和后M/2个扫描点距离测量值差的和大于设定阈值r_c，则认为其为角点，即

检验出角点之后，将对应点集根据角点再次划分，如果点集中点数目少于设定阈值c₀或组成的直线长度小于设定阈值l₀将被舍弃。

5.根据权利要求2所述的方法，其特征在于，所述直线特征参数估计包括针对划分好的点集，利用两个参数表征拟合直线，两个特征参数分别是拟合直线到载体坐标系原点的距离ρ，及其法线与载体坐标系X轴的夹角α，利用最小二乘法可以求得这两个参数，如下式所示，求得合适的参数使得点集内所有点到直线的垂直距离平方和最小：

J(α,ρ)＝min∑(ρ-x_icosα-y_isinα)² (3)

其中，x_i，y_i为第i个扫描点在载体坐标系中的坐标，令上式分别对两个参数求偏导，可求得

其中，S_xy是坐标x，y的协方差，表达式为S_yy为坐标y的方差，表达式为S_xx为坐标x的方差，表达式为和表示拟合直线点集中的坐标平均值，N表示点的数目。

6.根据权利要求2所述的方法，其特征在于，所述直线特征合并包括在求得所有直线特征参数之后，通过比较这些参数，如果不同直线的参数之差小于设定阈值，即

其中，ρ_i和ρ_j分别表示第i和j条直线到载体坐标系原点的距离，ρ_th表示直线到载体坐标系原点距离之差的阈值，α_i和α_j分别表示第i和j条直线法线与载体坐标系X轴的夹角，α_th表示直线法线与载体坐标系X轴的夹角之差的阈值；

将对应点集合并，并计算合并之后新的直线特征参数，迭代合并直线，直到直线特征不再变化。

7.根据权利要求1所述的方法，其特征在于，所述步骤3中，所述系统模型如下：

状态变量设为

其中，v表示速度，表示航向变化值；

在载体坐标系中构建模型：

其中，和分别表示速度和航向的动态噪声，表示由角速度、加速度和磁强度计算的航向，s_k表示采集者行走速度，通过计算步长和步频估计，

其中，K为与采集者有关的常数，a_max,k和a_min,k分别为当前步行加速度的最大值和最小值，Freq_stride,k和Freq_LiDAR,k分别表示步频和LiDAR扫描频率。

8.根据权利要求1所述的方法，其特征在于，所述步骤3中，所述观测模型如下：

观测量设为从二维雷达点云提取的直线特征相邻时刻的变化值，假设有n对直线特征可以用于提取观测量，即

Z_k＝[Δρ_k,1 … Δρ_k,n Δα_k,1 … Δα_k,n]^T (9)

其中，直线特征变化值由下式计算

根据几何关系，观测模型可表示为

其中，和分别表示观测值的动态噪声；

利用扩展卡尔曼滤波理论，线性化上式得到观测矩阵，

根据卡尔曼滤波理论迭代估计速度和航向改变值，计算采集者在每次滤波时刻的位置，即