CN101458096A - 一种高分辨率姿态角度测量方法及装置 - Google Patents

Abstract

本发明涉及姿态角测量技术，具体地说是一种高分辨率姿态角度测量方法及装置。取处于静止状态的角速率传感器输出的模拟电压信号，对模拟电压信号进行低通滤波去除高频部分，然后进行信号采集和模/数转换，将其转换为电压信号。数据处理程序按照最小均方差算法对其进行数字滤波，再经均值滤波，获得实际零点电压；再用实际零点电压根据后续处于运动情况，依次经RC低通滤波、采集和模/数转换、最小均方差数字滤波及均值滤波的电压信号进行比较，对所得的差值数值积分得到姿态角度值。与传统方法相比，本发明可以在低频范围内获得很高的角度分辨率，并且可以通过改变数字滤波的参数在分辨率和实时性间进行调整。

Description

一种高分辨率姿态角度测量方法及装置

技术领域

本发明涉及姿态角测量技术，具体地说是一种高分辨率姿态角度测量方法及装置，能被应用在需要测量如导航、机器人、模拟训练设备等领域。

背景技术

姿态角测量技术广泛应用于航空航天、制导、模拟训练设备、机器人技术等诸多领域中。姿态角的测量通常采用光电编码器、角速率传感器或电子罗盘等作为测角组件。其中，光电编码器需要与被测物同轴且体积较大，影响被测系统的设计，且不便于安装；地磁传感器受磁场干扰且分辨率较低。而角速率传感器具有灵敏度高、体积小、功耗小、便于安装等特点，用角速率传感器作为测角组件进行高分辨率的姿态角度测量被越来越广泛地应用。

采用角速率传感器的系统一般是将传感器输出的模拟电压信号直接进行采样量化，将其与角速率传感器标称的零点电压的差值数值积分得到角度；或者采用带有数字量输出的角速率传感器，这种角速率传感器只是将采样和量化集成在其内部，并没有加进的滤波功能，并且一般为串口输出，输出的速率较低，不能进行较复杂的后续数字滤波。由于角速率传感器的噪声和信号导线引入的噪声较大，这两种方法都不能很好地去除噪声，影响测量角度的分辨率。

发明内容

为了克服现有技术中噪声大、分辨率低的不足，本发明的目的在于提供一种噪声小、能提高角度测量分辨率的姿态角度测量方法及装置。

本发明解决其技术问题所采用的技术方案如下：高分辨率姿态角度测量方法：取处于静止状态的角速率传感器输出的模拟电压信号，对模拟电压信号进行低通滤波去除高频部分，然后进行信号采集和模/数转换，将其转换为电压信号。数据处理程序按照最小均方差算法对电压信号进行数字滤波，经均值滤波，获得实际零点电压；再根据实际零点电压与后续处于运动情况，依次经RC低通滤波、采集和模/数转换、最小均方差数字滤波及均值滤波的电压信号进行比较，对所得的差值数值积分得到姿态角度值。

高分辨率姿态角度测量装置，包括：

角速率传感器，安装时应使自身的轴向与被测物体的被测轴平行；

低通滤波器，接收处于静止/运动状态的角速率传感器输出的模拟电压信号，对模拟电压信号除噪处理后送至A/D采集转换模块；

A/D采集转换模块，进行数据采集、模/数转换；

运行于计算机的数据处理程序，进行数字滤波和数值积分。

其中所述数据处理程序流程是一个多线程程序，具有两个线程；主线程负责对信号不间断地采集转换，辅助线程负责检测A/D采集转换模块上先进先出缓存的状态，当其达到或超过半满时进行最小均方差数字滤波、均值滤波和数值积分，得出姿态角度值；两个线程之间是异步关系，同时执行。

所述角速率传感器采用高灵敏度的硅性MEMS陀螺。

本发明的有益效果是：

