CN113551665B - 一种用于运动载体的高动态运动状态感知系统及感知方法 - Google Patents

一种用于运动载体的高动态运动状态感知系统及感知方法 Download PDF

Info

Publication number
CN113551665B
CN113551665B CN202110711142.9A CN202110711142A CN113551665B CN 113551665 B CN113551665 B CN 113551665B CN 202110711142 A CN202110711142 A CN 202110711142A CN 113551665 B CN113551665 B CN 113551665B
Authority
CN
China
Prior art keywords
imu
data
frame
camera
matrix
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.)
Active
Application number
CN202110711142.9A
Other languages
English (en)
Other versions
CN113551665A (zh
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.)
National Space Science Center of CAS
Original Assignee
National Space Science Center of CAS
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 National Space Science Center of CAS filed Critical National Space Science Center of CAS
Priority to CN202110711142.9A priority Critical patent/CN113551665B/zh
Publication of CN113551665A publication Critical patent/CN113551665A/zh
Application granted granted Critical
Publication of CN113551665B publication Critical patent/CN113551665B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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/20—Instruments for performing navigational calculations

Abstract

本发明属于导航的运动状态感知技术领域，具体地说，涉及一种用于运动载体的高动态运动状态感知系统及感知方法，包括：摄像头、惯性传感器、倾斜仪、高度计、融合感知模块、动态补偿模块和接口模块；融合感知模块，用于对接收的30Hz的图像数据和降频后的IMU数据进行处理，先处理图像数据；再使用降频后的IMU数据，结合处理后的图像数据，得到视觉导航数据；动态补偿模块，用于对接收的3600Hz的IMU数据进行预处理和降频处理，获得降频后的IMU数据；还用于对降频后的IMU数据进行IMU状态递推，得到惯性导航信息；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；对IMU状态递推后的惯性导航信息进行融合修正，得到修正后的数据。

Description

