CN108036785A - 一种基于直接法与惯导融合的飞行器位姿估计方法 - Google Patents

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

Info

Publication number
CN108036785A
CN108036785A CN201711190702.0A CN201711190702A CN108036785A CN 108036785 A CN108036785 A CN 108036785A CN 201711190702 A CN201711190702 A CN 201711190702A CN 108036785 A CN108036785 A CN 108036785A
Authority
CN
China
Prior art keywords
mrow
msub
mover
msubsup
mtd
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201711190702.0A
Other languages
English (en)
Inventor
许超
翁桢
翁一桢
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN201711190702.0A priority Critical patent/CN108036785A/zh
Publication of CN108036785A publication Critical patent/CN108036785A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Medicines Containing Antibodies Or Antigens For Use As Internal Diagnostic Agents (AREA)

Abstract

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

Description

一种基于直接法与惯导融合的飞行器位姿估计方法
技术领域
本发明涉及飞行器定位领域，具体涉及一种直接法与惯导融合的飞行器位姿估计方法。
背景技术
自主定位是机器人自主导航***的核心组成部分，在实现了自主定位的基础上，机器人可以完成更多功能，比如障碍物规避、路径规划、自主导航等。
而对于无人飞行器(UAV,Unmanned Aerial Vehicle)的飞行控制则需要6自由度的姿态估计。当无人飞行器在室外飞行时，利用全球定位***(GPS,Global PositioningSystem)，在GPS卫星充足的情况下其精度可达1cm。但是，当存在建筑物遮挡或者飞行器处于室内飞行时，GPS可能无法精确定位，甚至无法使用。另一种方法是利用惯性测量单元(IMU,Inertial Measurement Unit)，对其获得的飞行器线加速度进行二次积分，得到飞行器在三维空间中的位置估计。但该方法存在非常大的累积误差，并且当飞行器处于匀速漂移或近似匀速运动时，惯性测量单元可能无法精确测量出飞行器漂移的加速度，这将给飞行器自主定位带来极大的误差。以上这些不利因素迫使我们寻找一种灵巧、轻便、实时、准确的方法，来实现无人飞行器的自主定位。
发明内容
本发明的目的在于针对现有技术的不足，提供一种基于直接法与惯导融合的飞行器位姿估计方法，本方法在无GPS信号等外界辅助导航情况下，能实现飞行器的长期准确定位，且鲁棒性好，累积误差更小。
本发明所提供的技术方案为：一种基于直接法与惯导融合的飞行器位姿估计方法，其特征在于，包括如下步骤：
(1)通过加速度计和陀螺仪分别测量飞行器实时的加速度和角速度信息，通过磁力计确定绝对偏航角，从而获得飞行器实时的姿态信息，所述的实时姿态信息包括飞行器的俯仰角、横滚角和偏航角；
(2)利用视觉传感器，采用直接法来获得尺度缺失的视觉传感器运动位姿估计，该步骤中获得的运动位姿包括：视觉传感器相对于参考坐标系的旋转和视觉传感器在参考坐标系中缺乏尺度的平移；
(3)建立运动方程，构造扩展卡尔曼滤波，利用加速度输出和角速度信息，进行扩展卡尔曼滤波器预测，结合视觉直接法输出的尺度缺失的视觉传感器运动位姿估计作为量测值，进行更新，获得飞行器位姿估计。
根据权利要求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)中建立运动方程，构造扩展卡尔曼滤波，利用加速度输出和角速度信息，进行扩展卡尔曼滤波器预测，具体如下：
构造扩展卡尔曼滤波器的状态量x：
分别表示为第i时刻IMU坐标系相对于world坐标系(由w表示)的位置、速度、姿态(用四元数表示)，bω、ba分别表示角速度和加速度的偏置，λ表示视觉***与真实物理***的尺度，分别表示相机坐标系相对IMU坐标系的位置和姿态，分别world坐标系相对于视觉坐标系(由v表示)的位置和姿态，则实际状态量的运动学方程为：
a＝am-ba-na ω＝ωm-bω-nω (7)
其中，C(·)表示四元数到旋转矩阵的变换关系，Ω(·)表示角速度与四元数的转换关系，nbw表示角速度偏置的漂移噪声，naw表示加速度偏置的漂移噪声，g表示重力加速度，a表示加速度的真实值，am表示惯性测量元件的加速度输出，na表示加速度的测量噪声，w表示角速度的真实值，ωm表示惯性测量元件的角速度输出，nω表示角速度的测量噪声；
由于噪声或者扰动的存在，实际中无法得到状态的真实值，需要对这些状态进行估计，此时认为加速度计和陀螺仪的测量不带噪声或扰动，则改写真实状态方程为：
其中，分别表示为第i时刻IMU坐标系相对于world坐标系(由w表示)的位置、速度、姿态(用四元数表示)的估计值，分别表示角速度和加速度的偏置的估计值，表示视觉***与真实物理***的尺度的估计值，分别表示相机坐标系相对IMU坐标系的位置和姿态的估计值，分别world坐标系相对于视觉坐标系(由v表示)的位置和姿态的估计值，表示加速度的真实值的估计值，表示角速度的真实值的估计值；
在式(6)和式(8)的状态表征中，使用四元数作为姿态的描述，但是四元数并不是姿态的最小维度表征，为了避免参数的超参数化或者冗余导致协方差矩阵的奇异性问题，在表征状态误差时将引入旋转向量θ作为姿态的最小维度描述，因此误差状态量为：
分别表示为第i时刻IMU坐标系相对于world坐标系(由w表示)的位置误差、速度误差、姿态误差(用四元数表示)，Δbω、Δba分别表示角速度和加速度的偏置误差，Δλ表示视觉***与真实物理***的尺度误差，分别表示相机坐标系相对IMU坐标系的位置误差和姿态误差，分别world坐标系相对于视觉坐标系(由v表示)的位置误差和姿态误差；
得到连续时间的误差状态运动方程为：
其中表示向量的反对称阵；
将式(11)归纳成连续时间的误差状态方程为：
其中，
对Fc进行离散化处理，得到Fd：
其中Δt表示离散采样时间间隔，Id表示单位矩阵；
而离散时间的噪声协方差矩阵Qd：
其中：
Qc表示为连续时间的***噪声协方差矩阵，其中依次表示为加速度测量噪声方差、加速度偏置漂移方差、角速度测量噪声方差、角速度偏置漂移方差；
由此，进行扩展卡尔曼滤波的预测步骤如下：
1.根据加速度和角速度信息迭代更新状态量；
2.计算离散***矩阵Fd和噪声协方差矩阵Qd；
3.根据黎卡提方程计算k-1时刻到k时刻的状态协方差矩阵：
其中Pk|k-1表示k-1时刻到k时刻预测的过程协方差矩阵，Pk-1|k-1表示k-1时刻的过程协方差矩阵。
进一步的，所述步骤(3)中结合视觉直接法输出的尺度缺失的视觉传感器运动位姿估计作为量测值，进行更新，获得飞行器位姿估计，具体如下：
计算扩展卡尔曼滤波量测方程：
位置误差为：
其中zp表示视觉***的位置测量值，表示视觉***的位置估计值，np表示视觉***位置测量值的噪声；
对式(16)进行线性化可得：
其中Hp为线性化后的量测方程矩阵；
同样建立姿态误差
其中zq表示视觉***的姿态测量值，表示视觉***的姿态估计值，表示四元数乘法；
对式(18)进行线性化可得：
其中Hq为线性化后的量测方程矩阵；
将式(17)和式(19)合并后得到完整的扩展卡尔曼滤波量测方程：
计算扩展卡尔曼滤波的更新步骤：
1.计算卡尔曼增益：
K＝Pk|k-1HT(HPk|k-1HT+Rm)-1 (21)
其中Rm表示视觉***测量值的方差；
2.更新状态量：
xk|k表示k时刻更新后的状态量，xk|k-1为k-1时刻到k时刻的预测值；
3.更新过程协方差矩阵：
Pk|k＝(I-KH)Pk|k-1(I-KH)T+KRmKT (23)
其中I表示单位矩阵；
最终获得具有真实尺度信息的视觉传感器的运动位姿估计。
同现有技术相比，本发明的有益效果体现在：
1)本发明采用直接法作为视觉***的核心算法，该方法不需要进行特征点描述子的计算，也不需要进行特征点匹配，从而节省了大量的计算资源，使得整个算法的运行频率较高；
2)和一般的单目视觉里程计相比，本发明和惯性测量元件IMU的加速度和角速度进行融合，避免了一般单目视觉里程计尺度缺失的问题，从而实现能长期运行且鲁棒性较高的飞行器定位功能。
附图说明
图1为本发明的硬件架构示意图；
图2为本发明实现飞行器定位的传感器数据处理流程图；
图中：1‐微型处理器(NUC)、2‐惯性测量元件(IMU)、3‐摄像头。
具体实施方式
如图1所示为本发明的硬件架构示意图，由微型处理器(NUC)1，惯性测量元件(IMU)2和摄像头3组成，惯性测量元件(IMU)2和摄像头3均与微型处理器(NUC)1相连。
微型处理器采用Intel公司的NUC(NUC5i7RYH)系列，整机重量仅为0.607Kg，该处理器具备体积小、多借口、处理速度快、功能强大、低功耗、散热快等优点。本发明主要的应用背景为飞行器的运动位姿估计，主要算法依赖对图像的快速处理，因此这款NUC5i7RYH微型处理器非常适合本发明的需求。
惯性测量元件采用LPMS系列，LP‐RESEARCH公司的该系列产品使用了先进的数据融合技术，为用户提供高精度、高鲁棒性、高稳定性的姿态信息以及3轴加速度、3轴角速度和地磁量、方位角等数据，为算法实现扩展卡尔曼滤波提供可靠的姿态信息，非常适合本发明的需求。
摄像头采用的是德国Matrix Vision公司的工业相机mvBlueFox‐MLC200w。该相机图像分辨率为752x480，最大帧率可达90Hz，曝光时间为6微妙‐460毫秒，采用全局曝光的MT9V034CMOS传感器，非常适合本发明的需求。
本发明提供的一种基于直接法与惯导融合的飞行器位姿估计方法，包括如下步骤：
(1)通过惯性测量元件中的加速度计和陀螺仪分别测量飞行器实时的加速度和角速度信息，通过磁力计确定绝对偏航角，从而获得飞行器实时的姿态信息；所述的实时姿态信息包括飞行器的俯仰角、横滚角和偏航角；
(2)利用视觉传感器(摄像头)，采用直接法来获得尺度缺失的摄像头运动位姿估计；该步骤中获得的运动位姿包括：摄像头相对于参考坐标系的旋转和摄像头在参考坐标系中缺乏尺度的平移；
(3)建立运动方程，构造扩展卡尔曼滤波，利用惯性测量元件的加速度输出和角速度信息，进行扩展卡尔曼滤波器预测，结合视觉直接法输出的尺度缺失的摄像头运动位姿估计作为量测值，进行更新，获得飞行器位姿估计。
所述步骤(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。
但是值得注意的是，由于上述展示的是单目视觉实现的摄像头运动位姿估计，因此该方法获得的摄像头在参考系下的平移是缺少真实的尺度信息，所谓“尺度缺失”的位姿估计。因此我们需要利用惯性测量元件IMU的加速度和角速度的测量，建立扩展卡尔曼滤波方程，完整地估计飞行器的运动位姿以及视觉***和真实的物理***之间的尺度信息。
所述步骤(3)中建立运动方程，构造扩展卡尔曼滤波，利用加速度输出和角速度信息，进行扩展卡尔曼滤波器预测，具体如下：
构造扩展卡尔曼滤波器的状态量x：
分别表示为第i时刻IMU坐标系相对于world坐标系(由w表示)的位置、速度、姿态(用四元数表示)，bω、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)的状态表征中，使用四元数作为姿态的描述，但是四元数并不是姿态的最小维度表征，为了避免参数的超参数化或者冗余导致协方差矩阵的奇异性问题，在表征状态误差时将引入旋转向量θ作为姿态的最小维度描述，因此误差状态量为：
分别表示为第i时刻IMU坐标系相对于world坐标系(由w表示)的位置误差、速度误差、姿态误差(用四元数表示)，Δbω、Δba分别表示角速度和加速度的偏置误差，Δλ表示视觉***与真实物理***的尺度误差，分别表示相机坐标系相对IMU坐标系的位置误差和姿态误差，分别world坐标系相对于视觉坐标系(由v表示)的位置误差和姿态误差；
得到连续时间的误差状态运动方程为：
其中表示向量的反对称阵；
将式(11)归纳成连续时间的误差状态方程为：
其中，
对Fc进行离散化处理，得到Fd：
其中Δt表示离散采样时间间隔，Id表示单位矩阵；
而离散时间的噪声协方差矩阵Qd：
其中：
Qc表示为连续时间的***噪声协方差矩阵，其中依次表示为加速度测量噪声方差、加速度偏置漂移方差、角速度测量噪声方差、角速度偏置漂移方差；
由此，进行扩展卡尔曼滤波的预测步骤如下：
1.根据加速度和角速度信息迭代更新状态量；
2.计算离散***矩阵Fd和噪声协方差矩阵Qd；
3.根据黎卡提方程计算k-1时刻到k时刻的状态协方差矩阵：
其中Pk|k-1表示k-1时刻到k时刻预测的过程协方差矩阵，Pk-1|k-1表示k-1时刻的过程协方差矩阵。
所述步骤(3)中结合视觉直接法输出的尺度缺失的视觉传感器运动位姿估计作为量测值，进行更新，获得飞行器位姿估计，具体如下：
计算扩展卡尔曼滤波量测方程：
位置误差为：
其中zp表示视觉***的位置测量值，表示视觉***的位置估计值，np表示视觉***位置测量值的噪声；
对式(16)进行线性化可得：
其中Hp为线性化后的量测方程矩阵；
同样建立姿态误差
其中zq表示视觉***的姿态测量值，表示视觉***的姿态估计值，表示四元数乘法；
对式(18)进行线性化可得：
其中Hq为线性化后的量测方程矩阵；
将式(17)和式(19)合并后得到完整的扩展卡尔曼滤波量测方程：
计算扩展卡尔曼滤波的更新步骤：
1.计算卡尔曼增益：
K＝Pk|k-1HT(HPk|k-1HT+Rm)-1 (21)
其中Rm表示视觉***测量值的方差；
2.更新状态量：
xk|k表示k时刻更新后的状态量，xk|k-1为k-1时刻到k时刻的预测值；
3.更新过程协方差矩阵：
Pk|k＝(I-KH)Pk|k-1(I-KH)T+KRmKT (23)
其中I表示单位矩阵；
最终获得具有真实尺度信息的视觉传感器的运动位姿估计。
如图2所示为本发明实现飞行器定位的传感器数据处理流程图。当算法初始化完成后，分为两个线程分别对摄像头的图像数据、惯性测量元件的加速度和角速度数据进行处理。图像处理线程在获取摄像头图像之后，采用直接法获取尺度缺失的相机运动位姿估计，该处理线程以20Hz稳定运行。另一个线程在获取惯性测量元件IMU的加速度和角速度数据后，对加速度和角速度按照运动方程进行积分，预测飞行器的运动位姿，该线程以100Hz稳定运行。此后，以较高频率(100Hz)运行的IMU数据处理线程若发现有较低频率运行(20Hz)的图像处理线程的结果产生时，则将通过扩展卡尔曼滤波进行数据融合，最终输出飞行器的运动位姿估计结果。

