CN113865582A - 一种果树行间农用车辆航向角与横向偏差的测定方法 - Google Patents
一种果树行间农用车辆航向角与横向偏差的测定方法 Download PDFInfo
- Publication number
- CN113865582A CN113865582A CN202111133146.XA CN202111133146A CN113865582A CN 113865582 A CN113865582 A CN 113865582A CN 202111133146 A CN202111133146 A CN 202111133146A CN 113865582 A CN113865582 A CN 113865582A
- Authority
- CN
- China
- Prior art keywords
- light spot
- shaped light
- point
- vehicle
- agricultural vehicle
- 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
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种果树行间农用车辆航向角与横向偏差的测定方法,所述农用车辆的车头上设有激光发射器和双目相机;该激光发射器能够发射红色水平激光束;所述激光发射器和双目相机均与工控机电性相连,通过激光在车辆行驶前方的树干上形成红色条形光斑标记,再通过相机采集图像后,传输至工控机,通过YOLOv3目标检测、双目相机三角测距法、最小二乘法等方法,计算得到航向角和横向偏差,具有测量速度快,精确度高,稳定性强等特点,且可避免因障碍物遮挡卫星定位信号等不良因素对导航参数测定的影响。
Description
技术领域
本发明涉及一种车辆导航参数检测方法,尤其是一种农业车辆的导航参数检测方法,具体的说是一种果树行间农用车辆航向角与横向偏差的检测方法。
背景技术
传统自动行驶农用车辆在测定航向角以及横向偏差时,大都采用基于双天线GNSS或GNSS与位姿传感器相结合的方法。但GNSS在受遮挡的果园中数据接收容易受到干扰,从而导致测得的航向角与横向偏差存在误差或得到错误的结果,最终容易使农用车辆与树干之间发生碰撞事故。
因此,需要对以上传统检测方法加以改进,以便农业车辆在果园行走时,可获得更优的导航精度和稳定性。
发明内容
本发明的目的是针对现有技术的不足,提供一种果树行间农用车辆航向角与横向偏差的测定方法,可以避免果园因高空遮挡而丢失卫星定位数据等因素对测定的影响,获得更优的导航精度和稳定性,而且,本方法还可在夜晚正常工作,极大的提高工作效率。
本发明的技术方案是:
一种果树行间农用车辆航向角与横向偏差的测定方法,所述农用车辆的车头上设有激光发射器和双目相机;该激光发射器能够发射红色水平激光束;所述激光发射器和双目相机均与工控机电性相连;
所述测定方法包括以下步骤:
1)农用车辆在果树行间低速行驶,并打开激光发射器,在车辆行驶方向前方左右两侧果树行的树干上形成红色条形光斑标记;同时,以车辆的几何中心点对地面的投影设为基准原点O,激光发射器几何中心点对地面的投影为F点,以作为y轴正方向,将 y轴绕基准原点O顺时针旋转90°得到x轴;根据右手定则,z轴竖直向上,建立车辆的基坐标系O-xyz;再测量得到O点到双目相机左相机光心O’在x、y、z方向上的平移量,通过对坐标系O-xyz平移,以O’为原点建立相机坐标系O’-x’y’z’;
2)双目相机采集图像,图像传入工控机中训练好的YOLOv3模型中进行红色条形光斑标记检测。当检测到图像中存在红色条形光斑标记时,目标检测框会将所有红色条形光斑标记逐个框住;将目标检测框的中心点坐标视为红色条形光斑标记的中心点坐标,由双目相机三角测距法得到每个红色条形光斑标记中心点在相机坐标系O’-x’y’z’中的位置坐标,再通过坐标平移得到红色条形光斑标记在O-xyz坐标系中的坐标。
3)将红色条形光斑标记中心点在O-xyz中的空间坐标向xOy进行投影,投影得到的点为Ai和Bj;其中,Ai表示农用车辆左侧第i排红色条形光斑标记中心点在xOy面投影得到的点,Bj表示农用车辆右侧第j排红色条形光斑标记中心点在xOy面投影得到的点;
4)统计红色条形光斑标记数量;
如果满足以下条件,转步骤5);否则,转步骤8):
其中,Num(*)表示满足*条件的点的个数;
5)将x>0的点与x<0的点之间两两取中点,记为Cij,Cij表示左侧的第i排与右侧的第j排组合取到的中点。
6)采用最小二乘法对得到的中点Cij进行拟合,得到拟合直线y=kx+b;其中k为拟合直线的斜率,b为拟合直线的截距;
7)计算拟合直线y=kx+b与y轴之间的夹角,即为航向角;
计算基准原点到拟合直线y=kx+b的垂直距离,即为横向偏差。
8)车辆停止行进,系统切换至人工遥控操作模式;
进一步的,所述激光发射器安装在农用车辆车头的中点;所述双目相机固定在车头正面并居中安装;所述激光发射器与双目相机互不干涉。
本发明的有益效果:
本发明采用激光红色水平发射器在车辆行驶前方的树干上打出红色条形光斑标记,通过双目视觉对红色条形光斑标记进行识别和定位,进而确定果树在相机坐标系下的位置坐标,通过坐标变换获得果树树干在车辆的基坐标系O-xyz中对地面投影的位置坐标,然后将不同侧的投影点两两取中点并进行直线拟合得到导航路径,通过计算得到航向角和横向偏差。本发明具有测量速度快,精确度高,稳定性强的特点,不仅可以使农用车辆避免因高空障碍物遮挡而丢失卫星定位数据等因素对导航参数测定造成的影响,还可在夜晚正常工作,极大的提高工作效率。
附图说明
图1是本发明的投影到xOy平面的示意图。
图2是斜率k与航向角α、横向偏差值L之间的关系图。
图3是车辆、激光发射器几何中心点投影关系;O-xyz与O’-x’y’z’位置以及坐标平移量关系图。
其中,1-农用车辆;2-激光发射器;3-双目相机;A1~A3、B1~B3-红色条形光斑标记中心点的投影点;C11~C33-投影点中点;α-航向角;L-横向偏差;xOy-坐标系。
具体实施方式
下面结合附图和实施例对本发明作进一步的说明。
如图1-3所示。
一种果树行间农用车辆航向角与横向偏差的测定方法,应用于自动行驶农用车辆。该农用车辆1的车头上设有激光发射器2和双目相机3。其中,所述激光发射器2能够发射红色水平激光束。该激光发射器2安装在所述农用车辆1车头的中点。同时,所述双目相机3安装在车头的正面且居中,并确保其与所述激光发射器互不干涉。所述激光发射器2和双目相机3均与工控机电性相连,所述双目相机对激光发射器与树干产生的光斑图像进行采集,并通过工控机对双目相机采集的图像进行处理。
本实施例中,以左右两侧各3棵树干为例。
所述测定方法包括以下步骤:
1)农用车辆在果树行间低速行驶,并打开激光发射器,在车辆行驶方向前方左右两侧果树行的6棵树干上形成红色条形光斑标记;同时,以车辆的几何中心点对地面的投影设为基准原点O,激光发射器几何中心点对地面的投影为F点,以作为y轴正方向,将y轴绕基准原点O顺时针旋转90°得到x轴;根据右手定则,z轴竖直向上,建立车辆的基坐标系O-xyz;再测量得到O点到双目相机左相机光心O’在x、y、z方向上的平移量,通过对坐标系O-xyz平移,以O’为原点建立相机坐标系O’-x’y’z’;
2)双目相机采集图像,图像传入工控机中训练好的YOLOv3模型中进行红色条形光斑标记检测;检测到图像中存在红色条形光斑标记,目标检测框组会将6棵树干上的红色条形光斑标记逐一框住;
将目标检测框的中心点坐标视为红色条形光斑标记的中心点坐标,由双目相机三角测距法得到每个红色条形光斑标记中心点在相机坐标中的位置坐标(Xh,Yh,Zh),其中 h={1,2,3,4,5,6};通过测量得到坐标系O-xyz与相机坐标系O’-x’y’z’之间的平移,得到平移距离表示为(X0,Y0,Z0);
本发明中,坐标系O-xyz与相机坐标系O’-x’y’z’之间只有平移,没有旋转关系,因此通过三维坐标系平移矩阵:
其中,dx表示x轴方向平移量;dy表示y轴方向平移量;dz表示z轴方向平移量;
通过坐标平移公式得到:
所以,本实例中的红色条形光斑标记的中心点在O-xyz坐标系中的坐标为 (Xh+X0,Yh+Y0,Zh+Z0),其中h={1,2,3,4,5,6};
3)将红色条形光斑标记中心点在O-xyz中的空间坐标向xOy进行投影,投影到xOy平面后,红色条形光斑标记的中心点可以表示为(Xh+X0,Yh+Y0),车辆右侧的光斑标记投影点在xOy坐标系中在y轴的右侧,即x正半轴区域,同理,左侧在x负半轴区域;将 Xh+X0<0的投影点记为Ai,将Xh+X0>0的投影点记为Bj,其中i={1,2,3},j={1,2,3};
4)统计红色条形光斑标记数量;如果满足以下条件,则转步骤5);否则转步骤
8):
其中,x=Xh+X0,y=Yh+Y0,Num(*)表示满足*条件的点的个数;
5)将Ai与Bj之间两两取中点,记为Cij,Cij表示左侧的第i排与右侧的第j排组合取到的中点;本实施例中由A1~A3,,B1~B3两两取中点,得到C11~C33这9个中点;
6)采用最小二乘法对得到的中点C11~C33进行拟合,得到拟合直线y=kx+b;其中k为拟合直线的斜率,b为拟合直线的截距;
7)计算拟合直线y=kx+b与y轴之间的夹角,即为航向角α,且α=(arctank)-90°;
8)车辆停止行进,系统切换至人工遥控操作模式。
本发明结构简单,适应性强,可以避免因障碍物遮挡卫星定位信号等不良因素对测定的影响,而且还具有以下特点:
(1)操作简单,使用方便,稳定性强,不受白天和夜晚的影响。
(2)测量速度快,测得的结果也更加精确。
本发明未涉及部分均与现有技术相同或可采用现有技术加以实现。
Claims (2)
1.一种果树行间农用车辆航向角与横向偏差的测定方法,其特征是,所述农用车辆的车头上设有激光发射器和双目相机;该激光发射器能够发射红色水平激光束;所述激光发射器和双目相机均与工控机电性相连;
所述测定方法包括以下步骤:
1)农用车辆在果树行间低速行驶,并打开激光发射器,在车辆行驶方向前方左右两侧果树行的树干上形成红色条形光斑标记;同时,以车辆的几何中心点对地面的投影设为基准原点O,激光发射器几何中心点对地面的投影为F点,以作为y轴正方向,将y轴绕基准原点O顺时针旋转90°得到x轴;根据右手定则,z轴竖直向上,建立车辆的基坐标系O-xyz;再测量得到O点到双目相机左相机光心O’在x、y、z方向上的平移量,通过对坐标系O-xyz平移,以O’为原点建立相机坐标系O’-x’y’z’;
2)双目相机采集图像,图像传入工控机中训练好的YOLOv3模型中进行红色条形光斑标记检测;当检测到图像中存在红色条形光斑标记时,目标检测框会将所有红色条形光斑标记逐个框住;将目标检测框的中心点坐标视为红色条形光斑标记的中心点坐标,由双目相机三角测距法得到每个红色条形光斑标记中心点在相机坐标系O’-x’y’z’中的位置坐标,再通过坐标平移得到红色条形光斑标记在O-xyz坐标系中的坐标。
3)将红色条形光斑标记中心点在O-xyz中的空间坐标向xOy进行投影,投影得到的点为Ai和Bj;其中,Ai表示农用车辆左侧第i排红色条形光斑标记中心点在xOy面投影得到的点,Bj表示农用车辆右侧第j排红色条形光斑标记中心点在xOy面投影得到的点;
4)统计捕获的红色条形光斑标记数量,如果满足以下条件,转步骤5);否则,转步骤8):
其中,Num(*)表示满足*条件的点的个数;
5)将x>0的点与x<0的点之间两两取中点,记为Cij;该Cij表示左侧的第i排与右侧的第j排组合取到的中点;
6)采用最小二乘法对得到的中点Cij进行拟合,得到拟合直线y=kx+b;其中k为拟合直线的斜率,b为拟合直线的截距;
7)计算拟合直线y=kx+b与y轴之间的夹角,即为航向角;
计算基准原点到拟合直线y=kx+b的垂直距离,即为横向偏差;
8)车辆停止前进,系统切换至人工遥控操作模式。
2.根据权利要求1所述的果树行间农用车辆航向角与横向偏差的测定方法,其特征是,所述激光发射器安装在农用车辆车头的中点;所述双目相机在车头正面居中安装;所述激光发射器与双目相机互不干涉。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111133146.XA CN113865582B (zh) | 2021-09-27 | 2021-09-27 | 一种果树行间农用车辆航向角与横向偏差的测定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111133146.XA CN113865582B (zh) | 2021-09-27 | 2021-09-27 | 一种果树行间农用车辆航向角与横向偏差的测定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113865582A true CN113865582A (zh) | 2021-12-31 |
CN113865582B CN113865582B (zh) | 2023-09-05 |
Family
ID=78990814
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111133146.XA Active CN113865582B (zh) | 2021-09-27 | 2021-09-27 | 一种果树行间农用车辆航向角与横向偏差的测定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113865582B (zh) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102368158A (zh) * | 2011-09-15 | 2012-03-07 | 西北农林科技大学 | 一种果园机械导航定位方法 |
CN102506852A (zh) * | 2011-11-01 | 2012-06-20 | 丁幼春 | 农用车辆视觉导航系统及其导航方法 |
CN108594245A (zh) * | 2018-07-04 | 2018-09-28 | 北京国泰星云科技有限公司 | 一种目标物运动监测系统及方法 |
CN112363503A (zh) * | 2020-11-06 | 2021-02-12 | 南京林业大学 | 一种基于激光雷达的果园车辆自动导航控制系统 |
CN113311873A (zh) * | 2021-05-07 | 2021-08-27 | 中国科学院沈阳自动化研究所 | 一种基于视觉的无人机伺服跟踪方法 |
US20210295060A1 (en) * | 2018-08-17 | 2021-09-23 | Sk Telecom Co., Ltd. | Apparatus and method for acquiring coordinate conversion information |
-
2021
- 2021-09-27 CN CN202111133146.XA patent/CN113865582B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102368158A (zh) * | 2011-09-15 | 2012-03-07 | 西北农林科技大学 | 一种果园机械导航定位方法 |
CN102506852A (zh) * | 2011-11-01 | 2012-06-20 | 丁幼春 | 农用车辆视觉导航系统及其导航方法 |
CN108594245A (zh) * | 2018-07-04 | 2018-09-28 | 北京国泰星云科技有限公司 | 一种目标物运动监测系统及方法 |
US20210295060A1 (en) * | 2018-08-17 | 2021-09-23 | Sk Telecom Co., Ltd. | Apparatus and method for acquiring coordinate conversion information |
CN112363503A (zh) * | 2020-11-06 | 2021-02-12 | 南京林业大学 | 一种基于激光雷达的果园车辆自动导航控制系统 |
CN113311873A (zh) * | 2021-05-07 | 2021-08-27 | 中国科学院沈阳自动化研究所 | 一种基于视觉的无人机伺服跟踪方法 |
Non-Patent Citations (2)
Title |
---|
刘沛;陈军;张明颖;: "基于激光导航的果园拖拉机自动控制系统", 农业工程学报 * |
周俊;胡晨;: "密植果园作业机器人行间定位方法", 农业机械学报 * |
Also Published As
Publication number | Publication date |
---|---|
CN113865582B (zh) | 2023-09-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
AU2020101932A4 (en) | Binocular vision–based method and system for pose measurement of cantilever tunneling equipment | |
CN109752701B (zh) | 一种基于激光点云的道路边沿检测方法 | |
CN110008893B (zh) | 一种基于车载图像传感器的车辆行驶跑偏自动检测方法 | |
CN109634279B (zh) | 基于激光雷达与单目视觉的物体定位方法 | |
CN108868268B (zh) | 基于点到面距离和互相关熵配准的无人车位姿估计方法 | |
CN107704821B (zh) | 一种弯道的车辆位姿计算方法 | |
JP7319003B2 (ja) | 非剛性ステレオ・ビジョン・カメラ・システム | |
CN102944224B (zh) | 一种无人驾驶车的自动环境感知系统的工作方法 | |
CN108226938A (zh) | 一种agv小车的定位系统和方法 | |
CN109631896A (zh) | 一种基于车辆视觉和运动信息的停车场自主泊车定位方法 | |
CN112396656B (zh) | 一种视觉与激光雷达融合的室外移动机器人位姿估计方法 | |
CN110243293B (zh) | 基于结构光和机器视觉的管片错台快速检测装置与方法 | |
CN113534184B (zh) | 一种激光感知的农业机器人空间定位方法 | |
Wiest et al. | Localization based on region descriptors in grid maps | |
CN111813114A (zh) | 智能小车视觉导航方法 | |
CN114413958A (zh) | 无人物流车的单目视觉测距测速方法 | |
CN114200442B (zh) | 一种基于毫米波雷达和视觉的道路目标检测与关联方法 | |
CN114295099B (zh) | 基于单目摄像头的测距方法、车载测距设备以及存储介质 | |
CN113340272B (zh) | 一种基于无人机微群的地面目标实时定位方法 | |
CN113865582A (zh) | 一种果树行间农用车辆航向角与横向偏差的测定方法 | |
CN112484722A (zh) | 结合惯性导航系统的视觉传感器全局定位方法 | |
Kim et al. | LiDAR configuration comparison for urban mapping system | |
CN115280960A (zh) | 一种基于田间视觉slam的联合收获机转向控制方法 | |
CN115588036A (zh) | 图像采集方法及装置、机器人 | |
CN109407665B (zh) | 一种小型半自动无人驾驶配送车及配送路径规划方法 |
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 |