用于农机无人驾驶的田间避障路径规划及其控制方法技术

本发明专利技术提供了农业无人驾驶控制技术领域内的一种用于农机无人驾驶的田间避障路径规划及其控制方法，农机自动绕开障碍物的具体步骤为，步骤1：通过传感器获取农机环境信息做出避障决策；步骤2：使用改进的最短切线法离线计算出一条理论避障路径；步骤3：利用基于Bezier曲线的路径优化方法优化步骤2中的理论避障路径得到实际避障路径，使用曲线跟踪方法获得实时曲线曲率和实时的农机航向偏差、横向偏差，用状态反馈控制器和自适应控制器的结合计算当前的前轮转向角，使农机沿着实际避障路径行走绕开障碍物；本发明专利技术中规划的避障路径容易控制且行走的路程短，控制精度高。

Obstacle avoidance path planning and control method for unmanned agricultural vehicle

The present invention provides agricultural unmanned control for agricultural field unmanned obstacle avoidance and path planning and control method in the field of technology, the specific steps of agricultural automatic obstacle avoidance for step 1: through the sensor to obtain agricultural environmental information make obstacle avoidance decision; step 2: the shortest tangent method off-line using improved calculation a theory of obstacle avoidance path; step 3: using the theory of path optimization method optimization step 2 Bezier curve of obstacle avoidance and path to get the actual path of obstacle avoidance based on tracking method using curve real-time curve curvature and real-time agricultural course deviation and horizontal deviation, combined with feedback controller and adaptive controller to calculate the current of the front wheel steering angle with the actual state, agricultural machinery along the obstacle avoidance path walking around obstacles; the path planning and control for easy It has short travel distance and high control accuracy.

