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

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

Info

Publication number
CN114536337B
CN114536337B CN202210177372.6A CN202210177372A CN114536337B CN 114536337 B CN114536337 B CN 114536337B CN 202210177372 A CN202210177372 A CN 202210177372A CN 114536337 B CN114536337 B CN 114536337B
Authority
CN
China
Prior art keywords
ring
linear path
representing
linear
conformal
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210177372.6A
Other languages
English (en)
Other versions
CN114536337A (zh
Inventor
丁烨
陈永学
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN202210177372.6A priority Critical patent/CN114536337B/zh
Publication of CN114536337A publication Critical patent/CN114536337A/zh
Application granted granted Critical
Publication of CN114536337B publication Critical patent/CN114536337B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Numerical Control (AREA)
  • Complex Calculations (AREA)

Abstract

本发明提供了一种机械臂线性运动轨迹平滑方法及系统，包括：步骤S1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点，经过转接曲线首末点和线性路径衔接点的圆环定义为过渡圆环；步骤S2：使用共形几何代数表示原始线性路径与过渡圆环；步骤S3：通过原始线性路径与过渡圆环之间的插值圆环构造转接曲线。本方法适用于在线性路径基础上获取机械臂光滑的运动轨迹，以提高机械臂运动效率与运动性能，在机械臂加工等工作中有着重要的应用。

Description

机械臂线性运动轨迹平滑方法及系统和存储介质
技术领域
本发明涉及机械臂运动轨迹规划领域，具体地，涉及一种机械臂线性运动轨迹平滑方法及系统和存储介质,更为具体地，涉及一种基于几何代数的机械臂线性运动轨迹平滑方法及系统和存储介质。
背景技术
目前线性路径在机械臂控制中被广泛应用，由于速度方向的改变，在线性路径衔接点处一般需要机械臂运行速度降为零，极大影响了机械臂的运行效率。为实现机械臂连续运动，运动轨迹平滑处理被广泛应用。现有线性轨迹平滑方法分为全局光顺和局部光顺两大类。
全局光顺方法通过对线性路径中的离散点进行全局逼近或插值从而实现平滑处理，但难以对生成路径与线性路径之间的误差进行控制；局部光顺方法通过在衔接点处插入参数曲线实现平滑处理，但现有局部方法多通过在线性路径转接角内部插入曲线，无法通过给定线性路径离散点，同时机械臂运行中通过转接曲线时速度较低。
专利文献WO2017219640A1(申请号：PCT/CN2016/113194)公开了一种机械臂的轨迹规划方法及规划装置，该规划方法包括：基于待工作的机械臂的关节参数，生成机械臂的工作区域；根据预先输入的起点的坐标及移动方向生成一条射线，并计算射线与工作区域的边界的交点，生成交点集合；计算交点集合内的每个交点与起点的距离，获取与最小距离对应的交点的坐标，并将交点标记为终点；基于起点的坐标以及终点的坐标，对机械臂的运动轨迹进行规划。该机械臂轨迹规划方法基于几何的方法来确定机械臂工作区域的边界，从而使得机器人在运动前就可以提前知道终点的位置，方便机器人进行运动轨迹的规划。但该发明不能通过改变直线段与过渡圆环之间插值圆环的构造方法，实现任意高阶几何连续。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种机械臂线性运动轨迹平滑方法及系统和存储介质。
根据本发明提供的一种机械臂线性运动轨迹平滑方法，包括：
步骤S1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点，将经过转接曲线首末点和线性路径衔接点的圆环定义为过渡圆环；
步骤S2：使用共形几何代数表示原始线性路径与过渡圆环；
步骤S3：通过原始线性路径与过渡圆环之间的插值圆环构造转接曲线。
优选地，在所述步骤S1中：
通过以下公式计算每个线性路径衔接点pi处相邻两段线性路径的长度li和li+1：
Figure BDA0003519447380000021
Figure BDA0003519447380000022
其中，Pi-1,Pi,Pi+1分别为pi-1,pi,pi+1在共形空间中的表示；
· 表示内积运算；
为保证相邻转接曲线不相交，转接长度di通过以下方程获得：
Figure BDA0003519447380000023
其中，ε为给定的最大光顺误差，li和li+1分别为对应衔接点处相邻两段线性路径的长度；θi表示对应衔接点处相邻两段线性路径的夹角；
计算得到转接曲线的首末点qi-与qi+的坐标：
qi-＝pi+divi-
qi+＝pi+divi+
其中，
Figure BDA0003519447380000024
定义通过点qi+,pi,qi+的圆环为过渡圆环。
优选地，在所述步骤S2中：
对每个线性路径衔接点pi,i＝1,2,…,n，使用共形几何代数，分别将相邻两段线性路径以及过渡圆环表示为
Li-＝Pi-1∧Qi-∧Pi
Li+＝Pi∧Qi+∧Pi+1
Ci＝Qi-∧Pi∧Qi+
其中，Pi-1,Pi,Pi+1,Qi-,Qi+分别为pi-1,pi,pi+1,qi-,qi+在共形空间中的表示；∧表示外积运算；
将Li-,Li+,Ci单位化：
Figure BDA0003519447380000031
Figure BDA0003519447380000032
Figure BDA0003519447380000033
优选地，在所述步骤S3中：
对每个线性路径衔接点pi,i＝1,2,…,n，计算Ci在Pi点处的切线与直线Li-的夹角αi：
Figure BDA0003519447380000034
其中·表示内积运算；
使用以下公式计算直线段Li-,Li+与过渡圆环Ci之间的插值圆环
Figure BDA0003519447380000035
Figure BDA0003519447380000036
其中，
Figure BDA0003519447380000037
和
Figure BDA0003519447380000038
分别表示Li-,Li+与过渡圆环Ci之间的插值圆环，λ∈[0,1]为插值参数；
Figure BDA0003519447380000039
和
Figure BDA00035194473800000310
为单位化的相邻两段线性路径；
Figure BDA00035194473800000311
为单位化的过渡圆环；
使用共形几何代数，将线段qi-1pi,piqi+1参数化：
Yi-＝(1-λ)Qi-+λPi+λ(1-λ)Qi-·Pie∞
Yi+＝(1-λ)Pi+λQi++λ(1-λ)Pi·Qi+e∞
其中，Yi-与Yi+分别为两线段qi-1pi与piqi+1的参数化表示，e∞为五维共形空间中表示无穷远处的基向量；Pi-1,Pi,Pi+1,Qi-,Qi+分别为pi-1,pi,pi+1,qi-,qi+在共形空间中的表示；
计算直线Li-与圆环
Figure BDA00035194473800000312
之间的变换算子以及直线Li+与圆环
Figure BDA00035194473800000313
之间的变换算子：
Figure BDA0003519447380000041
Figure BDA0003519447380000042
其中*表示几何积运算；
从而计算得到Qi-与Pi之间的转接曲线Xi-以及Pi与Qi+之间的转接曲线Xi+：
Figure BDA0003519447380000043
Figure BDA0003519447380000044
Ri-(λ)为直线Li-与圆环
Figure BDA0003519447380000045
之间的变换算子，
Figure BDA0003519447380000046
为Ri-(λ)的逆序，Ri+(λ)为直线Li+与圆环
Figure BDA0003519447380000047
之间的变换算子，
Figure BDA0003519447380000048
为Ri+(λ)的逆序。
根据本发明提供的一种机械臂线性运动轨迹平滑系统，包括：
模块M1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点，将经过转接曲线首末点和线性路径衔接点的圆环定义为过渡圆环；
模块M2：使用共形几何代数表示原始线性路径与过渡圆环；
模块M3：通过原始线性路径与过渡圆环之间的插值圆环构造转接曲线。
优选地，在所述模块M1中：
通过以下公式计算每个线性路径衔接点pi处相邻两段线性路径的长度li和li+1：
Figure BDA0003519447380000049
Figure BDA00035194473800000410
其中，Pi-1,Pi,Pi+1分别为pi-1,pi,pi+1在共形空间中的表示；
·表示内积运算；
为保证相邻转接曲线不相交，转接长度di通过以下方程获得：
Figure BDA00035194473800000411
其中，ε为给定的最大光顺误差，li和li+1分别为对应衔接点处相邻两段线性路径的长度；θi表示对应衔接点处相邻两段线性路径的夹角；
计算得到转接曲线的首末点qi-与qi+的坐标：
qi-＝pi+divi-
qi+＝pi+divi+
其中，
Figure BDA0003519447380000051
定义通过点qi+,pi,qi+的圆环为过渡圆环。
优选地，在所述模块M2中：
对每个线性路径衔接点pi,i＝1,2,…,n，使用共形几何代数，分别将相邻两段线性路径以及过渡圆环表示为
Li-＝Pi-1∧Qi-∧Pi
Li+＝Pi∧Qi+^Pi+1
Ci＝Qi-^Pi∧Qi+
其中，Pi-1,Pi,Pi+1,Qi-,Qi+分别为pi-1,pi,pi+1,qi-,qi+在共形空间中的表示；∧表示外积运算；
将Li-,Li+,Ci单位化：
Figure BDA0003519447380000052
Figure BDA0003519447380000053
Figure BDA0003519447380000054
优选地，在所述模块M3中：
对每个线性路径衔接点pi,i＝1,2,…,n，计算Ci在Pi点处的切线与直线Li-的夹角αi：
Figure BDA0003519447380000055
其中·表示内积运算；
使用以下公式计算直线段Li-,Li+与过渡圆环Ci之间的插值圆环
Figure BDA0003519447380000056
Figure BDA0003519447380000057
其中，
Figure BDA0003519447380000058
和
Figure BDA0003519447380000059
分别表示Li-,Li+与过渡圆环Ci之间的插值圆环，λ∈[0,1]为插值参数；
Figure BDA00035194473800000510
和
Figure BDA00035194473800000511
为单位化的相邻两段线性路径；
Figure BDA00035194473800000512
为单位化的过渡圆环；
使用共形几何代数，将线段qi-1pi,piqi+1参数化：
Yi-＝(1-λ)Qi-+λPi+λ(1-λ)Qi-·Pie∞
Yi+＝(1-λ)Pi+λQi++λ(1-λ)Pi·Qi+e∞
其中，Yi-与Yi+分别为两线段qi-1pi与piqi+1的参数化表示，e∞为五维共形空间中表示无穷远处的基向量；Pi-1,Pi,Pi+1,Qi-,Qi+分别为pi-1,pi,pi+1,qi-,qi+在共形空间中的表示；
计算直线Li-与圆环
Figure BDA0003519447380000061
之间的变换算子以及直线Li+与圆环
Figure BDA0003519447380000062
之间的变换算子：
Figure BDA0003519447380000063
Figure BDA0003519447380000064
其中*表示几何积运算；
从而计算得到Qi-与Pi之间的转接曲线Xi-以及Pi与Qi+之间的转接曲线Xi+：
Figure BDA0003519447380000065
Figure BDA0003519447380000066
Ri-(λ)为直线Li-与圆环
Figure BDA0003519447380000067
之间的变换算子，
Figure BDA0003519447380000068
为Ri-(λ)的逆序，Ri+(λ)为直线Li+与圆环
Figure BDA0003519447380000069
之间的变换算子，
Figure BDA00035194473800000610
为Ri+(λ)的逆序。
根据本发明提供的一种存储有计算机程序的计算机可读存储介质，所述计算机程序被处理器执行时实现任一项所述的方法的步骤，调用任一项所述的系统的模块。
与现有技术相比，本发明具有如下的有益效果：
1、本发明所述机械臂线性运动轨迹平滑方法，可以实现几何上的G3连续，同时通过改变直线段与过渡圆环之间插值圆环的构造方法，可以实现任意高阶几何连续；
2、本发明可以在保证光顺误差的基础上通过线性路径离散点；
3、本发明在机械臂关节角跃度约束下进行速度规划后，相比于在衔接角内部插入参数曲线，可以实现更高的运行效率。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为初始小线段路径；
图2为通过平滑处理后的光顺曲线；
图3为本发明方法生成曲线的速度规划结果；
图4为内部样条曲线方法生成曲线的速度规划结果；
图5为本发明步骤示意图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
实施例1：
本领域技术人员可以将本发明提供的一种机械臂线性运动轨迹平滑方法，理解为机械臂线性运动轨迹平滑系统的具体实施方式，即所述机械臂线性运动轨迹平滑系统可以通过执行所述机械臂线性运动轨迹平滑方法的步骤流程予以实现。
根据本发明提供的一种机械臂线性运动轨迹平滑方法，如图1-图5所示，包括：
步骤S1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点；
步骤S2：使用共形几何代数表示原始线性路径与过渡圆环；
步骤S3：通过原始线性路径与过渡圆环之间的插值圆环构造转接曲线。
具体地，在所述步骤S1中：
通过以下公式计算每个线性路径衔接点pi处相邻两段线性路径的长度li和li+1：
Figure BDA0003519447380000071
Figure BDA0003519447380000072
其中，Pi-1,Pi,Pi+1分别为pi-1,pi,pi+1在共形空间中的表示；
·表示内积运算；
为保证相邻转接曲线不相交，转接长度di通过以下方程获得：
Figure BDA0003519447380000081
其中，ε为给定的最大光顺误差，li和li+1分别为对应衔接点处相邻两段线性路径的长度；θi表示对应衔接点处相邻两段线性路径的夹角；
计算得到转接曲线的首末点qi-与qi+的坐标：
qi-＝pi+divi-
qi+＝pi+divi+
其中，
Figure BDA0003519447380000082
定义通过点qi+,pi,qi+的圆环为过渡圆环
具体地，在所述步骤S2中：
对每个线性路径衔接点pi,i＝1,2,…,n，使用共形几何代数，分别将相邻两段线性路径以及过渡圆环表示为
Li-＝Pi-1∧Qi-^Pi
Li+＝Pi∧Qi+∧Pi+1
Ci＝Qi-∧Pi∧Qi+
其中，Pi-1,Pi,Pi+1,Qi-,Qi+分别为pi-1,pi,pi+1,qi-,qi+在共形空间中的表示；∧表示外积运算；
将Li-,Li+,Ci单位化：
Figure BDA0003519447380000083
Figure BDA0003519447380000084
Figure BDA0003519447380000085
具体地，在所述步骤S3中：
对每个线性路径衔接点pi,i＝1,2,…,n，计算Ci在Pi点处的切线与直线Li-的夹角αi：
Figure BDA0003519447380000086
其中·表示内积运算；
使用以下公式计算直线段Li-,Li+与过渡圆环Ci之间的插值圆环
Figure BDA0003519447380000091
Figure BDA0003519447380000092
其中，
Figure BDA0003519447380000093
和
Figure BDA0003519447380000094
分别表示Li-,Li+与过渡圆环Ci之间的插值圆环，λ∈[0,1]为插值参数；
Figure BDA0003519447380000095
和
Figure BDA0003519447380000096
为单位化的相邻两段线性路径；
Figure BDA0003519447380000097
为单位化的过渡圆环；
使用共形几何代数，将线段qi-1pi,piqi+1参数化：
Yi-＝(1-λ)Qi-+λPi+λ(1-λ)Qi-·Pie∞
Yi+＝(1-λ)Pi+λQi++λ(1-λ)Pi·Qi+e∞
其中，Yi-与Yi+分别为两线段qi-1pi与piqi+1的参数化表示，e∞为五维共形空间中表示无穷远处的基向量；Pi-1,Pi,Pi+1,Qi-,Qi+分别为pi-1,pi,pi+1,qi-,qi+在共形空间中的表示；
计算直线Li-与圆环
Figure BDA0003519447380000098
之间的变换算子以及直线Li+与圆环
Figure BDA0003519447380000099
之间的变换算子：
Figure BDA00035194473800000910
Figure BDA00035194473800000911
其中*表示几何积运算；
从而计算得到Qi-与Pi之间的转接曲线Xi-以及Pi与Qi+之间的转接曲线Xi+：
Figure BDA00035194473800000912
Figure BDA00035194473800000913
Ri-(λ)为直线Li-与圆环
Figure BDA00035194473800000914
之间的变换算子，
Figure BDA00035194473800000915
为Ri-(λ)的逆序，Ri+(λ)为直线Li+与圆环
Figure BDA00035194473800000916
之间的变换算子，
Figure BDA00035194473800000917
为Ri+(λ)的逆序。
根据本发明提供的一种机械臂线性运动轨迹平滑系统，包括：
模块M1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点，将经过转接曲线首末点和线性路径衔接点的圆环定义为过渡圆环；
模块M2：使用共形几何代数表示原始线性路径与过渡圆环；
模块M3：通过原始线性路径与过渡圆环之间的插值圆环构造转接曲线。
通过以下公式计算每个线性路径衔接点pi处相邻两段线性路径的长度li和li+1：
Figure BDA0003519447380000101
Figure BDA0003519447380000102
其中，Pi-1,Pi,Pi+1分别为pi-1,pi,pi+1在共形空间中的表示；
·表示内积运算；
为保证相邻转接曲线不相交，转接长度di通过以下方程获得：
Figure BDA0003519447380000103
其中，ε为给定的最大光顺误差，li和li+1分别为对应衔接点处相邻两段线性路径的长度；θi表示对应衔接点处相邻两段线性路径的夹角；
计算得到转接曲线的首末点qi-与qi+的坐标：
qi-＝pi+divi-
qi+＝pi+divi+
其中，
Figure BDA0003519447380000104
定义通过点qi+,pi,qi+的圆环为过渡圆环。
具体地，在所述模块M2中：
对每个线性路径衔接点pi,i＝1,2,…,n，使用共形几何代数，分别将相邻两段线性路径以及过渡圆环表示为
Li-＝Pi-1∧Qi-∧Pi
Li+＝Pi∧Qi+∧Pi+1
Ci＝Qi-∧Pi∧Qi+
其中，Pi-1,Pi,Pi+1,Qi-,Qi+分别为pi-1,pi,pi+1,qi-,qi+在共形空间中的表示；∧表示外积运算；
将Li-,Li+,Ci单位化：
Figure BDA0003519447380000105
Figure BDA0003519447380000111
Figure BDA0003519447380000112
具体地，在所述模块M3中：
对每个线性路径衔接点pi,i＝1,2,…,n，计算Ci在Pi点处的切线与直线Li-的夹角αi：
Figure BDA0003519447380000113
其中·表示内积运算；
使用以下公式计算直线段Li-,Li+与过渡圆环Ci之间的插值圆环
Figure BDA0003519447380000114
Figure BDA0003519447380000115
其中，
Figure BDA0003519447380000116
和
Figure BDA0003519447380000117
分别表示Li-,Li+与过渡圆环Ci之间的插值圆环，λ∈[0,1]为插值参数；
Figure BDA0003519447380000118
和
Figure BDA0003519447380000119
为单位化的相邻两段线性路径；
Figure BDA00035194473800001110
为单位化的过渡圆环；
使用共形几何代数，将线段qi-1pi,piqi+1参数化：
Yi-＝(1-λ)Qi-+λPi+λ(1-λ)Qi-·Pie∞
Yi+＝(1-λ)Pi+λQi++λ(1-λ)Pi·Qi+e∞
其中，Yi-与Yi+分别为两线段qi-1pi与piqi+1的参数化表示，e∞为五维共形空间中表示无穷远处的基向量；Pi-1,Pi,Pi+1,Qi-,Qi+分别为pi-1,pi,pi+1,qi-,qi+在共形空间中的表示；
计算直线Li-与圆环
Figure BDA00035194473800001111
之间的变换算子以及直线Li+与圆环
Figure BDA00035194473800001112
之间的变换算子：
Figure BDA00035194473800001113
Figure BDA00035194473800001114
其中*表示几何积运算；
从而计算得到Qi-与Pi之间的转接曲线Xi-以及Pi与Qi+之间的转接曲线Xi+：
Figure BDA00035194473800001115
Figure BDA0003519447380000121
Ri-(λ)为直线Li-与圆环
Figure BDA0003519447380000122
之间的变换算子，
Figure BDA0003519447380000123
为Ri-(λ)的逆序，Ri+(λ)为直线Li+与圆环
Figure BDA0003519447380000124
之间的变换算子，
Figure BDA0003519447380000125
为Ri+(λ)的逆序。
根据本发明提供的一种存储有计算机程序的计算机可读存储介质，所述计算机程序被处理器执行时实现任一项所述的方法的步骤，调用任一项所述的系统的模块。
实施例2：
实施例2为实施例1的优选例，以更为具体地对本发明进行说明。
本发明涉及一种机械臂线性运动轨迹平滑方法，可在线性路径基础上获取机械臂光滑的运动轨迹。
针对现有技术的不足，本发明提供了一种机械臂线性运动轨迹平滑方法，利用共形几何代数，可在线性路径基础上解析计算机械臂光滑的运动轨迹。
根据本发明提供的机械臂线性运动轨迹平滑方法，包括：
步骤1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点；
步骤2：使用共形几何代数表示原始直线段与过渡圆环；
步骤3：通过直线与过渡圆环之间的插值圆环构造转接曲线。
初始小线段路径为蝴蝶形状路径曲线，如图1所示，该曲线由95条线性小线段路径组成。
使用e1,e2,e3表示三维欧氏空间中三个单位正交基向量，e0和e∞表示五维共性空间中另外两个基向量。
对于i＝1,2,…,94，将每个线性路径衔接点表示为pi＝pixe1+piye2+pize3，通过以下方程计算衔接点处相邻两段线性路径的夹角：
Figure BDA0003519447380000126
其中，pix,piy,piz分别表示pi点在欧式空间中的x轴、y轴和z轴的坐标；
·表示内积运算。
通过以下公式在共形空间中表示pi：
Figure BDA0003519447380000127
通过以下公式计算衔接点处相邻两段线性路径的长度li和li+1：
Figure BDA0003519447380000131
Figure BDA0003519447380000132
为保证相邻转接曲线不相交，转接长度di通过以下方程获得：
Figure BDA0003519447380000133
其中，ε为给定的最大光顺误差，取为ε＝0.0005m。
从而计算得到转接曲线的首末点qi-与qi+的坐标：
qi-＝pi+divi-
qi+＝pi+divi+
其中
Figure BDA0003519447380000134
Figure BDA0003519447380000135
通过以下公式在共形空间中表示qi-和qi+：
Figure BDA0003519447380000136
Figure BDA0003519447380000137
对每个线性路径衔接点pi,i＝1,2,…,94，使用共形几何代数，分别将相邻两段线性路径以及过渡圆环表示为
Li-＝Pi-1∧Qi-^Pi
Li+＝Pi∧Qi+^Pi+1
Ci＝Qi-^Pi∧Qi+
其中^表示外积运算。
将Li-,Li+,Ci单位化：
Figure BDA0003519447380000138
对每个线性路径衔接点pi,i＝1,2,…,94，计算Ci在Pi点处的切线与直线Li-的夹角αi：
Figure BDA0003519447380000141
使用以下公式计算直线段Li-,Li+与过渡圆环Ci之间的插值圆环
Figure BDA0003519447380000142
Figure BDA0003519447380000143
其中，λ∈[0,1]为插值参数。
使用共形几何代数，将线段qi-1pi,piqi+1参数化：
Yi-＝(1-λ)Qi-+λPi+λ(1-λ)Qi-·Pie∞
Yi+＝(1-λ)Pi+λQi++λ(1-λ)Pi·Qi+e∞
计算直线Li-与圆环
Figure BDA0003519447380000144
之间的变换算子以及直线Li+与圆环
Figure BDA0003519447380000145
之间的变换算子：
Figure BDA0003519447380000146
Figure BDA0003519447380000147
其中*表示几何积运算。
从而计算得到Qi-与Pi之间的转接曲线Xi-以及Pi与Qi+之间的转接曲线Xi+：
Figure BDA0003519447380000148
Figure BDA0003519447380000149
经过平滑处理后的曲线如图2所示，图中短划线表示所生成的转接曲线。
所生成平滑路径在UR10机械臂上执行。设置机械臂末端最大进给速度为0.5m/s，最大切向加速度为5m/s2，最大切向跃度为30m/s3，设置机械臂每个关节角最大关节速度为1rad/s，最大关节加速度为5rad/s2，最大关节跃度为50rad/s3，最大弓高误差为0.00005m，插补步长为8ms。速度规划结果如图3所示。
一种典型的基于B样条在转接角内部生成转接曲线的方法生成的转接曲线如图2中点虚线所示，在上述同样约束条件下进行速度规划，结果如图4所示。通过与图3对比可知，本发明所述方法可大幅缩短机械臂运行时间。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (7)

1.一种机械臂线性运动轨迹平滑方法，其特征在于，包括：
步骤S1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点，将经过转接曲线首末点和线性路径衔接点的圆环定义为过渡圆环；
步骤S2：使用共形几何代数表示原始线性路径与过渡圆环；
步骤S3：通过原始线性路径与过渡圆环之间的插值圆环构造转接曲线；
在所述步骤S1中：
通过以下公式计算每个线性路径衔接点
Figure QLYQS_1
处相邻两段线性路径的长度
Figure QLYQS_2
和
Figure QLYQS_3
：
Figure QLYQS_4
Figure QLYQS_5
其中，
Figure QLYQS_6
分别为
Figure QLYQS_7
在共形空间中的表示；
Figure QLYQS_8
表示内积运算；
为保证相邻转接曲线不相交，转接长度
Figure QLYQS_9
通过以下方程获得：
Figure QLYQS_10
其中，
Figure QLYQS_11
为给定的最大光顺误差，
Figure QLYQS_12
和
Figure QLYQS_13
分别为对应衔接点处相邻两段线性路径的长度；
Figure QLYQS_14
表示对应衔接点处相邻两段线性路径的夹角；
计算得到转接曲线的首末点
Figure QLYQS_15
与
Figure QLYQS_16
的坐标：
Figure QLYQS_17
Figure QLYQS_18
其中，
Figure QLYQS_19
，
Figure QLYQS_20
；
定义通过点
Figure QLYQS_21
的圆环为过渡圆环。
2.根据权利要求1所述的机械臂线性运动轨迹平滑方法，其特征在于，在所述步骤S2中：
对每个线性路径衔接点
Figure QLYQS_22
，使用共形几何代数，分别将相邻两段线性路径以及过渡圆环表示为
Figure QLYQS_23
Figure QLYQS_24
Figure QLYQS_25
其中，
Figure QLYQS_26
分别为
Figure QLYQS_27
在共形空间中的表示；
Figure QLYQS_28
表示外积运算；
将
Figure QLYQS_29
单位化：
Figure QLYQS_30
Figure QLYQS_31
Figure QLYQS_32
。
3.根据权利要求1所述的机械臂线性运动轨迹平滑方法，其特征在于，在所述步骤S3中：
对每个线性路径衔接点
Figure QLYQS_33
，计算
Figure QLYQS_34
在
Figure QLYQS_35
点处的切线与直线
Figure QLYQS_36
的夹角
Figure QLYQS_37
：
Figure QLYQS_38
其中
Figure QLYQS_39
表示内积运算；
使用以下公式计算直线段
Figure QLYQS_40
与过渡圆环
Figure QLYQS_41
之间的插值圆环
Figure QLYQS_42
Figure QLYQS_43
其中，
Figure QLYQS_45
和
Figure QLYQS_47
分别表示
Figure QLYQS_49
与过渡圆环
Figure QLYQS_46
之间的插值圆环，
Figure QLYQS_48
为插值参数；
Figure QLYQS_50
和
Figure QLYQS_51
为单位化的相邻两段线性路径；
Figure QLYQS_44
为单位化的过渡圆环；
使用共形几何代数，将线段
Figure QLYQS_52
参数化：
Figure QLYQS_53
Figure QLYQS_54
其中，
Figure QLYQS_55
与
Figure QLYQS_56
分别为两线段
Figure QLYQS_57
与
Figure QLYQS_58
的参数化表示，
Figure QLYQS_59
为五维共形空间中表示无穷远处的基向量；
Figure QLYQS_60
分别为
Figure QLYQS_61
在共形空间中的表示；
计算直线
Figure QLYQS_62
与圆环
Figure QLYQS_63
之间的变换算子以及直线
Figure QLYQS_64
与圆环
Figure QLYQS_65
之间的变换算子：
Figure QLYQS_66
Figure QLYQS_67
其中
Figure QLYQS_68
表示几何积运算；
从而计算得到
Figure QLYQS_69
与
Figure QLYQS_70
之间的转接曲线
Figure QLYQS_71
以及
Figure QLYQS_72
与
Figure QLYQS_73
之间的转接曲线
Figure QLYQS_74
：
Figure QLYQS_75
Figure QLYQS_76
Figure QLYQS_79
为直线
Figure QLYQS_80
与圆环
Figure QLYQS_83
之间的变换算子，
Figure QLYQS_78
为
Figure QLYQS_82
的逆序，
Figure QLYQS_85
为直线
Figure QLYQS_86
与圆环
Figure QLYQS_77
之间的变换算子，
Figure QLYQS_81
为
Figure QLYQS_84
的逆序。
4.一种机械臂线性运动轨迹平滑系统，其特征在于，包括：
模块M1：根据给定光顺误差计算每个线性路径衔接点处的转接长度，确定转接曲线的首末点，将经过转接曲线首末点和线性路径衔接点的圆环定义为过渡圆环；
模块M2：使用共形几何代数表示原始线性路径与过渡圆环；
模块M3：通过原始线性路径与过渡圆环之间的插值圆环构造转接曲线；
在所述模块M1中：
通过以下公式计算每个线性路径衔接点
Figure QLYQS_87
处相邻两段线性路径的长度
Figure QLYQS_88
和
Figure QLYQS_89
：
Figure QLYQS_90
Figure QLYQS_91
其中，
Figure QLYQS_92
分别为
Figure QLYQS_93
在共形空间中的表示；
Figure QLYQS_94
表示内积运算；
为保证相邻转接曲线不相交，转接长度
Figure QLYQS_95
通过以下方程获得：
Figure QLYQS_96
其中，
Figure QLYQS_97
为给定的最大光顺误差，
Figure QLYQS_98
和
Figure QLYQS_99
分别为对应衔接点处相邻两段线性路径的长度；
Figure QLYQS_100
表示对应衔接点处相邻两段线性路径的夹角；
计算得到转接曲线的首末点
Figure QLYQS_101
与
Figure QLYQS_102
的坐标：
Figure QLYQS_103
Figure QLYQS_104
其中，
Figure QLYQS_105
，
Figure QLYQS_106
；
定义通过点
Figure QLYQS_107
的圆环为过渡圆环。
5.根据权利要求4所述的机械臂线性运动轨迹平滑系统，其特征在于，在所述模块M2中：
对每个线性路径衔接点
Figure QLYQS_108
，使用共形几何代数，分别将相邻两段线性路径以及过渡圆环表示为
Figure QLYQS_109
Figure QLYQS_110
Figure QLYQS_111
其中，
Figure QLYQS_112
分别为
Figure QLYQS_113
在共形空间中的表示；
Figure QLYQS_114
表示外积运算；
将
Figure QLYQS_115
单位化：
Figure QLYQS_116
Figure QLYQS_117
Figure QLYQS_118
。
6.根据权利要求4所述的机械臂线性运动轨迹平滑系统，其特征在于，在所述模块M3中：
对每个线性路径衔接点
Figure QLYQS_119
，计算
Figure QLYQS_120
在
Figure QLYQS_121
点处的切线与直线
Figure QLYQS_122
的夹角
Figure QLYQS_123
：
Figure QLYQS_124
其中
Figure QLYQS_125
表示内积运算；
使用以下公式计算直线段
Figure QLYQS_126
与过渡圆环
Figure QLYQS_127
之间的插值圆环
Figure QLYQS_128
Figure QLYQS_129
其中，
Figure QLYQS_132
和
Figure QLYQS_133
分别表示
Figure QLYQS_135
与过渡圆环
Figure QLYQS_131
之间的插值圆环，
Figure QLYQS_134
为插值参数；
Figure QLYQS_136
和
Figure QLYQS_137
为单位化的相邻两段线性路径；
Figure QLYQS_130
为单位化的过渡圆环；
使用共形几何代数，将线段
Figure QLYQS_138
参数化：
Figure QLYQS_139
Figure QLYQS_140
其中，
Figure QLYQS_141
与
Figure QLYQS_142
分别为两线段
Figure QLYQS_143
与
Figure QLYQS_144
的参数化表示，
Figure QLYQS_145
为五维共形空间中表示无穷远处的基向量；
Figure QLYQS_146
分别为
Figure QLYQS_147
在共形空间中的表示；
计算直线
Figure QLYQS_148
与圆环
Figure QLYQS_149
之间的变换算子以及直线
Figure QLYQS_150
与圆环
Figure QLYQS_151
之间的变换算子：
Figure QLYQS_152
Figure QLYQS_153
其中
Figure QLYQS_154
表示几何积运算；
从而计算得到
Figure QLYQS_155
与
Figure QLYQS_156
之间的转接曲线
Figure QLYQS_157
以及
Figure QLYQS_158
与
Figure QLYQS_159
之间的转接曲线
Figure QLYQS_160
：
Figure QLYQS_161
Figure QLYQS_162
Figure QLYQS_164
为直线
Figure QLYQS_167
与圆环
Figure QLYQS_171
之间的变换算子，
Figure QLYQS_165
为
Figure QLYQS_168
的逆序，
Figure QLYQS_170
为直线
Figure QLYQS_172
与圆环
Figure QLYQS_163
之间的变换算子，
Figure QLYQS_166
为
Figure QLYQS_169
的逆序。
7.一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序被处理器执行时实现权利要求1至3中任一项所述的方法的步骤，调用权利要求4至6中任一项所述的系统的模块。
CN202210177372.6A 2022-02-24 2022-02-24 机械臂线性运动轨迹平滑方法及系统和存储介质 Active CN114536337B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210177372.6A CN114536337B (zh) 2022-02-24 2022-02-24 机械臂线性运动轨迹平滑方法及系统和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210177372.6A CN114536337B (zh) 2022-02-24 2022-02-24 机械臂线性运动轨迹平滑方法及系统和存储介质

Publications (2)

Publication Number Publication Date
CN114536337A CN114536337A (zh) 2022-05-27
CN114536337B true CN114536337B (zh) 2023-06-23

Family

ID=81679756

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210177372.6A Active CN114536337B (zh) 2022-02-24 2022-02-24 机械臂线性运动轨迹平滑方法及系统和存储介质

Country Status (1)

Country Link
CN (1) CN114536337B (zh)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105500354A (zh) * 2016-02-02 2016-04-20 南京埃斯顿机器人工程有限公司 一种工业机器人应用的过渡轨迹规划方法
CN106647623A (zh) * 2016-12-22 2017-05-10 广州数控设备有限公司 一种几何精度及衔接速度最优化的五轴联动平滑插补方法
CN108829031A (zh) * 2018-06-29 2018-11-16 深圳市汇川技术股份有限公司 轨迹间的局部光顺过渡方法、设备及存储介质
CN109571473A (zh) * 2018-12-03 2019-04-05 武汉工程大学 一种误差可控的小线段轨迹光顺方法
DE102019204651A1 (de) * 2019-04-02 2020-10-08 Continental Teves Ag & Co. Ohg Verfahren zum Planen einer Parktrajektorie eines von einem Parkassistenzsystem unterstützten Parkvorgangs
CN111857037A (zh) * 2020-06-19 2020-10-30 深圳市亿维自动化技术有限公司 一种过渡轨迹的生成方法、机器人及计算机可读存储介质
CN112504279A (zh) * 2020-11-27 2021-03-16 上海交通大学 适用于无人机的无碰撞路径规划方法、系统及介质
CN112659126A (zh) * 2020-12-23 2021-04-16 广东工业大学 一种基于非均匀b样条曲线的工业机器人轨迹段过渡方法
CN113608496A (zh) * 2021-07-28 2021-11-05 天津大学 空间路径g2转接光顺方法、设备及计算机可读存储介质

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106647282B (zh) * 2017-01-19 2020-01-03 北京工业大学 一种考虑末端运动误差的六自由度机器人轨迹规划方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105500354A (zh) * 2016-02-02 2016-04-20 南京埃斯顿机器人工程有限公司 一种工业机器人应用的过渡轨迹规划方法
CN106647623A (zh) * 2016-12-22 2017-05-10 广州数控设备有限公司 一种几何精度及衔接速度最优化的五轴联动平滑插补方法
CN108829031A (zh) * 2018-06-29 2018-11-16 深圳市汇川技术股份有限公司 轨迹间的局部光顺过渡方法、设备及存储介质
CN109571473A (zh) * 2018-12-03 2019-04-05 武汉工程大学 一种误差可控的小线段轨迹光顺方法
DE102019204651A1 (de) * 2019-04-02 2020-10-08 Continental Teves Ag & Co. Ohg Verfahren zum Planen einer Parktrajektorie eines von einem Parkassistenzsystem unterstützten Parkvorgangs
CN111857037A (zh) * 2020-06-19 2020-10-30 深圳市亿维自动化技术有限公司 一种过渡轨迹的生成方法、机器人及计算机可读存储介质
CN112504279A (zh) * 2020-11-27 2021-03-16 上海交通大学 适用于无人机的无碰撞路径规划方法、系统及介质
CN112659126A (zh) * 2020-12-23 2021-04-16 广东工业大学 一种基于非均匀b样条曲线的工业机器人轨迹段过渡方法
CN113608496A (zh) * 2021-07-28 2021-11-05 天津大学 空间路径g2转接光顺方法、设备及计算机可读存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
An efficient method for collision-free and jerk-constrained trajectory generation with sparse desired way-points for a flying robot;Chen, YongXue等;《SCIENCE CHINA-TECHNOLOGICAL SCIENCES》;第64卷(第8期);全文 *
基于改进 ＲＲT 算法的 6-DOF 机器人路径规划;李 季 ，史晨发， 邵 磊，刘宏利;《计算机应用与软件》;第37卷(第9期);全文 *

Also Published As

Publication number Publication date
CN114536337A (zh) 2022-05-27

Similar Documents

Publication Publication Date Title
CN107263484B (zh) 机器人关节空间点到点运动的轨迹规划方法
CN112757306B (zh) 一种机械臂逆解多解选择和时间最优轨迹规划算法
Zhang et al. Curve fitting and optimal interpolation on CNC machines based on quadratic B-splines
CN111761582B (zh) 一种基于随机采样的移动机械臂避障规划方法
CN105773620A (zh) 基于倍四元数的工业机器人自由曲线的轨迹规划控制方法
Rezende et al. Constructive time-varying vector fields for robot navigation
Huang et al. Real-time feedrate scheduling for five-axis machining by simultaneously planning linear and angular trajectories
JP5555838B2 (ja) 軌道生成方法および軌道生成装置
CN113433889B (zh) 一种基于三段式羊角曲线的五轴机床加工的刀具轨迹规划方法
Liu et al. Weld seam fitting and welding torch trajectory planning based on NURBS in intersecting curve welding
CN113084792A (zh) 关节奇异区域的确定方法、机器人及存储装置
CN109676603A (zh) 一种柔性超冗余度机器人控制方法
CN111633668A (zh) 一种用于机器人加工三维自由曲面的运动控制方法
CN113650011B (zh) 一种机械臂拼接路径规划方法及装置
Fang et al. Study on high-speed and smooth transfer of robot motion trajectory based on modified S-shaped acceleration/deceleration algorithm
CN116079714A (zh) 一种基于b样条的六轴机械臂轨迹规划方法
CN115213898A (zh) 一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法
CN114536337B (zh) 机械臂线性运动轨迹平滑方法及系统和存储介质
CN112356032B (zh) 一种姿态平滑过渡方法及系统
CN114063621B (zh) 一种轮式机器人编队跟踪与避障控制方法
CN113608496B (zh) 空间路径g2转接光顺方法、设备及计算机可读存储介质
CN111880480B (zh) 一种基于cnc铣床的铣刀切割路径生成方法及系统
CN113467376A (zh) 一种面向多加工场景的多轴轨迹压缩方法
CN115922705B (zh) 一种机器人关节速度计算方法、系统及计算机设备
Shirato et al. An Optimized Path Planning of Manipulator with Spline Curves Using Real Quantifier Elimination Based on Comprehensive Gröbner Systems

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant