CN112868623A - 一种基于多传感器的植保机械导航控制方法及系统 - Google Patents
一种基于多传感器的植保机械导航控制方法及系统 Download PDFInfo
- Publication number
- CN112868623A CN112868623A CN202011607088.5A CN202011607088A CN112868623A CN 112868623 A CN112868623 A CN 112868623A CN 202011607088 A CN202011607088 A CN 202011607088A CN 112868623 A CN112868623 A CN 112868623A
- Authority
- CN
- China
- Prior art keywords
- plant protection
- protection machine
- coordinate system
- measurement unit
- laser radar
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 26
- 241000196324 Embryophyta Species 0.000 claims abstract description 139
- 238000005259 measurement Methods 0.000 claims abstract description 46
- 240000008042 Zea mays Species 0.000 claims abstract description 24
- 235000005824 Zea mays ssp. parviglumis Nutrition 0.000 claims abstract description 24
- 235000002017 Zea mays subsp mays Nutrition 0.000 claims abstract description 24
- 235000005822 corn Nutrition 0.000 claims abstract description 24
- 238000006243 chemical reaction Methods 0.000 claims abstract description 16
- 238000004458 analytical method Methods 0.000 claims abstract description 9
- 230000004927 fusion Effects 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 27
- 230000009466 transformation Effects 0.000 claims description 19
- 241001057636 Dracaena deremensis Species 0.000 claims description 17
- 238000013519 translation Methods 0.000 claims description 9
- 238000009795 derivation Methods 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 description 5
- 206010063385 Intellectualisation Diseases 0.000 description 2
- 238000012271 agricultural production Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000011897 real-time detection Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- DMBHHRLKUKUOEG-UHFFFAOYSA-N diphenylamine Chemical compound C=1C=CC=CC=1NC1=CC=CC=C1 DMBHHRLKUKUOEG-UHFFFAOYSA-N 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 238000003306 harvesting Methods 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000000575 pesticide Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001988 toxicity Effects 0.000 description 1
- 231100000419 toxicity Toxicity 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Images
Classifications
-
- A—HUMAN NECESSITIES
- A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
- A01M—CATCHING, TRAPPING OR SCARING OF ANIMALS; APPARATUS FOR THE DESTRUCTION OF NOXIOUS ANIMALS OR NOXIOUS PLANTS
- A01M7/00—Special adaptations or arrangements of liquid-spraying apparatus for purposes covered by this subclass
- A01M7/0025—Mechanical sprayers
- A01M7/0032—Pressure sprayers
- A01M7/0042—Field sprayers, e.g. self-propelled, drawn or tractor-mounted
-
- A—HUMAN NECESSITIES
- A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
- A01M—CATCHING, TRAPPING OR SCARING OF ANIMALS; APPARATUS FOR THE DESTRUCTION OF NOXIOUS ANIMALS OR NOXIOUS PLANTS
- A01M7/00—Special adaptations or arrangements of liquid-spraying apparatus for purposes covered by this subclass
- A01M7/0089—Regulating or controlling systems
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64D—EQUIPMENT FOR FITTING IN OR TO AIRCRAFT; FLIGHT SUITS; PARACHUTES; ARRANGEMENT OR MOUNTING OF POWER PLANTS OR PROPULSION TRANSMISSIONS IN AIRCRAFT
- B64D1/00—Dropping, ejecting, releasing, or receiving articles, liquids, or the like, in flight
- B64D1/16—Dropping or releasing powdered, liquid, or gaseous matter, e.g. for fire-fighting
- B64D1/18—Dropping or releasing powdered, liquid, or gaseous matter, e.g. for fire-fighting by spraying, e.g. insecticides
Landscapes
- Life Sciences & Earth Sciences (AREA)
- Engineering & Computer Science (AREA)
- Pest Control & Pesticides (AREA)
- Insects & Arthropods (AREA)
- Wood Science & Technology (AREA)
- Zoology (AREA)
- Environmental Sciences (AREA)
- Aviation & Aerospace Engineering (AREA)
- Mechanical Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种基于多传感器的植保机械导航控制方法及系统，启动激光雷达，设置激光雷达的扫描角度和分辨率；启动里程计和惯性测量单元，里程计通过电机的光电编码器获得植保机械的速度，惯性测量单元获得该植保机械的位置姿态信息；进行激光雷达坐标系和惯性测量单元坐标系的联合标定；总控制器对数据进行融合转换分析，在玉米行间规划出一条导航路径，再根据当前位置姿态状态调整左右两驱动电机的转速以调整植保机械的运动状态，根据导航标定线的角度和方向控制植保机械的左右电机控制器，通过电子差速来控制植保机械的移动和转向。
Description
技术领域
本发明属于农业植保机械技术领域，具体地说，本发明涉及一种基于多传感器的植保机械导航控制方法及系统。
背景技术
随着农业机械技术的发展，农业机械向自动化、智能化方向的发展成为当今农业研究的热点，农业自动导航技术是实现农业机械装备自动化、信息化和智能化的关键技术之一，开发一套操作简单、成本低廉、导航精度高的植保机械自动导航系统，在农业生产中具有很高的现实意义，一方面可以提高作业质量，降低农民劳动强度，提高生产效率，另一方面可改善作业者的工作环境，避免农药对人体的毒害，减少了农业生产中生产力的投入、降低能耗、节约成本、增加农业经济效益。
目前的植保机械主要依托北斗导航定位系统实现，例如：植保无人机、植保机器人等。
如何精确实现耕、种、管、收中的“管”，使植保机械能够自如在玉米行间行走，并对玉米作物进行植保作业。本发明在对已有导航技术进行分析后，提出使用三维激光雷达、惯性测量单元、里程计作为主要的导航设备，进行不同田间环境下植保机械导航控制系统的研究，并确立了在玉米作物行间导航路径的拟合方法，设计了相应的导航控制系统，控制车辆沿拟合路径自动行走，完成导航控制。
发明内容
本发明提供一种基于多传感器的植保机械导航控制方法及系统，以解决上述背景技术中存在的问题。
为了实现上述目的，本发明采取的技术方案为：一种基于多传感器的植保机械导航控制方法，具体包括以下步骤，
步骤S1：调整植保机械的角度和位置，使其前端正对着玉米行间放置在田间地头；
步骤S2：启动激光雷达，设置激光雷达的扫描角度和分辨率；
步骤S3：启动里程计和惯性测量单元，里程计通过电机的光电编码器获得植保机械的速度，惯性测量单元获得该植保机械的位置姿态信息；
步骤S4：进行激光雷达坐标系和惯性测量单元坐标系的联合标定；
步骤S5：分别取植保机械左右两侧距离植保机械最近的三株玉米株作为扫描点，把扫描点两两组合，共能获得9组数据，并求其每组的中点坐标，作为该植保机械在玉米行间行走时导航路径的拟合数据点；
步骤S6：将激光雷达坐标系与惯性测量单元坐标系进行坐标转换的数据和里程计获得的植保机械电机转速数据发送给总控制器，总控制器对数据进行融合转换分析，根据导航标定线的角度和方向控制植保机械的左右电机控制器，通过电子差速来控制植保机械的移动和转向。
优选的，所述步骤S3中，里程计通过电机的光电编码器获得植保机械的速度，里程计通过电机的光电编码器能测量的数据，关系式如下：
矩阵形式表达为：
其中：v是底盘中心线线速度；ω是底盘中心线角速度；vR是右驱动轮线速度；vL是左驱动轮线速度；ωR是右驱动轮角速度；ωL是左驱动轮角速度；rR是右驱动轮转弯半径；rL是左驱动轮转弯半径；d是植保机械底盘中心圆弧运动的半径；J是转换变量。
优选的，所述步骤S4进行激光雷达坐标系和惯性测量单元坐标系的联合标定，具体步骤如下；
步骤S40：定义激光雷达坐标系为(O1X1Y1Z1),惯性测量单元坐标系为(O2X2Y2Z2)，则同一个扫描点在两个不同坐标系下的坐标分别表示为(x1,y1,z1)、(x2,y2,z2)；
步骤S41：激光雷达坐标系和惯性测量单元坐标系之间的坐标变换矩阵为4×4的T3D矩阵，且由旋转矩阵R和平移矩阵T组成；
(x1,y1,z1)T＝T3D T(x2,y2,z2)T (4)
其中：坐标变换矩阵表示为：
其中：
T＝[t41 t42 t43]对应平移变换，[t14 t24 t34]对应投影变换，[t44]反应整体比例的变换；
步骤S41：惯性测量单元坐标系(O2X2Y2Z2)相对激光雷达坐标系为(O1X1Y1Z1)的欧拉角为：俯仰角θ、横滚角γ、方位角ψ，相对于轴向的平移量为：tx、ty、tz，则：
根据旋转矩阵R，反解欧拉角得：
优选的，所述激光雷达坐标系和惯性测量单元坐标系之间的坐标变换为刚性转换，所以[t14 t24 t34]T＝[0 0 0]T，[t44]＝1。
优选的，所述步骤S5具体步骤如下：
步骤S50：分别取植保机械左右两侧距离植保机械最近的三株玉米株作为扫描点，把扫描点两两组合，共能获得9组数据，并求其每组的中点坐标，作为植保机械在玉米行间行走时导航路径的拟合数据点；
其中：(xLm,yLm),(xRn,yRn)分别为激光雷达检测到的行走行两侧玉米植株的第m个点和第n个点坐标，m，n∈{1,2,3}，(xk,yk)点是(xLm,yLm)和(xRn,yRn)两点的中点，k{1,2，...9}。
步骤S51：利用最小二乘法对导航路径进行拟合，拟合出植保机械的自走导航路径，则拟合路径的直线方程为：
y＝ax+b (10)
取偏差最小平方和Δ＝∑(yk-y)，将式(10)代入，得：
Δ＝∑(yk-ax-b)2 (11)
分别对方程中的a、b进行求导，得：
∑yk＝9b+(∑xk)a (12)
对式(12)和式(13)求解得：
优选的，所述步骤S6中，根据导航标定线的角度和方向控制植保机械的左右电机控制器，通过电子差速来控制植保机械的移动和转向，采用双履带异向差速转向方式，且植保机械的转向半径
其中，L为履带中心距，R为转向半径。
一种基于多传感器的植保机械导航控制系统，包括激光雷达、惯性测量单元、里程计和总控制器，所述激光雷达沿中心线安装在植保机械前端，且通过网口与总控制器连接，所述惯性测量单元沿中心线安装在植保机械质心位置，且通过USB接口与总控制器相连，所述里程计与植保机械左右驱动电机的光电编码器连接，且通过USB接口与总控制器相连。
采用以上技术方案的有益效果是：
1、该基于多传感器的植保机械导航控制方法，利用激光雷达对植保机械左右两侧距离植保机械最近的三株玉米株进行扫描获取坐标信息，里程计通过电机的光电编码器获得植保机械的速度，惯性测量单元获得该植保机械的位置姿态信息，将激光雷达坐标系与惯性测量单元坐标系进行坐标转换的数据和里程计获得的植保机械电机转速数据发送给总控制器，总控制器对数据进行融合转换分析，在玉米行间规划出一条导航路径，再根据当前位置姿态状态调整左右两驱动电机的转速以调整植保机械的运动状态，根据导航标定线的角度和方向控制植保机械的左右电机控制器，通过电子差速来控制植保机械的移动和转向；另外采用双履带异向差速转向方式，且植保机械的转向半径
实现了植保机械能够在较短时间内规划下一个玉米行间的导航路径。
2、所述步骤S3中，里程计通过电机的光电编码器获得植保机械的速度，里程计通过电机的光电编码器能测量的数据，关系式如下：
矩阵形式表达为：
使得公式(3)的矩阵形式表达为关于角速度的矩阵，方便了电机的光电编码器的直接识别，且将测量数据直接传输给总控制器，时效性高，机动灵活；得出底盘中心的线线速度和底盘中心的线角速度，然后进行导航航迹推算，求解当前时刻植保机械在惯性测量单元坐标系中的位置姿态。
3、所述步骤S4进行激光雷达坐标系和惯性测量单元坐标系的联合标定，将激光雷达扫描的数据相对位置坐标变换到惯性测量单元坐标系下，将两种传感器得到的信息进行统一，方便了总控制器对数据进行融合转换分析。
4、所述步骤S5具体步骤如下：
步骤S50：分别取植保机械左右两侧距离植保机械最近的三株玉米株作为扫描点，把扫描点两两组合，共能获得9组数据，并求其每组的中点坐标，作为植保机械在玉米行间行走时导航路径的拟合数据点；
其中：(xLm,yLm),(xRn,yRn)分别为激光雷达检测到的行走行两侧玉米植株的第m个点和第n个点坐标，m，n∈{1,2,3}，(xk,yk)点是(xLm,yLm)和(xRn,yRn)两点的中点，k{1,2，...9}。
步骤S51：利用最小二乘法对导航路径进行拟合，拟合出植保机械的自走导航路径，则拟合路径的直线方程为：
y＝ax+b (10)
取偏差最小平方和Δ＝∑(yk-y)，将式(10)代入，得：
Δ＝∑(yk-ax-b)2 (11)
分别对方程中的a、b进行求导，得：
∑yk＝9b+(∑xk)a (12)
对式(12)和式(13)求解得：
实时获取玉米行间左右两侧距离植保机械最近的三株玉米株的坐标信息，把扫描点坐标信息两两组合，共能获得9组数据，并求其每组的中点坐标，作为植保机械在玉米行间行走时导航路径的拟合数据点，实现动态实时探测，实时控制植保机械移动，实时性好，准确率高。
附图说明
图1是本发明的基于多传感器的植保机械结构示意图；
其中：
1、激光雷达；2、惯性测量单元；3、里程计；4、总控制器；5、电机；6、履带式底盘。
具体实施方式
下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，目的是帮助本领域的技术人员对本发明的构思、技术方案有更完整、准确和深入的理解，并有助于其实施。
如图1所示，本发明是一种基于多传感器的植保机械导航控制方法及系统，总控制器对数据进行融合转换分析，在玉米行间规划出一条导航路径，再根据当前位置姿态状态调整左右两驱动电机的转速以调整植保机械的运动状态，根据导航标定线的角度和方向控制植保机械的左右电机控制器，通过电子差速来控制植保机械的移动和转向。
具体的说，如图1所示，一种基于多传感器的植保机械导航控制方法，具体包括以下步骤，步骤S1：调整植保机械的角度和位置，使其前端正对着玉米行间放置在田间地头；
步骤S2：启动激光雷达1，设置激光雷达1的扫描角度和分辨率；
步骤S3：启动里程计3和惯性测量单元2，里程计3通过电机5的光电编码器获得植保机械的速度，惯性测量单元2获得该植保机械的位置姿态信息；
步骤S4：进行激光雷达坐标系和惯性测量单元坐标系的联合标定；
步骤S5：分别取植保机械左右两侧距离植保机械最近的三株玉米株作为扫描点，把扫描点两两组合，共能获得9组数据，并求其每组的中点坐标，作为该植保机械在玉米行间行走时导航路径的拟合数据点；
步骤S6：将激光雷达坐标系与惯性测量单元坐标系进行坐标转换的数据和里程计获得的植保机械电机转速数据发送给总控制器4，总控制器4对数据进行融合转换分析，根据导航标定线的角度和方向控制植保机械的左右电机控制器，通过电子差速来控制植保机械的移动和转向。
所述步骤S3中，里程计3通过电机5的光电编码器获得植保机械的速度，里程计3通过电机5的光电编码器能测量的数据，关系式如下：
矩阵形式表达为：
其中：v是底盘中心线线速度；ω是底盘中心线角速度；vR是右驱动轮线速度；vL是左驱动轮线速度；ωR是右驱动轮角速度；ωL是左驱动轮角速度；rR是右驱动轮转弯半径；rL是左驱动轮转弯半径；d是植保机械底盘中心圆弧运动的半径；J是转换变量。
所述步骤S4进行激光雷达坐标系和惯性测量单元坐标系的联合标定，具体步骤如下；
步骤S40：定义激光雷达坐标系为(O1X1Y1Z1),惯性测量单元坐标系为(O2X2Y2Z2)，则同一个扫描点在两个不同坐标系下的坐标分别表示为(x1,y1,z1)、(x2,y2,z2)；步骤S41：激光雷达坐标系和惯性测量单元坐标系之间的坐标变换矩阵为4×4的T3D矩阵，且由旋转矩阵R和平移矩阵T组成；
(x1,y1,z1)T＝T3D T(x2,y2,z2)T (4)
其中：坐标变换矩阵表示为：
其中：
T＝[t41 t42 t43]对应平移变换，[t14 t24 t34]对应投影变换，[t44]反应整体比例的变换；
步骤S41：惯性测量单元坐标系(O2X2Y2Z2)相对激光雷达坐标系为(O1X1Y1Z1)的欧拉角为：俯仰角θ、横滚角γ、方位角ψ，相对于轴向的平移量为：tx、ty、tz，则：
根据旋转矩阵R，反解欧拉角得：
所述激光雷达坐标系和惯性测量单元坐标系之间的坐标变换为刚性转换，所以[t14 t24 t34]T＝[0 0 0]T，[t44]＝1。
所述步骤S5具体步骤如下：
步骤S50：分别取植保机械左右两侧距离植保机械最近的三株玉米株作为扫描点，把扫描点两两组合，共能获得9组数据，并求其每组的中点坐标，作为植保机械在玉米行间行走时导航路径的拟合数据点；
其中：(xLm,yLm),(xRn,yRn)分别为激光雷达检测到的行走行两侧玉米植株的第m个点和第n个点坐标，m，n∈{1,2,3}，(xk,yk)点是(xLm,yLm)和(xRn,yRn)两点的中点，k{1,2，...9}。
步骤S51：利用最小二乘法对导航路径进行拟合，拟合出植保机械的自走导航路径，则拟合路径的直线方程为：
y＝ax+b (10)
取偏差最小平方和Δ＝∑(yk-y)，将式(10)代入，得：
Δ＝∑(yk-ax-b)2 (11)
分别对方程中的a、b进行求导，得：
∑yk＝9b+(∑xk)a (12)
对式(12)和式(13)求解得：
所述步骤S6中，根据导航标定线的角度和方向控制植保机械的左右电机5控制器，通过电子差速来控制植保机械的底盘6移动和转向，采用双履带异向差速转向方式，且植保机械的转向半径
其中，L为履带中心距，R为转向半径。
一种基于多传感器的植保机械导航控制系统，包括激光雷达1、惯性测量单元2、里程计3和总控制器4，所述激光雷达1沿中心线安装在植保机械前端，且通过网口与总控制器4连接，所述惯性测量单元2沿中心线安装在植保机械质心位置，且通过USB接口与总控制器4相连，所述里程计3与植保机械左右驱动电机5的光电编码器连接，且通过USB接口与总控制器4相连。
以下用具体实施例对具体工作方式进行阐述：
实施例1：
1、该基于多传感器的植保机械导航控制方法，利用激光雷达1对植保机械左右两侧距离植保机械最近的三株玉米株进行扫描获取坐标信息，里程计3通过电机5的光电编码器获得植保机械的速度，惯性测量单元2获得该植保机械的位置姿态信息，将激光雷达坐标系与惯性测量单元坐标系进行坐标转换的数据和里程计3获得的植保机械电机5转速数据发送给总控制器4，总控制器4对数据进行融合转换分析，在玉米行间规划出一条导航路径，再根据当前位置姿态状态调整左右两驱动电机5的转速以调整植保机械的运动状态，根据导航标定线的角度和方向控制植保机械的左右电机5控制器，通过电子差速来控制植保机械的移动和转向；另外采用双履带异向差速转向方式，且植保机械的转向半径
实现了植保机械能够在较短时间内规划下一个玉米行间的导航路径。
实施例2：
在实施例1的基础上，所述步骤S3中，里程计3通过电机5的光电编码器获得植保机械的速度，里程计3通过电机5的光电编码器能测量的数据，关系式如下：
矩阵形式表达为：
使得公式(3)的矩阵形式表达为关于角速度的矩阵，方便了电机5的光电编码器的直接识别，且将测量数据直接传输给总控制器4，时效性高，机动灵活；得出底盘中心的线线速度和底盘中心的线角速度，然后进行导航航迹推算，求解当前时刻植保机械在惯性测量单元坐标系中的位置姿态。
实施例3：
在实施例1的基础上，所述步骤S4进行激光雷达坐标系和惯性测量单元坐标系的联合标定，将激光雷达1扫描的数据相对位置坐标变换到惯性测量单元坐标系下，将两种传感器得到的信息进行统一，方便了总控制器4对数据进行融合转换分析。
实施例4：
在实施例1的基础上，所述步骤S5具体步骤如下：
步骤S50：分别取植保机械左右两侧距离植保机械最近的三株玉米株作为扫描点，把扫描点两两组合，共能获得9组数据，并求其每组的中点坐标，作为植保机械在玉米行间行走时导航路径的拟合数据点；
其中：(xLm,yLm),(xRn,yRn)分别为激光雷达检测到的行走行两侧玉米植株的第m个点和第n个点坐标，m，n∈{1,2,3}，(xk,yk)点是(xLm,yLm)和(xRn,yRn)两点的中点，k{1,2，...9}。
步骤S51：利用最小二乘法对导航路径进行拟合，拟合出植保机械的自走导航路径，则拟合路径的直线方程为：
y＝ax+b (10)
取偏差最小平方和Δ＝∑(yk-y)，将式(10)代入，得：
Δ＝∑(yk-ax-b)2 (11)
分别对方程中的a、b进行求导，得：
∑yk＝9b+(∑xk)a (12)
对式(12)和式(13)求解得：
实时获取玉米行间左右两侧距离植保机械最近的三株玉米株的坐标信息，把扫描点坐标信息两两组合，共能获得9组数据，并求其每组的中点坐标，作为植保机械在玉米行间行走时导航路径的拟合数据点，实现动态实时探测，实时控制植保机械移动，实时性好，准确率高。
以上结合附图对本发明进行了示例性描述，显然，本发明具体实现并不受上述方式的限制，只要是采用了本发明的方法构思和技术方案进行的各种非实质性的改进；或未经改进，将本发明的上述构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。
Claims (7)
1.一种基于多传感器的植保机械导航控制方法，其特征在于：具体包括以下步骤，
步骤S1：调整植保机械的角度和位置，使其前端正对着玉米行间放置在田间地头；
步骤S2：启动激光雷达(1)，设置激光雷达(1)的扫描角度和分辨率；
步骤S3：启动里程计(3)和惯性测量单元(2)，里程计(3)通过电机(5)的光电编码器获得植保机械的速度，惯性测量单元(2)获得该植保机械的位置姿态信息；
步骤S4：进行激光雷达坐标系和惯性测量单元坐标系的联合标定；
步骤S5：分别取植保机械左右两侧距离植保机械最近的三株玉米株作为扫描点，把扫描点两两组合，共能获得9组数据，并求其每组的中点坐标，作为该植保机械在玉米行间行走时导航路径的拟合数据点；
步骤S6：将激光雷达坐标系与惯性测量单元坐标系进行坐标转换的数据和里程计获得的植保机械电机转速数据发送给总控制器(4)，总控制器(4)对数据进行融合转换分析，根据导航标定线的角度和方向控制植保机械的左右电机控制器，通过电子差速来控制植保机械的移动和转向。
2.根据权利要求1所述的一种基于多传感器的植保机械导航控制方法，其特征在于：所述步骤S3中，里程计(3)通过电机(5)的光电编码器获得植保机械的速度，里程计(3)通过电机(5)的光电编码器能测量的数据，关系式如下：
矩阵形式表达为：
其中：v是底盘中心线线速度；ω是底盘中心线角速度；vR是右驱动轮线速度；vL是左驱动轮线速度；ωR是右驱动轮角速度；ωL是左驱动轮角速度；rR是右驱动轮转弯半径；rL是左驱动轮转弯半径；d是植保机械底盘中心圆弧运动的半径；J是转换变量。
3.根据权利要求1所述的一种基于多传感器的植保机械导航控制方法，其特征在于：所述步骤S4进行激光雷达坐标系和惯性测量单元坐标系的联合标定，具体步骤如下；
步骤S40：定义激光雷达坐标系为(O1X1Y1Z1),惯性测量单元坐标系为(O2X2Y2Z2)，则同一个扫描点在两个不同坐标系下的坐标分别表示为(x1,y1,z1)、(x2,y2,z2)；
步骤S41：激光雷达坐标系和惯性测量单元坐标系之间的坐标变换矩阵为4×4的T3D矩阵，且由旋转矩阵R和平移矩阵T组成；
(x1,y1,z1)T＝T3D T(x2,y2,z2)T (4)
其中：坐标变换矩阵表示为：
其中：
T＝[t41 t42 t43]对应平移变换，[t14 t24 t34]对应投影变换，[t44]反应整体比例的变换；
步骤S41：惯性测量单元坐标系(O2X2Y2Z2)相对激光雷达坐标系为(O1X1Y1Z1)的欧拉角为：俯仰角θ、横滚角γ、方位角ψ，相对于轴向的平移量为：tx、ty、tz，则：
根据旋转矩阵R，反解欧拉角得：
4.根据权利要求3所述的一种基于多传感器的植保机械导航控制方法，其特征在于：所述激光雷达坐标系和惯性测量单元坐标系之间的坐标变换为刚性转换，所以[t14 t24 t34]T＝[0 0 0]T，[t44]＝1。
5.根据权利要求1所述的一种基于多传感器的植保机械导航控制方法，其特征在于：所述步骤S5具体步骤如下：
步骤S50：分别取植保机械左右两侧距离植保机械最近的三株玉米株作为扫描点，把扫描点两两组合，共能获得9组数据，并求其每组的中点坐标，作为植保机械在玉米行间行走时导航路径的拟合数据点；
其中：(xLm,yLm),(xRn,yRn)分别为激光雷达检测到的行走行两侧玉米植株的第m个点和第n个点坐标，m，n∈{1,2,3}，(xk,yk)点是(xLm,yLm)和(xRn,yRn)两点的中点，k{1,2，...9}。
步骤S51：利用最小二乘法对导航路径进行拟合，拟合出植保机械的自走导航路径，则拟合路径的直线方程为：
y＝ax+b (10)
取偏差最小平方和Δ＝∑(yk-y)，将式(10)代入，得：
Δ＝∑(yk-ax-b)2 (11)
分别对方程中的a、b进行求导，得：
∑yk＝9b+(∑xk)a (12)
对式(12)和式(13)求解得：
6.根据权利要求1所述的一种基于多传感器的植保机械导航控制方法，其特征在于：所述步骤S6中，根据导航标定线的角度和方向控制植保机械的左右电机(5)控制器，通过电子差速来控制植保机械的底盘(6)移动和转向，采用双履带异向差速转向方式，且植保机械的转向半径
其中，L为履带中心距，R为转向半径。
7.一种基于多传感器的植保机械导航控制系统，其特征在于：包括激光雷达(1)、惯性测量单元(2)、里程计(3)和总控制器(4)，所述激光雷达(1)沿中心线安装在植保机械前端，且通过网口与总控制器(4)连接，所述惯性测量单元(2)沿中心线安装在植保机械质心位置，且通过USB接口与总控制器(4)相连，所述里程计(3)与植保机械左右驱动电机(5)的光电编码器连接，且通过USB接口与总控制器(4)相连。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011607088.5A CN112868623B (zh) | 2020-12-30 | 2020-12-30 | 一种基于多传感器的植保机械导航控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011607088.5A CN112868623B (zh) | 2020-12-30 | 2020-12-30 | 一种基于多传感器的植保机械导航控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112868623A true CN112868623A (zh) | 2021-06-01 |
| CN112868623B CN112868623B (zh) | 2021-11-26 |
Family
ID=76046381
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011607088.5A Active CN112868623B (zh) | 2020-12-30 | 2020-12-30 | 一种基于多传感器的植保机械导航控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112868623B (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114545944A (zh) * | 2022-02-24 | 2022-05-27 | 合肥工业大学 | 一种基于磁钉磁场强度纠正的agv航向定位导航方法 |
| CN116700319A (zh) * | 2023-08-04 | 2023-09-05 | 西安交通大学 | 基于微型雷达阵列的空中机器人自主起降系统及方法 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN205390106U (zh) * | 2016-02-16 | 2016-07-27 | 江苏省农业科学院 | 一种果树靶标探测系统 |
| CN109122644A (zh) * | 2018-10-30 | 2019-01-04 | 四川农业大学 | 一种温室用自动作业植保机器人 |
| CN208351332U (zh) * | 2018-05-10 | 2019-01-08 | 静快省（苏州）智能科技有限公司 | 基于卫星定位技术的无人驾驶植保喷施系统 |
-
2020
- 2020-12-30 CN CN202011607088.5A patent/CN112868623B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN205390106U (zh) * | 2016-02-16 | 2016-07-27 | 江苏省农业科学院 | 一种果树靶标探测系统 |
| CN208351332U (zh) * | 2018-05-10 | 2019-01-08 | 静快省（苏州）智能科技有限公司 | 基于卫星定位技术的无人驾驶植保喷施系统 |
| CN109122644A (zh) * | 2018-10-30 | 2019-01-04 | 四川农业大学 | 一种温室用自动作业植保机器人 |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114545944A (zh) * | 2022-02-24 | 2022-05-27 | 合肥工业大学 | 一种基于磁钉磁场强度纠正的agv航向定位导航方法 |
| CN114545944B (zh) * | 2022-02-24 | 2024-04-16 | 合肥工业大学 | 一种基于磁钉磁场强度纠正的agv航向定位导航方法 |
| CN116700319A (zh) * | 2023-08-04 | 2023-09-05 | 西安交通大学 | 基于微型雷达阵列的空中机器人自主起降系统及方法 |
| CN116700319B (zh) * | 2023-08-04 | 2023-10-20 | 西安交通大学 | 基于微型雷达阵列的空中机器人自主起降系统及方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112868623B (zh) | 2021-11-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112868623B (zh) | 一种基于多传感器的植保机械导航控制方法 | |
| CN107486834B (zh) | 温室作物生长巡检机器人 | |
| Torii | Research in autonomous agriculture vehicles in Japan | |
| Li et al. | Control of a path following caterpillar robot based on a sliding mode variable structure algorithm | |
| CN103487812B (zh) | 一种温室自主移动车辆的超声波导航装置及方法 | |
| Noguchi | Agricultural vehicle robot | |
| CN111837593A (zh) | 一种新型基于机器视觉和卷积神经网络算法的前胡除草机 | |
| CN113359710B (zh) | 一种基于los理论的农机路径跟踪方法 | |
| CN109526441A (zh) | 一种打顶机 | |
| Qiu et al. | Extended Ackerman Steering Principle for the coordinated movement control of a four wheel drive agricultural mobile robot | |
| CN111610523B (zh) | 一种轮式移动机器人的参数校正方法 | |
| CN103901889A (zh) | 一种基于蓝牙通信的多机器人编队控制路径跟踪方法 | |
| Raikwar et al. | Navigation and control development for a four-wheel-steered mobile orchard robot using model-based design | |
| CN109491390B (zh) | 基于多传感器的无轨化全向移动机器人系统及其控制方法 | |
| KR20110057544A (ko) | 어린모 검출 장치 | |
| Linz et al. | Autonomous service robots for orchards and vineyards: 3D simulation environment of multi sensor-based navigation and applications | |
| Corpe et al. | GPS-guided modular design mobile robot platform for agricultural applications | |
| CN113311826A (zh) | 一种基于环形红外阵列的自动跟随系统及其工作方法 | |
| Mosalanejad et al. | Evaluation of navigation system of a robot designed for greenhouse spraying | |
| CN106546253B (zh) | 一种农田作业机械路径追踪装置及其追踪方法 | |
| CN109542099B (zh) | 一种农机控制方法 | |
| Xue et al. | An agricultural robot for multipurpose operations in a greenhouse | |
| Luo et al. | Parallel point-to-point tracking for agricultural Wide-Span Implement Carrier (WSIC) | |
| Shamshiri et al. | Evaluation of laser and infrared sensors with CANBUS communication for collision avoidance of a mobile robot | |
| Rapoport et al. | Navigation and control problems in precision farming |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |