CN112613159B - 一种Stewart平台6-UHU结构运动学求解及误差标定方法 - Google Patents

Abstract

本发明公开了一种Stewart平台6‑UHU结构运动学求解及误差标定方法，建立Stewart平台6‑UHU结构运动学模型，基于静平台下基座铰链点坐标以及动平台下铰链点坐标转换到静平台下的坐标，计算各电动缸长度及其方向上的单位矢量；根据静平台上的虎克铰的运动特性，建立旋转矩阵，并与静平台下电动缸的方向矢量联立，得到静平台上的虎克铰的旋转角；将平台反过来看，以动平台为基准，求解得到动平台上的虎克铰的旋转角；基于该螺旋补偿角度建立逆解方程对电动缸杆长进行补偿，得到补偿后的杆长；通过迭代最小二乘估计法得到最佳误差估计参数。本发明通过在算法中的螺旋补偿与对机械结构误差的标定，提高Stewart平台精度。

Description

一种Stewart平台6-UHU结构运动学求解及误差标定方法

技术领域

本发明涉及并联机构技术领域，特别是涉及一种Stewart平台6-UHU结构运动学求解及误差标定方法。

背景技术

Stewart平台最初是作为飞行模拟器被研发出来，目前Stewart平台技术相对成熟，但是大多数依旧是用来作为模拟装置，例如6-RUS结构、6-SPS结构、6-PUS/UPS结构等，Stewart模拟平台在船舶自平衡、飞行模拟器、3D娱乐设施等方面应用广泛，其要求精度不高，模拟平台能够满足其自身需求。而精度要求较高的装置则不能用Stewart模拟平台作为对象，并联机构精度是衡量机床性能的重要指标，是衡量机械制造能力和技术发展水平的体现，也是国家科技水平的重要标志之一。模拟平台其存在的问题在于其结构，在结构设计上，模拟平台大多数无法完成对精度的提高，在算法上，大多数Stewart平台无法完成对结构误差的补偿，在误差标定上，没办法对其进行误差标定。

发明内容

本发明的目的是提供一种Stewart平台6-UHU结构运动学求解及误差标定方法，在六自由度并联结构的基础上，通过在算法中的螺旋补偿与对机械结构误差的标定，提高Stewart平台精度。

为实现上述目的，本发明提供了如下方案：

一种Stewart平台6-UHU结构运动学求解及误差标定方法，应用于六自由度并联结构，包括以下步骤：

S1，建立Stewart平台6-UHU结构运动学模型，取一条支链，建立空间直角坐标系，得到静平台下基座铰链点坐标、动平台下动平台铰链点坐标以及动平台下铰链点坐标转换到静平台下的坐标；

S2，基于静平台下基座铰链点坐标以及动平台下铰链点坐标转换到静平台下的坐标，计算各电动缸长度及其方向上的单位矢量，此长度为未补偿前带有误差的长度，并计算静平台下电动缸的方向矢量；

S3，根据静平台上的虎克铰的运动特性，建立旋转矩阵，并与静平台下电动缸的方向矢量联立，得到静平台上的虎克铰的旋转角；

S4，将平台反过来看，以动平台为基准，求得动平台下的电动缸方向矢量，根据旋转矩阵的性质，得动平台下的电动缸方向矢量，采用步骤S3的方法求解得到动平台上的虎克铰的旋转角；

S5，计算电动缸螺旋副伴随的螺旋补偿角度，求解逆解算法为：基于该螺旋补偿角度建立逆解方程对电动缸杆长进行补偿，得到补偿后的杆长；求解正解算法为：基于该螺旋补偿角度建立牛顿迭代公式，并基于牛顿迭代公式求导可得雅可比矩阵；

S6，建立误差参数模型，基于李群和李代数的关系，可得到动平台位姿误差旋量，通过迭代最小二乘估计法得到最佳误差估计参数。

进一步的，所述步骤S1中，

静平台下基座铰链点坐标为：B_i＝[Bx_i By_i Bz_i]^T；

动平台下动平台铰链点坐标为：A_i＝[Ax_i Ay_i Az_i]^T；

动平台下铰链点坐标转换到静平台下的坐标为：C_i＝[Cx_i Cy_i Cz_i]^T；

其中，C_i＝R·A_i，

R表示末端姿态旋转矩阵：

其中，s、c分别为sin、cos的简写。

进一步的，所述步骤S2中，

计算各电动缸长度及其方向上的单位矢量，此长度为未补偿前带有误差的长度ll_i，

计算静平台下电动缸的单位方向矢量，用Si表示为：

进一步的，所述步骤S3中，根据静平台上的虎克铰的运动特性，建立旋转矩阵，并与静平台下电动缸的方向矢量联立，得到静平台上的虎克铰的旋转角，具体包括：

对静平台上的虎克铰来说，先绕Y轴转动，转动角度记为θ_i1，再绕X轴旋转，旋转角度记为θ_i2，所以，静平台上虎克铰的旋转矩阵可以用Q1表示：

根据公式(2)得到：

联立公式(3)和公式(4)得到：

θ_i1＝atan2(Sx_i,Sz_i) (5)

θ_i2＝asin(-Sy_i) (6)

进一步的，所述步骤S4中，

将平台反过来看，以动平台为基准，求得动平台下的电动缸方向矢量Sa_i：

Sa_i＝R^-1·S_i (7)

根据旋转矩阵的性质：

R^-1＝R^T (8)

得动平台下的电动缸方向矢量：

Sa_i＝R^T·S_i＝[Sax_i Say_i Saz_i]^T (9)

与求解θ_i1、θ_i2原理一样，得到θ_i4、θ_i5，θ_i4为动平台端虎克铰上绕Y轴转动的角度，θ_i5为动平台端虎克铰上绕X轴转动的角度：

θ_i4＝atan2(Sax_i,Saz_i) (10)

θ_i5＝asin(-Say_i) (11)

进一步的，所述步骤S5中，

设电动缸螺旋副伴随的螺旋补偿角度为θ_i3，R_Y4表示θ_i4的旋转矩阵，R_X5表示θ_i5的旋转矩阵，R_X1表示θ_i1的旋转矩阵，R_Y2表示θ_i2的旋转矩阵，可列出公式为：

由此可得到螺旋补偿角度θ_i3；

对电动缸杆长进行补偿，得到补偿后的杆长，即求解逆解算法

l_i＝ll_i-h·θ_i3 (13)

其中，h为电动缸导程；

正解算法依旧需要执行逆解算法中求解螺旋补偿角度，牛顿迭代公式为

F＝[ll_i-h·θ_i3-l_i]^T (14)

对上式求导可得雅可比矩阵，其中，l_i为常数：

最后对位姿的角度和位置赋予初值，通过迭代得到α、β、γ、x、y、z，即求得正解。

进一步的，所述步骤S6中，

正交模型输入为：

X_j＝[l_j,A_xj,A_yj,A_zj,B_xj,B_yj,B_zj] (16)

其中，A_x0、A_y0、A_z0、B_x0、B_y0、B_z0分别表示动平台与静平台铰链座坐标名义参数值，l_j0表示电动缸长度初始名义参数值；

误差参数模型为：

ΔX_j＝[δl,δA_x,δA_y,δA_z,δB_x,δB_y,δB_z] (17)

其中，δA_x，δA_y，δA_z，δB_x,δB_y，δB_z分别表示动平台与静平台铰链座坐标误差参数值，δl表示电动缸长度误差参数值；

基于李群和李代数的关系，得到动平台位姿误差旋量：

ΔY_j＝[δθ_j,δp_j]^T (18)

δθ_j为姿态角度的1×3反对称阵，可由误差旋转变换矩阵ΔR变换得到，δp_j为位置的1×3反对称阵；

最后得到公式：

ΔY＝J·ΔX (19)

利用上式，通过迭代最小二乘估计法，得到最佳误差估计参数。

根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明提供的Stewart平台6-UHU结构运动学求解及误差标定方法，无论是机械结构的设计，还是算法、标定方法的设计，都在一定程度上提高了Stewart平台的精度，相比其他机械结构和模拟平台来说，本发明体现出绝对的优势，在航天装配、导弹位姿测试等高新技术方面具有重要作用。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是六自由度并联结构的结构示意图；

图2是本发明实施例6-UHU空间直角坐标系；

附图标记：1、动平台；2、上端虎克铰；3、电动缸；4、螺旋副；5、下端虎克铰；6、静平台。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种Stewart平台6-UHU结构运动学求解及误差标定方法，在六自由度并联结构的基础上，通过在算法中的螺旋补偿与对机械结构误差的标定，提高Stewart平台精度。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

本发明提供的Stewart平台6-UHU结构运动学求解及误差标定方法，应用于六自由度并联结构，包括以下步骤：

S1，建立Stewart平台6-UHU结构运动学模型，取一条支链，建立空间直角坐标系，得到静平台下基座铰链点坐标、动平台下动平台铰链点坐标以及动平台下铰链点坐标转换到静平台下的坐标；

S2，基于静平台下基座铰链点坐标以及动平台下铰链点坐标转换到静平台下的坐标，计算各电动缸长度及其方向上的单位矢量，此长度为未补偿前带有误差的长度，并计算静平台下电动缸的方向矢量；

S3，根据静平台上的虎克铰的运动特性，建立旋转矩阵，并与静平台下电动缸的方向矢量联立，得到静平台上的虎克铰的旋转角；

S4，将平台反过来看，以动平台为基准，求得动平台下的电动缸方向矢量，根据旋转矩阵的性质，得动平台下的电动缸方向矢量，采用步骤S3的方法求解得到动平台上的虎克铰的旋转角；

S5，计算电动缸螺旋副伴随的螺旋补偿角度，求解逆解算法为：基于该螺旋补偿角度建立逆解方程对电动缸杆长进行补偿，得到补偿后的杆长；求解正解算法为：基于该螺旋补偿角度建立牛顿迭代公式，并基于牛顿迭代公式求导可得雅可比矩阵；

S6，建立误差参数模型，基于李群和李代数的关系，可得到动平台位姿误差旋量，通过迭代最小二乘估计法得到最佳误差估计参数。

如图1所示，所述六自由度并联平台主要由6根电动缸3、静平台6、动平台1、6个上端虎克铰2、6个下端虎克铰5组成，电动缸3上设置有螺旋副4，上端虎克铰2和下端虎克铰5通过电动缸3连接形成六边形。取一条支链来说，建立空间直角坐标系，如图2所示。

其中，所述步骤S1中，

静平台下基座铰链点坐标为：B_i＝[Bx_i By_i Bz_i]^T；

动平台下动平台铰链点坐标为：A_i＝[Ax_i Ay_i Az_i]^T；

动平台下铰链点坐标转换到静平台下的坐标为：C_i＝[Cx_i Cy_i Cz_i]^T；

其中，C_i＝R·A_i，

R表示末端姿态旋转矩阵：

其中，s、c分别为sin、cos的简写。

所述步骤S2中，

计算各电动缸长度及其方向上的单位矢量，此长度为未补偿前带有误差的长度ll_i，

计算静平台下电动缸的单位方向矢量，用S_i表示为：

所述步骤S3中，根据静平台上的虎克铰的运动特性，建立旋转矩阵，并与静平台下电动缸的方向矢量联立，得到静平台上的虎克铰的旋转角，具体包括：

对静平台上的虎克铰来说，先绕Y轴转动，在绕X轴转，对静平台上的虎克铰来说，先绕Y轴转动，转动角度记为θ_i1，再绕X轴旋转，旋转角度记为θ_i2，所以，静平台上虎克铰的旋转矩阵可以用Q1表示，建立旋转矩阵如下：

对其他铰链座来说，需要加上一个角度去变换；根据公式(2)得到：

联立公式(3)和公式(4)得到：

θ_i1＝atan2(Sx_i,Sz_i) (5)

θ_i2＝asin(-Sy_i) (6)

所述步骤S4中，

将平台反过来看，以动平台为基准，求得动平台下的电动缸方向矢量Sa_i：

Sa_i＝R^-1·S_i (7)

对其他铰链座来说，需要加上一个角度去变换；根据旋转矩阵的性质：

R^-1＝R^T (8)

得动平台下的电动缸方向矢量：

Sa_i＝R^T·S_i＝[Sax_i Say_i Saz_i]^T (9)

与求解θ_i1、θ_i2原理一样，得到θ_i4、θ_i5，θ_i4为动平台端虎克铰上绕Y轴转动的角度，θ_i5为动平台端虎克铰上绕X轴转动的角度：

θ_i4＝atan2(Sax_i,Saz_i) (10)

θ_i5＝asin(-Say_i) (11)

所述步骤S5中，

设电动缸螺旋副伴随的螺旋补偿角度为θ_i3，R_Y4表示θ_i4的旋转矩阵，R_X5表示θ_i5的旋转矩阵，R_X1表示θ_i1的旋转矩阵，R_Y2表示θ_i2的旋转矩阵，可列出公式为：

由此可得到螺旋补偿角度θ_i3；

对电动缸杆长进行补偿，得到补偿后的杆长，即求解逆解算法

l_i＝ll_i-h·θ_i3 (13)

其中，h为电动缸导程；

Stewart精度平台正解算法才用相对简单的牛顿-欧拉迭代法，由于公式较复杂，需要对公式进行逐步分解，这种方法计算复杂，但是方法简单。部分算法依旧需要执行逆解算法中求解螺旋补偿角度，下面主要介绍正解的雅克比。正解算法依旧需要执行逆解算法中求解螺旋补偿角度，牛顿迭代公式为

F＝[ll_i-h·θ_i3-l_i]^T (14)

对上式求导可得雅可比矩阵，其中，l_i为常数：

最后对位姿的角度和位置赋予初值，通过迭代得到α、β、γ、x、y、z，即求得正解。逆解用来驱动电机转动，正解一般用来复位。

Stewart平台动平台在空间中作8水平6因素的正交实验，共产生64组位姿，精度要求至少达到0.01mm，用激光跟踪仪测量这64组位姿，将数据传输给参数辨识程序进行处理，得到参数误差。所述步骤S6中，

正交模型输入为：

X_j＝[l_j,A_xj,A_yj,A_zj,B_xj,B_yj,B_zj] (16)

其中，A_x0、A_y0、A_z0、B_x0、B_y0、B_z0分别表示动平台与静平台铰链座坐标名义参数值，l_j0表示电动缸长度初始名义参数值；

误差参数模型为：

ΔX_j＝[δl,δA_x,δA_y,δA_z,δB_x,δB_y,δB_z] (17)

其中，δA_x，δA_y，δA_z，δB_x,δB_y，δB_z分别表示动平台与静平台铰链座坐标误差参数值，δl表示电动缸长度误差参数值；δ，Δ默认是误差表示的形式；

基于李群和李代数的关系，得到动平台位姿误差旋量：

ΔY_j＝[δθ_j,δp_j]^T (18)

δθ_j为姿态角度的1×3反对称阵，可由误差旋转变换矩阵ΔR变换得到，δp_j为位置的1×3反对称阵；

最后得到公式：

ΔY＝J·ΔX (19)

利用上式，通过迭代最小二乘估计法，得到最佳误差估计参数。

本发明提供的Stewart平台6-UHU结构运动学求解及误差标定方法，无论是机械结构的设计，还是算法、标定方法的设计，都在一定程度上提高了Stewart平台的精度，相比其他机械结构和模拟平台来说，本发明体现出绝对的优势，在航天装配、导弹位姿测试等高新技术方面具有重要作用。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (7)

1.一种Stewart平台6-UHU结构运动学求解及误差标定方法，应用于六自由度并联结构，其特征在于，包括以下步骤：

S1，建立Stewart平台6-UHU结构运动学模型，取一条支链，建立空间直角坐标系，得到静平台下基座铰链点坐标、动平台下动平台铰链点坐标以及动平台下铰链点坐标转换到静平台下的坐标；

S2，基于静平台下基座铰链点坐标以及动平台下铰链点坐标转换到静平台下的坐标，计算各电动缸长度及其方向上的单位矢量，此长度为未补偿前带有误差的长度，并计算静平台下电动缸的方向矢量；

S3，根据静平台上的虎克铰的运动特性，建立旋转矩阵，并与静平台下电动缸的方向矢量联立，得到静平台上的虎克铰的旋转角；

S4，将平台反过来看，以动平台为基准，求得动平台下的电动缸方向矢量，根据旋转矩阵的性质，得动平台下的电动缸方向矢量，采用步骤S3的方法求解得到动平台上的虎克铰的旋转角；

S5，计算电动缸螺旋副伴随的螺旋补偿角度，求解逆解算法为：基于该螺旋补偿角度建立逆解方程对电动缸杆长进行补偿，得到补偿后的杆长；求解正解算法为：基于该螺旋补偿角度建立牛顿迭代公式，并基于牛顿迭代公式求导可得雅可比矩阵；

S6，建立误差参数模型，基于李群和李代数的关系，可得到动平台位姿误差旋量，通过迭代最小二乘估计法得到最佳误差估计参数。

2.根据权利要求1所述的Stewart平台6-UHU结构运动学求解及误差标定方法，其特征在于，所述步骤S1中，

静平台下基座铰链点坐标为：B_i＝[Bx_i By_i Bz_i]^T；

动平台下动平台铰链点坐标为：A_i＝[Ax_i Ay_i Az_i]^T；

动平台下铰链点坐标转换到静平台下的坐标为：C_i＝[Cx_i Cy_i Cz_i]^T；

其中，C_i＝R·A_i；

