CN108613671A

CN108613671A - 一种基于uwb定位和航迹定位的智能割草机定位装置及方法

Info

Publication number: CN108613671A
Application number: CN201810383426.8A
Authority: CN
Inventors: 王兴松; 田春雨; 田梦倩; 毛玉良
Original assignee: Southeast University
Current assignee: Southeast University
Priority date: 2018-04-25
Filing date: 2018-04-25
Publication date: 2018-10-02

Abstract

本发明公开了一种基于UWB定位和航迹定位的智能割草机定位装置及方法，该装置包括定位基站模块、定位标签模块、姿态角检测模块、航迹定位模块以及电机驱动模块。所述定位标签安装于智能割草机上，定位标签模块利用发送和接收信号时间差测得定位标签到不同基站的距离，用于求解定位标签的坐标，基于UWB定位实现对智能割草机的定位。所述姿态角检测模块检测智能割草机的姿态角，将姿态角检测模块检测到的航向角和电机驱动模块所测智能割草机轮子转数发给航迹定位模块，航迹定位模块通过选择不同定位模式来确定最终定位坐标，通过将UWB定位技术和航迹推算定位技术的融合使用，提高了智能割草机作业效率。

Description

一种基于UWB定位和航迹定位的智能割草机定位装置及方法

技术领域

本发明属于割草机领域，具体涉及一种基于UWB定位和航迹定位的智能割草机定位装置及方法。

背景技术

随着经济的发展，城市绿化面积和家庭草地的大幅度增加，智能割草机市场越来越大。目前市场在售智能割草机品种繁多，按其操作方式可以分为智能割草机和非智能割草机。智能割草机的主要特点是不需要人为干预，可以自主工作，自主充电等功能。市场上大多数智能割草机在工作前都安装有电子篱笆以防止智能割草机逃离工作区，当智能割草机碰到电子篱笆便以任何角度返回继续向前行进作业，目前大多数智能割草机都采用随机规划的方式进行割草，并未实现路径规划，虽然有智能割草机具备定位功能但是定位精度都未能满足路径规划的需求。显然，随机规划的割草方式不仅大大增加了智能割草机的耗能，还大大降低了其作业效率。

发明内容

发明目的：针对上述现有技术的不足，本发明提出一种基于UWB定位和航迹定位的智能割草机定位装置及方法，提高智能割草机的定位精度和范围。

技术方案：为了实现上述发明的目的，本发明公开了一种基于UWB定位和航迹定位的智能割草机定位装置，包括如下模块：

定位基站模块，部署在智能割草机需要定位的场地之中，用于接收定位标签发送的信号，并向定位标签发送反馈信号；

定位标签模块，利用发送和接收信号时间差测得定位标签到不同基站的距离，用于求解定位标签的坐标，基于UWB定位实现对智能割草机的定位；

姿态角检测模块，用于检测智能割草机的姿态角，并将所测姿态角传送至航迹定位模块；电机驱动模块，用于检测割草机的左轮的转数和右轮的转数，并将转数发送至航迹定位模块；

航迹定位模块，接受数据并进行数据处理，通过航迹定位实现对智能割草机的定位。

其中，定位标签模块具有以下单元：

UWB定位控制器单元，用于当定位标签模块可接受定位基站模块信号，基于UWB定位进行测距和计算定位标签坐标，对智能割草机进行定位。

并且，基站和标签模块都含有UWB定位控制器单元，基站的UWB定位控制器单元负责信号的发送和接受，与定位标签进行通信；定位标签的UWB定位控制器单元负责信号发送接受，并测距和解算出定位标签的坐标。

并且，所述UWB定位控制器单元功能如下：

(1)通过固定在智能割草机上的定位标签实时接收来自至少三个固定基站的信号，测得智能割草机上的定位标签到各个基站A₁，A₂，A₃……A_n之间的距离分别为d₁，d₂，d₃……d_n；

(2)通过定位算法得出绝对定位标签的坐标(x_a，y_a)，则割草机的位置坐标为：

x_e＝x_a-X_r

y_e＝y_a-Y_r

其中，(X_r，Y_r)为所述定位标签固定于智能割草机的固定点相对于智能割草机两后轮轴向连线中点的位置坐标。

其中，航迹定位模块具有以下单元：

航迹定位控制器单元，用于当定位标签模块不可接受定位基站模块信号，则使用航迹推算定位求解定位坐标，对智能割草机进行路径控制；并且，用于当定位标签模块再次接受到定位基站模块信号，切换到UWB定位的方式。

其中，所述航迹定位控制器单元功能如下：

(1)接收姿态角检测模块测得该时刻的偏航角θ，电机驱动模块算出智能割草机左轮的转数N_left和右轮的转数N_right，获取智能割草机的轮子直径为D，智能割草机单位时间内移动的位移为S，则满足如下表达式：

(2)智能割草机上一时刻的定位坐标为(x₀，y₀)，通过航迹推算求出该时刻的定位坐标，设定位坐标为(x₁，y₁)，则：

x₁＝x₀+Scosθ

y₁＝y₀+Ssinθ

优选的，UWB定位控制器单元属于定位标签和定位基站的组成部分，航迹推算定位控制器单元作为独立的单元，用来接收姿态角检测模块测得该时刻的偏航角θ，电机驱动模块算出智能割草机左轮的转数N_left和右轮的转数N_right，获取智能割草机的轮子直径为D，计算智能割草机单位时间内移动的位移为S。

优选的，所述定位基站和定位标签为单片机或芯片，所述姿态角检测模块为九轴传感器。

优选的，所述定位基站至少设有N个，且避免呈一条直线摆放，N为大于等于3的整数。

此外，本发明还公开了一种基于UWB定位和航迹定位的智能割草机定位装置实现的定位方法，该方法包括下列步骤：

(1)当定位标签模块可接受定位基站模块信号，基于UWB定位对智能割草机进行定位；

(2)当定位标签模块不可接受定位基站模块信号，则使用航迹推算定位求解定位坐标，对智能割草机进行定位；

(3)当定位标签模块再次接受到定位基站模块信号，回到步骤(1)的定位方式。

并且，所述航迹推算定位工作时，姿态角检测模块测得该时刻的偏航角θ，电机驱动模块算出智能割草机左轮的转数N_left和右轮的转数N_right，智能割草机的轮子直径为D，智能割草机单位时间内移动的位移为S，则满足如下表达式：

智能割草机上一时刻的定位坐标为(x₀，y₀)，通过航迹推算求出该时刻的定位坐标，设定位坐标为(x₁，y₁)，则：

x₁＝x₀+Scosθ

y₁＝y₀+Ssinθ

并且，当处于UWB定位时，所述固定在智能割草机上的定位标签实时接收来自至少三个固定基站的信号，测得智能割草机上的定位标到各个基站A₁，A₂，A₃……A_n之间的距离分别为d₁，d₂，d₃……d_n，通过定位算法得出定位标签的定位坐标(x_a，y_a)，则所求割草机的位置坐标为：

x_e＝x_a-X_r

y_e＝y_a-Y_r

通过如下方法求得定位标签的定位坐标(x_a，y_a)：固定在智能割草机上的定位标签A₀实时接收来自至少三个固定基站的信号，测得智能割草机上的定位标签A₀到N个基站A₁，A₂，A₃……A_n之间的距离分别为d₁，d₂，d₃……d_n，其中A₁，A₂，A₃，……A_n基站在所需定位的区域坐标系下坐标分别为(x₁，y₁)，(x₂，y₂)，(x₃，y₃)……(x_n，y_n)，设所求标签A₀位置坐标为(x_a，y_a)，满足以下方程：

方程(3)中第二个式子减去第一个式子，依次直到最后一个式子可得：

方程(4)写成矩阵形式：

Hx＝b (5)

其中：

上述方程解为：

x＝(H^TH)^-1H^Tb (6)

通过上述式(6)计算出定位标签的定位坐标(x_a，y_a)。

有益效果：与现有技术相比，本发明具有以下的有益技术效果：

本发明公开了一种基于UWB定位和航迹定位的智能割草机定位装置及方法，可以通过选择不同定位模式来确定割草机的最终定位坐标，融合UWB定位和航迹定位两种定位方法，使得智能割草机在作业范围内提供更加精准的定位数据，定位精度为10cm～20cm，扩大了单一使用基于UWB定位技术的定位范围，为后续路径规划奠定了基础，提高了智能割草机作业效率。

