【论文阅读】TRO2022: A Two-Stage Optimization-Based Motion Planner for Safe Urban Driving

TRO2022: A Two-Stage Optimization-Based Motion Planner for Safe Urban Driving

Summary: 探讨planning过程中的优化问题求解，收敛的不确定性和求解的质量研究；转为混合证书整数优化问题作为非线性的初值，然后使得非线性问题能得到较高收敛率和较高效率
Type: TRO
Year: 2022
引用量: 5

参考与前言

论文链接：

A Two-Stage Optimization-Based Motion Planner for Safe Urban Driving

代码链接：无

b站讲解：【看完了 觉得很清晰 值得一看】

学术论文上B站：2022TRO-自动驾驶运动规划_哔哩哔哩_bilibili

本文相关非常细节的公式推导就不详细推了，因为坐标转换挺多的，主要就是指明一下各个部分的意义；整体说清晰吧 也很清晰，但是说复现吧 总觉得不太行 hhh，所以吴院说的对：不开源的都跳过 hhhh

CSDN 链接（以免相关发布地方有些乱码问题）：Kin-Zhang 论文阅读 CSDN
博客园链接：Kin-Zhang 论文阅读 cnblog

1. Motivation

因为本身运动规划问题的约束就是存在优先级差异的，比如安全性和运动能力肯定要大于舒适性和能耗之类的。所以对前者用的硬约束，后者则用软约束；两个阶段由 阶段一找到较好的初解，阶段二进行收敛到转换任务后的全局最优解

Contribution

论文写的非常简单，主要贡献点就在构建和如何求解

1. 提出了应该怎样构建motion planning问题，并使用two-stage进行优化求解
2. 实验证明此方法可以有高收敛率 和 lower cost solution

2. Method

第一阶段：将前面定义好的非线性问题 进行转换 成为混合整数优化问题，此部分用Gurobi 8.1进行求解

第二阶段：使用第一阶段给出的结果作为初值，解非线性优化问题，此部分用IPOPT进行求解

2.1 框架

2.2 motion synthesis

这一部分主要是构建完整的问题形式，介绍了参考线坐标系和时间坐标系之间的相互转换；相关公式简单解释一下

注意可能习惯了frenet坐标系会有些弄混，这里的大X, Y是世界坐标系，小x, y是参考线坐标系（其实我感觉就是frenet frame）

这是参考线下的正切和法向量，其中 $\lambda$ 是指从参考线的起点，车沿着参考线走的距离 其实就是图三左边的小x，只是因为泛化公式所以引入了 $\lambda \in [0, |\mathcal{P}_{ref}|]$，正切是相对于世界坐标系的，所以是大X, Y各自对参数的导数，类似于 $[\Delta X, \Delta Y]^{T}$ 这样的感觉，法向量是因为和正切是垂直的 也就是点乘为0，所以就可以得到法向量长这样了

$$\mathbf{t}_{\lambda}=\left[\begin{array}{l}\frac{\partial X^{\mathcal{P}_{\text {ref }}(\lambda)}}{\partial \lambda} \\\frac{\partial Y^{\mathcal{P}_{\text {ref }}(\lambda)}}{\partial \lambda}\end{array}\right], \quad \mathbf{n}_{\lambda}=\left[\begin{array}{c}\frac{-\partial Y^{\mathcal{P}_{\text {ref }}(\lambda)}}{\partial \lambda} \\\frac{\partial X^{\mathcal{P}_{\text {ref }}(\lambda)}}{\partial \lambda}\end{array}\right] \tag{1}$$

公式一的书写主要是为下面的坐标之间的转换做准备，即使用正切和法向量来转到 小x, y下

一共有三个转换：pose transform, speed transform, covariance tranform

pose 包含 $x, y, \phi$ 其中有些惊讶的是 x是通过约束求的，即找最小的 $\lambda$ 让点的欧式距离最小

$$x=\underset{\lambda}{\operatorname{argmin}}\left(X-X^{\mathcal{P}_{\text {ref }}}(\lambda)\right)^{2}+\left(Y-Y^{\mathcal{P}_{\text {ref }}}(\lambda)\right)^{2}$$

• 不过一般的x都是直接通过for循环找到的（不断step 递进），这里难道是放在约束问题里嘛？

