CN113406960B - 一种农用无人车辆地头转向的实时路径规划与控制方法 - Google Patents

Abstract

本发明公开了一种农用无人车辆地头转向的实时路径规划与控制方法，包括以下步骤：(1)车辆开始转弯并初始化设备；(2)实时获取车辆位置和航向信息，作为实时路径规划的输入信息；(3)通过输入信息生成以当前位置为起点的最短规划路线；(4)根据车辆当前所处阶段的位置与该阶段实时更新的结束点的距离判断是否发出停车指令；(5)若发出停车指令，则进入下一阶段；若未发出倒车指令，则回到步骤(2)，继续更新路线。本发明能有效解决已有转弯路线规划与控制在发生偏航后航迹长度增长、准确度降低和能耗增加的问题，同时解决操舵控制和加减速控制频繁的问题。

Description

一种农用无人车辆地头转向的实时路径规划与控制方法

技术领域

本发明涉及农业机械自动化技术领域，具体涉及一种农用无人车辆地头转向的实时路径规划与控制方法。

背景技术

田间地头转向是指农用车辆从当前作物行驶出后，成功驶入下一作物行的过程。地头转向的自动控制是农用无人车辆自主路径规划和实现自动导航的重要组成部分之一。最大限度地减少地头转向所耗费的时间以及自适应选择最优的地头转向运动航迹是提高农业自动化作业效率的重要途径。

目前，农用无人车辆地头转向的路径规划与控制的相关研究着力于如何更准确地追踪路径，以及当车辆偏离路线后如何控制车辆回到规划路线上。然而，由于土壤环境、路面起伏、田块泥脚深度、车辆运行突发状况等因素的影响，车辆运行中侧滑造成的偏移和偏航是无法避免的。此时，如果仍然追踪原有的规划路线，车辆实际行走航迹与规划路线必然不同，造成一定的最终偏差和更长的航迹长度。同时，更长的航迹会导致能源的消耗增加。

与此同时，传统的根据既定转弯路线，进行循迹控制，存在频繁的操舵控制和加减速控制。频繁的机械控制会造成车辆零部件的机械疲劳和磨损，长时间的机械疲劳和磨损，会损伤农机零部件，缩短农机使用寿命，造成经济损失。

公告号为CN110440823B的专利说明书中公开了一种路径规划方法及路径规划系统，用于通过车辆当前位姿与目标位姿的距离的判断，选择对应的重新规划的连接方式，进行泊车的路径重新规划，进行误差消除，实现车辆平行泊车的准确性。本发明实施例方法包括：获取起点位姿和目标位姿；确定起点位姿和目标位姿的距离；若距离大于第一预置阈值，则将目标位姿朝着起点位姿的方向以第一转弯半径进行移动，得到第一当前位姿，第一转弯半径大于等于车辆的最小转弯半径；使用目标连接方法连接第一当前位姿和起点位姿，若成功则得到第一规划路径，第一规划路径包括起点位姿与第一当前位姿之间的路径，以及第一当前位姿与目标位姿之间的路径。所述根据所述起点位姿和所述目标位姿，使用所述目标连接方法，若失败之后，所述方法还包括：将所述起点位姿朝着所述目标位姿的方向以直线移动预置长度，得到第二当前位姿；使用所述目标连接方法连接所述第二当前位姿和所述目标位姿，若成功，则确定第三规划路径，所述第三规划路径包括所述起点位姿与所述第二当前位姿之间的路径，以及所述第二当前位姿与所述目标位姿之间的路径。

上述方案的路径规划方法针对于田间地头作物行间距狭窄，小于农用车辆最小转弯半径，不能一次性完成转弯的复杂农田环境，不能够很好的适用。

发明内容

本发明的目的在于提供一种农用无人车辆地头转向的实时路径规划与控制方法，能有效解决已有路线规划与控制在发生偏航后航迹长度增长、准确度降低和能耗增加的问题，同时解决操舵控制和加减速控制频繁的问题。

一种农用无人车辆地头转向的实时路径规划与控制方法，包括以下步骤：

(1)车辆开始转弯并初始化设备；

(2)实时获取车辆位置和航向信息，作为实时路径规划的输入信息；

(3)通过输入信息生成以当前位置为起点的最短规划路线；

(4)根据车辆当前所处阶段的位置与该阶段实时更新的结束点的距离判断是否发出停车指令；

