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

硕士学位论文
无人直升机最优轨迹规划设计与实现
作者姓名陈勇
学科专业控制理论与控制工程
指导教师方昌始高级工程师
所在学院自动化科学与工程学院
论文提交日期2010年6月
UA V Optimal Trajectory Planning Design And Implementation
A Dissertation Submitted for the Degree of Master
Candidate：Chen Y ong
Supervisor：Prof. Fang changshi
South 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模型；最优轨迹规划；飞行调度
Abstract
Today, 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 V ehicle; 3D-Dubins Model; Optimal Trajectory Planning; Flight Schedule;
目录
摘要 (I)
Abstract (II)
第一章绪论 (1)
1.1 研究课题背景和意义 (1)
1.2 无人直升机研究及其发展 (1)
1.3 研究现状和本文结构 (4)
1.4 名词解释 (5)
1.5 课题来源 (6)
第二章飞行控制系统硬件平台 (7)
2.1 UA V模型平台 (7)
2.2 飞行控制硬件平台总述 (8)
2.2.1飞行控制计算机与协处理器 (8)
2.2.2 传感器系统 (9)
2.2.3 伺服系统 (10)
2.2.4 地面监控系统 (10)
2.3 本章小结 (10)
第三章导航基础 (12)
3.1 位置与速度描述 (12)
3.1.1 地球中心坐标系 (12)
3.1.2 当地水平坐标系 (12)
3.1.3 Body-Frame (13)
3.1.4 WGS-84(World Geodetic System 1984)大地坐标 (13)
3.1.5坐标系转换与位置/速度获取 (14)
3.2 姿态描述 (16)
3.2.1 UA V姿态定义 (16)
3.2.2 欧拉角表示UA V姿态 (16)
3.3 GPS/INS组合导航 (17)
3.3.1 GPS/INS导航分析 (17)
3.3.2 GPS/INS导航实现 (18)
3.4 本章小结 (20)
第四章控制系统实现 (21)
4.1控制结构图 (21)
4.1.1 结构图模块概述 (21)
4.1.2 过程变量描述 (22)
4.2 控制器实现 (24)
4.2.1 导航控制器实现 (24)
4.2.2 姿态控制器实现 (25)
4.2.3 控制参数调试 (26)
4.3 参考轨迹 (27)
4.3.1 轨迹定义 (27)
4.3.2 轨迹集合 (28)
4.3.3 轨迹在线计算和数学描述 (29)
4.5 本章小结 (34)
第五章最优轨迹规划 (35)
5.1 飞行任务描述和分析 (35)
5.1.1 飞行任务 (35)
5.1.2 目标指令 (35)
5.1.3 任务执行过程 (37)
5.2 最优问题数学模型和描述 (39)
5.2.1 UA V最优问题数学描述 (39)
5.2.2 Dubins模型 (39)
5.2.3 Dubins问题的解 (40)
5.3 距离最优规划 (42)
5.3.1 最优Dubins曲线 (42)
5.3.2 求解最优Dubins曲线 (43)
5.3.3 Dubins模型3D扩展 (45)
5.3.4 距离最优轨迹 (45)
5.4 时间最优二次规划 (46)
5.4.1 时间最优问题的解 (46)
5.4.2 3D空间速度调度算法 (49)
5.5 飞行调度策略 (52)
5.5.1 点到点飞行 (52)
5.5.2 多点飞行 (54)
5.6 本章小结 (56)
第六章轨迹规划实现和试飞 (57)
6.1 软件实现 (57)
6.1.1 状态描述类 (57)
6.1.2 目标队列类 (58)
6.1.3 飞行规划类 (59)
6.2 试飞和数据分析 (61)
6.2.1 试飞说明 (61)
6.2.2 飞行数据 (62)
6.3 本章小结 (72)
结论与展望 (73)
参考文献 (74)
致谢 (78)
第一章绪论
第一章绪论
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 各研究单位滤波算法及控制算法分析比较University Sensors and processing techniques used on the Helicopter
Massachusetts Institute
of Technology X-Cell 60 Helicopter
ISIS-IMU with 100Hz update rate and 0.02 degrees/minute drift Honeywell HPB200A Altimeter with 2ft accuracy Superstar GPS receiver from Marconi with 1 Hz update rate
13-state extended Kalman filter for state estimation
LQR based control
Carnegie Mellon University Yamaha R-Max Helicopter
Litton LN-200 IMU with 400 Hz update rate and <=1ms latency Novatel RT-2 differential GPS with 2cm accuracy
KVH-100 flux-gate digital compass with 5 Hz update rate
12-state extended Kalman filter for state estimation
Control based on H inf control
Stanford University X-Cell 60 Helicopter
Trimble TANS Quadrex GPS with 3cm position accuracy
Used GPS with multiple antenna for attitude estimation No other sensors used State estimated using custom algorithms from carrier phase GPS
PD based control
Georgia Institute of Technology Yamaha R-50 Helicopter
ISIS-IMU with 100Hz update rate and 0.02 degrees/minute drift Novatel RT-2 differential GPS with 2cm accuracy
HMR-200 triaxial magnetometers
17-state extended Kalman filter for state estimation Feedback linearization with neural networks for control
University of Yamaha R-Max Helicopter
华南理工大学硕士学位论文
California Berkeley Boeing DQI-NP INS/GPS integration system
Novatel RT-2 differential GPS with 2cm accuracy
DQI-NP unit provides attitude, velocity and position information No state estimation necessary since the sensors provide state Hinf based control
University of Southern California Bergen twin Industrial Helicopter
CrossBow VGX IMU with 133Hz update rate and 2 degrees accuracy Novatel RT-2 differential GPS with 2cm accuracy
TCM-250 triaxial magnetometer
16-state Kalman filter for state estimation
PID based control
CSIRO X-Cell 60 Helicopter
Custom made IMU embedded with magnetometers with 76 Hz update rate
Ublox GPS unit with differential corrections using WAAS, 2m accuracy
Stereo vision for height estimation ，Velocity estimation using vision
Complimentary filters and extended Kalman filter used in conjunction
PID based control
1.3 研究现状和本文结构
本文研究课题是在实验室已有研究成果的基础上进行的。

