图优化在GNSS/INS/OD融合定位中的应用【附代码】

最新推荐文章于 2026-08-21 15:14:26 发布

原创最新推荐文章于 2026-08-21 15:14:26 发布 · 332 阅读

4 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

✅ 博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。

✅ 具体问题可以私信或扫描文章底部二维码。

(1) 设计基于混合滑动窗口与预积分因子的轻量级图优化模型。图优化将定位问题转化为一个最大后验概率估计问题，通过构建由节点（机器人位姿）和边（传感器测量约束）组成的图模型进行求解。随着时间增长，节点数量线性增加，导致计算量剧增。为解决此问题，本研究设计了一种混合滑动窗口优化器。它将优化窗口划分为“成熟区”和“成长区”。成熟区内包含经过多次优化、状态已相对稳定的历史位姿节点；成长区则包含最新的、待优化的位姿节点。在每次优化迭代时，主要对成长区节点和与成熟区相连的少数边缘节点进行优化，而成熟区内部节点的约束则通过边缘化技术转化为先验信息保留。这极大地减少了每次优化的变量规模，实现了计算复杂度的有界性。同时，针对高频的IMU和OD数据，创新性地采用了一种改进的预积分算法。该算法能够在IMU零偏或OD刻度因子发生微小变化时，利用一阶泰勒展开近似修正预积分结果，而无需从原始数据重新积分，显著提升了计算效率，满足了实时性要求

。

(2) 提出一种嵌套期望最大化算法以估计高斯混合误差模型，提升抗差能力。在复杂环境中，传感器测量值常因多径效应、非视距、颠簸路面等产生异常值，这些误差不服从高斯分布，使用标准的高斯假设优化器（如基于马氏距离的鲁棒核函数）可能失效。为此，本研究提出了一种自适应期望最大化算法来求解更通用的高斯混合误差模型。该模型将测量误差表示为多个高斯分布的加权和，其中一个分量用于建模正常的“内点”噪声（方差小），其他分量用于建模可能的“外点”噪声（方差大）。算法采用嵌套EM结构：内层EM算法负责估计当前迭代下每条约束边所属的高斯混合分量及其参数（权重、均值、方差）；外层EM算法则基于内层估计出的每个数据点的“责任度”（属于各分量的概率），重新估计系统的状态变量（位姿、速度等）。通过这种交替优化，算法能自动甄别并削弱异常测量值的影响，而不需要手动设置阈值。实验表明，与传统的DCS、Gauss-Newton等方法相比，该算法在含有大量异常值的场景下，仍能保持较高的定位精度和良好的收敛性。

(3) 实现紧耦合的GNSS原始观测值与多源信息融合框架。为进一步提升在GNSS信号部分可见或质量不佳时的性能，本研究超越了松耦合（位置/速度级融合）和紧耦合（伪距/载波相位级融合）的传统方式，将GNSS原始伪距、多普勒频移等观测值直接作为约束边加入到图优化模型中。INS/OD预积分提供的精确短时位姿预测，可以有效辅助GNSS整周模糊度的解算；反之，GNSS的绝对位置信息又能校正INS的累积误差和OD的刻度误差。在图模型中，除了位姿节点，还将接收机钟差、GNSS模糊度等作为待优化状态变量。构建的约束边类型包括：INS预积分产生的相邻位姿间相对运动约束，OD产生的相邻位姿间相对位移约束，以及GNSS接收机与多个卫星之间基于几何距离的伪距约束和多普勒速度约束。所有约束在统一的非线性最小二乘框架下进行优化。这种深度的紧耦合方式，即使在仅能接收到2-3颗卫星信号的极端情况下，也能通过与INS/OD的组合提供可用的定位解，极大地增强了系统的可用性和鲁棒性。仿真与实车测试表明，该融合算法在开阔环境下的定位精度与专业RTK相当，在复杂城市道路中的定位连续性和可靠性远高于传统的卡尔曼滤波方案。

import numpy as np
import sympy as sp
from scipy.sparse import lil_matrix, csr_matrix
from scipy.sparse.linalg import spsolve
import matplotlib.pyplot as plt

class GraphOptimizationFusion:
    def __init__(self, window_size=10):
        self.window_size = window_size
        self.state_dim = 3
        self.poses = []
        self.landmarks = []
        self.edges = []
        self.optimization_history = []

    def add_pose(self, pose):
        self.poses.append(pose.copy())
        if len(self.poses) > self.window_size:
            self.poses.pop(0)
            self._marginalize_oldest()

    def add_gnss_constraint(self, pose_id, gnss_measurement, info_matrix):
        self.edges.append(('gnss', pose_id, gnss_measurement, info_matrix))

    def add_imu_preintegral_constraint(self, pose_id_i, pose_id_j, delta_pose, info_matrix):
        self.edges.append(('imu', (pose_id_i, pose_id_j), delta_pose, info_matrix))

    def add_odometry_constraint(self, pose_id_i, pose_id_j, odom_meas, info_matrix):
        self.edges.append(('odom', (pose_id_i, pose_id_j), odom_meas, info_matrix))

    def _compute_error_and_jacobian(self, state_vector):
        num_poses = len(self.poses)
        total_state_size = num_poses * self.state_dim
        error = []
        rows = []
        cols = []
        data_J = []
        row_idx = 0
        for edge in self.edges:
            e_type, ids, measurement, info = edge
            sqrt_info = np.linalg.cholesky(info)
            if e_type == 'gnss':
                pose_id = ids
                if pose_id >= num_poses:
                    continue
                pose_idx = pose_id * self.state_dim
                est_pose = state_vector[pose_idx:pose_idx+self.state_dim]
                err = est_pose - measurement
                J = np.eye(self.state_dim)
                for i in range(self.state_dim):
                    for j in range(self.state_dim):
                        rows.append(row_idx + i)
                        cols.append(pose_idx + j)
                        data_J.append(J[i, j])
                error.append(sqrt_info @ err)
                row_idx += self.state_dim
            elif e_type == 'imu' or e_type == 'odom':
                id_i, id_j = ids
                if id_j >= num_poses:
                    continue
                idx_i = id_i * self.state_dim
                idx_j = id_j * self.state_dim
                pose_i = state_vector[idx_i:idx_i+self.state_dim]
                pose_j = state_vector[idx_j:idx_j+self.state_dim]
                pred_delta = pose_j - pose_i
                err = pred_delta - measurement
                J_i = -np.eye(self.state_dim)
                J_j = np.eye(self.state_dim)
                for i in range(self.state_dim):
                    for j in range(self.state_dim):
                        rows.append(row_idx + i)
                        cols.append(idx_i + j)
                        data_J.append(J_i[i, j])
                        rows.append(row_idx + i)
                        cols.append(idx_j + j)
                        data_J.append(J_j[i, j])
                error.append(sqrt_info @ err)
                row_idx += self.state_dim
        error = np.concatenate(error)
        J = csr_matrix((data_J, (rows, cols)), shape=(row_idx, total_state_size))
        return error, J

    def _marginalize_oldest(self):
        if len(self.poses) <= 1:
            return
        old_edges = [e for e in self.edges if (isinstance(e[1], tuple) and e[1][0] == 0) or (e[0] == 'gnss' and e[1] == 0)]
        for edge in old_edges:
            self.edges.remove(edge)
        self.edges = [('gnss', pid-1, meas, info) if etype == 'gnss' and pid>0 else 
                      (etype, (pid_i-1, pid_j-1), meas, info) if isinstance(ids, tuple) else 
                      (etype, ids, meas, info) 
                      for etype, ids, meas, info in self.edges]

    def gauss_newton_optimization(self, iterations=10):
        num_poses = len(self.poses)
        if num_poses == 0:
            return
        current_state = np.concatenate(self.poses)
        for it in range(iterations):
            error, J = self._compute_error_and_jacobian(current_state)
            b = -J.T @ error
            H = J.T @ J
            dx = spsolve(H + 1e-6 * np.eye(H.shape[0]), b)
            current_state += dx
            cost = error.T @ error
            self.optimization_history.append(cost)
        for i in range(num_poses):
            self.poses[i] = current_state[i*self.state_dim:(i+1)*self.state_dim]
        return current_state, cost

    def run_simulation(self):
        np.random.seed(2025)
        gt_trajectory = np.cumsum(np.random.randn(30, self.state_dim) * 0.5, axis=0)
        for i, gt_pose in enumerate(gt_trajectory):
            noisy_pose = gt_pose + np.random.randn(self.state_dim) * 2.0
            self.add_pose(noisy_pose)
            if i > 0:
                imu_delta = gt_trajectory[i] - gt_trajectory[i-1] + np.random.randn(self.state_dim)*0.1
                self.add_imu_preintegral_constraint(i-1, i, imu_delta, np.eye(self.state_dim)*100)
                odom_delta = gt_trajectory[i] - gt_trajectory[i-1] + np.random.randn(self.state_dim)*0.05
                self.add_odometry_constraint(i-1, i, odom_delta, np.eye(self.state_dim)*150)
            if i % 3 == 0:
                gnss_meas = gt_pose + np.random.randn(self.state_dim) * (5.0 if i % 9 == 0 else 1.0)
                self.add_gnss_constraint(i, gnss_meas, np.eye(self.state_dim)/4.0)
            if len(self.poses) >= self.window_size:
                opt_state, final_cost = self.gauss_newton_optimization(iterations=5)
        opt_trajectory = np.array(self.poses)
        return gt_trajectory[:len(opt_trajectory)], opt_trajectory

optimizer = GraphOptimizationFusion(window_size=8)
gt, est = optimizer.run_simulation()
fig, axes = plt.subplots(2, 2, figsize=(14, 10))
axes[0, 0].plot(gt[:, 0], gt[:, 1], 'k-', label='Ground Truth', linewidth=2)
axes[0, 0].plot(est[:, 0], est[:, 1], 'b--', label='Optimized', linewidth=1.5, marker='o', markersize=4)
axes[0, 0].set_xlabel('X (m)')
axes[0, 0].set_ylabel('Y (m)')
axes[0, 0].set_title('Trajectory Comparison (Top-Down View)')
axes[0, 0].legend()
axes[0, 0].grid(True)
axes[0, 0].axis('equal')
axes[0, 1].plot(range(len(gt)), gt[:, 2], 'k-', label='GT Z', linewidth=2)
axes[0, 1].plot(range(len(est)), est[:, 2], 'g--', label='Est Z', linewidth=1.5)
axes[0, 1].set_xlabel('Step')
axes[0, 1].set_ylabel('Z (m)')
axes[0, 1].set_title('Height Comparison')
axes[0, 1].legend()
axes[0, 1].grid(True)
pos_error = np.linalg.norm(gt[:len(est)] - est, axis=1)
axes[1, 0].plot(pos_error, 'r-')
axes[1, 0].set_xlabel('Step')
axes[1, 0].set_ylabel('Position Error (m)')
axes[1, 0].set_title('Optimization Position Error Over Time')
axes[1, 0].grid(True)
axes[1, 1].plot(optimizer.optimization_history, 'm-')
axes[1, 1].set_xlabel('Iteration')
axes[1, 1].set_ylabel('Total Cost')
axes[1, 1].set_title('Graph Optimization Convergence')
axes[1, 1].grid(True)
plt.tight_layout()
plt.show()