一种面向三维重建的多无人机路径规划方法及系统

本发明属于无人机，具体涉及一种面向三维重建的多无人机路径规划方法及系统。

背景技术：

1、随着无人机技术的快速发展，携带单个高分辨率相机的小型商业无人机正以越来越低的成本逐渐进入大众视野。而近年来国内外在基于图像的三维重建领域中取得的成功使无人机被广泛应用于大型场景的影像捕捉与三维重建。单目商业无人机具备低成本、小型化优势，而且灵活度高，能够穿梭于任意位置，捕获各个角落的场景影像，再基于目前最先进的多视图几何三维重建(multi-view stereo,mvs)算法，即可重建出目标场景的三维模型。然而为降低尺寸与起飞重量，无人机的电池容量往往被设计的非常小。因此在有限的能量下自动捕获对重建的贡献最高的影像数据是当下的研究重点。

2、目前利用无人机捕捉场景影像并重建高质量三维模型的方法存在三方面问题。其一，市场上的主流商业规划软件虽然已经可以为无人机自动规划出覆盖目标区域的网格式飞行路径，但这种路径通常是以固定的高度和视角均匀捕获场景数据，难以获取建筑物的立面细节，导致建模完整性与精细度均较低；其二，国内外最先进的方法虽然支持单架无人机在场景中自动捕捉多视角影像并重建高质量三维模型，但这些方法只能生成非连续轨迹，即需要无人机频繁加减速，并在悬停后拍摄影像，这对支持边飞行边摄影的无人机来说将消耗大量不必要的能量；其三，对于较大范围的场景，单架无人机不仅效率低，而且难以一次性完成全场景的捕获任务。虽然通过将单条轨迹分割为多段可以实现多机同时捕捉数据，但存在多机之间碰撞的风险。因此，如何同时为多架无人机规划出连续的飞行轨迹，实现多无人机协同捕捉场景影像并重建出高质量的三维模型，是当前亟待解决的问题。

技术实现思路

1、为了克服现有技术的缺点与不足，提出一种面向三维重建的多无人机路径规划方法，该方法实现了多无人机以低能耗协同捕捉场景影像并重建出高质量三维模型的自动化。

2、本发明方法的技术方案为一种面向三维重建的多无人机路径规划方法，包括以下步骤：

3、步骤1.粗略模型重建：使用单架无人机，在三维重建目标区域的上空，以固定高度沿网格式循环往返的轨迹对目标区域进行覆盖，并在飞行过程中以固定的间隔垂直向下拍摄影像，完成拍摄任务后，基于mvs算法重建出目标区域的粗略三维模型；

4、步骤2.前期准备：在粗略三维模型表面进行均匀采样得到多个表面点，并在场景所处空间中均匀生成多个候选视点，然后，基于mvs的重建规则，建立表面点的重建度预测模型，实现根据场景中视点的观察方向与所处位置，以及表面点的位置与表面法线方向，对表面点的重建效果进行预测；

5、步骤3.多机任务分配：首先，在水平维度按照一定分辨率将场景分割为多个网格，计算每个网格附近区域所有表面点的重建度损失值总和，然后，对所有网格的重建度损失值进行排序，在其中挑选出多个损失值最高，且相隔超过一定距离的网格，作为多无人机的任务所在位置，最后，采用布谷鸟搜索算法，以最小化适应度函数为目标为多无人机分配最优任务序列；

6、步骤4.多机路径搜索：多机将各自已有轨迹的末端视为起点，对于首轮多机路径搜索，搜索起点为各无人机的起飞点，按固定顺序轮流搜索路径，其中每架无人机按照各自的任务序列，依次访问各个任务所处位置，在这过程中，将所有候选视点视为搜索空间，通过最小化由连续轨迹的能耗、安全性、场景重建度因素组成的目标函数，将连续轨迹延伸至任务所在区域，当所有无人机均完成了各自任务序列的访问后，循环至步骤3，直至所有表面点的重建度的平均值超过阈值；

7、步骤5.影像捕捉与三维模型重建：每架无人机沿最终轨迹飞行，并在每个视点所处位置沿视点的观察方向进行拍摄，所有无人机均完成拍摄任务之后，将拍摄的所有影像输入mvs算法，重建出完整且精细的场景三维模型。

8、进一步地，所述步骤2中的多个表面点，具体表示如下：

9、s＝{si|i＝1,2,…,n}

10、其中si为粗模的第i个表面点，n为表面点总数；

11、多个候选视点，具体表示如下：

12、vcan＝{vi＝(pi,oi,ci,ni,otang,i)|i＝1,2,…,m}

13、其中vi为空间中的第i个候选视点，pi为视点vi的位置，oi为视点vi的观察方向，ci为候选视点的得分，被用来在延伸轨迹时判断视点的优先级，ni为视点vi的来源指针，指向vcan中的其他视点，借助ni对视点vi进行溯源，得到一条完整的路径pi，具体表示如下：

14、

15、其中为对vi进行溯源后得到的路径pi上的第j个视点，为路径pi上的视点总数，pi上最后一个视点

16、在vcan中，otang,i为以视点vi对应的连续轨迹的末端切线方向，其中为基于多项式的双重描述生成的具备最小能耗的连续轨迹，且按排列顺序途径pi中的所有视点，具体表示如下：

17、

18、其中li,k(t)为轨迹在不同维度上的坐标随时间t变化的平滑曲线：

19、

20、其中ak,j为轨迹的多项式系数，

21、在vcan中，m为候选视点总数，在步骤2中，视点vi的观察方向oi被初始化为[0 0 -1]t，对应的连续轨迹的末端切线方向otang,i被初始化为[0 0 1]t，指针ni为空，ci＝∞；

22、候选视点在粗模所处空间中以距离r为间隔均匀分布，因此候选视点在x轴、y轴、z轴上的数量分别为：

23、

24、

25、

26、其中mx为候选视点在x轴上的数量，my为候选视点在y轴上的数量，mz为候选视点在z轴上的数量，xmin为粗模所处空间在x轴上的最小值，xmax为粗模所处空间在x轴上的最大值，ymin为粗模所处空间在y轴上的最小值，ymax为粗模所处空间在y轴上的最大值，zmin为粗模所处空间在z轴上的最小值，zmax为粗模所处空间在z轴上的最大值。

27、进一步地，在步骤2中，视点vi的位置pi为：

28、

29、其中，

30、

31、

32、

33、其中，nx,i为第i个候选视点vi在x轴上的排序，ny,i为第i个候选视点vi在y轴上的排序，nz,i为第i个候选视点vi在z轴上的排序。

34、进一步地，所述步骤2中的重建度预测模型，具体如下：

35、当前场景中已有轨迹上的所有视点v对粗模的第k个表面点sk的重建度总贡献被表示为：

36、h(sk,v)＝2·hmax·(0.5-(1+exp(k1·h′))-1)

37、其中hmax为表面点重建度的最大值，k1权重参数，v的具体表示如下：

38、

39、其中v′i为当前场景中已有轨迹上的第i个视点，m′为当前场景中已有轨迹上的视点总数，h′被设计为：

40、

41、其中δ(sk,v′i)为表面点sk对视点v′i的可见性函数，δ(sk,v′j)为表面点sk对视点v′j的可见性函数，当sk不存在于v′i的视野当中时，δ(sk,v′i)＝0，否则δ(sk,v′i)＝1，w(sk,v′i,v′j)表示视点v′i和v′j对表面点sk的重建度贡献，具体表示为：

42、w(sk,v′i,v′j)＝w1(α)w2(α)w3(dm)cos(θm)

43、其中距离角度θm＝max(θi,θj)，θi和θj分别表示向量与sk的表面法线nk之间的夹角，α表示与之间的夹角，w1,w2,w3分别被表示为：

