CN103743400A - 一种磁航向解算装置 - Google Patents

Abstract

本发明涉及的磁航向解算装置是一种应用于磁航向***中的装置，其主要利用地球磁场测量方向。为解决目前磁航向***采用模拟电路进行信号处理时受到环境影响，稳定性差、***误差大等问题，本发明中的磁航向解算装置采用高精度AD单片机进行信号采样处理，进行航向解算，其中的正余弦和反正切函数均利用CORDIC算法实现，因此能够有效降低外部环境对电路在温度等其他因素的影响，同时，采用本发明中所使用的航向解算函数算法，能够减少磁航向***误差，提高磁航向***的总体精度。

Description

一种磁航向解算装置

技术领域

本发明涉及一种磁航向***中的磁航向解算装置，具体涉及一种基于CORDIC算法的航向解算装置。 

背景技术

磁航向***又称磁罗盘，是一种利用地球磁场测量方向的装置。以在无人机上使用为例，磁航向***的输出是无人机的机头指向。上世纪60年代以来，作为打靶训练靶标的无人机得到重视，远读磁罗盘由于体积、重量、成本等的优势在无人机航向测量中得到了应用，后与垂直陀螺结合，出现了平台式磁航向***，该***的缺点是结构复杂，体积大，造价高。单片机和微***技术的飞速发展，特别是单片机的应用，使磁航向测量进入了新的发展时期，复杂的机械结构和随动***被单片机的强大功能所取代，复杂的计算也不再是难题，即所谓的捷联数字式磁航向***。该***采用三轴磁传感器测量地磁场在飞机纵轴、横轴和竖轴的分量，从垂直陀螺获取俯仰角和倾斜角信号，用单片机直接解算出磁航向。磁航向***首次摆脱了复杂的机械结构，可靠性、测量精度大幅度提高，生产成本大幅下降。 

磁航向***用于飞机上时，磁航向定义为飞机纵轴方向在水平面上的投影与当地磁子午线(磁北)的夹角，用0°到360°的角度值表示，飞机向正东飞时，其值为90°。 

目前，捷联数字式磁航向***已成为较理想的磁航向测量装置，原理图如图6所示。***由三轴磁传感器、模拟电路、A/D转换电路和单片机组成。三轴磁传感器测出地磁场在飞机纵轴、竖轴和横轴上的分量H_x、H_y和H_z，经模拟电路处理连同飞机的俯仰角θ和倾斜角γ经A/D转换后送入单片机。在无误差的情况下，H_x、H_y、H_z以及θ、γ和航向角Φ的关系为： 

$\left[\begin{array}{c}{H}_x\\ H_y\\ H_z\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\γ}& -\sin \mathrm{\γ}\\ 0& \sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\θ}& -\sin \mathrm{\θ}& 0\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{H}_0\cos \mathrm{\Φ}\\ H_0\tan \mathrm{\α}\\ -H_0\sin \mathrm{\Φ}\end{array}\right]---(1-1)$

其中H₀为地磁场水平分量，α为磁倾角。航向角根据

求得。 

但是随着无人机技术的不断发展，对磁场测量在形态、稳定性和准确度等方面都提出了更苛刻的要求，传统磁航向***输出信号的温度稳定性差，对磁航向解算的公式简单，***误差大，准确度难以达到较高要求。 

发明的内容 

为了解决上述问题，本发明提供一种用于磁航向***中的磁航向解算装置，利用一个单片机采集数字磁通门输出信号并进行航向解算，其中的正余弦和反正切函数均利用CORDIC算法实现。使用本发明中的磁航向解算装置尽可能多使用单片机中的程序完成原有的模拟电路功能，能够有效降低外部环境对电路在温度等其他因素的影响，同时，采用本发明中所使用的航向解算函数算法，能够减少磁航向***误差，提高磁航向***的总体精度。 

