自动驾驶 | GREAT-FGO: 基于因子图优化的高精度定位导航技术解析

1. 引言

在当前卫星导航与精密定位领域,传统的扩展卡尔曼滤波(EKF)和最小二乘方法在处理多源传感器融合定位时面临诸多挑战。这些方法往往难以充分利用历史观测信息,在复杂城市峡谷环境中容易出现定位精度下降甚至失效的问题。为了服务大地测量与导航领域的研究者和工程技术人员,武汉大学GREAT团队开源了基于因子图优化技术的RTK与RTK/INS组合导航软件GREAT-FGO。该软件采用先进的因子图优化框架,能够在保持历史观测约束的同时实现高精度的实时定位,为精密导航应用提供了新的解决方案。

因子图优化作为一种图优化方法,近年来在机器人领域的同步定位与地图构建(SLAM)任务中取得了显著成功。其核心思想是将状态估计问题表示为因子图的形式,通过构建状态变量节点和观测约束因子之间的连接关系,利用非线性优化方法求解最大后验估计。相比传统滤波方法,因子图优化能够保持多个历史时刻的状态变量,充分利用时间窗口内的所有观测信息进行联合优化,从而获得更高的定位精度和鲁棒性。GREAT-FGO将这一先进技术引入卫星导航定位领域,针对GNSS RTK和RTK/INS紧组合导航的特点进行了系统性的算法设计和工程实现。

2. GREAT软件平台与研究背景

GREAT(GNSS+ REsearch, Application and Teaching)软件平台由武汉大学测绘学院设计开发,是一个面向空间大地测量数据处理、精密定位定轨以及多源融合导航的综合性软件系统。该平台整合了多个功能模块,涵盖了从基础GNSS数据处理到高级多传感器融合导航的完整技术链条。GREAT-FGO作为其中的核心模块之一,专注于因子图优化导航解算,提供了RTK、RTK/INS以及多传感器融合等多种先进算法实现。

GREAT团队长期从事卫星精密定位定轨与多源融合导航方面的研究与教学工作。团队在国际大地测量与地球科学领域发表了200余篇高水平SCI论文,Google Scholar总被引超过10000次,H指数达到55。团队成员入选爱思唯尔中国高被引学者和全球2%顶尖科学家榜单。研究成果先后荣获湖北省自然科学奖一等奖、教育部科技进步奖一等奖、国际大地测量和地球物理学联合会(IUGG)青年科学家奖等重要奖项。此次开源的GREAT-FGO软件正是团队多年研究成果的结晶,代表了因子图优化在GNSS精密定位领域应用的最新进展。下面是一些链接

GitHub仓库: https://github.com/GREAT-WHU/GREAT-FGO团队主页: http://igmas.users.sgg.whu.edu.cn/group团队GitHub: https://github.com/GREAT-WHUQQ交流群: 1009827379微信公众号: GREAT智能导航实验室Bilibili账号: GREAT智能导航实验室

3. 因子图优化理论基础

3.1 从传统滤波到图优化的范式转变

传统的GNSS定位算法主要依赖扩展卡尔曼滤波(EKF)或最小二乘方法进行状态估计。扩展卡尔曼滤波通过预测和更新两个步骤递推地估计系统状态,其优点是计算效率高、实时性好,但也存在明显的局限性。首先,EKF采用线性化近似处理非线性观测模型,在强非线性情况下容易导致滤波发散。其次,滤波器的马尔可夫性假设要求当前状态只与前一时刻状态相关,这导致历史观测信息被边缘化后无法重新利用。当出现观测异常或周跳时,滤波器难以利用前后时刻的观测信息进行自洽性检验和状态修正。

因子图优化提供了一种完全不同的思路。它将状态估计问题建模为一个概率图模型,通过因子图来表示状态变量与观测约束之间的关系。在因子图中,圆形节点代表待估计的状态变量,例如不同时刻的位置、速度、姿态等;方形节点代表观测因子,对应各类传感器的观测约束;边则表示状态变量与观测因子之间的依赖关系。整个估计问题被转化为在因子图上求解所有状态变量的最大后验概率,这等价于最小化所有观测因子残差的加权平方和。

