CN112589797A - 一种非球形手腕机械臂奇异点规避方法和系统 - Google Patents

Abstract

本发明涉及一种非球形手腕机械臂奇异点规避方法，包括：首先建立机械臂D‑H模型，得出机械臂的奇异点位姿构型，并得出机械臂奇异点位姿构型的轴集合；将机械臂的雅克比矩阵进行矩阵变换，得到奇异雅克比子矩阵；对奇异雅克比子矩阵进行矩阵求逆，得出使逆矩阵表达式中分母为零的各项因子，利用机械臂奇异因子k_i的逼近阻尼倒数f(k_i)，替换机械臂奇异因子k_i的倒数，使得当奇异因子为零时，奇异雅克比子矩阵仍然可逆，实现奇异点规避。本发明还公开了一种非球形手腕机械臂奇异点规避系统。本发明利用雅克比矩阵变化方法实现了非球形手腕机械臂的奇异因子推导，有较高的实时性能，满足工业计算实时需求。

Description

一种非球形手腕机械臂奇异点规避方法和系统

技术领域

本发明涉及机械臂技术领域，尤其是一种非球形手腕机械臂奇异点规避方法和系统。

背景技术

随着工业4.0科技革命的到来，工业机器人已成为工业化程度的重要标志。机械手臂是工业应用和理论研究中最为典型也是最早出现的工业机器人。它可以提高产品质量、提高劳动强度、避免人身事故的发生、改善劳动条件，尤其在高温、高压、低温、低压、粉尘、易爆、有毒气体和放射性等恶劣的环境中，可以代替人工工作，意义更为重大，并被广泛应用于焊接、喷涂、装配、搬运等作业环境中，大大节约了劳动成本。

机械臂的奇异点规避问题是机械臂应用领域中的主要的技术难点之一。奇异点是机械臂的固有属性，与机械臂的机械结构相关，当机械臂位于奇异点附近，机械臂会丧失某一自由度上的运动能力，此时的雅可比矩阵不可逆，有限的空间速度和加速度，会产生比较大的关节速度和关节加速度，使机械臂的轨迹跟踪性能和系统的稳定性大大降低。因此，机械臂的奇异点规避问题是实际应用中无法回避的问题和必须解决的问题之一。

关节型机械手臂是应用最广泛的一种机械臂，它具有自由度高，各关节相互独立，机构设计简单等特点。其中又以球形手腕机械臂，即末端连续三轴相交于一点的机械臂为主，其逆运动学位置和姿态是解耦的，使得该类机械臂求逆解，以及奇异点规避方便可行，但是机械局限性比较大。因此在实际工作中，为了克服机械局限性，提高作业灵活度，防止内部管线的缠结或扭断，设计出一种非球型空心手腕机械臂，即末端连续三轴不相交于一点，其逆运动学位置和姿态是耦合在一起的，对该类机械的求逆和奇异点规避问题增加难度。因此针对该种类型的机械臂提出一种实时的，具有较高轨迹精度的奇异点规避方法和系统具有较大的研究价值和实际意义。

发明内容

本发明的首要目的在于提供一种具有较高的实时性，提高了机械臂在奇异点附近的轨迹精度的非球形手腕机械臂奇异点规避方法。

为实现上述目的，本发明采用了以下技术方案：一种非球形手腕机械臂奇异点规避方法，该方法包括下列顺序的步骤：

(1)首先建立机械臂D-H模型，在机械臂工作空间内，利用格拉斯曼几何法，得出机械臂的奇异点位姿构型，并得出机械臂奇异点位姿构型的轴集合；

(2)根据机械臂奇异点位姿构型的轴集合，将机械臂的雅克比矩阵进行矩阵变换，得到奇异雅克比子矩阵；

(3)对奇异雅克比子矩阵进行矩阵求逆，得出使逆矩阵表达式中分母为零的各项因子，即为机械臂奇异因子k_i；当机械臂奇异因子k_i为零时，奇异雅克比子矩阵不可逆，机械臂处于奇异位姿；

(4)利用机械臂奇异因子k_i的逼近阻尼倒数f(k_i)，替换机械臂奇异因子k_i的倒数，使得当奇异因子为零时，奇异雅克比子矩阵仍然可逆，实现奇异点规避。

在步骤(2)中，所述将机械臂的雅克比矩阵进行矩阵变换包括关节坐标系变换、行列交换和奇异雅克比子矩阵计算，其中：

