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

1、基金项目：天津市科技计划资助项目（13ZCZDGX01154）；国家科技计划资助项目（2014BAH03F01）作者简介:陈云军(1976)，讲师，主要研究领域为智能控制；钟有博(1990)，男，硕士研究生，主要研究领域为电子与通信。六自由度运动(yndng)平台(pngti)位置逆解实现(shxin)与仿真研究陈云军 1, 钟有博2CHEN Yun jun1, ZHONG You bo21.天津工业大学 电气与自动化工程学院,天津 300387 2.天津工业大学 电子与信息工程学院,天津 3003871. School of Electrical Engineering and Automa

2、tion, Tianjin Polytechnic University, Tianjin 300387, China2. School of Electronics and Information Engineering, Tianjin Polytechnic University, Tianjin 300387, ChinaE-mail:CHEN Yun jun, ZHONG You bo. Simulation research and realization of position inverse solution of 6-Dof motion platform Abstract：

3、Firstly, use D-H method to analyze the kinematics of the 6-Dof motion platform, and establish its position inverse solution model base on Matlab/Simulink simulation integrated environment. Then, build up the physics model of the 6-Dof motion platform by the SimMechancis tool sets under Simulink, and

4、 establish the simulation and verification system in Simulink. Finally, input the desired reference trajectory to simulate and verify, and obtain the experimental results that the error is small and results match. Therefore, verify the correctness of the position inverse solution model.Key words：6-D

5、of parallel platform; D-H method; position inverse solution;Matlab/Simulink摘 要：首先，应用D-H法对六自由度运动平台的运动学进行分析，并在Matlab/Simulink仿真集成环境中建立其位置逆解模型；然后利用Simulink下的SimMechanics工具集搭建好六自由度运动平台物理模型，并在Simulink下搭建起仿真验证系统；最后输入期望参考运动轨迹，加以仿真验证，误差较小，结果吻合，从而验证位置逆解模型的正确性。 关键词： 六自由度运动平台; D-H法; 位置逆解; Matlab/Simulink 文献标识码

6、: A 中图分类号:TN913 引言近年来，由于六自由度运动平台具有极为广泛的应用前景，引起了国内外科研机构、研究院校广泛的研究。六自由度运动平台，又称Stewart并联平台，stewart平台的研究始于1965年，德国工程师stewart提出六自由度的并联机构用来作为飞行模拟器1。目前经典stewart运动平台的机构主要由上下两个平台和六个可以伸缩的杆件及它们和上下平台相连接的铰链构成。其中，上平台为负载平台，可以移动；下平台通常为底座，固定在地面。Stewart并联平台可以通过六条支杆的伸缩运动使得负载平台可在工作范围内实现空间六个自由度的联合运动（即俯仰、侧倾、航偏转动以及前后、左右、上

7、下平动），并具有刚度好，精度高，承载能力强，动态特性好等优点2。六自由度运动平台的位置逆解的求解和仿真是进行机构运动控制和结构优化的基础，它对六自由度运动平台进行位姿控制时的各种应用场合均有直接的实用价值。而Matlab是一种高精度的科学计算工具，它将计算、可视化和编程集成在一个容易使用的环境中，其典型功能包括数学计算、建模和仿真、数据分析、研究和可视化、创建图形用户接口。simulink是matlab中的一个重要部分，是一个用来对动态系统进行建模、仿真和分析的软件包。Simulink下的工具集SimMechanics3是matlab6.5新增的机构系统模块集，它可以建立起机构的物理模型，并可

8、以通过相应模块与Simulink模块或子系统相连接，进行联合测试仿真，为机构的运动学分析与建模实现提供了良好的技术基础。本文利用Simulink/Matlab集成开发环境实现位置逆解算法模型，并且通过在其环境下建立的六自由度运动平台物理模型进行了仿真验证。位置(wi zhi)逆解分析 六自由度运动平台的运动学分析主要包括位置分析、速度和加速度分析。位置分析是求输入输出构件之间的位置关系(gun x)，它包括位置正解和位置(wi zhi)逆解4。本文主要应用D-H法对六自由度运动平台的位置逆解进行分析并建模。D-H法5是由Denavit和Hartenberg共同提出来的一种通用方法，运用D-H法

9、建立运动学模型的大体步骤是：首先按照一定规律建立坐标系，再建立坐标系间齐次坐标变换矩阵，最后建立机构的运动学方程。2.1坐标系建立及坐标点的确定六自由度运动平台由上、下平台，六支运动杆件以及其与上下平台相连接的运动铰构成，借助六条运动杆的伸缩运动，完成上平台在三维空间六个自由度的运动，从而模拟出各种空间姿态。为了清楚地描述平台的运动，选取两个坐标系，即动坐标P（OXYZ）和静坐标系下G（OXYZ）。其中静坐标系固定在下平台中，静坐标原点与下平台的质心重合；动坐标系固定在上平台中，动坐标系原点与上平台的质心重合，如图1所示。图1 静坐标系与动坐标系位置示意图图2 上、下平台各铰支点的位置分布示意

