一种自动驾驶系统的优化度确定算法的制作方法

本发明涉及自动驾驶，特别涉及一种自动驾驶系统的优化度确定算法。

背景技术：

1、随着云定位和协同精密定位等技术的不断发展，农机自动驾驶技术已大规模应用于农业生产，有利于降低劳动强度、提高作业质量和提升劳动效率，致力于进一步减少劳动力数量，应对农业劳动力日益紧缺和用工成本日益高涨等严峻问题。

2、农机自动驾驶技术是精准农业实施的关键与基础，目前农机自动驾驶仪已得到大规模推广使用，广泛应用于耕作、播种、施肥、喷药、收获等农业生产过程。现阶段的农机自动驾驶主要以直线驾驶为主，这主要受制于拖拉机动力换挡技术。农机在田间作业时可能遇到电线杆、电塔桩、树木等障碍物，需要农机能自动绕过障碍物并迅速回到原有作业路线，因此自动避障是农机无人驾驶的必要技术条件。对于农机自动驾驶过程中的主动避障，一般采取局部路径规划方法，农机在行驶过程中利用传感器获取自身状态信息及周围环境信息，系统监测行驶路径上存在障碍物时，将在短时内提前规划避障路径，并控制农机沿规划路径避障。因此，避障路径的规划与控制将直接关系农机能否安全避障、避障精度及避障后直线行驶精度。

技术实现思路

1、本发明提供了一种自动驾驶系统的优化度确定算法，根据障碍物轮廓面积与障碍物轮廓外接圆面积比值自由选择预设的最短切线法或优化的bezier曲线规划农机的避障路径，使得避障路径曲线曲率连续，利于农机行驶控制，使得农机安全避障，提高避障精度及避障后直线行驶精度。

2、本发明提供了一种自动驾驶系统的优化度确定算法，包括：

3、获取基于wgs84位置数据坐标系采集的农田中的障碍物测绘点数据，并对所述障碍物测绘点数据进行处理得到障碍物轮廓点数据；

4、以所述障碍物轮廓点数据中相距最远的两个顶点之间的距离作为直径，确定障碍物轮廓的外接圆；

5、计算所述障碍物轮廓的面积作为第一面积，以及计算所述障碍物轮廓的外接圆的面积作为第二面积；

6、根据所述第一面积与第二面积的比值确定农机的避障路径，以对所述农机的作业路线进行优化；

7、控制所述农机按照具有所述避障路径的作业路线行驶。

8、进一步地，所述获取基于wgs84位置数据坐标系采集的农田中的障碍物测绘点数据，并对所述障碍物测绘点数据进行处理得到障碍物轮廓点数据的步骤，包括：

9、获取基于wgs84位置数据坐标系采集的农田中的障碍物测绘点数据；

10、对点名称进行解析和坐标系转换，获取每个障碍物对象的测绘点列表，字段包括障碍物类型、障碍物中的点序号、点尺寸、东向坐标和北向坐标；其中，所述障碍物包括点障碍物和线障碍物；

11、根据障碍物类型的不同，构造点障碍物和线障碍物轮廓计算方式，统一为多边形形式，并用其顶点表示：线障碍物的点坐标为边中点，根据点尺寸和点坐标，利用相邻点的矢量垂线生成该点所在边的两端点，进而计算障碍物多边形的顶点。

12、进一步地，所述根据所述第一面积与第二面积的比值确定农机的避障路径，以对所述农机的作业路线进行优化的步骤，包括：

13、判断所述第一面积与第二面积的比值是否小于面积阈值；

14、若所述第一面积与第二面积的比值小于面积阈值，则采用预设的最短切线法规划所述农机的避障路径；

15、若所述第一面积与第二面积的比值大于等于面积阈值，则采用优化的bezier曲线规划所述农机的避障路径。

16、进一步地，所述采用预设的最短切线法规划所述农机的避障路径，包括：

17、确定所述障碍物的特征圆，所述特征圆的半径r＝rmin+0.5l；其中，rmin为农机最小转弯半径，l为农机作业幅宽；

