CN111687833B - 机械手逆优先级阻抗控制系统及控制方法 - Google Patents

机械手逆优先级阻抗控制系统及控制方法 Download PDF

Info

Publication number
CN111687833B
CN111687833B CN202010369756.9A CN202010369756A CN111687833B CN 111687833 B CN111687833 B CN 111687833B CN 202010369756 A CN202010369756 A CN 202010369756A CN 111687833 B CN111687833 B CN 111687833B
Authority
CN
China
Prior art keywords
control
manipulator
task
inverse
priority
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
CN202010369756.9A
Other languages
English (en)
Other versions
CN111687833A (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 CN202010369756.9A priority Critical patent/CN111687833B/zh
Publication of CN111687833A publication Critical patent/CN111687833A/zh
Application granted granted Critical
Publication of CN111687833B publication Critical patent/CN111687833B/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
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • 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]

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了机械手逆优先级阻抗控制系统及控制方法。属于机械手冗余机械臂逆优先级阻抗控制技术领域，便于识别抓取物体。包括可移动的移动平台、机械手和控制机械手的控制台；所述机械手包括机械臂、安装座、竖直柱、输出抓手和竖直气缸；机械臂包括竖直升降移动台、臂段一、臂段二、臂段三和臂段四；在竖直柱的左表面上设有竖直轨道，竖直升降移动台上下竖直滑动设置在竖直轨道上；竖直柱的下端固定连接在安装座的上表面上，竖直气缸的缸座固定连接在位于竖直轨道左方的安装座的上表面上，竖直气缸的伸缩杆上端固定连接在竖直升降移动台的下表面上；安装座固定在移动平台上。在机械手前端设有摄像头。

Description

