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

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

最好的一篇四轴飞行器论文

xiongcaifei

大家帮顶起来啊，定高函数在ms5611.c文件里面，最下面那个函数），由于文件太大无法上传，大家进群200718960在群文件里面。

505357619

好讲解

WX001

我还是顶你一发吧

麦籽小立

好贴+10086

暴走蜗牛

好棒  好棒  必须顶

潮流子韩

顶 恒拓

步听雨

赞一个:victory::victory:

静待烟花美

6666666

钟意黑色

6666666666