最优轨迹生成(三)—— 无约束BIVP轨迹优化

2023-12-31 12:36

本文主要是介绍最优轨迹生成(三)—— 无约束BIVP轨迹优化,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

   本系列文章是学习深蓝学院-移动机器人运动规划课程第五章最优轨迹生成 过程中所记录的笔记,本系列文章共包含四篇文章,依次介绍了微分平坦特性、无约束BVP轨迹优化、无约束BIVP轨迹优、 带约束轨迹优化等内容

   本系列文章链接如下:

   最优轨迹生成(一)—— 微分平坦

   最优轨迹生成(二)—— 无约束BVP轨迹优化

   最优轨迹生成(三)—— 无约束BIVP轨迹优化

   最优轨迹生成(四)—— 带约束轨迹优化


   三、无约束BIVP轨迹优化

在这里插入图片描述

   如果用BVP方法来对如下所示的折线路径进行平滑时,需要对每段折线解一个BVP,且需要指定每段折线起始和终末状态,如果指定的状态中的速度过大会不可行,所以BVP的一个缺陷是需要找到合适的指定状态,那么我们能不能仅对状态中的位置进行指定,让其他状态量,比如速度、加速度等自己去进行优化呢?

   也就是,对于下面的路径,我们仅给定起始和终末状态(位置、速度、加速度、jerk等)以及中间经过状态点的位置(不对速度、加速度、jerk等其他状态量进行指定),即要求平滑后的路径要经过这些指定的位置点,但这些这些位置点处的速度、加速度、jerk等状态量通过算法优化自行得到,这样也会使得轨迹更加顺滑,这就是边界中间值问题(BIVP)。

在这里插入图片描述

在这里插入图片描述

   BIVP的解具有超出输入或所优化阶数的连续性,比如当s=3时,状态为位置、速度、加速度、输入为jerk,则优化的目标函数为 min ⁡ z ( t ) ∫ t 0 t M [ p ( 3 ) ] 2 d t , \min_{z(t)}\int_{t_0}^{t_{M}}[p^{\left(3\right)}]^2\mathrm{d}t, minz(t)t0tM[p(3)]2dt,,即最小化jerk,最优性条件表明最优解是5次多项式,BIVP的解可以进一步保证snap是连续的。当s=4时,是最小化snap,但BIVP的解可以进一步保证Pop是连续的。


   下图给出了s=3时的例子,状态p、v、a是连续的,最小化的目标量jerk也是连续的、更高阶的snap是分段的,但其在中间状态点处(黄色的小球处)也是连续的

