python - 用于视觉跟踪在排水沟上滑动的球的卡尔曼滤波器

Question

我正在做一个机器人需要将球保持在排水沟上所需位置的项目。排水沟的一端固定，另一端由机器人的执行器固定，因此机器人的执行器决定了它的方向，从而决定了球的位置。机器人的执行器可以垂直上下移动。机器人的控制器是基于深度学习的，并接受过模拟训练。训练完成后，我将其部署在物理机器人上，以便在现实世界中执行任务。控制器需要球的位置和球的速度。球的位置是通过使用低质量网络摄像头的视觉跟踪获得的，这意味着观察结果很嘈杂。速度是通过计算两个时间步长（~0.1s）之间球位置的差异来获得的。然而，球位中的噪声会在速度计算中产生很大的误差。即使控制器在嘈杂的观察中表现良好，我也想改进它。所以我开始研究卡尔曼滤波器。

我的状态由球的位置 (1d) 和速度 (1d) 组成，但是我只用相机测量球的位置。我的状态模型如下所示：

和

对于命令 U，我考虑了 4 种情况并得出每种情况的加速度：

u 是低于或高于排水沟平衡位置的效应器高度（受控制），= 0.06 是我发现给出最真实行为的塑料/塑料摩擦系数（球和排水沟都是塑料）（见下图）。在代码中，我正在检查符号并知道要使用哪个方程。我记录了机器人执行任务以验证模型的轨迹（预测步骤）。但是，该模型似乎不是很准确：. 预测的球位置并不完全符合真实（测量）位置。然而，预测的速度比简单计算的速度（测量的）更平滑。所以我的第一个问题是：我的预测步骤有问题吗？即使增加空气摩擦，预测的球轨迹仍然非常错误。无论如何，我尝试添加更新步骤以查看它是如何进行的：

• 过程协方差矩阵

和 ：

我在不知道该放什么的情况下选择了 Q 的值。

• 卡尔曼增益

在哪里 ：

H 和 R 是单位矩阵。

• 更正

在哪里 ：

. 由于我无法直接测量它，所以我把.

但是再一次，事情的表现并不好：。卡尔曼滤波位置没有平滑，卡尔曼滤波速度没有意义。我很难弄清楚我做错了什么。我怀疑预测步骤不正确，我应该为 R 和 Q 选择更合适的值，但我并不真正了解它们的作用以及如何选择它们。我是否使用了卡尔曼滤波器 correclty ？我哪里搞砸了？如果问题是由于我的实现，这里是我的代码：

def kf_predict(X, P, A, Q, B, U):
    X = dot(A, X) + dot(B, U)
    P = dot(A, dot(P, A.T)) + Q
    return(X,P) 


error_est_pos = 3
error_est_vel = 2
error_mes_pos = 1
dt = 0.1
g = 9.81 * 100
theta = 0
mu = 0.06
# Initialization of state matrices
X = np.array([[0.25*56], [0.0]])
P = np.diag((error_est_pos**2, error_est_vel**2))
A = np.array([[1, dt], [0, 1]])
Q = 0.1*np.eye(X.shape[0])
B = np.array([[1/2*(dt*dt)],[dt]])

# Measurement matrices
Y = np.array([ball_pos[0] + 0*np.random.randn(1)[0]])
H = np.identity(2)
R = np.diag([error_mes_pos**2]) 
d = 45
for obs,u in zip(ball_pos,effec_pos):
    theta = np.arctan(u/d)
    tmp = np.abs(theta)
    if X[1] == 0 and u ==0:
        U = np.array(0)
    else : 
        if theta * X[1][0]  >= 0 and theta >= 0: # case 1
            U = np.array(g*(np.sin(tmp) - mu*np.cos(tmp)))
        elif theta * X[1][0]  < 0 and theta >= 0: # case 2
            U = np.array(g*(np.sin(tmp) + mu*np.cos(tmp)))
        elif theta * X[1][0]  < 0 and theta <= 0:  # case 3
            U =  np.array(g*( - np.sin(tmp) - mu*np.cos(tmp)))
        elif theta * X[1][0]  >= 0 and theta <= 0: # case 4
            U =  np.array(g*( - np.sin(tmp) + mu*np.cos(tmp)))
    (X, P) = kf_predict(X, P, A, Q, B, U)
     # Calculating the Kalman Gain
    S = H.dot(P).dot(H.T) + R
    K = P.dot(H).dot(inv(S))
    Y = np.array([[obs +  np.random.randn(1)[0]],[0.0]])
    # Update the State Matrix
    # Combination of the predicted state, measured values, covariance matrix and Kalman Gain
    X = X + K.dot(Y - H.dot(X))
    # Update Process Covariance Matrix
    P = (np.identity(len(K)) - K.dot(H)).dot(P) 

提前感谢您提供任何提示或解决方案。

标签： pythoncomputer-visionfilteringsignal-processingkalman-filter