一种三维场景下多无人机互相避碰方法

本发明属于无人机的，具体涉及一种三维场景下多无人机互相避碰方法。

背景技术：

1、在一些作业过程中，多台无人机协同作业时，会有全局规划航线交叉的情况，此时需要各台无人机协同进行局部路径规划，也即协同互相避碰过程。

2、无人机路径规划又分为全局路径规划和局部路径规划，全局路径规划主要是为各台无人机规划出理论层面的航线路径，无人机按照该规划航线进行作业，可用的方法有：a*算法、d*算法、智能算法(遗传算法、粒子群算法等)；局部路径规划是指在无人机作业过程中遇到障碍物或者特殊情况下，需要偏离全局路径航线，在一个局部空间内重新规划可行的航线，有：动态窗口法、基于vo的系列方法。

3、但是在现有技术中，未考虑障碍物对规划航线的影响，在多台无人机协同互相避障的过程中场景可能同时存在有障碍物，需要考虑障碍物的影响；未拓展到三维空间下的航线规划，在实际作业场景下，不仅存在平原地区，还会有山地等较为崎岖的地形，这种情况下考虑无人机的局部路径规划需要将算法拓展到三维场景。

技术实现思路

1、为了克服现有技术存在的一个或者多个缺陷与不足，本发明的目的在于提供一种三维场景下多无人机互相避碰方法，用于在有障碍物下能够解决多台无人机互相协同避免碰撞的问题。

2、为了达到上述目的，本发明采用以下的技术方案。

3、一种三维场景下多无人机互相避碰方法，包括步骤如下：

4、构建无人机模型；

5、根据orca算法设置无人机的避碰策略；

6、进行障碍物处理，将三维空间中的障碍物降维到二维空间；

7、对多台无人机对称相向而行的情况进行完全对称处理；

8、结合无人机模型，更新无人机相互避免碰撞的速度、位置数据。

9、优选地，所述构建无人机模型，过程包括：

10、构建无人机的位置动力学模型；

11、构建无人机的运动学模型。

12、进一步地，所述位置动力学模型，使用表示各螺旋桨产生的竖直拉力，分别表示各螺旋桨转动的力矩，根据牛顿-欧拉方程，无人机在自身坐标系下各方向受力通过变换矩阵变换到地球坐标系，变换过程如下式所示：

13、

14、其中，

15、

16、

17、得到无人机的位置动力学模型如下式所示：

18、

19、其中，分别表示在世界坐标系下沿各坐标轴的受力大小，分别表示在无人机自身坐标系下沿各坐标轴的受力大小，分别表示在世界坐标系下沿各坐标轴的加速度大小。

20、进一步地，所述运动学模型如下式所示：

21、

22、其中，分别表示无人机在世界坐标系下沿各坐标轴的速度大小。

23、进一步地，所述无人机的避碰策略，包括：

24、对某台无人机a和另一台无人机b，构造无人机b相对无人机a的速度障碍使用表示点在r范围内的所有点集合，速度障碍的计算如下式所示：

25、

26、对于无人机a，在未来τ时刻内计算不与b碰撞的速度集如下式所示：

27、

28、其中，

29、在未来τ时间内不发生碰撞无人机a的速度取无人机b的速度取

30、定义无人机a的orca半平面为

31、定义无人机b的orca半平面为

32、对于避碰所需的合理速度和存在所有半径情况时下式成立：

33、

34、其中，为无人机a的当前速度，为无人机b的当前速度；

35、当时，计算互相避免碰撞的最小速度并均分该速度给无人机a和无人机b，表示相对速度与边界的最小向量，最小速度的计算如下式所示：

36、

37、使用表示边界上与方向相同的法向量，法向量如下式所示：

38、

39、由此计算得到无人机b相对无人机a产生的orca半平面如下式所示：

40、

41、其余无人机相对无人机a的产生的orca半平面，与无人机b相对无人机a产生的orca半平面计算过程相同，然后计算所有无人机相对无人机a产生的总orca半平面如下式：

42、

43、进一步地，所述进行障碍物处理，将三维空间中的障碍物降维到二维空间，过程包括：

44、对障碍物进行抽象处理设置合适的安全半径；

