[发明专利]冗余驱动并联机构驱动力优化方法及轴组控制验证平台

权利要求书

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