Optimization-Based Collision Avoidance 论文阅读推导记录

[TOC]
本文记录一下《Optimization-Based Collision Avoidance》的阅读与公式推导.
此论文是 Apollo open space planner 中的模型原型.

论文要点:

1. 将控制体与障碍物描述成凸集(convex sets such as polytopes or ellipsoids (or can be decomposed into a finite union of such convex sets)), 然后通过拉格朗日对偶变换成相应的凸优化形式.
2. 即便在计算失败碰撞时也要尽可能地侵入过少, 引入 signed distance 概念.
3. 仿真验证在无人机飞行避障/汽车倒车环境有效(使用 A* 搜索作为 warm start).

Problem Description

dynamics of the controlled object

$$
x_{k+1} = f(x_k, u_k)
$$

$f : \mathbb{R}^{n^x} × \mathbb{R}^{n^u} \rightarrow \mathbb{R}^{n^x}$

$k$ 表示时刻.
状态: $x_k\in \mathbb{R}^{n^x}$, 控制: $u_k\in \mathbb{R}^{n^u}$.
状态可以包括位置 $p_k \in \mathbb{R}^n$, 航向角 $\theta_k \in \mathbb{R}^n$.

state constraints

$$
h(x_k, u_k) ≤ 0
$$

$h : \mathbb{R}^{n^x} × \mathbb{R}^{n^u} \rightarrow \mathbb{R}^{n^h}$

可以用来表达 vehicle dynamic limitations and driving comforts.

objective function

$$
J =\sum_{k=0}^Nl(x_k, u_k)
$$
$l : \mathbb{R}^{n^x} × \mathbb{R}^{n^u} \rightarrow \mathbb{R}$

假定所有函数均 2 阶可微(目标函数肯定要设置成 $l_2$ 范式, 约束是凸多边形(线)/椭圆形, 都 2 阶可微).

其他 notation

自车

可以表示为一个 $\mathbb{B}$ 多边形(超平面交集), $k$ 时刻考虑位移 $t(x_k)$ 与姿态旋转 ${R}(x_k)$ 变换可表达如下:

$$
\mathbb{E}(x_k) ⊂ \mathbb{R}^n \
\mathbb{E}(x_k) = R(x_k)\mathbb{B} + t(x_k) \
\mathbb{B} := {y: Gy \succeq_{\mathcal{K}^{-}}g}
$$

这里省略把自车自车看作点(只适用于球状的被控体, 倒车工况下不适合否则连正常的库位都会认为无法泊入)的推导.

障碍物(m 个)

$$ \mathbb{O}^{(m)} ⊂ \mathbb{R}^n \\ \mathbb{O}^{(m)}=\left\{y \in \mathbb{\mathbb{R}}^{n}: A^{(m)} y \preceq_{\mathcal{K}} b^{(m)}\right\} $$ $$ A^{(m)} \in \mathbb{R}^{l \times n}, b^{(m)} \in \mathbb{R}^{l}, \text { and } \mathcal{K} \subset \mathbb{R}^{l} $$

$\mathcal{K}$ 是一个闭凸非内空的尖锥(性质非常好的锥, 如果把障碍物描述为矩形, $l=4$ 或者 1, 这里广义不等式可以被element-wise inequality $≤$ 常规不等式代替).

不碰撞表达式

$$
\mathbb{E}\left(x_{k}\right) \cap \mathbb{O}^{(m)}=\emptyset, \quad \forall m=1, \ldots, M
$$

拉格朗日对偶变换

其中不碰撞表达式需要进一步具体化, 并且要可微可求解. 考虑通过拉格朗日对偶变换成更好的形式.

把障碍物描述成多边形/椭圆形的原始问题如下:

$$ \operatorname{dist}(\mathbb{E}(x), \mathbb{O})=\min _{e, o}\left\{\|e-o\|: A o \preceq_{\mathcal{K}} b, e \in \mathbb{E}(x)\right\}=\min _{e^{\prime}, o}\left\{\left\|R(x) e^{\prime}+t(x)-o\right\|: A o \preceq \mathcal{K} .b, G e^{\prime} \preceq_{\overline{\mathcal{K}}} g\right\} $$

可构造问题如下:

$$
\begin{aligned}
\min_{e’,o} \qquad &||R(x)e’+t(x)-o|| \\
\text{s.t.} \qquad &Ao \leq b \\
\qquad &Ge’ \leq g
\end{aligned}
$$

