基于改进ESKF的植保无人机时延位姿补偿算法.pdf

第 45 卷 第 2 期2024 年 2 月仪器仪表学报Chinese Journal of Scientific InstrumentVol.45 No.2Feb.2024DOI:10.19650/ki.cjsi.J2312280收稿日期:2023-12-15 Received Date:2023-12-15基金项目:中国高校产学研创新基金-无人集群协同智能项目(2021ZYB02002)资助基于改进 ESKF 的植保无人机时延位姿补偿算法刘 慧,施志翔,沈亚运,储金城,沈 跃(江苏大学电气信息工程学院 镇江 212013)摘 要:为解决全球导航卫星系统和惯性测量单元融合时间不同步问题,提高植保无人机位姿估计精度,本文根据植保无人机大惯性、强振动的特性提出一种基于改进误差状态卡尔曼的时延位姿补偿算法。首先对名义状态变量线性预测,引入渐消因子提高强振动环境下的系统稳定性;接着采用互补滤波对角速度补偿,对姿态误差状态变量修正;最后结合测量的延迟时间,使用互补滤波外推数据,提高大惯性特性下的速度位置精度。实验结果表明,相较于误差状态卡尔曼算法,横滚角和俯仰角均方根误差减少 0.266 9和 0.241 4,偏航角均方根误差减少 0.076 4;正常航迹植保作业下,东北天方向速度均方根误差减少0.210 5、0.184 9、0.238 8 m/s;东北天方向位置均方根误差分别减少 0.21、0.19、0.23 m,有效提高位姿估计精度。关键词:植保无人机;误差状态卡尔曼滤波;延时补偿;信息融合;组合导航中图分类号:TH391.4 TH39 文献标识码:A 国家标准学科分类代码:520.60Time delay and attitude compensation algorithm for plant protection UAV based on the improved ESKFLiu Hui,Shi Zhixiang,Shen Yayun,Chu Jincheng,Shen Yue(School of Electrical and Information Engineering,Jiangsu University,Zhenjiang 212013,China)Abstract:To solve the problem of global navigation satellite systems and inertial measurement unit fusion time asynchronous and improve the accuracy of pose estimation of plant protection UAV,this article proposes a delay pose compensation algorithm based on the improved error state Kalman filter by using the characteristics of large inertia and strong vibration of plant protection UAV.Firstly,the nominal state variables are linearly predicted,and a fading factor is introduced to improve the system stability in strong vibration environments.Then,complementary filtering is used to compensate for diagonal velocity and correct the attitude error state variables.Finally,combined with the delay time measured,complementary filtering is used to extrapolate the data and improve the velocity and position accuracy under high inertia characteristics.Experimental results show that,compared with the error state Kalman filter algorithm,the root mean square error of roll angle and pitch angle is reduced by 0.266 9 and 0.241 4,and the root mean square error of yaw angle is reduced by 0.076 4.Under normal track plant protection operation,the root mean square error of velocity in the northeast sky direction values are decreased by 0.210 5,0.184 9,and 0.238 8 m/s.The root mean square errors of the northeast celestial position are reduced by 0.21,0.19,and 0.23 m,respectively.The algorithm effectively improves the accuracy of pose estimation.Keywords:plant protection UAV;error state Kalman filter;delay compensation;information fusion;integrated navigation0 引 言 随着我国农业航空技术的发展,植保无人机广泛应用在种料播撒1、病害检测2及农药喷洒3等环节,愈发需求高精度的位姿信息以完成复杂的作业任务。受制于成本、体积等因素,植保无人机上的惯性器件常采用微机电传感器,这类器件输出噪声大,误差模型具有非线性4,无法单独使用。因此,植保无人机一般采用全球导航卫星系统(global navigation satellite systems,GNSS)和惯性测量单元(inertial measurement unit,IMU)的组合导航方案5。相较于传统无人机,植保无人机由于负载过重具有大惯性、强振动6的特点。大惯性的特性需求更加精准的速316 仪 器 仪 表 学 报第 4 5 卷度和位置信息用于闭环控制,强振动的特性不利于姿态长时间解算。为获得更高精度的位姿信息,需保证融合信息的数据处于同一时刻。根据 GNSS 接受器设备的不同及处理器性能差异,通常一帧 GNSS 数据时延为 1595 ms7。在植保无人机爬升、加速等场景,时间异步所带来的误差会影响滤波器的精度,甚至导致滤波器发散8。针对这一问题,国内外学者对 GNSS 和 IMU 融合的时间延迟进行分析补偿9-11。宏观来说,补偿方法分为硬件方法和软件方法。硬件方法需不同传感器有相应的时间戳,或者传感器与处理器间有双向精准时间协议,如IEEE1588 协议12,但这势必会增加系统的制造成本和复杂程度。软件方法更加依赖于算法本身,基于量测对象的状态特征,设计相适应的时延补偿算法。卡尔曼滤波因其精度高、速度快等特点,在多传感器信息融合领域得到了广泛应用13-14。各国学者在卡尔曼的融合框架上,设计了不同的时延补偿算法。张涛等15提出一种在 FPGA 上检测 PPS 脉冲信号,从而中断时间同步的组合导航方案,其考虑了高中低 3 种动态情况的延时补偿,但低动态环境下提升不明显。章红平等16采用定时中断与数据缓存对 GNSS 和 IMU 的数据进行机械编排实现时间对准,可满足车载组合导航系统要求。Gamagedara 等17提出使用 IMU 的数据短时预测量测更新到当前时刻,采用误差状态卡尔曼(error state Kalman filter,ESKF)融合 GNSS 数据,计算效率得到提高,但实际使用时会因过度收敛导致滤波器精度下降。上述方法要缓存大量数据或通过 IMU 数据进行多步预测。若采用算法更大的处理器势必会增加无人机的飞行质量与制造成本18。相较于传统 ESKF,本文做了如下改进:针对植保无人机强振动的特性,引入渐消因子避免系统随时间推移整体协方差的过度收敛,对低动态场景姿态使用加速度计补偿;针对植保无人机大惯性的特性,在速度和位置上,采用互补滤波对时延信息进行外推融合补偿,获得当前时刻的伪量测,进而实现融合校准。本文阐述了 GNSS 和 IMU 融合算法与时延位姿补偿算法的实现过程,并通过仿真和实际飞行实验对算法进行验证,最后分析结果表明算法的有效性。1 IMU/GNSS 组合导航模型1.1 组合导航整体思路 本文使用改进的 ESKF 融合 IMU 和 GNSS 数据。如图 1 所示,为 IMU/GNSS 组合导航算法的基本框架。IMU数据判断低动态状态,低动态状态通过加速度计数据对角速度补偿,优化不考虑噪声的线性预测中的姿态求解;使用 GNSS 的姿态数据对姿态误差状态变量修正,实现姿态的最优估计;结合 IMU 的短时最优性,对相对延迟下的GNSS 数据修正,使用基于互补滤波补偿时间不同步误差,获得当前时间的伪量测;基于量测误差建立速度位置观测方程,实现对速度位置的误差状态变量的修正,最终归入名义状态变量,获得实际状态变量的最优估计。图 1 组合导航算法基本框架Fig.1 Basic framework of integrated navigation 第 2 期刘 慧 等:基于改进 ESKF 的植保无人机时延位姿补偿算法317 1.2 传感器模型建立 定义坐标系 b 为机体坐标系,原点位于机体重心;坐标系i 与机体固连且与安装位置有关,原点位于 GNSS 接收器中心,为 GNSS 传感器坐标系;坐标系 n 为本地东北天(ENU)坐标系,原点位于无人机的起始点。R 表示向量空间,任意的旋转矩阵 R R33可通过流形上的三维旋转正交群 SO(3)来描述:SO(3)=R R33|RTR=I33,detR=+1(1)将 IMU 测量的三轴加速度信息和角速度信息记为aIMUR3和IMUR3。对于低精度惯导系统,一般有确定性误差和随机游走。假设有高斯白噪声向量 a R3和 R3,有零偏向量 ba R3和 b R3。IMU 测量的加速度需通过旋转矩阵 Rnb R33将其转于 n 坐标系下,考虑到重力加速度 g R3,n 坐标系下的加速度v R3可表示为:v=Rnb(aIMU-ba+a)+g(2)在 b 坐标系下的真实角速度 l R3为:I=IMU-b+(3)如图 2 所示,IMU 和 GNSS 两类传感器时间滞后并不相同,GNSS 传感器的时间滞后较长。以 IMU 到达时序为基准,会产生一定的相对延迟时间为 Td。图 2 量测时序图Fig.2 Timing diagram of measurements假设 td时刻 GNSS 测量的位置、速度、姿态信息为xGNSS R3,vGNSS R3,RGNSS R33,和 IMU 数据一起到达中心处理器的时间为 tk,且满足 tk-1 td 1。引入观测噪声 x R3、v R3和 R R3,R表示 R的反对称矩阵,tk时刻的位置 x R3、速度 v R3、姿态R R33可表示为:x(k)=xGNSS(d)+x(4)v(k)=vGNSS(d)+v(5)R(k)=RGNSS(d)exp(R)(6)2 基于改进 ESKF 的位姿补偿算法2.1 状态变量预测 不失一般性,定义名义状态变量 X 和误差状态变量X。修饰的变量为名义状态变量XR3和实际状态变量 XlR18的误差量,其中 R3表示姿态误差向量,其反应姿态误差矩阵RR33局部的导数特性,是SO(3)的 R 对应的李代数向量。X=xvRnbbabgT(7)X=xvbabgT(8)XI=xIvIRIbIabIgIT(9)式中:x R3和 v R3表示坐标系 n 下的位置误差向量和速度误差向量;R3表示姿态误差向量,其反应姿态误差矩阵 R R33局部的导数特性,是 SO(3)的R 对应的李代数向量;ba R3表示加速度计的零偏误差向量;b R3表示陀螺仪的零偏误差向量;g R3表示重力的误差向量。实际状态变量 XI可通过不考虑噪声的名义状态变量和误差状态变量的广义加,姿态部分通过引入指数映射给定,表示 的反对称矩阵,具体为:xI=x+xvI=v+vRI=Rnbexp()bIa=ba+babI=b+bgI=g+g(10)建立误差状态变量 X 的状态方程19。X(k)=F(k-1)X(k-1)+W(k-1)Z(k)=H(k)X(k)+V(k)(11)式中:F(k-1)为线性系统模型,W(k-1)为系统噪声,Z(k)为量测数据,V(k)为观测噪声。名义状态变量 X 是直接使用 IMU 数据的线性预测,不考虑噪声项,记系统离散时间为 Ts,下一时刻估计的名义状态变量可以表示为:x(k)=x(k-1)+v(k-1)Ts+12(Rnb(k-1)(aIMU(k)-ba(k-1)+g(k-1)Ts2v(k)=v(k-1)+Rnb(k-1)(aIMU(k)-ba(k-1)+g(k-1)TsRnb(k)=Rnb(k-1)exp(IMU(k)-b(k-1)Ts)ba(k)=ba(k-1)b(k)=b(k-1)g(k)=g(k-1)(12)无人机位置误差 x 对时间的微分表示为:x=v(13)实际速度对时间的微分可展开如下形式:vI=RI(aIMU-bIa+a)+gI(14)对式(10)中的速度项两边求导展开,代入式(2),忽318 仪 器 仪 表 学 报第 4 5 卷略高阶小量,v 对时间的微分为:v=-Rnb(aIMU-ba)-Rnbba+a+g(15)无人机相邻时刻最优估计的姿态角误差极小,一般0 1。式(10)中的姿态项可近似为:R Rnb(16)两边求导,并通过刚体转动公式联立得:RnbI=RnbI+Rnb(17)两侧左乘逆矩阵 Rnb-1,移项并根据反对称矩阵性质,姿态误差向量的微分方程为:=-(IMU-b)-(18)考虑到陀螺仪和加速度计的零偏不稳定,采用一阶马尔科夫激励白噪声对 ba和 b建模。ba=-(a)-1ba+a(19)b=-()-1b+(20)式中:a和 是反时间相关常数矩阵,a和 是高斯白噪声矩阵。重力加速度误差 g 对时间的导数为:g=0(21)对式(13)、(15)、(18)(21)微分公式离散化,得线性化雅克比矩阵 F(k-1):F(k-1)=I33I33Ts033033033033033I33-Rnb(k-1)(aIMU(k)-ba(k-1)(k-1)Ts-Rnb(k-1)Ts033I33Ts033033-(IMU(k)-b(k-1)Ts033033033033033033I33-(a)-1033033033033033033I33-()-1033033033033033033I33(22)连续时间的噪声项可视为随机过程的能量谱密度,可使用标准差 表征随机变量。W(k)=0132(a)TIMU22(-)TIMU22(a)TIMU2()TIMU013T(k)(23)因误差状态变量 X 在一次更新后会被重置,对其预测意义不大,故误差部分是通过预测均方误差阵 P(k,k-1)指导整个滤波器的估计。P(k,k-1)=F(k)(P(k)FT(k)+W(k)(24)式中:是多重渐消因子矩阵对角阵,引入待定系数k(k 1),定义为:=diag(1,2,18)(25)常规卡尔曼滤波可认为待定系数等于单位阵的特殊情况。本质上,本文是一种强跟踪滤波器,即扩大了量测数据对状态估计的修正作用。强跟踪是从误差整体上的强跟踪,很难保证每一个状态分量都有良好的估计效果,因此需要补偿与校准保证最优性。2.2 姿态补偿与修正 据矩阵的链式公式,引入 GNSS 安装误差导致的坐标偏移矩阵 Rbi,可表示为:R=RIRbi(26)将式(6)和(10)中的 RI代入式(26),根据反对称矩阵的性质,写为:RGNSS(d)=Rnb(k)exp(k)-(RbiR)Rbi(27)式中:Rbi是一个固定矩阵,其反应了 GNSS 安装误差导致的坐标偏移。本文假设存在姿态向量 za R3,使得:za(k)=(k)-RbiR(28)根据泰勒公式,姿态向量 za又可近似为:za(k)RnbT(k)RGNSS(d)RbiT-I33(29)为保证上述近似值是一个反对称矩阵,故将其投影到反对称矩阵空间,并做反对称矩阵逆变换:za(k)=12(RnbT(k)RGNSS(d)RbiT-RbiRGNSS(d)TRnb(k)(30)式中:逆变换用 表示在 SO(3)上姿态矩阵 Rnb可通过式(12)迭代计算,据式(28)可构造误差姿态向量 的修正方程,并以GNSS 数据发送周期 TGNSS离散噪声项。za(k)=HRX(k)+VHR=036I33039VR=-Rbi2(R)TGNSS(31)接着,就可对误差状态变量 X 和预测均方误差阵P(k,k-1)修正,具体为:KR(k)=P(k,k-1)HR(HRP(k,k-1)HTR+VR)-1X+R(k)=KR(k)za(k)P+(k,k-1)=(I1818-KRHR)P(k,k-1)(32)修正完毕需将 X+R(k)归入名义状态变量 X+R(k)中,然后误差状态变量 X+R置0,完成1 次姿态修正。修正均方误差阵 P+(k,k-1)继续迭代。为进一步提高系统稳定性,在姿态修正前,本文设计了基于互补滤波的角速度补偿算法。其主要作用是低动态情况下对俯仰和横滚值的修正,以加速度计获取的实时姿态角来修正陀螺仪的累积误差,设定状态系数 coe。coe=abs aIMU-ba-g()(33)通过设定阈值判断有无进低动态状态。当处于低动 第 2 期刘 慧 等:基于改进 ESKF 的植保无人机时延位姿补偿算法319 态状态时,用单位向量外积表示误差为:error(k)=-Rbn(k-1)g(k)g(k)aIMU(k)-ba(k)aIMU(k)-ba(k)(34)使用误差和误差的积分去修正角速度,通过设定变量 KPR和 KIR补偿角度误差。对于误差积分的累计误差Interror,需在进入低动态状态一段时间才会发挥作用。退出低动态状态时,Interror默认归 0。Interror(k)=Interror(k-1)+KIRerror(k)+IMU(k)=IMU(k)+KPRerror(k)+Interror(k)(35)低动态情况下在 SO(3)上的姿态修正为:Rnb(k)=Rnb(k-1)exp(+IMU(k-1)-b)Ts(36)2.3 时延速度位置修正 GNSS 测量的位置、速度存在相对滞后时间 Td,这会导致和惯性传感器之间产生时间不同步误差,需对时间不同步误差估计和补偿。n 坐标系下 tk时刻可用 GNSS接受的相邻两帧数据推测位置x1和速度v1。x1(k)=x(k)+TdTGNSS(xk)-xk-1)(37)v1(k)=v(k)+TdTGNSS(v(k)-v(k-1)(38)根据捷联惯导算法中时间不同步误差补偿公式,n坐标系下 tk时刻位置x2、速度v2可通过不同步时间附近的平均加速度a 写成:x2(k)=x(k)+v(k)Td+12a(k)T2d(39)v2(k)=v(k)+a(k)Td(40)假设 IMU 输出周期为 TIMU,a 可通过惯导在两相邻时间内速度的平均变化来近似,本文使用tk-1到tk这段时间平均加速度近似。a(k)Rnb(k-1)(aIMU(k)-ba(k-1)+g(k-1)(41)IMU 积分获得的速度和位置短时间准确,一旦时间拉长存在发散,x2和v2的噪声具有低频特性;GNSS 长时间平均准确,短时间存在波动,x1和v1的噪声具有高频特性。本文使用 IMU 推算的位置x2和速度v2修正 GNSS推算的位置x1和速度v1。设计基于互补滤波的补偿算法,设定系数变量 KPx、KPv、KIx、KIv和累计误差 xInterror、vInterror。xInterror(k)=xInterror(k-1)+KIx(x1(k)-x2(k)x+(k)=x1(k)+KPx(x1(k)-x2(k)+xInterror(k)vInterror(k)=vInterror(k-1)+KIv(v1(k)-v2(k)v+(k)=v1(k)+KPv(v1(k)-v2(k)+vInterror(k)(42)在 tk时刻最优估计伪量测的 x+(k)和 v+(k),据式(12)中名义状态变量的预测值,可计算量测误差 zxv,代入ESKF 中进行量测更新,对于噪声阵 Vxv以 GNSS 数据发送周期 TGNSS离散噪声项 x和 v。zxv(k)=x+(k)-x(k)v+(k)-v(k)Tzxv(k)=HxvX+R(k)+VxvHxv=I330330312033I330312Vxv=2(x)TGPS0330332(v)TGPS(43)根据上一个步骤中的修正的均方误差阵 P+(k,k-1)和名义状态变量 X+R,计算新的增益矩阵 Kxv;预测新的误差状态变量 X+xv;更新均方误差阵 P(k)。Kx(k)=P+(k,k-1)Hxv(HxvP+(k,k-1)HTxv+Vxv)-1X+xv(k)=Kxv(k)zxv(k)P(k)=(I1818-KxvHxv)P+(k,k-1)(44)将 X+xv归入上一步骤修正的X+R中,实现对实际状态变量 XI的最优估计;误差状态变量 X+xv置 0,完成一次对速度和位置修正。3 实验与分析3.1 仿真实验 为验证改进 ESKF 算法有效性,进行仿真实验验证。如图 3 所示,仿真初始位置为东向 21 m,北向 0 m,天向0 m;各方向运动初始速度为 0 m/s;初始俯仰角、横滚角、偏航角都为 0;陀螺仪零偏 1/h,随机游走 0.1/(s h);加速度计零偏 20 mg,随机游走 0.2 mg/Hz;GNSS 位置精度 0.1 m,速度精度 0.02 m/s;为模拟植保无人机作业真实环境,GNSS 数据模拟相对延迟 20 ms;因植保无人机机身强振动,GNSS 输出的姿态角误差 4。图 3 仿真飞行轨迹图Fig.3 Simulated flight trajectory diagram为提高姿态估计精度和系统的稳定性,本文首先引入渐消因子,并与传统 ESKF 算法对比,性能参数如表 1所示。320 仪 器 仪 表 学 报第 4 5 卷表 1 姿态仿真绝对误差分析结果Table 1 Absolute error analysis results of attitude simulation欧拉角方法平均误差/()最大误差/()均方根误差/()俯仰角横滚角偏航角ESKF 算法0.966 13.921 90.626引入渐消因子0.926 63.015 20.195ESKF 算法1.185 50.832 00.424引入渐消因子1.061 60.507 00.185ESKF 算法0.827 62.791 00.997引入渐消因子0.740 41.703 00.594 渐消因子的引入可提高三轴姿态的跟随性能,一定程度减少了误差,并不能根本解决时延问题,速度和位置信息依然存在误差。因此本文在引入渐消因子的基础上,如图 4 和 5 所示,加入基于互补滤波的补偿环节,提高速度和位置的估计精度。图 4 东北天仿真速度对比Fig.4 Northeast day simulation speed comparison diagram图 5 东北天仿真位置对比Fig.5 Northeast day simulation positions comparison diagram改进后的东北天方向速度平均误差分别减少了0.09、0.07、0.002 m/s,东北天方向位置平均误差分别减少了 0.214、0.397、0.468 m。3.2 硬件平台 为了保证实验的安全性,如图 6 所示,本文使用上海中航欣盛航空技公司的 HF22-X80017 植保无人机为载体。植保无人机测量系统板的核心处理器使用 ST 公司的 STM32F407VET6 芯片。板上板载了博世公司的六轴IMU 测量单元 BMI088 和思南导航的 K823W 定位定向模块。GNSS 天线选用北天公司的 BT-560,使用有人互联网公司的 DR-154 超小型导轨式 Cat-1 模块作为差分数据传输模块。处理器通过 Microhard 公司的 P400 传输模块将数据传输给电脑。图 6 飞行实验平台Fig.6 Flight test platform为验证位姿估计的精确性,实验采用荷兰 XSENS 公司的 MTi-680 型惯性导航系统,外置思南导航公司的K823W 高精度定位定向 GNSS 模块,提供准确的姿态、速度、位置信息。其横滚、俯仰角精度可达 0.2,偏航精度达 1.0,速度精度达 0.05 m/s,位置精度 1 cm。在接收到 IMU 数据后,要对陀螺仪的数据进行均值滤波和对加速度计的数据低通滤波;在接收到 GNSS 数据后,要对 NEMA-0183 报文进行解析与处理。处理时间的不同会带来一定程度的时间误差,对位姿估计精度产生影响。如图 7 所示,本文提出一种纯软件的测量误差 第 2 期刘 慧 等:基于改进 ESKF 的植保无人机时延位姿补偿算法321 时间。使用定时器精准循环计数,一次循环时间为 1 s。每次在处理完 IMU 和 GNSS 的数据时,获得定时器的计数。使用最近一次 IMU 计数和 GNSS 计数作差,获取相对延时时间。图 7 时延误差程序流程Fig.7 Flow chart of the time delay difference procedure3.3 定点飞行实验 为验证引入渐消因子后的低动态姿态修正算法的可靠性,2024 年 1 月 18 日在 305543.8N,121157.9E进行1 min 42 s 定点飞行实验,以 MTi-680 型惯性导航系统提供的姿态信息作为参考值。本文采用 ESKF 算法和改进 ESKF 算法分别姿态估计,对比姿态估计精度。如图 8 所示。通过无线数传获得的数据进一步分析,实验数据如表 2 所示。与传统 ESKF 算法相比,俯仰和横滚角均方根误差减少了 0.266 9和 0.241 4,偏航角均方根误差减少了 0.076 4,并且平均误差减少了1.062 3。表 2 定点飞行姿态误差分析结果Table 2 The results of fixed-point flight attitude error analysis欧拉角方法平均误差/()最大误差/()均方根误差/()俯仰角横滚角偏航角ESKF 0.272 40.853 00.357 2改进 ESKF0.061 70.399 00.090 3ESKF0.341 91.136 40.436 0改进 ESKF0.133 40.982 00.194 6ESKF 1.170 71.138 70.254 0改进 ESKF0.108 41.092 20.177 6图 8 姿态估计对比Fig.8 Comparison of estimation of pose diagram3.4 航迹飞行实验 为验证本文提出的基于互补滤波的速度位置时延补偿环节,在相同场地进行3 min 58 s 的植保无人机规划航迹飞行实验,飞行航迹图如图 9 所示。软件测量的 IMU和 GNSS 相对延迟时间 48.6 ms。图 9 飞行航迹图Fig.9 Flight track diagram322 仪 器 仪 表 学 报第 4 5 卷采用改进 ESKF 算法和 ESKF 算法分别融合获得东北天三轴位置、速度,误差曲线如图 10 所示。如表 3 所示,以 MTi-680 的输出的东北天速度和位置作为参考值,东向的位置平均绝对误差减少了 0.138 4 m;天向的位置平均绝对误差减少了 0.109 6 m。整体上,速度均方根误差分别减少 0.210 5、0.184 9、0.238 8 m/s;东北天三向的位置均方根误差分别减少 0.21、0.19、0.23 m。本文提出方法整体优于 ESKF 算法,可有效减少相对时延带来的估计误差,提高位置速度的估计精度,更加有利于植保无人机的精准作业。图 10 速度位置估计误差对比Fig.10 Comparison of velocity position estimation errors diagram表 3 位置估计误差Table 3 Position estimation error方向方法误差范围/m平均绝对误差/m均方根误差/m东向北向天向ESKF-2.831.360.503 70.89改进 ESKF-0.310.570.365 30.68ESKF-1.461.460.491 40.63改进 ESKF-1.261.360.381 80.44ESKF-1.862.750.197 00.53改进 ESKF-1.362.750.186 70.304 结 论 为提高植保无人机的位姿估计精度,解决 IMU 和GNSS 的时间不同步问题,本文根据植保无人机特性针对性改进 ESKF,充分利用 IMU 和 GNSS 的信息,对时延信息进行外推融合补偿,提高植保无人机的位姿估计精度。为验证算法有效性,通过仿真验证算法有效性,设计搭载多种传感器的飞行实验平台,自主设计位姿融合板。进行实际飞行实验,验证本文提出的位姿补偿算法的有效性。实验结果表明,与传统 ESKF 算法相比,改进 ESKF算法有效提高位姿估计的精度,有利于植保作业的顺利完成。参考文献 1 周志艳,明锐,臧禹,等.中国农业航空发展现状及对策建议J.农业工程学报,2017,33(20):1-13.ZHOU ZH Y,MING R,ZANG Y,et al.Development status and countermeasures of agricultural aviation in China J.Transactions of Society of Agricultural Engineering in China,2017,33(20):1-13.2 ZHANG T,YANG Z,XU Z,et al.Wheat yellow rust severity detection by efficient DF-UNET and UAV multispectral imageryJ.IEEE Sensors Journal,2022,22(9):9057-9068.3 王大帅,LIU X G,李伟,等.基于多传感器融合的无人机精准自主飞行控制方法J.农业机械学报,2019,第 2 期刘 慧 等:基于改进 ESKF 的植保无人机时延位姿补偿算法323 50(12):98-106.WANG D SH,LIU X G,LI W,et al.UAV accurate autonomous flight control method based on multi-sensor fusion J.Transactions of Agricultural Machinery in China,2019,50(12):98-106.4 杨菊花,李文元,陈光武,等.引入滑模观测器的 GPS/INS 组合导航滤波方法J.仪器仪表学报,2019,40(9):78-86.YANG J H,LI W Y,CHEN G W,et al.GPS/INS integrated navigation filtering method with sliding mode observerJ.Chinese Journal of Scientific Instrument,2019,40(9):78-86.5 陈红梅,常林江,徐振方,等.复杂环境下 GNSS/INS/UWB 紧组合的无人机协同导航算法J.仪器仪表学报,2021,42(7):98-107.CHEN H M,CHANG L J,XU ZH F,et al.UAV collaborative navigation algorithm based on tight combination of GNSS/INS/UWB in complex environ-mentJ.Chinese Journal of Scientific Instrument,2021,42(7):98-107.6 吴和龙,白越,裴信彪,等.基于 CPF-EKF 算法的大载荷植保无人机姿态解算方法J.农业机械学报,2018,49(6):24-31,77.WU H L,BAI Y,PEI X B,et al.Attitude calculation method of large-load plant protection UAV based on CPF-EKF algorithm J.Transactions of Agricultural Machinery in China,2018,49(6):24-31,77.7 BEVLY D M,RYU J,GERDES J C.Integrating INS sensors with GPS measurements for continuous estimation of vehicle sideslip,roll,and tire cornering stiffnessJ.IEEE Transactions on Intelligent Transportation Systems,2006,7(4):483-493.8 HANSEN J M,FOSSEN T I,JOHANSEN T A.Nonlinear observer design for GNSS-aided inertial navigation systems with time-delayed GNSS measure-mentsJ.Control Engineering Practice,2017,60:39-50.9 刘红光,陈志刚,陈刚.基准信息时间延迟对速度匹配传递对准性能的影响分析J.中国惯性技术学报,2012,20(5):544-551.LIU H G,CHEN ZH G,CHEN G.Analysis of effect of time delay of reference information on performance of velocity matching transfer alignment J.Journal of Inertia Technology Transactions of Agricultural Machinery in China,2012,20(5):544-551.10 钱山,李鹏奎,张士峰,等.MIMU/GPS 组合导航建模及 GPS 时间延迟补偿算法研究J.系统工程与电子技术,2009,31(6):1432-1435.QIAN SH,LI P K,ZHANG SH F,et al.MIMU/GPS integrated navigation modeling and GPS time delay compensation algorithm J.Systems Engineering and Electronics Transactions of Agricultural Machinery in China,2009,31(6):1432-1435.11 杜瑾,李杰,罗丹瑶,等.高动态 GPS/INS 组合导航中时间延迟软硬件补偿算法研究J.传感技术学报