差速驱动运动学仿真¶

In [1]:

import numpy as np
import matplotlib.pyplot as plt
import json
from pathlib import Path
import run_sim

# 设置交互式图表（可缩放、平移）
%matplotlib widget
plt.rcParams['figure.figsize'] = [8, 12]
plt.rcParams['figure.dpi'] = 100

import numpy as np import matplotlib.pyplot as plt import json from pathlib import Path import run_sim # 设置交互式图表（可缩放、平移） %matplotlib widget plt.rcParams['figure.figsize'] = [8, 12] plt.rcParams['figure.dpi'] = 100

2. 理论轨迹计算（矩阵形式）¶

In [2]:

def compute_theory_trajectory(v_seq, omega_seq, dt, x0=0, y0=0, theta0=0):
    """
    根据输入序列计算理论轨迹（中点欧拉法）
    坐标系: X 右, Y 前, Z 上
    
    方法二：中点欧拉
    dx = v * dt * cos(theta + dtheta/2)
    dy = v * dt * sin(theta + dtheta/2)
    dtheta = omega * dt
    """
    n = len(v_seq)
    timestamps = np.arange(n) * dt
    
    x = np.zeros(n)
    y = np.zeros(n)
    theta = np.zeros(n)
    
    x[0], y[0], theta[0] = x0, y0, theta0
    
    for i in range(1, n):
        v = v_seq[i-1]
        omega = omega_seq[i-1]
        dtheta = omega * dt
        # 中点欧拉
        x[i] = x[i-1] + v * dt * np.cos(theta[i-1] + dtheta / 2)
        y[i] = y[i-1] + v * dt * np.sin(theta[i-1] + dtheta / 2)
        theta[i] = theta[i-1] + dtheta
    
    return timestamps, x, y, theta

def compute_theory_trajectory(v_seq, omega_seq, dt, x0=0, y0=0, theta0=0): """ 根据输入序列计算理论轨迹（中点欧拉法） 坐标系: X 右, Y 前, Z 上 方法二：中点欧拉 dx = v * dt * cos(theta + dtheta/2) dy = v * dt * sin(theta + dtheta/2) dtheta = omega * dt """ n = len(v_seq) timestamps = np.arange(n) * dt x = np.zeros(n) y = np.zeros(n) theta = np.zeros(n) x[0], y[0], theta[0] = x0, y0, theta0 for i in range(1, n): v = v_seq[i-1] omega = omega_seq[i-1] dtheta = omega * dt # 中点欧拉 x[i] = x[i-1] + v * dt * np.cos(theta[i-1] + dtheta / 2) y[i] = y[i-1] + v * dt * np.sin(theta[i-1] + dtheta / 2) theta[i] = theta[i-1] + dtheta return timestamps, x, y, theta

In [3]:

# 生成测试输入: 圆周运动
v, omega = 0.2, 0.2
duration = 4.0
dt = 0.01

n = int(duration / dt)
v_seq = np.full(n, v)
omega_seq = np.full(n, omega)

# 生成测试输入: 圆周运动 v, omega = 0.2, 0.2 duration = 4.0 dt = 0.01 n = int(duration / dt) v_seq = np.full(n, v) omega_seq = np.full(n, omega)

In [4]:

# 计算理论轨迹
t_theory, x_theory, y_theory, theta_theory = compute_theory_trajectory(v_seq, omega_seq, dt)
print(f"理论轨迹: {len(t_theory)} 点, X=[{x_theory.min():.2f}, {x_theory.max():.2f}], Y=[{y_theory.min():.2f}, {y_theory.max():.2f}]")

# 计算理论轨迹 t_theory, x_theory, y_theory, theta_theory = compute_theory_trajectory(v_seq, omega_seq, dt) print(f"理论轨迹: {len(t_theory)} 点, X=[{x_theory.min():.2f}, {x_theory.max():.2f}], Y=[{y_theory.min():.2f}, {y_theory.max():.2f}]")

理论轨迹: 400 点, X=[0.00, 0.72], Y=[0.00, 0.30]

3. MuJoCo 仿真轨迹¶

In [5]:

# 运行 mujoco 仿真
v_omega_seq = list(zip(v_seq, omega_seq))

traj_mujoco = run_sim.run_simulation(v_omega_seq, dt=dt, total_time=duration)
print(f"MuJoCo 轨迹: {len(traj_mujoco['timestamps'])} 点")

# 运行 mujoco 仿真 v_omega_seq = list(zip(v_seq, omega_seq)) traj_mujoco = run_sim.run_simulation(v_omega_seq, dt=dt, total_time=duration) print(f"MuJoCo 轨迹: {len(traj_mujoco['timestamps'])} 点")

车辆参数: 轮距 L=0.400m, 轮子半径 r=0.080m
MuJoCo 轨迹: 400 点

4. 轨迹对比与误差分析¶

In [6]:

