一种基于视觉锚点的无人机降落位姿滤波估计方法及系统与流程

本发明涉及无人机自主降落技术领域，尤其是一种基于视觉锚点的无人机降落位姿滤波估计方法及系统。

背景技术：

在无人机自主起降过程中，结合机载全球定位系统以及惯性导航系统实现对无人机自身位置和姿态的实时获取，是当前无人机感知自身状态的主要手段。考虑到环境中诸如磁场、温度的因素容易造成对机载定位系统的干扰，在无人机整个降落过程中，仅仅依赖机载定位系统无法保证为无人机提供稳定的精准位姿信息。利用地基单目视觉系统对无人机降落过程进行观测，可利用计算机视觉技术实现对无人机空间位置和姿态的实时估计，辅助机载定位系统为无人机提供更加准确和稳定的实时空间位姿信息。目前，利用双目测距原理、pnp问题求解等传统方法根据二维图像估计目标的空间位置和姿态在精确性和鲁棒性方面均存在较多不足。因此，迫切需要设计一种基于单目视觉的高精度、强鲁棒无人机位姿估计方法。

技术实现要素：

本发明提供一种基于视觉锚点的无人机降落位姿滤波估计方法及系统，用于克服现有技术中精度不高、强鲁棒不强等缺陷。

为实现上述目的，本发明提出一种基于视觉锚点的无人机降落位姿滤波估计方法，包括：

根据无人机降落过程中视觉锚点测量情况，构建无人机位姿估计扩展卡尔曼滤波模型；所述模型包括系统状态预测方程和系统观测方程；

根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值；

获取上一时刻无人机空间位姿，根据当前时刻无人机系统的输入和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值；

根据当前时刻无人机空间位姿预测值以及其他系统观测量，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值，根据所述测量值的预测值获得当前时刻无人机视觉锚点的图像位置预测值；

获取当前时刻无人机视觉锚点的图像位置测量值，根据无人机位姿估计扩展卡尔曼滤波模型，利用无人机空间位姿预测值、当前时刻无人机视觉锚点的图像位置预测值和当前时刻无人机视觉锚点的图像位置测量值，通过无人机状态更新方程获得当前时刻无人机空间位姿。

为实现上述目的，本发明还提出一种基于视觉锚点的无人机降落位姿滤波估计系统，包括：

模型构建模块，用于根据无人机降落过程中视觉锚点测量情况，构建无人机位姿估计扩展卡尔曼滤波模型；所述模型包括系统状态预测方程和系统观测方程；根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值；

无人机空间位姿预测模块，用于获取上一时刻无人机空间位姿，根据当前时刻无人机系统的输入和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值；

无人机视觉锚点的图像位置预测模块，用于根据当前时刻无人机空间位姿预测值以及其他系统观测量，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值，根据所述测量值的预测值获得当前时刻无人机视觉锚点的图像位置预测值；

无人机空间位姿获取模块，用于获取当前时刻无人机视觉锚点的图像位置测量值，根据无人机位姿估计扩展卡尔曼滤波模型，利用无人机空间位姿预测值、当前时刻无人机视觉锚点的图像位置预测值和当前时刻无人机视觉锚点的图像位置测量值，通过无人机状态更新方程获得当前时刻无人机空间位姿。

为实现上述目的，本发明还提出一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述所述方法的步骤。

与现有技术相比，本发明的有益效果有：

本发明提供的基于视觉锚点的无人机降落位姿滤波估计方法面向无人机降落过程中的空间位置和姿态估计需求，构建了基于视觉锚点测量的无人机位姿估计扩展卡尔曼滤波模型，依托扩展卡尔曼滤波器理论，实现了在最小误差二范数平方和指标下的无人机位姿的最优估计，并且有效降低了降落过程中地基视觉的观测误差对无人机位姿估计准确性的影响，相比传统方法大幅提升了无人机降落过程中位姿估计的准确性和鲁棒性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。

图1为本发明提供的基于视觉锚点的无人机降落位姿滤波估计方法的流程图；

图2为地基视觉系统实时估计无人机降落过程中涉及的多个物理坐标系；

图3为采用本发明提供的方法和传统方法生成的无人机降落轨迹以及定位和定姿误差曲线图；

图4为采用本发明提供的方法和传统方法在无人机降落三个阶段的目标位姿估计均方根误差图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

本发明提出一种基于视觉锚点的无人机降落位姿滤波估计方法，如图1所示，包括：

101：根据无人机降落过程中视觉锚点测量情况，构建无人机位姿估计扩展卡尔曼滤波模型；模型包括系统状态预测方程和系统观测方程；

无人机降落过程中地基视觉存在观测误差，会直接影响无人机位姿估计准确性，因此本发明构建无人机位姿估计扩展卡尔曼滤波模型，以降低观测误差。

102：根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值；

无人机的图像广义特征主要分为点、线、面3类。

应用场景特点，主要指无人机降落轨迹的差异以及天气、天时等环境因素差异，难以在无人机的成像中稳定形成线、面等条件更加苛刻的特征且目标运动范围广。

103：获取上一时刻无人机空间位姿，根据当前时刻无人机系统的输入和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值；

无人机空间位姿包括在世界坐标系中的位置和姿态欧拉角。

当前时刻无人机系统的输入主要为加速度项。

104：根据当前时刻无人机空间位姿预测值以及其他系统观测量，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值，根据测量值的预测值获得当前时刻无人机视觉锚点的图像位置预测值；

105：获取当前时刻无人机视觉锚点的图像位置测量值，根据无人机位姿估计扩展卡尔曼滤波模型，利用无人机空间位姿预测值、当前时刻无人机视觉锚点的图像位置预测值和当前时刻无人机视觉锚点的图像位置测量值，通过无人机状态更新方程获得当前时刻无人机空间位姿。

当前时刻无人机视觉锚点的图像位置测量值采用现有的视觉锚点检测方法检测获得，如利用图像中sift、harris等角点特征或log、dog等斑点特征。

本发明提供的基于视觉锚点的无人机降落位姿滤波估计方法面向无人机降落过程中的空间位置和姿态估计需求，构建了基于视觉锚点测量的无人机位姿估计扩展卡尔曼滤波模型，依托扩展卡尔曼滤波器理论，实现了在最小误差二范数平方和指标下的无人机位姿的最优估计，并且有效降低了降落过程中地基视觉的观测误差对无人机位姿估计准确性的影响，相比传统方法大幅提升了无人机降落过程中位姿估计的准确性和鲁棒性。

在其中一个实施例中，对于步骤101，无人机位姿估计扩展卡尔曼滤波模型包括系统状态预测方程fs和系统观测方程h。针对无人机位姿估计问题，无人机的状态x由无人机在世界坐标系中的位置速度姿态欧拉角以及对应的角速度构成：

在另一个实施例中，对于步骤102，根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值，包括：

根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值为：

式中，z为无人机视觉锚点测量值；m为视觉锚点的数量；为第m个视觉锚点的图像位置。

在无人机位姿估计扩展卡尔曼滤波模型中，无人机视觉锚点测量值z由无人机的图像广义特征构成。常见广义特征主要分为点、线、面3类。这些特征往往具有较为直观的物理含义，易于理解。其中，线、面特征虽然在无人机成像中更为常见，但很容易由于遮挡破坏其完整性，降低特征检测的精度。点特征往往对应图像中的物体轮廓的拐点、线条交叉点或同周围有差异的区域，相比线、面特征，其成像更为稳定。此外，在无人机降落应用场景中，考虑到无人机降落轨迹的差异以及天气、天时等环境因素差异，难以在无人机的成像中稳定形成线、面等条件更加苛刻的特征。综上所述，将无人机的视觉拐点作为目标的锚点广义特征，对测量值z进行定义。

在下一个实施例中，对于步骤103，根据当前时刻无人机系统的输入和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值，包括：

301：根据当前时刻无人机系统输入的加速度项和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值为：

xk|k-1＝fs(xk-1|k-1,uk)

式中，fs(·)为系统状态预测方程；uk为当前时刻无人机系统的输入；

302：根据无人机应用场景，忽略无人机运动的动力学部分，获得当前时刻无人机空间位姿预测值为：

