基于四元数法的捷联式惯性导航系统的姿态解算_张荣辉
飞控姿态解算理解
姿态解算理解 1、姿态的描述方法 前几天在论坛里偶尔看到一个帖子，帖子的内容是问的为什么不用倾斜角表示姿态，我认为他说的倾斜角是指的斜面与斜面的夹角，或者说是物体与垂线的夹角吧，这种想法可能来源于我们日常生活的思维。 图1立方体 比如有一个立方体，我们放在水平面上的时候它的底面是和水平面平行的，但是当我们把立方体的一个脚垫起一个角度时，这样一来，立方体的一条棱与水平面的垂线就有了一定的夹角了。我们所说倾斜了多少多少度就是指的这个夹角，这是我们直观的反应。我认为这样直观的反应甚至比欧拉角还要来的直观，因为欧拉角是基于旋转的，肯定不会说这个立方体X、Y轴各旋转了多少度（假设Z旋转无效），我们可能也没那个概念，我们直观的反应就是它倾斜了一定度数。 但是我们在姿态解算的时候为啥不用的这种描述方法呢，个人认为是虽然方便我们直观的表达但不适合数学上的计算，还有就是我们仅仅知道这个倾斜角我们怎么施加控制量呢？高中物理学习物体运动的时候我们知道，物体的运动是合运动，我们可以把它的运动矢量正交分解为几个运动的合成（不正交也是可以的，但那不是在自找麻烦吗），同样道理，我们可以把刚体的旋转分解为三个轴上的旋转，这个旋转的角度就是欧拉角，如图2。 图2zxz序规欧拉角 欧拉角 欧拉角的定义不仅仅和旋转角度有关系，还和旋转轴的旋转顺序有关系，任何一种旋转顺序都是合法的。根据定义，欧拉角有12种旋转顺序（维基），一个物体通过任意一个旋转顺序都可以达到同样的姿态，在各个学科里所以为了统一，航空航天领域规定XYZ为欧拉角的旋转顺序。 上面已经说了欧拉角的定义。欧拉角的定义也是很直观而且容易理解的，也利于我们的计算，因为我们用的惯性器件也是按照单个轴向运动来测量的。定义上的欧拉角还和我们所说的Yaw、Pitch、Roll不是一回事。因为定义上的欧拉角就是刚体绕三个轴的旋转角度，
惯性导航技术的工作原理
惯性导航技术的工作原 理 Document number：PBGCG-0857-BTDO-0089-PTT1998
惯性导航系统基本工作原理 惯性导航系统是十分复杂的高精度机电综合系统，只有当科学技术发展到一定高度时工程上才能实现这种系统，但其基本工作原理却以经典的牛顿力学为基础。 设质量m受弹簧的约束，悬挂弹簧的壳体固定在载体上，载体以加速度a 作水平运动，则m处于平衡后，所受到的水平约束力F与a的关系满足牛顿第 二定律： F a m 。测量水平约束力F，求的a，对a积分一次，即得水平速 度，再积分一次即得水平位移。以上所述是简单化了的理性情况。由于运载体不可能只作水平运动，当有姿态变化时，必须测得沿固定坐标系的加速度，所以加速度计必须安装在惯性平台上，平台靠陀螺维持要求的空间角位置，导航计算和对平台的控制由计算机完成。 陀螺仪组件测取沿运载体坐标系3个轴的角速度信号，并被送入导航计算机，经误差补偿计算后进行姿态矩阵计算。加速度计组件测取沿运载体坐标系3个轴的加速度信号，并被送入导航计算机，经误差补偿计算后，进行由运载体坐标系至“平台坐标系”的坐标变换计算。他们沿机体坐标系三轴安装，并且与机体固连，它们所测得的都是机体坐标系下的物理量。 参与控制和测量的陀螺和加速度计称为惯性器件，这是因为陀螺和加速度计都是相对惯性空间测量的，也就是说加速度计输出的是运载体的绝对加速度，陀螺输出的是运载体相对惯性空间的角速度或角增量。而加速度和角速度或角增量包含了运载体全部的信息，所以惯导系统仅靠系统本身的惯性器件就能获得导航用的全部信息，它既不向外辐射任何信息，也不需要任何其他系统
四元数转欧拉角代码解析
四元数转欧拉角代码解析 本文的内容就是解析正点原子MPU6050的mpu_dmp_get_data()函数中，三个欧拉角的由来，即如何将MPU6050输出的四元数转化为姿态解算所需要的欧拉角。 *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll *yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw 其实上述三个公式的核心就是将一次的姿态变换分别用四元数矩阵和欧拉角矩阵表示出来，由于这两个矩阵是等价的即对应元素都相等，通过简单的对比运算就可以得到上述的三个公式。 因此，我将从1.四元数矩阵的得到；2.欧拉角矩阵的得到；3.两个矩阵的等价运算三个部分进行说明。 1.四元数矩阵的得到 三重矢量计算公式： AX(BXC)=B(A·C)-C(A·B) 这个公式很好记，右边部分就是BACK-CAB(后面的出租车)
2.欧拉角矩阵的得到 q02+q12+q22+q32=1 从9.2.33到9.2.34的化简，其实就是利用 进行化简，把1去掉即可。 将右侧的矩阵乘开，可得到一个3x1矩阵， 与左边3x1矩阵对应元素相等，这个相等的 关系，就是上个框框中求出的三个等式。 各轴上的单位1，就是图1.2.2矩阵任意 行与列各个元素的平方和为1。
到这里，用欧拉角表示描述一次旋转变换已经结束了。然而，上述的姿态矩阵C n b仅仅是《惯性导航》这本书先Z,再X,最后Y旋转变换而形成的姿态矩阵，这样的旋转顺序其实是和很多大家实际使用的飞控代码不一样的（同样的，关于θφγ的实际意义其实也没有明确的规定）。此文目的就是解析“正点原子”飞控代码中四元数转欧拉角部分，因此，接下来，
四元数姿态的梯度下降法推导和解读
四元数姿态的梯度下降法推导和解读 笔者前面几篇文章讨论的是基于四元数的互补滤波算法，并单独对地磁计融合部分做了详细的讨论和解释。而本文讨论的姿态融合算法叫做梯度下降法，这部分代码可以参见Sebastian O.H. Madgwick在2010年4月发表的一篇论文（An efficient orientation filter for inertial andinertial/magneticsensor arrays），这篇论文利用四元数微分方程求解当前姿态，然后分别利用加速度计和地磁计进行补偿，推导出两种姿态融合算法。两种算法均为梯度下降法，而其中地磁计的处理方式笔者已经在《四元数姿态解算中的地磁计融合解读》一文中详细讨论了，这里笔者将对Madgwick对于加速度计和地磁计的梯度下降法做出详细的解释，期间一定有个人不足的地方，仅供参考，希望和各位网友一起学习！ 首先来谈谈什么是梯度。维基百科中解释的是“标量场中某一点上的梯度指向标量场增长最快的方向，梯度的长度是这个最大的变化率。”很显然，梯度和变化率有关。现在我们引入标量函数f(x)，对标量函数f(x)求导，不难得到f’(x)就是梯度，就是曲线在某一点的斜率。梯度下降法就是我们顺着这个在某一点下降速度最快的反方向一直走，走到一个极值点，这个点就是最优解（稳定解）。 那么这个梯度的概念和我们的姿态解算有什么关系？ 我在前面的文章中已经说明：我们求解姿态就是求解的转换矩阵（矩阵元素就是四元数）。这个转换矩阵是有误差的，我们所要做的工作就是采用某种算法，消除误差，最后得到的解就是我们的近似精确解，也就是姿态四元数了。消除误差四个字，在实际的实现过程中，是通过误差函数来实现的。定义误差函数ef(x)，那么我们的工作就是令ef(x)=0，求解上述方程得到x的值。我们在求解高阶方程的时候，一般的方法就是求导，求极值点，根据这些值来判断精确值个数和位置。这是我们高中所学习到的知识，在这里是一样的。只不过，这里的误差函数ef(x) 不再是之前讨论的简单的标量函数了，他的自变量x变成了向量[q0 q1 q2 q3]。这也就是说，原先的标量函数ef(q) 变成了如今的标量函数ef([q0 q1 q2 q3]) ，他仍然是标量函数，但是自变量是向量[q0 q1 q2 q3]。 对上述自变量是向量的标量函数，我们要用梯度法求解，就必须求导。标量函数对向量求导很简单，只需要分别对向量中的各个变量求偏导即可： 但是，我们的姿态解算是三维姿态，不是一维姿态，所以，这里的ef(q)并不是一个标量函数，实质上是一个向量函数ef ( q )，这个向量函数里面有三个元素，分别对应xyz轴的三个分量，每个分量又由一个四元数向量q构成。那么现在就引入了一个较为复杂的误差函数ef ( q )，该误差函数不光自变量是一个向量，并且因变量也是一个向量，这种函数叫做多元向量函数。那么我们现在的问题就转化为求多元向量函数的极值问题。 针对上述极值问题，在计算机中，多采用数值解法，如最速下降法、牛顿法、共轭梯度法。我们这里讨论就是第一种算法，又叫做梯度下降法。PS：梯
捷联式姿态解算过程
（1）加速度记测得载体相对惯性空间比力b ib a ，经过误差补偿之后的b ib a 经过姿态矩阵n b C 的变换得到n ib a ：n n b ib b ib =C a a 将n ib a 进行误差补偿之后通过积分运算得到速度分量n en V 。n en V 一方面作 为系统的输出，一方面作为输入用来求解位置角速率n en W 。 （2）陀螺仪测得载体相对惯性空间角速率b i b W ， 首先通过速度分量n en V 求得位移角速率n en W ，因为n eny W =0， n enx n eny W W ????????=22yt xt 22y yt xt yt t x x t t sin cos ()R R sin cos R 11sin cos R R 11sin cos R R R αααααααα?? -???????? -???-++? -（）（）n x n y V V ?? ?????? 这样就根据陀螺仪测得的角速率b i b W ，上面所求得的位移角速率n en W ， 加上已知的地球角速率e i e W 来求的姿态角速率b n b W 。 b b n b n e i e n i e n e i e W =C W =C C W 且 b b n e n n e n W =C W b b b b n b i b i e e n W =W W W --=b i b W b n e n n e i e e n C C W +W -（） 。 然后根据姿态角速率利用四元数微分方程求出四元数中的元素a,b,c,d. 四元数描述了一个坐标系或一个矢量相对某一个坐标系的旋转。a 是标量部分表示了转角的一半余弦值，b ，c ，d 是矢量部分表示瞬时转轴的方向，瞬时转动轴与参考坐标系轴间方向的余弦值。 表达式A=a+bi+cj+dk 。
辅助惯性导航系统的方法和算法发展
2008年8月 第36卷第4期 现代防御技术 M ODERN DEFENCE TECHNOLOGY Aug.2008 Vo.l36No.4 导航、制导与控制 辅助惯性导航系统的方法和算法发展* 武虎子,南英,付莹珍 (南昌航空大学航空与机械工程学院,江西南昌330063) 摘要:综述了辅助惯导的一些主要算法和方法,主要有:重力辅助的匹配方法、基于衰减记忆的匹配算法、基于贝叶斯算法、基于神经网络算法、基于迭代最近点算法、无线电高度与数字地图辅助方法、粒子滤波算法、声呐技术辅助方法、概率数据关联算法、成像激光雷达辅助方法。分别对各类辅助算法和方法的基本原理、主要优缺点进行了简要介绍,展望了辅助算法和方法的发展趋势。 关键词:惯性导航系统;辅助算法;辅助方法;发展趋势 中图分类号:V448122+4;U66611文献标识码:A文章编号:10092086X(2008)20420062206 The Developm en t of A i ded A l gor ithm and M ethods i n Iner ti a l N avi ga ti on Syste m WU H u2z,i NAN Y i n g,F U Y ing2z hen (Nanchang Un i versity of Aeronautics,School of Aero nauti c and M echanical Engi neeri ng,Ji angxi Nanchang330063,Ch i na) A bstra ct:So me main a l g orithms and methods i n a i d ed2inertial navi g ati o n are summ ar iz ed.They can be c lassified as f ollo ws:gravity a i d ed matchingm ethod,match i n g algorithm based on FadingMe mory,a l2 gorithm based on Bayes Rule,a l g orit h m based on A rtificial Neura lN et w ork,algorith m based on iterative closest poin,t a i d ed method of w ire less he i g ht and d i g italmap,partic le filter algorithm,aided m et h od of sonar technology,probab ilistic data association filter algorith m,a i d ed method of i m agi n g laser radar.The main pri n ciple and ma i n advantages and disadvan tages of a ll k i n ds of a l g orit h ms and methods are i n tro2 duced si m p l y and separately.The develop men t trend of the m is prospected. K ey words:i n ertial navi g ati o n syste m(I N S);a i d ed a l g orithm;a i d ed m et h ods;deve lopment trend 0引言 随着导航技术的逐渐成熟,飞行器对自主导航精度的要求也越来越高,因而辅助惯性导航方法与算法也快速兴起。所谓辅助惯性导航系统(i n erti a l navi g ation syste m,I N S)的方法与算法,就是一种能提高惯导导航精度的方式和途径(如导航精度参数CEP,S EP,R,R MS等达到规定的范围内)。采用这些方法与算法可以重调和校正单一的惯导系统(如位置和方位的重新调整、陀螺漂移的校正)。 在过去的几十年里,辅助惯性导航技术已经有了很大的发展。其辅助算法都可以通过建立数学模 *收稿日期:2007-12-01;修回日期:2008-02-12 作者简介:武虎子(1981-),男,陕西富平人。硕士生,研究方向为飞行控制与导航。 通信地址:330063南昌市丰和南大道696号南昌航空大学航空与机械工程学院
四轴姿态解算
又花了将近一个星期，终于把姿态解算的框架完成了。仅仅是把陀螺仪、加速度计、罗盘融合在一起，得出旋转姿态，没有对加速度积分，没有用到气压计，几乎没有滤波。算是阶段性的工作吧，把框架设计得合理一点，以后添加/修改就很简单了。 从传感器的读取，到四元数的学习，到空间旋转的处理方法，循序渐进，逐个击破。主要参考了以下资料（按阅读的时间循序）： 《计算机图形学几何工具算法详解》（四元数转矩阵的公式是错的！） 《交互式计算机图形学——基于OpenGL的自顶向下方法》 维基百科——四元数 框框的日记——四元数 青衫湮痕——四元数 Heath's 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”
基于四元数方法的姿态解算
基于四元数方法的姿态解算方法分析 摘要:载体的姿态解算算法是实现捷联式惯性导航系统精确导航的核心技术之一。分析了欧拉法、方向余弦法、四元数法求解姿态矩阵的优缺点，采用四元数法与方向余弦法两种解算方法分别计算载体姿态，两种方法的计算结果之差与理论真值比较以得到解算的相对误差，从而验证了四元数法的正确性和有效性。最后，指出提高采样频率和采用高阶计算算法能进一步减小姿态解算误差。数字化仿真与转台试验结果表明，本文提出的载体姿态解算法具有良好的实时性。 1引言 捷联惯导是一种自主式的导航方法。该方法将陀螺仪和加速度计直接安装在载体上，省掉机电式导航平台，利用计算机软件建立一个“数学平台”来代替机电平台实体[1]。由于其结构简单且抗干扰能力强，目前已成为航空航天、航海、机器人、智能交通等领域的研究热点之一。 姿态解算是捷联式惯性导航系统的关键技术，通过姿态矩阵可以得到载体的姿态和导航参数计算需要的数据，是捷联式惯导算法中的重要工作。载体的姿态和航向体现了载体坐标系与导航坐标系之间的方位关系，确定两个坐标系之间的方位关系需要借助矩阵法和力学中的刚体定点运动的位移定理。通过矩阵法推导方向余弦表，而刚体定点运动的位移定理表明，定点运动刚体的任何有限位移都可以绕过定点的某一轴经过一次转动来实现。目前描述动坐标相对参考坐标系方位关系的方法有多种，可简单地将其分为3类，即三参数法、四参数法和九参数法「1-2]。三参数法也叫欧拉角法，四参数法通常指四元数法，九参数法称作方向余弦法。欧拉角法由于不能用于全姿态飞行运载体上而难以广泛用于工程实践，且实时计算困难。方向余弦法避免了欧拉法的“奇点”现象，但方程的计算量大，工作效率低。随着飞行运载体导航控制系统的迅速发展和数字计算机在运动控制中的应用，控制系统要求导航计算环节能更加合理地描述载体的刚体空间运动，四元数法的研究得到了广泛重视。本文全面分析了3种解算方法的特点，通过对比四参法与九参法的计算结果以验证四元数法的正确性和有效性，基于数值仿真和转台实验相结合的分析方法得到进一步减少姿态解算误差的有效途径，为捷联式惯性导航技术的工程实践提供参考。（就是这部分内容需要程序解算，不会搞） 2姿态矩阵的计算方法 由于载体的姿态方位角速率较大，所以针对姿态矩阵的实时计算提出了更高的要求。通常假定捷联系统“数学平台”模拟地理坐标系，即导航坐标系;而确定载体的姿态矩阵即为研究载体坐标系(6)和导航坐标系(E)的空间转动关系，一般用载体坐标系相对导航坐标系的三次转动角确定，习惯上俯仰角和偏航角用B和必表示，滚转角用Y表示。目前主要的研究方法为:欧拉法、方向余弦法与四元数法。图1为捷联式惯性导航原理图。
惯性导航系统
惯性导航系统 以下是为大家整理的惯性导航系统的相关范文，本文关键词为惯性,导航,系统,，您可以从右上方搜索框检索更多相关文章，如果您觉得有用，请继续关注我们并推荐给您的好友，您可以在教育文库中查看更多范文。 目录 1.惯性导航系统的概念.........................22.惯导系统的发展历史及发展趋势 (3)
惯性导航系统的发展.......................3我国的惯性导航系统.......................5捷联惯导系统现状及发展趋势...............63.惯性导航系统的组成........................104、惯性导航系统的工作原理....................145、惯性导航系统的功能.......................186、惯性导航系统的服务模式与应用模式..........207、惯性导航系统当前的应用情况................218、惯性导航系统的特点 (23) 系统的主要优点......................23系统的主要缺点.....................249、惯性导航系统给我们的启示. (24) 1 惯性导航系统 一、惯性导航系统的概念 什么是惯性导航或惯性制导呢?惯性导航系统（Ins）是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。在给定的运动初始条件（初始地理坐标和初始速度）下，利用惯性敏感元件测量飞机相对惯性空间的线运动和角运动参数，用计算机推算出飞机的速度、位置和姿态等参数，从而引导飞机航行。 推算的方法是在运载体上安装加速度计，经过计算(一次积分和二次积分)，从而求得运动轨道(载体的运动速度和距离)，进而进行导航。在运载体上安装加速度计，用它来敏感、测量运载体运动的加速
四元数解算姿态完全解析及资料汇总
四元数完全解析及资料汇总 本文原帖出自匿名四轴论坛，附件里的资源请到匿名论坛下载： 感谢匿名的开源分享，感谢群友的热心帮助。 说什么四元数完全解析其实都是前辈们的解析，小弟真心是一个搬砖的，搬得不好希望大神们给以批评和指正，在此谢过了。因为本人是小菜鸟一枚，对，最菜的那种菜鸟······所以对四元数求解姿态角这么一个在大神眼里简单的算法，小弟我还是费了很大劲才稍微理解了那么一点点，小弟搬砖整理时也是基于小弟的理解和智商的，有些太基础，有些可能错了，大牛们发现了再骂过我后希望能够给与指正哈。 好，废话到此为止，开始说主体。四元数和姿态角怎么说呢先得给和我一样的小菜鸟们理一理思路，小鸟我在此画了一个“思维导图”（我承认我画的丑），四元数解算姿态首先分为两部分理解：第一部分先理解什么是四元数，四元数与姿态角间的关系；第二部分要理解怎么由惯性单元测出的加速度和角速度求出四元数，再由四元数求出欧拉角。 图1 渣渣思维导图 在讲解什么是四元数时，小弟的思维是顺着说的，先由四元数的定义说起，说到四元数与姿态角间的关系。但在讲解姿态解算时，小弟的思维是逆向的，就是反推回来的，从欧拉角一步步反推回到惯性器件的测量数据，这样逆向说是因为便于理解，因为实际在工程应用时和理论推导有很大差别。
实际应用时正确的求解顺序应该为图1中序号顺序，即1->2->3->……. 但在笔者讲解姿态求解时思路是如图2的。 ` 图2 逆向讲解思路 大家在看四元数时最好结合着代码一块看，小弟看的是匿名四轴的代码，感觉写的非常好也非常清晰，粘出来大家一块观摩。红色部分是核心代码，总共分为八个步骤，和图1中的八个步骤是一一对应的。讲解介绍时也是和代码对比起来讲解的。代码可以去匿名官网上下载，都是开源的，不是小弟的，所以小弟不方便加在附件中。 好的，下面搬砖开始！。。。。。。。。嘿咻嘿咻！！！！ · 一．
《惯性导航简介》
惯性导航简介 ——《导航概论》课程论文 专业：测绘工程A组姓名：师嘉奇学号：2015301610091 一.摘要与关键字 1.本文摘要：本文主要对导航工程的基本内涵，方法与原理进行简单介绍，主要介绍有关惯性导航的相关内容，并且根据在本学期《导航概论》这门课程上所学习的内容谈一谈自己对导航应用的设想以及对本课程教学的建议。 2.关键字：惯性导航，定位技术，应用与服务，发展与前景 二.导航工程基本内涵 导航定位的历史与人类自身发展的历史一样久远。人类的导航定位活动源自于其生活和生产的需要。陆地上的导航定位最早发生在人类祖先外出寻找食物或狩猎的过程中，那时，他们通常在沿途设置一些特殊的“标记”来解决回家迷路的问题。随着探索遥远地域的愿望与行动的出现，他们则通过观察和利用自然地标（如山峰、河流、树木、岩石等)以及自然天体（恒星)来解决导航定位问题这也使得他们能够翻越高山、跨越河流。谈到导航，很多人会认为这是一个在近现代才提出的词汇，但是，导航的历史已经非常久远了。从古代黄帝作战使用的指南车，到战国时期的司南，从近代航海使用的指南针，再到当今社会人手一部的智能手机，导航已经有了很悠久的历史。那么，导航工程的基本内涵到底是什么呢？
首先，我们可以通过两个英文的句子来大概了解一下到底什么是导航“when am I？”和“How and when to get there?”，这两个问题问的是我现在在哪？我要怎么到那里去？它们也指出了导航的内涵，那就是在哪，怎样去，多久到达。因此，通过科学的定义，将航行载体从起始点引导到目的地的过程称为导航，导航系统给出的基本参数是载体在空间的即时位置、速度和姿态、航向等，导航参数的确定由导航仪或导航系统完成。通过这种技术引导载体方向的过程即为导航。导航是解决人，事件，目标相互位置动态关系随时间变化的科学，技术，工程问题。 在室外或者自然环境中的导航，按照载体运动的范围，可分为海陆空天(海洋、陆地、空中、空间)导航四类；按照所采用的技术，常用的导航方法有，天文导航、惯性导航、陆基无线电导航、卫星导航、特征匹配辅助导航（如地形匹配、地磁匹配、重力匹配)等，以及上述导航方法之间的不同组合（组合导航）。室内定位导航作为当今导航技术发展的个重要分支，它借鉴室外导航的相关技术，同时结合现代通信技术、网络技术传感器技术以及计算机技术的最新发展，已经成为一个重要的研究热点并在人们日常工作和生活中逐步得到应用。室内导航与自然环境中的导航既有联系又有其自身的特点，其主要差异是来自于应用环境及所采用的技术方法不同。 导航系统有两种工作状态：指示状态和自动导航状态。如导航设备提供的导航信息仅供驾驶员操纵和引导载体用，则导航系统工作为指示状态，在指示状态下，导航系统不直接对载体进行控制，如果导
四元数姿态解析x
这个程序得到了四元数，怎么进一步计算姿态YAW，ROLL，PITCH？ //---------------------------------------------------------------------------------------------------- // Header files #include "AHRS.h" #include
惯性导航系统
惯性导航系统 惯性导航系统（INS，以下简称惯导）是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下。惯导的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。 惯性导航系统（英语：INS ）惯性导航系统是以陀螺和加速度计为敏感器件的导航参数解算系统，该系统根据陀螺的输出建立导航坐标系，根据加速度计输出解算出运载体在导航坐标系中的速度和位置。 惯性导航系统（INS，Inertial Navigation System）也称作惯性参考系统，是一种不依赖于外部信息、也不向外部辐射能量（如无线电导航那样）的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下。惯性导航的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。 惯性导航系统属于推算导航方式，即从一已知点的位置根据连续测得的运动体航向角和速度推算出其下一点的位置，因而可连续测出运动体的当前位置。惯性导航系统中的陀螺仪用来形成一个导航坐标系，使加速度计的测量轴稳定在该坐标系中，并给出航向和姿态角；加速度计用来测量运动体的加速度，经过对时间的一次积分得到速度，速度再经过对时间的一次积分即可得到距离。 惯性导航系统有如下优点：1、由于它是不依赖于任何外部信息，也不向外部辐射能量的自主式系统，故隐蔽性好，也不受外界电磁干扰的影响；2、可全天候、全时间地工作于空中、地球表面乃至水下；3、能提供位置、速度、航向和姿态角数据，所产生的导航信息连续性好而且噪声低；4、数据更新率高、短期精度和稳定性好。 其缺点是：1、由于导航信息经过积分而产生，定位误差随时间而增大，长期精度差；2、每次使用之前需要较长的初始对准时间；3、设备的价格较昂贵；4、不能给出时间信息。[1]但惯导有固定的漂移率，这样会造成物体运动的误差，因此射程远的武器通常会采用指令、GPS等对惯导进行定时修正，以获取持续准确的位置参数。惯导系统目前已经发展出挠性惯导、光纤惯导、激光惯导、微固态惯性仪表等多种方式。陀螺仪由传统的绕线陀螺发展到静电陀螺、激光陀螺、光纤陀螺、微机械陀螺等。激光陀螺测量动态范围宽，线性度好，性能稳定，具有良好的温度稳定性和重复性，在高精度的应用领域中一直占据着主导位置。由于科技进步，成本较低的光纤陀螺（FOG）和微机械陀螺（MEMS）精度越来越高，是未来陀螺技术发展的方向。 分类捷联式惯性导航系统 解析式惯性导航系统 半解析式惯性导航系 编辑本段应用惯性导航系统用于各种运动机具中，包括飞机、潜[2]艇、航天飞机等运输工具及导弹，然而成本及复杂性限制了其可以应用的场合。 惯性系统最先应用于火箭制导，美国火箭先驱罗伯特.戈达尔（ROBERT GODDARD ）试验了早期的陀螺系统。二战期间经德国人冯布劳恩改进应后，应用于V-2火箭制导。战后美国麻省理工学院等研究机构及人员对惯性制导进行深入研究，从而发展成应用飞机、火箭、航天飞机、潜艇的现代惯性导航系统。 编辑本段惯性技术的重要性惯性技术是对载体进行导航的关键技术之一，惯性技术是利用惯性原理或其它有关原理，自主测量和控制运载体运动过程的技术，它是惯性导航、惯性制导、惯性测量和惯性敏感器技术的总称。现代惯性技术在各国政府雄厚资金的支持下，
捷联系统的四元数法姿态算法
捷联系统的四元数法姿态算法 算法输入：物体的初始姿态，3轴陀螺仪不同时刻的Yaw，Pitch，Roll的角速度；算法输出：物体的当前姿态。 具体算法： 1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为A 2. 读取3轴陀螺仪当前时刻的Yaw,Pitch,Roll角速度，乘以上次计算以来的间隔时间，得到上一时刻以来(Yaw,Pitch,Roll)的变化量，命名为欧拉角b 3. b是Tait-Bryan angle定义的欧拉角，将其转为四元数B 4. A=A×B，做四元数乘法，即可得到当前姿态对应的新的四元数A 5.重复2～4部，即可连续更新姿态 6.将四元数A重新转换为Tait-Bryan angle形式的欧拉角a，就可以以直观的形式查看当前姿态 核心算法1，欧拉角转四元数 void Quaternion::FromEulerAngle(const EulerAngle &ea) { float fCosHRoll = cos(ea.fRoll * .5f); float fSinHRoll = sin(ea.fRoll * .5f); float fCosHPitch = cos(ea.fPitch * .5f); float fSinHPitch = sin(ea.fPitch * .5f); float fCosHYaw = cos(ea.fYaw * .5f); float fSinHYaw = sin(ea.fYaw * .5f); w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw; x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw; y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw; z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw; } 核心算法2，四元数转欧拉角 EulerAngle Quaternion::ToEulerAngle() const { EulerAngle ea; ea.fRoll = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x)); ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f)); ea.fYaw = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y)); return ea; }
第四章四元数在3D物体姿态变化中的应用_2
第四章 四元数在3D 物体姿态变化中的应用 4.1引言 对于三维仿真系统中鱼雷运动的可视化研究，一般的仿真系统采用了按照鱼雷的六自由度参数来直接驱动模型的视景仿真。为了保证计算速度，往往需要在计算运动方程时选取较大的计算步长，这样有时会无法实现仿真运动的光滑和连续，甚至出现鱼雷运动跳跃的现象，如果选取较小的计算步长，那么计算速度的降低同样会影响到鱼雷的可视化效果。 目前，国内的仿真系统里用来解决此问题的专用软件模块十分少见。本课题着重研究了四元数在三维仿真系统中的应用，包括利用四元数对物体进行自由旋转和利用四元数来解决鱼雷运动可视化中的问题，本文利用四元数对模型姿态角变化进行插值，很好地解决了上述的鱼雷运动可视化仿真中存在的矛盾。 4.2四元数简介 四元数是1843年由英国数学家William Rowan Hamilton 发明的数学概念。它的出现很好地把二维空间中的复数扩展到了三维空间。由于四元数的乘法不满足交换律，对它的研究较实数和复数都要困难，以至在它出现后一个多世纪都得不到很好的应用。直到1985年Ken Shoemake [20]将它引入计算机图形学之后，四元数在计算机动画和三维图形绘制方面才得到实际的应用。随着计算机技术和图形学的发展，四元数所表现出来的优势也日渐得到人们的重视。 4.2.1四元数记法 一个四元数包含了四个分量，一个标量分量和一个3D 向量分量。也可以说是一个实部，三个虚部，表示如式4.1所示： i = + +j +k Q w b c d (4.1) 式4.1中w , b , c , d 为实数，i , j , k 为虚数，满足i 2= j 2 = k 2 =－1。四元数也可以表示为：[w ,v ] 或 [w , (x ,y ,z )]，其中w 为标量，v ＝(x ,y ,z )为矢量。 4.2.2四元数的运算 四元数的加减法和一般复数的加减法相同，也满足交换律和结合律。但是四元数的乘法满足结合律但不满足交换律，这是与实数和复数最显著的不同[5]。 四元数加法： 121212[,]Q Q w w v v +=++ (4.2) 四元数点乘：1212121212 Q Q w w x x y y z z ?=+++ (4.3) 四元数的共轭：*[,(,,)]Q w x y z =??? (4.4) 四元数的模：Q (4.5)