一种基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法

1.本发明主要涉及到无人机目标跟踪技术领域，特指一种基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法。


背景技术：

2.目标跟踪不仅是无人机执行对地观测任务的基本形式，也是后续意图分析和态势生成的基础。它可以应用于边境管控，城市安防和航拍中。与固定相机相比，借助无人机可大大扩展相机的视场，从而增强对目标的持续监控能力。此外，考虑到固定翼无人机往往比旋翼无人机具有更长的续航时间和更快的飞行速度，因此研究固定翼无人机的目标跟踪是很有意义的。
3.借助于视觉传感器，可以采用视觉伺服方法来实现目标跟踪。基于反馈信息的差异，该方法主要分为两类，分别是pbvs和ibvs方法。其中，pbvs方法需要在获得目标的特征信息后，计算目标与无人机的相对位置。基于该方法，有从业者提出了技术方案用以实现对地面车辆的跟踪和运动估计，并提供了目标位置、速度和航向的实时估计。通过设计非线性自适应观测器来估计目标的状态、参数和位置，另有从业者提出了持续激励条件下用于目标跟踪和无人机机动的制导律。进一步，还有从业者开展了固定翼无人机对地面运动目标的跟踪飞行试验。然而，由于相机标定环节往往存在较大误差，导致使用pbvs方法时会存在定位偏差，进而影响无人机跟踪精度。
4.相比之下，ibvs方法无需对无人机进行定位，而是直接在图像平面上进行控制器的设计，因此对传感器模型和标定误差具有鲁棒性。然而，ibvs方法只关注图像特征信息的运动，却不能有效地处理约束。例如，它无法考虑固定翼无人机的动力学约束和视觉传感器的可视化约束，这可能导致无法获得控制的最优解，甚至在无人机跟踪时会出现目标超出相机视场的问题。为此，考虑引入mpc来解决带约束的优化问题，从而提高ibvs方法的性能。目前，基于mpc的ibvs在无人机上的应用还很少，这主要是由于同时考虑无人机复杂的动力学模型、非线性ibvs系统以及实时性需求的难度很大。
5.综上所述，目前固定翼无人机对目标的跟踪方案主要存在以下不足之处：
6.(1)在通过机载视觉传感器获得目标的图像后，有些研究借助pbvs方法进行控制，即先对相机内参进行标定，再结合图像特征点解算出目标相对于相机的相对位置，进而在笛卡尔空间中进行跟踪控制律的设计。然而，相机标定环节往往会引入较大误差，导致定位精度存在较大偏差，进而影响跟踪精度。
7.(2)利用经典的ibvs方法实现无人机对目标的跟踪时，由于其只关注当前特征点到期望特征点的收敛，却无法处理无人机动力学约束以及相机可视化约束等，这就导致在跟踪时目标可能会超出相机视场而导致跟踪失败。
8.(3)传统的mpc优化问题中如果存在非线性硬约束，在求解时会非常耗时甚至无法获得可行解。而固定翼无人机处于高速飞行的状态，对于实时性的要求很高，若无法在线快
速求解，会导致跟踪效果较差甚至目标丢失。
9.(4)目前基于ibvs方法的无人机跟踪控制律考虑的是当前特征点到期望特征点的收敛，但当无人机距离目标较远时收敛速率不会显著提升，从而导致无人机因无法快速靠近目标而难以观测到目标的细节信息，甚至影响目标的识别率而导致跟踪失败。


技术实现要素：

