6软硬约束下的轨迹优化

Introduction

Minimum snap trajectory optimization

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

方法

产生障碍物推力（人工势场法），添加硬约束（bound box绿色方块)

Hard/Soft constraints

$$\begin{gathered} \min f\left(x\right) \\ s.t.g_i\left(x\right)=c_i,i=1,...,n\text{等式约束} \\ {h}_j\left(x\right)⩾d_j,j=1,...,n\text{不等式约束} \end{gathered}$$
硬约束必须要满足

---

软约束目标是尽量减少下面函数值
$\min f\left(x\right)+{\lambda }_1\cdot g\left(x\right)+{\lambda }_2\cdot h\left(x\right)$
$\lambda$后面惩罚项/损失函数
•首选但不是严格要求的约束。
•包含各种损失函数。

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

如何将一段轨迹放在方框里，特别是重叠区域，中间区域

• 瞬时线性约束，比较好施加
• • 起始点，重点约束（$A_p=b$）
• • 变化点 （$A_p=b，A_p⩽b$)
• • 连续约束（$A_{p_i}=A_{p_{(}i+1)}$
• 间隔线性的约束
• 对于$A\left(t\right)p⩽b,\forall t\in \left[t_l,t_r\right]$
• 边界约束
• 动力学约束（v,a限制)

Advantages

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

Disadvantages

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

• 但是QP问题迭代求解非常耗时。
• 如果没有严格可行的解决方案满足所有约束。那我们要跑10次迭代来确定解决方案?
• 原因:
  •检验极值(多项式求极值转变为导数为0的问题)是另一个多项式求根问题。
  •四次等式有求根公式，容易求,但是高阶呢。

Polynomial roots finding

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

Bezier Curve Optimization贝塞尔曲线优化

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

Trajectory basis changing

使用Bernstein多项式基。
•将轨迹的基从单项多项式改为伯恩斯坦多项式
•Bézier曲线只是一个特殊的多项式，它可以通过以下方法映射为单项多项式:
$p=M\dot{c}$之前的推导仍然成立。

$\left[\begin{array}{c}n\\ i\end{array}\right]\text{相当于}{c}_n^i$

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性质实现, $f_1(0)=s_0,f_N(T_N)=s_N$
综上：约束条件仍然为线性等式约束和线性不等式约束，仍然为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)$表示t时刻位置,$c(p(t))$为惩罚函数,涉及到距离场;二者的确定方式:首先可以构建一个Euclidean signed distance field（ESDF），每个格子储存其到障碍物的最小距离。然后再经过一个幂次函数，或者barrier function，使得当里障碍物特别近的时候，惩罚急剧上升。
  • 由于是对路径积分，求导不太好求，所以把路径离散化掉再对$d_{pu}$求导,$c(p(T_k))和|v(t)$与其有关,$\delta t$是常量
• 动力学项,惩罚速度和加速度,与碰撞项类似,不过$c_v$不用构建ESDF.
• 目标函数(ESDF,这是由于距离场的特性决定的)并非凸函数,不能直接凸优化求解,而需要通过一步步求导,平滑项$J_s$,碰撞项对自由变量$d_{pu}$求导,得到关于它的雅可比矩阵

二阶求导结果

Euclidean signed distance field (ESDF)

Numerical optimization

基于梯度的数值优化方法，通过minimizef(x)求得关于$x^k$的数值序列使得$f(x^k)$逼近最优解，或者使f(x)的导数为0。有以下求解方法
一阶法，即梯度下降，向导数的负方向搜索：


判断梯度的变化大小进而判断是否收敛

线性搜索方法

• 二阶方法

一二阶方法之间

已经有很多方法,如ceres NLopt

Planning strategy

先执行一段固定轨迹,先考虑collision cost,再优化其他

一旦某一项出现情况就会急剧上升,总体J要保证下降

optimization strategy

optimize
optimal

两部走收敛结果好而快

论文: Gradient-Based Online Safe Trajectory Generation for Quadrotor Flight in Complex Environments, Fei Gao et al.
Source code

Case Study

Fast planner

Kinodynamic path searching

• B-Spline trajectory optimization
• time adjustment
  
  
  B样条
  
  优化速度的提升
  凸包特性:简化安全与动力约束
  连续性:节段连接处不需要约束
  
  
  
  
  

Homework

•在matlab中编写一个生成走廊约束分段贝塞尔曲线。
•给出了贝塞尔到单项多项式的转换。
•走廊是预先设定好的。
•只需要限制位置。