机器人运动学参数误差优化补偿方法及装置与流程

本发明涉及机器人参数校正技术领域，具体而言，涉及一种机器人运动学参数误差优化补偿方法及装置。

背景技术：

随着技术的发展，工业机器人的应用逐渐普及。目前，工业机器人的重复精度相对比较高，但定位精度却相对比较差。因此在很多应用中需要对机器人的相关参数进行标定。所谓标定是指运用相关的测量手段和适当的参数识别方法辨识出机器人模型的准确参数。

机器人运动学标定是比较常用的机器人参数标定方法，该方法通过建模、测量、辨识和补偿等步骤对机器人的运动学参数进行重新辨识和校正，以减小机器人在生产、装配和使用过程中产生的几何参数误差，从而达到提高机器人绝对定位精度的目的。

在对机器人运动学参数标定的过程中，如何将辨识出的参数误差补偿进机器人控制器是一个很重要的问题。发明在研究中发现，现有的将辨识出的参数误差补偿进机器人的方式存在导致标定失败的概率比较大，可能不仅无法提高机器人的定位精度，而且有可能使机器人运动学逆解析失败。

技术实现要素：

本发明的目的在于提供机器人运动学参数误差优化补偿方法及装置，以解决运动学参数补偿后导致的运动学逆解解析失败的问题。

第一方面，本发明实施例提供了一种机器人运动学参数误差优化补偿方法，包括：

根据机器人标定前和标定后的运动学参数，分别构建机器人运动学名义值模型和机器人运动学标定值模型，其中，在所述机器人运动学名义值模型和机器人运动学标定值模型中，采用末端执行器坐标系原点的位姿向量表示末端执行器坐标系与基坐标系之间的位姿转换关系；

在预设关节空间位置下，根据所述运动学标定值模型计算末端执行器坐标系原点的位姿；

构建参数补偿约束优化模型，其中，所述参数补偿约束优化模型用于约束根据运动学标定值模型计算得到的末端执行器坐标系原点的位姿与预设目标位姿之间的差值最小；

求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值；

将所述经过优化补偿后的关节空间坐标值替代由所述机器人运动学名义值模型进行运动学逆计算得到的关节空间坐标。

结合第一方面，在第一方面的第一种可能的实施方式中，所述在预设关节空间位置下，根据所述运动学标定值模型计算末端执行器坐标系原点的位姿，包括：

根据所述运动学标定值模型，计算所述预设关节空间位置对应的末端执行器坐标系在笛卡尔空间中相对于基坐标系的齐次变换矩阵；

根据所述齐次变换矩阵，确定所述末端执行器坐标系原点的位姿。

结合第一方面或第一方面的第一种可能的实施方式，在本发明实施例的第二种可能的实施方式中，所述参数补偿约束优化模型为：

minω(θ)＝||P_n-f_c(θ)||₂

其中，ω(θ)表示位姿误差向量的欧氏范数；Pn表示所述末端执行器坐标系转换矩阵计算得到的指定位姿向量[p_x p_y p_z α β γ]^T；f_c(θ)表示根据标定值模型和关节空间位置θ计算得到的实际位姿向量；和分别表示关节i的最小限位和最大限位，在移动关节中，该约束表示为关节在轴向上能够移动的最大位移。

结合第一方面的第二种可能的实施方式，在第一方面的第三种可能的实施方式中，所述求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值，包括：

采用内点法求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值。

结合第一方面的第三种可能的实施方式，在第一方面的第四种可能的实施方式中，所述采用内点法求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值，包括：

根据所述参数补偿约束优化模型构建无约束目标函数：

式中，r^(k)是惩罚因子，是一个递减的正数序列，即r⁽⁰⁾＞r⁽¹⁾＞...r^(k)，且有g_i(θ)是所述参数补偿约束优化模型中的约束条件；

对所述无约束目标函数进行迭代，得到所述经过优化补偿后的关节空间坐标值。

