0 引言

随着无人机应用越来越广泛，无人机的姿态解算与控制也成为导航制导领域的研究热点。无人机姿态测量系统通过获取GPS、惯性测量单元（IMU）和磁传感器等量测数据，解算出载体俯仰角、滚转角、航向角以及速度位置等信息，用于飞行器飞行控制，并利用融合算法不断修正各传感器的误差^[1]。对于大型载人飞行器来说，由于机体空间和能源较为充裕，大功耗机载计算机的运算能力较强^[2]，可以有效及时地处理大量导航信息，配合人工操作进行安全完善地控制飞行器姿态。当前无人机为了保证续航能力以及满足小型化要求，其机载计算机的处理器功耗较低，运算能力有限^[3]。而无人机高动态的飞行特点，又要求其对导航信息解算的快速性、实时性。可见解决无人机载计算机低运算力与高数据更新率两者之间的矛盾，是无人机发展过程中的关键问题。

无人机机载计算机主要使用嵌入式片上系统（SoC）进行导航信息的获取及处理。为了提高嵌入式机载计算机的运算效率，当前国内外研究通常从两点着手：1）通过硬件工艺进行解决，如采用多核心的处理器进行多线程运算^[4]；采用先进微纳米工艺，在保持低功耗的同时加大CPU主频，提高运算器指令处理速度^[5]。2）通过优化算法进行解决，如采用优化的数据融合算法，使用卡尔曼滤波相关算法代替粒子滤波，使用递推最小二乘法代替遗传算法^[6]，等等。本文使用工程中应用广泛的Cortex-M4F内核处理器，充分利用内核集成的DSP指令集、浮点运算协处理器以及高速存储器，针对无人机姿态解算算法特点进行优化。优化后的无人机在保证低成本、小体积、低功耗的同时，提高了无人机的解算效率及数据更新率，能够满足无人机姿态解算精度与实时性要求。

1 信息处理要求及硬件设计 1.1 信息处理时效分析

根据无人机姿态解算原理，无人机测姿系统需要采集磁传感器、加速度计和陀螺数据，接收处理GPS信息。实际应用表明，无人机在高动态环境下的姿态信息处理速率在500 Hz以上，可以较好保证测姿的实时性和准确性^[7]。即每次需要在2 ms以内完成信息的采集、解算和通信。除解算以外，无人机姿态测量主要包括以下两个方面：

1）姿态信息输出和GPS信息采集。在GPS信息更新率为10 Hz、以500 Hz输出姿态角的情况下，使用硬件直接内存访问（direct memory access，DMA）技术辅助CPU进行串口信息存取^[8]，可以将串口占用的CPU时间控制在数十微秒内，主要用于相关配置以及使能对应总线时钟。

2）传感器信息采集。传感器数据通过I²C总线传输。由于硬件及I²C协议限制，传感器I²C总线传输速率最高约400 kB/s。I²C总线本质上是半双工传输总线，传输中需要进行握手、应答等操作^[9]，实际应用中传输速率约13 kB/s。在每个传感器信息为6 B/s的条件下，18 B/s数据传输时间约为1.4 ms。

所以，考虑到其他指令开销的情况下，应限制解算时间在500 μs以内，才能保证单次数据处理周期小于2 ms，即数据更新率达到500 Hz以上。

1.2 硬件设计

无人机姿态解算系统的硬件结构与组成如图 1所示，GPS接收模块用于接收卫星信号，获得位置和速度信息；磁传感器、加速度计和陀螺可以测量无人机姿态信息。GPS和传感器信息分别通过串口和I²C总线与MCU连接，之后通过串口发送姿态信息。

针对无人机姿态解算与信息处理的要求，以及机载系统小型化、低功耗的特点，采用Cortex-M系列内核的STM32F405作为信息处理的主控芯片，总线宽度为32位的STM32F405微处理器，工作频率最高168 MHz，指令处理速度达210DMIPS^[10]，高主频可以保证系统的整体速度。STM32F4系列为了提高数据运算处理速度，使用ARM公司的Cortex-M4F内核，集成FPU（浮点运算单元）和DSP运算指令集。STM32F405集成有196kB的SRAM、两个DMA控制器。高容量SRAM有利于大数据量解算。此外，SRAM包含有64 kB的CCM（core coupled memory），即内核耦合存储器，用来提高数据交换效率。此芯片具有体积小、质量轻、功耗低等特点，适合于无人机传感信息处理与姿态解算。

2 硬件解算加速原理 2.1 无人机姿态解算算法特点

以MEMS陀螺为主的姿态测量算法^[11]主要通过四元数算法进行角速度积分，通过传感器数据融合修正测量误差，根据迭代结果转换欧拉角输出姿态。完成初始角和四元数初始化后，单周期运算主要有：

1）换算传感器数据为物理量，利用传感器标定的数据进行测量信息补偿。误差补偿公式为