式中，xk|k-1为当前时刻无人机空间位姿预测值；fk为状态转移矩阵；xk-1|k-1为上一时刻无人机空间位姿；i3×3为单位矩阵；δtk|k-1为对角元素为δt的3×3对角矩阵，δt为当前时刻与上一时刻的时间差；为位置；为速度；为姿态欧拉角；为角速度。

根据目标运动的运动学和动力学原理，目标从k-1时刻到k时刻的运动是由k-1时刻的速度、角速度和该时间段内的加速度和角加速度决定。在无人机降落应用场景中，相邻两次的位姿估计时间间隔以毫秒为单位计量，即k-1与k时刻的时间间隔δt较小，因而可忽略无人机运动的动力学部分，即无人机在时间段δt内做匀速运动和转动。

在下一个实施例中，对于步骤104，根据当前时刻无人机空间位姿预测值以及其他系统观测量，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值，包括：

根据当前时刻无人机空间位姿预测值以及其他系统观测量，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值为：

式中，zk|k-1为当前时刻无人机视觉锚点测量值的预测值；h(·)为系统观测方程；s为图像投影归一化因子；k′为摄像机内参数矩阵，f为摄像机的焦距，dx和dy分别为每个像素的实际宽度和高度，(cx,cy)为图像中心点的像素坐标；依据图2展示的地基视觉系统和无人机系统涉及的物理坐标系，表示从云台坐标系g到相机坐标系c的齐次转换矩阵；表示从云台基座坐标系g’到云台坐标系g的齐次转换矩阵；表示从世界坐标系w到云台基座坐标系g’的齐次转换矩阵；表示从无人机机体坐标系b到世界坐标系w的齐次转换矩阵，可根据坐标系之间欧拉角、平移向量与齐次转换矩阵的转换关系，直接由xk|k-1中包含的推导获得；为所有视觉锚点在目标体坐标系的空间位置齐次矩阵。

其他系统观测量包括和

系统观测方程h可以视为将所有视觉锚点从目标体坐标系转换至图像坐标系的过程，其间涉及多次坐标系转换以及空间点的图像投影。

本实施例中，根据当前时刻无人机视觉锚点测量值的预测值zk|k-1可直接获得当前时刻无人机视觉锚点的图像位置预测值(图像位置包括在世界坐标系中的位置和姿态欧拉角)

在另一个实施例中，对于步骤105，根据无人机位姿估计扩展卡尔曼滤波模型，利用无人机空间位姿预测值、当前时刻无人机视觉锚点的图像位置预测值和当前时刻无人机视觉锚点的图像位置测量值，通过无人机状态更新方程获得当前时刻无人机空间位姿，包括：

501：根据系统状态预测方程为线性方程，利用上一时刻误差协方差矩阵预测当前时刻误差协方差矩阵；

502：根据预测的当前时刻误差协方差矩阵，利用无人机空间位姿预测值和当前时刻无人机视觉锚点的图像位置预测值，获得当前时刻卡尔曼增益；

503：根据当前时刻卡尔曼增益更新无人机状态和误差协方差矩阵，利用当前时刻无人机视觉锚点的图像位置测量值，通过无人机状态更新方程获得当前时刻无人机空间位姿。

在某个实施例中，对于步骤501，根据系统状态预测方程为线性方程，利用上一时刻的误差协方差矩阵预测当前时刻的误差协方差矩阵，包括：

根据系统状态预测方程为线性方程，利用上一时刻误差协方差矩阵预测当前时刻误差协方差矩阵为：

式中，pk|k-1为当前时刻误差协方差矩阵的预测；pk-1|k-1为上一时刻误差协方差矩阵；fk为状态转移矩阵；qk为状态预测噪声的协方差矩阵。

在下一个实施例中，对于步骤502，根据预测的当前时刻误差协方差矩阵，利用无人机空间位姿预测值和当前时刻无人机视觉锚点的图像位置预测值，获得当前时刻卡尔曼增益，包括：

根据预测的当前时刻误差协方差矩阵，利用无人机空间位姿预测值xk|k-1和当前时刻无人机视觉锚点的图像位置预测值h(xk|k-1)，获得当前时刻卡尔曼增益为：

