做推广可以上那些网站,装修公司加盟费多少,互联网代理商联盟平台,网络营销的主要方式k目录 0 专栏介绍1 数值优化#xff1a;共轭梯度法2 基于共轭梯度法的轨迹优化2.1 障碍约束函数2.2 曲率约束函数2.3 平滑约束函数 3 算法仿真3.1 ROS C实现3.2 Python实现 0 专栏介绍
#x1f525;课程设计、毕业设计、创新竞赛、学术研究必备#xff01;本专栏涉及更高阶的… 目录 0 专栏介绍1 数值优化共轭梯度法2 基于共轭梯度法的轨迹优化2.1 障碍约束函数2.2 曲率约束函数2.3 平滑约束函数 3 算法仿真3.1 ROS C实现3.2 Python实现 0 专栏介绍
课程设计、毕业设计、创新竞赛、学术研究必备本专栏涉及更高阶的运动规划算法实战曲线生成与轨迹优化、碰撞模型与检测、多智能体群控、深度强化学习运动规划、社会性导航、全覆盖路径规划等内容每个模型都包含代码实现加深理解。
详情运动规划实战进阶 1 数值优化共轭梯度法
共轭梯度法是一种用于解决大型稀疏线性方程组或无约束优化问题的迭代数值方法。它利用了线性代数中的共轭概念并结合了梯度下降法的思想以更有效地找到函数的极小值点。 形式化地对于 n n n维二次优化问题 x ∗ a r g min x 1 2 x T Q x q T x \boldsymbol{x}^*\mathrm{arg}\min _{\boldsymbol{x}}\frac{1}{2}\boldsymbol{x}^T\boldsymbol{Qx}\boldsymbol{q}^T\boldsymbol{x} x∗argxmin21xTQxqTx
其中 Q \boldsymbol{Q} Q是 n n n维对称正定阵 q ∈ R n \boldsymbol{q}\in \mathbb{R} ^n q∈Rn共轭梯度法既克服了梯度下降法收敛慢的缺点又避免存储和计算牛顿类算法所需的二阶梯度信息其核心原理是求解矩阵 Q \boldsymbol{Q} Q的共轭向量组 d 0 , d 1 , ⋯ , d n \boldsymbol{d}_0,\boldsymbol{d}_1,\cdots ,\boldsymbol{d}_n d0,d1,⋯,dn作为 n n n个优化方向由于优化方向间彼此正交故每次迭代只需沿着一个方向 d i \boldsymbol{d}_i di寻优而互不影响。所以理论上最多 n n 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 ( X ) w o P o b s ( X ) w κ P c u r ( X ) w s P s m o ( X ) f\left( \mathcal{X} \right) w_oP_{\mathrm{obs}}\left( \mathcal{X} \right) w_{\kappa}P_{\mathrm{cur}}\left( \mathcal{X} \right) w_sP_{\mathrm{smo}}\left( \mathcal{X} \right) f(X)woPobs(X)wκPcur(X)wsPsmo(X)
2.1 障碍约束函数 P o b s ( X ) ∑ x i ∈ X σ o ( ∥ x i − o min ∥ 2 − d max ) P_{\mathrm{obs}}\left( \mathcal{X} \right) \sum_{\boldsymbol{x}_i\in \mathcal{X}}^{}{\sigma _o\left( \left\| \boldsymbol{x}_i-\boldsymbol{o}_{\min} \right\| _2-d_{\max} \right)} Pobs(X)xi∈X∑σo(∥xi−omin∥2−dmax)
惩罚机器人与障碍发生碰撞其中 σ o ( ⋅ ) \sigma _o\left( \cdot \right) σo(⋅)是惩罚函数(可选为二次型) o min \boldsymbol{o}_{\min} omin是距离 x i \boldsymbol{x}_i xi最近的障碍物 d max d_{\max} dmax是距离阈值节点与最近障碍物的距离超过阈值则不会受到惩罚。以二次型为例其梯度为 ∂ P o b s ( x i ) ∂ x i 2 ( ∥ x i − o min ∥ 2 − d max ) ∂ ∥ x i − o min ∥ 2 ∂ x i 2 ( ∥ x i − o min ∥ 2 − d max ) x i − o min ∥ x i − o min ∥ 2 \frac{\partial P_{\mathrm{obs}}\left( \boldsymbol{x}_i \right)}{\partial \boldsymbol{x}_i}2\left( \left\| \boldsymbol{x}_i-\boldsymbol{o}_{\min} \right\| _2-d_{\max} \right) \frac{\partial \left\| \boldsymbol{x}_i-\boldsymbol{o}_{\min} \right\| _2}{\partial \boldsymbol{x}_i}\\2\left( \left\| \boldsymbol{x}_i-\boldsymbol{o}_{\min} \right\| _2-d_{\max} \right) \frac{\boldsymbol{x}_i-\boldsymbol{o}_{\min}}{\left\| \boldsymbol{x}_i-\boldsymbol{o}_{\min} \right\| _2} ∂xi∂Pobs(xi)2(∥xi−omin∥2−dmax)∂xi∂∥xi−omin∥22(∥xi−omin∥2−dmax)∥xi−omin∥2xi−omin
这里最小障碍通过ESDF获取可以参考相关文章
ROS2从入门到精通5-1详解代价地图与costmap插件编写(以距离场ESDF为例)轨迹优化 | 图解欧氏距离场与梯度场算法(附ROS C/Python实现) [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-toB9MWvL-1721696975312)(https://i-blog.csdnimg.cn/direct/2da70f16131b48e2a2dfb8e1cbf7a89b.png?x-oss-processimage/watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBATXIuV2ludGVyYA,size_50,color_FFFFFF,t_30,g_se,x_16#pic_center 650x)]
2.2 曲率约束函数 P c u r ( X ) ∑ x i ∈ X σ κ ( Δ ϕ i ∥ Δ x i ∥ 2 − κ max ) P_{\mathrm{cur}}\left( \mathcal{X} \right) \sum_{\boldsymbol{x}_i\in \mathcal{X}}^{}{\sigma _{\kappa}\left( \frac{\varDelta \phi _i}{\left\| \varDelta \boldsymbol{x}_i \right\| _2}-\kappa _{\max} \right)} Pcur(X)xi∈X∑σκ(∥Δxi∥2Δϕi−κmax)
对每个节点轨迹的瞬时曲率进行了上界约束其中 σ κ ( ⋅ ) \sigma _{\kappa}\left( \cdot \right) σκ(⋅)是惩罚函数(可选为二次型) 是路径最大允许曲率——由机器人转向半径约束决定
其梯度为 ∂ P c u r ( x i ) ∂ x i α 1 ∂ κ i ∂ x i − 1 α 2 ∂ κ i ∂ x i α 3 ∂ κ i ∂ x i 1 \frac{\partial P_{\mathrm{cur}}\left( \boldsymbol{x}_i \right)}{\partial \boldsymbol{x}_i}\alpha _1\frac{\partial \kappa _i}{\partial \boldsymbol{x}_{i-1}}\alpha _2\frac{\partial \kappa _i}{\partial \boldsymbol{x}_i}\alpha _3\frac{\partial \kappa _i}{\partial \boldsymbol{x}_{i1}} ∂xi∂Pcur(xi)α1∂xi−1∂κiα2∂xi∂κiα3∂xi1∂κi
为了求解该梯度定义向量 a \boldsymbol{a} a在向量 b \boldsymbol{b} b的垂直分量为 a ⊥ b a − a T b ∣ b ∣ ⋅ b ∣ b ∣ \boldsymbol{a}\bot \boldsymbol{b}\boldsymbol{a}-\frac{\boldsymbol{a}^T\boldsymbol{b}}{\left| \boldsymbol{b} \right|}\cdot \frac{\boldsymbol{b}}{\left| \boldsymbol{b} \right|} a⊥ba−∣b∣aTb⋅∣b∣b
则令 p 1 Δ x i ⊥ ( − Δ x i 1 ) ∥ Δ x i ∥ 2 ∥ Δ x i 1 ∥ 2 , p 2 ( − Δ x i 1 ) ⊥ Δ x i ∥ Δ x i ∥ 2 ∥ Δ x i 1 ∥ 2 \boldsymbol{p}_1\frac{\varDelta \boldsymbol{x}_i\bot \left( -\varDelta \boldsymbol{x}_{i1} \right)}{\left\| \varDelta \boldsymbol{x}_i \right\| _2\left\| \varDelta \boldsymbol{x}_{i1} \right\| _2}, \boldsymbol{p}_2\frac{\left( -\varDelta \boldsymbol{x}_{i1} \right) \bot \varDelta \boldsymbol{x}_i}{\left\| \varDelta \boldsymbol{x}_i \right\| _2\left\| \varDelta \boldsymbol{x}_{i1} \right\| _2} p1∥Δxi∥2∥Δxi1∥2Δxi⊥(−Δxi1),p2∥Δxi∥2∥Δxi1∥2(−Δxi1)⊥Δxi
从而 ∂ κ i ∂ x i 1 ∥ Δ x i ∥ 2 − 1 1 − cos 2 Δ ϕ ( − p 1 − p 2 ) − Δ ϕ i Δ x i ∥ Δ x i ∥ 2 3 ∂ κ i ∂ x i − 1 1 ∥ Δ x i ∥ 2 − 1 1 − cos 2 Δ ϕ p 2 Δ ϕ i Δ x i ∥ Δ x i ∥ 2 3 ∂ κ i ∂ x i 1 1 ∥ Δ x i ∥ 2 − 1 1 − cos 2 Δ ϕ p 1 \begin{aligned} \frac{\partial \kappa _i}{\partial \boldsymbol{x}_i}\frac{1}{\left\| \varDelta \boldsymbol{x}_i \right\| _2}\frac{-1}{\sqrt{1-\cos ^2\varDelta \phi}}\left( -\boldsymbol{p}_1-\boldsymbol{p}_2 \right) -\frac{\varDelta \phi _i\varDelta \boldsymbol{x}_i}{\left\| \varDelta \boldsymbol{x}_i \right\| _{2}^{3}}\\\frac{\partial \kappa _i}{\partial \boldsymbol{x}_{i-1}}\frac{1}{\left\| \varDelta \boldsymbol{x}_i \right\| _2}\frac{-1}{\sqrt{1-\cos ^2\varDelta \phi}}\boldsymbol{p}_2\frac{\varDelta \phi _i\varDelta \boldsymbol{x}_i}{\left\| \varDelta \boldsymbol{x}_i \right\| _{2}^{3}}\\\frac{\partial \kappa _i}{\partial \boldsymbol{x}_{i1}}\frac{1}{\left\| \varDelta \boldsymbol{x}_i \right\| _2}\frac{-1}{\sqrt{1-\cos ^2\varDelta \phi}}\boldsymbol{p}_1\end{aligned} ∂xi∂κi∂xi−1∂κi∂xi1∂κi∥Δxi∥211−cos2Δϕ −1(−p1−p2)−∥Δxi∥23ΔϕiΔxi∥Δxi∥211−cos2Δϕ −1p2∥Δxi∥23ΔϕiΔxi∥Δxi∥211−cos2Δϕ −1p1
2.3 平滑约束函数 P s m o ( X ) ∑ x i ∈ X ∥ Δ x i − Δ x i 1 ∥ 2 2 P_{\mathrm{smo}}\left( \mathcal{X} \right) \sum_{\boldsymbol{x}_i\in \mathcal{X}}^{}{\left\| \varDelta \boldsymbol{x}_i-\varDelta \boldsymbol{x}_{i1} \right\| _{2}^{2}} Psmo(X)xi∈X∑∥Δxi−Δxi1∥22
期望每段轨迹的长度近似相等使整体运动更平坦其梯度为 ∂ P s m o ( x i ) ∂ x i 2 ( x i − 2 − 4 x i − 1 6 x i − 4 x i 1 x i 2 ) \frac{\partial P_{\mathrm{smo}}\left( \boldsymbol{x}_i \right)}{\partial \boldsymbol{x}_i}2\left( \boldsymbol{x}_{i-2}-4\boldsymbol{x}_{i-1}6\boldsymbol{x}_i-4\boldsymbol{x}_{i1}\boldsymbol{x}_{i2} \right) ∂xi∂Psmo(xi)2(xi−2−4xi−16xi−4xi1xi2)
3 算法仿真
3.1 ROS C实现
核心算法如下所示
bool CGOptimizer::optimize(Points2d path_o)
{// distance map updateboost::shared_ptrcostmap_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_castcostmap_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深度学习实战》《机器学习强基计划》《运动规划实战精讲》… 源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系
