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

摘要

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

主权项

1.一种自主导航农用车最短避障路径规划方法，其特征是包括以下步骤：步骤一：基于双目视觉的障碍物定位，包括：(1)通过双目相机实时采集车辆前方道路图像；(2)对采集得到的图像利用Harris‑SIFT算法进行特征点检测，获取障碍物特征点的像素坐标；(3)将获得的特征点像素坐标传入相机深度信息计算模块，得到障碍物在相机坐标系下的空间坐标；(4)再次经过坐标转换，将相机坐标系下的空间坐标转换到世界坐标系下，得到障碍物几何中心坐标(Xz，Yz)，完成障碍物的定位；步骤二：判断障碍物的有效性，包括：(1)建立矩形车辆几何模型，以车辆几何中心O为原点建立平面坐标系，x轴正方为水平向右，y轴正方向与车辆的航向重合；(2)以障碍物的几何中心Z点为圆心，以Z点到障碍物边界的最长距离Rz为半径作圆，构建防碰撞区域；(3)计算Z点到y轴的距离d，并比较d和Rz的差值c与半车宽L/2的大小，若c>L/2，则为无效障碍物，反之，则为有效障碍物；步骤三：车辆避障时极限转向半径的确定，包括：(1)设车辆能够避障的最大转向半径为rm，则可确定初始转向中心X1的位置为(‑rm，0)；(2)连接X1与Z点，与防碰撞区域圆Z相交于G点；(3)根据运动过程中的临界碰撞条件“车辆右前角F与G点重合”，列出方程：(4)解方程，可得最大转向半径rm；步骤四：四段式路径规划，包括：(1)设车辆的实际转向半径为r(r≤rm)，则实际转向中心X2为(‑r，0)；以X2为圆心，以r为半径作圆；(2)连接圆心X2与Z点，与圆X2相交于B点，则弧OB为第一段避障路径；(3)以Z为圆心，以ZB距离为半径作圆，与直线y＝YZ交于A点，则弧BA为第二段避障路径；(4)作B点关于直线y＝YZ的对称点D，则弧AD为第三段避障路径；(5)作X2关于直线y＝YZ的对称点X3，以X3为圆心，X3到D点的距离为半径作圆，与y轴相切于E点，则弧DE为第四段避障路径；步骤五：确定最优转向半径，包括：(1)以四段避障路径的总长度S为避障代价，经数学计算，得到S关于转向半径r的函数：(2)在r∈[r0,rm]区间内，找出令S最小时的r取值，该值即为最优转向半径；其中r0为车辆最小转向半径，可从车辆说明书中获取，rm为步骤三确定的车辆避障时极限转弯半径；步骤六：分段路径跟踪控制，包括：(1)根据车辆转向模型计算各段路径所对应的转向角α。其中K₁为车辆前后轴距，K₂为前轮左右轮距，R为转向半径；(2)在各段路径的起点，通过控制车辆的转向执行机构，使转向角等于α，进行路径跟踪，实现车辆在作业过程的最优避障。