Python 中对IMU进行积分得到位姿

从数据集中收集到一些IMU传感器输出的测量值和参考位姿，现需要对他们进行积分得到位姿，并与位姿真值进行对比

数据读取

准备的数据以txt格式保存，每行表示一组测量，其存放格式为

# 时间，真实位移x，真实位移x，真实位移x，真实四元数qx，真实四元数qx，真实四元数qx，真实四元数qx，测量加速度x，测量加速度y，测量加速度z，测量角速度x，测量角速度y，测量角速度z
t,x,y,z,qx,qy,qz,qw,accx,accy,accz,gyrox,gyroy,gyroz

数据示例：

0.000000000 -0.982430570 0.462775503 1.440096452 0.017048921 0.017442947 0.999681640 0.006457347 9.128356708 0.106238708 -2.606934458 0.094945911 0.020245819 0.058643063
0.010116100 -0.982516322 0.462718628 1.440084893 0.016769881 0.016830918 0.999696135 0.006567209 9.398039583 1.618097250 -2.705000958 -0.011170107 0.027925268 0.069813170
0.020016193 -0.982455216 0.462720566 1.440119365 0.017149872 0.017305289 0.999682528 0.006422704 8.989429167 -0.130755333 -2.713173167 -0.092153385 0.036302848 0.078888882
0.030037880 -0.982471853 0.462759382 1.440067216 0.016636406 0.016787754 0.999699626 0.006485327 9.299973083 -1.234003458 -2.476179125 -0.005585054 0.009075712 0.113795467
0.039942741 -0.982428366 0.462761779 1.440089186 0.017071389 0.017441812 0.999681696 0.006392007 9.275456458 0.073549875 -2.582417833 0.061435590 0.023736478 0.091455253
0.050014973 -0.982485600 0.462756273 1.440062406 0.016627783 0.016859991 0.999698348 0.006516953 9.471589458 0.400438208 -2.606934458 0.009773844 0.028623400 0.060039326
0.060015678 -0.982469652 0.462743111 1.440121145 0.017145077 0.017273791 0.999682861 0.006468433 9.332661917 0.122583125 -2.705000958 -0.032114058 0.042586034 0.086568331
0.069996595 -0.982430145 0.462763755 1.440059984 0.017014370 0.017147056 0.999688199 0.006324076 9.659550250 -0.375921583 -2.917478375 -0.030717795 0.041887902 0.094945911
0.080028534 -0.982400129 0.462789534 1.440090802 0.017226939 0.017420019 0.999680099 0.006282920 9.291800875 -0.490332500 -2.598762250 -0.000698132 0.024434610 0.077492619
0.090289354 -0.982467627 0.462647409 1.440103708 0.016840361 0.017015106 0.999695012 0.006064004 9.447072833 -0.098066500 -2.655967708 0.032812190 0.008377580 0.075398224

数据读取程序：

raw_data = []
with open("MyData.txt", "r") as f:
    for line in f.readlines():  
        line = line.strip('\n')  # 删除行尾的换行符号
        data = line.split('\n\t') 
        for str in data:
            sub_str = str.split(' ')
        if sub_str:
            raw_data.append(sub_str)

解释：

• .readlines() 方法用于读取所有行(直到结束符 EOF)并返回列表
• .strip(‘x’) 方法 删除s字符串中开头、结尾处的字符x，当rm为空时，默认删除空白符（包括’\n’, ‘\r’, ‘\t’, ’ ')
• .split(‘x’) 方法 拆分字符串。通过指定分隔符’x’对字符串进行切片，并返回分割后的字符串列表

list 转 numpy

由于直接读取的是 list 格式，为方便运算，需要将其转为 numpy 格式

raw_data = np.array(raw_data)
print(type(raw_data))
print(raw_data.shape)

截取参考位姿和IMU测量值

PoseTrue = raw_data[:,0:8]
IMURaw = raw_data[:,8:14]

注意：在使用任何数据集时，都需要重新梳理各个坐标系的关系
以Euroc数据集为例，IMU的X-Y-Z与VICON的X-Y-Z是不一致的。
为了方便对齐坐标系，可以将IMU测量的重力加速度乘以参考姿态角构成的矩阵，然后看与[0,0,g]是否大致相符，如果不相符，则适当调整IMU各轴的顺序

定义积分函数

积分函数的输入为 初始位姿，和 所有的 IMU 测量值