所述关节坐标系变换是指首先根据机械臂奇异点位姿构型的轴集合，计算轴集合中最小关节的关节坐标系下的雅克比矩阵；

所述行列交换是指将最小关节的关节坐标系下的雅克比矩阵的列数与机械臂奇异点位姿构型的轴集合中的轴数相同的所有列组合在一起，并将这些列中的非零行组合在一起，完成矩阵的行列交换；

所述奇异雅克比子矩阵计算是指将最小关节的关节坐标系下的雅克比矩阵通过行列变换，分解成四个子矩阵，其中将列数与机械臂奇异点位姿构型的轴集合中的轴数相同的所有列以及这些列中的非零行组合在一起的子矩阵，称为奇异雅克比子矩阵。

在步骤(4)中，所述逼近阻尼倒数f(k_i)的计算公式如下：

其中，k_i为机械臂的奇异因子，

λ₀＝0.3，均为阈值，用逼近阻尼倒数f(k_i)替换奇异因子的倒数；

将奇异雅克比子矩阵的逆矩阵中的奇异因子的倒数用奇异因子的逼近阻尼倒数代替，然后通过修改后的奇异雅克比子矩阵的逆矩阵计算各关节的关节速度。

本发明的另一目的在于提供一种非球形手腕机械臂奇异点规避方法的系统，包括：

运动控制模块，用于运动解析、机械臂正逆运动学求解、轨迹规划、轨迹插值、奇异点规避和外围设备控制，并将机械臂当前所处的运动状态，外围设备的状态实时发送给示教显示模块；

示教显示模块，用于用户，编写控制指令，示教机械臂运动点位姿，显示机器人运动控制模块反馈的机器人运动控制信息，以及显示机器人运动控制模块和电机驱动模块反馈的信息；

电机驱动模块，将运动控制模块发送的每个插值周期内的位移转换成脉冲个数，并将脉冲个数转换成波形信号反馈给驱动电机，以及电机数据的反馈过程；

通信模块，用于示教显示模块与运动控制模块之间的通信，运动控制模块与外围设备之间的通信，运动控制模块与电机驱动模块之间的通信。

所述运动控制模块包括：

运动解析模块，用于将用户编写的运动控制指令解析为机械臂控制器内部指令；

机械臂正逆运动学求解模块，用于机械臂关节角坐标系和机械臂笛卡尔空间坐标系之间的坐标变换运算；

轨迹规划模块，用于根据用户编写的运动控制指令中的运动和加速度，计算机械臂在笛卡尔空间中的运动轨迹曲线，并保证机械臂平滑、各轴同步地运动到目标点；

轨迹插值模块，通过轨迹规划模块得到的机械臂在笛卡尔空间中的运动轨迹，计算得到关节空间各轴的运动轨迹，并根据插值周期，将该段插值周期内的各轴位移值发送给电机驱动模块；

奇异点规避模块，用于在轨迹插值过程中，判断机械臂是否经过奇异点区域，如果经过奇异点区域，对机械臂进行奇异点规避，使机械臂平稳通过奇异点区域；

外围设备控制模块，用于将不同的外部设备的物理信号，转化为逻辑信号并在示教显示模块中显示，控制外部设备动作。

所述奇异点规避模块包括：

奇异点检测模块，在轨迹插值过程中，实时判断插值位置的奇异因子是否为零，如果奇异因子为零，则机械臂处在奇异点区域内；

奇异点避让模块，在轨迹插值过程中，当机械臂位于奇异点附近时，则利用逼近阻尼倒数的方法修改雅克比逆矩阵，实现奇异点规避。

由上述技术方案可知，本发明的有益效果为：本发明利用雅克比矩阵变化方法实现了非球形手腕机械臂的奇异因子推导，有较高的实时性能，满足工业计算实时需求；本发明利用逼近阻尼倒数方法实现奇异点规避，有效的提高了机械臂在奇异点附近的轨迹精度；本发明提出的奇异点规避系统，能够实时检测和规避奇异点，提高了系统稳定性以及有较好的轨迹跟踪性能，能够满足非球形手腕机械臂的工业用途需求。

附图说明

图1为本发明的系统框图；

图2为本发明的方法流程图。

具体实施方式

如图2所示，一种非球形手腕机械臂奇异点规避方法，该方法包括下列顺序的步骤：

