CN105459114B - 冗余驱动并联机构驱动力优化方法及轴组控制验证平台 - Google Patents

Abstract

本发明是为解决冗余驱动并联机构普遍存在的驱动力分配不均，机构内力过大，运动控制难以及由于采用点接触高副从而增加系统控制的复杂性等问题，提出一种冗余驱动并联机构驱动力优化分配方法及对此方法的轴组控制验证平台。冗余驱动并联机构轴组控制验证平台包括冗余驱动并联机构本体、冗余驱动控制模块、轴组运动控制模块、伺服及传感系统。驱动力优化分配方法可适用于冗余驱动并联机构运动控制领域。

Description

冗余驱动并联机构驱动力优化方法及轴组控制验证平台

技术领域

本发明属于冗余驱动并联机构运动控制技术领域，涉及一种冗余驱动并联机构的驱动力优化分配方法及轴组控制验证平台。

背景技术

仿生咀嚼机器人可以模拟人类下颌运动行为、再现其生物力学环境，其研究成果在口腔医学和食品科学等领域具有广阔应用前景。人体口颌系统下颌驱动肌肉数目明显多于下颌运动的自由度，所以口颌系统具有冗余驱动的特性。在颞下颌关节及关节盘等组织的约束下，左右两侧颞下颌各约束两个自由度，下颌运动只具有四个自由度。冗余驱动的特性具有许多优势，可以改善工作空间中的奇异位形，解决奇异点导致的运动精度降低、刚度减小和驱动关节无法实施控制等问题。但由于冗余驱动力的存在，使得逆动力学方程不存在唯一解，这增大了并联机构控制的难度，因此，需要找到一种适合冗余驱动并联机构力控制的方法。

冗余驱动并联机构的控制方法一般有两种，即运动学控制与动力学控制。运动学控制比较简单，一般应用在精度不高，速度较慢的场合。冗余驱动并联机构控制中易存在的位置误差以及瞬时反向驱动，使得机构中各个支链间容易产生较大的内力，过大的内力在高速和大负载的情况下会造成杆件的严重破坏。针对冗余驱动并联机构的研究主要集中在机构设计、奇异性分析等方面，通过机构优化来提高性能。然而，针对运动性能起关键作用的动力学控制研究较少，从某种程度上限制了冗余驱动并联机构的性能提升及推广应用。本发明旨在提出一种冗余驱动并联机构驱动力优化分配方法，解决机构内力分配不均的问题。

发明内容

本发明是为了解决冗余驱动并联机构普遍存在的驱动力分配不均，机构内力过大，运动控制难等问题，提出了一种冗余驱动并联机构驱动力优化分配方法及对此方法的轴组控制验证平台。

为了达到上述目的，本发明的技术方案为：

一种冗余驱动并联机构驱动力优化分配方法，包括以下步骤：

第一步，建立机器人坐标系描述机器人的运动，建立3个坐标系描述冗余驱动并联机构的运动，如图7所示。含有点接触高副的冗余驱动并联机构具有4个自由度，广义坐标只有4个，采用四个独立位置参数X_L,Y_L,β_L,γ_L描述动平台的位姿。

第二步，建立运动学方程；建立并联6PUS机构支链及点接触高副约束方程，并完成冗余驱动并联机构的运动学计算。

第三步，建立动力学方程；在运动学方程的基础上，求解机器人的动能与势能，建立冗余驱动并联机构的动力学模型。

第四步，建立拉格朗日方程；采用第一组与第二组第一类拉格朗日方程，通过引入6个拉格朗日算子求解驱动力/力矩，由于冗余驱动并联机构特性，由四个方程求解6个未知数，驱动力/力矩有无限组解。

第五步，伪逆优化方法求解驱动力；采用伪逆矩阵对六个拉格朗日算子λ_i(i＝1,2,…,6)进行优化，通过联立方程消除拉格朗日算子，求得驱动力/力矩的最小二范数解，得到最优驱动力。六个拉格朗日算子λ_i(i＝1,2,…,6)由公式(26)获得，但是很明显只有四个方程，因此，采用伪逆矩阵进行优化，得到六个拉格朗日算子的数值解。

求得冗余驱动并联机构的最优化驱动力，同时该方法可以推广用在其他冗余驱动并联机构的驱动力优化分配问题。

