[发明专利]一种基于卡尔曼滤波的IMU机载视觉姿态融合的方法

权利要求书

1.一种基于卡尔曼滤波的IMU机载视觉姿态融合的方法，其特征在于，主要包括步骤S300：基于卡尔曼滤波的IMU/机载视觉姿态融合；采集IMU实验数据并经过四元数解算得到姿态角数据，同时对图像采集的图片进行处理得到相机的内、外参数；采用卡尔曼滤波融合姿态角数据和外参数。

2.根据权利要求1所述的一种基于卡尔曼滤波的IMU机载视觉姿态融合的方法，其特征在于，所述步骤S300主要包括以下步骤：

步骤S301：IMU数据的采集频率为200Hz；

步骤S302：通过四元数解算姿态角：

步骤S303：相机内参数标定，相机的内部参数包括相机焦距f、主点坐标、像素大小；采用棋盘标定法间隙标定；

步骤S304：相机外参数解算：外参数求解就是旋转矩阵R与平移矩阵t的求解，然后进行坐标的变换；

步骤S305：将IMU数据和机载视觉数据进行融合：以实验采集数据模拟IMU和视觉信号，在拍摄时，变换摄像头姿态；相对于背景来说，有上视视角、右视视角和正面视角，采集连续的图片及对应的IMU数据，通过时间戳对齐，在matlab平台以及VS2013平台下，解算姿态角。

3.根据权利要求2所述的一种基于卡尔曼滤波的IMU机载视觉姿态融合的方法，其特征在于，所述步骤S302主要包括以下步骤：

R'＝qRq'

式中，R表示被旋转的矢量，R'表示经四元数旋转之后的矢量，q表示四元数，q'表示其转置，四元数的标量部分表示转角的余弦值，其矢量部分表示瞬时转轴n的方向；下式表示矢量R相对参考坐标系旋转一个转角θ，旋转轴的方向由四元数的虚部确定，cosα、cosβ、cosγ表示旋转轴n与参考坐标系轴间的方向余弦值：

q＝λ+p₁i+p₂j+p₃k

式中，λ表示四元数的实部，p₁、p₂和p₃表示四元数的虚部；

四元数法姿态矩阵计算如下：

①初始四元数的确定，其输入为初始的姿态角：

θ₀、ψ₀和γ₀代表俯仰角、偏航角、滚转角的初始值；

②四元数标量部分与矢量部分λ、p₁、p₂、p₃的实时计算：输入信号为陀螺仪的数字输出信号其中i为x、y、z，ω_ib为三轴角速率，Δθ为陀螺仪积分得到的角度增量；计算方法采用二阶龙格库塔法：

T为时间间隔，t代表当前时刻，Ω_b(t)由陀螺仪角速率构成的反对称矩阵，K₁、K₂和Y是中间变量，式子表示的是下一个时刻的四元数值由上一个时刻的四元数值和一个估算的斜率的乘积所决定；

③姿态矩阵的实时计算：确定姿态矩阵输入为λ(n)、p₁(n)、p₂(n)和p₃(n)：

式中，表示大地系转机体系旋转矩阵；

④载体姿态角计算，以确定姿态角θ、ψ和γ，输入为T₁₁(n)、T₁₂(n)、T₁₃(n)、T₂₃(n)和T₃₃(n)：

θ＝-arcsin(T₁₃(n))

方向余弦法姿态矩阵的计算与四元数法的区别主要是姿态矩阵的描述不同，其描述如式所示：

其解算方向余弦矩阵微分方程为得到方向余弦矩阵后可提取姿态角；其中Ω为旋转角速度的斜对称矩阵。