一种基于曲柄连杆机构的六自由平台姿态正解方法与流程

本发明涉及六自由度平台控制技术领域，特别涉及一种基于曲柄连杆机构的六自由平台姿态正解方法。

背景技术

六自由度平台是指通过六个并联的支撑机构的作用，能够进行空间六自由运动的平台。在机器人、各种载体的运动状态仿真、游戏的场景模拟等方面有广阔的应用前景。

目前的六自由平台多采用六个具有伸缩功能的支撑机构作为运动执行机构，典型结构如stewart平台(d.stewart.《aplatformwithsixdegreesoffreedom.》proeeedingsoftheimeehe,1965,180(15):371-385)。这种结构的六自由度平台多采用液压缸或电动缸驱动，但这两种驱动系统均结构复杂、造价高、体积大、维护不便。美国加州圣何塞州立大学(sanjosestateuniversity,sjsu)的机械工程学院的两位学生tyletkroymann和robertdee于2013年的一个学期项目fullmotiondynamic(fmd)中研制出了一种基于曲柄连杆的机构的六自由度平台。这种平台结构简单、动态特性好，引起了广泛的关注。国内北京航空航天大学、北京大华杰康科技有限公司等机构和个人也据此结构申请了相关的六自由度动态平台的专利。(谢峰，胡磊，于晓亮等《一种六自由度并联动感平台》，发明专利，申请号：201610373911.8；胡磊,刘洪升,张坚等《一种基于六自由度体感平台的滑雪模拟机》，实用新型专利，申请号：201720903740.5；唐姗姗，《用于模拟仿真的二、三、四、六自由度运动平台》，实用新型专利，申请号：201620888673.x)。

六自由度平台的运动学分析是提升各种应用场景下六自由度平台性能的关键技术，其中姿态正解是运动控制技术中不可或缺的一个部分，是平台姿态误差分析、动力学分析、故障恢复、机构尺寸优化等方面都需要的一项重要技术。六自由度平台的姿态正解，是指依据六个并联的支撑机构的当前状态求解平台当前的姿态和位置，对于stewart平台而言是依据六个伸缩杆的长度求解平台的姿态和位置；而对于基于曲柄连杆机构的六自由度平台则是依据曲柄当前的角位置信息求取平台的姿态。六自由度平台姿态反解问题比较简单，而正解问题则非常复杂，且伴有多值性问题，(任瑞，《6dof姿态控制平台关键技术研究》，中国工程物理研究院博士论文，2012年8月；吴培栋，《stewart平台的运动学与逆动力学的基础研究》，华东科技大学博士论文，2008年9月)。对于常见的stewart结构的六自由度平台，姿态正解的常用方法有：迭代法或优化法、解析法、同伦法、数学机械法和神经网络法等，也有学者提出了基于比例再生空间收缩粒子群优化算法(李磊，《六自由度平台位置正解及控制方法研究》，哈尔滨工程大学博士论文，2008年7月)。以上方法均是基于伸缩杆结构的六自由度平台，也就是stewart结构的平台的姿态正解方法。

目前尚没有公开发表的文献资料对基于曲柄连杆机构的六自由度平台的姿态正解方法。

技术实现要素：

本发明要解决的技术问题是：基于曲柄连杆机构的六自由度平台姿态正解问题，即依据曲柄当前的角位置信息求取平台的姿态。

本发明解决上述技术问题所采用的技术方案是：首先，建立平台基座坐标系，根据机械结构确定六个驱动电机或其传动结构与曲柄连接处的位置坐标；接着，根据曲柄当前角位置以及曲柄长度，确定曲柄与连杆连接处的支撑轴承中心坐标；然后，根据连杆的长度、上平台各个支撑点的几何关系，得到平台和支撑点的一组线性方程；最后，求解方程组，得到上平台的姿态和位置。

六自由度平台的机械结构如1所示，基座相对于地面固定，其上安装有六台电机；电机直接带动或通过减速机带动六个曲柄运动；所有曲柄的旋转轴线在同一平面上，且交于一点o，曲柄通过支撑轴承带动连杆；连杆通过支撑轴承与上平台的六个支撑点连接，带动上平台运动。

本方法的具体实施步骤为：

(1)建立平台基座坐标系e0，根据机械结构确定六个驱动电机或其传动机构的驱动轴与曲柄连接轴承的中心点坐标。具体做法如下：

建立平台基座坐标系e0如附图2所示：以天顶方向为z轴正方向；以六轴驱动电机或减速机旋转轴线所在的平面为z＝0的参考平面(也称xoy平面)；六个曲柄连杆与电机转轴连接处的支撑点构成一个六边形，取六条轴线交点为坐标原点o；以平台正前方为y轴正方向，右手方向为x轴正方向。绕坐标轴ox轴的旋转角度定义为俯仰角θ，绕oy轴的旋转角度定义为滚动角γ，绕oz轴的旋转角度定义为偏航角

定义曲柄和电机或其传动机构的驱动轴连接轴承的中心点为支撑点ai(i＝1,…,6)，六个支撑点构成一个六边形a1a2a3a4a5a6。根据机械结构确定六个支撑点的坐标ai(xai,yai,zai)(i＝1,…,6)。

(2)根据曲柄当前角位置，确定曲柄与连杆连接处的支撑轴承中心坐标。具体做法如下：

如图3所示，定义与支撑点ai连接的曲柄lai处于坐标系e0参考平面(xoy平面)内时为初始位置，记lai的初始位置相量记lai当前时刻位置与初始位置的夹角αi,(i＝1～6,逆时针为正)；曲柄与连杆连接处的轴承中心点di；联结连杆与上平台连接处的支撑轴承的中心点为bi(i＝1,…,6)；曲柄lai的旋转轴为矢量曲柄初始位置及旋转矢量pi在参考平面分布如图4所示，由图可知：pi＝[xai,yai,zai]。

由运动学知识可知，任何矢量围绕pi旋转角度αi的旋转矩阵表达式为：

z＝cosαii+(1-cosαi)pipi+sinαipi(1)

上式中pi为矢量pi对应的坐标阵,pipi为矢量pi对应的并矢，二者的表达式分别为：

曲柄lai旋转后的矢量的表达式为：

进而可以得到di点的坐标(xdi,ydi,zdi)：

(3)根据连杆的长度、上平台各个支撑点的几何关系，得到平台和支撑点的一组线性方程。具体做法如下：

定义上平台各支撑轴承球心中心点为：bi(i＝1,…,6)，bi在坐标系e0中的坐标为(xbi,ybi,zbi)。bi六个点分布如图5所示，这六个点构成六边形b1b2b3b4b5b6，六边形长边长度|b1b2|＝|b3b4|＝|b5b6|＝lb，短边长度|b2b3|＝|b4b5|＝|b6b1|＝hb，三条长边的延长线交于三点cb1、cb2、cb3，这三个点在坐标系e0中的坐标为(xcj,ycj,zcj)(j＝1,2,3)。，显然为一个等边三角形。

由图5可以知道三角形的边cbjcbk(j＝1,2,3k＝1,2,3且j≠k)的长度为：

|cbjcbk|＝2hb+lb(6)

bi、di与相邻的cbj的可以构成一个三角形，设bi、di之间的连杆为lbi，其矢量表达式为长度为lb，则根据bi、di、cbj的相互关系可以得到：

因为|cbjbi|＝hb，设k＝hb/|cbjcbk|＝hb/2hb+lb，可以将(7)式变形得到：

所以有：

根据公式9可以得到关于cb1、cb2、cb3三个点坐标(xcj,ycj,zcj)(j＝1,2,3)的六个方程：

再根据|cbjcbk|＝lc(j＝1,2,3，k＝1,2,3且j≠k)，可以列写三角形的边长公式，得到关于(xcj,ycj,zcj)(j＝1,2,3)的另外三个方程：

(xc1-xc2)²+(yc1-yc2)²+(zc1-zc2)²-lc²＝0(16)

这样(10)～(18)式构成了关于xc1、yc1、zc1、xc2、yc2、zc2、xc3、yc3、zc3这九个未知数的方程组。

(4)求解方程组得到上平台的姿态和位置。具体做法如下：

利用求解非线性方程组的方法求解(10)～(18)这九个方程构成的方程组，可以得到xc1、yc1、zc1、xc2、yc2、zc2、xc3、yc3、zc3这九个未知数。

定义与上平台固联的上平台坐标系eb，该坐标系的定义如图7所示：eb的参考平面(zb＝0平面)取上平台六个支撑轴承球心点bi(i＝1,…,6)所在平面；原点ob取为上平台几何中心；当六个曲柄处于图1所示的水平位置(即αi＝0，i＝1,2,…,6)时上平台处于初始状态，初始状态下obxb轴与平台基座坐标系e0中ox轴平行；obyb轴与平台基座坐标系e0中oy轴平行；此时，eb坐标系原点距离e0坐标系原点的高度为zb0。由于cb1、cb2、cb3分别为处于b1b2、b3b4、b5b6延长线的交点(如图5所示)，所以cb1、cb2、cb3也在eb的参考平面上。

根据前文的定义，上平台的三个轴向相量为在e0坐标系中的表达式为：

obxb＝(xc2-xc3,yc2-yc3,zc2-zc3)(19)

obzb＝obxb×obyb(21)

定义eb与e0之间的姿态变换关系：eb由e0按3-1-2的顺序依次逆时针旋转坐标轴(即先沿oz轴转动角度再沿转动后的ox′轴转动角度θ，最后沿转动后的oy″轴转动角度γ)，θ、γ为姿态角。这三次转动的姿态余弦阵分别为：

e0到eb的姿态变换余弦阵为：

六自由度平台还有三个平移运动的自由度，定义的三轴平移矢量t’＝[δx,δy,δz]’，初始时刻eb与e0坐标系原点的高度差为zb0，所以两坐标系之间的平移矢量修正为：

t＝[δxδyδz+zbo]′(24)

平台进行六自由度运动后，两坐标系间的坐标变换阵ct为：

平台上任意一点b在eb坐标系中的坐标(xb,yb,zb)经三轴平移和三轴旋转之后，对应e0中对应的坐标(x,y,z)为：

上式中(xbo,ybo,zbo)是eb坐标系原点ob在e0中的表达式。

由于平移运动不影响矢量的方向，eb坐标系中各坐标轴的轴向矢量在e0坐标系中的表达式为：

将(19)～(21)式对比(27)～(29)式，可以得到三轴姿态角θ、γ的表达式：

下面计算三轴位置偏移量。将上平台原点ob在eb坐标系中的坐标(0,0,0)代入公式(26)，可以得到其在e0中的坐标(δx0，δy0，δz0+zbo)，又因为ob为中心，所以三轴偏移量为：

δxo＝(xc1+xc2+xc3)/3

δyo＝(yc1+yc2+yc3)/3

δzo＝(zc1+zc2+zc3)/3-zb0(30)

三轴姿态角θ、γ、以及δxo、δyo、δzo即平台当前的姿态和位置，正解问题完成。

本发明具有以下优点：利用基座与上平台上的轴承连接点、曲柄、连杆的几何关系列写方程组，求解方程组，得到上平台的三轴姿态角和相对位置得到了曲柄连杆结构的六自由度平台的姿态正解方法。

附图说明

图1为基于曲柄连杆机构的六自由度平台结构图；

图2为本发明提出的平台基座坐标系示意图；

图3为曲柄连杆各连接点示意图；

图4为参考平面上曲柄初始相量及旋转相量示意图；

图5为上平台各支撑点位置坐标示意图；

图6为连杆两端点与上平台长边延长线交点位置关系示意图；

图7上平台坐标系eb示意图。

具体实施方式

下面结合附图对本发明做进一步的描述，但本发明的保护范围不局限于以下所述。

以下说明本发明的实施例。但以下的实施例仅限于解释本发明，本发明的保护范围应包括权利要求的全部内容，而且通过以下实施例对该领域的技术人员即可以实现本发明权利要求的全部内容。

下面以某型基于曲柄连杆结构的六自由度平台为例，介绍本发明及的姿态正解方法。

某型基于曲柄连杆结构的六自由度平台有关参数如下：

基座上曲柄和驱动轴连接轴承的中心点ai(xai,yai,zai)(i＝1,…,6)ai(i＝1,…,6)坐标顺次为：

与电机驱动轴连接的曲柄长度|lai|＝0.12m，曲柄和上平台之间的连杆长度|lb|＝0.47m。

上平台为一六边形，短边长度hb＝0.1m，长边长度lb＝0.7m，初始状态下，上平台六个支撑轴承球心点分别是：b1(-0.35,0.2598,0.3659)、b2(0.35,0.2598,0.3659)、b3(0.4,0.1732,0.3659)、b4(0.05,-0.4330,0.3659)、b5(-0.05,-0.4330,0.3659)、b6(-0.4,0.1732,0.3659)。

按以下步骤完成推出过程：

(1)建立平台基座坐标系e0，根据机械结构确定六个驱动电机或其传动结构与曲柄连接处的位置坐标：

a1(-0.21599,0.37413,0)、a2(0.21599,0.37413,0)、a3(0.432,0,0)、a4(0.216,-0.374,0)、a5(-0.216,-0.374,0)、a6(-0.432,0,0)。

(2)根据曲柄当前角位置，确定曲柄与连杆连接处的轴承中心坐标:

d1(0.1039，0.0600，0)，d2(-0.0943，0.0544，-0.0505)，d3(0，-0.1088，0.0507)，d4(0.0943，0.0544，-0.0505)，d5(-0.0942，0.0544，0.0507)，d6(0，-0.1089，-0.0505)。

(3)根据连杆的长度、上平台各个支撑点的几何关系，得到平台和支撑点的一组线性方程。根据本例的条件k＝0.1111，lb＝0.47m，lc＝0.9m。得到方程组：

(4)求解方程组得到上平台的姿态和位置。

先求取c1(0.08132，-0.5132，0.3659)，c2(0.4038，0.3270，0.3659)，c3(-0.4851，0.1862，0.3659)。

再求取三轴姿态角以及三轴偏移量：

θ＝0°，γ＝9°，δx0＝0，δy0＝0，δz0＝0。

以上姿态正解算法在同一台数字控制器实现。