一种动态环境下无人机轨迹规划方法及系统

本发明属于无人机轨迹规划，尤其涉及一种动态环境下无人机轨迹规划方法及系统。

背景技术：

1、近年来，随着感知、预测、规划和控制等领域技术的进步，无人机自主导航得到了快速发展。轨迹规划是无人机自主导航的重要研究方向，它可以在考虑计算时间和避障的情况下生成从初始位置到目标位置的可行轨迹，最后将这条轨迹交给控制器进行跟踪。轨迹规划过程中遇到的障碍物包括静态障碍物和动态障碍物。许多现有的工作仅考虑静态障碍物。动态环境下轨迹规划仍然是自主导航的最大挑战之一。

2、文献[3]zhou b,gao f,wang l,et al.robust and efficient quadrotortrajectory generation for fast autonomous flight[j].ieee robotics andautomation letters,2019,4(4):3529-3536使用改进的hybrid-state a*路径搜索算法生成初始轨迹，大大提高前端路径搜索的速度，但是该方法无法处理动态障碍物，且没考虑地图中的障碍物，在障碍物较多的环境中表现较差。文献[4]tordesillas j,how j p.mader:trajectory planner in multiagent and dynamic environments[j].ieeetransactions on robotics,2021,38(1):463-476提出了octopus search算法进行初始路径搜索，生成能够同时避开静态和动态障碍物的初始轨迹，并使用minvo基函数[5]tordesillas j,how j p.minvo basis:finding simplexes with minimum volumeenclosing polynomial curves[j].computer-aided design,2022,151:103341来获取轨迹的凸包，在无人机的轨迹凸包和运动物体之间添加分离平面约束以避免碰撞。该方法解决了动态障碍物避障问题，但是计算时间较长，不能满足在线轨迹规划对实时性的要求。文献[6]zhou x,zhu j,zhou h,et al.ego-swarm:a fully autonomous and decentralizedquadrotor swarm systemin cluttered environments[c]//2021ieee internationalconference on robotics and automation(icra).ieee,2021:4101-4107提出了一种完全基于梯度的轨迹优化方法，并使用拓扑规划生成多条候选轨迹。该方法计算速度较快，但是生成的轨迹较长，不能保证轨迹的最优性，且通过点离散化轨迹以施加避障约束，无法保证整条连续轨迹的安全性。

