1. Preliminaries

Ch2講了基于搜索的路徑規(guī)劃方法，Ch3講了基于采樣的路徑規(guī)劃方法，這些都是global methods，框架都是Exploration and Exploitation，且在算力足夠大的情況下，一定能夠找到全局最優(yōu)解。

除了global methods，還有l(wèi)ocal methods，主要是Deterministic Optimization確定性?xún)?yōu)化?；趦?yōu)化的方法，主要是利用cost function的0階和高階導(dǎo)數(shù)來(lái)進(jìn)行優(yōu)化（如梯度下降，迅速地尋找到一個(gè)局部極小值local minimum），與global methods不同的是，此類(lèi)方法少了探索的過(guò)程，所以不能保證結(jié)果最優(yōu)。

PRM*，RRT* 是AO(Asymptotic Optimality)漸進(jìn)最優(yōu)的方法，A*，JPS是RO(Resolution Optimality)分辨率最優(yōu)的方法。這些均屬于global methods，在低維時(shí)(如4維一以下)工作較好，但高維時(shí)就需要實(shí)時(shí)性更好的方法。

右側(cè)的均為local methods，CHOMP用于多機(jī)械臂軌跡優(yōu)化，DDP/iLQR用于無(wú)人車(chē)規(guī)劃，F(xiàn)latness用于無(wú)人機(jī)，MPC/NMPC用于一般的機(jī)器人軌跡優(yōu)化，這類(lèi)方法一般實(shí)時(shí)性較高。

如上為global methods和local methods的優(yōu)缺點(diǎn)對(duì)比。

global和local methods在實(shí)現(xiàn)上經(jīng)常被分為前端和后端，可以把來(lái)自環(huán)境的復(fù)雜度和來(lái)自于動(dòng)力學(xué)約束的復(fù)雜度分而治之[1,2,3,4]。

Q1：什么是軌跡？
A1：Trajectories are time-parameterized paths.
軌跡是時(shí)間參數(shù)化的路徑。

Q2：Are smooth curves trajectories?
A2：smooth curves不是traj，因?yàn)闆](méi)有時(shí)域信息，沒(méi)有時(shí)域信息的均當(dāng)作curves處理。

Q3：Are trajectories always smooth?
A3：幾何學(xué)與動(dòng)力學(xué)的smoothness不同，所以traj不一定smooth。

Q4：Can nonsmooth paths be trajectories?
A4：幾何上非光滑的path也可以是traj。

trajectory是同時(shí)包含時(shí)間和空間特性的。

traj就是把時(shí)間映射到各種space中，space中可能包含位置，速度，偏航角，旋轉(zhuǎn)等。

一個(gè)smooth的traj至少要

1. 滿(mǎn)足微分約束（系統(tǒng)方程通常由常微分方程給出），
2. 最小化能量泛函(最小化cost，如能量消耗等)

為什么后端是必須的？
盡管有一些考慮了kinodynamaic的前端生成一些path，給出一些traj的初值，但是后端可以進(jìn)一步優(yōu)化，在前端結(jié)果不那么好的時(shí)候也能收斂到較好的結(jié)果。

除了考慮動(dòng)力學(xué)約束，還需要考慮很多其他約束，如時(shí)間效率，驅(qū)動(dòng)器限制，任務(wù)要求等，綜合考慮這么多約束，優(yōu)化是一種非常好的方法。

2. Multicopter dynamics and differential flatness（多旋翼動(dòng)力學(xué)和微分平坦特性）

2.1 Differential Flatness

簡(jiǎn)單來(lái)說(shuō)，對(duì)于由常微分方程(ODE，Ordinary Differential Equation)描述的系統(tǒng)，在對(duì)系統(tǒng)施加了各種約束之后，由系統(tǒng)狀態(tài)$x$，系統(tǒng)狀態(tài)的導(dǎo)數(shù)$\dot{x}$，輸入$u$所張開(kāi)的空間$R$中，滿(mǎn)足約束的解 $z$ 只能在該空間的一個(gè)曲面上移動(dòng)，為了進(jìn)行求解，引入了微分平坦變換(differential flatness transformation)，能夠在一個(gè)無(wú)約束的空間中對(duì)軌跡 $z$ 進(jìn)行優(yōu)化，這個(gè)優(yōu)化結(jié)果也是滿(mǎn)足$R$中的約束的，微分平坦變換就是上圖的${\Psi }_x,{\Psi }_u$，該變換消除了微分約束，降低優(yōu)化難度，在優(yōu)化出$z$之后，可以通過(guò)微分平坦變換求出$x,u$，是滿(mǎn)足微分約束的。

（該部分比較數(shù)學(xué)，細(xì)節(jié)如果需要的話再深入探討）

槳對(duì)齊的四旋翼/多旋翼，線性風(fēng)阻的多旋翼，槳以一定規(guī)律排布的多旋翼都滿(mǎn)足differential flatness[5,6,7,8,9].

雖然使用differential flattness能夠簡(jiǎn)化無(wú)人機(jī)的優(yōu)化問(wèn)題，但是有兩個(gè)問(wèn)題需要考慮：

1. 無(wú)人機(jī)實(shí)際飛行時(shí)需要考慮風(fēng)阻drag（因?yàn)樗俣瓤鞎r(shí)必須考慮風(fēng)阻）
2. differential flattness存在奇異點(diǎn)，需要盡可能降低奇異點(diǎn)個(gè)數(shù)。

上圖列出了6種使用differential flattness時(shí)會(huì)出現(xiàn)奇異點(diǎn)的地方(即出現(xiàn)$\frac{1}{0}$的情況)

