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

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

【技术实现步骤摘要】
一种基于视觉锚点的无人机降落位姿滤波估计方法及系统
本专利技术涉及无人机自主降落
，尤其是一种基于视觉锚点的无人机降落位姿滤波估计方法及系统。
技术介绍
在无人机自主起降过程中，结合机载全球定位系统以及惯性导航系统实现对无人机自身位置和姿态的实时获取，是当前无人机感知自身状态的主要手段。考虑到环境中诸如磁场、温度的因素容易造成对机载定位系统的干扰，在无人机整个降落过程中，仅仅依赖机载定位系统无法保证为无人机提供稳定的精准位姿信息。利用地基单目视觉系统对无人机降落过程进行观测，可利用计算机视觉技术实现对无人机空间位置和姿态的实时估计，辅助机载定位系统为无人机提供更加准确和稳定的实时空间位姿信息。目前，利用双目测距原理、PnP问题求解等传统方法根据二维图像估计目标的空间位置和姿态在精确性和鲁棒性方面均存在较多不足。因此，迫切需要设计一种基于单目视觉的高精度、强鲁棒无人机位姿估计方法。
技术实现思路
本专利技术提供一种基于视觉锚点的无人机降落位姿滤波估计方法及系统，用于克服现有技术中精度不高、强鲁棒不强等缺陷。为实现上述目的，本专利技术提出一种基于视觉锚点的无人机降落位姿滤波估计方法，包括：根据无人机降落过程中视觉锚点测量情况，构建无人机位姿估计扩展卡尔曼滤波模型；所述模型包括系统状态预测方程和系统观测方程；根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值；获取上一时刻无人机空间位姿，根据当前时刻无人机系统的输入和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值；根据当前时刻无人机空间位姿预测值以及其他系统观测量，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值，根据所述测量值的预测值获得当前时刻无人机视觉锚点的图像位置预测值；获取当前时刻无人机视觉锚点的图像位置测量值，根据无人机位姿估计扩展卡尔曼滤波模型，利用无人机空间位姿预测值、当前时刻无人机视觉锚点的图像位置预测值和当前时刻无人机视觉锚点的图像位置测量值，通过无人机状态更新方程获得当前时刻无人机空间位姿。为实现上述目的，本专利技术还提出一种基于视觉锚点的无人机降落位姿滤波估计系统，包括：模型构建模块，用于根据无人机降落过程中视觉锚点测量情况，构建无人机位姿估计扩展卡尔曼滤波模型；所述模型包括系统状态预测方程和系统观测方程；根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值；无人机空间位姿预测模块，用于获取上一时刻无人机空间位姿，根据当前时刻无人机系统的输入和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值；无人机视觉锚点的图像位置预测模块，用于根据当前时刻无人机空间位姿预测值以及其他系统观测量，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值，根据所述测量值的预测值获得当前时刻无人机视觉锚点的图像位置预测值；无人机空间位姿获取模块，用于获取当前时刻无人机视觉锚点的图像位置测量值，根据无人机位姿估计扩展卡尔曼滤波模型，利用无人机空间位姿预测值、当前时刻无人机视觉锚点的图像位置预测值和当前时刻无人机视觉锚点的图像位置测量值，通过无人机状态更新方程获得当前时刻无人机空间位姿。为实现上述目的，本专利技术还提出一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述所述方法的步骤。与现有技术相比，本专利技术的有益效果有：本专利技术提供的基于视觉锚点的无人机降落位姿滤波估计方法面向无人机降落过程中的空间位置和姿态估计需求，构建了基于视觉锚点测量的无人机位姿估计扩展卡尔曼滤波模型，依托扩展卡尔曼滤波器理论，实现了在最小误差二范数平方和指标下的无人机位姿的最优估计，并且有效降低了降落过程中地基视觉的观测误差对无人机位姿估计准确性的影响，相比传统方法大幅提升了无人机降落过程中位姿估计的准确性和鲁棒性。附图说明为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。图1为本专利技术提供的基于视觉锚点的无人机降落位姿滤波估计方法的流程图；图2为地基视觉系统实时估计无人机降落过程中涉及的多个物理坐标系；图3为采用本专利技术提供的方法和传统方法生成的无人机降落轨迹以及定位和定姿误差曲线图；图4为采用本专利技术提供的方法和传统方法在无人机降落三个阶段的目标位姿估计均方根误差图。本专利技术目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。具体实施方式下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术的一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术保护的范围。另外，本专利技术各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本专利技术要求的保护范围之内。本专利技术提出一种基于视觉锚点的无人机降落位姿滤波估计方法，如图1所示，包括：101：根据无人机降落过程中视觉锚点测量情况，构建无人机位姿估计扩展卡尔曼滤波模型；模型包括系统状态预测方程和系统观测方程；无人机降落过程中地基视觉存在观测误差，会直接影响无人机位姿估计准确性，因此本专利技术构建无人机位姿估计扩展卡尔曼滤波模型，以降低观测误差。102：根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值；无人机的图像广义特征主要分为点、线、面3类。应用场景特点，主要指无人机降落轨迹的差异以及天气、天时等环境因素差异，难以在无人机的成像中稳定形成线、面等条件更加苛刻的特征且目标运动范围广。103：获取上一时刻无人机空间位姿，根据当前时刻无人机系统的输入和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值；无人机空间位姿包括在世界坐标系中的位置和姿态欧拉角。当前时刻无人机系统的输入主要为加速度项。104：根据当前时刻无人机空间位姿预测值以及其他系统观测量，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值，根据测量值的预测值获得当前时刻无人机视觉锚点的图像位置预测值；105：获取当前时刻无人机视觉锚点的图像位置测量值，根据无人机位姿估计扩展卡尔曼滤波模型，利用无人机空间位姿预测值、当前时刻无人机视觉锚点的图像位置预测值和当前时刻无人机视觉锚点的图像位置测量值，通过无人机状态更新方程获得当前时刻无人机空间位姿。当前时刻无人机视觉锚点的图像位置测量值采用现本文档来自技高网...

