反光板导航做完建图、做完匹配、做完位姿解算之后下一个绕不开的坎就是外参。我在前几篇里把反光板从扫描特征提取到全局匹配的过程都过了一遍但真正到了项目现场才发现最影响定位精度的往往不是算法本身而是雷达在车身上装歪了多少、装偏了多少。这篇直接讲硬核部分把外参当成一个待估计的状态放进基于滑窗的EKF-SLAM框架里和机器人位姿、反光板位置一起在线估计同时把观测模型、雅可比矩阵、边缘化细节全部推导一遍。适合正在做AGV/AMR导航、激光反光板定位、或者想搞懂EKF-SLAM公式推导的工程师建议配合前几篇的建图和数据关联内容一起看。1. 先回答三个为什么1.1 反光板定位到底在解决什么问题反光板定位的核心思路不复杂在场地里布置若干块高反光率的板子激光雷达扫描到这些板子时反射强度远高于普通墙面系统提取板心坐标再和地图里存好的反光板坐标做匹配反算出AGV在世界系下的位姿。原理和GPS拿卫星测距类似只不过这里的卫星是静止的反光板距离和角度来自激光雷达的极坐标观测。这套方案在仓储、工厂环境里非常受欢迎因为它不依赖环境纹理不受光照影响重复精度高。但前提是雷达必须知道自己在哪里也就是雷达坐标系相对车体坐标系的位置和朝向必须准确。这个关系就是外参通常写成三个量横向偏移 l_x、纵向偏移 l_y、角度偏移 l_θ。很多团队在开发阶段直接拿尺子量、拿水平仪对一开始确实能用。可AGV是天天在地上跑的设备碰撞、颠簸、换轮子、拆装雷达防护罩任何一次轻微的机械变动都会让外参漂移。我见过一台车外参偏了0.5°反光板在10米外横向误差就是约8.7厘米直接超过设备停位精度要求。人工去重新标定要架全站仪、要专用靶标、要停产折腾大半天过两周可能又漂了。所以在线估计外参不是炫技是刚需。1.2 标准EKF-SLAM为什么不够用最朴素的做法是把外参塞进标准EKF-SLAM的状态向量里和当前位姿、反光板坐标一起滤波。理论上没问题工程上会有三个隐患。第一标准EKF-SLAM是严格马尔可夫假设的当前时刻的后验概率只依赖于上一时刻的状态和当前观测更早的历史信息全部被压缩成一个高斯分布。可反光板定位里雷达对同一块板的观测往往分布在很多帧上如果只保留当前位姿那些早先帧的几何约束就被丢掉了。第二EKF的线性化只做一次线性化点在预测之后固定下来之后不再修正。车走直线还好一旦频繁转弯、加减速误差累积得很快。反光板场景里的地标是稀疏的观测更新间隔可能比较长固定线性化点的问题会被放大。第三协方差矩阵维度和反光板数量成正比。反光板建图规模稍微大一点几十块板子更新一次的主频就开始往下掉现场调参数很痛苦。滑窗EKF就是来治这三个病的窗口内保留最近的若干帧位姿旧帧在边缘化时把信息转换成先验不直接丢掉窗口内所有状态可以联合迭代重线性化减小线性化误差状态维度被窗口长度限制住不会无限膨胀。而外参放在这个框架里等于每次观测都能同时约束位姿、反光板和外参标定和定位一起完成。1.3 这篇文章的定位如果你只需要一个能跑的EKF-SLAM网上有很多开源实现抄作业就行。但这篇的重点是让你理解公式从哪来、雅可比矩阵怎么推、外参可观测性需要什么样的运动激励以及滑窗边缘化的数学原理。这些内容在工程调试时非常有用因为一旦外参估计发散你得能倒回去检查是哪一项偏导算错了而不是盲目调噪声参数。2. 状态设计与滑窗思路2.1 一个状态向量装下所有量滑窗EKF-SLAM的状态向量比普通EKF多一个东西历史位姿。用一个具体的符号定义X [ x_0, y_0, θ_0, x_1, y_1, θ_1, ... x_K, y_K, θ_K, l_x, l_y, l_θ, m_1x, m_1y, m_2x, m_2y, ... m_nx, m_ny ]其中从 x_0 到 x_K 是窗口内保留的最近K1帧机器人位姿l_x、l_y、l_θ 是雷达外参m_jx、m_jy 是第j块反光板的世界坐标。注意这里的外参定义是雷达在车体坐标系下的位姿也就是说当车体位姿是 (x, y, θ) 时雷达在世界坐标系下的位置可以算出来。这个状态设计最关键的一点是窗口内任意一帧位姿都可以和任意一块反光板建立观测约束。反光板在窗口内被多次观测到每次观测都相当于一个独立的量测方程把同一块反光板的坐标、当时的车体位姿和外参串在一起。这是滑窗相对标准EKF最大的优势。2.2 运动模型和外参模型机器人运动模型我用最常用的轮式里程计航迹推算模型。假设里程计给出帧间位移增量 Δs 和转角增量 Δθ那么位姿更新为x_{k1} x_k Δs * cos(θ_k Δθ/2) y_{k1} y_k Δs * sin(θ_k Δθ/2) θ_{k1} θ_k Δθ这里用的是一阶中值积分比纯欧拉积分精度高一些计算量几乎不增加。实际编码时注意Δs、Δθ 来自左右轮编码器或底盘反馈单位要保持一致有些底盘给的是 m/s 和 rad/s要乘上周期。外参和反光板坐标在运动模型里都是常量l_x l_x l_y l_y l_θ l_θ m_jx m_jx m_jy m_jy换句话说运动模型的雅可比矩阵相当稀疏只有当前位姿那一块是单位矩阵加一些三角函数项其余位置全是0。这也带来一个好处预测步骤的计算量主要花在协方差传播上而协方差矩阵是稠密的所以整体复杂度取决于状态维度的平方。2.3 滑窗如何兼顾精度与资源滑窗的直观理解是不再只用当前帧这一个位姿而是把最近N帧位姿都留在状态里形成一个局部时间窗口。窗口内每帧都可以和反光板建立观测约束这比标准EKF的马尔可夫假设松了很多信息利用率明显提高。那窗口满了怎么办不能直接丢掉最老的一帧否则信息就没了。正确做法是边缘化把最老位姿从状态里移除但把它对剩余状态的约束转成一个先验信息项叠加到当前协方差和信息向量上。这个过程在数学上就是Schur补后面第4部分详细写。窗口越大精度越高但计算量也越大。我自己的经验是反光板场景下窗口长度N取5到10就够用了再大边际收益不大反而引入更多线性化误差累积。实际项目里通常设N6配合一帧新的观测进来、一帧老的被边缘化稳态状态维度在可接受范围内。3. 带外参的观测模型公式推导3.1 从世界系到雷达系的坐标变换反光板观测模型是整个公式推导的核心。先明确坐标系关系反光板 m_j 的世界坐标是 (m_jx, m_jy)机器人位姿是 (x, y, θ)外参是 (l_x, l_y, l_θ)。外参的含义是雷达在车体坐标系下的横移、纵移和朝向偏角。那么雷达在世界坐标系下的位姿可以写成x_l x l_x * cosθ - l_y * sinθ y_l y l_x * sinθ l_y * cosθ θ_l θ l_θ这个变换本身就是一个小的推导点。机器人坐标系相对世界系旋转了 θ 角所以车体坐标系下的向量 (l_x, l_y) 转到世界系要左乘旋转矩阵 [[cosθ, -sinθ], [sinθ, cosθ]]。很多初学者在这里容易把旋转方向搞反记住车体到世界是正向旋转然后加上车体坐标 x、y 就对了。雷达朝向 θ_l 就是车体朝向 θ 加上外参角度 l_θ。注意 l_θ 也可能是负值表示雷达安装时故意偏转了一个角度。3.2 距离和角度的量测方程雷达对反光板的原始观测通常是极坐标距离 ρ 和角度 α。把反光板坐标转换到雷达坐标系下先算两个差值dx m_jx - x_l dy m_jy - y_l然后量测方程为ρ sqrt(dx² dy²) α atan2(dy, dx) - θ_lρ 是雷达测得的距离α 是雷达测得的相对于雷达主方向的方位角。这里有一个容易踩的坑atan2 返回的角度范围是 [-π, π]减去 θ_l 之后可能超出这个范围所以残差计算前必须做角度归一化wrap把角度限制到 [-π, π) 区间。否则明明只差 1° 的两个角可能因为跨越 ±π 边界而算出差了 359°EKF直接崩掉。从物理直觉上可以验证这个观测模型如果机器人向前走1米反光板在世界系不动dx 变小ρ 变小α 也会变化如果机器人原地转 90°那么所有反光板的角度观测都会跟着减去 90°这正好反映了车体朝向变化对观测的影响。公式要经得起这种直觉检验。3.3 雅可比矩阵逐项推导EKF更新需要观测方程对状态的雅可比矩阵H。状态里包含三类变量机器人位姿、外参、反光板坐标。所以H要分三块计算。先定义中间变量后面推导会反复用到d ρ dx m_jx - x_l dy m_jy - y_l第一块对机器人位姿 (x, y, θ) 的雅可比。ρ 对 x、y、θ 的偏导∂ρ/∂x -dx / ρ ∂ρ/∂y -dy / ρ ∂ρ/∂θ (dx * (l_x*sinθ l_y*cosθ) dy * (-l_x*cosθ l_y*sinθ)) / ρ为什么 ρ 对 θ 会有偏导因为雷达在世界系的位置 x_l、y_l 依赖 θ。通过链式法则dx 和 dy 对 θ 求导出现上面两个括号项。具体来说x_l 对 θ 的导数是 -l_xsinθ - l_ycosθ所以 dx m_jx - x_l 对 θ 的导数是 l_xsinθ l_ycosθy_l 对 θ 的导数是 l_xcosθ - l_ysinθ所以 dy 对 θ 的导数是 -l_xcosθ l_ysinθ。α 对 x、y、θ 的偏导要用 atan2 的导数公式∂atan2(dy, dx)/∂dx -dy / ρ² ∂atan2(dy, dx)/∂dy dx / ρ²于是∂α/∂x dy / ρ² ∂α/∂y -dx / ρ² ∂α/∂θ (-dy * (l_x*sinθ l_y*cosθ) dx * (-l_x*cosθ l_y*sinθ)) / ρ² - 1最后那个 -1 来自 α 定义式里减去的 θ_l而 θ_l 又包含 θ所以链式法则里必然有一项 -1。这个项经常被忽略一旦漏掉角度残差的雅可比就错了。第二块对外参 (l_x, l_y, l_θ) 的雅可比。ρ 对外参的偏导∂ρ/∂l_x -(dx*cosθ dy*sinθ) / ρ ∂ρ/∂l_y (dx*sinθ - dy*cosθ) / ρ ∂ρ/∂l_θ 0ρ 对 l_θ 为0在物理上很直观旋转雷达自身不改变雷达到反光板的距离。但角度会变所以 α 对 l_θ 的偏导是 -1。α 对外参的偏导∂α/∂l_x (dy*cosθ - dx*sinθ) / ρ² ∂α/∂l_y -(dy*sinθ dx*cosθ) / ρ² ∂α/∂l_θ -1第三块对反光板坐标 (m_jx, m_jy) 的雅可比。∂ρ/∂m_jx dx / ρ ∂ρ/∂m_jy dy / ρ ∂α/∂m_jx -dy / ρ² ∂α/∂m_jy dx / ρ²这一块相对简单因为反光板坐标只在 dx、dy 里出现不经过旋转矩阵。把这些偏导按列排成矩阵就是完整的观测雅可比。写成伪代码H [ H_rho_pose H_rho_ext H_rho_landmark ; H_alpha_pose H_alpha_ext H_alpha_landmark ]实际编码时建议先用数值差分比如在状态值附近加微小扰动差商近似偏导验证每个元素的解析雅可比。我每次写完推导都会写一个数值校验脚本解析值和数值值差在1e-6以内才算过。这个习惯帮我抓出过好几次符号错误。4. EKF更新与滑窗边缘化4.1 预测步的数学处理预测步本质上是对状态均值和协方差做一步传播。状态均值的预测直接用运动模型逐帧递推外参和反光板坐标保持不动。协方差预测的标准形式是P_pred F * P * F^T G * Q * G^TF 是整个状态的运动学雅可比因为状态里包含历史位姿、外参、反光板F 是稀疏块对角矩阵。新增一帧位姿后原来的 K 帧位姿变成 K1 帧如果超过窗口上限边缘化后维度又会降回来所以状态维度不是固定不变的。G 是运动噪声模型对状态的传播矩阵Q 是里程计增量的噪声协方差通常设成对角线幅度根据底盘打滑程度现场调。实际工程里有一个省事的技巧如果运动模型比较简单可以用直接采样预测的方式做均值的传播协方差仍然用解析雅可比。也就是状态均值部分不依赖雅可比只依赖运动模型本身协方差部分才用F和G。这样即使运动模型换了均值传播不会出错。4.2 更新步的新息与增益每次雷达检测到反光板并完成数据关联后做一次EKF更新。更新公式是标准的S H * P * H^T R K P * H^T * S^{-1} ΔX K * (z - h(X)) X X ΔX P (I - K * H) * P其中 R 是观测噪声协方差。反光板观测的噪声一般比较稳定距离噪声 σ_ρ 我通常设 0.01~0.03 米角度噪声 σ_α 设 0.005~0.01 弧度。注意这里距离和角度的噪声量级差了两个数量级如果不放到矩阵里归一化角度残差很容易被距离残差压住。新息残差里最关键的是角度部分要 wrap 到 [-π, π)。很多刚上手的朋友在仿真里忘了这步跑直线没问题一跑带拐弯的轨迹角度残差偶尔跳变一下协方差开始振荡最后发散。这个坑我踩过不止一次。4.3 滑窗边缘化与Schur补边缘化是滑窗EKF和普通EKF最大的区别。当窗口满了要踢掉最老的一帧位姿 x_old。不能直接删因为这帧位姿和反光板、外参、相邻帧之间都有协方差约束。正确的做法是把 x_old 的信息折叠到剩余状态上。假设删除前的状态协方差和信息向量写成分块形式待删除部分是 β保留部分是 αP [ P_aa P_ab ; P_ba P_bb ]边缘化后保留部分的协方差和信息可以通过Schur补得到P_aa_new P_aa - P_ab * P_bb^{-1} * P_ba这个公式的含义是原来被删除状态对保留状态的约束全部转化成一个额外的协方差收缩项。如果你用信息形式实现等价于Λ_aa_new Λ_aa - Λ_ab * Λ_bb^{-1} * Λ_ba η_a_new η_a - Λ_ab * Λ_bb^{-1} * η_b边缘化的代价是保留状态之间会引入额外的稠密关联。所以每次边缘化之后协方差矩阵会变得比以前更满这是正常的。窗口内帧数越多这种关联越密计算量也越高。实际实现时我建议在信息形式下做边缘化效率更高再把结果转回协方差形式或者直接整个滤波在信息形式下做避免来回转换。需要强调一个细节边缘化得到的先验信息是近似高斯分布只能近似表达非线性约束。这也是为什么窗口不能无限大的原因之一——窗口越深叠加的线性化误差越多反而可能变差。4.4 计算量到底涨了多少标准EKF-SLAM的状态维度是 3机器人位姿 3外参 2MM块反光板。更新时的H矩阵是 2×n计算S、K、P更新的复杂度是 O(n²) 到 O(n³)。如果反光板地图有50块板子n ≈ 100普通工控机毫无压力。但如果建图规模到几百块板子n涨到上千每次更新开始吃力。滑窗EKF的维度是 3NN帧位姿 3外参 2M反光板。N固定为6M是窗口内被观测到的反光板数量不是所有反光板。因为EKF状态里仍然存着所有反光板坐标但每次更新只处理当前帧观测到的那几块板的量测。协方差矩阵维度会随M增长但滑窗最重要的一点是历史位姿数量固定不会随着运行时间膨胀。所以总体复杂度可控比每次把所有反光板都拿去更新的方案高效得多。实践中还有一个小技巧把反光板按网格空间分桶只取当前位置附近一个范围内的反光板参与观测匹配能进一步压低有效M省下不少算力。5. 可观测性问题为什么有些运动标定不出来5.1 外参可观测性的直觉外参是雷达相对车体的固定偏移和偏角。从数学上看外参的不确定性和机器人位姿的不确定性在观测方程里是纠缠在一起的。如果机器人只做纯平移而且雷达和外参的变化方向完全平行那么某些外参分量和位姿分量会产生不可区分的模式也就是不可观测。举一个极端例子机器人沿着x轴直线平移雷达相对车体有个纵向安装偏移 l_y反光板都在机器人正前方。这时候激光雷达测得的角度变化可能既来自机器人平移也来自 l_y 的影响两条路径混在一起EKF无法把 l_y 单独拆出来。结论就是纯平移运动无法充分激励外参尤其是横向偏移和角度偏移。反过来说如果机器人走一段包含旋转的运动每次旋转都会改变雷达在世界系的朝向同一块反光板从不同的距离、角度被多次观测外参对量测的影响就和位姿的影响产生了差异系统才能把它们分离出来。5.2 现场标定轨迹设计基于上面的分析现场做外参自动标定时不能只让车跑直线。我建议用下面这套轨迹流程车开到空旷场地周围能看到至少3块以上反光板最好分布在前后左右多个方向。先原地旋转两圈慢速匀速旋转让雷达朝向覆盖360°。走一个8字形轨迹大小根据场地来保证转弯半径小、角度变化大。再反向走一遍8字。最后原地旋转一圈停车。整个过程中EKF会持续更新外参和反光板坐标。跑完后检查外参估计值是否收敛连续两圈变化小于阈值比如角度小于0.005°、位移小于0.003m就算标定完成。这套流程的核心是丰富的旋转多方向观测。注意原地旋转只旋转不平移对角度外参 l_θ 有很好的激励但对平移外参 l_x、l_y 的激励不足。8字轨迹既旋转又平移刚好补上。所以两者缺一不可。5.3 协方差初值怎么给外参的初始协方差设多少直接影响收敛速度和稳定性。给太小估计值几乎不动标定过程形同虚设给太大初期状态波动剧烈甚至把反光板坐标的估计带偏出现发散。我的经验值位置初始标准差给 0.02~0.05 米角度初始标准差给 0.01~0.02 弧度约0.6°~1.1°。如果现场安装位置是通过人工测量粗略获得的大概就是这个误差范围。如果雷达安装结构做过定位销误差更小可以收紧到0.01米、0.005弧度。反光板坐标在启动前如果已经有高精度的静态地图可以把它们的初始协方差设得很小比如0.001m²相当于地图是已知的只在线估计位姿和外参。如果地图也不准那就给大一点让EKF在跑的过程中自动修正。这里有一个工程建议先固定外参只做位姿和反光板的SLAM等地图收敛后再把外参松开进入联合估计稳定性会好很多。6. 工程落地时的几个大坑6.1 数据关联错误会直接毁掉估计EKF更新是高斯假设下的最优线性估计但它对错误的数据关联极度敏感。如果某一帧把观测到的反光板A当成了地图里的反光板B更新步会引入一个巨大的错误新息协方差变大后续的关联也跟着错最后发散。反光板没有身份ID这是数据关联难的根源。我的做法是标准的多假设加门限先根据预测位姿和协方差把地图反光板投影到雷达坐标系预报一个期望观测再把实际观测和所有候选预测值做马氏距离检验距离小于阈值比如卡方分布95%置信界的候选才接受。如果一块板被连续多帧观测还能用多帧几何一致性的投票来增加可靠性。滑窗在这里有一个额外的好处因为窗口内保留着多帧位姿同一块反光板的多次观测可以联合验证。比如反光板在窗口内被3帧观测到它们的几何关系应该自洽如果不自洽基本可以判定关联错误。6.2 外参不是绝对常量实际设备中外参并不是一成不变的。底盘承重变化、雷达支架热胀冷缩、震动磨损都会导致微小的外参漂移。如果EKF里把外参当成绝对的常量那估计出来的外参只能反映一个平均值一旦漂移发生定位误差还是会慢慢变大。处理方式有两种。第一种是给外参加一个很小的过程噪声例如角度过程噪声标准差设成 0.0001 rad/sqrt(s)位置设成 0.0001 m/sqrt(s)。这样EKF认为外参会缓慢游走在线更新时会持续跟踪漂移。但噪声不能给大否则外参估计会跟着观测噪声乱跳。第二种是定期触发自动标定流程比如每天开机后跑一圈标定轨迹把外参结果作为当天的初始值。两种方式可以结合日常用过程噪声缓慢跟踪定期用标定轨迹做个强收敛。我目前项目里就是这种组合策略。6.3 调试时需要盯住新息序列新息innovation是观测值与预测观测值的差即 z - h(X)。调试EKF-SLAM的时候我强烈建议把每一帧的新息序列记录下来画出来看。如果外参初值错误新息序列会呈系统性偏差比如距离新息永远偏大、角度新息总在同一方向。这说明状态估计已经偏移了但滤波可能还在慢慢拉回来。如果新息序列看起来像白噪声均值接近0协方差和理论值接近那说明滤波工作在健康状态。一个特别有用的观察是外参角度误差主要体现为角度新息的正负符号和旋转方向相关外参位置误差主要体现为距离新息随反光板方位角变化。凭这个现象你可以快速判断是哪个外参分量没收敛而不是盲目地加大Q或者R。6.4 数值细节决定稳定性最后说几个细节都是跑了很久才换回来的教训。第一协方差矩阵必须保持对称。EKF的P矩阵理论上是对称的但浮点误差会慢慢破坏对称性最后影响Cholesky分解或者导致负的特征值。我每次更新后都强制做一次 P (P P^T) / 2成本很低收益很大。第二角度归一化要写在最底层函数里。不管是残差计算、雅可比推导、还是新息记录只要涉及角度相减都过一遍 wrap 函数。这个习惯能省掉大量排查时间。第三注意单位的统一。雷达给的角度通常是弧度底盘里程计可能是 m 和 rad但有些底盘协议给的角度是 0.1 度 为单位。换算错误会让EKF的新息大得离谱而且表现特征是只在转大弯时出问题。第四反光板反射点的距离值通常会有一个系统偏差来源于板面反射中心和板子物理中心不重合。最好在板外表贴时预先做一次补偿标定否则这个偏差会作为先验误差灌进整个系统外参估计出来也会被这个偏差污染。一些个人体会这套基于滑窗的EKF-SLAM加上外参自动标定的方案在我实际做过的一个反光板定位项目里跑了大半年。调试过程中最深刻的一条经验是不要一开始就把外参放开。先把外参固定在初值上跑通位姿和反光板建图等位姿轨迹稳定了再放开外参的估计给它一个合理的初始协方差。这样即使外参初值差得比较多整体系统也不容易发散外参会在一个相对正确的位姿估计基础上慢慢被拉到准确值。我到现在都留着一条调试记录外参初始角度误差给了 3°系统跑完标定轨迹后收敛到误差小于0.05°用时大概30秒而协方差矩阵的轨迹在整个过程中非常平滑没有出现过大的振荡。如果你正在做类似的项目建议你把这篇文章里的雅可比矩阵和边缘化公式自己推一遍写成代码后再用数值差分验证一遍。这个过程的收益不在代码本身而是你会对外参和位姿之间的耦合关系产生很直观的理解将来遇到奇怪的定位跳变问题排查思路会快很多。