10、图如图2，为上、下平台各铰点的位置分布示意图。其中用矩阵B来表示支杆与下平台相连接的铰点Bi（i=1，2，3，4，5，6）在静坐标系中的坐标向量。矩阵B的第一列的第一行至第三行元素分别表示B1点在动坐标系中的X轴、Y轴、Z轴的坐标量，其余列的意义与第一列意义类似。则可得出： B=Rbcos(-b)Rbcos(b)Rbcos(23-b)Rbsin(-b)Rbsin(b)Rbsin(23-b)-hb-hb-hb Rbcos(23+b)Rbcos(43-b)Rbcos(43+b)Rbsin(23+b)Rbsin(43-b)Rbsin(43+b)-hb-hb-hb (1)式中：Rb为支杆与下平台相连接

11、的铰点（下铰点）分布圆的半径；b为B1点相对OX轴的偏移角度，即下铰点分布角；hb为下铰点离下平台质心的高度。用矩阵A来表示液压气缸下端各铰支点Ai（i=1，2，3，4，5，6）在动坐标系中的坐标向量。矩阵A的第一列的第一行至第三行元素分别表示A1点在动坐标系中的OX轴、OY轴、OZ轴的坐标量，其余列的意义与第一列意义类似。则可得出： A=Rtcos(53+a)Rtcos(3-a)Rtcos(3+a)Rtsin(53+a)Rtsin(3-a)Rtsin(3+a)hahaha Rtcos(-a)Rtcos(+a)Rtcos(53-a)Rtsin(-a)Rtsin(+a)Rtsin(53-a)ha

12、haha (2)式中：Rt为支杆与上平台(pngti)相连接的铰点（上铰点）分布(fnb)圆的半径；a为A1点相对(xingdu)OX轴的偏移角度，即上铰点分布角；ha为上铰点离上平台质心的高度。2.2坐标变换及次坐标变换矩阵刚体的位置可以用它在某个坐标系中的向量来描述，而动坐标系固定在上平台中，其位置矢量为： P=x y zT刚体的方位也称刚体的姿态，而描述转动刚体相对参考坐标系的方位都可以用三个角度作为广义坐标，这三个角称为欧拉角。它们可以描述刚体相对于参考坐标系的姿态，由三个欧拉角构成的旋转矩阵6为： R=cc-sc+cssss+cscsccc+sss-cs+ssc-scscc (3)式

13、中：c表示余弦cos，s表示正弦sin。动作坐标系相对于经坐标系的位置和姿态可以用广义坐标Q来表示，Q的分量为qi；其中q1、q2、q3为动坐标系与静坐标系下的三个姿态角（，），q4、q5、q6为动坐标系原点O在静坐标系下OX、OY、OZ三轴上的坐标(x，y，z)，即记Q=（q1，q2，q3 ，q4、q5，q6）=（，x，y，z）。齐次坐标系是用（n+1）维坐标来描述n维空间中的位置，齐次变换矩阵是44矩阵，它能把一个以其次坐标表示的位置矢量由一个坐标系映射到另外一个坐标系。若三维空间的位置矢量P表示成齐次坐标系，即P=x，y，z，1T，那么利用变换矩阵的概念，对转动33旋转矩阵可扩展成44齐

14、次坐标变换矩阵，该齐次变换矩阵为：T=R3x3P3x1O1x31 （4）2.3平台的位置逆解当已知上平台的位置和姿态而求解机构输入杆件的位置时，称为机构的位置逆解。运动平台的位置逆解，即给定上平台在空间中的位置和姿态时求解六条支杆的伸缩量。首先，将上、下铰点的坐标位置用齐次坐标的形式表示，则支杆与下平台相连接的铰点为：Bi=Bi1 (i=1，2，3，4，5，6) （5）下铰点位置坐标矩阵为：B=B116 (i=1，2，3，4，5，6) （6）支杆与下平台相连接的铰点为：Ai=Ai1 (i=1，2，3，4，5，6) （7）上铰点位置坐标矩阵为：A=A116 (i=1，2，3，4，5，6) （8）

15、根据前文所述，可以得出动坐标系与静坐标系之间的变换关系为： Li=T*Ai-Bi （9）运动杆的伸缩量可由上、下铰点间的距离与运动杆初始长度来确定，即： Li=Li-Li0=T*Ai-Bi-Li0 （i=1,2,3,4,5,6） （10）Li表示各运动杆的伸缩量，Li0为各运动杆的初始长度。 运动平台位置逆解模型的建立3.1位置逆解模型的整体结构根据上述反解过程分析，在Simulink中建立六自由度运动平台逆解仿真模型。位置逆解模型机构如图3所示。图3 位置(wi zhi)逆解模型(mxng)3.2 上平台(pngti)参考轨迹模块上平台参考轨迹模块用于上平台期望位资参数的输入，该模块中输入的