3、通过上述分析，现有技术存在的问题及缺陷为：(1)为应对动态场景下环境的不断变化，无人机需要一套快速的轨迹重规划方法，现有的技术在规划速度上无法满足实时快速规划的要求。(2)现有的方法为了保证轨迹的安全性生成过于保守的轨迹，导致过长的飞行距离或过长的飞行时间。(3)现有的基于规则的方法(如orca[7]van den berg j,guy sj,lin m,et al.reciprocal n-body collision avoidance[c]//robotics research:the14th international symposium isrr.berlin,heidelberg:springer berlinheidelberg,2011:3-19.)和硬约束的方法(如mader)不能保证求解成功率。

技术实现思路

1、针对现有技术存在的问题，本发明提供了一种动态环境下无人机轨迹规划方法及系统。

2、本发明是这样实现的，一种动态环境下无人机轨迹规划方法，所述动态环境下无人机轨迹规划方法包括前端路径搜索和后端轨迹规划，前端路径搜索部分基于hybrid-state a*算法，提出了oha*算法，在路径搜索中考虑环境中障碍物信息；后端轨迹规划部分使用minvo基函数生成轨迹凸包，并通过以最大化本无人机与动态障碍物的轨迹凸包之间分离程度为目标，通过求解优化问题得到最优分离平面，作为避障约束；最后将轨迹优化中的硬约束转换为软约束，用梯度下降的方法求解优化问题。

3、所述动态环境下无人机轨迹规划方法包括以下步骤：

4、步骤一，无人机调用oha*算法生成初始路径；

5、步骤二，将oha*算法得到的分段多项式路径转换为r阶b样条轨迹；

6、步骤三，结合地图为b样条轨迹的每一段生成安全飞行走廊，作为静态障碍物避障约束；结合动态障碍物的轨迹(来自预测模块)和无人机自身的b样条轨迹，生成避障的分离平面约束，作为动态障碍物避障约束；

7、步骤四，求解轨迹优化问题；

8、步骤五，将求解出的轨迹交给底层控制器进行执行。

9、进一步，步骤一生成初始路径的具体方法包括以下步骤：

10、步骤1，初始化保存待扩展节点的优先队列openset和保存已经扩展了的节点的集合closeset；

11、步骤2，判断openset是否为空，若为空则未搜索到可行路径，程序结束，返回失败；若不为空则从openset中取出队首节点nc，并加入closeset中；

12、步骤3，判断节点nc是否到达终点，若到达则转步骤7，否则继续执行；

13、步骤4，以节点nc生成运动元语集合；

14、步骤5，循环中每个运动元语ni；

15、步骤6，检查运动元语ni是否可行，若速度超过动力学约束，或与障碍物发生碰撞，或其在closeset中，则跳过该元语；如果运动元语ni不在openset中，则将其加入openset；如果运动元语ni在openset中，但是当前的g值降低了，则用当前运动元语替换openset中的ni，回到步骤2；

16、步骤7，取出搜索到的路径，程序结束，返回成功。

17、进一步，步骤1优先队列openset的排序依据是节点的代价函数f，f＝g+h，式中，g表示起点到当前节点的累计代价，h表示从当前节点到终点的估计代价。

18、进一步，步骤4运动元语通过采样加速度获得；

19、假设每个维度的加速度范围为[-amax，amax]，对加速度做2l+1个采样得到加速度采样值对三维加速度进行采样得到(2l+1)3个不同的加速度值，对每个加速度采样可以使用状态转移方程得到邻居节点状态，状态转移方程为：

20、si＝asc+bai

21、

22、

23、式中，sc和si分别表示当前节点nc和第i∈{1,2,…,(2l+1)3}个运动元语对应的状态，状态是一个6维向量，包括3维位置和3维速度；ai是第i个加速度采样，是个3维向量；i3表示3×3的单位矩阵；τ表示前向积分时间；nodes中运动元语的数量也为(2k+1)3。

24、进一步，步骤5首先计算g值为nc的g值加上nc到ni的代价，nc到ni的代价的计算公式如下式：

25、g(si)＝∫0t||a||2dt+ρt.

26、然后计算h值，使用obstacle-aware heuristic，以最小化代价函数为目标计算出一条从节点ni的位置pi到终点位置pg的多项式轨迹，该轨迹有解析解如下式：

27、

28、

29、

30、式中pk0,vk0分别表示节点ni对应的位置和速度，pke即为pg；

31、最优轨迹所对应的代价是不考虑障碍物信息的原始启发式代价，公式为：

32、

33、考虑到障碍物信息后，定义轨迹的碰撞比例为最优轨迹中穿过障碍物的长度与总轨迹的长度的比值，

34、

35、将于障碍物碰撞的那一段的代价函数值进行膨胀，则得到obstacle-awareheuristic：

36、h＝(1-α)j*+i·αj*

37、式中i>1为人为定义的膨胀系数。

38、进一步，步骤三生成安全飞行走廊，对分段轨迹，取每一段的起点和终点作线段相连，以线段为中心扩展一个长方体作为初始飞行走廊，检查该长方体的六个面是否包括了地图中静态障碍物，若某个面包含了则将该面进行收缩到不包含静态障碍物为止；使用kd树查找的技术手段加速操作。

39、进一步，步骤三生成分离平面约束，首先对无人机自身以及动态障碍物的轨迹分别用minvo基函数获得最小体积凸包，minvo基函数与多项式轨迹的系数之间存在转换关系c＝qmvamv.，式中c表示多项式轨迹的系数向量，qmv表示minvo基的控制点，即曲线最小凸包的顶点集，amv表示转换矩阵；

40、获得曲线的最小体积凸包后，通过求解优化问题得到最优分离平面，优化问题如下式：

41、

42、

43、

44、式中，qmv和uk分别表示无人机自身轨迹的凸包和第k个动态障碍物的轨迹凸包，优化变量为分离平面的参数，加粗的1表示全1的向量；该模型在保证uk位于分离平面大于0的一侧的条件下，让qmv尽多地小于0，求解出的分离平面能最大化两个凸包顶点集的分离程度。

45、进一步，步骤四优化目标变量qbs是b样条曲线的控制点，而p(t)是由控制点决定的连续曲线；

46、

47、s.t.p[2](0)＝s0,p[2](t)＝sf，  (28b)

48、

49、

50、

51、

52、优化目标函数(28a)旨在最小化曲线的三阶导，即加加速度jerk；(28b)表示0-2阶导数，即位置、速度、加速度的初始条件和终止条件；(28c)和(28d)表示速度和加速度的动力学限制；(28e)和(28f)分别表示飞行走廊约束和动态避障约束，其中j表示分段轨迹的第j段，m个控制点的r阶b样条曲线的段数为m-r；

53、将此带硬约束的优化问题转换为软约束优化问题如下式：

54、

55、其中js定义如(28a)所示，jv,ja,jf,jd的定义如下式：

56、

57、

58、

59、

60、本发明的另一目的在于提供一种动态环境下无人机轨迹规划方法的动态环境下无人机轨迹规划系统，该系统包括：

61、传感器模块，用于感知周围环境的障碍物和地形信息；

62、建图模块，用于将传感器模块获取的数据进行处理和分析，生成地图；

63、动态障碍物预测模块，用于预测动态障碍物的轨迹；

64、轨迹规划器模块，用于规划无人机飞行轨迹；

65、轨迹执行模块，用于执行规划的轨迹。

66、结合上述的技术方案和解决的技术问题，本发明所要保护的技术方案所具备的优点及积极效果为：

67、第一，本发明提出了一套完整的无人机动态环境轨迹规划方法，其包括前端路径搜索和后端轨迹规划，前端路径搜索上的基本思想是hybrid-state a*算法估计某一中间点到目标点的启发式代价时，将环境地图中的障碍物纳入考虑，对启发式代价做出更准确地估计，能够减少路径搜索过程中大量不必要的节点的扩展，加快搜索算法速度。后端轨迹规划上通过最优分离平面，将无人机与运动障碍物轨迹的最小体积凸包分隔开来，凸包保证了整条连续轨迹的安全性，最小体积凸包和最优分离平面保证了最大的求解空间，能避免过于保守的轨迹。最后将轨迹优化中的硬约束转换为软约束，用梯度下降的方法求解优化问题，加快了轨迹优化的求解速度，且保证了求解成功率。

68、第二，把技术方案看做一个整体或者从产品的角度，本发明所要保护的技术方案具备的技术效果和优点，具体描述如下：

69、1)前端路径搜索能够感知环境中的障碍物信息，减少无效节点的扩展，加快路径搜索算法的计算速度。

