一种基于三维动态避障的分布式飞行器编队实现方法与流程

本发明涉及无人机导航与控制的技术领域，更具体地，涉及一种基于三维动态避障的分布式飞行器编队实现方法。

背景技术：

当前飞行器技术日催成熟，单体飞行器也具有各种各样的功能。但是目前市场上绝大多数是单机执行任务，无法完成复杂程度较高的任务，远没有发挥飞行器的潜能。飞行器群协同编队控制，能更好地发挥飞行器灵活机动的特点，完成更复杂、更加系统化和有组织的任务。飞行器群协同编队控制在军事、农业、消防、巡逻等领域有广阔的应用市场。在高空中，飞行器考虑的更多是动态障碍物的问题，因此能够避开动态障碍是本发明考虑的主要问题。

对于常见的飞行器编队常采用上层是路径规划，下层是路径跟随的架构。必须在编队飞行前对整个编队过程进行规划路径，再将路径信息分别发送给飞行器，飞行器最后自主进行路径的跟随。这种架构有两大缺点，一方面是路径离线规划需要上位机作为主要的规划中心，对上位机要求比较高，而且一旦飞行器起飞之后，实时修改队形耗时长，对于飞行器的能源消耗是一大问题。另一方面是无人机在路径跟随的过程中必须同步，换句话说，需要规定某些规定时刻，每个无人机的位置必须明确指定，这样，要求很好的同步机制来实行整个过程，大大提高了整个系统复杂度。

从上述原因考虑，采用基于动态避障的分布式无人机编队方式，只需要设定无人机的几个关键的路径点，飞行器无人机即可自主地到达目标位置，简化对上位机的依赖与上位机的复杂度。

技术实现要素：

本发明为克服上述现有技术所述的至少一种缺陷，提供一种基于三维动态避障的分布式飞行器编队实现方法，减少飞行器在任务中对上位机的依赖，以增强飞行器编队的适应性。

为解决上述技术问题，本发明采用的技术方案是：一种基于三维动态避障的分布式飞行器编队实现方法，所述方法包括：

用户通过上位机设定编队参数；

飞行过程中飞行器获取相互的状态信息；

飞行器采用相同的自主控制方法完成给定任务；

飞行器完成任务后给上位机发送指示。

优选地，所述用户通过上位机设定编队参数的步骤包括：

用户能够利用上位机对一个或多个编队形状进行设定；

上位机根据每个队形中每个飞行器的位置作一个匹配；

用户点击界面按钮，触发上位机程序以轮询方式将目标位置信息发送给各个飞行器；

用户点击开始任务按钮，飞行器开始执行任务。

优选地，所述飞行过程中飞行器获取相互的状态信息的步骤包括：

飞行器通过GPS、惯性传感单元(IMU)与电子罗盘的信息融合获取自身的位置信息、速度信息与偏航信息；

各个无人机按照编号顺序广播发送自身的位置信息与速度信息；

当某个飞行器位置无法获取时，可以通过前一时刻的位置信息与速度信息，估算这一时刻的位置，速度假设不变。

优选地，所述飞行器采用相同的自主控制方法完成给定任务的步骤包括：

飞行器读取当前目标位置，并判断飞行器是否已经到达目标位置；

飞行器根据自身位置与目标位置计算出偏好速度v_pref；

将偏好速度v_pref、其他飞行器的位置与速度，利用动态避障算法(ORCA算法)计算出新的速度；

利用姿态信息，将新的速度进行坐标转换，从大地坐标转换到机体坐标下；

将转换后的速度输入到飞行控制器；

再次获取飞行器自身位置信息，判断是否已经到达目标位置。

优选地，所述当飞行器完成单个任务后，即发送完成指示，等待上位机发送下一步指令。

与现有技术相比，有益效果是：本发明采用基于三维动态避障的分布式编队方法，因此能够省去一般编队实现方法的规划步骤，直接自主进行控制与避障，更加具有实时性与鲁棒性，并且动态避障方法具有预测性与最优性，增强飞行器的避障能力。

附图说明

图1为本发明实施例中的一种基于三维动态避障的分布式飞行器编队实现方法的流程示意图；

图2为本发明实施例中的上位机设定编队参数的流程示意图；

图3为本发明实施例中的飞行器采用相同的自主控制方法完成指定任务的流程示意图；

图4为本发明实施例中的单个飞行器a产生的速度障碍区域VO_a的示意图；

图5为本发明实施例中的最优相互避障速度可行域ORCA_a区域的示意图；

图6为本发明实施例中的是计算新速度v_new的示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。

图1为本发明实施例中的一种基于三维动态避障的分布式飞行器编队实现方法的流程示意图，如图1所示，该方法包括：

S1，用户通过上位机设定编队参数；

S2，飞行过程中飞行器获取相互的状态信息；

S3，飞行器采用相同的自主控制方法完成给定任务；

S4，飞行器完成任务后给上位机发送指示。

图2是上位机设定编队参数的流程图。

进一步地，用户通过上位机设定编队参数的步骤包括：

用户能够利用上位机对一个或多个编队形状进行设定；

上位机根据每个队形中每个飞行器的位置作一个匹配；

用户点击界面按钮，触发上位机程序以轮询方式将目标位置信息发送给各个飞行器；

用户点击开始任务按钮，飞行器开始执行任务。

具体实施中，用户能够通过上位机设定飞行器编队队形。上位机除了提供基本的圆形、矩形、三角形和直线外，还能够通过点击地图，设计任意的形状。当然，为了适应室外GPS低精度环境，每个飞行器之间的最小间距需要超过3米。队形的大小既可以提供滚动条设定，也可以直接通过在地图界面中拖拉设定。地图提供比例尺参数，设定队形在地图界面中显示的大小与实际大小的比例，与地图的比例尺一致。地图的比例尺可以通过地图的API获得。

当各个编队形状设定后，从设定中获取编队中每个飞行器的目标位置，通过匹配算法(可使用Kuhn-Munkres(KM)算法)，以总的位移距离最短为原则，分配每个目标位置对应的飞行器，即可获得对应每个飞行器的目标位置数组。此时，各个目标位置数组存储在上位机中，便于更改。当用户点击上位机界面中的发送任务的按钮后，即马上将目标位置的数组通过无线模块，采用轮询的方式发送到各个飞行器当中。

飞行器接收到目标位置数组后，即清除之前的目标位置数组。只有用户点击上位机的开始任务按钮后，飞行器接收到任务开始命令后，才取目标位置数组中的第一个位置数据作为自己的目标位置。此时即开始采用自主的控制算法完成任务。

在飞行器飞行的过程中，不断通过GPS、惯性传感单元(IMU)与电子罗盘的信息融合获取自身的位置信息、速度信息与姿态信息。信息的获取周期必须小于或者等于控制周期或通信周期ΔT，实例中设置两个周期一致。

获取自身信息后，飞行器通过周期性的发送自身的位置与速度信息，以达到信息共享的目的。为了避免无线模块发送的信息冲突，在第一次开始时，先采用轮询方式进行通信同步。例如：现在设定通信周期为50毫秒，飞行器与飞行器之间的通信间隔为5毫秒。第一次通信时k号飞行器进行信息发送，与此同时，k号飞行器以此时刻为起点，每隔50毫秒发送一次数据；k+1号飞行器收到k号飞行器的信息的5毫秒后，自己进行信息发送。发送消息时，k+1号飞行器以此时刻为起点，每隔50毫秒发送一次数据；k+2号飞行器收到k+1号飞行器的信息的5毫秒以后，进行k+2号飞行器的信息发送。发送消息时，k+2号飞行器以此为起点，每隔50毫秒发送一次数据……在整个通信过程中，飞行器会不断地同步通信的时间。即当飞行器收到其他飞行器的信息时，会更新自己的时间。例如飞行器的编号为k。那么，当飞行器收到k-2号飞行器发出的信息时，他将更新自身的等待时间为10毫秒，即10毫秒之后将会发送自己的信息；当飞行器收到k-1号飞行器发出的信息时，他将更新自身的等待时间为5毫秒,如此类推。

若飞行器n在第k个周期内无法获取飞行器m的信息，那么假设飞行器m在两个周期内(周期k-1与周期k)速度并没有发生变化(即v_m(k)＝v_m(k-1))，那么，可以短期推断出飞行器m在周期k的位置。飞行器m在周期k内的位置为：p_m(k)＝p_m(k-1)+v_m(k-1)*ΔT。p_m(k)为飞行器m在k时刻的位置，p_m(k-1)为飞行器m在k-1时刻的位置。

获取自身和其他飞行器的位置与速度信息之后，即可使用动态避障算法(ORCA算法)进行动态避障。

下面结合图3至图6对S103进行详细说明。

假设总共有n架飞行器进行编队，编号为1到n。编队飞行前设置好每个飞行器的编号。

图3是S103整个步骤的流程图。下面举例说明k号飞行器进行动态避障的整个过程。

