6軟硬約束下的軌跡優化

本文轉載自查看原文 2021-08-04 11:00 129

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，使得當里障礙物特別近的時候，懲罰急劇上升。