基于matlab_simulink的六自由度运动平台的位置反解实现与仿真研究(初稿1)--钟有博

基金项目：天津市科技计划资助项目（13ZCZDGX01154）；国家科技计划资助项目（2014BAH03F01）作者简介:陈云军(1976－)，讲师，主要研究领域为智能控制；钟有博(1990－)，男，硕士研究生，主要研究领域为电子与通信。六自由度运动平台位置逆解实现与仿真研究陈云军1,钟有博2CHENYunjun1,ZHONGYoubo21.天津工业大学电气与自动化工程学院,天津3003872.天津工业大学电子与信息工程学院,天津3003871.SchoolofElectricalEngineeringandAutomation,TianjinPolytechnicUniversity,Tianjin300387,China2.SchoolofElectronicsandInformationEngineering,TianjinPolytechnicUniversity,Tianjin300387,ChinaE-mail:[e-mail],ZHONGYoubo.Simulationresearchandrealizationofpositioninversesolutionof6-DofmotionplatformAbstract：Firstly,useD-Hmethodtoanalyzethekinematicsofthe6-Dofmotionplatform,andestablishitspositioninversesolutionmodelbaseonMatlab/Simulinksimulationintegratedenvironment.Then,buildupthephysicsmodelofthe6-DofmotionplatformbytheSimMechancistoolsetsunderSimulink,andestablishthesimulationandverificationsysteminSimulink.Finally,inputthedesiredreferencetrajectorytosimulateandverify,andobtaintheexperimentalresultsthattheerrorissmallandresultsmatch.Therefore,verifythecorrectnessofthepositioninversesolutionmodel.Keywords：6-Dofparallelplatform;D-Hmethod;positioninversesolution;Matlab/Simulink摘要：首先，应用D-H法对六自由度运动平台的运动学进行分析，并在Matlab/Simulink仿真集成环境中建立其位置逆解模型；然后利用Simulink下的SimMechanics工具集搭建好六自由度运动平台物理模型，并在Simulink下搭建起仿真验证系统；最后输入期望参考运动轨迹，加以仿真验证，误差较小，结果吻合，从而验证位置逆解模型的正确性。关键词：六自由度运动平台;D-H法;位置逆解;Matlab/Simulink文献标识码:A中图分类号:TN9131.引言近年来，由于六自由度运动平台具有极为广泛的应用前景，引起了国内外科研机构、研究院校广泛的研究。六自由度运动平台，又称Stewart并联平台，stewart平台的研究始于1965年，德国工程师stewart提出六自由度的并联机构用来作为飞行模拟器[1]。目前经典stewart运动平台的机构主要由上下两个平台和六个可以伸缩的杆件及它们和上下平台相连接的铰链构成。其中，上平台为负载平台，可以移动；下平台通常为底座，固定在地面。Stewart并联平台可以通过六条支杆的伸缩运动使得负载平台可在工作范围内实现空间六个自由度的联合运动（即俯仰、侧倾、航偏转动以及前后、左右、上下平动），并具有刚度好，精度高，承载能力强，动态特性好等优点[2]。六自由度运动平台的位置逆解的求解和仿真是进行机构运动控制和结构优化的基础，它对六自由度运动平台进行位姿控制时的各种应用场合均有直接的实用价值。而Matlab是一种高精度的科学计算工具，它将计算、可视化和编程集成在一个容易使用的环境中，其典型功能包括数学计算、建模和仿真、数据分析、研究和可视化、创建图形用户接口。simulink是matlab中的一个重要部分，是一个用来对动态系统进行建模、仿真和分析的软件包。Simulink下的工具集SimMechanics[3]是matlab6.5新增的机构系统模块集，它可以建立起机构的物理模型，并可以通过相应模块与Simulink模块或子系统相连接，进行联合测试仿真，为机构的运动学分析与建模实现提供了良好的技术基础。本文利用Simulink/Matlab集成开发环境实现位置逆解算法模型，并且通过在其环境下建立的六自由度运动平台物理模型进行了仿真验证。2.位置逆解分析六自由度运动平台的运动学分析主要包括位置分析、速度和加速度分析。位置分析是求输入输出构件之间的位置关系，它包括位置正解和位置逆解[4]。本文主要应用D-H法对六自由度运动平台的位置逆解进行分析并建模。D-H法[5]是由Denavit和Hartenberg共同提出来的一种通用方法，运用D-H法建立运动学模型的大体步骤是：首先按照一定规律建立坐标系，再建立坐标系间齐次坐标变换矩阵，最后建立机构的运动学方程。2.1坐标系建立及坐标点的确定六自由度运动平台由上、下平台，六支运动杆件以及其与上下平台相连接的运动铰构成，借助六条运动杆的伸缩运动，完成上平台在三维空间六个自由度的运动，从而模拟出各种空间姿态。为了清楚地描述平台的运动，选取两个坐标系，即动坐标{P}（OXYZ）和静坐标系下{G}（O’X’Y’Z’）。其中静坐标系固定在下平台中，静坐标原点与下平台的质心重合；动坐标系固定在上平台中，动坐标系原点与上平台的质心重合，如图1所示。图1静坐标系与动坐标系位置示意图图2上、下平台各铰支点的位置分布示意图如图2，为上、下平台各铰点的位置分布示意图。其中用矩阵B来表示支杆与下平台相连接的铰点Bi（i=1，2，3，4，5，6）在静坐标系中的坐标向量。矩阵B的第一列的第一行至第三行元素分别表示B1点在动坐标系中的X轴、Y轴、Z轴的坐标量，其余列的意义与第一列意义类似。则可得出：B=[Rbcos(−b)Rbcos(b)Rbcos(2π3−b)Rbsin(−b)Rbsin(b)Rbsin(2π3−b)−hb−hb−hbRbcos(2π3+b)Rbcos(4π3−b)Rbcos(4π3+b)Rbsin(2π3+b)Rbsin(4π3−b)Rbsin(4π3+b)−hb−hb−hb](1)式中：Rb为支杆与下平台相连接的铰点（下铰点）分布圆的半径；b为B1点相对OX轴的偏移角度，即下铰点分布角；hb为下铰点离下平台质心的高度。用矩阵A来表示液压气缸下端各铰支点Ai（i=1，2，3，4，5，6）在动坐标系中的坐标向量。矩阵A的第一列的第一行至第三行元素分别表示A1点在动坐标系中的O’X’轴、O’Y’轴、O’Z’轴的坐标量，其余列的意义与第一列意义类似。则可得出：A=[Rtcos(5π3+a)Rtcos(π3−a)Rtcos(π3+a)Rtsin(5π3+a)Rtsin(π3−a)Rtsin(π3+a)hahahaRtcos(π−a)Rtcos(π+a)Rtcos(5π3−a)Rtsin(π−a)Rtsin(π+a)Rtsin(5π3−a)hahaha](2)式中：Rt为支杆与上平台相连接的铰点（上铰点）分布圆的半径；a为A1点相对OX轴的偏移角度，即上铰点分布角；ha为上铰点离上平台质心的高度。2.2坐标变换及次坐标变换矩阵刚体的位置可以用它在某个坐标系中的向量来描述，而动坐标系固定在上平台中，其位置矢量为：P=[xyz]T刚体的方位也称刚体的姿态，而描述转动刚体相对参考坐标系的方位都可以用三个角度作为广义坐标，这三个角称为欧拉角。它们可以描述刚体相对于参考坐标系的姿态，由三个欧拉角构成的旋转矩阵[6]为：R=[cαcβ−sβcγ+cαsβsγsαsγ+cαsβcγsαcβcαcγ+sαsβsγ−cαsγ+sαsβcγ−sβcβsγcβcγ](3)式中：c表示余弦cos，s表示正弦sin。动作坐标系相对于经坐标系的位置和姿态可以用广义坐标Q来表示，Q的分量为qi；其中q1、q2、q3为动坐标系与静坐标系下的三个姿态角（α，β，γ），q4、q5、q6为动坐标系原点O在静坐标系下O’X’、O’Y’、O’Z’三轴上的坐标(x，y，z)，即记Q=（q1，q2，q3，q4、q5，q6）=（α，β，γ，x，y，z）。齐次坐标系是用（n+1）维坐标来描述n维空间中的位置，齐次变换矩阵是4×4矩阵，它能把一个以其次坐标表示的位置矢量由一个坐标系映射到另外一个坐标系。若三维空间的位置矢量P表示成齐次坐标系，即P=[x，y，z，1]T，那么利用变换矩阵的概念，对转动3×3旋转矩阵可扩展成4×4齐次坐标变换矩阵，该齐次变换矩阵为：T=[R3x3P3x1O1x31]（4）2.3平台的位置逆解当已知上平台的位置和姿态而求解机构输入杆件的位置时，称为机构的位置逆解。运动平台的位置逆解，即给定上平台在空间中的位置和姿态时求解六条支杆的伸缩量。首先，将上、下铰点的坐标位置用齐次坐标的形式表示，则支杆与下平台相连接的铰点为：Bi‘=[Bi1](i=1，2，3，4，5，6)（5）下铰点位置坐标矩阵为：B‘=[B11×6](i=1，2，3，4，5，6)（6）支杆与下平台相连接的铰点为：Ai‘=[Ai1](i=1，2，3，4，5，6)（7）上铰点位置坐标矩阵为：A‘=[A11×6](i=1，2，3，4，5，6)（8）根据前文所述，可以得出动坐标系与静坐标系之间的变换关系为：Li=T∗Ai‘−Bi‘（9）运动杆的伸缩量可由上、下铰点间的距离与运动杆初始长度来确定，即：∆Li=Li−Li0=‖T∗Ai‘−Bi‘‖−Li0（i=1,2,3,4,5,6）（10）∆Li表示各运动杆的伸缩量，Li0为各运动杆的初始长度。3.运动平台位置逆解模型的建立3.1位置逆解模型的整体结构根据上述反解过程分析，在Simulink中建立六自由度运动平台逆解仿真模型。位置逆解模型机构如图3所示。图3位置逆解模型3.2上平台参考轨迹模块上平台参考轨迹模块用于上平台期望位资参数的输入，该模块中输入的上平台期望运动参数是动坐标系相对于经坐标系的旋转及平移变换量。输出参数Xang、Yang、Zang为上平台旋转姿态角，输出参数Xpos、Ypos、Zpos为上平台的位置坐标。上平台参考轨迹模块是一个子系统模块，打开该模块后，其结构如图4所示。这里信号源均设为正弦波信号，用SineWave模块，输出参考轨迹可以根据实际需求设置。图4上平台输入参考轨迹模块3.3旋转矩阵模型旋转矩阵模块是由9个函数封装而成的子系统模块，其内部机构框图如图5所示。每一个函数都是有Fcn模块构成，其相应的函数表达式与旋转矩阵R中的9个元素相对应。最后用Reshape模块将生成的9×1列向量转换成3×3矩阵，即为空间坐标的旋转变换矩阵。图5旋转矩阵模型3.4杆长向量计算模块杆长向量计算也是子系统模块，此子系统的作用是根据各杆件向量及初始位置杆长，计算输出各杆件的伸缩量，其内部机构框图如图6所示。再打开杆长1子系统模块，其内部结构框图如图7所示，该子系统模块通过杆件序号用Selector模块选择杆件向量，然后经过运算得到相应杆件的伸缩量。各杆件杆长向量的计算方法相同，只是输入时加上不同的杆件序号，并根据所选取的杆件序号进行相应的杆长向量计算。图6杆长向量计算模块图7杆长1子系统4.仿真验证4.1位置逆解仿真验证系统搭建为了更好地验证位置逆解算法模型的正确性，特在Simulink集成开发环境下搭建位置逆解模型仿真验证系统，如图8所示。该系