二重积分器（Double Integrator）

不要被这个东西高大尚的名称骗了，原理其实就是二重积分

假设你正在公路上开车，你看着车的仪表盘和导航，你知道车的历史位置，那么显然你可以计算出它的速度，作一阶的求导即可，而加速度，即在这基础上再导一遍，也可以求出。

那么二重积分器是什么呢？非常简单，就是将你求出的加速度a作为积分器的输入，然后进行二次积分，在此过程中，你可以得到以此加速度作为基础的速度和对应的位置。

而这个其实就是对下一位置的预计，也就是predict_position。

用一句话总结，你知道当前的加速度，请你求下一时刻的速度和位置（假设当前位置已知），那么，用二重积分器就对了。

示例：

这里用一个常见的二重积分作为示例,以上面的小车为例，

分别展示匀加速运动和小车刹车的场景。

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation# 设置中文字体
plt.rcParams['font.sans-serif'] = ['SimHei', 'Microsoft YaHei', 'Arial Unicode MS']
plt.rcParams['axes.unicode_minus'] = Falseclass VehicleMotionIntegrator:"""小车运动二重积分器"""def __init__(self, initial_position=0.0, initial_velocity=0.0, dt=0.1):"""初始化积分器参数:initial_position: 初始位置 (m)initial_velocity: 初始速度 (m/s)dt: 时间步长 (s)"""self.position = initial_positionself.velocity = initial_velocityself.dt = dt# 历史记录self.time_history = [0.0]self.position_history = [initial_position]self.velocity_history = [initial_velocity]self.acceleration_history = []def update(self, acceleration):"""根据加速度更新速度和位置（二重积分）参数:acceleration: 当前加速度 (m/s^2)返回:(position, velocity): 更新后的位置和速度"""# 一重积分: a → v# v(t+dt) = v(t) + a*dtself.velocity = self.velocity + acceleration * self.dt# 二重积分: v → s# s(t+dt) = s(t) + v*dtself.position = self.position + self.velocity * self.dt# 记录历史current_time = self.time_history[-1] + self.dtself.time_history.append(current_time)self.position_history.append(self.position)self.velocity_history.append(self.velocity)self.acceleration_history.append(acceleration)return self.position, self.velocitydef predict_position(self, acceleration, time_steps=1):"""预测未来位置（不改变当前状态）参数:acceleration: 假设的恒定加速度 (m/s^2)time_steps: 预测的时间步数返回:(predicted_positions, predicted_velocities): 预测的位置和速度数组"""predicted_positions = [self.position]predicted_velocities = [self.velocity]temp_position = self.positiontemp_velocity = self.velocityfor _ in range(time_steps):temp_velocity = temp_velocity + acceleration * self.dttemp_position = temp_position + temp_velocity * self.dtpredicted_positions.append(temp_position)predicted_velocities.append(temp_velocity)return predicted_positions, predicted_velocitiesdef reset(self, position=0.0, velocity=0.0):"""重置积分器状态"""self.position = positionself.velocity = velocityself.time_history = [0.0]self.position_history = [position]self.velocity_history = [velocity]self.acceleration_history = []def derive_velocity(positions, dt):"""从位置历史计算速度（一阶导数）"""velocities = np.diff(positions) / dtreturn velocitiesdef derive_acceleration(velocities, dt):"""从速度历史计算加速度（二阶导数）"""accelerations = np.diff(velocities) / dtreturn accelerationsdef example_1_constant_acceleration():"""示例1: 匀加速运动"""print("=" * 60)print("示例1: 匀加速运动")print("=" * 60)integrator = VehicleMotionIntegrator(initial_position=0.0,initial_velocity=0.0,dt=0.1)# 恒定加速度 2 m/s^2acceleration = 2.0duration = 5.0  # 5秒steps = int(duration / integrator.dt)for _ in range(steps):pos, vel = integrator.update(acceleration)# 可视化fig, axes = plt.subplots(3, 1, figsize=(10, 8))# 加速度axes[0].plot(integrator.time_history[1:], integrator.acceleration_history, 'r-', linewidth=2)axes[0].set_ylabel(r'加速度 (m/s$^2$)', fontsize=12)axes[0].set_title('匀加速运动 - 二重积分过程', fontsize=14)axes[0].grid(True, alpha=0.3)# 速度（一重积分）axes[1].plot(integrator.time_history, integrator.velocity_history, 'g-', linewidth=2)axes[1].set_ylabel(r'速度 (m/s)', fontsize=12)axes[1].grid(True, alpha=0.3)# 位置（二重积分）axes[2].plot(integrator.time_history, integrator.position_history, 'b-', linewidth=2)axes[2].set_ylabel(r'位置 (m)', fontsize=12)axes[2].set_xlabel(r'时间 (s)', fontsize=12)axes[2].grid(True, alpha=0.3)plt.tight_layout()plt.show()print(f"最终位置: {integrator.position:.2f} m")print(f"最终速度: {integrator.velocity:.2f} m/s")print(f"理论位置 (s = 0.5*a*t^2): {0.5 * acceleration * duration ** 2:.2f} m")print(f"理论速度 (v = a*t): {acceleration * duration:.2f} m/s")def example_2_forward_backward():"""示例2: 从历史位置推导加速度，再积分预测未来位置"""print("\\n" + "=" * 60)print("示例2: 从历史位置 → 求导得加速度 → 积分预测未来位置")print("=" * 60)dt = 0.1# 模拟真实的历史位置数据（匀加速运动）t_history = np.arange(0, 3, dt)a_real = 1.5  # 真实加速度s_history = 0.5 * a_real * t_history ** 2  # s = 0.5*a*t^2print("步骤1: 已知历史位置数据")print(f"  位置数据点数: {len(s_history)}")print(f"  起始位置: {s_history[0]:.2f} m")print(f"  当前位置: {s_history[-1]:.2f} m")# 一阶导数: 位置 → 速度v_derived = derive_velocity(s_history, dt)print(f"\\n步骤2: 一阶求导得到速度")print(f"  当前速度: {v_derived[-1]:.2f} m/s")# 二阶导数: 速度 → 加速度a_derived = derive_acceleration(v_derived, dt)print(f"\\n步骤3: 二阶求导得到加速度")print(f"  估计加速度: {np.mean(a_derived):.2f} m/s^2")print(f"  真实加速度: {a_real:.2f} m/s^2")# 使用估计的加速度进行二重积分预测integrator = VehicleMotionIntegrator(initial_position=s_history[-1],initial_velocity=v_derived[-1],dt=dt)a_estimate = np.mean(a_derived)prediction_steps = 20print(f"\\n步骤4: 使用二重积分器预测未来 {prediction_steps * dt:.1f} 秒")for _ in range(prediction_steps):integrator.update(a_estimate)t_future = np.arange(t_history[-1], t_history[-1] + prediction_steps * dt + dt, dt)s_real_future = 0.5 * a_real * t_future ** 2# 可视化对比fig, axes = plt.subplots(2, 1, figsize=(12, 8))# 位置对比axes[0].plot(t_history, s_history, 'b-', linewidth=2, label='历史位置')# 调整预测轨迹的时间轴：从预测起点开始（t_history[-1]）predict_time = np.arange(0, len(integrator.position_history) * integrator.dt, integrator.dt)predict_time = predict_time + t_history[-1]  # 让预测时间从“预测起点”开始axes[0].plot(predict_time, integrator.position_history, 'r--',linewidth=2, label='预测位置')axes[0].plot(t_future, s_real_future, 'g:', linewidth=2, label='真实未来位置')axes[0].axvline(x=t_history[-1], color='gray', linestyle='--', alpha=0.5)axes[0].text(t_history[-1], max(s_real_future) * 0.5, '预测起点',rotation=90, va='bottom')axes[0].set_ylabel('位置 (m)', fontsize=12)axes[0].set_title('从历史数据推导 → 预测未来位置', fontsize=14)axes[0].legend(fontsize=10)axes[0].grid(True, alpha=0.3)# 速度对比axes[1].plot(t_history[1:], v_derived, 'b-', linewidth=2, label='推导速度')# 调整预测速度的时间轴：从预测起点开始axes[1].plot(predict_time, integrator.velocity_history, 'r--',linewidth=2, label='预测速度')v_real_future = a_real * t_futureaxes[1].plot(t_future, v_real_future, 'g:', linewidth=2, label='真实未来速度')axes[1].axvline(x=t_history[-1], color='gray', linestyle='--', alpha=0.5)axes[1].set_xlabel('时间 (s)', fontsize=12)axes[1].set_ylabel('速度 (m/s)', fontsize=12)axes[1].legend(fontsize=10)axes[1].grid(True, alpha=0.3)plt.tight_layout()plt.show()print(f"\\n预测准确度:")print(f"  最终预测位置: {integrator.position:.2f} m")print(f"  真实最终位置: {s_real_future[-1]:.2f} m")print(f"  预测误差: {abs(integrator.position - s_real_future[-1]):.2f} m")def example_3_varying_acceleration():"""示例3: 变加速度运动（刹车场景）"""print("\\n" + "=" * 60)print("示例3: 小车刹车场景（变加速度）")print("=" * 60)integrator = VehicleMotionIntegrator(initial_position=0.0,initial_velocity=20.0,  # 初始速度 20 m/s (约72 km/h)dt=0.05)duration = 6.0steps = int(duration / integrator.dt)for i in range(steps):t = i * integrator.dt# 模拟刹车: 前2秒匀速，然后开始刹车if t < 2.0:acceleration = 0.0  # 匀速行驶elif t < 5.0:acceleration = -4.0  # 刹车加速度 -4 m/s^2else:acceleration = 0.0  # 刹车后静止if integrator.velocity < 0:integrator.velocity = 0  # 速度不能为负integrator.update(acceleration)# 可视化fig, axes = plt.subplots(3, 1, figsize=(10, 8))axes[0].plot(integrator.time_history[1:], integrator.acceleration_history,'r-', linewidth=2)axes[0].set_ylabel(r'加速度 (m/s$^2$)', fontsize=12)axes[0].set_title('小车刹车过程 - 加速度/速度/位置变化', fontsize=14)axes[0].grid(True, alpha=0.3)axes[0].axhline(y=0, color='k', linestyle='--', alpha=0.3)axes[1].plot(integrator.time_history, integrator.velocity_history,'g-', linewidth=2)axes[1].set_ylabel('速度 (m/s)', fontsize=12)axes[1].grid(True, alpha=0.3)axes[1].axhline(y=0, color='k', linestyle='--', alpha=0.3)axes[2].plot(integrator.time_history, integrator.position_history,'b-', linewidth=2)axes[2].set_ylabel('位置 (m)', fontsize=12)axes[2].set_xlabel('时间 (s)', fontsize=12)axes[2].grid(True, alpha=0.3)# 标注关键点axes[1].axvline(x=2.0, color='orange', linestyle='--', alpha=0.5)axes[1].text(2.0, max(integrator.velocity_history) * 0.5, '开始刹车',rotation=90, va='bottom')plt.tight_layout()plt.show()print(f"初始速度: 20.0 m/s (72 km/h)")print(f"刹车时间: 3.0 s")print(f"刹车距离: {integrator.position_history[-1] - integrator.position_history[int(2.0 / integrator.dt)]:.2f} m")print(f"总行驶距离: {integrator.position:.2f} m")def example_4_prediction():"""示例4: 实时预测演示"""print("\\n" + "=" * 60)print("示例4: 实时位置预测")print("=" * 60)integrator = VehicleMotionIntegrator(initial_position=0.0,initial_velocity=10.0,dt=0.1)# 当前加速度current_acceleration = 2.0# 实际运动10步print("实际运动中...")for _ in range(10):integrator.update(current_acceleration)print(f"当前位置: {integrator.position:.2f} m")print(f"当前速度: {integrator.velocity:.2f} m/s")print(f"当前加速度: {current_acceleration:.2f} m/s^2")# 预测未来轨迹prediction_steps = 20predicted_pos, predicted_vel = integrator.predict_position(current_acceleration,prediction_steps)print(f"\\n基于当前状态预测未来 {prediction_steps * integrator.dt:.1f} 秒:")print(f"  预测最终位置: {predicted_pos[-1]:.2f} m")print(f"  预测最终速度: {predicted_vel[-1]:.2f} m/s")# 可视化预测fig, axes = plt.subplots(2, 1, figsize=(10, 6))time_predict = np.arange(0, (prediction_steps + 1) * integrator.dt, integrator.dt)time_predict = time_predict + integrator.time_history[-1]# 位置预测axes[0].plot(integrator.time_history, integrator.position_history,'b-', linewidth=2, label='历史轨迹')axes[0].plot(time_predict, predicted_pos, 'r--', linewidth=2,label='预测轨迹', marker='o', markersize=3)axes[0].axvline(x=integrator.time_history[-1], color='gray',linestyle='--', alpha=0.5)axes[0].set_ylabel('位置 (m)', fontsize=12)axes[0].set_title('基于二重积分的位置预测', fontsize=14)axes[0].legend()axes[0].grid(True, alpha=0.3)# 速度预测axes[1].plot(integrator.time_history, integrator.velocity_history,'g-', linewidth=2, label='历史速度')axes[1].plot(time_predict, predicted_vel, 'r--', linewidth=2,label='预测速度', marker='o', markersize=3)axes[1].axvline(x=integrator.time_history[-1], color='gray',linestyle='--', alpha=0.5)axes[1].set_xlabel('时间 (s)', fontsize=12)axes[1].set_ylabel('速度 (m/s)', fontsize=12)axes[1].legend()axes[1].grid(True, alpha=0.3)plt.tight_layout()plt.show()if __name__ == "__main__":print("二重积分器 - 小车运动分析示例")print("=" * 60)print("核心概念:")print("  位置 --[求导]--> 速度 --[求导]--> 加速度")print("  加速度 --[积分]--> 速度 --[积分]--> 位置")print("=" * 60)print()# 运行所有示例example_1_constant_acceleration()example_2_forward_backward()example_3_varying_acceleration()example_4_prediction()print("\\n" + "=" * 60)print("总结:")print("  二重积分器通过已知的加速度，经过两次积分，")print("  可以预测出未来的速度和位置。")print("  这在轨迹预测、运动控制等领域有广泛应用。")print("=" * 60)