机械手逆优先级阻抗控制系统及控制方法
技术领域
本发明涉及机械手冗余机械臂逆优先级阻抗控制技术领域，尤其涉及机械手逆优先级阻抗控制系统及控制方法。
背景技术
目前工业机器人采用的控制方法是把机械手上每一个关节都当作一个单独的伺服机构，即每个轴对应一个伺服器，每个伺服器通过总线控制，由控制器统一控制并协调工作；
六个自由度的机械臂是具有完成空间定位能力最小自由度数的机械臂，多于六个自由度的机械臂统一称为冗余机械臂；
现在机械手的机械臂阻抗控制方法在不同层次结构下不能实现期望的阻抗控制任务，因此，涉及一种能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务的方法显得非常必要。
发明内容
本发明是为了解决现有机械臂阻抗控制方法在不同层次结构下不能实现期望的阻抗控制任务的不足，提供一种一是能控制机械手的平衡，机械手便于移动，便于识别抓取物体，使用灵活；二是能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务的机械手逆优先级阻抗控制系统及控制方法。
以上技术问题是通过下列技术方案解决的：
机械手逆优先级阻抗控制系统，包括机械手和控制机械手的控制台；还包括可移动的移动平台；所述机械手包括机械臂、安装座、竖直柱、输出抓手和竖直气缸；安装座固定在移动平台上；
机械臂包括竖直升降移动台、基臂段、臂段一、臂段二、臂段三和臂段四；
在竖直柱的左表面上设有竖直轨道，竖直升降移动台上下竖直滑动设置在竖直轨道上；竖直柱的下端固定连接在安装座的上表面上，竖直气缸的缸座固定连接在位于竖直轨道左方的安装座的上表面上，竖直气缸的伸缩杆竖直朝上布置，竖直气缸的伸缩杆上端固定连接在竖直升降移动台的下表面上；竖直升降移动台能在竖直气缸的伸缩杆的带动下可沿着竖直轨道上下运动，形成第一自由度；
臂段一包括A1段管和伸缩连接在A1段管的左管口内的A2段管，在A1段管内的右端固定设有伸缩杆水平朝左布置的一号气缸，一号气缸的伸缩杆固定连接在A2段管的右端；
臂段二包括B1段管和伸缩连接在B1段管的左管口内的B2段管，在B1段管内的右端固定设有伸缩杆水平朝左布置的二号气缸，二号气缸的伸缩杆固定连接在B2段管的右端；
在竖直升降移动台的左端设有由一台一号减速电机驱动的一号水平旋转轴，
基臂段的右端固定连接在一号水平旋转轴上，使基臂段能水平转动形成第二自由度；在一号水平旋转轴上还设有能控制一号水平旋转轴转动的一号电磁刹车；
在基臂段的左端设有由一台基减速电机驱动的A号横向竖直旋转轴，A1段管的右端固定连接在A号横向竖直旋转轴上，使臂段二能水平转动形成第三自由度；在A号横向竖直旋转轴上还设有能控制A号横向竖直旋转轴转动的A号电磁刹车；
在A2段管的左端设有由一台二号减速电机驱动的二号水平旋转轴，B1段管的右端固定连接在二号水平旋转轴上，使臂段二能水平转动形成第四自由度；在二号水平旋转轴上还设有能控制二号水平旋转轴转动的二号电磁刹车；
在B2段管的左端设有由一台三号减速电机驱动的三号水平旋转轴，臂段三的右端固定连接在三号水平旋转轴上，使臂段三能水平转动形成第五自由度；在三号水平旋转轴上还设有能控制三号水平旋转轴转动的三号电磁刹车；
在臂段三的左端设有由一台四号减速电机驱动的能在左右竖直面上转动的一号横向竖直旋转轴，臂段四的右端固定连接在一号横向竖直旋转轴上，使臂段四能在左右竖直面上竖直转动形成第六自由度；在一号横向竖直旋转轴上还设有能控制一号横向竖直旋转轴转动的四号电磁刹车；
在臂段四的左端设有由一台五号减速电机驱动的能在前后竖直面上转动的一号纵向竖直旋转轴，输出抓手的右端固定连接在一号纵向竖直旋转轴上，使输出抓手的右端能在前后竖直面上竖直转动形成第七自由度；在一号纵向竖直旋转轴上还设有能控制一号纵向竖直旋转轴转动的五号电磁刹车；
A2段管能在一号气缸的伸缩杆的带动下在A1段管内左右伸缩移动形成第八自由度；
B2段管能在二号气缸的伸缩杆的带动下在B1段管内左右伸缩移动形成第九自由度；
一根一号水平管的左端水平固定连接在竖直柱的右表面上，在一号水平管内左右滑动设有平衡调节块，在一号水平管内的左端固定连接有伸缩杆水平朝右的平衡调节气缸，平衡调节气缸的伸缩杆的右端固定连接在平衡调节块上；
基减速电机的控制端、A号电磁刹车的控制端、一号电磁刹车的控制端、二号电磁刹车的控制端、三号电磁刹车的控制端、四号电磁刹车的控制端、五号电磁刹车的控制端、一号减速电机的控制端、二号减速电机的控制端、三号减速电机的控制端、四号减速电机的控制端、五号减速电机的控制端、一号气缸的控制端、二号气缸的控制端、平衡调节气缸的控制端和竖直气缸的控制端分别控制连接在控制台上。
在一号纵向竖直旋转轴的外表面上固定套设有环形圈，在环形圈上设有由一台环绕电机驱动能沿着环形圈移动的一号移动块，在一号移动块上设有摄像头；摄像头的控制端和环绕电机的控制端分别与控制台相连接。便于识别抓取物体
平衡调节气缸对平衡调节块进行左右移动控制就能控制竖直柱的平衡，进而控制机械手的平衡。便于机械手移动。由于本发明有的机械手有八个自由度，灵活性好，使用灵活，可靠性高，易于完成控制任务。
一种移动机械手冗余机械臂逆优先级阻抗控制方法，包括如下步骤：
步骤1、建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略；
步骤2、建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略；
步骤3、建立奇异鲁棒解逆运动学分析模型；
步骤4、建立多任务冗余机械臂的逆优先控制策略；
步骤5、对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理；
步骤6、建立机械手的逆优先力控制策略；
步骤7、采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证；
步骤8、将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架。
本方案冗余机械臂在关节空间中的运动是按照相反的顺序导出的；然后将笛卡尔阻抗控制与逆优先阻抗控制相结合，解决了逆分层阻抗控制问题，将笛卡尔阻抗控制行为分为高优先阻抗控制和低优先阻抗控制。其中高优先阻抗控制任务不会干扰低优先阻抗控制任务，关节空间中的运动按相反的顺序会受到影响，要在相应的投影算子中工作；最后实现了高优先阻抗控制任务，避免了低优先阻抗控制任务中可能出现的奇异性引起的变形。因此，所提出的逆优先阻抗控制方法可使冗余机械臂在适当的层次结构下实现期望的阻抗控制任务。
作为优选，建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略实现过程如下：
定义末端执行器在笛卡尔空间的位姿、速度分别为x、
Figure GDA0004090629800000036
其关节空间的角位置、角速度分别为q、/>
Figure GDA0004090629800000031
J为n自由度机器人的雅可比矩阵，其中x∈Rn，/>
Figure GDA0004090629800000032
Figure GDA0004090629800000033
J∈Rmn；冗余自由度机械臂的正运动学方程可用下式描述：
Figure GDA0004090629800000034
式(1)也被称为机械臂运动学速度模型；
考虑到最小二乘法的解，最优问题可列为：
Figure GDA0004090629800000035
式(1)的解可通过寻找最佳
Figure GDA0004090629800000041
来解决；
Figure GDA0004090629800000042
因此，式(1)的伪逆解可表示为：
Figure GDA0004090629800000043
式中J+——雅可比矩阵的伪逆
I——单位矩阵
Figure GDA0004090629800000044
——任意零空间矢量
Figure GDA0004090629800000045
——最小范数解，定义了手的动作
Figure GDA0004090629800000046
一一齐次解，在末端不产生动作
式(4)代表了末端执行器的位置和姿态控制；在式(4)中加入任意残差，可得到包含零空间的一般表达式；利用上述方程可在零向量上实现多任务优化；
然而，上述方程忽略了雅可比矩阵的病态；正则化方程可通过添加额外的正则化值来修改，
Figure GDA0004090629800000047
其中λ≥0是加权矩阵，
Figure GDA0004090629800000048
为加权系数，并且满足
Figure GDA0004090629800000049
上述方程的解就可表示为：
Figure GDA00040906298000000410
式(7)也被称为冗余机械臂运动学模型；
冗余机械臂零空间向量的位置相关标量指数的关节限制梯度方向的关节限制函数为：
Figure GDA00040906298000000411
作为优选，建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略如下：
在雅可比矩阵的冗余机械臂求解中，优化任务是在主任务的零空间中实现的；反向任务运动学是在正向任务运动学基础上建立的：
Figure GDA00040906298000000412
其中
Figure GDA00040906298000000413
和/>
Figure GDA00040906298000000414
表示task1和task2
从表达式(5)中得出冗余机械臂的逆运动学方程为：
Figure GDA00040906298000000415
Task1作为主任务，task2作为辅助任务；也就是说，
Figure GDA00040906298000000416
是在/>
Figure GDA00040906298000000417
的零空间中实现的；冗余机械臂的最终逆运动学表达式如下所示：
Figure GDA0004090629800000051
其中
Figure GDA0004090629800000052
Figure GDA0004090629800000053
是投影矩阵，它给出了次要任务到主要任务的适用范围；/>
Figure GDA0004090629800000054
和/>
Figure GDA0004090629800000055
是所需的命令速度；/>
Figure GDA0004090629800000056
是主要任务，/>
Figure GDA0004090629800000057
是次要任务；
如果两个相关任务是相互依赖的，则相应的雅可比矩阵是奇异的；如果任务雅可比矩阵是奇异的，则相应的任务是不满足的；在这种情况下，雅可比相关矩阵将是奇点，定义为算法奇点；
也就是说，如果
Figure GDA0004090629800000058
其中ρ(·)是矩阵的秩；
很明显，算法的奇异性是由次要任务和主要任务之间的任务冲突引起的；此外，基于任务优先级的冗余机械臂逆运动学旨在提供更好的控制主要任务的有效性；
因此，让位置控制方向作为主要任务，从而使位置保证控制方向任务的准确性；然后建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略方程：
Figure GDA0004090629800000059
作为优选，建立奇异鲁棒解逆运动学分析模型如下：
基于雅可比伪逆解会出现运动学奇异性，这是由次矩阵引起的；对于运动奇异性问题，应该还要给出DLS(阻尼最小二乘)解；
对DLS解的成本函数可修改为：
Figure GDA00040906298000000510
因此，上述方程的奇异鲁棒伪逆解可表示为：
Figure GDA00040906298000000511
式(15)即为奇异鲁棒解逆运动学分析模型，设λ＝η2I，上述DLS解等价于附加正则化解，标量值η平衡了任务精度和奇异性；
对于雅可比矩阵伪逆解的计算，可给出雅可比矩阵的奇异值SVD分解形式
J＝UΣVT (16)
其中U∈∈Bm×m，V∈Rn×n，∑∈Rm×n，U是由列向量ui组成的一元矩阵，V是由列向量vi组成的一元矩阵，∑是m×n对角矩阵的块矩阵，该对角矩阵包含J的奇异值σi≥0以递减顺序包含n-m个零列向量；
Figure GDA00040906298000000512
其中，r≤m是矩阵J的秩；
对于运动奇异性，参考计算伪逆解时需要的奇异值分解SVD，大的产生的关节速度是由于最小的奇异值迅速接近0，如下式：
Figure GDA0004090629800000061
因子λ0将影响奇异性，λ0值越高，阻尼越大，联合速度就越接近奇异点；此外，定义可变阻尼因子的策略也不同；我们可得到
Figure GDA0004090629800000062
从上式中，我们可看出参数δ＞0监视最小的奇异值。
作为优选，建立多任务冗余机械臂的逆优先控制策略如下；
引入逆优先投影矩阵
Figure GDA0004090629800000063
该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间，所以得出
Figure GDA0004090629800000064
/>
Figure GDA0004090629800000065
其中Ji|j是与j-th任务线性无关的i-th任务的所有组件相关联的雅可比矩阵；
所以，优先级推导公式如下：
Figure GDA0004090629800000066
在上述推导中，k＝l，l-1，…，1；初始值
Figure GDA0004090629800000067
为了给出线性无关雅可比矩阵J的一般计算形式，定义逆增广Jacobian矩阵为：
Figure GDA0004090629800000068
就有了
Figure GDA0004090629800000069
其中
Figure GDA00040906298000000610
表示/>
Figure GDA00040906298000000611
的行；
于是乎，
Figure GDA00040906298000000612
的伪逆解就可表示为：
Figure GDA00040906298000000613
和
Figure GDA00040906298000000614
其中，Tk表示矩阵
Figure GDA0004090629800000071
的扩充；
最终的反向优先级投影可写为：
Figure GDA0004090629800000072
于是，我们就可得出伪逆解的表达式：
Figure GDA0004090629800000073
建立多任务冗余机械臂的逆优先控制策略方程如下：
Figure GDA0004090629800000074
作为优选，对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理如下：
对于六自由度或七自由度冗余机械臂来说，没有足够的六自由度DOF来完成多个层次的任务；实行双任务优先控制是必要的；也就是说，机械手的运动控制是主要任务和次要任务；
具有主要任务和次要任务的冗余机械臂的反向控制方程如下
Figure GDA0004090629800000075
/>
上面的公式与前面的表达式(11)有很大的不同，但是算法框架是相似的；在上面的方程中，
Figure GDA0004090629800000076
是次要任务，/>
Figure GDA0004090629800000077
是主任务；主任务在主任务的指定零空间中实现；反向优先级的核心点是投影矩阵/>
Figure GDA0004090629800000078
的计算；/>
Figure GDA0004090629800000079
的表达式如公式(30)：
Figure GDA00040906298000000710
利用前述公式(22)-(28)中的导子，可得到简化后具有主要任务和次要任务的冗余机械臂的反向控制方程：
Figure GDA00040906298000000711
作为优选，建立机械手的逆优先力控制策略如下：
力控制空间中机械手的动力学可写成：
Figure GDA00040906298000000712
其中X是笛卡尔空间中的位置，M(X)是惯性矩阵，
Figure GDA00040906298000000713
是非线性力，F是输入控制力，Fe是接触力；
此外，基于雅可比矩阵的变换可得到输入关节力矩
τ＝JT(q)F (33)
操纵器在力控制空间中的期望运动方程可定义如下：
Figure GDA00040906298000000714
其中Md和Bd是惯性和阻尼矩阵；Fd是指令力，Fe是接触力；
因此，环境和操纵器响应之间的关系可写成
Figure GDA0004090629800000081
上述两个方程的组合如下
Figure GDA0004090629800000082
从上面的方程可看出，如果Me、Be和Ke已知，则Md和Bd的调整将影响系统响应；
力控制使机械手能够与环境或人类相互作用；另外，在某些情况下，没有必要实现全方位的力控制，也没有必要保证全方位的力控制，也就是说，有时我们只是想保证某个方向的力跟踪控制精度；
因此有必要对机械手进行分级力控制；也就是说，有必要给出一个新的层次力控制框架；从上面的方程我们可得到期望的层次力控制关系如下
Figure GDA0004090629800000083
Figure GDA0004090629800000084
所以这两个方程的积分公式可写成
Figure GDA0004090629800000085
Figure GDA0004090629800000086
如果机械手末端执行器能够跟踪期望的笛卡尔速度为
Figure GDA0004090629800000087
和/>
Figure GDA0004090629800000088
则可实现机械手的精确力控制；笛卡尔速度与关节速度的关系应借鉴逆优先控制；因此，可得到机械手的逆优先力控制策略的方程：
Figure GDA0004090629800000089
上述方程所要求的关节速度将保证机械手的力控制；值得一提的是，上述力控制律只是速度级控制律，它依赖于内速度环控制；如果内位置控制效果良好，则可实现精确的力控制；由于内速度环控制可实现低频位置跟踪，所以外力环可实现低频力跟踪。
作为优选，采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证的实现方式如下：
当机械手实施力控制时，机械手在一定程度上起到了发起者的作用，也就是说，机械手已经做好了响应外部环境的准备；当机械臂作为阻抗控制模型工作时，机械臂会被动地响应外力；
外力与关节加速度的对应阻抗关系可表示为
Figure GDA00040906298000000810
Figure GDA0004090629800000091
参考速度可表示为
Figure GDA0004090629800000092
Figure GDA0004090629800000093
因此，机械手的逆优先阻抗控制保证的表达式为：
Figure GDA0004090629800000094
作为优选，将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架实现方式如下：
混合阻抗应用就是上述两种策略的结合，即笛卡尔任务可分为两种情况：第一种是位置控制子空间，阻抗控制是在该子空间中实现的；第二个是力控制子空间，力控制在该子空间中实现；
因此选择一个选择矩阵；外力与位置响应的关系如下
Figure GDA0004090629800000095
Figure GDA0004090629800000096
所以期望速度的简化形式可表示为
Figure GDA0004090629800000097
Figure GDA0004090629800000098
然后我们得到了基于反向优先级的解决方案
Figure GDA0004090629800000099
考虑到n层任务，相应的阻抗控制任务也属于n层框架，因此，机械手速度级逆优先阻抗控制的总体框架表达式如下
Figure GDA00040906298000000910
表达式(52)解决了将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算的机械手逆优先混合阻抗控制中去，能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务。
本发明能够达到如下效果：
本发明能控制机械手的平衡，机械手便于移动，便于识别抓取物体，使用灵活，能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务。
附图说明
图1为本发明力控制的动力学方案示意图。
图2为本发明阻抗控制的动力学方案示意图。
图3为本发明混合阻抗控制的动力学方案示意图。
图4为本发明实施例的一种七自由度机械手连接结构示意图。
图5为本发明实施例的一种电路原理连接结构示意框图。
具体实施方式
下面结合附图与实施例对本发明作进一步的说明。
实施例，机械手逆优先级阻抗控制系统，参见图4、图5所示，包括机械手和控制机械手的控制台S31；还包括可移动的移动平台S41；所述机械手包括机械臂、安装座S1、竖直柱S2、输出抓手S12和竖直气缸S23；安装座固定在移动平台上；
机械臂包括竖直升降移动台S3、基臂段S49、臂段一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。
基臂段的右端固定连接在一号水平旋转轴上，使基臂段能水平转动形成第二自由度；在一号水平旋转轴上还设有能控制一号水平旋转轴转动的一号电磁刹车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；摄像头的控制端和环绕电机的控制端分别与控制台相连接。
摄像头能沿着输出抓手转动，便于抓取物体。
第八自由度能让机械臂自由伸长，第九自由度能让机械臂自由伸长，大大增加了作业范围和灵活性。
平衡调节气缸对平衡调节块进行左右移动控制就能控制竖直柱的平衡，进而控制机械手的平衡。机械手便于移动。移动平台包括汽车。
由于本发明有的机械手有八个自由度，灵活性好，可靠性高，易于完成控制任务。
移动机械手冗余机械臂逆优先级阻抗控制方法，参见图1-3所示。包括如下步骤：
步骤1、建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略实现过程如下：
定义末端执行器在笛卡尔空间的位姿、速度分别为x、
Figure GDA0004090629800000121
其关节空间的角位置、角速度分别为q、/>
Figure GDA0004090629800000122
J为n自由度机器人的雅可比矩阵，其中x∈Rn，/>
Figure GDA0004090629800000123
Figure GDA0004090629800000124
J∈Rmn；冗余自由度机械臂的正运动学方程可用下式描述：
Figure GDA0004090629800000125
式(1)也被称为机械臂运动学速度模型；
考虑到最小二乘法的解，最优问题可列为：
Figure GDA0004090629800000126
式(1)的解可通过寻找最佳
Figure GDA0004090629800000127
来解决；
Figure GDA0004090629800000128
因此，式(1)的伪逆解可表示为：
Figure GDA0004090629800000129
式中J+——雅可比矩阵的伪逆
I——单位矩阵
Figure GDA00040906298000001210
——任意零空间矢量
Figure GDA00040906298000001211
——最小范数解，定义了手的动作
Figure GDA00040906298000001212
——齐次解，在末端不产生动作
式(4)代表了末端执行器的位置和姿态控制；在式(4)中加入任意残差，可得到包含零空间的一般表达式；利用上述方程可在零向量上实现多任务优化；
然而，上述方程忽略了雅可比矩阵的病态；正则化方程可通过添加额外的正则化值来修改，
Figure GDA00040906298000001213
/>
其中λ≥0是加权矩阵，
Figure GDA00040906298000001214
为加权系数，并且满足
Figure GDA00040906298000001215
上述方程的解就可表示为：
Figure GDA00040906298000001216
式(7)也被称为冗余机械臂运动学模型；
冗余机械臂零空间向量的位置相关标量指数的关节限制梯度方向的关节限制函数为：
Figure GDA0004090629800000131
步骤2、建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略如下：
在雅可比矩阵的冗余机械臂求解中，优化任务是在主任务的零空间中实现的；反向任务运动学是在正向任务运动学基础上建立的：
Figure GDA0004090629800000132
其中
Figure GDA0004090629800000133
和/>
Figure GDA0004090629800000134
表示task1和task2
从表达式(5)中得出冗余机械臂的逆运动学方程为：
Figure GDA0004090629800000135
Task1作为主任务，task2作为辅助任务；也就是说，
Figure GDA0004090629800000136
是在/>
Figure GDA0004090629800000137
的零空间中实现的；冗余机械臂的最终逆运动学表达式如下所示：
Figure GDA0004090629800000138
其中
Figure GDA0004090629800000139
Figure GDA00040906298000001310
是投影矩阵，它给出了次要任务到主要任务的适用范围；/>
Figure GDA00040906298000001311
和/>
Figure GDA00040906298000001312
是所需的命令速度；/>
Figure GDA00040906298000001313
是主要任务，/>
Figure GDA00040906298000001314
是次要任务；
如果两个相关任务是相互依赖的，则相应的雅可比矩阵是奇异的；如果任务雅可比矩阵是奇异的，则相应的任务是不满足的；在这种情况下，雅可比相关矩阵将是奇点，定义为算法奇点；
也就是说，如果
Figure GDA00040906298000001315
其中ρ(·)是矩阵的秩；
很明显，算法的奇异性是由次要任务和主要任务之间的任务冲突引起的；此外，基于任务优先级的冗余机械臂逆运动学旨在提供更好的控制主要任务的有效性；
因此，让位置控制方向作为主要任务，从而使位置保证控制方向任务的准确性；然后建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略方程：
Figure GDA00040906298000001316
步骤3、建立奇异鲁棒解逆运动学分析模型如下：
基于雅可比伪逆解会出现运动学奇异性，这是由次矩阵引起的；对于运动奇异性问题，应该还要给出DLS(阻尼最小二乘)解；
对DLS解的成本函数可修改为：
Figure GDA0004090629800000141
因此，上述方程的奇异鲁棒伪逆解可表示为：
Figure GDA0004090629800000142
式(15)即为奇异鲁棒解逆运动学分析模型，设λ＝η2I，上述DLS解等价于附加正则化解，标量值η平衡了任务精度和奇异性；
对于雅可比矩阵伪逆解的计算，可给出雅可比矩阵的奇异值SVD分解形式
J＝UΣVT (16)
其中U∈Bm×m，V∈Rn×n，∑∈Bm×n，U是由列向量ui组成的一元矩阵，V是由列向量vi组成的一元矩阵，∑是m×n对角矩阵的块矩阵，该对角矩阵包含J的奇异值σi≥0以递减顺序包含n-m个零列向量；
Figure GDA0004090629800000143
其中，r≤m是矩阵J的秩；
对于运动奇异性，参考计算伪逆解时需要的奇异值分解SVD，大的产生的关节速度是由于最小的奇异值迅速接近0，如下式：
Figure GDA0004090629800000144
因子λ0将影响奇异性，λ0值越高，阻尼越大，联合速度就越接近奇异点；此外，定义可变阻尼因子的策略也不同；我们可得到
Figure GDA0004090629800000145
从上式中，我们可看出参数δ＞0监视最小的奇异值。
步骤4、建立多任务冗余机械臂的逆优先控制策略如下；
引入逆优先投影矩阵
Figure GDA0004090629800000146
该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间，所以得出
Figure GDA0004090629800000147
Figure GDA0004090629800000148
其中Ji|j是与j-th任务线性无关的i-th任务的所有组件相关联的雅可比矩阵；
所以，优先级推导公式如下：
Figure GDA0004090629800000149
在上述推导中，k＝l，l-1，…，1；初始值
Figure GDA0004090629800000151
为了给出线性无关雅可比矩阵J的一般计算形式，定义逆增广Jacobian矩阵为：
Figure GDA0004090629800000152
就有了
Figure GDA0004090629800000153
其中
Figure GDA0004090629800000154
表示/>
Figure GDA0004090629800000155
的行；
于是乎，
Figure GDA0004090629800000156
的伪逆解就可表示为：
Figure GDA0004090629800000157
和
Figure GDA0004090629800000158
其中，TR表示矩阵
Figure GDA0004090629800000159
的扩充；
最终的反向优先级投影可写为：
Figure GDA00040906298000001510
于是，我们就可得出伪逆解的表达式：
Figure GDA00040906298000001511
建立多任务冗余机械臂的逆优先控制策略方程如下：
Figure GDA00040906298000001512
步骤5、对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理如下：
对于六自由度或七自由度冗余机械臂来说，没有足够的六自由度DOF来完成多个层次的任务；实行双任务优先控制是必要的；也就是说，机械手的运动控制是主要任务和次要任务；
具有主要任务和次要任务的冗余机械臂的反向控制方程如下
Figure GDA00040906298000001513
上面的公式与前面的表达式(11)有很大的不同，但是算法框架是相似的；在上面的方程中，
Figure GDA00040906298000001514
是次要任务，/>
Figure GDA00040906298000001515
是主任务；主任务在主任务的指定零空间中实现；反向优先级的核心点是投影矩阵/>
Figure GDA0004090629800000161
的计算；/>
Figure GDA0004090629800000162
的表达式如公式(30)：/>
Figure GDA0004090629800000163
利用前述公式(22)-(28)中的导子，可得到简化后具有主要任务和次要任务的冗余机械臂的反向控制方程：
Figure GDA0004090629800000164
步骤6、建立机械手的逆优先力控制策略如下：
力控制空间中机械手的动力学可写成：
Figure GDA0004090629800000165
其中X是笛卡尔空间中的位置，M(X)是惯性矩阵，
Figure GDA0004090629800000166
是非线性力，F是输入控制力，Fe是接触力；
此外，基于雅可比矩阵的变换可得到输入关节力矩
τ＝JT(q)F (33)
操纵器在力控制空间中的期望运动方程可定义如下：
Figure GDA0004090629800000167
其中Md和Bd是惯性和阻尼矩阵；Fd是指令力，Fe是接触力；
力控制的动力学方案如图1所示；
因此，环境和操纵器响应之间的关系可写成
Figure GDA0004090629800000168
上述两个方程的组合如下
Figure GDA0004090629800000169
从上面的方程可看出，如果Me、Be和Ke已知，则Md和Bd的调整将影响系统响应；
力控制使机械手能够与环境或人类相互作用；另外，在某些情况下，没有必要实现全方位的力控制，也没有必要保证全方位的力控制，也就是说，有时我们只是想保证某个方向的力跟踪控制精度；
例如，当机械手与刨床相互作用时，只需在垂直方向保持精确的力跟踪控制，而另一个方向不需要精确的力跟踪控制；在其他情况下，位置方向力控制比姿态方向力控制更为重要；
因此有必要对机械手进行分级力控制；也就是说，有必要给出一个新的层次力控制框架；从上面的方程我们可得到期望的层次力控制关系如下
Figure GDA00040906298000001610
Figure GDA00040906298000001611
所以这两个方程的积分公式可写成
Figure GDA00040906298000001612
Figure GDA0004090629800000171
如果机械手末端执行器能够跟踪期望的笛卡尔速度为
Figure GDA0004090629800000172
和/>
Figure GDA0004090629800000173
则可实现机械手的精确力控制；笛卡尔速度与关节速度的关系应借鉴逆优先控制；因此，可得到机械手的逆优先力控制策略的方程：
Figure GDA0004090629800000174
上述方程所要求的关节速度将保证机械手的力控制；值得一提的是，上述力控制律只是速度级控制律，它依赖于内速度环控制；如果内位置控制效果良好，则可实现精确的力控制；由于内速度环控制可实现低频位置跟踪，所以外力环可实现低频力跟踪。
步骤7、采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证的实现方式如下：
当机械手实施力控制时，机械手在一定程度上起到了发起者的作用，也就是说，机械手已经做好了响应外部环境的准备；当机械臂
作为阻抗控制模型工作时，机械臂会被动地响应外力；阻抗控制的动力学方案如图2所示；
外力与关节加速度的对应阻抗关系可表示为
Figure GDA0004090629800000175
Figure GDA0004090629800000176
参考速度可表示为
Figure GDA0004090629800000177
Figure GDA0004090629800000178
因此，机械手的逆优先阻抗控制保证的表达式为：
Figure GDA0004090629800000179
步骤8、将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架实现方式如下：
混合阻抗应用就是上述两种策略的结合，即笛卡尔任务可分为两种情况：第一种是位置控制子空间，阻抗控制是在该子空间中实现的；第二个是力控制子空间，力控制在该子空间中实现；
因此选择一个选择矩阵；外力与位置响应的关系如下
Figure GDA0004090629800000181
Figure GDA0004090629800000182
所以期望速度的简化形式可表示为
Figure GDA0004090629800000183
Figure GDA0004090629800000184
然后我们得到了基于反向优先级的解决方案
Figure GDA0004090629800000185
混合阻抗控制的动力学方案图3所示；
考虑到n层任务，相应的阻抗控制任务也属于n层框架，因此，机械手速度级逆优先阻抗控制的总体框架表达式如下
Figure GDA0004090629800000186
表达式(52)解决了将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算的机械手逆优先混合阻抗控制中去；能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务。

