Processing math: 1%
    • EI
    • CSA
    • CABI
    • 卓越期刊
    • CA
    • Scopus
    • CSCD
    • 核心期刊

基于UWB-IMU的设施园艺移动平台组合定位方法

丁俊杰, 许成果, 罗志涛, 郑恩来, 陈凤, 章永年, 施印炎, 沈成, 汪小旵

丁俊杰,许成果,罗志涛,等. 基于UWB-IMU的设施园艺移动平台组合定位方法[J]. 农业工程学报,2025,41(7):64-73. DOI: 10.11975/j.issn.1002-6819.202409099
引用本文: 丁俊杰,许成果,罗志涛,等. 基于UWB-IMU的设施园艺移动平台组合定位方法[J]. 农业工程学报,2025,41(7):64-73. DOI: 10.11975/j.issn.1002-6819.202409099
DING Junjie, XU Chengguo, LUO Zhitao, et al. Combined positioning method for a mobile platform in facility horticulture based on UWB-IMU[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2025, 41(7): 64-73. DOI: 10.11975/j.issn.1002-6819.202409099
Citation: DING Junjie, XU Chengguo, LUO Zhitao, et al. Combined positioning method for a mobile platform in facility horticulture based on UWB-IMU[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2025, 41(7): 64-73. DOI: 10.11975/j.issn.1002-6819.202409099

基于UWB-IMU的设施园艺移动平台组合定位方法

基金项目: 国家重点研发计划项目(2023YFD2000302;2023YFD2000304);江苏省农机研发制造推广应用一体化试点项目(JSYTH04)
详细信息
    作者简介:

    丁俊杰,研究方向为农机导航控制。Email:15268290607@163.com

    通讯作者:

    郑恩来,博士,教授,博士生导师,研究方向为智能农业装备与移动作业机器人。Email:enlaizheng@njau.edu.cn

  • 中图分类号: S147.2

Combined positioning method for a mobile platform in facility horticulture based on UWB-IMU

  • 摘要:

    为解决设施场景下遮挡严重所致的电驱移动平台定位精度低、稳定性差的问题,该研究设计了基于超宽带(ultra wide band,UWB)和惯性测量单元(inertial measurement unit,IMU)的组合定位方法,并提出基于卡尔曼滤波的自适应抗差组合定位算法(adaptive robust combination positioning algorithm,ARCPA)。首先,针对单传感器定位误差大的问题,搭建基于UWB与IMU两种传感器的组合定位系统;其次,为精准检测UWB数据的非视距误差,设计差分距离非视距误差检测滑动窗口;最后,为抑制卡尔曼滤波中的量测噪声,采用余弦重启函数实时调整渐消因子,并引入位置精度因子权值修正抗差因子。为验证所提定位方法的有效性,搭建设施园艺电驱移动平台和实时定位系统,并进行仿真与实车定位试验。试验结果表明,采用所提UWB-IMU组合定位方法的均方根误差为6.63 cm,比单传感器的定位精度提高了81.92%,相比于经典卡尔曼滤波、无迹卡尔曼滤波、扩展卡尔曼滤波、粒子滤波与自适应抗差卡尔曼滤波算法,所提ARCPA算法定位精度分别提高50.15%、74.63%、60.01%、60.88%、28.34%。研究结果可为设施园艺环境下的定位导航提供理论依据和实施方案。

    Abstract:

    Protected horticulture is required for the high positioning accuracy and stability of mobile platforms. However, the electric drive mobile platforms can be often confined to object obstruction under the natural scenes. In this study, a high-precision positioning system was designed using an ultra-wide band (UWB) and inertial measurement unit (IMU). Additionally, an adaptive robust combination positioning algorithm (ARCPA) with Kalman filtering was developed to effectively improve positioning accuracy and stability. Firstly, a multi-sensor integrated positioning was constructed to combine UWB and IMU. Secondly, a differential distance-based sliding window was introduced to precisely detect nonline of sight (NLOS) errors in the positioning data of UWB. Thirdly, a cosine restart function was used to dynamically adjust the fading factor in real time, in order to reduce the interference of noise fluctuations in Kalman filtering. The accuracy and robustness of the filter were enhanced after optimization. Finally, a position dilution of precision (PDOP) weighted robust factor was incorporated to improve the resilience of the positioning system against noise and model disturbances under-protected horticulture scenes. An electric drive mobile platform and real-time positioning were constructed to verify the effectiveness of the integrated system. Then the simulation and real experiments of vehicle positioning were conducted in the typical protected horticulture. The experimental results demonstrate that the root-mean-square errors before and after UWB ranging correction under line-of-sight condition were 119.0 and 49.0 cm, respectively, and the ranging accuracy was improved by 58.8% after correction. The positioning error of the integrated positioning was only 6.63 cm, which was 81.92% higher than that of the single sensor (36.68 cm). The maximum error of positioning accuracy was also reduced by 88.89%. Therefore, this integrated positioning significantly reduced the impact of NLOS and geometric error on the positioning accuracy of the electric drive mobile platform for protected horticulture. Moreover, the root-mean-square error and the maximum positioning error of ARCPA were 17.70 and 44.44 cm, respectively. Furthermore, the accuracy of the improved system increased by 50.15%, 74.63%, 60.01%, 60.88%, and 28.34%, respectively, compared with the Kalman filtering, unscented Kalman filtering, extended Kalman filtering, particle filtering, and adaptive robust Kalman filtering. Correspondingly, the maximum positioning error was then reduced by 66.83%, 73.67%, 69.07%, 71.2%, and 50.50%, respectively. The integrated positioning significantly improved the robustness of the electric drive mobile platform for protected horticulture. The finding can also provide a specific theoretical basis and plan for the positioning and navigation in a protected horticulture environment.

  • 随着农业科技水平的提高,设施园艺逐步实现机械化、自动化、信息化生产,并向智能化转型[1-4]。开发适合设施园艺应用的智能作业机械成为当前研究的热点,其中精准定位对于引导移动作业机械自主行驶与作业至关重要。设施园艺具有非结构化和高度可变的复杂环境,且存在大量的人工设施[5],为获得高度自主移动性,往往需要安装复杂、全面的传感器以获得更精确稳定的定位。目前,完全自动化的解决方案尚未在设施园艺领域实施。

    设施园艺环境中无法采用常用的全球导航卫星系统(global navigation satellite system,GNSS)信号[6],因此需要一种可替代的实时定位系统,以满足设施园艺精确定位的要求。目前,各种室内定位方案都取得了巨大的发展,并在实践中得到了广泛应用,包括声定位、同步定位与地图构建(simultaneous localization and mapping,SLAM)、超宽带(ultra wide band,UWB)等。HUANG等[7]采用载波频率检测的多普勒频移补偿算法提升补偿声信号的能力,实现了温室环境下的厘米级精度定位。声定位虽然可以实现低成本实时定位,但易受环境干扰且覆盖范围存在局限性。SLAM技术是近年来的热门研究方向,然而由于设施园艺环境的特殊性,限制了该方法在设施园艺定位领域的应用。设施园艺与大部分的室内环境不同,多跨大棚通常具有以下特点:(1)面积大且包含很少独特元素的空旷环境(农田、水泥路面、塑料薄膜、玻璃等);(2)整体结构高度重复且具有大量障碍物(大棚立柱、灌溉系统等);(3)作物生长导致环境动态变化[8-9]。因此,对于具有动态、大尺度、高重复的设施园艺环境,视觉SLAM和激光SLAM难以提取和匹配显著特征进行定位。超宽带具有低功耗、高精度、高分辨率和覆盖范围大等特点,成为设施园艺环境定位系统的研究热点。姚立健等[10]采用加权最小二乘法提高UWB定位精度,设计基于动态前视距离的纯跟踪模型,并通过试验证明了特定情况下基于UWB定位的可行性。解开婷等[11]提出一种全质心加权最小二乘的高速双边双向算法,通过组合固定解与模糊解,将定位目标求解转换为凸规划优化,解决了UWB因测距误差导致三角形公共区域缺失的问题。陈兆等[12]建立了UWB基站布设优化模型,使用遗传算法和萤火虫算法优化基站布设阵型,降低了定位误差的波动。LI等[13]提出一种基于预校正策略和粒子群优化算法的超宽带精确测距方法,进一步提高了UWB测距精度。然而,设施园艺中的复杂场景仍限制着UWB的使用,多径效应、基站布设等问题都会导致UWB定位精度出现较大的波动。此外,设施园艺中存在的铁架、大棚立柱、塑料薄膜、作物等,会在UWB数据中引入大量的非视距(non line of sight,NLOS)误差[14-15]。综上,单一传感器存在固有限制和缺陷,无法满足复杂场景的定位要求。因此,可尝试多传感器组合定位以增加定位的稳定性和可靠性。其中,IMU传感器具有短时精准位置估算的特点,可对UWB定位误差进行修正。此外,通过运用误差检测、修正策略、多传感器数据融合技术与各种滤波算法,可解决环境中的固定误差和传感器的累积误差,常用的滤波算法包括扩展卡尔曼滤波(extended kalman filtering,EKF)、Sage-Husa卡尔曼滤波(sage-husa kalman filtering,SHKF)、无迹卡尔曼滤波(unscented kalman filtering,UKF)和自适应卡尔曼滤波(adaptive kalman filtering,AKF)等[16-21]。这些组合定位方案及融合滤波算法在处理非线性系统、动态环境下的实时性和鲁棒性方面仍有较大改进空间[22]

    为此,本文提出一种基于自适应抗差组合定位算法(adaptive robust combination positioning algorithm,ARCPA)的UWB-IMU组合定位方法。首先,引入距离差分滑动窗口对UWB数据进行非视距误差鉴别。然后,为充分利用量测信息,提出余弦重启渐消因子,动态调整历史量测的权重。同时,为了减小几何误差的影响,设计基于位置精度因子权值的抗差因子。最后,通过仿真与实车定位试验验证所提组合定位方法的有效性。

    针对设施园艺特殊作业场景对电驱移动平台灵活作业与高操纵稳定性需求,设计了一种设施园艺电驱移动平台,整体结构设计如图1a所示。动力系统主要由轮毂电机、动力电池、整车控制器等组成,驱动系统结构如图1b所示。采用纯电动四轮独立转向-独立驱动机构,由锂电池(76.8 V、280 Ah)供电。

    图  1  设施园艺移动平台整体结构及其驱动系统
    Figure  1.  Overall structure and drive system of mobile platform for protected horticulture

    在设施园艺场景下,作物种植密集且存在大量人工设施,UWB测距信号易受到周围环境的干扰,极大影响定位精度,而IMU可提供短时的精准定位信息,结合两者的信息可以显著改善系统的定位性能。

    UWB定位系统如图2所示,在设施园艺场景中部署3个或3个以上的UWB基站,UWB标签固定于电驱移动平台机体。本文导航坐标系用n系表示,载体坐标系用b系表示,On固定于基站S1在设施园艺环境地面的投影,载体坐标系Ob与设施园艺移动平台固连。

    图  2  设施园艺UWB定位系统
    注:Ob为载体坐标系,固连于移动平台,按“右前上”顺序定义,Xb沿载体横轴指向载体右侧,Yb沿载体横轴指向载体前方,Zb垂直XbYb所在平面向上。On为导航坐标系,固定于基站S1的地面投影区域,Xn沿大棚横向指向大棚右侧,Yn沿大棚纵向指向大棚前方,Zn垂直XnYn所在平面向上;d1d2d3d4为定位标签到基站S1,S2,S3,S4距离,m。
    Figure  2.  Protected horticulture UWB positioning system
    Note: Ob is the carrier coordinate system, fixed to the mobile platform, defined in the order of “right front up”, with Xb pointing to the right side of the carrier along the transverse axis of the carrier, Yb pointing to the front of the carrier along the transverse axis of the carrier, and Zb perpendicularly upward in the plane where Xb and Yb are located. On is the navigation coordinate system fixed to the projection area of the base station S1 on the ground, Xn points to the right side of the shed along the transverse axis of the shed, Yn points to the front side of the shed along the longitudinal axis of the shed, and Zn is perpendicular to the plane upward where Xn and Yn are located; d1, d2, d3, and d4 denote the distances of the positioning tags to the base station S1, S2, S3, S4, m.

    常用的UWB无线定位方法有到达时间法(time of arrival,TOA)、到达时间差法(time difference of arrival,TDOA)、基于信号到达角法(angle of arrival,AOA)和基于信号接收强度法(received signal strength indicator,SRS)[23]。本文使用基于测量信号到达时间法,测量基站与定位标签之间的距离,并采用三球交互定位方法估计UWB标签的位置(见图3)。

    图  3  三球交汇定位原理图
    Figure  3.  Three-ball intersection positioning schematic

    基站与定位标签之间的测量距离计算如下:

    d=ct+σ (1)

    式中d为信号传播距离,m;t为信号传播时间,s;c为信号传播速度,m/s;σ为测量噪声与异常误差之和,m。

    IMU主要由三轴加速度计和三轴陀螺仪组成[24],定位流程如图4所示,通过对输出的比力和角速度观测值进行积分运算,从而对载体位置、速度和姿态进行更新。

    图  4  IMU定位解算原理
    注:λL分别表示经度和纬度,(°);h表示高度,m;φθγ表示俯仰角、航向角、横滚角,rad;VEVNVU表示东、北、天方向速度分量,m·s-1 \boldsymbol{\boldsymbol{f}}^{\mathit{b}} \boldsymbol{\boldsymbol{f}}^{\mathit{n}} 表示b系和n系下的比力投影,m·s-2 \mathit{\omega}_{ie}^n 表示地球自转角速度矢量在导航坐标系中的投影,rad·s-1 {{{\omega}{_{en}^n}{{{ }}}}{ }} 表示导航坐标系下,地球自转和地球表面运动的角速度,rad·s-1 {\omega_{in}^b} 表示载体坐标系下,导航坐标系相对惯性坐标系的角速度,rad·s-1 \omega_{nb}^b 表示载体坐标系下,导航坐标系相对载体的角速度,rad·s-1 \omega_{in}^n 表示导航坐标系下,载体坐标系相对于惯性坐标系的角速度,rad·s-1
    Figure  4.  IMU (inertial measurement unit) positioning calculation principle
    Note: λ, L represent longitude, latitude, respectively, (°); h represent altitude, m; φθ,and γ denote the pitch angle, yaw angle, and roll angle, respectively, rad; VE, VN, and VU are the velocity components in east, north, and up directions, respectively, m·s-1; {\boldsymbol{f}}^b and \boldsymbol{f}^n refer to specific force projections in b-frame and the n-frame, respectively, m·s-2; \omega_{ie}^n represents projection of the Earth's rotation angular velocity vector in navigation coordinate system, rad·s-1; \omega_{en}^n denotes angular velocity in navigation coordinate system due to both Earth's rotation and movement on Earth's surface, rad·s-1; \omega_{in}^b indicates angular velocity of navigation coordinate system relative to inertial coordinate system in body coordinate system, rad·s-1; \omega_{nb}^b denotes angular velocity of navigation coordinate system relative to body in body coordinate system, rad·s-1; \omega_{in}^n represents angular velocity of body coordinate system relative to inertial coordinate system in the navigation coordinate system, rad·s-1.

    首先通过四元数对姿态进行更新,姿态更新方程及姿态矩阵求解如下:

    {{\boldsymbol{Q}}}_{b(k)}^{n}={{\boldsymbol{Q}}}_{b(k-1)}^{n}{{\boldsymbol{Q}}}_{b(k)}^{b(k-1)} (2)
    \begin{split} & \qquad\qquad {\boldsymbol{C}}_b^n= \\ & \left[\begin{array}{cccccccccccc}\ q_0^2+q_1^2-q_2^2-q_3^2\ \ \ 2(q_1q_2-q_0q_3)\ \ \ \ 2(q_1q_3+q_0q_2) \\ \ 2(q_1q_2+q_0q_3)\ \ \ \ q_0^2-q_1^2+q_2^2-q_3^2\ \ \ 2(q_2q_3-q_0q_1) \\ \ 2(q_1q_3-q_0q_2)\ \ \ \ 2(q_2q_3-q_0q_1)\ \ \ q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right] \end{split} (3)

    式中 \boldsymbol{Q}_{b\text{(}k)}^n=\left[q_0\ q_1\ q_2\ q_3\right]\mathrm{^T} k时刻导航坐标系下的姿态四元数, \boldsymbol{\boldsymbol{Q}}_{b\left(k\right)}^{b\left(k-1\right)} k-1与k时刻之前的四元数变化量,Q为四元数,C为方向余弦矩阵,上角标表示所处坐标系,下角标表示转换坐标系。

    采用惯导比力方程和位置微分方程对速度、位置进行更新:

    {\dot{{\boldsymbol{V}}}}^{n}={{\boldsymbol{C}}}_{b}^{n}{{\boldsymbol{f}}}^{b}-(2{\omega }_{ie}^{n}+{\omega }_{en}^{n})\times {{\boldsymbol{V}}}^{n}+{g}^{n} (4)
    \left\{\begin{aligned} & \dot{L}=\frac{{V}_{\text{N}}^{}}{{R}_{\text{m}}+h}\\ & \dot{\lambda }=\frac{{V}_{\text{E}}^{}}{({R}_{\text{n}}+h)\mathrm{cos}L}\\ & \dot{h}={V}_{\text{U}}^{}\end{aligned}\right. (5)

    式中V为载体速度,m/s;gn为重力加速度在n系下的分量,m/s;RmRn为子午圈半径和卯酉圈半径,m。

    为满足设施园艺移动平台高精度定位要求,本文提出一种基于卡尔曼滤波的自适应抗差组合定位算法,设计一种差分距离非视距误差检测滑动窗口,同时引入余弦重启渐消因子和位置精度因子(position dilution of precision,PDOP)权值抗差因子,对移动平台运动状态进行实时精准估计。

    采用15维状态变量组建导航系统的状态方程,在导航坐标系中表示为

    {{\boldsymbol{X}}_k} = {\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{\phi}} ^{\text{T}}}}&{\delta {{\boldsymbol{V}}^{\text{T}}}}&{\delta {{\boldsymbol{P}}^{\text{T}}}}&{\varepsilon _{\text{g}}^{\text{T}}}&{{\boldsymbol{\nabla}} _{\text{a}}^{\text{T}}} \end{array}} \right]^{\text{T}}} (6)

    式中φT为失准角,δVT为车辆速度误差,δPT为位置误差,{\varepsilon _{\text{g}}^{\text{T}}} 为陀螺角零偏,{\boldsymbol{\nabla}}_{\mathrm{a}}^{\mathrm{T}} 为加速度计零偏。

    结合惯性导航误差微分方程可得系统状态方程为

    {{\boldsymbol{X}}_k} = {\boldsymbol{F}}{{\boldsymbol{X}}_{k - 1}} + {\boldsymbol{G}}{{\boldsymbol{W}}_k} (7)
    \boldsymbol{F}=\left[\begin{array}{*{20}{c}}\boldsymbol{O}_{\text{3}\times\text{3}} & \boldsymbol{O}_{\text{3}\times\text{3}} & \boldsymbol{O}_{\text{3}\times\text{3}} & -\boldsymbol{C}_n^b & \boldsymbol{O}_{\text{3}\times\text{3}} \\ (\boldsymbol{f}_{\text{sf}}^n)^* & \boldsymbol{O}_{\text{3}\times\text{3}} & \boldsymbol{O}_{\text{3}\times\text{3}} & \boldsymbol{O}_{\text{3}\times\text{3}} & \boldsymbol{C}_n^b \\ \boldsymbol{O}_{\text{3}\times\text{3}} & \boldsymbol{I}_{3\times3} & \boldsymbol{O}_{\text{3}\times\text{3}} & \boldsymbol{O}_{\text{3}\times\text{3}} & \boldsymbol{O}_{\text{3}\times\text{3}} \\ \boldsymbol{O}_{\text{3}\times\text{3}} & \boldsymbol{O}_{\text{3}\times\text{3}} & \text{O}_{\text{3}\times\text{3}} & -\boldsymbol{B}_{\text{G}} & \boldsymbol{O}_{\text{3}\times\text{3}} \\ \boldsymbol{O}_{\text{3}\times\text{3}} & \boldsymbol{O}_{\text{3}\times\text{3}} & \boldsymbol{O}_{\text{3}\times\text{3}} & \boldsymbol{O}_{\text{3}\times\text{3}} & -\boldsymbol{B}_{\text{A}}\end{array}\right] (8)

    式中Xk-1k-1时刻下的系统状态;F为运动方程对误差的雅可比矩阵,G为运动方程对噪声的雅可比矩阵。FG与当前时刻的IMU加速度、角速度及前一时刻的旋转有关,Wk为零均值(方差为Qk的高斯白噪声),BGBA分别为陀螺仪测量白噪声和加速度计测量白噪声,{\boldsymbol{f}}^n_{\mathrm{sf}} 为加速度计测量的比力在n系上的投影,( \boldsymbol{\boldsymbol{\boldsymbol{\boldsymbol{f}}}}_{\text{sf}}^n )*表示 \boldsymbol{\boldsymbol{f}}_{\text{sf}}^n 的反对称阵;O为零矩阵,I为单位矩阵。

    将IMU解算的一步位置预测值与UWB测距值解算的位置测量值之间的差值作为卡尔曼滤波器的观测向量,记作Zk。观测向量和对应的量测方程如(9)~(10)。

    \boldsymbol{Z}_k=\left[\boldsymbol{P}_k^{\mathrm{IMU}}-\boldsymbol{P}_k^{\mathrm{UWB}}\right] (9)
    {{\boldsymbol{Z}}_k} = {{\boldsymbol{H}}_k}{{\boldsymbol{X}}_k} + {{\boldsymbol{V}}_k} (10)

    式中Zk为观测向量,\boldsymbol{P}_k^{\mathrm{IMU}} {\boldsymbol{P}}_k^{\mathrm{UWB}} 分别为k时刻下IMU和UWB解算的位置;Hkk时刻下的量测矩阵;Vk为零均值(方差为Rk的高斯白噪声),Hk可表示为

    {{\boldsymbol{H}}_k} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{O}}_{{\text{3}} \times {\text{3}}}}}&{ {{\boldsymbol{I}}_{{\text{3}} \times {\text{6}}}}}&{ {{\boldsymbol{O}}_{{\text{3}} \times {\text{6}}}}} \end{array}} \right] (11)

    卡尔曼滤波的离散形式可表示为

    \left\{\begin{aligned} & {\widehat{{\boldsymbol{X}}}}_{k\text{,}k-1}={{\boldsymbol{\varPhi}} }_{k\text{,}k-1}{\widehat{{\boldsymbol{X}}}}_{k-1}+{{\boldsymbol{W}}}_{k}\\ & {{\boldsymbol{P}}}_{k\text{,}k-1}={{\boldsymbol{\varPhi}} }_{k\text{,}k-1}{{\boldsymbol{P}}}_{k-1}{{\boldsymbol{\varPhi}} }_{k\text{,}k-1}^{\text{T}}+{{\boldsymbol{Q}}}_{k-1}\\ & {{\boldsymbol{K}}}_{k}={{\boldsymbol{P}}}_{k\text{,}k-1}{{\boldsymbol{H}}}_{k}^{\text{T}}({{\boldsymbol{H}}}_{k}{{\boldsymbol{P}}}_{k\text{,}k-1}{{\boldsymbol{H}}}_{k}^{\text{T}}+{{\boldsymbol{R}}}_{k}{)}^{-1}\\ & {{\boldsymbol{P}}}_{k}=({\boldsymbol{I}}-{{\boldsymbol{K}}}_{k}{{\boldsymbol{H}}}_{k}){{\boldsymbol{P}}}_{k\text{,}k-1}\\ & {\widehat{{\boldsymbol{X}}}}_{k}={\widehat{{\boldsymbol{X}}}}_{k\text{,}k-1}+{{\boldsymbol{K}}}_{k}({{\boldsymbol{Z}}}_{k}-{{\boldsymbol{H}}}_{k}{\widehat{{\boldsymbol{X}}}}_{k\text{,}k-1})\end{aligned} \right. (12)

    式中{\hat {\boldsymbol{X}}_{k,k - 1}}k-1到k时刻的状态一步预测值;Φk,k-1为离散化的系统状态转移矩阵;Kkk时刻增益矩阵;Pk,k-1k-1时刻到k时刻的状态一步预测协方差矩阵;Pkk时刻的状态估计协方差矩阵;

    噪声参数不准确会导致系统输出不稳定,因此需要对量测噪声进行辨识估计,假设k时刻下的量测值Zk和量测预测值k,k-1之间存在误差,记作rk,其协方差为Dk,表示如下:

    {{\boldsymbol{r}}_k} = {{\boldsymbol{Z}}_k} - {\hat {\boldsymbol{Z}}_{k,k - 1}} (13)
    {{\boldsymbol{D}}_k} = {{\boldsymbol{H}}_k}{{\boldsymbol{P}}_k}{\boldsymbol{H}}_k^T + {{\boldsymbol{R}}_k} (14)

    式中Rk为量测噪声估计,属于未知量,需要通过Dk对其进行估计,记为{\hat R_k},计算如下:

    {\hat {\boldsymbol{R}}_k} = \left( {1 - \frac{1}{k}} \right){\hat {\boldsymbol{R}}_{k - 1}} + \frac{1}{k}\left( {{{\boldsymbol{r}}_k}{{\boldsymbol{r}}_k}^{\text{T}} - {{\boldsymbol{H}}_k}{{\boldsymbol{P}}_{k,k - 1}}{\boldsymbol{H}}_k^{\text{T}}} \right) (15)

    为了避免k无穷大,引入渐消因子βk=1/k,表达如下

    {\beta _k} = \frac{{{\beta _{k - 1}}}}{{{\beta _{k - 1}} + b}},0 < b < 1 (16)

    式中βk初值为1,b为遗忘因子,取值范围为0.95~0.99[25]

    为了避免噪声干扰,设计一种抗差因子 \mathit{\gamma}_k^i ,计算如下:

    \gamma_k^i=\left(\boldsymbol{r}_k^i\times(\boldsymbol{r}_k^i)^{\mathrm{T}}\right)-\boldsymbol{H}_k^i\boldsymbol{P}_{k,k-1}^i(\boldsymbol{H}_k^i)^{\text{T}} (17)

    式中i为向量或矩阵对角线上的第i个元素。

    最后,利用渐消因子和抗差因子对{\hat R_k}进行修正,计算如下:

    {\widehat{{\boldsymbol{R}}}}_{k}^{i}=(1-{\beta }_{k}){\widehat{{\boldsymbol{R}}}}_{k-1}^{i}+{\beta }_{k}{\gamma }_{k}^{i} (18)

    尽管自适应抗差卡尔曼滤波在处理异常数据和噪声方面具有一定优势,但定位过程中对多种误差源的鲁棒性和实时性仍存在局限性。因此,为提高移动平台在复杂环境中的定位精度和稳定性,本文提出一种自适应抗差组合定位算法。

    在UWB信号传输的过程中,由于路径存在障碍物遮挡,获取到的定位数值中会存在较大的离群值。因此,本文设计一个相邻滑动窗口进行NLOS误差检测,表达式如下

    uwb_k=[\boldsymbol{P}_k^{\mathrm{UWB}},\boldsymbol{P}_{k-1}^{\mathrm{UWB}},\cdots,\boldsymbol{P}_{k-9}^{\mathrm{UWB}}] (19)
    gap = {\left\| {uw{b_{k - 1}} - uw{b_k}} \right\|_2} (20)

    式中uwbk表示kk-9时刻之间的UWB定位数据集合;gap为检测NLOS误差的阈值,gap>0.2时,自适应抗差组合定位算法会跳过k时刻的UWB数据。

    在自适应滤波[26]中,大多采用渐消因子调整量测噪声,但由于其收敛速度较快,导致无法适应量测噪声的变化。因此,为有效利用新的UWB量测信息,本文提出一种余弦重启渐消因子αk,计算如下:

    {\alpha }_{k}=\frac{{\alpha }_{k-1}}{{\alpha }_{k-1}+(1-{b}_{k})} (21)
    {b_k} = \eta + 0.5 \times \left( {{b_{k - 1}} - {\text{η }}} \right) \times \left( {1 + \cos \left( {\frac{{{\text{π }} \times k}}{{{T_{}}}}} \right)} \right) (22)

    式中η为0.01,根据多次试验分析得到Tαk的最佳取值分别为350和0.13。

    PDOP值是估计卫星系统定位精度的指标[27],反应了目标节点与定位基站间几何分布情况。当几何分布数值越小(即PDOP值越小)时,表明观测条件越好,UWB观测信息越可信,定位精度越高。反之,说明此时分布结构较差,则需对UWB观测值进行降权处理。

    由于UWB在设施园艺场景内的定位精度会受到环境与基站布局的影响,因此,本文引入PDOP权值修正抗差因子,表示如下

    \gamma_k^p=\gamma_k\times\mathrm{e}^{\left(-\left(\frac{p_k}{2}-1\right)^2\right)} (23)

    式中 \gamma_k^p 为修正后的抗差因子,pkk时刻的PDOP值。

    在量测更新前使用差分距离滑动窗口检测,并结合式(22)和(23),利用αk \gamma_k^p 分别替换式(16)和(18)中的渐消因子和抗差因子。

    为验证所提组合定位方法的有效性,基于Matlab搭建仿真试验环境。依据移动平台运行参数和传感器特点,主要仿真参数设置如下:移动平台初始坐标为(2.5,2.5,1.5),初始速度为0.2 m/s,速度变化范围为0.1~0.3 m/s,加速度计零偏误差为0.05 m/s2,陀螺仪零偏误差为5 °/h。以设施园艺内常见的四基站布设方式为例,在24 m x 20 m的多跨大棚范围内,布设4个坐标分别为S1(0,0,2)、S2(0,24,2)、S3(20,0,2)、S4(20,24,2)的UWB基站。移动平台作业幅宽为1 m,在大棚1/2区域内,进行往复式全路径规划作业,作业路线如图5所示。

    图  5  定位仿真试验作业路径
    Figure  5.  Working path of positioning simulation test

    通过最小二乘法求解UWB基站距离方程组获得标签坐标,其中测距值包含均值为0、方差为0.1的高斯噪声表示测量过程中的随机误差。为模拟复杂的设施园艺定位环境,随机制造轻度、中度和重度3种非视距误差表示由不同生长阶段的作物、各种结构障碍物等遮挡引起的测距异常值。

    设施园艺场景与作业路径的PDOP值如图6所示。

    图  6  PDOP值仿真结果
    Figure  6.  PDOP (position dilution of precision) value simulation results

    图6a可看出,PDOP值关于区域中心基本呈对称分布,在区域中心PDOP值达到最小值1.11,并由中心区域向两边递增。图6b为作业路径上PDOP值变化情况,移动平台是全路径覆盖作业,需要确保整个作业区域的定位精度。当移动平台在大棚中间作业时的PDOP值低于1.20,定位效果较好。当移动平台在靠近仿真区域的边界移动时,PDOP值加速增加,UWB定位系统不稳定。

    为验证所提UWB-IMU组合定位方法的有效性,对比分析其与单传感器定位的作业轨迹,如图7所示。仿真中IMU采样频率为200 Hz,UWB采样频率为100 Hz。

    图  7  单传感器与UWB-IMU组合定位的仿真轨迹
    Figure  7.  Simulation trajectory of single sensor and combined UWB-IMU localization

    采用单传感器定位方法的误差为36.68 cm,最大误差为254.85 cm,波动较大且定位轨迹出现强烈的锯齿效应。而采用UWB-IMU组合定位方法的定位误差为17.53 cm,最大误差为136.68 cm,作业轨迹偏移程度较小,具有良好的定位效果。相比于单传感器定位,采用UWB-IMU组合定位,定位精度提高了52.2%,最大误差降低了46.37%。

    为验证所提组合定位算法的稳健性,分别选用卡尔曼滤波(kalman filtering,KF)、无迹卡尔曼滤波、扩展卡尔曼滤波、粒子滤波(particle filtering,PF)、自适应抗差卡尔曼(adaptive robust kalman filtering,ARKF)等常用滤波定位算法进行比较。图8为6种不同滤波定位算法获得的平面轨迹。

    图  8  不同算法仿真轨迹对比图
    Figure  8.  Comparison of simulated trajectories with different localization algorithms

    图8可知,虽然粒子滤波算法具有一定稳定性,但在设施场景下出现明显波动,其主要原因是PF在数据融合过程中过度依赖粒子权重更新,UWB的数据波动使粒子权重变化剧烈,导致定位精度下降。在复杂设施场景中,UKF表现最差,因为UKF对过程噪声和测量噪声的敏感性更高,无法有效滤除NLOS误差和几何误差带来的影响,导致滤波更容易发散。而采用ARCPA算法的定位精度远超其余5种算法,定位结果更稳定,更贴合参考轨迹,在面对NLOS误差时表现出更强的鲁棒性。

    图8中局部放大图①为非视距条件下各种算法受NLOS误差影响较大的区域。当出现NLOS误差时,KF、EKF、UKF与PF算法的定位精度均受到较大影响,整体轨迹都出现了严重偏移的现象;ARKF与ARCPA都具有一定的抗差效果,但ARKF随着遮挡时间的增加,定位轨迹逐渐偏离参考路径,且定位误差不断增加,在X轴坐标到达9 m时,Y轴方向误差达到最大为16.09 cm。

    图8中的局部放大图②为视距条件下各种算法受几何误差影响较大的区域。该时刻移动平台正处于仿真区域边界,结合图6可知设施园艺场景边界区域PDOP值较高,这表明定位标签与基站之间的锚节点几何分布程度较差,该处UWB定位系统精度较低。从局部放大图②中可以看出KF、EKF和UKF的作业轨迹发生明显偏离,采用ARKF的作业轨迹更接近参考值,但仍无法实现精准定位。而采用所提ARCPA算法的作业轨迹与参考轨迹的重合度最高,这是由于PDOP修正后的抗差因子调节了预测信息和观测信息的占比,提高了设施园艺移动平台在动态和复杂环境中的适应性和鲁棒性。

    图9统计了6种滤波算法在XY轴和欧式距离上的定位误差。由图9a图9b可知,在视距条件下ARCPA算法定位误差小于10.0 cm。在轻度非视距误差条件下,如图9a中的124~136 s时段,X轴最大定位误差结果仍能保持10.0 cm左右。中度的非视距误差条件下,如图9b中的344~368 s时段,Y轴定位结果仍能保持在20.0 cm以内的偏差。由图9c可知,在重度非视距误差条件下,最大定位误差保持在20.0 cm左右。这表明采用所提ARCPA算法在X轴和Y轴误差明显低于其他5种算法,能有效抑制几何误差和NLOS误差对组合定位系统的干扰。

    图  9  不同算法的定位误差对比
    Figure  9.  Comparison of localization errors of different algorithms

    为充分验证所提定位方法的有效性,统计分析了不同定位方法的误差,如表1所示。相比于单传感器定位,采用所提UWB-IMU组合定位方法并使用ARCPA算法的定位精度提高了81.92%,最大误差降低了88.89%。相比于传统的卡尔曼滤波算法,采用所提ARCPA算法的位置均值误差降低46.89%,均方误差降低86.97%,最大误差降低1.0836 m,均方根误差降低62.18%。所提ARCPA算法定位精度和稳定性均优于其他5种组合定位算法。

    表  1  不同定位方法误差统计表
    Table  1.  Error statistics of different positioning methods
    定位方法
    Positioning method
    位置偏差Positional deviation /cm
    均值误差
    Mean
    error
    均方误差
    Mean squared
    error
    最大误差
    Maximum
    error
    均方根误差
    Root mean
    square error
    单传感器
    Single Sensor
    UWB 23.77 27.94 254.85 36.683
    组合定位
    Integrated localization
    KF 9.79 3.07 136.68 17.53
    UKF 18.69 9.22 204.26 30.36
    EKF 10.59 2.32 72.76 15.22
    PF 24.15 7.86 161.46 28.03
    ARKF 5.94 0.65 34.55 8.07
    ARCPA 5.20 0.40 28.32 6.63
    下载: 导出CSV 
    | 显示表格

    为验证本文所提定位方法在设施场景下的有效性,开展了设施园艺电驱动移动平台(见图10a)定位试验。试验场景为江苏省康博农业科技有限公司的设施园艺基地,如图10b所示。

    图  10  设施园艺电驱移动平台实车定位试验
    Figure  10.  Protected horticulture electric drive mobile platform real vehicle positioning test

    UWB数据经STM32单片机计算得到测距值,并通过串口进行传输,IMU传感器将采集到的加速度和角速度等运动数据通过RS485通信协议进行传输和解析,通过上位机实时处理并记录UWB和IMU数据。

    大棚内的高温高湿环境会对UWB信号传播产生额外误差,特别是高湿环境对UWB波段信号具有吸收作用。此外,UWB内部模块存在的天线延时和算法延时,也会导致UWB信号测量产生偏差。为提高UWB测距精度,以激光测距仪的测距值作为真实值,通过测距试验获得真实距离和UWB测距值之间的映射关系,对视距通信下的UWB误差进行矫正。

    将基站固定于支架上,移动定位标签,每次间隔3 m,分别获取UWB和激光测距仪每个采样位置上的100个测距值,并将原始UWB测距数据和真实值使用最小二乘法进行拟合,结果如图11所示。

    图  11  UWB测距误差标定结果
    Figure  11.  UWB ranging error calibration results

    经过计算,视距条件下UWB原始测距结果的均方根误差(root mean squared error,RMSE)为1.19 m,修正后的均方根误差为0.49 m。相比于修正前,测距精度提高了58.8%。

    将4个UWB基站安装在大棚区域的四角,其坐标分别为(0,0,1.5)、(0,16,1.5)、(0,24,1.5)和(20,24,1.5)m。将UWB定位标签和IMU分别安装于设施园艺移动平台顶部和底部,移动平台沿着预设的轨迹匀速缓慢行驶,在预设路径上布设若干个检核点用于评价定位精度,利用激光测距仪确定检核点坐标。作业途中由行人、轮胎和金属铁板制造NLOS误差(如图12b所示)。行人身高1.75 m,在检核点27周围移动;轮胎遮挡则通过在检核点18旁1 m处架设直径0.6 m轮胎;金属铁板长1 m、宽0.7 m,固定在检核点23距离0.5 m的梯子上,高度为1.5 m。根据试验结果,行人遮挡引起的误差范围为0.4~0.7 m,轮胎遮挡引起的误差范围为1.0~1.3 m,铁门遮挡引起的误差大于1.7 m,误差统计如表2所示。

    图  12  动态定位试验PDOP值
    Figure  12.  Dynamic localization of test path PDOP values
    表  2  动态定位试验非视距误差统计
    Table  2.  Non line of sight error statistics in dynamic positioning test
    场景
    Scenario
    遮挡物
    Obscuring objects
    非视距误差
    Non line of sight error/m
    轻度行人0.4~0.7
    中度轮胎1~1.3
    重度金属铁板≥1.7
    下载: 导出CSV 
    | 显示表格

    设施园艺移动平台行驶的过程中的PDOP值如图12所示。由图12可知,布设4个基站时PDOP值与仿真结果(图6b)基本相同,这表明中心区域的UWB锚节点分布程度较好。图12中红色区域表示受NLOS误差影响的区域,可以看出PDOP值出现明显波动,此时由于金属铁板对UWB信号的遮挡,造成了严重的NLOS误差,这使得测距数据产生偏差,并直接影响基站几何分布,从而引发PDOP值的波动,表明UWB受到外界干扰导致的定位系统精度下降。

    为进一步分析所提ARCPA算法的定位精度,设计不同定位算法对比试验,得到的轨迹和检核点误差分别如图13图14所示。

    图  13  动态定位试验结果
    Figure  13.  Dynamic positioning test results
    图  14  动态定位试验检核点误差
    Figure  14.  Check point error of dynamic positioning test

    图13图14可知,UKF和PF在设施场景下表现出较大的波动和偏离,整体误差较高。EKF和KF存在一定的局限性,特别是在复杂环境和干扰较强的区域,误差峰值高于100.0 cm。相比之下,ARKF和ARCPA算法则表现出更为优越的性能。ARKF在严重几何误差和复杂NLOS场景下,其自适应机制无法完全消除估计误差。总体而言,ARCPA在整个试验过程中均表现出较高的精度和稳定性,且误差基本维持在20.0 cm以下,具有更好的自适应抗差效果。

    图13中的行人遮挡区域是受几何误差影响较大的区域,对应图14检核点的26、27、28处。结合图13图14可知,此时UKF与PF定位数据出现明显波动,定位精度下降;KF、EKF与ARKF具有一定的降噪作用但无法滤除UWB几何误差的干扰,轨迹出现偏离;ARCPA整体误差最小、轨迹更平滑,符合实际定位情况。

    图13中的金属铁板遮挡区域和轮胎遮挡区域,分别为重度和中度遮挡区域,对应图14检核点的18、19和23、24处。在受到NLOS误差影响的区域内,各算法的作业轨迹均出现了明显的偏移,UKF和PF算法出现较大的误差波动。在重度遮挡区域,KF、EKF、PF与UKF的最大定位误差分别为133.96143.67154.31168.78 cm,表现出对复杂环境的适应性不足。虽然ARKF整体误差较低,但作业轨迹存在一定波动,在重度遮挡区域内的最大误差接近100.0 cm,这表明ARKF自适应机制在面对高度几何误差和严重NLOS遮挡叠加时仍存在局限性。相比之下,ARCPA整体误差最低、波动最小,在复杂设施园艺场景下具有高精度和强鲁棒性。

    表3为动态定位试验的定位精度统计结果。其中,ARCPA的RMSE为17.70 cm,相比于KF、UKF、EKF、PF、ARKF分别减小了50.15%、74.63%、60.01%、60.88%、28.34%。ARCPA的最大定位误差相较于KF、UKF、EKF、PF、ARKF分别下降了66.83%、73.67%、69.07%、71.2%、50.50%。在复杂的设施园艺定位场景下,ARCPA算法仍能保持较高的稳定性和定位精度。

    表  3  动态定位试验不同算法最大误差和均方根误差统计
    Table  3.  Statistics of maximum error and root mean square error of different algorithms in dynamic positioning test
    定位算法
    Positioning algorithms
    位置偏差Positional deviation /cm
    最大误差
    Maximum error
    均方根误差
    Root-mean-square error
    KF 133.96 35.51
    UKF 168.78 69.78
    EKF 143.67 44.26
    PF 154.31 45.25
    ARKF 89.77 24.70
    ARCPA 44.44 17.70
    下载: 导出CSV 
    | 显示表格

    6种定位算法在不同运算平台的性能对比如表4所示。

    表  4  不同算法性能对比
    Table  4.  Performance comparison of different algorithms
    定位算法Positioning algorithms 运算平台A
    Operational platform A
    (Intel Core i7-13650HX)
    运算平台B
    Operational platform B
    (AMD Ryzen 5 7645HX)
    平均运行
    时间
    Average
    run time
    /ms
    处理器
    使用率
    Central processing
    unit utilization rate/%
    平均运行
    时间
    Average
    run time
    /ms
    处理器
    使用率
    Central processing
    unit utilization rate/%
    KF 72.4 25.2 77.7 33.9
    UKF 128.4 58.7 144.7 59.8
    EKF 109.6 45.9 119.3 55.7
    PF 148.1 50.5 188.5 60.2
    ARKF 83.8 40.2 92.9 46.5
    ARCPA 96.3 35.6 107.2 50.1
    下载: 导出CSV 
    | 显示表格

    结果显示,ARCPA算法在2种处理器上均具有良好的性能,平均运行时间为96.3和107.2 ms,表明ARCPA算法在计算复杂度和性能需求之间取得了良好的平衡。同时,ARCPA算法的处理器使用率分别为35.6%和50.1%,表明其资源利用的高效性以及运行稳定性,适用于移动平台在设施场景下的实时定位部署。

    为解决设施场景下遮挡严重所致的电驱移动平台定位精度低、稳定性差的问题,本文设计了一种适合设施园艺电驱移动平台的多传感器组合定位方法(UWB/IMU),并提出了一种自适应抗差组合定位算法,为验证所提方法的有效性,搭建了仿真与试验平台,并对应开展了设施园艺电驱移动平台的定位方法对比试验。主要结论如下:

    1)所提高精度组合定位方法可显著降低NLOS误差和几何误差对设施园艺电驱移动平台定位精度的影响,单传感器定位精度为36.68 cm,最大定位误差为254.85 cm,而采用所提UWB/IMU组合定位方法的定位精度提高了81.92%,最大误差降低了88.89%。

    2)实车定位试验表明,所提组合定位方法即使在复杂设施园艺作业场景下,仍能保持较高的稳健性,相较于常规卡尔曼滤波、无迹卡尔曼滤波、扩展卡尔曼滤波、粒子滤波和抗差自适应卡尔曼滤波,所提定位方法精度分别提升了50.15%、74.63%、60.01%、60.88%、28.34%。

    本文所设计的设施园艺电驱移动平台组合定位方法和提出的自适应抗差组合定位算法,能够为设施园艺大棚中作业的移动平台提供稳定且精确的定位的信息,对于提高设施园艺电驱移动平台的作业质量和作业效率具有重要实际生产指导意义。

  • 图  1   设施园艺移动平台整体结构及其驱动系统

    Figure  1.   Overall structure and drive system of mobile platform for protected horticulture

    图  2   设施园艺UWB定位系统

    注:Ob为载体坐标系,固连于移动平台,按“右前上”顺序定义,Xb沿载体横轴指向载体右侧,Yb沿载体横轴指向载体前方,Zb垂直XbYb所在平面向上。On为导航坐标系,固定于基站S1的地面投影区域,Xn沿大棚横向指向大棚右侧,Yn沿大棚纵向指向大棚前方,Zn垂直XnYn所在平面向上;d1d2d3d4为定位标签到基站S1,S2,S3,S4距离,m。

    Figure  2.   Protected horticulture UWB positioning system

    Note: Ob is the carrier coordinate system, fixed to the mobile platform, defined in the order of “right front up”, with Xb pointing to the right side of the carrier along the transverse axis of the carrier, Yb pointing to the front of the carrier along the transverse axis of the carrier, and Zb perpendicularly upward in the plane where Xb and Yb are located. On is the navigation coordinate system fixed to the projection area of the base station S1 on the ground, Xn points to the right side of the shed along the transverse axis of the shed, Yn points to the front side of the shed along the longitudinal axis of the shed, and Zn is perpendicular to the plane upward where Xn and Yn are located; d1, d2, d3, and d4 denote the distances of the positioning tags to the base station S1, S2, S3, S4, m.

    图  3   三球交汇定位原理图

    Figure  3.   Three-ball intersection positioning schematic

    图  4   IMU定位解算原理

    注:λL分别表示经度和纬度,(°);h表示高度,m;φθγ表示俯仰角、航向角、横滚角,rad;VEVNVU表示东、北、天方向速度分量,m·s-1 \boldsymbol{\boldsymbol{f}}^{\mathit{b}} \boldsymbol{\boldsymbol{f}}^{\mathit{n}} 表示b系和n系下的比力投影,m·s-2 \mathit{\omega}_{ie}^n 表示地球自转角速度矢量在导航坐标系中的投影,rad·s-1 {{{\omega}{_{en}^n}{{{ }}}}{ }} 表示导航坐标系下,地球自转和地球表面运动的角速度,rad·s-1 {\omega_{in}^b} 表示载体坐标系下,导航坐标系相对惯性坐标系的角速度,rad·s-1 \omega_{nb}^b 表示载体坐标系下,导航坐标系相对载体的角速度,rad·s-1 \omega_{in}^n 表示导航坐标系下,载体坐标系相对于惯性坐标系的角速度,rad·s-1

    Figure  4.   IMU (inertial measurement unit) positioning calculation principle

    Note: λ, L represent longitude, latitude, respectively, (°); h represent altitude, m; φθ,and γ denote the pitch angle, yaw angle, and roll angle, respectively, rad; VE, VN, and VU are the velocity components in east, north, and up directions, respectively, m·s-1; {\boldsymbol{f}}^b and \boldsymbol{f}^n refer to specific force projections in b-frame and the n-frame, respectively, m·s-2; \omega_{ie}^n represents projection of the Earth's rotation angular velocity vector in navigation coordinate system, rad·s-1; \omega_{en}^n denotes angular velocity in navigation coordinate system due to both Earth's rotation and movement on Earth's surface, rad·s-1; \omega_{in}^b indicates angular velocity of navigation coordinate system relative to inertial coordinate system in body coordinate system, rad·s-1; \omega_{nb}^b denotes angular velocity of navigation coordinate system relative to body in body coordinate system, rad·s-1; \omega_{in}^n represents angular velocity of body coordinate system relative to inertial coordinate system in the navigation coordinate system, rad·s-1.

    图  5   定位仿真试验作业路径

    Figure  5.   Working path of positioning simulation test

    图  6   PDOP值仿真结果

    Figure  6.   PDOP (position dilution of precision) value simulation results

    图  7   单传感器与UWB-IMU组合定位的仿真轨迹

    Figure  7.   Simulation trajectory of single sensor and combined UWB-IMU localization

    图  8   不同算法仿真轨迹对比图

    Figure  8.   Comparison of simulated trajectories with different localization algorithms

    图  9   不同算法的定位误差对比

    Figure  9.   Comparison of localization errors of different algorithms

    图  10   设施园艺电驱移动平台实车定位试验

    Figure  10.   Protected horticulture electric drive mobile platform real vehicle positioning test

    图  11   UWB测距误差标定结果

    Figure  11.   UWB ranging error calibration results

    图  12   动态定位试验PDOP值

    Figure  12.   Dynamic localization of test path PDOP values

    图  13   动态定位试验结果

    Figure  13.   Dynamic positioning test results

    图  14   动态定位试验检核点误差

    Figure  14.   Check point error of dynamic positioning test

    表  1   不同定位方法误差统计表

    Table  1   Error statistics of different positioning methods

    定位方法
    Positioning method
    位置偏差Positional deviation /cm
    均值误差
    Mean
    error
    均方误差
    Mean squared
    error
    最大误差
    Maximum
    error
    均方根误差
    Root mean
    square error
    单传感器
    Single Sensor
    UWB 23.77 27.94 254.85 36.683
    组合定位
    Integrated localization
    KF 9.79 3.07 136.68 17.53
    UKF 18.69 9.22 204.26 30.36
    EKF 10.59 2.32 72.76 15.22
    PF 24.15 7.86 161.46 28.03
    ARKF 5.94 0.65 34.55 8.07
    ARCPA 5.20 0.40 28.32 6.63
    下载: 导出CSV

    表  2   动态定位试验非视距误差统计

    Table  2   Non line of sight error statistics in dynamic positioning test

    场景
    Scenario
    遮挡物
    Obscuring objects
    非视距误差
    Non line of sight error/m
    轻度行人0.4~0.7
    中度轮胎1~1.3
    重度金属铁板≥1.7
    下载: 导出CSV

    表  3   动态定位试验不同算法最大误差和均方根误差统计

    Table  3   Statistics of maximum error and root mean square error of different algorithms in dynamic positioning test

    定位算法
    Positioning algorithms
    位置偏差Positional deviation /cm
    最大误差
    Maximum error
    均方根误差
    Root-mean-square error
    KF 133.96 35.51
    UKF 168.78 69.78
    EKF 143.67 44.26
    PF 154.31 45.25
    ARKF 89.77 24.70
    ARCPA 44.44 17.70
    下载: 导出CSV

    表  4   不同算法性能对比

    Table  4   Performance comparison of different algorithms

    定位算法Positioning algorithms 运算平台A
    Operational platform A
    (Intel Core i7-13650HX)
    运算平台B
    Operational platform B
    (AMD Ryzen 5 7645HX)
    平均运行
    时间
    Average
    run time
    /ms
    处理器
    使用率
    Central processing
    unit utilization rate/%
    平均运行
    时间
    Average
    run time
    /ms
    处理器
    使用率
    Central processing
    unit utilization rate/%
    KF 72.4 25.2 77.7 33.9
    UKF 128.4 58.7 144.7 59.8
    EKF 109.6 45.9 119.3 55.7
    PF 148.1 50.5 188.5 60.2
    ARKF 83.8 40.2 92.9 46.5
    ARCPA 96.3 35.6 107.2 50.1
    下载: 导出CSV
  • [1]

    ZHANG M R, HAN Y T, Li D P, et al. Smart horticulture as an emerging interdisciplinary field combining novel solutions: past development, current challenges, and future perspectives[J]. Horticultural Plant Journal, 2023, 21(2): 212-218.

    [2] 付学谦,杨菲菲,周亚中,等. 设施农业能源互联网智能预警理论:评述与展望[J]. 农业工程学报,2021,37(21):24-33. doi: 10.11975/j.issn.1002-6819.2021.21.004

    FU Xueqian, YANG Feifei, ZHOU Yazhong, et al. Intelligent early warning theory of the facility agricultural energy internet: Review and prospect[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2021, 37(21): 24-33. (in Chinese with English abstract) doi: 10.11975/j.issn.1002-6819.2021.21.004

    [3]

    YAO L, HU D, ZHAO C, et al. Wireless positioning and path tracking for a mobile platform in greenhouse[J]. International Journal of Agricultural and Biological Engineering, 2021, 14(1): 216-223. doi: 10.25165/j.ijabe.20211401.5627

    [4]

    JONES M H, BELL J, DREDGE D, et al. Design and testing of a heavy-duty platform for autonomous navigation in kiwifruit orchards[J]. Biosystems Engineering, 2019, 187: 129-146. doi: 10.1016/j.biosystemseng.2019.08.019

    [5]

    REN X, HUANG B, YIN H. A review of the large-scale application of autonomous mobility of agricultural platform[J]. Computers and Electronics in Agriculture, 2023, 206: 107628. doi: 10.1016/j.compag.2023.107628

    [6] 钟银,薛梦琦,袁洪良. 智能农机GNSS/INS组合导航系统设计[J]. 农业工程学报,2021,37(9):40-46. doi: 10.11975/j.issn.1002-6819.2021.09.005

    ZHONG Yin, XUE Mengqi, YUAN Hongliang. Design of the GNSS/INS integrated navigation system for intelligent agricultural machinery[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2021, 37(9): 40-46. (in Chinese with English abstract) doi: 10.11975/j.issn.1002-6819.2021.09.005

    [7]

    HUANG Z, SHIIGI T, TSAY L W J, et al. A sound-based positioning system with centimeter accuracy for mobile robots in a greenhouse using frequency shift compensation[J]. Computers and Electronics in Agriculture, 2021, 187: 106235. doi: 10.1016/j.compag.2021.106235

    [8]

    ZHANG W, GONG L, HUANG S H, et al. Factor graph-based high-precision visual positioning for agricultural robots with fiducial markers[J]. Computers and Electronics in Agriculture, 2022, 201: 107295. doi: 10.1016/j.compag.2022.107295

    [9]

    VROEGINDEWEI J, JORISVAN H, ELDERT J, et al. Probabilistic localisation in repetitive environments: Estimating a robot’s position in an aviary poultry house[J]. Computers and Electronics in Agriculture, 2016, 124: 303-317. doi: 10.1016/j.compag.2016.04.019

    [10] 姚立健,杨自栋,夏坡坡,等. 基于超宽带无线定位的农业设施内移动平台路径跟踪研究[J]. 农业工程学报,2019,35(2):17-24. doi: 10.11975/j.issn.1002-6819.2019.02.003

    YAO Lijian, YANG Zidong, XIA Popo, et al. Path tracking of mobile platform in agricultural facilities based on ultra wideband wireless positioning[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(2): 17-24. (in Chinese with English abstract) doi: 10.11975/j.issn.1002-6819.2019.02.003

    [11] 解开婷,张兆国,王法安,等. 基于全质心WLS-HDS-TWR算法的无人驾驶农业机械协同定位算法研究[J]. 农业机械学报,2024,55(7):27-36,110. doi: 10.6041/j.issn.1000-1298.2024.07.003

    XIE Kaiting, ZHANG Zhaoguo, WANG Fa'an, et al. Cooperative localization algorithm for full center mass of WLS-HDS-TWR driverless agricultural machines[J]. Transactions of the Chinese Society for Agricultural Machinery, 2024, 55(7): 27-36,110. (in Chinese with English abstract) doi: 10.6041/j.issn.1000-1298.2024.07.003

    [12] 陈兆,王玫,周陬,等. 基于多参数融合的超宽带基站布设优化方法[J]. 电子学报,2023,51(10):2855-2865.

    CHEN Zhao, WANG Mei, ZHOU Zhou, et al. Optimization method for ultra-wideband base station configuration based on multi-parameter fusion[J]. Acta Electronica Sinica, 2023, 51(10): 2855-2865. (in Chinese with English abstract)

    [13]

    LI D, WANG X, CHEN D, et al. A precise ultra-wide band ranging method using pre-corrected strategy and particle swarm optimization algorithm[J]. Measurement, 2022, 194: 110966. doi: 10.1016/j.measurement.2022.110966

    [14]

    CHOWDHURY B D B , KUBOTA C , et al. Automated workflow analysis in vegetable grafting using an Ultra-Wide Band based real-time indoor location tracking system[J]. Computers and Electronics in Agriculture, 2022, 194: 106773.

    [15]

    YU K, WEN K, LI Y, et al. A novel NLOS mitigation algorithm for UWB localization in harsh Indoor environments[J]. IEEE Transactions on Vehicular Technology, 2018: 686-699.

    [16] 毕松,张国轩,李志军,等. 基于测距值修正的温室植保机器人定位方法[J]. 农业机械学报,2023,54(8):347-358. doi: 10.6041/j.issn.1000-1298.2023.08.034

    BI Song, ZHANG Guoxuan, LI Zhijun, et al. Positioning method of greenhouse plant protection robot based on distance measurement value correction[J]. Transactions of the Chinese Society for Agricultural Machinery, 2023, 54(8): 347-358. (in Chinese with English abstract) doi: 10.6041/j.issn.1000-1298.2023.08.034

    [17] 尹康涌,梁伟,杨吉斌,等. 电力作业场景中一种高效的UWB和IMU融合定位算法[J]. 中国电力,2021,54(8):83-90.

    YIN Kangyong, LIANG Wei, YANG Jibin , et al. An efficient positioning algorithm based on UWB and IMU fusion in electric power operation scenes[J]. Electric Power, 2021, 54(8): 83-90. (in Chinese with English abstract)

    [18] 黄卫华,梅宇恒,章政,等. 基于改进SHKF算法的UWB/IMU组合定位方法[J]. 中国惯性技术学报,2024,32(1):34-41.

    HUANG Weihua, MEI Yuheng, ZHANG Zheng, et al. UWB/IMU combined positioning method based on improved SHKF algorithm[J]. Journal of Chinese Inertial Technology, 2024, 32(1): 34-41. (in Chinese with English abstract)

    [19] 杨秀建,皇甫尚昆,颜绍祥. 基于改进UKF的UWB/IMU/里程计融合定位方法[J]. 中国惯性技术学报,2023,31(5):462-471.

    YANG Xiujian, HUANGFU Shangkun, YAN Shaoxiang, et al. Fusion positioning method with UWB/IMU/odometer based on the improved UKF[J]. Journal of Chinese Inertial Technology, 2023, 31(5): 462-471. (in Chinese with English abstract)

    [20] 徐爱功,杜健,隋心,等. 零速修正与完备监测在UWB/MEMS IMU组合中的应用[J]. 测绘科学,2022,47(12):1-7.

    XU Aigong, DU Jian, SUI Xin, et al. Application of zero velocity update and integrity monitoring in UWB/MEMS IMU combination[J]. Science of Srveuying and Mapping, 2022, 47(12): 1-7. (in Chinese with English abstract)

    [21] 姚露,聂晓根,黄汉阳,等. 无迹卡尔曼滤波算法对UWB/IMU组合定位的研究[J/OL]. 机械科学与技术,(2023-06-04)[2024-09-07]. https://link.cnki.net/doi/10.13433/j.cnki.1003-8728.20230271

    Yao Lu, Nie Xiaogen, Huang Hanyang, et al. Research on UWB / IMU combined location based on Unscented Kalman filter algorithm[J/OL]. Mechanical Science and Technology for Aerospace Engineering, (2023-06-04) [2024-09-07]. https://link.cnki.net/doi/10.13433/j.cnki. 1003-8728.20230271. (in Chinese with English abstract)

    [22] 邱权,胡青含,樊正强,等. 基于自适应系数卡尔曼滤波的农业移动机器人组合定位[J]. 农业机械学报,2022,53(S1):36-43. doi: 10.6041/j.issn.1000-1298.2022.S1.004

    QIU Quan, HU Qinhan, FAN Zhengqiang, et al. Adaptive-coefficient kalman filter based combined positioning algorithm for agricultural mobile robots[J]. Transactions of the Chinese Society for Agricultural Machinery, 2022, 53(S1): 36-43. (in Chinese with English abstract) doi: 10.6041/j.issn.1000-1298.2022.S1.004

    [23]

    MA W, FANG X, LIANG L, et al. Research on indoor positioning system algorithm based on UWB technology[J]. Measurement: Sensors, 2024, 33: 101121. doi: 10.1016/j.measen.2024.101121

    [24] 袁洪良,杨浚宇,唐睿,等. 基于低成本惯导和运动约束的农机高精度定位系统研究[J]. 农业机械学报,2023,54(7):17-25. doi: 10.6041/j.issn.1000-1298.2023.07.002

    YUAN Hongliang, YANG Junyu, TANG Rui, et al. High-precision localization of autonomous agricultural machinery using low-cost IMU and motion constraints[J]. Transactions of the Chinese Society for Agricultural Machinery, 2023, 54(7): 17-25. (in Chinese with English abstract) doi: 10.6041/j.issn.1000-1298.2023.07.002

    [25] 王威,宫淑丽,李晓明. 基于IGWO-STCKF的目标跟踪算法[J/OL]. 北京航空航天大学学报,(2024-8-27) [2024-11-20]. https://link.cnki.net/doi.org/10.13700/j.bh.1001-5965.2024.0623.

    Wang Wei, Gong Shuli, Li Xiaoming. Target tracking algorithm based on IGWO-STCKF[J/OL]. Journal of Beijing University of Aeronautics and Astronautics, (2024-8-27) [2024-11-20]. https://link.cnki.net/doi.org/10.13700/j.bh.1001-5965.2024.0623. (in Cinese with English abstract)

    [26]

    LI S, ZHANG M, JI Y, et al. Agricultural machinery GNSS/IMU-integrated navigation based on fuzzy adaptive finite impulse response Kalman filtering algorithm[J]. Computers and Electronics in Agriculture, 2021, 191: 106524. doi: 10.1016/j.compag.2021.106524

    [27] 李得海,吴文坛,马会林,等. 室内测距型基站组网定位性能分析[J/OL]. 武汉大学学报(信息科学版),(2023-06-07)[2024-09-07]. https://link.cnki.net/doi/10.13203/j.whugis20220513

    Li Dehai, Wu Wentan, Ma Huilin, et al. Positioning Performance of the Indoor Networks of Range-based Reference Stations[J/OL]. Geomatics and Information Science of Wuhan University, (2023-0607) [2024-09-07]. https://link.cnki.net/doi/10.13203/j.whugis20220513. (in Chinese with English abstract)

图(14)  /  表(4)
计量
  • 文章访问数:  59
  • HTML全文浏览量:  9
  • PDF下载量:  20
  • 被引次数: 0
出版历程
  • 收稿日期:  2024-09-11
  • 修回日期:  2024-11-18
  • 网络出版日期:  2024-11-21
  • 刊出日期:  2025-04-14

目录

/

返回文章
返回