组合导航姿态解算学习笔记

2015-3-91.参考丁君《AHRS航姿解算中的两种滤波方法的比较研究》，发现使用加速度的数据可以解算横滚角（roll）和俯仰角（pitch）.2.因开发板上单片机无ADC，无法对购买模块进行处理，故仅仅参考附带的程序。

希望可以将adxl335模块的示例程序转移到mpu6050中，陀螺仪的数据暂时不用,仅仅使用加速度计的数据进行两个角度的解算。

3.老师想让我研究载体做圆周运动时加速度的解算，但是我想先从静态的开始，我觉得静态下的测试是基础，上来就研究最难的我接受不了。

所以我想先用三轴的加速度数据先把静态下的姿态解算出来。

4.我发现如果我仅仅可以解算静态下的姿态，无法解决载体做回转运动下的姿态我还是完不成本科生的任务。

因为本科生的任务是汽车姿态测量，所以光静止是不够的。

2015-3-115.校正这一环节是我所没有考虑到的，因为有偏差还有灵敏度不匹配。

但前提是我要先解算出来。

6.论文不应该是最后完成的，论文是边做边写的，最后应该是是复制粘贴修改格式和布局而已。

7.8.可以尝试将adxl335的示例程序（淘宝模块）移植到MPU6050中去解算横滚角和俯仰角。

2015-3-121.为什么示例程序产生的六轴数据跟我想象的不一样那呢？加速度计的数据不是9.8，没有小数点。

但是我发现买的arm模块数据也不是9.8，而且我用手机里的磁铁去干扰磁强计时，发现磁强计的数据发生了很大的变化，如果真的要使用磁强计一定要注意周围的磁场干扰。

看来加速度计的数据是可以用的，因为别人都可以做到。

为什么示例程序中减去偏移量，而我却不能减去偏移量，比如+x 的加速度最大值是16000，减去之后，在+x 该等于零时，又出现了-16000，这是我更不想看到的。

2.extern float atan2 (float y, float x);程序格式又搞错了，人家不是atan2 (float y/float x)，否则很容易出现错误too many actual parameters.3.现在的状态是不能进行全姿态解算，x 轴的显示范围是（90º~270º），和我想要的范围（-90º~+90º）正好差了180º，但是减去还不行，减去后串口上只显示一个负号。

还好汽车达不到那个角度±90º，哪怕是在汽车测试中，但是飞机能达到啊。

所以这个问题最终还是要解决的。

我想先把一个角度解算出来，然后去推广。

我在主函数里改动pitch=(int)(((atan(ratio)*180)/3.1415926)+180);这一句不行，后来我改动void lcd_printf(char *s,int temp_data)函数里面，在第一句我加上了temp_data-=180;然后俯仰角就输出正常了，也不知道为什么。

4.uchar 是一个8位无符号数,表示范围0到255,而uint 是十六位无符号数,表示范围0到65535.但是要注意的是8位单片机。

（摘自网络）5.现在能解一个俯仰角，下一个是横滚角。

我想这两个角的性质应该是比较接近的。

但是论文不是这么写的。

横滚角也解算出来了，但是航向角好像不能通过加速度计解算。

2015-3-131.因为航向角解算不出，所以找出MPU9150,希望采用其中的磁强计来解算磁航向角。

接下来下载相关datasheet 并阅读。

2.现在的解算方法还不涉及迭代，所以现在还没出现那种随着时间的推移，误差累积越来越严重的情况。

当前的解算与值与当前的采集数据有关。

2015年3月15日1.为什么不直接搞DSP 直接跳过ARM 那一关，害怕，害怕就去学。

2.老师可能需要我做一个松耦合组合导航，先让我去研究一下算法。

然后再去用硬件实现。

该整理资料了，整理完给老师一份，然后再说设计硬件编程的事。

等我把航向角结算出来后后立马去研究组合导航算法。

方向错了，停止就是前进，否则以后都不能和老师交流了。

这是很危险的。

我是否应该听老师的，先搞算法，然后再去考虑接下来的实现。

3.网上的GPS 模块没见有遵守I2C 通信协议的，都是一个收一个发送，这样我还真得考虑数据同步的问题。

2015年3月16日1.我终于知道为什么用示例程序在串口上显示的数据为什么和我想象的那么不一样了，因为你在初始化MPU6050的时候会对陀螺仪和加速度进行一些配置，其中包括一个叫做full scale range 的配置，就拿陀螺仪来说如果你将这个范围配置在s ︒±2000，那么这个范围所对应的sensitivity scale factor 就是16.4()s LSB ︒。

感谢唐朔飞老师的《计算机组成原理》，感谢日本Asahi Kasei 的数据手册，让我在看补数、补码的过程中，让我在看到日本磁强计的测量数据与磁通密度的对比中让我想到了陀螺仪和加速度计也是这样的。

谢谢你们。

一个好的数据手册就应该让user 看明白。

2.怎样才能在陀螺仪的寄存器中的16bit 数据中看出那个表示小数点？都不表示小数点，只有通过sensitivity scale factor 之后才会产生小数点，这时的数据才是精确地。

好了，这下可以全身心地投入到算法研究中去了，传感器输出的就是，加速度数据和角速度数据，接下来你要做的就是研究一个适合车辆检测的算法了。

