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

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

【技术实现步骤摘要】
一种微型无人机高速感知和定位方法
本专利技术属于无人机飞行的
，特别是涉及一种微型无人机高速感知和定位方法。
技术介绍
无人机逐渐朝着微小型化、自主化方向发展，国外一些高等研究机构处在这个领域的研究前沿。近年的研究表明，微型四旋翼无人机可以做得非常灵活且功能多样，能够执行复杂的高机动飞行任务。这些成果显示，研究微型高机动无人机是非常有价值的。要实现微型无人机在这些特定场景下的应用，其中一项重要技术就是借助机载传感器实现微型无人机在较复杂环境下的感知和定位功能。随着微型机电系统的发展进步，无人机能借助机载视觉和IMU传感器可以对周围环境进行感知并估计自身状态，其中自载摄像机和IMU传感器可以做的尺寸小、成本低但性能却不差。所以在未知的环境中，微型无身状态可以由相对环境的位置来表示，场景结构可以由从环境中提取的特征空间点进行标记。虽然视觉传感器可以提供丰富的环境信息，利用视觉定位算法，可以实现对微型无人机的位姿估计，这种方法在低动态下可以实现很好的精度，但在无人机高机动情形下，由于高速运动引起图像模糊，会使视觉位姿估计的效果变差。另外本文档来自技高网...

【技术保护点】
1.一种微型无人机高速感知和定位方法，其特征在于，具体包括以下步骤：/n步骤一、设定系统坐标系，构建以微型无人机惯导为移动中心的系统；/n步骤二、摄像机和IMU外参标定，在视觉和惯性融合中，标定的即为摄像机和IMU的时间延迟d和空间相对位姿变换T

【技术特征摘要】
1.一种微型无人机高速感知和定位方法，其特征在于，具体包括以下步骤：
步骤一、设定系统坐标系，构建以微型无人机惯导为移动中心的系统；
步骤二、摄像机和IMU外参标定，在视觉和惯性融合中，标定的即为摄像机和IMU的时间延迟d和空间相对位姿变换TCB，还包括：重力加速度g，以及加速度计的零偏ba和陀螺仪的零偏bg；
步骤三、基于EKF对系统的状态进行评估，假设传感器平台在x-y平面内运动运动方程为xs＝-50000+Δx，ys＝4t+Δy；其中t为时间，Δx，Δy是相互独立的、零均值高斯白噪声，其方差分别为rx＝1和ry＝1，且与过程噪声和量测噪声相互独立；利用EKF状态估计方法，给出目标位置与速度的真实轨迹和估计轨迹；
步骤四、基于多状态约束卡尔曼滤波的视觉惯性里程计；
步骤五、基于移动中心的EKF视觉惯性紧耦合方法；
步骤六、移动中心视觉惯性紧耦合方法的可观性分析；
步骤七、移动中心EKF视觉惯性算法前端的改进。


2.根据权利要求1所述的一种微型无人机高速感知和定位方法，其特征在于，所述步骤一具体包括以下步骤：将微型无人机机体坐标系和IMU坐标系固联在一起，设定移动中心坐标系；陀螺仪，加速度计测量值都在此坐标系下获得，摄像机的图像是在视觉参考坐标系C下获得，将世界坐标系定义为位于水平面X-Y轴，Z轴和重力方向对齐的右手系；
在微型无人机平台固定好IMU和单目摄像机后，构造一个移动的局部参照系而不是固定的全局参照系的视觉惯性导航系统，将IMU上的参考帧I设置为直接用于导航的局部参照系，表示为R，并将第一个局部参考帧R0作为固定的全局参照系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中的位置速度和姿态四元数加速度计的零偏ba、陀螺仪的零偏bg以及视觉比例因子λ；标定的外参状态是从IMU帧旋转到摄像机帧的以及摄像机中心在IMU帧中的位置；所构建的移动中心视觉惯性导航系统的状态向量由两部分组成：第一个是起始帧R0运动信息的全局状态，第二个表示从局部参考帧到当前IMU帧运动的IMU状态；那么在时间[tk，tk+1]里，局部参照系Rk的状态表示为：



公式(1)中，k表示图像的时间序列，t表示每两幅连续图像之间的IMU时间步长，I和C分别表示IMU帧和摄像机帧，R是在每一图像时间步长上用相应的IMU帧的移动中心参考系；表示随机变量x的估计，而是所述估计的误差；In和I0分别是n×n单位阵和零矩阵；是4×1的单位四元数，描述了从世界坐标系到Rk坐标系的旋转，PW是世界坐标系在Rk中的位置，和PIt表示从Rk到当前IMU帧It的相对旋转和平移，Vit是相对速度，bgt和bat分别表示IMU的陀螺仪和加速度偏置；世界参考系下重力矢量g也包括在了状态向量中；
相关状态误差表示为：



四元数误差定义为这里表示四元数乘法，是关于3自由度误差角度δθ的误差四元数，[]×是反对称矩阵算子；
在时间序列k中，对应的IMU帧Ik成为估计的参考帧Rk时，状态向量中最后n个移动中心参考帧之间相对位姿的滑动窗口表示为：



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



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



公式(5)中，和是IMU偏置的零均值高斯白噪声，ω和α分别是It中的角速度和线加速度，对于ω＝[ωx，ωy，ωz]，有表达式：



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



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



步骤(5-2)建立逆深度测量模型
设定一个特征空间点Lj是从nj的一组移动中心帧Rj观测到的，那么nj对应的摄像机帧Cj中Lj的测量量可直接由xyz坐标下的透视投影模型给出；









公式(18)中，是图像噪声，表示Lj在摄像机帧Cj中的位置；的逆深度形式为：



公式(21)中，是Lj在Cj的第一个摄像机帧中的位置，e是方向向量，其中φ和Ψ是用C1表示的俯仰角和方位角，...

【专利技术属性】
技术研发人员：郑宇，陈名熙，金滢，毕景成，赵雪冬，
申请(专利权)人：南京非空航空科技有限公司，
类型：发明
国别省市：江苏;32