IMU与GPS松耦合定位系统的EKF实现与优化

1. 项目概述：IMU与GPS松耦合定位系统实现

在移动机器人、自动驾驶和无人机导航领域，多传感器融合定位一直是核心技术难点。最近我在实验室完成了一个将IMU（惯性测量单元）和GPS通过扩展卡尔曼滤波（EKF）进行松耦合融合的项目，从Matlab原型成功移植到C++实现。这个系统基于位姿状态方程，能够实现比单一传感器更稳定、更精确的定位效果。

整个开发过程就像在解一道精密的空间几何题——需要同时考虑IMU的高频动态特性和GPS的低频绝对定位信息。Matlab环境下验证算法原理相对简单，但移植到C++时就像把理论物理公式变成可量产的工程产品，需要考虑实时性、数值稳定性和系统鲁棒性等一系列实际问题。

2. 系统架构与状态方程设计

2.1 状态向量定义

系统的核心是一个16维的状态向量，包含了位姿、速度和传感器误差等关键信息。在C++中我们使用Eigen库来定义这个结构体：

cpp复制struct State {
    Eigen::Vector3d position;  // 3D位置 (x,y,z)
    Eigen::Vector3d velocity;  // 3D速度 (vx,vy,vz)
    Eigen::Quaterniond quat;   // 姿态四元数 (qw,qx,qy,qz)
    Eigen::Vector3d acc_bias;  // 加速度计偏置 (bax,bay,baz)
    Eigen::Vector3d gyro_bias; // 陀螺仪偏置 (bgx,bgy,bgz)
    
    // 将状态转换为向量形式便于矩阵运算
    Eigen::Matrix<double,16,1> vec() const {
        Eigen::Matrix<double,16,1> x;
        x.segment<3>(0) = position;
        x.segment<3>(3) = velocity;
        x.segment<4>(6) = Eigen::Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
        x.segment<3>(10) = acc_bias;
        x.segment<3>(13) = gyro_bias;
        return x;
    }
};

选择四元数而非欧拉角表示姿态是为了避免万向锁问题。四元数虽然多了一个参数，但在连续旋转时不会出现奇异点，计算也更加稳定。加速度计和陀螺仪偏置作为状态量的一部分，可以实时估计并补偿传感器的系统性误差。

2.2 系统动力学模型

IMU提供了机体坐标系下的加速度和角速度测量值，我们需要将其转换到世界坐标系下。系统的状态预测方程基于惯性导航原理：

code复制位置导数 = 速度
速度导数 = R(加速度 - 加速度计偏置) + 重力
姿态导数 = 0.5*Ω(角速度 - 陀螺仪偏置)*当前姿态
加速度计偏置导数 = 0 (建模为随机游走)
陀螺仪偏置导数 = 0 (建模为随机游走)

其中R是从机体坐标系到世界坐标系的旋转矩阵，Ω是由角速度构成的四元数乘法矩阵。这个连续时间模型在代码实现时需要离散化处理。

3. EKF算法实现细节

3.1 预测步骤实现

预测步骤利用IMU数据进行状态和协方差矩阵的递推。以下是核心代码实现：

cpp复制void predict(const IMUData &imu, double dt) {
    // 去除偏置的传感器读数
    Eigen::Vector3d acc = imu.acc - state_.acc_bias;
    Eigen::Vector3d gyro = imu.gyro - state_.gyro_bias;
    
    // 姿态更新
    Eigen::Quaterniond dq = deltaQuat(gyro * dt);
    state_.quat = (state_.quat * dq).normalized();
    
    // 速度更新（考虑重力影响）
    Eigen::Vector3d acc_world = state_.quat * acc;
    state_.velocity += (acc_world + gravity_) * dt;
    
    // 位置更新
    state_.position += state_.velocity * dt;
    
    // 协方差预测
    Eigen::MatrixXd F = computeJacobianF(acc, dt);
    Eigen::MatrixXd G = computeJacobianG(dt);
    P_ = F * P_ * F.transpose() + G * Q_ * G.transpose();
}

其中deltaQuat函数计算角速度积分得到的小角度旋转四元数：

