CN113352327B - 五自由度机械臂关节变量确定方法 - Google Patents

Abstract

本发明公开一种五自由度机械臂关节变量确定方法，迭代次数有限、精度高、通用性好。本发明方法包括：(10)机械臂正逆运动学模型建立：根据DH参数，建立五自由度机械臂的正逆运动学求解模型；(20)机械臂关节量获取：由五自由度机械臂逆运动学计算公式求出5个机械臂关节量；(30)实际可达位姿获取：将机械臂关节量代入五自由度机械臂正解计算公式，求出实际可达位姿；(40)位姿误差计算：计算实际可达位姿与期望位姿之间的位姿误差；(50)收敛性判断：根据位姿误差是否低于位姿误差阈值，判断是否收敛；如是，则跳转至(70)步骤；(60)迭代控制：用控制量修正期望位姿，跳转至(20)步骤；(70)关节变量输出：输出5个机械臂关节量。

Description

五自由度机械臂关节变量确定方法

技术领域

本发明属于五自由度机器人技术领域，特别是一种五自由度机械臂关节变量确定方法。

背景技术

随着科学技术和控制技术的发展，机器人被广泛应用于科研、军事、工业以及物流等领域。为了满足通用的应用需求，机器人开发工作者倾向于六自由度机器人。然而针对考虑质量、尺寸和成本且对某一方向自由度无要求的场景，五自由度机器人更为适用，如五自由机器人应用在电弧焊行业。机械臂运动学用于分析各关节变量与末端位姿在笛卡尔空间下的位姿变换关系，属于机器人研究领域的基础技术。而机械臂逆运动学需要解决的问题是找出至少一组关节变量使得末端达到期望的位姿。

在求解五自由度机械臂运动学逆解过程中，由于自由度少于6个，因此对于任意位置和姿态的末端执行器，运动学逆解无法直接求解。

目前，确定五自由度机械臂任意位置和姿态的末端执行器的运动学逆解，即找出至少一组关节变量满足除自由度缺省方向上任意位姿的方法包括添加虚自由度(再采用代数法或几何法求解)、姿态映射和迭代法三种。

添加虚自由度方法中，有在第六轴添加虚自由度来求解无末端执行器的问题，但对于有末端执行器的应用场景不再适用。针对于这个问题，可以在末端执行器上添加一个虚自由度，但此方法适用于添加虚自由度后满足PIEPER准则的构型。

姿态映射的原理是将任意位姿映射到可解的五自由度姿态，然后代入逆解公式求解。常用方法有遍历法和神经网络法。遍历法将末端任意位姿在约束自由度方向上进行逐一取值做齐次坐标映射，最后判断误差是否满足要求。此方法保证可解，但计算量与要求精度成正比，在快速操作场景中不适用。神经网络法将六自由度姿态映射到可解的五自由度姿态，需要用大量数据修正，且精度无法保证。

迭代法中，可以计算五自由度雅可比矩阵，然后迭代求解。但此方法计算量大，且存在不收敛问题。同时，其他迭代算法对不同构型五自由度机器人不具备通用性。

总之，现有技术存在的问题是：任意位姿五自由度机械臂的关节变量确定方法难以同时满足计算量小、精度高的技术要求，且通用性差。

发明内容

本发明的目的在于一种五自由度机械臂关节变量确定方法，迭代次数有限、精度高、通用性好。

实现本发明目的的技术方案为：

一种五自由度机械臂关节变量确定方法，包括如下步骤：

(10)机械臂正逆运动学模型建立：根据结构设计确定五自由度机械臂的DH参数，并根据机器人学基础知识，建立五自由度机械臂的正逆运动学求解模型；

(20)机械臂关节量获取：将给定的五自由度机械臂末端在笛卡尔空间下的期望位姿代入五自由度机械臂逆运动学计算公式，求出5个机械臂关节量；

(30)实际可达位姿获取：将所述5个机械臂关节量代入五自由度机械臂正解计算公式，求出五自由度机械臂末端在笛卡尔空间下的实际可达位姿；

(40)位姿误差计算：计算所述实际可达位姿与期望位姿之间的位姿误差；

(50)收敛性判断：根据位姿误差是否低于位姿误差阈值，判断是否收敛；如是，则跳转至(70)关节变量输出步骤；

(60)迭代控制：用根据自由度缺省方向上的自由度缺省偏差量计算得到的控制量，修正期望位姿，跳转至(20)机械臂关节量获取步骤；

(70)关节变量输出：输出5个机械臂关节量。

与现有技术相比，本发明的显著优点为：

1、迭代次数有限：相对于姿态映射中的遍历法，本方法采用线性控制器调节迭代步长，能以更高的效率收敛到期望的精度，且避免发生收敛振荡；

