一种多无人机协同避障的规划方法

1.本发明涉及无人机规划技术领域，具体为一种多无人机协同避障的规划方法。


背景技术：

2.随着无人机在众多领域中的应用不断扩展，执行任务的难度也在不断提升，单无人机作业已经无法满足复杂任务需求，多无人机协同任务模式被广泛的认可和应用。在多无人机任务协同场景下，多无人机间通常采用分布式结构并且具备协同结构，也称任务队形，用以最大化任务的执行效率和灵活性。
3.在未知复杂环境下多无人机执行协同任务时，不可避免的涉及突发障碍物躲避问题和协同结构保持的冲突问题。如何有效利用感知信息为多无人机规划出平滑的协同避障轨迹是至关重要的，在满足各无人机的多类约束关系以及飞行安全的前提下，尽可能贴合协同结构执行无人机协同任务。可以说，合适的多无人机协同避障规划方法是顺利且高效执行多无人机协同任务的前提。因此，研究多无人机间协同避障规划具有更深远的应用价值。
4.在多无人机协同避障方面，现有手段大都将研究重心在于编队策略、协同控制和单机避障规划，在遇到障碍物时变换队形以规避障碍物，无法做到在协同机构的基础上协同避障，同时忽略了多无人机在避障规划过程中感知信息和飞行信息的共享和融合问题。所以传统的编队避障算法无法根据一致的感知信息和协同结构决策出最优协同避障规划动作。
5.公开专利cn113442140a提出了一种基于bezier寻优的笛卡尔空间避障规划方法，采用四阶bezier曲线作为算法优化的目标轨迹模型，确定中间三个控制点位置以确定轨迹形状，采用粒子群算法迭代优化三个中间控制点位置，利用四阶bezier曲线计算获得避障轨迹方程，结合实际控制采样周期，完成避障轨迹规划。然而该方法将起点和终点轨迹优化为一段只有五个关键点的bezier轨迹，采用粒子群算法迭代优化三个中间控制点位置以调整曲线的形状，在复杂环境下粒子群算法的迭代深度也会增大甚至无解，该方法的适应性较差。然而在起点和终点之间对轨迹进行分段化处理，可以更好解决迭代深度大的问题。
6.因此，将感知信息的一致性和协同结构的指导性相结合，设计一种灵活性和适应性强的多无人机协同避障规划方法，对于多无人机在未知复杂的环境中高效地完成协同任务具有重要意义。


技术实现要素：

