CN106054599A - 一种主从式水下机械臂的延时控制方法 - Google Patents

Abstract

本发明涉及机器人技术领域，提供的是一种能够保证主从式水下机械臂在作业环境恶劣、传输通讯延时条件下，实现水下机械臂主手与从手的协调控制的主从式水下机械臂的延时控制方法。本发明包括：给定水下机械臂的结构参数，根据水下机械臂的运动学模型，对主从机械臂进行时延分析；采集水动力因素，根据水下机械臂动力学模型，验证机械臂性能是否满足模型需求；对主手动作信号进行延时处理，将前n个时刻关节控制信号缓存在水下控制器中，然后基于多次幂曲线拟合方法计算出从手n+1时刻的关节理想运动位置。本发明解决了水下机械臂作业环境恶劣、传输通讯延时造成的水下机械臂工作间断性等难题。

Description

一种主从式水下机械臂的延时控制方法

技术领域

本发明涉及机器人技术领域，提供的是一种能够保证主从式水下机械臂在作业环境恶劣、传输通讯延时条件下，实现水下机械臂主手与从手的协调控制的主从式水下机械臂的延时控制方法。

背景技术

由于水下环境复杂，存在海水压力、能见度低、温度下降等不利于人类工作的环境条件，可用主从水下多功能机械臂配合作业型水下机器人代替人类在深海等危险环境工作，水下机械臂可进行海底勘探开采，水下管道维修，石油管道铺建等工作。水下机械臂不仅降低人类水下作业操作的危险，还满足人类扩展了感知、对未知领域探索的需求。因此设计主从水下机械臂控制***具有重要的研究和实际工程意义。

许多专家在水下机械臂的运动控制方面提出了很多方法。开关控制是水下机械臂最简单也是最古老的控制方式，水面控制器由操作平台开关按钮组成，水面操作人员通过操作对应关节的开关开合来控制水下机械臂从手各关节的运动，操作人员通过观察水下摄像器传输回的图像信息来判断从手关节的运动位置，由上述可知，水下机械臂开关控制方式为开环控制，并且开关控制的从手运动速度固定，所以控制精度主要依赖于操作人员的经验判断，水下作业工作效率有限，并且多关节协助控制时对操作人员难度较大。

相较于开关控制，速度控制的最大优点就是关节运动速度的可控性。理想条件下，关节从初始位置到目标位置肯定经过先加速、再匀速、再减速的过程，那么开关控制的关节运动速度固定，这样在最后很难准确的停在目标位置，而速度控制可以高效的调整关节运动。速度控制方式通过比例阀箱控制各关节油液流量从而控制机械臂从手各关节的运动速度。但是速度控制和开关控制有一个共同的缺点，那就是对水面操作人员的要求较高。速度控制采用操作杆作为水面控制器，操作杆的操作方向与关节的运动方向可能不同，如大臂关节为上下俯仰运动、操作杆操作方向为左右运动，这样会增加操作人员工作量，并且不能做到同时控制多个关节的速度。速度控制虽然解决了开关控制从手运动精准度不高的问题，但仍没解决实际控制中操控人员操控困难的问题。

随着技术发展，考虑前两种控制方式的弊端，研究人员设计位置反馈的控制方式，也称为主从控制方式，通过将操作杆改进成从手缩小版模型进行位置反馈，使得操作人员可以形象化得知从手位置信息，不用像开关控制和速度控制似得在头脑中构建机械臂运动模型，这样可以避免操作对机械臂的损伤，并且方便操控人员同时控制多个关节运动。主从控制方式使得操作人员操作水下机械臂时有人臂一体的感觉，有效地提高操作人员工作效率。并且位置反馈控制也具有速度控制的优点，通过操作主手发出的速度指令，也可以将速度指令值用控制装置经积分变换为位置信息，进行位置反馈控制。

但是在水下作业实际控制中，水下机械臂在深海作业时，机械臂主手控制信号传输距离远，传统的485总线通讯传输速度慢，所以传输延时时间长，这样就造成从手接收控制信号滞后于水面操作，水下机械臂从手在工作中出现滞后、间断运动等不良现象，影响水面操作人员对从手位置的判断，从而使操作人员的操作难度加大，进而影响完成水下任务的作业效率。

发明内容

本发明目的在于提供一种形式简单，减少计算量，能够按需要得到优化解，具有通用性和快速性的主从式水下机械臂的延时控制方法。

本发明的目的是这样实现的：

本发明包括以下步骤：

(1)给定水下机械臂的结构参数，根据水下机械臂的运动学模型，对主从机械臂进行时延分析；

(2)采集水动力因素，根据水下机械臂动力学模型，验证机械臂性能是否满足模型需求；

(3)对主手动作信号进行延时处理，将前n个时刻关节控制信号缓存在水下控制器中，然后基于多次幂曲线拟合方法计算出从手n+1时刻的关节理想运动位置；

(4)分析机械臂主手是否存在急停变向紧急操作以及机械臂从手是否存在洋流干扰因素，通过延时控制环节和洋流反馈校正环节的判断，实现主从水下机械臂平稳连续控制；

(5)根据运动学模型、动力学模型以及延时控制算法建立控制***模型，并基于该模型进行仿真，验证延时控制方法的有效性及可靠性。

