基于测距值修正的温室植保机器人定位方法

毕 松 张国轩 李志军 胡福文

(1.北方工业大学电气与控制工程学院, 北京 100144; 2.北方工业大学机械与材料工程学院, 北京 100144)

0 引言

温室内作物种植密集,工作空间狭小,生产过程以人工作业为主。温室生产过程智能化和自动化装备的应用可有效降低在温室高温潮湿环境下的劳动强度,提升我国设施农业竞争力。

植保作业是农业生产过程的必要环节,自动化植保作业可有效降低农业生产过程中的人工需求[1]并有效提升温室农业生产效率。自主导航是温室植保机器人自动化作业的基础,路径规划和路径跟踪是自主导航的基础[2],在温室内提供稳定的高精度实时定位是温室植保机器人自主运行的基础[3]。因此温室内移动机器人的高精度定位技术是实现植保机器人自动化作业的支撑技术。

GPS信号在室内精度低,无法用于温室环境下的机器人定位[4-5]。当工作环境存在频繁遮挡、环境纹理或结构相似等条件时,基于机器人自身携带的视觉或激光传感器进行定位的SLAM技术无法获得准确的定位信息[6],因此基于SLAM的定位技术不适用于温室机器人自主导航[7-8]。UWB技术具有时间分辨率高、抗多径效应能力强等优点,视距通讯下有着较高的定位精度[9-10],已被广泛应用于室内高精度定位,但在通信节点被遮挡的条件下,其定位精度快速下降。在温室机器人运行过程中,UWB定位节点将频繁出现非视距通信状态,使得UWB节点间的测距数据产生较大的波动。因此在温室内单独使用UWB系统的定位数据稳定性较差[11]。惯性导航定位方法以机器人运动状态为基础,通过估计机器人的相对位移实现定位,定位过程不受温室环境特点影响,对短期内位置变化估计准确,但由于该方法存在误差累积,因此长期定位精度较低[12-13]。

UWB定位方法能够提供温室环境内的高精度定位但受遮挡影响,惯性导航定位不受环境限制但长时间定位精度较低。在温室环境下,UWB和IMU数据的有效融合可为温室机器人作业过程提供稳定的高精度定位数据[14]。

UWB具有较高时间分辨率[15],这使得UWB比其他无线通信技术有更好的测距精度,是目前室内高精度定位的主要方法[16]。视距条件下UWB定位精度达到了分米级[17],但在非视距情况下UWB无线定位效果较差。为了提高温室环境下UWB的定位精度,林相泽等[18]通过K-means聚类和截断处理的方法优化了温室内定位,但并未考虑非视距遮挡条件下样本在整体偏离真实值的情况。王俊等[19]提出了一种基于遗传BP算法的温室无线传感器网络定位方法建立UWB的RSSI测距模型,该方法考虑了环境对RSSI测距模型的影响,但未考虑视距和非视距状态下测距模型的差异。温室环境条件下非视距通信频繁,非视距通信的多径效应导致测距结果误差较大[20]。因此,在频繁出现非视距的条件下,单纯依靠UWB进行定位具有局限性,定位精度较低且稳定性差[21],不能满足温室自主导航的定位要求。

将不同的传感器数据进行优势融合可以有效提高系统的定位精度和稳定性,为目前室内定位的主要研究方向。王新等[22]将惯性传感器和UWB原始测量数据进行融合,提出了一种基于多元数据融合理论的温室定位优化方法,相较于只使用UWB定位的方法提高了室内定位精度。郁露等[23]考虑了环境对UWB测距模型的影响,利用LSSVM模型修正UWB原始测距结果,但使用单一模型来对视距通信(LOS)和非视距通信(NLOS)两种条件下的测距数据进行修正,其定位精度提升有限。HAN等[24]和FENG等[25]通过滤波算法实现了IMU数据和UWB数据的融合,通过降低非视距通信下的异常测量数据在定位结果的权重来提升定位系统的稳定性和定位精度,但未对异常数据进行建模分析,因此该方法对定位精度提升有限。相较于单独使用UWB系统的温室内定位方法,基于UWB测距数据和IMU测量数据的融合定位方法可获得更高的定位精度。

