一种基于视觉检测的农业机器人定位导航方法及系统

1.本发明涉及农业机器人技术领域，具体涉及一种基于视觉检测的农业机器人定位导航方法及系统。


背景技术：

2.目前的室内导航系统，有自动巡线，无线定位和激光slam定位等多种方式。但是在农业温室大棚环境中，特别是一些路面未硬化的环境，自动巡线导航系统铺设不便，无线定位方式会受到信号干扰而激光slam定位导航结果很大成度上取决于激光扫描信息与地图的匹配效果，在农业环境中作物不同生长周期环境变化很大，无法通过前期建立的地图进行导航工作。


技术实现要素：

3.针对上述问题，本发明提出了一种基于视觉检测的农业机器人定位导航方法，包括：
4.在温室环境的农作物生长周期开始阶段，利用所述农业机器人安装的三维激光雷达建立所述温室环境的环境地图，并基于所述环境地图推算出温室环境垄间的视觉标签码在世界坐标系下的位姿；
5.基于所述环境地图和所述视觉标签码在世界坐标系的位姿，通过视觉检测法确定所述农业机器人的位姿；
6.将所述环境地图作为所述农业机器人的导航地图，并基于所述农业机器人的位姿对所述农业机器人进行自主导航。
7.可选的，农业机器人采用履带轮进行移动，所述农业机器人的车体顶部中心安装有三维激光雷达，车体顶部的可升降云台上安装有摄像头，车体底盘中心安装有imu姿态传感器，车体两侧车轮安装有编码器。
8.可选的，利用所述农业机器人安装的三维激光雷达建立所述温室环境的环境地图，包括：
9.通过所述三维激光雷达发射多线激光，并基于三维建图算法构建温室环境的3d点云地图，根据所述3d点云地图根据温室环境的垄的高度进行投影，基于所述投影确定用于农业机器人导航的2d栅格地图；
10.所述环境地图，包括：3d点云地图和2d栅格地图。
11.可选的，方法还包括：将视觉标签码在世界坐标系的位姿存储至标签码数组中。
12.可选的，基于所述环境地图和所述视觉标签码在世界坐标系下的位姿，通过视觉检测法确定所述农业机器人的位姿，包括：
13.通过农业机器人安装的摄像头识别温室环境垄间的视觉标签码，若所述摄像头识别到视觉标签码，调用标签码数组中存储的视觉标签码在世界坐标系下的位姿，通过视觉检测法对所述农业机器人的位姿进行修正，确定所述农业机器人的位姿。
14.可选的，通过农业机器人安装的摄像头识别温室环境垄间的视觉标签码，若所述摄像头未识别到视觉标签码，通过编码器和imu姿态传感器对所述农业机器人进行定位。
15.可选的，将所述环境地图作为所述农业机器人的导航地图，对所述农业机器人进行导航，包括：
16.调用环境地图，将所述环境地图中的2d栅格地图作为导航地图；
17.基于导航地图和调用ros的导航功能包，对所述农业机器人进行导航。
18.可选的，导航功能包，包括：预先规划出的农业机器人在温室环境垄间的运动路径。
19.再一方面，本发明还提出了一种基于视觉检测的农业机器人定位导航系统，包括：
20.数据采集单元，用于在温室环境的农作物生长周期开始阶段，利用所述农业机器人安装的三维激光雷达建立所述温室环境的环境地图，并基于所述环境地图推算出温室环境垄间的视觉标签码在世界坐标系下的位姿；
21.定位单元，基于所述环境地图和所述视觉标签码在世界坐标系下的位姿，通过视觉检测法确定所述农业机器人的位姿；
22.数据采集单元，用于将所述环境地图作为所述农业机器人的导航地图，并基于所述农业机器人的位姿对所述农业机器人进行自主导航。
23.与现有技术相比，本发明的有益效果为：
24.本发明通过在温室环境的农作物生长周期开始阶段，利用所述温室环境中的农业机器人通过安装的三维激光雷达建立所述温室环境的环境地图，并基于所述环境地图推算得到，所述温室环境垄间的视觉标签码在世界坐标系下的位姿；基于所述环境地图和所述视觉标签码相对于世界坐标系的位姿，通过视觉辅助所述农业机器人的定位；将所述环境地图作为所述农业机器人的导航地图，对所述农业机器人进行导航。得到一种适用于快速生长型作物的农业机器人视觉辅助定位方法。且本发明不需要铺设导航线路且基本不会受到农作物和杂草生长的影响，可以实现农业机器人在温室环境下农作物生长全周期的自主定位与导航功能。
附图说明
25.图1为本发明实施例的方法流程图；
26.图2为本发明实施例的农业机器人的结构示意图；
27.图3为本发明实施例的温室环境垄间视觉标签码的布置位置示意图；
28.图4为本发明实施例的步骤s1具体实施的流程图；
29.图5为本发明实施例的步骤s2具体实施的流程图；
30.图6为本发明实施例的系统结构图。
具体实施方式
31.现在参考附图介绍本发明的示例性实施方式，然而，本发明可以用许多不同的形式来实施，并且不局限于此处描述的实施例，提供这些实施例是为了详尽地且完全地公开本发明，并且向所属技术领域的技术人员充分传达本发明的范围。对于表示在附图中的示例性实施方式中的术语并不是对本发明的限定。在附图中，相同的单元/元件使用相同的附
图标记。
32.除非另有说明，此处使用的术语(包括科技术语)对所属技术领域的技术人员具有通常的理解含义。另外，可以理解的是，以通常使用的词典限定的术语，应当被理解为与其相关领域的语境具有一致的含义，而不应该被理解为理想化的或过于正式的意义。
33.实施例1：
34.本发明提出了一种基于视觉检测的农业机器人定位导航方法，如图1所示，包括：
35.步骤s1、在温室环境的农作物生长周期开始阶段，利用所述农业机器人安装的三维激光雷达建立所述温室环境的环境地图，并基于所述环境地图推算出温室环境垄间的视觉标签码在世界坐标系下的位姿；
36.步骤s2、基于所述环境地图和所述视觉标签码在世界坐标系下的位姿，通过视觉检测法确定所述农业机器人的位姿；
37.步骤s3、将所述环境地图作为所述农业机器人的导航地图，并基于所述农业机器人的位姿对所述农业机器人进行自主导航。
38.其中，步骤s1中所用的农业机器人，如图2所示，采用履带轮进行移动，所述农业机器人的车体顶部中心安装有三维激光雷达，车体顶部的可升降云台上安装有摄像头，车体底盘中心安装有imu姿态传感器，车体两侧车轮安装有编码器。
39.步骤s1中，用所述农业机器人安装的三维激光雷达建立所述温室环境的环境地图，包括：
40.通过所述三维激光雷达发射多线激光，并基于三维建图算法构建温室环境的3d点云地图，根据所述3d点云地图根据温室环境的垄的高度进行投影，基于所述投影确定用于农业机器人导航的2d栅格地图；
41.所述环境地图，包括：3d点云地图和2d栅格地图。
42.步骤s1的具体实施步骤，如图3所示，包括：:
43.步骤ss21在进行初次建图时，只需要使用三维激光雷达结合imu和编码器进行slam建图工作，并需要操纵机器人(农业机器人)在温室环境垄间运动，直到完成整个大棚的建图工作，进一步地将3d点云地图转换为2d栅格地图进一步地提供给后面的导航功能使用；
44.ss22：在使用多线激光雷达建图过程中，打开摄像头实时识别视觉标签码，一旦识别到标签码，便计算其与相机相对位置关系并记录下来；
45.ss23：利用激光slam算法实时匹配出来的机器人位姿和步骤ss22中获取的标签码与相机的相对位置关系，标定标签码在环境中的位姿，即通过将slam算法发布的机器人自身坐标系b到世界坐标系w的tf转换为齐次变换矩阵t
bw
，并实际测量相机坐标系c与机器人自身坐标系b的相对位置关系，获取相机坐标系c相对于机器人自身坐标系b的齐次变换矩阵t
cb
，当相机观察到标签码时可以计算出相机坐标系c相对于标签码坐标系m的齐次变换矩阵t
cm
，定义标签码坐标系m相对于世界坐标系w的齐次变换矩阵为t
mw
，t
mw
的计算公式如下；
46.t
mw
＝t
bw
t
cb
(t
cm
)-1
47.将齐次变换矩阵t
bw
中的平移矩阵与旋转矩阵提取出来，获得标签码在世界坐标系下的姿态pm＝(xm,ym,θm)；
48.ss24：判断相同id的标签码数据数量，当其大于100时执行ss25，否则跳转执行
ss22；
49.ss25：这一步计算同一id标签码的位姿均值，计算出来后剔除与其相差较大的值再二次求取均值；
50.ss26：将上述计算得到的视觉标签码位姿均值作为标签码在世界坐标系中的实际位姿保存在标签码数组中。
51.步骤s2中，基于所述环境地图和所述视觉标签码在世界坐标系下的位姿，通过视觉辅助所述农业机器人的定位，包括：
52.通过农业机器人安装的摄像头识别温室环境垄间的视觉标签码，视觉标签码在温室环境的位置如图4所示，若所述编码器识别到视觉标签码，调用标签码数组中存储的视觉标签码在世界坐标系下的位姿，对所述农业机器人的位姿进行修正，即完成视觉辅助所述农业机器人的定位。
53.步骤s2中通过农业机器人安装的标签码数组识别温室环境垄间的视觉标签码，若所述编码器未识别到视觉标签码，通过编码器和imu姿态传感器对所述农业机器人进行定位。
54.步骤s2具体实施如图5所示，包括：
55.ss31：接收传感器的数据，包括编码器、imu和视觉摄像头数据；
56.ss32：判断是否识别到视觉标签码的数据信息，如果没有则执行ss33，有的话执行ss34；
57.ss33：如果相机在t时刻没有观察到标签码，则计算k时刻(上一次观察到视觉标签码的时刻)利用视觉标签码获取的姿态θk与imu获取的姿态γk的差值δθ，在t时刻imu获取的姿态γ
t
中加入δθ，作为对imu姿态的修正，修正后t时刻机器人的姿态为θ
t
＝γ
t
+δθ。同时记录下t-1时刻编码器测得的左右轮速度为v
l
和vr，并计算机器人的速度v：
[0058][0059]
进一步计算出t-1时刻到t时刻机器人移动的距离δd及δd在x与y方向上的分量δx与δy：
[0060]
δd＝v
·
δt
[0061]
δx＝δd
·
cosθ
t-1
[0062]
δy＝δd
·
sinθ
t-1
[0063]
最后计算t时刻机器人的位置x
t
＝x
t-1
+δx，y
t
＝y
t-1
+δy，则t时刻机器人的姿态为p
t
＝(x
t
,y
t
,θ
t
)；
[0064]
ss34：如果相机在t时刻可以观察到标签码，则利用pnp算法求解出相机坐标系c相对于标签码坐标系m的旋转变换矩阵r与平移矩阵t，将其转换为齐次变换矩阵t
cm
：
[0065][0066]
ss35：利用ss33中标签码位置的标定可以获取标签码坐标系m相对于世界坐标系w的齐次变换矩阵t
mw
，通过实际测量相机坐标系c与机器人自身坐标系b的相对位置关系，获取机器人自身坐标系b相对于相机坐标系c的齐次变换矩阵t
bc
，定义机器人自身坐标系相对于世界坐标系的齐次变换矩阵为t
bw
，则：
[0067]
t
bw
＝t
mw
t
cm
t
bc
[0068]
最后将齐次变换矩阵t
bw
中的平移矩阵与旋转矩阵提取出来，获得t时刻机器人相对于世界的位姿p
t
＝(x
t
,y
t
,θ
t
)；
[0069]
ss36：将t时刻机器人在世界坐标系下的位姿发布出来。
[0070]
步骤s3中，将所述环境地图作为所述农业机器人的导航地图，并基于所述农业机器人的位姿对所述农业机器人进行自主导航，包括：
[0071]
调用环境地图，将所述环境地图中的2d栅格地图作为导航地图；
[0072]
基于导航地图和调用ros的导航功能包，对所述农业机器人进行导航。
[0073]
其中，导航功能包，包括：预先规划出的农业机器人在温室环境垄间的运动路径。
[0074]
基于同一发明构思，本发明还提出了一种基于视觉检测的农业机器人定位导航系统200，如图6所示，包括：
[0075]
数据采集单元201，用于在温室环境的农作物生长周期开始阶段，利用所述农业机器人安装的三维激光雷达建立所述温室环境的环境地图，并基于所述环境地图推算出温室环境垄间的视觉标签码在世界坐标系的位姿；
[0076]
定位单元202，基于所述环境地图和所述视觉标签码在世界坐标系下的位姿，通过视觉检测法确定所述农业机器人的位姿；
[0077]
数据采集单元203，用于将所述环境地图作为所述农业机器人的导航地图，并基于所述农业机器人的位姿对所述农业机器人进行自主导航。
[0078]
与现有技术相比，本发明的有益效果为：
[0079]
本发明通过在温室环境的农作物生长周期开始阶段，利用所述温室环境中的农业机器人通过安装的三维激光雷达建立所述温室环境的环境地图，并基于所述环境地图推算得到，所述温室环境垄间的视觉标签码相对于世界坐标系的位姿；基于所述环境地图和所述视觉标签码在世界坐标系下的位姿，通过视觉辅助所述农业机器人的定位；将所述环境地图作为所述农业机器人的导航地图，对所述农业机器人进行导航。得到一种适用于快速生长型作物的农业机器人视觉辅助定位方法。且本发明不需要铺设导航线路且基本不会受到农作物和杂草生长的影响，可以实现农业机器人在温室环境下农作物生长全周期的自主定位与导航功能。
[0080]
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0081]
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0082]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指
令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0083]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0084]
以上仅为本发明的实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均包含在发明待批的本发明的权利要求范围之内。