步骤2所述的水下机械臂动力学模型在不考虑水动力因素时，是运用牛顿—欧拉方程对机械臂进行动力学建模的，其中水下机械臂在任一运动时刻，将机械臂各个关节角度变量q_i、速度变量加速度变量i＝1,...n作为已知条件，解算各关节的力矩变量Q_i，i＝1,...n，根据牛顿—欧拉法首先向前迭代计算机械臂的各连杆的速度和加速度，其中基座初始运动状态是确定的，如果基座固定，则基坐标角速度⁰ω₀＝0，角加速度根据牛顿—欧拉法向后迭代计算机械臂的各关节的力及力矩；初始条件是力矩力水下机械臂末端点可在机械臂工作空间内自由活动，则M_end、F_end为零；

(2.1)向前迭代解算机械臂各连杆的关节角度变量、速度变量：

${\mathrm{\ω}_{i+1}}_{i+1}=R_{i+1,i}({\mathrm{\ω}_i}_i+{\stackrel{\·}{q}}_{i+1}{e_i}_i)$

${\stackrel{\·}{\mathrm{\ω}}_{i+1}}_{i+1}=R_{i+1,i}({\stackrel{\·}{\mathrm{\ω}}_i}_i+{\stackrel{\·\·}{q}}_{i+1}{e_i}_i+{\stackrel{\·}{q}}_{i+1}{\mathrm{\ω}_i}_i\×{e_i}_i)$

${\stackrel{\·}{v}_{i+1}}_{i+1}=R_{i+1,i}{\stackrel{\·}{v}_i}_i+{\stackrel{\·}{\mathrm{\ω}}_{i+1}}_{i+1}\×{l_{i+1}}_{i+1}+{\mathrm{\ω}_{i+1}}_{i+1}\×({\mathrm{\ω}_{i+1}}_{i+1}\×{l_{i+1}}_{i+1})$

ⁱv_ci＝ⁱv_i+ⁱω_i×ⁱρ_i

${\stackrel{\·}{v}_i}_{ci}={\stackrel{\·}{v}_i}_i+{\stackrel{\·}{\mathrm{\ω}}_i}_i\×{\mathrm{\ρ}_i}_i+{\mathrm{\ω}_i}_i\×({\mathrm{\ω}_i}_i\×{\mathrm{\ρ}_i}_i)$

${F_{i+1}}_{i+1}=m_{i+1}{\stackrel{\·}{v}_i}_{ci}$

${}^{i+1}{M}_{i+1}={I_c}_{i+1}{\stackrel{\·}{\mathrm{\ω}}_{i+1}}_{i+1}+{\mathrm{\ω}_{i+1}}_{i+1}\×\left({I_c}_{i+1}{\mathrm{\ω}_{i+1}}_{i+1}\right)$

其中，ⁱ⁺¹ω_i+1为第i+1个关节的角速度；为第i+1个关节的角加速度；R_i+1，i为第i个关节到第i+1个关机的旋转矩阵；为第i+1个关节的线加速度；ⁱv_ci为第i个关节的质心处的线加速度；为第i+1个关节自身的角速度；ⁱe_i为第i个关节的旋转轴线；ⁱ⁺¹l_i+1为第i+1个连杆的长度矢量；ⁱρ_i为i坐标系到i连杆质心处的位置矢量；^cI_i+1为第i+1个关节到质心坐标原点处的惯性张量；m_i+1为第i+1个关节的质量；ⁱ⁺¹F_i+1为第i+1个关节的惯性力；^{i+ 1}M_i+1为第i+1个关节的惯性力矩；

(2.2)向后迭代解算机械臂关节的力F_i和力矩Q_i i＝n,n-1,...,1：

其中，ⁱh_i为i坐标系到i连杆受力点的位置矢量；为第i个关节的关节力；为第i个关节相对于i坐标系的关节力矩；Q^M _i为第i个关节相对于i-1坐标系的关节力矩；

当机械臂连杆关节的重量作为考虑因素时，设固定的机器人基座受到的支撑作用相当向上的重力加速度g，考虑水动力因素时，选择瑞雷耗散函数描述粘性阻尼和流体阻力下物体的运动情况：

在拉格朗日动力学算法中引入瑞雷耗散函数：

$\mathrm{\φ}=\frac{1}{2}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(k_{ix}\stackrel{.}{{x_i}^2}+k_{iy}{y_i}^2+k_{iz}{z_i}^2),$

可得带有瑞雷耗散函数的拉格朗日方程：

$\frac{d}{dt}\frac{\∂T}{\∂{\tilde{q}}_s}-\frac{\∂T}{\∂q_s}=-\frac{\∂\mathrm{\φ}}{\∂q_s}+Q_s,(s=1,2,\mathrm{3.....}n),$

其中q_s为广义位置向量，为相应的速度向量，T为拉格朗日函数；

由于牛顿—欧拉方法求出不含水动力变量的力矩

则

$Q_s=\mathrm{\τ}+\frac{\∂\mathrm{\φ}}{\∂q_s},(s=1,2,\mathrm{3.....}n)$

对六自由度水下机械臂推导分析，各关节相对于前一关节的速度表示为^i-1v_i(i＝1,2,3.....6)，根据刚体定轴转动定理，得出各关节平动速度，进而得到各关节的质心点的运动绝对速度⁰v_i(i＝1,2,3.....6)；

求解机械臂各个关节的耗散函数φ_i(i＝1，2…..6)，

${\mathrm{\φ}}_i=\frac{u^0{v_i}^2}{2}=\frac{u}{2}{\∫}_0^{l_i}v_0{{}_i}^2{\mathrm{dx}}_i$

