|
根据这篇捷联惯导算法心得,初窥捷联惯导,但有两个疑问一直在困扰着我,请求各大侠大显灵通
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 顺序不一样,但是把它弄成一样后,计算欧拉角的元素不一样)
希望惯导大神帮小弟一把 |
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
阿莫论坛20周年了!感谢大家的支持与爱护!!
曾经有一段真挚的爱情摆在我的面前,我没有珍惜,现在想起来,还好我没有珍惜……
|