使用scipy.integrate.quad计算轨迹位置时结果异常的问题

算力阿峰

2026-6-22

问题分析与解决

核心问题定位

你遇到的错误本质是航向角θ(t)的计算或位置积分的映射逻辑出错，导致当k(t)=0时θ(t)未保持为0，或位置积分的被积函数使用错误，最终x方向积分结果偏离预期。

具体可能原因及解决方法

位置积分的坐标映射搞反
正确的轨迹坐标积分公式应为：
```
x(t) = ∫₀ᵗ v(τ) * cosθ(τ) dτ
y(t) = ∫₀ᵗ v(τ) * sinθ(τ) dτ
```
若你错误地将x方向写成v(τ)*sinθ(τ)，当θ=0时sinθ=0，积分结果会趋近于0（与你得到的4.235e-6数值误差吻合）。检查并修正位置积分的被积函数即可。
航向角初始条件错误
若你未将初始航向角θ₀设为0，而是设为π/2这类角度，cosθ₀=0，会导致x方向积分结果接近0。确认θ₀的初始值为0，匹配测试场景的理论假设。
曲率与航向角的微分关系误用
曲率k的定义是k = dθ/ds（s为弧长），而弧长变化率ds/dt = v(t)，因此航向角的微分关系应为：
```
dθ/dt = k(t) * v(t)
```
虽然k(t)=0时该错误不会体现，但后续扩展非零曲率场景时会出错。确保θ(t)的积分逻辑使用正确的微分关系。

验证代码示例

import numpy as np
from scipy.integrate import quad

# 测试参数
v = lambda t: 10.0
k = lambda t: 0.0
theta0 = 0.0  # 初始航向角

def theta(t):
    dtheta_dt = lambda tau: k(tau) * v(tau)
    integral, _ = quad(dtheta_dt, 0, t)
    return theta0 + integral

def x(t):
    integrand = lambda tau: v(tau) * np.cos(theta(tau))
    result, _ = quad(integrand, 0, t)
    return result

print(f"x(0.5) = {x(0.5)}")  # 输出应为5.0
print(f"theta(0.5) = {theta(0.5)}")  # 输出应为0.0