CN112549008B - 一种大型重载机械臂的投影运动学控制方法 - Google Patents

Abstract

本发明属于大型重载机器人领域，具体地说是一种大型重载机械臂的投影运动学控制方法。包括以下步骤：根据实际的机械臂建立为机械臂的三维几何模型；设置多个投影面；对该三维几何模型进行投影映射，分别得到关节J1～J4以及末端执行器末端点EE在各个投影面的投影；根据三维几何模型以及在各个投影面的投影，获取关节角θ₁，θ₂，θ₃，J4的伸长量以及与EE的对应关系。根据EE获取关节角θ₁、θ₂、θ₃的旋转角度，将该旋转角度分别作为关节电机的输入值，将采集的关节实际角度作为反馈值，通过闭环控制控制关节旋转，进而实现机械臂运动。本发明所提出的投影运动学控制方法为解析求解法，相比与其他数值解法，无需迭代求解，求解效率较高，保障控制系统实时性需求。

Description

一种大型重载机械臂的投影运动学控制方法

技术领域

本发明属于大型重载机器人领域，具体地说是一种大型重载机械臂的投影运动学控制方法。

背景技术

随着我国城市化进程的发展，建筑业迎来了新一轮的产业变革，即通过研发和研制建筑机器人来代替建筑工人完成一些“高危”和“脏、累、差”等重复性工作，进而让建筑工人们投身到更具价值和更轻松的岗位，以提高建设施工的效率，促进建筑领域的自动化、智能化的转型和升级。当前，对于建筑领域的重载操作任务，一般都是由操作人员控制大型液压机械臂，以实现重载目标的提升、搬运及转载。对重载机械臂进行自动化改造，首先需建立其运动模型，进而实现重载操作的自动控制。因此，建立大型重载机械臂的运动学控制方法，是实现重载机械臂自动化改造和高精准操控的基础和关键，具有非常重要的意义。

发明内容

针对现有大型重载机械臂逆运动学求解效率不高的问题，本发明提供一种大型重载机械臂的投影运动学控制方法。该方法通过将三维空间求解问题，转化成二维平面问题，求取机械臂关节变量。

本发明为实现上述目的所采用的技术方案是：一种大型重载机械臂的投影运动学控制方法，其特征在于，包括以下步骤：

1)根据实际的机械臂建立为机械臂的三维几何模型；

2)设置多个投影面，分别为投影面A、投影面B、投影面C以及投影面D；对该三维几何模型进行投影映射，分别得到回转关节J1、主臂俯仰关节J2、大臂俯仰关节J3、伸缩关节J4以及末端执行器的末端点EE在各个投影面的投影；

3)根据机械臂的三维几何模型以及在各个投影面的投影，获取回转关节J1的关节角，主臂俯仰关节J2的关节角以及大臂俯仰关节J3的关节角以及伸缩关节J4的伸长量；得到J1的关节角、J2的关节角、J3的关节角与末端执行器位置EE的对应关系。

4)根据末端执行器的末端位置EE得到J1的关节角、J2的关节角、J3的关节角的旋转角度，将该旋转角度分别作为关节电机的输入值，将采集的关节实际角度作为反馈值，通过闭环控制控制关节旋转，进而实现机械臂运动。

所述步骤2)中，具体为：

所述投影面A为水平于地面的平面；

所述投影面B为由主臂俯仰关节J2轴线与末端执行器的末端点EE形成的平面；

所述投影面C垂直于大臂俯仰关节J3轴线的竖直平面；

所述投影面D为由回转关节J1的轴线与末端执行器的末端点EE组成的平面。

所述步骤3)中，具体为：

(1)根据机械臂的三维几何模型在投影面A上的投影，得到回转关节J1的关节角θ₁；

(2)根据三维几何模型在投影面C上投影，得到末端执行器的末端点EE在投影面C上的映射点EE’；根据三维几何模型在投影面D上投影，得到主臂俯仰关节J2与末端执行器的末端点EE之间距离A2；

