多旋翼无人机导航与控制系统设计

摘要
多旋翼无人机已广泛应用于农业植保、电力巡检、航空摄影等行业。

然而，如何提高无人机在复杂环境下的安全稳定性始终是一个重要的课题。

本课题基于现有无人机硬件平台，以实现自主航点飞行为最终目的，研究导航算法和飞行控制算法。

主要研究内容如下：
研究选择现有的四旋翼机体和STM32嵌入式硬件，为导航和飞行控制算法研究提供实验测试平台。

导航算法研究，包括航姿解算和组合导航算法。

航姿解算以陀螺仪、加速度计、磁罗盘数据为基础，采用扩展卡尔曼理论推算机体的姿态角信息。

算法选择理想的卡尔曼滤波权重并设置为定值，以减少算法运算量。

此外，采用卫星定位数据对航向角进行补偿，提高姿态角信息的准确性。

组合导航算法以卫星定位信息为基础，并结合惯性传感器数据进行数据融合，弥补卫星定位信息的不稳定和数据延时等缺陷，得到平滑、稳定的速度与位置信息。

控制系统设计，从多旋翼无人机空间六自由度运动进行分析，建立姿态、速度、高度等运动的数学模型。

利用模型辨识工具，辨识出模型的具体参数。

根据控制系统结构，自主飞行控制系统的设计分为水平横向控制、水平纵向控制、高度控制、航向控制四个独立控制回路。

考虑到模型干扰以及易受负载变化影响的特点，采用模型参考滑模控制理论，以提高控制系统的鲁棒性能。

室外实验结果表明，设计的导航系统推算的姿态角、速度、位置数据与高精度参考设备输出数据基本吻合，可为无人机提供稳定精确的导航信息。

设计的控制系统完成了实验机的自主航点飞行，并具有良好的抗干扰与鲁棒性能，验证了导航算法与控制系统设计的有效性。

关键词：多旋翼，导航系统，扩展卡尔曼，飞行控制系统，模型参考滑模控制
I
ABSTRACT
Multi-rotor UA V has been widely used in agriculture, plant protection, power inspection, aerial photography and other industries. However, how to improve the safety and stability of UA V in complex environment is still a serious problem. Based on the existing hardware platform of UA V, the navigation algorithm and flight control algorithm are studied to realize autonomous point flight in this thesis. The main research contents are as follows.
The existing Quad-Rotor airframe and STM32 embedded hardware are selected to provide experimental test platform for navigation and flight control algorithm research. The research of navigation algorithm includes attitude calculation and integrated navigation. Based on gyroscope, accelerometer and magnetic compass data, the attitude information of the airframe is calculated by extended Kalman theory. The algorithm selects a stationary value as the Kalman filter weight, so that the computational complexity is reduced. In addition, the heading angle is compensated by satellite positioning data to improve the accuracy of attitude angle information. The data fusion of integrated navigation algorithm based on the satellite positioning information, combines inertial data to compensate for the instability and delay of satellite positioning information, and the smooth and stable speed and position information is obtained. To design the flight control system, the six-degree-of-freedom motion of multi-rotor UAV in space is analyzed, and the mathematical models of attitude, velocity and altitude are established. The system identification tool is used to identify the specific parameters of the model. According to the structure of the control system, the design of autonomous flight control system is divided into four independent control loops: horizontal lateral control, horizontal longitudinal control, altitude control and heading control. Considering the characteristics of model disturbance and susceptibility to load variation, the model reference sliding mode control theory is adopted to improve the robustness.
The outdoor experiment results show that, the calculated attitude angle, velocity and position data of the designed navigation system coincides well with the high-precision reference equipment, and can provide stable and accurate navigation information for UAV. The designed control system has completed the autonomous waypoint flight of the experimental machine, and has good anti-interference and robust performance, which verifies the effectiveness of the navigation algorithm and control system design.
Key Words：Multi-Rotor, Navigation system, Extended Kalman, Flight control system, Model reference sliding mode control
II
目录
摘要 (I)
ABSTRACT (II)
第一章绪论 (1)
1.1研究背景和意义 (1)
1.2国内外研究现状与发展趋势 (2)
1.3主要研究内容 (4)
第二章实验平台与整体方案设计 (6)
2.1多旋翼实验平台 (6)
2.1.1四旋翼机体 (6)
2.1.2嵌入式硬件平台 (7)
2.2整体方案设计 (9)
2.2.1导航系统方案设计 (10)
2.2.2控制系统方案设计 (10)
2.3本章小结 (11)
第三章导航系统设计 (12)
3.1 导航算法基础理论 (12)
3.1.1四元数理论 (12)
3.1.2卡尔曼滤波理论 (13)
3.2 传感器数据处理 (13)
3.2.1数据校准 (14)
3.2.2数据滤波 (14)
3.3 航姿参考子系统设计 (15)
3.4 组合导航子系统设计 (19)
3.5 本章小结 (22)
第四章飞行控制系统设计 (23)
4.1运动建模与模型辨识 (24)
4.1.1滚转运动模型 (24)
4.1.2速度运动模型 (25)
4.1.3位置模型 (26)
4.1.4航向运动模型 (26)
4.1.5高度运动模型 (26)
4.2 MRSMC控制器设计 (27)
III
4.2.1 参考模型设计 (28)
4.2.2 滑模控制器设计 (29)
4.3本章小结 (33)
第五章软件系统设计 (34)
5.1软件开发环境 (34)
5.2 IMU模块软件设计 (35)
5.3导航模块软件设计 (37)
5.4飞控软件设计 (37)
5.5 IAP固件升级 (40)
5.6本章小结 (42)
第六章实验验证与分析 (43)
6.1实验环境 (43)
6.1.1 MTI-G设备 (43)
6.1.2 RTK(Real-Time Kinematic)设备 (44)
6.1.3地面监控站 (44)
6.2导航算法验证 (45)
6.2.1航姿解算验证 (45)
6.2.2组合导航算法验证 (46)
6.3飞行控制系统验证 (47)
6.3.1姿态控制 (48)
6.3.2航向控制 (48)
6.3.3高度控制 (49)
6.3.4速度控制 (50)
6.3.5航点自主飞行控制 (50)
6.4本章小结 (51)
第七章总结与展望 (52)
7.1总结 (52)
7.2展望 (52)
致谢 (53)
参考文献 (54)
IV
第一章 绪论
1
第一章 绪论
1.1研究背景和意义
无人机为一种可借助遥控器操作或嵌入式算法编程完成自主飞行的无人驾驶的空中机器人。

与传统的有人飞机不同，无人机在很多领域内有着无比优越的安全性能，无人员伤亡危险，可以在人力所不能及的范畴提供新的帮助。

因此，无人机在当前和可预见的未来都具有非常广阔的应用前景[1-3]。

目前，市场上主流的无人机机型包括固定翼、直升机、多旋翼无人机等。

固定翼无人机主要用于高空探测、航拍测绘等，适用于长距离飞行应用场合。

但由于固定翼无人机的起降需要特定场地，无法定点悬停，以及高分子泡沫材料的机体结构，都限制了固定翼无人机在工业、农业等领域的发展与广泛应用。

直升机的研究目前主要针对军用需求，包括军用靶机、无人运输直升机等。

也有少量小型直升机应用于农业植保与电力巡线等民用领域。

多旋翼无人机的研究目前最为火热，具有机体结构简单、成本低廉、效费比高等优点。

现已广泛应用于现场救援、电力巡线、道路监测、农业植保、物流配送、气象环境检测等各行各业 [4-8]。

图1.1、1.2为多旋翼无人机典型应用。

图1.1 植保无人机 图1.2配送无人机
Figure1.1 Plant protection UAV Figure1.2 Distribution UAV 典型的多旋翼平台由无人机机体、动力单元、导航控制系统组成。

导航控制系统是无人机平台的核心，是整个无人机平台的神经中枢。

导航系统根据机载传感器设备以及复合导航算法，为无人机提供准确姿态角、速度和位置等状态反馈信息。

控制系统根据导航系统提供的反馈位姿，执行飞行控制算法，调整各旋翼电机的输出。

从而保持无人机飞行的稳定，完成特定的飞行方案。

导航的准确性以及控制的抗干扰与鲁棒性能对无人机稳定飞行至关重要。

因此，对导航与控制系统进行深入研究具有重要意义[9-11]。

江苏大学工程硕士学位论文
2
1.2国内外研究现状与发展趋势
国外针对多旋翼无人机的研究团队主要以大学的研究机构为主，主要研究多旋翼相关的科技前沿技术。

