CN112936274A - 一种机器人夹持的柔性打磨轮位姿辨识方法 - Google Patents

Abstract

本发明涉及一种机器人夹持的柔性打磨轮位姿辨识方法，包括：控制机器人携带移动探针以多种姿态接触固定探针，得到多个工具中心点坐标，采用Ransac算法和最小二乘法球心拟合完成固定探针位姿辨识；将打磨轮固定于机器人末端，对连接轴和打磨轮的交界圆选多个位置点分别接触固定探针，经机器人基坐标系向Too l 0转换得到多个位置点在Too l 0下的坐标；对多个位置点坐标采用Ransac算法和最小二乘法平面拟合得平面法向量记为X轴，以X轴为基准，对Too l 0下Z’轴施密特正交化得Z轴，右手定则得Y轴；对多个位置点坐标圆心拟合得交界圆圆心坐标，由打磨轮平均半径R可得磨削点位置坐标。本发明可有效解决目前打磨轮位姿辨识误差大、稳定性低、不利于实际生产加工的问题。

Description

一种机器人夹持的柔性打磨轮位姿辨识方法

技术领域

本发明涉及机器人技术领域，尤其涉及一种机器人夹持的柔性打磨轮位姿辨识方法、装置、设备及存储介质。

背景技术

橡胶轮、纱布轮、钢丝轮等常见柔性打磨轮主要用于金属、木材、亚克力等材料零件的表面精密加工，具有加工效率高及加工一致性好等优点。但用机器人夹持柔性打磨轮进行加工时，其显著的柔性特点给工具位姿辨识带来困难。传统位姿辨识方法主要分为三步：四点法完成固定探针位姿辨识；人工定位打磨轮磨削点位置；以Tool0姿态作为打磨轮姿态。传统方法主要存在三个弊端：当机器人运动误差较大或对点误差较大时，四点法辨识固定探针位置误差大；确定打磨轮磨削点位置时，将与Tool0下XOY平面平行的磨削轮切平面相切点作为磨削点，目测方式难以精确定位切平面，同时用固定探针接触柔性轮表面时会造成柔性轮接触点变形凹陷，而柔性轮高速转动工作时受离心力接触点会回弹，难以定位磨削点，磨削点定位精度差会造成工件过磨；由于存在装夹误差，打磨轮的轴线偏离Tool0下Z’轴，以Tool0的姿态作为打磨轮的姿态会造成工件非均匀磨削，且装夹误差越大、打磨轮长度越长非均匀磨削越严重。

综上所述，传统方法在装夹误差和人工定位误差较大的情况下难以满足后续加工要求。

发明内容

有鉴于此，有必要提供一种机器人夹持的柔性打磨轮位姿辨识方法、设备及存储介质，用以解决目前打磨轮位姿辨识误差大、稳定性低、不利于实际生产加工的问题。

第一方面，本发明提供一种机器人夹持的柔性打磨轮位姿辨识方法，包括如下步骤：

控制机器人以多种姿态携带移动探针接触固定探针，并获取各姿态下工具中心点的坐标；其中，所述固定探针固定于预设位置，所述移动探针固定于机器人末端；

采用随机抽样一致算法对获取的多个工具中心点的坐标进行球心拟合，剔除误差较大的坐标后，采用最小二乘法对剩余的工具中心点的坐标进行球心拟合，以得到固定探针在机器人基坐标系下的位置坐标；

将移动探针替换成柔性打磨轮后，在连接轴和打磨轮的交界圆上选取多个位置点，控制所述机器人移动使若干个位置点分别接触所述固定探针后，获取每次接触时机器人末端的位置和姿态，根据每次接触时机器人末端的位置和姿态进行坐标系转换后，根据所述固定探针在机器人基坐标系下的位置坐标计算出每个位置点在机器人末端坐标系中的坐标；

采用随机抽样一致算法对多个位置点在机器人末端坐标系中的坐标进行平面拟合，剔除误差较大点后，采用最小二乘法对剩余的坐标进行平面拟合，得到平面法向量，并记做X轴；

以X轴与机器人末端坐标系下X’轴呈锐角的方向作为X轴正方向，以X轴为基准，对机器人末端坐标系下Z’轴进行施密特正交化得Z轴，并采用右手定则得到Y轴，以得到柔性打磨轮坐标系；