(3)根据三角几何关系得到以及映射点EE’，得到主臂俯仰关节J2在投影面C上与末端执行器的末端点EE在投影面C上的映射点EE’之间的距离A3；

(4)根据三维几何模型在投影面C上投影，以及距离A3，通过三角几何关系，获取伸缩关节J4的伸长量；

(5)根据映射点EE’以及主臂俯仰关节J2距离投影面C原点的距离，通过三角几何关系，获取主臂俯仰关节J2的关节角θ₂以及大臂俯仰关节J3的关节角θ₃。

所述步骤(1)具体为：

将三维几何模型投射在投影面A，其中，J1与J2重合，求解如下：

θ₁＝θ₁₁+θ₁₂

其中，A1为在投影面A上大臂俯仰关节J3与伸缩关节J4之间偏置距离，EE为末端执行器的坐标为(x₀,y₀,z₀)，θ₁为回转关节J1的关节角，θ₁₁，θ₁₂为中间参数。

所述步骤(3)具体为：

将三维几何模型在投影面C和投影面D投影得到三维几何简图，根据末端执行器的末端点EE的坐标为(x₀,y₀,z₀)，得到末端执行器的末端点EE在投影面C上的映射点EE’；

则A2在投影面C上J2到EE’的距离A3，即：

其中，D1为回转臂的长度，即主臂俯仰关节J2到地面的距离，A1为在投影面A上大臂俯仰关节J3与伸缩关节J4之间偏置距离，A2为在投影面D上主臂俯仰关节J2到EE的距离。

所述步骤(4)中获取伸缩关节J4的伸长量，具体为：

将三维几何模型投射在投影面C，得到映射后的三维几何简图；根据三角形几何关系，确定伸缩关节伸长量，即：

A4+A5＞A3

其中，A4为J2到J3处主臂在投影面C上的映射，且A4等于J2到J3的距离，A5为伸缩关节在投影面C上的映射，且A5等于J4到EE的距离；俯仰关节J2的关节角，可通过A3，A4及A5组成的三角形求解；

当A5＜d_max，其中d_max为伸缩关节最大长度，则有：

A3-A4＜A5＜d_max

取定：A5＝k×(d_max-ΔA₃₄)+ΔA₃₄，其中，k为取值比例系数，k取0.3，ΔA₃₄＝A3-A4；

获取伸缩关节伸长量为：

d＝A5-d_min

其中，d为伸缩关节伸长量，d_min为伸缩关节固有长度。

所述步骤(5)中获取主臂俯仰关节J2的关节角θ₂，具体为：

根据末端执行器的末端点EE的投影点EE’与回转臂的长度D1的关系，判断主臂俯仰关节J2的关节角θ₂，即：

当投影点EE’的坐标z₀＜D1，则有：θ₂＝θ₂₂-θ₂₁；其中：

其中，θ₂₁，θ₂₂为中间参数；

当投影点EE’的坐标z₀≥D1，则有：θ₂＝θ₂₂+θ₂₁；其中：

所述步骤(5)中获取大臂俯仰关节J3的关节角，具体为：

大臂俯仰关节J3的关节角θ₃，即：

所述步骤4)具体为：

由笛卡尔坐标空间到关节空间映射完毕，即由末端执行器的末端点EE的坐标为(x₀,y₀,z₀)转换至回转关节J1的关节角θ₁，俯仰关节J2的关节角θ₂，俯仰关节J3的关节角θ₃及伸缩关节长度d₄。

本发明具有以下有益效果及优点：

1)本发明所提出的投影运动学控制方法，无需建立D-H参数模型，无需复杂的矩阵运算，该方法根据投影变换，仅需采用三角函数运算，提高控制系统运算效率。

2)本发明所提出的投影运动学控制方法为解析求解法，相比与其他数值解法，无需迭代求解，求解效率较高，保障控制系统实时性需求。

