本发明涉及定位设备技术领域，尤其涉及一种融合下视视觉与惯导信息的定向罗盘。
背景技术：
近年来，无人机在危险枯燥的任务和恶劣的工作环境中发挥着重要的作用，例如：航拍、植保、灾难救援、电力巡检等。作为一种新生力量，无人机得到了越来越多的研究机构和政府的重视与投入。自主导航功能是无人机实现上述任务的重要基础，而实现可靠准确的自主导航，无人机必须能够准确地获得航向信息，即拥有良好的定向功能。
根据无人机任务的特殊性以及飞行速度较快的特点，航向测量系统需要有非常可靠和较高实时性的特点，并且对环境变化拥有较好的鲁棒性。
传统的航向测量系统有：惯性导航系统(inertialnavigationsystem，ins)、全球定位系统(globalpositioningsystem,gps)，以及磁罗盘。其中惯导系统自主可靠，但是存在累计误差和温漂，无法长期独立工作；gps仅在室外可用，且单点gps悬停时航向信息不可用，高精度的差分gps成本高，双天线gps要实现高精度测量必须增大其基线长度(至少1m)，因此使用受限；传统的磁罗盘受磁场干扰大，在强磁或高压电线附近无法正常工作。
目前，视觉导航技术也得到了较大的发展，但是摄像头精度较低且实时性不高，因此单纯依靠视觉的导航系统未能满足无人机的工作要求。
单纯利用视觉信息进行航向角测量的视觉罗盘技术，由于均采用前视安装的摄像头，且多数采用全景摄像头，得到的图像序列可用信息少，且全景摄像头分辨率低、畸变严重、计算量大，因此现有的视觉罗盘无法满足无人机作业的要求。
技术实现要素：
本发明的目的在于提供一种多源信息融合的定向罗盘，其能获得可靠稳定且精度较高的无人机航向角信息，克服了现有罗盘技术中环境因素干扰大、数据不可靠等不足，同时避免了使用畸变大且预处理过程复杂的全景摄像头。
本发明的目的是通过以下技术方案来实现的：一种融合下视视觉与惯导信息的定向罗盘，包含数据采集单元和处理器：
所述数据采集单元，包括一个惯性测量单元(inertialmeasurementunit，imu)和一个下视安装的摄像头，所述惯性测量单元用于采集载体在三维空间中的三轴角速度和加速度，所述摄像头用于采集载体下方的俯视图像序列。
所述处理器，通过惯性测量单元采集的数据得到姿态角，通过摄像头采集的数据得到航向角，并对上述姿态角和航向角进行融合，得到载体最终的航向角。
进一步地，所述惯性测量单元固定安装在载体上，所述摄像头相对于载体采用下视安装，采集载体下方的俯视图像序列。
进一步地，所述惯性测量单元包括陀螺仪和加速度计，通过惯性测量单元采集的数据得到姿态角具体为：
(1)将陀螺仪的输出经过积分和校正后，求解微分方程并进行四元数更新得到姿态角；
(2)利用加速度计的输出计算横滚角和俯仰角；
(3)利用自适应的重力场互补滤波将陀螺仪和加速度计的计算结果进行融合得到初步的姿态角；所述自适应具体为：重力场互补滤波器的时间常数τ随重力加速度在载体坐标系中的分量和加速度计测量值的叉积e变化，即τ为关于e的分段单调增函数。
进一步地，所述通过摄像头采集的数据得到航向角具体为：
(1)对摄像头采集到的每张图像进行orb特征检测，提取出orb特征点；
(2)在摄像头采集到的相邻图像之间进行特征匹配和ransac野值剔除，得到匹配好的特征点对；
(3)根据匹配好的特征点对，在每张图像中利用pca主元分析法得到一个主要的方向矢量；
(4)通过求解相邻图像中方向矢量的方向变化，得到摄像头的航向角增量，也即载体的航向角增量，将航向角增量累加后即可得到载体的航向角。
进一步地，所述处理器的融合计算采用松耦合方式，具体为：利用卡尔曼滤波的方法将通过惯性测量单元得到的姿态角作为预测量，将通过摄像头得到的航向角作为观测量，并对二者进行融合，得到载体最终的航向角。
进一步地，所述处理器的融合计算采用紧耦合方式，具体为：
(1)通过惯性测量单元中加速度计的测量数据估计出载体在垂直方向的运动，得到摄像头采集到的图像粗略的尺度预估计值，根据尺度预估计值设置图像中orb特征检测的阈值、金字塔的层数以及比例因子，实现orb特征点的稳定提取；
(2)利用摄像头采集图像的频率低于惯性测量单元这一性质，将前一次视觉解算的结果和两次视觉解算之间由加速度计的测量数据计算得到的横滚角和俯仰角作为校正使用的姿态角，通过互补滤波对陀螺仪采集到的数据计算出的姿态角进行校正，最终得到载体的航向角。
进一步地，还包括辅助测量装置，所述辅助测量装置为两轴或三轴稳定云台，用于安装摄像头，使得摄像头始终采集到的是地面的水平俯视图，图像采集平面不会随着载体的横滚角和俯仰角而变化。
进一步地，所述惯性测量单元的坐标系与载体坐标系一致，采用右手笛卡尔坐标系；初始状态时，载体水平放置，载体坐标系与摄像头坐标系的xy平面平行，z轴在同一条直线上，方向相反。
进一步地，所述定向罗盘适用于无人机飞行任务，尤其是在悬停及电磁场干扰较大的环境。
与现有技术相比，本发明的有益效果是：本发明融合了下视视觉信息和惯性传感器信息，大大降低了惯性导航系统的累积误差和温漂，在避免使用全景摄像头的同时，利用惯导的辅助作用减少了视觉解算的复杂度，大大提高了系统的实时性。此外，两种全自主传感器的融合及算法的改进使得系统能够应用于多种环境和任务，包括室内外以及有干扰的环境和无人机悬停等任务。系统整体可靠性和抗干扰性强，解算精度高且能够缩减成本。
附图说明
图1为本发明定向罗盘整体结构示意图；
图2为本发明实施例的定向罗盘安装示意图；
图3为本发明实施例的松耦合融合方式解算航向角的算法整体框图；
图4为本发明实施例的imu姿态解算框图；
图5为本发明实施例的视觉解算流程；
图6为本发明实施例的紧耦合融合方式解算航向角的算法框图；
图7为本发明实施例的紧耦合融合方式解算航向角的算法时序框图。
具体实施方式
本发明实施例提供了一种融合下视视觉与惯导信息的定向罗盘，用于测量三维空间中载体的航向角，可以可靠地以高实时性、高精度的方式提供载体的航向角信息。
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例的附图对本发明的技术方案进行清楚、完整的描述。显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其它实施例，都应当属于本发明保护的范围。
图1和图2是本发明提供的一种融合下视视觉与惯导信息的定向罗盘结构示意图，该定向罗盘包括：数据采集单元、处理器4和辅助测量单元5；
所述数据采集单元，包含一个惯性测量单元2和一个下视安装的单目摄像头3。
惯性测量单元2固定在载体1上，其传感器坐标系与载体坐标系一致，采用“前-右-下”右手笛卡尔坐标系。惯性测量单元2包含陀螺仪和加速度计，分别测量载体在三维空间中的角速度和加速度。
下视安装在载体1上的单目摄像头3，其坐标系的xy平面平行于载体坐标系的xy平面，z轴与载体坐标系共轴。
所述处理器4，通过惯性测量单元2采集的数据得到姿态角，通过单目摄像头3采集的数据得到航向角，并对上述姿态角和航向角进行融合，得到载体1最终的航向角。
所述辅助测量单元5，采用三轴无刷云台，安装在载体1的下方，用于安装下视的单目摄像头3，使得摄像头坐标系的xy平面始终保持水平，单目摄像头3不受载体1的横滚角和俯仰角的变化而变化。
图3是本发明实施例的以松耦合的融合方式解算航向角的算法整体框图；参照图3，利用惯性测量单元2的测量数据解算得到载体的姿态角，同时利用单目摄像头3采集到的图像解算得到载体1的航向角，最终将惯性测量单元2和单目摄像头3解算的结果进行融合，最终得到载体的航向角，实现能够适应多种环境的融合下视视觉与惯导信息的定向罗盘。
图4是本发明实施例中惯导姿态解算的算法框图。参照图4，将惯性测量单元2中的陀螺仪输出的载体1在三维空间中的角速度进行积分校正得到角增量，如(1)所示：
其中：
ω(t)——陀螺仪采集到的载体在惯性坐标系中t时刻的三轴角速度；
ωx——陀螺仪采集到的载体在惯性坐标系中的x轴角速度；
ωy——陀螺仪采集到的载体在惯性坐标系中的y轴角速度；
ωz——陀螺仪采集到的载体在惯性坐标系中的z轴角速度；
[·]——反对称矩阵；
δβ——载体在惯性坐标系中的三轴角增量；
δβx——载体在惯性坐标系中的x轴角增量；
δβy——载体在惯性坐标系中的y轴角增量；
δβz——载体在惯性坐标系中的z轴角增量；
然后通过求解微分方程：
也即：
其中：
q——四元数，通常表示为：q＝q0+q1i+q2j+q3k，其中i、j、k是三个虚数单位。
得到微分方程的解析解为：
其中i为单位矩阵。
由于求解四元数解析解(3)的过程中使用了角速度矢量的积分(1)，从而引入了不可交换性误差，因此需要对角增量进行一定的处理，即在一个姿态更新周期内，用一次函数近似陀螺的角速度输出，求得等效旋转矢量φ：
其中t为采样时间。
最终将四元数微分方程转化为等效旋转矢量微分方程(bortz方程)：
其中
最后再用等效旋转矢量φ经过一定处理后代替四元数微分方程解中的δβ，即可消除四元数算法中的不可交换性误差得到载体1初步的姿态角。
同时将加速度计输出的载体1在三维空间中的加速度进行几何解算得到载体1的横滚角和俯仰角：
其中：
θ——载体的俯仰角；
最后将陀螺仪解算得到的横滚角、俯仰角和航向角以及加速度计解算得到的横滚角和俯仰角，进行自适应的重力场互补滤波。其中自适应具体为重力场互补滤波器的时间常数τ随重力加速度在载体坐标系中的分量和加速度计测量值的叉积e变化，即τ为关于e的分段单调增函数，在第一区间中τ随e的增长速率逐渐增大，第二区间中τ随e的增长速率逐渐减小。最终得到由惯性测量单元2解算得到的载体1的姿态角。
图5是本发明实施例中视觉航向角解算的算法流程。参照图5，视觉航向角解算采用基于图像特征的方法。首先对单目摄像头3采集到的图像进行orb特征检测，由于摄像头是安装在高空飞行的无人机上，其采集到的俯视图像尺度较大，可适当调整特征点检测的阈值、金字塔的层数以及比例因子，得到数量较少但更加可靠的特征点，对图像序列中的多幅图像进行特征匹配和ransac野值剔除，得到匹配好的特征点对。然后在每张图像中根据匹配好的特征点对，利用pca主元分析法计算出一个主要的方向矢量，作为该图像的总体特征，并根据相邻图像方向矢量的方向变化对摄像头3的运动进行姿态估计，得到摄像头3的航向角增量，通过航向角增量累加即可得到航向角的绝对值。由于辅助测量单元5的作用以及前述摄像头坐标系和载体坐标系的xy面共面，z轴共轴，摄像头3的航向角即为载体1的航向角。
根据上述定向罗盘装置的结构说明和载体航向角的计算方法示例，通过卡尔曼滤波的方法将下视视觉信息与惯导信息进行松耦合，将通过惯性测量单元得到的姿态角作为预测量，将通过摄像头得到的航向角作为观测量，使得二者相互辅助和校正，得到载体最终的航向角，提高了系统的可靠性和抗干扰能力。卡尔曼滤波包括时间更新和量测更新两个过程：
xk＝φk，k-1xk-1+γk，k-1wk-1(7)
zk＝hkxk+vk(8)
其中：
xk——系统当前时刻的状态矩阵，n*1维；
φk,k-1——表示系统的状态转移矩阵，n*n维；
γk,k-1——系统噪声输入矩阵，n*p维；
wk-1——系统噪声矩阵，p*1维；
zk——系统观测向量，m*1维；
hk——观测矩阵，m*n维；
vk——观测噪声，m*1维。
在一个更新周期内的具体步骤为：
(1)状态一步预测：
(2)一步预测均方误差估计：
(3)卡尔曼滤波增益计算：
(4)卡尔曼滤波系统状态估计：
(5)系统状态均方误差估计：
pk＝[i-kkhk]pk，k-1(13)
根据前述定向罗盘装置的结构说明和载体航向角的计算方法示例，本发明还可采用紧耦合的融合方式作为另一实施例。
图6是本发明实施例的紧耦合融合方式解算航向角的算法框图，图7是本发明实施例的紧耦合融合方式解算航向角的算法时序框图。参照图6和图7，将惯性测量单元和摄像头采集到的数据融合到相互之间的计算中，进一步提高系统的可靠性和精度。具体实施过程如下：
首先，通过惯性测量单元中加速度计的测量数据估计出载体在垂直方向的运动，从而得到摄像头采集到的图像粗略的尺度预估计值，根据尺度预估计值设置图像中orb特征检测的阈值、金字塔的层数以及比例因子，实现orb特征点的稳定提取。
其次，由于摄像头采集图像的频率低于惯性测量单元，因此可将上一次由摄像头采集的数据解算得到的航向角和两次视觉解算时间间隔中的由每一次加速度计测量数据计算得到的横滚角和俯仰角共同作为校正值，校正每一次由陀螺仪测量数据计算得到的姿态角，得到载体最终的航向角。
本发明采用的拥有较高计算能力的处理器4，大大提高了系统的实时性。此外，辅助测量单元5在保证精度的前提下减少了系统的计算量。
上述实施例仅为本发明较佳可行的实施例，用于说明本发明的技术方案，并非局限本发明的保护范围。尽管参照前述实施例对本发明进行了详细的说明，但是依然可以在不背离权利要求及其等同例的精神和范围下，对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换，因此这些修改或替换均在此技术方案保护范围之内。