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

1、2015-3-91参考丁君AHRS航姿解算中的两种滤波方法的比较研究，发现使用加速度的数据可以 解算横滚角（roll）和俯仰角（pitch）.2因开发板上单片机无ADC，无法对购买模块进行处理，故仅仅参考附带的程序。希望可以将adxl335模块的示例程序转移到mpu6050中，陀螺仪的数据暂时不用，仅仅使用加速度计的数据进行两个角度的解算。3老师想让我研究载体做圆周运动时加速度的解算，但是我想先从静态的开始，我觉得静态 下的测试是基础，上来就研究最难的我接受不了。所以我想先用三轴的加速度数据先把静态下的姿态解算出来。4我发现如果我仅仅可以解算静态下的姿态，无法解决载体做回转运动下的姿态我还是完

2、不成本科生的任务。因为本科生的任务是汽车姿态测量，所以光静止是不够的。2015-3-115校正这一环节是我所没有考虑到的，因为有偏差还有灵敏度不匹配。但前提是我要先解算 出来。6论文不应该是最后完成的，论文是边做边写的，最后应该是是复制粘贴修改格式和布局而 已。7.三轴加速度计和三辆磁传感器的姿态原理选择北东地坐标摹柞为导竝坐标系5系人戦体的纵轴0X,、横轴0Y,和竖轴0Z.构成的右手坐标系作为载休坐标 系（占系人则样爲到卜系的方向余弦矩阵为：sinrycosflcosinfainysinsinf +co$ysin#sin - siiryco(1)其中疔阳 分别为载体的横滚角（R胡）、俯仰角（

3、Pitch）和航向角（九闻人在载体坐标系b系下，重力场分城为# =g： g； g：l 当载体处于非加速运动状态时|导航坐标系-gsinBgsinycos(2)(4)nST的重力场分量为昭” =C0 0 g】S则有:从而得到横滾角和俯伸角分别为:y - arctanfgf/J)gcflsycos-1arcsin8可以尝试将adxl335的示例程序（淘宝模块）移植到 MPU6050中去解算横滚角和俯仰角。2015-3-121为什么示例程序产生的六轴数据跟我想象的不一样那呢？加速度计的数据不是98,没有小数点。但是我发现买的arm模块数据也不是98，而且我用手机里的磁铁去干扰磁强计时， 发现磁强计的

4、数据发生了很大的变化，如果真的要使用磁强计一定要注意周围的磁场干扰。看来加速度计的数据是可以用的，因为别人都可以做到。为什么示例程序中减去偏移量，而我却不能减去偏移量，比如+X的加速度最大值是16000,减去之后，在+x该等于零时，又出现了 -16000,这是我更不想看到的。2. extern float atan2 (float y, float x);程序格式又搞错了，人家不是atan2 (float y/float x)，否则很容易出现错误too many actual parameters.3. 现在的状态是不能进行全姿态解算，x轴的显示范围是(90(270o)，和我想要的范围(-90

5、5+90。)正好差了180o,但是减去还不行，减去后串口上只显示一个负号。还好汽车达不到那个角度土 90o,哪怕是在汽车测试中，但是飞机能达到啊。所以这个问题最终还 是要解决的。我想先把一个角度解算出来，然后去推广。我在主函数里改动 pitch=(i nt)(ata n(ratio)*180)/3.1415926)+180); 这一句不行，后来我改 动 void lcd_printf(char *s,int temp_data)函数里面，在第一句我加上了temp_data-=180;然后俯仰角就输出正常了，也不知道为什么。4. uchar是一个8位无符号数，表示范围0到255,而uint是十六

6、位无符号数，表示范围0到 65535.但是要注意的是8位单片机。(摘自网络)5. 现在能解一个俯仰角，下一个是横滚角。我想这两个角的性质应该是比较接近的。但 是论文不是这么写的。横滚角也解算出来了，但是航向角好像不能通过加速度计解算。2015-3-131. 因为航向角解算不出，所以找出MPU9150,希望采用其中的磁强计来解算磁航向角。 接下来下载相关datasheet并阅读。2. 现在的解算方法还不涉及迭代，所以现在还没出现那种随着时间的推移，误差累积越 来越严重的情况。当前的解算与值与当前的采集数据有关。20XX年3月15日1. 为什么不直接搞 DSP直接跳过ARM那一关，害怕，害怕就去学