研究内容具有一定深度，但产业化应用一般。

麻省理工学院主要研究方向为无人机的数学建模及其控制方法，实现了基于视觉的室内高精度定位飞行，以及基于激光扫描仪的复杂环境自主飞行和飞机编队飞行[12-13]，如图1.3所示；斯坦福大学主要研究STARMAC 系列多旋翼无人机，如图1.4所示；宾夕法尼亚大学采用Ascending 公司的无人机平台，搭载室内视觉定位模块可达到毫米级定位精度[14-16] ，如图1.5所示；日本千叶大学的实验平台如图1.6所示，研究多旋翼无人机与智能无人驾驶车辆协同合作，以实现复杂环境下自主探勘功能[17-18]。

图1.3 麻省理工无人机实验平台
Figure 1.3 UAV experiment platform of MIT
图1.4 斯坦福大学STARMAC Ⅱ无人机
Figure 1.4 STARMAC Ⅱ
UAV of Stanford University
第一章 绪论
3
图1.5 宾夕法尼亚大学无人机
Figure 1.5 UAV of Pennsylvania University
图1.6 千叶大学无人机及电池自动更换系统
Figure 1.6 UAV and battery automatic replacement system of Chiba University
图1.7 清华大学无人机
Figure 1.7 UAV of Tsinghua University
我国针对多旋翼无人机的研究虽然起步较晚，但发展异常迅速，现已涌现出一大批优秀的研究机构与企业。

如清华大学、南京航空航天大学、东南大学、哈尔滨工业大学、国防科技大学、深圳市大疆创新科技有限公司、广州极飞科技有限公司
( XAIRCRAFT )
江苏大学工程硕士学位论文
等[19-21]。

清华大学自动化学院研发的无人机实验平台可感知三维空间的周边环境，通过搭载的高性能处理器将信息下传至地面监控端并进行三维地图构建，实现主动局部定位，如图1.7所示。

东南大学科学研究中心应用新型导航算法，成功研发出更高精度的组合导航系统，并成功搭载于无人机实现高精度飞行，具有很高的实际应用价值[22-23]。

深圳市大疆创新科技研发的消费级产品主要有精灵、悟、晓、御等，在国内外都具有极高的市场占有率。

导航系统一般包括惯性导航系统（Inertial Navigation System, INS）和全球导航卫星系统（Global Navigation Satellite System, GNSS）。

惯性导航采用惯性传感器进行主动定位，是一种独立的增量式局部定位系统，不受外部环境的影响，具有较强的环境适应性。

但长时间工作存在累积误差，因此不适于多旋翼无人机长时间飞行应用场景。

全球导航卫星系统采用卫星进行全局定位，可覆盖地球的大部分地区。

但卫星定位依赖于卫星信号，其定位质量易受地理位置、天气、应用环境影响，且存在更新频率较低，具有数据延时等缺陷。

因此，独立卫星导航难以满足无人机对实时定位于环境适应性的需求。

因此，课题采用廉价的MEMS传感器结合GNSS导航，并利用信息融合技术进行数据融合，以此来提高产品的导航性能。

目前，各研究机构主要采用的控制算法包括经典PID控制、线性控制、非线性控制、智能控制等。

经典PID控制为多旋翼无人机领域应用最为广泛的控制算法，算法设计不依赖于被控对象的数学模型，具有意义明确，易于调节等优点。

线性控制具有一套较为完善的控制器设计方法，其核心思想为将多旋翼无人机的动力学模型进行线性化，然后基于线性模型设计线性控制器，如线性二次高斯控制理论（LQG）。

非线性控制技术较为复杂，控制性能依赖于数学模型的准确性，当飞机存在较大外部扰动和建模误差时，其效果变差。

研究非线性控制方法的机构主要有斯坦福大学、加州大学伯克利分校、上海交通大学等。

智能控制主要包括模糊控制、鲁棒自适应控制、遗传算法控制及神经网络控制等方法，这些算法降低了控制本身对模型的依赖性，因此具有一定的自适应能力。

尽管智能控制在无人机控制中已经有了不错的应用，但仍有一定的缺陷。

1.3主要研究内容
课题以现有多旋翼机体与嵌入式硬件为基础，研究多旋翼无人机导航与飞行控制系统设计，并实现室外复杂环境自主飞行。