需要說(shuō)明，yaw在無(wú)人機(jī)領(lǐng)域不一定是傳統(tǒng)意義上的定義，可根據(jù)應(yīng)用場(chǎng)景定義。

1，2使用不同轉(zhuǎn)序的Euler角，不考慮風(fēng)阻。
3將body x軸投影至world系，與world系的x軸所成夾角即為yaw，顯然奇異點(diǎn)在body的x軸投影與world系x軸平行時(shí)出現(xiàn)。
4考慮了線性的風(fēng)阻。
5通過(guò)Hopf fibration定義yaw，旋轉(zhuǎn)通過(guò)yaw和tilt定義(tilt即body的z軸與world的z軸所成夾角)，此方法僅有一個(gè)奇異點(diǎn)，出現(xiàn)在body的z軸與重力方向平行時(shí)(即機(jī)身完全翻轉(zhuǎn)時(shí))。
6對(duì)5中的奇異點(diǎn)在數(shù)學(xué)上進(jìn)行了證明[10]。

補(bǔ)充：

Hopf fibration（霍普夫纖維化）是一種在純數(shù)學(xué)中的概念，它描述了一個(gè)四維球面如何映射到三維空間上的球面。具體而言，Hopf fibration是一個(gè)從四維單位球面（稱(chēng)為四維球面S^{3）到三維球面（稱(chēng)為三維球面S}2）的映射。

這個(gè)映射的特殊之處在于，它將四維球面的每一個(gè)點(diǎn)映射到三維空間中的一個(gè)點(diǎn)，并且保持了一種特殊的關(guān)系：在四維球面上，每一條經(jīng)線（類(lèi)似于經(jīng)度線）都被映射為三維球面上的一個(gè)圓，而緯線（類(lèi)似于緯度線）則被映射為三維球面上的一條線。

Hopf fibration在數(shù)學(xué)和物理學(xué)中都具有重要的應(yīng)用。它在拓?fù)鋵W(xué)、幾何學(xué)和量子場(chǎng)論等領(lǐng)域中有著廣泛的研究和應(yīng)用價(jià)值。這個(gè)概念的提出者是德國(guó)數(shù)學(xué)家Heinz Hopf，他在1931年首次引入了這個(gè)概念并進(jìn)行了深入研究。

2.2 具體建模

參考

• state is $x=r,v,R,w$，分別為位移，速度，旋轉(zhuǎn)，在body系下的角速度，分別為
  ${\mathbb{R}}^3\times {\mathbb{R}}^3\times SO(3)\times {\mathbb{R}}^3$
• 輸入$u$為推力thrust的標(biāo)量和3軸扭矩向量$\tau$
• 動(dòng)力學(xué)方程(ODE)：$\dot{x}=fx+g(x)u$，后面細(xì)講
• 平坦輸出flat output：$z=r,\psi$分別為位移$\in {\mathbb{R}}^3$和2D yaw $\in SO(2)$

針對(duì)無(wú)人機(jī)動(dòng)力學(xué)方程：
這里的物理量均定義在world系（即慣性系）下。

1. $\dot{r}=v$
   位置對(duì)時(shí)間求導(dǎo)是速度。
2. 牛頓第二定律$ma=F$
   右側(cè)為無(wú)人機(jī)在飛行過(guò)程中收到的和力。

• $?mg{e}_3$：m質(zhì)量，g重力標(biāo)量，$e=(0,0,1{)}^T$表示豎直向上，與重力方向反向。該項(xiàng)表示無(wú)人機(jī)收到的重力(是向量)。

• $Rf{e}_3$：$R=R_{wb}$由body到world的旋轉(zhuǎn)，$f$為總推力thrust大小。該項(xiàng)表示系統(tǒng)的推力向量在world系下的向量。

• $?RD{R}^T\sigma (||\mathbf{v}||)v$：表示無(wú)人機(jī)在飛行過(guò)程中收到的風(fēng)阻。
  該項(xiàng)可如下理解：
  

  • $?R^{T}v$是無(wú)人機(jī)world系下速度轉(zhuǎn)到body下的速度取反
  • $D\sigma (||v||)$中：$||v||$表示world系下無(wú)人機(jī)速度大小，標(biāo)量$\sigma$表示縮放系數(shù)，$D$為對(duì)角矩陣，是風(fēng)阻系數(shù)（與無(wú)人機(jī)外形等有關(guān)）。
  • 故$D\sigma (||v||)(?R^{T}v)$為body系下收到的風(fēng)阻力向量，左乘$R$轉(zhuǎn)至world系下為$RD\sigma (||v||)(?R^{T}v)$，整理為$?RD{R}^T\sigma (||\mathbf{v}||)v$整體表示無(wú)人機(jī)飛行時(shí)在world系下所受的風(fēng)阻力向量。

• $\hat{R}=R\hat{\omega }$：旋轉(zhuǎn)矩陣更新(反對(duì)稱(chēng)使用$\hat{w}$表示，搞slam的很熟悉了)

• $M\dot{\omega }=\tau ?\omega \times M\omega ?A(\omega )?B(R^{T}v)$：歐拉公式加上修正項(xiàng)。其中$M\dot{\omega }=\tau ?\omega \times M\omega$為常見(jiàn)的歐拉公式，$M$為慣性張量矩陣，$\tau$為三軸力矩。$A(\omega )$：飛機(jī)轉(zhuǎn)動(dòng)時(shí)產(chǎn)生的阻尼力矩，$B(R^{T}v)$平動(dòng)時(shí)產(chǎn)生的力矩。

整個(gè)系統(tǒng)方程中，$D,\sigma ,A,B$可以通過(guò)參數(shù)擬合，系統(tǒng)辨識(shí)等方法來(lái)分別確定。

