CN108447477A - 一种基于自然语言理解的机器人控制方法 - Google Patents

Abstract

本发明提出了一种基于自然语言理解的机器人控制方法，包括步骤：(1)获取语音信号并转化为相应数字信号，而后通过动态时间规整算法将数字信号转换为相应的文本信息；(2)建立模型，对文本信息中所包含的关键信息与冗余信息进行匹配，并获取文本信息的分割结果，之后通过最大熵模型建立的分类器，对文本信息进行分类，并将分类结果中需要进一步转换的控制指令进行转换；(3)通过获取的机器人控制指令，将获得的机械臂末端执行器空间坐标反解为旋转关节的关节变量，控制机器人进行运动。

Description

一种基于自然语言理解的机器人控制方法

技术领域

本发明属于机器人运动领域，特别涉及一种基于自然语言理解的机器人控制方法。

背景技术

随着机器人的智能性和灵活性越来越高，传统的机器人控制方式已经逐渐的无法满足智能机器人的需要。因此，寻找合适的机器人智能控制方式已经成为了目前的一项研究方向。通过智能控制方式，机器人只需要理解人类的意图，通过其本身的智能性来完成剩余的控制工作。

如今，随着计算机技术以及语音识别技术的发展，语音作为同计算机的交流方式之一，已经逐步的被人们所接受。而语音作为人与人之间的常用交流方式，相比鼠标、键盘，具有更强的便利性。因此，语音成为了目前被人们所研究的机器人智能控制方式之一。相比传统的机器人控制方式，通过语音控制机器人不仅更加便捷，在工作效率上也能得到提高，十分符合人们的需要。

目前，大多数的基于语音识别的机器人控制系统只能简单地识别一些诸如“前进”、“左转”、“停止”等的基本指令，即使有少部分的能实现连续语音识别的机器人控制系统，也只能识别固定模式的机器人控制指令，与所期望实现的机器人的智能语音控制相差甚远。

发明内容

本发明提出了一种基于自然语言理解的机器人控制方法。这个方法首先从用户对机器人下达的语音指令中读取出语音流，并将其转换为文本信息，而后将文本信息通过意图理解算法转换为机器人控制指令，最后通过机器人运动学变换实现机器人的控制。

本发明一种基于自然语言理解的机器人控制方法，包括如下步骤：

S1、获取及分析语音信号；

S2、用户意图理解；

S3、控制机器人运动。

进一步地，所述步骤S1包括以下步骤：

操作者通过麦克风发出一系列连续的语音指令，麦克风获取语音信号将其化为语音流，计算机接收语音流，并将其转换为计算机可处理的数字信号。语音信号数字化之后可以反映为一副波形图，直观的表示其数字特征，首先通过预滤波、采样、A/D转换、分帧、数据加窗、高频提升等步骤进行语音信号的预处理，由于得到的语音信号和参考模板长度不一，需要对处理好的语音信号进行规整，本发明采用了动态时间规整算法。

假设有两个时间序列Q和C，他们的长度分别是n和m：

Q＝q₁,q₂,....,q_i,q_n

C＝c₁,c₂,....,c_j,c_m

为了对齐这两个序列，需要构造一个n x m的矩阵网格，矩阵元素(i,j)表示q_i和c_j两个点的距离d(q_i,c_j)。这里采用欧式距离：

d(q_i,c_j)＝(q_i-c_j)²

定义路径：W＝w₁,w₂,....,w_n表示两个序列的最短路径，因此有：

其中K为补偿参数，是用于对不同的长度的规整路径做补偿。

定义累积距离γ，从(0,0)点开始匹配这两个序列Q和C，每到一个点，之前所有的点计算的距离都会累加。到达终点(n,m)后，此距离γ(n,m)即为总距离，表示序列Q和C的相似度。

累积距离γ(i,j)可以按以下方式表示：

γ(i,j)＝d(q_i,c_j)+min{γ(i-1,j-1),γ(i-1,j),γ(i,j-1)}

按照以上方法可以求得语音信号所符的参考模板，从而将该语音信号转换为文本信息。

进一步地，所述步骤S2包括以下步骤：

1)文本信息提取及分割

在用户意图理解的过程中，首要步骤是从文本信息中采集出与机器人控制指令所关联的文本片段。在一段文本信息中，既存在与机器人控制指令相关的关键文本信息，也存在对一段文本信息的理解无关的冗余信息。关键文本信息包括动作词、物品的名字、物品的属性、位置词等，而冗余信息则包括语气助词、无关字词等。因此有必要建立一个模型分别匹配这些关键信息和冗余信息。

