基于RBPF与全阶EKF的双自动驾驶车辆协同SLAM算法对比及融合策略研究(Matlab代码实现)

   💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

🎁完整资源、论文复现、期刊合作、论文辅导及科研仿真定制事宜点击：

👉👉👉本文完整资源下载

⛳️座右铭：行百里者，半于九十。

 ⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能解答你胸中升起的一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

     或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥第一部分——内容介绍

基于RBPF与全阶EKF的双自动驾驶车辆协同SLAM算法对比及融合策略研究

摘要

同步定位与地图构建（SLAM）是自主车辆、移动机器人实现未知环境自主导航的核心关键技术，单车SLAM受传感器探测范围有限、观测盲区、随机噪声干扰及误差累积等问题制约，长期运行下定位漂移与建图精度退化问题突出。为解决单一智能体感知能力不足的缺陷，多车协同SLAM通过多平台观测信息共享与融合，充分利用空间分布式冗余观测，弥补单车感知缺陷，有效抑制滤波误差累积，提升复杂场景下定图与建图的稳定性与精度。本文以城市结构化道路低速行驶场景为研究载体，搭建双自主车辆协同SLAM仿真系统，系统性实现四类主流SLAM算法框架，涵盖三类传感器观测模式与两种核心滤波架构。

本文算法体系包含三种基于Rao-Blackwellized粒子滤波（RBPF）的改进SLAM算法与一种全增广状态扩展卡尔曼滤波（Full EKF）SLAM算法，分别对应测距测角完备观测、纯方位受限观测、含最近邻数据关联的笛卡尔坐标线性观测三种感知工况，覆盖理想观测、低成本受限感知、未知目标关联的典型工程场景。同时，为挖掘多车协同感知优势，本文设计无融合、状态融合、观测融合、协方差交叉融合四类协同信息交互策略，实现双车观测与地图状态的互补修正。仿真基于统一的50m×50m城市仿真路网与50个离散环境路标，所有算法共用同一车辆运动模型、仿真时序与噪声参数，保证对比实验的公平性与有效性。

实验结果表明，RBPF凭借粒子非参数分布特性，能够更好适配车辆运动非线性与不确定性，位姿估计整体鲁棒性更优；全阶EKF依托全局协方差矩阵完整保留状态间误差相关性，在路标建图精度上具备小幅优势，但单一高斯分布假设无法精准刻画复杂位姿不确定性。在不同感知工况中，测距测角RBPF综合性能最为均衡；纯方位RBPF可小幅优化车辆定位精度，但存在路标深度不可观问题，建图误差显著升高；基于最近邻的数据关联算法受密集路标错配干扰，建图精度衰减最为明显。多传感器融合策略可有效整合双车冗余观测信息，进一步抑制噪声与累积误差，显著提升协同SLAM系统的整体稳定性与精度。

关键词：协同SLAM；RBPF粒子滤波；全阶EKF；纯方位观测；数据关联；传感器融合；自主车辆

1 绪论

1.1 研究背景与意义

随着自动驾驶与移动机器人技术的快速迭代，未知环境下的同步定位与地图构建技术已成为智能载体自主导航、环境感知、路径规划的底层核心支撑。传统单车SLAM依赖单一车载传感器获取环境观测信息，受硬件探测距离、观测视角遮挡、随机测量噪声与运动过程噪声影响，在长时间、长距离运行过程中会出现明显的位姿漂移与地图畸变，难以满足高精度、高稳定性的自主导航需求。尤其在城市结构化道路场景中，密集建筑、交通设施会进一步压缩单车有效感知范围，加剧观测缺失与误差累积问题。

多智能体协同SLAM技术通过多台自主车辆协同感知、信息共享、状态融合，打破了单车感知的局限性。多车分布式布设可形成多角度、大范围的环境观测覆盖，弥补单车视野盲区，通过多源观测冗余信息相互校准，有效抑制单一传感器噪声与滤波累积误差，显著提升定位与建图精度。相较于单车SLAM，双车协同SLAM具备更强的环境适应性、更高的容错率与更优的拓展性，是当前智能交通、集群机器人导航领域的研究热点。