一种用于运动载体的高动态运动状态感知系统及感知方法
技术领域
本发明属于导航的运动状态感知技术领域，具体地说，涉及一种用于运动载体的高动态运动状态感知系统及感知方法。
背景技术
随着移动互联网化产业的不断发展，位置服务已经成为移动终端的一项重要指标，为载体提供实时的姿态和航向信息，在航空航天、机器人、无人机、汽车工业以及行人导航定位领域得到广泛的应用。
近十年中，SLAM(simultaneous localization and mapping，同步定位与建图)及其相关技术的研究取得了长足的进展，大量的SLAM实现方法涌现。SLAM包括：载体运动状态估计和传感器检测到的环境模型的构建，共2个部分。其中，载体的状态可由位姿来描述，而建图则是对载体所处环境的信息，如位置、障碍等的描述。随着研究人员越来越关注将该技术应用于微小型无人机、小型排雷机器人和小型反恐侦察机器人等小型运动载体平台，SLAM的研究重点开始从激光雷达转向相机和惯性传感器(IMU)。得益于芯片和微机电系统(MEMS)器件的快速发展，相机和MU等传感器实现了高精度化、小型化和低成本化。采用相机和IMU的SLAM方法被称为视觉惯性SLAM(visual-inertialSLAM，VI-SLAM)，当前，VI-SLAM方法是无人机、自动驾驶、机器人等领域解决环境感知与定位导航问题的有效手段之一。
对于视觉惯性SLAM，根据参与估计的位姿数量不同，主要分为基于滤波和基于优化两大类方法，这两种不同的分类方法相互组合可将视觉惯性SLAM细分为滤波松耦合、滤波紧耦合、优化松耦合和优化紧耦合等方法，但存在以下问题：
1、松耦合方式是将视觉信息和IMU信息分别放入两个独立的模块中处理，然后再对处理的结果进行融合。一般都是在视觉框架给出不带尺度的位姿结果后，再将这个结果作为状态量与IMU信息进行融合得到带有尺度信息的位姿结果。滤波松耦合一般是在扩展卡尔曼滤波框架下针对IMU和摄像头的融合方法，需要单独给出IMU误差模型和相机量测模型，同时要解决传感器数据不同步的问题。
优化松耦合是基于残差函数，使用函数关于状态量的雅克比矩阵，对状态量初值进行迭代求增量的方式。需要单独构造视觉误差项和惯导误差项，然后通过光束平差求解器对这两个误差项进行迭代优化得到状态量。
因此，无论是滤波松耦合还是优化松耦合，松耦合的特点是计算复杂度低，而且传感器数据单独处理再融合的方式可以有效提高算法的扩展性。但是，松耦合忽略了传感器数据之间的相关性，低精度传感器的噪声会在融合阶段影响到高精度传感器的数据，使得估计的误差变大，造成得到的惯性导航数据的精度降低。
2、紧耦合方法是将传感器数据进行联合估计，考虑不同传感器之间的相关性形成互相的约束，以此来降低估计的误差。基于扩展卡尔曼滤波的紧耦合方法，特征点信息会给入特征向量，多个特征点约束一个状态向量(位姿速度)。但是，该方法不仅计算量大，而且需要给定特征点的初始深度和初始协方差，如果初始条件给定不正确，极易导致之后的迭代不收敛，无法得到准确的位姿估计，且基于滤波的方法，存在无法消除线性化误差的问题。
3、优化紧耦合方法，尤其是非线性优化方法，可以提供更高的精度，但计算量较大。
综上所述，现有的采用视觉+IMU的感知方法存在计算量大，实时性低的问题，且无法解决快速运动的问题，仅能产生30Hz的运动状态感知信息，无法实现无人机和仿人机器人等运动载体的500Hz的高动态的运动状态感知需求。
发明内容
为解决现有技术存在的上述缺陷，本发明提出了一种用于运动载体的高动态运动状态感知系统及感知方法，针对无人机、机器人等运动载体执行高动态任务时，位姿状态难以高频率精确感知高动态运动状态的问题，采用双目视觉、IMU、高度计和倾斜仪的多传感器的信息融合，以及多传感器信息融合延迟自适应补偿，使得无人机、机器人等运动载体能够在执行500Hz的高动态任务时，位姿状态能够以高频率精确感知运动状态，为无人机等运动载体在路径规划和飞行控制对提供高频率精确的导航信息，也可解决机器人等运动载体在执行跑跳任务过程中由于高动态、间歇性大震动工况引起的位姿和感知精度低、可靠性差等问题。
本发明提供了一种用于运动载体的高动态运动状态感知系统，该系统包括：摄像头、惯性传感器、倾斜仪、高度计、融合感知模块、动态补偿模块和接口模块；
所述接口模块分别与惯性传感器、倾斜仪、高度计、融合感知模块、动态补偿模块通信连接；摄像头与融合感知单元通信连接；
所述摄像头，用于实时采集30Hz的图像数据，并将发送至融合感知模块；
所述惯性传感器，用于实时采集3600Hz的IMU数据，并将其发送至动态补偿模块；
所述倾斜仪，用于实时采集500Hz的倾角数据，并将其作为初始值发送至动态补偿模块；
所述高度计，用于实时采集运动载体的50Hz测量高度值，将其发送至动态补偿模块；
所述融合感知模块，用于对接收的30Hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；
根据动态补偿模块发送的降频后的IMU数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块；
所述动态补偿模块，用于对接收的3600Hz的IMU数据进行预处理和降频处理，获得降频后的IMU数据，并将其发送至融合感知模块；
还用于对降频后的IMU数据进行IMU状态递推，得到惯性导航信息；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；
根据得到的补偿后的视觉导航数据和高度计所采集的数据，对IMU状态递推后的惯性导航信息进行融合修正，得到修正后的数据，并输出。
作为上述技术方案的改进之一，所述融合感知模块包括：
图像处理与特征提取单元，用于根据接收的30Hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；和
状态感知单元，用于根据动态补偿模块发送的降频后的IMU数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块。
作为上述技术方案的改进之一，所述根据接收的30Hz的图像数据和动态补偿模块发送的降频后的IMU数据进行图像处理，获得处理后的图像数据；其具体过程为：
对于接收的30Hz的图像数据中的各图像帧，按照时间戳的时间顺序进行排序，得到具有连续性的图像数据，并对其中的每一帧图像进行直方图均衡处理，消除图像因亮度不适宜造成的影响，得到连续图像帧；
其中，该接收的30Hz的图像数据包括：摄像头采集的30Hz的图像帧及其对应的图像帧的时间戳；
对连续图像帧中的每一帧图像分别进行特征点提取和相邻帧特征点匹配，得到特征点对；
具体地，在第i帧图像上选取像素点P，并在该像素点P的周围选择M个像素点进行测试；
如果满足在M个像素点中存在N个连续像素点的灰度值均高于IP+t，或者满足在M个像素点中存在N个连续像素点的灰度值均低于IP-t，则像素点p为角点，并将其作为特征点；
其中，M和N均为正整数，M大于N，N除以M的比值的百分比为80％，IP为像素点P的灰度值；t为预先设定的阈值；
如果满足在M个像素点中存在N个连续像素点的灰度值均小于或等于IP+t，或者满足在M个像素点中存在N个连续像素点的灰度值均大于或等于IP-t，则像素点P删除，再继续寻找下一个像素点，重复上述过程；直至找到满足要求的像素点P，并将其作为特征点；
然后采用最邻近搜索法，在相邻帧中的第i帧和第i+1帧图像，进行特征点匹配，获得特征点对；
遍历每一图像帧，对每一图像帧进行上述处理，得到多对特征点对；
利用对极几何约束方程：
qTK-T[t]×RK-1p＝0
其中，qT为像素点P2的像素齐次坐标；K-T为相机内参转置取逆；[t]×为O2处相机原点相对于O1处相机的位移向量的反对称矩阵；R为O2处相机原点相对于O1处相机的姿态旋转矩阵；K-1为相机内参矩阵的逆；p为像素点P1的像素齐次坐标；K-T[t]×RK-1的结果称为基本矩阵；
对得到的特征点对进行筛选，遍历每一对特征点对，将每一对特征点对的坐标带入对极几何约束方程中，剔除不满足该极几何约束方程的特征点，得到多个筛选后的特征点；
对多个筛选后的特征点对中的部分特征点进行屏蔽，使剩余的特征点均匀分布，得到多个有效特征点，进而得到每帧图像的有效特征点的像素坐标；
对每个有效特征点的像素坐标进行去畸变和归一化处理，得到多个归一化处理后的特征点，及其对应的处理后的特征点坐标；
并根据每个处理后的特征点，以及每个处理后的特征点在左、右目图像中的视差uLuR，计算每个处理后的特征点的深度Z：
其中，uL为像素坐标系下的左目图像的像素水平坐标；uR为像素坐标系下的右目图像的像素水平坐标；uLuR为视差；fx为相机内参数；b为基线长度；
针对降频后的IMU数据，按照时间戳的时间顺序进行排序，得到具有连续性的IMU数据及其对应的IMU的测量值
从第i时刻的位置、速度和姿态四元数开始，对IMU的测量值进行积分，得到第j时刻的位置、速度和姿态四元数。
其中，为第j时刻预先建立的IMU本体系原点相对于定义的世界坐标系原点的位移；/>为第i时刻预先建立的IMU本体系原点相对于定义的世界坐标系原点的位移；/>为第i时刻预先建立的IMU本体系原点在定义的世界坐标系中的速度；Δt为第i时刻到第j时刻的时间差；/>为t时刻预先建立的IMU本体系相对于定义的世界坐标系的旋转四元数；为t时刻IMU加速度计的量测值，即实时加速度；gw为重力加速度向量，取[0 0 -9.81]T；为第j时刻预先建立的IMU本体系原点在定义的世界坐标系中的速度；/>为第j时刻预先建立的IMU本体系相对于定义的世界坐标系的旋转四元数；/>为四元数做向量积符号；
其中，假设IMU的真实值为ω,a，IMU的测量值为则有：
其中，为IMU的测量值中的陀螺仪测量的角速度；ωb为IMU本体系真实角速度；bg为陀螺仪的零漂误差；ng为陀螺仪的量测噪声；
其中，为IMU的测量值中的加速度计的量测值；qbw为IMU本体系相对于定义世界系的姿态四元数；aw为IMU本体系在定义世界系的惯性加速度；ba为加速度计的零漂误差；na为加速度计的量测噪声；
根据IMU的测量值，得到第i时刻到第j时刻的位置、速度和姿态四元数预积分分量：
其中，为IMU的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的位置变化量预积分分量；/>为IMU的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的速度变化量预积分分量；/>为IMU的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的姿态四元数预积分分量；
将IMU的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的位置变化量预积分分量速度变化量预积分分量/>和姿态四元数预积分分量/>作为第i时刻到第j时刻的位置、速度和姿态四元数预积分分量；
将每帧图像的有效特征点的像素坐标、多个处理后的归一化坐标、每个处理后的特征点的深度，以及从第i时刻到第j时刻的位置、速度和姿态四元数预积分部分作为处理后的图像数据。
作为上述技术方案的改进之一，所述根据动态补偿模块发送的降频后的IMU数据，结合处理后的图像数据，计算运动载体的位移、速度和姿态四元数，得到视觉导航数据，并将其输入至动态补偿模块；其具体过程为：
假设处理后的图像数据中包括M帧图像，从其第一帧图像开始，分别获取第2帧、第3帧…，第M帧与第一帧图像的特征点视差，并判断其得到的每个视差是否大于或等于预先设定的关键阈值；
如果其得到的视差大于或等于预先设定的关键阈值，则将第M帧作为关键帧，并统计关键帧的数量，直至达到10帧结束；
如果其得到的视差小于预先设定的关键阈值，则跳过当前帧，判断下一帧与第一帧的视差，直至找到下一个关键帧；
设定滑动窗口包括10帧关键帧，在滑动窗口内，计算每一帧到第11帧关键帧的视差，将具有最大视差的一帧作为参考帧，标记为第l帧，其中，1≤l≤10；根据基本矩阵K-T[t]×RK-1，解算第11帧到第l帧的旋转矩阵和平移向量/>并将其作为图像帧相对相机系的位姿关系；
将第11帧和第l帧组成特征点对，再结合上述所获得的图像帧相对相机系的位姿关系，三角化求解该特征点对在图像帧中所对应的有效特征点在第l帧相机系中的三维坐标；
根据求解得到的三维坐标和该有效特征点在其他帧中的归一化坐标，使用PnP问题，求解第l帧相机系到其他帧时刻相机系的旋转矩阵和平移向量，即第l帧相机系到其他帧时刻相机系的位姿关系；
重复上述过程，三角化求解滑动窗口内的各帧的有效特征点在第l帧相机系的三维坐标[xc yc zc]T；xc为三角化求解-的有效特征点在相机系x轴的坐标；yc为三角化求解的-有效特征点在相机系y轴的坐标；zc为三角化求解的有效特征点在相机系z轴的坐标，即深度信息；
由于三角化求取的三维坐标与真实值有误差，且后续会涉及到逆深度优化，此处将三角化求得的有效特征点的深度zc与每个处理后的特征点的深度Z取平均值xc和yc等比例伸缩；
采用优化工具库ceres，对三角化求解的滑动窗口内各帧的有效特征点在第l帧相机系的三维坐标进行BA(Bundle Adjustment，光束平差)优化，得到优化的滑动窗口内的各帧到第l帧的旋转矩阵和平移向量/>
对于获取的每个图像帧，利用和/>计算各帧时刻对应的IMU的本体系到第l帧的旋转矩阵/>
使用求解到的旋转矩阵作为约束，估计IMU的角速度的偏移量bias；依次取和两帧之间的位姿四元数预积分值/>最小化目标函数：
将式(1)带入上述的目标函数中：
其中，是使用量测值得到的两帧之间的位姿四元数预积分量；/>是预积分量关于陀螺仪的零漂bg的雅克比矩阵，
得到方程：
其中，()vec表示取四元数的虚部；
根据上述方程，计算陀螺仪零漂bg的误差δbg；
利用IMU的量测值和解算的δbg重新对和/>进行预积分，更新预积分分量和/>的结果，并使用误差传播方程，递推bg，得到窗口内每一关键帧处对应的IMU的bg；
将作为IMU的平移约束，估计重力方向、相机平移关系的尺度因子，利用重力加速度gw的模长优化第l帧的重力加速度/>通过第l帧到世界系的旋转矩阵/>将gcl旋转到gw，定义世界系原点与第1帧相机系重合，使用/>和/>递推IMU本体系相对于世界系的旋转矩阵；
基于上述过程，利用世界系作为过渡，能够实现相机系与IMU本体系对齐，并计算出各帧相机系相对于世界系的旋转矩阵和平移向量，由图像帧对IMU的偏移量作为约束，解算陀螺仪的零漂bg，并且带入预积分公式，重新对IMU的加速度和角速度量测值进行积分递推出各帧处IMU本体系相对于世界系的位置、速度、姿态四元数；
将由IMU数据递推出的相对于世界系的位置pwb，姿态四元数qwb，相对于世界系的速度加速度计零漂和陀螺仪零漂组成的非线性偏移量(ba，bg)，相机系与IMU本体系之间旋转矩阵/>相机系原点和IMU本体系原点的相对位移/>有效特征点对应空间点的逆深度λ，作为状态量的初值/>建立视觉量测残差和IMU预积分残差函数。
视觉残差函数：
其中，和/>是由第i帧图像有效特征点的归一化坐标，通过两帧图像对应的相机系的旋转平移关系解算得到的在第j帧上对应的有效特征点的相机系坐标，/>为第j帧对应的有效特征点直接解算得到的归一化坐标；
IMU预积分分量残差函数：
总的残差函数：
F＝ρ1rc(x)+ρ2rb(x)
其中，ρ1和ρ2是两部分残差的权重；
根据公式(2)，计算状态增量Δx：
JTf+JTJΔx＝0 (2)
其中，JT为各残差关于优化状态量的雅克比矩阵的转置；f为残差函数在状态量初值处的值组成的向量；J为各残差关于优化状态量的雅克比矩阵；
将计算得到的状态增量Δx，与状态量初值进行广义加法运算：
Δx1＝Δx+x
得到优化后的状态量Δx1；
滑动窗口移动后，根据优化后的状态量，得到不同时刻运动载体相对于起始时刻的位移、姿态四元数和实时速度，并将其组成视觉导航数据，并输入至动态补偿模块：
其中，rp为相对初始地点的位置；rq为相对初始地点的姿态四元数；rv为相对初始地点的速度。
作为上述技术方案的改进之一，所述动态补偿模块包括：预处理单元和修正单元；
所述预处理单元，用于对接收的3600Hz的IMU数据经过滤波器进行平滑预处理后，再依次顺序进行圆锥误差补偿，划桨误差补偿，以及初始偏差校正，得到校正后的数据，再将其降频至500Hz，获得降频后的IMU数据，并将其发送至融合感知模块；
所述修正单元，用于对降频后的IMU数据进行状态递推，得到惯性导航数据；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；根据得到的补偿后的视觉导航数据和高度计所采集的数据，对惯性导航数据进行修正，得到修正后的数据，并输出。
作为上述技术方案的改进之一，所述对降频后的IMU数据进行IMU状态递推，得到惯性导航数据；其具体过程为：
对降频后的IMU数据进行拆分，得到降频后的旋转角速度信息和降频后的比力信息；
利用IMU中的陀螺仪，获取运动载体的降频后的旋转角速度信息(ωx,ωy,ωz)，并对该运动载体的降频后的旋转角速度信息进行状态递推：
其中，为载体坐标系到地理坐标系的变换矩阵：/>为地理坐标系到载体坐标系的变换矩阵；
其中，p为俯仰角；r为滚转角；A为方位角；
为陀螺仪测量矢量的反对称矩阵：
为地球绕自转轴的旋转(ωe＝15°/h)在地理坐标系中产生的角速度的反对称矩阵：
为地理坐标系相对地心地固坐标系的角速度的反对称矩阵：
其中，为大地纬度，RN为地球的卯酉半径，RM为子午半径；ve为东向速度分量，vn为北向速度分量；h为高度。
根据上述公式，对该运动载体的降频后的旋转角速度信息进行状态递推，得到递推更新到当前时刻的并根据公式，计算得到初始的姿态角(p,r,A)：
利用IMU中的加速度计，获取运动载体的降频后的比力信息(fx,fy,fz)，并对其进行状态递推：
其中，为地理坐标系中的运动载体的速度的导数；vl为地理坐标系中的运动载体的速度；vl＝[ve,vn,vu]T，ve为东向速度分量，vn为北向速度分量，vu为天向速度分量；fb为加速度计的测量值，称为比力，fb＝[fx,fy,fz]T；gl为地球重力，gl＝[0,0,-g]T；
利用上式，对运动载体的降频后的比力信息(fx,fy,fz)进行状态递推，得到运动载体的初始速度信息vl＝[ve,vn,vu]T；
其中，该初始的速度信息包括：北向速度分量、天向速度分量和动向速度分量；
再根据公式，对得到的运动载体的初始速度信息vl＝[ve,vn,vu]T进行状态递推，得到运动载体的初始位置信息：
其中，为运动载体的纬度；λ为运动载体的经度；h为运动载体的惯性导航高度；
上式进行递推更新，得到初始的位置信息，该位置信息包括：纬度、经度和高度；将初始位置信息、初始的速度信息和初始的姿态角信息，汇总成初始的惯性导航信息，并将其作为惯性导航数据。
作为上述技术方案的改进之一，所述对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；根据得到的补偿后的视觉导航数据和高度计所采集的数据，对惯性导航数据进行修正，得到修正后的数据，并输出；其具体过程为：
接收的视觉导航数据为：
其中，rp为相对初始地点的位置；rq为相对初始地点的姿态四元数；rv为相对初始地点的速度；
首先将接收的视觉导航数据转换到地理坐标系，得到转换后的视觉导航数据，目的是与前面得到的惯性导航信息保持一致：
其中，为转换后的相对初始地点的位置；/>为转换后的相对初始地点的姿态四元数；/>为转换后的相对初始地点的速度；
假设当前时刻为k，传输至动态补偿模块时的延迟为n，则延迟后的视觉导航数据为：
在动态补偿模块中，用当前时刻k的加速度计的测量值(fx,fy,fz)k和陀螺仪的测量值(ωx,ωy,ωz)k，对延迟后的视觉导航数据进行自适应递推补偿，其对应的递推过程与对降频后的IMU数据进行IMU状态递推的递推过程一致；
得到当前时刻k的补偿后的视觉导航数据：
将补偿后的视觉导航数据和高度计的测量值，与IMU状态递推的惯性导航数据做差，得到误差测量值；
完整的惯性导航误差包括位置、速度、姿态，以及加速度计零偏和陀螺漂移误差分量，表示为：
其中，表示位置误差矢量；/>为地球参考速度误差矢量；/>为姿态误差矢量；/>为陀螺仪误差矢量；/>为加速度计误差矢量；
利用卡尔曼滤波器对惯性导航误差进行最优估计：
由于卡尔曼滤波器需要进行离散迭代更新，当前迭代时刻k的误差状态量记为δxk，上一迭代时刻k-1的误差状态量记为δxk-1，为误差状态量预测值；/>为误差状态量的滤波值，其他参数的上下标与此保持一致；/>分别为误差状态预测值和误差状态滤波值的误差协方差矩阵；δzk为误差测量值；
其中，为视觉导航得到的纬度；λvis为视觉导航得到的经度；hvis为高度计测量的高度；ve,v is为视觉导航得到的东向速度；vn,v is为视觉导航得到的北向速度；vu,v is为视觉导航得到的天向速度；
Kk为增益矩阵，I为单位矩阵；Φk/k-1＝(I+FΔt)k-1表示从第k-1时刻到第k时刻的状态转移矩阵；其中，F为动态系数矩阵，包括惯性导航位置、速度、姿态，以及惯性器件的误差模型；Δt为离散步长；为Φk/k-1的转置；G是噪声分布矢量，包含状态矢量的方差：G＝[σr,1*3,σv,1*3,σε,1*3,σω,1*3,σf,1*3]，σ项分别是3*3维与位置、速度、姿态、陀螺漂移以及加速度计零偏有关的对角矩阵，Gk-1为k-1时刻的噪声分布矢量；/>为Gk-1的转置；Hk为tk时刻的测量矩阵；/>为Hk的转置；Rk，Qk分别为测量噪声协方差矩阵和系统噪声协方差矩阵；
通过上述估计，得到惯性导航误差δx；
根据卡尔曼滤波器估计的惯性导航误差δx，对惯性导航数据进行修正：
其中，x为惯性导航数据δx为估计的惯性导航的误差(仅使用前9个误差状态)/>为修正后的惯性导航数据，具体为：/>分别为修正后的纬度，经度，高度，东向速度，北向速度，天向速度，俯仰角，滚转角，方位角；/>
得到修正后的惯性导航数据，即修正后的数据，并以位置、速度和姿态的形式实现融合导航输出。
本发明还提供了一种用于运动载体的高动态运动状态感知方法，该方法包括：
所述摄像头实时采集30Hz的图像数据，并将发送至融合感知模块；
所述惯性传感器实时采集3600Hz的IMU数据，并将其发送至动态补偿模块；
所述倾斜仪实时采集500Hz的倾角数据，并将其作为初始值发送至动态补偿模块；
所述高度计实时采集运动载体的50Hz测量高度值，将其发送至动态补偿模块；
所述融合感知模块对接收的30Hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；
所述动态补偿模块对接收的3600Hz的IMU数据进行预处理和降频处理，获得降频后的IMU数据，并将其发送至融合感知模块；
融合感知模块根据动态补偿模块发送的降频后的IMU数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块；
动态补偿模块对降频后的IMU数据进行IMU状态递推，得到惯性导航信息；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；
根据得到的补偿后的视觉导航数据和高度计所采集的数据，对IMU状态递推后的惯性导航信息进行融合修正，得到修正后的数据，并输出。
本发明与现有技术相比的有益效果是：
1、本发明的系统的处理速度快，可以满足高频率的需求，鲁棒性强；
2、本发明的系统融合了IMU的高动态和视觉的长期稳定低漂移优点，将图像处理和视觉位姿解算结果以30Hz的频率对IMU 500Hz的递推结果进行校正，由于视觉传感器采集的图像具有长期稳定的特性，可以保证感知融合精度保持在一定范围之内，为载体提供可靠的高动态感知信息；
3、通过对摄像头采集的30Hz的视觉信息和降频后的数据，以及高度计采集的50Hz测量高度值，经过融合处理，由于在处理过程中后会产生延迟，传统的方法对其进行固定时长的补偿量，由于不同场景下根据图像复杂程度会产生不定时长的补偿，故传统方法的补偿会存在一定误差；本发明提出的信息融合延迟自适应补偿技术可消除这种延迟，当动态补偿模块接收到融合感知模块输出的状态估计值时，会根据两者的时间戳信息，使用缓冲的IMU数据对融合感知模块输出的状态估计值递推到当前时刻，从而可以准确消除融合感知模块的延迟。
附图说明
图1是本发明的一种用于运动载体的高动态运动状态感知系统的结构示意图；
图2是本发明的一种用于运动载体的高动态运动状态感知方法的流程图；
图3是本发明的本发明的一种用于运动载体的高动态运动状态感知方法的特征点提取的示意图；
图4是本发明的本发明的一种用于运动载体的高动态运动状态感知方法的获取对极几何约束方程的示意图。
具体实施方式
现结合附图对本发明作进一步的描述。
如图1所示，本发明提供了一种用于运动载体的高动态运动状态感知系统，该系统包括：摄像头、惯性传感器、倾斜仪、高度计、融合感知模块、动态补偿模块和接口模块；
所述接口模块分别与惯性传感器、倾斜仪、高度计、融合感知模块、动态补偿模块电性连接和通信连接；摄像头与融合感知单元电性连接和通信连接；
所述摄像头，用于实时采集30Hz的图像数据，并将发送至融合感知模块；
其中，在本实施例中，所述摄像头选取Omnivision的OV9282，该摄像头为全局快门相机，最大可以实现每秒120帧720P分辨率的拍摄，且具有低时延和高精度特性，能够适用于高动态感知的应用。
所述惯性传感器，用于实时采集3600Hz的IMU数据，并将其发送至动态补偿模块；
其中，在本实施例中，该惯性传感器选取霍尼韦尔的HG4930CS36，HG4930 S-Class惯性测量单元是一种MEMS(Micro-Electro-Mechanical System，微机电系统)IMU(Inertial measurement unit，惯性测量单元)，可以在不牺牲精度的情况下，降低延迟和提高带宽，并且不受国际武器贸易协定(ITAR)约束，非常适用于平台控制、导航、机器人等应用。
所述倾斜仪，用于实时采集500Hz的倾角数据，并将其作为初始值发送至动态补偿模块；
其中，在本实施例中，该倾斜仪选择MuRata的2轴MEMS倾斜仪SCA100T-D02。运动载体在地面上静止时，倾斜仪用于测量初始的俯仰角和滚转角，根据测量值，可以对惯性传感器中的加速度计和陀螺仪进行偏差的校正。
所述高度计，用于实时采集运动载体的高度，并将其发送至动态补偿模块；
其中，在本实施例中，高度计选取Garmin公司的LIDAR-Lite v3 HP，该器件为紧凑型高性能激光距离传感器，主要用于无人机，机器人，无人驾驶车辆的距离与高度测量。
所述融合感知模块，用于对接收的30Hz的图像数据进行图像处理，对其进行特征点提取，获得处理后的图像数据，并为视觉导航做准备；
再根据动态补偿模块发送的降频后的IMU数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块；
其中，所述融合感知模块进一步包括：
图像处理与特征提取单元，用于根据接收的30Hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；和
其中，所述图像处理与特征提取单元的具体处理过程为：
(1)对于接收的30Hz的图像数据中的各图像帧，按照时间戳的时间顺序进行排序，得到具有连续性的图像数据，并对其中的每一帧图像进行直方图均衡处理，消除图像因亮度不适宜造成的影响，得到连续图像帧；
其中，该接收的30Hz的图像数据包括：摄像头采集的30Hz的图像帧及其对应的图像帧的时间戳；
(2)对连续图像帧中的每一帧图像分别进行特征点提取和相邻帧特征点匹配，得到特征点对；
具体地，a、在第i帧图像上选取像素点P，并在该像素点P的周围选择M个像素点进行测试；
b、如果满足在M个像素点中存在N个连续像素点的灰度值均高于IP+t，或者满足在M个像素点中存在N个连续像素点的灰度值均低于IPt，则像素点p为角点，并将其作为特征点；
其中，M和N均为正整数，M大于N，N除以M的百分比为80％，IP为像素点P的灰度值；t为预先设定的阈值；
如果满足在M个像素点中存在N个连续像素点的灰度值均小于或等于IP+t，或者满足在M个像素点中存在N个连续像素点的灰度值均大于或等于IPt，则像素点P删除，再继续寻找下一个像素点，重复上述过程；直至找到满足要求的像素点P，并将其作为特征点；
然后采用最邻近搜索法，在相邻帧中的第i帧和第i+1帧图像，进行特征点匹配，获得特征点对；
遍历每一图像帧，对每一图像帧进行上述处理，得到多对特征点对。
(3)利用对极几何约束方程，对特征点对进行筛选，剔除不满足该极几何约束方程的特征点，得到筛选后的特征点；
具体地，使用相机模型内参K和匹配的特征点对进行基本矩阵解算：
如图4所示，其中，O1和O2分别是两个时刻相机的相机坐标系的原点，空间一点P在左边图片的投影为P1，在右边图片投影为P2，连接O1与P1，O2与P2，在三维空间交会于点P。O1，O2和P确定的平面为极平面，极平面与左右图片的平面所交得到的相交线l1和l2称为极线，连接O1和O2交左右像平面于点e1和e2。
若假设左侧相机系与世界系重合，P1的像素齐次坐标为p，P2的像素齐次坐标为q，那么有：
zclp＝KP
zcrq＝K(RP+t)
注意，如果取：
x1＝K-1p
x2＝K-1q
这里的x1和x2是相机系归一化平面上的点：
合并上式，则有：
两边同时左乘[t]×，相当于两侧做外积：
/>
两边同时点乘x2 T:
观察等式左侧，[t]×x2是一个与t和x2都垂直的向量，再与x2 T做内积时，结果为0，于是有：
x2 T[t]×Rx1＝0
重新带入p和q，得到：
qTK-T[t]×RK-1p＝0
该式即为对极几何约束。
基本矩阵定义为：
F＝K-T[t]×RK-1
利用对极几何约束方程，对得到的特征点对进行筛选，遍历每一对特征点对，将每一对特征点对的坐标带入对极几何约束方程中，剔除不满足该极几何约束方程的特征点，得到多个筛选后的特征点；
(4)为了加速解算过程，根据剔除结果，对多个筛选后的特征点对中的部分特征点进行屏蔽，使剩余的特征点均匀分布，得到多个有效特征点，进而得到每帧图像的有效特征点的像素坐标。
(5)一个空间中的特征点随着运动进行，从相机的视野中消失，前后图像帧没有匹配的特征点对，针对该情况加入新的特征点进行追踪；
(6)对每个有效特征点的像素坐标进行去畸变和归一化处理，得到多个归一化处理后的特征点，及其对应的处理后的特征点坐标。
(7)并根据每个处理后的特征点，以及每个处理后的特征点在左、右目图像中的视差uLuR，计算每个处理后的特征点的深度Z：
如图3所示，假设像素点P在左、右目的成像平面上各成一点PL和PR；图3的方框为成像平面，成像平面在相机之前与像素平面一比一对应；左、右目相机的光心分别是OL和OR，f为相机焦距；Z表示像素点P的深度，即角点到相机左目光心OL的距离；b代表基线长度，也即处于同一水平面的双目相机左右目光心之间的距离。xL是像素点P到相机左目光心OL的距离在成像坐标系上该点的水平坐标，xR是像素点P到相机右目光心OR的距离在成像坐标系上该点的水平坐标(单位为：mm)。根据相似三角形的原理:
其中，(xLxR)是表示左、右目相同点的水平坐标差，即视差。由于像素坐标系和成像坐标系一一对应，像素坐标系坐标与成像坐标系坐标只存在一个α倍的坐标缩放和(cx,cy)的中心坐标偏移关系。这里用uL表示像素坐标系下的左目图像的像素水平坐标和uR表示像素坐标系下的右目图像的像素水平坐标；
d＝xL-xR
其中，fx为相机内参数；b为基线长度；
因此，建立双目视觉模型：
根据处理后的特征点在左、右目图像中的视差uLuR，计算该处理后的特征点的深度Z。
针对降频后的IMU数据，按照时间戳的时间顺序进行排序，得到具有连续性的IMU数据，
具有连续性的IMU数据包括IMU的真实值ω,a和IMU的测量值
从第i时刻的位置、速度和姿态四元数开始，对IMU的测量值进行积分，得到第j时刻的位置、速度和姿态四元数，即第j时刻的PVQ。
其中，为第j时刻预先建立的IMU本体系原点相对于定义的世界坐标系原点的位移；/>为第i时刻预先建立的IMU本体系原点相对于定义的世界坐标系原点的位移；/>为第i时刻预先建立的IMU本体系原点在定义的世界坐标系中的速度；Δt为第i时刻到第j时刻的时间差；/>为t时刻预先建立的IMU本体系相对于定义的世界坐标系的旋转四元数；/>为t时刻IMU加速度计的量测值，即实时加速度；gw为重力加速度向量，取[00-9.81]T；为第j时刻预先建立的IMU本体系原点在定义的世界坐标系中的速度；/>为第j时刻预先建立的IMU本体系相对于定义的世界坐标系的旋转四元数；/>为四元数做向量积符号；
其中，假设IMU的真实值为ω,a，IMU的测量值为则有：
其中，为IMU的测量值中的陀螺仪测量的角速度；ωb为IMU本体系真实角速度；bg为陀螺仪的零漂误差；ng为陀螺仪的量测噪声；
其中，a为IMU的测量值中的加速度计的量测值；qbw为IMU本体系相对于定义世界系的姿态四元数；aw为IMU本体系在定义世界系的惯性加速度；ba为加速度计的零漂误差；na为加速度计的量测噪声；
由于IMU的测量值中含有噪声，需要对其进行优化，将优化后的IMU的测量进行重新积分，但由于数据量太大，因此，定义预积分分量，预积分分量只与IMU的测量值有关，根据IMU的测量值，得到第i时刻到第j时刻的位置、速度和姿态四元数预积分分量：
其中，为IMU的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的位置变化量预积分分量；/>为IMU的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的速度变化量预积分分量；/>为IMU的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的姿态四元数预积分分量；
将IMU的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的位置变化量预积分分量速度变化量预积分分量/>和姿态四元数预积分分量/>作为第i时刻到第j时刻的位置、速度和姿态四元数预积分分量；
将每帧图像的有效特征点的像素坐标、多个处理后的归一化坐标、每个处理后的特征点的深度，以及从第i时刻到第j时刻的位置、速度和姿态四元数预积分部分作为处理后的图像数据。
其中，特征点提取是通过提取处理后的图像数据中的特征点，使用描述字进行描述区分后做帧间匹配。特征点的描述字具有高区分度等特点，对环境具有更强的适应能力。
其中，在本实施例中，该融合感知模块可以采用一个四核64位的ARM Cortex-A53MPCore应用处理器；在其中，还采用了一个ARM Mali-400 MP2 GPU，可用于视频解码，用于图像处理和视觉导航中的处理。
状态感知单元，用于根据动态补偿模块发送的降频后的IMU数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块。
其中，所述状态感知单元的具体处理过程为：
联合视觉、IMU信息，标定相机与IMU本体坐标系之间的旋转矩阵相对位移/>通过实际的安装尺寸确定；假设处理后的图像数据中包括M帧图像，从其第一帧图像开始，分别获取第2帧、第3帧…，第M帧与第一帧图像的特征点视差，并判断其得到的每个视差是否大于或等于预先设定的关键阈值；
如果其得到的视差大于或等于预先设定的关键阈值，则将第M帧作为关键帧，并统计关键帧的数量，直至达到10帧结束；
如果其得到的视差小于预先设定的关键阈值，则跳过当前帧，判断下一帧与第一帧的视差，直至找到下一个关键帧；
设定滑动窗口包括10帧关键帧，在滑动窗口内，计算每一帧到第11帧关键帧的视差，将具有最大视差的一帧作为参考帧，标记为第l帧，其中，1≤l≤10；根据基本矩阵K-T[t]×RK-1，解算第11帧到第l帧的旋转矩阵和平移向量/>并将其作为图像帧相对相机系的位姿关系；
将第11帧和第l帧组成特征点对，再结合上述所获得的图像帧相对相机系的位姿关系，三角化求解该特征点对在图像帧中所对应的有效特征点在第l帧相机系中的三维坐标；
根据求解得到的三维坐标和该有效特征点在其他帧中的归一化坐标，使用PnP(Perspective N Points)问题，求解第l帧相机系到其他帧时刻相机系的旋转矩阵和平移向量，即第l帧相机系到其他帧时刻相机系的位姿关系；
重复上述过程，三角化求解滑动窗口内的各帧的有效特征点在第l帧相机系的三维坐标[xc yc zc]T；xc为三角化求解-的有效特征点在相机系x轴的坐标；yc为三角化求解的-有效特征点在相机系y轴的坐标；zc为三角化求解的有效特征点在相机系z轴的坐标，即深度信息；
由于三角化求取的三维坐标与真实值有误差，且后续会涉及到逆深度优化，此处将三角化求得的有效特征点的深度zc与每个处理后的特征点的深度Z取平均值xc和yc等比例伸缩；
采用工具箱ceres，对三角化求解滑动窗口内的各帧的有效特征点在第l帧相机系的三维坐标进行BA(Bundle Adjustment，光束平差)优化，得到优化的滑动窗口内的各帧到第l帧的旋转矩阵和平移向量/>
对于获取的每个图像帧，利用和/>计算各帧时刻对应的IMU的本体系到第l帧的旋转矩阵/>
使用求解到的旋转矩阵作为约束，估计IMU的角速度的偏移量bias；依次取和两帧之间的位姿四元数预积分值/>最小化目标函数：
将式(1)带入上述的目标函数中：
其中，是使用量测值得到的两帧之间的位姿四元数预积分量；/>是预积分量关于陀螺仪的零漂bg的雅克比矩阵，
得到方程：
其中，()vec表示取四元数的虚部；
根据上述方程，计算陀螺仪零漂bg的误差δbg；
利用IMU的量测值和解算的δbg重新对和/>进行预积分，更新预积分分量和/>的结果，并使用误差传播方程，递推bg，得到窗口内每一关键帧处对应的IMU的bg；
将作为IMU的平移约束，估计重力方向、相机平移关系的尺度因子，利用重力加速度gw的模长优化第l帧的重力加速度/>通过第l帧到世界系的旋转矩阵/>将/>旋转到gw，定义世界系原点与第1帧相机系重合，使用/>和/>递推IMU本体系相对于世界系的旋转矩阵；
基于上述过程，利用世界系作为过渡，能够实现相机系与IMU本体系对齐，并计算出各帧相机系相对于世界系的旋转矩阵和平移向量，由图像帧对IMU的偏移量bias作为约束，解算陀螺仪的零漂bias(bg)，并且带入预积分公式，重新对IMU的加速度和角速度量测值进行积分递推出各帧处IMU本体系相对于世界系的位置、速度、姿态四元数；
将由IMU数据递推出的相对于世界系的位置pwb，姿态四元数qwb，相对于世界系的速度加速度计零漂和陀螺仪零漂组成的非线性偏移量(ba，bg)，相机系与IMU本体系之间旋转矩阵/>相机系原点和IMU本体系原点的相对位移/>有效特征点对应空间点的逆深度λ，作为状态量的初值/>建立视觉量测残差和IMU预积分残差函数。
视觉残差函数：
其中，/>和/>是由第i帧图像有效特征点的归一化坐标，通过两帧图像对应的相机系的旋转平移关系解算得到的在第j帧上对应的有效特征点的相机系坐标，/>为第j帧对应的有效特征点直接解算得到的归一化坐标。
IMU预积分分量残差函数：
总的残差函数：
F＝ρ1rc(x)+ρ2rb(x)
其中，ρ1和ρ2是两部分残差的权重。
根据公式(2)，计算状态增量Δx：
JTf+JTJΔx＝0 (2)
其中，JT为各残差关于优化状态量的雅克比矩阵的转置；f为残差函数在状态量初值处的值组成的向量；J为各残差关于优化状态量的雅克比矩阵；
将计算得到的状态增量Δx，与状态量初值进行广义加法运算：
Δx1＝Δx+x
得到优化后的状态量Δx1；
滑动窗口移动后，根据优化后的状态量，得到不同时刻运动载体相对于起始时刻的位移、姿态四元数和实时速度，并将其组成视觉导航数据，输入至动态补偿模块：
其中，rp为相对初始地点的位置；rq为相对初始地点的姿态四元数；rv为相对初始地点的速度。
所述动态补偿模块，用于对接收的3600Hz的IMU数据进行预处理和降频处理，获得降频后的IMU数据，并将其发送至融合感知模块；
还用于对降频后的IMU数据进行IMU状态递推，得到惯性导航信息；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；
根据得到的补偿后的视觉导航数据和高度计所采集的数据，对IMU状态递推后的惯性导航信息进行融合修正，得到修正后的数据，并输出。
所述动态补偿模块包括：预处理单元和修正单元；
所述预处理单元，用于对接收的3600Hz的IMU数据经过滤波器进行平滑预处理后，再依次顺序进行圆锥误差补偿，划桨误差补偿，以及初始偏差校正(使用倾斜仪提供的初始陀螺仪偏差值和加速度偏差估计值校正)，得到校正后的数据，再将其降频至500Hz，获得降频后的IMU数据，并将其发送至融合感知模块；
所述修正单元，用于对降频后的IMU数据进行状态递推，得到惯性导航数据；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；根据得到的补偿后的视觉导航数据和高度计所采集的数据，对惯性导航数据进行修正，得到修正后的数据，并输出。
其中，所述对降频后的IMU数据进行IMU状态递推，得到惯性导航数据；其具体过程为：
对降频后的IMU数据进行拆分，得到降频后的旋转角速度信息和降频后的比力信息；
利用IMU中的陀螺仪，获取运动载体的降频后的旋转角速度信息(ωx,ωy,ωz)，并对该运动载体的降频后的旋转角速度信息进行状态递推：
/>
其中，为载体坐标系到地理坐标系的变换矩阵：/>为地理坐标系到载体坐标系的变换矩阵；
其中，p为俯仰角；r为滚转角；A为方位角；
为陀螺仪测量矢量的反对称矩阵：
为地球绕自转轴的旋转(ωe＝15°/h)在地理坐标系中产生的角速度的反对称矩阵：
为地理坐标系相对地心地固坐标系的角速度的反对称矩阵：
其中，为大地纬度，RN为地球的卯酉半径，RM为子午半径；ve为东向速度分量，vn为北向速度分量；h为高度。
根据上述公式，对该运动载体的降频后的旋转角速度信息进行状态递推，得到递推更新到当前时刻的并根据公式，计算得到初始的姿态角(p,r,A)：
利用IMU中的加速度计，获取运动载体的降频后的比力信息(fx,fy,fz)，并对其进行状态递推：
其中，为地理坐标系中的运动载体的速度的导数；vl为地理坐标系中的运动载体的速度；vl＝[ve,vn,vu]T，ve为东向速度分量，vn为北向速度分量，vu为天向速度分量；fb为加速度计的测量值，称为比力，fb＝[fx,fy,fz]T；gl为地球重力，gl＝[0,0,-g]T；
利用上式，对运动载体的降频后的比力信息(fx,fy,fz)进行状态递推，得到运动载体的初始速度信息vl＝[ve,vn,vu]T；
其中，该初始的速度信息包括：北向速度分量、天向速度分量和动向速度分量；
再根据公式，对得到的运动载体的初始速度信息vl＝[ve,vn,vu]T进行状态递推，得到运动载体的初始位置信息：
其中，为运动载体的纬度；λ为运动载体的经度；h为运动载体的惯性导航高度；
上式进行递推更新，得到初始的位置信息，该位置信息包括：纬度、经度和高度；将初始位置信息、初始的速度信息和初始的姿态角信息，汇总成初始的惯性导航信息，并将其作为惯性导航数据。
其中，所述对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；根据得到的补偿后的视觉导航数据和高度计所采集的数据，对惯性导航数据进行修正，得到修正后的数据，并输出；其具体过程为：
由于视觉导航会产生大量运算，融合感知模块将视觉导航数据传输回动态补偿模块时，可能会产生不定时长的微小延迟，故在动态补偿模块中，需对延迟进行自适应补偿，以提升系统的实时性。具体过程为：
接收的视觉导航数据为：
其中，rp为相对初始地点的位置；rq为相对初始地点的姿态四元数；rv为相对初始地点的速度；
首先将接收的视觉导航数据转换到地理坐标系，得到转换后的视觉导航数据，目的是与前面得到的惯性导航信息保持一致：
/>
其中，为转换后的相对初始地点的位置；/>为转换后的相对初始地点的姿态四元数；/>为转换后的相对初始地点的速度；
假设当前时刻为k，传输至动态补偿模块时的延迟为n，则延迟后的视觉导航数据为：
在动态补偿模块中，用当前时刻k的加速度计的测量值(fx,fy,fz)k和陀螺仪的测量值(ωx,ωy,ωz)k，对延迟后的视觉导航数据进行自适应递推补偿，其对应的递推过程与对降频后的IMU数据进行IMU状态递推的递推过程一致；
得到当前时刻k的补偿后的视觉导航数据：
将补偿后的视觉导航数据和高度计的测量值，与IMU状态递推的惯性导航数据做差，得到误差测量值；
完整的惯性导航误差包括位置、速度、姿态，以及加速度计零偏和陀螺漂移误差分量，表示为：
其中，表示位置误差矢量；/>为地球参考速度误差矢量；/>为姿态误差矢量；/>为陀螺仪误差矢量(含漂移)；/>为加速度计误差矢量(含零偏)。
利用卡尔曼滤波器对惯性导航误差进行最优估计：
由于卡尔曼滤波器需要进行离散迭代更新，当前迭代时刻k的误差状态量记为δxk，上一迭代时刻k-1的误差状态量记为δxk-1，为误差状态量预测值；/>为误差状态量的滤波值，其他参数的上下标与此保持一致；/>分别为误差状态预测值和误差状态滤波值的误差协方差矩阵；δzk为误差测量值；/>
其中，为视觉导航得到的纬度；λvis为视觉导航得到的经度；hvis为高度计测量的高度；ve,vis为视觉导航得到的东向速度；vn,vis为视觉导航得到的北向速度；vu,vis为视觉导航得到的天向速度；
Kk为增益矩阵，I为单位矩阵；Φk/k-1＝(I+FΔt)k-1表示从第k-1时刻到第k时刻的状态转移矩阵；其中，F为动态系数矩阵，包括惯性导航位置、速度、姿态，以及惯性器件的误差模型；Δt为离散步长；为Φk/k-1的转置；G是噪声分布矢量，包含状态矢量的方差：σ项分别是3*3维与位置、速度、姿态、陀螺漂移以及加速度计零偏有关的对角矩阵，Gk-1为k-1时刻的噪声分布矢量；/>为Gk-1的转置；Hk为tk时刻的测量矩阵；/>为Hk的转置；Rk，Qk分别为测量噪声协方差矩阵和系统噪声协方差矩阵；
通过上述估计，得到惯性导航误差δx；
根据卡尔曼滤波器估计的惯性导航误差δx，对惯性导航数据进行修正：
其中，x为惯性导航数据δx为估计的惯性导航的误差(仅使用前9个误差状态)/>为修正后的惯性导航数据，具体为：/>分别为修正后的纬度，经度，高度，东向速度，北向速度，天向速度，俯仰角，滚转角，方位角；
得到修正后的惯性导航数据，即修正后的数据，并以位置、速度和姿态的形式实现融合导航输出。
其中，在本实施例中，该动态补偿模块可以采用一个双核32位的ARM Cortex-R5MPCore实时处理器，保证了同步操作；
由于视觉摄像头采集的图像具有长期稳定的特性，可以保证感知融合精度保持在一定范围之内。另外，在动态补偿模块和融合感知模块中对降频后的图像数据进行递推的递推结果进行冗余处理，用于补偿融合感知模块使用异步通讯造成的时延与不均匀数据包到达的时间不一致性。
融合感知模块融合了摄像头采集的30Hz的图像数据，动态补偿模块发送的预处理后的图像数据，生成完整的状态估计值，由于其运行在一个非实时的系统上，即使在最佳情况下，这些状态估计值传输至动态补偿模块时也会延迟几毫秒，因此，需要采用信息融合延迟自适应补偿方法，可以保证发送至动态补偿模块的状态估计值所对应的时刻和降频后的图像数据进行递推后得到的位姿所对应的时刻对齐，属于同一时刻，保证感知系统实时运行。
具体实现过程如下：在动态补偿模块和融合感知模块中对IMU递推过程进行冗余处理，当动态补偿模块接收到融合感知模块输出的视觉导航状态估计值时，会根据两者的时间戳信息，使用降频后的IMU数据，对融合感知模块输出的状态递推到当前时刻，从而可以消除融合感知模块的延迟。此方案对融合感知模块失效或者故障也具备一定的鲁棒性。
本申请采用了高动态运动状态感知技术和信息融合延迟自适应补偿技术，可为运动载体提供500Hz高动态的运动状态感知信息；例如，可对无人机进行快速路径规划和复杂飞行控制提供导航信息，或者对仿人机器人在执行跑跳等高速、大震动的任务提供高动态运动状态感知信息。
如图2所示，本发明还提供了一种用于运动载体的高动态运动状态感知方法，该方法包括：
所述摄像头实时采集30Hz的图像数据，并将发送至融合感知模块；
所述惯性传感器实时采集3600Hz的IMU数据，并将其发送至动态补偿模块；
所述倾斜仪实时采集500Hz的倾角数据，并将其作为初始值发送至动态补偿模块；
所述高度计实时采集运动载体的50Hz测量高度值，将其发送至动态补偿模块；
所述融合感知模块对接收的30Hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；
所述动态补偿模块对接收的3600Hz的IMU数据进行预处理和降频处理，获得降频后的IMU数据，并将其发送至融合感知模块；
融合感知模块根据动态补偿模块发送的降频后的IMU数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块；
动态补偿模块对降频后的IMU数据进行IMU状态递推，得到惯性导航信息；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；
根据得到的补偿后的视觉导航数据和高度计所采集的数据，对IMU状态递推后的惯性导航信息进行融合修正，得到修正后的数据，并输出。
最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (4)

