CN113799120A

CN113799120A - 冗余自由度机械臂的路径规划方法、装置及工程机械

Info

Publication number: CN113799120A
Application number: CN202110864557.XA
Authority: CN
Inventors: 付玲; 于晓颖; 皮皓杰; 曾杨
Original assignee: Zoomlion Heavy Industry Science and Technology Co Ltd
Current assignee: Zoomlion Heavy Industry Science and Technology Co Ltd
Priority date: 2021-07-15
Filing date: 2021-07-29
Publication date: 2021-12-17
Anticipated expiration: 2041-07-29
Also published as: CN113799120B

Abstract

本发明实施方式提供一种冗余自由度机械臂的路径规划方法、装置及工程机械，涉及机械臂路径规划技术领域。方法包括：确定机械臂的执行末端所要达到的目标位姿；确定机械臂的所有关节中当该机械臂的执行末端位于目标位姿时能够保持初始角度的关节；基于已确定的关节确定机械臂的执行末端达到目标位姿的路径数据，路径数据包括机械臂的所有关节的目标角度，其中至少一个关节的目标角度与该关节的初始角度相等；依据路径数据确定机械臂的运动路径，将运动路径中满足预设规则的运动路径作为机械臂的最终运动路径。本发明基于锁定的关节对机械臂进行运动路径规划，从而能够有效减少机械臂的执行末端在运动过程中的抖动。

Description

冗余自由度机械臂的路径规划方法、装置及工程机械

技术领域

本发明涉及机械臂路径规划技术领域，具体地涉及一种冗余自由度机械臂的路径规划方法、一种冗余自由度机械臂的路径规划装置及一种工程机械。

背景技术

现有的工程机械臂架通常具有较大的尺寸，在机械臂执行动作时，往往具有较大的惯性，又由于现有工程机械臂架的驱动装置往往为液压驱动，而液压泵的输出功率有限，因此机械臂架的多个关节联动导致机械臂的执行末端存在较大的抖动，在机械臂运动过程中，运动关节越多，机械臂的执行末端的抖动越大，尤其是越靠近机械臂架基座的关节的运动对于机械臂的执行末端的抖动影响越大。现有技术中，对机械臂的运动路径规划通常是在机械臂的整个关节空间内进行路径搜索，但并没有考虑如何减少机械臂的运动关节数量以减少机械臂执行末端的抖动。

发明内容

本发明实施方式的目的是提供一种于冗余自由度机械臂的路径规划方法、装置及工程机械，以解决现有技术无法减少机械臂执行末端抖动的问题。

为了实现上述目的，在本发明的第一方面，提供一种冗余自由度机械臂的路径规划方法，包括：

确定机械臂的执行末端所要达到的目标位姿；

确定机械臂的所有关节中当该机械臂的执行末端位于所述目标位姿时能够保持初始角度的关节；

基于已确定的所述关节确定所述机械臂的执行末端达到所述目标位姿的路径数据，所述路径数据包括所述机械臂的所有关节的目标角度，其中至少一个关节的目标角度与该关节的初始角度相等；

依据所述路径数据确定所述机械臂的运动路径，将所述运动路径中满足预设规则的运动路径作为机械臂的最终运动路径。

可选地，确定机械臂的所有关节中当该机械臂的执行末端位于所述目标位姿时能够保持初始角度的关节，包括：

锁定机械臂的当前关节的目标角度为初始角度，并针对所述目标位姿进行逆运动学求解，若能够求解得到该机械臂的执行末端达到所述目标位姿的初始路径数据，则确定机械臂的当前关节为当该机械臂的执行末端位于所述目标位姿时能够保持初始角度的关节，并锁定机械臂的当前关节的目标角度为初始角度；若求解结果为空，则将机械臂的当前关节的目标角度还原至锁定前的状态；

针对与机械臂的当前关节相邻的下一个关节，重复上述过程，直至完成对机械臂的所有关节中当该机械臂的执行末端位于所述目标位姿时能够保持初始角度的关节的确定。

可选地，在锁定机械臂的当前关节的目标角度为初始角度，并针对所述目标位姿进行逆运动学求解之前，该方法还包括：

确定机械臂的起点关节，以该机械臂的起点关节作为该机械臂的当前关节；

