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

目录

  • 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=argxmin21xTQx+qTx

其中 Q \boldsymbol{Q} Q n n n维对称正定阵, q ∈ R n \boldsymbol{q}\in \mathbb{R} ^n qRn,共轭梯度法既克服了梯度下降法收敛慢的缺点,又避免存储和计算牛顿类算法所需的二阶梯度信息,其核心原理是:求解矩阵 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)=xiXσo(xiomin2dmax)

惩罚机器人与障碍发生碰撞,其中 σ 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} xiPobs(xi)=2(xiomin2dmax)xixiomin2=2(xiomin2dmax)xiomin2xiomin

这里最小障碍通过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 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)=xiXσκ(Δxi2Δϕ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}_{i+1}} xiPcur(xi)=α1xi1κi+α2xiκi+α3xi+1κ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|} ab=abaTbbb

则令

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}_{i+1} \right)}{\left\| \varDelta \boldsymbol{x}_i \right\| _2\left\| \varDelta \boldsymbol{x}_{i+1} \right\| _2}, \boldsymbol{p}_2=\frac{\left( -\varDelta \boldsymbol{x}_{i+1} \right) \bot \varDelta \boldsymbol{x}_i}{\left\| \varDelta \boldsymbol{x}_i \right\| _2\left\| \varDelta \boldsymbol{x}_{i+1} \right\| _2} p1=Δxi2Δxi+12Δxi(Δxi+1),p2=Δxi2Δxi+12(Δxi+1)Δ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}_{i+1}}&=\frac{1}{\left\| \varDelta \boldsymbol{x}_i \right\| _2}\frac{-1}{\sqrt{1-\cos ^2\varDelta \phi}}\boldsymbol{p}_1\end{aligned} xiκixi1κixi+1κi=Δxi211cos2Δϕ 1(p1p2)Δxi23ΔϕiΔxi=Δxi211cos2Δϕ 1p2+Δxi23ΔϕiΔxi=Δxi211cos2Δϕ 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}_{i+1} \right\| _{2}^{2}} Psmo(X)=xiXΔxiΔxi+122

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

∂ 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}_{i+1}+\boldsymbol{x}_{i+2} \right) xiPsmo(xi)=2(xi24xi1+6xi4xi+1+xi+2)

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深度学习实战》
  • 《机器学习强基计划》
  • 《运动规划实战精讲》

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

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

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Unity3D UGUI适配不同分辨率详解

前言 在Unity3D开发中&#xff0c;UGUI&#xff08;Unitys Graphical User Interface&#xff09;是构建用户界面&#xff08;UI&#xff09;的重要工具。然而&#xff0c;随着移动设备和桌面设备的分辨率日益多样化&#xff0c;确保UI能够在不同分辨率下良好显示变得尤为重要…

CAS乐观锁原理

1、什么是CAS&#xff1f; compare and swap也就是比较和交换&#xff0c;他是一条CPU的并发原语。 他在替换内存的某个位置的值时&#xff0c;首先查看内存中的值与预期值是否一致&#xff0c;如果一致&#xff0c;执行替换操作。 这个操作是一个原子性操作。 Java中基于Un…

手机免费恢复照片的软件有哪些?这2个工具来帮忙

照片是我们情感的载体&#xff0c;是记忆的碎片。它们无声地诉说着过去的故事&#xff0c;记录着生活中的点点滴滴。但意外常常是突如其来的&#xff0c;当发现手机照片丢失时&#xff0c;我们往往心痛不已。 不用担心&#xff0c;这场看似绝望的危机&#xff0c;实则有解决之…

C++ OpenCV 实现多张图片叠加 叠加文字

C OpenCV 实现多张图片叠加 叠加文字 在C中使用OpenCV叠加多张图片以及添加文字的基本步骤如下&#xff1a; 加载多张图片。 确定叠加位置。 使用cv::addWeighted叠加图片&#xff0c;可以为叠加的图片添加透明度。 使用cv::putText在图片上添加文字。 显示或保存结果图片…

Sql Server缓冲池、连接池等基本知识(附Demo)

目录 前言1. 缓存池2. 连接池3. 彩蛋 前言 基本的知识推荐阅读&#xff1a; java框架 零基础从入门到精通的学习路线 附开源项目面经等&#xff08;超全&#xff09;Mysql优化高级篇&#xff08;全&#xff09;Mysql底层原理详细剖析常见面试题&#xff08;全&#xff09; 1…

Go 环境安装配置

1、下载 wget https://go.dev/dl/go1.21.6.linux-amd64.tar.gz 2、安装 sudo tar -xvf go1.21.6.linux-amd64.tar.gz -C $HOME/3、设置环境变量及代理 # 打开 ~/.bash_profile,输入&#xff1a; export PATH$PATH:$HOME/go/bin # 设置 Go 语言代理 export GOPROXYhttps://go…

AI发展下的伦理挑战

AI发展下的伦理挑战&#xff0c;应当如何应对&#xff1f; 人工智能飞速发展的同时&#xff0c;也逐渐暴露出侵犯数据隐私、制造“信息茧房”等种种伦理风险。随着AI技术在社会各个领域的广泛应用&#xff0c;关于AI伦理和隐私保护问题日趋凸显。尽管国外已出台系列法规来规范…

ls lsattr lsblk lscpu lsdiff lshw lsinitrd lsipc lslocks lslogins

