请问得到四元数后怎么计算YAW，ROLL，PITCH？

最新推荐文章于 2024-06-18 17:42:24 发布

Hustudent20080101

最新推荐文章于 2024-06-18 17:42:24 发布

阅读量8.9k

点赞数 1

分类专栏： AHRS

AHRS 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

请问得到四元数后怎么计算YAW，ROLL，PITCH？

[复制链接]

cw628

24 主题	0 好友	139 积分

注册会员

Rank: 2

莫元

127

发消息

电梯直达

1楼

发表于 2012-7-5 21:32:54 | 只看该作者 | 倒序浏览

我用这个程序得到了四元数，怎么进一步计算姿态YAW，ROLL，PITCH？
//----------------------------------------------------------------------------------------------------
// Header files

#include "AHRS.h"
#include <math.h>

//----------------------------------------------------------------------------------------------------
// 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;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;

// 测量正常化
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;

// 计算参考磁通方向
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;

//估计方向的重力和磁通（V和W）
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);

// 错误是跨产品的总和之间的参考方向的领域和方向测量传感器
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);

// 积分误差比例积分增益
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;
}

//====================================================================================================
// END OF CODE
//====================================================================================================

顶、支持0

踩、反对0

js200300953

7 主题	1 好友	331 积分

中级会员

Rank: 3 Rank: 3

莫元

315

发消息

2楼

发表于 2012-7-6 08:13:22 | 只看该作者

这里有讲

本帖子中包含更多资源

您需要登录才可以下载或查看，没有帐号？注册

js200300953

7 主题	1 好友	331 积分

中级会员

Rank: 3 Rank: 3

莫元

315

发消息

3楼

发表于 2012-7-6 10:59:20 | 只看该作者

这段话很奇怪：

// 正常化四元
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;

复制代码

既然精度都是float，为什么用sqrt？用sqrtf啊。
除法一般比乘法慢很多，应该把除法移到第一句。
单位化向量有“超级快速”的算法（刚刚才发现的），见平方根倒数速算法。

博客地址

cw628

24 主题	0 好友	139 积分

注册会员

Rank: 2

莫元

127

发消息

4楼

发表于 2012-7-6 23:41:10 | 只看该作者

js200300953 发表于 2012-7-6 10:59
这段话很奇怪：
既然精度都是float，为什么用sqrt？用sqrtf啊。
除法一般比乘法慢很多，应该把除法移 ...

四元数和卡尔曼滤波是什么关系？一直没有搞懂～
我用的卡尔曼是这样的：以Y轴为例，先用加速度计计算出倾角（atan2（x，z）），然后用卡尔曼滤波融合陀螺仪得到最终姿态，但是在Y轴指向地面时，X和Z的分量都是0，加速度计计算出倾角在90度附近时出问题，小于30度精度还比较不错，是否利用4元数可以计算任意角度的倾角，但是为何程序的入参里面会有陀螺仪的数据，不是加速度计和磁力计就可以计算出静态角度，然后和陀螺仪积分计算出姿态吗？不解～～

js200300953

7 主题	1 好友	331 积分

中级会员

Rank: 3 Rank: 3

莫元

315

发消息

5楼

发表于 2012-7-7 08:41:03 | 只看该作者

建议你翻一翻旧帖，这些问题都曾经讨论过。

cw628

24 主题	0 好友	139 积分

注册会员

Rank: 2

莫元

127

发消息

6楼

发表于 2012-7-7 21:39:50 | 只看该作者

js200300953 发表于 2012-7-7 08:41
建议你翻一翻旧帖，这些问题都曾经讨论过。

我翻过论坛好多姿态相关的帖子了，没有看到说这个的呢？

fsswl

0 主题	0 好友	21 积分

新手上路

Rank: 1

莫元

发消息

7楼

发表于 2012-7-10 13:11:01 | 只看该作者

cw628 发表于 2012-7-7 21:39
我翻过论坛好多姿态相关的帖子了，没有看到说这个的呢？

以我目前的理解,传感器是理想的话,通过四元数与欧拉角之间的相互变换,可以得到理想的三个姿态角,这是一个解耦过程,然后可以进一步地进行控制,但遗憾的是传感器都不可能是理想的,而卡尔曼滤波算法就是一个让我们找到相对更理想的数据进行处理
不对的地方请批评指正,谢谢

济南电子爱好者

8 主题	0 好友	76 积分

注册会员

Rank: 2

莫元

发消息

8楼

发表于 2012-10-7 11:45:29 | 只看该作者

