一种高自由度机器人逆运动学的快速求解方法及系统与流程

本发明涉及机器人控制技术领域，特别涉及一种高自由度机器人逆运动学的快速求解方法及系统。

背景技术：

机器人技术不仅可以应用于工业生产，而且可以服务于民众生活，是一项很有应用前景的技术。机器人一般是由很多关节组成，通过控制每个关节变量，达到位姿变化的功能，例如移动、行走和抓取等等。在机器人学中，每个关节各提供一个自由度。通常来讲，机器人的自由度越多(关节越多)，机器人功能就越强大，移动就越灵活。机器人运动学是机器人运动控制的基础，包含正运动学和逆运动学。正运动学，即给定每个关节变量θ，求解机器人的位姿P；逆运动学，即给定机器人的位姿P，求解机器人各个关节的关节变量θ，如图1所示。正运动学可以通过运动学方程求解，求解过程相对简单，相反，逆运动学求解复杂、耗时，对于高自由度机器人情况更糟。目前，机器人逆运动学求解主要采用：解析法、数值法和机器学习法。

解析法，通过构建逆运动学方程，可以很容易求解逆运动学问题。但是，对于任意机器人来讲，构建逆运动学方程非常复杂，并且很多情况是不存在逆运动学方程的。因此，解析法只能应用特定的机器人或机械臂中，并且机器人或机械臂的自由度只能很少。

数值法，一般需要通过多次迭代，寻找一个满足一定精度要求的近似解。其中，运用最多的是基于雅克比的方法，相比其他数值方法，该方法更加精确、稳定。基于雅克比的求解方法又包含两类：雅克比伪逆法和雅克比转置法。雅克比伪逆法收敛快，但是需要进行奇异值分解运算，复杂耗时，难并行；相反，雅克比转置法需要很多次迭代，但是，每次迭代运算简单，快速。

机器学习法，利用机器学习的方法对逆运动学方程进行近似，从而在有限时间内得到近似解。但是，该方法最大的问题是近似解与精确解的偏差较大，远大于数值法得到的近似解。同时，该方法需要大量数据进行训练。

目前，最常用的逆运动学求解方法是基于雅克比的求解方法，但是对高自由度机器人来讲，现有的基于雅克比的方法非常耗时，不能满足机器人控制的实时性要求，为此，本发明提出一种适用于并行体系结构中执行的快速求解高自由度机器人逆运动学问题的方法。

原始雅克比转置法在每次迭代过程中，首先生成一个参数值α，然后依据该参数更新关节变量θ，见附图2，发明人发现该参考值α的选取对于求解速度有严重影响，为此提出一种并行投机的选取方法。该方法在每次迭代时，生成多个参数值(投机值)α1,α2,...αm，依据这些不同的参数值可以得到多个关节变量更新值，然后从中选出最接近目标解的参数值及关节变量更新值，由于多个参数值的计算及后续的关节变量更新值计算之间没有依赖，可以通过并行结构同时执行，从而加速求解速度。

技术实现要素：

针对现有技术的不足，本发明提出一种高自由度机器人逆运动学的快速求解方法及系统。

本发明提出一种高自由度机器人逆运动学的快速求解方法，包括：

步骤1，将关节变量θ带入机器人运动学方程中，获得雅克比矩阵J，将所述雅克比矩阵J进行转置，获得雅克比转置矩阵J^T；

步骤2，生成一组投机值，为每个投机值计算相应的关节变量更新值，将每个关节变量更新值带入机器人正运动学方程中，获得相应的位姿P_k，为每个位姿P_k计算其与目标位姿P的位姿偏差Δe_k，及位姿偏差Δe_k的模error_k；

步骤3，在模error_k的集合中选取最小值error_min，及其对应的位姿偏差Δe_min与关节变量更新值Δθ_min，并更新位姿偏差为Δe＝Δe_min，更新关节变量θ＝θ+Δθ_min；

步骤4，判断error_min是否满足error_min<Threshold，其中Threshold为预设的error_min阈值，如果是，则输出关节变量θ并结束，否则，返至所述步骤1，继续执行。

所述步骤1之前还包括随机生成一组初始值θ_init，并且令θ＝θ_init；

将关节变量θ带入机器人正运动学方程中，获得相应的位姿P_init；

计算位姿P_init与目标位姿P的位姿偏差Δe＝P-P_k，及位姿偏差Δe的模error；

判断模error是否满足error<Threshold，如果是，则输出关节变量θ并结束，否则，执行所述步骤1。

所述步骤2中每个投机值大于0，小于1。

所述步骤2中为每个投机值计算相应的关节变量更新值的公式为：

Δθ_k＝α_kJ^TΔe

其中Δθ_k为关节变量更新值，α_k为投机值，J^T为雅克比转置矩阵，Δe为位姿偏差。

所述开启m个计算线程，每个线程生成一个投机值，其中第k个线程生成投机值α_k，计算公式为：

其中α_k为投机值，Δe为位姿偏差，J^T为雅克比转置矩阵，雅克比矩阵J。

本发明还提出一种高自由度机器人逆运动学的快速求解系统，包括：

获得雅克比转置矩阵模块，用于将关节变量θ带入机器人运动学方程中，获得雅克比矩阵J，将所述雅克比矩阵J进行转置，获得雅克比转置矩阵J^T；

获得位姿偏差模块，用于生成一组投机值，为每个投机值计算相应的关节变量更新值，将每个关节变量更新值带入机器人正运动学方程中，获得相应的位姿P_k，为每个位姿P_k计算其与目标位姿P的位姿偏差Δe_k，及位姿偏差Δe_k的模error_k；

更新关节变量模块，用于在模error_k的集合中选取最小值error_min，及其对应的位姿偏差Δe_min与关节变量更新值Δθ_min，并更新位姿偏差为Δe＝Δe_min，更新关节变量θ＝θ+Δθ_min；

判断模块，用于判断error_min是否满足error_min<Threshold，其中Threshold为预设的error_min阈值，如果是，则输出关节变量θ并结束，否则，返至所述获得雅克比转置矩阵模块，继续执行。

所述获得雅克比转置矩阵模块之前还包括随机生成一组初始值θ_init，并且令θ＝θ_init；

将关节变量θ带入机器人正运动学方程中，获得相应的位姿P_init；

计算位姿P_init与目标位姿P的位姿偏差Δe＝P-P_k，及位姿偏差Δe的模error；

判断模error是否满足error<Threshold，如果是，则输出关节变量θ并结束，否则，执行所述步骤1。

所述获得位姿偏差模块中每个投机值大于0，小于1。

所述获得位姿偏差模块中为每个投机值计算相应的关节变量更新值的公式为：

Δθ_k＝α_kJ^TΔe

其中Δθ_k为关节变量更新值，α_k为投机值，J^T为雅克比转置矩阵，Δe为位姿偏差。

所述开启m个计算线程，每个线程生成一个投机值，其中第k个线程生成投机值α_k，计算公式为：

其中α_k为投机值，Δe为位姿偏差，J^T为雅克比转置矩阵，雅克比矩阵J。由以上方案可知，本发明的优点在于：

本发明易并行化：本发明提出的快速求解方法改进了雅克比转置法，使其适合在并行体系结构中执行，例如多核处理器、图像处理器等；

高实时性：通过在并行体系结构中执行该算法，可以有效加速逆运动学求解，从而使得该方法可以在很短时间内得到满意结果。

附图说明

图1是逆运动学求解示意图；

图2是原始雅克比转置法流程图；

图3是逆运动学的快速求解方法流程图；

图4是高自由度机器人示意图。

具体实施方式

以下为本发明的整体流程，如图3所示，本发明方法包括：

步骤一，随机生成一组初始值θ_init，并且令θ＝θ_init；

步骤二，将关节变量θ带入机器人正运动学方程中，求出相应的位姿P_init＝f(θ)；

步骤三，计算位姿P_init与目标位姿P的偏差Δe＝P-P_k，及偏差Δe的模error；

步骤四，判断error是否满足精度要求，即error<Threshold，如果是，输出关节变量θ并结束，否则，继续执行步骤五。

步骤五，将关节变量θ带入机器人运动学方程中，求出雅克比矩阵J；

步骤六，将雅克比矩阵转置，获得雅克比转置矩阵J^T；

步骤七，生成一组投机值α₁,α₂,α₃,...α_m，且每个投机值大于0，小于1；

步骤八，为每个投机值α_k，计算相应的关节变量更新值Δθ_k＝α_kJ^TΔe；

步骤九，将每个关节变量更新值Δθ_k带入机器人正运动学方程中，求出相应的位姿P_k＝f(θ+Δθ_k)；

步骤十，为每个位姿P_k计算其与目标位姿P的位姿偏差Δe_k＝P-P_k，及位姿偏差Δe_k的模error_k；

步骤十一，在error₁,error₂,...error_m中选取最小值error_min，及其对应的位姿偏差Δe_min和关节变量更新值Δθ_min，并更新位姿偏差Δe＝Δe_min，更新关节变量θ＝θ+Δθ_min；

步骤十二，判断error_min是否满足精度要求，即error_min<Threshold，如果是，输出关节变量θ并结束，否则，返至步骤五，继续执行。

以下为本发明的一种高效的并行投机机制，附图3所示：

步骤一，开启m个计算线程；

步骤二，每个线程生成一个投机值，其中第k个线程生成投机值α_k，计算方式如下：

步骤三，每个线程根据自身生成的投机值α_k，进行位姿计算P_k＝f(θ+Δθ_k)和位姿偏差计算Δe_k＝P-P_k；

步骤四，收集所有线程计算所得的位姿偏差Δe₁，Δe₂,…Δe_m，选取其中模最小的位姿偏差Δe_min及其对应的投机值α_min；

步骤五，更新关节变量θ＝θ+α_kJ^TΔe和位姿偏差Δe＝Δe_min。

下面将结合附图对本发明进行详细说明。

本发明的应用环境为高自由度机器人。在该机器人中具有很多关节，同时装备可以并行计算的处理器，例如多核处理器(多核CPU)、图像处理器(GPU)或者定制FPGA等。图4显示了一个具有20个关节(自由度)的机器人，同时配备一个图像处理器GPU用于执行逆运动学求解算法。

图4中，给定抓手的空间位置P(x_o,y_o,z_o)，然后求解各个关节的角度θ(θ₁,θ₂,...θ₂₀)。

步骤一，处理器GPU随机生成20个数值组成一组初始值θ_init，并且令θ＝θ_init；

步骤二，处理器GPU将关节变量θ带入机器人运动学方程中，求出相应的位姿P_init＝f(θ)；

步骤三，处理器GPU计算初始位姿P_init(x_init,y_init,z_init)与目标位姿P(x_o,y_o,z_o)的偏差Δe＝P-P_k＝(x_o-x_init,y_o-y_init,z_o-z_init)及偏差Δe的模error；

步骤四，判断error是否满足精度要求，即error<Threshold，如果是，处理器GPI输出关节变量θ并结束，否则，执行步骤五。

步骤五，将关节变量θ带入机器人运动学方程中，求出雅克比矩阵J；

步骤六，将雅克比矩阵转置，获得雅克比转置矩阵J^T；

步骤七，生成m个投机值α₁,α₂,α₃,...α_m，且每个投机值大于0，小于1；

步骤八，处理器GPU为每个投机值α_k分配一个计算线程，每个线程负责计算相应的关节变量更新值Δθ_k＝α_kJ^TΔe；

步骤九，将每个关节变量更新值Δθ_k带入机器人正运动学方程中，求出相应的位姿P_k＝f(θ+Δθ_k)；

步骤十，为每个位姿P_k计算其与目标位姿P的位姿偏差Δe_k＝P-P_k，及位姿偏差Δe_k的模error_k；

步骤十一，在error₁,error₂,...error_m中选取最小值error_min，及其对应的位姿偏差Δe_min和关节变量更新值Δθ_min，并更新位姿偏差Δe＝Δe_min，更新关节变量θ＝θ+Δθ_min；

步骤十二，判断error_min是否满足精度要求，即error_min<Threshold，如果是，输出关节变量θ并结束，否则，返至步骤五，继续执行。

本发明还提出一种高自由度机器人逆运动学的快速求解系统，包括：

获得雅克比转置矩阵模块，用于将关节变量θ带入机器人运动学方程中，获得雅克比矩阵J，将所述雅克比矩阵J进行转置，获得雅克比转置矩阵J^T；

获得位姿偏差模块，用于生成一组投机值，为每个投机值计算相应的关节变量更新值，将每个关节变量更新值带入机器人正运动学方程中，获得相应的位姿P_k，为每个位姿P_k计算其与目标位姿P的位姿偏差Δe_k，及位姿偏差Δe_k的模error_k；

更新关节变量模块，用于在模error_k的集合中选取最小值error_min，及其对应的位姿偏差Δe_min与关节变量更新值Δθ_min，并更新位姿偏差为Δe＝Δe_min，更新关节变量θ＝θ+Δθ_min；

判断模块，用于判断error_min是否满足error_min<Threshold(其中Threshold为预设的error_min阈值，根据设计要求可以为0.1,0.05等数值)，如果是，则输出关节变量θ并结束，否则，返至所述获得雅克比转置矩阵模块，继续执行。

所述获得雅克比转置矩阵模块之前还包括随机生成一组初始值θ_init，并且令θ＝θ_init；

将关节变量θ带入机器人正运动学方程中，获得相应的位姿P_init；

计算位姿P_init与目标位姿P的位姿偏差Δe＝P-P_k，及位姿偏差Δe的模error；

判断模error是否满足error<Threshold，如果是，则输出关节变量θ并结束，否则，执行所述步骤1。

所述获得位姿偏差模块中每个投机值大于0，小于1。

所述获得位姿偏差模块中为每个投机值计算相应的关节变量更新值的公式为：

Δθ_k＝α_kJ^TΔe

其中Δθ_k为关节变量更新值，α_k为投机值，J^T为雅克比转置矩阵，Δe为位姿偏差。

所述开启m个计算线程，每个线程生成一个投机值，其中第k个线程生成投机值α_k，计算公式为：

其中α_k为投机值，Δe为位姿偏差，J^T为雅克比转置矩阵，雅克比矩阵J。