[发明专利]用于农机无人驾驶的路径规划及其控制方法

权利要求书

1.一种用于农机无人驾驶的路径规划及其控制方法，其特征在于，

步骤1：通过传感器获取农机环境信息做出掉头或避障决策；若为避障决策，规划避障路径轨迹，具体包括以下步骤，

步骤101：使用改进的最短切线法离线计算出一条理论避障路径；

步骤102：利用基于Bezier曲线的路径优化方法优化步骤101中的理论避障路径得到实际避障路径；

若为掉头决策，规划掉头路径轨迹，具体包括以下步骤，

步骤101’：输入农机的作业模式；

步骤102’：根据农机实际的作业模式选择对应的掉头路径；

步骤2：农机在前进过程中，农机通过传感器获得农机位置信息，使用曲线跟踪方法获得实时曲线曲率和实时的农机航向偏差、横向偏差，用状态反馈控制器和自适应控制器的结合计算当前的前轮转向角，控制农机的转向角实现农机的自动掉头或避障。

2.根据权利要求1所述的用于农机无人驾驶的路径规划及其控制方法，其特征在于，步骤101中，计算理论避障路径具体的为，计算农机前方障碍物的特征圆的大小、农机与障碍物的距离，根据特征圆的大小设定安全距离，根据农机的犁具宽度和农机最小转弯半径，设定一条理论避障路径；改进的最短切线法具体的为，以障碍物的中心为圆心做特征圆，特征圆的半径为r_min+w/2，理论避障路径由圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三组成，圆弧段一的一端与农机原始的直线路径相切，圆弧段一的另一端与直线段一的一端相切，直线段一的另一端和直线段二的一端分别与圆弧段二相切，直线段二的另一端与圆弧段三相切，圆弧段二为特征圆上的一段，圆弧段一和圆弧段三关于圆弧段二的中心线对称设置，农机依次经过圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三绕过障碍物；其中，r_min为农机的最小转弯半径，w为农机的作业宽度，障碍物的外接圆半径小于最小转弯半径。

3.根据权利要求2所述的用于农机无人驾驶的路径规划及其控制方法，其特征在于，所述圆弧段一的半径为r_min，所述圆弧段三的半径为r_min，圆弧段一的起点记为H点，圆弧段一的圆心记为O₁点，直线段一与农机原始的直线路径的相交点记为J，直线段一与圆弧段二的相切点记为D，农机原始路径与特征圆的相交点分别记为K和K’，JK=w/2，圆弧段二的圆心记为O点，O的坐标设为（a，b），圆弧段二的中心点记为B点，J点的坐标记为（x1，y1），JD的方程可以写成：

（1-1）；

特征圆的方程可以写成：

（1-2）

通过（1-1）和（1-2）可以求出k，D点为JD和特征圆的相交点，以此解出D点坐标；

设点O₁的坐标为（x₂,y₂），则点O₁到直线JD的距离为：

根据公式（1-3）和（1-4）求出O₁的坐标；则H点的坐标为（x₂，y₁），B点的坐标为（a，b+r）。