一种室内移动载体导航定位建模及路径规划方法与流程

本发明属于移动载体导航，具体为一种室内移动载体导航定位建模及路径规划方法。

背景技术：

1、室内移动载体导航，是针对机器人、agv、巡检车、仓储叉车等室内移动载体，在无gps 信号封闭空间实现自主定位与路径规划的技术体系，依托激光雷达、uwb、视觉、地磁等室内定位传感器获取载体实时坐标，结合室内地图完成环境建模，系统实时避障、动态规划最优行驶路线，控制载体自主抵达目标点位，适配厂房、商场、地下车库、机房等场景。

2、传统室内导航定位建模多依赖单一激光/视觉slam，长走廊、纯白墙面、对称户型室内易产生累计位姿漂移，建图存在尺度畸变，闭环匹配正确率低，校正精度极差；多传感器数据采样频率不同、时钟独立，时空无法精准对齐，融合误差持续性累积，面对动态人流、临时堆放物、可移动家具场景，定位稳定性大幅下降；路径规划采用原生a*、基础人工势场法，搜索遍历节点冗余量大，嵌入式终端运算耗时久，极易陷入局部极小值死锁；生成路径直角拐点多、平顺度差，未结合载体尺寸设置安全边界，狭窄通道碰撞概率高；建模仅构建二维几何栅格，无语义分层标注，全局局部规划相互割裂，环境突变无法毫秒级重规划，适配性与作业鲁棒性不足。

技术实现思路

1、本发明的目的在于提供一种室内移动载体导航定位建模及路径规划方法，以解决上述背景技术中提出的问题。

2、为了实现上述目的，本发明提供如下技术方案：一种室内移动载体导航定位建模及路径规划方法，具体步骤如下：

3、步骤一：多源感知数据同步采集预处理

4、统一校准激光雷达、视觉相机、imu惯性单元、uwb基站四类车载传感器时序与外参矩阵，同步采集多源感知数据，完成滤波降噪、坐标系统一转换，剔除异常无效感知数据；

5、步骤二：室内分层复合环境建模构建

6、基于预处理数据，耦合构建二维栅格地图、拓扑通路网络图、语义功能标签地图三层同坐标系复合地图，配置地图闭环纠偏与增量更新机制；

7、步骤三：多因子融合实时高精度定位解算

8、搭建自适应权重粒子滤波融合模型，融合地图特征、轮式里程、uwb测距、视觉特征四类观测值，动态适配场景权重解算载体实时位姿；

9、步骤四：导航起点、终点与通行约束初始化

10、录入导航起止点位姿，载入载体动力学参数、分级安全距离参数、语义全域通行约束，校验起止点可通行连通性后锁定导航约束；

11、步骤五：自适应加权全局最优路径粗规划

12、采用多维代价改进a*算法，结合距离、通道宽度、语义拥堵、能耗代价因子搜索节点，剪枝冗余节点，生成拐点式全局无碰撞粗路径；

13、步骤六：路径平滑优化与安全膨胀层校正

14、剔除粗路径小幅偏转冗余拐点，采用三阶b样条拟合连续曲率轨迹，分级构建障碍物安全膨胀缓冲区，生成合规基准平滑导航轨迹；

15、步骤七：动态障碍物感知与局部实时重规划

16、联动激光-视觉双识别模块预判动态障碍物运动轨迹，侵入基准轨迹时，依托改进动态窗口法开展局部绕行重规划，全局路径保持不变；

17、步骤八：轨迹跟踪闭环控制与地图迭代更新

18、通过双闭环pid控制器调控载体行驶姿态，补偿行驶轨迹偏差；导航作业全程增量采集环境数据，迭代更新三层复合地图，完成导航全流程闭环。

19、作为本发明的一种优选技术方案，步骤一中所述的多源感知数据同步采集预处理具体包括：主控板下发同频触发脉冲统一四类传感器10hz采样频率，完成传感器手眼标定与外参矩阵校准；同步采集三维激光点云、rgb深度图像、三轴角速度/加速度姿态数据、四点uwb基站测距原始数据；采用自适应统计滤波剔除激光反射伪点、远距离离群点，高斯滤波去除图像噪点，搭建imu温漂补偿模型修正静态零偏，将所有感知数据统一转换至载体底盘中心车体坐标系，剔除超出测距阈值的无效数据。

20、作为本发明的一种优选技术方案，步骤二中所述的室内分层复合环境建模构建具体包括：栅格地图采用gmapping-slam算法划分5cm精度栅格单元，二值化区分可通行栅格与障碍物栅格；拓扑地图以室内拐点、房门、电梯口为拓扑节点，拟合节点间可通行直线路径为连通边，连通边绑定通道宽度、坡度属性，简化全域通路逻辑；语义地图依托室内轻量化yolo模型，识别标注作业区、人行通道、禁行区语义标签，三层地图共用同一世界坐标系，同时搭载回环匹配模块，载体途经重复点位自动校正地图畸变，支持新增障碍物增量覆写更新，形成一体化可迭代复合环境模型。

