一种多旋翼无人机编队防撞控制方法与流程

1.本发明属于无人机技术领域，具体涉及一种多旋翼无人机编队防撞控制方法。


背景技术：

2.近年来，多旋翼无人机在军事、救援、勘探、航拍等领域得到了广泛应用，多无人机协同工作具备更高的工作效率，但多无人机编队协同工作时的动态防撞也成为了亟待解决的问题。
3.传统的无人机防撞算法只适用于静态环境，没有考虑动态障碍物的情形，然而，为了保证无人机编队协同工作的安全，必须考虑多无人机编队飞行时的动态防撞，为此本发明提出了一种用于多旋翼无人机编队的动态防撞方法。
4.无人机防撞方法可分为协作式防撞预非协作式防撞，协作式防撞在防撞过程中需要共享速度、位置和航线信息，通过协同控制实现编队防撞。非协作式防撞情形则更为复杂，需要借助传感器获取其他飞行器的速度、位置、高度等信息，并且需要考虑无人机性能约束以及入侵飞行器的不确定性，必须采用动态规划方法解决实时防撞问题。
5.无人机动态防撞方法主要有几何法、人工势场法、碰撞锥法、随机理论等方法。但这些方法不能有效考虑约束，例如无人机速度约束、转弯率约束、控制输入约束等。基于优化的方法可以处理约束，并且已被证明是解决编队控制问题较成功的方法之一，无人机防撞领域常用的基于优化的方法是非线性模型预测控制(nmpc)方法。
6.nmpc是一种反馈控制方法，其在每个采样时刻求解优化轨迹，选取最优序列的第一个控制输入，并在每个后续步骤中重复优化。该方法能有效克服模型精度误差等不利因素，通过在线反馈修正确定当前最优控制值，同时保证被控对象具有良好的稳定性。但nmpc方法存在计算成本高的问题，在多无人机编队的情况下，算法的执行效率无法满足快速实时防撞的需求。
7.总之，现有的nmpc方法在实时性方面还有很大提升空间。


技术实现要素：

