CN117733872B - 一种基于方向性能的串联机器人逆运动学控制方法 - Google Patents

Abstract

本发明公开了一种基于方向性能的串联机器人逆运动学控制方法。所述方法利用关节机器人在奇异构型处会失去一个或多个运动自由度使得机器人的运动灵巧性降低，从而产生奇异方向的特性，量化关节机器人在任意位置下的方向性能。接着运用加权最小二乘法并结合方向性能指标来优化机器人接收到任务后执行任务过程中的关节配置，提高求解成功率以及运动柔顺性。

Description

一种基于方向性能的串联机器人逆运动学控制方法

技术领域

本发明涉及机器人控制领域，具体涉及一种基于方向性能的串联机器人逆运动学控制方法。

背景技术

机器人运动灵巧性与运动主从协同性在执行操作任务中很重要，其末端坐标从笛卡尔空间映射到关节空间时，往往会让机器人进过或接近奇异构型，使得机器人运动柔顺性差。为了提高机器人运动灵巧性其中一个关键所在就是如何设计机器人逆运动学算法使对应的关节配置更合理，更远离奇异构型。

现在大多数机器人的逆运动学算法都是解析解，即根据机器人的几何结构，正运动学公式来建立机器人的数学模型进而求得机器人目标位姿的关节参数表达式。虽然该算法针对低自由度机器人求解效率高，但面对冗余系统以及多自由度机器人该求解方法无法从中得到收益，求解得到的关节配置鲁棒性不强，往往会让机器人陷入奇异区域。而一般的数值迭代算法虽然可以施加一定的约束，但其在求解成功率上往往不尽人意。

现有的机器人逆运动学算法如下：

解析法。主要根据机器人的几何结构，正运动学公式来建立机器人的数学模型进而求得机器人目标位姿的关节参数表达式。对于低自由度的求解效率高且准确，多适用于六自由度机器人。但其表达式的非线性化无法适用于冗余系统，且面对高自由度机器人，该求解方法无法从中得到收益。它简单的表达形式让它针对复杂的任务要求时无法处理约束问题，比如远程遥操作医疗机器人的各种诊断任务。且更换不同机器人时要重新建立数学模型也会使通用性降低。

基于牛顿法的数值迭代算法。牛顿法将逆运动学的求解转化为一个最优化问题。它可以返回平滑的运动，没有不稳定的不连续性，附近关节约束也很方面，且不受机器人雅可比矩阵的奇异性影响，但它们转化出来的最优化方程通常很复杂，计算成本很高，求解效率较低。

基于机器人速度运动学的数值迭代法。该类方法目前运用最多，比如伪逆解，阻尼最小二乘解等。该方法基于机器人速度运动学设计迭代方案来解决机器人逆运动学问题，但会受到雅可比矩阵奇异的影响，如何避免代数奇异性是该类方法的关键。

现有技术中，研究机器人逆运动学求解方法包括一种斜角六关节机器人的逆运动学求解方法（CN116533239A）该现有技术存在一定的缺陷：

1.只能应用6自由度关节机器人，对其余自由度包括冗余的关节机器人应用效果未知。

2.只有在陷入奇异区域内时才引入阻尼因子优化，不能避免其进入，只是在机器人进入后进行一定的优化逆运动学求解。

发明内容

本发明提出一种基于方向性能的串联机器人逆运动学控制方法，本发明利用关节机器人在奇异构型处会失去一个或多个运动自由度使得机器人的运动灵巧性降低，从而产生奇异方向的特性，量化关节机器人在任意位置下的方向性能。接着运用加权最小二乘法并结合方向性能指标来优化机器人接收到任务后执行任务过程中的关节配置，提高求解成功率以及运动柔顺性。

本发明对串联机器人基于速度运动学的逆运动学求解方法进行了改进，通过引入机器人实时构型下笛卡尔方向的方向性能指标提高串联机器人逆运动学求解的鲁棒性。即让串联机器人从笛卡尔空间转化为关节空间的轨迹更远离奇异区域，同时提高求解成功率。

本发明的目的至少通过如下技术方案之一实现。

一种基于方向性能的串联机器人逆运动学控制方法，包括以下步骤：

S1、输入机器人初始关节角度以及目标末端位姿/>，设置迭代次数n=0，设置笛卡尔性能指标初值/>为0；

S2、根据当前机器人关节角度由正运动学计算得到末端位置/>，并计算末端位置/>计算与目标末端位姿/>的误差值/>；

S3、判断误差值是否大于等于0.01，若是则进入步骤S4，否则中断循环输出当前机器人关节角度/>，并将根据当前机器人关节角度/>作用到机器人上进行机器人逆运动学的求解，进而控制机器人运动；

S4、由当前机器人关节角度计算引入的笛卡尔方向性能指标/>；

S5、比较笛卡尔性能指标初值和笛卡尔方向性能指标/>，计算权重矩阵；

S6、赋值；

S7、计算加权最小二乘法方程得到机器人关节角度增量；

S8、令n=n+1，更新当前机器人关节位置，返回步骤S2。

进一步地，步骤S2中，末端位置计算与目标末端位姿/>的误差值，具体如下：

其中，、/>、/>、/>、/>、/>为空间上6个维度的误差，/>、/>、/>为位移误差，/>、/>、/>为选择误差；/>为单位矩阵。

进一步地，步骤S4中，由当前机器人关节角度计算得到当前机器人关节角度下的雅可比矩阵/>，雅可比矩阵采用的是基于基坐标系的几何雅可比矩阵，根据机器人构型特性而求得：

其中，和/>分别为雅可比矩阵的位移速度分量矩阵和旋转角速度分量矩阵，和/>分别为/>与/>的第k列，/>为串联机器人第k个关节的位置向量，/>为机器人基座到末端的向量，/>为利用前k个关节用正向运动学计算得到的机器人基座到第k个关节的向量；由当前机器人关节角度/>计算得到/>、/>，再通过上式求得雅可比矩阵。

进一步地，步骤S4中，对雅可比矩阵的分解，获取奇异值以及奇异值矩阵具体如下：

其中，、/>分别为正交矩阵，/>，/>表示/>为m*m维矩阵，/>，/>表示/>为n*n维矩阵，/>为奇异值矩阵，奇异值为/>，/>，……，/>，且；m表示机器人末端执行器工作自由度；/>为奇异值矩阵/>表示的第m个奇异值。

进一步地，步骤S4中，引入笛卡尔方向性能指标，令/>，代表着此刻构型下机械臂末端以速度/>运动的运动性能，/>为笛卡尔方向性能指标的第i个分量，i的取值为1~m，m是机器人的工作空间维度，一般现实空间维度为6；为矩阵/>转置的第j行第i列的值。

进一步地，步骤S5中，权重矩阵，权重矩阵/>对角线上的第i个值/>具体如下：

其中，为/>的第i个分量，/>，/>为笛卡尔方向性能指标/>的第i个分量，/>为参数，/>的取值范围为[0,1]。

进一步地，参数的计算具体如下：

。

进一步地，步骤S7中，计算加权最小二乘法方程得到机器人关节角度增量，具体如下：

其中，表示当前机器人关节角度/>下的雅可比矩阵/>。

进一步地，步骤S3中，根据当前机器人关节角度作用到机器人上进行机器人逆运动学的求解，进而控制机器人运动，具体如下：

获取的当前机器人关节角度应用于串联机器人的运动控制中，串联机器人是指一个制动器控制一个自由度，然后将一系列这样的单自由度的轴串联在一起，控制结果就是让机器人末端运动到目标位置。

进一步地，适用的机器人包括6自由度或7自由度机器人。

相比与现有技术，本发明的优点在于：

与现有的一些机器人逆运动学算法相比，所提出的算法进行了基于机器人方向性能的优化，提高了逆运动学求解成功率，同时让机器人执行任务时的关节配置更加合理，更远离奇异构型。

附图说明

图1为本发明实施例中一种基于方向性能的串联机器人逆运动学控制方法的步骤流程图；

图2为本发明实施例中应用在UR5e机器人上的 300组逆解效果数据图，其中，a为最小奇异值分布数据图，b为最大奇异值分布数据图，c为末端位置误差log处理数据图；

