自平衡机器人实验报告.docx

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

2、统的开闭环曲线，并分析零极点，在此基础之上详细介绍了李雅普诺夫的方法分析开闭环的稳定性，线性二次型设计控制方法等。自平衡机器人又称为轮式倒立摆机器人，其特征在于其重心处于轮轴上方，形成一个倒立摆的构型，是一种典型的非线性、欠驱动、静态不稳定系统。标准的自平衡机器人由左轮、右轮、位于中部的车体三部分组成，其重心相对车体固定，并且一般运行在平坦水平面上。早期对这类机器人的研究集中在动力学建模、平衡控制算法、传感器方案、系统构建等方面，着重研究如何将自平衡机器人从概念变为实际的动态平衡机器人系统。随着研究的深入，研究者们开发了各式各样的改进型自平衡机器人。通过在标准的自平衡机器人上增加执行机构，世界

3、各国学者研究了利用自平衡机器人对物体进行操作、搬运。另外，随着自平衡机器人运用到交通通勤、娱乐、监控等多领域，研究者们逐渐深入研究了不同外部环境对机器人控制的挑战，如上下坡、台阶、头顶障碍、低附着力地面等条件下自平衡机器人的平衡和行走控制问题。由于标准的自平衡机器人结构相对简单，重心相对车体的位置固定，因而其控制也比较容易实现。然而，随着机器人负载的改变、机械手等执行器的增加、以及机器人结构的改变，自平衡机器人重心也不再固定。重心位置和所处工作环境的变化给机器人的控制、机械和传感系统等方面带来了新的挑战，形成了新的研究热点。 本文的设计仿真实验采用了经典的理论对“JOE: A Mobile,

4、Inverted Pendulum” 这篇文章仿真进行具体的控制，流程是是机械模型的建立-数学模型的建立-模型的线性化-机器人的运动学分析-控制器的设计李雅普诺夫进行开环、闭环稳定性-二次型分析。几种常见的机器人模型如下图所示：图1.1 Naojishiroma的搬运机器人 图1.2 日本早稻田大学的机器人模型WV-2R图1.3 Yamafuji的直立机器人 图 1.4 机器人JOE一、实验目的1、了解自平衡机器人的概念与原理；2、熟悉将物理模型转化为数学模型的方法，并建模；3、掌握微分方程与传递函数模型以及状态空间模型的转化与建立；4、掌握李雅普诺夫第二方法分析系统稳定；5、熟悉LQR设计控

5、制系统。二、实验环境实验软件：Matlab2012B实验人员：李佳鹏 王一帆 董昊三、实验内容（一）、分析自平衡小车原理与数学分析建模1、数学分析模型自平衡小车的系统模型如图1所示，下方为双轮小车，小车中间有一个2l长的摆杆。图1 数学分析模型2、系统的设计要求：（1）摆杆处于平衡状态，给定0.05rad的脉冲干扰，系统在5s内稳定在直立平衡状态，的初始角度为。则需要 设置作用时间小于等于5s 来自垂直方向角度限制条件为始终不超过0.05rad（2）在小车处于平衡站立状态，给定一个0.2m/s的期望速度，系统在5s之内达到期望速度，上升时间不超过0.5s，同时摆杆的摆角也不超过20度的误差。转

6、化为： 设置x与作用时间小于等于5s x的上升时间小于等于0.5s 摆杆角度与垂直方向不超过20（0.35rad） x与稳态误差不超过2%3、两轮自平衡小车的物理量及状态空间方程分析：根据牛顿定律，小车在水平方向上的力有下列等式： （1）摆杆在水平方向遵从的表达式如下式： （2）将（2）带入（1），化简可得 （3）为获得系统运动的二次方程，将作用在摆杆的力分析求和，得 （4）对于摆杆上的质心满足 （5）联立上述（4）与（5）两个方程式，得 （6）观察得到，系统是非线性化的，将系统按以下步骤实现线性化： （7） （8） （9）将上述三个等式带入（3）和（6），得线性化方程组 （10） （11）也

