极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 5023|回复: 13

KGJ飞控不同传统的算法

[复制链接]
发表于 2018-1-16 10:33:54 | 显示全部楼层 |阅读模式
本帖最后由 wujingyu 于 2018-2-7 11:58 编辑

KGJ飞控不同传统的算法

讲 KGJ 飞控算法之前,先讲个小故事。
Н.А.伯恩斯坦的《论灵活性及其发展》一书中的一个小故事:
有一则关于石头的古老故事。城市广场中心自古就有一块巨大的圆石。市参议会曾经宣布过搬走这块大石头的比赛。有位实业家打算花一千卢布建造一个大平台,装在大圆辊轴上,来拉走这块大石头。另一个人建议用炸药把大石头炸碎成几小块搬运,整个工程只需花费七百卢布。这又来了一位农夫说:
“我能搬走这块石头,我只拿你五十卢布。”
“你打算怎么做?”
“紧靠大石头边上挖一个大坑,将石头丢进去,坑里挖出来的土摊平,铺在广场上。”
农民这么做了,给了它五十卢布,小说里写道,另外又加给他五十,算作奖赏切合实际的想象力。


这个故事告诉我们,要避免经验主义,本本主意、避免心理定势的负面作用,而要灵活有效的处理问题。

现在的四轴飞控的算法基本上都离不开双环PID欧拉角四元素互补滤波卡尔曼滤波,这些都是哪来的呢?我看基本上都是从国外抄搬过来的,国内的博士论文抄,硕士论文抄,现在的本科论文也在抄,不抄这些,就显示不出水平来,也就拿不到各种学位了。

其实四轴飞行器的自稳真的需要搞得那么复杂吗?我们是否回到原点去看看有没有别的,更好的路可走呢?


后面我们来介绍 KGJ 飞控算法。



有兴趣讨论 KGJ 飞控算法的可加入QQ群(310073926)讨论。
回复

使用道具 举报

 楼主| 发表于 2018-1-16 14:26:29 | 显示全部楼层
本帖最后由 wujingyu 于 2018-1-18 19:21 编辑

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乘以“角速度误差”,也不是“角速度的积分”,仍然是个“角速度”。


回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-1-16 15:40:23 | 显示全部楼层
本帖最后由 wujingyu 于 2018-1-23 11:03 编辑

KGJ 飞控算法

一、典型 X 型四轴飞行器飞行原理
布局.png

1、垂直运动-上升
上升.png
4个电机的转速同时增加,带动四个桨叶产生的升力大于飞行器的重量。

2、垂直运动-下降
下降.png
4个电机的转速同时减少,带动四个桨叶产生的升力小于飞行器的重量。

3、俯仰运动-前进
前进.png
1、2减,3、4加,前面升力小,后面升力大,飞行器前倾前进。

4、俯仰运动-后退
后退.png
1、2加,3、4减,前面升力大,后面升力小,飞行器后仰后退。

5、滚转运动-右侧飞
右飞.png
1、4加,2、3减,左侧升力大,右侧升力小,飞行器右倾右侧飞。

6、滚转运动-左侧飞
左飞.png
2、3加,1、4减,右侧升力大,左侧升力小,飞行器左倾左侧飞。

7、偏航运动-左转
左转.png
1、3加,2、4减,飞行器左转。

8、偏航运动-右转
右转.png
2、4加,1、3减,飞行器右转。

二、理想飞行器的控制算法

如何理解请结合前面我们讲的上升/下降、前后、左右以及左右转的分析图。

电机1 = 油门控制 – 俯仰控制 + 横滚控制 – 偏航控制
电机2 = 油门控制 – 俯仰控制 – 横滚控制 + 偏航控制
电机3 = 油门控制 + 俯仰控制 – 横滚控制 – 偏航控制
电机4 = 油门控制 + 俯仰控制 + 横滚控制 + 偏航控制



这是原理,实际中还要对遥控器的俯仰、横滚、偏航这几个控制分量进行标定,否则就会溢出超限,不能工作在最佳区间。

这个理想的控制算法,假设还是理想的情况下以前进为例,我方向摇杆往前推,后两个电机转速快,前两个电机转速慢,这样飞行器前倾,就实现了往前飞的目的,摇杆推的角度越大,飞行器前倾越厉害,往前飞行的速度就越快,这就实现了摇杆的动作量与倾角相对应的关系,这是不是很简单呀!

那么这个理想的控制算法有什么问题呢?问题就是实际情况并不理想,在各种不平衡以及扰动的情况下,飞行器会出现各种偏离,这就需要人不断的控制摇杆来修正,但人是反应是跟不上这种干扰造成的变化的,所以是不现实的。


三、KGJ 飞控实际控制算法原理

在各种扰动下,如果我们能想办法自动控制飞行器在三个正交坐标中的转动角速度都为 0,是不是就可实现飞行器的可操控性了。既然要控制“角速度”为 0,我们就需要角速度传感器,也就是陀螺仪,要实现三个正交轴的角速度都为 0,KK 飞控板是很合适的。

