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

1.本发明属于导航的运动状态感知技术领域，具体地说，涉及一种用于运动载体的高动态运动状态感知系统及感知方法。


背景技术：

2.随着移动互联网化产业的不断发展，位置服务已经成为移动终端的一项重要指标，为载体提供实时的姿态和航向信息，在航空航天、机器人、无人机、汽车工业以及行人导航定位领域得到广泛的应用。
3.近十年中，slam(simultaneous localization and mapping，同步定位与建图)及其相关技术的研究取得了长足的进展，大量的slam实现方法涌现。slam包括：载体运动状态估计和传感器检测到的环境模型的构建，共2个部分。其中，载体的状态可由位姿来描述，而建图则是对载体所处环境的信息，如位置、障碍等的描述。随着研究人员越来越关注将该技术应用于微小型无人机、小型排雷机器人和小型反恐侦察机器人等小型运动载体平台，slam的研究重点开始从激光雷达转向相机和惯性传感器(imu)。得益于芯片和微机电系统(mems)器件的快速发展，相机和mu等传感器实现了高精度化、小型化和低成本化。采用相机和imu的slam方法被称为视觉惯性slam(visual
‑
inertialslam，vi
‑
slam)，当前，vi
‑
slam方法是无人机、自动驾驶、机器人等领域解决环境感知与定位导航问题的有效手段之一。
4.对于视觉惯性slam，根据参与估计的位姿数量不同，主要分为基于滤波和基于优化两大类方法，这两种不同的分类方法相互组合可将视觉惯性slam细分为滤波松耦合、滤波紧耦合、优化松耦合和优化紧耦合等方法，但存在以下问题：
5.1、松耦合方式是将视觉信息和imu信息分别放入两个独立的模块中处理，然后再对处理的结果进行融合。一般都是在视觉框架给出不带尺度的位姿结果后，再将这个结果作为状态量与imu信息进行融合得到带有尺度信息的位姿结果。滤波松耦合一般是在扩展卡尔曼滤波框架下针对imu和摄像头的融合方法，需要单独给出imu误差模型和相机量测模型，同时要解决传感器数据不同步的问题。
6.优化松耦合是基于残差函数，使用函数关于状态量的雅克比矩阵，对状态量初值进行迭代求增量的方式。需要单独构造视觉误差项和惯导误差项，然后通过光束平差求解器对这两个误差项进行迭代优化得到状态量。
7.因此，无论是滤波松耦合还是优化松耦合，松耦合的特点是计算复杂度低，而且传感器数据单独处理再融合的方式可以有效提高算法的扩展性。但是，松耦合忽略了传感器数据之间的相关性，低精度传感器的噪声会在融合阶段影响到高精度传感器的数据，使得估计的误差变大，造成得到的惯性导航数据的精度降低。
8.2、紧耦合方法是将传感器数据进行联合估计，考虑不同传感器之间的相关性形成互相的约束，以此来降低估计的误差。基于扩展卡尔曼滤波的紧耦合方法，特征点信息会给入特征向量，多个特征点约束一个状态向量(位姿速度)。但是，该方法不仅计算量大，而且需要给定特征点的初始深度和初始协方差，如果初始条件给定不正确，极易导致之后的迭
代不收敛，无法得到准确的位姿估计，且基于滤波的方法，存在无法消除线性化误差的问题。
9.3、优化紧耦合方法，尤其是非线性优化方法，可以提供更高的精度，但计算量较大。
10.综上所述，现有的采用视觉+imu的感知方法存在计算量大，实时性低的问题，且无法解决快速运动的问题，仅能产生30hz的运动状态感知信息，无法实现无人机和仿人机器人等运动载体的500hz的高动态的运动状态感知需求。


技术实现要素：

