参考《仿生四足机器人技术》
使用python实现四足机器人CPG控制网络二,即xi作为髋关节输出,对yi输出整型作为膝关节输出。
因为没有经验,不知道输出信号是否正确,发到网上希望能够得到一些指正。
代码如下:
import numpy as np
import matplotlib.pyplot as plt
from scipy import integrate
import time
import math
def get_param():
alpha = 10 #收敛速度
leg_num = 4 #腿的数量
gait = 2 #步态选择
mu = 1
a = 50
psai = [1, 1, -1, -1] #关节形式 膝式-1 肘式 1
omega_sw = 2*np.pi
u1=0
u2=0 #误差 影响x,y平衡位置
h = 0.02 #抬腿高度
v = 1 #行走速度
gait_t = 0.4#步态周期
s = v*gait_t
l = 0.4 #腿节长度
theta0 = math.radians(30)#髋关节和膝关节平衡位置与垂直线夹角
L = 2*l*np.cos(theta0)#髋关节与足端之间长度
ah = 15 #np.arcsin((beta*s)/(2*L)) 髋关节摆动幅度
ak =10 #膝关节摆动幅度
return [alpha,leg_num,gait,mu,a,psai,omega_sw,u1,u2]
def get_gait(gait_m):
#walk
global beta, phase, v, t, h, ah, ak
if gait_m == 1:
beta=0.75
phase=[0,0.5,0.25,0.75]
v=0.3
t=0.8
h=0.02
ah=7.5
ak=5.3
#trot
elif gait_m == 2:
beta=0.5
phase=[0,0.5,0,0.5]
v = 1
t = 0.4
h = 0.02
ah = 8.3
ak = 5.3
#pace
elif gait_m == 3:
beta=0.5
phase=[0,0.5,0.5,0]
v = 1.2
t = 0.4
h = 0.02
ah = 10
ak = 5.3
#gallop
elif gait_m == 4:
beta = 0.5
phase = [0,