式中，kk为当前时刻卡尔曼增益；hk为测量矩阵；sk为观测余量协方差矩阵；rk为观测噪声协方差矩阵。

在另一个实施例中，对于步骤503，根据当前时刻卡尔曼增益更新无人机状态和误差协方差矩阵，利用当前时刻无人机视觉锚点的图像位置测量值，获得当前时刻无人机空间位姿，包括：

根据当前时刻卡尔曼增益更新无人机状态和误差协方差矩阵，利用当前时刻无人机视觉锚点的图像位置测量值，获得当前时刻无人机空间位姿为：

xk|k＝xk|k-1+kk(zk-zk|k-1)

pk|k＝(1-kkhk)pk|k-1

式中，xk|k为当前时刻无人机空间位姿；zk为当前时刻无人机视觉锚点测量值；pk|k为当前时刻误差协方差矩阵。

对于本发明提供的基于视觉锚点的无人机降落位姿滤波估计方法，以一个具体应用实例进行说明，构建地基视觉实物系统，使用本发明的方法对无人机降落过程中的空间位姿进行实时估计。为了验证本发明的方法相较传统方法的优势，采用传统的pnp目标位姿解算算法与其进行比较。图3展示了使用本发明的方法(记为fp)和传统的pnp方法(记为np)生成的无人机降落轨迹。当无人机分别位于e、f和g点时，地基摄像机拍摄的无人机图像以及无人机的视觉锚点在图中得以展示。图4统计了两组实物试验中，fp和np分别在无人机降落三个阶段的目标位姿估计均方根误差(rmse)。总体来说，fp相比传统的np在位置和姿态上皆展现出了较高的估计精度。两种方法的估计误差随着无人机距离摄像机越近逐渐降低。这是由于随着无人机靠近摄像机，其成像尺度越大，同样像素的视觉锚点观测误差在目标区域的占比越低，对位姿估计精度的影响越小。根据无人机降落实际应用需求，整个降落过程无人机应尽量位于跑道上空，因此对无人机在跑道平面的定位误差容忍度由跑道长度和宽度决定。通常情况下，沿跑道方向的定位误差应低于70m，沿跑道垂直方向的定位误差应低于20m，即y方向均方根误差小于70m，x方向均方根误差小于20m。根据统计结果，fp在无人机降落各阶段皆满足上述需求。其次，无人机在近地阶段的离地高估计值精度十分关键，集中反应在拉飘阶段的z方向均方根误差。fp在两组试验中均实现了低于1m的z方向估计均方根误差。最后，无人机的滚转角和俯仰角精度在无人机拉飘阶段同样十分关键。fp算法在拉飘阶段的无人机滚转角和俯仰角均方根误差分别低于5°和2°。总体来说，fp相比传统方法np表现出了更高的无人机定位定姿精度以及对观测误差更强的鲁棒性。

综上所述，本发明面向无人机降落过程中的空间位置和姿态估计需求，构建了基于视觉锚点观测量的无人机位姿估计扩展卡尔曼滤波模型，相较传统方法实现了无人机位姿估计精确性和鲁棒性的显著提升，为构建无人机自主降落地基视觉辅助系统提供了有力的技术支撑，具有较强的实用价值。

本发明还提出一种基于视觉锚点的无人机降落位姿滤波估计系统，包括：

模型构建模块，用于根据无人机降落过程中视觉锚点测量情况，构建无人机位姿估计扩展卡尔曼滤波模型；所述模型包括系统状态预测方程和系统观测方程；

无人机空间位姿预测模块，用于获取上一时刻无人机空间位姿，根据当前时刻无人机系统的输入和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值；

无人机视觉锚点的图像位置预测模块，用于根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值；根据当前时刻无人机空间位姿预测值，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值，根据所述测量值的预测值获得当前时刻无人机视觉锚点的图像位置预测值；

无人机空间位姿获取模块，用于获取当前时刻无人机视觉锚点的图像位置测量值，根据无人机位姿估计扩展卡尔曼滤波模型，利用无人机空间位姿预测值、当前时刻无人机视觉锚点的图像位置预测值和当前时刻无人机视觉锚点的图像位置测量值，获得当前时刻无人机空间位姿。

本发明还提出一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述所述方法的步骤。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。