CN117601103B - 基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法、系统与计算机存储介质 - Google Patents

Abstract

本发明提供一种基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法、系统与计算机存储介质，该反解控制方法适用于以曲柄连杆机构为运动机构的三自由度并联运动平台，每一个曲柄连杆机构对应配置三个运动关节，分别为：曲柄与下平台之间的单旋转关节，曲柄由电机驱动，相对下平台做旋转运动；曲柄与连杆的单旋转关节；以及连杆顶端与上平台的球铰关节。定义上铰接点位于连杆的的顶端与上平台的球铰球心位置，铰接点位于连杆的运动平面与电机旋转轴延长线的交点位置。利用余弦定理和三角形摆动误差修正，获得三自由度曲柄连杆并联运动平台的位置反解，实现对曲柄连杆运动机构的曲柄旋转角度的精确控制，完成对曲柄摆动的反解。

Description

基于误差修正的三自由度并联运动平台的摆动位姿反解控制 方法、系统与计算机存储介质

技术领域

本发明涉及多自由度并联机器人技术领域，具体而言涉及一种基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法、系统与计算机存储介质。

背景技术

三自由度曲柄连杆并联运动平台是一种轻量化的多自由度运动平台，与传统的运动平台类似的是，将多个运动部件设置在上平台和下平台(底座、地面等)之间，基于运动部件的摆动运动(曲柄连杆摆动角度变化)实现对上平台的位置姿态的控制。在三自由度曲柄连杆并联运动平台的设计中，运动部件是基于电机驱动曲柄连杆的摆动而构造的，通过电机驱动曲柄连杆的摆动角度变化实现对上平台的位置、姿态控制。

与传统的电动缸式并联运动平台相比，采用曲柄连杆实现的三自由度并联运动平台具有体积小、结构简单、成本低的优点，但其位置反解比电动缸并联运动平台复杂。目前典型的基于曲柄连杆并联运动平台位置反解是通过列出曲柄末端点轨迹方程、上铰接点距离曲柄末端点距离为固定常数的原理，建立非线性方程组，求解曲柄末端点位置，进而求解位置反解，其解算获得的近似解，对平台位置和姿态的控制精度较低。

发明内容

根据本发明目的的第一方面，提出一种基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，利用余弦定理和三角形摆动误差修正，获得三自由度曲柄连杆并联运动平台的位置反解，实现对曲柄连杆运动机构的曲柄旋转角度的精确控制。

作为可选的实施例的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，包括以下步骤：

定义三自由度并联运动平台的曲柄连杆机构，及其与上平台、下平台之间的运动关节，每一个曲柄连杆机构对应配置三个运动关节，分别为：曲柄与下平台之间的单旋转关节，曲柄由电机驱动，相对下平台做旋转运动；曲柄与连杆的单旋转关节；以及连杆顶端与上平台的球铰关节；

定义上铰接点、下铰接点的位置；

对于任意一曲柄连杆运动机构i，根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取上铰接点与下铰接点连线的初始倾斜角度θ_i以及长度L_i，i＝1，2，3；

根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取其对应的上铰接点与下铰接点连线与对应的曲柄之间的夹角ω_i；

根据三自由度并联运动平台的位置姿态求解上铰接点运动后的坐标；

根据上铰接点运动后的坐标，获取上铰接点与下铰接点连线运动后的倾斜角度θ'_i以及长度L'_i；

根据上铰接点运动后的坐标，获取其对应的上铰接点与下铰接点连线和曲柄运动后的夹角ω'_i；

获取运动后上铰接点距离运动前曲柄末端点的距离L”_i以及运动后上铰接点与下铰接点连线与运动前曲柄的夹角ω”_i；

判断上平台的位置和姿态是否超过曲柄连杆机构的行程，如果任意一组曲柄连杆机构的上铰接点与下铰接点连线的初始长度L'_i大于SUM，或小于SUB，则上平台的位置和姿态超过曲柄连杆机构的行程，判定曲柄连杆机构反解无解并结束流程，否则进入下一步反解过程；其中SUM表示曲柄与连杆长度之和，SUB表示曲柄与连杆长度之差的绝对值；

基于上铰接点与下铰接点连线运动后的倾斜角度θ'_i与上铰接点与下铰接点连线的初始倾斜角度θ_i，计算倾斜角度差值△θ_i；

基于上铰接点与下铰接点连线和曲柄运动后的夹角ω'_i与上铰接点与下铰接点连线与对应的曲柄之间的夹角ω_i，计算曲柄夹角差值△ω_i；以及

基于倾斜角度差值△θ_i以及曲柄夹角差值△ω_i修正误差，获取曲柄摆动的精确角度Angle_i，获得曲柄连杆机构的曲柄摆动角度反解。

作为可选的实施方式，所述定义上铰接点、下铰接点的位置，包括：

上铰接点：位于连杆的的顶端与上平台的球铰球心位置；

曲柄末端点：位于曲柄与连杆的单旋转关节处，具体位于连杆轴线与该单旋转关节轴线交点位置；

下铰接点：位于连杆的运动平面与电机旋转轴延长线的交点位置；

并且，定义上铰接点、曲柄末端点以及下铰接点的坐标如下：

XB_i：下铰接点初始X坐标值；

YB_i：下铰接点初始Y坐标值；

ZB_i：下铰接点初始Z坐标值；

XP_i：上铰接点初始X坐标值；

YP_i：上铰接点初始Y坐标值；

ZP_i：上铰接点初始Z坐标值；

XM_i：曲柄末端点初始X坐标值；

YM_i：曲柄末端点初始Y坐标值；

ZM_i：曲柄末端点初始Z坐标值；

XP'_i：上铰接点运动后的X坐标值；

YP'_i：上铰接点运动后的Y坐标值；

ZP'_i：上铰接点运动后的Z坐标值。

根据本发明目的的第二方面，还提出一种计算机系统，包括：

一个或多个处理器；以及

存储器，存储可被操作的指令；

其中，所述指令在通过一个或多个处理器执行时使得前述的一个或多个处理器执行操作，所述操作包括执行前述实施例的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法的过程。

根据本发明目的的第三方面，还提出一种计算机可读存储介质，用于存储一个或多个程序，所述一个或多个程序包括能够被一个或者多个处理器执行的指令或者指令集；

其中，所述指令或者指令集在由一个或多个处理器执行时，执行前述实施例的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法的过程。

应当理解，前述构思以及在下面更加详细地描述的额外构思的所有组合只要在这样的构思不相互矛盾的情况下都可以被视为本公开的发明主题的一部分。另外，所要求保护的主题的所有组合都被视为本公开的发明主题的一部分。

结合附图从下面的描述中可以更加全面地理解本发明教导的前述和其他方面、实施例和特征。本发明的其他附加方面例如示例性实施方式的特征和/或有益效果将在下面的描述中显见，或通过根据本发明教导的具体实施方式的实践中得知。

附图说明

附图不意在按比例绘制。在附图中，在各个图中示出的每个相同或近似相同的组成部分可以用相同的标号表示。为了清晰起见，在每个图中，并非每个组成部分均被标记。现在，将通过例子并参考附图来描述本发明的各个方面的实施例。

图1是根据本发明实施例提供的三自由度曲柄连杆并联运动平台的结构示意图。

图2是根据本发明实施例提供的三自由度曲柄连杆并联运动平台的侧视图。

图3是根据本发明实施例提供的三自由度曲柄连杆并联运动平台的上铰接点与下铰接点的定义示意图。

图4是根据本发明实施例提供的三自由度曲柄连杆并联运动平台的曲柄连杆机构的等效示意图。

图5是根据本发明实施例提供的三自由度曲柄连杆并联运动平台的曲柄摆动反解原理示意图。

具体实施方式

为了更了解本发明的技术内容，特举具体实施例并配合所附图式说明如下。

在本公开中参照附图来描述本发明的各方面，附图中示出了许多说明的实施例。本公开的实施例不必定意在包括本发明的所有方面。应当理解，上面介绍的多种构思和实施例，以及下面更加详细地描述的那些构思和实施方式可以以很多方式中任意一种来实施，这是因为本发明所公开的构思和实施例并不限于任何实施方式。另外，本发明公开的一些方面可以单独使用，或者与本发明公开的其他方面的任何适当组合来使用。

结合图1、2、3所示，根据本发明实施例提供的三自由度曲柄连杆并联运动平台，是一种由上平台、下平台(例如大地、固定平台等构成)以及位于上平台、下平台之间的曲柄连杆机构构成的小体积、占地面积小、质量轻、成本低的轻量化运动平台，适用于家居、家庭娱乐以及理疗养生等领域应用。

结合图1、2所示，三自由度曲柄连杆并联运动平台由三组曲柄连杆机构驱动，每一组曲柄连杆机构在上平台的下方按照等边三角形的位置分布，并且每一组曲柄连杆机构由一个电机(横卧在下平台)驱动，使得曲柄摆动，进一步带动连杆运动，连杆的末端与上平台底部的球铰铰接连接，通过连杆的运动驱动上平台的位置、姿态发生变化，从而实现运动模拟。

结合图1、2、3所示，在本发明的实施例中，提出一种三自由度曲柄连杆并联运动平台的位置反解方法，在已知运动平台即上平台的位置和姿态条件下，求解曲柄的摆动，即旋转角度。

在本发明的实施例中，首先通过三自由度曲柄连杆并联运动平台的运动位置求解上铰接点坐标，然后计算上铰接点、下铰接点、曲柄末端点组成的三角形边长，通过余弦定理计算运动前后的三角形角度变化，最后通过三角形摆动误差修正，得到高精度的曲柄摆动角度，实现曲柄旋转的反解。

作为可选的实施例，结合图2-5所示，基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，包括：

定义三自由度并联运动平台的曲柄连杆机构，及其与上平台、下平台之间的运动关节，每一个曲柄连杆机构对应配置三个运动关节，分别为：曲柄与下平台之间的单旋转关节，曲柄由电机驱动，相对下平台做旋转运动；曲柄与连杆的单旋转关节；以及连杆顶端与上平台的球铰关节，如图1、2所示；

定义平台铰接点的位置，如图3所示的，包括上铰接点、下铰接点以及衢州末端点的位置；

对于任意一曲柄连杆运动机构i，根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取上铰接点与下铰接点连线的初始倾斜角度θ_i以及长度L_i，i＝1，2，3；

根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取其对应的上铰接点与下铰接点连线与对应的曲柄之间的夹角ω_i；

根据三自由度并联运动平台的位置姿态求解上铰接点运动后的坐标；

根据上铰接点运动后的坐标，获取上铰接点与下铰接点连线运动后的倾斜角度θ'_i以及长度L'_i；

根据上铰接点运动后的坐标，获取其对应的上铰接点与下铰接点连线和曲柄运动后的夹角ω'_i；

获取运动后上铰接点距离运动前曲柄末端点的距离L”_i以及运动后上铰接点与下铰接点连线与运动前曲柄的夹角ω”_i；

判断上平台的位置和姿态是否超过曲柄连杆机构的行程，如果任意一组曲柄连杆机构的上铰接点与下铰接点连线的初始长度L'_i大于SUM，或小于SUB，则上平台的位置和姿态超过曲柄连杆机构的行程，判定曲柄连杆机构反解无解并结束流程，否则进入下一步反解过程；其中SUM表示曲柄与连杆长度之和，SUB表示曲柄与连杆长度之差的绝对值；

基于上铰接点与下铰接点连线运动后的倾斜角度θ'_i与上铰接点与下铰接点连线的初始倾斜角度θ_i，计算倾斜角度差值△θ_i；

基于上铰接点与下铰接点连线和曲柄运动后的夹角ω'_i与上铰接点与下铰接点连线与对应的曲柄之间的夹角ω_i，计算曲柄夹角差值△ω_i；以及

结合图5所示，基于倾斜角度差值△θ_i以及曲柄夹角差值△ω_i修正误差，获取曲柄摆动的精确角度Angle_i，获得曲柄连杆机构的曲柄摆动角度反解。

结合图1、2、3所示的示例，定义上铰接点、下铰接点的位置，包括：

上铰接点：位于连杆的的顶端与上平台的球铰球心位置；

曲柄末端点：位于曲柄与连杆的单旋转关节处，具体位于连杆轴线与该单旋转关节轴线交点位置；

下铰接点：位于连杆的运动平面与电机旋转轴延长线的交点位置。

其中，结合图2、3所示，定义上铰接点、曲柄末端点以及下铰接点的坐标如下：

XB_i：下铰接点初始X坐标值；

YB_i：下铰接点初始Y坐标值；

ZB_i：下铰接点初始Z坐标值；

XP_i：上铰接点初始X坐标值；

YP_i：上铰接点初始Y坐标值；

ZP_i：上铰接点初始Z坐标值；

XM_i：曲柄末端点初始X坐标值；

YM_i：曲柄末端点初始Y坐标值；

ZM_i：曲柄末端点初始Z坐标值；

XP'_i：上铰接点运动后的X坐标值；

YP'_i：上铰接点运动后的Y坐标值；

ZP'_i：上铰接点运动后的Z坐标值。

进一步的实施例中，根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取上铰接点与下铰接点连线的初始倾斜角度θ_i以及长度L_i，包括：

进一步的实施例中，根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取其对应的上铰接点与下铰接点连线与对应的曲柄之间的夹角ω_i，包括：

其中，Crank表示曲柄的长度，Rod表示连杆的长度；假设在初始位置时曲柄水平，则初始时θ_i＝ω_i。

进一步的实施例中，根据三自由度并联运动平台的位置姿态求解上铰接点运动后的坐标，包括：

构建如下的位置姿态方程：

因为△γ、△X、△Y均为0，则有：

进一步的实施例中，根据上铰接点运动后的坐标，获取上铰接点与下铰接点连线运动后的倾斜角度θ'_i以及长度L'_i，包括：

进一步的实施例中，根据上铰接点运动后的坐标，获取其对应的上铰接点与下铰接点连线和曲柄运动后的夹角ω'_i，包括：

进一步的实施例中，获取运动后上铰接点距离运动前曲柄末端点的距离L”_i以及运动后上铰接点与下铰接点连线与运动前曲柄的夹角ω”_i，包括：

进一步的实施例中，结合图4、5所示，基于倾斜角度差值△θ_i以及曲柄夹角差值△ω_i修正误差，获取曲柄摆动的精确角度Angle_i，获得曲柄连杆机构的曲柄摆动角度反解，包括：

当ω_i>ω”_i时，Angle_i＝△ω_i+△θ_i；

当ω_i<ω”_i时，Angle_i＝△ω_i—△θ_i；

当ω_i＝ω”_i时，Angle_i＝△ω_i。

由此，在本发明所构建的轻量化的基于曲柄连杆机构实现的三自由度运动平台的基础上，利用余弦定理和三角形摆动误差修正，获得三自由度曲柄连杆并联运动平台的位置反解，实现对曲柄连杆运动机构的曲柄旋转角度的精确控制。

结合以上实施例的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，根据本发明公开的实施例，还提出一种计算机系统，其特征在于，包括：一个或多个处理器；以及存储器，其存储可被操作的指令。

其中，所述指令在通过一个或多个处理器执行时使得前述的一个或多个处理器执行操作，所述操作包括执行前述实施例的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法的过程。

结合以上实施例的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，根据本发明公开的实施例，还提出一种计算机可读存储介质，用于存储一个或多个程序，所述一个或多个程序包括能够被一个或者多个处理器执行的指令或者指令集。

其中，所述指令或者指令集在由一个或多个处理器执行时，执行前述实施例的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法的过程。

虽然本发明已以较佳实施例揭露如上，然其并非用以限定本发明。本发明所属技术领域中具有通常知识者，在不脱离本发明的精神和范围内，当可作各种的更动与润饰。因此，本发明的保护范围当视权利要求书所界定者为准。

Claims (11)

1.一种基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，包括：

定义三自由度并联运动平台的曲柄连杆机构，及其与上平台、下平台之间的运动关节，每一个曲柄连杆机构对应配置三个运动关节，分别为：曲柄与下平台之间的单旋转关节，曲柄由电机驱动，相对下平台做旋转运动；曲柄与连杆的单旋转关节；以及连杆顶端与上平台的球铰关节；

定义上铰接点、下铰接点的位置；

对于任意一曲柄连杆运动机构i，根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取上铰接点与下铰接点连线的初始倾斜角度θ_i以及长度L_i，i＝1，2，3；

根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取其对应的上铰接点与下铰接点连线与对应的曲柄之间的夹角ω_i；

根据三自由度并联运动平台的位置姿态求解上铰接点运动后的坐标；

根据上铰接点运动后的坐标，获取上铰接点与下铰接点连线运动后的倾斜角度θ'_i以及长度L'_i；

根据上铰接点运动后的坐标，获取其对应的上铰接点与下铰接点连线和曲柄运动后的夹角ω'_i；

获取运动后上铰接点距离运动前曲柄末端点的距离L”_i以及运动后上铰接点与下铰接点连线与运动前曲柄的夹角ω”_i；

判断上平台的位置和姿态是否超过曲柄连杆机构的行程，如果任意一组曲柄连杆机构的上铰接点与下铰接点连线的初始长度L'_i大于SUM，或小于SUB，则上平台的位置和姿态超过曲柄连杆机构的行程，判定曲柄连杆机构反解无解并结束流程，否则进入下一步反解过程；其中SUM表示曲柄与连杆长度之和，SUB表示曲柄与连杆长度之差的绝对值；

基于上铰接点与下铰接点连线运动后的倾斜角度θ'_i与上铰接点与下铰接点连线的初始倾斜角度θ_i，计算倾斜角度差值△θ_i；

基于上铰接点与下铰接点连线和曲柄运动后的夹角ω'_i与上铰接点与下铰接点连线与对应的曲柄之间的夹角ω_i，计算曲柄夹角差值△ω_i；以及

基于倾斜角度差值△θ_i以及曲柄夹角差值△ω_i修正误差，获取曲柄摆动的精确角度Angle_i，获得曲柄连杆机构的曲柄摆动角度反解。

2.根据权利要求1所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述定义上铰接点、下铰接点的位置，包括：

上铰接点：位于连杆的的顶端与上平台的球铰球心位置；

曲柄末端点：位于曲柄与连杆的单旋转关节处，具体位于连杆轴线与该单旋转关节轴线交点位置；

下铰接点：位于连杆的运动平面与电机旋转轴延长线的交点位置；

并且，定义上铰接点、曲柄末端点以及下铰接点的坐标如下：

XB_i：下铰接点初始X坐标值；

YB_i：下铰接点初始Y坐标值；

ZB_i：下铰接点初始Z坐标值；

XP_i：上铰接点初始X坐标值；

YP_i：上铰接点初始Y坐标值；

ZP_i：上铰接点初始Z坐标值；

XM_i：曲柄末端点初始X坐标值；

YM_i：曲柄末端点初始Y坐标值；

ZM_i：曲柄末端点初始Z坐标值；

XP'_i：上铰接点运动后的X坐标值；

YP'_i：上铰接点运动后的Y坐标值；

ZP'_i：上铰接点运动后的Z坐标值。

3.根据权利要求2所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取上铰接点与下铰接点连线的初始倾斜角度θ_i以及长度L_i，包括：

4.根据权利要求3所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述根据第i个曲柄连杆运动机构对应的上铰接点、下铰接点的位置，获取其对应的上铰接点与下铰接点连线与对应的曲柄之间的夹角ω_i，包括：

其中，Crank表示曲柄的长度，Rod表示连杆的长度；假设在初始位置时曲柄水平，则初始时θ_i＝ω_i。

5.根据权利要求2所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述根据三自由度并联运动平台的位置姿态求解上铰接点运动后的坐标，包括：

构建如下的位置姿态方程：

因为△γ、△X、△Y均为0，则有：

6.根据权利要求5所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述根据上铰接点运动后的坐标，获取上铰接点与下铰接点连线运动后的倾斜角度θ'_i以及长度L'_i，包括：

7.根据权利要求6所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述根据上铰接点运动后的坐标，获取其对应的上铰接点与下铰接点连线和曲柄运动后的夹角ω'_i，包括：

其中，Crank表示曲柄的长度，Rod表示连杆的长度。

8.根据权利要求7所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述获取运动后上铰接点距离运动前曲柄末端点的距离L”_i以及运动后上铰接点与下铰接点连线与运动前曲柄的夹角ω”_i，包括：

其中，Crank表示曲柄的长度，Rod表示连杆的长度。

9.根据权利要求1所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法，其特征在于，所述基于倾斜角度差值△θ_i以及曲柄夹角差值△ω_i修正误差，获取曲柄摆动的精确角度Angle_i，获得曲柄连杆机构的曲柄摆动角度反解，包括：

当ω_i>ω”_i时，Angle_i＝△ω_i+△θ_i；

当ω_i<ω”_i时，Angle_i＝△ω_i—△θ_i；

当ω_i＝ω”_i时，Angle_i＝△ω_i。

10.一种计算机系统，其特征在于，包括：

一个或多个处理器；以及

存储器，存储可被操作的指令；

其中，所述指令在通过一个或多个处理器执行时使得前述的一个或多个处理器执行操作，所述操作包括执行所述权利要求1-9中任意一项所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法的过程。

11.一种计算机可读存储介质，用于存储一个或多个程序，其特征在于，所述一个或多个程序包括能够被一个或者多个处理器执行的指令或者指令集；

其中，所述指令或者指令集在由一个或多个处理器执行时，执行所述权利要求1-9中任意一项所述的基于误差修正的三自由度并联运动平台的摆动位姿反解控制方法的过程。