一种机器狗厂区三维建图与定位方法与流程

本发明涉及激光定位与测绘，具体为一种机器狗厂区三维建图与定位方法。

背景技术：

1、在智慧工厂、变电站及仓储车间等厂区环境中，四足机器人能够搭载激光雷达执行自主巡检任务，并基于采集的点云数据完成环境三维建图与自身定位；例如，公开的发明专利申请cn111045017b公开了一种激光和视觉融合的巡检机器人变电站地图构建方法，通过融合视觉定位信息与激光定位信息，提高巡检机器人在特征不足场景下的地图构建能力；又如，公开的发明专利申请cn118163880b公开了一种建筑物病害检测四足机器人及检测方法，通过惯性测量信息对激光点云进行运动畸变校正，并根据校正后的点云实现定位与检测；

2、然而，在厂区长直巷道、连续墙面及重复布置的货架或立柱区域内，沿机器人行进方向可用于点云配准和位姿估计的有效几何特征较少，容易造成该方向的定位约束不足；与此同时，四足机器人在行进过程中存在周期性足端触地过程，足端触地会引起机体竖直冲击以及俯仰、横滚方向的姿态振动，使机器人在单帧激光扫描周期内的位姿发生非平滑变化，进而造成同一环境结构在点云中的位置拉伸、错位或重叠；由于几何退化判断通常依赖点云特征分布及配准约束信息，受步态冲击影响产生的点云畸变会干扰对真实几何退化状态的识别，使系统难以准确区分由环境结构单一造成的约束不足与由机器人自身振动造成的点云异常；在此情况下，激光点云匹配结果以及受同类运动扰动影响的惯性测量信息和腿式里程信息均可能产生偏差，导致机器人位姿误差持续累积，最终造成沿巷道方向的定位漂移、竖直方向的高程偏差以及回环区域三维地图的分层和重影；因此，亟需一种适用于四足机器人厂区巡检场景的三维建图与定位方法，能够降低步态冲击引起的激光点云运动畸变对几何退化判断和位姿估计的影响，提高退化场景下定位结果与三维地图构建结果的稳定性和准确性。

技术实现思路

1、针对现有技术的不足，本发明提供了一种机器狗厂区三维建图与定位方法，解决了上述背景技术中的问题。

2、为实现上述目的，本发明提供如下技术方案：

3、一种机器狗厂区三维建图与定位方法，包括：

4、s1、同步采集激光点云、惯性数据、腿部运动数据、足端触地信号及步态相位信号，建立激光点与步态相位、触地时刻之间的对应关系；

5、s2、依据步态相位和惯性数据，将单帧激光点云划分为低扰动时段点云和高扰动时段点云，并分别对激光点进行运动校正；

6、s3、依据低扰动时段点云识别弱约束方向，分别基于引入高扰动时段点云前后的弱约束方向变化和跨步态周期持续状态，确定几何退化方向；

7、s4、依据地面结构和侧向立面结构建立非退化方向约束，依据沿退化方向周期出现的结构特征建立退化方向校正位置，并依据腿部运动数据约束相邻校正位置之间的位姿变化；

8、s5、依据非退化方向约束和退化方向校正位置，对激光点云配准位姿增量与基于腿部运动数据确定的位姿增量进行一致性校验，更新机器人位姿，并依据更新后的机器人位姿配准点云和更新三维地图。

9、优选的，s1，包括：

10、在建立时间对应关系时，以激光帧起始时刻为时间基准，读取或依据扫描顺序生成激光点的点级时间戳；

11、将接地状态保持时间大于标定所得最大信号抖动持续时间的接地转换确定为有效足端触地事件；

12、按照不大于惯性采样周期一半的同步误差关联惯性数据、关节状态及步态相位，并记录激光点与最近有效足端触地事件之间的相对时间差。

13、优选的，s2，包括：

14、依据稳定支撑状态下的惯性基线形成综合扰动指标，并采用进入门限和退出门限确定点云处理状态；

15、依据有效支撑足对应的腿部运动数据校正低扰动点云；

16、依据高扰动时段前后的低扰动校正有效位姿，对高扰动点云进行受限插值校正；

17、依据结构拟合残差确定有效稳定结构。

18、优选的，依据高扰动时段前后的低扰动校正有效位姿，对高扰动点云进行受限插值校正，包括：

19、在对高扰动点云进行校正时，选取高扰动时段前后时间距离最近且具有低扰动校正有效标识的参考位姿，且前后参考位姿之间的时间跨度不超过一个当前步态周期；

20、依据激光点的点级时间戳确定插值比例；

21、对参考位姿的平移部分进行线性插值，对旋转部分进行单位四元数球面线性插值；