其中y 则是 $\hat{\mathbf{y}}=\left[\begin{array}{c}X-X^{\mathcal{P}_{\text {ref }}}(x) \\Y-Y^{\mathcal{P}_{\text {ref }}}(x)\end{array}\right]$ 即 一个从参考线点指向车中心的向量，然后乘法向量就是参考线坐标系下的y啦，公式也就是：$y=\frac{1}{\left\|\mathbf{n}_{x}\right\|} \mathbf{n}_{x}^{\top} \cdot \hat{\mathbf{y}}$

关于covariance呢则是因为每个障碍物都是有pose和协方差的，也就是此文考虑了预测/检测存在的不确定性 并放入约束中；其中不确定性考虑是参考：论文[24] Safe nonlinear trajectory generation for parallel autonomy with a dynamic vehicle model 里的图，如下

整个问题的公式是这样的：

$$\begin{array}{cl}\underset{\mathbf{z}_{1: N}, \mathbf{u}_{0: N-1}}{\operatorname{argmin}} & J\left(\mathbf{z}_{0: N}, \mathbf{u}_{0: N-1}\right) \\\text { s.t. } & \forall k \in\{0, \ldots, N\}: \\& \mathbf{z}_{k+1}=f_{\Delta t}\left(\mathbf{z}_{k}, \mathbf{u}_{k}\right) \\& \mathcal{S}\left(\mathbf{z}_{k}\right) \cap \mathcal{B}_{\text {out }}=\emptyset \\& \mathcal{S}\left(\mathbf{z}_{k}\right) \cap\left[\bigcup_{i \in\{1, \ldots, n\}} \mathcal{S}^{i}\left(\mathbf{o}_{k}^{i}, \mathbf{\Sigma}_{k}^{i}, p_{\epsilon}\right)\right]=\emptyset\end{array}$$

其中 最小化的是cost funtion由下面部分进行详细定义 与车位置和control有关；约束解释：

1. 在任意的k从0,N是指N个离散step下
2. 是指从时间k的状态$\mathbf z_k = (x_k, y_k, \phi_k, v_k)$ 当输入$\mathbf u_k = (a_k, \delta_k)$ 时 即加速度和转向角度，变成的下一个状态$z_{k+1}$的dynamical system
3. 在状态所占据的空间面积 $S(\mathbf z_k) \subset \R^2$ 和不安全的面积area为空集，即两者面积不相交
4. 是根据上述论文[24] 里每一个障碍物都有一个不确定的包围圈，对于障碍物i，其概率协方差为$\Sigma_k^i$，预测占据面积概率大于 $p_{\epsilon}$ 也和自身的状态所占据的空间为空集，即不相交

2.3 MILP and Optimization

在2.2里面我们已经把问题定义清楚了，但是还缺乏各个部分的详细公式，这个部分主要就是简化相关的步骤，给出更为详细版的约束，构建的成为的是NLP（非线性问题）主要是以下四个部分，和求解部分一起看，更简洁点

1. kinematic vehicle model
2. driveable area collision
3. traffic participnats’ collision
4. multiobjective cost function over soft constraints

然后把问题就转成了problem 3 (Receding horizon MILP)，虽然有点长：

$$\begin{array}{ll}\underset{\overline{\mathbf{z}}_{m+1: m+K}, \overline{\mathbf{u}}_{m: m+K-1}}{\operatorname{argmin}} & \sum_{k=m}^{m+K} J_{\mathcal{C}}^{\mathcal{M}, k}\left(\overline{\mathbf{z}}_{k}, \overline{\mathbf{u}}_{k}\right) \\\text { s.t. } & \forall k \in\{m, \ldots, m+K\}: \\& \overline{\mathbf{z}}_{k+1}=F_{\Delta t}\left(\overline{\mathbf{z}}_{k}, \overline{\mathbf{u}}_{k}\right) \\& v^{x} \geq \rho\left|v^{y}\right| \\& a_{\min }^{x} \leq a_{k}^{x} \leq a_{\max }^{x} \\& a_{\min }^{y} \leq a_{k}^{y} \leq a_{\max }^{y} \\& \left|a_{k+1}^{x}-a_{k}^{x}\right|<\Delta a_{\max }^{x} \Delta t \\& \left|a_{k+1}^{y}-a_{k}^{y}\right|<\Delta a_{\max }^{y} \Delta t \\& v_{\min }^{x} \leq v_{k}^{x} \leq v_{\max }^{x} \\& v_{\min }^{y} \leq v_{k}^{y} \leq v_{\max }^{y} \\& d+b_{l}^{\mathcal{M}}\left(x_{k}\right) \geq y_{k} \geq b_{r}^{\mathcal{M}}\left(x_{k}\right)-d \\& y_{k, \max }^{i}-M \mu_{k}^{i} \leq y_{k}, i \in\{1, \ldots, n\}\end{array}$$

