【花雕学编程】使用卡尔曼滤波器处理AS5600磁编码器数据

37款传感器与执行器的提法，在网络上广泛流传，其实Arduino能够兼容的传感器模块肯定是不止这37种的。鉴于本人手头积累了一些传感器和执行器模块，依照实践出真知（一定要动手做）的理念，以学习和交流为目的，这里准备逐一动手尝试系列实验，不管成功（程序走通）与否，都会记录下来——小小的进步或是搞不掂的问题，希望能够抛砖引玉。

【Arduino】168种传感器模块系列实验（资料代码+仿真编程+图形编程）
实验二百二十五：AS5600磁编码器 磁感应角度测量传感器 12bit高精度模块

核心芯片：AS5600
是一款易于编程的磁性旋转位置传感器，具有高分辨率 12 位模拟或 PWM 输出。这种非接触式系统测量径向磁化同轴磁体的绝对角度。这款 AS5600 专为非接触式电位计应用而设计，其坚固的设计消除了任何均匀的外部杂散磁场的影响。行业标准的I²C接口支持用户对非易失性参数进行简单编程，无需专门的编程器。此外，该器件还实现了在所谓的“3线模式”中轻松进行启动和停止位置编程，无需编程器或数字接口。输出的默认范围是 0 到 360 度。AS5600 可以通过编程零角度（开始位置）和最大角度（停止位置）来应用于较小的范围。AS5600 还配备了智能低功耗模式功能，可自动降低功耗。

Arduino实验接线示意图

  【Arduino】168种传感器模块系列实验（资料代码+仿真编程+图形编程）
  实验二百二十五：AS5600磁编码器 磁感应角度测量传感器  12bit高精度模块
  项目实验之十九：使用卡尔曼滤波器处理AS5600编码器数据

实验开源代码

/*
  【Arduino】168种传感器模块系列实验（资料代码+仿真编程+图形编程）
  实验二百二十五：AS5600磁编码器 磁感应角度测量传感器  12bit高精度模块
  项目实验之十九：使用卡尔曼滤波器处理AS5600编码器数据
*/

#include <Wire.h>
#include <SimpleFOC.h>
#include <AS5600.h>

// 使用SimpleFOC库中的低通滤波器
LowPassFilter as5600_filter(0.01); // 创建一个低通滤波器实例，滤波系数为0.01
AS5600 encoder; // 创建一个AS5600编码器实例

// 定义卡尔曼滤波相关变量
float measured_angle = 0; // 测量的角度
float estimated_angle = 0; // 估计的角度
float Q_angle = 0.001; // 过程噪声
float R_measure = 0.03; // 测量噪声
float P = 1; // 估计误差协方差
float K; // 卡尔曼增益

void setup() {
  Serial.begin(115200); // 初始化串口通信，波特率为115200
  Wire.begin(); // 初始化I2C通信
  encoder.begin(); // 初始化AS5600编码器
  Serial.println("已完成初始化I2C，Baize_FOC准备就绪"); // 输出初始化完成信息
}

void loop() {
  uint16_t readValue = 0; // 存储读取的原始数据
  byte readArray[2]; // 存储读取的字节数据

  // 通知设备即将读取数据
  Wire.beginTransmission(0x36); // 开始与I2C地址为0x36的设备通信
  Wire.write(0x0C); // 写入寄存器地址0x0C，准备读取角度数据
  Wire.endTransmission(false); // 结束传输，但保持I2C总线激活状态

  // 读取数据的MSB和LSB
  Wire.requestFrom(0x36, (uint8_t)2); // 请求从设备读取2个字节的数据
  if (Wire.available() == 2) { // 检查是否有2个字节可用
    readArray[0] = Wire.read(); // 读取第一个字节（MSB）
    readArray[1] = Wire.read(); // 读取第二个字节（LSB）
    readValue = (readArray[0] << 8) | readArray[1]; // 将两个字节合并为一个16位的值
    float raw_angle = (float)readValue / 4096.0 * 360.0; // 将原始数据转换为0-360度
    float filteredValue = as5600_filter(raw_angle); // 对角度数据进行低通滤波

    // 卡尔曼滤波
    measured_angle = filteredValue; // 更新测量的角度

    // 预测步骤
    P = P + Q_angle; // 更新估计误差协方差

    // 更新步骤
    K = P / (P + R_measure); // 计算卡尔曼增益
    estimated_angle = estimated_angle + K * (measured_angle - estimated_angle); // 更新估计的角度
    P = (1 - K) * P; // 更新估计误差协方差

    // 确保角度在0-360度范围内
    if (estimated_angle < 0) {
      estimated_angle += 360;
    } else if (estimated_angle >= 360) {
      estimated_angle -= 360;
    }

    // 输出滤波后的角度
    Serial.print("卡尔曼滤波后的角度: ");
    Serial.println(estimated_angle);
  } else {
    Serial.println("Error reading data"); // 如果读取数据失败，输出错误信息
  }
  delay(50); // 延迟50毫秒
}
复制代码

实验串口返回情况



要点解读1、初始化与配置：在setup()函数中，初始化串口通信、I2C总线和AS5600编码器，确保系统准备就绪。2、数据读取：通过I2C总线从AS5600磁编码器读取角度数据，确保读取的MSB和LSB数据正确组合成16位的角度值。3、角度转换：将AS5600编码器的原始数据转换为0-360度范围内的角度值，便于后续处理和输出。4、低通滤波：使用SimpleFOC库中的低通滤波器对读取的角度数据进行滤波，减少噪声，提高数据的平滑度。5、简化的卡尔曼滤波算法：简化后的卡尔曼滤波算法去掉了复杂的矩阵运算，适合在资源有限的Arduino上实现。包括预测和更新步骤，以提高角度测量的精度，并确保角度值在0-360度范围内。

实验串口绘图器返回情况

实验场景图