一种基于构型平面的水下机械臂运动学的求解方法
【技术领域】
[0001 ] 本发明提供的是一种基于构型平面的水下机械臂运动学的求解方法。
【背景技术】
[0002] 随着工业机器人技术的广泛的应用，空间6R串联机构的应用具有重要的意义。串 联机构运动学逆解是串联机器人控制计算的先决条件，它直接关系到机器人离线编程、轨 迹规划、实时控制等工作，在机器人学中占有重要地位，只有通过运动学逆解把空间位姿转 换为关节变量，才能实现对机器人末端执行器按空间位姿进行编程控制（如直线轨迹和圆 弧轨迹等）。
[0003] 串联机构运动学中，空间6R串联机构的运动学逆解是最困难的，该问题与空间机 构学中的单环7R机构运动学逆解属于同一问题，曾被喻为空间机构运动分析中的珠穆朗 玛峰。各国学者对此开展很多有益的探索和研宄。空间6R串联机构的运动学逆解求解方 法分为解析形式和数值形式。一般6R串联操作臂运动学逆解因涉及结构参数多、解的非线 性和耦合性以及需要求解代数方程等问题而变得难于得到解析解。解析解法适用于具有特 殊几何结构参数的6R串联操作臂，可以应用矢量、螺旋或李代数方法得到理论解，这种方 法具有计算结果准确、能够得到全部解等优点，但需要进行大量的代数和矩阵运算，推导过 程比较复杂，并且有解的条件是操作臂的位置和姿态具有解耦特征或其特征多项式的次数 小于等于4。廖启征将倍四元数引入空间串联机器人运动学研宄当中，解决了一个经典的 6R机器人的逆运动学问题。2006年，有学者提出把串联运动链拆成几个简单部分的组合， 但该方法只适于某些解耦的特殊情况。以往用于串联机构位置逆解数学建模的方法主要有 D-H矩阵法、球面三角法、实矩阵法、对偶数法等，得到了各不相同的逆解算法，不具有通用 性。Raghavan和Roth通过矢量运算由6个逆运动学等式构造14个基础方程，消元运算后 得到一元24次方程，求出最多16组逆运动学解，但存在8个增根.Manocha采用24阶矩阵 特征分解方法对Raghavan的算法进行改进，提高了逆运动学解算的稳定性和精度。为解决 空间7R机构的位移分析难题，分别采用复数方法和矩阵运算构造10个基础方程，进而得到 一元16次方程，消除了增根。借鉴前期学者研宄成果，将6R串联型机器人逆运动学求解问 题分为两类：封闭解法求解满足Pieper准则的6R机器人的逆运动学问题；矢量计算和符 号运算将Manocha得到的目标矩阵从24阶降低到16阶，并以矩阵特征分解方法提高一般 6R机器人逆运动学求解的效率和稳定性，并组合牛顿-拉夫森迭代算法解决非Pieper准则 的6R机器人的逆运动学问题。
[0004] 而对于6R串联型机器人实际运动作业下，仅仅需要一种能够实时快速找到满足 一定工作要求（如避障和动力学要求）和末端工作点位姿要求逆运动学解。为此产生了 很多种数值形式的串联机器人逆运动学求解方法。一个常用的数值方法是将6R串联操作 臂各关节的D-H参数中的径向参数ai、a i和轴向参数Si、0 i分离，运用双四元数方法或 李代数方法将6R串联操作臂运动学正解矩阵构造成两个独立的齐次变换线性方程组，通 过将两个方程组联立逐次迭代或消元而得到关于各关节转角的16组运动学逆解。如QIA0 等运用双四元数理论得到了一般6R串联操作臂运动学逆解的数值解；ROCCO等运用李群、 李代数等方法也得到了该问题的数值解。另一个比较常用的数值方法是将遗传算法和神 经网络等工具引入6R操作臂的运动学逆解问题中，通过设定关节转角进给值等约束条件， 以运动学正解和目标值之间的差值最小化为目标函数，采用上述算法求解最佳拟合的关节 转角进给值。如CHIDDARWAR等比较了预测型与常规型神经网络算法对求解效率的影响； KOKER等提出了一种考虑关节速度和加速度的3自由度机器人运动学逆解神经网络算 法；KALRA等提出了一种基于遗传算法的6自由度工业机器人运动学逆解算法；HAMM0UR等 采用连续传算法规划了 6R操作臂的运动轨迹；ZHA利用末端执行器位置和姿态矢量构成的 曲面特征，通过遗传算法搜寻该曲面最小特征值而获得最优轨迹规划等。
[0005] 本文借鉴其他方法的研宄经验，以6R转动关节机器人为研宄对象，运用构型平面 的方法，解决机器人逆运动学通用快速求解问题，为机器人关节控制提供运动参数。
【发明内容】
[0006] 本发明的目的在于提供一种克服传统解析方法的求解机器人构形的局限性和专 一性，也克服了通用的迭代方法非实时性和精度问题，能够解决6R机器人逆运动学求解问 题的基于构型平面的水下机械臂运动学的求解方法。
[0007] 本发明的主要目的是这样实现的：
[0008] -种基于构型平面的水下机械臂运动学的求解方法包括如下步骤：
[0009] (1)输入水下机械臂关节参数：输入水下机械臂关节参数，进行机械臂关节分析， 将已知的机器人工作构形按照关节模块的形式进行分解，对机器人关节根据运动形式分解 成相应的基本关节的形式输入机器人目标点的位置矩阵；
[0010] (2)基本关节建模：对组成空间机械臂进行基本运动关节进行运动学建模，并进 行归一化处理，形成统一形式的建模方法：
[0011] (2. 1)摇摆模块建模
[0012] 摇摆模块运动模型矩阵：
[0013]
[0014] 式中0yb-摇摆模块两部分旋转相对角度；hyb-摇摆中心到下一个模块连接面的 长度；
[0015] (2. 2)移动模块建模
[0016] 移动模块运动模型矩阵：
[0017]
[0018] 式中Hyd-移动模块中心到下一模块中心的长度；hyd-移动模块的移动量；
[0019] (2. 3)回转模块建模
[0020] 回转模块运动模型矩阵：
[0021]
[0022] 式中0hz-回转模块两部分旋转相对角度；lhz-回转中心到下一个模块中心的长 度；
[0023] (2. 4)连接模块建模
[0024] 连接模块运动模型矩阵：
[0025]
[0026]式中氏」一模块长度；
[0027] (2. 5)模块的统一表达形式
[0028] 基于前面各模块的建模方法，建立可重构机器人模块的统一表达方式：
[0029]
[0030] 式中0为该模块是回转模块时的回转角度，若为其他模块时为零；0为该模块是 摇摆模块时的摇摆角度，若为其他模块时为零；h为该模块是摇摆模块时的连接长度，若为 其他模块时为零；1为该模块是回转模块或连接模块时的连接长度，若为其他模块时为零； w为该模块是移动模块时的移动量，若为其他模块时为零；
[0031] (2. 6)通过基本模块的统一表达方式，在已知模块类型的情况下，建立整个机器人 运动学形式：
[0032] T(总）=
[0033] (3)构形平面分解：
[0034] 对水下机械臂进行构形平面分解，对多角度连接模块进行处理，形成合理分解，将 机械臂的位置和姿态进行有效分解；
[0035] 机械臂关键关节中心位置求解、通过位置求解其中三个关节运动量、通过姿态求 解剩余三个关节运动量、运动量校核、输入控制器进行控制；
[0036] 求解关键关节中心的三维空间位置坐标，以这些位置坐标和分解的两个构形平面 求解组成构形平面的关节广义运动量；
[0037] 求解关键关节中心点：
[0038]目标点位姿矩阵为：
[0039]
[0040] 2关节和5关节的中心位置
[0041] TS2=[0 0 dl+d2]
[0042]
[0043] 将2关节的中心设为新的原点，5关节的中心设为目标点，则新的目标点的位置表 示为
[0044]
[0045] 分解的两个构形平面模型表达式
[0046] 构形平面简化的转化矩阵为：
u u u i」Lu u u i
[0047]
[0048]
[0049] 利用位置关系求解几个关节的广义运动量
[0050]设 p'x= px-d6*ax
[0051] p'y=py-d6*a y
[0052] p' z=pz-d6*az-d1-d 2
[0053] TGX1*TGX2*[0 0 0 1]T=[p，xp'yp'z1]T
[0054] 可得：
[0055]