基于超宽带的农用无人车集群作业方法

本发明涉及无人车集群控制技术，具体涉及一种基于uwb(ultrawideband，超宽带)的农用无人车集群作业方法。

背景技术：

随着现代科学技术与农业的不断结合，未来农业生产活动更加注重智能化和信息化，农业生产环境逐步向智能农业大棚转变，农用无人车的推广和应用成为必然，其分工也越来越明确和精细化，因此人们开始关注具有不同功能的多辆农用无人车在这种标准大棚环境下的协同作业能力。其中最关键和最需要解决的问题是大棚内的车辆精确定位问题，目前广泛使用的室内定位技术有wi-fi、zigbee、蓝牙、超声波、光学成像、uwb等，其中wi-fi、zigbee、蓝牙等虽然成本低，但是定位精度不高，不利于无人车在大棚环境下的精确作业；超声波定位虽然具有较高精度，但是实时性较差；而光学成像又对环境光线要求较高，定位精度不稳定。相比之下，uwb技术具有精度高、抗干扰性好、多径分辨能力强、能实时定位等明显优势，其特点适合用于农用无人车在大棚内的精确定位，方便集群管理。

技术实现要素：

本发明的目的是提出一种支持多辆不同功能的农用无人车在标准农业大棚环境下集群作业以完成复杂农业任务的作业方法。

本发明采用的技术方案是：基于uwb的农用无人车集群作业方法包括以下步骤：

步骤1，利用三个基站和若干放置在农用无人车上的标签建立大棚环境下的农用无人车定位系统，其中三个基站呈“l”形放置以构成直角坐标系，用该直角坐标系下的坐标可以确定标签的位置，主要方法是利用基于测距的定位方法获取标签与各个基站之间的距离值，通过这些距离量建立用于定位的数学模型，进而通过建立的数学模型求解标签的位置坐标；步骤2，对农业大棚进行环境地图建模并将工作环境数字化，主要是将整个工作环境均匀地划分为一个个边长是m的正方形小栅格以实现对工作环境的栅格化处理，然后对栅格地图进行数字化，对这些栅格进行统一编号以简化数据的存储，并用二进制数0和1确定这些栅格是否为障碍物，为后面农用无人车在大棚内的路径规划提供依据；步骤3，进行农用无人车的路径规划，在已经构建定位系统和环境地图的基础上，确定农用无人车的起始栅格、目标栅格和静态障碍物栅格，利用人工势场法使多个农用无人车同时作业时按照规划好的路径移动且不与其他农用无人车和障碍物相撞，主要是将目标点视为对农用无人车的引力，其他农用无人车和障碍物视为对当前移动的农用无人车的斥力，农用无人车在每个单位运动时间内都根据矢量关系求得引力和斥力的合力，并根据合力确定其下一步的运动方向和期望到达的栅格位置；步骤4，根据步骤3矢量合成的力对农用无人车进行运动控制，将该力分解为沿x轴和y轴两个方向的力，将沿x轴方向的力作为农用无人车的方向期望输入到pid角速度环，将沿y轴方向的力作为农用无人车的油门期望输入到pid速度环，然后分别将这两个pid输出值进行加法运算和减法运算，作用到农用无人车的左右车轮上形成差速控制，农用无人车在左右车轮差速作用下向规划的方向和目标点移动并能够避让其他农用无人车。

进一步，所述步骤1具体包括：

首先放置三个固定的基站，将三个基站a、b、c摆放在矩形状农业大棚的其中三个角上，以中间的基站b作为原点建立直角坐标系；然后将标签安装在农用无人车上，利用基于测距的定位方法获取标签与各个基站之间的距离值，通过这些距离量建立用于定位的数学模型，进而通过建立的数学模型求解标签的位置坐标；

进一步，所述利用基于测距的定位方法获取距离值并建立数学模型求解具体坐标值的具体方法是：

步骤1.1，利用基于接收信号时间法的双程测距算法来测算标签离三个基站的距离值：标签在t0时刻向基站发送含有时间戳信息的数据包，基站对接收到的数据包中的时间信息进行同步，耗时tr，然后给标签回传一个数据包，标签在t1时刻接收到数据完成同步，根据信号传播时间信息可以计算出信号传播用时，进而求得标签与基站之间的距离；

