CN113127989A - 一种六自由度机械臂逆运动学解析求解控制方法 - Google Patents

Abstract

本发明涉及一种六自由度机械臂逆运动学解析求解控制方法。包括：步骤1：根据机械臂的构型特征参数建立运动学模型；步骤2：给定机械臂末端位姿，根据机械臂的逆运动学求解方法依次求解各关节角度，完成运动位置解算；步骤3：将各关节角度的变化指令输入给各关节驱动器，驱动控制器控制各个关节的电机转动使得各个关节角达到目标角度，实现机械臂的轨迹运动控制。本发明针对运动学逆解算法，控制机械臂各个关节运动，使机械臂末端运动符合规划控制的轨迹要求。

Description

一种六自由度机械臂逆运动学解析求解控制方法

技术领域

本发明属于机器人运动学解算控制领域，更具体的说是一种六自由度机械臂逆运动学解析求解控制方法。

背景技术

机器人的运动学逆解，是指给定机器人末端笛卡尔空间位置与姿态，求解其关节角度。机器人的运动学逆解是其进行轨迹规划与运动控制的基础，解算方法的速度、精度直接决定了机器人运动控制性能。机器人运动学逆解主要的方法有解析法与数值法。解析法可以通过代数表达式直接计算关节角度，简便高效速度快，是大多数工业机器人运动求解控制采用的方法。数值法是通过数值迭代求解关节角度，计算过程较繁琐，速度较慢，而且通常不能得到全部解。工业机器人通常选择腕关节三个轴相交于一点或连续三个轴平行的构型，以获得解析形式的运动学逆解。

目前工业机器人领域有若干成熟的机器人构型，这些构型的机器人逆解大多都有可参考的算法，但这些构型并不一定适用于所有场合。机器人构型稍作变动，其逆解就会完全不同，因此对特殊构型的机器人，需要研究其运动学逆解的解算问题，从而控制机器人各个关节运动。我国在建的空间站科学手套箱中，布置了一套六自由度机械臂，为了实现最优的操作性能，其构型不同于现有成熟的工业机器人构型，是一种特殊构型，为了实现在轨高效、实时、高精度的运动控制效果，需要对其提出一种运动学逆解的解析求解控制方法。

发明内容

本发明通过提出一种六自由度机械臂逆运动学解析求解控制方法，给出机械臂运动学逆解的代数表达式，达到实现空间站手套箱机械臂的实时高效运动控制的目的。

本发明为实现上述目的所采用的技术方案是：一种六自由度机械臂逆运动学解析求解控制方法，包括以下步骤：

步骤1：根据机械臂的构型特征参数建立运动学模型；

步骤2：给定机械臂末端位姿，根据机械臂的逆运动学求解方法依次求解各关节角度，完成运动位置解算；

步骤3：将各关节角度的变化指令输入给各关节驱动器，驱动控制器控制各个关节的电机转动使得各个关节角达到目标角度，实现机械臂的轨迹运动控制。

所述六自由度机械臂的构型包括6个转动关节，第1个关节为圆弧导轨形成的转动关节，后5个关节为模块化转动关节，关节变量分别为角度θ₁、θ₂、θ₃、θ₄、θ₅以及θ₆；其中，第2关节的轴线与第1关节的轴线平行，第3关节的轴线与第2关节的轴线垂直，第4关节、第5关节的轴线与第3关节的轴线平行，第6关节的轴线与第5关节的轴线垂直。

所述机械臂的构型特征参数包括：第i个连杆长度a_i、连杆扭角α_i、连杆偏距d_i、关节角θ_i以及各轴坐标系与基坐标系之间的转换关系。

所述运动学模型为：

根据机械臂的末端坐标系x₆y₆z₆相对于其基坐标系x₀y₀z₀的位姿变换关系

由前向运动学方程求得，建立如下方程：

其中，

为机械臂末端的位姿在基坐标系中的表示；设已知

为：

其中，

表示坐标系x₆y₆z₆的三个坐标轴矢量相对于基坐标系x₀y₀z₀的方向余弦组成的3×3矩阵，p_x、p_y、p_z为坐标系x₆y₆z₆的原点在基坐标系x₀y₀z₀的三个坐标轴上的位置。

所述求解各关节角度，完成运动位置解算包括：

2.1求解θ₆、θ₃+θ₄+θ₅、θ₁+θ₂

对式(2)进行变换，得到

2.1.1求解θ₆

由

的第3行第2列元素对应相等：

(1)当o_z、n_z不同时为零时，可以得到两组解：

θ₆＝a tan 2(o_z,-n_z) (6)

或：

θ₆＝a tan 2(o_z,-n_z)-π (7)

(2)当o_z、n_z同时为零时，θ₆可取任意值，但需要满足使其它关节有解。

2.1.2求解θ₃+θ₄+θ₅

用θ₃₄₅代替(θ₃+θ₄+θ₅)，由

的第3行第1列、第3行第3列元素分别对应相等，可得到：

θ₃₄₅＝a tan 2(n_zcosθ₆-o_zsinθ₆,-a_z) (10)

2.1.3求解θ₁+θ₂

用θ₁₂代替(θ₁+θ₂)，由

的第1行第2列、第2行第2列元素分别对应相等，可得到：

θ₁₂＝a tan 2(o_xcosθ₆+n_xsinθ₆,-o_ycosθ₆-n_ysinθ₆) (13)

2.2求解θ₂、θ₁、θ₄、θ₃、θ₅

对式(2)进行变换，得到

2.2.1求解θ₂、θ₁

由

的第3行第4列元素对应相等，则可解得θ₂：

θ₂＝a tan 2(Z₂,Y₂) (16)

其中，Z₂、Y₂分别为：

则θ₁为：

θ₁＝θ₁₂-θ₂ (19)

由于θ₆有两组解，根据式(13)可知θ₁₂也有两组解，由式(16)、(17)、(18)可知θ₂有四组解，由式(19)可知θ₁也有四组解；

2.2.2求解θ₄、θ₃、θ₅

由

的第1行第4列、第2行第4列元素分别对应相等，可解得θ₄：

其中：

上式中C₁、C₂分别为：

C₁＝p_xcosθ₁₂+p_ysinθ₁₂-a₁cosθ₂-a_xd₆cosθ₁₂-a_yd₆sinθ₁₂ (24)

C₂＝p_z-d₂-a_zd₆ (25)

由于θ₂有四组解，因此θ₄有八组解；

由式(20)、(21)可解得θ₃：

θ₃＝a tan 2(C₂(a₄cosθ₄+a₃)-C₁a₄sinθ₄,C₁(a₄cosθ₄+a₃)+C₂a₄sinθ₄) (26)

则θ₅为：

θ₅＝θ₃₄₅-θ₄-θ₃ (27)

至此，解出了所有的关节角度θ₁、θ₂、θ₃、θ₄、θ₅、θ₆，共有八组解，实际使用时根据需要进行取舍。

一种六自由度机械臂逆运动学解析求解控制系统，包括：上层的系统控制器、存储器与下层的各关节驱动控制器、驱动电机；

所述关节驱动控制器均放置于各关节内部，关节驱动控制器用于接受上层的系统控制器下发的运动指令，驱动关节运动，并向系统控制器反馈关节参数；

存储器存储有程序；系统控制器读取程序执行如上所述的方法步骤，实现六自由度机械臂的运动位置解算和运动轨迹控制。

所述系统控制器与各关节驱动控制器之间通过总线进行通讯，各关节驱动控制器均串联连接于总线上。

本发明与现有技术相比具有以下优点及有益效果：

1.本发明的求解控制方法适用于空间站手套箱机械臂的特殊构型：空间站手套箱为非规则、狭窄空间，为了实现其空间最大利用率与最优的操作性能，无法采用现有成熟的工业机器人构型，而是采用了一种特殊构型，需要研究针对性的运动学逆解算法，从而控制机械臂各个关节运动，使机械臂末端运动符合规划的轨迹要求。

2.本发明的求解控制方法解算更高效：相对于采用数值迭代解法，解析法能直接得到角度表达式，而且能得到所有不同解，显著提高了机械臂位姿的计算效率与轨迹规划控制的成功率。

附图说明

图1为一种六自由度机械臂的结构示意图；

图2为一种六自由度机械臂的运动学模型。

具体实施方式

为能对本发明的特征、目的及功能有更进一步的认识与了解，下面将结合实施例对本发明做进一步的详细说明。

本发明以空间站手套箱六自由度机械臂的构型为实施例，如图1所示，共包含6个转动关节，第1个关节为圆弧导轨形成的转动关节，后5个关节为模块化转动关节，关节变量分别为角度θ₁、θ₂、θ₃、θ₄、θ₅以及θ₆。从构型上说，第2关节的轴线与第1关节的轴线平行，第3关节的轴线与第2关节的轴线垂直，第4关节、第5关节的轴线与第3关节的轴线平行，第6关节的轴线与第5关节的轴线垂直。

位置解算的方法步骤包括如下两个步骤：

1.构建运动学方程：

构建运动学方程是指根据机械臂的构型特征，建立其运动学模型，规定其连杆坐标系，列写连杆参数表，并利用齐次变换矩阵列写机械臂的前向运动学方程。

空间站手套箱六自由度机械臂的结构示意图如图1所示，共包含6个转动关节，第1个关节为圆弧导轨形成的转动关节，后5个关节为模块化转动关节，关节变量分别为角度θ₁、θ₂、θ₃、θ₄、θ₅以及θ₆。从构型上说，第2关节的轴线与第1关节的轴线平行，第3关节的轴线与第2关节的轴线垂直，第4关节、第5关节的轴线与第3关节的轴线平行，第6关节的轴线与第5关节的轴线垂直。

根据机械臂的构型特征，建立运动学模型，机械臂连杆坐标系如图2所示，分别为x₀y₀z₀、x₁y₁z₁、x₂y₂z₂、x₃y₃z₃、x₄y₄z₄、x₅y₅z₅与x₆y₆z₆，对应的机械臂连杆参数如下表所示：

表1机械臂连杆参数

其中，a_i、α_i、d_i、θ_i分别表示第i个连杆长度、连杆扭角、连杆偏距、关节角。

机械臂第i个坐标系相对于第i-1个坐标系的位姿变换关系可表示为矩阵

形式：

机械臂的末端坐标系x₆y₆z₆相对于其基坐标系x₀y₀z₀的位姿变换关系

可由前向运动学方程求得，即：

其中，

为机械臂末端的位姿在基坐标系中的表示。机械臂的逆运动学求解，即已知

求解各关节角度θ₁、θ₂、θ₃、θ₄、θ₅与θ₆的过程。设已知

为：

其中，

表示坐标系x₆y₆z₆的三个坐标轴矢量相对于基坐标系x₀y₀z₀的方向余弦组成的3×3矩阵，p_x、p_y、p_z为坐标系x₆y₆z₆的原点在基坐标系x₀y₀z₀的三个坐标轴上的位置。

2.关节角度求解

在给定机械臂末端位姿情况下，根据机械臂的前向运动学方程，通过不同的变换计算，依次求出各关节角度。

2.1求解θ₆、θ₃+θ₄+θ₅、θ₁+θ₂

对式(2)进行变换，得到

2.1.1求解θ₆

由

的第3行第2列元素对应相等，得到：

o_z cosθ₆+n_z sinθ₆＝0 (5)

当o_z、n_z不同时为零时，可以得到两组解：

θ₆＝a tan 2(o_z,-n_z) (6)

或：

θ₆＝a tan 2(o_z,-n_z)-π (7)

当o_z、n_z同时为零时，θ₆可取任意值，但需要满足使其它关节有解。

2.1.2求解θ₃+θ₄+θ₅

用θ₃₄₅代替(θ₃+θ₄+θ₅)。由

的第3行第1列、第3行第3列元素分别对应相等，得到：

n_zcosθ₆-o_zsinθ₆＝sinθ₃₄₅ (8)

a_z＝-cosθ₃₄₅ (9)

由式(8)、(9)可得到：

θ₃₄₅＝a tan 2(n_zcosθ₆-o_zsinθ₆,-a_z) (10)

2.1.3求解θ₁+θ₂

用θ₁₂代替(θ₁+θ₂)。由

的第1行第2列、第2行第2列元素分别对应相等，得到：

sinθ₁₂＝o_xcosθ₆+n_xsinθ₆ (11)

-cosθ₁₂＝o_ycosθ₆+n_ysinθ₆ (12)由式(11)、(12)可得到：

θ₁₂＝a tan 2(o_xcosθ₆+n_xsinθ₆,-o_ycosθ₆-n_ysinθ₆) (13)

2.2求解θ₂、θ₁、θ₄、θ₃、θ₅

对式(2)进行变换，得到

2.2.1求解θ₂、θ₁

由

的第3行第4列元素对应相等，得到：

则可解得θ₂：

θ₂＝a tan 2(Z₂,Y₂) (16)

其中，Z₂、Y₂分别为：

则θ₁为：

θ₁＝θ₁₂-θ₂ (19)

由于θ₆有两组解，根据式(13)可知θ₁₂也有两组解，由式(16)、(17)、(18)可知θ₂有四组解，由式(19)可知θ₁也有四组解。

2.2.2求解θ₄、θ₃、θ₅

由

的第1行第4列、第2行第4列元素分别对应相等，得到：

a₄cosθ₃₄+a₃cosθ₃＝p_xcosθ₁₂+p_ysinθ₁₂-a₁cosθ₂-a_xd₆cosθ₁₂-a_yd₆sinθ₁₂ (20)

a₄sinθ₃₄+a₃sinθ₃＝p_z-d₂-a_zd₆ (21)

由式(20)、(21)可解得θ₄：

其中：

上式中C₁、C₂分别为：

C₁＝p_xcosθ₁₂+p_ysinθ₁₂-a₁cosθ₂-a_xd₆cosθ₁₂-a_yd₆sinθ₁₂ (24)

C₂＝p_z-d₂-a_zd₆ (25)

由于θ₂有四组解，因此θ₄有八组解。

由式(20)、(21)可解得θ₃：

θ₃＝a tan 2(C₂(a₄cosθ₄+a₃)-C₁a₄sinθ₄,C₁(a₄cosθ₄+a₃)+C₂a₄sinθ₄) (26)

则θ₅为：

θ₅＝θ₃₄₅-θ₄-θ₃ (27)

至此，解出了所有的关节角度θ₁、θ₂、θ₃、θ₄、θ₅、θ₆，共有八组解，实际使用时根据需要进行取舍。

3.运动控制：

将各关节角度的变化指令输入给各关节驱动器，驱动控制器控制各个关节的电机转动使得各个关节角达到目标角度，实现机械臂的轨迹运动控制。

上述方法的硬件控制系统包括：上层的系统控制器与下层的各关节驱动控制器(即驱动板卡，驱动板卡采用arm芯片)、驱动电机。关节驱动控制器均放置于关节内部，关节驱动控制器的作用是接受上层的系统控制器下发的运动指令，驱动关节运动，并向系统控制器反馈关节参数，包括关节电流、位置与速度等参数。系统控制器实现机械臂的运动规划、参数采集与监控，以及与更上一层控制器的交互通讯。系统控制器与各关节驱动控制器之间通过总线进行通讯，各关节驱动控制器均串联连接于总线上，实现线缆的最精简化设计与布置。

本发明所述的构建运动学方程和关节角度求解的方法步骤可以编写成程序化代码存储在存储介质中，系统控制器读取存储介质中的该程序代码执行构建运动学方程和关节角度求解的步骤，从而在给定机械臂末端位姿的情况下依次获取各个关节角，再发送指令给各个关节的驱动控制器(驱动板卡)，驱动控制器控制各个关节的电机转动使得各个关节角达到目标角度，完成一次位置解算和运动轨迹控制。

本发明提出的一种六自由度机械臂逆运动学解析解法，将用于我国空间站手套箱中的具有特殊构型的机械臂的运动控制中，利用解析解法求得各关节的角度，能保证运动控制中程序计算的高效性与实时性，规避了采用数值计算的复杂性，并且能得到多组不同解，大大提高了在存在障碍物空间内进行轨迹规划的成功率。因此，本发明在机械臂运动学逆解解算领域具备很强的竞争力，符合专利发明要求具备的新颖性、创造性、实用性。

按照上述实例，即可很好地实现本发明。需要说明的是，在本说明书的指导下本领域技术人员所做出的任何等同替代方法，或明显变形方式均在本发明的保护范围内。

Claims (7)

1.一种六自由度机械臂逆运动学解析求解控制方法，其特征在于，包括以下步骤：

步骤1：根据机械臂的构型特征参数建立运动学模型；

步骤2：给定机械臂末端位姿，根据机械臂的逆运动学求解方法依次求解各关节角度，完成运动位置解算；

步骤3：将各关节角度的变化指令输入给各关节驱动器，驱动控制器控制各个关节的电机转动使得各个关节角达到目标角度，实现机械臂的轨迹运动控制。

2.根据权利要求1所述的一种六自由度机械臂逆运动学解析求解控制方法，其特征在于，所述六自由度机械臂的构型包括6个转动关节，第1个关节为圆弧导轨形成的转动关节，后5个关节为模块化转动关节，关节变量分别为角度θ₁、θ₂、θ₃、θ₄、θ₅以及θ₆；其中，第2关节的轴线与第1关节的轴线平行，第3关节的轴线与第2关节的轴线垂直，第4关节、第5关节的轴线与第3关节的轴线平行，第6关节的轴线与第5关节的轴线垂直。

