• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

Minimum Snap轨迹规划详解(3)闭式求解

人工智能 一抹烟霞 1556次浏览 0个评论

如果QP问题只有等式约束没有不等式约束,那么是可以闭式求解(close form)的。闭式求解效率要快很多,而且只需要用到矩阵运算,不需要QPsolver。 这里介绍Nicholas Roy文章中闭式求解的方法。

1. QP等式约束构建

闭式法中的Q 矩阵计算和之前一样(参照文章一),但约束的形式与之前略为不同,在之前的方法中,等式约束只要构造成[…]p=b的形式就可以了,而闭式法中,每段poly都构造成 Minimum Snap轨迹规划详解(3)闭式求解

其中d0,dT为第i段poly的起点和终点的各阶导数组成的向量,比如只考虑PVA:Minimum Snap轨迹规划详解(3)闭式求解,当然也可以把jerk,snap等加入到向量。注意:这里是不管每段端点的PVA是否

Minimum Snap轨迹规划详解(3)闭式求解

已知,都写进来。 块合并各段轨迹的约束方程得到  k为轨迹段数,n为轨迹的阶数,设只考虑pva,Atotal的size为Minimum Snap轨迹规划详解(3)闭式求解这里为了简化,没有把每段poly的timestamp都改成从0开始,一般,为了避免timestamp太大引起数值问题,每段poly的timestamp都成0开始。 由上式可以看到,Atotal是已知的(怎么构造可参见文章一中的等式约束构造方法),而d中只有少部分(起点、终点的pva等)是已知的,其他大部分是未知的。如果能够求出d,那么轨迹参数可以通过p=A-1d很容易求得。  

2. 如何求d?

  闭式法的思路是:将向量中的变量分成两部分:”d中所有已知量组成的Fix部分dF”和”所有未知量组成的Free部分dP”。然后通过推导,根据dF求得dP,从而得到d,最后求得p。 下面介绍整个推导过程,  

2.1. 消除重复变量(连续性约束)

  可以会发现,上面构造等式约束时,并没有加入连续性约束,连续性约束并不是直接加到等式约束中。考虑到连续性(这里假设PVA连续),d   向量中很多变量其实重复了,即因此需要一个映射矩阵将一个变量映射到两个重复的变量上,怎么映射?

Minimum Snap轨迹规划详解(3)闭式求解

  • 如,将变量a映射到左边向量中的两个变量。   所以构造映射矩阵Minimum Snap轨迹规划详解(3)闭式求解        即d=Md’d=Md

2.2 向量元素置换

  消除掉重复变量之后,需要调整d’中的变量,把fix部分和free部分分开排列,可以左乘一个置换矩阵C,使得 Minimum Snap轨迹规划详解(3)闭式求解   C矩阵怎么构造?

  • 举个例子,设Minimum Snap轨迹规划详解(3)闭式求解其中a,b,c是已知(dF),b未知(dp),构造一个4×4的单位阵,取dF所在的(1,3,4)列放到左边,再取dp所在的(2)列放到右边,就构造出置换矩阵C: 

2.3 转成无约束优化问题

  由上面两步可得 Minimum Snap轨迹规划详解(3)闭式求解   代入优化函数

Minimum Snap轨迹规划详解(3)闭式求解     Minimum Snap轨迹规划详解(3)闭式求解

3. 闭式法步骤

  总结一下整个闭式法的步骤:

  1.先确定轨迹阶数(比如5阶),再确定向量中的约束量(pva),进而根据各段的时间分配求得AtotalAtotal

 2.根据连续性约束构造映射矩阵M,并确定d向量中哪些量是Fix(比如起点终点pva,中间点的p等),哪些量是Free,进而构造置换矩阵C,并求得K=A-1MCKM=A1MC 3.计算QP目标函数中的Q(min Jerk/Snap)并计算R=KTQK,根据fix变量的长度将R拆分成RFF,RFP,RPF,RPP块。 4.填入已知变量得到dF,并根据Minimum Snap轨迹规划详解(3)闭式求解计算得到d_p 5.根据公式计算得到参数pp

 

Minimum Snap轨迹规划详解(3)闭式求解p=p 闭式法主要计算量就在A矩阵的求逆,其他计算基本上是矩阵构造,所以效率比较高,但由于没有不等式约束,所以在中间点只能加强约束,corridor不能直接加到QP问题中,只能是通过压点来实现corridor。 在对计算效率要求比较高或者不想用QPsolver时,可以使用闭式法求解。 代码见这里,由于效果和文章一中的效果一样,这里就不再贴图。 参考链接:https://blog.csdn.net/q597967420/article/details/79031791

 


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明Minimum Snap轨迹规划详解(3)闭式求解
喜欢 (0)

您必须 登录 才能发表评论!

加载中……