第二方面，本发明实施例提供了一种机器人运动学参数误差优化补偿装置，包括：

第一模型构建单元，用于根据机器人标定前和标定后的运动学参数，分别构建机器人运动学名义值模型和机器人运动学标定值模型，其中，在所述机器人运动学名义值模型和机器人运动学标定值模型中，采用末端执行器坐标系原点的位姿向量表示末端执行器坐标系与基坐标系之间的位姿转换关系；

第一计算单元，用于在预设关节空间位置下，根据所述运动学标定值模型计算末端执行器坐标系原点的位姿；

第二模型构建单元，用于构建参数补偿约束优化模型，其中，所述参数补偿约束优化模型用于约束根据运动学标定值模型计算得到的末端执行器坐标系原点的位姿与预设目标位姿之间的差值最小；

第二计算单元，用于求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值；

补偿单元，用于将所述经过优化补偿后的关节空间坐标值替代由所述机器人运动学名义值模型进行运动学逆计算得到的关节空间坐标。

结合第二方面，在第二方面的第一种可能的实施方式中，所述第一计算单元在预设关节空间位置下，根据所述运动学标定值模型计算末端执行器坐标系原点的位姿，具体包括执行：

根据所述运动学标定值模型，计算所述预设关节空间位置对应的末端执行器坐标系在笛卡尔空间中相对于基坐标系的齐次变换矩阵；

根据所述齐次变换矩阵，确定所述末端执行器坐标系原点的位姿。

结合第二方面或者第二方面的第一种可能的实施方式，在第二方面的第二种可能的实施方式中，所述参数补偿约束优化模型为：

minω(θ)＝||P_n-f_c(θ)||₂

其中，ω(θ)表示位姿误差向量的欧氏范数；Pn表示所述末端执行器坐标系转换矩阵计算得到的指定位姿向量[p_x p_y p_z α β γ]^T；f_c(θ)表示根据标定值模型和关节空间位置θ计算得到的实际位姿向量；和分别表示关节i的最小限位和最大限位，在移动关节中，该约束表示为关节在轴向上能够移动的最大位移。

结合第二方面的第二种可能的实施方式，在第二方面的第三种可能的实施方式中，所述第二计算单元求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值，具体包括执行：

采用内点法求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值。

结合第二方面的第三种可能的实施方式，在第二方面的第四种可能的实施方式中，所述补偿单元采用内点法求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值，具体包括执行：

根据所述参数补偿约束优化模型构建无约束目标函数：

式中，r^(k)是惩罚因子，是一个递减的正数序列，即r⁽⁰⁾＞r⁽¹⁾＞...r^(k)，且有g_i(θ)是所述参数补偿约束优化模型中的约束条件；

对所述无约束目标函数进行迭代，得到所述经过优化补偿后的关节空间坐标值。

在本发明实施例方案中，利用机器人标定前和标定后的运动学参数分别构建机器人运动学名义值模型和标定值模型，并且在预设关节空间位置下，基于运动学标定值模型计算末端执行器坐标系原点的位姿，之后利用参数补偿约束优化模型使上述计算得到的末端执行器坐标系原点的位姿与预设目标位姿之间的差值最小，并且通过求解上述参数补偿约束优化模型得到经过优化补偿后的关节空间坐标值；得到经过优化补偿后的关节空间坐标值后，将经过优化补偿后的关节空间坐标值替换由所述机器人运动学名义值模型进行运动学逆计算得到的关节空间坐标。本发明实施例方案，标定方式简单，而且能够解决运动学参数补偿后导致的运动学逆解解析失败的问题。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1是本发明实施例机器人运动学参数误差优化补偿方法的流程图；

图2是本发明实施例构建机器人运动学模型的效果示意图；

图3是本发明实施例机器人运动学参数误差优化补偿装置的结构示意图。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

