CN111707261A - 一种微型无人机高速感知和定位方法 - Google Patents

Abstract

本发明公开了一种微型无人机高速感知和定位方法，属于无人机飞行的技术领域。包括：步骤一、设定系统坐标系；步骤二、摄像机和IMU外参标定；步骤三、基于EKF对系统的状态进行评估；步骤四、基于多状态约束卡尔曼滤波的视觉惯性里程计；步骤五、基于移动中心的EKF视觉惯性紧耦合方法；步骤六、移动中心视觉惯性紧耦合方法的可观性分析；步骤七、移动中心EKF视觉惯性算法前端的改进。本发明提高了系统的精度和鲁棒性，弥补仅依靠单目视觉定位方案的局限性，利用了视觉惯性紧耦合的方法进行运动估计，改进了移动中心VIO算法。

Description

一种微型无人机高速感知和定位方法

技术领域

本发明属于无人机飞行的技术领域，特别是涉及一种微型无人机高速感知和 定位方法。

背景技术

无人机逐渐朝着微小型化、自主化方向发展，国外一些高等研究机构处在这 个领域的研究前沿。近年的研究表明，微型四旋翼无人机可以做得非常灵活且功 能多样，能够执行复杂的高机动飞行任务。这些成果显示，研究微型高机动无人 机是非常有价值的。要实现微型无人机在这些特定场景下的应用，其中一项重要 技术就是借助机载传感器实现微型无人机在较复杂环境下的感知和定位功能。

随着微型机电系统的发展进步，无人机能借助机载视觉和IMU传感器可以 对周围环境进行感知并估计自身状态，其中自载摄像机和IMU传感器可以做的 尺寸小、成本低但性能却不差。所以在未知的环境中，微型无身状态可以由相对 环境的位置来表示，场景结构可以由从环境中提取的特征空间点进行标记。虽然 视觉传感器可以提供丰富的环境信息，利用视觉定位算法，可以实现对微型无人 机的位姿估计，这种方法在低动态下可以实现很好的精度，但在无人机高机动情 形下，由于高速运动引起图像模糊，会使视觉位姿估计的效果变差。另外IMU 的陀螺仪和加速度计虽可以实现位姿估计，但由于传感器噪声的存在，其位姿估 计存在积分漂移，长时估计效果很不理想，即使有优化算法可以实现对姿态角估 计漂移的抑制，但位置估计的漂移依然存在。因此，基于视觉和IMU等机载传 感器的多传感器融合算法是实现微型高机动无人机感知和定位技术的关键。

发明内容

本发明为解决上述背景技术中存在的技术问题，提供一种微型无人机高速感 知和定位方法。

本发明通过以下技术方案来实现：一种微型无人机高速感知和定位方法，具 体包括以下步骤：

步骤一、设定系统坐标系，构建以微型无人机惯导为移动中心的系统；

步骤二、摄像机和IMU外参标定，在视觉和惯性融合中，标定的即为摄像 机和IMU的时间延迟d和空间相对位姿变换T_CB，还包括：重力加速度g，以及加 速度计的零偏b_a和陀螺仪的零偏和b_g；

步骤三、基于EKF对系统的状态进行评估，假设传感器平台在x-y平面内 运动运动方程为x_s＝-50000+Δx，y_s＝4t+Δy；其中t为时间，Δx，Δy是相互独 立的、零均值高斯白噪声，其方差分别为r_x＝1和r_y＝1，且与过程噪声和量测噪声 相互独立；利用EKF状态估计方法，给出目标位置与速度的真实轨迹和估计轨 迹；

步骤四、基于多状态约束卡尔曼滤波的视觉惯性里程计；

步骤五、基于移动中心的EKF视觉惯性紧耦合方法；

步骤六、移动中心视觉惯性紧耦合方法的可观性分析；

步骤七、移动中心EKF视觉惯性算法前端的改进。

在进一步的实施例中，所述步骤一具体包括以下步骤：将微型无人机机体坐 标系和IMU坐标系固联在一起，设定移动中心坐标系；陀螺仪，加速度计测量 值都在此坐标系下获得，摄像机的图像是在视觉参考坐标系C下获得，将世界坐 标系定义为位于水平面X-Y轴，Z轴和重力方向对齐的右手系；

在微型无人机平台固定好IMU和单目摄像机后，构造一个移动的局部参照 系而不是固定的全局参照系的视觉惯性导航系统，将IMU上的参考帧I设置为直 接用于导航的局部参照系，表示为R，并将第一个局部参考帧R₀作为固定的全局 参照系W，与以世界为中心的标准全局视觉惯性导航系统相比，所构建的以微型 无人机惯导为移动中心的系统在整个导航过程中，R会在连续IMU帧之间转换。

在进一步的实施例中，所述步骤四具体包括以下步骤：

(4-1)初始化MSCKF中IMU测量协方差与预测协方差；

(4-2)导入测量数据与参考数据；

(4-3)初始化MSCKF：MSCKF状态中只有IMU相关的状态，没有摄像机 相关的状态；MSCKF中不将特征点作为状态，但也会记录跟踪的特征点，初始 化时认为第一帧所有特征点都被跟踪上；

(4-4)状态与协方差预测更新，部分循环进行；

(4-5)状态增广，在MSCKF状态中增广摄像机状态；

(4-6)遍历当前帧所有特征点，对特征跟踪部分更新：遍历当前帧所有特 征点，判断是否属于特征跟踪部分；如果该特征点在视野范围内：将特征点在摄 像机坐标系下的齐次坐标添加到特征跟踪部分中对应特征的观测中；

如果待优化摄像机状态超过最小跟踪长度，将其添加到列表中用于优化；

(4-7)MSCKF测量更新：遍历所有用于优化的特征点，构造观测模型，更 新MSCKF状态；

(4-8)历史状态更新：从MSCKF状态中更新IMU的历史状态，通过摄像 机的状态更新对应时刻IMU的位姿状态；

(4-9)剔除MSCKF中需要被删除的状态和对应的协方差矩阵块；

(4-10)MSCKF中的滑动窗口的优化：定义所述滑动窗口为MSCKF维护 一个位姿的先进先出序列，按照时间顺序排列；

(4-11)MSCKF算法估计效果：对多状态约束卡尔曼滤波视觉惯性里程计 算法的轨迹精度和估计误差进行测试。

在进一步的实施例中，所述步骤五具体包括以下步骤：

步骤(5-1)状态向量和状态传递解析

滤波器的状态包括：IMU在世界帧W中的位置

速度

和姿态四元数

加速度计的零偏b_a、陀螺仪的零偏b_g以及视觉比例因子λ；标定的外参状态是从 IMU帧旋转到摄像机帧的

以及摄像机中心在IMU帧

中的位置；所构建的 移动中心视觉惯性导航系统的状态向量由两部分组成：第一个是起始帧R₀运动信 息的全局状态，第二个表示从局部参考帧到当前IMU帧运动的IMU状态；那么 在时间[t_k，t_k+1]里，局部参照系R_k的状态表示为：

公式(1)中，k表示图像的时间序列，t表示每两幅连续图像之间的IMU时间 步长，I和C分别表示IMU帧和摄像机帧，R是在每一图像时间步长上用相应的 IMU帧的移动中心参考系；

