一种基于激光里程计的室内高精度自定位方法

本发明涉及激光雷达(light detection and ranging,lidar)室内自定位领域，具体是利用激光雷达对不同反射率材料具有不同反射强度测量值的特性，通过在参考位置引入特制形状的高反射率标记物，然后基于激光雷达测得的点云数据筛选出反射标记物参考点，通过参考点的角度和距离信息解算出激光雷达的位置坐标，在大幅度降低传统激光同步定位和绘图技术(simultaneous localization and mapping,slam)里程计(lidarodometry,lo)算法时间复杂度的同时能够提升定位精度。

背景技术：

1、近年来，随着机器人与传感器技术的飞速发展，室内移动机器人在机器人领域的地位举足轻重，室内定位技术研究也因此显得尤为重要。室内定位作为新一代物联网(internet of things,iot)的核心技术，市场需求巨大。由于室内空间拓扑及布局复杂易变，同时存在复杂电磁环境影响有效信号的传输，导致信号传播出现多径及非视距误差等问题。这些因素导致卫星定位导航等室外定位技术在室内环境中应用时，精度会大幅度下降。室内定位系统经过几十年的发展和完善，其相关技术包含wi-fi、蓝牙、超声波、射频识别(radio frequency identification,rfid)和超宽带信号(ultra-wideband,uwb)等。然而，这些基于无线信号的定位技术因信号多径传播对无线电信号造成的影响，在复杂室内环境下的定位精度难以保证。激光雷达具有分辨率高、抗干扰性强、不受光线影响等优点，基于激光雷达的室内定位技术逐渐成为热门研究领域。

2、利用激光点云解决slam中三维场景的定位和地图构建问题，最早可追溯到loam(lidar odometry and mapping)算法，该算法以新颖的方法提取激光点云线面特征减少计算量，并开创性地将运动估计问题分成两个独立算法来共同完成，一个算法执行高频率的里程计返回低精度的运动估计，另一个算法运行频率较低的匹配建图但返回高精确的运动估计，最终将两个数据融合成高频率高精度的运动估计，很好地权衡了精度和效率，实时性高，唯一的不足点在于缺少回环检测，这将给建图和位置估计带来较大的累计漂移误差。lego-loam(lightweight and ground-optimized lidar odometry and mapping)在loam的基础上衍生出新的框架，在特征提取上提升轻量化和地面优化，并增加了回环检测，整个框架与目前成熟的图优化3d激光雷达slam方案框架吻合，构建的地图更加完善。而基于velodynehdl-32e激光雷达提出的图优化(graph-based)slam方案hdl_gragh_slam，其通过在后端优化过程中融合全球定位系统(global position system,gps)、惯性测量单元(inertial measurement unit,imu)和路面约束信息，可以更好地构建全局一致性地图。然而，增加传感器和回环检测会给系统带来较大的计算开销且回环检测存在感知偏差和感知变异，需要在准确率和召回率之间作权衡处理，影响了slam的实时性。

3、除了建图，这些slam算法在全局位姿估计方面提出了基于点云配准算法获取当前帧的激光里程计。但是点云的稀疏性和运动干扰会使得激光里程计的精度急剧下降。通过融合imu的状态估计数据可提升激光里程计的精度。其主要分为两类方法，一类是松耦合方法，该方法分别计算激光雷达和imu的状态估计数据，以舍弃大量不相关数据信息为代价来换取较小的计算负载。另一类是紧耦合方法，通过imu测量提供状态预测，同时测量更新被用于修正预测的状态，领域内也称之为激光惯性里程计(lidar inertial odometry,lio)。lio_mapping(lidar inertial odometry and mapping)首次提出了紧耦合的lidar-imu融合方法，通过联合优化imu和lidar的测量数据校正漂移误差，相较于松耦合方法，紧耦合方法会使精度大幅提升。作为lego-loam的更新，lio-sam在图优化框架的基础上实现了紧耦合激光——惯性里程计。这类基于点云配准的里程计算法依赖于激光点云的特征提取以及运动状态估计，具有较高的时间复杂度，且里程计仅包含运动先验信息，容易造成位姿估计不准确。

4、先前的工作主要集中在如何处理纯几何信息以校正点云误差，忽略了强度信息。事实上，强度信息与材料的反射率有关，对于不同类别物体材料反射率的区别尤其明显，这对于定位和物体识别很有价值。如果在slam系统中引入反射标记物，可以利用激光雷达对于高反射率材料易辨别的特性，通过固定反射标记物的位置，根据测得的角度和距离信息来解算激光雷达位置坐标，从而实现激光雷达的自主定位。

