基于贝塞尔曲线二次优化的固定翼目标跟踪方法

1.本发明涉及无人机技术领域的一种固定翼跟踪目标的规划控制方法，尤其是涉及了一种固定翼无人机跟踪目标时生成预测运动轨迹的规划进而实现目标跟踪的方法，主要涉及到对贝塞尔曲线的二次优化以及固定翼无人机运动轨迹的生成。


背景技术：

2.固定翼飞行速度较快的情况下要实现快速跟踪难度较大容易产生目标丢失等问题。需要对轨迹进行预测，并生成符合固定翼运动的轨迹，用于更快跟踪目标，减少丢失概率。由于目前对于固定翼目标追击的研究比较少，未查找到相关研究数据。
3.国内有基于安全走廊对贝塞尔曲线进行约束，优化轨迹找到无碰撞路径的应用。但主要用于四旋翼，对固定翼的轨迹优化并没有涉及，关于固定翼快速跟踪目标时，对其运动轨迹进行优化的研究较少。


技术实现要素：

4.为了解决背景技术中存在的问题，本发明提出了一种基于贝塞尔曲线二次优化的固定翼目标跟踪方法，解决了固定翼飞行速度快而且受到动力学约束、在跟踪目标时容易丢失目标的技术问题。
5.为达到上述目的，本发明技术方案包括以下步骤：
6.s1、固定翼上实时观测目标的当前位置作为观测目标位置，将观测目标位置及其对应的时间戳信息组成数据存储；时间戳即时刻。
7.s2、根据贝塞尔曲线建立损失函数，进而建立损失函数的约束；
8.s3、对s1获得的历史所有时刻的观测目标位置及其时刻的数据输入到损失函数中，以最小化损失函数为目标进行求解获得最优贝塞尔曲线的参数，进而确定最优贝塞尔曲线作为初步预测跟踪轨迹；
9.s4、然后在初步预测跟踪轨迹上按照时刻顺序确定下一时刻的点作为预测目标位置为目标点，在实时获得的观测目标位置到预测目标位置之间，以固定翼的最小转弯半径为约束通过dubins曲线建立固定翼的最终预设轨迹，进而控制固定翼飞行对目标进行跟踪。
10.所述的固定翼上安装有云台，云台上安装相机，相机拍摄目标的图像进而图像分析处理后实现对目标位置的获取。
11.本发明是在观测目标位置基础上贝塞尔曲线拟合建立并获得轨迹，进而以时刻代入获得预测目标位置。
12.所述的损失函数具体如下：
13.[0014][0015][0016]
其中，j
pre
表预测目标与实际目标位置的损失值，表示二范数的平方，b(ti)为第i个时刻ti的预测目标位置，为第i个时刻ti的观测目标位置，为第i个时刻ti的权重，ω
p
为二阶正交项权重，用于避免过拟合；t1表示初始的第1个时刻，t
l
表示当前时刻，l为到当前时刻为止的时间长度，i表示时刻的序号；为t时刻的贝塞尔曲线系数，cu为贝塞尔曲线的第u个控制点的位置坐标，u表示贝塞尔曲线的控制点的序号，n表示贝塞尔曲线的控制点的总数；为第i个时刻ti的贝塞尔曲线系数，b(t)为t时刻的预测目标位置；b
(2)
(t)表示预测目标位置b(t)的二阶导数，其中为二次正交项。
[0017]
本实施例采用五阶贝塞尔曲线，n为5，五阶贝塞尔示意图如图2所示。
[0018]
所述的损失函数中的第i个时刻ti的权重具体设置为：
[0019][0020]
其中，t
l
为当前时刻，ti为历史的第i个时刻，k
t
表示预设的时间权重系数。由上述公式可知，随着时间推移t
l-ti的值越来越大，对应的时刻的权重越来越低。
[0021]
本发明在目标函数中使用双切曲线设计权重，使得权重随时间递减，观测到的时刻越靠前在损失函数中权重越低，进而优化不同时刻间的观测目标位置的置信度，实现准确的目标跟踪。
[0022]
本发明在损失函数中加入了设计的二阶正交项，能够避免过拟合，使目标轨迹和观测距离差最小。
[0023]
本发明以双切曲线设计的权重以及观测目标位置与贝塞尔曲线生成的预测目标位置之间的差值下的构思建立损失函数，且加入二阶正交项避免过拟合，能够更优地跟踪目标，实现准确跟踪。
[0024]
所述损失函数中的约束包括预测速度和加速度约束[-v
mp
，v
mp
]，[-a
mp
，a
mp
]，有预测速度和加速度约束建立为不等式约束：
[0025]-v
mp
≤n
·
(c
u-c
u-1
)≤v
mp
[0026]-a
mp
≤n
·
(n-1)
·
(c
u-2c
u-1
+c
u-2
)/s
t
≤a
mp
[0027]
其中，n是贝塞尔曲线的阶次，s
t
是时间尺度参数，cu为贝塞尔曲线的第u个控制点位置坐标，v
mp
表示目标的预测速度约束，a
mp
表示目标的预测加速度约束。
[0028]
将贝塞尔曲线被包裹在其控制点连成的凸包中，本发明利用贝塞尔曲线的凸包性质来约束控制点的位置；对贝塞尔曲线求导，其还是一个贝塞尔曲线，本发明利用贝塞尔曲线的导数属性将控制点用低阶贝塞尔曲线的控制点线性表示。
[0029]
本发明据贝塞尔曲线的凸包属性和导数属性加入预测速度与加速度约束的不等式约束，使目标轨迹可行。
[0030]
所述步骤s3中，贝塞尔曲线的二范数正定，将损失函数及其约束看作是带约束的二次优化问题，采用ooqp开源求解器求解损失函数及其约束，获得贝塞尔曲线中的每个控制点，最优的初步预测跟踪轨迹。
[0031]
贝塞尔曲线二范数正定，本发明二次优化损失函数，计算出使观测目标位置与预测目标位置之间差值最小的最优解，获得初步预测跟踪轨迹。
[0032]
所述步骤s4中，具体实施通过设置dubins路径计算模块，根据固定翼的转弯半径区间[rmin，rmax]以及预测目标位置生成到达目标的最短固定翼运动轨迹作为最终预设轨迹。
[0033]
本发明的有益效果是：
[0034]
本发明由轨迹跟踪目标可提高固定翼的追击速度，同时在目标丢失时，可按预测轨迹继续前进，重新寻找目标，大大减少了目标跟踪速度，提高了固定翼跟踪目标的速度减少了目标丢失的可能性。
附图说明
[0035]
构成本技术的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
[0036]
图1为本发明的架构示意图；
[0037]
图2为贝塞尔曲线示意图；
[0038]
图3为预测轨迹示意图；
[0039]
图4为ros仿真效果图。
具体实施方式
[0040]
为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施方式和附图，对本发明做进一步详细说明。在此，本发明的示意性实施方式及其说明用于解释本发明，但并不作为对本发明的限定。
[0041]
如图1所示，本发明的实施例及其实施过程如下：
[0042]
s1、固定翼上实时观测目标的当前位置作为观测目标位置，将观测目标位置及其对应的时间戳信息组成数据存储；时间戳即时刻。
[0043]
固定翼上安装有云台，云台上安装相机，相机拍摄目标的图像进而图像分析处理后实现对目标位置的获取。
[0044]
s2、根据贝塞尔曲线建立损失函数，进而建立损失函数的约束；
[0045]
损失函数具体如下：
[0046][0047]
[0048][0049]
其中，j
pre
表预测目标位置与实际目标位置的损失值，表示二范数的平方，b(ti)为第i个时刻ti的预测目标位置，为第i个时刻ti的观测目标位置，为第i个时刻ti的权重，ω
p
为二阶正交项权重，用于避免过拟合；t1表示初始的第1个时刻，t
l
表示当前时刻，l为到当前时刻为止的时间长度，i表示时刻的序号；为t时刻的贝塞尔曲线系数，cu为贝塞尔曲线的第u个控制点位置坐标，u表示贝塞尔曲线的控制点的序号，n表示贝塞尔曲线的控制点的总数；为第i个时刻ti的贝塞尔曲线系数，b(t)为t时刻的预测目标位置；b
(2)
(t)表示预测目标位置b(t)的二阶导数，其中为二次正交项。
[0050]
本实施例采用五阶贝塞尔曲线，n为5，五阶贝塞尔示意图如图2所示。
[0051]
损失函数中的第i个时刻ti的权重具体设置为：
[0052][0053]
其中，t
l
为当前时刻，ti为历史的第i个时刻，k
t
表示预设的时间权重系数。由上述公式可知，随着时间推移t
l-ti的值越来越大，对应的时刻的权重越来越低。
[0054]
损失函数中的约束包括预测速度和加速度约束[-v
mp
，v
mp
]，[-a
mp
，a
mp
]，有预测速度和加速度约束建立为不等式约束：
[0055]-v
mp
≤n
·
(c
u-c
u-1
)≤v
mp
[0056]-a
mp
≤n
·
(n-1)
·
(c
u-2c
u-1
+c
u-2
)/s
t
≤a
mp
[0057]
其中，n是贝塞尔曲线的阶次，s
t
是时间尺度参数，cu为贝塞尔曲线的第u个控制点位置坐标，v
mp
表示目标的预测速度约束，a
mp
表示目标的预测加速度约束。
[0058]
s3、对s1获得的历史所有时刻的观测目标位置及其时刻的数据输入到损失函数中，以最小化损失函数为目标进行求解获得最优贝塞尔曲线的参数，进而确定最优贝塞尔曲线作为初步预测跟踪轨迹；
[0059]
贝塞尔曲线的二范数正定，将损失函数及其约束看作是带约束的二次优化问题问题，采用ooqp开源求解器求解损失函数及其约束，获得贝塞尔曲线中的每个控制点，最优的初步预测跟踪轨迹。
[0060]
s4、然后在初步预测跟踪轨迹上按照时刻顺序确定下一时刻的点作为预测目标位置为目标点，在实时获得的观测目标位置到预测目标位置之间，以固定翼的最小转弯半径为约束通过dubins曲线建立固定翼的最终预设轨迹，进而控制固定翼飞行对目标进行跟踪。
[0061]
具体实施通过设置dubins路径计算模块，根据固定翼的转弯半径区间[rmin，rmax]以及预测目标位置生成到达目标的最短固定翼运动轨迹作为最终预设轨迹。
[0062]
如图3所示，实线部分为过去观测的目标位置曲线，虚线部分为预测的目标位置曲
线。
[0063]
如图4所示为ros操作系统中的仿真效果图，小汽车为被跟踪目标，固定翼飞机为本发明用于跟踪的固定翼无人机，粗曲线为预测目标运动曲线，细曲线为基于预测目标位置生成的固定翼预测运动轨迹。
[0064]
本发明针对固定翼无人机受到动力学约束在目标跟踪时不灵活的缺点，设计了一种基于贝塞尔曲线二次优化的目标跟踪方法，获得了目标未来一段时间的初步预测跟踪轨迹，在初步预测跟踪轨迹上确定下一时刻预测目标位置，在实时观测目标位置到预测目标位置之间，通过dubins曲线建立最终预设轨迹，固定翼无人机提前获得运动轨迹，提高跟踪速度，大大减少了固定翼无人机目标跟踪时目标丢失的概率。