四足机器人-cpg控制&Hopf振荡器的一种python实现

孤街醉人 提交于 2020-02-27 04:22:24

一、python 解微分方程

python有用于求解常微分方程的库,库里面提供了不少功能,例如sympy的dsolve,以及scipy.integrate.odeint。其具体用法可参考官方文档,这里就不过多赘述,举两个例子看下效果

1、利用dsolve求解析解

f(x)+f(x)=0{f(x)}'' + f(x) = 0

import sympy as sy
 
def fun(x,f):
    return sy.diff(f(x),x,2)+f(x) 
x=sy.symbols('x') # 变量符号
f=sy.Function('f') # 函数符号
sy.pprint(sy.dsolve(fun(x,f),f(x))) # 打印等式

我们能得到以下方程

f(x) = C₁⋅sin(x) + C₂⋅cos(x)

c1=1,c2=1c_1=1, c_2=1,我们用matplotlab把他的图像画出来:

t = np.arange(0, 5, 0.001)
y = np.sin(t) + np.cos(t)
figure = plt.figure()
plt.grid(ls="--")
plt.plot(t, y)
plt.show()

2、利用scipy.integrate.odeint求解

我们先尝试一阶的微分方程:dydt=y\frac{dy}{dt} = y,这条微分方程我们手动算的话很容易的出y=cexy=ce^x,注意使用odeint的话,微分方程必须化成标准形式,fun函数必须返回dy。

import matplotlib.pyplot as plt
import numpy as np
from scipy.integrate import odeint

def fun(y, t):
    return np.array(y)

t = np.arange(0, 5, 0.001)
result = odeint(fun, 1, t)
plt.plot(t, result[:, 0])
plt.grid()
plt.show()

对于二阶,如上一个例子所使用的方程,我们需要把他拆分成两个一阶微分方程,y=z,y=z=yy'= z, y'' = z'=-y,即:

{dydt=zdzdt=y\left\{\begin{matrix} \frac{dy}{dt} &= z\\ \\ \frac{dz}{dt} &= -y\\ \end{matrix}\right.

import matplotlib.pyplot as plt
import numpy as np
from scipy.integrate import odeint


def fun(y_list, t):

    y, z = y_list
    dz = -y
    dy = z
    return dy, dz


t = np.arange(0, 5, 0.001)
y0 = [1, 1]
result = odeint(fun, y0, t)
plt.plot(t, result[:, 0])
plt.grid()
plt.show()

我们能得到相同的图案:

二、hopf振荡器实现

之前已经介绍过hopf振荡器的数学模型了,是一组微分方程
dxdt=α(μr2)xωy\frac{dx}{dt} = \alpha(\mu - r^2)x - \omega y

dydt=α(μr2)y+ωx\frac {dy}{dt} = \alpha(\mu-r^2)y+\omega x下面我们用两种不同的方法去实现

1、numpy

这种方法比较传统,单纯地依靠numpy本身来进行运算,可以实现比较灵活改动。甚至能获取单个时间点的位置

def hopf(pos, steps, parms):
    x, y = pos
    alpha, a, mu, beta, omega = parms
    r_square = x**2 + y**2
    dx = alpha * (mu - r_square) * x - omega * y
    dy = alpha * (mu - r_square) * y + omega * x
    return [x+dx*steps, y+dy*steps]

我们只要通过以下方式调用就能获取到数据

d = []
for i in t:
    p = hopf(p, time_steps, parms)
    d.append(p)

2、odeint

这里跟第一章节差异不大,都是构造微分方程,然后计算数值解

    def hopf(self, pos, time_steps):
        x, y = pos
        alpha, a, mu, beta, omega_sw = self.get_parms()
        r_square = x**2 + y**2
        # omega = np.pi
        omega_st = ((1 - self.beta) / self.beta) * self.omega_sw
        omega = omega_st / (np.e ** (-a * y) + 1) + omega_sw / (np.e ** (a * y) + 1)
        dx = alpha * (mu - r_square) * x - omega * y
        dy = alpha * (mu - r_square) * y + omega * x
        return dx, dy
def calculate(self, t):
	data = integrate.odeint(self.hopf, self.p0, t)
    return data
    
t = np.arange(0, 5, self.time_steps)
date = self.calculate(t)

我们将其写成一个类,完整代码如下:

import numpy as np
import matplotlib.pyplot as plt
from scipy import integrate
import time

# 0.027
class Cpg(object):
    def __init__(self, alpha=100, a=50,
                 mu=1, beta=0.5, omega_sw=2*np.pi,
                 time_steps=0.01):
        '''
		初始化参数
		'''
        self.time_steps = time_steps
        self.alpha = alpha
        self.a = a
        self.mu = mu
        self.beta = beta
        self.omega_sw = omega_sw
        self.p0 = [1, 0]


    def hopf(self, pos, time_steps):
    	'''
		hopf振荡器数学模型
		'''
        x, y = pos
        alpha, a, mu, beta, omega_sw = self.get_parms()
        r_square = x**2 + y**2
        # omega = np.pi
        omega_st = ((1 - self.beta) / self.beta) * self.omega_sw
        omega = omega_st / (np.e ** (-a * y) + 1) + omega_sw / (np.e ** (a * y) + 1)
        dx = alpha * (mu - r_square) * x - omega * y
        dy = alpha * (mu - r_square) * y + omega * x

        return dx, dy

    def get_parms(self):
    	'''
    	获取模型参数
    	'''
        return self.alpha, self.a, self.mu, self.beta, self.omega_sw

    def calculate(self, t):
    	'''
    	调用振荡器函数,返回数据序列
    	'''
        data = integrate.odeint(self.hopf, self.p0, t)
        return data

    def show(self):
    	'''
    	将信号画出来
    	'''
        t = np.arange(0, 5, self.time_steps)
        t0 = time.time()
        date = self.calculate(t)
        t1 = time.time()
        fig1 = plt.figure()
        print('time: ', t1-t0)
        plt.plot(t, date[:, 0])
        plt.plot(t, date[:, 1])
        plt.show()

        fig2 = plt.figure()
        plt.axis('equal')
        plt.plot(date[:, 0], date[:, 1])
        plt.show()


if __name__ == '__main__':
    signal = Cpg()
    signal.show()

在这里插入图片描述

在这里插入图片描述

易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!