专利名称：一种高分辨率姿态角度测量方法及装置的制作方法
技术领域：
本发明涉及姿态角测量技术，具体地说是一种高分辨率姿态角度测量 方法及装置，能被应用在需要测量如导航、机器人、模拟训练设备等领域。
背景技术：
姿态角测量技术广泛应用于航空航天、制导、模拟训练设备、机器人 技术等诸多领域中。姿态角的测量通常采用光电编码器、角速率传感器或 电子罗盘等作为测角组件。其中，光电编码器需要与被测物同轴且体积较 大，影响被测系统的设计，且不便于安装；地磁传感器受磁场干扰且分辨 率较低。而角速率传感器具有灵敏度高、体积小、功耗小、便于安装等特 点，用角速率传感器作为测角组件进行高分辨率的姿态角度测量被越来越 广泛地应用。
采用角速率传感器的系统一般是将传感器输出的模拟电压信号直接进 行采样量化，将其与角速率传感器标称的零点电压的差值数值积分得到角 度；或者采用带有数字量输出的角速率传感器，这种角速率传感器只是将 采样和量化集成在其内部，并没有加进的滤波功能，并且一般为串口输出， 输出的速率较低，不能进行较复杂的后续数字滤波。由于角速率传感器的 噪声和信号导线引入的噪声较大，这两种方法都不能很好地去除噪声，影 响测量角度的分辨率。
发明内容
为了克服现有技术中噪声大、分辨率低的不足，本发明的目的在于提 供一种噪声小、能提高角度测量分辨率的姿态角度测量方法及装置。
本发明解决其技术问题所采用的技术方案如下高分辨率姿态角度测 量方法取处于静止状态的角速率传感器输出的模拟电压信号，对模拟电 压信号进行低通滤波去除高频部分，然后进行信号采集和模/数转换，将其 转换为电压信号。数据处理程序按照最小均方差算法对电压信号进行数字 滤波，经均值滤波，获得实际零点电压；再根据实际零点电压与后续处于 运动情况，依次经RC低通滤波、采集和模/数转换、最小均方差数字滤波 及均值滤波的电压信号进行比较，对所得的差值数值积分得到姿态角度值。
高分辨率姿态角度测量装置，包括
角速率传感器，安装时应使自身的轴向与被测物体的被测轴平行； 低通滤波器，接收处于静止/运动状态的角速率传感器输出的模拟电压 信号，对模拟电压信号除噪处理后送至A/D采集转换模块； A/D采集转换模块，进行数据采集、模/数转换；运行于计算机的数据处理程序，进行数字滤波和数值积分。 其中所述数据处理程序流程是一个多线程程序，具有两个线程；主线
程负责对信号不间断地采集转换，辅助线程负责检测A/D采集转换模块上
先进先出缓存的状态，当其达到或超过半满时进行最小均方差数字滤波、
均值滤波和数值积分，得出姿态角度值；两个线程之间是异步关系，同时 执行。
所述角速率传感器采用高灵敏度的硅性MEMS陀螺。 本发明的有益效果是
1. 噪声小、角度测量的分辨率高。本发明通过软硬件滤波相结合的方 法，可以很好地消除高频噪声，如用高速的A/D采集转换模块完成采样 量化，保证了采样速度和量化精度；通过RC低通滤波能较好地去除传感 器的噪声；而LMS数字滤波能进一步消除传感器的噪声和导线引入的噪 声；能够得到较准确的传感器实际零点电压。这些都保证了这种方法可以 获得更高的角度分辨率。与传统方法相比，可以在低频范围内获得很高的 角度分辨率(本实施例采用分辨率为0.05度/秒的型号为Gyro1050的陀螺， 模拟电压信号输出范围是0-5伏，经过上述处理后，其角度分辨率可以达 到0.00025度)，并且可以通过改变滤波器的参数在分辨率和实时性间进行 调整。
2. 由于采用硅性MEMS陀螺，使本发明具有体积小、重量轻、灵敏 度高、线形度好的特点，并且安装简便，不影响被测量物体的机械结构。
图la为数字滤波算法原理图(实际零点电压的获取)。
图lb为数字滤波算法原理图(姿态角度的计算)。
图2为本发明装置结构原理图。
图3为本发明装置信号处理流程图。
图4为图2中低通滤波器原理图。
图5为数据处理程序流程图。
具体实施例方式
下面通过结合附图对本发明作进一步详细说明。
高分辨率姿态角度测量方法取处于静止状态的角速率传感器输出的
模拟电压信号，对模拟电压信号进行低通滤波去除高频部分，然后进行信 号采集和模/数转换，将其转换为电压信号序列V[n]。数据处理程序按照最 小均方差(LMS)算法对电压信号进行数字滤波，再经均值滤波，获得实 际零点电压；再用实际零点电压与后续处于运动情况，依次经RC低通滤 波、采集和模/数转换、最小均方差数字滤波及均值滤波的电压信号序列进 行比较，对所得的差值积分得到姿态角度值。与传统方法相比，本发明可 以在低频范围内获得很高的角度分辨率，并且可以通过改变数字滤波的参 数在分辨率和实时性间进行调整。所述最小均方差(LMS)数字滤波算法是通过如式(1)的迭代运算， 根据数据不断修正自身的滤波系数W,，以使滤波后的电压序列r收敛于经
过滤波的电压值F'和经采样转换后的电压序列F的均方差最小的值。通过 调整系数更新幅度ri和滤波器的长度M，可以调整收敛速度，从而在精度 的实时性(收敛速度)间进行调整。本实施例幅度"=0.01,滤波系数的长 度1^= 10。
公式为F广2>,",卬 (1)
其中w, = w, + n(r厂^省)f,-m, j = M，M+l，... ， n+M(M<N)
F是经采样转换后的电压序列；w是滤波系数；F'是经过滤波的电压值； ti是系数更新幅度。
参见图la，实际零点电压的计算过程电压信号序列V[n]的前n个电 压数据V[O， 1， 2， ... ， n-l]经过LMS数字滤波和均值滤波，得到实际零 点电压Vc，代表角速率为零(静止时)的角速率传感器的输出的电压值， 作为比较基准，后续处于运动情况且依次经RC低通滤波、采集和模/数转 换、最小均方差数字滤波及均值滤波的电压信号序列与实际零点电压Vc 偏差代表角速率传感器转动的角速率。公式如下
co= (V，-Vc) /f (2)
其中co为角速率，单位是度/秒；V，是经过滤波的电压值；f是电压
与角速率的转换关系，由角速率传感器说明书标明。
参见图lb，姿态角度的计算过程后续经过采集和模/数转换的模拟电
压信号数据V[n, n+l， n+2，…]先经过RC低通滤波、LMS滤波和均值滤波 得到经过滤波的电压值V，，然后与实际零点电压Vc比较，对所得的差值 进行数值积分(求和)即可得到高分辨率的姿态角度。公式如下 a=Z(D*At (3)
其中a是角度值，单位为度；CO在式(2)中定义；At是采样周期，
单位为秒。
参见图2，本发明高分辨率姿态角度测量装置包括角速率传感器、低
通滤波器、A/D采集转换模块和运行于计算机的数据处理程序。其中，角 速率传感器安装时应使自身的轴向与被测物体的被测轴平行，处于静止/运 动状态的角速率传感器的输出电压为0-X(伏)的模拟电压信号(本实施例 X为5伏)，送至低通滤波器输入端，经过低通滤波器的低通滤波后送至 A/D采集转换模块，经采集、量化后通过数据处理程序进行数字滤波和数 值积分，得到具有高分辨率的姿态角度。
参见图3，本发明装置的信号处理过程如下
角速率传感器输出的是零点电压为O-X(V)的模拟电压信号，经过低通 滤波(RC电路)后，被去除高频部分，并被A/D采集转换模块进行数据采
5集和模/数转换得到成一个量化的电压信号序列V[n]，并按照最小均方差数 字滤波算法对其进行数字滤波，结果再被均值滤波后，得到实际零点电压 Vc。再将后续运动的传感器输出的模拟电压信号，即依次经低通滤波(RC)、 采集和模/数转换、最小均方差数字滤波及均值滤波的电压信号序列与Vc比
较，对所得的差值序列进行比例变换和数值积分得到姿态角度值。
本实施例采用高灵敏度的硅性MEMS陀螺作为角速率传感器，用RC 低通滤波器加数据处理程序的最小均方差数字滤波算法在低频范围内对其 输出信号进行模拟和数字滤波，数值积分后获得高分辨率的角度信号。本 实施例采用分辨率为0.05度/秒的型号为Gyml050的陀螺，模拟电压信号 输出范围是0-5伏，经过上述处理后，其角度分辨率可以达到0.00025度。
所述硅性MEMS (微型机电)陀螺具有灵敏度高、线形度好、动态范 围大、抗震性强等优点。其输出是一个标称零点电压为V。(伏)的模拟电 压量。角速率传感器的模拟电压信号与说明书中标明的额定零点电压的偏 差表征角速率，两者为正比关系。这样，测得输出电压，并与零点电压比 较后，偏差电压即可按所述正比关系转换为角速率，对其数值积分即可得 到角度。
图4为低通滤波器原理图。本实施例采用由电阻和电容构成的RC低 通滤波器，两者的值决定滤波，的截止频率，通常有-6dB截止频率= 1/(2*3.14*R*C)(Hz)，本实施例采用10Hz截止频率。
RC低通滤波器用于滤除高频变化的传感器模拟信号(一般被视为噪 声)。可以通过调整电阻和电容的值来设定其截至频率。模拟信号经过RC 低通滤波器后，绝大部分的高频噪声被滤掉，但仍需进一步的数字滤波才 能获得更高的精度。
所述A/D采集转换模块配有高速高精度的PCI接口 ，带有硬件先进先 出缓存(FIFO)，可以保证不会丢失数据。A/D采集转换模块对经过RC低 通滤波后的模拟电压信号进行不间断地采集量化，然后存放到缓冲区中， 供数据处理程序进行滤波。
参见图5，所述数据处理程序流程是一个多线程程序，具有两个线程。 主线程首先设置A/D采集转换模块参数，开始不间断的采样转换，再将采 样转换填充至先进先出缓存FIFO;然后启动辅助线程。辅助线程检査A/D 采集转换模块的先进先出缓存(FIFO)的状态，当其达到或超过半满或时， 将数据放置到电压信号序列V[n]中，再对电压信号序列V[n]进行最小均方 差数字滤波、均值滤波和数值积分，得出姿态角度值。两个线程之间是异 步关系，同时执行，在数字滤波的同时不间断采样，提高了效率。
权利要求
1. 一种高分辨率姿态角度测量方法，其特征是取处于静止状态的角速率传感器输出的模拟电压信号，对模拟电压信号进行低通滤波去除高频部分，然后进行信号采集和模/数转换，将其转换为电压信号。数据处理程序按照最小均方差算法对电压信号进行数字滤波，经均值滤波，获得实际零点电压；再根据实际零点电压与后续处于运动情况，依次经RC低通滤波、采集和模/数转换、最小均方差数字滤波及均值滤波的电压信号进行比较，对所得的差值数值积分得到姿态角度值。
2. 根据权利要求1所述高分辨率姿态角度测量方法，其特征是模拟电压信号为O-X(伏)范围的电压值，X的值在大于零的正数。
3. 根据权利要求1所述高分辨率姿态角度测量方法，其特征是所述 最小均方差算法通过如式(1)的迭代运算；<formula>formula see original document page 2</formula>其中<formula>formula see original document page 2</formula>F是经采样转换后的电压序列；w是滤波系数；F是经过滤波的电压值； T(是系数更新幅度；M为滤波系数的长度。
4. 一种高分辨率姿态角度测量装置，其特征在于包括 角速率传感器，安装时使自身的轴向与被测物体的被测轴平行； 低通滤波器，接收处于静止/运动状态的角速率传感器输出的模拟电压信号，对模拟电压信号除噪处理后送至A/D采集转换模块； A/D采集转换模块，进行数据采集、模/数转换； 运行于计算机的数据处理程序，进行数字滤波和数值积分。
5. 根据权利要求4所述高分辨率姿态角度测量装置，其特征在于所述数据处理程序流程是一个多线程程序，具有两个线程；主线程负责对信 号不间断地采集转换，辅助线程负责检测A/D采集转换模块上先进先出缓 存的状态，当其达到或超过半满时进行最小均方差数字滤波、均值滤波和 数值积分，得出姿态角度值。
6. 根据权利要求5所述高分辨率姿态角度测量装置，其特征在于两 个线程之间是异歩关系，同时执行。
7. 根据权利要求4所述高分辨率姿态角度测量装置，其特征在于所 述角速率传感器采用高灵敏度的硅性MEMS陀螺。
全文摘要
本发明涉及姿态角测量技术，具体地说是一种高分辨率姿态角度测量方法及装置。取处于静止状态的角速率传感器输出的模拟电压信号，对模拟电压信号进行低通滤波去除高频部分，然后进行信号采集和模/数转换，将其转换为电压信号。数据处理程序按照最小均方差算法对其进行数字滤波，再经均值滤波，获得实际零点电压；再用实际零点电压根据后续处于运动情况，依次经RC低通滤波、采集和模/数转换、最小均方差数字滤波及均值滤波的电压信号进行比较，对所得的差值数值积分得到姿态角度值。与传统方法相比，本发明可以在低频范围内获得很高的角度分辨率，并且可以通过改变数字滤波的参数在分辨率和实时性间进行调整。
文档编号G01D5/12GK101458096SQ20071015882
公开日2009年6月17日 申请日期2007年12月12日 优先权日2007年12月12日
发明者佟新鑫, 刘晓安, 帆 张, 张心宇 申请人:中国科学院沈阳自动化研究所