一种运行上述方法的冗余驱动并联机构轴组控制验证平台，包括冗余驱动控制模块、轴组运动控制模块、伺服驱动、传感系统及冗余驱动并联机构本体，如图3所示；所述冗余驱动控制模块位于上位机PC中，冗余驱动控制模块运行上述方法进行驱动力优化分配，发出控制指令；轴组运动控制模块位于下位机轴组运动控制器中，轴组运动控制器与伺服驱动位于控制柜中，轴组运动控制器接收冗余驱动控制模块发出的控制指令，并将控制指令通信至伺服驱动；传感系统与轴组运动控制模块相连，传感系统采集冗余驱动并联机构本体的数据，并将采集的数据反馈给轴组运动控制模块，实现对伺服电机的闭环控制，保证冗余驱动并联机构电机的驱动力矩；所述的采集的数据包括冗余驱动并联机构的受力、速度、位置等信息。

所述冗余驱动并联机构本体采用专利“一种具有仿生颞下颌关节的冗余驱动咀嚼机器人”，专利号：ZL201310602874.X。

所述的冗余驱动并联机构本体包括6条PUS并联支链和2个点接触高副HKP(HigherKinematic Pair)，机构形式为6PUS-2HKP，如图1所示。

所述的一条PUS驱动支链包括伺服电机1、联轴器2、滚珠丝杠3、万向节4、移动副5、驱动杆6和球副7；伺服电机1固定在上固定板9上，通过联轴器2带动滚珠丝杠3转动，滚珠丝杠3转动带动移动副5沿导轨直线运动，移动副5通过万向节4与驱动杆6连接，驱动杆6通过球副7与动平台8连接，六条PUS驱动支链最后固定在下固定板10上。通过电机驱动6PUS机构，进而驱动动平台8在三维空间平动或转动。

所述的一个HKP包括约束块12和约束杆13；约束块12下表面为约束曲面，约束杆13安装在动平台8上，约束杆13与约束曲面在运动过程中保持接触，形成点接触高副；两个点接触高副可以约束动平台两个自由度。因此，整个6PUS-2HKP冗余驱动并联机构是由六台电机驱动，只有四个自由度的冗余驱动机构，其运动特性与一般的并联机构有明显差异。

本发明提出一种冗余驱动并联机构的轴组控制验证平台，驱动力优化分配方法可适用于冗余驱动并联机构运动控制领域，能够解决冗余驱动并联机构普遍存在的驱动力分配不均，机构内力过大，运动控制难以及由于采用点接触高副从而增加系统控制的复杂性等问题，可以对冗余驱动并联机构驱动力优化分配方法进行实验验证。

附图说明

图1是冗余驱动并联机构原理图。

图2是冗余驱动并联机构驱动力优化分配方法流程图。

图3是冗余驱动并联机构轴组控制验证平台示意图。

图4是冗余驱动并联机构侧视图。

图5是冗余驱动并联机构正视图。

图6是点接触高副HKP示意图。

图7是冗余驱动并联机构坐标系示意图。

图中：1伺服电机；2联轴器；3滚珠丝杠；4万向节；5移动副；6驱动杆；7球副；8动平台；9上固定板；10下固定板；11点接触高副位置；12约束块；13约束杆。

具体实施方式

下面结合技术方案和附图详细叙述本发明的具体实施内容。

所述冗余驱动并联机构驱动力优化分配方法流程图见图2，具体步骤如下：

第一步，建立机器人坐标系

建立3个坐标系描述冗余驱动并联机构的运动，如图7所示；三个坐标系为基坐标系O_B-X_BY_BZ_B、瞬时坐标系O_L-X_LY_LZ_L和动平台坐标系O_M-X_MY_MZ_M。所述的O_B-X_BY_BZ_B在基座上；所述的O_L-X_LY_LZ_L在左侧点接触高副的约束杆13上。O_L-X_LY_LZ_L随机器人的运动而沿约束曲面滑动或转动，在O_L-X_LY_LZ_L中，Z_L始终垂直于约束曲面，X_L相切于约束曲面，Y_L与两者正交；所述的O_M-X_MY_MZ_M设置在初始位置瞬时坐标系Σ_B的位置。

