[发明专利]一种自主导航农用车最短避障路径规划方法

说明书

本发明涉及一种自主导航农用车最短避障路径规划方法，包括以下步骤：1)基于双目视觉的障碍物定位；2)判断障碍物的有效性；3)车辆避障时极限转向半径的确定，4)四段式路径规划；5)确定最优转向半径；6)分段路径跟踪控制。本发明设计合理，操控方便，可以更加符合车辆的运动规律，实现车辆运行过程中的最优避障，从而，可有效提高车辆的工作效率，充分满足市场的需求。

技术领域

本发明涉及一种农业机械，尤其是一种农用车的控制方法，具体的说是一种自主导航农用车最短避障路径规划方法。

背景技术

自主导航农用车辆能够显著提高作业效率、改善作业质量、节省人力成本。因此，国家和部分地方科技主管部门都将自主导航拖拉机作为重点支持项目列入科技计划。

目前，一般的自主导航农用车辆在田间工作时，遇到障碍物需自主执行避障动作，但普遍规划的避障路径大都较为复杂，有些不符合车辆的运动规律，有些大幅度偏离了原来的导航路径，不利于车辆的控制，也影响车辆的工作效率。

发明内容

本发明的目的是针对现有技术的不足，提供一种自主导航农用车最短避障路径规划方法，可以更加符合车辆的运动规律，提高车辆的工作效率。

本发明的技术方案是：

一种自主导航农用车最短避障路径规划方法，包括以下步骤：

步骤一：基于双目视觉的障碍物定位，包括：

(5)通过双目相机实时采集车辆前方道路图像；

(6)对采集得到的图像利用Harris-SIFT算法进行特征点检测，获取障碍物特征点的像素坐标；

(7)将获得的特征点像素坐标传入相机深度信息计算模块，得到障碍物在相机坐标系下的空间坐标；

(8)再次经过坐标转换，将相机坐标系下的空间坐标转换到世界坐标系下，得到障碍物几何中心坐标(X_z，Y_z)，完成障碍物的定位；

步骤二：判断障碍物的有效性，包括：

(4)建立矩形车辆几何模型，以车辆几何中心O为原点建立平面坐标系，x轴正方为水平向右，y轴正方向与车辆的航向重合；

(5)以障碍物的几何中心Z点为圆心，以Z点到障碍物边界的最长距离R_z为半径作圆，构建防碰撞区域；

(6)计算Z点到y轴的距离d，并比较d和R_z的差值c与半车宽L/2的大小，若cL/2，则为无效障碍物，反之，则为有效障碍物；

步骤三：车辆避障时最大转向半径的确定，包括：

(4)设车辆能够避障的最大转向半径为r_m，则可确定初始转向中心X₁的位置为(-r_m，0)；

(5)连接X₁与Z点，与防碰撞区域圆Z相交于G点；

(6)根据运动过程中的临界碰撞条件“车辆右前角F与G点重合”，列出方程：其中，X_f和Y_f是车辆右前角F的坐标；

(4)解方程，可得最大转向半径r_m；

步骤四：四段式路径规划，包括：

(6)设车辆的实际转向半径为r，r≤r_m，则实际转向中心X₂为(-r，0)；以X₂为圆心，以r为半径作圆；