CN111208472A - 一种温室大范围高精度定位系统与方法 - Google Patents

Abstract

本发明公开了一种温室大范围高精度定位系统与方法，由一维程控移动激光定位系统、三维UWB差分定位系统和融合定位控制系统组成。一维程控移动激光定位系统采集吊轨小车的一维定位信号，三维UWB差分定位系统采集辅助定位标签和定位基站的位置参数，融合定位控制系统将一维定位信号和所述位置参数进行融合，得到移动定位标签的状态向量X_k和下一步预测状态向量X_k+1，并修正预测状态向量，控制移动定位标签。本发明不受温室结构和作物遮挡的影响，有效解决了非视距和多路径效应带来的误差问题，实现高精度空间定位；另外，本发明定位范围大，可在整个温室内提供定位和导航信号，为设施农业自动化作业提供帮助，促进设施农业智能化发展。

Description

一种温室大范围高精度定位系统与方法

技术领域

本发明属于温室设施技术领域，具体涉及一种基于一维激光和三维UWB融合定位的温室大范围、高精度定位系统与方法。

背景技术

设施农业在我国农业领域具有重要的地位，我国连栋温室的面积处于世界前列。为节约劳动力、减轻劳动强度、提高作业效率，温室内智能化作业机具日益受到科技人员的重视。温室内作业项目涉及到每日的作物信息采集、作物保护管理、果实收获等，在大面积的连栋温室内利用智能化自主移动车辆有助于这些作业的高效完成。定位技术作为温室自动化以及机械化的核心技术已成为研究的热点和难点。

发明内容

有鉴于此，本发明的目的在于提供一种温室大范围高精度定位系统与方法，以适应温室作业环境中室内附属设施多、作物栽培密度大、空间利用率高、目标背景信息叠加多义等复杂环境特点，实现温室大范围、高精度定位。

为了实现上述技术目的，本发明采用如下技术手段：

一种温室大范围高精度定位系统，包括一维程控移动激光定位系统、三维UWB差分定位系统和融合定位控制系统；一维程控移动激光定位系统采集吊轨小车的一维定位信号；三维UWB差分定位系统采集辅助定位标签和定位基站的位置参数；融合定位控制系统将一维定位信号和所述位置参数进行融合，获取移动定位标签的定位信号。

上述技术方案中，所述一维程控移动激光定位系统包括导轨、吊轨小车和测距传感器，导轨设置在温室内，导轨上设置吊轨小车，吊轨小车上设置测距传感器。

上述技术方案中，所述三维UWB差分定位系统包括定位基站、移动定位标签和辅助定位标签。

上述技术方案中，所述辅助定位标签和一定位基站安装于一侧吊轨小车下方，另一侧吊轨小车下方安装两个定位基站。

上述技术方案中，所述同一侧吊轨小车下方的两个定位基站高度间隔h，该侧位置较高的定位基站与另一侧的定位基站、辅助定位标签高度相同，其中h≥1m。

一种温室大范围高精度定位方法，获取移动定位标签的状态向量X_k和下一步预测状态向量X_k+1，将所述预测状态向量X_k+1与移动定位标签的观测向量Z_k作差，当差值大于阈值，利用修正矩阵A对状态向量预测值X_k+1进行修正，控制移动定位标签。

进一步，所述修正矩阵A为：A＝(x′，y′，z′)^T-(x′₆，y′₆，z′₆)^T，其中(x′，y′，z′)为辅助定位标签的测量位置参数，(x′₆，y′₆，z′₆)为辅助定位标签的计算位置参数。

更进一步，所述计算位置参数由激光测距传感器采集的数据、辅助定位标签和激光测距传感器的水平距离进行作差获取。

进一步，所述定位方法还包括跨行移动，吊轨小车由融合定位控制系统控制跨行，重新获取辅助定位标签和定位基站位置参数，得到移动定位标签的状态向量X_k和下一步预测状态向量X_k+1，并修正预测状态向量，控制移动定位标签。

本发明的有益效果是：

1、本发明根据第一激光测距传感器采集的数据以及辅助定位标签在第一吊轨小车上的固定安装距离，可计算得到移动定位标签状态向量预测值的修正矩阵，可有效消除时钟同步误差、作物遮挡引起的多径效应和空气传输损耗等导致的定位误差，实现一定范围内的高精度差分定位。

2、本发明中一维程控移动激光定位系统获取视距范围内吊轨小车的高精度一维定位信号，三维UWB差分定位系统提供局部高精度三维定位信号(在对状态向量预测值进行修正时，三维定位信号为辅助定位标签在温室静态坐标系的位置参数；在吊轨小车跨行移动，三维定位信号为定位基站的位置参数)，融合控制系统将一维定位信号和局部定位信号进行融合，得到温室场景下的大范围高精度定位信号，可有效促进温室作业自动化水平，进而提高生产效率。

附图说明

图1为本发明温室大范围高精度定位系统的示意图；

图2为本发明温室大范围高精度定位方法的流程图；

