CN111300425B - 一种超冗余机械臂末端轨迹运动规划方法 - Google Patents

Abstract

本发明公开了一种超冗余机械臂末端轨迹运动规划方法，包括以下步骤：（1）根据工作环境，获得工作空间中需要通过的标记点；（2）利用三次B‑spline曲线，对标记点进行拟合，生成路径曲线；（3）根据基座进给量，获得机械臂各UCR并联关节的工作状态；（4）实时调整工作状态UCR并联关节的位姿，使得机械臂沿着三次B‑spline路径曲线运动；（5）根据各UCR并联关节的位姿和驱动方程，求解出各关节支链的杆长并输出。本发明优点：机械臂结构紧凑，刚度大，承载能力强，可靠性高；严格保证机械臂末端处于三次B‑spline路径曲线上，精度高；通过UCR并联关节驱动方程求解，求解精度高，效率快。

Description

一种超冗余机械臂末端轨迹运动规划方法

技术领域

本发明属于计算机辅助制造及机器人加工技术领域，更具体地，涉及一种超冗余机械臂末端轨迹运动规划方法。

背景技术

随着工作环境的复杂化和狭窄化，超冗余机器人得到了广泛的研究和应用。超冗余机器人是指自由度数远多于完成作业任务所需要最少自由度的机器人。超冗余机器人因其灵活性强，自由度高等特点，使其具备良好的环境适应性。目前，超冗余机器人在航空航天制造、核电设备检修、危险材料处理等领域得到了广泛的研究和应用。

实现避障运动是超冗余机械臂运动规划的主要目标之一。目前学者们提出了多种有效的避障规划方法，如人工势能场法、神经网络算法等。但是，上述避障算法只强调末端作业能力，而未对关节运动进行严格的约束。

为了进一步提高超冗余机械臂避障的能力，跟随末端轨迹的运动方法被提出。该方法通过保证超冗余机械臂的末端和关节尽可能逼近规划的路径曲线，来严格进行避障。但是，目前设计的超冗余机械臂多为串联结构，刚度低，承载能力差。且不能保证关节在路径曲线上，精度低。同时，求解复杂，效率低。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种超冗余机械臂末端轨迹运动规划方法，其目的在于提出一种适用于狭小空间和复杂深腔作业的串并联混合机械臂末端轨迹跟随方法。机械臂在进给基座的配合下，严格按照规划的路径曲线进行移动，从而进行严格避障。

为实现上述目的，按照本发明的一个方面，提供了一种超冗余机械臂末端轨迹运动规划方法，包括如下步骤：

(1)根据工作环境，获得工作空间中超冗余机械臂需要通过的标记点；

(2)利用三次B-spline曲线，对标记点进行拟合，生成路径曲线；

(3)根据基座进给量，获得机械臂各UCR并联关节的工作状态；

(4)实时调整工作状态UCR并联关节的位姿，使得机械臂沿着三次B-spline路径曲线运动；

(5)根据各UCR并联关节的位姿和驱动方程，求解出各关节支链的杆长，并输出。

优选地，其步骤(3)包括如下子步骤：

S11、所述运动机构由超冗余机械臂与进给基座两部分组成；其中，机械臂由8个UCR并联关节串联而成，相邻2个UCR并联关节共用一个平台，即每个UCR并联关节的动平台也是下一个UCR并联关节的定平台；每个UCR并联关节定平台和动平台之间存在沿关节轴线60°的相位差，并从基座开始对机械臂UCR并联关节进行顺序编号，分别记为1～8；

S12、假设机械臂末端动平台中心为三次B-spline路径曲线的起点，每个UCR并联关节的初始长度相等，且为d₀；机械臂在进行末端跟随运动时，基座提供进给量F，机械臂各UCR并联关节依次进入工作区域，并记录其对应的编号。

对本发明技术方案的优选，UCR(Universal-Cylindrical-Revolute)并联关节为具有绕X，Y轴转动和绕Z轴移动(两转一移)的三自由度并联机构。该机构由定平台、三个支链和动平台组成。支链从下至上由万向副(Universal)、圆柱副(Cylindrical)和转动副(Revolute)组成。支链的万向副与定平台相连，转动副与动平台相连。

对本发明技术方案的优选，UCR并联关节各支链的初始状态保持一致，即圆柱副的伸缩量(三个支链的长度)一致，动平台与定平台姿态相同。通过调整三个支链的长度，可以对UCR并联关节中动平台相对于定平台的位姿进行调节。

优选地，其步骤(4)包括如下子步骤：

S21、计算各工作状态UCR并联关节在B-spline路径曲线上的离散点；首先在工作状态UCR并联关节最大编号的定平台处建立基坐标系为：

坐标原点：

则第一个坐标系O₀-x₀y₀z₀已知，然后通过在[0，1]区间的二分法，在B-spline路径曲线上找到离散点，对应参数为t₁，使得通过位姿矩阵

反算出的UCR杆长处于行程范围，并记录参数t₁处对应的坐标系O₁-x₁y₁z₁。然后通过在[t₁，1]区间的二分法，在B-spline路径曲线上找到第二个离散点，对应参数t₂。依次类推，可以找到每个工作状态UCR并联关节动平台在B-spline路径曲线上对应的离散数据点；

S22、B-spline路径曲线中离散数据点的Z轴沿该点处的切向方向。假设相邻两个离散点对应的参数分t_n-1和t_n，则对应的离散点分别为：c(t_n-1)和c(t_n)。则Z_n-1和Z_n分别为切向量c′(t_n-1)和c′(t_n)的单位向量；

S23、三维空间中物体的姿态与旋转变换可以用四元数来描述。三维空间中，绕单位向量n旋转2θ可以用四元数q＝[cosθ，nsinθ]＝q₀+q₁i+q₂j+q₃k来描述。

其中，

2θ＝arccos(z_n-1·z_n)，q₀＝cosθ；q₁，q₂，q₃为向量nsinθ在其考坐标轴i，j，k上的分量。

则单位四元数q＝(q₀，q₁，q₂，q₃)^T对应的旋转矩阵

为：

假设O_n-1-x_n-1y_n-1z_n-1坐标系已知，则O_n在O_n-1-x_n-1y_n-1z_n-1中投影坐标为：

则相邻两个离散点之间的位姿矩阵

优选地，其步骤(5)包括如下子步骤：

S31、机械臂由8个UCR并联关节交错60°串联而成，而UCR并联关节内部三支链成120°对称分布，所以编号为奇数位和偶数位的UCR并联关节的驱动方程(机构驱动输出与位姿之间的函数关系)不同。

S32、机械臂沿B-spline路径曲线运动时，处于工作状态的UCR关节才进行位姿调整，未进入工作区间的UCR并联关节保持初始状态不变；在进行位姿调整时，首先，判断UCR并联机构编号的奇偶性，根据奇偶性，分别调用对应的驱动方程对UCR关节进行求解，得到对应三个支链的长度，并输出。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

(1)超冗余机械臂结构紧凑，刚度大，承载能力强，可靠性高；

(2)方法能严格保证机械臂末端处于三次B-spline路径曲线上，精度高，且三次B-spline轨迹G2连续，保证了运动的平顺；

(3)方法通过UCR并联关节驱动方程求解，求解精度高，效率快。

附图说明

图1是机械臂和基座示意图；

图2是UCR并联关节构型图；

图3是支链构型图；

图4是机械臂末端轨迹运动规划方法流程图；

图5是超冗余机械臂的典型工作情况；

图6是基于四元数旋转的坐标变换图；

图7是机械臂各UCR并联关节位姿图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图1-7及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

实施例1

本实施例中运动机构有25个自由度，且由超冗余机械臂1与进给基座2两部分组成，结构图如图1所示。

如图1所示，机械臂1由8个UCR并联关节串联而成，共有24个自由度。相邻2个UCR并联关节共用一个平台，即每个UCR并联关节的动平台也是下一个UCR并联关节的定平台。每个UCR并联关节定平台4和动平台5之间存在沿关节轴线60°的相位差。进给基座2提供一个进给自由度，极大提高了机械臂1的工作空间，并从基座1开始对机械臂UCR并联关节进行编号，分别记为1～8。

如图2所示，UCR(Universal-Cylindrical-Revolute)并联关节为具有绕X，Y轴转动和绕Z轴移动(两转一移)的三自由度并联机构。该机构由定平台4、三个支链和动平台5组成。支链的万向副与定平台相连，转动副与动平台相连。

如图3所示，支链从下至上由万向副(Universal)、圆柱副(Cylindrical)和转动副(Revolute)组成。UCR并联关节各支链的初始状态保持一致，即圆柱副的伸缩量一致，所以初始状态动平台与定平台姿态相同。通过调整三个支链的长度，可以对UCR并联关节中动平台相对于定平台的位姿进行调节。

本实施例的圆柱副为驱动副，本实施例采用电机驱动。

在第一个UCR并联关节定平台和动平台分别建立坐标系O₀-X₀Y₀Z₀和O₁-X₁Y₁Z₁。O₀和O₁分别为定平台和动平台圆心。三个支链与定平台和动平台的连接点分别为P_{1_i}，Q_{1_i}(i＝1，2，3)，其坐标均为O₀-X₀Y₀Z₀下的值。假设坐标系O₁-X₁Y₁Z₁相对于坐标系O₀-X₀Y₀Z₀的齐次变换矩阵为：

旋转矩阵为R₁，平移矩阵为c₁，P_{1_1}，P_{1_2}，P_{1_3}在坐标系O₀-X₀Y₀Z₀下的坐标为：

Q_{1_1}，Q_{1_2}，Q_{1_3}在坐标系O₁-X₁Y₁Z₁下的坐标为：

其中，r表示定平台和动平台半径。

则处于奇数位的UCR并联关节的驱动方程(即动平台相对于定平台的位姿与支链杆长之间的函数关系)为：

l_{1_i} ²＝||Q_{1_i}-P_{1_i}||²＝[R₁Q′_{1_i}+c₁-P_{1_i}]^T[R₁Q′_{1_i}+c₁-P_{1_i}](i＝1，2，3)

其中，l_1i表示奇数位第i支链的长度。

在第二个UCR并联关节定平台和动平台分别建立坐标系O₁-X₁Y₁Z₁和O₂-X₂Y₂Z₂。O₁和O₂分别为定平台和动平台圆心。三个支链与定平台和动平台的连接点分别为P_{2_i}，Q_{2_i}(i＝1，2，3)，其坐标均为O₁-X₁Y₁Z₁下的值。假设坐标系O₂-X₂Y₂Z₂相对于坐标系O₁-X₁Y₁Z₁的齐次变换矩阵为：

旋转矩阵为R₂，平移矩阵为c₂，P_{2_1}，P_{2_2}，P_{2_3}在坐标系O₁-X₁Y₁Z₁下的坐标为：

Q_{2_1}，Q_{2_2}，Q_{2_3}在坐标系O₂-X₂Y₂Z₂下的坐标为：

其中，r表示定平台和动平台半径。

则处于偶数位的UCR并联关节的驱动方程(即动平台相对于定平台的位姿与支链杆长之间的函数关系)为：

l_{2_i} ²＝||Q_{2_i}-P_{2_i}||²＝[R₂Q′_{2_i}+c₂-P_{2_i}]^T[R₂Q′_{2_i}+c₂-P_{2_i}](i＝1，2，3)

其中，l_2i表示偶数位第i支链的长度。

假设每个UCR并联关节动平台相对于定平台的齐次变换矩阵为：

则齐次变换矩阵表示为：

其中，α，β∈(-90°，90°)，α为绕X轴的旋转角，β为绕Y轴的旋转角，X_c，Y_C，Z_c分别为定平台X，Y，Z轴方向的平移向量。则第i个定平台相对于机器人基座的位姿态可以通过如下变换矩阵获得：

连续路径下跟随末段B样条轨迹运动实现包括五部分：(1)根据工作环境，获得工作空间中需要通过的标记点；(2)利用三次B-spline曲线，对标记点进行拟合，生成路径曲线；(3)根据基座进给量，获得机械臂各UCR并联关节的工作状态；(4)实时调整工作状态UCR并联关节的位姿，使得机械臂沿着三次B-spline路径曲线运动；(5)根据各UCR并联关节的位姿和驱动方程，求解出各关节支链的杆长，并输出。方法流程图如图4所示。

当获取环境参数后，通过智能算法或人工标记法，获得工作空间中需要通过的标记点

P₁…P_n。超冗余机械臂的工作情况和标记点如图5所示。

B-spline曲线以其通用性和易实现的特性被广泛应用在轨迹拟合中。B-spline曲线由控制点和节点矢量即可定义，以其表示路径轨迹可以减少数据存储量。此外B-spline曲线本身具备较高的连续性，三次B-spline曲线就G2连续，拟合后的轨迹有更好的光顺性，且经过原有数据点，严格保证轨迹精度。

设k次B-spline曲线方程为：

其中，P_i(i＝0，1，...，n)为控制点，N_i，k(t)(i＝0，1，...，n)称为k次B-spline曲线的基函数，本文中k＝3。节点矢量

图5中的标记点

P₁…P_n通过三次B-spline曲线。根据标记点生成三次B-spline路径曲线具体细节参见非专利文献《三次B样条反算的一种简便算法》。

假设机械臂末端动平台(编号为8的UCR并联关节)中心为三次B-spline路径曲线的起点。每个UCR并联关节的初始长度相等，且为d₀。机械臂在进行末端跟随运动时，基座提供进给量F，机械臂各UCR并联关节依次进入工作区域(处于工作状态)，并记录其对应的编号。

计算各工作状态UCR并联关节在B-spline路径曲线上的离散点；首先在工作状态UCR并联关节最大编号的定平台处建立基坐标系为：

坐标原点：

则第一个坐标系O₀-x₀y₀z₀已知，首先通过在[0，1]区间的二分法，在B-spline路径曲线上找到离散点，对应参数为t₁，使得通过位姿矩阵

反算出的UCR杆长处于行程范围，并记录参数t₁处对应的坐标系O₁-x₁y₁z₁。然后通过在[t₁，1]区间的二分法，在B-spline路径曲线上找到第二个离散点，对应参数t₂。依次类推，可以找到每个工作状态UCR并联关节动平台在B-spline路径曲线上对应的离散数据点。

B-spline路径曲线中离散数据点的Z轴沿该点处的切向方向。假设相邻两个离散点对应的参数分t_n-1和t_n，则对应的离散点分别为：c(t_n-1)和c(t_n)。则Z_n-1和

Z_n分别为切向量c′(t_n-1)和c′(t_n)的单位向量；

三维空间中物体的姿态与旋转变换可以用四元数来描述，如图6所示。三维空间中，绕单位向量n旋转2θ可以用四元数q＝[cosθ，nsinθ]＝q₀+q₁i+q₂j+q₃k来描述。

其中，

2θ＝arccos(z_n-1·z_n)，q₀＝cosθ；q₁，q₂，q₃为向量

nsinθ在其考坐标轴i，j，k上的分量。

则单位四元数q＝(q₀，q₁，q₂，q₃)^T对应的旋转矩阵

为：

假设O_n-1-x_n-1y_n-1z_n-1坐标系已知，则O_n在O_n-1-x_n-1y_n-1z_n-1中投影的坐标：

则相邻两个离散点之间的位姿矩阵

机械臂由8个UCR并联关节交错60°串联而成，而UCR并联关节内部三支链成120°对称分布，所以编号为奇数位和偶数位的UCR并联关节的驱动方程不同。当机械臂所有UCR并联关节均处于工作状态时，每个平台中心位置和坐标系如图7所示。

机械臂沿B-spline路径曲线运动时，处于工作状态的UCR关节才进行位姿调整，未进入工作区间的UCR并联关节保持初始状态不变；在进行位姿调整时，首先，判断UCR并联关节编号的奇偶性，根据奇偶性，分别调用对应的驱动方程对UCR关节进行求解，得到对应三个支链的长度，并输出。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种超冗余机械臂末端轨迹运动规划方法，其特征在于，包括以下步骤：

(1)根据工作环境，获得工作空间中超冗余机械臂需要通过的标记点；

(2)利用三次B-spline曲线，对标记点进行拟合，生成路径曲线；

(3)根据基座进给量，获得机械臂各UCR并联关节的工作状态；

(4)实时调整工作状态UCR并联关节的位姿，使得机械臂沿着三次B-spline路径曲线运动；

(5)根据各UCR并联关节的位姿和驱动方程，求解出各关节支链的杆长，并输出；

其中，步骤(3)包括如下子步骤：

S11、运动机构由超冗余机械臂与进给基座两部分组成；其中，机械臂由8个UCR并联关节串联而成，相邻2个UCR并联关节共用一个平台，即每个UCR并联关节的动平台也是下一个UCR并联关节的定平台；每个UCR并联关节定平台和动平台之间存在沿关节轴线60°的相位差，并从基座开始对机械臂UCR并联关节进行顺序编号，分别记为1～8；

S12、假设机械臂末端动平台中心为三次B-spline路径曲线的起点，每个UCR并联关节的初始长度相等，且为d₀；机械臂在进行末端跟随运动时，基座提供进给量F，机械臂各UCR并联关节依次进入工作区域，并记录其对应的编号。

2.根据权利要求1所述的超冗余机械臂末端轨迹运动规划方法，其特征在于，UCR并联关节为具有绕X，Y轴转动和绕Z轴移动的三自由度并联机构，该机构由定平台、三个支链和动平台组成，支链从下至上由万向副(Universal)、圆柱副(Cylindrical)和转动副(Revolute)组成，支链的万向副与定平台相连，转动副与动平台相连。

3.根据权利要求1所述的超冗余机械臂末端轨迹运动规划方法，其特征在于，UCR并联关节各支链的初始状态保持一致，即圆柱副的伸缩量一致，动平台与定平台姿态相同；通过调整三个支链的长度，可以对UCR并联关节中动平台相对于定平台的位姿进行调节。

4.根据权利要求1所述的超冗余机械臂末端轨迹运动规划方法，其特征在于，步骤(4)包括如下子步骤：

S21、计算各工作状态UCR并联关节在B-spline路径曲线上的离散点；首先在工作状态UCR并联关节最大编号的定平台处建立基坐标系为：

坐标原点：O₀＝[0 0 0]；则第一个坐标系O₀-x₀y₀z₀已知，然后通过在[0,1]区间的二分法，在B-spline路径曲线上找到离散点，对应参数为t₁，使得通过位姿矩阵

反算出的UCR杆长处于行程范围，并记录参数t₁处对应的坐标系O₁-x₁y₁z₁；然后通过在[t₁,1]区间的二分法，在B-spline路径曲线上找到第二个离散点，对应参数t₂；依次类推，可以找到每个工作状态UCR并联关节动平台在B-spline路径曲线上对应的离散数据点；

S22、B-spline路径曲线中离散数据点的Z轴沿该点处的切向方向，假设相邻两个离散点对应的参数分t_n-1和t_n，则对应的离散点分别为：c(t_n-1)和c(t_n)，则Z_n-1和Z_n分别为切向量c′(t_n-1)和c′(t_n)的单位向量；

S23、三维空间中物体的姿态与旋转变换可以用四元数来描述；三维空间中，绕单位向量n旋转2θ可以用四元数q＝[cosθ,nsinθ]＝q₀+q₁i+q₂j+q₃k来描述；

其中，

2θ＝arccos(z_n-1·z_n)，q₀＝cosθ；q₁，q₂，q₃为向量nsinθ在其考坐标轴i，j，k上的分量；

则单位四元数q＝(q₀,q₁,q₂,q₃)^T对应的旋转矩阵

为：

假设O_n-1-x_n-1y_n-1z_n-1坐标系已知，则O_n在O_n-1-x_n-1y_n-1z_n-1中投影坐标为：

则相邻两个离散点之间的位姿矩阵

5.根据权利要求1所述的超冗余机械臂末端轨迹运动规划方法，其特征在于，步骤(5)包括如下子步骤：

S31、机械臂由8个UCR并联关节交错60°串联而成，而UCR并联关节内部三支链成120°对称分布，UCR并联关节编号为奇数位和偶数位的关节的驱动方程不同；

S32、机械臂沿B-spline路径曲线运动时，处于工作状态的UCR关节才进行位姿调整，未进入工作区间的UCR并联关节保持初始状态不变；在进行位姿调整时，首先，判断UCR并联关节编号的奇偶性；其次，根据奇偶性，分别调用对应的驱动方程对UCR并联关节进行求解，得到UCR并联关节对应三个支链的长度，并输出。

Images