如何利用速度与位置采样值进行位置插值计算

核心思路 #

位置是速度对时间的积分，我们可以通过线性插值速度函数的解析积分来计算任意时刻的位置，充分利用已有速度采样数据，避免直接对位置做插值，更符合运动学规律。

由于你已经用np.interp实现了速度的线性插值，而线性函数的积分可通过梯形公式直接计算，无需数值积分，效率更高且结果精确。

具体实现逻辑 #

1. 定位目标时刻t在时间序列中的区间：用np.searchsorted找到对应索引k，即times[k] ≤ t < times[k+1]；若t早于所有采样时间则k=0，晚于所有则k=n-2（用最后两个点的斜率外推）。
2. 对每个维度，计算从times[k]到t的速度积分（位置变化量），再加上positions[k]对应维度的值，得到目标位置。

公式推导 #

对于区间[t_k, t_{k+1}]内的t，线性插值速度为：
v(t) = v_k + (v_{k+1} - v_k) * (t - t_k)/(t_{k+1} - t_k)
其中v_k、v_{k+1}分别为times[k]、times[k+1]对应的速度。

从t_k到t的位置变化量（速度积分）为梯形面积：
Δp = (v_k + v(t)) * (t - t_k) / 2

外推场景直接沿用上述公式，用边界相邻两点的斜率计算外推速度即可。

代码实现 #

以下是向量化的numpy实现，支持单个或多个时刻的批量计算：

import numpy as np

def interpolate_position(t, times, positions, velocities):
    # 统一转为numpy数组
    t = np.asarray(t)
    times = np.asarray(times)
    positions = np.asarray(positions)
    velocities = np.asarray(velocities)
    
    n = len(times)
    m = positions.shape[1]
    
    # 确定每个t对应的区间索引k
    k = np.searchsorted(times, t, side='right') - 1
    k[k < 0] = 0
    k[k >= n-1] = n-2  # 外推时用最后两个点的斜率
    
    # 获取对应时间、速度、位置数据
    t_k = times[k]
    t_next = times[k+1] if n > 1 else times[k]
    v_k = velocities[k]
    v_next = velocities[k+1] if n > 1 else velocities[k]
    p_k = positions[k]
    
    # 计算目标时刻的速度v(t)
    dt = t - t_k
    dt_interval = t_next - t_k
    dt_interval[dt_interval == 0] = 1e-10  # 避免除以0
    slope = (v_next - v_k) / dt_interval[:, np.newaxis]
    v_t = v_k + slope * dt[:, np.newaxis]
    
    # 计算位置变化量及目标位置
    delta_p = (v_k + v_t) * dt[:, np.newaxis] / 2
    p_t = p_k + delta_p
    
    # 适配单个时刻的返回格式
    return p_t.squeeze() if t.ndim == 0 else p_t

使用示例 #

# 测试数据
times = np.array([0, 1, 2, 3])
positions = np.array([[0, 0], [1, 2], [3, 5], [6, 9]])
velocities = np.array([[1, 2], [2, 3], [3, 4], [3, 4]])

# 单个时刻计算
t_single = 1.5
pos_single = interpolate_position(t_single, times, positions, velocities)
print(f"t={t_single}时的位置：{pos_single}")

# 多个时刻批量计算
t_multi = np.array([-0.5, 2.5, 4])
pos_multi = interpolate_position(t_multi, times, positions, velocities)
print(f"多个时刻的位置：\n{pos_multi}")

说明 #

• 该方法严格遵循运动学关系，位置计算基于速度积分，比直接插值位置更合理。
• 支持数据集时间范围外的外推计算，适配边界场景。
• 向量化操作确保计算效率，适合处理大规模数据。

内容的提问来源于stack exchange，提问作者Alex