图中：1-第一水平导轨，2-第二水平导轨，3-第一吊轨小车，4-第二吊轨小车，5-第一激光测距传感器，6-第二激光测距传感器，7-第一定位基站，8-第二定位基站，9-第三定位基站，10-第四定位基站，11-辅助定位标签，12-第五定位基站，13-移动定位标签，14-栽培槽，15-第一安装支架，16-第二安装支架。

具体实施方式

下面结合附图对本发明的技术方案做进一步说明，但本发明的保护范围并不限于此。

如图1所示，一种温室大范围高精度定位系统由一维程控移动激光定位系统、三维UWB差分定位系统和融合定位控制系统组成。

所述一维程控移动激光定位系统包括第一水平导轨1、第二水平导轨2、第一吊轨小车3、第二吊轨小车4、第一激光测距传感器5和第二激光测距传感器6，第一水平导轨1、第二水平导轨2安装于温室两端视线无遮挡的高处，导轨长度以能够跨越所有栽培槽14为基准；第一吊轨小车3、第二吊轨小车4设置在第一水平导轨1、第二水平导轨2上，控制算法模块控制第一吊轨小车3、第二吊轨小车4在第一水平导轨1、第二水平导轨2上做一维移动，第一吊轨小车3、第二吊轨小车4上分别安装有第一激光测距传感器5、第二激光测距传感器6，用于测量第一吊轨小车3、第二吊轨小车4在第一水平导轨1、第二水平导轨2上的移动距离。

所述三维UWB差分定位系统包括定位基站、移动定位标签13和辅助定位标签11，定位基站的个数大于等于3，本实施例设置为5个，具体包括第一定位基站7、第二定位基站8、第三定位基站9、第四定位基站10和第五定位基站12，第三定位基站9、第一定位基站7和辅助定位标签11通过第一安装支架15依次刚性安装于第一吊轨小车3的下方，第一吊轨小车3带动第一安装支架15移动；第四定位基站10、第二定位基站8和第五定位基站12通过第二安装支架16依次刚性安装于第二吊轨小车4的下方，第二吊轨小车4带动第二安装支架16移动。第一定位基站7、第二定位基站8、第三定位基站9、第四定位基站10安装于同一水平高度，用于获取移动定位标签13的平面定位信息；第五定位基站12在竖直方向上与第二定位基站8间隔h，用于获取移动定位标签13的垂直定位信息，其中h≥1m。移动定位标签13在温室中移动。

本实施例中第一定位基站7与第三定位基站9、辅助定位标签11的间距均为1.5m，第二定位基站8与第四定位基站10、第五定位基站12的间距均为1.5m，第一定位基站7、第三定位基站9、辅助定位标签11、第二定位基站8、第四定位基站10的离地高度均为3m，第五定位基站12的离地高度为1.9m。

所述融合控制系统包括通讯模块和控制算法模块，控制算法模块通过通讯模块与第一吊轨小车3、第二吊轨小车4的电机进行通信，控制算法模块还通过通讯模块与第一激光测距传感器5、第二激光测距传感器6、第一定位基站7、第二定位基站8、第三定位基站9、第四定位基站10、第五定位基站12、移动定位标签13和辅助定位标签11进行通信。

一种温室大范围高精度定位方法，具体包括以下步骤：

步骤(1)在温室内固定点建立温室静态坐标系(图1)，控制算法模块获取各定位基站在温室静态坐标系中的位置参数，具体的位置参数为A_i＝(x_i，y_i，z_i)，其中i为定位基站的编号，i＝1，…，5；设吊轨小车移动方向为X轴，Z轴为吊轨小车竖直高度方向；控制算法模块获取辅助定位标签11在温室静态坐标系的位置参数为T(x′，y′，z′)。

控制算法模块求解移动定位标签13的定位参数，具体过程如下：

不同定位基站获取移动定位标签13与自身之间的距离d_i：

根据d_i得到移动定位标签13在温室静态坐标系中的位置参数(x_k y_k z_k)；

由d_i表示移动定位标签13的观测向量为Z_k：

Z_k＝[d₁ d₂ d₃ d₄ d₅]^T (2)

设移动定位标签13在温室静态坐标系中的状态向量为X_k：

其中，(x_k y_k z_k)为移动定位标签13的位置参数，

为移动定位标签13的矢量速度。

由(1)、(2)、(3)式可知观测向量Z_k与状态向量X_k为非线性关系，将观测向量Z_k局部线性化，可简化为下式：

Z_k＝HX_k+v_k (4)

其中：

其中，H为状态向量X_k的雅可比矩阵，v_k为观测向量第k个采样的观测噪声，设观测噪声是加性且对角自相关的矩阵R。

已知状态方程满足如下关系：

X_k+1＝FX_k+Q (6)

其中，F为状态转移矩阵，Q为定位基站对移动定位标签13观测测量时产生的过程噪声协方差；且：

其中，T_c为采样时间；

其中，q为过程噪声的功率谱密度。

根据公式(1)-(8)可计算得到移动定位标签13的状态向量X_k和下一步预测状态向量X_k+1，从而得到移动定位标签13的定位参数。