45、对二维空间中的障碍物分别求取vo，计算出在二维空间中的orca半平面，沿着半平面法线得到三维空间中的orca半平面空间，电线状障碍物oel、块状障碍物ob、二维空间多边形障碍物oml，分别依次用下式所示的公式进行描述：

46、

47、ob＝{p,dp＝r}，

48、

49、其中，p表示某点坐标，表示位于空间某位置的障碍物线段向量，dp表示p点的安全半径，oml表示二维空间多边形障碍物；

50、多边形障碍物上线段与无人机a构建速度障碍如下式所示：

51、

52、对无人机a，其承担完整的速度避让向量如下式所示：

53、

54、相应的orca平面如下式所示：

55、

56、相应的多边形障碍物对无人机a产生的orca半平面如下式所示：

57、

58、结合无人机b对无人机a产生的orca半平面、所有障碍物对无人机a产生的orca半平面，计算得到无人机a可行orca半平面如下式所示：

59、

60、进一步地，所述对多台无人机对称相向而行的情况进行完全对称处理，过程包括：

61、引入速度更新量对无人机避碰的完全对称情况进行描述，速度更新量的计算如下式所示：

62、

63、其中，v′pref为基础更合适速度更新量，prandcosθrand、prand sinθrand、prand tanθrand依次表示对v′pref各分量添加的微小随机扰动量。

64、进一步地，所述结合无人机模型，更新无人机相互避免碰撞的速度、位置数据，过程包括：

65、根据无人机的位置动力学模型、运动学模型，将无人机在t时刻之后的允许最大加速度更新为a′，然后计算无人机相互避免碰撞所需的最新的位置、速度，计算如下式所示：

66、

67、

68、其中，c表示无人机需要计算的每个目标编号，目标编号对应包括其余各台无人机、各个障碍物。

69、

70、本发明技术方案与现有技术相比，具有如下有益效果：

71、将无人机相互避免碰撞问题拓展到三维场景，使用三维场景下的增量式线性规划求解器计算航线；考虑障碍物对路径规划的影响，通过全局的路径规划提前计算好通过路径，对障碍物构建了速度避障范围确保无人机以合理速度飞行。

技术特征：

1.一种三维场景下多无人机互相避碰方法，其特征在于，包括步骤如下：

2.根据权利要求1所述三维场景下多无人机互相避碰方法，其特征在于，所述构建无人机模型，过程包括：

3.根据权利要求2所述三维场景下多无人机互相避碰方法，其特征在于，所述位置动力学模型，使用表示各螺旋桨产生的竖直拉力，分别表示各螺旋桨转动的力矩，根据牛顿-欧拉方程，无人机在自身坐标系下各方向受力通过变换矩阵变换到地球坐标系，变换过程如下式所示：

4.根据权利要求3所述三维场景下多无人机互相避碰方法，其特征在于，所述运动学模型如下式所示：

5.根据权利要求4所述三维场景下多无人机互相避碰方法，其特征在于，所述无人机的避碰策略，包括：

6.根据权利要求5所述三维场景下多无人机互相避碰方法，其特征在于，所述进行障碍物处理，将三维空间中的障碍物降维到二维空间，过程包括：

7.根据权利要求6所述三维场景下多无人机互相避碰方法，其特征在于，所述对多台无人机对称相向而行的情况进行完全对称处理，过程包括：

8.根据权利要求7所述三维场景下多无人机互相避碰方法，其特征在于，所述结合无人机模型，更新无人机相互避免碰撞的速度、位置数据，过程包括：

技术总结本发明公开了一种三维场景下多无人机互相避碰方法。该方法包括步骤构建无人机模型；根据ORCA算法设置无人机的避碰策略；进行障碍物处理，将三维空间中的障碍物降维到二维空间；对多台无人机对称相向而行的情况进行完全对称处理；结合无人机模型，更新无人机相互避免碰撞的速度、位置数据。本发明能够实现在有障碍物下能够解决多台无人机互相协同避免碰撞的问题。技术研发人员：刘永桂,刘川垚受保护的技术使用者：华南理工大学技术研发日：技术公布日：2024/6/26