表示随机变量x的估计，而

是所述估计的 误差；I_n和I₀分别是n×n单位阵和零矩阵；

是4×1的单位四元数，描述了从 世界坐标系到R_k坐标系的旋转，P_W是世界坐标系在R_k中的位置，

和P_It表示从R_k到当前IMU帧I_t的相对旋转和平移，V_it是相对速度，b_gt和b_at分别表示IMU 的陀螺仪和加速度偏置；世界参考系下重力矢量g也包括在了状态向量中；

相关状态误差表示为：

四元数误差定义为

这里

表 示四元数乘法，

是关于3自由度误差角度δθ的误差四元数，[]_×是反对称矩阵 算子；

在时间序列k中，对应的IMU帧I_k成为估计的参考帧R_k时，状态向量中最后 n个移动中心参考帧之间相对位姿的滑动窗口表示为：

公式(3)中，q和p代表从R_i-1到R_i的相对旋转和平移；为了使状态向量在一段 时间内保持恒定大小，以滑动窗口的方式对其进行管理，即当窗口中包含新的相 对姿态时，将最旧的状态向量边缘化；

相应的误差状态由公式(4)给出：

在时间间隔[t_k,t_k+1]内全局坐标系相对于局部坐标系是静态的；

假设尺度和视觉参考系仅在空间上漂移，当使用基于关键帧的视觉算法来计 算摄像机姿态时，只有在创建关键帧时，根据视觉算法的性能，才能扩张视觉尺 度λ和视觉帧漂移状态的协方差；Ω(ω)是ω的四元数乘法矩阵，对于IMU的状态， 这里引入一个局部参数化动力学模型：

公式(5)中，

和

是IMU偏置的零均值高斯 白噪声，ω和α分别是I_t中的角速度和线加速度，对于ω＝[ω_x，ω_y，ω_z]，有表达式：

通常IMU提供陀螺仪和加速度计测量值，ω_m和a_m，在IMU参考系中表示 为：

对当前状态估计进行线性化得到连续时间IMU的状态传递量：

步骤(5-2)建立逆深度测量模型

设定一个特征空间点L_j是从n_j的一组移动中心帧R_j观测到的，那么n_j对应的 摄像机帧C_j中L_j的测量量可直接由xyz坐标下的透视投影模型给出；

公式(18)中，

是图像噪声，

表示L_j在摄像机帧C_j中的位置。

的逆深度形式为：

公式(21)中，

是L_j在C_j的第一个摄像机帧中的位置，e是方向向量，其中φ和 Ψ是用C₁表示的俯仰角和方位角，而ρ是沿e的逆深度；C到IMU的外参和滑窗 状态w为：

另外如果特征空间点在无穷远处，可以通过预先乘以ρ来正则化逆深度表达 式，以避免遇到可能发生的数值问题：

公式(23)中，方程中保留了摄像机坐标系逆深度基本方程的射影几何，同时 包含两个退化情况：在无穷远处观察地标和在两个摄像机位姿之间具有低视差； 对于上述两种情况，公式(23)都可以用

来近似，因此相应的测量 结果仍然可以提供摄像机方位信息；因此，为VIO引入以下基于逆深度的测量 模型：

令λ＝[φ，ψ，ρ]并在当前状态估计

和λ处线性化逆深度测量模型，残差方程：

公式(26)中，

和

分别是关于状态向量和逆深度向量的雅可比矩阵， 通过雅可比

的每个L_j测量与w中的相对姿态序列相关联；

由于计算

和

需要估计λ，因此首先使用测量值Z_j，i和相对姿态估计值

求解局部BA，对残差r_j，i进行叠加，得到：

假设从不同摄像机姿态获得的测量值是独立的，因此n_j的协方差矩阵是

当

(或

)被用来计算

时，逆深度误差

与

相关，为了找到EKF 更新的有效残差，将公式(27)投影到

的左零空间：

H_λj是具有列满秩和2_nj×3的2_nj-3维的零空间矩阵，协方差矩阵

变成：

步骤(5-3)EKF更新和组合形式

假设在时间步骤k+1内，有m个特征空间点的测量需要处理，可以叠加得到 的

j＝1～m，因此得到表达式：

其维度为

QR分解只能应用于

的非零部分，如：

其中，Q₁和Q₂分别是维度d×6(N-1)和d×(d-6(N-1))的酉矩阵，

是维度 6(N-1)的上三角矩阵；

根据这个定义有：

丢弃仅与测量噪声有关的较低的行数，使用较高的行数作为EKF更新的残 差项：

公式(33)中，

是协方差矩阵

的噪声矢 量；当d远大于6(N-1)时，使用Givens变换来完成在此基础上，标准EKF更新如 下：

在进一步的实施例中，在所述步骤(5-1)中：

使用四元数作为姿态描述，将状态误差

应用于状态变量，表示

和

结合公式(8)将连续时间的移动中心误差状态模型 表示为：

公式(9)中，n是噪声矢量

即IMU输入噪声向量，F是 移动中心误差状态转移矩阵，G是噪声雅可比矩阵；

实际实现EKF的时候需要离散时间传播模型，因此对IMU状态

进行估计：

(1)通过连续时间IMU的状态传递量的积分得到：

(2)测量量

和

可以用IMU预积分分别计算：

其中Δt＝τ_t-τ_k；预积分项Δp和Δv可以用所有输入的IMU测量值递归计 算；因此当前IMU帧中的速度估计表示为

(3)假设偏置的估计值

和

在时间间隔[t_k，t_k+1]中均为常 数，

对于协方差的传播，在时间间隔[τ_t，τ_t+1]内，采用前向欧拉法计算得到离散 时间误差状态转移矩阵Φ(τ_t+1，τ_t)：

公式(13)中，δτ＝τ_t+1-τ_t，使得协方差传递从时间k中的P_k开始：

公式(14)中，

表示连续时间输入噪声协方 差矩阵；状态

在滑动窗口中的相对姿态是静态的，即

相应的增广协 方差矩阵P_k可以根据移动中心状态和滑动窗口状态进行划分：

时间步长τ+1的传递协方差为：

可使用协方差递归方程计算，并在初始条件Φ_k，k＝I₂₄下计算其组合 误差状态转移矩阵：

在进一步的实施例中，所述步骤(5-3)中，在EKF更新之后对状态进行增 广，其中更新的相对位姿估计

会被附加到当前滑动窗口状 态

的末尾；因此，协方差的矩阵更新为：

每次更新完成时都会变更估计的参考系；

此时，IMU帧I_k+1会被设置为局部参考帧，即用R_k+1来替换前一帧R_k，状态 向量表示为：

公式(36)中，q₀＝[0，0，0，1]^T，这里IMU状态下的相对位姿被重置为原点， 求雅可比矩阵来计算相应的协方差：

其中V_k+1是关于移动中心状态的雅可比。

在进一步的实施例中，所述步骤六具体包括以下步骤：

在时间步骤k处的状态向量包括单个空间路标点L：

其中，

是相对于当前局部参考系R_k的空间点位置，使用公式(18)或逆深 度模型公式(24)，可观测性矩阵计算由公式(35)计算得到：