10.本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种原理简单、适用范围广、处理速度快、能够提高目标跟踪效果的基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法。
11.为解决上述技术问题，本发明采用以下技术方案：
12.一种基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，其包括：
13.步骤s1：无人机通过云台相机捕获目标图像；
14.步骤s2：基于目标检测算法获得当前图像上的特征点坐标；
15.步骤s3：通过姿态补偿解决无人机姿态和云台姿态变化对特征点造成的影响，并获得虚拟特征点，再结合ibvs方法构建图像运动学模型；
16.步骤s4：构造mpc优化问题，考虑固定翼无人机的动力学约束和相机的感知约束，通过在线求解得到系统的控制律，使无人机在对目标持续跟踪的同时将其保持在图像中心附近。
17.作为本发明方法的进一步改进：所述步骤s3包括以下流程：
18.步骤s301：对图像运动学模型的构造进行描述；
19.表示机体坐标系，其中原点ob位于固定翼无人机的重心，xb轴沿着无人机的机头方向，yb轴垂直xb轴指向机体左侧，zb轴垂直机身向上；表示图像坐标系，原点oi位于图像中心；xi轴和yi轴位于图像内并分别平行于图像的宽和高；表示相机坐标系，其中原点oc位于相机光心，zc轴沿光轴方向并垂直图像，xc轴和yc轴分别平行于xi轴和yi轴；
20.二自由度的云台相机拥有偏航角θ
p
和俯仰角θ
t
；其中，θ
p
绕x
p
轴进行旋转且x
p
轴垂直于水平面s
p
，θ
t
绕x
t
轴进行旋转且x
t
轴与xc轴重合并位于水平面s
p
内；由于该云台可全向偏航，而俯仰角是受限的，因此有θ
p
∈[-π,π]，θ
t
∈[γ1,γ2]；其中且γ2》0，并注意当θ
t
＝0时zc轴位于水平面s
p
内。
[0021]
步骤s302：构建模型；
[0022]
固定翼无人机是在恒定的高度以恒定的速度飞行，记高度和速度分别为h和v
t
，无人机的偏航角、俯仰角和滚转角分别表示为ψ,θ和φ，则基于unicycle整体系统模型描述为：
[0023]
[0024]
步骤s303：对图像运动学模型进行分析；
[0025]
定义目标在和中的坐标分别为p(xc,yc,zc)和s(ui,vi)，基于与的关系并结合三角形相似原理可知：
[0026][0027]
其中f表示相机焦距；
[0028]
定义相机在中的线速度和角速度分别为vc＝[t
x
,ty,tz]
t
和ωc＝[ω
x
,ωy,ωz]
t
，则有：
[0029][0030]
结合公式，得到特征点变化率和相机速度之间的关系：
[0031][0032]
其中图像雅可比矩阵为：
[0033][0034]
作为本发明方法的进一步改进：所述步骤s4包括以下流程：
[0035]
对于参考状态，它所对应的系统姿态表示如下：
[0036][0037]
其中是个给定的常数；
[0038]
定义目标在中的坐标为(u1,v1)，同时在参考状态下的坐标为(u2,v2)，构建的ibvs模型为：
[0039][0040]
同时，给出了云台的控制输入：
[0041][0042]
定义s2＝[u2,v2]
t
，则基于公式(7)，离散的ibvs模型可以表示为：
[0043][0044]
将公式表示为：
[0045]
s2(k+1)＝fs(s2(k),u
ψ
(k))
[0046]
由于固定翼无人机的偏航角速率是受限的，则满足以下约束：
[0047]uψ
∈u
set
,u
set
＝[-u
max
,u
max
]
[0048]
为保证u
ψ
的变化是平滑的，还给出了以下的加速度约束：
[0049]
δu∈a
set
,a
set
＝[-a
max
,a
max
]
[0050]
其中：
[0051]
δu(k+i)＝u
ψ
(k+i)-u
ψ
(k+i-1)。
[0052]
作为本发明方法的进一步改进：对相机的感知约束进行处理，包括：
[0053]
先将s2所处的图像平面转换到水平面上；定义特征点在该水平面上的坐标为s
⊥
(u
⊥
,v
⊥
)，考虑到公式中s2所在的虚拟图像平面对应的角度为θ
t
＝-α，则s2与s
⊥
之间存在如下关系：
[0054][0055]
其中根据上述公式求解u
⊥
和v
⊥
的值；
[0056]
飞行高度h是恒定的，则相机的最大感知距离可等效于最大水平距离rm，当且仅当以下条件满足时，目标在相机的清晰感知范围内：
[0057][0058]
定义的参考状态中α满足以下条件：
[0059][0060]
对u
ψ
的约束，以下的关系也满足：
[0061][0062]
定义则有k时刻的感知约束表示为：
[0063]
g1(k)≤0
[0064]
其中：
[0065][0066]
定义特征点的偏差为：
[0067]
δs(k+i|k)＝s
d-s2(k+i|k)
[0068]
其中sd＝[0,0]
t
是s2对应的期望特征点，s2(k|k)＝s2(k)是k时刻的状态观测量，δu(k|k)＝u
ψ
(k|k)-u
ψ
(k-1|k-1)，则优化问题的代价函数定义为：
[0069][0070]
其中：
[0071]
jr(k+i|k)＝δs(k+i|k)
t
·qs
·
δs(k+i|k)+δu(k+i-1|k)
·qu
·
δu(k+i-1|k)
[0072]
控制序列为：
[0073]
ur＝{u
ψ
(k|k),u
ψ
(k+1|k),
…
,u
ψ
(k+n
p-1|k)}，
[0074]
为预测域，qs＝diag{q1,q2}是正定的且
[0075]
j1＝0，当且仅当对于所有i∈{1,
…
,n
p
}，有δs(k+i|k)＝0和δu(k+i-1|k)＝0成
立，也就是对于静止目标，固定翼无人机将围绕其进行盘旋。
[0076]
作为本发明方法的进一步改进：收缩约束满足：
[0077]
h1(k+n
p
+1|k)≤0,
[0078]
其中：
[0079]
h1(k+n
[
+1|k)＝jr(k+n
p
+1|k)-λ
·
jr(k+1|k),
[0080]
且λ∈(0,1)；同时，控制序列重新定义为：
[0081]
um＝{ur,u
ψ
(k+n
p
|k)}。
[0082]
作为本发明方法的进一步改进：所述mpc优化问题构建为：
[0083][0084]
s.t.
[0085]
s2(k+i|k)＝fs(s2(k+i-1|k),u
ψ
(k+i-1|k))
[0086]uψ
(k+i-1|k)∈u
set
[0087]
δu(k+i-1|k)∈a
set
[0088]
g1(k+i|k)≤0
[0089]
h1(k+n
p
+1|k)≤0
[0090]
其中i∈{1,2,
…
,n
p
+1}。
[0091]
作为本发明方法的进一步改进：将硬约束添加到代价函数中，将其转化成软约束，其流程包括：
[0092]
将约束g1(k+i|k)≤0添加到jr中得到：
[0093]jm1
(k+i|k)＝jr(k+i|k)+β1·
max{g1(k+i|k),0}
[0094]
其中为常数；
[0095]
基于此，对应的收缩约束转化成：
[0096]
h2(k+n
p
+1|k)≤0
[0097]
其中：
[0098]
h2(k+n
p
+1|k)＝j
m1
(k+n
p
+1|k)-λ
·jm1
(k+1|k)
[0099]
g1重新定义为如下形式：
[0100][0101]
其中δr》0；则公式(28)可重新表示成：
[0102]jm1
(k+i|k)＝jr(k+i|k)+β1·
max{g2(k+i|k),0}；
[0103]
其次，将约束h1(k+n
p
+1|k)添加后表示成：
[0104]jm2
(k+i|k)＝j
m1
(k+i|k)+β2·
max{h2(k+n
p
+1|k),0}
[0105]
其中为常数；
[0106]
基于此，代价函数表示成：
[0107][0108]
mpc优化问题重构为：
[0109][0110]
s.t.
[0111]
s2(k+i|k)＝fs(s2(k+i-1|k),u
ψ
(k+i-1|k))
[0112]uψ
(k+i-1|k)∈u
set
[0113]
δu(k+i-1|k)∈a
set
[0114]
其中i∈{1,2,
…
,n
p
+1}。
[0115]
作为本发明方法的进一步改进：采用基于切换的控制策略；当相对距离超过rm时，当感知约束不满足时为加快g1(k)收敛到0，定义了代价函数：
[0116][0117]
其中：
[0118]jn2
(k+i|k)＝j
n1
(k+i|k)+β2·
max{h2(k+n
p
+1|k),0}
[0119]jn1
(k+i|k)＝δs
⊥
(k+i|k)
t
·
ps·
δs
⊥
(k+i|k)+δu(k+i-1|k)
·qu
·
δu(k+i-1|k)
[0120]
ps＝diag{p1,p2}是正定的且：
[0121][0122]
其中，h2(k+n
p
+1|k)表示成：
[0123]jn1
(k+n
p
+1|k)-λ
·jn1
(k+1|k),g1(k)》0
[0124]
引入阶跃函数：
[0125][0126]
用于构建如下基于切换的mpc优化问题：
[0127][0128]
s.t.
[0129]
s2(k+i|k)＝fs(s2(k+i-1|k),u
ψ
(k+i-1|k))
[0130]uψ
(k+i-1|k)∈u
set
[0131]
δu(k+i-1|k)∈a
set
[0132]
其中i∈{1,2,
…
,n
p
+1}。
[0133]
与现有技术相比，本发明的优点就在于：
[0134]
1、本发明的一种基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，原理简单、适用范围广、处理速度快、能够提高目标跟踪效果，本发明是针对固定翼无人机和云台相机的集成系统，所提出一种基于模型预测控制(mpc)的图像视觉伺服(ibvs)方法用于实现目标跟踪。该发明考虑了相机感知能力的影响，使目标在被保持在图像中心的同时也能处于相机清晰感知的范围内，从而产生更多的图像特征点用于精确的检测。也就是说，本发明采用ibvs方法进行模型的构建。当获得目标图像后，利用图像检测算法获得目标的特征点信息，再结合无人机自身的姿态信息直接在图像平面进行控制律的设计，从而避免位置解算环节引入为误差，有助于提升目标的跟踪精度。
[0135]
2、本发明的一种基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，采用快速mpc策略，用于保证在实际应用中的实时性需求。该发明首先将mpc中的硬约束转化为软约束，使其能在迭代过程中得到可行解；再给定初始可行解来实现优化过程的热启动，从
而加速mpc问题最优解的获取。也就是说，本发明引入mpc用以提升ibvs控制器的性能。在通过ibvs构建图像的运动学模型后，引入mpc对固定翼无人机的动力学约束以及相机的可视化约束进行考虑，使得优化问题的解能够满足约束的限制，这有助于保证无人机在跟踪目标时能长时间将其保持在相机视场内，从而实现持续跟踪。
[0136]
3、本发明的基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，采用基于切换的控制策略，用于解决目标超出相机清晰感知范围后使其快速返回的问题。该发明旨在避免因目标长时间超出相机的清晰感知范围，或超出太多而导致目标丢失的情况，同时有助于减少处理器的在线运算量。。
[0137]
4、本发明的基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，能够解决固定翼无人机对地面运动目标的持续跟踪问题。本发明通过对无人机和云台相机的整体系统进行控制，使得无人机在跟踪目标的同时，能将目标保持在图像中心附近，同时避免目标因远离无人机而无法被检测到。
[0138]
5、本发明的基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，将非线性约束通过max函数将其转化成软约束添加到代价函数中，这样可以保证非线性约束不用严格满足，但由于其在代价函数中的权重较大仍需要优先满足。此外，利用max函数的优势是，当非线性约束满足时，可以不需要计算该约束的梯度，进一步减小在线计算量。
附图说明
[0139]
图1是本发明方法的控制原理示意图。
[0140]
图2是本发明在具体应用实例中无人机目标跟踪的原理示意图。
[0141]
图3是本发明在具体应用实例中云台模型的示意图。
[0142]
图4是本发明在具体应用实例中无人机感知范围示意图。
[0143]
图5是本发明在具体应用实例中的算法框图。
[0144]
图6是本发明方法的流程示意图。
具体实施方式
[0145]
以下将结合说明书附图和具体实施例对本发明做进一步详细说明。
[0146]
本发明的基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，是在已有的ibvs模型中引入mpc，以解决带约束的目标跟踪问题，并提出一种mpc策略实现在线快速求解，最后设计一种切换控制器以应对目标超出相机清晰感知范围的情况，使其快速回到该范围内并实现无人机的持续跟踪。本发明首次将快速mpc与ibvs方法的结合用于解决固定翼无人机对目标的持续跟踪问题。
[0147]
如图1和图6所示，本发明的一种基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，其流程包括：
[0148]
步骤s1：无人机先通过云台相机捕获目标图像；
[0149]
步骤s2：基于目标检测算法获得当前图像上的特征点坐标；
[0150]
步骤s3：通过姿态补偿解决无人机姿态和云台姿态变化对特征点造成的影响，并获得虚拟特征点，再结合ibvs方法构建图像运动学模型。
[0151]
步骤s4：构造mpc优化问题，考虑固定翼无人机的动力学约束和相机的感知约束，
通过在线求解得到系统的控制律，使无人机在对目标持续跟踪的同时将其保持在图像中心附近。
[0152]
在具体应用实例中，本发明的步骤s3包括以下流程：
[0153]
步骤s301：在控制器设计前，先对模型的构造进行描述。
[0154]
如图2所示，表示机体坐标系，其中原点ob位于固定翼无人机的重心，xb轴沿着无人机的机头方向，yb轴垂直xb轴指向机体左侧，zb轴垂直机身向上。表示图像坐标系，原点oi位于图像中心；xi轴和yi轴位于图像内并分别平行于图像的宽和高。表示相机坐标系，其中原点oc位于相机光心，zc轴沿光轴方向并垂直图像，xc轴和yc轴分别平行于xi轴和yi轴。
[0155]
此外，本发明中使用的是二自由度的云台相机，参见图3，二自由度的云台相机拥有偏航角θ
p
和俯仰角θ
t
。其中，θ
p
绕x
p
轴进行旋转且x
p
轴垂直于水平面s
p
，θ
t
绕x
t
轴进行旋转且x
t
轴与xc轴重合并位于水平面s
p
内。由于该云台可全向偏航，而俯仰角是受限的，因此有θ
p
∈[-π,π]，θ
t
∈[γ1,γ2]。其中且γ2》0，并注意当θ
t
＝0时zc轴位于水平面s
p
内。
[0156]
步骤s302：构建模型；
[0157]
考虑的固定翼无人机是在恒定的高度以恒定的速度飞行，记高度和速度分别为h和v
t
，无人机的偏航角、俯仰角和滚转角分别表示为ψ,θ和φ，则基于unicycle整体系统模型可以描述为：
[0158][0159]
步骤s303：对图像运动学模型进行分析；
[0160]
定义目标在和中的坐标分别为p(xc,yc,zc)和s(ui,vi)，基于图2中与的关系并结合三角形相似原理可知：
[0161][0162]
其中f表示相机焦距。进一步，需要结合相机的运动进行分析。
[0163]
定义相机在中的线速度和角速度分别为vc＝[t
x
,ty,tz]
t
和ωc＝[ω
x
,ωy,ωz]
t
，则有：
[0164][0165]
结合公式(2)和(3)，可以得到特征点变化率和相机速度之间的关系：
[0166][0167]
其中图像雅可比矩阵为：
[0168][0169]
在具体应用实例中，本发明的步骤s4包括以下流程：
[0170]
一种参考状态被提出用以解决姿态变化对特征点的影响，从而构建了特征点变化和无人机控制输入u
ψ
之间的关系。对于参考状态，它所对应的系统姿态表示如下：
[0171][0172]
其中是个给定的常数。
[0173]
定义目标在中的坐标为(u1,v1)，同时在参考状态下的坐标为(u2,v2)，构建的ibvs模型为：
[0174][0175]
同时，给出了云台的控制输入：
[0176][0177]
定义s2＝[u2,v2]
t
，则基于公式(7)，离散的ibvs模型可以表示为：
[0178][0179]
为便于后续分析，将公式(9)表示为：
[0180]
s2(k+1)＝fs(s2(k),u
ψ
(k)) (10)
[0181]
由于固定翼无人机的偏航角速率是受限的，则需满足以下约束：
[0182]uψ
∈u
set
,u
set
＝[-u
max
,u
max
] (11)
[0183]
此外，为保证u
ψ
的变化是平滑的，还给出了以下的加速度约束：
[0184]
δu∈a
set
,a
set
＝[-a
max
,a
max
] (12)
[0185]
其中：
[0186]
δu(k+i)＝u
ψ
(k+i)-u
ψ
(k+i-1)
ꢀꢀꢀꢀ
(13)
[0187]
作为较佳的实施例，本发明进一步对相机的感知约束进行考虑。实际上，当相机选定后，由于参数不变，则目标在图像上能否被清晰观测并准确识别取决于其在图像上的特征点信息。具体而言，它与相机的最大感知距离有关，将其定义为lm。假定云台的俯仰使得相机视场可以覆盖到无人机的正下方，则结合云台的全向偏航，无人机的清晰感知范围等效于如图4所示的圆锥形。
[0188]
为确定目标是否在该范围内，需要先将s2所处的图像平面转换到水平面上。定义特征点在该水平面上的坐标为s
⊥
(u
⊥
,v
⊥
)，考虑到公式中s2所在的虚拟图像平面对应的角度为θ
t
＝-α，则s2与s
⊥
之间存在如下关系：
[0189]
[0190]
其中可以根据上述公式的第三个方程求解，从而可以求解u
⊥
和v
⊥
的值。
[0191]
由于该研究中飞行高度h是恒定的，则相机的最大感知距离可等效于最大水平距离rm，如图4所示。因此，当且仅当以下条件满足时，认为目标在相机的清晰感知范围内：
[0192][0193]
此外，为保证无人机绕静止目标盘旋时目标位于清晰感知范围内，则定义的参考状态(6)中α需要满足以下条件：
[0194][0195]
同时，考虑到公式(11)中对u
ψ
的约束，以下的关系也需满足：
[0196][0197]
定义则有基于公式(15)，k时刻的感知约束可以表示为：
[0198]
g1(k)≤0 (18)
[0199]
其中：
[0200][0201]
定义特征点的偏差为：
[0202]
δs(k+i|k)＝s
d-s2(k+i|k) (20)
[0203]
其中sd＝[0,0]
t
是s2对应的期望特征点，s2(k|k)＝s2(k)是k时刻的状态观测量，δu(k|k)＝u
ψ
(k|k)-u
ψ
(k-1|k-1)，则优化问题的代价函数定义为：
[0204][0205]
其中：
[0206][0207]
控制序列为：
[0208]
ur＝{u
ψ
(k|k),u
ψ
(k+1|k),
…
,u
ψ
(k+n
p-1|k)}， (23)
[0209]
为预测域，qs＝diag{q1,q2}是正定的且
[0210]
注意到j1＝0当且仅当对于所有i∈{1,
…
,n
p
}，有δs(k+i|k)＝0和δu(k+i-1|k)＝0成立，也就是对于静止目标，固定翼无人机将围绕其进行盘旋。
[0211]
此外，为确保系统的稳定性，以下的收缩约束也是必要的：
[0212]
h1(k+n
p
+1|k)≤0, (24)
[0213]
其中：
[0214]
h1(k+n
p
+1|k)＝jr(k+n
p
+1|k)-λ
·
jr(k+1|k), (25)
[0215]
且λ∈(0,1)。同时，控制序列重新定义为
[0216]
um＝{ur,u
ψ
(k+n
p
|k)}。 (26)
[0217]
因此，基于公式(10)，(11)，(12)，(18)，(21)和(24)，mpc优化问题可构建为：
[0218][0219]
s.t.
[0220]
s2(k+i|k)＝fs(s2(k+i-1|k),u
ψ
(k+i-1|k))
[0221]uψ
(k+i-1|k)∈u
set
[0222]
δu(k+i-1|k)∈a
set
[0223]
g1(k+i|k)≤0
[0224]
h1(k+n
p
+1|k)≤0
[0225]
其中i∈{1,2,
…
,n
p
+1}。
[0226]
作为较佳的实施例，本发明进一步采用快速mpc策略。这是由于随着n
p
的增加并受限于处理器的计算性能，在线解决上述带非线性硬约束g1(k+i|k)≤0和h1(k+n
p
+1|k)的优化问题是很耗时的，甚至可能会导致在预设时间内无法得到可行解的问题。为解决该问题，本发明将硬约束添加到代价函数中，从而将其转化成软约束，其流程包括：
[0227]
将约束g1(k+i|k)≤0添加到jr中可以得到：
[0228]jm1
(k+i|k)＝jr(k+i|k)+β1·
max{g1(k+i|k),0} (28)
[0229]
其中为常数。
[0230]
基于此，公式对应的收缩约束转化成：
[0231]
h2(k+n
p
+1|k)≤0 (29)
[0232]
其中：
[0233]
h2(k+n
p
+1|k)＝j
m1
(k+n
p
+1|k)-λ
·jm1
(k+1|k) (30)
[0234]
进一步，考虑到u
ψ
(k+i-1|k)∈u
set
和δu(k+i-1|k)∈a
set
对应的无人机动力学约束以及目标的运动是未知的，当g1(k)》0时可能无法使g1(k+1)≤0。因此，公式(19)中的g1重新定义为如下形式用于使公式(28)中的β1·
max{
·
}更快起作用。
[0235][0236]
其中δr》0。则公式(28)可重新表示成：
[0237]jm1
(k+i|k)＝jr(k+i|k)+β1·
max{g2(k+i|k),0}。 (32)
[0238]
其次，将约束h1(k+n
p
+1|k)添加到(32)中可表示成：
[0239]jm2
(k+i|k)＝j
m1
(k+i|k)+β2·
max{h2(k+n
p
+1|k),0} (33)
[0240]
其中为常数。
[0241]
基于此，代价函数可以表示成：
[0242][0243]
与硬约束中引入松弛变量的软约束法相比，本发明中使用的max函数更加高效。这是因为当g(k+i|k)≤0时，该非线性函数不参与内点法每次迭代的梯度求解。然而，在使用松弛变量时，该函数每次的梯度都需要被求解，从而更加耗时。
[0244]
综上，mpc优化问题可以重构为：
[0245][0246]
s.t.
[0247]
s2(k+i|k)＝fs(s2(k+i-1|k),u
ψ
(k+i-1|k))
[0248]uψ
(k+i-1|k)∈u
set
[0249]
δu(k+i-1|k)∈a
set
[0250]
其中i∈{1,2,
…
,n
p
+1}。
[0251]
此外，本发明中还引入软约束法进行加速。无人机的控制输入可以通过公式(10)并结合最小二乘法进行求解，并将求得的值记为注意到则初始控制序列可以表示为：
[0252][0253]
可以发现，u0可以轻易地满足不等式约束u
ψ
(k+i-1|k)∈u
set
和δu(k+i-1|k)∈a
set
。这表明u0可以作为优化问题的初始可行解。进一步，记k时刻的最优解为u
*
(k)，则k+1时刻的可行解u(k+1)可定义为：
[0254][0255]
作为较佳的实施例，本发明进一步采用基于切换的控制策略。这是因为随着目标速度加快，导致其在远离无人机的过程时可能超出相机的清晰感知范围，进而使得其细节无法被观测到，甚至影响目标的识别准确性。实际上，代价函数j2中的β1·
max{g2(k+i|k),0}(i＝1,
…
,n
p
+1)有助于避免相对距离超过rm。然而，优化问题(35)仍然更关心s2的收敛。而且，关于(u2,v2)和(u
⊥
,v
⊥
)的梯度都需要被在线计算，这也增加了计算的负载。为解决该问题，本发明提出了一种基于切换的控制方法。当相对距离不超过rm时，考虑优化问题(35)用于使无人机绕目标进行盘旋跟踪。然而，当相对距离超过rm时，考虑另一种优化问题用于使其快速回到相机清晰的感知范围内。
[0256]
根据公式(15)，可以发现感知约束直接取决于(u
⊥
,v
⊥
)。因此，当感知约束不满足时为加快g1(k)收敛到0，定义了如下新的代价函数：
[0257][0258]
其中：
[0259]jn2
(k+i|k)＝j
n1
(k+i|k)+β2·
max{h2(k+n
p
+1|k),0} (39)
[0260]jn1
(k+i|k)＝δs
⊥
(k+i|k)
t
·
ps·
δs
⊥
(k+i|k)+δu(k+i-1|k)
·qu
·
δu(k+i-1|k)(40)
[0261]
ps＝diag{p1,p2}是正定的且：
[0262][0263]
注意到公式(39)中的h2(k+n
p
+1|k)不同于公式(30)中的，它表示成：
[0264]jn1
(k+n
p
+1|k)-λ
·jn1
(k+1|k),g1(k)》0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(42)
[0265]
接着，引入阶跃函数：
[0266][0267]
用于构建如下基于切换的mpc优化问题：
[0268]
[0269]
s.t.
[0270]
s2(k+i|k)＝fs(s2(k+i-1|k),u
ψ
(k+i-1|k))
[0271]uψ
(k+i-1|k)∈u
set
[0272]
δu(k+i-1|k)∈a
set
[0273]
其中i∈{1,2,
…
,n
p
+1}。
[0274]
注意到公式(38)中的代价函数是一种基于状态的切换函数，与g1(k)的符号相关。因此，对于i∈{1,2,
…
,n
p
+1}，当且仅当δs
⊥
(k+i|k)＝0且δu(k+i-1|k)＝0时代价函数(38)取得最小值0。在具体应用过程中的算法如图5所示。
[0275]
在具体应用过程中，本发明进一步对于公式(44)对应的基于切换的mpc优化问题，为分析其稳定性，需要将其分成三步，分别对应于g1(k)≤0，g1(k)》0以及切换过程。接下来，将采用李雅普诺夫方法证明其稳定性，其中李雅普诺夫函数即为公式对应的代价函数。
[0276]
受限，当h2(k+n
p
+1|k)》0时希望其梯度能快速下降。为此，参数β2需要设置得足够大使其满足β2＞＞max{q1,q2,p1,p2,qu,β1}。此时，一旦h2(k+n
p
+1|k)≤0无法满足，公式(44)对应的代价函数就等效于β2·
h2(k+n
p
+1|k)，通过梯度下降使其快速满足该条件。
[0277]
接下来，当h2(k+n
p
+1|k)≤0时，闭环系统的稳定性证明如下：
[0278]
(1)g1(k)≤0；
[0279]
根据公式(7)对应的状态方程模型，存在以下的关系：
[0280]
s2(k+i|k+1)＝s2(k+i|k),i＝1,
…
,n
p
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(45)
[0281]
定义k时刻的最优控制序列为：
[0282][0283]
其对应的最优代价定义为则基于公式(38)得到的可行控制序列的代价为
[0284][0285]
由于当且仅当s2(k+1|k)＝sd且δu(k+1|k)＝0。因此，结合公式(29)，(30)和(47)后，当时，有：
[0286][0287]
由于k+1时刻的最优代价一定不超过因此可推导出：
[0288][0289]
(2)g1(k)》0；
[0290]
相比于g1(k)≤0的情况，此时的差别只有代价函数的不同，因此证明过程与上面类似。参照公式(48)，当时，有：
[0291]
[0292]
而后可以得到：
[0293][0294]
(3)切换过程；
[0295]
为分析该阶段闭环系统的稳定性，令公式(46)所对应的最优代价满足以下条件：
[0296][0297]
而后，k时刻的最优代价可以表示成：
[0298][0299]
同时，基于公式(37)得到的k+1时刻的可行控制序列的代价表示成：
[0300][0301]
根据公式(14)，(19)，(28)和(40)，可得：
[0302][0303]
基于公式(6)中的参考状态的定义，当初始时刻目标位于无人机右侧时，基于公式(35)得到的控制输入将使目标保持在无人机右侧。即，在切换过程时满足v
⊥
≤0，则v2对应的范围为此时，有以下关系成立：
[0304]f·
sinα-v
⊥
(k)
·
cosα≥f
·
sinα
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(56)
[0305][0306]
相关参数设置如下：
[0307][0308]
其中：
[0309][0310]
结合公式(55)，(56)，(56)和(58)，可得：
[0311][0312]
进一步，公式(54)中的可以写成：
[0313][0314]
类似的，可以得到：
[0315][0316]
综上，基于公式(49)，(51)和(62)，可以证明由本发明方法所形成的控制系统的闭环是渐近稳定的。
[0317]
通过采用本发明的上述方法，即基于mpc的ibvs方法，对固定翼无人机和云台的整体系统进行控制，使无人机在对目标持续跟踪的同时，将其保持在图像中心附近。该方法针对经典的ibvs方法无法考虑约束的问题，引入mpc方法考虑固定翼无人机的速度约束，加速度约束以及相机的感知约束，使得无人机在跟踪时能将目标保持在相机的清晰感知范围内，从而在图像上映射更多的特征信息，有助于目标细节的观测以及提高识别的准确性。
[0318]
针对带非线性硬约束的mpc优化问题，通过采用本发明的上述方法，采用基于软约束和热启动的快速mpc策略，使其可以在线快速求解，以满足固定翼无人机对目标跟踪的实时性需求。本发明的方法考虑到相机的感知约束以及收缩约束是非线性的，在作为硬约束进行求解时比较耗时且可能存在不可行解的问题，引入max函数将这些约束作为软约束添加到代价函数中，从而保证解的存在性以及求解的快速性。
[0319]
通过采用本发明的上述方法，利用基于切换的mpc控制策略，使得目标超出相机清晰感知范围时能快速回到该范围内。由于初始所提的mpc控制器旨在实现无人机对目标的盘旋跟踪，当目标超出该范围时无法快速靠近，同时所设计的控制器计算量也会增大。切换控制器的引入则有助于减少此时的在线计算量，并加快使目标返回该范围内。此外，通过分析证明了该切换控制器的稳定性。
[0320]
以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

技术特征：
1.一种基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，其特征在于，包括：步骤s1：无人机通过云台相机捕获目标图像；步骤s2：基于目标检测算法获得当前图像上的特征点坐标；步骤s3：通过姿态补偿解决无人机姿态和云台姿态变化对特征点造成的影响，并获得虚拟特征点，再结合ibvs方法构建图像运动学模型；步骤s4：构造mpc优化问题，考虑固定翼无人机的动力学约束和相机的感知约束，通过在线求解得到系统的控制律，使无人机在对目标持续跟踪的同时将其保持在图像中心附近。2.根据权利要求1所述的基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，其特征在于，所述步骤s3包括以下流程：步骤s301：对图像运动学模型的构造进行描述；表示机体坐标系，其中原点o
b
位于固定翼无人机的重心，x
b
轴沿着无人机的机头方向，y
b
轴垂直x
b
轴指向机体左侧，z
b
轴垂直机身向上；表示图像坐标系，原点o
i
位于图像中心；x
i
轴和y
i
轴位于图像内并分别平行于图像的宽和高表示相机坐标系，其中原点o
c
位于相机光心，z
c
轴沿光轴方向并垂直图像，x
c
轴和y
c
轴分别平行于x
i
轴和y
i
轴；二自由度的云台相机拥有偏航角θ
p
和俯仰角θ
t
；其中，θ
p
绕x
p
轴进行旋转且x
p
轴垂直于水平面s
p
，θ
t
绕x
t
轴进行旋转且x
t
轴与x
c
轴重合并位于水平面s
p
内；由于该云台可全向偏航，而俯仰角是受限的，因此有θ
p
∈[-π，π]，θ
t
∈[γ1，γ2]；其中且γ2＞0，并注意当θ
t
＝0时z
c
轴位于水平面s
p
内；步骤s302：构建模型；固定翼无人机是在恒定的高度以恒定的速度飞行，记高度和速度分别为h和v
t
，无人机的偏航角、俯仰角和滚转角分别表示为ψ，θ和φ，则基于unicycle整体系统模型描述为：步骤s303：对图像运动学模型进行分析；定义目标在和中的坐标分别为p(x
c
，y
c
，z
c
)和s(u
i
，v
i
)，基于与的关系并结合三角形相似原理可知：其中f表示相机焦距；定义相机在中的线速度和角速度分别为v
c
＝[t
x
，t
y
，t
z
]
t
和ω
c
＝[ω
x
，ω
y
，ω
z
]
t
，则有：
结合公式，得到特征点变化率和相机速度之间的关系：其中图像雅可比矩阵为：3.根据权利要求1所述的基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，其特征在于，所述步骤s4包括以下流程：对于参考状态，它所对应的系统姿态表示如下：θ
t
＝-α，θ＝0，φ＝0其中是个给定的常数；定义目标在中的坐标为(u1，v1)，同时在参考状态下的坐标为(u2，v2)，构建的ibvs模型为：同时，给出了云台的控制输入：定义s2＝[u2，v2]
t
，离散的ibvs模型可以表示为：将公式表示为：s2(k+1)＝f
s
(s2(k)，u
ψ
(k))由于固定翼无人机的偏航角速率是受限的，则满足以下约束：u
ψ
∈u
set
，u
set
＝[-u
max
，u
max
]为保证u
ψ
的变化是平滑的，还给出了以下的加速度约束：δu∈a
set
，a
set
＝[-a
max
，a
max
]其中：δu(k+i)＝u
ψ
(k+i)-u
ψ
(k+i-1)。4.根据权利要求3所述的基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，其特征在于，对相机的感知约束进行处理，包括：先将s2所处的图像平面转换到水平面上；定义特征点在该水平面上的坐标为s
⊥
(u
⊥
，v
⊥
)，考虑到公式中s2所在的虚拟图像平面对应的角度为θ
t
＝-α，则s2与s
⊥
之间存在如下关
系：其中根据上述公式求解u
⊥
和v
⊥
的值；飞行高度h是恒定的，则相机的最大感知距离可等效于最大水平距离r
m
，当且仅当以下条件满足时，目标在相机的清晰感知范围内：定义的参考状态中α满足以下条件：对u
ψ
的约束，以下的关系也满足：定义则有k时刻的感知约束表示为：g1(k)≤0其中：定义特征点的偏差为：δs(k+i|k)＝s
d-s2(k+i|k)其中s
d
＝[0，0]
t
是s2对应的期望特征点，s2(k|k)＝s2(k)是k时刻的状态观测量，δu(k|k)＝u
ψ
(k|k)-u
ψ
(k-1|k-1)，则优化问题的代价函数定义为：其中：j
r
(k+i|k)＝δs(k+i|k)
t
·
q
s
·
δs(k+i|k)+δu(k+i-1|k)
·
q
u
·
δu(k+i-1|k)控制序列为：u
r
＝{u
ψ
(k|k)，u
ψ
(k+1|k)，
…
，u
ψ
(k+n
p-1|k)}，为预测域，q
s
＝diag{q1，q2}是正定的且j1＝0，当且仅当对于所有i∈{1，
…
，n
p
}，有δs(k+i|k)＝0和δu(k+i-1|k)＝0成立，也就是对于静止目标，固定翼无人机将围绕其进行盘旋。5.根据权利要求4所述的基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，其特征在于，收缩约束满足：h1(k+n
p
+1|k)≤0，其中：h1(k+n
p
+1|k)＝j
r
(k+n
p
+1|k)-λ
·
j
r
(k+1|k)，且λ∈(0，1)；同时，控制序列重新定义为：u
m
＝{u
r
，u
ψ
(k+n
p
|k)}。
6.根据权利要求5所述的基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，其特征在于，所述mpc优化问题构建为：s.t.s2(k+i|k)＝f
s
(s2(k+i-1|k)，u
ψ
(k+i-1|k))u
ψ
(k+i-1|k)∈u
set
δu(k+i-1|k)∈a
set
g1(k+i|k)≤0h1(k+n
p
+1|k)≤0其中i∈{1，2，
…
，n
p
+1}。7.根据权利要求6所述的基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，其特征在于，将硬约束添加到代价函数中，将其转化成软约束，其流程包括：将约束g1(k+i|k)≤0添加到j
r
中得到：j
m1
(k+i|k)＝j
r
(k+i|k)+β1·
max{g1(k+i|k)，0}其中为常数；基于此，对应的收缩约束转化成：h2(k+n
p
+1|k)≤0其中：h2(k+n
p
+1|k)＝j
m1
(k+n
p
+1|k)-λ
·
j
m1
(k+1|k)g1重新定义为如下形式：其中δr＞0；则公式(28)可重新表示成：j
m1
(k+i|k)＝j
r
(k+i|k)+β1·
max{g2(k+i|k)，0}；其次，将约束h1(k+n
p
+1|k)添加后表示成：j
m2
(k+i|k)＝j
m1
(k+i|k)+β2·
max{h2(k+n
p
+1|k)，0}其中为常数；基于此，代价函数表示成：mpc优化问题重构为：s.t.s2(k+i|k)＝f
s
(s2(k+i-1|k)，u
ψ
(k+i-1|k))u
ψ
(k+i-1|k)∈u
set
δu(k+i-1|k)∈a
set
其中i∈{1，2，
…
，n
p
+1}。8.根据权利要求7所述的基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，其特征在于，采用基于切换的控制策略；当相对距离超过r
m
时，当感知约束不满足时为加快
g1(k)收敛到0，定义了代价函数：其中：j
n2
(k+i|k)＝j
n1
(k+i|k)+β2·
max{h2(k+n
p
+1|k)，0}j
n1
(k+i|k)＝δs
⊥
(k+i|k)
t
·
p
s
·
δs
⊥
(k+i|k)+δu(k+i-1|k)
·
q
u
·
δu(k+i-1|k)p
s
＝diag{p1，p2}是正定的且：其中，h2(k+n
p
+1|k)表示成：j
n1
(k+n
p
+1|k)-λ
·
j
n1
(k+1|k)，g1(k)＞0引入阶跃函数：用于构建如下基于切换的mpc优化问题：s.t.s2(k+i|k)＝f
s
(s2(k+i-1|k)，u
ψ
(k+i-1|k))u
ψ
(k+i-1|k)∈u
set
δu(k+i-1|k)∈a
set
其中i∈{1，2，
…
，n
p
+1}。

技术总结
本发明公开了一种基于快速视觉伺服预测控制的固定翼无人机目标跟踪方法，其包括：步骤S1：无人机通过云台相机捕获目标图像；步骤S2：基于目标检测算法获得当前图像上的特征点坐标；步骤S3：通过姿态补偿解决无人机姿态和云台姿态变化对特征点造成的影响，并获得虚拟特征点，再结合IBVS方法构建图像运动学模型；步骤S4：构造MPC优化问题，考虑固定翼无人机的动力学约束和相机的感知约束，通过在线求解得到系统的控制律，使无人机在对目标持续跟踪的同时将其保持在图像中心附近。本发明具有原理简单、适用范围广、处理速度快、能够提高目标跟踪效果等优点。踪效果等优点。踪效果等优点。


技术研发人员：王祥科 杨凌杰 刘志宏 丛一睿 俞先国 王冠政 胡新雨 刘云昊 李晓信
受保护的技术使用者：中国人民解放军国防科技大学
技术研发日：2023.06.30
技术公布日：2023/10/15