CN109101033B - 一种基于曲柄连杆机构的六自由平台姿态正解方法 - Google Patents

Abstract

本发明提供一种六自由度平台的姿态正解方法，适用于基于曲柄连杆机构的六自由度平台。首先，建立平台基座坐标系，确定六个驱动电机或其传动机构的驱动轴与曲柄连接轴承的中心点坐标；接着，确定曲柄与连杆连接处的轴承中心坐标；然后，根据连杆的长度、上平台各个支撑点的几何关系，得到平台和支撑点的一组线性方程；最后，求解方程组得到上平台的姿态和位置。本发明所提供的方法能够解决基于曲柄连杆机构的六自由度平台姿态正解问题。利用基座与上平台上的轴承连接点、曲柄、连杆的几何关系列写方程组，求解方程组，得到上平台的三轴姿态角和相对位置得到了曲柄连杆结构的六自由度平台的姿态正解方法。

Description

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

技术领域

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

背景技术

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

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

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

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

发明内容

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

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

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

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

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

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

定义曲柄和电机或其传动机构的驱动轴连接轴承的中心点为支撑点A_i(i＝1,…,6)，六个支撑点构成一个六边形A₁A₂A₃A₄A₅A₆。根据机械结构确定六个支撑点的坐标A_i(x_Ai,y_Ai,z_Ai)(i＝1,…,6)。

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

如图3所示，定义与支撑点A_i连接的曲柄LA_i处于坐标系E₀参考平面(xoy平面)内时为初始位置，记LA_i的初始位置相量

记LA_i当前时刻位置

与初始位置

的夹角α_i,(i＝1～6,逆时针为正)；曲柄与连杆连接处的轴承中心点D_i；联结连杆与上平台连接处的支撑轴承的中心点为B_i(i＝1,…,6)；曲柄LA_i的旋转轴为矢量

曲柄初始位置

及旋转矢量p_i在参考平面分布如图4所示，由图可知：p_i＝[x_Ai,y_Ai,z_Ai]。

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

Z＝cosα_iI+(1-cosα_i)p_ip_i+sinα_iP_i (1)

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

曲柄LA_i旋转后的矢量

的表达式为：

进而可以得到D_i点的坐标(x_di,y_di,z_di)：

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

定义上平台各支撑轴承球心中心点为：B_i(i＝1,…,6)，B_i在坐标系E₀中的坐标为(x_bi,y_bi,z_bi)。B_i六个点分布如图5所示，这六个点构成六边形B₁B₂B₃B₄B₅B₆，六边形长边长度|B₁B₂|＝|B₃B₄|＝|B₅B₆|＝l_b，短边长度|B₂B₃|＝|B₄B₅|＝|B₆B₁|＝h_b，三条长边的延长线交于三点C_b1、C_b2、C_b3，这三个点在坐标系E₀中的坐标为(x_cj,y_cj,z_cj)(j＝1,2,3)。，显然

为一个等边三角形。

由图5可以知道三角形

的边C_bjC_bk(j＝1,2,3k＝1,2,3且j≠k)的长度为：

|C_bjC_bk|＝2h_b+l_b (6)

B_i、D_i与相邻的C_bj的可以构成一个三角形，设B_i、D_i之间的连杆为LB_i，其矢量表达式为

长度为L_B，则根据B_i、D_i、C_bj的相互关系可以得到：

因为|C_bjB_i|＝h_b，设k＝h_b/|C_bjC_bk|＝h_b/2h_b+l_b，可以将(7)式变形得到：

所以有：

根据公式9可以得到关于C_b1、C_b2、C_b3三个点坐标(x_cj,y_cj,z_cj)(j＝1,2,3)的六个方程：

再根据|C_bjC_bk|＝L_C(j＝1,2,3，k＝1,2,3且j≠k)，可以列写三角形

的边长公式，得到关于(x_cj,y_cj,z_cj)(j＝1,2,3)的另外三个方程：

(x_c1-x_c2)²+(y_c1-y_c2)²+(z_c1-z_c2)²-L_c ²＝0 (16)

这样(10)～(18)式构成了关于x_c1、y_c1、z_c1、x_c2、y_c2、z_c2、x_c3、y_c3、z_c3这九个未知数的方程组。

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

利用求解非线性方程组的方法求解(10)～(18)这九个方程构成的方程组，可以得到x_c1、y_c1、z_c1、x_c2、y_c2、z_c2、x_c3、y_c3、z_c3这九个未知数。

定义与上平台固联的上平台坐标系E_b，该坐标系的定义如图7所示：E_b的参考平面(z_b＝0平面)取上平台六个支撑轴承球心点B_i(i＝1,…,6)所在平面；原点o_b取为上平台几何中心；当六个曲柄处于图1所示的水平位置(即α_i＝0，i＝1,2,…,6)时上平台处于初始状态，初始状态下o_bx_b轴与平台基座坐标系E₀中ox轴平行；o_by_b轴与平台基座坐标系E₀中oy轴平行；此时，E_b坐标系原点距离E₀坐标系原点的高度为z_b0。由于C_b1、C_b2、C_b3分别为处于B₁B₂、B₃B₄、B₅B₆延长线的交点(如图5所示)，所以C_b1、C_b2、C_b3也在E_b的参考平面上。

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

