一种基于引导采样的轨迹规划方法、系统、终端及介质

本发明涉及无人机轨迹设计，具体为一种基于引导采样的轨迹规划方法、系统、终端及介质。

背景技术：

1、在已知复杂环境中，由于目标位置的改变，导致多旋翼无人机需重新规划轨迹，即，多旋翼无人机必须实时、高效地生成新的全局轨迹，以适应目标变化所带来的导航调整需求。采样类方法因其对解空间的高效探索与开发而适合于状态空间维度较高的全局最优规划。而现有的基于采样的轨迹规划算法采取的随机均匀采样的方式，求解效率较低，还不能满足实时性要求。因此有必要探索高效的非均匀采样方法，以实现更快速、更可靠的全局轨迹规划，从而满足复杂环境中目标位置改变导致的实时调整需求。

2、在采样方面的研究主要分为重要性采样和自适应采样。重要性采样基于某一特定任务状态空间中各部分的不同重要性程度。oleynikova等人和blochliger等人对无人机的可行空间的中轴路径骨架进行近似提取，而ye等人则根据无人机速度方向的特征进行采样。其他工作则利用机器学习提取高维特征，尝试开发通用的方法获取采样的概率分布。自适应采样更能应对状态空间中各部分重要性程度变化的情况。gammell等人提出了informed-rrt*(informed rapidly exploring randomtree)，利用高维椭球约束提升解质量的采样，与均匀采样后拒绝采样相比大大提高了效率，但仅适用于最短路径问题。bit*、ait*(adaptively informed trees)等算法也利用此特性进行自适应采样。但对于其它优化目标，仍然难以直接获得有效的状态空间采样分布。

3、然而，综上可知，尽管一些研究通过利用高维椭球约束来提升解的质量，相较于均匀采样后拒绝采样的方法在效率上取得了显著提升，但这些方法仅适用于最短路径问题，而在轨迹规划问题上并不具备适用性。

技术实现思路

1、为了克服上述现有技术存在的缺陷，本发明的目的在于提供一种基于引导采样的轨迹规划方法、系统、终端及介质，以解决现有技术中无法对无人机轨迹规划优化的技术问题。

2、本发明是通过以下技术方案来实现：

3、第一方面，本发明提供了一种基于引导采样的轨迹规划方法，包括：

4、引导路径生成连接初始位置与目标位置的路径；

5、根据生成所述路径进行引导采样，得到连接初始状态以及目标状态的轨迹；

6、对所述轨迹进行优化，得到最终轨迹，完成基于引导采样的轨迹规划。

7、优选的，连接初始位置与目标位置的路径的生成过程如下：

8、s1，初始化一棵以初始状态为根节点的起点搜索树以及以目标状态为根节点的终点搜索树；

9、s2，判断是否获得初始解，当没有获得初始解时，采用随机均匀采样策略进行配置空间内的采样；反之则根据当前解的代价定义一个启发式采样空间，在该空间内进行随机均匀采样，并且随着解质量的提高，该启发式采样空间逐渐减小；

10、s3，分别在两棵搜索树中寻找最优的候选父节点，根据随机采样点通过候选父节点连接到初始状态或目标状态的代价来选择搜索树进行生长；

11、s4，新添加的状态节点和边带来了新的信息，若经由新添加的状态节点到达其某些近邻节点而使得这些节点的累积代价减小，且新构建的轨迹边满足约束，则将这些节点的父节点改变为新添加的状态节点；

12、s5，更新最优代价；

13、s6，重复执行s2-s5，得到连接初始状态以及目标状态的路径。

14、优选的，s3中，分别在两棵搜索树中寻找最优的候选父节点的具体过程如下：

15、如果随机采样点通过起点搜索树中的候选父节点连接到初始状态的代价小于通过终点搜索树中的候选父节点连接到目标状态的代价，则根据随机采样点连接到起点搜索树中；反之亦然。

16、优选的，连接初始状态以及目标状态的轨迹的具体过程如下：

17、l1，将初始状态添加到轨迹树中作为树的根节点，此时树中只有一个节点，没有轨迹边；

18、l2，随机采样上一节得到的引导路径上的一点，然后再在以该点为圆心、r为半径且垂直于该处引导路径的圆内随机采样一个位置点，得到新的节点；

19、l3，从邻近的节点集合中选择一个节点作为新采样节点的父节点，选择父节点的过程中需要考虑动态规划最优性准则，即所选的父节点不仅要使得新节点与父节点之间的轨迹边满足约束，并且使得从初始节点经由父节点到达新节点的累计代价值最小；

20、l4，当向轨迹树结构中添加一个新节点时，如果该新节点与目标状态的距离小于一定距离，则尝试以这个新节点的状态和目标状态作为边值条件求解一条轨迹，求解后得的轨迹满足约束条件，得到一条连接初始状态和目标状态的可行轨迹；

21、l5，经由新节点到达其近邻节点集合中的节点而使得这些节点的累积代价值减小，并且新构建的轨迹边满足约束，则将这些节点的父节点确定为新节点；

22、l6，重复执行l2至l5，得到一条连接初始状态以及目标状态的初始轨迹。

23、优选的，对所述轨迹优化过程如下：

24、k1，设定每个维度的多项式轨迹，并以前端轨迹生成的多项式系数作为初值，优化构件生成后端目标函数；

25、k2，根据后端目标函数将待优化的轨迹在位置上靠近初始轨迹，并在在同一个路径拓扑下的无障碍物区域得到最终轨迹。

26、进一步的，k1中，每个维度的多项式轨迹中，轨迹表达式如下：

27、

28、其中是第i段的系数，是时间基底，ti是该段的时长。

29、进一步的，k1中，所述后端目标函数的构建表达式如下：

30、

31、

32、其中是m段多项式的系数向量，js为整体平滑性的代价，jh为惩罚与初始轨迹路径的差异的避障代价，λs>0,λh>0为各惩罚项的相应权重；优化变量为c。

33、第二方面，本发明还提供了一种基于引导采样的轨迹规划系统，其特征在于，包括：

34、路径生成模块，用于引导路径生成连接初始位置与目标位置的路径；

35、引导采样模块，用于根据生成所述路径进行引导采样，得到连接初始状态以及目标状态的轨迹；

36、轨迹优化模块，用于对所述轨迹进行优化，得到最终轨迹，完成基于引导采样的轨迹规划。

37、第三方面，本发明还提供了一种移动终端，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述所述基于引导采样的轨迹规划方法的步骤。

38、第四方面，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述所述基于引导采样的轨迹规划方法的步骤。

39、与现有技术相比，本发明具有以下有益的技术效果：

40、本发明提供了一种基于引导采样的轨迹规划方法、系统、终端及介质，通过引导路径生成一条连接初始位置与目标位置的路径，利用kinodynamic rrt*算法，并根据得到的路径进行引导采样，得到一条连接初始状态以及目标状态的轨迹，对得到的轨迹进行优化，得到最终的轨迹，本发明改进了kinodynamic rrt*算法的采样策略，在采样时利用引导路径进行采样，以减小采样空间，加速算法收敛。在测试中，基于引导的采样策略可以在几毫秒的时间内找到一条初始轨迹，而随机均匀采样策略则需约500ms左右的时间找到一条初始轨迹，并且轨迹代价远大于基于引导的采样策略求解的到的初始轨迹。随着采样时间的增加，两种采样策略得到的最优轨迹代价均降低，但是在相同时间点，基于引导的采样策略得到的最优轨迹代价均小于随机均匀采样策略，有效的对无人机轨迹规划进行了优化。