CN111687832B - 空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法 - Google Patents

空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法 Download PDF

Info

Publication number
CN111687832B
CN111687832B CN202010369737.6A CN202010369737A CN111687832B CN 111687832 B CN111687832 B CN 111687832B CN 202010369737 A CN202010369737 A CN 202010369737A CN 111687832 B CN111687832 B CN 111687832B
Authority
CN
China
Prior art keywords
control
task
manipulator
inverse
vertical
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010369737.6A
Other languages
English (en)
Other versions
CN111687832A (zh
Inventor
刘海燕
苏宇
李敏斯
林春兰
吴雪颖
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangxi University of Science and Technology
Original Assignee
Guangxi University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangxi University of Science and Technology filed Critical Guangxi University of Science and Technology
Priority to CN202010369737.6A priority Critical patent/CN111687832B/zh
Publication of CN111687832A publication Critical patent/CN111687832A/zh
Application granted granted Critical
Publication of CN111687832B publication Critical patent/CN111687832B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Abstract

本发明公开了空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法。属于机械手冗余机械臂逆优先级阻抗控制技术领域，能控制机械手的平衡。包括机械手和控制机械手的控制台；所述机械手包括机械臂、安装座、竖直柱、输出抓手和竖直气缸；机械臂包括竖直升降移动台、臂段一、臂段二、臂段三和臂段四；在竖直柱的左表面上设有竖直轨道，竖直升降移动台上下竖直滑动设置在竖直轨道上；竖直柱的下端固定连接在安装座的上表面上，竖直气缸的缸座固定连接在位于竖直轨道左方的安装座的上表面上，竖直气缸的伸缩杆上端固定连接在竖直升降移动台的下表面上；竖直升降移动台能在竖直气缸的伸缩杆的带动下可沿着竖直轨道上下运动，形成第一自由度。

Description

空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法
技术领域
本发明涉及机械手冗余机械臂逆优先级阻抗控制技术领域，尤其涉及空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法。
背景技术
目前工业机器人采用的控制方法是把机械手上每一个关节都当作一个单独的伺服机构，即每个轴对应一个伺服器，每个伺服器通过总线控制，由控制器统一控制并协调工作；
六个自由度的机械臂是具有完成空间定位能力最小自由度数的机械臂，多于六个自由度的机械臂统一称为冗余机械臂；
现在机械手的机械臂阻抗控制方法在不同层次结构下不能实现期望的阻抗控制任务，因此，设计一种能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务的方法显得非常必要。
发明内容
本发明是为了解决现有机械臂阻抗控制方法在不同层次结构下不能实现期望的阻抗控制任务的不足，提供一种一是能控制机械手的平衡，二是能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务的空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法。
以上技术问题是通过下列技术方案解决的：
一种空间机械手冗余机械臂逆优先级阻抗控制方法，包括如下步骤：
步骤1、建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略；
步骤2、建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略；
步骤3、建立奇异鲁棒解逆运动学分析模型；
步骤4、建立多任务冗余机械臂的逆优先控制策略；
步骤5、对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理；
步骤6、建立机械手的逆优先力控制策略；
步骤7、采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证；
步骤8、将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架。
本方案冗余机械臂在关节空间中的运动是按照相反的顺序导出的；然后将笛卡尔阻抗控制与逆优先阻抗控制相结合，解决了逆分层阻抗控制问题，将笛卡尔阻抗控制行为分为高优先阻抗控制和低优先阻抗控制。其中高优先阻抗控制任务不会干扰低优先阻抗控制任务，关节空间中的运动按相反的顺序会受到影响，要在相应的投影算子中工作；最后实现了高优先阻抗控制任务，避免了低优先阻抗控制任务中可能出现的奇异性引起的变形。因此，所提出的逆优先阻抗控制方法可使冗余机械臂在适当的层次结构下实现期望的阻抗控制任务。
作为优选，建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略实现过程如下：
定义末端执行器在笛卡尔空间的位姿、速度分别为x、
Figure GDA0004090628290000021
其关节空间的角位置、角速度分别为q、/>
Figure GDA0004090628290000022
J为n自由度机器人的雅可比矩阵，其中x∈Rn，/>
Figure GDA0004090628290000023
Figure GDA0004090628290000024
J∈Rmn；冗余自由度机械臂的正运动学方程可用下式描述：
Figure GDA0004090628290000025
式(1)也被称为机械臂运动学速度模型；
考虑到最小二乘法的解，最优问题可列为：
Figure GDA0004090628290000026
式(1)的解可通过寻找最佳
Figure GDA0004090628290000027
来解决；
Figure GDA0004090628290000028
因此，式(1)的伪逆解可表示为：
Figure GDA0004090628290000029
式中J+——雅可比矩阵的伪逆
I——单位矩阵
Figure GDA00040906282900000210
——任意零空间矢量
Figure GDA00040906282900000211
——最小范数解，定义了手的动作
Figure GDA00040906282900000212
——齐次解，在末端不产生动作
式(4)代表了末端执行器的位置和姿态控制；在式(4)中加入任意残差，可得到包含零空间的一般表达式；利用上述方程可在零向量上实现多任务优化；
然而，上述方程忽略了雅可比矩阵的病态；正则化方程可通过添加额外的正则化值来修改，
Figure GDA00040906282900000213
其中λ≥0是加权矩阵，
Figure GDA00040906282900000214
为加权系数，并且满足
Figure GDA00040906282900000215
上述方程的解就可表示为：
Figure GDA00040906282900000216
式(7)也被称为冗余机械臂运动学模型；
冗余机械臂零空间向量的位置相关标量指数的关节限制梯度方向的关节限制函数为：
Figure GDA0004090628290000031
作为优选，建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略如下：
在雅可比矩阵的冗余机械臂求解中，优化任务是在主任务的零空间中实现的；反向任务运动学是在正向任务运动学基础上建立的：
Figure GDA0004090628290000032
其中
Figure GDA0004090628290000033
和/>
Figure GDA0004090628290000034
表示task1和task2
从表达式(9)中得出冗余机械臂的逆运动学方程为：
Figure GDA0004090628290000035
Task1作为主任务，task2作为辅助任务；也就是说，
Figure GDA0004090628290000036
是在/>
Figure GDA0004090628290000037
的零空间中实现的；冗余机械臂的最终逆运动学表达式如下所示：
Figure GDA0004090628290000038
其中
Figure GDA0004090628290000039
Figure GDA00040906282900000310
是投影矩阵，它给出了次要任务到主要任务的适用范围；/>
Figure GDA00040906282900000311
和/>
Figure GDA00040906282900000312
是所需的命令速度；/>
Figure GDA00040906282900000313
是主要任务，/>
Figure GDA00040906282900000314
是次要任务；
如果两个相关任务是相互依赖的，则相应的雅可比矩阵是奇异的；如果任务雅可比矩阵是奇异的，则相应的任务是不满足的；在这种情况下，雅可比相关矩阵将是奇点，定义为算法奇点；
也就是说，如果
Figure GDA00040906282900000315
其中ρ(·)是矩阵的秩；
很明显，算法的奇异性是由次要任务和主要任务之间的任务冲突引起的；此外，基于任务优先级的冗余机械臂逆运动学旨在提供更好的控制主要任务的有效性；
因此，让位置控制方向作为主要任务，从而使位置保证控制方向任务的准确性；然后建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略方程：
Figure GDA00040906282900000316
作为优选，建立奇异鲁棒解逆运动学分析模型如下：
基于雅可比伪逆解会出现运动学奇异性，这是由次矩阵引起的；对于运动奇异性问题，应该还要给出DLS(阻尼最小二乘)解；
对DLS解的成本函数可修改为：
Figure GDA0004090628290000041
因此，上述方程的奇异鲁棒伪逆解可表示为：
Figure GDA0004090628290000042
式(15)即为奇异鲁棒解逆运动学分析模型，设λ＝η2I，上述DLS解等价于附加正则化解，标量值η平衡了任务精度和奇异性；
对于雅可比矩阵伪逆解的计算，可给出雅可比矩阵的奇异值SVD分解形式
J＝UVVT (16)
其中U∈Rm×m，V∈Rn×n，∑∈Rm×n，U是由列向量ui组成的一元矩阵，V是由列向量vi组成的一元矩阵，∑是m×n对角矩阵的块矩阵，该对角矩阵包含J的奇异值σi≥0以递减顺序包含n-m个零列向量；
Figure GDA0004090628290000043
其中，r≤m是矩阵J的秩；
对于运动奇异性，参考计算伪逆解时需要的奇异值分解SVD，大的产生的关节速度是由于最小的奇异值迅速接近0，如下式：
Figure GDA0004090628290000044
因子λ0将影响奇异性，λ0值越高，阻尼越大，联合速度就越接近奇异点；此外，定义可变阻尼因子的策略也不同；我们可得到
Figure GDA0004090628290000045
从上式中，我们可看出参数δ＞0监视最小的奇异值。
作为优选，建立多任务冗余机械臂的逆优先控制策略如下；
引入逆优先投影矩阵
Figure GDA0004090628290000046
该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间，所以得出
Figure GDA0004090628290000047
Figure GDA0004090628290000048
其中Ji|j是与j-th任务线性无关的i-th任务的所有组件相关联的雅可比矩阵；
所以，优先级推导公式如下：
Figure GDA0004090628290000049
在上述推导中，k＝l，l-1，…，1，其中1为正整数；初始值
Figure GDA00040906282900000410
为了给出线性无关雅可比矩阵J的一般计算形式，定义逆增广Jacobian矩阵为：
Figure GDA0004090628290000051
就有了
Figure GDA0004090628290000052
其中
Figure GDA0004090628290000053
表示/>
Figure GDA0004090628290000054
的行；
于是乎，
Figure GDA0004090628290000055
的伪逆解就可表示为：
Figure GDA0004090628290000056
和
Figure GDA0004090628290000057
其中，Tk表示矩阵
Figure GDA0004090628290000058
的扩充；
最终的反向优先级投影可写为：
Figure GDA0004090628290000059
于是，我们就可得出伪逆解的表达式：
Figure GDA00040906282900000510
建立多任务冗余机械臂的逆优先控制策略方程如下：
Figure GDA00040906282900000511
作为优选，对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理如下：
对于六自由度或七自由度冗余机械臂来说，没有足够的自由度来完成多个层次的任务；实行双任务优先控制是必要的；也就是说，机械手的运动控制是主要任务和次要任务；
具有主要任务和次要任务的冗余机械臂的反向控制方程如下
Figure GDA00040906282900000512
上面的公式与前面的表达式(11)有很大的不同，但是算法框架是相似的；在上面的方程中，
Figure GDA00040906282900000513
是次要任务，/>
Figure GDA00040906282900000514
是主任务；主任务在主任务的指定零空间中实现；反向优先级的核心点是投影矩阵/>
Figure GDA00040906282900000515
的计算；/>
Figure GDA00040906282900000516
的表达式如公式(30)：
Figure GDA0004090628290000061
利用前述公式(22)-(28)中的导子，可得到简化后具有主要任务和次要任务的冗余机械臂的反向控制方程：
Figure GDA0004090628290000062
作为优选，建立机械手的逆优先力控制策略如下：
力控制空间中机械手的动力学可写成：
Figure GDA0004090628290000063
其中X是笛卡尔空间中的位置，M(X)是惯性矩阵，
Figure GDA0004090628290000064
是非线性力，F是输入控制力，Fe是接触力；
此外，基于雅可比矩阵的变换可得到输入关节力矩
τ＝JT(q)F (33)
操纵器在力控制空间中的期望运动方程可定义如下：
Figure GDA0004090628290000065
其中Md和Bd是惯性和阻尼矩阵；Fd是指令力，Fe是接触力；
因此，环境和操纵器响应之间的关系可写成
Figure GDA0004090628290000066
上述两个方程的组合如下
Figure GDA0004090628290000067
从上面的方程可看出，如果Me、Be和Ke已知，则Md和Bd的调整将影响系统响应；
力控制使机械手能够与环境或人类相互作用；另外，在某些情况下，没有必要实现全方位的力控制，也没有必要保证全方位的力控制，也就是说，有时我们只是想保证某个方向的力跟踪控制精度；
因此有必要对机械手进行分级力控制；也就是说，有必要给出一个新的层次力控制框架；从上面的方程我们可得到期望的层次力控制关系如下
Figure GDA0004090628290000068
Figure GDA0004090628290000069
所以这两个方程的积分公式可写成
Figure GDA00040906282900000610
Figure GDA00040906282900000611
如果机械手末端执行器能够跟踪期望的笛卡尔速度为
Figure GDA00040906282900000612
和/>
Figure GDA00040906282900000613
则可实现机械手的精确力控制；笛卡尔速度与关节速度的关系应借鉴逆优先控制；因此，可得到机械手的逆优先力控制策略的方程：
Figure GDA0004090628290000071
上述方程所要求的关节速度将保证机械手的力控制；值得一提的是，上述力控制律只是速度级控制律，它依赖于内速度环控制；如果内位置控制效果良好，则可实现精确的力控制；由于内速度环控制可实现低频位置跟踪，所以外力环可实现低频力跟踪。
作为优选，采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证的实现方式如下：
当机械手实施力控制时，机械手在一定程度上起到了发起者的作用，也就是说，机械手已经做好了响应外部环境的准备；当机械臂作为阻抗控制模型工作时，机械臂会被动地响应外力；
外力与关节加速度的对应阻抗关系可表示为
Figure GDA0004090628290000072
Figure GDA0004090628290000073
参考速度可表示为
Figure GDA0004090628290000074
Figure GDA0004090628290000075
因此，机械手的逆优先阻抗控制保证的表达式为：
Figure GDA0004090628290000076
Figure GDA0004090628290000077
作为优选，将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架实现方式如下：
混合阻抗应用就是上述两种策略的结合，即笛卡尔任务可分为两种情况：第一种是位置控制子空间，阻抗控制是在该子空间中实现的；第二个是力控制子空间，力控制在该子空间中实现；
因此选择一个选择矩阵；外力与位置响应的关系如下
Figure GDA0004090628290000078
Figure GDA0004090628290000081
所以期望速度的简化形式可表示为
Figure GDA0004090628290000082
Figure GDA0004090628290000083
然后我们得到了基于反向优先级的解决方案
Figure GDA0004090628290000084
/>
考虑到n层任务，相应的阻抗控制任务也属于n层框架，因此，机械手速度级逆优先阻抗控制的总体框架表达式如下
Figure GDA0004090628290000085
表达式(52)解决了将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算的机械手逆优先混合阻抗控制中去，能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务。
空间机械手冗余机械臂逆优先级阻抗控制系统，包括机械手和控制机械手的控制台；所述机械手包括机械臂、安装座、竖直柱、输出抓手和竖直气缸；
机械臂包括竖直升降移动台、臂段一、臂段二、臂段三和臂段四；
在竖直柱的左表面上设有竖直轨道，竖直升降移动台上下竖直滑动设置在竖直轨道上；竖直柱的下端固定连接在安装座的上表面上，竖直气缸的缸座固定连接在位于竖直轨道左方的安装座的上表面上，竖直气缸的伸缩杆竖直朝上布置，竖直气缸的伸缩杆上端固定连接在竖直升降移动台的下表面上；竖直升降移动台能在竖直气缸的伸缩杆的带动下可沿着竖直轨道上下运动，形成第一自由度；
臂段一包括A1段管和伸缩连接在A1段管的左管口内的A2段管，在A1段管内的右端固定设有伸缩杆水平朝左布置的一号气缸，一号气缸的伸缩杆固定连接在A2段管的右端；
臂段二包括B1段管和伸缩连接在B1段管的左管口内的B2段管，在B1段管内的右端固定设有伸缩杆水平朝左布置的二号气缸，二号气缸的伸缩杆固定连接在B2段管的右端；
在竖直升降移动台的左端设有由一台一号减速电机驱动的一号水平旋转轴，A1段管的右端固定连接在一号水平旋转轴上，使臂段一能水平转动形成第二自由度；在一号水平旋转轴上还设有能控制一号水平旋转轴转动的一号电磁刹车；
在A2段管的左端设有由一台二号减速电机驱动的二号水平旋转轴，B1段管的右端固定连接在二号水平旋转轴上，使臂段二能水平转动形成第三自由度；在二号水平旋转轴上还设有能控制二号水平旋转轴转动的二号电磁刹车；
在B2段管的左端设有由一台三号减速电机驱动的三号水平旋转轴，臂段三的右端固定连接在三号水平旋转轴上，使臂段三能水平转动形成第四自由度；在三号水平旋转轴上还设有能控制三号水平旋转轴转动的三号电磁刹车；
在臂段三的左端设有由一台四号减速电机驱动的能在左右竖直面上转动的一号横向竖直旋转轴，臂段四的右端固定连接在一号横向竖直旋转轴上，使臂段四能在左右竖直面上竖直转动形成第五自由度；在一号横向竖直旋转轴上还设有能控制一号横向竖直旋转轴转动的四号电磁刹车；
在臂段四的左端设有由一台五号减速电机驱动的能在前后竖直面上转动的一号纵向竖直旋转轴，输出抓手的右端固定连接在一号纵向竖直旋转轴上，使输出抓手的右端能在前后竖直面上竖直转动形成第六自由度；在一号纵向竖直旋转轴上还设有能控制一号纵向竖直旋转轴转动的五号电磁刹车；
A2段管能在一号气缸的伸缩杆的带动下在A1段管内左右伸缩移动形成第七自由度；
B2段管能在二号气缸的伸缩杆的带动下在B1段管内左右伸缩移动形成第八自由度；
一根一号水平管的左端水平固定连接在竖直柱的右表面上，在一号水平管内左右滑动设有平衡调节块，在一号水平管内的左端固定连接有伸缩杆水平朝右的平衡调节气缸，平衡调节气缸的伸缩杆的右端固定连接在平衡调节块上；
一号电磁刹车的控制端、二号电磁刹车的控制端、三号电磁刹车的控制端、四号电磁刹车的控制端、五号电磁刹车的控制端、一号减速电机的控制端、二号减速电机的控制端、三号减速电机的控制端、四号减速电机的控制端、五号减速电机的控制端、一号气缸的控制端、二号气缸的控制端、平衡调节气缸的控制端和竖直气缸的控制端分别控制连接在控制台上。
本发明能够达到如下效果：
本发明能控制机械手的平衡，机械手便于移动；能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务。
附图说明
图1为本发明力控制的动力学方案示意图。
图2为本发明阻抗控制的动力学方案示意图。
图3为本发明混合阻抗控制的动力学方案示意图。
图4为本发明实施例的一种七自由度机械手连接结构示意图。
图5为本发明实施例的一种电路原理连接结构示意框图。
具体实施方式
下面结合附图与实施例对本发明作进一步的说明。
实施例，空间机械手冗余机械臂逆优先级阻抗控制方法，参见图1-3所示。包括如下步骤：
步骤1、建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略实现过程如下：
定义末端执行器在笛卡尔空间的位姿、速度分别为x、
Figure GDA0004090628290000101
其关节空间的角位置、角速度分别为q、/>
Figure GDA0004090628290000102
J为n自由度机器人的雅可比矩阵，其中x∈Rn，/>
Figure GDA0004090628290000103
Figure GDA0004090628290000104
J∈Rmn；冗余自由度机械臂的正运动学方程可用下式描述：
Figure GDA0004090628290000105
式(1)也被称为机械臂运动学速度模型；
考虑到最小二乘法的解，最优问题可列为：
Figure GDA0004090628290000106
式(1)的解可通过寻找最佳
Figure GDA0004090628290000107
来解决；
Figure GDA0004090628290000108
因此，式(1)的伪逆解可表示为：
Figure GDA0004090628290000109
式中J+——雅可比矩阵的伪逆
I——单位矩阵
Figure GDA00040906282900001010
——任意零空间矢量
Figure GDA00040906282900001011
一—最小范数解，定义了手的动作
Figure GDA00040906282900001012
——齐次解，在末端不产生动作
式(4)代表了末端执行器的位置和姿态控制；在式(4)中加入任意残差，可得到包含零空间的一般表达式；利用上述方程可在零向量上实现多任务优化；
然而，上述方程忽略了雅可比矩阵的病态；正则化方程可通过添加额外的正则化值来修改，
Figure GDA00040906282900001013
其中λ≥0是加权矩阵，
Figure GDA00040906282900001014
为加权系数，并且满足
Figure GDA00040906282900001015
上述方程的解就可表示为：
Figure GDA00040906282900001016
式(7)也被称为冗余机械臂运动学模型；
冗余机械臂零空间向量的位置相关标量指数的关节限制梯度方向的关节限制函数为：
Figure GDA00040906282900001017
步骤2、建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略如下：
在雅可比矩阵的冗余机械臂求解中，优化任务是在主任务的零空间中实现的；反向任务运动学是在正向任务运动学基础上建立的：
Figure GDA0004090628290000111
其中
Figure GDA0004090628290000112
和/>
Figure GDA0004090628290000113
表示task1和task2
从表达式(9)中得出冗余机械臂的逆运动学方程为：
Figure GDA0004090628290000114
Task1作为主任务，task2作为辅助任务；也就是说，
Figure GDA0004090628290000115
是在/>
Figure GDA0004090628290000116
的零空间中实现的；冗余机械臂的最终逆运动学表达式如下所示：
Figure GDA0004090628290000117
其中
Figure GDA0004090628290000118
Figure GDA0004090628290000119
是投影矩阵，它给出了次要任务到主要任务的适用范围；/>
Figure GDA00040906282900001110
和/>
Figure GDA00040906282900001111
是所需的命令速度；/>
Figure GDA00040906282900001112
是主要任务，/>
Figure GDA00040906282900001113
是次要任务；/>
如果两个相关任务是相互依赖的，则相应的雅可比矩阵是奇异的；如果任务雅可比矩阵是奇异的，则相应的任务是不满足的；在这种情况下，雅可比相关矩阵将是奇点，定义为算法奇点；
也就是说，如果
Figure GDA00040906282900001114
其中ρ(·)是矩阵的秩；
很明显，算法的奇异性是由次要任务和主要任务之间的任务冲突引起的；此外，基于任务优先级的冗余机械臂逆运动学旨在提供更好的控制主要任务的有效性；
因此，让位置控制方向作为主要任务，从而使位置保证控制方向任务的准确性；然后建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略方程：
Figure GDA00040906282900001115
步骤3、建立奇异鲁棒解逆运动学分析模型如下：
基于雅可比伪逆解会出现运动学奇异性，这是由次矩阵引起的；对于运动奇异性问题，应该还要给出DLS(阻尼最小二乘)解；
对DLS解的成本函数可修改为：
Figure GDA00040906282900001116
因此，上述方程的奇异鲁棒伪逆解可表示为：
Figure GDA00040906282900001117
式(15)即为奇异鲁棒解逆运动学分析模型，设λ＝η2I，上述DLS解等价于附加正则化解，标量值η平衡了任务精度和奇异性；
对于雅可比矩阵伪逆解的计算，可给出雅可比矩阵的奇异值SVD分解形式
J＝UΣVT (16)
其中U∈Rm×m，V∈Rn×n，∑∈Rm×n，U是由列向量ui组成的一元矩阵，V是由列向量vi组成的一元矩阵，∑是m×n对角矩阵的块矩阵，该对角矩阵包含J的奇异值σi≥0以递减顺序包含n-m个零列向量；
Figure GDA0004090628290000121
其中，r≤n是矩阵J的秩；
对于运动奇异性，参考计算伪逆解时需要的奇异值分解SVD，大的产生的关节速度是由于最小的奇异值迅速接近0，如下式：
Figure GDA0004090628290000122
因子λ0将影响奇异性，λ0值越高，阻尼越大，联合速度就越接近奇异点；此外，定义可变阻尼因子的策略也不同；我们可得到
Figure GDA0004090628290000123
从上式中，我们可看出参数δ＞0监视最小的奇异值。
步骤4、建立多任务冗余机械臂的逆优先控制策略如下；
引入逆优先投影矩阵
Figure GDA0004090628290000124
该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间，所以得出
Figure GDA0004090628290000125
Figure GDA0004090628290000126
其中Ji|j是与j-th任务线性无关的i-th任务的所有组件相关联的雅可比矩阵；
所以，优先级推导公式如下：
Figure GDA0004090628290000127
在上述推导中，k＝l，l-1，…，1，其中l为正整数；初始值
Figure GDA0004090628290000128
为了给出线性无关雅可比矩阵J的一般计算形式，定义逆增广Jacobian矩阵为：
Figure GDA0004090628290000129
就有了
Figure GDA0004090628290000131
其中
Figure GDA0004090628290000132
表示/>
Figure GDA0004090628290000133
的行；
于是乎，
Figure GDA0004090628290000134
的伪逆解就可表示为：
Figure GDA0004090628290000135
和
Figure GDA0004090628290000136
其中，Tk表示矩阵
Figure GDA0004090628290000137
的扩充；
最终的反向优先级投影可写为：
Figure GDA0004090628290000138
于是，我们就可得出伪逆解的表达式：
Figure GDA0004090628290000139
/>
建立多任务冗余机械臂的逆优先控制策略方程如下：
Figure GDA00040906282900001310
步骤5、对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理如下：
对于六自由度或七自由度冗余机械臂来说，没有足够的自由度来完成多个层次的任务；实行双任务优先控制是必要的；也就是说，机械手的运动控制是主要任务和次要任务；
具有主要任务和次要任务的冗余机械臂的反向控制方程如下
Figure GDA00040906282900001311
上面的公式与前面的表达式(11)有很大的不同，但是算法框架是相似的；在上面的方程中，
Figure GDA00040906282900001312
是次要任务，/>
Figure GDA00040906282900001313
是主任务；主任务在主任务的指定零空间中实现；反向优先级的核心点是投影矩阵/>
Figure GDA00040906282900001314
的计算；/>
Figure GDA00040906282900001315
的表达式如公式(30)：
Figure GDA00040906282900001316
利用前述公式(22)-(28)中的导子，可得到简化后具有主要任务和次要任务的冗余机械臂的反向控制方程：
Figure GDA00040906282900001317
步骤6、建立机械手的逆优先力控制策略如下：
力控制空间中机械手的动力学可写成：
Figure GDA0004090628290000141
其中X是笛卡尔空间中的位置，M(X)是惯性矩阵，
Figure GDA0004090628290000142
是非线性力，F是输入控制力，Fe是接触力；
此外，基于雅可比矩阵的变换可得到输入关节力矩
τ＝J2(q)F (33)
操纵器在力控制空间中的期望运动方程可定义如下：
Figure GDA0004090628290000143
其中Md和Bd是惯性和阻尼矩阵；Fd是指令力，Fe是接触力；
力控制的动力学方案如图1所示；
因此，环境和操纵器响应之间的关系可写成
Figure GDA0004090628290000144
上述两个方程的组合如下
Figure GDA0004090628290000145
从上面的方程可看出，如果Me、Be和Ke已知，则Md和Bd的调整将影响系统响应；
力控制使机械手能够与环境或人类相互作用；另外，在某些情况下，没有必要实现全方位的力控制，也没有必要保证全方位的力控制，也就是说，有时我们只是想保证某个方向的力跟踪控制精度；
例如，当机械手与刨床相互作用时，只需在垂直方向保持精确的力跟踪控制，而另一个方向不需要精确的力跟踪控制；在其他情况下，位置方向力控制比姿态方向力控制更为重要；
因此有必要对机械手进行分级力控制；也就是说，有必要给出一个新的层次力控制框架；从上面的方程我们可得到期望的层次力控制关系如下
Figure GDA0004090628290000146
Figure GDA0004090628290000147
所以这两个方程的积分公式可写成
Figure GDA0004090628290000148
Figure GDA0004090628290000149
如果机械手末端执行器能够跟踪期望的笛卡尔速度为x1c和x2c，则可实现机械手的精确力控制；笛卡尔速度与关节速度的关系应借鉴逆优先控制；因此，可得到机械手的逆优先力控制策略的方程：
Figure GDA0004090628290000151
上述方程所要求的关节速度将保证机械手的力控制；值得一提的是，上述力控制律只是速度级控制律，它依赖于内速度环控制；如果内位置控制效果良好，则可实现精确的力控制；由于内速度环控制可实现低频位置跟踪，所以外力环可实现低频力跟踪。
步骤7、采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证的实现方式如下：
当机械手实施力控制时，机械手在一定程度上起到了发起者的作用，也就是说，机械手已经做好了响应外部环境的准备；当机械臂
作为阻抗控制模型工作时，机械臂会被动地响应外力；阻抗控制的动力学方案如图2所示；
外力与关节加速度的对应阻抗关系可表示为
Figure GDA0004090628290000152
Figure GDA0004090628290000153
参考速度可表示为
Figure GDA0004090628290000154
Figure GDA0004090628290000155
/>
因此，机械手的逆优先阻抗控制保证的表达式为：
Figure GDA0004090628290000156
步骤8、将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架实现方式如下：
混合阻抗应用就是上述两种策略的结合，即笛卡尔任务可分为两种情况：第一种是位置控制子空间，阻抗控制是在该子空间中实现的；第二个是力控制子空间，力控制在该子空间中实现；
因此选择一个选择矩阵；外力与位置响应的关系如下
Figure GDA0004090628290000161
Figure GDA0004090628290000162
所以期望速度的简化形式可表示为
Figure GDA0004090628290000163
Figure GDA0004090628290000164
然后我们得到了基于反向优先级的解决方案
Figure GDA0004090628290000165
混合阻抗控制的动力学方案图3所示；
考虑到n层任务，相应的阻抗控制任务也属于n层框架，因此，机械手速度级逆优先阻抗控制的总体框架表达式如下
Figure GDA0004090628290000166
表达式(52)解决了将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算的机械手逆优先混合阻抗控制中去；能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务。
空间机械手冗余机械臂逆优先级阻抗控制系统，参见图4、图5所示，包括机械手和控制机械手的控制台S31；所述机械手包括机械臂、安装座S1、竖直柱S2、输出抓手S12和竖直气缸S23；
机械臂包括竖直升降移动台S3、臂段一S6、臂段二S7、臂段三S8和臂段四S10；
在竖直柱的左表面上设有竖直轨道S24，竖直升降移动台上下竖直滑动设置在竖直轨道上；竖直柱的下端固定连接在安装座的上表面上，竖直气缸的缸座S21固定连接在位于竖直轨道左方的安装座的上表面上，竖直气缸的伸缩杆S22竖直朝上布置，竖直气缸的伸缩杆上端固定连接在竖直升降移动台的下表面上；竖直升降移动台能在竖直气缸的伸缩杆的带动下可沿着竖直轨道上下运动，形成第一自由度；
臂段一包括A1段管S13和伸缩连接在A1段管的左管口内的A2段管S14，在A1段管内的右端固定设有伸缩杆水平朝左布置的一号气缸S25，一号气缸的伸缩杆固定连接在A2段管的右端；
臂段二包括B1段管S16和伸缩连接在B1段管的左管口内的B2段管S17，在B1段管内的右端固定设有伸缩杆水平朝左布置的二号气缸37，二号气缸的伸缩杆固定连接在B2段管的右端；
在竖直升降移动台的左端设有由一台一号减速电机S26驱动的一号水平旋转轴S4，A1段管的右端固定连接在一号水平旋转轴上，使臂段一能水平转动形成第二自由度；在一号水平旋转轴上还设有能控制一号水平旋转轴转动的一号电磁刹车S32；
在A2段管的左端设有由一台二号减速电机S27驱动的二号水平旋转轴S15，B1段管的右端固定连接在二号水平旋转轴上，使臂段二能水平转动形成第三自由度；在二号水平旋转轴上还设有能控制二号水平旋转轴转动的二号电磁刹车S33；
在B2段管的左端设有由一台三号减速电机S28驱动的三号水平旋转轴S18，臂段三的右端固定连接在三号水平旋转轴上，使臂段三能水平转动形成第四自由度；在三号水平旋转轴上还设有能控制三号水平旋转轴转动的三号电磁刹车S34；
在臂段三的左端设有由一台四号减速电机S29驱动的能在左右竖直面上转动的一号横向竖直旋转轴S9，臂段四的右端固定连接在一号横向竖直旋转轴上，使臂段四能在左右竖直面上竖直转动形成第五自由度；在一号横向竖直旋转轴上还设有能控制一号横向竖直旋转轴转动的四号电磁刹车S35；
在臂段四的左端设有由一台五号减速电机S30驱动的能在前后竖直面上转动的一号纵向竖直旋转轴S11，输出抓手的右端固定连接在一号纵向竖直旋转轴上，使输出抓手的右端能在前后竖直面上竖直转动形成第六自由度；在一号纵向竖直旋转轴上还设有能控制一号纵向竖直旋转轴转动的五号电磁刹车S36；
A2段管能在一号气缸的伸缩杆的带动下在A1段管内左右伸缩移动形成第七自由度；
B2段管能在二号气缸的伸缩杆的带动下在B1段管内左右伸缩移动形成第八自由度；
一根一号水平管S39的左端水平固定连接在竖直柱的右表面上，在一号水平管内左右滑动设有平衡调节块S40，在一号水平管内的左端固定连接有伸缩杆水平朝右的平衡调节气缸S38，平衡调节气缸的伸缩杆的右端固定连接在平衡调节块上；
一号电磁刹车的控制端、二号电磁刹车的控制端、三号电磁刹车的控制端、四号电磁刹车的控制端、五号电磁刹车的控制端、一号减速电机的控制端、二号减速电机的控制端、三号减速电机的控制端、四号减速电机的控制端、五号减速电机的控制端、一号气缸的控制端、二号气缸的控制端、平衡调节气缸的控制端和竖直气缸的控制端分别控制连接在控制台上。第七自由度能让机械臂自由伸长，第八自由度能让机械臂自由伸长，大大增加了作业范围和灵活性。
平衡调节气缸对平衡调节块进行左右移动控制就能控制竖直柱的平衡，进而控制机械手的平衡。
由于本发明有的机械手有八个自由度，灵活性好，可靠性高，易于完成控制任务。

Claims (1)

1.一种空间机械手冗余机械臂逆优先级阻抗控制方法，其特征在于，包括如下步骤：
步骤1、建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略；
步骤2、建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略；
步骤3、建立奇异鲁棒解逆运动学分析模型；
步骤4、建立多任务冗余机械臂的逆优先控制策略；
步骤5、对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理；
步骤6、建立机械手的逆优先力控制策略；
步骤7、采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证；
步骤8、将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架；
建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略实现过程如下：
定义末端执行器在笛卡尔空间的位姿、速度分别为x、
Figure FDA0004090628270000011
其关节空间的角位置、角速度分别为q、/>
Figure FDA0004090628270000012
J为n自由度机器人的雅可比矩阵，其中x∈Rn，/>
Figure FDA0004090628270000013
J∈Rmn；冗余自由度机械臂的正运动学方程可用下式描述：
Figure FDA0004090628270000014
式(1)也被称为机械臂运动学速度模型；
考虑到最小二乘法的解，最优问题可列为：
Figure FDA0004090628270000021
式(1)的解可通过寻找最佳
Figure FDA0004090628270000022
来解决；
Figure FDA0004090628270000023
因此，式(1)的伪逆解可表示为：
Figure FDA0004090628270000024
式中J+——雅可比矩阵的伪逆
I——单位矩阵
Figure FDA0004090628270000025
——任意零空间矢量
Figure FDA0004090628270000026
——最小范数解，定义了手的动作
Figure FDA0004090628270000027
——齐次解，在末端不产生动作
式(4)代表了末端执行器的位置和姿态控制；在式(4)中加入任意残差，可得到包含零空间的一般表达式；利用上述方程可在零向量上实现多任务优化；
然而，上述方程忽略了雅可比矩阵的病态；正则化方程可通过添加额外的正则化值来修改，
Figure FDA0004090628270000028
其中λ≥0是加权矩阵，
Figure FDA0004090628270000029
为加权系数，并且满足
Figure FDA00040906282700000210
上述方程的解就可表示为：
Figure FDA00040906282700000211
式(7)也被称为冗余机械臂运动学模型；
冗余机械臂零空间向量的位置相关标量指数的关节限制梯度方向的关节限制函数为：
Figure FDA0004090628270000031
建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略如下：
在雅可比矩阵的冗余机械臂求解中，优化任务是在主任务的零空间中实现的；反向任务运动学是在正向任务运动学基础上建立的：
Figure FDA0004090628270000032
其中
Figure FDA0004090628270000033
和/>
Figure FDA0004090628270000034
表示task1和task2
从表达式(9)中得出冗余机械臂的逆运动学方程为：
Figure FDA0004090628270000035
Task1作为主任务，task2作为辅助任务；也就是说，task2
Figure FDA0004090628270000036
是在task1/>
Figure FDA0004090628270000037
的零空间中实现的；冗余机械臂的最终逆运动学表达式如下所示：
Figure FDA0004090628270000038
其中
Figure FDA0004090628270000039
Figure FDA00040906282700000310
是投影矩阵，它给出了次要任务到主要任务的适用范围；/>
Figure FDA00040906282700000311
和/>
Figure FDA00040906282700000312
是所需的命令速度；/>
Figure FDA00040906282700000313
是主要任务，/>
Figure FDA00040906282700000314
是次要任务；
如果两个相关任务是相互依赖的，则相应的雅可比矩阵是奇异的；如果任务雅可比矩阵是奇异的，则相应的任务是不满足的；在这种情况下，雅可比相关矩阵将是奇点，定义为算法奇点；
也就是说，如果
Figure FDA0004090628270000041
其中ρ(·)是矩阵的秩；
很明显，算法的奇异性是由次要任务和主要任务之间的任务冲突引起的；此外，基于任务优先级的冗余机械臂逆运动学旨在提供更好的控制主要任务的有效性；
因此，让位置控制方向作为主要任务，从而使位置保证控制方向任务的准确性；然后建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略方程：
Figure FDA0004090628270000042
建立奇异鲁棒解逆运动学分析模型如下：
基于雅可比伪逆解会出现运动学奇异性，这是由次矩阵引起的；对于运动奇异性问题，应该还要给出DLS(阻尼最小二乘)解；
对DLS解的成本函数可修改为：
Figure FDA0004090628270000043
因此，上述方程的奇异鲁棒伪逆解可表示为：
Figure FDA0004090628270000044
式(15)即为奇异鲁棒解逆运动学分析模型，设λ＝η2I，上述DLS解等价于附加正则化解，标量值η平衡了任务精度和奇异性；
对于雅可比矩阵伪逆解的计算，可给出雅可比矩阵的奇异值SVD分解形式
J＝UΣVT (16)
其中U∈Rm×n，V∈Rn×n，∑∈Rm×n，U是由列向量ui组成的一元矩阵，V是由列向量vi组成的一元矩阵，∑是m×n对角矩阵的块矩阵，该对角矩阵包含J的奇异值σi≥0以递减顺序包含n-m个零列向量；
Figure FDA0004090628270000051
其中，r≤m是矩阵J的秩；
对于运动奇异性，参考计算伪逆解时需要的奇异值分解SVD，大的产生的关节速度是由于最小的奇异值迅速接近0，如下式：
Figure FDA0004090628270000052
因子λ0将影响奇异性，λ0值越高，阻尼越大，联合速度就越接近奇异点；此外，定义可变阻尼因子的策略也不同；我们可得到
Figure FDA0004090628270000053
从上式中，我们可看出参数δ＞0监视最小的奇异值；
建立多任务冗余机械臂的逆优先控制策略如下；
引入逆优先投影矩阵
Figure FDA0004090628270000054
该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间，所以得出/>
Figure FDA0004090628270000055
Figure FDA0004090628270000056
其中Ji|j是与j-th任务线性无关的i-th任务的所有组件相关联的雅可比矩阵；
所以，优先级推导公式如下：
Figure FDA0004090628270000061
在上述推导中，k＝l，l-1，…，1，其中1为正整数；初始值
Figure FDA0004090628270000062
为了给出线性无关雅可比矩阵J的一般计算形式，定义逆增广Jacobian矩阵为：
Figure FDA0004090628270000063
就有了
Figure FDA0004090628270000064
其中
Figure FDA0004090628270000065
表示/>
Figure FDA0004090628270000066
的行；
于是乎，
Figure FDA0004090628270000067
的伪逆解就可表示为：
Figure FDA0004090628270000068
和
Figure FDA0004090628270000069
其中，Tk表示矩阵
Figure FDA00040906282700000610
的扩充；
最终的反向优先级投影可写为：
Figure FDA00040906282700000611
于是，我们就可得出伪逆解的表达式：
Figure FDA0004090628270000071
建立多任务冗余机械臂的逆优先控制策略方程如下：
Figure FDA0004090628270000072
对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理如下：
对于六自由度或七自由度冗余机械臂来说，没有足够的自由度来完成多个层次的任务；实行双任务优先控制是必要的；也就是说，机械手的运动控制是主要任务和次要任务；
具有主要任务和次要任务的冗余机械臂的反向控制方程如下
Figure FDA0004090628270000073
上面的公式与前面的表达式(11)有很大的不同，但是算法框架是相似的；在上面的方程中，
Figure FDA0004090628270000074
是次要任务，/>
Figure FDA0004090628270000075
是主任务；主任务在主任务的指定零空间中实现；反向优先级的核心点是投影矩阵/>
Figure FDA0004090628270000076
的计算；/>
Figure FDA0004090628270000077
的表达式如公式(30)：
Figure FDA0004090628270000078
利用前述公式(22)-(28)中的导子，可得到简化后具有主要任务和次要任务的冗余机械臂的反向控制方程：
Figure FDA0004090628270000079
建立机械手的逆优先力控制策略如下：
力控制空间中机械手的动力学可写成：
Figure FDA0004090628270000081
其中X是笛卡尔空间中的位置，M(X)是惯性矩阵，
Figure FDA0004090628270000082
是非线性力，F是输入控制力，Fe是接触力；
此外，基于雅可比矩阵的变换可得到输入关节力矩
τ＝JT(q)F (33)
操纵器在力控制空间中的期望运动方程可定义如下：
Figure FDA0004090628270000083
其中Md和Bd是惯性和阻尼矩阵；Fd是指令力，Fe是接触力；
因此，环境和操纵器响应之间的关系可写成
Figure FDA0004090628270000084
上述两个方程的组合如下
Figure FDA0004090628270000085
从上面的方程可看出，如果Me、Be和Ke已知，则Md和Bd的调整将影响系统响应；
力控制使机械手能够与环境或人类相互作用；另外，在某些情况下，没有必要实现全方位的力控制，也没有必要保证全方位的力控制，也就是说，有时我们只是想保证某个方向的力跟踪控制精度；
因此有必要对机械手进行分级力控制；也就是说，有必要给出一个新的层次力控制框架；从上面的方程我们可得到期望的层次力控制关系如下
Figure FDA0004090628270000086
Figure FDA0004090628270000087
/>
所以这两个方程的积分公式可写成
Figure FDA0004090628270000091
Figure FDA0004090628270000092
如果机械手末端执行器能够跟踪期望的笛卡尔速度为
Figure FDA0004090628270000093
和/>
Figure FDA0004090628270000094
则可实现机械手的精确力控制；笛卡尔速度与关节速度的关系应借鉴逆优先控制；因此，可得到机械手的逆优先力控制策略的方程：
Figure FDA0004090628270000095
上述方程所要求的关节速度将保证机械手的力控制；值得一提的是，上述力控制律只是速度级控制律，它依赖于内速度环控制；如果内位置控制效果良好，则可实现精确的力控制；由于内速度环控制可实现低频位置跟踪，所以外力环可实现低频力跟踪；
采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证的实现方式如下：
当机械手实施力控制时，机械手在一定程度上起到了发起者的作用，也就是说，机械手已经做好了响应外部环境的准备；当机械臂作为阻抗控制模型工作时，机械臂会被动地响应外力；
外力与关节加速度的对应阻抗关系可表示为
Figure FDA0004090628270000096
Figure FDA0004090628270000097
参考速度可表示为
Figure FDA0004090628270000101
Figure FDA0004090628270000102
因此，机械手的逆优先阻抗控制保证的表达式为：
Figure FDA0004090628270000103
将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架实现方式如下：
混合阻抗应用就是上述两种策略的结合，即笛卡尔任务可分为两种情况：第一种是位置控制子空间，阻抗控制是在该子空间中实现的；第二个是力控制子空间，力控制在该子空间中实现；
因此选择一个选择矩阵；外力与位置响应的关系如下
Figure FDA0004090628270000104
Figure FDA0004090628270000105
所以期望速度的简化形式可表示为
Figure FDA0004090628270000106
Figure FDA0004090628270000107
然后我们得到了基于反向优先级的解决方案
Figure FDA0004090628270000108
考虑到n层任务，相应的阻抗控制任务也属于n层框架，因此，机械手速度级逆优先阻抗控制的总体框架表达式如下
Figure FDA0004090628270000111
表达式(52)解决了将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算的机械手逆优先混合阻抗控制中去；能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务；
适用于空间机械手冗余机械臂逆优先级阻抗控制方法的空间机械手冗余机械臂逆优先级阻抗控制系统，所述空间机械手冗余机械臂逆优先级阻抗控制系统包括机械手和控制机械手的控制台(S31)；所述机械手包括机械臂、安装座(S1)、竖直柱(S2)、输出抓手(S12)和竖直气缸(S23)；
机械臂包括竖直升降移动台(S3)、臂段一(S6)、臂段二(S7)、臂段三(S8)和臂段四(S10)；
在竖直柱的左表面上设有竖直轨道(S24)，竖直升降移动台上下竖直滑动设置在竖直轨道上；竖直柱的下端固定连接在安装座的上表面上，竖直气缸的缸座(S21)固定连接在位于竖直轨道左方的安装座的上表面上，竖直气缸的伸缩杆(S22)竖直朝上布置，竖直气缸的伸缩杆上端固定连接在竖直升降移动台的下表面上；竖直升降移动台能在竖直气缸的伸缩杆的带动下可沿着竖直轨道上下运动，形成第一自由度；
臂段一包括A1段管(S13)和伸缩连接在A1段管的左管口内的A2段管(S14)，在A1段管内的右端固定设有伸缩杆水平朝左布置的一号气缸(S25)，一号气缸的伸缩杆固定连接在A2段管的右端；
臂段二包括B1段管(S16)和伸缩连接在B1段管的左管口内的B2段管(S17)，在B1段管内的右端固定设有伸缩杆水平朝左布置的二号气缸(37)，二号气缸的伸缩杆固定连接在B2段管的右端；
在竖直升降移动台的左端设有由一台一号减速电机(S26)驱动的一号水平旋转轴(S4)，A1段管的右端固定连接在一号水平旋转轴上，使臂段一能水平转动形成第二自由度；在一号水平旋转轴上还设有能控制一号水平旋转轴转动的一号电磁刹车(S32)；
在A2段管的左端设有由一台二号减速电机(S27)驱动的二号水平旋转轴(S15)，B1段管的右端固定连接在二号水平旋转轴上，使臂段二能水平转动形成第三自由度；在二号水平旋转轴上还设有能控制二号水平旋转轴转动的二号电磁刹车(S33)；
在B2段管的左端设有由一台三号减速电机(S28)驱动的三号水平旋转轴(S18)，臂段三的右端固定连接在三号水平旋转轴上，使臂段三能水平转动形成第四自由度；在三号水平旋转轴上还设有能控制三号水平旋转轴转动的三号电磁刹车(S34)；
在臂段三的左端设有由一台四号减速电机(S29)驱动的能在左右竖直面上转动的一号横向竖直旋转轴(S9)，臂段四的右端固定连接在一号横向竖直旋转轴上，使臂段四能在左右竖直面上竖直转动形成第五自由度；在一号横向竖直旋转轴上还设有能控制一号横向竖直旋转轴转动的四号电磁刹车(S35)；
在臂段四的左端设有由一台五号减速电机(S30)驱动的能在前后竖直面上转动的一号纵向竖直旋转轴(S11)，输出抓手的右端固定连接在一号纵向竖直旋转轴上，使输出抓手的右端能在前后竖直面上竖直转动形成第六自由度；在一号纵向竖直旋转轴上还设有能控制一号纵向竖直旋转轴转动的五号电磁刹车(S36)；
A2段管能在一号气缸的伸缩杆的带动下在A1段管内左右伸缩移动形成第七自由度；
B2段管能在二号气缸的伸缩杆的带动下在B1段管内左右伸缩移动形成第八自由度；
一根一号水平管(S39)的左端水平固定连接在竖直柱的右表面上，在一号水平管内左右滑动设有平衡调节块(S40)，在一号水平管内的左端固定连接有伸缩杆水平朝右的平衡调节气缸(S38)，平衡调节气缸的伸缩杆的右端固定连接在平衡调节块上；
一号电磁刹车的控制端、二号电磁刹车的控制端、三号电磁刹车的控制端、四号电磁刹车的控制端、五号电磁刹车的控制端、一号减速电机的控制端、二号减速电机的控制端、三号减速电机的控制端、四号减速电机的控制端、五号减速电机的控制端、一号气缸的控制端、二号气缸的控制端、平衡调节气缸的控制端和竖直气缸的控制端分别控制连接在控制台上。
CN202010369737.6A 2020-04-30 2020-04-30 空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法 Active CN111687832B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010369737.6A CN111687832B (zh) 2020-04-30 2020-04-30 空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010369737.6A CN111687832B (zh) 2020-04-30 2020-04-30 空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法

Publications (2)

Publication Number Publication Date
CN111687832A CN111687832A (zh) 2020-09-22
CN111687832B true CN111687832B (zh) 2023-06-02

Family

ID=72476988

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010369737.6A Active CN111687832B (zh) 2020-04-30 2020-04-30 空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法

Country Status (1)

Country Link
CN (1) CN111687832B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114083519B (zh) * 2021-12-31 2023-05-09 北京敏锐达致机器人科技有限责任公司 机器人零空间拖动示教方法及示教装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4999553A (en) * 1989-12-28 1991-03-12 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Method and apparatus for configuration control of redundant robots
CN108247631A (zh) * 2017-12-06 2018-07-06 西北工业大学 一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法
CN108839025A (zh) * 2018-07-12 2018-11-20 杭州电子科技大学 一种移动机械臂的运动规划方法和装置
CN110104216A (zh) * 2019-01-28 2019-08-09 西北工业大学深圳研究院 一种针对运动学冗余双臂空间机器人的协同路径规划方法

Family Cites Families (22)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS6416389A (en) * 1987-07-11 1989-01-19 Agency Ind Science Techn Control system of multi-joint type arm robot having redundancy
EP1854425A1 (de) * 2006-05-11 2007-11-14 BrainLAB AG Medizintechnische Positionsbestimmung mit redundanten Positionserfassungseinrichtungen und Prioritätsgewichtung für die Positionserfassungseinrichtungen
JP4508164B2 (ja) * 2006-06-26 2010-07-21 トヨタ自動車株式会社 多関節ロボット及びその制御プログラム
JP2009066685A (ja) * 2007-09-11 2009-04-02 Sony Corp ロボット装置及びロボット装置の制御方法
US8170718B2 (en) * 2008-12-18 2012-05-01 GM Global Technology Operations LLC Multiple priority operational space impedance control
US8364314B2 (en) * 2009-04-30 2013-01-29 GM Global Technology Operations LLC Method and apparatus for automatic control of a humanoid robot
JP2011183527A (ja) * 2010-03-10 2011-09-22 Toyota Motor Corp 冗長ロボットの関節目標値決定方法及び冗長ロボットの制御装置
CN205363898U (zh) * 2015-12-31 2016-07-06 中国科学院沈阳自动化研究所 可扩展的模块化机械臂
CN105676636B (zh) * 2016-01-11 2018-12-07 北京邮电大学 一种基于nsga-ii算法的冗余度空间机械臂多目标优化方法
CN107650120B (zh) * 2016-07-26 2022-04-19 深圳华清精密科技有限公司 一种用于确定9自由度机械臂的全部奇异构型的方法
DE102016222255B3 (de) * 2016-11-14 2018-04-12 Kuka Roboter Gmbh Roboterarm, mobiler Roboter und Logistiksystem
CN106708069B (zh) * 2017-01-19 2020-01-10 中国科学院自动化研究所 水下移动作业机器人的协调规划与控制方法
CN107290956B (zh) * 2017-08-01 2019-08-20 浙江大学 一种基于状态反馈的单关节柔性机械臂的位置控制方法
CN107351085B (zh) * 2017-08-21 2019-11-08 西北工业大学 一种基于多个控制点的空间机器人避撞方法
CN108153153B (zh) * 2017-12-19 2020-09-11 哈尔滨工程大学 一种学习变阻抗控制系统及控制方法
CN108241339B (zh) * 2017-12-27 2020-09-04 北京航空航天大学 仿人机械臂的运动求解和构型控制方法
CN108621163A (zh) * 2018-05-10 2018-10-09 同济大学 一种面向汇榫工艺的冗余双臂机器人协作控制方法
CN109079780B (zh) * 2018-08-08 2020-11-10 北京理工大学 基于广义坐标的分布式移动机械臂任务分层优化控制方法
CN209631457U (zh) * 2018-12-17 2019-11-15 清研同创机器人（天津）有限公司 一种全向移动式机器人喷涂系统
CN110497378A (zh) * 2019-09-04 2019-11-26 上海海事大学 一种用于零件加工的自动上下料机器人
CN110524522B (zh) * 2019-09-19 2024-03-26 浙江工业大学 一种多自由度冗余串并联拟人机械臂
CN111538234B (zh) * 2020-07-08 2020-10-09 深圳市优必选科技股份有限公司 任务分层控制方法、装置、机器人和可读存储介质

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4999553A (en) * 1989-12-28 1991-03-12 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Method and apparatus for configuration control of redundant robots
CN108247631A (zh) * 2017-12-06 2018-07-06 西北工业大学 一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法
CN108839025A (zh) * 2018-07-12 2018-11-20 杭州电子科技大学 一种移动机械臂的运动规划方法和装置
CN110104216A (zh) * 2019-01-28 2019-08-09 西北工业大学深圳研究院 一种针对运动学冗余双臂空间机器人的协同路径规划方法

Also Published As

Publication number Publication date
CN111687832A (zh) 2020-09-22

Similar Documents

Publication Publication Date Title
Petit et al. Cartesian impedance control for a variable stiffness robot arm
CN105974797A (zh) 考虑弹性影响与补偿的绳牵引并联机器人运动控制方法
Peng et al. Compliant motion control of kinematically redundant manipulators
CN108638058B (zh) 一种姿态决策动态规划方法
Chen et al. Optimal, fault-tolerant mappings to achieve secondary goals without compromising primary performance
CN111687835B (zh) 水下机械手冗余机械臂逆优先级阻抗控制系统及控制方法
De Lasa et al. Prioritized optimization for task-space control
Nemec et al. Force control of redundant robots in unstructured environment
Yan et al. A 5-DOF redundantly actuated parallel mechanism for large tilting five-face machining
Chen Reconfiguration of a parallel kinematic manipulator for the maximum dynamic load-carrying capacity
Dubowsky et al. The control of space manipulators subject to spacecraft attitude control saturation limits
Chung et al. Torque optimizing control with singularity-robustness for kinematically redundant robots
CN109623812A (zh) 考虑航天器本体姿态运动的机械臂轨迹规划方法
CN111805536A (zh) 一种水果分拣并联机器人机构考虑耦合作用自适应滑模控制方法
CN111687832B (zh) 空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法
Wimböck et al. Experimental study on dynamic reactionless motions with DLR's humanoid robot Justin
CN111687834B (zh) 移动机械手冗余机械臂逆优先级阻抗控制系统及控制方法
CN111702753B (zh) 冗余机械臂逆优先级阻抗控制系统及控制方法
Saab et al. Design and analysis of a discrete modular serpentine robotic tail for improved performance of mobile robots
Nagano et al. Simplification of motion generation in the singular configuration of a wheel-legged mobile robot
CN111687833B (zh) 机械手逆优先级阻抗控制系统及控制方法
Seraji Motion control of mobile manipulators
Qi et al. Generalized flexible hybrid cable-driven robot (HCDR): Modeling, control, and analysis
Pajak et al. Planning of a point to point collision-free trajectory for mobile manipulators
Park et al. Reliable computation of minimum‐time motions for manipulators moving in obstacle fields using a successive search for minimum‐overload trajectories

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant