一种基于智能农机的快速偏移矫正的路径规划方法与流程

1.本发明涉及一种基于智能农机的快速偏移矫正的路径规划方法，特别是在车辆打滑、农田崎岖、gps信号短时间丢失等情况下发挥作用的方法，属于智能农机装备领域。
技术背景
2.智能农机装备是提质增效、转变发展方式、增强农业综合生产能力的基础，智能农机的自动避障路径规划问题是智能车辆研究领域的热点之一。在国外，约翰迪尔，爱科、凯斯等知名农机厂商在此方面已取得长足发展。然而在国内，拖拉机信息化、智能化水平低下，还无法与国外产品相抗衡。
3.路径规划是在给定的环境中自动寻找一条从初始位置到目标位置的无碰撞路径的过程，其在智能农机系统、医学、计算机动画、现代工业等众多领域应用广泛。根据对环境信息的掌握程度不同，路径规划可以分为局部路径规划和全局路径规划。全局路径规划算法的研究比较成熟，其中经典的算法是栅格法和a*算法，二者均能快速实现无碰且最短径的规划。在局部路径规划算法中，人工势场法、动态窗口法和pure-pursuit局部路径规划算法，其中，pure-pursuit局部路径规划算法由于原理简单、算法清晰，被广泛使用。人工势场法将环境抽象为受力场，目标点产生引力，障碍物产生斥力，农机在引力与斥力的共同作用下运动，该算法结构简单、易于部署，但在狭长的走廊中会出现摆动的现象，且与障碍物距离过近时无法生成路径。动态窗口法则直接对农机的速度进行计算，更符合机器人的控制习惯,该算法充分考虑了机器人的物理限制、环境约束以及当前速度等因素，更加符合当前机器人路径规划的发展趋势，但是由于只生成短时间内的预测路径，因此往往在离障碍物较近的时候才开始避障，整体效果不佳。
4.由于pure-pursuit局部路径规划算法操作简单，计算效率高，故本文采用pure-pursuit局部路径规划器作为农田环境中的智能农机路径规划算法，进而完成自主耕作任务。
5.经实测可知，由于自然因素，如：车辆打滑、农田崎岖、gps信号短时间丢失等，会导致农机产生路径偏移。在这种情况下传统pure-pursuit局部路径规划算法会要求农机首先要回到偏移之前的初始点，这样会导致农机的随机搜索甚至直接停止，大大降低了实时性和稳定性。


技术实现要素：

