Design and experiment of low-power BDS-SPP/INS fusion positioning system
-
摘要:目的
融合定位设备大多依赖于BDS-RTK，而BDS-RTK功耗大，在网络信号不佳的区域无法获取差分链路，只能使用标准单点定位(Standard point positioning，SPP)，而SPP定位系统存在定位误差大、抗干扰能力弱的问题。本研究致力于解决这些问题。
方法提出一种惯性导航系统(Inertial navigation system，INS)和BDS-SPP传感器信息融合卡尔曼滤波方法，基于该方法开发了低功耗融合定位系统。采用BDS-RTK作为基准，测试了BDS-SPP的低功耗模块静态和动态的误差、航姿参考系统(Attitude and heading reference system，AHRS)零偏和噪声，同时进行滤波器融合定位试验，检测在单天线BDS受到干扰时AHRS的断点续航情况。
结果BDS定位的静态误差为0.4726 m，BDS-SPP/INS融合定位系统动态平均标准差小于1.9137 m，相较于融合前减少0.1652 m。断点续航试验结果表明，融合定位系统偏移距离平均标准差为3.6365 m，相较于融合前减少了2.5900 m。BDS-SPP/INS融合定位系统比BDS-RTK定位系统功率降低了33.3 W；融合后的输出频率较单独采用BDS-SPP情况提高了3倍。
结论本文的融合定位装置提高了BDS-SPP的抗干扰能力，减少了定位误差，可以在缺少RTK链路的情况下为农业机器人提供定位，可以为农业机器人导航研究提供了技术基础。
Abstract:ObjectiveMost fusion positioning devices rely on BDS-RTK, which has high power consumption. In areas with poor network signals, differential links cannot be obtained, and only standard point positioning (SPP) can be used. SPP system has problems such as large positioning errors and weak anti-interference ability. This study is committed to addressing these issues.
MethodThis study proposed a low-power fusion positioning device that used inertial navigation system (INS) and BDS-SPP sensors based on the Kalman filter fusion method. The device was tested using BDS-RTK as a benchmark to evaluate the static and dynamic errors of the BDS-SPP low-power module, the attitude and heading reference system (AHRS) zero bias and noise. Additionally, filtering fusion positioning experiments were conducted to test the AHRS breakpoint continuation in the case of BDS interference with a single antenna.
ResultThe static error of BDS positioning was 0.4726 m, the average standard deviation of the dynamic BDS-SPP/INS fusion positioning system was less than 1.9137 m, and the average standard deviation reduced by 0.1652 m compared to before fusion. The breakpoint continuation experiment showed that the average standard deviation of the offset distance of the fusion positioning system was 3.6365 m, which decreased by 2.5900 m compared to before fusion. The BDS-SPP/INS fusion positioning system reduced the power consumption by 33.3 W compared to the BDS-RTK positioning system, and increased the output frequency by three times.
ConclusionThe fusion positioning device in this article improves the anti-interference capability of BDS-SPP and reduces positioning errors. It can provide positioning for agricultural robots in the absence of an RTK link, and provide a technical basis for agricultural robot navigation research.
-
Keywords:
- Agricultural robot /
- Low-power /
- BDS /
- Information fusion /
- Kalman filter /
- INS
-
随着城镇化进程加快，进城务工的劳动力不断增加，加之老龄化问题日趋严重，“谁来种地”和“如何种地”成为全世界面临的共同问题[1]，无人农场和智慧农业是解决这一问题的重要途径[2-5]。
我国南方地区农田面积较小且分散，田块之间的边界不明显，大型农业设备难以在此类农田环境中开展工作，使用小型无人驾驶农机是解决此问题的有效途径，所以针对复杂的农田环境开发一套小型可靠的导航定位系统极为迫切，其中降低成本和减少功耗是需要解决的关键问题[6-7]。
关于自动驾驶技术对稳定可靠定位系统的要求，国内外研究者进行了很多相关研究[8-11]，庄朝文等[12]提出了一种利用多传感器融合技术来降低成本的组合导航定位系统，利用定位和航向输出，并以元件参数误差补偿校正，运用一阶马尔可夫模型建立传感器参数误差模型，该系统融合了Kalman滤波的全球定位系统(Global positioning system，GPS)和惯性导航系统(Inertial navigation system，INS)信息，通过仿真试验验证了算法的可行性。张京等[13]采用自适应Kalman滤波算法建立了一种综合滤波模型，融合了INS和全球导航卫星系统(Global navigation satellite system，GNSS)，通过提高GNSS 信号品质以及航向角的变化幅度指引信息融合策略，仿真试验结果显示在直线行驶时融合系统的航向平均误差为−0.02°，标准差为0.50°；转向行驶融合系统的航向平均误差为0.62°，标准差为2.42°。王力等[14]在此基础上提出了一种自适应序贯无迹卡尔曼滤波(Unscented Kalman filter，UKF)改进算法，提高了北斗卫星导航系统(Beidou navigation satellite system，BDS)与INS融合后的精度。Shen等[15]采用径向基函数多层感知机的容积卡尔曼滤波处理导航中GPS信号丢失的问题，当信号中断500 s时，平均误差为23.11 m。Brown等[16]针对无人驾驶车失锁问题，提出组合 GPS、微机电系统和惯性导航组件，车辆在失锁后20 s内，误差超过10 m。魏永超等[17]提出了基于DS-SVR的BDS/INS组合导航算法，利用Dempster-Shafer (DS)证据理论算法融合，在缺失卫星信号时通过支持向量回归机(Support vector regression，SVR)消除INS误差，试验结果表明DS-SVR算法优化了导航能力、提高了导航精度。张晓寒等[18]在农机受环境因素干扰以致BDS失调、INS不足以导航时，以BDS/INS两者之间的位置和速度差值为模型，利用蜂群算法优化BP神经网络辅助组合导航，结果表明误差小于0.6 m/s，位置误差小于30 m。
本文针对旱田农机导航系统[19-23]、水田农机导航系统[24-27]在复杂农田环境下的使用需求，研究一种低功耗的定位系统；针对低功耗的BDS标准单点定位(BDS-standard point positioning，BDS-SPP)系统输出频率低的问题，基于运动模型设计了融合INS的Kalman滤波器以提高输出频率，搭建基于低功耗微控制单元(Microcontroller unit，MCU)的测试系统平台，设计一种适应多种复杂环境要求的、基于Kalman滤波器的BDS-SPP/INS融合定位系统。
1. 设计运动学模型
本试验采用融合BDS-SPP/INS导航系统以克服信号短暂丢失的问题，张闻宇等[28]指出实时动态(Real-time kinematic，RTK)定位失锁时采用此方法在16.65 m内的误差小于20 cm。本文在此基础上采用低成本、低精度的BDS-SPP与航姿参考系统(Attitude and heading reference system，AHRS，INS的一种传感器)进行融合，进行信息融合前先构建一个机器人运动学模型(图1)。
![]()
图 1 机器人运行学模型图X和Y是质点运动方程的坐标系，(x，y)是此瞬时质点在坐标轴的投影，θ是此瞬时运动轨迹与X轴的夹角Figure 1. Model diagram of robot operationologyX and Y are the coordinate system of the motion equation of the particle, (x, y) is the instantaneous projection of the particle on the coordinate axis, and θ is the angle between the instantaneous motion trajectory and X axis$$ \begin{array}{c}\dot{x}=v(\mathrm{cos}\theta )+\left(0\right) \omega \text{，}\end{array} $$ (1) $$ \dot{y}=vv\mathrm{cos}\theta +\left(0\right)\omega \text{，} $$ (2) $$ \dot{\theta }=\left(0\right) v+\left(1\right)\omega \text{，} $$ (3) 式中，v为质点在此时刻的瞬时速度，
$ \omega$ 为质点的瞬时角速度，$ \theta$ 为质点的偏移角度，$ \dot \theta $ 为质点在偏移角度变化率，$ \dot x $ 为质点在X轴投影的速度，$ \dot y $ 为质点在Y轴投影的速度。2. Kalman滤波器
2.1 系统框架
本文设计的组合导航系统由BDS-SPP和INS组成，BDS-SPP提供载体的位置和速度信息，INS提供自身运动状态的姿态信息，结构如图2所示。采用三轴加速度计、三轴陀螺仪和三轴磁场计组成的AHRS作为惯性导航系统，其中包含了嵌入式的姿态数据解算单元与航向信息。
在低功耗融合系统架构下，BDS-SPP/INS定位系统的设计涉及定位精度和运行时间之间的权衡，虽然BDS可以长时间提供稳定的定位服务，但它容易受到干扰，导致定位漂移。相比之下，INS不依赖于电磁信号，可以在短时间内提供高精度定位，但随着时间积累其测量误差增加。为了解决这些问题，将BDS与INS融合，可以有效提高定位精度，满足长时间不间断导航的需求。本文采用Kalman滤波进行多传感器数据融合，当BDS-SPP因信号被遮挡或减弱而无法提供准确的定位信息时，INS可以暂时接替BDS-SPP进行导航，但INS会随着时间推移产生速度和位置的累计误差。因此，只能在较短的时间或距离内使用INS导航，当BDS-SPP信号恢复正常后，对INS的误差进行修正，使定位恢复正常。
2.2 基础Kalman滤波器
采用Kalman滤波融合BDS-SPP与INS是组合导航的有效方法[29-30]，通过建立物理系统模型，同时利用滤波算法，从带有误差的信号中估计出置信度更高的信号，将得到的误差估计值对INS进行修正[31]。
Kalman滤波算法是一种线性动态系统的最优估计算法。它基于2个假设：信息过程是基于模型的，由白噪声激发；每次测量信号都包含附加的白噪声分量。对于满足这些条件的系统，Kalman滤波器是最优的信息融合器。Kalman滤波算法的流程包括5个核心更新迭代公式。
向前推算状态变量：
$$ {\hat {x}}_{k}^-={\boldsymbol{A}}{\hat {x}}_{k-1}+{\boldsymbol{B}}{u}_{k-1}\text{，} $$ (4) $$ {\hat {{x}}}_{k}^-=\left[xy\theta v\right]\text{，} $$ (5) $$ {{u}}_{k}=[{a}_{{x}}{a}_{{y}}\dot{\theta ]}\text{，} $$ (6) $$ {\boldsymbol{A}}=\left[\begin{array}{cccc}1& 0& 0& t\mathrm{cos}\theta \\ 0& 1& 0& t\mathrm{sin}\theta \\ 0& 0& 1& 0\end{array}\right]\text{，} $$ (7) $$ {\boldsymbol{B}}=\left[\begin{array}{ccc}0.5t\mathrm{sin}\theta & 0.5t\mathrm{cos}\theta & 0\\ -0.5t\mathrm{cos}\theta & -0.5t\mathrm{sin}\theta & 0\\ 0& 0& 1\end{array}\right]t\text{，} $$ (8) 式中，
$ {\hat {x}}_{k}^{-} $ 为k时刻的先验状态估计结果，${\hat {x}}_{k-1} $ 为k−1时刻的后验状态估计结果，$ {u}_{k} $ 和${u}_{k-1} $ 分别为k和k−1时刻的状态方程输入，$ {a}_{{x}} $ 和$ {a}_{{y}} $ 分别为车辆坐标系X和Y轴加速度，${\boldsymbol{A}} $ 为状态转移矩阵，$ {\boldsymbol{B}} $ 为将输入转换为状态的矩阵(uk控制矩阵)，t为时间步长。向前推算误差协方差：
$$ {{\boldsymbol{P}}}_{k}^-={\boldsymbol{A}}{{\boldsymbol{P}}}_{k-1}{{\boldsymbol{A}}}^{\mathrm{T}}+{\boldsymbol{Q}}\text{，} $$ (9) 式中，
$ {{\boldsymbol{P}}}_{k}^{-} $ 为k时刻的先验估计协方差，$ {{\boldsymbol{P}}}_{k} $ 和$ {{\boldsymbol{P}}}_{k-1} $ 分别为k和k−1时刻的后验估计协方差，$ {\boldsymbol{Q}} $ 为过程激励噪声协方差。计算Kalman增益：
$$ {{\boldsymbol{K}}}_{k}={{\boldsymbol{P}}}_{k}^-{{\boldsymbol{H}}}^{{\rm{T}}}({\boldsymbol{H}}{P}_{k}^-{{\boldsymbol{H}}}^{{\rm{T}}}+{\boldsymbol{R}}{)}^{-1}\text{，} $$ (10) 式中，
$ {{\boldsymbol{K}}}_{k} $ 为滤波增益矩阵，$ {\boldsymbol{H}} $ 为状态变量到测量的转换矩阵，$ {\boldsymbol{R}} $ 为测量噪声协方差。由观测变量
$ {z}_{k} $ 更新估计：$$ {\hat {x}}_{k}={\hat {x}}_{k}^-+{{\boldsymbol{K}}}_{{\boldsymbol{k}}}\left({z}_{k}-{\boldsymbol{H}}{\hat {x}}_{k}^-\right)\text{，} $$ (11) 式中，
${\hat {x}}_{k} $ 为k时刻的后验状态估计结果，$ {z}_{k} $ 为测量值滤波的输入，$\left({z}_{k}-{\boldsymbol{H}}{\hat {x}}_{k}^{-}\right) $ 为实际观测和预测观测的残差。更新协方差估值：
$$ {{\boldsymbol{P}}}_{k}=\left({\boldsymbol{I}}-{{\boldsymbol{K}}}_{k}{\boldsymbol{H}}\right){{\boldsymbol{P}}}_{k}^-\ ， $$ (12) 式中，I为单位矩阵。
基于上述递推方程和状态方程设计了Kalman滤波器。公式(4)是用
$ k-1 $ 时刻系统的后验状态估计值$ {\hat {x}}_{k-1} $ 预测k时刻系统的状态值，同时用k−1时刻的误差协方差$ {{\boldsymbol{P}}}_{k-1} $ 和过程噪声$ {\boldsymbol{Q}} $ 预测k时刻的先验误差协方差$ {{\boldsymbol{P}}}_{k}^{-} $ ，即公式(9)，结合公式(4)、(9)组成时间更新方程；由公式(3)计算Kalman增益，公式(4)进行校正更新，为进行下一步估计$ k+1 $ 时刻的系统状态值的迭代更新操作，更新$ {{\boldsymbol{P}}}_{k} $ 值得到公式(12)，该公式利用Kalman增益参数进行更新，实现对系统状态值更准确的预测和估计。公式(5)中的x、y、θ和v由BDS-SPP定位系统获得，公式(6)中的$ {a}_{{x}}\mathrm{、}{a}_{{y}} $ 和θ由姿态传感器系统AHRS获得。通过前期试验调整Q和R，使估计误差的方差最小以获得最优估计。$$ {\boldsymbol{Q}}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\text{，} $$ (13) $${\boldsymbol{ R}}=\left[\begin{array}{cccc}30& 0& 0& 0\\ 0& 30& 0& 0\\ 0& 0& 20& 0\\ 0& 0& 0& 3\end{array}\right]。 $$ (14) 惯性传感器初始化时间为15 s，启动设备时同步开启BDS和AHRS模块初始化，在所有模块完成初始化后开始融合定位工作。该板卡会根据定位质量提供标志位，在
$ {\$} $ GPRMC通信协议中有一个定位状态标志位，“A”表示正常定位状态，而“F”则表示受干扰状态，状态由定位板卡自动判断。当标志位为“A”时，使用公式(14)中的R矩阵，正常进行融合定位获得较为精准的航向信息；当标志位为“F”时，Kalman 滤波器根据标志位将R矩阵切换为测量噪声协方差矩阵R′，如公式(15)所示，减小卫星定位信号的权重，相对增加AHRS传感器信号的权重；初始航向使用切换前的航向信息，能较好地提高惯性定位精度，实现2个信号源之间的无缝衔接。该切换发生在标志位改变的采样周期(0.1 s)。
$$ {{\boldsymbol{R}}}'=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 20& 0\\ 0& 0& 0& 3\end{array}\right]。 $$ (15) 3. 低功耗融合系统架构
3.1 BDS-SPP/INS融合定位系统设计
系统主要包含BDS-SPP卫星标准单点定位模块和AHRS惯性传感器模块。惯性传感器模块采用WTGAHRS2-AHRS提供姿态信息，WTGAHRS2是一款高精度惯性导航仪，其惯性传感器为高精度陀螺仪和加速度计，姿态精度可以达到0.01°。一个基于STM32F407ZGT6搭建的低功耗MCU测试系统采集BDS-SPP、BDS-RTK以及融合后的定位数据；试验对照是和芯星通公司生产的高精度地基增强RTK-UM482-BOX2定位设备同步测试的数据。此外还有一个数据记录模块和电源模块。设计的6个模块组合整体结构如图3所示，实物图如图4所示。本文设计的融合定位系统的功率为2.7 W，其中STM32F407ZGT6模块为2.0 W，WTGAHRS2-AHRS模块为0.4 W，BDS-SPP模块为0.3 W，较RTK-UM482-BOX2定位系统的功率(36.0 W)降低了33.3 W。
![]()
图 3 低功耗的BDS-SPP/INS融合定位系统结构示意图Figure 3. Structure diagram of BDS-SPP/INS fusion positioning system with low power![]()
图 4 低功耗的BDS-SPP/INS融合定位系统实物示意图1：RTK-GNNS 定位系统；2：接收卫星信号天线；3：4G天线；4：AHRS系统；5：低功耗MCU测试系统Figure 4. Physical diagram of BDS-SPP/INS fusion positioning system with low power1: RTK-GNNS positioning system; 2: Antenna to receive satellite signal; 3: 4G antenna; 4: AHRS system; 5: Low-power MCU test system3.2 系统工作流程
系统首先进行初始化，包括HAL坐标初始化、时钟初始化、延时初始化、LED灯初始化、解码初始化、串口初始化和GPIO初始化。系统初始化后开始对各个定位系统进行检测，判断系统是否接收到数据，若数据接收正常，则执行下一步解码，然后将解码得到的数据存储于SD卡；如接收数据出现异常，在系统判定没有接收到数据时系统会跳过解码流程，执行对下个导航定位系统的判定，当RTK、BDS-SPP/INS模块都检测完毕，系统将会检测BDS-SPP系统和INS系统是否全部接收到信号，当所有模块均能正常接收到信号时，将通过Kalman滤波对BDS-SPP/INS进行数据融合；系统将融合后的数据存储于SD卡，并回到初始化后的检测，判定3个导航系统是否接收到信号，循环执行，直至系统关闭，程序结束。
4. 设计试验
4.1 试验方案
试验以和芯星通公司生产的BDS-RTK[动态定位精度(15+D×10−6) mm，D为基站到移动站的距离，m]定位设备的同步测试数据为参考，采用对比检测方法，选取广州市天河区和增城区2个区域对低功耗的BDS-SPP/INS融合定位系统的区域定位性能进行测试和分析。在2个区域，分别进行静态和动态定位精度测试，静态测试点不少于3个，选择视野开阔地带进行测试，测试时间不少于30 min，每个测试点测试时打开接收机供电，记录各个接收机从开始收敛到收敛足够多的数据；动态测试不少于10组，每组测试范围不小于1 km，测试平台选用雅迪公司生产的电瓶车，天线稳固安装在电瓶车车头。
INS惯性传感器采用维特智能公司生产的WTGAHRS2-AHRS提供姿态信息，为保证精度、减少误差，将AHRS面向前方安装，接收机必须避开金属物品以及对天线起电磁干扰的遮挡物，以保证融合定位接收机数据的准确性。
为验证本研究设计系统的抗干扰性，针对复杂自然条件下BDS-SPP/INS融合定位系统可能存在的信号不好或者信号丢失情况，进行3组抗干扰试验，试验中采用金属绝缘材料，将BDS-SPP的天线接收器覆盖4~10 s不等的时间，隔绝信号接收，模拟自然环境下信号丢失情况，并观察在丢失信号后融合定位系统的定位续航情况。
4.2 数据结果与分析
按BDS-SPP/INS融合定位系统的定位终端性能测试指标测算方法，编写Matlab程序进行统计计算，绘制圆概率误差图和距离误差曲线图。
采用高斯−克吕格投影(等角横切椭圆柱投影)作为坐标转换方法将空间坐标转换为高斯坐标后，再以GPS静态定位真值作为坐标原点，从而将高斯坐标系转换为当地坐标系[32]。
4.2.1 静态数据结果与分析
设计信息融合滤波器需要获得先验误差协方差，所以对比低功耗BDS-SPP定位系统与高精度BDS-RTK的静态误差值(2种卫星定位系统处于静止状态，以高精度BDS-RTK定位作为基准)，在广州天河区和增城区进行多组静态定位性能测试，分别选取2组试验结果进行性能对比，测试时间为2021年7月，4组试验时长分别为2100、1800、900、2280 s，总时间为7 080 s，具体数据如表1所示。
表 1 BDS-SPP定位系统的静态误差Table 1. Static error of BDS-SPP positioning systemt/s 标准差/m
Standard deviation圆概率误差/m
Circular probable error平均误差/m
Mean error北纬/(°)
North latitude东经/(°)
East longitude2100 0.6005 0.4982 0.4627 23.1626 113.3399 1800 0.6827 0.5564 0.5401 23.1560 113.3584 900 0.2631 0.2181 0.2404 23.2423 113.6378 2280 0.7381 0.6068 0.6471 23.2432 113.6380 平均值 Avearage 0.5711 0.4699 0.4726 图6、7为广州市增城区第3组试验的静态误差图和圆概率误差图，根据图6数据计算得出在空旷地带BDS-SPP的静态误差为0.2404 m，对比图6A、6B，在同等条件下RTK 的误差精度以及波动小于BDS-SPP。将4组试验的数据进行计算，得到标准差、圆概率误差、平均差、纬度真值和经度(表1)。
![]()
图 6 BDS-SPP (A)和BDS-RTK (B)定位的静态误差图Figure 6. Static error diagrams of BDS-SPP (A) and BDS-RTK (B) positioning![]()
图 7 BDS-SPP定位以高精度BDS-RTK定位作为参照圆心的圆概率误差Figure 7. Probability error of BDS-SPP positioning using high-precision BDS-RTK positioning as the reference center for the circle4.2.2 动态数据结果与分析
为了对比低功耗BDS-SPP/INS的融合定位系统与高精度BDS-RTK的动态误差值，在广州天河区进行了6组动态定位性能测试，测试时间为2021年7月，总时长为2633 s，且每次测试时间不等。将BDS-SPP定位点与参照系BDS-RTK定位点之间的直线距离称为BDS-SPP定位偏移距离，将BDS-SPP/INS融合定位点与参照系BDS-RTK定位点之间的直线距离称为BDS-SPP/INS定位偏移距离，为分析误差大小求取其标准差，数据如表2所示，由表2可知，动态试验中BDS-SPP/INS定位偏移距离的最小标准差为1.1415 m，最大标准差为3.0516 m。BDS-SPP/INS定位偏移距离的平均标准差小于1.9137 m，比BDS-SPP定位偏移距离小0.1652 m。
表 2 不同定位系统的动态性能Table 2. Dynamic performance of different positioning systemst/s 速度/(m·s−1)
Speed行驶距离/m
Distance traveled偏移距离的标准差/m Standard deviation of offset distance BDS-SPP BDS-SPP/INS 450 5.77 2600 1.7635 1.7445 396 4.55 1800 1.8043 1.5160 441 5.21 2300 2.7938 2.7251 479 5.22 2500 1.1514 1.1415 456 5.15 2350 1.7726 1.4454 410 5.91 2500 3.2240 3.0516 平均值 Average 2.0789 1.9137 广州市动态试验的定位性能测试结果如图8所示， 分析图8A可以看到在同样条件下融合后得到的定位点变化更加贴合BDS-RTK定位点，融合后的输出频率为BDS-SPP输出频率的3倍，由图8B可知在此次试验中融合后的数据去除了高噪声影响，融合后的数据更加平缓、波动更小，更符合实际运动规律。
![]()
图 8 融合前、后的系统动态定位散点图(A)和偏移距离图(B)图A中，上方和下方的短平行线是对箭头指向位置的定位轨迹的放大Figure 8. Dynamic positioning scatter plot (A) and offset distance plot (B) of the system before and after fusionIn Figure A, the short parallel lines above and below magnify the positioning trajectories of the positions pointed by arrows4.2.3 抗干扰试验结果与分析
针对融合定位系统工作中信号丢失问题，设计了抗干扰试验，以高精度BDS-RTK为测试真值，观察当BDS信号丢失后的BDS-SPP/INS定位偏移距离以及BDS-SPP定位偏移距离。在广州华南农业大学校园内固定道路进行10组测试，测试时间为2021年12月，总时间为150 min，随机取3组数据进行分析，其中第1组试验结果如图9所示，具体偏移距离统计数据如表3所示，由表3可知，当试验中BDS丢失信号时，BDS-SPP定位偏移距离的平均标准差为6.2265 m， BDS-SPP/INS定位偏移距离平均标准差为3.6365 m，比BDS-SPP定位系统减少2.5900 m。
![]()
图 9 抗干扰第1号试验的系统动态定位散点图(A)和偏移距离图(B)Figure 9. Dynamic positioning scatter plot (A) and offset distance plot (B) of the system for anti-interference test No. 1表 3 抗干扰试验的不同定位系统动态性能Table 3. Dynamic performance for different positioning systems in anti-interference testt/s 速度/(m·s−1)
Speed行驶距离/m
Distance traveled偏移距离的标准差/m Standard deviation of offset distance BDS-SPP BDS-SPP/INS 1080 4.78 2500 5.7119 4.4576 1020 5.33 2350 8.7269 4.6847 1020 5.67 2300 5.2408 2.7674 平均值 Average 6.2265 3.6365 5. 讨论与结论
INS惯性导航装置中存在惯性传感器误差累积导致的惯性漂移问题，可以通过校准初始航向角和零偏的方法消除。未来可以进一步研究惯性传感器角速率测量零偏实时校准方法和自回归模型的航向校准方法，增强其自适应能力，提高惯性导航的精确度和可靠性，以更好地应对各种复杂环境和操作场景下的导航需求。
本文提出了一种针对低功耗定位系统需求的技术方案。该方案采用低功耗的BDS-SPP和INS系统进行融合，通过Kalman滤波器进行优化，与高精度地基增强BDS-RTK定位系统的对比试验结果表明，其功耗降低了33.3 W，在满足定位准确性要求的同时达到了功耗更低。此外，本文还针对复杂农田环境要求，搭建了基于低功耗MCU测试系统平台，设计出低功耗的BDS-SPP/INS融合定位系统。
进行了静态与动态试验，BDS-SPP静态漂移试验结果中，平均误差为0.4726 m，圆概率误差为0.4699 m；动态试验结果中，BDS-SPP/INS定位偏移距离的最小标准差为1.1415 m，最大标准差为3.0516 m。BDS-SPP/INS定位偏移距离的的平均标准差为1.9137 m，比低功耗BDS-SPP定位系统的小0.1652 m。
对BDS-SPP/INS融合定位系统在干扰条件下的试验结果得出，BDS-SPP/INS定位偏移距离的平均标准差为3.6365 m，比BDS-SPP定位系统减少2.5900 m。表明融合定位系统能够在复杂条件下工作，可以在缺少RTK链路的情况下提供基础保障。由于引入了INS的高频数据，融合后的输出频率较单独采用BDS-SPP提高了3倍，从而能够更及时地对状态进行响应，进一步提高了BDS-SPP的定位质量。这为小型农田机器人(如田间监测)的定位问题提供了技术基础。
-
![]()
图 1 机器人运行学模型图
X和Y是质点运动方程的坐标系，(x，y)是此瞬时质点在坐标轴的投影，θ是此瞬时运动轨迹与X轴的夹角
Figure 1. Model diagram of robot operationology
X and Y are the coordinate system of the motion equation of the particle, (x, y) is the instantaneous projection of the particle on the coordinate axis, and θ is the angle between the instantaneous motion trajectory and X axis
![]()
图 3 低功耗的BDS-SPP/INS融合定位系统结构示意图
Figure 3. Structure diagram of BDS-SPP/INS fusion positioning system with low power
![]()
图 4 低功耗的BDS-SPP/INS融合定位系统实物示意图
1：RTK-GNNS 定位系统；2：接收卫星信号天线；3：4G天线；4：AHRS系统；5：低功耗MCU测试系统
Figure 4. Physical diagram of BDS-SPP/INS fusion positioning system with low power
1: RTK-GNNS positioning system; 2: Antenna to receive satellite signal; 3: 4G antenna; 4: AHRS system; 5: Low-power MCU test system
![]()
图 6 BDS-SPP (A)和BDS-RTK (B)定位的静态误差图
Figure 6. Static error diagrams of BDS-SPP (A) and BDS-RTK (B) positioning
![]()
图 7 BDS-SPP定位以高精度BDS-RTK定位作为参照圆心的圆概率误差
Figure 7. Probability error of BDS-SPP positioning using high-precision BDS-RTK positioning as the reference center for the circle
![]()
图 8 融合前、后的系统动态定位散点图(A)和偏移距离图(B)
图A中，上方和下方的短平行线是对箭头指向位置的定位轨迹的放大
Figure 8. Dynamic positioning scatter plot (A) and offset distance plot (B) of the system before and after fusion
In Figure A, the short parallel lines above and below magnify the positioning trajectories of the positions pointed by arrows
![]()
图 9 抗干扰第1号试验的系统动态定位散点图(A)和偏移距离图(B)
Figure 9. Dynamic positioning scatter plot (A) and offset distance plot (B) of the system for anti-interference test No. 1
表 1 BDS-SPP定位系统的静态误差
Table 1 Static error of BDS-SPP positioning system
t/s 标准差/m
Standard deviation圆概率误差/m
Circular probable error平均误差/m
Mean error北纬/(°)
North latitude东经/(°)
East longitude2100 0.6005 0.4982 0.4627 23.1626 113.3399 1800 0.6827 0.5564 0.5401 23.1560 113.3584 900 0.2631 0.2181 0.2404 23.2423 113.6378 2280 0.7381 0.6068 0.6471 23.2432 113.6380 平均值 Avearage 0.5711 0.4699 0.4726 
下载: 导出CSV
表 2 不同定位系统的动态性能
Table 2 Dynamic performance of different positioning systems
t/s 速度/(m·s−1)
Speed行驶距离/m
Distance traveled偏移距离的标准差/m Standard deviation of offset distance BDS-SPP BDS-SPP/INS 450 5.77 2600 1.7635 1.7445 396 4.55 1800 1.8043 1.5160 441 5.21 2300 2.7938 2.7251 479 5.22 2500 1.1514 1.1415 456 5.15 2350 1.7726 1.4454 410 5.91 2500 3.2240 3.0516 平均值 Average 2.0789 1.9137
下载: 导出CSV
表 3 抗干扰试验的不同定位系统动态性能
Table 3 Dynamic performance for different positioning systems in anti-interference test
t/s 速度/(m·s−1)
Speed行驶距离/m
Distance traveled偏移距离的标准差/m Standard deviation of offset distance BDS-SPP BDS-SPP/INS 1080 4.78 2500 5.7119 4.4576 1020 5.33 2350 8.7269 4.6847 1020 5.67 2300 5.2408 2.7674 平均值 Average 6.2265 3.6365
下载: 导出CSV
-
[1] HASSALL J. Future trends in precision agriculture[R/OL]. [2023-05-01]. https://www.nuffieldscholar.org/sites/default/files/reports/2009_AU_James-Hassall_Future-Trends-In-Precision-Agriculture-A-Look-Into-The-Future-Of-Agricultural-Equipment.pdf.
[2] 罗锡文, 王在满. 水稻生产全程机械化技术研究进展[J]. 现代农业装备, 2014(1): 23-29. doi: 10.3969/j.issn.1673-2154.2014.01.008 [3] 罗锡文, 廖娟, 汪沛, 等. 提高农业机械化水平促进农业现代化发展[J]. 中国农村科技, 2021(1): 6-11. doi: 10.3969/j.issn.1005-9768.2021.01.003 [4] 陈庆根, 廖西元. 优质水稻生产投入与产出的经济效益评价[J]. 中国农村经济, 2001(3): 18-24. [5] 张智刚, 王进, 朱金光, 等. 我国农业机械自动驾驶系统研究进展[J]. 农业工程技术, 2018, 38(18): 23-27. [6] 韩树丰, 何勇, 方慧. 农机自动导航及无人驾驶车辆的发展综述(英文)[J]. 浙江大学学报(农业与生命科学版), 2018, 44(4): 381-391. [7] 张漫, 季宇寒, 李世超, 等. 农业机械导航技术研究进展[J]. 农业机械学报, 2020, 51(4): 1-18. doi: 10.6041/j.issn.1000-1298.2020.04.001 [8] 姚立健, SANTOSH K P, 杨自栋, 等. 基于超宽带无线定位的农业设施内移动平台路径跟踪研究[J]. 农业工程学报, 2019, 35(2): 17-24. doi: 10.11975/j.issn.1002-6819.2019.02.003 [9] 陈黎卿, 许鸣, 柏仁贵, 等. 高地隙植保机辅助驾驶系统设计与试验[J]. 农业机械学报, 2019, 50(9): 25-32. doi: 10.6041/j.issn.1000-1298.2019.09.003 [10] LIU Y, MA X Y, SHU L, et al. From Industry 4.0 to Agriculture 4.0: Current status, enabling technologies, and research challenges[J]. IEEE Transactions on Industrial Informatics, 2021, 17(6): 4322-4334. doi: 10.1109/TII.2020.3003910
[11] BECHAR A, VIGNEAULT C. Agricultural robots for field operations. Part 2: Operations and systems[J]. Biosystems Engineering, 2017, 153: 110-128. doi: 10.1016/j.biosystemseng.2016.11.004
[12] 庄朝文, 富立, 范跃祖. 多传感器融合技术在低成本车辆组合导航定位系统中的应用[J]. 中国惯性技术学报, 2002, 10(5): 1-5. doi: 10.3969/j.issn.1005-6734.2002.05.001 [13] 张京, 陈度, 王书茂, 等. 农机INS/GNSS组合导航系统航向信息融合方法[J]. 农业机械学报, 2015, 46(S1): 1-7. doi: 10.6041/j.issn.1000-1298.2015.S0.001 [14] 王力, 杨忠辉. 自适应序贯UKF的INS/BDS紧组合导航算法[J]. 计算机仿真, 2019, 36(12): 228-232. doi: 10.3969/j.issn.1006-9348.2019.12.047 [15] SHEN C, ZHANG Y, TANG J, et al. Dual-optimization for a MEMS-INS /GPS system during GPS outages based on the cubature Kalman filter and neural networks[J] Mechanical Systems and Signal Processing, 2019, 133(106222): 1-14.
[16] BROWN A K, LU Y. Performance test results of an integrated GPS/MEMS inertial navigation package[C]//Proceedings of the 17th International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS 2004). Long Beach, California: ION GNSS, 2004: 825-832.
[17] 魏永超, 罗城仔, 邓岚, 等. 基于DS-SVR算法优化的BDS/INS组合导航[J]. 航空计算技术, 2020, 50(1): 57-60. doi: 10.3969/j.issn.1671-654X.2020.01.014 [18] 张晓寒, 赵景波, 董振振. 农机BDS/INS组合导航算法研究[J]. 中国农机化学报, 2021, 42(2): 171-177. [19] 张智刚, 罗锡文, 赵祚喜, 等. 基于Kalman滤波和纯追踪模型的农业机械导航控制[J]. 农业机械学报, 2009, 40(S1): 6-12. [20] 丁幼春, 何志博, 夏中州, 等. 小型履带式油菜播种机导航免疫 PID 控制器设计[J]. 农业工程学报, 2019, 35(7): 12-20. doi: 10.11975/j.issn.1002-6819.2019.07.002 [21] 李逃昌, 胡静涛, 高雷, 等. 基于模糊自适应纯追踪模型的农业机械路径跟踪方法[J]. 农业机械学报, 2013, 44(1): 205-210. [22] 张闻宇, 丁幼春, 王雪玲, 等. 基于SVR逆向模型的拖拉机导航纯追踪控制方法[J]. 农业机械学报, 2016, 47(1): 29-36. [23] BEVLY D M, GERDES J C, PARKINSON B W. A new yaw dynamic model for improved high speed control of a farm tractor[J]. Journal of Dynamic Systems, Measurement and Control, 2002, 124(4): 659-667. doi: 10.1115/1.1515329
[24] 何杰, 朱金光, 张智刚, 等. 水稻插秧机自动作业系统设计与试验[J]. 农业机械学报, 2019, 50(3): 17-24. doi: 10.6041/j.issn.1000-1298.2019.03.002 [25] 段贤强, 陶建峰, 覃程锦, 等. 变速条件下农业机械路径跟踪稳定控制方法[J]. 农业机械学报, 2019, 50(9): 18-24. doi: 10.6041/j.issn.1000-1298.2019.09.002 [26] 刘兆朋, 张智刚, 罗锡文, 等. 雷沃ZP9500高地隙喷雾机的GNSS自动导航作业系统设计[J]. 农业工程学报, 2018, 34(1): 15-21. [27] 王辉, 王桂民, 罗锡文, 等. 基于预瞄追踪模型的农机导航路径跟踪控制方法[J]. 农业工程学报, 2019, 35(4): 11-19. [28] 张闻宇, 王进, 张智刚, 等. 基于自校准变结构Kalman的农机导航BDS失锁续航方法[J]. 农业机械学报, 2020, 51(3): 18-27. doi: 10.6041/j.issn.1000-1298.2020.03.002 [29] 靳标, 李建行, 朱德宽, 等. 基于自适应有限冲激响应−卡尔曼滤波算法的GPS/INS导航[J]. 农业工程学报, 2019, 35(3): 75-81. [30] 洪剑青, 赵德安, 孙月平, 等. 基于卡尔曼滤波离散滑模控制的明轮船直线跟踪[J]. 农业机械学报, 2019, 50(9): 33-39. doi: 10.6041/j.issn.1000-1298.2019.09.004 [31] 白秉旭, 田光兆. 基于模糊算法的自主导航拖拉机转向系统设计[J]. 中国农机化学报, 2014, 35(6): 135-137. [32] 刘健, 卢加华, 杨新云, 等. 中纬度地区兰勃特投影和高斯−克吕格投影变形分析[J]. 地矿测绘, 2019, 35(3): 12-14. doi: 10.16864/j.cnki.dkch.2019.0037
计量
- 文章访问数: 1134
- HTML全文浏览量: 12
- PDF下载量: 21
