基于无迹卡尔曼滤波的BDS_INS组合定位方法.pdf

沈 阳 航 空 航 天 大 学 学 报Journal of Shenyang Aerospace University第40卷 第4期2 0 2 3 年 8 月Vol.40 No.4Aug.2 0 2 3基于无迹卡尔曼滤波的BDS/INS组合定位方法王尔申a，b，朱骏a，徐嵩a，杨健b，宋建b，陈昌龙b，刘依凡a（沈阳航空航天大学 a.电子信息工程学院，b.辽宁通用航空研究院，沈阳 110136）摘要：针对车辆载体使用惯性导航系统（inertial navigation system，INS）时定位误差随时间而累积以及北斗卫星导航系统（BeiDou navigation satellite system，BDS）信号易受干扰影响定位精度和可靠性的问题，通过分析惯导定位、空间杆臂和时间不同步的误差，建立BDS/INS组合定位状态方程，研究基于无迹卡尔曼滤波（unscented kalman filter，UKF）的BDS/INS组合导航信息融合算法，并对算法进行验证。结果表明，UKF组合导航算法的定位性能优于扩展卡尔曼滤波（extended kalman filter，EKF）算法，其纬度与经度参数的均方根误差比EKF算法减小75.3%与83.8%，提升了BDS/INS组合定位精度。关键词：北斗卫星导航系统；惯性导航系统；组合定位；无迹卡尔曼滤波；定位误差中图分类号：TN967.2 文献标志码：Adoi：10.3969/j.issn.2095-1248.2023.04.003BDS/INS integrated positioning method based on unscented Kalman filterWANG Ershena，b，ZHU Juna，XU Songa，YANG Jianb，SONG Jianb，CHEN Changlongb，LIU Yifana（a.College of Electronic and Information Engineering，b.Liaoning General Aviation Academy，Shenyang Aerospace University，Shenyang 110136，China）Abstract：Aiming at the problem that the positioning error accumulates with time when the vehicle carrier uses the inertial navigation system（INS），and the BeiDou satellite navigation system（BDS）signal is susceptible to interference，the positioning accuracy and reliability are affected.By analyzing the errors of inertial navigation positioning，space arm and time asynchronous，BDS/INS integrated navigation state equation was established by analyzing the inertial positioning error，space arm error and time asynchronous error，and a BDS/INS integrated navigation information fusion algorithm based on unscented kalman filter（UKF）was obtained.The simulation results show that this integrated navigation method is superior to extended kalman filter（EKF）integrated navigation algorithm，and the root mean square error of latitude and longitude decrease by 75.3%and 83.8%compared with EKF algo收稿日期:2023-01-30基金项目:国家自然科学基金(项目编号:62173237);工信部民机专项(项目编号:01020220627066-3);辽宁省重点研发计划项目(项目编号:2020JH2/10100045);辽宁省应用基础研究计划项目(项目编号:2022063)作者简介:王尔申(1980-),男,辽宁辽阳人,教授,博士,主要研究方向:卫星导航接收机信号处理、航空电子技术,E-mail:wanges_。文章编号：2095-1248（2023）04-0019-06沈 阳 航 空 航 天 大 学 学 报第 40 卷rithm.The accuracy and reliability of navigation and positioning are improved and the cumulative error of inertial navigation is reduced.Key words：BeiDou navigation satellite system；inertial navigation system；integrated positioning；unscented Kalman filter；positioning error惯性导航系统 INS具有自主性好、数据更新率高、抗干扰能力强等优点，但是导航误差存在随时间积累的问题1。全球导航卫星系统（global navigation satellite system，GNSS）能够提供全球、全天候、高精度导航定位授时信息，但易受无线电干扰 2。组合导航系统利用两种或两种以上的导航设备提供多重信息，构成定位鲁棒性更强的系统3。北斗卫星导航系统（BDS）是我国自主建设、独立运行的全球卫星导航系统4，将惯导与北斗定位进行组合，发挥系统各自优势，成为组合导航技术研究的热点。组合导航的组合方式分为松组合、紧组合以及深组合5。松组合结构简单，BDS和 INS可以独立工作，使用 BDS 数据修正 INS；BDS和INS紧组合是伪距及伪距率的组合6；深组合又称超紧组合，是将 INS 的测量值反馈给BDS，以组合滤波后的导航信息修正INS7。近年来，研究人员采用不同的算法线性化组合导航数学模型，从而提高组合导航的精度。目前，常见的组合导航滤波算法主要有扩展卡尔曼滤波8EKF、无迹卡尔曼滤波9UKF、粒子滤波10（particle filter，PF）等，而 UKF 因无需计算雅可比矩阵，具有高精度及方便实现的特性，在组合导航数据融合算法中脱颖而出。本文主要研究松组合，并将基于UKF的组合导航定位结果与基于EKF得到的结果进行比较，验证了不同组合导航算法对定位精度的影响。1无迹卡尔曼滤波扩展卡尔曼滤波 EKF解决了经典卡尔曼滤波不能应用于非线性系统的问题11-12，但扩展卡尔曼滤波要求噪声服从高斯概率分布等13。无迹卡尔曼滤波 UKF 采用无迹变换（unscented transform，UT）的方法，对得到的确定的样本点（Sigma点）进行非线性变换处理，是一种基于确定性采样的非线性滤波算法14。离散非线性系统状态方程和观测方程如式（1）和式（2）所示xk=f(xk-1Vk-1)（1）zk=f(xkWk)（2）式中：xk为n维状态向量；Wk为p维系统噪声；Vk为q维量测噪声。针对组合导航的状态方程，在UKF滤波算法中需要对状态进行扩维处理，状态矢量经扩维后变为：xak=xTkWTkVTkT，则xak的状态协方差阵为Pak=E(xak-xak)(xak-xak)T（3）状态变量xak为L维列向量，则L=n+p+q。选取UT变换的变量xak，其表达式如下所示xk-1j=xk-1xk-1+()L+Pk-1xk-1-()L+Pk-1（4）式中：xk-1j为状态变量均值附近的第j个采样点；系数=2(L+)-L；和为待选调节参数。当状态变量为多变量时，一般选择=3-L01，其中是一个小量，以避免状态方程非线性严重时采样点的非局域性影响。适当调节和，可以提高估计均值的精度。UKF的时间更新方程为x*k|k-1j=f(x*k|k-1jxk-1j)（5）xk|k-1=j=02LWmjx*k|k-1（6）pk|k-1=j=02Lwcj(x*k-1j-xk|k-1)(x*k-1j-xk|k-1)T20王尔申，等：基于无迹卡尔曼滤波的BDS/INS组合定位方法第 4 期（7）zk|k-1=h(x*k|k-1xwk-1j)（8）zk|k-1=j=02LWmjzk|k-1（9）UKF的量测更新方程为pzkzk=j=02LWcj(zk|k-1-zk|k-1)(zk|k-1-zk|k-1)T（10）pxkzk=j=02LWcj(x*k|k-1j-xk|k-1)(zik|k-1j-zk|k-1)T（11）Kk=pxkykp-1ykyk（12）xk=xk-1Kk(zk-zk|k-1)（13）pk=pk|k-1-KkpzkzkKTk（14）式中：x*k|k-1j为一步更新后的 Sigma 点集；xk-1为状态量时间更新的一步预测值；pk|k-1为一步预测误差方差；zk|k-1为k-1开始到k时刻测量值z的估计值；pzkzk是观测量的方差值；观测量和状态量的协方差矩阵是pxkzk；Kk为k时刻的滤波算法增益；xk为k时刻的估计值：pk为k时刻滤波误差方差的估计值；参数0，W0=/(L+)，WCi=Wi=0.5/(L+)，WC0=W0+1-2+。根据组合导航系统的状态方程和量测方程，设定一定的滤波初值，利用上述的UKF滤波算法能够获得第k时刻的系统状态估计值xk(k=1234)。2基于无迹卡尔曼滤波的BDS/INS组合导航系统设计2.1BDS/INSBDS/INS 空间杆臂误差模型与时间不空间杆臂误差模型与时间不同步误差模型同步误差模型惯性导航系统一般将导航定位的参考基准设置为惯性元件的几何中心，然而卫星导航的参考基准通常为接收机天线的相位中心，当实际运载体中同时使用多种导航系统时，它们的安装位置会存在一定的偏差15。为将不同导航系统的导航信息进行比对和融合，需考虑空间杆臂误差。设惯性元件相对于地心的矢量为R，北斗卫星接收机天线相位中心相对于地心的矢量为r，天线相位中心相对于惯性元件的矢量l称为杆臂距离，三者之间的矢量关系满足r=R+lr（15）因接收机天线和惯性元件的安装位置通常固定不动，所以杆臂距离l在惯性元件坐标系下为常矢量，将公式（15）两边相对地球坐标系求导，可得公式（16），其中记ven(BDS)为卫星天线的地速，ven(INS)为惯导的地速ven(BDS)=ven(INS)+ebl（16）由于杆臂距离一般在米量级，所以两种导航坐标系之间的角度差非常小，近似认为bebbib，bebbnb，则杆臂速度误差vnL、位置误差pGL如下所示。式中矩阵Cnb表示载体坐标系相对于导航坐标系的姿态阵；beb为载体坐标系相对于地球坐标系的角速度；LINS为惯导定位经度；RMh、RNh分别为子午圈主曲率半径和卯酉圈主曲率半径。vnL=vnINS-vnGNSS=-Cnb(beblb)=-Cnb(beb)lb（17）pGL=pINS-pGNSS=-MpvCnblb（18）Mpv=01/RMh0secLINS/RNh00001（19）在BDS/INS组合导航系统中，计算传感器导航信息的时刻通常与传感器实际的信息采集时刻不同，从传感器信息采集到组合导航计算之间存在一定的时间滞后。文中在组合导航系统建立时，将对时间不同步误差进行估计和补偿。如图1所示，INS和BDS两类传感器相对组合导航计算滞后的时间一般并不相同，将两者之间的相对滞后记为时间不同步误差t，可图1时间不同步误差21沈 阳 航 空 航 天 大 学 学 报第 40 卷以得到惯导速度和卫星速度之间的关系为vnGNSS+ant=vnINS（20）式中：an是载体在不同步时间t附近的平均线加速度，如公式（21）所示anvnINS(m)-vnINS(m-1)T（21）由式（20）计算可得 BDS 与 INS 间的速度不同步误差vnt，同理可得位置不同步误差pt。vnt=vnINS-vnGNSS=ant（22）pt=pINS-pGNSS=MpvvnINSt（23）3.2BDS/INSBDS/INS组合导航系统模型组合导航系统模型文中建立状态方程在传统的15维状态向量的基础上，考虑到空间杆臂误差与时间不同步误差，将系统状态向量扩大到19维。即导航系统误差状态变量由姿态角误差、速度误差v、位置误差(L、h)、陀螺仪漂移误差、加速度计零偏误差、空间杆臂误差l和时间不同步误差t组成，误差状态向量如式（24）所示XI(t)=ENUVEVNVuLhbxbybzbxbybzlbxlbylbztT（24）导航系统的误差状态方程如式（25）所示XI(t)=FI(t)XI(t)+GI(t)WI(t)（25）式中：XI(t)为系统状态变量；FI(t)为状态转移矩阵；WI(t)为系统噪声矩阵；GI(t)为噪声驱动矩阵；系统的转移矩阵FI(t)为 1919 维的矩阵。松组合导航方式有两组观测值，分别是BDS 和 INS 输出的位置差值和速度差值。Vp和VV为BDS接收机沿坐标系3个轴方向上的位置与速度误差，则位置量测与速度量测方程为Zp=RcosL+nxRL+nyh+nz=HpX+Vp（26）ZV=Vx+nvxVy+nvyVz+nvz=HVX+VV（27）3BDS/INS 组合导航算法实验分析3.1实测数据采集实验平台实测数据采集实验平台为了验证基于无迹卡尔曼滤波的 BDS/INS组合导航方法的性能，本文通过组合导航开发系统，采集真实数据进行组合导航算法实验验证。实验数据采集设备使用组合导航平台 ZH3000A-OEM-ED，该平台配置有高性能的板载惯性测量单元 IMU 和高精度 GNSS 双天线接收板卡，能够输出BDS和IMU数据，支持高精度RTK定位。实验在体育场跑道上进行，在实验数据采集过程中，进行了加速、减速和转弯等机动性运动，实验轨迹如图2所示。3.3算法验证与结果分析算法验证与结果分析针对EKF与UKF两种滤波模型，采用实测数据进行组合导航数据融合实验。本文选取了其中420 s的实验数据用于算法验证，采用实验仪器配套的组合导航处理软件的高精度定位结果作为参考值，将两组实验的导航结果与参考值进行比较，分析不同滤波算法在组合导航系统中的性能。基于EKF算法的组合导航速度误差和位置误差如图3所示，而基于EKF算法的组合导航速度误差和位置误差如图4所示。通过对两种滤波方法的误差比较可以得出：EKF算法的东向、北向速度误差在4 m/s的图2实验数据采集轨迹22王尔申，等：基于无迹卡尔曼滤波的BDS/INS组合定位方法第 4 期范围内，而UKF算法的东向、北向速度误差控制在2 m/s的范围；EKF算法的经度、纬度误差在20 m左右，而UKF算法的经度、纬度速度误差控制在5 m的范围内。两种滤波算法定位结果的误差特性统计表和经纬度轨迹对比图如表1和图5所示。通过表1分析得出，UKF算法相比EKF算法定位误差明显减小，纬度和经度的均值分别减小 73.5%和 91.4%，均方根误差分别减小75.3%和83.8%。由图5看出，UKF算法的经纬度轨迹与参考系统输出的经纬度信息变化趋势接近，且误差与EKF算法相比更小。4结论文中研究了无迹卡尔曼滤波 BDS/INS 组合导航定位算法，将传统15维状态向量扩展到19维，并采用无迹卡尔曼滤波算法对导航信息进行处理。利用组合导航平台采集实测数据，验证组合导航EKF、UKF滤波算法的性能。验2 20 0-2-2-4-45050 100100 150150 200200 250250 300300 350350 400400 450450t/st/s2 20 0-2-2-4-45050 100100 150150 200200 250250 300300 350350 400400 450450t/st/s0 0-20-205050 100100 150150 200200 250250 300300 350350 400400 450450t/st/s0 0-10-10-20-205050 100100 150150 200200 250250 300300 350350 400400 450450t/st/s10100 0-10-10-20-205050 100100 150150 200200 250250 300300 350350 400400 450450t/st/s0 0-200-2005050 100100 150150 200200 250250 300300 350350 400400 450450t/st/s-400-400-40-40-60-60a?a?b?b?0 00 00 00 00 00 0L/mL/m/m/mH m/H m/V/()m s-1V/()m s-1 E EV/()m s-1V/()m s-1 N NVu/()m s-1Vu/()m s-1 图3EKF误差表1组合定位误差分析 m滤波算法EKFUKF纬度误差均值-1.124 2-0.298 0纬度误差RMSE7.707 31.900 3经度误差均值-1.840 4-0.157 9经度误差RMSE6.386 91.035 6?/()?/()41 9272.41 9272.41 9268.41 9268.41.926641.926641.926441.926441.926241.926241.926041.926041.925841.925841.925641.925641.925441.9254123 4012.123 4012.123 4014.123 4014.123 4016.123 4016.123 4018.123 4018.123 4022.123 4022.123 4026.123 4026.123 4020.123 4020.123 4024.123 4024.123 4028.123 4028.?/()?/()EKFEKFUKFUKFReal TrajectoryReal Trajectory41 9270.41 9270.图5经纬度轨迹对比1 10 0-1-1-2-25050 100100 150150 200200 250250 300300 350350 400400 450450t/st/s2 20 0-2-25050 100100 150150 200200 250250 300300 350350 400400 450450t/st/s0 0-20-20-40-40-60-605050 100100 150150 200200 250250 300300 350350 400400 450450t/st/s5 50 0-5-55050 100100 150150 200200 250250 300300 350350 400400 450450t/st/s5 50 0-5-55050 100100 150150 200200 250250 300300 350350 400400 450450t/st/s0 0-100-100-200-2005050 100100 150150 200200 250250 300300 350350 400400 450450t/st/sa?a?b?b?/m/mL/mL/mH m/H m/0 00 00 00 00 00 0V/()m s-1V/()m s-1 E EV/()m s-1V/()m s-1 N NVu/()m s-1Vu/()m s-1 图4UKF误差23沈 阳 航 空 航 天 大 学 学 报第 40 卷证结果表明：本文的组合导航定位算法相比EKF算法在组合导航定位经纬度参数上的均方根误差分别减小了83.8%与75.3%，较好地提升了组合导航定位精度。参考文献（References）：1Li K L，Chang L B，Chen Y B.Common frame based unscented quaternion estimator for inertial-integrated navigationJ.IEEE/ASME Transactions on Mechatronics,2018,23(5):2413-2423.2王尔申，孙彩苗，佟刚，等.基于 PSO 的多星座GNSS垂直保护级优化方法J.北京航空航天大学学报,2021,47(11):2175-2180.3秦永元，张洪钺，王叔华.卡尔曼滤波与组合导航原理 M.西安：西北工业大学出版社，2012.4王尔申，王欢，雷虹，等.基于麻雀搜索算法的ARAIM故障子集优化方法 EB/OL .（2022-10-09）2023-03-02 .https：/doi.org/10.13700/j.bh.1001-5965.2022.0596.5程保喜.GNSS与惯性导航组合系统在复杂环境下的定位研究 J.中北大学学报（自然科学版），2021，42（01）：89-96.6Huang Y R，Fu J H，Xu S Y，et al.Research on integrated navigation system of agricultural machinery based on RTK-BDS/INSJ.Agriculture,2022,12(8):1169.7刘娣，朱松青，陈桂，等.一种GPS/BDS/INS深组合导航系统降维滤波方法J.南京理工大学学报,2021,45(6):656-664.8易颂轩，高伟强，刘建群，等.基于图优化和EKF的 UWB 组合定位算法J.电子测量技术,2022,45(23):1-6.9荆蕾，孙炜玮，乔玉新，等.GNSS/SINS组合导航系统的自适应UKF算法J.大地测量与地球动力学,2023,43(3):255-258，281.10赵文晔，高井祥，李增科，等.地图匹配辅助的KF-PF室内定位算法模型J.武汉大学学报（信息科学版）,2018,43(5):806-812.11Chang L，Qin F，Jiang S.Strapdown inertial navigation system initial alignment based on modified process modelJ.IEEE Sensors Journal,2019,19(15):6381-6391.12杨晓明，王胜利，王海霞，等.基于EKF的GNSS/INS 组合导航系统应用J.山东科技大学学报（自然科学版）,2019,38(6):114-122.13郝 雨 时，徐 爱 功，隋 心，等.城 市 环 境 BDS/MEMS-IMU组合导航的模型误差抑制方法J.导航定位学报,2018,6(3):60-65.14乔玉新，林雪原，张吉松，等.发射系下的 INS/CNS/GNSS 组合导航 UKF 滤波算法J.中国空间科学技术,2021,41(5):103-109.15张大龙，余刚，李致远，等.考虑杆臂误差的组合导航分离协方差交叉算法J.郑州大学学报（工学版）,2022,43(3):8-14.（责任编辑：刘划 英文审校：杜文友）24