技术领域
[0001] 本
发明属于无人机导航室内定位技术领域，尤其涉及一种无人机视觉-惯性融合室内定位方法。
背景技术
[0002] 当无人机进行室内定位时，
信号受
建筑物等因素的影响，导致卫星室内定位
精度较低；惯性
传感器存在漂移误差，采用积分运算使得长时间工作下误差累积；
激光雷达室内定位精度虽高，但因其高昂的价格而得不到广泛的应用。
发明内容
[0003] 针对上述问题，本发明的目的在于提供一种无人机视觉-惯性融合室内定位方法，解决在缺少卫星室内定位情况下的无人机室内定位定姿问题。
[0004] 为实现上述目的，本发明提供如下技术方案：
[0005] 一种无人机视觉-惯性融合室内定位方法，所述室内定位方法应用于加载有双目相机和惯性传感器的无人机定位系统中，所述方法包括：
[0006] S1，对双目相机获得的图像进行ORB特征提取，确定左右图像的匹配关系，并初步估计无人机的
位置和
姿态；
[0007] S2，对惯性传感器的测量数据进行预积分，确定任意两
帧图像间无人机的运动轨迹与姿态；
[0008] S3，根据所述初步估计无人机的位置和姿态，以及任意两帧图像间无人机的运动轨迹和姿态，对双目相机和惯性传感器进行校准；
[0009] S4，采用基于滑动窗口的紧耦合
双目视觉-惯性
里程计对无人机的状态进行估计，根据所述无人机的状态进行室内定位。
[0010] 本发明技术方案的特点和进一步的改进为：
[0011] (1)S1中，对双目相机获得的图像进行ORB特征提取，提取的特征点在相机
坐标系下的坐标为：
[0012]
[0013]
[0014]
[0015] 式中，[Xc，Yc，Zc]T为任意特征点P带尺度的相机坐标；f为相机焦距，UL和VL分别为点特征点PL在相机坐标系下的横、纵坐标，b为双目
光心间实际距离；d 为
视差，d＝uL-uR；(uO，vO)为左目相机光心坐标；
[0016] 采用金字塔LK光流法对提取的特征点进行特征
跟踪，确定左右图像的匹配关系。
[0017] (2)S1中，初步估计无人机的位置和姿态，具体为：
[0018] 对当前图像帧进行特征提取，并与先前图像帧的特征点进行LK光流跟踪；若有大于预设个特征点的稳定跟踪，则使用五点法恢复相机的室内定位，否则将当前图像特征存入滑动窗口，并等待新的图像帧；
[0019] 相机的室内定位恢复以后，计算当前图像帧中特征点的三维空间坐标，并对滑动窗口内的其他帧图像进行室内定位估计；
[0020] 对滑动窗口内的图像帧进行全局光束调整，最小化特征点的重投影误差，完成无人机的位置和姿态的估计。
[0021] (3)S3，对双目相机和惯性传感器进行校准包括：对
陀螺仪和
加速度漂移的校准，对无人机速度和重
力方向的校准。
[0022] (4)S4，采用基于滑动窗口的紧耦合双目视觉-惯性里程计对无人机的状态进行估计时，关键帧的选择方法为：
[0023] 距离上一次插入关键帧经过了20帧图像；
[0024] 当前帧跟踪的特征点少于50个。
[0025] (5)S4，采用基于滑动窗口的紧耦合双目视觉-惯性里程计对无人机的状态进行估计时，边缘化滑动窗口的方法为：
[0026] 若当前帧是关键帧，则边缘化滑动窗口内最老的关键帧；
[0027] 若当前帧不是关键帧，则剔除当前帧的视觉测量数据，保存惯性测量数据。
[0028] (6)S4，采用基于滑动窗口的紧耦合双目视觉-惯性里程计对无人机的状态进行估计时，利用光流跟踪获得的
像素点运动速度以及利用IMU预积分进行重投影计算获得的特征点像素速度进行对比，排除位于环境中的动态物体上的特征点。
[0029] (7)在S4之后，所述方法还包括：利用词袋模型进行闭环检测，采用室内定位图进行全局优化，实现无人机的重室内定位。
[0030] 本发明的一种无人机视觉-惯性融合室内定位法，在无人机上搭载双目相机和惯性传感器，通过双目相机获取室内环境图像信息，联合优化惯性测量单元(Inertial measurement unit，以下简称IMU)预积分数据，估计无人机位置、姿态、速度、重力方向以及陀螺仪与
加速度计的漂移。采用词袋模型进行无人机闭环检测，使用全局室内定位图优化实现无人机的室内定位，消除长时间工作的累积误差，以获得无人机在室内的精确定位数据。
附图说明
[0032] 图2为双目视差法示意图；
[0033] 图3为系统初始化示意图；
[0034] 图4为滑动窗口内关键帧边缘化示意图；
[0035] 图5为系统重室内定位示意图；
[0036] 图6为全局室内定位图优化示意图。
具体实施方式
[0037] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及
实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
[0038] 本发明提供一种无人机视觉-惯性融合室内定位方法，将双目视觉传感器与IMU以紧耦合方式进行融合。利用双目视差法获得特征点的尺度信息；以金字塔LK光流
算法跟踪特征点；使用RANSAC(随机
采样一致性)算法，剔除误匹配的特征点；同时对惯性测量数据预积分，保持传感器间的数据同步；构建滑动窗口，对关键帧进行选择与边缘化管理，并采用图优化方式估计无人机的位置、姿态、速度、重力方向以及惯性器件的漂移。最后设计了无人机在动态环境下的室内定位以及对曾到达的位置进行闭环检测与重室内定位的功能，有效提高了无人机室内定位的精度与鲁棒性。
[0039] 图1为本发明进行无人机视觉-惯性融合室内定位解算的总体
流程图。
[0040] 步骤1：数据预处理
[0041] 对双目获得的图像进行ORB特征提取，并确定左右图像的匹配关系。
[0042] 首先对双目获得的图像进行提取ORB特征点，双目相机的成像模型，如图 2所示。图中，点P为物体特征点位置，OL和OR分别为左目相机和右目相机，由双目视差法获得特征点P的在相机坐标系下的坐标为：
[0043]
[0044]
[0045]
[0046] 式中，[Xc，Yc，Zc]T为点P带尺度的相机坐标；f为相机焦距，UL和VL分别为点PL在相机坐标系下的横、纵坐标，b为基线，即双目光心间实际距离；d为视差，d＝uL-uR；(uO，vO)为左目相机光心坐标。
[0047] 然后采用金字塔LK光流法进行特征跟踪(这里跟踪的作用应该是建立特征点之间的匹配关系)，确定左右图像的匹配关系。金字塔LK光流算法流程为：
[0048] ①对第一帧与第二帧图像构建各自的5层图像金字塔；
[0049] ②将第一帧原图像检测出的特征点的横纵坐标缩小为原来的1/16，即得到第一帧原图像的金字塔最顶层的
角点坐标；
[0050] ③用第一帧原图像的金字塔最顶层的角点坐标作为角点在第二帧图像的金字塔最顶层的坐标预测值；
[0051] ④从图像金字塔最顶层开始，对当前金字塔层，利用LK光流法对第二帧角点坐标预测值进行匹配和修正；
[0052] ⑤若未到达图像金字塔底层，则将第一帧图像的当前金字塔层中的特征点横纵坐标以及第二帧图像的当前金字塔层用LK光流法修正后的角点的横纵坐标都乘以2，得到第一帧图像的下一层金字塔层特征点的横纵坐标和第二帧图像的下一层金字塔预测特征点的横纵坐标。否则返回④。
[0053] 步骤2：视觉-惯性融合估计及校准计算
[0054] 系统初始化方法，从步骤1的纯视觉估计中，可以获得尺度信息，并粗略估计系统的重力、速度、陀螺仪与加速度漂移，如图3所示，通过初始化，将视觉传感器测量值与惯导预积分值对齐。
[0055] (1)滑动窗口(是视觉SLAM系统中的一种通用手段，就是把连续n帧的信息放在一个滑动窗口内，一起解算信息)内的纯视觉估计
[0056] 在步骤一中对最新的图像帧进行特征提取，并与先前图像帧特征点尝试进行LK光流跟踪。此时若能有大于30个特征点的稳定跟踪，则使用五点法恢复相机的室内定位，即旋转与平移矩阵。否则就将当前图像特征存入滑动窗口，并等待新的图像帧。其次，相机的初始室内定位恢复以后，计算特征点的三维空间坐标，并对滑动窗口内的其他帧图像进行室内定位估计。最后，对窗口内的图像帧进行全局光束调整，最小化特征点的重投影误差。完成利用纯视觉估计系统的位置和姿态
[0057] (2)IMU-相机校准
[0058] (a)陀螺仪漂移校准
[0059] 用一阶线性化更新预积分，构造代价函数,在滑动窗口内利用最小二乘法最小化代价函数，对陀螺仪漂移bω进行初始化。
[0060] (b)速度、重力粗校准
[0061] 构造估计量 其中 为第k帧图像在载体坐标系下的速度； 为在c0帧图像参考系下的重力向量。构建代价函数，在滑动窗口内利用最小二乘法求解上式，可以获得每帧图像的速度与c0帧图像参考系下的重力向量[0062] (c)重力精校准
[0063] 由于加速度计测量的是包括
重力加速度大小与方向在内的载体线加速度值，故排除重力加速度的干扰，获得载体真实的线加速度在IMU测量数据的利用中起着至关重要的作用。大多数情况下，重力的幅值大小都可认为是已知的，将世界坐标系下重力向量Gɡw分解到所求得重力向量 的方向上：
[0064] ɡw＝[0 0 1]T
[0065]
[0066]
[0067]
[0068]
[0069] 式中，ɡw为世界坐标系下重力的方向向量； 为单位向量，G为重力的幅值标量；为世界坐标系到c0帧图像参考系的旋转变换。
[0070] 通过上式可以得到c0帧图像参考系至世界坐标系的旋转变换 于是，所有以c0帧图像为参考系的量均可以转换到世界坐标系(·)w下。至此，搭载双目相机和惯性传感器的无人机系统的初始化完成(得到初始状态量)。
[0071] 步骤3：基于视觉-惯性融合的无人机室内定位解算
[0072] 系统初始化后，在此采用基于滑动窗口的紧耦合双目视觉-惯性里程计对定位系统(搭载双目相机和惯性传感器的无人机系统)状态进行精确鲁棒估计。
[0073] (1)关键帧选择与边缘化策略
[0074] 由于在此提出的视觉-惯性里程计是采用对滑动窗口内的关键帧优化方式进行状态量估计，为了限制系统计算复杂度，需要限制窗口中图像的数量，对窗口中冗余的关键帧进行边缘化，保证系统的实时性；同时又要确保窗口插入的关键帧的
质量，提高优化结果的准确性，故系统对关键帧的选择与边缘化的策略需进行特殊设计。
[0075] (a)关键帧选择
[0076] 关键帧的选择需要考虑到插入滑动窗口的关键帧的质量，同时也需要保证窗口内关键帧的更新速率，防止在无人机在运动中，滑动窗口内的最新关键帧与当前帧时间间隔太长，造成跟踪失败的情况。关键帧的选择策略如下所述：
[0077] ①距离上一次插入关键帧经过了20帧图像；
[0078] ②当前帧跟踪的特征点少于50个。
[0079] (b)边缘化策略
[0080] 同样是考虑滑动窗口内关键帧的数量与质量，需要对滑动窗口内的关键帧、 IMU状态量xk以及特征点进行边缘化处理，以保证滑动窗口内关键帧的流动性。如图4所示，边缘化策略如下所述：
[0081] ①当前帧若是关键帧，则边缘化滑动窗口内最老的关键帧；
[0082] ②当前帧若不是关键帧，则剔除当前帧的视觉测量数据，保存IMU测量数据。
[0083] 边缘化策略采用Sobel进行计算。在此构建一个基于所有边缘化测量数据的先验项，并加入到现有的先验项中。
[0084] (2)动态环境下的鲁棒跟踪
[0085] 对特征点的跟踪采用五层金字塔LK光流算法，算法能获得特征点对应的像素点的运动速度。由于空间中特征点相对相机的运动速度与其在图像平面上投影得到的像素点的运动速度具有对应关系，故可就用此对应关系获得重投影速度，与光流法获得的像素速度进行比较，从而在静态环境中分离出动态的特征点，再将其剔除，以保证跟踪的特征点均位于静态物体上，最大程度确保优化结果的准确性，同时提高无人机室内定位的鲁棒性。
[0086] 静态环境下，特征点对应的像素点的运动速度与无人机的真实室内定位具有约束关系，故提高动态环境下无人机室内定位鲁棒性的策略为：
[0087] ①视觉-惯性里程计使用光流算法对特征角点进行跟踪优化，获得无人机的位置、速度、姿态、重力、IMU陀螺仪与加速度计的漂移；
[0088] ②计算获得特征点的像素速度误差；
[0089] ③若特征点的像素速度误差大于
阈值，剔除该特征点，并重复①；
[0090] ④若特征点的像素速度误差小于阈值，则继续对图像进行跟踪。
[0091] 步骤4：闭环检测与重室内定位
[0092] 采用一种紧耦合的重室内定位方式，在无人机到达先前到过的位置时，能够进行闭环检测，构建新的约束，同时进行高精度的重室内定位，消除系统长时间工作的累积误差，如图5所示。图中，①②为闭环检测过程；③为重室内定位过程；④⑤⑥为多重闭环约束的构建同时重室内定位过程，详细步骤如下：
[0093] (1)闭环检测
[0094] 采用词袋模型进行闭环检测。使用基于公开数据集训练的词袋，并在视觉里程计对关键帧提取ORB特征点后计算每帧图像的单词向量，并将关键帧的空间与图像特征点信息以及单词向量存储在在此构建的视觉
数据库中。对新图像帧提取ORB特征后，计算其单词向量并带入视觉数据库进行查询，查询结果返回与当前帧有相似单词向量的闭环候选帧。闭环候选帧是与当前帧的相似度大于当前帧与滑动窗口内最新关键帧相似度三倍的关键帧。
[0095] 在完成初步闭环检测后，在此获得了闭环候选帧。由于与滑动窗口内的连续关键帧存在许多共视点，构建起数据库中闭环候选帧与窗口中连续关键帧间的闭环约束。由于ORB特征匹配存在大量误匹配的情况，需要对其进行特殊的处理。在此采用RANSAC算法去除特征点的误匹配情况，具体处理步骤为：
[0096] ①2D-2D：对空间特征点在当前帧的2D图像上的投影点与在数据库中关键帧的2D图像上的投影点之间，采用RANSAC算法进行
基础矩阵测试，剔除部分坏点；
[0097] ②3D-2D：对窗口中已知的空间特征点的3D位置与其在数据库中关键帧的 2D图像上的投影点之间，采用RANSAC算法进行PnP测试，剔除剩下的坏点。
[0098] (2)闭环重室内定位
[0099] 在获得最终的连续闭环帧后，对滑动窗口中的所有关键帧进行重室内定位。在重室内定位过程中，数据库中闭环帧的只为优化提供约束，其室内定位在优化过程中不发生变化。滑动窗口中重室内定位过程的优化项包括窗口内IMU测量数据、视觉测量数据、以及与闭环帧间共视的空间特征点。
[0100] (3)全局室内定位优化
[0101] 经过室内定位优化过程，滑动窗口内关键帧的累积误差被消除，但窗口外的数据库中的关键帧误差却依旧存在，故需要对数据库中的关键帧进行一次全局优化，以消除其累积误差，如图6所示。
[0102] 考虑到系统的计算资源有限，在此采用室内定位图进行全局优化，即全局优化估计的状态量为各关键帧的位置与姿态。由于无人机的
俯仰与
滚转角可直接由IMU测量获得，因此待优化的室内定位只有四个
自由度(x、y、z、航向角)。
[0103] 1)室内定位图中添加关键帧
[0104] 无人机在检测到闭环并重室内定位后继续向前运动，滑动窗口中继续添加新的关键帧，最老的关键帧被边缘化出窗口，这时，将被边缘化的关键帧被当作新的结点添加进室内定位图中，并建立结点的两种边。
[0105] ①建立顺序边：最新加入室内定位图的关键帧i与数据库中最近的关键帧j建立顺序边，顺序边包括位置 约束与航向角 约束：
[0106]
[0107]
[0108] ②建立闭环边：若被边缘化的关键帧为闭环帧，则建立其与数据库中有闭环关系的关键帧之间的闭环边，与上式类似。
[0109] 2)四自由度室内定位图全局优化
[0110] 利用视觉-惯性里程计估计获得的俯仰、滚转角度计算第i帧与第j帧间的四自由度室内定位残差，对四自由度室内定位全局优化代价函数进行优化。
[0111] 考虑到全局室内定位图优化占有资源较多，在此将其放在一个独立的线程里运行，保证系统的正常运行。
[0112] 3)室内定位图管理
[0113] 由于全局室内定位图随着无人机工作时间的增长而变得原来越庞大，为了确保系统能进行长时间的工作，在此对室内定位图进行适当地管理，限制其规模。其策略为：所有包含闭环边的关键帧将全部保留，其余的在时间和特征上太相近的帧将被剔除。
[0114] 本发明提出一种无人机视觉-惯性融合室内定位方法，由于室内环境空间有限，光照变化较小，且室内无人机运动速度较慢，在此采用双目视觉与IMU作为传感器，首先通过对惯性测量数据进行预积分，预测两帧图像间无人机的运动轨迹与姿态，同时，双目相机间进行ORB特征提取与匹配，通过双目视差法得特征点的尺度信息，并采用光流法对提取的特征点进行跟踪，获得特征点像素的运动速度，使用
迭代最近点(Iterative closest point，ICP)方法估计无人机的室内定位，再结合IMU的预测，完成初步室内定位。在完成传感器预处理和系统初始化后，采用IMU与视觉紧耦合的方式，对无人机的位置、速度、姿态、陀螺仪与加速度计的漂移以及双目视觉传感器相机与IMU的外参进行优化估计；利用光流跟踪获得的像素点运动速度以及利用IMU预积分进行重投影计算获得的特征点像素速度进行对比，排除由于特征点位于环境中的动态物体上而对系统估计造成的影响，从而实现高精度的室内定位。最后利用词袋模型进行闭环检测，采用室内定位图进行全局优化，实现无人机的重室内定位，消除无人机长时间工作的累积误差，保持定位精度。
[0115] 本发明的优点在于：是一种低成本、易实现的无人机室内实时定位方法，可推广运用在其它需要室内精确定位的
机器人上。克服单一传感器漂移、易受环境干扰等确定，有效提高了机器人室内定位的精度与鲁棒性。
