CN112765879A - 基于神经网络和构型编码的机械臂逆运动学求解方法 - Google Patents

Abstract

本发明提供一种基于神经网络和构型编码的机械臂逆运动学求解方法，包括：坐标系与运动学参数定义；机械臂构型编码参数定义；正向运动学构建；逆运动学求解神经网络设计；训练样本库构建；神经网络训练与测试；逆运动学求解器的应用。(1)本发明提出一种基于前向神经网络和构型编码的多自由度机械臂逆运动学求解方法，较好的解决多自由度机械臂求解空间较大时的多解性对逆运动网络训练收敛性的干扰和局部最优的问题。(2)所设计的基于前向神经网络和构型编码的多自由度机械臂逆运动学求解方法对不同自由度数的情况具有较好的通用性，而不会随情况的自由度增加而增加算法的复杂度。

Description

基于神经网络和构型编码的机械臂逆运动学求解方法

技术领域

本发明属于机器人技术领域，具体涉及一种基于神经网络和构型编码的机械臂逆运动学求解方法。

背景技术

多自由度机械臂在空间探测、工业生产装配和生活服务领域应用广泛。尤其是多自由度机械臂能够在满足末端操作位置姿态约束的情况下实现障碍规避。

机械臂逆运动学求解是多自由度机械臂动作规划、控制和执行的基础。传统的空间多自由度机械臂逆运动学求解方法包括代数法、几何法等。但是随着机械臂自由度增多，求解方法变的非线性强，同时存在多解性，因此，求解复杂，多数呈现指数增大。人工神经网络具有强大的非线性拟合能力，基于BP神经网络的机械臂逆运动学求解方法通过采样训练建立操作空间位置到机器人关节角度的映射关系，相比于传统的代数法和解析法更加简单高效。

但是，逆运动多解性会造成网络训练收敛速度慢或局部最优的问题，通常需要对机械臂的操作空间划分为较小的区间，进而在每个区间训练独立的神经网络逆运动学求解器，这种思路需要对机器人的操作空间进行精确划分和分析，同时其对于计算能力和内存要求较高，应用不方便。

发明内容

针对现有技术存在的缺陷，本发明提供一种基于神经网络和构型编码的机械臂逆运动学求解方法，可有效解决上述问题。

本发明采用的技术方案如下：

本发明提供一种基于神经网络和构型编码的机械臂逆运动学求解方法，包括以下步骤：

步骤1，坐标系与运动学参数定义：

步骤1.1，对于被研究的多自由度机械臂，其结构如下：

假设其自由度数为N，N＞2，从多自由度机械臂的基座向机械臂末端，共有N个关节，各关节依次编号为关节1～N；从多自由度机械臂的基座向机械臂末端，共有N个连杆，各连杆依次编号为连杆1～N；

对于关节n，n∈[1,N]，当n＝1时，关节n通过连杆n与关节n+1相连；当n＝2,3,...,N-1时，关节n的一端通过连杆n-1与关节n-1相连，关节n的另一端通过连杆n与关节n+1相连；当n＝N时，关节n的一端通过连杆n-1与关节n-1相连，关节n的另一端通过连杆n与机械臂末端相连；

该多自由度机械臂，关节2～N的关节转轴互相平行，并且与关节1的关节转轴保持垂直；当n＝2,3,...,N时，连杆n-1与连杆n的延长线与关节n的关节转轴相交于一点；

步骤1.2，坐标系定义如下：

以关节1的关节转轴和连杆1的交点为原点O，沿关节1的关节转轴方向为Z轴，当关节1转角为0时，沿连杆1并指向离开关节1的方向为X轴，按照右手法则确定Y轴，从而建立XYZ坐标系；

步骤1.3，变量定义如下：

在XYZ坐标系中，机械臂末端的位置坐标为(x,y,z)^T；

对于连杆n，n∈[1,N]，定义连杆n的长度为l_n；

对于关节n，n∈[1,N]，定义关节n的角位移为α_n；其中，当n＝1时，关节n的角位移α_n为连杆n与X轴的夹角；当n＝2,3,...,N时，关节n的角位移α_n为连杆n-1与连杆n的夹角；

对于关节n，n∈[1,N]，定义关节n的空间角度为θ_n；θ_n∈[0,π]；当n＝1时，关节n的空间角度θ_n等于关节n的角位移α_n；当n＝2,3,...,N时，关节n的空间角度θ_n为Z轴与连杆n的夹角；

关节n的角位移α_n与关节n的空间角度θ_n的关系通过以下公式1表示：

步骤2，机械臂构型编码参数定义如下：

步骤2.1，对于具有N个关节的多自由度机械臂，其关节3～关节N中的每个关节均具有两种构型，分别是上扑构型和上凹构型，因此，当n＝3,4,…,N时，定义关节n的节点构型变量为η_n，因此，具有N个关节的多自由度机械臂，共有N-2个节点构型变量，分别为η₃～η_N；

节点构型变量η_n的取值取决于关节的构型，具体通过以下公式2确定：

步骤2.2，将η₃～η_N按顺序排列为一个二进制数

作为机械臂整体构型二进制编码；

将机械臂整体构型二进制编码转化为十进制数，并作为机械臂构型编码参数λ_config的取值，即，通过以下公式3确定机械臂构型编码参数λ_config：

对于具有N个关节的多自由度机械臂，其空间构型共有2^N-2种，因此，每种空间构型对应一个机械臂构型编码参数λ_config，一共有2^N-2个机械臂构型编码参数λ_config，即：λ_config＝0,1,2,…,2^N-2-1；

步骤3，正向运动学构建：

正向运动学构建，是指构建从空间角度θ_n到机械臂末端位置(x,y,z)^T和机械臂构型编码参数λ_config的等式关系，具体构建以下公式4所示的运动学方程：

步骤4，逆运动学求解神经网络设计：

构建4层前向神经网络，用于多自由度机械臂的逆运动学的数据建模；其中，4层前向神经网络共有四层，分别为第1层、第2层、第3层和第4层；第1层、第2层和第3层为隐含层；第4层为输出层；第1层、第2层、第3层和第4层的神经元数量分别为16，16，16，N；

其中，网络的输入为4个节点，分别为输入变量(x,y,z,λ_config)^T的归一化后的值(x_normal,y_normal,z_normal,λ_{config,normal})^T；其中，x_normal为x归一化处理后的值；y_normal为y归一化处理后的值；z_normal为z归一化处理后的值；λ_{config,normal}为λ_config归一化处理后的值；

网络的输出层为N个节点，分别输出空间角度(θ₁,θ₂,…,θ_N)^T的归一化后的值(θ_1,normal,θ_2,normal,…,θ_N,normal)^T；

步骤5，训练样本库构建：

在多自由度机械臂操作半球空间内进行随机均匀采样，建立(θ₁,θ₂,…,θ_N)^T到(x,y,z,λ_config)^T的样本组合，其中满足θ_n∈[0,π]，进而构建得到训练样本库；

步骤6，神经网络训练与测试：

基于步骤5构建的训练样本库，对步骤4设计的神经网络进行参数训练，得到训练好的神经网络；

步骤7，逆运动学求解器的应用：

步骤7.1，末端目标位置为(x″,y″,z″)，归一化后的末端目标位置为(x″_normal,y″_normal,z″_normal)；对于具有N个关节的多自由度机械臂，其空间构型共有2^N-2种，因此，归一化后的机械臂构型编码参数λ″_{config,normal}等于未归一化的机械臂构型编码参数λ″_config，即：λ″_{config,normal}＝λ″_config，并且，λ″_{config,normal}有2^N-2个取值；

对于每个空间构型，以归一化后的末端目标位置和机械臂构型编码参数(x″_normal,y″_normal,z″_normal,λ″_{config,normal})^T为输入，输出当前空间构型下归一化目标空间角度值

由于共有2^N-2种空间构型，因此，输出的各种空间构型下归一化目标空间角度值

共有2^N-2种组合；

将输出的归一化目标空间角度值反归一化处理，得到各种空间构型下目标空间角度的准确值

步骤7.2，采用以下方式，对2^N-2种目标空间角度的准确值

进行筛选：

以机械臂末端控制精度为目标函数，将目标函数标记为F，其定义为：

式中：(x″,y″,z″)表示初始已知的末端目标位置为(x″,y″,z″)；

表示基于逆运动学求解空间角计算出的末端位置；

因此，对于每种空间构型，均计算出目标函数F的值；通过以下方式确定最优机械臂构型编码参数λ_config,best为：

确定出使末端控制精度最高的最优机械臂构型编码参数λ_config,best和对应的最优的目标空间角度的准确值

后，采用下式求出最优构型条件下的机械臂各关节的最优角位移(α_1,best,α_2,best,…,α_N,best)^T，基于最优角位移(α_1,best,α_2,best,…,α_N,best)^T，对机械臂进行控制。

优选的，步骤4中，采用以下方法，对输入变量(x,y,z,λ_config)^T进行归一化处理，得到输入变量(x,y,z,λ_config)^T的归一化后的值(x_normal,y_normal,z_normal,λ_{config,normal})^T：

k表示(x,y,z,λ_config)^T中的任意输入变量，采用下式，进行归一化处理：

其中：

k_normal为归一化后用于输入网络的归一化变量；

k_min表示在机械臂的操作空间范围内变量k可取的最小值；

k_max表示在机械臂的操作空间范围内变量k可取的最大值。

优选的，步骤4中，采用下式，对输出空间角度(θ₁,θ₂,…,θ_N)^T进行归一化处理，得到归一化后的值(θ_1,normal,θ_2,normal,…,θ_N,normal)^T：

其中：

θ_n,normal为归一化后的值。

优选的，步骤4中，4层前向神经网络中，隐含层和输出层均采用sigmoid函数作为激活函数。

优选的，步骤6中，采用以下方式，对步骤4设计的神经网络进行参数训练，得到训练好的神经网络：

基于所建立的由(θ₁,θ₂,…,θ_N)^T到(x,y,z,λ_config)^T的样本库，对所构建的前向神经网络进行参数训练，方式为：将样本库中90％的样本用于训练，将样本库中10％的样本用于网络测试；

神经网络训练时，训练学习率采用随训练迭代次数线性递减的策略，其表达式为：

其中：

iter表示当前迭代次数；

iter_total表示训练总迭代次数；

lr_iter表示当前迭代回合内采用的学习率；

lr_initial表示初始学习率；lr_final表示终止学习率；

训练损失函数设计为N个空间角度求解的均方误差，即：

其中：

Loss表示训练损失函数；

表示前向神经网络第n个节点的输出值；

θ_n,normal表示归一化后的样本标签值；

