MATLAB 代码转换为 Python 代码：Kimura 振荡器实现四足机器人步态控制

以下为将 MATLAB 代码转换为 Python 代码的结果：

import numpy as np
import matplotlib.pyplot as plt

leg_num = 4  # 足的个数
mm = 8  # m项反馈信息
Tr = 0.04  # 上升时间参数
Ta = 0.38
c = 0.23  # 直流输入
b = -2.0  # 适应系数
a = -1.0  # 抑制系数
phai = 1

gait = 2
w_w = np.zeros((leg_num, leg_num))  # 定义步态矩阵
if gait == 1:
    w_w = np.array([[0, -1, 1, -1], [-1, 0, -1, 1], [1, -1, 0, -1], [-1, 1, -1, 0]])
elif gait == 2:
    w_w = np.array([[0, -1, -1, 1], [-1, 0, 1, -1], [-1, 1, 0, -1], [1, -1, -1, 0]])
elif gait == 3:
    w_w = np.array([[0, 1, -1, -1], [1, 0, -1, -1], [-1, 1, 0, -1], [-1, -1, 1, 0]])
elif gait == 4:
    w_w = np.array([[0, -1, -1, -1], [-1, 0, -1, -1], [-1, -1, 0, -1], [-1, -1, -1, 0]])
else:
    w_w = np.array([[0, -1, 1, -1], [-1, 0, -1, 1], [1, -1, 0, -1], [-1, 1, -1, 0]])

vf = np.zeros((leg_num, 1))
ve = np.zeros((leg_num, 1))
Vfe = np.concatenate((vf, ve), axis=1)

yf = np.zeros((leg_num, 1))
ye = np.zeros((leg_num, 1))
Yfe = np.concatenate((yf, ye), axis=1)
Yef = np.concatenate((ye, yf), axis=1)
Y = np.zeros((leg_num, 1))  # 输出的关节映射矩阵

uf = np.random.rand(leg_num, 1) * c / 10
ue = np.random.rand(leg_num, 1) * c / 10
Ufe = np.concatenate((uf, ue), axis=1)

hf = np.zeros((mm, 1))
he = np.zeros((mm, 1))
Hfe = np.concatenate((hf, he), axis=1)

ss = np.zeros((leg_num, mm))
WYf = np.zeros((leg_num, 1))
WYe = np.zeros((leg_num, 1))
SHf = np.zeros((leg_num, 1))
SHe = np.zeros((leg_num, 1))

E = np.ones((leg_num, 2))

# 关节参数，此处参数需实测，试验数据参考书
h = 0.02  # 抬腿高度
v = 1  # 行走速度
T = 0.4  # 步态周期
S = v * T  # 步长
l = 0.25  # 腿节长度
theta0 = 30 / 180 * np.pi  # 髋关节和膝关节平衡位置与垂直线夹角 
L = 2 * l * np.cos(theta0)  # 髋关节与足端之间长度
Ah = np.arcsin((0.5 * S) / (2 * L))  # 髋关节摆动幅度
Ak = np.arccos((l * np.cos(theta0) - h) / l) - theta0  # 膝关节摆动幅度

# 时间，分辨率设置
# 对时间进行算法积分
u0 = np.array([[0.009, 0.0021, 0.0134, 0.0022],
               [0.0272, 0.0003, 0.0206, 0.0057],
               [0.0199, 0.0067, 0.0171, 0.0111],
               [0.0022, 0.0151, 0.0149, 0.0081]])  # 初值会影响相序
n = 2000
t_total = 8 * np.pi / 2
t = np.linspace(0, t_total, n)  # 转一圈
ts = t_total / n

uf = u0[:, 0]
vf = u0[:, 1]
ue = u0[:, 2]
ve = u0[:, 3]
shsum = np.zeros((leg_num, 1))
k = 0
trace_Y_Y = np.full((4, n), np.nan)  # 轨迹配置初始化，空值
for j in t:
    k += 1
    WYf, WYe = wye_sumeq(w_w, leg_num, yf, ye)
    uf_dot, vf_dot, ue_dot, ve_dot = kimura_stateq(
        Tr, Ta, uf, vf, ue, ve, yf, ye, a, b, c, WYf, WYe, shsum)
    uf = uf + ts * uf_dot
    vf = vf + ts * vf_dot
    ue = ue + ts * ue_dot
    ve = ve + ts * ve_dot
    Y_Y = uf - ue
    trace_Y_Y[:, k] = Y_Y
    yf, ye = returnyfe(uf, ue, leg_num)

