最近做小四轴的一些问题

最近在折腾原来做过的小四轴，把数据用Matlab处理了一下

主要有一个问题就是震动很大，平着放的话角度能震动到30-40度

但是装上桨之后就没那么震了，但是还有一个问题就是会死机

首先想到是电机转动的时候电池电压被拉低但是我调试的时候是连着串口线有供电的

所以猜想是程序的问题，我把PID部分去掉了，只保留姿态解算部分，在我不插桨的时候是好的

但是当我插上桨的时候就又死机了

但是我还不明白是怎么回事，好像和程序有关

感觉可能和芯片也有关

因为这个芯片可能是用过的旧芯片，即STM32F405

在我焊上去的时候是有读写保护的

在ST—Link Utility里面选Target选项更改Option Bytes.

先把几张图一存

上面是单片机1000Hz速度解算的角度（震到100多度了。。。）

下面是把陀螺仪加速度计的数据自己用Matlab处理的角度

这个角是pitch

角Roll

还是有些差别的

上面都是没插j桨的，插上桨的是这样

ax 滤波前 滤波后

ay 滤波前 滤波后

az 滤波前 滤波后

z的滤波的效果看起来还是不错的

三个角度 pitch roll yaw

imu的Matlab程序如下

clear all;clc;Kp=1.0;%比例常数Ki=0.005;%积分常数halfT=0.002;%半周期T=0.004;%周期为4ms%变量定义q0 = 1; q1 = 0; q2 = 0; q3 = 0;     %四元数exInt = 0;eyInt = 0; ezInt = 0;    %误差积分累计值 pitch=0.0; roll=0.0; yaw=0.0;t_pitch=zeros(2048,1);t_roll=zeros(2048,1);t_yaw=zeros(2048,1);%发送的频率为250Hz,pitch,roll,yawtest_dat=load('data_minicopter_1023_3.txt');gx=test_dat(1:2048,1);gy=test_dat(1:2048,2);gz=test_dat(1:2048,3);ax=test_dat(1:2048,7);ay=test_dat(1:2048,8);az=test_dat(1:2048,9);for ii=1:2048    %四元数积分，求得当前的姿态q0_last = q0;q1_last = q1;q2_last = q2;q3_last = q3;    %把加速度计的三维向量转成单位向量norm = 1/sqrt(ax(ii)*ax(ii) + ay(ii)*ay(ii) + az(ii)*az(ii));ax(ii) = ax(ii)* norm;ay(ii)= ay(ii)* norm;az(ii)= az(ii)* norm;%估计重力加速度方向在飞行器坐标系中的表示，为四元数表示的旋转矩阵的第三行vx = 2*(q1.*q3 - q0.*q2);vy = 2*(q0.*q1 + q2.*q3);vz = q0.*q0 - q1.*q1 - q2.*q2 + q3.*q3;    %加速度计读取的方向与重力加速度方向的差值，用向量叉乘计算ex = ay(ii).*vz - az(ii).*vy;ey = az(ii).*vx - ax(ii).*vz;ez = ax(ii).*vy - ay(ii).*vx;    %误差累积，已与积分常数相乘exInt = exInt + ex.*Ki;eyInt = eyInt + ey.*Ki;ezInt = ezInt + ez.*Ki;%用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量gx(ii) = gx(ii) + Kp.*ex + exInt;gy(ii) = gy(ii) + Kp.*ey + eyInt;gz(ii) = gz(ii) + Kp.*ez + ezInt;%一阶近似算法q0 = q0_last + (-q1_last.*gx(ii) - q2_last.*gy(ii) - q3_last.*gz(ii)).*halfT;q1 = q1_last + ( q0_last.*gx(ii) + q2_last.*gz(ii) - q3_last.*gy(ii)).*halfT;q2 = q2_last + ( q0_last.*gy(ii) - q1_last.*gz(ii) + q3_last.*gx(ii)).*halfT;q3 = q3_last + ( q0_last.*gz(ii) + q1_last.*gy(ii) - q2_last.*gx(ii)).*halfT; %四元数规范化norm2 = 1./sqrt(q0.*q0 + q1.*q1 + q2.*q2 + q3.*q3);q0 = q0 .* norm2;q1 = q1 .* norm2;q2 = q2 .* norm2;q3 = q3 .* norm2;%     t_yaw(ii)=gz*(0.0610351* 0.001);%     t_pitch(ii)= -atan2(2.0*(q0*q1 + q2*q3),q0*q0 - q1*q1 - q2*q2 + q3*q3)*57.2957795;%     t_roll(ii)= asin (2.0*(q0*q2 - q1*q3))*57.2957795;    t_yaw(ii)=gz(ii)*(0.0610351* 0.001)/0.0010653;    t_pitch(ii)= -atan2(2.0*(q0.*q1 + q2.*q3),q0.*q0 - q1.*q1 - q2.*q2 + q3.*q3)*57.2957795;    t_roll(ii)= asin (2.0*(q0.*q2 - q1.*q3))*57.2957795;end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%figure(1);subplot(211);test_pitch=test_dat(1:2048,10);plot(test_pitch);subplot(212);plot(t_pitch);figure(2);subplot(211);test_roll=test_dat(1:2048,11);plot(test_roll);subplot(212);plot(t_roll);