CN111687835A - 水下机械手冗余机械臂逆优先级阻抗控制***及控制方法 - Google Patents

Abstract

本发明公开了水下机械手冗余机械臂逆优先级阻抗控制***及控制方法。属于机械手冗余机械臂逆优先级阻抗控制技术领域，机械手有多个自由度，灵活性好，能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务。建立冗余机械臂运动学模型，通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略；建立奇异鲁棒解逆运动学分析模型；建立多任务冗余机械臂的逆优先控制策略；对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理；建立机械手的逆优先力控制策略；采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算。

Description

水下机械手冗余机械臂逆优先级阻抗控制***及控制方法

技术领域

本发明涉及机械手冗余机械臂逆优先级阻抗控制技术领域，尤其涉及水下机械手冗余机械臂逆优先级阻抗控制***及控制方法。

背景技术

目前工业机器人采用的控制方法是把机械手上每一个关节都当作一个单独的伺服机构，即每个轴对应一个伺服器，每个伺服器通过总线控制，由控制器统一控制并协调工作；

六个自由度的机械臂是具有完成空间定位能力最小自由度数的机械臂，多于六个自由度的机械臂统一称为冗余机械臂；

现在机械手的机械臂阻抗控制方法在不同层次结构下不能实现期望的阻抗控制任务，因此，设计一种能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务的方法显得非常必要。

发明内容

本发明是为了解决现有机械臂阻抗控制方法在不同层次结构下不能实现期望的阻抗控制任务的不足，提供一种一是机械手有多个自由度，灵活性好；二是能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务的水下机械手冗余机械臂逆优先级阻抗控制***及控制方法。

以上技术问题是通过下列技术方案解决的：

一种水下机械手冗余机械臂逆优先级阻抗控制方法，包括如下步骤：

步骤1、建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略；

步骤2、建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略；

步骤3、建立奇异鲁棒解逆运动学分析模型；

步骤4、建立多任务冗余机械臂的逆优先控制策略；

步骤5、对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理；

步骤6、建立机械手的逆优先力控制策略；

步骤7、采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证；

步骤8、将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架。

本方案冗余机械臂在关节空间中的运动是按照相反的顺序导出的；然后将笛卡尔阻抗控制与逆优先阻抗控制相结合，解决了逆分层阻抗控制问题，将笛卡尔阻抗控制行为分为高优先阻抗控制和低优先阻抗控制。其中高优先阻抗控制任务不会干扰低优先阻抗控制任务，关节空间中的运动按相反的顺序会受到影响，要在相应的投影算子中工作；最后实现了高优先阻抗控制任务，避免了低优先阻抗控制任务中可能出现的奇异性引起的变形。因此，所提出的逆优先阻抗控制方法可使冗余机械臂在适当的层次结构下实现期望的阻抗控制任务。

作为优选，建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略实现过程如下：

定义末端执行器在笛卡尔空间的位姿、速度分别为x、

其关节空间的角位置、角速度分别为q、

J为n自由度机器人的雅可比矩阵，其中x∈Rⁿ，

J∈R^m-n；冗余自由度机械臂的正运动学方程可用下式描述：

式(1)也被称为机械臂运动学速度模型；

考虑到最小二乘法的解，最优问题可列为：

式(1)的解可通过寻找最佳

来解决；

因此，式(1)的伪逆解可表示为：

式中J⁺——雅可比矩阵的伪逆

I——单位矩阵

——任意零空间矢量

——最小范数解，定义了手的动作

——齐次解，在末端不产生动作

式(4)代表了末端执行器的位置和姿态控制；在式(4)中加入任意残差，可得到包含零空间的一般表达式；利用上述方程可在零向量上实现多任务优化；

然而，上述方程忽略了雅可比矩阵的病态；正则化方程可通过添加额外的正则化值来修改，

其中λ≥0是加权矩阵，

为加权系数，并且满足

上述方程的解就可表示为：

式(7)也被称为冗余机械臂运动学模型；

冗余机械臂零空间向量的位置相关标量指数的关节限制梯度方向的关节限制函数为：

作为优选，建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略如下：

在雅可比矩阵的冗余机械臂求解中，优化任务是在主任务的零空间中实现的；反向任务运动学是在正向任务运动学基础上建立的：

其中

和

表示task1和task2

从表达式(5)中得出冗余机械臂的逆运动学方程为：

Task1作为主任务，task2作为辅助任务；也就是说，task2

是在task1

的零空间中实现的；冗余机械臂的最终逆运动学表达式如下所示：

其中

是投影矩阵，它给出了次要任务到主要任务的适用范围；

和

是所需的命令速度；

是主要任务，

是次要任务；

如果两个相关任务是相互依赖的，则相应的雅可比矩阵是奇异的；如果任务雅可比矩阵是奇异的，则相应的任务是不满足的；在这种情况下，雅可比相关矩阵将是奇点，定义为算法奇点；

也就是说，如果

