AHRS互补滤波（Mahony）算法及开源代码
文章目录
-
- 前言
- 算法流程
- 算法步骤
- 算法难点
- 开源源码
- 参考文献
前言
AHRS(Attitude and heading reference system，也就是航姿参考系统。在互补滤波算法中传感器主要采用了IMU（陀螺仪、加速度计）和磁力计。
AHRS的基本思想是，在载体没有平移运动的情况下，通过加速度感知重力分量，可以计算出载体的俯仰和横滚；磁力计可以感知磁北方向，因此可以计算载体的磁北航向；而陀螺仪测量输出载体的旋转角速度，通过积分可以计算得到横滚、俯仰、航向增量，但由于陀螺输出值含有误差，采用积分计算，误差会随着时间累积。陀螺仪动态响应特性良好，但计算姿态时会产生累积误差， 磁力计和加速度计测量姿态没有累积误差，但动态响应较差，那么采用加速度计和磁力计的即时输出值对陀螺进行修正，则可以达到优势互补的效果，提高测量精度和系统的动态性能。
算法流程
整体流程如下图，只不过图里面的公式用的四元数做旋转，而不是旋转矩阵。
算法步骤
所用的导航系N（地理系）为北东地，机体系B为前右下。假设加速度计测量值为ax,ay,az，陀螺测量值为gx,gy,gz，磁力计测量值为mx,my,mz。
1. 初始化四元数
由于姿态角和四元数是可以相互计算的，因此可以首先根据已知的姿态，或者根据加速度计和磁力计计算出来的姿态初始化四元数，具体公式如下。
2. 计算加速度计修正量
由四元数表示的从导航系到机体系的旋转矩阵为：
由于我用的导航系N是北东地，机体系为前右下，如果加速度计坐标系与机体系一致，载体静止且水平时加速度计测量值应为[0,0,-1]，Z方向为负（由于加速度计测的实际是1g的支持力，而非重力）。利用Cnb矩阵将地球矢量转到机体系前右下，得到vx, vy, vz。
将vx,vy,vz与实际加速度计输出（位于机体系中）求向量积，即为误差修正量:
3. 计算磁力计修正量
如果不考虑误差，磁力计的测量值经过正确Cbn转到地理系，理论上水平方向上只有北向有值，因此地球磁场的切线在南北方向上。但实际上由于航向不准，造成Cbn有误差，所以转换后的磁力计测量值在北向和东向都有值。
互补滤波算法中，先将磁力计的值用Cbn转到地理系：
由于在水平方向，无论Cbn在航向上有没有误差，转换后水平方向矢量和应该相等。因此可以得到如果航向正确的情况下，通过Cbn转换后的磁力计为[bx, 0, bz] = [sqrt(hx * hx + hy * hy), 0, hz]，再将这个值转到机体系下：
求向量积：
4. 修正陀螺仪输出值
根据pi调节，设置对陀螺测量值的修正量：
对陀螺仪测量值进行修正：
后面就按照正常四元数更新算法进行计算。
算法难点
我感觉难点在于怎么设置kp，ki这两个值，可能需要比较多的PID调节的经验和测试数据。
在参考【1】中提到：“关于这一块，现在研究的比较多就是如何实现自适应调参。固定的参数不能获得所有情况下的最优运动姿态角，可以设计参数可调的自适应算法在不同运动状态下进行调节参数的大小。其参数调节规则为：正常运动状态情况下，Kp和Ki值取为系统初始化值；当运动体具有较大运动加速度或姿态变化剧烈时，应选择较小的Kp值（可取其初始化值的0.1倍），而Ki值应在同一数量级内适当取大一点。具体取值需根据实际应用系统选取。”
严恭敏老师的博客最简单的航姿仪算法C程序（AHRS）中设置的kp,ki值为：
void MahonyInit(float tau)
{
float beta = 2.146f/tau;
Kp = 2.0f*beta, Ki = beta*beta;
q0 = 1.0f, q1 = q2 = q3 = 0.0f;
qua2cnb();
exInt = eyInt = ezInt = 0.0f;
tk = 0.0f;
}
Aceinna开源代码中设置的kp,ki值为：
def __init__(self):
'''
vars
'''
# algorithm description
self.input = ['fs', 'gyro', 'accel']#, 'mag']
self.output = ['att_quat', 'wb', 'ab']
self.batch = True
self.results = None
self.quat = None
self.wb = None
self.ab = None
# algorithm vars
# config
self.innovationLimit = 0.1
self.kp_acc_high = 1
self.kp_acc_low = 0.01
self.ki_acc_high = 0.5
self.ki_acc_low = 0.001
# state
self.ini = 0 # indicate if attitude is initialized
self.dt = 1.0 # sample period, sec
self.q = np.array([1.0, 0.0, 0.0, 0.0]) # quaternion
self.err_int = np.array([0.0, 0.0, 0.0]) # integral of error
self.kp_acc = 1
self.ki_acc = 0.001
self.gyro_bias = np.array([0.0, 0.0, 0.0])
self.tmp = np.array([0.0, 0.0, 0.0])
开源源码
下面是x-IMU关于互补滤波的开源代码（具体参见Open-Source-AHRS-With-x-IMU）因为使用的坐标系不同，所以重力加速度分量的正负不太一样：
public MahonyAHRS(float samplePeriod, float kp, float ki)
{
SamplePeriod = samplePeriod;
Kp = kp;
Ki = ki;
Quaternion = new float[] { 1f, 0f, 0f, 0f };
eInt = new float[] { 0f, 0f, 0f };
}
public void Update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3]; // short name local variable for readability
float norm;
float hx, hy, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float pa, pb, pc;
// Auxiliary variables to avoid repeated arithmetic
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = (float)Math.Sqrt(ax * ax + ay * ay + az * az);
if (norm == 0f) return; // handle NaN
norm = 1 / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = (float)Math.Sqrt(mx * mx + my * my + mz * mz);
if (norm == 0f) return; // handle NaN
norm = 1 / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
hx = 2f * mx * (0.5f - q3q3 - q4q4) + 2f * my * (q2q3 - q1q4) + 2f * mz * (q2q4 + q1q3);
hy = 2f * mx * (q2q3 + q1q4) + 2f * my * (0.5f - q2q2 - q4q4) + 2f * mz * (q3q4 - q1q2);
bx = (float)Math.Sqrt((hx * hx) + (hy * hy));
bz = 2f * mx * (q2q4 - q1q3) + 2f * my * (q3q4 + q1q2) + 2f * mz * (0.5f - q2q2 - q3q3);
// Estimated direction of gravity and magnetic field
vx = 2f * (q2q4 - q1q3);
vy = 2f * (q1q2 + q3q4);
vz = q1q1 - q2q2 - q3q3 + q4q4;
wx = 2f * bx * (0.5f - q3q3 - q4q4) + 2f * bz * (q2q4 - q1q3);
wy = 2f * bx * (q2q3 - q1q4) + 2f * bz * (q1q2 + q3q4);
wz = 2f * bx * (q1q3 + q2q4) + 2f * bz * (0.5f - q2q2 - q3q3);
// Error is cross product between estimated direction and measured direction of gravity
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);
if (Ki > 0f)
{
eInt[0] += ex; // accumulate integral error
eInt[1] += ey;
eInt[2] += ez;
}
else
{
eInt[0] = 0.0f; // prevent integral wind up
eInt[1] = 0.0f;
eInt[2] = 0.0f;
}
// Apply feedback terms
gx = gx + Kp * ex + Ki * eInt[0];
gy = gy + Kp * ey + Ki * eInt[1];
gz = gz + Kp * ez + Ki * eInt[2];
// Integrate rate of change of quaternion
pa = q2;
pb = q3;
pc = q4;
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * SamplePeriod);
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * SamplePeriod);
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * SamplePeriod);
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * SamplePeriod);
// Normalise quaternion
norm = (float)Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
Quaternion[0] = q1 * norm;
Quaternion[1] = q2 * norm;
Quaternion[2] = q3 * norm;
Quaternion[3] = q4 * norm;
}
参考文献
【1】基于AHRS的三类姿态解算算法对比(含代码)-基于手机端惯性传感器的航迹推算算法(下)
【2】Pixhawk-姿态解算-互补滤波
【3】Pixhawk之姿态解算篇（4）_补充篇
【4】最简单的航姿仪算法C程序（AHRS）
【5】Open-Source-AHRS-With-x-IMU
【6】Aceinna gnss_ins-sim
【7】PID控制算法原理（抛弃公式，从本质上真正理解PID控制）