一种基于速度障碍的感知约束下多无人机协同避碰方法与流程

2017 ieee international conference on robotics and automation(icra).ieee，2017，pp.1922
‑
1927.)基于一种速度选择受限的方法实现了带有感知约束的多无人机避碰，但是设计的方法对无人机平台的灵活性有较大限制，且使得系统的可扩展性变差。


技术实现要素：

9.本发明提出了一种基于速度障碍的分布式多无人机系统协同避碰方法，每个无人机仅基于局部观测获取外界信息，引入改进的速度障碍算法，使得无人机在受到感知约束的条件下仅基于局部观测信息就可以实现协同避碰。
10.一种基于速度障碍的分布式多无人机系统协同避碰方法，包括：。
11.步骤1、针对无人机系统中各个无人机，生成初始运动轨迹；
12.步骤2、基于观测信息的实时协同避碰，具体包括：
13.步骤21、假设存在无人机i和无人机j，其机体半径为r，视野角大小为fov；无人机i自身飞行速度为v
i
，且在飞行过程中观测到无人机j的位置信息p
j
，速度信息v
j
和机头朝向信息ψ
j
；基于观测信息，协同模式函数定义为：
[0014][0015]
当g(p，ψ)≥0时，两个无人机为互相观测模式；当g(p，ψ)＜0时，无人机i对无人机j单方面观测，即单向观测模式；
[0016]
步骤22、根据观测信息，视野向量场函数定义为：
[0017][0018]
其中，为无人机i在机身坐标系中x轴方向的单位向量，是观测速度v
j
的转置，是无人机j的位置信息p
j
在无人机i机身坐标系x轴上的分量，h(p
j
)，c(p
j
)是始终大于0的标量函数；当vvf返回的函数值大于0时，无人机i和无人机j存在潜在的碰撞风险；当函数值小于或等于0时，两个无人机无碰撞风险；
[0019]
步骤23、在判断可能存在碰撞风险后，通过如下步骤实现协同避碰：
[0020]
步骤231、基于观测信息生成相对速度障碍集，当观测模式为双向观测时，相对速度障碍集计算如下表达式：
[0021][0022][0023]
其中，u为向无人机输出的控制量，即速度改变量；argmin表示取函数的最小值，为不可行速度集合的边界，||
·
||表示取模运算；相对速度障碍集为基于控制量u构造的无碰撞速度集合，其几何形状为一个半平面；n为垂直于该半平面的法向量；τ为规划器的执行时间；
[0024]
当观测模式为单向观测时，如果vvf返回值为c(p
j
)，相对速度障碍集计算
如下表达式为：
[0025]
v
i
＝v
i
‑
λ1v
i
‑
λ2v
j
[0026][0027][0028]
如果vvf返回值为则相对速度障碍集计算如下表达式为：
[0029]
u＝
‑
λ3v
i
[0030][0031]
其中λ1，λ2，λ3为正实数；
[0032]
步骤232、生成安全速度集合其中v
max
为最大容许速度，∩表示取交集；
[0033]
d(0，v
max
)＝{p|||p
‑
0||＜v
max
}；
[0034]
p表示所有满足要求的速度向量；
[0035]
步骤233、在生成的安全速度集合内选择最优速度v
opt
，定义为：
[0036][0037]
步骤234、根据选择的最优速度生成连续的两条光滑轨迹序列定义为：
[0038][0039][0040]
其中δ1，δ2是满足δ1 δ2＜1的常数；
[0041]
的光滑轨迹序列计算如下：
[0042][0043]
s.t.ζ(0)＝p
i
[0044][0045][0046][0047]
其中，ζ表示实时生成的轨迹，是轨迹的二阶导数，是轨迹的三阶导数，表示初始时刻的位置和速度，表示δ1τ时刻的速度，a
max
表示飞机的最大加速度；
[0048]
的光滑序列计算如下：
[0049]
[0050][0051][0052][0053][0054]
其中ζ
new
为新生成的光滑轨迹序列，为上一段轨迹序列的末位置；
[0055]
步骤3、执行完两段连续的两段轨迹后，使无人机回归初始运动轨迹。
[0056]
较佳的，λ1，λ2，λ3计算过程如下：
[0057]
首先令
[0058][0059]
其中，是无人机j的位置信息p
j
在无人机i机身坐标系x轴上的分量，是无人机j的位置信息p
j
在无人机i机身坐标系y轴上的分量，为无人机i自身机体坐标系的位置，其数值都始终为0；
[0060]
由k
ij
得到：
[0061][0062]
其中，x1为无人机i，j的相对速度在机体x轴上的分量，x2为无人机j的位置在x轴上的投影。分别为无人机i，j的速度在x，y轴上的分量；
[0063]
令
[0064][0065][0066]
其中为无人机i，j位置连线中点的x、y轴坐标；
[0067]
得到：
[0068][0069]
其中y1为无人机j的位置在y轴上的投影，y2为无人机i和无人机j两个机身轮廓靠近一侧的两条公切线交点在y轴上的投影；
[0070]
由m的值得到λ1的最大值：
[0071][0072]
其中α
‑
θ为公切线与x轴的夹角；
[0073]
基于此，得到λ1的范围为：
[0074][0075]
根据几何关系，计算λ3：
[0076][0077]
较佳的，所述步骤3具体包括如下步骤：
[0078]
步骤31、将无人机已经经过的路径点和执行过程中未经过但小于距离阈值的路径点从当前路径点矩阵中剔除；
[0079]
步骤32、将剩余的时间进行重分配，生成与路径点矩阵匹配的新的时间点矩阵；
[0080]
步骤33、求解初始轨迹生成框架，得到一条通过剩余路径点的新轨迹。
[0081]
较佳的，基于每个无人机的初始任务，生成径点序列和与路径点对应的时间序列，将任务映射为运动轨迹，并采用3阶b样条曲线生成运动轨迹。
[0082]
较佳的，所述步骤1中，通过matlab的优化求解器quadprog实现运动轨迹生成。
[0083]
本发明具有如下有益效果：
[0084]
本发明提出一种基于速度障碍的感知约束下多无人机协同避碰方法，在多个旋翼无人机组成的分布式多无人机系统中，设计协同避碰算法，使无人机基于观测的状态信息(位置、速度)实现交互避碰，只基于观测信息实现避碰，这使得多无人机系统更为灵活，基于观测信息建立的避碰算法能够提高系统的安全性、鲁棒性，减少了系统对于通信的依赖，使得多无人机系统在复杂环境下具有更好的适应能力。
[0085]
本发明方法解决了一个工程中实际存在的问题，即考虑了视野约束，该方法仅需要一个相机传感器即可实现多无人机系统的交互避碰，可以起到节省硬件成本的作用；
[0086]
基于滚动优化框架进行规划，规划所需要的信息通过实时观测获得，这可以提高系统的鲁棒性；改进的避碰方法可以基于速度方向判断潜在碰撞风险，这可以提高系统的规划效率。
[0087]
本方法权衡了任务与安全两项指标，可以使得系统在保证安全的前提下完成既定任务。
附图说明
[0088]
图1表示两个无人机交互避碰示意图；
[0089]
图2表示基于感知范围建立的视野向量场；
[0090]
图3表示改进的相对速度障碍集合；
[0091]
图4表示改进的协同避碰算法示意图；
[0092]
图5表示改进的基于相对速度障碍集的飞行仿真实验图。
具体实施方式
[0093]
下面结合附图和实例对本发明做进一步说明：
[0094]
步骤1、生成面向子任务的运动轨迹
[0095]
考虑由n(n≥6)个无人机组成的多无人机系统。对于系统中的每个无人机考虑由n(n≥6)个无人机组成的多无人机系统。对于系统中的每个无人机其初始任务为给定的路径点序列和与路径点对应的时间序列将任务映射为运动轨迹，并采用3阶b样条曲线生成运动轨迹，即
[0096][0097]
其中c
j
∈c＝[c0，c1，...，c
m
‑1]
t
是b样条的控制点，是3阶b样条的基函数。任务初始目标是生成一条通过所有给定路径点的轨迹，同时要求轨迹要光滑，其优化框架定义为：
[0098]
min j
s
j
w
[0099][0100][0101]
其中j
s
是使轨迹光滑的代价函数，j
w
是使轨迹通过给定路径点的代价函数，是初始和终止状态的约束，是满足无人机性能的运动学约束，是控制点矩阵c的向量形式。其中，j
s
通过惩罚轨迹的三阶导数得到，即
[0102][0103]
在这里α是一个常数，
[0104][0105]
是一个半正定矩阵。
[0106]
给定a＝s/t，j
w
定义为：
[0107][0108]
这里矩阵h定义为
[0109][0110]
约束项定义为：
[0111][0112][0113][0114][0115][0116]
其中λ，a
n
，γ
n
是映射矩阵。
[0117]
上述问题可以通过matlab的优化求解器quadprog实现。
[0118]
步骤2、基于观测信息的实时协同避碰
[0119]
步骤21、假设存在无人机i和其他无人机j，其机体半径为r，视野角大小为fov。无人机i自身飞行速度为v
i
，且在飞行过程中可以观测到处在视野范围内其他无人机的位置信息p
j
，速度信息v
j
和机头朝向信息ψ
j
。基于观测信息，协同模式函数定义为
[0120][0121]
当g(p，ψ)≥0时，两个无人机为互相观测模式；当g(p，ψ)＜0时，无人机i对无人机j单方面观测，即单向观测模式。
[0122]
步骤22、根据观测信息，视野向量场函数定义为
[0123][0124]
其中为无人机i在机身x轴方向的单位向量，是观测速度v
j
的转置，是无人机j的位置信息p
j
在无人机i机身坐标系x轴上的分量，h(p
j
)，c(p
j
)是始终大于0的标量函数。当vvf返回的函数值大于0时，观测无人机和目标无人机存在潜在的碰撞风险；当函数值小于或等于0时，两个无人机无碰撞风险。
[0125]
步骤23、在判断可能存在碰撞风险后，通过如下步骤实现协同避碰：
[0126]
步骤231、基于观测信息生成相对速度障碍集mod_orca(p
j，
v
i
，v
j
，mode)，其中mode是观测模式，其数值由g(p，ψ)给出。当观测模式为双向观测时，计算如下表达式：
[0127][0128][0129]
其中，u为速度改变量，其计算方法为求解无人机i，j的相对速度到不可行速度集合边界的最小值。通过求解u可以得到从当前速度到期望速度的最小变化量。其中，argmin
表示取函数的最小值，为不可行速度集合的边界，||
·
||表示取模运算。为基于控制量u构造的无碰撞速度集合，其几何形状为一个半平面。的计算方法为找到所有与u的内积大于或等于0的速度，使得集合内的速度都是安全无碰撞的。其中，n为垂直于该半平面的法向量，其计算方法为取u的单位向量，τ为规划器的执行时间。
[0130]
当观测模式为单向观测时，如果vvf返回值为c(p
j
)，计算如下表达式：
[0131]
v
i
＝v
i
‑
λ1v
i
‑
λ2v
j
[0132][0133][0134]
如果vvf返回值为则
[0135]
u＝
‑
λ3v
i
[0136][0137]
其中λ1，λ2，λ3为正实数，结合改进的协同避碰算法示意图(图4)，其数值计算过程如下：
[0138]
首先令
[0139][0140]
其中，是无人机j的位置信息p
j
在无人机i机身坐标系x轴上的分量，是无人机j的位置信息p
j
在无人机i机身坐标系y轴上的分量，为无人机i自身机体坐标系的位置，其数值都始终为0。
[0141]
由k
ij
可以得到
[0142][0143]
其中，x1为无人机i，j的相对速度在机体x轴上的分量，x2为无人机j的位置在x轴上的投影。分别为无人机i，j的速度在x，y轴上的分量。
[0144]
令
[0145][0146][0147]
其中为无人机i，j位置连线中点的x、y轴坐标。
[0148]
可以得到
[0149][0150]
其中y1为无人机j的位置在y轴上的投影，y2为无人机i和无人机j两个机身轮廓(圆形)靠近一侧的两条公切线交点在y轴上的投影；
[0151]
由m的值可以得到λ1的最大值
[0152][0153]
其中α
‑
θ为公切线与x轴的夹角。
[0154]
基于此，可以得到λ1的范围为
[0155][0156]
根据几何关系，采用计算λ3的步骤，可以得到
[0157][0158]
步骤232、生成安全速度集合orca
τ
＝d(0，v
max
)∩mod_orca，其中v
max
为最大容许速度，∩表示取交集；
[0159]
d(0，v
max
)＝{p|||p
‑
0||＜v
max
}。
[0160]
其中，p表示所有满足要求的速度向量；d(0，v
max
)构造了一个圆形集合，其计算方法为找到所有模值小于v
max
的速度集合，该集合表示无人机动力学模型所容许的速度空间。通过求解d(0，v
max
)可以得到无人机在飞行时速度可以达到的最大值和最小值，通过求解orca
τ
可以得到在求解轨迹时满足动力学要求的安全无碰撞的速度集合。
[0161]
步骤233、在生成的安全速度集合内选择最优速度v
opt
，定义为：
[0162][0163]
步骤234、根据选择的最优速度生成连续的两条光滑轨迹序列定义为
[0164][0165][0166]
其中δ1，δ2是满足δ1 δ2＜1的常数；
[0167]
的光滑轨迹序列计算如下：
[0168][0169]
s.t.ζ(0)＝p
i
[0170]
[0171][0172][0173]
其中，ζ表示实时生成的轨迹，是轨迹的二阶导数，是轨迹的三阶导数，表示初始时刻的位置和速度，表示δ1τ时刻的速度，a
max
表示飞机的最大加速度；
[0174]
的光滑序列计算如下：
[0175][0176][0177][0178][0179][0180]
其中ζ
new
为新生成的光滑轨迹序列，为上一段轨迹序列的末位置；
[0181]
步骤3、在上述步骤实施完成后，无人机在当前交互过程中，即时间τ内，可以避免碰撞。在这之后，实施如下步骤，使无人机回归初始轨迹：
[0182]
步骤31、执行路径点更新函数waypoints_update(p)。该函数将无人机已经经过的路径点和执行过程中未经过但小于距离阈值的路径点从当前路径点矩阵中剔除；
[0183]
步骤32、执行时间更新函数time_reallocation(t)。该函数将剩余的时间进行重分配，生成与路径点矩阵匹配的新的时间点矩阵。如仅依靠剩余时间无法生成可行轨迹，则迭代执行t＝t δt，直到生成的轨迹可行。
[0184]
步骤33、求解初始轨迹生成框架，得到一条通过剩余路径点的新轨迹。在求解过程中，需要使用matlab的quadprog函数作为求解器。
[0185]
经过上面的步骤，无人机可以生成回到初始任务的轨迹。通过在线滚动优化，将避碰的步骤和回归的步骤重复执行，直到每个无人机都到达自己的终止位置。
[0186]
图1展示了两个无人机协同避碰的全过程。在初始任务给定的情况下，两个无人机通过实时观测信息，规划生成了避碰的轨迹；在避碰结束后生成了返回当前任务的轨迹。
[0187]
图2展示了构建的视野向量场。观测无人机构建基于自身机体坐标系的视野向量场，并通过场函数过滤没有潜在碰撞风险的无人机。
[0188]
图3至图4展示了改进的速度障碍集合以及具体改进的计算方法。
[0189]
接着，本发明对提出的控制方法进行了仿真与实物实验。本发明进行了两类仿真实验：一类是对于改进的避碰算法进行实验验证。在该仿真中，本发明生成了由两个无人机组成的系统，在实验过程中两个无人机始终保持单向观测状态，且两个无人机存在潜在的碰撞可能。通过执行协同避碰算法，观测无人机轻微改变当前轨迹躲避障碍无人机。对比传
统的方法，该方法更为智能，避碰效果更好；另一类是六个无人机组成的多无人机系统进行协同避碰仿真的实验。在该仿真中，给定每个无人机初始任务轨迹，无人机在运动过程中通过观测信息与其他无人机协同避碰，最终到达目标点。本发明给出整个运动过程中的位置和速度曲线证明算法的有效性。在实物实验中，本发明建立了六个无人机组成的多无人机系统，并基于第二类仿真的条件进行实验验证。在实验过程中，需要用c 的eigen库进行矩阵运算，用ooqp库求解优化问题。
[0190]
图5展示了第一类仿真的实验效果，其中两个无人机的路径点分别为p1＝[0，0；4，0]，p2＝[2，
‑
1.7；2，1.7]。
[0191]
六个无人机的路径点分别为：
[0192]
p1＝[3.97，
‑
0.05；0.04，
‑
0.05]，
[0193]
p2＝[0.02，0.05；3.90，0.05]，
[0194]
p3＝[1.8，1.7；1.8，
‑
0.05；2.1，
‑
1.7]，
[0195]
p4＝[2.0，
‑
1.7；2.0，0.05；1.7，1.7]，
[0196]
p5＝[0.04，1.2；2.20，
‑
1.00；3.47，
‑
1.2]，
[0197]
p6＝[3.47，1.6；2.80，0；0.04，
‑
1]。
[0198]
六个无人机在同一时刻两两之间满足安全距离的约束。
[0199]
在初始位置六个无人机分别得到初始任务轨迹。在执行任务的过程中如发现潜在碰撞风险，则执行协同避碰，在确定安全之后重新规划任务轨迹，继续执行既定任务，最终到达目标位置。
[0200]
通过仿真和实验验证，可以说明，使用这种基于速度障碍的感知约束下多无人机协同避碰方法，能够在只有局部观测信息的情况下，实现多无人机系统的协同避碰，并且能够使每个无人机在避碰结束后回归初始轨迹，在保证安全的前提下完成任务。此外，在线滚动优化过程中的所有优化问题都是标准的二次规划问题，采用优化求解器一定可以生成可行轨迹，这保证了算法的可行性。
[0201]
以上所述的仅为本发明的较佳实施例而已，本发明不仅仅局限于上述实施例，凡在本发明的精神和原则之内所做的局部改动、等同替换、改进等均应包含在本发明的保护范围之内。