得到水下机械臂***的耗散函数为φ＝φ₁+φ₂+φ₃+φ₄+φ₅+φ₆，将瑞雷耗散函数带入拉格朗日方程进行推导，求导瑞雷耗散函数φ的广义速率参数代入方程即可求得水动力阻尼系数。

步骤3所述的幂曲线拟合方法包括：曲线拟合是通过实验获得有限对测试数据(x_i,y_i)，利用这些数据来求取近似函数y＝f(x)的，其中x为输出量，y为被测物理量，延时控制中时间为输出量，关节角度为被测物理量；将曲线拟合法应用到主从水下机械臂延时控制中，延时环节时间设置为两个参数周期，将这种方法称为两周期延时，加上由于485总线传输造成的信号延时，从手的滞后主手三个周期运动，在主手运动i+3时刻，由于485通信信号的延时，i+3时刻水下控制器刚接收到主手i+2时刻位置，控制器得知三个时刻的位置点，即i+3时刻的从手位置、i+1时刻的主手控制信号、i+2时刻的主手控制信号，根据关节位置时间信息，提前判断出从手的运动变化趋势，从而利用两周期延时控制方法，计算出下一时刻从手理想运动点；

其中主手关节i在t_k-2时控制角度值为θ_k-2，在t_k-1时控制角度值为θ_k-1，从手关节在t_k时刻实际角度值为θ_k，得到坐标分别为(t_k-2,θ_k-2)、(t_k-1,θ_k-1)和(t_k,θ_k)，根据关节曲线运动拟合方程，得到t_k+1时刻的角度预测值为θ_k+1：

θ_k+1＝θ_k+v(k)×T+aT²/2

其中，a为关节的角加速度，T为传输周期0.1s；v(k)为k时刻关节i转动速度；

由匀加速度方程得到：

