智能三维空间姿态解算系统设计与实现毕业设计说明书

南 阳 理 工 学 院 本科生毕业设计（论文）学 院：电子与电气工程学院专 业： 电子信息工程 学 生： 指导教师： 完成日期 2014 年 5 月 南阳理工学院本科生毕业设计（论文）智能三维空间姿态解算系统设计与实现Design and Implementation of Intelligent Three-dimensional Attitude Calculation System总 计： 30 页表 格： 2 个插 图： 21 幅南南 阳阳 理理 工工 学学 院院 本本 科科 毕毕 业业 设设 计（论文）计（论文）智能三维空间姿态解算系统设计与实现Design and Implementation of Intelligent Three-dimensional Attitude Calculation System学 院： 电子与电气工程学院 专 业： 电子信息工程 学 生 姓 名： 学 号： 指 导 教 师（职称）： 评 阅 教 师： 完 成 日 期： 南阳理工学院Nanyang Institute of Technology智能三维空间姿态解算系统设计与实现智能三维空间姿态解算系统设计与实现智能三维空间姿态解算系统设计与实现电子信息工程专业 摘摘要要 本系统采用 STM32 作为微控制处理器，主要完成智能三维空间姿态解算系统的设计。该系统主要实现以下功能。首先微控制处理器对加速度传感器、陀螺仪传感器、磁力计传感器进行数据采集，然后对采集到的数据进行数据滤波和算法解算以得到姿态数据，再将目标物体的三维空间姿态数据在上位机上实时显示，通过不断重复以上过程最终实现智能三维空间姿态解算的功能。 关键词关键词 姿态解算；微机电系统；四元数；欧拉角Design and Implementation of Intelligent Three-dimensional Attitude Calculation SystemAbstract: The system uses a STM32 as the micro-control processor, mainly to complete the design of intelligent three-dimensional attitude calculation system. The system has realized the following functions. The micro-processor collected the data of triaxial accelerometer, three-axis gyroscope and three-axis geomagnetic sensor through sensor circuits. And then it filtrated and calculated the data to obtain three-dimensional attitude data, afterwards displayed the calculated attitude data on the upper computer in real time, eventually to realize the intelligent three-dimensional attitude calculation system by repeating the above process.Key words: Attitude algorithm; micro-electromechanical systems; quaternion; Euler angle智能三维空间姿态解算系统设计与实现目目 录录1 引言.11.1 三维空间姿态解算算法的发展历程.11.2 国内外研究现状与应用前景.11.3 论文组织结构安排.22 系统总体设计.22.1 姿态航向参考系统的设计.22.1.1 MEMS 技术的介绍.32.1.2 MEMS 传感器的工作原理.32.2 智能三维空间姿态解算系统的设计方案.43 智能三维空间姿态解算系统的硬件设计.43.1 微控制处理器最小系统设计.43.1.1 微控制处理器的选型.43.1.2 微控制器最小系统原理图.53.2 供电电路设计.63.2.1 供电电路芯片选型.63.2.2 供电电路芯片特点及原理图.63.3 姿态航向参考系统模块设计.73.3.1 MPU-6050 工作电路 .83.3.2 HMC5883L 工作电路.93.3.3 传感器模块设计.104 智能三维空间姿态解算系统的软件设计.134.1 软件开发平台.134.2 软件下载及调试方式.144.3 软件底层驱动程序设计.154.3.1 传感器驱动程序的通信协议.154.3.2 传感器驱动程序的设计.154.4 串口通信驱动程序设计.174.4.1 串口通信原理设计.174.4.2 串口底层驱动设计流程图.18智能三维空间姿态解算系统设计与实现4.5 数据处理方案.184.5.1 传感器数据采集滤波设计.184.5.2 三维空间姿态解算算法.185 系统调试.215.1 调试方法.215.2 调试结果及分析.23结束语.27参考文献.28附录.29致谢.30智能三维空间姿态解算系统设计与实现1 引言2014 年 3 月 8 日马来西亚飞往北京的航班 MH370 失事，随后确定已坠落于南印度洋海域。为了搜寻此架飞机的黑匣子耗费了巨大的人力物力财力。飞机上安装的有两种黑匣子，分别是驾驶舱语音记录器和飞行数据记录器。驾驶舱语音记录器主要用于记录驾驶舱机组人员的言谈和机务人员同地面控制人员的对话等，而飞行数据记录器主要记录飞机的飞行参数、飞机操纵状况、发动机状态、飞行姿态及各种仪表数据等1。因此，找到黑匣子就意味着能够了解到飞机在失事前发生的事情的全过程。其中，飞行姿态就是通过三维空间姿态解算来实现的。研究智能三维空间解算系统的设计与实现不仅在航空领域有重大的应用，而且在航海等其他领域也有很好的应用。1.1 三维空间姿态解算算法的发展历程近现代以来，科学技术高速发展，智能的时代一直在更新前进，人们的生活也正在一步一步地发生变化。人们出行从远古时代的步行或者骑马，到现在的带有导航系统的汽车以及民航飞机和油轮客轮，这些变化为我们的生活带来了极大的便利。而这些，离不开三维空间姿态解算技术的发展与应用。采集目标的仰俯角、航偏角和滚动角是确定目标三维姿态的必要数据。我们可以对角速度做积分来取得姿态数据，但是由于陀螺仪本身的误差会随温度变化，并且长时间对角速度积分等同于放大误差，加上角速度所计算出来的角度其中有一个为 90或 270时，转回原始坐标时就会产生万向节死锁的问题，导致转不回去，这是一个很大算法缺陷。1843 年爱尔兰数学家哈密顿提出了四元数的基本概念，但是由于没有实际的应用而并没有得到人们的重视。四元数方法开始获得应用是在理论力学中。因为单位四元数算法相似于旋转矩阵的运算，在处理刚体运动学的实际问题时四元数和四元数矩阵发挥了很大的作用。自此，人们对四元数产生了浓厚的兴趣。后来，四元数在光学领域、计算机领域和航空航天领域等都得到了广泛的应用。对四元数的研究与应用，使科学事业又向前迈进一步。用四元数解算出欧拉角，可以避免产生万向节死锁，使三维空间姿态解算算法得到了进一步的完善。智能三维空间姿态解算系统设计与实现1.2 国内外研究现状与应用前景我国国内发展比较晚，近几年才开始发展，随着微传感器技术的飞速发展，三轴加速度计、三轴陀螺仪、三轴磁力计等之类的微传感器也走向成熟与稳定。三维空间姿态解算这一技术也越来越受到大家的重视，近年来取得了较快的发展，在三维空间姿态解算这方面取得了较大的进步。在国外，三维空间姿态解算技术已经在民用和军用领域得到了广泛的认可。民用方面，精细农业用的机械与车辆、用于农药喷洒与林业防火的无人飞机等已部分分装该类型组合导航系统；军用方面，它在无人侦察机和卫星探测器等上也已被多数发达国家应用。随着时代的变化和科技的迅速发展，把微传感器做的更加小型化、智能化和集成化也成为了现代社会的必要。采用体积小、可靠性高、成本低的微传感器不仅可以优化国民经济生活，还可以促进国防事业的发展。智能三维空间姿态解算系统除了应用在航空航天领域中，同时在海洋探测领域同样有重要意义，比如说监测潜艇运行姿态和控制水下自动设备的稳定性等。不仅如此，在机器人运动领域、汽车工业领域、人体运动测量等方面也有很好的应用前景。以后的国民生活很可能离不开智能三维空间解算系统了，因为它将会遍布国民生活的各个角落。1.3 论文组织结构安排论文围绕智能三维空间姿态解算系统设计与实现展开论证。第 1 节从三维空间姿态解算的发展历程、国内外发展现状和应用前景方面进行了阐述。第 2节主要从姿态航向参考系统的组成和智能三维空间姿态解算系统的设计方案方面介绍了系统总体设计。第 3 节主要从系统微控制处理器最小系统、传感器采集电路、系统供电电路的设计入手，介绍智能三维空间姿态解算系统的整体硬件设计。第 4 节主要介绍软件设计平台、驱动程序设计以及姿态解算的算法等。第 5 节主要介绍系统的调试方法和设计结果与分析。2 系统总体设计2.1 姿态航向参考系统的设计为了获取空间目标物体的姿态信息，就需要建立 AHRS（Attitude and Heading Reference System） ，即姿态航向参考系统，它主要包含了基于 MEMS智能三维空间姿态解算系统设计与实现的三轴加速度传感器、三轴陀螺仪传感器、三轴电磁罗盘传感器，把它们合称为九轴传感器。随着科学技术的不断进步，电子行业也取得了飞速的发展。2.1.1 MEMS 技术的介绍MEMS（micro-electromechanical systems）是微机电系统的英文缩写，是一种先进的制造技术平台。它是指可以批量制造的，由微型传感器，微型执行器，信号处理和控制电路，通讯接口和电源等部件组成的一体化的微型器件或系统，其目的是把信息的提取、处理和执行集成在一起，组成具有多功能的微型系统，集成于打尺寸系统中，从而大幅度地提高系统的自动化、智能化和可靠性水平2。MEMS 涉及到了很多学科，包括电子、材料、物理、化学、生物和机械等。二十一世纪 MEMS 将逐步从实验室走向实用化，对工农业、信息、环境、生物工程、医疗、空间技术、国防和科学发展产生重大影响。MEMS 有如下几点特点：（1）微型化。（2）以硅为主要材料，机械电器性能良好。（3）批量生产。（4）集成化。（5）多学科交叉。MEMS 器件具有由于体积微小，功耗低，响应快，灵敏度高，质量轻，成本低的优点，因此在运动测量领域也逐渐开始采用微电子器件3。它作为新一代的惯性器件，在三维姿态解算系统中发挥着越来越重要的作用。2.1.2 MEMS 传感器的工作原理（1）加速度传感器的工作原理：采用弹簧质量系统的结构形式，质量块与电极之间的距离会随质量所受惯性力的变化而改变，因而电容值也随之改变且和加速度成正比关系。电容式加速度传感器具有很多优点，比如高灵敏度、容易适应环境以及零频率响应等。虽然它也有像输入输出呈非线性的关系等缺点，但在本系统中影响不是很大，基本上可以忽略，而主要充分利用了它的优点。（2）陀螺仪传感器的工作原理：陀螺仪是测角速度的传感器。MEMS 陀螺仪依赖转动坐标系角振动产生科里奥利力效应来检测角速度4。它通常有两个方向的可移动电容板，径向的电容板加震荡电压迫使导体作径向运动，横向的电容板测量由于横向科里奥利运动带来的电容变化。角速度可以依据科里奥利力和角速度成正比的关系计算出。（3）电磁罗盘传感器的工作原理：利用磁阻传感器测量地磁场强度，根据智能三维空间姿态解算系统设计与实现其水平分量的比值来确定载体相对北向的航向角。一般情况下，安装在机体上的电子磁罗盘的坐标系和机体坐标系重合。2.2 智能三维空间姿态解算系统的设计方案本系统的设计主要包括硬件设计和软件设计，其中姿态解算是依据数学和物理原理来实现的，并通过 C 语言编写成相应的程序，并结合实际电路进行调试，以实现将空间目标物体的瞬时姿态数据显示精确地在上位机上。设计本系统需要做的三个方面。（1）系统硬件平台的设计，主要有供电电路、单片机最小系统、传感器采集电路、串口通信这几部分构成。（2）系统软件的设计，主要是对传感器获取的数据，进行滤波，四元数、欧拉角算法的处理，最终输出结果。（3）三维空间姿态解算的调试工作，主要是通过串口把数据发到上位机进行调试，最终实现系统任务。二十一世纪是信息的时代，是科技的时代。只有及时地获得可靠的精确的信息数据才能够掌握先机，使自己处于优势的地位。因此，设计本系统，要做到能够快速并连续地提取姿态数据，并且保证姿态数据的精确性和准确性。不仅如此，在得到这些数据的同时，要及时地传送到上位机，以供检测和控制。然而在现实中，会存在不同程度的噪声干扰，并且通过姿态解算原理获得的更新数据也会存在些许误差，提高系统的精确度是完善智能三维空间姿态解算系统的必需之处。3 智能三维空间姿态解算系统的硬件设计硬件是一个电子系统的基础，一个良好的硬件平台是系统稳定型的标志，所以硬件平台的搭建是不容忽视的。智能三维空间姿态解算在硬件设计方面主要分为以下几个模块：供电模块，微控制器最小系统模块，传感器采集模块，串口通信模块，这几部分构成了整体的硬件设计平台。考虑到系统的稳定性，因此对于传感器这部分电路，可以采用现有的模块来实现，这样避免了由于人工焊接导致在一定程度上损伤传感器的稳定性和精确度，因此我们选择了 GY-86 这个九轴传感器模块。智能三维空间姿态解算系统设计与实现3.1 微控制处理器最小系统设计3.1.1 微控制处理器的选型微控制处理器的选型关系到整个系统设计是十分重要的，对于三维空间姿态解算这个系统的设计需要考虑到以下三个方面。（1）微控制器的执行速度，在本系统设计中由于要对多个传感器进行数据采集，并且采集的速率较高，因此微控制的时钟要高。（2）微控制器的数据处理能力，在本系统要通过四元数等算法去处理一些数据。其中尤其是关于浮点型数据的处理。（3）微控制器的稳定性，对于每一个综合电子系统，这一点都是必不可少的。系统的稳定性决定系统的成败。由于上述因素，最终选取了 STM32 作为智能三维空间姿态解算系统的微控制处理器。STM32F103x 增强型系列芯片使用高性能的 ARM Cortex-M3 32 位的RISC 内核，工作频率为最高可达 72MHz，内置高速存储器(高达 128K 字节的FLASH 和 20K 字节的 SRAM)，丰富的增强 I/O 端口和连接到两条 APB 总线的外设。所有型号的器件都包含 2 个 12 位的 ADC、3 个通用 16 位定时器和一个PWM 定时器，还包含标准和先进的通信接口：多达 2 个 I2C 和 SPI、3 个USART、一个 USB 和一个 CAN5。它的工作电压为 3.3V。该智能三维空间姿态解算系统的硬件电路是以 STM32F103C8T6 为微控制器，最终选择 C8T6 是因为它还具有以下优点：STM32 增强型系可以与所有的 ARM 工具和软件兼容。STM32 的内部 FLASH 是在线可编程的。STM32 有优秀的功耗控制。STM32 拥有强大的库函数。除此之外，它只有 48 个引脚，不仅满足了本次设计的需要，而且价格便宜，性价比高。3.1.2 微控制器最小系统原理图本设计的最小系统原理图如图 1 所示。主要有以下几部分构成：微控制处理器，SWD 调试下载接口，外围晶振电路，以及复位电路这几部分构成。这保证了本系统的能够完成单片机运作的基本功能，是实现三维空间姿态解算的基础。（1）微控制器：在本次设计中由于对 STM32 系列的单片机引脚和片内存储器容量要求较小，因此选用 STM32F103C8T6。智能三维空间姿态解算系统设计与实现（2）供电电路：工作电压为 2.0V 到 3.6V ，在本次设计中采用 3.3V 供电。（3）晶振电路：单片机内部自带 8MhzRC 震荡器，本系统使用外接 8Mhz的石英晶振。（4）复位电路：采用经典的按键复位电路，通过按键按下拉低 Reset 引脚，使其硬件复位。（5）下载电路：本次采用 SWD 方式和 ISP 方式下载程序，采用 SWD 模式也可以进行软件硬件仿真。+3.3VGNDTO05TO06TO07TO04UART1-TXUART1-RXUART2-RXUART2-TXBOOT0R310KGNDH-SDAH-SCLFSYNCINTADRDYMOSCKCSNCEIRQGND22pFC1022pFC91Y18MHz10KR10.1uFC11GND+3.3VS1switchADC-4ADC-5R210KGNDSWDIOSWCLKBeepLED2LED1powerNRSTMIPA0/TIM2-CH110PA1/TIM2-CH211PA2/TIM2-CH312PA3/TIM2-CH413PA6/TIM3-CH116PA7/TIM3-CH217PB0/TIM3-CH318PB1/TIM3-CH419PA4/ADC12-IN414PA5/ADC12-IN515PC13-ANTI-TAMP2PC14-OSC32-IN3PC15-OSC32-OUT4PA9/USART1-TX30PA10/USART1-RX31OSC-IN5OSC-OUT6PB6/TIM4-CH142PB7/TIM4-CH243PB8/TIM4-CH345PB9/TIM4-CH446PA11/TIM1-CH432PA8/TIM1-CH129PB10/I2C2-SCL21PB11/I2C2-SDA22PB12/SPI2-NSS25PB13/SPI2-SCK26PB14/SPI2-MISO27PB14/SPI2-MOSI28PB541PA1233PB3/JTDO39PB4/JNTRST40PA15/JTDI38PB2/BOOT120BOOT044PA14/JTCK/SWCLK37PA13/JTMI/SWDIO34NRST7VBRT1VDDA9VDD-124VDD-236VDD-348VSSA8VSS-223VSS-335VSS-447STM 32F103C 8T6*STM32F103C8T6(XIN)12345P1SWD+3.3VGNDBOOT0UART1-RXUART1-TX图 1 STM32F103C8T6 最小系统原理图3.2 供电电路设计3.2.1 供电电路芯片选型供电芯片的选型，这个要考虑系统的总体硬件设计需求。（1）本系统主要有微控制器，传感器，串口这几个部分需要供电。而在本次设计中，微控制器需要 3.3V 供电，传感器需要 3.3V 供电，串口也需要 3.3V供电。因此本次设计需要产生 3.3V 的电源。同时也要估计到供电电流和输出电智能三维空间姿态解算系统设计与实现流。（2）由于智能三维空间姿态解算系统这个设计一般是用在飞行器等对对空间姿态要求较高的地方使用，因此本次的输入电压设定为 12V。经过考虑最终选取了 LM2940-5.0 和 ASM1117-3.3 ，其中 LM2940-5.0 把12V 转化到 5V，ASM1117-3.3 把 5V 转化输出为 3.3V。3.2.2 供电电路芯片特点及原理图（1）LM2940-5.0 此芯片是输出电压固定的低压差三端稳压器。它的电气特性如下： 输出电压：5V 输出电流为 1A，输出电流为 1A 时，最小输入输出电压差小于 0.8V。 最大输入电压：26V 工作温度：-40+125 内含静态电流降低电路、电流限制、过热保护和反插入保护电路等。 因此稳压器完全可以满足本次设计的需要。（2）ASM1117-3.3 这个芯片固定电压 3.3V 输出电流为 1A 线路调整率；0.2%（MAX）负载调整率：0.4%（MAX） 。它的电气特性如下： 输出电压：3.2673.333V（0= IOUT=1A , 4.75V=VIN=12V） 线路调整（MAX）：10mV（4.75V=VIN=12V） 负载调节（MAX）：15mV（VIN=5V，0= IOUT=1A） 电压差（MAX）：1.3V 电流限制：9001500mA 静态电流（最大） ，10mA 纹波抑制（最小）：60dB通过对这两种稳压器的了解，参考对应芯片的芯片手册，设计了供电电路如图 2 所示，此供电方案达到了本系统的供电需求。10uFC3Cap Pol122uFC4Cap Pol1GND+3.3VGND1OUT2IN3AMS1117-3.3电源电路GND+12V30KR9Res210KR10Res2GND+12Vpower12D51N582212P11串串GND3OUT2IN1LM2940-5.0GND47uFC122uFC2+5V图 2 系统供电原理图智能三维空间姿态解算系统设计与实现3.3 姿态航向参考系统模块设计姿态航向参考系统 AHRS(Attitude and Heading Reference System)主要包含了基于 MEMS 的三轴加速度计，三轴陀螺仪和三轴磁阻传感器。空间目标物体的三轴加速度和三轴角速度分别由加速度计和三轴陀螺仪测量出。三轴磁阻传感器可以感应目标物体姿态变化时磁场在各测量轴向上的强度变化，并由此可以计算出偏航角6。传感器部分主要采用了 MPU-6050 和 HMC5883L 这两个芯片，由它们两个组成了姿态航向参考系统的硬件部分。三轴陀螺仪传感器和三轴加速度计传感器集成在芯片 MPU-6050 中，而 HMC5883L 包含了一个三轴地磁传感器。这两个芯片通过上拉电阻挂接在 I2C 总线上。图 3 表示了姿态航向参考系统的结构框图。图 3 姿态航向参考系统的结构框图3.3.1 MPU-6050 工作电路MPU-6050 集成了三轴 MEMS 陀螺仪和三轴 MEMS 加速度计，以及一个可扩展的数字运动处理器 DMP（Digital Motion Processor） 。MPU-6050 分别用了三个 16 位的 ADC 将三轴陀螺仪和三轴加速度计采集到的模拟量数据转化成可输出的数字量。用户可控传感器的测量范围能够更加精确地跟踪快速和慢速的运动。电源范围，MPU-6050 可支持 VDD 范围为2.5V5%，3.0V5%，3.3V5%。另 MPU-6050 还有 VLOGIC 引脚，可为 I2C输出提供逻辑电平，VLOGIC 电压可取 1.8V5%或者 VDD。 其他的参数如下： 陀螺仪运作电流：5mA 陀螺仪待命电流：5A 加速度计运作电流：500A智能三维空间姿态解算系统设计与实现 加速度计省电模式电流：40A10Hz芯片 MPU-6050 采用 5V 或 3.3V 电源输入，XDA/XCL 用于外接其他 I2C接口传感器。 SDA 和 SCL 分别连接到单片机。INT 产生中断信号，连接至单片机。AD0 接地。 注意使用 IO 口模拟 I2C 信号时候，要注意 SDA 所连接的单片机 IO 口输入输出方向设置。MPU-6050 使用 I2C 接口和芯片连接，并且总是作为从设备。连接主设备的逻辑电平用 VLOGIC 引脚设置。MPU-6050 的应用电路如 4 所示。CLKIN1AUX_DA6AUX_CL7VLOGIC8AD09REGOUT10FSYNC11INT12VDD13GND18RESV19CPOUT20RESV21CLKOUT22SCL23SDA24U2MPU-6050GNDXDAXCLVCC_3.3V0.01uFC180.1uFC20FSYNCINTA1KR16GNDSDASCL0.1uFC21GNDVCC_3.3V0.1uFC19GND图 4 MPU-6050 应用电路3.3.2 HMC5883L 工作电路HMC5883L 包括最先进的高分辨 HMC118X 系列磁阻传感器。磁阻传感器是利用各向异性磁电阻效应制作的传感器，各向异性磁阻效应指的是铁磁金属（例如：钻、铁、镍等及其合金）的磁化方向和电流方向平行时的电阻率和两者垂直是的电阻率有明显差别7。采用霍尼韦尔各向异性磁电阻（Anisotropic Magneto Resistive）技术的 HMC5883L，是一种表面贴装的高集成模块，并带有数字接口的弱磁传感器芯片，而且它内部附带了霍尼韦尔专利的集成电路，具有在轴向高灵敏度和线性高精度的特点。在地磁场传感器行业中，霍尼韦尔的磁传感器是灵敏度最好和可靠性最好的传感器。HMC5883L 具有以下特点： 三轴磁阻传感器和 ASIC 都被封装在 3.03.00.9mm LCC（Leadless Chip Carriers）表面装配中 12-bit ADC 与低干扰 AMR 传感器，能在8 高斯的磁场中四线 5 毫高斯分辨率 内置自检功能 低电压工作（2.16-3.6V）和超低功耗（100A）智能三维空间姿态解算系统设计与实现 内置驱动电路 I2C 数字接口 无引线封装结构 磁场范围广(+/-80e) 有相应软件及算法支持 最大输出频率可达 160Hz根据 HMC5883L 的各个管脚需要这样设计电路。管脚 2、4 和 13 并联并分两条支路，一支接 3.3V 的供电电路，一支和 0.1F 的电容 C12 串联并接地。XCL 和 XDA 用于接其他 I2C 接口传感器。STEP 和 SETC 通过 C16 串联起来用于驱动传感器。管脚 10 是需要串联电容并接地。DRDY 连接到 STM32C8T6的第 39 管脚 PB3/JTDO 处，它的内部被拉高，当数据位于输出寄存器时会在低电位上停 250sec。HMC5883L 的应用电路如图 5 所示。VDD_IO13S14VDD2SCL1SDA16STEP8SETC12C110GND9GND11DRDY15U1HMC5883LVCC_3.3V0.1uFC12GNDXCLXDA224C16DRDY475C17GND图 5 HMC5883L 应用电路3.3.3 传感器模块设计智能三维空间姿态解算系统的设计，要提高系统的精确度和稳定性，采用现有的传感器模块是十分有必要的。采用模块有以下两个优点：（1）减少了焊接的难度，三轴加速度，三轴陀螺仪，三轴电磁罗盘传感器都是 BGA 封装，如果自己焊接难度很大，并且在焊接的过程中对传感器的精度产生影响。（2）减少了机械布局导致的误差，对于元件的摆放，如果元件的摆放位置误差较大，会影响到系统的精确度，并且对于软件设计也会带来较多的麻烦。此模块主要有以下几部分构成，模块的供电电路，在本模块中由于考虑到智能三维空间姿态解算系统设计与实现用户供电电压一般为 3.3V 或 5V，因此该模块同时提供了这两种电源接口设计。另外该模块上面还用工作指示灯，当该模块正常工作时，此指示灯点亮，如指示灯处于熄灭状态，则要立即检查硬件设计电路，避免意外事件发生。另外该模块主要用 MPU-6050、HMC5883L 传感器进行数据采集，而 MPU-6050 有支持外接 I2C 接口器件，因此在此处选择了 HMC5883L 传感器挂接在 MPU-6050模块上，对于外接传感器的工作模式，可以对 MPU-6050 的寄存器进行配置来确定。传感器模块原理图如图 6 所示。VDD_IO13S14VDD2SCL1SDA16STEP8SETC12C110GND9GND11DRDY15U1HMC5883LI1GND2en3by4O5U3LDOCLKIN1AUX_DA6AUX_CL7VLOGIC8AD09REGOUT10FSYNC11INT12VDD13GND18RESV19CPOUT20RESV21CLKOUT22SCL23SDA24U2MPU-60500.1uFC13GNDGND10uFC140.1uFC15GND1KR11D6LED0GND4.7KR124.7KR13Q2N_MOS4.7KR144.7KR15Q3N_MOSVCC_5VVCC_3.3VVCC_3.3VVCC_5VH_SCLVCC_5VVCC_3.3VH_SDASDAVCC_3.3V0.1uFC12GNDXCLXDA224C16DRDY475C17GNDGNDXDAXCLVCC_3.3V0.01uFC180.1uFC20FSYNCINTA1KR16GND12345678P14Header 8HVCC_5VVCC_3.3VGNDH_SCLH_SDAFSYNCINTADRDYSDASCL0.1uFC21GNDVCC_3.3V0.1uFC19GNDSCL图 6 传感器模块原理图以上主要从微控制处理器最小系统、供电电路和姿态航向参考系统的硬件设计方面具体的介绍了智能三维空间姿态解算系统的几个模块。接下来，需要对智能三维空间姿态解算系统做一个整体性的阐述。（1）最小系统微控制器的设计，这是系统的控制核心，对于此部分的设计要考虑 CPU 的供电电压、晶振电路、复位电路、下载电路等的设计，尤其是对于下载电路的设计要注意布线规则，否则有可能导致芯片无法下载程序，进而导致设计失败。（2）系统的供电电源，这部分主要保证整个系统的供电需求以及系统的稳定性，因此要考虑系统的供电电压是否满足，电压的纹波是否过大，还有最重要的一点就是系统的整体供电电流的大小，否则会导致系统传感器数据采集出现异常现象。（3）传感器模块的设计，在这一部分主要考虑传感器的工作电压，以及传智能三维空间姿态解算系统设计与实现感器的数据接口，这两点可以保证传感器的数据正确采集。另外还有一点就是各个传感器的机械布局，因为本设计主要是进行姿态解算，传感器芯片的布局会影响到软件算法的设计，这三点考虑到就可以达到设计的需求了，最终本设计选择了成品传感器模块。另外在整个系统的硬件设计中，考虑到以后的扩展应用，又添加了一些LED 和蜂鸣器声光调试电路，AD 电压采集接口以及无线收发电路等，方便该系统以后的二次开发使用，提高硬件设计的潜在价值。结合以上三方面介绍，该系统的硬件设计原理图如图 7 所示。+3.3VGND123P31串串串123P42串串串123P53串串串123P64串串串GNDGNDGNDGNDTO04TO05TO06TO07四路电调(PWM)+5V+5V+5V+5V12345P1串串11234P2串串2+3.3V串口GNDGNDBOOT0UART1-RXUART1-TXUART2-RX+3.3VTO05TO06TO07TO04UART1-TXUART1-RXUART2-RXUART2-TXBOOT0R310KGNDH-SDAH-SCL12345678P810DOF+5V+3.3VH-SCLH-SDAINTADRDYFSYNC10DOF接口GNDFSYNCINTADRDYSCKMIMOCSNIRQCESPI(24L01)+3.3VGNDMOSCKCSNCEIRQGND22pFC1022pFC91Y18MHz10KR10.1uFC11GND+3.3VS1switchC50.1uFC60.1uFC70.1uFC80.1uFGND+3.3V10uFC3Cap Pol122uFC4Cap Pol1GND+3.3VGND1OUT2IN3AMS1117-3.3电源电路ADC-4ADC-512P9ADC-4串5模拟口R210KGNDADC-4ADC-5SWDIOSWCLKSWDIOSWCLKSW调试接口D1LED11KR4Res2D2LED11KR5Res2+5V+3.3VGNDGNDD3LED11KR6Res2+3.3VLED+5V560R8Res2GNDBeepBeepLED1蜂鸣D4LED11KR7Res2+3.3VLED2LED2LED1GND+12V123456P12Header 3X2123456P13Header 3X2+5V+3.3VGNDGND引出电源接口30KR9Res210KR10Res2GND+12Vpowerpower+3.3VNRSTA1K2LS1串串串Q1J3YUART2-TXMIGND123456P10Header 3X2ANRSTPA0/TIM2-CH110PA1/TIM2-CH211PA2/TIM2-CH312PA3/TIM2-CH413PA6/TIM3-CH116PA7/TIM3-CH217PB0/TIM3-CH318PB1/TIM3-CH419PA4/ADC12-IN414PA5/ADC12-IN515PC13-ANTI-TAMP2PC14-OSC32-IN3PC15-OSC32-OUT4PA9/USART1-TX30PA10/USART1-RX31OSC-IN5OSC-OUT6PB6/TIM4-CH142PB7/TIM4-CH243PB8/TIM4-CH345PB9/TIM4-CH446PA11/TIM1-CH432PA8/TIM1-CH129PB10/I2C2-SCL21PB11/I2C2-SDA22PB12/SPI2-NSS25PB13/SPI2-SCK26PB14/SPI2-MISO27PB14/SPI2-MOSI28PB541PA1233PB3/JTDO39PB4/JNTRST40PA15/JTDI38PB2/BOOT120BOOT044PA14/JTCK/SWCLK37PA13/JTMI/SWDIO34NRST7VBRT1VDDA9VDD-124VDD-236VDD-348VSSA8VSS-223VSS-335VSS-447STM 32F103C 8T6*STM32F103C8T6(XIN)GND1VCC2CE3CSN4SCK5MOSI6MISO7IRQ8P7NRF24L0112D51N582212P11串串+3.3VGND3OUT2IN1LM2940-5.0GND47uFC122uFC2+5V12345P1SWD+3.3VGNDBOOT0UART1-RXUART1-TX图 7 硬件设计原理图根据系统硬件的原理图可以利用 Protel 软件可以画出系统硬件的 PCB 板如图 8 所示.智能三维空间姿态解算系统设计与实现图 8 系统硬件的 PCB 板智能三维空间姿态解算系统的实物如图 9 所示。图 9 智能三维空间姿态解算系统实物智能三维空间姿态解算系统设计与实现4 智能三维空间姿态解算系统的软件设计智能三维空间姿态解算系统的软件设计是系统的灵魂。软件设计的好坏决定了设计的成败。这里主要从软件的编程环境，程序的下载及调试方式，传感器驱动的设计和三维姿态解算控制算法的设计来展开介绍。4.1 软件开发平台在本系统中采用了 Keil uVision4 作为系统软件的开发工具。Keil uVision4引入灵活的窗口管理系统，使开发人员能够使用多台监视器，并提供了视觉上的表面对窗口位置的完全控制的任何地方。新的用户界面可以更好地利用屏幕空间和更有效地组织多个窗口，提供一个整洁，高效的环境来开发应用程序。新版本支持更多最新的 ARM 芯片，还添加了一些其他新功能。在以下几个方面体现出来。（1）uVision4 引入了窗口管理系统灵活方便，支持多显示器窗口。（2）uVision4 在 uVision3 IDE 的基础上，增加了更多大众化的功能。（3）多显示器和灵活的窗口管理系统。（4）系统浏览器窗口的显示设备外设寄存器信息。（5）调试还原视图创建并保存多个调试窗口布局。（6）多项目工作区简化与众多的项目。因此在此版本下可以方便简洁的完成系统软件开发的需求，一个稳定易用的软件开发平台可以大大的提高我们的开发效率。4.2 软件下载及调试方式STM32F10 x 系列单片机有三种下载方式，分别是 J-Flash 下载（需要用到J-link） ，MDK 配置下载（需要用到 J-link） ，ISP 下载（通过串口下载） 。这三中下载方式各有特点。以下对这几部分进行介绍。（1）J-Flash：J-Link 是 SEGGER 公司为支持仿真 ARM 内核芯片推出的JTAG 仿真器。配合 IAR EWAR，ADS，KEIL，WINARM，RealView 等集成开发环境支持所有 ARM7/ARM9/ARM11,Cortex M0/M1/M3/M4, Cortex A4/A8/A9 等内核芯片的仿真，与 IAR,Keil 等编译环境无缝连接，操作方便、连接方便、简单易学，是学习开发 ARM 最好最实用的开发工具。（2）MDK：Keil MDK，也称 MDK-ARM、uVision4 等。目前三家国内代理商为 Keil MDK 提供技术支持和相关服务。MDK-ARM 软件为基于 Cortex-智能三维空间姿态解算系统设计与实现M、Cortex-R4、ARM7、ARM9 处理器设备提供了一个完整的开发环境。 MDK-ARM 专为微控制器应用而设计，不仅易学易用，而且功能强大，能够满足大多数苛刻的嵌入式应用。其环境配置部分图如图 10 所示。图 10 MDK 下载配置（3）ISP：ISP（In-System Programming）即在线系统编程，一种无需将存储芯片（如 EPROM）从嵌入式设备上取出就能对其进行编程的过程。在系统编程需要在目标板上有额外的电路完成编程任务。其优点是，即使器件焊接在电路板上，仍可对其重新进行编程。在系统可编程是 Flash 存储器的固有特性（通常无需额外的电路） ，Flash 几乎都采用这种方式编程8。经过对比可以看到，在 Keil uVision4 软件开发平台下，使用 MDK 结合 J-Link 的下载方式十分方便，简介只要配置好软件的下载环境，就可以方面使用，并且 J-Link 可以实现硬仿真的功能，更有利于我们找到问题的所在。其中 J-link 下载有几种接口，这次我们选用了 SWD 这种接口方式，SWD 这种接口占用的 IO 管脚少，并且下载方便快速稳定。4.3 软件底层驱动程序设计4.3.1 传感器驱动程序的通信协议在智能三维空间姿态解算系统中，我们主要用了三轴加速度、三轴陀螺仪以及三轴电磁罗盘，整体上简称为九轴传感器。九轴传感器都是通过 I2C 通信的，通过 I2C 协议对传感器进行读写操作，最终获取到相应的传感器数据。智能三维空间姿态解算系统设计与实现I2C（Inter-Integrated Circuit）总线是由 PHILIPS 公司开发的两线式串行总线，用于连接微控制器及其外围设备。是微电子通信控制领域广泛采用的一种总线标准。它是同步通信的一种特殊形式，具有接口线少，控制方式简单，器件封装形式小，通信速率较高等优点9。其有如下几个突出特点： 只要求两条总线线路：一条串行数据线 SDA，一条串行时钟线 SCL； 每个连接到总线的器件都可以通过唯一的地址和一直存在的简单的主机/从机关系软件设定地址，主机可以作为主机发送器或主机接收器； 它是一个真正的多主机总线，如果两个或更多主机同时初始化，数据传输可以通过冲突检测和仲裁防止数据被破坏； 串行的 8 位双向数据传输位速率在标准模式下可达 100kbit/s，快速模式下可达 400kbit/s，高速模式下可达 3.4Mbit/s； 连接到相同总线的 I2C 数量只受到总线的最大电容 400pF 限制。4.3.2 传感器驱动程序的设计传感器驱动程序的设计分为以下几部分：STM32 内部模块的初始化，传感器接口模块的初始化。其具体有以下几个步骤：（1）STM32 系统时钟的初始化。（2）STM32 对应传感器模块的 GPIO，I2C 模块的初始化。（3）传感器内部寄存器的初始化，这个初始化之后就可以读取传感器的数据了。（4）周期性获取传感器的数据。整体上传感器驱动的原理流程框图设计如图 11 所示。智能三维空间姿态解算系统设计与实现图 11 传感器驱动原理图4.4 串口通信驱动程序设计4.4.1 串口通信原理设计串口通信的概念非常简单，串口按位（bit）发送和接收字节。尽管比按字节（byte）的并行通信慢，但是串口可以在使用一根线发送数据的同时用另一智能三维空间姿态解算系统设计与实现根线接收数据。它很简单并且能够实现远距离通信。串行通信有两种最基本的通信方式：同步串行通信方式和异步串行通信方式。同步串行通信方式是指在相同的数据传送速率下，发送端和接受端的通信频率保持严格同步。由于不需要使用起始位和停止位，可以提高数据的传输速率，但发送器和接受器的成本较高。异步串行通信是指发送端和接受端在相同的波特率下不需要严格地同步，允许有相对的时间时延，即收、发两端的频率偏差在 10以内，就能保证正确实现通信10。在本系统中，由于要把串口数据发送到电脑的上位机上，因此需要用到USB 转串口线。另外串口通信的具体配置如下：（1）串口号：这个是 USB 转串口线连接到电脑上系统会自动串口号，选择相应的串口号即可。（2）串口波特率：在本系统中由于串口传送的数据量较大，并且有要保证数据传送的正确率，因此最终选取串口的波特率为 115200。（3）数据停位数：选用了常用的 8 bits 。（4）奇偶校验位：通过调试发现不设置奇偶校验位在此波特率下可以正常使用，在这里奇偶校验位没有设置。下面是串口设置的配置图，如图 12 所示。图 12 串口配置图4.4.2 串口底层驱动设计流程图串口通信底层设计流程主要有以下几个部分构成：STM32 串口通信模块的配置，串口上位机通信协议的设置。要把单片机处理后的发送到上位机上，并正确的显示，通讯协议的制定是必不可少的。智能三维空间姿态解算系统设计与实现串口设置系统工作流程如下：（1）STM32 内部串口模块初始化，包括串口时钟的初始化，串口 IO 口关键的配置。（2）STM32 底层驱动函数的设计，主要编写串口发送接收到传感器数据的函数，以及传感器发送三维空间姿态解算后的数据。（3）串口上位机参数周期性发送，这个主要是周期性更新串口上位机上的数据，实现动态实时显示。4.5 数据处理方案4.5.1 传感器数据采集滤波设计常用的数字滤波方法有以下几种：限幅滤波法，中位值滤波法，算数平均滤波法，中位值平均滤波法。在本次设计中主要应用了限幅滤波法和算数平均滤波法。以下对这两种滤波方法做简单介绍。（1）限幅滤波法：根据经验判断，确定两次采样允许的最大偏差值（设为A） ，每次检测到新值时判断：如果本次值与上次值之差A，则本次值无效，放弃本次值，用上次值代替本次值。此方法的优点是能有效克服因偶然因素引起的脉冲干扰，但此法的缺点也比较明显,就是无法抑制那种周期性的干扰，平滑度差。（2）算数平均滤波法：连续取 N 个采样值进行算术平均运算。N 值较大时：信号平滑度较高，但灵敏度较低；N 值较小时：信号平滑度较低，但灵敏度较高。此方法的优点是适用于对一般具有随机干扰的信号进行滤波，这样信号的特点是有一个平均值，信号在某一数值范围附近上下波动。但缺点也比较明显，就是对于测量速度较慢或要求数据计算速度较快的实时控制数据处理量较大。因此在本次设计中结合了这两种算法来达到滤波的效果，先用限幅滤波法去除传感器数据采集回来的噪点，再用算数平均值法取做平滑滤波。这种结合的方法最终取得了较好的效果，满足了本系统设计的需要。4.5.2 三维空间姿态解算算法在本系统中我们最终要获取目标物体的三维空间姿态即航向、横滚、俯仰。在本次系统设计中，为了保证三维空间姿态解算的稳定性和精确度，采用了三轴加速度、三轴陀螺仪、三轴电磁罗盘作为数据获取源。要最终解算出目标物的三维空间姿态，一般要有以下几个方面：智能三维空间姿态解算系统设计与实现（1）传感器初始化校正MEMS 传感器元件集成度高，并且成本较低，并且一般这样的传感器一般都有误差存在，因此传感器初始化要校正。我们把地球上相同地区的重力加速度误差视为零。预先测量在水平状态下的重力加速度 g 对应的传感器获取的数字量作为参考零点。因此在初始化目标物时要保证目标物保证水平，多次获取数据取得平均值作为参考零点。对于陀螺仪的校正，陀螺仪为测量角速度的元件，可知在陀螺仪静止时，角速度为零，因此我们可以利用此条件对陀螺仪进行初始化校正。在本系统中，采用的方法是：让目标物保持静止，然后此时连续采集陀螺仪数据 200 次，并求其平均值，并把此平均值作为陀螺仪的参考零点。由于地球磁场并非是均匀分布的，因此磁场的情况比较复杂，在不校正的情况下，很难获取正确的航向。因此在本次系统中，采用加速度传感器和陀螺仪传感器拟合出来的航向角与电磁罗盘进行互相校正来达到航向姿态的解算。（2）地理坐标系和机体坐标系描述空间目标运动物体的姿态，需要以地理坐标系为基准确定目标，再从地理坐标系换算成机体坐标系来确定目标姿态。地理坐标系如图 13 所示，它以运动体 M 和地球中心连线与地球表面交点O 为原点，OE 在当地水平面内指东，ON 在当地水平面内指北，O 沿当地地垂线方向并且指向天顶，与 OE、ON 组成右手坐标系11。即通常所说的 3 个坐标轴成东北天配置。 图 13 地理坐标系 图 14 地理坐标系和机体坐标系之间的方位关系机体坐标系和飞行器固联，其原点位于飞行器重心，x 轴位于飞行器参考平面内平行于机身轴线并指向飞行器前方，y 轴垂直于飞行器参考平面并指向飞行器右方与 x 轴垂直，z 轴垂直于 Oxy 平面并指向飞行器下方12。空间目标智能三维空间姿态解算系统设计与实现运动物体可以定义和设置类似于飞行器的属于它自身的机体坐标系。空间目标运动物体的姿态实际上就是地理坐标系和机体坐标系之间的方位关系如图 14 所示。空间目标运动物体 x 轴在水平面上的投影与地理子午线 N之间的夹角定义为航向角（Yaw） ，用 表示。目标物体绕横向水平轴转动时，目标物体 x 轴与纵向水平轴的夹角称为俯仰角（Pitch） ，用 表示。目标物体纵向对称平面与纵向铅垂平面之间的夹角称为横滚角（Roll） ，用 表示13。地理坐标系和机体坐标系之间的关系规定了姿态，解算出航向角，仰俯角和横滚角就需要借助于四元数与欧拉角之间的转换。（3）四元数与欧拉角之间的转换四元数的定义为：q=w+xi+yj+zk=+n （1）cos/2sin/2其中 为转动的角度，必须同时满足以下条件:|q|=w2+x2+y2+z2 =1 （2）i2=j2=k2=-1 （3）ij=-ji=k （4）jk=-kj=i （5）ki=-ik=j （6）将四元数对时间求导， （7）= 12sin2+sin2+ 12cos2其中 n2=-1，=0，= ，则有： （8）=12(cos2+ sin2)=12 （9） = + + =12( + + )(w + xi i+ yj j+ zk k)（10） = （11）120 0 0 0 智能三维空间姿态解算系统设计与实现记 ,则有: =0 0 0 0 （12）=12用一阶龙格库塔法算得 （13） + =+2 + + + + + + 欧拉角可以用四元数来表示：= （14）tan 12( + )1 2(2+ 2)sin 1(2( )tan 12( + )1 2(2+ 2)但由于反正切函数值域是（/2，/2） ，不能覆盖所求角度的范围。用以下公式来表示：= 2（2（ + ），1 2（2+ 2）sin 1(2( )2（2（ + ），1 2（2+ 2）（15）其中 atan2（y，x）是表示平面直角坐标系 Oxy 所对应的（x，y）坐标的角度，它的值域范围是（-，） 。atan2(y,x)是表示平面直角坐标系所对应的(x,y)坐标的角度，它的值域范围是（-，） 。有了这些公式的推导，就可用代码进行数据处理，其中有一些关于三角函数、反三角函数等比较复杂的解算，这些调用 C 语言的 matlab 函数库就可以实现14。5 系统调试前几节系统性地介绍了关于智能三维空间姿态解算系统的整体设计，这里主要介绍系统调试及结果分析。5.1 调试方法（1）硬件电路的调试，电路板设计的初期，硬件电路的调试是必不可少的，智能三维空间姿态解算系统设计与实现主要是关于电路原理设计的实质性验证，电路验证合格后，最终做出 PCB 板，增强硬件系统的稳定性15。（2）软件设计的调试，在这一部分主要借助串口上位机进行测试，串口上位机可以实时的显示当前传感器采集回来的数据，通过此方法可以方便地观察此时传感器采集回来的数据，并且通过上位机可以使各个传感器对应的数据绘制成一条曲