轨迹优化 | 基于ESDF的共轭梯度优化算法(附ROS C++/Python仿真)

0 专栏介绍

🔥课程设计、毕业设计、创新竞赛、学术研究必备！本专栏涉及更高阶的运动规划算法实战：曲线生成与轨迹优化、碰撞模型与检测、多智能体群控、深度强化学习运动规划、社会性导航、全覆盖路径规划等内容，每个模型都包含代码实现加深理解。

🚀详情：运动规划实战进阶

---

1 数值优化：共轭梯度法

共轭梯度法是一种用于解决大型稀疏线性方程组或无约束优化问题的迭代数值方法。它利用了线性代数中的共轭概念，并结合了梯度下降法的思想，以更有效地找到函数的极小值点。

形式化地，对于$n$维二次优化问题

$${\mathbf{x}}^{\ast }=\arg \underset{\mathbf{x}}{\min }\frac{1}{2}{\mathbf{x}}^T\mathbf{Qx}+{\mathbf{q}}^T\mathbf{x}$$

其中$\mathbf{Q}$是$n$维对称正定阵，$\mathbf{q}\in {\mathbb{R}}^n$，共轭梯度法既克服了梯度下降法收敛慢的缺点，又避免存储和计算牛顿类算法所需的二阶梯度信息，其核心原理是：求解矩阵$\mathbf{Q}$的共轭向量组${\mathbf{d}}_0,{\mathbf{d}}_1,\cdots ,{\mathbf{d}}_n$作为$n$个优化方向，由于优化方向间彼此正交，故每次迭代只需沿着一个方向${\mathbf{d}}_i$寻优而互不影响。所以理论上最多$n$次迭代就能找到最优解，收敛速度快，但实际应用中需要视具体情况确定阈值。

2 基于共轭梯度法的轨迹优化

对路径序列 X = { x i = ( x i , y i ) ∣ i ∈ [ 1 , N ] } \mathcal{X} =\left\{ \boldsymbol{x}_i=\left( x_i,y_i \right) |i\in \left[ 1,N \right] \right\} X={xi​=(xi​,yi​)∣i∈[1,N]}设计优化目标函数

$$f\left(\mathcal{X}\right)=w_o{P}_{\mathrm{obs}}\left(\mathcal{X}\right)+w_{\kappa }{P}_{\mathrm{cur}}\left(\mathcal{X}\right)+w_s{P}_{\mathrm{smo}}\left(\mathcal{X}\right)$$

2.1 障碍约束函数

$$P_{\mathrm{obs}}\left(\mathcal{X}\right)=\sum _{{\mathbf{x}}_i\in \mathcal{X}}{\sigma }_o\left({\Vert {\mathbf{x}}_i-{\mathbf{o}}_{\min }\Vert }_2-d_{\max }\right)$$

惩罚机器人与障碍发生碰撞，其中${\sigma }_o\left(\cdot \right)$是惩罚函数(可选为二次型)；${\mathbf{o}}_{\min }$是距离${\mathbf{x}}_i$最近的障碍物；$d_{\max }$是距离阈值，节点与最近障碍物的距离超过阈值则不会受到惩罚。以二次型为例，其梯度为

$$\begin{gathered} \frac{\partial P_{\mathrm{obs}}\left({\mathbf{x}}_i\right)}{\partial {\mathbf{x}}_i}=2\left({\Vert {\mathbf{x}}_i-{\mathbf{o}}_{\min }\Vert }_2-d_{\max }\right)\frac{\partial {\Vert {\mathbf{x}}_i-{\mathbf{o}}_{\min }\Vert }_2}{\partial {\mathbf{x}}_i} \\ =2\left({\Vert {\mathbf{x}}_i-{\mathbf{o}}_{\min }\Vert }_2-d_{\max }\right)\frac{{\mathbf{x}}_i-{\mathbf{o}}_{\min }}{{\Vert {\mathbf{x}}_i-{\mathbf{o}}_{\min }\Vert }_2} \end{gathered}$$

这里最小障碍通过ESDF获取，可以参考相关文章：

