WO2022199059A1 - 机械臂控制方法、装置、作业控制设备及可读存储介质 - Google Patents

机械臂控制方法、装置、作业控制设备及可读存储介质 Download PDF

Info

Publication number
WO2022199059A1
WO2022199059A1 PCT/CN2021/131202 CN2021131202W WO2022199059A1 WO 2022199059 A1 WO2022199059 A1 WO 2022199059A1 CN 2021131202 W CN2021131202 W CN 2021131202W WO 2022199059 A1 WO2022199059 A1 WO 2022199059A1
Authority
WO
WIPO (PCT)
Prior art keywords
joint
joint angle
value
arm
angle set
Prior art date
Application number
PCT/CN2021/131202
Other languages
English (en)
French (fr)
Inventor
张志豪
刘益彰
陈金亮
白杰
熊友军
Original Assignee
深圳市优必选科技股份有限公司
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 深圳市优必选科技股份有限公司 filed Critical 深圳市优必选科技股份有限公司
Publication of WO2022199059A1 publication Critical patent/WO2022199059A1/zh

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/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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • 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
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor

Abstract

一种机械臂控制方法、装置、作业控制设备及可读存储介质，该控制方法为：在获取到仿人机械臂在当前时刻的实际关节角及待实现位姿后，会计算待实现位姿所在的关节平面相对于对应参考平面的原始臂角值和与原始臂角值对应的待校关节角集，而后计算待实现位姿相对于机械臂关节角度的逆向雅可比矩阵，并利用关节避奇异特征限定条件对逆向雅可比矩阵进行参数修正，从而结合修正后的逆向雅可比矩阵、实际关节角集以及待校关节角集，针对待实现位姿计算对应的目标关节角集，以在按照目标关节角集控制仿人机械臂运动时，使该仿人机械臂可在避开机械臂奇异位置的同时安稳地运动达到目标位姿，增强仿人机械臂的可操作性。

Description