机器人在温室运行过程中会处于频繁的非视距通信和视距通信的切换之中,导致在温室机器人作业过程中定位节点测距数据可靠性降低。现有的融合定位算法以UWB节点间的可靠测距数据为基础得到可靠的定位结果,因此实现温室环境下的可靠定位需要提升UWB测距数据的可靠性。

针对温室环境特点和现有的UWB-IMU融合定位方法,本文提出一种UWB测距修正方法,通过两种误差修正模型对UWB原始测距结果进行修正,提高UWB测距数据的稳定性,降低测距误差,以解决温室环境下UWB节点之间频繁出现非视距通信导致UWB系统定位精度低和稳定性差的问题。

1 方法与模型

1.1 基于测距值修正的融合定位方法整体结构

温室内作物种植密集,机器人移动过程中,安装在机器人上的UWB定位节点与安装在温室内的UWB定位基站之间存在视距和非视距通信状态的频繁切换,机器人在温室作业过程中获得的UWB测距数据稳定性较差,由于IMU测量不受温室环境中非视距的影响,本文通过提升UWB测距精度和稳定性,并利用IMU和UWB数据融合,实现温室环境移动过程中的可靠定位。数据融合需统一测量数据坐标系,本文中定位系统的坐标关系如图1所示。

图1 定位系统坐标关系示意图Fig.1 Schematic of coordinate relations of positioning system

图1中,A0、A1、A2、A3为UWB定位基站,位于定位场地的四角;坐标系On为导航坐标系,固定于基站A0在温室环境地面的投影;坐标系Ob为机体坐标系,固定于机器人机体,机体坐标系和导航坐标系并不重合。

惯性定位系统通过测量移动物体的加速度和角速度,经过运动解算后得到定位数据。捷联式惯性导航系统测量基于机体坐标系的加速度,因此定位时需要将机体坐标系b下的加速度测量结果变换到导航坐标系n下。本文中IMU与UWB定位节点直接连接,并固定于待定位机器人机体,加速度测量坐标系与机体坐标系b平行并随机体坐标系进行运动。

UWB定位系统通过测量定位节点与基站的距离实现定位,因此温室环境对UWB定位系统的影响主要体现在测距阶段,降低UWB定位标签与定位基站之间的测距误差可提高定位精度。受温室环境影响,非视距通信条件下UWB原始测量结果精度较低,因此,在进行位置解算之前,需要对UWB测量进行修正。

基于双程TOA的距离测量方法通过测量UWB信号的飞行时间实现测距[26]。视距条件下,UWB信号在基站和定位标签点之间沿直线传播,信号飞行时间与二者之间的直线距离呈线性关系;非视距条件下,UWB信号经过反射或穿透遮挡到达接收端的过程中,其飞行路径受遮挡物大小、材质等诸多因素影响而存在随机性,信号飞行时间与二者之间的距离不是简单线性关系,因此非视距和视距通信条件下的修正模型不同。对UWB测距结果进行修正之前,首先需要根据通信类型对测量结果进行分类,然后分别对两类UWB测量结果建立修正模型并进行修正。

基于UWB测距值修正的UWB-IMU融合定位方法的整体结构如图2所示。

图2 基于UWB测距值修正的UWB-IMU融合定位方法Fig.2 UWB-IMU fusion localization method based on UWB range value correction

1.2 UWB通信类型判别方法

视距与非视距通信下的UWB测距误差产生原因不同,需要使用不同的模型进行修正。因此,修正前应对测量结果的通信类型进行判别。当出现严重遮挡时,UWB基站和定位节点之间的信号出现中断,导致其无法完成测距流程。因此,可直接根据UWB节点和基站的连接情况,对存在严重遮挡时的非视距通信状态进行判断。当出现信号未中断的非视距测量时,信号飞行时间会比视距条件下的时间长,导致测距结果偏大。本文设计了基于UWB测距残差的节点通信类型判别方法。通过计算测距残差实现对当前UWB系统通信类型的判别,UWB节点通信类型判别器的整体工作流程如图3所示。

图3 UWB通信类型判别器工作流程图Fig.3 Workflow diagram of UWB communication type discriminator

图3中,首先若UWB节点和基站的连接信号中断,则直接判定当前系统处于非视距测量状态。在UWB信号未中断的情况下,通过当前时刻的测距残差判断UWB系统的通信类型。

