四轴姿态解算.docxVIP

又花了将近一个星期，终于把姿态解算的框架完成了。仅仅是把陀螺仪、加速度计、罗盘融合在一起，得出旋转姿态，没有对加速度积分，没有用到气压计，几乎没有滤波。算是阶段性的工作吧，把框架设计得合理一点，以后添加/修改就很简单了。 从传感器的读取，到四元数的学习，到空间旋转的处理方法，循序渐进，逐个击破。主要参考了以下资料（按阅读的时间循序）： 《 HYPERLINK /product.aspx?product_id=8963918 \t _blank 计算机图形学几何工具算法详解》（四元数转矩阵的公式是错的！） 《 HYPERLINK /product.aspx?product_id\t _blank 交互式计算机图形学——基于OpenGL的自顶向下方法》  HYPERLINK /wiki/%E5%9B%9B%E5%85%83%E6%95%B0 \t _blank 维基百科——四元数  HYPERLINK /note/206638855/ \t _blank 框框的日记——四元数  HYPERLINK /Mrt-02/archive/2011/10/15/2213656.html \t _blank 青衫湮痕——四元数  HYPERLINK /heath/archive/2009/12/13/103127.html \t _blank Heaths blog——四元数与欧拉角之间的转换  HYPERLINK /forum.php?mod=viewthreadtid=4769138 \t _blank 阿莫电子论坛——【原创】姿态估计 下面总结一下“姿态解算”的过程，分为“传感器”、“四元数与旋转”、“姿态解算框架”、“长期融合”、“快速融合”四部分。 1.传感器 我用的是10轴姿态传感器模块，其中陀螺仪是L3G4200D，加速度计是ADXL345，罗盘是HMC5883L，气压计是BMP085。全部都通过一条共用的I2C总线访问，速度都支持400kHz。 先讲讲I2C库。要配置、读取传感器，首先把通信做好，这里就是I2C库了。现在大部分单片机都有支持中断的硬件I2C了，可以写个高效的I2C库。我只实现了主机发送和主机接收模式，这里简单介绍一下接口。接口函数主要有2个： uint8_t I2C_transmit???????????(uint8_t which,I2C_transmitCallback cb);void I2C_setNextCallback???????(uint8_t which,I2C_transmitCallback cb); I2C_transmit()用于触发一次传输（发送或接收），异步执行，调用后马上返回。其中有一个I2C_transmitCallback类型的参数，就是决定发送或接收、如何处理数据的回调函数了，其定义如下： /*?数据传输回调函数。?* 每（准备）传输一个字节都调用一次。?* 参数：?*??????seq??= 序号，第一次调用时为0，以后每次调用递增。?*??????data = seq==0时写(从机地址+W/R)到data。?*??????????????seq!=0时data是数据。发送就写data，接收就读data。?* 返回值表示下一步的行为：?*???I2C_RT_START = 发送开始信号。?*???I2C_RT_STOP??= 发送停止信号。?*???I2C_RT_REPEAT_START_OR_STOP = 如果有下一次传输，就发送RepeatStart，否则发送Stop。?*???I2C_RT_ACK???= 继续传送，回应ACK。?*???I2C_RT_NACK??= 继续传送，回应NACK。??*/typedef uint8_t (* I2C_transmitCallback)(uint8_t seq,uint8_t * data); 当用I2C_transmit()成功触发一次传输后，I2C库会根据需要调用回调函数，所以使用这个I2C库就是写回调函数了。 而I2C_setNextCallback()则是用来设置紧接着的一次传输的。当本次传输结束时，不发送“Stop”信号，而是发送“Repeat Start”信号，然后开始下一个传输。利用这个函数可以发起连续多次传输。 有了I2C库就可以操作传感器了，以L3G4200D为例讲解。先看看DataSheet，操作L3G4200D有两个步骤，首先配置好寄存器，然后不断获取数据。配置寄存器比较简单，就是发送一段数据，用到两个函数： /*??* 初始化芯片。?* 返回值 ： {L3G4200D_RT_NORMAL,L3G4200D_R