问题不大，下面一个个解释一下

---

1. 在k从m,m+K是指K个离散step下，应该满足一下约束

2. 首先是常见的自行车模型 → 但实际上在后面说明了，求解时是直接看做质点 emmm ，所以呢大概是这样的：

   

3. 然后 因为是质点 如果不加约束的话 那 v_y 速度过大就是横着走了 当然是不行的，所以呢 加了一个限制，其中 $\rho$为常数， 假设 $\phi_k\in[-\frac{\pi}{2},\frac{\pi}{2}]$ 也就是不考虑调头

   $$v^{x} \geq \rho\left|v^{y}\right| \tag{15}$$

   • 那为啥前面还写那么多自行车模型的 emmm 直接看后面这个部分，有没有前面铺垫丝毫不受影响

     因为还是需要非线性的部分的，是problem 2 会用非线性求解器求解，转成MILP是提前得到一个大致的初解！ 我悟了他的贡献点了

3-8 的约束全在加速度不能超过限制，速度不能超过限制，加加速度不能超限，就不过多解释了 emm 就是车都是有极限的 也不可能一脚油门 0.0001s就达到我们想要的300km/h的速度 这个意思

1. 正常道路的行驶，也就是自身不碰到路沿，其中 $b_{l}^{\mathcal{M}}$ 为左路沿，当然假设 $b_{l}^{\mathcal{M}} > b_{r}^{\mathcal{M}}$，其中d指的是车宽的，简单来讲虽然我们看做了一个质点，但是计算碰撞的时候 还是看做一个长方形，然后奔着最大的那个对角线去，$d=\sqrt{(w^2+l^2)}/2$

   $$d+b_{l}^{\mathcal{M}}\left(x_{k}\right) \geq y_{k} \geq b_{r}^{\mathcal{M}}\left(x_{k}\right)-d \tag{16}$$

2. 这个公式有些稍稍难解释，我们回到刚刚的图五，我再copy进来

   

   然后我们障碍物占据的空间，因为有协方差（所以是subset）其中 $\mathcal L$ 是指椭圆的公式哈 就是咱高中学的那种 $\frac{x^2}{a^2}+\frac{y^2}{b^2}=1$

   $$\mathcal{S}^{i} \subset \mathcal{L}\left(a_{\boldsymbol{\Sigma}_{k}^{i}}+a_{\text {shape }}, b_{\boldsymbol{\Sigma}_{k}^{i}}+b_{\text {shape }}\right)=\mathcal{L}\left(a_{k}^{i}, b_{k}^{i}\right)\tag{9}$$

   然后我们要找到椭圆边框离车辆长方形最近的地方， $d_x, d_y$ 为 车辆中心点 x,y 以车身长方形外扩的部分size

   $$\begin{aligned}x_{k, \min }^{i} &=\left[\min _{x} \mathcal{L}\left(a_{k}^{i}, b_{k}^{i}\right)\right]-d^{x} \\x_{k, \max }^{i} &=\left[\max _{x} \mathcal{L}\left(a_{k}^{i}, b_{k}^{i}\right)\right]+d^{x} \\y_{k, \min }^{i} &=\left[\min _{y} \mathcal{L}\left(a_{k}^{i}, b_{k}^{i}\right)\right]-d^{y} \\y_{k, \max }^{i} &=\left[\max _{y} \mathcal{L}\left(a_{k}^{i}, b_{k}^{i}\right)\right]+d^{y}\end{aligned} \tag{17}$$

   就是要满足，也就是说如果不在x范围内，他就一定不在y了，毕竟x是前后 如上图三坐标系

   $$\left(x_{k, \min }^{i} \leq x \leq x_{k, \max }^{i} \wedge y \geq y_{k, \min }^{i}\right) \Rightarrow y \geq y_{k, \max }^{i} \tag{18}$$

   然后 我们用大-M 法，去得到对应的混合整数约束，也就是当M无穷大时，公式18 可以转为 19

   $$\begin{aligned} y_{k, \max }^{i}-M \mu_{k}^{i} \leq y_{k} \text { where } & \\ \mu_{k}^{i}=\max \left(x_{k, \min }^{i}-x_{k}, 0\right)&+\max \left(x_{k}-x_{k, \max }^{i}, 0\right) \\ &+\max \left(y_{k, \min }^{i}-y_{k}, 0\right) \end{aligned} \tag{19}$$