2.3 Flatness Transformation的解析推導(dǎo)

如果已經(jīng)有了flat output，如何求出我們的目標(biāo)狀態(tài)呢？這時(shí)需要解析地求出微分平坦變換${\Psi }_x,{\Psi }_u$，需要指出，${\Psi }_x,{\Psi }_u$的表達(dá)式很復(fù)雜，表達(dá)式也可以通過(guò)迭代的方式來(lái)給出，也即如果我們知道如何迭代地求出$x,u$，也就相當(dāng)于知道了${\Psi }_x,{\Psi }_u$的表達(dá)式。

下面分別給出$r,v,R,\omega ,f,\tau$的求解過(guò)程。

對(duì)系統(tǒng)方程中的牛頓方程兩邊左乘body系下的x，y軸，即$(R{e}_i{)}^T,i\in (1,2)$，推導(dǎo)出下圖的結(jié)論：
$$\begin{array}{r}\dot{v}+\frac{d_h}{m}\sigma (||v||v+g{e}_3)\end{array}\text{\hspace{1em}?}$$
body的x，y軸與式(1)垂直

與$x_b,y_b$垂直的只有與$z_b$同向或反向，當(dāng)無(wú)人機(jī)懸停時(shí)，$v=0$得出式(1)為$g{e}_3$，為推力，與重力反向，所以式(1)與重力反向，與$z_b$同向。

接著牛頓方程兩邊同乘$z_b=(R{e}_3{)}^T$，可以推導(dǎo)出推力標(biāo)量$f$的表達(dá)式。

旋轉(zhuǎn)$R$可由偏航旋轉(zhuǎn)和傾斜旋轉(zhuǎn)求出，旋轉(zhuǎn)與四元數(shù)是1對(duì)2的關(guān)系，即一個(gè)旋轉(zhuǎn)可對(duì)應(yīng)兩個(gè)不同的四元數(shù)，而一個(gè)四元數(shù)可以確定一個(gè)唯一的旋轉(zhuǎn)。

旋轉(zhuǎn)四元數(shù)表示為旋轉(zhuǎn)軸$\mathbf{v}$和旋轉(zhuǎn)角度$\theta$，即$q=((cos\theta {)}^{1\times 1},(\mathbf{v}sin\theta {)}^{3\times 1})=(w,x,y,z{)}^T$

• 偏航四元數(shù)$q_{\psi }$，僅繞$e_3=(0,0,1{)}^T$旋轉(zhuǎn)，即$q_{\psi }$中$x,y=0$
• 傾斜四元數(shù)$q_{tilt}=q_z$不繞$z$軸進(jìn)行旋轉(zhuǎn)，可視作如下的旋轉(zhuǎn)：
  
  $e_3$與目標(biāo)$z_b$組成平面$P$，旋轉(zhuǎn)軸為$P$的法向量$\mathbf{v}$，垂直向內(nèi)，由$e_3$方向繞$\mathbf{v}$順時(shí)針旋轉(zhuǎn)$\theta$得目標(biāo)$z_b$，無(wú)$yaw$方向的旋轉(zhuǎn)，故$q_z$中$z=0$

由$q_{\psi }$和$q_z$可求出總體旋轉(zhuǎn)四元數(shù)$q=q_z?q_{\psi }$，由$q$可唯一地確定旋轉(zhuǎn)矩陣$R$，具體操作可參考[11]。

角速度$\omega$可由$\dot{R}=R\hat{\omega }$求出。
同樣也可求出$\dot{z_b}$，其中$\mathcal{DN}(x)$表示歸一化函數(shù)的一階導(dǎo)數(shù)。

得到$\omega$的公式后，根據(jù)動(dòng)力學(xué)模型的歐拉公式，可以推導(dǎo)出torque的表達(dá)式，于是$x,u$中的變量
$r,v,R,\omega ,f,\tau$均求出，對(duì)應(yīng)以上PPT中的標(biāo)紅部分，也即flatness transformation ${\Psi }_x,{\Psi }_u$的表達(dá)式已經(jīng)求出。

利用differential flatness可以將帶有等式約束的優(yōu)化問(wèn)題轉(zhuǎn)換為沒(méi)有等式約束的優(yōu)化問(wèn)題(但仍然存在一些不等式約束)，極大提高優(yōu)化求解速度。

在控制方面也可以使用微分平坦特性，如反饋和前饋(cal desire，類(lèi)似串級(jí)控制的內(nèi)環(huán)輸出)控制中的微分平坦變換。如果task為跟蹤一條比較高維的軌跡$x,u$，而我們只能控制一個(gè)四維的$z$，可以輸入高維的$x,u$，使用flattness計(jì)算出低維的flat output，進(jìn)行控制，再使用${\Psi }_x,{\Psi }_u$計(jì)算出高維的實(shí)際輸出，只要輸出可導(dǎo)即可。該過(guò)程可以看做一個(gè)壓縮(flat output)和解壓(flattness transformation ${\Psi }_x,{\Psi }_u$)的過(guò)程，該過(guò)程是解析地進(jìn)行的，所以速度較快。

實(shí)際中，使用樣條來(lái)獲得flat output有以下優(yōu)點(diǎn)：

1. 容易確定是否可導(dǎo)；
2. 容易求出閉式導(dǎo)數(shù)，閉式表達(dá)式等；
3. 可以解耦進(jìn)行，每個(gè)維度上獨(dú)立進(jìn)行(如x維使用2階近似，y使用3維，z使用4維，$\psi$使用5維等)；
4. 計(jì)算的數(shù)值穩(wěn)定性已有較多研究。

不清楚的方面：如何使用如spline這樣的方法獲得flat output?