训练时采取的损失值需要进一步在训练批量内求平均值；训练过程中，每间隔一定的训练迭代次数利用测试样本库进行一次测试，通过监控训练损失函数和测试损失函数的值判断网络收敛程度；

当训练损失函数和测试损失函数小于设定阈值时，认为训练成功收敛。

本发明提供的基于神经网络和构型编码的机械臂逆运动学求解方法具有以下优点：

(1)本发明提出一种基于前向神经网络和构型编码的多自由度机械臂逆运动学求解方法，较好的解决多自由度机械臂求解空间较大时的多解性对逆运动网络训练收敛性的干扰和局部最优的问题。

(2)所设计的基于前向神经网络和构型编码的多自由度机械臂逆运动学求解方法对不同自由度数的情况具有较好的通用性，而不会随情况的自由度增加而增加算法的复杂度。

附图说明

图1为本发明提供的基于神经网络和构型编码的机械臂逆运动学求解方法的流程示意图；

图2为本发明提供的多自由度机械臂示意图；

图3为本发明提供的坐标系与变量定义示意图；

图4为本发明提供的节点构型变量的取值方式示意图；

图5为本发明提供的基于BP神经网络的N自由度机械臂逆运动学求解器示意图；

图6为本发明实施例提供的多自由度机械臂示意图；

图7为本发明实施例提供的坐标系与变量定义示意图；

图8为本发明实施例提供的节点构型变量的取值方式示意图；

图9为本发明实施例提供的基于前向神经网络和构型编码的3自由度机械臂逆运动学求解器示意图。

具体实施方式

为了使本发明所解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

本发明属于机器人领域，提供一种高效的空间多自由度机械臂逆运动学求解方法，该方法可以拓展到复杂机械臂的逆运动学求解中。

本发明提出的一种新的基于前向神经网络和构型编码的空间多自由度机械臂逆运动学求解方法，主要思路为：

对多自由度机械臂的构型进行分析和编码；基于机械臂构型编码参数构建运动学方程，并构建训练数据库；设计前向神经网络结构，在网络输入中增加机械臂构型的编码参数的维度；通过监督学习建立机械臂末端位置和机械臂构型到各个关节角度的求解模型；应用时，分别以不同构型参数为输入，利用训练好的神经网络求解不同构型下的关节控制角度，并基于一定准则对不同构型下的结果进行选择。所提出的方法较好的解决了当机器人自由度较多时，逆运动学多解性对神经网络训练干扰的困难。

具体的：通常来说，多自由度机械臂的主体是由多个主动或被动的驱动机构和连接机构通过刚性连接构成的，其通过分布在不同位置和姿态自由度上的驱动机构实现三维空间可到达的能力。对基于关节驱动器的多自由度机械臂来说，通过机械臂关节角度量求解机械臂末端的位置和姿态的过程为正向运动求解；由机械臂末端的期待位置和姿态求解关节角度的过程为逆运动学求解。

本发明提出了一种基于前向神经网络和构型编码的空间多自由度机械臂逆运动学求解方法，步骤为：

构建包含构型编码参数的正向运动学方程，构建整个操作空间内由目标末端位置和构型编码参数到关节角度的“一对一”的训练样本库；以机械臂末端目标位置和机械臂构型编码参数为输入，以各个关节控制角度为输出，构造多层前向神经网络；基于所构建的训练样本库进行网络参数学习，建立从机械臂末端目标位置和机械臂构型编码参数到各个关节控制角度的数据模型；应用求解器时，分别以末端目标位置和不同的机械臂构型参数为输入，此时的网路输出为不同构型参数引导下的机械臂各关节的角度值；最后，根据一定准则对不同构型下的解进行筛选，得出最终关节角度值用于动作执行。在所提出的方法中，对多自由度机械臂的构型进行编码，将编码参数看作新的运动学参数，有效区别了每组末端位置坐标所对应的机械臂关节角度的多组解的情况。将机械臂构型编码参数作为网络的输入进行训练和求解，避免了求解空间内多解性对网络训练收敛的干扰和局部最优的问题。

所提出的方法包括运动学参数定义，机械臂构型编码，正向运动学构建，训练样本库建立，逆运动学网络设计，网络训练，逆运动学求解应用和最终解优选。流程图如图1所示，包括以下步骤：

步骤1，坐标系与运动学参数定义：

步骤1.1，对于被研究的多自由度机械臂，其结构如下：

所提出基于前向神经网络和构型编码参数的多自由度机械臂逆运动求解方法适用于图2所示的多自由度机械臂，假设其自由度数为N，N＞2，从多自由度机械臂的基座向机械臂末端，共有N个关节，各关节依次编号为关节1～N；从多自由度机械臂的基座向机械臂末端，共有N个连杆，各连杆依次编号为连杆1～N；

对于关节n，n∈[1,N]，当n＝1时，关节n通过连杆n与关节n+1相连；当n＝2,3,...,N-1时，关节n的一端通过连杆n-1与关节n-1相连，关节n的另一端通过连杆n与关节n+1相连；当n＝N时，关节n的一端通过连杆n-1与关节n-1相连，关节n的另一端通过连杆n与机械臂末端相连；

该多自由度机械臂，关节2～N的关节转轴互相平行，并且与关节1的关节转轴保持垂直；当n＝2,3,...,N时，各关节之间通过刚性连杆串联，连杆n-1与连杆n的延长线与关节n的关节转轴相交于一点，即：相邻连杆的延长线与所夹的关节转轴相交于一点；

步骤1.2，坐标系定义如下：

