四轴姿态解算分析.docxVIP

又花了将近一个星期，终于把姿态解算的框架完成了。仅仅是把陀螺仪、加速度计、罗盘融合在一起，得出旋转姿态，没有对加速度积分，没有用到气压计，几乎没有滤波。算是阶段性的工作吧，把框架设计得合理一点，以后添加/修改就很简单了。从传感器的读取，到四元数的学习，到空间旋转的处理方法，循序渐进，逐个击破。主要参考了以下资料（按阅读的时间循序）：《计算机图形学几何工具算法详解》（四元数转矩阵的公式是错的！）《交互式计算机图形学——基于OpenGL的自顶向下方法》维基百科——四元数框框的日记——四元数青衫湮痕——四元数Heaths blog——四元数与欧拉角之间的转换阿莫电子论坛——【原创】姿态估计下面总结一下“姿态解算”的过程，分为“传感器”、“四元数与旋转”、“姿态解算框架”、“长期融合”、“快速融合”四部分。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_RT_BUS_BUSY} */uint8_t l3g4200d_init(void){????if(I2C_transmit(L3G4200D_WHICH_I2C,l3g4200d_init_callback) == I2C_RT_TRANSMIT_NORMAL)????????return L3G4200D_RT_NORMAL;????return L3G4200D_RT_BUS_BUSY;}/*?* 初始化使用的I2C回调函数。 */uint8_t l3g4200d_init_callback(uint8_t seq,uint8_t * data){????const static uint8_t value[] = {????????I2C_addressToByte_write(L3G4200D_ADDRESS),?/* 总线地址。 */????????(0x80 | 0x20),?/* 寄存器地址。或0x80表示连续写。*/????????0xEF,?