【技术实现步骤摘要】
用于农机无人驾驶的田间避障路径规划及其控制方法
本专利技术涉及一种避障路径规划及其控制方法，特别涉及一种用于农机无人驾驶的田间避障路径规划及其控制方法。
技术介绍
农机在自动导航作业时多在环境部分未知的情况下运行，实现对人进行安全保护和对农作物的伤害程度减到最低，同时又能最大的发挥自主导航农业车辆的生产效率，将是一个重要的研究问题，同时农机可能会遇到电线杆、小石块等相对比较小的障碍物，需要农机能自动绕过这些障碍物并迅速回到作业的路线。现有技术中，针对比较小的障碍物，采用最短切线法设置避障路径，最短切线法形成的避障路径由两段直线段和一段圆弧段组成，直线段分别与圆弧段相切，这种避障路径虽然简单快捷，对于具有最小转弯半径的拖拉机很难按照折角进行转弯且很难控制，若控制农机按照这种避障路径行走，农机的控制精度很低。另外，在现有的农机路径跟踪方法上，主要有基于模型的控制方法和与模型无关的控制方法。在模型的控制方法上，主要是基于运动学模型和动力学模型的路径跟踪方法。基于运动学模型的控制方法是对模型进行小角度的线性化逼近，在常速假设条件下进行控制器设计这样不但引入了线性化误差，而且速度变化时控制器的鲁棒性较差；基于动力学模型的控制方法虽然模型的精度高，但是动力学模型的参数很难实时获取。在与模型无关的控制方法上面，纯追踪方法的前视距离的在线自适应确定问题还没有很好的解决，技术不成熟，控制精度低；智能方法虽然有传统的控制方法无法比拟的仿人智能和非线性映射能力，但是设计需要一定的经验知识和复杂的学习训练过程，总之，现有的路径轨迹的控制方法不仅无法同时实现高的控制精度和实时获取农机的运动参数，而且对设计人员的要求很高，适应性差。总而言之，无论是避障路径规划还是避障路径的控制方法，控制农机按照设定好的路径行走的精度很低，偏离设定的避障路径。
技术实现思路
针对现有技术中的缺陷，本专利技术的目的在于克服上述现有技术中的不足之处，解决现有技术中避障路径难控制且控制精度低的技术问题，提供一种用于农机无人驾驶的田间避障路径规划及其控制方法，本专利技术中的避障路径容易控制且行走的路程短，效率高，控制精度高。本专利技术的目的是这样实现的：一种用于农机无人驾驶的田间避障路径规划及其控制方法，农机自动绕开障碍物的具体步骤为，步骤1：通过传感器获取农机环境信息做出避障决策；步骤2：使用改进的最短切线法离线计算出一条理论避障路径；步骤3：利用基于Bezier曲线的路径优化方法优化步骤2中的理论避障路径得到实际避障路径，使得优化后的路径更加容易控制，使用预瞄和PI控制器组合起来控制农机的前轮转角使农机沿着实际避障路径行走绕开障碍物。本专利技术工作时，农机在行走过程中安装在农机上的传感器感应农机周围的环境信息，当农机前方有障碍物时，做出避障决策，使用改进的最短切线法计算出一条理论避障路径，对理论避障路径进行优化得到一条更加容易控制的实际避障路径，农机通过传感器检测获得农机的位置信息，使用曲线跟踪方法实时获得设定好路径的曲线曲率、农机航向偏差和横向偏差，用状态反馈控制器和自适应控制器的结合计算出当前的前轮转向角，农机在行走过程中通过实时调整农机的前轮转角使农机沿着设定的实际避障路径行走，从而实现农机的自动避障；本专利技术通过改进后的最短切线法计算出一条理论避障路径，使用基于Bezier曲线的路径优化方法对理论避障路径进行优化，使避障路径更加容易控制，通过状态反馈控制器和自适应控制器的结合控制农机的前轮转向角使农机沿着设定好的避障曲线行走，控制精度高；可应用于无人驾驶的农机在田间作业时自动避障的工作中。为了进一步提高获得理论避障路径的可靠性，步骤2中，计算理论避障路径具体地为，计算农机前方障碍物的特征圆的大小、农机与障碍物的距离，根据特征圆的大小设定安全距离，根据农机的犁具宽度和农机最小转弯半径，设定一条理论避障路径。为了使避障路径更加容易控制，步骤2中，最短切线法具体的为，以障碍物的中心为圆心做特征圆，特征圆的半径为rmin+w/2，理论避障路径由圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三组成，圆弧段一的一端与农机原始的直线路径相切，圆弧段一的另一端与直线段一的一端相切，直线段一的另一端和直线段二的一端分别与圆弧段二相切，直线段二的另一端与圆弧段三相切，圆弧段二为特征圆上的一段，圆弧段一和圆弧段三关于圆弧段二的中心线对称设置，农机依次经过圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三绕过障碍物，其中，rmin为农机的最小转弯半径，w为农机的作业宽度，障碍物的外接圆半径小于最小转弯半径。为了进一步提高农机掉头路径的精度，所述圆弧段一的半径为rmin，所述圆弧段三的半径为rmin，圆弧段一的起点记为H点，圆弧段一的圆心记为O1点，直线段一与农机原始的直线路径的相交点记为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点坐标；设点O1的坐标为（x2,y2），则点O1到直线JD的距离为：根据公式（1-3）和（1-4）求出O1的坐标；则H点的坐标为（x2，y1），B点的坐标为（a，b+r）；此设计对构成理论避障路径的各个线段建立数学关系的联系，明确曲线的具体形状，求出弯折点处坐标，方便下一步对理论避障路径的优化。为了优化本专利技术中利用改进的最短切线法设计的理论避障路径，步骤3中，利用基于Bezier曲线的路径优化方法优化步骤2中的理论避障路径，具体的为，建立Bezier方程，(1)给定空间n+1个点的位置矢量，则参数曲线上各点坐标的插值公式为：（2-1）其中构成该曲线的特征点,是n次Bernstein基函数：（2-2）由上述公式，可以得出三次和二次Bezier曲线的数学表达式，当n=3时，Q(t)为三次多项式，有四个控制点，其矩阵形式表达为：（2-3）当n=2，Q(t)为二次多项式，有三个控制点，矩阵表达式为：（2-4）（2）Bezier曲线的曲率表达式为：（2-5）其中，y=f（x）表示曲线的方程，y’为曲线的一阶导数，y”为二阶导数；曲率半径为：（2-6）；此设计中，提出Bezier曲线优化方法对理论避障路径进行优化，具体的将曲率不连续的理论避障路径优化成曲率连续的实际避障路径，实际避障路径更加容易控制。为了提高Bezier曲线的可控性，选取三次Bezier曲线，针对三次Bezier曲线：（2-7）（2-8）其中，X0、X1、X2、X3分别为P0点、P1点、P2点和P3点处的横向坐标，Y0、Y1、Y2和Y3分别为P0点、P1点、P2点和P3处的纵向坐标；P0点对应圆弧段一的起点H（x2，y1），P3点对应圆弧段二的中心点B（a，b+r），P1点（（x2+a）/2，y1），P2点（（x2+a）/2，b+r），则实际故障路径对应的曲线的曲率半径计算公式为：（2-9）；此设计中，选择理论避障路径上的弯折点作为Betizer曲线优化方法中的优化点，优化后的路径更加简单，曲率连续，易于控本文档来自技高网...

【技术保护点】
一种用于农机无人驾驶的田间避障路径规划及其控制方法，其特征在于，农机自动绕开障碍物的具体步骤为，步骤1：通过传感器获取农机环境信息做出避障决策；步骤2：使用改进的最短切线法离线计算出一条理论避障路径；步骤3：利用基于Bezier曲线的路径优化方法优化步骤2中的理论避障路径得到实际避障路径，使用曲线跟踪方法获得实时曲线曲率和实时的农机航向偏差、横向偏差，用状态反馈控制器和自适应控制器的结合计算当前的前轮转向角，控制农机的转向角使农机沿着实际避障路径行走，以绕开障碍物并回到原始导航路径上。

【技术特征摘要】
1.一种用于农机无人驾驶的田间避障路径规划及其控制方法，其特征在于，农机自动绕开障碍物的具体步骤为，步骤1：通过传感器获取农机环境信息做出避障决策；步骤2：使用改进的最短切线法离线计算出一条理论避障路径；步骤3：利用基于Bezier曲线的路径优化方法优化步骤2中的理论避障路径得到实际避障路径，使用曲线跟踪方法获得实时曲线曲率和实时的农机航向偏差、横向偏差，用状态反馈控制器和自适应控制器的结合计算当前的前轮转向角，控制农机的转向角使农机沿着实际避障路径行走，以绕开障碍物并回到原始导航路径上。2.根据权利要求1所述的用于农机无人驾驶的田间避障路径规划及其控制方法，其特征在于，步骤2中，计算理论避障路径具体地为，计算农机前方障碍物的特征圆的大小、农机与障碍物的距离，根据特征圆的大小设定安全距离，根据农机的犁具宽度和农机最小转弯半径，设定一条理论避障路径。3.根据权利要求2所述的用于农机无人驾驶的田间避障路径规划及其控制方法，其特征在于，步骤2中，最短切线法具体的为，以障碍物的中心为圆心做特征圆，特征圆的半径为rmin+w/2，理论避障路径由圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三组成，圆弧段一的一端与农机原始的直线路径相切，圆弧段一的另一端与直线段一的一端相切，直线段一的另一端和直线段二的一端分别与圆弧段二相切，直线段二的另一端与圆弧段三相切，圆弧段二为特征圆上的一段，圆弧段一和圆弧段三关于圆弧段二的中心线对称设置，农机依次经过圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三绕过障碍物，其中，rmin为农机的最小转弯半径，w为农机的作业宽度，障碍物的外接圆半径小于最小转弯半径。4.根据权利要求3所述的用于农机无人驾驶的田间避障路径规划及其控制方法，其特征在于，所述圆弧段一的半径为rmin，所述圆弧段三的半径为rmin，圆弧段一的起点记为H点，圆弧段一的圆心记为O1点，直线段一与农机原始的直线路径的相交点记为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点坐标；设点O1的坐标为（x2,y2），则点O1到直线JD的距离为：根据公式（1-3）和（1-4）求出O1的坐标；则H点的坐标为（x2，y1），B点的坐标为（a，b+r）。5.根据权利要求4所述的用于农机无人驾驶的田间避障路径规划及其控制方法，其特征在于，步骤3中，利用基于Bezier曲线的路径优化方法优化步骤2中的理论避障路径，具体的为，建立Bezier方程，(1)给定空间n+1个点的位置矢量，则参数曲线上各点坐标的插值公式为：（2-1）其中构成该曲线的特征点，是n次Bernstein基函数：（2-2）由上述公式，可以得出三次和二次Bezier曲线的数学表达式，当n=3时，Q(t)为三次多项式，有四个控制点，其矩阵形式表达为：（2-3）当n=2,Q(t)为二次多项式，有三个控制点，矩阵表达式为：（2-4）（2）Bezier曲线的曲率表达式为：（2-5）其中，y=f（x）表示曲线的方程，y’为曲线的一阶导数，y”为二阶导数；曲率半径为：（2-6）根据权利要求5所述的用于农机无人驾驶的田间避障路径规划...

【专利技术属性】
技术研发人员：程方，林德相，袁洪良，王烁，徐涛，王浩，吴飞，
申请(专利权)人：无锡卡尔曼导航技术有限公司，
类型：发明
国别省市：江苏,32