3)本发明对于机器人逆运动学，其解集往往非唯一，本发明所提的投影运动学控制方法，可通过限定伸缩关节长度，实现工作空间的三维位置向量与关节空间的三维向量之间的双射，即一一映射，因此本方法得到的解集具有唯一性，极大程度上简化了控制系统实现程度，提升系统响应速度。

附图说明

图1为本发明大型重载液压机械臂结构示意图；

图2为本发明大型重载液压机械臂的三维几何模型；

图3为本发明三维几何模型在投影面A上的投影；

图4为本发明三维几何模型部分投影简图；

图5为本发明三维几何模型在投影C上的映射(第一种情况)；

图6为本发明三维几何模型在投影C上的映射(第二种情况)。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明的具体实施方式做详细的说明。在下面的描述中阐述了很多具体细节以便于充分理解本发明。但本发明能够以很多不同于在此描述的其他方式来实施，本领域技术人员可以在不违背发明内涵的情况下做类似改进，因此本发明不受下面公开的具体实施的限制。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。

如图2所示，为本发明大型重载液压机械臂的三维几何模型，将模型抽象为三维几何模型，机械臂共包括4个关节：回转关节J1、主臂俯仰关节J2、大臂俯仰关节J3及伸缩关节J4。对模型进行投影映射，分别投射在4个平面，即投影面A-D。其中：投影面A为水平平面，投影面B为由J2轴线与末端执行器的末端点EE组成的平面，投影面C垂直于J3轴线的竖直平面，投影D为由J1轴线与末端执行器的末端点EE组成的平面。末端执行器的末端点EE的坐标为(x₀,y₀,z₀)，EE’为EE在投影面C上的映射；D1为连杆1长度，即主臂俯仰关节J2到地面的距离；A1为在投影面A上，大臂俯仰关节J3与伸缩关节J4之间偏置距离；A2为在投影面D上，主臂俯仰关节J2到EE的距离；A3为A2在投影面C上，主臂俯仰关节J2到EE’的距离；A4为连杆2，即主臂俯仰关节J2与大臂俯仰关节J3的连杆，在投影面C上的映射，等于主臂俯仰关节J2到大臂俯仰关节J3的距离；A5为伸缩关节在投影面C上的映射，等于伸缩关节J4到EE的距离。

如图3所示，为本发明三维几何模型在投影面A上的投影，将三维几何模型投射在投影面A上，J1与J2重合，其中A1为在投影面A上，J3与J4之间偏置距离，末端执行器的末端点EE的坐标为(x₀,y₀,z₀)，回转关节J1的关节角为θ₁，求解如下：

θ₁＝θ₁₁+θ₁₂

其中，A1为在投影面A上大臂俯仰关节J3与伸缩关节J4之间偏置距离，EE为末端执行器的坐标为(x₀,y₀,z₀)，θ₁为回转关节J1的关节角，θ₁₁，θ₁₂为中间参数。

如图4所示，为本发明三维几何模型部分投影简图，将三维几何简图与投影面C和投影面D单独提取出，末端执行器的末端点EE的坐标为(x₀,y₀,z₀)，EE’为EE在投影面C上的映射；D1为连杆1长度，即J2到地面的距离；A2为在投影面D上，J2到EE的距离；A3为A2在投影面C上，J2到EE’的距离；A4为连杆2在投影面C上的映射，等于J2到J3的距离；A5为伸缩关节在投影面C上的映射，等于J4到EE的距离。俯仰关节J2的关节角，可通过A3，A4及A5组成的三角形求解，所以需首先求取A3，求解如下：

其中，D1为回转臂的长度，即主臂俯仰关节J2到地面的距离，A1为在投影面A上大臂俯仰关节J3与伸缩关节J4之间偏置距离，A2为在投影面D上主臂俯仰关节J2到EE的距离。

