#第一版
import math
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
微型四軸飛行器（5）九軸姿態融合演算法A
所謂的九軸姿態融合就是將通過感測器獲得的3軸加速度 3軸角速度 3軸磁場資料，在相應的演算法處理後能夠得到飛行器的姿態資訊 尤拉角 輸入輸出如下圖 在慣性導航領域的尤拉角分別表示的是航向角 yaw 橫滾角 roll 俯仰角 pitch 我們擬建一空間直角座標系，在該座標系中，物體做出的任何姿態同樣也...
微型四軸飛行器（5）九軸姿態融合演算法B
飛行器在空中的執行姿態可以用平面和轉動來表示，為了方便使用向量表示，需要建立兩個空間直角座標系。設r表示單位向量在機體座標系下的三個軸的投影，b表示單位向量在地球座標系下的三個軸的投影。我們通常對飛行器的偏航 俯仰 橫滾了多少度的定義是參照地球座標系而下得出的，也就是我們只需要知道向量r相較於向量b...
對四元數解算姿態的理解
問題 為什麼不用尤拉角來表示旋轉而要引入四元數呢?前面介紹了什麼是尤拉角，而且尤拉角微分方程解算姿態關係簡單明瞭，概念直觀容易理解，那麼我們為什麼不用尤拉角來表示旋轉而要引入四元數呢？一方面是因為尤拉角微分方程中包含了大量的三角運算，這給實時解算帶 來了一定的困難。而且當俯仰角為90度時方程式會出現...