机械臂控制方法、装置、作业控制设备及可读存储介质
相关申请的交叉引用
本申请要求于2021年3月23日提交中国专利局的申请号为202110310049.7、名称为“机械臂控制方法、装置、作业控制设备及可读存储介质”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本申请涉及机械臂控制技术领域，具体而言，涉及一种机械臂控制方法、装置、作业控制设备及可读存储介质。
背景技术
随着科学技术的不断发展，机械臂技术因具有极大的研究价值及应用价值受到了各行各业的广泛重视，人们对机械臂操作能力的要求也在不断增加，为此具有人体手臂结构特性的仿人机械臂便被设计出来，用以提升机械臂对复杂环境的适用性。而对仿人机械臂而言，因仿人机械臂存在多个关节的运动范围较窄，仿人机械臂在某些位姿状态下容易出现某些关节角度失速状况，导致该仿人机械臂无法准确且安全地到达某些目标位置。由此，对仿人机械臂来说，如何控制仿人机械臂避开奇异位形运动达到目标位姿，便是一项亟需解决的重要问题。
申请内容
有鉴于此，本申请的目的包括提供一种机械臂控制方法、装置、作业控制设备及可读存储介质，能够使仿人机械臂安稳地避开机械臂奇异位置运动达成目标位姿，增强仿人机械臂的可操作性。
为了实现上述目的，本申请实施例采用的技术方案如下：
第一方面，本申请提供一种机械臂控制方法，所述方法包括：
获取仿人机械臂在当前时刻的实际关节角集及待实现位姿，其中所述实际关节角集包括所述仿人机械臂各关节在当前时刻的实际关节角值；
确定与所述待实现位姿对应的参考平面，并计算所述待实现位姿所在的关节平面相对于所述参考平面的原始臂角值，以及与所述原始臂角值对应的待校关节角集，其中所述待校关节角集包括所述仿人机械臂各关节的与所述待实现位姿对应的待校关节角值；
计算所述待实现位姿相对于机械臂关节角度的逆向雅可比矩阵，并根据关节避奇异特征限定条件对计算出的逆向雅可比矩阵进行参数更新；
根据更新后的逆向雅可比矩阵、所述实际关节角集以及所述待校关节角集，针对所述待实现位姿计算满足所述关节避奇异特征限定条件的目标关节角集，其中所述目标关节角集包括所述仿人机械臂各关节针对所述待实现位姿需要表达出的目标关节角值；
按照所述目标关节角集对所述仿人机械臂各关节角度进行调整。
在可选的实施方式中，所述根据关节避奇异特征限定条件对计算出的逆向雅可比矩阵进行参数更新的步骤，包括：
对计算出的逆向雅可比矩阵进行奇异值分解，得到该逆向雅可比矩阵所对应的奇异值矩阵、特征值矩阵、左奇异向量矩阵及右奇异向量矩阵；
将所述特征值矩阵中的最小特征值与所述关节避奇异特征限定条件所包括的逆雅可比特征阈值进行比对，得到对应的特征比对结果；
计算所述最小特征值相对于所述逆雅可比特征阈值的特征比例，并根据所述特征比对结果、所述特征比例以及预设奇异调整值计算对应的奇异加载值；
将计算出的所述奇异加载值对计算出的奇异值矩阵进行参数变更，得到变更后的奇异值矩阵；
对所述左奇异向量矩阵、右奇异向量矩阵以及所述变更后的奇异值矩阵进行反向式奇异值分解处理，得到更新后的逆向雅可比矩阵。
在可选的实施方式中，所述根据更新后的逆向雅可比矩阵、所述实际关节角集以及所述待校关节角集，针对所述待实现位姿计算满足所述关节避奇异特征限定条件的目标关节角集的步骤，包括：
计算所述实际关节角集与所述待校关节角集在相同关节处对应的关节角差值，得到对应的差异关节角集，其中所述差异关节角集包括所述仿人机械臂各关节的待校关节角值与实际关节角值之间的角度差值；
对所述差异关节角集与所述更新后的逆向雅可比矩阵进行矩阵乘法运算，得到对应的偏置关节角集，其中所述偏置关节角集包括所述仿人机械臂各关节需要加载的偏置关节角值；
将所述偏置关节角集与所述待校关节角集在相同关节处进行角度叠加，得到对应的待实施关节角集，以将所述待实施关节角集作为所述目标关节角集。
在可选的实施方式中，所述根据更新后的逆向雅可比矩阵、所述实际关节角集以及所述待校关节角集，针对所述待实现位姿计算满足所述关节避奇异特征限定条件的目标关节角集的步骤，还包括：
计算与所述待实施关节角集对应的期望臂角值；
根据所述期望臂角值以及所述仿人机械臂在历史时间段内连续的多个历史臂角值，构建对应的臂角变化曲线；
对所述臂角变化曲线进行平滑处理，并从平滑处理后的臂角变化曲线中提取与当前时刻对应的待校验臂角值；
计算与所述待校验臂角值对应的待处理关节角集，并将该待处理关节角集包括的每个关节的待处理关节角值限定到与该关节对应的预设关节角限位范围内，得到对应的待兑现关节角集，以将所述待兑现关节角集作为所述目标关节角集。
第二方面，本申请提供一种机械臂控制装置，所述装置包括：
运动信息获取模块，用于获取仿人机械臂在当前时刻的实际关节角集及待实现位姿，其中所述实际关节角集包括所述仿人机械臂各关节在当前时刻的实际关节角值；
臂角信息计算模块，用于确定与所述待实现位姿对应的参考平面，并计算所述待实现位姿所在的关节平面相对于所述参考平面的原始臂角值，以及与所述原始臂角值对应的待校关节角集，其中所述待校关节角集包括所述仿人机械臂各关节的与所述待实现位姿对应的待校关节角值；
关节奇异限定模块，用于计算所述待实现位姿相对于机械臂关节角度的逆向雅可比矩阵，并根据关节避奇异特征限定条件对计算出的逆向雅可比矩阵进行参数更新；
关节角度矫正模块，用于根据更新后的逆向雅可比矩阵、所述实际关节角集以及所述待校关节角集，针对所述待实现位姿计算满足所述关节避奇异特征限定条件的目标关节角集，其中所述目标关节角集包括所述仿人机械臂各关节针对所述待实现位姿需要表达出的目标关节角值；
关节运动控制模块，用于按照所述目标关节角集对所述仿人机械臂各关节角度进行 调整。
在可选的实施方式中，所述关节奇异限定模块包括：
矩阵奇异分解子模块，用于对计算出的逆向雅可比矩阵进行奇异值分解，得到该逆向雅可比矩阵所对应的奇异值矩阵、特征值矩阵、左奇异向量矩阵及右奇异向量矩阵；
矩阵特征比对子模块，用于将所述特征值矩阵中的最小特征值与所述关节避奇异特征限定条件所包括的逆雅可比特征阈值进行比对，得到对应的特征比对结果；
奇异加载计算子模块，用于计算所述最小特征值相对于所述逆雅可比特征阈值的特征比例，并根据所述特征比对结果、所述特征比例以及预设奇异调整值计算对应的奇异加载值；
奇异矩阵变更子模块，用于将计算出的所述奇异加载值对计算出的奇异值矩阵进行参数变更，得到变更后的奇异值矩阵；
矩阵反向分解子模块，用于对所述左奇异向量矩阵、右奇异向量矩阵以及所述变更后的奇异值矩阵进行反向式奇异值分解处理，得到更新后的逆向雅可比矩阵。
在可选的实施方式中，所述关节角度矫正模块包括：
差异角度计算子模块，用于计算所述实际关节角集与所述待校关节角集在相同关节处对应的关节角差值，得到对应的差异关节角集，其中所述差异关节角集包括所述仿人机械臂各关节的待校关节角值与实际关节角值之间的角度差值；
偏置角度计算子模块，用于对所述差异关节角集与所述更新后的逆向雅可比矩阵进行矩阵乘法运算，得到对应的偏置关节角集，其中所述偏置关节角集包括所述仿人机械臂各关节需要加载的偏置关节角值；
关节角度叠加子模块，用于将所述偏置关节角集与所述待校关节角集在相同关节处进行角度叠加，得到对应的待实施关节角集，以将所述待实施关节角集作为所述目标关节角集。
在可选的实施方式中，所述关节角度矫正模块还包括：
期望臂角计算子模块，用于计算与所述待实施关节角集对应的期望臂角值；
臂角曲线构建子模块，用于根据所述期望臂角值以及所述仿人机械臂在历史时间段 内连续的多个历史臂角值，构建对应的臂角变化曲线；
校验臂角提取子模块，用于对所述臂角变化曲线进行平滑处理，并从平滑处理后的臂角变化曲线中提取与当前时刻对应的待校验臂角值；
关节角度限定子模块，用于计算与所述待校验臂角值对应的待处理关节角集，并将该待处理关节角集包括的每个关节的待处理关节角值限定到与该关节对应的预设关节角限位范围内，得到对应的待兑现关节角集，以将所述待兑现关节角集作为所述目标关节角集。
第三方面，本申请提供一种作业控制设备，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机程序，所述处理器可执行所述计算机程序，以实现前述实施方式中任意一项所述的机械臂控制方法。
第四方面，本申请提供一种可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时，实现前述实施方式中任意一项所述的机械臂控制方法。
本申请实施例的有益效果包括以下内容：
本申请在获取到仿人机械臂在当前时刻的实际关节角及待实现位姿后，会相应地计算待实现位姿所在的关节平面相对于对应参考平面的原始臂角值，以及与原始臂角值对应的待校关节角集，而后计算待实现位姿相对于机械臂关节角度的逆向雅可比矩阵，并利用关节避奇异特征限定条件对计算出的逆向雅可比矩阵进行参数修正，从而结合修正后的逆向雅可比矩阵、实际关节角集以及待校关节角集，针对待实现位姿计算满足关节避奇异特征限定条件的目标关节角集，最后按照目标关节角集对仿人机械臂各关节角度进行调整，以确保该仿人机械臂能够在避开机械臂奇异位置的同时安稳地运动达到目标位姿，增强仿人机械臂的可操作性。
为使本申请的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以 根据这些附图获得其他相关的附图。
图1为本申请实施例提供的作业控制设备的组成示意图；
图2为本申请实施例提供的仿人机械臂的结构示意图；
图3为本申请实施例提供的机械臂控制方法的流程示意图；
图4为本申请实施例提供的参考平面与关节平面的夹角成型示意图；
图5为图3中的步骤S230包括的子步骤的流程示意图；
图6为图3中的步骤S240包括的子步骤的流程示意图之一；
图7为图3中的步骤S240包括的子步骤的流程示意图之二；
图8为本申请实施例提供的机械臂控制装置的组成示意图；
图9为图8中的关节奇异限定模块的组成示意图；
图10为图8中的关节角度矫正模块的组成示意图之一；
图11为图8中的关节角度矫正模块的组成示意图之二。
图标：10-作业控制设备；11-存储器；12-处理器；13-通信单元；100-机械臂控制装置；110-运动信息获取模块；120-臂角信息计算模块；130-关节奇异限定模块；140-关节角度矫正模块；150-关节运动控制模块；131-矩阵奇异分解子模块；132-矩阵特征比对子模块；133-奇异加载计算子模块；134-奇异矩阵变更子模块；135-矩阵反向分解子模块；141-差异角度计算子模块；142-偏置角度计算子模块；143-关节角度叠加子模块；144-期望臂角计算子模块；145-臂角曲线构建子模块；146-校验臂角提取子模块；147-关节角度限定子模块。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。
因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请中的实施例，本领域普 通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
在本申请的描述中，需要理解的是，术语“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本申请中的具体含义。
下面结合附图，对本申请的一些实施方式作详细说明。在不冲突的情况下，下述的实施例及实施例中的特征可以相互结合。
请参照图1，图1是本申请实施例提供的作业控制设备10的组成示意图。在本申请实施例中，所述作业控制设备10用于对仿人机械臂的作业任务进行调整，并控制所述仿人机械臂对应执行调整后的作业任务。其中，所述作业控制设备10可以与所述仿人机械臂远程通信连接，所述作业控制设备10也可以与所述仿人机械臂集成在一起，以在控制所述仿人机械臂执行作业任务时，使该仿人机械臂能够避开机械臂奇异位置进行运动，并安稳地运动达到目标位姿，从而增强仿人机械臂的可操作性及运动稳定性。
可选地，请参照图2，图2是本申请实施例提供的仿人机械臂的结构示意图。在本实施例中，受所述作业控制设备10操控的仿人机械臂为七自由度机械臂，每个自由度对应一个机械臂关节，由此该仿人机械臂可由机械臂基座、基座关节(如图2中的q 1所对应的关节)、肩部关节(如图2中的q 2所对应的关节)、大臂关节(如图2中的q 3所对应的关节)、肘部关节(如图2中的q 4所对应的关节)、小臂关节(如图2中的q 5所对应的关节)、腕部关节(如图2中的q 6所对应的关节)、臂末端关节(如图2中的q 7所对应的关节)以及多根连接臂杆等器件组成，从而可通过上述七个关节各自的关节角度数值 进行调控，实现所述仿人机械臂的运动轨迹调整，使所述作业控制设备10能够利用该仿人机械臂所具有的冗余自由度提升机械臂运动控制过程中的机械臂运动安全性及运动稳定性。其中，图2中的字符S即可代表所述仿人机械臂的肩部，图2中的字符E即可代表所述仿人机械臂的肘部，图2中的字符W即可代表所述仿人机械臂的腕部。
同时，在本实施例中，所述作业控制设备10可以包括存储器11、处理器12、通信单元13及机械臂控制装置100。其中，所述存储器11、所述处理器12及所述通信单元13各个元件相互之间直接或间接地电性连接，以实现数据的传输或交互。例如，所述存储器11、所述处理器12及所述通信单元13这些元件相互之间可通过一条或多条通讯总线或信号线实现电性连接。
在本实施例中，所述存储器11可以是，但不限于，随机存取存储器(Random Access Memory，RAM)，只读存储器(Read Only Memory，ROM)，可编程只读存储器(Programmable Read-Only Memory，PROM)，可擦除只读存储器(Erasable Programmable Read-Only Memory，EPROM)，电可擦除只读存储器(Electric Erasable Programmable Read-Only Memory，EEPROM)等。其中，所述存储器11用于存储计算机程序，所述处理器12在接收到执行指令后，可相应地执行所述计算机程序。
其中，所述存储器11还用于存储针对所述仿人机械臂的作业控制策略，其中该作业控制策略包括所述仿人机械臂在不同时刻需要运动达到的机械臂位姿。所述存储器11还用于存储对仿人机械臂奇异位置进行具体划分的关节避奇异特征限定条件，使所述仿人机械臂在满足关节避奇异特征限定条件的情况下运动时，能够自然地避开机械臂奇异位置。
在本实施例中，所述处理器12可以是一种具有信号的处理能力的集成电路芯片。所述处理器12可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、图形处理器(Graphics Processing Unit，GPU)及网络处理器(Network Processor，NP)、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件中的至少一种。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。
在本实施例中，所述通信单元13用于通过网络建立所述作业控制设备10与其他电子设备之间的通信连接，并通过所述网络收发数据，其中所述网络包括有线通信网络及 无线通信网络。例如，所述作业控制设备10可以通过所述通信单元13从作业规划设备处获取针对所述仿人机械臂的待执行作业任务，并通过所述通信单元13向所述仿人机械臂发送作业控制指令，使所述仿人机械臂按照所述作业控制指令进行运动以对应完成所述待执行作业任务。
在本实施例中，所述机械臂控制装置100包括至少一个能够以软件或固件的形式存储于所述存储器11中或者在所述作业控制设备10的操作系统中的软件功能模块。所述处理器12可用于执行所述存储器11存储的可执行模块，例如所述机械臂控制装置100所包括的软件功能模块及计算机程序等。所述作业控制设备10可通过所述机械臂控制装置100控制仿人机械臂避开机械臂奇异位置进行运动，并保证仿人机械臂能够安稳地运动达到目标位姿，从而增强仿人机械臂的可操作性及运动稳定性。
可以理解的是，图1所示的框图仅为所述作业控制设备10的一种组成示意图，所述作业控制设备10还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。图1中所示的各组件可以采用硬件、软件或其组合实现。
在本申请中，为确保所述作业控制设备10能够控制仿人机械臂避开机械臂奇异位置进行运动，并使仿人机械臂安稳地运动达到目标位姿，本申请实施例提供一种针对仿人机械臂的机械臂控制方法实现前述目的。下面对本申请提供的机械臂控制方法进行详细描述。
请参照图3，图3是本申请实施例提供的机械臂控制方法的流程示意图。在本申请实施例中，所述机械臂控制方法可以包括步骤S210～步骤S250。
步骤S210，获取仿人机械臂在当前时刻的实际关节角集及待实现位姿。
在本实施例中，所述待实现位姿用于表示所述仿人机械臂在当前时刻需要运动到达的位姿状态，所述实际关节角包括所述仿人机械臂各关节在当前时刻的实际关节角值。
步骤S220，确定与待实现位姿对应的参考平面，并计算待实现位姿所在的关节平面相对于参考平面的原始臂角值，以及与原始臂角值对应的待校关节角集。
在本实施例中，请结合参照图4，针对一个需要实现的机械臂位姿来说，当仿人机械臂参照该机械臂位姿进行姿态表达时，该仿人机械臂的腕部关节、肘部关节及肩部关节构成的平面即为该机械臂位姿所对应的关节平面(如图4中的关节平面SEW)，此时若将该机械臂位姿所对应的大臂关节角度值置为零(q 3＝0)而其他关节角度值维持不变， 使所述仿人机械臂对应调整肘部位置，最终形成由原腕部关节位置、新肘部关节位置及原肩部关节位置构成的参考平面(如图4中的参考平面SE 0W)，此时参考平面SE 0W与关节平面SEW必定存在一个夹角，该夹角即为臂角Ψ。
而针对关节平面SEW来说，肘部E在对应机械臂位姿下的坐标位置即可表达为
Figure PCTCN2021131202-appb-000001
腕部W在对应机械臂位姿下的坐标位置即可表达为
Figure PCTCN2021131202-appb-000002
其中P E用于表示肘部坐标系E在基座坐标系下的坐标表示， 0R i用于表示第i个机械臂关节坐标系在基座坐标系下的姿态表示(即第i个机械臂关节坐标系相对于机械臂基座的旋转矩阵，i＝1,2,3,…,7)， nR i用于表示第i个机械臂关节坐标系在第n个机械臂关节坐标系下的姿态表示(即第i个机械臂关节坐标系相对于第n个机械臂关节的旋转矩阵，n<i且i＝2,3,…,7)，
Figure PCTCN2021131202-appb-000003
用于表示第3个机械臂关节坐标系下肩部S到肘部E的向量
Figure PCTCN2021131202-appb-000004
P W用于表示腕部坐标系W在基座坐标系下的坐标表示，
Figure PCTCN2021131202-appb-000005
用于表示第4个机械臂关节坐标系下肘部E到腕部W的向量
Figure PCTCN2021131202-appb-000006
l se用于表示肩部S到肘部E的距离，l ew用于表示肘部E到腕部W的距离。此时在基座坐标系下的肘部E到腕部W的向量即可表达为
Figure PCTCN2021131202-appb-000007
而对参考平面SE 0W来说，在基座坐标系下第1个机械臂关节角度值与第2个机械臂关节角度值可分别采用
Figure PCTCN2021131202-appb-000008
和
Figure PCTCN2021131202-appb-000009
进行表达，此时
Figure PCTCN2021131202-appb-000010
和
Figure PCTCN2021131202-appb-000011
可参照如下式子计算得到：
Figure PCTCN2021131202-appb-000012
其中，在
Figure PCTCN2021131202-appb-000013
的求解过程中，若
Figure PCTCN2021131202-appb-000014
则θ＝0；若
Figure PCTCN2021131202-appb-000015
则
Figure PCTCN2021131202-appb-000016
同时，在参考平面SE 0W中，肘部E 0在基座坐标系下的坐标位置即可表达为
Figure PCTCN2021131202-appb-000017
此时，对应参考平面SE 0W与关节平面SEW之间的臂角ψ即可采用如下式子进行表达：
Figure PCTCN2021131202-appb-000018
其中
Figure PCTCN2021131202-appb-000019
Figure PCTCN2021131202-appb-000020
此时，即可根据罗德里格斯旋转公式确定出上述关节平面SEW与上述参考平面SE 0W之间的旋转矩阵 0R Ψ＝I 3+sinΨR sw+(1-cosΨ)[R sw] 2，其中R sw用于表示肩部S到肘部E的单位向量r sw的斜对称矩阵。
在此情况下，处于关节平面SEW内的第3个机械臂坐标系在基座坐标系下的旋转矩阵 0R 3＝ 0R Ψ 0R 3 0＝AssinΨ+BscosΨ+Cs，其中
Figure PCTCN2021131202-appb-000021
Figure PCTCN2021131202-appb-000022
用于表示q 3＝0时第3个机械臂坐标系相对于基座坐标系的旋转矩阵。
由此，当前与上述机械臂位姿相关的第1、2及3个机械臂关节角度可采用如下公式计算得到：
q 1＝atan2(-As(1,1)sinΨ-Bs(1,1)cosΨ-Cs(1,1),-As(0,1)sinΨ-Bs(0,1)cosΨ-Cs(0,1))；
q 2＝asin(-As(2,1)sinΨ-Bs(2,1)cosΨ-Cs(2,1))；
q 3＝atan2(-As(2,2)sinΨ-Bs(2,2)cosΨ-Cs(2,2),As(2,0)sinΨ+Bs(2,0)cosΨ+Cs(2,0))。
同时，在关节平面SEW内存在坐标系之间的变换关系为 0R 7＝ 0R 3 3R 4 5R 7，即可根据该变换关系得到 5R 7＝ 3R 4 T0R 3 T0R 7，此时当前与上述机械臂位姿相关的第5、6及7个机械臂关节角度可采用如下公式计算得到：
q 5＝atan2(Aw(1,2)sinΨ+Bw(1,2)cosΨ+Cw(1,2),Aw(0,2)sinΨ+Bw(0,2)cosΨ+Cw(0,2))；
q 6＝ascos(Aw(2,2)sinΨ+Bw(2,2)cosΨ+Cw(2,2))-π/2；
q 7＝atan2(Aw(2,0)sinΨ+Bw(2,0)cosΨ+Cw(2,0),Aw(2,1)sinΨ+Bw(2,1)cosΨ+Cw(2,1))+π/2。
其中，
Figure PCTCN2021131202-appb-000023
用于表示q 3＝0时第7个机械臂坐标系相对于第5个机械臂坐标系的旋转矩阵。
而与上述机械臂位姿相关的第4个机械臂关节角度的具体数值即为最初针对该机械臂位姿设定的数值，其数值并不会因臂角数值的不同而有所变化。
在此基础上，当所述作业控制设备10获取到当前时刻对应的待实现位姿后，可根据上述计算一个机械臂位姿所对应的臂角值的计算公式，确定出与所述待实现位姿对应的参考平面，并相应地计算所述待实现位姿所在的关节平面相对于参考平面的原始臂角 值，而后根据上述计算机械臂位姿所对应的各机械臂关节角度值的计算公式，计算出针对当前待实现位姿的与所述原始臂角值对应的需要避奇异验证操作的待校关节角集，此时所述待校关节角集包括所述仿人机械臂各关节的与所述待实现位姿对应的待校关节角值。
步骤S230，计算所述待实现位姿相对于机械臂关节角度的逆向雅可比矩阵，并根据关节避奇异特征限定条件对计算出的逆向雅可比矩阵进行参数更新。
在本实施例中，当所述作业控制设备10确定出仿人机械臂在当前时刻的与待实现位姿对应的待校关节角集后，可利用机械臂运动学原理建立所述待校关节角集与所述待实现位姿之间的关联关系，确定出所述待实现位姿与所述待校关节角集之间的雅可比矩阵，进而对确定出的该雅可比矩阵进行奇异值(Singular Value Decomposition，SVD)分解，求解出由位姿推导关节角的所述待实现位姿相对于机械臂关节角度的逆向雅可比矩阵。接着，所述作业控制设备10可直接利用存储的关节避奇异特征限定条件对计算出的逆向雅可比矩阵进行参数更新，完成对位姿实现过程中的臂角变化量的约束操作，确保所述仿人机械臂按照更新后的逆向雅可比矩阵执行连续运动操作后不会经过机械臂奇异位置，从而使该仿人机械臂能够避开机械臂奇异位置进行运动，并安稳地运动达到目标位姿。
可选地，请参照图5，图5是图3中的步骤S230包括的子步骤的流程示意图。在本实施例中，所述步骤S230中根据关节避奇异特征限定条件对计算出的逆向雅可比矩阵进行参数更新的步骤可以包括子步骤S231～子步骤S235，用以将仿人机械臂连续运动过程中的关节速度变化量限定在一定范围内，防止仿人机械臂在连续运动过程中经过机械臂奇异位置，增强仿人机械臂的运动稳定性。
子步骤S231，对计算出的逆向雅可比矩阵进行奇异值分解，得到该逆向雅可比矩阵所对应的奇异值矩阵、特征值矩阵、左奇异向量矩阵及右奇异向量矩阵。
在本实施例中，所述逆向雅可比矩阵在奇异值分解后可表达为：
Figure PCTCN2021131202-appb-000024
其中，
Figure PCTCN2021131202-appb-000025
用于表示所述逆向雅可比矩阵，σ i用于表示奇异值矩阵中的第i个矩阵元素内容(奇异值)，u i用于表示左奇异向量矩阵中的第i个矩阵元素内容(左奇异向量)， ν i用于表示右奇异向量矩阵中的第i个矩阵元素内容(右奇异向量)。而后，可根据式子
Figure PCTCN2021131202-appb-000026
计算出特征值矩阵中的第i个矩阵元素内容(特征值)。
子步骤S232，将特征值矩阵中的最小特征值与关节避奇异特征限定条件所包括的逆雅可比特征阈值进行比对，得到对应的特征比对结果。
在本实施例中，所述特征比对结果可以包括对应最小特征值大于或等于所述逆雅可比特征阈值，也可以包括对应最小特征值小于所述逆雅可比特征阈值。
子步骤S233，计算最小特征值相对于逆雅可比特征阈值的特征比例，并根据特征比对结果、特征比例以及预设奇异调整值计算对应的奇异加载值。
在本实施例中，所述预设奇异调整值用于表示对所述逆向雅可比矩阵对应的奇异值矩阵的最大奇异值调整量，所述奇异加载值用于对所述逆向雅可比矩阵对应的奇异值矩阵进行奇异值更新操作，以将对应逆向雅可比矩阵调整为具有避奇异特性的逆向雅可比矩阵。其中，若所述特征比对结果为对应最小特征值大于或等于所述逆雅可比特征阈值，则对应的所述奇异加载值为零；若所述特征比对结果为对应最小特征值小于所述逆雅可比特征阈值，则对应的所述奇异加载值等于一减去所述特征比例后得到的数值与所述预设奇异调整值之间的积值的平方值。此时所述奇异加载值的计算公式如下所示：
Figure PCTCN2021131202-appb-000027
其中，σ′用于表示所述奇异加载值，λ min用于表示所述特征值矩阵中的最小特征值，ε用于表示所述逆雅可比特征阈值，
Figure PCTCN2021131202-appb-000028
用于表示所述最小特征值相对于所述逆雅可比特征阈值的特征比例，
Figure PCTCN2021131202-appb-000029
用于表示所述预设奇异调整值。
子步骤S234，将计算出的奇异加载值对计算出的奇异值矩阵进行参数变更，得到变更后的奇异值矩阵。
在本实施例中，当计算得到所述奇异加载值后，可采用式子
Figure PCTCN2021131202-appb-000030
构建得到所述变更后的奇异值矩阵，其中σ′ i用于表示所述变更后的奇异值矩阵中第i个矩阵元素内容(奇异值)，σ i用于表示所述计算出奇异值矩阵(直接由最初的逆向雅可比矩阵分解得到)中的第i个矩阵元素内容(奇异值)，σ′用于表示所述奇异加载值。
子步骤S235，对左奇异向量矩阵、右奇异向量矩阵以及变更后的奇异值矩阵进行反向式奇异值分解处理，得到更新后的逆向雅可比矩阵。
在本实施例中，所述更新后的逆向雅可比矩阵可采用如下式子进行表达：
Figure PCTCN2021131202-appb-000031
其中，
Figure PCTCN2021131202-appb-000032
用于表示所述更新后的逆向雅可比矩阵，σ′ i用于表示所述变更后的奇异值矩阵中的第i个矩阵元素内容(奇异值)，u i用于表示与未更新前的逆向雅可比矩阵
Figure PCTCN2021131202-appb-000033
相关的左奇异向量矩阵中的第i个矩阵元素内容(左奇异向量)，ν i用于表示与未更新前的逆向雅可比矩阵
Figure PCTCN2021131202-appb-000034
相关的右奇异向量矩阵中的第i个矩阵元素内容(右奇异向量)。
由此，本申请可通过执行上述子步骤S231～子步骤S235，将仿人机械臂连续运动过程中的关节速度变化量限定在一定范围内，防止仿人机械臂在连续运动过程中经过机械臂奇异位置，增强仿人机械臂的运动稳定性。
步骤S240，根据更新后的逆向雅可比矩阵、实际关节角集以及待校关节角集，针对待实现位姿计算满足关节避奇异特征限定条件的目标关节角集。
在本实施例中，所述目标关节角集包括所述仿人机械臂各关节针对所述待实现位姿需要表达出的目标关节角值。所述作业控制设备10可控制所述仿人机械臂按照所述目标关节角集进行运动，使所述仿人机械臂能够在避开机械臂奇异位置的同时安稳地运动达到目标位姿，增强了仿人机械臂的可操作性。
可选地，请参照图6，图6是图3中的步骤S240包括的子步骤的流程示意图之一。在本实施例的一种实施方式中，图6所示的步骤S240可以包括子步骤S241～子步骤S243，用以简化所述目标关节角集的构建流程。
子步骤S241，计算实际关节角集与待校关节角集在相同关节处对应的关节角差值，得到对应的差异关节角集。
其中，所述差异关节角集包括所述仿人机械臂各关节的待校关节角值与实际关节角值之间的角度差值。
子步骤S242，对差异关节角集与更新后的逆向雅可比矩阵进行矩阵乘法运算，得到对应的偏置关节角集。
其中，所述偏置关节角集包括所述仿人机械臂各关节需要加载的偏置关节角值。
子步骤S243，将偏置关节角集与待校关节角集在相同关节处进行角度叠加，得到对应的待实施关节角集，以将待实施关节角集作为目标关节角集。
其中，上述角度叠加操作即为将相同关节处的偏置关节角值与待校关节角值之间的和值，此时所述待实施关节角集可采用式子
Figure PCTCN2021131202-appb-000035
计算得到，其中q D用于表示所述待实施关节角集，q用于表示所述待校关节角集，Δq用于表示所述差异关节角集。此时，所述作业控制设备10可直接采用计算出的待实施关节角集作为目标关节角集来控制所述仿人机械臂进行运动，使仿人机械臂能够在避开机械臂奇异位置的同时安稳地运动到达待实现位姿的状态。
由此，本申请可通过执行上述子步骤S241～子步骤S243，直接简便地计算出具有关节避奇异特性的目标关节角集，从而简化所述目标关节角集的构建流程。
可选地，请参照图7，图7是图3中的步骤S240包括的子步骤的流程示意图之二。在本实施例的另一种实施方式中，图7所示的步骤S240所包括的子步骤用于对经图6计算出的待实施关节角集进行处理，而后将处理得到的关节角集作为目标关节角集进行使用，以提升仿人机械臂运动时的防抖动性能，防止仿人机械臂在连续运动过程中出现抖动和冲击，让所述仿人机械臂能够平滑且稳定地避开奇异位置。此时，图7所示的步骤S240可以包括子步骤S244～子步骤S248。
子步骤S244，计算与待实施关节角集对应的期望臂角值。
在本实施例中，所述作业控制设备10可根据上述计算一个机械臂位姿所对应的臂角值的计算公式，以及上述计算机械臂位姿所对应的各机械臂关节角度值的计算公式，针对所述待实施关节角集反向式地计算出与该待实施关节角集对应的期望臂角值。
子步骤S245，根据期望臂角值以及仿人机械臂在历史时间段内连续的多个历史臂角值，构建对应的臂角变化曲线。
在本实施例中，所述历史时间段为以当前时刻为时间边界的一端时间，所述历史臂角值用于表示所述仿人机械臂在历史时间段内连续运动时所表现出的不同时刻的真实臂角值。由此，所述作业控制设备10可采用所述期望臂角值以及多个历史臂角值，构建出与所述仿人机械臂的连续运动过程相匹配的臂角变化曲线。
子步骤S246，对臂角变化曲线进行平滑处理，并从平滑处理后的臂角变化曲线中 提取与当前时刻对应的待校验臂角值。
在本实施例中，可采用卡尔曼滤波器、低通滤波器等平滑滤波器对所述臂角变化曲线进行平滑处理，使臂角变化曲线更加平稳和光滑，进而使得各机械臂关节的角度变化曲线也具有一定平滑性。而后，所述作业控制设备10将对应从平滑处理后的臂角变化曲线中提取与当前时刻对应的待校验臂角值。
子步骤S247，计算与待校验臂角值对应的待处理关节角集，并将该待处理关节角集包括的每个关节的待处理关节角值限定到与该关节对应的预设关节角限位范围内，得到对应的待兑现关节角集，以将待兑现关节角集作为目标关节角集。
在本实施例中，所述作业控制设备10可再次上述计算机械臂位姿所对应的各机械臂关节角度值的计算公式，计算出与待校验臂角值对应的待处理关节角集。而后，为确保经平滑处理变换得到的每个机械臂关节的角度值不会导致仿人机械臂运动到奇异位置，可通过预先针对每个机械臂关节配置一个符合运动学的预设关节角限位范围，并针对计算出的所述待处理关节角集中的每个待处理关节角值与对应预设关节角限位范围进行比较。若某个待处理关节角值小于对应预设关节角限位范围的角度下限值，则将所述待处理关节角集中的该待处理关节角值置为所述角度下限值；若某个待处理关节角值大于对应预设关节角限位范围的角度上限值，则将所述待处理关节角集中的该待处理关节角值置为所述角度上限值，从而将该待处理关节角值限定到对应预设关节角限位范围内，生成符合仿人机械臂的极限运动范围的待兑现关节角集，并直接将该待兑现关节角集作为所述目标关节角集来控制所述仿人机械臂进行运动，使仿人机械臂能够在避开机械臂奇异位置的同时安稳地运动到达待实现位姿的状态，并防止仿人机械臂在连续运动过程中出现抖动和冲击。
由此，本申请可在执行子步骤S241～子步骤S243的基础上继续执行上述子步骤S244～子步骤S247，以通过最终确定出的目标关节角集提升仿人机械臂运动时的防抖动性能，防止仿人机械臂在连续运动过程中出现抖动和冲击，让所述仿人机械臂能够平滑且稳定地避开奇异位置。
步骤S250，按照目标关节角集对仿人机械臂各关节角度进行调整。
在本实施例中，当所述作业控制设备10计算出针对所述待实现位姿满足所述关节避奇异特征限定条件的目标关节角集后，可按照所述目标关节角集对所述仿人机械臂中各关节的真实角度进行调整，使所述仿人机械臂能够在避开机械臂奇异位置的同时安稳 地运动到达待实现位姿的状态，增强了仿人机械臂的可操作性及运动稳定性。
由此，本申请可通过执行上述步骤S210～步骤S250，控制仿人机械臂避开机械臂奇异位置进行运动，并保证仿人机械臂能够安稳地运动达到目标位姿，增强仿人机械臂的可操作性及运动稳定性。
在本申请中，为确保所述作业控制设备10能够通过所述机械臂控制装置100执行上述机械臂控制方法，本申请通过对所述机械臂控制装置100进行功能模块划分的方式实现前述功能。下面对本申请提供的机械臂控制装置100的具体组成进行相应描述。
请参照图8，图8是本申请实施例提供的机械臂控制装置100的组成示意图。在本申请实施例中，所述机械臂控制装置100可以包括运动信息获取模块110、臂角信息计算模块120、关节奇异限定模块130、关节角度矫正模块140及关节运动控制模块150。
运动信息获取模块110，用于获取仿人机械臂在当前时刻的实际关节角集及待实现位姿，其中所述实际关节角集包括所述仿人机械臂各关节在当前时刻的实际关节角值。
臂角信息计算模块120，用于确定与所述待实现位姿对应的参考平面，并计算所述待实现位姿所在的关节平面相对于所述参考平面的原始臂角值，以及与所述原始臂角值对应的待校关节角集，其中所述待校关节角集包括所述仿人机械臂各关节的与所述待实现位姿对应的待校关节角值。
关节奇异限定模块130，用于计算所述待实现位姿相对于机械臂关节角度的逆向雅可比矩阵，并根据关节避奇异特征限定条件对计算出的逆向雅可比矩阵进行参数更新。
关节角度矫正模块140，用于根据更新后的逆向雅可比矩阵、所述实际关节角集以及所述待校关节角集，针对所述待实现位姿计算满足所述关节避奇异特征限定条件的目标关节角集，其中所述目标关节角集包括所述仿人机械臂各关节针对所述待实现位姿需要表达出的目标关节角值。
关节运动控制模块150，用于按照所述目标关节角集对所述仿人机械臂各关节角度进行调整。
可选地，请参照图9，图9是图8中的关节奇异限定模块130的组成示意图。在本实施例中，所述关节奇异限定模块130可以包括矩阵奇异分解子模块131、矩阵特征比对子模块132、奇异加载计算子模块133、奇异矩阵变更子模块134及矩阵反向分解子模块135。
矩阵奇异分解子模块131，用于对计算出的逆向雅可比矩阵进行奇异值分解，得到该逆向雅可比矩阵所对应的奇异值矩阵、特征值矩阵、左奇异向量矩阵及右奇异向量矩阵。
矩阵特征比对子模块132，用于将所述特征值矩阵中的最小特征值与所述关节避奇异特征限定条件所包括的逆雅可比特征阈值进行比对，得到对应的特征比对结果。
奇异加载计算子模块133，用于计算所述最小特征值相对于所述逆雅可比特征阈值的特征比例，并根据所述特征比对结果、所述特征比例以及预设奇异调整值计算对应的奇异加载值。
奇异矩阵变更子模块134，用于将计算出的所述奇异加载值对计算出的奇异值矩阵进行参数变更，得到变更后的奇异值矩阵。
矩阵反向分解子模块135，用于对所述左奇异向量矩阵、右奇异向量矩阵以及所述变更后的奇异值矩阵进行反向式奇异值分解处理，得到更新后的逆向雅可比矩阵。
可选地，请参照图10，图10是图8中的关节角度矫正模块140的组成示意图之一。在本实施例的一种实施方式中，所述关节角度矫正模块140可以包括差异角度计算子模块141、偏置角度计算子模块142及关节角度叠加子模块143。
差异角度计算子模块141，用于计算所述实际关节角集与所述待校关节角集在相同关节处对应的关节角差值，得到对应的差异关节角集，其中所述差异关节角集包括所述仿人机械臂各关节的待校关节角值与实际关节角值之间的角度差值。
偏置角度计算子模块142，用于对所述差异关节角集与所述更新后的逆向雅可比矩阵进行矩阵乘法运算，得到对应的偏置关节角集，其中所述偏置关节角集包括所述仿人机械臂各关节需要加载的偏置关节角值。
关节角度叠加子模块143，用于将所述偏置关节角集与所述待校关节角集在相同关节处进行角度叠加，得到对应的待实施关节角集，以将所述待实施关节角集作为所述目标关节角集。
可选地，请参照图11，图11是图8中的关节角度矫正模块140的组成示意图之二。在本实施例的另一种实施方式中，所述关节角度矫正模块140还可以包括期望臂角计算子模块144、臂角曲线构建子模块145、校验臂角提取子模块146及关节角度限定子模块147。
期望臂角计算子模块144，用于计算与所述待实施关节角集对应的期望臂角值。
臂角曲线构建子模块145，用于根据所述期望臂角值以及所述仿人机械臂在历史时间段内连续的多个历史臂角值，构建对应的臂角变化曲线。
校验臂角提取子模块146，用于对所述臂角变化曲线进行平滑处理，并从平滑处理后的臂角变化曲线中提取与当前时刻对应的待校验臂角值。
关节角度限定子模块147，用于计算与所述待校验臂角值对应的待处理关节角集，并将该待处理关节角集包括的每个关节的待处理关节角值限定到与该关节对应的预设关节角限位范围内，得到对应的待兑现关节角集，以将所述待兑现关节角集作为所述目标关节角集。
需要说明的是，本申请实施例所提供的机械臂控制装置100，其基本原理及产生的技术效果与前述的机械臂控制方法相同。为简要描述，本实施例部分未提及之处，可参考上述的针对机械臂控制方法的描述内容。
在本申请所提供的实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本申请的实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个可读存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品 存储在一个可读存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的可读存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
综上所述，在本申请提供的机械臂控制方法、装置、作业控制设备及可读存储介质中，本申请在获取到仿人机械臂在当前时刻的实际关节角及待实现位姿后，会相应地计算待实现位姿所在的关节平面相对于对应参考平面的原始臂角值，以及与原始臂角值对应的待校关节角集，而后计算待实现位姿相对于机械臂关节角度的逆向雅可比矩阵，并利用关节避奇异特征限定条件对计算出的逆向雅可比矩阵进行参数修正，从而结合修正后的逆向雅可比矩阵、实际关节角集以及待校关节角集，针对待实现位姿计算满足关节避奇异特征限定条件的目标关节角集，最后按照目标关节角集对仿人机械臂各关节角度进行调整，以确保该仿人机械臂能够在避开机械臂奇异位置的同时安稳地运动达到目标位姿，增强仿人机械臂的可操作性。
以上所述，仅为本申请的各种实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应当以权利要求的保护范围为准。

