关于四轴算法——四元数与欧拉角的转化
关于四元数与欧拉角的转化的疑惑看到好多人的飞控程序在处理偏航角——Yaw,俯仰角——Pitch,翻滚角——Rool的时候均用到这样两句代码：
Pitch= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Rool = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv
并且没有对Waw进行计算，但是自己查相关文献资料，可以得到四元数与欧拉角的如下转换公式：
Yaw= atan2f( 2 * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 )*57.3;//yaw
Pitch= asin(-2 * (q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Rool = atan2 ( 2 * (q1 * q2 + 2 * q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 )* 57.3; //roll
如附图所示，文件来源：全角度欧拉角与四元数转换研究.pdf
各位大侠，聊聊你们对这部分的理解吧。。。
既然程序都可以用，那应该是有一定的道理的，请问这部分为什么这样处理?
而且不需要求Yaw，这又是为什么呢/???
谢谢各位大神啦！！！
程序源码如下：
#include<math.h>
//---------------------------------------------------------------------------------------------------
// 变量定义
#define f 1
#define Kp 2.0f // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.005f // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.5f // 采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
float gx=0,gy=0,gz=0,ax=0,ay=0,az=0; //全局变量
//互补滤波
//时间常数 t=a/(1-a)*dt a=t/(t+dt) t 截至频率dt计算间隔时间
#define kfa 0.98
#define kfan1.0-kfa
//ang= kfa*ang+kfgn*acc;
#define kfg 0.80
#define kfgn1.0-kfg
float Yaw,Pitch,Rool;//偏航角，俯仰角，翻滚角
//ang= kfa*g+kfgn*acc;
//====================================================================================================
// Function
//====================================================================================================
//gx 采样时间弧度增量
//加数度为3轴采样数据，做归一化
//输出为4元数
void IMUupdate(float gxi, float gyi, float gzi, float axi, float ayi, float azi) {
float norm;
float vx, vy, vz;
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;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
ax=ax*kfa+kfan*axi;
ay=ay*kfa+kfan*ayi;
az=az*kfa+kfan*azi;
gx=gx*kfg+kfgn*gxi;
gy=gy*kfg+kfgn*gyi;
gz=gz*kfg+kfgn*gzi;
// 测量正常化
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// 估计方向的重力
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
// 积分误差比例积分增益
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// 调整后的陀螺仪测量
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// 整合四元数率和正常化
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// 正常化四元
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
Pitch= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Rool = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv
} 不明白四元数+1，帮顶！{:3_48:} 四元数只有一种，欧拉角跟旋转顺序有关的，不同旋转顺序有不同公式。 那个// 测量正常化是卖萌吗？ pdf我下载怎么是.attach的文件呢？ binham 发表于 2013-4-22 17:55 static/image/common/back.gif
pdf我下载怎么是.attach的文件呢？
没关系，直接打开就行。 帮顶，最近也在学四元数，等大神出现。 sgzzour 发表于 2013-4-22 18:01 static/image/common/back.gif
没关系，直接打开就行。
可是.attach的文件下载下来打不开啊 binham 发表于 2013-4-22 18:05 static/image/common/back.gif
可是.attach的文件下载下来打不开啊
你在属性里面改一下设置，选择你所用的pdf打开文件即可。 sgzzour 发表于 2013-4-22 18:07 static/image/common/back.gif
你在属性里面改一下设置，选择你所用的pdf打开文件即可。
好了 多谢~~ 保存的时候直接保存为PDF格式就OK了，等待大神。。。 LZ在哪看的别人的飞控程序啊，表示很难找到完整点的~~ 四元数不是用在陀螺仪中将上一时刻角度加上角度增量得到下一时刻的角度吗？ 加速度计和陀螺仪的融合和四元数有什么关系呢？ mark 不明白 halfT的时间是怎么算的？？需要一个时间间隔吗？？怎么我使用这个函数转化后得出的Pitch和Rool很奇怪~变化很小~ IMUupdate()這個函數只針對加速規及陀螺儀資料做融合，所以真的有修正到的只有pitch(俯仰)和roll(滾轉)，yaw(方位)是沒有修正到的。
要把電子羅盤的數據放進來yaw的輸出才有被修正。
例如Madgwick's IMU and AHRS algorithms.
http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
你也可以直接用
Yaw=atan2f( 2 * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 )*57.3;
計算yaw的方位角，可以發現在沒有電子羅盤輔助下這個角度是會飄的。
>>四元数不是用在陀螺仪中将上一时刻角度加上角度增量得到下一时刻的角度吗？ 加速度计和陀螺仪的融合和四元数有什么关系呢？
概念上是的，加速規是要去除陀螺儀的長期飄移，請見Madgwick的論文，這個很好的方式是用梯度下降法將加速規直接再四元素系統作融合。
>>halfT的时间是怎么算的？？需要一个时间间隔吗？？
前面代碼有寫
#define halfT 0.5f // 采样周期的一半
這裡說採樣週期不太嚴謹，這是是指姿態更新的週期，通常會用高取樣率取樣--->濾波--->低更新週期(ex:50Hz)做更新。
g921002 发表于 2013-5-5 09:32 static/image/common/back.gif
IMUupdate()這個函數只針對加速規及陀螺儀資料做融合，所以真的有修正到的只有pitch(俯仰)和roll(滾轉)，ya ...
您好，我将电子罗盘测量到的数据，进行AHRSupdate的四元数更新。
旋转设备以后，yaw跟着转，但是yaw马上又回到之前那个值，有点像指南针一样....不知道为什么会出这种问题...
初始化四元数的时候，yaw我是令的0，只有roll和pitch角，进行了计算。、、 lologame 发表于 2013-5-5 09:45 static/image/common/back.gif
您好，我将电子罗盘测量到的数据，进行AHRSupdate的四元数更新。
旋转设备以后，yaw跟着转，但是yaw马上 ...
你把电子指南针去掉 只用陀螺就行了 asha 发表于 2013-5-5 10:42 static/image/common/back.gif
你把电子指南针去掉 只用陀螺就行了
我用过IMUupdate,但是无法修正yaw。要修正yaw角的话，不是必须用指南针吗、、？yaw的数据稳定是我最想要的 lologame 发表于 2013-5-5 13:06 static/image/common/back.gif
我用过IMUupdate,但是无法修正yaw。要修正yaw角的话，不是必须用指南针吗、、？yaw的数据稳定是我最想 ...
那应该是你有的地方程序错了 要不应该不会这样的 asha 发表于 2013-5-5 13:13 static/image/common/back.gif
那应该是你有的地方程序错了 要不应该不会这样的
我没有进行yaw的初始化，不知道这样有没有影响.... asha 发表于 2013-5-5 13:13 static/image/common/back.gif
那应该是你有的地方程序错了 要不应该不会这样的
IMUupdate的时候，roll pitch角都可以纠正、
我直接把IMUupdate函数换成AHRSupdate，并添加了HMC5883L测量出来的磁力矢量（读取HMC5883L数据后，将数据进行正负转换）。
不知道这样会有没有什么不妥、 lologame 发表于 2013-5-5 13:51 static/image/common/back.gif
IMUupdate的时候，roll pitch角都可以纠正、
我直接把IMUupdate函数换成AHRSupdate，并添加了HMC5883L测 ...
不初始化不行 ，必须初始化。具体那个程序我没有看过，呵呵。推销下我的开发板嘎嘎 asha 发表于 2013-5-5 14:53 static/image/common/back.gif
不初始化不行 ，必须初始化。具体那个程序我没有看过，呵呵。推销下我的开发板嘎嘎...
呵呵，就是没找到yaw的初始化,哎。。、、 用电子罗盘进行初始化就行 在工业机器人里也有用到这些问题！ lologame 发表于 2013-5-5 09:45 static/image/common/back.gif
您好，我将电子罗盘测量到的数据，进行AHRSupdate的四元数更新。
旋转设备以后，yaw跟着转，但是yaw马上 ...
你有檢查電子羅盤是不是有正確的輸出？還有軸向是不是跟你的body座標定義一樣？
我是用Madgwick 的 AHRS algorithms.沒有這樣的問題，你要不要去試試看？ binham 发表于 2013-4-22 18:05 static/image/common/back.gif
可是.attach的文件下载下来打不开啊
或者直接改后缀 g921002 发表于 2013-5-5 19:14 static/image/common/back.gif
你有檢查電子羅盤是不是有正確的輸出？還有軸向是不是跟你的body座標定義一樣？
我是用Madgwick 的 AHRS...
电子罗盘的坐标输出不知道怎么检测是不是对的，我把读出来的数据，进行了正负转换的。
嗯，能提供一个网址吗？ g921002,真是大神啊，现在刚刚起步，还没有飞起来呢，继续研习算法，工作较忙，盼望您的帮助啊。。。 马克一下，正好用到 没入门，各种不懂 楼主你好，看了你的帖子学习了很多，有个疑问，你程序中的参数gx.gy.gz,ax,ay,az是传感器读出来的值单位化后的结果吗？gxi,gyi,gzi,axi，ayi,azi是怎么得来的呢 没入门，各种不懂,
马克一下 gxi,gyi,gzi,axi,ayi,azi是六轴输出数据，gx.gy.gz,ax,ay,az是经过滤波处理后的数据，最主要是角度转到四元数这一块。不要在其他地方下太多功夫。 hychwlq 发表于 2013-5-22 14:40 static/image/common/back.gif
gxi,gyi,gzi,axi,ayi,azi是六轴输出数据，gx.gy.gz,ax,ay,az是经过滤波处理后的数据，最主要是角度转到四元 ...
您好，四元数获得的矩阵，到底 与 以什么欧拉角旋转的方向余弦矩阵 相对应呢？ js200300953 发表于 2013-4-21 22:45 static/image/common/back.gif
四元数只有一种，欧拉角跟旋转顺序有关的，不同旋转顺序有不同公式。
大侠，具体是什么公式不一样呢？ 最后得到的rool 和pitch 是欧拉角表示吧？ 学习ing~~ 看了几天的代码，还是模模糊糊的{:mad:}{:mad:} g921002 发表于 2013-5-5 19:14 static/image/common/back.gif
你有檢查電子羅盤是不是有正確的輸出？還有軸向是不是跟你的body座標定義一樣？
我是用Madgwick 的 AHRS...
想请教一下,Madgwick 的AHRS函数参数,
AHRSupdate(Gyro_FinalX*0.0174,Gyro_FinalY*0.0174, Gyro_FinalZ*0.0174,-ACC_AVG_Y*0.0098,ACC_AVG_X*0.0098,ACC_AVG_Z*0.0098,magADC,magADC,magADC);
其中Gyro和ACC的参数都经过了预处理,但磁罗盘的参数我不知道该如何预处理,如果直接用椭圆修正之后的罗盘的直接值,得不到正确的解算,请问磁罗盘的数据输入是不是还要有一个系数什么的?? 好好研究一下四元数。 表示正在埋头找资料，有点看不懂。 用过没研究过 {:mad:} 看看迷惑中 牛贴mark 持续关注 谢谢，学习一下 这个关系还是比较重要的 哪里找的？这段代码很多人在用啊好像 binham 发表于 2013-4-22 18:09 static/image/common/back.gif
好了 多谢~~
重命名，把后缀改为PDF就可以用了。 下手了。 同问，yaw自动回漂啊 信誓旦旦的想做四轴，看来还有好多要学啊，
不懂+1 谢谢，学习一下 厉害学习了。。 感谢分享吧。 因为yaw还要根据电子磁罗盘修正吧，所以这个地方没有计算，你可以去其他地方找一下，应该有yaw的计算，而且是单独的计算 楼主能不能发一份完整的程序？谢谢~~{:lol:}770495569@qq.com g921002 发表于 2013-5-5 09:32
IMUupdate()這個函數只針對加速規及陀螺儀資料做融合，所以真的有修正到的只有pitch(俯仰)和roll(滾轉)，ya ...
您好！能不能请教下，对陀螺仪和加速度计数据融合和用这两个的数据来姿态解算有什么区别？我用卡尔曼算法把两者做数据融合得到的数据有什么用呢？？看多了反而有些迷糊，谢谢~ hz770495569 发表于 2014-3-1 16:28
您好！能不能请教下，对陀螺仪和加速度计数据融合和用这两个的数据来姿态解算有什么区别？我用卡尔曼算法 ...
看不懂你的問題。
g921002 发表于 2014-3-2 11:08
看不懂你的問題。
好吧！我也觉得我问的乱七八糟的~那我想问下
// 估计方向的重力
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
这段代码是什么意思？根据什么公式来的吗？ 可惜高等数学没学好 好腻害的赶脚啊！！ 楼主，那个四元数值是怎么来的？公式？q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; // 正常化四元 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; hz770495569 发表于 2014-3-2 14:56
好吧！我也觉得我问的乱七八糟的~那我想问下
// 估计方向的重力
vx = 2*(q1*q3 - q0*q2);
第一个知道是通过旋转矩阵来的，是重力在体坐标下的投影。第二部分的公式我也想问是怎么来的 四元数欧拉角标记 空白L 发表于 2014-4-18 17:49
第一个知道是通过旋转矩阵来的，是重力在体坐标下的投影。第二部分的公式我也想问是怎么来的 ...
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
第一部分公式是把重力向量在导航坐标系的投影（0,0,1）通过四元数姿态矩阵投影到载体坐标系，得到估计值（vx，vy，vz），重力向量在载体坐标系的测量值由加速度计直接输出（ax，ay，az），估计值和测量值做向量叉乘，就得上面的公式，至于为什么要做叉乘，目前正在思考中! 好资料谢谢楼主 这个算法也在研究中，原理上推导确实有一定难度，看起比较涩。 跟着学习一下 mark！ 好文章， 学习中，表示不懂 全角度欧拉角与四元数 你好，我想问一下9150内置DMP出来的四元数可否用来将加速度从机体坐标系转到地理坐标系？ ax=ax*kfa+kfan*axi;
ay=ay*kfa+kfan*ayi;
az=az*kfa+kfan*azi;
gx=gx*kfg+kfgn*gxi;
gy=gy*kfg+kfgn*gyi;
gz=gz*kfg+kfgn*gzi;
这个是互补滤波？ 学习中{:smile:} 请问为什么pitch roll 数据不稳定，会不断漂移？？还有，为什么有时候移动传感器，串口打印突然就停止了？？？请大神答疑~~· mark g921002 发表于 2013-5-5 09:32
IMUupdate()這個函數只針對加速規及陀螺儀資料做融合，所以真的有修正到的只有pitch(俯仰)和roll(滾轉)，ya ...
请问这里周期采样f单位是是什么，是秒还是毫秒？我们一般选取多打f? 好贴，学习！ koko1256 发表于 2014-11-14 13:21
请问这里周期采样f单位是是什么，是秒还是毫秒？我们一般选取多打f?
就是你執行這個IMUupdate()的周期。
比如你每20ms呼叫一次IMUupdate()，那他的周期就是0.02(second) 继续研究 做精品，透彻了解自己的产品 这篇论文有详细讲。。。确实是互补滤波 http://wenku.baidu.com/link?url=IGe6Wor_o5KSfDnsjbH0Uc-gbdunFIeVk6Bbk52HUXlbMKWzcR41Q2UQV78KVqvQGyoXCFPCJAEHqdA_sZscvlNN2zDATXmV8B7q-io05vW 标记一下，谢谢！ 标记下细看 我最近也做了个IMU玩玩，与楼住问题一样，动 pitch时 YAW也跟着动，停止后恢复。
而且把磁航向融合整个姿态收敛很慢。
源码是第七实验室公开（买板子送的）
* main.c file
编写者：lisn3188
编译环境：MDK-LiteVersion: 4.23
初版时间: 2012-04-25
测试： 本程序已在第七实验室的mini IMU上完成测试
功能：
姿态解算 IMU
将传感器的输出值进行姿态解算。得到目标载体的俯仰角和横滚角 和航向角
------------------------------------
*/
#include "IMU.h"
floatpitch ,roll ,yaw;
volatile float exInt, eyInt, ezInt;// 误差积分
volatile float q0, q1, q2, q3; // 全局四元数
volatile float integralFBhand,handdiff;
volatile uint32_t lastUpdate, now; // 采样周期计数 单位 us
/**************************实现函数********************************************
*函数原型: void IMU_init(void)
*功 能: 初始化IMU相关
初始化各个传感器
初始化四元数
将积分清零
更新系统时间
输入参数：无
输出参数：没有
*******************************************************************************/
void IMU_init(void)
{
int i;
for(i=0;i<100;i++)
Dof6_Update();
HMC5983L_SetUp();
now = micros();//读取时间
lastUpdate = now; //更新时间
// initialize quaternion
q0 = 1.0f;//初始化四元数
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
exInt = 0.0;
eyInt = 0.0;
ezInt = 0.0;
}
/**************************实现函数********************************************
*函数原型: void IMU_getValues(float * values)
*功 能: 读取加速度 陀螺仪 磁力计 的当前值
输入参数： 将结果存放的数组首地址
输出参数：没有
*******************************************************************************/
void IMU_getValues(float * values) {
//读取加速度和陀螺仪的当前ADC
Dof6_Update();
values = Accel_Xint;
values = Accel_Yint;
values = Accel_Zint;
values = Gyro_X; // 角速度X轴, 单位dps [度每秒]
values = Gyro_Y;
values = Gyro_Z;
HMC5983_mgetValues(&values); //读取磁力计的ADC值
}
// Fast inverse square-root
/**************************实现函数********************************************
*函数原型: float invSqrt(float x)
*功 能: 快速计算 1/Sqrt(x)
输入参数： 要计算的值
输出参数： 结果
*******************************************************************************/
float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
/**************************实现函数********************************************
*函数原型: void IMU_AHRSupdate
*功 能: 更新AHRS 更新四元数
输入参数： 当前的测量值。
输出参数：没有
*******************************************************************************/
#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.01f // integral gain governs rate of convergence of gyroscope biases
void IMU_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,halfT;
float tempq0,tempq1,tempq2,tempq3;
// 先把这些用得到的值算好
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
now = micros();//读取时间
if(now<lastUpdate){ //定时器溢出过了。
halfT =((float)(now + (0xffff- lastUpdate)) / 2000000.0f);
}
else {
halfT =((float)(now - lastUpdate) / 2000000.0f);
}
lastUpdate = now; //更新时间
norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
//把加计的三维向量转成单位向量。
norm = invSqrt(mx*mx + my*my + mz*mz);
mx = mx * norm;
my = my * norm;
mz = mz * norm;
/*
这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。
所以这里的vx\y\z，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的重力单位向量。
*/
// compute reference direction of flux
hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5f - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5f - q1q1 - q2q2);
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
/*
axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。
axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。
那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。
这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
*/
if(ex != 0.0f && ey != 0.0f && ez != 0.0f){
exInt = exInt + ex * Ki * halfT;
eyInt = eyInt + ey * Ki * halfT;
ezInt = ezInt + ez * Ki * halfT;
// 用叉积误差来做PI修正陀螺零偏
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
}
// 四元数微分方程
tempq0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
tempq1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
tempq2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
tempq3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// 四元数规范化
norm = invSqrt(tempq0*tempq0 + tempq1*tempq1 + tempq2*tempq2 + tempq3*tempq3);
q0 = tempq0 * norm;
q1 = tempq1 * norm;
q2 = tempq2 * norm;
q3 = tempq3 * norm;
}
/**************************实现函数********************************************
*函数原型: void IMU_getQ(float * q)
*功 能: 更新四元数 返回当前的四元数组值
输入参数： 将要存放四元数的数组首地址
输出参数：没有
*******************************************************************************/
float mygetqval; //用于存放传感器转换结果的数组
void IMU_getQ(float * q) {
IMU_getValues(mygetqval);
//将陀螺仪的测量值转成弧度每秒
//加速度和磁力计保持 ADC值 不需要转换
IMU_AHRSupdate(mygetqval * M_PI/180, mygetqval * M_PI/180, mygetqval * M_PI/180,
mygetqval, mygetqval, mygetqval, mygetqval, mygetqval, mygetqval);
q = q0; //返回当前值
q = q1;
q = q2;
q = q3;
}
/**************************实现函数********************************************
*函数原型: void IMU_getYawPitchRoll(float * angles)
*功 能: 更新四元数 返回当前解算后的姿态数据
输入参数： 将要存放姿态角的数组首地址
输出参数：没有
*******************************************************************************/
void IMU_getYawPitchRoll(float * angles) {
float q; // 四元数
volatile float gx=0.0, gy=0.0, gz=0.0; //估计重力方向
IMU_getQ(q); //更新全局四元数
angles = -atan2(2 * q * q + 2 * q * q, -2 * q*q - 2 * q * q + 1)* 180/M_PI; // yaw
angles = -asin(-2 * q * q + 2 * q * q)* 180/M_PI; // pitch
angles = atan2(2 * q * q + 2 * q * q, -2 * q * q - 2 * q * q + 1)* 180/M_PI; // roll
if(angles<0)angles+=360.0f;//将 -+180度转成0-360度
yaw = angles;
pitch = angles;
roll = angles;
}
//------------------End of File----------------------------
s15200380596 发表于 2014-3-8 14:43
楼主，那个四元数值是怎么来的？公式？q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; q1 = q1 + (q0* ...
这个就是四元数的微分方程，用于更新四元数用的！ 这里面的XYZ轴和欧拉角对应关系是啥?以哪个轴为机头方向？ 表示看不懂，还在努力研究中！ 学习、围观了~~
{:smile:} 谢谢分享， 以后会用到， 先学习一下 不错AAAAAAAAAAAA 不知道为什么加表测量重力单位向量，与通过陀螺积分姿态角坐标转换得到的机体坐标系下的重力表示的叉积，就是陀螺积分的姿态与加表测得的角度之间的误差呢？ tangwei039 发表于 2015-7-3 22:11
我最近也做了个IMU玩玩，与楼住问题一样，动 pitch时 YAW也跟着动，停止后恢复。
而且把磁航向融合整个姿 ...
不知道为什么加表测量重力单位向量，与通过陀螺积分姿态角坐标转换得到的机体坐标系下的重力表示的叉积，就是陀螺积分的姿态与加表测得的角度之间的误差呢？
页:
[1]