圆点博士小四轴算法快速入门--四元数和PID图解

参考http://www.eeboard.com/bbs/thread-32321-1-1.html

飞控的算法代码一般包括下面三个部分：滤波，姿态，PID

1，滤波可以用互补滤波来实现，互补滤波的资料很多，大家随便就能找到。基本公式是：

 


2，滤波完就是四元数拉。直接用老外Madgwick的IMU就可以。超级简单
unsigned char BS004_IMU_Update(float ax,float ay,float az,float gx,float gy,float gz) 
{
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;  
  //        
        //圆点博士:四元数乘法运算
        float q0q0 = q0 * q0;                                                        
        float q0q1 = q0 * q1;
        float q0q2 = q0 * q2;
        float q1q1 = q1 * q1;
        float q1q3 = q1 * q3;
        float q2q2 = q2 * q2;
        float q2q3 = q2 * q3;
        float q3q3 = q3 * q3;
        //        
        //圆点博士:归一化处理
        norm = sqrt(ax*ax + ay*ay + az*az);     
        if(norm==0) return 0;        
        ax = ax / norm;
        ay = ay / norm;
        az = az / norm;   
  //        
        //圆点博士:建立小四轴坐标系        
        vx = 2*(q1q3 - q0q2);                                                                
        vy = 2*(q0q1 + q2q3);
        vz = q0q0 - q1q1 - q2q2 + q3q3;
        //
        //圆点博士:坐标系和重力叉积运算
        ex = (ay*vz - az*vy);                                                                
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);
        //
        //圆点博士:比例运算
        exInt = exInt + ex*bs004_quad_Ki;
        eyInt = eyInt + ey*bs004_quad_Ki;
        ezInt = ezInt + ez*bs004_quad_Ki;
        //
        //圆点博士:陀螺仪融合
        gx = gx + bs004_quad_Kp*ex + exInt;
        gy = gy + bs004_quad_Kp*ey + eyInt;
        gz = gz + bs004_quad_Kp*ez + ezInt;
        //
        //圆点博士:整合四元数率
        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*bs004_quad_halfT;
        q1 = q1 + (q0*gx + q2*gz - q3*gy)*bs004_quad_halfT;
        q2 = q2 + (q0*gy - q1*gz + q3*gx)*bs004_quad_halfT;
        q3 = q3 + (q0*gz + q1*gy - q2*gx)*bs004_quad_halfT;  
        //
        //圆点博士:归一化处理
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        if(norm==0) return 0;        
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;
        //
        //圆点博士:欧拉角转换
        bs004_imu_roll=asin(-2*q1q3 + 2*q0q2)*57.30f;
  bs004_imu_pitch=atan2(2*q2q3 + 2*q0q1, -2*q1q1-2*q2q2 + 1)*57.30f; 
  bs004_imu_yaw=bs004_imu_yaw-gz*bs004_mpu6050_gyro_scale;
        //
        return 1;        
}


3, PID的代码其实也很简单，主要是要了解其中的原理，才能更好地调整参数。为了方便新手们理解，楼主建立了一个数学模型来让大家了解。（只针对新手，老手就算了)

========圆点博士小四轴之PID控制模式分析=======
PID控制的P是Proportional的缩写, 是比例的意思，I是Integral的缩写，是积分的意思，D是Derivative的缩写，是微分的意思。所以，PID就是我们常说的比例，积分，微分控制。
我们首先来看一个PID控制模型曲线图：
该图包含了比例控制，比例+积分控制，比较+积分+微分控制的电机响应图的对比。

 
下面我们对曲线进行具体分析：
PID中的比例控制是最容易理解的，比例控制就是把角度的误差乘以一个常数作为输出驱动。假定我们有一个理想模型的电机，1V电压的变化会带来小四轴1度的角度改变。假定现在电机控制电压是5V,小四轴在某一轴上的偏角是5度，目标角度是100度。我们把当前的电压量定义为Vin,把输出控制量定义为Vout。假定P等于0.2，那么比例控制的结果就是：
第一次：Vout=Vin+(100-5)*P=5V+19V=24V，得到电机电压是24V，对应的小四轴角度是24度,距离目标角度的误差是100-24=76度。
第二次：Vout=Vin+(100-24)*P=24V+15V=39V, 从而引起的角度是39度。
我们看到，在这么的一个比例控制系统下，小四轴角度在慢慢地向目标角度靠近。