步骤1.2，利用三边测量法得到标签在直角坐标系中的具体坐标值：通过步骤1.1可以得到标签i与三个基站a、b、c的距离，三个基站的坐标为已知的设定值，将标签的坐标表示为(xi,yi)，那么可以列出三个基站坐标关于标签坐标(xi,yi)的表达式，进而通过数学变换可以求得标签i的坐标(xi,yi)。

进一步，所述步骤2的具体方法是：

步骤2.1，对大棚下的工作环境进行栅格化，根据农用无人车的大小并结合uwb定位算法存在的误差，将栅格设定为边长是m(cm)的正方形，用这种栅格将工作环境均匀的进行划分；

步骤2.2，对栅格进行编号，从左至右、从下至上依次对栅格用序号的方法进行编号；

步骤2.3，为栅格赋值，用不同的值表示工作环境中能够行驶的路径信息和障碍物分布信息。

进一步，所述步骤3包括：

在步骤2所建立的栅格化地图的基础上建立人工势场来指示农用无人车的运动方向，目标点对农用无人车的力为引力，其他农用无人车和障碍物对当前移动的农用无人车来说是斥力；在势场中，根据矢量关系求得引力和斥力的合力，并根据合力确定农用无人车下一步的运动方向和期望到达的栅格位置。

进一步，所述建立人工势场的具体实现方法是：

步骤3.1，引力场：将农用机器人的位置简化为中心位置所在的栅格，设该位置为x，目标栅格所在的位置为x0，目标位置对当前农用无人车产生一个引力，其方向为运动对象和目标点的连线并指向目标点，该力随着运动对象到目标点的距离增加而单调增大，用式子fa(x)＝-gradua(x)＝-ka|x-x0|来表示；式中ka为正比例增益系数；

步骤3.2，斥力场：将其他农用无人车或者障碍物简化为中心位置所在的栅格，设该位置为xz，其对当前农用无人车产生一个斥力，其方向为运动对象与障碍物的连线并指向运动对象，该力随着运动对象到障碍物的距离增加而单调减小，当超过一定距离后可以认为斥力为0，用式子来表示；式中d0为斥力场对障碍物的影响范围，kr为斥力场常量。

进一步，所述步骤4的pid控制器由比例单元p、积分单元i和微分单元d组成，速度环以沿y轴方向的力作为油门期望，利用磁编码器解算当前车轮的速度，将解算得到的车速反馈到输入端与油门期望做差得到速度误差；角速度环以沿x轴方向的力作为方向期望，利用陀螺仪估计当前车身的角速度，将估计得到的角速度反馈到输入端与方向期望做差得到角速度误差。假设速度环和角速度环的输出分别为pid_speed_loop_out和pid_anglespeed_loop_out，则将pid_speed_loop_out+pid_anglespeed_loop_out作用到农用无人车左边的车轮上，将pid_speed_loop_out-pid_anglespeed_loop_out作用到农用无人车右边的车轮上，左右车轮形成差速，农用无人车在左右车轮差速作用下向规划的方向和目标点移动并能够避让其他农用无人车。

本发明的有益效果是：

传统的农用无人车作业往往只能在同一个时间安排一辆农用无人车完成单一任务，这不仅造成了作业效率的低下，而且仍需要耗费大量时间。在传统的多智能车协作控制技术中，为了获得每个单体的在团队中的位置信息，往往利用激光雷达、单目摄像头和双目摄像头等视觉技术，但是这些技术成本昂贵，在应用于数量比较多的目标时，实现起来比较困难。这里提出了一种基于uwb定位的农用无人车集群作业的方法。利用uwb技术进行定位使得农用无人车在通信环境复杂的大棚内也具有非常优秀的多径分辨能力，系统的距离分辨能力达到厘米级，能够满足多农用无人车在室内集群作业的需求。利用基于接收信号时间法的双程测距算法来测算标签离三个基站的距离值，有效解决了移动的农用无人车难以和基站做到时间同步的问题，也体现了uwb信号时间分辨率高的特点，充分发挥了超宽带技术的优势，并且使误差较小。利用栅格法将坐标系分为一个个栅格，算法简单，表达形式也比较简单，环境地图很容易被创建和维护。将人工势场法和栅格法相结合，使规划出的农用无人车行驶路径相对平滑，安全系数高，而且结构简单易于实现。

附图说明

下面结合附图和具体实施实施方式对本发明做进一步详细说明：

图1是基于uwb的农用无人车整体结构图。

图2是基于uwb定位的农用无人车集群作业方法的流程图。

