|
楼主 |
发表于 2015-8-3 10:22:11
|
显示全部楼层
本帖最后由 lcw_swust 于 2015-8-3 10:23 编辑
本次对机架做了较大修改:
将机架做成工字型,且前后桨约有1/4面积的重叠,看起来比较紧凑。
PID算法的改进:
mot_dx=err0*Pidx_p+temp_i*Pidx_i+degv0*Pidx_d+(degv0-degv1)*Pidx_d2;
degv0在前一版本用的是陀螺仪与加速度传感器融合之后的数据,存在较大的抖动
且不够精细,现在degv0直接用陀螺仪的数据,效果很好,且姿态解算与PID的
频率均改为100Hz,现在的飞行器比以前稳定多了,不会有晃动,打舵响应也不错。
各单片机的波特率均改为38400,改正了无线串口的一个BUG。
目前仍存在的问题是不容易悬停,主要是没有作定高处理。
看来还是应当把加速度和气压计用上。
关于姿态解算:
我用的比较笨的办法,将重力作为三维向量,陀螺仪的数据用来对这个三维向量
进行旋转,二者再将座标进行互补滤波。
旋转公式参考《欧拉角的求取与互补滤波器在姿态解算中的应用.pdf》
关于矩阵什么的我就不多说了,还要注意是否要加负号。
- //----------------------------------最初的公式
- Mpu_Gx=-Mpu_Gx;//角反向
- Mpu_Gz=-Mpu_Gz;
- sx=sin((float)Mpu_Gx/GYDIV*3.14/PIDEG);//求各角的正余弦
- sy=sin((float)Mpu_Gy/GYDIV*3.14/PIDEG);
- sz=sin((float)Mpu_Gz/GYDIV*3.14/PIDEG);
- cx=cos((float)Mpu_Gx/GYDIV*3.14/PIDEG);
- cy=cos((float)Mpu_Gy/GYDIV*3.14/PIDEG);
- cz=cos((float)Mpu_Gz/GYDIV*3.14/PIDEG);
- //旋转公式
- GAX=cz*cy*GAX+(-sz*cx-cz*sy*sx)*GAY+(sz*sx-cz*sy*cx)*GAZ;
- GAY=sz*cy*GAX+(cz*cx-sz*sy*sx)*GAY+(-cz*sx-sz*sy*cx)*GAZ;
- GAZ=sy*GAX+(cz*sx)*GAY+(cy*cx)*GAZ;
-
- //----------------------------------旋转公式的简化
- //(cx=cy=cz=1,sx*sy=0,sin(x)=x)(角度极小的情况)
- x=Mpu_Gx;
- y=Mpu_Gy;
- z=Mpu_Gz;
- GAX=GAX+z*GAY/SINDIV+(-y)*GAZ/SINDIV;
- GAY=(-z)*GAX/SINDIV+GAY+x*GAZ/SINDIV;
- GAZ=y*GAX/SINDIV+(-x)*GAY/SINDIV+GAZ;
- GAX=(GAX*49+AX_av*100)/50;//互补滤波GAX~AX_av*100
- GAY=(GAY*49+AY_av*100)/50;
- GAZ=(GAZ*49+AZ_av*100)/50;
复制代码
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|