通常情况下，机器人的运动一般用6个运动学参数描述，即：X_L,Y_L,Z_L,α_L,β_L,γ_L。由于点接触高副的存在，限制了下颌结构的位置和姿态部分自由度。含有点接触高副的冗余驱动并联机构具有4个自由度，广义坐标有4个，采用四个独立位置参数X_L,Y_L,β_L,γ_L来描述位姿。

第二步，建立运动学方程

建立并联6PUS机构支链及点接触高副约束方程，并完成冗余驱动并联机构的运动学计算。

对于6自由度的并联机器人来说，运动学一般可以写成：

Q_i＝f_i(X_L,Y_L,Z_L,α_L,β_L,γ_L) (1)

其中，Q_i＝[Q₁,Q₂,…,Q₆]^T是各关节空间参数，W＝[X_L,Y_L,Z_L,α_L,β_L,γ_L]^T是末端执行器直角坐标空间参数。

2.1点接触高副HKP约束方程

左右两侧HKP约束曲面的约束方程的数学一般表达式分别为

f_L(X_L,Y_L,Z_L)＝0 (2)

f_R(X_R,Y_R,Z_R)＝0 (3)

其中，f_L和f_R分别是左右两侧约束方程，X_L,Y_L,Z_L为左侧HKP的坐标，X_R,Y_R,Z_R为右侧HKP的坐标。

2.2并联机构6PUS支链约束方程

机器人单条支链示意图见图7。

向量B_iM_i存在如下关系：

B_iM_i＝B_iC_i+C_iM_i (4)

根据上述约束条件，每个滑块的移动量Q_i的计算公式：

其中，C_i是移动副的位置，B_i每条支链支撑板的固定位置，L_i是每条驱动杆的长度，肌肉连接点M_i在O_B-X_BY_BZ_B的坐标可以通过坐标转换矩阵^B T_M获得。

动平台8的6个位置参数关系表达式为：

其中，X_L,Y_L,Z_L,α_L,β_L,γ_L为6个位置参数，X_L,Y_L,β_L,γ_L为4个独立位置参数。

由上述公式(5)和(6)求得冗余驱动并联机构的运动学关系表达式为：

其中，T为运动学变换矩阵，T_M为位置变换矩阵；由公式(7)可知，已知4个独立位置参数X_L,Y_L,β_L,γ_L，用W_M表示，便可以通过运动学计算得到6个滑块的位移变化量Q_i(i＝1,2,…,6)。

即对于冗余驱动并联机构来说，

[Q_i]_6×1＝[T_M]_6×4·[W_M]_4×1 (8)

第三步，建立动力学方程

在运动学方程的基础上，求解机器人的动能与势能，建立冗余驱动并联机构的动力学模型。

3.1机器人动能计算

动能计算如下式：

其中，K_p是动平台的动能，K_ci是滑块的动能，K_cmi是支链驱动杆C_iM_i的动能，具体各项计算如下：

其中，m_p是动平台的质量，是动平台在固定坐标系下的平移速度，是动平台在固定坐标系下的转动速度，且满足

I_pB是动平台在固定坐标系下的惯性矩阵，计算如下：

I_pB＝^BR_p·I_pM·^BR_P ^T (12)

其中，I_pM是下颌机构关于点O_L在动坐标系中的惯性矩阵，I_pB是下颌机构关于点O_L在固定坐标系中的惯性矩阵。

滑块动能如下：

其中，Vs是滑块的速度。

驱动杆的动能推导：

其中，m_cm是杆的重量，V_cmi是驱动杆质量中心的速度矢量，I_cmB是杆在固定坐标系下的惯性矩阵，可以用下式计算：

I_cmB＝^BR_Li·I_cmM·^BR_Li ^T (15)

3.2机器人的势能计算

机器人的势能可以由如下公式进行计算：

其中，P_p是动平台的势能，P_ci是滑块的势能，P_cmi是支链驱动杆C_iM_i的势能，具体计算如下：

P_p＝m_pg^T·H_p (17)

其中，g＝[0 0 -9.8]^T，H_p是动平台质心与固定坐标系的垂直距离。

P_ci＝m_cig^T·H_ci (18)

其中，H_ci是滑块质心与固定坐标系的垂直距离。

P_cmi＝m_cig^T·H_cmi (19)

