专利名称：基于浮点型dsp的运动载体姿态倾角测量方法及装置的制作方法
技术领域：
本发明涉及载体姿态倾角测量技术，特别是一种基于浮点型DSP的运动载体姿态倾角测量方法及装置。
背景技术：
姿态倾角测量技术运用在许多领域中，如机器人、智能小车、航模控制、游戏设备、 个人导航设备、图像稳定系统等。静态的姿态倾角地测量较为容易且测量效果较为满意，但动态的姿态倾角地测量，尤其是被测设备动态效果较为强烈的情况下的姿态倾角测量，其测量技术和测量效果则显得差强人意。目前，对运动载体的姿态倾角测量装置主要使用高精度陀螺仪包括液浮陀螺仪、 光纤陀螺仪或激光陀螺仪等，这些陀螺仪的精度很高，但结构复杂，使用寿命短，成本高，一般仅限于导航方面，而难以在一般的运动控制系统中应用。MEMS (微机电系统)在融合多种微细加工技术，并应用现代信息技术的最新成果的基础上发展起来的高科技前沿学科，目前MEMS市场的主导产品为压力传感器、加速度计、 微陀螺仪等。满意的测量技术尤其是动态测量技术限定了传感器要满足实时性好、精度高、易于集成、成本功耗低、体积小等特点。近几年，基于MEMS技术的加速度计或陀螺仪被用来测量载体的姿态倾角，具有成本低、体积小、集成高、精度较高，但单一使用某种类型的传感器测量载体在运动情况下的动态姿态倾角时便会呈现很大的不足，主要原因如下
单一使用加速度计(一个或多个加速度计传感器)测量载体动态姿态倾角，其原理是通过测量载体的线性加速度，利用数学关系推出载体姿态角度值。加速度计是通过重力加速度在各轴上的分量值来推出载体倾角值，测量的角度值长期较稳定可靠，但是加速度计动态响应慢，敏捷性不够，且由于传感器噪声会随带宽的增大而增加，在跟踪动态效果较强烈的动态姿态倾角时得到的数据将会大大增加出错的机会，其测量的范围将会得到很大的限制，这使得使用单一加速度计检测运动载体的动态姿态倾角并不合适；
单一使用陀螺仪(一个或多个陀螺仪传感器)测量运动载体的角速度，通过不断积分得到载体姿态倾角。陀螺仪在跟踪动态环境下的载体姿态角，其敏捷性很高、动态响应较快， 但自身的先天性机械噪声无可避免地会产生测量误差，并且随着时间的推移，累计误差会越来越大，最终造成测量偏差。单独使用陀螺仪传感器测量运动载体的动态倾角，其结果也很难让人满意。另外从处理器硬件平台选择上考虑，一般的单片机或定点型处理器，在大量浮点运算和正余弦运算时程序执行效率低下，对复杂算法执行周期较长，不能满足对强烈动态环境下运动载体姿态角的实时跟踪，同时动态表示的精确度和范围也较低。

