非结构化场景多智能体网联协同感知与动态决策
本篇介绍一个多智能体协同感知,协同动态调度决策的系统。系统包括基于3D点云的SLAM建图(lego-loam),3维定位(hdl_localization),3D障碍物检测(传统聚类算法),部署了基于DWA的局部路径规划算法,改进的基于pid的路径跟随算法,以及基于改进时间窗方案的上层多智能体调度规划算法。
平台
-
室内实验用miniAGV,搭载16线lidar,imu,轮式里程计等
-
基于松灵 hunter2.0 底盘,自行组装的室外移动机器人,搭载16线lidar,imu,gps,intel-d435i双目深度相机,轮式里程计等传感器,使用的是阿普奇的工控机。
整体实现框架
SLAM建图
-
基于开源的lego-loam框架实现试验场地的三维建图
*可能是作者的雷达安装与imu安装坐标系没有对齐,在imuHander处理这一块将三个方向的数据进行了交换,因此最终建出的图是垂直的,所以我们进行了修改。
*在建图的过程中发现建图总会乱掉,经过多次尝试发现是接近地面信息太乱导致的,于是通过直通滤波器,手动滤除了地面信息。
*降采样并去顶过后的可视化截图如下:
-
后来做室外任务,经过调研尝试了imu与激光雷达紧耦合的开源框架lio-sam,建图效果很好。
hdl 定位
-我们采用开源的hdl-localization定位框架实现定位,结合了imu和激光雷达优化了转弯时定位漂移的情况。
障碍物检测
- 根据构图结果生成场景的占据栅格地图;
- 根据占据栅格地图分割场景与障碍物;
- 基于pcl库中的地面分割算法分割地面;
- 基于欧式聚类实现障碍物聚类,并将每个聚类生成对应的三维框;
- 根据实时定位全局地图的坐标与agv自身的局部坐标的tf转换矩阵,实现实时扫描点云障碍物的坐标转换;
- 障碍物检测与实时定位和路径跟随及中控联调。
局部路径规划与路径跟随
- 简单使用DWA实现局部路径规划;
- PID实现路径跟随,并结合时间窗实时调整速度;
- 使用速度选择器和安全控制节点保证安全;
- 补充:后尝试过CMU团队提供的自主导航开发环境,其路径跟随模块同样采用的PID,其局部路径规划模块用采样的离散点做前向模拟三次样条生成离线的路径点,实际运行时结合周围的障碍物信息滤除遮挡的路径,并选出最优的路径。效果也很好。
上层调度算法
- 核心规划算法采用的是时间窗与迪杰斯特拉最短路径算法结合的算法,同时结合多智能体实时扫描的障碍物信息实现超视距感知,动态决策规划。
Demo
补充
- 基于自己组装的机器人,做了一些自主探索的任务(基于CMU的开源框架),以及多机分布式协同绘图系统(Disco-slam,具体实现框架与优化方案这里不便展示),在此附上demo链接