UWB原始测距结果存在误差。视距通信条件下,UWB测距结果的误差项只包含均值为零的高斯噪声;非视距通信条件下,UWB测距结果的误差项包含均值为零的高斯噪声和一个随机的偏差。

当UWB系统所有基站都处于视距通信条件时,由原始测距结果通过最大似然估计法得到的位置估计是UWB节点真实位置的无偏估计。在视距通信下,估计的位置到基站的欧氏距离与原始测距结果之间的残差符合均值为零的正态分布,因此视距通信条件下系统的整体测量残差较小。

在实际应用中,系统中定位节点与各个基站之间的通信状态未知。当系统存在非视距通信的测量时,由当前测量得到位置估计是UWB节点真实位置的有偏估计。因此,在非视距通信下,估计的位置到基站的欧氏距离会整体偏离原始UWB测距结果。因此非视距通信下系统将出现较大的测量残差。

综上,当系统存在非视距测量结果时,整体残差会大于视距测量结果的整体残差。利用这一特点就可以实现对系统通信状态的判断。

基于当前的UWB测距结果对节点位置进行估计。根据UWB定位系统中定位节点和定位基站之间位置关系,可以得到

(1)

式中 (x1,y1)、(x2,y2)、(xn,yn)——基站坐标

(px,py)——定位节点坐标

对式(1)化简可得到关于定位节点坐标(px,py)的线性方程组,其矩阵形式为

Ax=b

(2)

其中

式中x——定位节点位置坐标

(3)

(4)

其中

式中pi——基站i的位置坐标

计算UWB距离测量残差

(5)

式中εi——基站i的测量残差

di——基站i原始测距值

计算UWB测量的整体残差ε为

(6)

式中n——基站数量

通过实验可以得到区分视距和非视距通信的残差阈值Tε,UWB通信类型判别式为

(7)

式中S——基站通信类型

α——判别系数

S=1则通信类型为非视距通信;S=0则通信类型为视距通信。

通信类型判别阈值Tε为定位环境中视距测量下的最大测距残差,因此该阈值不会随温室作物的生长变化而发生改变。判别系数α大于1,用于调整判别器对非视距通信状态的敏感程度,根据实际使用场景进行调整。

1.3 视距条件下UWB测距修正模型

视距条件下,UWB信号在空气中沿直线传播,基站和定位节点之间的距离与信号飞行时间成正比。UWB信号飞行时间与飞行速度的乘积就是距离测量结果。但实际测量过程中,飞行时间测量存在偏差,信号飞行速度与标准电磁波传播速度不相等,因此,视距通信下的UWB测距结果仍然存在误差,进行融合定位之前,需要对其进行修正。

受到UWB通信设备时钟误差和信号处理时间的影响,定位设备记录的UWB信号飞行时间存在偏差,导致实际测距结果存在偏差;实际环境中电磁波信号的飞行速度和标准值之间存在偏差,然而测量设备使用标准值来计算距离,导致实际测距结果存在尺度因子偏差。

由此可建立视距下的UWB测距修正模型,可以近似认为测距结果和真实距离之间存在一次多项式的关系,即

d1=a1d0+a0

(8)

式中d0——真实距离

d1——UWB测量结果

一次项系数a1表示当前环境电磁波传播速度与其真空标准值的比值。常数项a0表示距离测量的零点偏移。这两个系数与UWB设备、工作环境相关,可以通过实验进行估计。

视距条件下的修正模型为

(9)

di——定位节点与基站i的原始测距值

1.4 非视距条件下UWB测距修正模型

非视距通信条件下,定位标签和定位基站之间的遮挡角度、遮挡物的位置以及遮挡物材质等因素对UWB的节点间测距结果影响较为复杂,无法精确建模。因此,本文设计了基于机器人的历史加速度、历史速度以及历史位置信息的数据估计方法,实现对非视距通信条件下的UWB测距值预测。

机器人植保作业过程中行进速度较慢,UWB系统的定位周期和IMU测量周期T较小,因此,一次测量周期内认为机器做匀速运动或匀加速运动,不影响模型修正精度。利用k-1时刻的机器人运动状态估计时刻k机器人位置。根据牛顿惯性定律,得到方程

