一种飞行器高精度高动态自主拦截/碰撞的方法

本发明涉及飞行器自主拦截/碰撞的关键技术，包括基于图像的视觉伺服、目标估计、延迟卡尔曼滤波、自主拦截、自主碰撞、反无人机系统。特别是引入了基于旋转矩阵的控制方法和延迟卡尔曼滤波估计方法，以解决飞行器拦截/碰撞任务中的自主控制和目标估计问题。这涵盖了基于图像的视觉伺服、目标估计、延迟卡尔曼滤波、自主拦截、自主碰撞、反无人机系统等，旨在提高自主飞行器拦截/碰撞任务的成功率。

背景技术：

1、近年来，无人机，通俗地称为飞行器，在监视、侦察、交付和娱乐等各个领域迅猛发展。然而，这种扩张也凸显了对有效对策的紧迫需求，以应对未经授权或恶意飞行器操作可能带来的潜在安全威胁。针对这一不断增加的担忧，飞行器拦截/碰撞的概念在航空航天防御战略领域变得至关重要。

2、无人机拦截器可分为无线电频率干扰和硬杀伤敌方无人机。然而，由于全自动无人机的体积小、灵活性高，使用地面设备快速部署和拦截/碰撞入侵者是一项具有挑战性的任务。解决这个问题的一个方法是使用依赖于机载传感器的拦截无人机来探测并迫使入侵者降落。

3、在此前的工作中，我们开发了一种基于图像的前向单目相机的视觉伺服(ibvs)算法，并应用于5m/s的真实飞行实验。但是，由于模型采用了小角度近似，不适用于高精度高动态自主拦截/碰撞任务。多旋翼的高速移动暴露了许多以前被忽视的问题：相机成像和图像处理所带来的约100毫秒的延迟将不容忽视。例如，在20m/s的速度下，100毫秒的延迟会导致2米的控制误差，这使得图像处理完美的常见假设不再有效。此外，该相机的成像频率不满足控制要求。与控制器频率相比，较低的反馈频率会使系统的不稳定。为了解决高精度高动态自主拦截/碰撞问题，本发明提出了两个主要的挑战：

4、1.拦截器的机动性和高速度需要建立一个完整的姿态模型，并重新设计控制器，以满足拦截器在大的状态空间中的运动。旋转矩阵在三维特殊正交群上是连续的，记为so(3)。因此，需要在so(3)上开发控制器来执行高精度高动态拦截/碰撞控制任务。

5、2.在拦截器的高速运动过程中，延迟、低帧率和容易丢失的图像反馈会导致较大的拦截

6、/碰撞误差。因此，有必要实时估计目标在图像上的正确位置。

7、综合考虑这些挑战，成功实现飞行器拦截/碰撞任务需要集中关注如何优化控制器及确定其稳定性。这涉及到计算机视觉、机器学习、优化算法和自主控制等多个领域的深入研究，以确保拦截/碰撞任务的高效和成功执行。

技术实现思路

1、本发明提出了一种拦截/碰撞方案，用于应对高速移动的入侵目标，完善了照相机成像和图像处理所带来的延迟问题，从而提高了综合成功率。本发明引入了基于旋转矩阵的高精度高动态自主拦截/碰撞控制，作为支持拦截/碰撞的关键技术。本发明验证了算法在机载感知和控制下的有效性，巩固了其在实际应用中的可行性。这些贡献共同推动了高精度高动态自主拦截/碰撞技术的发展，为有效应对高速目标入侵威胁提供了重要的工具和方法。

2、本发明首先介绍与本发明相关的飞行器的六自由度刚体模型和相机成像模型。利用这些模型，本发明对高精度高动态自主拦截/碰撞控制问题进行了建模。此外，本发明还提出了imu测量模型和图像延迟测量模型，并进行了延迟状态估计问题的研究。

3、坐标系：对于拦截/碰撞飞行器，本发明使用了四个坐标系，如图1所示，包括：

4、·{e}＝{oe-xeyeze}是地球固联坐标系(efcs)，用于表示全局惯性系统中拦截器和目标的位置和速度；

5、·{b}＝{ob-xbybzb}是机体坐标系(bcs)，用于表示拦截器当前姿态的变量；

6、·{c}＝{oc-xcyczc}是相机坐标系(ccs)；

7、·{i}＝{oi-xiyi}是图像坐标系(ics)，用于表示在相机的第一人称视图中的目标特征的图像位置；

8、矩阵表示从坐标系到的旋转矩阵，其中三维特殊正交群由下式给出：

9、

10、飞行器模型：多旋翼无人机(拦截器)被建模为一个质量为m的刚体，其阻力与其速度成正比。将其飞行控制刚性模型总结为：

11、