其中，陀螺、加速度计和磁传感器原始数据分别为ω_m、A_m、H_m；K₁、K₂、K₃分别陀螺、加速度计和磁传感器误差补偿矩阵；它们的零点分别为ω₀、A₀、H₀。可以看出这部分主要在于浮点数运算和矩阵的运算。

2）使用加速度计、磁传感器与陀螺进行融合，进行四元数修正。主要计算公式如下：

式中：A^b、H^b——加速度计、磁传感器换算到载体坐标系下的数据；

Aⁿ、Hⁿ——加速度计、磁传感器在导航坐标系下数据；

C_n^b——姿态转换矩阵。

主要包含浮点、三角函数和矩阵运算。

3）利用四元数微分方程进行陀螺数据迭代，求出更新后的四元数，核心公式为

式中：ω_nb^b——三轴陀螺载体坐标系下的角速度矩阵；

Q——归一化后的四元数矩阵；

t——采样时间。

这部分主要是矩阵运算和浮点运算。

4）根据旋转矩阵对应关系四元数提取欧拉角输出。γ、θ、φ分别为滚转角、俯仰角和航向角。

可以看到，上述解算中主要包括浮点运算、向量乘法、三角函数和矩阵运算。因此选择合适的数据运算方式，可以有效加速解算处理。

2.2 算法的传统数据处理方法

传统方式的姿态解算及数据处理流程如下：

1）存储变量至SRAM中。SRAM通过总线矩阵和CPU进行连接。其中，总线矩阵采用循环调度算法进行主控总线之间的访问仲裁管理。

2）处理三角函数时，依据泰勒级数法进行近似和查表运算，求出近似解^[12]。处理浮点数时，分别进行定点和浮点运算；若没有浮点运算单元则需要进行浮点数规格化^[13]，但是规格化操作后会由于浮点数的多次换算和圆整造成一定精度损失。

3）矩阵运算时矩阵乘法需要对矩阵分解后逐项相乘，假设两矩阵均为N×N的方阵，其运算时间复杂度会达到O（N³）。对于矩阵转置和求逆，常用Gauss-Jordan消元求逆^[14]运算，时间复杂度较高。

可以看出，传统方式运算效率较低，全程需要CPU进行参与，占用大量CPU时间，不适合在实时性和精度要求较高的无人机上使用。

2.3 使用DSP与FPU对数据运算的加速

为了加速浮点数运算，必须使用浮点运算处理器代替对CPU加法器进行的浮点数规格化运算；而DSP指令集的应用可以提高数字信号处理效率。

Cortex-M4F的DSP单元可以支持单周期的乘加指令（MAC）、优化的单指令多数据指令（SMID）以及饱和算术等数字信号处理指令集，Cortex-M4F内核执行的所有DSP指令都可以单周期完成。因此，在解算过程中，针对复杂矩阵、三角函数和向量乘法运算，可以通过DSP单元的正余弦运算指令代替传统的查表法，使用矩阵乘法指令和求逆指令代替传统矩阵运算的消元法和向量乘法运算，从而使原本复杂的运算在少数时钟周期内解决。

对于高精度浮点运算，由于CPU中的加法器只能处理整数类型运算，因此应用FPU进行浮点运算，不仅可以提高运算速度，还可以保证浮点运算精确性。处理浮点数时，利用FPU在Cortex-M4F内核中作为CPU运算的协处理器，相对按照IEEE-754标准算法完成浮点数运算^[15]的定点CPU，十分节省CPU资源。FPU单周期指令支持32位单精度浮点的硬件指令集，相对于定点运算效率高出数十倍^[16]。

所以，将DSP与FPU结合起来用于无人机姿态解算，可以大大缩短消耗时间，提高采样速率并保证解算精度。

2.4 使用CCM对数据处理的优化

为保证解算过程中姿态解算周期不受数据通信影响，不仅需要提高解算效率，而且要保证CPU对数据存取的实时性。

在Cortex-M4F内核中，SRAM包含有64 kB的CCM存储器。CCM存储器直接通过数据总线与CPU连接，且只能与CPU进行通信。内核的部分总线矩阵结构如图 2所示。

可以看到，根据总线矩阵的结构设计，当变量和数据存储于SRAM时，SRAM不仅与内核的数据总线、系统总线和指令总线相连接，而且与两个DMA控制器相连接。由于解算过程中数据的收发启用了DMA控制器，因此当总线矩阵依据循环调度算法分配总线带宽时，CPU最多能占用67%的数据总线带宽^[17]。而当使用CCM存储临时变量时，CCM存储器与CPU的数据传输不受总线矩阵干扰，从而保证存取数据实时性和采样周期稳定性。

3 程序设计及优化方法

系统程序编译采用ARM公司推出的开发环境MDK-Keil-V4。主程序实现过程可分为信号处理、数据解算和通信3个部分。进程采用中断的前后台系统，保证对数据接收中断信号的及时响应。

对于关键的算法实现部分，在将变量加入CCM后，利用FPU和DSP进行解算算法的硬件加速，与协同CPU进行传感器数据处理。硬件加速步骤如下：

