（9-1）多模态融合理论与方法：低层融合

在本章内容中，系统介绍了机器人与智能系统中多传感器、多数据源信息的融合技术。涵盖低层融合的原始数据与特征层处理、中层融合在SLAM、导航与避障中的应用、高层融合的决策策略及高维多模态Transformer模型，同时深入讲解了不确定性建模的知识，包括卡尔曼滤波族与贝叶斯图优化方法。通过本章学习，读者可以掌握从数据采集到特征提取再到决策优化的完整多模态融合流程，理解不同融合层次的特点与适用场景，为构建鲁棒、高效的智能系统奠定理论与方法基础。

9.1 低层融合

在多模态感知系统中，传感器获取的原始数据往往存在异质性与噪声。低层融合通过在数据或特征的最早阶段进行整合，不仅能够充分利用各类传感器的互补信息，还能提升整体感知的精度与鲁棒性，为中层与高层融合奠定基础。

9.1.1 原始数据融合

原始数据融合是低层多模态融合的基础方法，其核心思想是直接对来自不同传感器

的原始观测信号

进行整合，而不依赖于高层特征提取或语义理解。设系统中有

种传感器，其观测向量为：

融合的目标是通过映射函数

将多模态数据对齐并整合为统一表示：

其中，

为低层融合输出，可作为特征提取或高层决策的输入。原始数据融合的优势在于最大限度保留原始信息，减少信息损失，同时为后续的中层和高层融合提供可靠基础。

1. 理论基础

原始数据融合可形式化为多传感器状态估计问题。若传感器观测噪声满足高斯分布并相互独立，可将融合问题表述为最小方差无偏估计（MVUE）：

其中，

为传感器

的协方差矩阵。优化解为加权融合公式：

该公式反映了不同传感器的可信度分配，协方差越小，融合权重越高，从而实现最优状态估计。

2. 时间同步（Temporal Alignment） 技术

在多传感器系统中，不同传感器采样频率不同且存在时延

，直接融合会导致时间错位，从而引入误差。时间同步的目标是将所有传感器数据映射到统一的参考时间

。

（1）数学描述：设传感器

在采样时刻

获取数据

，目标是得到参考时刻

的估计

：

（2）插值方法：

线性插值：适用于平滑信号，如IMU加速度数据。

多项式插值：适用于非线性变化较快的信号，如角速度。
样条插值（Spline）：平滑度高，适合连续时间建模。

（3）注意事项：

对高频传感器（IMU）与低频传感器（相机）融合时，需要确保插值不会引入噪声放大。
硬件同步（如触发信号）可减少软件插值误差，但增加系统复杂度。

3. 空间配准（Spatial Registration） 技术

不同传感器观测的坐标系不同，需要将数据映射到统一的世界坐标系或参考坐标系。空间配准是原始数据融合的核心。

（1）刚体变换：该变换用于将各传感器的观测统一到同一参考坐标系，使得来自不同传感器的空间信息可以进行直接比较和融合。

其中，

为旋转矩阵，

为平移向量。

对IMU、相机、LiDAR等多传感器系统，需要精确标定外参矩阵

。

（2）点云-点云配准：使用迭代最近点（ICP, Iterative Closest Point）算法优化变换矩阵

：

其中，

为参考点云，

为待配准点云。

（3）投影融合：将3D点云投影到相机像素平面：

其中，

为相机内参矩阵，

为外参矩阵。

注意：外参标定精度对融合结果影响最大，微小误差会导致融合失败。对动态场景 来说 ，空间配准需结合时间同步，否则会出现运动模糊。

4. 滤波与去噪（Filtering & Denoising） 技术

原始数据往往含有噪声或异常值，直接融合可能放大误差，因此需要滤波处理。

（1）卡尔曼滤波（KF）

针对线性高斯系统：为了在噪声干扰下对系统状态进行最优估计，假设系统状态和观测满足线性高斯模型，状态转移和观测过程可表示为：

其中，

为系统状态，

为观测向量，

与

分别为过程噪声和观测噪声，且均服从高斯分布。

卡尔曼滤波公式：卡尔曼滤波通过结合预测值和观测值，利用加权最小方差原则更新状态估计：

其中，

为预测状态，

为更新后的状态，

为观测残差。

