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

权利要求书

1.一种基于智能农机的快速偏移矫正的路径规划方法，其特征在于：引入了PSO算法，并改进了其中的自适应权重计算方法，具体包括以下步骤：

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

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

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

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

3.2)计算用于位置更新的自适应权重，所述的自适应权重用于在农机路径偏移时使农机更快速的进行纠偏，具体公式为：

其中，T_max是最大迭代次数，t是当前迭代次数，w_max∈(0，1)；

3.3)更新所有粒子的位置和速度：不断寻找当前迭代对应的每个粒子的局部最优解，以及当前迭代对应的所有粒子的全局最优值，进一步根据更新公式得到更新后的位置和速度，其中，所述的全局最优值为：与原路径规划中的农机目标跟踪点的欧式距离最近的粒子；所述的第i个粒子的局部最优解为：对于当前迭代之前的所有迭代，在每次迭代的过程中，记录本次迭代时第i个粒子与原路径规划中的农机目标跟踪点的欧式距离，第i个粒子的局部最优解即为欧式距离最短的粒子；

3.4)计算更新后的位置与原路径规划中的农机目标跟踪点之间的欧式距离，如果满足迭代精度，则迭代结束，继续下一步；否则就返回步骤3.2)继续迭代，寻找全局最优值；

4)将迭代完成的全局最优粒子作为Pure-pursuit局部路径规划算法的目标点即农机最终跟踪的目标位置，并将农机朝向目标点旋转；

5)驶向目标点，并在到达目标点后向全局规划给定的最终位姿变换。

2.根据权利要求1所述一种基于智能农机的快速偏移矫正的路径规划方

法，其特征在于：粒子的更新由以下方程式完成：

其中，l是迭代次数，w是本发明提出的自适应权重系数，c₁是个体学习因子，c₂是社会学习因子，r₁和r₂是0和1之间的均匀分布随机变量，是经过第l次迭代之后第i个粒子的局部最优解，是经过第l次迭代之后所有粒子的全局最优解，是经过第l次迭代之后第i个粒子的速度，是经过第l次迭代之后第i个粒子的位置，是经过第l+1次迭代之后第i个粒子的位置。