标签：TRO2022 Urban Planner mathbf text right mathcal lambda left
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，只是因为泛化公式所以引入了 \(\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} \]其中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有关；约束解释：
- 在任意的k从0,N是指N个离散step下
- 是指从时间k的状态\(\mathbf z_k = (x_k, y_k, \phi_k, v_k)\) 当输入\(\mathbf u_k = (a_k, \delta_k)\) 时 即加速度和转向角度，变成的下一个状态\(z_{k+1}\)的dynamical system
- 在状态所占据的空间面积 \(S(\mathbf z_k) \subset \R^2\) 和不安全的面积area为空集，即两者面积不相交
- 是根据上述论文[24] 里每一个障碍物都有一个不确定的包围圈，对于障碍物i，其概率协方差为\(\Sigma_k^i\)，预测占据面积概率大于 \(p_{\epsilon}\) 也和自身的状态所占据的空间为空集，即不相交
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)，虽然有点长：
\[\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} \]问题不大，下面一个个解释一下
-
在k从m,m+K是指K个离散step下，应该满足一下约束
-
首先是常见的自行车模型 → 但实际上在后面说明了，求解时是直接看做质点 emmm ，所以呢大概是这样的：
-
然后 因为是质点 如果不加约束的话 那 v_y 速度过大就是横着走了 当然是不行的，所以呢 加了一个限制，其中 \(\rho\)为常数， 假设 \(\phi_k\in[-\frac{\pi}{2},\frac{\pi}{2}]\) 也就是不考虑调头
\[v^{x} \geq \rho\left|v^{y}\right| \tag{15} \]-
因为还是需要非线性的部分的，是problem 2 会用非线性求解器求解，转成MILP是提前得到一个大致的初解！ 我悟了他的贡献点了
-
3-8 的约束全在加速度不能超过限制，速度不能超过限制，加加速度不能超限，就不过多解释了 emm 就是车都是有极限的 也不可能一脚油门 0.0001s就达到我们想要的300km/h的速度 这个意思
-
正常道路的行驶，也就是自身不碰到路沿，其中 \(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} \] -
这个公式有些稍稍难解释，我们回到刚刚的图五，我再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个子问题；
整体步骤再梳理一下：
- 构建完整问题，是一个非线性的问题
- 通过简化某些部分 比如车的模型，一些约束，来将问题转化为混合整数优化问题，然后通过连续求解 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,Urban,Planner,mathbf,text,right,mathcal,lambda,left 来源： https://www.cnblogs.com/kin-zhang/p/16345747.html
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。