为方便参数定义和数学推导，首先进行坐标系和变量定义。坐标系和变量定义如图3所示。

坐标系定义如下：以关节1的关节转轴和连杆1的交点为原点O，沿关节1的关节转轴方向为Z轴，当关节1转角为0时，沿连杆1并指向离开关节1的方向为X轴，按照右手法则确定Y轴，从而建立XYZ坐标系；

步骤1.3，变量定义如下：

在XYZ坐标系中，机械臂末端的位置坐标为(x,y,z)^T；

对于连杆n，n∈[1,N]，定义连杆n的长度为l_n；

对于关节n，n∈[1,N]，定义关节n的角位移为α_n；其中，当n＝1时，关节n的角位移α_n为连杆n与X轴的夹角；当n＝2,3,...,N时，关节n的角位移α_n为连杆n-1与连杆n的夹角；

对于关节n，n∈[1,N]，定义关节n的空间角度为θ_n；θ_n∈[0,π]；当n＝1时，关节n的空间角度θ_n等于关节n的角位移α_n；当n＝2,3,...,N时，关节n的空间角度θ_n为Z轴与连杆n的夹角；

关节n的角位移α_n与关节n的空间角度θ_n的关系通过以下公式1表示：

步骤2，机械臂构型编码参数定义如下：

对于上述机械臂，由于其空间自由度数量较多，对于某一个目标末端位置存在多组解，即同一个末端位置可能对应不同的机械臂空间构型。引入机械臂构型编码参数λ_config＝0,1,2,…,2^N-2-1用于区别不同的空间构型，即将机械臂的空间构型分为了2^N-2种。其中λ_config的取值的确定根据空间角度θ_n和θ_n-1的大小关系对机械臂的构型进行确定。

对于具有N个关节的机械臂，定义η_n为第n个节点处的节点构型变量，则有N-2个节点构型变量，分别为η₃～η_N。η_n的取值取决于关节的构型。当θ_n-1＜θ_n时，η_n＝0；当θ_n-1≤θ_n时，η_n＝1，其中n＝3,4,…,N。节点构型变量的取值如图4所示。具体步骤为：

步骤2.1，对于具有N个关节的多自由度机械臂，其关节3～关节N中的每个关节均具有两种构型，分别是上扑构型和上凹构型，因此，当n＝3,4,…,N时，定义关节n的节点构型变量为η_n，因此，具有N个关节的多自由度机械臂，共有N-2个节点构型变量，分别为η₃～η_N；

节点构型变量η_n的取值取决于关节的构型，具体通过以下公式2确定：

步骤2.2，将η₃～η_N按顺序排列为一个二进制数

作为机械臂整体构型二进制编码；

将机械臂整体构型二进制编码转化为十进制数，并作为机械臂构型编码参数λ_config的取值，即，通过以下公式3确定机械臂构型编码参数λ_config：

对于具有N个关节的多自由度机械臂，其空间构型共有2^N-2种，因此，每种空间构型对应一个机械臂构型编码参数λ_config，一共有2^N-2个机械臂构型编码参数λ_config，即：λ_config＝0,1,2,…,2^N-2-1；

步骤3，正向运动学构建：

机械臂的关节角位移α₁～α_n和末端位置(x,y,z)^T是本发明所针对的机械臂的主要运动学参数。本发明提出的机械臂构型编码参数也可以看作一种运动学参数。因此，正向运动学是指构建从α₁～α_n到末端位置(x,y,z)^T和构型参数λ_config的等式关系。本发明中使用空间角度θ_n进行机械臂正向运动学的构建，简化了数学推导过程，可容易得到公式4的运动学方程。

具体的，正向运动学构建，是指构建从空间角度θ_n到机械臂末端位置(x,y,z)^T和机械臂构型编码参数λ_config的等式关系，具体构建以下公式4所示的运动学方程：

步骤4，逆运动学求解神经网络设计：

构建一个多层人工神经网络用于所属多自由度机械臂的逆运动学的数据建模。如图5所示为一个4层前向神经网络，从第1层到输出层各层的神经元数量分别为16，16，16，N。其中，网络的输入为4个节点，分别输入变量(x,y,z,λ_config)^T，网络的输出层为N个节点，分别输出(θ₁,θ₂,L,θ_N)^T的取值。

具体的，构建4层前向神经网络，用于多自由度机械臂的逆运动学的数据建模；其中，4层前向神经网络共有四层，分别为第1层、第2层、第3层和第4层；第1层、第2层和第3层为隐含层；第4层为输出层；第1层、第2层、第3层和第4层的神经元数量分别为16，16，16，N；

为了加速网络的学习效果，将网络的输入和输入进行归一化处理。本步骤中，采用以下方法，对输入变量(x,y,z,λ_config)^T进行归一化处理，得到输入变量(x,y,z,λ_config)^T的归一化后的值(x_normal,y_normal,z_normal,λ_{config,normal})^T：

k表示(x,y,z,λ_config)^T中的任意输入变量，采用下式，进行归一化处理：

其中：

k_normal为归一化后用于输入网络的归一化变量；

k_min表示在机械臂的操作空间范围内变量k可取的最小值；

k_max表示在机械臂的操作空间范围内变量k可取的最大值。

采用下式，对输出空间角度(θ₁,θ₂,…,θ_N)^T进行归一化处理，得到归一化后的值(θ_1,normal,θ_2,normal,…,θ_N,normal)^T：

其中：

θ_n,normal为归一化后的值。

4层前向神经网络中，隐含层和输出层均采用sigmoid函数作为激活函数。

其中，网络的输入为4个节点，分别为输入变量(x,y,z,λ_config)^T的归一化后的值(x_normal,y_normal,z_normal,λ_{config,normal})^T；其中，x_normal为x归一化处理后的值；y_normal为y归一化处理后的值；z_normal为z归一化处理后的值；λ_{config,normal}为λ_config归一化处理后的值；

网络的输出层为N个节点，分别输出空间角度(θ₁,θ₂,…,θ_N)^T的归一化后的值(θ_1,normal,θ_2,normal,…,θ_N,normal)^T；

步骤5，训练样本库构建：

在多自由度机械臂操作半球空间内进行随机均匀采样，建立(θ₁,θ₂,…,θ_N)^T到(x,y,z,λ_config)^T的样本组合，其中满足θ_n∈[0,π]，进而构建得到训练样本库。

具体的，基于前向神经网络的机械臂逆运动学求解方法需要通过样本学习来建立机械臂的末端位置(x,y,z)^T与机械臂构型编码参数λ_config与机械臂各关节角度(θ₁,θ₂,…,θ_N)^T之间的数据模型。在机械臂操作半球空间内进行大规模随机均匀采样，建立(θ₁,θ₂,…,θ_N)^T到(x,y,z,λ_config)^T的样本组合，其中满足θ_n∈[0,π]。其中，要求采样数量足够大，从而实现对操作半球空间的全覆盖。

步骤6，神经网络训练与测试：

基于步骤5构建的训练样本库，对步骤4设计的神经网络进行参数训练，得到训练好的神经网络；

采用以下方式，对步骤4设计的神经网络进行参数训练，得到训练好的神经网络：

基于所建立的由(θ₁,θ₂,…,θ_N)^T到(x,y,z,λ_config)^T的样本库，对所构建的前向神经网络进行参数训练，方式为：将样本库中90％的样本用于训练，将样本库中10％的样本用于网络测试；

神经网络训练时，训练学习率采用随训练迭代次数线性递减的策略，其表达式为：

其中：

iter表示当前迭代次数；

iter_total表示训练总迭代次数；

lr_iter表示当前迭代回合内采用的学习率；

lr_initial表示初始学习率；lr_final表示终止学习率；

训练损失函数设计为N个空间角度求解的均方误差，即：

其中：

Loss表示训练损失函数；

表示前向神经网络第n个节点的输出值；

θ_n,normal表示归一化后的样本标签值；

训练时采取的损失值需要进一步在训练批量内求平均值；训练过程中，每间隔一定的训练迭代次数利用测试样本库进行一次测试，通过监控训练损失函数和测试损失函数的值判断网络收敛程度；

当训练损失函数和测试损失函数小于设定阈值时，认为训练成功收敛。因此，在网络训练前还需要对训练批量大小、训练迭代总次数、样本空间大小和损失函数阈值等超参数进行设定。

步骤7，逆运动学求解器的应用：

步骤7.1，上述前向神经网络训练后可用于机械臂逆运动学的求解。

末端目标位置为(x″,y″,z″)，归一化后的末端目标位置为(x″_normal,y″_normal,z″_normal)；对于具有N个关节的多自由度机械臂，其空间构型共有2^N-2种，因此，归一化后的机械臂构型编码参数λ″_{config,normal}等于未归一化的机械臂构型编码参数λ″_config，即：λ″_{config,normal}＝λ″_config，并且，λ″_{config,normal}有2^N-2个取值；

对于每个空间构型，以归一化后的末端目标位置和机械臂构型编码参数(x″_normal,y″_normal,z″_normal,λ″_{config,normal})^T为输入，输出当前空间构型下归一化目标空间角度值

由于共有2^N-2种空间构型，因此，输出的各种空间构型下归一化目标空间角度值

共有2^N-2种组合；

将输出的归一化目标空间角度值反归一化处理，得到各种空间构型下目标空间角度的准确值

步骤7.2，最终用于机械臂控制执行的关节角度值和空间构型只有一组，因此需要根据一定的准则对机械臂的构型进行筛选。为了追求最高的末端控制精度，这里以机械臂末端控制精度为目标函数，筛选出控制理论上控制精度最高的解。将目标函数标记为F，其定义为：

式中：(x″,y″,z″)表示初始已知的末端目标位置为(x″,y″,z″)；

表示基于逆运动学求解空间角计算出的末端位置；

因此，对于每种空间构型，均计算出目标函数F的值；通过以下方式确定最优机械臂构型编码参数λ_config,best为：

确定出使末端控制精度最高的最优机械臂构型编码参数λ_config,best和对应的最优的目标空间角度的准确值

后，采用下式求出最优构型条件下的机械臂各关节的最优角位移(α_1,best,α_2,best,…,α_N,best)^T，基于最优角位移(α_1,best,α_2,best,…,α_N,best)^T，对机械臂进行控制。此外，在机械臂构型筛选时，目标函数也可以定义根据避障安全性或关节路径之和等，从而筛选出安全性最高或控制能耗最低的机械臂构型和空间角度组合。

下面列举一个具体实施例：

下面通过一个案例分析说明本发明所提供的方法。案例针对一个3自由度机械臂，提供了基于前向神经网络和机械臂构型编码的逆运动学求解方法。

(1)坐标系与运动学参数定义