12、其中，表示飞行器在efcs下的位置；表示efcs下的速度矢量；是efcs下的加速度；m是飞行器的质量；g是本地重力加速度，g＝[0 0 g]t；是飞行器在efcs中的可控力；是efcs中多旋翼的空气动力阻力，d＝diag(dx,dy,dz)是一个常数对角矩阵，定义了阻力系数；是bcs下的角速度；j是多旋翼惯性矩；ga是陀螺转矩；τ是由螺旋桨在机体轴上产生的力矩。矩阵[bω]×是一个反对称矩阵，是角速度bω的叉乘映射。

13、叉乘映射[·]×：的定义如下，对于任意有[x]×y＝x×y，且叉乘映射的逆映射由vex映射vex(·)表示：其中3×3反对称矩阵集合对于叉乘映射和vex映射可总结为

14、

15、对于多旋翼拦截/碰撞飞行器，可控力由螺旋桨的推力产生，如下所示：

16、

17、其中，0≤f≤fm,fm是最大推力；rf∈so(3)是将推力矢量转换为bcs中obzb轴的旋转矩阵；对于飞行器，rf是常数，因为螺旋桨以固定方向相对机体提供推力，而对于倾转旋翼飞机，它是可变的。矢量e3是3×3单位矩阵i的第三列，i＝[e1 e2 e3]，e3＝[0 0 1]t。向量nf是ef的单位向量。

18、目标模型：目标模型的运动使用质点模型表示，如下所示：

19、

20、其中，和分别表示目标在efcs下的位置、速度和推力加速度；噪声是零均值的高斯随机变量。

21、使用epr指示拦截器和目标在efcs中的相对位置，epr＝ep-ept。此外，相对速度定义为evr＝ev-evt，相对加速度定义为ear＝ea-eat，请注意，使用视觉传感器测量目标的运动加速度eat是具有挑战性的。因此，在控制器和状态观察器的设计中，eat都被视为作用于它们的扰动。

22、相机成像模型：如图1所示，透视投影执行从3d空间到图像平面的映射：假设ccs的原点与bcs的原点重合。即它们的平移向量且旋转矩阵是常数。本发明使用的相机与机体一体化，当相机与目标接触时，即视为成功拦截/碰撞。因此，光轴与飞行器头部方向存在恒定的旋转

23、在大机动，高速度条件下，有必要对拦截/碰撞问题进行重新描述，如图1所示。ep和ept分别表示拦截器的位置和目标的位置；是目标的坐标；nt表示efcs中沿视线的目标单位向量；nc表示efcs中的光轴单位向量；ntd定义为efcs中设计的单位向量，称为设计的单位向量，称为设计los向量；iptd是ics中与图像平面的交点，称为拦截点。拦截/碰撞问题可以总结为nt和ntd是共线的，使ep和ept彼此接近。

24、los是拦截器和目标之间的一条连线。从拦截器到目标的单位矢量称为los矢量，记为nt。当目标位于传感器的fov中时，上述所有类型的传感器都可以通过统一的成像模型转换为透视图。向量nt可以表示为pr负方向的单位向量，也可以通过图像坐标和焦距foc得到

25、

26、fov是感知传感器可以成像的可观测区域的范围，包括水平视场(hfov)和垂直视场(vfov)。参数αhfov、αvfov为感知传感器的hfov和vfov的角度。具体来说，普通相机和固态激光雷达的fov值有限；云台相机的fov值在不断变化；全景相机和全向激光雷达有一个360度的fov。fov被定义为其中是x在ccs中的坐标。相机成像受fov约束，即目标在传感器的fov内，

27、ibvs模型：图像雅可比矩阵可以表示相机运动与图像特征运动之间的关系为：

28、

29、

30、其中，表示ccs中的速度和角速度量，包括刚体的线速度和角速度；表示ics中特征的归一化图像坐标。

31、飞行器刚体模型和相机成像模型描述了拦截/碰撞飞行器的运动和感知。对于大机动、高速度条件下的多旋翼拦截/碰撞过程，imu和图像的延迟量也需要进行考虑。

32、系统模型：对数映射ln(·):将旋转矩阵转换为一个旋转角度θ∈[-π,π)和一个单位旋转向量即

33、

34、用一个四维向量来紧表示拦截器的旋转矩阵称为方向四元数将目标中心的归一化图像坐标视为特征点，记为状态变量由一些必要的状态，包括拦截器的方向四元数q，相对位置pr和相对速度vr，图像特征点陀螺仪偏差bgyr＝[bgyr,x bgyr,y bgyr,z]t，加速度计偏差bacc＝[bacc,x bacc,y bacc,z]t。

35、离散状态方程写为：

36、xk＝fkxk-1+gkwk-1                          (7)