实验室已经搭建了一套基于
PC104硬件平台、以autoploit为原型的无人直升机系统，这套系统能够完成基本的INS无人导航飞行[2]。

但是由于PC104本身功耗大，autoploit功能也比较简单，为提升无人直升机在实际飞行任务中的应用，实验室以ARM7为核心计算机，选择更稳定的传感器硬件，以INS/GPS 导航模型，重新搭建了一套无人直升机导航的硬件平台[3]，本文的研究就是在这样的基础上进行的。

本文研究分两个步骤完成，首先是在ARM7无人直升机硬件平台上，完成控制器软件实现和调试，然后以实际巡航为目的，设计和实现无人直升机的轨迹规划，最终能完成实际巡航飞
第一章绪论
行。

第一章是绪论，介绍了无人直升机的发展背景，详细介绍无人直升机的研究现状和本文研究基础。

第二章概述本文无人直升机飞行系统的硬件基础和主要的性能参数。

第三章介绍无人直升机自主导航的背景知识，给出本系统的GPS/INS导航模型和实现方案简介。

第四章描述控制系统整体结构，详细介绍了控制器实现和参数调试，并给出了基础飞行轨迹描述和在线数据计算。

此章为本文研究的第一步，为最优轨迹规划打下基础。

第五章是最优轨迹规划的理论推导和实现方案，是本文的核心部分。

首先介绍基于Dubins 模型的无人直升机最优路径理论和求解过程，并扩展到三维Dubins模型上，进行时间最优的二次规划，以达到点对点间最优轨迹规划。

然后结合实际飞行过程，应用飞行调度策略，完成多个目标点的最优巡航过程。

第六章是在第四、五两章的基础上，介绍最优轨迹规划的软件实现，并给出了实际飞行数据和分析。

结论部分总结本文设计并对未来的研究工作进行了展望。

1.4 名词解释
无人直升机是一个前沿性的系统工程，有很多专有名词和用语，为了方便阅读，本节对文中出现的重要词汇进行说明。

对一些专用词汇，为维护与其它资料的统一性和延续性，文中将直接英文描述。