最后则是我们的最小化对象 cost funtion：$J\left(\mathbf{z}_{0: N}, \mathbf{u}_{0: N-1}\right)=\sum_{k=0}^{N} \sum_{\iota \in \mathcal{I}} \omega_{\iota} \theta_{\iota}\left(\mathbf{z}_{k}, \mathbf{u}_{k}\right)$

其中是包含两个的,，其中w是权重哈

• 去向目标点的进度 也就是离目标点距离，比如目标点x纵向距离，速度是否达到，还有就是距离reference path的横向偏移：$\theta_x=(x-x_g)^2, \theta_v=(v-v_g)^2, \theta_y=y^2$
• 则是乘客舒适度由加速度和转角度平方来算，比如 $\theta_a=a^2, \theta_{\delta}=\delta^2$

但实际上 我们将其也转换到了MILP：$J_{\mathcal{C}}^{\mathcal{M}, k}(\overline{\mathbf{z}}, \overline{\mathbf{u}})=\sum_{\iota \in \mathcal{C}} \Omega_{\iota} \Theta_{\iota}(\overline{\mathbf{z}}, \overline{\mathbf{u}})$

其实吧 和上面的是差不多的，只是不是平方了 是绝对值…

• $\Theta_{x}=\left|x-x_{g}\right|, \Theta_{v}=\left|v-v_{g}\right|,\Theta_{y}=\left|y\right|$
• 后者则是仅加速度 $\Theta_{a}=\left|a_y\right|$

---

至此，我们介绍完了… 终于求解用的是 K-step receding horizon 滚动时域？ 也就是连续求解 N-K个子问题；

整体步骤再梳理一下：

1. 构建完整问题，是一个非线性的问题
2. 通过简化某些部分 比如车的模型，一些约束，来将问题转化为混合整数优化问题，然后通过连续求解 N-K 个子问题，得到初解，送到非线性问题里去
3. 然后非线性求解器拿到初解，进行求解

！啊 我悟了… 所以主要贡献其实就是MILP的转换及求解，这么一看 妙啊… emmm

3. 实验及结果

当然实验部分就是证明：哎 我很快，哎 我的收敛率很高！

4. Conclusion

我看到了最后发现 哦吼related work原来在最后 hhhh！！！主要就是说明：哎 我们是第一个提出用混合整数优化问题作为NLP solver的warm-start的！

好的… 他承认了 yielding a comparable total runtime to our method 还是一个问题，主要是930ms 做不到真车实验吧… 不过应该是one cycle planning 一个周期的时间吧… 930ms真的还是太长了

碎碎念

1. 关于实验设备没有细说，已发邮件询问

   仔细看的时候；发现作者竟然都没提实验平台的硬件设置是怎样，就直接给出时间 作为TRO其实有点虎了… 一般对比时间都会给出实验平台的设备以便大家都进行大约的转化，就像学习领域说我在一个1030上训练2天，另一个说我只需要2小时，然后其实他是在一个3090上只需要2小时 hhh

2. 时间对于planning这任务来说还是太长了，假设仅计算的一个周期，仅考虑planning的计算时间

   给ab发这篇文章后，ab直接质问 这930ms的Planning耗时，搁谁谁能受得了 hhh，然后问了问实车正常 CPU i7-8700 16G内存，是得控制在100-300ms的响应时间的

   再比如，jg做的GPIR 整体的运算周期也是在 10-100ms之间的

---

赠人点赞 手有余香 😆；正向回馈 才能更好开放记录 hhh