CN112207800B - 三自由度转动曲柄连杆并联平台位姿控制方法 - Google Patents

Abstract

本发明涉及并联机器人运动学领域，具体涉及一种三自由度转动曲柄连杆并联平台位姿控制方法。本发明位姿控制方法包括：构建与平台位姿向量相关的目标函数和位姿向量矩阵，进行初始化，采用单纯型算法优化所述位姿向量矩阵；约束位姿向量搜索边界，对优化获得的位姿向量矩阵中的位姿向量执行惩罚操作，剔除超出搜索边界的位姿向量；搜索剩余的位姿向量对应的目标函数最小值，输出目标函数最小值对应的最优位姿向量。采用惩罚函数算法，避免寻优过程超出边界，以解决机构极限行程外正解无解，致使搜索算法不收敛的情况，能够实现快速收敛。本发明可应用于并联机器人的控制。

Description

三自由度转动曲柄连杆并联平台位姿控制方法

技术领域

本发明涉及并联机器人运动学领域，具体涉及一种三自由度转动曲柄连杆并联平台位姿控制方法。

背景技术

并联机器人是机器人中重要的一类，具备机构紧凑、刚度高、负载能力强等优势。其中，三自由度转动曲柄连杆(简称3-RSS/S)并联平台被广泛应用于航天、医疗和工业装配领域，其精确定姿和力学响应特性正是当下研究与应用的热点，而平台位姿监控是这些研究中不可或缺的技术环节。在实际生产中，由于安装传感器会较大地限制平台行程范围，因此直接测算平台位姿的方法往往难以满足使用要求，依靠促动器末端驱动量信息，通过正运动学求解平台位姿的方法往往更具应用前景。但是，三自由度并联平台正运动学求解的难点主要在于：并联机构正运动学无法直接求解，往往需通过逆解迭代或解析雅克比矩阵的方法求解，而在实际应用中机构驱动端输入量测量存在扰动，使得传统算法难以保证求解的准确性和稳定性；训练评估与反馈对平台位姿信息获取的实时性要求较高，需要正解求解算法快速收敛，而依据现有技术，三自由度转动曲柄连杆并联平台逆解模型依赖二元二次方程组建立，需要根据促动器相角范围判断可行解，这使得构建正运动学解析解的难度加大，迭代寻优法是正解模型构建的首选手段，但由于工程应用中对实时性的要求和测量误差的限制，亟需设计一种收敛讯速、精度高和抗扰能力强的寻优算法。

发明内容

本发明针对目前现有技术中的上述问题，提出一种适用于三自由度转动曲柄连杆并联平台的正解求解优化方法法，该方法基于三自由度转动曲柄连杆并联平台运动特性，以三转动促动器驱动量为输入，以平台旋转位姿为输出，具备收敛速度快，求解精度高，抗扰能力强的优势。

本发明的目的通过以下的技术方案实现：三自由度转动曲柄连杆并联平台位姿控制方法，包括以下步骤：

S100、构建与平台位姿向量相关的目标函数和位姿向量矩阵，进行初始化，采用单纯型算法优化所述位姿向量矩阵；

S200、约束位姿向量搜索边界，对优化获得的位姿向量矩阵中的位姿向量执行惩罚操作，剔除超出搜索边界的位姿向量；

S300、搜索剩余的位姿向量对应的目标函数最小值，输出目标函数最小值对应的最优位姿向量。

进一步地，所述S100中，根据三自由度转动曲柄连杆的机械结构和运动学逆解理论，建立逆解数学模型S＝inv(P_j)，其中，S为逆解得到的曲柄连杆促动器的角度变化量，P_j为平台位姿向量集，构建目标函数F(P_j)＝(inv(P_j)-S₀)²，其中，S₀为曲柄连杆促动器的角度变化量实测值。

进一步地，所述S100中，任意选取平台上的至少三个点，以至少三个点的N组位姿向量参数为元素构建位姿向量矩阵，完成位姿向量矩阵初始化。

进一步地，所述S200的具体步骤包括：

S201、建立惩罚函数：

其中，t为当前演替次数，

代表第t次演替中的第j组，

为原位姿向量，

为演替后的位姿向量，Φ_max为搜索上界，Φ_min为搜索下界，由三自由度转动曲柄连杆并联机构的最大行程决定；

S202、根据S201建立的惩罚函数，对位姿向量重新赋值。

进一步地，所述S300之前还包括S210、执行回溯操作。

进一步地，所述回溯操作的具体步骤包括：S211、对位姿向量进行逆解求解曲柄连杆促动器角度变化量，判断能否获得有效的角度变化量；S212、根据回溯函数：

进行回溯操作，其中，k由逆解方程在实数域是否存在有效解决定，若通过逆解未获得到有效解，则取1，否则k取0；ξ为回溯系数，取值为1。

进一步地，S300之后还包括步骤S400：判断S300中获得的位姿向量对应的目标函数值是否小于预设的最大允许误差值，如果是，则输出该位姿向量，得到最优解。

进一步地，如果判断结果为否，则采用粒子群算法搜索全局后，返回S300。

本发明的有益效果：

1、本发明的求解方法在粒子群算法基础上加入单纯形算法，并且采用惩罚函数算法，避免寻优过程超出边界，以解决机构极限行程外正解无解，致使搜索算法不收敛的情况，能够实现快速收敛；

2、本发明的求解过程中通过对位姿向量逆解求解促动器的角度变化值，可避免位姿传感器的使用，并避免传感器数据传输线对并联平台运动范围的影响，具有强的抗干扰能力。

附图说明

图1为本发明的三自由度转动曲柄连杆并联平台位姿控制方法流程图；

图2为三自由度转动曲柄连杆并联平台的机械结构图，其中，A-动平台，B-定平台，e2-第三定杆，f2-第四定杆，b2-第二约束球铰，a2-第四定杆与第二促动器的连接点，c1-第一定杆与动平台A的连接点，c2-第三定杆与动平台的连接点，c3-第五定杆与动平台A的连接点，411-第七定杆，412-第八定杆，O1-第四约束球铰，2-第二促动器；

图3为三自由度转动曲柄连杆与并联平台上各点在三自由度转动曲柄连杆并联平台坐标系中的位置示意图；

图4为第二驱动支链和中间约束支链上各点在三自由度转动曲柄连杆并联平台坐标系以及建模坐标系中的位置示意图；

图5为本发明的三自由度转动曲柄连杆并联平台另一位姿控制方法流程图。

具体实施方式

下面结合附图和具体实施例，对本发明的技术方案进行清楚，完整地描述。

具体实施例1：结合图1～图4说明本实施例，三自由度转动曲柄连杆并联平台位姿控制方法，如图1所示，包括以下步骤：

S100、构建与平台位姿向量相关的目标函数和位姿向量矩阵，进行初始化，采用单纯型算法优化所述位姿向量矩阵；

S200、约束位姿向量搜索边界，对优化获得的位姿向量矩阵中的位姿向量执行惩罚操作，剔除超出搜索边界的位姿向量；

S300、搜索剩余的位姿向量对应的目标函数最小值，输出目标函数最小值对应的最优位姿向量。惩罚函数算法的引入，避免寻优过程超出边界，以解决机构极限行程外正解无解，致使搜索算法不收敛的情况，能够实现快速收敛。

在一个实施例中，S100中根据三自由度转动曲柄连杆的机械结构和运动学逆解理论，建立逆解数学模型，进而构建目标函数。

例如，根据三自由度转动曲柄连杆的机械结构和运动学逆解理论，建立逆解数学模型S＝inv(P_j)，其中，S为逆解得到的曲柄连杆促动器的角度变化量，P_j为平台位姿向量集，如图2，三自由度转动曲柄连杆并联平台由动平台A、定平台B、三路驱动支链和中间约束支链构成。其中三路驱动支链包括第一驱动支链、第二驱动支链和第三驱动支链，第一驱动支链由第一定杆、第二定杆和第一约束球铰(图中未示出)组成，第二驱动支链由第三定杆e2、第四定杆f2和第二约束球铰b2组成，第三驱动支链由第五定杆定杆、第六定杆和第三约束球铰(图中未示出)组成，中间约束支链由第七定杆411、第八定杆412及第四约束球铰O1组成。第一定杆、第三定杆e2以及第五定杆与动平台A的连接点分别为c1、c2和c3；在图3或图4中，a1为第二定杆与第一促动器(图中未示出)的连接点，a2为第四定杆f2与第二促动器2的连接点，且为平台B上的点，a3为第六定杆与第三促动器(图中未示出)的连接点；定平台B固定不动，动平台A绕第四约束球铰O1只可实现三个方向的转动运动。三自由度转动曲柄连杆并联平台坐标系(xyz)如图3中所示，其中，以O为原点，x轴与至a2点所在直线平行；x、y所在平面与静平台所在平面平行，z轴竖直向上。如图4所示，动平台A的建模坐标(XYZ)定义为：以第四约束球铰O1抽象成的点为原点，Z轴与定平台B垂直，方向竖直向上，X轴方向与定平台B所在平面平行，对应三自由度转动曲柄连杆并联平台+x轴，Y轴由右手定则确定。

三自由度转动曲柄连杆并联平台中所指俯仰运动角θ为动平台A所在平面绕Y轴运动，偏摆运动角ψ为动平台A所在平面绕Z轴运动，横滚运动角

为动平台A所在平面绕X轴运动，动平台A的位姿描述方法为

动平台A所在平面到定平台所在静平面的旋转矩阵R可表示为：

利用逆解模型得到的第一促动器、第二促动器2和第三促动器的角度变化定义为S＝[σ1 σ2 σ3]^T，角度变化的实际测量值定义为

根据所述三自由度转动曲柄连杆并联平台结构特点与所述建模坐标建立逆运动学模型：

均为以三自由度转动曲柄连杆并联平台坐标系原点O为原点的位姿向量，

为以建模坐标系的原点O1为原点的位姿向量。

R_B为定平台B上的点a_i(i＝1,2,3)到三自由度转动曲柄连杆并联平台坐标系原点O的距离，R_A为动平台A上的点c_i(i＝1,2,3)到点O2的距离，点O2为建模坐标系的原点O1在动平台A上的正投影，H为动平台A与定平台B平行时两平台的间距，H₀为三自由度转动曲柄连杆并联平台坐标系原点O点到建模坐标系的原点O1点的距离。

定平台B上的点a1、a2、a3相对于中三自由度转动曲柄连杆并联平台坐标系的坐标分别为：

χ_a1：(R_B·cos30,-R_B·sin30,0)，

χ_a2：(-R_B,0,0)，

χ_a3：(R_B·cos30,R_B·sin30,0)，

动平台A上的点c1、c2、c3在建模坐标系内的坐标分别为：

χ_c1：(R_A·cos30,-R_A·sin30,H-H₀)；

χ_c2：(-R_A·cos30,0,H-H0)；

χ_c3：(R_A·cos30,-R_A·sin30,H-H₀)；

将建模坐标系下点c1、c2、c3坐标转换为三自由度转动曲柄连杆并联平台坐标系下坐标的转换公式如下：

Γ_ci＝R·χ_ci+[0,0,H₀]

b1、b2、b3在三自由度转动曲柄连杆并联平台坐标系下的坐标分别为：

χ_b1＝χ_a1+(L1·cosσ1·cos30,-L1·cosσ1·sin30,L1·sinσ1)；

χ_b2＝χ_a2+(0,L1·cosσ2,L1·sinσ2)；

χ_b3＝χ_a3+(-L1·cosσ3·cos30,-L1·cosσ3·sin30,L1·sinσ3)；

其中，σ1、σ2和σ3分别表示点a1、a2和a3处电机转角变化。L1代表定杆f_i(i＝1,2,3)的长度,根据式(2)和(3)，利用数学公式表达三自由度转动曲柄连杆并联平台的运行学逆解数学模型：

L2＝(χ_bi[1]-χ_ci[1])²+(χ_bi[2]-χ_ci[2])²+(χ_bi[3]-χ_ci[3])²(i＝1,2,3)i＝1,2,3 (4)

(sinσi)²+(cosσi)²＝1 (5)

i代表的是i个驱动支链，其中，i＝1,2,3。L2表示定杆ei的长度，其中，i＝1,2,3。

为便于阐述，采用逆解模型S＝inv(P_j)，其中，i＝1,2,3替代公式(4)和公式(5)，表示利用逆解模型求解第一促动器(图中未示出)、第二促动器2和第三促动器(图中未示出)的角度变化，构建目标函数F(P_j)＝(inv(P_j)-S₀)²。

其次，定义平台的位姿矩阵

在本实施例中，位姿矩阵由平台上任意3个点c₁、c₂和c₃的位姿向量参数构成，选取7组位姿向量构建初始位姿矩阵

t代表当前演替次数,

为搜索过程中t次演替中的c_i(i＝1,2,3)位置处的第j组原始位姿向量，

以初始位姿向量矩阵Q⁰中每个位姿向量矩阵对应的目标函数值为元素构建初始目标函数集矩阵f⁰，并寻找出最小目标函数

和及其对应的最优位姿向量

最后，采用单纯形法执行寻优操作：寻找位姿矩阵中对应目标函数值最大的三个位姿向量，取其中一位姿向量PN，位姿向量PN对应目标函数值为F(PN)，对位姿向量PN进行反射操作：定义反射位姿向量为PNr，PNr对应的目标函数值记为F(PNr)，令PNr＝PNc+λ(PNc-PN)，其中，反射系数λ＝1；

计算并比较F(PNr)和F(PNb)，如果F(PNr)<F(PNb)，则反射方向正确，执行扩张操作：定义扩张位姿向量PNe，PNe对应的函数值记为F(PNe)，令PNe＝PNc+ω(PNr-PNc)，其中，扩张系数ω＝1.8，计算F(PNe)并与F(PNb)比较，判断F(PNe)<F(PNb)是否成立，

是，则用PNe取代PN；

否，则用PNr取代PN；

如果F(PNr)>F(PNb)，则反射方向不正确，执行压缩操作：定义压缩位姿向量PNt，PNt对应的函数值记为F(PNt)，令

其中，压缩系数

计算F(PNt)并与F(PN)比较，判断F(PNt)<F(PN)是否成立，如果：

是，则用PNt取代步PN；

否，则PN的值保持不变；

如果F(PNr)＝F(PNb)，则PN的值保持不变；

比较函数值F(PN)、F(PNr)和F(PNb)的大小，并判断F(PN)>F(PNr)>F(PNb)是否成立，

如果F(PN)>F(PNr)>F(PNb)成立，则执行收缩操作：定义收缩位姿向量PNw，PNw对应的函数值记为F(PNw)，令PNw＝PNc-α(PN-PNc)，其中，收缩系数α＝0.6，计算F(PNw)的值并与F(PN)比较，判断F(PNw)<F(PN)是否成立，如果：

是，则用PNw取代PN；

否，则PN的值保持不变；

如果F(PN)>F(PNr)>F(PNb)不成立，则PN的值保持不变，执行下一步骤，根据重新获得的最优位姿向量优化初始位姿向量矩阵Q⁰中的各个元素，获得新的位姿向量矩阵Q′；

在本发明的其他实施例中，步骤S100可以通过其他方式实现，例如选取平台上4个、5个或更多的点的位姿向量参数来构建位姿矩阵；或者选取8组、9组或者更多组位姿向量构建初始位姿矩阵；或者在寻优过程中，反射系数选取0.9或1.1或其他能够实现收缩操作的数值，扩张系数选取1.7或1.9或其他能够实现扩张操作的数值，压缩系数选择0.5或0.7或者其他能够实现压缩操作的数值，收缩系数选择0.5或者0.7或其他能够实现收缩操作的数值。

在本发明的另一个实施例中，S200的具体步骤可以包括：

S201、构建惩罚函数：

其中，t为当前演替次数，

代表第t次演替中的第j组，

为原位姿向量，

为演替后的位姿向量，Φ_max为搜索上界，Φ_min为搜索下界，由三自由度转动曲柄连杆并联机构的最大行程决定；

S202、根据S201中所构建的惩罚函数，对S100中获得的最优位姿向量重新赋值。

引入惩罚函数的作用是剔除超出搜索边界的位姿向量，在本发明的其他实施例中，惩罚操作可以通过其他方式例如构建其他可以使边界收敛的惩罚函数实现。

如图5所示，本发明的另一实施例在具体实施例1的基础上进一步改进，所述S300之前还可以包括S210、执行回溯操作：

S211、对位姿向量进行逆解求解曲柄连杆促动器角度变化量，判断能否获得有效的角度变化量；

S212、根据回溯函数：

进行回溯操作，其中，k由逆解方程在实数域是否存在有效解决定，若通过逆解未获得到有效解，则取1，否则k取0；ξ为回溯系数，取值为1。

回溯操作的作用是解决位姿向量逆解无有效解的异常情形，在本发明的其他实施例中，可以构建其他能够实现相同作用的回溯函数。

在本发明的另一个实施例中，S300的之后还包括步骤S400：判断S300中获得的最优位姿向量对应的目标函数值是否小于预设的最大允许误差ε，如果是，则输出该最优位姿向量。

本发明的另一个实施例中，S400中，如果判断结果为最优位姿向量对应的目标函数值大于或等于预设的最大允许误差ε，则采用粒子群算法依据公式：

搜索界限内全部位姿向量，其中，

为t次演替中的第j个平台位姿的搜索速度，

为t+1次演替中的第j个平台位姿的搜索速度，μ₁、μ₂为惯性权重，为非负常数，取1.50312，t为当前迭代次数，δ₁，δ₂为分布在[0,1]的随机数，

为搜索过程中t次演替中的ci(i＝1,2,3)位置处的第j个平台位姿的个体极值，在本发明的其他实施例中，粒子群算法可以通过其他方式实现。

需要说明的是，以上实施例仅用于解释说明本发明的技术方案，并不用于限定本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施例或变更均应包含在本发明的保护范围之内。

Claims (6)

1.三自由度转动曲柄连杆并联平台位姿控制方法，其特征在于，包括以下步骤：

S100、构建与平台位姿向量相关的目标函数和位姿向量矩阵，进行初始化，采用单纯型算法优化所述位姿向量矩阵；

S200、约束位姿向量搜索边界，对优化获得的位姿向量矩阵中的位姿向量执行惩罚操作，剔除超出搜索边界的位姿向量；

S300、搜索剩余的位姿向量对应的目标函数最小值，输出目标函数最小值对应的最优位姿向量；

所述S100中，根据三自由度转动曲柄连杆的机械结构和运动学逆解理论，建立逆解数学模型S＝inv(P_j)，其中，S为逆解得到的曲柄连杆促动器的角度变化量，P_j为平台位姿向量集，构建目标函数F(P_j)＝(inv(P_j)-S₀)²，其中，S₀为曲柄连杆促动器的角度变化量实测值；

所述S200的具体步骤包括：

S201、根据三自由度转动曲柄连杆并联机构的最大行程，定义搜索上界Φ_max，搜索下界Φ_min，建立惩罚函数：

，其中，t为当前演替次数，

代表第t次演替中的第j个位姿，

为原位姿向量，

为演替后的位姿向量；

S202、根据S201建立的惩罚函数，对超出搜索界限的位姿向量执行惩罚操作，完成对超出搜索界限的位姿向量重新赋值。

2.根据权利要求1所述的三自由度转动曲柄连杆并联平台位姿控制方法，其特征在于，所述S100中，任意选取平台上的至少三个点，以至少三个点的N组位姿向量参数为元素构建位姿向量矩阵，完成位姿向量矩阵初始化。

3.根据权利要求1所述的三自由度转动曲柄连杆并联平台位姿控制方法，其特征在于，所述S300之前还包括步骤S210、执行回溯操作。

4.根据权利要求3所述的三自由度转动曲柄连杆并联平台位姿控制方法，其特征在于，所述回溯操作的具体步骤包括：

S211、对位姿向量进行逆解求解曲柄连杆促动器角度变化量，判断能否获得有效的角度变化量；

S212、根据回溯函数：

进行回溯操作，其中，k由逆解方程在实数域是否存在有效解决定，若通过逆解未获得到有效解，则取1，否则k取0；ξ为回溯系数，取值为1。

5.根据权利要求1所述的三自由度转动曲柄连杆并联平台位姿控制方法，其特征在于，S300的之后还包括步骤S400：判断S300中获得的位姿向量对应的目标函数值是否小于预设的最大允许误差值，如果是，则输出该位姿向量，得到最优解。

6.根据权利要求5所述的三自由度转动曲柄连杆并联平台位姿控制方法，其特征在于，如果判断结果为否，则采用粒子群算法搜索全局后，返回S300。

Images