目前主流SLAM滤波算法主要分为粒子滤波与卡尔曼滤波两大体系。其中Rao-Blackwellized粒子滤波通过状态解耦思想，将车辆位姿与路标地图状态分离求解，大幅降低高维联合状态的计算复杂度，依托非参数粒子分布适配非线性、非高斯噪声场景，工程适配性极强；全阶扩展卡尔曼滤波通过构建全局增广状态向量，完整保留所有状态间的误差互相关关系，建图信息完备性更高，但存在线性化误差、高斯假设局限与高维矩阵运算开销大的问题。两类算法的性能优劣、适用场景、感知适配性仍需系统性的对比验证。

基于此，本文搭建标准化双车协同SLAM仿真平台，分层设计多类感知模式与滤波架构，系统性对比不同算法、不同观测条件、不同融合策略下的定位建图性能，厘清各类算法的优势短板与适用工况，为城市道路场景下协同SLAM算法的选型、优化与工程落地提供可靠的仿真依据与理论支撑。

1.2 国内外研究现状

在SLAM滤波算法研究中，RBPF-SLAM凭借高效的状态解耦能力成为主流算法之一。该算法通过RB分解将复杂的联合后验概率拆分为车辆位姿后验与路标条件后验，以粒子集表征车辆位姿的不确定性，以轻量化EKF实现逐路标更新，兼顾计算效率与非线性适配能力，广泛应用于室外移动机器人与自动驾驶场景。但粒子滤波存在粒子退化、重采样误差、密集环境关联精度不足等问题，在复杂感知条件下性能受限。

全阶EKF-SLAM采用全局增广状态建模，将车辆位姿与全部环境路标纳入统一状态空间，通过完整协方差矩阵刻画状态间耦合关系，理论上能够实现全局最优估计。但该算法严格依赖高斯分布假设与一阶泰勒线性近似，在强非线性运动场景下易产生截断误差，引发滤波不一致问题，且高维协方差矩阵迭代运算会大幅提升算力开销，实时性较差。

在协同SLAM与传感器融合领域，多车信息融合主要分为状态融合、观测融合与协方差融合三类架构。状态融合对多智能体地图状态进行加权优化，观测融合直接复用原始观测冗余信息，协方差交叉融合摒弃统计独立假设，适配复杂关联场景，三类融合策略各有优劣，但目前缺乏针对RBPF与EKF两类滤波架构的系统性对比验证。同时，现有研究多聚焦于单一观测模式，对测距测角、纯方位、笛卡尔坐标等多类感知工况的横向对比研究较为匮乏。

1.3 研究内容与创新点

本文核心研究内容围绕多算法、多工况、多融合策略的双车协同SLAM对比实验展开，具体包括：1）搭建统一标准化仿真环境，完成双车运动建模与环境初始化，保证所有对比实验变量唯一；2）实现三类RBPF-SLAM算法，覆盖完备极坐标观测、纯方位受限观测、笛卡尔坐标数据关联观测场景；3）实现全阶增广EKF-SLAM算法，完成粒子滤波与卡尔曼滤波的机理与性能对比；4）设计四类传感器融合策略，验证多车协同信息共享的性能增益；5）通过量化误差指标，系统分析各类算法的优劣、局限性与适用场景。

本文主要创新点如下：第一，构建分层递进的算法验证体系，从观测完备度、数据关联难度、滤波架构三个维度完成全方位对比，弥补单一工况实验的局限性；第二，引入真实工程场景中的未知数据关联问题，通过最近邻门限算法模拟实际传感器目标匹配过程，提升仿真真实性；第三，系统性对比四类融合策略在不同滤波框架、不同感知模式下的适配性，明确不同融合算法的性能增益与适用条件；第四，量化分析RBPF与全阶EKF的性能取舍关系，厘清粒子滤波非参数优势与EKF全局相关性优势的工程价值。

