TDR-OBCA A Reliable Planner for Autonomous Driving in Free-Space Environment论文阅读记录
[TOC]
本文记录一下《TDR-OBCA: A Reliable Planner for Autonomous Driving in Free-Space Environment》论文的阅读与公式推导.
所的出来的模型为 Apollo open space planner 代码依据.
论文要点:
改进 H-OBCA(参见研读推导论文博客)模型:
- 2 个 warm start: Temporal and Dual variable warm starts.
- 改进模型, Reformulates the problem
PROBLEMS STATEMENT
自车状态向量
ego vehicle’s state vector:
$$
x(k) = [x_x(k), x_y(k), x_v(k), x_{\phi}(k)]^T \in \mathbb{R}^4
$$
x,y,v,yaw.
控制向量
control command:
$$
u(k) = [\delta(k), a(k)]^T \in \mathbb{R}^2
$$
打舵与刹车/油门
steering and brake/throttle
使用2轮自行车模型
kinematic bicycle model
$$
x_x(k+1) = x_v(k) \cos(x_{phi}) d_t + x_x(k) \\
x_y(k+1) = x_v(k) \sin(x_{phi}) d_t + x_y(k) \\
x_{phi}(k+1) = x_v(k) \frac{\tan(\delta(k))}{L} d_t + x_{phi}(k) \\
x_v(k+1) =a(k) d_t + x_v(k)
$$
L: wheelbase length
初始优化模型:
说明: $\boldsymbol{\lambda},\boldsymbol{\mu} \in \mathbb{R^{n \times m}}$ 为对偶变量. $h(x)$ 为对转角与加速度的约束以反映汽车动力学约束以及舒适度. $\textbf{G,g,R,t}$ 为描述自车形状以及位置. 障碍物用 $\textbf{A}_m,\textbf{b}_m$ 表示凸多边形(polygon),复杂的障碍物可以表示为多个 polygon 组合的形式. $\textbf{d}_{min}$ 表示对在第 $k$ 时刻第 $m$ 个障碍物的最小间距.
如果计算失败, 会有 fallback 的轨迹生成模块(源码中).
CORE ALGORITHMS
算法流程如下:
A. Temporal profile warm start
这个步骤的相关代码解读请参考Hybrid A*介绍博文, 得到 $\textbf{x}$ 在 $k$ 时刻的状态.
B. Dual variable warm start
对偶变量的 warm start. 即为在上述 temporal profile 的基础上优化 $\boldsymbol{\lambda}, \boldsymbol{\mu}$. 原优化模型的目标函数中没有 $\boldsymbol{\lambda}, \boldsymbol{\mu}$, 因此需要构造新的优化目标函数. 这里很自然地想到使用两个超平面距离(参见上篇博文)作为优化目标. 也就是引入 $\textbf{d}$ 松弛变量(the negative value of a safety distance between the ego vehicle and the $m-th$ obstacle polygon at time $k$)得到如下模型.
$$ \begin{aligned} \min _{\boldsymbol{\mu}, \boldsymbol{\lambda}, \boldsymbol{d}} \quad & \sum_{m=1}^{M} \sum_{k=1}^{K} d_{m}(k) \\\\ \text{s.t.} \quad &-g^{T} \mu_{m}(k)+\left(A_{m} t\left(\tilde{x}^{*}(k)\right)-b_{m}\right)^{T} \lambda_{m}(k)+d_{m}(k)=0 \\\\ &G^{T} \mu_{m}(k)+R\left(\tilde{x}^{*}(k)\right)^{T} A_{m}^{T} \lambda_{m}(k)=0 \\\\ &\left\|A_{m}^{T} \lambda_{m}(k)\right\|_{2} \leq 1, \\\\ &\lambda_{m}(k) \succeq 0, \mu_{m}(k) \succeq 0, d_{m}(k)<0, \\\\ &\text { for } k=1, \ldots K, m=1, \ldots, M \end{aligned} $$因为 $\textbf{d}$ 为负值因此越小越好为 min 问题. 其中 $\tilde{x}^{*}(k)$ 为temporal profile. 可见此问题的目标函数与约束均有 2 次函数, 因此为QCQP问题(二次约束二次规划问题). 不好解, 把$\left\|A_{m}^{T} \lambda_{m}(k)\right\|_{2} \leq 1$ 项从硬约束变成软约束, 转换成 QP 问题, 如下.
$$ \begin{aligned} \min _{\boldsymbol{\mu}, \boldsymbol{\lambda}, \boldsymbol{d}} &\quad \frac{1}{\beta} \sum_{m=1}^{M}\left\|A_{m}^{T} \lambda_{m}(k)\right\|_{2}^{2}+ \sum_{m=1}^{M} \sum_{k=1}^{K} d_{m}(k) \\\\ \text{s.t.} &-g^{T} \mu_{m}(k)+\left(A_{m} t\left(\tilde{x}^{*}(k)\right)-b_{m}\right)^{T} \lambda_{m}(k) +d_{m}(k)=0, \\\\ &G^{T} \mu_{m}(k)+R\left(\tilde{x}^{*}(k)\right)^{T} A_{m}^{T} \lambda_{m}(k)=0, \\\\ &\lambda_{m}(k) \succeq 0, \mu_{m}(k) \succeq 0, d_{m}(k)<0, \\\\ &\text { for } k=1, \ldots K, m=1, \ldots, M . \end{aligned} $$接下来论文用了一段很混乱的证明, 以我目前的水平看不明白, 至少逻辑语言看不明白.
证明出现这样的语句: $\text{if } A, B, C, D; \text{Otherwise }, E, F, G$.到底啥是条件啥是结果? Otherwise 包含 $\leq1$ 还是只包含 $=1$?(因为下面出现了声明如果 $<1$ 的情形).
即便我对 QCQP 模型进行一次拉格郎日对偶变换也得不到论文的结论.
这个问题我认为本来就跟拉格郎日对偶原理一样把硬约束换成软约束, 除非满足强对偶条件, 对偶问题的解只能提供原问题的参考下界($^{\star} p \leq ^{\star}d$),这里很明显不满足强对偶性.
为了求解性放宽约束是个思路, 咋非给搞成个数学证明出来, 有点无语.
C. MPC problem reformulation
对原来的优化目标函数有:
$$ \begin{aligned} &l(x(k), u(k-1)) = \alpha_{x}\|x(k)\|_{2}^{2}+\alpha_{x^{\prime}}\|x(k)-x(k-1)\|_{2}^{2} \\ &+\alpha_{u}\|u(k-1)\|_{2}^{2}+\alpha_{\tilde{u}}\|u(k-1)-\tilde{u}(k-1)\|_{2}^{2} \end{aligned} $$四项分别为位置/姿态的一阶二阶光滑, 操作的能量, 操作的光滑性.
因为不好求解, 实际中把末状态与松弛因子 $d$ 放入目标函数中, 因此得到最终的目标函数:
$$ \begin{aligned} &\mathcal{J}(\boldsymbol{x}, \boldsymbol{u}, \boldsymbol{d})=\sum_{k=1}^{K} l(x(k), u(k-1)) \\ &\quad+\alpha_{e}\left\|x(K)-x_{F}\right\|_{2}^{2}+\beta \sum_{m=1}^{M} \sum_{k=1}^{K} d_{m}(k) \end{aligned} $$最终的优化模型如下:
ps. 为啥这里的硬约束软约束化就不用数学证明了, 连同理都没标明…
仿真与实车测试结果
模块在平台中的定位如下图:
过程与结果略过.
总结
相对于原型的 H-OBCA 优化模型工程化了很多.
接下来的工作是把模型与代码对应起来.
TDR-OBCA A Reliable Planner for Autonomous Driving in Free-Space Environment论文阅读记录