用四元数控制四轴

js200300953 · 发表于 2012-9-15 17:53:23

本帖最后由 js200300953 于 2012-9-15 18:03 编辑

目前实现了起飞和降落，没有实现方向控制，所以只能用绳在水平方向拉着（见上一篇的视频）。每个控制周期分三步：计算当前姿态与目标姿态的误差，用误差得出需要的力矩，和用力矩控制四个电机的油门。

计算姿态误差
首先定义几个量，都是四元数。

其中旋转增量就是要得到的姿态误差了。
用误差计算力矩
用力矩控制油门
四轴的模型如图：（如无特别说明，所有坐标均为载体坐标系。）

地理坐标系采用东北天坐标系，X向东，Y向北，Z指天。机架为“X”型，在xOy平面上，第一二三四象限对应的电机为0、1、2、3号，0、2电机正转，1、3电机反转。
输出的油门是两个值的和：基础值和增量值。基础值由用来控制整体升力，各个电机相等。增量值用来实现稳定、加速等动作，相当于对基础值的微调，各个电机各不相同，且它们的和为0。力矩仅取决于增量值。假设电机提供的力矩与油门成正比（一般不是这样的，但没有设备确定《油门-力矩曲线》）。
如果需要x轴的力矩，则油门增量值应为：0、1电机正，2、3电机负，记作[1 1 -1 -1]。

y、z轴同理。

力矩修正系数用于平衡各轴的响应灵敏度。x、y的力矩由螺旋桨的升力直接提供，响应够灵敏，MO_x和MO_y相等，取比较小的值；而z则靠螺旋桨的反转矩，相同油门增量，能提供的力矩较小，所以MO_z应该比较大。想偷懒就可以都设为1。
把各轴的分量加起来就是任意轴的情况：

最后把油门增量值和基础值加起来，输出到电调，就完成一个控制周期了。

另外，昨天做了个“nRF24L01p转串口”，显示姿态、调整参数可以更方便了。

原文：四轴——完善中的控制算法

越写越乱了。
这个算法还不完整，目前就想到这些了。

hemeizhi · 发表于 2012-9-15 18:36:18

不错不错。

ck8215 · 发表于 2012-9-15 21:18:33

我关注你的新浪博客很久了，能发些相关的资料给我吗？我也想做一个。

js200300953 · 发表于 2012-9-15 21:27:19

ck8215 发表于 2012-9-15 21:18
我关注你的新浪博客很久了，能发些相关的资料给我吗？我也想做一个。

相关资料？

除了那篇论文，没有了，都是自己想出来的。
一起做吧，有问题就互相帮助啦。自己一个人太多东西要搞了。

ck8215 · 发表于 2012-9-15 23:41:26

哪篇论文啊？我看你在姿态解算过程中费了好大的劲，我想问一下你参考了德国MK的算法吗？

js200300953 · 发表于 2012-9-16 07:30:26

ck8215 发表于 2012-9-15 23:41
哪篇论文啊？我看你在姿态解算过程中费了好大的劲，我想问一下你参考了德国MK的算法吗？ ...

论文：http://www.amobbs.com/thread-5486551-2-1.html#pid6090721
姿态解算是控制的基础，很重要。
我没看过其他算法。根据其他人的讨论，很多其他算法都是航模遥控+姿态角控制的，而我想更智能+直接用四元数控制。
其实也不能说没有资料，论坛就有很多了。

kjspace123 · 发表于 2012-9-16 09:00:18

mark 一直想做四粥参考

boxboxbox · 发表于 2012-9-16 09:23:20

mark 参考

ck8215 · 发表于 2012-9-16 14:17:53

这是德国的MK，http://svn.mikrokopter.de/，算法很成熟啊。看看吧，我想多少会有帮助的！

fsswl · 发表于 2012-10-13 17:41:46

感觉楼主的方向很好,每一步的逻辑很严谨,但是又很通俗易懂,现在反而对楼主的研究学习的方法更感兴趣了~

顶楼主

fsswl · 发表于 2012-10-13 17:56:13

楼主的进度很快呢,我见你用NRF24L01做遥控,传输距离不太够远呢,你的机体比较大,也不太好室内飞

js200300953 · 发表于 2012-10-14 10:24:36

fsswl 发表于 2012-10-13 17:41
感觉楼主的方向很好,每一步的逻辑很严谨,但是又很通俗易懂,现在反而对楼主的研究学习的方法更感兴趣了~

顶 ...

“通俗易懂”，哈哈，听到这句话最高兴啦。
用nRF24l01，这个没办法，只有且只会这个。有其他无线模块推荐一下吗？数据速率不用很高，要双向的。

fsswl · 发表于 2012-10-14 10:40:18

js200300953 发表于 2012-10-14 10:24
“通俗易懂”，哈哈，听到这句话最高兴啦。
用nRF24l01，这个没办法，只有且只会这个。有其他无线模块推 ...

遥控的话我直接用航模用的遥控和接收机,当然平时室内调试是NRF24L01``````,如果是微型四轴室内玩玩的话,光NRF24L01就够了,只是大四轴室内各种不便``````

245680 · 发表于 2012-10-14 17:55:59

能说一下截图的这些东西是从哪篇论文弄来的吗

js200300953 · 发表于 2012-10-14 18:19:53

245680 发表于 2012-10-14 17:55
能说一下截图的这些东西是从哪篇论文弄来的吗

原创。
（字数不够）

shmtspd · 发表于 2012-10-21 12:54:09

楼主太强大了。支持。

qiangnubing · 发表于 2012-10-21 18:48:57

mark

reAper07 · 发表于 2012-10-24 20:27:39

真心看不懂。

agnd · 发表于 2012-10-24 23:05:20

js200300953 发表于 2012-9-16 07:30
论文：http://www.amobbs.com/thread-5486551-2-1.html#pid6090721
姿态解算是控制的基础，很重要。
我没 ...

是想直接用卡尔曼滤波控制吗？就是用卡尔曼滤波的“预测”算法先预测出下个解算周期的姿态，再与控制目标相比较决定控制策略？
俺数学和力学都忘差不多了，求算法公式。

螃蟹 · 发表于 2012-10-25 08:36:39

先mark,再学习

js200300953 · 发表于 2012-10-25 10:32:19

agnd 发表于 2012-10-24 23:05
是想直接用卡尔曼滤波控制吗？就是用卡尔曼滤波的“预测”算法先预测出下个解算周期的姿态，再与控制目标 ...

不懂卡尔曼。
就是把测得的姿态与目标姿态对比来控制。

huangrb · 发表于 2012-12-1 22:27:54

本帖最后由 huangrb 于 2012-12-1 22:32 编辑

关于旋转组合的问题。设原始姿态q0，绕x，y，z轴旋转的四元数为q1，q2，q3.那么组合就是q0*q1*q2*q3，如果是这样，我疑问的是，q0*q1后，q2的向量还是原来的y轴吗？我认为应该在乘q2前，通过q0*q1的结果确定飞机的y轴在世界坐标系里的真实坐标。是这样吗？
我现在是用旋转矩阵，飞机传回的是飞机四个臂的端点坐标，然后pc端直接将传回的端点坐标对角连线，近似显示一个飞机。飞机倾斜一个角度后，保持角度不变，只旋转z轴，结果pc显示的结果是，飞机绕世界坐标轴的z轴转，而不是飞机坐标轴的z轴

js200300953 · 发表于 2012-12-1 23:03:52

今天去试飞啦
http://v.youku.com/v_show/id_XNDgyNDYxMzIw.html

js200300953 · 发表于 2012-12-1 23:32:09

本帖最后由 js200300953 于 2012-12-1 23:34 编辑