8.为了解决上述问题，本发明提出了一种多旋翼无人机编队防撞控制方法，该方法采用滚动在线优化的nmpc方法，设计了一种基于规则的避障控制策略，能够有效解决多无人机编队防撞问题。
9.为了实现上述目的，本发明采取的技术方案为：
10.一种多旋翼无人机编队防撞控制方法，应用于多旋翼无人机编队中的每一架无人机，包括如下步骤：
11.(1)选取系统预测时域n和采样周期δt，设定加权参数q、r、λ，根据无人机性能设置输入约束；
12.(2)根据本机的飞行信息及飞行目标建立无人机运动学模型；
13.(3)建立无人机飞行保护区，获取入侵飞行器集合；
14.(4)建立nmpc非线性模型预测控制方程，根据无人机防撞控制非线性最优控制模型求解本机防撞冲突消解的控制序列；
15.(5)对于步骤(4)求解的冲突消解控制序列，进行碰撞规避。
16.进一步的，步骤(2)的具体方式为：
17.(201)建立本机的导航坐标系；所述导航坐标系采用东北天坐标系，载体坐标系点位于机体重心，x轴指向机体右侧，y轴指向机头前进方向，z轴指向机体上方；无人机在三维空间中的质点描述方程为：
[0018][0019]
其中，v为无人机速度，ψ为无人机航向角，γ为无人机航迹倾角；
[0020]
采用一阶线性系统对无人机防撞过程进行建模：
[0021][0022]
其中，τv、τ
ψ
和τ
γ
为一阶响应的时间常数，[vcψcγc]
t
表示本机的期望速度、期望航向角和期望航迹倾角；
[0023]
联立式(1)、式(2)，分离控制向量[vcψcγc]
t
，得到无人机防撞过程的二阶导数模型：
[0024][0025]
[0026][0027]
进一步的，步骤(3)的具体方式为：
[0028]
定义无人机安全保护区，所述无人机安全保护区为一个半径为dm的最小安全间隔球p：
[0029]
p＝{x|||x-ra||＜dm}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0030]
其中，x∈r2，||
·
||表示欧氏距离，ra是冲突目标的当前位置；定义ds为入侵飞行器与无人机航迹的最小垂直距离，当ds≤dm时视为有碰撞冲突，建立冲突飞行器集合。
[0031]
进一步的，步骤(4)的具体方式为：
[0032]
将式(1)所示无人机运动学方程离散化，并表示为nmpc系统方程：
[0033][0034]
其中，x(k)为无人机k时刻的状态，δt为采样周期，控制量为u(k)＝[ψc(k)γc(k)]
t
；
[0035]
防撞控制系统需要使无人机在向目标点飞行的同时以最小代价实现碰撞冲突解除。令定义代价函数:
[0036][0037]
其中，φ
t
rφ为防撞过程的能量代价，r为能量代价加权系数，φ(tf)为tf时刻无人机与目标点的距离，具体为：
[0038]
φ(tf)＝[xu(tf)-xc]
t
q[xu(tf)-xc]
[0039]
其中，tf时刻无人机位置矢量为xu(tf)＝[xu(tf)yu(tf)zu(tf)]
t
,目标点位置矢量为xc(tf)＝[xc(tf)yc(tf)zc(tf)]
t
，q为距离代价系数；
[0040]
g(t)为防撞代价，定义为：
[0041][0042]
其中，λ为代价函数的加权系数，b(t)为入侵无人机集合，ds(xu(t),xj(t))]为无人机与入侵无人机的最小垂直距离，ωj表示无人机与不同入侵飞行器的碰撞紧急程度；
[0043]
建立无人机防撞非线性离散模型预测控制优化模型：
[0044][0045]
其中，n表示预测时域，为系统在控制输出情况下的预测状态；对式(7)进行求解，得到预测时域n内的控制序列；
[0046][0047]
k时刻输出的控制量为：
[0048][0049]
根据式(7)对优化模型的定义，记k时刻预测时域为n时的优化目标值为jk(n)，记在相同控制序列下预测时域为{τ|τ＝1,2,
…
,n-1}时的优化目标为jk(τ)，无人机在防撞过程中也在向目标点飞行，可得：
[0050]jk
(τ)＜jk(n) φ(x(k|k))
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0051]
其中，φ(x(k|k))在x(k|k)时的代价表达为：
[0052]
φ(x(k|k))＝[x(k|k)-xc]
t
q[x(k|k)-xc]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0053]
根据式(8)，定义k 1时刻的候选控制序列为：
[0054][0055]
其中，候选控制序列的前n-1个元素为k时刻最优解的后n-1个元素，它们均满足控制约束；根据无人机状态方程计算无人机候选状态序列结合入侵无人机状态和无人机候选状态序列判断碰撞冲突，建立冲突无人机集合，根据式(7)计算候选优化目标值和代价φ(x(k|k))，遍历无人机机动策略树，以候选序列作为当前最优控制序列u
*
(k)，将第一项u
*
(k|k)作为无人机控制输入，并获取无人机状态反馈x(k 1|k 1)作为下一时刻的初始状态，最后令k＝k 1，滚动到步骤(3)。
[0056]
进一步的，步骤(5)的具体方式为：
[0057]
(501)根据式(7)计算候选优化目标值和代价φ(x(k|k))；
[0058]
(502)构建无人机可选机动策略树，机动策略树包括五种机动策略：最大爬升率上升、最大爬升率下降、最大转弯率左转、最大转弯率右转、转弯率和爬升率为零，其中，转弯率和爬升率为零即保持当前飞行状态；
[0059]
(503)按照深度优先方式遍历机动策略树，计算jk(τ)，τ＝1,2,
…
,n，若jk(τ)≥jk(n) φ(x(k|k))，则终止当前分支搜索，否则继续搜索；遍历完成后，候选序列作为当前最优控制序列u
*
(k)，输出第一项u
*
(k|k)作为无人机控制输入，并获取无人机状态反馈x(k 1|k 1)作为下一时刻的初始状态。
[0060]
本发明的有益效果在于：
[0061]
1、本发明提供了一种滚动优化非线性模型预测控制多旋翼无人机编队动态防撞方法，具有运行速度快、鲁棒性强的特点，对多旋翼无人机编队实时动态防撞给出了指导。
[0062]
2、本发明实现了模型计算参数的优化，提高了nmpc算法的时效性，其结合自学习实时算法和模型预测控制的多步优化搜索算法，将多无人机编队防撞问题转化为滚动优化问题，针对无人机防撞机动的一致性问题，设计了一种基于规则的防撞控制策略。测试结果表明，该方法能够有效解决无人机未知威胁规避与飞行路径重新规划的问题。
附图说明
[0063]
图1为多旋翼无人机编队防撞控制方法的流程图；
[0064]
图2为多传感器空域态势感知图；
[0065]
图3为无人机运动学建模原理图；
[0066]
图4为nmpc非线性模型预测控制原理图；
[0067]
图5为滚动优化流程图；
[0068]
图6为无人机安全间隔示意图；
[0069]
图7为规避机动策略搜索树；
[0070]
图8为双无人机编队防撞算法仿真示例图；
[0071]
图9为五无人机编队防撞算法仿真示例图。
具体实施方式
[0072]
为了更好地理解本发明的内容，下面结合具体实施方法对本发明内容作进一步说明，但本发明的保护内容不局限以下实施例。
[0073]
一种多旋翼无人机编队防撞控制方法，结合自学习实时算法和模型预测控制的多步优化搜索算法，将多无人机编队防撞问题转化为滚动优化问题，从而基于非线性模型预测控制方法解决编队防撞问题。
[0074]
该方法包括如下步骤：
[0075]
(1)多传感器空域态势感知；
[0076]
(2)无人机运动学建模；
[0077]
(3)nmpc模型预测控制；
[0078]
(4)滚动优化；
[0079]
(5)碰撞检测；
[0080]
(6)碰撞规避。
[0081]
其中，步骤(1)所述的多传感器包括dsp(digital signal processor)主控芯片、加速度计、陀螺仪、磁力计、北斗信号接收机、调频连续波雷达、高度计，所述多传感器用于获取本机和空域内其他飞行器航姿、高度、位置等信息。
[0082]
步骤(1)所述的空域态势感知是指通过处理多传感器获取的信息，得到空域中各飞行器的速度、加速度、飞行高度以及径向距离，进而建立以本机为中心的三维空间坐标，对入侵飞行器和障碍物进行感知、定位。
[0083]
步骤(2)所述的无人机运动学建模是指在三维坐标系内建立本机与入侵飞行器的运动学方程，本机与入侵飞行器使用三维空间中的质点描述；所述三维坐标系包括导航坐标系和载体坐标系，所述导航坐标系采用东北天(enu)坐标系，所述载体坐标系与无人机机体固连，坐标系符合右手法则，原点位于机体重心，x轴指向机体右侧，y轴指向机头前进方向，z轴指向机体上方。
[0084]
步骤(3)所述的nmpc模型预测控制是一种针对非线性系统的多变量控制策略，由预测模型、滚动优化和反馈修正三个部分组成。nmpc方法在每个采样时刻，根据系统当前测得的信息，求解一个有限时域的开环最优化问题，得到一个控制序列，并将该控制序列的第一个元素作用于系统，在下一个采样时刻重复上述操作，用新的测量值作为nmpc系统初始条件，求解新的优化问题。nmpc方法能有效改善模型精度误差，通过反馈修正和有限时域滚动优化确定系统最优控制值，保证系统具有良好的稳定性。
[0085]
步骤(3)所述的预测模型是系统利用受控对象当前状态和未来控制信息，预测未来一段时间的输出。
[0086]
步骤(3)所述的滚动优化本质是求解系统开环优化，通过某一性能指标的最优化来求解未来有限时域的最优控制率。
[0087]
步骤(3)所述的反馈修正是基于模型的预测输出对系统进行修正优化，利用目标性能函数和系统约束条件，将输出序列的第一个元素作为下一时刻控制系统的输入值，通过误差反馈来修正预测模型，提高预测精度。
[0088]
步骤(4)所述的滚动优化是nmpc方法的核心，其思想是通过求解一个有限时域优化问题，得到每个采样时刻的最优控制序列，从而实现系统在整个时域的在线闭环控制。无人机防撞nmpc控制器的滚动优化过程中，监测各无人机的状态参数，包括控制量参数和距离参数，通过预测模型和控制量参数预测系统状态，最后根据预设的优化指标求解优化问题，并将结果输出到无人机飞控单元。预测控制采用时间向前滚动式的有限时域优化策略，因此该步骤被称为滚动优化。
[0089]
步骤(5)所述的碰撞检测基于预先定义的无人机安全保护区进行检测，所述无人机安全保护区定义为一个半径为dm的最小安全间隔球，定义为如下集合p：
[0090]
p＝{x|||x-ra||＜dm}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0091]
其中，x∈r2,||
·
||表示欧氏距离，ra是冲突目标的当前位置，dm为无人机与冲突目标的距离裕度，也是飞行器的最小安全间隔半径。假设无人机感知探测半径和最小安全间隔球半径关系为rm＜＜dm，当入侵飞行器进入无人机探测范围时，无人机根据感知到的入
侵飞行器状态信息进行计算，判断它们之间是否有碰撞和冲突的可能，当入侵飞行器进入最小安全间隔球时，视为发生碰撞。
[0092]
步骤(6)所述的碰撞规避是通过步骤(3)中nmpc算法预测的安全路径进行垂直或水平机动以进行碰撞规避。无人机碰撞规避在满足时效性要求的前提下，通过指定无人机机动行为来切割优化算法的搜索空间，无人机的碰撞规避根据垂直和水平机动解耦，也就是说，无人机在任一时刻只用单一的防撞机动，利用最大机动能力在尽可能短的时间内消除碰撞冲突。本发明定义了五种机动策略，包括最大爬升率上升、最大爬升率下降、最大转弯率左转、最大转弯率右转、转弯率和爬升率为零。
[0093]
以下为一组更具体的例子：
[0094]
实施例1
[0095]
如图1所示，多旋翼无人机编队防撞方法包括如下步骤：
[0096]
(1)多传感器空域态势感知；
[0097]
(2)无人机运动学建模；
[0098]
(3)nmpc模型预测控制；
[0099]
(4)滚动优化；
[0100]
(5)碰撞检测；
[0101]
(6)碰撞规避。
[0102]
实施例2
[0103]
如图2所示，多传感器包括dsp主控芯片、加速度计、陀螺仪、磁力计、北斗信号接收机、调频连续波雷达、高度计，用于获取本机和空域内其他飞行器航姿、高度、位置等信息。空域态势感知是指通过处理多传感器获取的信息，得到空域中各飞行器的速度、加速度、飞行高度以及径向距离等信息，进而建立以本机为中心的三维空间坐标，对入侵飞行器和障碍物进行感知、定位。
[0104]
实施例3
[0105]
如图3所示，无人机运动学建模是在三维坐标系内建立本机与入侵飞行器的运动学方程，无人机可以通过实例2所述空域态势感知获取入侵飞行器的高度、位置矢量和速度矢量。导航坐标系采用东北天(enu)坐标系，载体坐标系与无人机机体固连，符合右手法则，原点位于机体重心，x轴指向机体右侧，y轴指向机头前进方向，z轴指向机体上方。本机与入侵飞行器使用三维空间中的质点描述，即为：
[0106][0107]
其中，v为无人机速度，ψ为无人机航向角，γ为无人机航迹倾角，无人机防撞过程可以用如下一阶线性系统描述。
[0108][0109]
其中，τv、τ
ψ
和τ
γ
为一阶响应的时间常数，并且满足v
min
≤v≤v
max
，γ
min
≤γ≤γ
max
，[vcψcγc]
t
表示系统的期望速度、航向角和航迹倾角。联立(2)、(3)式，分离控制向量[vcψcγc]
t
，得到无人机运动方程的二阶导数模型：
[0110][0111][0112][0113]
实施例4
[0114]
如图4所示，nmpc模型预测控制是一种针对非线性系统的多变量控制策略，由预测模型、滚动优化和反馈修正三个部分组成。nmpc方法在每个采样时刻，根据系统当前测得的信息，求解一个有限时域的开环最优化问题，得到一个控制序列，并将该控制序列的第一个元素作用于系统，在下一个采样时刻重复上述操作，用新的测量值作为nmpc系统初始条件，求解新的优化问题。nmpc方法能有效改善模型精度误差，通过反馈修正和有限时域滚动优化确定系统最优控制值，保证系统具有良好的稳定性。
[0115]
将(2)式无人机运动学方程离散化，可得：
[0116][0117]
其中，x(k)为无人机k时刻的状态，δt为采样周期，控制量为u(k)＝[ψc(k)γc(k)]
t
，该方程的数学优化模型可以写为以下nmpc优化方程：
[0118][0119]
其中，n表示预测时域，为系统在控制输出情况下的预测状态，解(8)所示nmpc优化问题，可得到预测时域n内的控制序列：
[0120][0121]
实施例5
[0122]
如图5所示，滚动优化是nmpc方法的核心，通过求解一个有限时域优化方程，得到每个采样时刻的最优控制序列，实现系统在整个时域的闭环控制。无人机防撞nmpc控制器的滚动优化过程中，监测各无人机的状态参数，包括控制量参数和距离参数，通过预测模型和控制量参数预测系统状态，最后根据预设的优化指标求解优化问题，并将结果输出到无人机飞控单元。由于预测控制采用时间向前滚动式的有限时域优化策略，因此该步骤被称为滚动优化。
[0123]
实施例6
[0124]
如图6所示，碰撞检测基于预先定义的无人机安全保护区进行检测，所述无人机安全保护区定义为一个半径为的最小安全间隔球定义为如下集合：
[0125]
p＝{x|||x-ra||＜dm}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0126]
其中，x∈r2,||
·
||表示欧氏距离，ra是冲突目标的当前位置，dm为无人机与冲突目标的距离裕度，也是飞行器的最小安全间隔半径。假设无人机感知探测半径和最小安全间隔球半径关系为rm＜＜dm，当入侵飞行器进入无人机探测范围时，无人机感知到入侵飞行器进入最小安全间隔球时，也就是径向距离小于dm时，视为发生碰撞。
[0127]
实施例7
[0128]
如表1所示，所述的碰撞规避是通过步骤(3)中nmpc算法预测的安全路径进行垂直或水平机动以进行碰撞规避。无人机碰撞规避在满足时效性要求的前提下，通过指定无人机机动行为来切割优化算法的搜索空间，无人机的碰撞规避根据垂直和水平机动解耦，也就是说，无人机在任一时刻只用单一的防撞机动，利用最大机动能力在尽可能短的时间内消除碰撞冲突。本发明定义了五种机动策略，包括最大爬升率上升、最大爬升率下降、最大转弯率左转、最大转弯率右转、转弯率和爬升率为零，五种规避机动策略构成了一个无人机规避机动策略集合e＝{u,d,l,r,s}，并且满足如表1所示。
[0129]
表1无人机规避机动策略
[0130][0131][0132]
实施例8
[0133]
如图7所示，预测时域n，可选控制序列构成一棵具有n层，5n个分支的五叉树，优化问题通过依次搜索机动策略树的分支求解，所得结果构成了(8)式的解空间，当预测时域增加时，计算量呈指数级增加，因此预测过程通过搜索算法优化。
[0134]
实施例9
[0135]
如图8所示，进行双无人机编队的防撞算法仿真，建立了一个400
×
500
×
600m3的三维空间，其中黑色球体代表目标无人机，其他球体表示编队其他无人机，仿真前先设定目标飞行器的出发坐标与目的坐标，设定无人机安全飞行距离为100m，模型预测控制算法通过多次迭代获得满足安全防撞距离的路径，迭代次数越多，得到的路径越精确，但是计算时间相应更长，为了满足无人机队实时防撞信息输出的要求，根据仿真测试，迭代次数n设置为10时，模型具有较好的防撞效果并且可以快速收敛。结果表明，本发明所提出的方法可有效计算出满足防撞条件的安全飞行路径。
[0136]
实施例10
[0137]
如图9所示，由于在实际情况中，无人机编队通常有多架飞行器，进行五无人机编队防撞算法仿真，算法选择其中距离入侵飞行器均方距离最大的路径为最优安全飞行路径，仿真结果显示该防撞方法能有效计算无碰撞路径，有效规避其他无人机。
[0138]
总之，本发明针对无人机防撞机动的一致性问题，设计了一种基于规则的防撞控制策略，利用指定的机动策略来剪切搜索空间，提高了方法的搜索效率，可以有效解决多无
人机编队防撞问题，有效提高无人机编队系统的飞行安全性，具有较大的理论研究价值和工程实践意义。
[0139]
以上所述仅为本发明的具体实施方式，不是全部的实施方式，本领域普通技术人员通过阅读本发明说明书而对本发明技术方案采取的任何等效的变换，均为本发明的权利要求所涵盖。