1.噪声小、角度测量的分辨率高。本发明通过软硬件滤波相结合的方法，可以很好地消除高频噪声，如：用高速的A/D采集转换模块完成采样量化，保证了采样速度和量化精度；通过RC低通滤波能较好地去除传感器的噪声；而LMS数字滤波能进一步消除传感器的噪声和导线引入的噪声；能够得到较准确的传感器实际零点电压。这些都保证了这种方法可以获得更高的角度分辨率。与传统方法相比，可以在低频范围内获得很高的角度分辨率(本实施例采用分辨率为0.05度/秒的型号为Gyro1050的陀螺，模拟电压信号输出范围是0-5伏，经过上述处理后，其角度分辨率可以达到0.00025度)，并且可以通过改变滤波器的参数在分辨率和实时性间进行调整。

2.由于采用硅性MEMS陀螺，使本发明具有体积小、重量轻、灵敏度高、线形度好的特点，并且安装简便，不影响被测量物体的机械结构。

附图说明

图1a为数字滤波算法原理图(实际零点电压的获取)。

图1b为数字滤波算法原理图(姿态角度的计算)。

图2为本发明装置结构原理图。

图3为本发明装置信号处理流程图。

图4为图2中低通滤波器原理图。

图5为数据处理程序流程图。

具体实施方式

下面通过结合附图对本发明作进一步详细说明。

高分辨率姿态角度测量方法：取处于静止状态的角速率传感器输出的模拟电压信号，对模拟电压信号进行低通滤波去除高频部分，然后进行信号采集和模/数转换，将其转换为电压信号序列V[n]。数据处理程序按照最小均方差(LMS)算法对电压信号进行数字滤波，再经均值滤波，获得实际零点电压；再用实际零点电压与后续处于运动情况，依次经RC低通滤波、采集和模/数转换、最小均方差数字滤波及均值滤波的电压信号序列进行比较，对所得的差值积分得到姿态角度值。与传统方法相比，本发明可以在低频范围内获得很高的角度分辨率，并且可以通过改变数字滤波的参数在分辨率和实时性间进行调整。

所述最小均方差(LMS)数字滤波算法是通过如式(1)的迭代运算，根据数据不断修正自身的滤波系数w_i，以使滤波后的电压序列V’收敛于经过滤波的电压值V’和经采样转换后的电压序列V的均方差最小的值。通过调整系数更新幅度η和滤波器的长度M，可以调整收敛速度，从而在精度的实时性(收敛速度)间进行调整。本实施例幅度η＝0.01，滤波系数的长度M＝10。

公式为： ${V^{,}}_j=\underset{i=0}{\overset{M-1}{\mathrm{\Σ}}}{w}_i\·V_{j-M}---\left(1\right)$

其中：w_i＝w_i+η(V’_j-V_j-M)V_j-M，j＝M，M+1，...，n+M(M<N)

V是经采样转换后的电压序列；w是滤波系数；V’是经过滤波的电压值；η是系数更新幅度。

参见图1a，实际零点电压的计算过程：电压信号序列V[n]的前n个电压数据V[0，1，2，...，n-1]经过LMS数字滤波和均值滤波，得到实际零点电压Vc，代表角速率为零(静止时)的角速率传感器的输出的电压值，作为比较基准，后续处于运动情况且依次经RC低通滤波、采集和模/数转换、最小均方差数字滤波及均值滤波的电压信号序列与实际零点电压Vc偏差代表角速率传感器转动的角速率。公式如下：

ω＝(V’-Vc)/f                 (2)

其中：ω为角速率，单位是度/秒；V’是经过滤波的电压值；f是电压与角速率的转换关系，由角速率传感器说明书标明。

参见图1b，姿态角度的计算过程：后续经过采集和模/数转换的模拟电压信号数据V[n，n+1，n+2，...]先经过RC低通滤波、LMS滤波和均值滤波得到经过滤波的电压值V’，然后与实际零点电压Vc比较，对所得的差值进行数值积分(求和)即可得到高分辨率的姿态角度。公式如下：

α＝∑ω·Δt                          (3)

其中：α是角度值，单位为度；ω在式(2)中定义；Δt是采样周期，单位为秒。

