接觸檢測(cè)

步態(tài)規(guī)劃器給出的接觸序列是嚴(yán)格按照時(shí)間進(jìn)行周期性計(jì)算的。而在實(shí)際運(yùn)行當(dāng)中，由于地形的不平整，又或者存在坡度等情況，腿部會(huì)發(fā)生提前或者延遲接觸等情況，因此只靠步態(tài)規(guī)劃器給出的接觸序列來(lái)控制機(jī)器人往往是不可靠的。

因此這里提出一個(gè)基于卡爾曼濾波的概率接觸檢測(cè)。其綜合考慮了步態(tài)規(guī)劃其給出的恒定接觸序列，足端高度，地形的不平整性，以及通過(guò)關(guān)節(jié)編碼器數(shù)據(jù)所計(jì)算出來(lái)的關(guān)節(jié)力矩，來(lái)提高接觸檢測(cè)的精度，同時(shí)減少了腿部由于電機(jī)力矩控制所引起的關(guān)節(jié)回彈現(xiàn)象

預(yù)測(cè)模型

卡爾曼濾波器的標(biāo)準(zhǔn)預(yù)測(cè)方程如下：

該模型有以下函數(shù)曲線：

當(dāng)參數(shù)（μ,σ）取不同值時(shí),圖像如下所示，可以看出，σ取不同值時(shí)，曲線的曲率會(huì)有所變化，當(dāng)曲率比較大時(shí)，其預(yù)測(cè)結(jié)果更精確，但是許用誤差范圍較小，容易造成系統(tǒng)的不穩(wěn)定；

而當(dāng)曲率變小時(shí)，預(yù)測(cè)模型的穩(wěn)定性更強(qiáng)，其許用誤差范圍較大，但預(yù)測(cè)結(jié)果相對(duì)來(lái)說(shuō)沒那么精確，實(shí)際參數(shù)的選取可根據(jù)實(shí)機(jī)結(jié)果進(jìn)行調(diào)整：

測(cè)試代碼如下：

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 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

因此，對(duì)于k個(gè)接觸點(diǎn)，該預(yù)測(cè)模型可以作為系統(tǒng)的瞬時(shí)輸入為：

協(xié)方差矩陣如下，該矩陣表示我們對(duì)預(yù)測(cè)精度的信賴程度

由于我們只關(guān)注瞬時(shí)接觸檢測(cè)（通過(guò)融合當(dāng)前可用的測(cè)量），所以狀態(tài)矩陣和輸入矩陣被定義為如下：