最优轨迹生成(三)—— 无约束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

相关文章

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.

Spring Boot + MyBatis Plus 高效开发实战从入门到进阶优化(推荐)

《SpringBoot+MyBatisPlus高效开发实战从入门到进阶优化(推荐)》本文将详细介绍SpringBoot+MyBatisPlus的完整开发流程,并深入剖析分页查询、批量操作、动... 目录Spring Boot + MyBATis Plus 高效开发实战:从入门到进阶优化1. MyBatis

MyBatis 动态 SQL 优化之标签的实战与技巧(常见用法)

《MyBatis动态SQL优化之标签的实战与技巧(常见用法)》本文通过详细的示例和实际应用场景,介绍了如何有效利用这些标签来优化MyBatis配置,提升开发效率,确保SQL的高效执行和安全性,感... 目录动态SQL详解一、动态SQL的核心概念1.1 什么是动态SQL?1.2 动态SQL的优点1.3 动态S

Python如何使用__slots__实现节省内存和性能优化

《Python如何使用__slots__实现节省内存和性能优化》你有想过,一个小小的__slots__能让你的Python类内存消耗直接减半吗,没错,今天咱们要聊的就是这个让人眼前一亮的技巧,感兴趣的... 目录背景:内存吃得满满的类__slots__:你的内存管理小助手举个大概的例子:看看效果如何?1.

一文详解SpringBoot响应压缩功能的配置与优化

《一文详解SpringBoot响应压缩功能的配置与优化》SpringBoot的响应压缩功能基于智能协商机制,需同时满足很多条件,本文主要为大家详细介绍了SpringBoot响应压缩功能的配置与优化,需... 目录一、核心工作机制1.1 自动协商触发条件1.2 压缩处理流程二、配置方案详解2.1 基础YAML

java中使用POI生成Excel并导出过程

《java中使用POI生成Excel并导出过程》:本文主要介绍java中使用POI生成Excel并导出过程,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录需求说明及实现方式需求完成通用代码版本1版本2结果展示type参数为atype参数为b总结注:本文章中代码均为

在java中如何将inputStream对象转换为File对象(不生成本地文件)

《在java中如何将inputStream对象转换为File对象(不生成本地文件)》:本文主要介绍在java中如何将inputStream对象转换为File对象(不生成本地文件),具有很好的参考价... 目录需求说明问题解决总结需求说明在后端中通过POI生成Excel文件流,将输出流(outputStre

MySQL中慢SQL优化的不同方式介绍

《MySQL中慢SQL优化的不同方式介绍》慢SQL的优化,主要从两个方面考虑,SQL语句本身的优化,以及数据库设计的优化,下面小编就来给大家介绍一下有哪些方式可以优化慢SQL吧... 目录避免不必要的列分页优化索引优化JOIN 的优化排序优化UNION 优化慢 SQL 的优化,主要从两个方面考虑,SQL 语