CN110388917B - 飞行器单目视觉尺度估计方法和装置、飞行器导航系统及飞行器 - Google Patents

飞行器单目视觉尺度估计方法和装置、飞行器导航系统及飞行器 Download PDF

Info

Publication number
CN110388917B
CN110388917B CN201810365375.6A CN201810365375A CN110388917B CN 110388917 B CN110388917 B CN 110388917B CN 201810365375 A CN201810365375 A CN 201810365375A CN 110388917 B CN110388917 B CN 110388917B
Authority
CN
China
Prior art keywords
scale
aircraft
imu
value
height
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
Application number
CN201810365375.6A
Other languages
English (en)
Other versions
CN110388917A (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.)
Beijing Jingdong Century Trading Co Ltd
Beijing Jingdong Shangke Information Technology Co Ltd
Original Assignee
Beijing Jingdong Century Trading Co Ltd
Beijing Jingdong Shangke Information 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 Beijing Jingdong Century Trading Co Ltd, Beijing Jingdong Shangke Information Technology Co Ltd filed Critical Beijing Jingdong Century Trading Co Ltd
Priority to CN201810365375.6A priority Critical patent/CN110388917B/zh
Publication of CN110388917A publication Critical patent/CN110388917A/zh
Application granted granted Critical
Publication of CN110388917B publication Critical patent/CN110388917B/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/20—Instruments for performing navigational calculations

Abstract

本公开提出一种飞行器单目视觉尺度估计方法和装置、飞行器导航系统及飞行，涉及飞行器技术领域。本公开的飞行器单目视觉尺度估计方法包括：获取当前飞行高度估计值；在飞行高度估计值小于预定高度的情况下，根据垂直向下探测的距离探测传感器的探测数据和惯性测量单元IMU的探测数据确定单目视觉尺度值。通过这样的方法，能够在飞行器的飞行高度小于预定高度的情况下，采用距离探测传感器与IMU配合的方式获取单目视觉尺度值，一方面无需人工参与，提高了自动化程度，另一方面也能够综合距离探测传感器和IMU的优势，减少单一测量方法造成的误差，提高尺度估计的准确度。

Description

