3RPS-SPS并联机构的力位混合冗余驱动控制

2023-10-30张氢赵伯伦陈文韬秦仙蓉孙远韬

中南大学学报（自然科学版） 2023年9期

张氢，赵伯伦，陈文韬，秦仙蓉，孙远韬

(同济大学机械与能源工程学院，上海，201804)

并联机构具有结构紧凑、运动精度高、机构刚度大和承载能力强[1]等优点。目前，以并联机构为主的执行机构在医疗器械、先进制造业等行业中得到了广泛应用[2-4]，例如，六轴外固定支架(6-UPS 并联机构[5])、踝关节康复机构(3-RPS/R 并联机构[6])、Tricept 混联机床(3UPS-UP 并联机构[7])和Exechon 混联机器人(2UPR-SPR 并联机构[8])等。但并联机构末端器的工作空间较小，动平台的灵活性也较差，容易产生奇异位形，这也极大限制了它的应用领域。为了克服上述不足，学者们提出了冗余驱动并联机构。冗余驱动并联机构是指的驱动关节数目大于结构所具有的自由度数目的并联机构。与一般的并联机构相比，冗余并联机构具有较多的驱动部件，因而具有更高的结构刚度和更强的承载能力，更适合于高速重载的应用场合[9-12]。

并联机构具有强耦合、非线性的动力学特性，运动学控制方法只能适用于对速度和精度要求不高的应用场合[13]。冗余并联机构具有闭环结构，使用运动学控制方法在控制过程中将产生较大的内力，严重影响机构运动性能[14]。如果所有执行器都使用力控制模式，需要精确的动力学模型[15]。在现实中，很难获得准确的机器人动力学参数(如摩擦因数)，尤其是对于复杂空间多自由度机器人。因此，对于冗余并联机构需要采用力位混合控制策略[16]，在运动学控制的基础上考虑并联机构的动力学模型，控制器根据逆动力学方程补偿并联机构因强耦合、非线性的动力学特性导致的运动误差，使机器人具有更高的控制性能[17]。

吴宇列等[18]提出了一种基于静态力平衡的改进PD 控制方法，并以2 自由度的冗余平面并联机构为对象，验证该控制方法的正确性。张耀欣等[19]设计了一种采用离散最优线性二次调节器方法，利用该方法控制并联机构的位移及速度。米建伟等[20]针对平面二自由度冗余并联机构提出一种同步控制方法，将惯性矩阵和离心力矩阵取定值，并引入了同步误差和耦合误差项。龙亿等[21]基于轨迹轮廓误差设计了自适应滑模同步控制器，将动力学方程进行线性化，并分析了平面冗余驱动并联机构的稳定性。崔学良等[22]建立了3RPS/UPS冗余驱动并联机构的鲁棒性轨迹跟踪控制模型，采用模糊PID控制策略对冗余驱动支链目标阻抗模型内力进行实时跟踪，实现了对冗余驱动并联机构的柔顺性控制。刘晓飞等[23-24]在传统力位混合驱动的基础上，提出了一种驱动力同步协调控制策略，并以6PUS+UPU 冗余驱动并联机构为对象进行了验证。李永泉等[25]设计了基于机器人系统动力学模型的前馈控制策略，并通过实验验证了该控制策略的可行性。

本文以3RPS-SPS 冗余驱动并联机构为对象，从运动学和动力学2个方面研究该新型机构，并根据3RPS-SPS并联机构的特点，设计了力位混合的PID冗余驱动控制策略。

1 机构构型与自由度分析

3RPS-SPS 并联机构如图1所示。3RPS-SPS 并联机构包括驱动支链、定平台和动平台3 个部分，动平台通过4个驱动支链与定平台相连。

图1 3RPS-SPS并联机构示意图Fig.1 Schematic diagrams of 3RPS-SPS parallel mechanism

为了便于分析，记4个驱动支链在动平台上的安装铰点分别为A1～A4，其中A1A2垂直于A3A4且相交于A3点；记驱动支链在定平台上的安装铰点分别为B1～B4，其中B1B2垂直于B3B4且相交于O点；AiBi对应于驱动支链i。动平台上安装有末端器，末端器端点为Q且A3Q垂直于动平台平面。驱动支链1～3 从定平台到动平台依次通过R 副、P副和S 副连接，其中，驱动支链1 和驱动支链2 的R 副轴线与B3B4平行，驱动支链3 的R 副轴线与B1B2平行。驱动支链4从定平台到动平台依次通过S副、P副和S副连接。4个驱动支链的P副轴线方向都是由定平台上的安装铰点指向动平台上的安装铰点。

为了分析该并联机器人的运动特性，建立如下坐标系：取B1B2和B3B4的交点O为坐标系原点，OB4方向为X轴正方向，OB2方向为Y轴正方向，建立定平台坐标系，坐标系固结在定平台上；取A3为坐标系原点，A3A4方向为X′轴正方向，A3A2方向为Y′轴正方向，建立动平台坐标系，坐标系固结在动平台上。

表1所示为并联机构驱动支链的运动副分析。由螺旋理论可以得出4 个驱动支链的运动螺旋系，如图2所示。对4个驱动支链运动螺旋系求反螺旋可得4个驱动支链的约束螺旋，联立各驱动支链的约束螺旋即可获得4个驱动支链对动平台的约束螺旋系。

表1 并联机构驱动支链的运动副分析Table 1 Kinematic pair analysis of parallel mechanism

图2 驱动支链运动螺旋Fig.2 Motion spiral of driving branch chains

求解上述约束螺旋系的反螺旋，可得动平台的运动螺旋系：

式中：$1为绕定平台坐标系X轴方向的旋转自由度；$2为绕动平台坐标系Y′轴的旋转自由度；$3为沿着定平台坐标系Z轴的平移自由度。

由式(2)可知，并联机构具有3个独立自由度和4个驱动支链，驱动数量大于自由度数量，为冗余驱动并联机构。

2 运动学分析

2.1 位置反解的解析表达

式(3)所示的旋转矩阵为通用旋转矩阵，包含4个自由度，而根据自由度分析可知该并联机构仅有3个独立自由度。因此，可根据各驱动支链对动平台的约束关系建立约束方程，对式(3)进行简化。取γ=0°，代入到式(3)可得

根据机构的几何参数可知，在定平台坐标系下，各点坐标分别为：B1(0，-d，0)，B2(0，d，0)，B3(ad，0，0)，B4(d，0，0)。在动平台坐标系下，各点坐标分别为：A1(0，-a，0)，A2(0，a，0)，A3(0，0，0)，A4(a，0，0)。

为了便于分析，以定平台坐标系作为参考坐标系，通过式(4)将Ai转化到定平台坐标系下表示，由定平台坐标系下的Ai和Bi坐标，可得到各驱动支链杆长li和并联机构位姿参数关系为

式(5)即为机构的运动反解的解析表达式，其中(α，β，pz)为描述动平台位姿的状态变量。

2.2 位置正解的解析表达

对设计的并联机器人求位置正解，即已知4个驱动支链的杆长，求解动平台的输出位姿。假设4个驱动支链的杆长分别为l1、l2、l3和l4。

由式(5)变形可得

经化简可得：

式(9)中，

由式(7)～(9)即可求得动平台的输出位姿参数α、β、pz。

3 动力学分析

3.1 速度与加速度分析

对式(5)关于时间t求导可得

式(11)中矩阵J即为并联机器人的速度雅可比矩阵。

继续将式(11)对时间t求导可得

式(11)即为速度微分方程，式(13)即为加速度微分方程。

3.2 驱动支链受力分析

3RPS-SPS 并联机构定平台固定，运动部件包括4 条驱动支链和动平台。其中，4 条驱动支链的结构形式类似，皆以移动副为驱动，两端通过旋转副或者球副与定平台、动平台连接。采用拉格朗日法建立并联机构的动力学模型，拉格朗日方程如下：

式中：L=Ek-Ep；Ek为系统动能；Ep为系统势能；q为系统广义坐标；τ为对应于广义坐标的广义力。

选取动平台的位姿参数q=(pz，α，β)作为广义坐标，分别对动平台和驱动支链进行动力学建模。

对动平台进行简化建模，取惯性张量矩阵I为

式中：Ix、Iy、Iz分别为动平台绕自身坐标系X'轴、Y'轴、Z'轴的转动惯量，可根据动平台的结构尺寸求得；md为动平台质量。选取定平台坐标系原点所在水平面作为零势能面，动平台势能Epd为

由式(15)和式(17)代入拉格朗日方程，求解拉格朗日方程可得动平台的广义力τd为

然后，对驱动支链进行动力学建模。并联机构的4条驱动支链都是以移动副为驱动，两端通过旋转副或者球副与动平台、定平台连接，铰接点分别为Ai和Bi(i=1，2，3，4)。根据驱动支链的运动特点，每个支链都可以分解为驱动杆和驱动缸体2个部分，其中，驱动缸体可绕着驱动支链与定平台的铰接点Bi旋转，而驱动杆不仅绕着铰接点Bi旋转，还沿着驱动支链方向的轴向移动。

以驱动支链1为例，建立动力学模型，驱动支链1的运动简图如图3所示。

图3 驱动支链1运动简图Fig.3 Motion diagram of drive branch chain No.1

首先，列出驱动支链1的动能表达式和势能表达式。根据图3，驱动支链1的动能Ek1可表示为驱动缸体的动能和驱动杆的动能之和。

式中：mb为驱动缸体的质量；vb为驱动缸体质心的速度；Ib为驱动缸体的转动惯量；ω为驱动支链的角速度；mt为驱动杆的质量；vt为驱动杆质心的速度；It为驱动杆的转动惯量。

选取定平台坐标系原点所在水平面作为零势能面，驱动支链1的势能Ep1为

式中：lb为驱动缸体的质心到驱动支链与定平台铰接点B1的距离；lt为驱动杆的质心到驱动支链与动平台铰接点A1的距离；θ1为驱动支链与水平线间的夹角。

将式(19)和(20)代入拉格朗日方程，即可求得驱动支链1产生的广义力τ1。同理，按照上述推导对支链2、支链3 和支链4 列拉格朗日方程可以得各驱动支链产生的广义力τ2、τ3、τ4。

在对动平台和驱动支链建立动力学模型时，选取的广义坐标和零势能面相同。因此，只需将对动平台和驱动支链求解得到的广义力相加即可得到并联机构广义力：

3.3 驱动力分析

用虚功原理求解各支链驱动力。假设驱动支链的虚位移为δl=[δl1δl2δl3δl4]T，驱动支链的驱动力为F=[δF1δF2δF3δF4]T；广义坐标的虚位移为δq=[δpzδαδβ]T，广义驱动力为τ=[τ1τ2τ3τ4]T。根据虚功原理，当机构发生虚位移时，惯性力和外力所作的虚功总和等于零，因此，在确定的运动状态下即惯性力确定时，广义驱动力在广义坐标的虚位移上做的虚功与实际每个驱动支链的驱动力在驱动支链的虚位移上做的虚功相等，即

对于确定的构型，广义坐标的虚位移和驱动支链的虚位移需要满足机构构型的几何约束要求，可以通过雅克比矩阵进行转换。

将式(23)代入式(22)可得广义驱动力与驱动支链驱动力之间的关系为

此时，式(24)中的(JT)-1∈R4×3具有无穷多组解，不同形式的(JT)-1就对应了不同的驱动支链驱动力分配方案。因此，可以根据某一特定的优化目标确定(JT)-1矩阵对驱动力进行优化配置，使得机构具有更好的运动性能。

对于并联机构最常见的优化目标为驱动力优化，优化问题可表述为：在约束条件JTF=τ下，使得目标函数Z=FTWF的支链驱动力F最小。式中，W为对角加权矩阵，表示各支链驱动力的权重。

对上述优化问题，可采用拉格朗日乘子法，引入拉格朗日乘子λ，构造新的函数Z′：

为使Z′取得极值，Z′对F和λ的偏导数等于0，求解得

取加权矩阵W为单位矩阵，则可得

4 力位混合控制策略

4.1 控制策略设计

根据机构自由度分析可知，3RPS-SPS 并联机构为冗余并联机构，存在冗余支链。因此，可在常规的非冗余驱动控制的基础上对冗余支链添加力控制。根据动力学模型设计了一种改进的计算力矩算法，即动力学前馈算法。

