机械臂线性运动轨迹平滑方法及系统和存储介质

1.本发明涉及机械臂运动轨迹规划领域，具体地，涉及一种机械臂线性运动轨迹平滑方法及系统和存储介质,更为具体地，涉及一种基于几何代数的机械臂线性运动轨迹平滑方法及系统和存储介质。


背景技术：

2.目前线性路径在机械臂控制中被广泛应用，由于速度方向的改变，在线性路径衔接点处一般需要机械臂运行速度降为零，极大影响了机械臂的运行效率。为实现机械臂连续运动，运动轨迹平滑处理被广泛应用。现有线性轨迹平滑方法分为全局光顺和局部光顺两大类。
3.全局光顺方法通过对线性路径中的离散点进行全局逼近或插值从而实现平滑处理，但难以对生成路径与线性路径之间的误差进行控制；局部光顺方法通过在衔接点处插入参数曲线实现平滑处理，但现有局部方法多通过在线性路径转接角内部插入曲线，无法通过给定线性路径离散点，同时机械臂运行中通过转接曲线时速度较低。
4.专利文献wo2017219640a1(申请号：pct/cn2016/113194)公开了一种机械臂的轨迹规划方法及规划装置，该规划方法包括：基于待工作的机械臂的关节参数，生成机械臂的工作区域；根据预先输入的起点的坐标及移动方向生成一条射线，并计算射线与工作区域的边界的交点，生成交点集合；计算交点集合内的每个交点与起点的距离，获取与最小距离对应的交点的坐标，并将交点标记为终点；基于起点的坐标以及终点的坐标，对机械臂的运动轨迹进行规划。该机械臂轨迹规划方法基于几何的方法来确定机械臂工作区域的边界，从而使得机器人在运动前就可以提前知道终点的位置，方便机器人进行运动轨迹的规划。但该发明不能通过改变直线段与过渡圆环之间插值圆环的构造方法，实现任意高阶几何连续。


技术实现要素：

5.针对现有技术中的缺陷，本发明的目的是提供一种机械臂线性运动轨迹平滑方法及系统和存储介质。
6.根据本发明提供的一种机械臂线性运动轨迹平滑方法，包括：
7.步骤s1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点，将经过转接曲线首末点和线性路径衔接点的圆环定义为过渡圆环；
8.步骤s2：使用共形几何代数表示原始线性路径与过渡圆环；
9.步骤s3：通过原始线性路径与过渡圆环之间的插值圆环构造转接曲线。
10.优选地，在所述步骤s1中：
11.通过以下公式计算每个线性路径衔接点pi处相邻两段线性路径的长度li和l
i+1
：
12.[0013][0014]
其中，p
i-1
,pi,p
i+1
分别为p
i-1
,pi,p
i+1
在共形空间中的表示；
[0015]
·
表示内积运算；
[0016]
为保证相邻转接曲线不相交，转接长度di通过以下方程获得：
[0017][0018]
其中，ε为给定的最大光顺误差，li和l
i+1
分别为对应衔接点处相邻两段线性路径的长度；θi表示对应衔接点处相邻两段线性路径的夹角；
[0019]
计算得到转接曲线的首末点q
i-与q
i+
的坐标：
[0020]qi-＝pi+div
i-[0021]qi+
＝pi+div
i+
[0022]
其中，
[0023]
定义通过点q
i+
,pi,q
i+
的圆环为过渡圆环。
[0024]
优选地，在所述步骤s2中：
[0025]
对每个线性路径衔接点pi,i＝1,2,
…
,n，使用共形几何代数，分别将相邻两段线性路径以及过渡圆环表示为
[0026]
l
i-＝p
i-1
∧q
i-∧pi[0027]
l
i+
＝pi∧q
i+
∧p
i+1
[0028]ci
＝q
i-∧pi∧q
i+
[0029]
其中，p
i-1
,pi,p
i+1
,q
i-,q
i+
分别为p
i-1
,pi,p
i+1
,q
i-,q
i+
在共形空间中的表示；∧表示外积运算；
[0030]
将l
i-,l
i+
,ci单位化：
[0031][0032][0033][0034]
优选地，在所述步骤s3中：
[0035]
对每个线性路径衔接点pi,i＝1,2,
…
,n，计算ci在pi点处的切线与直线l
i-的夹角αi：
[0036][0037]
其中
·
表示内积运算；
[0038]
使用以下公式计算直线段l
i-,l
i+
与过渡圆环ci之间的插值圆环
[0039][0040][0041]
其中，和分别表示l
i-,l
i+
与过渡圆环ci之间的插值圆环，λ∈[0,1]为插值参数；和为单位化的相邻两段线性路径；为单位化的过渡圆环；
[0042]
使用共形几何代数，将线段q
i-1
pi,piq
i+1
参数化：
[0043]yi-＝(1-λ)q
i-+λpi+λ(1-λ)q
i-·
p
ie∞
[0044]yi+
＝(1-λ)pi+λq
i+
+λ(1-λ)pi·qi+e∞
[0045]
其中，y
i-与y
i+
分别为两线段q
i-1
pi与piq
i+1
的参数化表示，e
∞
为五维共形空间中表示无穷远处的基向量；p
i-1
,pi,p
i+1
,q
i-,q
i+
分别为p
i-1
,pi,p
i+1
,q
i-,q
i+
在共形空间中的表示；
[0046]
计算直线l
i-与圆环之间的变换算子以及直线l
i+
与圆环之间的变换算子：
[0047][0048][0049]
其中*表示几何积运算；
[0050]
从而计算得到q
i-与pi之间的转接曲线x
i-以及pi与q
i+
之间的转接曲线x
i+
：
[0051][0052][0053]ri-(λ)为直线l
i-与圆环之间的变换算子，为r
i-(λ)的逆序，r
i+
(λ)为直线l
i+
与圆环之间的变换算子，为r
i+
(λ)的逆序。
[0054]
根据本发明提供的一种机械臂线性运动轨迹平滑系统，包括：
[0055]
模块m1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点，将经过转接曲线首末点和线性路径衔接点的圆环定义为过渡圆环；
[0056]
模块m2：使用共形几何代数表示原始线性路径与过渡圆环；
[0057]
模块m3：通过原始线性路径与过渡圆环之间的插值圆环构造转接曲线。
[0058]
优选地，在所述模块m1中：
[0059]
通过以下公式计算每个线性路径衔接点pi处相邻两段线性路径的长度li和l
i+1
：
[0060][0061][0062]
其中，p
i-1
,pi,p
i+1
分别为p
i-1
,pi,p
i+1
在共形空间中的表示；
[0063]
·
表示内积运算；
[0064]
为保证相邻转接曲线不相交，转接长度di通过以下方程获得：
[0065][0066]
其中，ε为给定的最大光顺误差，li和l
i+1
分别为对应衔接点处相邻两段线性路径的长度；θi表示对应衔接点处相邻两段线性路径的夹角；
[0067]
计算得到转接曲线的首末点q
i-与q
i+
的坐标：
[0068]qi-＝pi+div
i-[0069]qi+
＝pi+div
i+
[0070]
其中，
[0071]
定义通过点q
i+
,pi,q
i+
的圆环为过渡圆环。
[0072]
优选地，在所述模块m2中：
[0073]
对每个线性路径衔接点pi,i＝1,2,
…
,n，使用共形几何代数，分别将相邻两段线性路径以及过渡圆环表示为
[0074]
l
i-＝p
i-1
∧q
i-∧pi[0075]
l
i+
＝pi∧q
i+
^p
i+1
[0076]ci
＝q
i-^pi∧q
i+
[0077]
其中，p
i-1
,pi,p
i+1
,q
i-,q
i+
分别为p
i-1
,pi,p
i+1
,q
i-,q
i+
在共形空间中的表示；∧表示外积运算；
[0078]
将l
i-,l
i+
,ci单位化：
[0079][0080][0081][0082]
优选地，在所述模块m3中：
[0083]
对每个线性路径衔接点pi,i＝1,2,
…
,n，计算ci在pi点处的切线与直线l
i-的夹角αi：
[0084][0085]
其中
·
表示内积运算；
[0086]
使用以下公式计算直线段l
i-,l
i+
与过渡圆环ci之间的插值圆环
[0087][0088]
[0089]
其中，和分别表示l
i-,l
i+
与过渡圆环ci之间的插值圆环，λ∈[0,1]为插值参数；和为单位化的相邻两段线性路径；为单位化的过渡圆环；
[0090]
使用共形几何代数，将线段q
i-1
pi,piq
i+1
参数化：
[0091]yi-＝(1-λ)q
i-+λpi+λ(1-λ)q
i-·
p
ie∞
[0092]yi+
＝(1-λ)pi+λq
i+
+λ(1-λ)pi·qi+e∞
[0093]
其中，y
i-与y
i+
分别为两线段q
i-1
pi与piq
i+1
的参数化表示，e
∞
为五维共形空间中表示无穷远处的基向量；p
i-1
,pi,p
i+1
,q
i-,q
i+
分别为p
i-1
,pi,p
i+1
,q
i-,q
i+
在共形空间中的表示；
[0094]
计算直线l
i-与圆环之间的变换算子以及直线l
i+
与圆环之间的变换算子：
[0095][0096][0097]
其中*表示几何积运算；
[0098]
从而计算得到q
i-与pi之间的转接曲线x
i-以及pi与q
i+
之间的转接曲线x
i+
：
[0099][0100][0101]ri-(λ)为直线l
i-与圆环之间的变换算子，为r
i-(λ)的逆序，r
i+
(λ)为直线l
i+
与圆环之间的变换算子，为r
i+
(λ)的逆序。
[0102]
根据本发明提供的一种存储有计算机程序的计算机可读存储介质，所述计算机程序被处理器执行时实现任一项所述的方法的步骤，调用任一项所述的系统的模块。
[0103]
与现有技术相比，本发明具有如下的有益效果：
[0104]
1、本发明所述机械臂线性运动轨迹平滑方法，可以实现几何上的g3连续，同时通过改变直线段与过渡圆环之间插值圆环的构造方法，可以实现任意高阶几何连续；
[0105]
2、本发明可以在保证光顺误差的基础上通过线性路径离散点；
[0106]
3、本发明在机械臂关节角跃度约束下进行速度规划后，相比于在衔接角内部插入参数曲线，可以实现更高的运行效率。
附图说明
[0107]
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0108]
图1为初始小线段路径；
[0109]
图2为通过平滑处理后的光顺曲线；
[0110]
图3为本发明方法生成曲线的速度规划结果；
[0111]
图4为内部样条曲线方法生成曲线的速度规划结果；
[0112]
图5为本发明步骤示意图。
具体实施方式
[0113]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
[0114]
实施例1：
[0115]
本领域技术人员可以将本发明提供的一种机械臂线性运动轨迹平滑方法，理解为机械臂线性运动轨迹平滑系统的具体实施方式，即所述机械臂线性运动轨迹平滑系统可以通过执行所述机械臂线性运动轨迹平滑方法的步骤流程予以实现。
[0116]
根据本发明提供的一种机械臂线性运动轨迹平滑方法，如图1-图5所示，包括：
[0117]
步骤s1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点；
[0118]
步骤s2：使用共形几何代数表示原始线性路径与过渡圆环；
[0119]
步骤s3：通过原始线性路径与过渡圆环之间的插值圆环构造转接曲线。
[0120]
具体地，在所述步骤s1中：
[0121]
通过以下公式计算每个线性路径衔接点pi处相邻两段线性路径的长度li和l
i+1
：
[0122][0123][0124]
其中，p
i-1
,pi,p
i+1
分别为p
i-1
,pi,p
i+1
在共形空间中的表示；
[0125]
·
表示内积运算；
[0126]
为保证相邻转接曲线不相交，转接长度di通过以下方程获得：
[0127][0128]
其中，ε为给定的最大光顺误差，li和l
i+1
分别为对应衔接点处相邻两段线性路径的长度；θi表示对应衔接点处相邻两段线性路径的夹角；
[0129]
计算得到转接曲线的首末点q
i-与q
i+
的坐标：
[0130]qi-＝pi+div
i-[0131]qi+
＝pi+div
i+
[0132]
其中，
[0133]
定义通过点q
i+
,pi,q
i+
的圆环为过渡圆环
[0134]
具体地，在所述步骤s2中：
[0135]
对每个线性路径衔接点pi,i＝1,2,
…
,n，使用共形几何代数，分别将相邻两段线性路径以及过渡圆环表示为
[0136]
l
i-＝p
i-1
∧q
i-^pi[0137]
l
i+
＝pi∧q
i+
∧p
i+1
[0138]ci
＝q
i-∧pi∧q
i+
[0139]
其中，p
i-1
,pi,p
i+1
,q
i-,q
i+
分别为p
i-1
,pi,p
i+1
,q
i-,q
i+
在共形空间中的表示；∧表示外积运算；
[0140]
将l
i-,l
i+
,ci单位化：
[0141][0142][0143][0144]
具体地，在所述步骤s3中：
[0145]
对每个线性路径衔接点pi,i＝1,2,
…
,n，计算ci在pi点处的切线与直线l
i-的夹角αi：
[0146][0147]
其中
·
表示内积运算；
[0148]
使用以下公式计算直线段l
i-,l
i+
与过渡圆环ci之间的插值圆环
[0149][0150][0151]
其中，和分别表示l
i-,l
i+
与过渡圆环ci之间的插值圆环，λ∈[0,1]为插值参数；和为单位化的相邻两段线性路径；为单位化的过渡圆环；
[0152]
使用共形几何代数，将线段q
i-1
pi,piq
i+1
参数化：
[0153]yi-＝(1-λ)q
i-+λpi+λ(1-λ)q
i-·
p
ie∞
[0154]yi+
＝(1-λ)pi+λq
i+
+λ(1-λ)pi·qi+e∞
[0155]
其中，y
i-与y
i+
分别为两线段q
i-1
pi与piq
i+1
的参数化表示，e
∞
为五维共形空间中表示无穷远处的基向量；p
i-1
,pi,p
i+1
,q
i-,q
i+
分别为p
i-1
,pi,p
i+1
,q
i-,q
i+
在共形空间中的表示；
[0156]
计算直线l
i-与圆环之间的变换算子以及直线l
i+
与圆环之间的变换算子：
[0157][0158][0159]
其中*表示几何积运算；
[0160]
从而计算得到q
i-与pi之间的转接曲线x
i-以及pi与q
i+
之间的转接曲线x
i+
：
[0161][0162]
[0163]ri-(λ)为直线l
i-与圆环之间的变换算子，为r
i-(λ)的逆序，r
i+
(λ)为直线l
i+
与圆环之间的变换算子，为r
i+
(λ)的逆序。
[0164]
根据本发明提供的一种机械臂线性运动轨迹平滑系统，包括：
[0165]
模块m1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点，将经过转接曲线首末点和线性路径衔接点的圆环定义为过渡圆环；
[0166]
模块m2：使用共形几何代数表示原始线性路径与过渡圆环；
[0167]
模块m3：通过原始线性路径与过渡圆环之间的插值圆环构造转接曲线。
[0168]
通过以下公式计算每个线性路径衔接点pi处相邻两段线性路径的长度li和l
i+1
：
[0169][0170][0171]
其中，p
i-1
,pi,p
i+1
分别为p
i-1
,pi,p
i+1
在共形空间中的表示；
[0172]
·
表示内积运算；
[0173]
为保证相邻转接曲线不相交，转接长度di通过以下方程获得：
[0174][0175]
其中，ε为给定的最大光顺误差，li和l
i+1
分别为对应衔接点处相邻两段线性路径的长度；θi表示对应衔接点处相邻两段线性路径的夹角；
[0176]
计算得到转接曲线的首末点q
i-与q
i+
的坐标：
[0177]qi-＝pi+div
i-[0178]qi+
＝pi+div
i+
[0179]
其中，
[0180]
定义通过点q
i+
,pi,q
i+
的圆环为过渡圆环。
[0181]
具体地，在所述模块m2中：
[0182]
对每个线性路径衔接点pi,i＝1,2,
…
,n，使用共形几何代数，分别将相邻两段线性路径以及过渡圆环表示为
[0183]
l
i-＝p
i-1
∧q
i-∧pi[0184]
l
i+
＝pi∧q
i+
∧p
i+1
[0185]ci
＝q
i-∧pi∧q
i+
[0186]
其中，p
i-1
,pi,p
i+1
,q
i-,q
i+
分别为p
i-1
,pi,p
i+1
,q
i-,q
i+
在共形空间中的表示；∧表示外积运算；
[0187]
将l
i-,l
i+
,ci单位化：
[0188]
[0189][0190][0191]
具体地，在所述模块m3中：
[0192]
对每个线性路径衔接点pi,i＝1,2,
…
,n，计算ci在pi点处的切线与直线l
i-的夹角αi：
[0193][0194]
其中
·
表示内积运算；
[0195]
使用以下公式计算直线段l
i-,l
i+
与过渡圆环ci之间的插值圆环
[0196][0197][0198]
其中，和分别表示l
i-,l
i+
与过渡圆环ci之间的插值圆环，λ∈[0,1]为插值参数；和为单位化的相邻两段线性路径；为单位化的过渡圆环；
[0199]
使用共形几何代数，将线段q
i-1
pi,piq
i+1
参数化：
[0200]yi-＝(1-λ)q
i-+λpi+λ(1-λ)q
i-·
p
ie∞
[0201]yi+
＝(1-λ)pi+λq
i+
+λ(1-λ)pi·qi+e∞
[0202]
其中，y
i-与y
i+
分别为两线段q
i-1
pi与piq
i+1
的参数化表示，e
∞
为五维共形空间中表示无穷远处的基向量；p
i-1
,pi,p
i+1
,q
i-,q
i+
分别为p
i-1
,pi,p
i+1
,q
i-,q
i+
在共形空间中的表示；
[0203]
计算直线l
i-与圆环之间的变换算子以及直线l
i+
与圆环之间的变换算子：
[0204][0205][0206]
其中*表示几何积运算；
[0207]
从而计算得到q
i-与pi之间的转接曲线x
i-以及pi与q
i+
之间的转接曲线x
i+
：
[0208][0209][0210]ri-(λ)为直线l
i-与圆环之间的变换算子，为r
i-(λ)的逆序，r
i+
(λ)为直线l
i+
与圆环之间的变换算子，为r
i+
(λ)的逆序。
[0211]
根据本发明提供的一种存储有计算机程序的计算机可读存储介质，所述计算机程序被处理器执行时实现任一项所述的方法的步骤，调用任一项所述的系统的模块。
[0212]
实施例2：
[0213]
实施例2为实施例1的优选例，以更为具体地对本发明进行说明。
[0214]
本发明涉及一种机械臂线性运动轨迹平滑方法，可在线性路径基础上获取机械臂光滑的运动轨迹。
[0215]
针对现有技术的不足，本发明提供了一种机械臂线性运动轨迹平滑方法，利用共形几何代数，可在线性路径基础上解析计算机械臂光滑的运动轨迹。
[0216]
根据本发明提供的机械臂线性运动轨迹平滑方法，包括：
[0217]
步骤1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点；
[0218]
步骤2：使用共形几何代数表示原始直线段与过渡圆环；
[0219]
步骤3：通过直线与过渡圆环之间的插值圆环构造转接曲线。
[0220]
初始小线段路径为蝴蝶形状路径曲线，如图1所示，该曲线由95条线性小线段路径组成。
[0221]
使用e1,e2,e3表示三维欧氏空间中三个单位正交基向量，e0和e
∞
表示五维共性空间中另外两个基向量。
[0222]
对于i＝1,2,
…
,94，将每个线性路径衔接点表示为pi＝p
ix
e1+p
iy
e2+p
iz
e3，通过以下方程计算衔接点处相邻两段线性路径的夹角：
[0223][0224]
其中，p
ix
,p
iy
,p
iz
分别表示pi点在欧式空间中的x轴、y轴和z轴的坐标；
[0225]
·
表示内积运算。
[0226]
通过以下公式在共形空间中表示pi：
[0227][0228]
通过以下公式计算衔接点处相邻两段线性路径的长度li和l
i+1
：
[0229][0230][0231]
为保证相邻转接曲线不相交，转接长度di通过以下方程获得：
[0232][0233]
其中，ε为给定的最大光顺误差，取为ε＝0.0005m。
[0234]
从而计算得到转接曲线的首末点q
i-与q
i+
的坐标：
[0235]qi-＝pi+div
i-[0236]qi+
＝pi+div
i+
[0237]
其中
[0238]
[0239][0240]
通过以下公式在共形空间中表示q
i-和q
i+
：
[0241][0242][0243]
对每个线性路径衔接点pi,i＝1,2,
…
,94，使用共形几何代数，分别将相邻两段线性路径以及过渡圆环表示为
[0244]
l
i-＝p
i-1
∧q
i-^pi[0245]
l
i+
＝pi∧q
i+
^p
i+1
[0246]ci
＝q
i-^pi∧q
i+
[0247]
其中^表示外积运算。
[0248]
将l
i-,l
i+
,ci单位化：
[0249][0250]
对每个线性路径衔接点pi,i＝1,2,
…
,94，计算ci在pi点处的切线与直线l
i-的夹角αi：
[0251][0252]
使用以下公式计算直线段l
i-,l
i+
与过渡圆环ci之间的插值圆环
[0253][0254][0255]
其中，λ∈[0,1]为插值参数。
[0256]
使用共形几何代数，将线段q
i-1
pi,piq
i+1
参数化：
[0257]yi-＝(1-λ)q
i-+λpi+λ(1-λ)q
i-·
p
ie∞
[0258]yi+
＝(1-λ)pi+λq
i+
+λ(1-λ)pi·qi+e∞
[0259]
计算直线l
i-与圆环之间的变换算子以及直线l
i+
与圆环之间的变换算子：
[0260][0261][0262]
其中*表示几何积运算。
[0263]
从而计算得到q
i-与pi之间的转接曲线x
i-以及pi与q
i+
之间的转接曲线x
i+
：
[0264]
[0265][0266]
经过平滑处理后的曲线如图2所示，图中短划线表示所生成的转接曲线。
[0267]
所生成平滑路径在ur10机械臂上执行。设置机械臂末端最大进给速度为0.5m/s，最大切向加速度为5m/s2，最大切向跃度为30m/s3，设置机械臂每个关节角最大关节速度为1rad/s，最大关节加速度为5rad/s2，最大关节跃度为50rad/s3，最大弓高误差为0.00005m，插补步长为8ms。速度规划结果如图3所示。
[0268]
一种典型的基于b样条在转接角内部生成转接曲线的方法生成的转接曲线如图2中点虚线所示，在上述同样约束条件下进行速度规划，结果如图4所示。通过与图3对比可知，本发明所述方法可大幅缩短机械臂运行时间。
[0269]
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
[0270]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。