针对机器人在公司一期宿舍运行情况，出现多次定位丢失/漂移问题，总结如下2项问题发生原因：

问题1：静态障碍物与地图不匹配，变化占比面积接近30%，导致定位失准，可能出现明显漂移，甚至导致定位丢失。如下图（彩色点为激光点，黑色为地图特征），除红圈部分外，其他部分匹配基本准确，但依然导致全局地图定位漂移了近200mm，严重影响导航定位精度；

问题2：地面不平整、机器人运动速度和加速度过高等情况，导致机器人打滑，导致局部融合里程计失准，导致定位漂移甚至定位丢失问题；

针对如上问题，做问题归纳分析和解决方案分析，现有问题已完成开发和测试。

问题1：静态环境，临时增加占据传感器视野30%的长期静态障碍物（地图未有的障碍物），对激光雷达与地图匹配率的影响，导致定位漂移甚至定位丢失； ● 解决方案： ① 全局地图定位优化 上一代定位算法依赖amcl（自适应蒙特卡洛粒子滤波）和PP-ICP（点对点的迭代最近点匹配）对机器人进行运动追踪和激光点云地图匹配，机器人运动模型为差速轮模型（未考虑打滑和其它非线性误差），在定位鲁棒性方面存在问题； 2022年1月15日至今，在原有amcl算法基础上，针对定位鲁棒性不强问题，做了如下改进方案： 1）不再使用大地图进行匹配，而是从大地图提取机器人周边尺寸的小地图，进行地图线性特征提取和地图多重分辨率的变采样匹配搜索，丢弃匹配失败的结果（如临时增加的障碍物），提高了全局地图匹配精度，并增强了在超大型地图（4000平方以上）中进行定位的ICP算法收敛速度和定位鲁棒性； 2）建立机器人运动的雅可比矩阵+高斯牛顿梯度优化算法，对粒子滤波器产生的位姿进行非线性优化，即对现实误差及机器人运动模型进行数学建模升级，增强算法模型可靠性； 3）取消运动滤波器限制，即在机器人静态情况下，也进行粒子滤波采样，并限制重采样粒子簇大小，不再限制旋转/平移一段距离时才进行粒子采样，增强了其静态全局定位鲁棒性； 4）原有使用PP-ICP方法进行地图匹配，升级为PL-ICP方法（点对线的迭代最近点匹配），并在地图特征提取策略上做优化改进，提取满足一定条件的线性特征地图数据，与经过线性拟合的激光雷达进行匹配，并对帧间匹配位姿结果进行了基于高斯牛顿法的非线性优化，既降低整体算法的计算消耗（CPU占用率），又提高了整体的定位精度及鲁棒性； 5）通过上述改进，将原有算法每秒10HZ的全局定位输出频率，优化至20HZ以上，确保机器人在1.5m/s内的运动速度，均能被良好追踪，并为路径导航控制程序提供了足够的定位采样频率，提高整体导航和定位的精度；

● 测试效果：经过测试，在机器人靠近与地图不匹配的静态大型障碍物情况，全局定位均能呈现精准的定位效果、未发生定位漂移或丢失情况，定位精度和鲁棒性明显高于上一代定位算法；现已应用于1期宿舍运行的3台衣物搬运AGV，效果良好，达到预期；基本解决了公司宿舍衣物搬运场景的定位漂移/丢失问题，并使整体导航定位精度从±40mm提高到了±20mm以内（精度取决于地图精度和临时增加的静态障碍物空间占比情况）。

② 局部融合里程计优化 上一代局部里程计采用了将轮式里程计及IMU进行EKF（扩展卡尔曼滤波）融合，融合了轮式里程计的位姿及IMU的欧拉角及旋转加速度信息，在地面平整或不发生打滑的情况下，具备良好的定位鲁棒性； 2022年1月15日至今，在原有EKF融合里程计算法基础上，针对轮式里程计误差较大、机器人打滑、地面不平整时的定位鲁棒性不强问题，做了如下改进方案： 1）增加了基于csm相关性扫描匹配+高斯牛顿非线性优化的激光里程计； 2）在静态环境、特征良好的环境下，由于激光里程计不受机器人打滑影响，且具备20mm内的定位精度，故取位姿部分进行EKF扩展卡尔曼滤波融合，令局部里程计融合了轮式里程计+IMU+激光里程计，增强其定位鲁棒性；

