一种基于直接法与惯导融合的飞行器位姿估计方法技术

本发明专利技术公开了一种基于直接法与惯导融合的飞行器位姿估计方法，该方法通过基于直接法的视觉测量，优化飞行器六自由度位姿使重投影光度误差最小，与惯性导航元器件的测量融合获得尺度，在无GPS信号等外界辅助导航情况下，能实现飞行器的长期准确定位，且鲁棒性好。

A method of vehicle pose estimation based on direct method and INS fusion

The invention discloses a method for estimating the position and attitude of an aircraft based on direct method and inertial navigation. The method optimizes the six degree of freedom position and pose of the aircraft to minimize the error of the re projection luminosity by using the direct method of visual measurement, and obtains the scale with the measurement fusion of the inertial navigation components, and the external navigation without the GPS signal. It can achieve long term accurate positioning and good robustness.

【技术实现步骤摘要】
一种基于直接法与惯导融合的飞行器位姿估计方法
本专利技术涉及飞行器定位领域，具体涉及一种直接法与惯导融合的飞行器位姿估计方法。
技术介绍
自主定位是机器人自主导航系统的核心组成部分，在实现了自主定位的基础上，机器人可以完成更多功能，比如障碍物规避、路径规划、自主导航等。而对于无人飞行器(UAV,UnmannedAerialVehicle)的飞行控制则需要6自由度的姿态估计。当无人飞行器在室外飞行时，利用全球定位系统(GPS,GlobalPositioningSystem)，在GPS卫星充足的情况下其精度可达1cm。但是，当存在建筑物遮挡或者飞行器处于室内飞行时，GPS可能无法精确定位，甚至无法使用。另一种方法是利用惯性测量单元(IMU,InertialMeasurementUnit)，对其获得的飞行器线加速度进行二次积分，得到飞行器在三维空间中的位置估计。但该方法存在非常大的累积误差，并且当飞行器处于匀速漂移或近似匀速运动时，惯性测量单元可能无法精确测量出飞行器漂移的加速度，这将给飞行器自主定位带来极大的误差。以上这些不利因素迫使我们寻找一种灵巧、轻便、实时、准确的方法，来实现无人飞本文档来自技高网...

【技术保护点】
一种基于直接法与惯导融合的飞行器位姿估计方法，其特征在于，包括如下步骤：(1)通过加速度计和陀螺仪分别测量飞行器实时的加速度和角速度信息，通过磁力计确定绝对偏航角，从而获得飞行器实时的姿态信息，所述的实时姿态信息包括飞行器的俯仰角、横滚角和偏航角；(2)利用视觉传感器，采用直接法来获得尺度缺失的视觉传感器运动位姿估计，该步骤中获得的运动位姿包括：视觉传感器相对于参考坐标系的旋转和视觉传感器在参考坐标系中缺乏尺度的平移。(3)建立运动方程，构造扩展卡尔曼滤波，利用加速度输出和角速度信息，进行扩展卡尔曼滤波器预测，结合视觉直接法输出的尺度缺失的视觉传感器运动位姿估计作为量测值，进行更新，获得飞行器...

【技术特征摘要】
1.一种基于直接法与惯导融合的飞行器位姿估计方法，其特征在于，包括如下步骤：(1)通过加速度计和陀螺仪分别测量飞行器实时的加速度和角速度信息，通过磁力计确定绝对偏航角，从而获得飞行器实时的姿态信息，所述的实时姿态信息包括飞行器的俯仰角、横滚角和偏航角；(2)利用视觉传感器，采用直接法来获得尺度缺失的视觉传感器运动位姿估计，该步骤中获得的运动位姿包括：视觉传感器相对于参考坐标系的旋转和视觉传感器在参考坐标系中缺乏尺度的平移。(3)建立运动方程，构造扩展卡尔曼滤波，利用加速度输出和角速度信息，进行扩展卡尔曼滤波器预测，结合视觉直接法输出的尺度缺失的视觉传感器运动位姿估计作为量测值，进行更新，获得飞行器位姿估计。2.根据权利要求1所述的一种基于直接法与惯导融合的飞行器定位方法，其特征在于，所述步骤(2)中采用直接法获得视觉传感器尺度缺失的运动位姿估计的实施方法为：建立总光度误差Ephoto：其中，表示滑动窗口中图像帧的集合，p表示中的一个特征点坐标，表示第i帧图像中所有特征点坐标的集合，obs(p)表示特征点p所有观测的集合；其中Epj表示单个特征点的重投影光度误差，Np表示特征点所在的区块，wp表示权重，Ii和Ij分别表示第i帧图像和第j帧图像，p’表示第i帧图像中的p点重投影到第j帧图像上的坐标；ai、bi均表示第i帧图像的光度参数，aj、bj均表示第j帧图像的光度参数，ti和tj分别表示第i帧和第j帧图像的曝光时间，||·||γ表示使用Huber核函数；第i帧图像中的p点重投影到第j帧图像上的坐标p’为：其中πc为3D-2D的投影模型，为针孔成像模型，针孔成像模型的内参矩阵为：R表示从第i帧到第j帧的旋转，dp表示p点的逆深度，t表示从第i帧到第j帧的平移关系；内参矩阵K中：fx、fy、cx、cy分别表示相机x轴焦距、y轴焦距、光心在x轴的坐标和光心在y轴的坐标；优化式(3)中摄像头的相对位姿R和t，使总光度误差Ephoto最小，从而恢复摄像头的运动位姿估计R和t。3.根据权利要求2所述的一种基于直接法与惯导融合的飞行器位姿估计方法，其特征在于，所述步骤(3)中建立运动方程，构造扩展卡尔曼滤波，利用加速度输出和角速度信息，进行扩展卡尔曼滤波器预测，具体如下：构造扩展卡尔曼滤波器的状态量x：分别表示为第i时刻IMU坐标系相对于world坐标系(由w表示)的位置、速度、姿态(用四元数表示)，bw、ba分别表示角速度和加速度的偏置，λ表示视觉系统与真实物理系统的尺度，分别表示相机坐标系相对IMU坐标系的位置和姿态，分别world坐标系相对于视觉坐标系(由v表示)的位置和姿态，则实际状态量的运动学方程为：a＝am-ba-naω＝ωm-bω-nω(7)其中，C(·)表示四元数到旋转矩阵的变换关系，Ω(·)表示角速度与四元数的转换关系，nbw表示角速度偏置的漂移噪声，naw表示加速度偏置的漂移噪声，g表示重力加速度，a表示加速度的真实值，am表示惯性测量元件的加速度输出，na表示加速度的测量噪声，ω表示角速度的真实值，ωm表示惯性测量元件的角速度输出，nω表示角速度的测量噪声；由于噪声或者扰动的存在，实际中无法得到状态的真实值，需要对这些状态进行估计，此时认为加速度计和陀螺仪的测量不带噪声或扰动，则改写真实状态方程为：其中，分别表示为第i时刻IMU坐标系相对于world坐标系(由w表示)的位置、速度、姿态(用四元数表示)的估计值，分别表示角速度和加速度的偏置的估计值，表示视觉系统与真实物理系统的尺度的估计值，分别表示相机坐标系相对IMU坐标系的位置和姿态的估计值，分别world坐标系相对于视觉坐标系(由v表示)的位置和姿态的估计值，表示加速度的真实值的估计值，表示角速度的真实值的估计值；在式(6)和式(8)的状态表征中，使用四元数作为姿态的描述，但是四元数并不是姿态的最小维度表征，为了避免参数的超参数化或者冗余导致协方差矩阵的奇异性问题，在表征状态误差时将引入旋转向量θ...

【专利技术属性】
技术研发人员：许超，翁一桢，
申请(专利权)人：浙江大学，
类型：发明
国别省市：浙江,33