图3为本发明实施例中应用在sawyer机器人上的 300组逆解效果数据图，其中，a为最小奇异值分布数据图，b为最大奇异值分布数据图，c为末端位置误差log处理数据图；

图4为本发明实施例中应用在UR5e机器人上的路径跟随任务的关节轨迹示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合附图并举实施例，对本发明的具体实施进行详细说明。

实施例：

一种基于方向性能的串联机器人逆运动学控制方法，如图1所示，包括以下步骤：

S1、输入机器人初始关节角度以及目标末端位姿/>，设置迭代次数n=0，设置笛卡尔性能指标初值/>为0；

S2、根据当前机器人关节角度由正运动学计算得到末端位置/>，并计算末端位置/>计算与目标末端位姿/>的误差值/>；

末端位置计算与目标末端位姿/>的误差值/>，具体如下：

其中，、/>、/>、/>、/>、/>为空间上6个维度的误差，/>、/>、/>为位移误差，/>、/>、/>为选择误差；/>为单位矩阵。

S3、判断误差值是否大于等于0.01，若是则进入步骤S4，否则中断循环输出当前机器人关节角度/>，并将根据当前机器人关节角度/>作用到机器人上进行机器人逆运动学的求解，进而控制机器人运动；

S4、如图1所示，由当前机器人关节角度计算引入的笛卡尔方向性能指标/>；

由当前机器人关节角度计算得到当前机器人关节角度/>下的雅可比矩阵，具体如下：

其中，、/>分别为正交矩阵，/>，/>表示/>为m*m维矩阵，/>，表示/>为n*n维矩阵，/>为奇异值矩阵，奇异值为/>，/>，……，/>，且；m表示机器人末端执行器工作自由度；/>为奇异值矩阵/>表示的第m个奇异值。

引入笛卡尔方向性能指标，令/>，/>代表着此刻构型下机械臂末端以速度/>运动的运动性能，/>为笛卡尔方向性能指标/>的第i个分量，i的取值为1~m，m是机器人的工作空间维度，在一个实施例中，现实空间维度为6；/>为矩阵转置的第j行第i列的值。

S5、比较笛卡尔性能指标初值和笛卡尔方向性能指标/>，计算权重矩阵；

权重矩阵，权重矩阵/>对角线上的第i个值/>具体如下：

其中，为/>的第i个分量，/>，/>为笛卡尔方向性能指标的第i个分量，/>为参数，在一个实施例中，/>。

S6、赋值；

S7、计算加权最小二乘法方程得到机器人关节角度增量，具体如下：

其中，表示当前机器人关节角度/>下的雅可比矩阵/>。

S8、令n=n+1，更新当前机器人关节位置，返回步骤S2

在一个实施例中，如图2所示，其中，图2中的a为最小奇异值分布数据图，图2中的b为最大奇异值分布数据图，图2中的c为末端位置误差log处理数据图；本发明应用在UR5e机器人上的 300组逆解效果（初始位置为机器人的零位，目标位置为随机），如表1所示。

表1

在一个实施例中，如图3所示，其中，图3中的a为最小奇异值分布数据图，图3中的b为最大奇异值分布数据图，图3中的c为末端位置误差log处理数据图；本发明应用在sawyer机器人上的 300组逆解效果（初始位置为机器人的零位，目标位置为随机）如表2所示。

表2

本发明分别应用于6自由度与7自由度机器人上表明该算法的泛用性强，并且从所求逆解的雅可比矩阵最小奇异值值较大，最大奇异值较小可以看出求解的关节鲁棒性强，同时具有误差小、成功率高、运行时间短。

在一个实施例中，如图4所示，本发明应用在UR5e机器人上的路径跟随任务，起始位姿的关节角度为零位，目标位姿的笛卡尔位姿为，通过线性插值得到一系列笛卡尔轨迹，再通过本发明映射为一系列关节轨迹。从轨迹图可以发现关节角度变化平稳，稳定性强。