$\left\{\begin{array}{c}v(k-1)=({\mathrm{\θ}}_{k-1}-{\mathrm{\θ}}_{k-2})/T\\ v\left(k\right)=({\mathrm{\θ}}_k-{\mathrm{\θ}}_{k-1})/T\\ a=(v\left(k\right)-v\left(k-1\right))/T\end{array}\right.$

进行计算，得到t_k+1时刻的从手关节角度的理想值

${\mathrm{\θ}}_{k+1}=\frac{5}{2}{\mathrm{\θ}}_k-2{\mathrm{\θ}}_{k-1}+\frac{1}{2}{\mathrm{\θ}}_{k-2}$

最终由当前时刻从手关节角度和延时两个时刻的主手关节角度来计算下一时刻从手关节速度、角速度，从而得到理想的运动角度。

步骤(4)所述的延时控制环节包括：对主手运动曲线进行分析，从而判断机械臂主手是否存在变向，加速，减速的突变运动环节，进而对从手进行运动控制；

其中第k步主手输出目标值x(k)与第k+1步主手输出目标值x(k+1)之间的角度差为e(k)＝x(k+1)-x(k)，对该角度差进行判断分析，根据角度差范围的不同，控制从手下一步如何运动：

当|e(k)|<0.2°时，主手控制变化，处于从手运动能力范围内，不存在紧急操作，延时环节正常工作，利用多周期延时算法得出从手下一时刻的理想运动点；

当|e(k)|>0.2°时，此时角度差超出机械臂校正能力范围，主手出现急停避障运动；机械臂停止运动，保持当前位置，直到主手信号从新回到从手停止状态后，重新开始运动。

洋流反馈校正环节主要判断是否存在洋流干扰对机械臂从手运动造成影响，如果洋流影响超出了机械臂校正运动能力范围，认为洋流干扰影响过大，从手机械臂停止当前运动，直到机械臂主手从新回到机械臂从手停止位置，机械臂从手继续跟随主手运动；如果洋流影响较小，影响处于机械臂运动能力范围内，机械臂根据主手目标位置进行恰当的位置校正，再进行延时环节控制，使得机械臂从手平稳运动；

其中第k步主手关节输出目标值x(k)与从手关节实际运动位置之间的误差为对该误差进行判断分析，根据误差范围的不同，决定从手之后如何运动；

当|e(k)|<0.02°时，校正后的从手输出值无需校正，保持从手当前运动状态；

当0.02°<|e(k)|<0.2°时，校正后的从手输出值误差在机械臂校正范围能力内，对从手当前位置进行校正；

当|e(k)|>0.2°时，校正后从手输出值误差超出机械臂校正能力范围，机械臂停止运动，保持当前位置，直到主手信号从新回到从手停止状态后，重新开始运动。

本发明的有益效果在于：

通过结合主从机械臂的结构特点和工作方式，将延时控制方法引入到主从水下机械臂的运动控制中，该方法能够直观快速的实现主从式水下机械臂的平滑稳定控制。基于幂曲线拟合的延时控制算法，通过对已知位置信息多次迭代的方式对从手进行理想关节位置确定，该方法能够有效实现水下从机械臂运动轨迹平滑，避免洋流干扰和操作不当造成运动抖动和停顿。基于主从机械臂的延迟控制方法，为远程遥操作方式工作的机器人控制方法提供了借鉴，具有一定通用性和实用性。最终本发明解决了水下机械臂作业环境恶劣、传输通讯延时造成的水下机械臂工作间断性等难题。该方法形式简洁，具有很高的精度和求解速度，满足水下机械臂运动曲线平滑稳定的作业需要。

附图说明

图1主从机械臂的控制***流程图；

图2水下机械臂的杆件关系图；

图3曲线拟合法示意图；

图4洋流校正判断；

图5 Simulink控制***图；

图6水下机械臂结构简图；

图7洋流干扰模拟图；

图8洋流干扰的主从机械臂单关节两周期延时控制仿真图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明涉及主从水下机械臂控制***设计领域，提供了一种主从式水下机械臂的延时控制方法。本发明以主从式水下机械臂的时延分析入手，利用幂曲线拟合的延时控制算法，确定主从式机械臂协调控制的过程，通过运动学模型的建立、延时控制环节和洋流反馈环节的校正、动力学校核，快速找到水下机械臂从手各关节优化位置，能够保证主从式水下机械臂在作业环境恶劣、传输通讯延时条件下，实现水下机械臂主手与从手的协调控制。本发明解决的主从式机械臂协调控制的通用性问题，不依赖机器人构形，形式简单，降低了求解的难度，减少计算量，能够按需要得到优化解，具有通用性和快速性，满足给定目标任务和环境对主从式水下机械臂的协调控制的要求。

步骤1：给定水下机械臂的结构参数，建立机械臂的运动学模型，同时计算出主手到从手的通信延时时间。

步骤2：考虑水动力因素，建立水下机械臂动力学模型，用以验证机械臂性能是否满足模型需求，并且可以应用于实际控制操作中。

步骤3：对主手动作信号进行延时处理，将前n个时刻关节控制信号缓存在水下控制器中，然后基于多次幂曲线拟合方法计算出从手n+1时刻的关节理想运动位置。

步骤4：分析机械臂主手是否存在急停变向等紧急操作以及机械臂从手是否存在洋流干扰等因素，通过延时控制环节和洋流反馈校正环节的判断，实现主从水下机械臂平稳连续控制。

步骤5：根据运动学模型、动力学模型以及延时控制算法可以建立控制***模型，并基于该模型进行仿真，验证延时控制方法的有效性及可靠性。

实施例1，结合附图1，本发明的方法包括如下步骤：

步骤1：给定水下机械臂的结构参数，建立机械臂的运动学模型，同时计算出主手到从手的通信延时时间。

步骤2：考虑水动力因素，建立水下机械臂动力学模型，用以验证机械臂性能是否满足模型需求，并且可以应用于实际控制操作中。

步骤3：对主手动作信号进行延时处理，将前n个时刻关节控制信号缓存在水下控制器中，然后基于多次幂曲线拟合方法计算出从手n+1时刻的关节理想运动位置。

步骤4：分析机械臂主手是否存在急停变向等紧急操作以及机械臂从手是否存在洋流干扰等因素，通过延时控制环节和洋流反馈校正环节的判断，实现主从水下机械臂平稳连续控制。

步骤5：根据运动学模型、动力学模型以及延时控制算法可以建立控制***模型，并基于该模型进行仿真，验证延时控制方法的有效性及可靠性。

实施例2，结合附图2，建立水下机械臂的动力学模型。首先不考虑水动力因素，运用牛顿—欧拉方程对机械臂进行动力学建模。

水下机械臂在任一运动时刻，将机械臂各个关节角度变量q_i、速度变量加速度变量i＝1,...n作为已知条件，解算各关节的力矩变量Q_i，i＝1,...n。根据牛顿—欧拉法首先向前迭代计算机械臂的各连杆的速度和加速度。假设基座初始运动状态是确定的，如果基座固定，则⁰ω₀＝0，之后根据牛顿—欧拉法向后迭代计算机械臂的各关节的力及力矩。初始条件是 假设水下机械臂末端点可在机械臂工作空间内自由活动，则M_end、F_end为零。

(1)向前迭代解算机械臂各连杆的q_i、

${\mathrm{\&omega;}_{i+1}}_{i+1}=R_{i+1,i}({\mathrm{\&omega;}_i}_i+{\stackrel{\&CenterDot;}{q}}_{i+1}{e_i}_i)---\left(1\right)$

${\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_{i+1}}_{i+1}=R_{i+1,i}({\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_i}_i+{\stackrel{\&CenterDot;\&CenterDot;}{q}}_{i+1}{e_i}_i+{\stackrel{\&CenterDot;}{q}}_{i+1}{\mathrm{\&omega;}_i}_i\&times;{e_i}_i)---\left(2\right)$

${\stackrel{\&CenterDot;}{v}_{i+1}}_{i+1}=R_{i+1,i}{\stackrel{\&CenterDot;}{v}_i}_i+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_{i+1}}_{i+1}\&times;{l_{i+1}}_{i+1}+{\mathrm{\&omega;}_{i+1}}_{i+1}\&times;({\mathrm{\&omega;}_{i+1}}_{i+1}\&times;{l_{i+1}}_{i+1})---\left(3\right)$

ⁱv_ci＝ⁱv_i+ⁱω_i×ⁱρ_i (4)

${\stackrel{\&CenterDot;}{v}_i}_{ci}={\stackrel{\&CenterDot;}{v}_i}_i+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_i}_i\&times;{\mathrm{\&rho;}_i}_i+{\mathrm{\&omega;}_i}_i\&times;({\mathrm{\&omega;}_i}_i\&times;{\mathrm{\&rho;}_i}_i)---\left(5\right)$

${F_{i+1}}_{i+1}=m_{i+1}{\stackrel{\&CenterDot;}{v}_i}_{ci}---\left(6\right)$

${}^{i+1}{M}_{i+1}={I_c}_{i+1}{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_{i+1}}_{i+1}+{\mathrm{\&omega;}_{i+1}}_{i+1}\&times;\left({I_c}_{i+1}{\mathrm{\&omega;}_{i+1}}_{i+1}\right)---\left(7\right)$