1.4 论文章节安排

本文共分为十二个章节，具体结构如下：第一章为绪论，阐述研究背景、现状、内容与创新点；第二章介绍统一仿真场景与车辆运动学模型，明确仿真基础参数与环境配置；第三至第六章分别详细阐述四类SLAM算法的原理、观测模型、迭代逻辑与参数配置；第七章系统介绍双车协同多模式传感器融合策略的实现原理；第八章说明RBPF专用重采样机制与粒子优化策略；第九章介绍仿真工程架构与标准化运行流程；第十章汇总全工况仿真参数，形成完整参数体系；第十一章基于RMSE量化指标完成仿真结果分析与算法性能对比；第十二章总结全文研究成果，分析现有算法局限性并提出后续优化方向。

2 仿真场景与车辆运动建模

2.1 仿真环境配置

为保证所有算法对比实验的公正性与可复现性，本文四类SLAM算法均采用完全统一的仿真环境、运动参数与噪声配置。仿真区域为50m×50m的标准化城市道路场景，坐标范围约束在-25m至25m之间。环境内随机布设50个离散路标特征，所有路标均避开车辆行驶车道，模拟城市道路两侧交通标识、建筑边角、路缘石等可用于定位建图的自然特征点，贴合真实自动驾驶感知场景。

仿真平台搭载两台自主行驶车辆，两车沿预设顺时针闭合轨迹匀速行驶，标称行驶速度设置为2.4m/s。整体仿真流程包含100个离散时间步，单步仿真时长0.5s，总仿真时长50s，行驶轨迹包含三次90°右转动作，模拟城市路口转向的典型行驶工况。为复现真实车辆驱动系统的随机扰动特性，两车基于同一组参考控制指令，分别叠加独立的过程高斯噪声，保证车辆运动扰动的随机性与真实性。

车辆初始位姿差异化布设，一号车初始坐标为(-20m,20m)、航向角0rad，二号车初始坐标为(-10m,10m)、航向角0rad，形成差异化感知视角，为双车协同融合提供空间冗余信息。仿真随机种子固定为2025，彻底消除随机因素对实验结果的干扰。车载传感器最大有效探测距离为10m，仅处于探测范围内的路标可生成有效观测，符合常规车载感知设备的探测特性。

2.2 车辆运动学模型

本文采用移动机器人领域通用的独轮车运动学模型描述车辆平面运动，该模型能够精准表征低速自主车辆的行驶特性，适配城市道路匀速巡航与转向工况。模型以车辆全局坐标与航向角为状态量，结合线速度、角速度控制输入，实现离散时序状态递推。

车辆运动过程引入独立高斯过程噪声，分别对应线速度扰动与角速度扰动，模拟车辆电机调速误差、机械抖动等系统噪声。两类过程噪声参数固定，贯穿所有仿真工况，保证运动扰动条件统一。该运动模型结构简洁、非线性特性贴合实际，能够有效验证各类SLAM算法对运动非线性与随机噪声的适配能力。

3 测距测角RBPF-SLAM算法研究

3.1 RBPF核心滤波原理

Rao-Blackwellized粒子滤波是一种改进型粒子滤波算法，核心优势在于通过概率因式分解实现位姿与地图状态解耦，彻底解决传统粒子滤波高维状态计算量爆炸的问题。传统SLAM联合状态包含车辆位姿与海量路标坐标，直接求解维度极高、算力开销极大，而RBPF利用条件概率特性，将复杂的联合后验概率拆解为车辆位姿后验概率与给定位姿条件下的路标后验概率。

在该框架下，车辆时序位姿的不确定性通过加权粒子集表征，本文设置500个有效粒子，以非参数分布拟合车辆位姿的非线性、非高斯扰动特性，相比高斯滤波具备更强的场景适配性。同时，每个独立粒子配套一组专属EKF滤波器，单独完成每个路标的状态更新与协方差迭代，实现地图状态的轻量化、精细化求解，大幅降低全局计算复杂度。

