硬件
首先从硬件开始说起把，气压计、陀螺仪、磁力计，这三个不用说肯定是必备的，后面由于开发的需要还添加了激光测距，以及光流。但是在开发过程中遇到了很多问题，一个一个来说。
气压计
气压计的原理其实就是通过读取当前的温度和气压值，再通过一定的计算来得到当前所处的高度。气压计有很明显的缺点，无论是温度还是无人机起飞后螺旋桨转动导致的气压升高等等，都会影响气压计的精准程度，所以最好在设计硬件的阶段，就要让气压计原理螺旋桨，也要控制它的温度。
陀螺仪
可以说整个开发过程中遇到的最大的问题就来自于陀螺仪了。在我调试PID的过程中，发现无论我怎么调，飞机最终都会往一个方向偏，开始我以为是因为机架结构的问题导致螺旋桨的方向有偏差所以受力不均会偏离，但是后来想到这种问题PID应该是可以解决的。那既然是角度有偏差，就从角度开始查起，结果发现飞机在往左偏的过程中，测到的角度依然是0度，那么问题显而易见了，角度结算出了问题。在我更改了无数次参数后，这个问题是依然存在的，又只能去其他方向找问题，那就只剩下一个方向了，会不会是用于姿态解算的数据有问题了。终于在我一步一步推导的过程中发现了问题所在：陀螺仪的数据出现了零偏，而且零偏很大，我推测是温度过高导致了陀螺仪的零偏，然后我读取了芯片的温度发现果然是这样，在起飞时陀螺仪的温度大概在30度左右，但随着我飞行时间越长，温度就越高，最高甚至可以达到80度，这就难怪了，不过这其实属于硬件设计的问题，陀螺仪（气压计也是）距离电池的接口太近了，而且旁边还有电机的接口，不烫就怪了。最后因为硬件的设计已经定下来了，没办法只能外接了一个陀螺仪，问题也就解决了。
磁力计
这个磁力计几乎就没法用，因为我们的电流比较大，电生磁导致罗盘的方向几乎完全对不上，所以最后取消了罗盘的使用，不过好在PID参数够给力，基本可以解决YAW角偏转的问题。
激光测距
因为气压计没办法用，所以就换成了激光测距用来定高。相当好用，比气压计好用多了。
光流
因为我们的主要应用环境是在室内，所以GPS不适合用来定点，选择了光流用来做定点悬停。
软件
这里大概分为几部分来写把，毕竟无人机的整个控制系统相当复杂
底层驱动
这一部分其实在在之前几代的开发阶段就已经写好了，基本上就是拿来上一代的产品改改管脚，加多一些功能而已，就不详细写了。不过有一点，好像是说用的MCU的硬件I2C有些问题还是传感器的I2C有问题，所以只好用IO口自己写了一个软件I2C的模拟协议，具体的程序和之前写过的类似。
滤波算法
那么在硬件的驱动写好之后，就可以拿到原始数据了，但是此时拿到的原始数据的频率基本都会有较大的噪声，那么不可避免的就是需要对原始数据进行滤波了，不过有些传感器比如陀螺仪可能内部就有低通滤波可供使用。这里主要贴几个滤波算法的代码把，其实网上都可以找得到，仅供参考。
/**
* 二阶低通滤波
*/
void lpf2pInit(lpf2pData *lpfData, float sample_freq, float cutoff_freq)
{
if (lpfData == NULL || cutoff_freq <= 0.0f)
{
return;
}
lpf2pSetCutoffFreq(lpfData, sample_freq, cutoff_freq);
}
/**
* 设置二阶低通滤波截至频率
*/
void lpf2pSetCutoffFreq(lpf2pData *lpfData, float sample_freq, float cutoff_freq)
{
float fr = sample_freq / cutoff_freq;
float ohm = tanf(M_PI_FLOAT / fr);
float c = 1.0f + 2.0f * cosf(M_PI_FLOAT / 4.0f) * ohm + ohm * ohm;
lpfData->b0 = ohm * ohm / c;
lpfData->b1 = 2.0f * lpfData->b0;
lpfData->b2 = lpfData->b0;
lpfData->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
lpfData->a2 = (1.0f - 2.0f * cosf(M_PI_FLOAT / 4.0f) * ohm + ohm * ohm) / c;
lpfData->delay_element_1 = 0.0f;
lpfData->delay_element_2 = 0.0f;
}
float lpf2pApply(lpf2pData *lpfData, float sample)
{
float delay_element_0 = sample - lpfData->delay_element_1 * lpfData->a1 - lpfData->delay_element_2 * lpfData->a2;
float output = delay_element_0 * lpfData->b0 + lpfData->delay_element_1 * lpfData->b1 + lpfData->delay_element_2 * lpfData->b2;
lpfData->delay_element_2 = lpfData->delay_element_1;
lpfData->delay_element_1 = delay_element_0;
return output;
}
在使用时先使用函数lpf2pInit来初始化需要用到的参数、样本频率（数据输入频率）和截止频率（数据输出频率）。
需要注意因为无人机对于数据的实时性其实挺高的，所以不能把截止频率设的太低。
虽然说搞了很多滤波的算法，但其实真正用到的也就这一个。其他的算法后面吃透了再去写把，为了理解这个滤波的写法还跑去把大学的数字信号处理的课本翻出来看了一遍。
姿态解算
在完成了上面一系列的滤波后，总算来到了飞控的第一个难点，在得到了陀螺仪读出的加速度、角速度数据后，如何算出飞机当前的俯仰、横滚、偏航角度？方法有很多，什么欧拉角法、方向余弦法、四元数法等等。因为之前的飞控都用的四元数法，所以我这里也就没有去做过多的研究，毕竟都走老路也会省很多事情。但是由于陀螺仪只能测出角速度，并不能直接得到角度，所以如果想要知道各个方向的角度，需要用角速度积分，那么问题来了，积分就会产生偏差，积分越久，偏差就会越大，怎样解决偏差的问题呢，这里也有很多种方法，比如卡尔曼滤波，或者是互补滤波等等。也是根据之前飞控的写法，采用了Mohony算法（其实看内容类似于互补滤波的方法）。这里简单贴一下代码，网上也都可以查得到。通过调整imu_kp和imu_ki的值是的角度的值更贴近真实值。
static void imu_fushion_update(float dt,
bool useGyro, float gx, float gy, float gz,
bool useACC, float ax, float ay, float az,
bool useMag, float mx, float my, float mz)
{
if (!useGyro)
return;
// integral error terms scaled by Ki
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;
// Calculate general spin rate (rad/s)
// const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
// Use measured magnetic field vector
float ex = 0, ey = 0, ez = 0;
float recipNorm = sq(mx) + sq(my) + sq(mz);
//磁力计数据不全为0时
if (useMag && recipNorm > 0.01f)
{
// Normalise magnetometer measurement
recipNorm = invSqrt(recipNorm);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
// This way magnetic field will only affect heading and wont mess roll/pitch angles
// (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
// (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
// const float ez_ef = -(hy * bx);
const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
// const float hz = rMat[2][0] * mx + rMat[2][1] * my + rMat[2][2] * mz;
const float bx = sqrtf(hx * hx + hy * hy);
// const float bz = hz;
/*Rotate mag error vector back to BF and accumulate*/
// float wx = rMat[1][0] * bx + rMat[2][0] * bz;
// float wy = rMat[1][1] * bx + rMat[2][1] * bz;
// float wz = rMat[1][2] * bx + rMat[2][2] * bz;
// /**error is sum of cross product betwween reference direction of fields and direction measure by sensors */
// ex += my * wz - mz * wy;
// ey += mz * wx - mx * wz;
// ez += mx * wy - my * wx;
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
const float ez_ef = -(hy * bx);
// Rotate mag error vector back to BF and accumulate
ex += rMat[2][0] * ez_ef;
ey += rMat[2][1] * ez_ef;
ez += rMat[2][2] * ez_ef;
}
// #endif
// Use measured acceleration vector
recipNorm = sq(ax) + sq(ay) + sq(az);
if (useACC && recipNorm > 0.01f)
{
// Normalise accelerometer measurement
recipNorm = invSqrt(recipNorm);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Error is sum of cross product between estimated direction and measured direction of gravity
ex = (ay * rMat[2][2] - az * rMat[2][1]);
ey = (az * rMat[2][0] - ax * rMat[2][2]);
ez = (ax * rMat[2][1] - ay * rMat[2][0]);
}
// Compute and apply integral feedback if enabled
if (imu_ki > 0.0f)
{
// Stop integrating if spinning beyond the certain limit
// if (spin_rate < SPIN_RATE_LIMIT)
// {
// const float dcmKiGain = imu_ki;
integralFBx += imu_ki * ex * dt; // integral error scaled by Ki
integralFBy += imu_ki * ey * dt;
integralFBz += imu_ki * ez * dt;
// }
}
else
{
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
const float dcmKpGain = imu_kp * imuGetPGainScaleFactor();
// Apply proportional and integral feedback
gx += dcmKpGain * ex + integralFBx;
gy += dcmKpGain * ey + integralFBy;
gz += dcmKpGain * ez + integralFBz;
// Integrate rate of change of quaternion
// gx = gx * (0.5f * dt);
// gy = gy * (0.5f * dt);
// gz = gz * (0.5f * dt);
float halfT = 0.5f * dt;
const float qa = q0; //last q
const float qb = q1; //last q
const float qc = q2; //last q
const float qd = q3; //last q
q0 += (-qb * gx - qc * gy - qd * gz) * halfT;
q1 += (qa * gx + qc * gz - qd * gy) * halfT;
q2 += (qa * gy - qb * gz + qd * gx) * halfT;
q3 += (qa * gz + qb * gy - qc * gx) * halfT;
// Normalise quaternion
recipNorm = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3));
q0 = q0 * recipNorm;
q1 = q1 * recipNorm;
q2 = q2 * recipNorm;
q3 = q3 * recipNorm;
// Pre-compute rotation matrix from quaternion
imuComputeRotationMatrix();
imuUpdateEulerAngles();
// static bool firstStart = true;
// if (firstStart)
// {
// lastCouresCorrectTime = millis();
// firstStart = false;
// }
// if (!coures_corrected && (millis() - lastCouresCorrectTime) > 100)
// {
// update_course_correct_angle();
// }
}
可以看到其实就是用机体的加速度来做一个PI反馈，用于修正角速度积分所产生的误差。用当前计算出的四元数得到旋转矩阵
static void imuComputeRotationMatrix(void)
{
/**
* @brief 由四元数求得旋转矩阵R
* LINK https://blog.csdn.net/weixin_42587961/article/details/100150826
*/
float q1q1 = sq(q1);
float q2q2 = sq(q2);
float q3q3 = sq(q3);
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q3 = q2 * q3;
rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
rMat[0][1] = 2.0f * (q1q2 + -q0q3);
rMat[0][2] = 2.0f * (q1q3 - -q0q2);
//磁场方向
rMat[1][0] = 2.0f * (q1q2 - -q0q3);
rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
rMat[1][2] = 2.0f * (q2q3 + -q0q1);
//重力方向
rMat[2][0] = 2.0f * (q1q3 + -q0q2);
rMat[2][1] = 2.0f * (q2q3 - -q0q1);
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}
然后再通过旋转矩阵更新欧拉角得到当前姿态
attitude.values.roll = lrintf(atan2f(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
attitude.values.pitch = lrintf((-asinf(rMat[2][0])) * (1800.0f / M_PIf));
attitude.values.yaw = lrintf((atan2f(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf)));
高度解算
在好不容易解决了第一个难点之后，紧接着第二个难点也就来了，如何算出飞机所在的高度？
首先我们能够得到加速度的数据，但这时候得到的加速度数据其实是机体的加速度，并不是地系的加速度，需要通过旋转矩阵来转换（Z轴的加速度要记得减去重力加速度）
static void imuTransformVectorBodyToEarth(t_fp_vector *v)
{
/* From body frame to earth frame */
const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
v->V.X = x;
v->V.Y = -y;
v->V.Z = z;
}
虽然我们能够直接通过激光测距得到具体的高度，但是我们还需要得到无人机的速度来做高度控制的串级PID。也是使用互补滤波的方法（当然卡尔曼滤波也是可以用的），通过加速度计的积分得到高度，再与激光测距得到的高度相减得到误差，然后补偿加速度计积分的速度和高度。
uint16_t deltaT = micros() - UsPre;
UsPre = micros();
float dt = deltaT * 1e-6f;//单位：秒
float vel_alt_weight = 0.9f;//激光测距权重
float altError = Get_Laseraltitude() * getCosTiltAngle() - zroneEstimator.pos[Z];
zroneEstimator.pos[axis] += zroneEstimator.vel[axis] * dt + acc * dt * dt / 2.0f;
zroneEstimator.vel[axis] += acc * dt;
zroneEstimator.pos[axis]+=altError *vel_alt_weight *dt;
zroneEstimator.vel[axis]+=weight*altError* vel_alt_weight*dt;//二阶互补
位置解算
光流用于定点时要计算速度和位移，但是由于光流的特性，他在旋转时也会产生速度和位移，所以需要通过角速度来抵消掉旋转时产生的速度。需要注意，对角速度和光流速度滤波后要保证相位相同（尽可能保证幅值也相同），否则无法完全抵消旋转时产生的速度。在得到抵消角速度后的光流速度，需要乘以高度才能得到真正的线速度。再通过积分得到位移。
//KM是高度转换的系数，从光流手册中可以找到
trans->X_trans_motion = pixel_filt[X] - gyroRate_filt[X]; //修正后的像素
trans->Y_trans_motion = pixel_filt[Y] - gyroRate_filt[Y];
trans->X_dist = height * kM * trans->X_trans_motion * 10; //实际输出像素 单位：cm
trans->Y_dist = height * kM * trans->Y_trans_motion * 10;
trans->X_distance += trans->X_dist * dT; //积分位移
trans->Y_distance += trans->Y_dist * dT;
在得到光流的速度后，通过互补滤波与惯导速度融合。惯导速度由加速度积分而来，注意加速度是机体的加速度，可以从地系加速度转换而来
const float accVelScale = 980.665f / (float)acc.acc_1G; //加速度转换成 cm/s^2;
static void imuTransformVectorEarthToBody(t_fp_vector *v)
{
v->V.Y = -v->V.Y;
/* From earth frame to body frame */
const float x = rMat[0][0] * v->V.X + rMat[1][0] * v->V.Y;
const float y = rMat[0][1] * v->V.X + rMat[1][1] * v->V.Y;
v->V.X = x;
v->V.Y = y;
}
accel_body.V.X = accel_ned.V.X;
accel_body.V.Y = accel_ned.V.Y;
imuTransformVectorEarthToBody(&accel_body);
//注意方向，与光流方向保持一致
zroneEstimator.acc[Y] = -accel_body.V.X * accVelScale;
zroneEstimator.acc[X] = accel_body.V.Y * accVelScale;
//水平速度预估
zroneEstimator.vel[X] += zroneEstimator.acc[X] * dt;
zroneEstimator.vel[Y] += zroneEstimator.acc[Y] * dt;
//观测速度与融合速度的误差
SpeedError[X] = opticalFlow_Data.X_dist - zroneEstimator.vel[X];
SpeedError[Y] = opticalFlow_Data.Y_dist - zroneEstimator.vel[Y];
//水平速度校正
zroneEstimator.vel[X] += SpeedError[X] * opFixVelFactor;
zroneEstimator.vel[Y] += SpeedError[Y] * opFixVelFactor;
//观测距离与融合距离误差
PosError[X] = opticalFlow_Data.X_distance - zroneEstimator.pos[X];
PosError[Y] = opticalFlow_Data.Y_distance - zroneEstimator.pos[Y];
//位移矫正
zroneEstimator.pos[X] += PosError[X] * opFixPosFactor * dt;
zroneEstimator.pos[Y] += PosError[Y] * opFixPosFactor * dt;
PID调节
把所有需要的数据都解算好后，通过PID调节来控制电机的输出。
float PID_Control(PID_Controler *Controler)
{
//动态积分参数
// const float motorMixRange = getMotorMixRange();
// const float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
// const float tpaFactor = getThrottlePIDAttenuation();
/*******偏差计算*********************/
Controler->Last_Err = Controler->Err; //保存上次偏差
Controler->Err = Controler->Expect - Controler->FeedBack; //期望减去反馈得到偏差
if (Controler->Err_Limit_Flag == 1) //偏差限幅度标志位
{
if (Controler->Err >= Controler->Err_Max)
Controler->Err = Controler->Err_Max;
if (Controler->Err <= -Controler->Err_Max)
Controler->Err = -Controler->Err_Max;
}
/*******积分计算*********************/
// if (motorMixRange < 1.0f)
// Controler->Integrate += Controler->Ki * Controler->Err * dynKi;
Controler->Integrate += Controler->Ki * Controler->Err;
/*******积分限幅*********************/
if (Controler->Integrate_Limit_Flag == 1) //积分限制幅度标志
{
if (Controler->Integrate >= Controler->Integrate_Max)
Controler->Integrate = Controler->Integrate_Max;
if (Controler->Integrate <= -Controler->Integrate_Max)
Controler->Integrate = -Controler->Integrate_Max;
}
/*******没解锁时 积分不起作用*******/
if (!FC_ERROR_CHECK(FC_ARMED))
{
Controler->Integrate = 0;
}
/*******总输出计算*********************/
Controler->Last_Control_OutPut = Controler->Control_OutPut; //输出值递推
Controler->Control_OutPut = Controler->Kp * Controler->Err //比例
+ Controler->Integrate //积分
+ Controler->Kd * (Controler->Err - Controler->Last_Err); //微分
/*******总输出限幅*********************/
if (Controler->Control_OutPut >= Controler->Control_OutPut_Limit)
Controler->Control_OutPut = Controler->Control_OutPut_Limit;
if (Controler->Control_OutPut <= -Controler->Control_OutPut_Limit)
Controler->Control_OutPut = -Controler->Control_OutPut_Limit;
/*******返回总输出*********************/
return Controler->Control_OutPut;
}
float PID_Control_Yaw(PID_Controler *Controler)
{
/*******偏差计算*********************/
Controler->Last_Err = Controler->Err; //保存上次偏差
Controler->Err = Controler->Expect - Controler->FeedBack; //期望减去反馈得到偏差
/***********************偏航角偏差超过+-180处理*****************************/
if (Controler->Err < -180)
Controler->Err = Controler->Err + 360;
if (Controler->Err > 180)
Controler->Err = Controler->Err - 360;
if (Controler->Err_Limit_Flag == 1) //偏差限幅度标志位
{
if (Controler->Err >= Controler->Err_Max)
Controler->Err = Controler->Err_Max;
if (Controler->Err <= -Controler->Err_Max)
Controler->Err = -Controler->Err_Max;
}
/*******积分计算*********************/
Controler->Integrate += Controler->Ki * Controler->Err;
/*******积分限幅*********************/
if (Controler->Integrate_Limit_Flag == 1) //积分限制幅度标志
{
if (Controler->Integrate >= Controler->Integrate_Max)
Controler->Integrate = Controler->Integrate_Max;
if (Controler->Integrate <= -Controler->Integrate_Max)
Controler->Integrate = -Controler->Integrate_Max;
}
/*******没解锁时 积分不起作用*******/
if (!FC_ERROR_CHECK(FC_ARMED))
{
Controler->Integrate = 0;
}
/*******总输出计算*********************/
Controler->Last_Control_OutPut = Controler->Control_OutPut; //输出值递推
Controler->Control_OutPut = Controler->Kp * Controler->Err //比例
+ Controler->Integrate; //积分;
//微分
/*******总输出限幅*********************/
if (Controler->Control_OutPut >= Controler->Control_OutPut_Limit)
Controler->Control_OutPut = Controler->Control_OutPut_Limit;
if (Controler->Control_OutPut <= -Controler->Control_OutPut_Limit)
Controler->Control_OutPut = -Controler->Control_OutPut_Limit;
/*******返回总输出*********************/
return Controler->Control_OutPut;
}
航向角在做PID时，要注意180度这个界限。
