CORDIC算法在三轴电子罗盘中的应用

CORDIC算法在三轴电子罗盘中的应用

CORDIC算法在三轴电子罗盘中的应用*

李翔，李智

(桂林电子科技大学电子工程学院,广西桂林541004)

摘要：CORDIC算法是用于计算三角、反三角、指数、对数等超越函数的简捷算法。将该算法应用在以单片机为核心的三轴电子罗盘中，用于实现罗盘的倾斜补偿并计算俯仰角、横滚角和航向角。实验表明，该算法可有效地在单片机上运行，能够较好地兼顾计算精度与效率，有实用价值。

关键词：CORDIC；电子罗盘；单片机

中图分类号：TP368.1

文献标识码：A

ApplicationofCORDICalgorithmin3-axisdigitalcompass

LIXiang，LIZhi

(SchoolofElectronicEngineering,GuilinUniversityofElectronicTechnology,Guilin541004,China)

Abstract:CORDICisanefficientalgorithmforcomputingtranscendentalfunctionsincludingtrigonometric,inversetrigonometric,exponentialandlogarithmicfunctions.Ithasbeenappliedtoa3-axisdigitalcompass,whichusesmicro-controllerasthecontrolunit,fortilt-compensationandthecalculationofpitch,rollandheading.Experimentsshowthatthealgorithmcanruneffectivelyonthemicro-controllerandkeepafairtrade-offbetweenaccuracyandefficiency,anditspracticalityisdemonstrated.

Keywords:CORDIC；digitalcompass；micro-controller

CORDIC算法(CoordinateRotationDigitalComputer)(即

“坐标旋转数字计算机),最早是由VOLDERJE.于1959年提出[1]，当时是为研制B-58轰炸机的导航系统而设计的。但该算法仅仅用到了移位和加减运算，因此其硬件实现非常简单。后经众多学者加以研究和发展，使其成为数字电路中计算各种超越函数的一种简捷有效的算法[2]。CORDIC算法在FPGA中应用甚广[2-3]，在DSP上也有所应用[4]，但鲜有在单片机上的应用报道。

解为一系列步骤：

-sinαixi1-tanαixi

=cosαi(3)

cosαiyitanαi1yii+1i

由于αi=arctan(2-i)，式(3)中最右边的变换矩阵只需

i+1

i

cosαx

=iiiysinα

iiiiiii

用移位及加减即可实现。而作为公因子提到矩阵之外的

cosαi，可以看作是对矢量(xi,yi)作变换后，再对其模长所

做的修正。因此通常称式(4)所示的K为CORDIC算法的“模校正因子”。

n

n

1CORDIC算法简介

CORDIC算法的基本原理来自二维坐标的旋转变换。

当平面直角坐标系绕原点旋转一个角度θ时，新旧坐标间的变换关系为：

K=仪cosαi=仪

i=0

i=0

1姨(4)

CORDIC算法有矢量和旋转两种模式。在矢量模式

下，输入矢量(x0+y0)被旋转到与x轴正半轴重合，从而得

到姨00和arctan(y0/x0)。在旋转模式下，单位矢量

cosθx

=iiiysinθ

11

-sinθx0

=cosθy0

iii

(1)

式中,正弦和余弦函数是超越函数。为了便于在数字电路中完成这一计算，将θ表示为一系列角度之和：

n

θ=Σδiαi

i=0

(2)

(1,0)被旋转到指定的辐角θ，从而得到cosθ和sinθ。

以上介绍的CORDIC算法属于最基本的“圆周型”CORDIC算法。对迭代步骤稍作改动，即可将其变形为“直线型”或“双曲型”CORDIC算法，从而可用于计算乘

除法、双曲函数、指数函数和对数函数[2]。

式中,αi=arctan(2-i)，δi∈{-1,1}，由此可以把坐标旋转分

*基金项目：广西研究生教育创新计划资助项目（2009105950804M32）

CORDIC算法中的移位运算很适合于在FPGA中用

硬件实现，不但计算速度快，而且相对于查表法明显节

144

欢迎网上投稿http://doc.guandang.net《电子技术应用》2010年第6期

