本发明涉及路径规划技术领域，具体涉及一种改进rrt算法的智能农业车辆路径规划方法。
背景技术：
智能农业车辆路径规划所设涉及的技术领域大多从机器人领域经过适当的改进演变而来，是一项综合性较强的交叉技术领域，因其强大的技术应用前景获得研发人员的广泛关注。常用的路径规划方法有a*、智能仿生算法、快速搜索随机树(rapidlyrandom-exploringtrees,rrt)以及各自的变形优化算法，其中rrt作为一种迭代的增量式搜索算法，因其概率完整性以及适用于高维空间的特性，获得相关行业人员的广泛研究与优化。
原始的rrt算法在一定情况下存在搜索效率低、生成路径非最优的不足，对于现存问题的解决与统筹优化可以不断完善rrt算法的应用效果。其中，基于一定概率引导搜索的p-rrt算法、双向搜索算法b-rrt以及智能采样的双向搜索算法ib-rrt、变概率、变步长的rrt搜索算法等等，均在一定程度上优化rrt的搜索生成路径，但对于大面积的搜索领域，存在搜索效率的时效性问题，其应用受到一定的限制。
技术实现要素：
针对现有技术的不足，本发明涉及一种以四颗搜索树同时进行搜索的rrt算法，提高大面积搜索效率的同时进一步平滑所生成路径，使得生成路径满足车辆的行驶要求。
本发明提出以生成四颗搜索树同时进行搜索的路径规划方法，并结合车辆自身转向约束以及目标方向指引的搜索策略进行路径规划。在已知所需规划的环境范围、障碍物位置并给定起始点qstart、目标点qgoal的前提下，计算出两点距离之间的中点qm。
以(xs，ys)表示起始点qstart的位置，(xg，yg)表示目标点qgoal的位置，则中点qm的位置表示为
采用函数detect(q)判断中点qm所处位置是否为自由区域zfree，具体为运用感知设备，若为障碍物区域，则输出为1，不采用此中点；若是自由区域，则输出为0，采用此中点；若是自由区域则采用此中点；
若不是自由区域，则以调整步长d的方式迭代搜寻属于自由区域的中点并且设定迭代次数k，并且判断当前迭代次数kd≤k，此时中点
当中点qm设定后，分别以中点qm表示起始点qstart的暂定目标点和目标点qgoal的暂定起始点。
考虑执行车辆的转向约束|δ|＜δmax纳入到搜索策略当中。
将基于目标偏向的引导策略纳入到搜索策略当中，即以一定的概率p进行目标偏向搜索，然后以滚动优化的方法进行路径规划时的实时动态避障：
1)以车辆搭载的传感器获得实时障碍物信息，构建局部滚动窗口win(qr(t))，若此时目标点qgoal∈win(qr(t))，则取子目标点qsub-goal＝qgoal，否则利用启发式函数f(q)＝g(q) εh(q)选取使得f(q)取最小值的点为子目标点qsub-goal；
2)采用启发式方法生成局部子目标点，在当前滚动窗口内规划一条优化的局部可行路径，启发估价函数为：f(q)＝g(q) εh(q)；其中：
εh(q)是从当前节点到目标节点的估计代价，ε为调整因子，通过调整ε选择估价函数最小值的节点，从而确定最优子目标；
3)确定子目标后，进行下一轮的周期滚动规划，直到到达目标点。
考虑上述因素并同时生成起始点qstart、中点qm双向搜索和中点qm、目标点qgoal的双向搜索机制，共计同时生成四颗随机树进行路径搜索。以中点qm向起始点qstart回溯生成l1路径。以目标点qgoal向中点qm回溯生成l2路径。
最后从目标点qgoal回溯整树，生成路径l并对其进行平滑处理，生成车辆可行驶路径。
附图说明
图1为本发明的总流程图；
图2为智能农业车辆的自行车模型示意图；
图3为原始rrt扩展方式示意图；
图4为本发明的四棵树扩展方式示意图。
具体实施方式
下面结合附图对本发明做进一步说明。
第一步：确定智能农业车辆的运行环境，包括障碍物的具体位置，将任务空间定义为z，zfree表示自由区域，zobs表示障碍物区域，zfree和zobs同为任务空间z的子集，并且满足:
z＝zfree∪zobs
起始点qstart、中点qm以及目标点qgoal均属于自由空间zfree，最终规划出的路径是一条无碰撞且平滑的车辆行驶路径。原始的rrt扩展方式如图3所示，因其随机进行扩展，造成搜索效率不高、生成路径车辆难以执行，因此进行目标引导策略并引入车辆转角约束。第二步：确定智能农业车辆的转角约束范围及确定目标引导偏向策略。车辆在行驶过程中需要考虑执行机构的约束条件，其中转角约束就是其中一项。车辆自行车模型如图2所示，其中δ为前轮的转向角，l为轴距，r为给定的转向角下后轴遵循的圆的半径。由此可知：
因为车辆规划路径时前轮转角应小于车辆最大转向角，即转角约束范围为：
|δ|＜≤δmax；
通过选取一定的概率p进行目标点方向扩展，通常p＝0-1,在此选取p＝0.3。
第三步：依据起始点qstart和目标点qgoal的位置，计算出中点qm，并判断中点qm是否属于自由区域zfree。参考图1流程图，详细判断方法如下：
步骤1)：运用detect(q)函数判断中点qm是否为自由区域，若属于自由区域，则选定此点为中点；
步骤2)：若不是自由区域，设定迭代次数并设置具体步长，循环迭代直到确定中点qm。
第四步：中点qm分别为起始点qstart的暂定目标点和目标点qgoal的暂定起始点，即以中点qm分别向起始点qstart和目标点qgoal进行双向扩展。
第五步：以起始点qstart为根节点向暂定目标点qm进行扩展，同时以中点qm向起始点qstart方向扩展，即形成双向扩展树。
第六步：以中点qm为暂定起始点向目标点qgoal进行扩展，同时以目标点qgoal向中点qm方向扩展，即形成双向扩展树。
第七步：将车辆的转向约束以及各自目标点的引导加入到搜索机制当中，进行搜索路径的构建，共形成四棵同时搜索的随机树，如图4所示。以滚动优化的方法进行路径规划时的实时动态避障。
以车辆搭载的传感器获得实时障碍物信息，构建局部滚动窗口win(qr(t))，若此时目标点qgoal∈win(qr(t))，则取子目标点qsub-goal＝qgoal，否则利用启发式函数f(q)＝g(q) εh(q)选取使得f(q)取最小值的点为子目标点qsub-goal。
采用启发式方法生成局部子目标点，在当前滚动窗口内规划一条优化的局部可行路径，启发估价函数为：f(q)＝g(q) εh(q)。其中：
εh(q)是从当前节点到目标节点的估计代价，ε为调整因子，通过调整ε选择估价函数最小值的节点，从而确定最优子目标。确定子目标后，进行下一轮的滚动规划，直到到达目标点。
第八步：以中点qm向起始点qstart回溯生成l1路径。
第九步：以目标点qgoal向中点qm回溯生成l2路径。
第十步：以目标点qgoal回溯整树生成总的路径l，并对路径进行平滑处理，生成车辆安全行驶路径。
综上所述，本发明改进的rrt算法能够适应大面积场景的路径规划，适用于智能农业车辆的实际作业环境，运用滚动优化的方法进行动态避障，提升搜索路径生成效率的同时进一步提升农业车辆作业效率。本发明改进的rrt算法能够应用于智能农业车辆的路径规划，具有较强的推广性，可以有效扩展到港口智能机械、智能清扫机等。
1.一种基于改进rrt算法的智能农业车辆路径规划方法，其特征在于对rrt算法进行改进，使用改进的rrt算法能够快速为智能农业车辆规划出一条切实可行的路径，主要包括以下步骤：
步骤一：选定起始点和目标点的中点qm，保证能够选取起始点qstart的暂定目标点和目标点qgoal的暂定起始点，以中点qm同时作为起始点qstart的暂定目标点和目标点qgoal的暂定起始点，从而可以同时生成四棵搜索树进行路径搜索，提高路径规划速率；
步骤二：在搜索的过程中，根据车辆的转向约束以及目标引导策略，提升搜索效率，保障生成路径的可执行性；
步骤三：在搜索的过程中，运用滚动优化的方法生成滚动窗口，以利用实时感知到的窗口信息，进行迭代路径规划生成，从而实现动态避障；
步骤四：在路径回溯时，分别从中点qm向起始点qstart回溯路径以及从目标点qgoal向中点qm回溯路径，最后以目标点qgoal回溯整体路径，生成平滑路径。
2.根据权利要求1所述的一种基于改进rrt算法的智能农业车辆路径规划方法，其特征在于，所述步骤一中，选定起始点和目标点的中点qm，具体为：
以(xs，ys)表示起始点qstart的位置，(xg，yg)表示目标点qgoal的位置，则中点qm的位置表示为
运用函数detect(q)判断中点qm所处位置是否为自由区域zfree，若为障碍物区域，则输出为1，不采用此中点；若是自由区域，则输出为0，采用此中点；若不是自由区域，则以调整步长d的方式迭代搜寻属于自由区域的中点并且设定迭代次数k，并且判断当前迭代次数是否kd≤k，此时中点
3.根据权利要求1所述的一种基于改进rrt算法的智能农业车辆路径规划方法，其特征在于，所述步骤二中，基于车辆转向约束和目标引导策略生成路径，车辆转向约束为：|δ|＜δmax；目标引导策略为以一定的概率p向目标点进行扩展，有效提高搜索效率。
4.根据权利要求1所述的一种基于改进rrt算法的智能农业车辆路径规划方法，其特征在于，所述步骤三中，基于滚动窗口实现动态避障，具体为：
1)以车辆搭载的传感器获得实时障碍物信息，构建局部滚动窗口win(qr(t))，若此时目标点qgoal∈win(qr(t))，则取子目标点qsub-goal＝qgoal，否则利用启发式函数f(q)＝g(q) εh(q)，选取使得f(q)取最小值的点为子目标点qsub-goal；
2)采用启发式方法生成局部子目标点，在当前滚动窗口内规划一条优化的局部可行路径，启发估价函数为：f(q)＝g(q) εh(q)。其中：
g(q)＝d(qstart，q)；
其中，εh(q)是从当前节点到目标节点的估计代价，ε为调整因子，通过调整ε选择估价函数最小值的节点，从而确定最优子目标；
3)确定子目标后，进行下一轮的周期滚动规划，直到到达目标点。
5.根据权利要求1所述的一种基于改进rrt算法的智能农业车辆路径规划方法，其特征在于，在路径回溯时，分别从中点qm向起始点qstart回溯路径以及从目标点qgoal向中点qm回溯路径，最后以目标点qgoal回溯整体路径。
技术总结