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

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

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

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

4、。但是我发现买的arm模块数据也不是98，而且我用手机里的磁铁去干扰磁强计时， 发现磁强计的数据发生了很大的变化，如果真的要使用磁强计一定要注意周围的磁场干扰。 看来加速度计的数据是可以用的，因为别人都可以做到。 为什么示例程序中减去偏移量，而我却不能减去偏移量，比如+X的加速度最大值是 16000,减去之后，在+x该等于零时，又出现了 -16000,这是我更不想看到的。 2. extern float atan2 (float y, float x);程序格式又搞错了，人家不是atan2 (float y/float x)， 否则很容易出现错误too many actual paramete

5、rs. 3. 现在的状态是不能进行全姿态解算，x轴的显示范围是(90(270o)，和我想要的范 围(-905+90。)正好差了180o,但是减去还不行，减去后串口上只显示一个负号。还好汽 车达不到那个角度土 90o,哪怕是在汽车测试中，但是飞机能达到啊。所以这个问题最终还 是要解决的。我想先把一个角度解算出来，然后去推广。 我在主函数里改动 pitch=(i nt)(ata n(ratio)*180)/3.1415926)+180); 这一句不行，后来我改 动 void lcd_printf(char *s,int temp_data)函数里面，在第一句我加上了temp_data-=180;然

6、后俯 仰角就输出正常了，也不知道为什么。 4. uchar是一个8位无符号数，表示范围0到255,而uint是十六位无符号数，表示范围0到 65535.但是要注意的是8位单片机。(摘自网络) 5. 现在能解一个俯仰角，下一个是横滚角。我想这两个角的性质应该是比较接近的。但 是论文不是这么写的。横滚角也解算出来了，但是航向角好像不能通过加速度计解算。 2015-3-13 1. 因为航向角解算不出，所以找出MPU9150,希望采用其中的磁强计来解算磁航向角。 接下来下载相关datasheet并阅读。 2. 现在的解算方法还不涉及迭代，所以现在还没出现那种随着时间的推移，误差累积越 来越严重的情况。

7、当前的解算与值与当前的采集数据有关。 20XX年3月15日 1. 为什么不直接搞 DSP直接跳过ARM那一关，害怕，害怕就去学。 2. 老师可能需要我做一个松耦合组合导航，先让我去研究一下算法。然后再去用硬件实 现。该整理资料了，整理完给老师一份，然后再说设计硬件编程的事。等我把航向角结算出 来后后立马去研究组合导航算法。方向错了，停止就是前进，否则以后都不能和老师交流了。 这是很危险的。我是否应该听老师的，先搞算法，然后再去考虑接下来的实现。 3. 网上的GPS模块没见有遵守I2C通信协议的，都是一个收一个发送，这样我还真得 考虑数据同步的问题。 20XX年3月16日 1. 我终于知道为什么

8、用示例程序在串口上显示的数据为什么和我想象的那么不一样了， 因为你在初始化MPU6050的时候会对陀螺仪和加速度进行一些配置，其中包括一个叫做full scale range的配置，就拿陀螺仪来说如果你将这个范围配置在一 2000 s，那么这个范围所 对应的sensitivity scale factor就是16.4 LSB s。感谢唐朔飞老师的计算机组成原理， 感谢日本Asahi Kasei的数据手册，让我在看补数、补码的过程中，让我在看到日本磁强计 的测量数据与磁通密度的对比中让我想到了陀螺仪和加速度计也是这样的。谢谢你们。一个 好的数据手册就应该让 user看明白。 2. 怎样才能在陀螺

9、仪的寄存器中的16bit数据中看出那个表示小数点？都不表示小数 点，只有通过sensitivity scale factor之后才会产生小数点，这时的数据才是精确地。好了， 这下可以全身心地投入到算法研究中去了，传感器输出的就是，加速度数据和角速度数据， 接下来你要做的就是研究一个适合车辆检测的算法了。 学习必备欢迎下载 3. MPU6050的程序迁移到9150中没有发现问题，接下来采集磁强计的数据。仅仅修改 了取数据的顺序，加入了磁强计各个轴数据寄存器地址，没有使用WIA(Device ID)。采集成 功后开始解算航向角，接下来需要考虑的是磁强计的三轴正方向与GYRO与上 0 x8f 后 4

10、 位符号位只保留一位，值依然 没有改变，处理后的数据为03874，-30221,00336。但是这不能保证都是正数的补码，所以 我决定建立一个数组，长度为13. 3如何舍弃一个二进制数的前三位？不过我又发现多个符号位不影响从二进制转换为 十进制。 4. IICwriteByte(0 xd0,0 x37,0 x02);0 xd0 是 mpu9150 的 slaveaddress,0 x37 是旁路使 能配置寄存器55，配置为0 x02说明处理器能够直接读取辅助I2C的数据。 IICwriteByte(0 xD0,0 x6A,0 x00); /0 x6a是用户配置寄存器106,配置为辅助总线的逻辑

11、由主I2C总线决定。 IICwriteByte(0 x18,0 x0A,0 x01);0 x18 是磁强计的 slaveaddress,0 x0a 是磁强计 的控制寄存器CNTL,配置为单次测量模式。 阿莫电子论坛上网友出的招数。 SIme 日select CADO VSS VSS 0 0 0 1 1 0 0 R1W VSS VDD 0 001101 RA/V VDD VSS Q001110 RW VDD VDD 0 D 01 1 1* R/W CADO 14 open TST2 OS- open I 9 AK8975 13 5 学习必备欢迎下载 6,4.2. Single Measureme