def Integration(PoseTrue, VelInit, IMU):
    print("I am in Integration")
    # Position
    InitPos = PoseTrue[0][1:4] 
    # quaternion
    InitQuat = PoseTrue[0][4:8]
    # quat -> euler
    InitEuler = R.from_quat(InitQuat).as_euler('zyx')
    # attitude angles
    phi = np.zeros((len(IMU),1))
    theta = np.zeros((len(IMU),1))
    psi = np.zeros((len(IMU),1))
    C_ned2b = np.zeros((len(IMU), 3, 3))
    C_b2ned = np.zeros((len(IMU), 3, 3))
    Pos = np.zeros((len(IMU),3))
    Vel = np.zeros((len(IMU),3))
    time_s = PoseTrue[:,0];
    # 
    phi[0] = InitEuler[2] # x-axis
    theta[0] = InitEuler[1] # y-axis
    psi[0] = InitEuler[0] # z-axis
    Pos[0] = InitPos
    Vel[0] = VelInit
    # return value
    Rtn = np.zeros((len(IMU), 6))
    for i in range(len(IMURaw)-1): 
        dt = time_s[i+1] - time_s[i]
        # preparation
        C11 = math.cos(theta[i])*math.cos(psi[i])
        C12 = math.cos(theta[i])*math.sin(psi[i])
        C13 = -math.sin(theta[i])
        C21 = math.sin(phi[i])*math.sin(theta[i])*math.cos(psi[i])-math.cos(phi[i])*math.sin(psi[i])
        C22 = math.sin(phi[i])*math.sin(theta[i])*math.sin(psi[i])+math.cos(phi[i])*math.cos(psi[i])
        C23 = math.sin(phi[i])*math.cos(theta[i])
        C31 = math.cos(phi[i])*math.sin(theta[i])*math.cos(psi[i])+math.sin(phi[i])*math.sin(psi[i])
        C32 = math.cos(phi[i])*math.sin(theta[i])*math.sin(psi[i])-math.sin(phi[i])*math.cos(psi[i])
        C33 = math.cos(phi[i])*math.cos(theta[i])
        C_ned2b[i] = np.matrix([
            [C11, C12, C13],
            [C21, C22, C23],
            [C31, C32, C33]
            ])
        C_b2ned[i] = C_ned2b[i].transpose()
        C_bodyrate2eulerdot = np.matrix([
            [1, math.sin(phi[i])*math.tan(theta[i]), math.cos(phi[i])*math.tan(theta[i])],
            [0, math.cos(phi[i]), -math.sin(phi[i])],
            [0, math.sin(phi[i])/math.cos(theta[i]), math.cos(phi[i])/math.cos(theta[i])]
            ])
        # IMU
        wx = IMU[i][3]
        wy = IMU[i][4]
        wz = IMU[i][5]
        fx = IMU[i][0]
        fy = IMU[i][1]
        fz = IMU[i][2]
        vGyro = np.matrix([
            [wz],
            [-wy],
            [wx]
            ])
        vAcc = np.matrix([
            [fz],
            [-fy],
            [fx]
            ]) 
        vG = np.matrix([
            [0],
            [0],
            [9.82]
            ])
        dot_atti = np.dot(C_bodyrate2eulerdot, vGyro)
        acc_n = np.dot(C_b2ned[i], vAcc).astype(float) - vG.astype(float)
        Pos[i+1] = Pos[i] + Vel[i]*dt + 0.5*acc_n.A.squeeze()*dt*dt;
        Vel[i+1] = Vel[i] + acc_n.A.squeeze()*dt;
        atti_last = np.array([phi[i], theta[i], psi[i]])
        atti_next = atti_last + dot_atti*dt
        phi[i+1] = atti_next[0]
        theta[i+1] = atti_next[1]
        psi[i+1] = atti_next[2]
    
    Rtn[:,0:3] = Pos
    Rtn[:,3] = phi.squeeze()
    Rtn[:,4] = theta.squeeze()
    Rtn[:,5] = psi.squeeze()
    return Rtn

（与matlab相比，python的编程还是比较繁琐，需要时时关注数据的维度）

使用 matplotlib 绘图

使用 pip install matplotlib 安装matplotlib 库，然后引入

import matplotlib
matplotlib.use('Qt5Agg')#必须显式指明matplotlib的后端
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D