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

xiongcaifei

四轴飞行器姿态解算 超声波定高 光流定点 [复制链接]

 

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




四轴飞行器的前后、左右、顺时针和逆时针运动是通过姿态角（角度）来控制的，遥控器是用来设定目标姿态角的，只要测得的姿态与设定的目标姿态进行串级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厘米。

光流定点：先上视频下次帖子将为大家介绍  请尽情期待。

四轴飞行器交流群200718960   希望大家互相交流互相提高  更多资料去菜鸟飞行器https://shop127566156.taobao.com/index.htm?spm=2013.1.w5002-11745762171.2.VwngoE&qq-pf-to=pcqq.c2c

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

2015-8-3 21:34 上传

点击文件名下载附件

xiongcaifei

程序因为文件太大无法下载，大家可以到群里面下载，希望大家也在群里共享自己好的代码200718960

soso

楼主视频请查看一下，是否有效。我这里播放不了。

xiongcaifei

soso 发表于 2015-8-4 09:02
楼主视频请查看一下，是否有效。我这里播放不了。

我可以看到啊

soso

xiongcaifei 发表于 2015-8-4 10:27
我可以看到啊

xiongcaifei

soso 发表于 2015-8-4 10:32

http://v.youku.com/v_show/id_XOTUzNjA1MDY0.html
http://v.youku.com/v_show/id_XMTMwMDAyMzM4OA==.html

子不语1125

就在房间里飞，太危险了吧

Queen1

xiongcaifei

子不语1125 发表于 2015-8-4 11:28
就在房间里飞，太危险了吧

很稳的  有经验就没事的

小俊俊

已看呆，楼楼好强的说

xuanbo

好厉害，学习学习

栾耎栾

沙沙清木子

楼主，请问飞机循迹如何做到呢？

xiongcaifei

Queen1 发表于 2015-8-4 14:54

大家多多交流

xiongcaifei

小俊俊 发表于 2015-8-4 15:59
已看呆，楼楼好强的说

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

xiongcaifei

xuanbo 发表于 2015-8-4 16:44
好厉害，学习学习

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

xiongcaifei

栾耎栾 发表于 2015-8-4 18:38

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

xiongcaifei

沙沙清木子 发表于 2015-8-4 19:00
楼主，请问飞机循迹如何做到呢？

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

Queen1

大神，你有木有陀螺仪学习的资料啊。

sun4980991

求问，楼主是如何光流定位的