CN114571469B - 一种机械臂零空间实时避障控制方法及系统 - Google Patents

一种机械臂零空间实时避障控制方法及系统 Download PDF

Info

Publication number
CN114571469B
CN114571469B CN202210478391.2A CN202210478391A CN114571469B CN 114571469 B CN114571469 B CN 114571469B CN 202210478391 A CN202210478391 A CN 202210478391A CN 114571469 B CN114571469 B CN 114571469B
Authority
CN
China
Prior art keywords
mechanical arm
task
virtual
track
obstacle
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
CN202210478391.2A
Other languages
English (en)
Other versions
CN114571469A (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.)
University of Science and Technology Beijing USTB
Original Assignee
University of Science and Technology Beijing USTB
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 University of Science and Technology Beijing USTB filed Critical University of Science and Technology Beijing USTB
Priority to CN202210478391.2A priority Critical patent/CN114571469B/zh
Publication of CN114571469A publication Critical patent/CN114571469A/zh
Application granted granted Critical
Publication of CN114571469B publication Critical patent/CN114571469B/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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • 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/1612—Programme controls characterised by the hand, wrist, grip 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/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种机械臂零空间实时避障控制方法及系统，所述方法由感知任务、规划任务与控制任务组成，包括：采集障碍物信息，获取障碍物的三维坐标，完成所述感知任务；基于人工势场法，使用虚拟排斥力计算函数生成机械臂与所处工作空间内障碍物之间的排斥力，进而根据虚拟阻抗控制算法生成机械臂对当前工作空间内障碍物进行躲避的避障轨迹，完成所述规划任务；基于预设性能函数根据需求对机械臂的瞬态性能进行约束，设计机械臂轨迹跟踪控制程序，得到机械臂跟踪避障轨迹的关节力矩，并实时控制机械臂按照预设性能跟踪避障轨迹，完成所述控制任务。本发明能够实现机械臂在协作工作的同时躲避障碍物，提高协作机器人系统的安全性与高效性。

Description