将每个位置点在机器人末端坐标系中的坐标转换为在柔性打磨轮坐标系中的坐标后，采用随机抽样一致算法对获取的多个位置点在柔性打磨轮坐标系中的坐标进行球心拟合，剔除误差较大的坐标后，采用最小二乘法对剩余的坐标进行球心拟合后，得到交界圆圆心坐标，根据打磨轮平均半径、交界圆圆心坐标以及柔性打磨轮坐标系的单位方向向量得到磨削点坐标。

优选的，所述的机器人夹持的柔性打磨轮位姿辨识方法中，所述采用随机抽样一致算法对获取的多个工具中心点的坐标进行球心拟合的步骤具体为：

选取其中四个工具中心点的坐标作为组合，代入公式：(x_i-x₀)²+(y_i-y₀)²+(z_i-z₀)²＝R²中，求取出圆心(x₀，y₀，z₀)和半径R后，采用公式

计算出剩余的坐标的欧式距离d，若满足d≤δ₁，则视为内点，否则视为外点剔除；其中，δ₁为预设的第一距离阈值，(x_i，y_i，z_i)为工具中心点的坐标；

重新选取其中四个工具中心点的坐标作为组合，并进行内点筛选和外点剔除；

当所有的组合都计算完毕时，将内点数最多的组合及其内点作为最小二乘法的求解集合。

优选的，所述的机器人夹持的柔性打磨轮位姿辨识方法中，采用最小二乘法对剩余的工具中心点的坐标进行球心拟合，以得到固定探针在机器人基坐标系下的位置坐标的步骤具体为：

采用最小二乘法对求解集合进行球心拟合，其表达式如下：

其中，(x_i，y_i，z_i)为求解集合中各点坐标，R为拟合半径；

采用下列公式对x₀、y₀、z₀、R求偏导，

利用坐标差值公式将上述公式进行简化，具体公式为：

其中

分别为求解集合中各工具中心点坐标(x_i，y_i，z_i)的均值，求解得到a₀、b₀、c₀，根据a₀、b₀、c₀得到球心坐标(x₀，y₀，z₀)，即固定探针在机器人基坐标系下的位置坐标。

优选的，所述的机器人夹持的柔性打磨轮位姿辨识方法中，计算每个位置点在机器人末端坐标系中的坐标的方法具体为：

获取每次接触时机器人末端的位置和姿态，并计算得到机器人末端坐标系向机器人基坐标系的旋转矩阵R_i和平移矩阵T_i，则每个位置点在机器人末端坐标系下的坐标c_i(x_i’，y_i’，z_i’)的计算公式为：

优选的，所述的机器人夹持的柔性打磨轮位姿辨识方法中，采用随机抽样一致算法对多个位置点在机器人末端坐标系中的坐标进行平面拟合的步骤具体为：

选取其中三个位置点在机器人末端坐标系下的坐标作为组合，代入公式：

中，求取出A、B、C、D后，采用公式

计算出剩余的坐标的欧式距离d₂，若满足d₂≤δ₂，则视为内点，否则视为外点剔除；其中，δ₂为预设的第二距离阈值，(x_i’，y_i’，z_i’)为位置点在机器人末端坐标系下的坐标，(A，B，C)为所求X轴；

重新选取其中三个位置点在机器人末端坐标系下的坐标作为组合，并进行内点筛选和外点剔除；

当所有的组合都计算完毕时，将内点数最多的组合及其内点作为最小二乘法的求解集合。

优选的，所述的机器人夹持的柔性打磨轮位姿辨识方法中，采用最小二乘法对剩余的坐标进行平面拟合，得到平面法向量，并记做X轴的步骤具体为：

采用最小二乘法对求解集合进行平面拟合，其表达式如下：

分别对a₀、a₁、a₂求偏导数，可得公式：

对公式化简可得公式：

整理成矩阵形式可得公式：

即Ax＝b，求解得a₀、a₁、a₂，代入公式

得A、B、C，单位化向量(A，B，C)即为所求X轴标量。

优选的，所述的机器人夹持的柔性打磨轮位姿辨识方法中，所述以X轴与机器人末端坐标系下X’轴呈锐角的方向作为X轴正方向，以X轴为基准，对机器人末端坐标系下Z’轴进行施密特正交化得Z轴，并采用右手定则得到Y轴，以得到柔性打磨轮坐标系的步骤具体为：

对所求X轴计算X与机器人末端坐标系下的X’轴的点积，记θ＝X·X'，若θ＞0，则X＝X，否则X＝-X；以所得单位向量轴X为基准，对机器人末端坐标系下单位向量轴Z’轴做施密特正交化处理，具体公式为：

根据上述计算得Z轴，并通过右手定则得道Y轴，以得到柔性打磨轮坐标系。

第二方面，本发明还提供一种机器人夹持的柔性打磨轮位姿辨识装置，包括：

工具中心点坐标获取模块，用于控制机器人以多种姿态携带移动探针接触固定探针，并获取各姿态下工具中心点的坐标；其中，所述固定探针固定于预设位置，所述移动探针固定于机器人末端；

固定探针坐标获取模块，用于采用随机抽样一致算法对获取的多个工具中心点的坐标进行球心拟合，剔除误差较大的坐标后，采用最小二乘法对剩余的工具中心点的坐标进行球心拟合，以得到固定探针在机器人基坐标系下的位置坐标；

位置点坐标获取模块，用于将移动探针替换成柔性打磨轮后，在连接轴和打磨轮的交界圆上选取多个位置点，控制所述机器人移动使若干个位置点分别接触所述固定探针后，获取每次接触时机器人末端的位置和姿态，根据每次接触时机器人末端的位置和姿态进行坐标系转换后，根据所述固定探针在机器人基坐标系下的位置坐标计算出每个位置点在机器人末端坐标系中的坐标；

X轴计算模块，用于采用随机抽样一致算法对多个位置点在机器人末端坐标系中的坐标进行平面拟合，剔除误差较大点后，采用最小二乘法对剩余的坐标进行平面拟合，得到平面法向量，并记做X轴；

柔性打磨轮坐标系获取模块，用于以X轴与机器人末端坐标系下X’轴呈锐角的方向作为X轴正方向，以X轴为基准，对机器人末端坐标系下Z’轴进行施密特正交化得Z轴，并采用右手定则得到Y轴，以得到柔性打磨轮坐标系；

磨削点坐标获取模块，用于将每个位置点在机器人末端坐标系中的坐标转换为在柔性打磨轮坐标系中的坐标后，采用随机抽样一致算法对获取的多个位置点在柔性打磨轮坐标系中的坐标进行球心拟合，剔除误差较大的坐标后，采用最小二乘法对剩余的坐标进行球心拟合后，得到交界圆圆心坐标，根据打磨轮平均半径、交界圆圆心坐标以及柔性打磨轮坐标系的单位方向向量得到磨削点坐标。

第三方面，本发明还提供一种机器人夹持的柔性打磨轮位姿辨识设备，包括：处理器和存储器；

所述存储器上存储有可被所述处理器执行的计算机可读程序；

所述处理器执行所述计算机可读程序时实现如上所述的机器人夹持的柔性打磨轮位姿辨识方法中的步骤。

第四方面，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如上所述的机器人夹持的柔性打磨轮位姿辨识方法中的步骤。

相较于现有技术，本发明提供的机器人夹持的柔性打磨轮位姿辨识方法、装置、设备及存储介质，考虑了机器人误差、装夹误差、人工定位误差；Ransac算法结合最小二乘法精确辨识固定探针位置，提高辨识方法的精度和稳定性；拟合打磨轮轴线得X轴，对Tool0下Z’轴施密特正交化矫正得Z轴，右手定则得Y轴，避免装夹误差，精确辨识姿态。拟合交界圆圆心，并结合矫正得Z轴及打磨轮平均半径R精确辨识位置。本发明无需高精密仪器，操作简单，精度及稳定性高，剔除了机器人运动误差较大点，避免了装夹误差、减小了人工定位误差，适用于机器人夹持的柔性打磨轮位姿辨识，如橡胶轮、纱布轮、钢丝轮等常见柔性打磨轮。

附图说明

图1为本发明提供的机器人夹持的柔性打磨轮位姿辨识方法的一较佳实施例的流程图；

图2为本发明8点法辨识固定探针位置示意图；

图3为本发明8位置点固定探针接触打磨轮与连接轴交界圆示意图；

图4为8位置点拟合平面得法线X轴原理图；

图5为Tool0下Z’轴矫正示意图；

图6为传统方法磨削点位姿辨识原理图；

图7为本发明磨削点位姿辨识原理图；