这里Ψ_l，k是从时间k到l的状态转移矩阵，H_l是与时间l处的观测值相对应的 测量雅可比，每一行在

和

i＝k，...，l，...k+m进行进行评估；M的零空间 描述了状态空间的方向，且没有由测量提供信息，即不可观测状态子空间；

由于移动中心EKF包括三个步骤：传播、更新和组合，并且组合步骤改变 了局部参考系，R-VIO线性化系统具有一个常数不可观测子空间，即独立于线性 化点。

在进一步的实施例中，均方根误差在形式上表示为估计值与数据集真值偏差 的平方和与估计次数比值的平方根，具体计算方式如下：

公式(41)中，m表示实验过程中的位姿估计的次数，xⁱ是第i次摄像机的估 计位姿，

表示由数据集提供的位姿的真实值。

在进一步的实施例中，所述步骤七具体包括以下步骤：

(1)在关键帧上找9个关键点，分别在四个角落，上下左右，以及图像中 心，让上述关键点均匀分布，然后把这些关键点投影到当前帧上，只要投影上任 何一个，就认为他们共视；记录下对应关键帧和当前帧的距离，把共视关键帧的 距离从近到远排序，取最近的几个；根据共视帧之间地图点到光心连线夹角最小 的原则，依次把这些关键帧上面特征点对应的地图点都投影到当前帧上，并把夹 角最小的关键帧作为参考帧；

(2)在具体算法程序上采用网格法：在当前帧上画一个虚拟的网格，遍历 每个网格中特征对应的每一个地图点，每个网格只挑选一个最好的地图点，对挑 选总数进行限制，这意味着并非所有地图点都会用于后面的位姿和空间结构优 化，从而减小了计算开销。

本发明的有益效果：提高了系统的精度和鲁棒性，弥补仅依靠单目视觉定位 方案的局限性，利用了视觉惯性紧耦合的方法进行运动估计，改进了移动中心 VIO算法。首先，对摄像机和IMU的外参进行了离线标定，对多状态约束卡尔 曼滤波VIO算法性能进行测试，在其算法基础上引出移动中心EKF视觉惯性里 程计，对移动中心VIO的可观性进行分析，通过数据集对比试验验证了其相对 MSCKF的优越性。对R-VIO视觉前端进行改进，在飞行数据集上的对比证明改 进后的算法定位精度更高。

附图说明

图1为不同传感器的能力和局限性图。

图2为EKF位置和速度的估计图。

图3为EKF预测和更新位置速度的协方差图。

图4为视觉IMU导航系统坐标系图。

图5为MSCKF中滑窗优化和IMU与真值轨迹对比图。

图6为算法旋转误差和平移误差图。

图7为MSCKF和IMU与真值轨迹的对比图。

图8为考虑尺度影响视觉惯性方法与SVO的估计轨迹的对比图。

图9为三个方向上的位置误差对比图。

图10为随着时间推移VIO算法鲁棒性测试中三个方向上的位置误差对比 图。

图11为与数据集真值的三维轨迹对比图。

具体实施方式

下面结合具体实施例和说明书附图对本发明做进一步的描述。

对于微型无人机的视觉导航方案，单目方法本质的不足是因为缺乏度量尺 度，因此需要利用其他传感器来获取绝对尺度。不过激光测距仪太重，红外传感 器对太阳光太敏感，而超声波传感器在其测距范围内缺乏可靠的测距准确性，双 目方法会失去对离立体基线很远场景的测距能力，气压传感方法在室内不可靠 (关门、启动空调等时压力会突然变化)，所以目前使用机载摄像机和IMU融 合是最合适的导航定位方法。图1体现了不同传感器的能力和局限性。

基于EKF的松耦合方法是独立计算摄像机和IMU状态的估计算法，无法全 面利用好视觉和惯导的数据，因而定位精度不够好。

本发明采用基于EKF的紧耦合方法来实现视觉和惯性传感器的融合，并以 移动中心参考系的形式来构建融合框架。一般EKF紧耦合方式会将特征保持在 状态向量中，随着观测增多，不可避免地会导致计算量不断增加，本发明利用 MSCKF的滑动窗口形式处理数百个特征，但只在状态向量中保留少量的相对姿 态，从而降低计算成本。与之前的EKF紧耦合方法相比，本发明采用的解决方 案能够充分利用图像和IMU信息，又在计算复杂度和估计准确度上做了权衡。

一种微型无人机高速感知和定位方法，具体包括以下步骤：

步骤一、设定系统坐标系，构建以微型无人机惯导为移动中心的系统；

步骤二、为了在传感器融合过程中获得最佳的性能，不同的传感器之间必须 在时间和空间上进行对准。所以在多传感器融合之前首先要解决的问题就是多传 感器标定，标定的目的是获得传感器之间的时间和空间参数。摄像机和IMU外 参标定，在视觉和惯性融合中，标定的即为摄像机和IMU的时间延迟d和空间相 对位姿变换T_CB，还包括：重力加速度g，以及以及加速度计的零偏b_a和陀螺仪的 零偏和b_g，(零偏，输入信号为零的时候，放大器输出不为零的幅度)；

步骤三、为了验证EKF方法的状态估计理论，基于EKF对系统的状态进行 评估，假设传感器平台在x-y平面内运动运动方程为x_s＝-50000+Δx，y_s＝4t+Δy； 其中t为时间，Δx，Δy是相互独立的、零均值高斯白噪声，其方差分别为r_x＝1 和r_y＝1，且与过程噪声和量测噪声相互独立；利用EKF状态估计方法，给出目 标位置与速度的真实轨迹和估计轨迹，如图2；对滤波器的估计性能进行分析， 给出位置与速度方差曲线如图3；

实验结果分析：从图2可以看出，在跟踪初期，真实轨迹和估计轨迹绝对误 差较大，但随着跟踪步数的不断增加，扩展卡尔曼滤波器基本上可以实现跟踪要 求。从图3可以看出，位置和速度的更新、预测协方差误差有一定的差距，但是 能控制在一定范围波动。不过同时可以看到随着跟踪步数的增加，误差协方差均 有继续扩大的现象出现，这一点会对系统的稳定性和精度造成影响。在实验中， 改变产生噪声的随机序列，均能得到比较满意的跟踪结果。可见，扩展卡尔曼滤 波可以适用带高斯噪声的系统。

步骤四、基于多状态约束卡尔曼滤波的视觉惯性里程计；

步骤五、基于移动中心的EKF视觉惯性紧耦合方法；

步骤六、移动中心视觉惯性紧耦合方法的可观性分析；

步骤七、移动中心EKF视觉惯性算法前端的改进。

其中，所述步骤一具体包括以下步骤：如图4所示，因为本发明是为了讨论 微型无人机的状态估计问题，故直接将机体坐标系和IMU坐标系固联在一起， 设定移动中心坐标系。陀螺仪，加速度计测量值都在此坐标系下获得。摄像机的 图像是在视觉参考坐标系C下获得的。利用视觉方法和IMU估计得到的偏航角都 是相对意义下的偏航角，暂时不考虑使用磁力计估计绝对偏航角的问题。所以这 里考虑到算法初始化的便利，将世界坐标系定义为位于水平面X-Y轴，Z轴和重 力方向对齐的右手系。

