障碍物避障以及机器人足迹模型
简介：障碍物避障的实现，以及必要参数的设置对于机器人足迹模型和其对应的影响
1.障碍物避障是怎样工作的
1.1 惩罚项
障碍物避障作为整个路径优化的一部分。显然，优化是找出指定的代价函数（目标函数）的最小代价解决方案。简而言之：如果一个计划的位姿违背了与障碍物分离的期望，代价函数的代价必须增加。理想情况下，代价函数值必须是无限的在某些情况下或者优化器能够更好的拒绝这些区域。然而，这就要求优化器处理强约束（解决非线性问题）。
teb_local_planner放弃了考虑墙约束项，以便更好的考虑有效性。强约束项被转换为了软约束向，使得具有有限值的二次惩罚项的组成。
上图展示了对弈障碍物避障的惩罚项例子。允许的到障碍物的最小欧拉距离（参数min_obstacle_dist）设置为0.2m。因此低于0.2m产生一个非零的代价值。可以想象得到优化问题是由多个代价项构成。他们中的有些是相互冲突的，比如时间优化。因此，可能发生，优化器考虑一个小的违背（因此小的惩罚项）以便最小化全部的代价函数。有两个选择来调试行为：
1.调整优化权重（各自代价的变化，在这里是参数weight_obstacle）.如果选择值太高，优化问题会变得病态导致不好的收敛行为。
2.通过添加额外的边缘来转移增广。通过把一个小的额外的边缘添加到min_obstacle_dist里面，就可以隐含增加在0.2m处的代价值。可以在单个参数里面penalty_epsilon中来转移所有惩罚项，但是小心，这样做会广泛影响优化结果。
1.2 局部优化解决方案
注意，优化器本身只找具有优化解决方案。想象一下，机器人可能由两个障碍物横向包围。惩罚项实际上是非零，但是优化器会卡住在局部最小里面，因为横向移动对应位姿到任何一个障碍物会进一步增加总共代价。有点像下面的情况：
以上轨迹不能跳过障碍物。甚至位姿本身也被推离障碍物之间的区域（红色箭头）。很显然，在实际情况中，这种情况应该被避免。因此，同轮类的规划算法寻找替代的解决方案以及可行的检测来排斥这种解决方案在实际发送命令给机器人之前。
1.3位姿与障碍物之间的关系
下图描述了常见的规划方案情形：
以上规划的轨迹由多个机器人位姿组成。规划器目的在于根据期望的瞬时分辨率（参数dt_ref）来安排每两个连续的位姿。注意，实际的分辨率是不固定的，因为优化器为了寻找时间最优性需要调整过渡时间。
对于障碍物避障，在障碍物和规划的位姿之间的距离必须被固定为以下。图上例子轨迹由8个可变的位姿组成（起始点固定）。对于每一个障碍物（点，即占据栅格，线，多边形）离规划路径上的最近位姿被确定下来。根据参数obstacle_poses_affected的值，与最近的位姿的邻居位姿也会被考虑进来。只有选择的位姿子集被考虑进随后的优化步骤中（在这里有三个位姿和三个惩罚项）。本过程根据参数no_inner_iterations的值来决定重复次数。obstacle_poses_affected参数的值会轻微影响在障碍物附近轨迹的平滑性。更大的障碍物就要求更多连接的位姿来避免不可访问的捷近。可能需要选择高于轨迹长度的值来连接每一个障碍物的所有位姿。
2.机器人足迹模型
机器人足迹模型近似机器人的2d轮廓，主要用于优化目的。对于复杂的距离计算，这个模型是很关键的。因此，机器人足迹模型构成一个专用的参数而不是从公共的costmap_2d的参数中加载机器人足迹。优化足迹模型与costmap 的足迹模型不相同，costmap的足迹模型是用于可行性检测。
机器人足迹模型使用参数服务器来选择和配置。可以添加如下参数结构到teb_local_planner配置文件中：
TebLocalPlannerROS: footprint_model: # types: "point", "circular", "line", "two_circles", "polygon" type: "point" radius: 0.2 # for type "circular" line_start: [-0.3, 0.0] # for type "line" line_end: [0.3, 0.0] # for type "line" front_offset: 0.2 # for type "two_circles" front_radius: 0.2 # for type "two_circles" rear_offset: 0.2 # for type "two_circles" rear_radius: 0.2 # for type "two_circles" vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
2.1默认足迹模型是点类型。
机器人被模拟为单点类型，对于这种类型计算时间最少。
2.2 圈类型
机器人被模拟为简单的圆圈类型。距离计算类似于点类型的机器人，机器人的半径被添加到参数min_obstacle_dist的每一个函数调用中。也可以通过选择一个点类型的机器人来摆脱额外的增加，然后把半径添加到最小障碍物距离，一个先验信息。
2.3足迹类型：线
2.4两个圆的足迹类型
2.5足迹类型：多边形
通过定义一个闭合的多边形来表达一个复杂的模型。多边形是通过一个顶点表来定义的（每个顶点通过x，y坐标表示）。假设机器人的旋转轴位于(0,0)，单位m。第一个顶点和最后一个顶点不用重复。
3.可行性检查
可行性检查被执行，是在优化器返回一个轨迹之后，然后在发送速度命令到机器人之前。检查的目的在于验证优化器产生的轨迹的有效性。
当前，算法迭代的头n个位姿(feasibility_check_no_poses)从机器人当前位置开始，然后检查这些位姿是否不碰撞。
如果碰撞发生，costmap的footprint被使用。因此，验证模型可能比用于优化的footprint更复杂。
值feasibility_check_no_poses不应该选择很高，因为优化器可能不会完全收敛：比如，在将来的位姿规划中小障碍物的违背可以被纠正，当机器人向前移动到目标点的时候。
如果是在狭窄的环境中驱动时，确定配置合适的障碍物避障行为（局部规划和全局规划）。另外局部路径规划器可能排斥一个不可行的轨迹（从它的观点来看），但是全局规划器相反可能认为选择的全局规划是可行的：此时机器人卡住了。