基于变分贝叶斯的鲁棒自适应因子图优化组合导航算法.pdf

第 45 卷 第 1 期2024 年 1 月仪器仪表学报Chinese Journal of Scientific InstrumentVol.45 No.1Jan.2024DOI:10.19650/ki.cjsi.J2312045收稿日期:2023-10-20 Received Date:2023-10-20基金项目:国家自然科学基金(61873064)、江苏省重点研发计划(BE2022139)项目资助基于变分贝叶斯的鲁棒自适应因子图优化组合导航算法陈熙源,周云川,钟雨露,戈明明(东南大学仪器科学与工程学院 南京 210096)摘 要:复杂环境下的量测粗差和时变噪声严重影响了状态估计的精度和可靠性,对此提出了一种基于变分贝叶斯的鲁棒自适应因子图优化组合导航算法。首先,基于先验和后验两阶段更新将变分贝叶斯推断引入因子图优化框架中,以估计时变量测噪声协方差;其次,利用相邻帧间的平均新息构造量测协方差预测值,作为粗差判据来实现稳健估计。基于 INS/GNSS 组合导航的仿真和现场实验评估表明,所提方法能在粗差干扰的情况下有效估计时变量测噪声,相比 M 估计和滑动窗口自适应因子图优化算法的水平定位误差分别减小了 26.7%和 39.8%,兼顾了估计精度和抗差性能,具有较好的复杂环境适应性。关键词:因子图优化;变分贝叶斯;组合导航;鲁棒自适应估计中图分类号:TH89 TN96 文献标识码:A 国家标准学科分类代码:460.40 510.40Robust adaptive factor graph optimization integrated navigation algorithm based on variational BayesianChen Xiyuan,Zhou Yunchuan,Zhong Yulu,Ge Mingming(School of Instrument Science and Engineering,Southeast University,Nanjing 210096,China)Abstract:The accuracy and reliability of state estimation are seriously affected by measurement outliers and time-varying noise in complex environments.To address these issues,a robust adaptive factor graph optimization(FGO)integrated navigation algorithm based on variational Bayesian is proposed.First,the variational Bayesian inference is introduced into the FGO framework based on a priori and a posteriori two-stage updating to estimate the time-varying measurement noise covariance.Secondly,the mean innovation between neighboring keyframes is used to construct measurement covariance prediction as an outlier judgment to achieve robust estimation.Simulation and field tests based on INS/GNSS integrated navigation show that the proposed method can effectively estimate the time-varying measurement noise covariance in the presence of outlier interference,and reduce the horizontal position error by 26.7%and 39.8%compared to the M-estimation and sliding window adaptive FGO algorithms,which takes into account the accuracy and robust performance.It has an excellent adaptation to complex scenarios.Keywords:factor graph optimization;variational Bayesian;integrated navigation;robust adaptive estimation0 引 言 惯性导航系统(inertial navigation system,INS)递推积分求解姿态、速度、位置等全面的导航参数,具有自主性强、动态性能好等优点,但低 成 本 惯 性 测 量 单 元(inertial measurement unit,IMU)噪声水平大,误差累积迅速,无法长期单独导航1。全球卫星导航系统(global navigation satellite system,GNSS)提供不随时间发散且高精度的绝对位置和时间信息,但是 GNSS 信号脆弱,复杂环境下的多路径与非视距信号会导致 GNSS 量测性能退化,可靠性不佳。INS 和 GNSS 构成的组合导航系统因成熟易用、优势互补被广泛应用2。传统的 INS/GNSS 组合导航通常基于卡尔曼滤波(Kalman filter,KF)进行信息融合,相关理论和应用较为成熟,随着即时定位与地图构建(simultaneous localization 第 1 期陈熙源 等:基于变分贝叶斯的鲁棒自适应因子图优化组合导航算法121 and mapping,SLAM)技术的发展和计算机性能的提高,基于因子图优化(factor graph optimization,FGO)的状态估计展现出更好的性能3-4。FGO 使用图模型编码状态和量测间的条件概率来表示批量估计问题,更长时间尺度上的多次迭代线性化能获得更优的全局一致性结果,即插即用的特性也使它能有效处理存在异步和时延的多源信息融合问题5。FGO 的最优状态估计建立在准确的量测模型之上,城市峡谷、园区工厂等多元化的复杂场景中,GNSS 的量测粗差和时变的噪声统计特性极大降低了状态估计的精度和可靠性,各类鲁棒自适应算法被广泛研究。基于FGO 的鲁棒估计主要包括传统的 M 估计和基于图的方法6,M 估计通过设计鲁棒代价函数对超过阈值的粗差进行降权7,基于图的方法则通过增加额外的量测权重约束到因子图进行抗差,但鲁棒估计仅依赖单点残差修正量测权重,并不具备量测噪声的估计能力。自适应估计在滤波框架中已有深入研究,主要原理为基于新息或残差序列估计协方差,其中的贝叶斯方法是最为通用和全面的8。文献 9 首次将变分贝叶斯(variational bayesian,VB)推断引入 KF,以递归形式对状态和量测噪声的联合后验执行变分近似。文献10考虑了精确的过程噪声不成立的情况,通过选取逆威沙特(inverse wishart,IW)先验分布,扩展了 VB 对预测误差协方差的估计。此外,基于 VB 推断的自适应方法还在异步多速率多传感器集成中被使用11,也与各类基于非线性滤波的组合导航系统有较好的结合12-13。而针对 FGO 自适应估计的研究较少,文献14推导了 FGO 在无先验情况下,滑动窗口残差序列的量测协方差估计。文献15将线性回归和 FGO 问题对应,基于新息序列相关方法给出了无偏的噪声方差估计,使用迭代重加权最小二乘联合求解状态和方差。上述两种自适应 FGO 方法中,均是基于新息序列估计噪声协方差的方法,要求较大的数据窗口获得可靠估计,不仅处理负载较高,且对快速变化的噪声跟踪性能不佳,另一方面它们对量测粗差也不具有稳健性。文献16利用多个高斯分布的混合来替代学生 t分布处理粗差,基于 VB 推断去近似状态和噪声的后验参数,仿真验证了其为多传感器融合提供的鲁棒导航解,但其忽略了噪声的时间波动因素,也未具体区分能有效自适应的量测因子。基于此,本文提出了一种基于变分贝叶斯的鲁棒自适应因子图优化组合导航算法。首先,利用先验和后验两阶段更新将变分贝叶斯推断扩展至 INS/GNSS 因子图优化框架中,以估计时变量测 噪 声 协 方 差,给 出 了VB-FGO 的详细推导和算法框架;此外,基于极大似然原理,利用边缘化的先验状态协方差和相邻两帧 GNSS 的平均新息构造量测噪声协方差的预测值以提前判别粗差;仿真和现场实验的评估结果表明所提算法有效提升了组合导航系统在复杂环境中的估计精度和鲁棒性能。1 组合导航因子图模型1.1 基于因子图优化的状态估计 因子图是一种表示多元函数因式分解的二分图模型G=(F,X,E),它由因子节点集合F,变量节点集合X,以及连接两类节点的无向边集合E 组成。因子图G对函数f(X)的因式分解定义为:f(X)=fi(Xi)(1)因子节点 fi F 是因式分解出的局部函数,Xi X是与 fi相关的变量节点子集,当变量 xj与因子 fi相关时,两者间产生一条无向边 eij E 连接。组合导航中的批量状态估计问题可描述为:根据 t0至 tk的量测序列 z=z1,zk 对所有时刻的系统状态x=x0,xk 进行最优估计,对贝叶斯定理应用最大后验概率(maximize a posterior,MAP)准则来求解最优估计17。xMAP=arg maxx p(x z)=arg maxx p(z x)p(x)(2)由于各时刻的状态和量测相互独立,先验 p(x)和似然 p(z x)概率密度可以进行如下因式分解:p(x)=ki=0p(xi)p(z x)=ki=1p(zixi)(3)式(3)与因子图同为连乘形式,系统状态对应变量节点,先验和量测概率被编码为因子节点,因子图表示的MAP 估计具有如下通用形式:xMAP=arg maxxifi(Xi),Xi x(4)假设传感器量测服从高斯分布,因子节点具备指数形式:fi(Xi)=exp-12ei(Xi)2i()(5)e2=eT-1e是平方马氏距离,表示残差e的加权平方和,为量测协方差,e是残差,由量测方程导出。对MAP 估计取负对数转换为等价最小二乘问题:xMAP=argmaxx-lnifi(Xi)()=arg minx iei(Xi)2i(6)因子图表述的批量最小二乘问题可以使用经典的滑动窗口或增量平滑的新成果 iSAM 等在线求解18。1.2 IMU 预积分因子 IMU 预积分因子综合一段时间内的多个 IMU 数据对相邻状态构成约束19,在积分时间 tk-1,tk 内,INS122 仪 器 仪 表 学 报第 4 5 卷的递推积分形式表示为:pnk=pnk-1+vnk-1tk+0.5gnt2k+Cnbk-1pIMUbk-1bkvnk=vnk-1+gntk+Cnbk-1vIMUbk-1bkqnbk=qnbk-1qIMUbk-1bk(7)导航系 n 定义为东北天,载体系 b 定义为右前上,下标 k 表示时刻 tk,pnk是位置,vnk是速度,Cnbk是 b 系到 n 系的旋转矩阵,表示 Cnbk对应的四元数乘法,gn是重力。由于 INS 的积分项依赖于上一时刻的姿态,但姿态在优化中频繁更新,因此使用预积分提高计算效率,pIMUbk-1bk、vIMUbk-1bk、qIMUbk-1bk分别表示位置、速度、姿态的预积分,采用INS 机械编排递推求解:pIMUbk-1bm=pIMUbk-1bm-1+0.5(vIMUbk-1bm-1+vIMUbk-1bm)tmvIMUbk-1bm=vIMUbk-1bm-1+CIMUbk-1bm-1(vm+0.5mvm)qIMUbk-1bm=qIMUbk-1bm-1cos0.5mmmsin0.5m (8)m和 vm分别是 IMU 在采样时间 tm-1,tm 内的角增量和比力增量,m是 m的模值,表示取反对称矩阵。将 IMU 的陀螺零偏 bgk和加速度计零偏bak建模为随机游走,根据量测和状态定义 IMU 预积分因子残差为:Cbk-1n(pnk-pnk-1-vnk-1tk-0.5gnt2k)-pIMUbk-1bkCbk-1n(vnk-vnk-1-gntk)-vIMUbk-1bk2qIMUbkbk-1(qbk-1nqnbk)xyzbgk-bgk-1bak-bak-1(9)其中,xyz表示取四元数的虚部组成向量,上述残差可以记作:fIMUk(xk,xk-1)=xk-hIMU(xk-1,zIMUk-1,k)(10)其中,zIMUk-1,k表示 IMU 预积分,hIMU是 IMU 预积分量测函数,IMU 预积分的协方差 IMUk-1,k等价于 KF 协方差在积分时间内的多次时间更新,IMUk-1,k可以从零初始状态开始根据下式递推:IMUk-1,m=Fm-1IMUk-1,m-1FTm-1+Gm-1Qm-1GTm-1(11)其中,Fm和 Gm分别是误差状态转移矩阵和噪声驱动 矩 阵,Qm是 IMU 白 噪 声 和 零 偏 的 协 方 差 矩阵20。IMU 预积分和 INS 机械编排算法的关系如图 1 所示。IMU 预积分解耦相邻状态间的积分依赖,积分一段时间内的多个 IMU 数据生成等效的约束来适应非线性优化算法。图 1 IMU 预积分与 INS 机械编排Fig.1 IMU preintegration and INS mechanization1.3 GNSS 因子 GNSS 提供全局位置,量测方程定义为:zGNSSk=pnk+nGNSSk,nGNSSk N(0,GNSSk)(12)其中,zGNSSk是卫星接收机的实际位置输出,nGNSSk是位置量测白噪声,服从均值为 0,方差为 GNSSk的高斯分布,根据量测和状态构造 GNSS 因子的残差:fGNSSk(xk)=pnk-zGNSSk(13)2 变分贝叶斯鲁棒自适应因子图 变分贝叶斯选取合适的先验分布,利用固定点迭代,以估计未知量测协方差,建模过程包括 IMU 预测的先验更新,以及 GNSS 量测的后验更新。2.1 先验更新 先验概率 p(xkz1:k-1)建模为高斯分布:p(xkz1:k-1)=N(xkxkk-1,Pkk-1)(14)其中,xkk-1和 Pkk-1分别是先验状态和对应的协方差,z1:k-1=zik-1i=1表示 t1至 tk-1的量测。xkk-1根据式(10)由 tk-1时刻的状态和预积分给出:xkk-1=hIMU(xk-1,zIMUk-1,k)(15)因子图的滑动窗口保留了多个状态,设其窗口大小为 s+1,状态协方差 Pk利用滑窗内多个因子构造的雅克比矩阵 J 和协方差矩阵 W 基于舒尔补进行边缘化求解:=JTW-1J=mmmrrmrrPk=(rr-rm-1mmmr)-1(16)是信息矩阵,下标 m=k-s:k-1 表示边缘化的状态涉及 tk-s至 tk-1时刻,下标 r=k 表示保留的状态为 tk时刻。先验状态协方差 Pkk-1由 IMU 预积分因子预测获得,其所需的 J 和 W 定义为:Jk-s:kk-1=Jk-s:k-1,fIMUk/xk-s:kTWk-s:kk-1=diag(Wk-s:k-1,IMUk-1,k)(17)各因子残差对滑窗内状态按列求导后以行向量的形式扩充雅克比矩阵 J,各因子残差协方差直接按对角线形式扩展协方差矩阵 W。第 1 期陈熙源 等:基于变分贝叶斯的鲁棒自适应因子图优化组合导航算法123 为了估计时变的量测协方差 Rk,通常选择 IW 分布作为共轭先验分布10:p(Rkz1:k-1)=IW(Rkvkk-1,Vkk-1)(18)其中,vkk-1和 Vkk-1分别表示 IW 分布的自由度参数和 逆 尺 度 矩 阵。根 据 贝 叶 斯 定 理,先 验 分 布p(Rkz1:k-1)可以表述为:p(Rkz1:k-1)=p(RkRk-1)p(Rk-1z1:k-1)dRk-1(19)为了保证先验分布与后验分布具有一致性,后验分布 p(Rk-1z1:k-1)也使用 IW 分布更新:p(Rk-1z1:k-1)=IW(Rk-1vk-1,Vk-1)(20)式(19)中的预测模型 p(RkRk-1)用于反应 Rk的时间波动程度,根据文献12 构造遗忘因子 传播 IW 的先验分布参数:vkk-1=(vk-1-n-1)+n+1Vkk-1=Vk-1(21)其中,vk是实数,n 表示 Rk的量测维数,一般取 (0,1,取值越小,越能快速响应量测协方差的变化;取值越大,对量测协方差的变化不敏感,估计较为缓慢和平稳。2.2 后验更新 联合后验概率 p(xk,Rkz1:k)没有解析解,但变分贝叶斯能寻找自由因子形式的近似分布来推断真实分布。假设联合后验 p(xk,Rkz1:k)可近似为高斯分布 qx(xk)与 IW 分布 qR(Rk)的乘积:p(xk,Rkz1:k)qx(xk)qR(Rk)qx(xk)=N(xkxk,Pk)qR(Rk)=IW(Rkvk,Vk)(22)一般使用 Kullback-Leibler(KL)散度作为变分推断中真实后验 p与近似后验q 的距离测度,上式估计问题对应的 KL 散度定义为:KLqx(xk)qR(Rk)p(xk,Rkz1:k)=qx(xk)qR(Rk)logqx(xk)qR(Rk)p(xk,Rkz1:k)dxkdRk(23)最小化上述 KL 散度获得待估参数的最优估计10,Rk以及 qR(Rk)的分布参数求解结果如下:Rk=Vk/(vk-n-1)vk=vkk-1+1Vk=Vkk-1+HkPkHTk+(zk-h(xk)(zk-h(xk)T(24)其中,zk和 h 分别是自适应因子的量测值和量测函数,Hk=h/xk是 h 的残差雅克比,Rk是量测协方差,在INS/GNSS 组合中,GNSS 因子作为自适应因子。后验状态xk经非线性优化算法迭代收敛后获得:x=(JTW-1J)-1JTkW-1kfxk=x+xk(25)其中,x 是因子图滑动窗口内的所有状态,x 是状态增量,k表示选取 tk时刻的状态。后验状态xk及其协方差 Pk在添加 IMU 因子做先验更新的基础上,继续添加 GNSS 因子进行优化求解后获得,其所需的 J 和 W 定义为:Jk-s:k=Jk-s:kk-1,fGNSSk/xk-s:kTWk-s:k=diag(Wk-s:kk-1,GNSSk)(26)下标 k-s:k k-1表示因子图在 tk时刻添加了 IMU因子做预测,下标 k-s:k 表示因子图在 tk时刻添加了GNSS 因子做更新。2.3 帧间新息抗差 纳入粗差会极大地影响变分推断的稳定性,在变分更新前,基于相邻两帧 GNSS 的平均新息预测量测协方差可以提前判别粗差,有效提升复杂环境中的估计稳健性并节省变分后验迭代的计算。根据量测和预测构建新息 sk及其协方差 Sk:sk=zk-h(xkk-1)Sk=Rk+HkPkk-1HTk(27)根据极大似然原理21,基于新息序列计算量测协方差的预测值Rk:Rk=Sk-HkPkk-1HTkSk=(1-)sk-1sTk-1+sksTk(28)粗差的Rk较大,易于判别,为减小计算量和存储空间,取 =0.5,用新息的相邻时间平均做等权估计,根据精度需求设定粗差阈值 Rmax即可实现高效、准确的粗差剔除,其中 Pkk-1已在变分推断的先验更新中获得。2.4 算法框架与流程 基于 INS/GNSS 组合导航的变分贝叶斯鲁棒自适应因子图优化算法框架如图 2 所示。图 2 VB-FGO 算法框架Fig.2 VB-FGO algorithm framework124 仪 器 仪 表 学 报第 4 5 卷算法整体流程包括一步先验更新和迭代后验更新:先验阶段仅使用 IMU,预积分预测先验状态xkk-1,边缘化求解先验状态协方差 Pkk-1,遗忘因子 传播 GNSS 量测协方差 GNSSk的 IW 分布参数;后验阶段利用 IMU 的先验和 GNSS 量测执行粗差检测,进一步加入 GNSS 因子后,利用变分迭代更新后验状态xk、后验状态协方差 Pk以及 GNSSk的 IW 分布参数。3 仿真实验 将本文算法与几种流行方法在 INS/GNSS 组合导航仿真中进行对比,以验证所提方法在包含量测粗差和时变量测噪声的复杂环境中具有的优越性,各种对比算法的类型及说明如表 1 所示。表 1 对比的算法类型和说明Table 1 Types and descriptions of algorithms comparison算法说明FG常规因子图算法18MAFGM 估计自适应因子图算法6Huber 核函数的调谐常数 c=1.345SWAFG滑动窗口自适应因子图算法14残差序列窗口大小 s=30VBAFG变分贝叶斯自适应因子图算法粗差阈值 Rmax=(20 m)2,遗忘因子 =0.96 图 3 展示了仿真时长约 1 000 s 的机器人三维运动轨迹,机器人行驶通过斜坡、转弯等多样路况后到达终点,仿真传感器的性能指标参如表 2 所示。图 3 机器人仿真三维运动轨迹Fig.3 Robot simulation 3D motion trajectory3.1 量测噪声跟踪性能 首先评估所提算法对时变量测噪声的跟踪性能,记作仿真实验 1,设计了 GNSS 位置量测噪声协方差 Rk突变和缓变的两种场景:表 2 传感器仿真参数Table 2 Sensors simulation parameters传感器精度频率IMUGNSS陀螺零偏10(h-1)陀螺随机游走0.6(h12)加速度计零偏40 g加速度计随机游走75(g Hz12)位置标准差1 m100 Hz1 Hz Rk=102R0,k=200,4001+9sink-700200()2R0,k=700,900R0,其他(29)初始的 R0=diag(1 m2,1 m2,1 m2),在 200400 s内,标准差阶跃突变为初始值的 10 倍,在 700900 s 内,标准差以正弦函数发生缓变,其余时段标准差恒为初始值。VBAFG 的量测噪声分布参数初值设定为 v0=2,V0=R0,图 4 展示了不同算法估计的量测噪声标准差。图 4 仿真实验 1 量测噪声标准差Fig.4 Simulation test1 measurement noise standard deviationMAFG 计算的标准差并没有收敛到参考值,而是一种快速波动状态,原因是它仅基于固定的 Rk和权函数粗略地调整当前的 Rk,不具备估计收敛的能力。SWAFG的噪声估计依赖于残差窗口的大小,需要一定数据量来感知量测标准差的变化,存在明显的拖尾效应,而小窗数据下的噪声估计平稳性又欠佳。VBAFG 的噪声估计使用迭代逼近策略,对数据量不作要求,它的标准差估计比SWAFG 有更好的灵敏性,同时也兼具稳定性。第 1 期陈熙源 等:基于变分贝叶斯的鲁棒自适应因子图优化组合导航算法125 4 种算法的姿态和位置误差曲线分别如图 5 和 6 所示,表 3 给出了对应的均方根误差(root mean square error,RMSE)统计值。图 5 仿真实验 1 姿态误差Fig.5 Simulation test1 attitude error图 6 仿真实验 1 位置误差Fig.6 Simulation test1 position errorFG 在量测协方差时变的情况下误差最大,明显有偏的噪声建模导致 FG 的估计不再最优,降低了 INS 的有效利用。MAFG 和 SWAFG 的估计精度相较 FG 均有一定改善但差于 VBAFG,具体地:VBAFG 在东北方向上的位置误差相比 MAFG 减少了 16.1%、32.0%,相比 SWAFG减少了 12.1%、21.7%;在俯仰和航向角上的姿态误差相比 MAFG 减少了 36.9%、73.2%,相比 SWAFG 减少了23.3%、54.5%。由于机器人没有天向位移和横滚转动,这些指标提升有限。表 3 仿真实验 1 姿态和位置 RMSETable 3 Simulation test1 attitude and position RMSE参数算法FGMAFGSWAFGVBAFG位置误差/m姿态误差/()东向2.9952.7212.5952.281北向2.2141.9631.7031.333天向0.9890.8900.7490.728俯仰0.1230.0730.0600.046横滚0.1560.0920.0710.062航向0.8571.3900.8180.3723.2 量测粗差和时变噪声下的鲁棒自适应性能 进一步评估本文算法于既有时变量测噪声,又有量测粗差的复杂场景中的自适应抗差性能,记作仿真实验 2,设计了如图 7 所示的 GNSS 位置量测。图 7 存在粗差和时变噪声的 GNSS 量测Fig.7 GNSS measurement with outlier and time-varying noise400800 s 内的 GNSS 噪声标准差变为 10 m,其余时段为 1 m,并在 450750 s 内以 10%的概率出现标准差为100 m 的量测粗差:Rk=102R0w.p.0.91002R0w.p.0.1(30)3 种自适应算法估计的量测噪声标准差如图 810,分别给出了算法的姿态和位置误差曲线。图 8 仿真实验 2 量测噪声标准差Fig.8 Simulation test2 measurement noise standard deviation126 仪 器 仪 表 学 报第 4 5 卷图 9 仿真实验 2 姿态误差Fig.9 Simulation test2 attitude error图 10 仿真实验 2 位置误差Fig.10 Simulation test2 position errorMAFG 对粗差拥有很好的判别性,粗差均以较为明显的尖峰在图 8 中呈现出来。SWAFG 不具备抗差能力,粗差的分布特性与滑窗内的残差白噪声序列有明显差异,严重影响了其噪声估计的准确性,出现了较大的偏差,并需要一定时间来收敛到合理范围。VBAFG 引入了量测协方差预测机制,于变分后验更新前剔除粗差,兼具了鲁棒和自适应的优势,在图 9 和 10 中也具有明显最小的姿态和位置误差。4 现场实验 为进一步验证所提算法在实际应用中的性能,以四驱轮式移动机器人为实验载体,在校园内开展了现场实验,机器人实物如图 11 所示。图 11 轮式移动机器人Fig.11 Wheeled mobile robotJetson TX1 计算机运行 Ubuntu18.04 系统,基于 ROS框架采集传感器数据;DETA100R4G 组合导航设备内置消费级 IMU,并接入千寻网络 RTK(real-time kinematic)提供厘米级定位;u-blox m10 接收机提供 GNSS 位置量测;巨磁阻编码器提供轮速里程计(odometer,ODO)量测。实验以 RTK、IMU 和 ODO 的事后批量优化解作为位置基准,采用 GNSS、IMU 和 ODO 来运行 4 种对比算法,由于轮式载体未装备高精度姿态测量基准设备,因此现场实验仅对算法的位置性能进行了对比。机器人导航传感器参数如表 4 所示。表 4 机器人导航传感器参数Table 4 Robot navigation sensors parameters传感器精度频率IMU陀螺零偏10(h-1)陀螺随机游走0.2(h12)加速度计零偏40 g加速度计随机游走75(g Hz12)100 HzGNSS位置2.5 m1 HzRTK位置0.01 m1 HzODO速度0.02(m s-1)20 Hz 现场实验运行路线如图 12,机器人环绕校园一圈,途径 1 号标记的树木遮挡区域和 2 号标记的建筑遮挡区域,这两个区域分别存在一定时变的量测噪声和量测粗差。各类算法的二维平轨迹对比如图 13 所示,图例 REF表示位置基准轨迹,参数设定与仿真一致。图 13 下侧的树木遮挡区域中,VBAFG 与参考轨迹贴合度最优,表现出更好的融合定位精度。图 13 上侧建筑遮挡区域的粗差使得 FG 和 SWAFG 的定位结果受到 第 1 期陈熙源 等:基于变分贝叶斯的鲁棒自适应因子图优化组合导航算法127 图 12 现场实验运行路线Fig.12 Field test run route图 13 现场实验平面轨迹对比Fig.13 Comparison of field test plane trajectories明显扰动,后续时间里在北向产生了一定程度的估计发散;MAFG 和 VBAFG 对粗差具有鲁棒性,定位轨迹较平滑,也未出现估计发散,并且 VBAFG 具有更高的定位精度,自适应性能最佳。现场实验的位置误差曲线和对应的 RMSE 统计分别如图 14 和表 5 所示。图 14 现场实验位置误差Fig.14 Field test position error表 5 现场实验位置 RMSETable 5 Field test position RMSE参数算法FGMAFGSWAFGVBAFG位置误差/m东向3.892.732.632.30北向6.753.704.942.46天向1.491.191.441.07 图 14 中 VBAFG 在两个遮挡时段表现出更小的位置误差,整体定位精度也明显优于其他方法,水平定位误差维持在 3.5 m 以内,由于实验中的天向运动少,并引入了非完整性约束,各方法的天向位置精度优于水平方向。具体地,VBAFG 在东北天方向上的位置精度相比 FG 提升了 40.8%、63.5%、28.2%;相比 MAFG 提升了 15.7%、33.5%、10.1%;相 比 SWAFG 提 升 了 12.6%、50.2%、25.7%。5 结 论 本文提出了一种基于变分贝叶斯的 INS/GNSS 鲁棒自适应因子图优化算法来应对复杂环境中 GNSS 量测性能退化导致的状态估计精度降低和稳定性差的问题。引入变分推断至因子图优化框架中估计时变的量测噪声,基于帧间新息构造量测粗差判据实现稳健估计。仿真和现场实验证明了所提算法相比几种流行方法方法的优越性,该算法的复杂环境自适应性能好,兼顾了抗差和精度,能实现可靠、准确的状态估计。参考文献 1 李倩,蒋正华,孙炎,等.基于因子图的 INS/UWB 室内行人紧组合定位技术 J.仪器仪表学报,2022,43(5):32-45.LI Q,JIANG ZH H,SUN Y,et al.INS/UWB tight integrated localization technology for pedestrian indoor based on factor graphJ.Chinese Journal of Scientific Instrument,2022,43(5):32-45.2 BAI S,LAI J,LYU P,et al.Improved preintegration method for GNSS/IMU/in-vehicle sensors navigation using graph optimization J.IEEE Transactions on Vehicular Technology,2021,70(11):11446-11457.3 朱建良,王栋,徐旋孜.一种基于图优化的行人协同定位方法J.仪器仪表学报,2023,44(6):126-134.128 仪 器 仪 表 学 报第 4 5 卷ZHU J L,WANG D,XU X Z.A pedestrian cooperative localization method based on graph optimization J.Chinese Journal of Scientific Instrument,2023,44(6):126-134.4 WEN W,PFEIFER T,BAI X,et al.Factor graph optimization for GNSS/INS integration:A comparison with the extended Kalman filter J.NAVIGATION:Journal of the Institute of Navigation,2021,68(2):315-331.5 WANG Q,FAN S,ZHANG Y,et al.A novel adaptive sliding observation-based cooperative positioning algorithm under factor graph framework for multiple UUVs J.IEEE Transactions on Industrial Informatics,2023,19(8):8743-8753.6 WATSON R M,GROSS J N.Robust navigation in GNSS degraded environment using graph optimization C.Proceedings of the 30th International Technical Meeting of the Satellite Division of the Institute of Navigation(ION GNSS+2017),2017:2906-2918.7 WANG J,CHEN X,SHI C,et al.Robust M-estimation-based ICKF for GNSS outlier mitigation in GNSS/SINS navigation applicationsJ.IEEE Transactions on Instru-mentation and Measurement,2023,72:1-17.8 徐健,宋晓萍,张宏瀚,等.基于变分贝叶斯的 DR/UTP组合导航滤波方法J.仪器仪表学报,2016,37(12):2743-2749.XU J,SONG X P,ZHANG H H,et al.DR/UTP integrated navigation based on variational Bayes J.Chinese Journal of Scientific Instrument,2016,37(12):2743-2749.9 SARKKA S,NUMMENMAA A.Recursive noise adaptive Kalman filtering by variational Bayesian approxi-mationsJ.IEEE Transactions on Automatic Control,2009,54(3):596-600.10 HUANG Y,ZHANG Y,WU Z,et al.A novel adaptive Kalman filter with inaccurate process and measurement noise covariance matrices J.IEEE Transactions on Automatic Control,2018,63(2):594-601.11 DAVARI N,GHOLAMI A.Variational Bayesian adaptive Kalman filter for asynchronous multirate multi-sensor integrated navigation system J.Ocean Engineering,2019,174:108-116.12 ZHONG Y,CHEN X,ZHOU