在本发明中，对于某一段文本信息，建立的模型分别对此文本信息中的关键信息和冗余信息进行匹配，而后插入分割符对关键信息和冗余信息进行分割。

2)文本信息分类

得到文本分割的结果之后，需要建立自然语言指令和对应机器人控制指令之间的映射关系，这个过程可以看作是一个分类问题，本发明利用基于最大熵模型的分类器来解决。

本发明采用向量空间模型来表示文本特征。针对训练语料库，统计库中所有出现的词。假设某一语料文本中有N个词，那么该文本就可以表示为N维的特征向量。本发明利用TF‐IDF在分类之前进行特征向量加权。TF值是局部变量，IDF是全局变量。把全局和局部两方面结合给特征向量中的各项加权如公式：

TF_i,j＝n_i,j/∑_kn_k,j

IDF_i＝log(|D|/|{j:t_i∈d_j}|)

TFIDF_i,j＝TF_i,j*IDF_i

公式(1)中n_i,j表示该语料文本中该词出现的次数∑_kn_k,j表示该语料文本包含所有词的个数；公式(2)中l D I表示训练语料库中所有语料文本的个数，|{j:t_i∈d_j}|表示包含该词的语料文本的个数。

假设x为文本特征向量，对应的意图输出标签为Y(Y∈Y，Y is a finite set ofintent labels)。最大熵算法就是对条件概率P(y l x)进行建模，得到分布最均匀的模型，这是一个最优化求解问题。引入条件熵H(p)来衡量条件概率P(y l x)分布的均匀性，根据香农对熵的定义，H(p) 计算公式为：

其中，p(x)表示在训练数据库中文本特征向量的经验分布，P(y l x)为所要求解的模型中的条件概率分布。那么，求解最大熵模型的公式为公式(5)：

p_*＝argmaxH(p)

接下来需要确定约束条件。给定一个训练数据库，文本特征向量集合记作{x_i,i∈(1,N)}，意图标签集合记作{y_j,j∈(1,M)}。这样就可以统计“特征‐意图”二元组(x,y)的经验概率分布如公式(6)：

其中，num(x,y)表示(x,y)在训练数据库中出现的次数，N表示训练数据库中样本总数。特征函数f(x,y)相对于经验条件概率分布p(x,y)的期望值如公式：

f(x,y)相对于模型条件概率分布P(y|x)的期望值如公式：

在训练数据库中，令两个期望值相同，即得到最大熵模型求解的约束条件如公式：

综上，最大熵的求解问题可以归纳为如下最优化问题如公式：

max H(p)

根据拉格朗日乘子算法，可以求解得到概率分布p*，在Kulback‐Leibler距离上是最接近经验概率分布p(x,y)的如公式：

公式中，p*为求解的最大熵概率分布，f_i(x,y)为第i个特征函数，λ_i为(x,y)的权值， n为特征函数的个数，Z(x)为归一化因子。通过在训练数据库上的学习，可以得到参数λ_i的值，从而得到要求解的概率分布p*。为了求得参数λ_i，本发明采用GIS算法求解，这样就完成了最大熵模型的建立。

3)控制指令转换

根据最大熵模型得到的分类结果，可以将自然语言指令转换为相应机器人控制指令。在本发明中，通过引入3种属性变量(V_op，V_key，V_val)来定义了机器人控制指令。

然而，纯粹通过自然语言指令转换为的机器人控制指令往往缺少控制机器人所需要的相关信息，因此需要对某些关键文本进行进一步的转换。

进一步地，所述步骤S3包括以下步骤：

通过上述步骤已经得到了机器人的控制指令，一般控制工业机械臂所需的运动参数为各个关节角的数值，而通过自然语言指令转换为的机器人控制指令所给出的参数为工业机械臂末端的空间坐标。因此，要实现对机器人的控制需要用到反解算法。

以六自由度机械臂基座为原点，垂直方向为z轴建立基坐标，通常一个六自由度机械臂的末端执行器相对于基座的总变换矩阵可表示为：

式中:为机械臂末端执行器在基坐标系中的位姿矩阵，[p^x,p^y,p^z]^T为机械臂末端执行器在基坐标系中的位置，[n^x,n^y,n^z]^T为机械臂末端执行器坐标系的x轴在基坐标系中的方向矢量，[o^x,o^y,o^z]^T为机械臂末端执行器坐标系的y轴在基坐标系中的方向矢量，[a^x,a^y,a^z]^T为机械臂末端执行器坐标系的z轴在基坐标系中的方向矢量。

其中A₁,A₂,A₃,A₄,A₅,A₆分别表示每个关节对应的矩阵，分别可由对应的机器人的参数求得。

通过矩阵联立求解即可通过机械臂末端执行器的空间坐标求得旋转关节的关节变量。

与现有技术相比，本发明具有如下优点和技术效果：本发明提出了一种基于自然语言理解的机器人控制方法，这种方法实现了基于工业机器人的自然语言理解，使操作者无需理解机器人的运动学知识，也无需事先进行机器人控制的培训，即可对机器人进行智能语音控制。目前，大多数的基于语音识别的机器人控制系统只能简单地识别一些诸如“前进”、“左转”、“停止”等的基本指令，即使有少部分的能实现连续语音识别的机器人控制系统，也只能识别固定模式的机器人控制指令，与所期望实现的机器人的智能语音控制相差甚远。本发明的这个方法首先从用户对机器人下达的语音指令中读取出语音流，并将其转换为文本信息，而后将文本信息通过意图理解算法转换为机器人控制指令，最后通过机器人运动学变换实现了机器人的控制。

附图说明

图1是实施例中一种基于自然语言理解的机器人控制方法的流程图。

具体实施方式

下面结合实施例和附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。需指出的是，以下若有未特别详细说明之过程或者符号，均是本领域技术人员可参照现有技术实现或理解的。

实施例：

本实例的一种基于自然语言理解的机器人控制方法，如附图1所示，包括如下步骤：

S1、获取及分析语音信号；

S2、用户意图理解；

S3、控制机器人运动。

所述步骤S1包括以下步骤：

操作者通过麦克风发出一系列连续的语音指令，麦克风获取语音信号将其化为语音流，计算机接收语音流并将其转换为计算机可处理的数字信号。语音信号数字化之后可以反映为一副波形图，直观的表示其数字特征，首先通过预滤波、采样、A/D转换、分帧、数据加窗、高频提升等步骤进行语音信号的预处理，由于得到的语音信号和参考模板长度不一，需要对处理好的语音信号进行规整，本发明采用了动态时间规整算法。

假设有两个时间序列Q和C，他们的长度分别是n和m：

Q＝q₁,q₂,....,q_i,q_n

C＝c₁,c₂,....,c_j,c_m

为了对齐这两个序列，需要构造一个n x m的矩阵网格，矩阵元素(i,j)表示q_i和c_j两个点的距离d(q_i,c_j)。这里采用欧式距离：

d(q_i,c_j)＝(q_i-c_j)²

定义路径：W＝w₁,w₂,....,w_n表示两个序列的最短路径，因此有：

其中K为补偿参数，是用于对不同的长度的规整路径做补偿。

定义累积距离γ，从(0,0)点开始匹配这两个序列Q和C，每到一个点，之前所有的点计算的距离都会累加。到达终点(n,m)后，此距离γ(n,m)即为总距离，表示序列Q和C的相似度。

累积距离γ(i,j)可以按以下方式表示：

γ(i,j)＝d(q_i,c_j)+min{γ(i-1,j-1),γ(i-1,j),γ(i,j-1)}

按照以上方法可以求得语音信号所符的参考模板，从而将该语音信号转换为文本信息。

所述步骤S2包括以下步骤：

1)文本信息提取及分割

在用户意图理解的过程中，首要步骤是从文本信息中采集出与机器人控制指令所关联的文本片段。在一段文本信息中，既存在与机器人控制指令相关的关键文本信息，也存在对一段文本信息的理解无关的冗余信息。关键文本信息包括动作词、物品的名字、物品的属性、位置词等，而冗余信息则包括语气助词、无关字词等。因此有必要建立一个模型分别匹配这些关键信息和冗余信息。

在本发明中，对于某一段文本信息，通过建立的模型分别对此文本信息中的关键信息和冗余信息进行匹配，而后插入分割符对关键信息和冗余信息进行分割。