18、确定所述农机与所述障碍物的距离；其中，所述农机与障碍物的距离为所述农机与特征圆的最短距离；

19、根据所述农机的最小转弯半径和所述农机的位置确定避障转弯圆；

20、将所述障碍物的特征圆与所述避障转弯圆的最短切线作为直线段，所述农机的位置到所述直线段一端的路径作为前弧线段，所述障碍物的特征圆的垂直点到所述直线段另一端的路径作为后弧线段；其中，所述垂直点为与所述农机垂直的特征圆直径与所述特征圆的交点；

21、将所述前弧线段、直线段、后弧线段连接作为前半段路径，并将所述前半段路径以中心线为基准进行镜像，生成后半段路径；其中，所述中心线为与所述农机垂直的特征圆直径；

22、将所述前半段路径与后半段路径连接得到所述农机的避障路径。

23、进一步地，所述采用优化的bezier曲线规划所述农机的避障路径，包括：

24、根据所述预设的最短切线法确定所述农机的七个避障控制点，并将七个避障控制点分为前半段四个控制点和后半段四个控制点；

25、采用两段三阶bezier曲线确定所述前半段四个控制点的路径和后半段四个控制点的路径；其中，三阶bezier曲线为三次多项式，其矩阵为：

26、

27、则有：

28、

29、其中，x0、x1、x2、x3分别为四个控制点p0、p1、p2、p3的横坐标，y0、y1、y2、y3分别为四个控制点p0、p1、p2、p3的纵坐标。

30、进一步地，所述采用两段三阶bezier曲线确定所述前半段四个控制点的路径和后半段四个控制点的路径的步骤之后，还包括：

31、将前半段的第二控制点朝向障碍物移动设定距离，以及将前半段的第三控制点朝向远离障碍物的方向移动设定距离，得到前半段的新第二控制点、第三控制点；

32、将后半段的第二控制点朝向远离障碍物的方向移动设定距离，以及将后半段的第三控制点朝向障碍物移动设定距离，得到后半段的新第二控制点、第三控制点；

33、根据前半段的新第二控制点、第三控制点和后半段的新第二控制点、第三控制点，以及其他控制点新生成三阶bezier曲线。

34、进一步地，所述根据前半段的新第二控制点、第三控制点和后半段的新第二控制点、第三控制点，以及其他控制点新生成三阶bezier曲线的步骤之后，还包括：

35、计算前半段四个控制点的路径曲率，以及计算后半段四个控制点的路径曲率，以判定前半段路径和后半段路径组成的所述避障路径是否连续；其中，曲率的计算公式为：

36、

37、其中，c为曲率，x、y分别为控制点的横、纵坐标。

38、本发明还提供了一种自动驾驶系统的优化度确定装置，包括：

39、获取模块，用于获取基于wgs84位置数据坐标系采集的农田中的障碍物测绘点数据，并对所述障碍物测绘点数据进行处理得到障碍物轮廓点数据；

40、确定模块，用于以所述障碍物轮廓点数据中相距最远的两个顶点之间的距离作为直径，确定障碍物轮廓的外接圆；

41、计算模块，用于计算所述障碍物轮廓的面积作为第一面积，以及计算所述障碍物轮廓的外接圆的面积作为第二面积；

42、优化模块，用于根据所述第一面积与第二面积的比值确定农机的避障路径，以对所述农机的作业路线进行优化；

43、控制模块，用于控制所述农机按照具有所述避障路径的作业路线行驶。

44、本发明还提供了一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述方法的步骤。

45、本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述方法的步骤。

46、本发明的有益效果为：

47、本发明获取农田中的障碍物测绘点数据，并进行处理得到障碍物轮廓点数据，以此确定障碍物轮廓的外接圆；计算障碍物轮廓的面积，以及计算障碍物轮廓的外接圆的面积，并根据二者的面积比值确定农机的避障路径，自由选择预设的最短切线法或优化的bezier曲线规划农机的避障路径，以对农机的作业路线进行优化，使得避障路径曲线曲率连续，利于农机行驶控制，使得农机安全避障，提高避障精度及避障后直线行驶精度。