CN103822645B - 一种角度融合算法验证校正方法 - Google Patents

Abstract

本发明涉及一种角度融合算法验证校正方法，建立验证校正系统：利用电机驱动单摆运动，设定摆动频率来模拟使用环境，通过单摆同轴的角度传感器和加速度传感器、陀螺仪融合算法得到角度对比，来校正角度融合算法的各个参数。解决实际工程应用中角度融合算法的验证与校正，可满足一般工程实践要求，并且操作灵活方便、可靠性高。

Description

一种角度融合算法验证校正方法

技术领域

本发明涉及一种角度测试校正技术，特别涉及一种角度融合算法验证校正方法。

背景技术

目前，基于惯性传感器的姿态检测系统已被广泛应用于飞行器姿态控制、振动系统分析等工程实践上，为人们解决如何获取物体三维空间内的姿态问题提供了一种有效的方法，但是同时，这种基于惯性传感器的角度融合与姿态获取方案的实际误差往往不容易获得，在有些实际工程应用中会使用基于双GPS和光纤陀螺的高成本、高精度姿态检测系统，这对于一般工程应用而言造价太高，而且实验条件要求苛刻，不灵活。

发明内容

本发明是针对基于惯性传感器的姿态检测系统中角度融合验证精度要求高、成本高的问题，提出了一种角度融合算法验证校正方法，解决实际工程应用中角度融合算法的验证与校正，提供一种可靠的实验验证校正系统和角度读取对比方法。

本发明的技术方案为：一种角度融合算法验证校正方法，具体包括如下步骤：

1）建立验证校正系统：在水平平台同轴依次固定安装高精度角度传感器、电机与电机座、同轴联接器、第一轴承座、第二轴承座、联接器，联接器轴上挂摆杆，摆杆低端有摆锤，加速度传感器和陀螺仪放置在摆锤上；

2）微控制器设定摆动频率和最大摆动角度送电机驱动模块，电机开始工作，微处理器读取与单摆同轴相连的高精度角度传感器的测得数据，微控制器根据设定，期望角度值算法为，其中为设定的最大摆动角度，f为设定摆动频率，设定中断时间为1ms，n为进入中断的次数，角度传感器获得的数据转化为的角度值和期望角度值之差，经过PID算法处理，得到相应的PWM信号输出控制电机转动方向、角度，实现摆臂摆动频率与用户设定摆动频率相吻合；

3）微控制器采集摆锤上加速度传感器和陀螺仪上数据送角度融合算法处理后作为摆动的实时角度，同时微处理器读取与单摆同轴相连的高精度角度传感器的测得数据，作为实际角度；

4）微处理器将采集此刻的实时角度和实际角度经USART转USB模块转换后送上位机；

5）上位机将步骤4）得到的实时角度和实际角度进行对比，对角度拟合算法的计算参数进行调整，重新设定摆动频率和最大摆动角度，重复步骤2）到步骤4），得到不同频率下的对比值；

6）根据步骤5）所得各种摆动频率下的参数调整，对角度融合算法的动态、静态响应效果进行评估。

本发明的有益效果在于：本发明角度融合算法验证校正方法，独立、可靠、方便的角度融合算法实验验证校正系统，可满足一般工程实践要求，并且操作灵活方便、可靠性高。

附图说明

图1为本发明角度融合算法验证校正系统结构示意框图；

图2为本发明通过三轴加速度计获得某轴角度示意图；

图3为本发明角度融合算法示意图；

图4为本发明角度融合算法验证校正系统机械连接图；

图5为单摆摆动部分简化模型图；

图6为本发明角度融合算法验证校正系统中正弦函数傅里叶变换波形图；

图7为本发明上位机读取实际角度和拟合角度的效果；

图8为本发明改变融合算法中的放大系数得到的数据效果图；

图9为本发明角度融合算法实验验证系统软件流程图；

图10为本发明单摆摆动部分系统方框图。

具体实施方式

如图1所示角度融合算法验证校正系统硬件连接示意图，角度传感器与加速度、陀螺仪模块送入微控制器进行处理，按键模块将设定数据送入微处理器，微处理器输出控制信号通过伺服电机及电机驱动模块驱动摆臂运动，同时将计算数据经USART转USB模块转换后送上位机，电源模块给微处理器供电，微控制器内有高分辨率模数转换功能。