其中ρ(·)是矩阵的秩；

很明显，算法的奇异性是由次要任务和主要任务之间的任务冲突引起的；此外，基于任务优先级的冗余机械臂逆运动学旨在提供更好的控制主要任务的有效性；

因此，让位置控制方向作为主要任务，从而使位置保证控制方向任务的准确性；然后建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略方程：

作为优选，建立奇异鲁棒解逆运动学分析模型如下：

基于雅可比伪逆解会出现运动学奇异性，这是由次矩阵引起的；对于运动奇异性问题，应该还要给出DLS(阻尼最小二乘)解；

对DLS解的成本函数可修改为：

因此，上述方程的奇异鲁棒伪逆解可表示为：

式(15)即为奇异鲁棒解逆运动学分析模型，设λ＝η²I，上述DLS解等价于附加正则化解，标量值η平衡了任务精度和奇异性；

对于雅可比矩阵伪逆解的计算，可给出雅可比矩阵的奇异值SVD分解形式

J＝U∑V^T (16)

其中U∈R^m×m，V∈R^n×n，∑∈R^m×n，U是由列向量u_i组成的一元矩阵，V是由列向量v_i组成的一元矩阵，∑是m×n对角矩阵的块矩阵，该对角矩阵包含J的奇异值σ_i≥0以递减顺序包含n-m个零列向量；

其中，r≤m是矩阵J的秩；

对于运动奇异性，参考计算伪逆解时需要的奇异值分解SVD，大的产生的关节速度是由于最小的奇异值迅速接近0，如下式：

因子λ₀将影响奇异性，λ₀值越高，阻尼越大，联合速度就越接近奇异点；此外，定义可变阻尼因子的策略也不同；我们可得到

从上式中，我们可看出参数δ＞0监视最小的奇异值。

作为优选，建立多任务冗余机械臂的逆优先控制策略如下；

引入逆优先投影矩阵

该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间，所以得出

其中

是与j-th任务线性无关的i-th任务的所有组件相关联的雅可比矩阵；

所以，优先级推导公式如下：

在上述推导中，k＝l，l-1，…，1；初始值

为了给出线性无关雅可比矩阵J的一般计算形式，定义逆增广Jacobian矩阵为：

就有了

其中

表示

的行；

于是乎，

的伪逆解就可表示为：

和

其中，T_k表示矩阵

的扩充；

最终的反向优先级投影可写为：

于是，我们就可得出伪逆解的表达式：

建立多任务冗余机械臂的逆优先控制策略方程如下：

作为优选，对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理如下：

对于六自由度或七自由度冗余机械臂来说，没有足够的六自由度DOF来完成多个层次的任务；实行双任务优先控制是必要的；也就是说，机械手的运动控制是主要任务和次要任务；

具有主要任务和次要任务的冗余机械臂的反向控制方程如下

上面的公式与前面的表达式(11)有很大的不同，但是算法框架是相似的；在上面的方程中，

是次要任务，

是主任务；主任务在主任务的指定零空间中实现；反向优先级的核心点是投影矩阵

的计算；

的表达式如公式(30)：

利用以前公式(22)-(28)中类似的导子，可得到简化后具有主要任务和次要任务的冗余机械臂的反向控制方程：

作为优选，建立机械手的逆优先力控制策略如下：

力控制空间中机械手的动力学可写成：

其中X是笛卡尔空间中的位置，M(X)是惯性矩阵，

是非线性力，F是输入控制力，F_e是接触力；

此外，基于雅可比矩阵的变换可得到输入关节力矩

τ＝J^T(q)F (33)

操纵器在力控制空间中的期望运动方程可定义如下：

其中M_d和B_d是惯性和阻尼矩阵；F_d是指令力，F_e是接触力；

因此，环境和操纵器响应之间的关系可写成

上述两个方程的组合如下

从上面的方程可看出，如果M_e、B_e和K_e已知，则M_d和B_d的调整将影响***响应；

力控制使机械手能够与环境或人类相互作用；另外，在某些情况下，没有必要实现全方位的力控制，也没有必要保证全方位的力控制，也就是说，有时我们只是想保证某个方向的力跟踪控制精度；

因此有必要对机械手进行分级力控制；也就是说，有必要给出一个新的层次力控制框架；从上面的方程我们可得到期望的层次力控制关系如下

所以这两个方程的积分公式可写成

如果机械手末端执行器能够跟踪期望的笛卡尔速度为

和

则可实现机械手的精确力控制；笛卡尔速度与关节速度的关系应借鉴逆优先控制；因此，可得到机械手的逆优先力控制策略的方程：

上述方程所要求的关节速度将保证机械手的力控制；值得一提的是，上述力控制律只是速度级控制律，它依赖于内速度环控制；如果内位置控制效果良好，则可实现精确的力控制；由于内速度环控制可实现低频位置跟踪，所以外力环可实现低频力跟踪。

作为优选，采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证的实现方式如下：

当机械手实施力控制时，机械手在一定程度上起到了发起者的作用，也就是说，机械手已经做好了响应外部环境的准备；当机械臂作为阻抗控制模型工作时，机械臂会被动地响应外力；

外力与关节加速度的对应阻抗关系可表示为

参考速度可表示为

因此，机械手的逆优先阻抗控制保证的表达式为：

作为优选，将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架实现方式如下：

混合阻抗应用就是上述两种策略的结合，即笛卡尔任务可分为两种情况：第一种是位置控制子空间，阻抗控制是在该子空间中实现的；第二个是力控制子空间，力控制在该子空间中实现；

因此选择一个选择矩阵；外力与位置响应的关系如下

所以期望速度的简化形式可表示为

然后我们得到了基于反向优先级的解决方案

考虑到n层任务，相应的阻抗控制任务也属于n层框架，因此，机械手速度级逆优先阻抗控制的总体框架表达式如下

表达式(52)解决了将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算的机械手逆优先混合阻抗控制中去，能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务。

水下机械手冗余机械臂逆优先级阻抗控制***，包括机械手和控制机械手的控制台；所述机械手包括机械臂、安装座、竖直柱、输出抓手和竖直气缸；

机械臂包括竖直升降移动台、臂段一、臂段二、臂段三和臂段四；

在竖直柱的左表面上设有竖直轨道，竖直升降移动台上下竖直滑动设置在竖直轨道上；竖直柱的下端固定连接在安装座的上表面上，竖直气缸的缸座固定连接在位于竖直轨道左方的安装座的上表面上，竖直气缸的伸缩杆竖直朝上布置，竖直气缸的伸缩杆上端固定连接在竖直升降移动台的下表面上；竖直升降移动台能在竖直气缸的伸缩杆的带动下可沿着竖直轨道上下运动，形成第一自由度；

臂段一包括A1段管和伸缩连接在A1段管的左管口内的A2段管，在A1段管内的右端固定设有伸缩杆水平朝左布置的一号气缸，一号气缸的伸缩杆固定连接在A2段管的右端；

在竖直升降移动台的左端设有由一台一号减速电机驱动的一号水平旋转轴，A1段管的右端固定连接在一号水平旋转轴上，使臂段一能水平转动形成第二自由度；在一号水平旋转轴上还设有能控制一号水平旋转轴转动的一号电磁刹车；

在A2段管的左端设有由一台二号减速电机驱动的二号水平旋转轴，臂段二的右端固定连接在二号水平旋转轴上，使臂段二能水平转动形成第三自由度；在二号水平旋转轴上还设有能控制二号水平旋转轴转动的二号电磁刹车；

在臂段二的左端设有由一台三号减速电机驱动的三号水平旋转轴，臂段三的右端固定连接在三号水平旋转轴上，使臂段三能水平转动形成第四自由度；在三号水平旋转轴上还设有能控制三号水平旋转轴转动的三号电磁刹车；

在臂段三的左端设有由一台四号减速电机驱动的能在左右竖直面上转动的一号横向竖直旋转轴，臂段四的右端固定连接在一号横向竖直旋转轴上，使臂段四能在左右竖直面上竖直转动形成第五自由度；在一号横向竖直旋转轴上还设有能控制一号横向竖直旋转轴转动的四号电磁刹车；

在臂段四的左端设有由一台五号减速电机驱动的能在前后竖直面上转动的一号纵向竖直旋转轴，输出抓手的右端固定连接在一号纵向竖直旋转轴上，使输出抓手的右端能在前后竖直面上竖直转动形成第六自由度；在一号纵向竖直旋转轴上还设有能控制一号纵向竖直旋转轴转动的五号电磁刹车；

A2段管能在一号气缸的伸缩杆的带动下在A1段管内左右伸缩移动形成第七自由度；

一号电磁刹车的控制端、二号电磁刹车的控制端、三号电磁刹车的控制端、四号电磁刹车的控制端、五号电磁刹车的控制端、一号减速电机的控制端、二号减速电机的控制端、三号减速电机的控制端、四号减速电机的控制端、五号减速电机的控制端、一号气缸的控制端和竖直气缸的控制端分别控制连接在控制台上。机械手均采用防水设置。

本发明能够达到如下效果：

本发明机械手有多个自由度，灵活性好，能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务。

附图说明

图1为本发明力控制的动力学方案示意图。

图2为本发明阻抗控制的动力学方案示意图。

图3为本发明混合阻抗控制的动力学方案示意图。

图4为本发明实施例的一种七自由度机械手连接结构示意图。

图5为本发明实施例的一种电路原理连接结构示意框图。

具体实施方式

下面结合附图与实施例对本发明作进一步的说明。

实施例，水下机械手冗余机械臂逆优先级阻抗控制方法，参见图1-3所示。包括如下步骤：

步骤1、建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略实现过程如下：

定义末端执行器在笛卡尔空间的位姿、速度分别为x、

其关节空间的角位置、角速度分别为q、

