CN109471432A - 一种自主导航农用车最短避障路径规划方法 - Google Patents

Abstract

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

Description

一种自主导航农用车最短避障路径规划方法

技术领域

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

背景技术

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

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

发明内容

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

本发明的技术方案是：

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

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

(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的大小，若c>L/2，则为无效障碍物，反之，则为有效障碍物；

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

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

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

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

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

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

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

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

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

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

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

进一步的，所述步骤六中，考虑到土壤环境对转向模型的影响，需要对转向角α进行修正：旱地情况下α上浮5％，水田情况下α上浮10％。

进一步的，所述双目相机为自带深度信息计算模块的BumbleBee XB2双目相机。

本发明的有益效果：

本发明设计合理，操控方便，可以更加符合车辆的运动规律，实现车辆运行过程中的最优避障，从而可有效提高车辆的工作效率，充分满足市场的需求。

附图说明

图1构建防碰撞区域方法示意图。

图2极限转向半径的确定方法示意图。

图3四段式路径规划方法示意图。

图4相机坐标系与世界坐标系关系示意图。

图5最短避障路径示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的说明。

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

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

(1)通过自带深度信息计算模块的BumbleBee XB2双目相机采集车辆前方图像。

(2)利用Harris-SIFT算法检测障碍物的特征点。

(3)将获得的特征点像素坐标传入BumbleBee XB2深度信息计算模块，得到障碍物特征点在相机坐标下的空间坐标X_c,Y_c,Z_c。

(4)通过坐标转换矩阵：将X_c,Y_c,Z_c转换为X_w,Y_w,Z_w三轴世界坐标，完成障碍物的定位，如图4所示。其中，障碍物几何中心坐标(1，2.5)，X_w表示障碍物特征点的横坐标，Y_w表示世界坐标系原点到障碍物特征点的距离，Z_w表示障碍物特征点距离地面的高度，h为双目相机安装的高度，相机坐标系Z轴为相机采集道路信息方向。

步骤二：判断车辆是否需要执行避障动作，包括：

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

(2)如图1所示，设定障碍物的几何中心Z点坐标为(1.00，2.50)，d为Z点到Y轴的距离，则d＝1；以Z点到障碍物边界的最长距离R_z＝0.8为半径作圆，构建防碰撞区域。

(3)d与R_z的差值c为0.2，小于半车长L/2，则为有效障碍物。

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

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

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

(3)矩形右上角点为F，坐标为(0.50，0.80),根据运动过程中的临界碰撞条件“车辆右前角F与G点重合”，列出方程:

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

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

(1)如图3所示，设车辆的实际转向半径为r(r<8.1)，则实际转向中心X₂为(-r，0)。以X₂为圆心，以r为半径作圆。

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

(3)以Z为圆心，以ZB距离为半径作圆，与直线y＝2.5在x负半轴交于A点，弧BA为第二段避障路径。

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

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

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

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

(2)r₀为车辆最小转向半径，可从车辆说明书中获取为2.3。则在r∈[2.30，8.10]区间内，找出令S最小时的r取值，该值即为最优转向半径。S在r∈[2.30，8.10]区间内可导，且S的一阶导数在该区间上小于0，则说明S在该区间内递减：

则当r＝8.10时S最小，避障代价最小。

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

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

(2)考虑到土壤环境对转向模型的影响，需要对转向角α进行修正：旱地情况下α上浮5％，水田情况下α上浮10％。

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

具体为：利用步骤四，得到B点坐标为(-0.32，2.19)，A点坐标为(-0.34，2.50)，D点坐标为(-0.32，2.81)，E点坐标为(0，5.00)，规定圆弧OB为车辆行驶的第一段避障路径，圆弧BA为车辆行驶的第二段避障路径，圆弧AD为车辆行驶的第三段路径，圆弧DE为车辆行驶的第四段路径。

最后规划的最短避障路径，如图5所示，由圆弧OB，圆弧BA，圆弧AD，圆弧DE四段圆弧构成。

本发明未涉及部分均与现有技术相同或可采用现有技术加以实现。

Claims (3)

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的大小，若c>L/2，则为无效障碍物，反之，则为有效障碍物；

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

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

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

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

(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双目相机。