一种机械臂零空间实时避障控制方法及系统
技术领域
本发明涉及自动控制技术领域，特别涉及一种基于虚拟阻抗和预设性能函数的机械臂零空间实时避障控制方法及系统。
背景技术
近年来，智能机器人产业迅速发展，已经开始被广泛地应用于服务、娱乐、医疗等领域。越来越多的任务需要人与机器人协作完成，因此人机协作的相关问题得到了科研人员的高度重视。
人机协作控制相比于传统机器人控制的一大难点就是既要借助机器人的功能特点来提高人机协作的工作效率，同时还要保证人与机器人交互过程的柔顺性、安全性和智能性。因此对人机协作过程中智能控制方法的研究尤为重要。
现有的人机协作控制方法中主要依靠于触觉信息（交互力/力矩）来实现人机协作任务。这种单一的控制方法只能保证机器人可以跟随操作者的意图一起将物体协作移动到指定位置，以及保持机器人身体的柔顺性，但是在协作过程中机器人并不具备主动躲避障碍物的能力，因此该种控制方法安全性欠缺。并且现有的用于实现位置控制的控制器的控制效果不够理想，进而导致了机器人在人机协作过程中效率低下。
发明内容
针对上述问题，本发明的目的在于提供一种基于虚拟阻抗和预设性能函数的机械臂零空间实时避障控制方法及系统，以解决现有人机协作搬运系统在协作过程中机器人不具备主动躲避障碍物的问题以及机器人在人机协作过程中效率低下的问题。
为解决上述技术问题，本发明的实施例提供如下方案：
一方面，提供了一种机械臂零空间实时避障控制方法，所述方法基于包括机械臂、双目立体摄像头、服务器的机械臂零空间实时避障控制系统，所述方法由感知任务、规划任务与控制任务组成，包括以下步骤：
采集障碍物信息，获取障碍物的三维坐标，完成所述感知任务；
基于人工势场法，使用虚拟排斥力计算函数生成机械臂与所处工作空间内障碍物之间的排斥力，进而根据虚拟阻抗控制算法生成机械臂对当前工作空间内障碍物进行躲避的避障轨迹，完成所述规划任务；
基于预设性能函数根据需求对机械臂的瞬态性能进行约束，设计机械臂轨迹跟踪控制程序，得到机械臂跟踪避障轨迹的关节力矩，并实时控制机械臂按照预设性能跟踪避障轨迹，完成所述控制任务。
优选地，所述感知任务包括：得到障碍物表面与潜在碰撞点最近点的三维坐标，其表现形式如下：
Figure 543406DEST_PATH_IMAGE001
其中，
Figure 924709DEST_PATH_IMAGE002
分别表示所述方法探测到上述障碍物表面与潜在碰撞点最近点相对于机械臂基座坐标系的
Figure 420281DEST_PATH_IMAGE003
轴坐标，
Figure 166520DEST_PATH_IMAGE004
轴坐标和
Figure 114754DEST_PATH_IMAGE005
轴坐标；且机械臂上的潜在碰撞点的三维坐标的表现形式如下：
Figure 503010DEST_PATH_IMAGE006
潜在碰撞点的三维坐标通过机械臂前向运动学计算得到，机械臂上的潜在碰撞点和障碍物之间的距离表现形式如下：
Figure 525192DEST_PATH_IMAGE007
至此，完成所述感知任务。
优选地，所述规划任务包括：根据障碍物与机械臂之间的相对距离，实时生成避障轨迹；所述规划任务由虚拟排斥力函数与虚拟阻抗控制算法组成；
基于人工势场法，规划任务中的虚拟排斥力函数
Figure 397045DEST_PATH_IMAGE008
表现形式如下：
Figure 770257DEST_PATH_IMAGE009
其中，冒险空间是包围机械臂的一层虚拟保护外壳，
Figure 431046DEST_PATH_IMAGE010
为虚拟排斥力函数的输出，
Figure 307735DEST_PATH_IMAGE011
为虚拟排斥力函数的输入，
Figure 785990DEST_PATH_IMAGE012
是设置的最大虚拟排斥力，
Figure 380919DEST_PATH_IMAGE013
为虚拟排斥力函数的激活距离，也是进入冒险空间的边界距离，
Figure 845398DEST_PATH_IMAGE014
为设置的最小安全距离，
Figure 576594DEST_PATH_IMAGE015
为虚拟排斥力系数，此系数的表现形式如下：
Figure 897854DEST_PATH_IMAGE016
所述虚拟排斥力函数的作用在于，在三维空间中，在物理意义上直观地作用于机械臂上。
优选地，所述虚拟阻抗控制算法的输入是虚拟力矩，输出是机械臂各个关节对应的运动轨迹，所述虚拟阻抗控制算法的表现形式如下：
Figure 42396DEST_PATH_IMAGE017
其中
Figure 310567DEST_PATH_IMAGE018
是设置的虚拟阻抗控制参数，其作用为，根据障碍物的特点，生成匹配障碍物特点的避障轨迹；
Figure 161848DEST_PATH_IMAGE019
分别为机械臂执行避障任务时的关节期望轨迹的角加速度，角速度，角度；
Figure 981905DEST_PATH_IMAGE020
分别为机械臂的关节角真实的加速度、速度、角度；
Figure 291707DEST_PATH_IMAGE021
为关节虚拟排斥力矩，用于作为虚拟阻抗控制算法的输入。
优选地，虚拟排斥力为三维任务空间中的力，虚拟排斥力函数
Figure 97989DEST_PATH_IMAGE022
，而虚拟排斥力矩为机械臂关节空间中的力，三维任务空间的虚拟排斥力与机械臂关节空间中的虚拟排斥力矩之间的表现形式如下：
Figure 69356DEST_PATH_IMAGE023
其中
Figure 794736DEST_PATH_IMAGE024
代表机械臂潜在碰撞点对应的雅可比矩阵；
虚拟排斥力矩结合虚拟阻抗控制算法，得到下述实时避障参考轨迹的加速度：
Figure 851553DEST_PATH_IMAGE025
通过积分
Figure 195947DEST_PATH_IMAGE026
得到参考轨迹的速度与角度，从而得到避障的参考轨迹。
优选地，所述机械臂为冗余机械臂，关节个数为7个，而工作空间的自由度为6个，这使得机械臂的自由度大于工作空间的自由度，机械臂存在的冗余自由度，使机械臂可以完成次级任务；
机械臂的主任务在机械臂的末端执行器上执行；上述次级任务，即为不妨碍末端执行器执行主任务时，机械臂的身体，包括肩关节、肘关节、腕关节执行的任务；上述冗余机械臂的主任务与次级任务之间组合运动轨迹的表现形式如下：
Figure 21820DEST_PATH_IMAGE027
其中
Figure 918101DEST_PATH_IMAGE028
为主任务与次级任务的组合运动轨迹，
Figure 462215DEST_PATH_IMAGE029
为主任务对应轨迹，
Figure 610300DEST_PATH_IMAGE030
为次级任务对应轨迹，
Figure 290680DEST_PATH_IMAGE031
为主任务的零空间矩阵，
Figure 889020DEST_PATH_IMAGE032
为机械臂主任务的雅克比矩阵，
Figure 858113DEST_PATH_IMAGE033
为雅克比矩阵的广义逆矩阵，上述两式使用零空间投影技术。
优选地，所述零空间投影技术用于完成主任务与次级任务的优先级分层；所述主任务在机械臂零空间实时避障控制方法中，指末端执行器执行的任务，包括抓取任务、协作装配任务、协作搬运任务；所述次级任务为实时避障任务。
优选地，所述控制任务包括：基于反馈控制、前馈补偿、误差约束，计算出机械臂各个关节的控制力矩，以控制机械臂跟踪上述得到的期望位置和期望姿态，使机械臂在执行任务的同时实现实时避障；其控制力矩的控制器表现形式如下：
Figure 663084DEST_PATH_IMAGE034
其中，
Figure 463550DEST_PATH_IMAGE035
为对机械臂输入的控制力矩；
Figure 576999DEST_PATH_IMAGE036
;
Figure 830126DEST_PATH_IMAGE037
为控制增益矩阵；
Figure 444647DEST_PATH_IMAGE038
Figure 99619DEST_PATH_IMAGE039
为机械臂的当前速度，
Figure 649549DEST_PATH_IMAGE040
为虚拟控制量；
Figure 389972DEST_PATH_IMAGE041
为径向基函数神经网络；
Figure 542605DEST_PATH_IMAGE042
为神经网络的开关函数；
Figure 52083DEST_PATH_IMAGE043
为鲁棒项；
Figure 772915DEST_PATH_IMAGE044
为机械臂的动力学参数的上界；
Figure 634DEST_PATH_IMAGE045
为鲁棒项的系数；
鲁棒项可将逼近域为半全局的神经网络扩展为作用域为全局的带有鲁棒项的神经网络，当神经网络输入越过神经网络逼近域时，鲁棒项起作用；其中
Figure 629061DEST_PATH_IMAGE046
为预设性能函数误差约束系数，其设计方法如下：为了提高机械臂躲避障碍物的瞬态性能，首先设计如下的期望预设性能函数：
Figure 61223DEST_PATH_IMAGE047
其中
Figure 218535DEST_PATH_IMAGE048
分别代表预设的机械臂第
Figure 933550DEST_PATH_IMAGE049
个关节的误差边界的最大值、误差边界的最小值与误差收敛率；误差约束边界的表示如下：
Figure 100089DEST_PATH_IMAGE050
Figure 646477DEST_PATH_IMAGE051
与
Figure 771428DEST_PATH_IMAGE052
为预设的正常数，通过误差转换，将预设性能函数与误差建立联系，最终以预设性能函数误差约束系数形式影响控制力矩；误差转换的表示形式如下：
Figure 645843DEST_PATH_IMAGE053
通过误差转换得到
Figure 147231DEST_PATH_IMAGE054
，在此基础上，在稳定性分析阶段进而得到预设性能函数误差约束系数
Figure 282546DEST_PATH_IMAGE055
：
Figure 640715DEST_PATH_IMAGE056
。
一方面，提供了一种机械臂零空间实时避障控制系统，所述系统包括机械臂、双目立体摄像头和服务器，所述机械臂、所述双目立体摄像头与所述服务器通过ROS通信机制连接，并由所述服务器进行控制；
所述机械臂为七关节冗余机械臂，用于完成布置的各项任务；
所述双目立体摄像头用于采集位于所述机械臂运动空间内的障碍物信息，获取障碍物的三维坐标，完成感知任务；
所述服务器用于基于人工势场法，使用虚拟排斥力计算函数生成机械臂与所处工作空间内障碍物之间的排斥力，进而根据虚拟阻抗控制算法生成机械臂对当前工作空间内障碍物进行躲避的避障轨迹，完成规划任务；
所述服务器还用于基于预设性能函数根据需求对机械臂的瞬态性能进行约束，设计机械臂轨迹跟踪控制程序，得到机械臂跟踪避障轨迹的关节力矩，并实时控制机械臂按照预设性能跟踪避障轨迹，完成所述控制任务。
优选地，所述服务器用于完成所述感知任务、所述规划任务和所述控制任务中的计算；所述系统的工作流程如下：
双目立体摄像头实时获取机械臂当前运动空间内的障碍物信息；
服务器根据采集到的障碍物信息，基于人工势场法，使用虚拟排斥力计算函数生成当前障碍物在机械臂潜在碰撞点产生的虚拟排斥力；
同时，服务器根据计算得到的虚拟排斥力，基于预设的虚拟阻抗控制模型，利用虚拟阻抗控制算法生成每个关节的避障轨迹；
接着，服务器将机械臂末端执行任务和实时避障任务进行优先级分层，机械臂末端执行任务作为第一优先级，实时避障任务作为第二优先级，设计基于组合运动轨迹的全身控制框架，实现第二优先级任务执行不影响第一优先级任务，即在不影响第一优先级任务执行的情况下，调整机械臂与障碍物之间的距离以保证机械臂系统在执行任务过程中的安全性；
最后，服务器基于反馈控制网络、前馈补偿的径向基函数神经网络、带有瞬态约束的误差约束生成控制程序，计算出机械臂各个关节的控制力矩，以控制机械臂跟踪上述得到的期望位置和期望姿态，使机械臂在执行任务的同时实现实时避障。
本发明实施例提供的技术方案带来的有益效果至少包括：
1、本发明基于冗余机械臂，并搭配双目立体摄像头搭建的机械臂实时避障系统，使得机械臂在协作过程中具备了获取外界信息的能力，因此具备了完成更加复杂任务的硬件基础；
2、本发明将传统的阻抗控制与实时视觉信息相结合设计的虚拟阻抗控制方法结合了这两种控制方法的优势，实现了根据外界环境，实时修改期望轨迹的功能，赋予了机械臂实时避障能力与适应能力；
3、本发明的零空间虚拟阻抗控制方法相比传统的避障方法，利用零空间投影技术，将避障任务投影到机械臂的主任务的零空间中，可以实现不影响主任务进行的实时避障任务，使得机械臂可以更加高效和安全地完成任务；
4、本发明的实时避障控制方法使得机械臂在执行任务过程中，限制了跟踪误差的动态变化，使得跟踪误差的动态变化符合设定的要求，从而提升了机械臂在执行任务时的快速性和准确性。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种机械臂零空间实时避障控制系统的结构示意图；
图2是本发明实施例提供的一种机械臂零空间实时避障控制方法的原理示意图；
图3是本发明实施例中冒险空间的表现形式示意图；
图4是本发明实施例中设定边界示意图。
如图所示，为了能明确实现本发明的实施例的结构，在图中标注了特定的结构和器件，但这仅为示意需要，并非意图将本发明限定在该特定结构、器件和环境中，根据具体需要，本领域的普通技术人员可以将这些器件和环境进行调整或者修改，所进行的调整或者修改仍然包括在本发明的保护范围中。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的实施例提供了一种基于虚拟阻抗和预设性能函数的机械臂零空间实时避障控制方法，所述方法基于包括机械臂、双目立体摄像头、服务器的机械臂零空间实时避障控制系统，参考图1和图2所示，所述方法由感知任务、规划任务与控制任务组成，包括以下步骤：
采集障碍物信息，获取障碍物的三维坐标，完成所述感知任务；
基于人工势场法，使用虚拟排斥力计算函数生成机械臂与所处工作空间内障碍物之间的排斥力，进而根据虚拟阻抗控制算法生成机械臂对当前工作空间内障碍物进行躲避的避障轨迹，完成所述规划任务；
基于预设性能函数根据需求对机械臂的瞬态性能进行约束，设计机械臂轨迹跟踪控制程序，得到机械臂跟踪避障轨迹的关节力矩，并实时控制机械臂按照预设性能跟踪避障轨迹，完成所述控制任务。
其中，机械臂用于执行各项具体操作，双目立体摄像头用于采集障碍物信息，服务器用于完成感知任务、规划任务与控制任务中的计算。
进一步地，所述感知任务包括：得到障碍物表面与潜在碰撞点最近点的三维坐标，其表现形式如下：
Figure 64743DEST_PATH_IMAGE001
其中，
Figure 776347DEST_PATH_IMAGE002
分别表示所述方法探测到上述障碍物表面与潜在碰撞点最近点相对于机械臂基座坐标系的
Figure 252589DEST_PATH_IMAGE003
轴坐标，
Figure 453763DEST_PATH_IMAGE004
轴坐标和
Figure 427404DEST_PATH_IMAGE005
轴坐标；且机械臂上的潜在碰撞点的三维坐标的表现形式如下：
Figure 473857DEST_PATH_IMAGE006
潜在碰撞点的三维坐标通过机械臂前向运动学计算得到，机械臂上的潜在碰撞点和障碍物之间的距离表现形式如下：
Figure 114923DEST_PATH_IMAGE007
至此，完成所述感知任务。
进一步地，所述规划任务包括：根据障碍物与机械臂之间的相对距离，实时生成避障轨迹；所述规划任务由虚拟排斥力函数与虚拟阻抗控制算法组成；
基于人工势场法，规划任务中的虚拟排斥力函数
Figure 955840DEST_PATH_IMAGE008
表现形式如下：
Figure 416777DEST_PATH_IMAGE009
其中， 冒险空间是包围机械臂的一层虚拟保护外壳，
Figure 532501DEST_PATH_IMAGE010
为虚拟排斥力函数的输出，
Figure 700177DEST_PATH_IMAGE011
为虚拟排斥力函数的输入，
Figure 446416DEST_PATH_IMAGE012
是设置的最大虚拟排斥力，
Figure 66753DEST_PATH_IMAGE013
为虚拟排斥力函数的激活距离，也是进入冒险空间的边界距离，
Figure 314064DEST_PATH_IMAGE014
为设置的最小安全距离，
Figure 342106DEST_PATH_IMAGE015
为虚拟排斥力系数，此系数的表现形式如下：
Figure 55984DEST_PATH_IMAGE016
冒险空间的表现形式如图3所示，其中其中
Figure 366880DEST_PATH_IMAGE057
为虚拟包围外壳中，球体和圆柱体的圆半径，可以根据需求设置大小，用于根据避障的范围需求，生成冒险空间（机械臂虚拟包围外壳，即虚拟保护外壳）。
所述虚拟排斥力函数的作用在于，在三维空间中，在物理意义上直观地作用于机械臂上。
所述虚拟阻抗控制算法的输入是虚拟力矩，输出是机械臂各个关节对应的运动轨迹，例如，当虚拟力矩是为了躲避障碍物生成的虚拟力矩，则对应生成的运动轨迹为避障运动轨迹；所述虚拟阻抗控制算法的表现形式如下：
Figure 89985DEST_PATH_IMAGE017
其中
Figure 28991DEST_PATH_IMAGE018
是设置的虚拟阻抗控制参数，其作用为，根据障碍物的特点，生成匹配障碍物特点的避障轨迹；
Figure 444929DEST_PATH_IMAGE019
分别为机械臂执行避障任务时的关节期望轨迹的角加速度，角速度，角度；
Figure 977542DEST_PATH_IMAGE020
分别为机械臂的关节角真实的加速度、速度、角度；
Figure 238759DEST_PATH_IMAGE021
为关节虚拟排斥力矩，用于作为虚拟阻抗控制算法的输入。
虚拟排斥力为三维任务空间中的力，虚拟排斥力函数
Figure 969954DEST_PATH_IMAGE022
，而虚拟排斥力矩为机械臂关节空间中的力，三维任务空间的虚拟排斥力与机械臂关节空间中的虚拟排斥力矩之间的表现形式如下：
Figure 619110DEST_PATH_IMAGE023
其中
Figure 435757DEST_PATH_IMAGE024
代表机械臂潜在碰撞点对应的雅可比矩阵，且
Figure 703927DEST_PATH_IMAGE024
已可得到。
虚拟排斥力矩结合虚拟阻抗控制算法，得到下述实时避障参考轨迹的加速度：
Figure 289629DEST_PATH_IMAGE025
通过积分
Figure 781790DEST_PATH_IMAGE026
得到参考轨迹的速度与角度，从而得到避障的参考轨迹。
本发明实施例中，所述机械臂为冗余机械臂，关节个数为7个，而工作空间的自由度为6个，这使得机械臂的自由度大于工作空间的自由度，机械臂存在的冗余自由度，使机械臂可以完成次级任务。
机械臂的主任务在机械臂的末端执行器上执行；上述次级任务，即为不妨碍末端执行器执行主任务时，机械臂的身体，包括肩关节、肘关节、腕关节执行的任务；上述冗余机械臂的主任务与次级任务之间组合运动轨迹的表现形式如下：
Figure 407769DEST_PATH_IMAGE027
其中
Figure 276368DEST_PATH_IMAGE028
为主任务与次级任务的组合运动轨迹，
Figure 919839DEST_PATH_IMAGE029
为主任务对应轨迹，
Figure 582902DEST_PATH_IMAGE030
为次级任务对应轨迹，
Figure 702036DEST_PATH_IMAGE031
为主任务的零空间矩阵，
Figure 108747DEST_PATH_IMAGE032
为机械臂主任务的雅克比矩阵，
Figure 934620DEST_PATH_IMAGE033
为雅克比矩阵的广义逆矩阵，上述两式使用零空间投影技术。
其中，所述零空间投影技术用于完成主任务与次级任务的优先级分层；所述主任务在机械臂零空间实时避障控制方法中，指末端执行器执行的任务，包括抓取任务、协作装配任务、协作搬运任务；所述次级任务为实时避障任务。
进一步地，所述控制任务包括：基于反馈控制、前馈补偿、误差约束，计算出机械臂各个关节的控制力矩，以控制机械臂跟踪上述得到的期望位置和期望姿态，使机械臂在执行任务的同时实现实时避障。其控制力矩的控制器表现形式如下：
Figure 706267DEST_PATH_IMAGE034
其中，
Figure 984802DEST_PATH_IMAGE035
为对机械臂输入的控制力矩；
Figure 195203DEST_PATH_IMAGE036
；
Figure 937900DEST_PATH_IMAGE037
为控制增益矩阵；
Figure 942765DEST_PATH_IMAGE038
Figure 646279DEST_PATH_IMAGE039
为机械臂的当前速度，
Figure 400652DEST_PATH_IMAGE040
为虚拟控制量；
Figure 935538DEST_PATH_IMAGE041
为径向基函数神经网络；
Figure 439201DEST_PATH_IMAGE042
为神经网络的开关函数；
Figure 692328DEST_PATH_IMAGE043
为鲁棒项；
Figure 182215DEST_PATH_IMAGE044
为机械臂的动力学参数的上界；
Figure 837187DEST_PATH_IMAGE045
为鲁棒项的系数；
鲁棒项可将逼近域为半全局的神经网络扩展为作用域为全局的带有鲁棒项的神经网络，当神经网络输入越过神经网络逼近域时，鲁棒项起作用。
Figure 511751DEST_PATH_IMAGE041
是用于估计机械臂未知动力学模型的径向基函数神经网络，当神经网络的输入
Figure 986594DEST_PATH_IMAGE058
处于逼近域中，根据其逼近未知函数的能力，神经网络可以通过自适应更新率来拟合机械臂未知动力学模型的函数；其中
Figure 280173DEST_PATH_IMAGE059
为神经网络的输入向量，
Figure 789651DEST_PATH_IMAGE060
为神经网络的实际估计权重，
Figure 635116DEST_PATH_IMAGE061
为径向基函数如下：
Figure 862835DEST_PATH_IMAGE062
其中
Figure 694525DEST_PATH_IMAGE063
为神经网络接受域的中心，
Figure 76089DEST_PATH_IMAGE064
为高斯函数的宽度，
Figure 764559DEST_PATH_IMAGE065
为神经网络的节点数。神经网络的估计形式表示如下：
Figure 276312DEST_PATH_IMAGE066
其中，
Figure 974009DEST_PATH_IMAGE067
是理想情况下的神经网络的估计权重，
Figure 395763DEST_PATH_IMAGE068
为神经网络的估计误差。设计神经网络自适应率如下：
Figure 520714DEST_PATH_IMAGE069
其中，
Figure 519763DEST_PATH_IMAGE070
是为了保证系统稳定而设定的很小的正常数；其中神经网络的开关函数
Figure 755572DEST_PATH_IMAGE042
的表示如下：
Figure 31833DEST_PATH_IMAGE071
Figure 327685DEST_PATH_IMAGE072
其中，
Figure 814030DEST_PATH_IMAGE073
和
Figure 853530DEST_PATH_IMAGE074
是正常数，且满足
Figure 249876DEST_PATH_IMAGE075
，两者分别对应设定的开关函数的下界与上界；
Figure 722489DEST_PATH_IMAGE076
是一个正常数；
Figure 430551DEST_PATH_IMAGE077
为神经网络的第
Figure 273742DEST_PATH_IMAGE078
个关节的第
Figure 321333DEST_PATH_IMAGE079
个输入；开关函数可以将传统的神经网络与鲁棒项结合，当上述协作机械臂的关节角度跟踪误差较大，导致神经网络的输入超出神经网络的作用域时，鲁棒项将逐渐起主导作用，将协作机械臂的关节角度跟踪误差减小，同时将神经网络的输入拉回神经网络的作用域内，增强机械臂实时避障系统的稳定性。
其中
Figure 162250DEST_PATH_IMAGE080
为预设性能函数误差约束系数，其设计方法如下：为了提高机械臂躲避障碍物的瞬态性能，首先设计如下的期望预设性能函数：
Figure 29712DEST_PATH_IMAGE081
其中
Figure 473331DEST_PATH_IMAGE082
分别代表预设的机械臂第
Figure 641007DEST_PATH_IMAGE083
个关节的误差边界的最大值、误差边界的最小值与误差收敛率，如图4所示，图中的虚线即为根据设计的预设性能函数进行再次利用的误差约束边界。误差约束边界的表示如下：
Figure 387246DEST_PATH_IMAGE084
Figure 273163DEST_PATH_IMAGE051
与
Figure 254894DEST_PATH_IMAGE052
为预设的正常数，通过误差转换，将预设性能函数与误差建立联系，最终以预设性能函数误差约束系数形式影响控制力矩；误差转换的表示形式如下：
Figure 542656DEST_PATH_IMAGE053
通过误差转换得到
Figure 459797DEST_PATH_IMAGE085
，在此基础上，在稳定性分析阶段进而得到预设性能函数误差约束系数
Figure 561570DEST_PATH_IMAGE086
：
Figure 19097DEST_PATH_IMAGE056
误差约束系数可以将上述协作机械臂的跟踪误差约束在设定的预设性能函数内，增强机械臂实时避障系统的快速性。
本发明方法通过获得视觉感知等外部信息获取设备实时传递来的外界动态环境中障碍物的信息，使用人工势场法计算出障碍物与机械臂潜在碰撞点之间的虚拟排斥力，将虚拟排斥力作为虚拟阻抗控制的输入，进而利用虚拟阻抗控制算法生成每个关节的实时运动轨迹。根据实时生成的轨迹，设计基于预设性能函数的轨迹跟踪控制器，得到每个关节的控制力矩，控制力矩驱动机械臂的每个关节按照生成的实时运动轨迹运动，并时刻保证误差瞬态需求，通过机械臂各关节的运动相互配合，最终实现任务空间的动态障碍物的实时躲避。
本发明提出的机械臂零空间实时避障控制方法的优势如下：一、可以局部修改期望轨迹——当没有障碍物时根据初始的期望轨迹运行，存在障碍物时，期望轨迹被局部修改，躲避障碍。二、可以自动恢复期望轨迹——当避障过程完成后，局部修改的期望轨迹将恢复原状态，继续执行原任务。三、可以节约时间地完成障碍物躲避任务——本发明利用机械臂零空间避障，可以实现避障任务与末端执行器任务并行，任务同时执行可节省时间。四、响应时间短——带有瞬态约束的控制器，可以使避障任务根据需求快速动作，从而提高避障的瞬态性能。
相应地，本发明的实施例还提供了一种机械臂零空间实时避障控制系统，包括：
所述系统包括机械臂、双目立体摄像头和服务器，所述机械臂、所述双目立体摄像头、所述服务器支持机器人操作系统（Robot Operation Sysem，ROS）。所述机械臂、所述双目立体摄像头与所述服务器通过ROS通信机制连接，并由所述服务器进行控制。
所述机械臂为七关节冗余机械臂，用于完成布置的各项任务，如协作搬运、协作装配、抓取指定物体等任务；
所述双目立体摄像头用于采集位于所述机械臂运动空间内的障碍物信息，获取障碍物的三维坐标，完成感知任务；
所述服务器用于基于人工势场法，使用虚拟排斥力计算函数生成机械臂与所处工作空间内障碍物之间的排斥力，进而根据虚拟阻抗控制算法生成机械臂对当前工作空间内障碍物进行躲避的避障轨迹，完成规划任务；
所述服务器还用于基于预设性能函数根据需求对机械臂的瞬态性能进行约束，设计机械臂轨迹跟踪控制程序，得到机械臂跟踪避障轨迹的关节力矩，并实时控制机械臂按照预设性能跟踪避障轨迹，完成所述控制任务。
进一步地，所述服务器用于完成所述感知任务、所述规划任务和所述控制任务中的计算。
所述感知任务中，得到机械臂潜在碰撞点与障碍物表面间的最近距离。
所述规划任务中，根据感知层得到的障碍物与机械臂之间的最近距离，实时生成避障轨迹；此计算由虚拟排斥力函数与虚拟阻抗控制算法组成。
所述虚拟排斥力计算函数，在三维空间中，在物理意义上，直观地作用于机械臂上。规划任务中的另一个组成算法——虚拟阻抗控制算法，此算法的输入是虚拟力矩，输出是机械臂各个关节对应的运动轨迹，例如，当虚拟力矩是为了躲避障碍物生成的虚拟力矩，则对应生成的运动轨迹为避障运动轨迹。
所述服务器还用于完成所述规划任务中的分层计算，机械臂存在的冗余自由度，使机械臂可以完成次级任务。机械臂的主任务通常在机械臂的末端执行器上执行；上述次级任务，即为不妨碍末端执行器执行主任务时，机械臂的身体（肩关节、肘关节、腕关节）执行的任务；且上述主任务、次级任务，即可抽象为任务轨迹。
所述的零空间投影技术是指，完成主任务与次级任务的优先级分层。上述主任务在机械臂实时避障系统中，泛指末端执行器执行的任务，如抓取任务，协作装配，协作搬运等任务；上述次级任务为实时避障任务。
所述控制任务中，基于反馈控制、前馈补偿、基于预设性能函数的误差约束，计算出机械臂各个关节的控制力矩，使得机械臂的关节真实轨迹跟踪上述得到的期望位置和期望姿态，令所述冗余机械臂按要求执行任务的同时实现实时避障。
具体地，本实施例中的机械臂可以采用Kinova-Gen3机械臂，双目立体摄像头可以采用ZED2摄像头，服务器优选的配置为运行内存32G，显卡为GeForce GTX Titan X；利用固定支架，将双目立体摄像头ZED2安全、合理地固定在指定位置。
其中，服务器用于同时控制ZED2摄像头和 Kinova-Gen3机械臂；Kinova-Gen3机械臂用于利用其机械臂完成操作者指定任务；ZED2摄像头用于获取机械臂当前运动空间内的障碍物三维坐标信息；Kinova-Gen3机械臂使用Linux操作系统以及ROS控制，可以随时接收Kinova-Gen3机械臂的关节角度、角速度信息，并且可以实时对Kinova-Gen3机械臂发送控制消息。ZED2摄像头同样使用Linux操作系统以及ROS控制，可以实时获取机械臂当前运动空间内的障碍物位置信息。
所述系统的工作流程如下：
双目立体摄像头实时获取机械臂当前运动空间内的障碍物信息；
服务器根据采集到的障碍物信息，基于人工势场法，使用虚拟排斥力计算函数生成当前障碍物在机械臂潜在碰撞点产生的虚拟排斥力；其中，用于计算虚拟排斥力的函数是根据避障要求预先设定好的，并且可在机械臂潜在碰撞点上生成三维方向上的虚拟排斥力；
同时，服务器根据计算得到的虚拟排斥力，基于预设的虚拟阻抗控制模型，利用虚拟阻抗控制算法生成每个关节的避障轨迹；其中，由虚拟阻抗控制生成的每个关节的运动轨迹，在共同运动下，得到潜在碰撞点处的避障轨迹，实现潜在碰撞点的避障任务；调整机械臂与障碍物之间的距离以保证机械臂系统的安全性；
接着，服务器将机械臂末端执行任务和实时避障任务进行优先级分层，机械臂末端执行任务作为第一优先级，实时避障任务作为第二优先级，设计基于组合运动轨迹的全身控制框架，实现第二优先级任务执行不影响第一优先级任务，即在不影响第一优先级任务执行的情况下，调整机械臂与障碍物之间的距离以保证机械臂系统在执行任务过程中的安全性；
最后，服务器基于反馈控制网络、前馈补偿的径向基函数神经网络、带有瞬态约束的误差约束生成控制程序，计算出机械臂各个关节的控制力矩，以控制机械臂跟踪上述得到的期望位置和期望姿态，使机械臂在执行任务的同时实现实时避障，提高协作机器人系统的安全性与高效性。
本实施例中系统的具体执行算法可参考方法实施例，其实现原理和技术效果类似，此处不再赘述。
本实施例基于冗余机械臂，并搭配双目立体摄像头搭建的机械臂实时避障系统提高了机械臂在协作过程中获取外界信息的能力，使得机械臂具备了完成更加复杂协作任务的硬件基础；通过结合人工势场法与虚拟阻抗控制这两种方法，解决了给定的机械臂协作任务下的避障问题，提高了机械臂的对动态环境的适应能力；通过全身控制算法，解决了机械臂多任务执行时的优先级分配问题，提高了机械臂的高效性与灵活性。
需要说明的是，在说明书中提到“一个实施例”、“实施例”、“示例性实施例”、“一些实施例”等指示所述的实施例可以包括特定特征、结构或特性，但未必每个实施例都包括该特定特征、结构或特性。另外，在结合实施例描述特定特征、结构或特性时，结合其它实施例（无论是否明确描述）实现这种特征、结构或特性应在相关领域技术人员的知识范围内。
通常，可以至少部分从上下文中的使用来理解术语。例如，至少部分取决于上下文，本文中使用的术语“一个或多个”可以用于描述单数意义的任何特征、结构或特性，或者可以用于描述复数意义的特征、结构或特性的组合。另外，术语“基于”可以被理解为不一定旨在传达一组排他性的因素，而是可以替代地，至少部分地取决于上下文，允许存在不一定明确描述的其他因素。
如本文使用的，术语“标称/标称地”是指在生产或制造过程的设计阶段期间设置的针对部件或过程操作的特性或参数的期望或目标值，以及高于和/或低于期望值的值的范围。值的范围可能是由于制造过程或容限中的轻微变化导致的。如本文使用的，术语“大约”指示可以基于与主题半导体器件相关联的特定技术节点而变化的给定量的值。基于特定技术节点，术语“大约”可以指示给定量的值，其例如在值的5%-15%（例如，值的±5%、±10%或±15%）内变化。
可以理解的是，本公开中的“在……上”、“在……之上”和“在……上方”的含义应当以最宽方式被解读，以使得“在……上”不仅表示“直接在”某物“上”而且还包括在某物“上”且其间有居间特征或层的含义，并且“在……之上”或“在……上方”不仅表示“在”某物“之上”或“上方”的含义，而且还可以包括其“在”某物“之上”或“上方”且其间没有居间特征或层的含义。
此外，诸如“在…之下”、“在…下方”、“下部”、“在…之上”、“上部”等空间相关术语在本文中为了描述方便可以用于描述一个元件或特征与另一个或多个元件或特征的关系，如在附图中示出的。空间相关术语旨在涵盖除了在附图所描绘的取向之外的在设备使用或操作中的不同取向。设备可以以另外的方式被定向，并且本文中使用的空间相关描述词可以类似地被相应解释。
本发明涵盖任何在本发明的精髓和范围上做的替代、修改、等效方法以及方案。为了使公众对本发明有彻底的了解，在以下本发明优选实施例中详细说明了具体的细节，而对本领域技术人员来说没有这些细节的描述也可以完全理解本发明。另外，为了避免对本发明的实质造成不必要的混淆，并没有详细说明众所周知的方法、过程、流程、元件和电路等。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于计算机可读取存储介质中，如：ROM/RAM、磁碟、光盘等。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种机械臂零空间实时避障控制方法，其特征在于，所述方法基于包括机械臂、双目立体摄像头、服务器的机械臂零空间实时避障控制系统，所述方法由感知任务、规划任务与控制任务组成，包括以下步骤：
采集障碍物信息，获取障碍物的三维坐标，完成所述感知任务；
所述感知任务包括：得到障碍物表面与潜在碰撞点最近点的三维坐标，其表现形式如下：
Figure 76716DEST_PATH_IMAGE001
其中，
Figure 701733DEST_PATH_IMAGE002
分别表示所述方法探测到上述障碍物表面与潜在碰撞点最近点相对于机械臂基座坐标系的
Figure 233208DEST_PATH_IMAGE003
轴坐标，
Figure 310885DEST_PATH_IMAGE004
轴坐标和
Figure 953219DEST_PATH_IMAGE005
轴坐标；且机械臂上的潜在碰撞点的三维坐标的表现形式如下：
Figure 698322DEST_PATH_IMAGE006
潜在碰撞点的三维坐标通过机械臂前向运动学计算得到，机械臂上的潜在碰撞点和障碍物之间的距离表现形式如下：
Figure 400698DEST_PATH_IMAGE007
至此，完成所述感知任务；
基于人工势场法，使用虚拟排斥力计算函数生成机械臂与所处工作空间内障碍物之间的排斥力，进而根据虚拟阻抗控制算法生成机械臂对当前工作空间内障碍物进行躲避的避障轨迹，完成所述规划任务；
所述规划任务包括：根据障碍物与机械臂之间的相对距离，实时生成避障轨迹；所述规划任务由虚拟排斥力函数与虚拟阻抗控制算法组成；
基于人工势场法，规划任务中的虚拟排斥力函数
Figure 965672DEST_PATH_IMAGE008
表现形式如下：
Figure 411697DEST_PATH_IMAGE009
其中，
Figure 276884DEST_PATH_IMAGE010
是包围机械臂的一层虚拟保护外壳，
Figure 150163DEST_PATH_IMAGE011
为虚拟排斥力函数的输出，
Figure 202432DEST_PATH_IMAGE012
为虚拟排斥力函数的输入，
Figure 452148DEST_PATH_IMAGE013
是设置的最大虚拟排斥力，
Figure 906263DEST_PATH_IMAGE014
为虚拟排斥力函数的激活距离，也是进入
Figure 950442DEST_PATH_IMAGE010
的边界距离，
Figure 490008DEST_PATH_IMAGE015
为设置的最小安全距离，
Figure 277836DEST_PATH_IMAGE016
为虚拟排斥力系数，此系数的表现形式如下：
Figure 852036DEST_PATH_IMAGE017
所述虚拟排斥力函数的作用在于，在三维空间中，在物理意义上直观地作用于机械臂上；
所述虚拟阻抗控制算法的输入是虚拟力矩，输出是机械臂各个关节对应的运动轨迹，所述虚拟阻抗控制算法的表现形式如下：
Figure 67117DEST_PATH_IMAGE018
其中
Figure 359558DEST_PATH_IMAGE019
是设置的虚拟阻抗控制参数，其作用为，根据障碍物的特点，生成匹配障碍物特点的避障轨迹；
Figure 419918DEST_PATH_IMAGE020
分别为机械臂执行避障任务时的关节期望轨迹的角加速度，角速度，角度；
Figure 114205DEST_PATH_IMAGE021
分别为机械臂的关节角真实的加速度、速度、角度；
Figure 500187DEST_PATH_IMAGE022
为关节虚拟排斥力矩，用于作为虚拟阻抗控制算法的输入；
虚拟排斥力为三维任务空间中的力，虚拟排斥力函数
Figure 14345DEST_PATH_IMAGE008
，而虚拟排斥力矩为机械臂关节空间中的力，三维任务空间的虚拟排斥力与机械臂关节空间中的虚拟排斥力矩之间的表现形式如下：
Figure 143975DEST_PATH_IMAGE023
其中
Figure 427188DEST_PATH_IMAGE024
代表机械臂潜在碰撞点对应的雅可比矩阵；
虚拟排斥力矩结合虚拟阻抗控制算法，得到下述实时避障参考轨迹的加速度：
Figure 984072DEST_PATH_IMAGE025
通过积分
Figure 985526DEST_PATH_IMAGE026
得到参考轨迹的速度与角度，从而得到避障的参考轨迹；
基于预设性能函数根据需求对机械臂的瞬态性能进行约束，设计机械臂轨迹跟踪控制程序，得到机械臂跟踪避障轨迹的关节力矩，并实时控制机械臂按照预设性能跟踪避障轨迹，完成所述控制任务；
所述控制任务包括：基于反馈控制、前馈补偿、误差约束，计算出机械臂各个关节的控制力矩，以控制机械臂跟踪上述得到的期望位置和期望姿态，使机械臂在执行任务的同时实现实时避障；其控制力矩的控制器表现形式如下：
Figure 918847DEST_PATH_IMAGE027
其中，
Figure 322146DEST_PATH_IMAGE028
为对机械臂输入的控制力矩；
Figure 49931DEST_PATH_IMAGE029
为控制增益矩阵；
Figure 538681DEST_PATH_IMAGE030
Figure 10113DEST_PATH_IMAGE031
为机械臂的当前速度，
Figure 267919DEST_PATH_IMAGE032
为虚拟控制量；
Figure 166605DEST_PATH_IMAGE033
径向基函数神经网络；
Figure 142652DEST_PATH_IMAGE034
为神经网络的开关函数；
Figure 417775DEST_PATH_IMAGE035
为鲁棒项；
Figure 530088DEST_PATH_IMAGE036
为机械臂的动力学参数的上界；
Figure 865254DEST_PATH_IMAGE037
为鲁棒项的系数；
鲁棒项可将逼近域为半全局的神经网络扩展为作用域为全局的带有鲁棒项的神经网络，当神经网络输入越过神经网络逼近域时，鲁棒项起作用；其中
Figure 63017DEST_PATH_IMAGE038
为预设性能函数误差约束系数，其设计方法如下：为了提高机械臂躲避障碍物的瞬态性能，首先设计如下的期望预设性能函数：
Figure 141832DEST_PATH_IMAGE039
其中
Figure 843071DEST_PATH_IMAGE040
分别代表预设的机械臂第
Figure 349139DEST_PATH_IMAGE041
个关节的误差边界的最大值、误差边界的最小值与误差收敛率；误差约束边界的表示如下：
Figure 34198DEST_PATH_IMAGE042
Figure 385545DEST_PATH_IMAGE043
与
Figure 472450DEST_PATH_IMAGE044
预设的正常数，通过误差转换，将预设性能函数与误差建立联系，最终以预设性能函数误差约束系数形式影响控制力矩；误差转换的表示形式如下：
Figure 149419DEST_PATH_IMAGE045
通过误差转换得到
Figure 321774DEST_PATH_IMAGE046
，在此基础上，在稳定性分析阶段进而得到预设性能函数误差约束系数
Figure 742391DEST_PATH_IMAGE047
Figure 683803DEST_PATH_IMAGE048
所述机械臂为冗余机械臂，关节个数为7个，而工作空间的自由度为6个，这使得机械臂的自由度大于工作空间的自由度，机械臂存在的冗余自由度，使机械臂可以完成次级任务；
机械臂的主任务在机械臂的末端执行器上执行；上述次级任务，即为不妨碍末端执行器执行主任务时，机械臂的身体，包括肩关节、肘关节、腕关节执行的任务；上述冗余机械臂的主任务与次级任务之间组合运动轨迹的表现形式如下：
Figure 525813DEST_PATH_IMAGE049
其中
Figure 185465DEST_PATH_IMAGE050
为主任务与次级任务的组合运动轨迹，
Figure 144194DEST_PATH_IMAGE051
为主任务对应轨迹，
Figure 205691DEST_PATH_IMAGE052
为次级任务对应轨迹，
Figure 693304DEST_PATH_IMAGE053
为主任务的零空间矩阵，
Figure 840251DEST_PATH_IMAGE054
为机械臂主任务的雅克比矩阵，
Figure 602671DEST_PATH_IMAGE055
为雅克比矩阵的广义逆矩阵，上述两式使用零空间投影技术。
2.根据权利要求1所述的机械臂零空间实时避障控制方法，其特征在于，所述零空间投影技术用于完成主任务与次级任务的优先级分层；所述主任务在机械臂零空间实时避障控制方法中，指末端执行器执行的任务，包括抓取、协作装配、协作搬运；所述次级任务为实时避障任务。
3.一种机械臂零空间实时避障控制系统，其特征在于，所述系统包括机械臂、双目立体摄像头和服务器，所述机械臂、所述双目立体摄像头与所述服务器通过ROS通信机制连接，并由所述服务器进行控制；
所述机械臂为七关节冗余机械臂，用于完成布置的各项任务；
所述双目立体摄像头用于采集位于所述机械臂运动空间内的障碍物信息，获取障碍物的三维坐标，完成感知任务；
所述感知任务包括：得到障碍物表面与潜在碰撞点最近点的三维坐标，其表现形式如下：
Figure 518674DEST_PATH_IMAGE001
其中，
Figure 708347DEST_PATH_IMAGE002
分别表示探测到上述障碍物表面与潜在碰撞点最近点相对于机械臂基座坐标系的
Figure 77012DEST_PATH_IMAGE003
轴坐标，
Figure 377543DEST_PATH_IMAGE004
轴坐标和
Figure 148053DEST_PATH_IMAGE005
轴坐标；且机械臂上的潜在碰撞点的三维坐标的表现形式如下：
Figure 508627DEST_PATH_IMAGE006
潜在碰撞点的三维坐标通过机械臂前向运动学计算得到，机械臂上的潜在碰撞点和障碍物之间的距离表现形式如下：
Figure 364588DEST_PATH_IMAGE007
至此，完成所述感知任务；
所述服务器用于基于人工势场法，使用虚拟排斥力计算函数生成机械臂与所处工作空间内障碍物之间的排斥力，进而根据虚拟阻抗控制算法生成机械臂对当前工作空间内障碍物进行躲避的避障轨迹，完成规划任务；
所述规划任务包括：根据障碍物与机械臂之间的相对距离，实时生成避障轨迹；所述规划任务由虚拟排斥力函数与虚拟阻抗控制算法组成；
基于人工势场法，规划任务中的虚拟排斥力函数
Figure 468810DEST_PATH_IMAGE008
表现形式如下：
Figure 359405DEST_PATH_IMAGE009
其中，
Figure 625302DEST_PATH_IMAGE010
是包围机械臂的一层虚拟保护外壳，
Figure 968558DEST_PATH_IMAGE011
为虚拟排斥力函数的输出，
Figure 610892DEST_PATH_IMAGE012
为虚拟排斥力函数的输入，
Figure 355994DEST_PATH_IMAGE013
是设置的最大虚拟排斥力，
Figure 58371DEST_PATH_IMAGE014
为虚拟排斥力函数的激活距离，也是进入
Figure 623345DEST_PATH_IMAGE010
的边界距离，
Figure 69370DEST_PATH_IMAGE015
为设置的最小安全距离，
Figure 668978DEST_PATH_IMAGE016
为虚拟排斥力系数，此系数的表现形式如下：
Figure 542256DEST_PATH_IMAGE017
所述虚拟排斥力函数的作用在于，在三维空间中，在物理意义上直观地作用于机械臂上；
所述虚拟阻抗控制算法的输入是虚拟力矩，输出是机械臂各个关节对应的运动轨迹，所述虚拟阻抗控制算法的表现形式如下：
Figure 860105DEST_PATH_IMAGE018
其中
Figure 109821DEST_PATH_IMAGE019
是设置的虚拟阻抗控制参数，其作用为，根据障碍物的特点，生成匹配障碍物特点的避障轨迹；
Figure 563936DEST_PATH_IMAGE020
分别为机械臂执行避障任务时的关节期望轨迹的角加速度，角速度，角度；
Figure 342536DEST_PATH_IMAGE021
分别为机械臂的关节角真实的加速度、速度、角度；
Figure 147681DEST_PATH_IMAGE022
为关节虚拟排斥力矩，用于作为虚拟阻抗控制算法的输入；
虚拟排斥力为三维任务空间中的力，虚拟排斥力函数
Figure 935508DEST_PATH_IMAGE008
，而虚拟排斥力矩为机械臂关节空间中的力，三维任务空间的虚拟排斥力与机械臂关节空间中的虚拟排斥力矩之间的表现形式如下：
Figure 509709DEST_PATH_IMAGE023
其中
Figure 724790DEST_PATH_IMAGE024
代表机械臂潜在碰撞点对应的雅可比矩阵；
虚拟排斥力矩结合虚拟阻抗控制算法，得到下述实时避障参考轨迹的加速度：
Figure 17231DEST_PATH_IMAGE025
通过积分
Figure 77591DEST_PATH_IMAGE026
得到参考轨迹的速度与角度，从而得到避障的参考轨迹；
基于预设性能函数根据需求对机械臂的瞬态性能进行约束，设计机械臂轨迹跟踪控制程序，得到机械臂跟踪避障轨迹的关节力矩，并实时控制机械臂按照预设性能跟踪避障轨迹，完成控制任务；
所述控制任务包括：基于反馈控制、前馈补偿、误差约束，计算出机械臂各个关节的控制力矩，以控制机械臂跟踪上述得到的期望位置和期望姿态，使机械臂在执行任务的同时实现实时避障；其控制力矩的控制器表现形式如下：
Figure 506298DEST_PATH_IMAGE027
其中，
Figure 157859DEST_PATH_IMAGE028
为对机械臂输入的控制力矩；
Figure 672017DEST_PATH_IMAGE029
为控制增益矩阵；
Figure 536068DEST_PATH_IMAGE030
Figure 84861DEST_PATH_IMAGE031
为机械臂的当前速度，
Figure 641744DEST_PATH_IMAGE032
为虚拟控制量；
Figure 643199DEST_PATH_IMAGE033
径向基函数神经网络；
Figure 576519DEST_PATH_IMAGE034
为神经网络的开关函数；
Figure 979819DEST_PATH_IMAGE035
为鲁棒项；
Figure 707604DEST_PATH_IMAGE036
为机械臂的动力学参数的上界；
Figure 930775DEST_PATH_IMAGE037
为鲁棒项的系数；
鲁棒项可将逼近域为半全局的神经网络扩展为作用域为全局的带有鲁棒项的神经网络，当神经网络输入越过神经网络逼近域时，鲁棒项起作用；其中
Figure 402207DEST_PATH_IMAGE038
为预设性能函数误差约束系数，其设计方法如下：为了提高机械臂躲避障碍物的瞬态性能，首先设计如下的期望预设性能函数：
Figure 660013DEST_PATH_IMAGE039
其中
Figure 824278DEST_PATH_IMAGE040
分别代表预设的机械臂第
Figure 800324DEST_PATH_IMAGE041
个关节的误差边界的最大值、误差边界的最小值与误差收敛率；误差约束边界的表示如下：
Figure 809869DEST_PATH_IMAGE042
Figure 922181DEST_PATH_IMAGE043
与
Figure 991768DEST_PATH_IMAGE044
预设的正常数，通过误差转换，将预设性能函数与误差建立联系，最终以预设性能函数误差约束系数形式影响控制力矩；误差转换的表示形式如下：
Figure 455111DEST_PATH_IMAGE045
通过误差转换得到
Figure 268346DEST_PATH_IMAGE046
，在此基础上，在稳定性分析阶段进而得到预设性能函数误差约束系数
Figure 494885DEST_PATH_IMAGE047
Figure 735373DEST_PATH_IMAGE048
所述机械臂为冗余机械臂，关节个数为7个，而工作空间的自由度为6个，这使得机械臂的自由度大于工作空间的自由度，机械臂存在的冗余自由度，使机械臂可以完成次级任务；
机械臂的主任务在机械臂的末端执行器上执行；上述次级任务，即为不妨碍末端执行器执行主任务时，机械臂的身体，包括肩关节、肘关节、腕关节执行的任务；上述冗余机械臂的主任务与次级任务之间组合运动轨迹的表现形式如下：
Figure 420433DEST_PATH_IMAGE049
其中
Figure 37359DEST_PATH_IMAGE050
为主任务与次级任务的组合运动轨迹，
Figure 124263DEST_PATH_IMAGE051
为主任务对应轨迹，
Figure 801232DEST_PATH_IMAGE052
为次级任务对应轨迹，
Figure 973588DEST_PATH_IMAGE053
为主任务的零空间矩阵，
Figure 128626DEST_PATH_IMAGE054
为机械臂主任务的雅克比矩阵，
Figure 335616DEST_PATH_IMAGE055
为雅克比矩阵的广义逆矩阵，上述两式使用零空间投影技术。
4.根据权利要求3所述的机械臂零空间实时避障控制系统，其特征在于，所述服务器用于完成所述感知任务、所述规划任务和所述控制任务中的计算；所述系统的工作流程如下：
双目立体摄像头实时获取机械臂当前运动空间内的障碍物信息；
服务器根据采集到的障碍物信息，基于人工势场法，使用虚拟排斥力计算函数生成当前障碍物在机械臂潜在碰撞点产生的虚拟排斥力；
同时，服务器根据计算得到的虚拟排斥力，基于预设的虚拟阻抗控制模型，利用虚拟阻抗控制算法生成每个关节的避障轨迹；
接着，服务器将机械臂末端执行任务和实时避障任务进行优先级分层，机械臂末端执行任务作为第一优先级，实时避障任务作为第二优先级，设计基于组合运动轨迹的全身控制框架，实现第二优先级任务执行不影响第一优先级任务，即在不影响第一优先级任务执行的情况下，调整机械臂与障碍物之间的距离以保证机械臂系统在执行任务过程中的安全性；
最后，服务器基于反馈控制网络、前馈补偿的径向基函数神经网络、带有瞬态约束的误差约束生成控制程序，计算出机械臂各个关节的控制力矩，以控制机械臂跟踪上述得到的期望位置和期望姿态，使机械臂在执行任务的同时实现实时避障。
CN202210478391.2A 2022-05-05 2022-05-05 一种机械臂零空间实时避障控制方法及系统 Active CN114571469B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210478391.2A CN114571469B (zh) 2022-05-05 2022-05-05 一种机械臂零空间实时避障控制方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210478391.2A CN114571469B (zh) 2022-05-05 2022-05-05 一种机械臂零空间实时避障控制方法及系统

