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

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

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

2. Method

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

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

2.1 框架

2.2 motion synthesis

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

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

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

tλ=[∂XPref (λ)∂λ∂YPref (λ)∂λ],nλ=[−∂YPref (λ)∂λ∂XPref (λ)∂λ](1)\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} tλ=[∂λ∂XPref (λ)∂λ∂YPref (λ)],nλ=[∂λ−∂YPref (λ)∂λ∂XPref (λ)](1)

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

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

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

x=argmin⁡λ(X−XPref (λ))2+(Y−YPref (λ))2x=\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=λargmin(X−XPref (λ))2+(Y−YPref (λ))2

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

其中y 则是 y^=[X−XPref (x)Y−YPref (x)]\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^=[X−XPref (x)Y−YPref (x)] 即一个从参考线点指向车中心的向量，然后乘法向量就是参考线坐标系下的y啦，公式也就是：y=1∥nx∥nx⊤⋅y^y=\frac{1}{\left\|\mathbf{n}_{x}\right\|} \mathbf{n}_{x}^{\top} \cdot \hat{\mathbf{y}}y=∥nx∥1nx⊤⋅y^

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

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

argmin⁡z1:N,u0:N−1J(z0:N,u0:N−1)s.t. ∀k∈{0,…,N}:zk+1=fΔt(zk,uk)S(zk)∩Bout =∅S(zk)∩[⋃i∈{1,…,n}Si(oki,Σki,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−1argmin 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有关；约束解释：

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

2.3 MILP and Optimization

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

kinematic vehicle model
driveable area collision
traffic participnats’ collision
multiobjective cost function over soft constraints

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

argmin⁡z‾m+1:m+K,u‾m:m+K−1∑k=mm+KJCM,k(z‾k,u‾k)s.t. ∀k∈{m,…,m+K}:z‾k+1=FΔt(z‾k,u‾k)vx≥ρ∣vy∣amin⁡x≤akx≤amax⁡xamin⁡y≤aky≤amax⁡y∣ak+1x−akx∣<Δamax⁡xΔt∣ak+1y−aky∣<Δamax⁡yΔtvmin⁡x≤vkx≤vmax⁡xvmin⁡y≤vky≤vmax⁡yd+blM(xk)≥yk≥brM(xk)−dyk,max⁡i−Mμki≤yk,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−1argmin s.t. ∑k=mm+KJCM,k(zk,uk)∀k∈{m,…,m+K}:zk+1=FΔt(zk,uk)vx≥ρ∣vy∣aminx≤akx≤amaxxaminy≤aky≤amaxy∣∣ak+1x−akx∣∣<ΔamaxxΔt∣∣ak+1y−aky∣∣<ΔamaxyΔtvminx≤vkx≤vmaxxvminy≤vky≤vmaxyd+blM(xk)≥yk≥brM(xk)−dyk,maxi−Mμki≤yk,i∈{1,…,n}

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

在k从m,m+K是指K个离散step下，应该满足一下约束
首先是常见的自行车模型 → 但实际上在后面说明了，求解时是直接看做质点 emmm ，所以呢大概是这样的：
然后因为是质点如果不加约束的话那 v_y 速度过大就是横着走了当然是不行的，所以呢加了一个限制，其中 ρ\rhoρ为常数，假设 ϕk∈[−π2,π2]\phi_k\in[-\frac{\pi}{2},\frac{\pi}{2}]ϕk∈[−2π,2π] 也就是不考虑调头

vx≥ρ∣vy∣(15)v^{x} \geq \rho\left|v^{y}\right| \tag{15} vx≥ρ∣vy∣(15)
- 那为啥前面还写那么多自行车模型的 emmm 直接看后面这个部分，有没有前面铺垫丝毫不受影响
  
  因为还是需要非线性的部分的，是problem 2 会用非线性求解器求解，转成MILP是提前得到一个大致的初解！我悟了他的贡献点了

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

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

d+blM(xk)≥yk≥brM(xk)−d(16)d+b_{l}^{\mathcal{M}}\left(x_{k}\right) \geq y_{k} \geq b_{r}^{\mathcal{M}}\left(x_{k}\right)-d \tag{16} d+blM(xk)≥yk≥brM(xk)−d(16)
这个公式有些稍稍难解释，我们回到刚刚的图五，我再copy进来

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

Si⊂L(aΣki+ashape ,bΣki+bshape )=L(aki,bki)(9)\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} Si⊂L(aΣki+ashape ,bΣki+bshape )=L(aki,bki)(9)

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

xk,min⁡i=[min⁡xL(aki,bki)]−dxxk,max⁡i=[max⁡xL(aki,bki)]+dxyk,min⁡i=[min⁡yL(aki,bki)]−dyyk,max⁡i=[max⁡yL(aki,bki)]+dy(17)\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} xk,minixk,maxiyk,miniyk,maxi=[xminL(aki,bki)]−dx=[xmaxL(aki,bki)]+dx=[yminL(aki,bki)]−dy=[ymaxL(aki,bki)]+dy(17)

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