如图5所示，为本发明三维几何模型在投影C上的映射(第一种情况)，首先，根据三角形几何关系，确定伸缩关节伸长量，即：A4+A5＞A3，同时A5＜d_max，其中d_max为伸缩关节最大长度，进一步，有：A3-A4＜A5＜d_max。取定：A5＝0.3×(d_max-ΔA₃₄)+ΔA₃₄，其中0.3为取值比例系数，ΔA₃₄＝A3-A4。进一步，得到伸缩关节伸长量d₄＝A5-d_min，其中d_min为最小长度，也为固有长度。

然后，求取俯仰关节J2的关节角θ₂，此时存在两种情况：

1)末端EE’的坐标z₀＜D1，θ₂＝θ₂₂-θ₂₁。

2)末端EE’的坐标z₀≥D1，θ₂＝θ₂₂+θ₂₁。

如图6所示，为本发明三维几何模型在投影C上的映射(第二种情况)

进一步，可求得：

综上，对应回转关节J1的关节角θ₁，主臂俯仰关节J2的关节角θ₂，大臂俯仰关节J3的关节角θ₃及伸缩关节长度d₄均已求出。

综上，由笛卡尔坐标空间(工作空间)到关节空间转换完毕，即由末端执行器的末端点EE的坐标为(x₀,y₀,z₀)转换至回转关节J1的关节角θ₁，主臂俯仰关节J2的关节角θ₂，大臂俯仰关节J3的关节角θ₃及伸缩关节长度d₄，根据末端执行器的末端位置EE得到J1的关节角θ₁、J2的关节角θ₂、J3的关节角θ₃的旋转角度，将该旋转角度分别作为关节电机的输入值，将采集的关节实际角度作为反馈值，通过闭环控制控制关节旋转，进而实现机械臂运动。

本发明提出的投影运动学控制方法，提高控制系统响应速度，保障系统的实时性需求，为机械臂后续根据任务需求进行工作空间轨迹规划、实现抓取填充等任务提供稳定性及实时性保障。

Claims (9)

1.一种大型重载机械臂的投影运动学控制方法，其特征在于，包括以下步骤：

1)根据实际的机械臂建立为机械臂的三维几何模型；

2)设置多个投影面，分别为投影面A、投影面B、投影面C以及投影面D；对该三维几何模型进行投影映射，分别得到回转关节J1、主臂俯仰关节J2、大臂俯仰关节J3、伸缩关节J4以及末端执行器的末端点EE在各个投影面的投影；

3)根据机械臂的三维几何模型以及在各个投影面的投影，获取回转关节J1的关节角θ₁，主臂俯仰关节J2的关节角θ₂以及大臂俯仰关节J3的关节角θ₃以及伸缩关节J4的伸长量；得到J1的关节角θ₁、J2的关节角θ₂、J3的关节角θ₃与末端执行器位置EE的对应关系；

4)根据末端执行器的末端位置EE得到J1的关节角θ₁、J2的关节角θ₂、J3的关节角θ₃的旋转角度，将该旋转角度分别作为关节电机的输入值，将采集的关节实际角度作为反馈值，通过闭环控制控制关节旋转，进而实现机械臂运动。

2.根据权利要求1所述的一种大型重载机械臂的投影运动学控制方法，其特征在于，所述步骤2)中，具体为：

所述投影面A为水平于地面的平面；

所述投影面B为由主臂俯仰关节J2轴线与末端执行器的末端点EE形成的平面；

所述投影面C垂直于大臂俯仰关节J3轴线的竖直平面；

所述投影面D为由回转关节J1的轴线与末端执行器的末端点EE组成的平面。

3.根据权利要求1所述的一种大型重载机械臂的投影运动学控制方法，其特征在于，所述步骤3)中，具体为：

(1)根据机械臂的三维几何模型在投影面A上的投影，得到回转关节J1的关节角θ₁；

(2)根据三维几何模型在投影面C上投影，得到末端执行器的末端点EE在投影面C上的映射点EE’；根据三维几何模型在投影面D上投影，得到主臂俯仰关节J2与末端执行器的末端点EE之间距离A2；

(3)根据三角几何关系以及步骤(2)中得到的映射点EE’，得到主臂俯仰关节J2在投影面C上与末端执行器的末端点EE在投影面C上的映射点EE’之间的距离A3；

(4)根据三维几何模型在投影面C上投影，以及距离A3，通过三角几何关系，获取伸缩关节J4的伸长量；

(5)根据映射点EE’以及主臂俯仰关节J2距离投影面C原点的距离，通过三角几何关系，获取主臂俯仰关节J2的关节角θ₂以及大臂俯仰关节J3的关节角θ₃。

4.根据权利要求3所述的一种大型重载机械臂的投影运动学控制方法，其特征在于，所述步骤(1)具体为：

将三维几何模型投射在投影面A，其中，J1与J2重合，求解如下：

θ₁＝θ₁₁+θ₁₂

其中，A1为在投影面A上大臂俯仰关节J3与伸缩关节J4之间偏置距离，EE为末端执行器的坐标为(x₀,y₀,z₀)，θ₁为回转关节J1的关节角，θ₁₁，θ₁₂为中间参数。

5.根据权利要求3所述的一种大型重载机械臂的投影运动学控制方法，其特征在于，所述步骤(3)具体为：

将三维几何模型在投影面C和投影面D投影得到三维几何简图，根据末端执行器的末端点EE的坐标为(x₀,y₀,z₀)，得到末端执行器的末端点EE在投影面C上的映射点EE’；

则A2在投影面C上J2到EE’的距离A3，即：

其中，D1为回转臂的长度，即主臂俯仰关节J2到地面的距离，A1为在投影面A上大臂俯仰关节J3与伸缩关节J4之间偏置距离，A2为在投影面D上主臂俯仰关节J2到EE的距离。

6.根据权利要求3所述的一种大型重载机械臂的投影运动学控制方法，其特征在于，所述步骤(4)中获取伸缩关节J4的伸长量，具体为：

将三维几何模型投射在投影面C，得到映射后的三维几何简图；根据三角形几何关系，确定伸缩关节伸长量，即：

A4+A5＞A3

其中，A4为J2到J3处主臂在投影面C上的映射，且A4等于J2到J3的距离，A5为伸缩关节在投影面C上的映射，且A5等于J4到EE的距离；俯仰关节J2的关节角，可通过A3，A4及A5组成的三角形求解；

当A5＜d_max，其中d_max为伸缩关节最大长度，则有：

A3-A4＜A5＜d_max

取定：A5＝k×(d_max-ΔA₃₄)+ΔA₃₄，其中，k为取值比例系数，k取0.3，ΔA₃₄＝A3-A4；

获取伸缩关节伸长量为：

d＝A5-d_min

其中，d为伸缩关节伸长量，d_min为伸缩关节固有长度。

7.根据权利要求3所述的一种大型重载机械臂的投影运动学控制方法，其特征在于，所述步骤(5)中获取主臂俯仰关节J2的关节角θ₂，具体为：

根据末端执行器的末端点EE的投影点EE’与回转臂的长度D1的关系，判断主臂俯仰关节J2的关节角θ₂，即：

当投影点EE’的坐标z₀＜D1，则有：θ₂＝θ₂₂-θ₂₁；其中：

其中，θ₂₁，θ₂₂为中间参数；

当投影点EE’的坐标z₀≥D1，则有：θ₂＝θ₂₂+θ₂₁；其中：

8.根据权利要求3所述的一种大型重载机械臂的投影运动学控制方法，其特征在于，所述步骤(5)中获取大臂俯仰关节J3的关节角，具体为：

大臂俯仰关节J3的关节角θ₃，即：

9.根据权利要求1所述的一种大型重载机械臂的投影运动学控制方法，其特征在于，所述步骤4)具体为：

由笛卡尔坐标空间到关节空间映射完毕，即由末端执行器的末端点EE的坐标为(x₀,y₀,z₀)转换至回转关节J1的关节角θ₁，俯仰关节J2的关节角θ₂，俯仰关节J3的关节角θ₃及伸缩关节J4的伸长量d₄。

Images