式(28)可分解为2个部分τ1和τ2叠加，其中，

式(29)仅与理论广义位置、速度、加速度有关，与实时数据无关，可在轨迹规划时提前计算；式(30)形式简单，计算方便，减轻了实时计算负担。控制力矩算法框图如图4所示。

图4 控制力矩算法框图Fig.4 Control torque algorithm block diagram

力位混合的冗余驱动控制的系统框图如图5所示，位置驱动控制的系统框图如图6所示。力位混合驱动控制系统与位置驱动控制系统相比，增加了力控制支链。

图5 力位混合冗余驱动控制系统框图Fig.5 Block diagram of force position hybrid redundant drive control system

图6 位置驱动控制系统框图Fig.6 Block diagram of position drive control system

在力位混合的冗余驱动控制系统中，驱动支链按照控制方式分为力控制支链与位置控制支链2类。

1)对于位置控制支链，首先，将要求路径进行轨迹规划，并反解得到各支链的杆长信息；然后，根据各支链位置传感器的实时测量数据与规划杆长信息进行比较，得到位置差值信号；最后，将差值信号传入PID控制器，对伺服系统进行PID控制。

2)对于力控制支链，轨迹规划获得支链的位置、速度、加速度，并通过动力学反解得到系统驱动力。动力学反解模块可根据不同的控制要求进行设计。

3RPS-SPS 机构为了平衡各支链的驱动力，提升系统的驱动能力，采用3.3节中的驱动力优化算法。将动平台位姿的位差信号传入PD控制器，再乘以惯性矩阵M(qd)，对根据理论位置计算的驱动力进行反馈调节。在整个控制系统中，位置控制支链保证了机构的位置精度，力控制支链则通过调整驱动力以平衡所有支链的驱动力，使机构具有较高的整体刚度和较好的运动性能。

4.2 联合仿真实验

图7所示为建立的ADAMS仿真模型。为了验证冗余驱动控制系统的性能，在ADAMS动力学模型的基础上结合Simulink搭建联合仿真平台，如图8所示。并联机构的物理参数如表2所示。驱动杠为伺服电动缸，其内部为滚动丝杠结构可将伺服电机的旋转运动转化为电动缸的直线伸缩运动。

表2 并联机构物理参数Table 2 Physical parameters of parallel mechanism

图7 ADAMS仿真模型Fig.7 ADAMS simulation model

图8 冗余驱动控制联合仿真系统Fig.8 Joint simulation system of redundant drive control

设置动平台位姿参数随时间变化规律为

设定运动时间为5 s，将上述轨迹作为图8所示控制系统的输入，进行仿真，结果如图9至图11所示。

图9 力位混合冗余驱动运动轨迹Fig.9 Force/position hybrid redundant drive motion trajectory

对图9所示实际轨迹和理论轨迹的误差进行分析，得到图10。由图10 可得，动平台沿Z轴位移平均误差为4.01×10-2mm，最大误差为4.71×10-1mm；绕X轴旋转角平均误差为5.66×10-4rad，最大误差为4.52×10-3rad；绕Y′轴旋转角平均误差为3.45×10-4rad，最大误差为9.42×10-3rad。在冗余驱动方式下，动平台实际轨迹和理论轨迹的误差极小，轨迹跟踪效果明显，说明使用冗余驱动方式的并联机构具有较高的运动精度。

图10 力位混合冗余驱动运动轨迹误差Fig.10 Force/position hybrid redundant drive motion trajectory error

由图11 可知：采用力位混合冗余驱动方式，支链2出现最大驱动力约为66 N，小于非冗余位置驱动方式时支链3 出现的最大驱动力190 N。对比非冗余位置驱动的3个驱动支链的驱动力，可以发现驱动力分配极不均衡，支链1的驱动力小于支链3 的驱动力。而力位混合冗余驱动的4 个驱动支链的驱动力分配较均衡，支链1、支链2 和支链3 的驱动力在64 N 附近变化，而支链4 的驱动力在45 N 附近变化。因此，在力位冗余驱动方式下，各驱动支链的驱动力变化平稳，并且由于采用了驱动力优化算法，各支链驱动力分配较均匀，最大驱动力较小，可有效提升被控机构的承载能力。