1）根据Cortex-M4F存储地址的固定映射进行RAM地址修改。将RAM地址定义为CCM的地址（0×10 000 000），同时设置好Flash的缓存区，用于缓存多条指令以避免时间浪费。此时程序的临时变量会在CCM存储器中与CPU进行存取交换数据，其总线传输不受总线矩阵仲裁器的影响。

2）将浮点数解算精度定义为单精度浮点型，即32位浮点数，同时将所有用到的小数常量改为单精度浮点型。

3）开启FPU，置位CPACR协处理器控制寄存器的CP10和CP11。此时在编译后，将浮点数运算汇编指令从无FPU时的3条以上指令（MOVS、BL、POP）替换成为单周期的VADD加法指令、VMUL乘法指令。

4）加入DSP函数运算指令。加入DSP库函数，利用其单周期多指令运算的高速性，使用封装DSP指令的arm_sin_32f、arm_cos_32f函数进行正余弦三角函数运算处理；使用arm_matrix_inverse_32f、arm_sqrt_32f进行矩阵的求逆及四元数归一化，以及使用arm_matrix_mult_32f函数进行矩阵的转置运算。

4 实验结果及分析 4.1 加速性能测试

为验证解算加速效果，设计了如下实验方案：在进行传感信息采集、通信的同时，将无人机姿态解算函数的进入和返回部分加入GPIO口电平控制，在每个解算周期开始时拉低指定IO口的电平，在解算结束后再将电平升高。根据此方法可以直接通过示波器查看输出波形，了解单次数据处理周期的变化情况。硬件加速测试步骤如下：

1）不进行算法加速优化，进行数据处理时长测试。

2）开启FPU浮点运算单元，将解算时参与的浮点数都定义为单精度浮点类型。测试数据更新周期以及算法运算时间，验证FPU加速效果。

3）只进行DSP解算加速优化，加入DSP运算指令，测试DSP解算加速效果。

4）分别开启FPU和DSP，使用CCM存储器优化数据存取，测试优化效果与周期稳定性。

5）将FPU、DSP开启，同时使用CCM存储器。测试解算加速效果，测试结果见表 1。

可以看出，在没有对解算进行加速时，姿态角解算、数据处理、通信速度只有358.42 Hz，无法满足数据更新率要求。使用DSP和FPU对解算进行加速之后，分别可以将数据更新率提高至403.22 Hz和438.59 Hz。此基础上使用CCM存储器，减少解算时间约40~60 μs。使用完整的硬件加速解算后，达到了500 Hz的数据更新率，并确保了一定的冗余量。

4.2 机载采样率测试

对无人机机载测试系统进行改进后，将系统安装并进行实际测试。测试系统使用DSP和FPU加速姿态角解算，并使用CCM存储变量数据。通过定时器中断以500 Hz累加帧计数（16位），用来判断加速后是否丢帧。测试时，系统每2 ms进行一次数据采集和解算，并将解算数据与帧计数打包后发送至地面接收机。无人机安装与测试情况如图 3所示。

飞行测试解算的飞行姿态如图 4所示，分别为飞行过程中，无人机解算后的滚转角、俯仰角、航向角。图 5为无人机接收的GPS信息，分别为东西方向和南北方向的位移，以及相对地面的高度。

由图 4、图 5测量数据看到，无人机在起飞后进行了数圈盘旋飞行。在飞行过程中，机载测姿系统解算的俯仰角和滚转角短时间内变化幅值较大，说明其进行了高动态飞行机动，总飞行时间约203 s。此条件下采用500 Hz采样率进行数据更新，解算的航向角变化规律与GPS位移规律一致，没有明显的数据丢失，说明较高采样率能够保证高动态机动时测量解算失准角较小。测姿系统的帧计数情况如图 6所示，分别为帧计数变化情况、每个帧计数之间差值。

作为每个解算周期的时间标记，测姿系统的帧计数长度为2字节，其表示的范围为0~65 535。根据图 6可看出，测姿系统帧计数于327 s处达到最大值（65 535）后归零，其余时刻帧计数与上一时刻帧计数差值均为1。图 6的帧计数统计情况说明，系统的单个数据处理及解算周期在2 ms以内，并可以保证一定的冗余度。因此无人机机载解算的硬件加速效果较为明显，机载系统保持500 Hz采样率条件下并没有出现丢帧现象，同时解算精度没有明显降低。

5 结束语

无人机的姿态解算速度是提高采样率的瓶颈之一。实验表明，结合Cortex-M4F内核的浮点运算单元以及DSP处理特定数据运算周期短的优势，并使用内核中的CCM存储器代替SRAM，可以有效提高无人机姿态解算速度，并保证CPU数据存取的带宽。经过加速优化之后，算法的单周期解算时长减少至原时长的63.4%，数据更新提高到500 Hz以上，能够满足无人机姿态解算的精度和实时性要求。