机器人的逆运动学求解系统的制作方法

本发明涉及计算机技术领域，尤其涉及一种多自由度机器人的逆运动学求解系统。

背景技术：

随着传感器、人工智能、物联网以及大数据技术的发展，各种高级的机器人被用于工业生产，或者代替人类作一些重复性高或危险的工作。灵活性、可操作性是度量机器人能力的重要标准，多关节、高自由度使机器人的功能更强大，可以完成更多的任务。逆向运动学在求解高自由度机器人的控制参数中扮演着重要的角色，是机器人领域众多学者的研究方向。然而，由于逆向运动学求解困难、计算速度慢等因素，严重限制了机器人处理问题的实时能力，此外，逆向运动学的计算消耗大量的能量，这对于电池电量有限的移动式机器人而言，续航是一个巨大的挑战。

在现有技术中，机器人的逆运动学求解通常采用cpu和gpu来计算，然而对于高自由度机器人，cpu计算非常耗时，不能满足机器人控制的实时性要求；同时gpu功耗大，且设备便携度低，不能满足移动式机器人的功耗要求。

技术实现要素：

本发明的目的在于克服上述现有技术的缺陷，提供一种并行度高，功耗低，高自由度的机器人逆向运动学加速器。

根据本发明的一个方面，提供了一种机器人的逆运动学求解系统，用于基于输入的机器人的目标位姿值和自由度获得与所述目标位姿值对应的关节角度值，该系统包括：

参数初始化模块，用于获得参数初始值，所述参数初始值包括关节角度初始值、位姿初始值以及所述位姿初始值相对于所述目标位姿值的误差初始值；

逆运动学调度器，用于基于所述目标位姿值执行判断，以决定是将当前的关节角度值选择作为与所述目标位姿值对应的关节角度值还是将当前的关节角度值发送给雅克比计算单元，其中，所述当前的关节角度值是来自于参数初始化模块的关节角度初始值或来自于参数选择器的关节角度值；

雅克比计算单元，用于基于接收的所述关节角度值获得雅克比矩阵及雅克比转置矩阵；

位姿更新单元，用于基于所述雅克比矩阵和雅克比转置矩阵获得多个新的关节角度值；

参数选择器，用于从所述多个新的关节角度值中选择一个并发送给逆运动学调度器。

在本发明的系统中，所述参数初始化模块包括：用于生成关节角度初始值的随机信号发生器；用于获得位姿初始值的三角平方计算器和矩阵乘法器；用于获得误差初始值的加法器和三角平方计算器。

在本发明的系统中，所述逆运动学调度器通过将当前的关节角度值所对应的误差与预定阈值进行比较来决定是将当前的关节角度值选择作为与所述目标位姿值对应的关节角度值还是将当前的关节角度值发送给雅克比计算单元，其中，当前的关节角度值所对应的误差是指当前的关节角度值所对应的位姿值与所述目标位姿值之间的误差。

在本发明的系统中，所述雅克比计算单元包括用于获得雅克比矩阵及雅克比转置矩阵的三角平方计算器和矩阵乘法器。

在本发明的系统中，所述位姿更新单元包括：参数计算器，用于计算前向参数；变量更新模块，用于计算关节角度变量的增量；寄存器，用于保存关节角度变量的增量；位姿计算模块，用于根据关节角度变量的增量计算新的位姿值；误差计算器，用于计算新的位姿值和所述目标位姿值的之间的误差；控制器，用于控制变量更新模块、寄存器、位姿计算模块、误差计算器的执行。

在本发明的系统中，所述参数计算器采用矩阵乘法器来计算所述前向参数。

在本发明的系统中，所述矩阵乘法器是4*4矩阵乘法器。

在本发明的系统中，所述位姿计算模块采用加法器和三角平方计算器来计算新的位姿。

在本发明的系统中，所述参数选择器包括：用于比较多个关节角度值的比较器；用于选择一个关节角度值的数据选择器。

在本发明的系统中，所述位姿更新单元是至少两个，用于执行并行计算。

本发明以多关节、高自由度机器人为研究对象，解决机器人逆运动学的求解问题，以为机器人提供控制其运动的参数。

相对于现有技术，本发明的优点是：以专用的硬件系统求解控制机器人运动的参数，降低了计算功耗；通过高度并行的系统结构，可有效地加速逆运动学求解，从而保证对机器人实时控制；可以计算任意自由度的逆运动学问题，不局限与单一自由度的机器人控制问题，通用性更强，适用范围更广。

附图说明

以下附图仅对本发明作示意性的说明和解释，并不用于限定本发明的范围，其中：

图1示出了多自由度机器人的示意图。

图2示出了根据本发明一个实施例的基于雅克比转置法的逆运动学求解的流程图。

图3示出了根据本发明一个实施例的多自由度的机器人的逆运动学求解系统的框图。

图4示出了根据本发明一个实施例的位姿更新单元的框图。

图5示出了根据本发明一个实施例的三角平方计算器和4*4矩阵乘法器的示意图。

图6示出了根据本发明一个实施例的雅克比矩阵计算示例。

具体实施方式

为了使本发明的目的、技术方案、设计方法及优点更加清楚明了，以下结合附图通过具体实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

本发明的系统可应用于多自由度机器人的机械臂控制，如图1所示的机器人的机械臂有8个关节(即8个自由度)，通过控制各个关节的转动能够控制机器人运动。针对机械臂的逆运动学求解问题，是指从机器人的抓手的目标位姿pl(xo,yo,zo)出发，求解出机器人与目标位姿对应的各关节的角度。这种关联关系通常在对机械臂进行运动学分析的基础上，建立机器人的抓手目标位姿和各关节的角度变化之间的关系方程，通过采用雅克比矩阵进行逆运动学求解。对于关系方程的建立和雅克比矩阵求解的详细过程属于现有技术，在此不再赘述。本文将重点描述采用asic硬件实现机器人逆运动学求解的系统。

本发明的目的在于当给定机器人的目标位姿pl(xo,yo,zo)时，根据本发明提供的系统获得对应的各个关节的关节角度θ(θ1,θ2,...θ8)(以8自由度为例)，通过将获得的关节角度输入给机器人，可灵活的控制机器人的运动，如图1中的控制系统即可采用本发明的系统。

为了便于理解本发明的硬件实现，图2示出了本发明中基于雅可比转置法的逆运动学求解过程。

步骤一、输入目标位姿pl(xo,yo,zo)，其中，x0,y0,z0表示在三个方向的坐标值；

步骤二、随机生成一组关节角度变量初始值θinit，并且令关节角度变量θ＝θinit，计算初始化位姿pinit，并且令位姿变量p＝pinit，计算初始化误差δeinit(即初始化位姿和目标位姿之间的误差)，并且令误差变量error＝δeinit；

步骤三、将误差变量error与预定阈值(threshold)比较，以判断是否满足精度要求，即error＜threshold；如果满足，则输出关节角度变量θ，否则，继续执行以下步骤；

步骤四、根据关节角度变量θ计算雅克比矩阵j及其雅克比转置矩阵j^t；

步骤五、根据j、j^t、error并行计算出前向参数α1,α2,...,αk，并求解出对应的关节增量δθ；

步骤六、根据δθ、θ计算出新的位姿p1,p2,...,pk以及与目标位姿pl的误差e1,e2,...,ek；

步骤七、从e1,e2,...,ek中选择出最小的误差以及其对应的关节角度变量θ，并且令误差变量error＝该最小的误差；

步骤八、判断error是否满足精度要求，即error＜threshold；如果满足，输出关节角度变量θ，否则返回步骤四，继续执行。

图3示出了根据本发明一个实施例的多自由度逆运动学求解系统的框图。该系统包括参数初始化模块、逆运动学调度器、雅克比计算单元、位姿更新单元和参数选择器。该系统的输入是目标位姿pl(xo,yo,zo)和自由度n，其中，自由度n可以是任意值。输出是与目标位姿对应的各关节角度值。

参数初始化模块：用于随机生成初始化参数，例如初始化关节角度变量θinit、计算初始化位姿pinit和δeinit等。参数初始化模块可由随机信号发生器实现。

逆运动学调度器：用于协调整个系统中运行任务的调度和控制，例如，判断当前误差和预定的误差阈值的大小，控制迭代次数等。逆运动学调度器可由有限状态机实现，包含比较器、数据选择器等。

雅克比计算单元(jcu)：用于计算关节角度变量对应的雅克比矩阵及其转置矩阵。雅克比计算单元可由三角平方计算器和4*4的矩阵乘法器实现。

位姿更新单元(puu)：用于计算机器人的机械臂的新的位姿、关节角度变量和以及对应的误差，可采用多个位姿更新单元并行计算，例如，32个。位姿更新单元包含参数计算器和正向运动学计算器。

参数选择器：用于从多个误差中选择最小的一个误差并返回给逆运动学调度器。参数选择器可由比较器或排序器实现。

此外，该系统还提供输入输出接口，各模块中包含有存储器或寄存器，以保存中间的计算结果或保存从其他模块接收的参数，模块之间的参数通过总线传输。

具体而言，本发明的系统执行的操作过程为：从系统外部接收目标位姿pl(xo,yo,zo)以及自由度n(在此实施例中n＝8)；参数初始化模块随机初始化关节角度，计算初始化位姿；雅克比计算单元计算关节角度变量对应的雅克比矩阵及其转置矩阵；逆运动学调度器根据自由度n控制三角平方或雅克比矩阵计算的迭代次数；位姿更新单元并行计算出新的位姿、关节角度变量以及对应的误差；同时逆运动学调度器根据误差error是否小于阈值threshold来控制位姿更新单元的迭代次数；参数选择器从多个误差中选择最小的一个误差，返回给逆运动学调度器。在此过程中，当逆运动学调度器判断为误差error小于阈值threshold，则停止迭代并控制输出关节角度θ(θ1,θ2,...θ8)。

为进一步理解本发明，图4示出了图3中的位姿更新单元的框图。该位姿更新单元(puu)包括：参数计算器，可由乘法器和加法器实现；正向运动学计算器(fku)。该正向运动学计算器包含控制器、变量更新模块、寄存器、位姿计算模块、误差计算器。

位姿更新单元接收来自于图3中的雅克比计算单元的雅克比矩阵j以及转置j^t、误差error、当前位姿p和目标位姿pl，内部并行生成不同的前向参数α1,α2,...,αk并并行计算新的位姿；输出为对应的k组关节角度变量θ1,θ2,...,θk和误差e1,e2,...,ek。参数计算器用于计算正向运动学计算器需要的前向参数α1,α2,...,αk；正向运动学计算器计算新的关节角度变量和位姿。其中，正向运动学计算器的变量更新模块计算关节角度变量的增量δθ，并保存到寄存器中；位姿计算模块根据增量δθ计算新的位姿，传递给误差计算器；误差计算器计算新的位姿与目标位姿的误差；控制器用于控制变量更新模块、寄存器、位姿计算模块、误差计算器的执行顺序。正向运动学计算器采用流水线操作，控制器控制变量更新模块计算增量δθ后保存到寄存器中，同时控制位姿计算模块计算新的位姿以及控制变量更新模块继续接收前向参数αk计算增量，控制器整个流水线起调度作用。

图5示出了根据本发明的一个实施例的雅克比计算单元的框图，其包括三角平方计算器和4*4矩阵乘法器。

图6示出了利用图5的雅克比计算单元计算雅克比矩阵的一个实例。第一步，根据关节角度θ1,θ2,...,θk,...,θn构造n个4*4的矩阵t[n]，使用三角平方计算器计算矩阵元素的三角函数，得到tr[n]；第二步，4*4矩阵乘法器接收矩阵tr[k]与tr[k-1]，进行矩阵相乘操作，得到trl[n]；第三步，trl[n]减去trl[k]，并通过矩阵乘法器与trl[k]相乘，得到最终的雅克比矩阵j[n]。计算过程并不代表一定同时执行n个三角函数计算或者矩阵乘法，也可以使用流水线技术串行计算雅克比矩阵。

综上所述，根据本发明的多自由度逆运动学求解系统的具体过程为：

步骤一，初始化模块使用随机信号发生器随机生成一组关节角度变量初始值θinit，同时使用三角平方计算器以及4*4矩阵乘法器计算初始化位姿pinit，利用加法器和三角平方计算器计算初始化误差error，将参数发给逆运动学调度器；

步骤二，逆运动学调度器接收参数error使用比较器判断error是否满足精度要求，即error＜threshold；如果满足，数据选择器选择输出电路以输出关节角度变量θ，否则数据选择器选择计算电路将关节角度变量θ发给雅克比计算单元，继续执行。

步骤三，雅克比计算单元接收关节角度变量θ，使用三角平方计算器和4*4矩阵乘法器计算雅克比矩阵j及其雅克比转置矩阵j^t，逆运动学调度器根据输入的自由度控制雅克比矩阵计算的迭代次数，计算完成后，将获得的雅克比矩阵j及其转置矩阵j^t发送到各个并行的位姿更新单元puu；

步骤四，位姿更新单元根据j，j^t，error使用4*4矩阵乘法器计算参数α1,α2,...,αk并传递给内部的正向运动学计算器fku；

步骤五，正向运动学计算器中的变量更新模块接收error、j^t，使用4*4矩阵乘法器计算关节增量δθ并保存到寄存器中；

步骤六，位姿计算模块接收寄存器中的δθ，使用加法器和三角平方计算器计算出新的位姿p；

步骤七，误差计算器使用加法器和三角平方计算器计算新的位姿和目标位姿的误差e1,e2,...,ek；整个位姿计算单元输出新的k组关节角度变量θ1,θ2,...,θk和误差e1,e2,...,ek；

步骤八，参数选择器使用比较器从e1,e2,...,ek中选择出最小的偏差error以及使用数据选择器选择对应的关节角度变量θ，并发送给逆运动学调度器；

步骤九，逆运动学调度器使用比较器判断error是否满足精度要求，即error＜threshold；如果满足，数据选择器选择输出电路输出关节角度变量θ，否则，数据选择器选择计算电路将关节角度变量θ发给雅克比计算单元，重复步骤三，继续执行。

本领域的技术人员应理解的是，本文涉及比较器、判断器、三角平方计算器等可以采用市售的标准器件，4*4矩阵乘法器也可以采用其他维度的矩阵乘法器。

以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。