在这里插入图片描述


   所以,我们可以直接去施加这些轨迹上的连续性条件,得到一个关于多项式系数的等式Mc=b,只需要求一个M的逆就可以得到我们所需要的多项式的系数c,不需要去做优化,也不用去求 min ⁡ z ( t ) ∫ t 0 t M v ( t ) T W v ( t ) d t , \min_{z(t)}\int_{t_0}^{t_M}v(t)^{\mathrm{T}}\mathbf{W}v(t)\mathrm{d}t, minz(t)t0tMv(t)TWv(t)dt,这样一个问题

   那么如何构建出上述的Mc=b关系式呢?

   首先我们知道最优解一定是2s-1的多项式构成的样条,我们可以把每段多项式都先写出来,当s=3时,下式中N=2s-1=5

   f ( t ) = { f 1 ( t ) = ˙ ∑ i = 0 N p 1 , i t i T 0 ≤ t ≤ T 1 f 2 ( t ) = ˙ ∑ i = 0 N p 2 , i t i T 1 ≤ t ≤ T 2 ⋮ ⋮ f M ( t ) = ˙ ∑ i = 0 N p M , i t i T M − 1 ≤ t ≤ T M f(t)=\begin{cases}f_1(t)\dot{=}\sum_{i=0}^Np_{1,i}t^i&\quad T_0\le t\le T_1\\f_2(t)\dot{=}\sum_{i=0}^Np_{2,i}t^i&\quad T_1\le t\le T_2\\\vdots&\quad\vdots\\f_M(t)\dot{=}\sum_{i=0}^Np_{M,i}t^i&\quad T_{M-1}\le t\le T_M&\end{cases} f(t)= f1(t)=˙i=0Np1,itif2(t)=˙i=0Np2,itifM(t)=˙i=0NpM,itiT0tT1T1tT2TM1tTM

   接下来将给定的信息,以约束的形式写出来,比如将给定的起始状态和终末状态,分别写成第一段和最后一段的等式约束,如下所示:

   { f j ( k ) ( T j − 1 ) = x 0 , j ( k ) f j ( k ) ( T j ) = x T , j ( k ) \left\{\begin{matrix}{f_{j}^{(k)}(T_{j-1})}&{=x_{0,j}^{(k)}}\\{f_{j}^{(k)}(T_{j})}&{=x_{T,j}^{(k)}}\\\end{matrix}\right. {fj(k)(Tj1)fj(k)(Tj)=x0,j(k)=xT,j(k)

   中间状态点的位置信息也是给定的,也可以由等式约束的形式写出,此外,由前面的介绍可知相邻两段多项式要经过相同的状态点(位置、速度、加速度、jerk、snap均连续,也就是5个等式)

   f j ( k ) ( T j ) = f j + 1 ( k ) ( T j ) f_{j}^{(k)}(T_{j})=f_{j+1}^{(k)}(T_{j}) fj(k)(Tj)=fj+1(k)(Tj)

   通过这些条件就可以得到Mc=b关系式

在这里插入图片描述


   我们还需要为每段多项式轨迹分配时间,有两种不同的时间轴给定方法,第一种方法是每段多项式轨迹都独立计时,每段多项式轨迹的起点时间记为0,末端时间记为 T i T_i Ti,如下面的第一幅坐标轴所示。另一种方法是记录距离第一段轨迹开始处的时间差,从第一段轨迹的开始处计时为0,每段多项式轨迹的末端时间记为 T i T_i Ti,如下面的第二幅坐标轴所示。

   从数值稳定性上来看,上面的第一种方法更好一些

在这里插入图片描述


   通过上面的介绍,我们可以把BIVP问题,根据最优条件,即给定状态信息,写出每一段多项式系数的方程组Mc=b,其中M矩阵是带状的稀疏矩阵,可以调用稀疏求解器,比如带状的PLU器,来把每段多项式系数构成的矩阵c在线性时间内求解出来,从而得到每段多项式的表达式。

在这里插入图片描述


   那么这些中间的位置点如何确定呢?

   我们可以使用RRT*等全局规划算法来找到一条全局路径,在这个路径上取一些关键的点,来作为中间位置点,再使用上面介绍的方法生成轨迹。关键点的提取可以采用Douglas-Peukcer等算法。

在这里插入图片描述

   道格拉斯普克算法(Douglas-Peukcer)算法是一种简化线状要素的经典算法。其基本思想是对每一条曲线的首末点虚连一条直线,求所有点与直线的距离,并找出最大距离值dmax,用dmax与限差D相比。若dmax<D,这条曲线上的中间点全部舍去;若dmax ≥D,保留dmax对应的坐标点,并以该点为界,把曲线分为两部分,对这两部分重复使用该方法。

   算法的详细步骤如下:

   (1) 在曲线首尾两点间虚连一条直线,求出其余各点到该直线的距离,如下图1。

   (2) 选其最大者与阈值相比较,若大于阈值,则离该直线距离最大的点保留,否则将直线两端点间各点全部舍去,如下图2,第4点保留。

   (3) 依据所保留的点,将已知曲线分成两部分处理,重复第1、2步操作,迭代操作,即仍选距离最大者与阈值比较,依次取舍,直到无点可舍去,最后得到满足给定精度限差的曲线点坐标,如图3、图4依次保留第6点、第7点,舍去其他点,即完成线的化简。

在这里插入图片描述

   DP算法的实例程序

void BuildTree(DPNode *&root, vector<pcl::PointXYZ> points, pcl::PointXYZ headpoint, pcl::PointXYZ endpoint, double thres_ds)
{arrayoperation ArrExample;//创建一个新的根节点root = new DPNode;root->points = points;root->HeadPoint = headpoint;root->EndPoint = endpoint;if (points.size() <= 2)//点数少于2个的,不再进行划分{root->Left_node = NULL;root->Right_node = NULL;root->NodeType = false;//不能再划分}else{vector<double> disvec;//计算每个点到首尾两点构成直线的距离for (int i = 0; i < points.size(); i++){double tempds = Point2Dline(points[i], headpoint, endpoint);disvec.push_back(tempds);}double maxds = ArrExample.getMax_vector(disvec);double maxindex = ArrExample.GetIndexOfMax(disvec);//若整个点数为10个,那么maxindex一定是介于 2到9之间,因为不可能取首尾两个点,首尾点到直线的距离为0if (maxds < thres_ds)//小于阈值的,不再分割{root->Left_node = NULL;root->Right_node = NULL;root->NodeType = false;//不能再划分}else{root->NodeType = true;//可以继续划分//将点划分成2部分,左边与右边vector<pcl::PointXYZ> Leftpointsvec, Rightpointsvec;for (int i = 0; i < points.size(); i++){if (i <= maxindex){Leftpointsvec.push_back(points[i]);//左边树包含的点}}for (int i = 0; i < points.size(); i++){if (i >= maxindex){Rightpointsvec.push_back(points[i]);//右边树包含的点}}//左边子树的头部点与尾部点pcl::PointXYZ left_headpoint = headpoint;pcl::PointXYZ left_endpoint = points[maxindex];//右边子树的头部点与尾部点pcl::PointXYZ right_headpoint = points[maxindex];pcl::PointXYZ right_endpoint = endpoint;//创建左、右树root->Right_node = new DPNode();BuildTree(root->Left_node, Leftpointsvec, left_headpoint, left_endpoint, thres_ds);BuildTree(root->Right_node, Rightpointsvec, right_headpoint, right_endpoint, thres_ds);}}
}

   但前面介绍的通过Mc=b方法解得的多项式轨迹只能保证在中间位置点处是不碰撞的,无法保证整个轨迹是不与障碍物相交的,轨迹可能会与障碍物相交,如下图所示:

在这里插入图片描述

   一种解决方法是,首先保证全局路径规划算法找到的初始路径是无碰撞的,一但生成的多项式轨迹与障碍物相交了,则可以在发生碰撞的位置附近再插入新的中间位置点,来使生成的多项式轨迹更加贴合最初的全局路径,如下图所示:

在这里插入图片描述

   通过上面介绍的RRT* +BIVP的方案,我们可以把RRT在低维空间找到的可行路径,拓展到高维的空间,而且比Kinodynamic RRT*算法更高效可靠。

在这里插入图片描述

   上述方法也存在一些缺陷,比如在障碍物比较多的时候,可能需要加入很多中间位置点,轨迹要很贴合RRT*找到的原始路径才能保证安全性,无人机的飞行可能不顺滑。



   参考资料:

   1、深蓝学院-移动机器人运动规划

   2、道格拉斯普克算法(简化线段点)


这篇关于最优轨迹生成(三)—— 无约束BIVP轨迹优化的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/556045

相关文章

MySQL索引的优化之LIKE模糊查询功能实现

《MySQL索引的优化之LIKE模糊查询功能实现》:本文主要介绍MySQL索引的优化之LIKE模糊查询功能实现,本文通过示例代码给大家介绍的非常详细,感兴趣的朋友一起看看吧... 目录一、前缀匹配优化二、后缀匹配优化三、中间匹配优化四、覆盖索引优化五、减少查询范围六、避免通配符开头七、使用外部搜索引擎八、分

IDEA自动生成注释模板的配置教程

《IDEA自动生成注释模板的配置教程》本文介绍了如何在IntelliJIDEA中配置类和方法的注释模板,包括自动生成项目名称、包名、日期和时间等内容,以及如何定制参数和返回值的注释格式,需要的朋友可以... 目录项目场景配置方法类注释模板定义类开头的注释步骤类注释效果方法注释模板定义方法开头的注释步骤方法注

Python如何自动生成环境依赖包requirements

《Python如何自动生成环境依赖包requirements》:本文主要介绍Python如何自动生成环境依赖包requirements问题,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑... 目录生成当前 python 环境 安装的所有依赖包1、命令2、常见问题只生成当前 项目 的所有依赖包1、

MySQL中动态生成SQL语句去掉所有字段的空格的操作方法

《MySQL中动态生成SQL语句去掉所有字段的空格的操作方法》在数据库管理过程中,我们常常会遇到需要对表中字段进行清洗和整理的情况,本文将详细介绍如何在MySQL中动态生成SQL语句来去掉所有字段的空... 目录在mysql中动态生成SQL语句去掉所有字段的空格准备工作原理分析动态生成SQL语句在MySQL

Python通过模块化开发优化代码的技巧分享

《Python通过模块化开发优化代码的技巧分享》模块化开发就是把代码拆成一个个“零件”,该封装封装,该拆分拆分,下面小编就来和大家简单聊聊python如何用模块化开发进行代码优化吧... 目录什么是模块化开发如何拆分代码改进版:拆分成模块让模块更强大:使用 __init__.py你一定会遇到的问题模www.

Java利用docx4j+Freemarker生成word文档

《Java利用docx4j+Freemarker生成word文档》这篇文章主要为大家详细介绍了Java如何利用docx4j+Freemarker生成word文档,文中的示例代码讲解详细,感兴趣的小伙伴... 目录技术方案maven依赖创建模板文件实现代码技术方案Java 1.8 + docx4j + Fr

SpringBoot首笔交易慢问题排查与优化方案

《SpringBoot首笔交易慢问题排查与优化方案》在我们的微服务项目中,遇到这样的问题:应用启动后,第一笔交易响应耗时高达4、5秒,而后续请求均能在毫秒级完成,这不仅触发监控告警,也极大影响了用户体... 目录问题背景排查步骤1. 日志分析2. 性能工具定位优化方案:提前预热各种资源1. Flowable

SpringBoot3实现Gzip压缩优化的技术指南

《SpringBoot3实现Gzip压缩优化的技术指南》随着Web应用的用户量和数据量增加,网络带宽和页面加载速度逐渐成为瓶颈,为了减少数据传输量,提高用户体验,我们可以使用Gzip压缩HTTP响应,... 目录1、简述2、配置2.1 添加依赖2.2 配置 Gzip 压缩3、服务端应用4、前端应用4.1 N

Java编译生成多个.class文件的原理和作用

《Java编译生成多个.class文件的原理和作用》作为一名经验丰富的开发者,在Java项目中执行编译后,可能会发现一个.java源文件有时会产生多个.class文件,从技术实现层面详细剖析这一现象... 目录一、内部类机制与.class文件生成成员内部类(常规内部类)局部内部类(方法内部类)匿名内部类二、

使用Jackson进行JSON生成与解析的新手指南

《使用Jackson进行JSON生成与解析的新手指南》这篇文章主要为大家详细介绍了如何使用Jackson进行JSON生成与解析处理,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录1. 核心依赖2. 基础用法2.1 对象转 jsON(序列化)2.2 JSON 转对象(反序列化)3.