本申请发明一种磁航向解算装置，该装置包括外部辅助电路、外部信号调理电路、磁航向解算运算模块，其特征在于： 

该外部的辅助电路提供俯仰角和倾斜角信号； 

该外部信号调理电路进行信号调理； 

该磁航向解算运算模块选用内部带高精度AD的高性能单片机，对从磁航向***中获得的信号进行采样，然后进行模数转换，对信号进行滤波处理、误差补偿，航向解算等数字处理，最终得到稳定的磁航向信号，并与上位机实时通信。 

本发明的磁航向解算运算模块，其特征还在于：该磁航向解算运算模块由初始化模块、***监控模块、AD校准模块、信号采集模块，航向解算功能模块，同步调试模块、误差补偿功能模块以及通信功能模块组成。 

本发明中的航向解算功能模块，其特征还在于：该航向解算功能模块包括数字滤波，误差补偿，三角函数计算，姿态变换，航向解算步骤。 

附图说明

图1、本发明涉及的磁航向解算装置构成图 

图2、本发明涉及的外部辅助电路图 

图3、本发明涉及的外部信号调理电路图 

图4、本发明涉及的磁航向解算运算模块构成示意图 

图5、本发明涉及的信号采集模块流程图 

图6、捷联数字式航向测量***原理图 

图7、本发明涉及的航向解算程序流程图 

具体实施方式

本发明涉及的磁航向解算装置包括外部辅助电路、外部信号调理电路、磁航向解算运算模块三个部分。图1为磁航向解算装置的工作原理框图（包括磁航向***和通信***）。 

外部的辅助电路提供俯仰角和倾斜角信号，外部信号调理电路进行信号调理。磁航向系 统对俯仰角、倾斜角和磁通门传感器信号进行采样，由磁航向解算运算模块获得姿态信号和地磁场分量，然后进行模数转换，对信号进行滤波处理、误差补偿，航向解算等数字处理，最终得到稳定的磁航向信号，并与上位机实时通信。 

1、外部辅助电路 

外部的辅助电路为磁航向解算装置提供俯仰角和倾斜角信号。 

2、外部信号调理电路 

俯仰角、倾斜角信号为0^-+5V的电压信号，而ADC的的输入范围为0^-+2.5V，因此需要对 

该角度信号进行预处理。实验规定，调理电路需要满足的条件是： 

(1)输入0^-+5V，对应输出为+0.25V^-+2.25V； 

(2)当输入悬空时，输出为0度信号，即1.25V； 

电路如图3所示，主要完成信号放大和电位平移功能。 

3、磁航向解算运算模块 

本发明中磁航向解算运算模块选用内部带高精度AD的高性能单片机，主要完成采样各分量信号，并对其进行滤波处理，然后进行航向解算，并和上位机进行通信，传输航向信号和接收上位机功能。 

如图4所，磁航向解算运算模块包括：初始化模块、***监控模块、AD校准模块、信号采集模块，航向解算功能模块，同步调试模块、误差补偿功能模块以及通信功能模块。其中，初始化模块代码可用软件简单设置自动生成；***监控子程序模块通过状态标志位调用相应的子程序；AD校准模块由软件启动可自动完成。这些子程序模块流程简单，属于现有技术。 

磁航向解算运算模块的单片机处理器ADC共有五路输入信号，其中三路磁通门信号差分输入，两路姿态角信号单端输入，ADC工作在连续转换方式，使用SIN3滤波器输出。利用ADC中断存储采样数据，由定时器中断控制切换输入通道和输入方式，每个通道多次采样，以在后面程序中做滤波处理，采样数据循环存储在片内XRAM上，信号采集模块流程如图5所示。 

（一）磁航向解算功能模块步骤 

磁航向解算功能模块按照计算步骤包括数字滤波，误差补偿，三角函数计算，姿态变换，航向解算步骤。该程序使用多种算法，计算量大，因此选择合理的算法、进行模块划分和软件优化非常重要。 

