一种无人机集群在线障碍规避方法与流程

1.本发明涉及无人机集群技术领域，尤其涉及一种无人机集群在线障碍规避方法。


背景技术：

2.当无人机集群在一个地形起伏的山区执行任务时，需要根据当前时刻获知的局部地形信息规划飞行航路，以避免与地形发生碰撞；此外，当任务环境中存在禁飞区、敌方雷达、防空火力等时，还需要避开它们，以保证集群自身的安全。
3.已有地形/障碍物规避技术多是利用优化算法以离线形式实现的，计算量很大、只能离线计算，难以满足无人机集群的在线任务需求，且未考虑无人机的运动能力约束。


技术实现要素：

4.本发明要解决的技术问题是如何为无人机集群设计控制策略，使其避开地形起伏和障碍物约束的同时，到达目标点，本发明提出一种无人机集群在线障碍规避方法。
5.根据本发明实施例的无人机集群在线障碍规避方法，包括：
6.基于集群无人机将来一段时间与障碍间的位置关系，判断集群无人机是否存在碰撞风险；
7.若存在碰撞风险，则计算左转、右转、爬升所需的规避时间，并确定最速规避策略；
8.基于所述最速规避策略控制所述集群无人机的飞行状态，避开与所述障碍的碰撞。
9.根据本发明实施例的无人机集群在线障碍规避方法计算量小，可在线生成地形/障碍物规避策略，对无人机运动能力约束进行了显式处理，能同时处理地形约束和障碍物约束。
10.根据本发明的一些实施例，基于集群无人机将来一段时间与障碍间的位置关系，判断集群无人机是否存在碰撞风险，包括：
11.计算规避判断时间；
12.通过所述规避判断时间和集群无人机的速度，计算集群无人机经规避判断时间后的目标位置；
13.将集群无人机当前位置与目标位置间的轨迹离散为多个飞行点，并根据每个所述飞行点与障碍间的关系判断是否存在碰撞风险。
14.在本发明的一些实施例中，规避判断时间ta采用下式计算：
[0015][0016]
其中，max{}为最大值取值函数，ω
1max
、ω
2max
、γ
max
分别为无人机转向率、爬升率、爬升角的最大值，γi(t)∈[-π/2,π/2]为第i个无人机在时刻t的爬升角；
[0017]
集群无人机经规避判断时间ta后的目标位置为p
it
，
[0018]
p
it
＝pi(t)+vi(t)ta；
[0019]
其中，pi(t)＝(xi(t),yi(t),zi(t))
t
为第i个无人机在时刻t在选定直角坐标系xyz中的三维位置，vi(t)为第i个无人机在时刻t的速度。
[0020]
根据本发明的一些实施例，计算左转、右转、爬升所需的规避时间，并确定最速规避策略，包括：
[0021]
分别计算左转规避、右转规避及爬升规避需要转过的角度及所需时间；
[0022]
选择左转规避、右转规避及爬升规避中所需时间最小的规避策略作为所述最速规避策略。
[0023]
在本发明的一些实施例中，左转规避需要转过的角度δχ
i,l
为：
[0024]
δχ
i,l
＝n
min,i,l
δχ；
[0025]
其中，δχ为航向角离散网格角度，
[0026]
{线段pi(t)p
il,n
上的每个离散点均不与地形碰撞,也不与任何一个障碍物碰撞}；其中，argmin{}为目标函数最小时的自变量值；
[0027]
右转规避需要转过的角度δχ
i,r
为：
[0028]
δχ
i,r
＝n
min,i,r
δχ；
[0029]
其中，
[0030]
{线段pi(t)p
ir,n
上的每个离散点均不与地形碰撞,也不与任何一个障碍物碰撞}；
[0031]
爬升规避需要转过的角度δγi为：
[0032]
δγi＝n
min,i,c
δγ；
[0033]
其中，δγ为爬升角离散网格角度，
[0034]
{线段pi(t)p
ic,n
上的每个离散点均不与地形碰撞,也不与任何一个障碍物碰撞}。
[0035]
根据本发明的一些实施例，
[0036]
左转规避所需的时间t
i,l
为：
[0037][0038]
右转规避所需的时间t
i,r
为：
[0039][0040]
爬升规避所需的时间t
i,c
为：
[0041][0042]
ω
1max
、ω
2max
分别为无人机转向率、爬升率的最大值。
[0043]
在本发明的一些实施例中，
[0044]
若t
i,l
≤min{t
i,r
,t
i,c
}，则选择左转规避策略，令
[0045][0046]
其中，min{}为最小值取值函数，δt为采样周期；
[0047]
若t
i,r
≤min{t
i,l
,t
i,c
}，则选择右转规避策略，令
[0048][0049]
若t
i,c
≤min{t
i,l
,t
i,r
}，则选择爬升规避策略，令
[0050][0051]
其中，v
ci
(t)、χ
ci
(t)、γ
ci
(t)分别为第i个无人机的速度、转向角、爬升角的控制输入值，vi(t)、χi(t)∈(-π,π]、γi(t)∈[-π/2,π/2]分别为第i个无人机在时刻t的速度、转向角、爬升角，γ
max
为无人机爬升角的最大值，a
min
、ω
1min
分别为无人机加速度、转向率的最小值。
[0052]
根据本发明的一些实施例，集群无人机中的每个无人机均装备有导航模块，用于获取自身的位置、速度信息。
[0053]
在本发明的一些实施例中，集群无人机中的每个无人机均装备有感知模块，用于感知无人机周围预设范围内的地形变化，并转换为机载计算设备处理数据。
[0054]
根据本发明的一些实施例，所述障碍包括：地形障碍和障碍物障碍。
附图说明
[0055]
图1为根据本发明实施例的无人机集群在线障碍规避方法流程图；
[0056]
图2为根据本发明实施例的无人机集群低视角轨迹图；
[0057]
图3为根据本发明实施例的无人机集群高视角轨迹图。
具体实施方式
[0058]
为更进一步阐述本发明为达成预定目的所采取的技术手段及功效，以下结合附图及较佳实施例，对本发明进行详细说明如后。
[0059]
本发明中说明书中对方法流程的描述及本发明说明书附图中流程图的步骤并非必须按步骤标号严格执行，方法步骤是可以改变执行顺序的。而且，可以省略某些步骤，将多个步骤合并为一个步骤执行，和/或将一个步骤分解为多个步骤执行。
[0060]
如图1所示，根据本发明实施例的无人机集群在线障碍规避方法，包括：
[0061]
s100，基于集群无人机将来一段时间与障碍间的位置关系，判断集群无人机是否存在碰撞风险；
[0062]
s200，若存在碰撞风险，则计算左转、右转、爬升所需的规避时间，并确定最速规避策略；
[0063]
s300，基于最速规避策略控制集群无人机的飞行状态，避开与障碍的碰撞。
[0064]
本发明实施例中的无人机集群在线障碍规避方法计算量小，可在线生成地形/障碍物规避策略，对无人机运动能力约束进行了显式处理，能同时处理地形约束和障碍物约束。
[0065]
根据本发明的一些实施例，s100，基于集群无人机将来一段时间与障碍间的位置关系，判断集群无人机是否存在碰撞风险，包括：
[0066]
s110，计算规避判断时间；
[0067]
s120，通过规避判断时间和集群无人机的速度，计算集群无人机经规避判断时间后的目标位置；
[0068]
s130，将集群无人机当前位置与目标位置间的轨迹离散为多个飞行点，并根据每个飞行点与障碍间的关系判断是否存在碰撞风险。
[0069]
在本发明的一些实施例中，s110中规避判断时间ta采用下式计算：
[0070][0071]
其中，max{}为最大值取值函数，ω
1max
、ω
2max
、γ
max
分别为无人机转向率、爬升率、爬升角的最大值，γi(t)∈[-π/2,π/2]为第i个无人机在时刻t的爬升角；
[0072]
集群无人机经规避判断时间ta后的目标位置为p
it
，
[0073]
p
it
＝pi(t)+vi(t)ta；
[0074]
其中，pi(t)＝(xi(t),yi(t),zi(t))
t
为第i个无人机在时刻t在选定直角坐标系xyz中的三维位置，vi(t)为第i个无人机在时刻t的速度。
[0075]
根据本发明的一些实施例，s200中，计算左转、右转、爬升所需的规避时间，并确定最速规避策略，包括：
[0076]
s210，分别计算左转规避、右转规避及爬升规避需要转过的角度及所需时间；
[0077]
s220，选择左转规避、右转规避及爬升规避中所需时间最小的规避策略作为最速规避策略。
[0078]
在本发明的一些实施例中，左转规避需要转过的角度δχ
i,l
为：
[0079]
δχ
i,l
＝n
min,i,l
δχ；
[0080]
其中，δχ为航向角离散网格角度，
[0081]
{线段pi(t)p
il,n
上的每个离散点均不与地形碰撞,也不与任何一个障碍物碰撞}；
[0082]
其中，argmin{}为目标函数最小时的自变量值；
[0083]
右转规避需要转过的角度δχ
i,r
为：
[0084]
δχ
i,r
＝n
min,i,r
δχ；
[0085]
其中，
[0086]
{线段pi(t)p
ir,n
上的每个离散点均不与地形碰撞,也不与任何一个障碍物碰撞}；
[0087]
爬升规避需要转过的角度δγi为：
[0088]
δγi＝n
min,i,c
δγ；
[0089]
其中，δγ为爬升角离散网格角度，
[0090]
{线段pi(t)p
ic,n
上的每个离散点均不与地形碰撞,也不与任何一个障碍物碰撞}。
[0091]
根据本发明的一些实施例，
[0092]
左转规避所需的时间t
i,l
为：
[0093][0094]
右转规避所需的时间t
i,r
为：
[0095][0096]
爬升规避所需的时间t
i,c
为：
[0097][0098]
在本发明的一些实施例中，
[0099]
若t
i,l
≤min{t
i,r
,t
i,c
}，则选择左转规避策略，令
[0100][0101]
其中，min{}为最小值取值函数，δt为采样周期；
[0102]
若t
i,r
≤min{t
i,l
,t
i,c
}，则选择右转规避策略，令
[0103][0104]
若t
i,c
≤min{t
i,l
,t
i,r
}，则选择爬升规避策略，令
[0105][0106]
其中，v
ci
(t)、χ
ci
(t)、γ
ci
(t)分别为第i个无人机的速度、转向角、爬升角的控制输入值，vi(t)、χi(t)∈(-π,π]、γi(t)∈[-π/2,π/2]分别为第i个无人机在时刻t的速度、转向角、爬升角，a
min
、ω
1min
分别为无人机加速度、转向率的最小值。
[0107]
根据本发明的一些实施例，集群无人机中的每个无人机均装备有导航模块，用于获取自身的位置、速度信息。
[0108]
在本发明的一些实施例中，集群无人机中的每个无人机均装备有感知模块，用于感知无人机周围预设范围内的地形变化，并转换为机载计算设备处理数据。
[0109]
根据本发明的一些实施例，障碍包括：地形障碍和障碍物障碍。
[0110]
下面参照附图详细描述根据本发明的无人机集群在线障碍规避方法。值得理解的是，下述描述仅是示例性描述，而不应理解为对本发明的具体限制。
[0111]
本发明为无人机集群地形/障碍物规避问题提供一种满足无人机运动性能约束的在线解决方案，该方案能使无人机集群在执行任务过程中通过及时有效的规避机动避开与地形、障碍物的碰撞。本发明针对无人机集群碰撞风险的判断，采用一种离散化判断方法。针对无人机集群避撞控制策略的设计，采用一种基于网格的最速规避法实现。
[0112]
本发明建立在以下假设的基础上：
[0113]
1)无人机集群中的每个无人机均装备有导航模块，能输出自身的位置、速度信息；装备有执行单元，能快速响应控制输入。
[0114]
2)无人机集群中的每个无人机均装备有感知模块，能感知无人机周围一定范围内的地形变化，并转换为机载计算设备能处理的数据形式。
[0115]
3)任务环境中存在的障碍物的位置、形状、大小等信息在任务开始时已准确建模。
[0116]
结合图2和图3所示，设集群由n个固定翼无人机组成，第i个无人机的带有惯性控制环节的三自由度质点运动学模型为(i＝1,2,
…
,n)
[0117][0118]
其中，pi(t)＝(xi(t),yi(t),zi(t))
t
为第i个无人机在时刻t在选定直角坐标系xyz中的三维位置，vi(t)、χi(t)∈(-π,π]、γi(t)∈[-π/2,π/2]分别为第i个无人机在时刻t的速度、转向角、爬升角，v
ci
(t)、χ
ci
(t)、γ
ci
(t)分别为速度、转向角、爬升角的控制输入值，τv、τ
χ
、τ
γ
分别为速度、转向角、爬升角的时间常数。据此，第i个无人机在时刻t的速度矢量为vi(t)＝vi(t)[cos(γi(t))cos(χi(t)),cos(γi(t))sin(χi(t)),sin(γi(t))]
t
，其中上标t表示转置。另外，设无人机i的安全半径为ri。每个无人机在每一时刻t的速度v(t)、加速度a(t)、转向率ω1(t)、爬升率ω2(t)、爬升角γ(t)分别满足如下约束
[0119]vmin
≤v(t)≤v
max
ꢀꢀ
(2)
[0120]amin
≤a(t)≤a
max
ꢀꢀ
(3)
[0121]
ω
1min
≤ω1(t)≤ω
1max
ꢀꢀ
(4)
[0122]
ω
2min
≤ω2(t)≤ω
2max
ꢀꢀ
(5)
[0123]
γ
min
≤γ(t)≤γ
max
ꢀꢀ
(6)
[0124]
其中v
min
、a
min
、ω
1min
、ω
2min
、γ
min
分别为无人机速度、加速度、转向率、爬升率、爬升角的最小值，v
max
、a
max
、ω
1max
、ω
2max
、γ
max
分别为无人机速度、加速度、转向率、爬升率、爬升角的最大值。
[0125]
设第i个无人机的目标位置为p
t,i
(t)＝(x
t,i
(t),y
t,i
(t),z
t,i
(t))
t
。
[0126]
地形由高程数据{x,y,z}表示，其中x、y、z均为ny×nx
维的网格数据，x为经度网格，y为纬度网格，z为高度网格。对任意经纬度位置(x,y)
t
，令z(x,y)表示该处的高度值。z(x,y)可通过插值或函数逼近方法得到。对空间中任一位置(x,y,z)
t
，当z≥z(x,y)时，表示未与地形发生碰撞；当z＜z(x,y)时，表示与地形发生了碰撞。
[0127]
障碍物用如下方程表示：
[0128][0129]
其中q0＝(x0,y0,z0)
t
为实体的“中心”(例如球形中心、圆锥底面中心等)；ai、pi(i＝1,2,3)均为实数，当它们取不同值时，上述方程代表不同形状的障碍物，具体如下：
[0130]
a)当p1＝p2＝p3＝1、a1＝a2＝a3时，表示球体；
[0131]
b)当p1＝p2＝1、p3＞1、a1＝a2时，表示圆柱体；
[0132]
c)当p1＝p2＝1、0＜p3＜1、a1＝a2时，表示圆锥体；
[0133]
d)当p1,p2,p3＞1时，表示平行六面体。
[0134]
根据上述定义，当f(x,y,z)＜0时，点(x,y,z)
t
在障碍物内部；当f(x,y,z)＝0时，点(x,y,z)
t
在障碍物表面上；当f(x,y,z)＞0时，点(x,y,z)
t
在障碍物外部。设共有m个障碍物，方程分别为fm(x,y,z)≤0(m＝1,2,
…
,m)。
[0135]
地形/障碍物规避问题可以描述为，对每个无人机i，设计合适的控制输入v
ci
(t)、χ
ci
(t)、γ
ci
(t)，使其位置总满足zi(t)＞z(xi(t),yi(t))+ri，且对m＝1,2,
…
,m，总有fm(xi(t),yi(t),zi(t))＞0。
[0136]
步骤1、判断是否需要规避。
[0137]
根据下式计算规避判断时间ta，
[0138][0139]
其中，max{}为最大值取值函数。
[0140]
根据下式计算无人机在规避判断时间ta内按当前速度到达的最远点，
[0141]
p
it
＝pi(t)+vi(t)taꢀꢀꢀ
(9)
[0142]
将线段pi(t)p
it
离散化为k个点，判断每个点是否与地形发生碰撞，或者进入某个障碍物内部。若是，则认为有碰撞风险，需要规避；否则，认为无碰撞风险。
[0143]
步骤2、规避角度计算。
[0144]
若判断有碰撞风险，首先，根据下式计算左转规避需要转过的角度δχ
i,l
，
[0145]
δχ
i,l
＝n
min,i,l
δχ
ꢀꢀꢀ
(10)
[0146]
其中，δχ为一个较小的航向角离散网格角度，
[0147]
{线段pi(t)p
il,n
上的每个离散点均不与地形碰撞,也不与任何一个障碍物碰撞}
ꢀꢀ
(11)
[0148][0149]
其中为向上取整函数
ꢀꢀ
(13)
[0150]
其中，argmin{}为目标函数最小时的自变量值。
[0151]
然后，根据下式计算右转规避需要转过的角度δχ
i,r
，
[0152]
δχ
i,r
＝n
min,i,r
δχ
ꢀꢀꢀꢀ
(14)
[0153]
其中
[0154]
{线段pi(t)p
ir,n
上的每个离散点均不与地形碰撞,也不与任何一个障碍物碰撞}
ꢀꢀ
(15)
[0155][0156]
最后，根据下式计算爬升规避需要转过的角度δγi，
[0157]
δγi＝n
min,i,c
δγ
ꢀꢀꢀꢀ
(17)
[0158]
其中，δγ为一个较小的爬升角离散网格角度，
[0159]
{线段pi(t)p
ic,n
上的每个离散点均不与地形碰撞,也不与任何一个障碍物碰撞}
ꢀꢀ
(18)
[0160][0161][0162]
步骤3、最速规避策略计算。
[0163]
根据下列各式计算左转、右转、爬升所需的规避时间
[0164][0165][0166][0167]
若t
i,l
≤min{t
i,r
,t
i,c
}，则选择左转规避，令
[0168][0169]
其中min{}为最小值取值函数，δt为采样周期。
[0170]
若t
i,r
≤min{t
i,l
,t
i,c
}，则选择右转规避，令
[0171][0172]
若t
i,c
≤min{t
i,l
,t
i,r
}，则选择爬升规避，令
[0173][0174]
步骤4、目标点朝向计算。
[0175]
若判断无碰撞风险，则令
[0176][0177]
其中sat()为定义如下的饱和函数
[0178][0179]
步骤5、判断是否终止。
[0180]
当每个无人机均到达其目标点时，终止；否则，继续步骤1到步骤4。
[0181]
综上所述，本发明可在线生成地形/障碍物规避策略，对无人机运动能力约束进行了显式处理，能同时处理地形约束和障碍物约束。
[0182]
通过具体实施方式的说明，应当可对本发明为达成预定目的所采取的技术手段及功效得以更加深入且具体的了解，然而所附图示仅是提供参考与说明之用，并非用来对本发明加以限制。

