1、2023年月期第54卷第农报学业机械doi:10.6041/j.issn.1000-1298.2023.07.002基于低成本惯导和运动约束的农机高精度定位系统研究袁洪良！杨浚宇1唐睿？杜建伟（1.同济大学电子与信息工程学院，上海2 0 18 0 4；2.中移（成都）产业研究院，成都6 10 0 41）摘要：北斗定位系统（BDS）能够实现厘米级的定位，在农机导航系统中获得了广泛应用。然而，单纯的卫星定位系统存在一些局限性，比如易受外界遮挡、数据频率低等。因此，在导航系统中集成惯性元件并发展组合导航技术成为重要研究方向。考虑到高性能惯导成本很高，不利于推广应用，本文研究了农业场景下，使用低成本惯
2、导和BDS构成组合导航系统。为提高定位精度，解决由BDS中断导致误差发散的问题，设计了零速修正。同时，分析了BDS/INS组合导航系统中航向角误差不可观测的原因和误差来源，设计了航向约束方法。当BDS信息可用时，利用双天线航向角信息来抑制航向角误差的累积；当BDS中断时，使用零角速度修正。通过现场实验验证，当BDS可用时，零速修正可以将位置、速度和水平姿态的精度分别提高2 0%、40%和15%以上，航向约束可以将航向角精度提高90%以上；BDS中断时，零速修正可以将位置、速度的精度提高90%以上，水平姿态的精度提高8 0%以上，航向约束可以将航向角精度提高40%以上。关键词：农业机械；自主导航
3、；BDS/INS；扩展卡尔曼滤波；ZUPT；航向约束中图分类号：S24文献标识码：A文章编号：10 0 0-12 98（2 0 2 3)0 7-0 0 17-0 9OSID:High-precision Localization of Autonomous Agricultural MachineryUsing Low-cost IMU and Motion ConstraintsYUAN HongliangYANG JunyuTANG Rui?DU Jianwei?.2(1.College of Electronics and Information Engineering,Tongji U
4、niversity,Shanghai 201804,China2.China Mobile Chengdu Institute of Research and Development,Chengdu 610041,China)Abstract:The Beidou positioning system(BDS)can achieve centimeter level positioning,and has beenwidely used in agricultural machinery navigation systems.However,a simple satellite positio
5、ning systemhas some limitations,such as being vulnerable to external occlusion and low data frequency.Therefore,integrating inertial components and developing integrated navigation technology in navigation systems is animportant trend.Considering the high cost of high-performance inertial navigation
6、,which is not conduciveto promotion and application,the use of low-cost inertial navigation and BDS was studied to constitute anintegrated navigation system in agricultural scenarios.In order to improve positioning accuracy and solvethe problem of error divergence caused by BDS interruption,zero spe
7、ed correction was designed.At thesame time,the causes and sources of unobservable heading angle errors in the BDS/INS integratednavigation system were analyzed,and a heading constraint method was designed.When BDS informationwas available,dual antenna heading angle information was used to suppress t
8、he accumulation of headingangle errors.When BDS was interrupted,zero angular velocity correction was used.Field experimentsverified that when BDS was available,zero speed correction can improve the accuracy of position,velocity,and horizontal attitude by more than 20%,40%,and 15%,respectively.Headin
9、g constraintscan improve the accuracy of heading angle by more than 90%.When BDS was interrupted,zero speedcorrection can improve the accuracy of position and speed by more than 90%,horizontal attitudeaccuracy by more than 80%,and heading constraint can improve the accuracy of heading angle by moret
10、han 40%.Key words:agricultural machinery;autonomous navigation;BDS/INS;extended Kalman filter;ZUPT;heading constraint收稿日期：2 0 2 2-11-2 3修回日期：2 0 2 3-0 3-0 1基金项目：教育部-中国移动科研基金项目（MCM2020一J-2）作者简介：袁洪良（197 8），男，副教授，博士，主要从事农机自动导航等研究，E-mail：h y u a n t o n g j i.e d u.c n18农2023年机报业学械0引言自主导航农机是实现智能农业的核心技术装
11、备，可以全程参与耕、种、管、收环节的生产活动，达成提高作业效率、作业质量、土地利用率和降低劳动力成本等目标1-5。这些自主生产作业活动需要较高的定位精度支持，通常在厘米级。全球导航卫星系统（GNSS）6-8 可以提供准确的绝对定位信息，但具有低频率和易受环境影响的缺点。相比之下，微机电惯性导航系统（MEMSINS）可以在不受外部环境干扰的情况下稳定地提供相对定位信息19-10)，但具有误差累积的局限性。GNSS/INS组合导航系统(-1 可以通过扩展卡尔曼滤波器（KF）有效地融合来自每个子系统的数据，提供稳定的高精度定位信息。因此，越来越多的GNSS/INS组合系统正在取代GNSS导航系统，成
12、为农业机械自主导航系统的主要组成部分。然而，高性能的IMU成本非常高，影响自主导航农机推广，因此有必要研究基于低成本IMU的GNSS/INS组合导航系统，适应农业场景需求。但是，GNSS/INS组合导航系统应用在农机上,存在一些技术问题。比如，大部分农机没有悬架系统，当农机作业时，必然存在较大的车体振动，由此导致GNSS测量精度下降，方差增大，估计误差增加14，从而影响定位和控制效果。其次，农机在农田作业时，也有可能会遇到卫星信号遮挡、干扰或收星数不足等问题，导致GNSS短暂中断，GNSS/INS组合导航系统将退化为纯惯性导航系统15-18，导航误差会随着时间的推移迅速增长，可能导致农机失控，
13、造成经济损失。最后，GNSS/INS组合导航系统在低速或零速时存在航向角误差不可观测的问题19-2 1，导致航向角随着时间发散。许多研究中使用神经网络2 2 或者增加外部传感器2 3-2 41 来来解决GNSS中断时定位误差发散的问题，但是对于农机，使用高算力的人工智能芯片或者增加外部传感器会增加系统的成本和复杂度，不利于在农业应用中大范围推广本文以东风DF10042 型拖拉机为实验平台，设计BDS/INS组合导航系统。同时，为了解决BDS信号中断时的定位误差发散和组合导航系统中航向角误差累积问题，设计零速修正（ZUPT）和航向约束算法。该方法将在不增加其他外部传感器以及系统成本的基础上，解决
14、当前农机组合导航系统中的固有问题，以利于系统在农业中应用推广。1BDS/INS组合导航系统设计与实现1.1硬件架构设计硬件架构设计如图1所示，包括流动站和基准站。基准站接收来自北斗卫星的导航信号，并解析RTK校正数据，通过433MHz无线电台发送到安装在农机上的流动站。流动站接收来自北斗卫星的导航信号，同时应用来自基准站的RTK数据解算出高精度定位信息。自动驾驶控制器完成INS的机械编排，并通过扩展卡尔曼滤波（EKF）得出组合定位结果电台?.电台BDS天线BDS天线传输天线接收天线BDS定位板卡电台电台BDS定位板卡农机自动INS驾驶处理器基准站处理器导航信息基准站车载流动站图1组合导航系统架
15、构Fig.1Architecture of combined navigation system本研究MEMS-IMU模块为IMU383ZA-400，北斗定位模块为UB482，如图2 所示OACBINNA图2MEMS-IMU和GNSS模块Fig.2MEMS-IMU and GNSS modules1.2组合算法设计BDS/INS组合导航系统算法分为两部分：INS机械编排和基于EKF的组合导航算法。导航坐标系定义为北东地坐标系,用n表示；农机坐标系用b表示；地心地固坐标系用e表示；惯性坐标系用i表示。机械编排在n系下进行，具体过程如图3所示。MEMS-IMU输出的比力和角速度经过误差补偿后，通过
16、坐标转换矩阵，转化到n系下。其中，比力经过比力方程计算后可以得到对应的加速度，继续对其进行积分，即可获得由INS推算出的位置和速度信息。角速度则通过姿态计算微分方程对坐标转换矩阵进行更新，从而根据其与姿态角之间的对应关系得出姿态信息。对应的微分方程为diag(eTabbggdiag(三.bCAt133aa3X3+?enXt33PabatXX3Xt333d=diag一gtgdt2gtR+hR+hR+hg4tgt2gtX3Xt3300006gg0000dk.k-(5)0abaa000va000其中X(4）+W19袁洪良等：基于低成本惯导和运动约束的农机高精度定位系统研究第7 期位置ig=Cjfb-
17、(20g+20mxrg+g)ebbAccel速度MEMS-ofCIMU=C(ox)GyroenieCg=Cg(x)-(0 x)C动姿态C图3机械编排Fig.3INSmechanizationCfi-(20+w)v+giCC;(%)(1)CT(o)-()C)-VD式中农机相对于e系的速度INS输出的比力矢量bWib-INS输出的角速度矢量g当地重力加速度矢量C-b系到n系的坐标转换矩阵n系到e系的坐标转换矩阵nnn系相对于e系的旋转矢量en-n系相对于i系的旋转矢量nh农机高度VD农机垂向速度选取组合导航系统的维状太向量为=888.VV.VIT(2)式中L、入、h纬度、经度、高度误差一三轴速度误
18、差一三轴姿态误差8x18y182-三轴陀螺仪零偏VV、V一三轴加速度计零偏基于INS的误差模型2 5 在n系下可以表示为8in=-WnSr+Ovi=g-(20+w)v+f+C,afi=-(w+wn)xy-Cowb(3)式中一位置、速度和姿态误差一8g-重力加速度误差加速度计测量的比力误差陀螺仪测量的角速度误差将陀螺仪和加速度计误差建模为一阶马尔可夫过程，因此，离散EKF系统模型可以表示为式中k.k-1状态转移矩阵Wk-1过程噪声13x3-33单位矩阵At每个历元的时间间隔R-平均曲率半径TebT.-陀螺仪和加速度计的相关时间系统测量方程可以表示为Z:=Hx+V(6)其中ri-r0001133Z
19、=H=Lv-v013x30001n式中Z、H系统观测量和观测矩阵-INS推算的位置与速度BDS测量的位置与速度V一测量噪声向量EKF的更新方程为BDS给出的时刻的纬度P估计误差协方差矩阵Q测量噪声协方差矩阵农2023年20机报学械业PK,=Pvk-IH(HPu/k-HT+R)-I(7)XM=X/k-I+Kk(Z,-Hx/k-1)(Pv/k=(I-K,H)Pk/k-1式中K一一卡尔曼滤波增益P初始值由实地测试给出，即P。=d i a g(0.52,0.52,0.52,0.52,0.52,0.52,0.0 8 7,0.0872,0.174,0.000242,0.000242,0.000242,0.
20、012,0.012,0.012)因此，整个BDS/INS组合导航系统框图如图4所示。通过INS机械编排获得三轴位置和速度信息之后，将INS与BDS定位结果间的差值作为位置误差、速度误差的观测值，利用EKF估计定位误差，反馈校正定位结果。fh定位结果MEMS-IMU机械编排估计误差BDSEKF图4BDS/INS组合导航系统框图Fig.4Block diagram of BDS/INS combined navigation system1.3零速修正和航向约束算法设计(1)零速判断对于ZUPT和航向约束两种算法，判断农机是否处于静止状态是十分重要的问题。这里定义一个新的变量，为(Tlat-rla
21、t)(Rm+Th)(Tlon-rlon)(R,+rh)6,=一(8)式中16,1相邻两个历元之间的位置差值经度和高度BDS给出的上一时刻的纬度、经度和高度R、R,一子午圈和卯酉圈的曲率半径当18,1小于设置的阈值，则判定农机处于静止状态并使用ZUPT和航向约束算法(2)ZUPT算法设计当农机静止时，其3个方向的实际速度为零。然而，来自GNSS的速度信息存在随机误差。ZUPT通过将包含随机误差的速度信息替换为零，提高了速度观测值的精度，从而提高了组合导航系统的定位精度。此外，当GNSS中断时,ZUPT不会受到影响，可用于在此期间保持定位精度。基于这一原理，本文针对BDS信号可用和中断两种情况，设
22、计了ZUPT算法，并通过改变式（6）中的速度误差观测量来实现首先，当BDS信号可用时，将BDS给出的速度信息替换为0。因此，观测量和观测方程可以改写为ri-ri00 0013X3Z=V-01x300X+V(9)其次，当BDS中断时，组合导航系统会缺失来自BDS的位置、速度信息。因此，观测量中没有位置误差，但ZUPT可以提供速度误差观测值，此时系统的观测量和观测方程可以改写为Z:=v,-0=0133000 x+V(10)（3）航向角误差不可观测的原因分析低成本MEMSIM U，通常存在明显的噪声。此外，农机作业时也会产生车体振动，从而影响IMU的输出。与这些外部干扰相比，地球的自转速度和重力加速
23、度误差很小。因此，在IMU的输出中可以忽略不计，式（3）中的INS误差方程可以简化为oi=fxW+C,afh=0-f1800+CAFb(11)-J0式中横滚角、俯仰角和航向角误差由式（11）可以得出，航向角误差只能通过前向和右向的比力来观测。然而，农机通常处于静止或匀速工作状态，此时农机前向和右向的比力均为0，因此航向角误差不可观测。这导致了航向角会随时间发散。与之相比，横滚角和俯仰角可以通过垂向比力来观测。（4）航向约束算法设计在本文所设计的系统中,BDS选用双天线结构（D A），因此测量出的偏航角具有较高的精度，当BDS信号可用时，将BDS信息中给出的偏航角与INS推算出的航向角之间的差值
24、作为航向角误差的观测值，可以有效地抑制航向角误差的累积。因此，将式（9)中的测量方程扩展一维，系统的测量方程可以改写为r-rBZ=V,-VB=Hx+V=13x30000701,x3000X+V(12)00E0差方程可以改写为按照式（11）的处理方式中的姿态角误另外的航向约束算法法便失去了作用。因此，有必要针对此种情况设计信号的可用。在系统中断导致信号不可用时，该方然而，式（12）中利用DA的方法需要确保BDS21袁洪良等：基于低成本惯导和运动约束的农机高精度定位系统研究第7 期其中式中中-INS推算出的航向角BDS信息给出的偏航角(13)n系下，C,可以表示为ccd-cesd+sospcdsa
25、sb+cscdCH=csdcocd+sospsd-scd+cespsd-s4cOc(14)式中co、c o、c 中、s、s o 和s中表示cosp、c o s o、c o s 中、sinp、s in g 和sind将式（14）代人式（13）中，则航向角误差方程可以表示为-cocpowib(15)由式（15）可以得出，当农机静止在农田中时，横滚角和俯仰角接近于零，因此航向角误差主要来源为陀螺仪z轴测量的角速度误差。因此，可通过减小z轴角速度误差来抑制航向角误差的发散。当农机处于静止状态时，z轴角速度的真实值为0,因此可以将0 与陀螺仪z轴角速度的差值作为误差观测值。基于此，设计了零角速度修正算法
26、(ZARU）,与DA组合使用。同样将式（10）中的测量方程扩展一维，可以得到此时的系统测量方程为”-0Z.BDS_loss=Hx;+V=bWib,013x30001X+V(16)00E综上所述，整个系统的示意图如图5和图6 所示。当BDS信号可用时，由ZUPT和DA辅助的BDS/INS组合导航系统的框图如图5所示。INS推算出的航向角与BDS信息给出的航向角之间的差定位MEMS-IMU机械编排估计结果BN误差十BDSEKF静止？YEKF(ZUPT+DA)图5BDS/INS/ZUPT/DA组合导航系统框图Fig.5BDS/INS combined navigation system aidedb
27、y ZUPT and DAE=001值作为航向角误差的观测量增加到系统中。b定位MEMS-IMU机械编排b估计结果误差N静止？YEKF(ZUPT+ZARU)图6BDS/INS/ZUPT/ZARU组合导航系统框图Fig.6BDS/INS combined navigation system aidedby ZUPT and ZARU当BDS中断时，由ZUPT和ZARU辅助的BDS/INS组合导航系统框图如图6 所示。与BDS信号可用时不同，这里没有位置误差和航向角误差的观测值，但增加了z轴角速度误差作为观测量。2实验实验平台采用东风DF10042 型拖拉机搭建。使用第1节中的架构，设计并制作了基
28、准站和流动站的印刷电路板（PCB）。基准站放置在没有遮蔽的开放区域，以提供连续的RTK数据。流动站安装在拖拉机的驾驶舱内。BDS双天线水平放置在车顶上，基线长度为1.2 m。电台天线安装在左后轮挡泥板上。实验平台构成和测试现场如图7 所示BDS双天线接收电台天线基准站流动站图7农机实验平台和测试现场Fig.7Agricultural machinery experimental platformand testing site本文提出的算法在农机实验平台上进行测试。测试分为两部分：测试A是验证当BDS信号可用时,ZUPT和DA航向约束对定位精度的改善；测试B是验证当BDS信号中断时ZUPT和Z
29、ARU航向约束对定位精度的改善。两组测试实验中，在检测到农机静止后,ZUPT和航向约束每隔0.5s应用一次。2.1测试A当BDS信号可用时，将农机静止于稻田中，通农222023年机报学业械过BDS/INS、BD S/I NS/ZU PT 以及BDS/INS/ZUPT/DA获得的定位误差如图8 10 所示。表1中给出了定位误差的最大值和均方根误差。-BDS/INS-BDS/INS/ZUPTBDS/INSIZUPT/DA(r-s.w)/F0.02-0.02020406080100120140160180时间/sBDS/INSBDS/INS/ZUPTBDS/INSIZUPT/DA(is.w)0.02
30、山0-0.02-0.04020406080100120140160180时间/s0.05BDS/INSBDS/INS/ZUPTBDS/INSIZUPT/DA(-s.W)/回垂-0.05020406080100120140160180时间/s图8BDS可用时的速度误差Fig.8Velocity errorwhen BDS was availableBDS/INS一-BDS/INSIZUPTBDS/INS/ZUPT/DA0.02-0.02020406080100120 140 160180时间/s0.01BDS/INSBDS/INS/ZUPTBDS/INS/ZUPT/DA/回0东-0.01-0.0
31、2-020406080100120140160180时间0.05BDS/INSBDS/INS/ZUPTBDS/INS/ZUPT/DA0.05020406080100120 140160 180时间/s图9BDS可用时位置误差Fig.9Position error when BDS was availableBDS/INS-BDS/INS/ZUPT(。)/-BDS/INS/ZUPT/DA0.20.10020406080100120140 160180时间/s0.1BDS/INS-BDS/INS/ZUPTBDS/INS/ZUPT/DA0-0.1020406080100120140160180时间/
32、s-BDS/INS-BDS/INS/ZUPTBDS/INS/ZUPT/DA105020406080100120 140160180时间/s图10BDS可用时姿态误差Fig.10Attitude error when BDS was available如图8 和表1所示,BDS/INS组合导航系统在3个方向的最大速度误差分别为0.0 2 8、0.0 44、0.052m/s，相应的均方根误差分别为0.0 10、0.0 13、表1BDS信号正常时速度误差的最大值和均方根误差Tab.1Maximum and RMSE of velocity error whenBDSwasavailablem/s定位
33、算法评价指标北向东向垂向最大值0.0280.0440.052BDS/INS均方根误差0.0100.0130.016最大值0.0220.0330.039BDS/INS/ZUPT均方根误差0.0070.0100.012最大值0.0250.0230.039BDS/INS/ZUPT/DA均方根误差0.0070.0080.0130.016m/s。应用ZUPT后，3个方向上的最大速度误差分别降至0.0 2 2、0.0 33、0.0 39m/s，相应的均方根误差也变得更小，分别为0.0 0 7、0.0 10、0.0 12 m/s。准确度分别提高30%、2 3%和2 5%。而BDS/INS/ZUPT/DA与B
34、DS/INS/ZUPT相比，其对速度误差的影响较小。如图9和表2 所示，对于BDS/INS,北向和东向的最大位置误差分别为0.0 2 8 m和0.0 15m，最大垂向误差为0.0 48 m。3个方向的均方根误差分别为0.009、0.0 0 5、0.0 17 m。应用ZUPT之后，北向和东向的误差分别减小到0.0 12 m和0.0 10 m。垂直方向的最大误差减小到0.0 17 m。3个方向的均方根误差分别为0.0 0 5、0.0 0 3、0.0 0 6 m，精度分别提高44%、40%和6 5%。与速度误差的改善结果相同，增加DA不会显著影响位置误差。表2BDS信号正常时位置误差的最大值和均方根
35、误差Tab.2Maximum and RMSE of position error whenBDSwasavailablem定位算法评价指标北向东向垂向最大值0.0280.0150.048BDS/INS均方根误差0.0090.0050.017最大值0.0120.0100.017BDS/INS/ZUPT均方根误差0.0050.0030.006最大值0.0080.0080.016BDS/INS/ZUPT/DA均方根误差0.0030.0030.008如图10 和表3所示，BDS/INS组合导航系统可以提供准确的水平姿态角信息。最大横滚角误差为0.265，对应的均方根误差为0.0 6 9；最大俯仰角误
36、差为0.0 7 8，相应的均方根误差为0.0 30，航向角误差在18 0 s内累积到13.58 2。应用ZUPT后，横滚角和俯仰角的精度得到了提高。其中，横滚角的最大误差减小到0.2 0 0，均方根误差减小到0.034精度提高51%。俯仰角的最大误差减小到0.016，均方根误差减小到0.0 2 0，精度提高33%。此外,增加使用ZUPT后，对航向角精度的改善不大。应用DA之后，BDS/INS/ZUPT/DA将横滚角的最大误差减小到0.130，均方根误差减小到23袁洪良等：基于低成本惯导和运动约束的农机高精度定位系统研究第7 期0.024，精度提高2 9%。最大俯仰角误差减小到0.010，均方根
37、误差减小到0.0 17，精度提高15%。航向角最大误差为0.0 12，均方根误差为0.0 41，有效抑制了航向角误差的累积，解决了在静止时航向角误差发散的问题表3BDS可用时姿态误差的最大值和均方根误差Tab.3Maximum and RMSE of attitude errorwhenBDSwasavailable（）定位算法评价指标横滚角俯仰角航向角最大值0.2650.07813.582BDS/INS均方根误差0.0690.0304.376最大值0.2000.0169.350BDS/INS/ZUPT均方根误差0.0340.0203.145最大值0.1300.0100.012BDS/INS/
38、ZUPT/DA均方根误差0.0240.0170.041综上所述，BDS/INS组合导航系统可以稳定地为农机的自主驾驶提供定位信息。ZUPT通过提高速度误差观测值的准确性提高了速度的精度，减小了速度误差。同时，由于位置是通过速度积分获得的，ZUPT改善了速度误差，因此位置误差也会随之改善。然而，由于航向角误差不可观测，而ZUPT并不能提高航向角误差的可观测度，因此不能抑制航向角误差的发散。DA通过提供BDS的偏航角提高了航向角误差的可观测性，从而抑制了组合导航系统中航向角的发散。同时，由于姿态误差之间的耦合，横滚角和俯仰角的精度也得到了提高。2.2测试B在本节的测试中，农机仍静止在稻田中。BDS
39、/INS组合导航系统正常运行2 s后，人为地屏蔽BDS信号6 0 s，以模拟BDS信号不可用的情况。通过BDS/INS、BD S/I NS/ZU PT 和BDS/INS/ZUPT/ZARU获得的速度误差、位置误差和姿态误差如图11 13所示（图中未完全展示）。BDS信号中断的时刻在BDS/INSBDS/INSIZUPTBDS/INS/ZUPT/ZARU(-s.U)/F0-0.5-1.030401010205060时间/sBDS/INSBDS/INS/ZUPTBDS/INS/ZUPT/ZARU1.00.500102030405060时间sBDS/INS-BDS/INS/ZUPTBDS/INS/Z
40、UPT/ZARU(-s.u)/回垂1.00.500102030405060时间/s图11BDS不可用时速度误差Fig.11Velocity error when BDS was unavailable图中用黑色箭头标记。速度误差、位置误差和姿态误差的最大值和均方根误差见表4 6。-BDS/INSBDS/INSIZUPTBDS/INS/ZUPT/ZARU0-1102030405060时间/-BDS/INS-BDS/INSIZUPTBDS/INS/ZUPT/ZARU200102030405060时间/sBDS/INS-BDS/INSIZUPTBDS/INSIZUPT/ZARU2U/回垂100102
41、030405060时间s图12BDS不可用时位置误差Fig.12Position error when BDS was unavailable-BDS/INSBDS/INS/ZUPTBDS/INS/ZUPT/ZARU200102030405060时间/s1.5BDS/INSBDS/INS/ZUPTBDS/INS/ZUPTIZARU(。1.00.50-0.50102030405060时间/sBDS/INSBDS/INS/ZUPTBDS/INS/ZUPT/ZARU40102030405060时间/s图13BDS不可用时姿态误差Fig.13Attitude error when BDS was un
42、available表4BDS不可用时速度误差的最大值和均方根误差Tab.4Maximum and RMSE of velocity errorwhen BDS was unavailablem/s定位算法评价指标北向东向垂向最大值11.61220.7241.307BDS/INS均方根误差3.4556.2250.359最大值0.0670.0760.053BDS/INS/ZUPT均方根误差0.0210.0130.022最大值0.0840.0620.063BDS/INS/ZUPT/ZARU均方根误差0.0240.0130.023如图11和表4所示，在BDS中断的6 0 s内，速度误差迅速发散，3个方
43、向的最大误差分别为11.612、2 0.7 2 4、1.30 7 m/s。相应的均方根误差为3.455、6.2 2 5、0.359m/s，在实际生产中，这样的定位结果显然是不可用的。应用ZUPT后，速度误差不再发散，3个方向上的误差保持了与BDS信号可用时类似的精度，最大误差分别为0.0 6 7、0.0 7 6、农242023年机报业学械表5BDS不可用时位置误差的最大值和均方根误差Tab.5Maximum and RMSE of position errorwhen BDS was unavailablem定位算法评价指标北向东向垂向最大值195.834332.67221.741BDS/IN
44、S均方根误差56.63995.5746.205最大值0.0390.0390.057BDS/INS/ZUPT均方根误差0.0140.0180.018最大值0.0570.0510.056BDS/INS/ZUPT/ZARU均方根误差0.0180.0140.018表6BDS不可用时姿态误差的最大值和均方根误差Tab.6Maximum and RMSE of attitude errorwhen BDSwas unavailable(）定位算法评价指标横滚角俯仰角航向角最大值4.9721.3654.685BDS/INS均方根误差1.4910.4871.429最大值0.2090.4394.056BDS/I
45、NS/ZUPT均方根误差0.1030.0831.209最大值0.2140.4341.932BDS/INS/ZUPT/ZARU均方根误差0.0890.0810.6290.053m/s。相应的均方根误差分别降低到0.0 2 1、0.013、0.0 2 2 m/s，精度分别提高99%、99%和94%。在此基础上，进一步增加ZARU对速度没有进一步的改善如图12 和表5所示，在BDS中断期间，位置误差随时间迅速发散，最大误差分别为19 5.8 34、332.672、2 1.7 41m。相应的均方根误差分别为56.639、95.57 4、6.2 0 5m。应用ZUPT后，位置误差的发散问题得到解决。3个
46、方向的位置误差在0.0 6 m以内，相应的均方根误差分别为0.0 14、0.0 18、0.018m，3个方向精度均提高90%以上。结果表明，在没有BDS信号的情况下，ZUPT在农机中的应用显著地改善了位置精度。在ZUPT的基础上，进一步使用ZARU不会对位置误差产生显著影响。如图13和表6 所示，当BDS中断时，姿态误差逐渐发散。3个姿态角误差的最大值分别为4.972、1.36 5和4.6 8 5，相应的均方根误差分别为1.491、0.48 7 和1.42 9。应用ZUPT后，横滚角和俯仰角的精度显著提高。两个姿态角的误差均在0.5以内，均方根误差分别降低到0.10 3和0.083，精度分别提
47、高93%和8 3%。然而，航向角误差的改善并不明显，最大误差和均方根误差分别为4.0 56 和1.2 0 9，误差发散的问题仍然存在。继续增加提出的ZARU算法，最大误差减小到1.932，均方根误差减小到0.6 2 9对于航向角误差的精度具有显著的改善，准确度提高48%。同时，保持了横滚角和俯仰角的精度。通过对实验结果的分析，可以得出BDS/INS组合导航系统在BDS中断期间退化为纯INS，速度误差、位置误差和姿态误差均逐渐发散的结论。ZUPT可以通过提供速度观测值来抑制速度误差的发散。此外，虽然没有位置误差观测值，但位置是通过速度积分获得的，因此应用ZUPT之后，位置误差的发散也可以得到解决
48、。故而可以得出，ZUPT在复杂的农业生产环境中应用于农机可以达到令人满意的精度，这表明本文设计的组合导航系统和ZUPT算法具有良好的稳定性，能够满足农业生产的需要。然而，与BDS信号可用时相比，由于缺少位置观测值，精度略有降低。同时,ZUPT能够提高横滚角和俯仰角的精度，但是因为ZUPT不能够提高航向角误差的可观测度，因而不能提高航向角精度。继续增加ZARU，航向角误差明显改善。3结束语采用低成本MEMS-IMU设计了BDS/INS组合导航系统，并在东风DF10042 型拖拉机上进行了测试和验证。针对BDS信号中断期间定位误差发散以及组合导航系统中航向角误差不可观测的问题，设计了ZUPT和DA
49、与ZARU结合的航向约束算法。现场实验表明，所提出的方法是有效的,BDS/INS组合导航系统可以为农业生产提供准确的定位信息。当BDS可用时,ZUPT可以将位置、速度和水平姿态的精度分别提高2 0%、40%和15%以上，使用DA的航向约束可以将航向角精度提高9 0%以上;BDS中断时，ZUPT可以将位置、速度的精度提高90%以上，水平姿态的精度提高8 0%以上，使用ZARU的航向约束可以将航向角精度提高40%以上。此外，所提出的方法不需要增加额外的传感器，降低了农机导航系统的成本，有利于该系统推广。参考文献1张漫，季宇寒，李世超，等农业机械导航技术研究进展J.农业机械学报，2 0 2 0，51
50、（4）：1-18.ZHANG Man,JI Yuhan,LI Shichao,et al.Research progress of agricultural machinery navigation technology JJ.Transactions ofthe Chinese Society for Agricultural Machinery,2020,51(4):1-18.(in Chinese)2MORIMOTO E,HAYASHI K.Design of smart agriculture Japan modelJ.Advances in Animal Biosciences,20