Efficient Mixed-Integer Planning for UAVs in Cluttered Environments

1 混合整数规划（Mixed Integer Programing, MIP）

1、优化三要素：决策变量、约束条件、目标函数。根据三要素的不同，可以分为不同的类型。

2、在 混合整数规划（MIP） 问题中，一部分决策变量是连续的（可取任何实数值），而另一部分决策变量是离散的（只能取整数值）。

  • 混合整数二次规划（MIQP） 问题：具有二次目标但没有二次约束 。
  • 混合整数二次约束规划（MIQCP） 问题：具有二次约束 。
  • 混合整数线性规划（MILP） 问题：没有任何二次特征。

3、0-1整数规划：离散决策变量的取值为0或1。

2 论文动机

1、对于避障问题，通常无障碍的集合是非凸的，因此可将无障碍的非凸可行集建模为有限个凸可行集的并集，再应用混合整数凸优化。

2、之前的工作对于避障样本点的选取可能导致：

  • 路径与障碍物的拐角碰撞
  • 穿过非常薄的障碍物

虽然可以通过增加采样点数来减缓，但同时也增加了优化问题的复杂度。

error

3、对于分段线性轨迹，采用以下规则约束可以创建完全无碰撞的轨迹：

  • 两个端点必须包含在同一个凸安全区域内
  • 轨迹段之间的端点出现在两个凸集的交集中

3 论文方法

轨迹规划问题包含三个部分：
（1）生成凸安全区域（预处理阶段）
（2）将轨迹段分配给这些凸区域（混合整数凸优化求解）
（3）优化轨迹，同时确保它们完全保持在安全区域内。（混合整数凸优化求解）

3.1 凸安全区域的生成

Deits（作者）之前的工作：IRIS (Iterative Regional Inflation by Semidefinite programming)

3.2 轨迹段的分配

通过一个二进制整数变量矩阵H∈{0,1}R×NH\in\{0,1\}^{R\times N}对轨迹的每个多项式段和安全区域进行分配，多项式轨迹段为Pj(t)P_j(t)，凸安全区域为GrG_r。

Hr,j⟹Pj(t)∈Gr∀t∈[0,1](3.1)H_{r,j}\Longrightarrow P_j(t)\in G_r\quad\forall t\in[0,1]\tag{3.1}

可以通过下式 3.23.2 来确保多项式轨迹 jj 无碰撞。

∑r=1RHr,j=1(3.2)\sum_{r=1}^RH_{r,j}=1\tag{3.2}

3.3 多面体约束

将$n$ 维轨迹表示为单个变量 $t$ 的 $d$ 次分段多项式，

Pj(t)=∑k=1d+1Cj,kΦk(t)t∈[0,1](3.3)P_j(t)=\sum_{k=1}^{d+1}C_{j,k}\Phi_k(t)\quad t\in[0,1]\tag{3.3}

其中，Cj,k∈RnC_{j,k}\in\mathbb{R}^n为多项式系数，Φk(t)\Phi_k(t) 为多项式基函数。
对于每一个 mm面 凸多面体，如果Hr,j=1H_{r,j}=1，则有

ArPj(t)≤br(3.4)A_rP_j(t)\leq b_r\tag{3.4}

其中，Ar∈Rm×n,br∈RmA_r\in\mathbb{R}^{m\times n},b_r\in\mathbb{R}^m

Ar∑k=1d+1Cj,kΦk(t)≤br∀t∈[0,1](3.5)A_r\sum_{k=1}^{d+1}C_{j,k}\Phi_k(t)\leq b_r\quad\forall t\in[0,1] \tag{3.5}

上式包含 mm 个以下形式的约束

ar,ℓ⊤∑k=1d+1Cj,kΦk(t)≤br,ℓ∀t∈[0,1](3.6)a_{r,\ell}^\top\sum_{k=1}^{d+1}C_{j,k}\Phi_k(t)\leq b_{r,\ell}\quad\forall t\in[0,1]\tag{3.6}

Ar=[ar,1⊤ar,2⊤⋮ar,m⊤],b=[br,1br,2⋮br,m](3.7)A_r=\begin{bmatrix}a_{r,1}^\top\\a_{r,2}^\top\\\vdots\\a_{r,m}^\top\end{bmatrix},b=\begin{bmatrix}b_{r,1}\\b_{r,2}\\\vdots\\b_{r,m}\end{bmatrix}\tag{3.7}

约束等价于

q(t):=br,ℓ−∑k=1d+1(ar,ℓ⊤Cj,k)Φk(t)≥0∀t∈[0,1](3.8)q(t):=b_{r,\ell}-\sum_{k=1}^{d+1}(a_{r,\ell}^\top C_{j,k})\Phi_k(t)\geq0\quad\forall t\in[0,1] \tag{3.8}

上述约束满足当且仅当

q(t)={tσ1(t)+(1−t)σ2(t)if d is oddσ1(t)+t(1−t)σ2(t)if d is evenσ1(t),σ2(t) are sums of squares(3.9)q(t)=\begin{cases}t\sigma_1(t)+(1-t)\sigma_2(t)&\mathrm{if~}d\mathrm{~is~odd}\\\sigma_1(t)+t(1-t)\sigma_2(t)&\mathrm{if~}d\mathrm{~is~even}&\end{cases}\\\sigma_1(t),\sigma_2(t)\text{ are sums of squares}\tag{3.9}

其中，dd 为奇数时，σ1(t)\sigma_1(t) 和 σ2(t)\sigma_2(t) 为 d−1d-1 次多项式，如果为偶数，则为 dd 次多项式和 d−2d−2次多项式。σ1(t)\sigma_1(t) 和 σ2(t)\sigma_2(t)为平方和的条件要求每个都可以分解为平方项之和，这是单变量多项式非负的充要条件。

  • 当多项式阶次为1时，σ1(t)\sigma_1(t) 和 σ2(t)\sigma_2(t)为常数，问题为一个混合整数二次规划 (MIQP)问题。
  • 当多项式阶次为3时，

σi(t)=β1+β2t+β3t2=[1t][β1β22β22β3][1t](3.10)\sigma_i(t)=\beta_1+\beta_2t+\beta_3t^2=\begin{bmatrix}1&t\end{bmatrix}\begin{bmatrix}\beta_1&\frac{\beta_2}{2}\\\frac{\beta_2}{2}&\beta_3\end{bmatrix}\begin{bmatrix}1\\t\end{bmatrix} \tag{3.10}

等价于矩阵 3.113.11 半正定

[β1β22β22β3]⪰0(3.11)\begin{bmatrix}\beta_1&\frac{\beta_2}{2}\\\frac{\beta_2}{2}&\beta_3\end{bmatrix}\succeq0\tag{3.11}

等价于旋转二次锥约束

β22−4β1β3≤0β1,β3≥0(3.12)\begin{aligned}\beta_2^2-4\beta_1\beta_3&\leq0\\\beta_1,\beta_3&\geq0\end{aligned}\tag{3.12}

补充知识:
nn维二次锥约束：

Qn={x∈Rn∣x1≥x22+x32+⋯+xn2}\mathcal{Q}^n=\{x\in\mathbb{R}^n\mid x_1\geq\sqrt{x_2^2+x_3^2+\cdots+x_n^2}\}

nn维旋转二次锥约束：

Qrn={x∈Rn∣2x1x2≥x32+⋯+xn2;x1,x2≥0}\mathcal{Q}_r^n=\{x\in\mathbb{R}^n\mid2x_1x_2\geq x_3^2+\cdots+x_n^2;x_1,x_2\geq0\}

可以将 3 次多项式凸安全区域分配及约束问题写为混合整数二阶锥问题 (MISOCP)。

目标函数的选择

为了将问题简化为MISOCP问题，采用三阶多项式并且构建最小化jerk问题。

minimize ∑j=1N∥d3dt3Pj(t)∥2(3.13)\mathrm{minimize~}\sum_{j=1}^N\left\|\frac{d^3}{dt^3}P_j(t)\right\|^2\tag{3.13}

低阶轨迹的处理

由于Mellinger将轨迹的snap与无人机的推力相关联，因此针对三阶多项式轨迹，其snap为脉冲函数，显然不合理。因此，本文先采取三阶多项式求解轨迹和安全区域的分配问题，再采用高阶多项式进行优化。

问题构建

minimize⁡⁡P,H,σ∑j=1N∥d3dt3Pj(t)∥2\underset{P,H,\sigma}{\operatorname*{\operatorname*{minimize}}}\sum_{j=1}^N\left\|\frac{d^3}{dt^3}P_j(t)\right\|^2

subject to:P1(0)=X0,P˙1(0)=X˙0,P¨1(0)=X¨0PN(1)=Xf,P˙N(1)=X˙f,P¨N(1)=X¨fPj(1)=Pj+1(0),P˙j(1)=P˙j+1(0),P¨j(1)=P¨j+1(0)\begin{aligned}&\text{subject to:}\\&P_1(0)=X_0,&&\dot{P}_1(0)=\dot{X}_0,&&\ddot{P}_1(0)=\ddot{X}_0\\&P_N(1)=X_f,&&\dot{P}_N(1)=\dot{X}_f,&&\ddot{P}_N(1)=\ddot{X}_f\\&P_j(1)=P_{j+1}(0),&&\dot{P}_j(1)=\dot{P}_{j+1}(0),&&\ddot{P}_j(1)=\ddot{P}_{j+1}(0)\end{aligned}

Hr,j⟹br,ℓ−ar,ℓ⊤Pj(j)=tσℓ,j,1(t)+(1−t)σℓ,j,2(t)∀j∈{1,…,N}∀r∈{1,…,R}where σℓ,j,1(t),σℓ,j,2(t) are sums of squares\begin{aligned} H_{r,j}&\Longrightarrow b_{r,\ell}-a_{r,\ell}^\top P_j(j)=t\sigma_{\ell,j,1}(t)+(1-t)\sigma_{\ell,j,2}(t)\\ &\forall j\in\{1,\ldots,N\}\quad\forall r\in\{1,\ldots,R\}\\ &\mathrm{where~}\sigma_{\ell,j,1}(t),\sigma_{\ell,j,2}(t)\text{ are sums of squares}\end{aligned}

∑r=1RHr,j=1∀j∈{1,…,N}Hr,j∈{0,1}\begin{aligned}&\sum_{r=1}^RH_{r,j}=1\quad\forall j\in\{1,\ldots,N\}\\&H_{r,j}\in\{0,1\}\end{aligned}

障碍物面约束轨迹

将凸多面体的mm 个面线性约束替换为障碍物 oo 的rr 个面线性约束，因此对于每一个障碍物

∑r=1NfacesHo,r,j=1∀j∈{1,…,N}(3.13)\sum_{r=1}^{N_\mathrm{faces}}H_{o,r,j}=1\quad\forall j\in\{1,\ldots,N\}\tag{3.13}