3. Trajectory Optimization軌跡優(yōu)化

3.1 Problem formulation

如上即為traj optimization general problem formulation.

1. 我們的優(yōu)化目標(biāo)函數(shù)是最小化優(yōu)化項(xiàng)的energy(加權(quán)平方積分)，使其更加平滑。函數(shù)是基于平坦輸出$z(t)$而非直接的$x,u$，具體而言，優(yōu)化$z^{(s)}(t)$，s取不同值時(shí)即優(yōu)化不同項(xiàng)，

• s=1速度，
• s=2加速度，
• s=3，jerk；
• s=4，snap
  有不同的效果，后面會(huì)講。
  優(yōu)化$z(t)$的高階導(dǎo)數(shù)的能量，使其更加平滑。
  不光優(yōu)化$z(t)$，還優(yōu)化時(shí)間$T$，如果不優(yōu)化時(shí)間，當(dāng)$T\to +\infty$時(shí)，energy可能趨近于0，所以需要加上一個(gè)正則項(xiàng)$\rho (t)$（如$\rho (t)=100T$），使得整體的優(yōu)化時(shí)間不至于過(guò)長(zhǎng)。

2. 其中$\mathcal{G}(z(t)?z^{(s)}(t))<0$是約束，不是基于$x,u$，而是基于flat output和flatness transformation ${\Psi }_x,{\Psi }_u$的約束表示。和前面提到的一樣，約束中的變量比較靈活，可以添加任何想要加入的變量（如控制時(shí)間，無(wú)人機(jī)傾角限制等）

3. $z(t)$需要在free space $\mathcal{F}$中，是collision-free的：由于一張地圖中start->goal的拓?fù)浣M合非常多，所以給出$\mathcal{F}$非常困難。

4. 具體問(wèn)題中會(huì)給出起始和末端的狀態(tài)約束。

優(yōu)化不同的量會(huì)有不同的效果：

• 如優(yōu)化jerk的energy可以最小化角速度，便于保持穩(wěn)定；優(yōu)化
• 優(yōu)化snap的energy可以最小化微分推力，節(jié)省能量。

3.2 Unconstrained case

學(xué)界中已經(jīng)探討出了最優(yōu)性充要條件，如約束不存在，$\rho (T)$中$T$已給定，約束也不存在。
下面講解一種Unconstrained case[12]，這也是本節(jié)課講師已經(jīng)發(fā)表的工作：

上圖為最優(yōu)性的條件，約束中沒(méi)有的約束，

• s=3時(shí)，最優(yōu)$z^{?}(t)$為2s-1=5次多項(xiàng)式
• 需滿(mǎn)足邊界和中間約束(3.3節(jié)會(huì)講解沒(méi)有中間約束，自己生成，而非給定的情況BIVP)
• d=1時(shí)，$z^{?}(t)$為2s-d-1=4階連續(xù)，即其0，1，2，3，4階導(dǎo)數(shù)均存在。
  將各階導(dǎo)數(shù)組成的等式約束聯(lián)立求解即可求出最優(yōu)解。

與[13]一樣，此處的思路仍然是先只考慮邊界約束，不考慮動(dòng)力學(xué)等其他約束來(lái)生成一系列primitives，然后再使用約束對(duì)生成的primitives進(jìn)行check，選出最優(yōu)的。

根據(jù)邊界條件聯(lián)立方程組，求出$x(t)$的系數(shù)，也就求出了$x(t)$的表達(dá)式。

可以對(duì)一段軌跡進(jìn)行segmentation，分別構(gòu)造方程$d=A_F(T)?c$，對(duì)系數(shù)矩陣$A_F(T)$求逆可求解每一個(gè)seg的BVP，

• 使得整個(gè)軌跡看起來(lái)更順滑，且軌跡會(huì)滿(mǎn)足各個(gè)seg求解時(shí)的boundary condition，
• 整段軌跡更傾向于勻速運(yùn)動(dòng)
• 對(duì)于短的seg，時(shí)間T較小，矩陣$A_F(T)$求逆可能存在數(shù)值不穩(wěn)定的問(wèn)題，在[14]中給出了$A_F(T{)}^{?1}$的解析形式(也是講師的工作)，可以offline算出解析解的其余部分，online時(shí)只帶入$t$的高階項(xiàng)即可。

如上所示即為$A_F(T{)}^{?1}$的解析形式[13]，可以使BVP的求解過(guò)程更快。
這樣的好處是對(duì)于primitives-check這樣框架的方法來(lái)說(shuō)，可以使得path的光滑逼近問(wèn)題計(jì)算地更快，效率更高(解多個(gè)BVP問(wèn)題更快)。

如上即為multi-segment traj的應(yīng)用[13]。

如何對(duì)生成的primitives進(jìn)行check？且要求是在連續(xù)空間而非離散空間中是feasiable的。
每個(gè)維度構(gòu)成一個(gè)safe flight corridor，整個(gè)primitives都需要在corridor中才算是feasiable的。

結(jié)論：把連續(xù)時(shí)間約束表示為多元多項(xiàng)式，使得多元多項(xiàng)式滿(mǎn)足連續(xù)時(shí)間約束。
如：

表示速度<1，類(lèi)似的，我們的目標(biāo)是把其他約束都表示為類(lèi)似的多元多項(xiàng)式，對(duì)多元多項(xiàng)式進(jìn)行check。

介紹了三種方法：