(xk,min⁡i≤x≤xk,max⁡i∧y≥yk,min⁡i)⇒y≥yk,max⁡i(18)\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} (xk,mini≤x≤xk,maxi∧y≥yk,mini)⇒y≥yk,maxi(18)

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

yk,max⁡i−Mμki≤ykwhere μki=max⁡(xk,min⁡i−xk,0)+max⁡(xk−xk,max⁡i,0)+max⁡(yk,min⁡i−yk,0)(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} yk,maxi−Mμki≤yk where μki=max(xk,mini−xk,0)+max(xk−xk,maxi,0)+max(yk,mini−yk,0)(19)

最后则是我们的最小化对象 cost funtion：J(z0:N,u0:N−1)=∑k=0N∑ι∈Iωιθι(zk,uk)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)J(z0:N,u0:N−1)=∑k=0N∑ι∈Iωιθι(zk,uk)

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

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

但实际上我们将其也转换到了MILP：JCM,k(z‾,u‾)=∑ι∈CΩιΘι(z‾,u‾)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}})JCM,k(z,u)=∑ι∈CΩιΘι(z,u)

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

Θx=∣x−xg∣,Θv=∣v−vg∣,Θy=∣y∣\Theta_{x}=\left|x-x_{g}\right|, \Theta_{v}=\left|v-v_{g}\right|,\Theta_{y}=\left|y\right|Θx=∣x−xg∣,Θv=∣v−vg∣,Θy=∣y∣
后者则是仅加速度 Θa=∣ay∣\Theta_{a}=\left|a_y\right|Θa=∣ay∣

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

整体步骤再梳理一下：

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

！啊我悟了… 所以主要贡献其实就是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真的还是太长了

碎碎念

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

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

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

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

赠人点赞手有余香

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

论文阅读06——《CaEGCN: Cross-Attention Fusion based Enhanced Graph Convolutional Network for Clustering》
欢迎到我的个人博客看原文论文阅读06--<CaEGCN: Cross-Attention Fusion based Enhanced Graph Convolutional Network f ...
论文那些事—ZOO: Zeroth Order Optimization Based Black-box Attacks
ZOO: Zeroth Order Optimization Based Black-box Attacks to Deep Neural Networks without Training Subs ...
【论文阅读】RoadMap: A Light-Weight Semantic Map for Visual Localization towards Autonomous Driving
文章:RoadMap: A Light-Weight Semantic Map for Visual Localization towards Autonomous Driving 作者:Tong Q ...
【论文阅读】GRI: General Reinforced Imitation and its Application to Vision-Based Autonomous Driving
Column: December 30, 2021 11:01 PM Last edited time: January 10, 2022 4:45 PM Sensor/组织: 3 RGB; 曾经短暂 ...
论文阅读Targetless Calibration of LiDAR-IMU System Based on Continuous-time Batch Optimization（含代码解读）
原文链接: https://arxiv.org/pdf/2007.14759v1.pdf 代码地址GitHub - APRIL-ZJU/lidar_IMU_calib: Targetless Cali ...
论文阅读＞烟雾检测：Vision based smoke detection system using image energy and color information
原文 Vision based smoke detection system using image energy and color information 文章目录原文基于图像能量和颜色信息的 ...
【论文阅读】PSF-LO: Parameterized Semantic Features Based Lidar Odometry
这篇论文是阿里研究院的一篇论文,主要内容是结合语义.激光SLAM的一个前端里程计部分,除了语义信息与激光SLAM的整合,论文还使用了基于物体的思想,只保留了方便建图定位的静态四类物体:道路.建筑物.交 ...
论文阅读：A deep multimodal network based on bottleneck layer features fusion for action recognition
摘要:提出了一个深度瓶颈多模式特征融合(D-BMFF)框架,融合RGB.RGB- d(深度)和三维坐标信息三种不同的模式进行活动分类.在四个数据集: UT-Kinect, CAD-60, Floren ...
【论文阅读】An Iterative Instance Selection Based Framework for Multiple-Instance Learning
题目 An Iterative Instance Selection Based Framework for Multiple-Instance Learning 一种基于迭代实例选择的多示例学习框架 ...

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