四足机器人足端接触检测完整代码

总体模型

将以上两组单独的测量结果叠加起来，形成卡尔曼滤波器中使用的观测向量。 同样，每个度量的协方差矩阵形成一个整体块 对角线协方差矩阵如下：

目前，我们使用的卡尔曼滤波用实现，这一融合过程其实可以通过贝叶斯定律的静态似然最大化得到。

然而，上述过程是一个嵌入在标准卡尔曼更新中的过程，大概是因为在机器人学中更容易实现。

完整代码

import math
import numpy as np
import matplotlib.pyplot as plt


# 设定周期为2
T = 2


# 根据相位计算当前接触状态
def get_contact_state(phi):
    if phi < 0.5*T:
        state = 1
    else:
        state = 0
    return state




# 预测模型
def prediction_model(phi, state, params):
    """
    Given the gait schedule and the current phase, φ, of a leg,
    the gait scheduler provides an expected contact state s φ of
    each leg


    :param phi: phase
    :param state: contact state
    :param params: [mu, mu_bar, sigma, sigma_bar]
                    mu = [mu1, mu2] and so on
    :return: the probability of contact
    """
    mu0, mu1 = params[0]
    mu0_bar, mu1_bar = params[1]


    sigma0, sigma1 = params[2]
    sigma0_bar, sigma1_bar = params[3]


    a = math.erf((phi-mu0)/(sigma0*np.sqrt(2)))
        + math.erf((mu1-phi)/(sigma1*np.sqrt(2)))


    b = 2+math.erf((mu0_bar-phi)/(sigma0_bar*np.sqrt(2)))
        + math.erf((phi-mu1_bar)/(sigma1_bar*np.sqrt(2)))


    if state == 1:
        prob = 0.5 * (state * a)
    else:
        prob = 0.5 * (state * b)


    return prob


# 测量模型-离地高度
def ground_height(pz, params):
    """
    The probability of contact given foot heigh


    :param pz: ground height
    :param params: [mu_z, sigma_z]
    :return: The probability of contact
    """
    mu_z, sigma_z = params
    prob_ground_height = 0.5 * (1 + math.erf((mu_z-pz) / (sigma_z*np.sqrt(2))))


    return prob_ground_height




# 测量模型-反作用力
def contact_force(f, params):
    """
    the probability of contact given the estimated foot force


    :param f: contact force
    :param params: [mu_z, sigma_z]
    :return: The probability of contact
    """
    mu_f, sigma_f = params
    prob_force = 0.5 * (1 + math.erf((f-mu_f) / (sigma_f*np.sqrt(2))))


    return prob_force


# 概率分布绘图
def test_predict():
    Mu = [0, 1]
    Mu_bar = [0, 1]
    Sigma = [0.025, 0.025]
    Sigma_bar = [0.025, 0.025]


    t = np.linspace(0, 0.999, 1000)
    prediction_prob = []
    prediction_prob2 = []
    prediction_prob3 = []


    for time in t:
        phi = time % T
        state = get_contact_state(phi)


        p = prediction_model(phi, state, [Mu, Mu_bar, Sigma, Sigma_bar])
        p2 = prediction_model(phi, state, [Mu, Mu_bar, [0.05, 0.05], [0.05, 0.05]])
        p3 = prediction_model(phi, state, [Mu, Mu_bar, [0.01, 0.01], [0.01, 0.01]])


        prediction_prob.append(p)
        prediction_prob2.append(p2)
        prediction_prob3.append(p3)


    fig = plt.figure()
    plt.subplot(211)
    plt.title('contact phase')
    plt.grid()
    plt.plot(t, prediction_prob, label='$mu=[0, 1],sigma=[0.025, 0.025]$')
    plt.plot(t, prediction_prob2, label='$mu=[0, 1],sigma=[0.05, 0.05]$')
    plt.plot(t, prediction_prob3, label='$mu=[0, 1],sigma=[0.01, 0.01]$')
    plt.legend()


    plt.subplot(212)
    plt.title('swing phase')
    plt.grid()
    plt.plot(t, 1-np.array(prediction_prob), label='$mu=[0, 1],sigma=[0.025, 0.025]$')
    plt.plot(t, 1-np.array(prediction_prob2), label='$mu=[0, 1],sigma=[0.05, 0.05]$')
    plt.plot(t, 1-np.array(prediction_prob3), label='$mu=[0, 1],sigma=[0.01, 0.01]$')
    plt.legend()
    fig.tight_layout()


    plt.show()




def test_ground_height():
    height = np.linspace(-0.3, 0.3, 1000)
    ground_height_prob = []
    ground_height_prob2 = []
    ground_height_prob3 = []


    params = [0, 0.025]
    params2 = [0, 0.05]
    params3 = [0, 0.1]


    for h in height:
        ground_height_prob.append(ground_height(h, params))
        ground_height_prob2.append(ground_height(h, params2))
        ground_height_prob3.append(ground_height(h, params3))


    fig2 = plt.figure()
    plt.plot(height, ground_height_prob, label='$mu=0,sigma=0.025$')
    plt.plot(height, ground_height_prob2, label='$mu=0,sigma=0.05$')
    plt.plot(height, ground_height_prob3, label='$mu=0,sigma=0.1$')
    fig2.tight_layout()
    plt.legend()
    plt.grid()
    plt.show()




def test_contact_force():
    force = np.linspace(-50, 200, 1000)
    contact_force_prob = []
    contact_force_prob2 = []
    contact_force_prob3 = []


    params = [35, 10]
    params2 = [35, 25]
    params3 = [35, 50]


    for f in force:
        contact_force_prob.append(contact_force(f, params))
        contact_force_prob2.append(contact_force(f, params2))
        contact_force_prob3.append(contact_force(f, params3))


    fig3 = plt.figure()


    plt.plot(force, contact_force_prob, label='$mu=25,sigma=10$')
    plt.plot(force, contact_force_prob2, label='$mu=25,sigma=25$')
    plt.plot(force, contact_force_prob3, label='$mu=25,sigma=50$')


    fig3.tight_layout()
    plt.grid()
    plt.legend()
    plt.show()




# test_predict()
# test_ground_height()
test_contact_force()