图8为本发明打磨轮位姿辨识及传统方法打磨轮工件接触示意图；

图9为本发明打磨轮工件接触示意图。

附图标记：1-移动探针，2-固定探针，3-打磨轮，4-连接轴，5-交界圆，6-磨削点，7-Tool0下X’轴平行线，8-打磨轮轴线，9-传统方法打磨轮工件接触面，10-六轴法兰，11-本发明打磨轮工件接触面。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。

请参阅图1，本发明实施例提供的机器人夹持的柔性打磨轮位姿辨识方法，包括如下步骤：

S100、控制机器人以多种姿态携带移动探针接触固定探针，并获取各姿态下工具中心点的坐标；其中，所述固定探针固定于预设位置，所述移动探针固定于机器人末端。

本实施例中，如图2所示，机器人携带移动探针以8种位姿接触固定探针，所述姿态的数量为8种，为方便说明，以下以8种位姿状态来对本发明的技术方案进行详细说明。

S200、采用随机抽样一致算法对获取的多个工具中心点的坐标进行球心拟合，剔除误差较大的坐标后，采用最小二乘法对剩余的工具中心点的坐标进行球心拟合，以得到固定探针在机器人基坐标系下的位置坐标。

本实施例中，所述采用随机抽样一致算法(Ransac算法)对获取的多个工具中心点的坐标进行球心拟合的步骤具体为：

对得到的工具中心点在机器人坐标系下的坐标(x_i,y_i,z_i)应满足公式(1)：

(x_i-x₀)²+(y_i-y₀)²+(z_i-z₀)²＝R² 公式(1)

其中(x₀,y₀,z₀)为待求圆心，R为拟合半径，求解(x₀,y₀,z₀)和R需四个坐标点，有

共70种组合；对每一种组合计算圆心(x₀,y₀,z₀)和半径R后，对剩余四点坐标计算欧式距离

设一距离阈值δ₁＝0.1，若满足d≤δ₁，则视为内点，否则视为外点剔除；遍历70种组合，将内点数最多的组合及其内点作为最小二乘求解集合。

Ransac算法球心拟合剔除误差较大点时，对给定的最大容忍距离阈值δ_1max＝0.3，若剔除外点后最小二乘求解集合中点个数小于五，视为对点误差过大，重新实验。

进一步的，采用最小二乘法对剩余的工具中心点的坐标进行球心拟合，以得到固定探针在机器人基坐标系下的位置坐标的步骤具体为：

采用最小二乘法对求解集合进行球心拟合，其表达式如下：

其中，(x_i,y_i,z_i)为求解集合中各点坐标，R为拟合半径，对x₀、y₀、z₀、R求偏导如公式(3)：

利用坐标差值公式(4)将上式(3)简化为下式(5)：

其中

分别为求解集合中各工具中心点坐标(x_i,y_i,z_i)的均值，求解式(5)可以得到a₀、b₀、c₀，代入式(4)可以得到球心坐标(x₀,y₀,z₀)，也即固定探针在机器人基坐标系下的位置坐标。

S300、将移动探针替换成柔性打磨轮后，在连接轴和打磨轮的交界圆上选取多个位置点，控制所述机器人移动使若干个位置点分别接触所述固定探针后，获取每次接触时机器人末端的位置和姿态，根据每次接触时机器人末端的位置和姿态进行坐标系转换后，根据所述固定探针在机器人基坐标系下的位置坐标计算出每个位置点在机器人末端坐标系中的坐标。

本实施例中，如图3所示，机器人携带打磨装置移动使固定探针接触打磨轮和连接轴交界圆上的8个位置点，为方便说明，以下均以8个位置点来进行说明。

具体的，计算每个位置点在机器人末端坐标系中的坐标的方法具体为：

获取每次接触时机器人末端的位置和姿态，并计算得到机器人末端坐标系向机器人基坐标系的旋转矩阵R_i和平移矩阵T_i，则每个位置点在机器人末端坐标系下的坐标c_i(x_i’，y_i’，z_i’)的计算公式为：

S400、采用随机抽样一致算法对多个位置点在机器人末端坐标系中的坐标进行平面拟合，剔除误差较大点后，采用最小二乘法对剩余的坐标进行平面拟合，得到平面法向量，并记做X轴。

具体的，如图4所示，采用随机抽样一致算法对多个位置点在机器人末端坐标系中的坐标进行平面拟合的步骤具体为：