J为n自由度机器人的雅可比矩阵，其中x∈Rⁿ，

J∈R^m-n；冗余自由度机械臂的正运动学方程可用下式描述：

式(1)也被称为机械臂运动学速度模型；

考虑到最小二乘法的解，最优问题可列为：

式(1)的解可通过寻找最佳

来解决；

因此，式(1)的伪逆解可表示为：

式中J⁺——雅可比矩阵的伪逆

I——单位矩阵

——任意零空间矢量

——最小范数解，定义了手的动作

——齐次解，在末端不产生动作

式(4)代表了末端执行器的位置和姿态控制；在式(4)中加入任意残差，可得到包含零空间的一般表达式；利用上述方程可在零向量上实现多任务优化；

然而，上述方程忽略了雅可比矩阵的病态；正则化方程可通过添加额外的正则化值来修改，

其中λ≥0是加权矩阵，

为加权系数，并且满足

上述方程的解就可表示为：

式(7)也被称为冗余机械臂运动学模型；

冗余机械臂零空间向量的位置相关标量指数的关节限制梯度方向的关节限制函数为：

步骤2、建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略如下：

在雅可比矩阵的冗余机械臂求解中，优化任务是在主任务的零空间中实现的；反向任务运动学是在正向任务运动学基础上建立的：

其中

和

表示task1和task2

从表达式(5)中得出冗余机械臂的逆运动学方程为：

Task1作为主任务，task2作为辅助任务；也就是说，task2

是在task1

的零空间中实现的；冗余机械臂的最终逆运动学表达式如下所示：

其中

是投影矩阵，它给出了次要任务到主要任务的适用范围；

和

是所需的命令速度；

是主要任务，

是次要任务；

如果两个相关任务是相互依赖的，则相应的雅可比矩阵是奇异的；如果任务雅可比矩阵是奇异的，则相应的任务是不满足的；在这种情况下，雅可比相关矩阵将是奇点，定义为算法奇点；

也就是说，如果

其中ρ(·)是矩阵的秩；

很明显，算法的奇异性是由次要任务和主要任务之间的任务冲突引起的；此外，基于任务优先级的冗余机械臂逆运动学旨在提供更好的控制主要任务的有效性；

因此，让位置控制方向作为主要任务，从而使位置保证控制方向任务的准确性；然后建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略方程：

步骤3、建立奇异鲁棒解逆运动学分析模型如下：

基于雅可比伪逆解会出现运动学奇异性，这是由次矩阵引起的；对于运动奇异性问题，应该还要给出DLS(阻尼最小二乘)解；

对DLS解的成本函数可修改为：

因此，上述方程的奇异鲁棒伪逆解可表示为：

式(15)即为奇异鲁棒解逆运动学分析模型，设λ＝η²I，上述DLS解等价于附加正则化解，标量值η平衡了任务精度和奇异性；

对于雅可比矩阵伪逆解的计算，可给出雅可比矩阵的奇异值SVD分解形式

J＝U∑V^T (16)

其中U∈R^m×m，V∈R^n×n，∑∈R^m×n，U是由列向量u_i组成的一元矩阵，V是由列向量v_i组成的一元矩阵，∑是m×n对角矩阵的块矩阵，该对角矩阵包含J的奇异值σ_i≥0以递减顺序包含n-m个零列向量；

其中，r≤m是矩阵J的秩；

对于运动奇异性，参考计算伪逆解时需要的奇异值分解SVD，大的产生的关节速度是由于最小的奇异值迅速接近0，如下式：

因子λ₀将影响奇异性，λ₀值越高，阻尼越大，联合速度就越接近奇异点；此外，定义可变阻尼因子的策略也不同；我们可得到

从上式中，我们可看出参数δ＞0监视最小的奇异值。

步骤4、建立多任务冗余机械臂的逆优先控制策略如下；

引入逆优先投影矩阵

该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间，所以得出

其中J_i|j是与j-th任务线性无关的i-th任务的所有组件相关联的雅可比矩阵；

所以，优先级推导公式如下：

在上述推导中，k＝l，l-1，…，1；初始值

为了给出线性无关雅可比矩阵J的一般计算形式，定义逆增广Jacobian矩阵为：

就有了

其中

表示

的行；

于是乎，

的伪逆解就可表示为：

和

其中，T_k表示矩阵

的扩充；

最终的反向优先级投影可写为：

于是，我们就可得出伪逆解的表达式：

建立多任务冗余机械臂的逆优先控制策略方程如下：

步骤5、对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理如下：

对于六自由度或七自由度冗余机械臂来说，没有足够的六自由度DOF来完成多个层次的任务；实行双任务优先控制是必要的；也就是说，机械手的运动控制是主要任务和次要任务；

具有主要任务和次要任务的冗余机械臂的反向控制方程如下

上面的公式与前面的表达式(11)有很大的不同，但是算法框架是相似的；在上面的方程中，

是次要任务，

是主任务；主任务在主任务的指定零空间中实现；反向优先级的核心点是投影矩阵

的计算；

的表达式如公式(30)：

利用以前公式(22)-(28)中类似的导子，可得到简化后具有主要任务和次要任务的冗余机械臂的反向控制方程：

步骤6、建立机械手的逆优先力控制策略如下：

力控制空间中机械手的动力学可写成：

其中X是笛卡尔空间中的位置，M(X)是惯性矩阵，

是非线性力，F是输入控制力，F_e是接触力；

此外，基于雅可比矩阵的变换可得到输入关节力矩

τ＝J^T(q)F (33)

操纵器在力控制空间中的期望运动方程可定义如下：

其中M_d和B_d是惯性和阻尼矩阵；F_d是指令力，F_e是接触力；

力控制的动力学方案如图1所示；

因此，环境和操纵器响应之间的关系可写成

上述两个方程的组合如下

从上面的方程可看出，如果M_e、B_e和K_e已知，则M_d和B_d的调整将影响***响应；

力控制使机械手能够与环境或人类相互作用；另外，在某些情况下，没有必要实现全方位的力控制，也没有必要保证全方位的力控制，也就是说，有时我们只是想保证某个方向的力跟踪控制精度；

例如，当机械手与刨床相互作用时，只需在垂直方向保持精确的力跟踪控制，而另一个方向不需要精确的力跟踪控制；在其他情况下，位置方向力控制比姿态方向力控制更为重要；

因此有必要对机械手进行分级力控制；也就是说，有必要给出一个新的层次力控制框架；从上面的方程我们可得到期望的层次力控制关系如下

所以这两个方程的积分公式可写成

如果机械手末端执行器能够跟踪期望的笛卡尔速度为

和

则可实现机械手的精确力控制；笛卡尔速度与关节速度的关系应借鉴逆优先控制；因此，可得到机械手的逆优先力控制策略的方程：

上述方程所要求的关节速度将保证机械手的力控制；值得一提的是，上述力控制律只是速度级控制律，它依赖于内速度环控制；如果内位置控制效果良好，则可实现精确的力控制；由于内速度环控制可实现低频位置跟踪，所以外力环可实现低频力跟踪。

步骤7、采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证的实现方式如下：

当机械手实施力控制时，机械手在一定程度上起到了发起者的作用，也就是说，机械手已经做好了响应外部环境的准备；当机械臂

作为阻抗控制模型工作时，机械臂会被动地响应外力；阻抗控制的动力学方案如图2所示；

外力与关节加速度的对应阻抗关系可表示为

参考速度可表示为

因此，机械手的逆优先阻抗控制保证的表达式为：

步骤8、将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架实现方式如下：

混合阻抗应用就是上述两种策略的结合，即笛卡尔任务可分为两种情况：第一种是位置控制子空间，阻抗控制是在该子空间中实现的；第二个是力控制子空间，力控制在该子空间中实现；

因此选择一个选择矩阵；外力与位置响应的关系如下

所以期望速度的简化形式可表示为

然后我们得到了基于反向优先级的解决方案

混合阻抗控制的动力学方案图3所示；

考虑到n层任务，相应的阻抗控制任务也属于n层框架，因此，机械手速度级逆优先阻抗控制的总体框架表达式如下

表达式(52)解决了将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算的机械手逆优先混合阻抗控制中去；能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务。

水下机械手冗余机械臂逆优先级阻抗控制***，参见图4、图5所示，包括机械手和控制机械手的控制台S31；所述机械手包括机械臂、安装座S1、竖直柱S2、输出抓手S12和竖直气缸S23；

机械臂包括竖直升降移动台S3、臂段一S6、臂段二S7、臂段三S8和臂段四S10；

在竖直柱的左表面上设有竖直轨道S24，竖直升降移动台上下竖直滑动设置在竖直轨道上；竖直柱的下端固定连接在安装座的上表面上，竖直气缸的缸座S21固定连接在位于竖直轨道左方的安装座的上表面上，竖直气缸的伸缩杆S22竖直朝上布置，竖直气缸的伸缩杆上端固定连接在竖直升降移动台的下表面上；竖直升降移动台能在竖直气缸的伸缩杆的带动下可沿着竖直轨道上下运动，形成第一自由度；

臂段一包括A1段管S13和伸缩连接在A1段管的左管口内的A2段管S14，在A1段管内的右端固定设有伸缩杆水平朝左布置的一号气缸S25，一号气缸的伸缩杆固定连接在A2段管的右端；

在竖直升降移动台的左端设有由一台一号减速电机S26驱动的一号水平旋转轴S4，A1段管的右端固定连接在一号水平旋转轴上，使臂段一能水平转动形成第二自由度；在一号水平旋转轴上还设有能控制一号水平旋转轴转动的一号电磁刹车S32；

在A2段管的左端设有由一台二号减速电机S27驱动的二号水平旋转轴S15，臂段二的右端固定连接在二号水平旋转轴上，使臂段二能水平转动形成第三自由度；在二号水平旋转轴上还设有能控制二号水平旋转轴转动的二号电磁刹车S33；

在臂段二的左端设有由一台三号减速电机S28驱动的三号水平旋转轴S18，臂段三的右端固定连接在三号水平旋转轴上，使臂段三能水平转动形成第四自由度；在三号水平旋转轴上还设有能控制三号水平旋转轴转动的三号电磁刹车S34；

在臂段三的左端设有由一台四号减速电机S29驱动的能在左右竖直面上转动的一号横向竖直旋转轴S9，臂段四的右端固定连接在一号横向竖直旋转轴上，使臂段四能在左右竖直面上竖直转动形成第五自由度；在一号横向竖直旋转轴上还设有能控制一号横向竖直旋转轴转动的四号电磁刹车S35；

在臂段四的左端设有由一台五号减速电机S30驱动的能在前后竖直面上转动的一号纵向竖直旋转轴S11，输出抓手的右端固定连接在一号纵向竖直旋转轴上，使输出抓手的右端能在前后竖直面上竖直转动形成第六自由度；在一号纵向竖直旋转轴上还设有能控制一号纵向竖直旋转轴转动的五号电磁刹车S36；

A2段管能在一号气缸的伸缩杆的带动下在A1段管内左右伸缩移动形成第七自由度；

一号电磁刹车的控制端、二号电磁刹车的控制端、三号电磁刹车的控制端、四号电磁刹车的控制端、五号电磁刹车的控制端、一号减速电机的控制端、二号减速电机的控制端、三号减速电机的控制端、四号减速电机的控制端、五号减速电机的控制端、一号气缸的控制端和竖直气缸的控制端分别控制连接在控制台上。机械手均采用防水设置。第七自由度能让机械臂自由伸长，大大增加了作业范围和灵活性。

由于本发明有的机械手有七个自由度，灵活性好，可靠性高，易于完成控制任务。

Claims (10)

1.一种水下机械手冗余机械臂逆优先级阻抗控制方法，其特征在于，包括如下步骤：

步骤1、建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略；

步骤2、建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略；

步骤3、建立奇异鲁棒解逆运动学分析模型；

步骤4、建立多任务冗余机械臂的逆优先控制策略；

步骤5、对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理；

步骤6、建立机械手的逆优先力控制策略；

步骤7、采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证；

步骤8、将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架。

2.根据权利要求1所述的水下机械手冗余机械臂逆优先级阻抗控制方法，其特征在于，建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略实现过程如下：

定义末端执行器在笛卡尔空间的位姿、速度分别为x、

其关节空间的角位置、角速度分别为q、

J为n自由度机器人的雅可比矩阵，其中x∈Rⁿ，

J∈R^m-n；冗余自由度机械臂的正运动学方程可用下式描述：

式(1)也被称为机械臂运动学速度模型；

考虑到最小二乘法的解，最优问题可列为：

式(1)的解可通过寻找最佳

来解决；

因此，式(1)的伪逆解可表示为：

式中J⁺——雅可比矩阵的伪逆

I——单位矩阵

——任意零空间矢量

——最小范数解，定义了手的动作

——齐次解，在末端不产生动作

式(4)代表了末端执行器的位置和姿态控制；在式(4)中加入任意残差，可得到包含零空间的一般表达式；利用上述方程可在零向量上实现多任务优化；

然而，上述方程忽略了雅可比矩阵的病态；正则化方程可通过添加额外的正则化值来修改，

其中λ≥0是加权矩阵，

为加权系数，并且满足

上述方程的解就可表示为：

式(7)也被称为冗余机械臂运动学模型；

冗余机械臂零空间向量的位置相关标量指数的关节限制梯度方向的关节限制函数为：

3.根据权利要求2所述的水下机械手冗余机械臂逆优先级阻抗控制方法，其特征在于，建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略如下：

在雅可比矩阵的冗余机械臂求解中，优化任务是在主任务的零空间中实现的；反向任务运动学是在正向任务运动学基础上建立的：

其中

和

表示task1和task2

从表达式(5)中得出冗余机械臂的逆运动学方程为：

Task1作为主任务，task2作为辅助任务；也就是说，task2

是在task1

的零空间中实现的；冗余机械臂的最终逆运动学表达式如下所示：

其中

是投影矩阵，它给出了次要任务到主要任务的适用范围；

和

是所需的命令速度；

是主要任务，

是次要任务；

如果两个相关任务是相互依赖的，则相应的雅可比矩阵是奇异的；如果任务雅可比矩阵是奇异的，则相应的任务是不满足的；在这种情况下，雅可比相关矩阵将是奇点，定义为算法奇点；

也就是说，如果

其中ρ(·)是矩阵的秩；

很明显，算法的奇异性是由次要任务和主要任务之间的任务冲突引起的；此外，基于任务优先级的冗余机械臂逆运动学旨在提供更好的控制主要任务的有效性；

因此，让位置控制方向作为主要任务，从而使位置保证控制方向任务的准确性；然后建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略方程：

4.根据权利要求3所述的水下机械手冗余机械臂逆优先级阻抗控制方法，其特征在于，建立奇异鲁棒解逆运动学分析模型如下：

基于雅可比伪逆解会出现运动学奇异性，这是由次矩阵引起的；对于运动奇异性问题，应该还要给出DLS(阻尼最小二乘)解；

对DLS解的成本函数可修改为：

因此，上述方程的奇异鲁棒伪逆解可表示为：

式(15)即为奇异鲁棒解逆运动学分析模型，设λ＝η²I，上述DLS解等价于附加正则化解，标量值η平衡了任务精度和奇异性；

对于雅可比矩阵伪逆解的计算，可给出雅可比矩阵的奇异值SVD分解形式

J＝U∑V^T (16)

其中U∈R^m×m，V∈R^n×n，∑∈R^m×n，U是由列向量u_i组成的一元矩阵，V是由列向量v_i组成的一元矩阵，∑是m×n对角矩阵的块矩阵，该对角矩阵包含J的奇异值σ_i≥0以递减顺序包含n-m个零列向量；

其中，r≤m是矩阵J的秩；

对于运动奇异性，参考计算伪逆解时需要的奇异值分解SVD，大的产生的关节速度是由于最小的奇异值迅速接近0，如下式：

因子λ₀将影响奇异性，λ₀值越高，阻尼越大，联合速度就越接近奇异点；此外，定义可变阻尼因子的策略也不同；我们可得到

从上式中，我们可看出参数δ＞0监视最小的奇异值。

5.根据权利要求4所述的水下机械手冗余机械臂逆优先级阻抗控制方法，其特征在于，建立多任务冗余机械臂的逆优先控制策略如下；

引入逆优先投影矩阵

该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间，所以得出

其中J_ij是与j-th任务线性无关的i-th任务的所有组件相关联的雅可比矩阵；

所以，优先级推导公式如下：

在上述推导中，k＝l，l-1，…，1；初始值

为了给出线性无关雅可比矩阵J的一般计算形式，定义逆增广Jacobian矩阵为：

就有了

其中

表示

的行；

于是乎，

的伪逆解就可表示为：

和

其中，T_k表示矩阵

的扩充；

最终的反向优先级投影可写为：

于是，我们就可得出伪逆解的表达式：

建立多任务冗余机械臂的逆优先控制策略方程如下：

6.根据权利要求5所述的水下机械手冗余机械臂逆优先级阻抗控制方法，其特征在于，对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理如下：

对于六自由度或七自由度冗余机械臂来说，没有足够的六自由度DOF来完成多个层次的任务；实行双任务优先控制是必要的；也就是说，机械手的运动控制是主要任务和次要任务；

具有主要任务和次要任务的冗余机械臂的反向控制方程如下

上面的公式与前面的表达式(11)有很大的不同，但是算法框架是相似的；在上面的方程中，

是次要任务，

是主任务；主任务在主任务的指定零空间中实现；反向优先级的核心点是投影矩阵

的计算；

的表达式如公式(30)：

利用以前公式(22)-(28)中类似的导子，可得到简化后具有主要任务和次要任务的冗余机械臂的反向控制方程：

7.根据权利要求6所述的水下机械手冗余机械臂逆优先级阻抗控制方法，其特征在于，建立机械手的逆优先力控制策略如下：

力控制空间中机械手的动力学可写成：

其中X是笛卡尔空间中的位置，M(X)是惯性矩阵，

是非线性力，F是输入控制力，F_e是接触力；

此外，基于雅可比矩阵的变换可得到输入关节力矩

τ＝J^T(q)F (33)

操纵器在力控制空间中的期望运动方程可定义如下：

其中M_d和B_d是惯性和阻尼矩阵；F_d是指令力，F_e是接触力；

因此，环境和操纵器响应之间的关系可写成

上述两个方程的组合如下

从上面的方程可看出，如果M_e、B_e和K_e已知，则M_d和B_d的调整将影响***响应；

力控制使机械手能够与环境或人类相互作用；另外，在某些情况下，没有必要实现全方位的力控制，也没有必要保证全方位的力控制，也就是说，有时我们只是想保证某个方向的力跟踪控制精度；

因此有必要对机械手进行分级力控制；也就是说，有必要给出一个新的层次力控制框架；从上面的方程我们可得到期望的层次力控制关系如下

所以这两个方程的积分公式可写成

如果机械手末端执行器能够跟踪期望的笛卡尔速度为

和

则可实现机械手的精确力控制；笛卡尔速度与关节速度的关系应借鉴逆优先控制；因此，可得到机械手的逆优先力控制策略的方程：

