无人机小四轴定高篇

无人机定高篇

定高，也是无人机的标配功能，可以说必须要有的。一般来说定高怎么做，常规的思维就是计算出无人机的高度和Z轴速度，然后对高度和速度进行PID运算。实际上大思想就是这么的简单，但是要真的调的好那可就不简单了。
1、计算高度和速度
一般来说要定高就需要相应的能够计算高度的传感器，在这我以气压计为例。市面上的气压计很多种，气压计根据海拔高度不同而气压不同的这个原理可以相对的计算出无人机的高度，记住这个是相对高度，绝对高度并不精确。
但是气压计有它本身非常致命的缺点，那就是更新速率慢，一般的PID单是靠气压计完全没有办法实现定高。这是我们想到无人机还有一个传感器，那就是加速计。加速计这个器件反应很快，也很灵敏，但是会有积分飘漂移，所以单靠加速计实现定高也没办法实现。由此，聪明的人类就想到了为何不各采用他们两的优点呢，让他们形成一个取长补短的关系。这个关系就是气压计加速计的数据融合了。
最简单的融合方法就是互补滤波，下面我们就先来了解气压计加速计的互补滤波融合，直接上代码：

void ahrs_update_R_bf_to_ef(float angle_pitch,float angle_roll,float angle_yaw)
{
 float sin_pitch = sin(angle_pitch * M_DEG_TO_RAD);
 float cos_pitch = cos(angle_pitch * M_DEG_TO_RAD);
 float sin_roll = sin(angle_roll * M_DEG_TO_RAD);
 float cos_roll = cos(angle_roll * M_DEG_TO_RAD); 
 //以上为各个欧拉角的sin,cos值
 vehicle_att.R[2][0] = -sin_pitch;
 vehicle_att.R[2][1] = sin_roll * cos_pitch;
 vehicle_att.R[2][2] = cos_roll * cos_pitch;
 //vehicle_att.R这个数组的作用是用来后面计算加速计在重力方向的分量
}
acc_xyz[0]=accel_x*980/MV_ACC_RP;
acc_xyz[1]=accel_y*980/MV_ACC_RP;
acc_xyz[2]=accel_z*980/MV_ACC_RP;
//加速计单位转换，我这里是CM/S2
no_filteracczz=-acc_xyzz[1]*vehicle_att.R[2][0]-acc_xyzz[0]*vehicle_att.R[2][1]+acc_xyzz[2]*vehicle_att.R[2][2]-980*accel_zeroz/MV_ACC_RP;
//no_filteracczz就是计算出来的加速计在重力上的分量了，后面加速计积分直接用这个变量就好
void accel_filter_predict(float dt,float x[3])
{
 x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
 x[1] += x[2] * dt;
}//加速计计算位移
void inertial_filter_correct(float e,float dt,float x[3],int i,float w,float vp)
{
  float ewdt = e * w * dt;
 x[i] += ewdt;
 if(i == 0)
 {
  x[1] += vp*w * ewdt;
 }
}//加速计计算速度
z_est[2] = acc->zz - z_bias;
 accel_filter_predict(INAV_T,z_est);
 z_est[0] += corr_baro * w_z_baro * INAV_T;
 z_bias -= corr_baro * 0.05f * INAV_T;
 z_est[1] += corr_baro * baro_vel_gain * INAV_T;
 //这几行代码就是融合算法，是不是很简单
 //corr_baro为气压计跟加速计计算的位移之差
 //w_z_baro，baro_vel_gain为滤波增益
 //z_bias为气压计补偿加速计的偏移量，有可能静态时加速计不在零位，所以通过气压计修正

以上就是加速计气压计的互补滤波融合，增益参数等都要学会调适，最好可以调出相应变量出来观看其曲线变化。一般要先了解互补滤波的核心思想再去调试会更好调，气压计会受风的影响，加速计会受振动的影响，所以调试时会有各种实际影响因素，一般结构不太差的飞机定高都是可以调的好的，所以调试前也需要了解加速计气压计的各方面性能，优缺点。
实现计算了高度和速度，然后就是对高度速度进行PID控制了。这部分的控制很简单，也可以参考我的博客姿态控制https://blog.csdn.net/u014694105/article/details/104499158，也是用的串级PID控制，不过有一个点需要注意的是，因为飞机起飞需要抵抗重力，所以定高的油门输出除了PID控制的输出外还要加一个前馈控制量，就是直接给一个合适的油门初始值+PID输出，这个油门初始值根据飞机，电量等调整。
定高也是一个大难点，想要调的很好也是需要花费一定的精力跟时间的，经验分享给大家，祝大伙早日调通。