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

出处:tuwen 发布于:2010-08-23 22:04:42

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

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

  1 CORDIC算法简介

  CORDIC算法的基本原理来自二维坐标的旋转变换。当平面直角坐标系绕原点旋转一个角度θ时,新旧坐标间的变换关系为:



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

  CORDIC算法中的移位运算很适合于在FPGA中用硬件实现,不但计算速度快,而且相对于查表法明显节省了硬件资源。而在DSP中,由于缺少专门的硬件支持,CORDIC算法相对于幂级数展开法或查表法则缺乏优势[2-4]。对于经典的PIC、8051等单片机,因为每执行1条移位指令只能将数据左移或右移1位,而执行CORDIC算法时会在移位运算上消耗大量的时间,从而影响CORDIC算法的效率。

  2 罗盘载体姿态及其解算



  式(7)、式(8)中的Bxh与Byh即是地磁场在水平面内的2个正交分量,分别沿xh轴和yh轴方向。式(7)所描述的正是将载体坐标系中测得的地磁分量变换到地平坐标系的过程。

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

  CORDIC算法本身可以计算三角函数与反三角函数,因此可以直接将其用于上述算法中[6],但这样并没有完全发挥CORDIC算法的优势。如前所述,CORDIC算法的原理来自于坐标旋转变换,因此,可以直接利用CORDIC算法完成式⑺中的变换并由此获得载体的姿态,而无需计算三角与反三角函数。参考文献[5]所给出的第3种算法就是基于这一思想,其步骤为:

  (1)用矢量模式将重力加速度矢量在yOz平面内的投影旋转到与z轴重合,求得横滚角。

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

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

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

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

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

  3.1 用CORDIC算法计算三角函数

  表1给出了在VRS51L3074单片机上用CORDIC算法计算反正切函数的结果,并与Keil C51编译器(V8.12)所提供的浮点库函数进行了对比。VRS51L3074是美国Ramtron公司的产品,基于增强型8051内核,在兼容传统8051指令集的基础上提升了指令执行速度。同时,该单片机还拥有“增强型算术单元”的外围模块,该模块中有32位桶形移位器。表1也列出了在CORDIC算法中使用该移位器的结果。


  此处所用CORDIC算法的输入、输出均采用16位数据格式,角度以度为单位,小数点设在两字节之间,即角度分辨率为1/256度,迭代次数为14次(不含第1次45°的旋转),这是由角度的分辨率决定的(第15次及其后的迭代所转角度已小于1/256度)。为便于比较,所有计算结果均已换算为十进制格式(单位为度,保留4位小数)。表1还列出了由单片机的定时器T0所记录的函数执行时间,T0时钟为系统时钟(不分频)。因此,计时结果即为函数从调用到返回所经历的系统时钟周期数。

  从表1的结果可看出,CORDIC算法具有一定的速度优势,但其无法与浮点函数相比。另外,使用桶形移位器代替单片机的移位指令,可明显提升CORDIC算法的执行速度。这也证明了CORDIC算法在单片机上运行时,在移位操作上消耗了大量的时间。CORDIC算法的主要优点在于: (1)代码较为简洁;(2)可自定义输出角度单位及格式(浮点库函数所给出的角度单位总是弧度); (3)在计算y/x的反正切时不必计算除法,并且该算法在±90°时也是收敛的。

  在单片机上运行CORDIC算法还需注意,输入矢量的模长既不能过大也不能过小。如模长过大,则由于CORDIC算法在将矢量旋转的同时还会将其“拉长”(这也正是CORDIC算法需要模校正因子的原因),将会导致计算过程出现溢出。而若模长过小,则会导致降低。表2为输入矢量的模长对CORDIC算法的影响:在4个输入矢量的辐角均相等的情况下,第1个矢量由于模长过大导致CORDIC计算过程溢出,第2个矢量计算结果正常,第3和第4个矢量由于模长过小导致CORDIC计算降低。


  移位操作耗时过多以及计算受输入矢量模长影响这两个缺点,使得用CORDIC算法在单片机上计算三角函数的实用价值并不理想。

  3.2 CORDIC算法用于姿态解算

  如上所述,CORDIC算法并不适合于在单片机上直接计算三角函数。但将其用于罗盘载体的姿态解算仍然是可行的,这是基于以下两点:(1)由于传感器特性的不一致性,其采集的原始数据均需经过零点、增益等校准后才可用于姿态解算,这就相当于为CORDIC算法的输入矢量模长做了归一化处理;(2)如前所述,利用CORDIC算法计算俯仰角与横滚角并对其加以补偿时,并不需要具体计算三角与反三角函数,只需完成坐标变换即可。

  参考文献不仅给出了用CORDIC算法求解航向角的思路,并且在FPGA上进行了验证。将这一算法移植到单片机时,还可再稍作简化,具体计算步骤为:

  (1)将重力加速度矢量在yOz平面内的投影旋转到与z轴重合,同时地磁场矢量亦作同步的旋转,从而得到横滚角并同时对其加以补偿。

  (2)将重力加速度矢量(此时该矢量已在xOz平面内)旋转到与z轴重合,同时地磁场矢量亦作同步的旋转,从而得到俯仰角并同时对其加以补偿。

  (3)根据地磁场水平分量计算航向角。

  以上计算过程相当于把参考文献[5]所述算法步骤的第1、第3步和第2、第4步分别合并。与CORDIC算法的矢量模式和旋转模式在FPGA上的实现方式不同,在单片机上则不必拘泥于这两种模式的区分,从而可以使程序更加紧凑。

  表3给出了在VRS51L3074上使用增强算术单元实现CORDIC求解载体姿态的实验结果,并同时给出按式(5)~式(8)利用浮点运算所得结果进行对比。磁场矢量的模长已被限制到4 096(相当于十六进制的0x1000),重力加速度矢量的模长则限制为8 192(相当于十六进制的0x2000)。


  电子罗盘的俯仰角、横滚角和航向角要求达到0.1°的分辨率。由表3可见,CORDIC算法的计算能够满足这一要求,并且该算法完成1次姿态解算所需的时间仅相当于浮点运算所需时间的1/8~1/9,能很好地满足电子罗盘对姿态解算的实时性要求。

  CORDIC算法无需乘除运算和大规模的查找表即可实现三角函数、反三角函数以及其他超越函数的运算。尽管CORDIC算法通常被用于FPGA,实践结果证明,这一算法也可有效地在8051内核单片机上运行,并且具有优于C语言浮点库函数的运行速度和一定的。

  在单片机上,制约CORDIC算法效率的主要因素是移位操作耗时过多。若单片机拥有桶形移位器,如VRS51L3074单片机的增强型算术单元,则可改善移位操作耗时过多的问题,从而提升该算法的效率。此外,适当调节输入矢量的模长也是应用该算法时需要注意的问题。

  采用合理安排的计算步骤,CORDIC算法可以准确、高效地完成电子罗盘中的倾角补偿与航向计算,同时也为电子罗盘中的单片机节约了程序空间和时间。


  

