林地路径规划与抗扰动自适应轨迹跟踪方法及系统

本发明涉及导航技术，尤其是一种林地路径规划与抗扰动自适应轨迹跟踪方法及系统。

背景技术：

1、现如今，林业智能化已经是大势所趋，作为智能化的执行者，林业机器人发挥着重要作用。路径规划与轨迹跟踪是移动机器人领域的核心问题。

2、目前，在路径规划领域，基于智能群优化的路径规划算法有蚁群算法，粒子群算法，狼群算法等；基于栅格地图的优化算法有a*，dijkstra等；基于快速采样的算法有rrt。基于智能群优化的方法效率低下且不完备，基于快速采样的算法不考虑运动学模型，生成路径质量低。基于栅格地图的算法速度相对较快且完备，适用于实际生产过程，但是现有算法仍然存在只考虑单一优化指标，没有考虑林地现场作业条件的问题。

3、在轨迹跟踪领域，目前主流为mpc控制，但是mpc控制传统依赖精确建模，未能考虑林区长期颠簸工况下，前轮传向机构出现的中心角偏移的问题，且不考虑扰动补偿，导致跟踪时出现震荡，漂移的问题。

技术实现思路

1、本发明的目的在于提供一种林地路径规划与抗扰动自适应轨迹跟踪方法及系统，以解决现有技术存在的上述问题。

2、实现本发明目的的技术解决方案为：

3、根据本技术的一个方面，提供一种林地路径规划与抗扰动自适应轨迹跟踪方法，包括：

4、s1、根据林区常见路面情况以及对移动平台的影响，使用[0，1]之间的系数μ衡量通过该区域的难易程度；

5、s2、通过系数μ的论域生成7个模糊集合成员vs，s，ss，m，sl，l，vl；地形坡度θ的论域生成7个模糊集合成员nl，nm，ns，zo，ps，pm，pl；通过系数μ和地形坡度θ间的模糊规则生成路径模糊代价λ；定义边界惩罚因子ρv；构造dqn网络，定义dqn网络打分为q(s，a)，其中s为当前车辆周围环境，位置与朝向，a为dqn网络在当前状态下给出的动作，q(s，a)为当前状态与动作下的预期受益；

6、s3、构建a算法的代价函数g，构建a算法的启发函数h，由当前点与终点的六边形栅格地图坐标的距离与dqn网络打分q(s，a)融合得到，构建a算法的总体代价f=g+h，利用f，g与h进行路径规划，得到最优路径；

7、s4、构建小车抗扰动失配角补偿运动学模型，状态量包括t时刻的横纵坐标偏移和角度偏移，模型输入量包括t时刻的参考转角和失配角，以及当前采样时间、线速度和角速度；

8、s5、构建路面自适应mpc控制系统，包括前馈与反馈两部分，前馈部分基于参考状态与实际状态的误差进行自适应mpc控制，控制量输出给扰动观测器和线控底盘，反馈部分将实际状态量与mpc预测状态量送入rbf扰动观测器，将观察到的静差视作前轮失配误差用以修正模型，将扰动分离并与其余状态量一并反馈mpc；设计模糊权重决策器，通过当前扰动与路面自适应车速模块产生的参考速度，选择当前mpc优化目标的权重矩阵；

9、s6、构建mpc的优化目标函数，包含状态跟踪误差和控制量变化两项，由权重矩阵q和r决定其比重；使用路面通过系数μ与当前小车速度v之间的模糊规则来调节权重矩阵q和r的比值；

10、s7、将规划出的路径作为参考路径进行轨迹跟踪。

11、根据本技术的一个方面，步骤s1进一步为：

12、步骤s11、划分路面类型，包括干燥带，潮湿带，落叶带，多根带和沟壑带；

13、步骤s12、针对每种路面类型，根据其对移动平台的影响，给定系数μ，μ∈（0，1）。

14、根据本技术的一个方面，步骤s2进一步为：

15、步骤s21、将系数μ的论域设置为[0，1]，根据论域生成模糊集合，模糊集合成员包括vs，s，ss，m，sl，l，vl；分别代表通过系数非常小，小，较小，适中，大，非常大；

16、步骤s22、将地形坡度θ的论域设置为[-π/4，π/4]，模糊集合成员包括nl，nm，ns，zo，ps，pm，pl，分别代表坡度负大，负中，负小，零，正小，正中，正大；

17、步骤s23、根据系数μ和地形坡度θ构建模糊规则表，生成路径模糊代价；

18、步骤s24、定义边界惩罚因子ρv；

