当前位置:   article > 正文

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

软约束


前言

可以看看我的这几篇Blog1Blog2Blog3

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

  • 轨迹让机器人可以在某个时间点抵达某个航点。
  • 任何一个时刻,都能数学上求出期望的机器人的位置、速度、加速度、导数。
  • Minimum Snap可以把问题转换为凸优化问题。
    缺点:
  • Mnimum Snap可以控制轨迹一定经过中间的哪些航点,但是控制不了航点与航点之间生成的这个“OverShoot”,这在控制里面称之为超调,其实在此处不是叫超调,只是借用了一下这个概念,方便理解。也就是Minimum Snap没有考虑障碍物的环境信息。下图中的直观展示就是在直线上方,与红色曲线之间的差值部分,就是OverShoot,因为此时红线已经经过红色障碍物了。
    在这里插入图片描述
    **综上,Minimum Snap轨迹优化方法非常有利于生成生成光滑连续的轨迹的,但是其天然地不利于做避障。**所以,这次的重点就是,如何修改Minimum Snap,使得其可以去做避障。

约束

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

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

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

在这里插入图片描述

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


硬约束的轨迹优化

Corridor-Based Trajectory Optimization

在这里插入图片描述

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

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


在这里插入图片描述


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

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

  1. 将时间 T {T} T进行离散,加约束,使得每一个点都在 x , y {x},{y} x,y的边界之内,不过这样就要将 T {T} T离散成很多份,会使得这个凸优化问题有非常多约束同时非常难解。
    在这里插入图片描述
  2. 采用段与段之间的连接点先施加一个约束,可以保证两端轨迹的中间部分一定经过重叠部分,但没保证不在重叠部分的轨迹不会跑到飞行走廊外面。但是无法直接施加上的,不然很多约束。
    在这里插入图片描述
    采用后验的迭代检测。
    在这里插入图片描述
    与之前的Minimum Snap相比,其连接点是不可以移动的,但是在这里,连接点是可以在方框内自由移动的,因此就算一开始给的时间不是特别好,例如用速度曲线去分配时间,不好的话其迭代检测会自动的去分配时间,进而去优化连接点。

在这里插入图片描述


Bezier Curve Optimization

在这里插入图片描述

Bezier曲线一定是可以与某个多项式函数有映射的关系的。

以下是B曲线的一些性质。
在这里插入图片描述

B曲线总是能通过起始点,并且会终止于最后一个点。

在这里插入图片描述

在这里插入图片描述

各个性质的应用。


Other Options

来自因特尔实验室的一篇文章,也是前端找Path(RRT Star),在找Path之前会对环境按照方块的大小对障碍物进行膨胀,这个环境膨胀不光是防止碰撞的膨胀,还会把环境进行更多保守的膨胀,之后有了Path会有很多点,在很多点上再继续构造一个个小方块。
在这里插入图片描述

以下是利用在无人机编队的轨迹优化。
在这里插入图片描述
相当于规划安全距离。


软约束的轨迹优化

Distance-Based Trajectory Optimization

软约束是对机器人产生一个力,使得机器人远离障碍物。最直观的展现方式就是距离描述。
一些硬约束的缺陷如下,对传感器噪点敏感等;软约束就能通过推力而产生一条光滑的轨迹,其优势就是自然的产生光滑的轨迹。软约束也有缺陷,是倾向满足而不是严格满足。

在这里插入图片描述


在这里插入图片描述

在这里插入图片描述

以上推导参考上篇Blog的Minimum Snap推导

ESDF是距离场(参考人工势场法的思想,障碍物较远,惩罚就趋于0;障碍物较近,惩罚呈指数上升。)
在这里插入图片描述

优化方法

在这里插入图片描述

下降优化方法(容易陷入局部最小值)

在这里插入图片描述

线性搜索优化
在这里插入图片描述

梯度下降法(同样容易陷入局部最小值)
在这里插入图片描述

牛顿方法(不光只看梯度)
在这里插入图片描述
Leven法,就是一阶法与二阶法的折中。
在这里插入图片描述

在这里插入图片描述

在这里插入图片描述

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/代码探险家/article/detail/879113
推荐阅读
相关标签
  

闽ICP备14008679号