倒立摆机器人系统的数学模型描述

倒立摆机器人的模型倒立摆动力学模型示意图如图1.1所示。yx1q2q1cl2cl1l主动关节主动臂欠驱动臂欠驱动关节图1.1倒立摆动力学模型示意图表1.1参数说明参数名称参数定义1l主动臂的长度1cl主动臂相对于连接点到质心的距离2cl欠驱动臂相对于连接点到质心的距离1q主动臂相对于坐标轴的角度2q欠驱动臂相对于主动臂的角度1I主动臂相对于质心转动惯量2I欠驱动臂相对于质心转动惯量1m主动臂质量2m欠驱动臂质量g重力加速度拉格朗日动力学方程拉格朗日方程以广义坐标为自变量，通过拉格朗日函数来表示。拉格朗日体系分析力学处理问题时以整个力学系统作为对象，用广义坐标来描述整个力学系统，着眼于能量概念。对于机械系统，其拉格朗日函数都可以定义成该系统动能kE和势能pE之差，即：kpLEE(1.1)系统的动能和势能可以用任意选取的坐标系来表示。系统的动力学方程（第二类拉格朗日方程）为：dLLdtqq(1.2)由于势能不含速度项，因此动力学方程也可以写成：pkkEEEddtqqq(1.3)由此可见，对于Pendubot系统，其拉格朗日运动方程则为：1,,[0]()()()1,2TiiidKqqKqqPqdtqqqi(1.4)其中，,Kqq为Pendubot系统的动能之和，Pq为Pendubot系统的势能总和。摆臂受到的力矩为，只有摆臂与电机相连接的主动关节受力，而另一个关节是欠驱动的。由于两杆均为刚体，所以摆臂的动能与势能可根据每一根杆的总质量与相对于重心的惯量来唯一确定。欠驱动机械臂动力学模型根据式(1.4)，分析Pendubot摆臂的动能和势能。计算平移动能的一般表达式为22mvK。由上图可知，系统两个摆臂的角速度可以表示为：11212ωωqqq，(1.5)对于系统的主动臂，其平移动能可以直接描述成以下形式：22111112cKmlq(1.6)由于系统的势能大小与机械臂的质心位置有关系，这里可以用y坐标来表示摆臂的其位置高度，于是势能可以直接描述为：1111sin()cPmlgq(1.7)对于系统的欠驱动臂，要先得到其质心位置的笛卡儿坐标表达式，然后通过微分处理得到关节角速度。其中，欠驱动臂的质心位置用下式来表示：211212211212cos()cos()sin()sin()ccxlqlqqylqlqq(1.8)那么，通过对该位置进行微分处理，即可得到其速度的笛卡儿坐标分量为：211121212211121212sinsin()coscosccxlqqlqqqqylqqlqqqq(1.9)于是可得欠驱动臂在x和y方向上速度分量的平方和为：22222222112121212211222cos()ccvlqlqqqqllqqqq(1.10)因此，系统欠驱动臂的平移动能可以表示为：222222221121212122112122cos2ccKmlqlqqqqllqqqq(1.11)由于摆臂长度是已知的，可以得到欠驱动臂的势能为：2211212sinsincPmglqlqq(1.12)同时，由于Pendubot系统运动的特殊性，这里注意到系统的动能组成中，除了常规的平移动能外，还存在旋转动能部分，那么根据式(1.6)可以得到系统的旋转动能为：12211212222101111001122TvIIIqKQIIQqqIIq(1.13)根据式(1.1)给出的拉格朗日算子的描述方法，可以由上述系统动能和势能的关系得到Pendubot系统的拉格朗日算子，表示为：1212vLKPKKKPP(1.14)为了得到系统的动力学方程，根据式错误!未找到引用源。，对上式错误!未找到引用源。做关于1q和1q的微分处理，可得：2221112112212212211212221122222211212122121122221222221222111)2coscos2coscossin(cccccccccccLmlmlmlmllqmllqIIIdLmlmllllqIIdtmlllqImllqLmqqqqqqqqqqlqqqq2112212cossin()cmlgqmlgqq(1.15)由以上的各微分项，根据第二类拉格朗日方程，本文可以直接得到系统中主动臂的关节力矩表达式为1112221c121c21c22121222c21c222221c22221c22121c12112c212dτdt2coscossin2singcosgsin()LLqqmllIImlllqmllqllqllqqlmllqlIqmqmqqmmqq(1.16)将式(1.14)对2q和2q求微分，得到欠驱动臂的关节动力学方程：2222212221222222222212221222221221222212211222122cos()cossin()sincoscccccccccLmlmllqImlIdLmlmllqImlImllqqdtLmllqqqqmlgqqqqqqqqqq(1.17)于是可以得到欠驱动臂的关节动力学方程为：222222212221222222122122120cossincoscccccdLLdtqqmlmllqImlImllqqlgqqmqq(1.18)可以将式(1.16)和式(1.18)简化描述成如下形式：,DqqCqqqGq(1.19)其中各项可表示为：111221222212121,00ddDqddhqhqhqCqqhqGq上式中的各个参数表达式如下：2221111212112212212212211222222222[2cos()][2cos()]ccccccdmlmllllqIIddmlllqIdmlI2122sin()chmllq111211221222212cos()cos()cos()cccmlmlgqmlgqqmlgqq这里需要注意的是，如果系统动力学模型表示成式(1.19)，那么Dq和,Cqq并非两个完全独立的矩阵，两者之间存在关系2,DqCqq，为反对称矩阵。其实际物理意义是摆臂动能的微分量与摆臂重力矩和驱动机构力矩的功率输入总和相等。即：1[()]2TdqDqqGqdt(1.20)因此，对于任意时刻都有：T102qDCq(1.21)本文得到Pendubot系统动力学表达式如式(1.19)所示，在系统的实际应用中，为了方便参数计算与系统实现，引入以下五个代换参数表达式：2211121122222321241121522cccccmlmlImlImllmlmlml(1.22)因此，式(1.19)中的各项可以表示为：123223223222cos()cos()cos()qqDqq(1.23)322322321321sin()sin()sin(),sin()0qqqqqqCqqqq(1.24)42512512cos()cos()cos()gqgqqGqgqq(1.25)综上所述，本文已经得到了Pendubot系统的动力学模型，并将系统中的七个未知参量整理替换成了如式(1.22)所示五个系统参数，可以通过能量守恒的方法对其进行辨识。欠驱动机械臂系统控制器设计模型建立将式(1.19)中所描述的Pendubot系统的动力学方程通过简单的线性变换改写成如下的状态方程形式：112()[,]qDqCqqqGqq(1.26)引入状态变量，令11213242xqxxqqqx，，，得到系统方程如下的表示形式：(,,)xtFxtutfxbxu(1.27)则可根据系统的输入输出关系，得到Pendubot系统的状态方程为：121132434221(====)()=Txxxfxbxuxxxfxbxuyxx，(1.28)上式中的各项具体表达式如下：222332323241221233335133241221233322223323324322233312sin()[2cos()2()]()2cos(2)2cos()cos()2cos()+2cos(2)2sin()[cos()()cos()]()cos(2)22xxxxxfxxgxxxgxxxxxxxxfxx351333413222233312333122232324132223331215132412233312cos()cos()2cos()cos()cos(2)2cos(2)22sin()[()]cos(2)22[cos()cos()]cos(2)2gxxxgxxxxxxxxxgxxgxx(1.29)212212333233222333122()2cos(2)2[cos()]()cos(2)2bxxxbxx式(1.28)所描述的状态方程形式是十分具有代表性的，这种形式可以被视作一类欠驱动系统的规范表达式，不仅本文的研究对象Pendubot可表示为这种类型的状态方程，Acrobot系统、吊车系统和单摆系统等欠驱动系统的模型也都可以被整理成该形式。通过设计参数辨识程序，获得参数辨识结果。实验结果：123450.0215,0.0120,0.0102,0.1516,0.0742图1.2Pendubot模块LQR方法设计控制器本实验采用LQR最优控制方法设计平衡控制器来控制Pendubot，使之在不稳定平衡点附近保持平衡。图1.3控制系统结构框图为设计平衡控制器，首先必须将Pendubot的非线性运动方程线性化，同时设计一个具有线性模型的全状态反馈控制器。泰勒级数近似表达式用于线性化。xdxu-LQR控制器控制对象||rrrraarrxurxurfffxufxuxxuuxu，，，，(1.30)其中，x为状态向量，u为Pendubot唯一控制输入。rrxu，分别为Pendubot的平衡状态向量及控制输入。平衡控制器是将Pendubot控制在不稳定平衡点，因此afxu，为0，因此我们所要做的是找到偏微分矩阵并求其在平衡点的值。其中11111223332244()()axfxaxbxufxfxuxfxaxbxufx，(1.31)在平衡情况下，电机提供的转矩主要克服两根杆重力相对于支撑点的转矩，因此Pendubot的平衡点可作如下定义:4113cos()2rrrrugxxx(1.32)将Pendubot的动力学方程对于状态向量求偏微分则有11111234222222221234123433331234444412344444123401000001ffffxxxxffffffffxxxxxxxxfAffffxxxxxffffxxxxffffxxxx