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

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

Info

Publication number
CN111687832A
CN111687832A CN202010369737.6A CN202010369737A CN111687832A CN 111687832 A CN111687832 A CN 111687832A CN 202010369737 A CN202010369737 A CN 202010369737A CN 111687832 A CN111687832 A CN 111687832A
Authority
CN
China
Prior art keywords
manipulator
control
task
priority
inverse
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.)
Granted
Application number
CN202010369737.6A
Other languages
English (en)
Other versions
CN111687832B (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

    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • B25J9/1628Programme controls characterised by the control loop
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • B25J9/1679Programme controls characterised by the tasks executed
    • YGENERAL 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
    • Y02TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02PCLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02Total 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

空间机械手冗余机械臂逆优先级阻抗控制***及控制方法
技术领域
本发明涉及机械手冗余机械臂逆优先级阻抗控制技术领域,尤其涉及空间机械手冗余机械臂逆优先级阻抗控制***及控制方法。
背景技术
目前工业机器人采用的控制方法是把机械手上每一个关节都当作一个单独的伺服机构,即每个轴对应一个伺服器,每个伺服器通过总线控制,由控制器统一控制并协调工作;
六个自由度的机械臂是具有完成空间定位能力最小自由度数的机械臂,多于六个自由度的机械臂统一称为冗余机械臂;
现在机械手的机械臂阻抗控制方法在不同层次结构下不能实现期望的阻抗控制任务,因此,设计一种能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务的方法显得非常必要。
发明内容
本发明是为了解决现有机械臂阻抗控制方法在不同层次结构下不能实现期望的阻抗控制任务的不足,提供一种一是能控制机械手的平衡,二是能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务的空间机械手冗余机械臂逆优先级阻抗控制***及控制方法。
以上技术问题是通过下列技术方案解决的:
一种空间机械手冗余机械臂逆优先级阻抗控制方法,包括如下步骤:
步骤1、建立冗余机械臂运动学模型,并给出冗余机械臂零空间向量的梯度方向策略;
步骤2、建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略;
步骤3、建立奇异鲁棒解逆运动学分析模型;
步骤4、建立多任务冗余机械臂的逆优先控制策略;
步骤5、对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理;
步骤6、建立机械手的逆优先力控制策略;
步骤7、采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系,从而得到机械手的逆优先阻抗控制保证;
步骤8、将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算,从而获得机械手速度级逆优先阻抗控制的总体框架。
本方案冗余机械臂在关节空间中的运动是按照相反的顺序导出的;然后将笛卡尔阻抗控制与逆优先阻抗控制相结合,解决了逆分层阻抗控制问题,将笛卡尔阻抗控制行为分为高优先阻抗控制和低优先阻抗控制。其中高优先阻抗控制任务不会干扰低优先阻抗控制任务,关节空间中的运动按相反的顺序会受到影响,要在相应的投影算子中工作;最后实现了高优先阻抗控制任务,避免了低优先阻抗控制任务中可能出现的奇异性引起的变形。因此,所提出的逆优先阻抗控制方法可使冗余机械臂在适当的层次结构下实现期望的阻抗控制任务。
作为优选,建立冗余机械臂运动学模型,并给出冗余机械臂零空间向量的梯度方向策略实现过程如下:
定义末端执行器在笛卡尔空间的位姿、速度分别为x、
Figure BDA0002476502090000021
其关节空间的角位置、角速度分别为q、
Figure BDA0002476502090000022
J为n自由度机器人的雅可比矩阵,其中x∈Rn
Figure BDA0002476502090000023
Figure BDA0002476502090000024
J∈Rm-n;冗余自由度机械臂的正运动学方程可用下式描述:
Figure BDA00024765020900000214
式(1)也被称为机械臂运动学速度模型;
考虑到最小二乘法的解,最优问题可列为:
Figure BDA0002476502090000025
式(1)的解可通过寻找最佳
Figure BDA0002476502090000026
来解决;
Figure BDA0002476502090000027
因此,式(1)的伪逆解可表示为:
Figure BDA0002476502090000028
式中J+——雅可比矩阵的伪逆
I——单位矩阵
Figure BDA0002476502090000029
——任意零空间矢量
Figure BDA00024765020900000210
——最小范数解,定义了手的动作
Figure BDA00024765020900000215
——齐次解,在末端不产生动作
式(4)代表了末端执行器的位置和姿态控制;在式(4)中加入任意残差,可得到包含零空间的一般表达式;利用上述方程可在零向量上实现多任务优化;
然而,上述方程忽略了雅可比矩阵的病态;正则化方程可通过添加额外的正则化值来修改,
Figure BDA00024765020900000211
其中λ≥0是加权矩阵,
Figure BDA00024765020900000212
为加权系数,并且满足
Figure BDA00024765020900000213
上述方程的解就可表示为:
Figure BDA00024765020900000216
式(7)也被称为冗余机械臂运动学模型;
冗余机械臂零空间向量的位置相关标量指数的关节限制梯度方向的关节限制函数为:
Figure BDA0002476502090000031
作为优选,建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略如下:
在雅可比矩阵的冗余机械臂求解中,优化任务是在主任务的零空间中实现的;反向任务运动学是在正向任务运动学基础上建立的:
Figure BDA0002476502090000032
其中
Figure BDA0002476502090000033
Figure BDA0002476502090000034
表示task1和task2
从表达式(5)中得出冗余机械臂的逆运动学方程为:
Figure BDA0002476502090000035
Task1作为主任务,task2作为辅助任务;也就是说,
Figure BDA0002476502090000036
是在
Figure BDA0002476502090000037
的零空间中实现的;冗余机械臂的最终逆运动学表达式如下所示:
Figure BDA0002476502090000038
其中N1=(I-J1 +J1),
Figure BDA0002476502090000039
Figure BDA00024765020900000316
是投影矩阵,它给出了次要任务到主要任务的适用范围;
Figure BDA00024765020900000310
Figure BDA00024765020900000311
是所需的命令速度;
Figure BDA00024765020900000312
是主要任务,
Figure BDA00024765020900000313
是次要任务;
如果两个相关任务是相互依赖的,则相应的雅可比矩阵是奇异的;如果任务雅可比矩阵是奇异的,则相应的任务是不满足的;在这种情况下,雅可比相关矩阵将是奇点,定义为算法奇点;
也就是说,如果
Figure BDA00024765020900000314
其中ρ(·)是矩阵的秩;
很明显,算法的奇异性是由次要任务和主要任务之间的任务冲突引起的;此外,基于任务优先级的冗余机械臂逆运动学旨在提供更好的控制主要任务的有效性;
因此,让位置控制方向作为主要任务,从而使位置保证控制方向任务的准确性;然后建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略方程:
Figure BDA00024765020900000315
作为优选,建立奇异鲁棒解逆运动学分析模型如下:
基于雅可比伪逆解会出现运动学奇异性,这是由次矩阵引起的;对于运动奇异性问题,应该还要给出DLS(阻尼最小二乘)解;
对DLS解的成本函数可修改为:
Figure BDA0002476502090000041
因此,上述方程的奇异鲁棒伪逆解可表示为:
Figure BDA0002476502090000042
式(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 BDA0002476502090000043
其中,r≤m是矩阵J的秩;
对于运动奇异性,参考计算伪逆解时需要的奇异值分解SVD,大的产生的关节速度是由于最小的奇异值迅速接近0,如下式:
Figure BDA0002476502090000044
因子λ0将影响奇异性,λ0值越高,阻尼越大,联合速度就越接近奇异点;此外,定义可变阻尼因子的策略也不同;我们可得到
Figure BDA0002476502090000045
从上式中,我们可看出参数δ>0监视最小的奇异值。
作为优选,建立多任务冗余机械臂的逆优先控制策略如下;
引入逆优先投影矩阵
Figure BDA0002476502090000046
该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间,所以得出
Figure BDA0002476502090000047
其中Ji|j是与j-th任务线性无关的i-th任务的所有组件相关联的雅可比矩阵;
所以,优先级推导公式如下:
Figure BDA0002476502090000048
在上述推导中,k=l,l-1,…,1;初始值
Figure BDA0002476502090000049
为了给出线性无关雅可比矩阵J的一般计算形式,定义逆增广Jacobian矩阵为:
Figure BDA0002476502090000051
就有了
Figure BDA0002476502090000052
其中
Figure BDA0002476502090000053
表示
Figure BDA0002476502090000054
的行;
于是乎,
Figure BDA0002476502090000055
的伪逆解就可表示为:
Figure BDA0002476502090000056
Figure BDA0002476502090000057
其中,Tk表示矩阵
Figure BDA0002476502090000058
的扩充;
最终的反向优先级投影可写为:
Figure BDA0002476502090000059
于是,我们就可得出伪逆解的表达式:
Figure BDA00024765020900000510
建立多任务冗余机械臂的逆优先控制策略方程如下:
Figure BDA00024765020900000511
作为优选,对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理如下:
对于六自由度或七自由度冗余机械臂来说,没有足够的六自由度DOF来完成多个层次的任务;实行双任务优先控制是必要的;也就是说,机械手的运动控制是主要任务和次要任务;
具有主要任务和次要任务的冗余机械臂的反向控制方程如下
Figure BDA00024765020900000512
上面的公式与前面的表达式(11)有很大的不同,但是算法框架是相似的;在上面的方程中,
Figure BDA00024765020900000513
是次要任务,
Figure BDA00024765020900000514
是主任务;主任务在主任务的指定零空间中实现;反向优先级的核心点是投影矩阵
Figure BDA00024765020900000515
的计算;
Figure BDA00024765020900000516
的表达式如公式(30):
Figure BDA0002476502090000061
利用以前公式(22)-(28)中类似的导子,可得到简化后具有主要任务和次要任务的冗余机械臂的反向控制方程:
Figure BDA0002476502090000062
作为优选,建立机械手的逆优先力控制策略如下:
力控制空间中机械手的动力学可写成:
Figure BDA0002476502090000063
其中X是笛卡尔空间中的位置,M(X)是惯性矩阵,H(X,X)是非线性力,F是输入控制力,Fe是接触力;
此外,基于雅可比矩阵的变换可得到输入关节力矩
τ=JT(q)F (33)
操纵器在力控制空间中的期望运动方程可定义如下:
Figure BDA0002476502090000064
其中Md和Bd是惯性和阻尼矩阵;Fd是指令力,Fe是接触力;
因此,环境和操纵器响应之间的关系可写成
Figure BDA0002476502090000065
上述两个方程的组合如下
Figure BDA0002476502090000066
从上面的方程可看出,如果Me、Be和Ke已知,则Md和Bd的调整将影响***响应;
力控制使机械手能够与环境或人类相互作用;另外,在某些情况下,没有必要实现全方位的力控制,也没有必要保证全方位的力控制,也就是说,有时我们只是想保证某个方向的力跟踪控制精度;
因此有必要对机械手进行分级力控制;也就是说,有必要给出一个新的层次力控制框架;从上面的方程我们可得到期望的层次力控制关系如下
Figure BDA0002476502090000067
Figure BDA0002476502090000068
所以这两个方程的积分公式可写成
Figure BDA0002476502090000069
Figure BDA00024765020900000610
如果机械手末端执行器能够跟踪期望的笛卡尔速度为
Figure BDA00024765020900000611
Figure BDA00024765020900000612
则可实现机械手的精确力控制;笛卡尔速度与关节速度的关系应借鉴逆优先控制;因此,可得到机械手的逆优先力控制策略的方程:
Figure BDA0002476502090000071
上述方程所要求的关节速度将保证机械手的力控制;值得一提的是,上述力控制律只是速度级控制律,它依赖于内速度环控制;如果内位置控制效果良好,则可实现精确的力控制;由于内速度环控制可实现低频位置跟踪,所以外力环可实现低频力跟踪。
作为优选,采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系,从而得到机械手的逆优先阻抗控制保证的实现方式如下:
当机械手实施力控制时,机械手在一定程度上起到了发起者的作用,也就是说,机械手已经做好了响应外部环境的准备;当机械臂作为阻抗控制模型工作时,机械臂会被动地响应外力;
外力与关节加速度的对应阻抗关系可表示为
Figure BDA0002476502090000072
Figure BDA0002476502090000073
参考速度可表示为
Figure BDA0002476502090000074
Figure BDA0002476502090000075
因此,机械手的逆优先阻抗控制保证的表达式为:
Figure BDA0002476502090000076
作为优选,将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算,从而获得机械手速度级逆优先阻抗控制的总体框架实现方式如下:
混合阻抗应用就是上述两种策略的结合,即笛卡尔任务可分为两种情况:第一种是位置控制子空间,阻抗控制是在该子空间中实现的;第二个是力控制子空间,力控制在该子空间中实现;
因此选择一个选择矩阵;外力与位置响应的关系如下
Figure BDA0002476502090000077
Figure BDA0002476502090000081
所以期望速度的简化形式可表示为
Figure BDA0002476502090000082
Figure BDA0002476502090000083
然后我们得到了基于反向优先级的解决方案
Figure BDA0002476502090000084
考虑到n层任务,相应的阻抗控制任务也属于n层框架,因此,机械手速度级逆优先阻抗控制的总体框架表达式如下
Figure BDA0002476502090000085
表达式(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 BDA0002476502090000101
其关节空间的角位置、角速度分别为q、
Figure BDA0002476502090000102
J为n自由度机器人的雅可比矩阵,其中x∈Rn
Figure BDA0002476502090000103
Figure BDA0002476502090000104
J∈Rm-n;冗余自由度机械臂的正运动学方程可用下式描述:
Figure BDA0002476502090000105
式(1)也被称为机械臂运动学速度模型;
考虑到最小二乘法的解,最优问题可列为:
Figure BDA0002476502090000106
式(1)的解可通过寻找最佳
Figure BDA0002476502090000107
来解决;
Figure BDA0002476502090000108
因此,式(1)的伪逆解可表示为:
Figure BDA0002476502090000109
式中J+——雅可比矩阵的伪逆
I——单位矩阵
Figure BDA00024765020900001010
——任意零空间矢量
Figure BDA00024765020900001011
——最小范数解,定义了手的动作
Figure BDA00024765020900001017
——齐次解,在末端不产生动作
式(4)代表了末端执行器的位置和姿态控制;在式(4)中加入任意残差,可得到包含零空间的一般表达式;利用上述方程可在零向量上实现多任务优化;
然而,上述方程忽略了雅可比矩阵的病态;正则化方程可通过添加额外的正则化值来修改,
Figure BDA00024765020900001012
其中λ≥0是加权矩阵,
Figure BDA00024765020900001013
为加权系数,并且满足
Figure BDA00024765020900001014
上述方程的解就可表示为:
Figure BDA00024765020900001015
式(7)也被称为冗余机械臂运动学模型;
冗余机械臂零空间向量的位置相关标量指数的关节限制梯度方向的关节限制函数为:
Figure BDA00024765020900001016
步骤2、建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略如下:
在雅可比矩阵的冗余机械臂求解中,优化任务是在主任务的零空间中实现的;反向任务运动学是在正向任务运动学基础上建立的:
Figure BDA0002476502090000111
其中
Figure BDA0002476502090000112
Figure BDA0002476502090000113
表示task1和task2
从表达式(5)中得出冗余机械臂的逆运动学方程为:
Figure BDA0002476502090000114
Task1作为主任务,task2作为辅助任务;也就是说,
Figure BDA0002476502090000115
是在
Figure BDA0002476502090000116
的零空间中实现的;冗余机械臂的最终逆运动学表达式如下所示:
Figure BDA0002476502090000117
其中
Figure BDA00024765020900001117
Figure BDA0002476502090000118
是投影矩阵,它给出了次要任务到主要任务的适用范围;
Figure BDA0002476502090000119
Figure BDA00024765020900001110
是所需的命令速度;
Figure BDA00024765020900001111
是主要任务,
Figure BDA00024765020900001112
是次要任务;
如果两个相关任务是相互依赖的,则相应的雅可比矩阵是奇异的;如果任务雅可比矩阵是奇异的,则相应的任务是不满足的;在这种情况下,雅可比相关矩阵将是奇点,定义为算法奇点;
也就是说,如果
Figure BDA00024765020900001113
其中ρ(·)是矩阵的秩;
很明显,算法的奇异性是由次要任务和主要任务之间的任务冲突引起的;此外,基于任务优先级的冗余机械臂逆运动学旨在提供更好的控制主要任务的有效性;
因此,让位置控制方向作为主要任务,从而使位置保证控制方向任务的准确性;然后建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略方程:
Figure BDA00024765020900001114
步骤3、建立奇异鲁棒解逆运动学分析模型如下:
基于雅可比伪逆解会出现运动学奇异性,这是由次矩阵引起的;对于运动奇异性问题,应该还要给出DLS(阻尼最小二乘)解;
对DLS解的成本函数可修改为:
Figure BDA00024765020900001115
因此,上述方程的奇异鲁棒伪逆解可表示为:
Figure BDA00024765020900001116
式(15)即为奇异鲁棒解逆运动学分析模型,设λ=η2I,上述DLS解等价于附加正则化解,标量值η平衡了任务精度和奇异性;
对于雅可比矩阵伪逆解的计算,可给出雅可比矩阵的奇异值SVD分解形式
J=U∑T (16)
其中U∈Rm×m,V-∈Rn×n,∑∈Rm×n,U是由列向量ui组成的一元矩阵,V是由列向量vi组成的一元矩阵,∑是m×n对角矩阵的块矩阵,该对角矩阵包含J的奇异值σi≥0以递减顺序包含n-m个零列向量;
Figure BDA0002476502090000121
其中,r≤m是矩阵J的秩;
对于运动奇异性,参考计算伪逆解时需要的奇异值分解SVD,大的产生的关节速度是由于最小的奇异值迅速接近0,如下式:
Figure BDA0002476502090000122
因子λ0将影响奇异性,λ0值越高,阻尼越大,联合速度就越接近奇异点;此外,定义可变阻尼因子的策略也不同;我们可得到
Figure BDA0002476502090000123
从上式中,我们可看出参数δ>0监视最小的奇异值。
步骤4、建立多任务冗余机械臂的逆优先控制策略如下;
引入逆优先投影矩阵
Figure BDA0002476502090000124
该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间,所以得出
Figure BDA0002476502090000125
其中Ji|j是与j-th任务线性无关的i-th任务的所有组件相关联的雅可比矩阵;
所以,优先级推导公式如下:
Figure BDA0002476502090000126
在上述推导中,k=l,l-1,…,1;初始值
Figure BDA0002476502090000127
为了给出线性无关雅可比矩阵J的一般计算形式,定义逆增广Jacobian矩阵为:
Figure BDA0002476502090000128
就有了
Figure BDA0002476502090000131
其中
Figure BDA0002476502090000132
表示
Figure BDA0002476502090000133
的行;
于是乎,
Figure BDA0002476502090000134
的伪逆解就可表示为:
Figure BDA0002476502090000135
Figure BDA0002476502090000136
其中,Tk表示矩阵
Figure BDA0002476502090000137
的扩充;
最终的反向优先级投影可写为:
Figure BDA0002476502090000138
于是,我们就可得出伪逆解的表达式:
Figure BDA0002476502090000139
建立多任务冗余机械臂的逆优先控制策略方程如下:
Figure BDA00024765020900001310
步骤5、对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理如下:
对于六自由度或七自由度冗余机械臂来说,没有足够的六自由度DOF来完成多个层次的任务;实行双任务优先控制是必要的;也就是说,机械手的运动控制是主要任务和次要任务;
具有主要任务和次要任务的冗余机械臂的反向控制方程如下
Figure BDA00024765020900001311
上面的公式与前面的表达式(11)有很大的不同,但是算法框架是相似的;在上面的方程中,
Figure BDA00024765020900001312
是次要任务,
Figure BDA00024765020900001313
是主任务;主任务在主任务的指定零空间中实现;反向优先级的核心点是投影矩阵
Figure BDA00024765020900001314
的计算;
Figure BDA00024765020900001315
的表达式如公式(30):
Figure BDA00024765020900001316
利用以前公式(22)-(28)中类似的导子,可得到简化后具有主要任务和次要任务的冗余机械臂的反向控制方程:
Figure BDA00024765020900001317
步骤6、建立机械手的逆优先力控制策略如下:
力控制空间中机械手的动力学可写成:
Figure BDA0002476502090000141
其中X是笛卡尔空间中的位置,M(X)是惯性矩阵,
Figure BDA0002476502090000142
是非线性力,F是输入控制力,Fe是接触力;
此外,基于雅可比矩阵的变换可得到输入关节力矩
τ=JT(q)F (33)
操纵器在力控制空间中的期望运动方程可定义如下:
Figure BDA0002476502090000143
其中Md和Bd是惯性和阻尼矩阵;Fd是指令力,Fe是接触力;
力控制的动力学方案如图1所示;
因此,环境和操纵器响应之间的关系可写成
Figure BDA0002476502090000144
上述两个方程的组合如下
Figure BDA0002476502090000145
从上面的方程可看出,如果Me、Be和Ke已知,则Md和Bd的调整将影响***响应;
力控制使机械手能够与环境或人类相互作用;另外,在某些情况下,没有必要实现全方位的力控制,也没有必要保证全方位的力控制,也就是说,有时我们只是想保证某个方向的力跟踪控制精度;
例如,当机械手与刨床相互作用时,只需在垂直方向保持精确的力跟踪控制,而另一个方向不需要精确的力跟踪控制;在其他情况下,位置方向力控制比姿态方向力控制更为重要;
因此有必要对机械手进行分级力控制;也就是说,有必要给出一个新的层次力控制框架;从上面的方程我们可得到期望的层次力控制关系如下
Figure BDA0002476502090000146
Figure BDA0002476502090000147
所以这两个方程的积分公式可写成
Figure BDA0002476502090000148
Figure BDA0002476502090000149
如果机械手末端执行器能够跟踪期望的笛卡尔速度为
Figure BDA00024765020900001410
Figure BDA00024765020900001411
则可实现机械手的精确力控制;笛卡尔速度与关节速度的关系应借鉴逆优先控制;因此,可得到机械手的逆优先力控制策略的方程:
Figure BDA0002476502090000151
上述方程所要求的关节速度将保证机械手的力控制;值得一提的是,上述力控制律只是速度级控制律,它依赖于内速度环控制;如果内位置控制效果良好,则可实现精确的力控制;由于内速度环控制可实现低频位置跟踪,所以外力环可实现低频力跟踪。
步骤7、采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系,从而得到机械手的逆优先阻抗控制保证的实现方式如下:
当机械手实施力控制时,机械手在一定程度上起到了发起者的作用,也就是说,机械手已经做好了响应外部环境的准备;当机械臂
作为阻抗控制模型工作时,机械臂会被动地响应外力;阻抗控制的动力学方案如图2所示;
外力与关节加速度的对应阻抗关系可表示为
Figure BDA0002476502090000152
Figure BDA0002476502090000153
参考速度可表示为
Figure BDA0002476502090000154
Figure BDA0002476502090000155
因此,机械手的逆优先阻抗控制保证的表达式为:
Figure BDA0002476502090000156
步骤8、将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算,从而获得机械手速度级逆优先阻抗控制的总体框架实现方式如下:
混合阻抗应用就是上述两种策略的结合,即笛卡尔任务可分为两种情况:第一种是位置控制子空间,阻抗控制是在该子空间中实现的;第二个是力控制子空间,力控制在该子空间中实现;
因此选择一个选择矩阵;外力与位置响应的关系如下
Figure BDA0002476502090000161
Figure BDA0002476502090000162
所以期望速度的简化形式可表示为
Figure BDA0002476502090000163
Figure BDA0002476502090000164
然后我们得到了基于反向优先级的解决方案
Figure BDA0002476502090000165
混合阻抗控制的动力学方案图3所示;
考虑到n层任务,相应的阻抗控制任务也属于n层框架,因此,机械手速度级逆优先阻抗控制的总体框架表达式如下
Figure BDA0002476502090000166
表达式(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 (10)

1.空间机械手冗余机械臂逆优先级阻抗控制***,包括机械手和控制机械手的控制台(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),平衡调节气缸的伸缩杆的右端固定连接在平衡调节块上;
一号电磁刹车的控制端、二号电磁刹车的控制端、三号电磁刹车的控制端、四号电磁刹车的控制端、五号电磁刹车的控制端、一号减速电机的控制端、二号减速电机的控制端、三号减速电机的控制端、四号减速电机的控制端、五号减速电机的控制端、一号气缸的控制端、二号气缸的控制端、平衡调节气缸的控制端和竖直气缸的控制端分别控制连接在控制台上。
2.一种空间机械手冗余机械臂逆优先级阻抗控制方法,其特征在于,包括如下步骤:
步骤1、建立冗余机械臂运动学模型,并给出冗余机械臂零空间向量的梯度方向策略;
步骤2、建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略;
步骤3、建立奇异鲁棒解逆运动学分析模型;
步骤4、建立多任务冗余机械臂的逆优先控制策略;
步骤5、对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理;
步骤6、建立机械手的逆优先力控制策略;
步骤7、采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系,从而得到机械手的逆优先阻抗控制保证;
步骤8、将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算,从而获得机械手速度级逆优先阻抗控制的总体框架。
3.根据权利要求2所述的空间机械手冗余机械臂逆优先级阻抗控制方法,其特征在于,建立冗余机械臂运动学模型,并给出冗余机械臂零空间向量的梯度方向策略实现过程如下:
定义末端执行器在笛卡尔空间的位姿、速度分别为x、
Figure FDA0002476502080000041
其关节空间的角位置、角速度分别为q、
Figure FDA0002476502080000042
J为n自由度机器人的雅可比矩阵,其中x∈Rn
Figure FDA0002476502080000043
J∈Rm-n;冗余自由度机械臂的正运动学方程可用下式描述:
Figure FDA0002476502080000044
式(1)也被称为机械臂运动学速度模型;
考虑到最小二乘法的解,最优问题可列为:
Figure FDA0002476502080000045
式(1)的解可通过寻找最佳
Figure FDA0002476502080000046
来解决;
Figure FDA0002476502080000047
因此,式(1)的伪逆解可表示为:
Figure FDA0002476502080000048
式中J+——雅可比矩阵的伪逆
I——单位矩阵
Figure FDA0002476502080000049
——任意零空间矢量
Figure FDA0002476502080000051
——最小范数解,定义了手的动作
Figure FDA0002476502080000052
——齐次解,在末端不产生动作
式(4)代表了末端执行器的位置和姿态控制;在式(4)中加入任意残差,可得到包含零空间的一般表达式;利用上述方程可在零向量上实现多任务优化;
然而,上述方程忽略了雅可比矩阵的病态;正则化方程可通过添加额外的正则化值来修改,
Figure FDA0002476502080000053
其中λ≥0是加权矩阵,
Figure FDA0002476502080000054
为加权系数,并且满足
Figure FDA0002476502080000055
上述方程的解就可表示为:
Figure FDA0002476502080000056
式(7)也被称为冗余机械臂运动学模型;
冗余机械臂零空间向量的位置相关标量指数的关节限制梯度方向的关节限制函数为:
Figure FDA0002476502080000057
4.根据权利要求3所述的空间机械手冗余机械臂逆优先级阻抗控制方法,其特征在于,建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略如下:
在雅可比矩阵的冗余机械臂求解中,优化任务是在主任务的零空间中实现的;反向任务运动学是在正向任务运动学基础上建立的:
Figure FDA0002476502080000058
其中
Figure FDA0002476502080000061
Figure FDA0002476502080000062
表示task1和task2
从表达式(5)中得出冗余机械臂的逆运动学方程为:
Figure FDA0002476502080000063
Task1作为主任务,task2作为辅助任务;也就是说,
Figure FDA0002476502080000064
是在
Figure FDA0002476502080000065
的零空间中实现的;冗余机械臂的最终逆运动学表达式如下所示:
Figure FDA0002476502080000066
其中N1=(I-J1 +J1),
Figure FDA0002476502080000067
Figure FDA0002476502080000068
是投影矩阵,它给出了次要任务到主要任务的适用范围;
Figure FDA0002476502080000069
Figure FDA00024765020800000610
是所需的命令速度;
Figure FDA00024765020800000611
是主要任务,
Figure FDA00024765020800000612
是次要任务;
如果两个相关任务是相互依赖的,则相应的雅可比矩阵是奇异的;如果任务雅可比矩阵是奇异的,则相应的任务是不满足的;在这种情况下,雅可比相关矩阵将是奇点,定义为算法奇点;
也就是说,如果
Figure FDA00024765020800000613
其中ρ(·)是矩阵的秩;
很明显,算法的奇异性是由次要任务和主要任务之间的任务冲突引起的;此外,基于任务优先级的冗余机械臂逆运动学旨在提供更好的控制主要任务的有效性;
因此,让位置控制方向作为主要任务,从而使位置保证控制方向任务的准确性;然后建立通过奇异鲁棒解得出消除奇异性算法的任务优先级解决策略方程:
Figure FDA0002476502080000071
5.根据权利要求4所述的空间机械手冗余机械臂逆优先级阻抗控制方法,其特征在于,建立奇异鲁棒解逆运动学分析模型如下:
基于雅可比伪逆解会出现运动学奇异性,这是由次矩阵引起的;对于运动奇异性问题,应该还要给出DLS(阻尼最小二乘)解;
对DLS解的成本函数可修改为:
Figure FDA0002476502080000072
因此,上述方程的奇异鲁棒伪逆解可表示为:
Figure FDA0002476502080000073
式(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 FDA0002476502080000074
其中,r≤m是矩阵J的秩;
对于运动奇异性,参考计算伪逆解时需要的奇异值分解SVD,大的产生的关节速度是由于最小的奇异值迅速接近0,如下式:
Figure FDA0002476502080000081
因子λ0将影响奇异性,λ0值越高,阻尼越大,联合速度就越接近奇异点;此外,定义可变阻尼因子的策略也不同;我们可得到
Figure FDA0002476502080000082
从上式中,我们可看出参数δ>0监视最小的奇异值。
6.根据权利要求5所述的空间机械手冗余机械臂逆优先级阻抗控制方法,其特征在于,建立多任务冗余机械臂的逆优先控制策略如下;
引入逆优先投影矩阵
Figure FDA0002476502080000083
该矩阵包括了不依赖于第k个任务的最低优先级l-k-1任务的相应元素的零空间,所以得出
Figure FDA0002476502080000084
Figure FDA0002476502080000085
其中Ji|j是与j-th任务线性无关的i-th任务的所有组件相关联的雅可比矩阵;
所以,优先级推导公式如下:
Figure FDA0002476502080000086
在上述推导中,k=l,l-1,…,1;初始值
Figure FDA0002476502080000087
为了给出线性无关雅可比矩阵J的一般计算形式,定义逆增广Jacobian矩阵为:
Figure FDA0002476502080000088
就有了
Figure FDA0002476502080000091
其中
Figure FDA0002476502080000092
表示
Figure FDA0002476502080000093
的行;
于是乎,
Figure FDA0002476502080000094
的伪逆解就可表示为:
Figure FDA0002476502080000095
Figure FDA0002476502080000096
其中,Tk表示矩阵
Figure FDA0002476502080000097
的扩充;
最终的反向优先级投影可写为:
Figure FDA0002476502080000098
于是,我们就可得出伪逆解的表达式:
Figure FDA0002476502080000099
建立多任务冗余机械臂的逆优先控制策略方程如下:
Figure FDA00024765020800000910
7.根据权利要求6所述的空间机械手冗余机械臂逆优先级阻抗控制方法,其特征在于,对具有主要任务和次要任务的冗余机械臂的反向控制方程进行简化处理如下:
对于六自由度或七自由度冗余机械臂来说,没有足够的六自由度DOF来完成多个层次的任务;实行双任务优先控制是必要的;也就是说,机械手的运动控制是主要任务和次要任务;
具有主要任务和次要任务的冗余机械臂的反向控制方程如下
Figure FDA0002476502080000101
上面的公式与前面的表达式(11)有很大的不同,但是算法框架是相似的;在上面的方程中,
Figure FDA0002476502080000102
是次要任务,
Figure FDA0002476502080000103
是主任务;主任务在主任务的指定零空间中实现;反向优先级的核心点是投影矩阵
Figure FDA0002476502080000104
的计算;
Figure FDA0002476502080000105
的表达式如公式(30):
Figure FDA0002476502080000106
利用以前公式(22)-(28)中类似的导子,可得到简化后具有主要任务和次要任务的冗余机械臂的反向控制方程:
Figure FDA0002476502080000107
8.根据权利要求7所述的空间机械手冗余机械臂逆优先级阻抗控制方法,其特征在于,建立机械手的逆优先力控制策略如下:
力控制空间中机械手的动力学可写成:
Figure FDA0002476502080000108
其中X是笛卡尔空间中的位置,M(X)是惯性矩阵,
Figure FDA0002476502080000109
是非线性力,F是输入控制力,Fe是接触力;
此外,基于雅可比矩阵的变换可得到输入关节力矩
τ=JT(q)F (33)
操纵器在力控制空间中的期望运动方程可定义如下:
Figure FDA0002476502080000111
其中Md和Bd是惯性和阻尼矩阵;Fd是指令力,Fe是接触力;
因此,环境和操纵器响应之间的关系可写成
Figure FDA0002476502080000112
上述两个方程的组合如下
Figure FDA0002476502080000113
从上面的方程可看出,如果Me、Be和Ke已知,则Md和Bd的调整将影响***响应;
力控制使机械手能够与环境或人类相互作用;另外,在某些情况下,没有必要实现全方位的力控制,也没有必要保证全方位的力控制,也就是说,有时我们只是想保证某个方向的力跟踪控制精度;
因此有必要对机械手进行分级力控制;也就是说,有必要给出一个新的层次力控制框架;从上面的方程我们可得到期望的层次力控制关系如下
Figure FDA0002476502080000114
Figure FDA0002476502080000115
所以这两个方程的积分公式可写成
Figure FDA0002476502080000116
Figure FDA0002476502080000117
如果机械手末端执行器能够跟踪期望的笛卡尔速度为
Figure FDA0002476502080000118
Figure FDA0002476502080000119
则可实现机械手的精确力控制;笛卡尔速度与关节速度的关系应借鉴逆优先控制;因此,可得到机械手的逆优先力控制策略的方程:
Figure FDA0002476502080000121
上述方程所要求的关节速度将保证机械手的力控制;值得一提的是,上述力控制律只是速度级控制律,它依赖于内速度环控制;如果内位置控制效果良好,则可实现精确的力控制;由于内速度环控制可实现低频位置跟踪,所以外力环可实现低频力跟踪。
9.根据权利要求8所述的空间机械手冗余机械臂逆优先级阻抗控制方法,其特征在于,采用关节速度来解决机械手逆优先阻抗控制中的外力与关节加速度之间的关系,从而得到机械手的逆优先阻抗控制保证的实现方式如下:
当机械手实施力控制时,机械手在一定程度上起到了发起者的作用,也就是说,机械手已经做好了响应外部环境的准备;当机械臂
作为阻抗控制模型工作时,机械臂会被动地响应外力;
外力与关节加速度的对应阻抗关系可表示为
Figure FDA0002476502080000122
Figure FDA0002476502080000123
参考速度可表示为
Figure FDA0002476502080000124
Figure FDA0002476502080000125
因此,机械手的逆优先阻抗控制保证的表达式为:
Figure FDA0002476502080000131
10.根据权利要求9所述的空间机械手冗余机械臂逆优先级阻抗控制方法,其特征在于,将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算,从而获得机械手速度级逆优先阻抗控制的总体框架实现方式如下:
混合阻抗应用就是上述两种策略的结合,即笛卡尔任务可分为两种情况:第一种是位置控制子空间,阻抗控制是在该子空间中实现的;第二个是力控制子空间,力控制在该子空间中实现;
因此选择一个选择矩阵;外力与位置响应的关系如下
Figure FDA0002476502080000132
Figure FDA0002476502080000133
所以期望速度的简化形式可表示为
Figure FDA0002476502080000134
Figure FDA0002476502080000135
然后我们得到了基于反向优先级的解决方案
Figure FDA0002476502080000136
考虑到n层任务,相应的阻抗控制任务也属于n层框架,因此,机械手速度级逆优先阻抗控制的总体框架表达式如下
Figure FDA0002476502080000141
表达式(52)解决了将位置控制空间的逆优先计算扩展到力控制空间的逆优先计算的机械手逆优先混合阻抗控制中去;能在不同层次结构下可使机械手的冗余机械臂实现期望的阻抗控制任务。
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 true CN111687832A (zh) 2020-09-22
CN111687832B 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)

Cited By (1)

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

Citations (26)

* 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
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
EP1854425A1 (de) * 2006-05-11 2007-11-14 BrainLAB AG Medizintechnische Positionsbestimmung mit redundanten Positionserfassungseinrichtungen und Prioritätsgewichtung für die Positionserfassungseinrichtungen
JP2009066685A (ja) * 2007-09-11 2009-04-02 Sony Corp ロボット装置及びロボット装置の制御方法
US20100161127A1 (en) * 2008-12-18 2010-06-24 Gm Global Technology Operations, Inc. Multiple priority operational space impedance control
US20100234999A1 (en) * 2006-06-26 2010-09-16 Yuichiro Nakajima Multi-joint robot and control program thereof
CN101947786A (zh) * 2009-04-30 2011-01-19 通用汽车环球科技运作公司 用于类人机器人自动控制的方法和装置
JP2011183527A (ja) * 2010-03-10 2011-09-22 Toyota Motor Corp 冗長ロボットの関節目標値決定方法及び冗長ロボットの制御装置
CN105676636A (zh) * 2016-01-11 2016-06-15 北京邮电大学 一种基于nsga-ii算法的冗余度空间机械臂多目标优化方法
CN205363898U (zh) * 2015-12-31 2016-07-06 中国科学院沈阳自动化研究所 可扩展的模块化机械臂
CN106708069A (zh) * 2017-01-19 2017-05-24 中国科学院自动化研究所 水下移动作业机器人的协调规划与控制方法
CN107290956A (zh) * 2017-08-01 2017-10-24 浙江大学 一种基于状态反馈的单关节柔性机械臂的位置控制方法
CN107351085A (zh) * 2017-08-21 2017-11-17 西北工业大学 一种基于多个控制点的空间机器人避撞方法
CN107650120A (zh) * 2016-07-26 2018-02-02 深圳华清精密科技有限公司 一种用于确定9自由度机械臂的全部奇异构型的方法
DE102016222255B3 (de) * 2016-11-14 2018-04-12 Kuka Roboter Gmbh Roboterarm, mobiler Roboter und Logistiksystem
CN108153153A (zh) * 2017-12-19 2018-06-12 哈尔滨工程大学 一种学习变阻抗控制***及控制方法
CN108241339A (zh) * 2017-12-27 2018-07-03 北京航空航天大学 仿人机械臂的运动求解和构型控制方法
CN108247631A (zh) * 2017-12-06 2018-07-06 西北工业大学 一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法
CN108621163A (zh) * 2018-05-10 2018-10-09 同济大学 一种面向汇榫工艺的冗余双臂机器人协作控制方法
CN108839025A (zh) * 2018-07-12 2018-11-20 杭州电子科技大学 一种移动机械臂的运动规划方法和装置
CN109079780A (zh) * 2018-08-08 2018-12-25 北京理工大学 基于广义坐标的分布式移动机械臂任务分层优化控制方法
CN110104216A (zh) * 2019-01-28 2019-08-09 西北工业大学深圳研究院 一种针对运动学冗余双臂空间机器人的协同路径规划方法
CN209631457U (zh) * 2018-12-17 2019-11-15 清研同创机器人(天津)有限公司 一种全向移动式机器人喷涂***
CN110497378A (zh) * 2019-09-04 2019-11-26 上海海事大学 一种用于零件加工的自动上下料机器人
CN110524522A (zh) * 2019-09-19 2019-12-03 浙江工业大学 一种多自由度冗余串并联拟人机械臂
US20220009093A1 (en) * 2020-07-08 2022-01-13 Ubtech Robotics Corp Ltd Task hierarchical control method, and robot and computer readable storage medium using the same

Patent Citations (26)

* 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
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
EP1854425A1 (de) * 2006-05-11 2007-11-14 BrainLAB AG Medizintechnische Positionsbestimmung mit redundanten Positionserfassungseinrichtungen und Prioritätsgewichtung für die Positionserfassungseinrichtungen
US20100234999A1 (en) * 2006-06-26 2010-09-16 Yuichiro Nakajima Multi-joint robot and control program thereof
JP2009066685A (ja) * 2007-09-11 2009-04-02 Sony Corp ロボット装置及びロボット装置の制御方法
US20100161127A1 (en) * 2008-12-18 2010-06-24 Gm Global Technology Operations, Inc. Multiple priority operational space impedance control
CN101947786A (zh) * 2009-04-30 2011-01-19 通用汽车环球科技运作公司 用于类人机器人自动控制的方法和装置
JP2011183527A (ja) * 2010-03-10 2011-09-22 Toyota Motor Corp 冗長ロボットの関節目標値決定方法及び冗長ロボットの制御装置
CN205363898U (zh) * 2015-12-31 2016-07-06 中国科学院沈阳自动化研究所 可扩展的模块化机械臂
CN105676636A (zh) * 2016-01-11 2016-06-15 北京邮电大学 一种基于nsga-ii算法的冗余度空间机械臂多目标优化方法
CN107650120A (zh) * 2016-07-26 2018-02-02 深圳华清精密科技有限公司 一种用于确定9自由度机械臂的全部奇异构型的方法
DE102016222255B3 (de) * 2016-11-14 2018-04-12 Kuka Roboter Gmbh Roboterarm, mobiler Roboter und Logistiksystem
CN106708069A (zh) * 2017-01-19 2017-05-24 中国科学院自动化研究所 水下移动作业机器人的协调规划与控制方法
CN107290956A (zh) * 2017-08-01 2017-10-24 浙江大学 一种基于状态反馈的单关节柔性机械臂的位置控制方法
CN107351085A (zh) * 2017-08-21 2017-11-17 西北工业大学 一种基于多个控制点的空间机器人避撞方法
CN108247631A (zh) * 2017-12-06 2018-07-06 西北工业大学 一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法
CN108153153A (zh) * 2017-12-19 2018-06-12 哈尔滨工程大学 一种学习变阻抗控制***及控制方法
CN108241339A (zh) * 2017-12-27 2018-07-03 北京航空航天大学 仿人机械臂的运动求解和构型控制方法
CN108621163A (zh) * 2018-05-10 2018-10-09 同济大学 一种面向汇榫工艺的冗余双臂机器人协作控制方法
CN108839025A (zh) * 2018-07-12 2018-11-20 杭州电子科技大学 一种移动机械臂的运动规划方法和装置
CN109079780A (zh) * 2018-08-08 2018-12-25 北京理工大学 基于广义坐标的分布式移动机械臂任务分层优化控制方法
CN209631457U (zh) * 2018-12-17 2019-11-15 清研同创机器人(天津)有限公司 一种全向移动式机器人喷涂***
CN110104216A (zh) * 2019-01-28 2019-08-09 西北工业大学深圳研究院 一种针对运动学冗余双臂空间机器人的协同路径规划方法
CN110497378A (zh) * 2019-09-04 2019-11-26 上海海事大学 一种用于零件加工的自动上下料机器人
CN110524522A (zh) * 2019-09-19 2019-12-03 浙江工业大学 一种多自由度冗余串并联拟人机械臂
US20220009093A1 (en) * 2020-07-08 2022-01-13 Ubtech Robotics Corp Ltd Task hierarchical control method, and robot and computer readable storage medium using the same

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
ANTONIO PENALVER: "A Multi-Task Priority Framework for Redundant Robots with Multiple Kinematic Chains under Hard Joint and Cartesian Constraints", 《2018 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS》 *
FABRIZIO FLACCO: "A Reverse Priority Approach to Multi-Task Control of Redundant Robots", 《2014 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS》 *
OUSSAMA KANOUN: "Kinematic Control of Redundant Manipulators:Generalizing the Task-Priority Framework to Inequality Task", 《IEEE TRANSACTIONS ON ROBOTICS》 *
华磊等: "一种七自由度冗余机械臂阻抗控制研究", 《华中科技大学学报(自然科学版)》 *
黄水华: "多约束下的机械臂运动控制算法研究", 《中国博士学位论文全文数据库 信息科技辑》 *

Cited By (1)

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

Also Published As

Publication number Publication date
CN111687832B (zh) 2023-06-02

Similar Documents

Publication Publication Date Title
EP1728600B1 (en) Controlling the trajectory of an effector
Zhang et al. Time-optimal and smooth trajectory planning for robot manipulators
US8600554B2 (en) System and method for robot trajectory generation with continuous accelerations
Peng et al. Compliant motion control of kinematically redundant manipulators
Chen et al. Optimal, fault-tolerant mappings to achieve secondary goals without compromising primary performance
CN111702753B (zh) 冗余机械臂逆优先级阻抗控制***及控制方法
Mostashiri et al. A review of research aspects of redundantly actuated parallel robotsw for enabling further applications
CN111687835B (zh) 水下机械手冗余机械臂逆优先级阻抗控制***及控制方法
Yan et al. A 5-DOF redundantly actuated parallel mechanism for large tilting five-face machining
Nemec et al. Force control of redundant robots in unstructured environment
KR20220148857A (ko) 로봇 제어
Chung et al. Torque optimizing control with singularity-robustness for kinematically redundant robots
CN111687832B (zh) 空间机械手冗余机械臂逆优先级阻抗控制***及控制方法
CN111687834B (zh) 移动机械手冗余机械臂逆优先级阻抗控制***及控制方法
CN111805536A (zh) 一种水果分拣并联机器人机构考虑耦合作用自适应滑模控制方法
Wimböck et al. Experimental study on dynamic reactionless motions with DLR's humanoid robot Justin
CN109623812A (zh) 考虑航天器本体姿态运动的机械臂轨迹规划方法
Uzunoğlu et al. A multi-priority controller for industrial macro-micro manipulation
Egeland et al. Coordination of motion in a spacecraft/manipulator system
Smirnov et al. Energy efficient trajectories of industrial machine tools with parallel kinematics
CN111687833A (zh) 机械手逆优先级阻抗控制***及控制方法
Wen et al. Motion planning for image-based visual servoing of an underwater vehicle-manipulator system in task-priority frameworks
Seraji Motion control of mobile manipulators
Yang et al. Dynamic compensation control of flexible macro–micro manipulator systems
Shadpey et al. Compliant motion control and redundancy resolution for kinematically redundant manipulators

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