CN116494231A - 机械臂的控制方法、装置及电子设备 - Google Patents
机械臂的控制方法、装置及电子设备 Download PDFInfo
- Publication number
- CN116494231A CN116494231A CN202310464422.3A CN202310464422A CN116494231A CN 116494231 A CN116494231 A CN 116494231A CN 202310464422 A CN202310464422 A CN 202310464422A CN 116494231 A CN116494231 A CN 116494231A
- Authority
- CN
- China
- Prior art keywords
- joint
- speed
- mechanical arm
- jacobian matrix
- expected
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 96
- 239000011159 matrix material Substances 0.000 claims abstract description 230
- 238000012937 correction Methods 0.000 claims abstract description 118
- 238000004364 calculation method Methods 0.000 claims abstract description 20
- 239000013598 vector Substances 0.000 claims description 22
- 238000000354 decomposition reaction Methods 0.000 claims description 18
- 238000012545 processing Methods 0.000 claims description 11
- 238000012986 modification Methods 0.000 claims description 3
- 230000004048 modification Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 24
- 238000004891 communication Methods 0.000 description 10
- 230000006870 function Effects 0.000 description 8
- 230000008569 process Effects 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 238000013519 translation Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000004590 computer program Methods 0.000 description 2
- 238000010295 mobile communication Methods 0.000 description 2
- 239000007787 solid Substances 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
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/1607—Calculation of inertia, jacobian matrixes and inverses
-
- 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
-
- 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/1651—Programme controls characterised by the control loop acceleration, rate 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
- B25J9/1689—Teleoperation
-
- 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)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本申请涉及机械臂控制技术领域,公开了一种机械臂的控制方法、装置及电子设备,该机械臂包括末端和多个关节,该机械臂的控制方法,包括:修正第一雅可比矩阵,得到第二雅可比矩阵;基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度;根据最大约束速度阈值,对关节期望速度进行修正,得到第一修正结果;根据末端的运动方向与当前期望速度方向,对第一修正结果进行修正,得到第二修正结果;根据第二修正结果,得到修正后的关节期望速度,以控制机械臂的运动,本申请能够通过雅可比矩阵做最小二乘法计算,计算出最优的机械臂各关节的期望速度,进而使机械臂具有穿越奇异点的能力,在机械臂出现故障时也能够对机械臂进行遥控。
Description
技术领域
本申请实施方式涉及机械臂控制技术领域,特别是涉及一种机械臂的控制方法、装置及电子设备。
背景技术
机械臂遥控功能作为机械臂控制技术领域的一个基础功能一直备受重视。借助遥控功能,操作人员可以控制机械臂以预期的运动方式到达期望的位置。机械臂遥控技术一般有两种实现方式:在关节空间实现的机械臂遥控技术、在笛卡尔空间实现的机械臂遥控技术。
在关节空间实现的机械臂遥控技术的原理较为简单,只要控制关节转动到指定的角度即可,但这种遥控方式不符合人的直觉,而且无法满足笛卡尔空间的作业要求。在笛卡尔空间实现的机械臂遥控技术可以控制机械臂的末端在笛卡尔空间运动到指定的位置,更符合人的直觉,也可以满足笛卡尔空间的作业要求。但这种遥控方式缺乏对异常情况的适应能力,因为在笛卡尔空间控制机械臂需要求机械臂逆运动学的解,而出现下述状况时机械臂逆运动学是无解的。
(1)机械臂处于奇异点;
(2)机械臂中若干个关节出现故障,无法移动。
发明内容
本申请实施例提供一种机械臂的控制方法、装置及电子设备,通过雅可比矩阵做最小二乘法计算,计算出最优的机械臂各关节的期望速度,进而使机械臂具有穿越奇异点的能力,在机械臂出现故障时也能够对机械臂进行遥控。
本申请实施例提供以下技术方案:
第一方面,本申请实施例提供一种机械臂的控制方法,该机械臂包括末端和多个关节,该机械臂的控制方法,包括:
获取机械臂的各个关节的当前期望速度和关节角度;
基于机械臂微分运动学原理,得到第一雅可比矩阵;
修正第一雅可比矩阵,得到第二雅可比矩阵;
基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度;
根据最大约束速度阈值,对关节期望速度进行修正,得到第一修正结果;
根据末端的运动方向与当前期望速度方向,对第一修正结果进行修正,得到第二修正结果;
根据第二修正结果,得到修正后的关节期望速度,以控制机械臂的运动。
在一些实施例中,基于机械臂微分运动学原理,得到第一雅可比矩阵,包括:
基于机械臂微分运动学原理,根据机械臂的当前期望速度和关节角度,计算机械臂的当前雅可比矩阵;
将当前雅可比矩阵确定为第一雅可比矩阵。
在一些实施例中,修正第一雅可比矩阵,得到第二雅可比矩阵,包括:
将当前期望速度为零的关节确定为故障关节;
若存在故障关节,则将故障关节对应的列从第一雅可比矩阵中删除,得到第二雅可比矩阵,其中,每一关节一一对应第一雅克比矩阵中的一个列;
若不存在故障关节,则将第一雅可比矩阵确定为第二雅可比矩阵。
在一些实施例中,基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度,包括:
根据第二雅可比矩阵和第二雅可比矩阵的伪逆,确定第二雅可比矩阵的投影矩阵,具体包括:
其中,为第二雅可比矩阵的投影矩阵,Ja为第二雅可比矩阵,/>为第二雅可比矩阵的伪逆矩阵,U为第二雅可比矩阵进行奇异值分解得到的第一正交矩阵,V为第二雅可比矩阵进行奇异值分解得到的第二正交矩阵,Σ为第二雅可比矩阵进行奇异值分解得到的对角矩阵,UT为第一正交矩阵的转置矩阵,VT为第二正交矩阵的转置矩阵,Σ+为对角矩阵的伪逆矩阵;
根据第二雅可比矩阵的投影矩阵和当前期望速度,计算当前期望速度在第二雅可比矩阵列空间中的投影向量,具体包括:
其中,为当前期望速度在第二雅可比矩阵列空间中的投影向量,/>为第二雅可比矩阵的投影矩阵,/>为当前期望速度;
基于最小二乘法,结合投影向量、第二雅可比矩阵和期望速度,计算关节期望速度,具体包括:
其中,当时,则/>取得最小值,此时,得到关节期望速度/>等于/>
在一些实施例中,根据最大约束速度阈值,对关节期望速度进行修正,得到第一修正结果,包括:
设置最大约束速度阈值;
若存在若干个关节的关节期望速度大于最大约束速度阈值,则计算若干个关节的关节期望速度与最大约束速度阈值的比值,得到若干个比值;
将若干个比值中的最大比值确定为第一比值,并确定第一关节期望速度;
若所有的关节期望速度均小于或等于最大约束速度阈值,则将关节期望速度确定为第一关节期望速度;
将第一关节期望速度确定为第一修正结果;
将若干个比值中的最大比值确定为第一比值,并确定第一关节期望速度,具体包括:
根据第一比值,确定第一关节期望速度,具体包括:
其中,为第一关节期望速度,/>为关节期望速度,r为第一比值。
在一些实施例中,机械臂包括末端,根据末端的运动方向与当前期望速度方向,对第一修正结果进行修正,得到第二修正结果,包括:
计算末端的运动方向与当前期望速度方向的余弦值,计算公式为:
其中,cosα为末端的运动方向与当前期望速度方向的余弦值,为末端的速度,/>为当前期望速度,Ja为第二雅可比矩阵,/>为第一修正结果;
根据末端的运动方向与当前期望速度方向的夹角的余弦值,计算夹角的角度;
设置夹角的最大阈值;
若角度大于或等于夹角的最大阈值,则将机械臂的关节期望速度置为零,以停止机械臂运动;
若角度小于夹角的最大阈值,则将第一修正结果确定为第二修正结果。
在一些实施例中,在得到第二修正结果之后,方法还包括:
根据第二修正结果,计算末端修正后的运动速度,计算公式为:
其中,为末端修正后的运动速度,Ja为第二雅可比矩阵,/>为第二修正结果。
在一些实施例中,根据第二修正结果,得到修正后的关节期望速度,包括:
将第二修正结果确定为修正后的关节期望速度;
方法还包括:
获取刷新频率;
根据刷新频率,计算控制周期,计算公式为:
其中,Δt为控制周期,fc为刷新频率;
根据控制周期和第二修正结果,计算修正后的关节位置,计算公式为:
其中,qc为修正后的关节位置,q为机械臂的当前关节位置,为第二修正结果,Δt为控制周期。
第二方面,本申请实施例提供一种机械臂的控制装置,该机械臂包括末端和多个关节,该机械臂的控制装置,包括:
获取单元,用于获取机械臂的各个关节的当前期望速度和关节角度;
处理单元,用于基于机械臂微分运动学原理,得到第一雅可比矩阵;修正第一雅可比矩阵,得到第二雅可比矩阵;基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度;根据最大约束速度阈值,对关节期望速度进行修正,得到第一修正结果;根据末端的运动方向与当前期望速度方向,对第一修正结果进行修正,得到第二修正结果;根据第二修正结果,得到修正后的关节期望速度,以控制机械臂的运动。
第三方面,本申请实施例提供一种电子设备,包括:
至少一个处理器;和
与至少一个处理器通信连接的存储器;其中,
存储器存储有可被至少一个处理器执行的指令,指令被至少一个处理器执行,以使至少一个处理器能够执行如第一方面的机械臂的控制方法。
第四方面,本申请实施例提供一种非易失性计算机可读存储介质,非易失性计算机可读存储介质存储有计算机可执行指令,计算机可执行指令用于使电子设备执行如第一方面的机械臂的控制方法。
本申请实施方式的有益效果是:区别于现有技术的情况,本申请实施方式提供一种机械臂的控制方法,该机械臂包括末端和多个关节,该机械臂的控制方法,包括:获取机械臂的各个关节的当前期望速度和关节角度;基于机械臂微分运动学原理,得到第一雅可比矩阵;修正第一雅可比矩阵,得到第二雅可比矩阵;基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度;根据最大约束速度阈值,对关节期望速度进行修正,得到第一修正结果;根据末端的运动方向与当前期望速度方向,对第一修正结果进行修正,得到第二修正结果;根据第二修正结果,得到修正后的关节期望速度,以控制机械臂的运动,本申请能够通过雅可比矩阵做最小二乘法计算,计算出最优的机械臂各关节的期望速度,进而使机械臂具有穿越奇异点的能力,在机械臂出现故障时也能够对机械臂进行遥控。
附图说明
一个或多个实施例通过与之对应的附图进行示例性说明,这些示例性说明并不构成对实施例的限定,附图中具有相同参考数字标号的元件表示为类似的元件,除非有特别申明,附图中的图不构成比例限制。
图1是本申请实施例提供的一种应用环境的示意图;
图2是本申请实施例提供的一种机械臂的控制方法的流程示意图;
图3是图2中的步骤S202的细化流程示意图;
图4是图2中的步骤S203的细化流程示意图;
图5是图2中的步骤S204的细化流程示意图;
图6是图2中的步骤S205的细化流程示意图;
图7是图6中的步骤S2054的细化流程示意图;
图8是图2中的步骤S206的细化流程示意图;
图9是本申请实施例提供的一种计算末端修正后的运动速度的流程示意图;
图10是图2中的步骤S207的细化流程示意图;
图11是本申请实施例提供的一种计算修正后的关节位置的流程示意图;
图12是本申请实施例提供的一种机械臂的控制示意图;
图13是本申请实施例提供的一种机械臂的控制方法的整体示意图;
图14是本申请实施例提供的一种机械臂的控制装置的结构示意图;
图15是本申请实施例提供的一种电子设备的结构示意图。
附图标号说明:
标号 | 名称 | 标号 | 名称 |
100 | 应用环境 | 141 | 获取单元 |
10 | 机械臂 | 142 | 处理单元 |
20 | 服务器 | 150 | 电子设备 |
30 | 电子设备 | 151 | 处理器 |
140 | 机械臂的控制装置 | 152 | 存储器 |
具体实施方式
为了便于理解本申请,下面结合附图和具体实施方式,对本申请进行更详细的说明。需要说明的是,当元件被表述“固定于”另一个元件,它可以直接在另一个元件上、或者其间可以存在一个或多个居中的元件。当一个元件被表述“连接”另一个元件,它可以是直接连接到另一个元件、或者其间可以存在一个或多个居中的元件。本说明书所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只是为了说明的目的。
除非另有定义,本说明书所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本说明书中在本申请的说明书中所使用的术语只是为了描述具体的实施方式的目的,不是用于限制本申请。本说明书所使用的术语“和/或”包括一个或多个相关的所列项目的任意的和所有的组合。
下面结合说明书附图具体阐述本申请的技术方案:
请参阅图1,图1是本申请实施例提供的一种应用环境的示意图;
如图1所示,该应用环境100,包括:机械臂10、服务器20、以及电子设备30,其中,机械臂10与电子设备30通过网络通信连接,服务器20与电子设备30通过网络通信连接,其中,该网络包括有线网络和/或无线网络。可以理解的是,该网络包括2G、3G、4G、5G、无线局域网、蓝牙等无线网络,也可以包括串口线、网线等有线网络。
在本申请实施例中,该机械臂10包括多个运动关节、驱动器、感应器、末端工具、通信模块、控制器以及外壳。其中,多个运动关节用于控制机械臂的各个部分运动,驱动器包括离合器、变速器、万向传动装置、主减速器、差速器和半轴等,用于产生驱动力,驱动机械臂运动,感应器用于监测机械臂的位置、速度和加速度,末端工具用于完成各种任务,可以根据不同任务对机械臂的末端工具进行设置,如夹具、吸盘、刀具等,通信模块用于将机械臂的各个关节的当前期望速度和关节角度发送至服务器20,控制器用于接收电子设备30发送的期望速度,外壳用于固定和保护机械臂的各个部件。可以理解的是,该机械臂10包括但不限于液压驱动式机械臂、气压驱动式机械臂、电气驱动式机械臂、机械驱动式机械臂。
在本申请实施例中,通信模块,通信连接服务器20,用于接收服务器发送的指令,或者,向服务器发送数据,例如:接收服务器发送的控制机械臂运动的指令;或者,向服务器发送机械臂的各个关节的当前期望速度和关节角度的数据。在本申请实施例中,通信模块可以实现与因特网、互联网的通信,其中,通信模块包括但不限于WIFI模块、ZigBee模块、NB_IoT模块、4G模块、5G模块、蓝牙模块等通信单元。
在本申请实施例中,控制器设置于机械臂内部,控制器分别与驱动器、感应器以及末端工具电连接。该控制器还用于控制驱动器驱动机械臂运动到指定位置,或者,控制感应器获取机械臂各个关节的当前期望速度和关节角度的数据,或者,控制末端工具运动并执行相应的任务。
在本申请实施例中,控制器可以为通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)、单片机、ARM(Acorn RISC Machine)或其它可编程逻辑器件、分立门或晶体管逻辑、分立的硬件组件或者这些部件的任何组合。控制器还可以是任何传统处理器、控制器、微控制器或状态机。控制器也可以被实现为计算设备的组合,例如,DSP和微处理器的组合、多个微处理器、一个或多个微处理器结合DSP和/或任何其它这种配置,或者微控制单元(Microcontroller Unit,MCU)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)、系统级芯片(System on Chip,SoC)中的一种或多种组合。
在本申请实施例中,该服务器20,通信连接电子设备30,用于接收机械臂10发送的机械臂各个关节的当前期望速度和关节角度的数据,或者,向电子设备30发送控制命令,例如:控制电子设备对机械臂各个关节的当前期望速度进行修正,或者,控制机械臂运动。该服务器20的数量为多个,多个服务器可构成服务器集群,例如:该服务器集群包括:第一服务器、第二服务器,…,第N服务器,或者,该服务器集群可以是一个云计算服务中心,该云计算服务中心包括若干台服务器。本申请实施例中的服务器包括但不限于:塔式服务器、机架式服务器、刀片式服务器、云服务器。优选地,该服务器为云服务器(Elastic ComputeService,ECS)。
在本申请实施例中,该电子设备30包括6D鼠标,6D鼠标作为输入设备,通过电子设备将期望速度发送至机械臂的控制器,该电子设备30用于获取机械臂的各个关节的当前期望速度和关节角度,基于机械臂微分运动学原理,得到第一雅可比矩阵,修正第一雅可比矩阵,得到第二雅可比矩阵,基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度,根据最大约束速度阈值,对关节期望速度进行修正,得到第一修正结果,根据末端的运动方向与当前期望速度方向,对第一修正结果进行修正,得到第二修正结果,根据第二修正结果,得到修正后的关节期望速度,以控制机械臂的运动。可以理解的是,该电子设备30包括但不限于:固定电话、移动通信设备、移动个人计算机设备、或者其它具有上网功能的电子设备。
在本申请实施例中,该电子设备30包括控制器,控制器设置于电子设备内部。该控制器作为电子设备的控制核心,用于控制电子设备对机械臂的各个关节的当前期望速度,并将校正后的期望速度下发至机械臂的各个关节以及一些业务逻辑处理。
在本申请实施例中,控制器可以为通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)、单片机、ARM(Acorn RISC Machine)或其它可编程逻辑器件、分立门或晶体管逻辑、分立的硬件组件或者这些部件的任何组合。控制器还可以是任何传统处理器、控制器、微控制器或状态机。控制器也可以被实现为计算设备的组合,例如,DSP和微处理器的组合、多个微处理器、一个或多个微处理器结合DSP和/或任何其它这种配置,或者微控制单元(Microcontroller Unit,MCU)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)、系统级芯片(System on Chip,SoC)中的一种或多种组合。
可以理解的是,本申请实施例中的电子设备30还包括存储模块,存储模块包括但不限于:FLASH闪存、NAND闪存、垂直NAND闪存(VNAND)、NOR闪存、电阻随机存取存储器(RRAM)、磁阻随机存取存储器(MRAM)、铁电随机存取存储器(FRAM)、自旋转移扭矩随机存取存储器(STT-RAM)等设备中的一种或多种。
请参阅图2,图2是本申请实施例提供的一种机械臂的控制方法的流程示意图;
其中,该机械臂的控制方法,应用于电子设备,具体的,该机械臂的控制方法的执行主体为电子设备的一个或多个处理器,该机械臂包括末端、多个关节以及末端。
在本申请实施例中,笛卡尔空间为放射坐标系和笛卡尔坐标系平面向空间的推广:相交于原点的三条不共面的数轴构成空间的放射坐标系。三条数轴上度量单位相等的放射坐标系被称为空间笛卡尔坐标系。三条数轴互相垂直的笛卡尔坐标系被称为空间笛卡尔直角坐标系。本申请中的机械臂的控制方法应用于笛卡尔空间,可以满足在笛卡尔空间中作业的要求,也符合人的直觉。
如图2所示,该机械臂的控制方法,包括:
步骤S201:获取机械臂的各个关节的当前期望速度和关节角度;
具体的,将6D鼠标(3Dconnexion SpaceMouse)作为输入设备,其具有全方位立体控制能力,有前、后、左、右、上、下六个移动方向,而且可以组合出前右,左下等移动方向,使用6D鼠标作为输入设备的好处在于让用户更有操纵感和代入感,减少出错的概率,通过6D鼠标产生期望速度,该期望速度包括三维空间的期望线速度与三维空间的期望角速度,可通过6D鼠标操控机械臂在笛卡尔空间中运动。需要注意的是,该期望速度可以根据实际需要进行设置。在本申请实施例中,由电子设备读取6D鼠标发送的期望速度,同时通过机械臂各关节驱动器读取各个关节的编码器数据以获取关节角度,从而获取机械臂的各个关节的当前期望速度和关节角度,该当前期望速度为矢量,具有速度大小及速度方向,假设当前期望速度为则当前期望速度的表达式如下:
其中,为当前期望速度,ve为当前期望速度的期望线速度,ωe为当前期望速度的期望角速度,vex为期望线速度沿x轴平移的分速度,vey为期望线速度沿y轴平移的分速度,vez为期望线速度沿z轴平移的分速度,ωex为期望角速度沿x轴旋转的分速度,ωey为期望角速度沿y轴旋转的分速度,ωez为期望角速度沿z轴旋转的分速度。
步骤S202:基于机械臂微分运动学原理,得到第一雅可比矩阵;
请再参阅图3,图3是图2中的步骤S202的细化流程示意图;
如图3所示,步骤S202:基于机械臂微分运动学原理,得到第一雅可比矩阵,包括:
步骤S2021:基于机械臂微分运动学原理,根据机械臂的当前期望速度和关节角度,计算机械臂的当前雅可比矩阵;
具体的,机械臂微分运动学原理包括机械臂末端、加速度与关节速度、加速度之间的关系。雅可比矩阵是一个矢量的每一个元素对另一个矢量的每一个元素进行偏微分而组成的矩阵,可以用于对两个矢量之间的非线性关系进行局部线性化。而在机械臂运动学中,雅可比矩阵指的则是机械臂末端(end-effector)的位姿(位置与姿态)与机械臂各个关节位置值的偏微分关系。
在本申请实施例中,基于机械臂微分运动学原理,根据机械臂的当前期望速度和关节角度,以六轴机械臂为例,当前期望速度包括三维空间中的期望线速度(x,y,z)和期望角速度(θx,θy,θz),将当前期望速度的向量组记为F={x,y,z,θx,θy,θz},将当前期望速度关于关节角度q的函数记为f(q),有如下等式:
对上述等式进行进一步的计算,对向量组F中的每一项(即等式的左边)进行全微分,在上述等式的右边对函数中所有的q求偏导数,得到的计算结果可转化为如下矩阵形式,得到机械臂的当前雅可比矩阵:
步骤S2022:将当前雅可比矩阵确定为第一雅可比矩阵;
具体的,将当前雅可比矩阵确定为第一雅可比矩阵,该第一雅可比矩阵即为待修正的雅可比矩阵,以六轴机械臂为例,该六轴机械臂包括六个关节,在常规情况下,该六轴机械臂的第一雅可比矩阵为6x6的形式,其表达式如下:
其中,Jr为第一雅可比矩阵,由于第一雅可比矩阵是从关节的当前期望速度矢量向笛卡尔操作空间速度矢量的线性映射,因此第一雅可比矩阵的每一列代表机械臂6个关节的当前速度和末端线速度和角速度的传递比。
步骤S203:修正第一雅可比矩阵,得到第二雅可比矩阵;
请再参阅图4,图4是图2中的步骤S203的细化流程示意图;
如图4所示,步骤S203:修正第一雅可比矩阵,得到第二雅可比矩阵,包括:
步骤S2031:将当前期望速度为零的关节确定为故障关节;
具体的,机械臂包括多个关节,每一关节一一对应第一雅克比矩阵中的一个列,当若干个关节故障而无法运动时,即代表这些关节的当前期望速度锁定为零,将当前期望速度为零的关节确定为故障关节。
步骤S2032:判断是否存在故障关节;
具体的,判断是否存在故障关节,即判断是否存在当前期望速度为零的关节,若存在故障关节,则进入步骤S2033;若不存在故障关节,则进入步骤S2034。
步骤S2033:将故障关节对应的列从第一雅可比矩阵中删除,得到第二雅可比矩阵;
具体的,在第一雅可比矩阵中,每一列代表一个关节,若存在故障关节,则将故障关节对应的列从第一雅可比矩阵中删除,得到第二雅可比矩阵,以六轴机械臂为例,假设该六轴机械臂的第2关节和第6关节确定为故障关节,则得到第二雅克比矩阵的表达式如下:
其中,Ja为第二雅可比矩阵。
步骤S2034:将第一雅可比矩阵确定为第二雅可比矩阵;
具体的,若不存在故障关节,则将第一雅可比矩阵确定为第二雅可比矩阵,即当所有关节均未发生故障时,Ja=Jr。
在本申请实施例中,当若干关节出现故障而无法移动,即当前期望速度为零时,则对雅可比矩阵进行修改,再做最小二乘法计算。进而使机械臂在此种异常情况下依旧具备在笛卡尔空间被遥控的能力。在实际的应用场景中,机械臂在远离用户并且处在复杂工作环境中时,当若干个关节出现故障时,用户往往希望通过控制机械臂在笛卡尔空间运动从而实现脱困,此时就需要机械臂具备处理故障关节的能力。
步骤S204:基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度;
请再参阅图5,图5是图2中的步骤S204的细化流程示意图;
如图5所示,步骤S204:基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度,包括:
在本申请实施例中,基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度指的是假设Ja为第二雅可比矩阵,为当前期望速度,根据Ja和/>求解如下形式的最小二乘问题:
其中,Ja和为已知量,/>为关节期望速度(不包含故障关节的关节期望速度),由于/>为未知量,需要按照下述最小二乘法进行求解。
步骤S2041:根据第二雅可比矩阵和第二雅可比矩阵的伪逆,确定第二雅可比矩阵的投影矩阵;
具体的,首先,将第二雅克比矩阵做奇异值分解,得到下式:
Ja=UΣVT,
其中,Ja为第二雅可比矩阵,U为第二雅可比矩阵进行奇异值分解得到的第一正交矩阵,V为第二雅可比矩阵进行奇异值分解得到的第二正交矩阵,Σ为第二雅可比矩阵进行奇异值分解得到的对角矩阵,VT为第二正交矩阵的转置矩阵。第一正交矩阵U的计算方式为将做特征值分解得到/>因为/>是对称矩阵,所以U是正交矩阵,而且所有特征值都是非负数,这些特征值位于Λ1对角线上,将每个特征值都开平方,即得到Σ中对角线上的元素,这种关系也可以表示为:Λ1=ΣΣT,得到对角矩阵Σ。第二正交矩阵V的计算方式为将/>做特征值分解得到/>因为/>是对称矩阵,所以V是正交矩阵,而且所有特征值都是非负数,这些特征值位于Λ2对角线上,将每个特征值都开平方,利用这种计算方式也可以得到Σ中对角线上的元素,这种关系也可以表示为:Λ2=ΣTΣ,得到对角矩阵Σ。
其次,根据第二雅可比矩阵,计算第二雅可比矩阵的伪逆矩阵,得到第二雅克比矩阵的伪逆矩阵的表达式如下:
其中,为第二雅可比矩阵的伪逆矩阵,V为第二雅可比矩阵进行奇异值分解得到的第二正交矩阵,Σ+为对角矩阵的伪逆矩阵,UT为第一正交矩阵的转置矩阵。其中Σ+为Σ的伪逆。因为Σ是对角矩阵,所以将Σ转置并将其中所有非0元素取倒数即可得到Σ+。此外,因为U和V是正交矩阵,所以有:
UT=U-1,
VT=V-1。
最后,将第二雅可比矩阵与第二雅可比矩阵的伪逆矩阵相乘,即可确定第二雅可比矩阵的投影矩阵,其表达式如下:
其中,为第二雅可比矩阵的投影矩阵,Ja为第二雅可比矩阵,/>为第二雅可比矩阵的伪逆矩阵,U为第二雅可比矩阵进行奇异值分解得到的第一正交矩阵,V为第二雅可比矩阵进行奇异值分解得到的第二正交矩阵,Σ为第二雅可比矩阵进行奇异值分解得到的对角矩阵,UT为第一正交矩阵的转置矩阵,VT为第二正交矩阵的转置矩阵,Σ+为对角矩阵的伪逆矩阵。
步骤S2042:根据第二雅可比矩阵的投影矩阵和当前期望速度,计算当前期望速度在第二雅可比矩阵列空间中的投影向量;
具体的,根据第二雅可比矩阵的投影矩阵和当前期望速度,计算当前期望速度在第二雅可比矩阵列空间中的投影向量,其表达式如下:
其中,为当前期望速度在第二雅可比矩阵列空间中的投影向量,/>为第二雅可比矩阵的投影矩阵,/>为当前期望速度。
步骤S2043:基于最小二乘法,结合投影向量、第二雅可比矩阵和期望速度,计算关节期望速度;
具体的,基于最小二乘法,结合投影向量、第二雅可比矩阵和期望速度,计算关节期望速度,其表达式如下:
其中,因为是/>在Ja列空间中的投影,所以/>是Ja列空间中距离/>最近的向量。而当/>时,则/>进而可得当/>时,/>取得最小值。因此/>是此最小二乘问题的解,此时,得到关节期望速度/>等于/>
在本申请实施例中,当机械臂处于奇异点时,雅可比矩阵成为奇异矩阵,无法求逆,本申请利用雅可比矩阵做最小二乘法计算,计算出最优的机械臂各关节的期望速度,进而使机械臂具有穿越奇异点的能力,并且,采用伪逆进行最小二乘法计算的好处在于通用性强,不论雅可比矩阵是否为列满秩矩阵都可以进行计算,即该方法适用于多种雅可比矩阵。
步骤S205:根据最大约束速度阈值,对关节期望速度进行修正,得到第一修正结果;
请再参阅图6,图6是图2中的步骤S205的细化流程示意图;
如图6所示,步骤S205:根据最大约束速度阈值,对关节期望速度进行修正,得到第一修正结果,包括:
步骤S2051:设置最大约束速度阈值;
具体的,根据实际情况对最大约束速度阈值进行设置,例如,将最大约束速度阈值设置为0.5弧度/秒。
步骤S2052:判断是否存在若干个关节的关节期望速度大于最大约束速度阈值;
具体的,判断是否存在若干个关节的关节期望速度大于最大约束速度阈值,即判断关节期望速度中是否存在某些关节的速度超过最大约束速度阈值的限制,若存在若干个关节的关节期望速度大于最大约束速度阈值,则进入步骤S2053;若所有的关节期望速度均小于或等于最大约束速度阈值,则进入步骤S2055。
步骤S2053:计算若干个关节的关节期望速度与最大约束速度阈值的比值,得到若干个比值;
具体的,若存在若干个关节的关节期望速度大于最大约束速度阈值,说明需要对关节期望速度进行修正,则计算该若干个关节的关节期望速度与最大约束速度阈值的比值,得到若干个比值。
步骤S2054:将若干个比值中的最大比值确定为第一比值,并确定第一关节期望速度;
请再参阅图7,图7是图6中的步骤S2054的细化流程示意图;
如图7所示,步骤S2054:将若干个比值中的最大比值确定为第一比值,并确定第一关节期望速度,包括:
步骤S2541:根据第一比值,确定第一关节期望速度;
具体的,将若干个比值中的最大比值确定为第一比值,根据第一比值,确定第一关节期望速度,第一关节期望速度的计算公式如下:
其中,为第一关节期望速度,/>为关节期望速度,r为第一比值。
步骤S2055:将关节期望速度确定为第一关节期望速度;
具体的,若所有的关节期望速度均小于或等于最大约束速度阈值,则表示所有的关节期望速度均未超过最大约束速度阈值,不需要对关节期望速度进行修正,将关节期望速度确定为第一关节期望速度。
步骤S2056:将第一关节期望速度确定为第一修正结果;
具体的,将第一关节期望速度确定为第一修正结果表示第一次修正已完成。
步骤S206:根据末端的运动方向与当前期望速度方向,对第一修正结果进行修正,得到第二修正结果;
请再参阅图8,图8是图2中的步骤S206的细化流程示意图;
如图8所示,步骤S206:根据末端的运动方向与当前期望速度方向,对第一修正结果进行修正,得到第二修正结果,包括:
步骤S2061:计算末端的运动方向与当前期望速度方向的余弦值;
具体的,计算第二雅可比矩阵Ja的秩,秩是线性代数术语,在线性代数中,一个矩阵的秩是其非零子式的最高阶数,一个向量组的秩则是其最大无关组所含的向量个数,其计算方式为采用高斯消元法把第二雅可比矩阵化为行阶梯型矩阵,该行阶梯型矩阵的非零行的数量即为第二雅可比矩阵的秩。机械臂包括末端,由电子设备获取机械臂的末端的运动方向,此时,需要对第一关节期望速度进行第二次修正,当关节以第一关节期望速度速度运动时,机械臂末端以/>速度运动,假设末端的运动速度为/>则有/>末端的运动速度/>为矢量,包括速度大小和末端的运动方向。当第二雅可比矩阵ja的秩小于预设值时,说明末端的运动速度/>与当前期望速度/>的方向不重合,会导致机械臂末端的运动方向和期望的运动方向产生偏差,需要进行第二次修正,其中,该预设值可以根据实际情况进行设置,例如,将预设值设置为6,本申请对此不做限定。
计算末端的运动方向与当前期望速度方向的余弦值,具体包括:
其中,cosα为末端的运动方向与当前期望速度方向的余弦值,为末端的速度,/>为当前期望速度,Ja为第二雅可比矩阵,/>为第一修正结果。
步骤S2062:根据末端的运动方向与当前期望速度方向的夹角的余弦值,计算夹角的角度;
具体的,根据末端的运动方向与当前期望速度方向的夹角的余弦值cosα,利用arccos反余弦函数可以计算出该余弦值对应的夹角的角度α。
步骤S2063:设置夹角的最大阈值;
具体的,根据实际情况设置夹角的最大阈值αmax,若末端的运动方向与当前期望速度方向的夹角大于或等于夹角的最大阈值,说明机械臂末端的运动方向和当前期望速度的运动方向的偏差过大,若末端的运动方向与当前期望速度方向的夹角小于夹角的最大阈值,说明机械臂末端的运动方向和当前期望速度的运动方向的偏差处于合理范围内。
在本申请实施例中,夹角的最大阈值αmax可以根据具体需要进行设置,例如:设定为π/30弧度,即6°。
步骤S2064:判断角度是否大于或等于夹角的最大阈值;
具体的,判断角度是否大于或等于夹角的最大阈值,若末端的运动方向与当前期望速度方向的夹角大于或等于夹角的最大阈值,则进入步骤S2065;若末端的运动方向与当前期望速度方向的夹角小于夹角的最大阈值,则进入步骤S2066。
步骤S2065:将机械臂的关节期望速度置为零,以停止机械臂运动;
具体的,若末端的运动方向与当前期望速度方向的夹角大于或等于夹角的最大阈值,说明机械臂末端的运动方向和当前期望速度的运动方向的偏差过大,将机械臂的关节期望速度置为零,以停止机械臂运动。
步骤S2066:将第一修正结果确定为第二修正结果;
具体的,若末端的运动方向与当前期望速度方向的夹角小于夹角的最大阈值,说明机械臂末端的运动方向和当前期望速度的运动方向的偏差处于合理范围内,将第一修正结果确定为第二修正结果,即最终得到的第二修正结果为:/>
其中,为第二修正结果,α为末端的运动方向与当前期望速度方向的夹角,αmax为夹角的最大阈值。
在本申请实施例中,对各个关节的当前期望速度进行两次修正,得到第一修正结果和第二修正结果,可以综合考虑对机械臂关节速度的限制和对机械臂运动方向的限制,保证了机械臂控制的稳定性和安全性。
请再参阅图9,图9是本申请实施例提供的一种计算末端修正后的运动速度的流程示意图;
如图9所示,计算末端修正后的运动速度的流程,包括:
步骤S901:根据第二修正结果,计算末端修正后的运动速度;
具体的,根据第二修正结果可以计算得到末端修正后的运动速度为/>
步骤S207:根据第二修正结果,得到修正后的关节期望速度,以控制机械臂的运动;
请再参阅图10,图10是图2中的步骤S207的细化流程示意图;
如图10所示,步骤S207:根据第二修正结果,得到修正后的关节期望速度,以控制机械臂的运动,包括:
步骤S2071:将第二修正结果确定为修正后的关节期望速度;
具体的,将第二修正结果确定为修正后的关节期望速度,修正后的关节期望速度用于对机械臂的位姿(位置与姿态)进行控制。
请再参阅图11,图11是本申请实施例提供的一种计算修正后的关节位置的流程示意图;
如图11所示,计算修正后的关节位置的流程,包括:
步骤S1101:获取刷新频率;
具体的,该刷新频率由输入设备的信号频率决定,若输入设备为6D鼠标,则获取刷新频率,即为获取6D鼠标的信号频率,例如,6D鼠标的信号频率为100Hz。
步骤S1102:根据刷新频率,计算控制周期;
具体的,根据刷新频率,计算控制周期,计算公式如下:
其中,Δt为控制周期,fc为刷新频率。
步骤S1103:根据控制周期和第二修正结果,计算修正后的关节位置;
具体的,根据控制周期Δt和第二修正结果将/>与Δt相乘得到位置增量/>再将该位置增量与当前位置q相加,得到修正后的关节位置qc,最后将qc下发至机械臂各个关节的驱动器,以驱动关节电机运行,修正后的关节位置的计算公式如下:
其中,qc为修正后的关节位置,q为机械臂的当前关节位置,为第二修正结果,Δt为控制周期。/>
请再参阅图12,图12是本申请实施例提供的一种机械臂的控制示意图;
如图12所示,以六轴机械臂为例,该六轴机械臂包括六个关节,输入设备发送当前期望速度至机械臂的控制器,通过上述机械臂的控制方法进行修正,可以得到机械臂的末端修正后的运动速度。
请再参阅图13,图13是本申请实施例提供的一种机械臂的控制方法的整体示意图;
如图13所示,输入当前期望速度,获取机械臂各个关节的当前期望速度和关节角度,基于机械臂微分运动学原理,得到第一雅可比矩阵;获取机械臂故障关节的信息,修正第一雅可比矩阵,得到第二雅可比矩阵;基于最小二乘法,结合当前期望速度与第二雅克比矩阵,得到关节期望速度;设置最大约束速度阈值,对关节期望速度进行第一次修正,得到第一修正结果;根据末端的运动方向与当前期望速度方向,对第一修正结果进行第二次修正,得到第二修正结果;结合机械臂的当前关节位置和第二修正结果进行积分,得到修正后的关节位置;最后将修正后的关节位置下发至机械臂的驱动器,以控制机械臂的运动。
在本申请实施例中,通过提供一种机械臂的控制方法,该机械臂包括末端和多个关节,该方法包括:获取机械臂的各个关节的当前期望速度和关节角度;基于机械臂微分运动学原理,得到第一雅可比矩阵;修正第一雅可比矩阵,得到第二雅可比矩阵;基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度;根据最大约束速度阈值,对关节期望速度进行修正,得到第一修正结果;根据末端的运动方向与当前期望速度方向,对第一修正结果进行修正,得到第二修正结果;根据第二修正结果,得到修正后的关节期望速度,以控制机械臂的运动,本申请能够通过雅可比矩阵做最小二乘法计算,计算出最优的机械臂各关节的期望速度,进而使机械臂具有穿越奇异点的能力,在机械臂出现故障时也能够对机械臂进行遥控。
请再参阅图14,图14是本申请实施例提供的一种机械臂的控制装置的结构示意图;
其中,该机械臂的控制装置,应用于电子设备,具体的,该机械臂的控制装置应用于电子设备的一个或多个处理器。
如图14所示,该机械臂的控制装置140,包括:
获取单元141,用于获取机械臂的各个关节的当前期望速度和关节角度;
处理单元142,用于基于机械臂微分运动学原理,得到第一雅可比矩阵;修正第一雅可比矩阵,得到第二雅可比矩阵;基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度;根据最大约束速度阈值,对关节期望速度进行修正,得到第一修正结果;根据末端的运动方向与当前期望速度方向,对第一修正结果进行修正,得到第二修正结果;根据第二修正结果,得到修正后的关节期望速度,以控制机械臂的运动。
在本申请实施例中,获取单元141还用于:
获取刷新频率。
在本申请实施例中,该处理单元142还用于:
根据第二修正结果,计算末端修正后的运动速度。
在本申请实施例中,该机械臂的控制装置亦可以由硬件器件搭建成的,例如,机械臂的控制装置可以由一个或两个以上的芯片搭建而成,各个芯片可以互相协调工作,以完成上述各个实施例所阐述的机械臂的控制方法。再例如,机械臂的控制装置还可以由各类逻辑器件搭建而成,诸如由通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)、单片机、ARM(Acorn RISC Machine)或其它可编程逻辑器件、分立门或晶体管逻辑、分立的硬件组件或者这些部件的任何组合而搭建成。
本申请实施例中的机械臂的控制装置可以是装置,也可以是终端中的部件、集成电路、或芯片。该装置可以是移动电子设备,也可以为非移动电子设备。示例性的,移动电子设备可以为手机、平板电脑、笔记本电脑、掌上电脑、车载电子设备、可穿戴设备、超级移动个人计算机(ultra-mobile personal computer,UMPC)、上网本或者个人数字助理(personal digital assistant,PDA)等,非移动电子设备可以为服务器、网络附属存储器(Network Attached Storage,NAS)、个人计算机(personal computer,PC)、电视机(television,TV)、柜员机或者自助机等,本申请实施例不作具体限定。
本申请实施例中的机械臂的控制装置可以为具有操作系统的装置。该操作系统可以为安卓(Android)操作系统,可以为ios操作系统,还可以为其他可能的操作系统,本申请实施例不作具体限定。
本申请实施例提供的机械臂的控制装置能够实现图2实现的各个过程,为避免重复,这里不再赘述。
需要说明的是,上述机械臂的控制装置可执行本申请实施例所提供的机械臂的控制方法,具备执行方法相应的功能模块和有益效果。未在机械臂的控制装置实施例中详尽描述的技术细节,可参考上述实施例所提供的机械臂的控制方法。
在本申请实施例中,通过提供一种机械臂的控制装置,包括:获取单元,用于获取机械臂的各个关节的当前期望速度和关节角度;处理单元,用于基于机械臂微分运动学原理,得到第一雅可比矩阵;修正第一雅可比矩阵,得到第二雅可比矩阵;基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度;根据最大约束速度阈值,对关节期望速度进行修正,得到第一修正结果;根据末端的运动方向与当前期望速度方向,对第一修正结果进行修正,得到第二修正结果;根据第二修正结果,得到修正后的关节期望速度,以控制机械臂的运动,本申请能够通过雅可比矩阵做最小二乘法计算,计算出最优的机械臂各关节的期望速度,进而使机械臂具有穿越奇异点的能力,在机械臂出现故障时也能够对机械臂进行遥控。
请再参阅图15,图15是本申请实施例提供的一种电子设备的结构示意图;
如图15所示,该电子设备150包括一个或多个处理器151以及存储器152。其中,图15中以一个处理器151为例。
处理器151和存储器152可以通过总线或者其他方式连接,图15中以通过总线连接为例。
处理器151,用于提供计算和控制能力,以控制电子设备150执行相应任务,例如,电子设备150执行上述任一方法实施例中的机械臂的控制方法,包括:获取机械臂的各个关节的当前期望速度和关节角度;基于机械臂微分运动学原理,得到第一雅可比矩阵;修正第一雅可比矩阵,得到第二雅可比矩阵;基于最小二乘法,根据第二雅克比矩阵,计算机械臂的关节期望速度;根据最大约束速度阈值,对关节期望速度进行修正,得到第一修正结果;根据末端的运动方向与当前期望速度方向,对第一修正结果进行修正,得到第二修正结果;根据第二修正结果,得到修正后的关节期望速度,以控制机械臂的运动,本申请能够通过雅可比矩阵做最小二乘法计算,计算出最优的机械臂各关节的期望速度,进而使机械臂具有穿越奇异点的能力,在机械臂出现故障时也能够对机械臂进行遥控。
通过雅可比矩阵做最小二乘法计算,计算出最优的机械臂各关节的期望速度,进而使机械臂具有穿越奇异点的能力,在机械臂出现故障时也能够对机械臂进行遥控。
处理器151可以是通用处理器,包括中央处理器(Central Processing Unit,CPU)、网络处理器(Network Processor,NP)、硬件芯片或者其任意组合;还可以是数字信号处理器(Digital Signal Processing,DSP)、专用集成电路(Application SpecificIntegrated Circuit,ASIC)、可编程逻辑器件(programmable logic device,PLD)或其组合。上述PLD可以是复杂可编程逻辑器件(complex programmable logic device,CPLD),现场可编程逻辑门阵列(field-programmable gate array,FPGA),通用阵列逻辑(genericarray logic,GAL)或其任意组合。
存储器152作为一种非暂态计算机可读存储介质,可用于存储非暂态软件程序、非暂态性计算机可执行程序以及模块,如本申请实施例中的机械臂的控制方法对应的程序指令/模块。处理器151通过运行存储在存储器152中的非暂态软件程序、指令以及模块,可以实现下述任一方法实施例中的机械臂的控制方法。具体地,存储器152可以包括易失性存储器(volatile memory,VM),例如随机存取存储器(random access memory,RAM);存储器152也可以包括非易失性存储器(non-volatile memory,NVM),例如只读存储器(read-onlymemory,ROM),快闪存储器(flash memory),硬盘(hard disk drive,HDD)或固态硬盘(solid-state drive,SSD)或其他非暂态固态存储器件;存储器152还可以包括上述种类的存储器的组合。
存储器152可以包括高速随机存取存储器,还可以包括非易失性存储器,例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中,存储器152可选包括相对于处理器151远程设置的存储器,这些远程存储器可以通过网络连接至处理器151。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
一个或者多个模块存储在存储器152中,当被一个或者多个处理器151执行时,执行上述任意方法实施例中的机械臂的控制方法,例如,执行以上描述的图2所示的各个步骤;也可实现图14的各个模块或单元的功能。
在本申请实施例中,电子设备150还可以具有有线或无线网络接口、键盘以及输入输出接口等部件,以便进行输入输出,电子设备150还可以包括其他用于实现设备功能的部件,在此不做赘述。
本申请实施例还提供了一种计算机可读存储介质,例如包括程序代码的存储器,上述程序代码可由处理器执行以完成上述实施例中的机械臂的控制方法。例如,该计算机可读存储介质可以是只读存储器(Read-Only Memory,ROM)、随机存取存储器(RandomAccess Memory,RAM)、只读光盘(Compact Disc Read-Only Memory,CDROM)、磁带、软盘和光数据存储设备等。
本申请实施例还提供了一种计算机程序产品,该计算机程序产品包括一条或多条程序代码,该程序代码存储在计算机可读存储介质中。电子设备的处理器从计算机可读存储介质读取该程序代码,处理器执行该程序代码,以完成上述实施例中提供的机械臂的控制方法的方法步骤。
本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成,也可以通过程序来程序代码相关的硬件完成,该程序可以存储于一种计算机可读存储介质中,上述提到的存储介质可以是只读存储器,磁盘或光盘等。
通过以上的实施方式的描述,本领域普通技术人员可以清楚地了解到各实施方式可借助软件加通用硬件平台的方式来实现,当然也可以通过硬件。本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程是可以通过计算机程序来指令相关的硬件来完成,程序可存储于一计算机可读取存储介质中,该程序在执行时,可包括如上述各方法的实施例的流程。其中,存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory,ROM)或随机存储记忆体(Random Access Memory,RAM)等。
最后应说明的是:以上实施例仅用以说明本申请的技术方案,而非对其限制;在本申请的思路下,以上实施例或者不同实施例中的技术特征之间也可以进行组合,步骤可以以任意顺序实现,并存在如上述的本申请的不同方面的许多其它变化,为了简明,它们没有在细节中提供;尽管参照前述实施例对本申请进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本申请各实施例技术方案的范围。
Claims (10)
1.一种机械臂的控制方法,其特征在于,所述机械臂包括末端和多个关节,所述方法包括:
获取所述机械臂的各个关节的当前期望速度和关节角度;
基于机械臂微分运动学原理,得到第一雅可比矩阵;
修正所述第一雅可比矩阵,得到第二雅可比矩阵;
基于最小二乘法,根据所述第二雅克比矩阵,计算所述机械臂的关节期望速度;
根据最大约束速度阈值,对所述关节期望速度进行修正,得到第一修正结果;
根据所述末端的运动方向与当前期望速度方向,对所述第一修正结果进行修正,得到第二修正结果;
根据第二修正结果,得到修正后的关节期望速度,以控制所述机械臂的运动。
2.根据权利要求1所述的方法,其特征在于,所述基于机械臂微分运动学原理,得到第一雅可比矩阵,包括:
基于机械臂微分运动学原理,根据所述机械臂的当前期望速度和所述关节角度,计算所述机械臂的当前雅可比矩阵;
将所述当前雅可比矩阵确定为第一雅可比矩阵。
3.根据权利要求2所述的方法,其特征在于,所述修正所述第一雅可比矩阵,得到第二雅可比矩阵,包括:
将当前期望速度为零的关节确定为故障关节;
若存在所述故障关节,则将所述故障关节对应的列从第一雅可比矩阵中删除,得到第二雅可比矩阵,其中,每一所述关节一一对应所述第一雅克比矩阵中的一个列;
若不存在所述故障关节,则将所述第一雅可比矩阵确定为第二雅可比矩阵。
4.根据权利要求3所述的方法,其特征在于,所述基于最小二乘法,根据所述第二雅克比矩阵,计算所述机械臂的关节期望速度,包括:
根据所述第二雅可比矩阵和所述第二雅可比矩阵的伪逆,确定所述第二雅可比矩阵的投影矩阵,具体包括:
其中,为第二雅可比矩阵的投影矩阵,Ja为第二雅可比矩阵,/>为所述第二雅可比矩阵的伪逆矩阵,U为第二雅可比矩阵进行奇异值分解得到的第一正交矩阵,V为第二雅可比矩阵进行奇异值分解得到的第二正交矩阵,Σ为第二雅可比矩阵进行奇异值分解得到的对角矩阵,UT为第一正交矩阵的转置矩阵,VT为第二正交矩阵的转置矩阵,Σ+为对角矩阵的伪逆矩阵;
根据所述第二雅可比矩阵的投影矩阵和所述当前期望速度,计算所述当前期望速度在第二雅可比矩阵列空间中的投影向量,具体包括:
其中,为所述当前期望速度在第二雅可比矩阵列空间中的投影向量,/>为第二雅可比矩阵的投影矩阵,/>为所述当前期望速度;
基于最小二乘法,结合所述投影向量、第二雅可比矩阵和期望速度,计算所述关节期望速度,具体包括:
其中,当时,则/>取得最小值,此时,得到所述关节期望速度/>等于/>
5.根据权利要求3所述的方法,其特征在于,所述根据最大约束速度阈值,对所述关节期望速度进行修正,得到第一修正结果,包括:
设置最大约束速度阈值;
若存在若干个关节的关节期望速度大于所述最大约束速度阈值,则计算若干个所述关节的关节期望速度与最大约束速度阈值的比值,得到若干个比值;
将若干个所述比值中的最大比值确定为第一比值,并确定第一关节期望速度;
若所有的关节期望速度均小于或等于所述最大约束速度阈值,则将所述关节期望速度确定为第一关节期望速度;
将第一关节期望速度确定为第一修正结果;
所述将若干个所述比值中的最大比值确定为第一比值,并确定第一关节期望速度,具体包括:
根据第一比值,确定第一关节期望速度,具体包括:
其中,为第一关节期望速度,/>为关节期望速度,r为第一比值。
6.根据权利要求5所述的方法,其特征在于,所述机械臂包括末端,所述根据所述末端的运动方向与当前期望速度方向,对所述第一修正结果进行修正,得到第二修正结果,包括:
计算所述末端的运动方向与当前期望速度方向的余弦值,计算公式为:
其中,cosα为末端的运动方向与当前期望速度方向的余弦值,为末端的速度,/>为当前期望速度,Ja为第二雅可比矩阵,/>为所述第一修正结果;
根据所述末端的运动方向与当前期望速度方向的夹角的余弦值,计算夹角的角度;
设置夹角的最大阈值;
若所述角度大于或等于所述夹角的最大阈值,则将机械臂的关节期望速度置为零,以停止机械臂运动;
若所述角度小于所述夹角的最大阈值,则将所述第一修正结果确定为第二修正结果。
7.根据权利要求1所述的方法,其特征在于,在得到第二修正结果之后,所述方法还包括:
根据所述第二修正结果,计算末端修正后的运动速度,计算公式为:
其中,为末端修正后的运动速度,Ja为第二雅可比矩阵,/>为第二修正结果。
8.根据权利要求7所述的方法,其特征在于,所述根据第二修正结果,得到修正后的关节期望速度,包括:
将所述第二修正结果确定为修正后的关节期望速度;
所述方法还包括:
获取刷新频率;
根据所述刷新频率,计算控制周期,计算公式为:
其中,Δt为控制周期,fc为刷新频率;
根据所述控制周期和所述第二修正结果,计算修正后的关节位置,计算公式为:
其中,qc为修正后的关节位置,q为机械臂的当前关节位置,为第二修正结果,Δt为控制周期。
9.一种机械臂的控制装置,其特征在于,所述机械臂包括末端和多个关节,所述装置包括:
获取单元,用于获取所述机械臂的各个关节的当前期望速度和关节角度;
处理单元,用于基于机械臂微分运动学原理,得到第一雅可比矩阵;修正所述第一雅可比矩阵,得到第二雅可比矩阵;基于最小二乘法,根据所述第二雅克比矩阵,计算所述机械臂的关节期望速度;根据最大约束速度阈值,对所述关节期望速度进行修正,得到第一修正结果;根据所述末端的运动方向与当前期望速度方向,对所述第一修正结果进行修正,得到第二修正结果;根据第二修正结果,得到修正后的关节期望速度,以控制所述机械臂的运动。
10.一种电子设备,其特征在于,包括:
至少一个处理器;和
与所述至少一个处理器通信连接的存储器;其中,
所述存储器存储有可被所述至少一个处理器执行的指令,所述指令被所述至少一个处理器执行,以使所述至少一个处理器能够执行如权利要求1-8任一项所述的机械臂的控制方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310464422.3A CN116494231A (zh) | 2023-04-21 | 2023-04-21 | 机械臂的控制方法、装置及电子设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310464422.3A CN116494231A (zh) | 2023-04-21 | 2023-04-21 | 机械臂的控制方法、装置及电子设备 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116494231A true CN116494231A (zh) | 2023-07-28 |
Family
ID=87319724
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310464422.3A Pending CN116494231A (zh) | 2023-04-21 | 2023-04-21 | 机械臂的控制方法、装置及电子设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116494231A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117798938A (zh) * | 2024-03-01 | 2024-04-02 | 北京长木谷医疗科技股份有限公司 | 一种多关节机器人非奇异评价控制方法及装置 |
-
2023
- 2023-04-21 CN CN202310464422.3A patent/CN116494231A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117798938A (zh) * | 2024-03-01 | 2024-04-02 | 北京长木谷医疗科技股份有限公司 | 一种多关节机器人非奇异评价控制方法及装置 |
CN117798938B (zh) * | 2024-03-01 | 2024-05-28 | 北京长木谷医疗科技股份有限公司 | 一种多关节机器人非奇异评价控制方法及装置 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Zhang et al. | Variable joint-velocity limits of redundant robot manipulators handled by quadratic programming | |
CN109669479B (zh) | 一种基于事件触发的移动机器人轨迹跟踪控制方法 | |
Zhang et al. | Control of small formations using shape coordinates | |
US11833692B2 (en) | Method and device for controlling arm of robot | |
CN116494231A (zh) | 机械臂的控制方法、装置及电子设备 | |
CN113119111A (zh) | 机械臂及其轨迹规划方法和装置 | |
US20230130977A1 (en) | Robot control method, robot and computer-readable storage medium | |
CN113031579A (zh) | 双足机器人的行走控制方法、装置及双足机器人 | |
CN114147720B (zh) | 一种多自由度机械臂的逆运动学通用求解方法及装置 | |
CN115213905A (zh) | 冗余度机械臂位姿控制方法、系统及机器人 | |
Karpinska et al. | Performance-oriented design of inverse kinematics algorithms: extended jacobian approximation of the jacobian pseudo-inverse | |
CN106003057A (zh) | 一种冗余自由度机械臂构型奇异快速判决方法 | |
CN113084791B (zh) | 机械臂控制方法、机械臂控制装置及终端设备 | |
CN111055292B (zh) | 人机交互安全保障方法、装置及计算机可读存储介质 | |
CN112008731B (zh) | 一种空中作业机器人的柔顺控制方法、装置、终端、系统及可读存储介质 | |
Song | Modeling and control of three-degree-of-freedom medical assistant robot | |
CN116038686A (zh) | 机器人奇异点规避方法、装置、计算机设备和存储介质 | |
CN114851168A (zh) | 关节受限冗余并联机械臂的运动规划和控制方法、系统及机器人 | |
CN112894794B (zh) | 人体手臂动作模仿方法、装置、终端设备及存储介质 | |
Zhang et al. | Fixed-Time Control of a Robotic Arm Based on Disturbance Observer Compensation | |
Sename et al. | Robust h∞ control of bilateral teleoperation systems under communication time-delay | |
CN112428263A (zh) | 机械臂控制方法、装置及聚类模型训练方法 | |
CN109918840A (zh) | 一种通过旋转迭代求取主应力及主方向的方法 | |
Jain | Recursive algorithms using local constraint embedding for multibody system dynamics | |
US20240181633A1 (en) | Robot control method, robot, and computer-readable storage medium |
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 |