(10)

px(k-1)、vx(k-1)、ax(k-1)分别为k-1时刻机器人在导航坐标系n下,x轴方向的位置、速度、加速度的最优估计;py(k-1)、vy(k-1)、ay(k-1)分别为k-1时刻机器人在导航坐标系n下,y轴方向的位置、速度、加速度的最优估计。

(11)

(xi,yi)——导航坐标系下基站i的位置坐标

1.5 基于扩展卡尔曼滤波的UWB-IMU融合定位方法

基于统计学分析,UWB和IMU测量结果均含有均值为零的高斯噪声,因此本文采用卡尔曼滤波对这两种传感器数据进行融合。非视距通信下的UWB测距修正模型需要对机器人的运动信息进行实时估计,因此,定义x(k)为机器人在k时刻的状态变量

(12)

式中 (px(k),py(k))——k时刻机器人位置

(vx(k),vy(k))——k时刻机器人速度

(ax(k),ay(k))——k时刻机器人加速度

x(k)=Fx(k-1)+Bw(k)

(13)

其中

实际作业过程中,一次测量周期内,机器人运行的加速度并不恒定,导致状态更新过程存在噪声w(k),该噪声符合均值为零的正态分布,其协方差矩阵为

(14)

本文通过实验对过程噪声的方差进行估计。

状态更新方程实现了状态变量的先验估计,通过传感器测量对更新后的状态变量进行矫正,实现状态变量的后验估计。

对于平坦的铺装路面,机器人行进过程中的姿态的变化主要体现在航向轴。IMU的z轴垂直于地面,机器人受到驱动加速度只在x轴和y轴上产生分量,重力只在z轴上产生分量,实现二维定位只需要考虑机器人航向角就可实现加速度数据从机体坐标系b到导航坐标系n的转换。温室路面起伏,机器人在行走过程中会出现晃动,姿态变化较为复杂,不能只考虑航向轴的姿态变化。因此在温室环境下对加速度结果进行坐标系变换时,应考虑机器人三维姿态。

本文使用四元数来表达从机体坐标系b到导航坐标系n的旋转,定义该旋转为q(k),有

(15)

(16)

姿态四元数不能直接进行测量但可通过姿态解算得到。姿态需要角速度测量来更新,其更新方程为[15]

(17)

其中

式中bΩ——机体坐标系b下的旋转角速度矩阵

ωx、ωy、ωz——机体坐标系下的角速度测量值,即捷联式惯性导航系统IMU的陀螺仪测量值

导航坐标系下机器人加速度矢量为

(18)

其中

IMU对加速度的测量包含噪声,观测方程为

(19)

式中zIMU(k)——k时刻导航坐标系下的加速度矢量测量值

nax(k)、nay(k)——k时刻导航坐标系下x轴、y轴加速度的真实值

υx(k)、υy(k)——x轴、y轴的测量噪声

式(19)的矩阵表达形式为

zIMU(k)=HIMUx(k)+υIMU(k)

(20)

其中

式中υIMU(k)——k时刻的测量噪声向量

HIMU——测量矩阵

IMU对x轴和y轴的加速度测量独立,两测量噪声之间不相关,因此IMU测量噪声υIMU的协方差矩阵为

(21)

本文通过实验对加速度计的测量噪声的标准差σix、σiy进行测定。

加速度测量噪声的均值为零,方差较大且服从正态分布。卡尔曼滤波是一种基于统计学的最优状态估计算法,对高斯噪声具有良好的抑制作用。本文设计的UWB-IMU融合算法利用卡尔曼滤波算法对加速度的噪声进行抑制。

在UWB-IMU定位系统中,位置坐标无法直接进行测量,可以通过定位节点与已知位置基站之间的距离间接进行测量,观测方程为

(22)

式中di(k)——k时刻节点与基站i的真实距离

υi(k)——k时刻基站i的距离测量噪声

经过矫正后的UWB测量仍然包含噪声。UWB距离测量噪声的协方差矩阵RUWB为

(23)

本文通过实验测定得到定位节点与每一个基站的测距标准差σdi。

根据欧氏距离公式可以得到

(24)