例如，对于一段文本信息“去抓这个红色的水壶吧”，将其中的文本片段与机器人控制指令库中的指令进行匹配，得到的关键文本为“抓”，“这个”，“红色的水壶”，得到冗余文本为“去”，“吧”。因此可以得到分割后的文本“去#抓#这个#红色的水壶#吧”。

2)文本信息分类

得到文本分割的结果之后，需要建立自然语言指令和对应机器人控制指令之间的映射关系，这个过程可以看作是一个分类问题，本发明利用基于最大熵模型的分类器来解决。

本发明采用向量空间模型来表示文本特征。针对训练语料库，统计库中所有出现的词。假设某一语料文本中有N个词，那么该文本就可以表示为N维的特征向量。本发明利用TF‐IDF在分类之前进行特征向量加权。TF值是局部变量，IDF是全局变量。把全局和局部两方面结合给特征向量中的各项加权如公式：

TF_i,j＝n_i,j/∑_kn_k,j

TFi，j＝ni，j/∑knk，j

IDF_i＝log(|D|/|{j:t_i∈d_j}|)

TFIDF_i,j＝TF_i,j*IDF_i

公式(1)中n_i,j表示该语料文本中该词出现的次数∑_kn_k,j表示该语料文本包含所有词的个数；公式(2)中I D I表示训练语料库中所有语料文本的个数，|{j:t_i∈d_j}|表示包含该词的语料文本的个数。

假设x为文本特征向量，对应的意图输出标签为Y(Y∈Y，Y is a finite set ofintent labels)。最大熵算法就是对条件概率P(y l x)进行建模，得到分布最均匀的模型，这是一个最优化求解问题。引入条件熵H(p)来衡量条件概率P(y l x)分布的均匀性，根据香农对熵的定义，H(p) 计算公式为：

其中，p(x)表示在训练数据库中文本特征向量的经验分布，P(y l x)为所要求解的模型中的条件概率分布。那么，求解最大熵模型的公式为公式(5)：

p_*＝argmaxH(p)

接下来需要确定约束条件。给定一个训练数据库，文本特征向量集合记作{x_i,i∈(1,N)}，意图标签集合记作{y_j,j∈(1,M)}。这样就可以统计“特征‐意图”二元组(x,y)的经验概率分布如公式(6)：

其中，num(x,y)表示(x,y)在训练数据库中出现的次数，N表示训练数据库中样本总数。特征函数f(x,y)相对于经验条件概率分布p(x,y)的期望值如公式：

f(x,y)相对于模型条件概率分布P(y|x)的期望值如公式：

在训练数据库中，令两个期望值相同，即得到最大熵模型求解的约束条件如公式：

综上，最大熵的求解问题可以归纳为如下最优化问题如公式：

max H(p)

根据拉格朗日乘子算法，可以求解得到概率分布p*，在Kulback‐Leibler距离上是最接近经验概率分布p(x,y)的如公式：

公式中，p*为求解的最大熵概率分布，f_i(x,y)为第i个特征函数，λ_i为(x,y)的权值， n为特征函数的个数，Z(x)为归一化因子。通过在训练数据库上的学习，可以得到参数λ_i的值，从而得到要求解的概率分布p*。为了求得参数λ_i，本发明采用GIS算法求解，这样就完成了最大熵模型的建立。

3)控制指令转换

根据最大熵模型得到的分类结果，可以将自然语言指令转换为相应机器人控制指令。在本发明中，通过引入3种属性变量(V_op，V_key，V_val)来定义了机器人控制指令，例如，想要让机器人执行抓取一个目标，则V_op＝抓，V_key＝目标描述，V_val＝目标位置。

然而，纯粹通过自然语言指令转换为的机器人控制指令往往缺少控制机器人所需要的相关信息。

例如，对于以上文本信息“去抓这个红色的水壶吧”，得到分割后的文本为“去#抓#这个#红色的水壶#吧”，而在机器人控制指令库中，所定义的抓取目标的控制指令标准格式为 V_op＝抓取，V_key＝目标描述，V_val＝目标位置，根据最大熵模型得到的分类结果，V_val＝这个，显然无法满足控制机器人所需要的参数，因此需要对某些关键文本进行进一步的转换。

在以上实例中，V_op和V_key两个元素可以直接得到，V_op＝抓，V_key＝红色的水壶，而V_val为目标位置表述，无法通过关键文本“这个”直接获得，需要依赖外部软硬件设施，如通过传感器读取手指向的位置，并返回指定区域的坐标等，此部分不在本发明范畴之内，因此不在此进行详细描述。