12、nt Mode When single measurcniciU mode (X4ODE3:0“0 H=(Si ngle_Readl2C(MAGADD,REG_Address); Sin gle_Writel2C(MAGADD,CNTL, 0 x01); L=Si ngle_Readl2C(MAGADD,REG_Address-1); Sin gle_WriteI2C(MAGADD,CNTL, 0 x01); return (H8)+L;/ 合成数据 6现在求解出来的航向角波动很大，非常不稳定，变化范围在-180o180o之间。也许航 向角的解算与俯仰角和横滚角的取值范围也有关系。二者的取值范

13、围均为-90o90a 7现在航向角还是没有求出来，我想这可能是磁强计的数据有问题 20XX年3月18日 1现在的数据还是没有换算成真实数据，而且带小数点的数据通过串口往上传我还没试 过。 2另外提一点，我的俯仰角解算和横滚角解算用的方法和上面论文提到的有差异。 9 = tan L Agrr +Az,out X.OLT + A2 Y.OUT LCg 心W (11) (12) (13) AN1057 而且解出来的值也不是我想要的航向角, 而且我怀疑上面两个角度也不是我想要的航向角 和横滚角。 3到现在我才明白采样时间间隔内的角增量就是角速度，速度增量就是加速度，当时我 竟然还给老师说，用的数据不是

14、加速度和角速度，太愚蠢了，没听老师的话真是太失败了。 秦永元惯性导航第二版 P253 答：（X J表示的各分量构造成的叉乘斜对称矩阵， = a= axiayjazk b 二 bxi by j bzk c = Cxi Cy j Czk c = a b =aybz - azby iazbx -axbz j axby - aybxk = = i jk =ax ayaz bxbybz Cx0-azaybx Cy |=|az0_a*|by cj ayax0bj c = a b =a b i0-azayI （a汉）=|az0-ax ayax0 一 5汽车测试中存在划桨效应吗？需要划桨效应补偿量 ；：Vsg

15、im吗？划桨效应补偿项的 优化算法以划桨运动为环境条件， 那么在汽车测试中，尤其是做稳态回转时的环境条件又是 什么，这里可以分析一下，这里可以具体环境具体分析， 这里可以出现创新点，就是将工作 条件修改，这里就是研究生工作量的体现之处。 6汽车在做等半径稳态回转时，我觉得俯仰角和横滚角是不变的，只有航向角在发生变 化。我觉得汽车在做稳态回转运动中角速度和比力分别是 t i；= j B l cost f t i； = kC sin t 因为我觉得在车辆做稳态回转运动时，横滚角存在摆动，z轴上加速度波动较大。但只 是感性的猜测，没有理性的数据支持。 还有就是我希望自己的程序能够做到自适应，根据测试

16、环境自动切换姿态解算算法。因 为不同的环境条件下假设是不一样的，不能一概而论。 7JQIN2P296又出了一个恶心的名词直流分量，细节，但就是卡着你让你过不去，想了 半天了，搁置一下吧。 20XX 年 3 月 24 日 10:46 1将坐标系理解成为刚体，弓I入四元数描述刚体的转动，并建立四元数q。、q、q2、q? 与坐标变换矩阵C,也就是姿态矩阵C；之间的关系。一旦有了姿态矩阵就可以解算三个角 度。四元数Q包含了所有的姿态信息，捷联惯导中的姿态更新实质是如何计算四元数Q。 2我想尝试编写采用定时采样增量法解四元数微分方程解算姿态的程序。我觉得通过编 程可以更加深入的了解四元数算法。 3直在困

17、惑 GPS在姿态解算中到底起什么作用，今天终于释然了，在（ QIN2P255 ） 其中心的获取按下式进行 必=述 一 + 必 2* 50） 式中，応是捷联陀螺的输岀（对机械转子陀螺必须经过动、静态误差的补偿）匚由 姿态更新的最新值确定；胡“和就分别是位置速率和地球自转速率对于导航坐标 系取地理坐标系的悄况 咙十必= twifcosL l sinL 十 Ve Rn 式中为导航计算所得的最新值。 这里虽然没有提到 GPS,但是GPS的信息可以从这里引入，引入到位置速率和地球自转速 率，但是只能用用纬度信息 L,但是GPS输出的速度信息，而不是东北天方向的各个分量 所以V和Vn还是得不到。 证据：（

18、1） NE0-6M GPS TEST AT0MALIENTEK KETO：Upload MEA Data SW NMEA Data Up Load: OFF Longitude：113. 32866 E Latitude:23 30203 N Altitude:81. 3m Speed:0. 232kni/h Fix Mode： 3D Valid satellite:12 Visible satellite:15 iitc n+H7ni4/n?/is (2) 5f SGPVTG (地面速度信息，Track Made Good and Ground Speed) SGPVTG语句的基本格式如2

19、SGPVTG,(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,K,A*2A (3) (4) 学习必备欢迎下载 尿蚯.其施度和位制课罟相关性用亠阶马尔可走过程表示: 网r.r =民h

20、+论 (5) 低成本SINS/GPS组合导航系统的结构设计如图4.3J所示。 水 平 对 准 zlgjgfQl GFS信息 性 fl 航向对 (6) 图4.31低成本IMU/GPS组合导航系统结构设计 学习必备欢迎下载 fflfK :卅和g 电月砒置JI* 鼻*V唯 我只想说我缺少完整的速度信息（即三个方向），但是低成本的传感器不提供。 而且如果真要 做组合导航就得需要经度、纬度、高度三个信息都需要。 还有就是我以前不知道所谓位置解算，结果就是经度、纬度和高度，得到的就是地球上 的位置。 从上图最后一句话知道，要想解算姿态必须把速度和位置解算出来。也许下图中可能有 缺陷，但是他说明姿态速度位置

21、解算是一个整体。 3在惯性导航和 GPS组合导航系统中各子系统的误差源和量测中引入的误差都是随机 的，而卡尔曼滤波从概率统计最优的角度估算出系统误差并消除之。当然也可以使用经典的 回路反馈法。 这句话有力的说明了使用卡尔曼滤波的原因，就是为了消除误差。 加速度计 组合 比力坐标 变换 位速度 机体坐标系b 容态阵r 翌态阵 航向，竇 初值 解算 态解算 航向*姿态 组合 指令解算 学习必备欢迎下载 4想问问老师 GPS提供的信息是位置信息（经度、纬度和高度）和速度（东向、 北向和天向）吗？ 5因为我处理的信号都是随机信号，没有确定的频谱，所以就不能使用处理确定性 信号的常规的滤波了，比如低通、高通、带通等。 20XX 年 3 月 25 日 16:54:23 1想买原子的 mini开发板，