软约束、硬约束、Minimum Snap的轨迹优化方法

世界仙境与冷酷尽头

已于 2022-09-14 08:59:37 修改

阅读量3.1k

点赞数 11

分类专栏：算法文章标签：算法 c++ matlab 动态规划性能优化

于 2022-06-09 20:21:23 首次发布

本文链接：https://blog.csdn.net/weixin_44673253/article/details/125196420

版权

算法专栏收录该内容

29 篇文章 85 订阅

订阅专栏

文章目录

前言
- 约束
硬约束的轨迹优化
软约束的轨迹优化
- Distance-Based Trajectory Optimization
- 优化方法

前言

可以看看我的这几篇Blog1，Blog2，Blog3。

上次基于Minimum Snap的轨迹生成，有许多优点，比如：

轨迹让机器人可以在某个时间点抵达某个航点。
任何一个时刻，都能数学上求出期望的机器人的位置、速度、加速度、导数。
Minimum Snap可以把问题转换为凸优化问题。
缺点：
Mnimum Snap可以控制轨迹一定经过中间的哪些航点，但是控制不了航点与航点之间生成的这个“OverShoot”，这在控制里面称之为超调，其实在此处不是叫超调，只是借用了一下这个概念，方便理解。也就是Minimum Snap没有考虑障碍物的环境信息。下图中的直观展示就是在直线上方，与红色曲线之间的差值部分，就是OverShoot，因为此时红线已经经过红色障碍物了。

**综上，Minimum Snap轨迹优化方法非常有利于生成生成光滑连续的轨迹的，但是其天然地不利于做避障。**所以，这次的重点就是，如何修改Minimum Snap，使得其可以去做避障。

约束

环境当中的障碍物表示机器人不可通行的区域，其对偶问题就是环境里的可通行的区域。现在想让Minimum Snap不碰到障碍物：

加一个推力，使得快靠近障碍物的时候，把机器人推回可通行的区域，参考方法软约束的人工势场法。
在可通行的区域中加约束，也可以把轨迹吸引到可通行区域，比如在红色障碍物下面，加上一个绿色的方框，使得机器人走这条路的时候一定通过这个方框，这就是所谓的硬约束。

一个模型，其目标函数为 ${f}(x)$ ，有很多的约束条件（等式或者不等式），其中硬约束要求这两类的约束条件一定要在全局的条件下满足的。软约束就是把约束条件全部加到目标函数 ${f}(x)$ 里，相当于拉格朗日乘子？

在这里插入图片描述

这些约束是被倾向于实现的，而不是严格满足的。因为要减少全局，所以使得 ${h}(x)和{g}(x)$ 的值变小，其实一开始的目的是全局最小，没怎么要求 ${h}(x)和{g}(x)$ 的值，所以就是倾向性的。也称 ${\lambda}_{1}*g(x)+{\lambda}_{2}*h(x)$ 为损失函数，下面几幅图就是常用的损失函数。

硬约束的轨迹优化

Corridor-Based Trajectory Optimization

在这里插入图片描述

把环境中的信息塞入八叉树地图（格子有大有小，不像栅格地图那样规整），之后利用图搜索算法（A*，JPS）进行搜索，就找到了一条飞行走廊，参考图二的蓝色方框，之后由于空余的位置还很多，所以对走廊膨胀，参考黄色的部分，在膨胀过后的飞行走廊生成分段连续的多项式表示的无人机轨迹。12维到4维同样是利用之前的微分平坦性。

与之前不同的是，在Minimum Snap当中，一段段轨迹之间是有WayPoint的（连续性约束，必须要经过定好的WayPoint），现在的WayPoint是存在在前后两个方块的交界处，只需要通过交界处的走廊就行。

在这里插入图片描述

如红色的阴影面积。
在这里插入图片描述

那么问题来了，如何使一段多项式的轨迹一定位于这个Cube里面？

将时间 ${T}$ 进行离散，加约束，使得每一个点都在 ${x},{y}$ 的边界之内，不过这样就要将 ${T}$ 离散成很多份，会使得这个凸优化问题有非常多约束同时非常难解。
采用段与段之间的连接点先施加一个约束，可以保证两端轨迹的中间部分一定经过重叠部分，但没保证不在重叠部分的轨迹不会跑到飞行走廊外面。但是无法直接施加上的，不然很多约束。

采用后验的迭代检测。

与之前的Minimum Snap相比，其连接点是不可以移动的，但是在这里，连接点是可以在方框内自由移动的，因此就算一开始给的时间不是特别好，例如用速度曲线去分配时间，不好的话其迭代检测会自动的去分配时间，进而去优化连接点。