【论文阅读】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],{\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]\text{\hspace{1em}(1)}$$

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

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

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

$$x=\underset{\lambda }{\mathrm{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}{\Vert {\mathbf{n}}_x\Vert }{\mathbf{n}}_x^{\top }\cdot \hat{\mathbf{y}}$

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

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

argmin ⁡ z 1 : N , u 0 : N − 1 J ( z 0 : N , u 0 : N − 1 )  s.t.  ∀ k ∈ { 0 , … , N } : z k + 1 = f Δ t ( z k , u k ) S ( z k ) ∩ B out  = ∅ S ( z k ) ∩ [ ⋃ i ∈ { 1 , … , n } S i ( o k i , Σ k i , p ϵ ) ] = ∅ \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} z1:N​,u0:N−1​argmin​ s.t. ​J(z0:N​,u0:N−1​)∀k∈{0,…,N}:zk+1​=fΔt​(zk​,uk​)S(zk​)∩Bout ​=∅S(zk​)∩[⋃i∈{1,…,n}​Si(oki​,Σki​,pϵ​)]=∅​

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

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

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)，虽然有点长：

argmin ⁡ z ‾ m + 1 : m + K , u ‾ m : m + K − 1 ∑ k = m m + K J C M , k ( z ‾ k , u ‾ k )  s.t.  ∀ k ∈ { m , … , m + K } : z ‾ k + 1 = F Δ t ( z ‾ k , u ‾ k ) v x ≥ ρ ∣ v y ∣ a min ⁡ x ≤ a k x ≤ a max ⁡ x a min ⁡ y ≤ a k y ≤ a max ⁡ y ∣ a k + 1 x − a k x ∣ < Δ a max ⁡ x Δ t ∣ a k + 1 y − a k y ∣ < Δ a max ⁡ y Δ t v min ⁡ x ≤ v k x ≤ v max ⁡ x v min ⁡ y ≤ v k y ≤ v max ⁡ y d + b l M ( x k ) ≥ y k ≥ b r M ( x k ) − d y k , max ⁡ i − M μ k i ≤ y k , i ∈ { 1 , … , n } \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} zm+1:m+K​,um:m+K−1​argmin​ s.t. ​∑k=mm+K​JCM,k​(zk​,uk​)∀k∈{m,…,m+K}:zk+1​=FΔt​(zk​,uk​)vx≥ρ∣vy∣aminx​≤akx​≤amaxx​aminy​≤aky​≤amaxy​∣∣​ak+1x​−akx​∣∣​<Δamaxx​Δt∣∣​ak+1y​−aky​∣∣​<Δamaxy​Δtvminx​≤vkx​≤vmaxx​vminy​≤vky​≤vmaxy​d+blM​(xk​)≥yk​≥brM​(xk​)−dyk,maxi​−Mμki​≤yk​,i∈{1,…,n}​

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

---

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

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

   

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

   $$v^x\ge \rho \left|v^y\right|\text{\hspace{1em}(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)\ge y_k\ge b_r^{\mathcal{M}}\left(x_k\right)-d\text{\hspace{1em}(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_{{\mathbf{\Sigma }}_k^i}+a_{\text{shape }},b_{{\mathbf{\Sigma }}_k^i}+b_{\text{shape }}\right)=\mathcal{L}\left(a_k^i,b_k^i\right)\text{\hspace{1em}(9)}$$

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

   $$\begin{array}{rl}{x}_{k,\min }^i& =\left[\underset{x}{\min }\mathcal{L}\left(a_k^i,b_k^i\right)\right]-d^x\\ x_{k,\max }^i& =\left[\underset{x}{\max }\mathcal{L}\left(a_k^i,b_k^i\right)\right]+d^x\\ y_{k,\min }^i& =\left[\underset{y}{\min }\mathcal{L}\left(a_k^i,b_k^i\right)\right]-d^y\\ y_{k,\max }^i& =\left[\underset{y}{\max }\mathcal{L}\left(a_k^i,b_k^i\right)\right]+d^y\end{array}\text{\hspace{1em}(17)}$$

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

   $$\left(x_{k,\min }^i\le x\le x_{k,\max }^i\wedge y\ge y_{k,\min }^i\right)\Rightarrow y\ge y_{k,\max }^i\text{\hspace{1em}(18)}$$

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

   $$\begin{array}{rl}{y}_{k,\max }^i-M{\mu }_k^i\le 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{array}\text{\hspace{1em}(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