(1)首先建立机械臂D-H模型，在机械臂工作空间内，利用格拉斯曼几何法，得出机械臂的奇异点位姿构型，并得出机械臂奇异点位姿构型的轴集合；

(2)根据机械臂奇异点位姿构型的轴集合，将机械臂的雅克比矩阵进行矩阵变换，得到奇异雅克比子矩阵；

(3)对奇异雅克比子矩阵进行矩阵求逆，得出使逆矩阵表达式中分母为零的各项因子，即为机械臂奇异因子k_i；当机械臂奇异因子k_i为零时，奇异雅克比子矩阵不可逆，机械臂处于奇异位姿；

(4)利用机械臂奇异因子k_i的逼近阻尼倒数f(k_i)，替换机械臂奇异因子k_i的倒数，使得当奇异因子为零时，奇异雅克比子矩阵仍然可逆，实现奇异点规避。

在步骤(2)中，所述将机械臂的雅克比矩阵进行矩阵变换包括关节坐标系变换、行列交换和奇异雅克比子矩阵计算，其中：

所述关节坐标系变换是指首先根据机械臂奇异点位姿构型的轴集合，计算轴集合中最小关节的关节坐标系下的雅克比矩阵；

所述行列交换是指将最小关节的关节坐标系下的雅克比矩阵的列数与机械臂奇异点位姿构型的轴集合中的轴数相同的所有列组合在一起，并将这些列中的非零行组合在一起，完成矩阵的行列交换；

所述奇异雅克比子矩阵计算是指将最小关节的关节坐标系下的雅克比矩阵通过行列变换，分解成四个子矩阵，其中将列数与机械臂奇异点位姿构型的轴集合中的轴数相同的所有列以及这些列中的非零行组合在一起的子矩阵，称为奇异雅克比子矩阵。

在步骤(4)中，所述逼近阻尼倒数f(k_i)的计算公式如下：

其中，k_i为机械臂的奇异因子，

λ₀＝0.3，均为阈值，用逼近阻尼倒数f(k_i)替换奇异因子的倒数；

将奇异雅克比子矩阵的逆矩阵中的奇异因子的倒数用奇异因子的逼近阻尼倒数代替，然后通过修改后的奇异雅克比子矩阵的逆矩阵计算各关节的关节速度。图2中的虚线方框表示雅克比矩阵变换。

如图1所示，本系统包括：

运动控制模块，用于运动解析、机械臂正逆运动学求解、轨迹规划、轨迹插值、奇异点规避和外围设备控制，并将机械臂当前所处的运动状态，外围设备的状态实时发送给示教显示模块；

示教显示模块，用于用户，编写控制指令，示教机械臂运动点位姿，显示机器人运动控制模块反馈的机器人运动控制信息，以及显示机器人运动控制模块和电机驱动模块反馈的信息；

电机驱动模块，将运动控制模块发送的每个插值周期内的位移转换成脉冲个数，并将脉冲个数转换成波形信号反馈给驱动电机，以及电机数据的反馈过程；

通信模块，用于示教显示模块与运动控制模块之间的通信，运动控制模块与外围设备之间的通信，运动控制模块与电机驱动模块之间的通信。

所述运动控制模块包括：

运动解析模块，用于将用户编写的运动控制指令解析为机械臂控制器内部指令；

机械臂正逆运动学求解模块，用于机械臂关节角坐标系和机械臂笛卡尔空间坐标系之间的坐标变换运算；

轨迹规划模块，用于根据用户编写的运动控制指令中的运动和加速度，计算机械臂在笛卡尔空间中的运动轨迹曲线，并保证机械臂平滑、各轴同步地运动到目标点；

轨迹插值模块，通过轨迹规划模块得到的机械臂在笛卡尔空间中的运动轨迹，计算得到关节空间各轴的运动轨迹，并根据插值周期，将该段插值周期内的各轴位移值发送给电机驱动模块；

奇异点规避模块，用于在轨迹插值过程中，判断机械臂是否经过奇异点区域，如果经过奇异点区域，对机械臂进行奇异点规避，使机械臂平稳通过奇异点区域；

外围设备控制模块，用于将不同的外部设备的物理信号，转化为逻辑信号并在示教显示模块中显示，控制外部设备动作。

所述奇异点规避模块包括：

