面向低空无人机运行的高精度卫星测速算法.pdf

航空科学与工程航电与空管面向低空无人机运行的高精度卫星测速算法吉莉1,2，孙蕊1,2*（1.南京航空航天大学海上智能网信技术教育部重点实验室,江苏南京211106；2.南京航空航天大学空中交通管理系统全国重点实验室,江苏南京211106）摘要：针对无人机在城市低空环境下运行时存在多径误差导致星基导航测速精度较差的问题，提出一种利用卫星方位角加权和多普勒观测值辅助的优化测速算法。该算法融合多普勒观测值与载波相位观测值优势，根据无人机低空运行环境特点构建基于卫星方位角的加权模型，有效削弱了多路径效应对测速精度的影响，并通过抗差卡尔曼滤波提升速度估计的鲁棒性。基于实测数据的算法评估结果表明，该算法的测速精度较传统 TDCP 算法在 2 种场景下的三维速度分别提升了32.23%、10.37%、50.39%和 10.20%、30.19%、31.54%，证明了该算法的有效性。关键词：北斗卫星导航系统；无人机导航；卫星测速；加权最小二乘；抗差卡尔曼滤波中图分类号：P228;V324;V249.3文献标志码：A文章编号：1673159X(2024)01001606doi：10.12198/j.issn.1673159X.5184AnAlgorithmofHigh-preciseSatelliteSpeedPredictionforUAVOperationsinLow-airSpaceJILi1,2，SUNRui1,2*（1.Key Laboratory of Maritime Intelligent Cyberspace Technology(Nanjing University of Aeronautics and Astronautics),Ministry ofEducation,Nanjing 211106 China;2.State Key Laboratory of Air Traffic Management System(Nanjing University ofAeronautics and Astronautics),Ministry of Education,Nanjing 211106 China）Abstract:Tosolvetheproblemofpooraccuracyofsatellite-basednavigationvelocityestimationcausedbymulti-patherrorwhenUAVsoperateinurbanlowaltitudeenvironment,thispaperpresentsanoptimizationvelocityestimationalgorithmbasedonsatelliteazimuthweightinganddopplerobservation.The algorithm combines the advantages of doppler and carrier phase observations,and constructs aweightedmodelbasedonsatelliteazimuthangleaccordingtothecharacteristicsofthelow-altitudeoperat-ingenvironmentoftheUAV,whicheffectivelyweakenstheinfluenceofmulti-patheffectonvelocityes-timationaccuracy,andimprovestherobustnessofvelocityestimationthroughtherobustKalmanfilter.Thealgorithmevaluationresultsbasedonthemeasureddatashowthatindifferentscenarios,thevelocityestim-ationaccuracyoftheproposedalgorithmisimprovedby32.23%,10.37%,50.39%and10.20%,30.19%,收稿日期：20230912基金项目：国家自然科学基金项目（42222401、42174025、41974033）；工信部专项科研项目（TC220A04A-79）；江苏省“六大人才高峰”项目（KTHY-014）；江苏省自然科学基金项目（BK20211569）；中央高校基本科研业务费专项资金项目（xcxjh20220726）。*通信作者：孙蕊（1984），女，教授，博士生导师，主要研究方向为导航定位及智能交通。ORCID：0000000322529944E-mail：引用格式：吉莉，孙蕊.面向低空无人机运行的高精度卫星测速算法J.西华大学学报（自然科学版），2024，43（1）：1621.JILi,SUNRui.AnAlgorithmofHigh-preciseSatelliteSpeedPredictionforUAVOperationsinLow-airSpaceJ.JournalofXihuaUniversity（NaturalScienceEdition）,2024,43（1）:1621.第 43卷第 1 期西华大学学报（自然科学版）2024年1月Vol.43，No.1JournalofXihuaUniversity（NaturalScienceEdition）Jan.202431.54%,respectivelyinthethree-dimensionalofvelocitydirectioncomparedundertwosceneswiththetra-ditionalepochdifferentialcarrierphasemetho,whichprovestheeffectivenessofthealgorithm.Keywords:Beidou satellite navigation system（BDS）；UAV navigation；satellite velocitydetermination；weightedleastsquares；robustKalmanfiltering我国北斗卫星导航系统（Beidousatellitenav-igationsystem,BDS）的建成带动了全球导航卫星系统（globalnavigationsatellitesystem,GNSS）的快速发展。星基导航技术逐渐发展并应用于无人机低空运行的各个方面。准确可靠的速度信息对无人机自主感知和状态控制发挥着十分重要的作用，然而由于无人机体积与重量的限制，搭载额外的传感器必然会增加成本，而低成本的 GNSS 测速则十分切合无人机的自主状态感知需求。GNSS 测速方法主要有多普勒法、历元间载波相位差分法（time-differencedcarrierphase,TDCP）以及历元间伪距差分法（time-differencedpseudorange,TDPR）12。由于载波相位精度较高，TDCP 方法自提出以来就得到了广泛研究，但在观测质量较差的环境下，载波相位观测值容易受到污染，从而严重影响测速精度。通过将 TDCP 观测值与其他传感器融合可以提供稳健的速度信息，例如：Han 等3将 GNSS 与惯性导航系统（inertialnavigationsystem,INS）组合以固定模糊度；Kim 等4将 GNSS/INS 组合进行周跳检测和修复。但以上方法需要额外的惯性器件，增加了成本以及计算负担。此外，Li 等5提出了基于卡尔曼滤波平滑的测速算法，但算法存在过于依赖运动模型、在高动态环境下可靠性不足的问题。针对上述问题，考虑城市低空无人机运行环境的实际特征，本文充分应用 TDCP 观测信息，提出了一种面向低空无人机运行的高精度卫星测速算法。该算法首先通过多普勒辅助的 TDCP 方法得到无人机运行的航向角，然后通过卫星方位角与无人机的航向角夹角构建加权模型并解算测速结果，最后采用抗差卡尔曼滤波以减小粗差的影响，从而提高了测速结果的鲁棒性。1研究与方法1.1算法框架基于卫星方位角加权的多普勒辅助 TDCP 测速算法如图 1 所示。GNSS 接收机GNSS 观测文件RD-TDCP基于 LSM的定位卫星位置卫星位置变化量接收机与卫星的方向向量基于 LSM 的接收机历元间位移 rR几何关系解算无人机航向角 基于方位夹角构造权矩阵 W基于 WLS 更新接收机历元间位移 rR由 rR 求解无人机速度 V基于抗差卡尔曼滤波的速度平滑处理最终测速结果载波相位、多普勒频移星历伪距图1算法框架Fig.1AlgorithmframeworkrR步骤 1，根据 GNSS 接收机接收到的观测文件和星历文件获取载波相位、多普勒频移观测值以及卫星位置，进一步得到卫星位置变化量以及接收机与卫星的方向向量，从而根据历元间载波相位差分方程解算历元间接收机位移。AzW步骤 2，根据接收机的历元间位移由几何关系估计无人机航向角，得到每颗卫星的方位角与无人机航向角 的夹角。基于卫星仰角、载噪比以及方位夹角构造权矩阵。V步骤 3，根据历元间载波相位差分方程基于加权最小二乘法求解无人机速度。步骤 4，基于抗差卡尔曼滤波对步骤 3 解算的无人机速度进行滤波平滑，得到最终测速结果。1.2多普勒辅助的 TDCP 解算位置增量载波相位观测模型可以表示为=1(rI+T)+c(dtRdtS)+N+（1）rc式中：为以周数表示的载波相位观测值；为载波的波长，为卫星与接收机之间的几何距离；为光第1期吉莉等:面向低空无人机运行的高精度卫星测速算法17ITdtRdtSN速；和 分别为电离层延迟和对流层延迟；和分别为接收机钟差和卫星钟差；为载波的整周模糊度；表示观测噪声和其他未建模误差。在较短的采样间隔内，上述误差近似不变，同时整周模糊度在载波跟踪环路保持锁定时也可保持不变，因此，通过对载波相位观测方程在历元间做差分可得=kk1=1r+cdtR+（2）rdtR式中：和分别表示载波相位和卫星与接收机间的几何距离在相邻历元间的变化量；表示接收机钟差漂移。考虑到卫星与接收机之间的几何距离远大于历元间接收机的位置变化量，因此，前后历元由卫星指向接收机的单位向量可近似相等，根据几何关系可得r=uk(XSkXRk)uk(XSk1XRk1)=uk(rSrR)（3）ukXSkXSk1kk1XRkXRk1rSrR式中：为卫星指向接收机的单位向量，和分别为历元 和卫星的位置；和分别表示对应历元接收机的位置；和则分别表示卫星和接收机在历元间的几何位置增量。将式（3）代入式（2）并改写成向量模式可得uk1rRcdtR=ukrS+（4）在开阔环境及静止环境中，载波相位观测值具有更高的精度，但当接收机处于更为复杂的环境中或高动态状态时，多普勒观测值具有更高的可靠性和稳健性；因此，用多普勒观测值辅助 TDCP 以充分发挥 2 种观测值的优势。载波相位差可以由多普勒频移的一阶中心差分来近似6，即 DkDk12t（5）DkDk1kk1t式中：和分别表示历元 和的多普勒频移；为采样间隔。将式（5）代入式（4）可得uk1rRcdtR=ukrS+DkDk12t+（6）rR当卫星数满足定位条件时，可根据式（6）解算接收机在地心地固坐标系（earth-centeredearth-fixed,ECEF）中的历元间位移。1.3基于位置增量的航向角估计当接收机采样率较高时，可认为其位移方向与rRENUrR无人机航向近似相同。接收机在东北天（east-north-up,ENU）坐标系下的历元间位移可由ECEF 坐标系中的旋转得到，为rRENU=RENUECEFrR（7）RENUECEF式中为由 ECEF 坐标系旋转至 ENU 坐标系的旋转矩阵。由几何关系得到无人机的航向角7，为=cos1yRN/(xRE)2+(yRN)2,xRE 02cos1yRN/(xRE)2+(yRN)2,xRE 9intC/N05,else（10）无人机在城市低空环境飞行时，其运动方向大致为城市道路沿街方向，当卫星处于沿街方向范围时，可认为其信号受到遮挡的可能性较小，当处于过街方向时则认为其信号更容易受到建筑物的遮挡和反射。因此，应对沿街方向的卫星赋予更高的18西华大学学报（自然科学版）2024年权值，而对过街方向的卫星降低权值。0,/2记卫星方位角与无人机航向角所在直线的夹角为，则 可表示为=|Az|,|Az|2|Az|,else（11）AzAz 0,2)式中为卫星的方位角，。m在式（9）的基础上，进一步加入由夹角 决定的缩放因子，构造考虑卫星方位角的加权模型为=m9Sa0+a1expElEl0（12）m缩放因子 由指数函数模型自定义，为m=exp20（13）0式中为参考夹角，取值为 90。卫星权重取值为测量误差标准差的倒数，为w=1（14）jW=diag(w1,w2,wj)当观测到 颗卫星时，可以构造权矩阵为。对所有的卫星联立式（6）可得矩阵形式，为ArR=b（15）则加权最小二乘解为rR=(ATWTWA)1ATWTWb（16）最终得出基于卫星方位角加权的无人机速度，为V=rR/t（17）1.5抗差卡尔曼滤波在观测环境较差或出现异常观测值时，速度观测值可能会出现较大偏差。由于无人机的速度存在变化的连续性，因此，本文使用抗差卡尔曼滤波对无人机速度进一步估计以得到更加稳健的速度估计值。输入卡尔曼滤波器的状态向量为=VxVyVzT（18）VxVyVz式中、分别表示滤波估计的无人机三维速度分量。以匀速运动模型构建的抗差卡尔曼滤波的递推公式为：k,k1=k1（19）Pk,k1=Pk1+Qk1（20）Resk=Zkk,k1（21）Kk=Pk,k1Pk,k1+Rkk1（22）k=k1+KkResk（23）Pk=IKkPk,k1IKkT+KkRkkKTk（24）k,k1k1Pk,k1Qk1ZkReskKkRkkkPkkkk式中：表示由递推的一步预测状态向量；为一步预测的状态协方差矩阵；为运动模型的误差方差矩阵；为观测向量，即上一步所得的无人机速度；为新息；为增益矩阵；为观测噪声矩阵；为历元 的状态估计向量；为的状态协方差矩阵；为抗差因子矩阵。对中的每一个元素，均有ik=1，?Resik?didi/?Resik?,?Resik?di（25）ikResikkReskididi式中：和分别表示和的第 个元素；表示常值矩阵 的第 个元素。2结果与分析为验证所提测速算法的性能，在南京市区道路进行了 2 组实验，分别为南北向和东西向飞行场景，参考轨迹分别如图 2 及图 3 所示。本次实验采用浙江中裕通信的 ZYACF-S806 天线与北斗星通的 C520-AT 接收机搭配进行 GNSS 观测，使用采集到的 BDS 数据进行实验验证。实验 1 的数据采集时长为 243s，采样频率为 10Hz，共计 2430 个历元；实验 2 采集时长为 550s，采样频率为 10Hz，共计5500 个历元。由霍尼韦尔的高精度组合导航系统HGuideN580 采集参考数据，并由商业软件 InertialExplorer 进行后处理解算获取速度参考值。终点起点图2实验 1 参考轨迹Fig.2Referencerackofonboardexperiment1第1期吉莉等:面向低空无人机运行的高精度卫星测速算法19终点起点图3实验 2 参考轨迹Fig.3Referencerackofonboardexperiment22 组实验期间的可见卫星数量分别如图 4 及图 5 所示，表明实验数据采集场景下的可见 BDS卫星数满足测速解算的条件。根据采集的 BDS 观测数据分别基于传统 TDCP 算法和本文算法进行速度解算。050100150200250时间/s910111213141516可见卫星数/颗图4实验 1 可见卫星数Fig.4Numberofvisiblesatellitesinexperiment10100200300400500600时间/s681012141618可见卫星数/颗图5实验 2 可见卫星数Fig.5Numberofvisiblesatellitesinexperiment2实验 1 在三维方向上的测速误差如图 6 所示，图中对比了传统 TDCP 算法和本文算法的测速性能。可知，本文算法的测速精度比传统 TDCP 算法更高，大部分误差峰值得到了抑制。本文通过测速均方根误差（rootmeansquareerror,RMSE）来表征测速精度。由表 1 的传统 TDCP 算法和本文算法的 RMSE 值可知，本文算法的三维 RMSE 分别为0.0480、0.1093 和 0.2170m/s，相对于传统 TDCP算法，其在东向、北向和天向上分别实现了 32.23%、10.37%、50.39%的提升。其中，东向的提升率高于北向，这是因为实验 1 的轨迹为由南向北，基于卫星方位角的加权算法抑制了垂直于轨迹方向上的误差，因此，东西方向能够实现更好的测速精度提升。0501001502002500.500.5传统 TDCP 算法本文算法050100150200250101050100时间/s150200250505东向误差/(ms1)北向误差/(ms1)天向误差/(ms1)图6实验 1 算法误差对比Fig.6Errorscomparisonofthealgorithmsinexperiment1表1实验 1 算法误差均方根对比Tab.1RMSEcomparisonofthealgorithms算法RMSE/m东向北向天向传统TDCP算法0.07090.12180.4374本文算法0.04800.10930.2170提升率/%32.2310.3150.39实验 2 在三维方向上的 RMSE 如图 7 所示，算法的 RMSE 以及提升率如表 2 所示。实验 2 的结果同样证明了本文算法可以实现测速精度的有效提升。其中，本文算法相对于传统 TDCP 算法在东向、北向、天向上的测速精度分别提升了 10.20%、30.19%和 31.54%。由于实验 2 的轨迹为东西方向，因此，本次实验在南北方向实现了更好的测速精度提升。结合 2 组实验结果可知，本文基于卫星方位角加权的多普勒辅助 TDCP 测速算法相对于传统20西华大学学报（自然科学版）2024年TDCP 算法能够在城市低空无人机运行环境中实现更高精度的无人机测速。3结论1）针对传统 TDCP 测速算法在城市低空无人机运行环境中存在测速精度不足的问题，利用TDCP 解算的无人机航向角信息，构建了一种面向低空无人机运行的高精度卫星测速算法，融合多普勒与载波相位观测值，并将基于卫星方位角的加权和抗差卡尔曼滤波应用于测速解算中，改进了传统TDCP 测速算法，从而提升了算法的精度和鲁棒性。2）实测实验结果表明，相较于传统 TDCP 测速算法，本文的基于卫星方位角加权的多普勒辅助TDCP 测速算法在不同实验场景下三维方向的测速误差分别降低了 32.23%、10.37%、50.39%和10.20%、30.19%和 31.54%，说明本文算法可以实现测速精度的明显提升。3）本文算法原理简单且成本低廉，仅需通过GNSS 接收机已有的数据即可实现速度信息的获取，对于城市低空无人机的自主感知具有较高的应用价值。本文算法也可应用于车载导航等其他领域。参考文献1FREDAP,ANGRISANOA,GAGLIONES,etal.Time-differenced carrier phases technique for preciseGNSSvelocityestimationJ.GPSSolutions,2015,19（2）:335341.2JIL,SUNR,CHENGQ,etal.Evaluationoftheperformance of GNSS-based velocity estimation algori-thmsJ.SatelliteNavigation,2022,3（1）:116.3HANHZ,WANGJ,WANGJL,etal.Perform-anceanalysisoncarrierphase-basedtightly-coupledGPS/BDS/INSintegrationinGNSSdegradedanddeniedenvir-onmentsJ.Sensors（Basel,Switzerland）,2015,15（4）:86858711.4KIMY,SONGJ,KEEC,etal.GPScycleslipdetec-tionconsideringsatellitegeometrybasedonTDCP/INSin-tegratednavigationJ.Sensors(Basel,Switzerland),2015,15（10）:2533625365.5LIFC,GAOJX,ZHENGNS,etal.Anoveldual-domainfilteringmethodtoimproveGNSSperform-ancebasedonadynamicmodelconstructedbyTDCPJ.IEEEAccess,2020,8:7971679723.6CANNONME,LACHAPELLEG,SZARMESMC,etal.DGPSkinematiccarrierphasesignalsimulationanalysisforprecisevelocityandpositiondeterminationJ.Navigation,1997,44（2）:231245.7SUNR,CHENGQ,WANGJH.Precisevehicledynamicheadingandpitchangleestimationusingtime-dif-ferenced measurements from a single GNSS antennaJ.GPSSolutions,2020,24（3）:84.8刘成,李芳.不同卫星定位加权方法的比较与分析J.测绘科学,2018,43（8）:3944.LIUC,LIF.ComparisonandanalysisofdifferentGNSS weighting methodsJ.Science of Surveying andMapping,2018,43（8）:3944.9 LUO X,MAYER M,HECK B.Improving thestochasticmodelofGNSSobservationsbymeansofSNR-basedweightingC/ObservingourChangingEarth.Ber-linHeidelberg：Springer,2009:725734.10刘若普,翟传润,战兴群.基于分段信噪比加权的 GPS 定位方法J.信息技术,2008,32（9）:1720.LIUR,ZHAIC,ZHANX.AmethodofGPSposi-tioningbasedonpiecewiseandweightedsignal-to-noisera-tioJ.InformationTechnology,2008,32（9）:1720.(下转第 27 页)表2实验 2 算法误差均方根对比Tab.2RMSEcomparisonofthealgorithmsinexperiment2算法RMSE/m东向北向天向传统TDCP算法0.08260.09350.1524本文算法0.07420.06530.1043提升率/%10.2030.1931.540200100300时间/s40050060020202001003004005006001010200100300400500600101传统 TDCP 算法本文算法东向误差/(ms1)北向误差/(ms1)天向误差/(ms1)图7实验 2 算法误差对比Fig.7Errorscomparisonofthealgorithmsinexperiment2第1期吉莉等:面向低空无人机运行的高精度卫星测速算法212）针对飞行测试中采集的稀疏离散数据难以准确评估场型的问题，提出了一种利用径向基函数神经网络进行三维场型反演重构的方法。经过仿真验证，三维反演重构后航道跟随误差的重构误差大约为 1.4 英尺，远小于 FAA8200 飞行校验规范手册规定的 20 英尺的容限要求，为 MLS 飞行校验中完整三维空间信号场型评估方法提供了新的思路。参考文献1李小强,王永超.一种双 VDB 台站 GBAS 系统的飞行校验方法J.电子测量技术,2021,44（23）:111116.LIXQ,WANGYC.AflightinspectionmethodforGBASdualVDBstationJ.ElectronicMeasurementTech-nology,2021,44（23）:111116.2陆永东,任龙昊,王明甲,等.仪表着陆系统设备飞行校验结果对比分析方法及系统:CN111824453BP.20220920.LUYD,RENLH,WANGMJ,etal.Instrumentlandingsystemequipmentflightverificationresultcontrast-ive analysis method and system:CN111824453BP.20220920.3YANGR,LVRL,MENGZY.Abnormaldetec-tionstrategyinflightinspectionofinstrumentlandingsys-temusingfuzzysetdecisionsC/2019IEEE1stInterna-tionalConferenceonCivilAviationSafetyandInformationTechnology(ICCASIT).Kunming:IEEE,2020:504508.4周其焕.微波着陆系统 M.北京:国防工业出版社,1992.ZHOUQH.MicrowavelandingsystemM.Beijing:NationalDefenseIndustryPress,1992.5Manualontestingofradionavigationaidsvolumeitesting of ground-based radio navigation systems：ICAODOC-8071S.USA,2000.6 United states stand-ard flight inspection manual：Faa-order82001dS.USA,2015.7王鼎,谢洪森,王传刚,等.着舰引导雷达动态校验系统的设计与实现J.电子设计工程,2021,29（8）:9095.WANGD,XIEHS,WANGCG,etal.Designandimplementationofdynamicinspectionsystemforlandingguidance radarJ.Electronic Design Engineering,2021,29（8）:9095.8王振宇,黄伟奇,孙健,等.插值算法在辐射场重构中的应用现状J.兵工自动化,2022,41（7）:2935.WANGZY,HUANGWQ,SUNJ,etal.Applica-tionstatusofinterpolationalgorithminradiationfieldre-constructionJ.Ordnance Industry Automation,2022,41（7）:2935.9ADNANR,SAMADAM,TAJJUDINM,etal.ModelingoffloodwaterlevelpredictionusingimprovedRBFNNstructureC/2015IEEEInternationalConferenceonControlSystem,ComputingandEngineering(ICCSCE).Penang,Malaysia：IEEE,2016:552556.10马腾.改进的径向基三维插值算法的研究与应用 D.成都:成都理工大学，2014.MAT.Researchandapplicationofimprovedradialbasis function 3D interpolation algorithmD.Chengdu:ChengduUniversityofTechnology,2014.11ZHOUPZ,JIAOKK,WANGZH,etal.AnoceancurrentpredictionmethodforprofilingfloatbasedonRBFNNC/OCEANS2022.HamptonRoads,VA,USA：IEEE,2022:14.（编校：夏书林）(上接第 21 页)11刘志强,黄张裕,金建平.利用卫星高度角和信噪比提高 GPS 定位精度的试验分析J.测绘工程,2008,17（4）:5458.LIUZ,HUANGZ,JINJ.Anexperimentstudyofim-provingGPSpositioningprecisionbysatelliteelevationandSNRJ.Engineering of Surveying and Mapping,2008,17（4）:5458.12GROVESPD,JIANGZY.Heightaiding,C/N0weightingandconsistencycheckingforGNSSNLOSandmultipathmitigationinurbanareasJ.TheJournalofNav-igation,2013,66（5）:653669.13GAOCF,WUF,WEIWR，etal.AnimprovedweightstochasticmodelinGPSprecisepointpositioningC/Proceedingsof2011InternationalConfer-enceonTransportation,Mechanical,andElectricalEngin-eering.Changchun:IEEE,2011：629632.14杨元喜,何海波,徐天河.论动态自适应滤波J.测绘学报,2001,30（4）:293298.YANGYX,HEHB,XUTH.Adaptiverobustfilter-ing for kinematic GPS positioningJ.Acta Geodaetica etCartographicaSinica,2001,30（4）:293298.（编校：饶莉）第1期罗潇等:基于三维反演重构的微波着陆系统飞行校验评估方法27