四足機(jī)器人測(cè)量模型介紹

測(cè)量模型(update模型)

預(yù)測(cè)模型中極有可能包含了規(guī)劃器所產(chǎn)生的誤差，因此，我們可以使用更多的測(cè)量模型來糾正預(yù)測(cè)值，以獲得更精確的接觸概率估計(jì)。 標(biāo)準(zhǔn)卡爾曼濾波修正方程如下：

1、地形高度模型

同樣，取不同參數(shù)觀察其概率分布情況，橫坐標(biāo)為地形高度，縱坐標(biāo)為發(fā)生接觸的概率：

高度模型的代碼：

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

該模型提供的校正測(cè)量及其協(xié)方差矩陣如下：

2、反作用力模型

取不同參數(shù)觀察其概率分布圖像，橫坐標(biāo)為力的大小，縱坐標(biāo)為發(fā)生接觸的概率：

模型代碼如下：

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

該模型提供第二項(xiàng)校正測(cè)量結(jié)果：