3.2 测距测角观测模型

本章节采用车载激光雷达主流的极坐标观测模式，传感器同时输出车辆与路标的相对直线距离和相对方位角，是信息最完备、稳定性最高的感知方式。观测信息包含径向距离与切向方位，可全方位约束路标二维位置状态，不存在观测缺失与维度不足的问题。

观测过程叠加固定高斯测量噪声，分别对应测距误差与测角误差，贴合真实传感器测量特性。同时对输出方位角进行角度归一化处理，规避角度跳变带来的滤波突变问题，保证迭代过程平稳收敛。

3.3 滤波更新与权重迭代机制

基于传感器观测信息，算法对每个粒子、每个可视路标执行独立EKF更新，通过观测残差不断修正路标位置估计与不确定性协方差。同时，根据观测匹配精度实时更新粒子权重，观测拟合度越高的粒子权重越大，代表其表征的车辆位姿越接近真实状态。

该观测模式的核心优势为单次观测即可实现路标全状态可观，无需依赖车辆运动累积观测，滤波收敛速度快、稳定性强，无明显初始误差，是四类算法中综合感知条件最优的方案。

4 纯方位RBPF-SLAM算法研究

4.1 算法整体架构

纯方位RBPF-SLAM的粒子滤波框架、概率分解逻辑、粒子数量配置、重采样机制与测距测角RBPF完全一致，仅对传感器观测模型进行简化，取消测距维度，仅保留方位角观测，模拟单目视觉、低成本毫米波雷达等仅能测角、无法测距的轻量化感知设备工况，具备极强的工程实用价值。

4.2 纯方位观测特性

该算法仅以车辆与路标的相对方位角作为唯一观测信息，观测维度大幅简化，测量噪声仅保留方位角扰动，有效降低了观测噪声的维度复杂度。但单一方位观测存在天然的可观性缺陷，单次观测仅能将路标约束在以车辆为原点的射线上，无法识别路标距离，路标深度状态完全不可观测。

4.3 运动视差收敛机理

纯方位SLAM的路标收敛完全依赖车辆运动形成的视差效应。车辆行驶过程中不断变换位姿，从不同位置生成多组方位观测射线，多条射线的交点即可锁定路标真实位置。在仿真初期，由于观测数量不足、视差信息缺失，路标位置不确定性极大，地图误差较高；随着车辆持续行驶、有效观测累积，视差信息不断丰富，路标协方差逐步收敛，地图精度持续提升。

在可视化仿真中，由于无真实测距信息，所有方位射线统一绘制至传感器最大探测距离，直观体现纯方位观测的不确定性特征。

4.4 算法性能差异分析

相较于测距测角算法，纯方位RBPF观测维度更少、噪声参数更简单，对车辆位姿的约束更为纯粹，因此车辆定位误差小幅降低。但受深度不可观特性影响，路标估计收敛速度慢、残余误差大，整体建图精度显著低于完备观测模式，形成了“定位更优、建图偏弱”的性能特征。

5 基于最近邻数据关联的RBPF-SLAM算法研究

5.1 笛卡尔线性观测模型

本章节摒弃传统极坐标观测方式，采用笛卡尔相对直角坐标观测模型，传感器直接输出车辆与路标的相对横向、纵向坐标偏差。该观测方式为严格线性观测关系，无需复杂的非线性泰勒近似，不存在线性化截断误差，观测模型精度理论上高于极坐标非线性观测。

观测叠加二维直角坐标高斯噪声，均匀覆盖横向与纵向测量误差，贴合直角坐标感知设备的噪声特性。线性观测结构大幅简化了滤波迭代逻辑，规避了非线性迭代的精度损失问题。

5.2 最近邻+门限数据关联机制

前述两类RBPF算法均采用理想已知关联假设，默认传感器观测与环境路标一一对应、ID完全匹配，与真实工程场景差距较大。实际传感器无法识别目标路标身份，存在观测与目标匹配模糊的问题，因此数据关联是真实SLAM系统的核心难点。