7、。2. 老师可能需要我做一个松耦合组合导航，先让我去研究一下算法。然后再去用硬件实现。该整理资料了，整理完给老师一份，然后再说设计硬件编程的事。等我把航向角结算出 来后后立马去研究组合导航算法。方向错了，停止就是前进，否则以后都不能和老师交流了。这是很危险的。我是否应该听老师的，先搞算法，然后再去考虑接下来的实现。3. 网上的GPS模块没见有遵守I2C通信协议的，都是一个收一个发送，这样我还真得 考虑数据同步的问题。20XX年3月16日1. 我终于知道为什么用示例程序在串口上显示的数据为什么和我想象的那么不一样了， 因为你在初始化MPU6050的时候会对陀螺仪和加速度进行一些配置，其中包括一个

8、叫做fullscale range的配置，就拿陀螺仪来说如果你将这个范围配置在一 2000 s，那么这个范围所对应的sensitivity scale factor就是16.4 LSB s。感谢唐朔飞老师的计算机组成原理，感谢日本Asahi Kasei的数据手册，让我在看补数、补码的过程中，让我在看到日本磁强计 的测量数据与磁通密度的对比中让我想到了陀螺仪和加速度计也是这样的。谢谢你们。一个好的数据手册就应该让 user看明白。2. 怎样才能在陀螺仪的寄存器中的16bit数据中看出那个表示小数点？都不表示小数点，只有通过sensitivity scale factor之后才会产生小数点，这时的

9、数据才是精确地。好了， 这下可以全身心地投入到算法研究中去了，传感器输出的就是，加速度数据和角速度数据， 接下来你要做的就是研究一个适合车辆检测的算法了。3. MPU6050的程序迁移到9150中没有发现问题，接下来采集磁强计的数据。仅仅修改了取数据的顺序，加入了磁强计各个轴数据寄存器地址，没有使用WIA(Device ID)。采集成功后开始解算航向角，接下来需要考虑的是磁强计的三轴正方向与GYRO&ACC是不同的,所以在解算时提前要考虑好，GYRO&ACC的俯仰角和横滚角是什么，方向是怎么定义的，然后考虑相对于磁强计这些角度和方向又是如何。4定义绕x轴旋转为横滚角，绕 y轴为

10、俯仰角，绕z轴为航向角。5.Mpu9150显示9轴原始数据时出现问题，磁强计的数据不发生改变。我怀疑是磁强计 的CNTL寄存器没有配置，导致传感器处于掉电模式。问题依然没有解决。6现在的问题是和陀螺仪加速计在一起采集时，数据能采集出来，但是值一直保持不变，单独被采集时压根采集不出来。显示-00001，这个值哪怕不连接传感器都能做到。20XX年3月17日1磁强计与GYRO&ACC 使用的slaveaddress好像是一样的，这样读出的数据虽然不变 但是不是-00001 了，三轴读出的数据分别是 24354，-01549,00336。数据是一直往上传的。 难道是现在磁强计现在运行在单次测量

11、模式？2我觉得自己得好好处理一下磁强计的数据，因为磁强计的数据拥有四个符号位。H=(Single_Readl2C(REG_Address)&0x8f;与上 0x8f 后 4 位符号位只保留一位，值依然 没有改变，处理后的数据为03874，-30221,00336。但是这不能保证都是正数的补码，所以我决定建立一个数组，长度为13.3如何舍弃一个二进制数的前三位？不过我又发现多个符号位不影响从二进制转换为 十进制。4. IICwriteByte(0xd0,0x37,0x02);0xd0 是 mpu9150 的 slaveaddress,0x37 是旁路使能配置寄存器55，配置为0x02说明

12、处理器能够直接读取辅助I2C的数据。IICwriteByte(0xD0,0x6A,0x00);/0x6a是用户配置寄存器106,配置为辅助总线的逻辑由主I2C总线决定。IICwriteByte(0x18,0x0A,0x01);0x18 是磁强计的 slaveaddress,0x0a 是磁强计的控制寄存器CNTL,配置为单次测量模式。阿莫电子论坛上网友出的招数。SIme 日selectCADOVSS VSS 0 0 0 1 1 0 0 R1WVSS VDD 0 001101 RA/VVDD VSS Q001110 RWVDD VDD 0 D 01 1 1* R/WCADO14 openTST2O

13、S-open I9AK89756,4.2. Single Measurement ModeWhen single measurcniciU mode (X4ODE3:0“0&0l" is scth sensoi' is measured, find afte' sensor measurement and signal processing is finished, ineasureiiient data is stored to measurement data registers ( IXI. to HZ I), tlicn AKX975 C transi

14、ts tn 卩owcr-dcwii modeLy, On ransirinn t(i pavico-dovi'nmodeh X40DEp：0 rums to "0000： At the same time. DRDY bit in ST register turns to bilThis is called ''Data Ready11'. Wheri “ny of mgEiLwincnr dnra re号ish：rFlXL gerST2 regi骑ur 占苗山 o<tpcnuicnmode iw 皿皿ngeM frcuii Doxcr-tiei

15、wii iiidde to mtwr mocle- DRDY bit turns to "0' DRDY pin is in lie same state as DRDY bit.针对这一句话，每次读完数据我再配置为单次读写模式。int GetData(uchar REG_Address)uchar H,L;H=(Si ngle_Readl2C(MAGADD,REG_Address);Sin gle_Writel2C(MAGADD,CNTL, 0x01);L=Si ngle_Readl2C(MAGADD,REG_Address-1);Sin gle_WriteI2C(MAGA

16、DD,CNTL, 0x01);return (H<<8)+L;/ 合成数据6现在求解出来的航向角波动很大，非常不稳定，变化范围在-180o180o之间。也许航向角的解算与俯仰角和横滚角的取值范围也有关系。二者的取值范围均为-90o90a7现在航向角还是没有求出来，我想这可能是磁强计的数据有问题20XX年3月18日1现在的数据还是没有换算成真实数据，而且带小数点的数据通过串口往上传我还没试过。2另外提一点，我的俯仰角解算和横滚角解算用的方法和上面论文提到的有差异。9 = tan LAgrr+Az,outX.OLT + A2 Y.OUTLCg 心W(11)(12)(13)AN1057而

17、且解出来的值也不是我想要的航向角,而且我怀疑上面两个角度也不是我想要的航向角和横滚角。3到现在我才明白采样时间间隔内的角增量就是角速度，速度增量就是加速度，当时我 竟然还给老师说，用的数据不是加速度和角速度，太愚蠢了，没听老师的话真是太失败了。秦永元惯性导航第二版 P253答：（X J表示的各分量构造成的叉乘斜对称矩阵，© =a= axiayjazkb 二 bxi by j bzkc = Cxi Cy j Czkc = a b=aybz - azby iazbx -axbz j axby - aybxk= =i jk=ax ayazbxbybzCx0-azaybxCy |=|az0_

18、a*|bycj ayax0bjc = a b =a bi0-azayI（a汉）=|az0-axayax0 一5汽车测试中存在划桨效应吗？需要划桨效应补偿量 ；：Vsgim吗？划桨效应补偿项的 优化算法以划桨运动为环境条件， 那么在汽车测试中，尤其是做稳态回转时的环境条件又是 什么，这里可以分析一下，这里可以具体环境具体分析， 这里可以出现创新点，就是将工作 条件修改，这里就是研究生工作量的体现之处。6汽车在做等半径稳态回转时，我觉得俯仰角和横滚角是不变的，只有航向角在发生变 化。我觉得汽车在做稳态回转运动中角速度和比力分别是 t i；= j B' l cos't f t i；

19、= kC sin 't因为我觉得在车辆做稳态回转运动时，横滚角存在摆动，z轴上加速度波动较大。但只是感性的猜测，没有理性的数据支持。还有就是我希望自己的程序能够做到自适应，根据测试环境自动切换姿态解算算法。因为不同的环境条件下假设是不一样的，不能一概而论。7JQIN2P296又出了一个恶心的名词直流分量，细节，但就是卡着你让你过不去，想了 半天了，搁置一下吧。20XX 年 3 月 24 日 10:461将坐标系理解成为刚体，弓I入四元数描述刚体的转动，并建立四元数q。、q、q2、q?与坐标变换矩阵C,也就是姿态矩阵C；之间的关系。一旦有了姿态矩阵就可以解算三个角度。四元数Q包含了所有的

20、姿态信息，捷联惯导中的姿态更新实质是如何计算四元数Q。2我想尝试编写采用定时采样增量法解四元数微分方程解算姿态的程序。我觉得通过编程可以更加深入的了解四元数算法。3直在困惑 GPS在姿态解算中到底起什么作用，今天终于释然了，在（ QIN2P255 ）其中心的获取按下式进行必=述 一 + 必 2* 50）式中，応是捷联陀螺的输岀（对机械转子陀螺必须经过动、静态误差的补偿）匚由 姿态更新的最新值确定；胡“和就分别是位置速率和地球自转速率对于导航坐标系取地理坐标系的悄况咙十必=twifcosLl sinL 十VeRn式中为导航计算所得的最新值。这里虽然没有提到 GPS,但是GPS的信息可以从这里引入

21、，引入到位置速率和地球自转速 率，但是只能用用纬度信息 L,但是GPS输出的速度信息，而不是东北天方向的各个分量所以V和Vn还是得不到。证据：（1）NE0-6M GPS TESTAT0MALIENTEKKETO：Upload MEA Data SW NMEA Data Up Load: OFFLongitude：113. 32866 ELatitude:23< 30203 NAltitude:81. 3mSpeed:0. 232kni/hFix Mode： 3DValid satellite:12Visible satellite:15iitc n+H'7ni4/n?/is(2)

22、5f SGPVTG (地面速度信息，Track Made Good and Ground Speed)SGPVTG语句的基本格式如2SGPVTG,(1 )ST,(2),M,N,(4),K, *hh(CR)(LF)(1) 以真北为参考基准的地面航向(000359度，前面的0也将被传输)(2) 以磁北为参考基准的地面航向(000T59度，前面的0也将被传输)(3) 地而速率(000.0999.9节，前面的0也将被传输)(4) 地面速率(0000.01851.8公里/小时，前面的0也将被传输(5) 模式指示(Q自主定位，D二差分，E二估算，2数据无效)举例如下工$G PVTG195,N,0,361,

23、K,A*2A(3)(4)尿蚯.其施度和位制课罟相关性用亠阶马尔可走过程表示:网r.r =民h+论(5)低成本SINS/GPS组合导航系统的结构设计如图4.3J所示。水平对准 zlgjgfQlGFS信息性fl<1MU>航向对®图4.31低成本IMU/GPS组合导航系统结构设计fflfK:卅和g 电月砒置JI*鼻*V唯我只想说我缺少完整的速度信息（即三个方向），但是低成本的传感器不提供。而且如果真要 做组合导航就得需要经度、纬度、高度三个信息都需要。还有就是我以前不知道所谓位置解算，结果就是经度、纬度和高度，得到的就是地球上的位置。从上图最后一句话知道，要想解算姿态必须把速度和位置解算出来。也许下图中可能有缺陷，但是他说明姿态速度位置解算是一个整体。3在惯性导航和 GPS组合导航系统中各子系统的误差源和量测中引入的误差都是随机的，而卡尔曼滤波从概率统计最优的角度估算出系统误差并消除之。当然也可以使用经典的回路反馈法。这句话有力的说明了使用卡尔曼滤波的原因，就是为了消除误差。加速度计组合比力坐标变换位速度机体坐标系b容态阵r翌态阵航向，竇初值解算态解算航向*姿态组合指令解算4想问问老师 GPS提供的信息是位置信息（经度、纬度和高度）和速度（东向、 北向和天向）吗？5因为我处理的信号都是随机