其他分享
首页 > 其他分享> > MPU6050计算竖直平面的物体倾角

MPU6050计算竖直平面的物体倾角

作者:互联网

关于MPU6050计算竖直平面的物体倾角问题的记录

MPU6050的官方DMP是可以测量航向角的,但是它的航向角在没有磁力计的校正下会自增,只能加入一些滤波算法进行一些校正,比如卡尔曼滤波或者互补滤波等,但是这些都不能消除长时间的测量带来的角度自增的问题。而且测量竖直平面的物体倾角都不需要这么复杂的计算。我们只需要拿到加速度的值即可。
当MPU6050(拿的MPU9250做讲解)与水平面垂直放置时,如下图:
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