参见图2，本发明高分辨率姿态角度测量装置：包括角速率传感器、低通滤波器、A/D采集转换模块和运行于计算机的数据处理程序。其中，角速率传感器安装时应使自身的轴向与被测物体的被测轴平行，处于静止/运动状态的角速率传感器的输出电压为0-X(伏)的模拟电压信号(本实施例X为5伏)，送至低通滤波器输入端，经过低通滤波器的低通滤波后送至A/D采集转换模块，经采集、量化后通过数据处理程序进行数字滤波和数值积分，得到具有高分辨率的姿态角度。

参见图3，本发明装置的信号处理过程如下：

角速率传感器输出的是零点电压为0-X(V)的模拟电压信号，经过低通滤波(RC电路)后，被去除高频部分，并被A/D采集转换模块进行数据采集和模/数转换得到成一个量化的电压信号序列V[n]，并按照最小均方差数字滤波算法对其进行数字滤波，结果再被均值滤波后，得到实际零点电压Vc。再将后续运动的传感器输出的模拟电压信号，即：依次经低通滤波(RC)、采集和模/数转换、最小均方差数字滤波及均值滤波的电压信号序列与Vc比较，对所得的差值序列进行比例变换和数值积分得到姿态角度值。

本实施例采用高灵敏度的硅性MEMS陀螺作为角速率传感器，用RC低通滤波器加数据处理程序的最小均方差数字滤波算法在低频范围内对其输出信号进行模拟和数字滤波，数值积分后获得高分辨率的角度信号。本实施例采用分辨率为0.05度/秒的型号为Gyro1050的陀螺，模拟电压信号输出范围是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]进行最小均方差数字滤波、均值滤波和数值积分，得出姿态角度值。两个线程之间是异步关系，同时执行，在数字滤波的同时不间断采样，提高了效率。

Claims (7)

1.一种高分辨率姿态角度测量方法，其特征是：取处于静止状态的角速率传感器输出的模拟电压信号，对模拟电压信号进行低通滤波去除高频部分，然后进行信号采集和模/数转换，将其转换为电压信号。数据处理程序按照最小均方差算法对电压信号进行数字滤波，经均值滤波，获得实际零点电压；再根据实际零点电压与后续处于运动情况，依次经RC低通滤波、采集和模/数转换、最小均方差数字滤波及均值滤波的电压信号进行比较，对所得的差值数值积分得到姿态角度值。

2.根据权利要求1所述高分辨率姿态角度测量方法，其特征是：模拟电压信号为0-X(伏)范围的电压值，X的值在大于零的正数。

3.根据权利要求1所述高分辨率姿态角度测量方法，其特征是：所述最小均方差算法通过如式(1)的迭代运算；

${V^{,}}_j=\underset{i-1}{\overset{M-1}{\mathrm{\&Sigma;}}}{w}_i\&CenterDot;V_{j-M}---\left(1\right)$

其中：w_i＝w_i+η(V’_j-V_j-M)V_j-M，j＝M，M+1，...，n+M(M<N)

V是经采样转换后的电压序列；w是滤波系数；V’是经过滤波的电压值；η是系数更新幅度；M为滤波系数的长度。

4.一种高分辨率姿态角度测量装置，其特征在于包括：

角速率传感器，安装时使自身的轴向与被测物体的被测轴平行；

低通滤波器，接收处于静止/运动状态的角速率传感器输出的模拟电压信号，对模拟电压信号除噪处理后送至A/D采集转换模块；

A/D采集转换模块，进行数据采集、模/数转换；

运行于计算机的数据处理程序，进行数字滤波和数值积分。

5.根据权利要求4所述高分辨率姿态角度测量装置，其特征在于：所述数据处理程序流程是一个多线程程序，具有两个线程；主线程负责对信号不间断地采集转换，辅助线程负责检测A/D采集转换模块上先进先出缓存的状态，当其达到或超过半满时进行最小均方差数字滤波、均值滤波和数值积分，得出姿态角度值。

6.根据权利要求5所述高分辨率姿态角度测量装置，其特征在于：两个线程之间是异步关系，同时执行。

7.根据权利要求4所述高分辨率姿态角度测量装置，其特征在于：所述角速率传感器采用高灵敏度的硅性MEMS陀螺。

Images