CORDIC算法在三轴电子罗盘中的应用

正是将载体坐标系中测得的地磁分量变换到地平坐标系的过程。

参考文献[5]给出了3种求解罗盘载体姿态的算法，上文介绍的就是其中的第1种，而第2种算法原理与其完全相同,只是利用三角函数的性质略微简化了计算。应当指出,由于坐标系及角度定义上的差异，式(5)、式

省了硬件资源。而在DSP中，由于缺少专门的硬件支持，

CORDIC算法相对于幂级数展开法或查表法则缺乏优势[2-4]。对于经典的PIC、8051等单片机，因为每执行1条移位

指令只能将数据左移或右移1位，而执行CORDIC算法时会在移位运算上消耗大量的时间,从而影响CORDIC算法的效率。

2罗盘载体姿态及其解算

通常用俯仰、横滚和航向这3个角度来描述罗盘载体的姿态。这3个角度是由载体坐标系来定义的。

定义载体坐标系O-xyz为：x轴沿载体纵轴指向前方，z轴在载体对称平面内（载体为左右对称）垂直于x轴向上，y轴垂直于对称平面并与x轴、z轴构成右手坐标系;定义地平坐标系O-xhyhzh为：xh轴水平指向正北，yh轴水平指向正西，zh轴竖直向上。

首先将O-xhyhzh绕轴zh旋转，直至xh轴重合于x轴的水平投影，所转过的角度即为航向角H；其次再将此时的O-xhyhzh绕yh轴旋转，直至xh轴重合于x轴，所转过的角度即为俯仰角P；最后将此时的O-xhyhzh绕xh轴旋转，直至yh轴重合于y轴，所转过的角度即为横滚角R。

三轴电子罗盘使用磁阻传感器测得地磁场的3个正交分量为Bx、By和Bz，并用加速度传感器测得重力加速度的3个正交分量Ax、Ay和Az，则可利用这6个测量数据直接计算俯仰角P、横滚角R和航向角H：

(7)、式(8)与参考文献[5]中的公式略有不同，但并不影

响问题的本质。

CORDIC算法本身可以计算三角函数与反三角函

数，因此可以直接将其用于上述算法中[6]，但这样并没有完全发挥CORDIC算法的优势。如前所述，CORDIC算法的原理来自于坐标旋转变换,因此,可以直接利用

CORDIC算法完成式⑺中的变换并由此获得载体的姿

态，而无需计算三角与反三角函数。参考文献[5]所给出的第3种算法就是基于这一思想，其步骤为：

(1)用矢量模式将重力加速度矢量在yOz平面内的

投影旋转到与z轴重合，求得横滚角。

(2)再用矢量模式将重力加速度矢量（此时该矢量已在xOz平面内）旋转到与z轴重合，求得俯仰角。

(3)用旋转模式补偿横滚角，计算补偿后的地磁场分

量。

(4)再用旋转模式补偿俯仰角，计算补偿后的地磁场

分量。

A姨yzA

R=-arctanzP=arctan

hy

y

z

姨姨

hhy

hy

(5)(6)(7)

(5)用矢量模式，根据地磁场水平分量计算航向角。

3CORDIC算法在单片机上的实现

3.1用CORDIC算法计算三角函数

表1给出了在VRS51L3074单片机上用CORDIC算

法计算反正切函数的结果,并与KeilC51编译器（V8.12）所提供的浮点库函数进行了对比。VRS51L3074是美国Ramtron公司的产品，基于增强型8051内核，在兼容传统8051指令集的基础上提升了指令执行速度。

姨B=BcosR+BsinR

B

90°-arctan姨,B>0

BB

,B<0H=270°-arctan姨

姨

姨姨姨姨姨姨姨姨姨姨姨姨姨姨姨姨姨姨

Bxh=BxcosP+BysinPsinR-BzsinPcosR

hxy

hy

(8)同时，该单片机还拥有“增强型算术单元”的外围模块，该模块中有32位桶形移位器。表1也列出了在CORDIC算法中使用该移位器的结果 …… 此处隐藏：8167字，全部文档内容请下载后查看。喜欢就下载吧 ……