• ROS2从入门到精通5-1：详解代价地图与costmap插件编写(以距离场ESDF为例)
• 轨迹优化 | 图解欧氏距离场与梯度场算法(附ROS C++/Python实现)

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-toB9MWvL-1721696975312)(https://i-blog.csdnimg.cn/direct/2da70f16131b48e2a2dfb8e1cbf7a89b.png?x-oss-process=image/watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBATXIuV2ludGVyYA==,size_50,color_FFFFFF,t_30,g_se,x_16#pic_center =650x)]

2.2 曲率约束函数

$$P_{\mathrm{cur}}\left(\mathcal{X}\right)=\sum _{{\mathbf{x}}_i\in \mathcal{X}}{\sigma }_{\kappa }\left(\frac{\Delta {\varphi }_i}{{\Vert \Delta {\mathbf{x}}_i\Vert }_2}-{\kappa }_{\max }\right)$$

对每个节点轨迹的瞬时曲率进行了上界约束，其中${\sigma }_{\kappa }\left(\cdot \right)$是惩罚函数(可选为二次型)； 是路径最大允许曲率——由机器人转向半径约束决定

其梯度为

$$\frac{\partial P_{\mathrm{cur}}\left({\mathbf{x}}_i\right)}{\partial {\mathbf{x}}_i}={\alpha }_1\frac{\partial {\kappa }_i}{\partial {\mathbf{x}}_{i-1}}+{\alpha }_2\frac{\partial {\kappa }_i}{\partial {\mathbf{x}}_i}+{\alpha }_3\frac{\partial {\kappa }_i}{\partial {\mathbf{x}}_{i+1}}$$

为了求解该梯度，定义向量$\mathbf{a}$在向量$\mathbf{b}$的垂直分量为

$$\mathbf{a}\perp \mathbf{b}=\mathbf{a}-\frac{{\mathbf{a}}^T\mathbf{b}}{\left|\mathbf{b}\right|}\cdot \frac{\mathbf{b}}{\left|\mathbf{b}\right|}$$

则令

$${\mathbf{p}}_1=\frac{\Delta {\mathbf{x}}_i\perp \left(-\Delta {\mathbf{x}}_{i+1}\right)}{{\Vert \Delta {\mathbf{x}}_i\Vert }_2{\Vert \Delta {\mathbf{x}}_{i+1}\Vert }_2},{\mathbf{p}}_2=\frac{\left(-\Delta {\mathbf{x}}_{i+1}\right)\perp \Delta {\mathbf{x}}_i}{{\Vert \Delta {\mathbf{x}}_i\Vert }_2{\Vert \Delta {\mathbf{x}}_{i+1}\Vert }_2}$$

从而

$$\begin{array}{rl}\frac{\partial {\kappa }_i}{\partial {\mathbf{x}}_i}& =\frac{1}{{\Vert \Delta {\mathbf{x}}_i\Vert }_2}\frac{-1}{\sqrt{1-{\cos }^2\Delta \varphi }}\left(-{\mathbf{p}}_1-{\mathbf{p}}_2\right)-\frac{\Delta {\varphi }_i\Delta {\mathbf{x}}_i}{{\Vert \Delta {\mathbf{x}}_i\Vert }_2^3}\\ \frac{\partial {\kappa }_i}{\partial {\mathbf{x}}_{i-1}}& =\frac{1}{{\Vert \Delta {\mathbf{x}}_i\Vert }_2}\frac{-1}{\sqrt{1-{\cos }^2\Delta \varphi }}{\mathbf{p}}_2+\frac{\Delta {\varphi }_i\Delta {\mathbf{x}}_i}{{\Vert \Delta {\mathbf{x}}_i\Vert }_2^3}\\ \frac{\partial {\kappa }_i}{\partial {\mathbf{x}}_{i+1}}& =\frac{1}{{\Vert \Delta {\mathbf{x}}_i\Vert }_2}\frac{-1}{\sqrt{1-{\cos }^2\Delta \varphi }}{\mathbf{p}}_1\end{array}$$

2.3 平滑约束函数

$$P_{\mathrm{smo}}\left(\mathcal{X}\right)=\sum _{{\mathbf{x}}_i\in \mathcal{X}}{\Vert \Delta {\mathbf{x}}_i-\Delta {\mathbf{x}}_{i+1}\Vert }_2^2$$

期望每段轨迹的长度近似相等，使整体运动更平坦，其梯度为

$$\frac{\partial P_{\mathrm{smo}}\left({\mathbf{x}}_i\right)}{\partial {\mathbf{x}}_i}=2\left({\mathbf{x}}_{i-2}-4{\mathbf{x}}_{i-1}+6{\mathbf{x}}_i-4{\mathbf{x}}_{i+1}+{\mathbf{x}}_{i+2}\right)$$

3 算法仿真

3.1 ROS C++实现

核心算法如下所示：

bool CGOptimizer::optimize(Points2d& path_o)
{// distance map updateboost::shared_ptr<costmap_2d::DistanceLayer> distance_layer;bool is_distance_layer_exist = false;for (auto layer = costmap_ros_->getLayeredCostmap()->getPlugins()->begin();layer != costmap_ros_->getLayeredCostmap()->getPlugins()->end(); ++layer){distance_layer = boost::dynamic_pointer_cast<costmap_2d::DistanceLayer>(*layer);if (distance_layer){is_distance_layer_exist = true;break;}}if (!is_distance_layer_exist)ROS_ERROR("Failed to get a Distance layer for potentional application.");int iter = 0;while (iter < max_iter_){// choose the first three nodes of the pathfor (int i = 2; i < path_o.size() - 2; ++i){Eigen::Vector2d xi_c2(path_o[i - 2].first, path_o[i - 2].second);Eigen::Vector2d xi_c1(path_o[i - 1].first, path_o[i - 1].second);Eigen::Vector2d xi(path_o[i].first, path_o[i].second);Eigen::Vector2d xi_p1(path_o[i + 1].first, path_o[i + 1].second);Eigen::Vector2d xi_p2(path_o[i + 2].first, path_o[i + 2].second);Eigen::Vector2d correction = Eigen::Vector2d::Zero();correction = correction + _calObstacleTerm(xi, distance_layer);if (!_insideMap((xi - correction)[0], (xi - correction)[1]))continue;correction = correction + _calSmoothTerm(xi_c2, xi_c1, xi, xi_p1, xi_p2);if (!_insideMap((xi - correction)[0], (xi - correction)[1]))continue;correction = correction + _calCurvatureTerm(xi_c1, xi, xi_p1);if (!_insideMap((xi - correction)[0], (xi - correction)[1]))continue;Eigen::Vector2d gradient = alpha_ * correction / (w_obstacle_ + w_smooth_ + w_curvature_);if (std::isnan(gradient[0]) || std::isnan(gradient[1]))gradient = Eigen::Vector2d::Zero();xi = xi - gradient;path_o[i] = std::make_pair(xi[0], xi[1]);}iter++;}return true;
}

3.2 Python实现

核心算法如下所示：

while i < self.max_iter:for j in range(2, pts_num - 2):xjm2 = np.array([[optimized_path[j - 2][0]], [optimized_path[j - 2][1]]])xjm1 = np.array([[optimized_path[j - 1][0]], [optimized_path[j - 1][1]]])xj   = np.array([[optimized_path[j][0]], [optimized_path[j][1]]])xjp1 = np.array([[optimized_path[j + 1][0]], [optimized_path[j + 1][1]]])xjp2 = np.array([[optimized_path[j + 2][0]], [optimized_path[j + 2][1]]])gradient = np.zeros((2, 1))# obstacle avoidancegradient = gradient + self.obstacleTerm(xj)if not self.isOnGrid(xj - gradient):continue# smoothgradient = gradient + self.smoothTerm(xjm2, xjm1, xj, xjp1, xjp2)if not self.isOnGrid(xj - gradient):continue# curvaturegradient = gradient + self.curvatureTerm(xjm1, xj, xjp1)if not self.isOnGrid(xj - gradient):continuexj = xj - self.alpha * gradient / self.w_totaloptimized_path[j, :] = xj[:, 0]i += 1self.trajectory = optimized_path

完整工程代码请联系下方博主名片获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