四足机器人的步态规划测试代码

步态规划

因为我们的四足机器人足端，是在不停地与地面接触，悬空（摆动），所以这里说的步态规划，其实指的的一个连续（离散）时间上的接触序列。

我们以一条腿为例，本质上，可以看作是一个周期性的，用于生成各腿接触状态的阶跃函数。其输入值为时间，输出值为0或1，函数可以表示成如下：

其中t=t%T为时间的周期函数，其图像如下所示。

可以看出，步态规划器给出的是一段由0-1组成的接触序列。接下来加上相位关系。以tort步态为例，其相位为[0, 0.5, 0.5, 0]，步态规划随时间的图像如下所示：

测试代码：

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