导航及无人机姿态检测融合算法很难?用sensor fusion来搞定!
,程序还是使用MPU6050的驱动,缺点是会偶尔丢失数据,自带的姿态融合算法的更新频率不高);
2.MCU:GD32F103CB(Gigadevice公司的,ARMcortex-M3内核,32位MCU,主频最高108Mhz,48Pin,与ST同型号的32位MCU 直接兼容,性价比更高,外接8M晶振,也可使用内部8M晶振,晶振远离传感器,避免干扰磁力计,不过当时考虑不周,这个芯片的timer太少了,以后会先采用GD32F103VCT6或者GD32F107VCT6);
3.电源芯片:TLV70233DBVR(TI的LDO,输入2-6V,输出3.3V,只需要外接2个X7R无极性陶瓷电容,价格太高)
4.串口:MAX3232(方便调试)
5.USB供电,输出电压5V
6.目前机架、电调、电池、电机和螺旋桨已买好,来张图,比较大众化:
上图:
图1:PCB的3D效果图,测试版,先追求调通得出姿态角,后续改版会做大的调整:
第二部分:软件
1.使用keil,uvision4.1.0,工具链:RealViewMDK-ARM Version4.12;
2.驱动:官方的MPU6050驱动inv_mpu.c和inv_mpu_dmp_motion_driver.c;
先看几个图,然后再说坐标轴的设定和算法部分。
上图:
图1:系统初始化,顺序从上到下依次是:初始化MPU、设置需要使用哪些传感器、设置陀螺仪测量范围(我设的是正负500度/s)、设置加速度计测量范围(我设的是正负4g)、配置fifo、设置采样率、装载DMP、设置陀螺仪轴向(比较重要)、使能DMP的一些玩意儿、设置DMP的FIFO、自校准陀螺仪和加速度计、开启DMP、开始姿态融合,见下图:
图2:由四元数求出的最终姿态角,其中Yaw为航向角,表示机头偏离正北方多少度,范
围-180到+180;Pitch为俯仰角,表示机头正方向与水平线的夹角,范围-90到+90;Roll为翻滚角,表示机翼与水平线的夹角,范围:-180到+180。下图为机身水平,且机头正北偏西37度左右的数据:
图3:下图为机翼水平,机头指向正北,且机头向下25度的数据
图4:下图为机头指向正北,保持水平,且机翼的右翼向下倾斜23度的数据
图5:看下欧拉角的奇异点,在奇异点处一个转动状态对应无穷多组自由度值,当物体转到这些奇异点附近,便没法求解。图中当Pitch为+90度时,机体的姿态便没法控制,Roll的轴向发生了变化。如下图:
第三部分:如何确定自己的轴向
首先,轴向的定义跟初始化四元数和最后结算的欧拉角有关,跟四元数更新算法无关,换句话说,不管你的轴向如何定义,姿态融合算法随便用,但是初始化四元数的公式和最后结算欧拉角的公式要做适当的改变,这个后面算法中有说。加速度计也好,陀螺仪也好,磁力计也好,他们的轴向都要满足右手定理,如下图:
再附上一段注释用于解释如何定义合理的轴向,以及如何正确旋转传感器的轴向,解释这么多其实就是说定义好的轴向要满足右手定理,如下图:
下图,旋转前是[x y z],旋转后就是[-y x z]:
下图是如何确定旋转角度的正方向,用右手握住坐标轴,拇指指向轴向的正方向,四个指头弯曲的方向就是旋转角度的正方向,在初始化四元数时,计算出的欧拉角的正方向也要满足这个条件:
我的程序使用的轴向如下图所示,未作任何改变:
第四部分:算法
第一步是校准,加速度计和陀螺仪我用的是MPU9150内部自校准,磁力计的校准采用如下方法:见附件-磁力计校准
第二步是初始化四元数,常见的轴向定义是绕x轴旋转是Roll,绕y轴旋转是Pitch,绕z轴旋转是Yaw,我的程序中也有这样的定义,不过被我注释掉了,这里举个另外一种轴向定义来对初始化四元数进行说明,方便比较,也是我目前正在用的轴定义。
下面我们来定义绕x轴旋转是Pitch,绕y轴旋转是Roll,绕z轴旋转是Yaw,轴向的正方向如上图一样,不变。
先对加速度计和磁力计的数据进行处理,得到init_xx来供我们使用如下图:
然后通过公式计算出初始化的Roll、Pitch、Yaw,注意加负号保证旋转角度的正方向,如下图:
其中Yaw的正方向未必对,可以自己去验证下,具体参考公式见附件-ST电子罗盘计算Yaw
然后由上面的欧拉角求出初始化四元数,这时要注意旋转顺序的不同,公式也不同,大部分旋转顺序是Z-Y-X,我的程序里也用的这个顺序,在这里我们按Z-X-Y的顺序来旋转,并得出求四元数的公式以做比较,其旋转矩阵:
q=qyaw*qpitch*qroll=
(cos(0.5*Yaw)+ksin(0.5*Yaw)) *(cos(0.5*Pitch)+isin(0.5* Pitch)) * (cos(0.5*Roll)+jsin(0.5* Roll))
得出初始化四元数计算公式如下图所示:
其中i,j,k之间相乘的顺序不能随意变,在前的先计算,
地磁传感器 运动传感器 3轴加速度计 3轴陀螺仪 意法半导体 相关文章:
- 详解地磁感应器原理/应用及优缺点(09-09)
- 飞思卡尔MMA955xL运动传感器大比拼(09-20)
- MEMS传感器种类及国内外生产厂商(10-02)
- 意法半导体:混合动力及电动汽车解决方案(02-02)
- 立足整个信号链需求 意法半导体音频解决方案(02-07)
- 意法半导体数字电视和机顶盒解决方案(02-07)