当前位置：搜档网 › 捷联系统四元数姿态解算的精细积分法

捷联系统四元数姿态解算的精细积分法

飞控姿态解算理解

姿态解算理解 1、姿态的描述方法前几天在论坛里偶尔看到一个帖子，帖子的内容是问的为什么不用倾斜角表示姿态，我认为他说的倾斜角是指的斜面与斜面的夹角，或者说是物体与垂线的夹角吧，这种想法可能来源于我们日常生活的思维。图1立方体比如有一个立方体，我们放在水平面上的时候它的底面是和水平面平行的，但是当我们把立方体的一个脚垫起一个角度时，这样一来，立方体的一条棱与水平面的垂线就有了一定的夹角了。我们所说倾斜了多少多少度就是指的这个夹角，这是我们直观的反应。我认为这样直观的反应甚至比欧拉角还要来的直观，因为欧拉角是基于旋转的，肯定不会说这个立方体X、Y轴各旋转了多少度（假设Z旋转无效），我们可能也没那个概念，我们直观的反应就是它倾斜了一定度数。但是我们在姿态解算的时候为啥不用的这种描述方法呢，个人认为是虽然方便我们直观的表达但不适合数学上的计算，还有就是我们仅仅知道这个倾斜角我们怎么施加控制量呢？高中物理学习物体运动的时候我们知道，物体的运动是合运动，我们可以把它的运动矢量正交分解为几个运动的合成（不正交也是可以的，但那不是在自找麻烦吗），同样道理，我们可以把刚体的旋转分解为三个轴上的旋转，这个旋转的角度就是欧拉角，如图2。图2zxz序规欧拉角欧拉角欧拉角的定义不仅仅和旋转角度有关系，还和旋转轴的旋转顺序有关系，任何一种旋转顺序都是合法的。根据定义，欧拉角有12种旋转顺序（维基），一个物体通过任意一个旋转顺序都可以达到同样的姿态，在各个学科里所以为了统一，航空航天领域规定XYZ为欧拉角的旋转顺序。上面已经说了欧拉角的定义。欧拉角的定义也是很直观而且容易理解的，也利于我们的计算，因为我们用的惯性器件也是按照单个轴向运动来测量的。定义上的欧拉角还和我们所说的Yaw、Pitch、Roll不是一回事。因为定义上的欧拉角就是刚体绕三个轴的旋转角度，

四元数转欧拉角代码解析

四元数转欧拉角代码解析本文的内容就是解析正点原子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 。

四轴姿态解算

又花了将近一个星期，终于把姿态解算的框架完成了。仅仅是把陀螺仪、加速度计、罗盘融合在一起，得出旋转姿态，没有对加速度积分，没有用到气压计，几乎没有滤波。算是阶段性的工作吧，把框架设计得合理一点，以后添加/修改就很简单了。从传感器的读取，到四元数的学习，到空间旋转的处理方法，循序渐进，逐个击破。主要参考了以下资料（按阅读的时间循序）：《计算机图形学几何工具算法详解》（四元数转矩阵的公式是错的！）《交互式计算机图形学——基于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 渣渣思维导图在讲解什么是四元数时，小弟的思维是顺着说的，先由四元数的定义说起，说到四元数与姿态角间的关系。但在讲解姿态解算时，小弟的思维是逆向的，就是反推回来的，从欧拉角一步步反推回到惯性器件的测量数据，这样逆向说是因为便于理解，因为实际在工程应用时和理论推导有很大差别。

实际应用时正确的求解顺序应该为图1中序号顺序，即1->2->3->……. 但在笔者讲解姿态求解时思路是如图2的。 ` 图2 逆向讲解思路大家在看四元数时最好结合着代码一块看，小弟看的是匿名四轴的代码，感觉写的非常好也非常清晰，粘出来大家一块观摩。红色部分是核心代码，总共分为八个步骤，和图1中的八个步骤是一一对应的。讲解介绍时也是和代码对比起来讲解的。代码可以去匿名官网上下载，都是开源的，不是小弟的，所以小弟不方便加在附件中。好的，下面搬砖开始！。。。。。。。。嘿咻嘿咻！！！！ · 一．

四元数姿态解析x

这个程序得到了四元数，怎么进一步计算姿态YAW，ROLL，PITCH？ //---------------------------------------------------------------------------------------------------- // Header files #include "AHRS.h" #include //---------------------------------------------------------------------------------------------------- // Definitions #define Kp 2.0f // 比例增益支配收敛率accelerometer/magnetometer #define Ki 0.005f // 积分增益执政速率陀螺仪的衔接gyroscopeases #define halfT 0.5f // 采样周期的一半 //--------------------------------------------------------------------------------------------------- // Variable definitions float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向 float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差 //============================================================================= ======================= // Function //============================================================================= ======================= void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float norm; float hx, hy, hz, bx, bz; float vx, vy, vz, wx, wy, wz; float ex, ey, ez; // 辅助变量，以减少重复操作数 float q0q0 = q0*q0; float q0q1 = q0*q1; float q0q2 = q0*q2; float q0q3 = q0*q3; float q1q1 = q1*q1; float q1q2 = q1*q2;

捷联系统的四元数法姿态算法

捷联系统的四元数法姿态算法算法输入：物体的初始姿态，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)