16、上平台期望运动参数是动坐标系相对于经坐标系的旋转及平移变换量。输出参数Xang、Yang、Zang为上平台旋转姿态角，输出参数Xpos、Ypos、Zpos为上平台的位置坐标。上平台参考轨迹模块是一个子系统模块，打开该模块后，其结构如图4所示。这里信号源均设为正弦波信号，用Sine Wave模块，输出参考轨迹可以根据实际需求设置。图4 上平台输入参考轨迹模块3.3 旋转矩阵模型旋转矩阵模块是由9个函数封装而成的子系统模块，其内部机构框图如图5所示。每一个函数都是有Fcn模块构成，其相应的函数表达式与旋转矩阵R中的9个元素相对应。最后用Reshape模块将生成的91列向量转换成33矩阵，即为空间坐

17、标的旋转变换矩阵。图5 旋转矩阵模型3.4 杆长向量计算模块杆长向量计算也是子系统模块，此子系统的作用是根据各杆件向量及初始位置杆长，计算输出各杆件的伸缩量，其内部机构框图如图6所示。再打开杆长1子系统模块，其内部结构框图如图7所示，该子系统模块通过杆件序号用Selector模块选择杆件向量，然后经过运算得到相应杆件的伸缩量。各杆件杆长向量的计算方法相同，只是输入时加上不同的杆件序号，并根据所选取的杆件序号进行相应的杆长向量计算。图6 杆长向量计算模块图7 杆长1子系统仿真(fn zhn)验证 4.1位置(wi zhi)逆解仿真(fn zhn)验证系统搭建为了更好地验证位置逆解算法模型的正确性

18、，特在Simulink集成开发环境下搭建位置逆解模型仿真验证系统，如图8所示。该系统由逆解模型模块、微分模块、PID控制器模块、六自由度运动平台物理模型模块、惯性位置传感器模块、示波器显示模块6个模块组成。其中，逆解模型模块是由上述所建立的位置逆解模型打包封装而成的子系统，作用是实现位置逆解，将上平台输入的参考轨迹转化成6支杆的伸缩量，即6支杆的期望位移。微分模块的作用是将6支杆的期望位移转换成期望运动速度。PID控制器模块用于对六自由度运动平台的运动的控制。六自由度运动平台的物理模型模块是由Simulink下的工具集SimMechanics建立的物理模型7。惯性位置传感器模块目的是为了对上平

19、台的运动进行实时监测。示波器模块有4个scope block，用以显示期望杆长位置、实际杆长位置、杆长位置误差以及上平台质心位置。图8 位置逆解模型仿真验证系统SimMechanics提供了各种模块来代表机械系统中的实体部件、接头、约束及力等等，同时又紧密结合了Matlab/Simulink，具有建模灵活性高、参数自定义化、结构易调整、效率高等特点，另外还提供了3D可视化视图，可以很直观的查看所建立的模型，这为仿真模型的构建和进一步研究带来了极大的方便。如图9，为所建立的六自由度运动平台物理模型的可视化视图，在接下来的仿真验证过程中，我们可以在该模型浏览器中观察仿真演示过程。图9平台物理模型可

20、视化图形 4.2仿真验证分析在本节中，主要通过一个位置逆解实例来验证逆解的正确性。首先，设置平台物理模型相关参数，设置平台模型结构参数如下：上平台半径为30cm，厚度为2cm，上铰点分布圆的半径为Ra=18cm，分布角为a=15；下平台半径为40cm，厚度为8cm，下铰点分布圆半径为Rb=28cm，分布角为b=15；6支杆的初始长度为L0=31.52cm，万向铰高度为3cm，可知ha=4cm，hb=7cm，上平台的初始高度为H0=38.52cm，具体模型如图9所示。其次，对上平台的运动轨迹进行规划，使上平台上升一定高度后绕着中心位置做圆周运动；设置上升高度为5cm，圆周运动半径为2cm，运动轨

21、迹方程为： x=2sin(2)y=2cos(2)z=H0+5=43.52 （7）最后，综合考虑代价和控制效果调整PID控制8参数进行仿真验证，选取PID参数为：Kp=7.5e4，Ki=3.5e4，Kd=1.5e3。仿真结果如图10、图11、图12所示，其中图10为杆长位置误差，误差精度达到10-3cm，图11为上平台质心位置运动变化，平台实际高度为43.5cm，与期望高度误差为210-3cm，误差较小，图12为上平台质心的实际运动轨迹，与期望输入参考轨迹相符。图10 杆长位置(wi zhi)误差图11 上平台(pngti)质心位置图12 上平台质心实际运动(yndng)轨迹结论本文首先运用D-H法分析了六自由度运动平台的运动学位置逆解，建立了机构的位置逆解数学模型，并基于Matlab/Simulink仿真集成环境建立其Simulink模型，实现了平台位姿逆解实时解算。其次，通过Simulink