CN116380057A - 一种gnss拒止环境下无人机自主着陆定位方法 - Google Patents

一种gnss拒止环境下无人机自主着陆定位方法 Download PDF

Info

Publication number
CN116380057A
CN116380057A CN202310654364.0A CN202310654364A CN116380057A CN 116380057 A CN116380057 A CN 116380057A CN 202310654364 A CN202310654364 A CN 202310654364A CN 116380057 A CN116380057 A CN 116380057A
Authority
CN
China
Prior art keywords
coordinate system
unmanned aerial
aerial vehicle
northeast
factor
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310654364.0A
Other languages
English (en)
Other versions
CN116380057B (zh
Inventor
梁文斌
许浩
刘阳
卢云玲
唐辉煌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sichuan Tengdun Technology Co Ltd
Original Assignee
Sichuan Tengdun Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Sichuan Tengdun Technology Co Ltd filed Critical Sichuan Tengdun Technology Co Ltd
Priority to CN202310654364.0A priority Critical patent/CN116380057B/zh
Publication of CN116380057A publication Critical patent/CN116380057A/zh
Application granted granted Critical
Publication of CN116380057B publication Critical patent/CN116380057B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种GNSS拒止环境下无人机自主着陆定位方法，该方法包括前端和后端两个线程；前端线程的处理过程包括：接收到多源数据包后，首先通过对机载惯性导航系统获取的三轴加速度和三轴角速度进行积分，得到当前帧时刻相对于上一帧时刻的无人机相对运动，再根据相对运动和旧关键帧的东北天坐标系下的位姿，得到当前帧的东北天坐标系下的位姿；然后判断后端是否正在执行优化；旧关键帧指的是经过后端优化的最近一帧数据；后端线程在执行完一次优化后就进入休眠状态，当检测到后端优化队列中有新数据加入时，从队列中提取出新数据，再次执行优化。本发明的视觉定位较精确，能够得到近乎与真实轨迹完全重合的估计轨迹。

Description

一种GNSS拒止环境下无人机自主着陆定位方法
技术领域
本发明涉及无人机定位技术领域，特别是一种GNSS拒止环境下无人机自主着陆定位方法。
背景技术
大型固定翼无人机的自主着陆依赖GNSS和地面差分站，但在战争情况下，以上条件不一定能满足。在没有地面差分站和GNSS拒止场景下，无人机可以根据相机对机场跑道的观测来估计自身位置，称为视觉定位。但受天气和相机图像分辨率的影响，视觉定位精度低、噪声大，难以直接用于引导无人机着陆。一种平滑无人机视觉定位结果的方法是将其与机载惯性导航系统融合。目前的机载惯性导航系统信息融合方法基本都是基于扩展卡尔曼滤波(EKF)。EKF通过融合上一时刻的后验信息和当前时刻的先验信息来平滑视觉定位结果。由于EKF每次融合可以参考的信息太少，在视觉定位误差过大的情况下，EKF难以获得较好的效果。
发明内容
鉴于此，本发明提供一种GNSS拒止环境下无人机自主着陆定位方法，以解决上述技术问题。
本发明公开了一种GNSS拒止环境下无人机自主着陆定位方法，其包括前端和后端两个线程，前端线程负责根据机载惯性导航系统积分实时推测无人机的当前位置；后端线程负责视觉定位，构造信息融合因子图模型，优化并更新因子图模型中关键节点的位置坐标；
前端线程的处理过程包括：
接收到多源数据包后，首先通过对机载惯性导航系统获取的三轴加速度和三轴角速度进行积分，得到当前帧时刻相对于上一帧时刻的无人机相对运动，再根据相对运动和旧关键帧的东北天坐标系下的位姿，得到当前帧的东北天坐标系下的位姿；然后判断后端是否正在执行优化；旧关键帧指的是经过后端优化的最近一帧数据；
后端线程在执行完一次优化后就进入休眠状态，当检测到后端优化队列中有新数据加入时，从队列中提取出新数据，再次执行优化。
进一步地，在所述接收到多源数据包之前，还包括：
获取无人机着陆前，其前视相机观测无人机跑道的每一帧图像采样时刻对应的多源数据，并对该采样时刻的多源数据打包储在一个类中，并以独占指针的形式传递给系统的前端线程。
进一步地，所述多源数据的获取过程，包括：
分别设置无人机的飞行管理计算机上的机载相机、机载惯性导航系统、气压计、电磁高度表的数据采样频率，机载相机的采样频率最低；
以每一帧图像的采样时刻为基准，对机载惯性导航系统、气压计、电磁高度表的数据进行插值，得到与每一帧图像采样时刻相对应的多源数据。
进一步地，所述判断后端是否正在执行优化，包括：
若没有执行优化，则将下一个多源数据包连同积分的东北天坐标添加到后端优化队列，同时将当前推测的东北天坐标发送给无人机的飞行管理计算机；
若后端正在执行优化，则直接将当前推测的东北天坐标发送给无人机的飞行管理计算机。
进一步地，所述执行优化，包括：
从图像中识别出跑道，并提取出跑道的四个角点，根据跑道四个角点的像素位置和对应的角点GNSS坐标，计算无人机的当前位姿；
用多源数据构造因子并将其添加到因子图模型中，然后执行因子图模型优化，根据优化结果更新因子图模型中节点的位姿；最后执行边缘化，将过早的节点边缘化，以减少每次执行优化的时间。
进一步地，所述根据跑道四个角点的像素位置和对应的角点GNSS坐标，计算无人机的当前位姿，包括：
利用机载惯性导航系统提供的无人机的姿态信息，计算出机载相机在东北天坐标系下的姿态；
基于机载相机在东北天坐标系下的姿态，获取机场跑道四个角点在相机坐标系下的坐标，继而构造线性方程组，通过求解该线性方程组以得到人机机体坐标系在东北天坐标系下的位姿。
进一步地，所述利用机载惯性导航系统提供的无人机的姿态信息，计算出机载相机在东北天坐标系下的姿态，包括：
假设机载相机上观测到的跑道四个角点的像素坐标分别是
Figure SMS_3
，/>
Figure SMS_4
，
Figure SMS_7
，/>
Figure SMS_2
，构造四个角点的齐次坐标，并通过如下公式投影到相机坐标系的深度归一化平面上，分别得到四个向量/>
Figure SMS_5
、/>
Figure SMS_6
、/>
Figure SMS_8
、/>
Figure SMS_1
；
Figure SMS_9
其中，在图像坐标系中，
Figure SMS_11
、/>
Figure SMS_14
分别是左上角点的横坐标和纵坐标，/>
Figure SMS_16
、/>
Figure SMS_12
分别是右上角点的横坐标和纵坐标，/>
Figure SMS_13
、/>
Figure SMS_18
分别是左下角点的横坐标和纵坐标，/>
Figure SMS_19
、/>
Figure SMS_10
分别是右下角点的横坐标和纵坐标，/>
Figure SMS_15
为相机的内参矩阵；/>
Figure SMS_17
为图像上的素点投影到三维空间下，在相机坐标系下的坐标；图像的左上角像素为原点，横轴向右是u，纵轴向下是v，u、v的大小是距离左上角像素的像素数量；
假设跑道的四个角点的实际GNSS坐标已知，将其变换到东北天坐标系下，分别用向量
Figure SMS_20
、/>
Figure SMS_21
、/>
Figure SMS_22
、/>
Figure SMS_23
表示；
通过机载惯性导航系统得到无人机在北东地坐标系下的姿态，通过坐标系变换关系，得到相机在东北天坐标系下的姿态，记为旋转矩阵
Figure SMS_24
。
进一步地，所述基于机载相机在东北天坐标系下的姿态，获取机场跑道四个角点在相机坐标系下的坐标，继而构造线性方程组，通过求解该线性方程组以得到人机机体坐标系在东北天坐标系下的位姿，包括：
机载相机在东北天坐标系下的位置为未知量，用三维向量
Figure SMS_25
表示；机场跑道的角点通过/>
Figure SMS_26
和/>
Figure SMS_27
变换到相机坐标系下，变换关系为：
Figure SMS_28
通过该变换关系，分别得到机场跑道四个角点在相机坐标系下的坐标
Figure SMS_32
、/>
Figure SMS_36
、
Figure SMS_42
、/>
Figure SMS_31
，当向量/>
Figure SMS_40
与/>
Figure SMS_46
，/>
Figure SMS_52
与/>
Figure SMS_30
，/>
Figure SMS_37
与/>
Figure SMS_44
，/>
Figure SMS_50
与/>
Figure SMS_33
尽可能平行时，此时的/>
Figure SMS_38
即为求解得到的相机东北天坐标；分别计算向量/>
Figure SMS_43
、/>
Figure SMS_48
、/>
Figure SMS_49
、/>
Figure SMS_53
的两个零空间向量，记为/>
Figure SMS_54
、
Figure SMS_55
，/>
Figure SMS_29
、/>
Figure SMS_35
，/>
Figure SMS_41
、/>
Figure SMS_47
，/>
Figure SMS_34
、/>
Figure SMS_39
；/>
Figure SMS_45
与/>
Figure SMS_51
平行时，满足如下等式：
Figure SMS_56
四个角点能够构造八个等式，将等式写成矩阵形式的线性方程组：
Figure SMS_57
其中，A为系数矩阵，x为待求解变量，即
Figure SMS_58
，b为常数项；用最小二乘法求解该线性方程组，所得解即为机载相机在东北天坐标系下的位置坐标，通过变换得到无人机机体坐标系在东北天坐标系下的位姿。
进一步地，优化后的因子图模型包括第一类节点、第二类节点、第一类因子、第二类因子、第三类因子、第四类因子、第五类因子；
第一类节点之间通过第四类因子连接成一条直线，记为第一直线；第二类节点之间通过第五类因子连接成一条直线，记为第二直线；第一直线与第二直线平行；
每个第一类节点连接一个第三类因子；每个第一类节点分别通过第一类因子和第二类因子与第二类节点连接；
第一类节点表示东北天坐标系下任一时刻无人机的无人机位姿；该无人机位姿是一个六自由度的位姿变量，包括三自由度位置和三自由度姿态，东北天坐标系的站心建立在机场中心经纬坐标，海拔高度为零的位置；
第二类节点表示当前位置地面上的海拔高度；
第一类因子表示视觉定位因子，也即无人机的位姿；第二类因子表示电磁高度表的测量因子；第三类因子表示气压计因子；第四类因子表示机载惯性导航系统因子；第五类因子表示层高随机游走因子。
进一步地，所述因子图模型构建在时间窗口内，新的一帧到来后构造各类新因子并插入因子图模型进行求解；当因子图模型中最开始的帧与当前帧的时间差大于预设时间时，则边缘化掉最开始的帧；边缘化指的是将与最开始的帧相关的因子通过边缘化运算，转化为一个边缘化因子，使得边缘化后的因子图模型不再包含最开始的帧，同时又不丢失最开始的帧的信息。
由于采用了上述技术方案，本发明具有如下的优点：
无人机在距离机场较远，约2到4公里时，视觉定位误差在正负400m内震荡，本方法可以得到一条近乎完全平滑，且距离无人机真实位置误差较小的轨迹。在近距离，约0到2公里时，视觉定位较精确，本方法可以得到一条近乎与真实轨迹完全重合的估计轨迹。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明实施例中记载的一些实施例，对于本领域普通技术人员来讲，还可以根据这些附图获得其他的附图。
图1为本发明实施例的前端线程处理流程图；
图2为本发明实施例的后端线程处理流程图；
图3为本发明实施例的视觉定位原理示意图；
图4为本发明实施例的无人机着陆流程示意图；
图5为本发明实施例的无人机多源信息融合因子图模型；
图6为本发明实施例的机体坐标系和机载相机坐标系的位置示意图。
具体实施方式
结合附图和实施例对本发明作进一步说明，显然，所描述的实施例仅是本发明实施例一部分实施例，而不是全部的实施例。本领域普通技术人员所获得的所有其他实施例，都应当属于本发明实施例保护的范围。
本发明提供了一种GNSS拒止环境下无人机自主着陆定位方法的实施例，其中，本实施例的软件系统包括前端后端两个线程，前端线程负责根据机载惯性导航系统积分实时推测无人机当前位置，后端线程负责视觉定位、构造信息融合因子图模型、优化并更新关键节点位置坐标。
参见图1，前端线程的处理过程包括：
首先对多源数据进行插值处理。机载相机的数据采样频率是30Hz，机载惯性导航系统的采样频率是200Hz，气压计、电磁高度表的采样频率是100Hz。相机的采样频率最低，以每一帧图像的采样时刻为基准，对机载惯性导航系统、气压计、电磁高度表的数据进行插值，得到与每一帧图像采样时刻相对应的多源数据，然后对该时刻的多源数据打包储在一个C++类中，并以独占指针的形式传递给系统的前端线程。其中，每一帧图像指的是无人机着陆前，前视相机观测跑道的图像。机载相机、机载惯性导航系统、气压计、电磁高度表均设置在无人机的飞管计算机上。
系统前端线程接收到新一包数据后，首先通过对机载惯性导航系统获取的三轴加速度和三轴角速度进行积分，得到新一帧时刻相对于上一帧时刻的无人机相对运动，再根据相对运动和旧关键帧的东北天坐标系位姿得到新一帧的东北天坐标系位姿。然后判断后端是否正在执行优化。若没有执行优化，则将新一包数据连同积分的东北天坐标一起添加到后端优化队列，同时将当前推测的东北天坐标发送给无人机飞管计算机。若后端正在执行优化，则直接将当前推测的东北天坐标发送给无人机飞管计算机。
参见图2，后端线程的处理过程包括：
后端线程在执行完一次优化后就进入休眠状态，当检测到后端优化队列中有新数据加入，就从队列中提取出新数据，再次执行优化。
首先通过深度学习等方法从图像中识别出跑道，并提取出跑道的四个角点，然后根据跑道四个角点的像素位置和对应的角点GNSS坐标，计算无人机的当前位姿。用多源信息构造对应的因子并添加到因子图模型中，然后执行因子图模型优化，根据优化结果更新节点位姿。最后执行边缘化，将过早的节点边缘化掉，从而以保证每次执行优化的时间不会太久。例如，执行优化的时间大于20毫秒，则边缘化掉最后的两帧。
“因子图模型优化”是一种最优化数学工具，因子图模型是一种概率图模型，是指由“因子”和“节点”连接构成的图模型。其中节点是优化变量，“因子”与一个或多个节点相连，表示优化问题中的约束。比如一辆车从t0时刻运动到t1时刻，t0时刻和t1时刻的位置是两个节点，也是待优化变量，编码器得到车辆从t0时刻运动到t1时刻的相对位移，这个相对位移是一个优化问题中的“约束”，也就是因子。
计算无人机的当前位姿通过以下过程实现：
本实施例的视觉定位方法原理示意图如图3所示。图3中，1是相机坐标系，2表示成像平面，3表示跑道，4表示相机观测到的跑道，5表示根据相机当前位姿和跑道实际坐标以及相机内参，投影到成像平面上的跑道。6表示相机中心与观测到的跑道角点相连构成的三维空间向量，7表示相机中心与跑道真实三维坐标相连构成的三维空间向量。
无人机观测跑道进行定位的问题本质上是一个PnP问题，即通过3D点与2D图像观测的对应关系计算相机位姿的问题。但传统的PnP算法只有在有足够多的匹配点的情况下才能取得足够高的精度。通过观测跑道只能得到四组匹配点，PnP算法求解的位姿是六个自由度，匹配点的不足导致PnP求解的位姿误差很大。由于无人机机载惯性导航系统可以提供比较精确的姿态信息，本实施例利用机载惯性导航系统提供的无人机姿态信息计算出相机的姿态，从而构造一个只求解位置的视觉定位问题，通过减少求解量以提高视觉定位精度。
假设相机上观测到的跑道四个角点的像素坐标分别是
Figure SMS_59
，/>
Figure SMS_60
，/>
Figure SMS_61
，
Figure SMS_62
。构造四个角点的齐次坐标，并通过以下公式投影到相机坐标系的深度归一化平面上：
Figure SMS_63
其中，在图像坐标系中，
Figure SMS_65
、/>
Figure SMS_68
分别是左上角点的横坐标和纵坐标，/>
Figure SMS_71
、/>
Figure SMS_66
分别是右上角点的横坐标和纵坐标，/>
Figure SMS_69
、/>
Figure SMS_72
分别是左下角点的横坐标和纵坐标，/>
Figure SMS_73
、/>
Figure SMS_64
分别是右下角点的横坐标和纵坐标，/>
Figure SMS_67
为相机的内参矩阵；/>
Figure SMS_70
为图像上的素点投影到三维空间下，在相机坐标系下的坐标；图像的左上角像素为原点，横轴向右是u，纵轴向下是v，u、v的大小是距离左上角像素的像素数量；
得到四个向量
Figure SMS_74
、/>
Figure SMS_75
、/>
Figure SMS_76
、/>
Figure SMS_77
，如图3中的序号6所示的向量，其中/>
Figure SMS_78
为相机的内参矩阵。
假设跑道的四个角点的实际GNSS坐标已知，将其变换到东北天坐标系下，分别用向量
Figure SMS_79
、/>
Figure SMS_80
、/>
Figure SMS_81
、/>
Figure SMS_82
表示。
可以通过机载惯性导航系统得到无人机在北东地坐标系下的姿态，通过坐标系变换关系可以得到相机在东北天坐标系下的姿态，表示为旋转矩阵
Figure SMS_83
，相机在东北天坐标系下的位置为未知量，用三维向量/>
Figure SMS_84
表示。于是，机场跑道的角点可以通过/>
Figure SMS_85
和
Figure SMS_86
变换到相机坐标系下，变换关系为：
Figure SMS_87
得到机场跑道四个角点在相机坐标系下的坐标
Figure SMS_93
、/>
Figure SMS_96
、/>
Figure SMS_103
、/>
Figure SMS_89
，如图3中序号7所示。当向量/>
Figure SMS_97
与/>
Figure SMS_102
，/>
Figure SMS_108
与/>
Figure SMS_91
，/>
Figure SMS_95
与/>
Figure SMS_101
，/>
Figure SMS_107
与/>
Figure SMS_109
尽可能平行时，此时的/>
Figure SMS_112
即为求解得到的相机东北天坐标。为了构造求解方程，分别计算向量/>
Figure SMS_113
、/>
Figure SMS_114
、/>
Figure SMS_92
、/>
Figure SMS_98
的两个零空间向量，记为/>
Figure SMS_105
,/>
Figure SMS_111
,/>
Figure SMS_88
,/>
Figure SMS_94
,/>
Figure SMS_100
,/>
Figure SMS_104
,/>
Figure SMS_90
,/>
Figure SMS_99
。/>
Figure SMS_106
与/>
Figure SMS_110
平行时，满足两条等式：
Figure SMS_115
四个角点共可以构造八条等式，将等式写成矩阵形式的线性方程组：
Figure SMS_116
其中A为系数矩阵，x为待求解变量，也就是
Figure SMS_117
，b为常数项。用最小二乘法求解该线性方程组，所得解就是相机在东北天坐标系下的位置坐标，通过变换可以得到无人机机体坐标系在东北天坐标系下的位姿。无人机机体坐标系是相对于机身前（x）、右（y）、下（z）。相机在无人机上水平朝前安装，安装方向相对于机体是右（x），下（y），前（z），可参见图6中序号1所标识的位置。而图6中序号2所示的位置指的是机载相机坐标系。机体坐标系与机载相机坐标系的变换关系固定。
构造因子图模型的方法为：
无人机着陆流程如图4示意图所示。本实施例通过将降落过程抽象为一种因子图模型，通过非线性优化方法进行联合求解，以获得更精确，更平滑的降落轨迹。
本实施例用于描述降落过程的因子图模型如图5所示。图中的每个圆圈表示一组待求解变量，不同符号的圆圈表示不同类型的待求解变量。每个方框表示一个因子，不同颜色的方框表示不同类型的因子。因子可以理解为是一个误差项，也就是测量值与理论值之间的误差。不同的因子图模型优化方法，主要区别在于构建的因子图模型的结构以及不同因子的噪声设置。求解因子图模型的方法一般都是常规的非线性优化方法。
图5中序号1所示的圆圈表示某个时刻的无人机位姿，是一个东北天坐标系下的六自由度的位姿变量，包括三自由度位置和三自由度姿态，东北天坐标系的站心建立在机场中心经纬坐标，海拔高度为零的位置。图5中序号2表示该位置的地面距离海平面的层高，即当前位置地面上的海拔高度。东北天坐标系是以地表上某一点为站心建立的坐标系，坐标系站心确定后就不再改变，其能够以任意一点为站心，本实施例是以跑道中心为站心。
图5中序号3表示视觉定位因子，也即计算无人机的当前位姿求解得到的值构造的因子。由于视觉定位计算得到的高度是场高，需要加上该位置地面距离海平面的层高才能得到无人机的海拔高度。无人机距离跑道较远时视觉定位误差大，无人机距离跑道较近时视觉定位误差小。本实施例可以统计一个10秒的时间窗口内视觉定位的平均方差并乘以一个系数，作为视觉定位因子的噪声。
图5中序号4表示电磁高度表的测量因子。电磁高度表可以得到无人机距离地面的场高，但由于地面可能高低起伏变化，不同位置的场高有可能不同，因此电磁高度表的观测误差可能较大，但从全局来看，电磁高度表的测误差在一定范围内。本实施例将电磁高度表的测量作为多源信息也添加到因子图模型中，同时设置一个稍大一点的噪声。加入电磁高度表因子使得系统不受电磁高度表测量噪声影响的同时，把高度约束在一定范围内。
图5中序号5表示气压计因子，气压计可以测得无人机的海拔高，误差较大，不过测量值相对稳定。本实施例中将气压计高度也作为多源信息也添加到因子图模型中，同时设置一个稍大一点的噪声。使得无人机高度和层高值能被约束的更稳定。
图5中序号6表示机载惯性导航系统因子，是由机载惯性导航系统提供的相对运动约束。大型固定翼无人机使用光纤机载惯性导航系统或高精度机载惯性导航系统，在连续时间段内具有非常高的精度，因此给机载惯性导航系统因子设置的误差非常小。
图5中序号7表示层高随机游走因子，该因子的作用是将连续时刻之间的层高变化限制在一定范围内，使得高度估计更稳定。
本实施例的因子图模型可以构建在一个大小为100秒的时间窗口内，新帧到来后构造各类新因子并插入因子图模型进行求解。当因子图模型中最早（最开始）的帧与当前帧的时间差大于100秒，则边缘化掉最早的帧。边缘化指的是将与最早帧相关的因子通过边缘化运算，转化为一个边缘化因子，使得边缘化后的因子图模型不再包含最早帧，同时又不丢失旧帧的信息。
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。

Claims (10)

1.一种GNSS拒止环境下无人机自主着陆定位方法，其特征在于，包括前端和后端两个线程，前端线程负责根据机载惯性导航系统积分实时推测无人机的当前位置；后端线程负责视觉定位，构造信息融合因子图模型，优化并更新因子图模型中关键节点的位置坐标；
前端线程的处理过程包括：
接收到多源数据包后，首先通过对机载惯性导航系统获取的三轴加速度和三轴角速度进行积分，得到当前帧时刻相对于上一帧时刻的无人机相对运动，再根据相对运动和旧关键帧的东北天坐标系下的位姿，得到当前帧的东北天坐标系下的位姿；然后判断后端是否正在执行优化；旧关键帧指的是经过后端优化的最近一帧数据；
后端线程在执行完一次优化后就进入休眠状态，当检测到后端优化队列中有新数据加入时，从队列中提取出新数据，再次执行优化。
2.根据权利要求1所述的方法，其特征在于，在所述接收到多源数据包之前，还包括：
获取无人机着陆前，其前视相机观测无人机跑道的每一帧图像采样时刻对应的多源数据，并对该采样时刻的多源数据打包储在一个类中，并以独占指针的形式传递给系统的前端线程。
3.根据权利要求2所述的方法，其特征在于，所述多源数据的获取过程，包括：
分别设置无人机的飞行管理计算机上的机载相机、机载惯性导航系统、气压计、电磁高度表的数据采样频率，机载相机的采样频率最低；
以每一帧图像的采样时刻为基准，对机载惯性导航系统、气压计、电磁高度表的数据进行插值，得到与每一帧图像采样时刻相对应的多源数据。
4.根据权利要求1所述的方法，其特征在于，所述判断后端是否正在执行优化，包括：
若没有执行优化，则将下一个多源数据包连同积分的东北天坐标添加到后端优化队列，同时将当前推测的东北天坐标发送给无人机的飞行管理计算机；
若后端正在执行优化，则直接将当前推测的东北天坐标发送给无人机的飞行管理计算机。
5.根据权利要求1所述的方法，其特征在于，所述执行优化，包括：
从图像中识别出跑道，并提取出跑道的四个角点，根据跑道四个角点的像素位置和对应的角点GNSS坐标，计算无人机的当前位姿；
用多源数据构造因子并将其添加到因子图模型中，然后执行因子图模型优化，根据优化结果更新因子图模型中节点的位姿；最后执行边缘化，将过早的节点边缘化，以减少每次执行优化的时间。
6.根据权利要求5所述的方法，其特征在于，所述根据跑道四个角点的像素位置和对应的角点GNSS坐标，计算无人机的当前位姿，包括：
利用机载惯性导航系统提供的无人机的姿态信息，计算出机载相机在东北天坐标系下的姿态；
基于机载相机在东北天坐标系下的姿态，获取机场跑道四个角点在相机坐标系下的坐标，继而构造线性方程组，通过求解该线性方程组以得到人机机体坐标系在东北天坐标系下的位姿。
7.根据权利要求6所述的方法，其特征在于，所述利用机载惯性导航系统提供的无人机的姿态信息，计算出机载相机在东北天坐标系下的姿态，包括：
假设机载相机上观测到的跑道四个角点的像素坐标分别是
Figure QLYQS_2
，/>
Figure QLYQS_5
，/>
Figure QLYQS_7
，
Figure QLYQS_3
，构造四个角点的齐次坐标，并通过如下公式投影到相机坐标系的深度归一化平面上，分别得到四个向量/>
Figure QLYQS_4
、/>
Figure QLYQS_6
、/>
Figure QLYQS_8
、/>
Figure QLYQS_1
；
Figure QLYQS_9
其中，在图像坐标系中，
Figure QLYQS_11
、/>
Figure QLYQS_13
分别是左上角点的横坐标和纵坐标，/>
Figure QLYQS_16
、/>
Figure QLYQS_12
分别是右上角点的横坐标和纵坐标，/>
Figure QLYQS_15
、/>
Figure QLYQS_18
分别是左下角点的横坐标和纵坐标，/>
Figure QLYQS_19
、/>
Figure QLYQS_10
分别是右下角点的横坐标和纵坐标，/>
Figure QLYQS_14
为相机的内参矩阵；/>
Figure QLYQS_17
为图像上的素点投影到三维空间下，在相机坐标系下的坐标；图像的左上角像素为原点，横轴向右是u，纵轴向下是v，u、v的大小是距离左上角像素的像素数量；
假设跑道的四个角点的实际GNSS坐标已知，将其变换到东北天坐标系下，分别用向量
Figure QLYQS_20
、/>
Figure QLYQS_21
、/>
Figure QLYQS_22
、/>
Figure QLYQS_23
表示；
通过机载惯性导航系统得到无人机在北东地坐标系下的姿态，通过坐标系变换关系，得到相机在东北天坐标系下的姿态，记为旋转矩阵
Figure QLYQS_24
。
8.根据权利要求7所述的方法，其特征在于，所述基于机载相机在东北天坐标系下的姿态，获取机场跑道四个角点在相机坐标系下的坐标，继而构造线性方程组，通过求解该线性方程组以得到人机机体坐标系在东北天坐标系下的位姿，包括：
机载相机在东北天坐标系下的位置为未知量，用三维向量
Figure QLYQS_25
表示；机场跑道的角点通过/>
Figure QLYQS_26
和/>
Figure QLYQS_27
变换到相机坐标系下，变换关系为：
Figure QLYQS_28
通过该变换关系，分别得到机场跑道四个角点在相机坐标系下的坐标
Figure QLYQS_52
、/>
Figure QLYQS_54
、/>
Figure QLYQS_55
、
Figure QLYQS_30
，当向量/>
Figure QLYQS_36
与/>
Figure QLYQS_42
，/>
Figure QLYQS_48
与/>
Figure QLYQS_32
，/>
Figure QLYQS_37
与/>
Figure QLYQS_43
，/>
Figure QLYQS_49
与/>
Figure QLYQS_34
尽可能平行时，此时的/>
Figure QLYQS_40
即为求解得到的相机东北天坐标；分别计算向量/>
Figure QLYQS_47
、/>
Figure QLYQS_53
、/>
Figure QLYQS_33
、/>
Figure QLYQS_39
的两个零空间向量，记为/>
Figure QLYQS_45
、/>
Figure QLYQS_51
，/>
Figure QLYQS_29
、/>
Figure QLYQS_35
，/>
Figure QLYQS_41
、/>
Figure QLYQS_46
，/>
Figure QLYQS_31
、/>
Figure QLYQS_38
；/>
Figure QLYQS_44
与/>
Figure QLYQS_50
平行时，满足如下等式：
Figure QLYQS_56
四个角点能够构造八个等式，将等式写成矩阵形式的线性方程组：
Figure QLYQS_57
其中，A为系数矩阵，x为待求解变量，即
Figure QLYQS_58
，b为常数项；用最小二乘法求解该线性方程组，所得解即为机载相机在东北天坐标系下的位置坐标，通过变换得到无人机机体坐标系在东北天坐标系下的位姿。
9.根据权利要求5所述的方法，其特征在于，
优化后的因子图模型包括第一类节点、第二类节点、第一类因子、第二类因子、第三类因子、第四类因子、第五类因子；
第一类节点之间通过第四类因子连接成一条直线，记为第一直线；第二类节点之间通过第五类因子连接成一条直线，记为第二直线；第一直线与第二直线平行；
每个第一类节点连接一个第三类因子；每个第一类节点分别通过第一类因子和第二类因子与第二类节点连接；
第一类节点表示东北天坐标系下任一时刻无人机的无人机位姿；该无人机位姿是一个六自由度的位姿变量，包括三自由度位置和三自由度姿态，东北天坐标系的站心建立在机场中心经纬坐标，海拔高度为零的位置；
第二类节点表示当前位置地面上的海拔高度；
第一类因子表示视觉定位因子，也即无人机的位姿；第二类因子表示电磁高度表的测量因子；第三类因子表示气压计因子；第四类因子表示机载惯性导航系统因子；第五类因子表示层高随机游走因子。
10.根据权利要求9所述的方法，其特征在于，所述因子图模型构建在时间窗口内，新的一帧到来后构造各类新因子并插入因子图模型进行求解；当因子图模型中最开始的帧与当前帧的时间差大于预设时间时，则边缘化掉最开始的帧；边缘化指的是将与最开始的帧相关的因子通过边缘化运算，转化为一个边缘化因子，使得边缘化后的因子图模型不再包含最开始的帧，同时又不丢失最开始的帧的信息。
CN202310654364.0A 2023-06-05 2023-06-05 一种gnss拒止环境下无人机自主着陆定位方法 Active CN116380057B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310654364.0A CN116380057B (zh) 2023-06-05 2023-06-05 一种gnss拒止环境下无人机自主着陆定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310654364.0A CN116380057B (zh) 2023-06-05 2023-06-05 一种gnss拒止环境下无人机自主着陆定位方法

Publications (2)

Publication Number Publication Date
CN116380057A true CN116380057A (zh) 2023-07-04
CN116380057B CN116380057B (zh) 2023-08-29

Family

ID=86971630

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310654364.0A Active CN116380057B (zh) 2023-06-05 2023-06-05 一种gnss拒止环境下无人机自主着陆定位方法

Country Status (1)

Country Link
CN (1) CN116380057B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190248487A1 (en) * 2018-02-09 2019-08-15 Skydio, Inc. Aerial vehicle smart landing
CN111649737A (zh) * 2020-05-08 2020-09-11 中国航空工业集团公司西安航空计算技术研究所 一种面向飞机精密进近着陆的视觉-惯性组合导航方法
US10782418B1 (en) * 2019-11-28 2020-09-22 Beihang University Calculation method for visual navigation integrity monitoring
CN113371179A (zh) * 2021-05-26 2021-09-10 杭州湘豫科技有限公司 一种无人机用气动反推式起落架及其使用方法
CN113790719A (zh) * 2021-08-13 2021-12-14 北京自动化控制设备研究所 一种基于线特征的无人机惯性/视觉着陆导航方法
CN113932804A (zh) * 2021-09-17 2022-01-14 四川腾盾科技有限公司 机场跑道视觉与gnss/惯导导航相结合的定位方法
CN115016545A (zh) * 2022-08-05 2022-09-06 四川腾盾科技有限公司 无人机着陆的着陆点自主选择方法、设备及介质

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190248487A1 (en) * 2018-02-09 2019-08-15 Skydio, Inc. Aerial vehicle smart landing
US10782418B1 (en) * 2019-11-28 2020-09-22 Beihang University Calculation method for visual navigation integrity monitoring
CN111649737A (zh) * 2020-05-08 2020-09-11 中国航空工业集团公司西安航空计算技术研究所 一种面向飞机精密进近着陆的视觉-惯性组合导航方法
CN113371179A (zh) * 2021-05-26 2021-09-10 杭州湘豫科技有限公司 一种无人机用气动反推式起落架及其使用方法
CN113790719A (zh) * 2021-08-13 2021-12-14 北京自动化控制设备研究所 一种基于线特征的无人机惯性/视觉着陆导航方法
CN113932804A (zh) * 2021-09-17 2022-01-14 四川腾盾科技有限公司 机场跑道视觉与gnss/惯导导航相结合的定位方法
CN115016545A (zh) * 2022-08-05 2022-09-06 四川腾盾科技有限公司 无人机着陆的着陆点自主选择方法、设备及介质

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
FRANCESCA BALDINI等: "Learning Pose Estimation for UAV Autonomous Navigation and Landing Using Visual-Inertial Sensor Data", 《2020 AMERICAN CONTROL CONFERENCE (ACC)》, pages 2961 - 2966 *
刁灿;王英勋;王金提;苗淼;: "无人机自动着陆中的机器视觉辅助技术", 航空学报, no. 1 *
张怀捷等: "视觉与惯性融合的多旋翼飞行机器人室内定位技术", 《航空学报》, vol. 44, no. 5, pages 1 - 13 *
徐昊玮;廉保旺;刘尚波;: "基于滑动窗迭代最大后验估计的多源组合导航因子图融合算法", 兵工学报, no. 04 *
陆满君;席超敏;戚漫华;: "一种基于合作目标的视觉定位方法", 制导与引信, no. 02 *

Also Published As

Publication number Publication date
CN116380057B (zh) 2023-08-29

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN109887057B (zh) 生成高精度地图的方法和装置
CN113124856B (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
KR20210111180A (ko) 위치 추적 방법, 장치, 컴퓨팅 기기 및 컴퓨터 판독 가능한 저장 매체
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN111338383B (zh) 基于gaas的自主飞行方法及系统、存储介质
CN108592914A (zh) 无gps情况下的复杂区域巡视机器人定位、导航及授时方法
Steiner et al. A vision-aided inertial navigation system for agile high-speed flight in unmapped environments: Distribution statement a: Approved for public release, distribution unlimited
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN111649737A (zh) 一种面向飞机精密进近着陆的视觉-惯性组合导航方法
CN110929402A (zh) 一种基于不确定分析的概率地形估计方法
CN114323033A (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN111679308A (zh) 一种基于多传感器融合的无人车定位方法
CN117710476A (zh) 基于单目视觉的无人机位姿估计与稠密建图方法
CN117490683A (zh) 一种井下隧道多传感器融合算法的定位建图方法
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及系统
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统
CN116380057B (zh) 一种gnss拒止环境下无人机自主着陆定位方法
CN112923934A (zh) 一种适用于非结构化场景中结合惯导的激光slam技术
CN115562076A (zh) 用于无人驾驶矿车的仿真系统、方法以及存储介质
CN115290090A (zh) 基于多传感器信息融合的slam地图构建方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant