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

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

算法输入：物体的初始姿态，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;

}

核心算法3，四元数乘法

Quaternion Quaternion::Multiply(const Quaternion &b)

{

Quaternion c;

c.w=w*b.w -x*b.x -y*b.y -z*b.z;

c.x=w*b.x +x*b.w +y*b.z -z*b.y;

c.y=w*b.y -x*b.z +y*b.w +z*b.x;

c.z=w*b.z +x*b.y -y*b.x +z*b.w;

c.Normalize();

return c;

}

次要的规范化算法：

void Quaternion::Normalize(){

float s=getS();

w/=s;

x/=s;

y/=s;

z/=s;

}

float Quaternion::getS(){

return sqrt(w*w+x*x+y*y+z*z);

}

我的loop函数，算法的集成部分：

Quaternion nowQ;

void loop() {

int intx, inty,intz;

float pitch,roll,yaw;

gyro.ReadGyroOutCalibrated_Radian(&pitch, &roll, &yaw); EulerAngle dt;

dt.fRoll=roll;

dt.fPitch=pitch;

dt.fYaw=-yaw;

Quaternion dQ;

dQ.FromEulerAngle(dt);

nowQ=nowQ.Multiply(dQ);

count++;

if (count>1000){

EulerAngle nowG=nowQ.ToEulerAngle();

Serial.print(nowG.fRoll/3.1415926535*180,11);//横滚

Serial.print(",");

Serial.print(nowG.fPitch/3.1415926535*180,11);//俯仰

Serial.print(",");

Serial.print(nowG.fYaw/3.1415926535*180,11);//偏航

Serial.print(",");

Serial.print(nowQ.getS(),11);//偏航

Serial.println();

count=0;

}

}

四元数与欧拉角之间的转换

在3D图形学中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。本文主要归纳了两种表达方式的转换，计算公式采用3D笛卡尔坐标系：

图1 3D Cartesian coordinate System (from wikipedia)

定义分别为绕Z轴、Y轴、X轴的旋转角度，如果用Tait-Bryan angle 表示，分别为Yaw、Pitch、Roll。

图2 Tait-Bryan angles (from wikipedia)

一、四元数的定义

通过旋转轴和绕该轴旋转的角度可以构造一个四元数：

其中是绕旋转轴旋转的角度，为旋转轴在x,y,z方向的分量（由此确定了旋转轴）。

二、欧拉角到四元数的转换

三、四元数到欧拉角的转换

arctan和arcsin的结果是，这并不能覆盖所有朝向(对于角

的取值范围已经满足)，因此需要用atan2来代替arctan。

四、在其他坐标系下使用

在其他坐标系下，需根据坐标轴的定义，调整一下以上公式。如在Direct3D中，笛卡尔坐标系的X轴变为Z轴，Y轴变为X轴，Z轴变为Y轴（无需考虑方向）。

旋转的矩阵与四元数的转换算法