论文主要工作包括：导航系统设计，获取无人机的姿态、速度、位置等信息；基于参考模型滑模控制理论（Model Reference Sliding
4
第一章绪论
Mode Control，MRSMC）设计飞行控制系统，以获得较强的鲁棒性。

为提高导航系统性能，姿态解算时将扩展卡尔曼算法中卡尔曼权重设为定值，以减少算法运算量，并创新性的引入全球导航卫星系统（Global Navigation Satellite System，GNSS）数据对姿态角进行纠偏，提高姿态角的准确性。

第一章：绪论。

介绍导航与控制系统的研究背景和实际意义，并分析了当前国内外的研究现状。

第二章：平台介绍与方案设计。

介绍算法研究采用的机体与硬件平台架构，并确定导航系统、控制系统的整体设计方案。

第三章：导航系统算法设计。

基于扩展卡尔曼理论设计姿态解算与组合导航算法，推算姿态、速度、位置等信息。

第四章：飞行控制系统设计。

多旋翼无人机各自由度运动学建模，将无人机飞行控制系统划分为水平横向、水平纵向、高度和航向运动四个控制回路，基于MRSMC理论设计鲁棒控制器，并给出仿真实验结果。

第五章：软件系统设计。

针对飞控模块、IMU模块、导航模块分别进行软件系统整体流程设计。

第六章：实验验证与分析。

与高性能的商用传感器作对比，验证航姿和组合导航算法的实际效果。

通过飞行试验，验证飞行控制系统的性能。

第七章：总结与展望。

对论文工作内容进行总结与展望。

5
江苏大学工程硕士学位论文
6 第二章 实验平台与整体方案设计
本章节主要介绍实验平台并确定导航与自主飞行控制系统的整体设计方案。

研究以现有的多旋翼机体与嵌入式硬件平台为基础。

导航系统设计基于扩展卡尔曼理论，研究航姿解算与组合导航算法，获取无人机飞行位姿。

控制系统设计时，首先建立无人机运动学模型，并设计模型参考滑模控制器。

2.1多旋翼实验平台
本小节主要介绍现有的多旋翼机体与嵌入式硬件平台，用于验证论文研究的导航与飞行控制算法。

算法设计完成后，采用图2.1所示的四旋翼无人机实验平台进行算法性能测试与飞行验证。

2.1.1 四旋翼机体
图2.1 四旋翼实验平台
Figure 2.1 Quad-Rotor experimental platform
图2.1中的实验平台为团队设计的四旋翼“十字型”实验机型，轴距约700mm,主体机架采用碳纤维材料，具有较高的机体强度与较轻的质量。

动力系统选用格氏22.2V ，5300mAh 锂电池，电池尺寸140*43*60mm ，重量800g 。

电机选用朗宇X4108S(480KV)直流无刷电机，电机尺寸φ46.1*25.5mm ，最大可提供1.8Kg 拉力。

与电机配套的电调选择好赢40A ，允许最大瞬时电流60A,支持最高500Hz 的PWM 控制信号输入。

桨叶选择T-Motor 1335碳纤维桨。

整机机体含电池自重2Kg ，最大起飞重量约3Kg
，空载飞行时
第二章 实验平台与整体方案设计
间约20min 。

四旋翼平台采用的组件如表2.1所示，部分组件实物如图2.2所示。

表2.1 四旋翼无人机机体组件
Table 2.1 Airframe component of Quad-Rotor UAV
组件 型号/参数 制造商 质量/g 碳纤维机架 700*700*260 自制 580 电机 Sunnysky X4108S 朗宇 113*4 电调 Flyfun-40A 好赢 35*4 螺旋桨 T-Motor 1335 T-Motor 15*4 电池 22.2V, 5300mAh 格式 800 数传 Xbee-Pro 900HP Digi 50 遥控器
Futaba 14SG
日本双叶
----
图2.2 四旋翼组件实物图
Figure 2.2 Component drawing of Quad-Rotor
2.1.2 嵌入式硬件平台
嵌入式硬件为现有平台，从物理结构上可分为IMU 模块、导航模块、飞控模块三部分，模块之间通过CAN 总线进行数据通信。

为充分利用处理器资源，并尽量避免传感器数据传输导致的延时，课题研究的航姿算法在IMU 模块中运行，导航算法在导航模块中运行，飞行控制系统在飞控模块中运行。