# 提取数据
t_mujoco = np.array(traj_mujoco['timestamps'])
x_mujoco = np.array(traj_mujoco['x'])
y_mujoco = np.array(traj_mujoco['y'])
theta_mujoco = np.array(traj_mujoco['theta'])

# 时间轴已一致（dt=0.01s），直接取对应点
# t_mujoco 和 t_theory 对齐，无需插值

# 理论轨迹对应时间点
x_theory_aligned = x_theory[:len(t_mujoco)]
y_theory_aligned = y_theory[:len(t_mujoco)]
theta_theory_aligned = theta_theory[:len(t_mujoco)]

# 计算增量（位移增量）
dx_mujoco = np.diff(x_mujoco)
dy_mujoco = np.diff(y_mujoco)
dtheta_mujoco = np.diff(theta_mujoco)

dx_theory = np.diff(x_theory_aligned)
dy_theory = np.diff(y_theory_aligned)
dtheta_theory = np.diff(theta_theory_aligned)

# 计算增量误差 (delta - 增量对比)
delta_dx = dx_mujoco - dx_theory
delta_dy = dy_mujoco - dy_theory
delta_dtheta = dtheta_mujoco - dtheta_theory
delta_norm = np.sqrt(delta_dx**2 + delta_dy**2)

print(f"增量误差统计:")
print(f"  Δdx: mean={delta_dx.mean():.6f}, std={delta_dx.std():.6f}")
print(f"  Δdy: mean={delta_dy.mean():.6f}, std={delta_dy.std():.6f}")
print(f"  Δdθ: mean={delta_dtheta.mean():.6f}, std={delta_dtheta.std():.6f}")
print(f"  ||Δd||: mean={delta_norm.mean():.6f}, max={delta_norm.max():.6f}")

# 提取数据 t_mujoco = np.array(traj_mujoco['timestamps']) x_mujoco = np.array(traj_mujoco['x']) y_mujoco = np.array(traj_mujoco['y']) theta_mujoco = np.array(traj_mujoco['theta']) # 时间轴已一致（dt=0.01s），直接取对应点 # t_mujoco 和 t_theory 对齐，无需插值 # 理论轨迹对应时间点 x_theory_aligned = x_theory[:len(t_mujoco)] y_theory_aligned = y_theory[:len(t_mujoco)] theta_theory_aligned = theta_theory[:len(t_mujoco)] # 计算增量（位移增量） dx_mujoco = np.diff(x_mujoco) dy_mujoco = np.diff(y_mujoco) dtheta_mujoco = np.diff(theta_mujoco) dx_theory = np.diff(x_theory_aligned) dy_theory = np.diff(y_theory_aligned) dtheta_theory = np.diff(theta_theory_aligned) # 计算增量误差 (delta - 增量对比) delta_dx = dx_mujoco - dx_theory delta_dy = dy_mujoco - dy_theory delta_dtheta = dtheta_mujoco - dtheta_theory delta_norm = np.sqrt(delta_dx**2 + delta_dy**2) print(f"增量误差统计:") print(f" Δdx: mean={delta_dx.mean():.6f}, std={delta_dx.std():.6f}") print(f" Δdy: mean={delta_dy.mean():.6f}, std={delta_dy.std():.6f}") print(f" Δdθ: mean={delta_dtheta.mean():.6f}, std={delta_dtheta.std():.6f}") print(f" ||Δd||: mean={delta_norm.mean():.6f}, max={delta_norm.max():.6f}")

增量误差统计:
  Δdx: mean=0.000042, std=0.000116
  Δdy: mean=-0.000109, std=0.000047
  Δdθ: mean=-0.000285, std=0.000199
  ||Δd||: mean=0.000137, max=0.001061

In [7]:

# 可视化
fig, axes = plt.subplots(4, 1, figsize=(8, 12))

# 轨迹对比
axes[0].plot(x_theory, y_theory, 'b-', lw=2, label='Theory')
axes[0].plot(x_mujoco, y_mujoco, 'r--', lw=2, label='MuJoCo')
axes[0].set_xlabel('X (m) - Right')
axes[0].set_ylabel('Y (m) - Forward')
axes[0].set_title('Trajectory Comparison')
axes[0].legend()
axes[0].axis('equal')
axes[0].grid(True, alpha=0.3)

# 增量误差分量
t_delta = t_mujoco[1:]  # 增量对应的时间
axes[1].plot(t_delta, delta_dx, 'b-', label='Δdx')
axes[1].plot(t_delta, delta_dy, 'g-', label='Δdy')
axes[1].set_xlabel('Time (s)')
axes[1].set_ylabel('Delta (m)')
axes[1].set_title('Incremental Error (dx, dy)')
axes[1].legend()
axes[1].grid(True, alpha=0.3)

# 增量误差范数
axes[2].plot(t_delta, delta_norm, 'r-', lw=1.5)
axes[2].set_xlabel('Time (s)')
axes[2].set_ylabel('||Δd|| (m)')
axes[2].set_title('Incremental Euclidean Error')
axes[2].grid(True, alpha=0.3)

