1、最优控制实验指导书王 青北京航空航天大学2008 年 5 月这是一本为工科高年级学生和研究生编写的实验与实践教科书，可以作为控制系统领域各门控制课程的配套实验教材。包括如下两个实验：（1）单级倒立摆稳定控制实验；（2）机械臂最优路径规划；注意事项 1. 本指导书中例程的系统参数只具有指导意义，请同学自己完成控制系统设计及实现。2. 由于倒立摆系统是典型的多变量，强耦合，非线性系统，在经典控制器设计时在直线平台上可能导致小车“撞墙” ，因此要注意仔细设计和实验。在线性控制器设计时，由于系统的非线性影响，控制器仿真成功不一定就能实际控制倒立摆系统，因此控制器参数也要仔细调整。实验一：单级倒立摆稳定
2、控制实验1. 倒立摆系统原理简介与建模图1 一级倒立摆原理图一级倒立摆系统的原理框图如上所示。系统包括计算机、运动控制卡、伺服机构、倒立摆本体和光电码盘几大部分，组成了一个闭环系统。光电码盘1将连杆的角度、角速度信号反馈给伺服驱动器和运动控制卡，摆杆的角度、角速度信号由光电码盘2反馈回控制卡。计算机从运动控制卡中读取实时数据，确定控制决策 ，并由运动控制卡来实现该控制决策，产生相应的控制量，驱动电机转动，带动连杆运动，保持摆杆的平衡。在忽略了空气阻力，各种摩擦之后，可将直线一级倒立摆系统抽象成小车和匀质杆组成的系统，如下图2所示。图2 直线一级倒立摆系统 其中：M 小车质量 m 摆杆质量 b
3、小车摩擦系数 l 摆杆转动轴心到杆质心的长度 I 摆杆惯量 F 加在小车上的力 x 小车位置 摆杆与垂直向上方向的夹角 摆杆与垂直向下方向的夹角（考虑到摆杆初始位置为竖直向下）下图是系统中小车和摆杆的受力分析图。其中，N和P 为小车与摆杆相互作用力的水平和垂直方向的分量。 注意：在实际倒立摆系统中检测和执行装置的正负方向已经完全确定，因而矢量方向定义如图所示，图示方向为矢量正方向。图 3 （a ）小车隔离受力图；（b） 摆杆隔离受力图分析小车水平方向所受的合力，可以得到以下方程：(1)MxFbN由摆杆水平方向的受力进行分析可以得到下面等式： (2)2sindmlt即： 2cosinNmxll为
4、了推出系统的第二个运动方程，我们对摆杆垂直方向上的合力进行分析，可以得到下面方程：(3)2cosdPmglt即： 2sincosPmgll力矩平衡方程如下：(4)sincosPlNlI注意：此方程中力矩的方向，由于 ， ，cs故等式前面有负号。 sini合并这两个方程，约去P 和 N，得到第二个运动方程： (5)2sincosImlglmlx设 （ 是摆杆与垂直向上方向之间的夹角），假设 与1（单位为弧 度）相比很小，即 ，则可以进行近似处理： ， ，1cssin，用来u代表被控对象的输入力F，线性化后两个运动方程如下： 20dt(6)2ImlglxMxbu系统状态空间方程为 (7)XAByC
5、Du即： 2 222 222010010 0Imlb Imlx xglMIMmMul ll IIIl (8)100xxyu2. 倒立摆系统LQR控制器设计与仿真最优控制理论主要是依据庞德里亚金的极值原理，通过对性能指标的优化寻找可以使目标极小的控制器。其中线性二次型性能指标因为可以通过求解Riccatti方程得到控制器参数，并且随着计算机技术的进步，求解过程变得越来越简便，因而在线性多变量系统的控制器设计中应用较广。利用线性二次型性能指标设计的控制器称作LQR控制器。前面我们已经得到了直线一级倒立摆系统的比较精确的动力学模型，下面我们针对直线型一级倒立摆系统应用LQR法设计与调节控制器，控制摆
6、杆保持倒立平衡的同时，跟踪小车的位置。实际系统的模型参数如下:M 小车质量 1.096 Kg m 摆杆质量 0.109 Kg b 小车摩擦系数 0 .1N/m/sec l 摆杆转动轴心到杆质心的长度 0.2 5m I 摆杆惯量 0.0034 kg*m*m T 采样频率 0.005秒 注意：在进行实际系统的MATLAB仿真时，请将采样频率改为实际系统的采样频率。请用户自行检查系统参数是否与实际系统相符，否则请改用实际参数进行实验。由倒立摆系统状态方程： 0100.82.671.82.453.4.5xxu (9)100xxyu应用线性反馈控制器，控制系统结构如图 4。图中,R 是施加在小车上的阶跃
7、输入，四个状态量 ， ， ， 分别代表小车位移、小车速度、摆杆位置和摆杆角x速度，输出 包括小车位摆杆角度。设计控制器使得当给系统施加一个阶Ty跃输入时，摆杆会置和摆动，然后仍然回到垂直位置，小车可以到达新的指定位置。系统的开环极点可以用 Matlab 程序求出。开环极点为 0，-0.1428，5.5651，-5.6041，可以看出，有一个极点 5.5651 位于右半 S 平面，这说明开环系统不稳定。假设全状态反馈可以实现（四个状态量都可测），找出确定反馈控制规律K。用 Matlab 中的 lqr 函数，可以得到最优控制器对应的 K。lqr 函数允许选择两个参数( R 和 Q)，这两个参数用来
8、平衡输入量和状态量。最简单的情况是假设R=1， .当然，也可以通过改变 Q 矩阵中的非调节控制器以得到期望的响*C应。图 4 控制系统结构3. 实验要求要求同学们采用LQR控制算法，控制倒立摆摆动至竖直状态，并可以控制倒立摆左移和右移。实验中选择扰动的波形，及其频率和幅值大小，注意先启动伺服，再起摆，记录实验过程中的摆杆角度、摆杆角速度、连杆角位移和连杆角速度，并记录实验过程中的波形。4. 实验报告请各指导老师登陆该实验系统了解具体实验方法，并指导学生完成实验。学生结束实验后应完成相应的实验报告并交给指导老师。其中实验报告的主要内容包括：实验目的，实验内容，实验记录数据，数据分析与处理等。实验
9、二：机械臂最优路径规划实验1. 机械臂控制系统简介图5 机械臂硬件组成 如上图所示，整个机械臂由机械臂本体、运动控制卡和 PC 主机三部分组成。在主机主板的 PCI 插槽中插入 GT-400-SG 运动控制卡，安装对应的驱动程序。通过 25 芯“D”型插头连接电缆将运动控制卡和机械臂本体连接。在 PC 主机上对运动控制器编制程序驱动机械臂四个关节上的伺服电机运动，从而控制机械臂的运动。机械臂通常有两种坐标空间：关节坐标空间和直角坐标空间。两种坐标空间定义如下： 关节坐标空间定义 1机械臂的空间坐标直接由各个关节的坐标来确定，所有关节变量构成一个关节矢量。所有关节矢量构成的空间称为关节坐标空间。
10、因此关节坐标空间运动运动就是直接操作各个关节的运动来完成机械臂的运动。下图是关节坐标空间的定义。 图6 机械臂关节坐标定义 直角坐标空间定义 2机械臂末端的位置和方位通常是在直角坐标空间中描述。当进行机械臂操作任务时，通常采用直角坐标空间更为直观和方便。下图是直角坐标空间的定义。图7 机械臂直角坐标定义 机械臂系统特殊位置的定义 3机器人3工作台12图 8 机械臂特殊位置定义 本机械臂系统在 XY 平面有三个特殊位置的定义，如上图的 1、2、3 矩形框。定义如下（单位：毫米）： 矩形框 1：工作台起点框，直角坐标为（-62.5，342.5）；矩形框 2：工作台顶点框，直角坐标为（62.5，26
11、7.5）；矩形框 3：机械臂标定框，直角坐标为（200，200）。2. 基于状态空间的机械臂最优路径规划机械臂轨迹规划是指，在满足一定约束的条件下，从起始位姿到目标位姿规划一条无碰撞的运动轨迹。下图为二维空间中两连杆机械臂的简化模型。 2q1q,xyLinkink,xy图9 机械臂简化模型其中，x，y表示连杆2末端的位置， 为连杆2的方向与水平轴夹角。以直角坐标系为参考系，基坐标位置为B x，B y，两连杆长度为L 1，L 2，由q1，q 2可确定末端执行器的位置与姿态：(10)121212coss,inixyqq设关节角取值范围为 ，则当姿态角 不同时，该机械臂在0,iqc无障碍约束下的可达
12、区域为以 为圆心， 为半径的圆，12,C1L(11)12212cosos,inin.x xcy yBLqB而整个可达空间为一系列相同半径不同圆心的平面圆组成的曲面，机械臂只能执行曲面所对应的姿态任务。当工作空间存在障碍物时，机械臂的某些位姿无法实现，此时，状态空间以前的某些可达区域变为不可达。假设工作空间中有一个圆形障碍，使得 附近区域变为不可达。障碍约束使得系统状态的可达区域变小,机0.5械臂完成任务的能力减弱。计算出系统状态的可达区域后,为了得到任务的解，需要寻找一条由初始点 0到目标点 g的状态变迁轨迹，可通过搜索获得。状态轨迹可能不止一条，此时可定义一个性能指标函数以获得任务实现的最优
13、解。假定任务要求机械臂由初始状态 运动到目标状态0.5,/2，考虑最短路径（同样也可考虑其他性能指标），取：0.5,gg(12)22111,iiiiCxy在不考虑障碍的情况下，寻找最优控制序列q使得式(12)最小，则最短轨迹为一直线，如下图所示：-0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4 0.50.450.50.550.60.650.70.75图10 机械臂末端路径最优轨迹为一近似圆弧，两关节转角指令分别为：(13)23125.30.896.44710.57qtttMATLAB程序如下：%*x,fval=ga(ga_qq,7);drawfunction
14、 y = ga_qq( x )N=x(7);ts=0.01;y1=0.5;y2=0.5;y1_g=-0.5;y2_g=0.5;t=0;for i=1:1:Nt=t+ts;q1=x(1)*t+x(2)*t2+x(3)*t3;q2=x(4)*t+x(5)*t2+x(6)*t3+1.57;y1=y1;0.5*cos(q1)+0.5*cos(q1+q2);y2=y2;0.5*sin(q1)+0.5*sin(q1+q2);endresult=1000*sqrt(y1_g-y1(end)2+(y2_g-y2(end)2);for i=1:1:Nresult=result+sqrt(y1(i)-y1(i+1)2+(y2(i)-y2(i+1)2);endy=result;%*3. 实验要求在深入理解上述方法的基础上，分析在考虑障碍的情况下，机械臂的最优路径，对该路径进行MATLAB仿真，给出仿真结构以及程序。对所涉及的最优路径进行实物仿真。在实验台上设置障碍，观察机械臂对障碍的规避效果。注意：为保护仪器，障碍请采用不易碰撞的轻小物体。4. 实验报告包括实验原理，方法步骤，试验结果，心得体会。