现有技术中，将辨识出的参数误差补偿进入控制器的过程通常是通过修改机器人控制器中的机器人模型来完成，机器人模型修改的成功与否直接决定了运动学标定的成败。随着机器人普及化程度的提高，大多数非专业用户并不拥有机器人控制器的修改权限，因而难以将辨识出的补偿参数写入机器人控制器，这使得参数辨识的结果无法应用到实际操作中，也就无法实现机器人绝对定位精度的提高。

另一方面，即使用户拥有机器人控制器的修改权限，如果经过标定的运动学参数与名义值不符，例如经过标定的机器人腕关节三个轴坐标系的运动学参数与名义值不符，可能会造成补偿后的机器人模型中的腕关节三个轴线不再相交于一点，由此会导致机器人运动学逆解的解析失败。

为了解决现有技术中存在的运动学参数补偿后导致的运动学逆解解析失败的问题，本发明实施例提供了一种机器人运动学参数误差优化补偿方法。

图1是本发明实施例机器人运动学参数误差优化补偿方法的流程图。图1所示方法的执行主体为机器人中的控制器，所述方法包括：

步骤S101：根据机器人标定前和标定后的运动学参数，分别构建机器人运动学名义值模型和机器人运动学标定值模型。

在所述机器人运动学名义值模型和机器人运动学标定值模型中，采用末端执行器坐标系原点的位姿向量表示末端执行器坐标系与基坐标系之间的位姿转换关系。

在本发明实施例方案中，采用D-H参数构建上述的机器人运动学名义值模型和机器人运动学标定值模型。经典D-H参数常用于描述机器人运动学模型，是机器人连杆和关节建模的一种简单方法，可用于任何机器人构型。

本发明实施例方案中，采用D-H参数构建机器人运动学模型的方法包括：

(1)如图2所示，在机器人关节处以关节轴线为Z轴，以与该关节连接的连杆起点为坐标原点，建立每个关节的坐标系，其中，建立的每个关节的坐标系称为基坐标系。

(2)采用一个由机器人几何结构参数构造的4×4的齐次变换矩阵描述相邻两连杆的空间转换关系。

(3)根据相邻关节坐标系转换推导出末端执行器坐标系相对于基坐标系的等价齐次变换矩阵。

在规定了全部连杆的坐标系后，就可以确定用于表示坐标系i-1和坐标系i之间相对位置和方位关系的4个参数，即杆件长度ai-1，杆件扭角ɑi-1，关节距离di和关节转角θi，其中，杆件长度ai-1，杆件扭角ɑi-1，关节距离di和关节转角θi称为经典D-H参数。利用基本的齐次转动和齐次平移变换矩阵，可以得到相邻坐标系i-1和i的齐次变换矩阵i-1iT，即：

其中cɑi-1表示cos(ɑi-1)，sɑi-1表示sin(ɑi-1)，cθi表示cos(θi)，sθi表示sin(θi)。

因而，机器人基坐标系到末端执行器坐标系的齐次变换矩阵可以表示为：

其中n，o，a和p分别表示齐次变换矩阵中的元素。据此，可分别利用标定前运动学参数的名义值和标定后的标定值，建立机器人运动学名义值模型R_n和标定值模型R_c。其中名义值模型表示在控制器中末端坐标系相对于基坐标系的转换关系，其腕关节三个坐标系的轴线相交于一点，可根据指定末端执行器坐标系位姿利用运动学逆解得到对应的关节空间位置的解析解；而在经过运动学参数标定后的标定值模型表示经过标定后末端坐标系相对于基坐标系的准确转换关系，其杆件扭角不一定是0或90°，造成腕关节三个轴不一定相交，因而无法通过解析解得到其逆解。

根据RPY角方法将齐次变换矩阵转化为末端执行器坐标系原点在基坐标系中的位姿P，则x，y和z坐标分别为px，py和pz，而末端执行器坐标系绕基坐标系坐标轴x，y，z旋转的偏转角ɑ，俯仰角β和横滚角γ可分别计算为：

