CN115741695A - 一种六轴串联工业机器人末端的姿态轨迹过渡规划方法 - Google Patents

一种六轴串联工业机器人末端的姿态轨迹过渡规划方法 Download PDF

Info

Publication number
CN115741695A
CN115741695A CN202211456525.7A CN202211456525A CN115741695A CN 115741695 A CN115741695 A CN 115741695A CN 202211456525 A CN202211456525 A CN 202211456525A CN 115741695 A CN115741695 A CN 115741695A
Authority
CN
China
Prior art keywords
attitude
transition
track
point
section
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.)
Pending
Application number
CN202211456525.7A
Other languages
English (en)
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.)
Jiangnan University
Original Assignee
Jiangnan 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 Jiangnan University filed Critical Jiangnan University
Priority to CN202211456525.7A priority Critical patent/CN115741695A/zh
Publication of CN115741695A publication Critical patent/CN115741695A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明公开了一种六轴串联工业机器人末端的姿态轨迹过渡规划方法，本发明采用四元数描述姿态，利用了三次四元数准均匀B样条曲线作为姿态轨迹过渡段，实现姿态轨迹的平滑过渡。简化了四元数样条曲线控制点的求取方式，降低了算法的实施难度。姿态轨迹过渡段的角位移长度求取采用了复化六点高斯勒让德积分法，插补计算利用了牛顿迭代法结合六点高斯勒让德积分法的方式。从而驱动六轴串联工业机器人末端在整个平滑的路径上连续运动，获得了平滑的运动轨迹，避免了频繁的加减速，提高了六轴串联工业机器人的运行效率，满足了生产实际的工艺需求。

Description