嵌入式结构框图如图2.3所示。

电调
Xbee-Pro 900HP
格式锂电池好赢电调
朗宇电机
T-motor 桨叶
格式锂电池
好赢电调
朗宇电机T-motor 桨叶
格式锂电池
好赢电调
朗宇电机T-motor 桨叶
格式锂电池好赢电调
朗宇电机T-motor 桨叶
电机
桨叶
电池遥控器与接收机
江苏大学工程硕士学位论文
图2.3 嵌入式结构框图
Figure 2.3 Architecture of embedded system
图2.3中，IMU模块包含陀螺仪、加速度计、气压计等传感器，用于测量机体运动的角速度、加速度与高度信息。

处理器芯片读取传感器信息并数据处理后，进行航姿解算算法，获取无人机的姿态角信息。

导航模块主要包含GNSS、地磁计，用于接收卫星定位数据和地磁场数据。

处理器接收卫星定位信息，并结合CAN总线上的加速度、姿态角信息进行数据融合导航，获取机体速度与位置信息。

飞控模块主要负责遥控器信号解析，结合IMU模块与导航模块反馈的状态信息，进行飞行控制系统运算，输出PWM电机控制指令。

同时，通过无线电台与地面站监控平台进行数据交互。

各模块采用的处理器与传感器件如表2.2所示，对应的实物图如图2.4所示。

表2.2 嵌入式硬件器件表
Table 2.2 Hardware devices table of embedded system
第二章实验平台与整体方案设计
飞控模块IMU模块导航模块
图2.4嵌入式硬件模块实物图
Figure 2.4 Component drawing of embedded system module
2.2整体方案设计
课题以实现四旋翼无人机的航点飞行为目的，主要研究导航与控制系统。

导航系统负责实时反馈无人机的空间位姿，为控制决策提供数据基础。

控制系统根据任务目标与反馈信息生成相应控制量，控制飞机完成特定的动作。

基于2.1.2节介绍的STM32嵌入式硬件平台，论文主要研究框架如图2.5所示。

图2.5 研究框架图
Figure 2.5 Block diagram of research
江苏大学工程硕士学位论文
2.2.1导航系统方案设计
导航系统设计包含两部分内容：航姿参考子系统和组合导航子系统，其结构如图2.6所示。

其中，IMU 模块负责航姿参考子系统，输出无人机航姿信息。

然而，航姿信息无法从硬件传感器直接读取，需融合陀螺仪、加速度计、地磁计数据，经航姿解算算法推算得到。

导航模块负责组合导航子系统，输出实时速度、位置数据。

导航算法融合GNSS 模块、惯性传感器、高度计等多种传感器信息，得到更加稳定、精确的速度和位置数据，弥补GNSS 模块原始数据更新延迟、频率低、稳定性差等缺点。

航姿与导航算法设计采用扩展卡尔曼算法,算法通过动态调节卡尔曼滤波权重，能减小飞行过程中机体震动对推算结果的影响，在航空导航方面具有非常普遍的应用[24-26]。

图2.6导航系统结构框图
Figure 2.6 Structure diagram of navigation system
2.2.2控制系统方案设计
姿态角与水平位移之间存在耦合关系，因此多旋翼无人机六自由度的运动可简化为机体水平横向位移、水平纵向位移、高度方向位移、航向运动四个自由度运动。

对于一般的飞行任务可分解为空间三维位置和机头航向控制任务。

因此，整个控制系统可看作为四输入四输出的多输入多输出系统，如图2.7所示。

图中M1-M4为四个动力电机的PWM 控制指令。

多旋翼无人机控制系统设计以图2.7所示的多输入多输出系统为基础。

为简化研究，忽略各自由度运动的耦合，将上图的四输入四输出系统近似为四个相互独立运动的单输入单输出控制子系统，如图2.8所示。

其中，水平方向的位置控制采用三级串级控制结构，即内环姿态控制、中环位置控制、外环位置控制。

而偏航与高度运动模型具有较好的稳定性，可采用单一控制回路实现其运动控制。

陀螺仪加速度地磁
第二章实验平台与整体方案设计
图2.7 多输入多输出系统
Figure 2.7 MIMO System
图2.8 飞行控制系统结构框图
Figure 2.8 Structure diagram of flight control system 目前，多旋翼无人机应用最为广泛的仍为经典PID算法，但复杂应用环境下，其性能无法保证[27-31]。

