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

2015-3-9参考丁君《AHRS航姿解算中的两种滤波方法的比较研究》，发现使用加速度的数据可以解算横滚角（rol1）和俯仰角（pitch）.因开发板上单片机无ADC，无法对购买模块进行处理，故仅仅参考附带的程序。希望可以将adxl335模块的示例程序转移到mpu6050中，陀螺仪的数据暂时不用，仅仅使用加速度计的数据进行两个角度的解算。老师想让我研究载体做圆周运动时加速度的解算，但是我想先从静态的开始，我觉得静态下的测试是基础，上来就研究最难的我接受不了。所以我想先用三轴的加速度数据先把静态下的姿态解算出来。我发现如果我仅仅可以解算静态下的姿态，无法解决载体做回转运动下的姿态我还是完不成本科生的任务。因为本科生的任务是汽车姿态测量，所以光静止是不够的。2015-3-11校正这一环节是我所没有考虑到的，因为有偏差还有灵敏度不匹配。但前提是我要先解算出来。论文不应该是最后完成的，论文是边做边写的，最后应该是是复制粘贴修改格式和布局而已。7.三轴加速度计和三轴磁传感器的姿态原理选择北东地坐标系作为导航坐标系5系），载体的纵轴。名、横轴0%和竖轴ozb构成的右手坐标系作为载体坐标系3系），则n系到b系的方向余弦矩阵⑶为：~Costco*cos^sin^r-sin^"C=sinysin^cos^r-cosysin^rsinysin^sin^r+cosycostffsin,coM-cosysin^cos^+sinysin^rcosysin^sin^-sinycos^rcosycos^-（1）其中，祁部分别为载体的横滚角（R。"）、俯仰角（P血%）和航向角（丫。切）。在载体坐标系6系下，重力场分量为g"=以Srg”「，当载体处于非加速运动状态时，导航坐标系几系下的重力场分量为:g"=[00gJ则有：--gsitiH~TOC\o"1-5"\h\zg-C：g*=gsinycos^（2）-gcosycos^-从而得到横浪角和俯仰角⑷分别为：7-arctan（g；/g：）（3）0-arcsin（-gjg}⑷可以尝试将adx1335的示例程序（淘宝模块）移植到MPU6050中去解算横滚角和俯仰角。2015-3-12为什么示例程序产生的六轴数据跟我想象的不一样那呢？加速度计的数据不是9.8，没有小数点。但是我发现买的arm模块数据也不是9.8,而且我用手机里的磁铁去干扰磁强计时，发现磁强计的数据发生了很大的变化，如果真的要使用磁强计一定要注意周围的磁场干扰。看来加速度计的数据是可以用的，因为别人都可以做到。为什么示例程序中减去偏移量，而我却不能减去偏移量，比如+X的加速度最大值是16000，减去之后，在+x该等于零时，又出现了-16000，这是我更不想看到的。externfloatatan2(floaty,floatx);程序格式又搞错了，人家不是atan2(floaty/floatx)，否则很容易出现错误toomanyactualparameters.现在的状态是不能进行全姿态解算，x轴的显示范围是(90°〜270°)，和我想要的范围(-90°〜+90°)正好差了180°，但是减去还不行，减去后串口上只显示一个负号。还好汽车达不到那个角度±90°，哪怕是在汽车测试中，但是飞机能达到啊。所以这个问题最终还是要解决的。我想先把一个角度解算出来，然后去推广。我在主函数里改动pitch=(int)(((atan(ratio)*180)/3.1415926)+180);这一句不行，后来我改动voidlcd_printf(char*s,inttemp_data)函数里面，在第一句我加上了temp_data-=180;然后俯仰角就输出正常了，也不知道为什么。uchar是一个8位无符号数，表示范围0到255,而uint是十六位无符号数，表示范围0到65535.但是要注意的是8位单片机。(摘自网络)现在能解一个俯仰角，下一个是横滚角。我想这两个角的性质应该是比较接近的。但是论文不是这么写的。横滚角也解算出来了，但是航向角好像不能通过加速度计解算。2015-3-13因为航向角解算不出，所以找出MPU9150,希望采用其中的磁强计来解算磁航向角。接下来下载相关datasheet并阅读。现在的解算方法还不涉及迭代，所以现在还没出现那种随着时间的推移，误差累积越来越严重的情况。当前的解算与值与当前的采集数据有关。2015年3月15日为什么不直接搞DSP直接跳过ARM那一关，害怕，害怕就去学。老师可能需要我做一个松耦合组合导航，先让我去研究一下算法。然后再去用硬件实现。该整理资料了，整理完给老师一份，然后再说设计硬件编程的事。等我把航向角结算出来后后立马去研究组合导航算法。方向错了，停止就是前进，否则以后都不能和老师交流了。这是很危险的。我是否应该听老师的，先搞算法，然后再去考虑接下来的实现。网上的GPS模块没见有遵守I2C通信协议的，都是一个收一个发送，这样我还真得考虑数据同步的问题。2015年3月16日我终于知道为什么用示例程序在串口上显示的数据为什么和我想象的那么不一样了，因为你在初始化MPU6050的时候会对陀螺仪和加速度进行一些配置，其中包括一个叫做fullscalerange的配置，就拿陀螺仪来说如果你将这个范围配置在土2000。/，那么这个范围所对应的sensitivityscalefactor就是16.4LSB)(°/s)。感谢唐朔飞老师的《计算机组成原理》，感谢日本AsahiKasei的数据手册，让我在看补数、补码的过程中，让我在看到日本磁强计的测量数据与磁通密度的对比中让我想到了陀螺仪和加速度计也是这样的。谢谢你们。一个好的数据手册就应该让user看明白。怎样才能在陀螺仪的寄存器中的16bit数据中看出那个表示小数点？都不表示小数点，只有通过sensitivityscalefactor之后才会产生小数点，这时的数据才是精确地。好了，这下可以全身心地投入到算法研究中去了，传感器输出的就是，加速度数据和角速度数据，接下来你要做的就是研究一个适合车辆检测的算法了。

