[发明专利]机械手逆优先级阻抗控制系统及控制方法

权利要求书

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

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

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

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

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

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

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

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

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

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

定义末端执行器在笛卡尔空间的位姿、速度分别为x、其关节空间的角位置、角速度分别为q、J为n自由度机器人的雅可比矩阵，其中x∈Rⁿ，J∈R^mn；冗余自由度机械臂的正运动学方程可用下式描述：

式(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×n，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_i|j是与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)解决了将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算的机械手逆优先混合阻抗控制中去；能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务；

适用于移动机械手冗余机械臂逆优先级阻抗控制方法的机械手逆优先级阻抗控制系统，所述机械手逆优先级阻抗控制系统包括机械手和控制机械手的控制台(S31)；还包括可移动的移动平台(S41)；所述机械手包括机械臂、安装座(S1)、竖直柱(S2)、输出抓手(S12)和竖直气缸(S23)；安装座固定在移动平台上；

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

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

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

臂段二包括B1段管(S16)和伸缩连接在B1段管的左管口内的B2段管(S17)，在B1段管内的右端固定设有伸缩杆水平朝左布置的二号气缸(37)，二号气缸的伸缩杆固定连接在B2段管的右端；

在竖直升降移动台的左端设有由一台一号减速电机(S26)驱动的一号水平旋转轴(S4)；

基臂段的右端固定连接在一号水平旋转轴上，使基臂段能水平转动形成第二自由度；在一号水平旋转轴上还设有能控制一号水平旋转轴转动的一号电磁刹车(S32)；

在基臂段的左端设有由一台基减速电机(S48)驱动的A号横向竖直旋转轴(S47)，A1段管的右端固定连接在A号横向竖直旋转轴上，使臂段二能水平转动形成第三自由度；在A号横向竖直旋转轴上还设有能控制A号横向竖直旋转轴转动的A号电磁刹车(S46)；

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

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

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

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

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

B2段管能在二号气缸的伸缩杆的带动下在B1段管内左右伸缩移动形成第九自由度；

一根一号水平管(S39)的左端水平固定连接在竖直柱的右表面上，在一号水平管内左右滑动设有平衡调节块(S40)，在一号水平管内的左端固定连接有伸缩杆水平朝右的平衡调节气缸(S38)，平衡调节气缸的伸缩杆的右端固定连接在平衡调节块上；

基减速电机的控制端、A号电磁刹车的控制端、一号电磁刹车的控制端、二号电磁刹车的控制端、三号电磁刹车的控制端、四号电磁刹车的控制端、五号电磁刹车的控制端、一号减速电机的控制端、二号减速电机的控制端、三号减速电机的控制端、四号减速电机的控制端、五号减速电机的控制端、一号气缸的控制端、二号气缸的控制端、平衡调节气缸的控制端和竖直气缸的控制端分别控制连接在控制台上；

在一号纵向竖直旋转轴的外表面上固定套设有环形圈(S42)，在环形圈上设有由一台环绕电机(S43)驱动能沿着环形圈移动的一号移动块(S44)，在一号移动块上设有摄像头(S45)；摄像头的控制端和环绕电机的控制端分别与控制台相连接。