huangrb 发表于 2012-12-1 22:27
关于旋转组合的问题。设原始姿态q0，绕x，y，z轴旋转的四元数为q1，q2，q3.那么组合就是q0*q1*q2*q3，如果 ...

这个旋转顺序问题比较麻烦。
如果把绕xyz的旋转分成q_x、q_y、q_z，然后再组合在一起，这种情况就相当于用欧拉角控制了，要考虑旋转顺序。
至于转z轴，如果想转机体的z，就要把q_z放在前面，而如果想转地理坐标系的z轴，就要把q_z放在后面。

我的处理方法是这样的
遥控信号有4个：th_base、th_x、th_y、th_z。
th_base是油门，标准化到[0,1]
th_x、th_y、th_z是方向控制，标准化到[-1,1]

打开控制时，获取当前姿态，把x和y设为0，然后单位化，得到基准姿态q_h。
先用th_x和th_y构造q_xy = [1 th_x*FACTOR th_y*FACTOR 0]。
然后用th_z构造q_z = [1 0 0 th_z*FACTOR]。
计算目标姿态q_t = q_h * q_xy * q_z，然后单位化q_t。
用本文讲的控制算法输出电调值。

其实是把x和y平等化了，用th_x和th_y构造旋转轴，这样就不用考虑xy的顺序了。
然后q_z和q_xy的顺序，就是决定：打z时，绕机体的z，还是地理坐标的z转了。

lyreliu · 发表于 2012-12-2 14:32:41

不错不错，学习啦

玉米 · 发表于 2012-12-4 09:24:01

顶一帖

阿莫小子~ · 发表于 2012-12-5 01:23:16

楼主很强大……好好学习打算在大学毕业前做做，尽全力……

redwolf310 · 发表于 2012-12-13 09:43:19

mark

kmani · 发表于 2012-12-16 18:28:39

js200300953 发表于 2012-12-1 23:03
今天去试飞啦

楼主第一次就飞的这么问，羡慕。
楼主的飞机在空中能飞多久？电池多少毫安的？

js200300953 · 发表于 2012-12-17 11:14:08

kmani 发表于 2012-12-16 18:28
楼主第一次就飞的这么问，羡慕。
楼主的飞机在空中能飞多久？电池多少毫安的？ ...

没长时间飞过，只是试试起飞。
电池是3S 2200mAh的。

fengyuzhails · 发表于 2012-12-17 11:26:16

做得不错，为什么不参考成熟开源呢，在别人的基础上修改，能有更高的突破。

js200300953 · 发表于 2012-12-17 12:58:33

fengyuzhails 发表于 2012-12-17 11:26
做得不错，为什么不参考成熟开源呢，在别人的基础上修改，能有更高的突破。 ...

我把“参考”，认为是“依赖”。
因为是自己玩玩的项目，有自己的想法就试试咯。

fengyuzhails · 发表于 2012-12-17 15:23:34

js200300953 发表于 2012-12-17 12:58
我把“参考”，认为是“依赖”。
因为是自己玩玩的项目，有自己的想法就试试咯。 ...

要学会参考，不是每件事都要从头做起，这个世界有那么多研究，如果都从头做起那是拿自己的生命去浪费。要学会在别人的基础上建立自己的方向。楼主既然有心钻研，说明你已经具有不错的底子，是个积极向上，刻苦钻研的好学生。不过四轴已经有太多人去研究了，研究资料也有一大堆，但是细心的话你会发现真正做到好的，没几个，除了大疆创新和零度智控，其他都需要爱好者花大量时间去折腾的飞控。那为什么楼主不站在大家已有的基础上寻求突破呢，不是说偷懒或者依赖，首先你要吃透原先的研究就很难，研究彻底后会少走弯路，然后用自己的能力去提高，做出属于自己的高性能飞控，也会给自己创造一个很大的机遇！

js200300953 · 发表于 2012-12-18 13:21:34

