6软硬约束下的轨迹优化

Introduction

Minimum snap trajectory optimization

在这里插入图片描述

Minimum snap这种方法只限制轨迹应该会通过的中间路径点(中继点),collision checking存在于轨迹搜寻中。
优点在于计算成本低，易于实现。但对轨迹本身没有约束,轨迹的“超调”不可避免如图所示。
基于Minimum snap 框架有利于平滑曲线但不利于避障。所以需要我们进行优化.

方法

产生障碍物推力（人工势场法），添加硬约束（bound box绿色方块)
在这里插入图片描述

Hard/Soft constraints

硬约束必须要满足

软约束目标是尽量减少下面函数值

$λ后面惩罚项/损失函数 •首选但不是严格要求的约束。 •包含各种损失函数。$

Hard-constrained Optimization

Corridor-based Trajectory Optimization

[论文](Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments , Jing Chen et al.)
八叉树地图->搜寻处一条几何空间的路径（不考虑动力学）->对方格进行膨胀->生成走廊里的轨迹

问题是凸优化是不考虑初值的A

Problem formulation

如何将一段轨迹放在方框里，特别是重叠区域，中间区域
在这里插入图片描述

Advantages

效率:简化图中的路径搜索，凸优化在走廊上是高效率的。(二次规划问题)
高质量:走廊提供大的优化自由度。
红色为时间点，能够移动，可以根据需要调节

Disadvantages

所有的约束只在分段节点上执行，如何保证它们是有效的
迭代检查极值并添加额外的约束。将它压下去，直到符合给定约束的机制

但是QP问题迭代求解非常耗时。
如果没有严格可行的解决方案满足所有约束。那我们要跑10次迭代来确定解决方案?
原因:

　　•检验极值(多项式求极值转变为导数为0的问题)是另一个多项式求根问题。

　　•四次等式有求根公式，容易求,但是高阶呢

Polynomial roots finding

•高阶多项式需要数值解，使用伴随矩阵，转化为矩阵求根问题.在matlab的root公式中说明如下

Bezier Curve Optimization贝塞尔曲线优化

Y:替换多项式的形式,因为其不方便施加约束

Trajectory basis changing

使用Bernstein多项式基。
•将轨迹的基从单项多项式改为伯恩斯坦多项式
•Bézier曲线只是一个特殊的多项式，它可以通过以下方法映射为单项多项式:

Properties:
Bezier的系数c称为控制点，有实际的物理意义
•端点插值。贝塞尔曲线总是从第一个控制点开始，到最后一个控制点结束,绝不通过任何其他控制点。如图
•凸包。贝塞尔曲线𝐶(𝑢)由一组完全受限的控制点ci组成,一段Bezier曲线一定被其所有控制点围成的凸包包围住(P0~P4)
•Hodograph。Bezier曲线B(𝑢)的导数曲线B’(𝑢)被称为Hodograph，还是贝塞尔曲线，
并且是由n∙(c𝑗+1−c𝑗)定义的具有控制点的Bezier曲线，其中n为度。
• 固定的时间间隔。Bezier曲线总是在[0,1]上定义,所以需要轨迹进行缩放

Convex hull property

凸多边形如果被约束在飞行走廊中，那么轨迹也就被约束在飞行走廊中，同样轨迹的导数即速度以及加速度也可以利用凸包范围进行限制。
•飞行走廊由凸多边形组成。
•每一个立方体对应着一条贝塞尔曲线。
•这条曲线的控制点被约束在多边形内
•轨迹完全在所有点的凸包内

Trajectory Generation Formulation

目标函数(依然是二次型)
连续性约束只需要前端末尾后端开始两个被控制点相等，速度可以利用hodograph的性质n(c1-c0)=v,同理,加速度可以用速度表示边界，
安全性约束也都可以使用凸包性质,控制点在安全走廊中,不等式约束.

动力学约束可以使用凸包性质和Hodograph性质实现,

综上：约束条件仍然为线性等式约束和线性不等式约束，仍然为QP问题。不过优势是能够彻底地避免迭代求解问题.

Simulation result

[论文](Online Safe Trajectory Generation For Quadrotors Using Fast Marching Method and Bernstein Basis Polynomial , Fei Gao et al.)
source code

Other Options

Dense constraints(稠密约束)

•在离散时间点添加大量约束。
•每一个时间段恒定加速度。
•QP方案解决方案

速度慢，
总是生成过于保守的轨迹。
约束太多，计算负担高

[论文](A hybrid method for online trajectory planning of mobile robots in cluttered environments , L Campos-Macías et. al)

Mixed integer optimization

并非凸优化问题，用于无人机编队
对问题解决用数学描述为大M法，
[论文](Mixed-integer quadratic program trajectory generation for heterogeneous quadrotor teams , D. Mellinger et al.)

Soft-constrained Optimization

Distance-based Trajectory Optimization

Motivation

Vison-based无人机:
•有限的传感范围和质量
•噪声深度估计
Hard-constrained方法:
•所有解空间都是等价的
•解空间对噪音敏感

软约束能够让路径更加远离障碍物,但是如果目标函数设置不合理,不一定使得约束能够完全满足.

Problem formulation

利用微分平坦性将无人机参数空间降维，并用多项式模拟轨迹
定义目标函数,第一项为平滑项、第二项为碰撞项、第三项为动力学约束项。
平滑项可以使用Minimum Snap求解公式求解,用中继点的(p,v,a)来代替路径多项式的参数,但在这里中继点的位置是可以改变的

碰撞项,惩罚距离最近的障碍物的轨迹点(对曲线沿着路径进行积分ds),然后分解为小段之和.p ( t ) p(t)p(t)表示t时刻位置,c ( p ( t ) ) c(p(t))c(p(t))为惩罚函数,涉及到距离场;二者的确定方式:首先可以构建一个Euclidean signed distance field（ESDF），每个格子储存其到障碍物的最小距离。然后再经过一个幂次函数，或者barrier function，使得当里障碍物特别近的时候，惩罚急剧上升。