本文采用最近邻匹配+5m距离门限校验的组合关联策略，重构算法循环逻辑，将传统“遍历路标匹配观测”的模式反向优化为“遍历观测匹配最优路标”。对每一帧有效观测，遍历视野内所有路标，计算预测观测与真实观测的欧氏距离，选取距离最小的路标作为最优匹配目标；同时设置5m距离门限，剔除距离过远的离谱匹配与杂波观测，避免错误关联污染地图数据。

5.3 算法局限性与优化方向

最近邻关联算法逻辑简单、计算效率高，适配实时性要求较高的工程场景，但存在固有缺陷。在本文50个路标密集布设的复杂环境中，相邻路标间距较小，传感器噪声易导致观测偏移，引发路标错配问题。错误的观测关联会持续修正错误的路标状态，造成地图漂移、精度退化，这也是本算法路标RMSE为三类RBPF最高的核心原因。

为平衡关联精度与算力，本章节将粒子数量由500缩减至200，同时优化路标初始协方差，缩小初始地图不确定性，辅助关联算法快速收敛。后续可替换为联合兼容分支定界、概率数据关联滤波等高精度关联算法，进一步提升密集环境下的建图稳定性。

6 全阶增广EKF-SLAM算法研究

6.1 全局增广状态建模思路

全阶EKF-SLAM彻底摒弃RBPF的状态解耦思想，采用全局统一增广状态建模方式，将车辆三维位姿与全部50个路标二维坐标整合为单一高维状态向量，完整覆盖所有待估计状态。对应的协方差矩阵完整保留车辆位姿、各路标之间的误差互相关关系，所有状态的不确定性耦合信息全部显性存储，从理论上实现了SLAM状态信息的完备建模。

该算法是传统SLAM的经典最优框架，核心优势在于任意单一观测的更新，都可以通过协方差耦合关系同步修正车辆位姿与所有关联路标状态，实现全局状态协同优化。

6.2 预测与更新迭代逻辑

在状态预测阶段，仅车辆位姿根据运动学模型实时更新，环境路标作为静止特征点，坐标保持恒定，符合真实道路环境特征。状态转移矩阵采用分块对角结构，精准区分车辆动态变化与路标静态不变的特性，简化高维矩阵运算复杂度。同时将二维运动过程噪声映射至全局高维状态空间，精准刻画噪声在全局系统中的传播规律。

在观测更新阶段，采用逐路标串行更新策略，对每一个观测到的路标，构建稀疏观测矩阵，仅激活车辆位姿与当前路标对应的状态维度，在保证更新精度的同时降低运算开销。通过卡尔曼增益完成全局状态与协方差的同步修正，充分利用状态间的相关性实现全局最优估计。

6.3 EKF与RBPF算法性能取舍分析

全阶EKF与RBPF存在显著的性能取舍关系。在信息完备性上，全阶EKF显性保留全局状态互相关性，建图信息更完整，路标估计精度小幅占优；在不确定性表征上，全阶EKF全程采用单一高斯分布假设，无法适配车辆运动的非高斯、强非线性特性，位姿估计灵活性与鲁棒性弱于非参数粒子滤波。

在计算复杂度上，RBPF计算量随路标数量线性增长，算力开销可控；全阶EKF协方差运算量随状态维度平方增长，路标数量越多，算力代价越高，实时性越差。在滤波一致性上，RBPF充足粒子条件下可长期保持滤波稳定，而全阶EKF的一阶线性化误差会持续累积，长期运行易出现滤波不一致问题。

7 双车协同多模式传感器融合策略

为充分发挥双车协同感知优势，挖掘多平台观测冗余信息，本文设计四类融合模式，通过开关参数自由切换，实现双车信息独立运行与协同融合的灵活适配。所有融合策略仅在两台车辆同时观测到同一路标时触发，保证观测信息的有效性与互补性，1.4全阶EKF模块预留标准化融合接口，保证算法体系统一性。