其中：ⁱ⁺¹ω_i+1为第i+1个关节的角速度；

为第i+1个关节的角加速度；

R_i+1，i为第i个关节到第i+1个关机的旋转矩阵；

为第i+1个关节的线加速度；

ⁱv_ci为第i个关节的质心处的线加速度；

为第i+1个关节自身的角速度；

ⁱe_i为第i个关节的旋转轴线；

ⁱ⁺¹l_i+1为第i+1个连杆的长度矢量；

ⁱρ_i为i坐标系到i连杆质心处的位置矢量；

^cI_i+1为第i+1个关节到质心坐标原点处的惯性张量；

m_i+1为第i+1个关节的质量；

ⁱ⁺¹F_i+1为第i+1个关节的惯性力；

ⁱ⁺¹M_i+1为第i+1个关节的惯性力矩。

(2)向后迭代解算机械臂关节的力F_i和力矩Q_i i＝n,n-1,...,1：

其中：ⁱh_i为i坐标系到i连杆受力点的位置矢量；

为第i个关节的关节力；

为第i个关节相对于i坐标系的关节力矩；

Q^M _i为第i个关节相对于i-1坐标系的关节力矩。

当机械臂连杆关节的重量作为考虑因素时，可设固定的机器人基座受到的支撑作用相当向上的重力加速度g。这样处理与各模块重力的影响完全一样。有上述推导可得机械臂动力学模型。

考虑水动力因素时，选择瑞雷耗散函数描述粘性阻尼和流体阻力下物体的运动情况。具体求解机械臂的瑞雷耗散函数过程如下：

在拉格朗日动力学算法中引入瑞雷耗散函数

$\mathrm{\&phi;}=\frac{1}{2}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}(k_{ix}\stackrel{.}{{x_i}^2}+k_{iy}{y_i}^2+k_{iz}{z_i}^2)---\left(11\right)$

可得带有瑞雷耗散函数的拉格朗日方程

$\frac{d}{dt}\frac{\&part;T}{\&part;{\tilde{q}}_s}-\frac{\&part;T}{\&part;q_s}=-\frac{\&part;\mathrm{\&phi;}}{\&part;q_s}+Q_s,(s=1,2,\mathrm{3.....}n)---\left(12\right)$

其中q_s为广义位置向量，为相应的速度向量，T为拉格朗日函数。

由于牛顿—欧拉方法求出不含水动力变量的力矩

则

$Q_s=\mathrm{\&tau;}+\frac{\&part;\mathrm{\&phi;}}{\&part;q_s},(s=1,2,\mathrm{3.....}n)---\left(13\right)$

对六自由度水下机械臂推导分析，各关节相对于前一关节的速度表示为i-1v_i(i＝1,2,3.....6)。根据刚体定轴转动定理，可得出各关节平动速度，进而可以得到各关节的质心点的运动绝对速度⁰v_i(i＝1,2,3.....6)。

求解机械臂各个关节的耗散函数φ_i(i＝1，2…..6)，

${\mathrm{\&phi;}}_i=\frac{u^0{v_i}^2}{2}=\frac{u}{2}{\&Integral;}_0^{l_i}v_0{{}_i}^2{\mathrm{dx}}_i---\left(14\right)$

由此可得水下机械臂***的耗散函数为φ＝φ₁+φ₂+φ₃+φ₄+φ₅+φ₆。将瑞雷耗散函数带入拉格朗日方程进行推导，求导瑞雷耗散函数φ的广义速率参数代入方程即可求得水动力阻尼系数。

实施例3，结合附图3，曲线拟合是通过实验获得有限对测试数据(x_i,y_i)，利用这些数据来求取近似函数y＝f(x)。式中x为输出量，y为被测物理量。延时控制中时间为输出量，关节角度为被测物理量。

将曲线拟合法应用到主从水下机械臂延时控制中，延时环节时间设置为两个参数周期，将这种方法称为两周期延时。加上由于485总线传输造成的信号延时，从手的滞后主手三个周期运动，流程分析：在主手运动i+3时刻，由于485通信信号的延时，i+3时刻水下控制器刚接收到主手i+2时刻位置，由此控制器得知三个时刻的位置点，即i+3时刻的从手位置(根据水下控制闭环反馈得知)、i+1时刻的主手控制信号、i+2时刻的主手控制信号。根据关节位置时间信息，可以提前判断出从手的运动变化趋势，比如变向、急停、变速等情况，从而利用两周期延时控制方法，可以计算出下一时刻从手理想运动点。

假定主手关节i在t_k-2时控制角度值为θ_k-2，在t_k-1时控制角度值为θ_k-1，从手关节在t_k时刻实际角度值为θ_k，则可得其坐标分别为(t_k-2,θ_k-2)、(t_k-1,θ_k-1)和(t_k,θ_k)，根据关节曲线运动拟合方程，可得t_k+1时刻的角度预测值为θ_k+1，其值为

θ_k+1＝θ_k+v(k)×T+aT²/2 (15)

其中，a是关节的角加速度，T为传输周期0.1s；v(k)为k时刻关节i转动速度。

由匀加速度方程可以推出