在微型无人机平台固定好IMU和单目摄像机后。构造一个移动的局部参照 系而不是固定的全局参照系的视觉惯性导航系统。将IMU上的参考帧I设置为直 接用于导航的局部参照系，表示为R，并将第一个局部参考帧R₀作为固定的全局 参照系W，与以世界为中心的标准全局视觉惯性导航系统相比，所构建的以微型 无人机惯导为移动中心的系统在整个导航过程中，R会在连续IMU帧之间转换。

所述步骤二具体的，摄像机和IMU的标定可以看做状态估计的逆过程，在 通过标定板获得较为精确的状态估计后反过来逆推传感器之间的时间和空间参 数。在标定过程中待估计的状态变量包括[T_CB,d,g,b_g,b_a]，摄像机和IMU之间 的空间参数T_CB，时间参数d，重力加速度g，以及加速度计和陀螺仪的零偏b_a和 b_g。考虑到IMU的零偏对标定结果有很大的影响，并且传感器测得到重力加速 度和查表所得有所区别，所以在标定过程中加入了对IMU的零偏和重力向量的 估计。

本发明所使用的传感器包括单目摄像机和IMU。单目摄像机为mvBluefox2 全帧灰度摄像机，图像采集频率为30Hz，图像分辨率752*480。IMU数据由微 型无人机MAVROS读取Pixhawk获得，提供原始的加速度计和陀螺仪数据，数 据采集速率设置为120Hz。

标定过程如下：首先利用PTAM的摄像机标定工具包标定摄像机的内参， 并且查找IMU的技术手册获取加速度计和陀螺仪的噪声密度。然后对摄像机-IMU的外参进行标定，标定过程中对IMU的三个坐标轴进行充分地激励，并且 尽可能使标定板在摄像机中成像清晰。在摄像机和IMU的标定试验中采用的标 定板为Aprilgrid，不同于传统的棋盘格，它的每一个格子的图样都不相同，即使 仅采集到部分图像或者图像模糊时，也可以通过图像处理算法实现精确的定位。

摄像机-IMU的外参标定结果如表1所示，从以下几个方面评判标定结果是 否理想：相同条件下重复多次试验，标定结果在一定的范围内保持稳定；测量摄 像机坐标系中心和IMU坐标系中心的位移值，与标定结果比较；根据重投影误 差结果来判断，重投影误差越小，测量模型越准确，标定模型参数误差也越小， 即标定结果越接近真实值。论文标定的重投影误差的均值为0.2735802个像素， 从以上几条评判标准来看，标定结果较为理想。

表1 摄像机-IMU外参标定

所述步骤四具体包括以下步骤：

(4-1)初始化MSCKF中IMU测量协方差与预测协方差；

(4-2)导入测量数据与参考数据；

(4-3)初始化MSCKF：MSCKF状态中只有IMU相关的状态，没有摄像机 相关的状态；MSCKF中不将特征点作为状态，但也会记录跟踪的特征点，初始 化时认为第一帧所有特征点都被跟踪上；

(4-4)状态与协方差预测更新，部分循环进行；

(4-5)状态增广，在MSCKF状态中增广摄像机状态；

(4-6)遍历当前帧所有特征点，对特征跟踪部分更新：遍历当前帧所有特 征点，判断是否属于特征跟踪部分；如果该特征点在视野范围内：将特征点在摄 像机坐标系下的齐次坐标添加到特征跟踪部分中对应特征的观测中；

如果待优化摄像机状态超过最小跟踪长度，将其添加到列表中用于优化；

(4-7)MSCKF测量更新：遍历所有用于优化的特征点，构造观测模型，更 新MSCKF状态；

(4-8)历史状态更新：从MSCKF状态中更新IMU的历史状态，通过摄像 机的状态更新对应时刻IMU的位姿状态；

(4-9)剔除MSCKF中需要被删除的状态和对应的协方差矩阵块；

(4-10)MSCKF中的滑动窗口的优化：定义所述滑动窗口为MSCKF维护 一个位姿的先进先出序列，按照时间顺序排列；根据参考值获得初始化状态的旋 转和位置，通过IMU的角速度和速度测量值，更新第一个滑动窗口中的所有状 态。如果是第一帧，则用前滑动窗口大小个的IMU的预测状态初始化第一个滑 动窗口，否则预测最近的一帧状态添加到滑动窗口中。用IMU测量数据对窗口 中的状态进行预测更新。

实验结果分析：如图5所示，一个特征点在滑动窗口的几个位姿都被观察到 的话，就会在这几个位姿间建立约束，从而进行卡尔曼的更新。多帧图像保存在 一个按时间排序的滑动窗口先进先出序列中，跟踪多个特征点在多帧图像中的坐 标，从而建立各帧图像位姿之间的约束关系。同一时刻的摄像机位姿与IMU位 姿之间也是一个已知的约束，这个约束用在状态增广中。在这两种约束下，卡尔 曼滤波能够获得一个较好的估计。这个方法鲁棒性高，计算复杂度低。

(4-11)MSCKF算法估计效果：对多状态约束卡尔曼滤波视觉惯性里程计 算法的轨迹精度和估计误差进行测试。

对多状态约束卡尔曼滤波视觉惯性里程计算法的轨迹精度和估计误差进行 测试，测试结果如下：

(1)累计平移均方根误差：IMU为0.719718，MSCKF为0.335283。

(2)累计旋转均方根误差：IMU为0.002509，MSCKF为0.009920。

(3)最终平移误差:IMU为2.320214，MSCKF为1.015345。

实验结果分析：如图6和图7，算法估计结果明显由于IMU的测量数据， 这种基于多状态约束的VIO框架的最大创新点在于并未将路标点的位置加入到 状态向量中(因为加进去会导致状态向量一直增加，效率会越来越慢)，而是等 某个路标点不见或者太旧时，先通过Gauss-Newton优化方法计算出该路标点的 3D位置，然后将多个摄像机对这个路标点的观测作为一种约束，整合到EKF更 新中，这样既不损失信息，又不增加状态向量。

一般基于EKF的视觉惯性导航系统会由于EKF线性化引起不可观测性，而 受到估计不一致的影响。因此，在MSCKF这种多状态约束卡尔曼滤波的框架下， 希望通过对算法结构进行改进，优化不一致问题带来的影响，从而提高算法的精 度。近年来，受二维激光SLAM中以移动中心来定位建图方法的启发，在移动 中心的形式下基于滑动窗口滤波器的R-VIO算法能提高EKF一致性。因此，可 以使用以移动中心作为变换参考系的形式，直接以迭代EKF进行更新的视觉惯 性导航系统作为微型无人机感知定位问题的解决方案。

因此，所述步骤五具体包括以下步骤：

步骤(5-1)状态向量和状态传递解析

滤波器的状态包括：IMU在世界帧W中的位置

速度

和姿态四元数

以及加速度计的零偏ba和陀螺仪的零偏b_g；以及视觉比例因子λ；标定的外参状 态是从IMU帧旋转到摄像机帧的

以及摄像机中心在IMU帧

