一种用于农田建图的定位系统

1.本发明涉及农田建图的技术领域，更具体地，涉及一种用于农田建图的定位系统。


背景技术：

2.随着农业科技的发展，目前在农田劳作中逐渐采用机器劳作代替传统的手工劳作，大大提高了生产力。但是，机器劳作还有很多技术不成熟的地方，例如，目前市面上大部分的机器导航和建图定位都是采用gps和imu技术进行组合，但是由于gps在偏远山区的接收信号比较差，因此，使用gps和imu无法达到精准定位，限制了人们的使用。


技术实现要素：

3.本发明旨在克服上述现有技术的至少一种缺陷(不足)，提供一种用于农田建图的定位系统，用于解决农田机器的在农田上生成二维地图的精准定位问题，以便与在丘陵山区也能推广使用。
4.为解决上述技术问题，本发明采取的技术方案是：
5.一种用于农田建图的定位系统，包括底盘、控制组件、激光调整装置、激光发射与接收装置和激光信号反馈装置；
6.所述底盘用于对整个定位系统提供支撑和动力；
7.所述控制组件设置于底盘内，用于对整个定位系统进行控制以及信息交互；
8.所述激光调整装置设置于底盘上方，且与控制组件电连接，用于与控制组件进行信息交互；
9.所述激光发射与接收装置设置于激光调整装置的上方，用于发射或接收激光，并与所述激光调整装置电连接，以通过所述激光调整装置调节上下位移；
10.所述激光信号反馈装置包括若干，且设置于所需建图的农田边界处，用于向激光发射与接收装置反馈激光，以提示激光发射与接收装置发出的激光高度；
11.所述定位系统进行定位时包括以下步骤：
12.s1：启动工作，操作人员给控制组件发送运动指令和预定路径，控制组件驱动底盘在农田内行走，并启动激光发射与接收装置、激光调整装置；
13.s2：所述激光调整装置自动调节激光发射与接收装置的高度，使激光发射与接收装置的激光发射中心线始终与激光信号反馈装置的中心线齐平；
14.s3：利用卡尔曼滤波kef算法，计算底盘当前建图的位姿和协方差矩阵q，以建立农田的二维地图。
15.本技术方案中，操作人员通过遥控底盘，并启动激光发射与接收装置、激光调整装置，以利用激光发射与接收装置扫描农田边界的点云生成地图，并利用激光信号反馈装置获取底盘当前在地图的定位，以实现边建图边定位。
16.具体地，激光信号反馈装置的在农田边界上的设置位置不限，其可以随机设置于需要建图或控制的农田边界处。
17.具体地，步骤s1中，通过驱动底盘在农田行走，并启动激光发射与接收装置、激光调整装置，以在农田中进行建图的定位。具体地，激光发射与接收装置通过以一定的频率360
°
扫描农田周围的环境，以向周围的激光信号反馈装置发射光束，并接收从激光信号反馈装置反射回来的激光束，从而获取周围激光信号反馈装置的具体坐标和方位，从而在农田中进行建图的定位。
18.步骤s3中，通过步骤s1中获取定位方法，可以获取周围激光信号反馈装置的具体坐标和方位，以便于计算出底盘当前的状态空间中的坐标和协方差矩阵，从而建立农田地图。
19.进一步地，步骤s2中，所述激光调整装置自动调节激光发射与接收装置高度的具体步骤为：
20.s21：所述激光发射与接收装置不断向所述激光信号反馈装置发射激光，所述激光信号反馈装置通过反射将激光反馈回给激光发射与接收装置，所述激光发射与接收装置通过检测所发射和所接收激光的位置，判断底盘在竖直方向上发生的位移δzmm以及每一激光信号反馈装置相对于底盘的坐标信息，并将位移信息和坐标信息传输给激光调整装置；
21.s22：所述激光调整装置接收到激光发射与接收装置传输来的位移信息后，根据位移信息向上或向下移动δzmm，所述激光发射与接收装置随着激光调整装置的移动而移动，以使激光发射与接收装置的激光发射中心线与激光信号反馈装置的中心线齐平。
22.在农田行驶中，由于农田中会出现石块、小坡或小坑等情况，会导致农田的地表不平，进而导致激光发射与接收装置在行驶中会出现上、下位移的情况，导致激光发射与接收装置的激光发射中心线无法与激光信号反馈装置的中心线齐平。本技术方案中，通过激光调整装置自动调整激光发射与接收装置的位移，以使激光发射与接收装置的激光发射中心线与激光信号反馈装置的中心线始终保持重合，以使激光处于最佳的反射与接收状态，从而使激光发射与接收装置更有效地接收激光信号反馈装置的信息。
23.进一步地，步骤s3中，计算底盘当前建图的位姿和协方差矩阵q的具体计算步骤为：
24.s31：获取底盘初始的状态空间参数，并计算t时刻底盘的协方差预测方程；
25.s32：编写观测模型，并计算观测对状态空间总的雅克比矩阵；
26.s33：计算增益并更新协方差和状态空间。
27.进一步地，步骤s31中，获取底盘初始的状态空间参数，并计算t时刻底盘的协方差预测方程的具体步骤为：
28.以底盘为中心建立坐标系，设定底盘当前的位姿为x，y，θ；其中，x为底盘在x轴的坐标，y为底盘在y轴的坐标，θ为底盘的转动角度；
29.设定当前的状态空间中有n个反光柱，并设定指第n个反光柱的x、y坐标；其中，n＞0，此时，n个反光柱相对于底盘当前位姿的状态空间坐标集合x为：
[0030][0031]
设定底盘初始的位姿为x0，y0，θ0；则第t时刻，底盘的位姿为x
t
，y
t
，θ
t
；设定盘轮速计的线速度σv和角速度σ
ω
均服从均值为0的高斯分布，根据协方差的计算公式：
[0032][0033]
可得初始时刻，底盘轮速计控制的协方差∑
ξ，0
为：
[0034][0035]
其中，设定∑
x
，∑y，∑
θ
的初始值为1；
[0036]
而在第t时刻，底盘轮速计控制的协方差∑
t
为：
[0037][0038]
计算后得状态量在t时刻的协方差预测方程∑
t
为：
[0039][0040]
其中，g
ξ，t
是运动模型关于机器人位姿ξ
t-1
雅可比矩阵；
[0041][0042]gu
是运动模型关于控制(轮速计线速度和角速度)的雅可比矩阵；
[0043]
[0044]
进一步地，步骤s32中，编写观测模型，并计算观测对状态空间总的雅克比矩阵的具体计算步骤为：
[0045]
设定当前激光发射与接收装置检测到的农田上有k组激光信号反馈装置，并设定k组激光信号反馈装置的坐标信息分别为
[0046]
通过数据关联，判断关联性：其中，当利用状态空间的激光信号反馈装置的坐标能够位移和旋转得到的当前激光发射与接收装置中的激光信号反馈装置的坐标，则说明有关联；否则为无关联；设定状态空间中找到对应有关联的激光信号反馈装置为k2组；在状态空间无法关联的激光信号反馈装置有k3组；其中，k3＝k-k2；则观测模型的方程可写为：
[0047][0048]
其中，表示通过数据关联，匹配上状态空间内的激光信号反馈装置坐标；
[0049]
其中，设定观测的第i组激光信号反馈装置和状态空间的第j组激光信号反馈装置匹配上，该激光信号反馈装置对应的子雅克比矩阵为：
[0050][0051]
通过转换得观测对状态空间总的雅克比矩阵为：
[0052][0053]
然后进行匹配上的激光信号反馈装置更新。
[0054]
进一步地，步骤s33中，计算增益并更新协方差和状态空间的具体步骤为：
[0055]
通过观测的协方差矩阵q计算增益k
t
，其中，
[0056][0057]
更新协方差∑
t
，其中：
[0058]
∑
t
＝(i-k
tht
)∑
t
[0059]
更新状态空间x
t
，其中，
[0060]
[0061][0062]
这样利用卡尔曼滤波kef算法对之前观测到激光信号反馈装置坐标和当前观测到激光信号反馈装置的坐标进行匹配，完成了建图时的定位。
[0063]
进一步地，建立农田的二维地图的具体步骤为：
[0064]
利用步骤s3获得当前底盘建图的位姿和协方差，并调用外部的建图函数，生成局部地图；再通过局部地图回环，形成完整的栅格地图。
[0065]
激光雷达扫描农田的边界，调用外部的建图函数，例如激光slam函数，最终生成完整的农田二维地图，生成完整的二维地图可用于后面的路径规划以及农田作业规划。
[0066]
进一步地，外部的建图函数可以为slam建图算法，如cartograph算法、gmapping算法的建图匹配函数和类，生成局部地图，再通过局部地图回环，形成完整的栅格地图。
[0067]
进一步地，当系统显示建立出完整的地图时，即完成了农田的建图工作；当系统显示没有建立出完整的地图时，则系统重复步骤s33，以再次进行农田建图。
[0068]
进一步地，所述控制组件包括设置于底盘内、且相互电连接的电机、工控机、中央处理器和通信电路；所述通信电路与激光调整装置进行信号通信，所述工控机与激光发射与接收装置电连接，用于接收所述激光发射与接收装置传输来的信息；所述中央处理器与激光发射与接收装置进行信息交互，用于对激光发射与接收装置接收的点云信息处理以及生成建图数据。
[0069]
进一步地，所述激光调整装置包括电动推杆和连接件，所述连接件用于安装所述激光发射与接收装置，所述电动推杆与控制组件连接，用于推动所述连接件上下移动，进而推动所述激光发射与接收装置进行上下位移。
[0070]
进一步地，所述激光发射与接收装置包括二维激光雷达和激光数据传输线，所述二维激光雷达通过旋转不断发射激光并获取激光信号反馈装置的激光反馈信息，所述激光数据传输线用于将所述激光反馈信息传输给控制组件进行处理。
[0071]
本技术方案中，所述二维激光雷达可以360
°
旋转，以不断获取周围的激光发射信息；所述激光数据传输线与底盘的工控机互连，用于将激光反射信息传输至工控机中处理。
[0072]
进一步地，所述激光信号反馈装置为固定于农田边界的反光柱，用于向激光发射与接收装置反射激光光束。
[0073]
进一步地，所述反光柱上设置有反光贴，所述反光柱至少包括3组。
[0074]
通过设置至少3组反光柱，以及在反光柱上设置反光贴，以更好地反射所述激光发射与接收装置的激光束，进而便于激光发射与接收装置检测激光束在竖直方向上的位移距
离，从而以便于激光调整装置推动激光发射与接收装置进行上下移动，使激光发射与接收装置的发射中心线始终与激光信号反馈装置的中心线齐平。
[0075]
在农田内容易出现定位信号丢失，定位数据不准的情况，而利用激光雷达生成二维地图对定位精度要求较高，所以利用激光发射与接收装置、反光柱定位的方法能够用于生成农田的二维地图，便于后面连续在生成的二维农田地图进行路径规划工作。
[0076]
与现有技术相比，本发明的有益效果为：
[0077]
本发明提供的农田建图方法通过设置激光信号反馈装置以对激光发射与接收装置进行定位，进而对底盘进行定位，以有效地进行建立农田地图，且具有定位精度高、维护成本低的有益效果。
[0078]
本发明的农田建图定位方法还通过设置激光调整装置，以调整激光发射与接收装置的高度，使激光信号反馈装置、激光发射与接收装置两者始终处于最佳的接收位置，从而更精准、有效地在农田中进行建图定位，便于在丘陵山区的农田中快速而准确地生成农田的二维地图，用于后续的路径规划。
附图说明
[0079]
图1为本发明的定位系统结构示意图一。
[0080]
图2为本发明的定位系统结构示意图二。
[0081]
图3为本发明的定位系统工作流程图。
具体实施方式
[0082]
本发明附图仅用于示例性说明，不能理解为对本发明的限制。为了更好说明以下实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。
[0083]
实施例1
[0084]
如图1和图2所示，本实施例公开了一种用于农田建图的定位系统，包括底盘100、控制组件(图中未画出)、激光调整装置300、激光发射与接收装置200和激光信号反馈装置400。
[0085]
其中，所述底盘100位于整个定位系统的底部，用于对整个定位系统提供支撑和动力，并通过底盘100上的轮子进行移动行走。
[0086]
所述控制组件设置于底盘100内，包括相互电连接的电机、工控机、中央处理器和通信电路；其中，通信电路用于与激光调整装置300进行信号通信；工控机与激光发射与接收装置200电连接，用于接收激光发射与接收装置200传输来的信息；中央处理器与激光发射与接收装置200进行信息交互，用于对激光发射与接收装置200接收的点云信息处理以及生成建图数据。
[0087]
所述激光调整装置300设置于底盘100的正上方，且与控制组件电连接，包括电动推杆和连接件，所述连接件用于安装并固定激光发射与接收装置200；电动推杆与控制组件连接，用于与控制组件进行信息交互，并推动连接件上下移动，进而推动激光发射与接收装置200进行上下位移。
[0088]
所述激光发射与接收装置200设置于激光调整装置300的正上方，用于发射或接收
激光，并与所述激光调整装置300电连接，以通过所述激光调整装置300调节上下位移。
[0089]
所述激光信号反馈装置400至少包括3组，设置于所需建图的农田边界处，用于向激光发射与接收装置200反馈激光，以提示激光发射与接收装置200发出的激光高度。
[0090]
具体地，本实施例中，所述激光信号反馈装置400为反光装置，所述反光装置为3组固定大小的反光柱和反光贴，其中，反光贴贴设于反光柱上，用于反射激光发射与接收装置200的激光光束，以便激光发射与接收装置200识别检测激光的位移距离，进而便于激光调整装置300对激光发射与接收装置200的高度进行适应性调整，以使激光发射与接收装置200的激光发射中心线始终与激光信号反馈装置400的中心线齐平。
[0091]
具体地，本技术方案中，激光调整装置300对激光发射与接收装置200的高度进行适应性调整的具体步骤为：
[0092]
s1：启动工作，操作人员给控制组件发送运动指令和预定路径，控制组件驱动底盘100在农田内行走，并启动激光发射与接收装置200、激光调整装置300；
[0093]
s21：所述激光发射与接收装置200不断向所述激光信号反馈装置400发射激光，所述激光信号反馈装置400通过反射将激光反馈回给激光发射与接收装置200，所述激光发射与接收装置200通过检测所发射和所接收激光的位置，判断底盘100在竖直方向上发生的位移δzmm以及每一激光信号反馈装置400相对于底盘100的坐标信息，并将位移信息和坐标信息传输给激光调整装置300；
[0094]
s22：所述激光调整装置300接收到激光发射与接收装置200传输来的位移信息后，根据位移信息向上或向下移动δzmm，所述激光发射与接收装置200随着激光调整装置300的移动而移动，以使激光发射与接收装置200的激光发射中心线与激光信号反馈装置400的中心线齐平。
[0095]
进一步地，本技术方案的定位系统中，还包括对农田建图的定位以及农田栅格地图的建立方法，具体地，所述对农田建图的定位方法主要通过激光发射与接收装置200以固定频率向周围的激光信号反馈装置400发射激光光束，再通过激光信号反馈装置400反射激光光束；所述激光发射与接收装置200通过反射回来的激光光束，从而获取周围激光信号反馈装置400的具体坐标和方位，通过激光信号反馈装置400的具体坐标和方位，即可计算出底盘100的坐标和方位，从而对农田建图进行定位。
[0096]
进一步地，通过利用上述的农田建图的定位方法，即可计算出底盘100当前的状态空间中的坐标和协方差矩阵，从而便于进行农田栅格地图的建立。
[0097]
如图3所示，具体地，本技术方案的定位系统进行农田建图的定位时，包括以下步骤：
[0098]
s31：获取底盘100初始的状态空间参数，并计算t时刻底盘100的协方差预测方程；其中，底盘100初始的状态空间参数包括底盘的运动方程、协方差矩阵的初始参数等；
[0099]
s32：编写观测模型，并计算观测对状态空间总的雅克比矩阵；
[0100]
s33：计算增益并更新协方差和状态空间。
[0101]
其中，步骤s31中，获取底盘100初始的状态空间参数，并计算t时刻底盘100的协方差预测方程的具体步骤为：
[0102]
以底盘100为中心建立坐标系，设定底盘100当前的位姿为x，y，θ；其中，x为底盘100在x轴的坐标，y为底盘100在y轴的坐标，θ为底盘100的转动角度；
[0103]
设定当前的状态空间中有n个反光柱，并设定指第n个反光柱的x、y坐标；其中，n＞0，此时，n个反光柱相对于底盘100当前位姿的状态空间坐标集合x为：
[0104][0105]
设定底盘100初始的位姿为x0，y0，θ0；则第t时刻，底盘100的位姿为x
t
，y
t
，θ
t
；设定盘轮速计的线速度σv和角速度σ
ω
均服从均值为0的高斯分布，根据协方差的计算公式：
[0106][0107]
可得初始时刻，底盘100轮速计控制的协方差∑
ξ，0
为：
[0108][0109]
其中，设定∑
x
，∑y，∑
θ
的初始值为1；
[0110]
而在第t时刻，底盘100轮速计控制的协方差∑
t
为：
[0111][0112]
计算后得状态量在t时刻的协方差预测方程∑
t
为：
[0113][0114]
其中，g
ξ，t
是运动模型关于机器人位姿ξ
t-1
雅可比矩阵；
[0115][0116]gu
是运动模型关于控制(轮速计线速度和角速度)的雅可比矩阵；
[0117][0118]
步骤s32中，编写观测模型，并计算观测对状态空间总的雅克比矩阵的具体计算步骤为：
[0119]
设定当前激光发射与接收装置200检测到的农田上有k组激光信号反馈装置400，并设定k组激光信号反馈装置400的坐标信息分别为
[0120]
通过数据关联，判断关联性：其中，当利用状态空间的激光信号反馈装置400的坐标能够位移和旋转得到的当前激光发射与接收装置200中的激光信号反馈装置400的坐标，则说明有关联；否则为无关联；设定状态空间中找到对应有关联的激光信号反馈装置400为k2组；在状态空间无法关联的激光信号反馈装置400有k3组；其中，k3＝k-k2；则观测模型的方程可写为：
[0121][0122]
其中，表示通过数据关联，匹配上状态空间内的激光信号反馈装置400坐标；
[0123]
其中，设定观测的第i组激光信号反馈装置400和状态空间的第j组激光信号反馈装置400匹配上，该激光信号反馈装置400对应的子雅克比矩阵为：
[0124][0125]
通过转换得观测对状态空间总的雅克比矩阵为：
[0126][0127]
步骤s33中，计算增益并更新协方差和状态空间的具体步骤为：
[0128]
通过观测的协方差矩阵q计算增益k
t
，其中，
[0129][0130]
更新协方差∑
t
，其中：
[0131]
∑
t
＝(i-k
tht
)∑
t
[0132]
更新状态空间x
t
，其中，
[0133][0134][0135]
这样通过利用卡尔曼滤波kef算法对之前观测到反光柱坐标和当前观测到反光柱的坐标进行匹配，即完成了建图时的定位。
[0136]
具体地，本实施例中，对农田栅格地图的建立，可以通过以下方法实现：通过上述的农田建图定位方法，获取底盘100当前的坐标和协方差，再利用slam建图算法如cartograph算法、gmapping算法的建图匹配函数和类，最后通过局部地图回环，形成完整的栅格地图。
[0137]
显然，本发明的上述实施例仅仅是为清楚地说明本发明技术方案所作的举例，而并非是对本发明的具体实施方式的限定。凡在本发明权利要求书的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。