对IMU、轮速计等连续测量传感器，KF可以动态抑制噪声。

（2）扩展卡尔曼滤波（EKF）与无迹卡尔曼滤波（UKF）

适用于非线性系统，如IMU+LiDAR位姿融合。
将非线性状态方程线性化（EKF）或通过Sigma点传播（UKF）估计状态与协方差。

（3）图像与点云滤波

双边滤波：在保留边缘的前提下平滑图像。
体素滤波（Voxel Grid）：对点云进行下采样，减少数据量并平滑噪声。

（4）注意事项：

滤波器参数（如过程噪声

和观测噪声

）必须根据传感器特性调优，否则可能导致估计偏差。
对高速动态环境，滤波延迟需要评估，避免影响实时性。

5. 数据融合策略（Fusion Strategy）

（1）加权融合（Weighted Fusion）：假设各传感器误差协方差矩阵为

，则融合状态为：

权重由传感器可信度决定，协方差越小，权重越高。

（2）最小二乘融合（Least Squares Fusion）：将融合问题表述为：

可以推广至非线性测量

：

对应的数值求解可用高斯-牛顿或Levenberg-Marquardt方法。

（3）跨模态投影融合

将高维传感器数据映射到公共空间，例如点云投影到图像平面或深度图映射到LiDAR坐标系，保证数据空间一致性。

（4）注意事项：

融合策略需结合应用场景，实时性要求高时，可能优先选择加权融合而非最小二乘优化。
跨模态映射需考虑分辨率差异与遮挡问题，否则可能引入误差。

总之，原始数据融合强调在信息链条最前端整合多模态数据，通过时间同步、空间配准、滤波去噪和融合策略实现多源观测的最优组合。它不仅是低层多模态融合的理论基础，也为中高层特征提取与决策提供高质量输入，是智能机器人、无人车和无人机系统不可或缺的技术环节。

9.1.2 特征层融合

在低层多模态融合中，原始数据融合通过直接整合传感器观测实现信息的早期融合，而特征层融合则进一步提升了融合效率和表达能力。其核心思想是先从每个传感器获取经过预处理和特征提取后的向量或特征图，然后在特征层进行融合。相比直接处理原始数据，特征层融合能够在保留关键信息的同时显著降低数据维度，并在一定程度上抑制噪声的影响，从而提高多模态系统的计算效率和鲁棒性。设第

个传感器在时刻

提取特征向量

，其维度为

，则特征融合的过程可以形式化为：

其中，

为特征融合函数，可以是简单的拼接、加权组合，也可以是非线性映射或深度学习网络实现的复杂融合操作。融合后的特征

将作为中层或高层处理模块的输入，为环境理解、决策制定和控制提供高质量的表征。

1. 理论基础

特征层融合可以从统计学和信息论角度进行分析。假设每个传感器提取的特征向量独立且服从高斯分布：

为了得到最优线性组合，使融合后的特征方差最小且无偏，可以采用加权融合的方法：

其中，权重矩阵

可以由特征协方差矩阵

决定：

该方法在统计学上属于最优线性无偏估计（BLUE, Best Linear Unbiased Estimator），能够在不同特征质量和可信度差异显著的情况下实现信息的最优整合。

2. 特征提取（Feature Extraction） 技术

特征提取是特征层融合的前提和基础，其目的是将高维的原始数据压缩为低维、信息丰富的特征向量，从而便于后续融合和处理。不同类型传感器特征提取方法如下：

（1）视觉特征：对于图像和视频数据，可以使用传统计算机视觉方法如SIFT、ORB、HOG 等，也可以采用深度卷积神经网络（CNN）提取特征。假设卷积层输出特征图为

，通过全局平均池化或Flatten操作，可得到特征向量：

（2）点云特征：对于激光雷达或深度传感器生成的三维点云，可通过 PFH（Point Feature Histograms）、FPFH（Fast PFH）或基于 voxel 的 3D 卷积提取局部或全局特征，得到特征向量

。

（3）惯性传感器特征：对IMU数据，可通过时间序列统计特征（均值、方差、峰度等）或频域特征（傅里叶变换或小波变换）生成特征向量

。这些特征在运动状态估计中尤为重要。