这种表示方式的关键优势在于,因子图能够保持一个滑动窗口内所有历史状态变量,而不像EKF那样只保留当前状态。优化器可以同时调整窗口内所有状态量,使得整个轨迹段在所有观测约束下达到全局最优。当检测到某个时刻存在观测异常时,优化算法可以综合利用前后时刻的约束信息,对异常数据进行降权或剔除,从而提高系统的鲁棒性。

3.2 因子图的数学表述

从概率论角度,状态估计问题可以表述为求解状态变量在给定观测条件下的最大后验估计。根据贝叶斯定理,后验概率正比于先验概率与似然函数的乘积。在因子图框架下,这个联合概率分布可以分解为多个局部因子的乘积。假设状态变量集合为 ,观测集合为 ,则最大后验估计问题可以写为:

其中  表示观测似然,描述观测值与状态变量之间的关系; 表示状态转移概率,对应运动模型约束; 为初始状态的先验分布。假设观测噪声和过程噪声均服从高斯分布,取负对数后,最大化后验概率等价于最小化以下非线性最小二乘问题:

这里  先验残差, 为运动模型残差, 为观测残差。每一项残差都通过相应的协方差矩阵进行加权,反映观测的不确定性。这个优化问题可以使用高斯-牛顿法或Levenberg-Marquardt算法高效求解。在GREAT-FGO中,底层采用Google的Ceres Solver优化库实现非线性优化求解,该库提供了自动求导、稀疏矩阵处理等高级功能,能够处理大规模的状态估计问题。

3.3 滑动窗口优化策略

在实时导航应用中,状态变量的数量会随着时间不断增长,如果保留所有历史状态进行优化,计算复杂度将不可接受。滑动窗口优化是解决这一问题的关键技术。其基本思想是只在一个固定大小的时间窗口内保持状态变量,当窗口满时,通过边缘化技术移除最旧的状态,同时将其包含的信息转化为先验约束保留下来。

具体而言,假设滑动窗口大小为N,当第N+1个状态到来时,需要移除状态x₀。边缘化过程通过舒尔补(Schur Complement)技术实现:将优化问题的海森矩阵(Hessian Matrix)分块,将x₀对应的块消元,其蕴含的约束信息转化为其余状态变量之间的稠密先验约束。这样,虽然x₀被移除,但它与观测之间的约束关系被编码到了边缘化先验中,在后续优化时作为先验因子加入。这种处理方式在保持计算效率的同时,最大程度地利用了历史观测信息。

在GREAT-FGO的实现中,滑动窗口大小通过GWINDOW_SIZE参数配置。代码中定义了专门的边缘化信息结构GNSSInfo来存储边缘化后的先验约束,包括线性化点、雅可比矩阵和残差向量。每次窗口滑动时,_marginalization()函数会构建新的边缘化信息,_prior_factor()函数则将上一次的边缘化信息作为先验因子加入当前优化问题。这种设计使得系统能够在有限的计算资源下实现长时间、高精度的连续定位。

4. 基于因子图的GNSS RTK定位算法

4.1 RTK双差观测模型

实时动态定位RTK技术是GNSS高精度定位的主要手段,其核心在于利用载波相位观测值构建基站与流动站之间的双差观测方程。双差观测能够有效消除卫星钟差和接收机钟差的影响,同时显著削弱大气延迟等系统误差,使得载波相位整周模糊度参数能够被可靠固定为整数值。传统RTK算法主要采用扩展卡尔曼滤波进行状态估计,而GREAT-FGO创新性地将因子图优化框架引入RTK解算,通过保持历史观测信息和全局优化提升定位精度和可靠性。

GNSS载波相位双差观测方程可以表示为基站和流动站对两颗卫星的观测值差分。设基站为,流动站为,参考卫星为,非参考卫星为,则双差载波相位观测方程为:

