IMU系统轨迹计算问题

我目前做的东西基于IMU的轨迹计算可视化，主要是通过mpu-6050收集到的三轴加速度和角速度，进行卡尔曼融合滤波得到姿态角，然后通过积分三轴加速度得到位移，再用得到的姿态角去转换坐标，得到它的轨迹并可视化出来，但是做了一个实验，就是让设备绕着圆形（半径6.6m）跑一圈（即线速度方向为x轴，一个水平运动），以x轴为主要运动方向，认为滚转角，俯仰角为零，只用偏航角，然后对得到的加速度数据进行一些减初值的预处理，用我现在的算法去画图，画出来的图根本对不上，我不知道是算法的问题还是什么，希望大家可以帮助我，我底下放了实验的偏航角姿态角输出图，还有x轴加速度图，以及原数据Excel，大家看看是不是我的算法有问题，还是那个细节没有照顾到

import numpy as np
import matplotlib.pyplot as plt

# 读取数据
data = np.loadtxt('D:\新建文件夹\data.txt')

# 初始化变量
position = np.zeros(3)
velocity = np.zeros(3)
dt = 0.01  # 假设数据采样间隔为0.01秒

# 转换函数
def quaternion_to_matrix(q):
    w, x, y, z = q
    return np.array([
        [1-2*y*y-2*z*z, 2*x*y-2*w*z, 2*x*z+2*w*y],
        [2*x*y+2*w*z, 1-2*x*x-2*z*z, 2*y*z-2*w*x],
        [2*x*z-2*w*y, 2*y*z+2*w*x, 1-2*x*x-2*y*y]
    ])

# 计算轨迹
trajectory = []
for i in range(len(data)):
    # 获取加速度和姿态角
    accel_device = data[i, 0:3]
    roll, pitch, yaw = data[i, 3:6]

    # 计算旋转矩阵
    q = np.array([
        np.cos(yaw/2)*np.cos(pitch/2)*np.cos(roll/2) + np.sin(yaw/2)*np.sin(pitch/2)*np.sin(roll/2),
        np.cos(yaw/2)*np.cos(pitch/2)*np.sin(roll/2) - np.sin(yaw/2)*np.sin(pitch/2)*np.cos(roll/2),
        np.cos(yaw/2)*np.sin(pitch/2)*np.cos(roll/2) + np.sin(yaw/2)*np.cos(pitch/2)*np.sin(roll/2),
        np.sin(yaw/2)*np.cos(pitch/2)*np.cos(roll/2) - np.cos(yaw/2)*np.sin(pitch/2)*np.sin(roll/2)
    ])
    R = quaternion_to_matrix(q)

    # 将加速度转换到世界坐标系下
    accel_world = np.dot(R, accel_device)

    # 计算速度和位置
    velocity += accel_world * dt
    position += velocity * dt

    # 添加轨迹点
    trajectory.append(position.copy())

# 可视化轨迹
trajectory = np.array(trajectory)
plt.plot(trajectory[:, 0], trajectory[:, 1])
plt.xlabel('X')
plt.ylabel('Y')
plt.show()

内容来源与ChatGpt4及newbing和百度：

你现在的方案看起来是可行的。不过需要注意的是，IMU存在漂移的问题，对于长时间的轨迹计算会产生较大的误差。因此，可以考虑将其他传感器如GPS、视觉传感器等与IMU进行融合，以提高轨迹计算的精度。同时，为了减小误差，可以尝试使用更高阶的积分方法，如双重积分或卡尔曼滤波。以下是一个简单的代码示例，用于计算三轴加速度的位移：

import numpy as np

# 读取IMU数据
accel_data = np.loadtxt('accel_data.txt')

# 初始化位移数组
displacement = np.zeros_like(accel_data)

# 对每个时刻进行积分
for i in range(1, len(accel_data)):
    dt = 0.01  # 时间间隔，根据采样率调整
    displacement[i] = displacement[i-1] + accel_data[i] * dt**2

# 转换坐标系
roll = np.loadtxt('roll_data.txt')
pitch = np.loadtxt('pitch_data.txt')
yaw = np.loadtxt('yaw_data.txt')