其中 $e=\mathbb{B}(e’)$.

把目标函数中的变量替换出来:

$$
\begin{aligned}
\min_{e’,o} \qquad &||w|| \\
\text{s.t.} \qquad &Ao \leq b \\
\qquad &Ge’ \leq g \\
\qquad &R(x)e’+t(x)-o = w
\end{aligned}
$$

可得其对偶的拉格朗日函数:

$$
g(\lambda,z,\mu) =
\inf_{o,e’,w} \Big(||w|| +
\lambda^{T} (Ao-b) +
\mu^{T} (Ge - g) +
z^{T}(Re+t-o-w)
\Big) \\
= \inf_{o} ( \lambda Ao - z^T) +
\inf_{e’} (\mu Ge + z^TRe) +
\inf_{w}( ||w|| - z^Tw)
$$

若使 $g(\lambda,z,\mu)$ 有下确界, 可得下面三个式子成立:

$$
||z||_{*} \leq 1 \\
A^T \lambda = z \\
G^T \mu = z
$$

满足上面三个式子后的下确界为:

$$
-g^T \mu + (At(x)-b)^T \lambda
$$

从几何意义上来看即为 2 个超平面 $dist(X_1,X_2)$ 之间的距离, 类似于

$$
A_1 x_1 - A_2 x_2 \geq d_{min}
$$

可得对应的拉格朗日对偶问题:

$$ \begin{aligned} \max_{\mu,\lambda} \qquad &-g^T \mu + (At(x)-b)^T \lambda \\\\ \text{s.t.} \qquad &G^T + R^TA^T\lambda = 0 \\\\ & ||A^T \lambda ||_* \leq 1 \\\\ &\lambda \succeq_{\mathcal{K}^{*}} 0 \\\\ & \mu \succeq_{\mathcal{K}^{*}} 0 \end{aligned} $$

由于 $O,e’$ 都是非空内实的凸集, 因此此对偶为强对偶, 即对偶问题的最优解 ${}^\star p = {}^\star d$.

将碰撞约束转化成对偶形式后的优化形式如下:

$$ \begin{array}{ll} \min _{\mathbf{x}, \mathbf{u}, \boldsymbol{\lambda}, \boldsymbol{\mu}} & \sum_{k=0}^{N} \ell\left(x_{k}, u_{k}\right) \\\\ \text { s.t. } \quad & x_{0}=x(0), x_{N+1}=x_{F}, \\\\ & x_{k+1}=f\left(x_{k}, u_{k}\right), h\left(x_{k}, u_{k}\right) \leq 0 \\\\ & -g^{\top} \mu_{k}^{(m)}+\left(A^{(m)} t\left(x_{k}\right)-b^{(m)}\right)^{\top} \lambda_{k}^{(m)}>0 \\\\ & G^{\top} \mu_{k}^{(m)}+R\left(x_{k}\right)^{\top} A^{(m)^{\top}} \lambda_{k}^{(m)}=0 \\\\ & \left\|A^{(m)^{\top}} \lambda_{k}^{(m)}\right\|_{*} \leq 1, \lambda_{k}^{(m)} \succeq_{\mathcal{K}^{*}} 0, \mu_{k}^{(m)} \succeq_{\overline{\mathcal{K}}^{*}} 0, \\\\ & \text { for } k=0, \ldots, N, m=1, \ldots, M \end{array} $$

因为范数 $|| \quad ||_*$ 为 $l_2$ 范数, 再加上障碍物的描述(多边形/椭圆形)因此所有的约束都可 2 阶可微. 可以使用 IPOPT 求解器求解了.

4.Singed distance

但是这个优化出来的轨迹一旦算不出来 collision-free 的轨迹就有可能碰撞后侵入很深. 因此作者又规定了一个侵入的距离约束, 与防止碰撞的约束组合成 signed distance=$\operatorname{sd}(\mathbb{E}(x), \mathbb{O})$的约束.

$$
\operatorname{pen}(\mathbb{E}(x), \mathbb{O}):=\min _{t}{|t|:(\mathbb{E}(x)+t) \cap \mathbb{O}=\emptyset} \\
\operatorname{sd}(\mathbb{E}(x), \mathbb{O}):= \operatorname{dist}(\mathbb{E}(x), \mathbb{O})- \operatorname{pen}(\mathbb{E}(x), \mathbb{O})
$$

对 $\operatorname{pen}(\mathbb{E}(x), \mathbb{O})$ 同样地进行拉格朗日对偶变换得到:

$$ \operatorname{pen}(\mathbb{E}(x), \mathbb{O})= \min _{e, o}\left\{\|e-o\|: A o \preceq_{\mathcal{K}} b, e \in \mathbb{E}(x)\right\}=\\\\ \max _{\lambda, \mu}\left\{g^{\top} \mu+(-A t(x)+b)^{\top} \lambda : G^{\top} \mu+R(x)^{\top} A^{\top} \lambda=0, \left\|A^{\top} \lambda\right\|_{*} \leq 1, \lambda \succeq_{\mathcal{K}^{*}} 0, \mu \succeq_{\overline{\mathcal{K}}^{\star}} 0\right\} \\\\ < p_{max} $$

可统一 $\operatorname{dist}(\mathbb{E}(x), \mathbb{O})$与$\operatorname{pen}(\mathbb{E}(x), \mathbb{O})$ 为 $\operatorname{sd}(\mathbb{E}(x), \mathbb{O})$:

$$ \operatorname{sd}(\mathbb{E}(x), \mathbb{O})= \max _{\lambda, \mu}\left\{-g^{\top} \mu+(A t(x)-b)^{\top} \lambda : G^{\top} \mu+R(x)^{\top} A^{\top} \lambda=0,\left\|A^{\top} \lambda\right\|_{*} \leq 1, \lambda \succeq_{\mathcal{K}^{*}} 0, \mu \succeq_{\overline{\mathcal{K}}^{\star}} 0\right\} \\\\ {>} s_{min} $$

详细的统一推导过程可参见论文结尾的 Appendix. 具体的思路是同样引入 $z:||z||_*=1$ 情况下换成 min-max 问题. 这里不再重复.

5.最终优化模型

最后把 $-g^{\top} \mu+(A t(x)-b)^{\top} \lambda > s$ 这一硬约束转换为软约束, 引入松弛变量 $\textbf s$, 可得最终的优化模型:

$$ \begin{aligned} \min _{\mathbf{x}, \mathbf{u}, \mathbf{s}, \boldsymbol{\lambda}, \boldsymbol{\mu}} & \sum_{k=0}^{N}\left[\ell\left(x_{k}, u_{k}\right)+\kappa \cdot \sum_{m=1}^{M} s_{k}^{(m)}\right] \\ \text { s.t. } \quad & x_{0}=x_{S}, x_{N+1}=x_{F}, \\ & x_{k+1}=f\left(x_{k}, u_{k}\right), h\left(x_{k}, u_{k}\right) \leq 0 \\ &-g^{\top} \mu_{k}^{(m)}+\left(A^{(m)} t\left(x_{k}\right)-b^{(m)}\right)^{\top} \lambda_{k}^{(m)}>-s_{k}^{(m)}, \\ & G^{\top} \mu_{k}^{(m)}+R\left(x_{k}\right)^{\top} A^{(m)^{\top}} \lambda_{k}^{(m)}=0 \\ &\left\|A^{(m)^{\top}} \lambda_{k}^{(m)}\right\|_{*}=1 \\ & s_{k}^{(m)} \geq 0, \lambda_{k}^{(m)} \succeq_{\mathcal{K}^{*}} 0, \mu_{k}^{(m)} \succeq_{\bar{K}^{*}} 0 \\ & \text { for } k=0, \ldots, N, m=1, \ldots, M \end{aligned} $$

其中 $\lambda_k^{(m)},\mu_k^{(m)}$ 是 the dual variables associated with the obstacle $\mathbb{O}^{(m)}$ at step $k$.
$s_k^{(m)} \in \mathbb{R}_+$是slack variable associated with the obstacle $\mathbb{O}^{(m)}$ at step $k$.
广义不等式里的锥要么为标准锥, 要么为二阶锥.

6.Choice of Initial Guess

为了找到较好的 local optima, 初始值的 guess 很重要, 本文作者选用的 A*/Hybrid A* 搜索出来的结果作为 warm starting point.

Simulation and varification

略过. 作者还提供了基于 Julia 的源码支持.

总结

这篇论文抽象了一个基于避障部分对偶化的最优化模型.
Apollo 的 open space planner 基于这个模型进行了进一步具体化与改进, 即 TDR-OBCA.
具体会在下一篇博文中分析.