o_bx_b＝(x_c2-x_c3,y_c2-y_c3,z_c2-z_c3) (19)

o_bz_b＝o_bx_b×o_by_b (21)

定义E_b与E₀之间的姿态变换关系：E_b由E₀按3-1-2的顺序依次逆时针旋转坐标轴(即先沿oz轴转动角度

再沿转动后的ox′轴转动角度θ，最后沿转动后的oy″轴转动角度γ)，

θ、γ为姿态角。这三次转动的姿态余弦阵分别为：

E₀到E_b的姿态变换余弦阵

为：

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

T＝[Δx Δy Δz+z_bo]′ (24)

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

平台上任意一点B在E_b坐标系中的坐标(x_b,y_b,z_b)经三轴平移和三轴旋转之后，对应E₀中对应的坐标(x,y,z)为：

上式中(x_bo,y_bo,z_bo)是E_b坐标系原点o_b在E₀中的表达式。

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

将(19)～(21)式对比(27)～(29)式，可以得到三轴姿态角

θ、γ的表达式：

下面计算三轴位置偏移量。将上平台原点o_b在E_b坐标系中的坐标(0,0,0)代入公式(26)，可以得到其在E₀中的坐标(Δx₀，Δy₀，Δz₀+z_bo)，又因为o_b为

中心，所以三轴偏移量为：

Δx_o＝(x_c1+x_c2+x_c3)/3

Δy_o＝(y_c1+y_c2+y_c3)/3

Δz_o＝(z_c1+z_c2+z_c3)/3-z_b0 (30)

三轴姿态角θ、γ、

以及Δx_o、Δy_o、Δz_o即平台当前的姿态和位置，正解问题完成。

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

附图说明

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

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

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

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

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

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

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

具体实施方式

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

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

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

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

基座上曲柄和驱动轴连接轴承的中心点A_i(x_Ai,y_Ai,z_Ai)(i＝1,…,6)A_i(i＝1,…,6)坐标顺次为：

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

上平台为一六边形，短边长度h_b＝0.1m，长边长度l_b＝0.7m，初始状态下，上平台六个支撑轴承球心点分别是：B₁(-0.35,0.2598,0.3659)、B₂(0.35,0.2598,0.3659)、B₃(0.4,0.1732,0.3659)、B₄(0.05,-0.4330,0.3659)、B₅(-0.05,-0.4330,0.3659)、B₆(-0.4,0.1732,0.3659)。

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

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

A₁(-0.21599,0.37413,0)、A₂(0.21599,0.37413,0)、A₃(0.432,0,0)、A₄(0.216,-0.374,0)、A₅(-0.216,-0.374,0)、A₆(-0.432,0,0)。

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

D₁(0.1039，0.0600，0)，D₂(-0.0943，0.0544，-0.0505)，D₃(0，-0.1088，0.0507)，D₄(0.0943，0.0544，-0.0505)，D₅(-0.0942，0.0544，0.0507)，D₆(0，-0.1089，-0.0505)。

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

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

先求取C₁(0.08132，-0.5132，0.3659)，C₂(0.4038，0.3270，0.3659)，C₃(-0.4851，0.1862，0.3659)。

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

θ＝0°，γ＝9°，Δx₀＝0，Δy₀＝0，Δz₀＝0。

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

Claims (1)

1.一种基于曲柄连杆机构的六自由平台姿态正解方法,其特征在于：包括以下步骤：

A.建立平台基座坐标系E₀，根据机械结构确定六个驱动电机或其传动机构的驱动轴与曲柄连接轴承的中心点坐标；

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

定义曲柄和电机或其传动机构的驱动轴连接轴承的中心点为支撑点A_i，其中i＝1,...,6，六个支撑点构成一个六边形A₁A₂A₃A₄A₅A₆；根据机械结构确定六个支撑点的坐标A_i(x_Ai,y_Ai,z_Ai)，其中，i＝1,...,6；

B.根据曲柄当前角位置，确定曲柄与连杆连接处的轴承中心坐标；

定义与支撑点A_i连接的曲柄LA_i处于坐标系E₀参考平面内时为初始位置,其中坐标系E₀参考平面为xoy平面，记的LA_i初始位置相量

记LA_i当前时刻位置

与初始位置

的夹角α_i,其中i＝1～6,且以逆时针为正；

记曲柄与连杆连接处的轴承中心点D_i；联结连杆与上平台连接处的支撑轴承的中心点为B_i，其中i＝1,...,6，曲柄LA_i的旋转轴为矢量

