采用mpu6050的dmp得到四元数如何转换成与各轴的夹角

yzxwy · 发表于 2020-5-11 11:56:51

我想用mpu6050测得物体在各个方向的倾斜角度，现在用其dmp得到了四元数如何将其转换成与各轴的夹角呢？欧拉角存在奇异点的问题，无法实现我想要的功能，请问有什么算法能得到全角度范围的角度变化吗？这个问题困扰我很久了。

fnems · 发表于 2020-5-11 11:56:52

yzxwy 发表于 2020-5-12 17:52
如果采用卡尔曼滤波得到的角度也是欧拉角吗？

我上面发的那个github页面，里面代码包含了两个数据融合算法，分别是mahony算法和madwick算法。
你提到的卡尔曼滤波器，可以参考这里：
https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/estimator_kalman.c

另外再感叹一句，github是宝库啊

正点原子 · 发表于 2020-5-12 01:38:09

帮顶

yzxwy · 发表于 2020-5-12 08:31:18

正点原子发表于 2020-5-12 01:38
帮顶

谢谢！！

fnems · 发表于 2020-5-12 09:44:05

看crazyflies开源项目的代码，https://github.com/bitcraze/crazyflie-firmware/blob/2014.01/modules/src/sensfusion6.c，232行sensfusion6GetEulerRPY这个函数

yzxwy · 发表于 2020-5-12 11:02:04

fnems 发表于 2020-5-12 09:44
看crazyflies开源项目的代码，https://github.com/bitcraze/crazyflie-firmware/blob/2014.01/modules/src/ ...

谢谢，可是这个还是会存在在pitch的范围只能从+90度到-90度的问题，过了90度或-90就会使另外两个角度发生跳变

fnems · 发表于 2020-5-12 12:20:42

yzxwy 发表于 2020-5-12 11:02
谢谢，可是这个还是会存在在pitch的范围只能从+90度到-90度的问题，过了90度或-90就会使另外两个角度发生 ...

我觉得你应该举例说明示例中的函数与你期望的差异。比如给出一组四元数输入数据、示例函数的输出欧拉角、你的期望欧拉角。
文字描述并没有说明白问题出在哪里，以及出问题时是一种怎样的运动。

yzxwy · 发表于 2020-5-12 13:40:23

fnems 发表于 2020-5-12 12:20
我觉得你应该举例说明示例中的函数与你期望的差异。比如给出一组四元数输入数据、示例函数的输出欧拉角、 ...

[13:35:54.682]收←◆pitch = 0.739049,roll = -3.914774,yaw = -6.070997

[13:35:58.402]收←◆pitch = 0.938105,roll = -3.822975,yaw = -8.191296

[13:36:01.968]收←◆pitch = 2.812445,roll = -1.866396,yaw = 13.021045

[13:36:05.534]收←◆pitch = -26.242626,roll = 1.211309,yaw = 14.691387

[13:36:09.100]收←◆pitch = -43.075256,roll = 2.012465,yaw = 15.583175

[13:36:12.665]收←◆pitch = -63.319939,roll = 3.373324,yaw = 15.117490

[13:36:16.231]收←◆pitch = -79.101433,roll = 7.243810,yaw = 16.907490

[13:36:19.799]收←◆pitch = -81.680672,roll = -176.689835,yaw = -167.098679

[13:36:23.374]收←◆pitch = -71.674988,roll = -177.958038,yaw = -169.969925

[13:36:26.940]收←◆pitch = -60.323986,roll = 177.023849,yaw = -175.846970

[13:36:30.506]收←◆pitch = -70.925293,roll = 173.158401,yaw = -178.369720

[13:36:34.072]收←◆pitch = -81.549049,roll = -177.909393,yaw = -168.205414

[13:36:37.639]收←◆pitch = -86.819122,roll = -175.093994,yaw = -164.635468

[13:36:41.204]收←◆pitch = -89.392052,roll = -152.423187,yaw = -141.045242

[13:36:44.771]收←◆pitch = -87.745148,roll = -8.934621,yaw = 3.735306

就是板子水平放置，将其慢慢抬起，（绕y轴转动）pitch角度慢慢变大，当接近-90度的时候roll变成了-100多度，继续使角度增大，roll也是保存-100多度。我希望的是roll值保持不变，一直保持在0左右。

yzxwy · 发表于 2020-5-12 13:45:44

fnems 发表于 2020-5-12 12:20
我觉得你应该举例说明示例中的函数与你期望的差异。比如给出一组四元数输入数据、示例函数的输出欧拉角、 ...

就是希望在全角度范围内能够正确的显示角度的变化，现在的情况就是只有在pitch的范围在90度和-90度之间才正常（向上抬起90度或向下90度之后的角度变化就不正常了）

fnems · 发表于 2020-5-12 15:07:07

本帖最后由 fnems 于 2020-5-12 15:25 编辑