其中表示双差算子,载波相位观测值,为几何距离,为载波波长,为双差整周模糊度,为双差观测噪声。几何距离项可以进一步展开为:

其中为流动站位置,为基站位置,和分别为参考卫星和非参考卫星的位置。对于伪距观测,同样可以构建双差观测方程。伪距双差观测主要用于初始化和辅助载波相位定位,其观测噪声相对较大但不存在整周模糊度问题。伪距双差观测方程为:

这里为双差伪距观测值,为双差伪距观测噪声。在因子图框架下,每组双差观测对应一个观测因子,连接相关的状态变量节点。通过最小化所有观测因子的加权残差平方和,可以同时估计流动站位置和整周模糊度参数。

4.2 载波相位双差因子实现

在GREAT-FGO的代码实现中,载波相位双差观测被封装为CarrierphaseDDFactor类。该类继承自Ceres Solver的SizedCostFunction,定义了残差维度和参数维度。其核心方法是Evaluate函数,负责计算观测残差和雅可比矩阵。以下是实际项目代码:

class LibGREAT_LIBRARY_EXPORT CarrierphaseDDFactor : public ceres::SizedCostFunction<1, 3, 1, 1>{public:    /**     * @brief Carrier phase DD factor constructor     * Initializes with time, site pairs, parameters, satellite data and frequency band     */    CarrierphaseDDFactor(const t_gtime &cur_time, const pair<string, string> &base_rover_site, const t_gallpar  &params, const vector<pair<t_gsatdata, t_gsatdata>> &DD_sat_data, t_gbiasmodel *bias_model, const pair<FREQ_SEQ, GOBSBAND> &freq_band);    /**     * @brief Update parameters for carrier phase DD factor     * Updates rover coordinates, reference and non-reference satellite ambiguities in parameter set for linearization point adjustment     */void updatePara(t_gallpar & params_tmp, const double &ref_sd_amb, const double &nonref_sd_amb, const Eigen::Vector3d &Pi, const Eigen::Vector3d &Vi = Eigen::Vector3d::Identity()) const;    /**     * @brief Transform design matrices to Eigen format     * Converts sparse design matrix, weight matrix and residuals to dense Eigen matrices for efficient linear algebra operations in optimization     */void trans2Eigen(const vector<vector<pair<int, double>>> &B, const vector<double> &P, const vector<double> &l, Eigen::Matrix<double, 2, 5> &B_new, Eigen::Matrix<double, 2, 2> &P_new, Eigen::Matrix<double, 2, 1> &l_new) const;    /**     * @brief Evaluate carrier phase double-difference factor for optimization     *     * Computes residuals and Jacobians for carrier phase DD observations in factor graph.     * Handles coordinate updates, ambiguity parameters, and measurement weighting.     * Constructs DD equations from single-difference observations and transforms to optimization-friendly format with proper covariance scaling.     */virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;    /**     * @brief Debug function for factor verification     * Tests residual and Jacobian computation with current parameters     * Outputs numerical results for validation and debugging purposes     */void check(double **parameters);protected:    t_gtime _cur_time;    pair<string, string> _base_rover_site;    t_gallpar _params;    //shared_ptr<t_gbiasmodel> _gprecise_bias_model = nullptr;    t_gbiasmodel *_gprecise_bias_model = nullptr;    pair<FREQ_SEQ, GOBSBAND> _freq_band;    vector<pair<t_gsatdata, t_gsatdata>> _DD_sat_data;};

该因子的模板参数<1, 3, 1, 1>表示残差维度为1,状态变量分别为三维位置、参考卫星单差模糊度和非参考卫星单差模糊度。在Evaluate函数中,首先根据当前位置估计值计算几何距离,然后构建双差观测方程,计算残差。雅可比矩阵通过自动求导或解析求导获得,描述残差对各状态变量的偏导数。这种设计使得优化器能够高效地更新状态估计。

伪距双差因子PseudorangeDDFactor的实现与载波相位类似,但不包含整周模糊度参数。两类因子共同作用,在优化过程中相互约束,提高解算的稳定性。伪距因子在初始化阶段提供粗略位置信息,载波相位因子则在模糊度固定后提供厘米级定位精度。

4.3 RTK因子图构建与优化

RTK定位的因子图构建是一个动态过程,随着新观测数据的到来不断更新。在GREAT-FGO的t_gpvtfgo类中,processWindow函数负责每个历元的处理流程。其主要步骤包括数据准备、因子构建、优化求解和结果输出。具体而言,数据准备阶段通过_prepareData函数筛选质量合格的GNSS观测值,剔除信噪比过低、高程角过小的卫星观测。_combine_DD函数负责组合双差观测,选择信号质量最好的卫星作为参考星,与其他卫星构建双差观测方程。

因子构建阶段,系统为每组双差观测创建相应的因子对象,加入优化问题。对于载波相位观测,创建CarrierphaseDDFactor;对于伪距观测,创建PseudorangeDDFactor。如果存在历史边缘化先验,还会添加GNSSInfoFactor编码先验约束。初始历元还需要添加InitialPoseFactor约束初始位置。所有因子构成完整的因子图结构,通过Ceres Solver进行非线性最小二乘优化。

int gfgomsf::t_gpvtfgo::processWindow(const t_gtime & now, vector<t_gsatdata>* data_rover, vector<t_gsatdata>* data_base){    if (!_get_gdata(now, data_rover, data_base))        return -1;    t_gtime runEpoch = _data.begin()->epoch();    _epoch = runEpoch;    if (_reset_par > 0)    {        if (now.sod() % _reset_par == 0)        {            _reset_param();        }    }    // save apriory coordinates    if (_crd_est != CONSTRPAR::FIX)        _saveApr(runEpoch, _param, _Qx);    if (!_crd_xml_valid())        _sig_init_crd = 100.0;    // select obs for tb log (rover)    if (_prepareData() < 0) return -1;    if (_isBase)    {        _set_rec_info(_gallobj->obj(_site_base)->crd_arp(_epoch), _vBanc(4), _vBanc_base(4));    }    _get_initial_value(runEpoch); //for current epoch    assert(_data.size() >=_minsat);    if (_data.size() < _minsat)    {        if (_spdlog) SPDLOG_LOGGER_ERROR(_spdlog, string("gpvtfgo "), ("Not enough visible satellites!"));        return -1;    }    if (_combine_DD() < 0)    {        std::cout << "Epoch: " << runEpoch.sow() << " combine DD wrong!!!" << endl;        clearWindow();        return -1;    }    _optimization();    // ambiguity resolution    if (_last_gnss_info->valid)    {        _pre_amb_resolution();        _amb_resolution();    }    _marginalization();    //if (_rover_count == 1)    //    _initial_prior = false;    _slide_window();    return _amb_state ? 1 : 0;}

优化求解采用Levenberg-Marquardt算法,该算法结合了高斯-牛顿法和梯度下降法的优点,具有良好的收敛性能。Ceres Solver提供了高效的稀疏矩阵处理能力,能够充分利用因子图的稀疏性加速计算。优化完成后,_amb_resolution函数执行整周模糊度固定,采用LAMBDA算法搜索整数解空间,通过Ratio检验验证模糊度解的可靠性。只有当Ratio值超过设定阈值时,模糊度才被固定,系统输出固定解。

5. 基于因子图的RTK/INS紧耦合算法

5.1 IMU预积分理论

惯性测量单元IMU能够提供高频率的加速度和角速度测量,通常采样率达到100Hz甚至更高,远高于GNSS的1Hz采样率。如果将每个IMU测量都作为独立的观测加入因子图,状态变量数量将急剧增加,导致计算负担不可接受。IMU预积分技术正是为解决这一问题而提出的,它将两个GNSS观测时刻之间的多个IMU测量积分为一个相对运动约束,大幅降低了优化问题的规模。

预积分的基本思想是在IMU的局部坐标系中进行积分,避免反复计算全局坐标系下的运动轨迹。考虑j 。扣除零偏后的真实测量值为:

其中和分别为加速度计和陀螺仪零偏,和为测量噪声。在时刻的坐标系下,相对位置、速度和姿态的预积分可以递推计算:

这里为时刻的旋转矩阵,表示反对称矩阵,为重力加速度。预积分的关键优势在于,当时刻的状态估计值发生变化时,无需重新积分所有IMU测量,只需通过一阶线性修正即可更新预积分结果。这通过计算预积分对零偏的雅可比矩阵实现:

有了这些雅可比矩阵,当零偏估计更新时,预积分结果可以快速修正为:

这种设计使得IMU预积分因子能够高效地参与迭代优化过程,不会成为计算瓶颈。

5.2 IMU因子实现分析

GREAT-FGO中的IMUFactor类实现了IMU预积分约束在因子图中的表达。该因子连接相邻两个时刻的位姿、速度和零偏状态,构成一个15维残差的约束。其模板参数<15, 7, 9, 7, 9>表示残差维度为15,状态变量分别为i时刻位姿(7维quaternion+position)、i时刻速度和零偏(9维)、j时刻位姿(7维)、j时刻速度和零偏(9维)。以下是实际项目代码:

/***@brief IMUFactor Class for constructing pre-integration residual block*/class LibGREAT_LIBRARY_EXPORT IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>{public:    IMUFactor() = delete;    /**     * @brief IMU pre-integration factor constructor     * Initializes with pre-integrated IMU measurements between two states     */    IMUFactor(IntegrationBase* _pre_integration) :pre_integration(_pre_integration)    {    }    /**     * @brief Evaluate IMU pre-integration factor for optimization     *     * Computes residuals and Jacobians for IMU pre-integration between two states.     * Manages complex Jacobian computations for all state parameters across consecutive IMU frames.     */virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const{        Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);        Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);        Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);        Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);        Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);        Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);        Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);        Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);        Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);        Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);        //Eigen::Matrix<double, 15, 15> Fd;        //Eigen::Matrix<double, 15, 12> Gd;        //Eigen::Vector3d pPj = Pi + Vi * sum_t - 0.5 * g * sum_t * sum_t + corrected_delta_p;        //Eigen::Quaterniond pQj = Qi * delta_q;        //Eigen::Vector3d pVj = Vi - g * sum_t + corrected_delta_v;        //Eigen::Vector3d pBaj = Bai;        //Eigen::Vector3d pBgj = Bgi;        //Vi + Qi * delta_v - g * sum_dt = Vj;        //Qi * delta_q = Qj;        //delta_p = Qi.inverse() * (0.5 * g * sum_dt * sum_dt + Pj - Pi);        //delta_v = Qi.inverse() * (g * sum_dt + Vj - Vi);        //delta_q = Qi.inverse() * Qj;#if 0        if ((Bai - pre_integration->linearized_ba).norm() > 0.10 ||            (Bgi - pre_integration->linearized_bg).norm() > 0.01)        {            pre_integration->repropagate(Bai, Bgi);        }#endif        Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);        residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,            Pj, Qj, Vj, Baj, Bgj);        Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();        //sqrt_info.setIdentity();        //        residual = sqrt_info * residual;