至此，可以使用末端执行器坐标系原点的位姿向量P_n＝[p_x p_y p_z α β γ]^T来表示末端执行器坐标系与基坐标系之间的位姿转换关系。

步骤S102：在预设关节空间位置下，根据所述运动学标定值模型计算末端执行器坐标系原点的位姿。

其中，具体实施时，机器人控制器根据所述运动学标定值模型，计算所述预设关节空间位置对应的末端执行器坐标系在笛卡尔空间中相对于基坐标系的齐次变换矩阵；根据所述齐次变换矩阵，确定所述末端执行器坐标系原点的位姿。

步骤S103：构建参数补偿约束优化模型，其中，所述参数补偿约束优化模型用于约束根据运动学标定值模型计算得到的末端执行器坐标系原点的位姿与预设目标位姿之间的差值最小。

在本发明实施例方案中，构建参数补偿约束优化模型的步骤包括：

(1)由第一步得到的标定值模型R_c，在给定关节空间位置的前提下，可以计算该关节位置对应的末端执行器坐标系在笛卡尔空间中相对于基坐标系的齐次变换矩阵，进而求得末端执行器坐标系原点的位姿。在此约束优化问题中，标定值模型中的关节位置作为待优化变量，其优化目标是使由标定值模型计算得到的末端执行器坐标系原点的位姿尽可能地接近指定的目标位姿，即最小化位姿误差向量的欧氏范数。

据此可构建参数补偿约束优化模型：

minω(θ)＝||P_n-f_c(θ)||₂

其中，ω(θ)表示位姿误差向量的欧氏范数，即误差向量内积的平方根；Pn表示根据指定的末端执行器坐标系转换矩阵计算得到的指定位姿向量[p_x p_y p_z α β γ]^T；f_c(θ)表示根据标定值模型和关节空间位置θ计算得到的实际位姿向量；和分别表示关节i的最小限位和最大限位，在移动关节中，该约束表示为关节在轴向上能够移动的最大位移。

步骤S104：求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值。

在本发明实施例中，可以采用内点法求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值。其中，内点法具有计算速度快，算法比较成熟，结果可信度高等优点。

具体的，采用内点法求解所述参数补偿约束优化模型得到经过优化补偿后的关节空间坐标值的步骤包括：

一、根据上述参数补偿约束优化模型构造一个新的无约束目标函数，即惩罚函数

式中，r^(k)是惩罚因子，是一个递减的正数序列，即r⁽⁰⁾＞r⁽¹⁾＞...r^(k)，且有惩罚因子可以大幅提高模型收敛的速度；g_i(θ)是原优化模型中关于关节空间位置的12个约束条件，即等，均为小于或等于0的不等式。

二、对惩罚函数进行迭代，在可行域内逐步逼近原约束优化问题的最优解，具体的包括：

(1)取初始惩罚因子r⁽⁰⁾＞0，允许误差ε＞0；

(2)在可行域D内取初始点θ⁽⁰⁾，令k＝1，初始点可由运动学名义值模型根据给定的末端执行器位姿求逆解得到；

(3)从θ^(k-1)点出发，用无约束优化方法(如最速下降法或牛顿法)求解惩罚函数的极值点θ^*(r^(k))；

(4)检查迭代终止条件，如果满足

或

||θ^*(r^(k))-θ^*(r^(k-1))||≤ε₂，

则终止迭代计算，并以θ^*(r^(k))为原优化函数ω(θ)的约束最优解，否则转入下一步；

(5)取r^(k+1)＝Cr^(k)，θ⁽⁰⁾＝θ^*(r^(k))，k＝k+1，转入步骤3)。递减系数C＝0.1-0.5，常取0.1。

经过迭代计算，可得到参数补偿约束优化模型的最优解，即经过优化补偿后的关节空间坐标值。

步骤S105：将所述经过优化补偿后的关节空间坐标值替代由所述机器人运动学名义值模型进行运动学逆计算得到的关节空间坐标。

