CN112536792B - 一种基于球面几何解析法的机器臂逆解方法 - Google Patents

Abstract

本申请的目的是提供一种基于球面几何解析法的机器臂逆解方法，本申请通过基于六轴机器臂的姿态信息建立所述六轴机器臂的连杆坐标系模型；获取末端坐标系相对基坐标系的末端齐次变换矩阵；基于所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到运动学逆解，即通过建立所述六轴机器臂D‑H模型，采用结合球面几何的解析法分析其逆运动学方程式，在机器人操作系统(ROS，Robot Operating System)中进行代码的编写最终求得运动学逆解，将解析法的计算快速性与球面求解的灵活性结合，提高求解的快速性同时也提高了机器臂姿态的灵活性。

Description

一种基于球面几何解析法的机器臂逆解方法

技术领域

本申请涉及计算机领域，尤其涉及一种基于球面几何解析法的机器臂逆解方法。

背景技术

现有技术中，机械臂被广泛应用于机械制造、航空航天、医疗和原子能等领域，机械臂的逆运动学问题是其轨迹规划与控制的重要基础，逆运动学求解是否快速准确将直接影响到机械臂轨迹规划与控制的精度。机器臂逆运动学是根据已知末端执行器的位姿(位置和姿态)求解机器臂各关节的变量。传统的逆运动学求解方法有解析法、迭代法，智能方法有遗传算法、神经网络算法等，这些求解所得姿态较为单一，且计算速度及求解率不能满足脊柱检测用机器臂的特定需要。

因此，提高机器臂在逆运行方程求解过程中的求解率和快速性是本领域技术人员需要继续研究的方向。

发明内容

本申请的一个目的是提供一种基于球面几何解析法的机器臂逆解方法，以解决现有技术中如何提高机器臂逆运动学求解的精度和速度，以及增加求解结果的多样性的问题。

根据本申请的一个方面，提供了一种基于球面几何解析法的机器臂逆解方法，其特征在于，所述方法包括：

基于六轴机器臂的姿态信息建立所述六轴机器臂的连杆坐标系模型；

获取末端坐标系相对基坐标系的末端齐次变换矩阵；

基于所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到运动学逆解。

进一步地，上述基于球面几何解析法的机器臂逆解方法中，所述基于六轴机器臂的姿态信息建立所述六轴机器臂的连杆坐标系模型之后，还包括：

获取所述连杆坐标系模型的所述已知模型参数；

所述已知模型参数包括连杆转角、连杆长度、连杆偏距。

进一步地，上述基于球面几何解析法的机器臂逆解方法中，所述基于所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到运动学逆解，包括：

基于所述末端齐次变换矩阵以及已知模型参数，根据相对坐标系位置变换求解得到所述六轴机器臂的第一关节角和第五关节角；

根据所述第一关节角、所述第五关节角、所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到所述六轴机器臂的第六关节角；

基于所述第一关节角、所述第五关节角、所述第六关节角以及末端齐次变换矩阵，结合第二旋转轴、第三旋转轴以及第四旋转轴之间的平行关系计算得到第二关节角、第三关节角以及第四关节角。

进一步地，上述基于球面几何解析法的机器臂逆解方法中，所述基于所述末端齐次变换矩阵以及已知模型参数，根据相对坐标系位置变换求解得到所述六轴机器臂第一关节角和第五关节角，包括：

将第五坐标系相对于基坐标系进行位置变换，并根据所述末端齐次变换矩阵以及所述已知模型参数中第二连杆偏距计算得到所述第一关节角，计算公式为：

其中，⁰p_5x表示第五坐标系相对基坐标系在x轴方向的投影，单位为m；⁰p_5y表示第五坐标系相对基坐标系在y轴方向的投影，单位为m；d₂为所述第二连杆偏距，即表示沿Z2轴X1移动到X2的距离，单位为m；

将第六坐标系相对于基坐标系进行位置变换，并根据所述末端齐次变换矩阵以及所述已知模型参数中第六连杆偏距和所述第二连杆偏距计算得到第五关节角，计算公式为：

|⁰p_6y-d₄|≤d₆

其中，⁰p_6x表示第六坐标系相对基坐标系在x轴方向的投影，单位为m；⁰p_6y表示第六坐标系相对基坐标系在y轴方向的投影，单位为m；d₆为所述第六连杆偏距，即表示沿Z6轴X5移动到X6的距离，单位为m；d₄为所述第六连杆偏距，即表示沿Z6轴X5移动到X6的距离，单位为m。

进一步地，上述基于球面几何解析法的机器臂逆解方法中，所述根据所述第一关节角、所述第五关节角、所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到第六关节角，包括：

以第六坐标系建立球面坐标系；

根据所述第一关节角、所述第五关节角、所述末端齐次变换矩阵以及所述已知模型参数，通过球形几何解析算法得到第六关节角，计算公式为：

Y₁表示第一坐标系中的Y轴，使用

表示将Y₁映射到第六坐标系中的Y₁，使用球面坐标系表示

得到公式：

使用已知的

表示

得到公式：

θ₆＝atan2(w1，w2)其中，

其中，

表示

的第一列中的x(第一个元素)值；

表示

的第一列中的y(第二个元素)值；

表示

的第一列中的z(第三个元素)值；

表示

的第一行中的x(第一个元素)值；

表示

的第一行中的y(第二个元素)值；

表示

的第一行中的z(第三个元素)值。

进一步地，上述基于球面几何解析法的机器臂逆解方法中，所述步骤基于所述第一关节角、所述第五关节角、所述第六关节角以及末端齐次变换矩阵，结合第二旋转轴、第三旋转轴以及第四旋转轴之间的平行关系计算得到第三关节角、第二关节角以及第四关节角，包括：

基于所述第一关节角、所述第五关节角、所述第六关节角得到第一坐标系相对基坐标系齐次变换矩阵、第五坐标系相对于第四坐标系齐次变换矩阵、第六坐标系相对于第五坐标系齐次变换矩阵，同时结合所述末端齐次变换矩阵通过几何关系求解得到所述第三关节角和所述第二关节角，公式如下：

其中，a2表示第三连杆长度；a3表示第四连杆长度；¹p_4z表示第四坐标系相对基坐标系在z轴方向的投影，单位为m；¹p_4x表示第四坐标系相对基坐标系在x轴方向的投影，单位为m；

根据所述第一关节角、所述第五关节角、所述第六关节角、所述第三关节角以及所述第二关节角得到第四坐标系相对于第三坐标系齐次变换矩阵，取所述第四坐标系相对于第三坐标系齐次变换矩阵的第一列通过计算得到所述第四关节角，公式如下：

其中，

表示

的第一列中的y(第二个元素)值；

表示

的第一列中的x(第一个元素)值。

与现有技术相比，本申请通过基于六轴机器臂的姿态信息建立所述六轴机器臂的连杆坐标系模型；获取末端坐标系相对基坐标系的末端齐次变换矩阵；基于所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到运动学逆解，即通过建立所述六轴机器臂D-H模型，采用结合球面几何的解析法分析其逆运动学方程式，在机器人操作系统(ROS，Robot Operating System)中进行代码的编写最终求得运动学逆解，将解析法的计算快速性与球面求解的灵活性结合，提高求解的快速性同时也提高了机器臂姿态的灵活性。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：

图1示出根据本申请的一种基于球面几何解析法的机器臂逆解方法的流程示意图；

图2示出根据本申请的一种基于球面几何解析法的机器臂逆解方法中连杆坐标系模型示意图；

图3示出根据本申请的一种基于球面几何解析法的机器臂逆解方法中第五坐标系相对基坐标系位置示意图；

图4示出根据本申请的一种基于球面几何解析法的机器臂逆解方法中基坐标系与第五坐标系几何关系示意图；

图5示出根据本申请的一种基于球面几何解析法的机器臂逆解方法中基坐标系与第六坐标系几何关系示意图；

图6示出根据本申请的一种基于球面几何解析法的机器臂逆解方法中球面坐标系示意图；

图7示出根据本申请的一种基于球面几何解析法的机器臂逆解方法中球面坐标系中第二旋转轴、第四旋转轴、第三旋转轴以及第六旋转轴几何关系示意图；

图8示出根据本申请的一种基于球面几何解析法的机器臂逆解方法中第二、第三、第四坐标系关系示意图。

附图中相同或相似的附图标记代表相同或相似的部件。

具体实施方式

下面结合附图对本申请作进一步详细描述。

在本申请一个典型的配置中，终端、服务网络的设备和可信方均包括一个或多个处理器(例如中央处理器(Central Processing Unit，CPU)、输入/输出接口、网络接口和内存。

内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RandomAccess Memory，RAM)和/或非易失性内存等形式，如只读存储器(Read Only Memory，ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(Phase-Change RAM，PRAM)、静态随机存取存储器(Static Random Access Memory，SRAM)、动态随机存取存储器(Dynamic RandomAccess Memory，DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(Electrically Erasable Programmable Read-Only Memory，EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(Compact Disc Read-Only Memory，CD-ROM)、数字多功能光盘(Digital Versatile Disk，DVD)或其他光学存储、磁盒式磁带，磁带磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括非暂存电脑可读媒体(transitorymedia)，如调制的数据信号和载波。

图1示出根据本申请的一种基于球面几何解析法的机器臂逆解方法的流程示意图，该方法应用于逆运动学求解机器臂各关节的变量，该方法包括步骤S11、步骤S12及步骤S13，其中，具体包括：

步骤S11，基于六轴机器臂的姿态信息建立所述六轴机器臂的连杆坐标系模型，在此，所述六轴机器臂的连杆坐标系模型中基坐标系{0}与世界坐标系重合，为方便计算将第二坐标系、第三坐标系、第四坐标系、第五坐标系的原点位置建立在同一铅垂线上，如图2所示。

步骤S12，获取末端坐标系相对基坐标系的末端齐次变换矩阵。

步骤S13，基于所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到运动学逆解，在此，所述已知模型参数包括但不限于连杆转角、连杆长度、连杆偏距、关节角以及关节角的角度范围。

上述步骤S11至步骤S13，通过基于六轴机器臂的姿态信息建立所述六轴机器臂的连杆坐标系模型；获取末端坐标系相对基坐标系的末端齐次变换矩阵；基于所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到运动学逆解，即通过建立所述六轴机器臂D-H模型，采用结合球面几何的解析法分析其逆运动学方程式，在机器人操作系统(ROS，Robot Operating System)中进行代码的编写最终求得运动学逆解，将解析法的计算快速性与球面求解的灵活性结合，提高求解的快速性同时也提高了机器臂姿态的灵活性。

例如，六轴机器臂为AUBO-I5型机器臂，首先，基于所述六轴机器臂的姿态信息建立其机器臂连杆坐标系模型，其中基坐标系{0}与世界坐标系重合，坐标系{2}、{3}、{4}、{5}的原点位置建立在同一铅垂线上；然后，获取末端坐标系相对基坐标系的末端齐次变换矩阵

(末端的TCP位姿)；最后，基于所述末端齐次变换矩阵

以及已知模型参数(连杆转角、连杆长度、连杆偏距以及关节角的角度范围)，通过球形几何解析算法在ROS中进行代码的编写最终计算得到八组不同的六轴机器臂的运动学逆解，将解析法的计算快速性与球面求解的灵活性结合，提高了求解速度、求解姿态多样性以及求解成功率。

接着本申请的上述实施例，所述步骤S11基于六轴机器臂的姿态信息建立所述六轴机器臂的连杆坐标系模型之后，还包括：

获取所述连杆坐标系模型的所述已知模型参数；

所述已知模型参数包括但不限于连杆转角、连杆长度、连杆偏距。

例如，基于六轴机器臂的姿态信息建立D-H模型，获取所述D-H模型的已知模型参数，已知模型参数包括连杆转角、连杆长度、连杆偏距以及关节角的度数范围，以便于进一步计算所述六轴机器臂的运动学逆解完成步骤S12以及步骤S13，如下为D-H已知模型参数表：

其中，θ_i表示第i个关节的关节角度，di表示沿Zi轴X_i-1移动到Xi的距离即连杆偏距，a_i-1表示沿Xi轴Zi移动到Z_i+1的距离即连杆长度，α_i表示绕着Xi轴Zi旋转到Z_i+1的角度即连杆转角。

接着本申请的上述实施例，所述步骤S13基于所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到运动学逆解，包括：

步骤S131，基于所述末端齐次变换矩阵以及已知模型参数，根据相对坐标系位置变换求解得到所述六轴机器臂的第一关节角和第五关节角；

步骤S132，根据所述第一关节角、所述第五关节角、所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到所述六轴机器臂的第六关节角；

步骤S133，基于所述第一关节角、所述第五关节角、所述第六关节角以及末端齐次变换矩阵，结合第二旋转轴、第三旋转轴以及第四旋转轴之间的平行关系计算得到第二关节角、第三关节角以及第四关节角。在此，该算法对关节角求解顺序依次为θ₁、θ₅、θ₆、θ₃、θ₂、θ₄。求解的顺序根据已知条件优先求出最易求得的解θ₁、θ₅，再据此建立球面坐标系推出球面坐标系的θ₆，最后再求出三连杆关系的θ₃、θ₂、θ₄。即实现了六轴机器臂的逆运动学求解，提高了逆运动学求解的速度、精确度。

例如，经过结合球面几何的解析法计算得到：θ₁有两个解分别为θ₁₁、θ₁₂；θ₅有两个解分别为为θ₅₁、θ₅₂；θ₆有一个解为θ₆；θ₃有两个解分别为θ₃₁、θ₃₂；θ₂有一个解为θ₂；θ₄有一个解为θ₄；最终得到六轴机器臂的逆运动学解有八组解，分别是：

第一组解：θ₁₁ θ₅₁ θ₆ θ₃₁ θ₂ θ₄；

第二组解：θ₁₁ θ₅₁ θ₆ θ₃₂ θ₂ θ₄；

第三组解：θ₁₁ θ₅₂ θ₆ θ₃₁ θ₂ θ₄；

第四组解：θ₁₁ θ₅₂ θ₆ θ₃₂ θ₂ θ₄；

第五组解：θ₁₂ θ₅₁ θ₆ θ₃₁ θ₂ θ₄；

第六组解：θ₁₂ θ₅₁ θ₆ θ₃₂ θ₂ θ₄；

第七组解：θ₁₂ θ₅₂ θ₆ θ₃₁ θ₂ θ₄；

第八组解：θ₁₂ θ₅₂ θ₆ θ₃₂ θ₂ θ₄；

即实现了六轴机器臂的逆运动学求解，提高了逆运动学求解的速度、精确度以及求解姿态多样性。

接着本申请的上述实施例，所述步骤S131基于所述末端齐次变换矩阵以及已知模型参数，根据相对坐标系位置变换求解得到所述六轴机器臂第一关节角和第五关节角，包括：

将第五坐标系相对于基坐标系进行位置变换，并根据所述末端齐次变换矩阵以及所述已知模型参数中第二连杆偏距计算得到所述第一关节角，提高了逆运动学求解效率，具体计算步骤如下：

如图3所示，第五坐标系相对基座标系其位置变换为⁰p₅，且第五坐标系由第六坐标系沿着Z₆变换得到；已知末端齐次变换矩阵

和第二连杆偏距d₂，即

如图4所示，基坐标系相对第五坐标系的几何关系中，

其中：

得：

其中，⁰p_5x表示第五坐标系相对基坐标系在x轴方向的投影，单位为m；⁰p_5y表示第五坐标系相对基坐标系在y轴方向的投影，单位为m；d₂为所述第二连杆偏距，即表示沿Z2轴X1移动到X2的距离，单位为m。

接着，将第六坐标系相对于基坐标系进行位置变换，并根据所述末端齐次变换矩阵以及所述已知模型参数中第六连杆偏距和所述第二连杆偏距计算得到第五关节角，具体计算步骤如下：

基坐标系相对第六坐标的几何关系如图5所示，由几何关系得¹p_6y：

-¹p_6y＝d₂+d₆cosθ₅

由坐标系关系也可获得：

¹p_6y＝⁰p_6x·-sin(θ₁)+⁰p_6y·cos(θ₁)

联立上述公式得：

|⁰p_6y-d₄|≤d₆

其中，⁰p_6x表示第六坐标系相对基坐标系在x轴方向的投影，单位为m；⁰p_6y表示第六坐标系相对基坐标系在y轴方向的投影，单位为m；d₆为所述第六连杆偏距，即表示沿Z6轴X5移动到X6的距离，单位为m；d₄为所述第六连杆偏距，即表示沿Z6轴X5移动到X6的距离，单位为m。

接着本申请的上述实施例，所述步骤S132根据所述第一关节角、所述第五关节角、所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析

算法得到第六关节角，包括：

以第六坐标系建立球面坐标系；

根据所述第一关节角、所述第五关节角、所述末端齐次变换矩阵以及所述已知模型参数，通过球形几何解析算法得到第六关节角，具体计算过程如下：

如图6所示，-Y₁与Z₄重合，

表示在球面坐标系轴Y₁，

是由方位角-θ₆和极值角θ₅表示。

Y₁表示第一坐标系中的Y轴，使用

表示将Y₁映射到第六坐标系中的Y₁，将球面坐标

转换到笛卡尔坐标系中表示：

同时，

也可使用已知的

表达，Y₁是在{0}坐标系中x/y平面旋转了θ₁，得：

联立上述公式的前两项得：

得θ₆：

θ₆＝a tan2(w1，w2)其中，

其中，

表示

的第一列中的x(第一个元素)值；

表示

的第一列中的y(第二个元素)值；

表示

的第一列中的z(第三个元素)值；

表示

的第一行中的x(第一个元素)值；

表示

的第一行中的y(第二个元素)值；

表示

的第一行中的z(第三个元素)值。在求解θ₆的公式中sinθ₅不能为0，否则无解。在此情况下，关节的第二旋转轴、第三旋转轴、第四旋转轴与第六旋转轴对齐，如图7所示。

接着本申请的上述实施例，所述步骤S133基于所述第一关节角、所述第五关节角、所述第六关节角以及末端齐次变换矩阵，结合第二旋转轴、第三旋转轴以及第四旋转轴之间的平行关系计算得到第三关节角、第二关节角以及第四关节角，包括：

基于所述第一关节角、所述第五关节角、所述第六关节角得到第一坐标系相对基坐标系齐次变换矩阵、第五坐标系相对于第四坐标系齐次变换矩阵、第六坐标系相对于第五坐标系齐次变换矩阵，同时结合所述末端齐次变换矩阵通过几何关系求解得到所述第三关节角和所述第二关节角，具体计算过程如下：

如图8所示，第二旋转轴、第三旋转轴以及第四旋转轴是平行关系，既既可以将其看成一个3连杆机器臂的几何求解问题。

至此已经计算出

和

且已知

进而求得

从而得到¹p_4xx、¹p_4z和¹p_4x，从图8中可以看出|¹p_4xz|的值仅由θ₃确定，或者说仅有

间接决定，角度

可以通过余弦定律得到，进而得：

从图8中可以得到：

且由于

可以用θ₃代替

进而得到：

其中，a2表示第三连杆长度；a3表示第四连杆长度；¹p_4z表示第四坐标系相对基坐标系在z轴方向的投影，单位为m；¹p_4x表示第四坐标系相对基坐标系在x轴方向的投影，单位为m。

接下来，根据所述第一关节角、所述第五关节角、所述第六关节角、所述第三关节角以及所述第二关节角得到第四坐标系相对于第三坐标系齐次变换矩阵，取所述第四坐标系相对于第三坐标系齐次变换矩阵的第一列通过计算得到所述第四关节角，公式如下：

其中，

表示

的第一列中的y(第二个元素)值；

表示

的第一列中的x(第一个元素)值，即将解析法的计算快速性与球面求解的灵活性结合求得机器臂的逆运动学解，提高求解的快速性同时也提高了机器臂姿态的灵活性。

综上所述，本申请通过基于六轴机器臂的姿态信息建立所述六轴机器臂的连杆坐标系模型；获取末端坐标系相对基坐标系的末端齐次变换矩阵；基于所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到运动学逆解，即通过建立所述六轴机器臂D-H模型，采用结合球面几何的解析法分析其逆运动学方程式，在机器人操作系统(ROS，Robot Operating System)中进行代码的编写最终求得运动学逆解，将解析法的计算快速性与球面求解的灵活性结合，提高求解的快速性同时也提高了机器臂姿态的灵活性。

需要注意的是，本申请可在软件和/或软件与硬件的组合体中被实施，例如，可采用专用集成电路(ASIC)、通用目的计算机或任何其他类似硬件设备来实现。在一个实施例中，本申请的软件程序可以通过处理器执行以实现上文所述步骤或功能。同样地，本申请的软件程序(包括相关的数据结构)可以被存储到计算机可读记录介质中，例如，RAM存储器，磁或光驱动器或软磁盘及类似设备。另外，本申请的一些步骤或功能可采用硬件来实现，例如，作为与处理器配合从而执行各个步骤或功能的电路。

另外，本申请的一部分可被应用为计算机程序产品，例如计算机程序指令，当其被计算机执行时，通过该计算机的操作，可以调用或提供根据本申请的方法和/或技术方案。而调用本申请的方法的程序指令，可能被存储在固定的或可移动的记录介质中，和/或通过广播或其他信号承载媒体中的数据流而被传输，和/或被存储在根据所述程序指令运行的计算机设备的工作存储器中。在此，根据本申请的一个实施例包括一个装置，该装置包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器，其中，当该计算机程序指令被该处理器执行时，触发该装置运行基于前述根据本申请的多个实施例的方法和/或技术方案。

对于本领域技术人员而言，显然本申请不限于上述示范性实施例的细节，而且在不背离本申请的精神或基本特征的情况下，能够以其他的具体形式实现本申请。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本申请的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本申请内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。装置权利要求中陈述的多个单元或装置也可以由一个单元或装置通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。

Claims (3)

1.一种基于球面几何解析法的机器臂逆解方法，其特征在于，所述方法包括：

基于六轴机器臂的姿态信息建立所述六轴机器臂的连杆坐标系模型，获取所述连杆坐标系模型的已知模型参数，所述已知模型参数包括连杆转角、连杆长度、连杆偏距；

获取末端坐标系相对基坐标系的末端齐次变换矩阵；

基于所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到运动学逆解，包括：

基于所述末端齐次变换矩阵以及已知模型参数，根据相对坐标系位置变换求解得到所述六轴机器臂第一关节角和第五关节角，包括：

将第五坐标系相对于基坐标系进行位置变换，并根据所述末端齐次变换矩阵以及所述已知模型参数中第二连杆偏距计算得到所述第一关节角，计算公式为：

其中，⁰p5x表示第五坐标系相对基坐标系在x轴方向的投影，单位为m；⁰p5y表示第五坐标系相对基坐标系在y轴方向的投影，单位为m；d₂为所述第二连杆偏距，即表示沿Z2轴X1移动到X2的距离，单位为m；

将第六坐标系相对于基坐标系进行位置变换，并根据所述末端齐次变换矩阵以及所述已知模型参数中第六连杆偏距和所述第二连杆偏距计算得到第五关节角，计算公式为：

|⁰p6y-d₄|≤d₆

其中，⁰p6x表示第六坐标系相对基坐标系在x轴方向的投影，单位为m；⁰p6y表示第六坐标系相对基坐标系在y轴方向的投影，单位为m；d₆为所述第六连杆偏距，即表示沿Z6轴X5移动到X6的距离，单位为m；d₄为所述第六连杆偏距，即表示沿Z6轴X5移动到X6的距离，单位为m；

根据所述第一关节角、所述第五关节角、所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到所述六轴机器臂的第六关节角；

基于所述第一关节角、所述第五关节角、所述第六关节角以及末端齐次变换矩阵，结合第二旋转轴、第三旋转轴以及第四旋转轴之间的平行关系计算得到第二关节角、第三关节角以及第四关节角。

2.根据权利要求1所述的方法，其特征在于，所述根据所述第一关节角、所述第五关节角、所述末端齐次变换矩阵以及已知模型参数，通过球形几何解析算法得到第六关节角，包括：

以第六坐标系建立球面坐标系；

根据所述第一关节角、所述第五关节角、所述末端齐次变换矩阵以及所述已知模型参数，通过球形几何解析算法得到第六关节角，计算公式为：

Y₁表示第一坐标系中的Y轴，使用

表示将Y₁映射到第六坐标系中的Y₁，使用球面坐标系表示

得到公式：

使用已知的

表示

得到公式：

θ₆＝atan2(w1，w2)其中，

其中，

表示

的第一列中的x(第一个元素)值；

表示

的第一列中的y(第二个元素)值；

表示

的第一列中的z(第三个元素)值；

表示

的第一行中的x(第一个元素)值；

表示

的第一行中的y(第二个元素)值；

表示

的第一行中的z(第三个元素)值。

3.根据权利要求1所述的方法，其特征在于，所述基于所述第一关节角、所述第五关节角、所述第六关节角以及末端齐次变换矩阵，结合第二旋转轴、第三旋转轴以及第四旋转轴之间的平行关系计算得到第三关节角、第二关节角以及第四关节角，包括：

基于所述第一关节角、所述第五关节角、所述第六关节角得到第一坐标系相对基坐标系齐次变换矩阵、第五坐标系相对于第四坐标系齐次变换矩阵、第六坐标系相对于第五坐标系齐次变换矩阵，同时结合所述末端齐次变换矩阵通过几何关系求解得到所述第三关节角和所述第二关节角，公式如下：

其中，a2表示第三连杆长度；a3表示第四连杆长度；¹p4z表示第四坐标系相对基坐标系在z轴方向的投影，单位为m；¹p4x表示第四坐标系相对基坐标系在x轴方向的投影，单位为m；

根据所述第一关节角、所述第五关节角、所述第六关节角、所述第三关节角以及所述第二关节角得到第四坐标系相对于第三坐标系齐次变换矩阵，取所述第四坐标系相对于第三坐标系齐次变换矩阵的第一列通过计算得到所述第四关节角，公式如下：

其中，

表示

的第一列中的y(第二个元素)值；

表示

的第一列中的x(第一个元素)值。

Images