[发明专利]基于轴不变量的通用3R机械臂逆解建模与解算方法

权利要求书

1.一种基于轴不变量的通用3R机械臂逆解建模与解算方法，其特征是，

应用n个“n元N阶”多项式的Dixon消元与求解原理，进行位姿逆解计算，主要包括以下步骤：

【1】根据机械臂n元3D矢量位姿方程，获得n个“n元2阶”多项式方程；

【2】应用基于轴不变量的Dixon行列式计算式、分块矩阵的行列式计算式或对行列式进行行阶梯化计算式简化行列式计算；

【3】应用n个“n元N阶”多项式的Dixon消元与求解原理完成位姿逆解计算，其中：根据Dixon矩阵的行列式为0，得到一元高阶多项式方程，应用基于友阵的一元高阶多项式方程求解一元高阶多项式方程的解；

对于任意杆件l,定义与欧拉四元数同构的居-吉布斯即Ju-Gibbs规范四元数：

其中：为Gibbs矢量；Gibbs共轭四元数为：

其中：

式中，为居-吉布斯规范四元数模的平方；表达形式幂符表示的x次幂；右上角角标∧或表示分隔符；轴不变量是轴不变量的叉乘矩阵；是Gibbs矢量的叉乘矩阵；若用表示属性占位，则式中的表达形式表示成员访问符；

步骤【1】中，

对于轴链有

建立规范的姿态方程为：

建立规范的定位方程：

式中，k,为任意杆件，表达形式表示的x次幂；右上角角标∧或表示分隔符；是轴不变量的叉乘矩阵，杆件l,为杆件k,时同理替换；1为三维单位矩阵；ⁱQ_n表示姿态；为沿矢量轴的线位置；为零位时由原点至原点O_l的平动矢量；为投影符，为在大地坐标系的投影矢量。

2.根据权利要求1所述的基于轴不变量的通用3R机械臂逆解建模与解算方法，其特征是，

步骤【2】中，基于轴不变量的Dixon行列式计算式为：

根据运动链Dixon行列式性质有：

并记：

式中，为旋转变换矩阵；表示用辅助变量y_l的前l个依次替换原变量τ_l中的l个变量，记“|”为替换操作符；

式(80)将及转化为关于的多重线性型；同时，对y_l及τ_l具有对称性；

由式(47)得3R运动学方程

由式(90)得

由(91)式得

记

则由式(51)及式得(93)

由式(92)及式(93)得

由式(95)得3R运动学多项式方程

多项式系统F₃(Y₂|T₂)，根据双线性型行列式通式

则有

其中：

中组合变量系数为独立的列向量，故选取的系数来构成方阵剩余列向量一定与的各列相关；

由式(80)及式(93)得

分别表示轴2至轴3、轴3至轴3_S的零位矢量、径向矢量及轴向矢量；

得简化的3元N阶Dixon行列式为

式中，为大小为S×S的Dixon矩阵，其第[i][j]成员为单变量τ₁的N阶多项式。

3.根据权利要求2所述的基于轴不变量的通用3R机械臂逆解建模与解算方法，其特征是，

步骤【2】中，分块矩阵的行列式计算式为：

若记大小为(n+m)×(n+m)的方阵为M，大小为n×n的矩阵是方阵M的前n行及任意n列元素构成的子矩阵，大小为m×m的矩阵是方阵M后m行及剩余m列元素构成的子矩阵；由升序排列的矩阵列序号构成的序列cn及cm是序列[1:m+n]的子集，[cn,cm]∈<1:n+m>，且有cm∪cn＝[1:m+n]；则方阵M行列式与分块矩阵及的行列式关系为