(5)若发出停车指令，则进入下一阶段；若未发出停车指令，则回到步骤(2)，继续更新路线。

本方案，通过在行驶过程中不断更新最优转弯路线，即转弯路线时刻发生更新变化，无须根据既定转弯路线，进行循迹控制，避免频繁的操舵控制和加减速控制，同时避免航迹长度增长。

作为优选，所述步骤(3)具体为：

通过输入的信息求解最优路线的三个阶段圆弧的弧度值，来生成最短路线，并在行驶中迭代更新这个过程。

作为优选，所述步骤(4)具体为：

设置一个容差，当车辆当前所处阶段的位置与该阶段结束点的距离小于所述容差时，发出停车指令。

进一步优选，所述容差为0.1米。

作为优选，所述步骤(5)具体为：

发出停车指令时车辆若在第一阶段，则进入第二阶段，回到步骤(2)，继续更新路线，若未在第一阶段，则进入第三阶段。

进一步优选，还包括步骤：

(6)车辆在第三阶段进行纯追踪导航，若到达目标点，则转弯结束。

作为优选，转弯开始后，在前两个阶段车辆以最小转弯半径转弯。

作为优选，车辆位置和航向信息通过高精度GPS获取。

本发明的有益效果：

(1)降低车辆发生偏移后增加的航迹长度。

(2)提高车辆进入下一作物行时的准确度。

(3)对车辆的机械损耗更小，并且减少能耗。

附图说明

图1为车辆地头转向的航迹图；

图2为最短路径的航迹规划图；

图3为本发明的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，车辆地头转向的航迹为：车辆从A点出发，以前进最小转弯半径向前转到B点(结束点B)，然后开始倒车直到C点(结束点C)，倒车时车辆仍然以倒车最小转弯半径转弯。最后，车辆从C点开始以纯追踪控制方式追踪路线，到达目标点D。将从A点到B点的过程称作第一阶段，B点到C点的过程称作第二阶段，C点到D点的过程称作第三阶段。

转弯开始后，在前两个阶段车辆分别以前进和倒车的最小转弯半径转弯。本发明根据实时车辆位置和航向，实时更新转弯过程中路径上的B点和C点位置，进而控制和判断何时发出停车指令，达到实时更新路线的目的。是否发出停车指令用距离r来判断。当车辆处于第一阶段(弧AB)时，距离r为车辆与点B的直线距离；当车辆处于第二阶段(弧BC)时，距离r为车辆与点C之间的直线距离。出于车辆安全控制的考虑，设置一个容差。当距离r小于该容差时，即发出停车指令。在第三阶段，车辆不进行路线更新，而是通过纯追踪算法追踪既定路线。在第三阶段转弯结束后，地头转向任务结束。

上述部分是本发明更新路线的概述，下面对本发明进行详细阐释。

如图2和3所示，一种农用无人车辆地头转向的实时路径规划与控制方法，包括以下步骤：

(1)车辆开始转弯并初始化设备；

(2)通过高精度GPS实时获取车辆位置和航向信息，作为实时路径规划的输入信息；

(3)通过输入的信息生成以当前位置为起点的最短路线，本实施例通过求解最优路线的三段圆弧值，生成最短导航路线；

车辆从作物行1驶出，以车辆驶入作物行2的瞬间车辆所在点D为坐标原点(0,0)，建立二维直角坐标系。(a,b)为车辆当前位置A坐标，车辆航向与x轴正方向夹角为

点B与点C如图所示。θ₁为第一段圆弧的弧度，θ₂为第二段圆弧的弧度，θ₃为第三段圆弧的弧度，R_f为车辆最小前进转弯半径，R_b为车辆最小倒车转弯半径，R为第三段圆弧半径。此时，在转弯过程中，B点和C点的坐标实时更新算法如下：

B点坐标为：

C点坐标为：

其中，θ₁、θ₂、θ₃间的几何关系式为：

①

②

第三段圆弧半径R：

此时，车辆驶过的路径长度，即三段圆弧长度之和s为：

车辆处于第一阶段时，根据公式(1)以及θ₁、θ₂、θ₃之间的三元二项式关系可知最小的s(最短路径)有唯一确定的θ₁、θ₂、θ₃。车辆处于第二阶段时，只需要将θ₁＝0带入三元二项式即可求出θ₂、θ₃。根据所得的θ₁、θ₂、θ₃，即可确定一组唯一的转弯方案，该方案即当前状态下对应的最短路径组合。

