CN117870659A - 基于点线特征的视觉惯性组合导航算法 - Google Patents
基于点线特征的视觉惯性组合导航算法 Download PDFInfo
- Publication number
- CN117870659A CN117870659A CN202410025341.8A CN202410025341A CN117870659A CN 117870659 A CN117870659 A CN 117870659A CN 202410025341 A CN202410025341 A CN 202410025341A CN 117870659 A CN117870659 A CN 117870659A
- Authority
- CN
- China
- Prior art keywords
- image
- point
- pixel
- points
- imu
- 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.)
- Pending
Links
- 230000000007 visual effect Effects 0.000 title claims abstract description 29
- 238000000034 method Methods 0.000 claims abstract description 71
- 239000011159 matrix material Substances 0.000 claims abstract description 36
- 230000033001 locomotion Effects 0.000 claims abstract description 22
- 238000004364 calculation method Methods 0.000 claims abstract description 16
- 230000004044 response Effects 0.000 claims description 23
- 238000005457 optimization Methods 0.000 claims description 16
- 230000008859 change Effects 0.000 claims description 11
- 230000004927 fusion Effects 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 9
- 238000012545 processing Methods 0.000 claims description 9
- 239000013598 vector Substances 0.000 claims description 9
- 238000012360 testing method Methods 0.000 claims description 8
- 230000001133 acceleration Effects 0.000 claims description 6
- 238000001514 detection method Methods 0.000 claims description 6
- 230000014509 gene expression Effects 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 6
- 238000000605 extraction Methods 0.000 claims description 5
- 230000000694 effects Effects 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 238000001914 filtration Methods 0.000 claims description 3
- 238000007781 pre-processing Methods 0.000 claims description 3
- 238000009499 grossing Methods 0.000 claims description 2
- 230000002452 interceptive effect Effects 0.000 abstract description 3
- 238000003708 edge detection Methods 0.000 abstract 1
- 230000009466 transformation Effects 0.000 abstract 1
- 238000005286 illumination Methods 0.000 description 6
- 230000003287 optical effect Effects 0.000 description 6
- 238000006243 chemical reaction Methods 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 239000000284 extract Substances 0.000 description 2
- 238000007500 overflow downdraw method Methods 0.000 description 2
- 230000003252 repetitive effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 230000001010 compromised effect Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- 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
- 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/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/10—Image enhancement or restoration using non-spatial domain filtering
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/13—Edge detection
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20048—Transform domain processing
- G06T2207/20061—Hough transform
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20112—Image segmentation details
- G06T2207/20164—Salient point detection; Corner detection
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Image Analysis (AREA)
Abstract
本发明提出了一种基于点线特征的视觉惯性组合导航算法，该算法分别提取、匹配图像的点、线特征，并基于多模态估计的算法融合视觉惯性数据。算法内容包括：基于ORB方法提取图像的特征点；基于Canny边缘检测和霍夫变换提取图像的特征线段；应用灰度共生矩阵计算场景的纹理度情况，根据结果分配所需点、线特征的数量，使得***具有充分的灵活性，既保证了精度又提高了计算效率；匹配提取到的点线特征并应用视差法恢复这些特征的三维信息；应用多模态估计融合视觉信息和惯性里程计的信息，这里应用交互式多模型算法使用多个滤波器并行运行，每个滤波器代表不同的运动模型或假设，然后根据其性能合成这些估计，使得***能够适应多种工作环境。
Description
技术领域
本发明属于导航领域，具体涉及一种基于点线特征的视觉惯性组合导航算法。
背景技术
视觉惯性组合导航算法涉及到三点，第一是视觉导航的方法，第二是惯性导航的方法，第三是数据融合的方法。关于第一点目前有许多的视觉导航方法，比如光流法通过分析相机连续帧之间的像素变化来估计运动；基于特征的方法识别和跟踪场景中的特征点或特征区域，如角点、边缘或特定图案来估计运动；直接法直接利用图像的像素强度信息，而不是提取特征点等。对于数据融合方法，目前比较成熟的方案有滤波器方法使用卡尔曼滤波器及其变种来融合视觉和惯性数据；非线性优化方法通过构建一个代价函数，然后使用非线性优化技术来求解最优状态。
对于目前已有的视觉惯性组合导航相关的技术，都有一些不足的地方。比如，光流法依赖于分析相机连续帧之间的像素变化来估计运动，它对环境中的视觉特征高度依赖。在特征贫乏或者重复的环境中(如单一颜色的墙壁或高度重复的纹理)，光流算法可能难以准确工作，在动态环境中，尤其是当场景中有多个移动对象时，光流法的效果会大打折扣，因为它难以区分相机运动和场景中物体的运动，光流算法对光照条件非常敏感，强烈的光照变化或阴影可能导致光流估计不准确；特征法是一种依赖于检测和跟踪图像中特定特征点或特征区域的方法，虽然这种方法在很多应用中表现出色，但是在特征缺乏(如纯色墙壁)或特征过多(如复杂图案)的环境中，正确提取和跟踪特征点可能很困难，特征检测通常受光照条件和视角变化的影响，在极端光照或从不同角度观察时，原有的特征可能难以识别，在动态环境中，移动物体可能引入错误的特征点，导致导航***误判；直接法是一种不依赖于明确特征提取的方法，直接使用图像的像素强度信息来估计运动，这种方法在某些方面优于特征法，但是直接法对光照变化非常敏感，光照的变化会影响像素强度，从而影响运动估计的准确性，由于需要处理大量的像素数据，直接法往往计算量较大，这可能对计算资源有限的***构成挑战，在动态环境中，移动物体会导致像素强度变化，直接法可能难以区分相机运动和场景中物体的运动，像特征法一样，直接法也无法从单一图像中获取真实世界的尺度信息，需要额外的信息或假设来解决尺度模糊性问题，直接法对图像质量和纹理分布有较高的依赖性，图像模糊或纹理不足的区域可能导致估计失败，直接法通常在局部图像窗口中工作，因此在全局优化方面可能不如基于特征的方法有效；关于数据融合的滤波方法，扩展卡尔曼滤波器等滤波器通过线性化非线性模型来处理数据，这种线性化可能导致估计误差，特别是在***动态变化较大时，例如复杂的机器人运动或极端的飞行动态，传统滤波方法可能难以准确估计状态，滤波器的性能在很大程度上依赖于其参数设置。不当的参数选择可能导致性能下降，而这些参数通常需要手动调整和校准；关于数据融合的非线性优化方法，虽然在提高导航***精度和鲁棒性方面具有一定优势，但是非线性优化方法通常需要大量的计算资源，特别是在处理高分辨率图像和高频率的惯性数据时，可能导致高功耗和延迟，非线性优化问题的收敛性和稳定性受到初始猜测、优化问题的形状和约束条件的影响，在某些情况下，优化可能无法收敛或者需要很长时间才能收敛到合理的解。
发明内容
为解决上述问题，本发明公开了基于点线特征的视觉惯性组合导航算法，分别提取、描述、匹配图像的特征点和特征线段，然后基于多模态估计的算法将视觉信息和惯性里程计的信息融合，提高了计算效率，能够适应复杂的运动或极端的飞行动态，实现精确的位置和方向追踪，同时具有更加灵活的特征选择、对环境具有更好的适应性。
为达到上述目的，本发明的技术方案如下：
基于点线特征的视觉惯性组合导航算法，包括以下步骤：
(1)IMU数据的预处理；
(2)提取双目图像；
(3)图像的特征点的处理；
(4)图像的特征线段的处理；
(5)点线特征的数量分配；
(6)运动估计；
(7)数据融合。
进一步的，步骤(1)具体如下：
(11)从IMU得到测量数据，使用的是六轴IMU得到的数据为x，y，z三个方向的角速度(ω/s)和加速度(m/s2)。
(12)将测量得到的六轴IMU数据进行预处理。
IMU以很高的频率提供数据，通过对IMU数据进行预积分，利用IMU的高频数据在相邻的两帧图像之间累积得到的位姿变化信息，将连续的IMU测量数据转换为离散相机帧之间的相对运动，从而减少参与优化的变量数量，降低计算复杂度。
(13)进行视觉和IMU时间戳的对齐。
在视觉-IMU融合***中，时间同步是确保IMU数据和相机帧在时间上对齐的关键过程。由于IMU和相机的数据获取速率通常是不同的，因此需要使用插值或其他方法来确保数据同步。
假设有以下表示：
tc：当前相机帧的时间戳；
ti-1和ti：分别是与tc最近的两个IMU数据的时间戳，其中ti-1<tc<ti；
si-1和si：分别是在ti-1和ti的IMU数据样本；
对于给定的相机帧时间戳tc，要获取此时的IMU数据；使用线性插值方法。对于任何给定的量s，(如加速度或角速度)，在时间tc的插值表示为。
这个公式考虑了两个相邻IMU数据样本之间的变化，并估计了在相机帧时间戳tc时的值。
进一步的，步骤(2)具体如下：
使用双目相机提取图像，通过ROS***将图像数据传给算法处理***，要将ROS格式的图像信息转换成cv::Mat格式，然后进行处理。
进一步的，步骤(3)具体如下：
(32)为了实现尺度不变性，首先创建一个图像金字塔。这涉及生成图像的多个尺度版本，通常每个新的尺度都是原始图像的某个固定因子的缩小版本。这样，即使物体在图像中的大小变化，它的特征也可以在不同的尺度上被检测到。
图像金字塔由一系列逐渐缩小的图像层组成。底层是原始尺寸的图像，而每个高级别的层都是前一层的缩小版本。设I0为原始图像，对于每个金字塔的级别i，图像Ii通过下采样图像Ii-1得到。为了得到下一层的图像，当前图像会首先与一个高斯滤波器卷积，然后进行子采样。这有助于减少混叠效应。
其中，G为高斯滤波器，*代表卷积操作。接着进行子采样，通常是取原图的每隔一个像素，从而减少每个维度的大小到一半。
金字塔的层数或深度取决于应用的需求。在ORB中，通常会选择一个固定的深度，如8层，来确保在各种尺度下都能检测到关键点。
(32)对于每一个像素P，考虑其周围的16个像素的圆形区域。如果在这16个像素中有连续的n个像素其亮度明显高于P或明显低于P，则认为P是一个角点。
(33)选择一个像素P(x,y)和一个亮度阈值t。计算器亮度值IP。考虑P周围的16个像素的圆，首先检查像素1，5，9，13，如果所有这4个像素的亮度值都大于IP+t或都小于IP-t，则进一步检查其他像素。
(34)采用Harris进行角点检测
Harris角点响应基于一个观念：如果窗口在所有方向上移动时图像亮度发生显著变化，则该窗口中心的像素可能是角点。对于图像I，考虑窗口w(x,y)在(x,y)位置的小偏移(dx,dy)造成的亮度变化，如下式所示。
Ix和Iy是图像在x和y方向上的倒数。
矩阵M的两个特征值λ1和λ2(其中λ1≥λ2)描述了图像在两个主要方向上的变化。
Harris响应定义为：
R＝det(M)-k(trace(M))2
其中，det(M)＝λ1λ2，trace(M)＝λ1+λ2。常数k通常取值在0.04到0.06之间。当R值小(无论正负)，它意味着λ1和λ2都很小，该区域是一个平坦区域。当R是正值并且很大，它意味着λ1和λ2都很大且相近，该区域是一个角点。当R是负值，它意味着其中一个λ值很小，另一个λ值很大，该区域是一个边缘。
(35)所有检测到的角点根据其Harris响应值R进行降序排序。这意味着具有最大Harris响应的角点位于列表的前端。ORB特征提取器不仅仅选择响应值最高的n个关键点。为了确保提取的关键点在图像中均匀分布，ORB会将图像划分为多个网格，对于每个网格，从该网格中选择具有最高Harris响应的关键点。如果某个网格内关键点数量过多，则只保留响应最高的k个点。其中k取决于希望从每个网格中提取的关键点数量。如果某个网格内关键点数量过少或没有关键点，则可以考虑从邻近的网格中“借用”关键点。
(36)对于每个关键点，ORB计算其周围区域的矩。具体来说，考虑关键点p(x,y)，其周围的像素区域可以定义为一个小窗口或圆形区域。对于这个区域内的每个像素qi(i,j)，计算其亮度加权的中心矩：
其中，I(qi)是像素qi的亮度值。
使用上面计算的中心矩，关键点的方向θp可以通过以下公式计算：
一旦为每个关键点确定了方向，就可以相应地旋转其二进制描述子，使其具有旋转不变性。当在另一个图像中查找匹配的特征点时，由于描述子已经旋转到了参考方向，因此可以正确匹配到旋转后的关键点。
(37)BRIEF描述子通过对关键点周围的像素进行简单的亮度比较来生成二进制字符串。具体地说，选择图像中的一对像素点p(xp,yp)和q(xq,yq)，如果I(p)<I(q)，那么描述子的相应位为1，否则为0。
(38)在固定的关键点周围，随机选择N对像素点(例如,N＝256)以进行比较。这些像素对的选择是预先定义的，可以在多个图像上复用。ORB为每个关键点分配了一个方向。为了使BRIEF描述子具有旋转不变性，需要根据该方向旋转这些预定义的像素对。这样，即使图像旋转，仍然可以对旋转后的相同关键点生成相同的描述子。设关键点的方向为θ，则像素点p(xp,yp)旋转到新位置p`(xp `,yp `)的转换可以用以下公式表示：
`
xp＝xp cos(θ)-ypsin(θ)
`
yp＝xp sin(θ)+ypcos(θ)
(39)ORB描述子是二进制的，这意味着特征点之间的相似性可以通过计算它们描述子的汉明距离来评估。汉明距离是衡量两个字符串之间差异的一种方法，具体来说，它表示了两个等长字符串之间对应位置不同字符的数量。对于ORB的二进制描述子，汉明距离表示两个描述子之间对应位不同的数量。
设有两个ORB描述子D1和D2，它们都是长度为N的二进制向量，其中N通常是256。汉明距离H(D1,D2)是D1和D2之之间不同位的数量。数学上，可以使用异或操作(XOR)来计算这两个向量之间的差异，然后统计结果中的1的数量：
其中，D1和D2是描述子的第i位，⊕是XOR操作。
对于给定图像中的一个特征点，要找到另一个图像中与其最相似的特征点，通过查找具有最小汉明距离的描述子来实现。具体来说，给定一个描述子D1，在另一个图像的所有描述子集合中查找一个描述子D2，使得H(D1,D2)最小。为了过滤掉错误匹配，设置一个汉明距离的阈值。只有当两个描述子之间的汉明距离小于这个阈值时，才认为它们是一个匹配。为了提高匹配的鲁棒性，这里应用一种比率测试。这种测试基于这样的观察：对于一个好的匹配，最近的描述子和次近的描述子之间的汉明距离应该有一个显著的差距。具体地说，对于描述子D1，除了找到它的最近邻D2 +，还需要找到次近邻D2 -。然后计算以下比值：
如果这个比值小于某个阈值，则认为匹配是鲁棒的。
基于实际情况，如果特征点提取的数量太多会增加误匹配的情况，因为会提取到很多地面上面的特征点，因此设定特征点提取的数量为1000。此外，远近阈值设定为30以防止图像远近变化时的影响。
进一步的，步骤(4)具体如下：
(41)使用高斯滤波器平滑图像，减少图像中的噪声。高斯滤波器的数学表达式为：
其中，x和y是像素位置，σ是高斯滤波器的标准差。
计算每个像素点的梯度幅度和方向，在水平和垂直方向上应用Sobel算子来完成。梯度幅度G和梯度方向Θ的计算公式如下：
其中，Gx和Gy分别是图像在x和y方向上的梯度。
遍历图像中的每个像素，并消除非边缘像素，只保留梯度幅度在梯度方向上为局部最大的像素点。
使用两个阈值：低阈值和高阈值来检测和连接边缘。首先，所有梯度幅度高于高阈值的像素被认为是强边缘。那些低于低阈值的像素点被排除。处于两个阈值之间的像素点则根据其与强边缘的连接情况来确定是否为边缘。
(42)使用霍夫变化将图像空间中的直线转换为参数空间中的点。在图像空间中，一条直线可以表示为y＝mx+b其中m是斜率，b是y轴截距，这种表示方式在处理垂直线时会遇到问题。因此，霍夫变换采用极坐标***来表示直线，使用两个参数：r和θ。直线的极坐标方程为r＝xcosθ+ysinθ，其中r是直线到原点的垂直距离，θ是直线与x轴的夹角。对于图像中的每个点和一系列的θ值，计算对应的r值，并在累加器空间(通常是二维数组，对应不同的r和θ值)中增加计数。每一个像素点都会在参数空间中形成一条正弦曲线。当图像中存在一条直线时，这条直线上的所有点在参数空间中对应的曲线将在某点交汇，这个交点对应的r和θ值就代表了这条直线的参数。在本发明的实际运用中，存储线段的两个端点坐标表示线段。
(43)线段特征的匹配基于线段中心点的位置和线段的角度，这里使用到吕普克坐标[l,m,n,a,b,c]，[l,m,n]表示线段的方向，[a,b,c]表示线段上的点。本发明根据中心点距离和角度差进行线段的匹配。
进一步的，步骤(5)具体如下：
使用灰度共生矩阵进行图像的纹理分析，首先选择一个特定的方向和距离。然后，在这个方向和距离下，统计图像中每种灰度级像素对(i,j)的出现频率。结果是一个矩阵，其中行和列的数量等于图像中的灰度级数，矩阵中的每个元素表示特定的像素对在整个图像中出现的频率或概率。如果图像有N个灰度级，则灰度共生矩阵是一个N×N的矩阵M，其中矩阵的每个元素M(i,j)表示灰度值为i的像素与灰度值为j的像素按照定义的空间关系(方向和距离)共同出现的次数或概率。一旦计算出灰度共生矩阵，就从中提取多种纹理特征，其中对比度反映了图像中局部灰度变化的程度。
均匀性度量了图像纹理的均匀程度。
相关性度量了图像中灰度级之间的相关性。
其中，μi和μj是灰度级i和j的平均值，σi和σj是它们的标准差。
熵度量了图像纹理的不规则性或复杂性。
使用灰度共生矩阵的均匀性和熵来确定分配值，当均匀性下降而熵值上升时则表示图像的纹理丰富，此时尽可能选择较多的点特征，提高***计算效率，否则选择较多的线段特征以确保准确度。
进一步的，步骤(6)具体如下：
双目相机通过两个相机的视图估计特征的三维位置，在已知相机内参和n个三维空间点对应的二维图像点时，求解相机相对于这些三维点的姿态和位置。PnP问题的目标是从一组对应的3D-2D点对中恢复出相机的位姿。一般情况下，至少需要3个点对来求解这个问题，但是为了增加鲁棒性，通常会使用更多的点对。使用EPnP方法进行求解，它通过将空间点表示为虚拟点的线性组合来降低问题的复杂性。然后，使用图优化的方法，将估计的相机的位姿和特征的三维位置作为两个节点，将二维图像点和三维空间点的投影关系作为连接它们的边，以此同时优化相机位姿和三维空间点。
进一步的，步骤(7)具体如下：
首先，确定***可能遵循的所有模型。
对于模型集合中的每个模型j，计算从模型i切换到模型j的概率pij，这是一个模型转移概率矩阵。对于每个模型j，更新其模型概率μj如下：
其中是基于模型j的似然函数，它度量了观测值与该模型预测值之间的匹配程度。对于每个模型，使用该模型的状态转移矩阵对状态进行预测。然后，使用卡尔曼滤波器的更新步骤根据观测值来更新预测状态。
将所有模型的状态估计混合在一起，得到总的状态估计：
其中，是基于模型j的状态估计。
同样地，将所有模型的协方差混合，得到总的协方差估计。
其中，Pj是基于模型j的协方差估计。
本发明的有益效果为：
本发明公开了基于点线特征的视觉惯性组合导航算法，主要是针对复杂的运动或极端的飞行动态，先分别提取、描述、匹配图像的特征点和特征线段，然后基于多模态估计的算法将视觉信息和惯性里程计的信息融合，能够提高计算效率，实现精确的位置和方向追踪，同时具有更加灵活的特征选择、对环境具有更好的适应性。
附图说明
图1是整个算法框架的流程图；
图2是视觉和IMU数据融合的流程图；
图3是跑室外数据集的结果图；
图4是跑室内数据集的结果图；
图5是实测的结果图。
具体实施方式
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
如图所示，本发明所述的基于点线特征的视觉惯性组合导航算法，包括以下步骤：
IMU的预处理：IMU可以以很高的频率(例如200Hz或更高)提供数据，而相机的采样频率通常较低(例如30Hz)。如果在状态估计中直接使用原始的高频IMU数据，会导致计算量急剧增加。通过对IMU数据进行预积分，利用IMU的高频数据在相邻的两帧图像之间累积(积分)得到的位姿变化信息，将连续的IMU测量数据转换为离散相机帧之间的相对运动(包括位置、速度和姿态的变化)，从而减少参与优化的变量数量，降低计算复杂度。
在视觉-IMU融合***中，时间同步是确保IMU数据和相机帧在时间上对齐的关键过程。由于IMU和相机的数据获取速率通常是不同的(IMU的数据频率通常远高于相机)，因此需要使用插值或其他方法来确保数据同步。
假设有以下表示：
tc：当前相机帧的时间戳
ti-1和ti：分别是与tc最近的两个IMU数据的时间戳，其中ti-1<tc<ti
si-1和si：分别是在ti-1和ti的IMU数据样本
对于给定的相机帧时间戳tc，要获取此时的IMU数据。一个常见的方法是使用线性插值。对于任何给定的量s，(如加速度或角速度)，在时间tc的插值可以表示为。
这个公式简单地考虑了两个相邻IMU数据样本之间的变化，并估计了在相机帧时间戳tc时的值。
图像的特征点的处理：为了实现尺度不变性，首先创建一个图像金字塔。这涉及生成图像的多个尺度版本，通常每个新的尺度都是原始图像的某个固定因子的缩小版本。这样，即使物体在图像中的大小变化，它的特征也可以在不同的尺度上被检测到。
图像金字塔由一系列逐渐缩小的图像层组成。底层是原始尺寸的图像，而每个高级别的层都是前一层的缩小版本。设I0为原始图像，对于每个金字塔的级别i(从1开始)，图像Ii可以通过下采样图像Ii-1得到。为了得到下一层的图像，当前图像会首先与一个高斯滤波器卷积，然后进行子采样。这有助于减少混叠效应。
其中，G为高斯滤波器，*代表卷积操作。接着进行子采样，通常是取原图的每隔一个像素，从而减少每个维度的大小到一半。
金字塔的层数或深度取决于应用的需求。在ORB中，通常会选择一个固定的深度，如8层，来确保在各种尺度下都能检测到关键点。
对于每一个像素P，考虑其周围的16个像素的圆形区域。如果在这16个像素中有连续的n个像素(通常n＝12)其亮度明显高于P(超过给定的阈值)或明显低于P，则认为P是一个角点。
选择一个像素P(x,y)和一个亮度阈值t。计算器亮度值IP(通常使用加权平均计算亮度值L＝0.299R+0.587G+0.114B)。考虑P周围的16个像素的圆，首先检查像素1，5，9，13，如果所有这4个像素的亮度值都大于IP+t或都小于IP-t，则进一步检查其他像素。由于FAST首先只检查4个像素，并且只在这4个像素满足条件时才考虑其他像素，因此它计算速度很快。大多数非角点的像素都可以在这个初步检查阶段被快速排除。
Harris角点检测是一种常见的角点检测方法，其基于图像的局部变化来确定角点。在ORB中，虽然用FAST来检测角点，但用Harris角点响应来确定这些角点的强度，并根据这些响应值进行排序和筛选。
Harris角点响应基于一个观念：如果窗口在所有方向上移动时图像亮度发生显著变化，则该窗口中心的像素可能是角点。对于图像I，考虑窗口w(x,y)在(x,y)位置的小偏移(dx,dy)造成的亮度变化，如下式所示。
Ix和Iy是图像在x和y方向上的倒数。
矩阵M的两个特征值λ1和λ2(其中λ1≥λ2)描述了图像在两个主要方向上的变化。Harris响应定义为：
R＝det(M)-k(trace(M))2
其中，det(M)＝λ1λ2，trace(M)＝λ1+λ2。常数k通常取值在0.04到0.06之间。
当R值小(无论正负)，它意味着λ1和λ2都很小，该区域是一个平坦区域。当R是正值并且很大，它意味着λ1和λ2都很大且相近，该区域是一个角点。当R是负值，它意味着其中一个λ值很小，另一个λ值很大，该区域是一个边缘。
所有检测到的角点根据其Harris响应值R进行降序排序。这意味着具有最大Harris响应的角点位于列表的前端。ORB特征提取器不仅仅选择响应值最高的n个关键点。为了确保提取的关键点在图像中均匀分布，ORB会将图像划分为多个网格，比如5×5的网格。对于每个网格，从该网格中选择具有最高Harris响应的关键点。如果某个网格内关键点数量过多，则只保留响应最高的k个点。其中k取决于希望从每个网格中提取的关键点数量。如果某个网格内关键点数量过少或没有关键点，则可以考虑从邻近的网格中“借用”关键点。
对于每个关键点，ORB计算其周围区域的矩。具体来说，考虑关键点p(x,y)，其周围的像素区域可以定义为一个小窗口或圆形区域。对于这个区域内的每个像素qi(i,j)，计算其亮度加权的中心矩：
其中，I(qi)是像素qi的亮度值。
使用上面计算的中心矩，关键点的方向θp可以通过以下公式计算：
一旦为每个关键点确定了方向，就可以相应地旋转其二进制描述子，使其具有旋转不变性。当在另一个图像中查找匹配的特征点时，由于描述子已经旋转到了参考方向，因此可以正确匹配到旋转后的关键点。
BRIEF描述子通过对关键点周围的像素进行简单的亮度比较来生成二进制字符串。具体地说，选择图像中的一对像素点p(xp,yp)和q(xq,yq)，如果I(p)<I(q)，那么描述子的相应位为1，否则为0。
在固定的关键点周围，随机选择N对像素点(例如,N＝256)以进行比较。这些像素对的选择是预先定义的，可以在多个图像上复用。ORB为每个关键点分配了一个方向。为了使BRIEF描述子具有旋转不变性，需要根据该方向旋转这些预定义的像素对。这样，即使图像旋转，仍然可以对旋转后的相同关键点生成相同的描述子。设关键点的方向为θ，则像素点p(xp,yp)旋转到新位置p`(xp `,yp `)的转换可以用以下公式表示：
`
xp＝xp cos(θ)-ypsin(θ)
`
yp＝xp sin(θ)+ypcos(θ)
ORB描述子是二进制的，这意味着特征点之间的相似性可以通过计算它们描述子的汉明距离来评估。汉明距离是衡量两个字符串之间差异的一种方法，具体来说，它表示了两个等长字符串之间对应位置不同字符的数量。对于ORB的二进制描述子，汉明距离表示两个描述子之间对应位不同的数量。
设有两个ORB描述子D1和D2，它们都是长度为N的二进制向量，其中N通常是256。汉明距离H(D1,D2)是D1和D2之间不同位的数量。数学上，可以使用异或操作(XOR)来计算这两个向量之间的差异，然后统计结果中的1的数量：
其中，D1和D2是描述子的第i位，⊕是XOR操作。
对于给定图像中的一个特征点，要找到另一个图像中与其最相似的特征点。这可以通过查找具有最小汉明距离的描述子来实现。具体来说，给定一个描述子D1，在另一个图像的所有描述子集合中查找一个描述子D2，使得H(D1,D2)最小。为了过滤掉错误匹配，设置一个汉明距离的阈值。只有当两个描述子之间的汉明距离小于这个阈值时，才认为它们是一个匹配。
为了提高匹配的鲁棒性，这里应用一种比率测试。这种测试基于这样的观察：对于一个好的匹配，最近的描述子和次近的描述子之间的汉明距离应该有一个显著的差距。具体地说，对于描述子D1，除了找到它的最近邻D2 +，还需要找到次近邻D2 -。然后计算以下比值：
如果这个比值小于某个阈值(如0.8)，则认为匹配是鲁棒的。这种方法有助于过滤掉那些由于噪声或其他干扰而产生的错误匹配。
图像特征线段的处理：
使用高斯滤波器平滑图像，减少图像中的噪声。高斯滤波器的数学表达式为：
/>
其中，x和y是像素位置，σ是高斯滤波器的标准差。
计算每个像素点的梯度幅度和方向，在水平和垂直方向上应用Sobel算子来完成。梯度幅度G和梯度方向Θ的计算公式如下：
其中，Gx和Gy分别是图像在x和y方向上的梯度。
遍历图像中的每个像素，并消除非边缘像素，只保留梯度幅度在梯度方向上为局部最大的像素点。
使用两个阈值：低阈值和高阈值来检测和连接边缘。首先，所有梯度幅度高于高阈值的像素被认为是强边缘。那些低于低阈值的像素点被排除。处于两个阈值之间的像素点则根据其与强边缘的连接情况来确定是否为边缘。
使用霍夫变化将图像空间中的直线转换为参数空间中的点。在图像空间中，一条直线可以表示为y＝mx+b其中m是斜率，b是y轴截距，这种表示方式在处理垂直线时会遇到问题(斜率无穷大)。因此，霍夫变换采用极坐标***来表示直线，使用两个参数：r和θ。直线的极坐标方程为r＝x cosθ+ysinθ，其中r是直线到原点的垂直距离，θ是直线与x轴的夹角。
对于图像中的每个点和一系列的θ值，计算对应的r值，并在累加器空间(通常是二维数组，对应不同的r和θ值)中增加计数。每一个像素点(假设是边缘点)都会在参数空间中形成一条正弦曲线。当图像中存在一条直线时，这条直线上的所有点在参数空间中对应的曲线将在某点交汇，这个交点对应的r和θ值就代表了这条直线的参数。
假设有两组线段，每组线段分别来自不同的图像或不同的视角，要找到这两组线段之间的匹配，首先为每条线段定义一个特征向量，这个特征向量可以包含线段的长度、方向、位置。设定RANSAC将要执行的迭代次数以及距离阈值。对于每次迭代，从第一组线段中随机选择一个线段作为样本，在第二组线段中找到与选定样本特征最接近的线段，通过计算特征向量之间的欧氏距离来实现。对于第二组中的每条线段，如果其特征向量与第一组中选定样本的特征向量之间的距离小于设定的阈值，那么这条线段就被认为是一个内点。如果当前迭代产生的内点数量超过之前的任何迭代，记录当前的样本和所有内点。完成所有迭代后，选择产生最多内点的样本及其匹配集合作为最终的匹配结果。
点线特征的数量分配：
使用灰度共生矩阵进行图像的纹理分析，首先选择一个特定的方向和距离。例如，方向可以是0度(水平)、45度、90度(垂直)或135度，距离可以是相邻像素(通常为1)。然后，在这个方向和距离下，统计图像中每种灰度级像素对(i,j)的出现频率。结果是一个矩阵，其中行和列的数量等于图像中的灰度级数，矩阵中的每个元素表示特定的像素对在整个图像中出现的频率或概率。如果图像有N个灰度级，则灰度共生矩阵是一个N×N的矩阵M，其中矩阵的每个元素M(i,j)表示灰度值为i的像素与灰度值为j的像素按照定义的空间关系(方向和距离)共同出现的次数或概率。一旦计算出灰度共生矩阵，就可以从中提取多种纹理特征，比如对比度反映了图像中局部灰度变化的程度。
均匀性度量了图像纹理的均匀程度。
相关性度量了图像中灰度级之间的相关性。
其中，μi和μj是灰度级i和j的平均值，σi和σj是它们的标准差。
熵度量了图像纹理的不规则性或复杂性。
使用灰度共生矩阵的均匀性和熵来确定分配值，当均匀性下降而熵值上升时则表示图像的纹理丰富，此时尽可能选择较多的点特征，提高***计算效率，否则选择较多的线段特征以确保准确度。
运动估计：双目相机可以通过两个相机的视图估计特征的三维位置，在已知相机内参和n个三维空间点对应的二维图像点时，求解相机相对于这些三维点的姿态(旋转)和位置(平移)。PnP问题的目标是从一组对应的3D-2D点对中恢复出相机的位姿。一般情况下，至少需要3个点对来求解这个问题(即3PnP)，但是为了增加鲁棒性，通常会使用更多的点对。使用EPnP方法进行求解，它通过将空间点表示为虚拟点的线性组合来降低问题的复杂性。然后，使用图优化的方法，将估计的相机的位姿和特征的三维位置作为两个节点，将二维图像点和三维空间点的投影关系作为连接它们的边，以此同时优化相机位姿和三维空间点。
数据融合：交互式多模型算法是一种估计非线性、多模型***状态的方法。主要应用于目标跟踪和其他相关领域，尤其当目标的运动模型可能在几种不同的模型之间切换时。IMM通过对所有考虑的模型进行加权组合来估计状态。
首先，确定***可能遵循的所有模型。例如，一个目标可能遵循“匀速直线运动”模型或“转弯”模型。对于每个模型，都有一个相应的状态转移矩阵和测量矩阵。
对于模型集合中的每个模型j，计算从模型i切换到模型j的概率pij，这是一个模型转移概率矩阵。
对于每个模型j，更新其模型概率μj如下：
其中是基于模型j的似然函数，它度量了观测值与该模型预测值之间的匹配程度。对于每个模型，使用该模型的状态转移矩阵对状态进行预测。然后，使用卡尔曼滤波器的更新步骤根据观测值来更新预测状态。
将所有模型的状态估计混合在一起，得到总的状态估计：
其中，是基于模型j的状态估计。
同样地，将所有模型的协方差混合，得到总的协方差估计。
其中，Pj是基于模型j的协方差估计。
要注意几点：
1、双目相机的内参已知，首先进行图像的畸变矫正、去除噪声、平滑处理。然后首先计算图像的纹理丰富度，根据这个数据分配将要提取的特征点和特征线段的数量，一次提取图像的特征点和特征线段、描述特征点和特征线段、匹配特征点和特征线段。根据左右相机匹配的特征结合三角测量方法得到这些特征的三维坐标，并将这些三维坐标保存在局部地图中，之后每一帧的图像得到的特征都和局部地图的三维特征相匹配，使用PnP问题的EPnP方法计算相机的位姿作为初始值，之后将这个初始值和局部地图的三维特征一起使用图优化的方法仅有优化处理，每接收到一帧图像都要对局部地图进行更新，要加入新提取的三维特征，剔除一直没有检测到的三维特征。在整个过程中，还要生成一个全局地图，用来回环检测消除累积误差。最后，使用交互式多模型算法融合双目视觉和IMU的数据，状态值是相机和IMU估计的位姿，观测值是局部地图保存的三维特征。
2、在整个过程中有好几个关键点，首先是特征的提取其实并非是越多越好，提取的特征太多，本发明实测时发现提取的特征太多，会有很大一部分特征提取到了地上，导致误匹配数量的增大，最后使得计算的位移比实际情况小了很多，其次在明暗变换和远近变换的时候是最容易丢失特征，此时如果设定的参数不合适，***容易失效，对于以上这两种情况本发明的处理方式是减小特征提取的数量(本发明设定在1000)，同时增加远近识别的阈值(本发明设定的是80)，此时就能比较好的估计运动。
3、还有一个比较明显的问题就是不加入IMU数据时大角度转弯时这个航向角一直估计偏差大，加入IMU一开始***就跑飞(其实ORB-SLAM3和VINS-Fusion加不加IMU数据都会跑飞)，为了解决这个问题，本发明参考过ORB-SLAM3和VINS-Fusion，发现它们直接就跑飞了，后来本发明解析了传感器测量得到的数据，发现IMU的有些测量的偏差较大，比如Z轴加速度计数据平常的偏差在100％，最高处的偏差达到了300％，这也确实和本发明测试用的载体本身有关(在整个测试过程中很明显载体在上下左右颠簸)，因此既然视觉导航本身就只差航向角和位移估计的校正，那么本发明只使用IMUx和y轴加速度计的数据，z轴陀螺仪的数据(本方法只适合地面车辆的使用)，数据融合时当检测到航向角明显单调变化非波动时加大状态值的信任度，否者加大观测值的信任度。
需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。
Claims (8)
1.基于点线特征的视觉惯性组合导航算法，其特征在于：包括以下步骤：
(1)IMU的预处理；
(2)提取图像；
(3)图像的特征点的处理；
(4)图像的特征线段的处理；
(5)点线特征的数量分配；
(6)运动估计；
(7)数据融合。
2.根据权利要求1所述的基于点线特征的视觉惯性组合导航算法，其特征在于：步骤(1)具体如下：
(11)从IMU得到测量数据，使用的是六轴IMU得到的数据为x，y，z三个方向的角速度(ω/s)和加速度(m/s2)；
(12)将测量得到的六轴IMU数据进行预处理；
IMU以很高的频率提供数据，通过对IMU数据进行预积分，利用IMU的高频数据在相邻的两帧图像之间累积得到的位姿变化信息，将连续的IMU测量数据转换为离散相机帧之间的相对运动，从而减少参与优化的变量数量，降低计算复杂度；
(13)进行视觉和IMU时间戳的对齐；
在视觉-IMU融合***中，时间同步是确保IMU数据和相机帧在时间上对齐的关键过程；由于IMU和相机的数据获取速率通常是不同的，因此需要使用插值或其他方法来确保数据同步；
假设有以下表示：
tc：当前相机帧的时间戳；
ti-1和ti：分别是与tc最近的两个IMU数据的时间戳，其中ti-1<tc<ti；
si-1和si：分别是在ti-1和ti的IMU数据样本；
对于给定的相机帧时间戳tc，要获取此时的IMU数据；使用线性插值方法；对于任何给定的量s，(如加速度或角速度)，在时间tc的插值表示为：
这个公式考虑了两个相邻IMU数据样本之间的变化，并估计了在相机帧时间戳tc时的值。
3.根据权利要求1所述的基于点线特征的视觉惯性组合导航算法，其特征在于：步骤(2)具体如下：
使用双目相机提取图像，通过ROS***将图像数据传给算法处理***，要将ROS格式的图像信息转换成cv::Mat格式，然后进行处理。
4.根据权利要求1所述的基于点线特征的视觉惯性组合导航算法，其特征在于：步骤(3)具体如下：
(31)为了实现尺度不变性，首先创建一个图像金字塔；这涉及生成图像的多个尺度版本，通常每个新的尺度都是原始图像的某个固定因子的缩小版本；这样，即使物体在图像中的大小变化，它的特征也可以在不同的尺度上被检测到；
图像金字塔由一系列逐渐缩小的图像层组成；底层是原始尺寸的图像，而每个高级别的层都是前一层的缩小版本；设I0为原始图像，对于每个金字塔的级别i，图像Ii通过下采样图像Ii-1得到；为了得到下一层的图像，当前图像会首先与一个高斯滤波器卷积，然后进行子采样；这有助于减少混叠效应：
其中，G为高斯滤波器，*代表卷积操作；接着进行子采样，通常是取原图的每隔一个像素，从而减少每个维度的大小到一半：
金字塔的层数或深度取决于应用的需求；在ORB中，通常会选择一个固定的深度，如8层，来确保在各种尺度下都能检测到关键点；
(32)对于每一个像素P，考虑其周围的16个像素的圆形区域；如果在这16个像素中有连续的n个像素其亮度明显高于P或明显低于P，则认为P是一个角点；
(33)选择一个像素P(x,y)和一个亮度阈值t；计算器亮度值IP；考虑P周围的16个像素的圆，首先检查像素1，5，9，13，如果所有这4个像素的亮度值都大于IP+t或都小于IP-t，则进一步检查其他像素；
(34)采用Harris进行角点检测
Harris角点响应基于一个观念：如果窗口在所有方向上移动时图像亮度发生显著变化，则该窗口中心的像素可能是角点；对于图像I，考虑窗口w(x,y)在(x,y)位置的小偏移(dx,dy)造成的亮度变化，如下式所示：
E(dx,dy)＝∑xyw(x,y)[I(x+dx,y+dy)-I(x,y)]2
Ix和Iy是图像在x和y方向上的倒数；
矩阵M的两个特征值λ1和λ2(其中λ1≥λ2)描述了图像在两个主要方向上的变化；
Harris响应定义为：
R＝det(M)-k(trace(M))2
其中，det(M)＝λ1λ2，trace(M)＝λ1+λ2；常数k通常取值在0.04到0.06之间；当R值小(无论正负)，它意味着λ1和λ2都很小，该区域是一个平坦区域；当R是正值并且很大，它意味着λ1和λ2都很大且相近，该区域是一个角点；当R是负值，它意味着其中一个λ值很小，另一个λ值很大，该区域是一个边缘；
(35)所有检测到的角点根据其Harris响应值R进行降序排序；这意味着具有最大Harris响应的角点位于列表的前端；ORB特征提取器不仅仅选择响应值最高的n个关键点；为了确保提取的关键点在图像中均匀分布，ORB会将图像划分为多个网格，对于每个网格，从该网格中选择具有最高Harris响应的关键点；如果某个网格内关键点数量过多，则只保留响应最高的k个点；其中k取决于希望从每个网格中提取的关键点数量；如果某个网格内关键点数量过少或没有关键点，则可以考虑从邻近的网格中“借用”关键点；
(36)对于每个关键点，ORB计算其周围区域的矩；具体来说，考虑关键点p(x,y)，其周围的像素区域可以定义为一个小窗口或圆形区域；对于这个区域内的每个像素qi(i,j)，计算其亮度加权的中心矩：
其中，I(qi)是像素qi的亮度值；
使用上面计算的中心矩，关键点的方向θp通过以下公式计算：
一旦为每个关键点确定了方向，就相应地旋转其二进制描述子，使其具有旋转不变性；当在另一个图像中查找匹配的特征点时，由于描述子已经旋转到了参考方向，因此能够正确匹配到旋转后的关键点；
(37)BRIEF描述子通过对关键点周围的像素进行简单的亮度比较来生成二进制字符串；具体地说，选择图像中的一对像素点p(xp,yp)和q(xq,yq)，如果I(p)<I(q)，那么描述子的相应位为1，否则为0；
(38)在固定的关键点周围，随机选择N对像素点(例如,N＝256)以进行比较；这些像素对的选择是预先定义的，可以在多个图像上复用；ORB为每个关键点分配了一个方向；为了使BRIEF描述子具有旋转不变性，需要根据该方向旋转这些预定义的像素对；这样，即使图像旋转，仍然可以对旋转后的相同关键点生成相同的描述子；设关键点的方向为θ，则像素点p(xp,yp)旋转到新位置p`(xp`,yp`)的转换可以用以下公式表示：
xp`＝xp cos(θ)-ypsin(θ)
yp`＝xp sin(θ)+ypcos(θ)
(39)ORB描述子是二进制的，这意味着特征点之间的相似性可以通过计算它们描述子的汉明距离来评估；汉明距离是衡量两个字符串之间差异的一种方法，具体来说，它表示了两个等长字符串之间对应位置不同字符的数量；对于ORB的二进制描述子，汉明距离表示两个描述子之间对应位不同的数量；
设有两个ORB描述子D1和D2，它们都是长度为N的二进制向量，其中N通常是256；汉明距离H(D1,D2)是D1和D2之间不同位的数量；数学上，可以使用异或操作(XOR)来计算这两个向量之间的差异，然后统计结果中的1的数量：
其中，D1和D2是描述子的第i位，是XOR操作；
对于给定图像中的一个特征点，要找到另一个图像中与其最相似的特征点，通过查找具有最小汉明距离的描述子来实现；具体来说，给定一个描述子D1，在另一个图像的所有描述子集合中查找一个描述子D2，使得H(D1,D2)最小；为了过滤掉错误匹配，设置一个汉明距离的阈值；只有当两个描述子之间的汉明距离小于这个阈值时，才认为它们是一个匹配；为了提高匹配的鲁棒性，这里应用一种比率测试；这种测试基于这样的观察：对于一个好的匹配，最近的描述子和次近的描述子之间的汉明距离应该有一个显著的差距；具体地说，对于描述子D1，除了找到它的最近邻D2 +，还需要找到次近邻D2 -；然后计算以下比值：
如果这个比值小于某个阈值，则认为匹配是鲁棒的；
基于实际情况，如果特征点提取的数量太多会增加误匹配的情况，因为会提取到很多地面上面的特征点，因此设定特征点提取的数量为1000；此外，远近阈值设定为30以防止图像远近变化时的影响。
5.根据权利要求1所述的基于点线特征的视觉惯性组合导航算法，其特征在于：步骤(4)具体如下：
(41)使用高斯滤波器平滑图像，减少图像中的噪声；高斯滤波器的数学表达式为：
其中，x和y是像素位置，σ是高斯滤波器的标准差；
计算每个像素点的梯度幅度和方向，在水平和垂直方向上应用Sobel算子来完成；梯度幅度G和梯度方向Θ的计算公式如下：
其中，Gx和Gy分别是图像在x和y方向上的梯度；
遍历图像中的每个像素，并消除非边缘像素，只保留梯度幅度在梯度方向上为局部最大的像素点；
使用两个阈值：低阈值和高阈值来检测和连接边缘；首先，所有梯度幅度高于高阈值的像素被认为是强边缘；那些低于低阈值的像素点被排除；处于两个阈值之间的像素点则根据其与强边缘的连接情况来确定是否为边缘；
(42)使用霍夫变化将图像空间中的直线转换为参数空间中的点；在图像空间中，一条直线可以表示为y＝mx+b其中m是斜率，b是y轴截距，这种表示方式在处理垂直线时会遇到问题；因此，霍夫变换采用极坐标***来表示直线，使用两个参数：r和θ；直线的极坐标方程为r＝xcosθ+ysinθ，其中r是直线到原点的垂直距离，θ是直线与x轴的夹角；对于图像中的每个点和一系列的θ值，计算对应的r值，并在累加器空间中增加计数；每一个像素点都会在参数空间中形成一条正弦曲线；当图像中存在一条直线时，这条直线上的所有点在参数空间中对应的曲线将在某点交汇，这个交点对应的r和θ值就代表了这条直线的参数；在本发明的实际运用中，存储线段的两个端点坐标表示线段；
(43)线段特征的匹配基于线段中心点的位置和线段的角度，这里使用到吕普克坐标[l,m,n,a,b,c]，[l,m,n]表示线段的方向，[a,b,c]表示线段上的点；本发明根据中心点距离和角度差进行线段的匹配。
6.根据权利要求1所述的基于点线特征的视觉惯性组合导航算法，其特征在于：步骤(5)具体如下：
使用灰度共生矩阵进行图像的纹理分析，首先选择一个特定的方向和距离；然后，在这个方向和距离下，统计图像中每种灰度级像素对(i,j)的出现频率；结果是一个矩阵，其中行和列的数量等于图像中的灰度级数，矩阵中的每个元素表示特定的像素对在整个图像中出现的频率或概率；如果图像有N个灰度级，则灰度共生矩阵是一个N×N的矩阵M，其中矩阵的每个元素M(i,j)表示灰度值为i的像素与灰度值为j的像素按照定义的空间关系共同出现的次数或概率；一旦计算出灰度共生矩阵，就从中提取多种纹理特征，其中对比度反映了图像中局部灰度变化的程度：
Contrast＝∑i,j(i-j)2M(i,j)
均匀性度量了图像纹理的均匀程度：
Uniformity＝∑i,jM(i,j)2
相关性度量了图像中灰度级之间的相关性：
其中，μi和μj是灰度级i和j的平均值，σi和σj是它们的标准差；
熵度量了图像纹理的不规则性或复杂性：
Entropy＝-∑i,jM(i,j)log(M(i,j))
使用灰度共生矩阵的均匀性和熵来确定分配值，当均匀性下降而熵值上升时则表示图像的纹理丰富，此时尽可能选择较多的点特征，提高***计算效率，否则选择较多的线段特征以确保准确度。
7.根据权利要求1所述的基于点线特征的视觉惯性组合导航算法，其特征在于：步骤(6)具体如下：
双目相机通过两个相机的视图估计特征的三维位置，在已知相机内参和n个三维空间点对应的二维图像点时，求解相机相对于这些三维点的姿态和位置；PnP问题的目标是从一组对应的3D-2D点对中恢复出相机的位姿；正常情况下，至少需要3个点对来求解这个问题，但是为了增加鲁棒性，通常会使用更多的点对；使用EPnP方法进行求解，它通过将空间点表示为虚拟点的线性组合来降低问题的复杂性；然后，使用图优化的方法，将估计的相机的位姿和特征的三维位置作为两个节点，将二维图像点和三维空间点的投影关系作为连接它们的边，以此同时优化相机位姿和三维空间点。
8.根据权利要求1所述的基于点线特征的视觉惯性组合导航算法，其特征在于：步骤(7)具体如下：
首先，确定***可能遵循的所有模型；
对于模型集合中的每个模型j，计算从模型i切换到模型j的概率pij，这是一个模型转移概率矩阵；对于每个模型j，更新其模型概率μj如下：
其中是基于模型j的似然函数，它度量了观测值与该模型预测值之间的匹配程度；对于每个模型，使用该模型的状态转移矩阵对状态进行预测；然后，使用卡尔曼滤波器的更新步骤根据观测值来更新预测状态；
将所有模型的状态估计混合在一起，得到总的状态估计：
其中，是基于模型j的状态估计；
同样地，将所有模型的协方差混合，得到总的协方差估计：
其中，Pj是基于模型j的协方差估计。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410025341.8A CN117870659A (zh) | 2024-01-08 | 2024-01-08 | 基于点线特征的视觉惯性组合导航算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410025341.8A CN117870659A (zh) | 2024-01-08 | 2024-01-08 | 基于点线特征的视觉惯性组合导航算法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117870659A true CN117870659A (zh) | 2024-04-12 |
Family
ID=90576694
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410025341.8A Pending CN117870659A (zh) | 2024-01-08 | 2024-01-08 | 基于点线特征的视觉惯性组合导航算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117870659A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118097191A (zh) * | 2024-04-22 | 2024-05-28 | 山东科技大学 | 一种抗遮挡的刚体目标快速匹配及位姿估计方法及*** |
-
2024
- 2024-01-08 CN CN202410025341.8A patent/CN117870659A/zh active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118097191A (zh) * | 2024-04-22 | 2024-05-28 | 山东科技大学 | 一种抗遮挡的刚体目标快速匹配及位姿估计方法及*** |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109059895B (zh) | 一种基于手机摄像头和传感器的多模态室内测距及定位方法 | |
| CN110555901B (zh) | 动静态场景的定位和建图方法、装置、设备和存储介质 | |
| CN103325112B (zh) | 动态场景中运动目标快速检测方法 | |
| EP2720171B1 (en) | Recognition and pose determination of 3D objects in multimodal scenes | |
| WO2016034059A1 (zh) | 基于颜色-结构特征的目标对象跟踪方法 | |
| CN110334762B (zh) | 一种基于四叉树结合orb和sift的特征匹配方法 | |
| CN110490158B (zh) | 一种基于多级模型的鲁棒人脸对齐方法 | |
| CN109544599B (zh) | 一种基于相机位姿估计的三维点云配准方法 | |
| Yu et al. | Robust robot pose estimation for challenging scenes with an RGB-D camera | |
| CN104933434A (zh) | 一种结合LBP特征提取和surf特征提取方法的图像匹配方法 | |
| Liu et al. | Direct visual odometry for a fisheye-stereo camera | |
| CN103136525B (zh) | 一种利用广义Hough变换的异型扩展目标高精度定位方法 | |
| CN109636854A (zh) | 一种基于line-mod模板匹配的增强现实三维跟踪注册方法 | |
| CN112419497A (zh) | 基于单目视觉的特征法与直接法相融合的slam方法 | |
| CN110021029B (zh) | 一种适用于rgbd-slam的实时动态配准方法及存储介质 | |
| CN105160649A (zh) | 基于核函数非监督聚类的多目标跟踪方法及*** | |
| CN108229500A (zh) | 一种基于函数拟合的sift误匹配点剔除法 | |
| CN107516322A (zh) | 一种基于对数极空间的图像物体大小和旋转估计计算方法 | |
| CN114782499A (zh) | 一种基于光流和视图几何约束的图像静态区域提取方法及装置 | |
| CN108550165A (zh) | 一种基于局部不变特征的图像匹配方法 | |
| CN117870659A (zh) | 基于点线特征的视觉惯性组合导航算法 | |
| Chen et al. | A stereo visual-inertial SLAM approach for indoor mobile robots in unknown environments without occlusions | |
| CN108447092B (zh) | 视觉定位标识物的方法及装置 | |
| CN113393524A (zh) | 一种结合深度学习和轮廓点云重建的目标位姿估计方法 | |
| CN110942473A (zh) | 一种基于特征点网格化匹配的运动目标跟踪检测方法 |
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 |