cpp复制Eigen::Quaterniond deltaQuat(const Eigen::Vector3d &w) {
    double theta = w.norm();
    if(theta < 1e-6) {
        return Eigen::Quaterniond::Identity();
    }
    Eigen::Vector3d axis = w.normalized();
    return Eigen::Quaterniond(cos(theta/2), 
                             axis.x()*sin(theta/2),
                             axis.y()*sin(theta/2),
                             axis.z()*sin(theta/2));
}

3.2 雅可比矩阵计算

状态转移矩阵F的构造是EKF实现中最复杂的部分之一。我们需要对系统动力学模型进行线性化：

cpp复制Eigen::MatrixXd computeJacobianF(const Eigen::Vector3d &acc, double dt) {
    Eigen::MatrixXd F = Eigen::MatrixXd::Identity(16, 16);
    
    // 位置对速度的偏导
    F.block<3,3>(0,3) = Eigen::Matrix3d::Identity() * dt;
    
    // 速度对姿态的偏导
    Eigen::Matrix3d R = state_.quat.toRotationMatrix();
    F.block<3,3>(3,6) = -R * skewSymmetric(acc) * dt;
    
    // 速度对加速度计偏置的偏导
    F.block<3,3>(3,9) = -R * dt;
    
    // 姿态对陀螺仪偏置的偏导
    F.block<3,3>(6,12) = -R * dt;
    
    return F;
}

其中skewSymmetric函数实现向量到反对称矩阵的转换：

cpp复制Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d &v) {
    Eigen::Matrix3d m;
    m << 0, -v.z(), v.y(),
         v.z(), 0, -v.x(),
         -v.y(), v.x(), 0;
    return m;
}

3.3 GPS观测更新

当GPS数据到达时，我们进行EKF的更新步骤。GPS提供绝对位置信息，观测矩阵H相对简单：

cpp复制void update(const GPSData &gps) {
    // 观测残差
    Eigen::Vector3d z = gps.position - state_.position;
    
    // 观测矩阵（仅观测位置）
    Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, 16);
    H.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
    
    // 卡尔曼增益计算
    Eigen::MatrixXd S = H * P_ * H.transpose() + R_gps_;
    Eigen::MatrixXd K = P_ * H.transpose() * S.inverse();
    
    // 状态更新
    state_.vec() += K * z;
    
    // 协方差更新
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(16, 16);
    P_ = (I - K * H) * P_;
    
    // 四元数重新归一化
    state_.quat.normalize();
}

4. 关键实现技巧与优化

4.1 数值稳定性处理

在实现过程中，有几个数值稳定性问题需要特别注意：

1. 四元数归一化：每次姿态更新后必须重新归一化四元数，否则会逐渐偏离单位四元数性质。我们在状态更新后显式调用normalize()方法。

2. 协方差矩阵对称性保持：由于浮点计算误差，协方差矩阵P可能会逐渐失去对称性。可以通过强制对称来避免：

   cpp复制P_ = 0.5 * (P_ + P_.transpose());
   

3. 矩阵求逆稳定性：在计算卡尔曼增益时，需要对矩阵S求逆。可以添加小的正则化项来避免奇异：

   cpp复制S += 1e-6 * Eigen::Matrix3d::Identity();
   

4.2 性能优化技巧

从Matlab到C++的移植过程中，性能优化是关键。以下是几个有效的优化手段：

1. 内存预分配：所有中间矩阵在初始化时就预先分配好内存，避免运行时动态分配。

2. Eigen的Map功能：当需要与其他库（如ROS）交换数据时，使用Eigen的Map功能避免数据拷贝：

   cpp复制double *data = ...; // 外部数据指针
   Eigen::Map<Eigen::Vector3d> vec(data);
   

3. 编译器优化：启用现代CPU的SIMD指令集：

   bash复制-march=native -O3
   

4. 矩阵运算简化：利用矩阵分块和对称性减少计算量，例如：

   cpp复制// 而不是直接计算P*H^T
   Eigen::MatrixXd PHt = P_.block<16,3>(0,0); 
   