7.1 无融合基准模式

无融合模式作为对照组基准，两台车辆完全独立运行完整SLAM算法，无任何观测数据、地图状态、协方差信息的交互与共享，所有误差与收敛特性完全由单车算法决定，为其余融合策略的性能增益评估提供参照标准。

7.2 状态加权融合

状态融合以两车独立输出的路标最优估计与协方差为基础，基于卡尔曼最优加权准则完成状态融合优化。该策略假设两车观测估计相互统计独立，通过协方差权重分配，为精度更高、不确定性更小的状态分配更大权重，实现双车地图状态的最优融合修正，有效平均单车估计误差，提升地图状态的整体精度。

7.3 原始观测融合

观测融合直接拼接两车原始观测数据，构建联合高维观测向量，等效于单次双冗余高精度观测。该策略不依赖单车滤波结果，直接从数据源层面实现信息融合，最大程度保留原始感知信息，避免滤波迭代带来的信息损耗，精度提升幅度在四类策略中最为显著。

7.4 协方差交叉融合

前述融合策略均依赖统计独立假设，在两车观测视角重叠、信息耦合的场景下存在局限性。协方差交叉融合摒弃独立假设，通过自适应权重优化算法，求解最优融合权重使整体不确定性最小化，输出一致性最优的融合状态与协方差。该策略鲁棒性最强、适配场景最广，能够有效应对复杂关联、观测耦合的协同感知场景，稳定性优于传统加权融合。

8 RBPF粒子重采样机制

粒子退化是粒子滤波的固有问题，迭代过程中大量粒子权重趋近于零，仅少数粒子主导估计结果，导致有效粒子数量不足、滤波精度下降。本文采用序贯重要性重采样算法，仅在本帧存在有效路标更新、粒子权重发生变化时触发重采样，避免无效运算，提升算法实时性。

算法根据粒子总权重划分三类自适应处理策略：当权重总和极大时，判定为严重退化，仅保留最优粒子；当权重总和处于正常区间时，执行标准权重归一化与重采样，复制高权重有效粒子、剔除低权重无效粒子；当权重总和趋近于零时，判定为滤波失效，统一重置所有粒子权重，保证滤波持续有效。该机制有效抑制粒子退化问题，保障RBPF滤波的稳定性与收敛性。

9 仿真工程架构与运行流程

9.1 分层工程架构

本文仿真工程采用模块化分层设计，四类算法独立封装文件夹，各模块功能清晰、互不干扰，同时共用核心公共脚本，保证代码复用性与实验统一性。工程包含四大算法子模块，分别对应测距测角RBPF、纯方位RBPF、数据关联RBPF、全阶EKF-SLAM，每个子模块独立配置观测函数与核心滤波逻辑。公共脚本统一实现车辆运动递推、粒子重采样、多模式融合、误差分析绘图等通用功能，最大程度保证实验变量唯一。

9.2 标准化仿真流程

仿真运行流程标准化、规范化，首先切换至对应算法目录，配置融合模式参数，随后运行主程序完成环境初始化、车辆状态初始化、滤波器初始化；迭代过程中逐帧完成运动预测、观测采集、滤波更新、粒子优化、信息融合；仿真结束后自动调用误差分析模块，输出车辆位姿RMSE时序曲线、路标RMSE时序曲线、协方差范数变化曲线，全方位量化算法性能。

10 仿真参数体系

本文所有仿真参数统一标准化，仅根据算法特性微调差异化参数，核心环境参数、运动参数、噪声参数全局统一。固定参数包含仿真尺寸、路标数量、仿真步长、传感器探测距离、过程噪声、观测噪声等；差异化参数包含粒子数量、路标初始协方差、数据关联门限等，适配不同算法的运行特性。完整参数体系保证了四类算法对比实验的科学性、严谨性与可复现性。

11 仿真结果与量化分析