7、可换算出系统的传递函数， （12） （13） （14）式中将与合并化简得到系统状态方程：（15） （16）（二）、系统的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;a1=J/R2;a2=(Mp*L)2*(cos(alpha)2)/(Jp+Mp*L2);H0=Mp+M+a1-a2; A1=-(Mp2*L2*g*(cos(alpha)2)/(Jp+Mp*L2);T1=A1/H0; d1=Mp+M+J/R2;d2=(Jp+Mp*L2

8、)/(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*L2);t2=1/R+t1;T3=t2/H0;h1=(Mp+M+J/R2)/(Mp*L*cos(alpha);h2=1/R+h1;H3=h2/d; K=-0.1 -0.097 -0.543 -0.081;A=0 1 0 0;0 0 T1 0;0 0 0 1;0 0 H1 0;B=0;T3;0;H3;C=1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1;D=0;0;0;0;sys=ss(A,B

9、,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(1);subplot(2,2,1);plot(t,y(:,1);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)

10、;仿真得：图1 当alpha=15度时图2 当alpha=18度时图3当alpha=20度时图4当alpha=30度时（三）、李雅普诺夫第二方法分析稳定性在使用李雅普诺夫第二方法分析系统稳定性时，构造李雅普诺夫函数时遇到了困难，由于本次实验仿真的为四阶矩阵构造的双轮自平衡机器人状态空间，所以在构造李雅普诺夫函数时，要考虑削去最多十个参量，这些参量都不能判断李雅普诺夫函数的导函数为负定。以分析系统闭环情况下稳定性为例，如图5的系统方框图，带入自平衡机器人的参量，可以得到它的状态空间形式为：图5系统方框图 （17）设李雅普诺夫方程为，则的设计可以改变16个参量，来实现矩阵p为正定矩阵，而中，Q为负

11、定矩阵。这样就对V(x)的设计要求极高。在本次实验中，尝试使用（18）： （18）式19中由四个变量进行构造消参使得中Q为负定。由式18求导可得式19： （19） （20）由（20）可知四个未知参量的设定并不能通过消元使得中的Q矩阵为负定。所以若想设计出可行的李雅普诺夫函数就要把矩阵p设定为16个变量的正定矩阵，这个工作技巧性强，工作量大，在本次试验中并没有实现，是今后改正、学习的方向。（四）、线性二次型设计控制系统首先判断系统是不是能控。用MATLAB中ctrb生成矩阵，用rank判定矩阵的秩，若矩阵的秩是满矩阵的秩则为能控系统。然后需计算状态反馈增益矩阵K，最简单的情况是先假设R=1，Q=

12、C*C。通过lqr指令计算出K，再根据图形修改Q矩阵中值，此处根据前人经验修改Q元素中的（1,1）和（3,3）值为5000和100。这时发现系统的阶跃响应很理想。代码与结果如下：M = 0.5;m = 0.2;b = 0.1;I = 0.006;g = 9.8;l = 0.3;p = I*(M+m)+M*m*l2; %denominator for the A and B matricesA = 0 1 0 0; 0 -(I+m*l2)*b/p (m2*g*l2)/p 0; 0 0 0 1; 0 -(m*l*b)/p m*g*l*(M+m)/p 0;B = 0; (I+m*l2)/p; 0;

13、m*l/p;C = 1 0 0 0; 0 0 1 0;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

14、 = 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,cart position (m)set(get(AX(2)

15、,Ylabel),String,pendulum angle (radians) 要想完美的系统响应，那么需要重新构造一个系统，利用原有系统中的y和u作为输入信号，构成状态观测器，根据前人经验，状态观测器如下图所连接方式。图6状态观测器根据系统的连接框图，编写的程序如下：M = 0.5;m = 0.2;b = 0.1;I = 0.006;g = 9.8;l = 0.3;p = I*(M+m)+M*m*l2; %denominator for the A and B matricesA = 0 1 0 0; 0 -(I+m*l2)*b/p (m2*g*l2)/p 0; 0 0 0 1; 0 -(

16、m*l*b)/p m*g*l*(M+m)/p 0;B = 0; (I+m*l2)/p; 0; m*l/p;C = 1 0 0 0; 0 0 1 0;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 = 1 0 0 0;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

17、-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 = Cc zeros(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;

18、r = 0.2*ones(size(t);y,t,x=lsim(sys_est_cl,r,t);AX,H1,H2 = plotyy(t,y(:,1),t,y(:,2),plot);set(get(AX(1),Ylabel),String,cart position (m)set(get(AX(2),Ylabel),String,pendulum angle (radians)title(Step Response with Observer-Based State-Feedback Control)4、实验心得 这个学期我们学习了线性系统理论这门课程，它是一门综合应用相关课程的知识和内容来解决

19、控制学方面问题的课程。通过这次仿真实验,使我学到了不少实用的知识,更重要的是,做实验的过程,思考问题的方法,这与做其他的实验是通用的,真正使我们受益匪浅，从物理模型的分析到数学模型的转化与建立，从微分方程及传递函数模型到状态空间模型的转化与建立，每一个环节我们都或多或少的遇到一些问题。由于自己的理论知识基础不好，在实验过程遇到了许多的难题，也使我感到理论知识的重要性。但是我并没有气馁，在实验中发现问题，自己看书，独立思考，最终解决问题，从而也就加深我对课本理论知识的理解，达到了“双赢”的效果。同时老师给了相关论文及资料，在此基础上，我们分头查阅学习，讨论可行办法并逐一尝试，使我们顺利地完成了仿真实验，同时也对线性系统理论有了更深的认识。