一种六轴串联工业机器人末端的姿态轨迹过渡规划方法
技术领域
本发明涉及机器人控制技术领域，特别涉及一种六轴串联工业机器人末端的姿态轨迹过渡规划方法。
背景技术
六轴串联工业机器人末端的姿态轨迹规划算法需要在两个或多个姿态示教点之间构造平滑的姿态轨迹，但姿态轨迹缺乏直观性且描述姿态的方式相对复杂，因此关于姿态轨迹规划的研究比较困难。然而六轴串联工业机器人末端的姿态轨迹规划算法对于六轴串联工业机器人的运行效率同样有着很大的影响，好的的姿态轨迹规划算法可以极大的提高六轴串联工业机器人的运动精度，降低六轴串联工业机器人受到的振动冲击，从而延长六轴串联工业机器人的使用寿命，因此对于六轴串联工业机器人末端的姿态轨迹规划算法的研究十分重要。但常见的姿态轨迹规划算法如基于四元数的球面线性插补算法构造出的姿态轨迹在轨迹衔接处同样存在轨迹突变问题，导致六轴串联工业机器人在姿态轨迹衔接处的角速度必须降为零，六轴串联工业机器人仍会进行频繁的加减速，降低了六轴串联工业机器人的运行效率。因此需要设计姿态轨迹过渡规划算法利用姿态样条曲线构造姿态轨迹过渡段对姿态轨迹进行过渡处理，但是采用欧拉角、旋转矩阵和轴角法描述姿态时很难推导出姿态样条曲线的参数表达式，算法实施难度大。
发明内容
本发明要解决的技术问题是提供一种可实现姿态轨迹的平滑过渡、提高机器人运行效率的六轴串联工业机器人末端的姿态轨迹过渡规划方法。
为了解决上述问题，本发明提供了一种六轴串联工业机器人末端的姿态轨迹过渡规划方法，包括以下步骤：
S1、令i＝1，N为给定运动路径的总段数；
S2、在由姿态示教点Qi和Qi+1构成的球面线性插补轨迹和由姿态示教点Qi+1和Qi+2构成的球面线性插补轨迹之间进行平滑过渡；其中，利用三次四元数准均匀B样条曲线作为姿态轨迹过渡段；姿态示教点Qi和Qi+1分别为第i段姿态轨迹的起始姿态点和终止姿态点；姿态示教点Qi+1和Qi+2分别为第i+1段姿态轨迹的起始姿态点和终止姿态点；
S3、根据第i段姿态轨迹的姿态示教起始姿态点Qi和终止姿态点Qi+1求取第i段姿态轨迹的角位移长度θi和参数Q′i(u)；参数Q′i(u)的表达式如下：
Figure BDA0003953482090000021
S4、根据第i+1段姿态轨迹的姿态示教起始姿态点Qi+1和终止姿态点Qi+2求取第i+1段姿态轨迹的角位移长度θi+1和参数Q′i+1(u)；参数Q′i+1(u)的表达式如下：
Figure BDA0003953482090000022
S5、根据第i段姿态轨迹的角位移长度θi、第i+1段姿态轨迹的角位移长度θi+1和给定姿态过渡半径
Figure BDA0003953482090000025
求取实际姿态过渡半径
Figure BDA0003953482090000026
S6、根据第i段姿态轨迹的角位移长度θi和实际姿态过渡半径
Figure BDA0003953482090000027
求取第i段姿态轨迹过渡段的姿态过渡起点对应的参数值，再根据参数Q′i(u)和导数的求取方式求取第i段姿态轨迹在姿态过渡起点处的姿态、一阶导数和二阶导数；
S7、根据第i+1段姿态轨迹的角位移长度θi+1和实际姿态过渡半径
Figure BDA0003953482090000028
求取第i段姿态轨迹过渡段的姿态过渡终点对应的参数值，再根据参数Q′i+1(u)和导数的求取方式求取第i+1段姿态轨迹在姿态过渡终点处的姿态、一阶导数和二阶导数；
S8、建立第i段姿态轨迹过渡段的参数表达式qi(u)；
S9、根据两条姿态曲线在曲线拼接实现曲率连续需要满足的条件建立方程组，并求取姿态控制点的姿态后，代入第i段姿态轨迹过渡段的参数表达式qi(u)；
S10、计算第i段姿态轨迹过渡段的角位移长度
Figure BDA0003953482090000023
S11、根据第i段姿态轨迹过渡段的角位移长度
Figure BDA0003953482090000024
进行速度规划，获得角位移长度与时间的函数关系，从而根据插补点的时刻得到插补点在姿态轨迹过渡段上的相对角位移长度，接着根据插补点在姿态轨迹过渡段上的相对角位移长度反解出插补点对应的参数值，最后将插补点对应的参数值代入姿态轨迹过渡段的参数表达式获得插补点的姿态；
S12、令i＝i+1；判断i小于N-2是否成立，成立则执行步骤S2，不成立则执行步骤S14；
S13、通过运动学逆解求得六轴串联工业机器人各关节转动的角度以驱动六轴串联工业机器人转动，从而实现六轴串联工业机器人末端的姿态轨迹过渡。
作为本发明的进一步改进，第i段姿态轨迹的角位移长度θi，如下：
θi＝arccos(Qi,Qi+1)
第i+1段姿态轨迹的角位移长度θi+1，如下：
θi+1＝arccos(Qi+1,Qi+2)。
作为本发明的进一步改进，步骤S5还包括：
判断
Figure BDA0003953482090000031
或
Figure BDA0003953482090000032
是否成立，不成立则令
Figure BDA0003953482090000033
成立则再判断θi＞θi+1是否成立，成立则令
Figure BDA0003953482090000034
不成立则令
Figure BDA0003953482090000035
作为本发明的进一步改进，步骤S6包括：
根据第i段姿态轨迹的角位移长度θi和实际姿态过渡半径
Figure BDA0003953482090000036
求取第i段姿态轨迹过渡段的姿态过渡起点对应的参数值
Figure BDA0003953482090000037
如下：
Figure BDA0003953482090000038
再根据参数Q′i(u)和导数的求取方式求取第i段姿态轨迹在姿态过渡起点处的姿态
Figure BDA0003953482090000039
一阶导数
Figure BDA00039534820900000310
和二阶导数
Figure BDA00039534820900000311
Figure BDA00039534820900000312
Figure BDA00039534820900000313
Figure BDA00039534820900000314
作为本发明的进一步改进，步骤S7包括：
根据第i+1段姿态轨迹的角位移长度θi+1和实际姿态过渡半径
Figure BDA00039534820900000315
求取第i段姿态轨迹过渡段的姿态过渡终点对应的参数值
Figure BDA00039534820900000316
如下：
Figure BDA0003953482090000041
再根据参数Q′i+1(u)和导数的求取方式求取第i+1段姿态轨迹在姿态过渡终点处的姿态
Figure BDA0003953482090000042
一阶导数
Figure BDA0003953482090000043
和二阶导数
Figure BDA0003953482090000044
如下：
Figure BDA0003953482090000045
Figure BDA0003953482090000046
Figure BDA0003953482090000047
作为本发明的进一步改进，步骤S8中，建立第i段姿态轨迹过渡段的参数表达式qi(u)，如下：
Figure BDA0003953482090000048
其中，Bj,3(u),j＝0,1,2,3,4的求取如式(12)所示，将式(13)-式(18)的N0,3(u)、N1,3(u)、N2,3(u)、N3,3(u)和N4,3(u)代入式(12)依次求得B0,3(u)、B1,3(u)、B2,3(u)、B3,3(u)和B4,3(u)；
Figure BDA00039534820900000412
和
Figure BDA00039534820900000413
是作为姿态轨迹过渡段的六个姿态控制点的姿态；
Figure BDA0003953482090000049
Figure BDA00039534820900000410
Figure BDA00039534820900000411
Figure BDA0003953482090000051
Figure BDA0003953482090000052
Figure BDA0003953482090000053
Figure BDA0003953482090000054
根据qi(u)求得qi(0)如式(19)所示，qi(1)如式(20)所示，接着根据qi(u)求得一阶导数
Figure BDA0003953482090000055
如式(21)所示，
Figure BDA0003953482090000056
如式(22)所示，然后求得二阶导数
Figure BDA0003953482090000057
如式(23)所示，
Figure BDA0003953482090000058
如式(24)所示；
Figure BDA0003953482090000059
Figure BDA00039534820900000510
Figure BDA00039534820900000511
Figure BDA00039534820900000512
Figure BDA00039534820900000513
Figure BDA0003953482090000061
作为本发明的进一步改进，步骤S9包括：根据两条姿态曲线在曲线拼接实现曲率连续需要满足的条件建立方程组，如下：
Figure BDA0003953482090000062
代入公式(19)-公式(24)可得公式(26)，如下：
Figure BDA0003953482090000063
其中，α和β均为大于零的任意常数；
根据公式(26)求得姿态控制点
Figure BDA0003953482090000064
和
Figure BDA0003953482090000065
的姿态后，代入公式(11)。作为本发明的进一步改进，所述根据公式(26)求得姿态控制点
Figure BDA0003953482090000066
和
Figure BDA0003953482090000067
的姿态，包括：
根据公式(26)求取公式(27)，接着根据求出的姿态控制点
Figure BDA0003953482090000068
和
Figure BDA0003953482090000069
以及参数α和β代入式(28)求得姿态控制点
Figure BDA00039534820900000610
和
Figure BDA00039534820900000611
最后再将求出的姿态控制点
Figure BDA00039534820900000612
Figure BDA00039534820900000613
和
Figure BDA00039534820900000614
以及参数α和β代入式(29)求得姿态控制点
Figure BDA00039534820900000615
和
Figure BDA00039534820900000616
Figure BDA0003953482090000071
Figure BDA0003953482090000072
Figure BDA0003953482090000073
α和β可以为大于零的任意常数，所以存在无数组满足条件的姿态控制点；引入参数λ代替参数α和β，参数λ与参数α和β的关系如式(30)所示；
Figure BDA0003953482090000074
作为本发明的进一步改进，步骤S10包括：
采用复化六点高斯勒让德积分法来求取第i段姿态轨迹过渡段的角位移长度
Figure BDA0003953482090000075
如式(31)：
Figure BDA0003953482090000076
其中，xk为积分节点，Ak为积分系数，具体的数值如式(32)：
Figure BDA0003953482090000077
作为本发明的进一步改进，步骤S11包括：
S111、给定插补点在第i段姿态轨迹过渡段上沿着超球面上的轨迹方向距离第i段姿态轨迹过渡段的姿态过渡起点的相对角位移长度θl，
Figure BDA0003953482090000086
S112、根据相对角位移长度θl和各区间求取的角位移长度找出插补点所在区间的段号m，m＝1,2,···,6，从而获得牛顿迭代法的搜索区间为
Figure BDA0003953482090000081
并根据式(33)对相对角位移长度θl进行修正；
Figure BDA0003953482090000082
S113、令牛顿迭代法的迭代初值ul＝(m-1)/6；
S114、构造如式(34)所示的以ul为自变量的误差函数f(ul)，代入ul的值计算f(ul)的值；
Figure BDA0003953482090000083
其中，在计算误差函数f(ul)的值时也需要进行积分求取，采用六点高斯勒让德积分法进行积分求取，f(ul)的求取公式如式(35)所示。
Figure BDA0003953482090000084
其中，xk为积分节点，Ak为积分系数，具体的数值如式(32)所示；
S115、判断f(ul)的绝对值大于0.00001是否成立，成立则跳转至步骤S6，不成立则跳转至步骤(8)；
S116、对ul进行迭代，迭代公式如式(36)所示；
Figure BDA0003953482090000085
S117、将迭代后的ul的值代入式(35)求得f(ul)的值，跳转至步骤S5；
S118、令u＝ul代入第i段姿态轨迹过渡段的公式(11)，从而获得插补点的姿态ql＝qi(u)。
本发明的有益效果：
本发明采用四元数描述姿态，利用了三次四元数准均匀B样条曲线作为姿态轨迹过渡段，实现姿态轨迹的平滑过渡。简化了四元数样条曲线控制点的求取方式，降低了算法的实施难度。姿态轨迹过渡段的角位移长度求取采用了复化六点高斯勒让德积分法，插补计算利用了牛顿迭代法结合六点高斯勒让德积分法的方式。从而驱动六轴串联工业机器人末端在整个平滑的路径上连续运动，获得了平滑的运动轨迹，避免了频繁的加减速，提高了六轴串联工业机器人的运行效率，满足了生产实际的工艺需求。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图，详细说明如下。
附图说明
图1为本发明实施例中六轴串联工业机器人末端的姿态轨迹过渡规划方法的流程图；
图2为本发明实施例中在给定运动路径之间构造姿态轨迹过渡路径的示意图；
图3为本发明实施例中λ取不同值时的姿态轨迹过渡示意图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。
如图1-2所示，为本发明优选实施例中六轴串联工业机器人末端的姿态轨迹过渡规划方法，包括以下步骤：
步骤S1、令i＝1，N为给定运动路径的总段数。
步骤S2、在由姿态示教点Qi和Qi+1构成的球面线性插补轨迹和由姿态示教点Qi+1和Qi+2构成的球面线性插补轨迹之间进行平滑过渡；其中，利用三次四元数准均匀B样条曲线作为姿态轨迹过渡段；其中，姿态示教点Qi和Qi+1分别为第i段姿态轨迹的起始姿态点和终止姿态点，给定最大角速度约束为wi，给定姿态过渡半径为
Figure BDA0003953482090000101
姿态示教点Qi+1和Qi+2分别为第i+1段姿态轨迹的起始姿态点和终止姿态点，给定最大角速度约束为wi+1；
Figure BDA0003953482090000102
为第i段姿态轨迹与第i+1段姿态轨迹进行过渡时的实际姿态过渡半径，
Figure BDA0003953482090000103
和
Figure BDA0003953482090000104
分别为第i段姿态轨迹过渡段的姿态过渡起点和姿态过渡终点，
Figure BDA0003953482090000105
为第i段姿态轨迹过渡段的角位移长度，
Figure BDA0003953482090000106
为姿态轨迹过渡段上的角速度敏感点的最大角速度约束，
Figure BDA0003953482090000107
和
Figure BDA0003953482090000108
是作为姿态轨迹过渡段的三次四元数准均匀B样条曲线的六个姿态控制点的姿态。
步骤S3、根据第i段姿态轨迹的姿态示教起始姿态点Qi和终止姿态点Qi+1求取第i段姿态轨迹的角位移长度θi和参数Q′i(u)；参数Q′i(u)的表达式如下：
Figure BDA0003953482090000109
第i段姿态轨迹的角位移长度θi，如下：
θi＝arccos(Qi,Qi+1)。
步骤S4、根据第i+1段姿态轨迹的姿态示教起始姿态点Qi+1和终止姿态点Qi+2求取第i+1段姿态轨迹的角位移长度θi+1和参数Q′i+1(u)；参数Q′i+1(u)的表达式如下：
Figure BDA00039534820900001010
第i+1段姿态轨迹的角位移长度θi+1，如下：
θi+1＝arccos(Qi+1,Qi+2)。
步骤S5、根据第i段姿态轨迹的角位移长度θi、第i+1段姿态轨迹的角位移长度θi+1和给定姿态过渡半径
Figure BDA00039534820900001011
求取实际姿态过渡半径
Figure BDA00039534820900001012
还包括：
判断
Figure BDA00039534820900001013
或
Figure BDA00039534820900001014
是否成立，不成立则令
Figure BDA00039534820900001015
成立则再判断θi＞θi+1是否成立，成立则令
Figure BDA00039534820900001016
不成立则令
Figure BDA00039534820900001017
步骤S6、根据第i段姿态轨迹的角位移长度θi和实际姿态过渡半径
Figure BDA00039534820900001018
求取第i段姿态轨迹过渡段的姿态过渡起点对应的参数值，再根据参数Q′i(u)和导数的求取方式求取第i段姿态轨迹在姿态过渡起点处的姿态、一阶导数和二阶导数；可选地，步骤S6包括：
根据第i段姿态轨迹的角位移长度θi和实际姿态过渡半径
Figure BDA0003953482090000111
求取第i段姿态轨迹过渡段的姿态过渡起点对应的参数值
Figure BDA0003953482090000112
如下：
Figure BDA0003953482090000113
再根据参数Q′i(u)和导数的求取方式求取第i段姿态轨迹在姿态过渡起点处的姿态
Figure BDA0003953482090000114
一阶导数
Figure BDA0003953482090000115
和二阶导数
Figure BDA0003953482090000116
Figure BDA0003953482090000117
Figure BDA0003953482090000118
Figure BDA0003953482090000119
步骤S7、根据第i+1段姿态轨迹的角位移长度θi+1和实际姿态过渡半径
Figure BDA00039534820900001110
求取第i段姿态轨迹过渡段的姿态过渡终点对应的参数值，再根据参数Q′i+1(u)和导数的求取方式求取第i+1段姿态轨迹在姿态过渡终点处的姿态、一阶导数和二阶导数；可选地，步骤S7包括：
根据第i+1段姿态轨迹的角位移长度θi+1和实际姿态过渡半径
Figure BDA00039534820900001111
求取第i段姿态轨迹过渡段的姿态过渡终点对应的参数值
Figure BDA00039534820900001112
如下：
Figure BDA00039534820900001113
再根据参数Q′i+1(u)和导数的求取方式求取第i+1段姿态轨迹在姿态过渡终点处的姿态
Figure BDA00039534820900001114
一阶导数
Figure BDA00039534820900001115
和二阶导数
Figure BDA00039534820900001116
如下：
Figure BDA00039534820900001117
Figure BDA00039534820900001118
Figure BDA00039534820900001119
步骤S8、建立第i段姿态轨迹过渡段的参数表达式qi(u)；如下：
Figure BDA0003953482090000121
其中，Bj,3(u),j＝0,1,2,3,4的求取如式(12)所示，将式(13)-式(18)的N0,3(u)、N1,3(u)、N2,3(u)、N3,3(u)和N4,3(u)代入式(12)依次求得B0,3(u)、B1,3(u)、B2,3(u)、B3,3(u)和B4,3(u)；
Figure BDA0003953482090000122
和
Figure BDA0003953482090000123
是作为姿态轨迹过渡段的六个姿态控制点的姿态；
Figure BDA0003953482090000124
Figure BDA0003953482090000125
Figure BDA0003953482090000126
Figure BDA0003953482090000127
Figure BDA0003953482090000128
Figure BDA0003953482090000131
Figure BDA0003953482090000132
根据qi(u)求得qi(0)如式(19)所示，qi(1)如式(20)所示，接着根据qi(u)求得一阶导数
Figure BDA0003953482090000133
如式(21)所示，
Figure BDA0003953482090000134
如式(22)所示，然后求得二阶导数
Figure BDA0003953482090000135
如式(23)所示，
Figure BDA0003953482090000136
如式(24)所示；
Figure BDA0003953482090000137
Figure BDA0003953482090000138
Figure BDA0003953482090000139
Figure BDA00039534820900001310
Figure BDA00039534820900001311
Figure BDA00039534820900001312
步骤S9、根据两条姿态曲线在曲线拼接实现曲率连续需要满足的条件建立方程组，并求取姿态控制点的姿态后，代入第i段姿态轨迹过渡段的参数表达式qi(u)；
其中，根据两条姿态曲线在曲线拼接实现曲率连续需要满足的条件建立方程组，如下：
Figure BDA0003953482090000141
代入公式(19)-公式(24)可得公式(26)，如下：
Figure BDA0003953482090000142
其中，α和β均为大于零的任意常数；
根据公式(26)求得姿态控制点
Figure BDA0003953482090000143
和
Figure BDA0003953482090000144
的姿态后，代入公式(11)。
由式(26)可以看出姿态控制点
Figure BDA0003953482090000145
和
Figure BDA0003953482090000146
的求取不受参数α和β的影响，其中，所述根据公式(26)求得姿态控制点
Figure BDA0003953482090000147
和
Figure BDA0003953482090000148
的姿态，包括：
根据公式(26)求取公式(27)，接着根据求出的姿态控制点
Figure BDA0003953482090000149
和
Figure BDA00039534820900001410
以及参数α和β代入式(28)求得姿态控制点
Figure BDA00039534820900001411
和
Figure BDA00039534820900001412
最后再将求出的姿态控制点
Figure BDA00039534820900001413
Figure BDA00039534820900001414
和
Figure BDA00039534820900001415
以及参数α和β代入式(29)求得姿态控制点
Figure BDA00039534820900001416
和
Figure BDA00039534820900001417
Figure BDA00039534820900001418
Figure BDA0003953482090000151
Figure BDA0003953482090000152
α和β可以为大于零的任意常数，所以存在无数组满足条件的姿态控制点；引入参数λ代替参数α和β，参数λ与参数α和β的关系如式(30)所示；
Figure BDA0003953482090000153
将姿态轨迹过渡规划算法在MATLAB软件中进行仿真实验，通过姿态轨迹的可视化处理来更加直观地比较参数λ取不同值时所构造的姿态轨迹过渡段的过渡效果；λ为0.18时所构造的姿态轨迹过渡段实现了与相邻姿态轨迹的平滑衔接，过渡效果满足预期目标，可以有效提高六轴串联工业机器人末端的运行效率；最终选择令λ＝0.18来求取合适的姿态控制点，获得姿态轨迹过渡段的参数表达式。
步骤S10、计算第i段姿态轨迹过渡段的角位移长度
Figure BDA0003953482090000154
可选地，步骤S10包括：
采用复化六点高斯勒让德积分法来求取第i段姿态轨迹过渡段的角位移长度
Figure BDA0003953482090000155
如式(31)：
Figure BDA0003953482090000156
其中，xk为积分节点，Ak为积分系数，具体的数值如式(32)：
Figure BDA0003953482090000161
步骤S11、根据第i段姿态轨迹过渡段的角位移长度
Figure BDA0003953482090000162
进行速度规划，获得角位移长度与时间的函数关系，从而根据插补点的时刻得到插补点在姿态轨迹过渡段上的相对角位移长度，接着根据插补点在姿态轨迹过渡段上的相对角位移长度反解出插补点对应的参数值，最后将插补点对应的参数值代入姿态轨迹过渡段的参数表达式获得插补点的姿态；可选地，步骤S11包括：
S111、给定插补点在第i段姿态轨迹过渡段上沿着超球面上的轨迹方向距离第i段姿态轨迹过渡段的姿态过渡起点的相对角位移长度θl，
Figure BDA0003953482090000163
S112、根据相对角位移长度θl和各区间求取的角位移长度找出插补点所在区间的段号m，m＝1,2,···,6，从而获得牛顿迭代法的搜索区间为
Figure BDA0003953482090000164
并根据式(33)对相对角位移长度θl进行修正；
Figure BDA0003953482090000165
S113、令牛顿迭代法的迭代初值ul＝(m-1)/6；
S114、构造如式(34)所示的以ul为自变量的误差函数f(ul)，代入ul的值计算f(ul)的值；
Figure BDA0003953482090000166
其中，在计算误差函数f(ul)的值时也需要进行积分求取，采用六点高斯勒让德积分法进行积分求取，f(ul)的求取公式如式(35)所示。
Figure BDA0003953482090000167
其中，xk为积分节点，Ak为积分系数，具体的数值如式(32)所示；
S115、判断f(ul)的绝对值大于0.00001是否成立，成立则跳转至步骤S6，不成立则跳转至步骤(8)；
S116、对ul进行迭代，迭代公式如式(36)所示；
Figure BDA0003953482090000171
S117、将迭代后的ul的值代入式(35)求得f(ul)的值，跳转至步骤S5；
S118、令u＝ul代入第i段姿态轨迹过渡段的公式(11)，从而获得插补点的姿态ql＝qi(u)。
步骤S12、令i＝i+1；判断i小于N-2是否成立，成立则执行步骤S2，不成立则执行步骤S14；
步骤S13、通过运动学逆解求得六轴串联工业机器人各关节转动的角度以驱动六轴串联工业机器人转动，从而实现六轴串联工业机器人末端的姿态轨迹过渡。
在其中一实施例中，为了分析参数λ的数值对姿态轨迹过渡段的影响，需要对姿态轨迹进行可视化处理，建立超球面表面的单位四元数与单位球面上点的映射关系，从而将超球面表面的用单位四元数描述的姿态轨迹投影到单位球面，更加直观的分析姿态轨迹过渡段的过渡效果。对姿态轨迹进行可视化处理需要先给定初始点位px，px可以为单位球面任意的点，对初始点位px施加旋转变换q后获得的点位pn就是旋转变化q对应的单位四元数投影在单位球面上的点。令px为纯四元数qx＝[0,px]，与四元数qn存在如式(37)所示的映射关系，而四元数qn的虚数部分即为pn。
qn＝q×qx×q-1 (37)
将姿态轨迹过渡规划算法在MATLAB软件中进行仿真实验，通过姿态轨迹的可视化处理来更加直观地比较参数λ取不同值时所构造的姿态轨迹过渡段的过渡效果。图3中图(a)、(b)、(c)、(d)分别代表λ取0.05、0.18、0.3和0.5时的姿态轨迹过渡示意图。
由图3中(a)可以看出，λ接近于0时，姿态轨迹在姿态过渡起止点处仍存在进行轨迹突变的情况，过渡效果不理想，仍然会导致六轴串联工业机器人运行效率的降低。如图3中(b)所示，λ为0.18时所构造的姿态轨迹过渡段实现了与相邻姿态轨迹的平滑衔接，过渡效果满足预期目标，可以有效提高六轴串联工业机器人的运行效率。在图3中(c)中，λ取0.3，可以看出姿态轨迹过渡段在接近姿态原始轨迹的部分仍存在进行轨迹突变的情况，姿态过渡效果不满足预期，仍会导致六轴串联工业机器人运行效率的降低。通过图3中(d)可以看出当λ＝0.5时的姿态轨迹过渡段存在轨迹环绕的问题，不符合六轴串联工业机器人的姿态过渡需求，且会导致六轴串联工业机器人运行效率的降低。通过分析和对比λ取0.05、0.18、0.3和0.5时的姿态轨迹过渡的效果，最终选择令λ＝0.18来求取合适的姿态控制点，获得姿态轨迹过渡段的参数表达式。
本发明采用四元数描述姿态，利用了三次四元数准均匀B样条曲线作为姿态轨迹过渡段，实现姿态轨迹的平滑过渡。简化了四元数样条曲线控制点的求取方式，降低了算法的实施难度。姿态轨迹过渡段的角位移长度求取采用了复化六点高斯勒让德积分法，插补计算利用了牛顿迭代法结合六点高斯勒让德积分法的方式。从而驱动六轴串联工业机器人末端在整个平滑的路径上连续运动，获得了平滑的运动轨迹，避免了频繁的加减速，提高了六轴串联工业机器人的运行效率，满足了生产实际的工艺需求。
以上实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。

Claims (10)

1.一种六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，包括以下步骤：
S1、令i＝1，N为给定运动路径的总段数；
S2、在由姿态示教点Qi和Qi+1构成的球面线性插补轨迹和由姿态示教点Qi+1和Qi+2构成的球面线性插补轨迹之间进行平滑过渡；其中，利用三次四元数准均匀B样条曲线作为姿态轨迹过渡段；姿态示教点Qi和Qi+1分别为第i段姿态轨迹的起始姿态点和终止姿态点；姿态示教点Qi+1和Qi+2分别为第i+1段姿态轨迹的起始姿态点和终止姿态点；
S3、根据第i段姿态轨迹的姿态示教起始姿态点Qi和终止姿态点Qi+1求取第i段姿态轨迹的角位移长度θi和参数Q′i(u)；参数Q′i(u)的表达式如下：
Figure FDA0003953482080000011
S4、根据第i+1段姿态轨迹的姿态示教起始姿态点Qi+1和终止姿态点Qi+2求取第i+1段姿态轨迹的角位移长度θi+1和参数Q′i+1(u)；参数Q′i+1(u)的表达式如下：
Figure FDA0003953482080000012
S5、根据第i段姿态轨迹的角位移长度θi、第i+1段姿态轨迹的角位移长度θi+1和给定姿态过渡半径
Figure FDA0003953482080000013
求取实际姿态过渡半径
Figure FDA0003953482080000014
S6、根据第i段姿态轨迹的角位移长度θi和实际姿态过渡半径
Figure FDA0003953482080000015
求取第i段姿态轨迹过渡段的姿态过渡起点对应的参数值，再根据参数Q′i(u)和导数的求取方式求取第i段姿态轨迹在姿态过渡起点处的姿态、一阶导数和二阶导数；
S7、根据第i+1段姿态轨迹的角位移长度θi+1和实际姿态过渡半径
Figure FDA0003953482080000016
求取第i段姿态轨迹过渡段的姿态过渡终点对应的参数值，再根据参数Q′i+1(u)和导数的求取方式求取第i+1段姿态轨迹在姿态过渡终点处的姿态、一阶导数和二阶导数；
S8、建立第i段姿态轨迹过渡段的参数表达式qi(u)；
S9、根据两条姿态曲线在曲线拼接实现曲率连续需要满足的条件建立方程组，并求取姿态控制点的姿态后，代入第i段姿态轨迹过渡段的参数表达式qi(u)；
S10、计算第i段姿态轨迹过渡段的角位移长度
Figure FDA0003953482080000021
S11、根据第i段姿态轨迹过渡段的角位移长度
Figure FDA0003953482080000022
进行速度规划，获得角位移长度与时间的函数关系，从而根据插补点的时刻得到插补点在姿态轨迹过渡段上的相对角位移长度，接着根据插补点在姿态轨迹过渡段上的相对角位移长度反解出插补点对应的参数值，最后将插补点对应的参数值代入姿态轨迹过渡段的参数表达式获得插补点的姿态；
S12、令i＝i+1；判断i小于N-2是否成立，成立则执行步骤S2，不成立则执行步骤S14；
S13、通过运动学逆解求得六轴串联工业机器人各关节转动的角度以驱动六轴串联工业机器人转动，从而实现六轴串联工业机器人末端的姿态轨迹过渡。
2.如权利要求1所述的六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，第i段姿态轨迹的角位移长度θi，如下：
θi＝arccos(Qi,Qi+1)
第i+1段姿态轨迹的角位移长度θi+1，如下：
θi+1＝arccos(Qi+1,Qi+2)。
3.如权利要求1所述的六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，步骤S5还包括：
判断
Figure FDA0003953482080000023
或
Figure FDA0003953482080000024
是否成立，不成立则令
Figure FDA0003953482080000025
成立则再判断θi＞θi+1是否成立，成立则令
Figure FDA0003953482080000026
不成立则令
Figure FDA0003953482080000027
4.如权利要求1所述的六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，步骤S6包括：
根据第i段姿态轨迹的角位移长度θi和实际姿态过渡半径
Figure FDA0003953482080000028
求取第i段姿态轨迹过渡段的姿态过渡起点对应的参数值
Figure FDA0003953482080000029
如下：
Figure FDA00039534820800000210
再根据参数Q′i(u)和导数的求取方式求取第i段姿态轨迹在姿态过渡起点处的姿态
Figure FDA0003953482080000031
一阶导数
Figure FDA0003953482080000032
和二阶导数
Figure FDA0003953482080000033
Figure FDA0003953482080000034
Figure FDA0003953482080000035
Figure FDA0003953482080000036
5.如权利要求1所述的六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，步骤S7包括：
根据第i+1段姿态轨迹的角位移长度θi+1和实际姿态过渡半径
Figure FDA0003953482080000037
求取第i段姿态轨迹过渡段的姿态过渡终点对应的参数值
Figure FDA0003953482080000038
如下：
Figure FDA0003953482080000039
再根据参数Q′i+1(u)和导数的求取方式求取第i+1段姿态轨迹在姿态过渡终点处的姿态
Figure FDA00039534820800000310
一阶导数
Figure FDA00039534820800000311
和二阶导数
Figure FDA00039534820800000312
如下：
Figure FDA00039534820800000313
Figure FDA00039534820800000314
Figure FDA00039534820800000315
6.如权利要求1所述的六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，步骤S8中，建立第i段姿态轨迹过渡段的参数表达式qi(u)，如下：
Figure FDA00039534820800000316
其中，Bj,3(u),j＝0,1,2,3,4的求取如式(12)所示，将式(13)-式(18)的N0,3(u)、N1,3(u)、N2,3(u)、N3,3(u)和N4,3(u)代入式(12)依次求得B0,3(u)、B1,3(u)、B2,3(u)、B3,3(u)和B4,3(u)；
Figure FDA00039534820800000317
和
Figure FDA00039534820800000318
是作为姿态轨迹过渡段的六个姿态控制点的姿态；
Figure FDA00039534820800000319
Figure FDA0003953482080000041
Figure FDA0003953482080000042
Figure FDA0003953482080000043
Figure FDA0003953482080000044
Figure FDA0003953482080000045
Figure FDA0003953482080000046
根据qi(u)求得qi(0)如式(19)所示，qi(1)如式(20)所示，接着根据qi(u)求得一阶导数
Figure FDA0003953482080000047
如式(21)所示，
Figure FDA0003953482080000048
如式(22)所示，然后求得二阶导数
Figure FDA0003953482080000049
如式(23)所示，
Figure FDA00039534820800000410
如式(24)所示；
Figure FDA00039534820800000411
Figure FDA0003953482080000051
Figure FDA0003953482080000052
Figure FDA0003953482080000053
Figure FDA0003953482080000054
Figure FDA0003953482080000055
7.如权利要求6所述的六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，步骤S9包括：根据两条姿态曲线在曲线拼接实现曲率连续需要满足的条件建立方程组，如下：
Figure FDA0003953482080000056
代入公式(19)-公式(24)可得公式(26)，如下：
Figure FDA0003953482080000057
其中，α和β均为大于零的任意常数；
根据公式(26)求得姿态控制点
Figure FDA0003953482080000061
和
Figure FDA0003953482080000062
的姿态后，代入公式(11)。
8.如权利要求7所述的六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，所述根据公式(26)求得姿态控制点
Figure FDA0003953482080000063
和
Figure FDA0003953482080000064
的姿态，包括：
根据公式(26)求取公式(27)，接着根据求出的姿态控制点
Figure FDA0003953482080000065
和
Figure FDA0003953482080000066
以及参数α和β代入式(28)求得姿态控制点
Figure FDA0003953482080000067
和
Figure FDA0003953482080000068
最后再将求出的姿态控制点
Figure FDA0003953482080000069
Figure FDA00039534820800000610
和
Figure FDA00039534820800000611
以及参数α和β代入式(29)求得姿态控制点
Figure FDA00039534820800000612
和
Figure FDA00039534820800000613
Figure FDA00039534820800000614
Figure FDA00039534820800000615
Figure FDA00039534820800000616
α和β可以为大于零的任意常数，所以存在无数组满足条件的姿态控制点；引入参数λ代替参数α和β，参数λ与参数α和β的关系如式(30)所示；
Figure FDA00039534820800000617
9.如权利要求8所述的六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，步骤S10包括：
采用复化六点高斯勒让德积分法来求取第i段姿态轨迹过渡段的角位移长度
Figure FDA00039534820800000618
如式(31)：
Figure FDA0003953482080000071
其中，xk为积分节点，Ak为积分系数，具体的数值如式(32)：
Figure FDA0003953482080000072
10.如权利要求9所述的六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，步骤S11包括：
S111、给定插补点在第i段姿态轨迹过渡段上沿着超球面上的轨迹方向距离第i段姿态轨迹过渡段的姿态过渡起点的相对角位移长度θl，
Figure FDA0003953482080000076
S112、根据相对角位移长度θl和各区间求取的角位移长度找出插补点所在区间的段号m，m＝1,2,···,6，从而获得牛顿迭代法的搜索区间为
Figure FDA0003953482080000073
并根据式(33)对相对角位移长度θl进行修正；
Figure FDA0003953482080000074
S113、令牛顿迭代法的迭代初值ul＝(m-1)/6；
S114、构造如式(34)所示的以ul为自变量的误差函数f(ul)，代入ul的值计算f(ul)的值；
Figure FDA0003953482080000075
其中，在计算误差函数f(ul)的值时也需要进行积分求取，采用六点高斯勒让德积分法进行积分求取，f(ul)的求取公式如式(35)所示。
Figure FDA0003953482080000081
其中，xk为积分节点，Ak为积分系数，具体的数值如式(32)所示；
S115、判断f(ul)的绝对值大于0.00001是否成立，成立则跳转至步骤S6，不成立则跳转至步骤(8)；
S116、对ul进行迭代，迭代公式如式(36)所示；
Figure FDA0003953482080000082
S117、将迭代后的ul的值代入式(35)求得f(ul)的值，跳转至步骤S5；
S118、令u＝ul代入第i段姿态轨迹过渡段的公式(11)，从而获得插补点的姿态ql＝qi(u)。
CN202211456525.7A 2022-11-21 2022-11-21 一种六轴串联工业机器人末端的姿态轨迹过渡规划方法 Pending CN115741695A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211456525.7A CN115741695A (zh) 2022-11-21 2022-11-21 一种六轴串联工业机器人末端的姿态轨迹过渡规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211456525.7A CN115741695A (zh) 2022-11-21 2022-11-21 一种六轴串联工业机器人末端的姿态轨迹过渡规划方法