PID中的积分控制就是把把所有角度误差相加起来，然后乘上一个常数作为输出驱动。在上述例子中，假定I=0.2, 我们来看看比例和积分控制同时起作用下的系统反应。
第一次：Vout=Vin+(100-5)*P+(100-5)*I=5V+19V+19V=43V，这时候小四轴角度为43度。
由于第一次控制前的误差是100-5=95,第二次控制前的误差是100-43=57，所以积分结果是152。
第二次：Vout=Vin+(100-43)*P+((100-5)+(100-43)）*I=43V+11V+30V=84V, 这时候小四轴角度变为84度。
第三次：Vout=Vin+(100-84)*P+((100-5)+(100-43)+（100-84））*I=84+3V+33V=120V。这时小四轴角度变为120度。
我们看到，在增加了积分控制后，小四轴角度在快速向目标角度靠近。

PID中的微分控制就是把角度的变化乘上一个常数来作为电机驱动输出。在上述例子中，假定D=0.2, 我们来看看比例,积分和微分共同控制下的系统反应。假定第一次前，电机转速保持5转，那么第一次前的角度变化为0。
第一次：Vout=Vin+(100-5)*P+(100-5)*I-（5-5）*D=5V+19V+19V-0V=43V，这时候小四轴角度为43度。
和上一次相比，角度从5度变化到了43度，所以小四周角度变化是43-5=38度。
第二次：Vout=Vin+(100-43)*P+((100-5)+(100-43)）*I-（43-5）*D=43V+11V+30V-7V=77V, 这时候小四周角度77度。

把上述的计算结果列出来，我们看到：
 

从上面的数据，我们可以看到：
1，单独比例控制的时候，数据慢慢接近目标 （图表中的红色线）
2，加入积分控制之后，数据快速接近目标 （图表中的蓝色线）
3，微分控制起到抑制变化的作用。（图表中的绿色线）

有了这些理论基础，就可以写PID控制代码拉。

========圆点博士小四轴之PID控制代码分析=======
在圆点博士小四轴2014版代码里，我们只使用到PD参数。
首先我们来看PID中的比例控制。跟上一节模型提到的一样，比例是针对误差的控制。
首先我们获取小四轴当前角度。
bs004_angle_cur_pitch=bs004_imu_pitch;
bs004_angle_cur_roll =bs004_imu_roll;
把当前角度和目标角度相减，就可以得到角度偏差。
bs004_angle_err_pitch=bs004_angle_cur_pitch-bs004_angle_target_pitch;
bs004_angle_err_roll=bs004_angle_cur_roll-bs004_angle_target_roll;
然后进行比例控制：
bs004_fly_m1=bs004_fly_m1
+bs004_pitch_p*bs004_angle_err_pitch
-bs004_roll_p *bs004_angle_err_roll
-bs004_yaw_p*bs004_angle_err_yaw;
bs004_fly_m2=bs004_fly_m2
-bs004_pitch_p*bs004_angle_err_pitch
-bs004_roll_p *bs004_angle_err_roll
+bs004_yaw_p*bs004_angle_err_yaw;

bs004_fly_m3=bs004_fly_m3
-bs004_pitch_p*bs004_angle_err_pitch
+bs004_roll_p *bs004_angle_err_roll
-bs004_yaw_p*bs004_angle_err_yaw;
bs004_fly_m4=bs004_fly_m4
+bs004_pitch_p*bs004_angle_err_pitch
+bs004_roll_p *bs004_angle_err_roll
+bs004_yaw_p*bs004_angle_err_yaw;

在上一节模型中，我们提到PID中的微分控制针对的是角度变化而进行的控制。
所以我们首先要得到当前角度和上一次角度的差异。
bs004_angle_dif_pitch=bs004_angle_cur_pitch-bs004_angle_last_pitch;
bs004_angle_dif_roll =bs004_angle_cur_roll-bs004_angle_last_roll;
bs004_angle_dif_yaw  =bs004_angle_last_yaw-bs004_angle_cur_yaw;
然后进行微分控制：
bs004_fly_m1=bs004_fly_m1
+bs004_pitch_d*bs004_angle_dif_pitch
-bs004_roll_d *bs004_angle_dif_roll
-bs004_yaw_d*bs004_angle_dif_yaw;
bs004_fly_m2=bs004_fly_m2
-bs004_pitch_d*bs004_angle_dif_pitch
-bs004_roll_d *bs004_angle_dif_roll
+bs004_yaw_d*bs004_angle_dif_yaw;
bs004_fly_m3=bs004_fly_m3
-bs004_pitch_d*bs004_angle_dif_pitch
+bs004_roll_d *bs004_angle_dif_roll
-bs004_yaw_d*bs004_angle_dif_yaw;
bs004_fly_m4=bs004_fly_m4
+bs004_pitch_d*bs004_angle_dif_pitch
+bs004_roll_d *bs004_angle_dif_roll
+bs004_yaw_d*bs004_angle_dif_yaw;

从上面的描述我们可以看出，小四轴的PID控制还是比较简单的。