APM 姿态抗风(干扰 突变) 优化处理

问题描述：

   将amp 移植到stm32 平台后，fast_loop执行频率 可以达到400hz，且可以使用卡尔曼滤波，飞行效果还可以，但gps 定点飞行时发现：有突变风的时候机体容易出现轻微摇晃，和大僵的对比，小动作明显多些，感觉没法接受。

   同样的情况，在室内飞行时，飞机容易发生加速平移，加大积分参数后加速平移现象可以得到有效抑制，但新的情况：出现不稳定气流时，机体同样出现轻微摇晃，小动作频繁。经过数据分析，发现主要是大积分参数导致，且大积分参数还有其他各种负面影响。

   经过各种实验和数据收集分析，得出以下结论:

      1.导致这种现象的原因是外界气流的变化，作用于飞机产生姿态偏移。

      2.飞机的抗风能力差，小动作频繁都是这个原因导致的。（这个抗风指的不是逆向抗风维持飞机位置，而是指突变风导致的姿态变化维持姿态稳定）
3.上述问题，最终归结于控制系统的干扰抑制问题，要解决这个问题，需要增强控制系统的干扰抑制能力。

      4.通过调整PID 参数无法圆满的解决这个问题，通过Matlab 仿真，可以证明，纯PID 无法有效抑制干扰。

问题解决：

   方案1 

      采用自抗扰PID设计，对于这种做法，关键在于观察器的误差精度，通过matlab 仿真，感觉效果不能满意，所以只作为备用方案。

方案2

      采用H∞鲁棒控制，通过matlab仿真理想的H∞状态反馈控制效果堪称完美，对干扰的抑制极强。但实际情况是：首先是不稳定系统，其次是高灵敏系统（即输入到状态的变化很快，都是豪秒级别），再就是本系统属于不确定时滞系统。这样选取适合的H∞模型方程就很关键，本次打算首先采用这种方法尝试。

   方案3

      采用最优控制+干扰抑制，这方向尚未仔细研究，也没有做matlab仿真，作为备用方案。

方案4

      自适应控制+观测器,具体待续。

方案2实施

   首先，确定状态反馈阵，本次不详叙该过程。

   采用的模型方程为:
dX(t)=(A+△)X(t) + (B+△)u(t - d) + (Bw+△)w(t)

   其次，修改apm 姿态控制算法，修改原则是尽量保证不破坏原有结构，方便以后其他算法无缝替换

   以roll 定高为例先回顾一下姿态控制过程：

fast_loop()

attitude_control.rate_controller_run() (以下都在姿态控制对象中)

_motors.set_roll(rate_bf_to_motor_roll(_rate_bf_target.x))根据当前角速度计算姿态控制输出

         angle_ef_roll_pitch_rate_ef_yaw_smooth 函数，参数包含了目标姿态roll角度 (以下都在姿态控制对象中)

_angle_ef_target.x = roll_angle_ef  目标姿态roll角度

angle_ef_error.x = _angle_ef_target.x - _ahrs.roll_sensor计算角度差，这个我们用不到

frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error) 坐标转换

update_rate_bf_targets()

_rate_bf_target.x = _p_angle_roll.kP() * _angle_bf_error.x;   计算目标角速度， 这个我们用不到

H∞ 反馈控制对象

   输入：当前roll角度当前roll角速度目标角度

   输出： 控制输出

定义接口对象 AC_StateController 状态控制器，该对象虚函数 virtual float to_motor(float state1,float state2,float state3,float state4,float state5),该对象仅仅是

--实现状态反馈而已，所以非常简单。

根据以上控制流程，为了不破坏原有结构，以及兼容以后的新加控制算法，在AC_AttitudeControl中增加新的对象

AC_StateController* _rollController  (初始值 NULL)

AC_StateController*_pitchController (初始值 NULL)

修改 rate_controller_run 函数为:

if(_rollController)
_motors.set_roll(_rollController->to_motor(_angle_bf_error.x,_ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100));

    else 
_motors.set_roll(rate_bf_to_motor_roll(_rate_bf_target.x));
if(_pitchController)
_motors.set_roll(_pitchController->to_motor( _angle_bf_errory,_ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100));

    else 
_motors.set_pitch(rate_bf_to_motor_pitch(_rate_bf_target.y));

以上就完成了主要修改，完成细节部分后就可以直接测试了。

测试过程及现象，待续.

经过一段时间的调试，效果很差，具体就不描述了。

根据分析，感觉应该是模型方程不准确导致的，经过多次辨识，发现之前忽略了一些问题：系统可能是个时变系统

目前没有更好的办法，下一步打算采用 自适应控制：直接极点配置。