亿加合和智能车制作
标题: 四轴飞行器姿态解算 超声波定高、光流定点详细解释 [打印本页]
作者: xiongcaifei 时间: 2015-8-3 19:37
标题: 四轴飞行器姿态解算 超声波定高、光流定点详细解释
四轴飞行器的前后、左右、顺时针和逆时针运动是通过姿态角（角度）来控制的，遥控器是用来设定目标姿态角的，只要测得的姿态与设定的目标姿态进行串级PID控制就可以使四轴飞行器稳定飞行了。所以做四轴最重要、最关键的就是姿态角测量和串级PID控制。
姿态结算：测量姿态的传感器主要是三轴加速度计、三轴陀螺仪和三轴磁力计。整个姿态描述的原理是：通过解算地理坐标系和机体坐标系的角度位置关系来得到姿态，由于传感器的测量误差（主要是陀螺仪的积分误差和振动引起的加速度计误差），导致测得的机体坐标系不准，得到的姿态也就会不准确。因为地理坐标系中的四轴飞行器所受的重力和磁场是个常量，所以将地理坐标系中的重力向量和磁场向量转换到机体坐标系中，此时转换到机体坐标系的重力向量和磁场向量与机体坐标系中测出来的重力向量和磁场向量会有误差，只要消除此误差，就可以校正机体坐标系，进而得到准确的姿态。消除误差的方法有卡尔曼滤波法、互补滤波法、姿态插值法等。考虑到计算能力和现有的资料，本设计采用互补滤波法，达到的效果也比较好。
[attach]80745[/attach]
姿态解算算法步骤：
姿态解算涉及到坐标的变换，还有互补滤波算法部分。该算法的实现步骤为：
[attach]80746[/attach]
[attach]80747[/attach]
[attach]80749[/attach]
姿态解算C语言算法具体实现：
norm =Q_rsqrt(ax*ax + ay*ay + az*az); //加速度计数据归一化
ax = ax*norm;
ay = ay *norm;
az = az *norm;
vx = 2*(q1q3- q0q2); //转换成机体坐标系中的重力向量
vy = 2*(q0q1+ q2q3);
vz = q0q0 -q1q1 - q2q2 + q3q3 ;
ex = (ay*vz -az*vy) ; //向量外积就是误差
ey = (az*vx -ax*vz) ;
ez = (ax*vy -ay*vx) ;
exInt = exInt+ VariableParameter(ex) * ex * Ki;//对误差积分
eyInt = eyInt+ VariableParameter(ey) * ey * Ki;
ezInt = ezInt+ VariableParameter(ez) * ez * Ki;
gx = gx + Kp* VariableParameter(ex) * ex + exInt; //补偿陀螺仪
gy = gy + Kp* VariableParameter(ey) * ey + eyInt;
gz = gz + Kp* VariableParameter(ez) * ez + ezInt;
q0 = q0 +(-q1*gx - q2*gy - q3*gz)*halfT; //四元数的微分方程
q1 = q1 +(q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 +(q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 +(q0*gz + q1*gy - q2*gx)*halfT;
norm =Q_rsqrt(q0q0 + q1q1 + q2q2 + q3q3); //四元数归一化
q0 = q0 *norm;
q1 = q1 *norm;
q2 = q2 *norm;
q3 = q3 *norm;
angle.pitch =asin(-2*q1q3 + 2*q0q2); //四元数转换成欧拉角
angle.roll =atan2(2*q2q3 + 2*q0q1, -2*q1q1 - 2*q2q2 + 1);
串级PID控制：姿态角PID+姿态角速度PID
姿态角速度就是陀螺仪测量的数据，为了使四轴飞行器稳定飞行，应该使飞行器姿态稳定的时候，姿态速度也应为零，故采用角度和角速度串级PID控制器。由于姿态速度响应的速度比姿态响应的速度快，四轴飞行器飞行的时候引起的姿态速度变化也比姿态速度变化大，故姿态速度控制作为内环，姿态控制作为外环，也就是角度外环和角速度内环组成的串级PID控制器。
[attach]80750[/attach]
串级PID具体C语言代码：
if(ctrl.ctrlRate >= 2) //内环进行2次控制、外环进行1次控制、内环控制频率为外环2倍
{
//*****************外环PID**************************//
//俯仰计算//
pit=pit- (Rc_Data.PITCH - Rc_Data.pitch_offset)/30;
ctrl.pitch.shell.increment+= pit; //俯仰方向误差积分
//积分限幅
if(ctrl.pitch.shell.increment> ctrl.pitch.shell.increment_max)
ctrl.pitch.shell.increment= ctrl.pitch.shell.increment_max;
elseif(ctrl.pitch.shell.increment < -ctrl.pitch.shell.increment_max)
ctrl.pitch.shell.increment= -ctrl.pitch.shell.increment_max;
ctrl.pitch.shell.pid_out= ctrl.pitch.shell.kp * pit + ctrl.pitch.shell.ki * ctrl.pitch.shell.increment+ ctrl.pitch.shell.kd * (pit - pitch_old);
pitch_old= pit; //储存 俯仰偏差
//横滚计算//
rol=rol- (Rc_Data.ROLL - Rc_Data.roll_offset)/30;
ctrl.roll.shell.increment+= rol; //横滚方向误差积分
//积分限幅
if(ctrl.roll.shell.increment> ctrl.roll.shell.increment_max)
ctrl.roll.shell.increment= ctrl.roll.shell.increment_max;
elseif(ctrl.roll.shell.increment < -ctrl.roll.shell.increment_max)
ctrl.roll.shell.increment= -ctrl.roll.shell.increment_max;
ctrl.roll.shell.pid_out = ctrl.roll.shell.kp * rol +ctrl.roll.shell.ki * ctrl.roll.shell.increment + ctrl.roll.shell.kd * (rol -roll_old);
roll_old= rol; //储存 横滚偏差
//航向计算////////////
ctrl.yaw.shell.pid_out = ctrl.yaw.shell.kp * (Rc_Data.YAW -Rc_Data.yaw_offset)/10 + ctrl.yaw.shell.kd * sensor.gyro.origin.z;
yaw_old=yaw;
ctrl.ctrlRate = 0;
}
ctrl.ctrlRate++;
//*************内环(角速度环)PD****************************//
ctrl.roll.core.kp_out= ctrl.roll.core.kp * (ctrl.roll.shell.pid_out + sensor.gyro.radian.y *RtA);
ctrl.roll.core.kd_out= ctrl.roll.core.kd * (sensor.gyro.origin.y - sensor.gyro.histor.y);
ctrl.pitch.core.kp_out= ctrl.pitch.core.kp * (ctrl.pitch.shell.pid_out + sensor.gyro.radian.x * RtA);
ctrl.pitch.core.kd_out= ctrl.pitch.core.kd * (sensor.gyro.origin.x - sensor.gyro.histor.x);
ctrl.yaw.core.kp_out= ctrl.yaw.core.kp * (ctrl.yaw.shell.pid_out + sensor.gyro.radian.z * RtA);
ctrl.yaw.core.kd_out= ctrl.yaw.core.kd * (sensor.gyro.origin.z - sensor.gyro.histor.z);
ctrl.roll.core.pid_out= ctrl.roll.core.kp_out + ctrl.roll.core.kd_out;
ctrl.pitch.core.pid_out= ctrl.pitch.core.kp_out + ctrl.pitch.core.kd_out;
ctrl.yaw.core.pid_out= ctrl.yaw.core.kp_out + ctrl.yaw.core.kd_out;
ctrl.roll.core.pid_out、ctrl.pitch.core.pid_out和ctrl.yaw.core.pid_out为电机输出值。
超声波定高：
超声波定高就是测量高度数据与设定数据进行PID控制，在此I需要限幅，高度控制输出值代替油门输出即可。下面定高视频是一键起飞、自主定高、一键降落，定高1米，变化范围不超过5厘米。
光流定点：先上视频下次帖子将为大家介绍 请尽情期待。
四轴飞行器交流群（电子竞赛）：200718960 更多资料去https://shop127566156.taobao.com/index.htm?spm=2013.1.w5002-11745762171.2.VwngoE&qq-pf-to=pcqq.c2c
最好的一篇四轴飞行器论文[attach]80751[/attach]
[attach]80751[/attach][attach]80751[/attach][attach]80751[/attach]
作者: xiongcaifei 时间: 2015-8-3 19:38
本帖最后由 xiongcaifei 于 2015-8-3 19:43 编辑
大家帮顶起来啊，定高函数在ms5611.c文件里面，最下面那个函数），由于文件太大无法上传，大家进群200718960在群文件里面。
作者: 505357619 时间: 2015-8-4 08:00
好讲解
作者: WX001 时间: 2015-8-4 09:36
我还是顶你一发吧
作者: 麦籽小立 时间: 2015-8-4 09:41
好贴+10086
作者: 暴走蜗牛 时间: 2015-8-4 09:58
好棒 好棒 必须顶
作者: 潮流子韩 时间: 2015-8-4 10:07
顶 恒拓
作者: 步听雨 时间: 2015-8-4 10:27
赞一个
:victory::victory:
作者: 静待烟花美 时间: 2015-8-4 10:35
6666666
作者: 钟意黑色 时间: 2015-8-4 10:58
6666666666
作者: □□□ 时间: 2015-8-4 11:52
6的不行
作者: 泰庆电子 时间: 2015-8-4 12:13
用的是不是MPU6050 偏航角用磁力计矫正没。
作者: x502817492 时间: 2015-8-4 14:35
楼主大神，感谢你的定高代码呢 不过我们定点做的各种震荡 不知道你的是什么思路 好想看到讲解啊
:Q
作者: xiongcaifei 时间: 2015-8-4 15:43
是6050 偏航角校正了的
作者: dreamwei 时间: 2015-8-4 23:59
32个赞，顶起来，分享传播的力量
作者: 机遇 时间: 2015-8-5 08:45
lihai
作者: wwwio 时间: 2015-8-5 11:16
楼主好强大，我能问你几个问题吗？
1.四轴起飞后会向某个方向一直匀速移动，初步怀疑是电机启动不同时问题？楼主有遇过吗？怎么解决的？
2.四轴的电池电压的问题，电压对PID有影响吗？同样的PWM对不同的电压升力不一样，飞机高度以及飞行效果也完全不一样，应该怎么解决？只有零点几伏的差距。
望回复。
作者: xiongcaifei 时间: 2015-8-5 11:34
可以加群交流下的，更多资料可以访问淘宝店菜鸟飞行器的
作者: xiongcaifei 时间: 2015-8-5 11:35
本帖最后由 xiongcaifei 于 2015-8-5 12:05 编辑
可以加群交流下的，都志同道合嘛
作者: xiongcaifei 时间: 2015-8-5 11:35
可以加群交流下的，更多资料可以访问淘宝店菜鸟飞行器，提供技术指导，有售后保障的
作者: xiongcaifei 时间: 2015-8-5 11:36
是菜鸟飞行器~~不是一家店的
作者: xiongcaifei 时间: 2015-8-5 11:36
本帖最后由 xiongcaifei 于 2015-8-5 12:05 编辑
可以加群交流下的，都志同道合嘛
作者: MJPS3C2440 时间: 2015-8-5 11:37
这也是恒拓？
作者: xiongcaifei 时间: 2015-8-5 11:38
不是的，店名菜鸟飞行器 不是和他一家的我们提供技术指导的 可以加群交流下的
作者: xiongcaifei 时间: 2015-8-5 11:39
本帖最后由 xiongcaifei 于 2015-8-5 12:06 编辑
互相交流，分享资料经验嘛
作者: xiongcaifei 时间: 2015-8-5 11:39
本帖最后由 xiongcaifei 于 2015-8-5 12:06 编辑
互相交流，分享资料经验嘛
作者: xiongcaifei 时间: 2015-8-5 11:39
本帖最后由 xiongcaifei 于 2015-8-5 12:06 编辑
互相交流，分享资料经验嘛
作者: xiongcaifei 时间: 2015-8-5 11:40
本帖最后由 xiongcaifei 于 2015-8-5 12:07 编辑
也就一般了
作者: xiongcaifei 时间: 2015-8-5 11:40
是6050 校正了的。更多资料可以加群交流下，可以访问淘宝店菜鸟飞行器，提供技术指导的
作者: xiongcaifei 时间: 2015-8-5 11:41
本帖最后由 xiongcaifei 于 2015-8-5 12:07 编辑
嗯，互相交流，共享资料经验，大家一起提高嘛
作者: xiongcaifei 时间: 2015-8-5 11:42
嗯 这个等我们出了产品会有详细讲解的 请尽情期待哈
作者: xiongcaifei 时间: 2015-8-5 11:46
第一个问题就是说所的漂了，你又没加光流定点，往一边缓慢飞是很正常的
第二个问题就是电池质量问题了，电池电压必须是高于11V的 否则电池就损坏了 性能就会下降，最好用新电池吧 第一次充足电来，以后得好好保养电池，更多资料可以访问淘宝店菜鸟飞行器，提供技术指导的，也可以加群交流得
作者: FAruba611 时间: 2015-8-5 16:56
赞一个
:lol:lol
作者: MJPS3C2440 时间: 2015-8-5 17:25
恒拓是开源的吧
作者: xiongcaifei 时间: 2015-8-6 19:14
本帖最后由 xiongcaifei 于 2015-8-6 19:17 编辑
我们不是恒拓 我们是菜鸟飞行器，开源的
作者: xiongcaifei 时间: 2015-8-6 19:17
知道做四轴刚开始很难的，分享下经验一起提高嘛，也提高下咱大中华民族科技精神嘛
作者: 好心的路人 时间: 2015-8-6 20:13
好厉害
作者: xshenpan 时间: 2015-8-6 21:14
好贴，，赞赞赞
作者: Arthur打酱油 时间: 2015-8-7 05:54
楼主你的内环只用了PD处理么。。
作者: xiongcaifei 时间: 2015-8-7 07:39
是的
作者: 山广东 时间: 2015-8-7 08:06
:lol:lol:lol:lol:lol:lol
作者: foolwood 时间: 2015-8-7 17:26
有点屌~~~
作者: 若城 时间: 2015-8-8 17:50
确实挺好
作者: 若城 时间: 2015-8-8 17:52
楼主,那个姿态结算里面的Ki参数是自己设定的么,还是计算的? 解析非常不错
作者: ozxdno 时间: 2015-8-9 10:26
楼主你是说直接内环角速度，外环角度控制，然后根据pid输出角度去控制四路PWM吗？
pid控制结果与PWM之间的转换关系是什么啊？
作者: 风中的一匹狼 时间: 2015-8-9 11:53
:):)
作者: 风中溯 时间: 2015-8-26 11:37
:):)赞赞
作者: 49741933o 时间: 2015-9-2 17:07
本帖最后由 49741933o 于 2015-9-2 17:13 编辑
LZ,看了你贴的姿态解析核心代码里面有一个这样的函数：VariableParameter(ex)，不知道有什么含义？这好像跟论文里面的有点区别。
另外，我想问问从MPU6050读出来的原始数据是不是要进行滤波处理？滤波是不是为了除去因为电机震动而导致MPU6050输出的一些杂波。是不是滤完波的数据才有利用价值？滤完波之后就进行姿态结算？
作者: zb918 时间: 2015-9-7 16:01
楼主，PX4Flow光流模块求指导啊。。。
作者: whatghost 时间: 2015-9-27 10:43
可以:D:D
作者: 六步上篮 时间: 2015-12-2 23:17
好贴 终于明白些了
作者: 好心的路人 时间: 2015-12-28 15:33
感觉自己高数没学好
作者: ぐ雨灬枫ゾ 时间: 2015-12-29 08:58
厉害，非得很稳
作者: 智能车设计 时间: 2016-1-21 12:41
我还在等着你的光流定点的讲解呢
作者: 快乐！ 时间: 2016-2-22 17:47
楼主，你们用的哪个光流模块？
作者: 快乐！ 时间: 2016-4-7 21:35
大神，有空讨论讨论PX4flow可以么？？
作者: 牵手。。柱间 时间: 2016-4-27 21:48
请问菜鸟飞行器光流定点开源吗？？gps定点开源吗？？开源我就买
作者: 情尽江湖 时间: 2016-5-17 16:00
好贴
作者: dasjdbsajhdas 时间: 2016-5-23 18:59
HTHAWT垃圾飞控 用过的人才知道
作者: cim 时间: 2016-6-27 13:48
好棒 必须顶
作者: cim 时间: 2016-6-27 13:58
好讲解
作者: 望若闲云 时间: 2016-7-7 00:04
关于光流部分的解释呢，表示很难理解那个
作者: node 时间: 2016-7-7 06:33
楼主真棒。绝对是自己亲自做过。
作者: tt545571022 时间: 2016-8-10 11:14
麻烦说一下群号码 啊 我想请教一个问题
作者: lzr325 时间: 2016-9-8 13:13
好讲解
作者: 逸城枫景 时间: 2016-9-10 21:42
谢谢分享
作者: 局外人a 时间: 2016-10-21 11:29
好厉害 完全看不懂
作者: suitaquba 时间: 2020-5-4 13:52
66666
| 欢迎光临 亿加合和智能车制作 (http://www.znczz.com/) |
Powered by Discuz! X3.2 |