请教，这个函数输入的参数都指的是什么？还有你的初始欧拉角是用加速度计计算得到的吗？

cw628

24 主题	0 好友	139 积分

注册会员

Rank: 2

莫元

127

发消息

9楼

发表于 2012-11-27 22:36:29 | 只看该作者

本帖最后由 cw628 于 2012-11-27 22:39 编辑

其实我也没有搞明白这段代码的意思，不过看了几篇关于四元数和卡尔曼的资料，我的理解这个函数的输入参数是陀螺、加计和磁计的值，初始角就是直接用加速度计算出来的，输出是四元数，每个陀螺仪的采样周期调用一次函数（是陀螺仪的周期还是加计的周期？？）
短期融合：利用陀螺仪的输出积分进行短期融合。用加速度计和磁计来进行长期修正，长期融合是为了消除陀螺的漂移和误差。在被测物体静态时的姿态结算结果与加计和磁计保持一致，而瞬时的姿态变化和陀螺仪角速度积分一致。这样理解不知正确否，请明白的大大指点一下。

redwolf310

1 主题	0 好友	80 积分

注册会员

Rank: 2

莫元

发消息

10楼

发表于 2012-11-27 23:11:28 | 只看该作者

Mark.

业未央

11 主题	0 好友	85 积分

注册会员

Rank: 2

莫元

发消息

11楼

发表于 2013-3-12 15:18:41 | 只看该作者

你好，四元数更新中函数void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)中的9个参数指的是什么？

jade0606

0 主题	0 好友	41 积分

新手上路

Rank: 1

莫元

发消息

12楼

发表于 2013-3-15 09:58:16 | 只看该作者

本帖最后由 jade0606 于 2013-3-15 10:45 编辑

在C語言中你使用的tan反函數要使用 Atan2(x,y) 這個, 可以避免邊界問題,另外可以研究下codic 算法...

Zoiphy

1 主题	1 好友	101 积分

注册会员

Rank: 2

莫元

发消息

13楼

发表于 2013-5-3 20:44:32 | 只看该作者

学习了!

jmp2002911911

1 主题	1 好友	51 积分

注册会员

Rank: 2

莫元

发消息

14楼

发表于 2013-5-3 23:11:23 来自手机 | 只看该作者

业未央发表于 2013-3-12 15:18
你好，四元数更新中函数void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, fl ...

gx, 陀螺仪x轴角速度，
gy,
gz,
ax, 加速度计x轴加速度
ay,
az,
mx, 磁传感器x轴方位吧（这个不太确定）
my,
mz
其他的就是相应的y,z轴数据

小Oy

12 主题	1 好友	65 积分

注册会员

Rank: 2

莫元

发消息

15楼

发表于 2013-5-12 15:38:21 | 只看该作者

ROLL; PITCH; YAW; 请问这三个变量是什么意思？

小Oy

12 主题	1 好友	65 积分

注册会员

Rank: 2

莫元

发消息

16楼

发表于 2013-5-12 15:45:33 | 只看该作者

ROLL; PITCH; YAW;请问这个是什么意思

开始研究姿态解算了

[复制链接]

js200300953

7 主题	1 好友	331 积分

中级会员

Rank: 3 Rank: 3

莫元

315

发消息

电梯直达

1楼

发表于 2012-6-25 14:56:38 | 只看该作者 | 倒序浏览

本帖最后由 js200300953 于 2012-6-25 15:56 编辑

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

下面总结一下“姿态解算”的过程，分为“传感器”、“四元数与旋转”、“姿态解算框架”、“长期融合”、“快速融合”四部分。