4.3 时间同步处理

IMU和GPS的数据频率通常不同（IMU 100-1000Hz，GPS 1-10Hz），且存在传输延迟。我们实现了一个双缓冲机制来对齐时间戳：

cpp复制class SensorBuffer {
public:
    void addImu(const IMUData &imu) {
        std::lock_guard<std::mutex> lock(mutex_);
        imu_buffer_.push_back(imu);
        if(imu_buffer_.size() > 500) {
            imu_buffer_.pop_front();
        }
    }
    
    bool syncData(double gps_time, std::vector<IMUData> &imus) {
        std::lock_guard<std::mutex> lock(mutex_);
        auto it = std::lower_bound(imu_buffer_.begin(), imu_buffer_.end(), gps_time,
            [](const IMUData &imu, double t) { return imu.timestamp < t; });
        
        if(it == imu_buffer_.begin() || it == imu_buffer_.end()) {
            return false;
        }
        
        imus.assign(imu_buffer_.begin(), it);
        return true;
    }
    
private:
    std::deque<IMUData> imu_buffer_;
    std::mutex mutex_;
};

5. 实际测试与问题排查

5.1 典型问题与解决方案

在系统实现和测试过程中，我们遇到了几个典型问题：

1. 定位漂移：初期测试时，系统在几分钟内就会漂移数百米。经排查发现是四元数未归一化导致旋转矩阵逐渐失真。解决方法是在每次姿态更新后强制归一化。

2. GPS跳变：城市环境中GPS信号可能突然跳变几米甚至几十米。我们通过设置合理的观测噪声协方差R_gps来适应不同定位精度：当GPS信号质量差时自动增大R_gps，降低对GPS的信任度。

3. 初始化问题：系统启动时，如果姿态初始化不正确会导致后续积分发散。我们实现了基于初始加速度计数据的水平姿态初始化：

   cpp复制void initOrientation(const Eigen::Vector3d &acc) {
       Eigen::Vector3d down = -acc.normalized();
       Eigen::Vector3d east = Eigen::Vector3d::UnitY().cross(down);
       east.normalize();
       Eigen::Vector3d north = down.cross(east);
       Eigen::Matrix3d R;
       R.col(0) = north;
       R.col(1) = east;
       R.col(2) = down;
       state_.quat = Eigen::Quaterniond(R);
   }
   

5.2 测试结果分析

我们在校园环境中进行了实地测试，设备配置如下：

• IMU: Xsens MTi-300 (200Hz)
• GPS: u-blox M8T (5Hz)
• 处理器: Intel i7-1185G7

测试结果显示：

• 纯GPS定位的均方根误差(RMSE)为2.5米
• 纯IMU积分在30秒后误差超过10米
• EKF融合后的定位RMSE为0.8米，且轨迹明显平滑

特别是在GPS信号短暂丢失的情况下（如隧道场景），融合系统能依靠IMU维持几十秒的可靠定位，误差增长速率约为0.3%/秒。

6. 扩展与改进方向

当前系统还有一些可以改进的空间：

1. 自适应噪声估计：根据传感器实际表现动态调整过程噪声Q和观测噪声R，而不是使用固定值。

2. 多传感器融合：加入轮速计、视觉或激光雷达观测，进一步提升复杂环境下的鲁棒性。

3. 误差建模改进：考虑IMU的温度效应、尺度因子误差等更精细的误差模型。

4. 基于优化的方法：实现紧耦合的优化框架，如基于因子图的融合方法，可能获得更高的精度。

5. 开源框架集成：将算法移植到ROS或Apollo等开源框架中，方便与其他模块集成。

在实际部署中发现，系统对IMU和GPS的相对安装位置很敏感。如果两者天线中心不重合，需要仔细测量并补偿这个杆臂效应。我们通过以下方式实现：

cpp复制Eigen::Vector3d gps_in_imu_frame; // 测量得到的杆臂向量

void applyLeverArm(Eigen::Vector3d &gps_position) {
    Eigen::Matrix3d R = state_.quat.toRotationMatrix();
    gps_position -= R * gps_in_imu_frame;
}