一种基于卡尔曼滤波的IMU机载视觉姿态融合的方法与流程

本发明属于无人机导航的技术领域，具体涉及一种基于卡尔曼滤波的imu机载视觉姿态融合的方法。

背景技术：

要成功完成自动着舰任务需要精确的引导信息，规划一个平滑的着舰轨迹，才能顺利的降落在甲板着舰点上。此外，有限的舰船甲板空间也对引导的精确度提出了更高的要求。多传感器信息融合方法为精确引导提供了一种可行方法；它可以利用多传感器的辅助属性进行估计，以获得更准确的估计。并且在此基础上，为了保证导引系统的兼容性以及容错率的提高，在gps、视觉引导系统、激光雷达等传感器的支持下，对可能影响引导方案变化的因素进行了全面的分析，并针对性地制定了相应的解决措施，在最大程度上保证了着舰的准确性以及稳定性。

着舰引导系统采用多信息源冗余设计，充分考虑到复杂的海洋环境及其他应用环境，相互之间互为余度，互相补充，才能够保证在各种干扰下具有较高的可靠性和导引精度。

技术实现要素：

本发明的目的在于提供一种基于卡尔曼滤波的imu机载视觉姿态融合的方法，使飞机的实际位置测量更准确，本发明融合imu及视觉信号，并采用扩展卡尔曼滤波算法进行多传感器信息融合以实现多传感器信息融合，本发明提高了引导信息的精度和可靠性。

本发明主要通过以下技术方案实现：一种基于卡尔曼滤波的imu机载视觉姿态融合的方法，主要包括步骤s300：基于卡尔曼滤波的imu/机载视觉姿态融合；采集imu实验数据并经过四元数解算得到姿态角数据，同时对图像采集的图片进行处理得到相机的内、外参数；采用卡尔曼滤波融合姿态角数据和外参数。

为了更好的实现本发明，进一步的，所述步骤s300主要包括以下步骤：

步骤s301：imu数据的采集频率为200hz；

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

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

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

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

为了更好的实现本发明，进一步的，所述步骤s302主要包括以下步骤：

r'＝qrq'

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

q＝λ+p1i+p2j+p3k

式中，λ表示四元数的实部，p1、p2和p3表示四元数的虚部；

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

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

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

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

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

③姿态矩阵的实时计算：确定姿态矩阵输入为λ(n)、p1(n)、p2(n)和p3(n)：

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

④载体姿态角计算，以确定姿态角θ、ψ和γ，输入为t11(n)、t12(n)、t13(n)、t23(n)和t33(n)：

θ＝-arcsin(t13(n))

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

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

为了更好的实现本发明，进一步的，所述步骤s304中对于旋转矩阵r的解算如下：

①据连续的两张图片得到特征匹配点计算对极几何关系，得到基础矩阵f：

两图中任意一对匹配点满足x'^tfx＝0，记两图匹配点坐标分别为x＝(x,y,1)^t，x'＝(x',y',1)^t，fij为基础矩阵f中i行j列元素，则满足：

展开后有：

x'xf11+x'yf12+x'f13+y'xf21+y'yf22+y'f23+xf31+yf32+f33＝0

将基础矩阵f写成列向量f的形式，则有：

[x'xx'yx'y'xy'yy'xy1]f＝0

随机从匹配点对中选定8组，设为则有：

令左边矩阵为a，则有af＝0，进行线性运算易得基础矩阵f，用f进行验算，计算验算成功的点对数n；

再随机选择8组点对，重复以上步骤，找到使n最大的f作为最终求得的基础矩阵f；

②对基础矩阵f通过归一化图像坐标进行转换得到本质矩阵e：

假设摄像机外参矩阵p＝[r|t]，则x＝px，式中，r表示旋转坐标系，t表示两张图片的平移，x表示相机坐标系下坐标，x表示世界坐标系下坐标；已知摄像机标定矩阵k，将k的逆矩阵作用于点x得到点则是图像上的点在归一化坐标下的表示，本质矩阵即在归一化图像坐标下的基础矩阵；

得到本质矩阵e和基础矩阵f的关系：

e＝k'^tfk

在已知摄像机内参矩阵的条件下，将基础矩阵f转换为本质矩阵e；

③对本质矩阵e进行奇异值分解求解出旋转矩阵r：

