如何将物体空间加速度整合到世界空间位置（2D）

Question

我想把物体坐标系中的二维加速度数据进行双重积分，得到世界坐标系中的二维位置。物体总是指向速度的方向（假设火车）。在

所以我试着用velocity verlet积分对加速度值进行数值积分，将每一步的方向改为世界坐标系中的先前速度，由velocity verlet算法提供：

import numpy as np
from math import sqrt
from matplotlib import pyplot as plt

def rotate(a, newXAxis):
    r = newXAxis
    normX = r / sqrt(np.dot(r.T,r))
    normY = [-normX[1], normX[0]]
    b = np.dot(np.array([normX, normY]).T, a)
    return(b)

"""return true if v > 1 km/h or any speed given"""
def isMoving(deltaXPosition, deltaYPosition, deltaTime, fasterThankmh=1.0):
    x = deltaXPosition
    y = deltaYPosition
    t = deltaTime
    if t*t == 0.:
        return False
    if hasattr(x, "__len__"):
        x = x[0]
    if hasattr(y, "__len__"):
        y = y[0]
    if hasattr(t, "__len__"):
        t = t[0]
    speed = float(fasterThankmh)
    return((x*x + y*y) / (t*t) > 0.077160*speed*speed)

def velocity_verlet_integration(Xacc, Yacc,
                                x0=0., y0=0.,
                                vx_0=0, vy_0=0,
                                forward=np.array([1.0, 0.0])):
    vx = np.zeros(len(Xacc))
    vy = np.zeros(len(Xacc))
    x = np.zeros(len(Xacc))
    y = np.zeros(len(Xacc))
    x[0] = x0
    y[0] = y0
    vx[0] = vx_0
    vy[0] = vy_0
    for i in range(len(Xacc)-1):
        dt = Xacc[i+1]-Xacc[i]
        a = rotate(Yacc[i,:], forward)
        x[i+1] = x[i] + vx[i]*dt + 1.0/2.0*a[0]*dt*dt
        y[i+1] = y[i] + vy[i]*dt + 1.0/2.0*a[1]*dt*dt
        if isMoving(x[i+1]-x[i], y[i+1]-y[i], dt):
            forward = np.array([x[i+1]-x[i], y[i+1]-y[i]])
        aNext = rotate(Yacc[i+1,:], forward)
        vx[i+1] = vx[i] + dt*(a[0] + aNext[0])/2
        vy[i+1] = vy[i] + dt*(a[1] + aNext[1])/2
    return x, y

用一个简单的圆周运动测试：

^{pr2}$

这会导致一个向外漂移，因为电流方向是基于最后的速度，这显然是一个错误的近似。在

谁能告诉我更好的解决办法吗？在

---

一些想法：

• 在下一个世界里，把这些坐标加起来，就可以得到更好的近似值。但在我的方法中，下一个速度取决于世界坐标系中的下一个加速度，而这个加速度又需要下一个方向（追逐它自己的尾巴）。在
• 如果我用我的方法得到下一个速度的第一个近似值，从而得到下一个方向，我可以用这个方法，通过上面的想法来改进当前的方向。现在我可以对下一个速度和下一个方向做一个更好的近似，并再次使用它来改善当前的方向。这可能是一个可能的解决方案，尽管它看起来很难看。在

Answer 1

基于我的想法（在我问题的最后），我添加了一个糟糕的解决方案，所以我不会接受它作为答案。在

def my_integration(t, a_object,
                   x0=0., y0=0.,
                   vx_0=0, vy_0=0,
                   forward=np.array([1.0, 0.0])):
    v = np.zeros((len(t), 2))
    p = np.zeros((len(t), 2))
    p[0,:] = np.array([x0, y0])
    v[0,:] = np.array([vx_0, vy_0])
    v[1,:] = np.array([vx_0, vy_0])
    for i in range(len(t)-1):
        """this feels like a hack"""
        for j in range(10):
            dt = t[i+1]-t[i]
            a     = rotate(a_object[i,:],   v[i,:]+v[i+1,:])
            p[i+1,:] = p[i,:] + v[i,:]*dt + 1.0/2.0*a*dt*dt
            aNext = rotate(a_object[i+1,:], v[i,:]+v[i+1,:])
            v[i+1,:] = v[i,:] + dt*(a + aNext)/2.
            if i < len(t)-2:
                v[i+2,:] = v[i+1,:]
    return p

对于情节，加上这个：

^{pr2}$