其中，机械臂的起点关节为远离该机械臂的执行末端一侧的关节。

可选地，基于已确定的所述关节确定所述机械臂的执行末端达到所述目标位姿的路径数据，包括：

基于求解得到的所述初始路径数据对机械臂进行碰撞检测，若确定所述机械臂不与任务空间内的障碍物发生碰撞，则以求解得到的所述初始路径数据作为所述机械臂的执行末端达到所述目标位姿的路径数据。

可选地，基于求解得到的所述初始路径数据对机械臂进行碰撞检测，包括：

基于求解得到的所述初始路径数据对与所述初始路径数据中锁定目标角度为初始角度的关节连接的机械臂连杆进行碰撞检测；

若确定所述机械臂不与任务空间内的障碍物发生碰撞，则以求解得到的所述初始路径数据作为所述机械臂的执行末端达到所述目标位姿的路径数据，包括：

若确定所述机械臂连杆不与任务空间内的障碍物发生碰撞，则以求解得到的所述初始路径数据作为所述机械臂的执行末端达到所述目标位姿的路径数据。

可选地，直至完成对机械臂的所有关节中当该机械臂的执行末端位于所述目标位姿时能够保持初始角度的关节的确定，包括：

若当前已确定的关节的数量达到设定阈值，则确定完成对机械臂的所有关节中当该机械臂的执行末端位于所述目标位姿时能够保持初始角度的关节的确定。

可选地，依据所述路径数据确定所述机械臂的运动路径，将所述运动路径中满足预设规则的运动路径作为机械臂的最终运动路径，包括：

针对每一路径数据，基于采样的路径搜索算法搜索机械臂的执行末端达到所述目标位姿时不与任务空间内的障碍物碰撞的运动路径；

遍历搜索到的所有运动路径，以满足预设规则的运动路径作为机械臂的最终运动路径。

可选地，所述预设规则包括：

计算所有运动路径的路径距离；

以路径距离最短的运动路径作为机械臂的最终运动路径。

可选地，所述方法还包括：

针对每一路径数据，若基于采样的路径搜索算法无法搜索到机械臂的执行末端达到所述目标位姿时不与任务空间内的障碍物碰撞的运动路径，且对应路径数据中已锁定目标角度为初始角度的关节数量大于1，则将对应路径数据中已锁定目标角度为初始角度的关节中最靠近机械臂的执行末端的关节的目标角度还原至锁定前的状态，得到还原后的路径数据；

若其余的路径数据中不存在与所述还原后的路径数据相同的路径数据，则针对所述还原后的路径数据基于采样的路径搜索算法进行运动路径搜索。

在本发明的第二方面，提供一种冗余自由度机械臂的路径规划装置，包括：

位姿确定模块，被配置为确定机械臂的执行末端所要达到的目标位姿；

路径数据生成模块，被配置为确定机械臂的所有关节中当该机械臂的执行末端位于所述目标位姿时能够保持初始角度的关节，以及基于已确定的所述关节确定所述机械臂的执行末端达到所述目标位姿的路径数据，所述路径数据包括所述机械臂的所有关节的目标角度，其中至少一个关节的目标角度与该关节的初始角度相等；

路径规划模块，被配置为依据所述路径数据确定所述机械臂的运动路径，将所述运动路径中满足预设规则的运动路径作为机械臂的最终运动路径。

在本发明的第三方面，提供一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述的冗余自由度机械臂的路径规划方法。

在本发明的第四方面，提供一种计算机可读介质，所述计算机可读介质存储有计算机程序，所述计算机程序被处理执行时实现上述的冗余自由度机械臂的路径规划方法。

在本发明的第五方面，提供一种工程机械，包括：

上述的冗余自由度机械臂的路径规划装置。

本发明上述技术方案通过确定机械臂的执行末端达到目标位姿时能够锁定目标角度为初始角度的关节，并基于锁定的关节对机械臂进行运动路径规划，从而能够减少机械臂的执行末端达到目标位姿时运动的关节数量，进而减少机械臂的执行末端在运动过程中的抖动。

本发明实施方式的其它特征和优点将在随后的具体实施方式部分予以详细说明。

附图说明

附图是用来提供对本发明实施方式的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明实施方式，但并不构成对本发明实施方式的限制。在附图中：