在本发明实施例方案中，利用标定前后机器人的运动学参数分别构建机器人运动学名义值模型和标定值模型；根据指定的机器人末端执行器位姿和标定值模型，将指定位姿和由标定值模型计算到位姿的差值向量的最小欧氏范数作为优化目标，将各运动关节的关节运动范围作为约束条件，构建约束优化模型；利用非线性多元函数优化算法对模型进行求解，得到补偿后的关节空间坐标值，将该组值替代由名义值模型的运动学逆解计算得到的关节空间坐标并由控制器操作机器人运动。

本发明实施例方法的机器人运动学参数误差优化补偿方法中，运动学参数补偿不需要修改机器人控制器中的原有参数，方便标定；另外，本发明实施例方法解决了机器人运动学参数标定可能造成的腕关节三个轴线不再相交于一个点而导致的逆解解析失效的问题。

图3是本发明实施例提供的一种机器人运动学参数误差优化补偿装置。如图3所示，该装置包括：第一模型构建单元201、第一计算单元202、第二模型构建单元203、第二计算单元204和补偿单元205；其中，

第一模型构建单元201，用于根据机器人标定前和标定后的运动学参数，分别构建机器人运动学名义值模型和机器人运动学标定值模型，其中，在所述机器人运动学名义值模型和机器人运动学标定值模型中，采用末端执行器坐标系原点的位姿向量表示末端执行器坐标系与基坐标系之间的位姿转换关系；

第一计算单元202，用于在预设关节空间位置下，根据所述运动学标定值模型计算末端执行器坐标系原点的位姿；

第二模型构建单元203，用于构建参数补偿约束优化模型，其中，所述参数补偿约束优化模型用于约束根据运动学标定值模型计算得到的末端执行器坐标系原点的位姿与预设目标位姿之间的差值最小；

第二计算单元204，用于求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值；

补偿单元205，用于将所述经过优化补偿后的关节空间坐标值替代由所述机器人运动学名义值模型进行运动学逆计算得到的关节空间坐标。

在一种可能的设计中，所述第一计算单元202在预设关节空间位置下，根据所述运动学标定值模型计算末端执行器坐标系原点的位姿，具体包括执行：

根据所述运动学标定值模型，计算所述预设关节空间位置对应的末端执行器坐标系在笛卡尔空间中相对于基坐标系的齐次变换矩阵；

根据所述齐次变换矩阵，确定所述末端执行器坐标系原点的位姿。

在一种可能的设计中，所述参数补偿约束优化模型为：

minω(θ)＝||P_n-f_c(θ)||₂

其中，ω(θ)表示位姿误差向量的欧氏范数；Pn表示所述末端执行器坐标系转换矩阵计算得到的指定位姿向量[p_x p_y p_z α β γ]^T；f_c(θ)表示根据标定值模型和关节空间位置θ计算得到的实际位姿向量；和分别表示关节i的最小限位和最大限位，在移动关节中，该约束表示为关节在轴向上能够移动的最大位移。

在一种可能的设计中，所述第二计算单元204求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值，具体包括执行：

采用内点法求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值。

在一种可能的设计中，所述补偿单元205采用内点法求解所述参数补偿约束优化模型，得到经过优化补偿后的关节空间坐标值，具体包括执行：

根据所述参数补偿约束优化模型构建无约束目标函数：

式中，r^(k)是惩罚因子，是一个递减的正数序列，即r⁽⁰⁾＞r⁽¹⁾＞...r^(k)，且有g_i(θ)是所述参数补偿约束优化模型中的约束条件；

对所述无约束目标函数进行迭代，得到所述经过优化补偿后的关节空间坐标值。

本发明实施例所提供的进行机器人运动学参数误差优化补偿方法的计算机程序产品，包括存储了程序代码的计算机可读存储介质，所述程序代码包括的指令可用于执行前面方法实施例中所述的方法，具体实现可参见方法实施例，在此不再赘述。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。