本文采用均方根误差（RMSE）作为核心量化指标，分别统计全程车辆位姿平均RMSE与路标建图平均RMSE，精准表征各类算法的定位精度与建图精度，所有结果均基于无融合基准工况，保证对比公平性。

11.1 测距测角RBPF结果分析

测距测角RBPF凭借完备的观测信息，无可观性缺陷、无数据关联误差，综合性能最为均衡。一号车平均定位RMSE为0.7689m，路标RMSE为7.9728m；二号车视野感知条件更优，定位RMSE低至0.5595m，路标RMSE为8.1010m。该算法定位与建图误差处于中等偏优水平，收敛稳定、无明显漂移，是三类RBPF中综合性能最优的方案。

11.2 纯方位RBPF结果分析

纯方位观测模式下，车辆定位精度进一步优化，一号车、二号车位姿RMSE分别降至0.6322m、0.4933m，得益于观测噪声维度简化，位姿估计更为精准。但受深度不可观与视差收敛滞后影响，建图精度显著下降，两路标RMSE分别升至8.2970m与8.6064m，体现了纯方位SLAM“定位优、建图弱”的典型特性。

11.3 最近邻数据关联RBPF结果分析

受密集路标环境错配问题影响，本算法整体精度最差，是三类RBPF中唯一出现明显精度退化的方案。一号车位姿RMSE达到0.9802m，路标RMSE高达8.9475m；二号车依托优质行驶视野，定位精度仍保持0.4769m的最优水平，但路标RMSE仍处于8.8344m的高位。实验结果充分证明，数据关联误差是密集环境下SLAM建图精度衰减的核心诱因。

11.4 RBPF与全阶EKF横向对比分析

在相同测距测角观测条件下，全阶EKF与RBPF呈现明显性能取舍。全阶EKF凭借全局协方差耦合优势，一号车路标RMSE降至7.9145m，建图精度小幅优于RBPF；但单一高斯假设无法适配复杂位姿不确定性，二号车位姿RMSE升至0.7381m，定位精度显著劣于粒子滤波。该结果验证了EKF建图信息完备、RBPF位姿适配性更强的核心特性。

11.5 融合策略增益分析

多组融合实验表明，三类协同融合策略均可有效提升双车SLAM系统精度与稳定性。观测融合依托原始数据冗余，精度提升幅度最大；协方差交叉融合适配性最强、鲁棒性最优，可适配复杂观测耦合场景；状态融合计算量最小、实时性最优。所有融合模式均可有效抑制误差累积，弥补单车感知缺陷，实现协同感知增益。

12 总结与展望

12.1 全文总结

本文基于统一仿真平台，系统性完成三类RBPF-SLAM与一类全阶EKF-SLAM算法的建模、实现与对比实验，覆盖完备观测、受限观测、未知数据关联三类典型工程场景，同时验证了四类多车协同融合策略的性能增益。实验结论如下：测距测角RBPF观测信息完备、无关联误差、收敛稳定，综合定位建图性能最优；纯方位RBPF可优化车辆定位精度，但存在深度不可观缺陷，建图精度较差；最近邻数据关联算法受密集路标错配干扰，整体建图精度显著退化；全阶EKF全局状态相关性建模完备，建图精度小幅占优，但高斯假设限制了位姿不确定性表征能力，定位灵活性弱于RBPF；多车传感器融合可有效整合冗余观测信息，全方位提升协同SLAM系统的精度与鲁棒性。

12.2 未来展望

针对本文研究存在的局限性，后续可从三方面开展优化研究：一是优化数据关联算法，替换最近邻算法为JCBB、PDAF等高鲁棒性关联方案，解决密集路标环境错配问题；二是拓展复杂噪声与非线性工况，进一步验证两类滤波算法的一致性与鲁棒性；三是拓展多车集群协同架构，搭建分布式融合SLAM系统，降低高维状态运算开销，适配大规模集群智能体导航场景。同时可结合真实车载传感器数据，完成仿真算法的实物落地验证，进一步提升算法的工程应用价值。

