weixin_43827285的博客

ROS1和ROS2导航框架中用到的各种底层算法基本相同，比如代价地图，全局路径规划和局部路径规划等，它们最大的不同在于整个系统框架设计。

该图优化以g2o优化框架实现，以机器人在各个离散时刻的位姿和离散时刻之间的时间间隔为顶点，约束其中的加速度、速度，到达时间和到障碍物的距离等值，优化目标是使得机器人在其运动学约束下绕开障碍物最快到达目标点，实现了高效的局部路径规划功能。对于每一个动态障碍物，所有TEB位姿都会被考虑进去，与静态障碍物的处理不同之处在于，会根据障碍物的速度和时间对未来其位置进行预测，以达到动态避障的目的。图优化涉及到多种约束边时，各自的权重很重要，代码中对各个权重做了注释，也能很好地看出各个约束边的作用。

DWA/DWB采样时，采样速度会倾向于目标速度，这使得速度只会单调变化（如单调减少，或者单调增加），对于线速度而言没什么明显不合适，但对于旋转而言，则造成每条采样轨迹只能朝一个方向延伸，如下图所示（黑色表示不合理采样轨迹，绿色无箭头附着的表示合理轨迹，绿色有箭头附着的为想跟随的参考轨迹），不适用于Z字形连续弯道和需要频繁转向的动态避障，这也是一个值得优化的点。有点则在于算法简单高效，低动态场景下适用。评分函数：使用每一个预设加载的评分函数对轨迹进行评分，然后进行加权求和，评分越低，代表该轨迹越优。