yzxwy 发表于 2020-5-12 13:45
就是希望在全角度范围内能够正确的显示角度的变化，现在的情况就是只有在pitch的范围在90度和-90度之间才 ...

大概看明白了。但是欧拉角是个状态量，只跟当前的姿态有关系，而跟怎么到达这个姿态没关系。
在你描述的运动中，是持续沿Y轴旋转超过了90度，导致imu姿态到达类似后仰的状态。这这种“背朝下、肚子朝上”的姿态就应该是对应roll转过180度的状态。

我明白你的需求中需要区分如何到达这种姿态：是pitch上仰造成，还是roll旋转造成。但是这种姿态描述体系不是标准的欧拉角姿态描述体系。

能不能把你的最终需求描述一下呢，为什么需要区分这两种姿态？

或许有其他技术手段可以绕过欧拉角的描述方式来满足你的要求，也或许欧拉角的姿态描述体系不适合你的应用场景。

fnems · 发表于 2020-5-12 15:20:34

本帖最后由 fnems 于 2020-5-12 15:23 编辑

fnems 发表于 2020-5-12 15:07
大概看明白了。但是欧拉角是个状态量，只跟当前的姿态有关系，而跟怎么到达这个姿态没关系。
在你描述的 ...

不知道我说的“状态量与过程无关”有没有说明白。举个不太恰当的例子，某人开着火箭从上海出发一直向东飞行，绕地球将近一圈后最后到达新疆上空。
你希望强调的是时区位置增加了22个时区（过程），但是从状态上看，终点与起点相比时区ID减少了2，到达东6区。

yzxwy · 发表于 2020-5-12 15:36:56

本帖最后由 yzxwy 于 2020-5-12 15:47 编辑

fnems 发表于 2020-5-12 15:07
大概看明白了。但是欧拉角是个状态量，只跟当前的姿态有关系，而跟怎么到达这个姿态没关系。
在你描述的 ...

就是需要用mpu6050检测我设备在任意方向是否发生了超过倾斜阈值的倾斜，而且设备初始位置是不一定的（重力加速度可能在z，y，x轴任意一个轴的方向上），并且也要能显示目前的位置信息。

yzxwy · 发表于 2020-5-12 15:45:55

fnems 发表于 2020-5-12 15:20
不知道我说的“状态量与过程无关”有没有说明白。举个不太恰当的例子，某人开着火箭从上海出发一直向东飞 ...

嗯，明白了，谢谢解答。那我这种情况是不是不适合用这种方法？

fnems · 发表于 2020-5-12 16:08:02

yzxwy 发表于 2020-5-12 15:36
就是需要用mpu6050检测我设备在任意方向是否发生了超过倾斜阈值的倾斜，而且设备初始位置是不一定的（重 ...

这个设备是具有两个旋转自由度吗，是铰链约束还是球铰链约束的？
如果不考虑运动学配置的话，就要看阈值大小吧，比如倾斜角小于70、80度就可以用pitch欧拉角，如果可能接近或者超过90度就需要额外再参考roll角（roll超过正负90表示开始肚子朝天了）

yzxwy · 发表于 2020-5-12 16:31:07

fnems 发表于 2020-5-12 16:08
这个设备是具有两个旋转自由度吗，是铰链约束还是球铰链约束的？
如果不考虑运动学配置的话，就要看阈值 ...

主要就是考虑roll和pitch的变化，检测任意一个是否超过阈值，阈值一般不超过90度。现在是不满足初始时重力加速度在x轴方向时的检测（pitch为90度左右，roll也为90度左右），这时候无法准确判断角度变化。往任意方向旋转的角度都没有规律

[16:29:34.328]收←◆pitch = 72.828056,roll = -175.669739,yaw = -160.105637

[16:29:38.045]收←◆pitch = 72.319527,roll = -175.915024,yaw = -154.562164

[16:29:41.612]收←◆pitch = 74.348061,roll = -178.163010,yaw = -156.780762

[16:29:45.177]收←◆pitch = 67.396568,roll = 130.328018,yaw = 147.738770

[16:29:48.744]收←◆pitch = 53.414188,roll = 107.248131,yaw = 124.743080

[16:29:52.319]收←◆pitch = 50.197933,roll = 105.644760,yaw = 126.130074

[16:29:55.885]收←◆pitch = 72.837318,roll = 139.874756,yaw = 159.643341

[16:29:59.451]收←◆pitch = 78.661575,roll = -176.438858,yaw = -155.309998

[16:30:03.017]收←◆pitch = 61.066078,roll = -134.380875,yaw = -110.248596

[16:30:06.584]收←◆pitch = 65.881920,roll = -136.951492,yaw = -114.579674

[16:30:10.149]收←◆pitch = 84.309952,roll = -9.423935,yaw = 8.599518

[16:30:13.716]收←◆pitch = 61.734283,roll = 178.590103,yaw = -165.499359

[16:30:17.281]收←◆pitch = 55.792679,roll = 175.751083,yaw = -166.701263