特征提取的目标是将各类传感器信息转化为统一可处理的低维向量，同时保留最有价值的环境或运动信息，从而为后续的融合和推理提供可靠基础。

3. 特征对齐（Feature Alignment） 技术

在融合之前，不同传感器的特征往往在维度、时间或空间尺度上存在差异，因此需要进行对齐操作：

（1）维度对齐：不同特征维度可以通过线性映射或投影变换到统一空间：

这种线性映射可以确保不同模态的特征在同一维度上可比较，并可以进行后续的加权或非线性融合。

（2）时间对齐：对于时间序列特征，需要使用插值或循环缓冲方法将不同采样频率的特征对齐到统一时间步：

特征对齐的意义在于保证多模态特征在同一语义空间和时间尺度上具有一致性，使融合操作能够有效利用不同传感器的互补信息。

4 . 特征融合策略（Feature Fusion Strategy） 技术

（1）拼接融合（Concatenation）：在特征层融合中，最直观且实现成本最低的方法是将来自不同传感器或模态的特征向量直接组合成一个更高维的联合特征表示。通过这种方式，各模态特征的信息能够被完整保留，为后续模型提供最充分的输入，其数学形式可表示为：

这种方法简单直接，能够保留每个传感器的完整特征，但可能导致维度过高，需要后续降维处理或稀疏化策略。

（2）加权融合（Weighted Sum）：在多模态特征层融合中，不同传感器或特征通道在精度、稳定性和环境适应性方面往往存在差异，因此需要通过合理的权重分配来反映各特征在当前任务中的相对重要性。加权融合方法通过对各模态特征赋予不同权重，实现信息的最优线性组合，其数学形式可表示为：

其中，权重

可以通过传感器可靠性、特征置信度或注意力机制自适应计算：

其中，

为特征映射函数，计算每个模态在当前环境下的重要性。

（3）降维融合（Dimensionality Reduction Fusion）：将拼接后的高维特征向量通过PCA、LDA 或自编码器降维：

降维不仅减小计算量，还可以滤除冗余或噪声特征，提高融合特征的稳定性。

（4）非线性映射融合（Non-linear Mapping Fusion）：使用多层感知机（MLP）或卷积网络实现不同模态间的复杂非线性关系：

这种方法可以捕捉特征间的非线性相关性，使融合特征在语义表达上更为丰富和鲁棒。

5 . 特征层融合的应用

（1）SLAM（同步定位与建图）：

融合视觉特征

与深度或LiDAR特征

构建高鲁棒性地图。
特征融合增强了系统在光照变化、遮挡或低纹理环境下的稳定性。

（2）机器人导航与避障：

通过融合激光雷达、IMU 和视觉特征实现精确位姿估计和环境感知。
特征层融合可以过滤冗余信息，提高决策模块的计算效率。

（3）自动驾驶：

将车辆前视摄像头的 CNN 特征、LiDAR 点云特征和毫米波雷达特征融合，用于目标检测、轨迹预测和行为规划。
特征融合增强了对复杂场景的理解能力，提高行驶安全性。

总而言之，特征层融合在低层融合中起承上启下作用。相比原始数据融合，它在保留多模态信息的同时降低了维度，提高了计算效率，并通过加权、注意力或非线性映射增强了特征表达能力。在理论上，它可以看作是不同观测特征的最优组合，而在工程实践中，它在SLAM、机器人导航和自动驾驶等应用中起到关键作用，为中高层决策提供可靠、信息丰富的输入。

9.1.3 实战演练：人形机器人多模态低层融合实践

本实例展示了人形机器人多模态低层融合的过程，涵盖了原始数据融合（含时间同步、空间配准、协方差加权融合）与特征层融合（含特征提取、维度对齐、拼接/加权/PCA/MLP多策略融合），通过可视化图直观展示各环节效果，既落地了多模态融合理论，也为实际机器人感知系统的低层数据处理提供了可参考的流程示例。

实例9-1 ：基于Open3D 的点云预处理与ICP 配准（源码路径：codes\9\Di.py ）

实例文件Di.py的主要实现流程如下所示。

（1）下面代码的功能是生成人形机器人多传感器模拟原始数据，涵盖IMU（高频带高斯噪声）、相机（低频带噪声且时间偏移）、LiDAR（中频带噪声且空间偏移）三类传感器数据，同时定义各传感器的协方差矩阵，为后续低层融合提供贴合真实场景特征的数据源。