针对图6所示的机械臂，其自由度数为N＝3，从机械臂基座向其末端对机械臂关节依次编号为关节1,2,3，从机械臂基座向其末端对机械臂的连杆依次编号为连杆1,2,3，其中连杆3与末端相连。该3自由度机械臂的特征在于：第2，3个关节转轴互相平行，同时与第1个关节保持垂直，各关节之间通过刚性连杆串联，且相邻连杆的延长线与所夹的关节转轴相交于一点。

坐标系定义如下：

坐标系和变量定义如图7所示。以关节1转轴和连杆1的交点为原点O，z轴沿关节1的转轴方向，x轴为当关节1转角为0时沿连杆1并指向离开关节1的方向，y轴按照右手法则确定，则机械臂末端的位置坐标为(x,y,z)^T。定义连杆n的长度依次为l_n，定义关节n的角度(即相临连杆的夹角)为α_n，其中n∈[1,N]。为方便数学推导，定义空间角度θ_n∈[0,π]。其中θ₁＝α₁；当n＞1时，θ_n为坐标系z轴到连杆n的夹角。容易知道关节角位移α_n和空间角度θ_n的关系可以表示为：

(2)机械臂构型编码参数定义

对于所设计的3自由度机械臂，通过简单的几何分析易知，某一个目标末端位置对应存在两组关节角度位置，即同一个末端位置可能对应两种不同的机械臂空间构型。引入机械臂构型编码参数λ_config＝0,1用于区别不同的空间构型，即将机械臂的空间构型分为了2种。其中λ_config的取值的确定根据空间角度θ₂和θ₃的大小关系对机械臂的构型进行确定。

定义η₃为关节3处的节点构型变量，其取值取决于关节的构型。如图8所示，当θ₂＜θ₃时，η_n＝0，此时λ_config＝0；当θ₂≥θ₃时，η₃＝1，此时λ_config＝1。

(3)正向运动学构建

机械臂的关节角位移(α₁,α₂,α₃)^T和末端位置(x,y,z)^T是所涉及的3自由度机械臂的主要运动学参数。本发明提出的机械臂构型编码参数也可以看作一种运动学参数。因此，正向运动学是指构建从(α₁,α₂,α₃)^T到末端位置(x,y,z)^T和构型参数λ_config的等式关系。本发明中使用空间角度θ_n进行机械臂正向运动学的构建，简化了数学推导过程，可容易得到如下所示的运动学方程。

(4)逆运动学求解神经网络设计

构建一个多层人工神经网络用于所述机械臂的逆运动学的数据建模。如图9所示为一个4层前向神经网络，从第1层到输出层各层的神经元数量分别为16，16，16，3。其中，网络的输入为4个节点，分别输入变量(x,y,z,λ_config)^T，网络的输出层为3个节点，分别输出(θ₁,θ₂,θ₃)^T的取值。为了加速网络的学习效果，将网络的输入和输入进行归一化处理。输入变量按照

进行归一化，其中k表示(x,y,z,λ_config)^T中的任意输入变量，k_normal为归一化后用于输入网络的归一化变量，k_min表示在机械臂的操作空间范围内变量k可取的最小值，k_max表示在机械臂的操作空间范围内变量k可取的最大值。其中在当前的坐标系和变量顶一下，有：

同时，网络结构的输出变量按照

进行归一化。网络的隐含层和输出层都采用sigmoid函数作为激活函数。

(5)训练样本库构建

基于前向神经网络的机械臂逆运动学求解方法需要通过样本学习来建立机械臂的末端位置(x,y,z)^T和与机械臂构型编码参数λ_config与机械臂各关节角度(θ₁,θ₂,θ₃)^T之间的数据模型。

在机械臂操作半球空间内进行大规模随机均匀采样，建立(θ₁,θ₂,θ₃)^T到(x,y,z,λ_config)^T的样本组合，其中满足θ₁,θ₂,θ₃∈[0,π]。其中，要求采样数量足够大，从而实现对操作半球空间的全覆盖。

(6)神经网络训练与测试

按照步骤(5)所述方法建立由(θ₁,θ₂,…,θ_N)^T到(x,y,z,λ_config)^T的样本库，采样数量设置为100000个采样点，对所构建的前向神经网络进行参数训练。在样本库中随机抽取90000的样本充当网络训练样本库，剩余的10000个样本用于网络测试样本库。网络训练的超参数设置如下表所示，其中初始学习率设为0.01，最终学习率设为0.0000001，训练批量大侠设为32，训练迭代次数设为2000000000。

表1：基于前向神经网络和构型编码的机械臂逆运动学求解器训练超参数设置

神经网络训练时，训练学习率采用随训练迭代次数线性递减的策略，其表达式为

其中iter表示当前迭代次数。训练损失函数设计为3个空间角度求解的均方误差，即

式中Loss表示训练损失函数，

表示前向神经网络第n个节点的输出值，θ_n,normal表示归一化后的样本标签值。训练时采取的损失值需要进一步在训练批量内求平均值。训练过程中，每间隔一定的训练迭代次数利用测试样本库进行一次测试，通过监控训练损失函数和测试损失函数的值来判断网络收敛程度。

(7)逆运动学求解器的应用

前向神经网络训练后可用于所涉及的3自由度机械臂逆运动学的求解。以归一化后的末端目标位置和机械臂构型编码参数(x″_normal,y″_normal,z″_normal,λ_c″_onfig,normal)^T为输入，其中，λ″_{config,normal}＝λ″_config，并且有2个取值，因此网络的输出为当前空间构型λ″_config下归一化目标空间角度值

共有2种组合。将输出的归一化目标空间角度值反归一化处理，得到不同构型下空间角度的准确值

为了追求最高的末端控制精度，这里以机械臂末端控制精度为目标函数，筛选出控制理论上控制精度最高的解。将目标函数标记为F，其定义为：

式中，x,y,z表示目标末端位置，

表示基于逆运动学求解空间角计算出的末端位置，

表示基于神经网络求解出的关节n的空间角度值。最优机械臂构型为

由上式确定出使末端控制精度最高的机械臂构型λ_config,best和

后，进一步利用步骤(2)中的公式求出最优构型条件下的机械臂各关节的最优角位移(α_1,best,α_2,best,α_3,best)^T，并作为机械臂3个关节的控制执行的目标角度位置。

本发明提供的基于神经网络和构型编码的机械臂逆运动学求解方法，具有以下优点：

(1)本发明提出一种基于前向神经网络和构型编码的多自由度机械臂逆运动学求解方法，首次提出将机械臂构型编码参数与目标末端位置一起作为网络输入的思路，构建包含机械臂构型编码参数的运动学方程，构建全部操作空间内“一对一”训练样本库进行逆运动学求解网络的训练。通过训练和测试证明这种处理方法能够较好的解决多自由度机械臂求解空间较大时的多解性对逆运动网络训练收敛性的干扰和局部最优的问题。

(2)所设计的基于前向神经网络和构型编码的多自由度机械臂逆运动学求解方法对不同自由度数的情况具有较好的通用性，而不会随情况的自由度增加而增加算法的复杂度。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视本发明的保护范围。

Claims (5)

1.一种基于神经网络和构型编码的机械臂逆运动学求解方法，其特征在于，包括以下步骤：

步骤1，坐标系与运动学参数定义：

步骤1.1，对于被研究的多自由度机械臂，其结构如下：

假设其自由度数为N，N＞2，从多自由度机械臂的基座向机械臂末端，共有N个关节，各关节依次编号为关节1～N；从多自由度机械臂的基座向机械臂末端，共有N个连杆，各连杆依次编号为连杆1～N；

对于关节n，n∈[1,N]，当n＝1时，关节n通过连杆n与关节n+1相连；当n＝2,3,...,N-1时，关节n的一端通过连杆n-1与关节n-1相连，关节n的另一端通过连杆n与关节n+1相连；当n＝N时，关节n的一端通过连杆n-1与关节n-1相连，关节n的另一端通过连杆n与机械臂末端相连；

该多自由度机械臂，关节2～N的关节转轴互相平行，并且与关节1的关节转轴保持垂直；当n＝2,3,...,N时，连杆n-1与连杆n的延长线与关节n的关节转轴相交于一点；

步骤1.2，坐标系定义如下：

以关节1的关节转轴和连杆1的交点为原点O，沿关节1的关节转轴方向为Z轴，当关节1转角为0时，沿连杆1并指向离开关节1的方向为X轴，按照右手法则确定Y轴，从而建立XYZ坐标系；

步骤1.3，变量定义如下：

在XYZ坐标系中，机械臂末端的位置坐标为(x,y,z)^T；

对于连杆n，n∈[1,N]，定义连杆n的长度为l_n；

对于关节n，n∈[1,N]，定义关节n的角位移为α_n；其中，当n＝1时，关节n的角位移α_n为连杆n与X轴的夹角；当n＝2,3,...,N时，关节n的角位移α_n为连杆n-1与连杆n的夹角；

对于关节n，n∈[1,N]，定义关节n的空间角度为θ_n；θ_n∈[0,π]；当n＝1时，关节n的空间角度θ_n等于关节n的角位移α_n；当n＝2,3,...,N时，关节n的空间角度θ_n为Z轴与连杆n的夹角；

关节n的角位移α_n与关节n的空间角度θ_n的关系通过以下公式1表示：

步骤2，机械臂构型编码参数定义如下：

步骤2.1，对于具有N个关节的多自由度机械臂，其关节3～关节N中的每个关节均具有两种构型，分别是上扑构型和上凹构型，因此，当n＝3,4,…,N时，定义关节n的节点构型变量为η_n，因此，具有N个关节的多自由度机械臂，共有N-2个节点构型变量，分别为η₃～η_N；

节点构型变量η_n的取值取决于关节的构型，具体通过以下公式2确定：

步骤2.2，将η₃～η_N按顺序排列为一个二进制数

作为机械臂整体构型二进制编码；

将机械臂整体构型二进制编码转化为十进制数，并作为机械臂构型编码参数λ_config的取值，即，通过以下公式3确定机械臂构型编码参数λ_config：

对于具有N个关节的多自由度机械臂，其空间构型共有2^N-2种，因此，每种空间构型对应一个机械臂构型编码参数λ_config，一共有2^N-2个机械臂构型编码参数λ_config，即：λ_config＝0,1,2,…,2^N-2-1；

步骤3，正向运动学构建：

正向运动学构建，是指构建从空间角度θ_n到机械臂末端位置(x,y,z)^T和机械臂构型编码参数λ_config的等式关系，具体构建以下公式4所示的运动学方程：

步骤4，逆运动学求解神经网络设计：

构建4层前向神经网络，用于多自由度机械臂的逆运动学的数据建模；其中，4层前向神经网络共有四层，分别为第1层、第2层、第3层和第4层；第1层、第2层和第3层为隐含层；第4层为输出层；第1层、第2层、第3层和第4层的神经元数量分别为16，16，16，N；

其中，网络的输入为4个节点，分别为输入变量(x,y,z,λ_config)^T的归一化后的值(x_normal,y_normal,z_normal,λ_{config,normal})^T；其中，x_normal为x归一化处理后的值；y_normal为y归一化处理后的值；z_normal为z归一化处理后的值；λ_{config,normal}为λ_config归一化处理后的值；

网络的输出层为N个节点，分别输出空间角度(θ₁,θ₂,…,θ_N)^T的归一化后的值(θ_1,normal,θ_2,normal,…,θ_N,normal)^T；

步骤5，训练样本库构建：

在多自由度机械臂操作半球空间内进行随机均匀采样，建立(θ₁,θ₂,…,θ_N)^T到(x,y,z,λ_config)^T的样本组合，其中满足θ_n∈[0,π]，进而构建得到训练样本库；

步骤6，神经网络训练与测试：

基于步骤5构建的训练样本库，对步骤4设计的神经网络进行参数训练，得到训练好的神经网络；

步骤7，逆运动学求解器的应用：

步骤7.1，末端目标位置为(x",y",z")，归一化后的末端目标位置为(x"_normal,y"_normal,z"_normal)；对于具有N个关节的多自由度机械臂，其空间构型共有2^N-2种，因此，归一化后的机械臂构型编码参数λ"_{config,normal}等于未归一化的机械臂构型编码参数λ"_config，即：λ"_{config,normal}＝λ"_config，并且，λ"_{config,normal}有2^N-2个取值；

对于每个空间构型，以归一化后的末端目标位置和机械臂构型编码参数(x"_normal,y"_normal,z"_normal,λ"_{config,normal})^T为输入，输出当前空间构型下归一化目标空间角度值

由于共有2^N-2种空间构型，因此，输出的各种空间构型下归一化目标空间角度值

共有2^N-2种组合；

将输出的归一化目标空间角度值反归一化处理，得到各种空间构型下目标空间角度的准确值

步骤7.2，采用以下方式，对2^N-2种目标空间角度的准确值

进行筛选：

以机械臂末端控制精度为目标函数，将目标函数标记为F，其定义为：

式中：(x",y",z")表示初始已知的末端目标位置为(x",y",z")；

表示基于逆运动学求解空间角计算出的末端位置；

因此，对于每种空间构型，均计算出目标函数F的值；通过以下方式确定最优机械臂构型编码参数λ_config,best为：

确定出使末端控制精度最高的最优机械臂构型编码参数λ_config,best和对应的最优的目标空间角度的准确值

后，采用下式求出最优构型条件下的机械臂各关节的最优角位移(α_1,best,α_2,best,…,α_N,best)^T，基于最优角位移(α_1,best,α_2,best,…,α_N,best)^T，对机械臂进行控制。

2.根据权利要求1所述的基于神经网络和构型编码的机械臂逆运动学求解方法，其特征在于，步骤4中，采用以下方法，对输入变量(x,y,z,λ_config)^T进行归一化处理，得到输入变量(x,y,z,λ_config)^T的归一化后的值(x_normal,y_normal,z_normal,λ_{config,normal})^T：

k表示(x,y,z,λ_config)^T中的任意输入变量，采用下式，进行归一化处理：

其中：

k_normal为归一化后用于输入网络的归一化变量；

k_min表示在机械臂的操作空间范围内变量k可取的最小值；

k_max表示在机械臂的操作空间范围内变量k可取的最大值。

3.根据权利要求1所述的基于神经网络和构型编码的机械臂逆运动学求解方法，其特征在于，步骤4中，采用下式，对输出空间角度(θ₁,θ₂,…,θ_N)^T进行归一化处理，得到归一化后的值(θ_1,normal,θ_2,normal,…,θ_N,normal)^T：

其中：

θ_n,normal为归一化后的值。

4.根据权利要求1所述的基于神经网络和构型编码的机械臂逆运动学求解方法，其特征在于，步骤4中，4层前向神经网络中，隐含层和输出层均采用sigmoid函数作为激活函数。

5.根据权利要求1所述的基于神经网络和构型编码的机械臂逆运动学求解方法，其特征在于，步骤6中，采用以下方式，对步骤4设计的神经网络进行参数训练，得到训练好的神经网络：

基于所建立的由(θ₁,θ₂,…,θ_N)^T到(x,y,z,λ_config)^T的样本库，对所构建的前向神经网络进行参数训练，方式为：将样本库中90％的样本用于训练，将样本库中10％的样本用于网络测试；

神经网络训练时，训练学习率采用随训练迭代次数线性递减的策略，其表达式为：

其中：

iter表示当前迭代次数；

iter_total表示训练总迭代次数；

lr_iter表示当前迭代回合内采用的学习率；

lr_initial表示初始学习率；lr_final表示终止学习率；

训练损失函数设计为N个空间角度求解的均方误差，即：

其中：

Loss表示训练损失函数；

表示前向神经网络第n个节点的输出值；

θ_n,normal表示归一化后的样本标签值；

训练时采取的损失值需要进一步在训练批量内求平均值；训练过程中，每间隔一定的训练迭代次数利用测试样本库进行一次测试，通过监控训练损失函数和测试损失函数的值判断网络收敛程度；

当训练损失函数和测试损失函数小于设定阈值时，认为训练成功收敛。

Images