Claims (10)

  1. 一种机械臂控制方法，其特征在于，所述方法包括：
    获取仿人机械臂在当前时刻的实际关节角集及待实现位姿，其中所述实际关节角集包括所述仿人机械臂各关节在当前时刻的实际关节角值；
    确定与所述待实现位姿对应的参考平面，并计算所述待实现位姿所在的关节平面相对于所述参考平面的原始臂角值，以及与所述原始臂角值对应的待校关节角集，其中所述待校关节角集包括所述仿人机械臂各关节的与所述待实现位姿对应的待校关节角值；
    计算所述待实现位姿相对于机械臂关节角度的逆向雅可比矩阵，并根据关节避奇异特征限定条件对计算出的逆向雅可比矩阵进行参数更新；
    根据更新后的逆向雅可比矩阵、所述实际关节角集以及所述待校关节角集，针对所述待实现位姿计算满足所述关节避奇异特征限定条件的目标关节角集，其中所述目标关节角集包括所述仿人机械臂各关节针对所述待实现位姿需要表达出的目标关节角值；
    按照所述目标关节角集对所述仿人机械臂各关节角度进行调整。
  2. 根据权利要求1所述的方法，其特征在于，所述根据关节避奇异特征限定条件对计算出的逆向雅可比矩阵进行参数更新的步骤，包括：
    对计算出的逆向雅可比矩阵进行奇异值分解，得到该逆向雅可比矩阵所对应的奇异值矩阵、特征值矩阵、左奇异向量矩阵及右奇异向量矩阵；
    将所述特征值矩阵中的最小特征值与所述关节避奇异特征限定条件所包括的逆雅可比特征阈值进行比对，得到对应的特征比对结果；
    计算所述最小特征值相对于所述逆雅可比特征阈值的特征比例，并根据所述特征比对结果、所述特征比例以及预设奇异调整值计算对应的奇异加载值；
    将计算出的所述奇异加载值对计算出的奇异值矩阵进行参数变更，得到变更后的奇异值矩阵；
    对所述左奇异向量矩阵、右奇异向量矩阵以及所述变更后的奇异值矩阵进行反向式奇异值分解处理，得到更新后的逆向雅可比矩阵。
  3. 根据权利要求1或2所述的方法，其特征在于，所述根据更新后的逆向雅可比矩阵、所述实际关节角集以及所述待校关节角集，针对所述待实现位姿计算满足所述关节避奇异特征限定条件的目标关节角集的步骤，包括：
    计算所述实际关节角集与所述待校关节角集在相同关节处对应的关节角差值，得到对应的差异关节角集，其中所述差异关节角集包括所述仿人机械臂各关节的待校关节角值与实际关节角值之间的角度差值；
    对所述差异关节角集与所述更新后的逆向雅可比矩阵进行矩阵乘法运算，得到对应的偏置关节角集，其中所述偏置关节角集包括所述仿人机械臂各关节需要加载的偏置关节角值；
    将所述偏置关节角集与所述待校关节角集在相同关节处进行角度叠加，得到对应的待实施关节角集，以将所述待实施关节角集作为所述目标关节角集。
  4. 根据权利要求3所述的方法，其特征在于，所述根据更新后的逆向雅可比矩阵、所述实际关节角集以及所述待校关节角集，针对所述待实现位姿计算满足所述关节避奇异特征限定条件的目标关节角集的步骤，还包括：
    计算与所述待实施关节角集对应的期望臂角值；
    根据所述期望臂角值以及所述仿人机械臂在历史时间段内连续的多个历史臂角值，构建对应的臂角变化曲线；
    对所述臂角变化曲线进行平滑处理，并从平滑处理后的臂角变化曲线中提取与当前时刻对应的待校验臂角值；
    计算与所述待校验臂角值对应的待处理关节角集，并将该待处理关节角集包括的每个关节的待处理关节角值限定到与该关节对应的预设关节角限位范围内，得到对应的待兑现关节角集，以将所述待兑现关节角集作为所述目标关节角集。
  5. 一种机械臂控制装置，其特征在于，所述装置包括：
    运动信息获取模块，用于获取仿人机械臂在当前时刻的实际关节角集及待实现位姿，其中所述实际关节角集包括所述仿人机械臂各关节在当前时刻的实际关节角值；
    臂角信息计算模块，用于确定与所述待实现位姿对应的参考平面，并计算所述待实现位姿所在的关节平面相对于所述参考平面的原始臂角值，以及与所述原始臂角值对应 的待校关节角集，其中所述待校关节角集包括所述仿人机械臂各关节的与所述待实现位姿对应的待校关节角值；
    关节奇异限定模块，用于计算所述待实现位姿相对于机械臂关节角度的逆向雅可比矩阵，并根据关节避奇异特征限定条件对计算出的逆向雅可比矩阵进行参数更新；
    关节角度矫正模块，用于根据更新后的逆向雅可比矩阵、所述实际关节角集以及所述待校关节角集，针对所述待实现位姿计算满足所述关节避奇异特征限定条件的目标关节角集，其中所述目标关节角集包括所述仿人机械臂各关节针对所述待实现位姿需要表达出的目标关节角值；
    关节运动控制模块，用于按照所述目标关节角集对所述仿人机械臂各关节角度进行调整。
  6. 根据权利要求5所述的装置，其特征在于，所述关节奇异限定模块包括：
    矩阵奇异分解子模块，用于对计算出的逆向雅可比矩阵进行奇异值分解，得到该逆向雅可比矩阵所对应的奇异值矩阵、特征值矩阵、左奇异向量矩阵及右奇异向量矩阵；
    矩阵特征比对子模块，用于将所述特征值矩阵中的最小特征值与所述关节避奇异特征限定条件所包括的逆雅可比特征阈值进行比对，得到对应的特征比对结果；
    奇异加载计算子模块，用于计算所述最小特征值相对于所述逆雅可比特征阈值的特征比例，并根据所述特征比对结果、所述特征比例以及预设奇异调整值计算对应的奇异加载值；
    奇异矩阵变更子模块，用于将计算出的所述奇异加载值对计算出的奇异值矩阵进行参数变更，得到变更后的奇异值矩阵；
    矩阵反向分解子模块，用于对所述左奇异向量矩阵、右奇异向量矩阵以及所述变更后的奇异值矩阵进行反向式奇异值分解处理，得到更新后的逆向雅可比矩阵。
  7. 根据权利要求5或6所述的装置，其特征在于，所述关节角度矫正模块包括：
    差异角度计算子模块，用于计算所述实际关节角集与所述待校关节角集在相同关节处对应的关节角差值，得到对应的差异关节角集，其中所述差异关节角集包括所述仿人机械臂各关节的待校关节角值与实际关节角值之间的角度差值；
    偏置角度计算子模块，用于对所述差异关节角集与所述更新后的逆向雅可比矩阵进行矩阵乘法运算，得到对应的偏置关节角集，其中所述偏置关节角集包括所述仿人机械臂各关节需要加载的偏置关节角值；
    关节角度叠加子模块，用于将所述偏置关节角集与所述待校关节角集在相同关节处进行角度叠加，得到对应的待实施关节角集，以将所述待实施关节角集作为所述目标关节角集。
  8. 根据权利要求7所述的装置，其特征在于，所述关节角度矫正模块还包括：
    期望臂角计算子模块，用于计算与所述待实施关节角集对应的期望臂角值；
    臂角曲线构建子模块，用于根据所述期望臂角值以及所述仿人机械臂在历史时间段内连续的多个历史臂角值，构建对应的臂角变化曲线；
    校验臂角提取子模块，用于对所述臂角变化曲线进行平滑处理，并从平滑处理后的臂角变化曲线中提取与当前时刻对应的待校验臂角值；
    关节角度限定子模块，用于计算与所述待校验臂角值对应的待处理关节角集，并将该待处理关节角集包括的每个关节的待处理关节角值限定到与该关节对应的预设关节角限位范围内，得到对应的待兑现关节角集，以将所述待兑现关节角集作为所述目标关节角集。
  9. 一种作业控制设备，其特征在于，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机程序，所述处理器可执行所述计算机程序，以实现权利要求1-4中任意一项所述的机械臂控制方法。
  10. 一种可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现权利要求1-4中任意一项所述的机械臂控制方法。
