使用python代替matlab仿真线性控制系统（倒立摆）

matlab可以仿真很多控制系统，其实python也有这种中功能。不仅是基础的自动控制原理所涉及的定理如伯德图，奈奎斯特曲线，pid之类的能够仿真，较为复杂的线性系统理论上面的一些原理也可以仿真。

这是对旋转式倒立摆进行一个简单的介绍

随后对倒立摆进行建模，利用牛顿定律和拉格朗日定律建模

以上是对于倒立摆系统进行简单的介绍和matlab仿真，下面程序是将matlab转换成python的
除了使用numpy库和matplotlib库之外，还用到control和slycot，这个库比较难装，需要用anaconda添加清华大学的镜像pip安装，别的方法试了试都不好用，要么装不上要么装上了用不了。

// A code block
var foo = 'bar';

import control
import numpy as np
import matplotlib.pyplot as plt
A = np.array([[0,0,1,0],[0,0,0,1],[65.875,-16.875,-0.00010191,-1.248e-006],[-82.212,82.212,-2.0918e-005,-1e-006]])
b = np.array([[0],[0],[5.2184],[-6.5125]])
c = np.mat([[1,0,0,0],[0,1,0,0]])
d = np.array([[0],[0]])
P = np.array([-5+2j,-5-2j,-8-6j,-8+6j])#desire pole
sys = control.StateSpace(A, b, c, d)#现代控制系统的基本形式
sysd = sys.sample(0.5, method='bilinear')
print(sys)
print(np.linalg.matrix_rank(control.ctrb(A, b)))
print(np.linalg.matrix_rank(control.obsv(A, c)))
h,i = np.linalg.eig(A)
print(format(h)) #特征值
print(control.place(A, b, P))
P1 = np.array([-50+3j,-50-3j,-80-6j,-80+6j])
print(control.place(A, b, P))#get k
L = control.place(A.transpose(),c.transpose(), P1)
print(L.transpose())#观测矩阵极点

非线性仿真
下面展示一些 内联代码片。

// A code block
var foo = 'bar';

// An highlighted block
import numpy as np
import math
from scipy.integrate import odeint
from pylab import *
import matplotlib.pyplot as plt
m1=0.200
m2=0.052 
L1=0.10
L2=0.12 
r=0.20 
km=0.0236
ke=0.2865
g=9.8 
J1=0.004 
J2=0.001 
f1=0.01 	
f2=0.001
a=J1+m2*r*r 
b=m2*r*L2
c=J2 
d=f1+km*ke 
e=(m1*L1+m2*r)*g  
f=f2  
h=m2*L2*g
print(a,b,c,d,e,f,h)
def dflun(y,t,a,b,c,d,e,f,g,h):
	sita1,sita2,w1,w2 = y
	K = np.mat([[  0.58498033 ,-69.40930131 , -5.2454833 ,  -8.19545906]])
	xx = np.array([[sita1],[sita2],[w1],[w2]])
	us = np.dot(-K,xx)
	u = us[0,0]
	dydt = [w1,w2,((-d*c)*w1+(f*b*math.cos(sita2-sita1))*w2+b*b*math.sin(sita2-sita1)*math.cos(sita2-sita1)*w1*w1-b*c*math.sin(sita1-sita2)*w2*w2+e*c*math.sin(sita1)-h*b*math.sin(sita2)*math.cos(sita2-sita1)+km*c*u)/(a*c-b*b*math.cos(sita1-sita2)*math.cos(sita2-sita1)),((d*b*math.cos(sita1-sita2))*w1-(a*f)*w2-a*b*math.sin(sita2-sita1)*w1*w1+b*b*math.sin(sita1-sita2)*math.cos(sita1-sita2)*w2*w2-e*b*math.sin(sita1)*math.cos(sita1-sita2)+a*h*math.sin(sita2)-b*math.cos(sita1-sita2)*km*u)/(a*c-b*b*math.cos(sita1-sita2)*math.cos(sita2-sita1))]
	return dydt
y0=[-0.1,0.05,0,0]
t = np.linspace(0, 10)
sol = odeint(dflun, y0, t, args=(a, b,c,d,e,f,g,h))#将微分方程解，该函数官方库或者百度有详细的解释
plt.plot(t, sol[:, 0], 'b', label='angle1(t)')
plt.plot(t, sol[:, 1], 'g', label='angle2(t)')
plt.plot(t, sol[:, 2], 'r', label='w1(t)')
plt.plot(t, sol[:, 3], 'y', label='w2(t)')
plt.legend(loc='best')
plt.xlabel('t')
plt.grid()
plt.show()

最后画出来的

还可以画控制反馈阶跃响应的图来对齐进行分析

下面展示一些 内联代码片。

// A code block
var foo = 'bar';

// An highlighted block
import control
import numpy as np
import matplotlib.pyplot as plt
import slycot
A = np.array([[0,0,1,0],[0,0,0,1],[65.875,-16.875,-0.00010191,-1.248e-006],[-82.212,82.212,-2.0918e-005,-1e-006]])
b = np.array([[0],[0],[5.2184],[-6.5125]])
c = np.mat([[1,0,0,0],[0,1,0,0]])
d = np.array([[0],[0]])
P = np.array([-5+3j,-5-3j,-8-7j,-8+7j])#desire 
K = np.array([[  3.56808799, -59.64866393,  -4.15501195 , -7.01457373]])
v = A-np.dot(b,K)
#sys = control.ss(v, b, c, d)#feedback sysrtem
sys = control.ss(A, b, c, d)#non-feedback system