IntegrationBase类负责IMU数据的预积分计算和雅可比矩阵维护。每当新的IMU测量到来时,push_back函数执行中值积分更新预积分结果,同时更新协方差矩阵和雅可比矩阵。协方差矩阵通过误差传播定律递推计算,描述了预积分结果的不确定性。在优化过程中,这个协方差矩阵的逆作为信息矩阵,对残差进行加权,确保不同类型观测的合理融合。

IMU因子的残差包含15个分量,分别对应位置误差3维、旋转误差3维、速度误差3维、加速度计零偏误差3维和陀螺仪零偏误差3维。位置残差定义为预测位置与实际位置的差:

速度和姿态残差类似地定义。零偏残差则为简单的差值,反映零偏的随机游走特性。通过最小化这些加权残差,优化器能够在GNSS观测和IMU约束之间找到最优的状态估计轨迹。

5.3 RTK/INS紧耦合因子图架构

RTK/INS紧耦合系统将GNSS双差观测因子和IMU预积分因子统一在同一个因子图中进行优化。这种架构充分发挥了两种传感器的互补优势:GNSS提供绝对位置约束,避免累积漂移;IMU提供高频运动约束,在GNSS信号遮挡期间维持定位连续性。紧耦合的关键在于状态变量的共享,GNSS因子和IMU因子同时约束位置、速度和姿态参数,使得优化器能够在两类观测间进行全局平差。

