标签：acc angle 倾角 MPU6050 角度 竖直 offset 加速度
关于MPU6050计算竖直平面的物体倾角问题的记录
MPU6050的官方DMP是可以测量航向角的，但是它的航向角在没有磁力计的校正下会自增，只能加入一些滤波算法进行一些校正，比如卡尔曼滤波或者互补滤波等，但是这些都不能消除长时间的测量带来的角度自增的问题。而且测量竖直平面的物体倾角都不需要这么复杂的计算。我们只需要拿到加速度的值即可。
当MPU6050（拿的MPU9250做讲解）与水平面垂直放置时，如下图：
此时我们不需要去管Z轴，只需要留意X轴和Y轴的加速度即可。
初始状态是，加速度计读数是A_X =1g; A_Y=0; 如下图：
读出此时X、Y轴的加速度值，需要多测几组，用于去除飘移。比如测量500组，然后X、Y各拿出一个平均值，作为一个基准值。记为offset_x, offset_y。
此时，MPU6050开始基于Z轴进行旋转，与水平面形成一个夹角θ，如下图：
进行一个模拟图，如下：
此时，竖直方向X仍然是1g的加速度，所以将旋转后得到的X1和Y1进行一个矢量的叠加，反正切即可求出旋转角度θ的值，如下：
此时，即可通过反正切公式算出角度：
angle = atan2(ay1,ax1)
此时算出的角度是弧度制，若需转换为角度，则：
angle = atan2(ay1,ax1)*180 / PI
注：PI为圆周率
现在，便可开始计算竖直面物体的倾斜角度了，通过刚开始拿到的offset_x和offset_y的值，计算出静止时物体的最初角度，得到最初角度：
angle_offset = atan2(offset_y,offset_x) x 180 / PI
然后，就是当前倾斜角度的计算了，我们这时已经拿到了静止时的角度angle_offset，然后，开始读取移动后mpu6050的加速度值，为了消除一些外部影响，我们做个简单的滤波处理，定义n个数组，初始化的时候就给这n个数组分别赋与此时XY的加速度值，比如我设n=5，如下：
static int acc_x[5] = {0};
static int acc_y[5] = {0};
short ax,ay,az;
u8 i;
for(i = 0; i < 5; i++){ //如果数组没有拿到5个值，就先拿满五个值 这儿都是初始化时才会直接拿五个
if(acc_x[i] == 0){
MPU_Get_Accelerometer(&ax,&ay,&az);
acc_x[i] = ax;
acc_y[i] = ay;
}
}
n的取值也会影响角度的读取，加大n值能使角度变化更细腻，减小n值会使角度变化反应更迅速，需自己去测试一个中间值。
然后，我们通过计算，拿到此时的角度值angle，注意，现在拿到的这个角度值不是物体当前的倾斜角度，物体当前的倾斜角度需要在进行计算一下。
我们通过读取此时Y轴的加速度，来判断物体往左偏还是右偏，如下：
if(ay >= 0){ //判断角度的正反（即往左还是往右偏），此代码中左偏为负，右偏为正
angle = (angle - fabs(angle_offset)); //此时的值减去初始值，就是当前的倾斜角度
}
现在便能读取到角度，最后再加入防止360°反偏的处理，即可得到竖直平面的任意倾斜角度。
最后附上代码链接：https://download.csdn.net/download/cxieyunsky/12241408
--------------------------------------------------------------------------------------------------------------------------@曼珠沙华
---------------------------------------------------------------------------------------------------------------------------2020/3/11
标签：acc,angle,倾角,MPU6050,角度,竖直,offset,加速度 来源： https://blog.csdn.net/cxieyunsky/article/details/104793012
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。