奇异点检测模块，在轨迹插值过程中，实时判断插值位置的奇异因子是否为零，如果奇异因子为零，则机械臂处在奇异点区域内；

奇异点避让模块，在轨迹插值过程中，当机械臂位于奇异点附近时，则利用逼近阻尼倒数的方法修改雅克比逆矩阵，实现奇异点规避。

本发明具体的操作过程如下：

步骤(1)建立非球形手腕机械臂的D-H模型，确定模型参数，并确定机械臂工作空间范围；

步骤(2)遍历机械臂工作空间范围，利用格拉斯曼几何确定机械臂奇异位姿，得到机械臂奇异位姿下的关节轴集合；

步骤(3)将机械臂的雅克比矩阵变换到步骤(2)中的关节轴集合的最小关节坐标系下，将雅可比矩阵中列数与该奇异轴集合中轴数相同的所有列，组合在一起，并将这些列中的非零行组合在一起，完成矩阵的行列交换，并将列数与该奇异轴集合中轴数相同的所有列以及这些列中非零行组合在一起的子矩阵，称为奇异雅克比子矩阵；

步骤(4)对奇异雅克比子矩阵求逆，得出使逆矩阵表达式中分母为零的各项因子，即为机械臂奇异因子；

步骤(5)确认非球形手腕机械臂接线准确，以及配置文件设置准确；

步骤(6)首先打开摇杆功能，用摇杆操作机器人，将机械臂各轴移动到机械上标定的机械零点，然后点击示教显示模块中的校零按钮，将电机位置设置为0，则此位置即为该机械臂的零点，该步骤在机械臂零点准确的情况下，可以忽略；

步骤(7)首先点击示教显示模块中的程序页面，编写机械臂运动控制指令；

步骤(8)点击示教显示模块中的示教页面，打开摇杆功能，用摇杆操作机械臂，示教机械臂运动过程中的各个目标点，点击保存按钮；

步骤(9)点击示教显示模块中的运行界面，点击单步按钮，查看步骤(8)中示教的机械臂的各个目标点是否正确；

步骤(10)机械臂运动到目标点的过程中，实时检测步骤(4)中的奇异因子是否为零，若为零，则表示机械臂将接近奇异点区域附近，此时利用逼近阻尼倒数方法规避奇异点，使得机械臂平稳通过奇异点区域；

步骤(11)点击示教显示模块中的运行界面，首先查看机械臂各模块，以及界面上显示的外围设备信号是否正常，如果机械臂一切正常，则点击运行按钮；

步骤(12)在机械臂运行的状态下，可以点击示教显示模块中的暂停或者停止按钮，停止机械臂。点击暂停之后继续运行机械臂，机械臂会按暂停之前的位置继续运行，点击停止之后继续运行机械臂，机械臂会从头开始运行；

步骤(13)在机械臂运行的过程中，点击示教显示模块中的修改速度和加速度按钮，可以改变机械臂的运行速度；

步骤(14)机械臂在有报错的情况下，要首先确认清除相应错误之后，才能继续运行机械臂；

步骤(15)更换了机械臂的运动控制指令后，需要重新从步骤(7)之后开始操作。

综上所述，本发明利用雅克比矩阵变化方法实现了非球形手腕机械臂的奇异因子推导，有较高的实时性能，满足工业计算实时需求；本发明利用逼近阻尼倒数方法实现奇异点规避，有效的提高了机械臂在奇异点附近的轨迹精度；本发明提出的奇异点规避系统，能够实时检测和规避奇异点，提高了系统稳定性以及有较好的轨迹跟踪性能，能够满足非球形手腕机械臂的工业用途需求。

Claims (6)

1.一种非球形手腕机械臂奇异点规避方法，其特征在于：该方法包括下列顺序的步骤：

(1)首先建立机械臂D-H模型，在机械臂工作空间内，利用格拉斯曼几何法，得出机械臂的奇异点位姿构型，并得出机械臂奇异点位姿构型的轴集合；

(2)根据机械臂奇异点位姿构型的轴集合，将机械臂的雅克比矩阵进行矩阵变换，得到奇异雅克比子矩阵；

(3)对奇异雅克比子矩阵进行矩阵求逆，得出使逆矩阵表达式中分母为零的各项因子，即为机械臂奇异因子k_i；当机械臂奇异因子k_i为零时，奇异雅克比子矩阵不可逆，机械臂处于奇异位姿；