上位机用于显示角度传感器测量的角度值与角度融合算法处理后的角度值，角度传感器模块测量的单摆摆动角度，作为标准角度数值，用于校正角度融合算法的参数。用户可通过微控制器以及周边功能电路实现设定单摆的摆动频率。

一种通过惯性传感器获取单轴摆动角度的方法。在本系统特殊的安装环境下，使用三轴加速度计和三轴陀螺仪进行角度获取时，只需要用到三轴加速度两轴数据和陀螺仪一轴数据。利用加速度计获取单轴角度计算方法如图2所示。

例如，当三轴加速度计沿Y轴即地球水平方向，偏转一定角度时，在地球Z轴分量上加速度=，X轴上对应加速度=（g为重力加速度），、为三轴加速度计Z轴、X轴数据，可通过微处理器直接读取对应寄存器获得。根据三角函数原理有，，使用这种计算方法比通过使用可获得更高的计算精度，原因在于该算法可以避免由于物体运动在X、Z上产生的附加加速度直接参与运算。

通过陀螺仪获取角度时，通过积分算法获得，其中，第一个代表通过陀螺仪获得的角度最新值，第二个代表通过陀螺仪获得的上一次角度值，为本次陀螺仪采集的角加速度的数值，陀螺仪采样周期。

获得三轴加速度计和陀螺仪的数据后进行滤波和融合算法以获得更高精度的角度值，本发明中采用了不同于卡尔曼滤波和其他融合方法的融合算法。如图3所示角度融合算法示意图，该算法可视为包含两个一阶惯性环节，第一项物理意义为加速度传感器得出的角度值经一阶惯性环节滤波得到的角度值，第二项相当于陀螺仪输出的角加速度积分后形成的角度值与第一项得到的角度值相减后乘以比例系数再与陀螺仪输出的角加速度相加积分输出最终角度值，该项用于减小陀螺仪角速度经积分后的误差放大作用。通过该算法可降低加速度计的大范围跳变值对动态性能的影响，同时又能避免积分下陀螺仪随机漂移对静态性能的影响。在实际处理中，比例环节比例系数a决定最终角度依赖陀螺仪数据和加速度数据两者的比重。

一种角度对比的方法。是实验过程中，用户将惯性传感器安放在单摆上，在该情况下，陀螺仪或者三轴加速度计是在绕单独某一轴运动，因此可以减少工作量，便于用户调试。

如图4所示角度融合算法验证校正系统机械连接图，在水平平台上同轴依次固定安装角度传感器1、电机与电机座2、同轴联接器3、第一轴承座4、第二轴承座5、联接器6，联接器6轴上挂摆杆7，摆杆7低端有摆锤8，加速度传感器和陀螺仪放置在摆锤8上。其中角度传感器与电机已通过电机自带同轴联接联接，电机前轴与水平轴通过同轴联接器联接，水平轴与垂直方向的摆锤通过联接器联接。

微控制器采集摆锤8上加速度传感器和陀螺仪上数据并将数据发送至上位机，同时发送经角度融合算法处理后的角度值到上位机，微处理器读取与单摆同轴相连的高精度角度传感器的测得数据，作为实际角度送上位机，通过角度融合算法处理后的角度值与实际角度对比方式可以直观显示出使用角度融合算法得出的角度和实际角度之间的误差，方便修正角度拟合算法的计算参数，并对拟合算法的动态、静态响应效果进行评估。本发明中，用户可以通过键盘设定需要的单摆摆动频率、单摆摆动的最大角度。微处理器中设置时间间隔为1ms定时器中断，通过算法计算此时理论的角度期望值，其中n为进入中断的次数，当一周期结束会被清零，下个周期重新计数，正弦函数由C编译器标准库<stdlib.h>提供，的目标为用户设置的摆动频率，为用户设置的摆动最大角度。该期望角度值与角度传感器的测量的角度值形成角度误差，对该误差进行PID运算产生对应的脉冲波进而控制伺服电机使摆杆处于期望的位置。同时也可将单摆设置为自由摆动状态。

