自平衡机器人实验报告

线性系统理论题目：自平衡机器人实验报告学生姓名：李佳鹏王一帆董昊学号：S201602215S201602212S201602216专业：控制工程指导老师：龚道雄2016年12月11日自平衡机器人是一种特殊的轮式移动机器人，它运动灵活，成本低，适合在狭小和危险的空间工作，可以零半径转弯，有着广泛的应用前景。同时，自平衡机器人作为一个本征不稳定典型控制系统，具有多变量、高阶次、非线性和参数不确定特点，也是一种复杂的研究性实验装置，已成为理想的控制理论和控制技术研究的实验平台。本实验是利用牛顿运动定律对该系统进行数学建模，并对其线性化处理，得到相应的状态方程。画系统的开闭环曲线，并分析零极点，在此基础之上详细介绍了李雅普诺夫的方法分析开闭环的稳定性，线性

二次型设计控制方法等。自平衡机器人又称为轮式倒立摆机器人，其特征在于其重心处于轮轴上方，形成一个倒立摆的构型，是一种典型的非线性、欠驱动、静态不稳定系统。标准的自平衡机器人由左轮、右轮、位于中部的车体三部分组成，其重心相对车体固定，并且一般运行在平坦水平面上。早期对这类机器人的研究集中在动力学建模、平衡控制算法、传感器方案、系统构建等方面，着重研究如何将自平衡机器人从概念变为实际的动态平衡机器人系统。随着研究的深入，研究者们开发了各式各样的改进型自平衡机器人。通过在标准的自平衡机器人上增加执行机构，世界各国学者研究了利用自平衡机器人对物体进行操作、搬运。另外，随着自平衡机器人运用到交通通勤、娱乐、监控等多领域，研究者们逐渐深入研究了不同外部环境对机器人控制的挑战，如上下坡、台阶、头顶障碍、低附着力地面等条件下自平衡机器人的平衡和行走控制问题。由于标准的自平衡机器人结构相对简单，重心相对车体的位置固定，因而其控制也比较容易实现。然而，随着机器人负载的改变、机械手等执行器的增加、以及机器人结构的改变，自平衡机器人重心也不再固定。重心位置和所处工作环境的变化给机器人的控制、机械和传感系统等方面带来了新的挑战，形成了新的研究热点。图1.1Naojishiroma的搬运机器人图1.2日本早稻田大学的机器人模型WV-2R图图1.1Naojishiroma的搬运机器人图1.2日本早稻田大学的机器人模型WV-2R图1.3Yamafuji的直立机器人图1.4机器人JOE一、实验目的1、 了解自平衡机器人的概念与原理；2、 熟悉将物理模型转化为数学模型的方法，并建模