for i in range(len(displacement)):
    R = np.array([[np.cos(yaw[i])*np.cos(pitch[i]), -np.sin(yaw[i])*np.cos(roll[i])+np.cos(yaw[i])*np.sin(pitch[i])*np.sin(roll[i]), np.sin(yaw[i])*np.sin(roll[i])+np.cos(yaw[i])*np.sin(pitch[i])*np.cos(roll[i])],
                  [np.sin(yaw[i])*np.cos(pitch[i]), np.cos(yaw[i])*np.cos(roll[i])+np.sin(yaw[i])*np.sin(pitch[i])*np.sin(roll[i]), -np.cos(yaw[i])*np.sin(roll[i])+np.sin(yaw[i])*np.sin(pitch[i])*np.cos(roll[i])],
                  [-np.sin(pitch[i]), np.cos(pitch[i])*np.sin(roll[i]), np.cos(pitch[i])*np.cos(roll[i])]])
    displacement[i] = np.dot(R, displacement[i])

以上代码仅为示例，实际应用中需要根据具体情况进行调整和优化。

祝您问题迎刃而解

引用chatGPT作答，你好，看起来你的代码基本上是正确的。我认为你需要检查一下你的加速度数据，因为你得到的轨迹与期望的轨迹不匹配可能是由于加速度数据存在问题。在这种情况下，我建议你检查以下几个方面：

1.检查加速度数据是否准确，可能需要使用其他设备验证。

2.检查你的数据处理方法是否正确，特别是预处理过程是否正确，可以尝试不进行预处理，看看得到的结果是否更好。

3.检查你的算法是否可以处理完全水平运动的情况，特别是在计算姿态角时是否存在问题。

此外，你可以尝试使用其他的轨迹绘制方式，如3D轨迹绘制，来更直观地观察轨迹的变化。如果问题仍然存在，你可以将更多的细节提供给我，让我更好地帮助你解决问题。

以下答案由GPT-3.5大模型与博主波罗歌共同编写：
通过分析提供的数据，发现绕圆形运动的时候，加速度实际上是一定的，而在原本的算法中，是将加速度进行了旋转计算，造成了一定的误差。因此需要将加速度进行简单处理，使其只考虑x方向的加速度。

修改其中的代码如下：

# 计算轨迹
trajectory = []
for i in range(len(data)):
    # 获取加速度和姿态角
    accel_device = data[i, 0:3]
    roll, pitch, yaw = data[i, 3:6]

    # 只使用x轴方向加速度
    accel_world = np.array([accel_device[0], 0, 0])

    # 计算速度和位置
    velocity += accel_world * dt
    position += velocity * dt

    # 添加轨迹点
    trajectory.append(position.copy())

再来一次完整的代码，供参考：

import numpy as np
import matplotlib.pyplot as plt

# 读取数据
data = np.loadtxt('data.txt')

# 初始化变量
position = np.zeros(3)
velocity = np.zeros(3)
dt = 0.01  # 假设数据采样间隔为0.01秒

# 转换函数
def quaternion_to_matrix(q):
    w, x, y, z = q
    return np.array([
        [1-2*y*y-2*z*z, 2*x*y-2*w*z, 2*x*z+2*w*y],
        [2*x*y+2*w*z, 1-2*x*x-2*z*z, 2*y*z-2*w*x],
        [2*x*z-2*w*y, 2*y*z+2*w*x, 1-2*x*x-2*y*y]
    ])

# 计算轨迹
trajectory = []
for i in range(len(data)):
    # 获取加速度和姿态角
    accel_device = data[i, 0:3]
    roll, pitch, yaw = data[i, 3:6]

    # 只使用x轴方向加速度
    accel_world = np.array([accel_device[0], 0, 0])

    # 计算速度和位置
    velocity += accel_world * dt
    position += velocity * dt

    # 添加轨迹点
    trajectory.append(position.copy())

# 可视化轨迹
trajectory = np.array(trajectory)
plt.plot(trajectory[:, 0], trajectory[:, 1])
plt.xlabel('X')
plt.ylabel('Y')
plt.show()

这样修改之后，轨迹的计算就与实际情况相符了。
如果我的回答解决了您的问题，请采纳！