关于 捷联惯导姿态解算 IMU算法的两个疑惑

lologame

根据这篇捷联惯导算法心得，初窥捷联惯导，但有两个疑问一直在困扰着我，请求各大侠大显灵通
http://www.amobbs.com/forum.php? ... p;page=1#pid6107728

1.
这是IMUupdate里面 用叉乘做两向量的角度差的函数

{

.....//省略

// error is sum of cross product between reference direction of field and direction measured by sensor
        ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);
/*
axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。
axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。
那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。
这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
*/

.....//省略

}
1.IMUupdate里面，计算载体坐标系的ex ey ez，是通过
ax ay az和vx vy vz做向量叉积，用以计算向量的3个角度差值。
但是这个ex ey ez，理论上应该是和(sin弧度)成正的数据。
但是并不是与弧度成正比，那么为什么这里可以用该值做比例运算后，直接用来修正陀螺仪的数据？

以2维为例，以d为ab之间的夹角,i为x方向的单位向量，j为y方向单位向量
叉乘公式1: a X b = |a|*|b|*sin(d); 方向与ab所在平面垂直(用右手规则确定)
叉乘公式2:
a = (x1, y1) = x1*i + y1*j
b = (x2, y2) = x2*i + y2*j
a X b = (x1 X i + y1 X j) X (x2 X i + y2 X j) = x1 X i X (x2 X i + y2 X j) + y1 X j X (x2 X i + y2 X j) = x1 X x2 X i X i    +    x1 X y2 X i X j    +    y1 X x2 X i X j       +      y1 X Y2 X j X j

因为 i X i =0    j X j = 0    (夹角为0，叉乘为0 sin(0) = 0)        i X j = m(设m为垂直i和j的单位向量,其实也就是相当于z轴的单位向量)

所以a X b = (x1 X y2 + y1 X x2) X m

结合叉乘公式1 和 叉乘公式2. :
(x1 X y2 + y1 X x2) X m = |a|*|b|*sin(d)
(x1 X y2 + y1 X x2) = (|a|*|b|/ m) * sin(d)
这样的话x1 X y2 + y1 X x2 只是和sin(弧度值)成正比，而不是和弧度本身成正比，陀螺仪测出来的是角速度，为什么可以用来直接修正呢？




2.
以下是四元数-》欧拉角的转换公式
//更新方向余弦矩阵
        t11=q.q0*q.q0+q.q1*q.q1-q.q2*q.q2-q.q3*q.q3;
        t12=2.0*(q.q1*q.q2+q.q0*q.q3);
        t13=2.0*(q.q1*q.q3-q.q0*q.q2);
        t21=2.0*(q.q1*q.q2-q.q0*q.q3);
        t22=q.q0*q.q0-q.q1*q.q1+q.q2*q.q2-q.q3*q.q3;
        t23=2.0*(q.q2*q.q3+q.q0*q.q1);
        t31=2.0*(q.q1*q.q3+q.q0*q.q2);
        t32=2.0*(q.q2*q.q3-q.q0*q.q1);
        t33=q.q0*q.q0-q.q1*q.q1-q.q2*q.q2+q.q3*q.q3;
        //求出欧拉角
        imu.euler.roll = atan2(t23,t33);
        imu.euler.pitch = -asin(t13);
        imu.euler.yaw = atan2(t12,t11);
        if (imu.euler.yaw < 0){
                imu.euler.yaw += ToRad(360);
        }

四元数实际上是可以 转换成 坐标变换矩阵，
坐标变换矩阵 有一个参考坐标系
方向余弦矩阵 有一个参考坐标系
当两个坐标系均为同一坐标系时（捷联惯导中常为地理坐标系）
两个矩阵元素对应相等
就可以通过四元数反推方向余弦矩阵，再通过方向余弦矩阵求出欧拉角。

一下是捷联惯导算法心得中的欧拉角求解代码
{

//更新方向余弦矩阵
        t11=q.q0*q.q0+q.q1*q.q1-q.q2*q.q2-q.q3*q.q3;
        t12=2.0*(q.q1*q.q2+q.q0*q.q3);
        t13=2.0*(q.q1*q.q3-q.q0*q.q2);
        t21=2.0*(q.q1*q.q2-q.q0*q.q3);
        t22=q.q0*q.q0-q.q1*q.q1+q.q2*q.q2-q.q3*q.q3;
        t23=2.0*(q.q2*q.q3+q.q0*q.q1);
        t31=2.0*(q.q1*q.q3+q.q0*q.q2);
        t32=2.0*(q.q2*q.q3-q.q0*q.q1);
        t33=q.q0*q.q0-q.q1*q.q1-q.q2*q.q2+q.q3*q.q3;
        //求出欧拉角
        imu.euler.roll = atan2(t23,t33);
        imu.euler.pitch = -asin(t13);
        imu.euler.yaw = atan2(t12,t11);
        if (imu.euler.yaw < 0){
                imu.euler.yaw += ToRad(360);
        }
}

我的问题就出在这儿 欧拉角计算公式为什么是
imu.euler.roll = atan2(t23,t33);
imu.euler.pitch = -asin(t13);
imu.euler.yaw = atan2(t12,t11);

根据 秦永元 的“惯性导航”一书， 欧拉角求解公式应该如图所示（这里面的txx（如t11 t12 t13等） 和惯性导航中的Txx 顺序不一样，但是把它弄成一样后，计算欧拉角的元素不一样）


希望惯导大神帮小弟一把

john800422

1.
這裡的誤差是指"傳感器上測得的加速度"與"從地理座標轉到載體上的重力加速度"的"""重合"""程度
利用此誤差及PID對陀螺儀做修正，單位不是必要的

2.
矩陣內的元素會因為你座標的定義而有所不同
但基本上值都一樣，只是位置不同，知道自己在做甚麼就好了

lologame

john800422 发表于 2013-5-20 15:30
1.
這裡的誤差是指"傳感器上測得的加速度"與"從地理座標轉到載體上的重力加速度"的"""重合"""程度
利用此 ...

1、我又想了下，按我的理解，是由于两个加速度偏角 应该不是很大
就例如，sin(d)当d很小的时候，sin(d)就可以约等于d。

2、矩陣內的元素會因為你座標的定義而有所不同
这个坐标该如何去理解它？
都是以地理坐标系作导航坐标系的啊。

john800422

lologame 发表于 2013-5-20 15:55
1、我又想了下，按我的理解，是由于两个加速度偏角 应该不是很大
就例如，sin(d)当d很小的时候，sin(d) ...

重點是在於旋轉
可以搜看看歐拉角

asha

旋转次序不一样，如果你参考的是秦永元的书的姿态矩阵的话  它是按照-Z X Y旋转，一般现在流行的程序是按照Z  Y   X旋转出来的。对应的欧拉角定义不一样

clqfly

john800422 发表于 2013-5-20 15:30
1.
這裡的誤差是指"傳感器上測得的加速度"與"從地理座標轉到載體上的重力加速度"的"""重合"""程度
利用此 ...

第一个疑问我也有，听你这么一分析豁然开朗，原来是利用了控制理论

lologame

asha 发表于 2013-5-20 20:10
旋转次序不一样，如果你参考的是秦永元的书的姿态矩阵的话  它是按照-Z X Y旋转，一般现在流行的程序是按照 ...

那么我们应该如何判断当前的矩阵是按哪种欧拉角旋转方式得到的呢？

我看了下程序里面四元数的更新处，和书本上的是一样的。（即以Z X Y为旋转顺序）

仅仅只是四元数初始化的时候，不一样。

asha

lologame 发表于 2013-5-21 12:33
那么我们应该如何判断当前的矩阵是按哪种欧拉角旋转方式得到的呢？

我看了下程序里面四元数的更新处，和 ...

可以自己推倒一下 ，用四元数的复数表示方法 比如Z Y X的选择次序 四元数对应的就是 (cos+k*sin)*(cos+j*sin)*(cos+i*sin)，忘记复数是cos为实部还是sin为实部了  cos  sin里面的角度是对应定义角度的0.5倍

lologame

asha 发表于 2013-5-21 14:58
可以自己推倒一下 ，用四元数的复数表示方法 比如Z Y X的选择次序 四元数对应的就是 (cos+k*sin)*(cos+j* ...

越来越晕了

这样好吗、如果想把代码变成 Z X Y 的旋转方式，

需要在源代码上做哪些变更呢？

asha

lologame 发表于 2013-5-21 15:51
越来越晕了

这样好吗、如果想把代码变成 Z X Y 的旋转方式，

告诉你方法了，你自己推一下吧。不是很难

lologame

asha 发表于 2013-5-21 16:24
告诉你方法了，你自己推一下吧。不是很难

多谢大侠指点。

imu.euler.roll = atan2(t23,t33);
imu.euler.pitch = -asin(t13);
imu.euler.yaw = atan2(t12,t11);

这个公式里面，roll和pitch好像整反了？

CCALM

转换出来的角度是什么范围的？

我用四元数转换出来的欧拉角只能在0到180度范围内显示姿态，

不知道为什么？？

asha

lologame 发表于 2013-5-21 20:31
多谢大侠指点。

imu.euler.roll = atan2(t23,t33);

这个和旋转次序有关  。我没有仔细看，只要推导一遍公式就都知道了

lologame

asha 发表于 2013-5-21 21:47
这个和旋转次序有关  。我没有仔细看，只要推导一遍公式就都知道了

我又看了两天书 秦永远 的惯性导航中 对四元数的讲解

还是有地方无法理解。

书里面说 参考坐标系都是一样的时候，   四元数求出来的 坐标变换阵 和 姿态矩阵 是相等的关系，并没有给出相关证明。

然而它的 姿态矩阵的旋转方向 如您所说  和程序里面不一样。

我推到了一下四元数，也查阅了相关资料，发现，两种旋转方向下(Z Y X 和 Z X Y)，仅仅是四元数的初始化有区别（按照您的方法所推导）

但是四元数的更新都是一样的！  我对该处表示不理解。

大神能再提点一下吗？

lologame

asha 发表于 2013-5-21 21:47
这个和旋转次序有关  。我没有仔细看，只要推导一遍公式就都知道了

另：
我以前进行过如此处理， 四元数算法  进行了初始化更改（以Z X Y为旋转顺序） 其余没有进行任何更改。

用这些四元数数据得到的 方向余弦矩阵，看做为以 Z X Y为旋转顺序的矩阵，再进行欧拉角推导。

roll pitch yaw 都可以顺利进行解算，但是有一个地方让我百思不得其解，也是我发该帖的最初动机。

就是旋转roll 和 yaw正常，相互之间没有什么影响（影响很微弱）， 但是旋转pitch，发现pitch变化的时候，对roll会有影响，pitch角度小的时候 还可以忍受，当pitch大了，就发现出roll出现大偏差。
---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

用Z Y X为旋转顺序的矩阵， 推导出来的欧拉角，就没有此类情况。

（仅仅当pitch 超过90°的时候，roll进行了一个非常快速的翻转）

STLJ

mark，学点知识后再看

窗外枫叶

lologame 发表于 2013-5-22 21:16
另：
我以前进行过如此处理， 四元数算法  进行了初始化更改（以Z X Y为旋转顺序） 其余没有进行任何更改 ...

您好，请问您的第二个问题解决了吗？麻烦解释一下呗。太谢谢了。