PCT/CN2021/131202 2021-03-23 2021-11-17 机械臂控制方法、装置、作业控制设备及可读存储介质 WO2022199059A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202110310049.7A CN113021347A (zh) 2021-03-23 2021-03-23 机械臂控制方法、装置、作业控制设备及可读存储介质
CN202110310049.7 2021-03-23

Publications (1)

Publication Number Publication Date
WO2022199059A1 true WO2022199059A1 (zh) 2022-09-29

Family

ID=76473028

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2021/131202 WO2022199059A1 (zh) 2021-03-23 2021-11-17 机械臂控制方法、装置、作业控制设备及可读存储介质

Country Status (2)

Country Link
CN (1) CN113021347A (zh)
WO (1) WO2022199059A1 (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113021347A (zh) * 2021-03-23 2021-06-25 深圳市优必选科技股份有限公司 机械臂控制方法、装置、作业控制设备及可读存储介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103955619A (zh) * 2014-05-09 2014-07-30 大连大学 一种七自由度空间机械臂最小基座扰动解析逆运动学求解方法
US20150120059A1 (en) * 2013-10-30 2015-04-30 Denso Wave Incorporated Axis angle determination method for six-axis robot and control apparatus for six-axis robot
CN107066645A (zh) * 2016-12-01 2017-08-18 西北工业大学 一种七自由度偏置机械臂逆解方法
CN108608427A (zh) * 2018-06-05 2018-10-02 中科新松有限公司 机器人力控牵引过程中的避奇异方法及装置
CN110977974A (zh) * 2019-12-11 2020-04-10 遨博（北京）智能科技有限公司 一种机器人规避奇异位型的导纳控制方法、装置及系统
CN112091979A (zh) * 2020-10-10 2020-12-18 哈尔滨工业大学 一种基于位置级逆运动学的七自由度机械臂限位优化方法
CN113021347A (zh) * 2021-03-23 2021-06-25 深圳市优必选科技股份有限公司 机械臂控制方法、装置、作业控制设备及可读存储介质

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5159249A (en) * 1989-05-16 1992-10-27 Dalila Megherbi Method and apparatus for controlling robot motion at and near singularities and for robot mechanical design
CN103909522B (zh) * 2014-03-19 2016-08-17 华南理工大学 一种六自由度工业机器人通过奇异域的方法
CN105382835B (zh) * 2015-12-11 2017-06-20 华中科技大学 一种可穿越腕部奇异点的机器人路径规划方法
CN109605369B (zh) * 2018-12-07 2021-09-07 英华达（上海）科技有限公司 机械臂奇异点控制方法及系统
CN109571481A (zh) * 2018-12-30 2019-04-05 深圳市越疆科技有限公司 一种关节奇异点处理方法、装置、设备和存储介质
US11667035B2 (en) * 2019-07-01 2023-06-06 Wisconsin Alumni Research Foundation Path-modifying control system managing robot singularities

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150120059A1 (en) * 2013-10-30 2015-04-30 Denso Wave Incorporated Axis angle determination method for six-axis robot and control apparatus for six-axis robot
CN103955619A (zh) * 2014-05-09 2014-07-30 大连大学 一种七自由度空间机械臂最小基座扰动解析逆运动学求解方法
CN107066645A (zh) * 2016-12-01 2017-08-18 西北工业大学 一种七自由度偏置机械臂逆解方法
CN108608427A (zh) * 2018-06-05 2018-10-02 中科新松有限公司 机器人力控牵引过程中的避奇异方法及装置
CN110977974A (zh) * 2019-12-11 2020-04-10 遨博（北京）智能科技有限公司 一种机器人规避奇异位型的导纳控制方法、装置及系统
CN112091979A (zh) * 2020-10-10 2020-12-18 哈尔滨工业大学 一种基于位置级逆运动学的七自由度机械臂限位优化方法
CN113021347A (zh) * 2021-03-23 2021-06-25 深圳市优必选科技股份有限公司 机械臂控制方法、装置、作业控制设备及可读存储介质

Also Published As

Publication number Publication date
CN113021347A (zh) 2021-06-25

Similar Documents

Publication Publication Date Title
CN109828600B (zh) 时间最优快速三维避障路径规划方法
JP6321905B2 (ja) 関節システムの制御方法、記憶媒体、制御システム
US9156159B2 (en) Robot cyclic locomotion using a dynamic object
JP7324932B2 (ja) 動的計画コントローラ
CN109048890A (zh) 基于机器人的协调轨迹控制方法、系统、设备及存储介质
WO2022199080A1 (zh) 机器人迈步控制方法、装置、机器人控制设备及存储介质
Kussaba et al. Hybrid kinematic control for rigid body pose stabilization using dual quaternions
CN107272677A (zh) 一种移动机器人的变结构自适应轨迹跟踪控制方法
WO2017132905A1 (zh) 控制运动系统的方法和装置
WO2022183785A1 (zh) 机器人定位方法、装置、机器人和可读存储介质
Frye et al. Fast convergent observer design for output feedback stabilisation of a planar vertical takeoff and landing aircraft
WO2022199059A1 (zh) 机械臂控制方法、装置、作业控制设备及可读存储介质
WO2020093253A1 (zh) 机器人的运动控制方法、控制系统和存储装置
WO2022199058A1 (zh) 机器人步长处理方法、装置、机器人控制设备及存储介质
Zhang et al. Two hybrid multiobjective motion planning schemes synthesized by recurrent neural networks for wheeled mobile robot manipulators
Yan et al. Analytical inverse kinematics of a class of redundant manipulator based on dual arm-angle parameterization
Raiesdana Control of quadrotor trajectory tracking with sliding mode control optimized by neural networks
JP6563596B2 (ja) 画像処理装置、画像処理方法およびプログラム
WO2022205941A1 (zh) 运动控制方法、装置、机器人控制设备及可读存储介质
Zhang et al. Global stabilization of underactuated spring-coupled three-link horizontal manipulator using position measurements only
Lam et al. Motion planning for tree climbing with inchworm‐like robots
Samiei et al. Attitude stabilization of rigid spacecraft with minimal attitude coordinates and unknown time-varying delay
Kanoun et al. An optimization formulation for footsteps planning
Zou et al. Imaged-based visual servo control for a VTOL aircraft
CN114454981A (zh) 双足机器人的弹跳运动控制方法、装置及双足机器人

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21932667

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21932667

Country of ref document: EP

Kind code of ref document: A1