附图说明

图1为本发明中基于UWB定位求解定位坐标；

图2为本发明中航迹推算示意图；

图3为本发明中UWB定位和航迹推算融合定位的流程图；

图4为本发明的模块示意图；

图5为本发明中定位基站或定位标签的模块示意图。

具体实施方式

下面结合实施例对本发明技术方案进行详细的说明。

如图1至图5所示，本发明的一种基于UWB的智能割草机定位装置，包括信号连接的定位基站模块1、定位标签模块2、姿态角检测模块3、航迹定位模块4以及电机驱动模块5，定位基站模块1和定位标签模块2为同一种硬件结构，而软件版本有所不同。定位标签2安装于智能割草机上，姿态角检测模块3检测智能割草机的姿态角，并将所测姿态角传送至航迹定位模块4。电机驱动模块记录左轮N_left的转数和右轮N_right的转数并发送至航迹定位模块4。航迹定位模块4接收定位标签A0解算的定位坐标、姿态角检测模块测算的姿态角中的偏航角θ、电机驱动模块所测的转数左轮N_left的转数和右轮N_right的转数。航迹定位模块还用于求解航迹推算的相对定位坐标以及UWB定位与航迹推算定位的自动切换。

定位基站模块1和定位标签模块2为单片机或芯片，姿态角检测模块3为九轴传感器，定位基站模块1和定位标签模块2属于同一种硬件结构，其由型号为STM32F405的单片机和型号为DWM1000的模块或DW1000的芯片构成，姿态角检测模块3采用型号为MPU9250的九轴传感器，航迹定位模块4采用型号为STM32F407的单片机，定位基站1至少3个，且摆放时应注意避免呈一条直线摆放，所述的定位标签安装在智能割草机上，所述定位标签的天线位置竖直向上，且相对于智能割草机两后轮轴向连线中点的相对位置坐标为(Xr，Yr)，测量并记下。由于航迹推算所求的位置坐标为智能割草机两后轮轴向连线中点的坐标，而定位标签求解出的坐标为标签在智能割草机上固定点的位置坐标，为了方便航迹推算计算，特定将智能割草机两后轮轴向连线中点作为本发明求解定位坐标的基准点。

定位标签2的天线位置竖直向上，定位基站1设有至少三个并分别标号A₁、A₂、A₃……A_n，其中基于UWB定位和航迹定位的智能割草机定位装置的定位方法，UWB定位技术和航迹推算定位技术的融合使用，包括下列步骤：

(1)当定位标签2可接受定位基站1信号，基于UWB定位对智能割草机进行定位；

(2)当定位标签2不可接受定位基站1信号则使用航迹推算定位求解定位坐标，对智能割草机进行定位；

(3)当定位标签2再次接受到定位基站1信号，回到步骤(1)的定位方式。

航迹推算可实现短期定位，当智能割草机定位标签接收到定位基站1的信号，即切换到利用UWB定位模式，通过不断重复上述过程，从而实现UWB和航迹推算的融合定位，当智能割草机在非工作阶段，定位基站1和定位标签2进入休眠状态，等智能割草机进入工作状态，再被唤醒。

航迹定位工作时，姿态角检测模块3测得该时刻的偏航角θ，电机驱动模块5算出智能割草机左轮的转数N_left和右轮的转数N_right，规定向前为正，向后为负，智能割草机的轮子直径为D，智能割草机单位时间内移动的位移为S，单位时间为航迹定位工作采样周期，则满足如下表达式：

智能割草机上一时刻的定位坐标为(x₀，y₀)，通过航迹推算求出该时刻的定位坐标，设智能割草机两后轮轴向连线中点的位置坐标为(x_e，y_e)，即割草机位置坐标为：

当UWB定位工作时，固定在智能割草机上的定位标签A₀实时接收来自至少三个固定基站的信号，通过双向飞行时间法，测得智能割草机上的定位标签A₀到N个基站A₁，A₂，A₃……A_n之间的距离分别为d₁，d₂，d₃……d_n，其中A₁，A₂，A₃，……A_n基站在所需定位的区域坐标系下坐标分别为(x₁，y₁)，(x₂，y₂)，(x₃，y₃)……(x_n，y_n)，设所求标签A₀位置坐标为(x_a，y_a)，满足以下方程：

