[发明专利]一种基于智能农机的快速偏移矫正的路径规划方法

说明书

本发明涉及一种基于智能农机的快速偏移矫正的路径规划方法，用于解决农机产生路径偏移时，传统局部路径规划可能降低农机实时性和稳定性的问题。具体采用了粒子群算法对Pure‑pursuit局部路径规划算法进行优化。首先判断当前行驶位置是否偏离规定路线，如果偏离，则调用PSO算法，寻找目标矫正导航点进行快速纠偏。其中本发明对PSO算法进行了改进，将传统PSO算法中固定的惯性权重改为自适应权重，有助于农机路径偏移时的快速纠偏。通过不断迭代，当更新后的粒子与原路径规划中的农机目标跟踪点的欧式距离达到设定要求时，停止纠偏，此时PSO得到的全局最优粒子即为下一时刻的导航目标位置。

技术领域

本发明涉及一种基于智能农机的快速偏移矫正的路径规划方法，特别是在车辆打滑、农田崎岖、GPS信号短时间丢失等情况下发挥作用的方法，属于智能农机装备领域。

技术背景

智能农机装备是提质增效、转变发展方式、增强农业综合生产能力的基础，智能农机的自动避障路径规划问题是智能车辆研究领域的热点之一。在国外，约翰迪尔，爱科、凯斯等知名农机厂商在此方面已取得长足发展。然而在国内，拖拉机信息化、智能化水平低下，还无法与国外产品相抗衡。

路径规划是在给定的环境中自动寻找一条从初始位置到目标位置的无碰撞路径的过程，其在智能农机系统、医学、计算机动画、现代工业等众多领域应用广泛。根据对环境信息的掌握程度不同，路径规划可以分为局部路径规划和全局路径规划。全局路径规划算法的研究比较成熟，其中经典的算法是栅格法和A*算法，二者均能快速实现无碰且最短径的规划。在局部路径规划算法中，人工势场法、动态窗口法和Pure-pursuit局部路径规划算法，其中，Pure-pursuit局部路径规划算法由于原理简单、算法清晰，被广泛使用。人工势场法将环境抽象为受力场，目标点产生引力，障碍物产生斥力，农机在引力与斥力的共同作用下运动，该算法结构简单、易于部署，但在狭长的走廊中会出现摆动的现象，且与障碍物距离过近时无法生成路径。动态窗口法则直接对农机的速度进行计算，更符合机器人的控制习惯,该算法充分考虑了机器人的物理限制、环境约束以及当前速度等因素，更加符合当前机器人路径规划的发展趋势，但是由于只生成短时间内的预测路径，因此往往在离障碍物较近的时候才开始避障，整体效果不佳。

由于Pure-pursuit局部路径规划算法操作简单，计算效率高，故本文采用Pure-pursuit局部路径规划器作为农田环境中的智能农机路径规划算法，进而完成自主耕作任务。

经实测可知，由于自然因素，如：车辆打滑、农田崎岖、GPS信号短时间丢失等，会导致农机产生路径偏移。在这种情况下传统Pure-pursuit局部路径规划算法会要求农机首先要回到偏移之前的初始点，这样会导致农机的随机搜索甚至直接停止，大大降低了实时性和稳定性。

发明内容

为了解决农机产生路径偏移时，传统Pure-pursuit局部路径规划可能导致农机的随机搜索甚至直接停止，从而降低农机实时性和稳定性的问题，本发明采用了粒子群算法对Pure-pursuit局部路径规划算法进行优化。

本发明所采取的技术方案是：一种基于智能农机的快速偏移矫正的路径规划方法，由PSO算法模块、自适应权重模块和Pure-pursuit局部路径规划算法模块组成的。本发明引入了PSO算法，并改进了其中的自适应权重计算方法，具体包括以下步骤：

1)实时计算农机当前位置与实际规定路线位置的偏移误差Error；

2)若偏移误差Error小于误差阈值则继续行驶，若偏移误差Error大于误差阈值则进行步骤3)；

3)调用PSO算法，寻找目标矫正导航点，用于快速纠偏，具体包括：

3.1)粒子初始化：在当前农机发生偏移的转弯半径范围内随机撒点，每一个点对应一个粒子的初始位置，每个粒子代表一个农机跟踪的潜在目标位置；