式中 (px(k),py(k))——k时刻定位节点位置坐标

(xi,yi)——基站i的坐标

该方程为非线性方程,需要对其进行线性化。为了减小算法的运算量,线性化结果只保留一次项,即

zUWB(k)=HUWB(k)x(k)+υUWB(k)

(25)

其中

式中HUWB(k)——测量矩阵

υUWB(k)——UWB的距离测量噪声向量

融合定位系统整体测量方程为

z(k)=H(k)x(k)+υ(k)

(26)

其中

式中z(k)——k时刻系统测量向量

H(k)——k时刻观测矩阵

υ(k)——k时刻测量噪声向量

IMU和UWB测量过程独立,各测量噪声之间不相关,因此测量噪声υ(k)的协方差矩阵为

(27)

扩展卡尔曼滤波算法整体计算流程如下:

(1) 状态预测

首先,给定状态变量的初始值x(0)=x0和状态变量初始协方差矩阵P(0)=P0,每一个更新周期,利用上个周期状态的最优估计对当前时刻的状态进行预测。

(k|k-1)=Fx(k-1|k-1)

(28)

(2) 状态协方差矩阵预测

利用上个周期的状态协方差矩阵的最优估计P(k-1|k-1)对当前时刻的协方差矩阵进行预测,即

P(k|k-1)=FP(k-1|k-1)FT+BQBT

(29)

式中B——过程噪声传递矩阵

(3) 计算卡尔曼增益

K=P(k|k-1)HT(k)(H(k)P(k|k-1)HT(k)+R)-1

(30)

(4) 状态更新

利用测量结果对当前时刻k状态预测结果(k|k-1)进行修正,得到当前时刻k状态的最优估计结果

x(k|k)=(k|k-1)+K(z(k)-H(k)(k|k-1))

(31)

(5) 状态协方差矩阵更新

利用当前时刻k的卡尔曼增益K对当前状态协方差矩阵进行更新,即

P(k|k)=(In-KH(k))P(k|k-1)

(32)

式中In——n维单位方阵

P(k|k)——k时刻状态协方差矩阵的最优估计值

本文定位方法的整体工作流程如下:首先,对UWB和IMU的原始测量数据进行处理;将IMU的加速度测量结果进行坐标系变换,将机体坐标系下的加速度测量结果转换到导航坐标系;对UWB的距离测量进行修正,提升测量精度。然后使用扩展卡尔曼滤波对修正后的UWB测距数据和经过坐标系转换后加速度进行融合,实现位置和系统运动状态的最优估计。

2 实验

2.1 通信类型判别实验

为了验证本文提出的通信类型判别方法的效果,本文设计通信类型判别实验。

实验采用DWM1000模块进行UWB通信,测距周期10 ms,使用双边测距法TWR进行距离测量。本实验采用四基站进行定位,机器人行进过程中,对4个UWB基站A0、A1、A2、A3中的基站A0进行遮挡,使定位节点与基站A0处于非视距通信状态,记录运动过程中的UWB测距结果,最后对每个测量周期的整体测距残差进行计算。本实验中UWB测距残差如图4所示。

图4 通信类型判别实验结果Fig.4 Experimental results of communication type discrimination

从图4可以看出,在本实验过程中,开始遮挡基站A0之后,UWB定位系统的测量残差相较于未遮挡时明显增大。可以找到一个明显的阈值Tε=370 mm, 将视距通信和非视距通信的测量结果进行分类。在实际定位过程中,通过该阈值就可以实现对UWB测量通信类型的判别。

2.2 视距条件UWB测距误差分析实验

为了验证视距条件下,UWB测距修正模型的准确性并估计视距修正模型的参数,本文设计了实物实验验证模型。

实验环境为温室环境,实验采用DWM1000模块进行UWB通信,测距周期10 ms,使用双边测距法TWR进行飞行时间的测量,距离解算使用的电磁波传播速度为299 702 547.0 m/s,采集范围为 120～500 cm,采样间隔为50 cm。为减小测量噪声对拟合结果的影响,每个点取20组距离测量值,最终取平均值作为测距最终结果,最后将原始测距数据和真实距离值使用最小二乘法进行拟合,结果如图5所示。