1. 離散時(shí)間采樣：優(yōu)點(diǎn)：靈活。缺點(diǎn)：低分辨率容易漏檢，高分辨率算的太慢。
2. 遞歸check[13]：只針對(duì)thrust/a/v的5次多項(xiàng)式的check，構(gòu)造一個(gè)上界，二次的最大值，有解析解，當(dāng)T->0時(shí)是最tight的upper bound，優(yōu)點(diǎn)：高效。缺點(diǎn)：僅適用于5次多項(xiàng)式check。
3. 極值check[15]：對(duì)目標(biāo)函數(shù)求導(dǎo)=0，求出極值，對(duì)極值進(jìn)行check。優(yōu)點(diǎn)：穩(wěn)定，對(duì)所有場(chǎng)景有效（要么有解析解，要么可以用數(shù)值解）。缺點(diǎn)：數(shù)值解需要進(jìn)行迭代，慢。

那么有沒(méi)有一種通用的解決多元多項(xiàng)式時(shí)間約束check的方法？[16]給出(高產(chǎn)！)

檢查
是否成立，轉(zhuǎn)換為在$t\in [0,T]$內(nèi)是否有根，如果有根，則式(2)必定不成立。

感覺(jué)這篇學(xué)術(shù)產(chǎn)出都是把問(wèn)題轉(zhuǎn)換為一個(gè)數(shù)學(xué)問(wèn)題，然后看在數(shù)學(xué)領(lǐng)域有沒(méi)有現(xiàn)成的比較成熟的求解方法，應(yīng)用到這里，就完成了一項(xiàng)工作。

關(guān)于在給定區(qū)間內(nèi)方程是否有根，在數(shù)學(xué)上有比較成熟的方法： Sturm Theorem

定義 Sturm Sequence，其中Rem()是求$\frac{a}{b}$的多項(xiàng)式除法的余項(xiàng)，同樣也是多項(xiàng)式。

定義$V(t)$為$[0,T]$之間的$g$值的正負(fù)跳變次數(shù)，計(jì)算方法是$V(t)=V(T)=V(0)$，上圖所示的算例為4次跳變。
先計(jì)算出Sturm Sequence的表達(dá)式，然后把左邊界(這里是-1)帶入各個(gè)sequence計(jì)算出統(tǒng)計(jì)符號(hào)變換次數(shù)，為4；右邊界帶入sequence計(jì)算出符號(hào)變化次數(shù)為0，則根的個(gè)數(shù)為4-0=4個(gè)，實(shí)際$G(t)$通過(guò)因式分解也能看出四個(gè)根分別為0,1,3,6。
特殊地，如果sequence值出現(xiàn)0，則直接刪除0繼續(xù)進(jìn)行判斷。

針對(duì)具體問(wèn)題，

1. 根據(jù)實(shí)際約束（如速度）構(gòu)建如上圖所示的不等式，
2. 把多項(xiàng)式P帶入，計(jì)算出不等式$\mathcal{G}<0$，
3. 將左右邊界帶入看是否滿(mǎn)足$\mathcal{G}<0$，若滿(mǎn)足，則
4. 求取$\mathcal{G}$的Sturm Sequence，利用上述方法判斷根的個(gè)數(shù)，若不為0，則不滿(mǎn)足約束；若為0，則滿(mǎn)足約束。

[16]中的方法既快速又穩(wěn)定，可以拓展到很多凸分解的約束checking。

3.3 Unconstrained Case: BIVP(Multi-segment with intermediate values)

這里討論的是更加General的問(wèn)題，

$p$的0階導(dǎo)為本身，一階導(dǎo)$v$，二階導(dǎo)$a$，三階導(dǎo)$jerk$，四階導(dǎo)$snap$，五階導(dǎo)$crackle$，六階導(dǎo)$pop$。

BIVP經(jīng)常被用于中間狀態(tài)只有p約束的規(guī)劃，這樣的解具有超出input階次的更高階的連續(xù)性[12]，如優(yōu)化jerk的energy，solution使得snap連續(xù)；優(yōu)化snap的energy，solution使得pop連續(xù)。

如3.2節(jié)所述，構(gòu)造$MC=b$這樣一個(gè)方程組，對(duì)$M$求逆即可解出最優(yōu)解的coeffieient。連cost function的形式都無(wú)需構(gòu)建。

由最優(yōu)性條件可知，最優(yōu)解一定是一個(gè)2s-1的多項(xiàng)式（cost function不一定是多項(xiàng)式），可以分段寫(xiě)出每一段的樣條（需要知道每一段的time durations）

通過(guò)線性條件（導(dǎo)數(shù)約束和連續(xù)性約束）構(gòu)建方程組進(jìn)行求解。（這塊聽(tīng)不太懂，需要看論文具體才行）

實(shí)際實(shí)現(xiàn)過(guò)程中，通常分別以每一個(gè)seg的開(kāi)始為0時(shí)刻，避免了系數(shù)矩陣中出現(xiàn)過(guò)大的值導(dǎo)致數(shù)值不穩(wěn)定的情況。

寫(xiě)出M之后會(huì)發(fā)現(xiàn)是一個(gè)稀疏的帶狀矩陣，可以使用稀疏求解器，如PLU分解來(lái)求逆，可以在線性的時(shí)間內(nèi)求完成$c$的求解。

另一種方法是多層次的BIVP，通常是前端的path planning+后端的BIVP
前端用于生成軌跡來(lái)獲取中間狀態(tài)，后端使用中間狀態(tài)構(gòu)建BIVP進(jìn)行求解，優(yōu)化solution。

核心思想：把低維的MP方法用在高維BIVP后端方法的關(guān)鍵點(diǎn)選取上。

前端方法比如：

1. 可以使用RRT*這種方法來(lái)global地求出一個(gè)solution，然后選取solution中的若干點(diǎn)作為中間狀態(tài)的p，在使用BIVP，
2. 使用DP算法求出關(guān)鍵點(diǎn)，然后BIVP。