中的位置；所 构建的移动中心视觉惯性导航系统的状态向量由两部分组成：第一个是起始帧R₀运动信息的全局状态，第二个表示从局部参考帧到当前IMU帧运动的IMU状态； 那么在时间[t_k，t_k+1]里，局部参照系R_k的状态表示为：

公式(1)中，k表示图像的时间序列，t表示每两幅连续图像之间的IMU时间 步长，I和C分别表示IMU帧和摄像机帧，R是在每一图像时间步长上用相应的 IMU帧的移动中心参考系；

表示随机变量x的估计，而

是所述估计的 误差；I_n和I₀分别是n×n单位阵和零矩阵；

是4×1的单位四元数，描述了从 世界坐标系到R_k坐标系的旋转，P_W是世界坐标系在R_k中的位置，

和P_It表示从 R_k到当前IMU帧I_t的相对旋转和平移，V_it是相对速度，b_gt和b_at分别表示IMU 的陀螺仪和加速度偏置；世界参考系下重力矢量g也包括在了状态向量中；

相关状态误差表示为：

四元数误差定义为

这里

表 示四元数乘法，

是关于3自由度误差角度δθ的误差四元数，[]_×是反对称矩阵 算子；

在时间序列k中，对应的IMU帧I_k成为估计的参考帧R_k时，状态向量中最后 n个移动中心参考帧之间相对位姿的滑动窗口表示为：

公式(3)中，q和p代表从R_i-1到R_i的相对旋转和平移；为了使状态向量在一段 时间内保持恒定大小，以滑动窗口的方式对其进行管理，即当窗口中包含新的相 对姿态时，将最旧的状态向量边缘化；

相应的误差状态由公式(4)给出：

在时间间隔[t_k，t_k+1]内全局坐标系相对于局部坐标系是静态的；

假设尺度和视觉参考系仅在空间上漂移，当使用基于关键帧的视觉算法来计 算摄像机姿态时，只有在创建关键帧时，根据视觉算法的性能，才能扩张视觉尺 度λ和视觉帧漂移状态的协方差；Ω(ω)是ω的四元数乘法矩阵，对于IMU的状态， 这里引入一个局部参数化动力学模型：

公式(5)中，

和

是IMU偏置的零均值高斯 白噪声，ω和α分别是I_t中的角速度和线加速度，对于ω＝[ω_x，ω_y，ω_z]，有表达式：

通常IMU提供陀螺仪和加速度计测量值，ω_m和a_m，在IMU参考系中表示 为：

对当前状态估计进行线性化得到连续时间IMU的状态传递量：

在上述的状态表示中，使用四元数作为姿态描述。在这种情况下，通常不是 用数值差异来表示误差及其协方差，而是借助误差四元数来表示，因为这可以增 加数值稳定性，处理四元数更为方便。

将状态误差

应用于状态变量，表示

和

结合公式(8)将连续时间的移动中心误差状态模型表示为：

公式(9)中，n是噪声矢量

即IMU输入噪声向量，F是 移动中心误差状态转移矩阵，G是噪声雅可比矩阵；

实际实现EKF的时候需要离散时间传播模型，因此对IMU状态

进行估计：

(1)通过连续时间IMU的状态传递量的积分得到：

(2)测量量

和

可以用IMU预积分分别计算：

其中Δt＝τ_t-τ_k；预积分项Δp和Δv可以用所有输入的IMU测量值递归计 算；因此当前IMU帧中的速度估计表示为

(3)假设偏置的估计值

和

在时间间隔[t_k，t_k+1]中均为常 数，

对于协方差的传播，在时间间隔[τ_t，τ_t+1]内，采用前向欧拉法计算得到离散 时间误差状态转移矩阵Φ(τ_t+1，τ_t)：

公式(13)中，δτ＝τ_t+1-τ_t，使得协方差传递从时间k中的P_k开始：

公式(14)中，

表示连续时间输入噪声协方 差矩阵；状态

在滑动窗口中的相对姿态是静态的，即

相应的增广协 方差矩阵P_k可以根据移动中心状态和滑动窗口状态进行划分：

时间步长τ+1的传递协方差为：

可使用协方差递归方程计算，并在初始条件Φ_k，k＝I₂₄下计算其组合 误差状态转移矩阵：

步骤(5-2)建立逆深度测量模型

设定一个特征空间点L_j是从n_j的一组移动中心帧R_j观测到的，那么n_j对应的 摄像机帧C_j中L_j的测量量可直接由xyz坐标下的透视投影模型给出；

公式(18)中，

是图像噪声，

表示L_j在摄像机帧C_j中的位置。

的逆深度形式为：

公式(21)中，

是L_j在C_j的第一个摄像机帧中的位置，e是方向向量，其中φ和 Ψ是用C₁表示的俯仰角和方位角，而ρ是沿e的逆深度；C到IMU的外参和滑窗 状态w为：

另外如果特征空间点在无穷远处，可以通过预先乘以ρ来正则化逆深度表达 式，以避免遇到可能发生的数值问题：

公式(23)中，方程中保留了摄像机坐标系逆深度基本方程的射影几何，同时 包含两个退化情况：在无穷远处观察地标和在两个摄像机位姿之间具有低视差； 对于上述两种情况，公式(23)都可以用

来近似，因此相应的测量 结果仍然可以提供摄像机方位信息；因此，为VIO引入以下基于逆深度的测量 模型：

令λ＝[φ，ψ，ρ]并在当前状态估计

和λ处线性化逆深度测量模型，残差方程：

公式(26)中，

和

分别是关于状态向量和逆深度向量的雅可比矩阵， 通过雅可比

的每个L_j测量与w中的相对姿态序列相关联；

由于计算

和

需要估计λ，因此首先使用测量值Z_j，i和相对姿态估计值

求解局部BA，对残差r_j，i进行叠加，得到：

假设从不同摄像机姿态获得的测量值是独立的，因此n_j的协方差矩阵是

当

(或

)被用来计算

时，逆深度误差

与

相关，为了找到EKF 更新的有效残差，将公式(27)投影到

的左零空间：

H_λj是具有列满秩和2_nj×3的2_nj-3维的零空间矩阵，协方差矩阵

变成：

步骤(5-3)EKF更新和组合形式

加速度和角速度用于状态转移，而摄像机位姿的测量用于滤波器更新。也就 是说，考虑了在传播阶段使用的传感器的时间漂移。姿态测量可能在其姿态和位 置上发生空间漂移。暂时忽略位置漂移，假设测量输入只在所有三个角方向上漂 移，这意味着

为零，在滤波方程中可以忽略不计。

假设在时间步骤k+1内，有m个特征空间点的测量需要处理，可以叠加得到 的

j＝1～m，因此得到表达式：

其维度为

QR分解只能应用于

的非零部分，如：

其中，Q₁和Q₂分别是维度d×6(N-1)和d×(d-6(N-1))的酉矩阵，

是维度 6(N-1)的上三角矩阵；

根据这个定义有：

丢弃仅与测量噪声有关的较低的行数，使用较高的行数作为EKF更新的残 差项：

公式(33)中，

是协方差矩阵

的噪声矢 量；当d远大于6(N-1)时，使用Givens变换来完成在此基础上，标准EKF更新如 下：