44、w1(α)＝(1+exp(-k2·(α-α1)))-1,

45、w2(α)＝1-(1+exp(-k3·(α-α2)))-1,

46、

47、其中k2,k3,α1和α2均为参数，dmax表示视点能够观察的最远距离；

48、当前场景中已有轨迹上的第i个视点对全场景表面点s的重建度贡献被表示为：

49、

50、其中

51、h0(sk,v,v′i)＝∑j＝1,…,i-1,i+1,…,m′δ(sk,v′i)δ(sk,v′j)w(sk,v′i,v′j)。

52、进一步地，所述步骤3中的网格，具体表示如下：

53、

54、其中gi,j为网格在x轴第i个，y轴第j个格子，gx为网格g在x轴上的格子总数，gy为网格在y轴上的格子总数，具体表示分别如下：

55、

56、其中r0为网格的分割分辨率，xmin为粗模所处空间在x轴上的最小值，xmax为粗模所处空间在x轴上的最大值，ymin为粗模所处空间在y轴上的最小值，ymax为粗模所处空间在y轴上的最大值；

57、步骤3所述的每个网格附近区域所有表面点的重建度损失值总和，具体计算如下：

58、

59、其中g(s,v,gi,j,r1)表示在由表面点s组成的场景中，当场景中已有视点集v时，在以gi,j为中心，半径为r1的范围内的所有表面点的重建度损失值总和，ρ1(gi,j,sk,r1)为距离判断函数，当表面点sk与gi,j的水平距离大于r1时，ρ1(gi,j,sk,r1)＝0，否则ρ1(gi,j,sk,r1)＝1。

60、进一步地，步骤3所述的多无人机的任务所在位置，具体表示如下：

61、

62、其中ti为多无人机的第i个任务，nt为多无人机的任务总数，多无人机的所有任务均是在网格g中挑选出来的，具体挑选步骤如下：

63、(1)根据网格g中每个格子附近区域所有表面点的重建度损失值总和按从大到小的顺序对格子进行排序；

64、(2)从大到小逐个遍历g中的格子。对于第1个格子，直接将其添加进集合中，然后继续遍历，对于第2个及以后的格子，首先判断其与已经被添加进的所有格子的距离是否大于dmax，当都大于dmax时，将当前格子添加进中，否则跳过当前格子，直至中存在的格子数量达到nt。

65、进一步地，步骤3所述的采用布谷鸟搜索算法，以最小化适应度函数为目标为多无人机分配最优任务序列，具体步骤如下：

66、(1)将多无人机当前轨迹的末端视点作为多机任务分配的起点，其中多无人机当前轨迹由按照一定顺序排列的视点组成，表示为：

67、

68、其中为第i架无人机当前的轨迹，vi,j为第i架无人机当前轨迹上的第j个视点，为第i架无人机当前的轨迹上的视点总数，vcan为候选视点集合，则第i架无人机当前轨迹的末端视点为当第i架无人机当前轨迹上的视点数量时，首个视点vi,1的位置为第i架无人机的起飞位置，观察方向被设置为[0 0 -1]t，以视点vi,1对应的连续轨迹的末端切线方向被设置为[0 0 1]t。

69、(2)采用布谷鸟算法，将多个任务按照一定顺序分配给多无人机，得到多个任务序列，使适应度函数最小，其中多个任务序列被表示为：

70、

71、其中nuav为多无人机的总数，为第i架无人机的任务序列，被表示为：

72、

73、其中ti,j表示第i架无人机的第j个任务，nt,i表示第i架无人机的任务总数；

74、适应度函数被表示为：

75、

76、其中k4、k5和k6均为参数，vstart为任务分配的起点：

77、

78、其中表示第i架无人机当前轨迹上的第个视点，即末端视点。

79、适应度函数中βmax为最大的前后任务之间的拐角，具体表示为：

80、

