本帖最后由 KGJ 于 2018-2-7 11:25 编辑
KGJ 飞控算法的特点与演变
一、KGJ飞控算法的特点
1、操控特点
摇杆偏离中心位置的大小与飞行器倾斜角度相对应，摇杆回中，飞行器回到水平姿态，大家可能觉得这个没什么特别的呀，几乎任何飞控都有这个自稳功能呀，接着请看下一点！
2、传感器
该飞控仅使用陀螺仪，无需加速度计，大家看到这里是不是觉得有点小意外了，但可能仍有人觉得，是不是利用角速度积分得到了角度，从而实现了姿态角的控制，再接着往下看吧！
3、控制算法
没有任何姿态的解算，也没有复杂的PID控制，仅有一个陀螺仪数据的比例叠加到电机上，是不是觉得有点神奇了，如果觉得还有点意思，就请继续关注下面的详细介绍吧。
二、KGJ飞控算法的演变
1、KGJ飞控的硬件
KGJ飞控的硬件完全兼容KK V5.5版本，区别在于处理器换成了Arduino Nano模块，这样更便于开发调试，PC机上有一个类似MWC飞控的GUI的参数观察与调试，当然是相当简化的最基本内容。
2、Rolf R Bakke 的 KK 算法核心介绍
最早的KK飞控是由 Rolf R Bakke 设计的，其算法的核心采用了PI控制算法，是用汇编语言写成的，也是相当经典的，当然也有很多辅助功能（这些辅助功能我感觉没什么用），现将部分能说明问题的代码摘录如下：
;-*- Quadrocopter (Quadrotor) Controller V4.7X-*-
;-*- All Code And Hardware Design -*-
;-*- By Rolf R Bakke, April, juli 2010 -*-
………………………………………………
; M1,CW M3,CCW
; * *
; \ /
; \ /
; \ /
; +
; / \
; / \
; / \
; * *
; M2,CCW M4,CW
………………………………………………
b16mov MotorOut1,RxInCollective
b16mov MotorOut2,RxInCollective
b16mov MotorOut3,RxInCollective
b16mov MotorOut4,RxInCollective
; 这一部分是将遥控接收的油门控制数据输出的四个电机的控制输出
;--- 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
; 上面这一段是计算横滚控制命令的 PI 修正
;--- Add roll command output to motor 1,2,3,4 ---
b16add MotorOut1, CtrlCommand
b16add MotorOut2, CtrlCommand
b16sub MotorOut3, CtrlCommand
b16sub MotorOut4, CtrlCommand
; 再将横滚的 PI 修正叠加到四个电机的输出
后面还有俯仰、偏航等几乎同样的算法，在此就不过多的赘述了。
但这里的核心是先求“摇杆控制数据”与“陀螺仪数据”的差，也就是“角速度误差”；再求的是“角速度误差的积分”；最后是“角速度误差” *P + “角速度误差的积分” * I，这是一个标准的PI算法。
这里提一个小插曲，网络上搜到的KK有两种说法，一个说是法国的开源项目，另一个说是韩国人开发的，据我所知后来发展的KK是韩国人做的，但前面这个肯定不是，从 Rolf R Bakke 的名字，和他最初的帖子 Build KapteinKUKs Simple and Low Part Count Quad, Hex and Tricopter Flight Controller 中的 KapteinKUK 来看，有点像是挪威人，这只是我的猜测，由于我的知识有限，也不能确切的获得答案。
3、德国人Minsoo Kim 修改的 KK 算法核心介绍
;-*- 四轴X模式控制器 V1.0 -*-
;-*- 所有代码和硬件设计 -*-
;-*- 作者：Rolf R Bakke -*-
;-*- 日期：2010年4月 -*-
;-*- -*-
;-*- 2010年10月，由Minsoo Kim修改的代码 -*-
;……………………………………………………
;M1 CW *\ /* M2 CCW
; \ /
; +
; / \
;M4 CCW */ \* M3 CW
;
;……………………………………………………
;--- 油门混合到 4 个电机的输出 ---
b16mov MotorOut1,RxInCollective
b16mov MotorOut2,RxInCollective
b16mov MotorOut3,RxInCollective
b16mov MotorOut4,RxInCollective
;--- 计算横滚输出命令 ---
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
德国人修改后的算法取消了积分环节 I，只留下了“角速度误差” *P，很多辅助功能还保留的，我真是有点不明白，陀螺仪的安装方向都已经确定了，还要保留陀螺仪的方向设置这项干什么，真是不可思议！
4、KK_C飞控算法核心介绍
KK_C 的作者是福建的 Gale，具体名字不详，这个程序在国内的影响还是比较大的，详情大家可参看下帖：
重磅：KK_C飞控软件下 载/详细说明书/完整源代码
和KK相比，KK_C 1.0版本增加了不少实用的功能：
（1）开机等待遥控器信号功能（保障使用安全）
（2）飞行器模式选择功能（免烧固件）
（3）全遥控器设置（免调电位器）
（4）软件消震动算法（可配置开/关）
（5）支持摇杆指数功能（可配置开/关）
（6）PI控制算法（电位器调节感度）
（7）333Hz高精度电调信号输出
（8）电位器正反向识别功能
作者是这样描述 KK_C 的：“我下载了KK的源码进行研究，比较麻烦的是，KK的源码是用汇编写的，很多人是看不懂的。…… 因此我花了几个晚上的时间，用C语言完整从零编写了KK飞控的源代码，并且增强了一些很有价值的功能，使得KK的使用更加方便。”
其他问题我也不讨论了，也仅谈谈其核心的 PI 控制算法。
核心代码如下：
// X Mode X模式
// 1 2
// X
// 3 4
Motor1=MotorLimitValue(thr + ail - ele + rud);
Motor2=MotorLimitValue(thr - ail - ele - rud);
Motor3=MotorLimitValue(thr + ail + ele - rud);
Motor4=MotorLimitValue(thr - ail + ele + rud);
//MotorLimitValue 是限幅，很简单此处不讨论
其中 ail、ele、rud 如下：
thr=RxThr;
ail=RxAil/4;
ele=RxEle/4;
//将陀螺仪信号累加到调节量上
ail+=GainAdj(GyroRol,GainRol)+GainAdj(GyroRolI,GainPit);
ele+=GainAdj(GyroPit,GainRol)+GainAdj(GyroPitI,GainPit);
rud=RxRud/4+GainAdj(GyroYaw,GainYaw);//+GainAdj(GyroYawI,GainPit);
// 感度调整
int GainAdj(int gyro,uchar gain)
{
int r;
r=gyro/8;
r*=gain;
return r/(128/8);
}
至此，我们先看 KK_C 的 P 算法，该算法是感度直接乘上陀螺仪的角速度值（注意这里不是角速度的误差值），也就是 P * Gyro，从这里我们已经看到，这不是一个 PID 算法中的 P 算法了。而且输出是将遥控器的摇杆控制量与陀螺仪*增益直接相加的，所以这里一步不知道是作者该写成 C 过程中的疏忽错误还是有意而为之（这在他公开发表的帖子里没看出是有意而为之的）！
关于 KK_C 的 I ，我们下面再来讨论。
//带回归计算积分值
GyroRolI=GyroIntegral(GyroRolI,GyroRol);
//带限幅/带回归积分函数
int GyroIntegral(int v,int delta)
{
//内部积分增益
delta/=4; //delta=-511~511 -> -127~127
v+=delta;
//限幅
if(v>2000) v=2000;
if(v<-2000) v=-2000;
//回归
v/=2;
return v;
}
这个函数，我们再加以简化，先去掉限幅，我们来看看这个函数可以简化成一个什么样的算式。
v = 0.5*v + 0.5*(delta/4)
这里 v 就是 KK_C 的积分，delta 就是零偏修正后的陀螺仪值，从上面这个式子，我们能得出一个什么结论呢？
这个式子是一个典型的一阶滤波算法，也就是对陀螺仪输出值的1/4进行了一个低通滤波（这个依然等效于陀螺仪的输出，KK飞控板的硬件上陀螺仪是经过RC低通滤波的），再乘上增益，最后与遥控器的控制量叠加在一起输出到电机。作者为什么这样做呢？那是因为前面的 P 项感度已经调到很大了，效果还不好，说明 P 值仍然偏小，为此增加这个所谓的积分项，感觉好一些了，其实作用就是将 P 又增大了一些而已，作者也说过，太大了会摆动，总之，这一切也就等效个 P，但注意这里仍然不是 P乘以“角速度误差”，也不是“角速度的积分”，仍然是个“角速度”。
|
来自安卓客户端
/2 
发表于 2018-2-7 11:19
收藏
楼主