飞行器单目视觉尺度估计方法和装置、飞行器导航系统及飞 行器
技术领域
本公开涉及飞行器技术领域，特别是一种飞行器单目视觉尺度估计方法和装置、飞行器导航系统及飞行器。
背景技术
精确的位姿估计是小型旋翼飞行器等飞行器进行自主飞行控制的关键。目前大多数飞行器采用GNSS(Global Navigation Satellite System，全球卫星导航系统)与IMU(Inertial Measurement Unit，惯性测量单元)组合进行位姿估计，但是在GNSS信号强度和准确度不佳的情况下无法正常工作，通过视觉SLAM(Simultaneous Localization andMapping，即时定位与地图构建)进行位姿估计，可以解决卫星信号无法获取时飞行器的位姿估计问题。
对于单目视觉SLAM中，单目视觉尺度估计是进行精确位姿估计的前提，目前，为了得到单目视觉的绝对尺度信息，需要人为设定尺度标杆。很多学者在这方面进行了研究，如，单目视觉SLAM中的经典算法PTAM(Parallel Tracking and Mapping，追踪与绘制)采用人为操作来对尺度进行估计，在其初始化过程中沿着坐标轴平移固定的距离，通过这个已知的距离对尺度进行估计，但是这样的方法需要人为干预手动进行初始化，无法实现完全自主。
发明内容
本公开的一个目的在于提高飞行器单目视觉尺度估计的自动化程度和准确度。
根据本公开的一个方面，提出一种飞行器单目视觉尺度估计方法，包括：获取当前飞行高度估计值；在飞行高度估计值小于预定高度的情况下，根据垂直向下探测的距离探测传感器的探测数据和IMU的探测数据确定单目视觉尺度值。
可选地，根据距离探测传感器的探测数据和IMU的探测数据确定单目视觉尺度值包括：根据距离探测传感器的探测数据获取SLAM算法的尺度估计值，作为第一尺度因子；根据IMU的探测数据获取SLAM算法的尺度估计值，作为第二尺度因子；基于尺度因子融合策略，根据第一尺度因子和第二尺度因子确定单目视觉尺度值。
可选地，尺度因子融合策略包括：根据公式：
λ＝λ1*θ1+λ2*θ2
确定单目视觉尺度值λ，其中，λ1为第一尺度因子，λ2为第二尺度因子，θ1为第一尺度因子的权重，θ2为第二尺度因子的权重，θ1与距离探测传感器的置信度与距离探测传感器和IMU的置信度之和的比值正相关，θ1和θ2为正数，且θ1+θ2＝1。
可选地，根据距离探测传感器的探测数据和单目视觉探测数据获取第一尺度因子包括：根据SLAM算法的输出频率获取距离探测传感器探测的高度变化情况；根据距离探测传感器统计的高度变化情况，以及ORB(Oriented FAST and Rotated BRIEF，快速特征点提取和描述)-SLAM算法确定的高度变化情况，基于加权最小二乘法确定单目视觉尺度值，其中，距离当前时刻越近的数据权重越大。
可选地，还包括：在飞行器高度大于等于预定高度的情况下根据IMU的探测数据获取SLAM算法的尺度估计值，作为单目视觉尺度值。
可选地，根据IMU的探测数据确定SLAM算法的尺度估计值包括：按照SLAM算法的输出频率获取IMU位姿信息，确定在任意相邻两帧单目图像的采集时刻之间IMU统计的位姿变化情况；根据IMU统计的位姿变化情况，以及ORB-SLAM算法确定的位姿变化情况，基于最小二乘法确定单目视觉尺度值。
可选地，距离探测传感器包括超声波传感器、激光雷达或毫米波雷达中的一种或多种。
可选地，当前飞行高度估计值为通过距离探测传感器获取。
通过这样的方法，能够在飞行器的飞行高度小于预定高度的情况下，采用距离探测传感器与IMU配合的方式获取单目视觉尺度值，一方面无需人工参与，提高了自动化程度，另一方面也能够综合距离探测传感器和IMU的优势，减少单一测量方法造成的误差，提高尺度估计的准确度。
根据本公开的另一个方面，提出一种飞行器单目视觉尺度估计装置，包括：高度估计模块，被配置为获取当前飞行高度估计值；尺度计算模块，被配置为在飞行高度估计值小于预定高度的情况下，根据垂直向下探测的距离探测传感器的探测数据和IMU的探测数据确定单目视觉尺度值。
可选地，尺度计算模块包括：第一尺度估计单元，被配置为根据距离探测传感器的探测数据获取SLAM算法的尺度估计值，作为第一尺度因子；第二尺度估计单元，被配置为根据IMU的探测数据获取SLAM算法的尺度估计值，作为第二尺度因子；融合单元，被配置为基于尺度因子融合策略，根据第一尺度因子和第二尺度因子确定单目视觉尺度值。
可选地，融合单元被配置为：根据公式：
λ＝λ1*θ1+λ2*θ2
确定单目视觉尺度值λ，其中，λ1为第一尺度因子，λ2为第二尺度因子，θ1为第一尺度因子的权重，θ2为第二尺度因子的权重，θ1与距离探测传感器的置信度与距离探测传感器和IMU的置信度之和的比值正相关，θ1和θ2为正数，且θ1+θ2＝1。
可选地，第一尺度估计单元被配置为：根据SLAM算法的输出频率获取距离探测传感器探测的高度变化情况；根据距离探测传感器统计的高度变化情况，以及ORB-SLAM算法确定的高度变化情况，基于加权最小二乘法确定单目视觉尺度值，其中，距离当前时刻越近的数据权重越大。
可选地，还包括：在飞行器高度大于等于预定高度的情况下根据IMU的探测数据获取SLAM算法的尺度估计值，作为单目视觉尺度值。
可选地，根据IMU的探测数据确定SLAM算法的尺度估计值包括：按照SLAM算法的输出频率获取IMU位姿信息，确定在任意相邻两帧单目图像的采集时刻之间IMU统计的位姿变化情况；根据IMU统计的位姿变化情况，以及ORB-SLAM算法确定的位姿变化情况，基于最小二乘法确定单目视觉尺度值。
可选地，距离探测传感器包括超声波传感器、激光雷达或毫米波雷达中的一种或多种。
可选地，高度估计模块被配置为通过距离探测传感器获取当前飞行高度估计值。
根据本公开的又一个方面，提出一种飞行器单目视觉尺度估计装置，包括：存储器；以及耦接至存储器的处理器，处理器被配置为基于存储在存储器的指令执行上文中任意一种飞行器单目视觉尺寸估计方法。
这样的装置能够在飞行器的飞行高度小于预定高度的情况下，采用距离探测传感器与IMU配合的方式获取单目视觉尺度值，一方面无需人工参与，提高了自动化程度，另一方面也能够综合距离探测传感器和IMU的优势，减少单一测量方法造成的误差，提高尺度估计的准确度。
根据本公开的再一个方面，提出一种计算机可读存储介质，其上存储有计算机程序指令，该指令被处理器执行时实现上文中任意一种飞行器单目视觉尺寸估计方法的步骤。
这样的计算机可读存储介质通过执行其上的指令，能够在飞行器的飞行高度小于预定高度的情况下，采用距离探测传感器与IMU配合的方式获取单目视觉尺度值，一方面无需人工参与，提高了自动化程度，另一方面也能够综合距离探测传感器和IMU的优势，减少单一测量方法造成的误差，提高尺度估计的准确度。
根据本公开的其中一个方面，提出一种飞行器导航系统，包括：距离探测传感器，被配置为探测飞行器到地面的距离；惯性测量单元IMU，被配置为探测飞行器的位姿信息；图像采集设备，被配置为采集地面的单目视觉图像；上文中任意一种飞行器单目视觉尺度估计装置；位姿运算器，被配置为根据单目视觉图像和单目视觉尺度值确定飞行器的位姿；和，路线规划设备，被配置为根据飞行器的位姿确定飞行器的飞行路径。
这样的飞行器导航系统在飞行器的飞行高度小于预定高度的情况下，采用距离探测传感器与IMU配合的方式获取单目视觉尺度值，提高尺度估计的准确度，从而提高位姿确定和导航的准确性。
另外，根据本公开的一个方面，提出一种飞行器，包括：飞行设备，被配置为带动飞行器飞行；能量供给设备，被配置为为飞行设备提供能量；和上文中提到的飞行器导航系统。
这样的飞行器在飞行器的飞行高度小于预定高度的情况下，采用距离探测传感器与IMU配合的方式获取单目视觉尺度值，提高尺度估计的准确度，进而提高位姿确定和导航的准确性，从而提高飞行器飞行的路线可靠性。
附图说明
此处所说明的附图用来提供对本公开的进一步理解，构成本公开的一部分，本公开的示意性实施例及其说明用于解释本公开，并不构成对本公开的不当限定。在附图中：
图1为本公开的飞行器单目视觉尺度估计方法的一个实施例的流程图。
图2为本公开的飞行器单目视觉尺度估计方法的另一个实施例的流程图。
图3为本公开的飞行器单目视觉尺度估计装置的一个实施例的示意图。
图4为本公开的飞行器单目视觉尺度估计装置中尺度计算模块的一个实施例的示意图。
图5为本公开的飞行器单目视觉尺度估计装置的另一个实施例的示意图。
图6为本公开的飞行器单目视觉尺度估计装置的又一个实施例的示意图。
图7为本公开的飞行器导航系统的一个实施例的示意图。
图8为本公开的飞行器的一个实施例的示意图。
具体实施方式
下面通过附图和实施例，对本公开的技术方案做进一步的详细描述。
本公开的飞行器单目视觉尺度估计方法的一个实施例的流程图如图1所示。
在步骤101中，获取当前飞行高度估计值。在一个实施例中，飞行高度估计值可以由飞行器上安装的、探测方向竖直向下的距离探测传感器获取。当飞行高度超过传感器的测量范围导致无法探测时，可以默认飞行高度估计值为预定门限值，预定门限值高于步骤102中的预定高度。
在步骤102中，将飞行高度估计值与预定高度相比较。在飞行高度估计值小于预定高度的情况下执行步骤103。在一个实施例中，可以持续进行高度探测和比较，当飞行高度估计值与预定高度的大小发生变化时，状态稳定达到预定时长则确定模式切换，从而避免偶然现象导致模式反复切换。
在步骤103中，根据垂直向下探测的距离探测传感器的探测数据和IMU的探测数据确定单目视觉尺度值。在一个实施例中，可以分别利用距离探测传感器的探测数据、IMU的探测数据获得两种方式的单目视觉尺度值，再将两者进行融合计算。
在一个实施例中，预定高度可以在15～100米范围内，根据距离探测传感器的性能、型号，以及飞行器飞行速度的不同而调整，如若距离探测传感器为超声波传感器，则预定高度优选为20米。在一个实施例中，距离探测传感器包括超声波传感器、激光雷达或毫米波雷达中的一种或多种。在一个实施例中，预定高度的设置值可以与距离探测传感器的探测波速正相关，与飞行器的飞行速度负相关。
通过这样的方法，能够在飞行器的飞行高度小于预定高度的情况下，采用距离探测传感器与IMU配合的方式获取单目视觉尺度值，一方面无需人工参与，提高了自动化程度，另一方面也能够综合距离探测传感器和IMU的优势，减少单一测量方法造成的误差，提高尺度估计的准确度。
本公开的飞行器单目视觉尺度估计方法的另一个实施例的流程图如图2所示。
在步骤201中，获取当前飞行高度估计值。
在步骤202中，将飞行高度估计值与预定高度相比较。在飞行高度估计值小于预定高度的情况下执行步骤203；否则，执行步骤206。
在步骤203中，根据距离探测传感器的探测数据获取SLAM算法的尺度估计值，作为第一尺度因子。
在一个实施例中，可以先根据SLAM算法的输出频率获取距离探测传感器探测的高度变化情况，再根据距离探测传感器统计的高度变化情况，以及ORB-SLAM算法确定的高度变化情况，基于加权最小二乘法确定单目视觉尺度值，其中，生成事件距离当前时刻越近的数据权重越大。
在步骤204中，根据IMU的探测数据获取SLAM算法的尺度估计值，作为第二尺度因子。
在一个实施例中，可以先按照SLAM算法的输出频率获取IMU位姿信息，确定在任意相邻两帧单目图像的采集时刻之间IMU统计的位姿变化情况，再根据IMU统计的位姿变化情况，以及ORB-SLAM算法确定的位姿变化情况，基于最小二乘法确定单目视觉尺度值。
在步骤205中，基于尺度因子融合策略，根据第一尺度因子和第二尺度因子确定单目视觉尺度值，进而返回执行步骤201进行持续探测。
在一个实施例中，可以根据公式：
λ＝λ1*θ1+λ2*θ2
确定单目视觉尺度值λ，其中，λ1为第一尺度因子，λ2为第二尺度因子，θ1为第一尺度因子的权重，θ2为第二尺度因子的权重，θ1与距离探测传感器的置信度与距离探测传感器和IMU的置信度之和的比值正相关，θ1和θ2为正数，且θ1+θ2＝1。
在步骤206中，根据IMU的探测数据，采用与步骤204中相似的算法获取SLAM算法的尺度估计值，作为单目视觉尺度值，进而返回执行步骤201进行持续探测。
通过这样的方法，能够在飞行高度较低的情况下综合距离探测传感器与IMU的探测结果，提高准确度；同时在飞行器飞行高度突然升高时，能够切换为只采用IMU的探测结果进行尺度估计，由于基于IMU的探测数据确定尺度估计值的运算一直在计算中，因此能够保证切换的效率，也能够提高切换时刻尺度估计的准确性，防止基于IMU探测结果的计算突然启动造成初始信息不准确，进而对尺度估计的准确度造成持续影响。
在一个实施例中，θ1、θ2的值可以是根据设备性能设定的预定固定值；θ1、θ2也可以是变值，其中θ1随飞行器高度的升高而降低，当飞行器飞行高度估计值高于预定高度时，θ1＝0，θ2＝1。
通过这样的方法，能够考虑到距离探测传感器的探测准确度在达到一定高度的情况下随着高度升高而降低的特点，逐渐降低对距离探测传感器的依赖程度，提高IMU探测结果的权重，从而将进一步提高在飞行高度低于预定高度时尺度估计的准确度，同时也能够在飞行高度升高到预定高度时提供更为准确的基础值，从而进一步提高尺度估计的准确度。
在一个实施例中，距离探测传感器以超声波传感器为例，采用超声波脉冲探测物体并利用其回波测量物体的距离，可以用来进行距离测量、目标识别以及避障。超声波传感器体积小、重量轻、功耗低、价格低廉，且工作环境很少受到限制。超声波传感器可以安装于飞行器的机体下方向下探测，以便测得飞行器的高度值。
当飞行器的飞行高度较低(如低于20米)时，如在飞行器的起飞阶段(即单目ORB-SLAM算法的初始化阶段)，可以测量飞行器的高度值。为了得到更加鲁棒的尺度估计结果，观测值越多越好，因此在飞行器的起飞阶段，需要高度变化尽可能多。
例如，在起飞阶段，单目ORB-SLAM算法可以得到固定时间间隔内飞行器变化的高度差估计值zi，同时超声波传感器也可测得相同时间间隔内飞行器变化的高度差hi。由于超声波传感器测得的高度为相对地面的绝对高度，因此尺度λ近似满足hi/zi。超声波传感器的测量噪声近似服从高斯分布，通过多组数据样本可减少噪声带来的测量误差，在得到一系列样本数据{(z1，h1)，(z2，h2)，…,(zn，hn)}之后即可采用最小二乘法对数据进行处理，求解尺度。
对于模型为h＝λz的函数进行参数估计，在获得一系列的样本数据后，采用最小二乘法估计。在测量噪声呈现高斯分布时采用最小二乘估计可实现无偏估计。
最小二乘法估计是通过最小化误差平方和来求得最优估计值。假设有n组独立观测值(z1，h1)，(z2，h2)，…,(zn，hn)，那么根据尺度与高度的关系式可以得到：
Figure GDA0003005189810000091
其中，λ为需要估计的参数，ε1，ε2，…，εn为误差值，且相互独立。最小二乘法的目的是估计出一个最优尺度估计值
Figure GDA0003005189810000092
使得其误差的平方和最小：
Figure GDA0003005189810000093
为了求得最优
Figure GDA0003005189810000094
对公式(2)进行求导：
Figure GDA0003005189810000095
令导数为零即可求得λ的最优估计值：
Figure GDA0003005189810000096
实际应用中，单目ORB-SLAM算法的尺度存在漂移，需要对不同时间段测量到的数据施加不同的权重。给就近的测量值较大的权重，历史时刻的测量值权重适当减小，采用加权最小二乘法实现尺度估计，即在计算残差平方和时加上权重w，如下所示：
Figure GDA0003005189810000101
同样根据上面的方法可求得加权最小二乘法最终得到的估计值为：
Figure GDA0003005189810000102
经过加权处理可以使得当前测量值在整个参数估计过程中占较大的比重，同时也能够在一定程度上消除单目ORB-SLAM算法尺度漂移带来的影响。通过多次实验发现，该方法得到的尺度估计误差小，同时稳定性较强，能够在飞行器上实时进行尺度估计。由于超声波传感器测距范围的限制，使用该方法进行尺度估计时对飞行器的飞行高度有一定的约束，若飞行器的飞行高度超过超声波测距范围，那么尺度估计将会失效。同时，在使用超声波传感器测量高度时，需要保证地面水平，无障碍物，保证声波能够顺利反射从而进行精确测量，尤其在室内或室外小场景环境中能够提高尺度估计的准确度。
在一个实施例中，相比于使用超声波传感器进行尺度估计时存在的环境限制问题，采用IMU进行尺度估计的算法对于工作环境的限制条件少，适用于室外大场景环境中。
IMU可以根据自身输出的加速度与角速度得到六自由度的位姿信息，其位置估计结果与当前速度相关，不能直接通过加速度输出值得到任意时间间隔内IMU的位移量，必须知道积分开始时刻的初始速度才能准确的获得位置。在使用IMU进行单目ORB-SLAM算法的尺度估计时，需要利用IMU得到的六自由度的位姿估计值作为观测值对视觉输出的位姿进行校正。因此，必须得到相邻两图像帧之间IMU的位姿变化信息，这就需要对IMU数据进行预处理，使其能够获得任意时间间隔内的位姿变化。
由于相机和IMU的输出频率不等，例如相机的输出频率为30Hz，而IMU的输出频率为200Hz，因此将IMU得到的位姿信息作为观测值时，只需得到IMU与视觉SLAM输出频率一致的位姿输出结果即可，不必将IM的每一帧数据都进行位姿估计处理，以免造成不必要的资源浪费，从而降低了数据处理量，降低了对设备的要求和负担，另外也能够增强IMU与视觉SLAM数据的相互对应。
在一个实施例中，可以对IMU的数据进行积分组合，使其位姿输出频率与视觉一致。按照相机的输出频率，将相机两次输入时刻之间的IMU数据进行一次性积分处理得到位姿输出结果，进行数据预处理后IMU的位姿输出频率将和视觉输出频率一致。
通过单目相机与IMU的外参标定，二者之间的位姿关系(pc i，qc i)已知。根据单目视觉尺度的定义以及单目相机和IMU的相对位姿关系，可得到IMU在世界坐标系下的坐标与相机在世界坐标系下的坐标之间的关系：
Figure GDA0003005189810000111
Figure GDA0003005189810000112
其中，s代表尺度值，Cc w代表相机坐标系与IMU坐标系之间的方向余弦矩阵，(pi w，qi w)为IMU的位姿估计结果，可由IMU的数据预处理得到，(pc w，qc w)为单目ORB-SLAM算法输出的不带尺度的位姿估计结果。
根据IMU的运动学模型与惯导解算方法，可以对IMU的数据进行预处理得到位姿计算公式:
Figure GDA0003005189810000113
Figure GDA0003005189810000114
Figure GDA0003005189810000115
上式中，
Figure GDA0003005189810000116
k，k+1分别代表图像帧来到的时刻，在k与k+1时间间隔内将会有多个IMU数据，因此对k与k+1时间段内的IMU数据进行预积分处理：
Figure GDA0003005189810000121
Figure GDA0003005189810000122
Figure GDA0003005189810000123
预积分处理的好处在于每次积分都是相对于第k时刻开始的，积分初始值为零。这样可以得到任意两个图像帧时刻之间IMU的位姿变化，且不会引入累积误差。最后将公式(9)带入(8)，则公式(8)可简化为：
Figure GDA0003005189810000124
Figure GDA0003005189810000125
Figure GDA0003005189810000126
为了计算尺度值，将公式(7)带入公式(9)，可以得到第k个图像帧时刻关于尺度s的线性方程如下：
Figure GDA0003005189810000127
上式中g表示重力向量，△tk则表示第k帧与第k+1帧之间的时间间隔，vi w(k)表示第k帧时刻IMU的速度。通过对上述方程进行求解即可获得单目ORB-SLAM算法的尺度值s。
通过单目ORB-SLAM算法与IMU解算可以获得多组位姿信息，而需要求解的未知数只有一个，因此该方程组是一个超定方程组。为了能够方便分析求解，将公式(11)写成矩阵表达形式：
Figure GDA0003005189810000128
其中，
Figure GDA0003005189810000129
求解上述方程，需要得到每个时刻IMU的速度vi w(k)。这将会带来较大的累积误差，为了避免求解每个时刻的速度值，可以采用差分的方法。
考虑到第k+1帧时刻上述线性方程可表示为：
Figure GDA00030051898100001210
Figure GDA00030051898100001211
用公式(11)乘△tk与公式(10)乘△tk-1相减，即可得到连续两帧之间的速度差，从而简化了求解IMU速度的过程：
Figure GDA00030051898100001212
其中各个分量的定义如下：
Figure GDA0003005189810000131
Figure GDA0003005189810000132
上式中各分量的值通过单目ORB-SLAM和IMU位姿解算求得，其中，pi c相机与IMU之间的固定位置关系，可通过外参标定得到。pi w(k,k+1),vi w(k,k+1)分别代表IMU在k到k+1时间段内的位置与速度变化，通过前面的IMU预积分处理得到。pc w,Cc w分别代表相机的位置与姿态，由ORB-SLAM算法得到。
通过上述分析可知，进行差分求解至少需要三帧图像，若图像帧数量为N，那么公式(12)左侧矩阵维数是3(N-2)×1，由于未知数数量为1，因此该方程为超定方程，此时方程的求解问题同样变成了一个最小二乘问题。
实际应用中，在ORB-SLAM算法初始化完成后，选取100帧左右的图像以及IMU数据进行尺度估计，这样能得到比较好的尺度估计结果，同时也不会占用过多的计算资源，可以在短时间内实现单目ORB-SLAM算法的尺度估计。
本公开的飞行器单目视觉尺度估计装置的一个实施例的示意图如图3所示。高度估计模块31能够获取当前飞行高度估计值。在一个实施例中，飞行高度估计值可以通过飞行器上安装的、探测方向竖直向下的距离探测传感器获取。尺度计算模块32能够在飞行高度估计值小于预定高度的情况下，根据垂直向下探测的距离探测传感器的探测数据和IMU的探测数据确定单目视觉尺度值。在一个实施例中，可以分别利用距离探测传感器的探测数据、IMU的探测数据获得两种方式的单目视觉尺度值，再将两者进行融合计算。
这样的飞行器单目视觉尺度估计装置能够在飞行器的飞行高度小于预定高度的情况下，采用距离探测传感器与IMU配合的方式获取单目视觉尺度值，一方面无需人工参与，提高了自动化程度，另一方面也能够综合距离探测传感器和IMU的优势，减少单一测量方法造成的误差，提高尺度估计的准确度。
本公开的飞行器单目视觉尺度估计装置中尺度计算模块的一个实施例的示意图如图4所示。
第一尺度估计单元421能够根据距离探测传感器的探测数据获取SLAM算法的尺度估计值，作为第一尺度因子。在一个实施例中，可以先根据SLAM算法的输出频率获取距离探测传感器探测的高度变化情况，再根据距离探测传感器统计的高度变化情况，以及快速特征点提取和描述ORB-SLAM算法确定的高度变化情况，基于加权最小二乘法确定单目视觉尺度值，其中，生成时间距离当前时刻越近的数据权重越大。
第二尺度估计单元422能够根据IMU的探测数据获取SLAM算法的尺度估计值，作为第二尺度因子。在一个实施例中，可以先按照SLAM算法的输出频率获取IMU位姿信息，确定在任意相邻两帧单目图像的采集时刻之间IMU统计的位姿变化情况，再根据IMU统计的位姿变化情况，以及快速特征点提取和描述ORB-SLAM算法确定的位姿变化情况，基于最小二乘法确定单目视觉尺度值。
融合单元423能够基于尺度因子融合策略，根据第一尺度因子和第二尺度因子确定单目视觉尺度值。
在一个实施例中，可以根据公式：
λ＝λ1*θ1+λ2*θ2
确定单目视觉尺度值λ，其中，λ1为第一尺度因子，λ2为第二尺度因子，θ1为第一尺度因子的权重，θ2为第二尺度因子的权重，θ1与距离探测传感器的置信度与距离探测传感器和IMU的置信度之和的比值正相关，θ1和θ2为正数，且θ1+θ2＝1。
在一个实施例中，尺度计算模块能够实时比较飞行高度估计值的变化与预定高度的大小，在飞行高度估计值变为大于等于预定高度的情况下，尺度计算模块切换至有第二尺度估计单元422将根据IMU的探测数据获取SLAM算法的尺度估计值作为单目视觉尺度值；在飞行高度估计值变为小于预定高度的情况下，激活尺度计算模块的其他模块共同确定单目视觉尺度值。
这样的飞行器单目视觉尺度估计装置能够在飞行器飞行高度较低的情况下综合考虑距离探测传感器与IMU的探测结果，提高准确度；同时在飞行器飞行高度突然升高时，能够切换为只采用IMU的探测结果进行尺度估计，由于基于IMU的探测数据确定尺度估计值的运算一直在计算中，因此能够保证切换的效率，也能够提高切换时刻尺度估计的准确性，防止基于IMU探测结果的计算突然启动造成初始信息不准确，进而对尺度估计的准确度造成持续影响。
本公开飞行器单目视觉尺度估计装置的一个实施例的结构示意图如图5所示。飞行器单目视觉尺度估计装置包括存储器501和处理器502。其中：存储器501可以是磁盘、闪存或其它任何非易失性存储介质。存储器用于存储上文中飞行器单目视觉尺度估计方法的对应实施例中的指令。处理器502耦接至存储器501，可以作为一个或多个集成电路来实施，例如微处理器或微控制器。该处理器502用于执行存储器中存储的指令，能够实现提高尺度估计的自动化程度和准确度。
在一个实施例中，还可以如图6所示，飞行器单目视觉尺度估计控制600包括存储器601和处理器602。处理器602通过BUS总线603耦合至存储器601。该飞行器单目视觉尺度估计装置600还可以通过存储接口604连接至外部存储装置605以便调用外部数据，还可以通过网络接口606连接至网络或者另外一台计算机系统(未标出)。此处不再进行详细介绍。
在该实施例中，通过存储器存储数据指令，再通过处理器处理上述指令，能够实现提高尺度估计的自动化程度和准确度。
在另一个实施例中，一种计算机可读存储介质，其上存储有计算机程序指令，该指令被处理器执行时实现飞行器单目视觉尺度估计方法对应实施例中的方法的步骤。本领域内的技术人员应明白，本公开的实施例可提供为方法、装置、或计算机程序产品。因此，本公开可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本公开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用非瞬时性存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本公开的飞行器导航系统的一个实施例的示意图如图7所示。距离探测传感器71可以为超声波传感器、激光雷达或毫米波雷达中的一种或多种，能够实时探测飞行器的飞行高度。IMU72能够探测飞行器的位姿信息。图像采集设备73能够采集地面的单目视觉图像。飞行器单目视觉尺度估计装置74可以为上文中的任意一种，能够根据距离探测传感器71、IMU72和图像采集设备73采集的信息，采用上文中任意一种飞行器单目视觉尺度估计方法实时确定单目视觉图像的尺度值。位姿运算器75能够根据单目视觉图像和单目视觉尺度值确定飞行器的位姿，以便于路线规划设备76根据飞行器的位姿确定飞行器的飞行路径。
这样的飞行器导航系统在飞行器的飞行高度小于预定高度的情况下，采用距离探测传感器与IMU配合的方式获取单目视觉尺度值，提高尺度估计的准确度，进而提高位姿确定和导航的准确性，从而提高飞行器飞行的路线可靠性。
本公开的飞行器的一个实施例的示意图如图8所示。飞行设备81可以为旋翼等设备，能够带动飞行器飞行。能量供给设备82能够为飞行设备提供能量，在一个实施例中，能量供给设备82还能够为飞行器的其他部分，如飞行器导航系统83供电；飞行器导航系统83可以为上文如图7所示的飞行器导航系统，能够确定飞行路线并实时纠正，提高飞行器飞行路线的可靠性。
这样的飞行器在飞行器的飞行高度小于预定高度的情况下，采用距离探测传感器与IMU配合的方式获取单目视觉尺度值，提高尺度估计的准确度，进而提高位姿确定和导航的准确性，从而提高飞行器飞行的路线可靠性。
本公开是参照根据本公开实施例的方法、设备(系统)和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
至此，已经详细描述了本公开。为了避免遮蔽本公开的构思，没有描述本领域所公知的一些细节。本领域技术人员根据上面的描述，完全可以明白如何实施这里公开的技术方案。
可能以许多方式来实现本公开的方法以及装置。例如，可通过软件、硬件、固件或者软件、硬件、固件的任何组合来实现本公开的方法以及装置。用于所述方法的步骤的上述顺序仅是为了进行说明，本公开的方法的步骤不限于以上具体描述的顺序，除非以其它方式特别说明。此外，在一些实施例中，还可将本公开实施为记录在记录介质中的程序，这些程序包括用于实现根据本公开的方法的机器可读指令。因而，本公开还覆盖存储用于执行根据本公开的方法的程序的记录介质。
最后应当说明的是：以上实施例仅用以说明本公开的技术方案而非对其限制；尽管参照较佳实施例对本公开进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本公开的具体实施方式进行修改或者对部分技术特征进行等同替换；而不脱离本公开技术方案的精神，其均应涵盖在本公开请求保护的技术方案范围当中。

