一种主从式水下机械臂的延时控制方法
【专利摘要】本发明涉及机器人技术领域，提供的是一种能够保证主从式水下机械臂在作业环境恶劣、传输通讯延时条件下，实现水下机械臂主手与从手的协调控制的主从式水下机械臂的延时控制方法。本发明包括：给定水下机械臂的结构参数，根据水下机械臂的运动学模型，对主从机械臂进行时延分析；采集水动力因素，根据水下机械臂动力学模型，验证机械臂性能是否满足模型需求；对主手动作信号进行延时处理，将前n个时刻关节控制信号缓存在水下控制器中，然后基于多次幂曲线拟合方法计算出从手n+1时刻的关节理想运动位置。本发明解决了水下机械臂作业环境恶劣、传输通讯延时造成的水下机械臂工作间断性等难题。
【专利说明】
一种主从式水下机械臂的延时控制方法
技术领域
[0001] 本发明涉及机器人技术领域，提供的是一种能够保证主从式水下机械臂在作业环 境恶劣、传输通讯延时条件下，实现水下机械臂主手与从手的协调控制的主从式水下机械 臂的延时控制方法。
【背景技术】
[0002] 由于水下环境复杂，存在海水压力、能见度低、温度下降等不利于人类工作的环境 条件，可用主从水下多功能机械臂配合作业型水下机器人代替人类在深海等危险环境工 作，水下机械臂可进行海底勘探开采，水下管道维修，石油管道铺建等工作。水下机械臂不 仅降低人类水下作业操作的危险，还满足人类扩展了感知、对未知领域探索的需求。因此设 计主从水下机械臂控制系统具有重要的研究和实际工程意义。
[0003] 许多专家在水下机械臂的运动控制方面提出了很多方法。开关控制是水下机械臂 最简单也是最古老的控制方式，水面控制器由操作平台开关按钮组成，水面操作人员通过 操作对应关节的开关开合来控制水下机械臂从手各关节的运动，操作人员通过观察水下摄 像器传输回的图像信息来判断从手关节的运动位置，由上述可知，水下机械臂开关控制方 式为开环控制，并且开关控制的从手运动速度固定，所以控制精度主要依赖于操作人员的 经验判断，水下作业工作效率有限，并且多关节协助控制时对操作人员难度较大。
[0004] 相较于开关控制，速度控制的最大优点就是关节运动速度的可控性。理想条件下， 关节从初始位置到目标位置肯定经过先加速、再匀速、再减速的过程，那么开关控制的关节 运动速度固定，这样在最后很难准确的停在目标位置，而速度控制可以高效的调整关节运 动。速度控制方式通过比例阀箱控制各关节油液流量从而控制机械臂从手各关节的运动速 度。但是速度控制和开关控制有一个共同的缺点，那就是对水面操作人员的要求较高。速度 控制采用操作杆作为水面控制器，操作杆的操作方向与关节的运动方向可能不同，如大臂 关节为上下俯仰运动、操作杆操作方向为左右运动，这样会增加操作人员工作量，并且不能 做到同时控制多个关节的速度。速度控制虽然解决了开关控制从手运动精准度不高的问 题，但仍没解决实际控制中操控人员操控困难的问题。
[0005] 随着技术发展，考虑前两种控制方式的弊端，研究人员设计位置反馈的控制方式， 也称为主从控制方式，通过将操作杆改进成从手缩小版模型进行位置反馈，使得操作人员 可以形象化得知从手位置信息，不用像开关控制和速度控制似得在头脑中构建机械臂运动 模型，这样可以避免操作对机械臂的损伤，并且方便操控人员同时控制多个关节运动。主从 控制方式使得操作人员操作水下机械臂时有人臂一体的感觉，有效地提高操作人员工作效 率。并且位置反馈控制也具有速度控制的优点，通过操作主手发出的速度指令，也可以将速 度指令值用控制装置经积分变换为位置信息，进行位置反馈控制。
[0006] 但是在水下作业实际控制中，水下机械臂在深海作业时，机械臂主手控制信号传 输距离远，传统的485总线通讯传输速度慢，所以传输延时时间长，这样就造成从手接收控 制信号滞后于水面操作，水下机械臂从手在工作中出现滞后、间断运动等不良现象，影响水 面操作人员对从手位置的判断，从而使操作人员的操作难度加大，进而影响完成水下任务 的作业效率。
【发明内容】
[0007] 本发明目的在于提供一种形式简单，减少计算量，能够按需要得到优化解，具有通 用性和快速性的主从式水下机械臂的延时控制方法。
[0008] 本发明的目的是这样实现的：
[0009] 本发明包括以下步骤：
[0010] (1)给定水下机械臂的结构参数，根据水下机械臂的运动学模型，对主从机械臂进 行时延分析；
[0011] (2)采集水动力因素，根据水下机械臂动力学模型，验证机械臂性能是否满足模型 需求；
[0012] (3)对主手动作信号进行延时处理，将前n个时刻关节控制信号缓存在水下控制器 中，然后基于多次幂曲线拟合方法计算出从手n+1时刻的关节理想运动位置；
[0013] (4)分析机械臂主手是否存在急停变向紧急操作以及机械臂从手是否存在洋流干 扰因素，通过延时控制环节和洋流反馈校正环节的判断，实现主从水下机械臂平稳连续控 制；
[0014] (5)根据运动学模型、动力学模型以及延时控制算法建立控制系统模型，并基于该 模型进行仿真，验证延时控制方法的有效性及可靠性。
[0015] 步骤2所述的水下机械臂动力学模型在不考虑水动力因素时，是运用牛顿一欧拉 方程对机械臂进行动力学建模的，其中水下机械臂在任一运动时刻，将机械臂各个关节角 度变量qi、速度变量么、加速度变量矣，i = 1，. . .n作为已知条件，解算各关节的力矩变量Qi， i = l，.. .n，根据牛顿一欧拉法首先向前迭代计算机械臂的各连杆的速度和加速度，其中基 座初始运动状态是确定的，如果基座固定，则基坐标角速度 13 ?〇 = 〇,角加速度根据牛 顿一欧拉法向后迭代计算机械臂的各关节的力及力矩；初始条件是力矩^心力 &+i 冰下机械臂末端点可在机械臂工作空间内自由活动，则MdAnd为零；
[0016] (2.1)向前迭代解算机械臂各连杆的关节角度变量、速度变量：
[0024]其中，i+1coi+1为第i + 1个关节的角速度,11為+:1为第i+1个关节的角加速度;Ri+1,i为 第i个关节到第i+1个关机的旋转矩阵为第i+1个关节的线加速度;为第i个关节的 质心处的线加速度;U第i+1个关节自身的角速度;1ei为第i个关节的旋转轴线;1+11 1+1为 第i+1个连杆的长度矢量;ipiSi坐标系到i连杆质心处的位置矢量;11+1为第i+1个关节到 质心坐标原点处的惯性张量;m 1+1为第i+1个关节的质量;1+1F1+1为第i+1个关节的惯性力； 1+ iMl+1为第i+1个关节的惯性力矩；
[0025] (2.2)向后迭代解算机械臂关节的力?:和力矩弘i=n，n-l，? ? ?，1:
[0029] 其中，1为1坐标系到i连杆受力点的位置矢量;％为第i个关节的关节力/馬，为 第i个关节相对于i坐标系的关节力矩;QMi为第i个关节相对于i-1坐标系的关节力矩；
[0030] 当机械臂连杆关节的重量作为考虑因素时，设固定的机器人基座受到的 支撑作用相当向上的重力加速度g，考虑水动力因素时，选择瑞雷耗散函数描述粘性阻尼和 流体阻力下物体的运动情况：
[0031] 在拉格朗日动力学算法中引入瑞雷耗散函数：
[0033]可得带有瑞雷耗散函数的拉格朗日方程：
[0035] 其中qs为广义位置向量，&为相应的速度向量，T为拉格朗日函数；
[0036] 由于牛顿一欧拉方法求出不含水动力变量的力矩
[0037] 贝1J
[0039]对六自由度水下机械臂推导分析，各关节相对于前一关节的速度表示为(i = 1,2,3.....6)，根据刚体定轴转动定理，得出各关节平动速度，进而得到各关节的质心点的 运动绝对速度°vi(i = l，2,3……6);
[0040]求解机械臂各个关节的耗散函数巾i(i = 1，. .6)，
[0042]得到水下机械臂系统的耗散函数为伞=伞1+伞2+伞3+伞4+伞5+伞6,将瑞雷耗散函数带 入拉格朗日方程进行推导，求导瑞雷耗散函数巾的广义速率i，参数代入方程-? 即可求得水动力阻尼系数。
[0043]步骤3所述的幂曲线拟合方法包括：曲线拟合是通过实验获得有限对测试数据 (Xl，yi)，利用这些数据来求取近似函数y = f(x)的，其中X为输出量，y为被测物理量，延时控 制中时间为输出量，关节角度为被测物理量;将曲线拟合法应用到主从水下机械臂延时控 制中，延时环节时间设置为两个参数周期，将这种方法称为两周期延时，加上由于485总线 传输造成的信号延时，从手的滞后主手三个周期运动，在主手运动i+3时刻，由于485通信信 号的延时，i+3时刻水下控制器刚接收到主手i+2时刻位置，控制器得知三个时刻的位置点， 即i+3时刻的从手位置、i+1时刻的主手控制信号、i+2时刻的主手控制信号，根据关节位置 时间信息，提前判断出从手的运动变化趋势，从而利用两周期延时控制方法，计算出下一时 刻从手理想运动点；
[0044] 其中主手关节i在tk-2时控制角度值为0k-2，在控制角度值为0H，从手关节在 tk时刻实际角度值为0k，得到坐标分别为(tk-2，0 k-2)、（tk-Jk-〇和(tk，0k)，根据关节曲线运 动拟合方程，得到t k+1时刻的角度预测值为0k+1:
[0045] 0k+i = 0k+v(k)XT+aT2/2
[0046] 其中，a为关节的角加速度，T为传输周期0.1s;v(k)为k时刻关节i转动速度；
[0047]由匀加速度方程得到：
[0049]进行计算，得到^+1时刻的从手关节角度的理想值
[0051] 最终由当前时刻从手关节角度和延时两个时刻的主手关节角度来计算下一时刻 从手关节速度、角速度，从而得到理想的运动角度。
[0052] 步骤(4)所述的延时控制环节包括:对主手运动曲线进行分析，从而判断机械臂主 手是否存在变向，加速，减速的突变运动环节，进而对从手进行运动控制；
[0053] 其中第k步主手输出目标值x(k)与第k+1步主手输出目标值x(k+l)之间的角度差 为e(k)=X(k+l)-x(k)，对该角度差进行判断分析，根据角度差范围的不同，控制从手下一 步如何运动：
[0054]当|e(k)|〈0.2°时，主手控制变化，处于从手运动能力范围内，不存在紧急操作，延 时环节正常工作，利用多周期延时算法得出从手下一时刻的理想运动点；
[0055]当| e(k) | >0.2°时，此时角度差超出机械臂校正能力范围，主手出现急停避障运 动；机械臂停止运动，保持当前位置，直到主手信号从新回到从手停止状态后，重新开始运 动。
[0056] 洋流反馈校正环节主要判断是否存在洋流干扰对机械臂从手运动造成影响，如果 洋流影响超出了机械臂校正运动能力范围，认为洋流干扰影响过大，从手机械臂停止当前 运动，直到机械臂主手从新回到机械臂从手停止位置，机械臂从手继续跟随主手运动;如果 洋流影响较小，影响处于机械臂运动能力范围内，机械臂根据主手目标位置进行恰当的位 置校正，再进行延时环节控制，使得机械臂从手平稳运动；
[0057] 其中第k步主手关节输出目标值x(k)与从手关节实际运动位置.小纟丨之间的误差为 = A# ：) ，对该误差进行判断分析，根据误差范围的不同，决定从手之后如何运动；
[0058] 当| e(k) |〈0 ? 02°时，校正后的从手输出值a: (/〇 = .Y(幻，无需校正，保持从手当前运 动状态；
[0059] 当0 ? 02°〈 | e(k) |〈0 ? 2°时，校正后的从手输出值x及)= i(f) + e(f)，误差在机械臂 校正范围能力内，对从手当前位置进行校正；
[0060] 当|6(1〇|>0.2°时，校正后从手输出值'(幻=4幻，误差超出机械臂校正能力范 围，机械臂停止运动，保持当前位置，直到主手信号从新回到从手停止状态后，重新开始运 动。
[0061] 本发明的有益效果在于：
[0062] 通过结合主从机械臂的结构特点和工作方式，将延时控制方法引入到主从水下机 械臂的运动控制中，该方法能够直观快速的实现主从式水下机械臂的平滑稳定控制。基于 幂曲线拟合的延时控制算法，通过对已知位置信息多次迭代的方式对从手进行理想关节位 置确定，该方法能够有效实现水下从机械臂运动轨迹平滑，避免洋流干扰和操作不当造成 运动抖动和停顿。基于主从机械臂的延迟控制方法，为远程遥操作方式工作的机器人控制 方法提供了借鉴，具有一定通用性和实用性。最终本发明解决了水下机械臂作业环境恶劣、 传输通讯延时造成的水下机械臂工作间断性等难题。该方法形式简洁，具有很高的精度和 求解速度，满足水下机械臂运动曲线平滑稳定的作业需要。
【附图说明】
[0063] 图1主从机械臂的控制系统流程图；
[0064] 图2水下机械臂的杆件关系图；
[0065]图3曲线拟合法示意图；
[0066]图4洋流校正判断；
[0067] 图5 Simulink控制系统图；
[0068]图6水下机械臂结构简图；
[0069]图7洋流干扰模拟图；
[0070] 图8洋流干扰的主从机械臂单关节两周期延时控制仿真图。
【具体实施方式】
[0071] 下面结合附图对本发明做进一步描述。
[0072] 本发明涉及主从水下机械臂控制系统设计领域，提供了一种主从式水下机械臂的 延时控制方法。本发明以主从式水下机械臂的时延分析入手，利用幂曲线拟合的延时控制 算法，确定主从式机械臂协调控制的过程，通过运动学模型的建立、延时控制环节和洋流反 馈环节的校正、动力学校核，快速找到水下机械臂从手各关节优化位置，能够保证主从式水 下机械臂在作业环境恶劣、传输通讯延时条件下，实现水下机械臂主手与从手的协调控制。 本发明解决的主从式机械臂协调控制的通用性问题，不依赖机器人构形，形式简单，降低了 求解的难度，减少计算量，能够按需要得到优化解，具有通用性和快速性，满足给定目标任 务和环境对主从式水下机械臂的协调控制的要求。
[0073] 步骤1:给定水下机械臂的结构参数，建立机械臂的运动学模型，同时计算出主手 到从手的通信延时时间。
[0074]步骤2:考虑水动力因素，建立水下机械臂动力学模型，用以验证机械臂性能是否 满足模型需求，并且可以应用于实际控制操作中。
[0075]步骤3:对主手动作信号进行延时处理，将前n个时刻关节控制信号缓存在水下控 制器中，然后基于多次幂曲线拟合方法计算出从手n+1时刻的关节理想运动位置。
[0076] 步骤4:分析机械臂主手是否存在急停变向等紧急操作以及机械臂从手是否存在 洋流干扰等因素，通过延时控制环节和洋流反馈校正环节的判断，实现主从水下机械臂平 稳连续控制。
[0077] 步骤5:根据运动学模型、动力学模型以及延时控制算法可以建立控制系统模型， 并基于该模型进行仿真，验证延时控制方法的有效性及可靠性。
[0078] 实施例1，结合附图1，本发明的方法包括如下步骤：
[0079] 步骤1:给定水下机械臂的结构参数，建立机械臂的运动学模型，同时计算出主手 到从手的通信延时时间。
[0080] 步骤2:考虑水动力因素，建立水下机械臂动力学模型，用以验证机械臂性能是否 满足模型需求，并且可以应用于实际控制操作中。
[0081 ]步骤3:对主手动作信号进行延时处理，将前n个时刻关节控制信号缓存在水下控 制器中，然后基于多次幂曲线拟合方法计算出从手n+1时刻的关节理想运动位置。
[0082]步骤4:分析机械臂主手是否存在急停变向等紧急操作以及机械臂从手是否存在 洋流干扰等因素，通过延时控制环节和洋流反馈校正环节的判断，实现主从水下机械臂平 稳连续控制。
[0083]步骤5:根据运动学模型、动力学模型以及延时控制算法可以建立控制系统模型， 并基于该模型进行仿真，验证延时控制方法的有效性及可靠性。
[0084]实施例2,结合附图2,建立水下机械臂的动力学模型。首先不考虑水动力因素，运 用牛顿一欧拉方程对机械臂进行动力学建模。
[0085] 水下机械臂在任一运动时刻，将机械臂各个关节角度变量9:、速度变量么、加速度 变量成，i = l,. . .n作为已知条件，解算各关节的力矩变量Qi，i = l,. . .n。根据牛顿一欧拉法 首先向前迭代计算机械臂的各连杆的速度和加速度。假设基座初始运动状态是确定的，如 果基座固定，则却=〇。之后根据牛顿一欧拉法向后迭代计算机械臂的各关节的力 及力矩。初始条件是，巧 +1 = 。假设水下机械臂末端点可在机械臂工作空间 内自由活动，则Mend、Fend为零。
[0086] (1)向前迭代解算机械臂各连杆的qi、4 / = 0.1…n-1-.
[0094] 其中：i+1?i+1为第i+1个关节的角速度；
[0095] ;+>+1为第i+1个关节的角加速度；
[0096] R1+1>1为第i个关节到第i+1个关机的旋转矩阵；
[0097] f+S>f+1为第i+1个关节的线加速度；
[0098] Vci为第i个关节的质心处的线加速度；
[0099] 么+,为第i+1个关节自身的角速度；
[0100] iei为第i个关节的旋转轴线；
[0101] i+1li+1为第i+1个连杆的长度矢量；
[0102] ipi为i坐标系到i连杆质心处的位置矢量；
[0103] 11+1为第i+1个关节到质心坐标原点处的惯性张量；
[0104] mi+1为第i+1个关节的质量；
[0105] i+1Fi+1为第i+1个关节的惯性力；
[0106] 1+1M1+1为第i+1个关节的惯性力矩。
[0107] (2)向后迭代解算机械臂关节的力?:和力矩弘i=n，n-l，? ? ?，1:
[0111]其中"hi为i坐标系到i连杆受力点的位置矢量；
[0112] 为第i个关节的关节力；
[0113] 为第i个关节相对于i坐标系的关节力矩；
[0114] 为第i个关节相对于i-1坐标系的关节力矩。
[0115]当机械臂连杆关节的重量作为考虑因素时，可设°匕=§，固定的机器人基座受到 的支撑作用相当向上的重力加速度g。这样处理与各模块重力的影响完全一样。有上述推导 可得机械臂动力学模型。
[0116]考虑水动力因素时，选择瑞雷耗散函数描述粘性阻尼和流体阻力下物体的运动情 况。具体求解机械臂的瑞雷耗散函数过程如下：
[0117]在拉格朗日动力学算法中引入瑞雷耗散函数
[0119]可得带有瑞雷耗散函数的拉格朗日方程
[0121]其中qs为广义位置向量，为相应的速度向量，T为拉格朗日函数。
[0122] 由于牛顿一欧拉方法求出不含水动力变量的力矩
[0123] 贝 IJ
[0125] 对六自由度水下机械臂推导分析，各关节相对于前一关节的速度表示为i_lvi(i = 1,2,3.....6)。根据刚体定轴转动定理，可得出各关节平动速度，进而可以得到各关节的 质心点的运动绝对速度Vi (i = 1，2，3……6)。
[0126] 求解机械臂各个关节的耗散函数巾i(i = 1，. .6)，
[0128] 由此可得水下机械臂系统的耗散函数为巾=巾巾2+巾3+巾4+巾5+巾6。将瑞雷耗 散函数带入拉格朗日方程进行推导，求导瑞雷耗散函数巾的广义速率4，参数代入方程
即可求得水动力阻尼系数。
[0129] 实施例3,结合附图3,曲线拟合是通过实验获得有限对测试数据(Xl，yi)，利用这些 数据来求取近似函数y = f (x)。式中x为输出量，y为被测物理量。延时控制中时间为输出量， 关节角度为被测物理量。
[0130] 将曲线拟合法应用到主从水下机械臂延时控制中，延时环节时间设置为两个参数 周期，将这种方法称为两周期延时。加上由于485总线传输造成的信号延时，从手的滞后主 手三个周期运动，流程分析:在主手运动i+3时刻，由于485通信信号的延时，i+3时刻水下控 制器刚接收到主手i+2时刻位置，由此控制器得知三个时刻的位置点，即i+3时刻的从手位 置(根据水下控制闭环反馈得知）、i+l时刻的主手控制信号、i+2时刻的主手控制信号。根据 关节位置时间信息，可以提前判断出从手的运动变化趋势，比如变向、急停、变速等情况，从 而利用两周期延时控制方法，可以计算出下一时刻从手理想运动点。
[0131] 假定主手关节i在tk-2时控制角度值为0k-2，在控制角度值为0H，从手关节在 tk时刻实际角度值为0k，则可得其坐标分别为(tk-2，0 k-2)、（tk-10k-〇和(tk，0k)，根据关节曲 线运动拟合方程，可得t k+1时刻的角度预测值为0k+1，其值为
[0132] 0k+i = 0k+v(k) XT+aT2/2 (15)
[0133] 其中，a是关节的角加速度，T为传输周期0.1s;v(k)为k时刻关节i转动速度。
[0134] 由匀加速度方程可以推出
[0136]综合(15)、（16)式进行计算，得到^+1时刻的从手关节角度的理想值
[0138]这就是两周期延时的计算公式。由当前时刻从手关节角度和延时两个时刻的主手 关节角度来计算下一时刻从手关节速度、角速度，从而得到理想的运动角度。
[0139] 实施例4,结合附图4,延时控制使得水下机械臂从手滞后于主手运动，使得控制器 得知主手的后续运动轨迹，可以得出从手运动的理想值，但实际操作中存在急停变向等紧 急操作，所以需要对主手运动曲线进行分析，从而判断机械臂主手是否存在变向，加速，减 速的突变运动环节，进而对从手进行运动控制。
[0140] 假设第k步主手输出目标值x(k)与第k+1步主手输出目标值x(k+l)之间的角度差 为e(k)=X(k+l)-x(k)，对该角度差进行判断分析，根据角度差范围的不同，控制从手下一 步如何运动。
[0141 ]当| e(k) |〈0.2°时，主手控制变化，处于从手运动能力范围内，不存在避障、急速变 向等紧急操作，延时环节正常工作，利用多周期延时算法得出从手下一时刻的理想运动点。
[0142] 当| e(k) | >0.2°时，此时角度差超出机械臂校正能力范围，此状况是由主手出现急 停避障运动。机械臂停止运动，保持当前位置，直到主手信号从新回到从手停止状态后，重 新开始运动。
[0143] 洋流反馈校正环节的特征是计算从手实际运动位置和主手相应时刻目标位置的 差值，判读示意图如附图4所示，主要判断是否存在洋流干扰对机械臂从手运动造成影响， 如果洋流影响超出了机械臂校正运动能力范围，认为洋流干扰影响过大，从手机械臂停止 当前运动，直到机械臂主手从新回到机械臂从手停止位置，机械臂从手继续跟随主手运动， 反之，如果洋流影响较小，影响处于机械臂运动能力范围内，机械臂根据主手目标位置进行 恰当的位置校正，再进行延时环节控制，使得机械臂从手平稳运动。
[0144] 假设第k步主手关节输出目标值x(k)与从手关节实际运动位置i(/c)之间的误差为 以人：>=a# :>;，对该误差进行判断分析，根据误差范围的不同，决定从手之后如何运动。
[0145] 当|e(k)|〈0.02°时，校正后的从手输出值'(幻=.说），误差不大，无需校正，保持 从手当前运动状态。
[0146] 当0 ? 02°〈 | e(k) |〈0 ? 2°时，校正后的从手输出值A: (/f) = x(/〇 ，误差在机械臂 校正范围能力内，对从手当前位置进行校正。
[0147] 当| e(k) | >0 ? 2°时，校正后从手输出值⑷=x(/〇，此时误差超出机械臂校正能力 范围，此状况是由于洋流干扰过大造成。机械臂停止运动，保持当前位置，直到主手信号从 新回到从手停止状态后，重新开始运动。
[0148] 实施5，结合图5，水下机械臂是一个六输入六输出的系统，控制系统包括控制算法 部分，运动学模型部分及动力学模型部分，设定七功能机械臂各个关节的运动轨迹，运动轨 迹为连续的曲线，对其进行分段处理，每隔单位时间根据控制算法对控制信号进行采样，运 用M文件编写控制算法。
[0149] 根据延时控制算法，可以得出新的关节运动目标点。将新的运动目标点带入运动 学模型中，并封装在M文件中，解算出各关节的运动轨迹。
[0150]动力学模型封装在相应的M文件中，得到各关节运动轨迹后，对各个关节进行动力 学力矩分析，从而得出各个关节的相关控制力矩。
[0151] Simul ink控制系统框图如附图5所示。
[0152] 实施6,结合附图6,举例进行验证，举例水下七功能机械臂的关节参数如表1所示， 七功能水下机械臂结构简图如附图6所示。
[0153] 表1七功能机械臂各杆参数
[0155] 运用Pro/E工具对多功能水下机械臂进行模型搭建，从而解算出机械臂各连杆的 重心向量、质量和转动惯性矩等参数，七功能机械臂各关节参数如下表2、3所示。
[0156] 表2七功能机械臂各连杆质量及重心向量
[0158] 表3七功能机械臂各连杆惯性矩
[0159]
[0160] 假设六自由度水下机械臂各关节初始状态为:qz = [ 0，0，0，0，0，0 ]
[0162] 采用相同的一组随机数作为洋流干扰，洋流干扰如附图7所示。主手操作人员操作 主手单关节，先慢后快的增大关节角度，突然急停变向，操作主手关节重新回到从手停止处 后，继续操作关节角度增加，之后改变关节角度变化方向，操作关节角度匀速减小。
[0163] 洋流干扰的单关节两周期延时控制仿真如附图8所示。
【主权项】
1. 一种主从式水下机械臂的延时控制方法，其特征在于，包括以下步骤： (1) 给定水下机械臂的结构参数，根据水下机械臂的运动学模型，对主从机械臂进行时 延分析； (2) 采集水动力因素，根据水下机械臂动力学模型，验证机械臂性能是否满足模型需 求； (3) 对主手动作信号进行延时处理，将前n个时刻关节控制信号缓存在水下控制器中， 然后基于多次幂曲线拟合方法计算出从手n+1时刻的关节理想运动位置； (4) 分析机械臂主手是否存在急停变向紧急操作以及机械臂从手是否存在洋流干扰因 素，通过延时控制环节和洋流反馈校正环节的判断，实现主从水下机械臂平稳连续控制； (5) 根据运动学模型、动力学模型以及延时控制算法建立控制系统模型，并基于该模型 进行仿真，验证延时控制方法的有效性及可靠性。2. 根据权利要求1所述的一种主从式水下机械臂的延时控制方法，其特征在于：步骤 (2)所述的水下机械臂动力学模型在不考虑水动力因素时，是运用牛顿一欧拉方程对机械 臂进行动力学建模的，其中水下机械臂在任一运动时刻，将机械臂各个关节角度变量qi、速 度变量#、加速度变量氣，i = l，. . .n作为已知条件，解算各关节的力矩变量Qi，i = l，. . .n， 根据牛顿一欧拉法首先向前迭代计算机械臂的各连杆的速度和加速度，其中基座初始运动 状态是确定的，如果基座固定，则基坐标角速度％() = 〇,角加速度4=〇;根据牛顿一欧拉法 向后迭代计算机械臂的各关节的力及力矩;初始条件是力矩力巧水 下机械臂末端点可在机械臂工作空间内自由活动，则MmcKF-为零； (2.1) 向前迭代解算机械臂各连杆的关节角度变量、速度变量：其中，i+1 i+1为第i+1个关节的角速度严1為+1为第i+1个关节的角加速度;Ri +1, i为第i个 关节到第i+1个关机的旋转矩阵广为第i+1个关节的线加速度;为第i个关节的质心 处的线加速度;为第i+1个关节自身的角速度; 1&为第i个关节的旋转轴线;1+111+1为第i+ 1个连杆的长度矢量;ipiSi坐标系到i连杆质心处的位置矢量^1 1+1为第i + 1个关节到质心 坐标原点处的惯性张量;m1+1为第i+1个关节的质量;1+1F 1+1为第i+1个关节的惯性力;1+1M1+1 为第i+1个关节的惯性力矩； (2.2) 向后迭代解算机械臂关节的力Fi和力矩Qi i = n,n-l, ? ? ? ,1: ^M^'Ad^-R + ^FJiA QM,=Kx；MJ^elA 其中，1为1坐标系到i连杆受力点的位置矢量;％为第i个关节的关节力/焉；为第i个 关节相对于i坐标系的关节力矩;QMi为第i个关节相对于i_l坐标系的关节力矩； 当机械臂连杆关节的重量作为考虑因素时，设= g，固定的机器人基座受到的支撑作 用相当向上的重力加速度g，考虑水动力因素时，选择瑞雷耗散函数描述粘性阻尼和流体阻 力下物体的运动情况： 在拉格朗日动力学算法中引入瑞雷耗散函数：可得带有瑞雷耗散函数的拉格朗日方程：其中qs为广义位置向量，I为相应的速度向量，T为拉格朗日函数；由于牛顿一欧拉方法求出不含水动力变量的 则对六自由度水下机械臂推导分析，各关节相对于前一关节的速度表示为(i = 1，2， 3……6)，根据刚体定轴转动定理，得出各关节平动速度，进而得到各关节的质心点的运动 绝对速度°vi(i = l，2,3……6); 求解机械臂各个关节的耗散函数巾i(i = 1，. . 6)，得到水下机械臂系统的耗散函数为1+ 2+巾3+巾4+巾5+巾6，将瑞雷耗散函数带 入拉格朗日方程进行推导，求导瑞雷耗散函数巾的广义速率&，参数代入 即可求得水动力阻尼系数。3.根据权利要求1所述的一种主从式水下机械臂的延时控制方法，其特征在于：步骤 (3)所述的幂曲线拟合方法包括：曲线拟合是通过实验获得有限对测试数据(Xl，yi)，利用这 些数据来求取近似函数y = f (x)的，其中x为输出量，y为被测物理量，延时控制中时间为输 出量，关节角度为被测物理量;将曲线拟合法应用到主从水下机械臂延时控制中，延时环节 时间设置为两个参数周期，将这种方法称为两周期延时，加上由于485总线传输造成的信号 延时，从手的滞后主手三个周期运动，在主手运动i+3时刻，由于485通信信号的延时，i+3时 刻水下控制器刚接收到主手i+2时刻位置，控制器得知三个时刻的位置点，即i+3时刻的从 手位置、i + 1时刻的主手控制信号、i+2时刻的主手控制信号，根据关节位置时间信息，提前 判断出从手的运动变化趋势，从而利用两周期延时控制方法，计算出下一时刻从手理想运 动点； 其中主手关节i在tk-2时控制角度值为0k-2，在控制角度值为0!^，从手关节在t k时 亥嫉际角度值为知，得到坐标分别为⑷-以^以^^滴^^丄根据关节曲线运动拟 合方程，得到tk+1时刻的角度预测值为0 k+1: 0k+i = 9k+v(k) XT+aT2/2 其中，a为关节的角加速度，T为传输周期0.1 s; v (k)为k时刻关节i转动速度； 由匀加速度方程得到：进行计算，得到^+1时刻的从手关节角度的理想值最终由当前时刻从手关节角度和延时两个时刻的主手关节角度来计算下一时刻从手 关节速度、角速度，从而得到理想的运动角度。4.根据权利要求1所述的一种主从式水下机械臂的延时控制方法，其特征在于：步骤 (4)所述的延时控制环节包括:对主手运动曲线进行分析，从而判断机械臂主手是否存在变 向，加速，减速的突变运动环节，进而对从手进行运动控制； 其中第k步主手输出目标值x(k)与第k+1步主手输出目标值x(k+l)之间的角度差为e (k) = x(k+l)_x(k)，对该角度差进行判断分析，根据角度差范围的不同，控制从手下一步如 何运动： 当|e(k)|〈0.2°时，主手控制变化，处于从手运动能力范围内，不存在紧急操作，延时环 节正常工作，利用多周期延时算法得出从手下一时刻的理想运动点； 当|e(k)| >0.2°时，此时角度差超出机械臂校正能力范围，主手出现急停避障运动;机 械臂停止运动，保持当前位置，直到主手信号从新回到从手停止状态后，重新开始运动。 洋流反馈校正环节主要判断是否存在洋流干扰对机械臂从手运动造成影响，如果洋流 影响超出了机械臂校正运动能力范围，认为洋流干扰影响过大，从手机械臂停止当前运动， 直到机械臂主手从新回到机械臂从手停止位置，机械臂从手继续跟随主手运动;如果洋流 影响较小，影响处于机械臂运动能力范围内，机械臂根据主手目标位置进行恰当的位置校 正，再进行延时环节控制，使得机械臂从手平稳运动； 其中第k步主手关节输出目标值x(k)与从手关节实际运动位置4/()之间的误差为 e(fc) = ，对该误差进行判断分析，根据误差范围的不同，决定从手之后如何运动； 当|e(k)|〈0.02°时，校正后的从手输出值.= ，无需校正，保持从手当前运动状 态； 当0.02°〈 | e(k) |〈0.2°时，校正后的从手输出值，误差在机械臂校正 范围能力内，对从手当前位置进行校正； 当I e(k) | >0 ? 2°时，校正后从手输出值.Y<.⑷=a?⑷，误差超出机械臂校正能力范围，机械 臂停止运动，保持当前位置，直到主手信号从新回到从手停止状态后，重新开始运动。
【文档编号】G05B13/04GK106054599SQ201610352534
【公开日】2016年10月26日
【申请日】2016年5月25日
【发明人】魏延辉, 罗姗姗, 姚贵鹏, 乔金鹤, 张皓渊, 李宁波, 郭锐, 洪国庆
【申请人】哈尔滨工程大学