现在都流行开源硬件,所以我改造成 Arduino Nano 版的 KK 飞控硬件。另外现在大于250克的飞行器都不能随便乱飞了,所以我又改成控制有刷(空心杯)电机的小四轴,重量不超过90克,这样更安全,在室内都可随意飞了。

这样,我们在理想四轴的每个控制分量上都增加“角速度”控制分量,就能实现我们的目标,具体原理算式如下:

输出修正量 = 遥控给定值 + Kp×角速度(陀螺仪输出)

我们这里也可以说是 PID 算法中的 P 项,但要注意一点,我们应该这样去理解:

                Kp×(角速度 - 0

这里的 0 是 PID 算法中的目标值。

回复 支持 反对

使用道具 举报

发表于 2018-1-17 09:55:53 | 显示全部楼层
厉害~
虽然我没有怎么没看懂这个。。。。
回复 支持 反对

使用道具 举报

发表于 2018-1-19 10:16:08 | 显示全部楼层
控制原理确实是这个,但现在算法之所以复杂,是为了不断优化可靠性及功能,比如空翻、抗风、电磁等干扰、路径控制。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-1-19 11:44:26 来自手机 | 显示全部楼层
本帖最后由 wujingyu 于 2018-1-19 12:10 编辑

可靠性、电磁干扰主要是硬件和软件的冗余设计,与这个核心算法关系不大,抗风如果仅用陀螺仪和加速度计复杂的算法也是没什么效果的,路径控制是与定位系统有关的,与这个层次的算法也是没什么关系的,只有一键自动空翻与复杂算法是有关系的。
回复 支持 反对

使用道具 举报

发表于 2018-1-19 12:11:31 | 显示全部楼层
先膜拜一下,虽然一时半刻领悟不到多少,就当先马克吧
回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-1-20 21:17:52 来自手机 | 显示全部楼层
本帖最后由 wujingyu 于 2018-1-21 19:44 编辑

直升机锁尾陀螺仪的工作原理

1、传统型陀螺仪的运作
当直升机在停悬状态受到侧风的吹袭时,尾舵会产生偏移的现象,此时传统型陀螺仪会送出控制讯号至尾舵舵机,以相反的方向抵制尾舵的偏移,当尾舵停止偏移时,陀螺仪的控制讯号也随之消失。若侧风持续地吹袭直升机时,会造成尾舵不断地偏移,此时陀螺仪会一直抵制尾舵的偏移,直到尾舵移动至下风处,这就是风标效应。

2、AVCS 陀螺仪的运作
当尾舵受到侧风吹袭而产生偏移的现象时,陀螺仪会抵制尾舵的偏移,同时陀螺仪会计算出偏移的角度,并持续送出控制讯号以抵抗侧风,因此即使侧风不停地吹袭直升机时,尾舵依然不会产生偏移。换言之,陀螺仪会自动修正因侧风所引起的尾舵偏移。当直升机执行自转的动作时,尾舵舵机会依据机体旋转的角速度而摆动,当直升机停止自转时,尾舵舵机会判断应停止的位置,这就是自动偏移功能。

其实KGJ的算法与这个类似,只不过直机锁尾仅控制一个轴,四轴飞行器控制了三个轴而已。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-1-23 00:04:13 | 显示全部楼层
本帖最后由 wujingyu 于 2018-2-23 11:56 编辑

KGJ飞控的相关程序

1、KGJ 飞控程序

2、KGJ_GUI 程序

KGJ_GUI.png
回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-1-23 00:11:31 | 显示全部楼层
本帖最后由 wujingyu 于 2018-1-23 11:13 编辑

KGJ 四轴飞行器硬件

1、KGJ 四轴飞行器电机布局图
电机布局.jpg

2、KGJ 四轴飞行器系统连接图
系统连接图.png

3、KGJ 飞控原理图
KGJ 原理图.png

回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-1-23 11:05:56 | 显示全部楼层
本帖最后由 wujingyu 于 2018-1-31 21:52 编辑

KGJ 四轴飞行器材料清单
清单.png
回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-1-23 12:00:06 | 显示全部楼层
KGJ 四轴的安装
桨叶安装1.jpg

解锁
解锁.jpg

锁定
锁定.jpg
回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-1-31 21:58:56 | 显示全部楼层
本帖最后由 wujingyu 于 2018-2-1 09:47 编辑

KGJ 套件采购链接
采购.rar (9.14 KB, 下载次数: 8)
回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-2-4 16:38:19 | 显示全部楼层
本帖最后由 wujingyu 于 2018-2-5 15:38 编辑

KGJ 的 Mixly (米思齐)图形编程
KK_Mixly程序图.png
回复 支持 反对

使用道具 举报

 楼主| 发表于 2018-2-5 15:42:15 | 显示全部楼层
本帖最后由 wujingyu 于 2018-2-23 11:57 编辑

KGJ 的 Mixly 程序

飞控程序

配套的Mixly库

上位机程序


回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

Archiver|联系我们|极客工坊 ( 浙ICP备09023225号 )

GMT+8, 2019-7-17 05:27 , Processed in 0.055747 second(s), 26 queries .

Powered by Discuz! X3.4 Licensed

© 2001-2017 Comsenz Inc.

快速回复 返回顶部 返回列表