Ransac算法对8个位置点坐标平面拟合剔除误差较大点，具体方法为：

平面的一般方程如式(7)：

Ax+By+Cz+D＝0 公式(7)

其中(A,B,C)为平面法向量即为所求X轴，对平面方程变形可得：

求解A、B、C、D需要三个位置坐标点，有

共56种组合；对每一种组合计算平面参数A、B、C、D后，对剩余五点坐标计算欧式距离

设一距离阈值δ₂＝0.1，若满足d₂≤δ₂，则视为内点，否则视为外点剔除；遍历56种组合，将内点数最多的组合及其内点作为最小二乘求解集合。

Ransac算法平面拟合剔除误差较大点时，对给定的最大容忍距离阈值δ_2max＝0.3，若剔除外点后最小二乘求解集合中点个数小于四，视为对点误差过大，重新实验。

进一步的，采用最小二乘法对剩余的坐标进行平面拟合，得到平面法向量，并记做X轴的步骤具体为：

由求解集合所有点平面距离最小化可得函数表达式如下：

分别对a₀、a₁、a₂求偏导数，可得公式(10)：

对公式(10)化简可得公式(11)：

整理成矩阵形式可得公式(12)：

即Ax＝b，求解可得a₀、a₁、a₂，代入公式(8)可得A、B、C，单位化向量(A,B,C)即为所求X轴标量。

S500、以X轴与机器人末端坐标系下X’轴呈锐角的方向作为X轴正方向，以X轴为基准，对机器人末端坐标系下Z’轴进行施密特正交化得Z轴，并采用右手定则得到Y轴，以得到柔性打磨轮坐标系。

具体的，如图5所示，所述步骤S500具体包括：

对所求X轴计算X与机器人末端坐标系下的X’轴的点积，记θ＝X·X'，若θ＞0，则X＝X，否则X＝-X；以所得单位向量轴X为基准，对机器人末端坐标系下单位向量轴Z’轴做施密特正交化处理，具体公式为：

由公式(13)可得Z轴，右手定则可得Y轴，完成柔性打磨轮姿态辨识。

S600、将每个位置点在机器人末端坐标系中的坐标转换为在柔性打磨轮坐标系中的坐标后，采用随机抽样一致算法对获取的多个位置点在柔性打磨轮坐标系中的坐标进行球心拟合，剔除误差较大的坐标后，采用最小二乘法对剩余的坐标进行球心拟合后，得到交界圆圆心坐标，根据打磨轮平均半径、交界圆圆心坐标以及柔性打磨轮坐标系的单位方向向量得到磨削点坐标。

具体来说，如图6所示，传统方法辨识打磨轮磨削点位置时通过人工定位，存在随机误差，a点为理想定位点，a点以外所有点均会导致工件过磨，如b、c两点，人工定位难以精确定位a点；如图7为本发明打磨轮磨削点位姿辨识原理图，其打磨轮磨削点辨识位置始终为理想定位点，柔性打磨轮坐标系(TCF)的磨削点位置坐标求解的具体方法为：

与8个工具中心点拟合球心原理相同，Ransac算法拟合交界圆圆心剔除误差较大点，构建求解集合后，若求解集合中的点数目小于五个，则重新实验；采用最小二乘法对求解集合中的点拟合得交界圆圆心坐标O₁(x₁,y₁,z₁)，由打磨轮平均半径R可得磨削点位置坐标O₂(x₂,y₂,z₂+R*Z)，完成柔性打磨轮位姿辨识。

如图8为本发明TCF辨识示意图，传统方法以Tool0的姿态作为TCF的姿态，由于存在装夹误差，轴线偏离Tool0下X’轴，会使打磨轮与工件接触面倾斜，造成非均匀磨削；如图9，本发明对TCF姿态重新辨识，轴线与TCF下X轴是平行的，打磨轮与工件接触面平行，属于均匀磨削。

基于上述机器人夹持的柔性打磨轮位姿辨识方法，本发明还相应的提供一种机器人夹持的柔性打磨轮位姿辨识装置，包括：

工具中心点坐标获取模块，用于控制机器人以多种姿态携带移动探针接触固定探针，并获取各姿态下工具中心点的坐标；其中，所述固定探针固定于预设位置，所述移动探针固定于机器人末端；