其中，H_cmi是驱动杆C_iM_i质心与固定坐标系的垂直距离。

第四步，建立拉格朗日方程

采用第一组与第二组第一类拉格朗日方程，通过引入6个拉格朗日算子求解驱动力/力矩，由于冗余驱动并联机构特性，由四个方程求解6个未知数，驱动力/力矩有无限组解。

由机构的运动学可以得到六个约束方程，Γ_i(i＝1,2,…6)，约束方程是根据机构中支链C_iM_i是长度是固定值L_i获得。

Γ_i＝||C_iM_i||²-L_i ²＝0(i＝1,2,...,6) (20)

综上，整个机器人系统的朗格朗日方程建立为：

L＝K-P (21)

其中，L为拉格朗日方程，K为机器人动能，P为机器人势能；

由运动学得到的关系

Z_L＝f_L(X_L,Y_L) (22)

γ_L＝f_R(X_L,Y_L,f_L(X_L,Y_L),β_L,γ_L),β_L,γ_L) (23)

分别对其两边求导，并联立式(5)可得关系式：

根据不同坐标系下表达关系可得：

由上两式可以求得广义坐标下的F_gx,F_gy,M_gy,M_gz。

4.1第一组第一类拉格朗日方程

根据第一类拉格朗日方程，得出一个含有六个未知数λ_i(i＝1,2,…,6)的四个方程的矩阵方程组，λ_i(i＝1,2,…,6)为六个拉格朗日算子；拉格朗日算子λ_i(i＝1,2,…,6)理论上有无限组解；

所述的四个方程的矩阵方程组表达式为：

4.2第二组第一类拉格朗日方程

将求得的拉格朗日算子λ_i(i＝1,2,…,6)的解代入第二组第一类拉格朗日公式得到六个驱动力/力矩，且六个驱动力/力矩具有无穷组解；

再根据第二组第一类拉格朗日公式，当j＝5,6,…10时，写成：

其中，τ是驱动力/力矩。

第五步，伪逆优化方法求解驱动力

采用伪逆矩阵对六个拉格朗日算子λ_i(i＝1,2,…,6)进行优化，通过联立方程消除拉格朗日算子，求得驱动力/力矩的最小二范数解，得到最优驱动力。

六个拉格朗日算子λ_i(i＝1,2,…,6)由公式(26)获得，但是很明显只有四个方程，因此，采用伪逆矩阵进行优化，得到六个拉格朗日算子的数值解。

令

由上面公式(26)和(27)可以消除拉格朗日算子，进而得到：

其中，τ是驱动力/力矩的矩阵。

根据最小二范数解的定义，可以得到，

其中，F_gx、F_gy为广义力，M_gy、M_gz为扭矩，是矩阵的伪逆矩阵，保证了驱动力/力矩的最小二范数解。进而求得冗余驱动并联机构的最优化驱动力，同时该方法可以推广用在其他冗余驱动并联机构的驱动力优化分配问题。

所述冗余驱动并联机构驱动力优化分配的方法运行在冗余驱动并联机构轴组控制验证平台的冗余驱动控制模块中。

所述冗余驱动并联机构轴组控制验证平台，包括冗余驱动控制模块、轴组运动控制模块、伺服驱动、传感系统及冗余驱动并联机构本体，如图3所示。所述冗余驱动控制模块位于上位机PC中，轴组运动控制模块位于下位机轴组运动控制器中，轴组运动控制器与伺服驱动位于控制柜中，传感系统采集冗余驱动并联机构本体的数据，并与轴组运动控制模块相连。

所述的冗余驱动并联机构本体包含6条PUS并联支链与2个点接触高副HKP，机构原理图如图1所示。

所述的一条PUS驱动支链包括伺服电机1、联轴器2、滚珠丝杠3、万向节4、移动副5、驱动杆6和球副7；伺服电机1固定在上固定板9上，通过联轴器2带动滚珠丝杠3转动，滚珠丝杠3转动带动移动副5沿导轨直线运动，移动副5通过万向节4与驱动杆6连接，驱动杆6通过球副7与动平台8连接，六条PUS驱动支链最后固定在下固定板10上。通过电机驱动6PUS机构，进而驱动动平台8在三维空间平动或转动。

