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

权利要求书

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获取。