yPlot = trace_Y_Y.T
Judge1 = np.diff(yPlot[:, 0])
Judge2 = np.diff(yPlot[:, 1])
Judge3 = np.diff(yPlot[:, 2])
Judge4 = np.diff(yPlot[:, 3])
kplot = np.zeros((len(yPlot[:, 0]), 4))
thetah = np.zeros(4)
thetah[0] = np.max(yPlot[:, 0])
thetah[1] = np.max(yPlot[:, 1])
thetah[2] = np.max(yPlot[:, 2])
thetah[3] = np.max(yPlot[:, 3])
for j in range(len(yPlot[:, 0]) - 1):
    if Judge1[j] > 0:
        kplot[j, 0] = 1 * (Ak / Ah) * (1 - (yPlot[j, 0] / thetah[0]) ** 2)
    else:
        kplot[j, 0] = 0
    if Judge2[j] > 0:
        kplot[j, 1] = 1 * (Ak / Ah) * (1 - (yPlot[j, 1] / thetah[1]) ** 2)
    else:
        kplot[j, 1] = 0
    if Judge3[j] > 0:
        kplot[j, 2] = -1 * (Ak / Ah) * (1 - (yPlot[j, 2] / thetah[2]) ** 2)
    else:
        kplot[j, 2] = 0
    if Judge4[j] > 0:
        kplot[j, 3] = -1 * (Ak / Ah) * (1 - (yPlot[j, 3] / thetah[3]) ** 2)
    else:
        kplot[j, 3] = 0

plt.figure(2)
plt.subplot(4, 1, 1)
plt.plot(t, yPlot[:, 0], t, kplot[:, 0], 'green')
plt.ylabel('LF')
plt.axis([0, 10, -2, 2])  # XY坐标均衡
plt.grid(True)

plt.subplot(4, 1, 2)
plt.plot(t, yPlot[:, 1], t, kplot[:, 1], 'green')
plt.ylabel('RF')
plt.axis([0, 10, -2, 2])  # XY坐标均衡
plt.grid(True)

plt.subplot(4, 1, 3)
plt.plot(t, yPlot[:, 2], t, kplot[:, 2], 'green')
plt.ylabel('RB')
plt.axis([0, 10, -2, 2])  # XY坐标均衡
plt.grid(True)

plt.subplot(4, 1, 4)
plt.plot(t, yPlot[:, 3], t, kplot[:, 3], 'green')
plt.xlabel('时间（t/s）')
plt.ylabel('LB')
plt.axis([0, 10, -2, 2])  # XY坐标均衡
plt.grid(True)
plt.show()



def returnyfe(uf, ue, legnum):
    yf = np.zeros((legnum, 1))
    ye = np.zeros((legnum, 1))
    for i in range(legnum):
        yf[i] = max(uf[i], 0)
        ye[i] = max(ue[i], 0)
    return yf, ye

def kimura_stateq(Tr, Ta, uf, vf, ue, ve, yf, ye, a, b, c, wyf_sum, wye_sum, shsum):
    uf_dot = (1 / Tr) * (-uf + b * vf + a * ye + wyf_sum + shsum + c)
    vf_dot = (1 / Ta) * (yf - vf)
    ue_dot = (1 / Tr) * (-ue + b * ve + a * yf + wye_sum + shsum + c)
    ve_dot = (1 / Ta) * (ye - ve)
    return uf_dot, vf_dot, ue_dot, ve_dot

def wye_sumeq(w, legnum, yfaugment, yeaugment):
    WYf = np.zeros((legnum, 1))
    WYe = np.zeros((legnum, 1))
    for i_i in range(legnum):
        for j_j in range(legnum):
            WYf[i_i] = WYf[i_i] + w[i_i, j_j] * yfaugment[j_j]
            WYe[i_i] = WYe[i_i] + w[i_i, j_j] * yeaugment[j_j]
    return WYf, WYe

这段代码实现了 Kimura 振荡器，用于控制四足机器人的步态。代码包含以下步骤：

参数设置：设置足的个数、反馈信息的项数、上升时间参数、直流输入、适应系数、抑制系数等参数。
步态矩阵定义：根据选择的步态，定义步态矩阵，该矩阵用于描述每个足的运动顺序。
状态方程求解：利用 Kimura 振荡器方程，计算每个足的关节角度变化率。
关节角度计算：根据计算出的关节角度变化率，积分得到每个足的关节角度。
可视化结果：将每个足的关节角度随时间变化的曲线绘制出来。

代码中包含三个辅助函数：returnyfe() 用于计算每个足的关节角度，kimura_stateq() 用于计算关节角度变化率，wye_sumeq() 用于计算步态矩阵与关节角度的乘积。

需要注意的是，代码中的部分参数需要根据实际情况进行调整，例如腿节长度、抬腿高度等。此外，代码仅实现了基础的步态控制，如果需要实现更复杂的步态，需要进行进一步的修改和扩展。

这段代码可以作为学习和参考，帮助您理解 Kimura 振荡器在四足机器人步态控制中的应用。