python 复制代码

# -------------------------- 1. 模拟传感器数据生成 --------------------------
def generate_sensor_data():
    """生成带噪声、时间偏移的多传感器原始数据"""
    # 基础时间轴（100ms步长，共5秒）
    t_base = np.linspace(0, 5, 50)
    
    # 1. IMU数据（高频：100Hz，带高斯噪声）
    t_imu = np.linspace(0, 5, 500)  # IMU采样频率100Hz
    acc_x = np.sin(2 * np.pi * 0.5 * t_imu) + np.random.normal(0, 0.1, len(t_imu))  # 加速度x
    gyro_z = np.cos(2 * np.pi * 0.3 * t_imu) + np.random.normal(0, 0.08, len(t_imu))  # 角速度z
    imu_data = np.vstack([acc_x, gyro_z]).T  # (500, 2)
    imu_cov = np.diag([0.1**2, 0.08**2])  # IMU协方差矩阵（理论9.1.1中的R_i）
    
    # 2. 相机数据（低频：10Hz，带噪声，时间偏移+0.05s）
    t_cam = np.linspace(0.05, 5.05, 50)  # 相机采样频率10Hz，时间偏移0.05s
    cam_pixel = np.random.normal(100, 20, (len(t_cam), 2))  # 特征点像素坐标 (50, 2)
    cam_cov = np.diag([20**2, 20**2])  # 相机协方差矩阵
    
    # 3. LiDAR数据（中频：20Hz，带噪声，空间偏移）
    t_lidar = np.linspace(0.02, 5.02, 100)  # LiDAR采样频率20Hz，时间偏移0.02s
    lidar_xyz = np.random.normal(0, 1, (len(t_lidar), 3))  # 点云坐标 (100, 3)
    lidar_cov = np.diag([1**2, 1**2, 1**2])  # LiDAR协方差矩阵
    
    return {
        "t_base": t_base, "t_imu": t_imu, "imu_data": imu_data, "imu_cov": imu_cov,
        "t_cam": t_cam, "cam_data": cam_pixel, "cam_cov": cam_cov,
        "t_lidar": t_lidar, "lidar_data": lidar_xyz, "lidar_cov": lidar_cov
    }

（2）下面代码的功能是实现原始数据融合核心逻辑，包含时间同步（通过线性插值将不同传感器数据对齐到基准时间轴）、空间配准（通过刚体变换统一LiDAR点云坐标系）、加权融合（基于协方差矩阵计算权重实现多传感器数据最优融合），落地原始数据融合的关键理论方法。

python 复制代码

# -------------------------- 2. 原始数据融合模块 --------------------------
def temporal_alignment(t_target, t_source, data_source):
    """
    时间同步（理论9.1.1的时间对齐）
    :param t_target: 目标时间轴
    :param t_source: 源数据时间轴
    :param data_source: 源数据 (N, d)
    :return: 对齐后的数据 (len(t_target), d)
    """
    aligned_data = []
    for dim in range(data_source.shape[1]):
        # 线性插值（适用于IMU等平滑信号，对应9.1.1的线性插值公式）
        f = interpolate.interp1d(t_source, data_source[:, dim], kind="linear", fill_value="extrapolate")
        aligned_data.append(f(t_target))
    return np.vstack(aligned_data).T

def spatial_registration(lidar_data, R=None, t=None):
    """
    空间配准（理论9.1.1的刚体变换）
    :param lidar_data: LiDAR原始点云 (N, 3)
    :param R: 旋转矩阵 (3,3)，默认绕z轴旋转5度
    :param t: 平移向量 (3,)，默认平移[0.1, 0.05, 0]
    :return: 配准后的点云 (N, 3)
    """
    # 默认刚体变换参数（模拟外参标定结果）
    if R is None:
        theta = np.deg2rad(5)  # 旋转5度
        R = np.array([[np.cos(theta), -np.sin(theta), 0],
                      [np.sin(theta), np.cos(theta), 0],
                      [0, 0, 1]])
    if t is None:
        t = np.array([0.1, 0.05, 0])
    
    # 刚体变换：x_world = R*x_sensor + t（对应9.1.1的空间配准公式）
    registered_data = (R @ lidar_data.T).T + t
    return registered_data