本质矩阵e已知，即可恢复舰载机机载摄像机的内参矩阵，令e＝[t]×r＝sr，s为反对称矩阵，则可求解旋转矩阵r；选择拍摄的物体位于舰载机机载摄像机的前方所对应的点，得到机载摄像机的外参矩阵p的解，从而得到旋转矩阵。

为了更好的实现本发明，进一步的，所述步骤s305中以欧拉角为状态量xk，单目视觉测量的结果为观测量yk，取ak、ck均为单位矩阵，将w和v看作是独立分布的、满足正态分布的白噪声，下面分别为状态方程和量测方程：

xk＝xk-1+wk-1

yk＝xk+vk

p(w)～n(0，q)，p(v)～n(0，r)，其中q是过程噪声的方差，r是测量噪声的方差，p表示噪声的概率密度；q、r随着时间变化，这里设定为常量；由于两种数据的采集采用的是不同的频率，在不到50s的实验中，imu的测量采集了9162个数据，视觉采集了687张图片，因此对应的分别是9162个姿态角和687个姿态角；为了对应好两者的数据，选择将9162个姿态角的每40个姿态角进行平均取值，687个姿态角中每3个姿态角进行平均取值，这样可以对应出224个数据，在卡尔曼滤波的程序中进行融合。

本发明的有益效果：

(1)本发明融合imu及视觉信号，并采用扩展卡尔曼滤波算法进行多传感器信息融合以实现多传感器信息融合，本发明提高了引导信息的精度和可靠性。

附图说明

图1为本发明融合系统时间对准采样图；

图2为本发明像素坐标到世界坐标的变换示意图；

图3为本发明位置传感器融合算法流程图；

图4为本发明姿态融合算法流程图；

图5为本发明卡尔曼滤波流程图。

具体实施方式

实施例1：

一种基于卡尔曼滤波的imu机载视觉姿态融合的方法，主要包括以下步骤：

步骤s100：如图1所示，基于最小二乘法进行信息同步：

步骤s200：基于贝叶斯估计的着舰引导传感器融合；在步骤s100的信息同步后，获取第k次测量间隔内各个传感器的数据并计算第k次传感器数据的最大后验，计算k次测量的局部平均值la，进一步计算全局均值ga；计算每个传感器的测量贴进度，若测量贴进度大于等于预设门限d，则计算得到单一的融合值作为相似性输出：

其中，n∈(1,2)为经门限逻辑比较后选用的传感器个数，xtotal为单一的融合值；

步骤s300：基于卡尔曼滤波的imu/机载视觉姿态融合；采集imu实验数据并经过四元数解算得到姿态角数据，同时对图像采集的图片进行处理得到相机的内、外参数；采用卡尔曼滤波融合姿态角数据和外参数。

本发明将多传感器测量信息融合并进行滤波估计，实现精确着舰引导。相比单个传感器引导而言，对于飞机的实际位置测量更准确，本发明提高了引导系统的兼容性和容错率，充分发挥了多传感器融合的优势，在最大程度上保证了着舰的准确性以及稳定性。

实施例2：

本实施例是在实施例1的基础上进行优化，融合系统时间对准采样如图1所示，步骤s100中设激光雷达和视觉传感器的采样周期分别为t1和t2且t1/t2＝m/n，因此选择跟踪系统的采样周期为t，t为采样周期的最小公倍数，就是此时激光雷达采样n次，视觉系统采样m次。而在最小二乘法的支持下，所得到的传感器数据融合时间的关系可表达为：

上式中，c和var(c)为传感器在对准时刻的目标测量值和测量方差，ci当传感器时间保持不一致时的第i次测量值，σ表示为未融合状态下，各传感器在测量过程中的方差，1和2分别代表激光雷达和视觉传感器，n表示传感器的测量次数。

本实施例的其他部分与上述实施例1相同，故不再赘述。

实施例3：

本实施例是在实施例2的基础上进行优化，步骤s200主要包括以下步骤：

分别读取多个传感器的传感器测量值，在经由步骤1信息同步之后，获取第k次测量间隔内各个传感器的数据并计算第k次传感器数据的最大后验，再利用读取的数据计算k次测量的局部平均值la，计算全局均值ga，下一步计算全局均值ga，根据每个传感器的贴进度判断是否剔除本次传感器数据，最终计算la(i)是每个传感器第k次测量的局部平均值，n是传感器数据判断为正确的传感器数量。

如图3所示，以下是基于贝叶斯估计的传感器融合算法的步骤：