1.一种用于运动载体的高动态运动状态感知系统，其特征在于，该系统包括：摄像头、惯性传感器、倾斜仪、高度计、融合感知模块、动态补偿模块和接口模块；
所述接口模块分别与惯性传感器、倾斜仪、高度计、融合感知模块、动态补偿模块通信连接；摄像头与融合感知单元通信连接；
所述摄像头，用于实时采集30Hz的图像数据，并将发送至融合感知模块；
所述惯性传感器，用于实时采集3600Hz的IMU数据，并将其发送至动态补偿模块；
所述倾斜仪，用于实时采集500Hz的倾角数据，并将其作为初始值发送至动态补偿模块；
所述高度计，用于实时采集运动载体的50Hz测量高度值，将其发送至动态补偿模块；
所述融合感知模块，用于对接收的30Hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；
再根据动态补偿模块发送的降频后的IMU数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块；
所述动态补偿模块，用于对接收的3600Hz的IMU数据进行预处理和降频处理，获得降频后的IMU数据，并将其发送至融合感知模块；
还用于对降频后的IMU数据进行IMU状态递推，得到惯性导航信息；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；
根据得到的补偿后的视觉导航数据和高度计所采集的数据，对IMU状态递推后的惯性导航信息进行融合修正，得到修正后的数据，并输出；
所述根据动态补偿模块发送的降频后的IMU数据，结合处理后的图像数据，计算运动载体的位移、速度和姿态四元数，得到视觉导航数据，并将其输入至动态补偿模块；其具体过程为：
假设处理后的图像数据中包括M帧图像，从其第一帧图像开始，分别获取第2帧、第3帧…，第M帧与第一帧图像的特征点视差，并判断其得到的每个视差是否大于或等于预先设定的关键阈值；
如果其得到的视差大于或等于预先设定的关键阈值，则将第M帧作为关键帧，并统计关键帧的数量，直至达到10帧结束；
如果其得到的视差小于预先设定的关键阈值，则跳过当前帧，判断下一帧与第一帧的视差，直至找到下一个关键帧；
设定滑动窗口包括10帧关键帧，在滑动窗口内，计算每一帧到第11帧关键帧的视差，将具有最大视差的一帧作为参考帧，标记为第l帧，其中，1≤l≤10；根据基本矩阵K-T[t]×RK-1，解算第11帧到第l帧的旋转矩阵和平移向量/>并将其作为图像帧相对相机系的位姿关系；其中，K-T为相机内参转置取逆，[t]×为O2处相机原点相对于O1处相机的位移向量的反对称矩阵；R为O2处相机原点相对于O1处相机的姿态旋转矩阵；K-1为相机内参矩阵的逆；
将第11帧和第l帧组成特征点对，再结合上述所获得的图像帧相对相机系的位姿关系，三角化求解该特征点对在图像帧中所对应的有效特征点在第l帧相机系中的三维坐标；
根据求解得到的三维坐标和该有效特征点在其他帧中的归一化坐标，使用PnP问题，求解第l帧相机系到其他帧时刻相机系的旋转矩阵和平移向量，即第l帧相机系到其他帧时刻相机系的位姿关系；
重复上述过程，三角化求解滑动窗口内的各帧的有效特征点在第l帧相机系的三维坐标[xc yc zc]T；xc为三角化求解-的有效特征点在相机系x轴的坐标；yc为三角化求解的-有效特征点在相机系y轴的坐标；zc为三角化求解的有效特征点在相机系z轴的坐标，即深度信息；
由于三角化求取的三维坐标与真实值有误差，且后续会涉及到逆深度优化，此处将三角化求得的有效特征点的深度zc与每个处理后的特征点的深度Z取平均值xc和yc等比例伸缩；
采用优化工具库ceres，对三角化求解的滑动窗口内各帧的有效特征点在第l帧相机系的三维坐标进行光束平差优化，得到优化的滑动窗口内的各帧到第l帧的旋转矩阵和平移向量/>
对于获取的每个图像帧，利用和/>计算各帧时刻对应的IMU的本体系到第l帧的旋转矩阵/>
使用求解到的旋转矩阵作为约束，估计IMU的角速度的偏移量bias；依次取/>和两帧之间的位姿四元数预积分值/>最小化目标函数：
将式(1)带入上述的目标函数中：
其中，是使用量测值得到的两帧之间的位姿四元数预积分量；/>是预积分量/>关于陀螺仪的零漂bg的雅克比矩阵，
得到方程：
其中，()vec表示取四元数的虚部；
根据上述方程，计算陀螺仪零漂bg的误差δbg；
利用IMU的量测值和解算的δbg重新对和/>进行预积分，更新预积分分量和/>的结果，并使用误差传播方程，递推bg，得到窗口内每一关键帧处对应的IMU的bg；
将作为IMU的平移约束，估计重力方向、相机平移关系的尺度因子，利用重力加速度gw的模长优化第l帧的重力加速度/>通过第l帧到世界系的旋转矩阵/>将/>旋转到gw，定义世界系原点与第1帧相机系重合，使用/>和/>递推IMU本体系相对于世界系的旋转矩阵；
基于上述过程，利用世界系作为过渡，能够实现相机系与IMU本体系对齐，并计算出各帧相机系相对于世界系的旋转矩阵和平移向量，由图像帧对IMU的偏移量作为约束，解算陀螺仪的零漂bg，并且带入预积分公式，重新对IMU的加速度和角速度量测值进行积分递推出各帧处IMU本体系相对于世界系的位置、速度、姿态四元数；
将由IMU数据递推出的相对于世界系的位置pwb，姿态四元数qwb，相对于世界系的速度加速度计零漂和陀螺仪零漂组成的非线性偏移量(ba，bg)，相机系与IMU本体系之间旋转矩阵/>相机系原点和IMU本体系原点的相对位移/>有效特征点对应空间点的逆深度λ，作为状态量的初值/>建立视觉量测残差和IMU预积分残差函数；
视觉残差函数：
其中，和/>是由第i帧图像有效特征点的归一化坐标，通过两帧图像对应的相机系的旋转平移关系解算得到的在第j帧上对应的有效特征点的相机系坐标，/>为第j帧对应的有效特征点直接解算得到的归一化坐标；
IMU预积分分量残差函数：
其中，为第j时刻预先建立的IMU本体系原点相对于定义的世界坐标系原点的位移；为第i时刻预先建立的IMU本体系原点相对于定义的世界坐标系原点的位移；/>为第i时刻预先建立的IMU本体系原点在定义的世界坐标系中的速度；Δt为第i时刻到第j时刻的时间差；gw为重力加速度向量，取[0 0 -9.81]T；/>为第j时刻预先建立的IMU本体系原点在定义的世界坐标系中的速度；/>为第j时刻预先建立的IMU本体系相对于定义的世界坐标系的旋转四元数；/>为四元数做向量积符号；
总的残差函数：
F＝ρ1rc(x)+ρ2rb(x)
其中，ρ1和ρ2是两部分残差的权重；
根据公式(2)，计算状态增量Δx：
JTf+JTJΔx＝0 (2)
其中，JT为各残差关于优化状态量的雅克比矩阵的转置；f为残差函数在状态量初值处的值组成的向量；J为各残差关于优化状态量的雅克比矩阵；
将计算得到的状态增量Δx，与状态量初值进行广义加法运算：
Δx1＝Δx+x
得到优化后的状态量Δx1；
滑动窗口移动后，根据优化后的状态量，得到不同时刻运动载体相对于起始时刻的位移、姿态四元数和实时速度，并将其组成视觉导航数据，并输入至动态补偿模块：
其中，rp为相对初始地点的位置；rq为相对初始地点的姿态四元数；rv为相对初始地点的速度；
所述动态补偿模块包括：预处理单元和修正单元；
所述预处理单元，用于对接收的3600Hz的IMU数据经过滤波器进行平滑预处理后，再依次顺序进行圆锥误差补偿，划桨误差补偿，以及初始偏差校正，得到校正后的数据，再将其降频至500Hz，获得降频后的IMU数据，并将其发送至融合感知模块；
所述修正单元，用于对降频后的IMU数据进行状态递推，得到惯性导航数据；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；根据得到的补偿后的视觉导航数据和高度计所采集的数据，对惯性导航数据进行修正，得到修正后的数据，并输出；
所述对降频后的IMU数据进行IMU状态递推，得到惯性导航信息；其具体过程为：
对降频后的IMU数据进行拆分，得到降频后的旋转角速度信息和降频后的比力信息；
利用IMU中的陀螺仪，获取运动载体的降频后的旋转角速度信息(ωx,ωy,ωz)，并对该运动载体的降频后的旋转角速度信息进行状态递推：
其中，为载体坐标系到地理坐标系的变换矩阵：/>为地理坐标系到载体坐标系的变换矩阵；
其中，p为俯仰角；r为滚转角；A为方位角；
为陀螺仪测量矢量的反对称矩阵：
为地球绕自转轴的旋转(ωe＝15°/h)在地理坐标系中产生的角速度的反对称矩阵：
为地理坐标系相对地心地固坐标系的角速度的反对称矩阵：
其中，为大地纬度，RN为地球的卯酉半径，RM为子午半径；ve为东向速度分量，vn为北向速度分量；h为高度；
根据上述公式，对该运动载体的降频后的旋转角速度信息进行状态递推，得到递推更新到当前时刻的并根据公式，计算得到初始的姿态角(p,r,A)：
利用IMU中的加速度计，获取运动载体的降频后的比力信息(fx,fy,fz)，并对其进行状态递推：
其中，为地理坐标系中的运动载体的速度的导数；vl为地理坐标系中的运动载体的速度；vl＝[ve,vn,vu]T，ve为东向速度分量，vn为北向速度分量，vu为天向速度分量；fb为加速度计的测量值，称为比力，fb＝[fx,fy,fz]T；gl为地球重力，gl＝[0,0,-g]T；
利用上式，对运动载体的降频后的比力信息(fx,fy,fz)进行状态递推，得到运动载体的初始速度信息vl＝[ve,vn,vu]T；
其中，该初始的速度信息包括：北向速度分量、天向速度分量和动向速度分量；
再根据公式，对得到的运动载体的初始速度信息vl＝[ve,vn,vu]T进行状态递推，得到运动载体的初始位置信息：
其中，为运动载体的纬度；λ为运动载体的经度；h为运动载体的惯性导航高度；
上式进行递推更新，得到初始的位置信息，该位置信息包括：纬度、经度和高度；将初始位置信息、初始的速度信息和初始的姿态角信息，汇总成初始的惯性导航信息，并将其作为惯性导航数据；
所述对接收的视觉导航数据进行动态补偿，得到补偿后的视觉导航数据；根据得到的补偿后的视觉导航数据和高度计所采集的数据，对惯性导航数据进行修正，得到修正后的数据，并输出；其具体过程为：
接收的视觉导航数据为：
其中，rp为相对初始地点的位置；rq为相对初始地点的姿态四元数；rv为相对初始地点的速度；
首先将接收的视觉导航数据转换到地理坐标系，得到转换后的视觉导航数据，目的是与前面得到的惯性导航信息保持一致：
其中，为转换后的相对初始地点的位置；/>为转换后的相对初始地点的姿态四元数；/>为转换后的相对初始地点的速度；
假设当前时刻为k，传输至动态补偿模块时的延迟为n，则延迟后的视觉导航数据为：
在动态补偿模块中，用当前时刻k的加速度计的测量值(fx,fy,fz)k和陀螺仪的测量值(ωx,ωy,ωz)k，对延迟后的视觉导航数据进行自适应递推补偿，其对应的递推过程与对降频后的IMU数据进行IMU状态递推的递推过程一致；
得到当前时刻k的补偿后的视觉导航数据：
将补偿后的视觉导航数据和高度计的测量值，与IMU状态递推的惯性导航数据做差，得到误差测量值；
完整的惯性导航误差包括位置、速度、姿态，以及加速度计零偏和陀螺漂移误差分量，表示为：
其中，表示位置误差矢量；/>为地球参考速度误差矢量；/>为姿态误差矢量；/>为陀螺仪误差矢量；为加速度计误差矢量；
利用卡尔曼滤波器对惯性导航误差进行最优估计：
由于卡尔曼滤波器需要进行离散迭代更新，当前迭代时刻k的误差状态量记为δxk，上一迭代时刻k-1的误差状态量记为δxk-1，为误差状态量预测值；/>为误差状态量的滤波值，其他参数的上下标与此保持一致；/>分别为误差状态预测值和误差状态滤波值的误差协方差矩阵；δzk为误差测量值；
其中，为视觉导航得到的纬度；λvis为视觉导航得到的经度；hvis为高度计测量的高度；ve,vis为视觉导航得到的东向速度；vn,vis为视觉导航得到的北向速度；vu,vis为视觉导航得到的天向速度；
Kk为增益矩阵，I为单位矩阵；Φk/k-1＝(I+FΔt)k-1表示从第k-1时刻到第k时刻的状态转移矩阵；其中，F为动态系数矩阵，包括惯性导航位置、速度、姿态，以及惯性器件的误差模型；Δt为离散步长；为Φk/k-1的转置；G是噪声分布矢量，包含状态矢量的方差：G＝[σr,1*3,σv,1*3,σε,1*3,σω,1*3,σf,1*3]，σ项分别是3*3维与位置、速度、姿态、陀螺漂移以及加速度计零偏有关的对角矩阵，Gk-1为k-1时刻的噪声分布矢量；/>为Gk-1的转置；Hk为tk时刻的测量矩阵；/>为Hk的转置；Rk，Qk分别为测量噪声协方差矩阵和系统噪声协方差矩阵；
通过上述估计，得到惯性导航误差δx；
根据卡尔曼滤波器估计的惯性导航误差δx，对惯性导航数据进行修正：
其中，x为惯性导航数据δx为估计的惯性导航的误差 为修正后的惯性导航数据，具体为：分别为修正后的纬度，经度，高度，东向速度，北向速度，天向速度，俯仰角，滚转角，方位角；
得到修正后的惯性导航数据，即修正后的数据，并以位置、速度和姿态的形式实现融合导航输出。
2.根据权利要求1所述的用于运动载体的高动态运动状态感知系统，其特征在于，所述融合感知模块包括：
图像处理与特征提取单元，用于对接收的30Hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；和
状态感知单元，用于根据动态补偿模块发送的降频后的IMU数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块。
3.根据权利要求2所述的用于运动载体的高动态运动状态感知系统，其特征在于，所述对接收的30Hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；其具体过程为：
对于接收的30Hz的图像数据中的各图像帧，按照时间戳的时间顺序进行排序，得到具有连续性的图像数据，并对其中的每一帧图像进行直方图均衡处理，消除图像因亮度不适宜造成的影响，得到连续图像帧；
其中，该接收的30Hz的图像数据包括：摄像头采集的30Hz的图像帧及其对应的图像帧的时间戳；
对连续图像帧中的每一帧图像分别进行特征点提取和相邻帧特征点匹配，得到特征点对；
具体地，在第i帧图像上选取像素点P，并在该像素点P的周围选择M个像素点进行测试；
如果满足在M个像素点中存在N个连续像素点的灰度值均高于IP+t，或者满足在M个像素点中存在N个连续像素点的灰度值均低于IP-t，则像素点p为角点，并将其作为特征点；
其中，M和N均为正整数，M大于N，N除以M的比值的百分比为80％，IP为像素点P的灰度值；t为预先设定的阈值；
如果满足在M个像素点中存在N个连续像素点的灰度值均小于或等于IP+t，或者满足在M个像素点中存在N个连续像素点的灰度值均大于或等于IP-t，则像素点P删除，再继续寻找下一个像素点，重复上述过程；直至找到满足要求的像素点P，并将其作为特征点；
然后采用最邻近搜索法，在相邻帧中的第i帧和第i+1帧图像，进行特征点匹配，获得特征点对；
遍历每一图像帧，对每一图像帧进行上述处理，得到多对特征点对；
利用对极几何约束方程：
qTK-T[t]×RK-1p＝0
其中，qT为像素点P2的像素齐次坐标；K-T为相机内参转置取逆；[t]×为O2处相机原点相对于O1处相机的位移向量的反对称矩阵；R为O2处相机原点相对于O1处相机的姿态旋转矩阵；K-1为相机内参矩阵的逆；p为像素点P1的像素齐次坐标；K-T[t]×RK-1的结果称为基本矩阵；
对得到的特征点对进行筛选，遍历每一对特征点对，将每一对特征点对的坐标带入对极几何约束方程中，剔除不满足该极几何约束方程的特征点，得到多个筛选后的特征点；
对多个筛选后的特征点对中的部分特征点进行屏蔽，使剩余的特征点均匀分布，得到多个有效特征点，进而得到每帧图像的有效特征点的像素坐标；
对每个有效特征点的像素坐标进行去畸变和归一化处理，得到多个归一化处理后的特征点，及其对应的处理后的特征点坐标；
并根据每个处理后的特征点，以及每个处理后的特征点在左、右目图像中的视差uL-uR，计算每个处理后的特征点的深度Z：
其中，uL为像素坐标系下的左目图像的像素水平坐标；uR为像素坐标系下的右目图像的像素水平坐标；uL-uR为视差；fx为相机内参数；b为基线长度；
针对降频后的IMU数据，按照时间戳的时间顺序进行排序，得到具有连续性的IMU数据及其对应的IMU的测量值
从第i时刻的位置、速度和姿态四元数开始，对IMU的测量值进行积分，得到第j时刻的位置、速度和姿态四元数；
其中，为第j时刻预先建立的IMU本体系原点相对于定义的世界坐标系原点的位移；为第i时刻预先建立的IMU本体系原点相对于定义的世界坐标系原点的位移；/>为第i时刻预先建立的IMU本体系原点在定义的世界坐标系中的速度；Δt为第i时刻到第j时刻的时间差；/>为t时刻预先建立的IMU本体系相对于定义的世界坐标系的旋转四元数；/>为t时刻IMU加速度计的量测值，即实时加速度；gw为重力加速度向量，取[0 0 -9.81]T；/>为第j时刻预先建立的IMU本体系原点在定义的世界坐标系中的速度；/>为第j时刻预先建立的IMU本体系相对于定义的世界坐标系的旋转四元数；/>为四元数做向量积符号；
其中，假设IMU的真实值为ω,a，IMU的测量值为则有：
其中，为IMU的测量值中的陀螺仪测量的角速度；ωb为IMU本体系真实角速度；bg为陀螺仪的零漂误差；ng为陀螺仪的量测噪声；
其中，为IMU的测量值中的加速度计的量测值；qbw为IMU本体系相对于定义世界系的姿态四元数；aw为IMU本体系在定义世界系的惯性加速度；ba为加速度计的零漂误差；na为加速度计的量测噪声；
根据IMU的测量值，得到第i时刻到第j时刻的位置、速度和姿态四元数预积分分量：
其中，为IMU的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的位置变化量预积分分量；/>为IMU的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的速度变化量预积分分量；/>为IMU的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的姿态四元数预积分分量；
将IMU的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的位置变化量预积分分量速度变化量预积分分量/>和姿态四元数预积分分量/>作为第i时刻到第j时刻的位置、速度和姿态四元数预积分分量；
将每帧图像的有效特征点的像素坐标、多个处理后的归一化坐标、每个处理后的特征点的深度，以及从第i时刻到第j时刻的位置、速度和姿态四元数预积分部分作为处理后的图像数据。
4.一种用于运动载体的高动态运动状态感知方法，其特征在于，该方法基于上述权利要求1-3中任一所述的用于运动载体的高动态运动状态感知系统实现，该方法包括：
所述摄像头实时采集30Hz的图像数据，并将发送至融合感知模块；
所述惯性传感器实时采集3600Hz的IMU数据，并将其发送至动态补偿模块；
所述倾斜仪实时采集500Hz的倾角数据，并将其作为初始值发送至动态补偿模块；
所述高度计实时采集运动载体的50Hz测量高度值，将其发送至动态补偿模块；
所述融合感知模块对接收的30Hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；
所述动态补偿模块对接收的3600Hz的IMU数据进行预处理和降频处理，获得降频后的IMU数据，并将其发送至融合感知模块；
融合感知模块根据动态补偿模块发送的降频后的IMU数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块；
动态补偿模块对降频后的IMU数据进行IMU状态递推，得到惯性导航信息；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；
根据得到的补偿后的视觉导航数据和高度计所采集的数据，对IMU状态递推后的惯性导航信息进行融合修正，得到修正后的数据，并输出。
CN202110711142.9A 2021-06-25 2021-06-25 一种用于运动载体的高动态运动状态感知系统及感知方法 Active CN113551665B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110711142.9A CN113551665B (zh) 2021-06-25 2021-06-25 一种用于运动载体的高动态运动状态感知系统及感知方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110711142.9A CN113551665B (zh) 2021-06-25 2021-06-25 一种用于运动载体的高动态运动状态感知系统及感知方法

Publications (2)

Publication Number Publication Date
CN113551665A CN113551665A (zh) 2021-10-26
CN113551665B true CN113551665B (zh) 2023-08-11

Family

ID=78102406

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110711142.9A Active CN113551665B (zh) 2021-06-25 2021-06-25 一种用于运动载体的高动态运动状态感知系统及感知方法

Country Status (1)

Country Link
CN (1) CN113551665B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114199236A (zh) * 2021-11-29 2022-03-18 北京百度网讯科技有限公司 定位数据处理的方法、装置、电子设备和自动驾驶车辆
CN114111798A (zh) * 2021-12-07 2022-03-01 东南大学 一种基于仿射因子补偿的改进iccp方法
CN114500842A (zh) * 2022-01-25 2022-05-13 维沃移动通信有限公司 视觉惯性标定方法及其装置
CN114419109B (zh) * 2022-03-29 2022-06-24 中航金城无人系统有限公司 一种基于视觉和气压信息融合的飞行器定位方法
CN117699055B (zh) * 2024-02-06 2024-04-12 中国科学院国家空间科学中心 一种基于深度强化学习的卫星对运动目标跟瞄方法及系统

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6240367B1 (en) * 1998-11-27 2001-05-29 Ching-Fang Lin Full fusion positioning method for vehicle
JP2001174275A (ja) * 1999-12-17 2001-06-29 Furuno Electric Co Ltd ハイブリッド航法およびその装置
KR100815152B1 (ko) * 2006-11-07 2008-03-19 한국전자통신연구원 다중 필터 융합을 이용한 복합 항법 장치 및 이를 이용한항법 정보 제공 방법
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN103743395A (zh) * 2014-01-17 2014-04-23 哈尔滨工程大学 一种惯性重力匹配组合导航系统中时间延迟的补偿方法
CN105066990A (zh) * 2015-07-07 2015-11-18 哈尔滨工程大学 一种适用于捷联惯性导航的高精度数字滤波器
CN106643743A (zh) * 2016-12-29 2017-05-10 中国科学院国家空间科学中心 一种双折射太阳敏感器及其载体三轴姿态的测量方法
WO2018214227A1 (zh) * 2017-05-22 2018-11-29 深圳市靖洲科技有限公司 一种无人车实时姿态测量方法
KR20190001832A (ko) * 2017-06-28 2019-01-07 국방과학연구소 시간지연 적응 보상기능을 갖는 관성항법장치 및 그의 급속 초기정렬 방법
CN110095116A (zh) * 2019-04-29 2019-08-06 桂林电子科技大学 一种基于lift的视觉定位和惯性导航组合的定位方法
CN110763229A (zh) * 2019-11-12 2020-02-07 武汉大学 一种便携式惯性导航定位杆及其定位定姿方法
CN112902953A (zh) * 2021-01-26 2021-06-04 中国科学院国家空间科学中心 一种基于slam技术的自主位姿测量方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9632169B2 (en) * 2013-09-05 2017-04-25 Ricoh., Ltd. Using inertial navigation for calibration of indoor positioning system
CN108139216B (zh) * 2015-04-07 2021-11-05 深圳市大疆创新科技有限公司 用于提供简单而可靠的惯性测量单元的系统和方法
CN105203098B (zh) * 2015-10-13 2018-10-02 上海华测导航技术股份有限公司 基于九轴mems传感器的农业机械全姿态角更新方法

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6240367B1 (en) * 1998-11-27 2001-05-29 Ching-Fang Lin Full fusion positioning method for vehicle
JP2001174275A (ja) * 1999-12-17 2001-06-29 Furuno Electric Co Ltd ハイブリッド航法およびその装置
KR100815152B1 (ko) * 2006-11-07 2008-03-19 한국전자통신연구원 다중 필터 융합을 이용한 복합 항법 장치 및 이를 이용한항법 정보 제공 방법
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN103743395A (zh) * 2014-01-17 2014-04-23 哈尔滨工程大学 一种惯性重力匹配组合导航系统中时间延迟的补偿方法
CN105066990A (zh) * 2015-07-07 2015-11-18 哈尔滨工程大学 一种适用于捷联惯性导航的高精度数字滤波器
CN106643743A (zh) * 2016-12-29 2017-05-10 中国科学院国家空间科学中心 一种双折射太阳敏感器及其载体三轴姿态的测量方法
WO2018214227A1 (zh) * 2017-05-22 2018-11-29 深圳市靖洲科技有限公司 一种无人车实时姿态测量方法
KR20190001832A (ko) * 2017-06-28 2019-01-07 국방과학연구소 시간지연 적응 보상기능을 갖는 관성항법장치 및 그의 급속 초기정렬 방법
CN110095116A (zh) * 2019-04-29 2019-08-06 桂林电子科技大学 一种基于lift的视觉定位和惯性导航组合的定位方法
CN110763229A (zh) * 2019-11-12 2020-02-07 武汉大学 一种便携式惯性导航定位杆及其定位定姿方法
CN112902953A (zh) * 2021-01-26 2021-06-04 中国科学院国家空间科学中心 一种基于slam技术的自主位姿测量方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于实时调节GPS射频前端采样周期的INS辅助GPS组合导航方法;王熙赢；陈熙源;《中国惯性技术学报》;第23卷(第01期);49-53 *

