CN113865582B - 一种果树行间农用车辆航向角与横向偏差的测定方法 - Google Patents

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进行投影，投影得到的点为A_i和B_j；其中，A_i表示农用车辆左侧第i排红色条形光斑标记中心点在xOy面投影得到的点，B_j表示农用车辆右侧第j排红色条形光斑标记中心点在xOy面投影得到的点；

4)统计红色条形光斑标记数量；

如果满足以下条件，转步骤5)；否则，转步骤8)：

其中，Num(*)表示满足*条件的点的个数；

5)将x>0的点与x<0的点之间两两取中点，记为C_ij，C_ij表示左侧的第i排与右侧的第j排组合取到的中点。

6)采用最小二乘法对得到的中点C_ij进行拟合，得到拟合直线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-双目相机；A₁～A₃、B₁～B₃－红色条形光斑标记中心点的投影点；C₁₁～C₃₃－投影点中点；α－航向角；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棵树干上的红色条形光斑标记逐一框住；

将目标检测框的中心点坐标视为红色条形光斑标记的中心点坐标，由双目相机三角测距法得到每个红色条形光斑标记中心点在相机坐标中的位置坐标(X_h,Y_h,Z_h)，其中 h＝{1,2,3,4,5,6}；通过测量得到坐标系O-xyz与相机坐标系O^’-x^’y^’z^’之间的平移，得到平移距离表示为(X₀,Y₀,Z₀)；

本发明中，坐标系O-xyz与相机坐标系O^’-x^’y^’z^’之间只有平移，没有旋转关系，因此通过三维坐标系平移矩阵：

其中，dx表示x轴方向平移量；dy表示y轴方向平移量；dz表示z轴方向平移量；

通过坐标平移公式得到：

所以，本实例中的红色条形光斑标记的中心点在O-xyz坐标系中的坐标为 (X_h+X₀,Y_h+Y₀,Z_h+Z₀)，其中h＝{1,2,3,4,5,6}；

3)将红色条形光斑标记中心点在O-xyz中的空间坐标向xOy进行投影，投影到xOy平面后，红色条形光斑标记的中心点可以表示为(X_h+X₀,Y_h+Y₀)，车辆右侧的光斑标记投影点在xOy坐标系中在y轴的右侧，即x正半轴区域，同理，左侧在x负半轴区域；将 X_h+X₀<0的投影点记为A_i，将X_h+X₀>0的投影点记为B_j，其中i＝{1,2,3}，j＝{1,2,3}；

4)统计红色条形光斑标记数量；如果满足以下条件，则转步骤5)；否则转步骤

8)：

其中，x＝X_h+X₀，y＝Y_h+Y₀，Num(*)表示满足*条件的点的个数；

5)将A_i与B_j之间两两取中点，记为C_ij，C_ij表示左侧的第i排与右侧的第j排组合取到的中点；本实施例中由A₁～A_3,，B₁～B₃两两取中点，得到C₁₁～C₃₃这9个中点；

6)采用最小二乘法对得到的中点C₁₁～C₃₃进行拟合，得到拟合直线y＝kx+b；其中k为拟合直线的斜率，b为拟合直线的截距；

7)计算拟合直线y＝kx+b与y轴之间的夹角，即为航向角α，且α＝(arctank)-90°；

计算基准原点到拟合直线y＝kx+b的垂直距离，即为横向偏差L，且

8)车辆停止行进，系统切换至人工遥控操作模式。

本发明结构简单，适应性强，可以避免因障碍物遮挡卫星定位信号等不良因素对测定的影响，而且还具有以下特点：

(1)操作简单，使用方便，稳定性强，不受白天和夜晚的影响。

(2)测量速度快，测得的结果也更加精确。

本发明未涉及部分均与现有技术相同或可采用现有技术加以实现。

Claims (1)

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进行投影，投影得到的点为A_i和B_j；其中，A_i表示农用车辆左侧第i排红色条形光斑标记中心点在xOy面投影得到的点，B_j表示农用车辆右侧第j排红色条形光斑标记中心点在xOy面投影得到的点；

4)统计捕获的红色条形光斑标记数量，如果满足以下条件，转步骤5)；否则，转步骤8)：

其中，Num(*)表示满足*条件的点的个数；

5)将x>0的点与x<0的点之间两两取中点，记为C_ij；该C_ij表示左侧的第i排与右侧的第j排组合取到的中点；

6)采用最小二乘法对得到的中点C_ij进行拟合，得到拟合直线y＝kx+b；其中k为拟合直线的斜率，b为拟合直线的截距；

7)计算拟合直线y＝kx+b与y轴之间的夹角，即为航向角；

计算基准原点到拟合直线y＝kx+b的垂直距离，即为横向偏差；

8)车辆停止前进，系统切换至人工遥控操作模式。