CN115256400B - 机器人三自由度电驱动耦合关节的运动可行范围线性界定方法 - Google Patents

Abstract

本发明公开了机器人三自由度电驱动耦合关节的运动可行范围线性界定方法，由未超限电机转角向量对应的关节角向量构成运动可行关节角向量集合，将运动可行关节角向量集合沿任意关节的关节可行运动范围分层切片，得到数据切片子集并进行边缘检测，得到边缘可行切片集合；整合所有边缘可行切片集合，按所属近似平面分类，得到待拟合集合；对每一个待拟合集合进行内切平面拟合，求解拟合内切平面表达式；整合所有拟合内切平面表达式，得到耦合关节运动空间可行运动范围凸多面体半空间表达式，凸多面体半空间表达式的线性不等式用于机器人关节端的轨迹规划或轨迹优化。本发明能对耦合关节的耦合关系进行线性描述，最大程度的界定可行的线性运动范围。

Description

机器人三自由度电驱动耦合关节的运动可行范围线性界定 方法

技术领域

本发明涉及机器人技术领域，具体涉及一种机器人三自由度电驱动耦合关节的运动可行范围线性界定方法。

背景技术

机器人的运动自由度由对应的运动关节实现。针对电机驱动的关节，根据机构构型，不同的运动关节和驱动电机的对应关系并不相同。出于精简机构构型与体积、提高驱动最大输出力矩等原因，许多机器人整机设计上不再采用单一的串联关节，而是采用耦合关节。比如，两个驱动电机同时控制机器人两个关节的运动，这两个关节的运动范围存在非线性的耦合关系，即使两个关节的关节角都处于各自的运动可行范围内，仍然可能造成驱动电机超限。耦合关节的电机和关节的运动不再具有串联关节那样简单的一一对应的线性关系，因此，在对具有耦合关节的机器人进行运动轨迹规划或运动轨迹优化时，需要更进一步的保证电机端没有超出运动范围限制。现有技术往往直接对耦合关节进行运动范围解耦，将各关节近似为串联关节，仅界定各关节的独立可行运动范围，其界定的运动范围比较保守，无法充分发挥机器人的运动潜能，一些高动态、接近运动极限的运动无法实现。

发明内容

针对现有技术中存在不足，本发明提供了一种机器人三自由度电驱动耦合关节的运动可行范围线性界定方法，能够对三自由度耦合关节的运动范围耦合关系进行线性描述，在耦合关节允许的运动范围内，可充分发挥机器人的运动潜能。

本发明是通过以下技术手段实现上述技术目的的。

机器人三自由度电驱动耦合关节的运动可行范围线性界定方法：

设定耦合关节中各关节的关节角遍历范围及遍历采样间隔，验证每个关节角向量对应的电机转角向量是否超出物理限制，将未超限电机转角向量对应的关节角向量作为运动可行关节角向量集合SetQ_adm的元素；

将所述集合SetQ_adm沿任意关节的关节可行运动范围分层切片，得到数据切片子集，对所述数据切片子集进行边缘检测，得到边缘可行切片集合；

整合所有边缘可行切片集合，按所属近似平面分类，得到待拟合集合；

对每一个待拟合集合，进行内切平面拟合，求解拟合内切平面表达式；

整合所有拟合内切平面表达式，得到耦合关节运动空间可行运动范围凸多面体半空间表达式，所述凸多面体半空间表达式的线性不等式用于机器人关节端的轨迹规划或轨迹优化。

进一步的技术方案，所述电机转角向量是否超出物理限制的判定规则为：当电机转角向量中各电机转角均处于各自极值范围内时，判定电机转角向量对应的关节角向量为可行关节角向量，否则，判定关节角向量为超限关节角向量并将其舍弃。

进一步的技术方案，所述数据切片子集进行边缘检测，得到边缘可行切片集合的具体过程为：将切片子集对应的二维点云图像转化为二值化黑白图像，对二值化黑白图像进行边缘检测，得到图形轮廓边缘像素点，基于所述边缘像素点与可行关节角向量的映射关系，确定边缘可行关节角向量，进而得到边缘可行切片集合。

进一步的技术方案，每一个待拟合集合对应的拟合内切平面表达式为：

其中为拟合平面系数，q₁为1关节的关节角，q₂为2关节的关节角，q₃为3关节的关节角。

更进一步的技术方案，所述拟合平面系数满足：

其中：ω为方向调节因子，A^c、B^c、C^c为拟合平面方程系数，

更进一步的技术方案，所述拟合平面系数满足：

其中：表示拟合平面最终的法向量，且/>为最靠近凸多面体的拟合可行关节角向量。

更进一步的技术方案，所述最靠近凸多面体的拟合可行关节角向量为：其中：/>为待拟合可行关节角向量，/>为待拟合可行关节角向量集合。

更进一步的技术方案，所述耦合关节运动空间可行运动范围凸多面体半空间表达式为：αQ≤β，其中α为拟合平面法向量矩阵，β为拟合平面距离矩阵，Q为耦合关节的关节角，且Q＝(q₁ q₂ q₃)^T，C为耦合关节运动范围对应的三维曲面几何体面数。

本发明的有益效果为：本发明机器人三自由度电驱动耦合关节的运动可行范围线性界定方法，以线性不等式约束的形式界定耦合关节最大线性运动范围，直接用于机器人关节端的轨迹规划或轨迹优化，能够最大程度的界定可行的线性运动范围，充分发挥机器人的运动潜能。

附图说明

图1为本发明所述机器人三自由度电驱动耦合关节的运动可行范围线性界定方法流程图；

图2为本发明所述机器人三自由度电驱动耦合关节的运动可行范围线性界定方法几何表征变化说明图。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。

因机构设计、运动需求等原因，机器人关节中存在运动范围耦合关系的关节被称为耦合(并联)关节，耦合关节的关节数大于等于2。

本发明所述机器人电驱动耦合关节的运动可行范围线性界定方法如图1所示，首先，设定耦合关节中各关节的关节角q_i(i＝1，2，3)的遍历范围及遍历采样间隔q_s，验证每个关节角向量Q_α所对应的电机端电机转角向量E_α是否超出物理限制，对未超限的E_α，记录其对应的Q_α，作为运动可行关节角向量集合SetQ_adm的元素；其次，对SetQ_adm，沿任意关节的关节可行运动范围将其分层切片，得到数据切片子集，对数据切片子集，采用Sobel算子进行边缘检测，得到边缘可行切片集合；然后，整合所有边缘可行切片集合，将其按所属近似平面分类，得到待拟合集合；随后，对每一个待拟合集合，进行内切平面拟合，求解拟合内切平面表达式；最后，整合所有拟合内切平面表达式，得到耦合关节运动空间可行运动范围凸多面体半空间表达式，其线性不等式的形式可作为直接验证耦合关节各关节角是否超限的依据，用于机器人轨迹规划或轨迹优化。具体的步骤如下：

步骤一，遍历所有耦合关节的关节角向量，仅保留满足电机端电机转角运动范围的可行关节角向量Q_adm，得到可行关节角向量集合(可行集合)；

记三自由度耦合关节各关节为1关节、2关节和3关节，为保证不遗漏任何可行关节角向量Q_adm，设定各关节的关节角遍历范围均为(-π，π)，即：

-π＜q_i＜π，i＝1，2，3 (1)

设定关节角遍历采样间隔为q_s，则对每个关节，其关节角遍历采样个数为：

其中为第i个关节的遍历采样个数，round(·)为取整函数；则第r个待测关节角向量/>为：

其中r，m，n，p分别为待测关节角向量1关节的关节角、2关节的关节角和3关节的关节角的序号(为正整数)，且有：

待测关节角向量的总数num_α为则待测关节角向量集合SetQ_α可表达为：

对第r个待测关节角向量经并联机构逆运动学求解，可得到其对应第r个电机转角向量/>其表达式为：

其中inversekinematic(·)为耦合关节所对应并联机构的逆运动学函数，e₁、e₂和e₃分别为1电机、2电机和3电机的电机转角。

各电机的转角运动范围在生产制造时已经确定，设各电机的转角运动极大极小值分别为和/>则关节角向量电机端可行的判定规则为，当电机转角向量/>中各电机转角均处于各自极大极小值范围内时，判定/>对应的/>为可行关节角向量Q_adm，否则，判定/>为超限关节角向量Q_imp，将其舍弃，即：

其中E^min、E^max分别为电机转角极小值向量和极大值向量，且

依据公式(7)，对所有待测关节角向量进行电机端可行判定，得到可行关节角向量集合SetQ_adm，其表达式为：

显然SetQ_adm为SetQ_α的子集，即

步骤二，对可行关节角向量集合(可行集合)沿某一关节的关节可行运动范围进行集合切片，将其分解为若干个子集，每个子集的切片关节的关节角为固定值；

对可行关节角向量集合SetQ_adm，遍历其元素可得到三个关节的可行运动极大极小值为和/>选取其中一个关节作为切片关节，如选取3关节作为切片关节，则对第k层切片子集，其元素关节角向量的3关节的关节角为：

其中层数最小值层数最大值/>

依据公式(9)，第k层切片子集SetQ_sli的表达式为：

显然为SetQ_adm的子集，且所有切片子集的并集等于SetQ_adm，各切片子集交集为零集；Q_adm(3)为可行关节角向量的3关节分量。

步骤三，对切片子集，通过边缘检测算法分离出切片子集中边缘可行关节角向量，从而剥离出边缘可行切片集合；

对每层切片子集，因3关节的关节角为固定值，因此，其空间表达可降维为平面表达，即以1关节的关节角q₁和2关节的关节角q₂分别为X轴、Y轴，以零点为原点，可获得XY坐标系下可行关节角向量的二维点云图像表达，每个属于切片子集的可行关节角向量即为对应图像上的点；对切片子集，为分离其二维点云中最外围的轮廓点云(可行关节角向量)，采用Sobel算子对点云图像进行边缘检测，以分离出边缘可行切片集合。

首先，将切片二维点云图像转化为二值化黑白图像，黑白图像的大小由1关节和2关节在切片子集中的极大值、极小值与采样间隔的比值确定，第k层切片子集对应的黑白图像大小为：

其中size(Figure^k)为第k层切片子集对应的黑白图像的大小，即像素个数； 分别为1关节和2关节属于第k层切片子集的可行关节角向量的极大值和极小值。

对该黑白图像，对第k层切片子集中所有可行关节角向量位置对应的像素点赋1(白)，对其余点赋0(黑)。切片子集中的可行关节角向量和黑白图像Figure^k中值为1的像素点建立了一一对应的映射关系。

对黑白图像Figure^k，采用Sobel算子进行边缘检测，Sobel水平算子和Sobel垂直算子分别为：

黑白图像Figure^k的水平梯度图和垂直梯度图/>可由对应算子与Figure^k进行卷积后得到，即：

其中*代表卷积运算。

则Figure^k的梯度图F^k为：

梯度图F^k与图像Figure^k相比，仅保留了图形轮廓边缘的值为1的像素点。依据可行关节角向量和像素点的映射关系，从这些像素点找到与其对应的边缘可行关节角向量这些边缘可行关节角向量则组成了第k层边缘可行关节角向量集合/>即：

其中表示与边缘可行关节角向量对应的值为1的像素点。

运用公式(10)-(15)可得到所有切片子集对应的边缘可行关节角向量集合(边缘可行切片集合)；边缘可行关节角向量集合是切片子集的子集，即且各边缘可行关节角向量集合交集为零集。

步骤四，整合所有边缘可行关节角向量集合(边缘可行切片集合)，对边缘可行关节角向量按所属近似平面分类，得到待拟合可行关节角向量集合；

将不同层的边缘可行关节角向量集合进行合并，可得到边缘可行关节角向量总集合/>(边缘可行总集合)，即：

以1关节的关节角q₁、2关节的关节角q₂和3关节的关节角q₃分别为X轴、Y轴和Z轴，以零点为原点，可获得XYZ坐标系下边缘可行关节角向量总集合SetQ_edge的三维点云图像表示，从图像中可直观的看出耦合关节运动范围是三维曲面几何体，印证了耦合关节的耦合关系的非线性，为了得到耦合关系的线性描述，需要求取三维曲面几何体的内切最大凸多面体的数学表征，即耦合关节运动空间可行运动范围凸多面体半空间表达式。从三维点云图像可确定该几何体的面数，记该几何体的面数为C，以所属面作区分，将SetQ_edge分解为C个待拟合可行关节角向量集合即：

其中Surface^c代表待拟合可行关节角向量所属面；所有/>的并集为边缘可行关节角向量总集合SetQ_edge，即：

步骤五，依据待拟合可行关节角向量集合(待拟合集合)中的待拟合可行关节角向量，分别作内切平面拟合；

待拟合可行关节角向量集合在XYZ坐标系中是三维曲面几何体的其中一个面，如果该面为曲面，则需要将该面通过点云数据平面拟合的方式近似为平面，以求取耦合关节耦合关系的线性表达。

对第c个拟合可行关节角向量集合设其内切拟合平面的方程为：

A^cq₁+B^cq₂+C^cq₃+D^c＝0 (19)

其中A^c、B^c、C^c、D^c为平面方程系数，q¹、q²、q³为XYZ坐标系下的X轴、Y轴和Z轴坐标(对应关节的关节角)；

采用矩阵奇异值分解(SVD)法求解拟合平面，其约束条件为：

(A^c)²+(B^c)²+(C^c)²＝1 (20)

对中的待拟合可行关节角向量，其平均向量可表达为：

其中分别为/>中的待拟合可行关节角向量的1关节分量2关节分量/>和3关节分量/>的平均值，即：

其中num_fit为中待拟合可行关节角向量/>的总数。

系数矩阵R的表达式为：

其中为/>中第u个待拟合可行关节角向量， 代表实数域上的num_fit×3维线性空间，其余表示以此类推。对系数矩阵R进行SVD分解，即：

R＝UDV^T (24)

其中为左奇异酉矩阵；/>为奇异值对角矩阵，且最后一个对角元素为矩阵R的最小奇异值；/>为右奇异酉矩阵，V(ρ，σ)为矩阵V第ρ行、第σ列的元素(1≤ρ≤3，1≤σ≤3)；

则求得平面方程系数A^c、B^c、C^c分别为：

该拟合平面的法向量为：

η^c＝(A^c B^c C^c)^T (26)

为保证后续求解得到的耦合关节可行运动范围凸多面体的半空间表达符号一致性，要求各平面法向量指向凸多面体外侧，若η^c指向内侧，则将平面方程系数A^c、B^c、C^c取反，若η^c指向外侧，则保留原系数，即最终的拟合平面系数为：

其中ω为方向调节因子，如η^c指向凸多面体外侧，则ω＝0，η^c指向凸多面体内侧，则ω＝l；

对平面方程系数D^c，为保证拟合平面内切所代表的曲面，则应选取最靠近凸多面体的拟合可行关节角向量作为通过该拟合平面的坐标点，记最靠近凸多面体的拟合可行关节角向量为/>其求取方式为：

其中代表向量内积运算，代表拟合平面最终的法向量，其表达式为：

拟合平面系数与平面方程系数D^c符号相同，则其求解公式为：

则对其对应的拟合平面方程为：

通过式(19)-(31)，可求解所有拟合可行关节角向量集合所对应的拟合平面方程。

步骤六，使用步骤五求解得到的所有拟合平面方程的系数构建耦合关节运动空间可行运动范围凸多面体半空间表达式；

耦合关节运动空间可行运动范围凸多面体半空间表达式为：

αQ≤β (32)

其中α为拟合平面法向量矩阵，β为拟合平面距离矩阵，其表达式分别为：

Q为耦合关节的关节角，即：

Q＝(q₁ q₂ q₃)^T (34)

式(32)是线性不等式，是对机器人电驱动耦合关节各关节耦合关系的线性描述，该式以耦合关节运动范围内切凸多面体的几何形式界定了耦合关节的最大线性运动范围，用于机器人关节端的轨迹规划或轨迹优化。

步骤一到步骤六的几何表征变化如图2所示，将高度非线性的复杂曲面体转换为线性的内切最大凸多面体。

基于与一种机器人电驱动耦合关节的运动可行范围线性界定方法相同的发明构思，本申请还提供了一种电子设备，该电子设备包括一个或多个处理器和一个或多个存储器，存储器中存储了计算机可读代码，其中，计算机可读代码当由一个或多个处理器执行时，进行机器人电驱动耦合关节的运动可行范围线性界定方法的实施。其中，存储器可以包括非易失性存储介质和内存储器；非易失性存储介质可存储操作系统和计算机可读代码。该计算机可读代码包括程序指令，该程序指令被执行时，可使得处理器执行任意一种机器人电驱动耦合关节的运动可行范围线性界定方法。处理器用于提供计算和控制能力，支撑整个电子设备的运行。存储器为非易失性存储介质中的计算机可读代码的运行提供环境，该计算机可读代码被处理器执行时，可使得处理器执行任意一种机器人电驱动耦合关节的运动可行范围线性界定方法。

应当理解的是，处理器可以是中央处理单元(Central Processing Unit，CPU)，该处理器还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。其中，通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

本申请的实施例中还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可读代码，所述计算机可读代码中包括程序指令，所述处理器执行所述程序指令，实现本申请的机器人电驱动耦合关节的运动可行范围线性界定方法。

其中，所述计算机可读存储介质可以是前述实施例所述电子设备的内部存储单元，例如所述计算机设备的硬盘或内存。所述计算机可读存储介质也可以是所述电子设备的外部存储设备，例如所述电子设备上配备的插接式硬盘、智能存储卡(SmartMedia Card，SMC)、安全数字(Secure Digital，SD)卡、闪存卡(Flash Card)等。

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (8)

1.机器人三自由度电驱动耦合关节的运动可行范围线性界定方法，其特征在于：

设定耦合关节中各关节的关节角遍历范围及遍历采样间隔，验证每个关节角向量对应的电机转角向量是否超出物理限制，将未超限电机转角向量对应的关节角向量作为运动可行关节角向量集合SetQ_adm的元素；

将所述集合SetQ_adm沿任意关节的关节可行运动范围分层切片，得到数据切片子集，对所述数据切片子集进行边缘检测，得到边缘可行切片集合；

整合所有边缘可行切片集合，按所属近似平面分类，得到待拟合集合；

对每一个待拟合集合，进行内切平面拟合，求解拟合内切平面表达式；

整合所有拟合内切平面表达式，得到耦合关节运动空间可行运动范围凸多面体半空间表达式，所述凸多面体半空间表达式的线性不等式用于机器人关节端的轨迹规划或轨迹优化。

2.根据权利要求1所述的机器人三自由度电驱动耦合关节的运动可行范围线性界定方法，其特征在于，所述电机转角向量是否超出物理限制的判定规则为：当电机转角向量中各电机转角均处于各自极值范围内时，判定电机转角向量对应的关节角向量为可行关节角向量，否则，判定关节角向量为超限关节角向量并将其舍弃。

3.根据权利要求1所述的机器人三自由度电驱动耦合关节的运动可行范围线性界定方法，其特征在于，所述数据切片子集进行边缘检测，得到边缘可行切片集合的具体过程为：将切片子集对应的二维点云图像转化为二值化黑白图像，对二值化黑白图像进行边缘检测，得到图形轮廓边缘像素点，基于所述边缘像素点与可行关节角向量的映射关系，确定边缘可行关节角向量，进而得到边缘可行切片集合。

4.根据权利要求1所述的机器人三自由度电驱动耦合关节的运动可行范围线性界定方法，其特征在于，每一个待拟合集合对应的拟合内切平面表达式为：

其中为拟合平面系数，q₁为1关节的关节角，q₂为2关节的关节角，q₃为3关节的关节角。

5.根据权利要求4所述的机器人三自由度电驱动耦合关节的运动可行范围线性界定方法，其特征在于，所述拟合平面系数满足：

其中：ω为方向调节因子，A^c、B^c、C^c为拟合平面方程系数。

6.根据权利要求4所述的机器人三自由度电驱动耦合关节的运动可行范围线性界定方法，其特征在于，所述拟合平面系数满足：

其中：表示拟合平面最终的法向量，且/> 为最靠近凸多面体的拟合可行关节角向量。

7.根据权利要求6所述的机器人三自由度电驱动耦合关节的运动可行范围线性界定方法，其特征在于，所述最靠近凸多面体的拟合可行关节角向量为：其中：/>为待拟合可行关节角向量，/>为待拟合可行关节角向量集合。

8.根据权利要求7所述的机器人三自由度电驱动耦合关节的运动可行范围线性界定方法，其特征在于，所述耦合关节运动空间可行运动范围凸多面体半空间表达式为：αQ≤β，其中α为拟合平面法向量矩阵，β为拟合平面距离矩阵，Q为耦合关节的关节角，且Q＝(q₁ q₂q₃)^T，C为耦合关节运动范围对应的三维曲面几何体面数。