21、作为本发明的一种优选技术方案，步骤三中所述的多因子融合实时高精度定位解算具体包括：调取步骤二预建复合地图，实时提取激光平面特征、视觉角点特征完成地图匹配初定位；依托车载轮式里程计输出载体行进里程增量，结合imu姿态数据推算短时航迹，弥补特征缺失时段定位空白；引入uwb四点测距约束方程，构建全局坐标约束边界，消除航迹推算累积误差；搭建动态权重粒子滤波融合框架，设置场景自适应调权规则：空旷长走廊提升激光定位权重占比65%，弱纹理纯白区域提升uwb无线定位权重，人流遮挡区域提升视觉特征定位权重；设定定位收敛阈值，单次迭代粒子残差小于0.03m即输出结果，实时输出载体厘米级平面坐标、航向角、行驶速度六位实时位姿数据，定位频率稳定10hz。

22、作为本发明的一种优选技术方案，步骤四中所述的导航起点、终点与通行约束初始化具体包括：接收上位机调度指令或人工触控指令，选取复合地图内可通行栅格点位，锁定导航起始点位姿、目标终止点位姿，同步匹配起止点最优朝向角度；录入载体固有动力学参数：底盘外接半径、最大直行速度、最大转向角速度、加减速响应时长、最小转弯曲率；分级配置三类安全避让距离：常规通道0.3m基础安全距离、狭窄通道0.5m强化安全距离、人行交汇区0.8m避让安全距离；批量导入语义地图固化约束：永久性禁行区、限时关闭通道、低速通行区、单侧通行通道权限规则；同时判定起止点连通性，当点位处于障碍物栅格、禁行语义区域，弹窗报错并重新选取点位，校验通过后锁定全部导航约束参数，进入路径规划环节。

23、作为本发明的一种优选技术方案，步骤五中所述的自适应加权全局最优路径粗规划具体包括：改良a*欧式距离启发函数，新增多维加权代价模型，代价权重自适应迭代调整，其中通行距离权重保障路径里程最短，通道宽度权重筛除小于载体通行宽度的窄缝路径，语义代价权重规避高频人流拥堵区、颠簸地面、高能耗通行区域；改进a*算法核心代价公式为：f(n)=g(n)+h(n)，式中f(n)为节点综合通行代价，g(n)为起点至当前节点实际通行代价，h(n)为当前节点至终点欧式距离启发代价；算法遍历栅格节点时，增设节点剪枝规则，剔除共线、迂回高代价拓展节点，以全域通行代价最小值为标准输出由离散拐点组成、无全局碰撞风险、里程最优、合规通行的全局拐点粗路径。

24、作为本发明的一种优选技术方案，步骤六中所述的路径平滑优化与安全膨胀层校正具体包括：遍历全局粗路径拐点集合，采用斜率差值判定法，剔除偏转角度小于8°的路径共线冗余拐点，精简路径锚点数量；采用三阶b样条曲线插值拟合剩余离散拐点，约束路径最大曲率不超过载体最小转弯曲率，消除90°直角急转、锐角转向，保证全程轨迹曲率连续；基于载体底盘半径，对路径周边栅格做分级腐蚀膨胀处理：永久障碍物周边做一级膨胀扩距，临时可移障碍物周边做二级动态膨胀，空旷通行区域做最小基础膨胀；校验拟合后轨迹所有点位，判定轨迹栅格与障碍物膨胀栅格无交集，生成兼具行驶平顺性、全域被动防撞能力的基准平滑导航轨迹。

25、作为本发明的一种优选技术方案，步骤七中所述的动态障碍物感知与局部实时重规划具体包括：依托激光雷达前沿探测和视觉目标识别，划定载体前方3m×2m前置预警感知区，分类识别行人、转运小车、临时堆放三类动态障碍物，通过连续三帧探测数据测算障碍物移动速率、行进方向、占用通行时长，预判障碍物3s运动轨迹，判定是否侵入基准安全导航轨迹；障碍物侵入轨迹时，截取前后1.5m局部路径区间并结合实时车速、转向极限，快速生成绕行子轨迹，进行重规划，重规划仅修改局部区间路线，绕行轨迹首尾点位对接原有全局基准路径，重规划响应时延≤120ms，绕行结束后自动回归原路径。

26、作为本发明的一种优选技术方案，步骤八中所述的轨迹跟踪闭环控制与地图迭代更新具体包括：采用外环位姿偏差pid运算、内环动力调速双闭环控制器，将载体行驶横向、航向偏差控制在±2cm以内；载体行进全过程持续采集周边环境增量点云、图像数据，比对原始复合地图，识别新增障碍物、移除类障碍物，夜间低光环境自动提升激光更新权重；导航抵达终点后校验停靠位姿精度，同步增量覆写三层复合地图数据，实现多任务连续闭环导航。

27、本发明的有益效果如下：

28、本发明实现多源传感器硬件时钟同步+时空配准融合建模，搭建栅格-拓扑-语义三层耦合复合地图，添加特征双重闭环校验机制，有效抑制定位漂移，适配弱纹理、对称、长通道特殊场景；采用语义加权改进a*算法联动动态窗口法，全局路径剪枝降噪，局部路径样条平滑，路径无直角急转，适配载体运动动力学特性；动态分级设置障碍物安全膨胀缓冲区，规避人机、载体物体碰撞，联动全域语义禁行区自主绕行，打通全局-局部规划联动通道，动态障碍物来袭即时局部重规划，兼顾定位精度、通行效率、行驶平顺性，适配仓储agv、楼宇巡检机器人等全品类室内载体。