MPU6050虚拟现实联动

最近在制作四轴，想从底层完全实现一个飞控锻炼自己。首先从MPU6050模块开始，这个模块买了好几年了之前一直没时间做，现在找出来让他发挥余光余热了。

视频：https://www.bilibili.com/video/av60425189

然后发现他被我焊的零漂特别严重，15度左右，果断买了一块新的。然后，我使用的是树莓派，用的比较熟，看网上也没有用树莓派做的，我想尝试一下。其中比较难的点主要有两点，一个是卡尔曼滤波一个是通讯。原谅我网络通讯的弱项。把RPY角用web.py传给局域网中的笔记本。用opengl做可视化。

首先是I2C数据采集。MPU6050有角度传感器（动态响应好）和加速度传感器（静态计算角度准）所以就引出了姿态角的卡尔曼互补。我进行数据采集使用的是有人写好的MPU6050库，python库，pip下载好直接调用就好。

if __name__ == "__main__":
    sensor = mpu6050(0x68)

    y_zit = 0

    sum = 0
    zy = 1.365


    ang_x,ang_y,ang_z=d_er(sensor)
    #设定好RPY角
    Row = 0
    Pitch = 0
    Yaw = 0
    #初始矩阵
    rpy_ini = np.mat([[1,0 ,0], [0, 1,0],[0,0,1]])


    starttime = time.clock()
    endtime = time.clock()

很简单的MPU6050库，直接获取角速度及加速度信息6个参数。重点是要对角速度进行一个偏移并且进行一个低通滤波。

 

看到网上很多人对于为什么要用四元数来表示姿态变化好像理解的不太好。我这里解释一下。把传感器首先x轴旋转90度，再y轴旋转90度，再x轴旋转-90度，可以看到实际模块是绕z旋转了90度而已。但是矩阵计算时候如果用定系计算公式就会出现错误了，所以要用动系来计算姿态这样才能实际反应四轴的真实状态。

机器人课上讲过的坐标系计算，这个理解的就比较好了。

然后就是卡尔曼滤波了使用加速度计算的姿态角作为观测值。

我在树莓派安装opengl总是失败，最后放弃了直接把数据传到我电脑上用我电脑做可视化吧。

from mpu6050 import mpu6050
from time import sleep
import datetime
import time
import math
import array
import numpy as np


pai =3.1415926

#kalman  states
Xr=np.mat([[0],
         [0]])
Pr=np.mat([[1,0],
         [0,1]])


Xp=np.mat([[0],
         [0]])
Pp=np.mat([[1,0],
         [0,1]])

Q=np.mat([[0.05,0],
         [0,0.05]])

H=np.mat([1,0])
R=np.mat([0.2])



###################


def d_er(sensor):
    ang_x=0
    ang_y = 0
    ang_z=0
    for i in range(1,2000):
        gyro_data = sensor.get_gyro_data()
        ang_x += gyro_data['x']
        ang_y += gyro_data['y']
        ang_z += gyro_data['z']
    ang_x /=2000
    ang_y /= 2000
    ang_z /= 2000
    return ang_x,ang_y,ang_z

def get_cgy(gy_x,gy_y,gy_z,ang_x,ang_y,ang_z):
    #进行低通滤波+剔除零漂
    
    gy_x = gy_x - ang_x
    if((gy_x<0.2)&(gy_x>0)|(gy_x<0)&(gy_x>(-0.2))):
        gy_x=0
    else:
        gy_x = min(gy_x+0.2,gy_x-0.2,gy_x)
    gy_y = gy_y - ang_y
    if ((gy_y < 0.2) & (gy_y > 0) | (gy_y < 0) & (gy_y > (-0.2))):
        gy_y = 0
    else:
        gy_y = min(gy_y+0.2,gy_y-0.2,gy_y)
    gy_z = gy_z - ang_z
    if ((gy_z < 0.2) & (gy_z> 0) | (gy_z < 0) & (gy_z > (-0.2))):
        gy_z = 0
    else:
        gy_z = min(gy_z+0.2,gy_z-0.2,gy_z)

    return gy_x,gy_y,gy_z



def rever_con(rpy_ini):
    rpy_ar=rpy_ini.getA()
    cos_beta=math.sqrt(rpy_ar[0][0]*rpy_ar[0][0]+rpy_ar[1][0]*rpy_ar[1][0])
    real_arf=0
    real_beta=0
    real_gam=0
    if cos_beta!=0 :
        real_beta=math.atan2(-rpy_ar[2][0],cos_beta)
        real_arf=math.atan2(rpy_ar[1][0],rpy_ar[0][0])
        real_gam = math.atan2(rpy_ar[2][1], rpy_ar[2][2])
    else :
        print("match!!!!!!!!!!!!!match!!!!!!!"+"\n")
        real_beta=pai/2
        real_arf=0
        real_gam=math.atan2(rpy_ar[0][1], rpy_ar[1][1])
    return real_gam*180/pai,real_beta*180/pai,real_arf*180/pai