81、其中βi为以为终点的轨迹的末端切线方向与向量之间的夹角，其中为视点的位置，ti,1为第i架无人机的第1个任务位置，β′i,j表示向量和之间的夹角；

82、适应度函数中dave为相邻任务之间的距离的平均值，具体表示为：

83、

84、其中为视点的位置，ti,j表示第i架无人机的第j个任务，nt,i表示第i架无人机的任务总数，nt表示多无人机的全部任务总数；

85、适应度函数中ddelta表示多任务序列中最长与最短距离的之间的差值，具体表示为：

86、

87、其中i＝1,2,…,nuav，nuav为多无人机数量，nt,i为第i架无人机的任务总数。

88、进一步地，所述步骤4中多机将各自已有轨迹的末端视为起点，按固定顺序轮流搜索路径，其中每架无人机按照各自的任务序列，依次访问各个任务所处位置，具体步骤如下：

89、(1)对于第i架无人机，将其现在的轨迹pi的末端视点作为路径搜索的起点，将第i架无人机的第j个任务ti,j作为路径搜索的终点，步骤4刚开始时i＝1，j＝1；

90、(2)基于改进的a*算法延伸第i架无人机的轨迹，当轨迹的末端视点与ti,j之间的水平距离小于dend时，将第i架无人机的轨迹pi更新为此时的延伸结果；

91、(3)若j<nt,i，则j＝j+1，并跳转至步骤(1)；

92、(4)若j＝nt,i且i<nuav，则i＝i+1,j＝1，并跳转至步骤(1)；

93、(5)若j＝nt,i且i＝nuav，则步骤4结束。

94、进一步地，所述步骤4中的改进的a*算法，具体步骤如下：

95、对于第i架无人机，将其当前路径延伸至第i架无人机的第j个任务ti,j附近的具体步骤为：

96、(1)记录开始时第i架无人机的路径对应的连续轨迹为

97、(2)定义两个空的视点集合open_set与closed_set，并将轨迹pi上最后一个视点添加进open_set；

98、(3)从open_set中移出得分最小的视点，表示为vcurr，并将其添加进closed_set；

99、(4)若vcurr与任务ti,j之间的水平距离小于dend，则转到步骤(8)；

100、(5)计算轨迹延伸中心位置pext＝pcurr+otang·dext，其中pcurr为视点vcurr的位置，otang为vcurr对应的连续轨迹的末端切线方向，dext为延伸距离；

101、(6)基于k-nn算法在候选视点集vcan中搜索出以位置pext为中心，rneib为半径的范围内的所有视点vneib；

102、(7)逐个遍历vneib中的视点vneib。当vneib不在closed_set中且处于安全空间时：1、根据目标函数计算vneib此时的得分2、若vneib不在open_set中，则将vneib的得分cneib更新为并将vneib添加进open_set；3、若vneib处于open_set中且则将vneib的得分cneib更新为并将vneib的来源指针nneib指向vcurr；

103、所述目标函数表示为：

104、

105、其中s为粗模的所有表面点，b2和b3为权重参数，v为当前场景中已有轨迹上的所有视点，dsafe为多机连续轨迹之间的最小安全距离，目标函数中w1的表示为：

106、

107、其中为步骤(1)中记录的开始时刻路径对应的连续轨迹。为路径ppre对应的连续轨迹，ppre的表示为：

108、ppre＝pcurr∪{vneib,vend}

109、其中pcurr为对视点vcurr进行溯源后得到的路径，vend表示一个水平位置与ti,j相同，高度与vneib相同的视点，函数用于计算连续轨迹的能耗，具体表示为：

110、

111、

112、其中ak,i为轨迹的多项式系数；

113、目标函数中w2表示每个视点对场景重建度的平均贡献，表示为：

114、

115、其中

116、pneib＝pcurr∪{vneib}

117、mneib为pneib中视点的数量，vneib,i为pneib中的第i个视点，表示vneib,i对场景中所有表面垫的重建度的总贡献；