【技术保护点】
1.一种基于视觉锚点的无人机降落位姿滤波估计方法，其特征在于，包括：/n根据无人机降落过程中视觉锚点测量情况，构建无人机位姿估计扩展卡尔曼滤波模型；所述模型包括系统状态预测方程和系统观测方程；/n根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值；/n获取上一时刻无人机空间位姿，根据当前时刻无人机系统的输入和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值；/n根据当前时刻无人机空间位姿预测值以及其他系统观测量，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值，根据所述测量值的预测值获得当前时刻无人机视觉锚点的图像位置预测值；/n获取当前时刻无人机视觉锚点的图像位置测量值，根据无人机位姿估计扩展卡尔曼滤波模型，利用无人机空间位姿预测值、当前时刻无人机视觉锚点的图像位置预测值和当前时刻无人机视觉锚点的图像位置测量值，通过无人机状态更新方程获得当前时刻无人机空间位姿。/n

【技术特征摘要】
1.一种基于视觉锚点的无人机降落位姿滤波估计方法，其特征在于，包括：
根据无人机降落过程中视觉锚点测量情况，构建无人机位姿估计扩展卡尔曼滤波模型；所述模型包括系统状态预测方程和系统观测方程；
根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值；
获取上一时刻无人机空间位姿，根据当前时刻无人机系统的输入和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值；
根据当前时刻无人机空间位姿预测值以及其他系统观测量，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值，根据所述测量值的预测值获得当前时刻无人机视觉锚点的图像位置预测值；
获取当前时刻无人机视觉锚点的图像位置测量值，根据无人机位姿估计扩展卡尔曼滤波模型，利用无人机空间位姿预测值、当前时刻无人机视觉锚点的图像位置预测值和当前时刻无人机视觉锚点的图像位置测量值，通过无人机状态更新方程获得当前时刻无人机空间位姿。


2.如权利要求1所述的基于视觉锚点的无人机降落位姿滤波估计方法，其特征在于，根据当前时刻无人机系统的输入和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值，包括：
根据当前时刻无人机系统输入的加速度项和上一时刻无人机空间位姿，利用系统状态预测方程，获得当前时刻无人机空间位姿预测值为：
xk|k-1＝fs(xk-1|k-1,uk)



式中，fs(·)为系统状态预测方程；uk为当前时刻无人机系统的输入；
根据无人机应用场景，忽略无人机运动的动力学部分，获得当前时刻无人机空间位姿预测值为：



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


3.如权利要求1所述的基于视觉锚点的无人机降落位姿滤波估计方法，其特征在于，根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值，包括：
根据无人机的图像广义特征和应用场景特点，定义无人机视觉锚点测量值为：






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


4.如权利要求1所述的基于视觉锚点的无人机降落位姿滤波估计方法，其特征在于，根据当前时刻无人机空间位姿预测值以及其他系统观测量，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值，包括：
根据当前时刻无人机空间位姿预测值以及其他系统观测量，利用系统观测方程获得当前时刻无人机视觉锚点测量值的预测值为：









式中，zk|k-1为当前时刻无人机视觉锚点测量值的预测值；h(·)为系统观测方程；s为图像投影归一化因子；K′为摄像机内参数矩阵，f为摄像机的焦距，dx和dy分别为每个像素的实际宽度和高度，(cx,cy)为图像中心点的像素坐标；表示从云台坐标系g到相机坐标系c的齐次转换矩阵；表示从云台基座坐标系g’到云台坐标系g的齐次转换矩阵；表示从世界坐标系w到云台基座坐标系g’的齐次转换矩阵；表示从无人机机体坐标系b到世界坐标系w的齐次转换矩阵；为所有视觉锚点在目标体坐标系的空间位置齐次矩阵。


5.如权利要求1所述的基于视觉锚点的无人机降落位姿滤波估计方法，其特征在于，根据无人机位姿估计扩展卡尔曼滤波模型，利用无人机空间位姿预测值、当前时刻无人机视觉锚点的图像位置预测值和当前时刻无人机视觉锚点的图像位置测量值...

【专利技术属性】
技术研发人员：相晓嘉，周晗，唐邓清，常远，闫超，黄依新，陈紫叶，兰珍，刘兴宇，李子杏，
申请(专利权)人：中国人民解放军国防科技大学，
类型：发明
国别省市：湖南;43