python - 如何将物体空间加速度集成到世界空间位置(2D)

Question

我想对对象坐标中的 2D 加速度数据进行双重积分，以获得世界坐标中的 2D 位置。物体始终指向速度方向(例如假设火车)。

所以我尝试将加速度值与 velocity verlet 进行数值积分积分，将每一步的方向更改为世界坐标中的先前速度，由速度 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

通过简单的圆周运动来测试:

"""test circle"""
centripetal=-0.2
N = 0.01
xCircle = np.array(range(int(100*10**N)))/float(10**N)
yCircle = np.array([[0.0, centripetal] for i in xCircle])
xvvi, yvvi = velocity_verlet_integration(xCircle, yCircle, 0., 0., 1., 0.)
#plot it
plt.plot(xvvi, yvvi, ".-", label='position with "velocity verlet" integration')

这会导致向外漂移，因为当前方向是基于最后的速度，这显然是一个不好的近似。

有人能给我指出更好的解决方案吗？

---

一些想法:

• 最理想的情况是，我们需要世界坐标中的最后一个速度和下一个速度，通过对它们进行平均(例如相加)来做出更好的近似。但在我的方法中，下一个速度取决于世界坐标中的下一个加速度，而这又需要下一个方向(追逐自己的尾部)。
• 如果我使用我的方法来获得下一个速度的第一个近似值，从而获得下一个方向，我可以使用它通过上述想法来改进当前方向。现在我可以对下一个速度和下一个方向做出更好的近似，并再次使用它来改进当前方向。这可能是一个可能的解决方案，尽管它看起来真的很难看。

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

对于情节，添加以下内容:

plt.plot(np.cos(pi*2*np.array(range(21))/20)/centripetal,
        (np.sin(pi*2*np.array(range(21))/20)+1)/centripetal,
        "x", label='ground truth')
myi = my_integration(t, a, 0., 0., 1., 0.)
plt.plot(myi[:,0], myi[:,1], "--", label='position with my integration')
plt.legend(fontsize = 'x-small')