GREAT-FGO中的t_ginsfgo类实现了RTK/INS紧耦合算法。其因子图结构包含以下几类因子:首先是InitialPoseFactor和InitialBiasFactor,为初始状态提供先验约束,防止优化过程中的不适定问题。然后是IMUFactor,连接滑动窗口内相邻时刻的状态,编码IMU运动约束。CarrierphaseDDFactor和PseudorangeDDFactor提供GNSS观测约束,连接当前历元的位置状态和模糊度参数。MarginalizationFactor包含历史边缘化信息,将滑窗外的约束以先验形式保留。

系统初始化是紧耦合算法的关键环节。传统INS/GNSS组合导航通常需要静态对准,要求载体保持静止状态数十秒以完成初始姿态估计。GREAT-FGO实现了动态快速初始化能力,利用因子图优化框架在运动状态下同时估计位置、速度、姿态和IMU零偏。初始化过程首先通过纯GNSS定位获取粗略位置,然后在短时间窗口内累积IMU和GNSS观测,构建初始化因子图。通过优化求解,可以在车辆运动状态下快速完成系统初始化,显著提升用户体验。

动态初始化的实现依赖于合理的观测条件和约束设计。当GNSS观测条件良好时,位置信息能够被较准确地估计,此时IMU的主要作用是提供姿态和速度约束。初始化因子图通常包含更强的先验约束,例如对零偏范围的限制,避免优化陷入局部极小值。经过几个历元的数据累积,系统状态逐渐收敛到真实值附近,完成初始化过程。随后系统进入正常运行模式,滑动窗口不断更新,持续输出高精度定位结果。

