基于扩展自适应区间卡尔曼的无人机组合导航方法与装置制造方法及图纸

本发明专利技术公布了一种基于扩展自适应区间卡尔曼的无人机组合导航方法与装置，本发明专利技术采用微机电惯性测量单元测量无人机的惯性导航参数，视觉导航系统测量无人机的视觉导航参数；使用数字信号处理处理模块作为导航计算机，通过扩展自适应区间卡尔曼滤波技术作为组合方法融合惯性、视觉导航参数，从而得到系统导航参数的最优估计。本发明专利技术装置易于实现，成本低廉，质量较小，功耗较低，适合民用领域的推广；通过扩展自适应区间卡尔曼滤波技术，可得到线性化的系统模型；并将系统的时变参数建模为自适应的区间模型从而提高导航系统在温度变化、运载体频繁的姿态变化、光照条件不理想，或者视觉传感器定焦困难等复杂、恶劣情形下的导航定位任务。

Integrated navigation method and device for Unmanned Aerial Vehicle Based on extended adaptive interval Calman

The invention discloses a UAV navigation method and device combination of extended adaptive interval based on Calman, the invention adopts the MEMS inertial measurement unit to measure the parameters of UAV inertial navigation, visual navigation parameter measurement without man-machine vision navigation system; digital signal processing module of the navigation computer, by extending the adaptive interval Calman filter as a combination of fusion method, visual navigation inertial parameters, so as to get the optimal estimation of navigation parameter. The device of the invention is easy to implement, low cost, low quality, low power consumption, suitable for the promotion of civil field; through extended adaptive interval Calman filter technology, can obtain the linearized model of the system; and the system parameter is modeled as an adaptive interval model to improve the navigation system in the temperature change, attitude change, carrier body frequent light conditions, navigation tasks or visual sensor focusing difficult complex and bad condition.

