提出一种基于高精地图的视觉传感器为主导的低成本定位方案。IMU+GPS虽然很多场景效果可靠并且性价比也不错，但是GPS的精度往往没有那么可靠，基于激光点云或者视觉特征点云的定位虽然实验效果不错，但是长时间的运转会产生大量的误差，并且不适应环境的变化，并且白天晚上，夏天冬天环境变化也会导致定位鲁棒性下降，另外点云存储有个问题在于内存需求过大，不易于存储(HD地图高度结构化，内存需求不大).提出语义定位方法，将HD map与相机语义信息结合，提出一种定位方案。

主要贡献

利用视觉语义分割和高精地图，提出完整的定位方法
提出初始化，局部跟踪，丢失恢复方案。
适用与多个相机传感器

语义分割

主要对视觉图像中三个语义信息进行分割,提取图像语义的网络是resnet-18，分别是车道(LA) ,杆状物体(PO), 广告牌(SB)。并采用一些形态学滤波的方法作为预处理，提高优化算法收敛速度。处理后的图像数据称为代价地图(CostMap)，数值为0 ~ 1。

初始化

过GPS获得一个粗糙的初值, 由于车辆可以处于静止状态，因此将两个 GPS 点的距离设置为一个适中的值，从GPS中获得车辆的XY坐标，并且基于搜索的近地图地面元素获得z坐标。翻转角和俯仰角设置为0，偏航角设置为两次gps的计算值，为了获得较高的初始化成功率和更准确的初始姿态结果，通过在预定义网格中进行穷举姿态搜索来细化粗初始姿态，这个操作的代价函数可以定义为所有语义地标的光度误差。

\[ f(T_b^w)=\sum^N_{i=0}\|I_s(\pi((T^w_b\cdot T^b_c)^{-1}P^w))-1.0\|_2 \]

展开：

\[ f(T_b^w)=\sum^N_{i=0}\|I_s(\pi( {R_c^b}^{-1} * \left( {R_b^w}^{-1} * \left( P^w - t^w_b \right) - t^b_c \right) ))-1.0\|_2 \]

大致的意思是高精地图中的object投影到图像平面与图像分割结果越接近，表示位姿估计越准，这个方法可以实现成搜索算法，也可以实现成优化算法。

对于优化算法，需要给出雅克比矩阵：

\[ \frac{\partial f(T_b^w)}{\partial{T_b^w}} = \frac{\partial I_s}{\partial{uv}}_{1\times2} * \frac{\partial Iuv}{\partial{p^c}}_{2\times3} * \frac{\partial p^c}{\partial{T^w_b}}_{3\times6} \]

对于图像导数，由于图像不连续，可以使用插值的方法:

\[ \left[ \begin{matrix} a \\ b \\ c \\ d \end{matrix}\right] = \left[ \begin{matrix} 0.5 * (-p0 + 3.0 * p1 - 3.0 * p2 + p3) \\ 0.5 * (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3) \\ 0.5 * (-p0 + p2) \\ p1 \end{matrix}\right] \\ \\ f = ax^3 + bx^2 + cx + d \\ dfdx = 3ax^2 + 2bx + c \]

对于相机模型导数：

\[ \frac{\partial uv}{\partial{pc}} = \left[ \begin{matrix} \frac{\partial u}{\partial{x}} & \frac{\partial u}{\partial{y}} & \frac{\partial u}{\partial{z}} \\ \frac{\partial v}{\partial{x}} & \frac{\partial v}{\partial{y}} & \frac{\partial v}{\partial{z}} \\ \end{matrix} \right] = \left[ \begin{matrix} \frac{1}{z} & 0 & \frac{-x}{z^2} \\ 0 & \frac{1}{z} & \frac{-y}{z^2} \\ \end{matrix} \right] \]

对于相机坐标系下点对位姿导数，采用李群右扰动形式：

\[ \frac{\partial p^c}{\partial{T^w_b}} = \left[ \begin{matrix} \frac{\partial p^c}{\partial{R^w_b}} & \frac{\partial p^c}{\partial{t^w_b}} \end{matrix}\right] = \left[ \begin{matrix} {R^b_c}^{-1} * \left[ {R^w_b}^{-1} * \left( p^w - t^w_b \right) \right]_{\times} & -\left( R^w_b * R^b_c \right)^{-1} * t^w_b \end{matrix}\right] \]

跟踪

主要分为三步，第一步是根据外部里程计(比如轮式里程计)获得当前帧的粗糙位姿，如果驾驶场景满足纵向约束，则执行来自全局地图步骤的裁剪局部地图。否则，首先应用纵向位置校正过程。

基于全局地图的局部地图裁剪: 地图元素(LA、PO 和 SB)使用当前粗略的车辆姿态在预定义的短距离内从全局地图中查询。然后将查询到的局部地图应用于无漂移视觉定位。地图元素 E 投影回图像点 P。为了获得准确的位姿优化，P 中的点在图像空间中均匀采样

纵向位置调整: 在驾驶场景不满足纵向约束的情况下，纵向定位在很长一段时间后可能会发生显着漂移,例如，当查询的车道（1）彼此平行，（2）笔直向前，以及（3）没有标志牌或电线杆来限制车辆在环境中的纵向平移时，就会发生这种情况。在这种情况下，就需要根据GPS进行纵向的位置调整。

\[ T_{wb}^{k+1} = T_{wb}^k * T_{b}^{k->k + 1} \]

第二步是通过与高清地图元素的图像对齐来细化 6 DoF 车辆姿态。代价函数图是基于图像语义分割和形态学操作构建。为了防止垂直地标的丢失，首先计算z轴的角度和y轴的平移，然后再计算y轴的角度和z轴的平移，x轴的角度和平移可以被忽视，因为道路场景下，翻滚角通常很小

第三步通过滑窗的因子图优化来获得一个平滑的位姿，总共两个因子，第一个是根据视觉获得的先验位姿因子，第二个是通过轮式里程计建立的相邻帧因子。

\[ T=argmin\sum_{i,j\in W}\|ln(T_i(T^*_i)^{-1})^{\bigvee}\|_2+\lambda\|ln(T^{-1}_jT_{i}T_{i,j})^{\bigvee}\|_{2} \]

跟踪丢失恢复

当检测到错误状态时，采用轮式里程计进行恢复，错误状态主要有以下三种: 一是车辆不在高清地图的操作域内，二是姿态优化失败总数超过阈值，三是严重遮挡的连续帧数超过阈值（例如，在语义地图元素完全不可见的交通堵塞情况下）

最后在各种环境，(1-2) 弯路; (3) 晴天的长直路; (4) 雨天; (5) 挡风玻璃雨刮器挡住了部分图像; (6)分流匝道; (7) 低光照; (8-9) 交通拥堵都得到来比较好的验证。

Reference

[1] Coarse-to-fine Semantic Localization with HD Map for Autonomous Driving in Structural Scenes IROS2021