6. 软件架构与核心模块

6.1 LibGREAT核心算法库

GREAT-FGO的算法核心封装在LibGREAT库中,该库采用模块化设计,包含多个子模块分别负责不同功能。gfactor模块定义了各类因子的实现,包括IMUFactor、CarrierphaseDDFactor、PseudorangeDDFactor、MarginalizationFactor等,这些类继承自Ceres Solver的CostFunction基类,实现了残差计算和雅可比求导接口。gfgo模块提供因子图优化的框架类,管理状态变量、因子添加、优化求解等流程。gfgognss模块实现GNSS RTK因子图算法,gfgomsf模块实现多传感器融合算法。

gambfix模块负责GNSS整周模糊度解算,实现了LAMBDA算法及其改进版本。LAMBDA算法通过整数最小二乘搜索,在模糊度解空间中寻找最优整数解。该算法首先对浮点解的协方差矩阵进行降相关去相关操作,通过Z变换将相关的模糊度参数转换为近似独立的组合,然后在变换空间中进行整数搜索。搜索过程采用高效的树搜索策略,通过上下界剪枝避免遍历全部候选解。找到的候选解需要通过Ratio检验,只有当最优解与次优解的残差比值超过阈值时,才认为模糊度固定可靠。

gins模块实现了基础的惯性导航算法,包括姿态更新、速度更新、位置更新和误差方程推导。姿态更新采用四元数微分方程或旋转向量算法,能够精确处理高动态情况下的姿态变化。速度和位置更新通过在导航坐标系下积分加速度计输出实现,需要考虑地球自转和重力模型的影响。gmodels模块提供各类误差改正模型,如对流层延迟模型、电离层延迟模型、天线相位中心改正、固体潮改正等,这些模型对于高精度定位至关重要。

6.2 LibGnut GNSS数据处理库

