MPU6050 计算加速度位移值

qq_41155369 2018-01-24 09:37:13
最近了解了一点惯性导航的知识,正好手上有一块MPU6050,图1是读到的数据img=https://img-bbs.csdn.net/upload/201801/24/1516756724_477074.png][/img] MPU 能读到各轴角度值及各轴方向上的加速度值,想法是通过Z-Y-X欧拉角转换,将三个轴上的加速值转换成 大地水平面(XY面)与垂直方向上(Z方向) 三个方向的加速度值,意味着静止状态下,不论MPU6050姿态如何,z轴上的加速度值等于g值,且理论上读到的加速度|a|与转换后的加速度|a|值应该是相等的。R_ab为 Z-Y-X欧拉角变换矩阵,A矩阵为读到的加速值,代码如下,问题是无法得到想要的加速度值。

                    double x_rad = Math.Atan2(System.Math.Sin(x_angle), System.Math.Cos(x_angle));
double y_rad = Math.Atan2(System.Math.Sin(y_angle), System.Math.Cos(y_angle));
double z_rad = Math.Atan2(System.Math.Sin(z_angle), System.Math.Cos(z_angle));
double r11 = Math.Cos(z_rad) * Math.Cos(y_rad);
double r12 = Math.Cos(z_rad) * Math.Sin(y_rad) * Math.Sin(x_rad) -
Math.Sin(z_rad) * Math.Cos(x_rad);
double r13 = Math.Cos(z_rad) * Math.Sin(y_rad) * Math.Cos(x_rad) +
Math.Sin(z_rad) * Math.Sin(x_rad);
double r21 = Math.Sin(z_rad) * Math.Cos(y_rad);
double r22 = Math.Sin(z_rad) * Math.Sin(y_rad) * Math.Sin(x_rad) -
Math.Cos(z_rad) * Math.Cos(x_rad);
double r23 = Math.Sin(z_rad) * Math.Sin(y_rad) * Math.Cos(x_rad) -
Math.Cos(z_rad) * Math.Sin(x_rad);
double r31 = -(Math.Sin(y_rad));
double r32 = Math.Cos(y_rad) * Math.Sin(x_rad);
double r33 = Math.Cos(y_rad) * Math.Cos(x_rad);


Matrix R_ab = new double[,] { { r11, r12, r13 }, { r21, r22, r23 }, { r31, r32, r33 } };

Matrix A = new double[,] { { x_a }, { y_a }, { z_a } };
Matrix R = R_ab.Inverse();
Matrix result = R * A;
...全文
2838 3 打赏 收藏 转发到动态 举报
写回复
用AI写文章
3 条回复
切换为时间正序
请发表友善的回复…
发表回复
leon51771116 2018-12-11
  • 打赏
  • 举报
回复
你的方法没有问题,不过计算算式可能存在错误
我用glm矩阵库进行计算,得出的结果应该是你想要的效果
将单位矩阵按顺序绕z轴、y轴、x轴逆旋转,得出一个新的矩阵
之后新矩阵与加速度数据相乘,即可将加速度数据转到大地坐标系来计算 如图所示:

dengchonglin 2018-03-02
  • 打赏
  • 举报
回复
老总,做得如何了? 交流一下?
qq_41155369 2018-01-24
  • 打赏
  • 举报
回复
右边这组为转换后的加速度值

110,499

社区成员

发帖
与我相关
我的任务
社区描述
.NET技术 C#
社区管理员
  • C#
  • Web++
  • by_封爱
加入社区
  • 近7日
  • 近30日
  • 至今
社区公告

让您成为最强悍的C#开发者

试试用AI创作助手写篇文章吧