R表示末端姿态旋转矩阵：

其中，s、c分别为sin、cos的简写。

3.根据权利要求2所述的Stewart平台6-UHU结构运动学求解及误差标定方法，其特征在于，所述步骤S2中，

计算各电动缸长度及其方向上的单位矢量，此长度为未补偿前带有误差的长度ll_i，

计算静平台下电动缸的单位方向矢量，用S_i表示为：

4.根据权利要求3所述的Stewart平台6-UHU结构运动学求解及误差标定方法，其特征在于，所述步骤S3中，根据静平台上的虎克铰的运动特性，建立旋转矩阵，并与静平台下电动缸的方向矢量联立，得到静平台上的虎克铰的旋转角，具体包括：

对静平台上的虎克铰来说，先绕Y轴转动，转动角度记为θ_i1，再绕X轴旋转，旋转角度记为θ_i2，所以，静平台上虎克铰的旋转矩阵可以用Q1表示：

根据公式(2)得到：

联立公式(3)和公式(4)得到：

θ_i1＝atan2(Sx_i,Sz_i) (5)

θ_i2＝asin(-Sy_i) (6)。

5.根据权利要求4所述的Stewart平台6-UHU结构运动学求解及误差标定方法，其特征在于，所述步骤S4中，

将平台反过来看，以动平台为基准，求得动平台下的电动缸方向矢量，用Sa_i表示为：

Sa_i＝R^-1·S_i (7)

根据旋转矩阵的性质：

R^-1＝R^T (8)

得动平台下的电动缸方向矢量：

Sa_i＝R^T·S_i＝[Sax_i Say_i Saz_i]^T (9)

与求解θ_i1、θ_i2原理一样，得到θ_i4、θ_i5，θ_i4为动平台端虎克铰上绕Y轴转动的角度，θ_i5为动平台端虎克铰上绕X轴转动的角度：

θ_i4＝atan2(Sax_i,Saz_i) (10)

θ_i5＝asin(-Say_i) (11)。

6.根据权利要求5所述的Stewart平台6-UHU结构运动学求解及误差标定方法，其特征在于，所述步骤S5中，

设电动缸螺旋副伴随的螺旋补偿角度为θ_i3，R_Y4表示θ_i4的旋转矩阵，R_X5表示θ_i5的旋转矩阵，R_X1表示θ_i1的旋转矩阵，R_Y2表示θ_i2的旋转矩阵，可列出公式为：

由此可得到螺旋补偿角度θ_i3；

对电动缸杆长进行补偿，得到补偿后的杆长，即求解逆解算法

l_i＝ll_i-h·θ_i3 (13)

其中，h为电动缸导程；

正解算法依旧需要执行逆解算法中求解螺旋补偿角度，牛顿迭代公式为

F＝[ll_i-h·θ_i3-l_i]^T (14)

对上式求导可得雅可比矩阵，其中，l_i为常数：

最后对位姿的角度和位置赋予初值，通过迭代得到α、β、γ、x、y、z，即求得正解。

7.根据权利要求6所述的Stewart平台6-UHU结构运动学求解及误差标定方法，其特征在于，所述步骤S6中，

正交模型输入为：

X_j＝[l_j,A_xj,A_yj,A_zj,B_xj,B_yj,B_zj] (16)

A_xj＝A_x0+δA_x,A_yj＝A_y0+δA_y,A_zj＝A_z0+δA_z

B_xj＝B_x0+δB_x,B_yj＝B_y0+δB_y,B_zj＝B_z0+δB_z

l_j＝l_j0+δl

其中，A_x0、A_y0、A_z0、B_x0、B_y0、B_z0分别表示动平台与静平台铰链座坐标名义参数值，l_j0表示电动缸长度初始名义参数值；

误差参数模型为：

ΔX_j＝[δl,δA_x,δA_y,δA_z,δB_x,δB_y,δB_z] (17)

其中，δA_x，δA_y，δA_z，δB_x,δB_y，δB_z分别表示动平台与静平台铰链座坐标误差参数值，δl表示电动缸长度误差参数值；

基于李群和李代数的关系，得到动平台位姿误差旋量：

ΔY_j＝[δθ_j,δp_j]^T (18)

δθ_j为姿态角度的1×3反对称阵，可由误差旋转变换矩阵ΔR变换得到，δp_j为位置的1×3反对称阵；

最后得到公式：

ΔY＝J·ΔX (19)

利用上式，通过迭代最小二乘估计法，得到最佳误差估计参数。