LibGnut库提供底层的GNSS数据处理功能,是GREAT-FGO的基础支撑。gcoders模块实现各类GNSS数据格式的编解码,支持RINEX 2/3观测文件、导航电文文件、SP3精密星历文件、SINEX站坐标文件、ANTEX天线文件等标准格式。RINEX是GNSS数据交换的通用格式,包含接收机观测值、卫星导航电文等信息。SP3格式存储精密卫星轨道和钟差产品,精度优于广播星历。这些编解码器使得软件能够读取各种来源的GNSS数据,具有良好的通用性。

gdata模块定义了GNSS数据的内部表示结构,包括观测数据类gobsgnss、星历数据类gnav及其派生类gnavgps/gnavglo/gnavgal/gnavbds、卫星数据类gsatdata等。这些类封装了GNSS观测和导航电文的详细信息,提供便捷的访问接口。gmodels模块实现各类GNSS相关模型,如Bancroft算法用于单点定位初始化,对流层延迟模型包括Saastamoinen模型、GPT模型等,天线相位中心改正使用ANTEX文件提供的参数。

gproc模块提供基础的定位解算功能,包括单点定位SPP和精密单点定位PPP算法。单点定位使用伪距观测和广播星历,通过最小二乘或扩展卡尔曼滤波估计位置和接收机钟差,定位精度通常为米级。精密单点定位使用载波相位观测和精密星历、精密钟差产品,能够实现分米至厘米级定位,但需要较长的收敛时间。这些算法为因子图优化提供初值和参考,也可以独立使用。

LibGnut库的核心数据结构用于存储和管理GNSS观测数据。观测数据类t_gobsgnss封装了单个历元的观测信息,包括时间标签、卫星编号、各类观测值(伪距、载波相位)以及数据质量指示(失锁标志LLI、信噪比SNR)。卫星数据类t_gsatdata在观测数据基础上增加了计算得到的中间结果,如卫星位置和速度、钟差改正、对流层和电离层延迟等。这些类提供了统一的数据接口,使得上层算法可以方便地访问和处理GNSS数据,无需关心底层格式细节。

7. 软件安装与使用指南

7.1 编译环境配置

GREAT-FGO采用CMake跨平台构建系统,支持在Windows和Linux环境下编译。Windows平台推荐使用Visual Studio 2019或更高版本,需要安装CMake 3.15及以上。首先从GitHub仓库克隆或下载源代码,解压到本地目录。打开CMake GUI工具,设置源代码路径为GREAT-FGO根目录,设置构建路径为build子目录。点击Configure按钮,选择Visual Studio编译器和平台架构x64,CMake将自动检测依赖库并生成配置。

第三方库已经预编译并包含在Third-party目录中,无需用户额外安装。这些库包括Ceres Solver、Eigen、spdlog等,针对不同平台提供了相应的静态库或动态库文件。CMake配置完成后,点击Generate按钮生成Visual Studio解决方案文件。打开生成的.sln文件,在Visual Studio中选择Release配置,编译整个解决方案。编译成功后,可执行文件将生成在build/Release目录下,包括GREAT_PVTFGO.exe用于RTK定位,GREAT_GINSFGO.exe用于RTK/INS组合导航。

Linux平台编译相对简单,确保系统安装了GCC 7.0或更高版本、CMake 3.15及以上。在终端进入GREAT-FGO目录,创建build子目录并进入,执行cmake ..命令生成Makefile,然后执行make命令编译。编译过程可能需要几分钟时间,取决于计算机性能。如果编译出现错误,检查依赖库是否完整,确认编译器版本是否满足要求。成功编译后,可执行文件位于build目录下,可通过./GREAT_PVTFGO命令运行。

# Linux编译步骤cd GREAT-FGOmkdir build && cd buildcmake .. -DCMAKE_BUILD_TYPE=Releasemake -j4  # 使用4个线程并行编译,加快速度

7.2 配置文件设置

