CN116380057B - 一种gnss拒止环境下无人机自主着陆定位方法 - Google Patents
一种gnss拒止环境下无人机自主着陆定位方法 Download PDFInfo
- Publication number
- CN116380057B CN116380057B CN202310654364.0A CN202310654364A CN116380057B CN 116380057 B CN116380057 B CN 116380057B CN 202310654364 A CN202310654364 A CN 202310654364A CN 116380057 B CN116380057 B CN 116380057B
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 238000005457 optimization Methods 0.000 claims abstract description 50
- 230000000007 visual effect Effects 0.000 claims abstract description 24
- 230000001133 acceleration Effects 0.000 claims abstract description 4
- 238000005070 sampling Methods 0.000 claims description 18
- 239000013598 vector Substances 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 13
- 230000009466 transformation Effects 0.000 claims description 11
- 238000005259 measurement Methods 0.000 claims description 6
- 230000004927 fusion Effects 0.000 claims description 5
- 230000001131 transforming effect Effects 0.000 claims description 5
- 238000010606 normalization Methods 0.000 claims description 3
- 230000002028 premature Effects 0.000 claims description 3
- 238000005295 random walk Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 5
- 230000004807 localization Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 1
- 239000003086 colorant Substances 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
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拒止场景下，无人机可以根据相机对机场跑道的观测来估计自身位置，称为视觉定位。但受天气和相机图像分辨率的影响，视觉定位精度低、噪声大，难以直接用于引导无人机着陆。一种平滑无人机视觉定位结果的方法是将其与机载惯性导航系统融合。目前的机载惯性导航系统信息融合方法基本都是基于扩展卡尔曼滤波(EKF)。EKF通过融合上一时刻的后验信息和当前时刻的先验信息来平滑视觉定位结果。由于EKF每次融合可以参考的信息太少，在视觉定位误差过大的情况下，EKF难以获得较好的效果。
发明内容
鉴于此，本发明提供一种GNSS拒止环境下无人机自主着陆定位方法，以解决上述技术问题。
本发明公开了一种GNSS拒止环境下无人机自主着陆定位方法，其包括前端和后端两个线程，前端线程负责根据机载惯性导航系统积分实时推测无人机的当前位置；后端线程负责视觉定位，构造信息融合因子图模型，优化并更新因子图模型中关键节点的位置坐标；
前端线程的处理过程包括：
接收到多源数据包后，首先通过对机载惯性导航系统获取的三轴加速度和三轴角速度进行积分，得到当前帧时刻相对于上一帧时刻的无人机相对运动，再根据相对运动和旧关键帧的东北天坐标系下的位姿，得到当前帧的东北天坐标系下的位姿；然后判断后端是否正在执行优化；旧关键帧指的是经过后端优化的最近一帧数据；
后端线程在执行完一次优化后就进入休眠状态，当检测到后端优化队列中有新数据加入时，从队列中提取出新数据，再次执行优化。
进一步地，在所述接收到多源数据包之前，还包括：
获取无人机着陆前，其前视相机观测无人机跑道的每一帧图像采样时刻对应的多源数据，并对该采样时刻的多源数据打包储在一个类中，并以独占指针的形式传递给系统的前端线程。
进一步地，所述多源数据的获取过程，包括：
分别设置无人机的飞行管理计算机上的机载相机、机载惯性导航系统、气压计、电磁高度表的数据采样频率，机载相机的采样频率最低；
以每一帧图像的采样时刻为基准，对机载惯性导航系统、气压计、电磁高度表的数据进行插值，得到与每一帧图像采样时刻相对应的多源数据。
进一步地，所述判断后端是否正在执行优化，包括：
若没有执行优化，则将下一个多源数据包连同积分的东北天坐标添加到后端优化队列，同时将当前推测的东北天坐标发送给无人机的飞行管理计算机；
若后端正在执行优化，则直接将当前推测的东北天坐标发送给无人机的飞行管理计算机。
进一步地，所述执行优化，包括：
从图像中识别出跑道，并提取出跑道的四个角点，根据跑道四个角点的像素位置和对应的角点GNSS坐标，计算无人机的当前位姿；
用多源数据构造因子并将其添加到因子图模型中，然后执行因子图模型优化，根据优化结果更新因子图模型中节点的位姿；最后执行边缘化，将过早的节点边缘化，以减少每次执行优化的时间。
进一步地，所述根据跑道四个角点的像素位置和对应的角点GNSS坐标，计算无人机的当前位姿，包括：
利用机载惯性导航系统提供的无人机的姿态信息，计算出机载相机在东北天坐标系下的姿态；
基于机载相机在东北天坐标系下的姿态，获取机场跑道四个角点在相机坐标系下的坐标，继而构造线性方程组，通过求解该线性方程组以得到人机机体坐标系在东北天坐标系下的位姿。
进一步地，所述利用机载惯性导航系统提供的无人机的姿态信息，计算出机载相机在东北天坐标系下的姿态，包括：
假设机载相机上观测到的跑道四个角点的像素坐标分别是，/>，，/>，构造四个角点的齐次坐标，并通过如下公式投影到相机坐标系的深度归一化平面上，分别得到四个向量/>、/>、/>、/>；
其中，在图像坐标系中，、/>分别是左上角点的横坐标和纵坐标，/>、/>分别是右上角点的横坐标和纵坐标，/>、/>分别是左下角点的横坐标和纵坐标，/>、/>分别是右下角点的横坐标和纵坐标，/>为相机的内参矩阵；/>为图像上的像素点投影到三维空间下，在相机坐标系下的坐标；图像的左上角像素为原点，横轴向右是u，纵轴向下是v，u、v的大小是距离左上角像素的像素数量；
假设跑道的四个角点的实际GNSS坐标已知，将其变换到东北天坐标系下，分别用向量、/>、/>、/>表示；
通过机载惯性导航系统得到无人机在北东地坐标系下的姿态，通过坐标系变换关系，得到相机在东北天坐标系下的姿态，记为旋转矩阵。
进一步地，所述基于机载相机在东北天坐标系下的姿态，获取机场跑道四个角点在相机坐标系下的坐标，继而构造线性方程组，通过求解该线性方程组以得到人机机体坐标系在东北天坐标系下的位姿，包括：
机载相机在东北天坐标系下的位置为未知量，用三维向量表示；机场跑道的角点通过/>和/>变换到相机坐标系下，变换关系为：
通过该变换关系，分别得到机场跑道四个角点在相机坐标系下的坐标、/>、、/>，当向量/>与/>，/>与/>，/>与/>，/>与/>尽可能平行时，此时的/>即为求解得到的相机东北天坐标；分别计算向量/>、/>、/>、/>的两个零空间向量，记为/>、，/>、/>，/>、/>，/>、/>；/>与/>平行时，满足如下等式：
四个角点能够构造八个等式，将等式写成矩阵形式的线性方程组：
其中，A为系数矩阵，x为待求解变量，即，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求解的位姿误差很大。由于无人机机载惯性导航系统可以提供比较精确的姿态信息，本实施例利用机载惯性导航系统提供的无人机姿态信息计算出相机的姿态，从而构造一个只求解位置的视觉定位问题，通过减少求解量以提高视觉定位精度。
假设相机上观测到的跑道四个角点的像素坐标分别是，/>，/>，/>。构造四个角点的齐次坐标，并通过以下公式投影到相机坐标系的深度归一化平面上：
其中，在图像坐标系中，、/>分别是左上角点的横坐标和纵坐标，/>、/>分别是右上角点的横坐标和纵坐标，/>、/>分别是左下角点的横坐标和纵坐标，/>、/>分别是右下角点的横坐标和纵坐标，/>为相机的内参矩阵；/>为图像上的像素点投影到三维空间下，在相机坐标系下的坐标；图像的左上角像素为原点，横轴向右是u，纵轴向下是v，u、v的大小是距离左上角像素的像素数量；
得到四个向量、/>、/>、/>，如图3中的序号6所示的向量，其中/>为相机的内参矩阵。
假设跑道的四个角点的实际GNSS坐标已知，将其变换到东北天坐标系下，分别用向量、/>、/>、/>表示。
可以通过机载惯性导航系统得到无人机在北东地坐标系下的姿态，通过坐标系变换关系可以得到相机在东北天坐标系下的姿态，表示为旋转矩阵，相机在东北天坐标系下的位置为未知量，用三维向量/>表示。于是，机场跑道的角点可以通过/>和变换到相机坐标系下，变换关系为：
得到机场跑道四个角点在相机坐标系下的坐标、/>、/>、/>，如图3中序号7所示。当向量/>与/>，/>与/>，/>与/>，/>与/>尽可能平行时，此时的/>即为求解得到的相机东北天坐标。为了构造求解方程，分别计算向量/>、/>、/>、/>的两个零空间向量，记为/>,/>,/>,/>,/>,/>,/>,/>。/>与/>平行时，满足两条等式：
四个角点共可以构造八条等式，将等式写成矩阵形式的线性方程组：
其中A为系数矩阵，x为待求解变量，也就是，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 (8)
1.一种GNSS拒止环境下无人机自主着陆定位方法，其特征在于，包括前端和后端两个线程，前端线程负责根据机载惯性导航系统积分实时推测无人机的当前位置；后端线程负责视觉定位，构造信息融合因子图模型，优化并更新因子图模型中关键节点的位置坐标；
前端线程的处理过程包括：
接收到多源数据包后，首先通过对机载惯性导航系统获取的三轴加速度和三轴角速度进行积分，得到当前帧时刻相对于上一帧时刻的无人机相对运动，再根据相对运动和旧关键帧的东北天坐标系下的位姿，得到当前帧的东北天坐标系下的位姿；然后判断后端是否正在执行优化；旧关键帧指的是经过后端优化的最近一帧数据；
后端线程在执行完一次优化后就进入休眠状态，当检测到后端优化队列中有新数据加入时，从队列中提取出新数据，再次执行优化；
所述执行优化，包括：
从图像中识别出跑道，并提取出跑道的四个角点，根据跑道四个角点的像素位置和对应的角点GNSS坐标，计算无人机的当前位姿；
用多源数据构造因子并将其添加到因子图模型中，然后执行因子图模型优化，根据优化结果更新因子图模型中节点的位姿；最后执行边缘化，将过早的节点边缘化，以减少每次执行优化的时间；
优化后的因子图模型包括第一类节点、第二类节点、第一类因子、第二类因子、第三类因子、第四类因子、第五类因子；
第一类节点之间通过第四类因子连接成一条直线，记为第一直线；第二类节点之间通过第五类因子连接成一条直线，记为第二直线；第一直线与第二直线平行；
每个第一类节点连接一个第三类因子；每个第一类节点分别通过第一类因子和第二类因子与第二类节点连接；
第一类节点表示东北天坐标系下任一时刻无人机的无人机位姿；该无人机位姿是一个六自由度的位姿变量，包括三自由度位置和三自由度姿态，东北天坐标系的站心建立在机场中心经纬坐标，海拔高度为零的位置；
第二类节点表示当前位置地面上的海拔高度；
第一类因子表示视觉定位因子，也即无人机的位姿；第二类因子表示电磁高度表的测量因子；第三类因子表示气压计因子；第四类因子表示机载惯性导航系统因子；第五类因子表示层高随机游走因子。
2.根据权利要求1所述的方法，其特征在于，在所述接收到多源数据包之前，还包括：
获取无人机着陆前，其前视相机观测无人机跑道的每一帧图像采样时刻对应的多源数据，并对该采样时刻的多源数据打包储在一个类中，并以独占指针的形式传递给系统的前端线程。
3.根据权利要求2所述的方法，其特征在于，所述多源数据的获取过程，包括：
分别设置无人机的飞行管理计算机上的机载相机、机载惯性导航系统、气压计、电磁高度表的数据采样频率，机载相机的采样频率最低；
以每一帧图像的采样时刻为基准，对机载惯性导航系统、气压计、电磁高度表的数据进行插值，得到与每一帧图像采样时刻相对应的多源数据。
4.根据权利要求1所述的方法，其特征在于，所述判断后端是否正在执行优化，包括：
若没有执行优化，则将下一个多源数据包连同积分的东北天坐标添加到后端优化队列，同时将当前推测的东北天坐标发送给无人机的飞行管理计算机；
若后端正在执行优化，则直接将当前推测的东北天坐标发送给无人机的飞行管理计算机。
5.根据权利要求1所述的方法，其特征在于，所述根据跑道四个角点的像素位置和对应的角点GNSS坐标，计算无人机的当前位姿，包括：
利用机载惯性导航系统提供的无人机的姿态信息，计算出机载相机在东北天坐标系下的姿态；
基于机载相机在东北天坐标系下的姿态，获取机场跑道四个角点在相机坐标系下的坐标，继而构造线性方程组，通过求解该线性方程组以得到人机机体坐标系在东北天坐标系下的位姿。
6.根据权利要求5所述的方法，其特征在于，所述利用机载惯性导航系统提供的无人机的姿态信息，计算出机载相机在东北天坐标系下的姿态，包括：
假设机载相机上观测到的跑道四个角点的像素坐标分别是，/>，/>，/>，构造四个角点的齐次坐标，并通过如下公式投影到相机坐标系的深度归一化平面上，分别得到四个向量/>、/>、/>、/>；
其中，在图像坐标系中，、/>分别是左上角点的横坐标和纵坐标，/>、/>分别是右上角点的横坐标和纵坐标，/>、/>分别是左下角点的横坐标和纵坐标，/>、/>分别是右下角点的横坐标和纵坐标，/>为相机的内参矩阵；/>为图像上的像素点投影到三维空间下，在相机坐标系下的坐标；图像的左上角像素为原点，横轴向右是u，纵轴向下是v，u、v的大小是距离左上角像素的像素数量；
假设跑道的四个角点的实际GNSS坐标已知，将其变换到东北天坐标系下，分别用向量、/>、/>、/>表示；
通过机载惯性导航系统得到无人机在北东地坐标系下的姿态，通过坐标系变换关系，得到相机在东北天坐标系下的姿态，记为旋转矩阵。
7.根据权利要求6所述的方法，其特征在于，所述基于机载相机在东北天坐标系下的姿态，获取机场跑道四个角点在相机坐标系下的坐标，继而构造线性方程组，通过求解该线性方程组以得到人机机体坐标系在东北天坐标系下的位姿，包括：
机载相机在东北天坐标系下的位置为未知量，用三维向量表示；机场跑道的角点通过/>和/>变换到相机坐标系下，变换关系为：
通过该变换关系，分别得到机场跑道四个角点在相机坐标系下的坐标、/>、/>、，当向量/>与/>，/>与/>，/>与/>，/>与/>尽可能平行时，此时的/>即为求解得到的相机东北天坐标；分别计算向量/>、/>、/>、/>的两个零空间向量，记为/>、/>，/>、/>，/>、/>，/>、/>；/>与/>平行时，满足如下等式：
四个角点能够构造八个等式，将等式写成矩阵形式的线性方程组：
其中，A为系数矩阵，x为待求解变量，即，b为常数项；用最小二乘法求解该线性方程组，所得解即为机载相机在东北天坐标系下的位置坐标，通过变换得到无人机机体坐标系在东北天坐标系下的位姿。
8.根据权利要求1所述的方法，其特征在于，所述因子图模型构建在时间窗口内，新的一帧到来后构造各类新因子并插入因子图模型进行求解；当因子图模型中最开始的帧与当前帧的时间差大于预设时间时，则边缘化掉最开始的帧；边缘化指的是将与最开始的帧相关的因子通过边缘化运算，转化为一个边缘化因子，使得边缘化后的因子图模型不再包含最开始的帧，同时又不丢失最开始的帧的信息。
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 CN116380057A (zh) | 2023-07-04 |
| CN116380057B true 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 (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 | 四川腾盾科技有限公司 | 无人机着陆的着陆点自主选择方法、设备及介质 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11242144B2 (en) * | 2018-02-09 | 2022-02-08 | Skydio, Inc. | Aerial vehicle smart landing |
-
2023
- 2023-06-05 CN CN202310654364.0A patent/CN116380057B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 (1)
| Title |
|---|
| 视觉与惯性融合的多旋翼飞行机器人室内定位技术;张怀捷等;《航空学报》;第44卷(第5期);第1-13页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116380057A (zh) | 2023-07-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
| CN109887057B (zh) | 生成高精度地图的方法和装置 | |
| CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
| CN109029417B (zh) | 基于混合视觉里程计和多尺度地图的无人机slam方法 | |
| EP3715785A1 (en) | Slam assisted ins | |
| CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| CN113124856B (zh) | 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 | |
| CN111338383B (zh) | 基于gaas的自主飞行方法及系统、存储介质 | |
| CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| CN112987065B (zh) | 一种融合多传感器的手持式slam装置及其控制方法 | |
| 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 | |
| CN110779496B (zh) | 三维地图构建系统、方法、设备和存储介质 | |
| CN113763548B (zh) | 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统 | |
| CN112967392A (zh) | 一种基于多传感器触合的大规模园区建图定位方法 | |
| CN110929402A (zh) | 一种基于不确定分析的概率地形估计方法 | |
| Andert et al. | Optical-aided aircraft navigation using decoupled visual SLAM with range sensor augmentation | |
| CN117387604A (zh) | 基于4d毫米波雷达和imu融合的定位与建图方法及系统 | |
| CN116576849A (zh) | 一种基于gmm辅助的车辆融合定位方法及系统 | |
| CN116380057B (zh) | 一种gnss拒止环境下无人机自主着陆定位方法 | |
| CN112923934A (zh) | 一种适用于非结构化场景中结合惯导的激光slam技术 | |
| CN111679308A (zh) | 一种基于多传感器融合的无人车定位方法 | |
| CN115930948A (zh) | 一种果园机器人融合定位方法 | |
| CN115290090A (zh) | 基于多传感器信息融合的slam地图构建方法 | |
| CN114812554A (zh) | 基于滤波的多源融合机器人室内绝对定位方法 | |
| Ready et al. | Inertially aided visual odometry for miniature air vehicles in gps-denied environments |
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 |