$\left\{\begin{array}{c}v(k-1)=({\mathrm{\&theta;}}_{k-1}-{\mathrm{\&theta;}}_{k-2})/T\\ v\left(k\right)=({\mathrm{\&theta;}}_k-{\mathrm{\&theta;}}_{k-1})/T\\ a=(v\left(k\right)-v\left(k-1\right))/T\end{array}\right.---\left(16\right)$

综合(15)、(16)式进行计算，得到t_k+1时刻的从手关节角度的理想值

${\mathrm{\&theta;}}_{k+1}=\frac{5}{2}{\mathrm{\&theta;}}_k-2{\mathrm{\&theta;}}_{k-1}+\frac{1}{2}{\mathrm{\&theta;}}_{k-2}---\left(17\right)$

这就是两周期延时的计算公式。由当前时刻从手关节角度和延时两个时刻的主手关节角度来计算下一时刻从手关节速度、角速度，从而得到理想的运动角度。

实施例4，结合附图4，延时控制使得水下机械臂从手滞后于主手运动，使得控制器得知主手的后续运动轨迹，可以得出从手运动的理想值，但实际操作中存在急停变向等紧急操作，所以需要对主手运动曲线进行分析，从而判断机械臂主手是否存在变向，加速，减速的突变运动环节，进而对从手进行运动控制。

假设第k步主手输出目标值x(k)与第k+1步主手输出目标值x(k+1)之间的角度差为e(k)＝x(k+1)-x(k)，对该角度差进行判断分析，根据角度差范围的不同，控制从手下一步如何运动。

当|e(k)|<0.2°时，主手控制变化，处于从手运动能力范围内，不存在避障、急速变向等紧急操作，延时环节正常工作，利用多周期延时算法得出从手下一时刻的理想运动点。

当|e(k)|>0.2°时，此时角度差超出机械臂校正能力范围，此状况是由主手出现急停避障运动。机械臂停止运动，保持当前位置，直到主手信号从新回到从手停止状态后，重新开始运动。

洋流反馈校正环节的特征是计算从手实际运动位置和主手相应时刻目标位置的差值，判读示意图如附图4所示，主要判断是否存在洋流干扰对机械臂从手运动造成影响，如果洋流影响超出了机械臂校正运动能力范围，认为洋流干扰影响过大，从手机械臂停止当前运动，直到机械臂主手从新回到机械臂从手停止位置，机械臂从手继续跟随主手运动，反之，如果洋流影响较小，影响处于机械臂运动能力范围内，机械臂根据主手目标位置进行恰当的位置校正，再进行延时环节控制，使得机械臂从手平稳运动。

假设第k步主手关节输出目标值x(k)与从手关节实际运动位置之间的误差为对该误差进行判断分析，根据误差范围的不同，决定从手之后如何运动。

当|e(k)|<0.02°时，校正后的从手输出值误差不大，无需校正，保持从手当前运动状态。

当0.02°<|e(k)|<0.2°时，校正后的从手输出值误差在机械臂校正范围能力内，对从手当前位置进行校正。

当|e(k)|>0.2°时，校正后从手输出值此时误差超出机械臂校正能力范围，此状况是由于洋流干扰过大造成。机械臂停止运动，保持当前位置，直到主手信号从新回到从手停止状态后，重新开始运动。

实施5，结合图5，水下机械臂是一个六输入六输出的***，控制***包括控制算法部分，运动学模型部分及动力学模型部分，设定七功能机械臂各个关节的运动轨迹，运动轨迹为连续的曲线，对其进行分段处理，每隔单位时间根据控制算法对控制信号进行采样，运用M文件编写控制算法。

根据延时控制算法，可以得出新的关节运动目标点。将新的运动目标点带入运动学模型中，并封装在M文件中，解算出各关节的运动轨迹。

动力学模型封装在相应的M文件中，得到各关节运动轨迹后，对各个关节进行动力学力矩分析，从而得出各个关节的相关控制力矩。

Simulink控制***框图如附图5所示。

实施6，结合附图6，举例进行验证，举例水下七功能机械臂的关节参数如表1所示，七功能水下机械臂结构简图如附图6所示。

表1七功能机械臂各杆参数

运用Pro/E工具对多功能水下机械臂进行模型搭建，从而解算出机械臂各连杆的重心向量、质量和转动惯性矩等参数，七功能机械臂各关节参数如下表2、3所示。

表2七功能机械臂各连杆质量及重心向量

	连杆质量(kg)	连杆重心向量(x，y，z)(m)
			关节1	2.096	(-0.003，-0.062，0.0063)
关节2	9.67	(0.219，-0.003，0.0449)
			关节3	2.814	(-0.002，-0.08，0.002)
关节4	6.092	(0.169，-0.005，0.045)
			关节5	0.098	(-0.0073，-0.0053，0.041)
关节6	0.103	(-0.44，0.049，0.11)

表3七功能机械臂各连杆惯性矩

假设六自由度水下机械臂各关节初始状态为:qz＝[0,0,0,0,0,0]

各关节目标状态为:

采用相同的一组随机数作为洋流干扰，洋流干扰如附图7所示。主手操作人员操作主手单关节，先慢后快的增大关节角度，突然急停变向，操作主手关节重新回到从手停止处后，继续操作关节角度增加，之后改变关节角度变化方向，操作关节角度匀速减小。

洋流干扰的单关节两周期延时控制仿真如附图8所示。

Claims (4)

1.一种主从式水下机械臂的延时控制方法，其特征在于，包括以下步骤：

(1)给定水下机械臂的结构参数，根据水下机械臂的运动学模型，对主从机械臂进行时延分析；

(2)采集水动力因素，根据水下机械臂动力学模型，验证机械臂性能是否满足模型需求；

(3)对主手动作信号进行延时处理，将前n个时刻关节控制信号缓存在水下控制器中，然后基于多次幂曲线拟合方法计算出从手n+1时刻的关节理想运动位置；

(4)分析机械臂主手是否存在急停变向紧急操作以及机械臂从手是否存在洋流干扰因素，通过延时控制环节和洋流反馈校正环节的判断，实现主从水下机械臂平稳连续控制；

(5)根据运动学模型、动力学模型以及延时控制算法建立控制***模型，并基于该模型进行仿真，验证延时控制方法的有效性及可靠性。

2.根据权利要求1所述的一种主从式水下机械臂的延时控制方法，其特征在于：步骤(2)所述的水下机械臂动力学模型在不考虑水动力因素时，是运用牛顿—欧拉方程对机械臂进行动力学建模的，其中水下机械臂在任一运动时刻，将机械臂各个关节角度变量q_i、速度变量加速度变量i＝1,...n作为已知条件，解算各关节的力矩变量Q_i，i＝1,...n，根据牛顿—欧拉法首先向前迭代计算机械臂的各连杆的速度和加速度，其中基座初始运动状态是确定的，如果基座固定，则基坐标角速度⁰ω₀＝0，角加速度根据牛顿—欧拉法向后迭代计算机械臂的各关节的力及力矩；初始条件是力矩力水下机械臂末端点可在机械臂工作空间内自由活动，则M_end、F_end为零；

(2.1)向前迭代解算机械臂各连杆的关节角度变量、速度变量：

${\mathrm{\&omega;}_{i+1}}_{i+1}=R_{i+1,i}({\mathrm{\&omega;}_i}_i+{\stackrel{\&CenterDot;}{q}}_{i+1}{e_i}_i)$

${\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_{i+1}}_{i+1}=R_{i+1,i}({\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_i}_i+{\stackrel{\&CenterDot;\&CenterDot;}{q}}_{i+1}{e_i}_i+{\stackrel{\&CenterDot;}{q}}_{i+1}{\mathrm{\&omega;}_i}_i\&times;{e_i}_i)$

${\stackrel{\&CenterDot;}{v}_{i+1}}_{i+1}=R_{i+1,i}{\stackrel{\&CenterDot;}{v}_i}_i+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_{i+1}}_{i+1}\&times;{l_{i+1}}_{i+1}+{\mathrm{\&omega;}_{i+1}}_{i+1}\&times;({\mathrm{\&omega;}_{i+1}}_{i+1}\&times;{l_{i+1}}_{i+1})$

ⁱv_ci＝ⁱv_i+ⁱω_i×ⁱρ_i

${\stackrel{\&CenterDot;}{v}_i}_{ci}={\stackrel{\&CenterDot;}{v}_i}_i+{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_i}_i\&times;{\mathrm{\&rho;}_i}_i+{\mathrm{\&omega;}_i}_i\&times;({\mathrm{\&omega;}_i}_i\&times;{\mathrm{\&rho;}_i}_i)$

${F_{i+1}}_{i+1}=m_{i+1}{\stackrel{\&CenterDot;}{v}_i}_{ci}$

${}^{i+1}{M}_{i+1}={I_c}_{i+1}{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}_{i+1}}_{i+1}+{\mathrm{\&omega;}_{i+1}}_{i+1}\&times;\left({I_c}_{i+1}{\mathrm{\&omega;}_{i+1}}_{i+1}\right)$

其中，ⁱ⁺¹ω_i+1为第i+1个关节的角速度；为第i+1个关节的角加速度；R_i+1，i为第i个关节到第i+1个关机的旋转矩阵；为第i+1个关节的线加速度；ⁱv_ci为第i个关节的质心处的线加速度；为第i+1个关节自身的角速度；ⁱe_i为第i个关节的旋转轴线；ⁱ⁺¹l_i+1为第i+1个连杆的长度矢量；ⁱρ_i为i坐标系到i连杆质心处的位置矢量；^cI_i+1为第i+1个关节到质心坐标原点处的惯性张量；m_i+1为第i+1个关节的质量；ⁱ⁺¹F_i+1为第i+1个关节的惯性力；ⁱ⁺¹M_i+1为第i+1个关节的惯性力矩；

(2.2)向后迭代解算机械臂关节的力F_i和力矩Q_i i＝n,n-1,...,1：

其中，ⁱh_i为i坐标系到i连杆受力点的位置矢量；为第i个关节的关节力；为第i个关节相对于i坐标系的关节力矩；Q^M _i为第i个关节相对于i-1坐标系的关节力矩；

当机械臂连杆关节的重量作为考虑因素时，设固定的机器人基座受到的支撑作用相当向上的重力加速度g，考虑水动力因素时，选择瑞雷耗散函数描述粘性阻尼和流体阻力下物体的运动情况：

在拉格朗日动力学算法中引入瑞雷耗散函数：

$\mathrm{\&phi;}=\frac{1}{2}\underset{i=1}{\overset{N}{\&Sigma;}}(k_{ix}\stackrel{\&CenterDot;}{{x_i}^2}+k_{iy}{y_i}^2+k_{iz}{z_i}^2),$

可得带有瑞雷耗散函数的拉格朗日方程：

$\frac{d}{dt}\frac{\&part;T}{\&part;{\tilde{q}}_s}-\frac{\&part;T}{\&part;q_s}=-\frac{\&part;\mathrm{\&phi;}}{\&part;q_s}+Q_s,(s=1,2,\mathrm{3.....}n),$

其中q_s为广义位置向量，为相应的速度向量，T为拉格朗日函数；

由于牛顿—欧拉方法求出不含水动力变量的力矩

则

