【云台】开源版本SimpleBGC的传感器校准与姿态融合

传感器校准

俄版云台使用的是 MPU6050，磁力计使用的是 HMC5883

MPU6050校准

MPU6050校准包含两部分：加速度计与陀螺仪。校准过程想要校准出的数据也就是加速度计与陀螺仪的温漂与零漂。

正常来说，如果将 IMU 朝上静置，加速度计与陀螺仪的读数应该是这样：

MPU6050是按照14位计数的，加速度计最大范围记录2G，那么重力加速度1G就对应 8192

加速度 XY 轴应该是 0，Z 轴由于有重力影响，应该是重力加速度G，同时 XYZ 三轴的角速度也应该是0。但在实际中，总是会有误差的，静置时，加速度计与陀螺仪的读数相对上表中的标准读数是有偏差的，就是零偏，当温度上升时，这个零偏值也会变化，就是温漂。

俄版云台的做法是将温漂与零漂看作是一次函数的模型，分两次校准，刚开始记录一组数据求平均，记录零偏，之后再等待10分钟，等温度上升之后，再记录一组数据求平均，得到第二组温度升高时的零偏，之后按照 y = k * x + b 的方式，y 为零偏值，x 为温度值，求出 k 与 b。得到零漂与温漂的模型，在进行姿态解算时，需要带入零偏，就根据温度以及一次函数求解出对应温度下的零偏。

磁力计校准

磁力计校准需要绕多轴旋转，并且在1分钟内完成，在理想情况下，磁力计在水平方向上旋转 360 度之后，x 与 y 轴的读数是一个正圆形，在垂直方向上也是一样的，如果磁力计存在干扰与误差，那么旋转一周后，读数形成的圆就会偏离圆心的位置，三轴都旋转一周，读数就形成一个球体，磁力计校准就是需要记录偏离球心的位置。

俄版云台也是同样，在采集了多轴旋转的数据之后，通过最小二乘法拟合出数据形成的球体，计算球心，就是磁力计校准需要记录的数据。

姿态解算

俄版云台的姿态解算主要分为：

1）原始数据低通滤波

2）互补滤波计算姿态角(方位角)

3）四元数计算姿态角(航姿角)

云台中使用了两种方法计算姿态角，一种是互补滤波，一种是四元数，互补滤波的权重系数由于是固定的，因此不会发散，在云台中也一直计算，作为一个显示对比数据，通过四元数计算的姿态角加入了运动中的影响，估计的姿态角作为控制层的反馈状态。

使用两种方法在实际出问题时，做数据对比。

互补滤波计算方位角

互补滤波中计算方位角时，由于是直接使用转换为欧拉角进行计算，因此需要在融合陀螺仪的角速度数据时，需要区分欧拉角速度与机体角速度的区别，它们之间有转换矩阵需要进行转换。

欧拉角速度与机体角速度是不同的

欧拉角是相对地面而言的，对欧拉角求导的欧拉角速度也是相对地面的。而机体角速度是相对机体本身的，也就是相对于IMU的原点位置。当机体系与地面坐标系本身就有一定的偏差时，两者差异就会比较大，因此需要一个转换矩阵。

互补滤波得到俯仰角与横滚角

陀螺仪直接测量到的角速度是机体角速度，在转换为欧拉角速度后，乘以时间间隔，再加上上次的估计值，就得到一个陀螺仪积分计算的角度，作为预测值。

加速度计测量到的加速度值可以计算出pitch与roll，作为测量值。

在云台中，陀螺仪的角速度是短期可靠的，加速度计容易受到振动的影响，在长期是可靠的，互补滤波就是给陀螺仪积分得到的角度与加速度计得到的角度分别给权重，利用测量值(加速度计测量得到的角度)校正预测值(陀螺仪积分得到的角度)。

在云台中，权重给的比较简单：

orient[PITCH]   = (orient[PITCH] + gyroRate[PITCH] * dt) + 0.0002f * (smoothAcc[PITCH] - orient[PITCH]);

航向角融合