5、本方法仅通过激光雷达获取三维场景反射参考点的角度和距离信息完成位置解算，并未引入类似于里程计等利用先验信息进行后验估计的算法，能够校正运动过程中由帧间估计导致的累计漂移误差，从而代替计算复杂度高和检测效率低的回环检测，有望实现在室内场景下的高精度定位。然而，在实际情况下，反射标记物激光点云并非理想质点模型，而是由散点集合表示，这就要求必须考虑激光雷达角度分辨率以及反射标记物的形状大小来筛选参考点，避免由于参考点选择不当导致的定位误差。

技术实现思路

1、本发明的目的在于提供一种基于激光里程计的室内高精度自定位方法。与传统激光里程计方法相比，本发明利用激光雷达对于高反射率材料具有较高反射强度值的特性，通过在参考位置引入反射标记物，进而根据标记物的角度和距离信息解算激光雷达的位置；同时考虑到在实际应用中反射标记物并非理想质点模型，需要根据激光雷达角度分辨率以及反射标记物的形状大小来选取参考点。由于受到激光雷达测量距离限制以及运动畸变干扰，仅靠单个反射标记物难以完成复杂环境下的远距离室内高精度定位，该方法引入了n个形状不同的反射标记物以完成远距离定位，可在实现校正里程计累计漂移误差的同时大幅度降低传统激光里程计的算法时间复杂度。

2、本发明所采用的技术方案为：一种基于激光里程计的室内高精度自定位方法，包含机械旋转式三维多线激光雷达移动平台、便携式计算设备和n个参考反射标记物。将激光雷达设备固定于移动平台上，将激光雷达作为原点的坐标系称为激光雷达坐标系，其对应每一帧点云所构成的局部地图，而以激光雷达初始位置为原点的坐标系世界称为世界坐标系，对应所有点云构成的全局地图。在室内环境中参考位置放置特制形状的反射标记物ri(1≤i≤n)，其由高反射率的反光材料构成，特征为具有较高的反射强度值，记录ri在世界坐标系下的坐标值其特征在于包括以下步骤：

3、步骤一、启动激光雷达，通过相连接的便携式计算设备获取点云数据并存储到集合同时取出的反射强度值i归一化后存放在集合排除同一物体因为距离变化导致强度值i不同的影响，以便后续用于反射标记物ri的特征提取，其表示为：

4、

5、步骤二、对收集到的集合逐帧将ii从大到小进行排序，滤去个别强度值较大的孤立点集，记录其索引值index，得到集合

6、步骤三、对第i帧处理后的点云集合中的最大强度值利用聚类算法分为m个子集合cu(1≤u≤m≤n)，对应于空间中m个反射标记物的点云(单帧点云中可能无法出现所有反射标记物)；

7、步骤四、从子集合cu中取出反射强度值最大的点及其距离和方位角等原始数据，存放在ti中，分别对集合cu构成直线和集合cu构成平面这两种情况进行处理；

8、步骤五、若ti集合中仅包含单条线束上的点云，则对index取均值即可得到中心点的位置idi，若ti集合中包含多条线束点云，则对id号求平均值取整后得到单条线束索引值idmid，再将idmid中的点云id号求平均值得到idi，最后将计算得到的idi保存到ref集合(即反射标记物ri对应的参考点云集合)；

9、步骤六、记录激光雷达坐标系绕世界坐标系z轴旋转的角度根据参考点ref的索引号index可在点云集合中得到激光雷达坐标系下入射激光束与xoy平面形成的入射角α、参考点ref在xoy平面的投影与y轴形成的夹角γ以及激光雷达到ref的距离值d；

10、步骤七、根据激光雷达坐标系绕世界坐标系z轴旋转的角度激光雷达坐标系下的入射角α和水平旋转角γ以及距离值d计算出参考点ref在激光雷达坐标系下的坐标值其关系满足：

11、

12、步骤八、假设激光雷达在世界坐标系下的坐标值为(xl,yl,zl)，根据激光雷达坐标系和世界坐标系的转换关系，可将参考点ref从激光雷达坐标系转换到世界坐标系，其转换关系表示为：

13、

14、步骤九、根据上述转换关系，可求得在每一帧点云中激光雷达在世界坐标系下的坐标值(xl,yl,zl)为：

15、

16、其中，为参考点ref在激光雷达坐标系下的坐标，其等价关系为：

17、

18、步骤十、可进一步将其转化为矩阵的形式：

19、

20、步骤十一、上述步骤得到了基于某个反射标记物ri的激光雷达位置坐标，进一步对步骤三的m个子集合cu重复步骤四至十，可得到分别由m个反射标记物计算出的激光雷达位置坐标(xl,yl,zl)；

21、步骤十二、根据步骤五中每个子集cu对应的ref，归一化其距离值d之和，计算各参考点的距离值权重来估计激光雷达的位置(x′l,y′l,z′l)，其表达式为：

22、

23、步骤十三、对点云集合中所有帧进行上述步骤计算则可得到slam过程中激光雷达的运动轨迹，完成激光雷达室内高精度自定位。