为了利用最精确的相对运动信息进行估计，在EKF更新之后对状态进行增广， 其中更新的相对位姿估计

会被附加到当前滑动窗口状态

的末尾。因此，协方差的矩阵更新为：

每次更新完成时都会变更估计的参考系；

此时，IMU帧I_k+1会被设置为局部参考帧，即用R_k+1来替换前一帧R_k，状态 向量表示为：

公式(36)中，q₀＝[0，0，0，1]^T，这里IMU状态下的相对位姿被重置为原点， 求雅可比矩阵来计算相应的协方差：

其中V_k+1是关于移动中心状态的雅可比。

系统可观性在一致性状态估计的设计中非常重要，它检验了现有测量数据所 提供的信息是否足以不模糊地对状态或参数进行估计。当系统可观时，可观测矩 阵是可逆的，这也与协方差矩阵密切相关。由于这个矩阵描述了测量中可用的信 息，通过研究它的零空间，了解状态空间中估计器应该沿着哪个方向获取信息。

在基于EKF的VIO算法中，过程噪声和观测噪声均假设为高斯噪声，并对 非线性模型进行线性化处理以实现卡尔曼滤波估计。基于EKF的VIO算法通过 计算近似的期望值和协方差来描述状态不确定性。

但是，会出现两个问题：1)由于线性化处理，估计状态值和估计协方差是 近似的，有可能与真实的状态值和协方差不匹配、不一致。2)真实的概率分布 可能不是高斯分布，所以用高斯分布来描述真实状态可能是不充分的、不准确的。 而这些不利因素会影响估计的一致性。特别在微型高机动无人机活动的大范围环 境下，由于非线性误差积累导致的估计偏差增大，以及环境复杂性造成的状态描 述不准确，使上面两个制约因素的影响变得十分明显，最终导致算法的不一致。

而以移动中心构建VIO的方式从公式推导的形式上规避了可观性的问题。 R-VIO算法中借鉴以机器人为中心建图的这个方法，解决了一致性的问题，其中 特征空间点路标的位置以逆深度参数和二维方位向量表示，而不是三维世界坐 标。因此所述步骤六具体包括以下步骤：在时间步骤k处的状态向量包括单个空 间路标点L：

其中，

是相对于当前局部参考系R_k的空间点位置，使用公式(18)或逆深 度模型公式(24)，可观测性矩阵计算由公式(35)计算得到：

这里Ψ_l，k是从时间k到l的状态转移矩阵，H_l是与时间l处的观测值相对应的 测量雅可比，每一行在

和

i＝k，...，l，...k+m进行进行评估；M的零空间 描述了状态空间的方向，且没有由测量提供信息，即不可观测状态子空间；

由于移动中心EKF包括三个步骤：传播、更新和组合，并且组合步骤改变 了局部参考系，R-VIO线性化系统具有一个常数不可观测子空间，即独立于线性 化点。

这不仅保证了系统具有正确的不可观测维度，而且还具有期望的不可观测方 向，从而能够提高估计的一致性。并且以移动中心作为参考坐标系的紧耦合VIO 不存在已被证明是不一致主要原因的可观测失配问题。综上分析，R-VIO会在多 状态约束卡尔曼滤波VIO算法的基础上提高了估计性能。为了直观的表现移动 中心EKF视觉惯性紧耦合框架对可观性进行了修正，从而使算法估计精度有所 提升，本发明在EuRoC数据集上对两者定位精度进行对比，实验测试结果如表 2所示：

表2移动中心对可观性修正后和MSCKF定位精度对比

在表2中，所有误差均采用均方根误差(Root Mean Square Error,RMSE)的形 式来计算，单位为m。均方根误差在形式上表示为估计值与数据集真值偏差的平 方和与估计次数比值的平方根，具体计算方式如下：

公式(41)中，m表示实验过程中的位姿估计的次数，xⁱ是第i次摄像机的估 计位姿，

表示由数据集提供的位姿的真实值。

分别对R-VIO和SVO算法进行评估。实验结果如图8所示，黑色较粗的曲 线是Vicon真值提供的轨迹，红色较浅的是R-VIO估计轨迹，蓝色较深的是SVO 估计的轨迹，SVO直接估计的轨迹和真值轨迹非常相似但是大小差别较大，原 因是纯视觉的运动估计算法无法从环境信息中恢复出环境的真实尺度。

所述步骤七具体包括以下步骤：

(1)在关键帧上找9个关键点，分别在四个角落，上下左右，以及图像中 心，让上述关键点均匀分布，然后把这些关键点投影到当前帧上，只要投影上任 何一个，就认为他们共视；记录下对应关键帧和当前帧的距离，把共视关键帧的 距离从近到远排序，取最近的几个；根据共视帧之间地图点到光心连线夹角最小 的原则，依次把这些关键帧上面特征点对应的地图点都投影到当前帧上，并把夹 角最小的关键帧作为参考帧；

(2)在具体算法程序上采用网格法：在当前帧上画一个虚拟的网格，遍历 每个网格中特征对应的每一个地图点，每个网格只挑选一个最好的地图点，对挑 选总数进行限制，这意味着并非所有地图点都会用于后面的位姿和空间结构优 化，从而减小了计算开销。

为了直观的显示改进后的算法对单目视觉惯性里程计系统定位精度的提升 效果，在EuRoC数据集上，实验测试算法在多个数据集上的定位误差统计如表 3所示：

表3改进前后系统的位姿估计误差对比

表3给出了R-VIO估计的轨迹误差和改进后算法对轨迹估计误差的对比， 误差同样为均方根大小，单位为m，并给出了改进后算法对定位精度的提升效果。 整体来看，由于改进后的系统对于场景变化具有更强的适应性，在大部分数据集 下系统对摄像机位姿的估计精度均有所提升。

改进的移动中心VIO算法准确性测试

通过时间戳对齐，sim3变换，将每一个时刻算法估计的定位结果和真实轨 迹相对应，从而给出算法估计的定位误差。本发明分别将定位算法的估计结果和 数据集的真值进行对比分析，从定位精度上来看，改进的移动中心VIO算法较 VO算法在xyz各个方向上的轨迹平均误差更小。

如图9表示了随着时间推移的位置误差。多次试验后，测试证明算法利用相 对环境中空间点位置和IMU信息融合用于微型无人机的运动估计是可行的。

改进的移动中心VIO算法鲁棒性测试

本发明VO算法受限于单一传感器的先天短板，在困难数据集中无法正常完 成跟踪任务，而本发明的VIO算法可以完成整个状态估计过程，评估结果如图 10所示。在光照条件较暗等更为苛刻的环境下，仅基于视觉传感器的定位方法 往往不再可靠，视觉惯性融合的定位算法能体现其鲁棒性强的优势。

接下来，在较高机动场景下的飞行数据集中进行算法测试，最大速度3m/s， 最高加速度4m/s²。如图11所示，从二维轨迹对比中可以看出，在做大旋转运动 时，本发明VIO算法的估计结果仍然能够较好地跟踪真值轨迹。而在纯视觉定 位的情况下，基于对极线几何约束的本质矩阵E会退化，接近0，这时旋转矩阵R无 法正常求解。同时也无法根据三角化解算出空间点。如果当前帧的特征点与空间 中的三维点匹配变少，则估计结果将变差，而加入IMU运动信息，可以准确地 提供帧之间的运动增量，提高算法快速旋转运动状态下的鲁棒性，适合应用于微 型高机动无人机的感知和定位任务。