Publications (1)

Publication Number Publication Date
CN115741695A true CN115741695A (zh) 2023-03-07

Family

ID=85333656

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211456525.7A Pending CN115741695A (zh) 2022-11-21 2022-11-21 一种六轴串联工业机器人末端的姿态轨迹过渡规划方法

Country Status (1)

Country Link
CN (1) CN115741695A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116330286A (zh) * 2023-03-28 2023-06-27 重庆智能机器人研究院 一种通过工业机器人动态轨迹和姿态规划抑制抖动的方法
CN116422551A (zh) * 2023-04-13 2023-07-14 盛景智能科技(嘉兴)有限公司 异型工件喷涂方法、装置、设备、存储介质及机器人
CN116985136A (zh) * 2023-08-16 2023-11-03 睿尔曼智能科技（北京）有限公司 基于四元数的机械臂节点姿态速度前瞻控制方法及装置

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116330286A (zh) * 2023-03-28 2023-06-27 重庆智能机器人研究院 一种通过工业机器人动态轨迹和姿态规划抑制抖动的方法
CN116330286B (zh) * 2023-03-28 2023-11-07 重庆智能机器人研究院 一种通过工业机器人动态轨迹和姿态规划抑制抖动的方法
CN116422551A (zh) * 2023-04-13 2023-07-14 盛景智能科技(嘉兴)有限公司 异型工件喷涂方法、装置、设备、存储介质及机器人
CN116985136A (zh) * 2023-08-16 2023-11-03 睿尔曼智能科技（北京）有限公司 基于四元数的机械臂节点姿态速度前瞻控制方法及装置
CN116985136B (zh) * 2023-08-16 2024-02-09 睿尔曼智能科技（北京）有限公司 基于四元数的机械臂节点姿态速度前瞻控制方法及装置