【技术实现步骤摘要】
基于扩展自适应区间卡尔曼的无人机组合导航方法与装置
本专利技术涉及一种用于无人机的基于扩展自适应区间卡尔曼的惯性/视觉/超声波组合导航方法及装置。
技术介绍
随着导航系统微型化、智能化、自主化、低成本的发展趋势，并且在导航系统自主化重要性日益突出的背景下，惯性/视觉组合导航技术以其良好的优势互补和自主特性，已经成为了导航领域一个重要发展方向。在无人机的定点悬停、自主飞行与降落与机器人的目标识别与跟踪及环境检测等任务中，惯性/视觉导航技术表现出能与外界环境相交互、动态性能好、实时性高等诸多优越性。在民用级惯性/视觉导航系统中，由于器件或设备本身精度和稳定性的不足，系统参数和噪声统计特性往往会随着温度、光照条件等外界环境的变化而发生显著改变。对于实际的工程条件中，系统模型往往是非线性的、噪声统计特性也因某些条件会在一定区间范围内波动，比如在温度变化、运载体频繁的姿态变化、光照条件不理想，或者视觉传感器定焦困难等情形下，惯性传感器和视觉传感器的测量值往往会包含成分较为复杂的噪声分量甚至会出现随机野值，并且噪声的成分通常也会随着外界条件的变化而发生改变。传统的卡尔曼滤波器在上述条件下将无法获得系统的最优估计，甚至会发生滤波器发散而导致导航系统崩溃的现象。研究者们一般在处理卡尔曼滤波过程中系统模型非线性的问题时，往往是将非线性系统通过近似方法线性化的方式得到系统的线性方程，这类方法主要包括扩展卡尔曼滤波、无迹卡尔曼滤波等。扩展卡尔曼滤波算法在系统函数特征点附近通过泰勒展开的方式省略了系统模型高阶项从而得到系统的线性模型，有效提高了滤波器的鲁棒性和滤波精度。但当系统参数或噪声统计特性时变或者不确切已知时，扩展卡尔曼滤波对此无能为力。区间卡尔曼滤波算法是在区间概念的基础上，通过将系统或噪声参数的不确定性建模为区间模型的一种处理无法精确对系统或噪声建模时的鲁棒滤波算法。区间卡尔曼滤波算法在不增加额外计算量的基础上可有效降低模型不精确条件下对滤波器造成的影响。
技术实现思路
本专利技术的技术解决问题是：对卡尔曼滤波技术进行拓展和融合，结合扩展卡尔曼方法和区间卡尔曼方法两种滤波算法的优点，建立了基于惯性/视觉/超声波的新型组合导航方法。该方法有效克服了传统方法由于非线性系统模型以及不确定因素影响下导航精度及鲁棒性的欠缺，得到置信度更高、信息更为完备的导航参数，有效提高了导航系统在未知环境中的稳定性和适应性，增强了导航系统的实时性，可使运载体完成过程更复杂、环境更恶劣的导航任务。本专利技术为实现上述目的，采用如下技术方案：一种基于扩展自适应区间卡尔曼的无人机组合导航方法，包括：通过惯性导航获得无人机在导航坐标系下的速度信息和位置信息，作为组合导航参数的状态信息；通过视觉导航获得无人机在摄像头坐标系下的沿水平方向上的光流速度信息和飞行高度信息，作为组合导航参数的观测信息；通过扩展卡尔曼滤波将非线性系统线性化，并利用自适应区间卡尔曼滤波将时变的系统参数建模为自适应的区间模型，构建组合导航滤波模型，融合惯性、视觉导航参数，对系统导航参数做出最优估计。作为优选，所述组合导航滤波模型为：其中，与分别为k时刻系统状态向量的最优估计和先验估计，为k-1时刻系统状态向量的最优估计；Kk为k时刻卡尔曼滤波增益；与Pk分别为k时刻一步预测误差方差矩阵和估计误差方差矩阵，Pk-1则为k-1时刻的估计误差方差矩阵；f代表系统状态函数；Bk为系统的控制分配矩阵；Uk为系统的控制向量；Ak为状态方程的雅克比矩阵；Hk为观测方程的雅克比矩阵；Wk为状态噪声分配矩阵；Vk为观测噪声分配矩阵；Zk为观测向量；与分别为状态噪声矩阵和观测噪声矩阵的区间模型；上标I为区间矩阵标识，表示该矩阵向量为区间矩阵向量；表示观测噪声为零时，关于先验估计的系统观测函数。作为优选，与分别表示为：其中，ΔQk与ΔRk分别为状态噪声矩阵和观测噪声矩阵的变化范围；通过计算残差的实际量测方差与理论方差的矩阵的迹之比来对滤波模型的准确性作出评价；当模型估计偏差较大时，根据ΔRk＝(τe|ε-1|-1)·C修正观测噪声，其中，ak为平滑因子,τ是自适应函数的比例因子,C为动态观测噪声的基准矩阵。作为优选，系统状态向量和观测向量分别为：Zk＝[vxvyZc]T其中，Xn,Yn,Zn分别表示通过惯性导航得到的无人机在导航坐标系下沿x,y,z三个轴的位置，分别表示通过惯性导航得到的无人机在导航坐标系下沿x,y,z三个轴的速度；vx,vy分别表示通过视觉导航得到的无人机在摄像头坐标系下沿x,y两个轴即水平方向上的光流速度信息，Zc表示无人机的飞行高度。作为优选，状态方程和观测方程的雅克比矩阵分别为其中，dt为状态更新周期；l为视觉导航中视觉传感器的焦距。一种采用上述组合导航方法的基于扩展自适应区间卡尔曼的无人机组合导航装置，包括微机电系统惯性测量单元、超声波传感器、视觉传感器和数字信号处理模块；所述微机电系统惯性测量单元用于获取载体的角速度和线速度信息；所述视觉传感器用于获取载体周围图像信息；所述超声波传感器用于获取无人机飞行高度信息；所述数字信号处理模块，包括：惯性导航解算单元，用于根据载体角速度和线速度信息解算得到无人机的姿态、速度和位置信息；视觉导航解算单元，用于根据载体周围图像信息及飞行高度解算得到无人机在水平方向上的速度信息；以及滤波算法单元，用于根据所获取的导航参数结合组合导航滤波模型执行扩展自适应区间卡尔曼滤波算法对系统导航参数做出最优估计。作为优选,所述微机电系统惯性测量单元采用型号为MPU9250的传感器，其内包括三轴陀螺，三轴加速度计，三轴磁力计；所述数字信号处理模块采用型号为TMS320C5509的DSP；所述视觉传感器型号为MT9V034；所述超声波传感器型号为MB1043。本专利技术提出的一种基于扩展自适应区间卡尔曼滤波的无人机的惯性/视觉/超声波组合导航方法及装置，其中新型的滤波方法融合了扩展卡尔曼滤波和区间卡尔曼滤波两者的优点，主要解决了两个问题，其一是将非线性系统模型通过扩展卡尔曼方法得到了系统的线性模型；其二是通过区间概念建立状态噪声和观测噪声的区间模型，并通过自适应方法动态调整区间的变化范围，以降低噪声统计特性变化对滤波效果的影响。本专利技术与现有技术相比的优点在于：(1)应用于无人机的基于MEMS的惯性测量单元MPU9250、视觉传感器MT9V034、超声波传感器MB1043、数字信号处理模块TMS320C5509的组合导航装置易于实现，成本低廉，质量较小，功耗较低，适合民用领域的推广；(2)融合了扩展卡尔曼和区间卡尔曼两种滤波算法的优点，不仅可以对非线性系统进行滤波降噪，还可以将系统参数的不确定性通过区间建模的方法进行处理；(3)避免了针对特殊环境优化的复杂建模过程，将系统的噪声统计特性描述为在一定范围内波动的区间模型，并通过自适应的方法动态改变区间的范围，是一种动态的滤波算法，避免各种不确定性或随机野值对算法的影响，可有效提高导航系统对不确定因素的适应性，从而在温度变化、运载体频繁的姿态变化、光照条件不理想，或者视觉传感器无法定焦等情形下提供精度更高、数据更为可靠的导航定位服务。附图说明图1为本专利技术的用于无人机的组合导航系统原理示意图；图2为本专利技术的视觉导航参数计本文档来自技高网...