以上公开的本申请优选实施例只是用于帮助理解本发明及核心思想。对于本领域的一般技术人员，依据本发明的思想，在具体应用场景和实施操作上均会有改变之处，本说明书不应理解对本发明的限制。本发明仅受权利要求书及其全部范围和等效物的限制。

Claims (6)

1.一种基于方向性能的串联机器人逆运动学控制方法，其特征在于，包括以下步骤：

S1、输入机器人初始关节角度以及目标末端位姿/>，设置迭代次数n=0，设置笛卡尔性能指标初值/>为0；

S2、根据当前机器人关节角度由正运动学计算得到末端位置/>，并计算末端位置计算与目标末端位姿/>的误差值/>；末端位置/>计算与目标末端位姿/>的误差值，具体如下：

其中，、/>、/>、/>、/>、/>为空间上6个维度的误差，/>、/>、/>为位移误差，、/>、/>为选择误差；/>为单位矩阵；

S3、判断误差值是否大于等于0.01，若是则进入步骤S4，否则中断循环输出当前机器人关节角度/>，并将根据当前机器人关节角度/>作用到机器人上进行机器人逆运动学的求解，进而控制机器人运动；

S4、由当前机器人关节角度计算引入的笛卡尔方向性能指标/>；由当前机器人关节角度/>计算得到当前机器人关节角度/>下的雅可比矩阵/>，雅可比矩阵采用的是基于基坐标系的几何雅可比矩阵，根据机器人构型特性求得：

其中，和/>分别为雅可比矩阵的位移速度分量矩阵和旋转角速度分量矩阵，/>和分别为/>与/>的第k列，/>为串联机器人第k个关节的位置向量，/>为机器人基座到末端的向量，/>为利用前k个关节用正向运动学计算得到的机器人基座到第k个关节的向量；由当前机器人关节角度/>计算得到/>、/>，再通过上式求得雅可比矩阵/>；

对雅可比矩阵的分解，获取奇异值以及奇异值矩阵具体如下：

其中，、/>分别为正交矩阵，/>，/>表示/>为m*m维矩阵，/>，表示/>为n*n维矩阵，/>为奇异值矩阵，奇异值为/>，/>，……，/>，且；m表示机器人末端执行器工作自由度；/>为奇异值矩阵/>表示的第m个奇异值；

引入笛卡尔方向性能指标，令/>，/>代表着此刻构型下机械臂末端以速度/>运动的运动性能，/>为笛卡尔方向性能指标/>的第i个分量，i的取值为1~m，m是机器人的工作空间维度；/>为矩阵/>转置的第j行第i列的值；

S5、比较笛卡尔性能指标初值和笛卡尔方向性能指标/>，计算权重矩阵；

S6、赋值；

S7、计算加权最小二乘法方程得到机器人关节角度增量；

S8、令n=n+1，更新当前机器人关节位置，返回步骤S2。

2.根据权利要求1所述的一种基于方向性能的串联机器人逆运动学控制方法，其特征在于，步骤S5中，权重矩阵，权重矩阵/>对角线上的第i个值/>具体如下：

其中，为/>的第i个分量，/>，/>为笛卡尔方向性能指标的第i个分量，/>为参数，/>的取值范围为[0,1]。

3.根据权利要求2所述的一种基于方向性能的串联机器人逆运动学控制方法，其特征在于，参数的计算具体如下：

。

4.根据权利要求1所述的一种基于方向性能的串联机器人逆运动学控制方法，其特征在于，步骤S7中，计算加权最小二乘法方程得到机器人关节角度增量，具体如下：

其中，表示当前机器人关节角度/>下的雅可比矩阵/>。

5.根据权利要求1所述的一种基于方向性能的串联机器人逆运动学控制方法，其特征在于，步骤S3中，根据当前机器人关节角度作用到机器人上进行机器人逆运动学的求解，进而控制机器人运动，具体如下：

获取的当前机器人关节角度应用于串联机器人的运动控制中，控制结果就是让机器人末端运动到目标位置。

6.根据权利要求1所述的一种基于方向性能的串联机器人逆运动学控制方法，其特征在于，适用的机器人包括6自由度或7自由度机器人。