118、目标函数中w3表示轨迹的单位能耗对场景重建度的贡献，表示为：

119、

120、其中为路径pneib对应的连续轨迹；

121、目标函数中为路径pneib对应的连续轨迹与其他无人机当前的连续轨迹的最小实时距离计算函数。任意两架无人机之间的实时距离duav可以计算为：

122、

123、其中与分别代表了不同无人机的飞行轨迹。

124、(8)判断此时open_set是否为空，若为非空，则转到步骤(2)；

125、(9)对vcurr进行溯源，得到路径pcurr，并将第i架无人机的当前路径更新为pcurr。

126、另一方面，本发明还提供一种面向三维重建的多无人机路径规划系统，包括：

127、粗略模型重建模块：使用单架无人机，在三维重建目标区域的上空，以固定高度沿网格式循环往返的轨迹对目标区域进行覆盖，并在飞行过程中以固定的间隔垂直向下拍摄影像，完成拍摄任务后，基于mvs算法重建出目标区域的粗略三维模型；

128、前期准备模块：在粗略三维模型表面进行均匀采样得到多个表面点，并在场景所处空间中均匀生成多个候选视点，然后，基于mvs的重建规则，建立表面点的重建度预测模型，实现根据场景中视点的观察方向与所处位置，以及表面点的位置与表面法线方向，对表面点的重建效果进行预测；

129、多机任务分配模块：首先，在水平维度按照一定分辨率将场景分割为多个网格，计算每个网格附近区域所有表面点的重建度损失值总和，然后，对所有网格的重建度损失值进行排序，在其中挑选出多个损失值最高，且相隔超过一定距离的网格，作为多无人机的任务所在位置，最后，采用布谷鸟搜索算法，以最小化适应度函数为目标为多无人机分配最优任务序列；

130、多机路径搜索模块：多机将各自已有轨迹的末端视为起点，对于首轮多机路径搜索，搜索起点为各无人机的起飞点，按固定顺序轮流搜索路径，其中每架无人机按照各自的任务序列，依次访问各个任务所处位置，在这过程中，将所有候选视点视为搜索空间，通过最小化由连续轨迹的能耗、安全性、场景重建度因素组成的目标函数，将连续轨迹延伸至任务所在区域，当所有无人机均完成了各自任务序列的访问后，循环至多机任务分配模块，直至所有表面点的重建度的平均值超过阈值；

131、影像捕捉与三维模型重建模块：每架无人机沿最终轨迹飞行，并在每个视点所处位置沿视点的观察方向进行拍摄，所有无人机均完成拍摄任务之后，将拍摄的所有影像输入mvs算法，重建出完整且精细的场景三维模型。

132、与现有技术相比，本发明具有如下优点和创新性：

133、(1)提出了一种新的基于任务与搜索的路径规划框架，通过在目标场景中生成多个目标任务，并驱使多机向各自的任务所处位置逐渐延伸路径，实现了多无人机以协同的方式自主捕捉场景影像，并重建高质量三维模型；

134、(2)通过建立场景表面点的重建度数学模型实现对场景重建效果的有效估计，借此我们对全场景的重建度损失进行统计，并挑选出重建度损失最高的若干区域作为多无人机的任务，使多无人机能够更加均匀的捕捉场景的影像，提升了重建的全局最优性；

135、(3)通过最小化由多机任务序列的转角与长度组成的适应度函数，实现任务分配结果更有利于平滑且低能耗的多机轨迹的生成，从而提升多机协同性能；

136、(4)提出了一种新的a*路径搜索算法，使路径搜索过程更加注重轨迹的平滑性。此外，在路径搜索过程中，我们通过最小化由轨迹安全性、能耗以及场景重建度共同组成的目标函数，实现对轨迹的平滑性以及场景的重建效果进行联合优化，在确保安全的同时，提升轨迹与重建质量。