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

实用新型内容本实用新型的目的在于提供一种对动态姿态倾角跟踪效果好、实时性高、精确度高、表示范围大、数据处理效率高的基于浮点型DSP的运动载体姿态倾角测量方法及其装置。实现本实用新型目的的技术解决方案为一种基于浮点型DSP的运动载体姿态倾角测量装置，包括惯性测量单元MU、电源电路、浮点型DSP电路、SCI转USB通信电路、上位机，电源电路分别与IMU模块、浮点型DSP电路、SCI转USB通信电路进行连接，IMU模块通过IIC总线与浮点型DSP电路进行连接，浮点型DSP电路通过SCI接口与SCI转USB电路进行连接，SCI转USB通信电路与上位机进行连接；惯性测量单元MU包括陀螺仪及其处理电路和加速度计及其处理电路电路，陀螺仪及其处理电路包括依次连接的陀螺仪、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路，加速度计及其处理电路包括依次连接的加速度计、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路。 本实用新型与现有技术相比，其显著优点I、本实用新型是不仅可以对载体静态姿态角进行准确测量，也可以对运动强烈环境下的载体动态姿态角进行精确且跟踪效果好的动态测量；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为本实用新型实施例一种基于浮点型D SP的运动载体姿态倾角测量装置的上位机软件测试分立显示波形图。图9为本实用新型实施例一种基于浮点型DSP的运动载体姿态倾角测量装置的软件流程图。
具体实施方式
本实用新型一种基于浮点型DSP的运动载体姿态倾角测量装置，包括惯性测量单元IMU、电源电路201、浮点型DSP电路301、SCI转USB通信电路401、上位机501，电源电路201分别与IMU模块、浮点型DSP电路301、SCI转USB通信电路401进行连接，IMU模块通过IIC总线与浮点型DSP电路301进行连接，浮点型DSP电路301通过SCI接口与SCI转USB电路401进行连接，SCI转USB通信电路401与上位机501进行连接；其中，惯性测量单元IMU包括陀螺仪及其处理电路101和加速度计及其处理电路102电路，陀螺仪及其处理电路101包括依次连接的陀螺仪、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路，加速度计及其处理电路102包括依次连接的加速度计、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路。所述的浮点型DSP电路301，其内部自带一个单精度浮点单元，支持硬件浮点运
笪
ο所述的上位机501中的虚拟示波器是使用虚拟仪器软件和虚拟仪器软件架构编写的以虚拟示波器方式显示波形的上位机软件。
以下结合附图说明对本实用新型的实施作详细描述。结合图I所示，本实用新型所提供的一种基于浮点型DSP的运动载体姿态倾角测量装置包括陀螺仪及其信号处理电路101、加速度计及其信号处理电路102，两者构成惯性测量单元(頂U);浮点型DSP电路301与MU模块通过SPI接口或IIC接口电路连接进行数据通信；数据处理模块302在DSP内部，其负责将SPI总线或IIC总线采集的信息进行读取和计算、卡尔曼滤波、数据格式转换，然后通过DSP的SCI发送出去；SCI转USB通信电路401连接的是DSP电路301和上位机501 ；电源电路201电路连接到陀螺仪电路101、加速度计电路102、DSP电路301、SCI转USB通信电路401 ;上位机501上使用LabVIEW和VISA编写用户软件，实现波形的实时动态记录、保存和显示。本实用新型所提供一种基于浮点型DSP的运动载体姿态倾角测量装置，既可以测量静态环境下的姿态角，也可以用来准确测量和跟踪动态环境下的姿态角。图2为本测量装置的SCI转USB电路原理图；图3为本测量装置的MU模块的原理图，即陀螺仪和加速度计的外围电路原理图；图4为本测量装置的电源电路原理图；图5为本测量装置的DSP电路原理图；图6为本测量装置的DSP构成最小系统所需的振荡器、复位、JTAG仿真口、外扩RAM电路原理图。本实用新型实施例一种基于浮点型DSP的运动载体姿态倾角测量装置，包括陀螺仪及其处理电路101、加速度计及其处理电路102、电源电路201、浮点型DSP电路301、数据处理模块302、通信电路401和上位机501，进一步说明I)采用陀螺仪和加速度计两种传感器构成惯性测量单元(MU)固定于被测运动载体上，用来实时测量运动载体的姿态角度和角速度信息；陀螺仪芯片周围需要外加特定数值的电容、电阻以配置二阶低通滤波电路，用来同步驱动和感应接口，如图3所示，取Cl=O. 47uF，C2=0. OluF, R3=10K ;陀螺仪集成芯片型号为L3G4200D，加速度计集成芯片型号
为ADXL345，占为模式选择端，高
电平时为IIC通信模式，低电平时为SPI通信模式，图3使用IIC模式进行通信；2)采用由TI (德州仪器)公司生产的型号为TMS320F28335的32位浮点型DSP用作本装置的主芯片，DSP在对大量的浮点运算和正余弦运算时具有很大的优势，对复杂算法程序执行效率高，得到的动态姿态倾角精度高。从节约成本、产品开发、数据处理效率 和精确度等方面考虑，在实施中可采用核心板加底板构成装置测量板将主芯片DSP的最小系统做成核心板，其他的各子模块电路以底板形式制作，这样既可以节约成本又方便日后的维修、多功能扩展和二次开发；DSP通过IIC总线与IMU模块进行IIC连接通信，结合图9所示，具体流程有1、DSP对SCI、IIC、定时器初始化、中断初始化，DSP对加速计和陀螺仪进行校正；2、DSP每2ms进行一次中断，浮点型DSP内部进行实时数据处理，包括DSP
对MU进行校正和读写、DSP使用公式―肌加rr^·.*EuIarlDsg得到加速
'丨(oc z) +(U)-
度计测量的 X 轴角度值、DSP 使用公SCgfrojratw—x-i—asis—init—up) *L3G4200D SCALE
得到陀螺仪测量的X轴角速度；3、DSP采用卡尔曼滤波将两种信号进行融合，取初始化值
0003O
Q =
为L0 a.0013], Ts=O. 02s ; 4、将得到的浮点型数据转成四字节整型数据；5、制 Jt=[rm] = Qm
定通信协议，其格式为A0XXXXB0A1XXXXB1A2XXXXB2，其中XXXX表示四字节型角度值，总共有三组有效数据分别为陀螺仪测量的动态姿态角度值、加速度计测量的动态姿态角度值、DSP进行数据处理、计算和滤波融合后的动态姿态角度；6、DSP控制SCI，将上述格式的18个字节角度信息通过SCI转USB电路发送给上位机；3)上位机使用NI (美国国家仪器)公司生产的LabVIEW8. 2版本的软件进行编写，底层驱动使用VISA (虚拟仪器软件架构)和下位机进行数据通信。使用简单，仅需一根USB数据线连接，上位机收到18个字节数据进行数据格提取、校验，对不符合格式的数据进行踢除，再进行整型数据到浮点型数据格式的转换，得到三个浮点型角度值分别为陀螺仪测量的角度、加速度计测量的角度、基于DSP数据处理、滤波融合和转换的角度，并用虚拟示波器进行动态实时显示、记录，还可以进行波形的保存和回放，省去了体积笨重、携带不便且成本昂贵的传统示波器。图7为本实用新型实施例一种基于浮点型DSP的运动载体姿态倾角测量装置的上位机软件层叠显示波形图。三种曲线分别是陀螺仪测量的角度、加速度计测量的角度、基于DSP数据处理、滤波融合和转换之后的角度，并可以实时显示、保存具体的角度值，也可以保存曲线和进行波形回放。[0037]图8为本实用新型实施例一种基于浮点型DSP的运动载体姿态倾角测量装置的上位机软件测试分立显示波形图。从图7、图8上可以看出加速度计采集的波形较为平滑，但动态响应不够，在跟踪很陡角度信号时误差较大；陀螺仪动态效果较好，但因先天性噪声干扰，随着时间的推移，经过积分后得到的角度值不准确，时常有一些跳变。本装置利用两种不同类型的传感器进行优势上实质性互补，消除了单一传感器的随机噪声和漂移误差，有效地避免了错误数据的输出，尤其是跟踪动态姿态角上的效果较单一传感器更敏捷、精确。本实用新型采用加速度计和陀螺仪结合的方式检测运动载体的姿态角，利用陀螺仪短时测量准确且反应敏捷的优势和加速度计长期稳定的特点，使用浮点型DSP作为核心处理器，充分发掘其对复杂算法的处理能力较强、实时性和精确度高的优势，对这两种类型的传感器进行实时数据采集、分析和处理，加入卡尔曼滤波实现两者信号进行融合，消除单一传感器的随机噪声和漂移误差，有效地避免了错误数据的输出，得到动态跟踪效果好、实时性高且更接近实际真值的姿态角，为高性能控制系统提高控制精度提供了强有力的数据支撑。上位机使用LabVIEW软件和VISA进行编写，示波器以虚拟仪器的方式显示出来，将 传统体积笨重、携带不便的示波器装进普通的PC机，节省了昂贵的测量调试成本。
权利要求1.一种基于浮点型DSP的运动载体姿态倾角测量装置，包括惯性测量单元IMU、电源电路(201)、浮点型DSP电路(301 )、SCI转USB通信电路(401)、上位机(501)，电源电路(201)分别与IMU模块、浮点型DSP电路(301)、SCI转USB通信电路(401)进行连接，IMU模块通过IIC总线与浮点型DSP电路(301)进行连接，浮点型DSP电路(301)通过SCI接口与SCI转USB电路(401)进行连接，SCI转USB通信电路(401)与上位机(501)进行连接；其特征在于惯性测量单元MU包括陀螺仪及其处理电路(101)和加速度计及其处理电路(102)电路，陀螺仪及其处理电路(101)包括依次连接的陀螺仪、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路，加速度计及其处理电路(102)包括依次连接的加速度计、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路。
2.根据权利要求I所述的一种基于浮点型DSP的运动载体姿态倾角测量装置，其特征在于所述的浮点型DSP电路(301)，其内部自带一个单精度浮点单元，支持硬件浮点运算。
专利摘要本实用新型公开了一种基于浮点型DSP的运动载体姿态倾角测量装置，该装置包括惯性测量单元IMU、电源电路、浮点型DSP电路、SCI转USB通信电路、上位机，其中，惯性测量单元IMU包括陀螺仪及其处理电路和加速度计及其处理电路电路，陀螺仪及其处理电路包括依次连接的陀螺仪、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路，加速度计及其处理电路包括依次连接的加速度计、模数转换电路ADC、数字滤波电路、带FIFO的IIC接口电路；本实用新型对动态姿态倾角跟踪效果好、实时性高、精确度高、表示范围大、数据处理效率高。
文档编号G01C1/00GK202710030SQ20122012537
公开日2013年1月30日 申请日期2012年3月29日 优先权日2012年3月29日
发明者高建龙, 潘晓伟, 单梁, 戴跃伟, 李颖, 韩成祥, 杨阳, 李曦 申请人:南京理工大学