📚第二部分——运行结果

2.1 算例1

编辑

编辑

编辑

编辑

2.2 算例2

编辑

编辑

编辑

2.3 算例3

2.4 算案4

由于运行结果图比较多，就不一一展示。

%% Car Trajectoryfigure; hold on;title('Car Trajectories');plot(real_car1(1,:),real_car1(2,:),'r');plot(real_car2(1,:),real_car2(2,:),'b');plot(pos_car1(1,:),pos_car1(2,:),'r:');plot(pos_car2(1,:),pos_car2(2,:),'b:');grid on; xlabel('x (m)'); ylabel('y (m)');axis([-30, 30, -30, 30]);legend('True Car1','True Car2','Estimated Car1','Estimated Car2');%% Car Position Errorerror_car1 = pos_car1-real_car1;error_car2 = pos_car2-real_car2;rmse_car1 = sqrt(sum(error_car1.^2,1));rmse_car2 = sqrt(sum(error_car2.^2,1));figure; set(gcf,'position',[0 0 500 1000]);subplot(311); hold on;title('Car Estimation Error');plot(time,rmse_car1,'r');plot(time,rmse_car2,'b');grid on; xlabel('Time (s)'); ylabel('RMSE [m]'); legend('Car1','Car2');disp(['Average Car1 Position Estimation Error: ',num2str(mean(rmse_car1))]);disp(['Average Car2 Position Estimation Error: ',num2str(mean(rmse_car2))]);%% Landmark Position Errorerror_landmark1 = pos_landmark1-real_landmarks;error_landmark2 = pos_landmark2-real_landmarks;error_landmark1 = reshape(error_landmark1(:),[2*num_landmarks,timesteps]);error_landmark2 = reshape(error_landmark2(:),[2*num_landmarks,timesteps]);rmse_landmark1 = sqrt(sum(error_landmark1.^2,1));rmse_landmark2 = sqrt(sum(error_landmark2.^2,1));subplot(312); hold on; title('Landmark Estimation Error');plot(time,rmse_landmark1,'r');plot(time,rmse_landmark2,'b');grid on; xlabel('Time (s)'); ylabel('RMSE [m]'); legend('Car1','Car2');disp(['Average Landmark Estimation Error from Car1: ',num2str(mean(rmse_landmark1))]);disp(['Average Landmark Estimation Error from Car2: ',num2str(mean(rmse_landmark2))]);if fusion_flag>0    error_landmark_SF = pos_landmark_SF-real_landmarks;    error_landmark_SF = reshape(error_landmark_SF(:),[2*num_landmarks,timesteps]);    rmse_landmark_SF = sqrt(sum(error_landmark_SF.^2,1));    plot(time,rmse_landmark_SF,'g');    legend('Car1','Car2','Fusion');    disp(['Average Landmark Estimation Error from Fusion: ',num2str(mean(rmse_landmark_SF))]);end%% Covariance Matrixcov_landmark1 = reshape(cov_landmark1(:),[4*num_landmarks,timesteps]);cov_landmark2 = reshape(cov_landmark2(:),[4*num_landmarks,timesteps]);sum_cov_landmark1 = sqrt(sum(cov_landmark1.^2,1));sum_cov_landmark2 = sqrt(sum(cov_landmark2.^2,1));subplot(313); hold on; title('Landmark Covariance');plot(time,sum_cov_landmark1,'r');plot(time,sum_cov_landmark2,'b');grid on; xlabel('Time (s)'); ylabel('Covariance [m^2]'); legend('Car1','Car2');if fusion_flag>0    cov_landmark_SF = reshape(cov_landmark_SF(:),[4*num_landmarks,timesteps]);    sum_cov_landmark_SF = sqrt(sum(cov_landmark_SF.^2,1));    plot(time,sum_cov_landmark_SF,'g');    legend('Car1','Car2','Fusion');end

🎉第三部分——参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。(文章内容仅供参考，具体效果以运行结果为准)