Claims (4)

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’为：
<msub> <mi>&amp;pi;</mi> <mi>c</mi> </msub> <mrow> <mrow> <mo>(</mo> <mi>R</mi> <msubsup> <mi>&amp;pi;</mi> <mi>c</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>p</mi> <mo>,</mo> <msub> <mi>d</mi> <mi>p</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi></mi> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
其中πc为3D-2D的投影模型，为针孔成像模型，针孔成像模型的内参矩阵为：
<mrow> <mi>K</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>f</mi> <mi>x</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>c</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mi>c</mi> <mi>y</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <msup> <mi>K</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>f</mi> <mi>x</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>f</mi> <mi>x</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>c</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <msubsup> <mi>f</mi> <mi>y</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>f</mi> <mi>y</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>c</mi> <mi>y</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
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：
<mrow> <mi>x</mi> <mo>=</mo> <mfenced open = "{" close = "}"> <mtable> <mtr> <mtd> <msubsup> <mi>p</mi> <mi>i</mi> <mrow> <mi>w</mi> <mi>T</mi> </mrow> </msubsup> </mtd> <mtd> <msubsup> <mi>v</mi> <mi>i</mi> <mrow> <mi>w</mi> <mi>T</mi> </mrow> </msubsup> </mtd> <mtd> <msubsup> <mi>q</mi> <mi>i</mi> <mrow> <mi>w</mi> <mi>T</mi> </mrow> </msubsup> </mtd> <mtd> <mrow> <msup> <msub> <mi>b</mi> <mi>w</mi> </msub> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>b</mi> <mi>a</mi> </msub> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mi>&amp;lambda;</mi> </mtd> <mtd> <mrow> <msup> <msubsup> <mi>p</mi> <mi>c</mi> <mi>i</mi> </msubsup> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msubsup> <mi>q</mi> <mi>c</mi> <mi>i</mi> </msubsup> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msubsup> <mi>p</mi> <mi>w</mi> <mi>v</mi> </msubsup> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msubsup> <mi>q</mi> <mi>w</mi> <mi>v</mi> </msubsup> <mi>T</mi> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
分别表示为第i时刻IMU坐标系相对于world坐标系(由w表示)的位置、速度、姿态(用四元数表示)，bw、ba分别表示角速度和加速度的偏置，λ表示视觉***与真实物理***的尺度，分别表示相机坐标系相对IMU坐标系的位置和姿态，分别world坐标系相对于视觉坐标系(由v表示)的位置和姿态，则实际状态量的运动学方程为：
<mrow> <msubsup> <mover> <mi>p</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>=</mo> <msubsup> <mi>v</mi> <mi>i</mi> <mi>w</mi> </msubsup> </mrow>
<mrow> <msubsup> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>=</mo> <mi>C</mi> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mi>i</mi> <mi>w</mi> </msubsup> <mo>)</mo> </mrow> <mi>a</mi> <mo>-</mo> <mi>g</mi> </mrow>
<mrow> <msubsup> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mi>&amp;Omega;</mi> <mrow> <mo>(</mo> <mi>&amp;omega;</mi> <mo>)</mo> </mrow> <msubsup> <mi>q</mi> <mi>i</mi> <mi>w</mi> </msubsup> </mrow>
<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>b</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>&amp;omega;</mi> </msub> <mo>=</mo> <msub> <mi>n</mi> <msub> <mi>b</mi> <mi>&amp;omega;</mi> </msub> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mover> <mi>b</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>a</mi> </msub> <mo>=</mo> <msub> <mi>n</mi> <msub> <mi>b</mi> <mi>a</mi> </msub> </msub> </mrow> </mtd> <mtd> <mrow> <mover> <mi>&amp;lambda;</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> </mfenced>
<mrow> <mtable> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>c</mi> <mi>i</mi> </msubsup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> <mtd> <mrow> <msubsup> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>c</mi> <mi>i</mi> </msubsup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> <mtd> <mrow> <msubsup> <mover> <mi>p</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>w</mi> <mi>v</mi> </msubsup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> <mtd> <mrow> <msubsup> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>w</mi> <mi>v</mi> </msubsup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>
a＝am-ba-na ω＝ωm-bω-nω (7)
其中，C(·)表示四元数到旋转矩阵的变换关系，Ω(·)表示角速度与四元数的转换关系，nbw表示角速度偏置的漂移噪声，naw表示加速度偏置的漂移噪声，g表示重力加速度，a表示加速度的真实值，am表示惯性测量元件的加速度输出，na表示加速度的测量噪声，ω表示角速度的真实值，ωm表示惯性测量元件的角速度输出，nω表示角速度的测量噪声；
由于噪声或者扰动的存在，实际中无法得到状态的真实值，需要对这些状态进行估计，此时认为加速度计和陀螺仪的测量不带噪声或扰动，则改写真实状态方程为：
<mrow> <msubsup> <mover> <mover> <mi>p</mi> <mo>^</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>v</mi> <mo>^</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>,</mo> </mrow>
<mrow> <msubsup> <mover> <mover> <mi>v</mi> <mo>^</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>=</mo> <mi>C</mi> <mrow> <mo>(</mo> <msubsup> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>)</mo> </mrow> <mover> <mi>a</mi> <mo>^</mo> </mover> <mo>-</mo> <mi>g</mi> <mo>,</mo> </mrow>
<mrow> <msubsup> <mover> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mi>&amp;Omega;</mi> <mrow> <mo>(</mo> <mover> <mi>&amp;omega;</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <msubsup> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>,</mo> </mrow>
<mrow> <msub> <mover> <mover> <mi>b</mi> <mo>^</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mi>&amp;omega;</mi> </msub> <mo>=</mo> <mn>0</mn> <mo>,</mo> <msub> <mover> <mover> <mi>b</mi> <mo>^</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mi>a</mi> </msub> <mo>=</mo> <mn>0</mn> <mo>,</mo> <mover> <mover> <mi>&amp;lambda;</mi> <mo>^</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mn>0</mn> <mo>,</mo> </mrow>
<mrow> <msubsup> <mover> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mi>c</mi> <mi>i</mi> </msubsup> <mo>=</mo> <mn>0</mn> <mo>,</mo> <msubsup> <mover> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mi>c</mi> <mi>i</mi> </msubsup> <mo>=</mo> <mn>0</mn> <mo>,</mo> <msubsup> <mover> <mover> <mi>p</mi> <mo>^</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mi>w</mi> <mi>v</mi> </msubsup> <mo>=</mo> <mn>0</mn> <mo>,</mo> <msubsup> <mover> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mi>w</mi> <mi>v</mi> </msubsup> <mo>=</mo> <mn>0</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <mtable> <mtr> <mtd> <mrow> <mover> <mi>a</mi> <mo>^</mo> </mover> <mo>=</mo> <msub> <mi>a</mi> <mi>m</mi> </msub> <mo>-</mo> <msub> <mover> <mi>b</mi> <mo>^</mo> </mover> <mi>a</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mover> <mi>&amp;omega;</mi> <mo>^</mo> </mover> <mo>=</mo> <msub> <mi>&amp;omega;</mi> <mi>m</mi> </msub> <mo>-</mo> <msub> <mover> <mi>b</mi> <mo>^</mo> </mover> <mi>&amp;omega;</mi> </msub> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
其中，分别表示为第i时刻IMU坐标系相对于world坐标系(由w表示)的位置、速度、姿态(用四元数表示)的估计值，分别表示角速度和加速度的偏置的估计值，表示视觉***与真实物理***的尺度的估计值，分别表示相机坐标系相对IMU坐标系的位置和姿态的估计值，分别world坐标系相对于视觉坐标系(由v表示)的位置和姿态的估计值，表示加速度的真实值的估计值，表示角速度的真实值的估计值；
在式(6)和式(8)的状态表征中，使用四元数作为姿态的描述，但是四元数并不是姿态的最小维度表征，为了避免参数的超参数化或者冗余导致协方差矩阵的奇异性问题，在表征状态误差时将引入旋转向量θ作为姿态的最小维度描述，因此误差状态量为：
<mrow> <mover> <mi>x</mi> <mo>~</mo> </mover> <mo>=</mo> <mfenced open = "{" close = "}"> <mtable> <mtr> <mtd> <mrow> <msup> <msubsup> <mi>&amp;Delta;p</mi> <mi>i</mi> <mi>w</mi> </msubsup> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msubsup> <mi>&amp;Delta;v</mi> <mi>i</mi> <mi>w</mi> </msubsup> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msubsup> <mi>&amp;delta;&amp;theta;</mi> <mi>i</mi> <mi>w</mi> </msubsup> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>&amp;Delta;b</mi> <mi>w</mi> </msub> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msub> <mi>&amp;Delta;b</mi> <mi>a</mi> </msub> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <mi>&amp;Delta;</mi> <mi>&amp;lambda;</mi> </mrow> </mtd> <mtd> <mrow> <msup> <msubsup> <mi>&amp;Delta;p</mi> <mi>c</mi> <mi>i</mi> </msubsup> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msubsup> <mi>&amp;delta;&amp;theta;</mi> <mi>c</mi> <mi>i</mi> </msubsup> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msubsup> <mi>&amp;Delta;p</mi> <mi>w</mi> <mi>v</mi> </msubsup> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <msup> <msubsup> <mi>&amp;delta;&amp;theta;</mi> <mi>w</mi> <mi>v</mi> </msubsup> <mi>T</mi> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>
分别表示为第i时刻IMU坐标系相对于world坐标系(由w表示)的位置误差、速度误差、姿态误差(用四元数表示)，Δbw、Δba分别表示角速度和加速度的偏置误差，Δλ表示视觉***与真实物理***的尺度误差，分别表示相机坐标系相对IMU坐标系的位置误差和姿态误差，分别world坐标系相对于视觉坐标系(由v表示)的位置误差和姿态误差；
得到连续时间的误差状态运动方程为：
<mrow> <mi>&amp;Delta;</mi> <msubsup> <mover> <mi>p</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>=</mo> <msubsup> <mi>&amp;Delta;v</mi> <mi>i</mi> <mi>w</mi> </msubsup> <mo>,</mo> </mrow>
<mrow> <mi>&amp;delta;</mi> <msubsup> <mover> <mi>&amp;theta;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>=</mo> <mo>-</mo> <mover> <mi>&amp;omega;</mi> <mo>^</mo> </mover> <mo>&amp;times;</mo> <msubsup> <mi>&amp;delta;&amp;theta;</mi> <mi>i</mi> <mi>w</mi> </msubsup> <mo>-</mo> <msub> <mi>&amp;Delta;b</mi> <mi>&amp;omega;</mi> </msub> <mo>-</mo> <msub> <mi>n</mi> <mi>&amp;omega;</mi> </msub> <mo>,</mo> </mrow>
<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>&amp;Delta;</mi> <msub> <mover> <mi>b</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>&amp;omega;</mi> </msub> <mo>=</mo> <msub> <mi>n</mi> <msub> <mi>b</mi> <mi>&amp;omega;</mi> </msub> </msub> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>&amp;Delta;</mi> <msub> <mover> <mi>b</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>a</mi> </msub> <mo>=</mo> <msub> <mi>n</mi> <msub> <mi>b</mi> <mi>a</mi> </msub> </msub> </mrow> </mtd> <mtd> <mrow> <mi>&amp;Delta;</mi> <mover> <mi>&amp;lambda;</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mn>0</mn> <mo>,</mo> </mrow> </mtd> </mtr> </mtable> </mfenced>
<mrow> <mi>&amp;Delta;</mi> <msubsup> <mover> <mi>p</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>c</mi> <mi>i</mi> </msubsup> <mo>=</mo> <mn>0</mn> <mo>,</mo> <mi>&amp;delta;</mi> <msubsup> <mover> <mi>&amp;theta;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>c</mi> <mi>i</mi> </msubsup> <mo>=</mo> <mn>0</mn> <mo>,</mo> <mi>&amp;Delta;</mi> <msubsup> <mover> <mi>p</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>w</mi> <mi>v</mi> </msubsup> <mo>=</mo> <mn>0</mn> <mo>,</mo> <mi>&amp;delta;</mi> <msubsup> <mover> <mi>&amp;theta;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>w</mi> <mi>v</mi> </msubsup> <mo>=</mo> <mn>0</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>
其中表示向量的反对称阵；
将式(11)归纳成连续时间的误差状态方程为：
<mrow> <mover> <mover> <mi>x</mi> <mo>~</mo> </mover> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <msub> <mi>F</mi> <mi>c</mi> </msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mo>+</mo> <msub> <mi>G</mi> <mi>c</mi> </msub> <mi>n</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
其中，对Fc进行离散化处理，得到Fd：
<mrow> <msub> <mi>F</mi> <mi>d</mi> </msub> <mo>=</mo> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mi>F</mi> <mi>c</mi> </msub> <mi>&amp;Delta;</mi> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>I</mi> <mi>d</mi> </msub> <mo>+</mo> <msub> <mi>F</mi> <mi>c</mi> </msub> <mi>&amp;Delta;</mi> <mi>t</mi> <mo>+</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mo>!</mo> </mrow> </mfrac> <msubsup> <mi>F</mi> <mi>c</mi> <mn>2</mn> </msubsup> <mi>&amp;Delta;</mi> <mi>t</mi> <mo>+</mo> <mo>...</mo> </mrow>
其中Δt表示离散采样时间间隔，Id表示单位矩阵；
而离散时间的噪声协方差矩阵Qd：
<mrow> <msub> <mi>Q</mi> <mi>d</mi> </msub> <mo>=</mo> <msub> <mo>&amp;Integral;</mo> <mrow> <mi>&amp;Delta;</mi> <mi>t</mi> </mrow> </msub> <msub> <mi>F</mi> <mi>d</mi> </msub> <mrow> <mo>(</mo> <mi>&amp;tau;</mi> <mo>)</mo> </mrow> <msub> <mi>G</mi> <mi>c</mi> </msub> <msub> <mi>Q</mi> <mi>c</mi> </msub> <msubsup> <mi>G</mi> <mi>c</mi> <mi>T</mi> </msubsup> <msub> <mi>F</mi> <mi>d</mi> </msub> <msup> <mrow> <mo>(</mo> <mi>&amp;tau;</mi> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mi>d</mi> <mi>&amp;tau;</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>
其中：
<mrow> <msub> <mi>Q</mi> <mi>c</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>&amp;sigma;</mi> <msub> <mi>n</mi> <mi>a</mi> </msub> <mn>2</mn> </msubsup> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msubsup> <mi>&amp;sigma;</mi> <msub> <mi>n</mi> <msub> <mi>b</mi> <mi>a</mi> </msub> </msub> <mn>2</mn> </msubsup> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msubsup> <mi>&amp;sigma;</mi> <msub> <mi>n</mi> <mi>&amp;omega;</mi> </msub> <mn>2</mn> </msubsup> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msubsup> <mi>&amp;sigma;</mi> <msub> <mi>n</mi> <msub> <mi>b</mi> <mi>&amp;omega;</mi> </msub> </msub> <mn>2</mn> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>
Qc表示为连续时间的***噪声协方差矩阵，其中依次表示为加速度测量噪声方差、加速度偏置漂移方差、角速度测量噪声方差、角速度偏置漂移方差；
由此，进行扩展卡尔曼滤波的预测步骤如下：
1.根据加速度和角速度信息迭代更新状态量；
2.计算离散***矩阵Fd和噪声协方差矩阵Qd；
3.根据黎卡提方程计算k-1时刻到k时刻的状态协方差矩阵：
<mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>F</mi> <mi>d</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>F</mi> <mi>d</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>Q</mi> <mi>d</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>
其中Pk|k-1表示k-1时刻到k时刻预测的过程协方差矩阵，Pk-1|k-1表示k-1时刻的过程协方差矩阵。
4.根据权利要求3所述的一种基于直接法与惯导融合的飞行器位姿估计方法，其特征在于，所述步骤(3)中结合视觉直接法输出的尺度缺失的视觉传感器运动位姿估计作为量测值，进行更新，获得飞行器位姿估计，具体如下：
计算扩展卡尔曼滤波量测方程：
位置误差为：
<mrow> <msub> <mover> <mi>z</mi> <mo>~</mo> </mover> <mi>p</mi> </msub> <mo>=</mo> <msub> <mi>z</mi> <mi>p</mi> </msub> <mo>-</mo> <msub> <mover> <mi>z</mi> <mo>^</mo> </mover> <mi>p</mi> </msub> <mo>=</mo> <mi>C</mi> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mi>w</mi> <mi>v</mi> </msubsup> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msubsup> <mi>p</mi> <mi>i</mi> <mi>w</mi> </msubsup> <mo>+</mo> <mi>C</mi> <mo>(</mo> <msubsup> <mi>q</mi> <mi>i</mi> <mi>w</mi> </msubsup> <mo>)</mo> <msubsup> <mi>p</mi> <mi>c</mi> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mi>&amp;lambda;</mi> <mo>+</mo> <msub> <mi>n</mi> <mi>p</mi> </msub> <mo>-</mo> <mi>C</mi> <mrow> <mo>(</mo> <msubsup> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>w</mi> <mi>v</mi> </msubsup> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msubsup> <mover> <mi>p</mi> <mo>^</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>+</mo> <mi>C</mi> <mo>(</mo> <msubsup> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>)</mo> <msubsup> <mover> <mi>p</mi> <mo>^</mo> </mover> <mi>c</mi> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mover> <mi>&amp;lambda;</mi> <mo>^</mo> </mover> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>
其中zp表示视觉***的位置测量值，表示视觉***的位置估计值，np表示视觉***位置测量值的噪声；
对式(16)进行线性化可得：
<mrow> <msub> <mover> <mi>z</mi> <mo>~</mo> </mover> <mi>p</mi> </msub> <mo>=</mo> <msub> <mi>H</mi> <mi>p</mi> </msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow>
其中Hp为线性化后的量测方程矩阵；
同样建立姿态误差
<mrow> <msub> <mover> <mi>z</mi> <mo>~</mo> </mover> <mi>q</mi> </msub> <mo>=</mo> <msub> <mi>z</mi> <mi>q</mi> </msub> <mo>-</mo> <msub> <mover> <mi>z</mi> <mo>^</mo> </mover> <mi>q</mi> </msub> <mo>=</mo> <msubsup> <mi>q</mi> <mi>w</mi> <mi>v</mi> </msubsup> <mo>&amp;CircleTimes;</mo> <msubsup> <mi>q</mi> <mi>i</mi> <mi>w</mi> </msubsup> <mo>&amp;CircleTimes;</mo> <msubsup> <mi>q</mi> <mi>c</mi> <mi>i</mi> </msubsup> <mo>&amp;CircleTimes;</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>w</mi> <mi>v</mi> </msubsup> <mo>&amp;CircleTimes;</mo> <msubsup> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>i</mi> <mi>w</mi> </msubsup> <mo>&amp;CircleTimes;</mo> <msubsup> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>c</mi> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>
其中zq表示视觉***的姿态测量值，表示视觉***的姿态估计值，表示四元数乘法；
对式(18)进行线性化可得：
<mrow> <msub> <mover> <mi>z</mi> <mo>~</mo> </mover> <mi>q</mi> </msub> <mo>=</mo> <msub> <mi>H</mi> <mi>q</mi> </msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>
其中Hq为线性化后的量测方程矩阵；
将式(17)和式(19)合并后得到完整的扩展卡尔曼滤波量测方程：
计算扩展卡尔曼滤波的更新步骤：
1.计算卡尔曼增益：
K＝Pk|k-1HT(HPk|k-1HT+Rm)-1 (21)
其中Rm表示视觉***测量值的方差；
2.更新状态量：
<mrow> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mi>K</mi> <mover> <mi>z</mi> <mo>~</mo> </mover> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow>
xk|k表示k时刻更新后的状态量，xk|k-1为k-1时刻到k时刻的预测值；
3.更新过程协方差矩阵：
Pk|k＝(I-KH)Pk|k-1(I-KH)T+KRmKT (23)
其中I表示单位矩阵；
最终获得具有真实尺度信息的视觉传感器的运动位姿估计。
CN201711190702.0A 2017-11-24 2017-11-24 一种基于直接法与惯导融合的飞行器位姿估计方法 Pending CN108036785A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711190702.0A CN108036785A (zh) 2017-11-24 2017-11-24 一种基于直接法与惯导融合的飞行器位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711190702.0A CN108036785A (zh) 2017-11-24 2017-11-24 一种基于直接法与惯导融合的飞行器位姿估计方法

Publications (1)

Publication Number Publication Date
CN108036785A true CN108036785A (zh) 2018-05-15

Family

ID=62093066

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711190702.0A Pending CN108036785A (zh) 2017-11-24 2017-11-24 一种基于直接法与惯导融合的飞行器位姿估计方法

Country Status (1)

Country Link
CN (1) CN108036785A (zh)

Cited By (26)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108648215A (zh) * 2018-06-22 2018-10-12 南京邮电大学 基于imu的slam运动模糊位姿跟踪算法
CN108921898A (zh) * 2018-06-28 2018-11-30 北京旷视科技有限公司 摄像机位姿确定方法、装置、电子设备和计算机可读介质
CN108956003A (zh) * 2018-07-17 2018-12-07 杭州崧智智能科技有限公司 一种实时标定六维传感器姿态的方法、装置及终端设备
CN109099912A (zh) * 2017-08-11 2018-12-28 黄润芳 室外精确定位导航方法、装置、电子设备及存储介质
CN109211241A (zh) * 2018-09-08 2019-01-15 天津大学 基于视觉slam的无人机自主定位方法
CN109269511A (zh) * 2018-11-06 2019-01-25 北京理工大学 未知环境下行星着陆的曲线匹配视觉导航方法
CN109520497A (zh) * 2018-10-19 2019-03-26 天津大学 基于视觉和imu的无人机自主定位方法
CN109544638A (zh) * 2018-10-29 2019-03-29 浙江工业大学 一种用于多传感器融合的异步在线标定方法
CN109631875A (zh) * 2019-01-11 2019-04-16 京东方科技集团股份有限公司 一种对传感器姿态融合测量方法进行优化的方法和***
CN109916399A (zh) * 2019-04-04 2019-06-21 中国人民解放***箭军工程大学 一种阴影下的载体姿态估计方法
CN110160522A (zh) * 2019-04-16 2019-08-23 浙江大学 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法
CN110260861A (zh) * 2019-06-13 2019-09-20 北京华捷艾米科技有限公司 位姿确定方法及装置、里程计
CN110702107A (zh) * 2019-10-22 2020-01-17 北京维盛泰科科技有限公司 一种单目视觉惯性组合的定位导航方法
WO2020024182A1 (zh) * 2018-08-01 2020-02-06 深圳市大疆创新科技有限公司 一种参数处理方法、装置及摄像设备、飞行器
CN110850817A (zh) * 2019-10-18 2020-02-28 杭州电子科技大学 一种网络化工业控制***的安全估计方法
CN110929402A (zh) * 2019-11-22 2020-03-27 哈尔滨工业大学 一种基于不确定分析的概率地形估计方法
CN111367319A (zh) * 2020-05-06 2020-07-03 仿翼(北京)科技有限公司 飞行器、飞行器的控制方法及计算机可读存储介质
CN111383282A (zh) * 2018-12-29 2020-07-07 杭州海康威视数字技术股份有限公司 位姿信息确定方法及装置
CN111398522A (zh) * 2020-03-24 2020-07-10 山东智翼航空科技有限公司 基于微型无人机的室内空气质量检测***及检测方法
CN112161639A (zh) * 2020-07-29 2021-01-01 河海大学 一种基于角度光流法的垂直双目惯导里程计及其计算方法
CN112504261A (zh) * 2020-11-09 2021-03-16 中国人民解放军国防科技大学 一种基于视觉锚点的无人机降落位姿滤波估计方法及***
CN112767481A (zh) * 2021-01-21 2021-05-07 山东大学 一种基于视觉边缘特征的高精度定位及建图方法
CN113065572A (zh) * 2019-12-31 2021-07-02 北京凌宇智控科技有限公司 多传感器融合的数据处理方法、定位装置及虚拟现实设备
CN114419109A (zh) * 2022-03-29 2022-04-29 中航金城无人***有限公司 一种基于视觉和气压信息融合的飞行器定位方法
CN114660641A (zh) * 2022-02-28 2022-06-24 华南理工大学 一种自适应gps融合定位***、方法及介质
CN117405109A (zh) * 2023-12-15 2024-01-16 北京神导科技股份有限公司 基于四元数球面线性加权的三套惯导***姿态表决方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102538781B (zh) * 2011-12-14 2014-12-17 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
US20160163114A1 (en) * 2014-12-05 2016-06-09 Stmicroelectronics S.R.L. Absolute rotation estimation including outlier detection via low-rank and sparse matrix decomposition
CN105973238A (zh) * 2016-05-09 2016-09-28 郑州轻工业学院 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法
CN106017463A (zh) * 2016-05-26 2016-10-12 浙江大学 一种基于定位传感装置的飞行器定位方法
CN107341814A (zh) * 2017-06-14 2017-11-10 宁波大学 基于稀疏直接法的四旋翼无人机单目视觉测程方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102538781B (zh) * 2011-12-14 2014-12-17 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
US20160163114A1 (en) * 2014-12-05 2016-06-09 Stmicroelectronics S.R.L. Absolute rotation estimation including outlier detection via low-rank and sparse matrix decomposition
CN105973238A (zh) * 2016-05-09 2016-09-28 郑州轻工业学院 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法
CN106017463A (zh) * 2016-05-26 2016-10-12 浙江大学 一种基于定位传感装置的飞行器定位方法
CN107341814A (zh) * 2017-06-14 2017-11-10 宁波大学 基于稀疏直接法的四旋翼无人机单目视觉测程方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
JAKOB ENGEL 等，: ""Direct Sparse Odometry"", 《IEEE TRANSACTIONS ON PATTERN ANALYSIS AND MACHINE INTELLIGENCE》 *
叶波，: ""基于四旋翼平台的融合单目视觉与惯性传感的里程计方法研究"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
茹祥宇 等，: ""单目视觉惯性融合方法在无人机位姿估计中的应用"", 《控制与信息技术》 *

Cited By (41)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109099912B (zh) * 2017-08-11 2022-05-10 黄润芳 室外精确定位导航方法、装置、电子设备及存储介质
CN109099912A (zh) * 2017-08-11 2018-12-28 黄润芳 室外精确定位导航方法、装置、电子设备及存储介质
CN108648215A (zh) * 2018-06-22 2018-10-12 南京邮电大学 基于imu的slam运动模糊位姿跟踪算法
CN108648215B (zh) * 2018-06-22 2022-04-15 南京邮电大学 基于imu的slam运动模糊位姿跟踪算法
CN108921898A (zh) * 2018-06-28 2018-11-30 北京旷视科技有限公司 摄像机位姿确定方法、装置、电子设备和计算机可读介质
CN108921898B (zh) * 2018-06-28 2021-08-10 北京旷视科技有限公司 摄像机位姿确定方法、装置、电子设备和计算机可读介质
CN108956003B (zh) * 2018-07-17 2020-10-20 崧智智能科技(苏州)有限公司 一种实时标定六维传感器姿态的方法、装置及终端设备
CN108956003A (zh) * 2018-07-17 2018-12-07 杭州崧智智能科技有限公司 一种实时标定六维传感器姿态的方法、装置及终端设备
WO2020024182A1 (zh) * 2018-08-01 2020-02-06 深圳市大疆创新科技有限公司 一种参数处理方法、装置及摄像设备、飞行器
CN109211241A (zh) * 2018-09-08 2019-01-15 天津大学 基于视觉slam的无人机自主定位方法
CN109211241B (zh) * 2018-09-08 2022-04-29 天津大学 基于视觉slam的无人机自主定位方法
CN109520497A (zh) * 2018-10-19 2019-03-26 天津大学 基于视觉和imu的无人机自主定位方法
CN109520497B (zh) * 2018-10-19 2022-09-30 天津大学 基于视觉和imu的无人机自主定位方法
CN109544638A (zh) * 2018-10-29 2019-03-29 浙江工业大学 一种用于多传感器融合的异步在线标定方法
CN109544638B (zh) * 2018-10-29 2021-08-03 浙江工业大学 一种用于多传感器融合的异步在线标定方法
CN109269511A (zh) * 2018-11-06 2019-01-25 北京理工大学 未知环境下行星着陆的曲线匹配视觉导航方法
CN111383282B (zh) * 2018-12-29 2023-12-01 杭州海康威视数字技术股份有限公司 位姿信息确定方法及装置
CN111383282A (zh) * 2018-12-29 2020-07-07 杭州海康威视数字技术股份有限公司 位姿信息确定方法及装置
CN109631875A (zh) * 2019-01-11 2019-04-16 京东方科技集团股份有限公司 一种对传感器姿态融合测量方法进行优化的方法和***
CN109916399B (zh) * 2019-04-04 2019-12-24 中国人民解放***箭军工程大学 一种阴影下的载体姿态估计方法
CN109916399A (zh) * 2019-04-04 2019-06-21 中国人民解放***箭军工程大学 一种阴影下的载体姿态估计方法
CN110160522A (zh) * 2019-04-16 2019-08-23 浙江大学 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法
CN110260861B (zh) * 2019-06-13 2021-07-27 北京华捷艾米科技有限公司 位姿确定方法及装置、里程计
CN110260861A (zh) * 2019-06-13 2019-09-20 北京华捷艾米科技有限公司 位姿确定方法及装置、里程计
CN110850817A (zh) * 2019-10-18 2020-02-28 杭州电子科技大学 一种网络化工业控制***的安全估计方法
CN110702107A (zh) * 2019-10-22 2020-01-17 北京维盛泰科科技有限公司 一种单目视觉惯性组合的定位导航方法
CN110929402A (zh) * 2019-11-22 2020-03-27 哈尔滨工业大学 一种基于不确定分析的概率地形估计方法
CN113065572A (zh) * 2019-12-31 2021-07-02 北京凌宇智控科技有限公司 多传感器融合的数据处理方法、定位装置及虚拟现实设备
CN113065572B (zh) * 2019-12-31 2023-09-08 北京凌宇智控科技有限公司 多传感器融合的数据处理方法、定位装置及虚拟现实设备
CN111398522B (zh) * 2020-03-24 2022-02-22 山东智翼航空科技有限公司 基于微型无人机的室内空气质量检测***及检测方法
CN111398522A (zh) * 2020-03-24 2020-07-10 山东智翼航空科技有限公司 基于微型无人机的室内空气质量检测***及检测方法
CN111367319A (zh) * 2020-05-06 2020-07-03 仿翼(北京)科技有限公司 飞行器、飞行器的控制方法及计算机可读存储介质
CN111367319B (zh) * 2020-05-06 2021-01-08 仿翼(北京)科技有限公司 飞行器、飞行器的控制方法及计算机可读存储介质
CN112161639A (zh) * 2020-07-29 2021-01-01 河海大学 一种基于角度光流法的垂直双目惯导里程计及其计算方法
CN112504261A (zh) * 2020-11-09 2021-03-16 中国人民解放军国防科技大学 一种基于视觉锚点的无人机降落位姿滤波估计方法及***
CN112504261B (zh) * 2020-11-09 2024-02-09 中国人民解放军国防科技大学 一种基于视觉锚点的无人机降落位姿滤波估计方法及***
CN112767481A (zh) * 2021-01-21 2021-05-07 山东大学 一种基于视觉边缘特征的高精度定位及建图方法
CN114660641A (zh) * 2022-02-28 2022-06-24 华南理工大学 一种自适应gps融合定位***、方法及介质
CN114419109A (zh) * 2022-03-29 2022-04-29 中航金城无人***有限公司 一种基于视觉和气压信息融合的飞行器定位方法
CN117405109A (zh) * 2023-12-15 2024-01-16 北京神导科技股份有限公司 基于四元数球面线性加权的三套惯导***姿态表决方法
CN117405109B (zh) * 2023-12-15 2024-04-16 北京神导科技股份有限公司 基于四元数球面线性加权的三套惯导***姿态表决方法

Similar Documents

Publication Publication Date Title
CN108036785A (zh) 一种基于直接法与惯导融合的飞行器位姿估计方法
CN110030994B (zh) 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN101726295B (zh) 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
CN106289246B (zh) 一种基于位置和姿态测量***的柔性杆臂测量方法
WO2021180128A1 (zh) 一种基于rgbd传感器和imu传感器的定位方法
CN106979780B (zh) 一种无人车实时姿态测量方法
CN108592950B (zh) 一种单目相机和惯性测量单元相对安装角标定方法
CN109991636A (zh) 基于gps、imu以及双目视觉的地图构建方法及***
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN105953796A (zh) 智能手机单目和imu融合的稳定运动跟踪方法和装置
CN105698765A (zh) 双imu单目视觉组合测量非惯性系下目标物位姿方法
CN107490378B (zh) 一种基于mpu6050与智能手机的室内定位与导航的方法
Omari et al. Metric visual-inertial navigation system using single optical flow feature
CN108981693A (zh) 基于单目相机的vio快速联合初始化方法
CN103940442A (zh) 一种采用加速收敛算法的定位方法及装置
CN107728182A (zh) 基于相机辅助的柔性多基线测量方法和装置
CN110702113B (zh) 基于mems传感器的捷联惯导***数据预处理和姿态解算的方法
CN108917772A (zh) 基于序列图像的非合作目标相对导航运动估计方法
CN103175502A (zh) 一种基于数据手套低速运动的姿态角检测方法
CN112815939A (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN110160522A (zh) 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法
CN103712598A (zh) 一种小型无人机姿态确定***与确定方法
CN109764870B (zh) 基于变换估计量建模方案的载体初始航向估算方法
CN106153069A (zh) 自主导航***中的姿态修正装置和方法
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20180515