Claims (18)

1.一种飞行器单目视觉尺度估计方法，包括：
获取当前飞行高度估计值；
在所述飞行高度估计值小于预定高度的情况下，根据垂直向下探测的距离探测传感器的探测数据和惯性测量单元IMU的探测数据确定单目视觉尺度值，包括：
根据所述距离探测传感器的探测数据获取即时定位与地图构建SLAM算法的尺度估计值，作为第一尺度因子；
根据所述IMU的探测数据获取所述SLAM算法的尺度估计值，作为第二尺度因子；
基于尺度因子融合策略，根据所述第一尺度因子和所述第二尺度因子确定所述单目视觉尺度值。
2.根据权利要求1所述的方法，其中，所述尺度因子融合策略包括：
根据公式：
λ＝λ1*θ1+λ2*θ2
确定所述单目视觉尺度值λ，其中，λ1为所述第一尺度因子，λ2为所述第二尺度因子，θ1为所述第一尺度因子的权重，θ2为所述第二尺度因子的权重，θ1与所述距离探测传感器的置信度与所述距离探测传感器和所述IMU的置信度之和的比值正相关，θ1和θ2为正数，且θ1+θ2＝1。
3.根据权利要求1所述的方法，其中，所述根据所述距离探测传感器的探测数据和单目视觉探测数据获取第一尺度因子包括：
根据SLAM算法的输出频率获取所述距离探测传感器探测的高度变化情况；
根据所述距离探测传感器统计的高度变化情况，以及快速特征点提取和描述ORB-SLAM算法确定的高度变化情况，基于加权最小二乘法确定所述单目视觉尺度值，其中，距离当前时刻越近的数据权重越大。
4.根据权利要求1所述的方法，还包括：
在所述飞行器高度大于等于所述预定高度的情况下根据所述IMU的探测数据获取所述SLAM算法的尺度估计值，作为单目视觉尺度值。
5.根据权利要求1或4所述的方法，其中，所述根据所述IMU的探测数据确定所述SLAM算法的尺度估计值包括：
按照SLAM算法的输出频率获取IMU位姿信息，确定在任意相邻两帧单目图像的采集时刻之间IMU统计的位姿变化情况；
根据IMU统计的位姿变化情况，以及快速特征点提取和描述ORB-SLAM算法确定的位姿变化情况，基于最小二乘法确定所述单目视觉尺度值。
6.根据权利要求1所述的方法，其中，
所述距离探测传感器包括超声波传感器、激光雷达或毫米波雷达中的一种或多种。
7.根据权利要求1或6所述的方法，其中，
所述当前飞行高度估计值为通过所述距离探测传感器获取。
8.一种飞行器单目视觉尺度估计装置，包括：
高度估计模块，被配置为获取当前飞行高度估计值；
尺度计算模块，被配置为在所述飞行高度估计值小于预定高度的情况下，根据垂直向下探测的距离探测传感器的探测数据和惯性测量单元IMU的探测数据确定单目视觉尺度值，包括：
第一尺度估计单元，被配置为根据所述距离探测传感器的探测数据获取即时定位与地图构建SLAM算法的尺度估计值，作为第一尺度因子；
第二尺度估计单元，被配置为根据所述IMU的探测数据获取所述SLAM算法的尺度估计值，作为第二尺度因子；
融合单元，被配置为基于尺度因子融合策略，根据所述第一尺度因子和所述第二尺度因子确定所述单目视觉尺度值。
9.根据权利要求8所述的装置，其中，所述融合单元被配置为：
根据公式：
λ＝λ1*θ1+λ2*θ2
确定所述单目视觉尺度值λ，其中，λ1为所述第一尺度因子，λ2为所述第二尺度因子，θ1为所述第一尺度因子的权重，θ2为所述第二尺度因子的权重，θ1与所述距离探测传感器的置信度与所述距离探测传感器和所述IMU的置信度之和的比值正相关，θ1和θ2为正数，且θ1+θ2＝1。
10.根据权利要求8所述的装置，其中，所述第一尺度估计单元被配置为：
根据SLAM算法的输出频率获取所述距离探测传感器探测的高度变化情况；
根据所述距离探测传感器统计的高度变化情况，以及快速特征点提取和描述ORB-SLAM算法确定的高度变化情况，基于加权最小二乘法确定所述单目视觉尺度值，其中，距离当前时刻越近的数据权重越大。
11.根据权利要求8所述的装置，还包括：
在所述飞行器高度大于等于所述预定高度的情况下根据所述IMU的探测数据获取所述SLAM算法的尺度估计值，作为单目视觉尺度值。
12.根据权利要求8或11所述的装置，其中，所述根据所述IMU的探测数据确定所述SLAM算法的尺度估计值包括：
按照SLAM算法的输出频率获取IMU位姿信息，确定在任意相邻两帧单目图像的采集时刻之间IMU统计的位姿变化情况；
根据IMU统计的位姿变化情况，以及快速特征点提取和描述ORB-SLAM算法确定的位姿变化情况，基于最小二乘法确定所述单目视觉尺度值。
13.根据权利要求8所述的装置，其中，
所述距离探测传感器包括超声波传感器、激光雷达或毫米波雷达中的一种或多种。
14.根据权利要求8或13所述的装置，其中，
所述高度估计模块被配置为通过所述距离探测传感器获取所述当前飞行高度估计值。
15.一种飞行器单目视觉尺度估计装置，包括：
存储器；以及
耦接至所述存储器的处理器，所述处理器被配置为基于存储在所述存储器的指令执行如权利要求1至7任一项所述的方法。
16.一种计算机可读存储介质，其上存储有计算机程序指令，该指令被处理器执行时实现权利要求1至7任意一项所述的方法的步骤。
17.一种飞行器导航系统，包括：
距离探测传感器，被配置为探测飞行器到地面的距离；
惯性测量单元IMU，被配置为探测飞行器的位姿信息；
图像采集设备，被配置为采集地面的单目视觉图像；
权利要求8～15任意一项所述的飞行器单目视觉尺度估计装置；
位姿运算器，被配置为根据所述单目视觉图像和单目视觉尺度值确定所述飞行器的位姿；和，
路线规划设备，被配置为根据飞行器的位姿确定所述飞行器的飞行路径。
18.一种飞行器，包括：
飞行设备，被配置为带动飞行器飞行；
能量供给设备，被配置为向所述飞行设备提供能量；和，
权利要求17所述的飞行器导航系统。
CN201810365375.6A 2018-04-23 2018-04-23 飞行器单目视觉尺度估计方法和装置、飞行器导航系统及飞行器 Active CN110388917B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810365375.6A CN110388917B (zh) 2018-04-23 2018-04-23 飞行器单目视觉尺度估计方法和装置、飞行器导航系统及飞行器

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810365375.6A CN110388917B (zh) 2018-04-23 2018-04-23 飞行器单目视觉尺度估计方法和装置、飞行器导航系统及飞行器