传感器
我用的是10轴姿态传感器模块，其中陀螺仪是L3G4200D，加速度计是ADXL345，罗盘是HMC5883L，气压计是BMP085。全部都通过一条共用的I2C总线访问，速度都支持400kHz。
先讲讲I2C库。要配置、读取传感器，首先把通信做好，这里就是I2C库了。现在大部分单片机都有支持中断的硬件I2C了，可以写个高效的I2C库。我只实现了主机发送和主机接收模式，这里简单介绍一下接口。接口函数主要有2个：
1. uint8_t I2C_transmit (uint8_t which,I2C_transmitCallback cb);
2. void I2C_setNextCallback (uint8_t which,I2C_transmitCallback cb);
复制代码
I2C_transmit()用于触发一次传输（发送或接收），异步执行，调用后马上返回。其中有一个I2C_transmitCallback类型的参数，就是决定发送或接收、如何处理数据的回调函数了，其定义如下：
1. /*
2. * 数据传输回调函数。
3. * 每（准备）传输一个字节都调用一次。
4. * 参数：
5. * seq => 序号，第一次调用时为0，以后每次调用递增。
6. * data => seq==0时写(从机地址+W/R)到data。
7. * seq!=0时data是数据。发送就写data，接收就读data。
8. * 返回值表示下一步的行为：
9. * I2C_RT_START => 发送开始信号。
10. * I2C_RT_STOP => 发送停止信号。
11. * I2C_RT_REPEAT_START_OR_STOP => 如果有下一次传输，就发送RepeatStart，否则发送Stop。
12. * I2C_RT_ACK => 继续传送，回应ACK。
13. * I2C_RT_NACK => 继续传送，回应NACK。 */
14. typedef uint8_t (* I2C_transmitCallback)(uint8_t seq,uint8_t * data);
复制代码
当用I2C_transmit()成功触发一次传输后，I2C库会根据需要调用回调函数，所以使用这个I2C库就是写回调函数了。而I2C_setNextCallback()则是用来设置紧接着的一次传输的。当本次传输结束时，不发送“Stop”信号，而是发送“RepeatStart”信号，然后开始下一个传输。利用这个函数可以发起连续多次传输。
有了I2C库就可以操作传感器了，以L3G4200D为例讲解。先看看DataSheet，操作L3G4200D有两个步骤，首先配置好寄存器，然后不断获取数据。配置寄存器比较简单，就是发送一段数据，用到两个函数：
1. /*
2. * 初始化芯片。
3. * 返回值： {L3G4200D_RT_NORMAL,L3G4200D_RT_BUS_BUSY} */
4. uint8_t l3g4200d_init(void)
5. {
6. if(I2C_transmit(L3G4200D_WHICH_I2C,l3g4200d_init_callback) == I2C_RT_TRANSMIT_NORMAL)
7. return L3G4200D_RT_NORMAL;
8. return L3G4200D_RT_BUS_BUSY;
9. }
11. /*
12. * 初始化使用的I2C回调函数。 */
13. uint8_t l3g4200d_init_callback(uint8_t seq,uint8_t * data)
14. {
15. const static uint8_t value[] = {
16. I2C_addressToByte_write(L3G4200D_ADDRESS), /* 总线地址。 */
17. (0x80 | 0x20), /* 寄存器地址。或0x80表示连续写。*/
18. 0xEF, /* CTRL_REG1 */
19. 0x00, /* CTRL_REG2 */
20. 0x00, /* CTRL_REG3 */
21. 0x00, /* CTRL_REG4，250dps量程。 */
22. /* 其它默认。 */
23. };
24. //
25. if(seq == sizeof(value))
26. return I2C_RT_STOP;
27. //
28. *data = value[seq];
29. return I2C_RT_ACK;
30. }
复制代码
读取数据就有点麻烦，因为读之前要设置寄存器指针，所以其实包含两次传输：第一次是发送，设置当前寄存器指针；第二次是接收，获取测量值。第一次传输结束前要用I2C_setNextCallback()设置第二次传输的回调函数。见代码：
1. /*
2. * 获取角速度。
3. * 异步，读取成功后，l3g4200d_measureCompleted()会被调用。
4. * 返回值： {L3G4200D_RT_NORMAL,L3G4200D_RT_BUS_BUSY} */
5. uint8_t l3g4200d_measureOmega(void)
6. {
7. if(I2C_transmit(L3G4200D_WHICH_I2C,l3g4200d_measureOmega_callback_reg) == I2C_RT_TRANSMIT_NORMAL)
8. return L3G4200D_RT_NORMAL;
9. return L3G4200D_RT_BUS_BUSY;
10. }
12. /*
13. * 读角速度的I2C回调函数，设置当前寄存器地址。 */
14. uint8_t l3g4200d_measureOmega_callback_reg(uint8_t seq,uint8_t * data)
15. {
16. if(seq == 0)
17. {
18. *data = I2C_addressToByte_write(L3G4200D_ADDRESS);
19. return I2C_RT_ACK;
20. }
21. if(seq == 1)
22. {
23. *data = L3G4200D_REG_OMEGA; /* 角速度数据的寄存器地址。 */
24. return I2C_RT_ACK;
25. }
26. //
27. I2C_setNextCallback(L3G4200D_WHICH_I2C,l3g4200d_measureOmega_callback_read);
28. return I2C_RT_REPEAT_START_OR_STOP;
29. }
31. /*
32. * 读角速度的I2C回调函数，读取数据。 */
33. uint8_t l3g4200d_measureOmega_callback_read(uint8_t seq,uint8_t * data)
34. {
35. if(seq == 0)
36. {
37. *data = I2C_addressToByte_read(L3G4200D_ADDRESS);
38. return I2C_RT_ACK;
39. }
40. if(seq > L3G4200D_BUFFER_SIZE)
41. return I2C_RT_STOP;
42. //
43. l3g4200d_var.buffer_8u[seq-1] = *data;
44. if(seq == L3G4200D_BUFFER_SIZE)
45. {
46. l3g4200d_measureCompleted();
47. return I2C_RT_NACK;
48. }
49. return I2C_RT_ACK;
50. }
复制代码
这样就可以读取角速度了。所有数据都是在I2C中断里处理，所以要注意数据安全性，合理利用volatile。
其他传感器的工作方式几乎一样，Crtl+C，Ctrl+V，然后改改名字，改改参数就可以用了。
四元数与旋转
姿态解算的核心在于旋转，一般旋转有4种表示方式：矩阵表示、欧拉角表示、轴角表示和四元数表示。矩阵表示适合变换向量，欧拉角最直观，轴角表示则适合几何推导，而在组合旋转方面，四元数表示最佳。因为姿态解算需要频繁组合旋转和用旋转变换向量，所以采用四元数保存组合姿态、辅以矩阵来变换向量的方案。下面介绍一下四元数，然后给出几种旋转表示的转换。