曲柄初始位置

及旋转矢量p_i在参考平面分布，其中p_i＝[x_Ai,y_Ai,z_Ai]；

根据运动学知识中，任何矢量围绕p_i旋转角度α_i的旋转矩阵表达式推导得到：曲柄LA_i旋转后的矢量

的表达式为：

进而可以得到D_i点的坐标(x_di,y_di,z_di)：

C.根据连杆的长度、上平台各个支撑点的几何关系，得到平台和支撑点的一组线性方程；

定义上平台各支撑轴承球心中心点为：B_i，其中i＝1,...,6，B_i在坐标系E₀中的坐标为(x_bi,y_bi,z_bi)；B_i共有六个点，这六个点构成六边形B₁B₂B₃B₄B₅B₆；

记六边形长边长度|B₁B₂|＝|B₃B₄|＝|B₅B₆|＝l_b，短边长度|B₂B₃|＝|B₄B₅|＝|B₆B₁|＝h_b，三条长边的延长线交于三点C_b1、C_b2、C_b3，这三个点在坐标系E₀中的坐标为(x_cj,y_cj,z_cj)，其中j＝1,2,3，

为一个等边三角形；

三角形

的边C_bjC_bk的长度为：|C_bjC_bk|＝2h_b+l_b，其中j＝1,2,3k＝1,2,3且j≠k，B_i、D_i与相邻的C_bj的可以构成一个三角形，设B_i、D_i之间的连杆为LB_i，其矢量表达式为

长度为L_B，则根据B_i、D_i、C_bj的相互关系可以得到：

因为|C_bjB_i|＝h_b，设

可以得到：

进而得到关于C_b1、C_b2、C_b3三个点坐标(x_cj,y_cj,z_cj)的六个方程，其中j＝1,2,3；

再根据|C_bjC_bk|＝L_C，其中j＝1,2,3，k＝1,2,3且j≠k，可以列写三角形

的边长公式，得到关于(x_cj,y_cj,z_cj)的另外三个方程，其中，j＝1,2,3；上述九个方程，构成了关于x_c1、y_c1、z_c1、x_c2、y_c2、z_c2、x_c3、y_c3、z_c3这九个未知数的方程组；

D.求解方程组得到上平台的姿态和位置；

利用求解非线性方程组的方法求解步骤C中的九个方程构成的方程组，可以得到x_c1、y_c1、z_c1、x_c2、y_c2、z_c2、x_c3、y_c3、z_c3这九个未知数；

定义与上平台固联的上平台坐标系E_b：E_b的参考平面，即Z_b＝0平面取上平台六个支撑轴承球心点B_i所在平面，其中i＝1,...,6；原点O_b取为上平台几何中心；

记六个曲柄处于水平位置，即α_i＝0，i＝1,2,...,6时上平台处于初始状态，初始状态下o_bx_b轴与平台基座坐标系E₀中ox轴平行；o_by_b轴与平台基座坐标系E₀中oy轴平行；此时，E_b坐标系原点距离E₀坐标系原点的高度为Z_b0；上平台的三个轴向相量在E₀坐标系中的表达式为：

o_bx_b＝(x_c2-x_c3,y_c2-y_c3,z_c2-z_c3)

o_bz_b＝o_bx_b×o_by_b；

定义E_b与E₀之间的姿态变换关系：E_b由E₀按3-1-2的顺序依次逆时针旋转坐标轴，即先沿oz轴转动角度

再沿转动后的ox′轴转动角度θ，最后沿转动后的oy″轴转动角度γ，

θ、γ为姿态角；得到三次转动的姿态余弦阵分别为：

E₀到E_b的姿态变换余弦阵

为：

六自由度平台还有三个平移运动的自由度，定义的三轴平移矢量T’＝[Δx,Δy,Δz]’，初始时刻E_b与E₀坐标系原点的高度差为z_b0，所以两坐标系之间的平移矢量修正为：T＝[Δx Δy Δz+z_bo]′

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

平台上任意一点B在E_b坐标系中的坐标(x_b,y_b,z_b)经三轴平移和三轴旋转之后，对应E₀中对应的坐标(x,y,z)为：

上式中(x_bo,y_bo,z_bo)是E_b坐标系原点o_b在E₀中的表达式；

由于平移运动不影响矢量的方向，得到E_b坐标系中各坐标轴的轴向矢量在E₀坐标系中的表达式为：

将上平台的三个轴向相量在E₀坐标系中的表达式与上述三个表达式对比，可以得到三轴姿态角

θ、γ的表达式：

将上平台原点o_b在E_b坐标系中的坐标(0,0,0)代入公式

可以得到其在E₀中的坐标(Δx₀，Δy₀，Δz₀+z_bo)，又因为o_b为

中心，求得三轴姿态角θ、γ、

以及Δx_o、Δy_o、Δz_o即平台当前的姿态和位置：

Images