方程(4)写成矩阵形式：

Hx＝b (5)

其中：

上述方程解为：

x＝(H^TH)^-1H^Tb (6)

式(6)所求定位坐标为标签在智能割草机上固定点的位置坐标(x_a，y_a)，根据标签所求的定位坐标，则智能割草机两后轮轴向连线中点的位置坐标，即割草机位置坐标为(x_e，y_e)，满足下式：

通过以上技术方案，可以使智能割草机在正常工作情况下定位精度在10～20cm范围之内，同时也扩大了单一使用基于UWB定位的范围，为后续智能割草机路径规划提供定位保障。

Claims

1.一种基于UWB定位和航迹定位的智能割草机定位装置，其特征在于，包括如下模块：

姿态角检测模块，用于检测智能割草机的姿态角，并将所测姿态角传送至航迹定位模块；

电机驱动模块，用于检测割草机的左轮的转数和右轮的转数，并将转数发送至航迹定位模块；

2.根据权利要求1所述的一种基于UWB定位和航迹定位的智能割草机定位装置，其特征在于，定位标签模块具有以下单元：

3.根据权利要求1所述的一种基于UWB定位和航迹定位的智能割草机定位装置，其特征在于，航迹定位模块具有以下单元：

航迹定位控制器单元，用于当定位标签模块不可接受定位基站模块信号，则使用航迹推算定位求解定位坐标，对智能割草机进行定位；并且，用于当定位标签模块再次接受到定位基站模块信号时，切换到UWB定位的方式。

4.根据权利要求2所述的一种基于UWB定位和航迹定位的智能割草机定位装置，其特征在于，所述UWB定位控制器单元功能如下：

(2)通过定位算法得出定位标签的坐标(x_a，y_a)，则割草机的位置坐标为：

x_e＝X_a-X_r

y_e＝y_a-Y_r

5.根据权利要求3所述的一种基于UWB定位和航迹定位的智能割草机定位装置，其特征在于，所述航迹定位控制器单元功能如下：

(2)智能割草机上一时刻的定位坐标为(x₀，y₀)，通过航迹推算求出该时刻的割草机的定位坐标(x₁，y₁)，则：

x₁＝x₀+Scosθ

y₁＝y₀+Ssinθ。

6.根据权利要求1所述的一种基于UWB定位和航迹定位的智能割草机定位装置，其特征在于：所述定位基站和定位标签为单片机或芯片，所述姿态角检测模块为九轴传感器。

7.根据权利要求1所述的一种基于UWB定位和航迹定位的智能割草机定位装置，其特征在于：所述定位基站至少设有N个，且避免呈一条直线摆放，N为大于等于3的整数。

8.根据权利要求1～7任一项所述的一种基于UWB定位和航迹定位的智能割草机定位装置实现的定位方法，其特征在于，包括下列步骤：

(1)当定位标签模块可接受定位基站模块信号，基于UWB定位方法对智能割草机进行定位；

(2)当定位标签模块不可接受定位基站模块信号，则使用航迹推算定位方法求解定位坐标，对智能割草机进行定位；

9.根据权利要求8所述的一种基于UWB定位和航迹定位的智能割草机定位装置实现的定位方法，其特征在于，所述UWB定位方法如下：所述固定在智能割草机上的定位标签实时接收来自至少三个固定基站的信号，测得智能割草机上的定位标到各个基站A₁，A₂，A₃……A_n之间的距离分别为d₁，d₂，d₃……d_n，通过定位算法得出定位标签的定位坐标(x_a，y_a)，则所求割草机的位置坐标为：

x_e＝x_a-X_r

y_e＝y_a-Y_r

10.根据权利要求8所述的一种基于UWB定位和航迹定位的智能割草机定位装置实现的定位方法，其特征在于，所述航迹定位方法如下：姿态角检测模块测得该时刻的偏航角θ，电机驱动模块算出智能割草机左轮的转数N_left和右轮的转数N_right，智能割草机的轮子直径为D，智能割草机单位时间内移动的位移为S，则满足如下表达式：

智能割草机上一时刻的定位坐标为(x₀，y₀)，通过航迹推算求出该时刻割草机的定位坐标，则：

x₁＝x₀+Scosθ

y₁＝y₀+Ssinθ。