第一步，飞行器获取当前的目标位置信息p_g,通过S102中信息融合后获取的当前位置p_now，计算它们之间的偏差，判断是否已经到达目标位置。如果偏差小于一定的阈值，则认为飞行器到达目标位置，进入S104；反之，没有到达目标位置则进入下一步。

第二步，定义偏好速度v_pref为使无人机能够在ΔT内到达目标位置的速度，所以v_pref的方向为指向目标位置p_g，因此，可以通过如下公式计算：v_pref＝(p_g-p_now)/ΔT。为了满足物理的限制，加入保方向饱和控制，使v_pref不超过给定的最大速度v_max。如果v_pref的幅值超过最大速度v_max，则将v_pref＝v_pref/||v_pref||*v_max。

第三步，利用偏好速度v_pref、自身及其他飞行器的位置与速度，使用ORCA算法计算出新的速度v_new。主要步骤包括：

首先，通过S102的信息交流获得其他飞行器(1,2,…,k-1,k+1,…,n)的位置与速度，自身的位置与速度通过GPS、惯性传感单元(IMU)与电子罗盘的信息融合获取。定义自身的位置与速度分别定义为p_k与v_k，其他飞行器的位置与速度分别定义为p_a与v_a,a∈(1,2,...,k-1,k+1,...,n)。

其次，计算各个飞行器相对于自身的位置，判断各个相对位置是否小于5倍的安全距离r_A,即||p_k-p_a||≤5r_A。安全距离r_A即为飞行器间不能超过的最近距离，通过上位机参数设定。假设有m个飞行器符合||p_k-p_a||≤5r_A的要求，飞行器k内部对其他飞行器重新编号为1,2,…,m，自己则编号为0。换句话说，自身的位置与速度分别定义为p₀与v₀，其他飞行器的位置与速度分别定义为p_a与v_a,a∈(1,2,...,m)。对于剩下的n-m个飞行器的信息不作处理。

定义VO_a为由飞行器a导致两飞行器发生碰撞的速度障碍区域，即如果两个飞行器的相对速度处于VO_a内，那么会导致飞行器在周期ΔT内与飞行器a发生碰撞。

图4是由单个飞行器a产生的速度障碍区域VO_a的示意图，阴影部分即为VO_a。计算VO_a时，圆心p＝(p_a-p₀)/ΔT，半径r＝(r_a+r₀)/ΔT。数学表达上，VO的定义为其中D(p,r)＝{q|||q-p||＜r}，v＝v₀-v_a。

接着，定义u为v到VO_a边界最近的向量，数学表达式为其中为VO_a的边界。

图5是最优相互避障速度可行域ORCA_a区域的示意图。最优相互避障速度可行域ORCA_a的定义为其中定义n为u的单位向量，n＝u/||u||。。

图6是计算新速度v_new的示意图。对于a∈(1,2,...,m)分别计算最优相互避障速度可行域ORCA_a后，即可得到一个总交集ORCA，图中使用栅格表示。由于每个ORCA_a均能对应于其中一个飞行器避免碰撞的情况，那么，当新的速度v_new处于所有ORCA时，飞行器在周期ΔT内将避免与其他飞行器发生碰撞。为了达到最优，将v_new选择为ORCA内最靠近偏好速度v_pref的速度，数学定义为

因为ORCA为线性限制组成的凸区域，作为优化问题，可以采用一般的二次规划方法求解。优化的目标函数为

第四步，获取新的速度v_new之后，由于其为在大地坐标系下表示的速度，必须通过坐标变换，转换到飞行器的机体坐标下。假设现在欧拉角的旋转顺序为ZYX，并假设飞行器的滚转角roll、俯仰角pitch、偏航角yaw分别为φ、θ、ψ，那么可以得到旋转矩阵R：

其中c代表cos，s代表sin。

对于ORCA算法算出来的v_new，通过旋转矩阵R即可获得v_new在机体坐标下的表示，

最后，将输入到飞行器的控制器当中。

飞行器每个周期ΔT中，飞行器会根据当前位置与目标位置之差，判断是否已经到达目标位置，如果到达目标位置则更改自身的标志位，并且同时发送完成指示给上机。等待其他飞行器也完成任务后，通过用户点击上位机上的按钮，开始下一个任务。

对于上述为单个飞行器实行动态避障的实例，对于其他每个飞行器均采用相同的控制方式进行。编写飞行器控制程序时，只需要改变其在整个队伍中的编号即可。

本发明实施例中，采用基于三维动态避障的分布式编队方法，因此能够省去一般编队实现方法的规划步骤，直接自主进行控制与避障，更加具有实时性与鲁棒性，并且动态避障方法具有预测性与最优性，增强飞行器的避障能力。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。