def kalman(acc_r,gyro_w,acc_p,gyro_pw,dt):
    global Xr,Pr ,Xp,Pp,   Q,H,R
    F=np.mat([[1,dt],
             [0,1]])
    DI=np.mat([[1,0],
              [0,1]])
   

    
    
    Xr_=F*Xr
    Xp_=F*Xp
    
    Pr_=F*Pr*F.T+Q
    Pp_=F*Pp*F.T+Q
    
    Kr=Pr_*H.T/(H*Pr*H.T+R)
    Kp=Pp_*H.T/(H*Pp*H.T+R)
    
    Xr=Xr_+Kr*(acc_r-H*Xr_)
    Xp=Xp_+Kp*(acc_p-H*Xp_)
    
    Pr=(DI-Kr*H)*Pr_
    Pp=(DI-Kp*H)*Pp_
    
    kal_r=Xr.getA()
    kal_p=Xp.getA()
    return kal_r[0][0],kal_p[0][0]
def cal_rpy(kal_r,kal_p,cwc_y):
    hr=kal_r/180*pai
    hp=kal_p/180*pai
    hy=cwc_y/180*pai
    
    rz=np.mat([[math.cos(hy),-math.sin(hy),0],
                [math.sin(hy),math.cos(hy),0],
                [0,0,1]])
    
    ry=np.mat([[math.cos(hp),0,math.sin(hp)],
                [0,1,0],
                [-math.sin(hp),0,math.cos(hp)]])
    
    rx=np.mat([[1,0,0],
                [0,math.cos(hr),-math.sin(hr)],
                [0,math.sin(hr),math.cos(hr)]])
    
    DII=np.mat([[1,0,0],
                [0,1,0],
                [0,0,1]])
    DII=DII*rz*ry*rx
    return DII

if __name__ == "__main__":
    filename = "1.txt"
    sensor = mpu6050(0x68)

    y_zit = 0

    sum = 0
    zy = 1.365


    ang_x,ang_y,ang_z=d_er(sensor)
    print(str(ang_x)[0:6]+" "+str(ang_y)[0:6]+" "+str(ang_z)[0:6])
    #设定好RPY角
    Row = 0
    Pitch = 0
    Yaw = 0
    ys_x=0
    ys_y=0
    ys_z=0
    #初始矩阵
    rpy_ini = np.mat([[1,0 ,0], [0, 1,0],[0,0,1]])


    starttime = time.time()
    endtime = time.time()
    # t_ states
    cwc_r_t=0
    cwc_p_t=0
    while True:
        accel_data=sensor.get_accel_data()
        gyro_data=sensor.get_gyro_data()
        temp=sensor.get_temp()

        #开始获取
        gy_x = gyro_data['x']
        gy_y = gyro_data['y']
        gy_z = gyro_data['z']
       
        gy_x,gy_y,gy_z=get_cgy(gy_x,gy_y,gy_z,ang_x,ang_y,ang_z)
        
        
    
        starttime=endtime
        endtime=time.time()
        
        d_time=endtime-starttime
        dt=d_time
        ys_x+=gy_x * d_time
        ys_y+=gy_y * d_time
        ys_z+=gy_z * d_time
        #print(str(ys_x)+" "+str(ys_z)+" "+str(ys_y))
        
        #with open(filename,'a') as file_object:
        #    file_object.write(str(gy_x * d_time)+" "+str(gy_y * d_time)+" "+str(gy_z * d_time)+"\n")

        Gy_x = gy_x * d_time
        Gy_y = gy_y * d_time
        Gy_z = gy_z * d_time
        
        Gy_x=Gy_x*pai/180
        Gy_y=Gy_y*pai/180
        Gy_z=Gy_z*pai/180
        
        
        

        rot_xita=np.mat([[1,-Gy_z ,Gy_y], [Gy_z, 1,-Gy_x],[-Gy_y,Gy_x,1]])

        rpy_ini=rpy_ini*rot_xita
        #print(rpy_ini)
        
        
       
        #反求rpy角   zyx欧拉角
        cwc_r,cwc_p,cwc_y=rever_con(rpy_ini)
        
        
        #print(str(cwc_r)[0:6]+" "+str(cwc_p)[0:6]+" "+str(cwc_y)[0:6])
        
   
        #使用加速度计求py
        accel_data = sensor.get_accel_data()
        ac_x = accel_data['x']
        ac_y = accel_data['y']
        ac_z = accel_data['z']
        
        acc_p = -math.atan2(ac_x, ac_z)
        acc_p=acc_p*180/pai
        
        acc_r = math.atan2(ac_y, ac_z)
        acc_r = acc_r*180/pai
        #print(str(cwc_p)[0:6]+" "+str(acc_r)[0:6]+" "+str(acc_p)[0:6])
        gyro_w=(cwc_r-cwc_r_t)/dt
        gyro_pw=(cwc_p-cwc_p_t)/dt
        kal_r,kal_p=kalman(acc_r,gyro_w,acc_p,gyro_pw,dt)
        
        
        
        rpy_ini=cal_rpy(kal_r,kal_p,cwc_y)
        
        #print(str(cwc_r)[0:6]+" "+str(acc_r)+" "+str(kal_r))
        print(dt)
        cwc_r_t=kal_r
        cwc_p_t=kal_p
        with open(filename,'a') as file_object:
            file_object.write(str(kal_r)+" "+str(kal_p)+" "+str(cwc_y)+"\n")

愉快的写到web里。然后用电脑的opengl展示出来。

视频正在b站审核。我以后放上来。