70、2)后端轨迹规划使用minvo基函数获得轨迹最小体积凸包，并基于优化问题获得最优分离平面，能解决规划出的轨迹过于保守的问题。

71、3)基于软约束的轨迹优化算法，加快求解速度且增加求解成功率。

72、第三，本发明的技术方案是否解决了人们一直渴望解决、但始终未能获得成功的技术难题：本发明实现了稳定高效的动态环境下无人机轨迹规划系统，为动态环境下无人机自主导航提供了可能性。

73、该轨迹规划系统使用oha*算法进行前端路径搜索，加快了算法执行速度，使无人机面对动态障碍物时能够做出及时的避障反应；后端轨迹优化部分将动态障碍物避障建模成最优分离平面约束，在保证轨迹安全性的同时，解决了规划出的轨迹过于保守的问题；轨迹优化求解时，将优化问题转化为带软约束的优化问题，提高了求解成功率。

74、以上优点使本算法能够对动态场景下环境的突然改变做出快速反应，而且以高求解成功率保证了系统的稳定性，使无人机能够在复杂的动态环境下完成自主导航。

75、第四，本发明所描述的动态环境下无人机轨迹规划方法利用oha*算法和minvo基函数，结合优化技术，能够为无人机在复杂和不断变化的环境中生成安全有效的飞行路径。这种方法带来的技术进步主要体现在以下几个方面：

76、1)动态环境适应性：通过考虑动态障碍物的轨迹信息，所提出的方法能够使无人机在动态环境中更加灵活地避障，提高了无人机在变化环境中的适用性和安全性。

77、2)路径搜索效率：利用oha*算法对hybrid-state a*算法的改进，可以更有效地搜索到初始路径。相较于传统的hybrid-state a*算法，oha*算法在保留启发式搜索优势的同时，更好地处理了动态环境中的障碍物信息，提高了搜索效率。

78、3)轨迹平滑性和可行性：通过将分段多项式路径转换为r阶b样条轨迹，无人机的飞行轨迹更加平滑，同时也更易于由底层控制器执行。平滑的轨迹有助于减少机械磨损，并提高无人机的运行效率。

79、4)安全飞行走廊的构建：结合地图生成的安全飞行走廊为无人机提供了一个预先定义的安全区域，确保无人机在飞行过程中避免与静态障碍物相撞。

80、5)实时避障能力：结合动态障碍物预测模块和无人机自身轨迹的避障分离平面约束，增强了无人机对动态障碍物的避障能力，使得无人机能够实时响应环境变化，从而在不断变化的环境中维持高安全标准。

81、6)优化问题的求解：轨迹优化问题的求解使得无人机不仅仅是避开障碍，还能以最优或接近最优的路径飞行，这意味着能够在确保安全的同时，最大化飞行效率，节省时间和能源。

82、7)约束的灵活处理：将轨迹优化中的硬约束转换为软约束，并用梯度下降法求解，提高了问题求解的灵活性，可以在保证主要约束得到满足的前提下，对其他约束进行适当的权衡，找到更加优化的轨迹。

83、8)系统集成性：整个方法的步骤清晰，便于与无人机系统其他部分，如预测模块、底层控制器等集成，提高了系统的可操作性和实用性。

84、总的来说，这种方法能够在动态环境下为无人机提供一种既安全又高效的轨迹规划方案，这对于无人机在城市空间、灾难响应、物流配送等多种场景中的应用都是有显著帮助的。