一种多自由度机械臂的逆运动学通用求解方法及装置与流程

1.本发明涉及一种机器人逆运动学通用求解方法，具体来说是一种多自由度机械臂的逆运动学通用求解方法及装置。


背景技术：

2.随着机器人行业的发展，机械臂作为机器人行业至关重要的一部分，在未来将占据越来越重要的作用；机械臂的运动逆解，是当机械臂末端处于某一点及某一姿态，求出机械臂每个自由度旋转角度的一种方法；机械臂的运动学逆解是机器人学中最基础也是最重要的一部分，它直接关系到机器人的运动控制、路径规划等一系列问题。目前传统机器人行业中广泛应用的机械臂逆解方法是以d-h参数法为基础的解析法，该方法实时性较高，但是其计算过程相当复杂；解析法与机械臂的自由度数量和构型关系较大，如研发过程中改变结构构型或增减自由度数量，则需重新进行计算。


技术实现要素：

3.为了克服目前已有的逆解方法复杂性、通用性等不足，本发明提供了一种适用于多自由度机械臂的通用逆运动学通用求解方法。
4.本发明所采用的技术方案是：提供一种多自由度机械臂的逆运动学通用求解方法，其特征在于：包括以下步骤：1）将所述机械臂各关节自由度当前角度作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵及运动至目标位置的运动旋量；2）判断所述运动旋量是否小于等于误差，如果是，此时的关节自由度角度即为所述机械臂的逆运动学解，计算终止；如果否，则进入步骤3）;3）计算所述机械臂各关节的下一迭代自由度角度，将新各自由度角度带入步骤1）和2），进行迭代计算，直到其满足计算终止条件。
5.进一步地，所述步骤1）还包括以下步骤：1.1）利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵，构建末端正解的表达公式。
6.进一步地，所述步骤1.1）还包括以下步骤：1.1.1） 利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵；自由度机械臂的单位旋转轴矩阵为：
其中为自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，为自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示；1.1.2） 通过指数积法表示空间构型，机械臂末端正解公式为：其中为机械臂初始状态下末端的齐次变换矩阵。
7.进一步地，所述步骤1）还包括以下步骤：1.2） 令为机械臂各关节自由度当前角度，通过上述步骤1.1.2）得到机械臂末端当前状态下的齐次变换矩阵。
8.进一步地，所述步骤1）还包括以下步骤：1.3）输入机械臂末端目标状态下的齐次变换矩阵，计算机械臂从至的运动旋量；将机械臂正解对时间求导得到为：将机械臂正解求逆得到为：其运动旋量为：其中为机械臂第个自由度的单位旋转矩阵, 为第次迭代的第个自由度的旋转角度, 为第次迭代的第个自由度的旋转速度。
9.进一步地，所述步骤3）还包括以下步骤：3.1） 计算机械臂各自由度的空间雅可比矩阵，表示为关于的函数： 3.2） 其空间旋量表示为：，
单位时间内，各自由度角度的变化表示为：其中为其机械臂空间雅可比的伪逆；3.3）下一迭代自由度角度表示为：将新的各自由度角度代入步骤2）中进行迭代计算。
10.本发明还提供了一种多自由度机械臂的逆运动学通用求解装置，包括运动计算单元、判断单元和迭代计算单元，其特征在于：所述运动计算单元，用于将所述机械臂各关节自由度当前角度作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵及运动至目标位置的运动旋量；所述判断单元，用于判断所述运动旋量是否小于等于误差，是否计算终止；所述迭代计算单元，用于计算所述机械臂各关节的下一迭代自由度角度，将新各自由度角度带入所述运动计算单元进行迭代计算，直到满足计算终止条件。
11.进一步地，所述运动计算单元还用于：利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵，构建末端正解的表达公式。
12.进一步地，所述利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵为：其中为自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，为自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示；所述通过指数积法表示空间构型，机械臂末端正解公式为：其中为机械臂初始状态下末端的齐次变换矩阵。
13.进一步地，所述运动计算单元还用于：令为机械臂各关节自由度当前角度，通过所述机械臂末端正解公式得到机械臂末端当前状态下的齐次变换矩阵。
14.进一步地，所述运动计算单元还用于：接收输入机械臂末端目标状态下的齐次变换矩阵，计算机械臂从至的运动旋量；将机械臂正解对时间求导得到为：
将机械臂正解求逆得到为：其运动旋量为：，其中为机械臂第个自由度的单位旋转矩阵, 为第次迭代的第个自由度的旋转角度, 为第次迭代的第个自由度的旋转速度。
15.进一步地，所述迭代计算单元用于： 计算机械臂各自由度的空间雅可比矩阵，表示为关于的函数：其空间旋量表示为：，单位时间内，各自由度角度的变化表示为：其中为其机械臂空间雅可比的伪逆；下一迭代自由度角度表示为：将新的各自由度角度代入所述运动计算单元中进行迭代计算。
16.本发明还提供一种电子设备，包括存储器及处理器，所述存储器中储存有计算机程序，其特征在于，所述计算机程序被所述处理器执行时，使得所述处理器执行上述的多自由度机械臂的逆运动学通用求解方法的步骤。
17.本发明还提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述的多自由度机械臂的逆运动学通用求解方法的步骤。
18.本发明还提供一种机械臂控制装置，其特征在于：包括一个或多个处理器；存储器；以及一个或多个计算机程序，其中所述一个或多个计算机程序被存储在所述存储器中，所述一个或多个计算机程序包括指令，当所述指令被所述装置执行时，使得所述装置执行上述求解方法；机械臂，用于受控于所述控制装置，以执行运动操作。
19.本发明还提供一种机器人，其特征在于，包括：机械臂和如上所述的机械控制装置。
20.本发明的有益效果是：本方法巧妙利用了数值求解法对机械臂各自由度旋转角度进行迭代，求得机械臂末端当前位置至目标位置的运动旋量，利用空间雅可比矩阵求得个自由度角度增量，从而求得新的迭代点，通过计算并判断机械臂末端新位置至目标位置的
运动旋量，不断地更新迭代点，直到满足迭代终止条件。本方法计算量小，收敛速度快，精度高，并不受机械臂构型与自由度数的影响，可通用于多自由度的机械臂中。
21.本方法的运动学模型利用了李群李代数的概念对末端齐次变换矩阵进行定义，利用指数积方法构建多自由度机械臂空间构型，不同于传统的机械臂d-h参数法的多坐标系建立，该方法只需要构建初始坐标系与目标坐标系，利用旋量的概念描述刚体的几何运动，使机械臂运动学计算更系统化。
附图说明
22.图1：计算机械臂的逆运动学通用求解方法流程图；图2：六自由度机械臂初始状态示意图；图3： 六自由度机械臂运动旋量示意图。
23.图4：八自由度机械臂运动旋量示意图。
具体实施方式
24.现结合实施例、附图对本发明进行进一步描述：图1示出本发明计算机械臂的逆运动学通用求解方法流程图。如图1所示，该求解方法包括以下步骤： 1）将所述机械臂各关节自由度当前角度作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵及运动至目标位置的运动旋量；2）判断所述运动旋量是否小于等于误差，如果是，此时的关节自由度角度即为所述机械臂的逆运动学解，计算终止；如果否，则进入步骤3）;3）计算所述机械臂各关节的下一迭代自由度角度，将新各自由度角度带入步骤1）和2），进行迭代计算，直到其满足计算终止条件。
25.以图2所示六自由机械臂为例，其单位旋转矩阵为：其中为六自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，为六自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示。
26.本发明中角度可表示为第次迭代运算中第个自由度的角度。
27.如图3所示，为机械臂当前状态下各关节角度，为机械臂末端当前状态下的齐次变换矩阵，为机械臂末端目标状态下的齐次变换矩阵，为机械臂从至的运动旋量。
28.通过指数积法表示空间构型，坐标原点至第一个自由度的齐次变换矩阵表示
为：其中为即为指数积坐标，表示为：其中为的李代数。
29.同理可得机械臂各自由度间的齐次变换矩阵，即可知机械臂第次迭代过程中末端正解表示为：其中为机械臂初始状态下末端的齐次变换矩阵，为机械臂第个自由度的单位旋转矩阵, 为第个自由度的旋转角度。
30.其机械臂的运动旋量表示为：将机械臂正解对时间求导得到为：将机械臂正解求逆得到为：其运动旋量为：其中为机械臂第个自由度的单位旋转矩阵, 为第次迭代的第个自由度的旋转角度, 为第次迭代的第个自由度的旋转速度。
31.根据以上公式可得，当时，其运动旋量与各关节角度变换的关系：其中定义为各个自由度的空间雅可比矩阵，表示为关于的函数：通过上述计算，旋量可表示为：
单位时间内，各自由度角度的变化表示为：其中为其机械臂空间雅可比的伪逆。
32.此时下一组迭代法角度表示为：表示为：为机械臂末端当前状态下的齐次变换矩阵，为机械臂末端目标状态下的齐次变换矩阵，为机械臂从至的运动旋量。
33.当运动旋量小于等于可接受误差时停止迭代，此时角度组即为末端其次变化矩阵的逆解。
34.如时，继续进行迭代至，此时角度组即为末端其次变化矩阵的逆解。
35.该方法可通用于市面上其他六自由度机械臂。
36.以图3为例，当所示六自由度机械臂每个自由度角度旋转时，其机械臂末端齐次变换矩阵（正解）为：通过改变初始角度组的值，当机械臂末端齐次变换矩阵为上式的情况下，可接受误差为0.001时，其全部逆解可求为：将求得所有逆解结果代入正解公式验证，全部逆解都是正确的。
37.第7组的角度为输入角度组，即每个自由度角度为。
38.求得目标齐次变换矩阵与初始齐次变换矩阵变化最小的一组逆解为第7组：
即六自由度机械臂逆解可以求出。
39.对于更多自由度的冗余机械臂，该方法同样有效。
40.以图4为例，当所示八自由度机械臂每个自由度角度旋转时，其机械臂末端齐次变换矩阵（正解）为：通过选择50组初始角度组的值，当机械臂末端齐次变换矩阵为上式的情况下，可接受误差为0.001时，共解得45组逆解，其中前10组逆解为：将求得所有逆解结果代入正解公式验证，全部逆解都是正确的。
41.第27组的角度为输入角度组，即每个自由度角度为。
42.求得目标齐次变换矩阵与初始齐次变换矩阵变化最小的一组逆解为第27组：自由度1自由度2自由度3自由度4自由度5自由度6自由度7自由度80.52360.52360.52360.52360.52360.52360.52360.5236即逆解可以求出。
43.本发明还提供了一种多自由度机械臂的逆运动学通用求解装置，包括运动计算单元、判断单元和迭代计算单元，其特征在于：所述运动计算单元，用于将所述机械臂各关节自由度当前角度作为初始迭代角
度，计算所述机械臂当前状态下的末端齐次变换矩阵及运动至目标位置的运动旋量；所述判断单元，用于判断所述运动旋量是否小于等于误差，是否计算终止；所述迭代计算单元，用于计算所述机械臂各关节的下一迭代自由度角度，将新各自由度角度带入所述运动计算单元进行迭代计算，直到满足计算终止条件。
44.各个单元的功能与上述逆解计算方法相对应，此不赘述。
45.本发明还提供一种电子设备，包括存储器及处理器，所述存储器中储存有计算机程序，其特征在于，所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求1至6中任一项所述的多自由度机械臂的逆运动学通用求解方法的步骤。
46.本发明还提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6中任一项所述的多自由度机械臂的逆运动学通用求解方法的步骤。
47.本发明还提供一种机械臂控制装置，其特征在于：包括一个或多个处理器；存储器；以及一个或多个计算机程序，其中所述一个或多个计算机程序被存储在所述存储器中，所述一个或多个计算机程序包括指令，当所述指令被所述装置执行时，使得所述装置执行如权利要求1至6任一项所述的方法；机械臂，用于受控于所述控制装置，以执行运动操作。
48.本发明还提供一种机器人，其特征在于，包括：机械臂和如权利要求15所述的机械控制装置。
49.本领域普通技术人员可以意识到，本文中公开的实施例中描述的各单元及算法步骤，能够以电子硬件、计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本技术的范围。
50.所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
51.在本技术所提供的几个实施例中，任一功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本技术的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本技术各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(read-only memory；以下简称：rom)、随机存取存储器(random access memory；以下简称：ram)、磁碟或者光盘等各种可以存储程序代码的介质。
52.以上所述，仅为本技术的具体实施方式，任何熟悉本技术领域的技术人员在本技术揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本技术的保护范围之内。本技术的保护范围应以所述权利要求的保护范围为准。

技术特征：
1.一种多自由度机械臂的逆运动学通用求解方法，其特征在于：包括以下步骤： 1）将所述机械臂各关节自由度当前角度作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵及运动至目标位置的运动旋量；2）判断所述运动旋量是否小于等于误差，如果是，此时的关节自由度角度即为所述机械臂的逆运动学解，计算终止；如果否，则进入步骤3）;3）计算所述机械臂各关节的下一迭代自由度角度，将新各自由度角度带入步骤1）和2），进行迭代计算，直到其满足计算终止条件。2.根据权利要求1所述的一种多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤1）还包括以下步骤：1.1）利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵，构建末端正解的表达公式。3.根据权利要求2所述的多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤1.1）还包括以下步骤：1.1.1） 利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵；自由度机械臂的单位旋转轴矩阵为：其中为自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，为自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示；1.1.2） 通过指数积法表示空间构型，机械臂末端正解公式为：其中为机械臂初始状态下末端的齐次变换矩阵，其中为机械臂第个自由度的单位旋转矩阵, 为第个自由度的旋转角度。4.根据权利要求3所述的多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤1）还包括以下步骤：1.2） 令为机械臂各关节自由度当前角度，通过上述步骤1.1.2）得到机械臂末端当前状态下的齐次变换矩阵。5.根据权利要求4所述的多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤1）还包括以下步骤：1.3）输入机械臂末端目标状态下的齐次变换矩阵，计算机械臂从至的运动旋量
；将机械臂正解对时间求导得到为：将机械臂正解求逆得到为：其运动旋量为：其中为机械臂第个自由度的单位旋转矩阵, 为第次迭代的第个自由度的旋转角度, 为第次迭代的第个自由度的旋转速度。6.根据权利要求5所述的多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤3）还包括以下步骤：3.1） 计算机械臂各自由度的空间雅可比矩阵，表示为关于的函数：3.2） 其空间旋量表示为：，单位时间内，各自由度角度的变化表示为：其中为其机械臂空间雅可比的伪逆；3.3）下一迭代自由度角度表示为：将新的各自由度角度代入步骤2）中进行迭代计算。7.一种多自由度机械臂的逆运动学通用求解装置，包括运动计算单元、判断单元和迭代计算单元，其特征在于：所述运动计算单元，用于将所述机械臂各关节自由度当前角度作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵及运动至目标位置的运动旋量；
所述判断单元，用于判断所述运动旋量是否小于等于误差，是否计算终止；所述迭代计算单元，用于计算所述机械臂各关节的下一迭代自由度角度，将新各自由度角度带入所述运动计算单元进行迭代计算，直到满足计算终止条件。8.一种电子设备，包括存储器及处理器，所述存储器中储存有计算机程序，其特征在于：所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求1至6中任一项所述的多自由度机械臂的逆运动学通用求解方法的步骤。9.一种机械臂控制装置，其特征在于：包括一个或多个处理器；存储器；以及一个或多个计算机程序，其中所述一个或多个计算机程序被存储在所述存储器中，所述一个或多个计算机程序包括指令，当所述指令被所述装置执行时，使得所述装置执行如权利要求1至6任一项所述的方法；机械臂，用于受控于所述控制装置，以执行运动操作。10.一种机器人，其特征在于，包括：机械臂和如权利要求15所述的机械臂控制装置。

技术总结
本发明涉及一种多自由度机械臂的逆运动学通用求解方法及装置，使用指数积方法建立机械臂末端当前及目标状态下的齐次变换矩阵；定义机械臂当前各自由度角度；使用牛顿-拉弗森方法，通过比较机械臂末端运动旋量对机械臂各自由度角度进行迭代，从而求出机械臂的运动学逆解；本方法空间构型简洁、末端控制精度高、有利于动力学模型建立，实现了多自由度机械臂的逆运动学通用求解。逆运动学通用求解。逆运动学通用求解。


技术研发人员：何川甫 陈万楷 吴易谦 江微杰 刘彦禹 童伟宇 郑淳馨
受保护的技术使用者：杭州锐沃机器人科技有限公司
技术研发日：2021.12.14
技术公布日：2022/3/8