图3是在农业大棚中建立基于uwb定位方案的坐标系示例。

图4是对某农业大棚环境地图进行栅格化并对栅格进行编号的示例。

图5是多辆农用无人车在大棚下工作示意图。

其中：1-舵机；2-控制器；3-底板；4-联轴器；5-轴；6-法兰盘；7-电机；8-车轮。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

如图1所示，一种基于uwb的农用无人车结构示意图，由舵机1、控制器2、底板3、联轴器4、轴5、法兰盘6、电机7、车轮8等部分构成。进一步，所述底板3上预留有安装孔，舵机1和控制器2均通过m3防松螺母和m3螺丝与安装孔相连以便固定在底板上，所述舵机1通过舵盘和m3螺丝与联轴器4相连，所述联轴器4通过顶丝与轴5固定，所述轴5通过法兰盘6和m3螺丝与电机7相连，所述车轮8通过垫片和螺丝与电机上延伸出的轴相连。通过这种结构可以使农用无人车灵活转向且保持良好的稳定性，从而提高农用无人车在大棚环境下的作业效率。

图2为基于uwb定位的农用无人车集群作业方法的流程图，首先利用至少三个基站和若干标签建立大棚环境下的农用无人车定位系统，利用基于测距的定位方法得到农用无人车在大棚中的具体坐标值；然后对农业大棚进行环境地图建模并将工作环境数字化，为后面的路径规划提供依据；最后进行农用无人车的路径规划，使多个农用无人车同时作业时完全遍历整个工作区域且不与其他农用无人车相撞。

步骤1：利用至少三个基站和若干标签建立大棚环境下的农用无人车定位系统，利用基于测距的定位方法得到农用无人车在大棚中的具体坐标值。

(1)放置基站和标签，建立坐标系

具体方法是在矩形状农业大棚的其中三个角上放置三个基站a、b、c，图3是在农业大棚中建立基于uwb定位系统的坐标系示例，如图3所示以中间的基站b为原点、以直线ba为y轴、以直线bc为x轴建立直角坐标系，然后将标签i安装在农用无人车上，则在该直角坐标系中能用唯一确定的两个数来表示农用无人车的具体位置。

具体的，uwb定位基站和标签的最小系统硬件设计采用相同的结构，mcu为stm32f405rgt6，uwb定位模块为dwm1000，两者通过spi接口进行连接。其中，标签的软件设计具体过程包括：1)初始化spi等端口；2)进行自检，若通过自检则进行下一步，否则提示自检未通过；3)周期性发送广播信息；4)检测是否接收到基站应答，若接收到基站应答则进行下一步，否则继续进行上一步；5)得到时间数据并解算；6)求解标签到不同基站的距离；7)求解坐标数据。基站的软件设计具体过程包括：1)初始化spi等端口；2)进行自检，若通过自检则进行下一步，否则提示自检未通过；3)等待接收标签广播信息，若接收到则进行下一步，否则继续等待接收；4)向标签发送应答信息。

(2)利用基于测距的定位方法获取标签与各个基站之间距离值。

假设标签在t0时刻向基站发送含有时间戳信息的数据包，基站对接收到的数据包中的时间信息进行同步，耗时tr，然后给标签回传一个数据包，标签在t1时刻接收到数据表示同步完成，则信号传播时间为：

由于信号以光速传播，故标签与基站之间的距离为

d＝tt×c

式中c表示光速。

(3)通过距离量建立用于定位的数学模型，进而通过建立的数学模型求解标签的位置坐标。

假设测得标签i与三个基站a、b、c的距离分别为dai、dbi、dci，三个基站的坐标为已知的设定值(0,x0)、(0,0)、(0,y0)，将标签的坐标表示为(xi,yi)，那么可以列出式子

进而可以求得标签i的坐标(xi,yi)为

步骤2：对农业大棚进行环境地图建模并将工作环境数字化，为后面的路径规划提供依据。

(1)对大棚下的工作环境进行栅格化。

根据农用无人车的大小并结合uwb定位算法存在的误差，将栅格设定为边长是m(cm)的正方形，用这种栅格将工作环境均匀的进行划分。

(2)对栅格进行编号。

从左至右、从下至上依次对栅格用序号的方法进行编号，图4为栅格化地图的一个简单示例，其中栅格边长为30cm，黑色栅格表示作物种植区域。

(3)为栅格赋值。

