树莓派pico mpu6050 一阶互补滤波&四元数法 解算姿态角

• micro-python:一阶互补滤波&四元数法 代码

本系统由供电部分, 主控部分, 姿态传感器与通信部份组成

电池为一节14500锂电池, 容量1300mAh, 额定电压3.7V. 该电池连接一个AMS1117降压电路, 得到稳定的3V3输出, 为后续系统供电.

树莓派Pico是树莓派基金会自主研发的一款双核MCU:RP2040，提供了超低功耗、极低的I/O延迟，可用micro-python或C语言编写程序。

RP2040 芯片是 7×7mm QFN-56 封装的，具体规格参数如下：

• 双核 Arm Cortex-M0 + @ 133MHz
• 芯片内置 264KB SRAM 和 2MB 的板载闪存
• 通过专用 QSPI 总线支持最高 16MB 的片外闪存
• DMA 控制器
• 30 个 GPIO 引脚，其中 4 个可用作模拟输入
• 2 个 UART、2 个 SPI 控制器和 2 个 I2C 控制器
• 16 个 PWM 通道
• USB 1.1 主机和设备支持
• 8 个树莓派可编程 I/O（PIO）状态机，用于自定义外围设备支持
• 支持 UF2 的 USB 大容量存储启动模式，用于拖放式编程

HC05模块是ALIENTEK一款高性能主从一体蓝牙串口模块，支持非常宽的波特率范围：4800~1382400, 可以同各种带蓝牙功能的电脑、蓝牙主机、手机、PDA、PSP等智能终端配对

InvenSense公司推出的全球首款整合性6轴运动处理组件,输出三轴加速度ax,ay,az和三轴角速度gx,gy,gz,体积4x4x9mm,i2c通信,量程范围角速度±250，±500，±1000，±2000°/sec(dps),加速度±2g，±4g，±8g，±16g

加速度计的特点是测量比较准确, 但容易受到噪声的影响, 比如附近有电机转动时, 加速度输出值会有剧烈波动; 陀螺仪的特点是不受到高频噪声的影响, 但数值会有漂移, 时间越长, 误差越大.

两个传感器可以弥补相互缺点,单纯以六轴传感器不能得到准确的Yaw角,但在跌倒场景中,Yaw角并不是特别重要.

比如对于pitch角(绕y轴旋转角),由加速度计计算得到的绕y轴角度(x轴转过的角度) a n g l e A y = a x a y 2 + a z 2 angleAy=frac{ax}{sqrt{ay^2+az^2}} angleAy=ay2+az2 ​ax​ ,由陀螺仪计算得到的绕y轴角度 a n g l e G y = l a s t p i t c h + g y × d t angleGy=last_{pitch}+gytimes dt angleGy=lastpitch​+gy×dt

定义k代表加速度计的权重,1-k即陀螺仪的权重,最终得到 p i t c h = k × a n g l e A y + ( 1 − k ) × a n g l e G y pitch=ktimes angleAy+(1-k)times angleGy pitch=k×angleAy+(1−k)×angleGy

此外还有其他方法可以得到三轴姿态角:

• 四元数

• 卡尔曼滤波

• 数字运动处理器DMP
  

• MPU6050 抄底解读

• MPU6050 姿态解算系列一：加速度姿态解算

• MPU6050 姿态解算系列二：陀螺仪姿态解算

• MPU6050 姿态解算系列三：互补滤波

• MPU6050 姿态解算系列四：线性 Kalman 滤波

• micropython-mpu6050-esp8266

• MPU6050误差分析