图1是本发明优选实施方式提供的一种冗余自由度机械臂的路径规划方法的方法流程图；

图2是本发明优选实施方式提供的机械臂结构示意图；

图3是本发明优选实施方式提供的备用解求解流程示意图；

图4是本发明优选实施方式提供的路径规划流程示意图；

图5是本发明优选实施方式提供的一种冗余自由度机械臂的路径规划装置的示意框图。

具体实施方式

以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。

如图1所示，在本实施方式的第一方面，提供一种冗余自由度机械臂的路径规划方法，包括：

确定机械臂的执行末端所要达到的目标位姿；确定机械臂的所有关节中当该机械臂的执行末端位于目标位姿时能够保持初始角度的关节；基于已确定的关节确定机械臂的执行末端达到目标位姿的路径数据，路径数据包括机械臂的所有关节的目标角度，其中至少一个关节的目标角度与该关节的初始角度相等；依据路径数据确定机械臂的运动路径，将运动路径中满足预设规则的运动路径作为机械臂的最终运动路径。

如此，本实施方式通过确定机械臂的执行末端达到目标位姿时能够锁定目标角度为初始角度的关节，并基于锁定的关节对机械臂进行运动路径规划，从而能够减少机械臂的执行末端达到目标位姿时运动的关节数量，进而减少机械臂的执行末端在运动过程中的抖动。

具体的，现有的大型工程机械中，机械臂的尺寸往往达到几十米，因此，在机械臂作业过程中，往往具有较大的惯性，又由于现有机械臂的驱动装置通常为液压驱动，液压泵的输出功率有限，机械臂在作业过程中，机械臂各关节的联动导致机械臂的执行末端存在较大抖动，对机械臂执行末端执行布料任务造成了影响。本实施方式中，令机械臂的各关节为J0～Jn，其中，J0为连接臂架基座的初始关节，Jn为最靠近执行末端的关节；令L₀为连接机械臂关节J0与关节J1的臂架连杆，L₁为连接关节J1和关节J2的臂架连杆，以此类推，L_n为关节Jn与执行末端连接的臂架连杆。机械臂在执行布料任务之前，需要确定机械臂执行末端的目标位姿，并对机械臂进行路径规划，确定各关节的目标角度，以控制机械臂的执行末端能够按照预定路径到达目标位姿。如图2所示，A为机械臂执行末端的初始位置，B为机械臂执行末端达到目标位姿时的目标点，本实施方式以n＝6，即机械臂有7个关节为例，在进行路径规划之前，首先建立机械臂任务空间的坐标系，例如可以以机械臂基座为坐标原点建立笛卡尔坐标系，确定目标位姿在笛卡尔坐标系中的坐标，获取机械臂执行末端在初始位置A时机械臂各关节的初始角度

其中，

表示关节J0的初始角度，

表示关节J1的初始角度，以此类推。在关节J0～J6的初始角度

臂架连杆L₀～L₆长度以及机械臂执行末端的目标位姿坐标已知的情况下，即可通过几何法或梯度投影法针对目标位姿进行逆运动学求解，求得机械臂执行末端的目标位姿对应的运动学逆解组，每个逆解均包括机械臂的执行末端达到目标位姿时各关节的目标角度

