smac 路径优化器分析——距离成本和代价地图成本分析

参考

泰勒级数直观详解

前向差分，后向差分，中心差分

smac 路径优化器分析——平滑度成本分析

smac 路径优化器分析——曲率成本分析

距离成本

距离成本函数

用优化后的点与原路径点的欧氏距离的平方作为成本。

下图中蓝色原点是原路径点，红色原点是优化后路径点。

距离成本函数为：

$Cost_{distance}=\overrightarrow{(p_i-x_i)} \cdot \overrightarrow{(p_i-x_i)}$

$p_i$ 是优化后的路径点， $x_i$ 是原路径点。

距离成本梯度函数

距离成本函数表示为：

$Cost_{distance}=\overrightarrow{(p_i-x_i)} \cdot \overrightarrow{(p_i-x_i)}= (p_{ix}-x_{ix})^2+(p_{iy}-x_{iy})^2$

偏导数可以得到：

$\frac{\partial{p_i}}{\partial x}=2*(p_{ix}-x_{ix})$

$\frac{\partial{p_i}}{\partial y}=2*(p_{iy}-x_{iy})$

代价地图成本

代价地图成本函数

Smac 直接使用路径点所在的 costmap2D 地图栅格的代价值的平方作为代价地图成本。
下图中黄色圆点表示路径点，背景是 costmap2D 地图。

代价地图成本函数为：

$Cost_{costmap}=(costmap.getCost(p_{ix}, p_{iy}))^2$

代价地图成本梯度函数

按源码提示是根据泰勒级数展开计算的，但是我颠来倒去都推导不出源码的公式｡ﾟ(TヮT)ﾟ｡

如果是使用中心差分法，那么 Δh 步进越小，计算得到的梯度才越精确。在栅格地图中最小自变量偏移是 1 个栅格。

令 $f(x)=costmap.getCost(x)$ ，那么代价地图成本为 $c(x)=f(x)^2$ ，根据中心差分法，在点 a 处对 x 求偏导有

$\begin{aligned} \frac{\partial c(x,y)}{\partial x} &= \frac{\partial f(x_a,y_a)^2}{\partial x} \\ &= 2*f(x_a,y_a)*f'(x_a,y_a) \\ &=2*f(x_a,y_a) \frac{f(x_a+1,y_a)-f(x_a-1,y_a)}{2} \\ &=f(x_a,y_a)(f(x_a+1,y_a)-f(x_a-1,y_a)) \end{aligned}$

同理，在点 a 处对 y 求偏导有

$\frac{\partial c(x,y)}{\partial y}=f(x_a,y_a)(f(x_a,y_a+1)-f(x_a,y_a-1))$

代价地图梯度函数优化对比

smac 的 smoother 路径平滑器仅打开代价地图成本和距离成本函数和成本梯度函数的优化对比。
绿色路径是随机生成的路径，红色路径是 smac 源码优化后的路径，黄色路径是使用本文代价地图成本梯度函数优化后的路径。由于路径的起点和终点并不参与优化过程，所以起点和终点的位置始终不会变，这里的路径发布我去掉了优化后路径的终点。

新旧对比代价梯度-红旧黄新

为了观察清楚代价地图成本优化效果，costmap2d 的膨胀半径增大，同时将代价缩放因子调整到合适的参数，使得代价值能够平缓地在膨胀半径边缘降到最低。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mfbz.cn/a/586302.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！