一种基于角度的点云数据纠正方法与流程

本申请涉及传感器数据纠正的，尤其适用于一种基于角度的点云数据纠正方法。

背景技术：

1、在旋转式点云采集系统中，例如机器人的激光雷达，专用于采集环境点云数据以进行定位导航，由于机械振动或运动误差，采集的点云数据常存在坐标偏移。传统方法控制机器人处于静态下校准点云数据，但无法适应动态误差变化。

2、专利申请号为202111453891.2的中国发明专利公开基于点云特征点的惯导误差消除方法，惯导误差消除方法为了校正机器人惯导的累积误差，会选择在在移动机器人行走过程中对采集的点云进行特征点匹配，这一匹配过程中需要建立描述符，再加上采用pnp(perspective-n-point)算法，得到特征点的位姿变换，但是，后续将惯导的位姿变换与3d-tof点云特征点的位姿变换比较，再将比较得到的偏差转移为惯导上的误差，最后对惯导的误差进行动态校正，没有直接对3d-tof传感器采集的原始点云数据进行校正，从而无法实时跟踪定位原始点云处的障碍物与机器人之间位置关系，若采用实时采集的点云数据检测障碍物，则机器人可能在碰触到障碍物的情况下才准确检测到障碍物，这一过程会耗费时间和较多算力去处理实时采集的点云数据，导致产生机器人持续顶住障碍物、机器人接触障碍物时间较长等等碰撞行为，给机器人带来未知危险。

技术实现思路

1、本申请旨在提供一种基于角度的点云数据纠正方法，具体技术方案如下：

2、一种基于角度的点云数据纠正方法，点云数据纠正方法的执行主体是安装码盘和激光传感器的机器人；机器人使用激光传感器采集到一帧点云后，机器人朝着所述一帧点云所分布的位置行走并在行走的过程中使用码盘测得运动距离；机器人朝着所述一帧点云所分布的位置行走的过程中执行点云数据纠正方法；所述点云数据纠正方法包括：步骤a、按照预设采样周期对码盘测得的运动距离进行采样，再将每个预设采样周期内采样到的运动距离换算为周期采样距离和周期采样角度，并且逐个预设采样周期地将周期采样距离和周期采样角度保存于目标内存空间；然后执行步骤b；步骤b、根据预设采样周期与单位角度采样时间的比值设置机器人在预设采样周期内对应的角度索引遍历范围；再在对应的角度索引遍历范围内，按照同一角度索引值对每个预设采样周期内保存的周期采样距离进行累加，直至累加完目标内存空间内所有的周期采样距离，得到同一角度索引值对应的点云距离纠正值；并按照同一角度索引值对每个预设采样周期内保存的周期采样角度进行累加，直至累加完目标内存空间内所有的周期采样角度，得到同一角度索引值对应的点云角度纠正值；然后执行步骤c；其中，一个预设采样周期对应一个角度索引遍历范围，一个角度索引遍历范围支持划分成多个角度索引值；步骤c、对所述一帧点云的位姿角度进行遍历，每当从所述一帧点云中遍历到位姿角度等于所述角度索引值的点时，基于所述角度索引值对应的点云距离纠正值和点云角度纠正值计算点云坐标偏移纠正值，再使用点云坐标偏移纠正值对遍历到的点的坐标进行纠正，以纠正完所述一帧点云中各个点的坐标。

3、综上，本申请通过执行步骤a至步骤c，为所述一帧点云中对应角度索引值的点分别计算多个预设采样周期内累加出的点云距离纠正值和点云角度纠正值，再基于所述角度索引值对应的点云距离纠正值和点云角度纠正值对所述一帧点云中对应匹配的角度索引值的点的坐标进行纠正，最终实现对所述一帧点云的运动纠正，等效于机器人为所述一帧点云补偿了其最近行走向所述一帧点云处分布的障碍物所耗费的运动时间(大于等于所述多个预设采样周期)内积累的点云距离纠正值和点云角度纠正值，起到预测所述一帧点云在同等时间内的相对运动位置，从而为机器人本身提供可能与障碍物发生碰撞区域，使碰撞行为的检测具有实时性，不会让机器人持续顶住障碍物时才检测到发生碰撞。

1.一种基于角度的点云数据纠正方法，点云数据纠正方法的执行主体是安装码盘和激光传感器的机器人；

2.根据权利要求1所述点云数据纠正方法，其特征在于，在步骤b中，所述根据预设采样周期与单位角度采样时间的比值设置机器人在预设采样周期内对应的角度索引遍历范围的方法包括：

3.根据权利要求2所述点云数据纠正方法，其特征在于，所述根据结束角度索引值计算目标上限索引值的方法包括：

4.根据权利要求3所述点云数据纠正方法，其特征在于，所述步骤b具体包括：

5.根据权利要求4所述点云数据纠正方法，其特征在于，第一次执行步骤b5时，还包括：

6.根据权利要求4所述点云数据纠正方法，其特征在于，每执行步骤b7一次，更新出的起始角度索引值增大一次，然后执行步骤b1和步骤b2，使步骤b2计算出的目标上限索引值增大一次，进而带动所述当前预设采样周期内对应的角度索引遍历范围扩大一次。

7.根据权利要求4所述点云数据纠正方法，其特征在于，在步骤a中，换算周期采样距离和周期采样角度的方法包括：

8.根据权利要求7所述点云数据纠正方法，其特征在于，在计算得到周期采样距离和周期采样角度后，将机器人的左驱动轮安装的码盘在当前时刻测得的运动距离更新为机器人的左驱动轮安装的码盘在参考时刻测得的运动距离；并将机器人的右驱动轮安装的码盘在当前时刻测得的运动距离更新为机器人的右驱动轮安装的码盘在参考时刻测得的运动距离。

9.根据权利要求1所述点云数据纠正方法，其特征在于，在步骤c对所述一帧点云的位姿角度进行遍历之前，存在：从0度开始按照1度的角度间隔为所述一帧点云递增设置位姿角度；每设置一个位姿角度则为对应点的坐标添加位姿角度，直至添加的位姿角度递增至359，确定为所述一帧点云添加360个位姿角度并形成各个点的索引顺序，使所述一帧点云按照所述角度索引值转换成分辨率为1度的点的集合；

10.根据权利要求9所述点云数据纠正方法，其特征在于，在步骤c中，每当从所述一帧点云中遍历到位姿角度等于所述角度索引值的点时，基于所述角度索引值对应的点云距离纠正值和点云角度纠正值计算点云坐标偏移纠正值，再使用点云坐标偏移纠正值对遍历到的点的坐标进行纠正的方法包括：

11.根据权利要求10所述点云数据纠正方法，其特征在于，在步骤c中，基于所述角度索引值对应的点云距离纠正值和点云角度纠正值计算点云坐标偏移纠正值，再使用点云坐标偏移纠正值对遍历到的点的坐标进行纠正的方法包括：

12.根据权利要求11所述点云数据纠正方法，其特征在于，在得到所述角度索引值对应的一个点的纠正横坐标及其纠正纵坐标后，将所述角度索引值对应的一个点的纠正横坐标更新为当前遍历的位姿角度对应的一个点的横坐标，并将所述角度索引值对应的一个点的纠正纵坐标更新为当前遍历的位姿角度对应的一个点的纵坐标；然后执行步骤c8。