Publications (2)

Publication Number Publication Date
CN110388917A CN110388917A (zh) 2019-10-29
CN110388917B true CN110388917B (zh) 2021-07-13

Family

ID=68284426

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810365375.6A Active CN110388917B (zh) 2018-04-23 2018-04-23 飞行器单目视觉尺度估计方法和装置、飞行器导航系统及飞行器

Country Status (1)

Country Link
CN (1) CN110388917B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113272625A (zh) * 2020-05-06 2021-08-17 深圳市大疆创新科技有限公司 飞行器定位方法、装置、飞行器及存储介质
CN114485649B (zh) * 2022-02-09 2023-09-12 北京自动化控制设备研究所 面向无人机的惯性、视觉和高度信息融合导航方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103292804B (zh) * 2013-05-27 2015-07-15 浙江大学 一种单目自然视觉路标辅助的移动机器人定位方法
US9424647B2 (en) * 2013-08-12 2016-08-23 Apple Inc. Visual-based inertial navigation
CN105844674B (zh) * 2016-03-02 2018-11-30 淮阴师范学院 基于三元数小波变换的彩色图像融合系统及方法
CN107248171B (zh) * 2017-05-17 2020-07-28 同济大学 一种基于三角剖分的单目视觉里程计尺度恢复方法
CN107607111A (zh) * 2017-09-07 2018-01-19 驭势科技（北京）有限公司 加速度偏置估计方法和装置、视觉惯性里程计及其应用

