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

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

因為我們的四足機(jī)器人足端，是在不停地與地面接觸，懸空（擺動），所以這里說的步態(tài)規(guī)劃，其實指的的一個連續(xù)（離散）時間上的接觸序列。

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

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

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

測試代碼：

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