2、精度高：相比于姿态映射中的神经网络方法，本方法将迭代误差进行闭环控制，若不满足误差要求，则再次迭代，可以保证收敛的精度；

3、通用性好：相比于添加虚自由度(再采用代数法或几何法求解)，本方法不考虑机器人的构型，而是直接求出在自由度缺省方向的偏差，作为下一次迭代的补偿。适用于通用构型的五自由度机械臂，而不仅仅适用于添加虚自由度后满足特定构型才方便求解的机械臂。相比于其他计算公式类的迭代法，针对不同构型的机械臂，本方法仅需调整线性控制器单参数即可，应用型更强。

下面结合附图和具体实施例，进一步阐明本发明。

附图说明

图1是本发明五自由度机械臂关节变量确定方法的主流程图。

图2是图1中迭代控制步骤的流程图。

图3是图2中期望位姿修正步骤的流程图。

具体实施方式

如图1所示，本发明五自由度机械臂关节变量确定方法，包括如下步骤：

(10)机械臂正逆运动学模型建立：根据结构设计确定五自由度机械臂的DH参数，并根据机器人学基础知识，建立五自由度机械臂的正逆运动学求解模型；

机械臂的DH参数为本领域常用参数，其用四个参数表达两对关节连杆之间位置角度关系的机械臂数学模型。

(20)机械臂关节量获取：将给定的五自由度机械臂末端在笛卡尔空间下的期望位姿代入五自由度机械臂逆运动学计算公式，求出5个机械臂关节量；

根据机器人学基础知识可以求解出，虽然公式会随机械臂不同而有差异，但是公知的，需要根据当前的机械臂求解出。

(30)实际可达位姿获取：将所述5个机械臂关节量代入五自由度机械臂正解计算公式，求出五自由度机械臂末端在笛卡尔空间下的实际可达位姿；

(40)位姿误差计算：计算所述实际可达位姿与期望位姿之间的位姿误差；

(50)收敛性判断：根据位姿误差是否低于位姿误差阈值，判断是否收敛；如是，则跳转至(70)关节变量输出步骤；

(60)迭代控制：用根据自由度缺省方向上的自由度缺省偏差量计算得到的控制量，修正期望位姿，跳转至(20)机械臂关节量获取步骤；

如图2所示，所述(60)迭代控制步骤包括：

(61)自由度缺省偏差量计算：计算五自由度机械臂期望位姿与实际可达位姿在自由度缺省方向上的自由度缺省偏差量；

(62)自由度缺省控制量计算：将自由度缺省偏差量输入到线性控制器调节迭代步长，计算得到五自由度机械臂末端期望位姿与实际可达位姿在自由度缺省方向上的控制量；

(63)期望位姿修正：根据所述控制量，在自由度缺省方向上将期望位姿修正为新期望位姿；以所述新期望位姿作为五自由度机械臂末端在笛卡尔空间下的期望位姿，跳转至步骤(20)获取机械臂关节量。

如图3所示，所述(63)期望位姿修正步骤包括：

(631)控制量齐次变换矩阵构建：缺省自由度若为平移自由度，则构建旋转矩阵为单位矩阵的齐次变换矩阵。若为转动描述，则构建平移矩阵为0的位置向量的齐次变换矩阵；

(632)期望位姿右乘：将期望位姿右乘齐次变换矩阵修正得到新期望位姿，跳转至步骤(20)获取机械臂关节量。

(70)关节变量输出：输出5个机械臂关节量。

下面按末端缺省Z向旋转自由度的五自由度机械臂举例。

(10)建立五自由度机械臂的正逆运动学方程，并计算运动学正逆解，正解可表示为：T＝Forward(θ)，逆解可表示为：θ＝Backward(T)；

(20)针对机械臂末端期望位姿T_target₁，代入运动学逆解公式，求得θ₁＝Backward(T_target₁)；

(30)将θ₁代入正运动学公式T_solve₁＝Forward(θ₁)，计算出实际能到达的末端位姿T_solve₁；

(40)计算T_target₁与T_solve₁之间的位姿误差e₁，计算公式如下：

e₁＝||ρ_target₁-ρ_solve₁||

ρ表示位姿，将末端位姿矩阵通过矩阵变换求出：

(50)判断e₁是否小于设定的误差阈值e_min，若小于，则可跳转至(70)输出逆解θ₁；

(60)计算位姿在缺省自由度上的偏差，如上所述，缺省自由度为Z向旋转自由度，因此计算如下：

R_solve₁＝R_target₁*R_Δ

R_solve₁、R_target₁分别为实际末端位姿旋转矩阵和期望末端位姿旋转矩阵，R_Δ为缺省自由度偏差矩阵：

(61)可计算：

通过反正切函数求出Z向旋转偏差角：

(62)将偏差角

输入线性控制器，得到