总体来讲，利用视觉和惯性器件融合，实现了不同传感器之间的互补，不仅 利用IMU信息解决了单目尺度误差的问题，避免了纯旋转或者视觉失效的情况， 同时反过来还通过视觉有效地弥补了IMU测量的漂移，使系统算法更具有鲁棒 性。

Claims (9)

1.一种微型无人机高速感知和定位方法，其特征在于，具体包括以下步骤：

步骤一、设定系统坐标系，构建以微型无人机惯导为移动中心的系统；

步骤二、摄像机和IMU外参标定，在视觉和惯性融合中，标定的即为摄像机和IMU的时间延迟d和空间相对位姿变换T_CB，还包括：重力加速度g，以及加速度计的零偏b_a和陀螺仪的零偏b_g；

步骤三、基于EKF对系统的状态进行评估，假设传感器平台在x-y平面内运动运动方程为x_s＝-50000+Δx，y_s＝4t+Δy；其中t为时间，Δx，Δy是相互独立的、零均值高斯白噪声，其方差分别为r_x＝1和r_y＝1，且与过程噪声和量测噪声相互独立；利用EKF状态估计方法，给出目标位置与速度的真实轨迹和估计轨迹；

步骤四、基于多状态约束卡尔曼滤波的视觉惯性里程计；

步骤五、基于移动中心的EKF视觉惯性紧耦合方法；

步骤六、移动中心视觉惯性紧耦合方法的可观性分析；

步骤七、移动中心EKF视觉惯性算法前端的改进。

2.根据权利要求1所述的一种微型无人机高速感知和定位方法，其特征在于，所述步骤一具体包括以下步骤：将微型无人机机体坐标系和IMU坐标系固联在一起，设定移动中心坐标系；陀螺仪，加速度计测量值都在此坐标系下获得，摄像机的图像是在视觉参考坐标系C下获得，将世界坐标系定义为位于水平面X-Y轴，Z轴和重力方向对齐的右手系；

在微型无人机平台固定好IMU和单目摄像机后，构造一个移动的局部参照系而不是固定的全局参照系的视觉惯性导航系统，将IMU上的参考帧I设置为直接用于导航的局部参照系，表示为R，并将第一个局部参考帧R₀作为固定的全局参照系W，与以世界为中心的标准全局视觉惯性导航系统相比，所构建的以微型无人机惯导为移动中心的系统在整个导航过程中，R会在连续IMU帧之间转换。

3.根据权利要求1所述的一种微型无人机高速感知和定位方法，其特征在于，所述步骤四具体包括以下步骤：

(4-1)初始化MSCKF中IMU测量协方差与预测协方差；

(4-2)导入测量数据与参考数据；

(4-3)初始化MSCKF：MSCKF状态中只有IMU相关的状态，没有摄像机相关的状态；MSCKF中不将特征点作为状态，但也会记录跟踪的特征点，初始化时认为第一帧所有特征点都被跟踪上；

(4-4)状态与协方差预测更新，部分循环进行；

(4-5)状态增广，在MSCKF状态中增广摄像机状态；

(4-6)遍历当前帧所有特征点，对特征跟踪部分更新：遍历当前帧所有特征点，判断是否属于特征跟踪部分；如果该特征点在视野范围内：将特征点在摄像机坐标系下的齐次坐标添加到特征跟踪部分中对应特征的观测中；

如果待优化摄像机状态超过最小跟踪长度，将其添加到列表中用于优化；

(4-7)MSCKF测量更新：遍历所有用于优化的特征点，构造观测模型，更新MSCKF状态；

(4-8)历史状态更新：从MSCKF状态中更新IMU的历史状态，通过摄像机的状态更新对应时刻IMU的位姿状态；

(4-9)剔除MSCKF中需要被删除的状态和对应的协方差矩阵块；

(4-10)MSCKF中的滑动窗口的优化：定义所述滑动窗口为MSCKF维护一个位姿的先进先出序列，按照时间顺序排列；

(4-11)MSCKF算法估计效果：对多状态约束卡尔曼滤波视觉惯性里程计算法的轨迹精度和估计误差进行测试。

4.根据权利要求1所述的一种微型无人机高速感知和定位方法，其特征在于，所述步骤五具体包括以下步骤：

步骤(5-1)状态向量和状态传递解析

滤波器的状态包括：IMU在世界帧W中的位置

速度

和姿态四元数

加速度计的零偏b_a、陀螺仪的零偏b_g以及视觉比例因子λ；标定的外参状态是从IMU帧旋转到摄像机帧的

以及摄像机中心在IMU帧

中的位置；所构建的移动中心视觉惯性导航系统的状态向量由两部分组成：第一个是起始帧R₀运动信息的全局状态，第二个表示从局部参考帧到当前IMU帧运动的IMU状态；那么在时间[t_k，t_k+1]里，局部参照系R_k的状态表示为：

公式(1)中，k表示图像的时间序列，t表示每两幅连续图像之间的IMU时间步长，I和C分别表示IMU帧和摄像机帧，R是在每一图像时间步长上用相应的IMU帧的移动中心参考系；

表示随机变量x的估计，而

是所述估计的误差；I_n和I₀分别是n×n单位阵和零矩阵；

是4×1的单位四元数，描述了从世界坐标系到R_k坐标系的旋转，P_W是世界坐标系在R_k中的位置，

和P_It表示从R_k到当前IMU帧I_t的相对旋转和平移，V_it是相对速度，b_gt和b_at分别表示IMU的陀螺仪和加速度偏置；世界参考系下重力矢量g也包括在了状态向量中；

相关状态误差表示为：

四元数误差定义为

这里

表示四元数乘法，

是关于3自由度误差角度δθ的误差四元数，[]_×是反对称矩阵算子；

在时间序列k中，对应的IMU帧I_k成为估计的参考帧R_k时，状态向量中最后n个移动中心参考帧之间相对位姿的滑动窗口表示为：

公式(3)中，q和p代表从R_i-1到R_i的相对旋转和平移；为了使状态向量在一段时间内保持恒定大小，以滑动窗口的方式对其进行管理，即当窗口中包含新的相对姿态时，将最旧的状态向量边缘化；

相应的误差状态由公式(4)给出：

在时间间隔[t_k，t_k+1]内全局坐标系相对于局部坐标系是静态的；

假设尺度和视觉参考系仅在空间上漂移，当使用基于关键帧的视觉算法来计算摄像机姿态时，只有在创建关键帧时，根据视觉算法的性能，才能扩张视觉尺度λ和视觉帧漂移状态的协方差；Ω(ω)是ω的四元数乘法矩阵，对于IMU的状态，这里引入一个局部参数化动力学模型：

公式(5)中，

和

是IMU偏置的零均值高斯白噪声，ω和α分别是I_t中的角速度和线加速度，对于ω＝[ω_x，ω_y，ω_z]，有表达式：