Claims (1)

1.一种移动机械手冗余机械臂逆优先级阻抗控制方法，其特征在于，包括如下步骤：
步骤1、建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略；
步骤2、建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略；
步骤3、建立奇异鲁棒解逆运动学分析模型；
步骤4、建立多任务冗余机械臂的逆优先控制策略；
步骤5、对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理；
步骤6、建立机械手的逆优先力控制策略；
步骤7、采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证；
步骤8、将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架；
建立冗余机械臂运动学模型，并给出冗余机械臂零空间向量的梯度方向策略实现过程如下：
定义末端执行器在笛卡尔空间的位姿、速度分别为x、
Figure FDA0004090629790000011
其关节空间的角位置、角速度分别为q、/>
Figure FDA0004090629790000012
J为n自由度机器人的雅可比矩阵，其中x∈Rn，/>
Figure FDA0004090629790000013
J∈Rmn；冗余自由度机械臂的正运动学方程可用下式描述：
Figure FDA0004090629790000014
式(1)也被称为机械臂运动学速度模型；
考虑到最小二乘法的解，最优问题可列为：
Figure FDA0004090629790000021
式(1)的解可通过寻找最佳
Figure FDA0004090629790000022
来解决；
Figure FDA0004090629790000023
因此，式(1)的伪逆解可表示为：
Figure FDA0004090629790000024
式中J+——雅可比矩阵的伪逆
I——单位矩阵
Figure FDA0004090629790000025
——任意零空间矢量
Figure FDA0004090629790000026
——最小范数解，定义了手的动作
Figure FDA0004090629790000027
——齐次解，在末端不产生动作
式(4)代表了末端执行器的位置和姿态控制；在式(4)中加入任意残差，可得到包含零空间的一般表达式；利用上述方程可在零向量上实现多任务优化；
然而，上述方程忽略了雅可比矩阵的病态；正则化方程可通过添加额外的正则化值来修改，
Figure FDA0004090629790000028
其中λ≥0是加权矩阵，
Figure FDA0004090629790000029
为加权系数，并且满足
Figure FDA00040906297900000210
上述方程的解就可表示为：
Figure FDA00040906297900000211
式(7)也被称为冗余机械臂运动学模型；
冗余机械臂零空间向量的位置相关标量指数的关节限制梯度方向的关节限制函数为：
Figure FDA0004090629790000031
建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略如下：
在雅可比矩阵的冗余机械臂求解中，优化任务是在主任务的零空间中实现的；反向任务运动学是在正向任务运动学基础上建立的：
Figure FDA0004090629790000032
其中
Figure FDA0004090629790000033
和/>
Figure FDA0004090629790000034
表示task1和task2
从表达式(5)中得出冗余机械臂的逆运动学方程为：
Figure FDA0004090629790000035
Task1作为主任务，task2作为辅助任务；也就是说，task2
Figure FDA0004090629790000036
是在task1/>
Figure FDA0004090629790000037
的零空间中实现的；冗余机械臂的最终逆运动学表达式如下所示：
Figure FDA0004090629790000038
其中
Figure FDA0004090629790000039
Figure FDA00040906297900000310
是投影矩阵，它给出了次要任务到主要任务的适用范围；/>
Figure FDA00040906297900000311
和/>
Figure FDA00040906297900000312
是所需的命令速度；/>
Figure FDA00040906297900000313
是主要任务，/>
Figure FDA00040906297900000314
是次要任务；
如果两个相关任务是相互依赖的，则相应的雅可比矩阵是奇异的；如果任务雅可比矩阵是奇异的，则相应的任务是不满足的；在这种情况下，雅可比相关矩阵将是奇点，定义为算法奇点；
也就是说，如果
Figure FDA0004090629790000041
其中ρ(·)是矩阵的秩；
很明显，算法的奇异性是由次要任务和主要任务之间的任务冲突引起的；此外，基于任务优先级的冗余机械臂逆运动学旨在提供更好的控制主要任务的有效性；
因此，让位置控制方向作为主要任务，从而使位置保证控制方向任务的准确性；然后建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略方程：
Figure FDA0004090629790000042
建立奇异鲁棒解逆运动学分析模型如下：
基于雅可比伪逆解会出现运动学奇异性，这是由次矩阵引起的；对于运动奇异性问题，应该还要给出DLS(阻尼最小二乘)解；
对DLS解的成本函数可修改为：
Figure FDA0004090629790000043
因此，上述方程的奇异鲁棒伪逆解可表示为：
Figure FDA0004090629790000044
式(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 FDA0004090629790000051
其中，r≤m是矩阵J的秩；
对于运动奇异性，参考计算伪逆解时需要的奇异值分解SVD，大的产生的关节速度是由于最小的奇异值迅速接近0，如下式：
Figure FDA0004090629790000052
因子λ0将影响奇异性，λ0值越高，阻尼越大，联合速度就越接近奇异点；此外，定义可变阻尼因子的策略也不同；我们可得到
Figure FDA0004090629790000053
从上式中，我们可看出参数δ＞0监视最小的奇异值；
建立多任务冗余机械臂的逆优先控制策略如下；
引入逆优先投影矩阵
Figure FDA0004090629790000054
该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间，所以得出/>
Figure FDA0004090629790000055
Figure FDA0004090629790000056
其中Ji|j是与j-th任务线性无关的i-th任务的所有组件相关联的雅可比矩阵；
所以，优先级推导公式如下：
Figure FDA0004090629790000061
在上述推导中，k＝l，l-1，…，1；初始值
Figure FDA0004090629790000062
为了给出线性无关雅可比矩阵J的一般计算形式，定义逆增广Jacobian矩阵为：
Figure FDA0004090629790000063
就有了
Figure FDA0004090629790000064
其中
Figure FDA0004090629790000065
表示/>
Figure FDA0004090629790000066
的行；
于是乎，
Figure FDA0004090629790000067
的伪逆解就可表示为：
Figure FDA0004090629790000068
和
Figure FDA0004090629790000069
其中，Tk表示矩阵
Figure FDA00040906297900000610
的扩充；
最终的反向优先级投影可写为：
Figure FDA00040906297900000611
于是，我们就可得出伪逆解的表达式：
Figure FDA0004090629790000071
建立多任务冗余机械臂的逆优先控制策略方程如下：
Figure FDA0004090629790000072
对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理如下：
对于六自由度或七自由度冗余机械臂来说，没有足够的六自由度DOF来完成多个层次的任务；实行双任务优先控制是必要的；也就是说，机械手的运动控制是主要任务和次要任务；
具有主要任务和次要任务的冗余机械臂的反向控制方程如下
Figure FDA0004090629790000073
上面的公式与前面的表达式(11)有很大的不同，但是算法框架是相似的；在上面的方程中，
Figure FDA0004090629790000074
是次要任务，/>
Figure FDA0004090629790000075
是主任务；主任务在主任务的指定零空间中实现；反向优先级的核心点是投影矩阵/>
Figure FDA0004090629790000076
的计算；/>
Figure FDA0004090629790000077
的表达式如公式(30)：
Figure FDA0004090629790000078
利用前述公式(22)-(28)中的导子，可得到简化后具有主要任务和次要任务的冗余机械臂的反向控制方程：
Figure FDA0004090629790000079
建立机械手的逆优先力控制策略如下：
力控制空间中机械手的动力学可写成：
Figure FDA0004090629790000081
其中X是笛卡尔空间中的位置，M(X)是惯性矩阵，
Figure FDA0004090629790000082
是非线性力，F是输入控制力，Fe是接触力；
此外，基于雅可比矩阵的变换可得到输入关节力矩
τ＝JT(q)F (33)
操纵器在力控制空间中的期望运动方程可定义如下：
Figure FDA0004090629790000083
其中Md和Bd是惯性和阻尼矩阵；Fd是指令力，Fe是接触力；
因此，环境和操纵器响应之间的关系可写成
Figure FDA0004090629790000084
上述两个方程的组合如下
Figure FDA0004090629790000085
从上面的方程可看出，如果Me、Be和Ke已知，则Md和Bd的调整将影响系统响应；
力控制使机械手能够与环境或人类相互作用；另外，在某些情况下，没有必要实现全方位的力控制，也没有必要保证全方位的力控制，也就是说，有时我们只是想保证某个方向的力跟踪控制精度；
因此有必要对机械手进行分级力控制；也就是说，有必要给出一个新的层次力控制框架；从上面的方程我们可得到期望的层次力控制关系如下
Figure FDA0004090629790000086
Figure FDA0004090629790000087
/>
所以这两个方程的积分公式可写成
Figure FDA0004090629790000091
Figure FDA0004090629790000092
如果机械手末端执行器能够跟踪期望的笛卡尔速度为
Figure FDA0004090629790000093
和/>
Figure FDA0004090629790000094
则可实现机械手的精确力控制；笛卡尔速度与关节速度的关系应借鉴逆优先控制；因此，可得到机械手的逆优先力控制策略的方程：
Figure FDA0004090629790000095
上述方程所要求的关节速度将保证机械手的力控制；值得一提的是，上述力控制律只是速度级控制律，它依赖于内速度环控制；如果内位置控制效果良好，则可实现精确的力控制；由于内速度环控制可实现低频位置跟踪，所以外力环可实现低频力跟踪；
采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系，从而得到机械手的逆优先阻抗控制保证的实现方式如下：
当机械手实施力控制时，机械手在一定程度上起到了发起者的作用，也就是说，机械手已经做好了响应外部环境的准备；当机械臂作为阻抗控制模型工作时，机械臂会被动地响应外力；
外力与关节加速度的对应阻抗关系可表示为
Figure FDA0004090629790000096
Figure FDA0004090629790000097
参考速度可表示为
Figure FDA0004090629790000101
Figure FDA0004090629790000102
因此，机械手的逆优先阻抗控制保证的表达式为：
Figure FDA0004090629790000103
将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算，从而获得机械手速度级逆优先阻抗控制的总体框架实现方式如下：
混合阻抗应用就是上述两种策略的结合，即笛卡尔任务可分为两种情况：第一种是位置控制子空间，阻抗控制是在该子空间中实现的；第二个是力控制子空间，力控制在该子空间中实现；
因此选择一个选择矩阵；外力与位置响应的关系如下
Figure FDA0004090629790000104
Figure FDA0004090629790000105
所以期望速度的简化形式可表示为
Figure FDA0004090629790000106
Figure FDA0004090629790000107
然后我们得到了基于反向优先级的解决方案
Figure FDA0004090629790000108
考虑到n层任务，相应的阻抗控制任务也属于n层框架，因此，机械手速度级逆优先阻抗控制的总体框架表达式如下
Figure FDA0004090629790000111
表达式(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)；摄像头的控制端和环绕电机的控制端分别与控制台相连接。
CN202010369756.9A 2020-04-30 2020-04-30 机械手逆优先级阻抗控制系统及控制方法 Active CN111687833B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010369756.9A CN111687833B (zh) 2020-04-30 2020-04-30 机械手逆优先级阻抗控制系统及控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010369756.9A CN111687833B (zh) 2020-04-30 2020-04-30 机械手逆优先级阻抗控制系统及控制方法

Publications (2)

Publication Number Publication Date
CN111687833A CN111687833A (zh) 2020-09-22
CN111687833B true CN111687833B (zh) 2023-06-02

Family

ID=72476994

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010369756.9A Active CN111687833B (zh) 2020-04-30 2020-04-30 机械手逆优先级阻抗控制系统及控制方法

Country Status (1)

Country Link
CN (1) CN111687833B (zh)

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 (23)

* 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 西北工业大学 一种基于多个控制点的空间机器人避撞方法
CN108098786B (zh) * 2017-12-19 2020-12-04 扬州大学 聚变堆用内窥机械臂
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
CN111687833A (zh) 2020-09-22

Similar Documents

Publication Publication Date Title
Fang et al. Motion control of a tendon-based parallel manipulator using optimal tension distribution
Albu‐Schäffer et al. The DLR lightweight robot: design and control concepts for robots in human environments
Nenchev et al. Reaction null-space control of flexible structure mounted manipulator systems
Pierrot et al. Optimal design of a 4-DOF parallel manipulator: From academia to industry
Williams et al. Planar translational cable‐direct‐driven robots
EP1728600B1 (en) Controlling the trajectory of an effector
Nakanishi et al. Comparative experiments on task space control with redundancy resolution
Nemec et al. Force control of redundant robots in unstructured environment
Focchi et al. Torque-control based compliant actuation of a quadruped robot
De Lasa et al. Prioritized optimization for task-space control
CN111687835B (zh) 水下机械手冗余机械臂逆优先级阻抗控制系统及控制方法
Jean et al. Static balancing of planar parallel manipulators
Trevisani Planning of dynamically feasible trajectories for translational, planar, and underconstrained cable-driven robots
Dubowsky et al. The control of space manipulators subject to spacecraft attitude control saturation limits
Yan et al. A 5-DOF redundantly actuated parallel mechanism for large tilting five-face machining
Wimböck et al. Experimental study on dynamic reactionless motions with DLR's humanoid robot Justin
CN111805536A (zh) 一种水果分拣并联机器人机构考虑耦合作用自适应滑模控制方法
CN111687834B (zh) 移动机械手冗余机械臂逆优先级阻抗控制系统及控制方法
CN111687832B (zh) 空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法
CN111702753B (zh) 冗余机械臂逆优先级阻抗控制系统及控制方法
CN111687833B (zh) 机械手逆优先级阻抗控制系统及控制方法
Saab et al. Design and analysis of a discrete modular serpentine robotic tail for improved performance of mobile robots
Uzunoğlu et al. A multi-priority controller for industrial macro-micro manipulation
Agahi et al. Redundancy resolution of wire-actuated parallel manipulators
Ganesh et al. Inverse dynamics of a 3-DOF translational parallel kinematic machine

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