(4)利用机械臂奇异因子k_i的逼近阻尼倒数f(k_i)，替换机械臂奇异因子k_i的倒数，使得当奇异因子为零时，奇异雅克比子矩阵仍然可逆，实现奇异点规避。

2.根据权利要求1所述的非球形手腕机械臂奇异点规避方法，其特征在于：在步骤(2)中，所述将机械臂的雅克比矩阵进行矩阵变换包括关节坐标系变换、行列交换和奇异雅克比子矩阵计算，其中：

所述关节坐标系变换是指首先根据机械臂奇异点位姿构型的轴集合，计算轴集合中最小关节的关节坐标系下的雅克比矩阵；

所述行列交换是指将最小关节的关节坐标系下的雅克比矩阵的列数与机械臂奇异点位姿构型的轴集合中的轴数相同的所有列组合在一起，并将这些列中的非零行组合在一起，完成矩阵的行列交换；

所述奇异雅克比子矩阵计算是指将最小关节的关节坐标系下的雅克比矩阵通过行列变换，分解成四个子矩阵，其中将列数与机械臂奇异点位姿构型的轴集合中的轴数相同的所有列以及这些列中的非零行组合在一起的子矩阵，称为奇异雅克比子矩阵。

3.根据权利要求1所述的非球形手腕机械臂奇异点规避方法，其特征在于：在步骤(4)中，所述逼近阻尼倒数f(k_i)的计算公式如下：

其中，k_i为机械臂的奇异因子，

λ₀＝0.3，均为阈值，用逼近阻尼倒数f(k_i)替换奇异因子的倒数；

将奇异雅克比子矩阵的逆矩阵中的奇异因子的倒数用奇异因子的逼近阻尼倒数代替，然后通过修改后的奇异雅克比子矩阵的逆矩阵计算各关节的关节速度。

4.实施权利要求1至3中任一项所述的非球形手腕机械臂奇异点规避方法的系统，其特征在于：包括：

运动控制模块，用于运动解析、机械臂正逆运动学求解、轨迹规划、轨迹插值、奇异点规避和外围设备控制，并将机械臂当前所处的运动状态，外围设备的状态实时发送给示教显示模块；

示教显示模块，用于用户，编写控制指令，示教机械臂运动点位姿，显示机器人运动控制模块反馈的机器人运动控制信息，以及显示机器人运动控制模块和电机驱动模块反馈的信息；

电机驱动模块，将运动控制模块发送的每个插值周期内的位移转换成脉冲个数，并将脉冲个数转换成波形信号反馈给驱动电机，以及电机数据的反馈过程；

通信模块，用于示教显示模块与运动控制模块之间的通信，运动控制模块与外围设备之间的通信，运动控制模块与电机驱动模块之间的通信。

5.根据权利要求4所述的系统，其特征在于：所述运动控制模块包括：

运动解析模块，用于将用户编写的运动控制指令解析为机械臂控制器内部指令；

机械臂正逆运动学求解模块，用于机械臂关节角坐标系和机械臂笛卡尔空间坐标系之间的坐标变换运算；

轨迹规划模块，用于根据用户编写的运动控制指令中的运动和加速度，计算机械臂在笛卡尔空间中的运动轨迹曲线，并保证机械臂平滑、各轴同步地运动到目标点；

轨迹插值模块，通过轨迹规划模块得到的机械臂在笛卡尔空间中的运动轨迹，计算得到关节空间各轴的运动轨迹，并根据插值周期，将该段插值周期内的各轴位移值发送给电机驱动模块；

奇异点规避模块，用于在轨迹插值过程中，判断机械臂是否经过奇异点区域，如果经过奇异点区域，对机械臂进行奇异点规避，使机械臂平稳通过奇异点区域；

外围设备控制模块，用于将不同的外部设备的物理信号，转化为逻辑信号并在示教显示模块中显示，控制外部设备动作。

6.根据权利要求5所述的系统，其特征在于：所述奇异点规避模块包括：

奇异点检测模块，在轨迹插值过程中，实时判断插值位置的奇异因子是否为零，如果奇异因子为零，则机械臂处在奇异点区域内；

奇异点避让模块，在轨迹插值过程中，当机械臂位于奇异点附近时，则利用逼近阻尼倒数的方法修改雅克比逆矩阵，实现奇异点规避。

Images