Similar Documents

Publication Publication Date Title
CN115741695A (zh) 一种六轴串联工业机器人末端的姿态轨迹过渡规划方法
WO2018133439A1 (zh) 一种考虑末端运动误差的六自由度机器人轨迹规划方法
US6845295B2 (en) Method of controlling a robot through a singularity
CN112757306A (zh) 一种机械臂逆解多解选择和时间最优轨迹规划算法
JP4273335B2 (ja) ロボットアーム
CN108568817B (zh) 一种基于贝塞尔曲线的Delta机器人轨迹连接控制方法
CN107756400B (zh) 一种基于旋量理论的6r机器人逆运动学几何求解方法
WO2019041657A1 (zh) 一种工业机器人轨迹五次多项式规划方法
CN113601512B (zh) 一种机械臂奇异点的通用规避方法与系统
CN109834706B (zh) 在机器人运动规划中避免运动学奇异点的方法及装置
CN113146600B (zh) 基于运动学迭代学习控制的柔性机器人轨迹规划方法及装置
CN106313047A (zh) 一种基于Bezier样条的机器人实时拐角过渡方法
TWI704039B (zh) 機械臂奇異點控制方法及系統
CN109623825B (zh) 一种移动轨迹规划方法、装置、设备和存储介质
CN112506199B (zh) 一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法
CN107538493A (zh) 一种避免线缆干涉约束的工业机器人轨迹规划方法
CN110561419B (zh) 臂型线约束柔性机器人轨迹规划方法及装置
CN107038275B (zh) 一种机械臂误差分析方法
CN106844951B (zh) 基于分段几何法求解超冗余机器人逆运动学的方法及系统
CN111309002A (zh) 一种基于矢量的轮式移动机器人避障方法及系统
CN111123943A (zh) 一种基于伪逆约束的超冗余机器人轨迹规划方法及系统
CN113334385A (zh) 一种自驱动关节臂测量机直线轨迹间平滑过渡的规划方法
CN105563482A (zh) 用于工业机器人的末端执行器的旋转运动规划方法
US20220105627A1 (en) Singularity-free kinematic parameterization of soft robot manipulators
CN116117796B (zh) 一种工业机器人姿态轨迹过渡与速度规划方法及系统

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