如何用Python绘制积分漂移与车辆圆周运动真值轨迹对比可视化图

问题说明 #

需要实现的可视化效果中，红色为车辆圆周行驶的真实路径，绿色为积分运算产生的积分漂移误差。目前已完成真值圆周轨迹绘制，使用Python绘制积分漂移曲线时出现异常，尝试用scipy的integrate.cumtrapz方法替换部分乘法运算未得到预期结果。

问题定位 #

代码中仅xA1id和xA2id两个变量的计算逻辑存在问题，其余逻辑正确，原待调整代码如下：

psie = 0 + integrate.cumtrapz(psidote, t);                                #  rad

xA1id = R0 + integrate.cumtrapz(0 + integrate.cumtrapz(xAddot1e, t), t);            #  m
xA2id = 0 + integrate.cumtrapz(R0*omega0 + integrate.cumtrapz(xAddot2e, t), t);     #  m

错误原因 #

1. scipy.integrate.cumtrapz默认返回的数组长度比输入数组少1，没有对齐时间序列长度，也没有匹配初始值为0的设定。
2. 原代码中直接使用t*xAddot1e的计算方式仅适用于匀加速运动，而圆周运动的加速度随时间实时变化，必须通过数值积分逐段累计速度和位移。

修复方案 #

调用cumtrapz时添加initial=0参数，补全数组长度同时设置初始积分值为0，修正后的积分漂移计算部分代码如下：

# 角度积分
psie = integrate.cumtrapz(psidote, t, initial=0)

# 加速度转位移，两次积分
# 计算x方向速度、位移
v_x = integrate.cumtrapz(xAddot1e, t, initial=0)
xA1id = R0 + integrate.cumtrapz(v_x, t, initial=0)

# 计算y方向速度、位移
v_y = R0 * omega0 + integrate.cumtrapz(xAddot2e, t, initial=0)
xA2id = integrate.cumtrapz(v_y, t, initial=0)

完整可运行代码 #

import numpy as np
from scipy import integrate
import matplotlib.pyplot as plt

# 基础参数设置
R0 = 5
omega0 = np.pi

# 绘制真值圆周轨迹
dt_truth = 0.001
N_truth = 1
te = np.arange(0, 2 * N_truth, dt_truth)
psie_truth = omega0 * te
xA1e = R0 * np.cos(psie_truth)
xA2e = R0 * np.sin(psie_truth)

# 积分漂移计算
dt_drift = 0.01
N_drift = 100
t = np.arange(0, 2 * N_drift, dt_drift)

# 无噪声IMU数据
psidote = omega0 * np.ones(len(t))
a1e = -R0 * omega0 ** 2 * np.ones(len(t))
a2e = np.zeros_like(a1e)

psie = integrate.cumtrapz(psidote, t, initial=0)

# 转换到全局坐标系加速度
xAddot1e = np.cos(psie) * a1e - np.sin(psie) * a2e
xAddot2e = np.sin(psie) * a1e + np.cos(psie) * a2e

# 两次积分得到位移
v_x = integrate.cumtrapz(xAddot1e, t, initial=0)
xA1id = R0 + integrate.cumtrapz(v_x, t, initial=0)

v_y = R0 * omega0 + integrate.cumtrapz(xAddot2e, t, initial=0)
xA2id = integrate.cumtrapz(v_y, t, initial=0)

# 合并绘图
plt.figure(figsize=(8,8))
plt.plot(xA1e, xA2e, color='red', label='真值轨迹')
plt.plot(xA1id, xA2id, color='green', label='积分漂移轨迹')
plt.axis('equal')
plt.legend()
plt.show()

内容的提问来源于stack exchange，提问作者Ilan Aizelman WS