社区导航

 

搜索
查看: 10976|回复: 28

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

[复制链接]

25

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2015-8-3 21:35 | 显示全部楼层 |阅读模式
本帖最后由 xiongcaifei 于 2015-8-4 10:28 编辑

和大家分享下我做四轴的经验,参加电子竞赛时候接触到四轴飞行器,目前还在继续做。先上最新进展视频




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




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

1

1


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

2

2

3

3

4

4

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

5

5
串级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_outctrl.pitch.core.pid_outctrl.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

分享一下最好的一篇四轴飞行器论文 最好最详细四轴飞行器论文.pdf (4.93 MB, 下载次数: 3087)



此帖出自电子竞赛论坛


回复

使用道具 举报

25

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

 楼主| 发表于 2015-8-3 21:37 | 显示全部楼层
程序因为文件太大无法下载,大家可以到群里面下载,希望大家也在群里共享自己好的代码200718960


回复

使用道具 举报

2万

TA的帖子

74

TA的资源

管理员

Rank: 13Rank: 13Rank: 13Rank: 13

发表于 2015-8-4 09:02 | 显示全部楼层
楼主视频请查看一下,是否有效。我这里播放不了。

不管是哪年,都要加油!继续为中国电子行业做出小小的贡献吧! 扣扣 1206973913



回复

使用道具 举报

25

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

 楼主| 发表于 2015-8-4 10:27 | 显示全部楼层
soso 发表于 2015-8-4 09:02
楼主视频请查看一下,是否有效。我这里播放不了。

我可以看到啊

点评

[attachimg]208857[/attachimg]  详情 回复 发表于 2015-8-4 10:32


回复

使用道具 举报

2万

TA的帖子

74

TA的资源

管理员

Rank: 13Rank: 13Rank: 13Rank: 13

发表于 2015-8-4 10:32 | 显示全部楼层

不管是哪年,都要加油!继续为中国电子行业做出小小的贡献吧! 扣扣 1206973913



回复

使用道具 举报

25

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

 楼主| 发表于 2015-8-4 11:16 | 显示全部楼层


回复

使用道具 举报

9

TA的帖子

0

TA的资源

一粒金砂(初级)

Rank: 1

发表于 2015-8-4 11:28 | 显示全部楼层
就在房间里飞,太危险了吧


回复

使用道具 举报

4

TA的帖子

0

TA的资源

一粒金砂(初级)

Rank: 1

发表于 2015-8-4 14:54 | 显示全部楼层


回复

使用道具 举报

25

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

 楼主| 发表于 2015-8-4 15:43 | 显示全部楼层
子不语1125 发表于 2015-8-4 11:28
就在房间里飞,太危险了吧

很稳的  有经验就没事的


回复

使用道具 举报

4

TA的帖子

0

TA的资源

一粒金砂(初级)

Rank: 1

发表于 2015-8-4 15:59 | 显示全部楼层
已看呆,楼楼好强的说


回复

使用道具 举报

3

TA的帖子

0

TA的资源

一粒金砂(初级)

Rank: 1

发表于 2015-8-4 16:44 | 显示全部楼层
好厉害,学习学习


回复

使用道具 举报

3

TA的帖子

0

TA的资源

一粒金砂(初级)

Rank: 1

发表于 2015-8-4 18:38 | 显示全部楼层


回复

使用道具 举报

18

TA的帖子

0

TA的资源

一粒金砂(初级)

Rank: 1

发表于 2015-8-4 19:00 | 显示全部楼层
楼主,请问飞机循迹如何做到呢?


回复

使用道具 举报

25

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

 楼主| 发表于 2015-8-5 11:11 | 显示全部楼层

大家多多交流


回复

使用道具 举报

25

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

 楼主| 发表于 2015-8-5 11:11 | 显示全部楼层
小俊俊 发表于 2015-8-4 15:59
已看呆,楼楼好强的说

感兴趣可以进群多多交流的


回复

使用道具 举报

25

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

 楼主| 发表于 2015-8-5 11:12 | 显示全部楼层
xuanbo 发表于 2015-8-4 16:44
好厉害,学习学习

可以多多支持菜鸟飞行器的


回复

使用道具 举报

25

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

 楼主| 发表于 2015-8-5 11:13 | 显示全部楼层
本帖最后由 xiongcaifei 于 2015-8-5 12:08 编辑

互相交流,分享资料经验嘛


回复

使用道具 举报

25

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

 楼主| 发表于 2015-8-5 11:16 | 显示全部楼层
本帖最后由 xiongcaifei 于 2015-8-5 12:08 编辑
沙沙清木子 发表于 2015-8-4 19:00
楼主,请问飞机循迹如何做到呢?

CCD 摄像头 激光循迹。方向的改变可通过改变给定(目标)姿态角来改变的,就相当于改变遥控器的设定值来改变方向的。


回复

使用道具 举报

4

TA的帖子

0

TA的资源

一粒金砂(初级)

Rank: 1

发表于 2015-8-5 20:41 | 显示全部楼层
大神,你有木有陀螺仪学习的资料啊。

点评

可以进群大家交流的  详情 回复 发表于 2015-8-6 19:19


回复

使用道具 举报

8

TA的帖子

0

TA的资源

一粒金砂(初级)

Rank: 1

发表于 2015-8-5 22:08 | 显示全部楼层
求问,楼主是如何光流定位的


回复

使用道具 举报

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

关闭

站长推荐上一条 1/7 下一条

  • 论坛活动 E手掌握

    扫码关注
    EEWORLD 官方微信

  • EE福利  唾手可得

    扫码关注
    EE福利 唾手可得

Archiver|手机版|小黑屋|电子工程世界 ( 京ICP证 060456 )

GMT+8, 2020-5-31 22:22 , Processed in 0.603094 second(s), 43 queries , Gzip On, MemCache On.

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