步骤(2)，第一激光测距传感器5采集的数据减去辅助定位标签11在第一吊轨小车3上的固定安装距离(第一激光测距传感器5与辅助定位标签11的水平距离)，精确获取辅助定位标签11的计算位置参数T’(x′₆，y′₆，z′₆)。根据计算位置参数对测量位置参数T(x′，y′，z′)进行修正，得到修正矩阵A：

A＝(x′，y′，z′)^T-(x′₆，y′₆，z′₆)^T (9)

根据状态方程(式(6))和线性化后的观测方程(式(4))，通过普通扩展卡尔曼滤波(extended Kalman filtering，EKF)进行状态更新及观测更新，得到状态向量与均方误差向量P_k的预测迭代方程组：

由于以温室静态坐标系各个方向的误差向量来衡量特定条件下定位系统的定位精度比较困难，因此引入均方误差向量P_k对X、Y、Z三个方向误差向量的均值进行估计预测，并可以通过上式(式(10))N次采样迭代获取P_k，用以判断定位的准确性与可靠性，作为衡量定位精度的标准。

在温室环境下，温室结构、作物、作业装置等障碍物都会对UWB信号造成遮挡，对移动定位标签13带来非视距和多径误差，根据温室内设施环境和作物环境以及作业机器人类型人为设定阈值，即可接受误差的上下限。将移动定位标签13的观测向量Z_k与移动定位标签13的状态向量X_k进行比较，再将二者的差值与阈值相比。若差值小于等于阈值时，可认为状态向量X_k是视距场景下的，所产生的误差是可接受的，滤波增益不作改变；若差值大于阈值时，表明状态向量X_k存在非视距和多径误差，因此采用修正矩阵A对状态向量X_k进行修正，抑制非视距和多径误差，提高定位精度。

步骤(3)，控制算法模块首先控制吊轨小车跨行移动(为现有技术)，并获取新的定位基站位置参数。T₀时刻定位基站的位置参数为A_i(x_iy_iz_i)，T₁时刻吊轨小车跨行移动结束，激光测距传感器(第一激光测距传感器5、第二激光测距传感器6)测量得到吊轨小车的移动距离x′，此时定位基站的位置参数为：

A_i＝(x_i+x′，y_i，z_i) (11)

重复步骤(1)-(2)得到修正后的移动定位标签13在T₁时刻的状态向量X_k ^*和下一步预测状态向量X_k+1 ^*，从而实现温室大范围、高精度的定位。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。应当说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围之内。

Claims (9)

1.一种温室大范围高精度定位系统，其特征在于，包括一维程控移动激光定位系统、三维UWB差分定位系统和融合定位控制系统；一维程控移动激光定位系统采集吊轨小车的一维定位信号；三维UWB差分定位系统采集辅助定位标签和定位基站的位置参数；融合定位控制系统将一维定位信号和所述位置参数进行融合，获取移动定位标签的定位信号。

2.根据权利要求1所述的温室大范围高精度定位系统，其特征在于，所述一维程控移动激光定位系统包括导轨、吊轨小车和测距传感器，导轨设置在温室内，导轨上设置吊轨小车，吊轨小车上设置测距传感器。

3.根据权利要求1所述的温室大范围高精度定位系统，其特征在于，所述三维UWB差分定位系统包括定位基站、移动定位标签(13)和辅助定位标签(11)。

4.根据权利要求3所述的温室大范围高精度定位系统，其特征在于，所述辅助定位标签(11)和一定位基站安装于一侧吊轨小车下方，另一侧吊轨小车下方安装两个定位基站。

5.根据权利要求4所述的温室大范围高精度定位系统，其特征在于，所述同一侧吊轨小车下方的两个定位基站高度间隔h，该侧位置较高的定位基站与另一侧的定位基站、辅助定位标签(11)高度相同，其中h≥1m。

6.一种根据权利要求1-5任意一项权利要求所述的温室大范围高精度定位方法，其特征在于，获取移动定位标签(13)的状态向量X_k和下一步预测状态向量X_k+1，将所述预测状态向量X_k+1与移动定位标签(13)的观测向量Z_k作差，当差值大于阈值，利用修正矩阵A对状态向量预测值X_k+1进行修正，控制移动定位标签(13)移动。

7.根据权利要求6所述的温室大范围高精度定位方法，其特征在于，所述修正矩阵A为：A＝(x′，y′，z′)^T-(x′₆，y′₆，z′₆)^T，其中(x′，y′，z′)为辅助定位标签(11)的测量位置参数，(x′₆，y′₆，z′₆)为辅助定位标签(11)的计算位置参数。

8.根据权利要求7所述的温室大范围高精度定位方法，其特征在于，所述计算位置参数由激光测距传感器采集的数据、辅助定位标签和激光测距传感器的水平距离进行作差获取。

9.根据权利要求6所述的温室大范围高精度定位方法，其特征在于，还包括跨行移动，吊轨小车由融合定位控制系统控制跨行，重新获取辅助定位标签和定位基站位置参数，得到移动定位标签(13)的状态向量X_k和下一步预测状态向量X_k+1，并修正预测状态向量，控制移动定位标签(13)。

Images