陈勇-无人直升机最优轨迹规划设计与实现.doc

硕士学位论文无人直升机最优轨迹规划设计与实现作者姓名陈勇学科专业控制理论与控制工程指导教师方昌始 高级工程师所在学院自动化科学与工程学院论文提交日期2010年6月UAV Optimal Trajectory Planning Design And ImplementationA Dissertation Submitted for the Degree of MasterCandidate：Chen YongSupervisor：Prof. Fang changshiSouth China University of Technology Guangzhou, China分类号：TP273 学校代号：10561学 号：200720110434 华南理工大学硕士学位论文无人直升机最优轨迹规划设计与实现作者姓名： 陈勇 指导教师姓名、职称： 方昌始 高工申请学位级别： 硕士 学科专业名称： 控制理论与控制工程研究方向： 论文提交日期： 2010年 6 月 4 日 论文答辩日期： 2010年 6 月 9 日学位授予单位： 华南理工大学 学位授予日期： 年 月 日答辩委员会成员：主席： 委员： 华南理工大学学位论文原创性声明本人郑重声明：所呈交的论文是本人在导师的指导下独立进行研究所取得的研究成果。除了文中特别加以标注引用的内容外，本论文不包含任何其他个人或集体已经发表或撰写的成果作品。对本文的研究做出重要贡献的个人和集体，均已在文中以明确方式标明。本人完全意识到本声明的法律后果由本人承担。作者签名： 日期： 年 月 日学位论文版权使用授权书本学位论文作者完全了解学校有关保留、使用学位论文的规定，即：研究生在校攻读学位期间论文工作的知识产权单位属华南理工大学。学校有权保存并向国家有关部门或机构送交论文的复印件和电子版，允许学位论文被查阅（除在保密期内的保密论文外）；学校可以公布学位论文的全部或部分内容，可以允许采用影印、缩印或其它复制手段保存、汇编学位论文。本人电子文档的内容和纸质论文的内容相一致。本学位论文属于：保密，在 年解密后适用本授权书。不保密,同意在校园网上发布，供校内师生和与学校有共享协议的单位浏览；同意将本人学位论文提交中国学术期刊(光盘版)电子杂志社全文出版和编入CNKI中国知识资源总库，传播学位论文的全部或部分内容。 (请在以上相应方框内打“”)作者签名： 日期：指导教师签名： 日期：作者联系电话： 电子邮箱：联系地址(含邮编) ：摘 要当今，由于在民用及国防等诸多领域中的广泛应用，空中机器人技术已经越来越被人们所重视，并吸引了各国专家学者的注意。而基于模型直升机开发出来的小型旋翼空中机器人更是在近几年中得到迅速发展，并依靠其较高的灵活机动性，在各个领域展现出其独特优势。本文是在实验室已有无人直升机研究基础上，进行延续性设计和开拓性的应用。以雷虎90型模型机为平台，实验室设计了一套基于ARM7为控制核心的硬件平台，该平台的传感器数据采集和INS/GPS数据滤波功能已经实现。本文在此基础上，移植了一套经过实验验证的双闭环PID控制算法。然后调试与试飞整个系统，并完善控制器和调整控制参数，以达到理想的跟踪效果。在完成控制器调试后，依次实现了悬停、转向、定点飞行、圆弧飞行等一系列独立的飞行过程和对应的参数调试。在此基础上，按照距离最优和时间最优的原则，以扩展3D-Dubins模型，设计并实现了多个目标点巡航的轨迹规划和飞行调度。最优轨迹规划和飞行调度是本文研究的核心，最优轨迹规划是以3D-Dubins模型为飞行模型，计算空间任意两目标点间的最短飞行路径，然后在此路径上，进行约束条件下的速度调度，使无人直升机能在最短时间完成该路径的飞行。飞行调度是管理地面目标指令，调度相邻等待目标点，完成最优规划功能，使整个飞行过程按照预定的方式进行。关键词：无人直升机； 3D-Dubins模型；最优轨迹规划；飞行调度AbstractToday, as in many other areas of civil and national defense of the widely used, air robotics has been attracted the attention of national experts.The developed model-based small-scale helicopter is in rapid development in recent years and rely on its high flexible and mobile, in various fields has demonstrated its unique advantages.This article is the laboratory study has been based on the unmanned helicopter, for continuity of design and pioneering applications.Based on Tiger 90 model helicopter, my laboratory has designed parts of navigation system based on ARM7 core hardware platform ,which sensor data acquisition and INS / GPS data filtering has been achieved.On this basis, the transplant was experimental verification of a dual-loop PID control algorithm.Then debug and test the entire system, and improve the control and adjustment of control parameters to achieve the desired tracking performance.After the completion of commissioning the controller, to achieve a hover turnning, point to point flight, circular flight and the corresponding flight parameters debugging.On this basis, the optimal distance and time in accordance with the principles of optimal to extend the 3D-Dubins model, designed and implemented a number of target points during cruise flight path planning and scheduling.Optimal trajectory planning and flight scheduling is the core of this paper. Optimal trajectory planning is based on 3D-Dubins flight model, Firstly ,calculate the shortest flight path between any two target points by Dubins Path , then on this path, for constraints speed planning, get the optimal trajectory in the shortest flight time to complete the shortest path. Flight scheduling is the management of ground flying instruction, scheduling target point adjacent to wait to complete the optimal planning functions to the flight in accordance with the intended manner.Keywords: Unmanned Aerial Vehicle; 3D-Dubins Model; Optimal Trajectory Planning; Flight Schedule;目 录摘 要IAbstractII第一章 绪论11.1 研究课题背景和意义11.2 无人直升机研究及其发展11.3 研究现状和本文结构41.4 名词解释51.5 课题来源6第二章 飞行控制系统硬件平台72.1 UAV模型平台72.2 飞行控制硬件平台总述82.2.1飞行控制计算机与协处理器82.2.2 传感器系统92.2.3 伺服系统102.2.4 地面监控系统102.3 本章小结10第三章 导航基础123.1 位置与速度描述123.1.1 地球中心坐标系123.1.2 当地水平坐标系123.1.3 Body-Frame133.1.4 WGS-84(World Geodetic System 1984)大地坐标133.1.5坐标系转换与位置/速度获取143.2 姿态描述163.2.1 UAV姿态定义163.2.2 欧拉角表示UAV姿态163.3 GPS/INS组合导航173.3.1 GPS/INS导航分析173.3.2 GPS/INS导航实现183.4 本章小结20第四章 控制系统实现214.1控制结构图214.1.1 结构图模块概述214.1.2 过程变量描述224.2 控制器实现244.2.1 导航控制器实现244.2.2 姿态控制器实现254.2.3 控制参数调试264.3 参考轨迹274.3.1 轨迹定义274.3.2 轨迹集合284.3.3 轨迹在线计算和数学描述294.5 本章小结34第五章 最优轨迹规划355.1 飞行任务描述和分析355.1.1 飞行任务355.1.2 目标指令355.1.3 任务执行过程375.2 最优问题数学模型和描述395.2.1 UAV最优问题数学描述395.2.2 Dubins模型395.2.3 Dubins问题的解405.3 距离最优规划425.3.1 最优Dubins曲线425.3.2 求解最优Dubins曲线435.3.3 Dubins模型3D扩展455.3.4 距离最优轨迹455.4 时间最优二次规划465.4.1 时间最优问题的解465.4.2 3D空间速度调度算法495.5 飞行调度策略525.5.1 点到点飞行525.5.2 多点飞行545.6 本章小结56第六章 轨迹规划实现和试飞576.1 软件实现576.1.1 状态描述类576.1.2 目标队列类586.1.3 飞行规划类596.2 试飞和数据分析616.2.1 试飞说明616.2.2 飞行数据626.3 本章小结72结论与展望73参考文献74致 谢78V。第二章 飞行控制系统硬件平台第一章 绪论第一章 绪论1.1 研究课题背景和意义无人直升机是一个极具挑战性的多学科交叉的前沿性研究课题。其中涉及新材料技术、惯导技术，全球定位技术，计算机视觉技术、通信和网络、自动控制、系统辨识、最优估计、多传感器数据融合、人工智能等多个领域理论和技术。由于直升机较固定翼飞机而言，具有悬停等灵活、机动的飞行性能，起落受限制少、能适应复杂多变的环境等优势, 无人直升机在空中机器人家族中占有举足轻重的地位，大多数研究机构选用直升机作为微小型无人机的研究平台。在实际应用中，由于高度的灵活性和很强的适应性，无人直升机在军事、民用和科学研究上都有广泛的应用前景。在军用方面，无人直升机主要用于低空侦察、电子干扰等任务。这样不仅可以减少部队在侦察过程中的伤亡，还可大大提高作战效率。而在民用领域中可用于通信中继、环境研究、自然灾害的监视与支援。在科学研究上，可用于大气研究，对核生化污染区的取样与监控、新技术新设备与新飞行器的试验验证等。国外对无人直升机很早就开始研究，但由于技术保密缘故，国内无从获取其技术细节，而且国内起步相对较晚，技术相对落后。因此在国内研究无人机导航系统是非常有益和必要的。1.2 无人直升机研究及其发展开展这一方面研制的国家主要是美国、俄罗斯、日本、欧洲国家、以色列和中国，其中比较典型的有以下几种：1. 典型的GPS/INS导航系统。MIT大学研究的Xcell-60小型自动飞行器如图1-1所示，旋翼直径1524mm，总重8.1kg，其中电子设备重3.2kg。Xcell-60上的设备主要有：带有266MHz主频CPU的PC104计算机，运行QNX实时操作系统；传感器包括1GPS接收器（频率是1hz，精度是2米），1六自由度的惯性测量单元，Honeywell HMC2003三轴磁罗盘，Honeywell HPB200A高度计，系统同时使用了Kalman滤波器提高导航的精度，并改善系统的可靠性以及容错能力。图1.1 MIT的Xcell-60直升飞机2多GPS联合导航系统。斯坦福大学研制的能够自主悬停的hummingbird直升飞机如图1-2所示，它机身长1580mm。Hummingbird导航系统采用的是差分载波相位全球定位系统(DCPGPS-differential carrier phase GPS)。hummingbird直升飞机最具特色的技术是通过机体上放置的4个GPS接收天线，该系统就能够检测直升飞机的姿态、航向信息，从而省去了高度计、惯性导航仪等繁杂的设备。图1-2 Stanford hummingbird 直升飞机3 中国的第60研究所、南京航空航天大学、北京航空航天大学都开发了各自的小型/超小型无人驾驶直升飞机系统，在2009年的珠海航空航天展上展示了两款北京航空航天大学完全自主设计的无人直升机，如图1.4是一款型为FH-2的无人直升机。图1.4 北航超小型共轴式无人直升飞机表1-1是一份关于国外主要大学无人直升机研究状况的分析和总结1。表1-1 各研究单位滤波算法及控制算法分析比较UniversitySensors and processing techniques used on the HelicopterMassachusetts Instituteof TechnologyX-Cell 60 Helicopter ISIS-IMU with 100Hz update rate and 0.02 degrees/minute drift Honeywell HPB200A Altimeter with 2ft accuracySuperstar GPS receiver from Marconi with 1 Hz update rate13-state extended Kalman filter for state estimation LQR based controlCarnegie MellonUniversityYamaha R-Max Helicopter Litton LN-200 IMU with 400 Hz update rate and 4Kg尺寸机身长度1410mm机身高度476mm机身宽度190mm主旋翼直径1605mm尾旋翼直径260mm发动机型号OS/SZ91燃料甲醇/硝基甲烷气缸容量10cc制冷方式风冷齿轮比8.45:1:4.65飞行时间 15-25min图2-1 雷虎90直升机平台2.2 飞行控制硬件平台总述UAV硬件平台包括四个子功能系统：飞行控制系统、传感器系统、伺服控制系统和地面监测系统。1) 飞行控制系统的核心是AT91SAM7SE(512)，是飞行导航的主控计算机；2) 传感器系统包括惯性测量元件（IMU）、电子罗盘(Compass)、GPS(Global Position System)和声纳(Sonar)组成；3) 伺服控制系统由RC(Radio Controler)遥控器、接收机和伺服舵机组成。4) 地面监控系统包括无线路由、地面监控平台两部分。系统结构如图2-2所示4。 图2.2 系统硬件架构图2.2.1飞行控制计算机与协处理器导航飞行控制计算机采用AT91SAM7SE512（ARM7）微处理器5，它是在国内应用广泛的基于ARM7TDMI内核的芯片，是目前应用最为广泛的32位嵌入式RISC处理器，具有稳定、体积小、运算快、功耗低的优点。该处理器负责传感器数据融合、导航规划、控制运算以及与地面监控平台的交互，是整个飞行控制系统的核心。协处理器包括AVR Mega8L单片机和PC/104工控板。Mega8L单片机是Atmel公司生产的高性能低功耗CMOS单片机，可以实现近1MIPS/MHz的指令执行速度，负责遥控器信号(PCM1024 )解码和传输。PC104工控板是工业应用级别的小型计算机，CPU工作频率可达900MHz，同时具有丰富的外围设备和计算能力，是实验室上一代飞行控制系统的主控平台, 负责ARM7与地面监控系统数据流的缓冲与传输。2.2.2 传感器系统1、 IMU用来测量UAV在飞行过程中的三轴加速度和角速度，使用ADI公司的高精度惯性感测(MEMS)模块ADIS 16350，该型号IMU提供了六组自由度(six-degrees-of-freedom)的动作感测，并且内嵌校正与滤波算法，能直接提供精确的3轴角速度与加速度测量值，标准数字SPI接口减少了AD转换的噪声干扰，避免后期繁杂的滤波以及校正处理。2、 COMPASS( 电子罗盘 )电子罗盘采用PNI公司提供的磁通传感器SEN-S65和3轴磁通传感器驱动控制芯片PNI-11096组成。通过它提供的3个垂直安装的磁通传感器所感应磁通量的测量值，得到精确的航向角。该传感器电路结构简单，通过全数字SPI口很方便的与ARM7连接。3、 GPSGPS用来定位UAV在空间的坐标和运动速度。GPS接收板选用的是NovAtel的OEMV-1，它体积紧凑、功耗低，可提供0.03m/s的测速精度，数据更新率达20Hz ，单点定位精度为1.8 m，如采用NovAtel成熟的RT20技术，实时定位精度可提高至20cm。4、 Sonar声纳模块主要应用在无人机离地面7米以内的高度测距，用于弥补GPS在低空高度测量上的精度缺陷，本声纳系统选用了Senscomp公司生产的Polaroid 6500 系列超声波测距模块。2.2.3 伺服系统伺服系统硬件采用了Futaba公司通用Radio Control伺服系统：T9CHP PCM1024遥控器、R149DP接收机、 四个S9206高扭力舵机（分别控制直升机的Roll 、 Pitch 、 YAW 和 COLL）、GY401锁尾陀螺仪和S9255数位式锁尾舵机。S9206舵机是飞行控制系统的控制输出执行器，响应为0.19秒/60度，PWM工作周期是30ms，它决定了飞行控制器的工作频率约为33Hz。2.2.4 地面监控系统地面监测系统是整个无人直升飞机系统的重要组成部分。监控平台与机载程序之间具有双工通讯能力：一方面，机载程序需要把各种飞行数据发送给地面的监控平台，另一方面，监控平台需要能够随时接受用户的命令输入，并及时将命令发送给UAV。监控平台硬件是一台DELL的Laptop，在上面运行一套自行编写的GS(Ground Station)软件来完成上述功能6。无线传输部分(WLAN)选用了思科公司的WRT54GC-CN路由器作为机载无线通讯设备，它采用802.11g作为通讯协议，最大传输速率达到54M, 监控平台采用内置符合802.11g协议无线以太网卡的Laptop。如要进行远程飞行，则由思科公司的WAP54G无线AP接入路由器。2.3 本章小结本章首先介绍了小型直升飞机的结构，以及基本参数，接着介绍飞控系统的硬件系统构架，详细描述了各个子系统的功能和硬件资料，表2-2和表2-3详细总结了飞控系统硬件信息，本章的知识是搭建自主飞行软件系统的基础。表 2-2 核心计算器性能参数名称 型号主要参数主处理器AT91SAM7SE51254MHz主频CPU，32位ARM RISC架构，512Kb高速flash协处理器(PCM解码)AVR Mega8L16MHz主频，8位AVR微处理器，8Kb flash协处理器(数据传输)OEM PC2000 PC/104PIII级别533MHz主频CPU，64MKb内存，支持串口、并口、USB和无线网络外围接口地面站DEll D620Intel Core T2300E CPU ， 512M内存，80G硬盘表 2-3 传感器性能参数表名称厂商与型号主要参数IMUADIS 16350滤波后角度精度可以到达1度以内，零点漂移导致的角度偏差低于2度每20分钟CompassSEN-S65 & PNI-11096航向角抖动控制在在1-2度左右，动态效果理想GPSNovAtel OEMV-1数据更新频率5Hz-20Hz，差分精度0.20m，速度精度0.3m/sSonarSenscomp Polaroid 6500 采样频率为7HZ，测量高度为7m，误差可以控制在4cm以内第三章 导航基础第三章 导航基础UAV自主飞行涉及到一些专业的控制和导航知识，例如导航坐标系、姿态角、导航控制、姿态控制、INS/GPS导航等等78，为方便理解，本章会做一些基本的介绍。导航要解决的基本问题是：现在在哪里？要去哪里？如何去？本章首先回答第一个问题，也就是位置、速度和姿态角的描述，然后介绍INS/GPS导航算法。其它两个问题将在四、五两章回答。3.1 位置与速度描述对UAV进行导航的首要问题就是要实时的确定其飞行状态，具体是指UAV当前位置、速度和姿态角。飞行状态量需要在某个具体的坐标系中描述，这些坐标系包括地球中心坐标系(Earth Centered Earth Fixed frame)、当地水平坐标系(Earth Surface North-East-Down Frame)和Body-Frame(Body frame )，本节首先介绍这几个坐标系，然后介绍如何依靠GPS信号获得飞机的速度与位置量。3.1.1 地球中心坐标系地球中心坐标系(简称ECEF)是与地球固联并随地球的转动而转动的坐标系，如图3-1所示。地球中心坐标系用在GPS系统中就称为WGS-84坐标系。原点为地球的质心，X轴为通过Greenwich线和赤道线，轴与X-Z轴构成右手法则，Z轴为通过地心指向北极。3.1.2 当地水平坐标系 当地水平坐标系（简称NED），是在导航时根据导航系统工作的需要而选取的作为导航基准的坐标系，是用来做导航计算时使用的，同样是通过满足右手正交法则的三个向量轴来表示。向量指向的是北方（North），向量指向的是东方（East），向量是沿着当地重力加速度向量指向地（Down）。，向量与地球表面相切，故称之为当地水平坐标系。如图3-1所示。图3-1 地球中心坐标系(ECEF)与当地水平坐标系(NED)图3-1中向量是地球中心坐标系的三个轴，向量是NED坐标系的三个轴，表示经度，表示纬度。3.1.3 Body-FrameBody-Frame(Body Frame)是固连在UAV机体上的坐标系。原点位于UAV的重心处，沿机体纵轴指向前，沿机体横轴指向右，垂直于并相对于、成右手法则，沿UAV的竖轴指向下。如图3-2所示。图3-2 Body-Frame示意图3.1.4 WGS-84(World Geodetic System 1984)大地坐标GPS单点定位的坐标以及相对定位中解算的基线向量属于WGS-84大地坐标系, 因为GPS卫星星历是以WGS-84坐标为根据而建立的9。WGS-84椭球及有关常数采用国际大地测量和地球物理联合会第17届大会大地测量常数的推荐值, 四个基本常数为：l 长半轴: l 地心引力常数(含大气层): l 正常化二阶带谐系数: l 地球自转角速率: 通过GPS的输出信息, 可以得到:l 经度(Longitude) : 在Greenwich线经度为零, 即l 纬度(Latitude) : 在地球赤道, ; 在北极, ; 在南极,l 高度 : GPS输出的高度信息是相对于海平面的高度3.1.5坐标系转换与位置/速度获取第一步 ： WGS84转换到ECEFGPS接收机获得的信息实际是一串固定格式的数据报，通过解析这串数据报，得到WGS-84大地坐标系下的地理信息，包括：经度，纬度，海拔，航向Course和航速Speed。 首先将GPS输出的经度、纬度及高度信息转换到地球中心坐标系(ECEF)上，转换公式如下： (3-1) (3-2) (3-3)这里采用的单位是米, 式中表示经度,表示纬度（单位：弧度）,是相对与椭球体表面的高度(单位:米), 表示曲率半径(单位:米), 其定义为 (3-4) 第二步： ECEF转换到NED 在取NED坐标系前，需要知道NED坐标系原点的经度和纬度，从ECEF转换到NED坐标系的方向余弦表示为 (3-5) 是NED原点的ECEF坐标，由ECEF坐标系中的位置转换到NED坐标系位置表达式为 （3-6) 由ECEF坐标系中的速度转换到NED坐标系中的速度表达式为 (3-7)第三步： NED转换到Body-frame NED坐标系是导航坐标系，规划飞行目标和飞行轨迹，是在NED坐标系下进行的。而Body-frame坐标系是控制坐标系，控制器运算和执行结果都要在Body-Frame中反映。由NED向Body-frame转换，需要知道UAV的即时姿态，下一节会详细描述UAV姿态。这里先定义NED到Body-frame坐标系的转动矩阵，下标表示由NED转到Body-Frame，那么转换公式如下： （3-8）式中的在公式(3-13) 中描述，向量是Body-frame坐标原点在NED坐标系中的位置，也即是UAV的NED位置坐标。 由NED中的速度转换到Body-frame的速度表达式为 (3-9)3.2 姿态描述3.2.1 UAV姿态定义描述UAV的姿态是用三个不同的转动角，分别称之为横滚（Roll）、俯仰（Pitch）和偏航（Yaw） ， 统称为欧拉角8。在UAV上，通过捷联(Strap-down)的安装IMU可以测得UAV的加速度，在机体系上的三个分量为；同时也可以获得转动的角速率，在机体系上的三个分量为。 计算时，需要将这些向量转换到NED, 转动的时候要用到三个欧拉角，即。这三个角称之为横滚角、俯仰角和偏航角，简称为：Roll、Pitch、Yaw。图3-3表示了UAV的姿态角和转动方向。图3-3 UAV的姿态角3.2.2 欧拉角表示UAV姿态为了采用欧拉角表示机体在空间中的转动，在机体中这三个角的转动顺序是很重要的。定义转动时采用的顺序是：初始时UAV处于与地面平行的状态，Body-Frame的X轴与NED的N轴重合，Body-Frame的Y轴与NED的E轴重合。首先旋转是角，其次是角，最后是角。转动时，需要三个不同的转动矩阵。这三个矩阵是正交的，也就是说满足。下面的式(3-10)表示轴的转动，式(3-11)表示绕轴的转动，式(3-12)表示绕轴的转动。那么UAV从当地水平状态转到期望的姿态角时的转动将是首先绕轴转动，再绕轴转动，最后绕轴转动。最后获得的转动矩阵，下标表示由NED转到Body-Frame，如式(3-13)所示。 (3-10) (3-11) (3-12) (3-13)3.3 GPS/INS组合导航3.3.1 GPS/INS导航分析组合导航系统利用两个导航系统的互补特性，以便产生一个比任何一个独立工作的分系统更高精度的系统。惯性导航系统(INS)噪声相对较低，但会随时间漂移；相对而言，卫星导航系统(GPS)的位置估值噪声相对较大，但不会长时间漂移。表3-1对惯性系统和卫星系统各自的优缺点进行了总结。表3-1 INS导航系统与GPS系统优缺点的比较优缺点优点缺点惯性导航系统 数据率高 既可提供平移数据，也可提供转动数据 自主不易受干扰 误差增长不受限制 需要知道重力卫星导航系统 误差增长受到限制 数据率低 无姿态信息 既易受人为干扰，也易受非人为干扰下面介绍一些常用的INS/GPS组合方案，组合结构可以分为4个主要类型101112。(1) 非耦合系统。在系统中，GPS的估计位置只简单地用于每隔一定的时间对INS指示的位置进行重新设置。(2) 松耦合系统。在该系统中，INS和GPS的位置估值和速度估值进行比较，得到差值形成Kalman滤波器的测量输入值。(3) 紧耦合系统。在该系统中，GPS的伪距测量值和伪距率测量值与惯性生成的这些量的估值进行比较。(4) 深耦合系统。该系统将GPS信号跟踪功能与INS/GPS组合结合在一起，形成一个单一算法。 本文是基于松耦合的GPS/INS导航模型，其结构如下图所示： 图3-4 INS/GPS组合导航结构3.3.2 GPS/INS导航实现前面已经提到，导航坐标系是NED，需要用到的导航信息为9维，即UAV姿态（建模时使用四元数8来描述姿态），三维位置（分别为北向位置，东向位置和高度），三维速度。在建立直升机运动学模型时，状态量必须包含9维的导航信息。另外，我们还要考虑对三维的角速度计和三维的加速度计的误差进行建模，从而获得一组滤波后的三维的角速度和三维的加速度。综上所述，我们应该选取过程状态变量 (3-14)系统观测量 (3-15)在本文的设计中，根据空中机器人的非线性系统特性，建立了空中机器人的非线性系统模型。该非线性系统模型是根据飞行器的运动学特性得到的能够反映飞行器运动学特点的系统模型。其非线性模型表示如下： (3-16) (3-17)其中式(3-16)为系统的状态方程，式(3-17)为系统的观测方程。为系统的16维状态向量，为系统的10维观测向量，和分别为已知的非线性16维和10维向量函数，为4维控制向量，方程推导过程和结果参见参考文献2, 5-29。根据GPS/INS硬件特点，采用自行设计的extern kalman filter（EKF）， 融合处理加速度、角速度（IMU提供）和磁传感器(Compass)的数据，利用加速度和磁传感器稳态性能好，无零漂的特点来抑制角速度计的零点漂移和高频扰动，同时利用角速度的动态响应灵敏的优点，获的理想的动态效果。本系统数据滤波处理见和实现过程见参考文献13, 49-58 ， 图3.5给出该部分的实现流程。 图3-5 松耦合INS/GPS Kalman滤波计算流程图3.4 本章小结这一章是导航的基础理论知识介绍。首先定义导航中使用到的坐标系和姿态表示。在此