λ的作用为调整迭代步长，一般取0.2-0.7，根据实际情况调整设定值；

(63)将期望目标位姿往缺省自由度方向修正；

(631)先构建控制量齐次变换矩阵，计算如下：

(632)将期望位姿右乘齐次变换矩阵修正得到新期望位姿，计算如下：

T_target₂＝T_target₁*T_update

将T_target₂代入(20)获取机械臂关节量；

(70)输出5个机械臂关节量。

本发明的基本原理在于：

当给定五自由度机械臂期望位姿后，经过五自由度运动学逆解公式求解得到五关节变量值，再将五关节变量值代入正运动学求解公式，计算出机械臂末端的实际可达位姿，由于机械臂构型只能满足五自由度位姿条件，因此计算出的实际末端位姿有可能达不到期望位姿，两者之间存在位姿误差，而此误差为五自由度机械臂在自由度缺省方向上无法动作导致的。因此，计算出期望位姿与实际可达位姿在自由度缺省方向上的偏差，再经过线性控制器调节迭代步长，得到在自由度缺省方向上的控制量，将期望位姿在自由度缺省方向上往实际可达位姿修正，修正值为控制量。将修正后的期望位姿重新代入计算，此时的姿态误差会缩小，往复迭代，误差会趋于一定值，判断是否满足误差收敛条件。针对不同构型的五自由度机械臂，可以通过调整线性控制器使误差逐步收敛。

本方法可以有效解决现有五自由度机械臂运动学逆解存在的上述问题，包括：

计算五自由度机械臂在自由度缺省方向上的偏差，可保证误差收敛；

采用线性控制器调节在自由度缺省方向上的迭代步长，保证收敛速度，提高运算效率；

按自由度缺省方向优化期望的末端位姿，可提高收敛精度。

以上本方法所采用的技术方案与现有技术相比，具有以下技术优点：

相比于添加虚自由度(再采用代数法或几何法求解)，本方法适用于通用构型的五自由度机械臂，而不仅仅适用于添加需自由度后满足特定构型才方便求解的机械臂；相对于姿态映射中的遍历法，本方法执行的效率更高，能更快收敛到设定精度；相比于姿态映射中的神经网络方法，本方法可以保证收敛的精度；相比于其他计算公式类的迭代法，针对不同构型的机械臂，本方法仅需调整线性控制器单参数即可，应用型更强。

Claims (3)

1.一种五自由度机械臂关节变量确定方法，其特征在于，包括如下步骤：

(10)机械臂正逆运动学模型建立：根据结构设计确定五自由度机械臂的DH参数，并根据机器人学基础知识，建立五自由度机械臂的正逆运动学求解模型；

(20)机械臂关节量获取：将给定的五自由度机械臂末端在笛卡尔空间下的期望位姿代入五自由度机械臂逆运动学计算公式，求出5个机械臂关节量；

(30)实际可达位姿获取：将所述5个机械臂关节量代入五自由度机械臂正解计算公式，求出五自由度机械臂末端在笛卡尔空间下的实际可达位姿；

(40)位姿误差计算：计算所述实际可达位姿与期望位姿之间的位姿误差；

(50)收敛性判断：根据位姿误差是否低于位姿误差阈值，判断是否收敛；如是，则跳转至(70)关节变量输出步骤；

(60)迭代控制：用根据自由度缺省方向上的自由度缺省偏差量计算得到的控制量，修正期望位姿，跳转至(20)机械臂关节量获取步骤；

(70)关节变量输出：输出5个机械臂关节量。

2.根据权利要求1所述的关节变量确定方法，其特征在于，所述(60)迭代控制步骤包括：

(61)自由度缺省偏差量计算：计算五自由度机械臂期望位姿与实际可达位姿在自由度缺省方向上的自由度缺省偏差量；

(62)自由度缺省控制量计算：将自由度缺省偏差量输入到线性控制器调节迭代步长，计算得到五自由度机械臂末端期望位姿与实际可达位姿在自由度缺省方向上的控制量；

(63)期望位姿修正：根据所述控制量，在自由度缺省方向上将期望位姿修正为新期望位姿；以所述新期望位姿作为五自由度机械臂末端在笛卡尔空间下的期望位姿，跳转至步骤(20)获取机械臂关节量。

3.根据权利要求2所述的关节变量确定方法，其特征在于，所述(63)期望位姿修正步骤包括：

(631)控制量齐次变换矩阵构建：缺省自由度若为平移自由度，则构建旋转矩阵为单位矩阵的齐次变换矩阵；若为转动自由度，则构建平移矩阵为0的位置向量的齐次变换矩阵；

(632)期望位姿右乘：将期望位姿右乘齐次变换矩阵修正得到新期望位姿，跳转至步骤(20)获取机械臂关节量。

Images