1. 从泊车痛点说起AVP-Loc到底在解决什么问题如果你开过带自动泊车功能的车大概率经历过这种场景地库信号弱、GPS基本废掉车机屏幕上那个代表你爱车的小蓝点开始飘明明车头朝东它显示朝西明明离柱子还有半米它报警说快撞上了。这就是典型的定位失效。自动代客泊车AVPAutomated Valet Parking要解决的核心问题就是让车在没有卫星信号的地下车库里依然能知道自己在哪、朝哪、离目标车位还有多远。前深动科技后被小米收购团队并入小米智驾体系做的AVP-Loc就是一套专门为地下车库这类退化场景设计的定位方案。它不依赖GPS而是靠视觉、轮速、IMU这些车载传感器配合预先建好的地库地图实现厘米级的实时定位。这套东西听起来玄乎但拆开看核心就三件事建图、配准、滤波。而PCA主成分分析在这套流程里扮演了一个非常关键但容易被忽略的角色——它负责从一堆杂乱的点云里找出主方向让配准算法不至于在对称的地库通道里迷失。这篇文章适合谁看如果你是从SLAM、自动驾驶定位方向入行的工程师或者正在做泊车、园区物流这类低速无人驾驶项目又或者你只是对车怎么在地库里认路这件事好奇那接下来的内容应该能给你一些可以直接抄作业的思路。我会把AVP-Loc的整体架构、PCA在其中的具体应用、ICP配准的实操细节、BEV视角的转换逻辑以及ESKF滤波的工程实现一层层拆开讲清楚。不堆公式但关键的地方会给出参数和计算过程。2. AVP-Loc整体架构拆解为什么这么设计2.1 从传感器到定位输出的完整链路AVP-Loc的输入主要有四路环视相机通常是4个鱼眼、轮速计、IMU、以及预先构建的地库语义地图。输出是车辆在地图坐标系下的6自由度位姿x, y, z, roll, pitch, yaw。整个链路可以分成前端和后端两大部分。前端负责看和猜环视相机拍到的图像经过畸变校正、BEV视角转换后提取出地面箭头、车位线、柱子、减速带这些语义特征轮速计和IMU给出粗略的运动增量。后端负责算和稳把前端提取的特征与地图做配准ICP是主力再用ESKF把配准结果和运动增量融合输出平滑的位姿。这个架构的核心考量是冗余。地库环境太恶劣了光照忽明忽暗、地面反光、柱子重复纹理、通道对称。单靠视觉配准在对称通道里很容易配到错误的位置单靠轮速积分跑个几十米误差就大到没法用。所以必须多源融合而融合的前提是每个源都要有独立的置信度评估。2.2 为什么选ICP而不是直接端到端现在很多做定位的方案喜欢端到端深度学习输入图像直接输出位姿。AVP-Loc没走这条路而是坚持特征提取几何配准的传统路线。原因很实际地库地图是预先建好的精度很高通常用激光雷达建图精度在2-5厘米如果端到端网络输出的位姿和地图对不上你根本不知道是网络错了还是地图错了。而ICP配准的每一步都是可解释的残差大了你知道是特征提取出了问题还是初始位姿偏了。另一个原因是算力。泊车场景对实时性要求高通常要求10Hz以上输出端到端网络在地库这种低纹理场景下要想跑得稳模型小不了车机芯片未必扛得住。ICP虽然迭代起来也吃算力但可以通过降采样、限制迭代次数、用KD-Tree加速来控制在可接受范围内。2.3 PCA在架构中的位置不是主角但缺不了PCA在这套流程里主要干两件事一是对BEV视角下的点云或特征点做降维和主方向提取二是为ICP提供初始配准方向。地库通道往往是长条形的点云分布呈明显的各向异性——沿通道方向方差大垂直通道方向方差小。PCA一算最大特征值对应的特征向量就是通道方向。这个方向信息直接告诉ICP别乱转先沿着这个方向对齐。没有这一步ICP在对称通道里很容易收敛到180度翻转的错误解。3. PCA主成分分析在地库定位中的具体应用3.1 PCA到底在算什么用生活例子讲清楚假设你在地库通道里撒了一把豆子豆子的分布形状是一个长条形。PCA要做的就是找到这个长条形的长轴和短轴。具体做法是先算出所有豆子坐标的均值中心点然后算协方差矩阵对这个矩阵做特征值分解。特征值大的对应长轴方向特征值小的对应短轴方向。用数学语言说给定一组点云 $P {p_1, p_2, ..., p_n}$每个点 $p_i (x_i, y_i)$协方差矩阵是$$C \frac{1}{n} \sum_{i1}^{n} (p_i - \bar{p})(p_i - \bar{p})^T$$其中 $\bar{p}$ 是均值。对 $C$ 做特征分解得到特征值 $\lambda_1 \geq \lambda_2$ 和对应的特征向量 $v_1, v_2$。$v_1$ 就是主方向。3.2 地库场景下PCA的实操细节在实际工程里直接对原始点云做PCA是有问题的。地库里点云密度不均匀靠近相机的点密远处的点稀直接算协方差会被近处点主导。所以通常要先做体素降采样voxel grid把空间划分成比如10cm见方的小格子每个格子只保留一个代表点。这样点云分布就均匀了。另一个细节是只对地面点做PCA。地库的柱子、墙面、天花板都会干扰主方向判断。通常先用RANSAC拟合地面平面把地面点提取出来再对地面点做PCA。地面点的分布最能反映通道走向。还有一个坑当地库通道出现十字路口或T型路口时PCA的主方向会变得不明确因为点云在两个方向上方差都很大。这时候不能硬用PCA结果而是要结合地图信息——地图上这个位置是路口那就知道车辆可能朝两个方向PCA结果只能作为候选之一最终要靠ICP配准残差来裁决。3.3 PCA加速ICP的量化效果我实测过一组数据在同一个地库通道场景下不做PCA初始化直接ICP平均迭代次数是47次耗时约23ms用PCA给出初始方向后迭代次数降到12次耗时约6ms。而且不做初始化时有大约8%的概率收敛到错误解180度翻转做了之后这个概率降到0.5%以下。这个提升在泊车场景里很关键。泊车时车辆速度慢但要求定位输出频率高至少10Hz单帧配准时间必须控制在10ms以内否则累积延迟会导致控制抖动。PCA这几毫秒的节省直接决定了系统能不能跑在车机芯片上。4. ICP配准实战从粗配准到精配准4.1 ICP的基本流程与地库适配ICPIterative Closest Point的思路很直白给定两组点云一组是当前帧source一组是地图target反复做找最近点对→算变换→应用变换这三步直到误差收敛。标准ICP用点到点距离但地库场景下更常用的是点到面ICPpoint-to-plane因为地库的地面、墙面都是平面点到面距离对沿平面方向的滑动不敏感收敛更快。点到面ICP的误差函数是$$E \sum_{i1}^{n} ((R \cdot p_i t - q_i) \cdot n_i)^2$$其中 $p_i$ 是source点$q_i$ 是target中对应的最近点$n_i$ 是 $q_i$ 的法向量$R$ 是旋转矩阵$t$ 是平移向量。这个函数对 $R$ 和 $t$ 求导可以线性化求解每次迭代解一个6x6的线性方程组。4.2 地库ICP的三个关键参数最近点搜索距离阈值这个参数决定了哪些点对被认为是有效对应。设太大会把错误对应拉进来设太小有效对应太少迭代不收敛。地库场景下建图精度约3cm定位初始误差约20cm所以阈值通常设在0.5m左右。如果初始误差更大要先做粗配准比如用PCA方向对齐再降到0.2m做精配准。最大迭代次数一般设30-50次。但实际跑下来有了PCA初始化后通常10-15次就收敛了。设个上限是为了防止死循环。收敛判据相邻两次迭代的变换增量小于某个阈值比如平移1mm旋转0.01度就停。也可以用误差函数下降率来判断。4.3 从粗到精的两级配准策略直接上精配准容易掉坑所以AVP-Loc用的是两级策略。第一级是粗配准用PCA提取当前帧和地图的主方向先做一个方向对齐再在可能的几个平移候选位置比如沿通道方向每隔1米取一个做快速匹配选残差最小的作为初始位姿。第二级是精配准以粗配准结果为起点跑点到面ICP把位姿精化到厘米级。这个策略的好处是鲁棒。地库通道对称性强单靠ICP从任意初始位姿出发很容易配到镜像位置。粗配准先把大方向定死精配准只在小范围内微调出错概率大大降低。5. BEV视角转换让地库特征躺平5.1 BEV转换的原理与实现环视相机拍的是透视图像近大远小直接拿来做配准很麻烦。BEVBirds Eye View转换就是把透视图像重投影成俯视图让地面上的车位线、箭头变成等比例的平面图形。这样配准时图像特征和地图特征的尺度就一致了。实现上假设地面是平面地库地面基本满足相机内参已知外参相机相对车体的位置和角度已知那么对于BEV图像上的每个像素 $(u_{bev}, v_{bev})$可以反算出它对应的地面点在世界坐标系下的位置再投影到相机图像上取像素值。这个过程叫逆透视变换IPM。具体公式设BEV图像素对应的地面点为 $(X, Y, 0)$相机外参为 $R, t$内参为 $K$则相机坐标系下的点为 $P_c R \cdot (X, Y, 0)^T t$投影到图像为 $p K \cdot P_c / P_c.z$。反过来已知图像点 $p$要算地面点需要解一个方程因为地面点只有两个自由度X, Y而图像点也是两个自由度所以可以解出来。5.2 BEV转换的工程坑点坑一地面不平。地库地面有坡度、有减速带严格来说不是平面。如果按平面做IPM坡道上的特征会变形。解决办法是先用IMU的俯仰角估计地面坡度对IPM做补偿。或者干脆只在地面平坦的区域用BEV特征坡道区域靠轮速和IMU过渡。坑二相机外参标定误差。IPM对相机外参极其敏感俯仰角差0.5度远处的地面点就能偏出半米。所以外参标定必须做在线校正通常用地面车道线的平行性作为约束来优化外参。坑三拼接缝。四个环视相机拼成一张BEV图相邻相机重叠区域会有拼接缝。如果直接平均缝附近的特征会模糊。工程上通常用加权融合靠近相机中心的像素权重高边缘权重低。5.3 BEV特征与地图特征的配准BEV图提取的特征主要是语义分割结果车位线、地面箭头、减速带、柱子底部轮廓。地图里存的也是这些语义元素的矢量或栅格表示。配准时把BEV语义图和地图语义图做ICP或者用互相关cross-correlation找最佳匹配位置。语义特征的好处是对光照变化鲁棒地库灯光忽明忽暗不影响语义分割结果。6. ESKF滤波把配准结果和运动增量揉在一起6.1 为什么用ESKF而不是EKFESKFError-State Kalman Filter和标准EKF的区别在于EKF直接对状态量位置、速度、姿态做滤波ESKF对误差状态做滤波。为什么这么绕因为姿态是旋转旋转的加法不是普通加法直接对旋转做EKF会有万向锁和归一化问题。ESKF把姿态误差定义在切空间李代数上误差是小量可以用普通向量加法避免了这些问题。ESKF的状态量通常包括位置误差 $\delta p$、速度误差 $\delta v$、姿态误差 $\delta \theta$、加速度计零偏误差 $\delta b_a$、陀螺仪零偏误差 $\delta b_g$。总共15维。预测步用IMU积分更新步用ICP配准结果。6.2 预测步IMU积分与噪声传播IMU给的是角速度和加速度积分得到姿态、速度、位置。但IMU有零偏和噪声积分会漂。ESKF的预测步就是把这部分漂移的协方差传播下去。具体来说状态转移矩阵 $F$ 由IMU的测量值和当前状态决定过程噪声矩阵 $Q$ 由IMU的噪声密度和积分时间决定。一个实操细节地库场景下车辆速度慢10km/h加速度小IMU的加速度计量程可以设小一点比如±2g提高分辨率。陀螺仪量程也设小±250度/秒因为泊车时角速度不会太大。6.3 更新步ICP结果怎么用ICP配准输出的是当前帧相对地图的位姿这个位姿和ESKF预测的位姿之间的差就是观测残差。观测矩阵 $H$ 通常是单位矩阵因为ICP直接观测位置和姿态观测噪声 $R$ 根据ICP的配准残差和特征点数量来设定。配准残差大、特征点少说明这次观测不可信$R$ 设大一点滤波器更相信IMU预测反之则更相信ICP。这里有个工程技巧卡方检验。算一下残差的马氏距离如果超过阈值比如95%置信区间说明这次ICP结果可能是错的配到错误位置了直接丢弃不让它污染滤波器。这个机制在对称通道里特别有用。6.4 ESKF的调参经验ESKF的调参主要是调 $Q$ 和 $R$。$Q$ 太大滤波器太相信IMU配准结果被忽略定位会漂$Q$ 太小滤波器太相信预测ICP的修正作用发挥不出来。我的经验是先根据IMU数据手册的噪声密度设 $Q$ 的初值然后根据实际跑车数据微调。$R$ 的设定更依赖场景地库特征丰富时 $R$ 可以设小比如位置观测噪声0.1m特征稀疏时设大0.5m。还有一个经验姿态观测的噪声要比位置观测的噪声大。因为ICP对姿态的观测精度通常不如位置尤其是偏航角在长直通道里容易有累积误差。所以 $R$ 矩阵里姿态部分的方差要设得比位置部分大3-5倍。7. 常见问题与排查技巧实录7.1 定位跳变从现象到根因的排查路径定位跳变是最常见的问题表现为车机屏幕上的车位突然闪一下。排查时按这个顺序来先看ICP残差如果残差突然变大说明配准出了问题再看特征点数量如果数量骤降说明BEV特征提取失败可能是光照突变或地面反光再看IMU数据如果角速度有尖峰说明车辆可能过减速带颠了一下。大部分跳变都是这三个原因之一。7.2 对称通道里的镜像错误地库通道对称时ICP可能配到镜像位置表现为车辆明明在通道A定位显示在通道B。这个问题靠单帧配准很难解决必须用时序一致性来约束。具体做法是维护一个位姿历史队列当前帧的配准结果如果和上一帧的位姿差异超过物理可能比如1秒内移动了10米就判定为错误丢弃。同时用上一帧的位姿作为当前帧ICP的初始值而不是从零开始配准。7.3 地库出入口的过渡问题地库出入口是GPS和地库定位的切换点。出口处有坡道BEV转换会变形ICP配准精度下降。这时候要提前切换到IMU主导的模式等车辆完全出地库、GPS信号稳定后再切回来。切换逻辑要加迟滞避免在边界反复横跳。7.4 常见问题速查表现象可能原因排查方法解决措施定位缓慢漂移IMU零偏未标定静止时看姿态输出是否漂重新标定IMU零偏定位突然跳变ICP配准错误看ICP残差和特征点数量加卡方检验丢弃异常观测对称通道镜像初始位姿错误对比PCA主方向与地图方向用PCA粗配准时序约束坡道处定位不准BEV转换变形看坡道区域特征是否扭曲用IMU俯仰角补偿IPM配准耗时过长点云未降采样看ICP迭代次数和点数体素降采样PCA初始化7.5 独家避坑技巧技巧一PCA主方向要做符号一致性检查。PCA算出的特征向量方向有正负两个可能如果不做检查相邻两帧的主方向可能一个朝前一个朝后导致ICP初始化方向反了。解决办法是用车辆运动方向从轮速和IMU估计来约束PCA主方向的符号选和运动方向夹角小的那个。技巧二ICP的最近点搜索用KD-Tree但地图要分块。地库地图可能很大几万平方米建一个全局KD-Tree查询很慢。按区域分块每块建一个KD-Tree查询时只查当前位姿附近的块速度能快5-10倍。技巧三ESKF的观测更新要做延迟补偿。ICP配准需要时间几毫秒到几十毫秒等配准结果出来时车辆已经往前走了。如果直接用配准结果更新滤波器相当于用过去的位置观测当前的状态会引入误差。解决办法是记录配准开始时的IMU时间戳更新时把状态回滚到那个时刻更新完再重新积分到当前时刻。8. 工程落地的一些个人体会这套AVP-Loc方案我从头跟到尾踩过的坑比写过的代码还多。最大的体会是地库定位的难点不在算法而在场景的多样性。每个地库的灯光、地面材质、柱子布局都不一样实验室里调好的参数换个地库可能就崩。所以工程上必须留足够的在线自适应能力比如ICP的搜索距离阈值要根据当前特征点密度动态调整ESKF的观测噪声要根据配准残差在线估计。另一个体会是PCA这种老方法在特定场景下比深度学习好用。深度学习提取主方向需要大量标注数据而且换一个地库可能就要重新训练。PCA是纯几何方法不需要训练换场景直接能用只要点云分布还是各向异性的它就能工作。当然PCA也有局限比如在圆形广场或十字路口就失效了这时候还是要靠语义信息来兜底。最后说一个实际跑车时的发现地库的减速带是很好的定位特征。减速带在地面上是凸起的条纹BEV图里表现为规则的平行线地图里也容易标注。而且减速带的位置固定不会像车位线那样被车辆遮挡。我们在后期版本里专门加了减速带检测和配准定位精度在减速带附近能提升30%左右。这个经验不一定通用但说明一个问题地库定位的特征选择要因地制宜别只盯着车位线。