固定探针坐标获取模块，用于采用随机抽样一致算法对获取的多个工具中心点的坐标进行球心拟合，剔除误差较大的坐标后，采用最小二乘法对剩余的工具中心点的坐标进行球心拟合，以得到固定探针在机器人基坐标系下的位置坐标；

位置点坐标获取模块，用于将移动探针替换成柔性打磨轮后，在连接轴和打磨轮的交界圆上选取多个位置点，控制所述机器人移动使若干个位置点分别接触所述固定探针后，获取每次接触时机器人末端的位置和姿态，根据每次接触时机器人末端的位置和姿态进行坐标系转换后，根据所述固定探针在机器人基坐标系下的位置坐标计算出每个位置点在机器人末端坐标系中的坐标；

X轴计算模块，用于采用随机抽样一致算法对多个位置点在机器人末端坐标系中的坐标进行平面拟合，剔除误差较大点后，采用最小二乘法对剩余的坐标进行平面拟合，得到平面法向量，并记做X轴；

柔性打磨轮坐标系获取模块，用于以X轴与机器人末端坐标系下X’轴呈锐角的方向作为X轴正方向，以X轴为基准，对机器人末端坐标系下Z’轴进行施密特正交化得Z轴，并采用右手定则得到Y轴，以得到柔性打磨轮坐标系；

磨削点坐标获取模块，用于将每个位置点在机器人末端坐标系中的坐标转换为在柔性打磨轮坐标系中的坐标后，采用随机抽样一致算法对获取的多个位置点在柔性打磨轮坐标系中的坐标进行球心拟合，剔除误差较大的坐标后，采用最小二乘法对剩余的坐标进行球心拟合后，得到交界圆圆心坐标，根据打磨轮平均半径、交界圆圆心坐标以及柔性打磨轮坐标系的单位方向向量得到磨削点坐标。

由于上文已对机器人夹持的柔性打磨轮位姿辨识方法进行详细描述，所述机器人夹持的柔性打磨轮位姿辨识方法具备的技术效果，所述机器人夹持的柔性打磨轮位姿辨识装置同样具备，故在此不再赘述。

基于上述机器人夹持的柔性打磨轮位姿辨识方法，本发明还相应的提供一种机器人夹持的柔性打磨轮位姿辨识设备，包括：处理器和存储器；

所述存储器上存储有可被所述处理器执行的计算机可读程序；

所述处理器执行所述计算机可读程序时实现如上述各实施例所述的机器人夹持的柔性打磨轮位姿辨识方法中的步骤。

由于上文已对机器人夹持的柔性打磨轮位姿辨识方法进行详细描述，所述机器人夹持的柔性打磨轮位姿辨识方法具备的技术效果，所述机器人夹持的柔性打磨轮位姿辨识设备同样具备，故在此不再赘述。

基于上述机器人夹持的柔性打磨轮位姿辨识方法，本发明还相应的提供一种计算机可读存储介质，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如上述各实施例所述的机器人夹持的柔性打磨轮位姿辨识方法中的步骤。

由于上文已对机器人夹持的柔性打磨轮位姿辨识方法进行详细描述，所述机器人夹持的柔性打磨轮位姿辨识方法具备的技术效果，所述计算机可读存储介质同样具备，故在此不再赘述。

综上所述，本发明提供的机器人夹持的柔性打磨轮位姿辨识方法、装置、设备及存储介质，考虑了机器人误差、装夹误差、人工定位误差；Ransac算法结合最小二乘法精确辨识固定探针位置，提高辨识方法的精度和稳定性；拟合打磨轮轴线得X轴，对Tool0下Z’轴施密特正交化矫正得Z轴，右手定则得Y轴，避免装夹误差，精确辨识姿态。拟合交界圆圆心，并结合矫正得Z轴及打磨轮平均半径R精确辨识位置。本发明无需高精密仪器，操作简单，精度及稳定性高，剔除了机器人运动误差较大点，避免了装夹误差、减小了人工定位误差，适用于机器人夹持的柔性打磨轮位姿辨识，如橡胶轮、纱布轮、钢丝轮等常见柔性打磨轮。

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

Claims (10)

1.一种机器人夹持的柔性打磨轮位姿辨识方法，其特征在于，包括如下步骤：

控制机器人以多种姿态携带移动探针接触固定探针，并获取各姿态下工具中心点的坐标；其中，所述固定探针固定于预设位置，所述移动探针固定于机器人末端；