Also Published As

Publication number Publication date
CN110388917A (zh) 2019-10-29

Similar Documents

Publication Publication Date Title
US11506512B2 (en) Method and system using tightly coupled radar positioning to improve map performance
Li et al. LIDAR/MEMS IMU integrated navigation (SLAM) method for a small UAV in indoor environments
CN106705936B (zh) 一种无人机高度优化方法及装置
KR20200044420A (ko) 위치 추정 방법 및 장치
Shen et al. Optical flow sensor/INS/magnetometer integrated navigation system for MAV in GPS-denied environment
CN109807911B (zh) 基于gnss、uwb、imu、激光雷达、码盘的室外巡逻机器人多环境联合定位方法
CN109282808B (zh) 用于桥梁三维巡航检测的无人机与多传感器融合定位方法
CN112230242B (zh) 位姿估计系统和方法
US8723987B2 (en) Uncertainty estimation of planar features
CN109937341B (zh) 用于估计自己的位置的方法和设备
US7792330B1 (en) System and method for determining range in response to image data
CN110889808A (zh) 一种定位的方法、装置、设备及存储介质
KR20150043392A (ko) 바람 벡터 추산
EP2923175A1 (en) Improving kalman filtering with indirect noise measurements
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN110764531A (zh) 基于激光雷达与人工势场法的无人机编队飞行避障方法
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN110388917B (zh) 飞行器单目视觉尺度估计方法和装置、飞行器导航系统及飞行器
Scannapieco et al. Ultralight radar for small and micro-UAV navigation
CN108227734A (zh) 用于控制无人机的电子控制装置、相关的无人机、控制方法及计算机程序
Høglund Autonomous inspection of wind turbines and buildings using an UAV
Bikmaev et al. Improving the accuracy of supporting mobile objects with the use of the algorithm of complex processing of signals with a monocular camera and LiDAR
CN114861725A (zh) 一种目标感知跟踪的后处理方法、装置、设备及介质
US20210199798A1 (en) Continuous wave radar terrain prediction method, device, system, and unmanned aerial vehicle
US10670396B2 (en) Multi-sensor target location registration

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