上述方程所要求的关节速度将保证机械手的力控制；值得一提的是，上述力控制律只是速度级控制律，它依赖于内速度环控制；如果内位置控制效果良好，则可实现精确的力控制；由于内速度环控制可实现低频位置跟踪，所以外力环可实现低频力跟踪。

8.根据权利要求7所述的水下机械手冗余机械臂逆优先级阻抗控制方法，其特征在于，采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证的实现方式如下：

当机械手实施力控制时，机械手在一定程度上起到了发起者的作用，也就是说，机械手已经做好了响应外部环境的准备；当机械臂

作为阻抗控制模型工作时，机械臂会被动地响应外力；

外力与关节加速度的对应阻抗关系可表示为

参考速度可表示为

因此，机械手的逆优先阻抗控制保证的表达式为：

9.根据权利要求8所述的水下机械手冗余机械臂逆优先级阻抗控制方法，其特征在于，将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架实现方式如下：

混合阻抗应用就是上述两种策略的结合，即笛卡尔任务可分为两种情况：第一种是位置控制子空间，阻抗控制是在该子空间中实现的；第二个是力控制子空间，力控制在该子空间中实现；

因此选择一个选择矩阵；外力与位置响应的关系如下

所以期望速度的简化形式可表示为

然后我们得到了基于反向优先级的解决方案

考虑到n层任务，相应的阻抗控制任务也属于n层框架，因此，机械手速度级逆优先阻抗控制的总体框架表达式如下

表达式(52)解决了将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算的机械手逆优先混合阻抗控制中去；能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务。

10.水下机械手冗余机械臂逆优先级阻抗控制***，包括机械手和控制机械手的控制台(S31)；其特征在于，所述机械手包括机械臂、安装座(S1)、竖直柱(S2)、输出抓手(S12)和竖直气缸(S23)；

机械臂包括竖直升降移动台(S3)、臂段一(S6)、臂段二(S7)、臂段三(S8)和臂段四(S10)；

在竖直柱的左表面上设有竖直轨道(S24)，竖直升降移动台上下竖直滑动设置在竖直轨道上；竖直柱的下端固定连接在安装座的上表面上，竖直气缸的缸座(S21)固定连接在位于竖直轨道左方的安装座的上表面上，竖直气缸的伸缩杆(S22)竖直朝上布置，竖直气缸的伸缩杆上端固定连接在竖直升降移动台的下表面上；竖直升降移动台能在竖直气缸的伸缩杆的带动下可沿着竖直轨道上下运动，形成第一自由度；

臂段一包括A1段管(S13)和伸缩连接在A1段管的左管口内的A2段管(S14)，在A1段管内的右端固定设有伸缩杆水平朝左布置的一号气缸(S25)，一号气缸的伸缩杆固定连接在A2段管的右端；

在竖直升降移动台的左端设有由一台一号减速电机(S26)驱动的一号水平旋转轴(S4)，A1段管的右端固定连接在一号水平旋转轴上，使臂段一能水平转动形成第二自由度；在一号水平旋转轴上还设有能控制一号水平旋转轴转动的一号电磁刹车(S32)；

在A2段管的左端设有由一台二号减速电机(S27)驱动的二号水平旋转轴(S15)，臂段二的右端固定连接在二号水平旋转轴上，使臂段二能水平转动形成第三自由度；在二号水平旋转轴上还设有能控制二号水平旋转轴转动的二号电磁刹车(S33)；

在臂段二的左端设有由一台三号减速电机(S28)驱动的三号水平旋转轴(S18)，臂段三的右端固定连接在三号水平旋转轴上，使臂段三能水平转动形成第四自由度；在三号水平旋转轴上还设有能控制三号水平旋转轴转动的三号电磁刹车(S34)；

在臂段三的左端设有由一台四号减速电机(S29)驱动的能在左右竖直面上转动的一号横向竖直旋转轴(S9)，臂段四的右端固定连接在一号横向竖直旋转轴上，使臂段四能在左右竖直面上竖直转动形成第五自由度；在一号横向竖直旋转轴上还设有能控制一号横向竖直旋转轴转动的四号电磁刹车(S35)；

在臂段四的左端设有由一台五号减速电机(S30)驱动的能在前后竖直面上转动的一号纵向竖直旋转轴(S11)，输出抓手的右端固定连接在一号纵向竖直旋转轴上，使输出抓手的右端能在前后竖直面上竖直转动形成第六自由度；在一号纵向竖直旋转轴上还设有能控制一号纵向竖直旋转轴转动的五号电磁刹车(S36)；

A2段管能在一号气缸的伸缩杆的带动下在A1段管内左右伸缩移动形成第七自由度；

一号电磁刹车的控制端、二号电磁刹车的控制端、三号电磁刹车的控制端、四号电磁刹车的控制端、五号电磁刹车的控制端、一号减速电机的控制端、二号减速电机的控制端、三号减速电机的控制端、四号减速电机的控制端、五号减速电机的控制端、一号气缸的控制端和竖直气缸的控制端分别控制连接在控制台上。

Images