技术特征：
1.一种无人机集群在线障碍规避方法，其特征在于，包括：基于集群无人机将来一段时间与障碍间的位置关系，判断集群无人机是否存在碰撞风险；若存在碰撞风险，则计算左转、右转、爬升所需的规避时间，并确定最速规避策略；基于所述最速规避策略控制所述集群无人机的飞行状态，避开与所述障碍的碰撞。2.根据权利要求1所述的无人机集群在线障碍规避方法，其特征在于，基于集群无人机将来一段时间与障碍间的位置关系，判断集群无人机是否存在碰撞风险，包括：计算规避判断时间；通过所述规避判断时间和集群无人机的速度，计算集群无人机经规避判断时间后的目标位置；将集群无人机当前位置与目标位置间的轨迹离散为多个飞行点，并根据每个所述飞行点与障碍间的关系判断是否存在碰撞风险。3.根据权利要求2所述的无人机集群在线障碍规避方法，其特征在于，规避判断时间t
a
采用下式计算：其中，max{}为最大值取值函数，ω
1max
、ω
2max
、γ
max
分别为无人机转向率、爬升率、爬升角的最大值，γ
i
(t)∈[-π/2,π/2]为第i个无人机在时刻t的爬升角；集群无人机经规避判断时间t
a
后的目标位置为p
it
，p
it
＝p
i
(t)+v
i
(t)t
a
；其中，p
i
(t)＝(x
i
(t),y
i
(t),z
i
(t))
t
为第i个无人机在时刻t在选定直角坐标系xyz中的三维位置，v
i
(t)为第i个无人机在时刻t的速度。4.根据权利要求1所述的无人机集群在线障碍规避方法，其特征在于，计算左转、右转、爬升所需的规避时间，并确定最速规避策略，包括：分别计算左转规避、右转规避及爬升规避需要转过的角度及所需时间；选择左转规避、右转规避及爬升规避中所需时间最小的规避策略作为所述最速规避策略。5.根据权利要求4所述的无人机集群在线障碍规避方法，其特征在于，左转规避需要转过的角度δχ
i,l
为：δχ
i,l
＝n
min,i,l
δχ；其中，δχ为航向角离散网格角度，其中，argmin{}为目标函数最小时的自变量值；右转规避需要转过的角度δχ
i,r
为：δχ
i,r
＝n
min,i,r
δχ；其中，爬升规避需要转过的角度δγ
i
为：
δγ
i
＝n
min,i,c
δγ；其中，δγ为爬升角离散网格角度，6.根据权利要求5所述的无人机集群在线障碍规避方法，其特征在于，左转规避所需的时间t
i,l
为：右转规避所需的时间t
i,r
为：爬升规避所需的时间t
i,c
为：ω
1max
、ω
2max
分别为无人机转向率、爬升率的最大值。7.根据权利要求6所述的无人机集群在线障碍规避方法，其特征在于，若t
i,l
≤min{t
i,r
,t
i,c
}，则选择左转规避策略，令其中，min{}为最小值取值函数，δt为采样周期；若t
i,r
≤min{t
i,l
,t
i,c
}，则选择右转规避策略，令若t
i,c
≤min{t
i,l
,t
i,r
}，则选择爬升规避策略，令其中，v
ci
(t)、χ
ci
(t)、γ
ci
(t)分别为第i个无人机的速度、转向角、爬升角的控制输入值，v
i
(t)、χ
i
(t)∈(-π,π]、γ
i
(t)∈[-π/2,π/2]分别为第i个无人机在时刻t的速度、转向角、爬升角，γ
max
为无人机爬升角的最大值，a
min
、ω
1min
分别为无人机加速度、转向率的最小值。8.根据权利要求1所述的无人机集群在线障碍规避方法，其特征在于，集群无人机中的每个无人机均装备有导航模块，用于获取自身的位置、速度信息。9.根据权利要求1所述的无人机集群在线障碍规避方法，其特征在于，集群无人机中的每个无人机均装备有感知模块，用于感知无人机周围预设范围内的地形变化，并转换为机载计算设备处理数据。
10.根据权利要求1-9中任一项所述的无人机集群在线障碍规避方法，其特征在于，所述障碍包括：地形障碍和障碍物障碍。

技术总结
本发明提出了一种无人机集群在线障碍规避方法，包括：基于集群无人机将来一段时间与障碍间的位置关系，判断集群无人机是否存在碰撞风险；若存在碰撞风险，则计算左转、右转、爬升所需的规避时间，并确定最速规避策略；基于最速规避策略控制集群无人机的飞行状态，避开与障碍的碰撞。本发明中的无人机集群在线障碍规避方法计算量小，可在线生成地形/障碍物规避策略，对无人机运动能力约束进行了显式处理，能同时处理地形约束和障碍物约束。能同时处理地形约束和障碍物约束。能同时处理地形约束和障碍物约束。


技术研发人员：赵彦杰 梁月乾 袁莞迈 李包华 喻洋
受保护的技术使用者：中国电子科技集团公司信息科学研究院
技术研发日：2023.04.26
技术公布日：2023/9/14