(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211348263.2 (22)申请日 2022.10.31 (71)申请人 南京理工大 学 地址 210094 江苏省南京市玄武区孝陵卫 街道孝陵卫 街200号 (72)发明人 蔡晨晓　杨轶　姚娟　邹云　 殷明慧　谢云云　卜京　 (74)专利代理 机构 南京佰腾智 信知识产权代理 事务所(普通 合伙) 32509 专利代理师 任丽丽 (51)Int.Cl. G01C 21/20(2006.01) G01C 21/16(2006.01) G01C 11/04(2006.01) G06T 7/73(2017.01)G06T 7/269(2017.01) G06T 7/246(2017.01) (54)发明名称 拒止环境下基于视觉惯性的四旋翼无人机 自主定位方法 (57)摘要 本发明涉及无人机定位技术领域， 尤其涉及 一种拒止环境下基于视觉惯性的四旋翼无人机 自主定位方法， 首先设计前端数据处理模块， 预 处理双目相机图像信息和IMU测量信息， 对 图像 进行改进的快速特征提取与跟踪， 并将图像帧与 IMU帧对齐， 给出IMU预积分形式； 其次基于滑动 窗口模型构建位姿估计最小化误差函数， 减少位 姿估计的累积误差； 最后利用词袋模 型进行回环 检测， 消除漂移， 构建全局一致的运动轨迹， 实现 完整的无人机自主定位。 本发明方法不依赖于传 统的GPS系统与繁琐的地面站控制， 仅靠重量较 轻的双目相机与IMU， 并通过机载计算机进行实 时处理， 能够有效提高无人机的高机动自主定位 性能， 具备高精度、 高可靠性的优点。 权利要求书3页 说明书12页 附图3页 CN 115406447 A 2022.11.29 CN 115406447 A 1.拒止环境下基于视觉惯性的四旋翼无人机自主定位方法， 其特征在于： 四旋翼无人 机在GPS拒止环境下的复杂场景中进行快速的高机动飞行， 定位方法基于VIO算法框架， 融 合双目相机视觉图像信息和IMU测量信息进 行同步定位， 并使用基于优化的紧耦合方法， 实 现四旋翼无 人机位姿估计； 定位方法具体包括如下步骤： S1、 在前端数据处理部分， 对于安装在四旋翼无人机前部的双目相机所实时采集到的 图像帧序列， 依靠图像处理方法对视觉图像进行特征提取与跟踪， 并剔除其中特征跟踪匹 配失败的异常点； S2、 在前端数据处理部分， 针对IMU测量信息， 将图像观测频率与IMU测量频率进行对 齐， 在两个连续图像帧之间利用IMU预积分技术进行处理， 将图像帧之间的IMU测量数据的 积分形式约束在相对运动状态之外； S3、 在后端优化估计部分， 基于滑动窗口的后端优化方法来减少VIO算法框架中位姿估 计的累积误差， 通过非线性优化， 用紧耦合的方法构建滑动窗口位姿估计最小化 误差函数； S4、 四旋翼无人机由于长时间的运动， 对于返回已访问的区域难免会有累积漂移， 运用 基于描述符的词袋模型进行回环检测， 将视觉图像的特征描述符转换成词袋向量， 计算相 似度， 通过验证， 完成回环检测。 2.根据权利要求1所述的拒止环境下基于视觉惯性的四旋翼无人机自主定位方法， 其 特征在于： 基于优化的方法通过使用双目相机视觉图像信息和IMU测量信息联合最小化残 差来获得最优估计； 而紧耦合则将双目相 机的全局状态和IMU的全局状态融合到一个观测 方程中进行状态估计。 3.根据权利要求1所述的拒止环境下基于视觉惯性的四旋翼无人机自主定位方法， 其 特征在于： 在步骤S1 中， 采用改进的基于FAST角点提取与改进的KLT光流跟踪的图像处理方 法， 并基于PROSAC方法剔除异常点。 4.根据权利要求3所述的拒止环境下基于视觉惯性的四旋翼无人机自主定位方法， 其 特征在于： 改进的基于FAST角点提取方法， 针对候选角点， 定义为中心像素 p， 像素灰度值 Ip， 按顺时针方向计 算像素圆中像素灰度值 Ii， 若由16个像素组成的像素圆中存在12个连续 的像素i满足公式1， 则中心像素 p为角点， 否则筛除像素 p；     （1） 其中，T表示为灰度阈值， circle( p)表示为围绕中心像素 p的像素圆； 将角点与其对应的连续12个像素组成的像素弧之间灰度差绝对值的总和作为特征响 应值函数S， 如公式2所示， 保留特 征响应最大的角点作为唯一角点；  （2） 其中， arc( p)表示为围绕中心像素 p的像素弧。 5.根据权利要求3所述的拒止环境下基于视觉惯性的四旋翼无人机自主定位方法， 其 特征在于： 改进的KLT光流跟踪以连续图像帧中的前一帧 I (x, y)和后一帧 J (x, y)的原 始图像作为L0层， 建立图像金字塔模型， 依次生成L1 ‑L4层， 前一层图像的大小是后一层图 像的4倍， 图像I (x, y)和J (x, y)的金字塔模型记为：权　利　要　求　书 1/3 页 2 CN 115406447 A 2  （3） 其中，Lm = 4， 表示第L层图像I (x, y) ， 表示第L层图像J (x, y)； 在金字塔模型中自顶而下迭代 计算第L层的光流场矢量， 建立以残差光流矢量Ld为变量 的最小化匹配跟踪误差函数， (4) 其中，x表示为图像帧中角点的 x像素坐标值， y表示为图像帧中角点的 y像素坐标值，Le 表示为第 L层的最小化匹配跟踪误差函数，Ld表示为第 L层迭代的角点残差光流矢量，Ldx表 示为第L层中x方向上迭代的角点残差 光流矢量，Ldy表示为第 L层中y方向上迭代的角点残差 光流矢量，Lux表示为第 L层迭代的角点像素二维坐标 x点，Luy表示为第 L层迭代的角点像素 二维坐标 y点，wx = wy = 2， {Lux  ‑ wx, Lux + wx, Luy  – wy, Luy + wy}组成邻域为(2 wx +  1)  × (2wy + 1)的像素窗口，Lg表示为第 L层迭代的角点光流估计值，Lgx表示为第 L层中x 方向上迭代的角点 光流估计值，Lgy表示为第L层中y方向上迭代的角点 光流估计值； 基于PROSAC方法为基于PROSAC渐进一致采样算法， 降序 排列在前后图像帧中特征跟踪 点对的最小欧氏距离， 将其作为评价判断， 选取一定量评价值最高的数据， 从中随机采样4 组数据通过迭代， 寻找到最优的参数模型使得能匹配上的特征点对最多， 记为模型 M， 剔除 与模型M的误差大于阈值的点对。 6.根据权利要求1所述的拒止环境下基于视觉惯性的四旋翼无人机自主定位方法， 其 特征在于： 在步骤S2中， 以连续图像帧 i的时间刻度为基准， 将双目相机图像帧与IMU测量帧 进行帧对齐， 并在对齐帧之间进行IMU预积分， 有如下IMU预积分形式： （5） 其中， 表示为机体坐标系 b中第i帧和第i + 1帧之间位置的预积分量， 表示为 机体坐标系 b中第i帧和第i + 1帧之间速度的预积分量， 表示为机体坐标系 b中第i帧 和第i + 1帧之间旋转的预积分量， t表示为第 i帧和第i + 1帧之间的时间 间隔， 表示为 机体坐标系 b中t时刻下相对于第 i帧的旋转矩阵， 表示为机体坐标系 b中第i帧和t时刻 下帧之间旋转的预积分量， 表示为t时刻下IMU的加速度计原始测 量值， 表示为t时刻 下IMU的陀螺仪原始测量值， 表示为t时刻下IMU的加速度计偏置， 表示为t时刻下IMU 的陀螺仪偏置。权　利　要　求　书 2/3 页 3 CN 115406447 A 3