图5 UWB测量值与真实值拟合曲线Fig.5 UWB measurement and real value fitting curves

从图5可以看出,本实验中测得的UWB测量值与实际距离不相等,但两者之间存在线性关系,使用最小二乘法进行线性拟合,可得到真实距离和UWB测距值之间的映射关系。

本实验中用到的UWB模块通过计算信号飞行时间和电磁波传播速度的乘积得到距离。其中,信号飞行时间的测量存在一定的偏差,UWB节点在计算距离时用到的电磁波传播速度与实验环境也存在偏差,这两个偏差分别造成了真实距离和UWB测量的关系式在截距和斜率上的偏差。

经过计算,视距条件下UWB原始测距结果的均方根误差(RMSE)为50.1 cm。经过本次实验建立的修正模型进行修正后的均方根误差为27.8 cm,测距误差降低44.5%。

2.3 基于UWB测距值修正的UWB-IMU融合定位实验

为了验证本文提出的基于UWB测距值修正的融合定位方法的定位精度以及定位系统的鲁棒性,本文设计了仿真实验和温室内定位实验。

基于CoppeliaSim仿真软件平台搭建实验场地,温室为10 m×10 m正方形场地,场地共有植被4排,植被为1.2 m×8 m的矩形区域,植被行间可通行区域宽1.6 m。植保机器人长0.5 m,宽0.33 m,采用差速转向。机器人装有UWB定位节点,定位基站位于场地四角,高度为2 m。UWB测距值含有均值为0、方差为0.1的高斯噪声。机器人沿设定轨迹运动,记录由运动过程中CoppeliaSim平台产生的传感器仿真数据,包括捷联式导航系统IMU加速度数据和角速度数据以及UWB标签点与4个基站之间的距离数据。机器人按照工作路径行进,行驶过程中人为制造非视距测量,仿真环境以及行驶路径如图6所示。

图6 UWB定位仿真实验行驶路径Fig.6 Driving paths for UWB positioning simulation experiments

本实验中使用MEMS传感器对加速度进行测量,测量噪声较大。通过卡尔曼滤波算法将UWB和IMU数据进行融合,能有效抑制噪声,并得到加速度的最优估计,实验结果如图7所示。

图7 加速度融合滤波结果Fig.7 Acceleration fusion filtering results

从图7可以看出,经过融合滤波算法,加速度的测量噪声得到了有效的抑制。经过计算本实验中x轴方向的加速度原始测量样本标准差为0.857 6g,最优估计结果的标准差为0.063 9g;y轴方向的加速度原始测量样本的标准差为0.588 4g,最优估计结果的标准差为0.051 3g。实验结果表明本文提出的融合滤波方法能有效降低加速度测量数据的噪声水平。

为了测试定位系统在非视距测量条件下的定位稳定性,本文通过模拟机器人在移动过程中定位系统因遮挡导致UWB标签点和4个基站之间完全失去连接的情况,在该情况下UWB不再产生新的测距结果,输出值维持最后时刻的观测值。遮挡开始于126 s,遮挡时长2 s。实验结果如图8所示。

图8 仿真环境UWB-IMU融合定位实验结果Fig.8 Experimental results of UWB-IMU fusion positioning in simulation environment

从图8a可以看出,导航坐标系下,定位结果x轴坐标值随着实验的进行先变大再变小,在-5～5 m之间做往复变化,y轴坐标值呈阶梯形变大,y轴坐标值增长的时间段,x轴坐标基本保持不变,该阶段机器人处于行间行驶。定位结果坐标轨迹和图6 中实验设计的路径一致。从图8b可以看出,出现非视距测量后,未进行测距值修正的融合定位结果开始偏移真实定位轨迹,随着遮挡时间的增长,定位误差不断增大,127.95 s时定位误差达到最大值,x轴方向最大误差达到49 cm。经过测距值修正的融合定位结果在出现非视距测量后,也出现了偏离真实定位轨迹的情况,最大误差为9 cm,误差降低81.6%。