发明内容
本发明的目的在于提供一种对动态姿态倾角跟踪效果好、实时性高、精确度高、表示范围大、数据处理效率高的基于浮点型DSP的运动载体姿态倾角测量方法及其装置。实现本发明目的的技术解决方案为
一种基于浮点型DSP的运动载体姿态倾角测量方法，包括以下步骤
步骤一将陀螺仪及其处理电路和加速度计及其处理电路构成的惯性测量单元IMU安装在被测运动载体上，用于测量反馈运动载体的姿态角度和角速度信息；
步骤二 32位浮点型DSP通过IIC总线将IMU采集的数据进行实时分析处理后得到三种动态角度值，即陀螺仪测量值、加速度计测量值、经DSP滤波处理后的动态值，之后DSP通过SCI把数据发送出去；
步骤三以虚拟示波器方式对步骤二中的三种数据波形进行实时记录、保存和显示。一种使用基于浮点型DSP的运动载体姿态倾角测量方法的装置，包括惯性测量单元MU、电源电路、浮点型DSP电路、SCI转USB通信电路、上位机，电源电路分别与MU模块、 浮点型DSP电路、SCI转USB通信电路进行连接，IMU模块通过IIC总线与浮点型DSP电路进行连接，浮点型DSP电路通过SCI接口与SCI转USB电路进行连接，SCI转USB通信电路与上位机进行连接；其中惯性测量单元MU包括陀螺仪及其处理电路和加速度计及其处理电路电路，陀螺仪及其处理电路包括依次连接的陀螺仪、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路，加速度计及其处理电路包括依次连接的加速度计、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路。本发明与现有技术相比，其显著优点
1、本发明是不仅可以对载体静态姿态角进行准确测量，也可以对运动强烈环境下的载体动态姿态角进行精确且跟踪效果好的动态测量；
2、本发明采用陀螺仪和加速度计结合的方式检测运动载体的姿态角，将两种不同类型的传感器进行性能上互补，充分利用DSP强大的数据处理优势，通过卡尔曼滤波消除单一传感器的随机噪声和漂移误差，有效地避免了错误数据的输出，获得动态跟踪效果好、实时性高且更接近实际真值的姿态角；
3、本发明使用的数据信号处理器为采用32位浮点型DSP，在对大量的浮点运算和正余弦运算时具有很大的优势，对复杂算法程序执行效率高，得到的动态姿态倾角精度高；从节约成本、产品开发、数据处理效率和精确度等方面考虑，开发人员可在此基础上进行多功能扩展和二次开发，如用于高精度伺服控制上；
4、本发明的上位机软件采用NI(美国国家仪器)的LabVIEW软件进行编写，底层驱动使用VISA (虚拟仪器软件架构)进行编写，该软件界面友好，可移植性高，以虚拟示波器非常直观地将数据实时、动态地进行显示，并具有波形数据本地保存和回放的功能，在实际使用时非常直观、方便，代替了体积笨重、携带不便且成本昂贵的传统示波器。


图I为本发明实施例一种基于浮点型DSP的运动载体姿态倾角测量装置的总体结构框图。图2为本发明实施例一种基于浮点型DSP的运动载体姿态倾角测量装置的SCI转 USB电路原理图。图3为本发明实施例一种基于浮点型DSP的运动载体姿态倾角测量装置的IMU模块的原理图，即陀螺仪和加速度计的外围电路原理图。图4为本发明实施例一种基于浮点型DSP的运动载体姿态倾角测量装置的电源电 路原理图。图5为本发明实施例一种基于浮点型DSP的运动载体姿态倾角测量装置的DSP电 路原理图。图6为本发明实施例一种基于浮点型DSP的运动载体姿态倾角测量装置的DSP构 成最小系统所需的振荡器、复位、JTAG仿真口、外扩RAM电路原理图。图7为本发明实施例一种基于浮点型DSP的运动载体姿态倾角测量装置的上位机 软件层叠显示波形图。图8为本发明实施例一种基于浮点型DSP的运动载体姿态倾角测量装置的上位机 软件测试分立显示波形图。图9为本发明实施例一种基于浮点型DSP的运动载体姿态倾角测量装置的软件流 程图。图10为本发明方法的流程图。图11为本发明浮点型DSP进行数据处理过程的流程图。
具体实施例方式本发明一种基于浮点型DSP的运动载体姿态倾角测量方法，包括以下步骤 步骤一陀螺仪及其处理电路、加速度计及其处理电路构成惯性测量单元GMU)安装
在被测运动载体上，用于实时反馈运动载体的姿态角度和角速度信息；
步骤二 32位浮点型DSP通过IIC总线对1中的MU进行实时数据采集、分析和处理得 到三种动态角度值陀螺仪测量值、加速度计测量值、经DSP滤波处理后的动态值，之后DSP 通过SCI把数据发送出去；
步骤三以虚拟示波器方式对2中的三种数据波形进行实时记录、保存和显示。其中步骤二中的32位浮点型DSP进行数据采集、处理，方法过程包括
步骤1，DSP通过IIC总线对IMU模块进行校正、读写；
步骤 2 ， D S P 使用公式
i十算:加速度i十传感器所测J量;的x轴的角度值，同
、丨_(oc—z) +(oc_v)
理可得到其他轴的角度；其中ac_x、ac_y、ac_z分别为加速度计传感器测的各轴加速计分 量，EalarlDeg是弧度到角度的转换常量为57. 29578 ；
步骤 3，DSP 使用公式x_arisJmt_op) *L3G4200D_SCAL1 得到陀螺仪测
量的x轴角速度，同理可得到其他轴的角速度值；其中Kfpo_ _3c和x—aris—inil:_op分
别为陀螺仪实时测量的角速度和陀螺仪校正初值,L3G42MD_SCAtl为陀螺仪的灵敏度 8. 75mdps ；
步骤4，DSP采用卡尔曼滤波算法将两种信号进行融合。其原理为根据前一时刻的角 度值预测当前时刻的实际角度值，再由预测得到的当前时刻的角度值得到当前时刻的高斯 噪声的方差，然后卡尔曼滤波器进行递归运算得到当前最优的角度值；状态方程和测量方程
权利要求
1.一种基于浮点型DSP的运动载体姿态倾角测量方法，其特征在于，包括以下步骤 步骤一将陀螺仪及其处理电路和加速度计及其处理电路构成的惯性测量单元IMU安装在被测运动载体上，用于测量反馈运动载体的姿态角度和角速度信息；步骤二 32位浮点型DSP通过IIC总线将IMU采集的数据进行实时分析处理后得到三种动态角度值，即陀螺仪测量值、加速度计测量值、经DSP滤波处理后的动态值，之后DSP通过SCI把数据发送出去；步骤三以虚拟示波器方式对步骤二中的三种数据波形进行实时记录、保存和显示。
2.根据权利要求I所述的测量方法，其特征在于，所述步骤二中的浮点型DSP进行数据处理包括以下步骤；步骤一 DSP通过IIC总线对IMU模块进行校正和读写；步骤二 D S P 使用公式
3.根据权利要求I所述的测量方法，其特征在于，所述步骤三中的虚拟示波器是在PC 机中使用虚拟仪器软件和虚拟仪器软件架构VISA编写的以虚拟示波器方式显示波形的上位机软件；其中所使用的虚拟仪器软件为常用的LabVIEW软件。
4.一种使用基于浮点型DSP的运动载体姿态倾角测量方法的装置，包括惯性测量单元 MU、电源电路(201)、浮点型DSP电路(301)、SCI转USB通信电路(401)、上位机(501)，电源电路(201)分别与MU模块、浮点型DSP电路(301)、SCI转USB通信电路(401)进行连接，MU模块通过IIC总线与浮点型DSP电路(301)进行连接，浮点型DSP电路(301)通过 SCI接口与SCI转USB电路(401)进行连接，SCI转USB通信电路(401)与上位机进行连接； 其特征在于惯性测量单元IMU包括陀螺仪及其处理电路(101)和加速度计及其处理电路 (102)电路，陀螺仪及其处理电路(101)包括依次连接的陀螺仪、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路，加速度计及其处理电路(102)包括依次连接的加速度计、 模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路。
5.根据权利要求4所述的一种使用基于浮点型DSP的运动载体姿态倾角测量方法的装置，其特征在于所述的浮点型DSP电路(301)，其内部自带一个单精度浮点单元，支持硬件浮点运算。
全文摘要
本发明公开了一种基于浮点型DSP的运动载体姿态倾角测量方法及装置，该方法包括安装IMU模块到被测运动载体上，DSP实时采集、分析、处理得到三种姿态角信息，虚拟示波器进行动态波形的记录、显示和保存；该装置包括惯性测量单元IMU、电源电路、浮点型DSP电路、SCI转USB通信电路、上位机，其中，惯性测量单元IMU包括陀螺仪及其处理电路和加速度计及其处理电路电路，陀螺仪及其处理电路包括依次连接的陀螺仪、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路，加速度计及其处理电路包括依次连接的加速度计、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路；本发明对动态姿态倾角跟踪效果好、实时性高、精确度高、表示范围大、数据处理效率高。
文档编号G01C1/00GK102607509SQ201210087728
公开日2012年7月25日 申请日期2012年3月29日 优先权日2012年3月29日
发明者单梁, 戴跃伟, 李曦, 李颖, 杨阳, 潘晓伟, 韩成祥, 高建龙 申请人:南京理工大学