ls: 描述&#xff1a;列出目录内容。用法&#xff1a;ls [选项] [文件或目录]示例&#xff1a; ls&#xff1a;列出当前目录的文件和子目录。ls -l&#xff1a;以详细列表形式显示目录内容。ls /path/to/directory&#xff1a;列出指定路径下的内容。 lsattr: 描述&#xff1a;…

【VSCode】安装 【ESP-IDF】插件及【ESP32-S3】新建工程和工程配置

一、搭建基础工程 二、基础工程的文件架构解析 三、调试相关工具介绍 1、串口下载2、JTAG 下载与调试 四、工程的文件架构解析 五、基础工程配置 一、搭建基础工程 在 VS Code 中新建 ESP-IDF 基础工程的步骤如下&#xff1a; 1、启动 VS Code 并打开命令面板 按下“Ctrl…

逆向案例二十八——某高考志愿网异步请求头参数加密,以及webpack

网址&#xff1a;aHR0cDovL3d3dy54aW5nYW9rYW90Yi5jb20vY29sbGVnZXMvc2VhcmNo 抓包分析&#xff0c;发现请求头有参数u-sign是加密的&#xff0c;载荷没有进行加密&#xff0c;直接跟栈分析。 进入第二个栈&#xff0c;打上断点&#xff0c;分析有没有加密位置。 可以看到参数…

Chapter17 表面着色器——Shader入门精要学习

Chapter17 表面着色器 一、编译指令1.表面函数2.光照函数3.其他可选参数 二、两个结构体1.Input 结构体&#xff1a;数据来源2.SurfaceOutput 三、Unity背后做了什么四、表面着色器的缺点 一、编译指令 作用&#xff1a;指明该表面着色器的 表面函数 和 光照函数&#xff0c;并…

【React】深入探索React:表单控制、组件通信、副作用管理、自定义Hook

一、React表单控制 表单控制是React应用中常见的需求&#xff0c;React Hooks提供了两种方式来实现&#xff1a;受控组件和非受控组件。 1.1 受控组件 受控组件是将表单输入的值保存在组件的状态中。这样&#xff0c;表单数据就完全由React控制。 import React, { useState…

【系统架构设计 每日一问】三 Redis支持事务么,Redis的事务如何保证

实际上&#xff0c;关于Redis事务的说法“Redis 的事务只能保证隔离性和一致性&#xff08;I 和 C&#xff09;&#xff0c;无法保证原子性和持久性&#xff08;A 和 D&#xff09;”并不完全准确。下面我将分别解释Redis事务的四个特性&#xff1a;原子性&#xff08;Atomicit…

探索Mojo模型的超参数优化:自定义搜索策略全解析

探索Mojo模型的超参数优化&#xff1a;自定义搜索策略全解析 在机器学习领域&#xff0c;超参数的调整是提高模型性能的关键步骤。Mojo模型&#xff0c;作为一种高效的模型部署方式&#xff0c;其超参数的搜索同样至关重要。本文将深入探讨如何在Mojo模型中实现自定义的超参数…

5.波士顿房价预测(KNN,决策树,线性回归)

波士顿房价预测 1. 机器学习中的任务分类2. 波士顿房价预测2.1 分析数据2.2 比较 MAE 和 MSE2.2 代码 1. 机器学习中的任务分类 有监督学习&#xff08;supervised&#xff09;&#xff1a;有特征也有标签 分类问题 classification预测离散量 回归问题 regression预测连续量 …

DPDK收包流程和Linux内核收包流程对比

DPDK 网卡收包流程-腾讯云开发者社区-腾讯云NIC 在接收到数据包之后&#xff0c;首先需要将数据同步到内核中&#xff0c;这中间的桥梁是 rx ring buffer。它是由 NIC 和驱动程序共享的一片区域&#xff0c;事实上&#xff0c;rx ring buffer 存储的并不是实际的 packet 数据&a…

【Gin】精准应用:Gin框架中工厂模式的现代软件开发策略与实施技巧(上)

【Gin】精准应用&#xff1a;Gin框架中工厂模式的现代软件开发策略与实施技巧(上) 大家好 我是寸铁&#x1f44a; 【Gin】精准应用&#xff1a;Gin框架中工厂模式的现代软件开发策略与实施技巧(上)✨ 喜欢的小伙伴可以点点关注 &#x1f49d; 前言 本次文章分为上下两部分&…

Golang使用docker sdk管理docker

包括列出容器、创建容器、删除容器、进入容器、构建镜像等操作。 package dockertoolimport ("context""fmt""github.com/docker/docker/api/types""github.com/docker/docker/api/types/container""github.com/docker/docker/ap…

【RaspberryPi】树莓派系统UI优化

接上文&#xff0c;如何去定制一个树莓派的桌面系统&#xff0c;还是以CM4为例。 解除CM4上电USB无法使用问题 将烧录好的tf卡通过读卡器插入到电脑上&#xff0c;进入boot磁盘&#xff0c;里面有一个Config文件&#xff0c;双击用记事本打开&#xff0c;在【pi4】一栏里加入一…

从零开始手写STL库:List

从零开始手写STL库–List部分 Github链接&#xff1a;miniSTL 文章目录 从零开始手写STL库–List部分List是什么&#xff1f;List需要包含什么函数1&#xff09;基础成员函数2&#xff09;核心功能3)其他功能 基础成员函数的编写核心功能的编写其他功能编写总结 List是什么&am…