从图8b可以看出,在失去所有UWB测距数据的情况下,UWB测量结果维持遮挡前最后时刻的测距数据。纯UWB的定位系统缺少足够的信息只能维持遮挡前最后时刻的定位结果,未进行UWB测距值修正的融合定位方法依靠IMU数据进行定位,对机器人行驶轨迹有一定的预测效果,但随着时间定位误差不断增大,使用经过本文提出的UWB测距值修正模型修正的测距结果的融合定位方法仍然可以保持较好的定位精度。

温室内的实验场地尺寸为10 m×8 m,UWB基站安装高度为2 m,UWB定位标签安装高度为1.3 m。实验场地示意图如图9a所示。基站位于矩形实验场地4个顶点,以基站A0在地面的竖直投影点为原点,以基站A0在地面的投影点到A1在地面的投影点的方向为x轴正方向,以基站A0在地面的投影点到A3在地面的投影点的方向为y轴正方向建立导航坐标系n。移动机器人在行间往返行走,经过UWB覆盖区域内所有作物行间。实验行驶路径和实验场地如图9所示。

图9 UWB实物定位实验环境Fig.9 Experimental environment for UWB positioning

UWB通信模块采用DWM1000模块,控制器采用stm32f405,板载imu模块,对两个传感器数据进行同步采集。通过串口输出UWB测距数据和IMU加速度以及姿态数据到机器人控制器,由机器人主控制器对实验数据进行记录。

本实验使用两个UWB定位节点进行定位实验,记录两个定位节点T0、T1在机器人移动过程中的所有测量数据。实验过程中,定位节点T0在多处出现了非视距通信,节点T0非视距通信期间,节点T1处于视距测量状态,以节点T1的定位结果作为机器人真实位置的参考。两个定位节点相对位置固定,通过机器人姿态信息和两定位节点的相对位置信息就可以得到定位节点T0真实位置的参考值,设置对照组为未进行UWB测距值修正的融合定位结果,如图10所示。

图10 温室环境UWB-IMU融合定位实验结果Fig.10 Experimental results of UWB-IMU fusion localization in greenhouse environment

从图10a可以看出,随着实验的进行,导航坐标系下定位结果x轴坐标值呈现阶梯状变大,x轴坐标变大期间y轴坐标值基本保持不变。y轴坐标值在0.5～9.5 m之间往复变化。融合定位结果的坐标值变化趋势与图9a中设计的实验路径一致。图10a 中红色框内的时间段,定位节点T0受到非视距影响,可以看出未进行测距值修正的融合定位结果在x轴、y轴上都明显偏离真实位置的参考值,其中红色实线框内出现的误差最大。以红色实线框内的y轴定位结果为例展开分析,从图10b可以看出,随着遮挡时间的增长,两种方法都相对于实际位置参考值出现了一定程度的定位误差。出现遮挡后,定位结果误差随着遮挡时间的增长越来越大,遮挡消失后,定位误差逐渐降低。

从图10b可以看出,定位误差在83.55 s达到最大值。经计算,未进行UWB测距值修正的融合定位结果,在x轴、y轴上的最大定位误差分别为4.82 cm和70.59 cm,欧氏距离误差为70.75 cm。该时刻,经过测距修正的定位结果在x轴、y轴的最大定位误差为11.23 cm和4.08 cm,欧氏距离误差为11.95 cm。实验结果表明,经过测距值修正的融合定位方法在实际温室环境中的定位误差远低于未经过测距值修正的融合定位方法的定位误差。

仿真实验和实物实验的结果表明,在定位系统出现短时非视距测量的情况下,基于测距值修正的融合定位方法仍然可以保证较好的定位精度。

3 结束语

提出了一种基于UWB测距值修正的IMU-UWB融合定位方法。首先,根据温室环境下非视距测量数据的特点,设计了UWB通信类型判别器,对定位系统的通信类型进行判别;其次,分别建立了视距和非视距两种通信条件下的UWB测距修正模型,对不同通信类型下的UWB测距结果进行修正;然后,采用扩展卡尔曼滤波算法对UWB-IMU进行数据融合,得到位置的最优估计结果;最后通过实验对本文方法的定位效果进行了验证。实验结果表明,在非视距通信情况下,未对UWB测距进行修正的定位误差为70.75 cm,经本文方法对UWB测距值进行修正后的定位误差为11.95 cm,定位精度提升83.11%。该方法可以为温室机器人提供稳定且高精度的位置信息。