基于cartographer的优化和功能开发，以及二维码、反光板定位

长安城外少年与梦

已于 2024-06-09 22:15:26 修改

阅读量1.2k

点赞数 22

文章标签：算法人工智能

于 2024-03-15 11:56:57 首次发布

本文链接：https://blog.csdn.net/Legend_zZ/article/details/136671734

版权

基于cartographer的优化和功能开发，以及二维码、反光板定位

一、编辑地图功能
二、二维码SLAM（usb_cam,apriltag_ros,apriltag)
三、反光板SLAM（reflecter_ekf_slam）
四、landmark 位姿的增删改查（UpdateLandmarkService)
五、业务流程优化
六、优化代码逻辑和性能
七、修复BUG

一、编辑地图功能

后续操作均需以非冻结模式加载地图并且不开启新轨迹。

{
    "op":"call_service",
    "service": "/slam/set_cmd",
    "args":{
        "cmd":7,
        "map_name": "we"
    }
}

1.删除子图（TrimSubmapService）

流程：

传入 map_name ，返回地图和子图的pose点位；
map_name 为空，传入 submap_point_list ，预览删除后的地图效果；

{
    "op":"call_service",
    "service": "/trim_submap",
    "args":{
        "map_name": "map1",
        "submap_point_list": [
            {
                "trajectory_id": 0,
                "submap_index": 7,
                "keep": false,
                "y": 161.1341552734375,
                "x": 447.395263671875
            },
            {
                "trajectory_id": 0,
                "submap_index": 8,
                "keep": false,
                "y": 238.68544006347656,
                "x": 448.4936218261719
            }
        ]
    }
}

删除轨迹（DeleteTrajectoryService）
传入 map_name 和 trajectory_id ，即可删除轨迹：

{
   "op":"call_service",
   "service": "/delete_trajectory",
   "args":{
       "map_name": "map1",
       "trajectory_id": 0,
       "keep": false
       
   }
}

2.匹配地图（手动回环）（AddConstraintService）

{
    "op":"call_service",
    "service": "/add_constraint",
    "args":{
        "map_name": "map",
        "trajectory_id": 0,
        "submap_id": 0,
        "initial_pose":{
            "x":0,
            "y":0,
            "theta":0
        }
    }
}

流程：

if(!req.map_name.empty()),初始化submap点位;
"map_name"为空,initial_pose.x = 0 && initial_pose.y = 0 && initial_pose.theta = 0,返回去除指定子图的mapPicture,单张子图的submapPicture.,和submap左上角坐标;
"map_name"为空，!(initial_pose.x = 0 && initial_pose.y = 0 && initial_pose.theta = 0,传位姿,执行匹配.

3.保存地图

传入 map_name

{
    "op":"call_service",
    "service": "/slam/set_cmd",
    "args":{
        "cmd":0,
        "map_name": "we"
    }
}

二、二维码SLAM（usb_cam,apriltag_ros,apriltag)

二维码安置：将二维码板垂直于墙贴放，正反两面都贴二维码，可让机器的正前方摄像头识别到。（如要利用二维码的角度，正反应该贴不一样的二维码，建议不用）
精度和探测距离：选定合适的镜头和全局快门(克服动态模糊)，在足够的光照条件下，机器在5m内，1.2m/s的速度下，二维码距离精度在5-10cm左右，角度精度得在2m内较为稳定。
影响因素：
①相机内参外参精度
②光照条件（相机曝光时间越短，亮度越低，动态模糊性能越好；反之，亮度越高，动态模糊性能越差）
③镜头与二维码相对角度
④机器运动速度
⑤摄像头分辨率
⑥二维码大小
⑦采用二维码组合（组合能提高稳定性，但是有限，并不能提高检测距离)，
CPU占用：在640*480分辨率下，帧率5 FPS，usb_cam + apriltag 在 i51135 占用率为单核50%。(选用apriltag不用aruco的原因是，apriltag距离更远)
影响因素：
①分辨率
②帧率

三、反光板SLAM（reflecter_ekf_slam）

优点：cpu占用率比二维码方案低很多，单核10%以下。检测距离远近，取决于雷达分辨率。
缺点：超出设置距离后，无法提供回环。
流程图：
请添加图片描述

雷达前段滤波，计算reflector在位置（laser_reflector）：
①订阅lidar数据，过滤特定范围内和激光强度大于反光板阈值的点，将连续的高强度点云放入同一reflector集合中。考虑点云中间出现断点，合并距离很近的reflector集合。如果雷达为360°，需将点云头尾的reflector集合合并；
②判断reflector半径是否达到实际半径，考虑误差；
③计算reflector中心点，将反光板集合输出给后端。
后端根据机器实时位置，计算reflector在 map 中的位置（reflecter_ekf_slam）
①订阅tf来实时更新机器实时位姿；
②判断 reflector 是否与记录的 reflector 匹配，匹配的话使用 EKF 更新 reflector ，不匹配则录入新 reflector，并将landmark发送给 cartographer；
③订阅 cartographer 的 landmark_poses_list ,来保持 cartographer 和 reflecter_ekf_slam 后端 reflector 的一致性。

四、landmark 位姿的增删改查（UpdateLandmarkService)

{
    "op":"call_service",
    "service": "/update_landmark",
    "args":{
        "landmark_id":0,
        "x": "2.56"
        "y": "1.5"
    }
}

①传入 landmark_id ，更新 x, y；

②传入 landmark_id ，x = 0 && y = 0 ,删除 landmark；

③传入新 landmark_id ，增加新 landmark。

五、业务流程优化

扩展地图前，将地图压缩备份；
建图时监控工控机的CPU，和内存占用率；

六、优化代码逻辑和性能

优化扩展地图时CPU占用过高，取消全局约束的生成，只生成局部约束；

七、修复BUG

修复扩展建图开始时，地图的跳动问题；
修复雷达时间戳异常导致的 cartographer 崩溃。