UA V 全称是Unmanned Aerial V ehicle，译为无人驾驶飞机，本文专
指无人直升机，下文均采用此简称。

Navigation System 导航系统，是UA V导航和路径规划的统称。

本文实现的导航
系统包括UA V自主飞行、轨迹规划和飞行调度等几个部分统称。

GPS Global Position System ，全球定位系统，为UA V提供飞行位
置和速度信息。

IMU Inertial measurement unit ，惯性测量模块，测量UA V机体坐
标系的惯性信息。

华南理工大学硕士学位论文
GPS/INS GPS与INS联合导航系统，基于GPS信息和IMU信息的松
耦合导航系统，是本文采用的UA V导航原理。

Trajectory UA V飞行和规划轨迹，指UA V的在飞行过程中的位置、速
度和姿态变化过程，用三轴位置坐标、三轴速度向量、三个欧
拉角共九个变量来描述。

Trajectory Planning 轨迹规划过程，指在具体飞行目标和约束条件下，生成UA V飞
行轨迹的过程。

Ground Station 地面监控软件平台，指在地面运行的、用来监控UA V飞行信
息、发送飞行指令的软件合集。

以下简称GS或地面站。

1.5 课题来源
本课题来源于华南理工大学自动化科学与工程学院控制与信息工程系裴海龙教授主持的“小型无人机自主导航与控制”项目，并得到以下项目基金资助：国家自然科学基金资助项目（60574004），国家自然科学基金资助重点项目(60736024)，教育部科技创新工程重大项目培育资金项目(708069)。

第二章飞行控制系统硬件平台
第二章飞行控制系统硬件平台
本章将介绍小型无人直升飞机的硬件系统，包括飞机模型平台和飞行控制系统的主要硬件平台。

作为嵌入式控制系统，首先要了解控制对象，其次对传感器、执行机构等都必须清晰的认识，本章硬件知识是本文整个设计的基础之一。

2.1 UAV模型平台
研究中采用台湾生产的雷虎90型直升机作为飞行平台，该型直升机与国外通用的X-Cell 60直升机是同一级别的，其主要技术参数如表2-1所示，外观见图2.1。

表2-1 雷虎90直升机技术参数
重量
模型直升机重量 4.8Kg
有效载重量（含燃料）>4Kg
尺寸
机身长度1410mm 机身高度476mm 机身宽度190mm 主旋翼直径1605mm 尾旋翼直径260mm
发动机
型号OS/SZ91 燃料甲醇/硝基甲烷气缸容量10cc
制冷方式风冷
齿轮比——8.45:1:4.65 飞行时间15-25min
华南理工大学硕士学位论文
图2-1 雷虎90直升机平台
2.2 飞行控制硬件平台总述
UA V硬件平台包括四个子功能系统：飞行控制系统、传感器系统、伺服控制系统和地面监测系统。

1) 飞行控制系统的核心是A T91SAM7SE(512)，是飞行导航的主控计算机；2) 传感器系统包括惯性测量元件（IMU）、电子罗盘(Compass)、GPS(Global Position System)和声纳(Sonar)组成；3) 伺服控制系统由RC(Radio Controler)遥控器、接收机和伺服舵机组成。

4) 地面监控系统包括无线路由、地面监控平台两部分。

系统结构如图2-2所示[4]。

图2.2 系统硬件架构图
2.2.1飞行控制计算机与协处理器
导航飞行控制计算机采用A T91SAM7SE512（ARM7）微处理器[5]，它是在国内应用广泛的
第二章飞行控制系统硬件平台
基于ARM7TDMI内核的芯片，是目前应用最为广泛的32位嵌入式RISC处理器，具有稳定、体积小、运算快、功耗低的优点。

该处理器负责传感器数据融合、导航规划、控制运算以及与地面监控平台的交互，是整个飞行控制系统的核心。

协处理器包括A VR Mega8L单片机和PC/104工控板。

Mega8L单片机是Atmel公司生产的高性能低功耗CMOS单片机，可以实现近1MIPS/MHz的指令执行速度，负责遥控器信号(PCM1024 )解码和传输。