“Douglas-Peucker算法”（也稱(chēng)為Ramer-Douglas-Peucker算法, 或簡(jiǎn)稱(chēng)DP算法）這是一個(gè)用于減少點(diǎn)數(shù)或簡(jiǎn)化路徑的算法，常用于軌跡簡(jiǎn)化或地圖繪制領(lǐng)域。

Douglas-Peucker算法會(huì)遞歸地選擇軌跡上的點(diǎn)，以在簡(jiǎn)化軌跡和保留軌跡形狀之間達(dá)到平衡。它通過(guò)設(shè)定一個(gè)容差值，然后找到與直線段起點(diǎn)和終點(diǎn)形成最大距離的點(diǎn)。如果這個(gè)距離大于容差值，那么這個(gè)點(diǎn)會(huì)被保留，軌跡會(huì)被分成兩部分，算法會(huì)遞歸地應(yīng)用到分割后的兩個(gè)軌跡上。如果距離小于容差值，則這個(gè)點(diǎn)可以被移除。通過(guò)這種方式，算法減少軌跡點(diǎn)數(shù)，同時(shí)盡可能保持軌跡的總體形狀。

如何保證BVIP的軌跡是collision-free的？

當(dāng)RRT* 發(fā)生碰撞時(shí)，嘗試把RRT*中間更多的waypoint加入作為關(guān)鍵點(diǎn)給BIVP(來(lái)自一幾年的IJRR的一篇論文，Bry IJRR 2015 adopted the unconstrained QP formulation)。

把RRT* 這種只能用于低維的motion planning的方法用在決定后端優(yōu)化的高維的BIVP的關(guān)鍵點(diǎn)選取上。

RRT*+BIVPs比Kinodynamic RRT*速度更快，質(zhì)量更高。

這種RRT*+BIVP的方法能夠計(jì)算地很快，但是存在一些問(wèn)題：

1. 為了使得最后的solution更優(yōu)，當(dāng)障礙物較多時(shí)，需要加入更多的waypoint，使得無(wú)人機(jī)很貼合這些拐角的waypoint才能保證飛行的安全，導(dǎo)致solution不那么順滑。
2. quality static準(zhǔn)靜態(tài)。當(dāng)無(wú)人機(jī)飛的足夠慢時(shí)，可以在任何位置懸停，可以以無(wú)限的精度逼近solution，但是這要求是res to res（起始和末尾v，a，高階導(dǎo)等都為0）。

這類(lèi)hierarchical的方法的一個(gè)研究方向是如何顯式地去優(yōu)化waypoint。

3.4 Constrained Case

考慮free space $\mathcal{F}$和動(dòng)力學(xué)約束${\mathcal{G}}_D(x,u)<0$約束，時(shí)間$T$，軌跡形狀等約束的問(wèn)題。

可以根據(jù)上述藍(lán)字部分的思路進(jìn)行簡(jiǎn)化：

1. 用樣條來(lái)進(jìn)行參數(shù)化
2. 固定$T$或者allocate大概的時(shí)間占比。
3. 固定一些基本的約束，而不管底層約束(角速度，推力等)
4. 可以提取Configuration space中的一部分，使得我們關(guān)心的問(wèn)題是locally convex，局部地解一些凸優(yōu)化問(wèn)題。

3.4.1 Constrained Case: Convex Simplification

如上所示為對(duì)costfunction進(jìn)行局部簡(jiǎn)化[17]，將cost function分成多段，約束分為各個(gè)凸多面體。

1. 相鄰兩段之間的連續(xù)性約束：
   
   如第1段末尾和第二段開(kāi)始的p是相等的。

2. boundary value condition，每一段的起始和末尾的狀態(tài)是指定的
   
   如第1段起始和末尾的pva都指定。需要注意，這里的中間狀態(tài)waypoint我們是不知道的，需要進(jìn)行優(yōu)化。

3. 安全約束（與BVP和BIVP最大的不同）
   
   $$A_i^T{\Phi }_i(t)\le b_i,i=0,?,N?1$$
   這里${\Phi }_i(t)$表示$t$時(shí)刻位置，保證飛行安全，可以在每段內(nèi)進(jìn)行采樣，check每個(gè)采樣點(diǎn)是否滿(mǎn)足安全約束，只要采樣足夠密，就能夠保證安全。最終，安全約束被轉(zhuǎn)換為QP問(wèn)題。

“QP”（Quadratic Programming，二次規(guī)劃），這是一類(lèi)特殊的數(shù)學(xué)優(yōu)化問(wèn)題。在二次規(guī)劃問(wèn)題中，目標(biāo)函數(shù)是一個(gè)二次函數(shù)，同時(shí)約束條件可以是等式和/或不等式，這些約束通常是線性的。

。二次規(guī)劃問(wèn)題可以表示為以下的標(biāo)準(zhǔn)形式：

目標(biāo)函數(shù):
$$\min \left(\frac{1}{2}{x}^{T}Qx+c^{T}x\right)$$

約束條件（不等式約束和等式約束）:
$$\begin{array}{r}\{\begin{array}{l}Ax\le b\\ Ex=d\end{array}\end{array}$$

其中，$x$是決策變量向量，$Q$ 是一個(gè)對(duì)稱(chēng)的二次項(xiàng)系數(shù)矩陣，$c$ 是線性項(xiàng)系數(shù)向量，$A$是不等式約束的系數(shù)矩陣，$b$ 是不等式約束的常數(shù)項(xiàng)向量，$E$ 是等式約束的系數(shù)矩陣，$d$ 是等式約束的常數(shù)項(xiàng)向量。

如果給定了控制時(shí)間T和被積函數(shù)的系數(shù)$p_j$，則可以把cost function解析地寫(xiě)出。

等式約束(即3.4.1的前兩個(gè)約束等式)：如果想讓軌跡經(jīng)過(guò)某個(gè)waypoint，則可構(gòu)建如上等式，將未知的系數(shù)$p_{j,i}$與系數(shù)矩陣分離開(kāi)，即可寫(xiě)出等式約束：
$$A_j{p}_j=d_j$$

上圖在每個(gè)seg使用了絕對(duì)時(shí)刻，如果使用相對(duì)時(shí)刻，則分別為$0$和$T$。

不等式約束也類(lèi)似，將待優(yōu)化的系數(shù)$p_{j,i}$提出，至此cost fucntion和所有的等式、不等式約束均表達(dá)出來(lái)，下面可以調(diào)用凸優(yōu)化求解器進(jìn)行求解。

針對(duì)凸函數(shù)Convex function，如果函數(shù)$f(x)$滿(mǎn)足
$$\begin{array}{r}f(\theta x+(1?\theta )y)\le \theta f(x)+(1?\theta )f(y)\end{array}$$
則$f(x)$是凸函數(shù)。（結(jié)合上左圖理解，兩點(diǎn)連線之間的函數(shù)值均在連線的下方）
如果嚴(yán)格滿(mǎn)足小于$<$，則是strictly convex。(當(dāng)$f(x)$為線性時(shí)，等號(hào)成立)

且如果該問(wèn)題的不等式約束是一個(gè)凸集合(convex set，集合中任意兩點(diǎn)連線均在集合內(nèi)部，則該集合為凸集合)，則該問(wèn)題是一個(gè)凸問(wèn)題。

如果還存在等式約束，則需要是線性等式約束，該問(wèn)題才能稱(chēng)之為凸問(wèn)題。

上圖左側(cè)是優(yōu)化問(wèn)題的standard form，右側(cè)是凸優(yōu)化問(wèn)題的standard form。

大多數(shù)問(wèn)題在formulate時(shí)是非凸的，reformulateing problem是一門(mén)藝術(shù)，沒(méi)有系統(tǒng)的方法…

針對(duì)凸優(yōu)化，下面介紹general的形式：

Disciplined convex optimization programs （“規(guī)范凸優(yōu)化范式”）：

①LP線性規(guī)劃：cost function，不等式約束，等式約束均為線性。

②對(duì)于二次規(guī)劃（QP）問(wèn)題，要求系數(shù)矩陣$P$半正定，才能稱(chēng)為凸問(wèn)題
分別假設(shè)，同時(shí)設(shè)$q=0,r=0$，則目標(biāo)函數(shù)分別為$\frac{1}{2}{x}^2$和$?\frac{1}{2}{x}^2$，前者為凸，而后者非凸，如下所示：

③QCQP(Quadratically Constrained Quadratic Programming，二次約束的二次規(guī)劃)：目標(biāo)函數(shù)是一個(gè)二
次函數(shù)，同時(shí)不等式約束條件也都是二次的（保證系數(shù)為半正定）。

④SOCP(Second-Order Cone Programming，二階錐規(guī)劃)：
目標(biāo)函數(shù)通常是線性的，約束條件可以包括線性約束、二次約束
主要是不等式約束有變化，舉例：$||x_2||\le x_1$
則圖像如下所示，三軸分別為$x_1,x_2(0),x_2(1)$，由于$x_1\ge 0$，所以在$x_1$正半軸，$x_2$模長(zhǎng)隨著$x_1$增大而增大，呈圓錐形(Second-Order Cone 或ice cream cone)，將圖像投影到$x_1{x}_2(1)$平面即為圓錐的投影。

SOCP是最general的凸優(yōu)化問(wèn)題形式，①②③均可轉(zhuǎn)換為SOCP，且二階錐約束使得SOCP能夠模擬一些特定的非線性關(guān)系，同時(shí)保持問(wèn)題的凸性質(zhì)，這意味著可以使用高效的算法找到全局最優(yōu)解。也即LP，QP，QCQP，SOCP均可找到全局最優(yōu)解。

給出幾種凸優(yōu)化問(wèn)題的solver：
CVX：http://cvxr.com/cvx/
Mosek：https://www.mosek.com/
OOQP：http://pages.cs.wisc.edu/~swright/ooqp/
GLPK：https://www.gnu.org/software/glpk/

在求解凸優(yōu)化問(wèn)題時(shí)可能會(huì)遇到數(shù)值不穩(wěn)定的問(wèn)題，可以使用一些Normalization的方法來(lái)提高數(shù)值穩(wěn)定性：

1. Time normalization：使用相對(duì)時(shí)間，并進(jìn)行歸一化，歸一化為0~1.0之間
2. Scale：如果實(shí)際數(shù)值非常大，可以進(jìn)行scale，求解一個(gè)數(shù)值較小的問(wèn)題，然后再re-scale回去（如$10^5$這個(gè)偏置去掉，求解0~100的問(wèn)題）。需要注意，有些問(wèn)題不是scale invarinat的(不同scale對(duì)于最終結(jié)果有影響)，有些是（scale對(duì)結(jié)果無(wú)影響）。

實(shí)際的一種生成軌跡的步驟：

1. 檢測(cè)障礙物；
2. 生成長(zhǎng)方體（一種特殊地凸多面體）的flight corridor；
3. 擴(kuò)展flight corridor，使得其重合區(qū)域更大，提升解的質(zhì)量；
4. 在flight corridor中生成feasiable的軌跡。

實(shí)現(xiàn)方式：
等式約束，不等式約束（安全邊界約束，速度、加速度最值約束）。

3.4.2 Issues of simplification

前面沒(méi)有關(guān)于時(shí)間$T$的優(yōu)化，但是timeallocation對(duì)于最終的solutioin影響很大，原因是如果加上時(shí)間約束，問(wèn)題通常是非凸的，無(wú)法求解。

使用flight corridor代替waypoint的好處是flight corridor的overlap部分可以提供更大的優(yōu)化空間，可能找到更優(yōu)的解。

一種時(shí)間分配的方式是按照加速-勻速-減速的方式分配，只能大概計(jì)算出相對(duì)的時(shí)間占比，但是速度大小無(wú)法確定，所以絕對(duì)時(shí)間$T$也無(wú)法確定。

3.4.3 Constrained Case: Spatial-Temporal Deformation (Brief)

本節(jié)簡(jiǎn)要討論更加Gneral的軌跡優(yōu)化，即考慮$\mathcal{F},\mathcal{G}$和time allocation。

$P,T$，優(yōu)化時(shí)間序列和空間序列：

1. 確定了P和T，通過(guò)解BIVP問(wèn)題可以唯一地確定corridor內(nèi)的軌跡，軌跡在時(shí)間上和空間上的形變打破來(lái)滿(mǎn)足約束，通過(guò)梯度實(shí)現(xiàn)，如軌跡超出corridor，向梯度內(nèi)拉伸使其滿(mǎn)足corridor約束
2. 可以顯著降低優(yōu)化問(wèn)題的維度。加速求解。

上圖所示是FAST-LAB提出的一項(xiàng)工作，計(jì)算速度很快，且解質(zhì)量較高。

同時(shí)給出一些多旋翼無(wú)人機(jī)軌跡優(yōu)化的參考論文[18~23]

通過(guò)flat transformation將施加在state上的約束變換為關(guān)于平坦輸出$z$及其高階導(dǎo)數(shù)的多項(xiàng)式約束，集合時(shí)空形變能夠優(yōu)化推力和角速度，使drone穿過(guò)很窄的縫。

本章講的較深，需要看論文、繼續(xù)研究才能理解。

4. Reference

[1] Bry et al., Aggressive flight of fixed-wing and quadrotor aircraft in dense indoor environments, IJRR 2015.

[2] Gao et al., Gradient-based online safe trajectory generation for quadrotor flight in complex environments, IROS 2017.

[3] Zhou et al., Ego-planner: An esdf-free gradient-based local planner for quadrotors. RAL 2020.

[4] Wang et al., Generating Large-Scale Trajectories Efficiently using Double Descriptions of Polynomials, ICRA 2021.

[5] Mellinger et al., Minimum Snap Trajectory Generation and Control for Quadrotors, ICRA 2011.

[6] Faessler et al., Differential Flatness of Quadrotor Dynamics Subject to Rotor Drag for Accurate Tracking of High-Speed Trajectories, RAL 2017.

[7] Ferrin et al., Differential Flatness Based Control of a Rotorcraft For Aggressive Maneuvers. IROS 2011.

[8] Mu et al., Trajectory Generation for Underactuated Multirotor Vehicles with Tilted Propellers via a Flatness-based Method, AIM 2019.

[9] Watterson et al., Control of Quadrotors Using the Hopf Fibration on SO(3), ISRR 2017.

[10] Lee et al., Geometric tracking control of a quadrotor UAV on SE(3), CDC 2010.

[11] Vince, Quaternions for Computer Graphics, Springer 2011.

[12] Wang et al., Geometrically Constrained Trajectory Optimization for Multicopters, TRO 2022.

[13] Mueller et al., A Computationally Efficient Motion Primitive for Quadrocopter Trajectory Generation, TRO 2015

[14] Wang et al., Generating Large-Scale Trajectories Efficiently using Double Descriptions of Polynomials, ICRA 2021.

[15] Burri et al., Real-Time Visual-Inertial Mapping, Re-localization and Planning Onboard MAVs in Unknown Environments, IROS 2015.

[16] Wang et al., Alternating Minimization Based Trajectory Generation for Quadrotor Aggressive Flight, RAL 2020.

[17] Liu et al., Planning Dynamically Feasible Trajectories for Quadrotors Using Safe Flight Corridors in 3-D Complex Environments, RAL 2017.

[18] Gao F, Wang L, Zhou B, Zhou X, Pan J and Shen S (2020) Teach-Repeat-Replan: A complete and robust system for aggressive flight in complex environments. IEEE Transactions on Robotics 36(5): 1526–1545.

[19] Deits R and Tedrake R (2015b) Efficient mixed-integer planning for UAVs in cluttered environments. In: IEEE International Conference on Robotics and Automation. Seattle, USA, pp. 42–49.

[20] Mellinger D and Kumar V (2011) Minimum snap trajectory generation and control for quadrotors. In: IEEE International Conference on Robotics and Automation. Shanghai, China, pp. 2520–2525.

[21] Tordesillas J, Lopez BT and How JP (2019) FASTER: Fast and safe trajectory planner for flights in unknown environments. In: IEEE International Conference on Intelligent Robots and Systems. Macau, China, pp. 1934–1940.

[22] Sun W, Tang G and Hauser K (2020) Fast UAV trajectory optimization using bilevel optimization with analytical gradients. In: American Control Conference. pp. 82–87.

[23] Patterson MA and Rao AV (2014) GPOPS-II: A MATLAB software for solving multiple-phase optimal control problems using hpadaptive gaussian quadrature collocation methods and sparse nonlinear programming. ACM Transactions on Mathematical Software 41(1): 1–37.文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-826677.html

到了這里，關(guān)于【深藍(lán)學(xué)院】移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃--第5章 最優(yōu)軌跡生成--筆記的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！