6软硬约束下的轨迹优化

明码

已于 2023-11-30 21:00:44 修改

阅读量1.9k

点赞数 9

分类专栏： Motion Planning 文章标签：算法自动驾驶 matlab

于 2021-06-11 14:23:45 首次发布

本文链接：https://blog.csdn.net/qq_37087723/article/details/116152961

版权

Motion Planning 专栏收录该内容

18 篇文章 47 订阅

订阅专栏

Introduction

Minimum snap trajectory optimization

在这里插入图片描述

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

方法

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

Hard/Soft constraints

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

软约束目标是尽量减少下面函数值
$\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 \leqslant b$ )
- 连续约束（ $A_{p_i}=A_{p_(i+1)}$
间隔线性的约束
对于 $A\left( t \right) p \leqslant 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曲线只是一个特殊的多项式，它可以通过以下方法映射为单项多项式:
$\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中编写一个生成走廊约束分段贝塞尔曲线。
•给出了贝塞尔到单项多项式的转换。
•走廊是预先设定好的。
•只需要限制位置。
在这里插入图片描述

ref

https://www.shenlanxueyuan.com/course/633

明码

关注

9
点赞
踩
21

收藏

觉得还不错? 一键收藏
3
评论
6软硬约束下的轨迹优化

IntroductionMinimum snap trajectory optimization我们只限制中间路径点轨迹应该会通过。•计算成本低，易于实现。•对轨迹本身没有约束。•轨迹的“超调”不可避免。基于Minimum snap 框架有利于平滑曲线代，不利于避障。所以需要我们进行修改，产生障碍物推力（人工势场法），添加硬约束（bound box绿色方块)Hard/ Soft constraintsmin⁡f(x)s.t.gi(x)=ci,i=1,...,n 等式约束 hj(
复制链接

扫一扫