fengyuzhails 发表于 2012-12-17 15:23
要学会参考，不是每件事都要从头做起，这个世界有那么多研究，如果都从头做起那是拿自己的生命去浪费。要 ...

哈哈，不用那么认真。
只是觉得现有的方法有缺陷，需要从根改起。
多种方法，百花齐放，不是很好吗？

fengyuzhails · 发表于 2012-12-19 22:41:37

js200300953 发表于 2012-12-18 13:21
哈哈，不用那么认真。
只是觉得现有的方法有缺陷，需要从根改起。
多种方法，百花齐放，不是很好吗？ ...

其实我也是看很多人从头搞，然后搞个半吊子就拉到了。既然你选择从根改起，希望你能够坚持，一定要做到最后，把东西做到最好，性能更加优越，做到这点，你就成功了，到时候可以找我，我们一起合作。加我扣扣，545506015

js200300953 · 发表于 2012-12-21 08:52:26

fengyuzhails 发表于 2012-12-19 22:41
其实我也是看很多人从头搞，然后搞个半吊子就拉到了。既然你选择从根改起，希望你能够坚持，一定要做到最 ...

我会坚持下去的，尽管这是个无底洞。
现在在搞INS，对加速度积分，受到不少挫折，但会越挫越勇。

fengyuzhails · 发表于 2012-12-21 10:35:37

js200300953 发表于 2012-12-21 08:52
我会坚持下去的，尽管这是个无底洞。
现在在搞INS，对加速度积分，受到不少挫折，但会越挫越勇。 ...

好好干，加油，相信你会坚持下去

js200300953 · 发表于 2012-12-21 16:25:16

fengyuzhails 发表于 2012-12-21 10:35
好好干，加油，相信你会坚持下去

谢谢。

业未央 · 发表于 2013-3-7 14:47:46

js200300953 发表于 2012-12-21 16:25
谢谢。

你好，你的四轴现在怎么样了？

monkeynav · 发表于 2013-3-7 18:01:41

之前在uavdesign.net看到一篇文章讲解四元数表示的控制环
解释要比lz的更清晰一些，也有明确的物理意义

作者改变了角度的坐标系，把当前姿态到目标姿态的旋转分解成3个角度：
摇摆方向，摇摆量，自旋量（Z轴）
因为这三个量都是基于机体坐标系的，而且没有奇点，相对来说更有利于编写控制算法和调整参数。

http://www.uavdesign.net/adaptiv ... howall=&start=4

rovershie · 发表于 2013-3-7 23:05:13

本帖最后由 rovershie 于 2013-3-7 23:06 编辑

楼主的理论已经成功应用在小四轴上了。

请参看帖子： http://www.amobbs.com/thread-5504090-1-1.html

lpdpzc · 发表于 2013-3-8 09:39:20

MARK，关注中！

绿篱 · 发表于 2013-3-8 09:45:40

牛人啊。

js200300953 · 发表于 2013-3-10 11:42:37

业未央发表于 2013-3-7 14:47
你好，你的四轴现在怎么样了？

暂时停滞了。

js200300953 · 发表于 2013-3-10 11:43:42

monkeynav 发表于 2013-3-7 18:01
之前在uavdesign.net看到一篇文章讲解四元数表示的控制环
解释要比lz的更清晰一些，也有明确的物理意义

嗯，先看看，我觉得的应该差不多。

yzaka · 发表于 2013-8-26 15:43:18

还是标记一下吧

电子三剑客 · 发表于 2013-8-31 10:06:58

mark!!!!!!!!!!!!!!!!!!!!!!!!!

笑傲江湖 · 发表于 2013-9-1 19:38:51

阿禹_lsy · 发表于 2014-1-15 12:51:39

楼主的每一篇新浪博文我都仔细学习了，希望可以加楼主为好友。

用四元数控制四轴

本帖子中包含更多资源

阿莫论坛20周年了！感谢大家的支持与爱护！！