参考文献:

[1]. VRS51L3074 datasheet https://www.dzsc.com/datasheet/VRS51L3074_1590108.html.


关键词:IC电子

版权与免责声明

凡本网注明“出处:维库电子市场网”的所有作品,版权均属于维库电子市场网,转载请必须注明维库电子市场网,https://www.dzsc.com,违反者本网将追究相关法律责任。

本网转载并注明自其它出处的作品,目的在于传递更多信息,并不代表本网赞同其观点或证实其内容的真实性,不承担此类作品侵权行为的直接责任及连带责任。其他媒体、网站或个人从本网转载时,必须保留本网注明的作品出处,并自负版权等法律责任。

如涉及作品内容、版权等问题,请在作品发表之日起一周内与本网联系,否则视为放弃相关权利。

相关技术资料
上传BOM文件: BOM文件
*公司名:
*联系人:
*手机号码:
QQ:
应用领域:

有效期:
OEM清单文件: OEM清单文件
*公司名:
*联系人:
*手机号码:
QQ:
有效期:

扫码下载APP,
一键连接广大的电子世界。

在线人工客服

买家服务:
卖家服务:

0571-85317607

客服在线时间周一至周五
9:00-17:30

关注官方微信号,
第一时间获取资讯。

建议反馈

联系人:

联系方式:

按住滑块,拖拽到最右边
>>
感谢您向阿库提出的宝贵意见,您的参与是维库提升服务的动力!意见一经采纳,将有感恩红包奉上哦!