每个逆解即为一条路径数据，通过控制机械臂各关节转动到目标角度，便可控制机械臂的执行末端达到目标位姿，其中，求运动学逆解组的方法为现有技术，此处对求解过程不再赘述。然而现有方法中，在求目标位姿对应的运动学逆解组时，并没有考虑如何降低机械臂执行末端达到目标位姿的过程中存在较大抖动的问题。为了解决机械臂执行末端在运动过程中存在较大抖动的问题，本实施方式通过锁定机械臂关节的目标角度为初始角度来求目标位姿对应的运动学逆解组，即以被锁定关节的目标角度为已知值，且该值等于被锁定关节的初始角度，若该关节被锁定，则将连接该关节的两个连杆视为一个整体来求目标位姿对应的运动学逆解组，例如，若关节J3的初始角度为45°，当J3被锁定时，在针对目标位姿进行运动学求逆解时，令J3的目标角度为45°，同时视连接J3的连杆L2及L3为一个整体，作为求目标位姿对应的运动学逆解组的已知参数。可以理解的是，在求目标位姿对应的运动学逆解组时，被锁定的关节可以为多个，例如，在求目标位姿对应的运动学逆解组时，先锁定机械臂的关节J1，求J1被锁定时目标位姿对应的运动学逆解组，再锁定机械臂的关节J2，求J2被锁定时目标位姿对应的运动学逆解组，依次类推，直至求得J6被锁定时目标位姿对应的运动学逆解组；同时，也可以求关节J1及J2被同时锁定时目标位姿对应的运动学逆解组，或者关节J1及J3被同时锁定时目标位姿对应的运动学逆解组；同理，也可以求关节J1、J2及J3被同时锁定时目标位姿对应的运动学逆解组，以此类推，此处不再枚举。这样，通过本实施方式的方法求得的每个运动学逆解中均至少包括一个被锁定的关节，因此，能够保证在根据预设规则筛选得到的最终的逆解中必然包括被锁定的关节，在依据最终的逆解控制机械臂动作的过程中，能够相比现有技术有效的降低机械臂执行末端的抖动。可以理解的是，确定在机械臂的执行末端位于目标位姿时能够被锁定的关节可以有多种方法，例如，求目标位姿对应的所有运动学逆解组，遍历得到的所有逆解，判断各逆解中是否存在目标角度与初始角度相同的关节，若存在，则将该逆解作为备用解，并抛弃其他逆解，从所有备用解中选择满足预设规则的逆解作为最终的路径规划数据；又例如，可以每次锁定一个关节，判断在锁定当前关节的情况下是否能求得目标位姿对应的逆解组，若能够求得逆解组，则确定当前关节能够被锁定，将对应的逆解组作为备用解，若无法求得逆解组，即求解结果为空，则确定当前关节不能被锁定，以此类推，直至对所有关节及各关节的组合均进行锁定求解判断后，对得到的所有备用解进行机械臂的路径规划，从得到的所有运动路径中选择满足预设规则的运动路径作为最终的运动路径。

如图3所示，为了减少计算量，提高计算效率，确定机械臂的所有关节中当该机械臂的执行末端位于目标位姿时能够保持初始角度的关节，包括：锁定机械臂的当前关节的目标角度为初始角度，并针对目标位姿进行逆运动学求解，若能够求解得到该机械臂的执行末端达到目标位姿的初始路径数据，则确定机械臂的当前关节为当该机械臂的执行末端位于目标位姿时能够保持初始角度的关节，并锁定机械臂的当前关节的目标角度为初始角度；若求解结果为空，则将机械臂的当前关节的目标角度还原至锁定前的状态；针对与机械臂的当前关节相邻的下一个关节，重复上述过程，直至完成对机械臂的所有关节中当该机械臂的执行末端位于目标位姿时能够保持初始角度的关节的确定。

确定机械臂关节能否被锁定时，首先任意选择一个机械臂关节，例如关节J2，锁定关节J2，假设关节J2的初始角度为45°，令关节J2的目标角度等于初始角度45°，将关节J2的目标角度作为已知参数，求目标位姿对应的运动学逆解组，若此时求解得到目标位姿的运动学逆解组，则确定关节J2能够被锁定，保持锁定关节J2，锁定与关节J2相邻的下一个关节J3，假设关节J3的初始角度为40°，令关节J3的目标角度等于初始角度40°，将关节J2的目标角度45°、关节J3的目标角度40°作为已知参数，求目标位姿对应的运动学逆解组；若在关节J2锁定的情况下，无法求得目标位姿对应的运动学逆解组，则释放关节J2的锁定，将关节J2的目标角度作为变量，锁定关节J3，并在关节J2的锁定被释放，关节J3被锁定的情况下求目标位姿对应的运动学逆解组。同理，若在关节J2及J3均被锁定的情况下求解得到目标位姿的运动学逆解组，则进一步在保持锁定关节J2及关节J3的情况下，锁定关节J4，判断是否能够求解得到目标位姿的运动学逆解组，若不能，则释放关节J4的锁定，在保持关节J2及J3锁定的情况下，锁定关节J5，判断是否能够求解得到目标位姿的运动学逆解组，以此类推，直至对关节J2之后的所有关节均进行判断后，便能确定能够被锁定的关节。这样，在判断当前关节能被锁定的同时，便能确定当前关节被锁定时能够求得的目标位姿的运动学逆解组为备用解，从而有效提高了计算效率。需要理解的是，在进行关节锁定判定时，初始关节的选择可以根据实际情况自定义，例如，若不希望某些关节如关节J0被锁定，则在选择当前关节时选择关节J1作为初始的关节；若不希望关节J0及J1被锁定，则可选择关节J2作为初始的关节。