采用随机抽样一致算法对获取的多个工具中心点的坐标进行球心拟合，剔除误差较大的坐标后，采用最小二乘法对剩余的工具中心点的坐标进行球心拟合，以得到固定探针在机器人基坐标系下的位置坐标；

将移动探针替换成柔性打磨轮后，在连接轴和打磨轮的交界圆上选取多个位置点，控制所述机器人移动使若干个位置点分别接触所述固定探针后，获取每次接触时机器人末端的位置和姿态，根据每次接触时机器人末端的位置和姿态进行坐标系转换后，根据所述固定探针在机器人基坐标系下的位置坐标计算出每个位置点在机器人末端坐标系中的坐标；

采用随机抽样一致算法对多个位置点在机器人末端坐标系中的坐标进行平面拟合，剔除误差较大点后，采用最小二乘法对剩余的坐标进行平面拟合，得到平面法向量，并记做X轴；

以X轴与机器人末端坐标系下X’轴呈锐角的方向作为X轴正方向，以X轴为基准，对机器人末端坐标系下Z’轴进行施密特正交化得Z轴，并采用右手定则得到Y轴，以得到柔性打磨轮坐标系；

将每个位置点在机器人末端坐标系中的坐标转换为在柔性打磨轮坐标系中的坐标后，采用随机抽样一致算法对获取的多个位置点在柔性打磨轮坐标系中的坐标进行球心拟合，剔除误差较大的坐标后，采用最小二乘法对剩余的坐标进行球心拟合后，得到交界圆圆心坐标，根据打磨轮平均半径、交界圆圆心坐标以及柔性打磨轮坐标系的单位方向向量得到磨削点坐标。

2.根据权利要求1所述的机器人夹持的柔性打磨轮位姿辨识方法，其特征在于，所述采用随机抽样一致算法对获取的多个工具中心点的坐标进行球心拟合的步骤具体为：

选取其中四个工具中心点的坐标作为组合，代入公式：(x_i-x₀)²+(y_i-y₀)²+(z_i-z₀)²＝R²中，求取出圆心(x₀，y₀，z₀)和半径R后，采用公式

计算出剩余的坐标的欧式距离d，若满足d≤δ₁，则视为内点，否则视为外点剔除；其中，δ₁为预设的第一距离阈值，(x_i，y_i，z_i)为工具中心点的坐标；

重新选取其中四个工具中心点的坐标作为组合，并进行内点筛选和外点剔除；

当所有的组合都计算完毕时，将内点数最多的组合及其内点作为最小二乘法的求解集合。

3.根据权利要求2所述的机器人夹持的柔性打磨轮位姿辨识方法，其特征在于，采用最小二乘法对剩余的工具中心点的坐标进行球心拟合，以得到固定探针在机器人基坐标系下的位置坐标的步骤具体为：

采用最小二乘法对求解集合进行球心拟合，其表达式如下：

其中，(x_i，y_i，z_i)为求解集合中各点坐标，R为拟合半径；

采用下列公式对x₀、y₀、z₀、R求偏导，

利用坐标差值公式将上述公式进行简化，具体公式为：

其中

分别为求解集合中各工具中心点坐标(x_i，y_i，z_i)的均值，求解得到a₀、b₀、c₀，根据a₀、b₀、c₀得到球心坐标(x₀，y₀，z₀)，即固定探针在机器人基坐标系下的位置坐标。

4.根据权利要求3所述的机器人夹持的柔性打磨轮位姿辨识方法，其特征在于，计算每个位置点在机器人末端坐标系中的坐标的方法具体为：

获取每次接触时机器人末端的位置和姿态，并计算得到机器人末端坐标系向机器人基坐标系的旋转矩阵R_i和平移矩阵T_i，则每个位置点在机器人末端坐标系下的坐标c_i(x_i’，y_i’，z_i’)的计算公式为：

5.根据权利要求4所述的机器人夹持的柔性打磨轮位姿辨识方法，其特征在于，采用随机抽样一致算法对多个位置点在机器人末端坐标系中的坐标进行平面拟合的步骤具体为：

选取其中三个位置点在机器人末端坐标系下的坐标作为组合，代入公式：

中，求取出A、B、C、D后，采用公式

计算出剩余的坐标的欧式距离d₂，若满足d₂≤δ₂，则视为内点，否则视为外点剔除；其中，δ₂为预设的第二距离阈值，(x_i’，y_i’，z_i’)为位置点在机器人末端坐标系下的坐标，(A，B，C)为所求X轴；