所述的HKP包括约束块12和约束杆13；所处位置如图4中的11，约束块12下表面为约束曲面，约束杆13安装在动平台8上，约束杆13与约束曲面在运动过程中保持接触，形成点接触高副；两个点接触高副可以约束动平台两个自由度。因此，整个6PUS-2HKP冗余驱动并联机构是由六台电机驱动，只有四个自由度的冗余驱动机构。

所述冗余驱动并联机构轴组控制验证平台，包括冗余驱动控制模块、轴组运动控制模块、伺服驱动、传感系统及冗余驱动并联机构本体，如图3所示。所述冗余驱动控制模块位于上位机PC中，轴组运动控制模块位于下位机轴组运动控制器中，轴组运动控制器与伺服驱动位于控制柜中，传感系统采集冗余驱动并联机构本体的数据，并与轴组运动控制模块相连。

Claims (2)

1.一种冗余驱动并联机构驱动力优化分配方法，其特征在于，包括以下步骤：

(1)建立机器人坐标系

建立3个坐标系描述冗余驱动并联机构的运动；所述的三个坐标系为基坐标系O_B-X_BY_BZ_B、瞬时坐标系O_L-X_LY_LZ_L和动平台坐标系O_M-X_MY_MZ_M；所述的O_B-X_BY_BZ_B在基座上；所述的O_L-X_LY_LZ_L在左侧点接触高副约束杆(13)上，O_L-X_LY_LZ_L随机器人的运动沿约束曲面滑动或转动，在O_L-X_LY_LZ_L中，Z_L垂直于约束曲面，X_L相切于约束曲面，Y_L与两者正交；所述的O_M-X_MY_MZ_M设置在初始位置O_L-X_LY_LZ_L的位置；

含有点接触高副的冗余驱动并联机构具有4个自由度，广义坐标有4个，采用四个独立位置参数X_L,Y_L,β_L,γ_L描述动平台(8)的位姿；

(2)建立运动学方程

构建点接触高副和并联6PUS机构支链约束方程，完成冗余驱动并联机构的运动学计算；

2.1点接触高副HKP约束方程

左右两侧点接触高副约束曲面的约束方程为：

f_L(X_L,Y_L,Z_L)＝0 (2)

f_R(X_R,Y_R,Z_R)＝0 (3)

其中，f_L和f_R分别是左右两侧约束方程，X_L,Y_L,Z_L为左侧HKP的坐标，X_R,Y_R,Z_R为右侧HKP的坐标；

2.2并联机构6PUS支链约束方程

每个滑块的移动量Q_i的计算公式为：

$Q_i=B_i{M}_i\·e\±\sqrt{{(B_i{M}_i\·e)}^2-\left|\right|B_i{M}_i|{|}^2+{L_i}^2}---\left(5\right)$

其中，B_iM_i用如下公式求得：

B_iM_i＝B_iC_i+C_iM_i (4)

其中，C_i是移动副的位置，B_i是每条支链支撑板的固定位置，连接点M_i在O_B-X_BY_BZ_B的坐标通过坐标转换矩阵^BT_M获得；

动平台8的6个位置参数关系表达式为：

$\begin{array}{c}{f}_i(X_L,Y_L,Z_L,{\mathrm{\α}}_L,{\mathrm{\β}}_L,{\mathrm{\γ}}_L)\\ =f_i(X_L,Y_L,f_L(X_L,Y_L),f_R(X_L,Y_L,f_L(X_L,Y_L),{\mathrm{\β}}_L,{\mathrm{\γ}}_L),\\ {\mathrm{\β}}_L,{\mathrm{\γ}}_L)\\ =f_{Q_i}(X_L,Y_L,{\mathrm{\β}}_L,{\mathrm{\γ}}_L)\end{array}---\left(6\right)$

其中，X_L,Y_L,Z_L,α_L,β_L,γ_L为6个位置参数，X_L,Y_L,β_L,γ_L为4个独立位置参数；

由上述公式(5)和(6)求得冗余驱动并联机构的运动学关系表达式为：