MPU6050的程序迁移到9150中没有发现问题，接下来米集磁强计的数据。仅仅修改了取数据的顺序，加入了磁强计各个轴数据寄存器地址，没有使用WIA(DeviceID)。采集成功后开始解算航向角，接下来需要考虑的是磁强计的三轴正方向与GYRO&ACC是不同的，所以在解算时提前要考虑好，GYRO&ACC的俯仰角和横滚角是什么，方向是怎么定义的，然后考虑相对于磁强计这些角度和方向又是如何。定义绕x轴旋转为横滚角，绕y轴为俯仰角，绕z轴为航向角。Mpu9150显示9轴原始数据时出现问题，磁强计的数据不发生改变。我怀疑是磁强计的CNTL寄存器没有配置，导致传感器处于掉电模式。问题依然没有解决。现在的问题是和陀螺仪加速计在一起采集时，数据能采集出来，但是值一直保持不变，单独被采集时压根采集不出来。显示-00001，这个值哪怕不连接传感器都能做到。2015年3月17日磁强计与GYRO&ACC使用的slaveaddress好像是一样的，这样读出的数据虽然不变但是不是-00001了，三轴读出的数据分别是24354，-01549,00336。数据是一直往上传的。难道是现在磁强计现在运行在单次测量模式？我觉得自己得好好处理一下磁强计的数据，因为磁强计的数据拥有四个符号位。H=(Single_ReadI2C(REG_Address))&0x8f;与上0x8f后4位符号位只保留一位，值依然没有改变，处理后的数据为03874,-30221,00336。但是这不能保证都是正数的补码，所以我决定建立一个数组，长度为13.如何舍弃一个二进制数的前三位？不过我又发现多个符号位不影响从二进制转换为十进制。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，配置为单次测量模式。阿莫电子论坛上网友出的招数。open—jDRDYopen—|tst6Slaveaddressopen—jDRDYopen—|tst6CAD1CADOaddressVSSVSS0001100R/WVSSVDD0001101R/WVDDVSS0001110R/WVDDVDD0001111R/WAK897513CADO14Open—TQToAK89755.