6.为了解决农机产生路径偏移时，传统pure-pursuit局部路径规划可能导致农机的随机搜索甚至直接停止，从而降低农机实时性和稳定性的问题，本发明采用了粒子群算法对pure-pursuit局部路径规划算法进行优化。
7.本发明所采取的技术方案是：一种基于智能农机的快速偏移矫正的路径规划方法，由pso算法模块、自适应权重模块和pure-pursuit局部路径规划算法模块组成的。本发明引入了pso算法，并改进了其中的自适应权重计算方法，具体包括以下步骤：
8.1)实时计算农机当前位置与实际规定路线位置的偏移误差error；
9.2)若偏移误差error小于误差阈值则继续行驶，若偏移误差error大于误差阈值则进行步骤3)；
10.3)调用pso算法，寻找目标矫正导航点，用于快速纠偏，具体包括：
11.3.1)粒子初始化：在当前农机发生偏移的转弯半径范围内随机撒点，每一个点对应一个粒子的初始位置，每个粒子代表一个农机跟踪的潜在目标位置；
12.3.2)计算用于位置更新的自适应权重，所述的自适应权重用于在农机路径偏移时使农机更快速的进行纠偏，具体公式为：
[0013][0014]
其中，t
max
是最大迭代次数，t是当前迭代次数，w
max
∈(0，1)；
[0015]
3.3)更新所有粒子的位置和速度：不断寻找当前迭代对应的每个粒子的局部最优解，以及当前迭代对应的所有粒子的全局最优值，进一步根据更新公式得到更新后的位置和速度，其中，所述的全局最优值为：与原路径规划中的农机目标跟踪点的欧式距离最近的粒子；所述的第i个粒子的局部最优解为：对于当前迭代之前的所有迭代，在每次迭代的过程中，记录本次迭代时第i个粒子与原路径规划中的农机目标跟踪点的欧式距离，第i个粒子的局部最优解即为欧式距离最短的粒子；
[0016]
3.4)计算更新后的位置与原路径规划中的农机目标跟踪点之间的欧式距离，如果满足迭代精度，则迭代结束，继续下一步；否则就返回步骤3.2)继续迭代，寻找全局最优值；
[0017]
4)将迭代完成的全局最优粒子作为pure-pursuit局部路径规划算法的目标点即农机最终跟踪的目标位置，并将农机朝向目标点旋转；
[0018]
5)驶向目标点，并在到达目标点后向全局规划给定的最终位姿变换。
[0019]
进一步的，粒子的更新由以下方程式完成：
[0020][0021][0022]
其中，l是迭代次数，w是本发明提出的自适应权重系数，c1是个体学习因子，c2是社会学习因子，r1和r2是0和1之间的均匀分布随机变量，是经过第l次迭代之后第i个粒子的局部最优解，是经过第l次迭代之后所有粒子的全局最优解，是经过第l次迭代之后第i个粒子的速度，是经过第l次迭代之后第i个粒子的位置，是经过第l 1次迭代之后第i个粒子的位置。
[0023]
其中，所述农机型号为：任意农机型号；
[0024]
其中，所述实验场景：任意实验场景。
[0025]
有益效果
[0026]
一方面，本发明解决了传统的pure-pursuit局部路径规划算法会使得农机行进路线发生很大偏差，甚至直接停止的问题。在理想状态下，传统的pure-pursuit局部路径规划算法能够发挥出很好的作用。但是在实际测试中需要考虑众多环境因素对农机的影响，如果仍然采用传统的pure-pursuit局部路径规划算法，会使得农机行进路线发生很大偏差，甚至直接停止。
[0027]
另一方面，传统的pso算法在进行粒子速度、位置更新的过程中，权重系数是固定值，导致pso算法的粒子收敛速度慢，严重影响了系统的实时性。本发明采用改进的权重系数加快了粒子收敛速度，提高了系统的实时性，
附图说明
[0028]
图1、本发明方法流程图；
[0029]
图2(a)、在笛卡尔坐标系下，农机翻滚角(roll)误差对比图；
[0030]
图2(b)、在笛卡尔坐标系下，农机俯仰角(pitch)误差对比图；
[0031]
图2(c)、在笛卡尔坐标系下，农机偏航角(yaw)误差对比图；
[0032]
图3、本发明提出算法运行路径与原路径对比图；
[0033]
图4(a)、在y坐标轴视角下，本发明提出的算法与真实路径在x、z坐标下的误差对比图；
[0034]
图4(b)、在z坐标轴视角下，本发明提出的算法与真实路径在x、y坐标下的误差对比图；
[0035]
图4(c)、在x坐标轴视角下，本发明提出的算法与真实路径在y、z坐标下的误差对比图；
[0036]
图5、本发明提出的算法与传统局部路径规划算法误差性能对比图；
[0037]
图6、pso算法初始化粒子个数对农机偏移误差的影响示意图；
具体实施方式
[0038]
粒子群(pso)优化算法是一种有效的启发式算法，它在解决多维非线性优化问题方面的优势使pso算法在现代技术研究方面博得了专家学者的关注。与其他启发式算法相似，pso算法通过鸟类中个体之间的竞争和合作，从而得到最优解。粒子群算法具有全局收敛性强和收敛速度快的优点，可以改进传统pure-pursuit局部路径规划算法在实测情况下路径偏移和效率低的问题。
[0039]
为了克服上述传统pure-pursuit局部路径规划算法以及传统的pso算法存在的缺陷，本发明的目的在于提出了一种基于智能农机的快速偏移矫正的路径规划方法，其流程图如图1所示。
[0040]
本发明所采取的技术方案是：一种基于智能农机的快速偏移矫正的路径规划方法，是由pso算法模块、自适应权重模块和pure-pursuit局部路径规划算法模块组成的。
[0041]
方法具体的实现步骤分为下面六个步骤：
[0042]
1)实时计算农机当前位置与实际规定路线位置的偏移误差error；
[0043]
2)若偏移误差error小于误差阈值则继续行驶，若偏移误差error大于误差阈值则进行步骤3)；
[0044]
3)调用pso算法，寻找目标矫正导航点，用于快速纠偏，具体包括：
[0045]
3.1)粒子初始化：在当前农机发生偏移的转弯半径范围内随机撒点，每一个点对应一个粒子的初始位置，每个粒子代表一个农机跟踪的潜在目标位置；其中，种群规模和最小迭代精度等相关参数由工程经验决定；
[0046]
3.2)计算用于位置更新的自适应权重，所述的自适应权重用于在农机路径偏移时
使农机更快速的进行纠偏，具体公式为：
[0047][0048]
其中，t
max
是最大迭代次数，t是当前迭代次数，w
max
＝0.9；
[0049]
通常情况下，传统pso算法的惯性权重w取0.4-0.9之间的一个固定值，但是这样不利于搜索全局最优值。对于传统pso算法，在搜索的初期，w取值越大，粒子的迭代速度越快；在搜索的后期，w取值越小，粒子越快地收敛到最优值。
[0050]
3.3)更新所有粒子的位置和速度：不断寻找当前迭代对应的每个粒子的局部最优解，以及当前迭代对应的所有粒子的全局最优值，进一步根据更新公式得到更新后的位置和速度，其中，所述的全局最优值为：与原路径规划中的农机目标跟踪点的欧式距离最近的粒子；所述的第i个粒子的局部最优解为：对于当前迭代之前的所有迭代，在每次迭代的过程中，记录本次迭代时第i个粒子与原路径规划中的农机目标跟踪点的欧式距离，第i个粒子的局部最优解即为欧式距离最短的粒子；
[0051]
粒子的更新由以下方程式完成：
[0052][0053][0054]
其中，l是迭代次数，w是本发明提出的自适应权重系数，c1是个体学习因子，c2是社会学习因子，r1和r2是0和1之间的均匀分布随机变量，是经过第l次迭代之后第i个粒子的局部最优解，是经过第l次迭代之后所有粒子的全局最优解，是经过第l次迭代之后第i个粒子的速度，是经过第l次迭代之后第i个粒子的位置，是经过第l 1次迭代之后第i个粒子的位置。
[0055]
其中，通常c1＝c2＝2，第i个粒子的局部最优解的初值即为第i个粒子的初始位置，全局最优值的初值为与原路径规划中的农机目标跟踪点的欧式距离最近的粒子的初始位置。
[0056]
3.4)计算更新后的位置与原路径规划中的农机目标跟踪点之间的欧式距离，如果满足迭代精度，则迭代结束，继续下一步；否则就返回步骤3.2)继续迭代，寻找全局最优值；
[0057]
4)将迭代完成的全局最优粒子作为pure-pursuit局部路径规划算法的目标点即农机最终跟踪的目标位置，并将农机朝向目标点旋转；
[0058]
5)驶向目标点，并在到达目标点后向全局规划给定的最终位姿变换。
[0059]
其中本发明提出的算法在三种情况下与传统pure-pursuit局部路径规划算法进行对比：
[0060]
(1)改进算法与真实路径的局部路径对比，对比结果如图2(a)-图2(c)所示。
[0061]
(2)改进算法与真实路径的总体路径对比，对比结果如图3，图4(a)-图4(c)所示。
[0062]
(3)改进算法与传统pure-pursuit局部路径规划算法的路径误差对比，对比结果如图5所示。
[0063]
(4)pso算法的初始化粒子个数对农机行进中偏移误差的影响对比，对比结果如图6所示。
[0064]
(5)改进算法与传统pure-pursuit局部路径算法的评价指标对比，对比结果如表1所示。
[0065]
根据本发明避免了传统的pure-pursuit局部路径规划算法在车辆打滑、农田崎岖、gps信号短时间丢失等情况下使得农机规划无效路径，大大提高了农机运行精度以及降低了农机的能源消耗。
[0066]
表1
[0067][0068]
以上对本发明的实施方式进行了说明，但该实施方式仅作为一个范例，并不是有限定发明范围的意图。本发明能够通过其他各种形态实施，能够在不超出发明主旨的范围内进行各种各样的变更。