● 测试效果：经过测试，升级算法后，在强行抵住机器人强行运动打滑、手动搬运机器人情况下，局部里程计均能呈现良好的运动追踪和里程计位姿输出结果；

问题2：打滑、地面不平及机器人运动速度、加速度过高导致的局部里程计漂移失准问题； 解决方案： 同问题1的局部融合里程计方案；增加激光里程计进行EKF融合即可解决该问题，测试效果即上文所述。

总结： 定位问题实际可分成两部分考虑：全局地图定位和局部融合里程计；两部分是递进关系，局部里程计为全局定位提供了良好的先验估计值，才能进一步提供整体定位鲁棒性；若单靠全局地图定位，不对局部里程计进行优化，局部融合里程计的漂移还是会产生定位失效（漂移或丢失）问题。 综上，本次算法优化分别针对了全局地图定位和局部融合里程计进行了优化，解决公司1期宿舍场景的机器人定位丢失问题；

如上文解决的问题，是在公司1期宿舍的衣物搬运场景所遇到的问题，并提供了足够良好的解决定位算法方案； 但实际后续推广应用还存在如下问题点，并相应地提出了解决方案：

问题1：静态环境，空旷、低特征的仓库或长廊环境中，激光雷达仅能扫描到机器人一侧或左右两侧环境，导致激光里程计无法匹配、融合里程计失效问题（机器人实际在运动，激光里程计却静止不动） ● 解决方案： 1） 硬件升级，更换采用远距离雷达，保证在使用环境中可以稳定扫描到前后左右，或至少三个方向的环境特征，对软件算法要求较低。

2）设置空旷区域的路径参数，机器人在空旷、长廊区域时，降低激光里程计在EKF算法中的权重及影响力，在此环境仅依赖轮式里程计+IMU进行局部融合里程计积分计算。

3）增加视觉传感器以及VIO视觉里程计方案，扫描地面纹理、天花板纹理等特征进行里程计积分计算，并将其融合入EKF局部融合里程计中（视觉VIO不适用低纹理、暗光环境）；

4）在空旷、低特征环境增加部署反光板，开发反光板融合slan算法，在原有激光栅格地图层基础上，对齐反光板地图层，设置高动态区域路线参数，使空旷、低特征环境中，机器人主动切换使用反光板进行全局反光板地图定位（客户需同意部署反光板）；

问题2：高动态环境，大型障碍物发生位移运动，占据激光/视觉传感器视野30%以上，使得视觉/激光局部融合里程计追踪大型障碍物运动，导致全局定位漂移问题； ● 解决方案： 1）增加视觉传感器及机器学习算法模型，联合标定视觉及激光雷达等传感器对障碍物识别的位姿，识别人类、客户现场中的高动态物体（堆垛箱子、卡板、大型移动设备等），对齐其他传感器识别到的障碍物全局地图坐标，将此类已知高动态物体的点云进行删除处理，避免其影响基于激光点云的全局地图定位匹配；

2）开发反光板融合slan算法，设置高动态区域路线参数，使高动态环境中，机器人主动切换使用反光板进行全局反光板定位；

1、极智嘉-VXSLAM架构方案（极智嘉研发总监-陈超） 视频讲解：https://www.bilibili.com/video/BV1Vt4y1Y78Z/ 文章：https://zhidx.com/p/229397.html

2、学术报告（基于多源传感器融合的机器人定位与建图-浙大-张宇教授） 视频讲解：https://www.bilibili.com/video/BV11R4y147f5

3、思岚科技-多传感器融合slam架构方案 视频讲解：https://www.bilibili.com/video/BV1WK4y1X7Yo