大学生电子设计
直播中

xiongcaifei

11年用户 25经验值
私信 关注
[分享]

四轴飞行器姿态解算 超声波定高 光流定点

本帖最后由 xiongcaifei 于 2015-11-22 10:42 编辑

和大家分享下我做四轴的经验，参加电子竞赛时候接触到四轴飞行器，目前还在继续做。先上最新进展视频[media]http://v.youku.com/v_show/id_XOTUzNjA2NjMy.html[/media]



四轴飞行器的前后、左右、顺时针和逆时针运动是通过姿态角（角度）来控制的，遥控器是用来设定目标姿态角的，只要测得的姿态与设定的目标姿态进行串级PID控制就可以使四轴飞行器稳定飞行了。所以做四轴最重要、最关键的就是姿态角测量和串级PID控制。


姿态结算：测量姿态的传感器主要是三轴加速度计、三轴陀螺仪和三轴磁力计。整个姿态描述的原理是：通过解算地理坐标系和机体坐标系的角度位置关系来得到姿态，由于传感器的测量误差（主要是陀螺仪的积分误差和振动引起的加速度计误差），导致测得的机体坐标系不准，得到的姿态也就会不准确。因为地理坐标系中的四轴飞行器所受的重力和磁场是个常量，所以将地理坐标系中的重力向量和磁场向量转换到机体坐标系中，此时转换到机体坐标系的重力向量和磁场向量与机体坐标系中测出来的重力向量和磁场向量会有误差，只要消除此误差，就可以校正机体坐标系，进而得到准确的姿态。消除误差的方法有卡尔曼滤波法、互补滤波法、姿态插值法等。考虑到计算能力和现有的资料，本设计采用互补滤波法，达到的效果也比较好。


姿态解算算法步骤：
姿态解算涉及到坐标的变换，还有互补滤波算法部分。该算法的实现步骤为：






姿态解算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控制器。


串级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厘米。
[media]http://v.youku.com/v_show/id_XMTMwMDAyMzM4OA==.html[/media]
光流定点：先上视频下次帖子将为大家介绍  请尽情期待。
[media]http://v.youku.com/v_show/id_XOTUzNjA1MDY0.html[/media]
四轴飞行器交流群200718960   希望大家互相交流互相提高  更多资料去菜鸟飞行器https://shop127566156.taobao.com/index.htm?spm=2013.1.w5002-11745762171.2.VwngoE&qq-pf-to=pcqq.c2c
最新GPS定点出来了[media]http://v.youku.com/v_show/id_XMTM4Nzc0ODEwOA==.html?from=y1.7-1.2[/media]
分享一下最好的一篇四轴飞行器论文
附件: 您需要登录才可以下载或查看附件。没有帐号？注册

回帖（38）

xiongcaifei

2015-8-3 21:47:49
程序因为文件太大无法下载，大家可以到群里面下载，希望大家也在群里共享自己好的代码200718960
1 举报

nihao2016

2015-8-7 10:58:17

膜拜！学习！赞赞赞赞赞！
举报

天明许也

2015-8-5 17:54:11
非常不错，很给力，刚好调试飞行器非常头疼的时候，从中得到启发和动力
举报

cc

2015-8-5 19:46:59
这才是真正滴大神
举报

haoliangli

2015-8-5 22:11:17
赞赞赞赞赞赞赞赞赞赞赞赞
举报

245388327

2015-8-6 15:08:53
hhhhhhhhhhhhhhhhhhhhhhh
举报

暮色渐蓝

2015-8-6 16:03:35
正在学光流，希望能有收获
举报

暮色渐蓝

2015-8-6 16:04:05
正在学光流，希望能有收获吧
举报

h1654155601.2376

2015-8-6 21:17:29
膜拜！学习！赞赞赞赞赞！
举报

dongying123

2015-8-7 09:36:11
引用: 越努力越幸运 发表于 2015-8-5 19:46
这才是真正滴大神

是啊，四轴飞行器姿态解算 超声波定高 光流定点 很难哦
举报

dongying123

2015-8-7 09:37:22
引用: 暮色渐蓝 发表于 2015-8-6 16:03
正在学光流，希望能有收获

你真牛，特别受用，希望再有好的资料可以分享
举报

xiongcaifei

2015-8-7 23:14:08
引用: 天明许也 发表于 2015-8-5 17:54
非常不错，很给力，刚好调试飞行器非常头疼的时候，从中得到启发和动力

大家一起交流嘛。那个菜鸟飞行器老板是个专业做四轴的  会很耐心解答我们技术算法问题的
举报

xiongcaifei

2015-8-7 23:15:42
引用: 越努力越幸运 发表于 2015-8-5 19:46
这才是真正滴大神

我不是大神的 我也是从菜鸟飞行器老板那学习到的，他们给的资料很好很全的  你可以去看看 就是最下面那个链接的
举报

xiongcaifei

2015-8-7 23:16:48
引用: haoliangli 发表于 2015-8-5 22:11
赞赞赞赞赞赞赞赞赞赞赞赞

大家一起交流嘛  可以进群去交流的 很不错的群  最后那个练级的老板很懂技术的  你可以去看看
举报

xiongcaifei

2015-8-7 23:19:23
引用: 暮色渐蓝 发表于 2015-8-6 16:04
正在学光流，希望能有收获吧

光流不错  技术问题可以进群去交流的 很不错的群  最后那个练级的老板很懂技术的  你可以去看看
举报

xiongcaifei

2015-8-7 23:21:46
引用: 脑残心不残 发表于 2015-8-6 21:17
膜拜！学习！赞赞赞赞赞！

强烈推荐那群 很不错的群  最后那个链接的老板很懂技术的  你可以去看看
举报

xiongcaifei

2015-8-7 23:22:49
引用: dongying123 发表于 2015-8-7 09:36
是啊，四轴飞行器姿态解算 超声波定高 光流定点 很难哦

大家一起学习提高嘛   学习算法的话推荐那群  最后那个链接的老板很懂技术的  你可以去看看的
举报

legendzhang

2015-8-9 11:51:49
引用: xiongcaifei 发表于 2015-8-3 21:47
程序因为文件太大无法下载，大家可以到群里面下载，希望大家也在群里共享自己好的代码200718960

ctrl.roll.core.kp_out= ctrl.roll.core.kp * (ctrl.roll.shell.pid_out + sensor.gyro.radian.y *RtA);请问下后面的sensor.gyro.radian.y *RtA分别是什么含义?谢了
举报

legendzhang

2015-8-9 11:59:32
ctrl.yaw.core.kp_out= ctrl.yaw.core.kp * (ctrl.yaw.shell.pid_out + sensor.gyro.radian.z * RtA);  请问后面的sensor.gyro.radian.z * RtA分别是什么含义?谢了
举报

更多回帖

38 1 7

相关帖子

飞行器 电路板 四轴飞行器
发帖
电子发烧友APP
|投诉反馈|电子发烧友网
© 2021 bbs.elecfans.com
湘ICP备2023018690号
×
20
完善资料，
赚取积分