四元数法求姿态角，九轴

coding:utf-8

四元数法

9轴计算

#第一版

import math

IMU算法更新

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

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

# 传感器框架相对于辅助框架的四元数(初始化四元数的值)
q0 = 1
q1 = 0
q2 = 0
q3 = 0

# 由Ki缩放的积分误差项(初始化)
exInt = 0
eyInt = 0
ezInt = 0

q0q0 = q0*q0
q0q1 = q0*q1
q0q2 = q0*q2
q0q3 = q0*q3
q1q1 = q1*q1
q1q2 = q1*q2
q1q3 = q1*q3
q2q2 = q2*q2
q2q3 = q2*q3
q3q3 = q3*q3


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

norm = math.sqrt(mx * mx + my * my + mz * mz)
mx = mx / norm
my = my / norm
mz = mz / norm


hx = 2 * mx * (0.5 - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2)
hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5 - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1)
hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2)
bx = math.sqrt((hx * hx) + (hy * hy))
bz = hz

# 估计方向的重力
vx = 2 * (q1q3 - q0q2)
vy = 2 * (q0q1 + q2q3)
vz = q0q0 - q1q1 - q2q2 + q3q3
wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2)
wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3)
wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2)

# 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex = (ay * vz - az * vy) + (my * wz - mz * wy)
ey = (az * vx - ax * vz) + (mz * wx - mx * wz)
ez = (ax * vy - ay * vx) + (mx * wy - my * wx)

# 积分误差比例积分增益
exInt += ex * Ki*1/50
eyInt += ey * Ki*1/50
ezInt += ez * Ki*1/50

# 调整后的陀螺仪测量
gx += Kp * ex + exInt
gy += Kp * ey + eyInt
gz += Kp * ez + ezInt

# 整合四元数
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT
q3 = 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.atan((2 * q2 * q3 + 2 * q0 * q1)/(-2 * q1 * q1 - 2 * q2 * q2 + 1)) * 57.3
yaw = math.atan((2 * q1 * q2 + 2 * q0 * q3)/(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3)) * 57.3
return pitch, roll, yaw