KK算法讨论-个人见解.docx

KK算法讨论 1. KK飞控不同版本的控制结构 我们先来看看一般的反馈控制结构示意图，下图是摘自某个自动控制教程中的一个形象反馈控制结构图。 与此图相对应的，较专业一点的框图描述如下。 这个图中的图1是最常见的反馈控制结构，其中红色框中就类似于形象图中的“大脑”。而图2这种结构我在互联网和各种教科书中还没找到，同样的红色框中就类似于“大脑”，很显然图1、与图2是有明显的差别，这就像两个人在解决同一问题时有不同的策略与处理方式一样，但这并不违背反馈控制的定义，同样是一个闭环修正过程。 KK的汇编源代码中创始人Rolf R Bakke，2010年4月的控制结构采用图1模式。 德版作者Minsoo Kim，2010年10月（在前者基础上修改）和KK_C的控制结构采用图2模式。 2. KK核心算法源代码及分析 我们仅以横滚通道为例来比较分析不同版本的核心控制算法。 2.1 Rolf R Bakke，2010年4月算法的部分源代码 ;--- Calculate roll command output --- lds t, RollGyroDirection tst t brpl ma60 b16neg GyroRoll ma60: b16fdiv GyroRoll, ScaleGyro ;scale inputs b16fdiv RxInRoll, ScaleStick b16mov CtrlCommand, GyroRoll ;calculate error b16add CtrlCommand, RxInRoll b16add IntegralRoll, CtrlCommand ;I b16mov Integral, IntegralRoll ;I-term limit b16ldi IntegralLimit, I_TermLimitRoll rcall ILimit b16mov IntegralRoll, Integral b16mul CtrlCommand, GainPotP ;P gain b16mov Temp, IntegralRoll ;I gain b16mul Temp, GainPotI b16add CtrlCommand, Temp ;--- Add roll command output to motor 1,2,3,4 --- b16add MotorOut1, CtrlCommand b16add MotorOut2, CtrlCommand b16sub MotorOut3, CtrlCommand b16sub MotorOut4, CtrlCommand 该算法先求摇杆控制给定量与陀螺仪输出的“误差”，再对这个“误差”进行积分（注意！！！是对角速度误差的积分，而不是对角速度的积分），然后分别求比例项与积分项，再相加后分配到4个电机。 2.2 Minsoo Kim，2010年10月算法的部分源代码 ;--- 读陀螺仪 --- %%%%% 核心算法部分 %%%%% b16load GainInRoll ;横滚比例输出 ldi t, PotScaleRoll rcall FastDivide ;除以 2^t（快速除法） lds t, RollGyroDirection ;读取陀螺仪方向 tst t brpl ma60 rcall NegateXY ;取负值 ma60: b16store Temp b16mul GyroRoll, Temp ;乘横滚陀螺仪 b16load GainInRoll ;摇杆比例输出 ldi t, StickScaleRoll rcall FastDivide ;除以 2^t b16store Temp b16mul RxInRoll, Temp ;乘摇杆横滚输入 b16add RxInRoll, GyroRoll ;两个输出相加（摇杆+陀螺仪） ;--- 将横滚命令输出添加到 4 个电机 --- Minsoo Kim (2010.09.09) ;b16ldi Temp, 1.5 ;RxInRoll = RxInRoll * 1.5 ;b16mul RxInRoll, Temp b16sub MotorOut2, RxInRoll b16sub MotorOut3, RxInRoll b16add MotorOut1, RxInRoll b16add MotorOut4, RxInRoll 这个算法的核心是先将陀螺仪输出的角速度标定后乘以一个系数，再将摇杆给定值标定后再乘以一个系数，两者