以自由摆动状态为例，对发明的工作流程进行分析，以解释在发明中进行摆动运动的必要性。将本发明系统简化，如图5所示单摆摆动部分简化模型图。其中y代表摆长，在不改变使用位置情况下决定了自由摆动时的摆动周期，代表单摆摆动的最大角度。通过使用轴承和增大单摆自身重量来减少空气阻力以及其他阻尼对于系统的影响。

单摆的运动方程为，（代表单摆的摆动角度，代表摆动角度的二阶微分，该公式由刚体转动的角速度守恒定律得，用于分析摆杆摆动的运动特性）。在小范围角度范围内，单摆周期经推导可得，理想情况下单摆摆动周期与角度的关系，为正弦或者余弦函数。

在进行传感器研发测试中，常用的方法是静态测试和动态测试，静态测试即传感器及其载体处于静止状态，动态测试即传感器及其载体处于某种频率的运动状态，实际应用场合中，动态环境运动频率在30-60Hz内。在动态测试中，原有的实验方法往往无法保证实验所用传感器的动态变化频率处于单一数值，这不便于研究角度拟合算法在不同频率下的实际响应效果。对于角度拟合算法，不同的使用环境对应着不同的计算参数。在单摆上，通过对单摆摆动角度与时间关系的函数进行傅里叶变换，图6中右图所示为理论傅里叶变换图（结果），图6中左图（左半部分）显示系统处于正弦（或者余弦）运动时其频率只包含单一频率。即，此时传感器和摆一起处于单一变化频率下，在该环境下得到的数据与标准角度数据进行对比校正以改善融合算法动态特性。

如图7所示上位机读取实际角度和拟合角度的效果图，其中曲线1代表电位计角度传感器获得的高精度摆动角度，曲线2为角度融合算法计算后的角度值。

图8是通过对图7数据分析后改变融合算法中的比例系数a得到的数据，其中曲线3代表电位计角度传感器获得的高精度摆动角度，曲线4为角度融合算法计算后的角度值，其动态跟随性明显得到改善。

图9为角度融合算法验证校正系统软件流程图。用户进入菜单，进行摆动频率和最大摆动角度设定，由角度传感器，微处理器，电机及其驱动电路构成角度控制回路，其中误差为与角度传感器数据转换而来的角度值之差。误差会在处理器中经过PID算法处理，得到相应的PWM波用于控制电机转动方向、角度，最终实现摆臂摆动频率与用户设定摆动频率相吻合。单摆摆动算法执行过程如图10系统方框图所示。同时角度传感器拟合后的角度值与角度传感器采集的实际角度值将通过USART接口发送至上位机进行显示。

Claims (1)

1.一种角度融合算法验证校正方法，其特征在于，具体包括如下步骤：

1）建立验证校正系统：在水平平台同轴依次固定安装高精度角度传感器、电机与电机座、同轴联接器、第一轴承座、第二轴承座、联接器，联接器轴上挂摆杆，摆杆低端有摆锤，加速度传感器和陀螺仪放置在摆锤上；

2）微控制器设定摆动频率和最大摆动角度送电机驱动模块，电机开始工作，微处理器读取与单摆同轴相连的高精度角度传感器的测得数据，微控制器根据设定，期望角度值算法为，其中为设定的最大摆动角度，f为设定摆动频率，设定中断时间为1ms，n为进入中断的次数，角度传感器获得的数据转化为的角度值和期望角度值之差，经过PID算法处理，得到相应的PWM信号输出控制电机转动方向、角度，实现摆臂摆动频率与用户设定摆动频率相吻合；

3）微控制器采集摆锤上加速度传感器和陀螺仪上数据送角度融合算法处理后作为摆动的实时角度，同时微处理器读取与单摆同轴相连的高精度角度传感器的测得数据，作为实际角度；

4）微处理器将采集此刻的实时角度和实际角度经USART转USB模块转换后送上位机；

5）上位机将步骤4）得到的实时角度和实际角度进行对比，对角度拟合算法的计算参数进行调整，重新设定摆动频率和最大摆动角度，重复步骤2）到步骤4），得到不同频率下的对比值；

6）根据步骤5）所得各种摆动频率下的参数调整，对角度融合算法的动态、静态响应效果进行评估。