$\left[\begin{array}{c}{Q}_1\\ Q_2\\ Q_3\\ Q_4\\ Q_5\\ Q_6\end{array}\right]={\[T\]}_{6\×6}\·{\left[\begin{array}{c}{X}_L\\ Y_L\\ Z_L\\ {\mathrm{\α}}_L\\ {\mathrm{\β}}_L\\ {\mathrm{\γ}}_L\end{array}\right]}_{6\×1}={\[T_M\]}_{6\×4}\·{\left[\begin{array}{c}{X}_L\\ Y_L\\ {\mathrm{\β}}_L\\ {\mathrm{\γ}}_L\end{array}\right]}_{4\×1}---\left(7\right)$

其中，T为运动学变换矩阵，T_M为位置变换矩阵；已知4个独立位置参数X_L,Y_L,β_L,γ_L，由公式(5)求得6个滑块的位移变化量Q_i(i＝1,2,…,6)；

(3)建立动力学方程

在运动学方程的基础上，求解机器人的动能与势能，建立冗余驱动并联机构的动力学模型；

3.1计算机器人动能

动能计算表达式为：

$K=K_p+\underset{i=1}{\overset{6}{\Σ}}(K_{ci}+K_{cmi})---\left(9\right)$

其中，K_p是动平台的动能，K_ci是滑块的动能，K_cmi是支链驱动杆C_iM_i的动能；

3.2计算机器人的势能

机器人的势能计算公式为：

$P=P_p+\underset{i=1}{\overset{6}{\Σ}}(P_{ci}+P_{cmi})---\left(16\right)$

其中，P_p是动平台的势能，P_ci是滑块的势能，P_cmi是支链驱动杆C_iM_i的势能；

(4)建立拉格朗日方程

采用第一组与第二组第一类拉格朗日方程，通过引入6个拉格朗日算子求解驱动力/力矩，由于冗余驱动并联机构特性，由四个方程求解6个未知数，驱动力/力矩有无限组解；

整个机器人系统的朗格朗日方程为：

L＝K-P (21)

其中，L为拉格朗日方程，K为机器人动能，P为机器人势能；

根据第一组第一类拉格朗日方程，得出一个含有六个拉格朗日算子的四个方程的矩阵方程组，求得的拉格朗日算子有无限组解；

所述的四个方程的矩阵方程组表达式为：

${\left[\begin{array}{cccc}\frac{\∂{\mathrm{\Γ}}_1}{\∂X_L}& \frac{\∂{\mathrm{\Γ}}_2}{\∂X_L}& \mathrm{...}& \frac{\∂{\mathrm{\Γ}}_6}{\∂X_L}\\ \frac{\∂{\mathrm{\Γ}}_1}{\∂Y_L}& \frac{\∂{\mathrm{\Γ}}_2}{\∂Y_L}& \mathrm{...}& \frac{\∂{\mathrm{\Γ}}_6}{\∂Y_L}\\ \frac{\∂{\mathrm{\Γ}}_1}{\∂{\mathrm{\β}}_L}& \frac{\∂{\mathrm{\Γ}}_2}{\∂{\mathrm{\β}}_L}& \mathrm{...}& \frac{\∂{\mathrm{\Γ}}_6}{\∂{\mathrm{\β}}_L}\\ \frac{\∂{\mathrm{\Γ}}_1}{\∂{\mathrm{\γ}}_L}& \frac{\∂{\mathrm{\Γ}}_2}{\∂{\mathrm{\γ}}_L}& \mathrm{...}& \frac{\∂{\mathrm{\Γ}}_6}{\∂{\mathrm{\γ}}_L}\end{array}\right]}_{4\×6}\·\left[\begin{array}{c}{\mathrm{\λ}}_1\\ {\mathrm{\λ}}_2\\ {\mathrm{\λ}}_3\\ {\mathrm{\λ}}_4\\ {\mathrm{\λ}}_5\\ {\mathrm{\λ}}_6\end{array}\right]=\left[\begin{array}{c}\frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{X}}_L}\right)-\frac{\∂L}{\∂X_L}-F_{gx}\\ \frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{Y}}_L}\right)-\frac{\∂L}{\∂Y_L}-F_{gy}\\ \frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{\mathrm{\β}}}_L}\right)-\frac{\∂L}{\∂{\mathrm{\β}}_L}-M_{gy}\\ \frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{\mathrm{\γ}}}_L}\right)-\frac{\∂L}{\∂{\mathrm{\γ}}_L}-M_{gz}\end{array}\right]---\left(26\right)$

其中，λ_i(i＝1,2,…,6)为拉格朗日算子，Γ_i是并联机构第i个支链几何约束方程，i＝1,2…,6；几何约束方程是根据机构中支链C_iM_i的长度是固定值L_i获得；

Γ_i＝||C_iM_i||²-L_i ²＝0(i＝1,2,...,6) (20)

将求得的拉格朗日算子的解代入第二组第一类拉格朗日公式得到六个驱动力/力矩，且六个驱动力/力矩具有无穷组解；

所述的第二组第一类拉格朗日公式为：

其中，τ是驱动力/力矩；

(5)伪逆优化方法求解驱动力

采用伪逆矩阵对六个拉格朗日算子进行优化，得到六个拉格朗日算子的数值解，通过建立联立方程消除拉格朗日算子，求得驱动力/力矩的最小二范数解，得到最优驱动力；

求解过程如下：

$\left[\begin{array}{c}{\mathrm{\λ}}_1\\ {\mathrm{\λ}}_2\\ {\mathrm{\λ}}_3\\ {\mathrm{\λ}}_4\\ {\mathrm{\λ}}_5\\ {\mathrm{\λ}}_6\end{array}\right]={\left[\begin{array}{cccc}\frac{\∂{\mathrm{\Γ}}_1}{\∂X_L}& \frac{\∂{\mathrm{\Γ}}_2}{\∂X_L}& \mathrm{...}& \frac{\∂{\mathrm{\Γ}}_6}{\∂X_L}\\ \frac{\∂{\mathrm{\Γ}}_1}{\∂Y_L}& \frac{\∂{\mathrm{\Γ}}_2}{\∂Y_L}& \mathrm{...}& \frac{\∂{\mathrm{\Γ}}_6}{\∂Y_L}\\ \frac{\∂{\mathrm{\Γ}}_1}{\∂{\mathrm{\β}}_L}& \frac{\∂{\mathrm{\Γ}}_2}{\∂{\mathrm{\β}}_L}& \mathrm{...}& \frac{\∂{\mathrm{\Γ}}_6}{\∂{\mathrm{\β}}_L}\\ \frac{\∂{\mathrm{\Γ}}_1}{\∂{\mathrm{\γ}}_L}& \frac{\∂{\mathrm{\Γ}}_2}{\∂{\mathrm{\γ}}_L}& \mathrm{...}& \frac{\∂{\mathrm{\Γ}}_6}{\∂{\mathrm{\γ}}_L}\end{array}\right]}_{4\×6}^{RM}\·\left[\begin{array}{c}\frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{X}}_L}\right)-\frac{\∂L}{\∂X_L}-F_{gx}\\ \frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{Y}}_L}\right)-\frac{\∂L}{\∂Y_L}-F_{gy}\\ \frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{\mathrm{\β}}}_L}\right)-\frac{\∂L}{\∂{\mathrm{\β}}_L}-M_{gy}\\ \frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{\mathrm{\γ}}}_L}\right)-\frac{\∂L}{\∂{\mathrm{\γ}}_L}-M_{gz}\end{array}\right]---\left(28\right)$

令

$P=\left[\begin{array}{cccc}\frac{\∂{\mathrm{\Γ}}_1}{\∂X_L}& \frac{\∂{\mathrm{\Γ}}_2}{\∂X_L}& \mathrm{...}& \frac{\∂{\mathrm{\Γ}}_6}{\∂X_L}\\ \frac{\∂{\mathrm{\Γ}}_1}{\∂Y_L}& \frac{\∂{\mathrm{\Γ}}_2}{\∂Y_L}& \mathrm{...}& \frac{\∂{\mathrm{\Γ}}_6}{\∂Y_L}\\ \frac{\∂{\mathrm{\Γ}}_1}{\∂{\mathrm{\β}}_L}& \frac{\∂{\mathrm{\Γ}}_2}{\∂{\mathrm{\β}}_L}& \mathrm{...}& \frac{\∂{\mathrm{\Γ}}_6}{\∂{\mathrm{\β}}_L}\\ \frac{\∂{\mathrm{\Γ}}_1}{\∂{\mathrm{\γ}}_L}& \frac{\∂{\mathrm{\Γ}}_2}{\∂{\mathrm{\γ}}_L}& \mathrm{...}& \frac{\∂{\mathrm{\Γ}}_6}{\∂{\mathrm{\γ}}_L}\end{array}\right]---\left(29\right)$