11.为解决现有技术存在的上述缺陷，本发明提出了一种用于运动载体的高动态运动状态感知系统及感知方法，针对无人机、机器人等运动载体执行高动态任务时，位姿状态难以高频率精确感知高动态运动状态的问题，采用双目视觉、imu、高度计和倾斜仪的多传感器的信息融合，以及多传感器信息融合延迟自适应补偿，使得无人机、机器人等运动载体能够在执行500hz的高动态任务时，位姿状态能够以高频率精确感知运动状态，为无人机等运动载体在路径规划和飞行控制对提供高频率精确的导航信息，也可解决机器人等运动载体在执行跑跳任务过程中由于高动态、间歇性大震动工况引起的位姿和感知精度低、可靠性差等问题。
12.本发明提供了一种用于运动载体的高动态运动状态感知系统，该系统包括：摄像头、惯性传感器、倾斜仪、高度计、融合感知模块、动态补偿模块和接口模块；
13.所述接口模块分别与惯性传感器、倾斜仪、高度计、融合感知模块、动态补偿模块通信连接；摄像头与融合感知单元通信连接；
14.所述摄像头，用于实时采集30hz的图像数据，并将发送至融合感知模块；
15.所述惯性传感器，用于实时采集3600hz的imu数据，并将其发送至动态补偿模块；
16.所述倾斜仪，用于实时采集500hz的倾角数据，并将其作为初始值发送至动态补偿模块；
17.所述高度计，用于实时采集运动载体的50hz测量高度值，将其发送至动态补偿模块；
18.所述融合感知模块，用于对接收的30hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；
19.根据动态补偿模块发送的降频后的imu数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块；
20.所述动态补偿模块，用于对接收的3600hz的imu数据进行预处理和降频处理，获得降频后的imu数据，并将其发送至融合感知模块；
21.还用于对降频后的imu数据进行imu状态递推，得到惯性导航信息；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；
22.根据得到的补偿后的视觉导航数据和高度计所采集的数据，对imu状态递推后的惯性导航信息进行融合修正，得到修正后的数据，并输出。
23.作为上述技术方案的改进之一，所述融合感知模块包括：
24.图像处理与特征提取单元，用于根据接收的30hz的图像数据进行图像处理，并对
其进行特征点提取，获得处理后的图像数据；和
25.状态感知单元，用于根据动态补偿模块发送的降频后的imu数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块。
26.作为上述技术方案的改进之一，所述根据接收的30hz的图像数据和动态补偿模块发送的降频后的imu数据进行图像处理，获得处理后的图像数据；其具体过程为：
27.对于接收的30hz的图像数据中的各图像帧，按照时间戳的时间顺序进行排序，得到具有连续性的图像数据，并对其中的每一帧图像进行直方图均衡处理，消除图像因亮度不适宜造成的影响，得到连续图像帧；
28.其中，该接收的30hz的图像数据包括：摄像头采集的30hz的图像帧及其对应的图像帧的时间戳；
29.对连续图像帧中的每一帧图像分别进行特征点提取和相邻帧特征点匹配，得到特征点对；
30.具体地，在第i帧图像上选取像素点p，并在该像素点p的周围选择m个像素点进行测试；
31.如果满足在m个像素点中存在n个连续像素点的灰度值均高于i
p
+t，或者满足在m个像素点中存在n个连续像素点的灰度值均低于i
p
‑
t，则像素点p为角点，并将其作为特征点；
32.其中，m和n均为正整数，m大于n，n除以m的比值的百分比为80％，i
p
为像素点p的灰度值；t为预先设定的阈值；
33.如果满足在m个像素点中存在n个连续像素点的灰度值均小于或等于i
p
+t，或者满足在m个像素点中存在n个连续像素点的灰度值均大于或等于i
p
‑
t，则像素点p删除，再继续寻找下一个像素点，重复上述过程；直至找到满足要求的像素点p，并将其作为特征点；
34.然后采用最邻近搜索法，在相邻帧中的第i帧和第i+1帧图像，进行特征点匹配，获得特征点对；
35.遍历每一图像帧，对每一图像帧进行上述处理，得到多对特征点对；
36.利用对极几何约束方程：
37.q
t
k
‑
t
[t]
×
rk
‑1p＝0
[0038]
其中，q
t
为像素点p2的像素齐次坐标；k
‑
t
为相机内参转置取逆；[t]
×
为o2处相机原点相对于o1处相机的位移向量的反对称矩阵；r为o2处相机原点相对于o1处相机的姿态旋转矩阵；k
‑1为相机内参矩阵的逆；p为像素点p1的像素齐次坐标；k
‑
t
[t]
×
rk
‑1的结果称为基本矩阵；
[0039]
对得到的特征点对进行筛选，遍历每一对特征点对，将每一对特征点对的坐标带入对极几何约束方程中，剔除不满足该极几何约束方程的特征点，得到多个筛选后的特征点；
[0040]
对多个筛选后的特征点对中的部分特征点进行屏蔽，使剩余的特征点均匀分布，得到多个有效特征点，进而得到每帧图像的有效特征点的像素坐标；
[0041]
对每个有效特征点的像素坐标进行去畸变和归一化处理，得到多个归一化处理后的特征点，及其对应的处理后的特征点坐标；
[0042]
并根据每个处理后的特征点，以及每个处理后的特征点在左、右目图像中的视差
u
l
‑
u
r
，计算每个处理后的特征点的深度z：
[0043][0044]
其中，u
l
为像素坐标系下的左目图像的像素水平坐标；u
r
为像素坐标系下的右目图像的像素水平坐标；u
l
‑
u
r
为视差；f
x
为相机内参数；b为基线长度；
[0045]
针对降频后的imu数据，按照时间戳的时间顺序进行排序，得到具有连续性的imu数据及其对应的imu的测量值
[0046]
从第i时刻的位置、速度和姿态四元数开始，对imu的测量值进行积分，得到第j时刻的位置、速度和姿态四元数。
[0047][0048][0049][0050]
其中，为第j时刻预先建立的imu本体系原点相对于定义的世界坐标系原点的位移；为第i时刻预先建立的imu本体系原点相对于定义的世界坐标系原点的位移；为第i时刻预先建立的imu本体系原点在定义的世界坐标系中的速度；δt为第i时刻到第j时刻的时间差；为t时刻预先建立的imu本体系相对于定义的世界坐标系的旋转四元数；为t时刻imu加速度计的量测值，即实时加速度；g
w
为重力加速度向量，取[0 0
ꢀ‑
9.81]
t
；为第j时刻预先建立的imu本体系原点在定义的世界坐标系中的速度；为第j时刻预先建立的imu本体系相对于定义的世界坐标系的旋转四元数；为四元数做向量积符号；
[0051]
其中，假设imu的真实值为ω,a，imu的测量值为则有：
[0052][0053]
其中，为imu的测量值中的陀螺仪测量的角速度；ω
b
为imu本体系真实角速度；b
g
为陀螺仪的零漂误差；n
g
为陀螺仪的量测噪声；
[0054][0055]
其中，为imu的测量值中的加速度计的量测值；q
bw
为imu本体系相对于定义世界系的姿态四元数；a
w
为imu本体系在定义世界系的惯性加速度；b
a
为加速度计的零漂误差；n
a
为加速度计的量测噪声；
[0056]
根据imu的测量值，得到第i时刻到第j时刻的位置、速度和姿态四元数预积分分量：
[0057][0058]
[0059][0060]
其中，为imu的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的位置变化量预积分分量；为imu的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的速度变化量预积分分量；为imu的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的姿态四元数预积分分量；
[0061]
将imu的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的位置变化量预积分分量速度变化量预积分分量和姿态四元数预积分分量作为第i时刻到第j时刻的位置、速度和姿态四元数预积分分量；
[0062]
将每帧图像的有效特征点的像素坐标、多个处理后的归一化坐标、每个处理后的特征点的深度，以及从第i时刻到第j时刻的位置、速度和姿态四元数预积分部分作为处理后的图像数据。
[0063]
作为上述技术方案的改进之一，所述根据动态补偿模块发送的降频后的imu数据，结合处理后的图像数据，计算运动载体的位移、速度和姿态四元数，得到视觉导航数据，并将其输入至动态补偿模块；其具体过程为：
[0064]
假设处理后的图像数据中包括m帧图像，从其第一帧图像开始，分别获取第2帧、第3帧
…
，第m帧与第一帧图像的特征点视差，并判断其得到的每个视差是否大于或等于预先设定的关键阈值；
[0065]
如果其得到的视差大于或等于预先设定的关键阈值，则将第m帧作为关键帧，并统计关键帧的数量，直至达到10帧结束；
[0066]
如果其得到的视差小于预先设定的关键阈值，则跳过当前帧，判断下一帧与第一帧的视差，直至找到下一个关键帧；
[0067]
设定滑动窗口包括10帧关键帧，在滑动窗口内，计算每一帧到第11帧关键帧的视差，将具有最大视差的一帧作为参考帧，标记为第l帧，其中，1≤l≤10；根据基本矩阵k
‑
t
[t]
×
rk
‑1，解算第11帧到第l帧的旋转矩阵和平移向量并将其作为图像帧相对相机系的位姿关系；
[0068]
将第11帧和第l帧组成特征点对，再结合上述所获得的图像帧相对相机系的位姿关系，三角化求解该特征点对在图像帧中所对应的有效特征点在第l帧相机系中的三维坐标；
[0069]
根据求解得到的三维坐标和该有效特征点在其他帧中的归一化坐标，使用pnp问题，求解第l帧相机系到其他帧时刻相机系的旋转矩阵和平移向量，即第l帧相机系到其他帧时刻相机系的位姿关系；
[0070]
重复上述过程，三角化求解滑动窗口内的各帧的有效特征点在第l帧相机系的三维坐标[x
c y
c z
c
]
t
；x
c
为三角化求解
‑
的有效特征点在相机系x轴的坐标；y
c
为三角化求解的
‑
有效特征点在相机系y轴的坐标；z
c
为三角化求解的有效特征点在相机系z轴的坐标，即深度信息；
[0071]
由于三角化求取的三维坐标与真实值有误差，且后续会涉及到逆深度优化，此处将三角化求得的有效特征点的深度z
c
与每个处理后的特征点的深度z取平均值x
c
和y
c
等比例伸缩；
[0072]
采用优化工具库ceres，对三角化求解的滑动窗口内各帧的有效特征点在第l帧相机系的三维坐标进行ba(bundle adjustment，光束平差)优化，得到优化的滑动窗口内的各帧到第l帧的旋转矩阵和平移向量
[0073]
对于获取的每个图像帧，利用和计算各帧时刻对应的imu的本体系到第l帧的旋转矩阵
[0074]
使用求解到的旋转矩阵作为约束，估计imu的角速度的偏移量bias；依次取和两帧之间的位姿四元数预积分值最小化目标函数：
[0075][0076]
将式(1)带入上述的目标函数中：
[0077][0078]
其中，是使用量测值得到的两帧之间的位姿四元数预积分量；是预积分量关于陀螺仪的零漂b
g
的雅克比矩阵，
[0079]
得到方程：
[0080][0081]
其中，()
vec
表示取四元数的虚部；
[0082]
根据上述方程，计算陀螺仪零漂b
g
的误差δb
g
；
[0083]
利用imu的量测值和解算的δb
g
重新对和进行预积分，更新预积分分量和的结果，并使用误差传播方程，递推b
g
，得到窗口内每一关键帧处对应的imu的b
g
；
[0084]
将作为imu的平移约束，估计重力方向、相机平移关系的尺度因子，利用重力加速度g
w
的模长优化第l帧的重力加速度通过第l帧到世界系的旋转矩阵将旋转到g
w
，定义世界系原点与第1帧相机系重合，使用和递推imu本体系相对于世界系的旋转矩阵；
[0085]
基于上述过程，利用世界系作为过渡，能够实现相机系与imu本体系对齐，并计算出各帧相机系相对于世界系的旋转矩阵和平移向量，由图像帧对imu的偏移量作为约束，解算陀螺仪的零漂b
g
，并且带入预积分公式，重新对imu的加速度和角速度量测值进行积分递推出各帧处imu本体系相对于世界系的位置、速度、姿态四元数；
[0086]
将由imu数据递推出的相对于世界系的位置p
wb
，姿态四元数q
wb
，相对于世界系的速度加速度计零漂和陀螺仪零漂组成的非线性偏移量(b
a
，b
g
)，相机系与imu本体系之间旋转矩阵相机系原点和imu本体系原点的相对位移有效特征点对应空间点的逆深度λ，作为状态量的初值建立视觉量测残差和imu预积分残差函数。
[0087]
视觉残差函数：
[0088][0089]
其中，和是由第i帧图像有效特征点的归一化坐标，通过两帧图像对应的相机系的旋转平移关系解算得到的在第j帧上对应的有效特征点的相机系坐标，为第j帧对应的有效特征点直接解算得到的归一化坐标；
[0090]
imu预积分分量残差函数：
[0091][0092]
总的残差函数：
[0093]
f＝ρ1r
c
(x)+ρ2r
b
(x)
[0094]
其中，ρ1和ρ2是两部分残差的权重；
[0095]
根据公式(2)，计算状态增量δx：
[0096]
j
t
f+j
t
jδx＝0
ꢀꢀ
(2)
[0097]
其中，j
t
为各残差关于优化状态量的雅克比矩阵的转置；f为残差函数在状态量初值处的值组成的向量；j为各残差关于优化状态量的雅克比矩阵；
[0098]
将计算得到的状态增量δx，与状态量初值进行广义加法运算：
[0099]
δx1＝δx+x
[0100]
得到优化后的状态量δx1；
[0101]
滑动窗口移动后，根据优化后的状态量，得到不同时刻运动载体相对于起始时刻的位移、姿态四元数和实时速度，并将其组成视觉导航数据，并输入至动态补偿模块：
[0102][0103]
其中，r
p
为相对初始地点的位置；r
q
为相对初始地点的姿态四元数；r
v
为相对初始地点的速度。
[0104]
作为上述技术方案的改进之一，所述动态补偿模块包括：预处理单元和修正单元；
[0105]
所述预处理单元，用于对接收的3600hz的imu数据经过滤波器进行平滑预处理后，再依次顺序进行圆锥误差补偿，划桨误差补偿，以及初始偏差校正，得到校正后的数据，再将其降频至500hz，获得降频后的imu数据，并将其发送至融合感知模块；
[0106]
所述修正单元，用于对降频后的imu数据进行状态递推，得到惯性导航数据；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；根据得到的补偿后的视觉导航数据和高度计所采集的数据，对惯性导航数据进行修正，得到修正后的数据，并输出。
[0107]
作为上述技术方案的改进之一，所述对降频后的imu数据进行imu状态递推，得到惯性导航数据；其具体过程为：
[0108]
对降频后的imu数据进行拆分，得到降频后的旋转角速度信息和降频后的比力信息；
[0109]
利用imu中的陀螺仪，获取运动载体的降频后的旋转角速度信息(ω
x
,ω
y
,ω
z
)，并对该运动载体的降频后的旋转角速度信息进行状态递推：
[0110][0111]
其中，为载体坐标系到地理坐标系的变换矩阵：为地理坐标系到载体坐标系的变换矩阵；
[0112][0113]
其中，p为俯仰角；r为滚转角；a为方位角；
[0114]
为陀螺仪测量矢量的反对称矩阵：
[0115][0116]
为地球绕自转轴的旋转(ω
e
＝15
°
/h)在地理坐标系中产生的角速度的反对称矩阵：
[0117][0118]
为地理坐标系相对地心地固坐标系的角速度的反对称矩阵：
[0119][0120]
其中，为大地纬度，r
n
为地球的卯酉半径，r
m
为子午半径；v
e
为东向速度分量，v
n
为北向速度分量；h为高度。
[0121]
根据上述公式，对该运动载体的降频后的旋转角速度信息进行状态递推，得到递推更新到当前时刻的并根据公式，计算得到初始的姿态角(p,r,a)：
[0122][0123]
利用imu中的加速度计，获取运动载体的降频后的比力信息(f
x
,f
y
,f
z
)，并对其进行状态递推：
[0124][0125]
其中，为地理坐标系中的运动载体的速度的导数；v
l
为地理坐标系中的运动载体的速度；v
l
＝[v
e
,v
n
,v
u
]
t
，v
e
为东向速度分量，v
n
为北向速度分量，v
u
为天向速度分量；f
b
为加速度计的测量值，称为比力，f
b
＝[f
x
,f
y
,f
z
]
t
；g
l
为地球重力，g
l
＝[0,0,
‑
g]
t
；
[0126]
利用上式，对运动载体的降频后的比力信息(f
x
,f
y
,f
z
)进行状态递推，得到运动载体的初始速度信息v
l
＝[v
e
,v
n
,v
u
]
t
；
[0127]
其中，该初始的速度信息包括：北向速度分量、天向速度分量和动向速度分量；
[0128]
再根据公式，对得到的运动载体的初始速度信息v
l
＝[v
e
,v
n
,v
u
]
t
进行状态递推，得到运动载体的初始位置信息：
[0129][0130]
其中，为运动载体的纬度；λ为运动载体的经度；h为运动载体的惯性导航高度；
[0131]
上式进行递推更新，得到初始的位置信息，该位置信息包括：纬度、经度和高度；将
初始位置信息、初始的速度信息和初始的姿态角信息，汇总成初始的惯性导航信息，并将其作为惯性导航数据。
[0132]
作为上述技术方案的改进之一，所述对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；根据得到的补偿后的视觉导航数据和高度计所采集的数据，对惯性导航数据进行修正，得到修正后的数据，并输出；其具体过程为：
[0133]
接收的视觉导航数据为：
[0134][0135]
其中，r
p
为相对初始地点的位置；r
q
为相对初始地点的姿态四元数；r
v
为相对初始地点的速度；
[0136]
首先将接收的视觉导航数据转换到地理坐标系，得到转换后的视觉导航数据，目的是与前面得到的惯性导航信息保持一致：
[0137][0138]
其中，为转换后的相对初始地点的位置；为转换后的相对初始地点的姿态四元数；为转换后的相对初始地点的速度；
[0139]
假设当前时刻为k，传输至动态补偿模块时的延迟为n，则延迟后的视觉导航数据为：
[0140][0141]
在动态补偿模块中，用当前时刻k的加速度计的测量值(f
x
,f
y
,f
z
)
k
和陀螺仪的测量值(ω
x
,ω
y
,ω
z
)
k
，对延迟后的视觉导航数据进行自适应递推补偿，其对应的递推过程与对降频后的imu数据进行imu状态递推的递推过程一致；
[0142]
得到当前时刻k的补偿后的视觉导航数据：
[0143]
将补偿后的视觉导航数据和高度计的测量值，与imu状态递推的惯性导航数据做差，得到误差测量值；
[0144]
完整的惯性导航误差包括位置、速度、姿态，以及加速度计零偏和陀螺漂移误差分量，表示为：
[0145][0146]
其中，表示位置误差矢量；为地球参考速
度误差矢量；为姿态误差矢量；为陀螺仪误差矢量；为加速度计误差矢量；
[0147]
利用卡尔曼滤波器对惯性导航误差进行最优估计：
[0148][0149][0150]
由于卡尔曼滤波器需要进行离散迭代更新，当前迭代时刻k的误差状态量记为δx
k
，上一迭代时刻k
‑
1的误差状态量记为δx
k
‑1，为误差状态量预测值；为误差状态量的滤波值，其他参数的上下标与此保持一致；分别为误差状态预测值和误差状态滤波值的误差协方差矩阵；δz
k
为误差测量值；
[0151][0152]
其中，为视觉导航得到的纬度；λ
vis
为视觉导航得到的经度；h
vis
为高度计测量的高度；v
e,vis
为视觉导航得到的东向速度；v
n,vis
为视觉导航得到的北向速度；v
u,vis
为视觉导航得到的天向速度；
[0153]
k
k
为增益矩阵，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维与位置、速度、姿态、陀螺漂移以及加速度计零偏有关的对角矩阵，g
k
‑1为k
‑
1时刻的噪声分布矢量；为g
k
‑1的转置；h
k
为t
k
时刻的测量矩阵；为h
k
的转置；r
k
，q
k
分别为测量噪声协方差矩阵和系统噪声协方差矩阵；
[0154]
通过上述估计，得到惯性导航误差δx；
[0155]
根据卡尔曼滤波器估计的惯性导航误差δx，对惯性导航数据进行修正：
[0156][0157]
其中，x为惯性导航数据δx为估计的惯性导航的误差(仅使用前9个误差状态)为修正后的惯性导航数据，具体为：分别为修正后的纬度，经度，高度，东向速度，北向速度，天向速度，俯仰角，滚转角，方位角；
[0158]
得到修正后的惯性导航数据，即修正后的数据，并以位置、速度和姿态的形式实现融合导航输出。
[0159]
本发明还提供了一种用于运动载体的高动态运动状态感知方法，该方法包括：
[0160]
所述摄像头实时采集30hz的图像数据，并将发送至融合感知模块；
[0161]
所述惯性传感器实时采集3600hz的imu数据，并将其发送至动态补偿模块；
[0162]
所述倾斜仪实时采集500hz的倾角数据，并将其作为初始值发送至动态补偿模块；
[0163]
所述高度计实时采集运动载体的50hz测量高度值，将其发送至动态补偿模块；
[0164]
所述融合感知模块对接收的30hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；
[0165]
所述动态补偿模块对接收的3600hz的imu数据进行预处理和降频处理，获得降频后的imu数据，并将其发送至融合感知模块；
[0166]
融合感知模块根据动态补偿模块发送的降频后的imu数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块；
[0167]
动态补偿模块对降频后的imu数据进行imu状态递推，得到惯性导航信息；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；
[0168]
根据得到的补偿后的视觉导航数据和高度计所采集的数据，对imu状态递推后的惯性导航信息进行融合修正，得到修正后的数据，并输出。
[0169]
本发明与现有技术相比的有益效果是：
[0170]
1、本发明的系统的处理速度快，可以满足高频率的需求，鲁棒性强；
[0171]
2、本发明的系统融合了imu的高动态和视觉的长期稳定低漂移优点，将图像处理和视觉位姿解算结果以30hz的频率对imu 500hz的递推结果进行校正，由于视觉传感器采集的图像具有长期稳定的特性，可以保证感知融合精度保持在一定范围之内，为载体提供可靠的高动态感知信息；
[0172]
3、通过对摄像头采集的30hz的视觉信息和降频后的数据，以及高度计采集的50hz测量高度值，经过融合处理，由于在处理过程中后会产生延迟，传统的方法对其进行固定时长的补偿量，由于不同场景下根据图像复杂程度会产生不定时长的补偿，故传统方法的补偿会存在一定误差；本发明提出的信息融合延迟自适应补偿技术可消除这种延迟，当动态补偿模块接收到融合感知模块输出的状态估计值时，会根据两者的时间戳信息，使用缓冲的imu数据对融合感知模块输出的状态估计值递推到当前时刻，从而可以准确消除融合感知模块的延迟。
附图说明
[0173]
图1是本发明的一种用于运动载体的高动态运动状态感知系统的结构示意图；
[0174]
图2是本发明的一种用于运动载体的高动态运动状态感知方法的流程图；
[0175]
图3是本发明的本发明的一种用于运动载体的高动态运动状态感知方法的特征点提取的示意图；
[0176]
图4是本发明的本发明的一种用于运动载体的高动态运动状态感知方法的获取对极几何约束方程的示意图。
具体实施方式
[0177]
现结合附图对本发明作进一步的描述。
[0178]
如图1所示，本发明提供了一种用于运动载体的高动态运动状态感知系统，该系统包括：摄像头、惯性传感器、倾斜仪、高度计、融合感知模块、动态补偿模块和接口模块；
[0179]
所述接口模块分别与惯性传感器、倾斜仪、高度计、融合感知模块、动态补偿模块电性连接和通信连接；摄像头与融合感知单元电性连接和通信连接；
[0180]
所述摄像头，用于实时采集30hz的图像数据，并将发送至融合感知模块；
[0181]
其中，在本实施例中，所述摄像头选取omnivision的ov9282，该摄像头为全局快门相机，最大可以实现每秒120帧720p分辨率的拍摄，且具有低时延和高精度特性，能够适用于高动态感知的应用。
[0182]
所述惯性传感器，用于实时采集3600hz的imu数据，并将其发送至动态补偿模块；
[0183]
其中，在本实施例中，该惯性传感器选取霍尼韦尔的hg4930cs36，hg4930 s
‑
class惯性测量单元是一种mems(micro
‑
electro
‑
mechanical system，微机电系统)imu(inertial measurement unit，惯性测量单元)，可以在不牺牲精度的情况下，降低延迟和提高带宽，并且不受国际武器贸易协定(itar)约束，非常适用于平台控制、导航、机器人等应用。
[0184]
所述倾斜仪，用于实时采集500hz的倾角数据，并将其作为初始值发送至动态补偿模块；
[0185]
其中，在本实施例中，该倾斜仪选择murata的2轴mems倾斜仪sca100t
‑
d02。运动载体在地面上静止时，倾斜仪用于测量初始的俯仰角和滚转角，根据测量值，可以对惯性传感器中的加速度计和陀螺仪进行偏差的校正。
[0186]
所述高度计，用于实时采集运动载体的高度，并将其发送至动态补偿模块；
[0187]
其中，在本实施例中，高度计选取garmin公司的lidar
‑
lite v3 hp，该器件为紧凑型高性能激光距离传感器，主要用于无人机，机器人，无人驾驶车辆的距离与高度测量。
[0188]
所述融合感知模块，用于对接收的30hz的图像数据进行图像处理，对其进行特征点提取，获得处理后的图像数据，并为视觉导航做准备；
[0189]
再根据动态补偿模块发送的降频后的imu数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块；
[0190]
其中，所述融合感知模块进一步包括：
[0191]
图像处理与特征提取单元，用于根据接收的30hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；和
[0192]
其中，所述图像处理与特征提取单元的具体处理过程为：
[0193]
(1)对于接收的30hz的图像数据中的各图像帧，按照时间戳的时间顺序进行排序，得到具有连续性的图像数据，并对其中的每一帧图像进行直方图均衡处理，消除图像因亮度不适宜造成的影响，得到连续图像帧；
[0194]
其中，该接收的30hz的图像数据包括：摄像头采集的30hz的图像帧及其对应的图像帧的时间戳；
[0195]
(2)对连续图像帧中的每一帧图像分别进行特征点提取和相邻帧特征点匹配，得到特征点对；
[0196]
具体地，a、在第i帧图像上选取像素点p，并在该像素点p的周围选择m个像素点进行测试；
[0197]
b、如果满足在m个像素点中存在n个连续像素点的灰度值均高于i
p
+t，或者满足在m个像素点中存在n个连续像素点的灰度值均低于i
p
‑
t，则像素点p为角点，并将其作为特征点；
[0198]
其中，m和n均为正整数，m大于n，n除以m的百分比为80％，i
p
为像素点p的灰度值；t为预先设定的阈值；
[0199]
如果满足在m个像素点中存在n个连续像素点的灰度值均小于或等于i
p
+t，或者满足在m个像素点中存在n个连续像素点的灰度值均大于或等于i
p
‑
t，则像素点p删除，再继续寻找下一个像素点，重复上述过程；直至找到满足要求的像素点p，并将其作为特征点；
[0200]
然后采用最邻近搜索法，在相邻帧中的第i帧和第i+1帧图像，进行特征点匹配，获得特征点对；
[0201]
遍历每一图像帧，对每一图像帧进行上述处理，得到多对特征点对。
[0202]
(3)利用对极几何约束方程，对特征点对进行筛选，剔除不满足该极几何约束方程的特征点，得到筛选后的特征点；
[0203]
具体地，使用相机模型内参k和匹配的特征点对进行基本矩阵解算：
[0204]
如图4所示，其中，o1和o2分别是两个时刻相机的相机坐标系的原点，空间一点p在左边图片的投影为p1，在右边图片投影为p2，连接o1与p1，o2与p2，在三维空间交会于点p。o1，o2和p确定的平面为极平面，极平面与左右图片的平面所交得到的相交线l1和l2称为极线，连接o1和o2交左右像平面于点e1和e2。
[0205]
若假设左侧相机系与世界系重合，p1的像素齐次坐标为p，p2的像素齐次坐标为q，那么有：
[0206]
z
cl
p＝kp
[0207]
z
cr
q＝k(rp+t)
[0208]
注意，如果取：
[0209]
x1＝k
‑1p
[0210]
x2＝k
‑1q
[0211]
这里的x1和x2是相机系归一化平面上的点：
[0212][0213][0214]
合并上式，则有：
[0215][0216]
两边同时左乘[t]
×
，相当于两侧做外积：
[0217]
[0218][0219]
两边同时点乘x
2t
:
[0220][0221]
观察等式左侧，[t]
×
x2是一个与t和x2都垂直的向量，再与x
2t
做内积时，结果为0，于是有：
[0222]
x
2t
[t]
×
rx1＝0
[0223]
重新带入p和q，得到：
[0224]
q
t
k
‑
t
[t]
×
rk
‑1p＝0
[0225]
该式即为对极几何约束。
[0226]
基本矩阵定义为：
[0227]
f＝k
‑
t
[t]
×
rk
‑1[0228]
利用对极几何约束方程，对得到的特征点对进行筛选，遍历每一对特征点对，将每一对特征点对的坐标带入对极几何约束方程中，剔除不满足该极几何约束方程的特征点，得到多个筛选后的特征点；
[0229]
(4)为了加速解算过程，根据剔除结果，对多个筛选后的特征点对中的部分特征点进行屏蔽，使剩余的特征点均匀分布，得到多个有效特征点，进而得到每帧图像的有效特征点的像素坐标。
[0230]
(5)一个空间中的特征点随着运动进行，从相机的视野中消失，前后图像帧没有匹配的特征点对，针对该情况加入新的特征点进行追踪；
[0231]
(6)对每个有效特征点的像素坐标进行去畸变和归一化处理，得到多个归一化处理后的特征点，及其对应的处理后的特征点坐标。
[0232]
(7)并根据每个处理后的特征点，以及每个处理后的特征点在左、右目图像中的视差u
l
‑
u
r
，计算每个处理后的特征点的深度z：
[0233]
如图3所示，假设像素点p在左、右目的成像平面上各成一点p
l
和p
r
；图3的方框为成像平面，成像平面在相机之前与像素平面一比一对应；左、右目相机的光心分别是o
l
和o
r
，f为相机焦距；z表示像素点p的深度，即角点到相机左目光心o
l
的距离；b代表基线长度，也即处于同一水平面的双目相机左右目光心之间的距离。x
l
是像素点p到相机左目光心o
l
的距离在成像坐标系上该点的水平坐标，x
r
是像素点p到相机右目光心o
r
的距离在成像坐标系上该点的水平坐标(单位为：mm)。根据相似三角形的原理:
[0234][0235]
其中，(x
l
‑
x
r
)是表示左、右目相同点的水平坐标差，即视差。由于像素坐标系和成像坐标系一一对应，像素坐标系坐标与成像坐标系坐标只存在一个α倍的坐标缩放和(c
x
,c
y
)的中心坐标偏移关系。这里用u
l
表示像素坐标系下的左目图像的像素水平坐标和u
r
表示像素坐标系下的右目图像的像素水平坐标；
[0236]
d＝x
l
‑
x
r
[0237][0238][0239][0240]
其中，f
x
为相机内参数；b为基线长度；
[0241]
因此，建立双目视觉模型：
[0242][0243]
根据处理后的特征点在左、右目图像中的视差u
l
‑
u
r
，计算该处理后的特征点的深度z。
[0244]
针对降频后的imu数据，按照时间戳的时间顺序进行排序，得到具有连续性的imu数据，
[0245]
具有连续性的imu数据包括imu的真实值ω,a和imu的测量值
[0246]
从第i时刻的位置、速度和姿态四元数开始，对imu的测量值进行积分，得到第j时刻的位置、速度和姿态四元数，即第j时刻的pvq。
[0247][0248][0249][0250]
其中，为第j时刻预先建立的imu本体系原点相对于定义的世界坐标系原点的位移；为第i时刻预先建立的imu本体系原点相对于定义的世界坐标系原点的位移；为第i时刻预先建立的imu本体系原点在定义的世界坐标系中的速度；δt为第i时刻到第j时刻的时间差；为t时刻预先建立的imu本体系相对于定义的世界坐标系的旋转四元数；为t时刻imu加速度计的量测值，即实时加速度；g
w
为重力加速度向量，取[0 0
ꢀ‑
9.81]
t
；为第j时刻预先建立的imu本体系原点在定义的世界坐标系中的速度；为第j时刻预先建立的imu本体系相对于定义的世界坐标系的旋转四元数；为四元数做向量积符号；
[0251]
其中，假设imu的真实值为ω,a，imu的测量值为则有：
[0252][0253]
其中，为imu的测量值中的陀螺仪测量的角速度；ω
b
为imu本体系真实角速度；b
g
为陀螺仪的零漂误差；n
g
为陀螺仪的量测噪声；
[0254][0255]
其中，为imu的测量值中的加速度计的量测值；q
bw
为imu本体系相对于定义世界
系的姿态四元数；a
w
为imu本体系在定义世界系的惯性加速度；b
a
为加速度计的零漂误差；n
a
为加速度计的量测噪声；
[0256]
由于imu的测量值中含有噪声，需要对其进行优化，将优化后的imu的测量进行重新积分，但由于数据量太大，因此，定义预积分分量，预积分分量只与imu的测量值有关，根据imu的测量值，得到第i时刻到第j时刻的位置、速度和姿态四元数预积分分量：
[0257][0258][0259][0260]
其中，为imu的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的位置变化量预积分分量；为imu的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的速度变化量预积分分量；为imu的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的姿态四元数预积分分量；
[0261]
将imu的测量值从第i时刻递推到第j时刻的、在定义的世界坐标系中的位置变化量预积分分量速度变化量预积分分量和姿态四元数预积分分量作为第i时刻到第j时刻的位置、速度和姿态四元数预积分分量；
[0262]
将每帧图像的有效特征点的像素坐标、多个处理后的归一化坐标、每个处理后的特征点的深度，以及从第i时刻到第j时刻的位置、速度和姿态四元数预积分部分作为处理后的图像数据。
[0263]
其中，特征点提取是通过提取处理后的图像数据中的特征点，使用描述字进行描述区分后做帧间匹配。特征点的描述字具有高区分度等特点，对环境具有更强的适应能力。
[0264]
其中，在本实施例中，该融合感知模块可以采用一个四核64位的arm cortex
‑
a53 mpcore应用处理器；在其中，还采用了一个arm mali
‑
400 mp2 gpu，可用于视频解码，用于图像处理和视觉导航中的处理。
[0265]
状态感知单元，用于根据动态补偿模块发送的降频后的imu数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块。
[0266]
其中，所述状态感知单元的具体处理过程为：
[0267]
联合视觉、imu信息，标定相机与imu本体坐标系之间的旋转矩阵相对位移通过实际的安装尺寸确定；假设处理后的图像数据中包括m帧图像，从其第一帧图像开始，分别获取第2帧、第3帧
…
，第m帧与第一帧图像的特征点视差，并判断其得到的每个视差是否大于或等于预先设定的关键阈值；
[0268]
如果其得到的视差大于或等于预先设定的关键阈值，则将第m帧作为关键帧，并统计关键帧的数量，直至达到10帧结束；
[0269]
如果其得到的视差小于预先设定的关键阈值，则跳过当前帧，判断下一帧与第一
帧的视差，直至找到下一个关键帧；
[0270]
设定滑动窗口包括10帧关键帧，在滑动窗口内，计算每一帧到第11帧关键帧的视差，将具有最大视差的一帧作为参考帧，标记为第l帧，其中，1≤l≤10；根据基本矩阵k
‑
t
[t]
×
rk
‑1，解算第11帧到第l帧的旋转矩阵和平移向量并将其作为图像帧相对相机系的位姿关系；
[0271]
将第11帧和第l帧组成特征点对，再结合上述所获得的图像帧相对相机系的位姿关系，三角化求解该特征点对在图像帧中所对应的有效特征点在第l帧相机系中的三维坐标；
[0272]
根据求解得到的三维坐标和该有效特征点在其他帧中的归一化坐标，使用pnp(perspective n points)问题，求解第l帧相机系到其他帧时刻相机系的旋转矩阵和平移向量，即第l帧相机系到其他帧时刻相机系的位姿关系；
[0273]
重复上述过程，三角化求解滑动窗口内的各帧的有效特征点在第l帧相机系的三维坐标[x
c y
c z
c
]
t
；x
c
为三角化求解
‑
的有效特征点在相机系x轴的坐标；y
c
为三角化求解的
‑
有效特征点在相机系y轴的坐标；z
c
为三角化求解的有效特征点在相机系z轴的坐标，即深度信息；
[0274]
由于三角化求取的三维坐标与真实值有误差，且后续会涉及到逆深度优化，此处将三角化求得的有效特征点的深度z
c
与每个处理后的特征点的深度z取平均值x
c
和y
c
等比例伸缩；
[0275]
采用工具箱ceres，对三角化求解滑动窗口内的各帧的有效特征点在第l帧相机系的三维坐标进行ba(bundle adjustment，光束平差)优化，得到优化的滑动窗口内的各帧到第l帧的旋转矩阵和平移向量
[0276]
对于获取的每个图像帧，利用和计算各帧时刻对应的imu的本体系到第l帧的旋转矩阵
[0277]
使用求解到的旋转矩阵作为约束，估计imu的角速度的偏移量bias；依次取和两帧之间的位姿四元数预积分值最小化目标函数：
[0278][0279]
将式(1)带入上述的目标函数中：
[0280][0281]
其中，是使用量测值得到的两帧之间的位姿四元数预积分量；是预积分量关于陀螺仪的零漂b
g
的雅克比矩阵，
[0282]
得到方程：
[0283]
[0284]
其中，()
vec
表示取四元数的虚部；
[0285]
根据上述方程，计算陀螺仪零漂b
g
的误差δb
g
；
[0286]
利用imu的量测值和解算的δb
g
重新对和进行预积分，更新预积分分量和的结果，并使用误差传播方程，递推b
g
，得到窗口内每一关键帧处对应的imu的b
g
；
[0287]
将作为imu的平移约束，估计重力方向、相机平移关系的尺度因子，利用重力加速度g
w
的模长优化第l帧的重力加速度通过第l帧到世界系的旋转矩阵将旋转到g
w
，定义世界系原点与第1帧相机系重合，使用和递推imu本体系相对于世界系的旋转矩阵；
[0288]
基于上述过程，利用世界系作为过渡，能够实现相机系与imu本体系对齐，并计算出各帧相机系相对于世界系的旋转矩阵和平移向量，由图像帧对imu的偏移量bias作为约束，解算陀螺仪的零漂bias(b
g
)，并且带入预积分公式，重新对imu的加速度和角速度量测值进行积分递推出各帧处imu本体系相对于世界系的位置、速度、姿态四元数；
[0289]
将由imu数据递推出的相对于世界系的位置p
wb
，姿态四元数q
wb
，相对于世界系的速度加速度计零漂和陀螺仪零漂组成的非线性偏移量(b
a
，b
g
)，相机系与imu本体系之间旋转矩阵相机系原点和imu本体系原点的相对位移有效特征点对应空间点的逆深度λ，作为状态量的初值建立视觉量测残差和imu预积分残差函数。
[0290]
视觉残差函数：
[0291][0292]
其中，和是由第i帧图像有效特征点的归一化坐标，通过两帧图像对应的相机系的旋转平移关系解算得到的在第j帧上对应的有效特征点的相机系坐标，为第j帧对应的有效特征点直接解算得到的归一化坐标。
[0293]
imu预积分分量残差函数：
[0294][0295]
总的残差函数：
[0296]
f＝ρ1r
c
(x)+ρ2r
b
(x)
[0297]
其中，ρ1和ρ2是两部分残差的权重。
[0298]
根据公式(2)，计算状态增量δx：
[0299]
j
t
f+j
t
jδx＝0
ꢀꢀ
(2)
[0300]
其中，j
t
为各残差关于优化状态量的雅克比矩阵的转置；f为残差函数在状态量初值处的值组成的向量；j为各残差关于优化状态量的雅克比矩阵；
[0301]
将计算得到的状态增量δx，与状态量初值进行广义加法运算：
[0302]
δx1＝δx+x
[0303]
得到优化后的状态量δx1；
[0304]
滑动窗口移动后，根据优化后的状态量，得到不同时刻运动载体相对于起始时刻的位移、姿态四元数和实时速度，并将其组成视觉导航数据，输入至动态补偿模块：
[0305][0306]
其中，r
p
为相对初始地点的位置；r
q
为相对初始地点的姿态四元数；r
v
为相对初始地点的速度。
[0307]
所述动态补偿模块，用于对接收的3600hz的imu数据进行预处理和降频处理，获得降频后的imu数据，并将其发送至融合感知模块；
[0308]
还用于对降频后的imu数据进行imu状态递推，得到惯性导航信息；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；
[0309]
根据得到的补偿后的视觉导航数据和高度计所采集的数据，对imu状态递推后的惯性导航信息进行融合修正，得到修正后的数据，并输出。
[0310]
所述动态补偿模块包括：预处理单元和修正单元；
[0311]
所述预处理单元，用于对接收的3600hz的imu数据经过滤波器进行平滑预处理后，再依次顺序进行圆锥误差补偿，划桨误差补偿，以及初始偏差校正(使用倾斜仪提供的初始陀螺仪偏差值和加速度偏差估计值校正)，得到校正后的数据，再将其降频至500hz，获得降频后的imu数据，并将其发送至融合感知模块；
[0312]
所述修正单元，用于对降频后的imu数据进行状态递推，得到惯性导航数据；对接
收的视觉导航数据进行动态补偿，得到补偿后的导航数据；根据得到的补偿后的视觉导航数据和高度计所采集的数据，对惯性导航数据进行修正，得到修正后的数据，并输出。
[0313]
其中，所述对降频后的imu数据进行imu状态递推，得到惯性导航数据；其具体过程为：
[0314]
对降频后的imu数据进行拆分，得到降频后的旋转角速度信息和降频后的比力信息；
[0315]
利用imu中的陀螺仪，获取运动载体的降频后的旋转角速度信息(ω
x
,ω
y
,ω
z
)，并对该运动载体的降频后的旋转角速度信息进行状态递推：
[0316][0317]
其中，为载体坐标系到地理坐标系的变换矩阵：为地理坐标系到载体坐标系的变换矩阵；
[0318][0319]
其中，p为俯仰角；r为滚转角；a为方位角；
[0320]
为陀螺仪测量矢量的反对称矩阵：
[0321][0322]
为地球绕自转轴的旋转(ω
e
＝15
°
/h)在地理坐标系中产生的角速度的反对称矩阵：
[0323][0324]
为地理坐标系相对地心地固坐标系的角速度的反对称矩阵：
[0325][0326]
其中，为大地纬度，r
n
为地球的卯酉半径，r
m
为子午半径；v
e
为东向速度分量，v
n
为北向速度分量；h为高度。
[0327]
根据上述公式，对该运动载体的降频后的旋转角速度信息进行状态递推，得到递推更新到当前时刻的并根据公式，计算得到初始的姿态角(p,r,a)：
[0328][0329]
利用imu中的加速度计，获取运动载体的降频后的比力信息(f
x
,f
y
,f
z
)，并对其进行状态递推：
[0330][0331]
其中，为地理坐标系中的运动载体的速度的导数；v
l
为地理坐标系中的运动载体的速度；v
l
＝[v
e
,v
n
,v
u
]
t
，v
e
为东向速度分量，v
n
为北向速度分量，v
u
为天向速度分量；f
b
为加速度计的测量值，称为比力，f
b
＝[f
x
,f
y
,f
z
]
t
；g
l
为地球重力，g
l
＝[0,0,
‑
g]
t
；
[0332]
利用上式，对运动载体的降频后的比力信息(f
x
,f
y
,f
z
)进行状态递推，得到运动载体的初始速度信息v
l
＝[v
e
,v
n
,v
u
]
t
；
[0333]
其中，该初始的速度信息包括：北向速度分量、天向速度分量和动向速度分量；
[0334]
再根据公式，对得到的运动载体的初始速度信息v
l
＝[v
e
,v
n
,v
u
]
t
进行状态递推，得到运动载体的初始位置信息：
[0335][0336]
其中，为运动载体的纬度；λ为运动载体的经度；h为运动载体的惯性导航高度；
[0337]
上式进行递推更新，得到初始的位置信息，该位置信息包括：纬度、经度和高度；将初始位置信息、初始的速度信息和初始的姿态角信息，汇总成初始的惯性导航信息，并将其作为惯性导航数据。
[0338]
其中，所述对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；根据得到的补偿后的视觉导航数据和高度计所采集的数据，对惯性导航数据进行修正，得到修正后的数据，并输出；其具体过程为：
[0339]
由于视觉导航会产生大量运算，融合感知模块将视觉导航数据传输回动态补偿模块时，可能会产生不定时长的微小延迟，故在动态补偿模块中，需对延迟进行自适应补偿，以提升系统的实时性。具体过程为：
[0340]
接收的视觉导航数据为：
[0341][0342]
其中，r
p
为相对初始地点的位置；r
q
为相对初始地点的姿态四元数；r
v
为相对初始地点的速度；
[0343]
首先将接收的视觉导航数据转换到地理坐标系，得到转换后的视觉导航数据，目的是与前面得到的惯性导航信息保持一致：
[0344][0345]
其中，为转换后的相对初始地点的位置；为转换后的相对初始地点的姿态四元数；为转换后的相对初始地点的速度；
[0346]
假设当前时刻为k，传输至动态补偿模块时的延迟为n，则延迟后的视觉导航数据为：
[0347][0348]
在动态补偿模块中，用当前时刻k的加速度计的测量值(f
x
,f
y
,f
z
)
k
和陀螺仪的测量值(ω
x
,ω
y
,ω
z
)
k
，对延迟后的视觉导航数据进行自适应递推补偿，其对应的递推过程与对降频后的imu数据进行imu状态递推的递推过程一致；
[0349]
得到当前时刻k的补偿后的视觉导航数据：
[0350]
将补偿后的视觉导航数据和高度计的测量值，与imu状态递推的惯性导航数据做差，得到误差测量值；
[0351]
完整的惯性导航误差包括位置、速度、姿态，以及加速度计零偏和陀螺漂移误差分量，表示为：
[0352][0353]
其中，表示位置误差矢量；为地球参考速度误差矢量；为姿态误差矢量；为陀螺仪误差矢量(含漂移)；为加速度计误差矢量(含零偏)。
[0354]
利用卡尔曼滤波器对惯性导航误差进行最优估计：
[0355][0356][0357]
由于卡尔曼滤波器需要进行离散迭代更新，当前迭代时刻k的误差状态量记为δx
k
，上一迭代时刻k
‑
1的误差状态量记为δx
k
‑1，为误差状态量预测值；为误差状态量的滤波值，其他参数的上下标与此保持一致；分别为误差状态预测值和误差状态滤波值的误差协方差矩阵；δz
k
为误差测量值；
[0358][0359]
其中，为视觉导航得到的纬度；λ
vis
为视觉导航得到的经度；h
vis
为高度计测量的高度；v
e,vis
为视觉导航得到的东向速度；v
n,vis
为视觉导航得到的北向速度；v
u,vis
为视觉导航得到的天向速度；
[0360]
k
k
为增益矩阵，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维与位置、速度、姿态、陀螺漂移以及加速度计零偏有关的对角矩阵，g
k
‑1为k
‑
1时刻的噪声分布矢量；为g
k
‑1的转置；h
k
为t
k
时刻的测量矩阵；为h
k
的转置；r
k
，q
k
分别为测量噪声协方差矩阵和系统噪声协方差矩阵；
[0361]
通过上述估计，得到惯性导航误差δx；
[0362]
根据卡尔曼滤波器估计的惯性导航误差δx，对惯性导航数据进行修正：
[0363][0364]
其中，x为惯性导航数据δx为估计的惯性导航的误差(仅使用前9个误差状态)(仅使用前9个误差状态)为修正后的惯性导航数据，具体为：分别为修正后的纬度，经度，高度，东向速度，北向速度，天向速度，俯仰角，滚转角，方位角；
[0365]
得到修正后的惯性导航数据，即修正后的数据，并以位置、速度和姿态的形式实现融合导航输出。
[0366]
其中，在本实施例中，该动态补偿模块可以采用一个双核32位的arm cortex
‑
r5 mpcore实时处理器，保证了同步操作；
[0367]
由于视觉摄像头采集的图像具有长期稳定的特性，可以保证感知融合精度保持在
一定范围之内。另外，在动态补偿模块和融合感知模块中对降频后的图像数据进行递推的递推结果进行冗余处理，用于补偿融合感知模块使用异步通讯造成的时延与不均匀数据包到达的时间不一致性。
[0368]
融合感知模块融合了摄像头采集的30hz的图像数据，动态补偿模块发送的预处理后的图像数据，生成完整的状态估计值，由于其运行在一个非实时的系统上，即使在最佳情况下，这些状态估计值传输至动态补偿模块时也会延迟几毫秒，因此，需要采用信息融合延迟自适应补偿方法，可以保证发送至动态补偿模块的状态估计值所对应的时刻和降频后的图像数据进行递推后得到的位姿所对应的时刻对齐，属于同一时刻，保证感知系统实时运行。
[0369]
具体实现过程如下：在动态补偿模块和融合感知模块中对imu递推过程进行冗余处理，当动态补偿模块接收到融合感知模块输出的视觉导航状态估计值时，会根据两者的时间戳信息，使用降频后的imu数据，对融合感知模块输出的状态递推到当前时刻，从而可以消除融合感知模块的延迟。此方案对融合感知模块失效或者故障也具备一定的鲁棒性。
[0370]
本技术采用了高动态运动状态感知技术和信息融合延迟自适应补偿技术，可为运动载体提供500hz高动态的运动状态感知信息；例如，可对无人机进行快速路径规划和复杂飞行控制提供导航信息，或者对仿人机器人在执行跑跳等高速、大震动的任务提供高动态运动状态感知信息。
[0371]
如图2所示，本发明还提供了一种用于运动载体的高动态运动状态感知方法，该方法包括：
[0372]
所述摄像头实时采集30hz的图像数据，并将发送至融合感知模块；
[0373]
所述惯性传感器实时采集3600hz的imu数据，并将其发送至动态补偿模块；
[0374]
所述倾斜仪实时采集500hz的倾角数据，并将其作为初始值发送至动态补偿模块；
[0375]
所述高度计实时采集运动载体的50hz测量高度值，将其发送至动态补偿模块；
[0376]
所述融合感知模块对接收的30hz的图像数据进行图像处理，并对其进行特征点提取，获得处理后的图像数据；
[0377]
所述动态补偿模块对接收的3600hz的imu数据进行预处理和降频处理，获得降频后的imu数据，并将其发送至融合感知模块；
[0378]
融合感知模块根据动态补偿模块发送的降频后的imu数据，结合处理后的图像数据，计算运动载体的状态感知信息，得到视觉导航数据，并将其输入至动态补偿模块；
[0379]
动态补偿模块对降频后的imu数据进行imu状态递推，得到惯性导航信息；对接收的视觉导航数据进行动态补偿，得到补偿后的导航数据；
[0380]
根据得到的补偿后的视觉导航数据和高度计所采集的数据，对imu状态递推后的惯性导航信息进行融合修正，得到修正后的数据，并输出。
[0381]
最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。