一种消除云台航向轴漂移以及电机磁场干扰的方法与流程

本发明涉及增稳设备的控制与姿态解算方法，常见应用与手持三轴增稳云台、飞行器增稳云台航向角控制领域中。

背景技术：

目前的增稳设备中航向角往往因为电机磁场的影响，或者加速度计解算不出航向角的原因，往往只采用陀螺仪来进行增稳设备的航向轴的姿态解算，其结果是可以满足动态响应的要求，但是在静止情况下由于陀螺仪的零漂往往会造成航向轴严重漂移的问题，因此需要一个既可以消除电机磁场影响又可以解决陀螺仪零漂的方法。

技术实现要素：

本发明的目的是针对上述现有技术中的不足，提出了一种增稳设备中航向角姿态解算的方法。

本发明是通过以下技术方案实现的：陀螺仪检测转子的角速度，加速度计检测电机底座连接柄所处的加速度参数，磁力计检测电机底座连接柄所处的磁场参数，加速度计参数和磁力计参数融合计算之后可以得到准确的电机底座连接柄航向角数据MagAngle，由于电机底座与转子并不同轴，所以只靠电机底座连接柄的航向角数据MagAngle并不能得到电机转子的航向角数据YAW，引入电机转子位置传感器检测电机转子相对于电机底座的位置MotorAngle，电机底座连接柄航向角数据MagAngle与电机转子位置MotorAngle相结合以后就可以得到电机转子的航向角YAW，由于MagAngle和MotorAngle融合得到的航向角YAW存在噪声所以引入了电机转子的角速度Gyro,YAW再与Gyro进行角度融合滤波即可以得到准确的航向轴数据RealYAW，然后参与控制器的计算。计算逻辑如下：增稳设备启动时记录下来此时磁力计与加速度计解算出来的角度MagAngle和电机转子初始位置MotorAngle，并将MotorAngle角度数据归零，接着根据电机转子位置MotorAngle的增量和MagAngle的值解算出电机转子相对地球磁场的真正航向角RealYAW，再结合转子角速度Gyro进行一阶滤波或者卡尔曼滤波得到比较准确无噪声的航向角。

本发明中的转子位置检测传感器可以有多种方式检测：霍尔传感器，电位器，磁性编码器等等。

本方案中的原始航向角和陀螺仪角速度的角度融合算法可以用一阶互补滤波，卡尔曼滤波，或者AHRS融合算法。

本发明中的陀螺仪放置在转子位置，磁力计加速度计放置在电机底座连接柄的末端远离了电机磁场，有与传统方式不一样的机械结构和传感器放置位置。

本发明中与只用磁力计加速度计陀螺仪来获取航向角的传统方法不一样，引入了电机转子位置传感器，再结合磁力计加速度计进行间接的角度转换再融合陀螺仪角速度得到航向角。

附图说明

图1是本发明的机械电子结构结构示意图，系统中包括无刷电机，转子位置传感器，电机驱动，控制器，处理器，三轴陀螺仪，三轴加速度计，三轴磁力计。图 2是本发明的一个实例应用系统结构图。

具体实施方式

下面结合附图及实施例对本发明作进一步说明附图1为系统结构图:，图2为具体实现时用的具体器件。图2中控制器1为Ti公司的数字信号处理器DSP，电机驱动的控制器2为ST公司的32位单片机，电机转子位置检测传感器3为磁性旋转编码器或者旋转电位器，无刷电机4，电机驱动电路5为以MOS管搭建的三路全桥驱动，放置在转子上的三轴陀螺仪6，放置在电机底座连接柄末端的三轴加速度计三轴磁力计7。

本实例中的控制器是采用DSP，具有数字信号处理能力强特点，驱动电路是采用常见的经典无刷电机驱动电路，驱动电路的控制器采用ST公司的STM32处理器，磁性旋转编码器可以实时检测电机转子角度，有效数据输出频率可达10kHZ，能完全达到实时性的要求，分辨率为12位可达0.08度，MPU6050采用MEMS技术可以采集三轴加速度数据三轴陀螺仪数据，输出速率可达1KHZ满足控制频率，三轴磁力计输出可达10HZ。控制器DSP28335通过I2C协议与处理器连接传输数据，磁性编码器通过SPI协议与处理器通信，三轴加速度数据三轴陀螺仪通过I2C协议与DSP通信，最终各方数据在DSP里融合计算输出给STM32，STM32将驱动信号传输至电机驱动实现控制系统的正常运行。

使用该方案时将磁性编码器放置在电机轴中央检测转子，或者转子连接旋转电位器进行转子位置检测。

该方案的机械结构与传统方式不同，磁力计加速度计放置在电机底座连接柄的末端，原理电机磁场干扰，并且航向角的解算又引入了转子位置数据，通过间接的运算测量，得到转子的航向角。

以上是本发明的较佳实施例子，凡依本发明技术方案所作的改变，所产生的功能作用未超出本发明技术方案的范围时，均属于本发明的保护范围。