绕指定方向的轴线的旋转 旋转的矩阵与四元数的转换算法 一、基本知识 四元数 q=w+xi+yj+zk, q=w+v, q ?1=|q|2q ?. 其中q ?= w ?xi ?yj ?zk 表示q 的共轭。 vv' =v×v' ?v?v' …… ① qq'= (ww'?v?v') + (wv' +vw' +v×v'), …… ② v ?v' = ?(vv' +v'v) /2 , …… ③ v×v' = (vv' ?v'v) /2 . …… ④ v 2=?|v|2. 内积v ?v =|v|2. 二、旋转表示为四元数 绕指定方向n 的旋转公式为 x'=(x ?n)n(1?cosθ ) + cosθ x +sinθ n×x …… ⑤ 其中的向量运算转化为四元数运算得到 x'= ?(x n+nx)n (1?cosθ)/2 +cosθx +sinθ (n x?x n)/2, 由于|n|=1, 故n ?1=?n. 因此有 x'= (1+cosθ)x/2 +sinθ (n x?x n )/2 ?nxn (1?cosθ)/2 . …… ⑥ x 的平行、垂直分量记为x p =(x?n)n , x v = x?x p , 则?nxn= x p ? x v , 与x 关于n 轴对称。 (nx?xn)/2= n×x, 是x v 旋转+90°的结果。 若 n ⊥x, x?n =0,即xn=?nx, 此时nxn=x, 故 x' =(cosθ+n sinθ)x . q=r(cosθ+nsinθ)称为四元数的三角式，其中n 为单位向量，它的几何意义是，qx 表示对向量x 绕方向n 旋转θ角，然后长度扩大为r 倍。任何四元数都有三角式q=w+v= |q|(w/|q|+n|v|/|q|)=r(cosθ+nsinθ). φ(x)=qxq ?1称为四元数域上的合同变换。若q=a+bn, |n|=1, 则 |q|2 =a 2+b 2 , |q|2φ(x) = qx q ? =( a+bn)x(a ?bn) =(ax+bnx)(a ?bn) =a 2x+ bnxa ? axbn ?bnxbn= a 2x+ ab(n x?xn)?b 2nxn. 即 |q|2(qxq ?1) = a 2x+ ab(n x?xn)?b 2nxn …… ⑦

基于信息融合技术的液压支架姿态监测方法研究

基于信息融合技术的液压支架姿态监测方法研究随着煤矿综采工作面智能化发展,作为采煤链上的重要一环,液压支架发挥着核心推动作用。液压支架智能化关键技术之一的姿态监测技术目前在理论方面还不够完善,因此对其进行深层次的研究是很有必要的。本文在对两柱掩护式液压支架与围岩耦合关系分析的基础上,建立了液压支架姿态监测模型,研究了姿态角度和支护高度两参数的多传感器融合方法,设计了姿态监测虚拟仪器平台,核心内容总结如下:(1)以液压支架底座水平、顶梁前端抬头这一典型姿态为例建立了液压支架几何模型,在此基础上建立了液压支架顶梁受载解析模型,用MATLAB推导出姿态角度,立柱、平衡千斤顶压力和受载合力大小、位置的关系式,得出了监测受载合力的基础是对液压支架姿态进行监测的结论;对不同的底座姿态下可能出现的液压支架姿态种类进行总结,得出了液压支架姿态监测的参数要求,建立了融合倾角传感器和陀螺仪数据的液压支架姿态监测模型。(2)研究了基于卡尔曼滤波算法的液压支架姿态角度融合求解方法,并利用MATLAB对该算法进行仿真,结果表明卡尔曼滤波融合算法的融合效果较好,融合后最大振幅由3.753°降到0.746°,角度最大误差由10.8672°降到几乎趋近于零,对误差起到了很好的收敛作用。 样机实验结果表明,在静止环境下,陀螺仪的累计误差使最大角度偏差为0.28°,经算法融合后最大偏差降到0.09°;在动态环境下,倾角传感器受加速度影响测量波动大,融合后角度更接近真实值。(3)研究了倾角传感器双轴耦合关系,建立底座在不同姿态下各支护高度求解公式。利用倾角传感器和陀螺仪的测量角度数据,研究了分批估计算法和自适应加权算法,提出了一种精确求解液压支架支护高度的二级融合算法,经样机实验数据处理后,与算术均值法和普通分批估计法对比,二级融合算法相对误差为%00786.0,融合效果最好。(4)建立了基于图形化语言LabVIEW软件的虚拟仪器姿态数据采集平台,结合对实际液压支架姿态监测需求的分析,设计了姿态数据采集平台功能,利用LabVIEW编写了姿态数据采集虚拟仪器平台各功能的后面板程序,实现了液压支架姿态的有效监测。

飞控姿态解算理解

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

四元数

为什么使用四元数 为了回答这个问题，先来看看一般关于旋转（面向）的描述方法－欧拉描述法。它使用最简单的x,y,z值来分别表示在x，y，z轴上的旋转角度，其取值为0-360(或者0-2pi），一般使用roll，pitch，yaw来表示这些分量的旋转值。需要注意的是，这里的旋转是针对世界坐标系说的，这意味着第一次的旋转不会影响第二、三次的转轴，简单的说，三角度系统无法表现任意轴的旋转，只要一开始旋转，物体本身就失去了任意轴的自主性，这也就导致了万向轴锁（Gimbal Lock）的问题。 还有一种是轴角的描述方法（即我一直以为的四元数的表示法），这种方法比欧拉描述要好，它避免了Gimbal Lock，它使用一个3维向量表示转轴和一个角度分量表示绕此转轴的旋转角度，即(x,y,z,angle)，一般表示为(x,y,z,w)或者(v,w)。但这种描述法却不适合插值。 那到底什么是Gimbal Lock呢？正如前面所说，因为欧拉描述中针对x,y,z的旋转描述是世界坐标系下的值，所以当任意一轴旋转90°的时候会导致该轴同其他轴重合，此时旋转被重合的轴可能没有任何效果，这就是Gimbal Lock，这里有个例子演示了Gimbal Lock，点击这里下载。运行这个例子，使用左右箭头改变yaw 为90°，此时不管是使用上下箭头还是Insert、Page Up键都无法改变Pitch，而都是改变了模型的roll。 那么轴、角的描述方法又有什么问题呢？虽然轴、角的描述解决了Gimbal Lock，但这样的描述方法会导致差值不平滑，差值结果可能跳跃，欧拉描述同样有这样的问题。 什么是四元数 四元数一般定义如下： q=w+xi+yj+zk 其中w是实数，x,y,z是虚数，其中: i*i=-1 j*j=-1 k*k=-1 也可以表示为： q=[w,v] 其中v=(x,y,z)是矢量，w是标量，虽然v是矢量，但不能简单的理解为3D空间的矢量，它是4维空间中的的矢量，也是非常不容易想像的。 四元数也是可以归一化的，并且只有单位化的四元数才用来描述旋转（面向），四元数的单位化与Vector

四元数姿态的梯度下降法推导和解读

四元数姿态的梯度下降法推导和解读 笔者前面几篇文章讨论的是基于四元数的互补滤波算法，并单独对地磁计融合部分做了详细的讨论和解释。而本文讨论的姿态融合算法叫做梯度下降法，这部分代码可以参见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、arduino控制器 arduino，是一个开源的单片机控制器，采用了基于开放源代码的软硬件平台，构建于开放源代码simple I/O接口版，并且具有使用类似Java，C语言的Processing/Wiring开发环境。 arduino的特色： （1）基于创用CC开放源代码的电路图设计。 （2）免费下载，也可依需求自己修改，但需遵照姓名标示。 （3）arduino可使用ICSP线上烧入器，将bootloader烧入新的 IC 芯片。 （4）可依据官方电路图，简化arduino模组，完成独立运作的微处理控制。 （5）可简单地与传感器，各式各样的电子元件连接，如红外线、超音波、热敏电阻、光敏电阻、伺服马达…等。 （6）支援多样的互动程式，如Adobe Flash, Max/MSP, VVVV, Pure Data, C, Processing…等。 （7）使用低价格的微处理控制器(A TMEGA 8–328)。 （8）USB 接口，不需外接电源。另外有提供 9V 直流电源输入。 （9）应用方面，利用arduino，突破以往只能使用鼠标、键盘、CCD 等输入的装置的互动内容，可以更简单地达成单人或多人游戏互动。 2、arduino nano 3.0

图1 arduino nano 3.0的正反面图片以及针脚标注 二、传感器 1、六轴姿态模块

图2 六轴姿态模块的实物图与针脚图 此六轴姿态模块有三轴加速度计ADXL335 + LPR550AL（MEMS motion sensor dual axis pitch and roll ±500°/s analog gyroscope）+ L Y550ALH（MEMS motion sensor high performance yaw ±500°/s analog gyroscope）组成。有以下优点：? 2.7-3.6VDC power supply ?Low power consumption ?Pitch, yaw, and roll gyro outputs ?1x and 4x amplified (0.5 and 2 mV/°/s sensitivity, respectively) ?±500°/s range ?x-, y-, and z-axis accelerometer outputs ?330mV/g sensitivity ?±3g range ?All necessary filtering components ?Access to gyro's self-test, power-down, and high-pass filter reset pins 2、三轴加速度计ADXL335 如图3： 图3 三轴加速度计ADXL335的性能参数截图 2、三轴陀螺仪：LPR550AL+LY550ALH

[J]基于四元数姿态确定的扩展卡尔曼滤波方法

２０。４中国控制与决策学术年会论文集 Ｐｒｅｃｅｄｉｎｇｓｏｆ２００４ａ妊，螂ｆＣｏｎｔｒｏｌａｎｄ￡ｋ口ｎ。力Ｃｏｎｆｅｒｅｎｃｅ 基于四元数姿态确定的扩展卡尔曼滤波方法 李金良，刘向东，张宇河，修春波 （北京理工大学自动控制系，北京］０００８１） 摘要：针对卫星机尉情况下姿寿碓定闸燕提出了一种扩展卡车曼媳敢方法，栗用陀螺忮、太阳敏麝器和红井地平戗作为测量元件．秉用姿态四元撤设计荽态估计嚣井建立更加准砖有兢的盏性误差方程．仿真结果表明，使用所提出的姿卷确定算法能较好地完威卫星婆奄确定的要采． 关键词：姿态四元敷；姿志．喃定；扩展÷尔曼蘸ｊ置；卫星 ＡｔｔｉｔｕｄｅｄｅｔｅｒｍｉｎａｔｉｏｎｏｆｅｘｔｅｎｄｅｄＫａｌｍａｎｆｉｌｔｅｒｉｎｇｂａｓｅｄｏｎｑｕａｔｅｒｎｉｏｎ ＬＩＪｉｎ—ｌｉａｎｇ，Ｌ见ＴＸｉａｎｇ—ｄａｎｇ，ＺＨＡＮＧＹｕ—ｋ．盖几．Ｃｈｕｎ—ｂｏ （ＤｅｐａｒｔｍｅｎｔｏｆＡｕｔｏｍａｔｉｃＣｏｎｔｒｏｌ，＆ｉｊ／ｎｇＩｎｓｔｉｔｕｔｅｏｆＴｅｃｈｎｏｌｏｇｙ，Ｂｅｉｊｌｎｇ１０００８１，Ｃｈｉｎａ．Ｃｏｒｒｅｓｐｏｎｄｅｍ．ＬｔＪ１．一ｉｉａｎｇ，Ｅ—ｍａｉｌ：ｌｊｌｉａｎｇ＠ｂｉｔ．ｅｄｕ．ｃｎ） Ａｂｓｔｒａｃｔ：ＡｎｅｘｔｅｎｄｅｄＫａｌｍａｎｆｉｈｅｒｍｇｔｅｃｈｎｉｑｕｅｉｏｒｔｈｅｌａｒｇｅａｎｇｕｌａｒｍａｎｅｕｖｅｒｉｎｇａｎｄｎｏｒｍａｌｍｏｄｅａｔ：ｉｔｕｄｅｄｅｔｅｒｎａｉｎａｄｏｎ。ｆＥｓａｔｅｌｌｉｔｅｉｓｐｒｅｓｅｎｔｅｄｂｙｕｓｉｎｇｇｙｒｃ，ｓｕｎＳｅｘｑｓｏｒｓａｎｄｉｎｆｒａｒｅｄｈｏｒｉｚｏｎｓｃａｌａｎｅＴｅｏ．ＴｈｅａｔｔｉｔｕｄｅｑｕａｔｅｒｍｏｎｒｅｐｒｅｓｅｎｔａｔｉｏｎｉｓａｄａｐｔｅｄｔＯｄｅｓＮｎａＤａｔｔｉｔｕｄｅｅｓｔｉｍａｔｏｒａｎｄｉｎｔｅｇｒａｔｉｏｎｌｉｎｅａｒｅｒｒｏｒｅｑｕａｔｉｏｎｓａｒｅｅｓｚａｈｌｉｓｈｅｄ．Ｔｈｅｒｅｓｕｌｔｓｏｆｃｈｅｓｉｍｕｌａｔｉｏｎｓｈｏｗｔｈａｌａｔｕｔｕｄｅｄｅｔｅｒｍｉｎａｔｉｏｎｉｓａｃｃｕｒａｔｅｌｙａｃｈｉｅｖｅｄｂｙｕｓｍｇｔｈｅａｔｔｉｚｕｄｅｄｅｔｅｒｍｉｎａｔｉｏｎａｌｇｏｒｉｔｈｍ． ｒ：≈ｗｏｒｄｓ：ａｔｔｉｔｕｄｅｑｕａｔｅｒｉｎｏｎ；ａｔｔｉｔｕｄｅ ｄｅｔｅｒ叫ｎａｔ】。ｎ：ｅｘ『ｔｅｎｄｅｄＫａｌｍａ口ｉｉ］ｔｅｒｉｎｇ：ｓａｔｅｌｌｉｔｅ ｉ引言 卫星姿态确定系统决定了卫星控制和指向精度．而姿态敏感器的测量精度和姿态确定算法直接影响确定系统的精度．在常规的确定系统中，通常采用陀螺作为基准元件测量连续的姿态角速度，使用太阳敏感器、红外地平仪或磁强计＝耐量姿态位置．目前，已有很多姿态确定算法，诸如宜适应滤波、预测滤波、卡尔曼滤波、自适应卡尔曼滤渡、扩展卡尔曼滤波等，用于减小敏感器的测量误差和提高确定系统的测量精度．本文使用扩展卡尔曼滤波方法研究了太阳同步轨道卫星的姿态确定，该卫星采用了陀螺、太阳敏感器、红外地平仪构成的测量系统．为各敏感器建立了更加详细的姿态估计器．考虑在大姿态角度下Ｅｕｌｅｒ角描述容易引起奇异性，本文采用了四元数描述建立卫星运动学方程．仿真结果表明，该方法有效地提高了卫星的测量精度． ２姿态运动学方程 使用四元数ｑ＊＝ｂ。目，口：ｇａｊ７描述在星体坐标系相对于当地轨道坐标系的卫星姿态，它的矩阵可用四元数表示如下： Ｔ。（‰）＝ ｒ２（酣＋爵）一１２（吼ｇｌ＋口。舶）２（口妇１一ｇ。ｇ：）一 ｌ２曲：ｑ２一ｇｏ吼）２（毹＋一）一ｌ２（口２吼＋ｑ。ｑ１）｜ ｌ２（目，目。＋ｇｏｑ２）２（口ｚ啦一口。口】）２（矗＋一）一１Ｊ ＼▲，相应四元数描述的动力学方程为 １ ｊ－一÷口。＠Ａ（ｎｋ）， ● ｃ‰＝Ⅲ－一７乙（‰）吣．（２）式中：ｏ表示四元数乘法，Ａ（‰）一［ｏ‰１］。，％ 收稿日期：２００３一ｌｏ－１２． 作者简介：李金良（１§７７一），男，北京人，硬±生，从事卫星姿志控制的研究 Ｔ　＝　－Ｔ转置方向余弦矩阵的四元数表示

捷联惯导姿态算法中的圆锥误差与量化误差

第27卷　第1期航　空　学　报 Vol 127No 11　2006年 1月ACTA A ERONAU TICA ET ASTRONAU TICA SIN ICA J an.　2006 收稿日期:2004209230;修订日期:2005204227基金项目:国家自然科学基金(60234030)、国家杰出青年科学基金 (60225015)和教育部高校青年教师奖资助项目 文章编号:100026893(2005)0120098206 捷联惯导姿态算法中的圆锥误差与量化误差 练军想,胡德文,胡小平,吴文启 (国防科技大学机电工程与自动化学院自动控制系,湖南长沙　410073) R esearch on Coning E rror and Q uantization E rror of SINS Attitude Algorithm L IAN J un 2xiang ,HU De 2wen ,HU Xiao 2ping ,WU Wen 2qi (Department of Automatic Control ,College of Mechatronics and Automation ,National University of Denfense Technology ,Changsha 410073,China ) 摘　要:对捷联惯导系统的误差源进行了研究,利用几何方法分析了不可交换性误差和量化误差的形成机理,以及它们的相互影响。针对工程应用中激光陀螺输出脉冲采样量化条件,就多子样算法进行了讨论,并设计了基于MA TL AB/Simulink 的仿真。研究结果表明,当考虑量化误差的影响时,选取适当的量化因子,三子样等效旋转矢量算法比其它算法具有更好的综合性能。 关键词:捷联惯导;姿态算法;不可交换性误差;量化误差;多子样中图分类号:V249.3 文献标识码:A Abstract :In this paper ,the error sources of strapdown inertial navigation system (SINS )are researched.The mechanism of noncommutativity error and quantization error is illustrated f rom the geometrical point of view ,and the mutual influence between them is analyzed.Considering the sample condition of the output pulses of the ring laser gyroscope (RL G ),the multi 2sample attitude algorithm of SINS is discussed.The simu 2lation is carried out using MA TL AB/Simulink.It is concluded that the three 2sample rotation vector algorithm with the proper quantization factor outperforms others when the impact of quantization errors is taken into ac 2count. K ey w ords :SINS ;attitude algorithm ;noncommutativity error ;quantization error ;multi 2sample 对于捷联惯导系统姿态更新算法的研究,目前国内外同行们有很多成果。普遍的观点认 为[1,2]:四元数姿态更新算法比方向余弦和欧拉角等其它方法的计算量小、适应性好;双子样、三子样等效旋转矢量算法在抑制不可交换性误差方面比单子样有显著改善;将姿态更新分成快慢两个回路,即快速回路更新旋转矢量,慢速回路更新姿态参数,既能有效抑制不可交换性误差,还可以减少算法的计算量。但上述观点很少有结合捷联惯导系统工程实际来考虑的。本文针对工程中激光陀螺输出脉冲采样量化条件,就多子样算法展开研究,并得出了相应的结论。 1　误差源分析 评价算法优劣的最重要的标准之一是算法的 精度。要提高算法的精度就必须研究各种误差及其补偿、抑制方法。 在对惯导系统作原理性分析时,尚可以理想化地认为平台坐标系(平台式或捷联式)准确、无误差地模拟地理坐标系,但实际的惯导系统不可避免地受到各种误差的影响[3]。通常,可将误差分为源信息误差和解算方法误差。对捷联惯导系统,解算方法误差是指数值求解姿态矩阵和比力方程时产生的原理性误差,比如不可交换性误差、舍入误差、截断误差等。源信息误差可分为解析源信息误差和测量源信息误差。测量源信息误差就是惯性传感器的误差,此类误差因惯性器件的物理原理和仪表的精度不同而各不相同。比如陀螺的常值漂移和随机漂移、刻度因子误差、非线性误差、输出值的量化误差等。 下面重点对捷联姿态解算时不可交换性误差和陀螺采样的量化误差进行分析。111　不可交换性误差 在利用方向余弦矩阵微分方程或四元数微分方程进行姿态更新时,都会遇到角速度矢量的积分问题。由于刚体有限转动的不可交换性,当转

捷联式姿态解算过程

（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 。

基于四元数方法的姿态解算

基于四元数方法的姿态解算方法分析 摘要:载体的姿态解算算法是实现捷联式惯性导航系统精确导航的核心技术之一。分析了欧拉法、方向余弦法、四元数法求解姿态矩阵的优缺点，采用四元数法与方向余弦法两种解算方法分别计算载体姿态，两种方法的计算结果之差与理论真值比较以得到解算的相对误差，从而验证了四元数法的正确性和有效性。最后，指出提高采样频率和采用高阶计算算法能进一步减小姿态解算误差。数字化仿真与转台试验结果表明，本文提出的载体姿态解算法具有良好的实时性。 1引言 捷联惯导是一种自主式的导航方法。该方法将陀螺仪和加速度计直接安装在载体上，省掉机电式导航平台，利用计算机软件建立一个“数学平台”来代替机电平台实体[1]。由于其结构简单且抗干扰能力强，目前已成为航空航天、航海、机器人、智能交通等领域的研究热点之一。 姿态解算是捷联式惯性导航系统的关键技术，通过姿态矩阵可以得到载体的姿态和导航参数计算需要的数据，是捷联式惯导算法中的重要工作。载体的姿态和航向体现了载体坐标系与导航坐标系之间的方位关系，确定两个坐标系之间的方位关系需要借助矩阵法和力学中的刚体定点运动的位移定理。通过矩阵法推导方向余弦表，而刚体定点运动的位移定理表明，定点运动刚体的任何有限位移都可以绕过定点的某一轴经过一次转动来实现。目前描述动坐标相对参考坐标系方位关系的方法有多种，可简单地将其分为3类，即三参数法、四参数法和九参数法「1-2]。三参数法也叫欧拉角法，四参数法通常指四元数法，九参数法称作方向余弦法。欧拉角法由于不能用于全姿态飞行运载体上而难以广泛用于工程实践，且实时计算困难。方向余弦法避免了欧拉法的“奇点”现象，但方程的计算量大，工作效率低。随着飞行运载体导航控制系统的迅速发展和数字计算机在运动控制中的应用，控制系统要求导航计算环节能更加合理地描述载体的刚体空间运动，四元数法的研究得到了广泛重视。本文全面分析了3种解算方法的特点，通过对比四参法与九参法的计算结果以验证四元数法的正确性和有效性，基于数值仿真和转台实验相结合的分析方法得到进一步减少姿态解算误差的有效途径，为捷联式惯性导航技术的工程实践提供参考。（就是这部分内容需要程序解算，不会搞） 2姿态矩阵的计算方法 由于载体的姿态方位角速率较大，所以针对姿态矩阵的实时计算提出了更高的要求。通常假定捷联系统“数学平台”模拟地理坐标系，即导航坐标系;而确定载体的姿态矩阵即为研究载体坐标系(6)和导航坐标系(E)的空间转动关系，一般用载体坐标系相对导航坐标系的三次转动角确定，习惯上俯仰角和偏航角用B和必表示，滚转角用Y表示。目前主要的研究方法为:欧拉法、方向余弦法与四元数法。图1为捷联式惯性导航原理图。

四轴姿态解算

又花了将近一个星期，终于把姿态解算的框架完成了。仅仅是把陀螺仪、加速度计、罗盘融合在一起，得出旋转姿态，没有对加速度积分，没有用到气压计，几乎没有滤波。算是阶段性的工作吧，把框架设计得合理一点，以后添加/修改就很简单了。 从传感器的读取，到四元数的学习，到空间旋转的处理方法，循序渐进，逐个击破。主要参考了以下资料（按阅读的时间循序）： 《计算机图形学几何工具算法详解》（四元数转矩阵的公式是错的！） 《交互式计算机图形学——基于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”

【CN110008913A】基于姿态估计与视点机制融合的行人再识别方法【专利】

(19)中华人民共和国国家知识产权局 (12)发明专利申请 (10)申请公布号 (43)申请公布日 (21)申请号 201910287259.1 (22)申请日 2019.04.08 (71)申请人 南京工业大学 地址 211816 江苏省南京市江北新区浦珠 南路30号 (72)发明人 徐曾春　裴嘉震　胡平　 (51)Int.Cl. G06K 9/00(2006.01) G06K 9/62(2006.01) (54)发明名称基于姿态估计与视点机制融合的行人再识别方法(57)摘要本发明设计一种基于姿态估计与视点机制融合的行人再识别方法。用于解决各种需要进行刑侦，失踪人员寻找问题，为了达到该目的，该方法通过考虑行人的姿势结构信息，采用姿势估计网络Openpose提取行人关键点，同时考虑相机拍摄的视点位置，根据图像的拍摄的角度，生成行人身体关键区域，帮助对齐图像中的身体区域特征并可以避免相机拍摄到行人侧面时行人关键区域被遮挡等问题。最终，通过特征融合子网络将局部特征图与全局特征图进行融合来合并不同语义级别的区域特征。通过对行人数据集的训练，得到对应的模型，通过加载模型并采用本文的神经网络结构预测出所有与目标行人匹配的 行人。权利要求书2页 说明书4页 附图1页CN 110008913 A 2019.07.12 C N 110008913 A

1.基于姿态估计与视点机制融合的行人再识别方法，其特征在于，所述方法包括以下步骤： 步骤一：行人再识别数据集采集，获取行人再识别公开数据集，区分训练集与测试集，进入步骤二； 步骤二：行人再识别数据集训练，将行人再识别公开数据集的训练集作为深度卷积神经网络模型的训练集，最终得到行人再识别模型进行进入步骤三； 步骤三：特征提取与融合，根据步骤二，得到的行人再识别模型，对查询图像与库图像提取网络中的全局特征与局部特征，将一张图像输入网络后，通过特征嵌入网络与特征融合网络最终获得一个2048维向量特征进入步骤四； 步骤四：再识别分析，根据步骤三获得的2048维特征向量，计算查询图像特征与待查询图像特征之间的欧几里得距离，将该距离由小到大排序，排名较前的图像较大可能与查询图像为同一行人，并利用CMC累积匹配曲线中的RANK1，RANK5，RANK10，RANK20与平均精度mAP作为算法性能的评价指标。 2.根据权利要求1所述的基于姿态估计与视点机制融合的行人再识别方法，其特征在于，步骤一中所述得到行人再识别数据集方法包括以下步骤： 步骤一：获取广泛使用的行人再识别数据集CHUK 03，Market -1501，与DukeMTMC -reID 数据集进行实验，将CHUK03中的第三组的100个行人图像作为测试集，剩余1367个行人的图像作为训练集，Market1501训练集有751人，包含12,936张图像；测试集有750人，包含19,732张图像；训练集有702个行人的16522张图像，测试集有702个行人的17661张图像。 3.根据权利要求1所述的基于姿态估计与视点机制融合的行人再识别方法，其特征在于，步骤二所述的卷积神经网络模型训练包括以下步骤： 步骤一：训练过程中，输入图像进入我们的卷积神经网络，进行特征提取，深度卷积神经网络框架由特征嵌入网络与特征融合网络组成，其主干网络为Resnet50； 步骤二：考虑训练过程中的损失函数，我们采用三个Softmax loss作为Loss函数，分别为全局损失，融合损失，局部损失， 统一用以下公式作为损失函数： 4.根据权利要求1所述的基于姿态估计与视点机制融合的行人再识别方法，其特征在于，步骤三所述的特征提取与融合包括以下步骤： 步骤一：首先输入待查询图像，对图像中行人进行姿态估计与视点判别，获取行人姿势关节位置，并通过视图分类器获取图像视点方向，根据不同的视点方向以及姿势关节位置组合成不同的人体部位推荐，视点位置为“前”，“后”时图像角度良好，推荐6个局部身体区域，分别为{头部B 1，躯干B 2，左臂B 3，右臂B 4，左腿B 5，右腿B 6}。视点位置为“侧”时，图像角度较差，推荐3个局部身体区域，分别为{头肩部A 1＝(B 1)，上身A 2＝(B 2，B 3，B 4)，下身A 3＝(B 5，B 6)}； 其中15个关节点K i ＝[X i ，Y i ]，(i＝1，2，...，15)可以通过NMS(非极大值抑制)方法获取， 通过以下公式(2)表示：式中为每个身体关节的响应图 权　利　要　求　书1/2页2CN 110008913 A

四元数的初步总结

四元数的初步总结 （一） 前一阵子，以前公司的一位同事向我请教一段计算机图形程序中的算法，其中涉及齐次坐标和四元数。齐次坐标问题到好讲解，但四元数方面以前所知几乎为零。正好我看到齐民友在《复分析，可视化方法》译后记中提到的一本书：《高观点下的初等数学》([德]克莱因著，以下简称《初等数学》)当中有一段讲到四元数，于是就细读了一遍，把这个专题的整理笔记写下来。 但是那本书里有很多结果依靠繁杂的机械运算，让人看了不知道这样的结果是怎么得出来的。因此我们这里用向量代数的观点重新审视四元数的一些结果，让四元数的特性看起来更直观，更自然。另外还有一些我认为重要的有关四元数引入的背景知识，例如数域的扩充问题的证明，那本书里只有一部分提示，这里也试着补全一些。 一、四元数引入的理论背景 将实数域扩充到复数域，并用复数来表示平面向量，用复数的加、乘运算表示平面向量的合成、伸缩和旋转变换，这些观念已经在中学课程中学过了。那么，很自然的问题就是，在三维，或更高维空间中是否也有复数的类似物？也就是说，像扩充实数那样，在复数域的基础上添加一个或几个新的元素，并且让它们跟原来的复数做加减乘除，是否就可以得到一个新的数集，并且其中的元素还可以像复数域那样做加、减、乘、除运算，并满足通常复数的那些运算律，包括加法和乘法的交换律与结合律、乘法对加法的分配律等待？更进一步，我们是否可以期望用这样的数来表示三维或更高维空间中的伸缩和旋转，就像用复数表示平面向量的伸缩旋转那样方便？ 把问题说得明确一些，即是说，我们是否可以像得到复数域那样，在复数域中再添加一个新的元素（因 此也是在实数基础上添加两个元素和），得到一个类似于复数集合 ，这个集合中的元素当时就是普通的复数，当时就是普通的实数，并且通常数的加减乘除运算及其性质都可以在这个集合上保持，即满足： 1、对于任意两个数，它们的和是唯一确定的。 2、对于任意两个数，它们的积是唯一确定的。 3、存在一个数0，它具有性质：对于任意a，均有a+0=a。 4、对于每一个数a，均存在数x，适合等式a+x=0。 5、加法适合交换律：a+b=b+a。 6、加法适合结合律：(a+b)+c=a+(b+c)。 7、乘法适合交换律：a·b=b·a。 8、乘法适合结合律：(a·b)·c=a·(b·c)。 9、乘法对加法适合分配律：a (b+c)=ab+ac 和(a+b)c=ac+bc。 10、1 是乘法单位元，即仍然满足1·a=a·1=a 11、乘法有逆元，即对每个非零数a，存在唯一的数x，满足等式xa=ax=1。 历史上有很多数学家试图寻找过三维的复数，但后来证明这样的三维复数是不存在的。有关这个结论的证明，我没有查到更明确的版本，据《古今数学思想》中的一个理由，三维空间中的伸缩旋转变换需要四个变量来决定：两个变量决定轴的方向，一个变量决定旋转角度，一个变量决定伸缩比例。这样，只有三个变量的三维复数无法满足这样的要求。

捷联惯导姿态算法中的圆锥误差与量化误差

第27卷 第1期航 空 学 报 Vo l 27No 1 2006年 1月ACT A A ERON A U T ICA ET A ST RO N AU T ICA SIN ICA Jan. 2006 收稿日期:2004 09 30;修订日期:2005 04 27基金项目:国家自然科学基金(60234030)、国家杰出青年科学基金 (60225015)和教育部高校青年教师奖资助项目 文章编号:1000 6893(2005)01 0098 06捷联惯导姿态算法中的圆锥误差与量化误差 练军想,胡德文,胡小平,吴文启 (国防科技大学机电工程与自动化学院自动控制系,湖南长沙 410073) Research on C oning Error and Quantization Error of SINS Attitude Algorithm LIAN Jun x iang ,H U De w en,H U Xiao ping ,WU W en qi (Department of A utomatic Contr ol,Colleg e of M echat ronics and A utomation,N atio nal U niver sity of Denfense T echno log y,Chang sha 410073,China) 摘 要:对捷联惯导系统的误差源进行了研究,利用几何方法分析了不可交换性误差和量化误差的形成机理,以及它们的相互影响。针对工程应用中激光陀螺输出脉冲采样量化条件,就多子样算法进行了讨论,并设计了基于M A T L A B/Simulink 的仿真。研究结果表明,当考虑量化误差的影响时,选取适当的量化因子,三子样等效旋转矢量算法比其它算法具有更好的综合性能。 关键词:捷联惯导;姿态算法;不可交换性误差;量化误差;多子样中图分类号:V 249.3 文献标识码:A Abstract:In this paper,the er ror so urces o f str apdow n iner tial nav igat ion system (SIN S)are r esear ched.T he mechanism of nonco mmut ativity er ro r and quantizatio n er ro r is illustr ated fr om the g eometrical point of v iew,and the mutual influence betw een them is analy zed.Considering the sample quantizing co ndition of the o utput pulses of the r ing laser g yro sco pe (R LG ),the multi sample att itude alg or ithm o f SIN S is discussed.T he simu latio n is car ried out using M A T L A B/Simulink.It is co ncluded that the three sam ple rotat ion vector alg or ithm wit h t he pro per quant izat ion factor outper for ms o thers when the impact of quant izat ion err ors is taken into ac count. Key words:SI NS;attitude algo rithm;no nco mmutat ivit y er ror ;quantizatio n er ro r;multi sample 对于捷联惯导系统姿态更新算法的研究,目 前国内外同行们有很多成果。普遍的观点认为[1,2]:四元数姿态更新算法比方向余弦和欧拉角等其它方法的计算量小、适应性好;双子样、三子样等效旋转矢量算法在抑制不可交换性误差方面比单子样有显著改善;将姿态更新分成快慢两个回路,即快速回路更新旋转矢量,慢速回路更新姿态参数,既能有效抑制不可交换性误差,还可以减少算法的计算量。但上述观点很少有结合捷联惯导系统工程实际来考虑的。本文针对工程中激光陀螺输出脉冲采样量化条件,就多子样算法展开研究,并得出了相应的结论。 1 误差源分析 评价算法优劣的最重要的标准之一是算法的 精度。要提高算法的精度就必须研究各种误差及其补偿、抑制方法。 在对惯导系统作原理性分析时,尚可以理想化地认为平台坐标系(平台式或捷联式)准确、无误差地模拟地理坐标系,但实际的惯导系统不可避免地受到各种误差的影响[3]。通常,可将误差分为源信息误差和解算方法误差。对捷联惯导系统,解算方法误差是指数值求解姿态矩阵和比力方程时产生的原理性误差,比如不可交换性误差、舍入误差、截断误差等。源信息误差可分为解析源信息误差和测量源信息误差。测量源信息误差就是惯性传感器的误差,此类误差因惯性器件的物理原理和仪表的精度不同而各不相同。比如陀螺的常值漂移和随机漂移、刻度因子误差、非线性误差、输出值的量化误差等。 下面重点对捷联姿态解算时不可交换性误差和陀螺采样的量化误差进行分析。1 1 不可交换性误差 在利用方向余弦矩阵微分方程或四元数微分 方程进行姿态更新时,都会遇到角速度矢量的积分问题。由于刚体有限转动的不可交换性,当转