专利名称：用于基准传递的多传感器姿态测量装置的制作方法
技术领域：
本实用新型主要涉及到惯性测量设备领域，特指一种姿态测量装置。
背景技术：
姿态测量装置是根据牛顿力学定律，利用陀螺仪、倾角仪或加速度计等惯性器件 敏感载体运动，并通过计算机进行姿态解算。长期以来，姿态测量装置一直采用传统的惯性 测量组件和通用计算机的体系结构。该结构存在精度低、集成度低、体积大、成本高、功耗大 等不足。随着惯性技术的发展，姿态测量装置小型化、集成化和高精度的趋势愈实用新型 显。就小型化集成化而言，采用合适的微处理器以替代通用计算机，具有重要意义。DSP器件 采用了与传统计算机系统不同的总线结构和硬件运算单元，极大的增强了系统数据处理能 力，满足了姿态测量装置的姿态实时解算要求，而且缩小了系统体积，降低了系统成本。就 测量精度而言，主要取决于惯性器件的精度。在启动初期，姿态测量装置的精度是优良的， 但由于陀螺漂移的存在，姿态测量精度会随着时间的推移而降低。目前，提高精度的主要技 术手段a.对惯性器件进行精确的误差补偿；b.采用误差不积累的外部参考量对姿态信息 进行修正。
实用新型内容本实用新型要解决的技术问题就在于针对现有技术存在的技术问题，本实用新 型提供一种结构简单紧凑、成本低廉、功耗低、可靠性高、适用范围广的用于基准传递的多 传感器姿态测量装置。为解决上述技术问题，本实用新型采用以下技术方案。一种用于基准传递的多传感器姿态测量装置，其特征在于包括惯性测量组件、信 号调理单元、数据采集单元、处理器单元、通讯单元以及电源单元，所述惯性测量组件将载 体运动的实时信号传递给信号调理单元，所述信号调理单元对输入的信号进行差分调制并 抑制采集过程中的噪声干扰后经数据采集单元传递给处理器单元，所述处理器单元根据实 时信号得到载体的姿态角信号，并通过通讯单元传递给上位机。作为本实用新型的进一步改进所述惯性测量组件包括三轴正交的陀螺、两轴水平安装的倾角仪以及安装夹具， 所述陀螺实时测量载体在敏感轴方向的角速度信号，所述倾角仪实时测量沿敏感轴方向的 角位置信号，并输出与所述角速度信号和所述角位置信号相对应的原始电压信号。与现有技术相比，本实用新型的优点就在于1、本实用新型用于基准传递的多传感器姿态测量装置，将信号调理、数据采集、处 理、通讯电路集成为一体，集成度高、体积小、功耗低；2、本实用新型用于基准传递的多传感器姿态测量装置，姿态解算精度高，克服了 陀螺测量精度随时间推移而降低，以及倾角仪测量精度容易受振动和加速度干扰的问题。[0012]3、本实用新型用于基准传递的多传感器姿态测量装置，实时性好，惯性传感器的 误差补偿、噪声处理和数据融合算法均采用实时的递推结构，能为上位机提供至少IKHz以 上的姿态信号。4、本实用新型用于基准传递的多传感器姿态测量装 置，具有完全自主性，不受外 界环境影响，能够全天候工作。
图1是本实用新型的总体框架结构示意图；图2是本实用新型的工作流程示意图；图3是本实用新型的立体结构示意图；图4是本实用新型中信号调理单元的电路原理示意图；图5是本实用新型中数据采集单元的电路原理示意图；图6是本实用新型中处理器单元的电路原理示意图；图7是本实用新型中通讯单元的电路原理示意图；图8是本实用新型中电源单元的电路原理示意图。图例说明1、惯性测量组件；101、陀螺；102、倾角仪；103、夹具；2、信号调理单元；3、数据采
集单元；4、处理器单元；401、多传感器数据融合算法；5、通讯单元；6、电源单元。
具体实施方式
以下将结合具体实施例和说明书附图对本实用新型做进一步详细说明。如图1，本实用新型用于基准传递的多传感器姿态测量装置，包括惯性测量组件 1、信号调理单元2、数据采集单元3、处理器单元4、通讯单元5以及电源单元6，惯性测量组 件1将载体运动的实时信号传递给信号调理单元2，信号调理单元2对输入的信号进行差 分调制并抑制采集过程中噪声干扰后经数据采集单元3传递给处理器单元4，处理器单元 4根据实时信号得到载体的姿态角信号，并通过通讯单元5传递给上位机。本实施例中，惯 性测量组件1包括三轴正交的陀螺101、两轴水平安装的倾角仪102以及安装夹具103，如 图3所示，三轴正交的陀螺101的敏感轴通过高精度的机械安装面与安装夹具103的安装 轴X，1，ζ精确对准，两轴水平安装的倾角仪102的敏感轴通过高精度的机械安装面与安装 夹具103的安装轴X，y精确对准，同时安装夹具103，通过定位螺栓与载体固连。那么，可 以实现陀螺101实时测量载体在敏感轴方向的角速度信号，倾角仪102实时测量载体沿敏 感轴方向的角位置信号，并输出与角速度信号和角位置信号相对应的原始电压信号。多传感器姿态测量装置的工作步骤为1、六自由度的惯性测量组件1能够实时敏 感载体运动特性通过陀螺101测量载体在其敏感轴方向的角速度信号，通过倾角仪102测 量沿其敏感轴方向的角位置信号，并将得到的原始电压信号输出到处理器单元4 ;2、信号 调理单元2对输入的电压信号进行差分调制，同时抑制采集过程的噪声干扰；3、高精度的 数据采集单元3对得到的惯性传感器多路调制信号进行高速采集，将模拟电压转换为数字 信号，并传输到处理器单元4 ;4、处理器单元4采用数字处理器DSP，根据惯性传感器的误差 模型，对采集的原始数据进行误差补偿和噪声处理，并将得到的载体运动理想数据输出到多传感器数据融合算法401中；5、多传感器数据融合算法401可以在DSP中通过软件实现，即将输入的载体运动参数，在卡尔曼滤波框架下，实时递推得到载体的姿态角信号；6、解算 得到的载体姿态角信号，通过通讯单元5发送到上位机。DSP中姿态解算程序的软件流程，如图2所示，可采用C语言编写，由于F2812属于 32位定点DSP，所以在编写软件程序时，涉及到程序中浮点运算如何处理的问题，为了兼顾 程序的实时性和姿态解算的精度，需要在编写程序时注意以下几个方面的问题采用定时 中断的方式工作，降低DSP的开销，以保证系统的低功耗和实时性；在处理浮点运算时，采 用Ti公司提供的用于实现定点DSP仿真浮点运算的算法库IQmath，通过调用IQmath可以 很好地将姿态解算程序移植到TMS320F2812中，从而保证了计算精度。软件设计过程中，首 先在主程序里完成初始化设置，包括系统初始化、GPIO初始化、中断向量和中断向量表初始 化、SPI初始化、定时中断初始化、Can接口初始化和SCI初始化；然后，采用中断方式来完 成误差补偿、噪声处理以及多传感器数据融合算法。如图4所示，信号调理单元2的主要功能是对输入的原始电压信号进行差分调制、 增益变换、模拟滤波，以及增强对数据采集芯片的驱动能力。本实施例中构成信号调理电路 的运算放大器选用TI公司0PA1632，它是一款高带宽(150MHz)、低噪声(1. 3nV/ V Hz)、低 失真度(0.000022%)的差分放大器，专门应用于驱动高性能的AD采集芯片。如图5所示，数据采集单元3的主要功能是完成惯性测量组件1输出的角速度和 角位置模拟信号转换为数字信号。设计时必须充分考虑到惯性测量组件1的通道数，惯性 测量组件1尤其是陀螺输出信号的带宽、噪声、分辨率、精度和测量范围等因素。由于采用 陀螺的基本性能为带宽Ο-lKHz、噪声0. 0005°/S、分辨率0. 0003°/S、精度0. 003°/s、测量范 围士 200°/s，其动态特性为116dB。本实施例中选用TI公司数据采集芯片ADC1278，它是一 种24位的高精度模数转换器，动态特性达138dB，具有8通道、50kHz带宽、105kSPS转换速 率、1. 8μ V/°C失调漂移以及高达109dB信噪比，完全满足惯性传感器高精度数据采集的需 求。同时，ADS1278专门为与高性能TMS320DSP平台协同工作而进行了优化，设计帧同步和 SPI串行接口，每个接口均支持菊花链结构，便于数据的传输和通道的扩展。如图6所示，处理器单元4采用DSP处理电路，其主要功能是实现惯性传感组件1 的误差补偿、噪声处理和数据融合算法，以及对数据采集芯片和通讯芯片的控制管理。本实 施例中，数字信号处理器DSP选用TI公司的TMS320F2812，它是专门针对工业控制推出的低 功耗、高性能32位定点数字信号处理器，具有6级流水线结构，主频高达150MHz，内部集成 18KBRAM, 128KBFlash存储器，以及定时器、事件管理器、SPI、SCI、CAN、AD等丰富的片内外 设，高效率的C/C++编译程序、独特的“ IQmath”程序库和专用程序包，适于嵌入式系统、智 能仪器仪表等需要高速数据处理的系统。如图7所示，通讯单元5的主要功能是实现上位机与处理器单元4的实时数据 交互。本实施例中，通讯单元5的CAN通讯芯片选用SN65HVD230，RS422通讯芯片选用 MAX3490。如图8所示，电源单元6的主要功能是为惯性测量组件1及其硬件电路提供电源 保障。电源电路为惯性测量组件1提供士 15V和+5V电源，为信号调理单元2提供士 15V 电源，为数据采集单元3提供+5V和+3. 3V电源，为DSP处理电路提供+5V、+3. 3V、+1. 8V电 源，为通讯单元5提供+3. 3V电源。由于电源单元6的噪声特性直接决定了数据采集的精度，因此电源单元6的设计以及电源�？榈难∪。谡鲎爸玫纳杓浦杏任匾�。本实施例 中电源�？檠∮贸舻缭碒IC24S05MC和HJC24D15MC，宽电压输入18-36V、输出功率10W，输 出纹波50mVp_p(200MHz)，输出效率75%。DSP电源芯片选用TI公司的TPS767D318。现进一步阐述工作步骤，惯性传感组件1中陀螺101的误差补偿和噪声处理方法。典型的陀螺测量模型可以表示为
( 1)式中土=叉，7，2;Ui------陀螺原始测量值，V ；Kg------陀螺标度因数，mV/ (0/s)；(Oi------陀螺测量角速度，°/s ；ε j------陀螺零偏电压，V;Wgi------陀螺白噪声，V。陀螺标度因数中存在非线性影响因素和温度影响因素。在计算中，如果不考虑非 线性和温度对标度因数的影响，则会产生较大的误差。因此建立标度因数的误差模型，如下 所示Kg = Kg0 ·、-kT(2)式中Kg------陀螺标度因数，mV/ (Vs)；Kg0------陀螺常温下测试得到的标度因数，mV/ (Vs)；------陀螺标度因数的非线性修正系数；kT------陀螺标度因数的温度修正系数；陀螺标度因数的非线性修正系数、可以表示为
(3)式中 >为陀螺原始测量值；Umax为陀螺最大测量值屯，k2为非线性修正参数。陀螺标度因数的温度修正系数kT可以表示为kT = 1+ti · T+t2 · T2(4)式中T为温度测量值；、，t2为温度修正参数。同时，陀螺零偏中也存在温度影响因素。那么建立零偏的温度误差模型为ε j = ε i(l+Cl · T+c2 · T2+c3 · T2(5)式中ε ,为陀螺零偏电压；ε i0为常温下陀螺一次启动零偏电压；T为温度测量 值；Cl，C2, C3为多项式拟合得到的零偏温度修正参数。式⑴经过式(2)和⑶中标度因数和零偏的误差补偿后，可得陀螺测量角速度钱为
(6)式中=Coi------载体角速度，°/s ； ε ri------残余误差，°/s ；Wgi------白噪声，°/s。针对白噪声信号Wgi，采用数字低通滤波器来滤除数据采集过程中，串入的高频噪 声干扰。数字低通滤波属于经典滤波范畴，其原理是设定适宜的通频带，将高于通频带的噪 声进行衰减，滤波效果与滤波器的截止频率有关，截止频率越低，效果越好。数字低通滤波实现简单，应用广泛，可以用差分方程来描述 其中，ak, bk为滤波系数；X(Ii)为当前滤波输入；y (η)为当前滤波输出；x(n-k)为 前第k时刻输入；y (n-k)为前第k时刻滤波输出；M, N为滤波器阶数。设计低通巴特沃斯 滤波器，对误差补偿后的陀螺测量角速度钱进行滤波，可以将高于截止频率的高频噪声进 行较大幅度衰减。由于陀螺101零偏启动重复性以及误差模型不准确等因素的影响，陀螺101测量 角速度钱中存在少量的残余误差ε Η，这时需要引入外部参考量，采用数据融合算法对其进 行修正。现进一步阐述工作步骤中的多传感器数据融合算法401。首先，采用欧拉角法来描述载体的姿态运动学方程。姿态运动的表示法有许多种， 如欧拉角法、方向余弦法、四元数法、旋转矢量法等。由于设计的对象是在水平面附近小 角度晃动，采用欧拉角法不存在姿态解算的奇异点；同时欧拉角微分方程关系简单，概念直 观，容易理解，利于观察和分析姿态估计算法的效果。因此采用欧拉角法来表示桅杆姿态角 θ = [φ θ ψ]τ，其中φ为滚动角、θ为俯仰角、Ψ为偏航角，由欧拉角微分方程可得姿态 运动学模型Θ = Α}ω(8)式中 针对惯性测量组件中陀螺误差补偿后的数学模型 式中，白噪声Wgi和相关漂移噪声Wri的统计特性为 'E[wgXt)] = E[wri(t)] = 0 由于残余误差ε ri的存在，会造成姿态角测量误差随时间而增长。那么将ε ri设 为状态变量的一部分，对其进行在线实时估计和补偿，进而提高测量精度。针对惯性测量组件倾角仪误差补偿后的数学模型 式中，θ为俯仰角，φ为滚动角，乞，\分别为俯仰轴和滚动轴倾角仪的测量值，疋， 是载体运动引入的加速度干扰，该项为未知量；vx，Vy为测量噪声，其统计特性为[0079] 由上式可知，当倾角仪处于静态时S = 0，具有极高的测量精度，但处于动态时，容 易受到载体振动和加速度的干扰3 * 0，那么，在动态情况下单独使用倾角仪测量载体姿态， 会带来较大测量误差。因此考虑将倾角仪的测量值作为融合数据，构建系统量测方程，实现 对桅杆姿态的组合测量，同时在线估计陀螺残余误差。令状态变量X= [Θ ε]τ，输入向量 =[成 γ屯Γ，干扰加速度 · 二阪dyf,
观测向量ζ =[乞t]7·，那么可得三维条件下的系统状态空间方程为 式中 系统噪声W和量测噪声V 噪声方差矩阵分别为Q和R 易知式(12)是一个非线性连续系统，其中状态矩阵Α，输入矩阵B和Γ均包含有 非线性项~。在工程实践中较为普遍地应用扩展卡尔曼滤波方法将其线性化和离散化 式中 相应的等效离散系统噪声方差阵和量测噪声方差阵分别为Qk和Rk那么构建卡尔曼滤波框架下的算法递推流程为·状态变量时间更新 ·状态预测方差阵 ·卡尔曼滤波增益 状态变量量测更新 由于倾角仪测量值Zk中包含有未知的加速度干扰量义，如果直接应用增益Kk，对 进行量测更新，只能获得次优估计值，因此，提出对<采用�：评硐低辰新呒�
断，采用加权的形式来实现i^H的量测更新，即·状态变量加权量测更新 ·状态估计协方差阵 式中义代表着载体当前的运动状态，为未知加速度干扰量，其数值无法进行精确的测 量。那么可以采用�：评硐低扯栽靥逶硕刺信卸希范尤ㄖ礝kW大小。以上所述仅是本实用新型的优选实施方式，本实用新型的保护范围并不仅局限于 上述实施例，凡属于本实用新型思路下的技术方案均属于本实用新型的保护范围。应当指 出，对于本技术领域的普通技术人员来说，在不脱离本实用新型原理前提下的若干改进和 润饰，这些改进和润饰也应视为本实用新型的保护范围。
权利要求一种用于基准传递的多传感器姿态测量装置，其特征在于包括惯性测量组件(1)、信号调理单元(2)、数据采集单元(3)、处理器单元(4)、通讯单元(5)以及电源单元(6)，所述惯性测量组件(1)将载体运动的实时信号传递给信号调理单元(2)，所述信号调理单元(2)对输入的信号进行差分调制并抑制采集过程中的噪声干扰后经数据采集单元(3)传递给处理器单元(4)，所述处理器单元(4)根据实时信号得到载体的姿态角信号，并通过通讯单元(5)传递给上位机。
2.根据权利要求1所述的用于基准传递的多传感器姿态测量装置，其特征在于所述 惯性测量组件(1)包括三轴正交的陀螺(101)、两轴水平安装的倾角仪(102)以及安装夹具 (103)，所述陀螺(101)实时测量载体在敏感轴方向的角速度信号，所述倾角仪(102)实时 测量沿敏感轴方向的角位置信号，并输出与所述角速度信号和所述角位置信号相对应的原 始电压信号。
专利摘要一种用于基准传递的多传感器姿态测量装置，包括惯性测量组件、信号调理单元、数据采集单元、处理器单元、通讯单元以及电源单元，所述惯性测量组件将载体运动的实时信号传递给信号调理单元，所述信号调理单元对输入的信号进行差分调制并抑制采集过程中噪声干扰后经数据采集单元传递给处理器单元，所述处理器单元根据实时信号得到载体的姿态角信号，并通过通讯单元传递给上位机。本实用新型是一种结构简单紧凑、成本低廉、功耗低、可靠性高、适用范围广的用于基准传递的多传感器姿态测量装置。
文档编号G01C1/00GK201600133SQ201020102739
公开日2010年10月6日 申请日期2010年1月27日 优先权日2010年1月27日
发明者周晓尧, 周英, 张智永, 张连超, 洪华杰, 范大鹏 申请人:中国人民解放军国防科学技术大学
专业数控铣床产品供应商
20年生产经验，实力工厂，精工品质，质量有保障