22、缺少前后参考位姿时，将对应激光点标记为未校正受扰点。

23、优选的，s3，包括：

24、建立低扰动校正点云中的有效稳定结构与局部参考点云之间的结构对应关系；

25、依据局部平面结构和局部边缘结构的约束残差，确定各运动方向对应的约束强度；

26、分别基于低扰动校正点云以及低扰动校正点云与高扰动校正点云的合并点云，判定各运动方向的弱约束状态；

27、将在不少于一个当前步态周期内持续满足弱约束判定条件的同一运动方向确定为几何退化方向。

28、优选的，依据局部平面结构和局部边缘结构的约束残差，确定各运动方向对应的约束强度，包括：

29、对有效结构对应关系中的局部平面结构确定点到参考平面的距离残差，对局部边缘结构确定点到参考边线的垂直距离残差；

30、依据结构有效点数量和平均拟合残差确定结构权重；

31、将各结构残差按照六个运动方向进行线性化处理并加权累加形成约束信息矩阵，根据约束信息矩阵的归一化二次型值确定各运动方向的约束强度。

32、优选的，s4，包括：

33、以最近一次确认的退化方向校正位置或者非周期锚定结构位置作为位置基准；

34、依据腿部运动数据确定周期结构特征在几何退化方向上的预测位置；

35、将唯一落入匹配允许范围且符合基准序列间隔关系的周期结构特征位置确定为退化方向校正位置；

36、将位于允许位移范围内且累计位置误差未达到累计位置误差允许值的短时位姿变化用于相邻退化方向校正位置之间的位置更新。

37、优选的，依据腿部运动数据确定周期结构特征在几何退化方向上的预测位置，包括：

38、确定周期结构特征的预测位置时，剔除滑移可疑足对应的腿部运动数据；

39、依据剩余有效支撑足的关节角度变化和腿部运动学关系确定自位置基准起的短时位移；

40、将短时位移叠加至位置基准，并结合短时位移误差边界确定周期结构特征的预测位置范围。

41、优选的，s5，包括：

42、位姿更新时，以各运动方向对应的一致性容差对激光点云配准位姿增量与本体位姿增量进行校验；

43、对于非退化方向，依据已确认的结构对应关系确定对应位姿更新量；

44、对于几何退化方向，依据退化方向校正位置以及位于允许位移范围内且累计位置误差上限小于累计位置误差允许值的短时位姿变化确定对应位置更新量；

45、将经结构对应关系确认的低扰动校正点云写入稳定结构地图，将高扰动校正点云写入临时占据地图。

46、优选的，对于非退化方向，依据已确认的结构对应关系确定对应位姿更新量，包括：

47、地面结构对应竖直平移、俯仰及横滚方向，侧向立面结构对应横向平移及偏航方向；

48、当激光点云配准位姿增量与本体位姿增量的差值不大于对应一致性容差时，按照标定误差方差对二者进行加权融合；

49、当差值大于对应一致性容差且结构匹配残差不大于结构拟合残差允许值时，采用激光点云配准位姿增量更新对应方向位姿。

50、与现有技术相比，本发明提供了一种机器狗厂区三维建图与定位方法，具备以下有益效果：

51、1、本发明，通过将激光点云与步态相位、有效足端触地事件及惯性观测值进行逐点时间关联，依据扰动状态分别对低扰动时段点云和高扰动时段点云进行运动校正，并根据引入高扰动校正点云前后的方向约束变化及跨步态周期持续状态确定几何退化方向；进一步利用地面结构和侧向立面结构约束非退化方向位姿，利用周期结构校正位置、非周期锚定结构及受限短时位姿变化约束几何退化方向位置，并结合位姿增量一致性校验和分层地图更新处理点云数据；由此，能够降低足端触地冲击造成的点云畸变对几何退化判断和位姿更新的干扰，减少长直通道内的定位漂移、高程偏差以及重访区域中的地图分层和重影，提高厂区巡检三维建图与定位的稳定性和准确性。

52、2、本发明，通过对时间同步状态、有效足端触地事件、支撑足滑移状态和点云结构有效性进行判定，对时间关联无效点、滑移可疑足关联数据、未校正受扰点以及结构匹配未通过的数据进行标记处理；将经有效结构对应关系确认的低扰动校正点云写入稳定结构地图，将高扰动校正点云对应的占据信息写入临时占据地图，并依据后续校正有效观测对待确认结构进行复核；由此，能够减少异常数据和短时受扰观测对稳定结构地图的错误写入，保持地面结构、侧向立面结构及锚定结构地图信息的可靠性，提高局部遮挡、足端滑移或者传感数据异常情况下地图更新的连续性。