四足機(jī)器人的步態(tài)規(guī)劃測(cè)試代碼

步態(tài)規(guī)劃

因?yàn)槲覀兊乃淖?a target="_blank">機(jī)器人足端，是在不停地與地面接觸，懸空（擺動(dòng)），所以這里說(shuō)的步態(tài)規(guī)劃，其實(shí)指的的一個(gè)連續(xù)（離散）時(shí)間上的接觸序列。

我們以一條腿為例，本質(zhì)上，可以看作是一個(gè)周期性的，用于生成各腿接觸狀態(tài)的階躍函數(shù)。其輸入值為時(shí)間，輸出值為0或1，函數(shù)可以表示成如下：

其中t=t%T為時(shí)間的周期函數(shù)，其圖像如下所示。

可以看出，步態(tài)規(guī)劃器給出的是一段由0-1組成的接觸序列。接下來(lái)加上相位關(guān)系。以tort步態(tài)為例，其相位為[0, 0.5, 0.5, 0]，步態(tài)規(guī)劃隨時(shí)間的圖像如下所示：

測(cè)試代碼：

import numpy as np
import matplotlib.pyplot as plt


T = 1
t = np.linspace(0, 5, 100)
states = []
FR = []
FL = []
BR = []
BL = []


phi_offset = [FR, FL, BR, BL]
offset = [0, 0.5, 0.5, 0]




def step(t):
    if t < 0.5*T:
        state = 0
    else:
        state = 1
    return state




def draw():
    for time in t:
        for i in range(len(offset)):
            phi_offset[i].append(step((time+offset[i]) % T))


    plt.subplot(411)
    plt.ylabel('FR')
    plt.plot(t, phi_offset[0])


    plt.subplot(412)
    plt.ylabel('FL')
    plt.plot(t, phi_offset[1])


    plt.subplot(413)
    plt.ylabel('BR')
    plt.plot(t, phi_offset[2])


    plt.subplot(414)
    plt.ylabel('BL')
    plt.plot(t, phi_offset[3])




fig = plt.figure()
draw()
fig.tight_layout()
plt.show()