四元数可以理解为一个实数和一个向量的组合，也可以理解为四维的向量。这里用一个圈表示q是一个四元数，很可能不是规范的表示方式。

四元数的长度（模）与普通向量相似。

下面是对四元数的单位化，单位化的四元数可以表示一个旋转。

四元数相乘，旋转的组合就靠它了。

旋转的“轴角表示”转“四元数表示”。这里创造一个运算q(w,θ)，用于把绕单位向量w转θ角的旋转表示为四元数。

通过q(w,θ)，引伸出一个更方便的运算q(f,t)。有时需要把向量f的方向转到向量t的方向，这个运算就是生成表示对应旋转的四元数的。后面会用到。

然后是“四元数表示”转“矩阵表示”。再次创造运算，用R(p)表示四元数p对应的矩阵（后面用到）。

多个旋转的组合可以用四元数的乘法来实现。

“四元数表示”转“欧拉角表示”。
姿态解算框架
姿态解算框架其实就是程序框架了。设计框架既要准确，又要高效。总体设计是这样的：
- 用一个计时器定时触发测量；
- 所有测量过程都靠中断推进；
- 在main函数里不断检查测量是否完成，完成就进行解算。
测量过程比较耗时间，而这样设计，测量和解算可以同时进行，不会浪费CPU时间在（等待）测量上。而通过计时器触发测量，最大限度保证积分间隔的准确。
长期融合
长期融合的目的有两个：一、得到初始姿态；二、用直接测量的姿态（下称直接姿态），纠正陀螺仪积分得出的姿态。直接测量的量包括加速度和磁场强度。长期融合有两个阶段，第一阶段是获得直接姿态，第二阶段是用直接姿态纠正当前姿态。首先讲第一阶段：用加速度和磁场强度计算直接姿态。
获取直接姿态的过程，其实是利用了空间中的两个场——重力场和地磁场，把测得的加速度和磁场强度旋转到“原来的位置”，其中的“旋转”就是我们需要的直接姿态了。但由于干扰和误差，测得的值不可能旋转到与实际的场一致。
用了净两天的时间来思考，还是想不出高效的办法。下面讲我的笨办法。

首先定义几个量（"w" for world,"m" for measure)：

单位化：

变换到对角线和平面法线。因为不能把加速度和磁场强度旋转到与对应的场一致，于是变换一下，使他们的对角线和平面法线与场对应的量重合，这是可以做到的。（"d"for diagonal，"c" for cross）

然后旋转分两步：首先使对角线重合，得到第一个旋转。

在第二次旋转前，要用第一个旋转变换一下测量量的平面法线。因为经过第一次旋转，测量量的平面法线也跟着转了。

接着就是第二次旋转，使平面法线重合。

组合两次旋转，直接姿态就出来了。第一阶段完成。

第二阶段是纠正当前姿态，这里用的是最简单的——线性插值再单位化。
快速融合
快速融合比长期融合简单多了。

先用测得的角速度和积分间隔构造“微旋转”，这里用了小角三角近似，sin(Δ)≈Δ，cos(Δ)≈1。

然后用四元数乘法组合，就成了。