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

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(2)连接圆心X₂与Z点，与圆X₂相交于B点，则弧OB为第一段避障路径；

(3)以Z为圆心，以ZB距离为半径作圆，与直线y＝Y_z交于A点，则弧BA为第二段避障路径；

(4)作B点关于直线y＝Y_z的对称点D，则弧AD为第三段避障路径；

(5)作X₂关于直线y＝Y_z的对称点X₃，以X₃为圆心，X₃到D点的距离为半径作圆，与y轴相切于E点，则弧DE为第四段避障路径；

步骤五：确定最优转向半径，包括：

(1)以四段避障路径的总长度S为避障代价，经数学计算，得到S关于转向半径r的函数：

(2)在r∈[r₀,r_m]区间内，找出令S最小时的r取值，该值即为最优转向半径；其中r₀为车辆最小转向半径，可从车辆说明书中获取，r_m为步骤三确定的车辆避障时最大转向半径；

步骤六：分段路径跟踪控制，包括：

(1)根据车辆转向模型计算各段路径所对应的转向角α，其中K₁为车辆前后轴距，K₂为前轮左右轮距，R为转向半径；

(2)在各段路径的起点，通过控制车辆的转向执行机构，使转向角等于α，进行路径跟踪，实现车辆在作业过程的最优避障。

2.根据权利要求1所述的自主导航农用车最短避障路径规划方法，其特征是所述步骤六中，考虑到土壤环境对转向模型的影响，需要对转向角α进行修正：旱地情况下α上浮5％，水田情况下α上浮10％。

3.根据权利要求1所述的自主导航农用车最短避障路径规划方法，其特征是所述双目相机为自带深度信息计算模块的BumbleBee XB2双目相机。