在单片机中，数据x＝2^N，N表示数据有效位长度。 

$\mathrm{\θ}=\arctan \left(x\right)=\arctan \left(2^N\right)=>{\mathrm{\θ}}^{\text{'}}=\frac{1}{1+2^N}---(3-3)$

其中θ为航向角。由式(3-3)根据***要求精度选择合适的数据有效位，本发明中涉及的操作数均用双字节表示。 

下面对各功能模块详细介绍。 

(1)数字滤波 

***采样的16位AD，分辨率高，也极易受外界干扰的影响，因此不仅靠硬件的设计减小干扰，软件的处理也很有效果。AD在每个通道采样多个数据，为滤波处理提供基础。单片机通常采样的数据滤波方法有：中位值滤波，加权平均滤波，移动平均滤波，低通滤波，复合滤波等等。本发明对比了中位值（方法1）、移动平均（方法2）和去极值平均（方法3）等三种滤波方法，误差值为在不同的输入情况下得到的，对比结果见表3-1。 

表3-1三种滤波方法误差对比(单位：10^-4V) 

可以看出，采用移动平均滤波的方式效果最好。前三个通道采用的是补码存储，后面两个通道是无符号存储，因此算法设计也要加以区分。 

(2)误差补偿和越限处理 

数字磁通门输出的三路信号零位、灵敏度等不一致，且又经过本***的放大，采样等环节，采样到的输入信号存在误差，因此应首先对其进行误差补偿。根据误差分析知，磁通门输出三轴信号分量的误差补偿关系式为： 

$\left[\begin{array}{c}{X}_1\\ Y_1\\ Z_1\end{array}\right]=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]\begin{array}{}\end{array}\left[\begin{array}{c}{X_1}^{\text{'}}+d_1\\ {Y_1}^{\text{'}}+d_2\\ {Z_1}^{\text{'}}+d_3\end{array}\right]---(3-4)$

姿态角信号由外部基准电压提供，本发明设提供的姿态角无误差，误差只存在本***的调理和采样环节，因此补偿计算相对简单，关系式为： 

$\left[\begin{array}{c}{U}_T\\ U_G\end{array}\right]=\left[\begin{array}{cc}{a}_1& 0\\ 0& a_2\end{array}\right]\left[\begin{array}{c}{U}_T^{\text{'}}+b_1\\ U_G^{\text{'}}+b_2\end{array}\right]---(3-5)$

补偿矩阵系数预先存储在Flash中，单片机上电后，加载到片内RAM中。输入信号经补 偿计算后得到无误差的分量信号。 

但实际操作中，在输入信号在最大值处时，由于干扰的存在，可能会超过规定的范围，因此应该进行越限处理。由于此情况只出现在输入在极值时，因此直接进行限幅即可，而不会影响***计算。对三个分量信号分别做限幅处理后，输出完全达到后续程序要求。 

(3)三角函数解算 

在姿态变换计算中，要使用姿态角信号的正余弦函数值，因此需要解算姿态角三角函数。 

三角函数的计算方法有查表法]、多项式近似法、有理数近似，CORDIC（Coordinate Rotation Digital Computer）算法等。其中查表法速度快，算法简单易实现，但占用存储空间太大，尤其是在要求精度高时；多项式近似法可达到较高的精度，但一般逼近多项式阶数高，因此计算量大，且耗占大量的硬件资源，计算速度受到限制.；有理数近似相比多项式近似法更容易达到较高的精度，但分块操作占用大量的计算时间；CORDIC算法即坐标旋转数字计算方法，很适合用于三角函数、双曲线、指数、对数的计算，它通过基本的加和移位运算代替乘法运算，计算速度快。因此本发明选择CORDIC算法解算。 

在实际应用中，又提出一些改进的CORDIC算法，其中MVR-CORDIC（Modified Vector Rotational CORDIC）执行速度快、执行效率高、算法复杂度小，本发明选择MVR-CORDIC算法实现。MVR-CORDIC算法作为一种用于矢量旋转计算的迭代算法，它的基本思想是把预旋转的某一特定角度分解，用一组预先规定的基本角度集的线性组合表示，即进行多次基本角度的旋转。表达式为： 

$\mathrm{\θ}=\underset{i=0}{\overset{R_m-1}{\mathrm{\Σ}}}\mathrm{\μ}\left(i\right)*\mathrm{\α}\left(S\left(i\right)\right)+\mathrm{\ξ}---(3-6)$

其中μ(i)∈{-1,0,1}，表示第i次旋转的方向；S(i)∈{0,1,2,…N-1}；α[S(i)]＝arctan(2^-S(i))，指第i次微旋转时S(i)对应的一个基本角度元素；ξ表示迭代后的角度残余误差。一般情况下，迭代次数R_m＜＜W。 

基本角度集的元素数量由数据有效位长度确定。关系如下： 

$\mathrm{\α}\left(m\right)=\arctan \left(2^{-m}\right)\≤\frac{1}{2^N}---(3-7)$

其中m为基本角度集的最少元素数量，一般基本角度元素以列表的形式预先存储在单片机中。 

CORDIC算法有旋转模式和向量模式两种计算模式，其中旋转模式用于计算输入角θ的正弦、余弦和正切值；向量模式用于计算给定向量的长度和角度。迭代方程如下： 

$\left\{\begin{array}{c}X(i+1)=X\left(i\right)-\mathrm{\μ}\left(i\right)*Y\left(i\right)*2^{-s\left(i\right)}\\ Y(i+1)=Y\left(i\right)-\mathrm{\μ}\left(i\right)*X\left(i\right)*2^{-s\left(i\right)}\\ Z(i+1)=Z\left(i\right)-\mathrm{\μ}\left(i\right)*\mathrm{\α}\left[S\left(i\right)\right]\end{array}\right.---(3-8)$

在旋转模式中，μ(i)为符号函数，值由Z(i)(后用Z_i表示)的符号确定。首先初始化X(0)＝1,Y(0)＝0,Z(0)＝θ。迭代开始，选取合适的S(i)查找α[S(i)]使|Z(i+1)|最小，把此时的μ(i)、S(i)代入迭代方程中的式1、2中计算出X(i+1)和Y(i+1)，至此完成一次迭代过程。经过R_m次迭代后，方程变为： 

$\left\{\begin{array}{c}X(i+1)=P\cos \left(Z_0\right)\\ Y(i+1)=P\sin \left(Z_0\right)\\ Z(i+1)=0\end{array}\right.,$ 其中 $P=\left(\underset{i=0}{\overset{R_m-1}{\mathrm{\∏}}}\sqrt{1+2^{-2S\left(i\right)}}\right)---(3-9)$

上式中，由于比例因子P的存在和不确定，因此需加上比例因子的计算和矢量补偿步骤，即令K＝1/P， 

$\left\{\begin{array}{c}{X(i+1)}^{\text{'}}=K*X(i+1)\\ {Y(i+1)}^{\text{'}}=K*Y(i+1)\end{array}\right.---(3-10)$

则迭代结果最终为： 

$\left\{\begin{array}{c}X(i+1)=\cos \left(Z_0\right)\\ Y(i+1)=\sin \left(Z_0\right)\\ Z(i+1)=0\end{array}\right.---(3-11)$

可以看出，MVR-CORDIC迭代可同时求出角度θ的正弦和余弦值，且精度很容易满足要求。 

程序中，算法的实现流程为： 

a)角度预处理：将任意输入角度θ转化为[0°,45°)内的值θ'，并为转换过程编码； 

b)CORDIC迭代：通过迭代求出θ'对应公式(3-9)的函数值； 

c)比例因子补偿：结算出θ'的最终三角函数值； 

d)函数值处理：根据编码，将θ'的函数值转化为θ的函数值。 

两次利用该算法，分别求出俯仰角和倾斜角的三角函数值，并存储在单片机片内RAM中，用于姿态变换。 

(4)姿态变换 

***姿态是任意的，航向解算时，需要把测得的地磁场的信号分量折算到俯仰角和倾斜 角信号都为零时的输出状态值，即进行姿态变换，计算关系式为^[1]： 

$\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\γ}& \sin \mathrm{\γ}\\ 0& -\sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{X}_1\\ Y_1\\ Z_1\end{array}\right]---(3-12)$

其中θ,γ分别为俯仰角和倾斜角值，X₁,Y₁,Z₁为输入的信号分量，X,Y,Z为折算到水平状态后的信号分量值。 

程序中，这些数据都是实时变化的，因此存储在单片机片内RAM中，但由于存储容量有限，子程序把关系式分为两步计算，即： 

$\left\{\begin{array}{c}{X}^{\text{'}}=X_1\\ \left[\begin{array}{c}{Y}^{\text{'}}\\ Z^{\text{'}}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\γ}& \sin \mathrm{\γ}\\ -\sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{Y}_1\\ Z_1\end{array}\right]\end{array}\right.---(3-13)$

和 

$\left\{\begin{array}{c}\left[\begin{array}{c}X\\ Y\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{X}^{\text{'}}\\ Y^{\text{'}}\end{array}\right]\\ Z=Z^{\text{'}}\end{array}\right.---(3-14)$

由此，把原来需要存储18个系数，变为8个系数，大大减少占用空间，计算量也相应减少。计算后，得到飞机平飞时地磁场在三轴方向上的信号分量用于航向解算。 

(5)航向解算 

航向解算公式为： 

$\mathrm{\Φ}=\arctan (-\frac{Z}{X})---(3-15)$

其中X,Z分别为θ＝0,γ＝0时地磁场在飞机纵轴X和横轴Z上的分量。 

可以看出，航向解算即进行反正切计算。本发明利用MVR-CORDIC算法的向量模式实现，迭代方程见式(3-8)。 

向量模式中，即μ(i)为符号函数，值由Y(i)的符号确定，X(0),Y(0)(后用X₀,Y₀)为两给定向量的两个分量值，Z(0)＝0。迭代开始，选取合适的S(i)使Y(i+1)最小，把此时的μ(i)、S(i)代入迭代方程的式1中求相应的X(i+1)，由μ(i)和S(i)查表得到的α[S(i)]求Z(i+1)，至此完成一次迭代过程。经过R_m次迭代后，输出结果为： 

$\left\{\begin{array}{c}X(i+1)=P\sqrt{X_0^2+Y_0^2}\\ Y(i+1)=0\\ Z(i+1)=\arctan (Y_0/X_0)\end{array}\right.,$ 其中 $P=\underset{i=0}{\overset{R_m-1}{\mathrm{\∏}}}\sqrt{1+2^{-2s\left(i\right)}}---(3-16)$

同样需要加上比例因子的计算和矢量模的补偿步骤，即令K＝1/P， 

X(i+1)'＝K*X(i+1) 

迭代结果最终为： 

$\left\{\begin{array}{c}X(i+1)=\sqrt{X_0^2+Y_0^2}\\ Y(i+1)=0\\ Z(i+1)=\arctan (Y_0/X_0)\end{array}\right.---(3-17)$

可以看出，经过MVR-CORDIC迭代，不仅求出了反正切值，还附带求出了矢量的模，精度满足要求。 

程序实现航向解算的流程图如图7所示，迭代求出的仅仅是与角度值的对应的数据，还需要进一步转化为实际的角度值，以便输出到上位机中。***设定角度的分辨率为0.1°。 

（二）同步调试模块 

在将***设置为同步调试模式后，进入同步模式，同时发送同步命令，然后进入等待返回状态，接收***返回的同步调试结果，并把结果传给上位机，调试结束，退出调试模式。 

单片机间通过SMBus总线通信，单片机与上位机则通过串口通信，均可直接调用相应通信子程序。 

（三）误差补偿子程序 

在航向解算时，需要误差补偿矩阵补偿传感器信号误差。误差补偿子程序在接收到上位机的补偿命令后，进入误差补偿程序，初始化误差系数矩阵C为单位阵，矩阵D为零矩阵。此时***输出航向角和各传感器信号分量，上位机获取足够的信息，计算出系数矩阵并发送，***接收、更新系数矩阵，并向上位机返回接收状态，退出补偿程序。 

（四）串行通信子程序 

(1)SMBus总线通信子程序 

子程序主要用于***调试时本***与传感器***的通信。向传感器***发送调试命令，等待接收调试返回状态，因此只定义主发送器和从接收器方式。定义标志位区分发送的数据类型，设定完成状态标志位，对接收的返回状态进行处理。协议与传感器***SMBus通信协议相同。 

(2)串口通信子程序 

***把上位机作为主机，单片机为从机，利用串口进行通信。规定串口分层通信协议为： 

a)异步串行通信协议：每个数据字节共10位：一个起始位、八个数据位和一个停止位； 

b)中间层协议：在硬件连接后，接收握手信号，并返回相应的应答信号；提供超时控制，用于重复发送命令；一个完整的帧包括：帧首、帧类型、长度、数据、错误冗余，其中帧首和帧类型使用同一字节；冗余信息由求和的方式得到，即前面传输的字节与校验字节的和为零，错误数据直接丢掉；提供接收数据缓冲机制，允许在执行前一个命令未完成的情况下接收第二个命令。因为工作中上位机一直发送相同命令，因此不需要顺序控制。该***也不用进行流量控制和链路控制。 

c)应用层：接收外部命令，然后根据当前单片机状态直接执行或者等待执行；接收外部 

数据，缓存至相应的数据单元；提供相应的执行状态标志位。 

磁航向串口通信程序包括： 

a)发送程序：向上位机发送数据和返回状态信息； 

b)接收程序：接收并缓存上位机的命令和数据； 

c)数据校验：对接收到的数据和命令进行校验，检验接收数据是否正确； 

d)命令解释和执行：把接收的外部命令转化为单片机内部相应的命令，并执行命令； 

通信程序利用中断方式执行。在***工作时，接收命令，发送磁航向角数据；在***调试时，接收外部发来的数据，并存储数据；接收外部发来的命令，解释和执行命令；输出相应的航向信息、***内部数据以及状态信息。 

采用上述模块构成的磁航向解算装置，由于在该磁航向解算装置中使用了高精度AD单片机，能够有效提高磁航向的计算速度，并减少外部环境对磁航向***的影响；同时，采用本发明中磁航向解算算法，能够明显减少误差，提高磁航向的精度。 

Claims (3)

1.一种磁航向解算装置，该装置包括外部辅助电路、外部信号调理电路、磁航向解算运算模块，其特征在于：

该外部的辅助电路提供俯仰角和倾斜角信号；

该外部信号调理电路进行信号调理；

该磁航向解算运算模块选用内部带高精度AD的高性能单片机，对从磁航向***中获得的信号进行采样，然后进行模数转换，对信号进行滤波处理、误差补偿，航向解算等数字处理，最终得到稳定的磁航向信号，并与上位机实时通信。

2.如权利要求1的磁航向解算运算模块，其特征还在于：该磁航向解算运算模块由初始化模块、***监控模块、AD校准模块、信号采集模块，航向解算功能模块，同步调试模块、误差补偿功能模块以及通信功能模块组成。

3.如权利要求2所述的航向解算功能模块，其特征还在于：该航向解算功能模块包括数字滤波，误差补偿，三角函数计算，姿态变换，航向解算步骤。

Images