使用二进制数0和1为栅格赋值来表示工作环境中能够行驶的路径信息和障碍物分布信息，当栅格所对应的位置有障碍物时用1表示，当栅格所对应的位置没有障碍物时用0表示，然后将栅格信息用行列矩阵的形式存储。

步骤3：进行农用无人车的路径规划，包括多个农用无人车同时开始作业时完全遍历整个工作区域且不相撞的全区域路径规划，单个农用无人车从工作区域以最短路径返回停放点的传统点到点的路径规划。

(1)建立人工势场。

在步骤2所建立的栅格化地图的基础上建立人工势场来指示农用无人车的运动方向，目标点对农用无人车的力为引力，其他农用无人车和障碍物对当前移动的农用无人车来说是斥力；在势场中，根据矢量关系求得引力和斥力的合力即为农用无人车下一步的运动方向，农用无人车受这两个力的合力作用向目标点移动并避让其他农用无人车。

进一步，所述建立人工势场的具体实现方法是：

建立引力场：将农用机器人的位置简化为中心位置所在的栅格，设该位置为x，目标栅格所在的位置为x0，目标位置对当前农用无人车产生一个引力，其方向为运动对象和目标点的连线并指向目标点，该力与距离正相关，表示为

fa(x)＝-gradua(x)＝-ka|x-x0|，

式中ka为正比例增益系数；

建立斥力场：将其他农用无人车或者障碍物简化为中心位置所在的栅格，设该位置为xz，其对当前农用无人车产生一个斥力，其方向为运动对象与障碍物的连线并指向运动对象，该力与距离反相关，表示为

式中d0为斥力场对障碍物的影响范围，kr为斥力场常量。

(2)进行路径规划。

以图5所示三辆农用无人车在大棚内同时进行作业为例进行说明，其具体过程为：

首先进行初始化，根据所建立的栅格地图确定农用无人车的起始点信息、目标点信息和静态障碍物信息。其中起始点信息可以简化为农用无人车中心点所对应的栅格坐标；目标点信息包括所规划的路径所经过的所有栅格坐标，其中离农用无人车最近的下一个栅格为农用无人车的实时目标点，当农用无人车到达该目标点后实时目标点转化为该栅格的下一个栅格；静态障碍物信息为大棚四周的墙壁和中间的作物种植区域所对应的栅格坐标。

然后利用人工势场确定农用无人车下一步前进的方向，如图5中a车所示，在基于uwb的栅格地图中已知a车位置信息、左侧墙壁位置信息、右侧作物位置信息的情况下，并且限定障碍物能够作用的距离范围，可以求出左侧墙壁对农用无人车产生一个向右的力f3，右侧作物对农用无人车产生一个向左的力f1，前方目标路径所在栅格对农用无人车产生一个向前的力f2，对这三个力求合力得矢量其大小为f，方向为(cosθ,sinθ)，使农用无人车沿该方向前进到达目标点。当农用无人车在行进过程中与另一辆农用无人车相遇时，则将该农用无人车也看成障碍物进行人工势场的分析并求合力，根据合力确定下一步的前进方向避免两辆农用无人车相撞。

步骤4：根据步骤3确定的前进方向对农用无人车进行运动控制。

将合力进行分解，引力沿x轴方向的分力为f·cosθ，该力作为农用无人车的方向期望，利用陀螺仪估计当前车身的角速度，将估计得到的角速度反馈到输入端与方向期望做差得到角速度误差errω(t)，对角速度误差进行pid控制，得到期望的角速度值ω(t)为：

其中kωp是角速度环的比例项系数，kωi是角速度环的积分项时间常数，kωd是角速度环的微分项时间常数。

引力沿y轴方向的分力为f·sinθ，该力作为农用无人车的油门期望，利用磁编码器解算当前车轮的速度，将解算得到的车速反馈到输入端与油门期望做差得到速度误差errv(t)，对速度误差进行pid控制，得到期望的速度值v(t)为：

其中kvp是角速度环的比例项系数，kvi是角速度环的积分项时间常数，kvd是角速度环的微分项时间常数。

然后分别将这两个pid输出值进行加法运算和减法运算，得到左右车轮的速度分别为：

left_speed＝w(t)+v(t)

right_speed＝w(t)-v(t)

将这两个速度分别作用到农用无人车的左右车轮上形成差速控制，使农用无人车向规划的方向和目标点移动并能够避让其他农用无人车。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。