在方位角的估计中，直接使用欧拉航向角速度进行积分得到航向角。

四元数计算航姿角

与方位角的估算不同的是，使用四元数计算航姿角引入了两个概念来达到校正的目的：

1）使用重力分量作为参考量计算角速度积分得到的姿态与加速度计得到的姿态之间的误差

2）使用PI控制器消除误差

大致流程如下：

1.使用角速度以及龙格库塔积分更新四元数

q0i = (-q1 * gx - q2 * gy - q3 * gz) * halfT;
q1i = (q0 * gx + q2 * gz - q3 * gy) * halfT;
q2i = (q0 * gy - q1 * gz + q3 * gx) * halfT;
q3i = (q0 * gz + q1 * gy - q2 * gx) * halfT;
q0 += q0i;
q1 += q1i;
q2 += q2i;
q3 += q3i;

其中 gx，gy，gz 是机体坐标系下的融合角速度

2.将当前四元数转化为方向余弦矩阵(机体坐标系)并提取出重力分量，得到在机体坐标系下角速度积分得到的姿态(四元数可以看作是经过加速度计修正的角速度积分得到的位姿)中的重力分量向量vg

重力加速度向量是以 [0，0，-g]，在云台中，进行了归一化处理，也就是重力加速度向量为 [0,0,-1]，因此在俄版云台中，得到角速度积分的位姿的重力分量的实现为：

vx = 2.0f * (q1q3 - q0q2);//计算方向余弦矩阵
vy = 2.0f * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;

3.加速度计就是实测出来的在机体坐标系下的重力加速度，归一化后作为重力向量的测量值 ag ，但由于可能处于运动状态，此时就不单单是重力加速度了，因此计算了一个可信度参数，叠加到 Kp 参数上

calculateAccConfidence(norm);//由于处于运动状态，所有要计算加速度数据归一化后的可信度
kpAcc = eepromConfig.KpAcc * accConfidence; //加速度比例系数 * 可信度
kiAcc = eepromConfig.KiAcc * accConfidence;//加速度积分系数 * 可信度
normR = 1.0f / norm; //加速度归一化
ax *= normR;
ay *= normR;
az *= normR;

4.计算 vg 与 ag 的叉积，得到两者的偏差向量，可以理解为通过积分得到的姿态与加速度计得到的姿态的角度偏差

exAcc = vy * az - vz * ay; 
eyAcc = vz * ax - vx * az;
ezAcc = vx * ay - vy * ax;

5.偏差经过 Kp，相当于是互补滤波中的加速度计计算的角度的权重，第4步计算出来的叉积，相当于是向量 vg，绕某轴，旋转n度，就可以旋转到向量 ag ，向量 vg 相当于是通过角速度积分得到的角度，向量 ag 相当于是加速度计测量得到的角度，如果 Kp = 0，那么相当于不旋转，以 vg 向量为准，如果 Kp 比较大，那相当于旋转的角度比较大，以 ag 为准。

gx += exAcc * kpAcc;//比例增益控制加速度计的收敛速度
gy += eyAcc * kpAcc;
gz += ezAcc * kpAcc;

同时，也增加了积分项，逐步消除两种测量方式之间的误差：

exAccInt += exAcc * kiAcc;
eyAccInt += eyAcc * kiAcc;
ezAccInt += ezAcc * kiAcc;
gx += exAccInt;
gy += eyAccInt;
gz += ezAccInt;

上述相当于是，角度误差->PI控制器->校正角速度。也就是 gx，gy，gz 就是校正角速度。

6.将校正角速度代入四元数，更新四元数

    q0i = (-q1 * gx - q2 * gy - q3 * gz) * halfT;
    q1i = (q0 * gx + q2 * gz - q3 * gy) * halfT;
    q2i = (q0 * gy - q1 * gz + q3 * gx) * halfT;
    q3i = (q0 * gz + q1 * gy - q2 * gx) * halfT;
    q0 += q0i;
    q1 += q1i;
    q2 += q2i;
    q3 += q3i;