最终，通过文本信息“去抓这个红色的水壶吧”，得到机器人控制指令V_op＝抓，V_key＝红色的水壶，V_val＝目标位置。

所述步骤S3包括以下步骤：

通过上述步骤已经得到了机器人的控制指令，一般控制工业机械臂所需的运动参数为各个关节角的数值，而通过自然语言指令转换为的机器人控制指令所给出的参数为工业机械臂末端的空间坐标。因此，要实现对机器人的控制需要用到反解算法。

以六自由度机械臂基座为原点，垂直方向为z轴建立基坐标，通常一个六自由度机械臂的末端执行器相对于基座的总变换矩阵可表示为：

式中:为机械臂末端执行器在基坐标系中的位姿矩阵，[p^x,p^y,p^z]^T为机械臂末端执行器在基坐标系中的位置，[n^x,n^y,n^z]^T为机械臂末端执行器坐标系的x轴在基坐标系中的方向矢量，[o^x,o^y,o^z]^T为机械臂末端执行器坐标系的y轴在基坐标系中的方向矢量，[a^x,a^y,a^z]^T为机械臂末端执行器坐标系的z轴在基坐标系中的方向矢量。

其中A₁,A₂,A₃,A₄,A₅,A₆分别表示每个关节对应的矩阵，分别可由对应的机器人的参数求得。例如，对于DH参数如表1所示的机器人来说：

表1机器人的DH参数表

其中s_i,c_i(i＝1,2,3…)分别表示sinθ_i,cosθ_i，θ表示旋转关节的关节变量，d表示华东关节的关节变量。

由以上各式即可通过机械臂末端执行器的空间坐标求得θ₁θ₂…θ₆。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (7)

1.一种基于自然语言理解的机器人控制方法，其特征在于包括如下步骤：

S1、获取及分析语音信号：通过麦克风发出一系列连续的语音指令，麦克风将语音信号化为语音流，计算机接收语音流，并将语音流转换为计算机可处理的数字信号；

S2、用户意图理解，包括文本信息提取及分割、文本信息分类、控制指令转换；

S3、控制机器人运动，通过获取的机器人控制指令，将获得的机械臂末端执行器空间坐标反解为旋转关节的关节变量，控制机器人进行运动。

2.根据权利要求1所述的一种基于自然语言理解的机器人控制方法，其特征在于所述步骤S1中，语音信号数字化之后反映为一副波形图，直观的表示其数字特征，首先通过预滤波、采样、A/D转换、分帧、数据加窗、高频提升步骤进行语音信号的预处理，由于得到的语音信号和参考模板长度不一，需要对处理好的语音信号进行规整。

3.根据权利要求2所述的一种基于自然语言理解的机器人控制方法，其特征在于步骤S1所述规整采用了动态时间规整算法：

假设有两个时间序列Q和C，它们的长度分别是n和m，n、m为正整数：

Q＝q₁,q₂,....,q_i,q_n

C＝c₁,c₂,....,c_j,c_m；

为了对齐这两个序列，需要构造一个n x m的矩阵网格，矩阵元素(i,j)表示q_i和c_j两个点的距离d(q_i,c_j)，i＝1～n，j＝1～m；采用欧式距离：

d(q_i,c_j)＝(q_i-c_j)²

定义路径：W＝w₁,w₂,....,w_n表示两个序列的最短路径，因此有：

其中K为补偿参数，是用于对不同的长度的规整路径做补偿，k＝1～K；

定义累积距离γ，从(0,0)点开始匹配这两个序列Q和C，每到一个点，之前所有的点计算的距离都会累加，到达终点(n,m)后，此距离γ(n,m)即为总距离，表示序列Q和C的相似度；

累积距离γ(i,j)能按以下方式表示：

γ(i,j)＝d(q_i,c_j)+min{γ(i-1,j-1),γ(i-1,j),γ(i,j-1)}

按照以上过程求得语音信号所符的参考模板，从而将该语音信号转换为文本信息。

4.根据权利要求1所述的一种基于自然语言理解的机器人控制方法，其特征在于步骤S2所述文本信息提取及分割具体包括：