重新选取其中三个位置点在机器人末端坐标系下的坐标作为组合，并进行内点筛选和外点剔除；

当所有的组合都计算完毕时，将内点数最多的组合及其内点作为最小二乘法的求解集合。

6.根据权利要求5所述的机器人夹持的柔性打磨轮位姿辨识方法，其特征在于，采用最小二乘法对剩余的坐标进行平面拟合，得到平面法向量，并记做X轴的步骤具体为：

采用最小二乘法对求解集合进行平面拟合，其表达式如下：

分别对a₀、a₁、a₂求偏导数，可得公式：

对公式化简可得公式：

整理成矩阵形式可得公式：

即Ax＝b，求解得a₀、a₁、a₂，代入公式

得A、B、C，单位化向量(A，B，C)即为所求X轴标量。

7.根据权利要求6所述的机器人夹持的柔性打磨轮位姿辨识方法，其特征在于，所述以X轴与机器人末端坐标系下X’轴呈锐角的方向作为X轴正方向，以X轴为基准，对机器人末端坐标系下Z’轴进行施密特正交化得Z轴，并采用右手定则得到Y轴，以得到柔性打磨轮坐标系的步骤具体为：

对所求X轴计算X与机器人末端坐标系下的X’轴的点积，记θ＝X·X'，若θ＞0，则X＝X，否则X＝-X；以所得单位向量轴X为基准，对机器人末端坐标系下单位向量轴Z’轴做施密特正交化处理，具体公式为：

根据上述计算得Z轴，并通过右手定则得道Y轴，以得到柔性打磨轮坐标系。

8.一种机器人夹持的柔性打磨轮位姿辨识装置，其特征在于，包括：

工具中心点坐标获取模块，用于控制机器人以多种姿态携带移动探针接触固定探针，并获取各姿态下工具中心点的坐标；其中，所述固定探针固定于预设位置，所述移动探针固定于机器人末端；

固定探针坐标获取模块，用于采用随机抽样一致算法对获取的多个工具中心点的坐标进行球心拟合，剔除误差较大的坐标后，采用最小二乘法对剩余的工具中心点的坐标进行球心拟合，以得到固定探针在机器人基坐标系下的位置坐标；

位置点坐标获取模块，用于将移动探针替换成柔性打磨轮后，在连接轴和打磨轮的交界圆上选取多个位置点，控制所述机器人移动使若干个位置点分别接触所述固定探针后，获取每次接触时机器人末端的位置和姿态，根据每次接触时机器人末端的位置和姿态进行坐标系转换后，根据所述固定探针在机器人基坐标系下的位置坐标计算出每个位置点在机器人末端坐标系中的坐标；

X轴计算模块，用于采用随机抽样一致算法对多个位置点在机器人末端坐标系中的坐标进行平面拟合，剔除误差较大点后，采用最小二乘法对剩余的坐标进行平面拟合，得到平面法向量，并记做X轴；

柔性打磨轮坐标系获取模块，用于以X轴与机器人末端坐标系下X’轴呈锐角的方向作为X轴正方向，以X轴为基准，对机器人末端坐标系下Z’轴进行施密特正交化得Z轴，并采用右手定则得到Y轴，以得到柔性打磨轮坐标系；

磨削点坐标获取模块，用于将每个位置点在机器人末端坐标系中的坐标转换为在柔性打磨轮坐标系中的坐标后，采用随机抽样一致算法对获取的多个位置点在柔性打磨轮坐标系中的坐标进行球心拟合，剔除误差较大的坐标后，采用最小二乘法对剩余的坐标进行球心拟合后，得到交界圆圆心坐标，根据打磨轮平均半径、交界圆圆心坐标以及柔性打磨轮坐标系的单位方向向量得到磨削点坐标。

9.一种机器人夹持的柔性打磨轮位姿辨识设备，其特征在于，包括：处理器和存储器；

所述存储器上存储有可被所述处理器执行的计算机可读程序；

所述处理器执行所述计算机可读程序时实现如权利要求1-7任意一项所述的机器人夹持的柔性打磨轮位姿辨识方法中的步骤。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如权利要求1-7任意一项所述的机器人夹持的柔性打磨轮位姿辨识方法中的步骤。

Images