CN117739972A - 一种无全球卫星定位系统的无人机进近阶段定位方法 - Google Patents
一种无全球卫星定位系统的无人机进近阶段定位方法 Download PDFInfo
- Publication number
- CN117739972A CN117739972A CN202410180418.9A CN202410180418A CN117739972A CN 117739972 A CN117739972 A CN 117739972A CN 202410180418 A CN202410180418 A CN 202410180418A CN 117739972 A CN117739972 A CN 117739972A
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- matrix
- optical flow
- key frame
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 238000013459 approach Methods 0.000 title claims abstract description 14
- 238000005259 measurement Methods 0.000 claims abstract description 41
- 230000003287 optical effect Effects 0.000 claims abstract description 40
- 238000006073 displacement reaction Methods 0.000 claims abstract description 17
- 238000010276 construction Methods 0.000 claims abstract description 8
- 238000001514 detection method Methods 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 47
- 230000001133 acceleration Effects 0.000 claims description 20
- 230000008569 process Effects 0.000 claims description 12
- 238000001914 filtration Methods 0.000 claims description 9
- 238000012545 processing Methods 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 6
- 239000013598 vector Substances 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 230000004927 fusion Effects 0.000 description 4
- 238000005457 optimization Methods 0.000 description 4
- 238000009825 accumulation Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000005111 flow chemistry technique Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
- 238000005303 weighing Methods 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
本发明涉及无人机导航技术领域，公开一种无全球卫星定位系统的无人机进近阶段定位方法，包括步骤：采集无人机在进近阶段航拍的数据图像，通过光流法获取相邻帧间特征点的像素位移，追踪帧间特征点运动轨迹；使用惯性测量单元获取的数据和光流传感器获取的数据预测无人机下一时刻的运动状态，并在下一时刻与光流传感器获取的实际运动状态进行比较，以提高预测能力；创建局部地图构建线程，通过纠正累积误差，解决漂移问题，完成闭环检测，对无人机周围环境进行实时更新。本发明的目的在于对全球卫星定位系统拒止环境下无人机的进近阶段实现精准的定位与导航。
Description
技术领域
本发明涉及无人机导航技术领域，特别涉及一种无全球卫星定位系统的无人机进近阶段定位方法。
背景技术
无人机导航定位可以看作是一个机器人如何安全快速地到达目标位置的计划过程，主要依赖于当前的环境和位置。为了工程完成预定任务，无人机必须完全了解任务状态，包括位置、导航速度、航向以及起点和目标位置。迄今而至，已经提出了各种导航方法，主要分为惯性导航、卫星导航和基于视觉的导航三类。其中基于视觉的导航是使用视觉传感器，被证明是自主导航最有前途的研究方向。
通常，无人机从外部感知和本体感知传感器中获取自身和周围环境的信息状态。用于导航的传统传感器主要是全球卫星定位系统（Global Navigation SatelliteSystem，GNSS）、轴加速度计、陀螺仪和惯性导航系统（Inertial Navigation System，INS）。视觉导航与GNSS等传统传感器相比，因其干扰能力强而高度适合于动态环境的感知，可以获得丰富的周围环境信息，包括颜色、纹理及其他视觉信息。
无人机在进近阶段，容易受到城市复杂地形和电磁环境的影响，高层建筑会对GNSS信号产生多径干扰和屏蔽效应，因此无人机缺乏有效的信息获取手段，难以在GNSS受到干扰的情况下保证自主飞行能力，增加空中碰撞的风险。
因此在GNSS拒止环境下，无人机需要依赖其他传感器和技术来获取位置信息，而视觉传感器可以感知周围环境并估计无人机位置，如光流传感器、视觉同步定位与地图构建（Simultaneous Localization and Mapping，SLAM）等，这对于无人机导航尤为重要。那么具体如何使用视觉传感器，为无人机进近阶段的安全提供保障，依然是有待研究的问题。
发明内容
本发明的目的在于对GNSS拒止环境下无人机的进近阶段实现精准的定位与导航，提供一种无全球卫星定位系统的无人机进近阶段定位方法。
为了实现上述发明目的，本发明实施例提供了以下技术方案：
一种无全球卫星定位系统的无人机进近阶段定位方法，包括以下步骤：
步骤1，采集无人机在进近阶段航拍的数据图像，通过光流法获取相邻帧间特征点的像素位移，追踪帧间特征点运动轨迹；
步骤2，使用惯性测量单元获取的数据和光流传感器获取的数据预测无人机下一时刻的运动状态，并在下一时刻与光流传感器获取的实际运动状态进行比较，以提高预测能力；
步骤3，创建局部地图构建线程，通过纠正累积误差，解决漂移问题，完成闭环检测，对无人机周围环境进行实时更新。
与现有技术相比，本发明的有益效果：
本发明对GNSS拒止环境下无人机的进近阶段定位算法的研究具有极其重要的意义，其稳定可靠的特性使得无人机在各种复杂环境下都能够实现精准的定位与导航，而且不受天气、光照和建筑物等外界因素的干扰，进一步提高了系统的可靠性和适用性。本发明具有以下的几个特点和优点：
（1）本发明基于惯性测量单元（IMU）和光流传感器融合的定位算法，以解决单一惯性导航系统（INS）在时间上精度发散、积分积累误差逐渐增大的问题。通过引入光流传感器进行信息融合，将光流传感器的实际测量值用于修正惯性测量单元获取的数据中的时间积累误差。接着，利用卡尔曼滤波算法将两者融合，实现更为精确的无人机实时定位与位姿估计，从而实现飞机的稳定自主飞行。相较于单一的惯性导航系统，本发明充分利用光流传感器的数据，通过对惯性测量单元的修正，有效地减小了定位过程中的误差积累；光流传感器获取的数据提供了对相对运动的更准确感知，从而改善了无人机的导航性能。
（2）本发明采用惯性测量单元和光流传感器作为两种传感器来实现无人机的定位任务，这一设计具有成本较低和适用环境广泛的优势。惯性测量单元和光流传感器的组合不仅在经济上更为可行，而且能够适应多样化的环境条件，为无人机提供了可靠的定位能力。此外，本发明所采用的两种传感器的体积小，重量轻便，因而显著减小了无人机的机载负担，这对于无人机而言尤为关键，因为轻量化设计有助于提高飞行性能、延长续航时间，并且对飞机结构和电池寿命等方面都产生积极影响。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1为本发明方法流程示意图。
具体实施方式
下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本发明的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性，或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。另外，术语“相连”、“连接”等可以是元件之间直接相连，也可以是经由其他元件的间接相连。
为了应对GNSS拒止条件下无人机进近阶段的定位问题，本发明通过融合惯性测量单元（以下简称IMU）和光流传感器的信息，进行分析处理后获取无人机的位姿信息，从而实现在GNSS拒止环境下的高精度定位。
具体而言，本发明通过下述技术方案实现，如图1所示，一种无全球卫星定位系统的无人机进近阶段定位方法，包括以下步骤：
步骤1，采集无人机在进近阶段航拍的数据图像，通过光流法获取相邻帧间特征点的像素位移，追踪帧间特征点运动轨迹。
无人机为旋翼无人机，相机固定安装在无人机上，在进近阶段拍摄数据图像，对数据图像进行时间戳对应处理，构成运行数据集。
通过光流法选择运行数据集中关键帧（如第k帧）的特征点，并对特征点进行光流跟踪，使用PnPRANsac（Perspective-n-Point with Random Sample Consensus）去除错误特征点匹配对，以估计无人机位姿。通过光流法融合特征点的方式获得相邻帧之间特征点的像素位移，追踪帧间的运动轨迹，与静态特征点进行融合以提高对动态场景的处理能力。
步骤2，使用惯性测量单元获取的数据和光流传感器获取的数据预测无人机下一时刻的运动状态，并在下一时刻与光流传感器获取的实际运动状态进行比较，以提高预测能力。
GNSS拒止环境下，IMU根据加速度计和陀螺仪的输出，得到无人机的加速度和角速度；光流传感器通过计算机视觉技术连续采集图像，并分析图像数据矩阵，以获得速度的偏移量。用卡尔曼滤波算法对IMU数据和光流传感器数据进行解析，结合当前时刻估计的位置、位姿信息，来预测下一时刻的位置、位姿信息；然后将光流传感器测量值与预测的状态进行比较，得到光流观测值，并使用卡尔曼滤波算法对预测的状态进行更新，得以优化，通过权衡测量和预测的误差，使估计的位置、位姿信息更加准确。
卡尔曼滤波算法包括预测和更新两个步骤，其中预测步骤用于估计系统的下一个运动状态，更新步骤用于将光流传感器实际测量值融合到估计的运动状态中。
对于估计的运动状态，通常包括位置、速度、加速度等。系统的动态模型、状态方程，以及相应的状态转移矩阵（A）和过程噪声协方差矩阵（Q），可以使用IMU提供的加速度和角速度进行建模，原理如下：
IMU的测量值和真值的关系为：
（1）
（2）
其中，I表示IMU坐标系；G表示全局坐标系；a为加速度真值，am为加速度测量值；为角速度真值，/>为角速度测量值；na为加速度测量值的白噪声；/>为角速度测量值的白噪声；ba为加速度测量值零偏；/>为角速度测量值零偏；/>为从全局坐标系到IMU坐标系的旋转矩阵；g为重力加速度。
IMU的运动模型为：
（3）
（4）
（5）
（6）
（7）
（8）
其中、/>、/>分别表示全局坐标系的位置、速度和姿态；/>、/>、/>分别表示、/>、/>的导数；/>表示重力矢量，/>表示/>的导数；/>为角速度的高斯噪声；/>为加速度的高斯噪声；/>、/>分别为/>和/>的导数；符号/>表示向量/>的反对称矩阵，N3表示一个3阶矩阵。
用IMU提供的运动模型构建卡尔曼滤波算法的状态方程，状态方程为：
（9）
其中，Xk表示k时刻机器人的状态向量；f表示运动模型，描述了机器人在时刻k-1到时刻k之间的运动；Uk表示机器人的控制输入，来自IMU的加速度和角速度；为运动模型中的过程噪音，表示在运动过程中的不确定性。
系统的观测方程由光流传感器解析得到的数据进行建模，原理如下：
通过本方案改进的光流法对相邻帧间特征点的像素梯度和灰度变化进行求和：
（10）
其中，lx、ly分别表示特征点在x、y方向上的像素梯度，可通过Sobel算子等方法计算获得；lt表示相邻帧之间的时间差异，即灰度变化，可通过像素值相减获得；u、v分别表示特征点水平方向和垂直方向的速度分量，通过高斯牛顿法求解式（10）可得特征点的运动速度(u,v)，从而得到相邻帧特征点位移增量wk；[lx,ly]为lx、ly的矩阵形式；[u,v]为u、v的矩阵形式，[u,v]T为[u,v]的矩阵转置。
假设当前帧为第k帧，用光流法追踪关键帧k的特征点坐标pk=[uk,vk,1]T，特征点具有三维坐标（其中第3个元素“1”是为了在归一化处理时用来缩放像素坐标，以便将其转换为单位向量），计算关键帧k到相邻关键帧k+1的位移增量wk，可预测得到相邻关键帧k+1的特征点坐标pk+1：
（11）
其中，pk表示第k帧的特征点坐标，pk+1表示第k+1帧的特征点坐标；uk、vk分别表示第k帧特征点水平方向、垂直方向的速度分量；uk+1、vk+1分别表示第k+1帧特征点水平方向、垂直方向的速度分量；wk表示第k帧到第k+1帧的特征点位移增量；dkx、dky分别表示第k帧到第k+1帧中特征点在x轴、y轴的位移增量。
设关键帧k的归一化坐标为(n1,n2)，n1为关键帧k归一化后的横坐标，n2为关键帧k归一化后的纵坐标；相邻关键帧k+1的归一化坐标为，/>为相邻关键帧k+1归一化后的横坐标，/>为相邻关键帧k+1归一化后的纵坐标。将相机在k时刻归一化坐标为(n1,n2)下的深度定义为d，转换成特征点坐标表示为d[n1,n2,1]T，根据相机坐标到世界坐标的Rt矩阵变换，可得关键帧k的特征点坐标Rd[n1,n2,1]T，以及相邻关键帧k+1的特征点坐标Rd[n1,n2,1]T+t，R表示相机坐标系到世界坐标系的矩阵变换。令m=Rd[n1,n2,1]T，则可得相邻关键帧k+1中的相机坐标系Z为：
（12）
其中，d表示k时刻下的深度；m1表示d与m中的n1内积后的参数，m2表示d与m中的n2内积后的参数，m3表示d与m中的1内积后的参数；t表示矩阵变换中的位移常量；t1、t2、t3分别表示m1、m2、m3的位移常量。
将Z乘dm3+t3，可得下一关键帧k+2的相机坐标系Z`：
（13）
用Z`减去Z可得：
（14）
式（14）中的d为k+1时刻需要预测的深度，并要对其进行优化，求取d的最小值。通过Z`减去Z计算实际观察到的特征点在图像平面上的投影位置与由三维点位置计算得到的投影位置之间的重投影误差，通过优化该误差的范数，能够得到最佳的相机深度。最终有助于优化相机的姿态估计，使其更符合实际观测到的特征点。
此时需求的最小值，其中：
（15）
（16）
可得d的最小值：
（17）
其中，、/>、/>、/>、/>、/>为中间参数；；符号/>表示范数。
设关键帧k上的特征点pk的空间三维坐标为(x,y,z)，则有：
（18）
然后将式（18）得到的坐标信息用于构建观测方程，以及相应的观测矩阵（H）和观测噪声协方差矩阵（R）。观测方程如下所示：
（19）
其中，Uk为IMU的测量数据，即解析的加速度和角速度；H为对光流传感器测量值处理后的观测矩阵；为测量噪声。
在卡尔曼滤波开始前，需要对系统的初始状态及其协方差矩阵进行估计，本方案采用无人机的初始位置作为坐标系的原点对系统进行初始化设置。
①预测步骤：使用系统状态方程和IMU的测量值，通过卡尔曼滤波算法的预测步骤来估计系统的下一个状态方程和协方差矩阵。卡尔曼滤波算法的预测方程为：
（20）
（21）
其中，为系统状态的先验估计；/>为系统状态协方差的先验估计；A为状态转移矩阵；AT为A的矩阵转置；B为输入矩阵；Uk为IMU解析的加速度和角速度；Q为过程噪声协方差矩阵。
②更新步骤：在每个采样时间间隔，使用光流传感器的测量值进行更新，以校正状态估计和协方差矩阵，计算公式如下：
（22）
（23）
（24）
其中，Kk为卡尔曼增益；H为对光流传感器测量值处理后的观测矩阵；HT为H的矩阵转置；R为观测噪声协方差矩阵；Uk为k时刻IMU的测量数据；为k时刻的状态预测值；/>为k时刻的状态协方差预测值。
最后重复进行预测和更新步骤，以连续的校正和更新系统状态，得到较为准确的无人机实时位置、位姿信息。
步骤3，创建局部地图构建线程，通过纠正累积误差，解决漂移问题，完成闭环检测，对无人机周围环境进行实时更新。
局部地图构建线程在光流处理之后，通过识别具有显著信息的关键帧用于地图的构建。首先将选中的关键帧插入到局部地图构建线程的缓冲队列中，然后线程对当前缓冲队列进行连续不间断的查询。一旦查询到待处理的关键帧，线程就会对其进行处理，剔除劣质地图点，其中召回率低于0.25的点被认为是劣质地图点。
新插入的关键帧将与其共视程度最高的前10个共视关键帧进行特征匹配，以生成新的地图点。具体而言，该过程包括两个阶段：首先是正向融合，即将当前关键帧的地图点融合到各共视关键帧中；其次是反向融合，即将各共视关键帧的地图点融合到当前关键帧中。这样的匹配和融合操作有助于在地图中建立更全面和准确的特征点。
新生成的地图点会与相同帧中图像的特征点进行匹配，以实现地图点的融合。完成新地图点的创建后，进行局部的BA（Bundle Adjustment）优化。具体来说，当前关键帧的一级共视关键帧的位姿会被优化，而二级共视关键帧会加入优化图，但其位姿不会被优化。此外，所有局部地图点的位姿都会接受优化，以提高整体地图的准确性和一致性。
为了减少冗余和提高计算效率，会剔除冗余的关键帧。冗余关键帧的判断标准是，如果某关键帧观测到的90%以上的地图点都能被超过3个其他关键帧观测到，则认为该关键帧是冗余的。这一步有助于维持地图的精简性和高效性，确保地图中的关键帧和地图点集合是最为优化和紧凑的。整个流程的设计使得局部地图的构建更加稳健和高效，为无人机驾驶在复杂环境中的导航提供了可靠的地图支持。
局部地图构建完成后，系统进入闭环检测阶段。具体而言，如果连续4个关键帧都能在数据库中找到对应的闭环匹配关键帧组，并且这些闭环关键帧组是连续的，那么系统认为闭环已经成功实现。闭环检测阶段包括以下步骤：
步骤3-1，闭环候选关键帧选取：针对当前关键帧，系统找到闭环候选关键帧，这些候选关键帧选自于与当前关键帧具有相同的BOW（词袋）向量，但不存在直接连接的关键帧。
步骤3-2，构建关键组：将找到的闭环候选关键帧和它们的共视关键帧组合成关键组。这一步旨在形成一个潜在的闭环关键帧序列。
步骤3-3，寻找连续关系：在当前关键组和之前的连续关键组之间寻找连续关系。如果在当前关键组和之前的连续关键组中找到了连续关系，那么当前连续关键组的连续长度加1；反之，当前连续关键组的长度为0。若某关键组的连续长度达到3，系统认为该关键帧实现了闭环。
闭环检测阶段的关键在于通过对候选关键帧的选择和关键组的构建，找到具有一定连续性的关键帧序列。一旦满足连续性标准，系统就能够成功检测到闭环。检测到闭环后，系统将进行闭环修正和全局BA（Bundle Adjustment）优化，以提高整个地图的一致性和精确性。闭环检测和优化的过程有助于系统更好地理解环境，纠正地图中可能存在的漂移问题，提高导航的准确性和鲁棒性。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。
Claims (6)
1.一种无全球卫星定位系统的无人机进近阶段定位方法，其特征在于：包括以下步骤：
步骤1，采集无人机在进近阶段航拍的数据图像，通过光流法获取相邻帧间特征点的像素位移，追踪帧间特征点运动轨迹；
步骤2，使用惯性测量单元获取的数据和光流传感器获取的数据预测无人机下一时刻的运动状态，并在下一时刻与光流传感器获取的实际运动状态进行比较，以提高预测能力；
步骤3，创建局部地图构建线程，通过纠正累积误差，解决漂移问题，完成闭环检测，对无人机周围环境进行实时更新。
2.根据权利要求1所述的一种无全球卫星定位系统的无人机进近阶段定位方法，其特征在于：所述步骤2具体包括以下步骤：
对无人机的状态方程、观测方程进行建模；
预测步骤：使用状态方程和惯性测量单元获取的数据，通过卡尔曼滤波算法估计下一时刻的状态方程和协方差矩阵；
更新步骤：使用光流传感器的测量值对预测步骤所估计的下一时刻的状态方程和协方差矩阵进行更新。
3.根据权利要求2所述的一种无全球卫星定位系统的无人机进近阶段定位方法，其特征在于：所述步骤2的具体步骤中，对无人机的状态方程进行建模的步骤，包括：
惯性测量单元的测量值和真值的关系为：
（1）
（2）
其中，I表示惯性测量单元坐标系；G表示全局坐标系；a为加速度真值，am为加速度测量值；为角速度真值，/>为角速度测量值；na为加速度测量值的白噪声；/>为角速度测量值的白噪声；ba为加速度测量值零偏；/>为角速度测量值零偏；/>为从全局坐标系到惯性测量单元坐标系的旋转矩阵；g为重力加速度；
惯性测量单元的运动模型为：
（3）
（4）
（5）
（6）
（7）
（8）
其中、/>、/>分别表示全局坐标系的位置、速度和姿态；/>、/>、/>分别表示/>、、/>的导数；/>表示重力矢量，/>表示/>的导数；/>为角速度的高斯噪声；/>为加速度的高斯噪声；/>、/>分别为/>和/>的导数；符号/>表示向量/>的反对称矩阵，N3表示一个3阶矩阵；
用惯性测量单元提供的运动模型构建卡尔曼滤波算法的状态方程，状态方程为：
（9）
其中，Xk表示k时刻的状态向量；f表示运动模型；Uk表示控制输入，来自惯性测量单元的加速度和角速度；为运动模型中的过程噪音。
4.根据权利要求3所述的一种无全球卫星定位系统的无人机进近阶段定位方法，其特征在于：所述步骤2的具体步骤中，对无人机的观测方程进行建模的步骤，包括：
使用光流法对相邻帧间特征点的像素梯度和灰度变化进行求和：
（10）
其中，lx、ly分别表示特征点在x、y方向上的像素梯度；lt表示相邻帧之间的灰度变化；u、v分别表示特征点水平方向和垂直方向的速度分量，通过高斯牛顿法求解式（10）得特征点的运动速度(u,v)，从而得到相邻帧特征点位移增量wk；[lx,ly]为lx、ly的矩阵形式；[u,v]为u、v的矩阵形式，[u,v]T为[u,v]的矩阵转置；
假设当前关键帧为第k帧，用光流法追踪关键帧k的特征点坐标pk=[uk,vk,1]T，计算关键帧k到相邻关键帧k+1的位移增量wk，预测得到相邻关键帧k+1的特征点坐标pk+1：
（11）
其中，pk表示第k帧的特征点坐标，pk+1表示第k+1帧的特征点坐标；uk、vk分别表示第k帧特征点水平方向、垂直方向的速度分量；uk+1、vk+1分别表示第k+1帧特征点水平方向、垂直方向的速度分量；wk表示第k帧到第k+1帧的特征点位移增量；dkx、dky分别表示第k帧到第k+1帧中特征点在x轴、y轴的位移增量；
设关键帧k的归一化坐标为(n1,n2)，n1为关键帧k归一化后的横坐标，n2为关键帧k归一化后的纵坐标；相邻关键帧k+1的归一化坐标为，/>为相邻关键帧k+1归一化后的横坐标，/>为相邻关键帧k+1归一化后的纵坐标；将相机在k时刻归一化坐标为(n1,n2)下的深度定义为d，转换成特征点坐标表示为d[n1,n2,1]T，根据相机坐标到世界坐标的Rt矩阵变换，得关键帧k的特征点坐标Rd[n1,n2,1]T，以及相邻关键帧k+1的特征点坐标Rd[n1,n2,1]T+t，R表示相机坐标系到世界坐标系的矩阵变换；
令m=Rd[n1,n2,1]T，得相邻关键帧k+1中的相机坐标系Z：
（12）
其中，d表示k时刻下的深度；m1表示d与m中的n1内积后的参数，m2表示d与m中的n2内积后的参数，m3表示d与m中的1内积后的参数；t表示矩阵变换中的位移常量；t1、t2、t3分别表示m1、m2、m3的位移常量；
将Z乘dm3+t3，得下一关键帧k+2的相机坐标系Z`：
（13）
用Z`减去Z得：
（14）
式（14）中的d为k+1时刻需要预测的深度，并要对其进行优化，求取d的最小值：
此时求的最小值，其中：
（15）
（16）
得d的最小值：
（17）
其中，、/>、/>、/>、/>、/>为中间参数；
设关键帧k上的特征点pk的空间三维坐标为(x,y,z)，有：
（18）
将式（18）得到的坐标信息用于构建观测方程，以及相应的观测矩阵和观测噪声协方差矩阵；观测方程为：
（19）
其中，Uk为惯性测量单元的测量数据；H为对光流传感器测量值处理后的观测矩阵；为测量噪声。
5.根据权利要求4所述的一种无全球卫星定位系统的无人机进近阶段定位方法，其特征在于：所述步骤2的具体步骤中，预测步骤具体为：
使用状态方程和惯性测量单元的测量值，通过卡尔曼滤波算法的预测步骤来估计系统下一个时刻的状态方程和协方差矩阵；
卡尔曼滤波算法的预测方程为：
（20）
（21）
其中，为系统状态的先验估计；/>为系统状态协方差的先验估计；A为状态转移矩阵；AT为A的矩阵转置；B为输入矩阵；Uk为惯性测量单元解析的加速度和角速度；Q为过程噪声协方差矩阵。
6.根据权利要求5所述的一种无全球卫星定位系统的无人机进近阶段定位方法，其特征在于：所述步骤2的具体步骤中，更新步骤具体为：
使用光流传感器的测量值对预测步骤估计的状态方程和协方差矩阵进行更新，以校正状态估计和协方差矩阵，计算公式如下：
（22）
（23）
（24）
其中，Kk为卡尔曼增益；H为对光流传感器测量值处理后的观测矩阵；HT为H的矩阵转置；R为观测噪声协方差矩阵；Uk为惯性测量单元的测量数据；为k时刻的状态预测值；/>为k时刻的状态协方差预测值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410180418.9A CN117739972B (zh) | 2024-02-18 | 2024-02-18 | 一种无全球卫星定位系统的无人机进近阶段定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410180418.9A CN117739972B (zh) | 2024-02-18 | 2024-02-18 | 一种无全球卫星定位系统的无人机进近阶段定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117739972A true CN117739972A (zh) | 2024-03-22 |
| CN117739972B CN117739972B (zh) | 2024-05-24 |
Family
ID=90261176
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410180418.9A Active CN117739972B (zh) | 2024-02-18 | 2024-02-18 | 一种无全球卫星定位系统的无人机进近阶段定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117739972B (zh) |
Citations (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106813662A (zh) * | 2016-06-08 | 2017-06-09 | 极翼机器人(上海)有限公司 | 一种基于光流的导航方法 |
| CN107389968A (zh) * | 2017-07-04 | 2017-11-24 | 武汉视览科技有限公司 | 一种基于光流传感器和加速度传感器的无人机定点实现方法和装置 |
| KR20190001086A (ko) * | 2017-06-26 | 2019-01-04 | 서울대학교산학협력단 | 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치 |
| CN110108894A (zh) * | 2019-04-26 | 2019-08-09 | 北京航空航天大学 | 一种基于相位相关和光流法的多旋翼速度测量方法 |
| CN111006655A (zh) * | 2019-10-21 | 2020-04-14 | 南京理工大学 | 机场巡检机器人多场景自主导航定位方法 |
| CN112712558A (zh) * | 2020-12-25 | 2021-04-27 | 北京三快在线科技有限公司 | 一种无人驾驶设备的定位方法及装置 |
| CN113503872A (zh) * | 2021-06-03 | 2021-10-15 | 浙江工业大学 | 一种基于相机与消费级imu融合的低速无人车定位方法 |
| CN115077517A (zh) * | 2022-05-27 | 2022-09-20 | 浙江工业大学 | 一种基于立体相机与imu融合的低速无人车定位方法及系统 |
| US20220392083A1 (en) * | 2021-06-02 | 2022-12-08 | Toyota Research Institute, Inc. | Systems and methods for jointly training a machine-learning-based monocular optical flow, depth, and scene flow estimator |
| CN115619826A (zh) * | 2022-10-17 | 2023-01-17 | 西南科技大学 | 一种基于重投影误差和深度估计的动态slam方法 |
| US20230145875A1 (en) * | 2021-11-02 | 2023-05-11 | Samsung Electronics Co., Ltd. | Apparatus for accelerating simultaneous localization and mapping and electronic device including the same |
| CN116358547A (zh) * | 2022-12-09 | 2023-06-30 | 珠海创智科技有限公司 | 一种基于光流估计获取agv位置的方法 |
| CN116929348A (zh) * | 2023-07-26 | 2023-10-24 | 桂林电子科技大学 | 基于单基站uwb和视觉惯性的工厂agv定位方法 |
| CN116952229A (zh) * | 2023-08-29 | 2023-10-27 | 北京卓翼智能科技有限公司 | 无人机定位方法、装置、系统和存储介质 |
| CN117053782A (zh) * | 2023-08-14 | 2023-11-14 | 南京理工大学 | 一种水陆两栖机器人组合导航方法 |
| CN117451032A (zh) * | 2023-11-01 | 2024-01-26 | 中兵智能创新研究院有限公司 | 一种低算力与松耦合的激光雷达和imu的slam方法及系统 |
-
2024
- 2024-02-18 CN CN202410180418.9A patent/CN117739972B/zh active Active
Patent Citations (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106813662A (zh) * | 2016-06-08 | 2017-06-09 | 极翼机器人(上海)有限公司 | 一种基于光流的导航方法 |
| KR20190001086A (ko) * | 2017-06-26 | 2019-01-04 | 서울대학교산학협력단 | 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치 |
| CN107389968A (zh) * | 2017-07-04 | 2017-11-24 | 武汉视览科技有限公司 | 一种基于光流传感器和加速度传感器的无人机定点实现方法和装置 |
| CN110108894A (zh) * | 2019-04-26 | 2019-08-09 | 北京航空航天大学 | 一种基于相位相关和光流法的多旋翼速度测量方法 |
| CN111006655A (zh) * | 2019-10-21 | 2020-04-14 | 南京理工大学 | 机场巡检机器人多场景自主导航定位方法 |
| CN112712558A (zh) * | 2020-12-25 | 2021-04-27 | 北京三快在线科技有限公司 | 一种无人驾驶设备的定位方法及装置 |
| US20220392083A1 (en) * | 2021-06-02 | 2022-12-08 | Toyota Research Institute, Inc. | Systems and methods for jointly training a machine-learning-based monocular optical flow, depth, and scene flow estimator |
| CN113503872A (zh) * | 2021-06-03 | 2021-10-15 | 浙江工业大学 | 一种基于相机与消费级imu融合的低速无人车定位方法 |
| US20230145875A1 (en) * | 2021-11-02 | 2023-05-11 | Samsung Electronics Co., Ltd. | Apparatus for accelerating simultaneous localization and mapping and electronic device including the same |
| CN115077517A (zh) * | 2022-05-27 | 2022-09-20 | 浙江工业大学 | 一种基于立体相机与imu融合的低速无人车定位方法及系统 |
| CN115619826A (zh) * | 2022-10-17 | 2023-01-17 | 西南科技大学 | 一种基于重投影误差和深度估计的动态slam方法 |
| CN116358547A (zh) * | 2022-12-09 | 2023-06-30 | 珠海创智科技有限公司 | 一种基于光流估计获取agv位置的方法 |
| CN116929348A (zh) * | 2023-07-26 | 2023-10-24 | 桂林电子科技大学 | 基于单基站uwb和视觉惯性的工厂agv定位方法 |
| CN117053782A (zh) * | 2023-08-14 | 2023-11-14 | 南京理工大学 | 一种水陆两栖机器人组合导航方法 |
| CN116952229A (zh) * | 2023-08-29 | 2023-10-27 | 北京卓翼智能科技有限公司 | 无人机定位方法、装置、系统和存储介质 |
| CN117451032A (zh) * | 2023-11-01 | 2024-01-26 | 中兵智能创新研究院有限公司 | 一种低算力与松耦合的激光雷达和imu的slam方法及系统 |
Non-Patent Citations (4)
| Title |
|---|
| XIAOMING LIU等: "Multiple optical flow sensors aiding inertial systems for UAV navigation", 2016 UKACC 11TH INTERNATIONAL CONFERENCE ON CONTROL (CONTROL), 1 January 2016 (2016-01-01), pages 1 - 7 * |
| 周晨: "基于多传感器融合的四旋翼无人机定位系统研究与实现", 《中国优秀硕士学位论文全文数据库 工程科技II辑》, no. 01, 15 January 2022 (2022-01-15), pages 031 - 283 * |
| 王洲等: ""基于多传感器融合的多旋翼无人机近地面定位算法"", 《成都信息工程大学学报》, vol. 33, no. 3, 30 June 2018 (2018-06-30), pages 261 - 267 * |
| 钱建国: "基于视觉/惯性融合的自适应导航关键技术研究", 《中国博士学位论文全文数据库信息科技辑》, no. 07, 15 July 2023 (2023-07-15), pages 136 - 91 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117739972B (zh) | 2024-05-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109993113B (zh) | 一种基于rgb-d和imu信息融合的位姿估计方法 | |
| CN111024066B (zh) | 一种无人机视觉-惯性融合室内定位方法 | |
| CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
| CN104729506B (zh) | 一种视觉信息辅助的无人机自主导航定位方法 | |
| CN109540126A (zh) | 一种基于光流法的惯性视觉组合导航方法 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN110865650B (zh) | 基于主动视觉的无人机位姿自适应估计方法 | |
| KR102226846B1 (ko) | Ｉｍｕ 센서와 카메라를 이용한 하이브리드 실내 측위 시스템 | |
| CN112197770A (zh) | 一种机器人的定位方法及其定位装置 | |
| US10757333B2 (en) | Method for determining bias in an inertial measurement unit of an image acquisition device | |
| CN110231028B (zh) | 飞行器导航方法、装置和系统 | |
| CN114623817A (zh) | 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法 | |
| CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
| Chen et al. | Stereo visual inertial pose estimation based on feedforward-feedback loops | |
| CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
| CN114723811A (zh) | 一种非结构化环境四足机器人立体视觉定位建图方法 | |
| CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
| CN117739972B (zh) | 一种无全球卫星定位系统的无人机进近阶段定位方法 | |
| CN115235455A (zh) | 一种基于智能手机pdr与视觉修正的行人定位方法 | |
| CN113632029B (zh) | 信息处理装置、程序及信息处理方法 | |
| CN114234967A (zh) | 一种基于多传感器融合的六足机器人定位方法 | |
| Hosen et al. | A vision-aided nonlinear observer for fixed-wing UAV navigation | |
| Bi et al. | A fast stereo visual-inertial odometry for mavs | |
| CN114660641B (zh) | 一种自适应gps融合定位系统、方法及介质 | |
| Chathuranga et al. | Aerial image matching based relative localization of a uav in urban 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 |