3.根据权利要求1所述的一种六自由度机械臂逆运动学解析求解控制方法，其特征在于，所述机械臂的构型特征参数包括：第i个连杆长度a_i、连杆扭角α_i、连杆偏距d_i、关节角θ_i以及各轴坐标系与基坐标系之间的转换关系。

4.根据权利要求1所述的一种六自由度机械臂逆运动学解析求解控制方法，其特征在于，所述运动学模型为：

根据机械臂的末端坐标系x₆y₆z₆相对于其基坐标系x₀y₀z₀的位姿变换关系

由前向运动学方程求得，建立如下方程：

其中，

为机械臂末端的位姿在基坐标系中的表示；设已知

为：

其中，

表示坐标系x₆y₆z₆的三个坐标轴矢量相对于基坐标系x₀y₀z₀的方向余弦组成的3×3矩阵，p_x、p_y、p_z为坐标系x₆y₆z₆的原点在基坐标系x₀y₀z₀的三个坐标轴上的位置。

5.根据权利要求4所述的一种六自由度机械臂逆运动学解析求解控制方法，其特征在于，所述求解各关节角度，完成运动位置解算包括：

2.1求解θ₆、θ₃+θ₄+θ₅、θ₁+θ₂

对式(2)进行变换，得到

2.1.1求解θ₆

由

的第3行第2列元素对应相等：

(1)当o_z、n_z不同时为零时，可以得到两组解：

θ₆＝a tan 2(o_z,-n_z) (6)

或：

θ₆＝a tan 2(o_z,-n_z)-π (7)

(2)当o_z、n_z同时为零时，θ₆可取任意值，但需要满足使其它关节有解。

2.1.2求解θ₃+θ₄+θ₅

用θ₃₄₅代替(θ₃+θ₄+θ₅)，由

的第3行第1列、第3行第3列元素分别对应相等，可得到：

θ₃₄₅＝a tan 2(n_zcosθ₆-o_z sinθ₆,-a_z) (10)

2.1.3求解θ₁+θ₂

用θ₁₂代替(θ₁+θ₂)，由

的第1行第2列、第2行第2列元素分别对应相等，可得到：

θ₁₂＝a tan 2(o_xcosθ₆+n_xsinθ₆,-o_ycosθ₆-n_ysinθ₆) (13)

2.2求解θ₂、θ₁、θ₄、θ₃、θ₅

对式(2)进行变换，得到

2.2.1求解θ₂、θ₁

由

的第3行第4列元素对应相等，则可解得θ₂：

θ₂＝a tan 2(Z₂,Y₂) (16)

其中，Z₂、Y₂分别为：

则θ₁为：

θ₁＝θ₁₂-θ₂ (19)

由于θ₆有两组解，根据式(13)可知θ₁₂也有两组解，由式(16)、(17)、(18)可知θ₂有四组解，由式(19)可知θ₁也有四组解；

2.2.2求解θ₄、θ₃、θ₅

由

的第1行第4列、第2行第4列元素分别对应相等，可解得θ₄：

其中：

上式中C₁、C₂分别为：

C₁＝p_xcosθ₁₂+p_ysinθ₁₂-a₁cosθ₂-a_xd₆cosθ₁₂-a_yd₆sinθ₁₂ (24)

C₂＝p_z-d₂-a_zd₆ (25)

由于θ₂有四组解，因此θ₄有八组解；

由式(20)、(21)可解得θ₃：

θ₃＝a tan 2(C₂(a₄cosθ₄+a₃)-C₁a₄sinθ₄,C₁(a₄cosθ₄+a₃)+C₂a₄sinθ₄) (26)

则θ₅为：

θ₅＝θ₃₄₅-θ₄-θ₃ (27)

至此，解出了所有的关节角度θ₁、θ₂、θ₃、θ₄、θ₅、θ₆，共有八组解，实际使用时根据需要进行取舍。

6.一种六自由度机械臂逆运动学解析求解控制系统包括：上层的系统控制器、存储器与下层的各关节驱动控制器、驱动电机；

所述关节驱动控制器均放置于各关节内部，关节驱动控制器用于接受上层的系统控制器下发的运动指令，驱动关节运动，并向系统控制器反馈关节参数；

存储器存储有程序；系统控制器读取程序执行如权利要求1-5任意一项所述的方法步骤，实现六自由度机械臂的运动位置解算和运动轨迹控制。

7.根据权利要求6所述的一种六自由度机械臂逆运动学解析求解控制方法，其特征在于，所述系统控制器与各关节驱动控制器之间通过总线进行通讯，各关节驱动控制器均串联连接于总线上。

Images