为了最大程度的减小执行末端在运动过程中的抖动，本实施方式在锁定机械臂的当前关节的目标角度为初始角度，并针对目标位姿进行逆运动学求解之前，该方法还包括：确定机械臂的起点关节，以该机械臂的起点关节作为该机械臂的当前关节；其中，机械臂的起点关节为远离该机械臂的执行末端一侧的关节。由于越靠近机械臂基座的关节的运动对机械臂执行末端的抖动影响越大，因此，本实施方式优先锁定机械臂大臂上的关节，具体的，以机械臂中距离机械臂的执行末端最远，即距离机械臂基座最近的关节作为判定关节锁定时的起点关节，这样在进行关节锁定判定时，能够以机械臂基座到执行末端的顺序进行关节的锁定判定，从而能够保证最终得到的备用解中被锁定的关节尽量靠近机械臂基座，进而最大程度的减小执行末端的抖动。另外，由于连接机械臂基座的关节往往为回转关节，若回转关节被锁定，则机械臂的执行末端通常无法达到目标位姿，因此，当连接机械臂基座的关节为回转关节时例如本实施方式中的关节J0为回转关节时，以关节J1作为起点关节，在进行关节锁定判定时，按照J1-J2-J3-J4-J5-J6的顺序进行判定，这样，在进行关节锁定判定时，无需计算关节J0被锁定时的运动学逆解组，能够进一步减小计算量，提高计算效率。

为了更准确的判定当前关节能否被锁定，基于已确定的关节确定机械臂的执行末端达到目标位姿的路径数据，包括：基于求解得到的初始路径数据对机械臂进行碰撞检测，若确定机械臂不与任务空间内的障碍物发生碰撞，则以求解得到的初始路径数据作为机械臂的执行末端达到目标位姿的路径数据。

在对机械臂进行路径规划之前，往往需要确定机械臂的任务空间并获取机械臂任务空间内的障碍物信息如坐标信息，则机械臂进行碰撞检测的方法可以为：将障碍物进行规则化处理，以长方体或者圆柱体将障碍物包括进去，将机械臂的连杆以圆柱体进行包络，为了便于处理，可以将机械臂等效成线，而对障碍物进行膨胀处理，则在对机械臂进行碰撞检测时可简化为当机械臂的等效线与障碍物的面干涉时即可认为发生了碰撞。机械臂的碰撞检测为现有技术，此处不再赘述。若机械臂的当前关节能够被锁定，则基于对当前关节被锁定时求得的运动学逆解组进行碰撞检测，以当机械臂关节J1锁定时能够求得目标位姿对应的运动学逆解组为例，对于求得的每个逆解，获取每个逆解中各关节的目标角度，通过预先建立的机械臂模型模拟各关节到达目标角度过程中机械臂的运动轨迹，并检测机械臂是否与障碍物发生碰撞，若会发生碰撞，则说明当前关节不能被锁定，抛弃当前的逆解，若不会发生碰撞，则将当前的逆解作为备用解，重复上述过程，直至对所有逆解进行碰撞检测。

为了减少关节锁定判定阶段的计算量，提高获取备用解的效率，基于求解得到的初始路径数据对机械臂进行碰撞检测，包括：基于求解得到的初始路径数据对与初始路径数据中锁定目标角度为初始角度的关节连接的机械臂连杆进行碰撞检测；若确定机械臂不与任务空间内的障碍物发生碰撞，则以求解得到的初始路径数据作为机械臂的执行末端达到目标位姿的路径数据，包括：若确定机械臂连杆不与任务空间内的障碍物发生碰撞，则以求解得到的初始路径数据作为机械臂的执行末端达到目标位姿的路径数据。。为了快速判断当前关节能否被锁定，确定备用解，在判定当前关节能否被锁定并确定备用解时，仅对与被锁定关节相关的机械臂连杆进行碰撞检测。以锁定关节J1为例，锁定后的关节J1的目标关节角度

并将连杆L₀与L₁视为一个整体，求锁定关节J1后执行末端的目标位姿对应的运动学逆解组，对于每个逆解，获取当机械臂的执行末端达到目标位姿时各关节的目标角度q_B，若求解得到的q_B不为空，则进一步对连杆L0与L1进行碰撞检测，判断回转关节J0从

运动到

过程中连杆L₀与连杆L₁是否与障碍物发生干涉，若发生干涉，则说明关节J1不能被锁定，抛弃当前逆解，若不发生干涉，则说明关节J1能够被锁定，将当前逆解作为备用解，直至针对所有逆解均进行碰撞检测、确定备用解后，继续判断关节J2是否能够被锁定。

在机械臂工作过程中，机械臂至少需要保证一定的自由度才能够使机械臂的执行末端达到目标位姿，若被锁定的关节过多，则机械臂的执行末端无法达到目标位姿，因此，为了避免不必要的计算，本实施方式中，直至完成对机械臂的所有关节中当该机械臂的执行末端位于目标位姿时能够保持初始角度的关节的确定，包括：若当前已确定的关节的数量达到设定阈值，则确定完成对机械臂的所有关节中当该机械臂的执行末端位于目标位姿时能够保持初始角度的关节的确定。由于J0为连接机械臂基座的回转关节，不能被锁定，为了保证机械臂的执行末端能够达到目标位姿，还需要至少保证2个关节不被锁定，因此，不能被锁定的关节数量为3，能够被锁定的关节数量最多为4个，则设定阈值为4，在判断当前关节是否能被锁定时，对不能被锁定的关节进行计数，例如，令能够被锁定的关节计数值初值为0，判断关节J1被锁定后能否求得目标位姿对应的运动学逆解组，若不能求得逆解组，则关节J1不能被锁定；若能够求得逆解组，进一步判断连杆L0及L1是否与障碍物发生干涉，若连杆L0及L1与障碍物不发生干涉，则判断关节J1能够被锁定，计数值加1。按照上述步骤依次对关节J2～J6进行判断，直至计数值为4时，停止继续对当前关节进行锁定判断，以当前已确定的逆解作为备用解，例如，若按上述步骤已确定关节J1、J3、J4能够被锁定，关节J2不能被锁定，当前计数值为3。若判断关节J5不能被锁定，则继续判断关节J6能否被锁定；若判断关节J5能够被锁定，计数值加1，此时计数值为4，达到设定阈值，则不再继续对关节J6进行判断，确定关节J1、J3、J4、J5能够被锁定，以当前已经得到的所有逆解作为备用解。

如图4所示，得到备用解后，针对每个备用解进行路径规划，以判断机械臂的执行末端能否在任务空间内达到目标位姿，因此，依据路径数据确定机械臂的运动路径，将运动路径中满足预设规则的运动路径作为机械臂的最终运动路径，包括：针对每一路径数据，基于采样的路径搜索算法搜索机械臂的执行末端达到目标位姿时不与任务空间内的障碍物碰撞的运动路径；遍历搜索到的所有运动路径，以满足预设规则的运动路径作为机械臂的最终运动路径。其中，基于采样的路径搜索算法为RRT算法，即快速探索随机树算法，RRT算法是一种通用方法，不管机械臂是什么类型、具有几个自由度、约束多复杂都能适用，因此，在机械臂/机器人路径规划中，通常基于RRT算法构建的路径规划器。可以理解的是，任务空间内障碍物的位置信息如障碍物在笛卡尔坐标系中的坐标信息是已知的，也可以是实时获取生成的，以供路径规划器在路径规划过程中进行碰撞检测。RRT算法的原理为：确定机械臂关节在任务空间的起点及终点，以起点作为根节点，在搜索的关节空间中随机生成采样点，连接当前节点及采样点，对连线进行碰撞检测，若连线不与障碍物碰撞，则当前节点能够运动到采样节点，将两个节点建立连接，通过多次迭代采样即可搜索从起点到终点的运动路径。针对所有备用解，以机械臂各关节的初始角度q_A及机械臂各关节的目标角度q_B作为路径规划器的输入，经路径规划器针对未锁定的关节空间规划机械臂各关节从初始角度q_A运动到目标角度q_B的运动路径，对于得到的所有运动路径，选择满足预设规则的运动路径作为最终的运动路径。其中，预设规则包括：计算所有运动路径的路径距离；以路径距离最短的运动路径作为机械臂的最终运动路径。

为了避免搜索不到运动路径而导致死循环，通常以设定最大迭代次数作为路径规划器的终止条件，若路径规划器的迭代次数大于设定阈值，则可能是由于关节锁定导致无法搜索到执行末端到达目标位姿的运动路径，因此，需要释放相应的锁定关节以增加机械臂的自由度，从而提高搜索到运动路径的概率，因此，本实施方式的方法还包括：针对每一路径数据，若基于采样的路径搜索算法无法搜索到机械臂的执行末端达到目标位姿时不与任务空间内的障碍物碰撞的运动路径，且对应路径数据中已锁定目标角度为初始角度的关节数量大于1，则将对应路径数据中已锁定目标角度为初始角度的关节中最靠近机械臂的执行末端的关节的目标角度还原至锁定前的状态，得到还原后的路径数据；若其余的路径数据中不存在与还原后的路径数据相同的路径数据，则针对还原后的路径数据基于采样的路径搜索算法进行运动路径搜索。可以理解的，若q_B中，被锁定的关节数量为1，则当路径规划器达到迭代阈值仍无法搜索到运动路径时，释放被锁定关节后，q_B中将不存在被锁定的关节，此时针对释放被锁定关节后的q_B进行运动路径的搜索，输出得到的运动路径。当被锁定的关节数量大于1时，若路径规划器达到迭代阈值仍无法搜索到运动路径，则确定q_B中所有被锁定的关节中最靠近机械臂的执行末端的关节，并释放对该关节的锁定，由于释放该关节后的q_B可能存在与得到的备用解中，因此，为了避免重复计算，在释放该关节的锁定后，若判断剩余的备用解中不存在与被释放该关节后的q_B相同的备用解，则针对被释放后的q_B继续进行路径搜索，重复上述过程，直至搜索到运动路径；若判断剩余的备用解中存在与被释放该关节后的q_B相同的备用解，判断剩余的备用解中是否存在未进行路径规划的备用解，若存在，选取一个备用解，重复上述过程搜索运动路径，若不存在，输出所有搜索到的运动路径。这样，每次当路径规划器无法搜索到运动路径时均释放最靠近机械臂的执行末端的关节，从而能保证最终得到的运动路径能够最大程度的降低执行末端在运动过程中的抖动。

以图2所示的机械臂为例，设机械臂关节J0～J6的初始角度为q_A＝[0，90，45，0，10，10，6]，确定机械臂执行末端的目标位姿，通过上述方法求锁定关节的情况下，目标位姿对应的运动学逆解组，得到以下两个解：

q_B＝[40,90,45,13,10,10,20]；

q_B＝[40,90,45,60,10,15,9]；

对比起始关节状态q_A可知，q_B中，关节J1、J2、J4及J5的目标角度等于起始角度，因此q_B中被锁定的关节为J1、J2、J4及J5，同理，q_B中锁定的关节为J1、J2及J4。首先以q_A作为路径规划的起点，以q_B作为路径规划的目标点，利用基于随机采样的RRT路径规划器进行路径搜索，搜索过程为：首先将q_A作为当前节点，在未被锁定的关节空间中随机生成多个采样点，并结合锁定的关节的角度形成最终的臂架采样的一组样本点，将当前节点与从当前节点出发运动到采样点未发生碰撞的采样节点进行连接，然后选取样本点中的某个节点作为当前节点，再次进行采样并连接，如此经过多次迭代进行路径的搜索。若迭代次数超过一定阈值，则从机械臂执行末端一侧开始，释放最靠近执行末端的一个被锁定的关节J5，将q_B中的关节J5释放后，q_B中被锁定的关节与q_B一致，因此此时的q_B对于减少机械臂关节运动数量已经失效，故重新选择q_B作为目标点再次进行路径规划。

如图5所示，在本发明的第二方面，提供一种冗余自由度机械臂的路径规划装置，包括：

路径数据生成模块，被配置为确定机械臂的所有关节中当该机械臂的执行末端位于目标位姿时能够保持初始角度的关节，以及基于已确定的关节确定机械臂的执行末端达到目标位姿的路径数据，路径数据包括机械臂的所有关节的目标角度，其中至少一个关节的目标角度与该关节的初始角度相等；

路径规划模块，被配置为依据路径数据确定机械臂的运动路径，将运动路径中满足预设规则的运动路径作为机械臂的最终运动路径。

在本发明的第三方面，提供一种终端设备，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述的冗余自由度机械臂的路径规划方法。

在本发明的第四方面，提供一种计算机可读介质，计算机可读介质存储有计算机程序，计算机程序被处理执行时实现上述的冗余自由度机械臂的路径规划方法。

在本发明的第五方面，提供一种工程机械，包括：上述的冗余自由度机械臂的路径规划装置。

综上所述，本实施方式利用机械臂架冗余自由度的特性搜索能够被锁定的部分关节，能够有效减少臂架从起始状态运动到目标状态的过程中运动关节的数量，同时也减少了路径搜索过程中规划计算的维度，降低了冗余自由度臂架路径规划问题的复杂度，提高了算法搜索到目标路径的可能性；同时，优先从机械臂架靠近基座的大臂开始搜索能够锁定的关节，能够有效减小臂架在运动过程中臂架末端的抖动，提高臂架在布料过程中的安全性和工作效率；基于求运动学逆解组输出多种关节锁定情况下的备用解，基于备用解进行路径规划，在路径搜索的过程中当迭代次数大于设定的阈值，则将锁定的关节中最靠近臂架末端的关节进行释放，增加采用空间的维度，提高了在复杂场景中路径规划的成功率，同时判断释放锁定关节后的臂架的关节锁定形式是否存在于备用解中，从而避免重复搜索同样锁定形式的运动路径，提高了搜索效率。

以上结合附图详细描述了本发明的可选实施方式，但是，本发明实施方式并不限于上述实施方式中的具体细节，在本发明实施方式的技术构思范围内，可以对本发明实施方式的技术方案进行多种简单变型，这些简单变型均属于本发明实施方式的保护范围。

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明实施方式对各种可能的组合方式不再另行说明。

此外，本发明的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施方式的思想，同样应当视为本发明实施方式所公开的内容。

Claims

1.一种冗余自由度机械臂的路径规划方法，其特征在于，包括：

确定机械臂的执行末端所要达到的目标位姿；

2.根据权利要求1所述的路径规划方法，其特征在于，确定机械臂的所有关节中当该机械臂的执行末端位于所述目标位姿时能够保持初始角度的关节，包括：

3.根据权利要求2所述的路径规划方法，其特征在于，在锁定机械臂的当前关节的目标角度为初始角度，并针对所述目标位姿进行逆运动学求解之前，该方法还包括：

4.根据权利要求2所述的路径规划方法，其特征在于，基于已确定的所述关节确定所述机械臂的执行末端达到所述目标位姿的路径数据，包括：

5.根据权利要求4所述的路径规划方法，其特征在于，基于求解得到的所述初始路径数据对机械臂进行碰撞检测，包括：

6.根据权利要求2所述的路径规划方法，其特征在于，直至完成对机械臂的所有关节中当该机械臂的执行末端位于所述目标位姿时能够保持初始角度的关节的确定，包括：

7.根据权利要求4所述的路径规划方法，其特征在于，依据所述路径数据确定所述机械臂的运动路径，将所述运动路径中满足预设规则的运动路径作为机械臂的最终运动路径，包括：

8.根据权利要求7所述的路径规划方法，其特征在于，所述预设规则包括：

计算所有运动路径的路径距离；

以路径距离最短的运动路径作为机械臂的最终运动路径。

9.根据权利要求7所述的路径规划方法，其特征在于，所述方法还包括：

10.一种冗余自由度机械臂的路径规划装置，其特征在于，包括：

11.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1～9中任一项权利要求所述的冗余自由度机械臂的路径规划方法。

12.一种计算机可读介质，所述计算机可读介质存储有计算机程序，其特征在于，所述计算机程序被处理执行时实现权利要求1～9中任一项权利要求所述的冗余自由度机械臂的路径规划方法。

13.一种工程机械，其特征在于，包括：权利要求10所述的冗余自由度机械臂的路径规划装置。