一种基于轨迹配准的室内环境多毫米波雷达定位方法

1.本发明涉及毫米波雷达定位领域，尤其涉及一种室内环境多毫米波雷达间定位方法。


背景技术：

2.毫米波雷达应用于室内人员监测与跟踪是最近几年兴起的新的使用方式，毫米波雷达具有体积小、成本低和数据处理快等特点，同时能够有效保护受监控人员的个人隐私。将单个雷达应用在监控较大的室内环境时存在覆盖面积有限和障碍物遮挡等问题，进而导致跟踪目标丢失，因此需要多个雷达同时合作才能做到无盲区的监控，进而需要一种配准方法将多个雷达的点云进行精准拼接。
3.由于毫米波雷达点云数据的离散程度大，难以根据点云数据提取特征，同时存在多径反射现象而产生噪声干扰问题，所以现有的传统点云配准方法如迭代最近邻点算法(iterativeclosestpoint，简称“icp”)和正态分布点云算法(normaldistributionstransform，简称“ndt”)等，难以满足配准需求。


技术实现要素：

4.本发明实施方式的目的在于提供一种针对毫米波雷达稀疏点云数据的配准方法，提高多雷达的点云点云配准的准确度，满足室内监控需求。
5.为实现上述目的，本发明实施方式提供了一种基于行走轨迹点云的配准方法，具体包括以下步骤：
6.步骤s1：采集两雷达(以下分别简称为雷达1，雷达2)覆盖重叠区域内行人行走的点云数据；
7.步骤s2：分别对雷达1，雷达2划分感兴趣区域，去除部分噪声影响；
8.步骤s3：点云聚类自适应提取行走轨迹点云；
9.步骤s4：拟合行走轨迹，轨迹对齐得到初始的变换矩阵；
10.步骤s5：根据拟合轨迹建立两雷达点云轨迹点对点的对应关系；
11.步骤s6：ba优化初始变换矩阵，得到最优定位结果；
12.步骤s7：定位结果精度评价。
13.进一步地，在所述步骤s2中根据行走轨迹在各自雷达坐标系分别划分感兴趣区域，去除部分多径反射噪声，感兴趣区域完整包含行走轨迹，行走轨迹控制在5-6m，感兴趣区域范围从8m*8m到10m*10m内调整。
14.进一步，步骤s3中，对感兴趣区域范围内的点云采用dbscan聚类处理，对区域内不同的点云类统计其速度均值，选取速度最高的点云类作为行走轨迹点云，根据前0-10帧点云质心与70-80帧点云质心计算其行走方向向量。
15.进一步，步骤s4具体包括如下步骤：
16.步骤s41：将提取出的轨迹点云进行凸包滤波处理，提取轨迹点云凸包点云集，通
过改进dbscan算法，遍历凸包点，满足密度要求则保留该点，不满足，则删除，遍历完所有凸包点后，对轨迹点云选取新的凸包点，再次滤波，直至所有凸包点均满足密度要求；
17.步骤s42：对滤波后的轨迹点云集通过ransac算法拟合行走轨迹直线；
18.步骤s43：取两雷达拟合直线的中心点坐标，以雷达1坐标系为世界坐标系，雷达2轨迹中心向雷达1轨迹中心平移对齐，得到雷达2坐标变换至世界坐标系的平移向量；
19.步骤s44：根据两条拟合直线斜率作差，得到雷达2坐标变换至世界坐标系的旋转矩阵，结合平移向量，得到雷达2变换至雷达1坐标系的变换矩阵m。
20.进一步，步骤s5具体包括如下步骤：
21.步骤s51：根据步骤s4的拟合直线和拟合直线的法线对轨迹点云建立占据网格。以拟合直线为中心向外取不同截距但等长的数条直线，计算拟合直线的法线斜率并取不同截距的数条法线，由不同的直线及其法线组成点云轨迹的占据网格，单个网格边长应略大于点云分辨率，雷达1与雷达2的占据网格总数量应该相等且一一对应，网格根据轨迹点云方向向量统一编号，以保证对应关系的准确；
22.步骤s52：统计每个网格内是否有点存在，若没有，则网格占据状态设为0；若有一个，则状态设为1并取该点坐标表示该网格位置；若存在多个点，则状态设为1并取其点云质心坐标表示该网格位置；
23.步骤s53：筛选出雷达1与雷达2网格中占据状态为1且编号对应的网格，取其表示网格位置的点组成雷达1与雷达2点云的对应点对。
24.进一步，在步骤s6中，根据步骤s5得到的对应点关系，采用非线性优化算法ba(bundle adjustment)将步骤s4得到的初识变换矩阵m进行迭代优化，见如下公式：
[0025][0026]
上述公式中，k表示迭代次数，表示最优变换矩阵，zk表示雷达1轨迹点云经占据网格提取的对应点，yk表示在原雷达2坐标系下雷达2轨迹点云经占据网格提取的对应点，mk表示第k次迭代得到的变换矩阵，h(mk,yk)表示yk经mk变换至世界坐标系下的对应点坐标，qk为点云噪声协方差矩阵。
[0027]
定义残差函数：
[0028]ek
＝z
k-h(mk,yk)
[0029]
将残差函数代入上式则得到：
[0030]
e(m)＝e
t
∑e
[0031]
令e(m)对m求导为零，即可得到最优矩阵
[0032]
进一步的，步骤s7中，将雷达2轨迹点云经最优变换矩阵变换至雷达1坐标系，根据均方根误差rmse和本发明提出的评价指标进行定位精度质量评价，本发明提出的评价指标如下：
[0033]
指标1：统计配准后遍历所有历史帧的同一时间戳下两雷达点云帧质心距离ds＝{s1,s2…
sn}，记ds均值为a
ds
，a
ds
值越小，则定位精度越高；
[0034]
指标2：统计双雷达点云配准融合后数据按时间序列前后帧点云的质心距离dm＝{m1,m2…mn-1
}，计算dm平均值，记为a
dm
，a
dm
值越小，则定位精度越高；
[0035]
指标3：计算变换后两轨迹点云集最大对应点距离dis
max
，其大小能够表示定位角度误差大小，dis
max
值越小，代表角度误差越小。
[0036]
本发明的有益效果是，通过上述用于基于优化的多毫米波雷达相互定位方法，极大程度的去除了镜像反射噪声的影响，解决了现有低成本毫米波雷达的离散程度大的点云建立对应关系困难的问题，有效提高了多雷达间定位精度，可应用于室内环境监控的多雷达定位。
附图说明
[0037]
图1为本发明的整体算法流程图；
[0038]
图2为本发明的点云边界凸包滤波示意图；
[0039]
图3为本发明的点云滤波前后对比图；
[0040]
图4为本发明的占据网格建立对应关系示意图。
具体实施方式
[0041]
为使本发明提供的技术方案的优点和应用方式体现的更清楚，现结合附图对本发明提供的技术方案的几个步骤具体实施方式进行进一步详细的说明。
[0042]
本发明所述的一种基于轨迹配准的室内环境多毫米波雷达定位方法，其具体流程图见图1，分别对每个雷达的点云数据划定roi(regionofinterest)区域，去除部分场景变化和多径反射噪声，并对选定区域内点云通过dbscan聚类处理筛选出真实轨迹；对筛选出的轨迹点云通过ransac拟合出其路径，根据两雷达拟合结果得到雷达间近似的位置对应关系，将其作为优化初值；进入后端，根据拟合直线建立占据网格，确定点云对应关系，根据点对的对应关系对优化初值迭代得到最终配准结果，并作配准质量评价。首先在雷达数据获取阶段，根据雷达有效覆盖范围可以推算出两雷达重叠覆盖区域，确保实施人员能够行走在重叠区域内，同时，感兴趣区域也根据该重叠区域选取。
[0043]
在步骤s3中，对感兴趣区域范围内的点云采用dbscan聚类处理，聚类半径参数的设置应大于雷达的点云分辨率，聚类半径内点云数量参数根据使用雷达的点云质量调参得到。因为安装环境内动态目标只有行人，其余均为动态障碍物，如墙体、柱子等，对区域内不同的点云类统计其速度均值，故可以认定聚类点云集中速度均值低的不是目标轨迹点云，选取速度最高的点云类作为行走轨迹点云。
[0044]
在步骤s41中，将提取出的轨迹点云进行凸包滤波处理，见图2，对离散程度大的点云数据进行拟合其结果并不理想，提取提取轨迹点云集的凸包点云，即轨迹点云最外围的一圈点，遍历该凸包点集，满足密度要求则保留该点，不满足，则删除，遍历完所有凸包点后，对轨迹点云选取新的凸包点，再次滤波，直至所有凸包点均满足密度要求。在筛选点集时，为避免误删真实点，加入强度判断，凸包点半径内点云强度均值如果大于整体轨迹点云强度均值，则判断该凸包点强度是否达到真实点强度，若达到，则即使该点密度达不到要求，仍保留该点，该点不计入下次循环的凸包点集，但是可以计入凸包点半径内点云集；若没达到真实点强度或者凸包点半径内点云强度均值小于整体轨迹点云强度均值，则删除该点，实际滤波效果对比见图3。
[0045]
在步骤s44后，加入方向向量约束来避免斜率计算错误角度计算，理论上经初始变
换矩阵变换后雷达1与雷达2的方向向量夹角应小于90
°
，实际情况应远小于90
°
，如果不满足该约束，则将雷达2点云轨迹以拟合直线中心为原点旋转180
°
，重新计算是否满足约束。
[0046]
在步骤s51中，根据拟合直线和拟合直线的法线对轨迹点云建立占据网格，见图4，拟合直线的长度基本等于行走轨迹点云的长度，法线长度基本等于行走轨迹点云的宽度，点云轨迹长度与宽度根据实际点云质量确定。根据轨迹点云宽度和单个网格宽度确定需要取几个不同直线截距，同理，可以确定需要取几个不同的法线截距来确定法线数量。
[0047]
截距即单个网格边长的选取根据实际点云分辨率调整，一般选取略大于分辨率的边长，保证不会出现单个网格内最多只出现一个点的情况。网格内有多个点存在时，选取能够表示点云密度的点云质心表示该网格位置。在网格编号时，纵向编号根据点云行走方向向量依次增加，横向编号根据点云行走方向向量夹角为正90
°
的法线方向向量依次增加。
[0048]
在步骤s6中，在残差函数e(m)对m求导时，由于m矩阵为变换矩阵，在做矩阵加法运算时不满足变换矩阵封闭性，通过李代数映射到李群将矩阵加法转换为矩阵乘法问题处理。
[0049]
在步骤s7中，配准评价标准的设计参考点云跟踪的评价指标，一般的点云跟踪多采用行人聚类质心为跟踪目标，前后点云帧之间的质心距离的大小会影响跟踪目标的数据关联程度，a
ds
和a
dm
均能代表变换轨迹重合程度，dis
max
能够表示定位角度误差大小，dis
max
值越小，代表角度误差越小。