(4)根据车辆当前所处阶段的位置与该阶段实时更新的结束点的距离判断是否发出停车指令；

本实施例中，设置容差为0.1米，当车辆处于第一阶段(弧AB)时，车辆与点B的直线距离r小于0.1米时，发出停车指令；当车辆处于第二阶段(弧BC)时，车辆与点C的直线距离r小于0.1米时，发出停车指令。

(5)若发出停车指令，则进入下一阶段；若未发出倒车指令，则回到步骤(2)，继续更新路线；

发出停车指令时车辆若在第一阶段，则进入第二阶段，回到步骤(1)，继续更新路线，若未在第一阶段，则进入第三阶段。

(6)车辆在第三阶段进行纯追踪导航，若到达目标点，则转弯结束；

当车辆处于第三阶段时，停止更新路线，此阶段只进行纯追踪导航，若到达目标点D，则转弯结束。

尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换,凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种农用无人车辆地头转向的实时路径规划与控制方法，其特征在于，包括以下步骤：

(1)车辆开始转弯并初始化设备；

(2)实时获取车辆位置和航向信息，作为实时路径规划的输入信息；

(3)通过输入信息生成以当前位置为起点的最短规划路线；

(4)根据车辆当前所处阶段的位置与该阶段实时更新的结束点的距离判断是否发出停车指令；

(5)若发出停车指令，则进入下一阶段；若未发出停车指令，则回到步骤(2)，继续更新路线；

所述步骤(3)具体为：

通过输入的信息求解最优路线的三个阶段圆弧的弧度值，来生成最短路线，并在行驶中迭代更新这个过程；

转弯过程中，计算三个阶段圆弧的弧度值的方法为：车辆从第一作物行驶出，以车辆驶入第二作物行的瞬间车辆所在点D为坐标原点(0,0)，建立二维直角坐标系，(a,b)为车辆当前位置A坐标，车辆航向与x轴正方向夹角为φ，点B为第一段圆弧终点，点C为第二段圆弧终点，θ₁为第一段圆弧的弧度，θ₂为第二段圆弧的弧度，θ₃为第三段圆弧的弧度，R_f为车辆最小前进转弯半径，R_b为车辆最小倒车转弯半径，R为第三段圆弧半径，此时，在转弯过程中，B点和C点的坐标实时更新算法如下：

B点坐标为：

C点坐标为：

其中，θ₁、θ₂、θ₃间的几何关系式为：

①

②

第三段圆弧半径R：

此时，车辆驶过的路径长度，即三段圆弧长度之和s为：

车辆处于第一阶段时，根据公式(1)以及θ₁、θ₂、θ₃之间的三元二项式关系可知最小的s有唯一确定的θ₁、θ₂、θ₃，车辆处于第二阶段时，将θ₁＝0带入几何关系式①和②，根据所得的θ₁、θ₂、θ₃，确定一组唯一的转弯方案。

2.根据权利要求1所述的农用无人车辆地头转向的实时路径规划与控制方法，其特征在于，所述步骤(4)具体为：

设置一个容差，当车辆当前所处阶段的位置与该阶段结束点的距离小于所述容差时，发出停车指令。

3.根据权利要求2所述的农用无人车辆地头转向的实时路径规划与控制方法，其特征在于，所述容差为0.1米。

4.根据权利要求1所述的农用无人车辆地头转向的实时路径规划与控制方法，其特征在于，所述步骤(5)具体为：

发出停车指令时车辆若在第一阶段，则进入第二阶段，回到步骤(2)，继续更新路线，若未在第一阶段，则进入第三阶段。

5.根据权利要求4所述的农用无人车辆地头转向的实时路径规划与控制方法，其特征在于，还包括步骤(6)：

车辆在第三阶段进行纯追踪导航，若到达目标点，则转弯结束。

6.根据权利要求1所述的农用无人车辆地头转向的实时路径规划与控制方法，其特征在于，转弯开始后，在前两个阶段车辆以最小转弯半径转弯。

7.根据权利要求1所述的农用无人车辆地头转向的实时路径规划与控制方法，其特征在于，车辆位置和航向信息通过高精度GPS获取。

Images