PC104工控板是工业应用级别的小型计算机，CPU工作频率可达900MHz，同时具有丰富的外围设备和计算能力，是实验室上一代飞行控制系统的主控平台, 负责ARM7与地面监控系统数据流的缓冲与传输。

2.2.2 传感器系统
1、IMU
用来测量UA V在飞行过程中的三轴加速度和角速度，使用ADI公司的高精度惯性感测(MEMS)模块ADIS 16350，该型号IMU提供了六组自由度(six-degrees-of-freedom)的动作感测，并且内嵌校正与滤波算法，能直接提供精确的3轴角速度与加速度测量值，标准数字SPI接口减少了AD转换的噪声干扰，避免后期繁杂的滤波以及校正处理。

2、COMP ASS( 电子罗盘)
电子罗盘采用PNI公司提供的磁通传感器SEN-S65和3轴磁通传感器驱动控制芯片
PNI-11096组成。

通过它提供的3个垂直安装的磁通传感器所感应磁通量的测量值，得到精确的航向角。

该传感器电路结构简单，通过全数字SPI口很方便的与ARM7连接。

3、GPS
GPS用来定位UA V在空间的坐标和运动速度。

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 、Y A W 和COLL）、GY401锁尾陀螺仪和S9255数位式锁尾舵机。

S9206舵机是飞行控制系统的控制输出执行器，响应为0.19秒/60度，PWM工作周期是30ms，它决定了飞行控制器的工作频率约为33Hz。

2.2.4 地面监控系统
地面监测系统是整个无人直升飞机系统的重要组成部分。

监控平台与机载程序之间具有双工通讯能力：一方面，机载程序需要把各种飞行数据发送给地面的监控平台，另一方面，监控平台需要能够随时接受用户的命令输入，并及时将命令发送给UA V。

监控平台硬件是一台DELL 的Laptop，在上面运行一套自行编写的GS(Ground Station)软件来完成上述功能[6]。

无线传输部分(WLAN)选用了思科公司的WRT54GC-CN路由器作为机载无线通讯设备，它采用802.11g作为通讯协议，最大传输速率达到54M, 监控平台采用内置符合802.11g 协议无线以太网卡的Laptop。

如要进行远程飞行，则由思科公司的W AP54G无线AP接入路由器。

2.3 本章小结
本章首先介绍了小型直升飞机的结构，以及基本参数，接着介绍飞控系统的硬件系统构架，详细描述了各个子系统的功能和硬件资料，表2-2和表2-3详细总结了飞控系统硬件信息，本章的知识是搭建自主飞行软件系统的基础。

第二章飞行控制系统硬件平台
表2-2 核心计算器性能参数
名称型号主要参数
主处理器A T91SAM7SE512 54MHz主频CPU，32位ARM RISC架构，
512Kb高速flash
协处理器
(PCM解码)
A VR Mega8L 16MHz主频，8位A VR微处理器，8Kb flash
协处理器(数据传输) OEM PC2000 PC/104 PIII级别533MHz主频CPU，64MKb内存，
支持串口、并口、USB和无线网络外围接
口
地面站DEll D620 Intel Core T2300E CPU ，512M内存，80G
硬盘
表2-3 传感器性能参数表
名称厂商与型号主要参数
IMU ADIS 16350 滤波后角度精度可以到达1度以内，零点漂移导致的角度偏差低于2度每20分钟
Compass SEN-S65 &&
PNI-11096
航向角抖动控制在在1-2度左右，动态
效果理想
GPS NovAtel OEMV-1 数据更新频率5Hz-20Hz，差分精度0.20m，速度精度0.3m/s
Sonar
Senscomp
Polaroid 6500
采样频率为7HZ，测量高度为7m，误
差可以控制在4cm以内
华南理工大学硕士学位论文
第三章导航基础
UA V自主飞行涉及到一些专业的控制和导航知识，例如导航坐标系、姿态角、导航控制、姿态控制、INS/GPS导航等等[7][8]，为方便理解，本章会做一些基本的介绍。

导航要解决的基本问题是：现在在哪里？要去哪里？如何去？本章首先回答第一个问题，也就是位置、速度和姿态角的描述，然后介绍INS/GPS导航算法。