GREAT-FGO通过XML格式的配置文件控制算法参数和运行选项。sample_data目录下的示例数据包含完整的配置文件,用户可以基于这些模板进行修改。配置文件主要包括以下几个部分:输入输出设置,指定GNSS观测文件、导航电文文件、IMU数据文件、输出结果文件的路径;处理时间段设置,定义处理的开始和结束时间;测站信息设置,包括基站和流动站的近似坐标、天线高等;处理选项设置,选择定位模式RTK或RTK/INS、卫星系统、观测值类型、截止高度角等。

因子图优化相关参数也在配置文件中设置,包括滑动窗口大小、边缘化策略、优化器参数等。滑动窗口大小决定了保留多少历史状态,通常设置为5到10个历元,在精度和计算效率间取得平衡。优化器可选择Levenberg-Marquardt或Dogleg算法,设置最大迭代次数、收敛阈值等。IMU相关参数包括噪声模型、采样率、零偏初值等,需要根据实际传感器规格设置。模糊度固定参数包括Ratio检验阈值,通常设置为2.0到3.0,值越大固定越保守。

<!-- 配置文件示例片段 --><config>  <!-- 处理时间段 -->  <gen>    <beg>2021-10-12 00:00:00</beg>    <end>2021-10-12 01:00:00</end>    <sampling>1.0</sampling>  </gen>  <!-- 测站信息 -->  <rec>    <rec id="BASE">      <X>-2267749.123</X>      <Y>5009154.789</Y>      <Z>3220896.456</Z>    </rec>    <rec id="ROVER">      <X>0</X><Y>0</Y><Z>0</Z>  <!-- 0表示未知 -->    </rec>  </rec>  <!-- 处理选项 -->  <proc>    <sys>GPS+BDS</sys>    <mode>RTK</mode>    <freq>L1+L2</freq>    <elev>10</elev>  <!-- 截止高度角(度) -->  </proc>  <!-- 因子图参数 -->  <fgo>    <window_size>7</window_size>    <solver>LM</solver>  <!-- Levenberg-Marquardt -->    <max_iter>30</max_iter>  </fgo>  <!-- IMU参数 -->  <imu>    <rate>100</rate>  <!-- 采样率Hz -->    <acc_noise>0.01</acc_noise>  <!-- 加速度计噪声m/s²/√Hz -->    <gyro_noise>0.001</gyro_noise>  <!-- 陀螺噪声rad/s/√Hz -->  </imu></config>

7.3 运行示例与结果查看

RTK/INS组合导航的运行方式类似,使用GREAT_GINSFGO可执行文件和相应的配置文件。由于涉及IMU数据处理,输出结果除了位置,还包含详细的速度和姿态信息。软件支持多种IMU数据格式,用户需要在配置文件中指定格式类型和数据文件路径。如果IMU格式不在预定义列表中,可以参考源代码中的gimudata.h文件,添加自定义格式的解析代码,然后重新编译软件。这种灵活性使得GREAT-FGO能够适应各种应用场景。

# 运行RTK定位示例cd sample_data/FGO_20211012../../build/Release/GREAT_PVTFGO.exe -c config_rtk.xml# 运行RTK/INS组合导航示例cd sample_data/FGO_20211012../../build/Release/GREAT_GINSFGO.exe -c config_rtkins.xml# 生成分析报告cd plotpython Evaluation.py ../sample_data/FGO_20211012/output/

8. 总结与展望

GREAT-FGO作为武汉大学GREAT团队多年研究成果的结晶,将因子图优化这一先进技术引入GNSS精密定位领域,为高精度导航应用提供了新的解决方案。软件实现了基于因子图优化的RTK和RTK/INS紧耦合算法,支持GPS、GLONASS、Galileo、BDS等多卫星系统,具有定位精度高、鲁棒性强、扩展性好等显著优势。通过模块化设计和成熟第三方库的集成,GREAT-FGO实现了算法的高效实现和工程化部署。丰富的测试数据和可视化工具降低了使用门槛,使得广大研究人员和工程技术人员能够快速掌握和应用该技术。

9. 参考链接