Publications (2)

Publication Number Publication Date
CN114571469A CN114571469A (zh) 2022-06-03
CN114571469B true CN114571469B (zh) 2022-07-26

Family

ID=81784644

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210478391.2A Active CN114571469B (zh) 2022-05-05 2022-05-05 一种机械臂零空间实时避障控制方法及系统

Country Status (1)

Country Link
CN (1) CN114571469B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115026816A (zh) * 2022-06-09 2022-09-09 安徽工业大学 一种基于虚拟力的机械臂末端避障方法
CN115256395B (zh) * 2022-08-17 2024-06-18 北京理工大学 基于控制障碍函数的模型不确定机器人安全控制方法
CN115229772B (zh) * 2022-08-23 2023-07-18 深圳市越疆科技股份有限公司 机器人及其控制方法、装置、设备、存储介质、机械臂
CN115958610B (zh) * 2023-03-16 2023-06-30 季华实验室 一种机械臂避障方法、装置、电子设备及存储介质
CN117075617B (zh) * 2023-10-12 2024-01-26 北京航空航天大学杭州创新研究院 机器人轨迹规划方法、装置、存储介质及电子设备

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102902269A (zh) * 2012-09-21 2013-01-30 北京邮电大学 预选择最小距离指标冗余机器人动态避障方法
CN104097205A (zh) * 2013-04-07 2014-10-15 同济大学 基于任务空间的机器人实时运动自碰撞避免控制方法
CN106078742A (zh) * 2016-06-29 2016-11-09 北京科技大学 一种针对带有输出约束的柔性机械臂的振动控制方法
CN107139171A (zh) * 2017-05-09 2017-09-08 浙江工业大学 一种基于力矩控制的工业机器人避障轨迹规划方法
CN108829137A (zh) * 2018-05-23 2018-11-16 中国科学院深圳先进技术研究院 一种机器人目标追踪的避障方法及装置
CN113146610A (zh) * 2020-12-08 2021-07-23 安徽工业大学 基于零空间避障的机械臂末端轨迹跟踪算法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102011106321A1 (de) * 2011-07-01 2013-01-03 Kuka Laboratories Gmbh Verfahren und Steuermittel zum Steuern eines Roboters
DE102014222809B3 (de) * 2014-11-07 2016-01-14 Kuka Roboter Gmbh Event-basierte Redundanzwinkelkonfiguartion für Gelenkarmroboter
JP2018126798A (ja) * 2017-02-06 2018-08-16 セイコーエプソン株式会社 制御装置、ロボットおよびロボットシステム
CN110696000B (zh) * 2019-11-21 2020-12-01 河北工业大学 一种机械臂试探感知的避障方法
CN111168675B (zh) * 2020-01-08 2021-09-03 北京航空航天大学 一种家用服务机器人的机械臂动态避障运动规划方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102902269A (zh) * 2012-09-21 2013-01-30 北京邮电大学 预选择最小距离指标冗余机器人动态避障方法
CN104097205A (zh) * 2013-04-07 2014-10-15 同济大学 基于任务空间的机器人实时运动自碰撞避免控制方法
CN106078742A (zh) * 2016-06-29 2016-11-09 北京科技大学 一种针对带有输出约束的柔性机械臂的振动控制方法
CN107139171A (zh) * 2017-05-09 2017-09-08 浙江工业大学 一种基于力矩控制的工业机器人避障轨迹规划方法
CN108829137A (zh) * 2018-05-23 2018-11-16 中国科学院深圳先进技术研究院 一种机器人目标追踪的避障方法及装置
CN113146610A (zh) * 2020-12-08 2021-07-23 安徽工业大学 基于零空间避障的机械臂末端轨迹跟踪算法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
A Novel Null-Space-Based UAV Trajectory Tracking Controller With Collision Avoidance;Milton Cesar Paes Santos;《A Novel Null-Space-Based UAV Trajectory Tracking Controller With Collision Avoidance》;IEEE;20170926;第2543-2553页 *
Human-Robot Variable Impedance Skills Transfer Learning Based on Dynamic Movement Primitives;Xinbo Yu;《Human-Robot Variable Impedance Skills Transfer Learning Based on Dynamic Movement Primitives》;IEEE Xplore;20220225;第6463-6470页 *
七轴冗余机器人避障运动规划研究;许志远;《七轴冗余机器人避障运动规划研究》;中国优秀硕士学位论文全文数据库 信息科技辑;20200515;I140-63 *
协作机器人智能控制与人机交互研究综述;黄海丰;《协作机器人智能控制与人机交互研究综述》;工程科学学报;20220409;第44卷(第4期);第780-791页 *