37、其中，和为从时间tk-1到时间tk的状态转移矩阵和噪声矩阵。wk是过程噪声向量，其中元素遵循均值为零且互不相关的高斯分布，矢量wk由陀螺仪偏噪声和加速度计偏噪声由组成。

38、imu测量模型：imu(惯性测量单元)提供了在bcs下的陀螺仪角增量和加速度计速度增量测量。将这些测量与飞行器刚体模型结合，得到了状态q和vr的递归表达式。

39、·方向四元数q的导数，为角速度bω，由陀螺仪获得，测量为：

40、bω＝ωgyr-bgyr-ngyr                              (8)

41、其中，陀螺仪测量噪声ngyr是一个均值为0的高斯随机变量；

42、偏置bgyr遵循维纳过程，如下所示：

43、

44、·速度vr的导数，即加速度ea，可以从加速度计测量值得到为

45、

46、其中，加速度计测量噪声nacc是零均值的高斯随机变量；偏置bacc遵循维纳过程，

47、

48、图像延迟测量模型：图像处理提供目标点的图像坐标，经过归一化后得到状态由于相机成像和图像处理需要时间，因此得到的测量值是一个时期以前的数据。假设测量的基于imu更新频率的延迟为d周期，延迟时间td＞0已知。然后将t时刻的图像测量值表示为：

49、

50、其中图像测量噪声nimg是零均值的高斯随机变量。请注意，这个飞行器的延迟时间td包括相机成像和图像处理时间，并采用两种方法进行估计：①基于相机参数和内部程序计时器的直接计算；②用相机捕捉计时器的实验测量。

51、根据前文提到的相机固定连接假设，假设多旋翼状态和图像测量是准确可用的。同时，目标和拦截器在惯性系统中的位置也是未知的。对于拦截器模型(1)，设计f,bω的控制器，使图像跟踪误差在设定范围内，即拦截器持续接近目标，即pr→0。

52、本发明提出了基于旋转矩阵和延迟卡尔曼滤波来解决高精度高动态自主拦截/碰撞的方法，其流程图如图2所示，在之前定义的基础上，其实现步骤具体如下：

53、步骤一：基于旋转矩阵的控制器

54、在步骤一中，基于旋转矩阵的控制器根据收集到的图像信息和飞行器自身状态，计算拦截控制量。飞行器模型、目标模型、相机成像模型共同构成了步骤一的数学模型。

55、s11、输入图像测量相对速度vr和自身姿态

56、s12、根据拦截器当前状态初始化ad和rd，具体为ad＝0,

57、s13、根据相机成像模型和步骤2中估计的图像特征计算nt，具体为

58、s14、在拦截/碰撞中，根据公式：

59、

60、

61、计算姿态控制器命令，包括期望拉力fd和期望角速度ωd。

62、s15、判断图像特征是否更新，若已更新，则跳转步骤s16，若不，则跳转步骤s18。

63、s16、根据

64、

65、ad＝-k2vr-k3z1+eat                             (16)

66、

67、计算共线控制器命令ω1和姿态控制设定值ad,rd。

68、s17、综合控制指令ωd为：

69、ωd＝ωd+ω1                                 (18)

70、s18、如果拦截/碰撞任务未结束，t时间后再次跳转至步骤s14。

71、步骤二：基于延迟卡尔曼滤波的观测器

72、飞行器模型、目标模型、相机成像模型、ibvs模型共同构成了系统模型、imu测量模型和图像延迟测量模型，这些模型用于延迟卡尔曼滤波的状态预测和测量更新。

73、s21、输入imu的测量量：陀螺仪测量的角速度ωgyr和加速度计测量的加速度aacc，检测模块输出的图像测量

74、s22、初始化p0,k0。

75、s23、判断检测模块的图像测量是否更新，若更新，则跳转步骤24，若不，则跳转步骤s25。

76、s24、根据公式：

77、

78、

79、利用imu测量递推状态。

80、s25、根据公式：

81、

82、pkd＝i-kh pk-d|k-d-1            (22)

83、k＝pk-d|k-d-1hts-1,s＝hpk-d|k-d-1ht+r     (23)

84、估计tk-d时刻状态

85、s26、根据公式(18)，(19)向后递推状态，重复s24步骤，直至将有延迟的状态估计更新到当前时刻。

86、s27、如果拦截/碰撞任务未结束，跳转至步骤s21。

87、本发明引入了基于旋转矩阵的高精度高动态自主拦截/碰撞控制，作为支持拦截/碰撞的关键技术。本发明验证了算法在机载感知和控制下的有效性，巩固了其在实际应用中的可行性。这些贡献共同推动了运动目标高精度高动态自主拦截/碰撞技术的发展，为有效应对高速目标入侵威胁提供了重要的工具和方法。