def weighted_fusion(aligned_datas, cov_matrices):
    """
    加权融合（理论9.1.1的最优加权融合公式）
    :param aligned_datas: 时间/空间对齐后的各传感器数据列表 [(T, d1), (T, d2), ...]
    :param cov_matrices: 各传感器协方差矩阵列表 [R1, R2, ...]
    :return: 融合后的结果 (T, d)
    """
    # 统一维度（这里简化为取前2维）
    d = 2
    aligned_datas = [data[:, :d] for data in aligned_datas]
    cov_matrices = [cov[:d, :d] for cov in cov_matrices]
    
    # 计算权重：W = (ΣR_i⁻¹)⁻¹ * R_i⁻¹（对应9.1.1的加权融合公式）
    inv_cov_sum = sum([inv(cov) for cov in cov_matrices])
    inv_cov_sum_inv = inv(inv_cov_sum)
    
    # 逐时间步融合
    fusion_result = []
    for t in range(len(aligned_datas[0])):
        weighted_sum = sum([inv(cov) @ aligned_datas[i][t] for i, cov in enumerate(cov_matrices)])
        fusion_t = inv_cov_sum_inv @ weighted_sum
        fusion_result.append(fusion_t)
    return np.array(fusion_result)

（3）下面代码的功能是完成特征层融合全流程，先针对IMU、相机、LiDAR提取专属特征，再通过线性映射统一特征维度，归一化协方差矩阵避免计算异常，最终实现拼接、加权、PCA降维、MLP非线性四种特征融合策略，覆盖特征层融合核心技术。

python 复制代码

# -------------------------- 3. 特征层融合模块 --------------------------
def feature_extraction(sensor_data, sensor_type):
    """
    特征提取（理论9.1.2的特征提取技术）
    :param sensor_data: 原始数据 (N, d)
    :param sensor_type: 传感器类型 ["imu", "cam", "lidar"]
    :return: 提取的特征 (N, feat_dim)
    """
    if sensor_type == "imu":
        # IMU：时间序列统计特征（均值、方差、峰度）
        window_size = 10
        features = []
        for i in range(0, len(sensor_data)-window_size, window_size):
            window = sensor_data[i:i+window_size]
            mean = np.mean(window, axis=0)
            var = np.var(window, axis=0)
            kurt = np.mean(((window - mean)/np.std(window))**4, axis=0) - 3  # 峰度
            features.append(np.hstack([mean, var, kurt]))
        return np.array(features)
    
    elif sensor_type == "cam":
        # 相机：模拟HOG特征（降维到5维）
        features = np.random.normal(0, 1, (len(sensor_data), 5))
        return features
    
    elif sensor_type == "lidar":
        # LiDAR：模拟FPFH特征（降维到8维）
        features = np.random.normal(0, 1, (len(sensor_data), 8))
        return features

def feature_alignment(features_list):
    """
    特征对齐（理论9.1.2的维度对齐）
    :param features_list: 各传感器特征列表 [(N, d1), (N, d2), (N, d3)]
    :return: 对齐后的特征列表（统一维度为10）
    """
    aligned_features = []
    target_dim = 10
    for feat in features_list:
        # 线性映射：f̃_i = W_i*f_i + b_i（对应9.1.2的维度对齐公式）
        W = np.random.normal(0, 0.1, (target_dim, feat.shape[1]))
        b = np.random.normal(0, 0.01, target_dim)
        aligned_feat = (W @ feat.T).T + b
        aligned_features.append(aligned_feat)
    return aligned_features

def normalize_covariance(cov_mat, target_dim=10):
    """
    统一协方差矩阵维度（填充小噪声值避免奇异）
    :param cov_mat: 原始协方差矩阵
    :param target_dim: 目标维度
    :return: 扩展后的协方差矩阵 (target_dim, target_dim)
    """
    new_cov = np.eye(target_dim) * 1e-6  # 初始化为小噪声单位矩阵
    # 填充原始协方差矩阵的值
    h, w = cov_mat.shape
    new_cov[:h, :w] = cov_mat
    return new_cov