3、掌握微分方程与传递函数模型以及状态空间模型的转化与建立；4、掌握李雅普诺夫第二方法分析系统稳定；5、熟悉LQR设计控制系统。二、实验环境实验软件：Matlab2012B实验人员：李佳鹏王一帆董昊三、实验内容、分析自平衡小车原理与数学分析建模1、数学分析模型自平衡小车的系统模型如图1所示，下方为双轮小车，小车中间有一个2l长的摆杆。n0打I图1n0打I图1数学分析模型2、 系统的设计要求：(1)摆杆处于平衡状态，给定0・05rad的脉冲干扰，系统在5s内稳定在直立平衡状态，0的初始角度为0=n。则需要设置0作用时间小于等于5s0来自垂直方向角度限制条件为始终不超过0・05rad(2)在小车处于平衡站立状态，给定一个0・2m/s的期望速度，系统在5s之内达到期望速度，上升时间不超过0・5s,同时摆杆的摆角也不超过20度的误差。转化为：设置x与0作用时间小于等于5sx的上升时间小于等于0・5s摆杆角度与垂直方向不超过20°(0・35rad)x与0稳态误差不超过2%3、 两轮自平衡小车的物理量及状态空间方程分析：根据牛顿定律，小车在水平方向上的力有下列等式：MxbxNF1)摆MxbxNF杆在水平方向遵从的表达式如下式：Nmxmlcosml2sinNmxmlcosml2sin2)将（2）带入（1），化简可得(Mm)xbxmlcosml2sinF（3为获得系统运动的二次方程，将作用在摆杆的力分析求和，得PsinNcosmgsinmlmxcos（4对于摆杆上的质心满足PlsinNlcosI(5联立上述（4）与（5）两个方程式，得(I2ml)mglsinmlxcos（6观察得到，系统是非线性化的，将系统按以下步骤实现线性化：coscos()1(7sinsin()(8220(9将上述三个等式带入（3）和（6），得线性化方程组(Iml2)mglmlx(10（Mm）xbxmlu11也可换算出系统的传递函数，(Iml2)(s)s2mgl(s)mlX(s)s2(12)(Mm)X(s)s2bX(s)sml(s)s2U(s)(13)ml2Uml2U((ss))4b(Iml2)式中q（Mm）（Iml2）（ml）2将⑩与?合并化简得到系统状态方程：20I(Mm)Mml(Iml)b2m22gl xI(Mm)Mml020I(Mm)Mml2mlb0I(Mm)Mmlmgl(Mm)2I(Mm)Mml22ImlmlI(Mm)Mml10y000x、系统的Matlab仿真根据?与?在MATLAB中进行运算。代码如下：M=0.076;Mp=0.6;J=3.42*10=5;Jp=1.34*10=2;R=0.03;L=0.15;g=9.8;alpha=0;al二J/R八2;a2=((Mp*L)^2*(cos(alpha)厂2)/(Jp+Mp*L^2);H0=Mp+M+a1-a2;A1=-(Mp八2*L^2*g*(cos(alpha)厂2)/(Jp+Mp*L^2);T1=A1/H0;d1二Mp+M+J/RV;d2=(Jp+Mp*L^2)/(Mp*L*cos(alpha));d3=-d1*d2;d=Mp*L+d3;d4=-d1*(g/cos(alpha));H1=d4/d;t1=(Mp*L*cos(alpha))/(Jp+Mp*L^2);t2=1/R+t1;T3=t2/H0;h1=(Mp+M+J/R^2)/(Mp*L*cos(alpha));h2=1/R+h1;H3=h2/d;K=[-0.1-0.097-0.543-0.081];A=[0100;00T10;0001;00H10];B=[0;T3;0;H3];C=[1000;0100;0010;0001];D=[0;0;0;0];sys=ss(A,B,C,D);A1=(A-B*K);B1=B;C1=C;D1=D;sys_c=ss(A1,B1,C1,D1);t=0:0.01:5;x0=[0;0;10;0];U=0.2*ones(size(t));[y,t,x]=lsim(sys_c,U,t,x0);figure(l);subplot(2,2,l);plot(t,y(:,l));ylabel('x');subplot(2,2,2);plot(t,y(:,2));ylabel('x0');subplot(2,2,3);plot(t,y(:,3));ylabel('theta');subplot(2,2,4);plot(t,y(:,4));ylabel('theta0');仿真得：图1当alpha=15图2当alpha=18度时

图3当alpha=20度时图4当alpha=30度时（三）、李雅普诺夫第二方法分析稳定性在使用李雅普诺夫第二方法分析系统稳定性时，构造李雅普诺夫函数时遇到了困难，由于本次实验仿真的为四阶矩阵构造的双轮自平衡机器人状态空间，所以在构造李雅普诺夫函数时，要考虑削去最多十个参量，这些参量都不能判断李雅普诺夫函数的导函数为负定。以分析系统闭环情况下稳定性为例，如图5的系统方框图，带入自平衡机器人的参量，可以得到它的状态空间形式为?：图5系统方框图Z*X[XiII■图5系统方框图Z*X[XiII■ar1I-0.18182.6727设李雅普诺夫方程为V(xp为正定矩阵，而V(x)xTQx实验中，尝试使用(18)：0xpx,中，Q为负定矩阵。-0.4545则V(x)31.1848rI■0X21.8182+X30丿申丿4.5455X z0的设计可以改变16个参量，来实现矩阵这样就对V(x)的设计要求极高。在本次V(x)axbxcxdx(18)12223242式19中由四个变量进行构造消参使得V(x)中Q为负定。由式18求导可得式19:V(x)2ax1x12bx2x22cx3x32dx4x4(19)2V(x)2axx0.3636bx5.3454bxx3.6364bxu2cxx12 22 23234()0.9090dxx62.3636dxx9.091dux42344由(20)可知四个未知参量的设定并不能通过消元使得V(x)中的Q矩阵为负定。所以若想设计出可行的李雅普诺夫函数就要把矩阵p设定为16个变量的正定矩阵，这个工作技巧性强，工作量大，在本次试验中并没有实现，是今后改正、学习的方向。、线性二次型设计控制系统首先判断系统是不是能控。用MATLAB中ctrb生成矩阵，用rank判定矩阵的秩，若矩阵的秩是满矩阵的秩则为能控系统。然后需计算状态反馈增益矩阵 K最简单的情况是先假设R=1,Q=C'*C。通过lqr指令计算出K再根据图形修改Q矩阵中值，此处根据前人经验修改Q元素中的(1,1)和(3,3)值为5000和100。这时发现系统的阶跃响应很理想。代码与结果如下：M=0.5;m=0.2;b=0.1;I=0.006;gg=9.8;l=0.3;p=I*(M+m)+M*m*「2A=[010-p=I*(M+m)+M*m*「2A=[010-(I+m*「2)*b/p000-(m*l*b)/pB=[0;(I+m*l八2)/p;(m^2*g*「2)/p0;\o"CurrentDocument"0 1;m*g*l*(M+m)/p 0];m*l/0p;];C=[1000;0010];D=[0;0];states={'x','x_dot','phi','phi_dot'};inputs={'u'};outputs={'x';'phi'};sys_ss=ss(A,B,C,D,'statename',states,'inputname',inputs,'outputname',outputs);co=ctrb(sys_ss);controllability=rank(co);Q=C'*C;Q(1,1)=5000;Q(3,3)=100R=1;K=lqr(A,B,Q,R)Ac=[(A-B*K)];Bc=[B];Cc=[C];Dc=[D];states={'x','x_dot','phi','phi_dot'};inputs={'r'};outputs={'x';'phi'};sys_cl=ss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);t=0:0.01:5;r=0.2*ones(size(t));[y,t,x]=lsim(sys_cl,r,t);[AX,H1,H2]=plotyy(t,y(:,1),t,y(:,2),'plot');set(get(AX(1),'Ylabel'),'String','cartposition(m)')set(get(AX(2),'Ylabel'),'String','pendulumangle(radians)')要想完美的系统响应，那么需要重新构造一个系统，利用原有系统中的y和u作为输入信号构成状态观测器，根据前人经验，状态观测器如下图所连接方式。

根据系统的连接框图，编写的程序如下：M二0.5;m=0.2;b=0.1;I=根据系统的连接框图，编写的程序如下：M二0.5;m=0.2;b=0.1;I=0.006;g=9.8;p=1*(M+m)+M*m*l2%denominatorfortheAandBmatricesA=[010-00;(m^2*g*「2)/p0;(I+m*「2)*b/p000-(m*l*b)/pB=[0;(I+m*l八2)/p;01;m*g*l*(M+m)/p 0];0;m*l/p];C=[1000;0010];D=[0;0];Q=C'*C;Q(1,1)=5000;Q(3,3)=100R=1;K=lqr(A,B,Q,R)Ac=[(A-B*K)];Bc=[B];Cc=[C];Dc=[D];Cn=[1000];sys_ss=ss(A,B,Cn,0);Nbar=rscale(sys_ss,K)ob=obsv(sys_ss);observability=rank(ob)poles=eig(Ac)P=[-40-41-42-43];L=place(A',C',P)'Ace=[(A-B*K)(B*K);zeros(size(A))(A-L*C)];Bce=[B*Nbar;zeros(size(B))];Cce=[Cczeros(size(Cc))];Dce=[0;0];states={'x''x_dot''phi''phi_dot''e1''e2''e3''e4'};inputs={'r'};outputs={'x';'phi'};sys_est_cl=ss(Ace,Bce,Cce,Dce,'statename',states,'inputname',inputs,'outputname',outputs);t=0:0.01:5