【技术保护点】
一种基于扩展自适应区间卡尔曼的无人机组合导航方法，其特征在于，包括：通过惯性导航获得无人机在导航坐标系下的速度信息和位置信息，作为组合导航参数的状态信息；通过视觉导航获得无人机在摄像头坐标系下沿水平方向上的光流速度信息和飞行高度信息，作为组合导航参数的观测信息；通过扩展卡尔曼滤波将非线性系统线性化，并利用自适应区间卡尔曼滤波将时变的系统参数建模为自适应的区间模型，构建组合导航滤波模型，融合惯性、视觉导航参数，对系统导航参数做出最优估计。

【技术特征摘要】
1.一种基于扩展自适应区间卡尔曼的无人机组合导航方法，其特征在于，包括：通过惯性导航获得无人机在导航坐标系下的速度信息和位置信息，作为组合导航参数的状态信息；通过视觉导航获得无人机在摄像头坐标系下沿水平方向上的光流速度信息和飞行高度信息，作为组合导航参数的观测信息；通过扩展卡尔曼滤波将非线性系统线性化，并利用自适应区间卡尔曼滤波将时变的系统参数建模为自适应的区间模型，构建组合导航滤波模型，融合惯性、视觉导航参数，对系统导航参数做出最优估计。2.根据权利要求1所述的一种基于扩展自适应区间卡尔曼的无人机组合导航方法，其特征在于，所述组合导航滤波模型为：其中，与分别为k时刻系统状态向量的最优估计和先验估计，为k-1时刻系统状态向量的最优估计；Kk为k时刻卡尔曼滤波增益；与Pk分别为k时刻一步预测误差方差矩阵和估计误差方差矩阵，Pk-1则为k-1时刻的估计误差方差矩阵；f代表系统状态函数；Bk为系统的控制分配矩阵；Uk为系统的控制向量；Ak为状态方程的雅克比矩阵；Hk为观测方程的雅克比矩阵；Wk为状态噪声分配矩阵；Vk为观测噪声分配矩阵；Zk为观测向量；与分别为状态噪声矩阵和观测噪声矩阵的区间模型；上标I为区间矩阵标识，表示该矩阵向量为区间矩阵向量；表示观测噪声为零时，关于先验估计的系统观测函数。3.根据权利要求2所述的一种基于扩展自适应区间卡尔曼的无人机组合导航方法，其特征在于，与分别表示为：其中，ΔQk与ΔRk分别为状态噪声矩阵和观测噪声矩阵的变化范围；通过计算残差的实际量测方差与理论方差的矩阵的迹之比来对滤波模型的准确性作出评价；当模型估计偏差较大时，根据ΔRk＝(τe|ε-1|-1)RC修正观测噪声，其中，ak为平滑因子,τ是自适应函数的比例因子,C为动态观测噪声的基准矩阵。4.根据权利要求2所述的一种基于扩展自适应区间卡尔曼的无人机组合导航方法，其特征在于,系统状态向量和观测向量分别为：Zk＝[vxvyZc]T其中，Xn,Yn,Zn分别表示通过...

【专利技术属性】
技术研发人员：陈熙源，章怀宇，方琳，柳笛，汤新华，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏,32