def feature_fusion_strategies(aligned_features, cov_matrices):
    """
    特征融合策略（理论9.1.2的4种策略）
    :param aligned_features: 对齐后的特征列表 [(N, 10), (N, 10), (N, 10)]
    :param cov_matrices: 协方差矩阵列表 [R1, R2, R3]
    :return: 不同策略的融合结果
    """
    # 统一协方差矩阵维度为10x10
    target_dim = 10
    normalized_covs = [normalize_covariance(cov, target_dim) for cov in cov_matrices]
    
    # 1. 拼接融合（Concatenation）
    concat_fusion = np.hstack(aligned_features)
    
    # 2. 加权融合（Weighted Sum，BLUE估计）
    inv_cov_sum = sum([inv(cov) for cov in normalized_covs])
    inv_cov_sum_inv = inv(inv_cov_sum)
    weights = [inv_cov_sum_inv @ inv(cov) for cov in normalized_covs]
    
    # 逐样本计算加权融合
    weighted_fusion = []
    for i in range(len(aligned_features[0])):
        feat_sum = sum([weights[j] @ aligned_features[j][i] for j in range(len(aligned_features))])
        weighted_fusion.append(feat_sum)
    weighted_fusion = np.array(weighted_fusion)
    
    # 3. 降维融合（PCA）
    pca = PCA(n_components=2)
    pca_fusion = pca.fit_transform(concat_fusion)
    
    # 4. 非线性融合（MLP）
    mlp = MLPRegressor(hidden_layer_sizes=(20, 10), max_iter=500, random_state=42)
    mlp.fit(concat_fusion, weighted_fusion)
    nonlinear_fusion = mlp.predict(concat_fusion)
    
    return {
        "concat": concat_fusion, "weighted": weighted_fusion,
        "pca": pca_fusion, "nonlinear": nonlinear_fusion
    }

（4）下面代码的功能是可视化低层融合结果，通过多子图展示原始数据融合的时间序列对比、LiDAR空间配准效果，以及特征层融合的四种策略对比，将抽象的融合过程和结果转化为直观图表，便于理解不同融合方式的效果差异。

python 复制代码

# -------------------------- 4. 可视化模块 --------------------------
def visualize_results(data, raw_fusion_result, feature_fusion_result):
    """可视化所有融合结果"""
    plt.rcParams["font.size"] = 10
    fig = plt.figure(figsize=(20, 15))
    
    # 子图1：原始数据vs原始融合数据（时间序列）
    ax1 = plt.subplot(3, 3, (1, 3))
    ax1.plot(data["t_imu"][::10], data["imu_data"][::10, 0], 'r.', label='IMU原始加速度', alpha=0.5)
    ax1.plot(data["t_cam"], data["cam_data"][:, 0], 'g.', label='相机原始像素', alpha=0.5)
    ax1.plot(data["t_base"], raw_fusion_result[:, 0], 'b-', label='原始融合结果', linewidth=2)
    ax1.set_xlabel('时间 (s)')
    ax1.set_ylabel('数值')
    ax1.set_title('原始数据融合 - 时间序列对比')
    ax1.legend()
    ax1.grid(True)
    
    # 子图2：空间配准前后对比
    ax2 = plt.subplot(3, 3, (4, 6))
    lidar_original = data["lidar_data"][:50]
    lidar_registered = spatial_registration(lidar_original)
    ax2.scatter(lidar_original[:, 0], lidar_original[:, 1], c='r', label='配准前LiDAR点云', alpha=0.6)
    ax2.scatter(lidar_registered[:, 0], lidar_registered[:, 1], c='g', label='配准后LiDAR点云', alpha=0.6)
    ax2.set_xlabel('X (m)')
    ax2.set_ylabel('Y (m)')
    ax2.set_title('原始数据融合 - 空间配准效果')
    ax2.legend()
    ax2.grid(True)
    
    # 子图3：特征融合-拼接vs加权
    ax3 = plt.subplot(3, 3, 7)
    ax3.scatter(feature_fusion_result["concat"][:, 0], feature_fusion_result["concat"][:, 1], c='b', label='拼接融合', alpha=0.5)
    ax3.scatter(feature_fusion_result["weighted"][:, 0], feature_fusion_result["weighted"][:, 1], c='r', label='加权融合', alpha=0.5)
    ax3.set_xlabel('特征维度1')
    ax3.set_ylabel('特征维度2')
    ax3.set_title('特征层融合 - 拼接vs加权')
    ax3.legend()
    ax3.grid(True)
    
    # 子图4：特征融合-PCA降维
    ax4 = plt.subplot(3, 3, 8)
    ax4.scatter(feature_fusion_result["pca"][:, 0], feature_fusion_result["pca"][:, 1], c='purple', label='PCA降维融合', alpha=0.7)
    ax4.set_xlabel('PCA维度1')
    ax4.set_ylabel('PCA维度2')
    ax4.set_title('特征层融合 - PCA降维')
    ax4.legend()
    ax4.grid(True)
    
    # 子图5：特征融合-非线性MLP
    ax5 = plt.subplot(3, 3, 9)
    ax5.scatter(feature_fusion_result["nonlinear"][:, 0], feature_fusion_result["nonlinear"][:, 1], c='orange', label='MLP非线性融合', alpha=0.7)
    ax5.set_xlabel('MLP维度1')
    ax5.set_ylabel('MLP维度2')
    ax5.set_title('特征层融合 - MLP非线性')
    ax5.legend()
    ax5.grid(True)
    
    plt.tight_layout()
    plt.savefig("人形机器人低层融合可视化.png", dpi=300, bbox_inches='tight')
    plt.show()