通常IMU提供陀螺仪和加速度计测量值，ω_m和a_m，在IMU参考系中表示为：

对当前状态估计进行线性化得到连续时间IMU的状态传递量：

步骤(5-2)建立逆深度测量模型

设定一个特征空间点L_j是从n_j的一组移动中心帧R_j观测到的，那么n_j对应的摄像机帧C_j中L_j的测量量可直接由xyz坐标下的透视投影模型给出；

公式(18)中，

是图像噪声，

表示L_j在摄像机帧C_j中的位置；

的逆深度形式为：

公式(21)中，

是L_j在C_j的第一个摄像机帧中的位置，e是方向向量，其中φ和Ψ是用C₁表示的俯仰角和方位角，而ρ是沿e的逆深度；C到IMU的外参和滑窗状态w为：

另外如果特征空间点在无穷远处，可以通过预先乘以ρ来正则化逆深度表达式，以避免遇到可能发生的数值问题：

公式(23)中，方程中保留了摄像机坐标系逆深度基本方程的射影几何，同时包含两个退化情况：在无穷远处观察地标和在两个摄像机位姿之间具有低视差；对于上述两种情况，公式(23)都可以用

来近似，因此相应的测量结果仍然可以提供摄像机方位信息；因此，为VIO引入以下基于逆深度的测量模型：

令λ＝[φ，ψ，ρ]并在当前状态估计

和λ处线性化逆深度测量模型，残差方程：

公式(26)中，

和

分别是关于状态向量和逆深度向量的雅可比矩阵，通过雅可比

的每个L_j测量与w中的相对姿态序列相关联；

由于计算

和H_λj，i需要估计λ，因此首先使用测量值Z_j，i和相对姿态估计值

求解局部BA，对残差r_j，i进行叠加，得到：

假设从不同摄像机姿态获得的测量值是独立的，因此n_j的协方差矩阵是

当

(或

)被用来计算

时，逆深度误差

与

相关，为了找到EKF更新的有效残差，将公式(27)投影到

的左零空间：

H_λj是具有列满秩和2_nj×3的2_nj-3维的零空间矩阵，协方差矩阵

变成：

步骤(5-3)EKF更新和组合形式

假设在时间步骤k+1内，有m个特征空间点的测量需要处理，可以叠加得到的

因此得到表达式：

其维度为

QR分解只能应用于

的非零部分，如：

其中，Q₁和Q₂分别是维度d×6(N-1)和d×(d-6(N-1))的酉矩阵，

是维度6(N-1)的上三角矩阵；

根据这个定义有：

丢弃仅与测量噪声有关的较低的行数，使用较高的行数作为EKF更新的残差项：

公式(33)中，

是协方差矩阵

的噪声矢量；当d远大于6(N-1)时，使用Givens变换来完成在此基础上，标准EKF更新如下：

5.根据权利要求4所述的一种微型无人机高速感知和定位方法，其特征在于，在所述步骤(5-1)中：

使用四元数作为姿态描述，将状态误差

应用于状态变量，表示

和

结合公式(8)将连续时间的移动中心误差状态模型表示为：

公式(9)中，n是噪声矢量

即IMU输入噪声向量，F是移动中心误差状态转移矩阵，G是噪声雅可比矩阵；

实际实现EKF的时候需要离散时间传播模型，因此对IMU状态

进行估计：

(1)通过连续时间IMU的状态传递量的积分得到：

(2)测量量

和

可以用IMU预积分分别计算：

其中Δt＝τ_t-τ_k；预积分项Δp和Δv可以用所有输入的IMU测量值递归计算；因此当前IMU帧中的速度估计表示为

(3)假设偏置的估计值

和

在时间间隔[t_k，t_k+1]中均为常数，

对于协方差的传播，在时间间隔[τ_t，τ_t+1]内，采用前向欧拉法计算得到离散时间误差状态转移矩阵Φ(τ_t+1，τ_t)：

公式(13)中，δτ＝τ_t+1-τ_t，使得协方差传递从时间k中的P_k开始：

公式(14)中，

表示连续时间输入噪声协方差矩阵；状态

在滑动窗口中的相对姿态是静态的，即

相应的增广协方差矩阵P_k可以根据移动中心状态和滑动窗口状态进行划分：

时间步长τ+1的传递协方差为：

可使用协方差递归方程计算，并在初始条件Φ_k，k＝I₂₄下计算其组合误差状态转移矩阵：

6.根据权利要求4所述的一种微型无人机高速感知和定位方法，其特征在于，所述步骤(5-3)中，在EKF更新之后对状态进行增广，其中更新的相对位姿估计

会被附加到当前滑动窗口状态

的末尾；因此，协方差的矩阵更新为：

每次更新完成时都会变更估计的参考系；

此时，IMU帧I_k+1会被设置为局部参考帧，即用R_k+1来替换前一帧R_k，状态向量表示为：

公式(36)中，q₀＝[0，0，0，1]^T，这里IMU状态下的相对位姿被重置为原点，求雅可比矩阵来计算相应的协方差：

其中V_k+1是关于移动中心状态的雅可比。

7.根据权利要求1所述的一种微型无人机高速感知和定位方法，其特征在于，所述步骤六具体包括以下步骤：

在时间步骤k处的状态向量包括单个空间路标点L：

其中，

是相对于当前局部参考系R_k的空间点位置，使用公式(18)或逆深度模型公式(24)，可观测性矩阵计算由公式(35)计算得到：

这里Ψ_l，k是从时间k到l的状态转移矩阵，H_l是与时间l处的观测值相对应的测量雅可比，每一行在

和

i＝k，...，l，...k+m进行进行评估；M的零空间描述了状态空间的方向，且没有由测量提供信息，即不可观测状态子空间；

由于移动中心EKF包括三个步骤：传播、更新和组合，并且组合步骤改变了局部参考系，R-VIO线性化系统具有一个常数不可观测子空间，即独立于线性化点。

8.根据权利要求7所述的一种微型无人机高速感知和定位方法，其特征在于，均方根误差在形式上表示为估计值与数据集真值偏差的平方和与估计次数比值的平方根，具体计算方式如下：

公式(41)中，m表示实验过程中的位姿估计的次数，xⁱ是第i次摄像机的估计位姿，

表示由数据集提供的位姿的真实值。

9.根据权利要求1所述的一种微型无人机高速感知和定位方法，其特征在于，所述步骤七具体包括以下步骤：

(1)在关键帧上找9个关键点，分别在四个角落，上下左右，以及图像中心，让上述关键点均匀分布，然后把这些关键点投影到当前帧上，只要投影上任何一个，就认为他们共视；记录下对应关键帧和当前帧的距离，把共视关键帧的距离从近到远排序，取最近的几个；根据共视帧之间地图点到光心连线夹角最小的原则，依次把这些关键帧上面特征点对应的地图点都投影到当前帧上，并把夹角最小的关键帧作为参考帧；

(2)在具体算法程序上采用网格法：在当前帧上画一个虚拟的网格，遍历每个网格中特征对应的每一个地图点，每个网格只挑选一个最好的地图点，对挑选总数进行限制，这意味着并非所有地图点都会用于后面的位姿和空间结构优化，从而减小了计算开销。

Images