GVIL：基于图优化的GNSS PPP/视觉/惯性/激光雷达紧组合算法 GVIL： Tightly-Coupled GNSS PPP/Visual/INS/LiDAR SLAM Based on Graph Optimization 1 first-author 廖健驰，博士，助理研究员，主要从事组合导航理论和方法研究。liaojianchi@whu.edu.cn 廖健驰，博士，助理研究员，主要从事组合导航理论和方法研究。liaojianchi@whu.edu.cn liaojianchi@whu.edu.cn 1 2 corresp 李星星，博士，教授。xxli@sgg.whu.edu.cn 李星星，博士，教授。xxli@sgg.whu.edu.cn xxli@sgg.whu.edu.cn 2 湖北珞珈实验室，湖北 武汉，430079 湖北珞珈实验室，湖北 武汉，430079 Hubei Luojia Laboratory, Wuhan University, Wuhan 430079,China Hubei Luojia Laboratory, Wuhan University, Wuhan 430079,China 武汉大学测绘学院，湖北 武汉，430079 武汉大学测绘学院，湖北 武汉，430079 School of Geodesy and Geomatics, Wuhan University, Wuhan 430079,China School of Geodesy and Geomatics, Wuhan University, Wuhan 430079,China 高精度定位与导航服务在移动机器人、无人机与自动驾驶等新兴领域中发挥着至关重要的作用。视觉/惯性/激光雷达组合算法相较于视觉/惯性组合算法，可同时利用环境的空间结构与纹理信息以实现更为鲁棒的位姿估计结果，然而其在大尺度场景下仍存在误差累计问题。为此提出了一种全球导航卫星系统（global navigation satellite system，GNSS）精密单点定位（precise point positioning，PPP）/视觉/惯性/激光雷达紧组合算法。该算法首先通过4种传感器的联合初始化，实现了不同传感器空间基准的统一；然后,用双频无电离层组合后的GNSS伪距、相位观测值与视觉、惯性、激光雷达原始观测值共同构成误差因子；最后，通过基于关键帧与滑动窗口的因子图优化实现了全局位姿的精确、鲁棒估计。经车载实验验证，所提出的GNSS PPP/视觉/惯性/激光雷达紧组合算法通过4种传感器在原始观测值层面的组合，可以显著提升系统在复杂环境下的位姿估计的精度、连续性与可靠性，实现无缝导航。 Objectives 2 High-precision positioning and navigation services play a crucial role in emerging fields such as mobile robots, drones, and autonomous driving. Compared with visual-inertial system algorithms, visual-inertial-LiDAR fusion algorithms can utilize the spatial structure and texture information of the environment to achieve more robust pose estimation results. However, they still suffer from error accumulation problems in large-scale scenes. Therefore, we propose a global navigation satellite system (GNSS) precise point positioning (PPP)/vision/inertial/LiDAR tightly-coupled fusion algorithm (GVIL). Methods 2 First, the algorithm initially performs a joint initialization of four sensors, which results in the unification of the spatial reference frames of the different sensors. Second, the original observations from vision, inertial, and LiDAR are combined with the dual-frequency ionosphere-free combination of GNSS pseudo range and phase observations to generate error factors. Finally, our algorithm achieves global pose estimation by using factor graph optimization based on keyframe strategy and sliding windows. Results 2 The vehicle-borne experiment results show that even under GNSS-constrained observation conditions, the proposed four-sensor tightly-coupled algorithm can improve the position estimation accuracy by more than 84% and the attitude estimation accuracy by more than 66% compared with the VIL combination algorithm. Conclusions 2 It has been demonstrated that the GVIL algorithm can significantly enhance the accuracy, continuity, and reliability of pose estimation in complex environments by combining the raw observation data from four types of sensors, achieving continuous navigation. GNSS 图优化 同时定位与建图（SLAM） 传感器组合 GNSS graph optimization simultaneous localization and mapping (SLAM) sensor fusion 随着智能载体所搭载传感器的不断增多，如何有效地利用多源传感器相互组合，充分发挥不同传感器优势，是近年来导航定位领域研究的热点问题，也是实现复杂环境下的高精度、高可靠、高完好性导航定位服务的关键 ［ 1 1 ］ 。近年来，以相机、惯导、激光雷达等传感器为基础的同时定位与建图（simultaneous localization and mapping， SLAM）技术在相对定位领域中得到了广泛研究 ［ 2 2 ］ 。视觉惯性组合系统（visual-inertial system， VINS）组合相机与惯性传感器，利用惯性设备短时间内预报精度较高的特性，为视觉传感器在高动态、弱纹理等退化场景下提供良好信息补充 ［ 3 3 ］ 。文献［ 4 4 ］利用惯性测量单元（inertial measurement unit， IMU）积分对系统状态进行预报，提出了基于多状态约束卡尔曼滤波（multi-state constraint Kalman filter，MSCKF）的视觉惯性里程计（visual-inertial odometry， VIO），在其基础上基于滤波的VINS算法得到迅速发展；文献［ 5 5 ］在原有的视觉惯性里程计上增加了对双目相机观测与相机外参估计的支持；文献［ 6 6 ］将参考坐标由世界坐标系转换至当前载体的相机坐标系，避免了系统初始化问题。随着研究的深入，SLAM求解中的天然稀疏性使得基于因子图优化的VINS求解变为可能。文献［ 7 7 ］将IMU观测值构建为预积分项加入后端图优化中；文献［ 8 8 ］在其基础上采用关键帧与滑动窗口策略实现了针对单目相机的VIO算法，具有初始化、重定位、闭环检测等功能。相较于滤波算法，基于因子图优化的算法可以更好地保留历史观测信息，在求解过程中可以进行多次线性化且对问题的描述更为直观，因而在后续的研究中得到了更为广泛的应用 ［ 9 9 ］ 。视觉传感器高度依赖环境纹理特征，在一些弱纹理、低光照的环境下表现不佳。相比之下，激光雷达不受环境光照影响并提供了空间结构特征，与VINS算法相结合可以实现信息互补，基于视觉、惯性、雷达三者组合系统的（visual-inertial-LiDAR system，VIL）算法受到了广泛关注。文献［ 10 10 ］以MSCKF框架为基础实现了3种传感器基于滤波的紧组合算法，并对IMU到相机与激光雷达的外参在线估计；文献［ 11 11 - 12 12 11-12 ］以图优化为基础实现了3种传感器的紧组合，通过雷达点云辅助视觉恢复深度，并利用视觉特征增强的激光雷达闭环优化，显著提升了算法的精度与可靠性。 尽管VIL算法无论在理论上还是精度上都取得了长足进步，但作为相对定位算法，随着算法运行时间的增加，其导航误差会不断累计，最终导致整个系统定位结果发散。全球导航卫星系统（global navigation satellite system，GNSS）作为目前唯一一个可以全球全天候、全天时运行的导航定位系统，除可以提供高精度的全局位置信息外，还可以为不同传感器的组合提供统一的时间、空间基准 ［ 13 13 ］ 。通过将GNSS、视觉、惯性和激光雷达传感器进行融合，可以充分挖掘不同传感器的优势，极大地提升组合导航系统的性能与鲁棒性，是当前多源传感器组合算法的重要发展方向。在早期的研究中，GNSS与惯性、视觉等传感器的组合主要以松组合为主，如文献［ 14 14 - 15 15 14-15 ］以MSCKF框架为基础，通过松组合的方式将GPS位置解算结果与视觉、惯性量测组合，实现了对无人机位置和姿态的鲁棒估计；文献［ 16 16 ］利用因子图将GNSS解算结果同VINS结合，并通过卡方检测剔除异常观测，以提高其在动态环境下的精度。为了进一步挖掘不同传感器的有效信息，其组合方式由松组合逐步向紧组合不断发展，文献［ 17 17 ］以因子图优化为基础提出了一种GNSS与VINS半紧组合算法，将局部里程的位置估计构建为虚拟观测方程，并将其作为位置先验约束加入GNSS的求解，提升了GNSS解算的精度和可用性。文献［ 18 18 ］以MSCKF框架为基础，实现了多频多系统实时动态定位（real-time kinematic，RTK）与视觉、惯性间的紧组合，并在通过车载实验与RTK/惯性紧组合算法进行了对比验证，实验结果表明，相比于RTK/惯性组合结果，视觉信息的加入可以显著降低惯导的发散，提升组合算法的精度。文献［ 19 19 - 20 20 19-20 ］分别以图优化为基础实现了GNSS伪距和多普勒观测值与视觉、惯性的紧组合，通过GNSS观测消除了VIO的累计误差，实现了室内外无缝导航。目前，与GNSS组合相关的研究主要集中在GNSS、视觉、惯性间的组合上，而对GNSS、视觉、惯性、激光雷达4种传感器间紧组合的相关研究还比较少。在组合方式上多采用基于MSCKF的滤波框架对GNSS伪距的单点定位或多普勒观测值进行融合，而基于RTK与精密单点定位（precise point positioning，PPP）等GNSS高精度定位算法和SLAM中广泛使用的图优化模型的相关组合算法仍处于起步阶段。 综上所述，本文提出了一种基于图优化的GNSS PPP/视觉/惯性/激光雷达紧组合算法（GNSS PPP/vision/inertial/LiDAR tightly-coupled fusion algorithm，GVIL），该算法通过4种传感器的联合初始化统一了不同传感器的空间基准，以因子图优化为基础将双频无电离层组合的GNSS伪距和相位观测值、视觉特征点、惯性数据、激光雷达特征点云在原始观测值层面进行融合，实现了多源传感器信息的有效利用，并通过室外复杂环境下的车载实验对算法综合性能进行了验证。 1　系统状态量 1 为了方便后续算法介绍，首先给出本文所涉及的参考坐标系基本定义。 1）地心地固坐标系。地心地固坐标系（Earth-centered Earth-fixed frame， <math id="M1"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系）原点位于地球中心，坐标轴与地球固连并随地球旋转， <math id="M2"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079229&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079216&type= 2.53999996 2.79399991 轴平行于地球平均自转轴， <math id="M3"><mi>X</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079209&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079207&type= 2.96333337 2.79399991 轴位于赤道平面内指向赤道与平均格林尼治子午线的交点， <math id="M4"><mi>Y</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079213&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079196&type= 3.13266683 2.79399991 轴与 <math id="M5"><mi>X</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079209&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079207&type= 2.96333337 2.79399991 轴和 <math id="M6"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079229&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079216&type= 2.53999996 2.79399991 轴互相垂直构成右手坐标系。 2）载体坐标系。载体坐标系（body frame， <math id="M7"><mi>b</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079162&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079172&type= 1.86266661 2.79399991 系）原点选在惯性设备的量测中心，载体坐标系 <math id="M8"><mi>X</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079209&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079207&type= 2.96333337 2.79399991 轴沿载体的横轴方向向右， <math id="M9"><mi>Y</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079213&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079196&type= 3.13266683 2.79399991 轴沿载体前进方向， <math id="M10"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079229&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079216&type= 2.53999996 2.79399991 轴沿着载体的立轴方向向上，构成右-前-上载体坐标系。 3）导航坐标系。导航坐标系（navigation coordinate system， <math id="M11"><mi>n</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079205&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079203&type= 1.86266661 2.79399991 系）又称当地水平坐标系，其原点与载体坐标系相同， <math id="M12"><mi>X</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079209&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079207&type= 2.96333337 2.79399991 轴沿参考椭球卯酉圈切线方向向东， <math id="M13"><mi>Y</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079213&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079196&type= 3.13266683 2.79399991 轴沿当前参考椭球子午线切线方向向北， <math id="M14"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079229&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079216&type= 2.53999996 2.79399991 轴垂直于参考椭球面向上，构成东-北-天（east-north-up， ENU）坐标系。 4）世界坐标系。世界坐标系（world frame， <math id="M15"><mi>w</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080209&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080208&type= 2.96333337 2.79399991 系）是SLAM算法中最为常见的坐标系之一，它是一种人为指定的参考坐标系，其坐标轴彼此正交且符合右手定则，原点与坐标轴朝向根据需求的不同可以任意指定。 为了获取系统的全局位姿信息，系统状态量被定义在以IMU为中心的地心地固坐标系下。为减少计算量，本文基于滑动窗口对系统状态进行估计，窗口中的系统状态量 <math id="M16"><mi>χ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079237&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079235&type= 2.03200006 3.64066648 可表示为： <math display="block" id="M17"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><mi>χ</mi><mo>=</mo><mo stretchy="false">{</mo><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><mi>i</mi></mrow></msub><mo stretchy="false">}</mo></mtd></mtr><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mtext>  </mtext><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mtext>  </mtext><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mtext>  </mtext><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><msub><mrow><mi>I</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><mtext>  </mtext><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><msub><mrow><mi>C</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><mtext>  </mtext><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><msub><mrow><mi>G</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></mrow></mfenced></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079242&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079243&type= 44.11133575 11.26066685 式中， <math id="M18"><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><mi>i</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080091&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080090&type= 3.04800010 3.72533321 表示滑动窗口中 <math id="M19"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻的系统状态； <math id="M20"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079593&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079586&type= 4.06400013 3.89466691 、 <math id="M21"><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079587&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079599&type= 4.48733330 3.80999994 、 <math id="M22"><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079275&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079272&type= 3.64066648 3.80999994 分别表示 <math id="M23"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻系统在 <math id="M24"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系下的位置、姿态与速度； <math id="M25"><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><msub><mrow><mi>I</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079304&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079286&type= 3.55599999 3.64066648 为窗口中与IMU相关的状态量； <math id="M26"><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><msub><mrow><mi>C</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079293&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079308&type= 4.23333359 3.64066648 与 <math id="M27"><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><msub><mrow><mi>G</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080092&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080074&type= 4.40266657 3.64066648 表示窗口中与相机相和GNSS相关的状态量。 <math display="block" id="M28"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msub><mrow><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><mi>I</mi></mrow></msub></mrow><mrow><msub><mrow/><mrow><mi>i</mi></mrow></msub></mrow></msub><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><mtext>  </mtext><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>g</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></mrow></mfenced></mtd></mtr><mtr><mtd><msub><mrow><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><mi>C</mi></mrow></msub></mrow><mrow><msub><mrow/><mrow><mi>i</mi></mrow></msub></mrow></msub><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><msub><mrow><mi>λ</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mtext>  </mtext><msub><mrow><mi>λ</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mtext>  </mtext><mo>⋯</mo><mtext>  </mtext><msub><mrow><mi>λ</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></mfenced></mtd></mtr><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><msub><mrow><mi>G</mi></mrow><mrow><mi>t</mi></mrow></msub></mrow></msub><mo>=</mo><mo stretchy="false">[</mo><msub><mrow><mi>t</mi></mrow><mrow><mi>r</mi><mo>,</mo><mi>s</mi></mrow></msub><mtext>  </mtext><mi>T</mi><mtext>  </mtext><msub><mrow><mi>N</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mtext>  </mtext><msub><mrow><mi>N</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mtext>  </mtext><mo>⋯</mo><mtext>  </mtext><msub><mrow><mi>N</mi></mrow><mrow><mi>m</mi></mrow></msub><mo stretchy="false">]</mo></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079300&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079316&type= 44.61933517 17.10266685 式中， <math id="M29"><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079321&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079319&type= 3.55599999 3.64066648 和 <math id="M30"><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>g</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079326&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079323&type= 3.55599999 3.72533321 分别表示 <math id="M31"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻IMU的加速度计零偏与陀螺零偏； <math id="M32"><mi>λ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080041&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080039&type= 1.77800000 2.79399991 为 <math id="M33"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻系统所能跟踪到的相机特征点的逆深度； <math id="M34"><msub><mrow><mi>t</mi></mrow><mrow><mi>r</mi><mo>,</mo><mi>s</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079352&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079351&type= 3.72533321 3.72533321 为 <math id="M35"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻不同卫星系统的接收机钟差， r 表示接收机， <math id="M36"><mi>s</mi><mo>∈</mo><mfenced separators="|"><mrow><mi mathvariant="normal">G</mi><mo>,</mo><mi mathvariant="normal">R</mi><mo>,</mo><mi mathvariant="normal">E</mi><mo>,</mo><mi mathvariant="normal">C</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079363&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079345&type= 23.96066666 3.97933316 ， <math id="M37"><mi mathvariant="normal">G</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079361&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079366&type= 2.79399991 2.79399991 、 <math id="M38"><mi mathvariant="normal">R</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079381&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079369&type= 2.62466669 2.79399991 、 <math id="M39"><mi mathvariant="normal">E</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079385&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079382&type= 2.53999996 2.79399991 、 <math id="M40"><mi mathvariant="normal">C</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079377&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079387&type= 2.62466669 2.79399991 分别表示GPS、格洛纳斯导航卫星系统（global navigation satellite system， GLONASS）、伽利略导航卫星系统（Galileo navigation satellite system，Galileo）与北斗卫星导航系统（BeiDou satellite navigation system， BDS） 4大卫星导航系统； <math id="M41"><mi>T</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079395&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079390&type= 2.87866688 2.79399991 表示 <math id="M42"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻的对流层延迟； <math id="M43"><mi>N</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079413&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079411&type= 3.30200005 2.87866688 为 <math id="M44"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻系统所能观测到卫星的载波相位整周模糊度。 2　误差因子构建 1 2.1　IMU误差因子构建 2 本文通过预积分算法 ［ 7 7 ， 21 21 ］ 来构建IMU误差因子，与VINS中IMU预积分项相比，本文的系统状态量表示在 <math id="M45"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系下，其预积分项受到重力方向的影响而存在差异，因此需要给出 <math id="M46"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系下预积分的形式，由IMU量测模型可得其加速度与角速度的量测方程为： <math display="block" id="M47"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msub><mrow><mover><mrow><mi mathvariant="bold-italic">a</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>t</mi></mrow></msub><mo>=</mo><msub><mrow><mi mathvariant="bold-italic">a</mi></mrow><mrow><mi>t</mi></mrow></msub><mo>+</mo><msup><mrow><mfenced separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>t</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><mi>e</mi></mrow></msup><mo>+</mo><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>t</mi></mrow></msub></mrow></msub><mo>+</mo><msub><mrow><mi mathvariant="bold-italic">n</mi></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>t</mi></mrow></msub></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mover><mrow><mi mathvariant="bold-italic">ω</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>t</mi></mrow></msub><mo>=</mo><msub><mrow><mi mathvariant="bold-italic">ω</mi></mrow><mrow><mi>t</mi></mrow></msub><mo>+</mo><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>ω</mi></mrow><mrow><mi>t</mi></mrow></msub></mrow></msub><mo>+</mo><msub><mrow><mi mathvariant="bold-italic">n</mi></mrow><mrow><msub><mrow><mi>ω</mi></mrow><mrow><mi>t</mi></mrow></msub></mrow></msub></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079428&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079427&type= 47.07466888 11.59933376 式中， <math id="M48"><msub><mrow><mover><mrow><mi mathvariant="bold-italic">a</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>t</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079446&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079443&type= 2.70933342 3.72533321 与 <math id="M49"><msub><mrow><mover><mrow><mi mathvariant="bold-italic">ω</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>t</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079436&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079433&type= 3.38666677 3.72533321 为 <math id="M50"><mi>t</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079548&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079546&type= 1.26999998 2.79399991 时刻IMU输出的加速度与角速度量测值； <math id="M51"><msub><mrow><mi mathvariant="bold-italic">a</mi></mrow><mrow><mi>t</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079468&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079458&type= 2.70933342 3.72533321 与 <math id="M52"><msub><mrow><mi mathvariant="bold-italic">ω</mi></mrow><mrow><mi>t</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079469&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079464&type= 3.38666677 3.72533321 为IMU测量的真实加速度与角速度； <math id="M53"><msub><mrow><mi mathvariant="bold-italic">n</mi></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>t</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079483&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079473&type= 3.72533321 3.64066648 与 <math id="M54"><msub><mrow><mi mathvariant="bold-italic">n</mi></mrow><mrow><msub><mrow><mi>ω</mi></mrow><mrow><mi>t</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079487&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079477&type= 4.06400013 3.64066648 为加速度与角速度的量测噪声，本文中将其建模为高斯白噪声； <math id="M55"><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>t</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079490&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079482&type= 3.47133350 3.64066648 与 <math id="M56"><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>ω</mi></mrow><mrow><mi>t</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079501&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079492&type= 3.80999994 3.64066648 为加速度与角速度零偏，本文将其建模为随机游走噪声； <math id="M57"><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><mi>e</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079513&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079503&type= 3.30200005 3.89466691 表示 <math id="M58"><mi>t</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079548&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079546&type= 1.26999998 2.79399991 时刻系统位置重力在 <math id="M59"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系下的投影，可以由 <math id="M60"><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><mi>e</mi></mrow></msup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>n</mi></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><mi>n</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079510&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079527&type= 15.74800014 4.82600021 计算得到。其中， <math display="block" id="M61"><mtext> </mtext><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>n</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mo>-</mo><mi mathvariant="normal">s</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi>η</mi></mtd><mtd><mo>-</mo><mi mathvariant="normal">s</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi>φ</mi><mi mathvariant="normal">c</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">s</mi><mi>η</mi></mtd><mtd><mi mathvariant="normal">c</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">s</mi><mi>φ</mi><mi mathvariant="normal">c</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">s</mi><mi>η</mi></mtd></mtr><mtr><mtd><mi mathvariant="normal">c</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">s</mi><mi>η</mi></mtd><mtd><mo>-</mo><mi mathvariant="normal">s</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi>φ</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi>η</mi></mtd><mtd><mi mathvariant="normal">c</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">s</mi><mi>φ</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi>η</mi></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mi mathvariant="normal">c</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">s</mi><mi>φ</mi></mtd><mtd><mi mathvariant="normal">s</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi>φ</mi></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079544&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079530&type= 64.43132782 16.25600052 式中， <math id="M62"><mi>η</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079539&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079536&type= 1.86266661 3.64066648 与 <math id="M63"><mi>φ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079561&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079542&type= 2.37066650 3.72533321 分别代表 <math id="M64"><mi>t</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079548&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079546&type= 1.26999998 2.79399991 时刻系统位置的经度与纬度； <math id="M65"><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><mi>n</mi></mrow></msup><mo>=</mo><msup><mrow><mfenced open="[" close="]" separators="|"><mrow><mn mathvariant="normal">0</mn><mtext>  </mtext><mn mathvariant="normal">0</mn><mtext>  </mtext><mo>-</mo><mi>g</mi></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079565&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079551&type= 26.24666595 5.50333309 为当地水平坐标系下的重力向量。对于任意 <math id="M66"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079960&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079959&type= 1.60866666 3.64066648 时刻的系统位置 <math id="M67"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079574&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079572&type= 3.97933316 4.14866638 、速度 <math id="M68"><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079578&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079576&type= 3.64066648 4.14866638 、姿态 <math id="M69"><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079582&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079580&type= 4.48733330 4.14866638 ， 可由 <math id="M70"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻的系统位置 <math id="M71"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079593&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079586&type= 4.06400013 3.89466691 、速度 <math id="M72"><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079597&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079595&type= 3.64066648 3.80999994 、姿态 <math id="M73"><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079587&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079599&type= 4.48733330 3.80999994 和IMU在 <math id="M74"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻与 <math id="M75"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079960&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079959&type= 1.60866666 3.64066648 时刻间量测值在 <math id="M76"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系下积分得到： <math display="block" id="M77"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>+</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mo stretchy="false">[</mo><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mi mathvariant="normal">Δ</mi><mi>t</mi><mo>+</mo><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><msup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>+</mo></mrow></mtd></mtr><mtr><mtd><mtext>          </mtext><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">a</mi></mrow><mrow><mi>k</mi></mrow></msub><mo>-</mo><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mo stretchy="false">)</mo><mi mathvariant="normal">Δ</mi><msup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo stretchy="false">]</mo></mtd></mtr><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>+</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><mi>t</mi><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">a</mi></mrow><mrow><mi>k</mi></mrow></msub><mo>-</mo><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mo stretchy="false">)</mo><mi mathvariant="normal">Δ</mi><mi>t</mi></mrow></mtd></mtr><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mstyle displaystyle="true"><munderover><mo>∏</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mi mathvariant="normal">e</mi><mi mathvariant="normal">x</mi><mi mathvariant="normal">p</mi><mo stretchy="false">(</mo><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">ω</mi></mrow><mrow><mi>k</mi></mrow></msub><mo>-</mo><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>ω</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mo stretchy="false">)</mo><mi mathvariant="normal">Δ</mi><mi>t</mi><mo stretchy="false">)</mo></mrow></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079629&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079627&type= 64.76999664 40.97866440 可以看到在积分过程中，重力向量的积分变化主要由 <math id="M78"><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079605&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079631&type= 4.65666676 3.80999994 决定，而 <math id="M79"><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079634&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079633&type= 4.65666676 3.80999994 的变化主要来自于载体位置的变化，相比于量测噪声，其在积分时刻内所导致的重力方向变化很小，可以视为常量，即： <math display="block" id="M80"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><msup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>=</mo></mrow><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><msup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>=</mo></mrow><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><msubsup><mrow><mi>t</mi></mrow><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd></mtr><mtr><mtd><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><mi>t</mi><mo>=</mo></mrow><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><mi>t</mi><mo>=</mo></mrow><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><msub><mrow><mi>t</mi></mrow><mrow><mi>i</mi><mo>,</mo><mi>j</mi></mrow></msub></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079636&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079608&type= 73.57534027 20.48933220 式中， <math id="M81"><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mi mathvariant="normal">Δ</mi><mi>t</mi></mrow><mo>=</mo><mi mathvariant="normal">Δ</mi><msub><mrow><mi>t</mi></mrow><mrow><mi>i</mi><mi>j</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079611&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079610&type= 17.94933319 8.97466660 。 则 式（5） 式（5） 可以改写为： <math display="block" id="M82"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mi mathvariant="normal">Δ</mi><msub><mrow><mi>t</mi></mrow><mrow><mi>i</mi><mi>j</mi></mrow></msub><mo>+</mo><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><msubsup><mrow><mi>t</mi></mrow><mrow><mi>i</mi><mi>j</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>+</mo></mtd></mtr><mtr><mtd><mtext>          </mtext><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">a</mi></mrow><mrow><mi>k</mi></mrow></msub><mo>-</mo><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mo stretchy="false">)</mo><mi mathvariant="normal">Δ</mi><msup><mrow><mi>t</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></mrow></mtd></mtr><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><msub><mrow><mi>t</mi></mrow><mrow><mi>i</mi><mi>j</mi></mrow></msub><mo>+</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">a</mi></mrow><mrow><mi>k</mi></mrow></msub><mo>-</mo><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mo stretchy="false">)</mo><mi mathvariant="normal">Δ</mi><mi>t</mi></mrow></mtd></mtr><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mstyle displaystyle="true"><munderover><mo>∏</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mi mathvariant="normal">e</mi><mi mathvariant="normal">x</mi><mi mathvariant="normal">p</mi><mo stretchy="false">(</mo><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">ω</mi></mrow><mrow><mi>k</mi></mrow></msub><mo>-</mo><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>ω</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mo stretchy="false">)</mo><mi mathvariant="normal">Δ</mi><mi>t</mi><mo stretchy="false">)</mo></mrow></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079640&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079639&type= 65.78600311 40.97866440 对 式（7） 式（7） 进行移项整理，左右两侧同乘 <math id="M83"><msup><mrow><mfenced separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079643&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079641&type= 11.00666618 5.41866684 并代入预积分项可得： <math display="block" id="M84"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>e</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup><mfenced separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mi mathvariant="normal">Δ</mi><msub><mrow><mi>t</mi></mrow><mrow><mi>i</mi><mi>j</mi></mrow></msub><mo>-</mo><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><msubsup><mrow><mi>t</mi></mrow><mrow><mi>i</mi><mi>j</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow></mfenced><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">α</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>e</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup><mfenced separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><msub><mrow><mi>t</mi></mrow><mrow><mi>i</mi><mi>j</mi></mrow></msub></mrow></mfenced><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">β</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>e</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">γ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079647&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079645&type= 66.63265991 22.77533340 式中， <math id="M85"><msubsup><mrow><mi mathvariant="bold-italic">α</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079650&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079648&type= 3.72533321 4.14866638 、 <math id="M86"><msubsup><mrow><mi mathvariant="bold-italic">β</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079653&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079651&type= 4.06400013 4.14866638 和 <math id="M87"><msubsup><mrow><mi mathvariant="bold-italic">γ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079617&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079616&type= 3.80999994 4.14866638 为 <math id="M88"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻到 <math id="M89"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079960&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079959&type= 1.60866666 3.64066648 时刻间的IMU预积分项。 由此可得 <math id="M90"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系下IMU预积分项的残差 <math id="M91"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>I</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080113&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080081&type= 2.45533323 3.72533321 可以表示为： <math display="block" id="M92"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>I</mi></mrow></msub><mo stretchy="false">(</mo><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">z</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup><mo>,</mo><mi mathvariant="bold-italic">x</mi><mo stretchy="false">)</mo><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mi mathvariant="normal">δ</mi><msubsup><mrow><mi mathvariant="bold-italic">α</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></mtd></mtr><mtr><mtd><mi mathvariant="normal">δ</mi><msubsup><mrow><mi mathvariant="bold-italic">β</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></mtd></mtr><mtr><mtd><mi mathvariant="normal">δ</mi><msubsup><mrow><mi mathvariant="bold-italic">θ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></mtd></mtr><mtr><mtd><mi mathvariant="normal">δ</mi><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>a</mi></mrow></msub></mtd></mtr><mtr><mtd><mi mathvariant="normal">δ</mi><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>g</mi></mrow></msub></mtd></mtr></mtable></mrow></mfenced><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>e</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup><mo stretchy="false">(</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mi mathvariant="normal">Δ</mi><msub><mrow><mi>t</mi></mrow><mrow><mi>i</mi><mi>j</mi></mrow></msub><mo>-</mo><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><msubsup><mrow><mi>t</mi></mrow><mrow><mi>i</mi><mi>j</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo stretchy="false">)</mo><mo>-</mo><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">α</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>e</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup><mo stretchy="false">(</mo><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><msub><mrow><mi>n</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msup><mi mathvariant="normal">Δ</mi><msub><mrow><mi>t</mi></mrow><mrow><mi>i</mi><mi>j</mi></mrow></msub><mo stretchy="false">)</mo><mo>-</mo><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">β</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></mtd></mtr><mtr><mtd><mn mathvariant="normal">2</mn><msub><mrow><mfenced open="[" close="]" separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">q</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><msup><mrow><mi>e</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msup></mrow></msubsup><mo>⊗</mo><msubsup><mrow><mi mathvariant="bold-italic">q</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>⊗</mo><msup><mrow><mfenced separators="|"><mrow><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">γ</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></mrow></mfenced></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msup></mrow></mfenced></mrow><mrow><mi>x</mi><mi>y</mi><mi>z</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>-</mo><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>ω</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>-</mo><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><msub><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079671&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079670&type= 99.05999756 38.09999847 式中， <math id="M93"><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">z</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079683&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079672&type= 3.55599999 4.14866638 为IMU观测量； <math id="M94"><mi mathvariant="normal">δ</mi><msubsup><mrow><mi mathvariant="bold-italic">α</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079685&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079674&type= 5.41866684 4.14866638 、 <math id="M95"><mi mathvariant="normal">δ</mi><msubsup><mrow><mi mathvariant="bold-italic">β</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079688&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079675&type= 5.58799982 4.14866638 和 <math id="M96"><mi mathvariant="normal">δ</mi><msubsup><mrow><mi>θ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079691&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079689&type= 5.33400011 4.14866638 为 <math id="M97"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻到 <math id="M98"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079960&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079959&type= 1.60866666 3.64066648 时刻间的IMU预积分项残差项； <math id="M99"><mi mathvariant="normal">δ</mi><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>a</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079699&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079698&type= 4.74133301 3.72533321 和 <math id="M100"><mi mathvariant="normal">δ</mi><msub><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>g</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079701&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079700&type= 4.74133301 3.72533321 表示 <math id="M101"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻到 <math id="M102"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079960&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079959&type= 1.60866666 3.64066648 时刻间的IMU加速度计与陀螺零偏项残差项； <math id="M103"><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">α</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079718&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079717&type= 3.72533321 4.14866638 、 <math id="M104"><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">β</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079726&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079719&type= 4.06400013 4.57200003 和 <math id="M105"><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">γ</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079738&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079727&type= 3.80999994 4.14866638 表示 <math id="M106"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 时刻到 <math id="M107"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079960&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079959&type= 1.60866666 3.64066648 时刻间的IMU预积分观测值。 2.2　GNSS-PPP误差因子的构建 2 PPP技术是目前应用最为广泛的GNSS实时精密定位技术之一，与差分动态定位技术相比，PPP技术可以有效摆脱对地面参考站的依赖，具有精度高、成本低、机动性强等特点 ［ 22 22 ］ 。本文采用PPP技术构建GNSS误差因子，GNSS原始伪距和载波观测可以表示为： <math display="block" id="M108"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msubsup><mrow><mi>P</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi>ρ</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup><mo>+</mo><msub><mrow><mi>t</mi></mrow><mrow><mi>r</mi></mrow></msub><mo>-</mo><msup><mrow><mi>t</mi></mrow><mrow><mi>s</mi></mrow></msup><mo>+</mo><msubsup><mrow><mi>I</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup><mo>+</mo><msubsup><mrow><mi>T</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup><mo>+</mo><msub><mrow><mi>b</mi></mrow><mrow><mi>r</mi></mrow></msub><mo>-</mo><msup><mrow><mi>b</mi></mrow><mrow><mi>s</mi></mrow></msup><mo>+</mo><msub><mrow><mi>ε</mi></mrow><mrow><mi>P</mi></mrow></msub></mtd></mtr><mtr><mtd><msubsup><mrow><mi>L</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi>ρ</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup><mo>+</mo><msub><mrow><mi>t</mi></mrow><mrow><mi>r</mi></mrow></msub><mo>-</mo><msup><mrow><mi>t</mi></mrow><mrow><mi>s</mi></mrow></msup><mo>-</mo><msubsup><mrow><mi>I</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup><mo>+</mo><msubsup><mrow><mi>T</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup><mo>+</mo></mtd></mtr><mtr><mtd><mi>λ</mi><mfenced separators="|"><mrow><msubsup><mrow><mi>N</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup><mo>+</mo><msub><mrow><mi>B</mi></mrow><mrow><mi>r</mi></mrow></msub><mo>-</mo><msup><mrow><mi>B</mi></mrow><mrow><mi>s</mi></mrow></msup></mrow></mfenced><mo>+</mo><msub><mrow><mi>ε</mi></mrow><mrow><mi>L</mi></mrow></msub></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079745&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079753&type= 65.78600311 16.59466553 式中， <math id="M109"><msubsup><mrow><mi>P</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079748&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079746&type= 3.64066648 3.97933316 和 <math id="M110"><msubsup><mrow><mi>L</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079752&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079750&type= 3.47133350 3.97933316 分别表示接收机 <math id="M111"><mi>r</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080004&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080002&type= 1.69333339 2.79399991 对卫星 <math id="M112"><mi>s</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079774&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079772&type= 1.26999998 2.79399991 的伪距和载波相位观测值； <math id="M113"><msubsup><mrow><mi>ρ</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079776&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079764&type= 3.04800010 4.74133301 表示卫星天线相位中心到接收天线相位中心的几何距离； <math id="M114"><msub><mrow><mi>t</mi></mrow><mrow><mi>r</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079783&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079778&type= 2.11666679 3.72533321 和 <math id="M115"><msup><mrow><mi>t</mi></mrow><mrow><mi>s</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079799&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079781&type= 2.11666679 3.04800010 分别指接收机钟差和卫星钟差； <math id="M116"><msubsup><mrow><mi>I</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079805&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079802&type= 2.62466669 3.97933316 为电离层延迟； <math id="M117"><msubsup><mrow><mi>T</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079809&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079785&type= 3.89466691 3.97933316 为对流层延迟； <math id="M118"><mi>λ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080041&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080039&type= 1.77800000 2.79399991 表示其波长； <math id="M119"><msubsup><mrow><mi>N</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>s</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079792&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079815&type= 4.31799984 4.06400013 为载波相位整周模糊度； <math id="M120"><msub><mrow><mi>b</mi></mrow><mrow><mi>r</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079796&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079794&type= 2.87866688 3.72533321 和 <math id="M121"><msup><mrow><mi>b</mi></mrow><mrow><mi>s</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079821&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079820&type= 2.53999996 3.04800010 分别为接收机端和卫星端的伪距硬件延迟； <math id="M122"><msub><mrow><mi>B</mi></mrow><mrow><mi>r</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079824&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079830&type= 3.55599999 3.72533321 和 <math id="M123"><msup><mrow><mi>B</mi></mrow><mrow><mi>s</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079826&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079833&type= 3.38666677 3.04800010 分别为接收机端和卫星端的相位延迟； <math id="M124"><msub><mrow><mi>ε</mi></mrow><mrow><mi>P</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079844&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079843&type= 2.87866688 3.72533321 和 <math id="M125"><msub><mrow><mi>ε</mi></mrow><mrow><mi>L</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079840&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079846&type= 2.87866688 3.72533321 分别为伪距观测值和载波相位观测值上残余的误差项，包括观测噪声和多路径误差的总和。值得注意的是，对于GNSS观测中能够精确模型化的误差，如相对论效应、相位缠绕、固体潮汐等在构建观测方程时需要进行改正。 在PPP处理中，通常采用双频无电离层组合（ionosphere-free，IF）来消除一阶电离层延迟的影响： <math display="block" id="M126"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msubsup><mrow><mi>P</mi></mrow><mrow><mi>r</mi><mo>,</mo><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi><mo stretchy="false">(</mo><mi>i</mi><mo>,</mo><mi>j</mi><mo stretchy="false">)</mo></mrow><mrow><mi>s</mi></mrow></msubsup><mo>=</mo><mfrac><mrow><msubsup><mrow><mi>f</mi></mrow><mrow><mi>i</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow><mrow><msubsup><mrow><mi>f</mi></mrow><mrow><mi>i</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>-</mo><msubsup><mrow><mi>f</mi></mrow><mrow><mi>j</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow></mfrac><msubsup><mrow><mi>P</mi></mrow><mrow><mi>r</mi><mo>,</mo><mi>i</mi></mrow><mrow><mi>s</mi></mrow></msubsup><mo>-</mo><mfrac><mrow><msubsup><mrow><mi>f</mi></mrow><mrow><mi>j</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow><mrow><msubsup><mrow><mi>f</mi></mrow><mrow><mi>i</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>-</mo><msubsup><mrow><mi>f</mi></mrow><mrow><mi>j</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow></mfrac><msubsup><mrow><mi>P</mi></mrow><mrow><mi>r</mi><mo>,</mo><mi>j</mi></mrow><mrow><mi>s</mi></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mi>L</mi></mrow><mrow><mi>r</mi><mo>,</mo><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi><mo stretchy="false">(</mo><mi>i</mi><mo>,</mo><mi>j</mi><mo stretchy="false">)</mo></mrow><mrow><mi>s</mi></mrow></msubsup><mo>=</mo><mfrac><mrow><msubsup><mrow><mi>f</mi></mrow><mrow><mi>i</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow><mrow><msubsup><mrow><mi>f</mi></mrow><mrow><mi>i</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>-</mo><msubsup><mrow><mi>f</mi></mrow><mrow><mi>j</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow></mfrac><msubsup><mrow><mi>L</mi></mrow><mrow><mi>r</mi><mo>,</mo><mi>i</mi></mrow><mrow><mi>s</mi></mrow></msubsup><mo>-</mo><mfrac><mrow><msubsup><mrow><mi>f</mi></mrow><mrow><mi>j</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow><mrow><msubsup><mrow><mi>f</mi></mrow><mrow><mi>i</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>-</mo><msubsup><mrow><mi>f</mi></mrow><mrow><mi>j</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow></mfrac><msubsup><mrow><mi>L</mi></mrow><mrow><mi>r</mi><mo>,</mo><mi>j</mi></mrow><mrow><mi>s</mi></mrow></msubsup></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079858&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079841&type= 56.98066711 21.08200073 式中， <math id="M127"><msub><mrow><mi>f</mi></mrow><mrow><mi>i</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079862&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079852&type= 2.53999996 4.57200003 与 <math id="M128"><msub><mrow><mi>f</mi></mrow><mrow><mi>j</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079866&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079864&type= 2.53999996 4.57200003 表示不同GNSS信号频率，其组合后的无电离层组合观测方程为： <math display="block" id="M129"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msub><mrow><mover><mrow><mi>P</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow></msub><mo>=</mo><mfenced open="‖" close="‖" separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>r</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>s</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced><mo>+</mo><msub><mrow><mi>t</mi></mrow><mrow><mi>r</mi></mrow></msub><mo>-</mo><msup><mrow><mi>t</mi></mrow><mrow><mi>s</mi></mrow></msup><mo>+</mo><msub><mrow><mi>m</mi></mrow><mrow><mi>w</mi></mrow></msub><msub><mrow><mi>z</mi></mrow><mrow><mi>w</mi></mrow></msub><mo>+</mo><msub><mrow><mi>ε</mi></mrow><mrow><mi>P</mi><mo>,</mo><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mover><mrow><mi>L</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow></msub><mo>=</mo><mfenced open="‖" close="‖" separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>r</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>s</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced><mo>+</mo><msub><mrow><mi>t</mi></mrow><mrow><mi>r</mi></mrow></msub><mo>-</mo><msup><mrow><mi>t</mi></mrow><mrow><mi>s</mi></mrow></msup><mo>+</mo><msub><mrow><mi>m</mi></mrow><mrow><mi>w</mi></mrow></msub><msub><mrow><mi>z</mi></mrow><mrow><mi>w</mi></mrow></msub><mo>+</mo><msub><mrow><mi>λ</mi></mrow><mrow><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow></msub><mo>⋅</mo><msubsup><mrow><mi>N</mi></mrow><mrow><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow><mrow/></msubsup><mo>+</mo></mtd></mtr><mtr><mtd><mtext>            </mtext><msub><mrow><mi>ε</mi></mrow><mrow><mi>L</mi><mo>,</mo><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow></msub></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079868&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079856&type= 73.15200043 17.44133186 式中， <math id="M130"><msub><mrow><mover><mrow><mi>P</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079870&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079875&type= 4.65666676 4.40266657 和 <math id="M131"><msub><mrow><mover><mrow><mi>L</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079889&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079871&type= 4.65666676 4.40266657 分别为伪距与载波相位无电离层组合后含有观测噪声的观测值； <math id="M132"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>s</mi></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079893&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079891&type= 3.38666677 4.82600021 为卫星坐标； <math id="M133"><msup><mrow><mi>t</mi></mrow><mrow><mi>s</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079884&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079882&type= 2.11666679 3.04800010 为接收机钟差，可由精密轨道与钟差中获得； <math id="M134"><msub><mrow><mi>z</mi></mrow><mrow><mi>w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079898&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079896&type= 3.47133350 3.72533321 与 <math id="M135"><msub><mrow><mi>m</mi></mrow><mrow><mi>w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079902&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079900&type= 4.57200003 3.72533321 为对流层湿延迟与其投影函数，对流层延迟通常可分为干分量与湿分量，其中干分量通常通过萨斯塔莫宁模型 ［ 23 23 ］ 等先验模型直接进行改正，而湿分量由于具有较强的不确定性，通常需要作为参数进行估计； <math id="M136"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>r</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079908&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079906&type= 3.80999994 3.89466691 为 <math id="M137"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系下GNSS接收机测量中心的位置； <math id="M138"><msubsup><mrow><mi>λ</mi></mrow><mrow><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow><mrow/></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079922&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079914&type= 3.89466691 3.97933316 与 <math id="M139"><msubsup><mrow><mi>N</mi></mrow><mrow><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow><mrow/></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079934&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079917&type= 5.07999992 4.06400013 分别为无电离层组合的波长与整周模糊度。由于GNSS的测量基准为接收机中心，而本文算法的参考基准为IMU量测中心，两者空间不同步，需要利用 b 系下GNSS中心相对于IMU中心的矢量（又称杆臂） <math id="M140"><msup><mrow><mi mathvariant="bold-italic">l</mi></mrow><mrow><mi>b</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079928&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079935&type= 2.53999996 3.04800010 进行改正： <math display="block" id="M141"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>r</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">l</mi></mrow><mrow><mi>b</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079939&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079930&type= 24.12999916 4.14866638 则GNSS观测量 <math display="block" id="M142"><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">z</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow/><mrow><mi>G</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079932&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079931&type= 3.55599999 3.97933316 的误差 <math display="block" id="M143"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi mathvariant="normal">G</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079949&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079948&type= 3.30200005 3.72533321 可表示为： <math display="block" id="M144"><mtext>      </mtext><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi mathvariant="normal">G</mi></mrow></msub><mo stretchy="false">(</mo><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">z</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow/><mrow><mi mathvariant="normal">G</mi></mrow></msubsup><mo>,</mo><mi mathvariant="bold-italic">x</mi><mo stretchy="false">)</mo><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><mfenced open="‖" close="‖" separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">l</mi></mrow><mrow><mi>b</mi></mrow></msup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>s</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced><mo>+</mo><msub><mrow><mi>t</mi></mrow><mrow><mi>r</mi></mrow></msub><mo>-</mo><msup><mrow><mi>t</mi></mrow><mrow><mi>s</mi></mrow></msup><mo>+</mo><msub><mrow><mi>m</mi></mrow><mrow><mi>w</mi></mrow></msub><msub><mrow><mi>z</mi></mrow><mrow><mi>w</mi></mrow></msub><mo>-</mo><msub><mrow><mover><mrow><mi>P</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow></msub></mtd></mtr><mtr><mtd><mfenced open="‖" close="‖" separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">l</mi></mrow><mrow><mi>b</mi></mrow></msup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>s</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced><mo>+</mo><msub><mrow><mi>t</mi></mrow><mrow><mi>r</mi></mrow></msub><mo>-</mo><msup><mrow><mi>t</mi></mrow><mrow><mi>s</mi></mrow></msup><mo>+</mo><msub><mrow><mi>m</mi></mrow><mrow><mi>w</mi></mrow></msub><msub><mrow><mi>z</mi></mrow><mrow><mi>w</mi></mrow></msub><mo>+</mo><msubsup><mrow><mi>N</mi></mrow><mrow><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow><mrow/></msubsup><mo>-</mo><msub><mrow><mover><mrow><mi>L</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi mathvariant="normal">I</mi><mi mathvariant="normal">F</mi></mrow></msub></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079965&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079963&type= 99.99134064 12.10733318 2.3　视觉误差因子的构建 2 对于视觉观测，采用重投影误差构建视觉误差因子。对于任意特征点 <math id="M145"><mi>k</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080167&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080166&type= 1.69333339 2.79399991 ，其第 <math id="M146"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079960&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079959&type= 1.60866666 3.64066648 帧观测量与第 <math id="M147"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 帧观测量之间的重投影误差项 <math id="M148"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>C</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079982&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079979&type= 3.13266683 3.72533321 可以表示为： <math display="block" id="M149"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>C</mi></mrow></msub><mo stretchy="false">(</mo><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">z</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow><mrow><mi>c</mi></mrow></msubsup><mo>,</mo><mi mathvariant="bold-italic">χ</mi><mo stretchy="false">)</mo><mo>=</mo><msup><mrow><msub><mrow><mi mathvariant="bold-italic">π</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msup><mfenced separators="|"><mrow><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msubsup><mrow><mover><mrow><mi>u</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>m</mi><mo>,</mo><mtext> </mtext><mi>j</mi></mrow></msub></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mover><mrow><mi>v</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>m</mi><mo>,</mo><mtext> </mtext><mi>j</mi></mrow></msub></mrow></msubsup></mtd></mtr></mtable></mrow></mfenced></mrow></mfenced><mo>-</mo><msubsup><mrow><mover accent="true"><mi mathvariant="bold-italic">P</mi><mo>^</mo></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>m</mi><mo>,</mo><mtext> </mtext><mi>j</mi></mrow></msub></mrow></msubsup><mo>,</mo><mi>m</mi><mo>∈</mo><mfenced separators="|"><mrow><mi>l</mi><mo>,</mo><mi>r</mi></mrow></mfenced></mtd></mtr><mtr><mtd><msubsup><mrow><mover accent="true"><mi mathvariant="bold-italic">P</mi><mo>^</mo></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi><mo>,</mo><mtext> </mtext><mi>j</mi></mrow></msub></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>b</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi></mrow></msub></mrow></msubsup><mo stretchy="false">(</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>e</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msubsup><mo stretchy="false">(</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo stretchy="false">(</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi></mrow></msub></mrow><mrow><mi>b</mi></mrow></msubsup><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><msub><mrow><mi>λ</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></mfrac><msubsup><mrow><mi>π</mi></mrow><mrow><mi>c</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msubsup><mo stretchy="false">(</mo><mfenced open="[" close="]" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msubsup><mrow><mover><mrow><mi>u</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi><mo>,</mo><mtext> </mtext><mi>i</mi></mrow></msub></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mover><mrow><mi>v</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi><mo>,</mo><mtext> </mtext><mi>i</mi></mrow></msub></mrow></msubsup></mtd></mtr></mtable></mrow></mfenced><mo stretchy="false">)</mo><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi></mrow></msub></mrow><mrow><mi>b</mi></mrow></msubsup><mo stretchy="false">)</mo><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo stretchy="false">)</mo><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi></mrow></msub></mrow><mrow><mi>b</mi></mrow></msubsup><mo stretchy="false">)</mo></mtd></mtr><mtr><mtd><msubsup><mrow><mover accent="true"><mi mathvariant="bold-italic">P</mi><mo>^</mo></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>r</mi><mo>,</mo><mtext> </mtext><mi>j</mi></mrow></msub></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>b</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>r</mi></mrow></msub></mrow></msubsup><mo stretchy="false">(</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>e</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msubsup><mo stretchy="false">(</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo stretchy="false">(</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi></mrow></msub></mrow><mrow><mi>b</mi></mrow></msubsup><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><msub><mrow><mi>λ</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></mfrac><msubsup><mrow><mi>π</mi></mrow><mrow><mi>c</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msubsup><mo stretchy="false">(</mo><mfenced open="[" close="]" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msubsup><mrow><mover><mrow><mi>u</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi><mo>,</mo><mtext> </mtext><mi>i</mi></mrow></msub></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mover><mrow><mi>v</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi><mo>,</mo><mtext> </mtext><mi>i</mi></mrow></msub></mrow></msubsup></mtd></mtr></mtable></mrow></mfenced><mo stretchy="false">)</mo><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi></mrow></msub></mrow><mrow><mi>b</mi></mrow></msubsup><mo stretchy="false">)</mo><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi>e</mi></mrow></msubsup><mo stretchy="false">)</mo><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>r</mi></mrow></msub></mrow><mrow><mi>b</mi></mrow></msubsup><mo stretchy="false">)</mo></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079955&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079983&type= 95.16532898 34.45933533 式中， <math id="M150"><mo stretchy="false">(</mo><msubsup><mrow><mover><mrow><mi>u</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>m</mi><mo>,</mo><mi>j</mi></mrow></msub></mrow></msubsup><mo>,</mo><msubsup><mrow><mover><mrow><mi>v</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>m</mi><mo>,</mo><mi>j</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079986&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079985&type= 12.95400047 3.64066648 ）为特征点 <math id="M151"><mi>k</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080167&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080166&type= 1.69333339 2.79399991 在第 <math id="M152"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079960&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079959&type= 1.60866666 3.64066648 帧的 <math id="M153"><mi>m</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079962&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079993&type= 2.87866688 2.79399991 相机下的像素坐标， <math id="M154"><mi>m</mi><mo>∈</mo><mfenced separators="|"><mrow><mi>l</mi><mo>,</mo><mi>r</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079996&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079995&type= 14.30866718 3.97933316 ， <math id="M155"><mi>l</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080000&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45079998&type= 1.26999998 2.79399991 与 <math id="M156"><mi>r</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080004&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080002&type= 1.69333339 2.79399991 分别表示左相机与右相机； <math id="M157"><msup><mrow><mi>π</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080010&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080009&type= 4.99533367 3.04800010 表示由2D像素坐标到3D归一化相机坐标的投影；（ <math id="M158"><msubsup><mrow><mover><mrow><mi>u</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi><mo>,</mo><mtext> </mtext><mi>i</mi></mrow></msub></mrow></msubsup><mo>,</mo><msubsup><mrow><mover><mrow><mi>v</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi><mo>,</mo><mtext> </mtext><mi>i</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080007&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080006&type= 10.92199993 3.55599999 ）为特征点 <math id="M159"><mi>k</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080167&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080166&type= 1.69333339 2.79399991 在第 <math id="M160"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 帧的左相机下的像素坐标； <math id="M161"><mi>λ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080041&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080039&type= 1.77800000 2.79399991 为特征点在第 <math id="M162"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080045&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080043&type= 1.10066664 2.79399991 帧下的逆深度； <math id="M163"><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi></mrow></msub></mrow><mrow><mi>b</mi></mrow></msubsup><mi mathvariant="normal">、</mi><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>l</mi></mrow></msub></mrow><mrow><mi>b</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080047&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080024&type= 9.65200043 3.89466691 与 <math id="M164"><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>r</mi></mrow></msub></mrow><mrow><mi>b</mi></mrow></msubsup><mi mathvariant="normal">、</mi><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>r</mi></mrow></msub></mrow><mrow><mi>b</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080026&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080049&type= 9.90600014 3.89466691 为左相机与右相机到IMU中心的外参。 2.4　激光雷达误差因子的构建 2 对于激光雷达观测，本文参考文献［ 11 11 ］，基于雷达点云中边缘特征与平面特征来构造雷达误差因子。对于新一帧雷达观测，首先计算不同扫描平面上各点的曲率值，并以此为基础提取边缘特征与平面特征；随后，利用与当前位置较近的（通常小于50 m）雷达关键帧位姿与特征点云信息构建局部特征地图；最后，对提取的边缘与平面特征点与局部特征地图中的特征点进行匹配，并构造雷达误差因子： <math display="block" id="M165"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><msub><mrow><mi>e</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><mo>=</mo><mfrac><mrow><mfenced open="|" close="|" separators="|"><mrow><mfenced separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi>p</mi></mrow><mrow><mi>j</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced><mo>×</mo><mfenced separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>k</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced></mrow></mfenced></mrow><mrow><mfenced open="|" close="|" separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>j</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>k</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080028&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080051&type= 44.19599915 11.85333347 <math display="block" id="M166"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><msub><mrow><mi>p</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><mo>=</mo><mfrac><mrow><mfenced open="|" close="|" separators="|"><mrow><mtable><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>j</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mtd></mtr><mtr><mtd><mfenced separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>j</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>k</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced><mo>×</mo><mfenced separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>j</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>m</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced></mtd></mtr></mtable></mrow></mfenced></mrow><mrow><mfenced open="|" close="|" separators="|"><mrow><mfenced separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>j</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>k</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced><mo>×</mo><mfenced separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>j</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>m</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced></mrow></mfenced></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080030&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080053&type= 45.12733078 18.45733452 式中， <math id="M167"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><msub><mrow><mi>e</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080033&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080031&type= 3.04800010 3.64066648 与 <math id="M168"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><msub><mrow><mi>p</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080056&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080034&type= 3.21733332 3.72533321 分别表示雷达边缘误差与平面误差；对于边缘误差， <math id="M169"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080035&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080058&type= 3.38666677 4.82600021 为当前帧点云中提取的边缘特征点， <math id="M170"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>j</mi></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080083&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080037&type= 3.38666677 4.82600021 与 <math id="M171"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>k</mi></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080086&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080084&type= 3.47133350 4.82600021 为特征地图中的边缘特征匹配点；对于平面误差， <math id="M172"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080068&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080066&type= 3.38666677 4.82600021 为当前帧点云中提取的平面特征点， <math id="M173"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>j</mi></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080083&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080037&type= 3.38666677 4.82600021 、 <math id="M174"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>k</mi></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080086&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080084&type= 3.47133350 4.82600021 与 <math id="M175"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>m</mi></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080089&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080087&type= 4.06400013 4.82600021 为特征地图中的平面特征匹配点。 2.5　基于关键帧与滑动窗口的GVIL因子图构建 2 本文提出的GVIL组合算法在后端状态估计中所维护的基于滑动窗口的因子图如 图1 图1 所示，其中圆形节点对应算法中所估计的状态量，包含系统状态量 <math id="M176"><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><mi>i</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080091&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080090&type= 3.04800010 3.72533321 、GNSS相关的状态量 <math id="M177"><msub><mrow><mi mathvariant="bold-italic">x</mi></mrow><mrow><msub><mrow><mi>G</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080092&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080074&type= 4.40266657 3.64066648 、相机特征点的逆深度 <math id="M178"><msub><mrow><mi>λ</mi></mrow><mrow><mi>i</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080077&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080093&type= 2.53999996 3.72533321 等，与其相连的矩形方块表示系统中各项误差因子。针对传入系统的不同观测量，本文分别构建了IMU误差因子、GNSS-PPP误差因子、视觉误差因子与激光雷达误差因子4种误差因子。为了降低系统优化的计算量，本文采用关键帧策略与滑动窗口的方式对因子图进行更新，虚线框内为当前滑动窗口中所保存的系统状态量。当新一帧观测到来时，首先为其构造相应的系统状态量节点并加入到因子图中，根据其观测值类型建立相应的误差因子，并将其与相关的状态量节点相连。若此时窗口已满，则需要将窗口中最老帧对应的节点进行边缘化处理，移除该节点并为窗口内与其相关的节点构造先验约束项因子，参与后续系统全局状态优化，通过这种方式可以在减少计算量的同时最大程度地保留系统观测信息。在构建GNSS误差因子时，需要注意对于同一颗卫星的模糊度参数在未发生周跳的弧段内应采用同一模糊度参数进行参数估计，只有当其发生周跳时才会将其构造为新的模糊度参数，因而不同时刻的系统状态会与同一个模糊度参数节点相连。 由滑动窗口中各因子所构建的优化问题为： <math display="block" id="M179"><mtable columnalign="left"><mtr><mtd><munder><mrow><mi mathvariant="normal">m</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi></mrow><mrow><mi>x</mi></mrow></munder><mo stretchy="false">{</mo><msup><mrow><mfenced open="‖" close="‖" separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>p</mi></mrow></msub><mo>-</mo><msub><mrow><mi mathvariant="bold-italic">H</mi></mrow><mrow><mi>p</mi></mrow></msub><mi mathvariant="bold-italic">x</mi></mrow></mfenced></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>+</mo><munder><mo movablelimits="false">∑</mo><mrow><mi>i</mi><mo>∈</mo><mi>I</mi></mrow></munder><mrow><msubsup><mrow><mfenced open="‖" close="‖" separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>I</mi></mrow></msub><mo stretchy="false">(</mo><msubsup><mrow><mover accent="true"><mi mathvariant="bold-italic">z</mi><mo>^</mo></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup><mo>,</mo><mi mathvariant="bold-italic">x</mi><mo stretchy="false">)</mo></mrow></mfenced></mrow><mrow><msubsup><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow><mo>+</mo></mtd></mtr><mtr><mtd><munder><mo movablelimits="false">∑</mo><mrow><mfenced separators="|"><mrow><mi>j</mi><mo>,</mo><mtext> </mtext><mi>n</mi></mrow></mfenced><mo>∈</mo><mi>C</mi></mrow></munder><mrow><msubsup><mrow><mfenced open="‖" close="‖" separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>C</mi></mrow></msub><mo stretchy="false">(</mo><msubsup><mrow><mover accent="true"><mi mathvariant="bold-italic">z</mi><mo>^</mo></mover></mrow><mrow><mi>n</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msubsup><mo>,</mo><mi mathvariant="bold-italic">x</mi><mo stretchy="false">)</mo></mrow></mfenced></mrow><mrow><msubsup><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>n</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msubsup></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow><mo>+</mo><munder><mo movablelimits="false">∑</mo><mrow><mfenced separators="|"><mrow><mi>k</mi><mo>,</mo><mtext> </mtext><mi>m</mi></mrow></mfenced><mo>∈</mo><mi>L</mi></mrow></munder><mrow><msubsup><mrow><mfenced open="‖" close="‖" separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>L</mi></mrow></msub><mo stretchy="false">(</mo><msubsup><mrow><mover accent="true"><mi mathvariant="bold-italic">z</mi><mo>^</mo></mover></mrow><mrow><mi>m</mi></mrow><mrow><msub><mrow><mi>l</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msubsup><mo>,</mo><mi mathvariant="bold-italic">x</mi><mo stretchy="false">)</mo></mrow></mfenced></mrow><mrow><msubsup><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>m</mi></mrow><mrow><msub><mrow><mi>l</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msubsup></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow><mo>+</mo></mtd></mtr><mtr><mtd><munder><mo movablelimits="false">∑</mo><mrow><mfenced separators="|"><mrow><mi>s</mi><mo>,</mo><mi>o</mi></mrow></mfenced><mo>∈</mo><mi>G</mi></mrow></munder><mrow><msubsup><mrow><mfenced open="‖" close="‖" separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi mathvariant="normal">G</mi></mrow></msub><mo stretchy="false">(</mo><msubsup><mrow><mover accent="true"><mi mathvariant="bold-italic">z</mi><mo>^</mo></mover></mrow><mrow><mi>o</mi></mrow><mrow><msub><mrow><mi>G</mi></mrow><mrow><mi>s</mi></mrow></msub></mrow></msubsup><mo>,</mo><mi mathvariant="bold-italic">x</mi><mo stretchy="false">)</mo></mrow></mfenced></mrow><mrow><msubsup><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>o</mi></mrow><mrow><mi>G</mi><mi>s</mi></mrow></msubsup></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow><mo stretchy="false">}</mo></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080079&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080099&type= 67.73333740 28.02466965 式中， <math id="M180"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>p</mi></mrow></msub><mi mathvariant="normal">、</mi><msub><mrow><mi mathvariant="bold-italic">H</mi></mrow><mrow><mi>p</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080102&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080101&type= 8.97466660 3.97933316 为边缘化所产生的先验约束的残差与系数阵； <math id="M181"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>I</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080113&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080081&type= 2.45533323 3.72533321 、 <math id="M182"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi mathvariant="bold-italic">C</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080115&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080104&type= 3.13266683 3.72533321 、 <math id="M183"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>L</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080117&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080106&type= 3.04800010 3.72533321 和 <math id="M184"><msub><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi mathvariant="normal">G</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080108&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080119&type= 3.30200005 3.72533321 分别表示预积分项残差、视觉观测残差、激光雷达观测残差与GNSS观测残差； <math id="M185"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080110&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080121&type= 6.26533365 3.89466691 、 <math id="M186"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>n</mi></mrow><mrow><msub><mrow><mi>c</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080128&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080112&type= 3.80999994 4.06400013 、 <math id="M187"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>m</mi></mrow><mrow><msub><mrow><mi>l</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080123&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080129&type= 4.06400013 3.89466691 、 <math id="M188"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>o</mi></mrow><mrow><msub><mrow><mi>G</mi></mrow><mrow><mi>s</mi></mrow></msub></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080126&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080125&type= 4.57200003 3.89466691 为其各自相应的协方差矩阵。其中，惯性测量的协方差矩阵依据IMU预积分采用递推形式进行计算，视觉测量的协方差矩阵可由像素观测噪声协方差与相机的投影矩阵计算，激光雷达观测协方差矩阵可利用雷达平面特征与边缘特征中点到线和点到面的距离残差计算，其详细计算方式可以参考文献［ 11 11 ］。对于GNSS观测的协方差矩阵，本文依据卫星的高度角信息确定其最终权重： <math display="block" id="M189"><msup><mrow><mi>σ</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>=</mo><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msubsup><mrow><mi>σ</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>,</mo><mi>e</mi><mo>≥</mo><mn mathvariant="normal">30</mn><mo>°</mo></mtd></mtr><mtr><mtd><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn><mi mathvariant="normal">s</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi>e</mi></mrow></mfrac><msubsup><mrow><mi>σ</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>,</mo><mi>e</mi><mo><</mo><mn mathvariant="normal">30</mn><mo>°</mo></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080132&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080131&type= 34.45933533 13.88533401 式中， <math id="M190"><msub><mrow><mi>σ</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080133&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080143&type= 2.87866688 3.72533321 表示观测量初始精度，本文中依据经验通常取伪距观测值0.3 m、载波相位3 mm作为先验精度； <math id="M191"><mi>σ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080135&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080134&type= 1.86266661 2.79399991 为经高度角模型确定的精度； <math id="M192"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 表示卫星的高度角，根据该随机模型，当卫星高度角大于30°时，对观测值采用等权处理，当卫星高度角小于30°时，根据其高度不同进行降权处理。此外，为了降低不同传感器中异常观测对于后端估计的影响，增强后端优化的稳定性，本文采用CauchyLoss作为核函数对于异常观测进行降权处理。 3　系统基本框架 1 本文所提出的GVIL紧组合算法数据处理流程如 图2 图2 所示，可以分为数据预处理、联合初始化与因子图优化两个部分，其中有关IMU、视觉、激光雷达的数据预处理在文献［ 12 12 ］有详细的叙述，本文中不再赘述，主要介绍有关GNSS观测值的预处理与初始化过程。预处理主要指观测值粗差及周跳的探测，首先系统需要获取当前历元的伪距与载波相位观测值以及精密轨道和钟差文件；然后采用TurboEdit算法，通过MW（Melbourne-Wübbena）组合与电离层残差组合进行粗差和周跳的探测。本文对于发生周跳的历元直接引入新的模糊度参数，而并不进行修复，以避免周跳修复错误影响定位结果；最后根据相关模型对GNSS相对论效应、海洋潮汐、地球自转等模型化误差进行改正，并构建PPP双频无电离层组合观测方程。 在初始化过程中，首先需要针对当前帧是否为关键帧进行判断。本文所提出的GVIL算法对于视觉、雷达观测的关键帧的判断主要依据载体位移量与旋转量估计，与文献［ 8 8 ， 11 11 ］基本保持一致。而对于GNSS观测，由于PPP定位过程中参数的收敛需要一定时间，需要确保卫星观测的连续性。因而当GNSS卫星观测传入时，系统一定会将其构建为关键帧并加入滑动窗口。 待滑动窗口集满关键帧后，需要进行视觉、惯性、激光雷达的联合初始化，有关三者初始化内容可以参考文献［ 11 11 ］。初始化后系统状态量 表示在 <math id="M193"><mi>w</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080209&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080208&type= 2.96333337 2.79399991 系下，由于GVIL组合算法采用 <math id="M194"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系作为参考坐标系，仍需进一步转换。在转换参数的计算上，其平移量可由GNSS测量解算得到，而旋转量初始化后的 <math id="M195"><mi>w</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080209&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080208&type= 2.96333337 2.79399991 系水平姿态角与当地水平坐标系保持一致，因而只需对其进行航向角估计。由于本文主要面向微机电系统（micro-electro-mechanical system，MEMS）级低成本IMU的应用，测量噪声较大，地球自转角速度无法从测量噪声中分离，需要借助其他传感器进行辅助。本文中主要采用动对准模式 ［ 24 24 ］ ，利用GNSS所测量的位移矢量实现航向角初值估计。假设载体在对准过程中呈直线运动，利用GNSS量测可得到其在 <math id="M196"><mi>k</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080167&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080166&type= 1.69333339 2.79399991 时刻与 <math id="M197"><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080159&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080158&type= 8.38199997 2.96333337 时刻在 <math id="M198"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系下的位置矢量 <math id="M199"><msubsup><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>k</mi></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080175&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080174&type= 2.96333337 3.97933316 与 <math id="M200"><msubsup><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080165&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080164&type= 6.18066692 3.97933316 ，则可得 <math id="M201"><mi>k</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080167&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080166&type= 1.69333339 2.79399991 时刻位置对应的子午圈切线单位矢量 <math id="M202"><msub><mrow><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>¯</mo></mover></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080169&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080168&type= 2.79399991 3.72533321 为： <math display="block" id="M203"><msub><mrow><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>¯</mo></mover></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>=</mo><msub><mrow><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>¯</mo></mover></mrow><mrow><mi>k</mi></mrow></msub><mo>×</mo><mfenced separators="|"><mrow><msub><mrow><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>¯</mo></mover></mrow><mrow><mi>z</mi></mrow></msub><mo>×</mo><msub><mrow><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>¯</mo></mover></mrow><mrow><mi>k</mi></mrow></msub></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080171&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080170&type= 28.36333466 3.97933316 式中， <math id="M204"><msub><mrow><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>¯</mo></mover></mrow><mrow><mi>k</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080173&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080172&type= 2.62466669 3.72533321 为位置矢量 <math id="M205"><msubsup><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>k</mi></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080175&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080174&type= 2.96333337 3.97933316 的单位矢量； <math id="M206"><msub><mrow><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>¯</mo></mover></mrow><mrow><mi>z</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080177&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080176&type= 2.62466669 3.72533321 为 <math id="M207"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系下沿其 <math id="M208"><mi>z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080181&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080180&type= 1.77800000 2.79399991 轴的单位矢量。则载体航向角 <math id="M209"><mi>ψ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080203&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080202&type= 2.11666679 3.55599999 为： <math display="block" id="M210"><mi>ψ</mi><mo>=</mo><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">c</mi><mi mathvariant="normal">c</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">s</mi><mfenced separators="|"><mrow><msub><mrow><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>¯</mo></mover></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mo>⋅</mo><mi mathvariant="normal">Δ</mi><msub><mrow><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>¯</mo></mover></mrow><mrow><mi>k</mi></mrow></msub></mrow></mfenced><mo>,</mo><mi mathvariant="normal">Δ</mi><msub><mrow><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>¯</mo></mover></mrow><mrow><mi>k</mi></mrow></msub><mo>=</mo><mfrac><mrow><msubsup><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>k</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow><mrow><mfenced open="‖" close="‖" separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">r</mi></mrow><mrow><mi>k</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080185&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080184&type= 60.19800186 9.90600014 在初始化过程中，载体在静止一段时间后进行直线运动，此时对于视觉、惯性、雷达观测利用VIL紧组合算法完成初始化，并不断估计其在 <math id="M211"><mi>w</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080209&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080208&type= 2.96333337 2.79399991 系下的位姿，GNSS观测量依据PPP模型单独解算，获取系统在 <math id="M212"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系下的位置，当位移长度满足预定阈值（5~10 m）后便尝试利用位置矢量进行航向角对准，在得到其初始航向角 <math id="M213"><mi>ψ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080203&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080202&type= 2.11666679 3.55599999 后，记录当前时刻GNSS天线位置 <math id="M214"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080193&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080192&type= 3.38666677 4.82600021 ，则此时 <math id="M215"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系与 <math id="M216"><mi>w</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080209&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080208&type= 2.96333337 2.79399991 系间旋转关系为： <math display="block" id="M217"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>n</mi></mrow><mrow><mi>e</mi></mrow></msubsup><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi></mrow><mrow><mi>n</mi></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>w</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>r</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>n</mi></mrow><mrow><mi>e</mi></mrow></msubsup><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi></mrow><mrow><mi>n</mi></mrow></msubsup><mo stretchy="false">(</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>w</mi></mrow></msubsup><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi>w</mi></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">l</mi></mrow><mrow><mi>b</mi></mrow></msup><mo stretchy="false">)</mo></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080199&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080198&type= 45.97399902 9.99066734 式中， <math id="M218"><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi></mrow><mrow><mi>n</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080201&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080200&type= 4.57200003 3.97933316 可由初始航向角 <math id="M219"><mi>ψ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080203&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080202&type= 2.11666679 3.55599999 得到； <math id="M220"><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>n</mi></mrow><mrow><mi>e</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080205&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080204&type= 3.97933316 3.97933316 与载体的位置相关，可由 式（4） 式（4） 进行计算。在得到二者转换关系 <math id="M221"><mfenced open="{" close="}" separators="|"><mrow><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi></mrow><mrow><mi>e</mi></mrow></msubsup><mo>,</mo><msubsup><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>w</mi></mrow><mrow><mi>e</mi></mrow></msubsup></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080207&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080206&type= 12.53066730 4.48733330 后，需要将先前估计的系统状态由 <math id="M222"><mi>w</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080209&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080208&type= 2.96333337 2.79399991 系转到 <math id="M223"><mi>e</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080211&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080210&type= 1.52400005 2.79399991 系下。初始化成功后可将后续观测加入因子图，进行基于因子图的整体优化，并对其中的异常观测值进行检测与剔除。对于滑出窗口项，通过边缘化的方式将其构造为先验项加入后续优化。在整体优化完成后，利用最新的系统状态对局部点云地图进行更新。 4　车载实验与结果分析 1 4.1　实验数据情况 2 为了测试组合算法的性能，2020-10-27在武汉大学校园内进行了两组车载实验。实验数据采集使用了自主搭建的组合导航数据采集平台。如 图3 图3 所示，该实验平台搭载了Septentrio PolaRx5 GNSS高精度多频多系统接收机与NovAtel GPS-702-GG天线，支持GPS （L1、L2、L5）、GLONASS （L1、L2、L3）、Galileo （E1、E5a、E5b、AltBoc、E6）与BDS （B1、B2、B3）频率数据采集。两组FLIR BFS-PGE-31S4C工业相机构成双目立体相机，采用全局曝光，最大图像分辨率为2 048 <math id="M224"><mo>×</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080213&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080212&type= 2.53999996 2.79399991 1 536像素，最大采集频率为35 Hz。Velodyne VLP-16激光雷达，最大测量范围为100 m，垂直角测量范围为+ <math id="M225"><mn mathvariant="normal">15</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080217&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080216&type= 3.72533321 2.79399991 °~－ <math id="M226"><mn mathvariant="normal">15</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080217&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080216&type= 3.72533321 2.79399991 °，垂直角度测量分辨率为 <math id="M227"><mn mathvariant="normal">2</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080219&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080218&type= 1.86266661 2.79399991 °，水平角测量范围为 <math id="M228"><mn mathvariant="normal">360</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080221&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080220&type= 5.58799982 2.79399991 °，测量精度为 <math id="M229"><mo>±</mo><mn mathvariant="normal">3</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080223&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=45080222&type= 5.33400011 2.87866688 cm。StarNeto XW-GI7660战术级惯导与AIDS-16470 MEMS IMU，其性能参数如 表1 表1 所示。实验过程中GNSS数据采样率为1 Hz，相机采样率为10 Hz，激光雷达采样率为10 Hz，IMU采样率为200 Hz。 2 IMU设备 2 零偏稳定性 2 随机游走 陀螺 /(°·h —1 ) 加速度计 /mg 角度 /(°·h －1/2 ) 速度 /（m·s —1 ·h －1/2 ） StarNeto 0.3 100 0.01 AIDS 8 1 500 0.34 0.037 为保证采集数据的时间同步，本文采用了以GNSS的秒脉冲信号为基准的时间同步方案，GNSS接收机通过GNSS天线接收卫星信号，在与GPS时间对齐后，以1 Hz的频率向外输出秒脉冲信号，将不同传感器的时间校准到GPS时刻。在空间同步上以惯性设备的测量中心为原点建立载体坐标系，对于GNSS天线与IMU的空间关系可以通过测量GNSS天线中心相对于IMU测量中心的位置向量（杆臂值）确定；双目相机与IMU间的空间关系可以采用Kalibr工具箱 ［ 25 25 ］ 对相机与IMU进行联合标定获得；激光雷达与IMU间的空间关系可以采用Autoware工具箱 ［ 26 26 ］ 先确定激光雷达与相机间的空间关系，再转换至IMU坐标系下。 两组车载实验均为复杂城市场景，其中实验‑01全长1 600 m左右，实验-02全长2 600 m左右。 图4 图4 与 图5 图5 为两组实验的轨迹与典型场景，其道路两旁存在树木和建筑等结构化特征，这些特征有利于相机和激光雷达的特征匹配，但会对GNSS信号产生严重的干扰，一些场景存在光照明暗变化、过度曝光等情况，部分路段还存在人、车等动态物体的干扰。 在测试过程中，车辆进行了多次加减速、转弯等机动操作以实现对IMU的充分激励。本算法基于武汉大学测绘学院GREAT软件实现 ［ 27 27 ］ ，为了获取车辆的参考轨迹，实验采用NovAtel公司商用软件解算的StarNeto战术级惯导与GNSS事后双向平滑的紧组合结果作为参考轨迹，其中GNSS基准站设置在武汉大学信息学部教学实验大楼楼顶，并使用Trimble NetR9多频多模接收机以1 Hz频率进行数据采集。 4.2　卫星可见性分析 2 实验-01与实验-02中GNSS各系统卫星数与相应的空间位置精度因子（position dilution of precision，PDOP）值的变化情况见 图6 图6 、 图7 图7 与 表2 表2 ，可以看出，实验-01与实验-02所在环境下，单系统可见卫星数仅为2~4颗。采用4系统观测后，可见卫星数有一定提升，但卫星数整体波动较大，部分路段卫星数不足4颗，且相应的PDOP值显著升高，给GNSS的精确解算带来较大困难。 2 数据序列 5 卫星系统 GNSS GLONASS Galileo BDS 4系统组合 实验-01 3.8 2.2 3.7 3.5 13.2 实验-02 4.0 2.3 3.8 4.0 14.1 4.3　实验结果与分析 2 实验-01与实验-02采用4系统所解算的PPP定位结果如 图8 图8 与 表3 表3 所示，结果表明在车载实验环境下，受卫星遮挡、多路径效应等观测条件的影响，单PPP解算精度不高，水平（东（east，E）、北（north，N））方向均方根误差（root mean squared error， RMSE）为1~2 m，垂直（天（up，U））方向RMSE为5.59~3.79 m。此外，在蓝色区域内，PPP解算精度出现了大幅波动，该区域内解算精度的下降与卫星受到环境遮挡几何结构强度下降相关。通常认为需要达到分米级定位才能满足自动驾驶车载导航需求。经统计，在实验-01中，PPP解算结果整体RMSE小于1 m的共52个历元，占全部历元数的16%；实验-02为156个历元，占全部历元数的26%。可以看出，在实验测试环境下，依靠单GNSS难以满足车载导航应用需求。 2 数据序列 4 RMSE E N U 三维位置 实验-01 1.820 2.067 5.587 6.229 实验-02 1.148 1.219 3.793 4.096 为了验证算法的有效性，本文采用文献［ 8 8 ］所改进的双目VIO算法与文献［ 12 12 ］所提出的VIL算法同本文所实现的GNSS、视觉、惯性紧组合算法（GNSS-visual-inertial odometry，GVIO）与GVIL算法进行对比分析。实验结果如 图9 图9 、 图10 图10 与 表4 表4 、 表5 表5 所示，可以看出，在VIO与VIL等相对定位算法的基础上，引入GNSS观测可以显著改善VIO与VIL算法在全局位置与姿态上的可观性，修正累计误差，提升组合系统的位姿估计性能。在位置估计上，实验-01中GVIO在E、N、U 3个方向上RMSE相较于VIO分别提升了94.8%、94%与84.2%；GVIL在E、N、U 3个方向上RMSE相较于VIL分别提升了85.1%、90.7%与27.6%。实验-02中GVIO在E、N、U 3个方向上RMSE相较于VIO分别提升了94.9%、95%与80.3%；GVIL在E、N、U 3个方向上RMSE相较于VIL分别提升了96.5%、94.4%与84.4%。在姿态估计上，实验-01中GVIO的航向角估计RMSE相较于VIO提升了66.3%，GVIL的航向角估计RMSE相较于VIL提升了55.7%；实验-02中GVIO的航向角估计RMSE相较于VIO提升了87.2%，GVIL的航向角估计RMSE相较于VIL提升了91.8%。 2 算法 5 RMSE E/m N/m U/m 三维位置/m 航向角/(°) VIO 14.09 10.79 1.46 17.82 1.72 VIL 2.82 2.26 0.76 3.69 0.88 GVIO 0.73 0.65 0.23 1.01 0.58 GVIL 0.42 0.21 0.55 0.72 0.39 2 算法 5 RMSE E/m N/m U/m 三维位置/m 航向角/(°) VIO 16.85 10.56 1.93 19.98 6.40 VIL 8.51 6.30 1.73 10.73 3.80 GVIO 0.86 0.53 0.38 1.08 0.82 GVIL 0.3 0.35 0.27 0.54 0.31 此外，通过GVIO与GVIL间的比较可以看出，激光雷达观测的加入可以有效降低组合算法在GNSS失锁条件下的误差漂移，同时其所提供的位置信息也有益于GNSS的解算。在实验-01中，GVIL相较于GVIO在E、N两个方向上位置估计RMSE分别提升了42.4%、67.7%，航向角估计RMSE提升了32.8%；实验-02中，GVIL相较于GVIO在E、N、U 3个方向上位置估计RMSE分别提升了65.1%、34%与28.9%，航向角估计RMSE提升了62.2%。 5　结 语 1 本文提出了一种基于因子图的GNSS、视觉、惯性、激光雷达紧组合算法，该算法以位移矢量为基础实现了4种传感器联合初始化，并通过基 于滑动窗口的因子图优化对4种传感器进行紧组合估计，实现了原始观测值层面下的GNSS全局定位信息与VIL相对定位信息间的融合。本文利用车载实验对算法性能进行测试，实验结果表 明，即使在GNSS受限的观测条件下，本文所提出的四者紧组合算法仍可大幅度提升算法的精度与可用性，相较于VIL组合算法，其位置估计精度可以提升84%以上，姿态估计精度可以提升66%以上。 参考文献 Zhang Hengcai ， Yu Baoguo ， Bei Jinzhong ， et al . A Survey of Scene-Based Augmentation Systems for Comprehensive PNT ［J］. Geomatics and Information Science of Wuhan University ， 2023 ， 48 （ 4 ）： 491 - 505 . A Survey of Scene-Based Augmentation Systems for Comprehensive PNT 张恒才 ， 蔚保国 ， 秘金钟 ， 等 . 综合PNT场景增强系统研究进展及发展趋势 ［J］. 武汉大学学报（信息科学版） ， 2023 ， 48 （ 4 ）： 491 - 505 . 综合PNT场景增强系统研究进展及发展趋势 Bailey T ， Durrant-Whyte H . Simultaneous Localization and Mapping （SLAM）： Part II ［J］. IEEE Robotics and Automation Magazine ， 2006 ， 13 （ 3 ）： 108 ‑ 117 . Simultaneous Localization and Mapping （SLAM）： Part II Huang G . Visual-Inertial Navigation： A Concise Review ［C］// International Conference on Robotics and Automation ， Montreal， Canada ， 2019 . Visual-Inertial Navigation： A Concise Review Mourikis A I ， Roumeliotis S I . A Multi-state Constraint Kalman Filter for Vision-Aided Inertial Navigation ［C］// IEEE International Conference on Robotics and Automation ， Rome， Italy ， 2007 . A Multi-state Constraint Kalman Filter for Vision-Aided Inertial Navigation Sun K ， Mohta K ， Pfrommer B ， et al . Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight ［J］. IEEE Robotics and Automation Letters ， 2018 ， 3 （ 2 ）： 965 ‑ 972 . Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight Huai Z ， Huang G . Robocentric Visual–Inertial Odometry ［J］. The International Journal of Robotics Research ， 2022 ， 41 （ 7 ）： 667 - 689 . Robocentric Visual–Inertial Odometry Forster C ， Carlone L ， Dellaert F ， et al . On-Manifold Preintegration for Real-Time Visual–Inertial Odometry ［J］. IEEE Transactions on Robotics ， 2017 ， 33 （ 1 ）： 1 ‑ 21 . On-Manifold Preintegration for Real-Time Visual–Inertial Odometry Qin T ， Li P ， Shen S . VINS-Mono： A Robust and Versatile Monocular Visual-Inertial State Estimator ［J］. IEEE Transactions on Robotics ， 2018 ， 34 （ 4 ）： 1004 - 1020 . VINS-Mono： A Robust and Versatile Monocular Visual-Inertial State Estimator Strasdat H ， Montiel J M M ， Davison A J . Visual SLAM： Why Filter？ ［J］. Image and Vision Computing ， 2012 ， 30 （ 2 ）： 65 - 77 . Visual SLAM： Why Filter？ Zuo X ， Geneva P ， Lee W ， et al . LIC-Fusion： LiDAR-Inertial-Camera Odometry ［C］// 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems ， Macau， China ， 2019 . LIC-Fusion： LiDAR-Inertial-Camera Odometry Shan T ， Englot B ， Ratti C ， et al . LVI-SAM： Tightly-Coupled LiDAR-Visual-Inertial Odometry via Smoothing and Mapping ［C］// IEEE International Conference on Robotics and Automation ， Xi’an， China ， 2021 . LVI-SAM： Tightly-Coupled LiDAR-Visual-Inertial Odometry via Smoothing and Mapping Wang Xuanbin ， Li Xingxing ， Liao jianchi ， et al . Tightly-Coupled Stereo Visual-Inertial-LiDAR SLAM Based on Graph Optimization ［J］. Acta Geodaetica et Cartograohica Sinica ， 2022 ， 51 （ 8 ）： 1744 - 1756 . Tightly-Coupled Stereo Visual-Inertial-LiDAR SLAM Based on Graph Optimization 王铉彬 ， 李星星 ， 廖健驰 ， 等 . 基于图优化的紧耦合双目视觉/惯性/激光雷达SLAM方法 ［J］. 测绘学报 ， 2022 ， 51 （ 8 ）： 1744 - 1756 . 基于图优化的紧耦合双目视觉/惯性/激光雷达SLAM方法 Wen W ， Hsu L T . Towards Robust GNSS Positioning and Real-Time Kinematic Using Factor Graph Optimization ［C］// IEEE International Conference on Robotics and Automation ， Xi’an， China ， 2021 . Towards Robust GNSS Positioning and Real-Time Kinematic Using Factor Graph Optimization Angelino C V ， Baraniello V R ， Cicala L . UAV Position and Attitude Estimation Using IMU， GNSS and Camera ［C］// The 15th International Conference on Information Fusion ， Suntec City，Singapore ， 2012 . UAV Position and Attitude Estimation Using IMU， GNSS and Camera Lynen S ， Achtelik M W ， Weiss S ， et al . A Robust and Modular Multi-sensor Fusion Approach Applied to MAV Navigation ［C］// IEEE/RSJ International Conference on Intelligent Robots and Systems ， Tokyo， Japan ， 2013 . A Robust and Modular Multi-sensor Fusion Approach Applied to MAV Navigation Niu X ， Tang H ， Zhang T ， et al . IC-GVINS： A Robust， Real-Time， INS-Centric GNSS-Visual-Inertial Navigation System ［J］. IEEE Robotics and Automation Letters ， 2023 ， 8 （ 1 ）： 216 – 223 . IC-GVINS： A Robust， Real-Time， INS-Centric GNSS-Visual-Inertial Navigation System Li X ， Wang X ， Liao J ， et al . Semi-Tightly Coupled Integration of Multi-GNSS PPP and S-VINS for Precise Positioning in GNSS-Challenged Environments ［J］. Satellite Navigation ， 2021 ， 2 ： 1 - 14 . Semi-Tightly Coupled Integration of Multi-GNSS PPP and S-VINS for Precise Positioning in GNSS-Challenged Environments Liao J ， Li X ， Wang X ， et al . Enhancing Navigation Performance Through Visual-Inertial Odometry in GNSS-Degraded Environment ［J］. GPS Solutions ， 2021 ， 25 ： 1 - 18 . Enhancing Navigation Performance Through Visual-Inertial Odometry in GNSS-Degraded Environment Cao S ， Lu X ， Shen S . GVINS： Tightly Coupled GNSS–Visual–Inertial Fusion for Smooth and Consistent State Estimation ［J］. IEEE Transactions on Robotics ， 2022 ， 38 （ 4 ）： 2004 – 2021 . GVINS： Tightly Coupled GNSS–Visual–Inertial Fusion for Smooth and Consistent State Estimation Liu J ， Gao W ， Hu Z . Optimization-Based Visual-Inertial SLAM Tightly Coupled with Raw GNSS Measurements ［C］// IEEE International Conference on Robotics and Automation ， Xi’an， China ， 2021 . Optimization-Based Visual-Inertial SLAM Tightly Coupled with Raw GNSS Measurements Cui Xiaozhen ， Zhou Qi ， Wu Dongjie ， et al . Factor Graph Fusion Localization Method with Tight and Loose Coupling of GNSS/IMU and Odometry ［J］. Geomatics and Information Science of Wuhan University ， 2023 ， DOI： 10.13203/j.whugig20220321 http://dx.doi.org/10.13203/j.whugig20220321 . Factor Graph Fusion Localization Method with Tight and Loose Coupling of GNSS/IMU and Odometry 崔晓珍 ， 周琪 ， 武东杰 ， 等 . GNSS/IMU与里程计紧-松耦合的因子图融合定位方法 ［J］. 武汉大学学报（信息科学版） ， 2023 ， DOI： 10.13203/j.whugig20220321 http://dx.doi.org/10.13203/j.whugig20220321 . GNSS/IMU与里程计紧-松耦合的因子图融合定位方法 Li B ， Zang N ， Ge H ， et al . Single-Frequency PPP Models： Analytical and Numerical Comparison ［J］. Journal of Geodesy ， 2019 ， 93 （ 12 ）： 2499 – 2514 . Single-Frequency PPP Models： Analytical and Numerical Comparison Saastamoinen J . Contributions to the Theory of Atmospheric Refraction ［J］. Bull Geodesique ， 1972 ， 105 ： 279 – 298 . Contributions to the Theory of Atmospheric Refraction Liu W ， Duan R ， Zhu F . A Robust Cascaded Strategy of In-Motion Alignment for Inertial Navigation Systems ［J］. International Journal of Distributed Sensor Networks ， 2017 ， 13 （ 9 ）： 1550147717732919 . A Robust Cascaded Strategy of In-Motion Alignment for Inertial Navigation Systems Furgale P ， Rehder J ， Siegwart R . Unified Temporal and Spatial Calibration for Multi-Sensor Systems ［C］// IEEE/RSJ International Conference on Intelligent Robots and System ， Tokyo， Japan ， 2013 . Unified Temporal and Spatial Calibration for Multi-Sensor Systems Autoware . Autoware Calibration Toolkit ［EB/OL］. （ 2021-08-20 ）［ 2026-06-01 ］. https：//gitlab.com/autowarefoundationautoware.ai /autoware https://gitlab.com/autowarefoundationautoware.ai/autoware . Autoware Calibration Toolkit Li X ， Han X ， Li X ， et al . GREAT-UPD： An Open-Source Software for Uncalibrated Phase Delay Estimation Based on Multi-GNSS and Multi-frequency Observations ［J］. GPS Solutions ， 2021 ， 25 （ 66 ）： 1 - 9 . GREAT-UPD： An Open-Source Software for Uncalibrated Phase Delay Estimation Based on Multi-GNSS and Multi-frequency Observations 10.13203/j.whugis20230119 P228 A 1671-8860（2023）07-1204-12 廖健驰,李星星,冯绍权.GVIL：基于图优化的GNSS PPP/视觉/惯性/激光雷达紧组合算法[J].武汉大学学报（信息科学版）,2023,48(7):1204-1215.DOI: 10.13203/j.whugis20230119 http://dx.doi.org/10.13203/j.whugis20230119 廖健驰,李星星,冯绍权.GVIL：基于图优化的GNSS PPP/视觉/惯性/激光雷达紧组合算法[J].武汉大学学报（信息科学版）,2023,48(7):1204-1215.DOI:10.13203/j.whugis20230119 LIAO Jianchi,LI Xingxing,FENG Shaoquan.GVIL: Tightly-Coupled GNSS PPP/Visual/INS/LiDAR SLAM Based on Graph Optimization[J].Geomatics and Information Science of Wuhan University,2023,48(7):1204-1215.DOI: 10.13203/j.whugis20230119 http://dx.doi.org/10.13203/j.whugis20230119 LIAO Jianchi,LI Xingxing,FENG Shaoquan.GVIL: Tightly-Coupled GNSS PPP/Visual/INS/LiDAR SLAM Based on Graph Optimization[J].Geomatics and Information Science of Wuhan University,2023,48(7):1204-1215.DOI:10.13203/j.whugis20230119 国家重点研发计划（2021YFB2501102）。 2023-04-04 2023-07-05 2023-06-29 39072475 武汉大学学报（信息科学版） 7 48