1.算法仿真效果
matlab2022a仿真结果如下：
2.算法涉及理论知识概要
倒立摆是一个开环不稳定的强非线性系统，其控制策略与杂技运动员顶杆平衡表演的技巧有异曲同工之处，目的在于使得摆杆处于临界稳定状态，是进行控制理论研究的典型实验平台。20世纪50年代，麻省理工学院的控制论专家根据火箭助推器原理设计出了第一套倒立摆实验设备，开启了最初的相关研究工作。倒立摆的种类丰富多样，按照其结构可将其分为：直线倒立摆、环形倒立摆以及平面倒立摆等，按照摆杆级数又可将其分为：一级、二级甚至三级等。
倒立摆系统是是一个具有高阶次、不稳定、多变量、非线性和强藕合特性的不稳定系统。在控制过程中，为了简化直线一级倒立摆系统分析，在建立实际数学模型过程中，忽略了空气阻力和各种摩擦之后，将直线一级倒立摆系统直接抽象成小车和匀质杆组成的系统，如图1所示。
M 小车质量
m 摆杆质量
b 小车摩擦系数
l 摆杆转动轴心到杆质心的长度
I 摆杆惯量
F 加在小车上的力
x 小车位置
摆杆与垂直向上方向的夹角
摆杆与垂直向下方向的夹角（考虑到摆杆初始位置为竖直向下）
其中，N——小车与摆杆相互作用力的水平方向的分量，
P——小车与摆杆相互作用力的垂直方向的分量。
以下是应用牛顿法建立直线一级倒立摆系统的动力学方程过程：
通过受力分析可知，由小车水平方向所受的合力，可得如下方程：
M*d2x/dt2=F –b*dx/dt- N （1）= - - && &
由摆杆水平方向所受的合力，可得如下方程：
N=m*d2/dt2(x+lsin)
即：N=m*d2x/dt2+m*l*d2/ dt2*cos- m* l* d2/ dt2*sin （2）
把式（2）带入式（1）中，则得到系统的第一个运动方程：
(M+m)*d2x/dt2+b*dx/dt + m*l*d2/ dt2*cos- m* l* d2/ dt2*sin=F (3)(
对摆杆垂直方向上的合力进行分析，可得如下方程:
P-mg=m*d2 (l cos)/dt2)
即：P-mg=- m*l*（d/ dt）2 * cos- m* l* d2/ dt2*sin （4）
系统力矩平衡方程如下：
-P*l* sin-N*l *cos=I* d2/ dt2 （5）
合并式（4）和式（5），消去 P 和 N，则得到系统的第二个运动方程：
（I+ml2）d2/ dt2+mgl sin=-ml d2x/dt2 cos （6）
由于=π+，cos=- cos，sin=- sin， 所以由式（3）和式（6）可得
系统的数学模型为如下方程组：
（M+m）d2x/dt2+b*dx/dt- m*l*d2/ dt2*cos+ m* l* d2/ dt2*sin=F (7)(
（I+ml2）d2/ dt2+mgl sin=-ml d2x/dt2 cos
由方程组（7）知该系统是明显的非线性系统。为便于控制器的设计，需要将系统在工作点（= 0）进行线性化处理。当摆杆与垂直向上方向之间的夹角与 1（单位为弧度）之比很小时，则可以进行近似处理：
cos=-1，sin=-，（d/ dt）2=0。用 u 表示被控对象的输入力 F，经线性化处理后系统的数学模型成为如下微分方程表达式：
（I+ml2）d2/ dt2+mgl sin=-ml d2x/dt2 （8）
(M+m)*d2x/dt2+b*dx/dt- m*l* m*l*d2/ dt2=u （8）
1.系统传递函数模型：
3.MATLAB核心程序
for i=1:TT
z1 =(A-B*Ks)*(z-zf);
z = z + z1*dt;
y(:,i) = z;
%动画
x1 = z(3)-k/2+z(4)*dt*i;x2=z(3)+k/2+z(4)*dt*i;
y1 =-4;
y2 =-2.5;
x4 =(x1+x2)/2+l*sin(z(1));
y4 = y2+l*cos(z(1));
t = 0:pi/100:2*pi;
%倒立摆上的球体
x3 = x4+r*cos(t);
y3 = y4+r*sin(t);
axes(handles.axes1);
plot(x3,y3,'ro',...
'LineWidth',1,...
'MarkerSize',6,...
'MarkerEdgeColor','r',...
'MarkerFaceColor',[0.9,0.9,0.0]);
hold on
%轨道
plot([-40,40],[-4,-4],'k','linewidth',20);
hold on
%模拟小车
plot([x1,x2],[y1,y1],'b','linewidth',2);
hold on
plot([x2,x2],[y1,y2],'b','linewidth',2);
hold on
plot([x1,x2],[y2,y2],'b','linewidth',2);
hold on
plot([x1,x1],[y1,y2],'b','linewidth',2);
hold on
%倒立摆
plot([(x1+x2)/2,x4],[y2,y4],'g','linewidth',3);
hold off
xlabel('小车位移');
ylabel('高度');
axis([-30 30 -5 15])
Y = [Y,(x1+x2)/2];
axes(handles.axes2);
plot(Y,'r','linewidth',2);
axis([0,TT,-15,15]);
xlabel('时间');
ylabel('小车位移');
pause(0.001);
if (i==1)
pause(0.01);
end
if (abs(z(1)-zf(1))<=0.001)&(abs(z(3)-zf(3))<=0.002)
break;
end
end
% --- Executes on button press in pushbutton8.
function pushbutton8_Callback(hObject, eventdata, handles)
% hObject handle to pushbutton8 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
%参数
global R1;
global I1;
global R2;
global I2;
global R3;
global I3;
global R4;
global I4;
global M;
global k;
global V;
global dt;
global l;
global TT;
CC1 = R1 + sqrt(-1)*I1;
CC2 = R2 + sqrt(-1)*I2;
CC3 = R3 + sqrt(-1)*I3;
CC4 = R4 + sqrt(-1)*I4;
fid = fopen('双质量摆动系统虚拟试验软件测试报告.txt','wt');
fprintf(fid,'试验参数： \n');
fprintf(fid,'01、小车宽度： ');fprintf(fid,'%2.2f\n',k);
fprintf(fid,'02、小车速度： ');fprintf(fid,'%2.2f\n',V);
fprintf(fid,'03、倒立摆长度：');fprintf(fid,'%2.2f\n',l);
fprintf(fid,'04、小球质量： ');fprintf(fid,'%2.2f\n',M);
if I1 > 0
fprintf(fid,'05、零极点配置：');fprintf(fid,'%2.2f',R1); fprintf(fid,'+i*'); fprintf(fid,'%2.2f\n',abs(I1));
else
fprintf(fid,'05、零极点配置：');fprintf(fid,'%2.2f',R1); fprintf(fid,'-i*'); fprintf(fid,'%2.2f\n',abs(I1));
end
if I2 > 0
fprintf(fid,' ');fprintf(fid,'%2.2f',R2); fprintf(fid,'+i*'); fprintf(fid,'%2.2f\n',abs(I2));
else
fprintf(fid,' ');fprintf(fid,'%2.2f',R2); fprintf(fid,'-i*'); fprintf(fid,'%2.2f\n',abs(I2));
end
if I3 > 0
fprintf(fid,' ');fprintf(fid,'%2.2f',R3); fprintf(fid,'+i*'); fprintf(fid,'%2.2f\n',abs(I3));
else
fprintf(fid,' ');fprintf(fid,'%2.2f',R3); fprintf(fid,'-i*'); fprintf(fid,'%2.2f\n',abs(I3));
end
if I4 > 0
fprintf(fid,' ');fprintf(fid,'%2.2f',R4); fprintf(fid,'+i*'); fprintf(fid,'%2.2f\n',abs(I4));
else
fprintf(fid,' ');fprintf(fid,'%2.2f',R4); fprintf(fid,'-i*'); fprintf(fid,'%2.2f\n',abs(I4));
end
fprintf(fid,'06、模拟时间： ');fprintf(fid,'%2.2f\n\n',TT);
fprintf(fid,'试验结果： \n');
fclose(fid);