Also Published As

Publication number Publication date
CN114571469A (zh) 2022-06-03

Similar Documents

Publication Publication Date Title
CN114571469B (zh) 一种机械臂零空间实时避障控制方法及系统
Lim et al. Robot system of DRC‐HUBO+ and control strategy of team KAIST in DARPA robotics challenge finals
Zhang et al. A dual neural network for redundancy resolution of kinematically redundant manipulators subject to joint limits and joint velocity limits
US5430643A (en) Configuration control of seven degree of freedom arms
US5737500A (en) Mobile dexterous siren degree of freedom robot arm with real-time control system
Rahal et al. Caring about the human operator: haptic shared control for enhanced user comfort in robotic telemanipulation
Seraji et al. Motion control of 7-DOF arms: The configuration control approach
CN110825076B (zh) 基于视线和力反馈的移动机器人编队导航半自主控制方法
Iossifidis et al. Dynamical systems approach for the autonomous avoidance of obstacles and joint-limits for an redundant robot arm
Wan et al. A study on avoiding joint limits for inverse kinematics of redundant manipulators using improved clamping weighted least-norm method
Tan et al. Unified model approach for planning and control of mobile manipulators
WO2017132905A1 (zh) 控制运动系统的方法和装置
EP3845346A1 (en) Method, system and computer program product for controlling the teleoperation of a robotic arm
Walker et al. Robot-human handovers based on trust
Mbakop et al. Path planning and control of mobile soft manipulators with obstacle avoidance
You et al. Semi-autonomous bilateral teleoperation of hexapod robot based on haptic force feedback
Chung et al. Torque optimizing control with singularity-robustness for kinematically redundant robots
Zhang et al. Sensor-based redundancy resolution for a nonholonomic mobile manipulator
JP2018089736A (ja) マスタスレーブシステム
Li et al. Design and analysis of a whole-body controller for a velocity controlled robot mobile manipulator
Iossifidis et al. Autonomous reaching and obstacle avoidance with the anthropomorphic arm of a robotic assistant using the attractor dynamics approach
Shah et al. Modelling and optimization of robotic manipulator mechanism for computed tomography guided medical procedure
Li et al. Hybrid trajectory replanning-based dynamic obstacle avoidance for physical human-robot interaction
Wang et al. Design of stable visual servoing under sensor and actuator constraints via a Lyapunov-based approach
Sian et al. Operating humanoid robots in human environments

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