19、

20、ρv（x，y）是（x，y）处的边界势能，α为势能的下降速度，d0（x，y）为节点到达障碍物的距离，dv（x，y）是节点到达维诺地图边的最近距离，d0max是边界势能的半径；

21、ρv（x，y）取值从0到1，当d0＞d0max时，（x，y）势能为0，势能值到达最大的时候是（x，y）在障碍物上或者里面，势能值到达最小的时候是（x，y）在广义泰森多边形的边上；

22、步骤s25、设立移动平台对应的奖励函数，奖励如下：

23、

24、rt为当前节点移动的奖励，μ为步骤s1定义的系数；

25、步骤s26、基于奖励函数构造dqn，dqn的输入是平台周围10*10的感受野，输出为平台向周围六个节点移动的评价函数。dqn的td target为：

26、

27、其中y为td target，rt为当前行动的奖励，γ为衰减率，q(st+1,a;w)为dqn在行动后的下一个状态下的打分，st+1为下一个时刻的状态，w为当前dqn模型参数，a为dqn得到的动作；

28、利用td target对dqn进行训练，训练结束后，dqn可对当前移动平台的6个方向进行打分，记作q(s，a)。

29、根据本技术的一个方面，步骤s3进一步为：

30、步骤s31、构建代价函数g；；

31、λ为路径长度，ρv为边界因子，n为边界增益系数，l为系数；

32、步骤s32、构建启发函数h；

33、；

34、其中q、s、r为当前点的六边形栅格地图坐标，qe、se、re为终点的六边形栅格地图坐标，ζ为栅格边长；q(s，a)为dqn输出的全局启发值，k为坐标距离与dqn的融合系数。

35、步骤s33、构建总体代价f=g+h进行路径规划。

36、根据本技术的一个方面，步骤s33进一步为：

37、步骤s331、对六边形栅格地图，障碍进行初始化，确定起点与终点的位置；

38、步骤s332、建立待搜索列表与已搜索列表，对待搜索列表进行初始化，将起点加入已搜索列表；

39、步骤s333、开始搜索，遍历待搜索列表，选择待搜索列表中f最小的节点，将该节点移出待搜索列表，放入已搜索列表；

40、步骤s334、将该节点设置为父节点，筛选与障碍不碰撞的子节点；

41、步骤s335、遍历筛选出来的子节点，分别计算每个节点对应的模糊代价，dqn网络打分，边界惩罚因子，构造代价值g与启发值h，g+h得到总体代价，作为总体代价f；

42、步骤s336、判断当前子节点是否已经在待搜索列表中：

43、如果当前节点已经在待搜索列表中，对节点的总体代价f进行比较：

44、如果该节点新计算的总体代价比原先计算的f更低，则分别更新该节点对应的g，h与f，并且更新该节点所属的父节点；如果相等或更高，则不更新；

45、如果当前节点不在待搜索列表中，则将该节点的g， h与f保存在待搜索列表中，并且更新该节点所属的父节点；

46、步骤s337、判断该节点是否为目标节点；如果是目标节点，则进入步骤s338，如果否，则跳转到步骤s333继续执行；

47、步骤s338、输出从起点到终点的路径。

48、根据本技术的一个方面，步骤s4进一步为：

49、步骤s41、构建小车抗扰动失配角补偿运动学模型

50、；

51、

52、；；；

53、其中，为系统状态矩阵，为系统输入矩阵，为系统时刻的状态量，u(k)为系统k时刻的输入量，，与分别为系统在k时刻的横纵坐标偏移与角度偏移，为k时刻的角度扰动，θr（k）与dr分别为k时刻的参考角度与失配角，vr为k时刻的参考速度，ts为当前采样时间，v(k)与w(k)为当前车辆线速度与角速度。

54、根据本技术的一个方面，步骤s5进一步为：

55、步骤s51、构建前馈部分，根据车辆参考状态与实际状态的误差进行自适应mpc控制，在进行滚动优化与最小二次型求解之后，将求解出的控制量输出给扰动观测器，同时将控制量输出给线控底盘；其中，扰动观测器采用rbf状态观测器；

56、步骤s52、构建反馈部分，将当前实际状态量与mpc预测的状态量送入rbf扰动观测器，将观察到的静差视作前轮失配产生的误差用以修正模型，将扰动分离并与其余状态量一并反馈mpc控制器；

57、步骤s53、模糊权重决策器通过当前扰动与路面自适应车速模块产生的参考速度，选择当前优化目标的权重矩阵。

58、根据本技术的一个方面，所述步骤s6进一步为：

59、步骤s61、构建mpc的优化目标函数，包含状态跟踪误差和控制量变化两项，由权重矩阵q和r决定其比重；；其中与为优化目标的权重矩阵，与为当前时刻到时刻的状态量与控制量，为优化目标；

60、步骤s62、使用路面通过系数与当前小车速度之间的模糊规则来调节权重矩阵q和r的比值；通过控制权重矩阵的比值，调节系统的动态指标。

61、根据本技术的一个方面，所述步骤s62进一步为：

62、步骤s621、小车速度v的论域为[0，10m/s]，生成7个模糊集合成员：vs，s，ss，m，sl，l，vl；

63、步骤s622、权重比r/q的论域为[0，10]，生成3个模糊集合成员：s，m，l，分别代表更看重收敛速度，平衡，更看重平顺度；

64、步骤s623、根据路面情况和速度v，通过模糊规则推理得到合适的r/q值，动态调整mpc性能。

65、根据本技术的一个方面，还包括步骤s63，具体为：

66、步骤s631. 采用动态tsk模糊系统，使用神经网络拟合结论部分的参数；

67、前提部分使用λ与v的模糊隶属度：λ={vs，s，ss，m，sl，l，vl}；v={vs，s，ss，m，sl，l，vl}；结论部分改为关于λ和v的函数r/q = f(λ，v)= a0+ a1λ+ a2v + a3λ·v + b；其中，a0，a1，a2，a3为输出部分各个线性项的系数，b为偏移常数；

68、步骤s632、通过梯度下降优化算法训练函数参数{a0，a1，a2，a3，b}，以拟合离线采集的专家操控数据；

69、步骤s633、通过在线滚动更新模型，持续学习新采集的人机交互数据，提高决策器的适应能力。

70、本发明还提供一种林地路径规划与抗扰动自适应轨迹跟踪系统，用于实现上述的方法，系统包括：

71、第一模块，用于根据林区常见路面情况以及对移动平台的影响，使用[0，1]之间的系数μ衡量通过该区域的难易程度；

72、第二模块，通过系数μ的论域生成7个模糊集合成员vs，s，ss，m，sl，l，vl；地形坡度θ的论域生成7个模糊集合成员nl，nm，ns，zo，ps，pm，pl；通过系数μ和地形坡度θ间的模糊规则生成路径模糊代价λ；定义边界惩罚因子ρv；构造dqn网络，定义dqn网络打分为q(s，a)，其中s为当前车辆周围环境，位置与朝向，a为dqn网络在当前状态下给出的动作，q(s，a)为当前状态与动作下的预期受益；

73、第三模块，用于构建a算法的代价函数g，构建a算法的启发函数h，由当前点与终点的六边形栅格地图坐标的距离与dqn网络打分q(s，a)融合得到，构建a算法的总体代价f=g+h，利用f，g与h进行路径规划，得到最优路径；

74、第四模块，用于构建小车抗扰动失配角补偿运动学模型，状态量包括t时刻的横纵坐标偏移和角度偏移，模型输入量包括t时刻的参考转角和失配角，以及当前采样时间、线速度和角速度；

75、第五模块，用于构建路面自适应mpc控制系统，包括前馈与反馈两部分，前馈部分基于参考状态与实际状态的误差进行自适应mpc控制，控制量输出给扰动观测器和线控底盘，反馈部分将实际状态量与mpc预测状态量送入rbf扰动观测器，将观察到的静差视作前轮失配误差用以修正模型，将扰动分离并与其余状态量一并反馈mpc；设计模糊权重决策器，通过当前扰动与路面自适应车速模块产生的参考速度，选择当前mpc优化目标的权重矩阵；

76、第六模块，用于构建mpc的优化目标函数，包含状态跟踪误差和控制量变化两项，由权重矩阵q和r决定其比重；使用路面通过系数μ与当前小车速度v之间的模糊规则来调节权重矩阵q和r的比值；

77、第七模块，用于将规划出的路径作为参考路径进行轨迹跟踪。

78、与现有技术相比，本发明的有益效果为：本发明通过引入通过系数与模糊代价，综合考虑了不同路面对路径整体质量的影响，提高了实际应用中生成路径的质量，且具备良好的可扩展性；通过设计抗扰动跟踪控制系统，实现了静态角误差的自补偿，解决了林地长期颠簸工况下前轮中心角偏移的问题；通过设计路面自适应模糊控制器，实现了对不同路面与行驶速度下的动态特性自适应控制。