$Q_s=\mathrm{\&tau;}+\frac{\&part;\mathrm{\&phi;}}{\&part;q_s},(s=1,2,\mathrm{3.....}n)$

对六自由度水下机械臂推导分析，各关节相对于前一关节的速度表示为^i-1v_i(i＝1,2,3.....6)，根据刚体定轴转动定理，得出各关节平动速度，进而得到各关节的质心点的运动绝对速度⁰v_i(i＝1,2,3.....6)；

求解机械臂各个关节的耗散函数φ_i(i＝1，2…..6)，

${\mathrm{\&phi;}}_i=\frac{u^0{v_i}^2}{2}=\frac{u}{2}{\&Integral;}_0^{l_i}v_0{{}_i}^2{\mathrm{dx}}_i$

得到水下机械臂***的耗散函数为φ＝φ₁+φ₂+φ₃+φ₄+φ₅+φ₆，将瑞雷耗散函数带入拉格朗日方程进行推导，求导瑞雷耗散函数φ的广义速率参数代入方程即可求得水动力阻尼系数。

3.根据权利要求1所述的一种主从式水下机械臂的延时控制方法，其特征在于：步骤(3)所述的幂曲线拟合方法包括：曲线拟合是通过实验获得有限对测试数据(x_i,y_i)，利用这些数据来求取近似函数y＝f(x)的，其中x为输出量，y为被测物理量，延时控制中时间为输出量，关节角度为被测物理量；将曲线拟合法应用到主从水下机械臂延时控制中，延时环节时间设置为两个参数周期，将这种方法称为两周期延时，加上由于485总线传输造成的信号延时，从手的滞后主手三个周期运动，在主手运动i+3时刻，由于485通信信号的延时，i+3时刻水下控制器刚接收到主手i+2时刻位置，控制器得知三个时刻的位置点，即i+3时刻的从手位置、i+1时刻的主手控制信号、i+2时刻的主手控制信号，根据关节位置时间信息，提前判断出从手的运动变化趋势，从而利用两周期延时控制方法，计算出下一时刻从手理想运动点；

其中主手关节i在t_k-2时控制角度值为θ_k-2，在t_k-1时控制角度值为θ_k-1，从手关节在t_k时刻实际角度值为θ_k，得到坐标分别为(t_k-2,θ_k-2)、(t_k-1,θ_k-1)和(t_k,θ_k)，根据关节曲线运动拟合方程，得到t_k+1时刻的角度预测值为θ_k+1：

θ_k+1＝θ_k+v(k)×T+aT²/2

其中，a为关节的角加速度，T为传输周期0.1s；v(k)为k时刻关节i转动速度；

由匀加速度方程得到：

$\left\{\begin{array}{c}v(k-1)=({\mathrm{\&theta;}}_{k-1}-{\mathrm{\&theta;}}_{k-2})/T\\ v\left(k\right)=({\mathrm{\&theta;}}_k-{\mathrm{\&theta;}}_{k-1})/T\\ a=(v\left(k\right)-v\left(k-1\right))/T\end{array}\right.$

进行计算，得到t_k+1时刻的从手关节角度的理想值

${\mathrm{\&theta;}}_{k+1}=\frac{5}{2}{\mathrm{\&theta;}}_k-2{\mathrm{\&theta;}}_{k-1}+\frac{1}{2}{\mathrm{\&theta;}}_{k-2}$

最终由当前时刻从手关节角度和延时两个时刻的主手关节角度来计算下一时刻从手关节速度、角速度，从而得到理想的运动角度。

4.根据权利要求1所述的一种主从式水下机械臂的延时控制方法，其特征在于：步骤(4)所述的延时控制环节包括：对主手运动曲线进行分析，从而判断机械臂主手是否存在变向，加速，减速的突变运动环节，进而对从手进行运动控制；

其中第k步主手输出目标值x(k)与第k+1步主手输出目标值x(k+1)之间的角度差为e(k)＝x(k+1)-x(k)，对该角度差进行判断分析，根据角度差范围的不同，控制从手下一步如何运动：

当|e(k)|<0.2°时，主手控制变化，处于从手运动能力范围内，不存在紧急操作，延时环节正常工作，利用多周期延时算法得出从手下一时刻的理想运动点；

当|e(k)|>0.2°时，此时角度差超出机械臂校正能力范围，主手出现急停避障运动；机械臂停止运动，保持当前位置，直到主手信号从新回到从手停止状态后，重新开始运动。

洋流反馈校正环节主要判断是否存在洋流干扰对机械臂从手运动造成影响，如果洋流影响超出了机械臂校正运动能力范围，认为洋流干扰影响过大，从手机械臂停止当前运动，直到机械臂主手从新回到机械臂从手停止位置，机械臂从手继续跟随主手运动；如果洋流影响较小，影响处于机械臂运动能力范围内，机械臂根据主手目标位置进行恰当的位置校正，再进行延时环节控制，使得机械臂从手平稳运动；

其中第k步主手关节输出目标值x(k)与从手关节实际运动位置之间的误差为对该误差进行判断分析，根据误差范围的不同，决定从手之后如何运动；

当|e(k)|<0.02°时，校正后的从手输出值无需校正，保持从手当前运动状态；

当0.02°<|e(k)|<0.2°时，校正后的从手输出值误差在机械臂校正范围能力内，对从手当前位置进行校正；

当|e(k)|>0.2°时，校正后从手输出值误差超出机械臂校正能力范围，机械臂停止运动，保持当前位置，直到主手信号从新回到从手停止状态后，重新开始运动。