一种基于因子图的UAV多源导航信息处理系统

一种基于因子图的uav多源导航信息处理系统
技术领域
1.本发明涉及组合导航、多源信息融合领域，特别涉及一种基于因子图的uav多源导航信息处理系统。


背景技术：

2.高空长航时无人机(unmanned aerial vehicle，uav)能够对所侦察地区进行昼夜持续监视，在战场态势感知方面发挥着关键作用，已发展成为侦察卫星的重要补充手段。高空长航时无人机的作战环境复杂，一般面临着强烈的电磁干扰，这对其上搭载的导航系统稳定性和可靠性有着更高的要求，单一的gnss乃至惯用的ins/gnss组合导航系统已无法满足其导航定位需求，亟待需要一种新的导航解决方案。
3.复杂环境下的长时间精确导航定位依赖更多导航传感器的整合集成，这需要对多源导航技术进行研究。信息融合是多源导航技术的最基本问题，目前业内一般通过联邦卡尔曼滤波对多传感器数据进行融合，然而各导航传感器的信息更新频率大多异步，联邦滤波只能等待所有信息到来后才能进行信息融合，随之带来信息迟滞误差，会造成导航精度下降，并不能发挥的多传感器集成的全部优势，迫切需要开发一种新的多源导航信息处理系统。


技术实现要素：

4.本发明解决的技术问题是：针对现有技术中复杂环境下已有组合导航系统不能满足长航时uav高精度导航定位需求的情况，本发明提供了一种基于因子图的uav多源导航信息处理系统，该系统将卫星惯性组合导航系统、高精度imu、星敏感器、气压高度计、低精度ahrs多种导航传感器与内置因子图多源信息融合算法的导航主模块进行软硬件集成，并可通过导航io模块进行后续传感器扩展。本发明以高精度imu为核心，融合了多种导航传感器，能够根据时空背景对传感器组合进行灵活配置，实现异步异构信息的最优融合，满足长航时uav高精度的导航定位需求。
5.实现本发明目的的技术解决方案为：
6.所述导航io模块，用于将导航主模块解算的导航数据传输给uav上的其他电子设备；
7.所述imu作为参考导航传感器，用于向导航主模块传输uav的三轴加速度和三轴角速度数据；
8.所述卫星惯性组合导航系统，用于向导航主模块传输uav的三轴加速度、三轴角速度、三轴速度、纬度、经度、高度数据；
9.所述星敏感器，用于向导航主模块传输uav的三轴姿态角数据；
10.所述气压高度计，用于向导航主模块传输uav的气压高度数据；
11.所述ahrs，用于向导航主模块传输uav的航姿参考数据；
12.所述导航主模块，用于集成多种导航传感器，在设置的滑动窗口内基于因子图理
论综合处理各传感器返回的数据，解算导航状态。
13.本发明的显著优点是：
14.(1)本发明所涉及的多源信息融合算法，通过简单地增减因子节点即可实现传感器的优化配置和即插即用，同时基于最大后验概率对导航状态集进行最优估计，对于因子图随时间扩张使得算法计算量增大的问题，创新性的引入了自适应滑动窗口，在保证导航精度的基础上通过动态限制参与优化的因子节点范围，有效的减少了算法计算量，提高了算法效率，进一步提高了导航性能。
15.(2)本发明的导航io模块预留有丰富的传感器接口，将来可直接用于多源导航系统的传感器扩展，因此本发明具有拓展性强的特点。
16.(3)本发明的导航传感器较多的采用了mems惯性器件，较之传统的惯性器件，降低了体积与功耗，且易于集成，因此本发明的硬件系统具有小型化、轻量化、低功耗的特点。
附图说明
17.图1虚框内为本发明一种基于因子图的uav多源导航信息处理系统的硬件连接示意图。
18.图2为本发明一种基于因子图的uav多源导航信息处理系统的多源信息融合算法原理示意图。
19.图3为本发明一种基于因子图的uav多源导航信息处理系统的多源信息融合算法流程示意图。
20.图4为本发明一种基于因子图的uav多源导航信息处理系统的硬件效果图。
21.图5为本发明一种基于因子图的uav多源导航信息处理系统的硬件效果俯视图。
22.图6为本发明一种基于因子图的uav多源导航信息处理系统的仿真效果图。
23.图7为本发明一种基于因子图的uav多源导航信息处理系统的仿真位置误差曲线图。
具体实施方式
24.下面结合附图及具体实施例对本发明做进一步的介绍。
25.结合图1，本实施例的一种基于因子图的uav多源导航信息处理系统，包括导航主模块、导航io模块、卫星惯性组合导航系统、高精度imu、星敏感器、气压高度计和低精度ahrs。
26.进一步地，所述导航主模块是一块arm芯片，型号为stm32h753zi，具备cortex-m7内核，作为多源导航信息处理系统的核心器件读取各传感器数据，运行基于因子图的多源信息融合算法程序。导航主模块通过隔离型rs-232接口连接卫星惯性组合导航系统；通过rs-422接口连接高精度imu；通过隔离型rs-422接口连接星敏感器；通过异步通信串口uart连接ahrs；通过同步串行接口spi连接气压高度计；通过spi串口和uart串口连接导航io模块进行数据通讯。导航主模块的算法可以由专业人员通过隔离型rs-232接口进行上位机测试维护。
27.进一步地，所述导航io模块是一块arm芯片，型号为stm32h753zi，具备cortex-m7内核，可通过标准flexray接口连接uav上的其他电子设备，具有4个隔离型rs-422接口可用
于后续传感器扩展。
28.进一步地，所述的卫星惯性组合导航系统包括mems三轴加速度计、mems三轴陀螺仪、gnss板卡，能够测量三轴加速度、三轴角速度、三轴速度、纬度、经度、高度，并能通过隔离型rs-232接口向导航主模块传输卫星惯性组合导航数据。
29.进一步地，所述的高精度imu包括mems三轴加速度计、mems三轴陀螺仪，能够测量三轴加速度、三轴角速度，并能通过rs-422接口向导航主模块传输imu数据。
30.进一步地，所述的星敏感器能够测量三轴姿态角，并通过隔离型rs-422接口向导航主模块传输姿态角数据。
31.进一步地，所述的气压高度计型号为压阻式硅微传感器ms5611，能够测量气压和高度，通过同步串行接口spi向导航主模块传输气压高度数据。
32.进一步地，所述的低精度ahrs型号为9轴绝对方向传感器bno055，集成三轴陀螺仪、三轴加速度计与三轴磁强计，能够在uav平稳飞行条件下自主测量飞行姿态角，通过异步通信串口uart向导航主模块提供航姿参考数据。
33.结合图2和图3，所述的导航主模块运行的基于因子图的多源信息融合算法程序包括以下3个处理单元：因子图模型构建单元、滑动窗宽度确定单元、导航状态解算单元。
34.进一步地，所述的因子图模型构建单元，用于对多源导航信息处理系统进行抽象建模，为基于因子图进行导航状态估计提供基础，其具体处理过程为：
35.a.采用因子图描述导航状态变量的联合概率分布，将uav多源导航信息处理系统的导航状态抽象为变量节点，将来自不同导航传感器的异步异构测量信息抽象为因子节点，并根据不同导航传感器特性建立因子节点相对应的代价函数，构建多源导航信息处理系统的因子图模型，当新的测量信息到来后，通过在已有因子图模型的基础上增加变量节点和因子节点的方式扩展模型；该因子图模型实质是多个局部函数乘积所表示的全局函数，具有多个变量，每个变量对应一个变量节点，每个局部函数对应一个因子节点，因子图的边线由变量节点和因子节点的连线组成，并且仅当相应变量节点是因子节点的自变量时，两者才会相连。
36.b.利用当前时刻的导航状态表示模型新扩展的变量节点式中下标k代表不同时刻；ak＝[θ γ ψ]
t
为uav的俯仰角θ、滚转角γ、方位角ψ组成的姿态向量；vk＝[v
e v
n vu]
t
为uav在当地地理坐标系下的东向速度ve、北向速度vn、天向速度vu组成的速度向量；pk＝[l λ h]
t
为uav所在的纬度l、经度λ、高度h组成的位置向量；为加速度计的三轴偏置；为陀螺仪的三轴偏置。
[0037]
c.利用多元随机变量的联合概率密度函数表示不同导航传感器的异步异构测量信息抽象得到的因子节点。参考导航传感器测量信息抽象得到的因子节点表示为f(x
k+1
|xk)，其他导航传感器测量信息抽象得到的因子节点表示为式中x
k+1
为参考导航传感器根据当前导航状态变量xk对下一导航状态的预测，为除参考导航传感器外第i个导航传感器对当前导航状态变量xk的测量信息。
[0038]
d.因实际的联合概率密度函数难以获取，建立代价函数描述因子节点对应的联合
概率密度函数。假设测量信息与测量噪声分别服从高斯分布，所述的参考导航传感器高精度imu的因子节点可展开如下
[0039][0040]
式中，表示平方马氏距离，n为导航状态向量xk的维度，为当前时刻高精度imu的测量信息，为高精度imu的测量模型函数，为当前时刻高精度imu的测量噪声协方差矩阵。将高精度imu因子节点的代价函数用平方马氏距离表示如下
[0041][0042]
e.所述的其它导航传感器的因子节点可展开如下
[0043][0044]
式中，上标si代表除imu外的第i个传感器，zk表示该传感器当前时刻的测量信息，为该传感器的测量模型函数，为当前时刻该传感器的测量噪声协方差矩阵。同样使用平方马氏距离表示其它导航传感器的代价函数
[0045][0046]
进一步地，所述的滑动窗宽度确定单元，用于限制参与状态估计的历史信息量大小，提高导航状态集解算速度和估计准确性，其具体处理过程为：
[0047]
a.设置自适应的滑动窗口，并采用定步长、变窗宽的滑动规则，使其能够根据uav动态性自适应的调整窗口宽度，窗口宽度与窗口内变量节点数目相等。
[0048]
b.建立uav姿态与滑动窗口宽度w
sw
的关联模型，在新的滑动窗口产生前，根据该关联模型确定窗口宽度，uav姿态变化与滑动窗口宽度w
sw
的关联模型具体如下
[0049]wsw
＝c-n
att
*sum(sign(δa
k-a
*
))
[0050]
式中，sum(.)为向量求和函数；sign(.)为定义的符号函数，当输入参数为非负数时，返回值为1，否则返回值为0，输入参数为向量时返回由1和0组成的向量；c为滑动窗口宽度上限，取值视导航主模块芯片的算力而定，且最小为10；n
att
为姿态向量的维度，即n
att
取3，δak为uav当前时刻相对于上时刻的姿态变化向量，计算公式为
[0051]
[0052]
其中，为当前时刻各测姿传感器对uav姿态测量信息的加权平均，有权值mi可通过计算，为第i个测姿传感器误差协方差矩阵的逆，为上时刻对uav的姿态估计，δθk、δγk、δψk分别为计算所得俯仰角、滚转角、方位角的改变量，所述的测姿传感器包括ahrs、imu和星敏感器；a
*
为姿态变化阈值向量，有a
*
＝[θ
* γ
* ψ
*
]
t
，θ
*
、γ
*
、ψ
*
分别为俯仰角、滚转角、方位角的容许阈值。
[0053]
c.新的变量节点加入因子图后，窗口滑动产生新的窗，并根据uav姿态与滑动窗口宽度w
sw
的关联模型确定新窗口宽度，实现滑动窗口宽度随uav姿态改变而缩短，在uav平稳飞行时窗口宽度为c；在uav的俯仰角、滚转角、方位角三者其一改变超过容许阈值时，窗口宽度缩短为c-n
att
；在三轴姿态角三者其二的改变超过容许阈值时，窗口宽度缩短为c-2n
att
；三轴姿态角的改变均超过容许阈值时，窗口宽度为c-3n
att
。
[0054]
进一步地，所述的导航状态集解算单元，用于解算滑动窗口内变量节点所代表的导航状态集，得到当前时刻的导航状态估计并优化历史状态估计，其具体处理过程为：
[0055]
a.选取东北天即e-n-u地理坐标系为多源导航信息处理系统导航解算的坐标系，利用构建的代价函数基于最大后验概率对滑动窗口内的变量节点集合进行最优估计，构建多源导航信息处理系统的多源信息融合模型。
[0056]
b.所述的后验概率为滑动窗口内所有因子节点表示的联合概率密度函数乘积，表示如下
[0057][0058]
式中，x为滑动窗口内m个变量节点的集合；z为滑动窗口内所有测量信息的集合。基于最大后验概率对扩展的变量节点集合进行最优估计，等价于求解最小代价函数和的最优解集x
[0059][0060]
c.通过上述步骤将多源信息融合的最优状态估计问题转化为了一个典型非线性最小二乘优化问题，对于该问题采用通用的莱温伯格-马夸特算法(lm算法)求解，解集包括当前时刻的导航状态估计以及优化后的历史导航状态估计。
[0061]
具体的算法流程为：当前时刻未知的导航状态通过因子图模型构建单元抽象为新的变量节点加入因子图，因子节点由各传感器对于当前导航状态的测量信息抽象得到，在因子图扩展新的变量节点后，滑动窗向右滑动囊括新的节点，并通过滑动窗宽度确定单元计算新窗口宽度限制历史信息量，在此单元内根据uav姿态变化与姿态检测阈值的比较结果判断窗口宽度，待新的滑动窗形成后窗口内部的变量节点构成待解算导航状态集，进入导航状态集解算单元通过莱温伯格-马夸特算法(lm算法)解算，得到当前未知导航状态的最大后验估计，并对一定范围内的历史导航状态进行优化。在时间更新后，进入下一轮循环。
[0062]
结合如图4和图5，本发明的硬件外部设计有6个矩形连接器作为连接插座，用于连接uav上的其他电子设备与外部导航传感器，包括1个flexray接口1，1个隔离型rs-232接口
2，4个隔离型rs-422接口3、4、5、6。
[0063]
通过如下仿真试验验证本发明所提系统的性能效果：
[0064]
在仿真中设置imu采样时间间隔为0.02s，陀螺常值零偏为[0.01 0.015 0.02]
t
，单位(
°
/h)；加速度计常值偏置为[80 90 100]
t
单位(ug)；角度随机游走系数为[0.001 0.001 0.001]
t
，单位速度随机游走系数为[1 1 1]
t
，单位星敏感器采样时间间隔为0.1s，ahrs的采样时间间隔为0.02s，气压高度计的采样时间间隔为0.01s，卫星惯性组合导航系统的更新时间间隔为0.1s；滑动窗口宽度上限c设置为10。
[0065]
为充分验证本发明所提基于因子图的导航信息处理系统的导航精度，特与采用定窗宽(窗宽为10)的因子图方法对比分析。在仿真得到uav的规划航迹后，分别采用定窗宽的因子图方法和本发明的自适应窗口因子图方法进行解算，得到相应的位置导航数据，具体如图6所示。
[0066]
图7为仿真得到的位置误差曲线，可以看出在uav机动引发姿态变化时，定窗宽因子图方法会产生较大的位置估计误差，而本发明所提的方法采取自适应窗口，在uav姿态出现变化时适时的缩短窗宽，以此来减少历史信息量对当前导航状态估计的影响，进而提高进一步导航精度。
[0067]
以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。