#sysd = sys.sample(0.5, method='bilinear')
#print(sys)
#print(np.linalg.matrix_rank(control.ctrb(A, b)))
#print(np.linalg.matrix_rank(control.obsv(A, c)))
h,i = np.linalg.eig(A)
#print(format(h))
#print(control.place(A, b, P))
P1 = np.array([-50+3j,-50-3j,-80-6j,-80+6j])
#print(control.place(A, b, P))#get k
L = control.place(A.transpose(),c.transpose(), P1)
#print(L.transpose())
syss = control.tf(sys)
tfinal = 5
d = np.linspace(0, 1, 500)
t, rec = control.step_response(sys, d,)
plt.plot(t, rec[0,:],'g',label='angle1')
plt.plot(t, rec[1,:],'b',label='angle2')
plt.legend(loc='best')
plt.xlabel('t')
plt.ylabel('angle')

以上我们的仿真任务就完成了，为了更好的说明极点的选取与超调量之间的关系，我们还可以用极点实部为x轴，虚部为y轴，其中一个超调为z坐标画出散点图来
下面展示一些 内联代码片。

// A code block
var foo = 'bar';

// An highlighted block
import numpy as np
import math
from scipy.integrate import odeint
from pylab import *
import matplotlib.pyplot as plt
import control

from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import pandas as pd
fig = plt.figure()
ax = Axes3D(fig)
for real in range(1,7):
    for imaginary in range(2,7):
        m1=0.200
        m2=0.052 
        L1=0.10
        L2=0.12 
        r=0.20 
        km=0.0236
        ke=0.2865
        g=9.8 
        J1=0.004 
        J2=0.001 
        f1=0.01 	
        f2=0.001
        a=J1+m2*r*r 
        b=m2*r*L2
        c=J2 
        d=f1+km*ke 
        e=(m1*L1+m2*r)*g  
        f=f2  
        h=m2*L2*g
        A = np.array([[0,0,1,0],[0,0,0,1],[65.875,-16.875,-0.00010191,-1.248e-006],[-82.212,82.212,-2.0918e-005,-1e-006]])
        bb = np.array([[0],[0],[5.2184],[-6.5125]])
        cc = np.mat([[1,0,0,0],[0,1,0,0]])
        dd = np.array([[0],[0]])
        P = np.array([-real+imaginary*1j,-real-imaginary*1j,-8-6j,-8+6j])
        sys = control.StateSpace(A, bb, cc, dd)
        #sysd = sys.sample(0.5, method='bilinear')
        KS = control.place(A,bb,P)
        K0=KS[0,0]
        K1=KS[0,1]
        K2=KS[0,2]
        K3=KS[0,3]
        def dflun(y,t,a,b,c,d,e,f,h,K0,K1,K2,K3):
            sita1,sita2,w1,w2 = y
            K=np.mat([K0,K1,K2,K3])
            xx = np.array([[sita1],[sita2],[w1],[w2]])
            us = np.dot(-K,xx)
            u = us[0,0]
            dydt = [w1,w2,((-d*c)*w1+(f*b*math.cos(sita2-sita1))*w2+b*b*math.sin(sita2-sita1)*math.cos(sita2-sita1)*w1*w1-b*c*math.sin(sita1-sita2)*w2*w2+e*c*math.sin(sita1)-h*b*math.sin(sita2)*math.cos(sita2-sita1)+km*c*u)/(a*c-b*b*math.cos(sita1-sita2)*math.cos(sita2-sita1)),((d*b*math.cos(sita1-sita2))*w1-(a*f)*w2-a*b*math.sin(sita2-sita1)*w1*w1+b*b*math.sin(sita1-sita2)*math.cos(sita1-sita2)*w2*w2-e*b*math.sin(sita1)*math.cos(sita1-sita2)+a*h*math.sin(sita2)-b*math.cos(sita1-sita2)*km*u)/(a*c-b*b*math.cos(sita1-sita2)*math.cos(sita2-sita1))]
            return dydt
        y0=[-0.1,0.05,0,0]
        t = np.linspace(0, 10)
        sol = odeint(dflun, y0, t,args=(a,b,c,d,e,f,h,K0,K1,K2,K3))
       # plt.plot(t, sol[:, 0], 'b', label='angle1(t)')
      #  print('angle1',max(sol[:, 0])),
        #plt.plot(1,1,1,1, 1, 1, markevery=[1,1,1])  
        #ax.scatter(real, imaginary,max(sol[:, 0]) , c='r', label=format(max(sol[:, 0]), '.4f'))
        ax.scatter(real, imaginary,max(sol[:, 0]) , c='r')
        ax.text(real,imaginary,max(sol[:, 0]),format(max(sol[:, 0]), '.4f'))
        #ax.annotate('abc',(1,1))     
       # plt.text(-2,2,1,r'$this\ is\ a\ function$',fontdict={'size':12,'color':'purple'}
        # plt.text(5,1, 10, t, fontsize=18, style='oblique', ha='center',va='top',wrap=True)
        #plt.plot(t, sol[:, 1], 'g', label='angle2(t)')
    #plt.plot(t, sol[:, 2], 'r', label='w1(t)')
    #plt.plot(t, sol[:, 3], 'y', label='w2(t)')
    #plt.legend(loc='best')
    #plt.xlabel('t')
   # plt.grid()

ax.legend(loc='best')
ax.set_xlabel('real part')
ax.set_ylabel('imaginary part')
ax.set_zlabel('angel 1 max')
plt.show()

最后画出来的图如下，这样分析起来就简便多了

以上只是简单的介绍，如果想查看仿真控制系统的详细信息请看：https://python-control.readthedocs.io/en/latest/control.html
关于对旋转式倒立摆介绍这一部分转载至
转至线性系统理论 第一版 科学出版社 陆军，王晓凌编著