# 速度曲线 (setpoint in label, actual in plot)
v_actual = np.array(traj_mujoco['v'])
omega_actual = np.array(traj_mujoco['omega'])
axes[3].plot(t_mujoco, v_actual, 'b-', lw=1.5, label=f'v (set={v:.1f} m/s)')
axes[3].plot(t_mujoco, omega_actual, 'g-', lw=1.5, label=f'omega (set={omega:.1f} rad/s)')
axes[3].set_xlabel('Time (s)')
axes[3].set_ylabel('Value')
axes[3].set_title('Actual Velocity & Angular Velocity')
axes[3].legend()
axes[3].grid(True, alpha=0.3)

plt.tight_layout()
plt.show()

print(f"\nSetpoint: v={v} m/s, omega={omega} rad/s")
print(f"Actual mean: v={v_actual.mean():.4f} m/s, omega={omega_actual.mean():.4f} rad/s")

# 可视化 fig, axes = plt.subplots(4, 1, figsize=(8, 12)) # 轨迹对比 axes[0].plot(x_theory, y_theory, 'b-', lw=2, label='Theory') axes[0].plot(x_mujoco, y_mujoco, 'r--', lw=2, label='MuJoCo') axes[0].set_xlabel('X (m) - Right') axes[0].set_ylabel('Y (m) - Forward') axes[0].set_title('Trajectory Comparison') axes[0].legend() axes[0].axis('equal') axes[0].grid(True, alpha=0.3) # 增量误差分量 t_delta = t_mujoco[1:] # 增量对应的时间 axes[1].plot(t_delta, delta_dx, 'b-', label='Δdx') axes[1].plot(t_delta, delta_dy, 'g-', label='Δdy') axes[1].set_xlabel('Time (s)') axes[1].set_ylabel('Delta (m)') axes[1].set_title('Incremental Error (dx, dy)') axes[1].legend() axes[1].grid(True, alpha=0.3) # 增量误差范数 axes[2].plot(t_delta, delta_norm, 'r-', lw=1.5) axes[2].set_xlabel('Time (s)') axes[2].set_ylabel('||Δd|| (m)') axes[2].set_title('Incremental Euclidean Error') axes[2].grid(True, alpha=0.3) # 速度曲线 (setpoint in label, actual in plot) v_actual = np.array(traj_mujoco['v']) omega_actual = np.array(traj_mujoco['omega']) axes[3].plot(t_mujoco, v_actual, 'b-', lw=1.5, label=f'v (set={v:.1f} m/s)') axes[3].plot(t_mujoco, omega_actual, 'g-', lw=1.5, label=f'omega (set={omega:.1f} rad/s)') axes[3].set_xlabel('Time (s)') axes[3].set_ylabel('Value') axes[3].set_title('Actual Velocity & Angular Velocity') axes[3].legend() axes[3].grid(True, alpha=0.3) plt.tight_layout() plt.show() print(f"\nSetpoint: v={v} m/s, omega={omega} rad/s") print(f"Actual mean: v={v_actual.mean():.4f} m/s, omega={omega_actual.mean():.4f} rad/s")

Figure

Setpoint: v=0.2 m/s, omega=0.2 rad/s
Actual mean: v=0.1994 m/s, omega=0.1880 rad/s

5. 方差指标¶

In [8]:

print("=" * 50)
print("增量误差评估指标")
print("=" * 50)
print(f"方差 (Var Δdx): {np.var(delta_dx):.10f} m²")
print(f"方差 (Var Δdy): {np.var(delta_dy):.10f} m²")
print(f"方差 (Var Δdθ): {np.var(delta_dtheta):.10f} rad²")
print(f"方差 (Var ||Δd||): {np.var(delta_norm):.10f} m²")
print(f"MSE:             {np.mean(delta_norm**2):.10f} m²")
print(f"RMSE:            {np.sqrt(np.mean(delta_norm**2)):.10f} m")
print("=" * 50)

print("=" * 50) print("增量误差评估指标") print("=" * 50) print(f"方差 (Var Δdx): {np.var(delta_dx):.10f} m²") print(f"方差 (Var Δdy): {np.var(delta_dy):.10f} m²") print(f"方差 (Var Δdθ): {np.var(delta_dtheta):.10f} rad²") print(f"方差 (Var ||Δd||): {np.var(delta_norm):.10f} m²") print(f"MSE: {np.mean(delta_norm**2):.10f} m²") print(f"RMSE: {np.sqrt(np.mean(delta_norm**2)):.10f} m") print("=" * 50)

==================================================
增量误差评估指标
==================================================
方差 (Var Δdx): 0.0000000134 m²
方差 (Var Δdy): 0.0000000022 m²
方差 (Var Δdθ): 0.0000000397 rad²
方差 (Var ||Δd||): 0.0000000106 m²
MSE:             0.0000000293 m²
RMSE:            0.0001712759 m
==================================================