(1)假设：视觉传感器及激光雷达安装于无人机导航坐标系原点下部，即ozn轴上，两者之间相对距离为d，传感器之间的测量贴近度是指根据量测计算出来的两个传感器之间在同一坐标轴下的相对距离，可定义为：上式中，ga为两个传感器测量值局部均值的均值也可称为全局均值；σi是传感器i的标准差。

(2)定义每个传感器第k次测量的局部平均值：则全局均值，可以表示为

(3)max(i)是指在量测间隔内对传感器测量值中最接近真值状态数据的最优估计，表达式为：是该传感器的最大后验，第i个传感器的后验概率是指以量测值xi为条件的p(si|xi)，si为第i个传感器的状态量。

(4)基于贝叶斯准则可以得到各个传感器的后验概率：式中：s为状态量，x为服从正态分布的观测，s的概率密度是以量测值x为条件，服从正态分布n(μ,σ²)，即后验概率密度，其中，μ是均值，σ是标准差，似然函数的均值如下：然后可以得出：px＝∑px|sp(s)，上式中，等号右边用于计算所有的观测，并且除以所有值的和来获得p(x|s)，因此每当获得一个新的观测通过设置先验概率并进行迭代计算，并且在每次迭代中抽出最大后验概率。

(5)计算出每个传感器的di后，可以与预先设置的门限d，进行比较，假设d通过预先校准为已知的，从而决定是否选择使用这个传感器的测量。最终，获得单一的融合值作为相似性输出：式中，n∈(1,2)为经门限逻辑比较后选用的传感器个数，xtotal为单一的融合值。

(6)经传感器融合后的观测值，俯角和方位角融合后观测误差最大为10mrad，相对距离误差最大为1m，说明融合后的效果要明显高于单一传感器测量的结果，下一步就是对融合后的信息进行滤波估计，进一步提高位置测量精度。

本实施例的其他部分与实施例2相同，故不再赘述。

实施例4：

本实施例是在实施例3的基础上进行优化，如图4所示，所述步骤s300主要包括以下步骤：

步骤s301：imu数据的采集频率为200hz；

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

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

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

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

本实施例的其他部分与实施例3相同，故不再赘述。

实施例5：

本实施例是在实施例4的基础上进行优化，采集imu实验数据同时对图像采集的图片进行处理得到相机的内外参数，解算出姿态角，得到变化图像，最后卡尔曼滤波融合两者数据。

本发明相比单个传感器引导而言，对于飞机的实际位置测量更准确，提高了引导系统的兼容性和容错率，充分发挥了多传感器融合的优势，在最大程度上保证了着舰的准确性以及稳定性，为实现无人机精确定点着舰提供了可能。

融合imu及视觉信号，采用扩展卡尔曼滤波算法进行多传感器信息融合技术研究。以实现多传感器信息融合，并提高引导信息的精度和可靠性，其算法流程图如图4所示。

以下是基于卡尔曼滤波的imu/视觉姿态融合技术的具体步骤：

(1)imu数据导入

首先是实验的采集数据，imu的数据采集频率为200hz，将imu数据保存在一个.txt文件中，在matlab中导入该文件，将里边的信息逐一命名，导出为.mat文件，待用。

(2)通过四元数解算姿态角

r'＝qrq'

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

q＝λ+p1i+p2j+p3k

式中，λ表示四元数的实部，p1、p2和p3表示四元数的虚部。

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

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

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

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

t为时间间隔，t代表当前时刻，ωb(t)由陀螺仪角速率构成的反对称矩阵，k1、k2和y是中间变量，式子表示的是下一个时刻的四元数值由上一个时刻的四元数值和一个估算的斜率的乘积所决定。

③姿态矩阵的实时计算，确定姿态矩阵输入为λ(n)、p1(n)、p2(n)和p3(n)。

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

④载体姿态角计算，以确定姿态角θ、ψ和γ。输入为t11(n)、t12(n)、t13(n)、t23(n)和t33(n)。

θ＝-arcsin(t13(n))

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

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

(3)相机内参数标定

利用视觉来测定目标的姿态的关键因素就是特征点的像素坐标和世界坐标之间的关系，而特征点的像素坐标和世界坐标之间的关系是由相机成像的几何模型所决定的，鉴于这个原因，相机模型参数就很关键。

相机的内部参数包括相机焦距f、主点坐标、像素大小等。