在用户意图理解的过程中，首要步骤是从文本信息中采集出与机器人控制指令所关联的文本片段；在一段文本信息中，既存在与机器人控制指令相关的关键文本信息，也存在对一段文本信息的理解无关的冗余信息；关键文本信息包括动作词、物品的名字、物品的属性、位置词，而冗余信息则包括语气助词、无关字词；通过建立模型分别匹配这些关键信息和冗余信息；对于一段文本信息，分别对此文本信息中的关键信息和冗余信息进行匹配，而后插入分割符对关键信息和冗余信息进行分割。

5.根据权利要求1所述的一种基于自然语言理解的机器人控制方法，其特征在于步骤S2所述文本信息分类具体包括：

得到文本分割的结果之后，需要建立自然语言指令和对应机器人控制指令之间的映射关系，这个过程能看作是一个分类问题，利用基于最大熵模型的分类器来解决；

采用向量空间模型来表示文本特征：针对训练语料库，统计库中所有出现的词；假设一语料文本中有N个词，那么该文本就表示为N维的特征向量；利用TF‐IDF在分类之前进行特征向量加权；

假设x为文本特征向量，对应的意图输出标签为Y；最大熵算法就是对条件概率P(ylx)进行建模，得到分布最均匀的模型，这是一个最优化求解问题；引入条件熵H(p)来衡量条件概率P(ylx)分布的均匀性，根据香农对熵的定义，H(p)计算公式为：

其中，p(x)表示在训练数据库中文本特征向量的经验分布，P(ylx)为所要求解的模型中的条件概率分布；那么，求解最大熵模型的公式为：

p_*＝arg max H(p)

接下来需要确定约束条件；给定一个训练数据库，文本特征向量集合记作{x_i,i∈(1,N)}，意图标签集合记作{y_j,j∈(1,M)}；这样就能统计“特征‐意图”二元组(x,y)的经验概率分布

其中，num(x,y)表示(x,y)在训练数据库中出现的次数，N表示训练数据库中样本总数；特征函数f(x,y)相对于经验条件概率分布p(x,y)的期望值如公式：

f(x,y)相对于模型条件概率分布P(y|x)的期望值如公式：

在训练数据库中，令两个期望值相同，即得到最大熵模型求解的约束条件如公式：

综上，最大熵的求解问题可以归纳为如下最优化问题如公式：

max H(p)

根据拉格朗日乘子算法，可以求解得到概率分布p^*，在Kulback‐Leibler距离上是最接近经验概率分布p(x,y)的如公式：

公式中，p^*为求解的最大熵概率分布，f_i(x,y)为第i个特征函数，λ_i为(x,y)的权值，n为特征函数的个数，Z(x)为归一化因子；通过在训练数据库上的学习，可以得到参数λ_i的值，从而得到要求解的概率分布p^*；为了求得参数λ_i，采用GIS算法求解，完成最大熵模型的建立。

6.根据权利要求1所述的一种基于自然语言理解的机器人控制方法，其特征在于步骤S2所述控制指令转换包括：

根据最大熵模型得到的分类结果，将自然语言指令转换为相应机器人控制指令；通过引入3种属性变量(V_op，V_key，V_val)来定义了机器人控制指令。

7.根据权利要求1所述的一种基于自然语言理解的机器人控制方法，其特征在于所述步骤S3包括以下步骤：

通过已经得到了机器人的控制指令，控制工业机械臂所需的运动参数为各个关节角的数值，而通过自然语言指令转换为的机器人控制指令所给出的参数为工业机械臂末端的空间坐标；实现对机器人的控制需要用到反解算法：

以六自由度机械臂基座为原点，垂直方向为z轴建立基坐标，通常一个六自由度机械臂的末端执行器相对于基座的总变换矩阵可表示为：

式中:为机械臂末端执行器在基坐标系(x，y，z)中的位姿矩阵，[p^x,p^y,p^z]^T为机械臂末端执行器在基坐标系中的位置，[n^x,n^y,n^z]^T为机械臂末端执行器坐标系的x轴在基坐标系中的方向矢量，[o^x,o^y,o^z]^T为机械臂末端执行器坐标系的y轴在基坐标系中的方向矢量，[a^x,a^y,a^z]^T为机械臂末端执行器坐标系的z轴在基坐标系中的方向矢量；

其中A₁,A₂,A₃,A₄,A₅,A₆分别表示每个关节对应的矩阵，分别可由对应的机器人的参数求得；

通过矩阵联立求解即可通过机械臂末端执行器的空间坐标求得旋转关节的关节变量。