3.MPU6050的程序迁移到9150中没有发现问题，接下来采集磁强计的数据。

仅仅修改了取数据的顺序，加入了磁强计各个轴数据寄存器地址，没有使用WIA(Device ID)。

采集成功后开始解算航向角，接下来需要考虑的是磁强计的三轴正方向与GYRO&ACC是不同的，所以在解算时提前要考虑好，GYRO&ACC的俯仰角和横滚角是什么，方向是怎么定义的，然后考虑相对于磁强计这些角度和方向又是如何。

4.定义绕x轴旋转为横滚角，绕y轴为俯仰角,绕z轴为航向角。

5.Mpu9150显示9轴原始数据时出现问题，磁强计的数据不发生改变。

我怀疑是磁强计的CNTL寄存器没有配置，导致传感器处于掉电模式。

问题依然没有解决。

6.现在的问题是和陀螺仪加速计在一起采集时，数据能采集出来，但是值一直保持不变，单独被采集时压根采集不出来。

显示-00001，这个值哪怕不连接传感器都能做到。

2015年3月17日1.磁强计与GYRO&ACC使用的slaveaddress好像是一样的，这样读出的数据虽然不变但是不是-00001了，三轴读出的数据分别是24354，-01549,00336。

数据是一直往上传的。

难道是现在磁强计现在运行在单次测量模式？2.我觉得自己得好好处理一下磁强计的数据，因为磁强计的数据拥有四个符号位。

H=(Single_ReadI2C(REG_Address))&0x8f;与上0x8f后4位符号位只保留一位，值依然没有改变，处理后的数据为03874，-30221,00336。

但是这不能保证都是正数的补码，所以我决定建立一个数组，长度为13.3.如何舍弃一个二进制数的前三位？不过我又发现多个符号位不影响从二进制转换为十进制。

4. IICwriteByte(0xd0,0x37,0x02); //0xd0是mpu9150的slaveaddress,0x37是旁路使能配置寄存器55，配置为0x02说明处理器能够直接读取辅助I2C的数据。

IICwriteByte(0xD0,0x6A,0x00);//0x6a是用户配置寄存器106,配置为辅助总线的逻辑由主I2C总线决定。

IICwriteByte(0x18,0x0A,0x01); //0x18是磁强计的slaveaddress,0x0a是磁强计的控制寄存器CNTL,配置为单次测量模式。

阿莫电子论坛上网友出的招数。

5.针对这一句话，每次读完数据我再配置为单次读写模式。

int GetData(uchar REG_Address){uchar H,L;H=(Single_ReadI2C(MAGADD,REG_Address));Single_WriteI2C(MAGADD,CNTL, 0x01);L=Single_ReadI2C(MAGADD,REG_Address-1);Single_WriteI2C(MAGADD,CNTL, 0x01);return (H<<8)+L; //合成数据}6.现在求解出来的航向角波动很大，非常不稳定，变化范围在-180º~180º之间。

也许航向角的解算与俯仰角和横滚角的取值范围也有关系。

二者的取值范围均为-90º~90º。

7.现在航向角还是没有求出来，我想这可能是磁强计的数据有问题2015年3月18日1.现在的数据还是没有换算成真实数据，而且带小数点的数据通过串口往上传我还没试过。

2.另外提一点，我的俯仰角解算和横滚角解算用的方法和上面论文提到的有差异。

[AN1057]而且φ解出来的值也不是我想要的航向角，而且我怀疑上面两个角度也不是我想要的航向角和横滚角。

3.到现在我才明白采样时间间隔内的角增量就是角速度，速度增量就是加速度，当时我竟然还给老师说，用的数据不是加速度和角速度，太愚蠢了，没听老师的话真是太失败了。

4.()⨯Φ=⎪⎪⎭⎫ ⎝⎛⨯Φφφφφsin sin ？[秦永元惯性导航第二版P253]答：()⨯Φ表示Φ的各分量构造成的叉乘斜对称矩阵，Φ=φ。

()()()zy x z y xx y y x z x x z y z z y z y x z y x z y x b b b a a a kj i kb a b a j b a b a i b a b a b ac kc j c i c c k b j b i b b k a j a i a a =-+-+-=⨯=++=++=++=()()⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡---=⨯⨯=⨯=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡---=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡000000x y x zy z z y x x y x zy z z y x a a a a a a a ba b a c b b b a a a a a a c c c5.汽车测试中存在划桨效应吗？需要划桨效应补偿量sculm V ∆吗？划桨效应补偿项的优化算法以划桨运动为环境条件，那么在汽车测试中，尤其是做稳态回转时的环境条件又是什么，这里可以分析一下，这里可以具体环境具体分析，这里可以出现创新点，就是将工作条件修改，这里就是研究生工作量的体现之处。

6.汽车在做等半径稳态回转时，我觉得俯仰角和横滚角是不变的，只有航向角在发生变化。

我觉得汽车在做稳态回转运动中角速度和比力分别是 ()()tC k t f t B j t Ω=ΩΩ=sin cos ω 因为我觉得在车辆做稳态回转运动时，横滚角存在摆动，z 轴上加速度波动较大。