采用的标定方法是棋盘标定法，也称张正有标定法。张正有法顾名思义是由张正有提出的基于二维平面标定模板的相机标定方法，需要相机至少从两个不同的方向拍摄同一标定模板，标定模板和相机都可以自由运动，而且不需要知道相机和靶标的任何运动参数，但是需要确定标定模板上的特征点在世界坐标系中的坐标以及其投影点在像素坐标系中的坐标之间的对应关系，进而可以求出相机的内参数。

(4)外参数解算

相机的外部参数描述的是相机在三维空间中的姿态，从像素坐标到世界坐标的变换，如图2所示。这里的外参数求解就是旋转矩阵r与平移矩阵t的求解，然后才能够进行坐标的变换。

对于旋转矩阵r的解算如下：

①据连续的两张图片得到特征匹配点计算对极几何关系，得到基础矩阵f：

两图中任意一对匹配点满足x'^tfx＝0，记两图匹配点坐标分别为x＝(x,y,1)^t，x'＝(x',y',1)^t，fij为基础矩阵f中i行j列元素，则满足：

展开后有：

x'xf11+x'yf12+x'f13+y'xf21+y'yf22+y'f23+xf31+yf32+f33＝0

将基础矩阵f写成列向量f的形式，则有：

[x'xx'yx'y'xy'yy'xy1]f＝0

随机从匹配点对中选定8组，设为则有：

令左边矩阵为a，则有af＝0，进行线性运算易得基础矩阵f，用f进行验算，计算验算成功的点对数n；

再随机选择8组点对，重复以上步骤，找到使n最大的f作为最终求得的基础矩阵f。

②对基础矩阵f通过归一化图像坐标进行转换得到本质矩阵e：

假设摄像机外参矩阵p＝[r|t]，则x＝px，式中，r表示旋转坐标系，t表示两张图片的平移，x表示相机坐标系下坐标，x表示世界坐标系下坐标。已知摄像机标定矩阵k，将k的逆矩阵作用于点x得到点则是图像上的点在归一化坐标下的表示，本质矩阵即在归一化图像坐标下的基础矩阵。

得到本质矩阵e和基础矩阵f的关系：

e＝k'^tfk

在已知摄像机内参矩阵的条件下，可根据上述公式将基础矩阵f转换为本质矩阵e。

③对本质矩阵e进行奇异值分解求解出旋转矩阵r：

本质矩阵e已知，即可恢复舰载机机载摄像机的内参矩阵，令e＝[t]×r＝sr。

s为反对称矩阵，则可求解旋转矩阵r。选择拍摄的物体位于舰载机机载摄像机的前方所对应的点，得到机载摄像机的外参矩阵p的解，从而得到旋转矩阵。

(5)将imu数据和机载视觉数据进行融合

卡尔曼滤波算法流程图如图5所示。以实验采集数据模拟imu和视觉信号，在拍摄时，变换摄像头姿态。相对于背景来说，有上视视角，右视视角和正面视角。采集连续的图片及对应的imu数据，通过时间戳对齐。工作环境：在matlab平台以及vs2013平台下，解算姿态角。

以欧拉角为状态量xk，单目视觉测量的结果为观测量yk，取ak、ck均为单位矩阵，将w和v看作是独立分布的、满足正态分布的白噪声，下面分别为状态方程和量测方程：

xk＝xk-1+wk-1

yk＝xk+vk

p(w)～n(0，q)，p(v)～n(0，r)。其中q是过程噪声的方差，r是测量噪声的方差，p表示噪声的概率密度。q、r应该随着时间应该是变化的，这里设定为常量。

由于两种数据的采集采用的是不同的频率，在不到50s的实验中，imu的测量采集了9162个数据，视觉采集了687张图片，因此对应的分别是9162个姿态角和687个姿态角。为了对应好两者的数据，选择将9162个姿态角的每40个姿态角进行平均取值，687个姿态角中每3个姿态角进行平均取值，这样可以对应出224个数据，在卡尔曼滤波的程序中进行融合。

本发明融合imu及视觉信号，采用扩展卡尔曼滤波算法进行多传感器信息融合技术研究以实现多传感器信息融合，并提高引导信息的精度和可靠性。

本实施例的其他部分与上述实施例4相同，故不再赘述。

以上所述，仅是本发明的较佳实施例，并非对本发明做任何形式上的限制，凡是依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化，均落入本发明的保护范围之内。