（5）下面代码的功能是作为主程序串联整个低层融合流程，依次执行模拟数据生成、原始数据融合、特征层融合、结果可视化，输出各环节执行状态提示，确保全流程闭环运行并生成可视化成果文件。

python 复制代码

# -------------------------- 主程序 --------------------------
if __name__ == "__main__":
    # 1. 生成模拟数据
    sensor_data = generate_sensor_data()
    print("✅ 传感器数据生成完成")
    
    # 2. 原始数据融合
    # 2.1 时间同步（对齐到基准时间轴）
    imu_aligned = temporal_alignment(sensor_data["t_base"], sensor_data["t_imu"], sensor_data["imu_data"])
    cam_aligned = temporal_alignment(sensor_data["t_base"], sensor_data["t_cam"], sensor_data["cam_data"])
    lidar_aligned = temporal_alignment(sensor_data["t_base"], sensor_data["t_lidar"], sensor_data["lidar_data"])
    print("✅ 原始数据时间同步完成")
    
    # 2.2 空间配准（LiDAR→相机坐标系）
    lidar_registered = spatial_registration(lidar_aligned)
    print("✅ 原始数据空间配准完成")
    
    # 2.3 加权融合
    raw_fusion = weighted_fusion(
        [imu_aligned, cam_aligned, lidar_registered],
        [sensor_data["imu_cov"], sensor_data["cam_cov"], sensor_data["lidar_cov"]]
    )
    print("✅ 原始数据加权融合完成")
    
    # 3. 特征层融合
    # 3.1 特征提取
    imu_feat = feature_extraction(sensor_data["imu_data"], "imu")
    cam_feat = feature_extraction(sensor_data["cam_data"], "cam")
    lidar_feat = feature_extraction(sensor_data["lidar_data"], "lidar")
    # 统一长度（取最小长度）
    min_len = min(len(imu_feat), len(cam_feat), len(lidar_feat))
    imu_feat = imu_feat[:min_len]
    cam_feat = cam_feat[:min_len]
    lidar_feat = lidar_feat[:min_len]
    print("✅ 特征提取完成")
    
    # 3.2 特征对齐
    aligned_feats = feature_alignment([imu_feat, cam_feat, lidar_feat])
    print("✅ 特征对齐完成")
    
    # 3.3 特征融合
    feature_fusion = feature_fusion_strategies(
        aligned_feats,
        [sensor_data["imu_cov"], sensor_data["cam_cov"], sensor_data["lidar_cov"]]
    )
    print("✅ 特征层融合完成")
    
    # 4. 可视化结果
    visualize_results(sensor_data, raw_fusion, feature_fusion)
    print("✅ 可视化图表生成完成！")

执行后绘制人形机器人多模态低层融合的技术可视化图，包含原始数据融合与特征层融合的效果展示，如图9-1所示。

图9-1 人形机器人多模态低层融合的技术可视化图