IMU 算法更新

filename = 'data.txt' # 文件名，请自行修改，绝对路径或相对路径都可以

data = [] with open(filename) as f: for line in f: data.append(line.strip().split(','))

func = lambda i: [float(x[i]) for x in data[1:]]

Kp = 10 # 比例增益控制加速度计/磁强计的收敛速度 Ki = 0.002 # 积分增益控制陀螺偏差的收敛速度 halfT = 0.001 # 采样周期的一半

传感器框架相对于辅助框架的四元数 (初始化四元数的值)

q0 = 1 q1 = 0 q2 = 0 q3 = 0

由 Ki 缩放的积分误差项 (初始化)

exInt = 0 eyInt = 0 ezInt = 0

def Update_IMU(ax, ay, az, gx, gy, gz): global q0 global q1 global q2 global q3 global exInt global eyInt global ezInt # print(q0)

# 测量正常化

norm = math.sqrt(ax * ax + ay * ay + az * az) # 单元化 ax = ax / norm ay = ay / norm az = az / norm

# 估计方向的重力

vx = 2 * (q1 * q3 - q0 * q2) vy = 2 * (q0 * q1 + q2 * q3) vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3

# 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和

ex = (ay * vz - az * vy) ey = (az * vx - ax * vz) ez = (ax * vy - ay * vx)

# 积分误差比例积分增益

exInt += ex * Ki eyInt += ey * Ki ezInt += ez * Ki

# 调整后的陀螺仪测量

gx += Kp * ex + exInt gy += Kp * ey + eyInt gz += Kp * ez + ezInt

# 整合四元数

q0 += (-q1 * gx - q2 * gy - q3 * gz) * halfT q1 += (q0 * gx + q2 * gz - q3 * gy) * halfT q2 += (q0 * gy - q1 * gz + q3 * gx) * halfT q3 += (q0 * gz + q1 * gy - q2 * gx) * halfT

# 正常化四元数

norm = math.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3) q0 /= norm q1 /= norm q2 /= norm q3 /= norm

# 获取欧拉角 pitch、roll、yaw

pitch = math.asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3 roll = math.atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3 yaw = math.atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3 return roll, pitch, yaw

基于四元数的姿态解算，为什么要用到 PID 内容：

基于四元数的姿态解算本质上是一种估计算法，它通过将测量的加速度计和磁强计数据与陀螺仪数据结合起来，估计出物体在空间中的姿态（即旋转角度和旋转轴）。然而，由于传感器的误差和噪声等原因，估计出的姿态往往存在误差。为了减小这些误差，需要使用反馈控制算法对估计出的姿态进行调整，PID 控制就是一种常用的反馈控制算法。

PID 控制器通过比较目标值和实际值之间的差异，计算出控制量，从而使实际值逐渐趋近于目标值。在基于四元数的姿态解算中，PID 控制器可以用来调整陀螺仪测量值，从而减小估计出的姿态与实际姿态之间的误差。具体来说，PID 控制器中的比例增益控制加速度计和磁强计的收敛速度，积分增益控制陀螺偏差的收敛速度，通过调整陀螺仪测量值来减小估计误差。因此，PID 控制器可以在基于四元数的姿态解算中起到优化估计算法的作用，提高姿态解算的精度和稳定性。