其它两个问题将在四、五两章回答。

3.1 位置与速度描述
对UA V进行导航的首要问题就是要实时的确定其飞行状态，具体是指UA V当前位置、速度和姿态角。

飞行状态量需要在某个具体的坐标系中描述，这些坐标系包括地球中心坐标系(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线和赤道线，Y轴与X-Z轴构成右手法则，Z轴为通过地心指向北极。

3.1.2 当地水平坐标系
当地水平坐标系（简称NED），是在导航时根据导航系统工作的需要而选取的作为导航基准的坐标系，是用来做导航计算时使用的，同样是通过满足右手正交法则的三个向量轴来表示。

N向量指向的是北方（North），E向量指向的是东方（East），D向量是沿着当地重力加速度向量指向地（Down）。

N，E向量与地球表面相切，故称之为当地水平坐标系。

如图3-1所示。

第三章
导航基础
图3-1 地球中心坐标系(ECEF)与当地水平坐标系(NED)
图3-1中[]Z Y X 向量是地球中心坐标系的三个轴，[]D E N 向量是NED 坐标系的三个轴，λ表示经度，ϕ表示纬度。

3.1.3 Body-Frame
Body-Frame(Body Frame)是固连在UA V 机体上的坐标系。

原点b o 位于UA V 的重心处，b x 沿机体纵轴指向前，b y 沿机体横轴指向右，b z 垂直于b b b o x y 并相对于b x 、b y 成右手法则，沿UA V 的竖轴指向下。

如图3-2所示。

b
x b
y b
z b
O
图3-2 Body-Frame 示意图 3.1.4 WGS-84(World Geodetic System 1984)大地坐标
GPS 单点定位的坐标以及相对定位中解算的基线向量属于WGS-84大地坐标系, 因为GPS 卫星星历是以WGS-84坐标为根据而建立的[9]。

WGS-84椭球及有关常数采用国际大地测量和地球物理联合会第17届大会大地测量常数的
华南理工大学硕士学位论文
推荐值, 四个基本常数为：
● 长半轴: m a 26378137±=
● 地心引力常数(含大气层): )(10)6.03986005(238-⋅⨯±=s m G m
● 正常化二阶带谐系数: 9160.21030.11016685.484-⨯±⨯-=C
● 地球自转角速率: )(101500.010729211511111---⋅⨯±⨯=s rad ω
通过GPS 的输出信息, 可以得到:
● 经度(Longitude) λ: 在Greenwich 线经度为零, 即0=λ
● 纬度(Latitude) ϕ : 在地球赤道, 0=ϕ; 在北极, 90=ϕ ; 在南
极, 90-=ϕ
● 高度 h : GPS 输出的高度信息是相对于海平面的高度
3.1.5坐标系转换与位置/速度获取
第一步 ： WGS84转换到ECEF
GPS 接收机获得的信息实际是一串固定格式的数据报，通过解析这串数据报，得到WGS-84大地坐标系下的地理信息，包括：经度λ，纬度ϕ，海拔h ，航向Course 和航速Speed 。

首先将GPS 输出的经度、纬度及高度信息转换到地球中心坐标系(ECEF)上，转换公式如下：
λϕcos cos )(h N X += (3-1)
λϕsin cos )(h N Y += (3-2)
ϕsin )(22
h N a
b Z += (3-3) 这里采用的单位是米, 式中λ表示经度,ϕ表示纬度（单位：弧度）,h 是相对与椭球体表面的高度(单位:米), N 表示曲率半径(单位:米), 其定义为
ϕ22sin 1e a N -=
(3-4)
第二步： ECEF 转换到NED 在取NED 坐标系前，需要知道NED 坐标系原点的经度0λ和纬度0ϕ，从ECEF 转换到NED 坐标系的方向余弦表示为
⎥⎥⎥⎦
⎤⎢⎢⎢⎣⎡------=0000000
0000
000sin sin cos cos cos 0cos sin cos sin sin cos sin ),(ϕλϕλϕλλϕλϕλϕϕλC (3-5) T Z Y X ],,[000是NED 原点的ECEF 坐标，由ECEF 坐标系中的位置转换到NED 坐标系位置。