Also Published As

Publication number Publication date
CN113551665A (zh) 2021-10-26

Similar Documents

Publication Publication Date Title
CN113551665B (zh) 一种用于运动载体的高动态运动状态感知系统及感知方法
US10295365B2 (en) State estimation for aerial vehicles using multi-sensor fusion
CN111156998B (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
CN109887057B (zh) 生成高精度地图的方法和装置
CN107747941B (zh) 一种双目视觉定位方法、装置及系统
JP5992184B2 (ja) 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム
CN110084832B (zh) 相机位姿的纠正方法、装置、系统、设备和存储介质
Kelly et al. A general framework for temporal calibration of multiple proprioceptive and exteroceptive sensors
JP5027747B2 (ja) 位置測定方法、位置測定装置、およびプログラム
JP5027746B2 (ja) 位置測定方法、位置測定装置、およびプログラム
CN111750853B (zh) 一种地图建立方法、装置及存储介质
CN111380514A (zh) 机器人位姿估计方法、装置、终端及计算机存储介质
US20180075614A1 (en) Method of Depth Estimation Using a Camera and Inertial Sensor
CN108375383B (zh) 多相机辅助的机载分布式pos柔性基线测量方法和装置
CN110207693B (zh) 一种鲁棒立体视觉惯性预积分slam方法
CN111932674A (zh) 一种线激光视觉惯性系统的优化方法
CN113516692A (zh) 一种多传感器融合的slam方法和装置
CN114693754B (zh) 一种基于单目视觉惯导融合的无人机自主定位方法与系统
CN115371673A (zh) 一种未知环境中基于Bundle Adjustment的双目相机目标定位方法
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN114638897A (zh) 基于无重叠视域的多相机系统的初始化方法、系统及装置
Hinzmann et al. Flexible stereo: constrained, non-rigid, wide-baseline stereo vision for fixed-wing aerial platforms
CN113436267B (zh) 视觉惯导标定方法、装置、计算机设备和存储介质
CN113110556B (zh) 一种基于视觉传感器的无人机位置估计系统及估计方法
CN112113564A (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
GR01 Patent grant
GR01 Patent grant