6.4.2.SingleMeasurementModeWhensinglemeasurementmode(MODE[3:0]="0001‘‘)isset,sensorismeasured,andaftersensormeasurementandsignalprocessingisfinished,measurementdataisstoredtomeasurementdataregisters(HXLtoHZH),thenAK8975/Ctransitstopower-downmodeautomatically.Ontransitiontopower-downmode,MODE[3:0]turnsto"0000”.Atthesametime,DRDYbitinSTIregisterturnsto"1".Thisiscalled"DataReady".Whenanyofmeasurementdataregister(HXLtoHZH)orST2registerisread,oroperationmodeischangedfrompower・downmodetoothermode,DRDYbitturnstoDRDYpinisinthesamestateasDRDYbit.针对这一句话，每次读完数据我再配置为单次读写模式。intGetData(ucharREG_Address){ucharH,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;//合成数据}现在求解出来的航向角波动很大，非常不稳定，变化范围在-180。〜180。之间。也许航向角的解算与俯仰角和横滚角的取值范围也有关系。二者的取值范围均^-90O~90°O现在航向角还是没有求出来，我想这可能是磁强计的数据有问题2015年3月18日现在的数据还是没有换算成真实数据，而且带小数点的数据通过串口往上传我还没试过。8=tan9=tanW=tanACy,ouTA^XQUT8=tan9=tanW=tanACy,ouTA^XQUTA^ZQUTY,OUT+A2Z,OUT/X,OUT+龙2Z,OUTJX,OUT+A2'Y,OUTI(11)(12)(13)[AN1057]而且4解出来的值也不是我想要的航向角，而且我怀疑上面两个角度也不是我想要的航向角和横滚角。到现在我才明白采样时间间隔内的角增量就是角速度，速度增量就是加速度，当时我竟然还给老师说，用的数据不是加速度和角速度，太愚蠢了，没听老师的话真是太失败了。4.sin巾普4（Ox）？［秦永元惯性导航第二版4

答：(①X)表示①的各分量构造成的叉乘斜对称矩阵，4=闽-丁，r,a=ai+aj+akxyzc一04.sin巾普4（Ox）？［秦永元惯性导航第二版4c一0—aaxcaz0y—aycz」c=z—aLy—raxb=ax(aX)r0bxbyb'z，、0(ax)=az-aLy—az0axay—ax05.汽车测试中存在划桨效应吗？需要划桨效应补偿量AVsculm吗？划桨效应补偿项的xyz—rc=axb(=\ab—abK+(abyzzyzx—rl—rj—rk=aaaxyzbbbxyz优化算法以划桨运动为环境条件，那么在汽车测试中，尤其是做稳态回转时的环境条件又是什么，这里可以分析一下，这里可以具体环境具体分析，这里可以出现创新点，就是将工作条件修改，这里就是研究生工作量的体现之处。6.汽车在做等半径稳态回转时，我觉得俯仰角和横滚角是不变的，只有航向角在发生变化。我觉得汽车在做稳态回转运动中角速度和比力分别是^(t)=jBQcosQtf(t)=kCsinQt因为我觉得在车辆做稳态回转运动时，横滚角存在摆动，z轴上加速度波动较大。但只是感性的猜测，没有理性的数据支持。还有就是我希望自己的程序能够做到自适应，根据测试环境自动切换姿态解算算法。因为不同的环境条件下假设是不一样的，不能一概而论。[QIN2P296]又出了一个恶心的名词直流分量，细节，但就是卡着你让你过不去，想了半天了，搁置一下吧。2015年3月24日10:46将坐标系理解成为刚体，引入四元数描述刚体的转动，并建立四元数q、q、q、q0123与坐标变换矩阵«也就是姿态矩阵*之间的关系。一旦有了姿态矩阵就可以解算三个角度。四元数Q包含了所有的姿态信息，捷联惯导中的姿态更新实质是如何计算四元数Q。我想尝试编写采用定时采样增量法解四元数微分方程解算姿态的程序。我觉得通过编程可以更加深入的了解四元数算法。一直在困惑GPS在姿态解算中到底起什么作用，今天终于释然了，在（QIN2P255）其中，就'的获取按下式进行敬一U（碱+磷）（9.2.50）式中，法是捷联陀螺的输出（对机械转子陀螺必须经过动、静态误差的补偿）；C*由姿态更新的最新值确定；或”和况分别是位置速率和地球自转速率，对于导航坐标系取地理坐标系的情况VEwiecosL+VE%,sinL+B~tanL魅N式中为导航计算所得的最新值。这里虽然没有提到GPS，但是GPS的信息可以从这里引入，引入到位置速率和地球自转速率，但是只能用用纬度信息乙，但是GPS输出的速度信息，而不是东北天方向的各个分，所以VE和VN还是得不到。证据：（1）(2)5,SGPVTG(地面速度信息，TrackMadeGoodandGroundSpeed)$GPVTG语句的基本格式如下：$GPVTG,⑴,T,(2),M,(3),N,⑷K(5)*hh(CR)(LF)以真北为参考基准的地面航向(000〜359度，前面的0也将被传输)以磁北为参考基准的地面航向(000〜359度，前面的0也将被传输)地面速率(000.0〜999.9节，前面的0也将被传输)地面速率(0000.0〜1X51.8公里/小时，前面的0也将被传输)模式指示(A=自主定位，D=差分,E=估算,N=数据无效)举例如下：$GPVrrG„r„M,0.195,N,0.361,K,AA2A(3)，(0网)：GPS信息航向对准曼滤，(0网)：GPS信息航向对准曼滤波(6)图43-1低成本IMU/GPS组合导航系统结构设计降低.其速度和位置误差相关性用一阶马尔可夫过程表示:扉"土风515-—为刖+STX\N(5)低成本SINS/GPS组合导航系统的结构设计如图4.3-1所示令水平对准

便置.s.¥03L我只想说我缺少完整的速度信息（即三个方向），但是低成本的传感器不提供。魅W|鹿慝的iKlfJJLF-T«*iES【忡便置.s.¥03L我只想说我缺少完整的速度信息（即三个方向），但是低成本的传感器不提供。魅W|鹿慝的iKlfJJLF-T«*iES【忡曾、速I*第I补丽M少♦町it毫叠一棒上i*mzu瞬）:褊mQ帏SllKMSlAl^而且如果真要做组合导航就得需要经度、纬度、高度三个信息都需要。还有就是我以前不知道所谓位置解算，结果就是经度、纬度和高度，得到的就是地球上的位置。从上图最后一句话知道，要想解算姿态必须把速度和位置解算出来。也许下图中可能有缺陷，但是他说明姿态速度位置解算是一个整体。在惯性导航和GPS组合导航系统中各子系统的误差源和量测中引入的误差都是随机的，而卡尔曼滤波从概率统计最优的角度估算出系统误差并消除之。当然也可以使用经典的回路反馈法。这句话有力的说明了使用卡尔曼滤波的原因，就是为了消除误差。速度，位置初值想问问老师GPS提供的信息是位置信息（经度、纬度和高度）和速度（东向、北向和天向）吗？因为我处理的信号都是随机信号，没有确定的频谱，所以就不能使用处理确定性信号