7.本发明的目的是：针对现有技术中多无人机在未知复杂的环境中不能高效地完成协同任务的问题，提出一种多无人机协同避障的规划方法。
8.本发明为了解决上述技术问题采取的技术方案是：
9.一种多无人机协同避障的规划方法，包括以下步骤：
10.步骤一：设置任务场景参数和任务参数；
11.所述任务场景参数包括：任务环境场景大小、相同构型的无人机数量n
uav
和栅格离散度resolution；
12.所述任务参数包括：协同避障规划的起点坐标start_pt、终点坐标end_pt以及无人机的可探测距离，所述无人机的可探测距离为sense_horizon；
13.步骤二：无人机通过无人机通信邻居表与相邻无人机进行同构信息交换；
14.所述同构信息包括感知信息和飞行状态信息；
15.所述感知信息包括无人机当前位置在世界坐标系中的坐标和自身持有的局部占据栅格地图；
16.所述飞行状态信息包括无人机的自身全局路径和预期轨迹；
17.所述局部占据栅格地图构建步骤为：
18.首先，利用栅格离散度resolution对任务环境场景大小进行离散化，得到局部占据栅格地图的大小，然后利用无人机在飞行过程中不断通过机载传感器获取自身感知范围的障碍物的位置和几何性质，并将障碍物的位置和几何性质进行凸化膨胀处理，得到障碍物栅格区域，最后，根据局部占据栅格地图的大小和障碍物栅格区域构建局部占据栅格地图；
19.步骤三：基于最大一致性理论和分布式通信策略设计分布式信息融合算法，然后利用分布式信息融合算法和当前无人机与其他无人机交换的感知信息对当前无人机自身持有的局部占据栅格地图进行迭代更新，并设置标识缓存池，当标识缓存池中的信息无更新时，停止迭代，此时，无人机持有的局部占据栅格地图即为最优全局占据栅格地图g；
20.步骤四：在协同避障规划的起点坐标start_pt和终点坐标end_pt之间搜索一条无障碍的时间最短全局路径；
21.步骤五：利用无障碍的时间最短全局路径在sense_horizon范围内选取局部目标点tag_pt，并对协同避障规划的起点坐标start_pt和局部目标点tag_pt之间的路径进行轨迹优化，得到一条由起点坐标start_pt通往局部目标点tag_pt的光滑可行预期轨迹；
22.步骤六：利用步骤三中得到的最优全局占据栅格地图g对步骤五中得到的光滑可行预期轨迹进行碰撞检测，碰撞检测具体步骤为：
23.设定前向检查时间t_head和紧急制动时间t_stop，然后计算碰撞概率p_collision，碰撞概率p_collision表示为：
[0024][0025]
其中，dv为新检测出障碍物的速度，dp为障碍物的当前位置，odompt为无人机的当前位置，getoccupy为最优全局占据栅格地图g的占据概率获取函数，prept为无人机在t时刻无人机的预期轨迹点；
[0026]
若碰撞概率p_collision大于0.5，则判定光滑可行预期轨迹与新检测出的障碍物产生了碰撞冲突；
[0027]
当碰撞冲突时机在前向检查时间t_head内时，将碰撞指标置为1；当碰撞冲突时机在制动时间t_stop内时，使无人机进入紧急悬停状态；
[0028]
步骤七：当达到重规划的时机时，将协同避障规划的起点start_pt更新为无人机
当前时刻超前0.5秒的预期轨迹点，重新执行步骤四、步骤五和步骤六；
[0029]
所述重规划的时机为：
[0030]
(1)局部目标点tag_pt与无人机当前位置odompt的欧式距离达到设定阈值，并且无人机未到达终点；
[0031]
(2)步骤五获得的碰撞指标值等于1；
[0032]
(3)当前无人机与邻居无人机间的距离大于协同距离或者小于安全距离，所述当前无人机与邻居无人机间的距离通过当前无人机和其他无人机交换的飞行状态信息得到。
[0033]
进一步的，所述步骤四中无障碍的时间最短全局路径通过以下步骤得到：
[0034]
利用当前无人机自身局部占据栅格地图，以起点坐标start_pt为波源，模拟波在环境中的传播，在传播过程中对当前传播环境节点的邻节点进行评价，得到评价值，并选取评价值最小的邻节点为下一传播环境节点，当波扩展到终点坐标end_pt时结束传播过程，并在终点坐标end_pt处沿到达时间的梯度下降方向回溯传播路径，得到一条从终点坐标end_pt到起点坐标start_pt的最短时间的传播路径，即为无障碍的时间最短全局路径。
[0035]
进一步的，所述评价值通过当前传播环境节点的评价函数f(n)得到，所述评价函数f(n)包括时间成本函数t(n)、启发信息函数h(n)和协同避障收益函数e(n)；
[0036]
所述评价函数f(n)表示为：
[0037]
f(n)＝γ
t
t(n)+γhh(n)-γee(n)
[0038]
其中，γ
t
,γh,γe分别为时间成本函数t(n)、启发信息函数h(n)和协同避障收益函数e(n)在评价函数f(n)中的比例系数。
[0039]
进一步的，所述时间成本函数t(n)表示为：
[0040][0041]
其中，t
x
,ty,tz分别为栅格n在x,y,z方向上所有邻居到达时间的最小值，并且t
x
＞ty＞tz，h为相邻栅格的间距，f为波在栅格n处的传播速度。
[0042]
所述f通过速度场函数得到，所述速度场函数根据局部占据栅格地图并利用esdf地图建立；
[0043]
所述速度场函数表示为：
[0044][0045]
所述启发信息函数h(n)表示为：
[0046][0047]
所述协同避障收益函数e(n)表示为：
[0048][0049]
其中，v
max
为无人机的最大飞行速度，d为由esdf地图得到的当前栅格n与最近障碍
物的带符号欧式距离，e为欧拉常数约为2.71828，为双曲正切函数；dx、dy、dz分别为栅格n与终点坐标end_pt在x,y,z方向上的距离差值，a
ij
为无人机i和无人机j之间的通信关系，d
ij
为无人机i和无人机j对应参考虚拟中心的偏差。
[0050]
进一步的，所述步骤五的具体步骤为：
[0051]
步骤五一：基于分段连续的八阶bezier曲线建立无人机的预期轨迹模型；
[0052]
步骤五二：将无人机的预期轨迹模型中控制点的集合作为优化对象，在无障碍安全凸集内求解飞行能量消耗代价je与协同结构一致代价jc的最小化问题，即轨迹优化问题，并根据轨迹优化问题求解得到优化对象的值，最后将优化对象的值带入预期轨迹模型中，得到一条由起点坐标start_pt通往局部目标点tag_pt的光滑可行预期轨迹。
[0053]
进一步的，所述无人机的预期轨迹模型表示为：
[0054][0055]
其中，m为预期轨迹模型中的轨迹分段数量，n为预期轨迹模型中bezier曲线的阶数，i为上标号且是n阶bezier曲线基函数，即bernstein多项式，p
1,i
、p
2,i
…
p
m,i
为预期轨迹模型中的轨迹分段标号为1,2,...,m的轨迹对应的n阶bezier曲线控制点；t为时间参数，t0、tm分别为预期轨迹设定的起始时间和终止时间，t0、t1…
t
m-1
分别代表为预期轨迹模型中的轨迹分段标号为1,2,...,m的轨迹起始时间，s1、s2…
sm为预期轨迹模型中的轨迹分段标号为1,2,...,m的轨迹时间间隔缩放比例因子，用于将时间参数t从时间间隔[t
m-1
,tm]缩放到固定时间间隔[0,1]。
[0056]
进一步的，所述优化对象表示为：
[0057][0058]
其中，p
1,0
…
p
1,8
…
p
m,0
…
p
m,8
为预期轨迹模型中的轨迹分段标号为1,2,...,m的轨迹对应的n阶bezier曲线控制点p
1,i
、p
2,i
…
p
m,i
取n＝8,i∈[0,n]的详细表述；x
1,0
…
x
1,8
…
x
m,0
…
x
m,8
、y
1,0
…y1,8
…ym,0
…ym,8
、z
1,0
…z1,8
…zm,0
…zm,8
分别为预期轨迹模型中的轨迹分段标号为1,2,...,m的轨迹对应的n阶bezier曲线控制点p
1,i
、p
2,i
…
p
m,i
取n＝8,i∈[0,n]在x,
y,z三个维度上的展开表述。
[0059]
进一步的，所述轨迹优化问题表示为：
[0060]
min j＝λeje+λcjc[0061][0062][0063][0064]
其中，j为轨迹优化问题的目标函数，je为飞行能量消耗代价函数，jc为协同结构一致代价函数，λe、λc为比例系数，n为当前无人机i的邻居无人机数量，j为邻居无人机的编号，aij表示无人机i和无人机j之间的通信关系，分别为无人机i和无人机j对应参考虚拟中心位置，fi(t)为无人机i在t时刻的位置，oi为无人机i的参考虚拟中心向量。
[0065]
进一步的，所述得到一条由起点坐标start_pt通往局部目标点tag_pt的光滑可行预期轨迹的具体步骤为：
[0066]
步骤1：在步骤四中得到的无障碍的时间最短全局路径截取起点坐标start_pt和局部目标点tag_pt间的路径，对路径中的每一个栅格按所在栅格膨胀为安全空间内最大的立方体，当立方体边界触碰到障碍物时或膨胀次数达到safe_ite时，结束膨胀；
[0067]
将安全空间内最大的立方体进行简化后的立方体的个数作为预期轨迹模型中轨迹的段数m；
[0068]
对轨迹的段数m中的第1段轨迹设置轨迹段起点状态,所述起点状态包括无人机起点处的位置start_pt、速度start_vel、加速度start_acc，表示如下：
[0069]
p
1,0
·
s1＝start_pt
[0070]
8(p
1,1-p
1,0
)＝start_vel
[0071][0072]
对轨迹的段数m中的第m段轨迹设置轨迹段终点状态，所述终点状态对应局部目标点位置tag_pt，速度和加速度设置为零，表示如下：
[0073]
p
m,5
·
sm＝tag_pt
[0074]
8(p
m,6-p
m,5
)＝[0,0,0]
[0075][0076]
步骤2：对预期轨迹模型增加最大速度限制、加速度限制以及分段轨迹间连续性限制；所述最大速度限制为：
[0077]-v
max
≤n
·
(p
j,i-p
j,i-1
)≤v
max
,i＝1,2,...,n
[0078]
所述加速度限制表示为：
[0079][0080]
所述分段轨迹间连续性限制表示为：
[0081]
sj·
p
j,n-s
j+1
·
p
j+1,0
＝0
[0082]
(p
j,n-p
j,n-1
)-(p
j+1,1-p
j+1,0
)＝0
[0083][0084]
步骤3：将简化后的立方体作为定义域，并将步骤1和步骤2中内容进行整合，得到二次规划问题，二次规划问题的解即为无人机的预期轨迹模型中控制点位置，进而得到一条通往局部目标点的光滑可行预期轨迹；
[0085]
其中，n为预期轨迹模型中bezier曲线的阶数，v
max
、a
max
分别为最大速度和加速度，p
j,i-2
、p
j,i-1
、p
j,i
、p
j,n-2
、p
j,n-1
、p
j,n
分别为轨迹的段数m中的第j段轨迹对应的n阶bezier曲线中上标号为i-2、i-1、i、n-2、以及n的控制点，p
j+1,0
、p
j+1,1
、p
j+1,2
分别为轨迹的段数m中的第j+1段轨迹对应的n阶bezier曲线中上标号为0、1、2的控制点，sj、s
j+1
分别为轨迹的段数m中的第j段和第j+1段的轨迹时间间隔缩放比例因子。
[0086]
进一步的，所述二次规划问题表示为：
[0087]
min p
t
qp+c
t
p+cf[0088]
s.t.pj∈ωj,j＝1,2,...,m
[0089]aeq
p＝b
eq
[0090]
l
ieq
≤a
ieq
p≤u
ieq
[0091]
其中，m轨迹的段数，q为目标函数j的hessian矩阵，c为目标函数j的线性系数矩阵，cf为目标函数j的常数项，p为预期轨迹模型中的轨迹分段标号为1,2,...,m的轨迹对应的n阶bezier曲线控制点p
1,i
、p
2,i
…
p
m,i
的矩阵表示，pj为轨迹的段数m中的第j段轨迹对应的n阶bezier曲线控制点的矩阵表示，ωj为矩阵pj的无障碍安全凸集，即矩阵pj的定义域，a
eq
、b
eq
为目标函数j的等式约束的系数矩阵和常量矩阵，a
ieq
为目标函数j的不等式约束的系数矩阵，l
ieq
、u
ieq
对应的最小值常量矩阵和最大值常量矩阵。
[0092]
本发明的有益效果是：
[0093]
本技术采用的分布式结构通过邻近无人机的信息交互设计分布式信息融合算法，经有限次迭代可使连通网络中所有无人机在有限的感知范围获得全局一致的感知信息，可以极大地提高各无人机对于实时态势的感知能力，协助无人机完成协同任务可有效降低协同任务的失败率。同时通过飞行信息的交互，实时调整自身规划状态并优化预期轨迹，可以最大限度的贴合协同结构并避免多无人机间发生碰撞。与传统的编队避障规划方法相比丰富了避障规划的协同性和适应性，使多无人做出最优协同避障规划动作。
[0094]
本技术基于fast marching算法设计栅格的评价函数集合，减少模拟波在非必要栅格处的传导，可以加快全局路径的搜索速度，提高规划效率；并且只对局部轨迹进行优化，保证了无人机运动的平滑性且不需要额外的平滑过滤算法，与全局轨迹优化相比大幅减小了计算量，可以很大程度上节省时间成本，更能满足实时在线规划需求。
[0095]
本技术方法同时考虑了静态障碍物和动态障碍物的干扰，提高了无人机在未知复
杂环境中的飞行安全性。
[0096]
本技术将感知信息的一致性和协同结构的指导性相结合，弥补未知复杂环境下的多无人机协同避障规划方法灵活性和适应性不足等缺点，从而有效提高未知环境下的多无人机协同任务的执行效率。
附图说明
[0097]
图1为本技术的多无人机协同避障的规划流程图；
[0098]
图2为本技术的分布式信息融合算法伪代码示意图；
[0099]
图3为多无人机协同避障的全局路径搜索结果示意图；
[0100]
图4为多无人机协同避障的局部轨迹优化结果示意图。
具体实施方式
[0101]
需要特别说明的是，在不冲突的情况下，本技术公开的各个实施方式之间可以相互组合。
[0102]
具体实施方式一：参照图1具体说明本实施方式，本实施方式所述的一种多无人机协同避障的规划方法，包括：
[0103]
步骤1：初始化参数集。包括任务参数、协同结构，以及协同避障规划算法等相关参数。
[0104]
步骤2：环境感知与融合。无人机对未知环境设置为理想的无障碍区域，无人机在飞行过程中不断通过机载传感器获取自身感知范围的障碍物位置和几何性质等信息，对所述障碍物做凸化膨胀处理，构建局部占据栅格地图。
[0105]
相同构型的飞机以一定的频率不断与相邻无人机进行同构信息的交换，所述同构信息包括进行感知信息和飞行状态信息。所述感知信息包括无人机当前位置在世界坐标系中的坐标和自身持有的局部占据栅格地图；所述飞行状态信息包括无人机经协同避障规划算法得到的自身全局路径和预期轨迹。
[0106]
基于最大一致性理论和分布式通信策略设计分布式信息融合算法，经有限次迭代可使连通网络中所有无人机在有限的感知范围获得全局一致的感知信息。此外通过与邻机飞行信息的交互，为协同避障规划提供决策基础，实时调整自身规划状态并优化预期轨迹。
[0107]
步骤3：全局路径搜索。确定协同避障规划的起点坐标和终点坐标，利用工具fiesta构建增量式esdf地图，并根据所述esdf地图对任务区域建立合理的速度场。基于fast marching算法设计当前栅格n的评价函数集合{时间成本t(n)、启发信息h(n)和协同避障收益e(n)}，加快全局路径的搜索速度，在起点和终点之间搜索一条无障碍的最优全局路径。
[0108]
搜索一条无障碍的最优全局路径通过fast marching(fm)算法进行，fast marching(fm)算法是由sethian首次提出的一种用于求解程函方程的数值方法，是图像处理领域解决波传播问题的一种水平集方法。该方法将波传播的数学模型表示为程函方程(eikonal equation)，即其中f为波传播速度，t为某一环境节点的到达时间。该算法通过一阶数值近似来高效求解程函方程，并使用迎风策略来估计即波只会由t
值小的位置向t值更大的方向传播。
[0109]
目前，fm算法逐渐应用于移动机器人的路径规划领域，无人机从起始点到目标点的最短路径可以认为是波从波源到达目标点的时间最短路径。将路径搜索过程等价于波的前向传播过程，在传播过程中，利用程函方程的一阶数值近似来计算波到达邻节点所需的时间，并将其作为对邻节点的评价值，选择评价值最小的邻节点作为波的扩展方向。
[0110]
此外通过设置合理的传播速度f，可以使fm算法搜索到的路径点尽可能远离障碍物表面以及环境中狭窄的空间，处于更广阔的空间，这有益于减小无人机飞行过程中的姿态变化率，降低能量消耗和飞行难度。
[0111]
其中时间成本t(n)描述栅格n的最短到达时间，可通过求解eikonal方程得到，使路径更倾向于选择稀疏障碍物环境从而避免频繁地改变飞行姿态；启发信息h(n)描述从栅格n到达终点的最佳路径估计代价，使搜索过程更倾向于向终点方向扩展；协同避障收益e(n)描述在全局路径搜索过程中应该使路径尽可能的位于多无人机的累积参考虚拟中心偏差较小的区域，指导各无人机的全局路径尽可能贴合协同结构。
[0112]
步骤4：局部轨迹优化。根据步骤3得到的最优全局路径在可观测范围内选取局部目标点tag_pt，只对无人机起点start_pt和局部目标点tag_pt之间的路径做轨迹优化，生成一条通往局部目标点的光滑可行预期轨迹指导飞行。具体包括如下步骤：
[0113]
步骤4.1：确定无人机的预期轨迹模型。采用分段连续的八阶bezier曲线，可避免各段轨迹连接点处位置、速度、加速度、角速度的突变，分段轨迹的灵活性也保证覆盖了绝大多数可行轨迹。预期轨迹模型表达式如下：
[0114][0115]
其中预期轨迹模型的分段数为m，阶数n＝8，上标i∈[0,n]。是预期轨迹模型控制点的集合，为bernstein多项式。[t1,t2,...,tm]代表为每段轨迹预分配的结束时间，总时间为t＝t
m-t0，[s1,s2,...,sm]代表时间比例因子，用于将时间参数t从[t
m-1
,tm]缩放到bezier曲线的固定时间间隔[0,1]，使轨迹优化问题更加稳定。
[0116]
步骤4.2：确定优化问题模型。将优化问题的目标函数定义为在无障碍安全凸集内求解飞行能量消耗代价je与协同结构一致代价jc的最小化问题。优化问题的模型表达式如下：
[0117]
min j＝λeje+λcjc[0118]
其中λe,λc为比例系数，飞行能量消耗代价je描述轨迹四阶导数的平方的积分，直接对应飞行能量消耗。协同结构一致代价jc描述当前位置与协同结构期望位置偏差的二范数，当所述位置偏差越大，则达到协同结构一致的代价越大。
[0119]
步骤4.3：确定约束条件。对预期轨迹增加最大速度和加速度限制，以及分段轨迹间连续性限制。所述分段轨迹间连续性限制描述每两段轨迹在轨迹连接点处保证无人机位置、速度、加速度相等。
[0120]
步骤4.4：优化对象注入。局部目标点选取为全局路径与可观测范围内的交接点，将无人机起点start_pt和局部目标点位置tag_pt设置为局部轨迹优化问题的起始点和目标点。
[0121]
步骤4.5：优化问题求解。由于上述轨迹优化问题为线性约束二次型最小化问题，并且定义域为凸集，因此可以转化为求解二次规划qp问题。
[0122]
步骤5：碰撞检测。由于无人机自身感知的环境不断变化，因此需要在轨迹执行阶段及时对预期轨迹从无人机当前位置odompt处进行前向安全性检查。
[0123]
设定前向检查时间t_ahead，紧急制动时间t_stop，碰撞概率p_collision和默认值为0的碰撞指标。当在前向检查时间t_head内发现预期轨迹有现碰撞冲突时，将碰撞指标置为1；当碰撞冲突时机在制动时间t_stop内时，同时使无人机进入紧急悬停状态。
[0124]
根据碰撞概率p_collision是否大于0.5定义判断在时刻t预期轨迹点prept是否与新检测出的障碍物产生了碰撞冲突，碰撞概率p_collision具体为：
[0125][0126]
步骤6：在线重规划。确定重规划的时机，将协同避障规划的起点start_pt更新无人机当前时刻超前一段时间t_duration的预期轨迹点，重新执行步骤3，4，5。重规划的时机包括如下：
[0127]
(1)局部目标点与无人机所在位置的欧式距离接近某一阈值并且无人机未接近终点。
[0128]
(2)步骤5获得的碰撞指标值等于1。
[0129]
(3)与邻居无人机间的距离大于协同距离或者小于安全距离。
[0130]
实施例：
[0131]
步骤1：初始化参数集。主要包括
[0132]
设置任务环境场景大小s
x
×
sy×
sz，障碍物数量n
obstacles
，相同构型的无人机数量n
uav
，栅格离散度resolution，协同结构等相关参数，以及无人机通信邻居表、最大迭代次env_ite等分布式信息融合算法相关参数和协同避障规划的起点坐标start_pt和终点坐标end_pt、安全飞行区最大膨胀次数safe_ite等全局路径搜索和局部轨迹优化相关参数。
[0133]
本发明设置任务环境区域为s，无人机的传感器视场角为120
°
，可探测距离为sense_horizon，无人机将任务区域s按照离散度resolution划分为三维占据栅格结构gridmap。最大迭代次数env_ite设置为所在连通子图的最大直径n
uav-1，其中n
uav
为参与协同避障规划的无人机数量。
[0134]
本发明基于虚拟结构法将多无人机的协同结构的平面几何中心设置为虚拟中心，根据所述虚拟中心与各无人机的相对位置预设虚拟中心参考向量ok。
[0135]
无人机在飞行过程中不断通过机载传感器获取自身感知范围的障碍物位置和几何性质，并利用障碍物位置和几何性质进行凸化膨胀处理，构建局部占据栅格地图；
[0136]
步骤2：环境感知与融合。考虑到任务环境是全局未知的而无人机姿态是动态变化的，无人机在飞行过程中不断探测自身传感器范围内覆盖的任务环境，并将探测到的障碍物膨胀化为立方体模型，采用占据概率的方法更新gridmap，并构建局部占据栅格地图collision_localmap。在障碍物膨胀过程中对涉及到的栅格的占据概率设置为1。
[0137]
相同构型的飞机不断与相邻无人机进行同构信息的交换，所述同构信息共有两部分组成。分别为：
[0138]
(1)感知信息：包括无人机当前位置在世界坐标系中的坐标和自身持有的局部占据栅格地图collision_localmap。
[0139]
(2)飞行状态信息：包括无人机经协同避障规划算法得到的自身全局路径和预期轨迹。
[0140]
基于最大一致性理论和分布式通信策略设计分布式信息融合算法，如图2所示。最多经env_ite次迭代可使连通网络中所有无人机在有限的感知范围获得全局一致的感知信息。全局一致判断方法定义为标识缓存池无更新，具体来说每架无人机在与邻机通信时同时交换缓存池信息，并在标识缓存池中新增与之直接或间接通信的其他无人机标识，当缓存池中的信息无更新时认为达到全局一致。
[0141]
在进行第k次迭代时，无人机向通信范围内的邻机共享同构信息，同时基于最大一致性协议通过邻机共享的感知信息更新自身局部占据栅格地图collision_localmap。最大一致性协议的更新法则为：
[0142][0143]
此外通过与邻机飞行信息的交互，为协同避障规划提供决策基础，实时调整自身规划状态并优化预期轨迹。
[0144]
步骤3：全局路径搜索。确定协同避障规划的起点坐标start_pt和终点坐标end_pt，基于fast marching算法设计当前节点n的评价函数集合{时间成本t(n)、启发信息h(n)和协同避障收益e(n)}，在起点和终点之间搜索一条无障碍的时间最短全局路径。具体包括：
[0145]
在使用改进fast marching算法搜索全局路径之前，需要根据局部三维占据栅格地图collision_localmap利用工具fiesta构建增量式esdf地图，从而方便地对感知范围内的障碍物进行距离和梯度信息查询。
[0146]
针对飞行环境建立合理的速度场，使栅格地图gridmap中每个栅格的设定飞行速度与该栅格最近障碍物的距离成正比。根据esdf地图建立速度场函数如下：
[0147][0148]
其中d为由障碍物esdf地图得到的当前点与最近障碍物的带符号欧式距离，e为欧拉常数约为2.71828，v
max
为四旋翼无人机的最大飞行速度，为双曲正切函数。
[0149]
从起点出发模拟波传导，设计当前栅格n的评价函数集合，减少模拟波在非必要栅格处的传导，加快全局路径的搜索速度。栅格n的评价函数表达式如下：
[0150]
f(n)＝γ
t
t(n)+γhh(n)-γee(n)
[0151]
评价函数集合由三部分共同组成，分别为：
[0152]
(1)时间成本t(n)：时间成本t(n)描述栅格n的最短到达时间，通过求解eikonal方程得到，使搜索过程更倾向于稀疏障碍物环境从而避免频繁地改变飞行姿态。表达式如下：
[0153][0154]
其中t是当前栅格n的待求到达时间，t
x
,ty,tz分别是栅格n在x,y,z方向上所有邻居的到达时间最小值，并且t
x
＞ty＞tz，h为相邻栅格的间距。
[0155]
(2)启发信息h(n)：启发信息h(n)描述从栅格n到达终点的最佳路径估计代价，使搜索过程更倾向于向终点方向扩展。表达式如下：
[0156][0157]
其中dx,dy,dz分别为栅格n与终点在x,y,z方向上的距离差。
[0158]
(3)协同避障收益e(n)：在全局路径搜索过程中应该使路径尽可能的位于多无人机的累积参考虚拟中心偏差较小的区域，指导各无人机的全局路径尽可能贴合协同结构。无人机选择搜索方向上所对应的累积参考虚拟中心偏差越小、协同结构越完整，则协同避障收益越大。表达式如下：
[0159][0160]
其中a
ij
表示无人机i和无人机j之间的通信关系，d
ij
为二者对应参考虚拟中心偏差。
[0161]
当栅格点n为终点时结束搜索过程，并在终点处沿到达时间的梯度下降方向回溯路径，就得到了一条从终点到起点的最短时间路径，如图3所示。
[0162]
步骤4：局部轨迹优化。根据步骤3得到的最优全局路径在可观测范围内选取局部目标点tag_pt，只对无人机起点start_pt和局部目标点tag_pt之间的路径做轨迹优化，生成一条通往局部目标点的光滑可行预期轨迹指导飞行。具体包括如下步骤：
[0163]
步骤4.1：确定无人机的预期轨迹模型。采用分段连续的八阶bezier曲线，可避免各段轨迹连接点处位置、速度、加速度、角速度的突变，分段轨迹的灵活性也保证覆盖了绝大多数可行轨迹。预期轨迹模型表达式如下：
[0164]
[0165]
其中预期轨迹模型的分段数为m，阶数n＝8，上标i∈[0,n]。[t1,t2,...,tm]代表为每段轨迹预分配的结束时间，总时间为t＝t
m-t0，[s1,s2,...,sm]代表时间比例因子，用于将时间参数t从[t
m-1
,tm]缩放到bezier曲线的固定时间间隔[0,1]，使轨迹优化问题更加稳定。
[0166]
在预期轨迹模型中[p
1,i
,p
2,i
,...,p
m,i
]是m段连续八阶bezier曲线控制点的集合，每段轨迹均采用8个控制点对应无人机的状态，所述状态包括无人机的位置、速度、加速度、角速度。其中控制点[p
l,0
,p
l,1
,p
l,2
,p
l,3
],l∈[1,m]代表轨迹段的起点状态，控制点[p
l,5
,p
l,6
,p
l,7
,p
l,8
],l∈[1,m]对应轨迹段终点状态，保证各段轨迹间连续平滑。
[0167]
bernstein多项式是n阶bezier曲线基函数，的表达式如下：
[0168][0169]
将预期轨迹模型的基函数换成幂基ti，那么当m＝1时的预期轨迹模型对应矩阵表示形式具体为：
[0170][0171]
步骤4.2：确定优化问题模型。
[0172]
确定无人机的预期轨迹模型中控制点位置[p
1,i
,p
2,i
,...,p
m,i
]以确定分段轨迹的形状，由于预期轨迹模型是m段连续的八阶bezier曲线，且控制点均为三维坐标，因此整体优化对象是一个(8+1)
·m×
3维度的粒子，具体而言：
[0173][0174]
将优化问题的目标函数定义为在无障碍安全凸集内求解飞行能量消耗代价je与协同结构一致代价jc的最小化问题。优化问题的模型表达式如下：
[0175]
min j＝λeje+λcjc[0176]
飞行能量消耗代价je：描述轨迹四阶导数的平方的积分，根据四旋翼无人机的微分平坦特性，轨迹的四阶导数对应角加速与电机转速相关直接对应飞行能量消耗，表达式如下：
[0177][0178]
协同结构一致代价jc：描述当前位置与协同结构期望位置偏差的二范数，所述协同结构期望位置结合相邻无人机间的飞行状态信息获得。当各无人机当前位置与协同结构期望位置偏差小于一定阈值时可认为此时多无人机贴合协同结构，对应协同结构一致。当所述位置偏差越大，则达到协同结构一致的代价越大。表达式如下：
[0179][0180]
其中a
ij
表示无人机i和无人机j之间的通信关系，分别为无人机i和无人机j对应参考虚拟中心。所述参考虚拟中心描述为无人机的实际位置减去预设虚拟中心参考位置，表达式如下：
[0181][0182]
步骤4.3：确定约束条件。对预期轨迹增加最大速度和加速度限制，以及分段轨迹间连续性限制。
[0183]
基于bezier曲线的凸包性质和曲线形状随控制点变化的性质，最大速度和加速度限制可转化为对预期轨迹及轨迹高阶导数对应控制点的相应约束。其中最大速度和加速度限制在第j段轨迹上对应的约束表示为：
[0184]-v
max
≤n
·
(p
j,i-p
j,i-1
)≤v
max
,i＝1,2,...,n
[0185][0186]
分段轨迹间连续性限制描述每两段轨迹在轨迹连接点处保证无人机位置、速度、加速度相等。各段轨迹连续性限制在第j段和第j+1段轨迹段间对应的约束表示为：
[0187]
sj·
p
j,n-s
j+1
·
p
j+1,0
＝0
[0188]
(p
j,n-p
j,n-1
)-(p
j+1,1-p
j+1,0
)＝0
[0189][0190]
步骤4.4：优化对象注入。局部目标点选取为全局路径与可观测范围内的交接点，将无人机起点start_pt和局部目标点位置tag_pt设置为局部轨迹优化问题的起始点和目标点。
[0191]
将起点和局部目标点间的路径按所在栅格膨胀为安全空间内最大的立方体，初始化最大膨胀次数为safe_ite，当立方体边界触碰到障碍物时提前结束膨胀过程。
[0192]
修剪掉路径上重复和包含的立方体，将简化后的立方体的个数作为预期轨迹模型中轨迹的段数。
[0193]
由于立方体是典型的凸包结构，因此可将分段轨迹优化问题转化为在单个立方体内对单段轨迹控制点的求解问题。
[0194]
计算两个立方体对应的路径长度，使用梯形速度为单段轨迹预分配飞行时间。
[0195]
对第1段轨迹设置轨迹段起点状态，所述起点状态包括无人机起点处的位置
start_pt、速度start_vel、加速度start_acc等，表示如下：
[0196]
p
1,0
·
s1＝start_pt
[0197]
8(p
1,1-p
1,0
)＝start_vel
[0198][0199]
对第m段轨迹设置轨迹段终点状态，所述终点状态对应局部目标点位置tag_pt，速度和加速度设置为零，表示如下：
[0200]
p
m,5
·
sm＝tag_pt
[0201]
8(p
m,6-p
m,5
)＝[0,0,0]
[0202][0203]
步骤4.5：优化问题求解。由于上述轨迹优化问题为线性约束二次型最小化问题，并且定义域为凸集，因此可以转化为求解二次规划qp问题，表示如下。
[0204]
min p
t
qp+c
t
p+cf[0205]
s.t.pj∈ωj,j＝1,2,...,m
[0206]aeq
p＝b
eq
[0207]
l
ieq
≤a
ieq
p≤u
ieq
[0208]
上述问题的解就是无人机的预期轨迹模型中控制点位置[p
1,i
,p
2,i
,...,p
m,i
]，从而将多段光滑的轨迹拼接成一条最优轨迹，如图4所示。
[0209]
步骤5：碰撞检测。由于无人机自身感知的环境不断变化，因此需要在轨迹执行阶段及时对预期轨迹从无人机当前位置odompt处进行前向安全性检查。
[0210]
设定前向检查时间t_ahead，紧急制动时间t_stop，碰撞概率p_collision和默认值为0的碰撞指标。当在前向检查时间t_head内发现预期轨迹有现碰撞冲突时，将碰撞指标置为1；当碰撞冲突时机在制动时间t_stop内时，同时使无人机进入紧急悬停状态。
[0211]
根据碰撞概率p_collision是否大于0.5定义判断在时刻t预期轨迹点prept是否与新检测出的障碍物产生了碰撞冲突，碰撞概率p_collision具体为：
[0212][0213]
当新检测出障碍物的速度dv大于0时判定为动态障碍物，根据所述动态障碍物当前位置dp和当前速度dv预估在时间t之前与无人机位置odompt的距离的大小表示为碰撞概率。
[0214]
当新检测出障碍物的速度dv接近0时，通过getoccupy函数获取局部占据栅格地图collision_localmap在时刻t预期轨迹点prept处的占据概率。
[0215]
步骤6：在线重规划。确定重规划的时机，将协同避障规划的起点start_pt更新无人机当前时刻超前一段时间t_duration的预期轨迹点，重新执行步骤3，4，5。重规划的时机包括如下：
[0216]
(1)局部目标点与无人机所在位置的欧式距离接近某一阈值并且无人机未接近终
点。
[0217]
(2)步骤5获得的碰撞指标值等于1。
[0218]
(3)与邻居无人机间的距离大于协同距离或者小于安全距离。
[0219]
需要注意的是，具体实施方式仅仅是对本发明技术方案的解释和说明，不能以此限定权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的，仍应落入本发明的保护范围内。