Matlab 到 Python：四足机器人步态控制算法代码转换

以下是将 Matlab 代码转换成 Python 代码的结果：

import numpy as np

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


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 in range(legnum):
        for j in range(legnum):
            WYf[i] += w[i, j] * yfaugment[j]
            WYe[i] += w[i, j] * yeaugment[j]
    return WYf, WYe


def shsumeq(s, legnum, haugment):
    shsum = np.zeros((legnum, 1))
    for i in range(legnum):
        for j in range(legnum):
            shsum[i] += s[i, j] * haugment[j]
    return shsum

该代码使用 NumPy 库进行矩阵运算和向量操作。

leg_num：足的个数
mm：m项反馈信息
Tr：上升时间参数
Ta：下降时间参数
c：直流输入
b：适应系数
a：抑制系数
phai：相位
gait：步态类型
w_w：步态矩阵
uf，ue：前腿和后腿的输入
vf，ve：前腿和后腿的速度
yf，ye：前腿和后腿的输出
Y：关节映射矩阵
h：抬腿高度
v：行走速度
T：步态周期
S：步长
l：腿节长度
theta0：髋关节和膝关节平衡位置与垂直线夹角
L：髋关节与足端之间长度
Ah：髋关节摆动幅度
Ak：膝关节摆动幅度
u0：初始值，会影响相序
n：时间步长
t_total：总时间
t：时间向量
ts：时间步长
trace_Y_Y：轨迹记录矩阵

关键函数：

returnyfe(uf, ue, legnum)：计算前腿和后腿的输出，即对输入值进行非负值处理
kimura_stateq(Tr, Ta, uf, vf, ue, ve, yf, ye, a, b, c, wyf_sum, wye_sum, shsum)：计算前腿和后腿的输入和速度的变化率，即根据当前状态和反馈信息进行状态更新
wye_sumeq(w, legnum, yfaugment, yeaugment)：计算步态矩阵与前腿和后腿输出的乘积，即反馈信息的计算
shsumeq(s, legnum, haugment)：计算传感器反馈信息与对应系数的乘积，即反馈信息的计算

代码中的循环用于模拟四足机器人的运动，计算每个时间步长下的状态变化。最终，代码将输出每个时间步长下各个足的关节角和膝关节角的变化。

该代码仅供参考，实际应用中需要根据具体情况进行调整和修改。