此外，无人机作为一个飞行平台，其运动模型易受负载变化影响。

为追求控制系统较强的鲁棒性，采用模型参考滑模控制理论设计控制器。

2.3本章小结
本章首先介绍了课题研究所采用的飞行实验平台和嵌入式结构。

然后，针对课题主要研究内容，分别设计导航系统与控制系统的整体实施方案。

江苏大学工程硕士学位论文
第三章 导航系统设计
本章节研究航姿解算和组合导航算法，推算机体运动过程中的空间位姿信息，为飞行控制提供基础。

首先，介绍导航算法涉及的基础理论知识。

然后，设计航姿解算算法，并结合GNSS 信息，优化推算的姿态角；最后，采用EKF 理论设计组合导航算法，获取平滑精确的定位数据。

3.1 导航算法基础理论
3.1.1四元数理论
假设四元数为q ，由三个虚部1q 、2q 、3q 和一个实部0q 组成，即：
0123q q q i q j q k =+++ (3.1)
式中i 、j 、k 符合
2221i j k ijk ====-
(3.2)
假设，b
n λ为导航坐标系下的向量且通过坐标原点，若导航坐标系沿b
n λ旋转b
n ϕ角后与机体坐标系重合，则该转动过程可表示为：
cos sin sin sin 2222b b b b
b
n n n n n
q l m n ϕϕϕϕ⎡⎤⎛⎫⎛⎫⎛⎫⎛⎫=⎢⎥ ⎪ ⎪ ⎪ ⎪⎝⎭⎝⎭⎝⎭⎝⎭⎣⎦
(3.3)
为描述机体的运动过程，引入四元数的微分方程：
1()(t)2
b b b
n n n q q ω=
⊗ (3.4)
式中，b
n ω是导航坐标系下旋转角速度，而机载惯性器件得到的角速度n
b ω是相对于机体坐标系，将等式展开得：
0012311
322230133
2
1
0012x b y b z b q q q q q q q
q q q q q q q q q q q q q ωωω---⎡⎤⎡⎤⎡⎤
⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥-⎣⎦⎣⎦⎣⎦
(3.5)
其中，x
b ω，y
b ω，z
b ω为三轴角速度。

另外，结合四元数乘法，上式（3.5）可表示为：
2222
0123120313022222
12030123230122221302230101232()
2()2()
2()2()
2()
b n b n b n X q q q q q q q q q q q q X Y q q q q q q q q q q q q Y Z q q q q q q q q q q q q Z ⎡⎤+--+-⎡⎤⎡⎤
⎢⎥⎢⎥⎢⎥=--+-+⎢
⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢
⎥+---+⎣⎦⎣⎦⎣⎦
(3.6)
第三章 导航系统设计
结合方向余弦变换矩阵，则可得到姿态角关于四元数的表达式：
()()()()()()()
(
)
22012312013122031223arctan 2,12arcsin 2arctan 2,12q q q q q q q q q q q q q q q q φθψ⎡⎤+-+⎡⎤⎢⎥⎢⎥⎢⎥=-⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦+-+⎣
⎦
(3.7)
3.1.2卡尔曼滤波理论
卡尔曼滤波理论一般用于数据滤波和状态变量的估计，在航空导航方面具有不可替代的地位[32-33]。

首先，定义模型差分表达式： ()111,,k k k k x f x u w ---=
(3.8)
(),k k k z h x v =
(3.9)
式（3.8）为状态方程，式（3.9）为观测方程。

其中，n x ∈ℜ为状态变量，1k w -为模型噪声，k v 为观测噪声。

针对上述模型，卡尔曼滤波理论的核心公式如表3.1所示，包括时间更新和状态更新。

时间更新公式依据1k -时刻数据更新k 时刻的状态变量和协方差，观测更新公式依据当前测量值k z 不断修正状态变量和方差的估计值。

k A 、k H 、k W 、k V 为k 时刻相应的雅可比矩阵。

k R 为k 时刻观测噪声方差，1k Q -则为1k -时刻模型噪声方差。

表3.1 卡尔曼滤波核心公式
Table 3.1 The key formula of Kalman filter
EKF 时间更新公式
EKF 观测更新公式
,111ˆˆ(,,0)k k k k x
f x u ---= 1,1,1()T T
k k k k k k k k k k k K P H H P H V R V ---=+
,111+T
T
k k k k k k k k P A P A W Q W ---=
,1,1ˆˆˆ(())k k k k k k k x
x K z h x --=+-
,1()k k k k P I K H P -=-
3.2 传感器数据处理
算法运行前，需要对传感器数据进行一定的处理，包括零点校准，AD 转换系数校准，低通滤波等。

江苏大学工程硕士学位论文
3.2.1数据校准
惯性测量单元生产过程中存在个体差异，导致其AD 转换系数与理论值存在一定的偏差。

因此，需要对陀螺仪、加速度计AD 转换系数校准，磁罗盘进行磁干扰校准[34]。

陀螺仪校准：陀螺仪输出角速度数据，其积分即为角度。

校准时，将IMU 置于水平转台设备旋转一周，陀螺仪输出的角速度数据为w ，假设陀螺仪系数为k ，则有：
2*()k w t dt π=⎰
(3.10)
即可得到对应轴的陀螺仪系数：
2*()k w t dt
π
=
⎰ (3.11)
加速度校准：加速度校准采用六面校准法，其原理为IMU 静止不动时，其总加速度向量即为重力加速度。

假设三轴加速度的零点值分别为0x a 、0y a 、0z a ，
系数分别为ax k 、ay k 、az k 。

当IMU 与任意位置静止时，有：
22222
22000()()()ax x x ay y y az z z k a a k a a k a a G -+-+-=
(3.12)
将IMU 模块的六面分别置于水平面，取得六个平面下三轴加速度值，并建立如式（3.12）的六面方程表达式，即可求取加速度的零点与系数的最优解。

校准后，其测量误差小于0.1 m /s 2。

地磁校准：地磁校准主要校准机体坐标系下的定磁场干扰，一般采用三维椭球拟合校准方法。

在空旷环境下，将IMU 在三维空间内任意转动，记录不同位置下三轴磁场变化曲线。

然后根据地球磁场恒定，其三轴磁场分布应近似为一个标准圆球体，即可求得地磁校准的零点与系数参数。

3.2.2数据滤波
机载传感器测量数据中包含高频的噪音，一般需设计相应的滤波器进行处理 [35-36]，本研究采用的滤波算法包括如下两种。

限幅滤波：设定相邻两周期采样值之间的最大变化量为Max ，k 时刻的测量值为k y ，
k 时刻滤波后的值为ˆk y
，则有：
第三章 导航系统设计
111,ˆ,
k k k k k k k y y y Max y
y y y Max
---⎧-≤⎪=⎨->⎪⎩ (3.13)
IIR 滤波：IIR 滤波器为一种带有反馈回路的递归滤波器，可参考现有的模拟滤波器，易于实现。

这里选用Butterworth 滤波器，选择合适的阶数，保障其低于截止频率的频段幅值保持不变，而高于截止频率时幅频裕度下降迅速。

其表达式如下：
01
()1M
i i i N j
j j b z H z a z
-=-==
+∑∑
(3.14)
则对于N 阶的滤波器，输入为x ，其输出y 可表示为：
01()()()M
N
i j i j y n b x n i a y n j ===-+-∑∑
(3.15)
式中，j a 、i b 为滤波器参数。

3.3 航姿参考子系统设计
航姿参考子系统设计依赖于加速度计和陀螺仪惯性传感器。

由于飞行过程中，机体震动较大，且传感器数据存在零点漂移问题。

因此，无法依靠单一的加速度计或陀螺仪积分计算姿态角。

此外，引入磁罗盘测量地球磁场，并结合航向角速度数据，推算载体绝对航向角度 [37-38]。

迄今为止，应用最为广泛的扩展卡尔曼算法虽然效果较为理想，但算法运算量较大，在一定程度上受处理器硬件的限制。

为此，将EKF 算法权重设为恒定不变，从而降低处理器中算法运算的负担。

机体当前时刻角度值可由前一时刻的角度加上该时间周期内角速度的积分，即
(1)(1)()*K K Angle k Angle T ω--=+
(3.16)
T 为采样时间。

考虑对陀螺仪的静态漂移进行补偿，建立如下方程：
1_01_0
measure Angle dt Angle dt
Q bias Q bias ω-=+
(3.17)
式3.17是先验估计，推导可得状态转移方程：。