由公式(26)和(27)消除拉格朗日算子得到：

${\[P{\left(\frac{\∂\mathrm{\Γ}}{\∂q}\right)}^{-1}\]}_{4\×6}\·\mathrm{\τ}={\[P{\left(\frac{\∂\mathrm{\Γ}}{\∂q}\right)}^{-1}\]}_{4\×6}\·\left(\frac{d}{dt}\left[\begin{array}{c}\frac{\∂L}{\∂{\stackrel{\·}{q}}_1}\\ \frac{\∂L}{\∂{\stackrel{\·}{q}}_2}\\ .\\ .\\ .\\ \frac{\∂L}{\∂{\stackrel{\·}{q}}_6}\end{array}\right]-\left[\begin{array}{c}\frac{\∂L}{\∂q_1}\\ \frac{\∂L}{\∂q_2}\\ .\\ .\\ .\\ \frac{\∂L}{\∂q_6}\end{array}\right]\right)-\left[\begin{array}{c}\frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{X}}_L}\right)-\frac{\∂L}{\∂X_L}-F_{gx}\\ \frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{Y}}_L}\right)-\frac{\∂L}{\∂Y_L}-F_{gy}\\ \frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{\mathrm{\β}}}_L}\right)-\frac{\∂L}{\∂{\mathrm{\β}}_L}-M_{gy}\\ \frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{\mathrm{\γ}}}_L}\right)-\frac{\∂L}{\∂{\mathrm{\γ}}_L}-M_{gz}\end{array}\right]---\left(30\right)$

由最小二范数解的定义得到：

$\mathrm{\τ}=\frac{d}{dt}\left[\begin{array}{c}\frac{\∂L}{\∂{\stackrel{\·}{q}}_1}\\ \frac{\∂L}{\∂{\stackrel{\·}{q}}_2}\\ .\\ .\\ .\\ \frac{\∂L}{\∂{\stackrel{\·}{q}}_6}\end{array}\right]-\left[\begin{array}{c}\frac{\∂L}{\∂q_1}\\ \frac{\∂L}{\∂q_2}\\ .\\ .\\ .\\ \frac{\∂L}{\∂q_6}\end{array}\right]-{\[P{\left(\frac{\∂\mathrm{\Γ}}{\∂q}\right)}^{-1}\]}_{4\×6}^{RM}\left[\begin{array}{c}\frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{X}}_L}\right)-\frac{\∂L}{\∂X_L}-F_{gx}\\ \frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{Y}}_L}\right)-\frac{\∂L}{\∂Y_L}-F_{gy}\\ \frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{\mathrm{\β}}}_L}\right)-\frac{\∂L}{\∂{\mathrm{\β}}_L}-M_{gy}\\ \frac{d}{dt}\left(\frac{\∂L}{\∂{\stackrel{\·}{\mathrm{\γ}}}_L}\right)-\frac{\∂L}{\∂{\mathrm{\γ}}_L}-M_{gz}\end{array}\right]---\left(15\right)$

由公式(15)得到最优驱动力τ；

其中，F_gx、F_gy为广义力，M_gy、M_gz为扭矩，是矩阵的伪逆矩阵，保证驱动力/力矩的最小二范数解。

2.一种运行权利要求1所述的方法的冗余驱动并联机构轴组控制验证平台，包括冗余驱动控制模块、轴组运动控制模块、伺服驱动、传感系统及冗余驱动并联机构本体；所述冗余驱动控制模块位于上位机中，冗余驱动控制模块进行驱动力优化分配，发出控制指令；轴组运动控制模块位于下位机轴组运动控制器中，轴组运动控制器与伺服驱动位于控制柜中，轴组运动控制器接收冗余驱动控制模块发出的控制指令，并将控制指令通信至伺服驱动；传感系统与轴组运动控制模块相连，传感系统采集冗余驱动并联机构本体的受力、速度和位置数据，并将采集的数据反馈给轴组运动控制模块，实现对伺服电机的闭环控制，保证冗余驱动并联机构电机的驱动力矩。