CN115870974A - 基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法 - Google Patents

基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法 Download PDF

Info

Publication number
CN115870974A
CN115870974A CN202211423987.9A CN202211423987A CN115870974A CN 115870974 A CN115870974 A CN 115870974A CN 202211423987 A CN202211423987 A CN 202211423987A CN 115870974 A CN115870974 A CN 115870974A
Authority
CN
China
Prior art keywords
scalar
acceleration
track
mvc
time optimal
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.)
Granted
Application number
CN202211423987.9A
Other languages
English (en)
Other versions
CN115870974B (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.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
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 South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202211423987.9A priority Critical patent/CN115870974B/zh
Publication of CN115870974A publication Critical patent/CN115870974A/zh
Application granted granted Critical
Publication of CN115870974B publication Critical patent/CN115870974B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Numerical Control (AREA)

Abstract

本发明公开了一种基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，此方法先将获取的任务轨迹转化成参数路径轨迹，然后设置离散步长组，以将参数路径轨迹进行离散化；再依次计算时间最优轨迹规划，并对轨迹进行相似度比较，直到相似度比较结果大于相似度阈值；接着对后结轨迹进行插值，并判断加速度是否超过插值轨迹加速度限制；基于判断结果，先择不超限的轨迹，计算得到每个时刻各个轴的角位移；最后将各轴角位移转换为控制指令脉冲信号。本发明减少了时间最优轨迹的时间与轨迹规划的计算时间，得到了更平滑的时间最优轨迹。

Description

基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法
技术领域
本发明涉及多轴运动系统领域的技术，具体涉及一种基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法及推算系统。
背景技术
多轴运动系统得到广泛的应用，其结构包括并联结构与串联结构，轴的移动方式包括转动和移动两种。
对于时间最优轨迹规划，运用数值积分方法进行时间最优轨迹规划，减少了计算量，缩短了计算时间，其将动力学模型用参数方程表示，即将电机力矩约束转化成参数化加速度的约束，运用数值积分法寻找时间最优解决方案，使电机工作在临界饱和力矩的状态下，最大化运行速度，从而最小化运行时间。大多数数值积分算法是将给定轨迹用参数函数表示，并在相平面进行时间最优轨迹规划。在规划时，需要遍历轨迹，在约束条件下计算每一个点的相平面最大标量速度、标量加速度与实际速度，因此需要多次使用动力学模型，耗费较多规划计算时间。且对于一条确定的轨迹，其中许多轨迹点是不重要的，因此会产生不必要的计算，导致耗费更多的规划计算时间。
对于时间最优平滑轨迹规划，可在规划过程中设置动力学约束，如力矩变化率约束，或设置加运动学约束，如加速度约束，或同时设置动力学和运动学约束。在规划得到时间最优平滑轨迹之后，需要根据控制周期对轨迹进行插值，可利用多项式函数对轨迹每一个区间进行插值，得到分段多项式轨迹。对于轨迹平滑，加速度连续是必不可少的。当轨迹离散点较多，子区间较小时，分段多项式中间的点速度或是加速度并不一定满足给定的约束。
发明内容
本发明的目的是为了克服以上现有技术存在的不足，提供了一种基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法。此基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法在保证得到时间最优平滑轨迹的情况下，可减少规划时间。
本发明的目的通过以下的技术方案实现：本基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，包括以下步骤：
S1、获取任务轨迹，并将任务轨迹转化为参数路径轨迹；
S2、设置离散步长组DSL(n)＝0.1/2nm，相似度阈值为X，插值轨迹加速度限制为Y，其中，m为单位米，n＝0，1，2，…，10；基于离散步长组对参数路径轨迹进行离散化；
S3、对多轴运动系统进行动力学建模，再建立时间最优数学模型，计算出相平面
Figure BDA0003944062620000021
最大速度限制曲线，利用类数值积分算法，计算时间最优轨迹；
S4、对时间最优轨迹进行相似度比较，是在利用类数值积分时间最优轨迹规划算法进行轨迹规划后，利用余弦相似度比较算法，对相邻离散步长对应的相平面时间最优轨迹中的标量速度数据进行相似度比较，选取出相似度高的步长x；
S5、重复步骤S3-S4，直到相似度比较结果大于相似度阈值，即x＞X；
S6、对后续轨迹进行插值，利用分段五次多项式对后续离散步长对应的轨迹进行插值，以计算得到后续轨迹的最大加速度y，并判断最大加速度y是否大于插值轨迹加速度限制Y；
S7、若y＞Y，计算得到每个时刻各个轴的角位移；
S8、将各轴角位移转换为控制指令脉冲信号。
优选的，步骤S1中，将任务轨迹转化为参数路径轨迹的具体过程为：
根据路径轨迹位移的总长度，将路径轨迹归一化，用标量参数s描述多轴运动系统末端任务轨迹的标量位移，得到参数路径轨迹。
优选的，步骤S3的具体步骤如下：
S31、对单轴动力学结构模型进行分析建模，得
Figure BDA0003944062620000031
其中，Jm为电机转动惯量，Jc1为电机端联轴器的转动惯量，Jc2为丝杠端联轴器的转动惯量，Js为丝杆转动惯量，m1为滚珠丝杠螺母的质量，m2为工作台的质量，klb和krb分别为左端和右端轴承的径向刚度，klg为工作台与线性导轨的接触面刚度。Clg为工作台与线性导轨接触面的阻尼系数，fc为工作台与线性导轨之间的库伦摩擦系数，符号(·)表示对时间t的导数；qm为电机输出轴转角，qs为滚珠丝杆转角，qn为丝杠螺母的相对转角，vb为丝杠整体因轴承而产生的径向振动位移，v为与线性导轨接触的工作台产生垂直方向的振动位移，u为与滚珠丝杠螺母固定连接的工作台产生轴向位移；
用qm代替qs和qn，u可用qn表示，qn＝2πu/ph，ph为丝杠导程，对式(1)进行简化得：
τm＝Hb·β，(2)
其中，
Figure BDA0003944062620000032
β＝[M fc fv C]T，/>
Figure BDA0003944062620000037
M＝Jm+Jc1+Jc2+Js+ph(m1+m2)/2π，/>
Figure BDA0003944062620000034
其中，C为常数项；
将qm视为关于标量参数s的函数q(s)，并采用分段最小乘拟合方法，分段拟合q(s)，得
q(s)＝a0+a1s+…+aksk，(3)
其中，a0,…,ak是各关节拟合的表达式系数矩阵，k是用于最小二乘拟合的表达式阶数；
电机角速度
Figure BDA0003944062620000035
和角加速度/>
Figure BDA0003944062620000036
转化为标量参数s的函数，得：
Figure BDA0003944062620000041
Figure BDA0003944062620000042
/>
其中，
Figure BDA0003944062620000043
将式(3)-(5)代入式(2)，可得多轴运动系统的动力学模型：
Figure BDA0003944062620000044
其中，m(s)＝M·q′(s),v(s)＝M·q″(s),f(s)＝fv·q′(s),c(s)＝fc·sign[q′(s)]+C；
S32、对于给定速度约束
Figure BDA0003944062620000045
和/>
Figure BDA0003944062620000046
加速度约束/>
Figure BDA0003944062620000047
和/>
Figure BDA0003944062620000048
力矩约束τmax和τmin，可得约束不等式：
Figure BDA0003944062620000049
时间最优轨迹的目标是使轨迹运行时间最小，则优化目标函数设置为：
Figure BDA00039440626200000410
类数值积分时间最优轨迹的规划算法是在相平面
Figure BDA00039440626200000411
上进行轨迹规划，将微元dt换成ds，得：
Figure BDA00039440626200000412
进行规划时，对于离散点数为N的轨迹，优化目标函数最终为：
Figure BDA00039440626200000413
在相平面
Figure BDA00039440626200000414
中，s(0)＝0,s(T)＝1，且在时间最优轨迹起始点必有
Figure BDA00039440626200000415
时间最优轨迹的离散点数为N，则可得相平面/>
Figure BDA00039440626200000416
中对于起始点的限制为：
Figure BDA0003944062620000051
联立式(4)-(7)、(10)及(11)，可得时间最优轨迹的数学模型为：
Figure BDA0003944062620000052
其中，每个离散点处电机转角相对于标量位移s的一阶导q′(sk)和二阶导q″(sk)可根据式(3)对s求导得到；
S33、在进行时间最优轨迹规划前，需要计算出相平面
Figure BDA0003944062620000053
最大速度限制曲线，由式(12)可得，对于离散后的相平面轨迹，在离散点sk处有：
Figure BDA0003944062620000054
Figure BDA0003944062620000055
在相平面
Figure BDA0003944062620000056
上，由式(13)和(14)，可得在离散点sk处，其中k＝1，2，……,N；X轴和Z轴各自的关节角加速度约束/>
Figure BDA0003944062620000057
和关节力矩约束(τmin,τmax)构成标量加速度/>
Figure BDA0003944062620000058
可行区域，在可行域内，离散点sk的最大标量加速度/>
Figure BDA0003944062620000059
与最小标量加速度/>
Figure BDA00039440626200000510
分别为：
Figure BDA00039440626200000511
Figure BDA00039440626200000512
当
Figure BDA00039440626200000513
时，/>
Figure BDA00039440626200000514
或/>
Figure BDA00039440626200000515
取合适步长，从/>
Figure BDA00039440626200000516
或/>
Figure BDA00039440626200000517
点处开始，根据所选步长沿着/>
Figure BDA0003944062620000061
轴向前搜索，计算此时/>
Figure BDA0003944062620000062
处对应的/>
Figure BDA0003944062620000063
与/>
Figure BDA0003944062620000064
当
Figure BDA0003944062620000065
时，说明此时/>
Figure BDA0003944062620000066
位于区间/>
Figure BDA0003944062620000067
内，停止搜索，利用黄金分割法在
Figure BDA0003944062620000068
中搜索得到满足精度要求的最大速度/>
Figure BDA0003944062620000069
遍历轨迹每一个离散点sk，k＝1，2，……N，得到一系列
Figure BDA00039440626200000610
数据，构成相平面/>
Figure BDA00039440626200000611
上满足关节加速度约束与关节力矩约束的最大速度限制曲线MVCacc&tor；/>
再考虑关节角速度约束
Figure BDA00039440626200000612
Figure BDA00039440626200000613
由式(17)及X轴Z轴各自的关节角速度约束，可得速度约束下的最大速度限制曲线MVCvel＝min(MVCXvel,MVCZvel)，最终得到满足速度，加速度，力矩约束的最大速度限制曲线MVC＝min(MVCacc&tor,MVCvel)；
S34、假设相邻离散点之间的运动为匀加速运动，即在规划时，当向前计算
Figure BDA00039440626200000614
时，假设sk-1到sk间以在sk-1处的最大标量加速度/>
Figure BDA00039440626200000615
作匀加速运动,由匀加速运动方程：
Figure BDA00039440626200000616
可得：
Figure BDA00039440626200000617
其中，
Figure BDA00039440626200000618
当向后计算
Figure BDA00039440626200000619
时，假设sk+1到sk间以在sk+1处的最小标量加速度/>
Figure BDA00039440626200000620
作匀加速运动，由匀加速运动方程
Figure BDA00039440626200000621
可得：
Figure BDA00039440626200000622
其中，
Figure BDA0003944062620000071
为了设置转换点，用g(s)来表示MVC中s与
Figure BDA0003944062620000072
一一对应的关系，并构造表达式如下：
Figure BDA0003944062620000073
其中，
Figure BDA0003944062620000074
或/>
Figure BDA0003944062620000075
Figure BDA0003944062620000076
表示MVC的斜率，对于离散的轨迹，/>
Figure BDA0003944062620000077
当沿着MVC向前搜索时，若k(sk-1)·k(sk)<0，则将sk处对应的g(sk)设置为转换点；
S35、在相平面
Figure BDA0003944062620000078
上求出MVC，与对应的标量位移s，最大标量加速度/>
Figure BDA0003944062620000079
最小标量加速度/>
Figure BDA00039440626200000710
/>
S36、在起始点
Figure BDA00039440626200000711
处，利用式(19)以最大标量加速度/>
Figure BDA00039440626200000712
向前计算标量速度/>
Figure BDA00039440626200000713
定义用/>
Figure BDA00039440626200000714
向前计算的得到曲线为α-profile；
若α-profile与MVC相交，即
Figure BDA00039440626200000715
从α-profile与MVC相交的交点开始，沿着MVC向前搜索转换点，定义搜索得到部分MVC曲线为γ-profile；
若α-profile与s＝sN相交，即sk＝sN，从终点
Figure BDA00039440626200000716
以最小标量加速度
Figure BDA00039440626200000717
向后计算标量速度/>
Figure BDA00039440626200000718
直到与已规划曲线相交，定义用最小加速度向后计算的曲线为β-profile；
S37、从转换点处，利用式(21)以最小标量加速度
Figure BDA00039440626200000719
向后计算标量速度/>
Figure BDA00039440626200000720
直到与已规划的曲线相交；同时从该转换点以最大标量加速度/>
Figure BDA00039440626200000721
向前计算标量速度/>
Figure BDA00039440626200000722
若规划的曲线与MVC相交，、从α-profile与MVC相交的交点开始，沿着MVC向前搜索转换点。定义搜索得到部分MVC曲线为γ-profile；
若规划的曲线与s＝sN相交，从终点
Figure BDA0003944062620000081
以最小标量加速度/>
Figure BDA0003944062620000082
向后计算标量速度/>
Figure BDA0003944062620000083
直到与已规划的曲线相交，定义用最小加速度向后计算的曲线为β-profile。
S38、最终按顺序将得到的α-profile、β-profile、γ-profile和δ-profile连接构成完整的最优轨迹，即可得到时间最优轨迹。
优选的，在步骤S33中，在相平面
Figure BDA0003944062620000084
中，MVC上的区域为不满足约束条件的不可行域，MVC本身及其下方的区域为满足约束条件的可行域，时间最优轨迹在可行域内进行规划。
优选的，步骤S4的具体步骤如下：
S41、对给定轨迹进行时间最优轨迹规划后，可得到数据集
Figure BDA0003944062620000085
由两组一一对应的数据集组成，即标量位移数据集D(s)和标量速度数据集/>
Figure BDA0003944062620000086
其中，/>
Figure BDA0003944062620000087
为相平面时间最优轨迹；对前后离散步长进行规划后分别得到D1(s)与/>
Figure BDA0003944062620000088
D2(s)与/>
Figure BDA0003944062620000089
根据前后离散化步长对应的标量位移数据集D1(s)与D2(s)之间的关系，将较大的离散步长对应的标量速度数据集/>
Figure BDA00039440626200000810
运用直线插补方法，插补至与较小离散步长对应的标量速度数据集/>
Figure BDA00039440626200000811
令标量速度数据集/>
Figure BDA00039440626200000812
和标量速度数据集/>
Figure BDA00039440626200000813
具有相同数据长度；
S42、采用余弦相似度比较算法，度量两条时间最优轨迹相似度，假设相邻离散步长对应时间最优轨迹的速度数据集分别为Tra1与Tra2，经过上述直线插补处理后，Tra1与Tra2具有相同的维数，则其相似度为：
Figure BDA00039440626200000814
利用离散步长数据组0.1/2nm，对时间最优轨迹进行重新离散化，将相邻离散化步长对应的规划得到的时间最优轨迹，插补到相同数据长度后，即相同向量维数，运用式(23)进行余弦相似度比较，以得相似度比较结果x。
优选的，步骤S6的具体步骤如下：：
S61、对给定轨迹进行时间最优轨迹规划后，得到了一一对应的标量位移数据集D(s)，标量速度数据集
Figure BDA0003944062620000091
和标量加速度数据集/>
Figure BDA0003944062620000092
根据轨迹数据中相邻点的标量速度
Figure BDA0003944062620000093
与标量加速度/>
Figure BDA0003944062620000094
计算五次多项式各个系数，再根据相邻点之间的轨迹运行时间及其对应五次多项式，以伺服驱动器的控制周期，对标量位移s数据的各个子区间进行插值，得到了分段五次多项式插值轨迹，表达式如下：
s(t)＝c0+c1t+c2t2+c3t3+c4t4+c5t5，(24)
其中，c0,…,c5为五次多项式表达式系数矩阵；
S62、由台达伺服驱动器的加减速时间设定，设置插值轨迹加速度限制Ym/s2，将式(24)分段五次多项式插值轨迹s(t)对t求一阶导和二阶导得到每一个点的标量速度
Figure BDA0003944062620000095
与标量加速度/>
Figure BDA0003944062620000096
再对式(3)关节角度关于s的函数q(s)对s求一阶导和二阶导得到速度q′(sk)与加速度q″(sk)，由式(13)，可得：
Figure BDA0003944062620000097
又
Figure BDA0003944062620000098
其中，ph为导程，在设置加速度限制Y后，
Figure BDA0003944062620000099
联立式(25)和(26)，可得
Figure BDA00039440626200000910
由式(27)可判断轨迹每一点的加速度是否在限制以内，由此选取出加速度不超限的轨迹。
S63、根据相平面速度
Figure BDA00039440626200000911
相平面加速度/>
Figure BDA00039440626200000912
与控制周期，利用五次多项式对D2(s)进行插值得到D'2(s)，并对t求二阶导，计算得到最大加速度y。
优选的，在步骤S63之后，还包括以下步骤：
S64、若y<Y，令n＝n+1，计算n对应离散化步长DSL(n)的相平面时间最优曲线
Figure BDA0003944062620000101
并且转到步骤S53。
优选的，步骤S7包括以下步骤：
输出n-1对应离散化步长DSL(n-1)的用五次样条曲线插补得到的D'2(s)，利用分段最小二乘拟合法将插值得到的D'2(s)数据转到关节空间。
本发明相对于现有技术具有如下的优点：
本发明采用了类数值积分相平面时间最优轨迹规划算法，设置了一组逐渐细化的离散步长，利用余弦相似度比较算法，对相邻离散步长对应相平面时间最优轨迹进行相似度比较，选取出相似度高的一组步长，再利用五次多项式对该组步长对应轨迹进行分段插值，设置加速度限制，根据分段多项式轨迹的加速度，在该组步长中选出不超限的轨迹，最终得到时间最优平滑轨迹，减少了规划时间。
附图说明
图1是本发明实施例的基于余弦相似度比较的多轴运动系统的时间最优平滑轨迹规划方法流程示意图。
图2是本发明实施例的相平面类数值积分时间最优轨迹规划示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步说明。
如图1所示的一种基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，包括以下步骤：
S1、获取任务轨迹，并将任务轨迹转化为参数路径轨迹；具体的，步骤S1中，将任务轨迹转化为参数路径轨迹的具体过程为：
根据路径轨迹位移的总长度，将路径轨迹归一化，用标量参数s描述多轴运动系统末端任务轨迹的标量位移，得到参数路径轨迹。规定时间t，轨迹起始点t＝0,s(0)＝0，结束点t＝T，s(T)＝1。在时间t时，对s(t)有s(0)＝0 s(t)1s(T#)。
S2、设置离散步长组DSL(n)＝0.1/2nm，n＝0，1，2，…，10；即设置一组首项为0.1m，公比为1/2的等比数列，相似度阈值为X，插值轨迹加速度限制为Y，其中，m为单位米，基于离散步长组对参数路径轨迹进行离散化；该离散步长组中的步长逐渐减小，根据该离散步长组对参数路径轨迹进行重新离散化，可得到一组离散点逐渐增多的轨迹。
S3、对多轴运动系统进行动力学建模，再建立时间最优数学模型，计算出相平面
Figure BDA0003944062620000111
最大速度限制曲线，利用类数值积分算法，计算时间最优轨迹；
步骤S3的具体步骤如下：
S31、对单轴动力学结构模型进行分析建模，得
Figure BDA0003944062620000112
其中，Jm为电机转动惯量，Jc1为电机端联轴器的转动惯量，Jc2为丝杠端联轴器的转动惯量，Js为丝杆转动惯量，m1为滚珠丝杠螺母的质量，m2为工作台的质量，klb和krb分别为左端和右端轴承的径向刚度，klg为工作台与线性导轨的接触面刚度。Clg为工作台与线性导轨接触面的阻尼系数，fc为工作台与线性导轨之间的库伦摩擦系数，符号(·)表示对时间t的导数；qm为电机输出轴转角，qs为滚珠丝杆转角，qn为丝杠螺母的相对转角，vb为丝杠整体因轴承而产生的径向振动位移，v为与线性导轨接触的工作台产生垂直方向的振动位移，u为与滚珠丝杠螺母固定连接的工作台产生轴向位移；
用qm代替qs和qn，u可用qn表示，qn＝2πu/ph，ph为丝杠导程，对式(1)进行简化得：
τm＝Hb·β，(2)
其中，
Figure BDA0003944062620000121
β＝[M fc fv C]T，/>
Figure BDA00039440626200001215
M＝Jm+Jc1+Jc2+Js+ph(m1+m2)/2π，/>
Figure BDA0003944062620000123
其中，C为常数项,即于振动位移量vb和v微小且难以测量，可用常数项C近似拟合；
进行动力学参数辨识可得到β中的参数。将qm视为关于标量参数s的函数q(s)，并采用分段最小乘拟合方法，分段拟合q(s)，得
q(s)＝a0+a1s+…+aksk，(3)
其中，a0,…,ak是各关节拟合的表达式系数矩阵，k是用于最小二乘拟合的表达式阶数；
在计算得到q(s)后，为了在相平面
Figure BDA0003944062620000124
进行规划，电机角速度/>
Figure BDA0003944062620000125
和角加速度
Figure BDA0003944062620000126
转化为标量参数s的函数，得：
Figure BDA0003944062620000127
Figure BDA0003944062620000128
其中，
Figure BDA0003944062620000129
将式(3)-(5)代入式(2)，可得多轴运动系统的动力学模型：
Figure BDA00039440626200001210
其中，m(s)＝M·q′(s),v(s)＝M·q″(s),f(s)＝fv·q′(s),c(s)＝fc·sign[q′(s)]+C；
S32、对于给定速度约束
Figure BDA00039440626200001211
和/>
Figure BDA00039440626200001212
加速度约束/>
Figure BDA00039440626200001213
和/>
Figure BDA00039440626200001214
力矩约束τmax和τmin，可得约束不等式：
Figure BDA0003944062620000131
时间最优轨迹的目标是使轨迹运行时间最小，则优化目标函数设置为：
Figure BDA0003944062620000132
类数值积分时间最优轨迹的规划算法是在相平面
Figure BDA0003944062620000133
上进行轨迹规划，将微元dt换成ds，得：
Figure BDA0003944062620000134
进行规划时，对于离散点数为N的轨迹，优化目标函数最终为：
Figure BDA0003944062620000135
在相平面
Figure BDA0003944062620000136
中，s(0)＝0,s(T)＝1，且在时间最优轨迹起始点必有
Figure BDA0003944062620000137
时间最优轨迹的离散点数为N，则可得相平面/>
Figure BDA0003944062620000138
中对于起始点的限制为：
Figure BDA0003944062620000139
联立式(4)-(7)、(10)及(11)，可得时间最优轨迹的数学模型为：
Figure BDA00039440626200001310
其中，每个离散点处电机转角相对于标量位移s的一阶导q′(sk)和二阶导q″(sk)可根据式(3)对s求导得到；
S33、在进行时间最优轨迹规划前，需要计算出相平面
Figure BDA0003944062620000141
最大速度限制曲线，由式(12)可得，对于离散后的相平面轨迹，在离散点sk处有：
Figure BDA0003944062620000142
Figure BDA0003944062620000143
在相平面
Figure BDA0003944062620000144
上，由式(13)和(14)，可得在离散点sk处，其中k＝1，2，……,N；X轴和Z轴各自的关节角加速度约束/>
Figure BDA0003944062620000145
和关节力矩约束(τmin,τmax)构成标量加速度/>
Figure BDA0003944062620000146
可行区域，在可行域内，离散点sk的最大标量加速度/>
Figure BDA0003944062620000147
与最小标量加速度/>
Figure BDA0003944062620000148
分别为：
Figure BDA0003944062620000149
Figure BDA00039440626200001410
当
Figure BDA00039440626200001411
时，/>
Figure BDA00039440626200001412
或/>
Figure BDA00039440626200001413
取合适步长，如0.1m，从/>
Figure BDA00039440626200001414
或/>
Figure BDA00039440626200001415
点处开始(若/>
Figure BDA00039440626200001416
不存在则从/>
Figure BDA00039440626200001417
处开始)，根据所选步长沿着/>
Figure BDA00039440626200001418
轴向前搜索，计算此时/>
Figure BDA00039440626200001419
处对应的/>
Figure BDA00039440626200001420
与/>
Figure BDA00039440626200001421
当/>
Figure BDA00039440626200001422
时，说明此时/>
Figure BDA00039440626200001423
位于区间/>
Figure BDA00039440626200001424
内，停止搜索，利用黄金分割法在/>
Figure BDA00039440626200001425
中搜索得到满足精度要求的最大速度/>
Figure BDA00039440626200001426
遍历轨迹每一个离散点sk，k＝1，2，……N，得到一系列
Figure BDA00039440626200001427
数据，构成相平面/>
Figure BDA00039440626200001428
上满足关节加速度约束与关节力矩约束的最大速度限制曲线MVCacc&tor；
再考虑关节角速度约束
Figure BDA00039440626200001429
Figure BDA00039440626200001430
由式(17)及X轴Z轴各自的关节角速度约束，可得速度约束下的最大速度限制曲线MVCvel＝min(MVCXvel,MVCZvel)，最终得到满足速度，加速度，力矩约束的最大速度限制曲线MVC＝min(MVCacc&tor,MVCvel)；
在相平面
Figure BDA0003944062620000151
中，MVC上的区域为不满足约束条件的不可行域，MVC本身及其下方的区域为满足约束条件的可行域，时间最优轨迹在可行域内进行规划。
S34、假设相邻离散点之间的运动为匀加速运动，即在规划时，当向前计算
Figure BDA0003944062620000152
时，假设sk-1到sk间以在sk-1处的最大标量加速度/>
Figure BDA0003944062620000153
作匀加速运动,由匀加速运动方程：
Figure BDA0003944062620000154
可得：
Figure BDA0003944062620000155
其中，
Figure BDA0003944062620000156
当向后计算
Figure BDA0003944062620000157
时，假设sk+1到sk间以在sk+1处的最小标量加速度/>
Figure BDA0003944062620000158
作匀加速运动，由匀加速运动方程
Figure BDA0003944062620000159
/>
可得：
Figure BDA00039440626200001510
其中，
Figure BDA00039440626200001511
为了设置转换点，用g(s)来表示MVC中s与
Figure BDA00039440626200001512
一一对应的关系，并构造表达式如下：
Figure BDA00039440626200001513
其中，
Figure BDA00039440626200001514
或/>
Figure BDA00039440626200001515
Figure BDA00039440626200001516
表示MVC的斜率，对于离散的轨迹，/>
Figure BDA0003944062620000161
当沿着MVC向前搜索时，若k(sk-1)·k(sk)<0，则将sk处对应的g(sk)设置为转换点；
S35、在相平面
Figure BDA0003944062620000162
上求出MVC，与对应的标量位移s，最大标量加速度/>
Figure BDA0003944062620000163
最小标量加速度/>
Figure BDA0003944062620000164
S36、在起始点
Figure BDA0003944062620000165
处，利用式(19)以最大标量加速度/>
Figure BDA0003944062620000166
向前计算标量速度/>
Figure BDA0003944062620000167
定义用/>
Figure BDA0003944062620000168
向前计算的得到曲线为α-profile；
若α-profile与MVC相交，即
Figure BDA0003944062620000169
从α-profile与MVC相交的交点开始，沿着MVC向前搜索转换点，定义搜索得到部分MVC曲线为γ-profile；
若α-profile与s＝sN相交，即sk＝sN，从终点
Figure BDA00039440626200001610
以最小标量加速度
Figure BDA00039440626200001611
向后计算标量速度/>
Figure BDA00039440626200001612
直到与已规划曲线相交，定义用最小加速度向后计算的曲线为β-profile；
S37、从转换点处，利用式(21)以最小标量加速度
Figure BDA00039440626200001613
向后计算标量速度/>
Figure BDA00039440626200001614
直到与已规划的曲线相交；同时从该转换点以最大标量加速度/>
Figure BDA00039440626200001615
向前计算标量速度/>
Figure BDA00039440626200001616
若规划的曲线与MVC相交，、从α-profile与MVC相交的交点开始，沿着MVC向前搜索转换点。定义搜索得到部分MVC曲线为γ-profile；
若规划的曲线与s＝sN相交，从终点
Figure BDA00039440626200001617
以最小标量加速度/>
Figure BDA00039440626200001618
向后计算标量速度/>
Figure BDA00039440626200001619
直到与已规划的曲线相交，定义用最小加速度向后计算的曲线为β-profile。
S38、最终按顺序将得到的α-profile、β-profile、γ-profile和δ-profile连接构成完整的最优轨迹，即可得到时间最优轨迹。
S4、对时间最优轨迹进行相似度比较，是在利用类数值积分时间最优轨迹规划算法进行轨迹规划后，利用余弦相似度比较算法，对相邻离散步长对应的相平面时间最优轨迹中的标量速度数据进行相似度比较，选取出相似度高的步长x；
步骤S4的具体步骤如下：
S41、对给定轨迹进行时间最优轨迹规划后，可得到数据集
Figure BDA0003944062620000171
由两组一一对应的数据集组成，即标量位移数据集D(s)和标量速度数据集/>
Figure BDA0003944062620000172
其中，/>
Figure BDA0003944062620000173
为相平面时间最优轨迹；对前后离散步长进行规划后分别得到D1(s)与/>
Figure BDA0003944062620000174
D2(s)与/>
Figure BDA0003944062620000175
根据前后离散化步长对应的标量位移数据集D1(s)与D2(s)之间的关系，将较大的离散步长对应的标量速度数据集/>
Figure BDA0003944062620000176
运用直线插补方法，插补至与较小离散步长对应的标量速度数据集
Figure BDA0003944062620000177
令标量速度数据集/>
Figure BDA0003944062620000178
和标量速度数据集/>
Figure BDA0003944062620000179
具有相同数据长度；
S42、采用余弦相似度比较算法，度量两条时间最优轨迹相似度，假设相邻离散步长对应时间最优轨迹的速度数据集分别为Tra1与Tra2，经过上述直线插补处理后，Tra1与Tra2具有相同的维数，则其相似度为：
Figure BDA00039440626200001710
利用离散步长数据组0.1/2nm，对时间最优轨迹进行重新离散化，将相邻离散化步长对应的规划得到的时间最优轨迹，插补到相同数据长度后，即相同向量维数，运用式(23)进行余弦相似度比较，以得相似度比较结果x。
S5、重复步骤S3-S4，直到相似度比较结果大于相似度阈值，即x＞X；若x＞X，则该离散步长及其之后的步长均可以作为离散步长选择。
S6、对后续轨迹进行插值，利用分段五次多项式对后续离散步长对应的轨迹进行插值，以计算得到后续轨迹的最大加速度y，并判断最大加速度y是否大于插值轨迹加速度限制Y；
步骤S6的具体步骤如下：：
S61、对给定轨迹进行时间最优轨迹规划后，得到了一一对应的标量位移数据集D(s)，标量速度数据集
Figure BDA0003944062620000181
和标量加速度数据集/>
Figure BDA0003944062620000182
根据轨迹数据中相邻点的标量速度
Figure BDA0003944062620000183
与标量加速度/>
Figure BDA0003944062620000184
计算五次多项式各个系数，再根据相邻点之间的轨迹运行时间及其对应五次多项式，以伺服驱动器的控制周期，对标量位移s数据的各个子区间进行插值，得到了分段五次多项式插值轨迹，表达式如下：
s(t)＝c0+c1t+c2t2+c3t3+c4t4+c5t5，(24)
其中，c0,…,c5为五次多项式表达式系数矩阵；
S62、由台达伺服驱动器的加减速时间设定，设置插值轨迹加速度限制Ym/s2，将式(24)分段五次多项式插值轨迹s(t)对t求一阶导和二阶导得到每一个点的标量速度
Figure BDA0003944062620000185
与标量加速度/>
Figure BDA0003944062620000186
再对式(3)关节角度关于s的函数q(s)对s求一阶导和二阶导得到速度q′(sk)与加速度q″(sk)，由式(13)，可得：
Figure BDA0003944062620000187
又
Figure BDA0003944062620000188
其中，ph为导程，在设置加速度限制Y后，
Figure BDA0003944062620000189
联立式(25)和(26)，可得
Figure BDA00039440626200001810
由式(27)可判断轨迹每一点的加速度是否在限制以内，由此选取出加速度不超限的轨迹。
S63、根据相平面速度
Figure BDA00039440626200001811
相平面加速度/>
Figure BDA00039440626200001812
与控制周期，利用五次多项式对D2(s)进行插值得到D'2(s)，并对t求二阶导，计算得到最大加速度y。
优选的，在步骤S63之后，还包括以下步骤：
S64、若y<Y，令n＝n+1，计算n对应离散化步长DSL(n)的相平面时间最优曲线
Figure BDA0003944062620000191
并且转到步骤S53。
S7、若y＞Y，计算得到每个时刻各个轴的角位移；步骤S7包括以下步骤：
输出n-1对应离散化步长DSL(n-1)的用五次样条曲线插补得到的D'2(s)，利用分段最小二乘拟合法将插值得到的D'2(s)数据转到关节空间。
S8、将各轴角位移转换为控制指令脉冲信号。
本发明将给定轨迹根据一组逐渐细化的离散化步长进行重新离散化，使用类数值积分相平面时间最优轨迹规划算法进行轨迹规划，将时间最优轨迹视为多维向量，利用余弦相似度比较算法对相邻步长对应轨迹进行比较，得出轨迹间的相似度，设置相似度阈值，当相似度大于阈值时，即认为此时两条轨迹足够相似，选取此时的离散步长及其之后的步长。再对规划出的时间最优轨迹利用五次多项式进行分段插值，同时，为了防止由于离散步长过小，导致离散点过多，同时子区间过小，插值后子区间内点的加速度可能会超限，导致轨迹不平滑的问题，针对分段五次多项式轨迹设置了加速度限制，在所选取的步长中选出加速度不超限的分段五次多项式轨迹，得到时间最优平滑轨迹，减少轨迹规划计算时间。最后运用分段最小二乘法拟合法，将轨迹转换到关节空间，计算出各轴的关节角度。将各轴角位移转换为控制指令脉冲信号，输入到控制程序中，控制多轴运动系统进行运动。本发明使用余弦相似度比较与分段五次多项式插值的方法，选取了最优的离散点数，有效减少了多轴运动系统的时间最优轨迹的时间与轨迹规划的计算时间，得到了更平滑的时间最优轨迹。
上述具体实施方式为本发明的优选实施例，并不能对本发明进行限定，其他的任何未背离本发明的技术方案而所做的改变或其它等效的置换方式，都包含在本发明的保护范围之内。

Claims (8)

1.基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：包括以下步骤：
S1、获取任务轨迹，并将任务轨迹转化为参数路径轨迹；
S2、设置离散步长组DSL(n)＝0.1/2nm，相似度阈值为X，插值轨迹加速度限制为Y，其中，m为单位米，n＝0，1，2，…，10；基于离散步长组对参数路径轨迹进行离散化；
S3、对多轴运动系统进行动力学建模，再建立时间最优数学模型，计算出相平面
Figure FDA0003944062610000011
最大速度限制曲线，利用类数值积分算法，计算时间最优轨迹；
S4、对时间最优轨迹进行相似度比较，是在利用类数值积分时间最优轨迹规划算法进行轨迹规划后，利用余弦相似度比较算法，对相邻离散步长对应的相平面时间最优轨迹中的标量速度数据进行相似度比较，选取出相似度高的步长x；
S5、重复步骤S3-S4，直到相似度比较结果大于相似度阈值，即x＞X；
S6、对后续轨迹进行插值，利用分段五次多项式对后续离散步长对应的轨迹进行插值，以计算得到后续轨迹的最大加速度y，并判断最大加速度y是否大于插值轨迹加速度限制Y；
S7、若y＞Y，计算得到每个时刻各个轴的角位移；
S8、将各轴角位移转换为控制指令脉冲信号。
2.根据权利要求1所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：步骤S1中，将任务轨迹转化为参数路径轨迹的具体过程为：
根据路径轨迹位移的总长度，将路径轨迹归一化，用标量参数s描述多轴运动系统末端任务轨迹的标量位移，得到参数路径轨迹。
3.根据权利要求1所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：步骤S3的具体步骤如下：
S31、对单轴动力学结构模型进行分析建模，得
Figure FDA0003944062610000021
其中，Jm为电机转动惯量，Jc1为电机端联轴器的转动惯量，Jc2为丝杠端联轴器的转动惯量，Js为丝杆转动惯量，m1为滚珠丝杠螺母的质量，m2为工作台的质量，klb和krb分别为左端和右端轴承的径向刚度，klg为工作台与线性导轨的接触面刚度，Clg为工作台与线性导轨接触面的阻尼系数，fc为工作台与线性导轨之间的库伦摩擦系数，符号(·)表示对时间t的导数；qm为电机输出轴转角，qs为滚珠丝杆转角，qn为丝杠螺母的相对转角，vb为丝杠整体因轴承而产生的径向振动位移，v为与线性导轨接触的工作台产生垂直方向的振动位移，u为与滚珠丝杠螺母固定连接的工作台产生轴向位移；
用qm代替qs和qn，u可用qn表示，qn＝2πu/ph，ph为丝杠导程，对式(1)进行简化得：
τm＝Hb·β， (2)
其中，
Figure FDA0003944062610000022
β＝[M fc fv C]T，/>
Figure FDA0003944062610000025
M＝Jm+Jc1+Jc2+Js+ph(m1+m2)/2π，/>
Figure FDA0003944062610000024
/>
其中，C为常数项；
将qm视为关于标量参数s的函数q(s)，并采用分段最小乘拟合方法，分段拟合q(s)，得
q(s)＝a0+a1s+…+aksk， (3)
其中，a0,…,ak是各关节拟合的表达式系数矩阵，k是用于最小二乘拟合的表达式阶数；
电机角速度
Figure FDA0003944062610000031
和角加速度/>
Figure FDA0003944062610000032
转化为标量参数s的函数，得：
Figure FDA0003944062610000033
Figure FDA0003944062610000034
其中，
Figure FDA0003944062610000035
将式(3)-(5)代入式(2)，可得多轴运动系统的动力学模型：
Figure FDA0003944062610000036
其中，m(s)＝M·q′(s),v(s)＝M·q″(s),f(s)＝fv·q′(s),c(s)＝fc·sign[q′(s)]+C；
S32、对于给定速度约束
Figure FDA0003944062610000037
和/>
Figure FDA0003944062610000038
加速度约束/>
Figure FDA0003944062610000039
和/>
Figure FDA00039440626100000310
力矩约束τmax和τmin，可得约束不等式：
Figure FDA00039440626100000311
时间最优轨迹的目标是使轨迹运行时间最小，则优化目标函数设置为：
Figure FDA00039440626100000312
类数值积分时间最优轨迹的规划算法是在相平面
Figure FDA00039440626100000313
上进行轨迹规划，将微元dt换成ds，得：
Figure FDA00039440626100000314
进行规划时，对于离散点数为N的轨迹，优化目标函数最终为：
Figure FDA00039440626100000315
在相平面
Figure FDA00039440626100000316
中，s(0)＝0,s(T)＝1，且在时间最优轨迹起始点必有
Figure FDA00039440626100000317
时间最优轨迹的离散点数为N，则可得相平面/>
Figure FDA00039440626100000318
中对于起始点的限制为：/>
Figure FDA0003944062610000041
联立式(4)-(7)、(10)及(11)，可得时间最优轨迹的数学模型为：
Figure FDA0003944062610000042
其中，每个离散点处电机转角相对于标量位移s的一阶导q′(sk)和二阶导q″(sk)可根据式(3)对s求导得到；
S33、在进行时间最优轨迹规划前，需要计算出相平面
Figure FDA0003944062610000043
最大速度限制曲线，由式(12)可得，对于离散后的相平面轨迹，在离散点sk处有：
Figure FDA0003944062610000044
Figure FDA0003944062610000045
在相平面
Figure FDA0003944062610000046
上，由式(13)和(14)，可得在离散点sk处，其中k＝1，2，……,N；X轴和Z轴各自的关节角加速度约束/>
Figure FDA0003944062610000047
和关节力矩约束(τmin,τmax)构成标量加速度/>
Figure FDA0003944062610000048
可行区域，在可行域内，离散点sk的最大标量加速度/>
Figure FDA0003944062610000049
与最小标量加速度/>
Figure FDA00039440626100000410
分别为：
Figure FDA00039440626100000411
Figure FDA00039440626100000412
当
Figure FDA0003944062610000051
时，/>
Figure FDA0003944062610000052
或/>
Figure FDA0003944062610000053
取合适步长，从/>
Figure FDA0003944062610000054
或/>
Figure FDA0003944062610000055
点处开始，根据所选步长沿着/>
Figure FDA0003944062610000056
轴向前搜索，计算此时/>
Figure FDA0003944062610000057
处对应的/>
Figure FDA0003944062610000058
与/>
Figure FDA0003944062610000059
当
Figure FDA00039440626100000510
时，说明此时/>
Figure FDA00039440626100000511
位于区间/>
Figure FDA00039440626100000512
内，停止搜索，利用黄金分割法在
Figure FDA00039440626100000513
中搜索得到满足精度要求的最大速度/>
Figure FDA00039440626100000514
/>
遍历轨迹每一个离散点sk，k＝1，2，……N，得到一系列
Figure FDA00039440626100000515
数据，构成相平面
Figure FDA00039440626100000516
上满足关节加速度约束与关节力矩约束的最大速度限制曲线MVCacc&tor；
再考虑关节角速度约束
Figure FDA00039440626100000517
Figure FDA00039440626100000518
由式(17)及X轴Z轴各自的关节角速度约束，可得速度约束下的最大速度限制曲线MVCvel＝min(MVCXvel,MVCZvel)，最终得到满足速度，加速度，力矩约束的最大速度限制曲线MVC＝min(MVCacc&tor,MVCvel)；
S34、假设相邻离散点之间的运动为匀加速运动，即在规划时，当向前计算
Figure FDA00039440626100000519
时，假设sk-1到sk间以在sk-1处的最大标量加速度/>
Figure FDA00039440626100000520
作匀加速运动,由匀加速运动方程：
Figure FDA00039440626100000521
可得：
Figure FDA00039440626100000522
其中，
Figure FDA00039440626100000523
当向后计算
Figure FDA00039440626100000524
时，假设sk+1到sk间以在sk+1处的最小标量加速度/>
Figure FDA00039440626100000525
作匀加速运动，由匀加速运动方程
Figure FDA00039440626100000526
可得：
Figure FDA0003944062610000061
其中，
Figure FDA0003944062610000062
为了设置转换点，用g(s)来表示MVC中s与
Figure FDA0003944062610000063
一一对应的关系，并构造表达式如下：
Figure FDA0003944062610000064
其中，
Figure FDA0003944062610000065
或/>
Figure FDA0003944062610000066
Figure FDA0003944062610000067
表示MVC的斜率，对于离散的轨迹，
Figure FDA0003944062610000068
当沿着MVC向前搜索时，若k(sk-1)·k(sk)<0，则将sk处对应的g(sk)设置为转换点；
S35、在相平面
Figure FDA0003944062610000069
上求出MVC，与对应的标量位移s，最大标量加速度/>
Figure FDA00039440626100000610
最小标量加速度/>
Figure FDA00039440626100000611
S36、在起始点
Figure FDA00039440626100000612
处，利用式(19)以最大标量加速度/>
Figure FDA00039440626100000613
向前计算标量速度/>
Figure FDA00039440626100000614
定义用/>
Figure FDA00039440626100000615
向前计算的得到曲线为α-profile；
若α-profile与MVC相交，即
Figure FDA00039440626100000616
从α-profile与MVC相交的交点开始，沿着MVC向前搜索转换点，定义搜索得到部分MVC曲线为γ-profile；
若α-profile与s＝sN相交，即sk＝sN，从终点
Figure FDA00039440626100000617
以最小标量加速度
Figure FDA00039440626100000618
向后计算标量速度/>
Figure FDA00039440626100000619
直到与已规划曲线相交，定义用最小加速度向后计算的曲线为β-profile；
S37、从转换点处，利用式(21)以最小标量加速度
Figure FDA00039440626100000620
向后计算标量速度/>
Figure FDA00039440626100000621
直到与已规划的曲线相交；同时从该转换点以最大标量加速度/>
Figure FDA00039440626100000622
向前计算标量速度/>
Figure FDA00039440626100000623
若规划的曲线与MVC相交，、从α-profile与MVC相交的交点开始，沿着MVC向前搜索转换点，定义搜索得到部分MVC曲线为γ-profile；
若规划的曲线与s＝sN相交，从终点
Figure FDA0003944062610000071
以最小标量加速度/>
Figure FDA0003944062610000072
向后计算标量速度/>
Figure FDA0003944062610000073
直到与已规划的曲线相交，定义用最小加速度向后计算的曲线为β-profile；
S38、最终按顺序将得到的α-profile、β-profile、γ-profile和δ-profile连接构成完整的最优轨迹，即可得到时间最优轨迹。
4.根据权利要求2所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：在步骤S33中，在相平面
Figure FDA0003944062610000074
中，MVC上的区域为不满足约束条件的不可行域，MVC本身及其下方的区域为满足约束条件的可行域，时间最优轨迹在可行域内进行规划。
5.根据权利要求1所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：步骤S4的具体步骤如下：
S41、对给定轨迹进行时间最优轨迹规划后，可得到数据集
Figure FDA0003944062610000075
由两组一一对应的数据集组成，即标量位移数据集D(s)和标量速度数据集/>
Figure FDA0003944062610000076
其中，/>
Figure FDA0003944062610000077
为相平面时间最优轨迹；对前后离散步长进行规划后分别得到D1(s)与/>
Figure FDA0003944062610000078
D2(s)与/>
Figure FDA0003944062610000079
根据前后离散化步长对应的标量位移数据集D1(s)与D2(s)之间的关系，将较大的离散步长对应的标量速度数据集/>
Figure FDA00039440626100000710
运用直线插补方法，插补至与较小离散步长对应的标量速度数据集
Figure FDA00039440626100000711
令标量速度数据集/>
Figure FDA00039440626100000712
和标量速度数据集/>
Figure FDA00039440626100000713
具有相同数据长度；
S42、采用余弦相似度比较算法，度量两条时间最优轨迹相似度，假设相邻离散步长对应时间最优轨迹的速度数据集分别为Tra1与Tra2，经过上述直线插补处理后，Tra1与Tra2具有相同的维数，则其相似度为：
Figure FDA0003944062610000081
利用离散步长数据组0.1/2nm，对时间最优轨迹进行重新离散化，将相邻离散化步长对应的规划得到的时间最优轨迹，插补到相同数据长度后，即相同向量维数，运用式(23)进行余弦相似度比较，以得相似度比较结果x。
6.根据权利要求1所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：步骤S6的具体步骤如下：：
S61、对给定轨迹进行时间最优轨迹规划后，得到了一一对应的标量位移数据集D(s)，标量速度数据集
Figure FDA0003944062610000082
和标量加速度数据集/>
Figure FDA0003944062610000083
根据轨迹数据中相邻点的标量速度/>
Figure FDA0003944062610000084
与标量加速度/>
Figure FDA0003944062610000085
计算五次多项式各个系数，再根据相邻点之间的轨迹运行时间及其对应五次多项式，以伺服驱动器的控制周期，对标量位移s数据的各个子区间进行插值，得到了分段五次多项式插值轨迹，表达式如下：
s(t)＝c0+c1t+c2t2+c3t3+c4t4+c5t5， (24)
其中，c0,…,c5为五次多项式表达式系数矩阵；
S62、由台达伺服驱动器的加减速时间设定，设置插值轨迹加速度限制Ym/s2，将式(24)分段五次多项式插值轨迹s(t)对t求一阶导和二阶导得到每一个点的标量速度
Figure FDA0003944062610000086
与标量加速度/>
Figure FDA0003944062610000087
再对式(3)关节角度关于s的函数q(s)对s求一阶导和二阶导得到速度q′(sk)与加速度q″(sk)，由式(13)，可得：
Figure FDA0003944062610000088
又
Figure FDA0003944062610000089
其中，ph为导程，在设置加速度限制Y后，
Figure FDA00039440626100000810
联立式(25)和(26)，可得
Figure FDA0003944062610000091
由式(27)可判断轨迹每一点的加速度是否在限制以内，由此选取出加速度不超限的轨迹；
S63、根据相平面速度
Figure FDA0003944062610000092
相平面加速度/>
Figure FDA0003944062610000093
与控制周期，利用五次多项式对D2(s)进行插值得到D'2(s)，并对t求二阶导，计算得到最大加速度y。
7.根据权利要求6所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：在步骤S63之后，还包括以下步骤：
S64、若y<Y，令n＝n+1，计算n对应离散化步长DSL(n)的相平面时间最优曲线
Figure FDA0003944062610000094
并且转到步骤S53。/>
8.根据权利要求1所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：步骤S7包括以下步骤：
输出n-1对应离散化步长DSL(n-1)的用五次样条曲线插补得到的D'2(s)，利用分段最小二乘拟合法将插值得到的D'2(s)数据转到关节空间。
CN202211423987.9A 2022-11-15 2022-11-15 基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法 Active CN115870974B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211423987.9A CN115870974B (zh) 2022-11-15 2022-11-15 基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211423987.9A CN115870974B (zh) 2022-11-15 2022-11-15 基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法

Publications (2)

Publication Number Publication Date
CN115870974A true CN115870974A (zh) 2023-03-31
CN115870974B CN115870974B (zh) 2024-05-17

Family

ID=85759876

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211423987.9A Active CN115870974B (zh) 2022-11-15 2022-11-15 基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法

Country Status (1)

Country Link
CN (1) CN115870974B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190184560A1 (en) * 2017-01-19 2019-06-20 Beijing University Of Technology A Trajectory Planning Method For Six Degree-of-Freedom Robots Taking Into Account of End Effector Motion Error
CN110162586A (zh) * 2019-05-24 2019-08-23 中国科学院地理科学与资源研究所 一种适用于移动目标分支轨迹的相似度查询系统及方法
CN110196590A (zh) * 2019-04-23 2019-09-03 华南理工大学 一种用于机器人路径跟踪的时间最优轨迹规划系统及方法
CN110209048A (zh) * 2019-05-20 2019-09-06 华南理工大学 基于动力学模型的机器人时间最优轨迹规划方法、设备
CN110221538A (zh) * 2019-04-26 2019-09-10 华南理工大学 一种结合迭代学习的时间最优轨迹规划控制器及方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190184560A1 (en) * 2017-01-19 2019-06-20 Beijing University Of Technology A Trajectory Planning Method For Six Degree-of-Freedom Robots Taking Into Account of End Effector Motion Error
CN110196590A (zh) * 2019-04-23 2019-09-03 华南理工大学 一种用于机器人路径跟踪的时间最优轨迹规划系统及方法
CN110221538A (zh) * 2019-04-26 2019-09-10 华南理工大学 一种结合迭代学习的时间最优轨迹规划控制器及方法
CN110209048A (zh) * 2019-05-20 2019-09-06 华南理工大学 基于动力学模型的机器人时间最优轨迹规划方法、设备
CN110162586A (zh) * 2019-05-24 2019-08-23 中国科学院地理科学与资源研究所 一种适用于移动目标分支轨迹的相似度查询系统及方法

Also Published As

Publication number Publication date
CN115870974B (zh) 2024-05-17

Similar Documents

Publication Publication Date Title
CN111399514B (zh) 一种机器人时间最优轨迹规划方法
Li et al. Dual sliding mode contouring control with high accuracy contour error estimation for five-axis CNC machine tools
Xiao et al. Smooth and near time‐optimal trajectory planning of industrial robots for online applications
CN110561438A (zh) 基于动力学参数辨识的工业机器人力/位柔顺控制方法
CN113253678B (zh) 精密运动平台的运动轨迹规划方法
Dong et al. Optimal feed-rate scheduling for high-speed contouring
CN114102606B (zh) 机器人运动信息规划方法及相关装置
Cen et al. Nonholonomic wheeled mobile robot trajectory tracking control based on improved sliding mode variable structure
CN112799304A (zh) 一种基于时变摩擦补偿的双电机同步控制方法及设备
Lu et al. Experimental determination of dynamic parameters of robotic arms
Zhang et al. Robotic curved surface tracking with a neural network for angle identification and constant force control based on reinforcement learning
Li et al. Contouring compensation control based on high accuracy contour error estimation for multi-axis motion systems
Li et al. Friction identification and compensation design for precision positioning
CN111427308A (zh) 一种用于数控平台轨迹规划的误差补偿综合控制方法
CN105929791B (zh) 平面直角坐标运动系统的直接轮廓控制方法
Cheng et al. Mechanism-based structured deep neural network for cutting force forecasting using CNC inherent monitoring signals
CN115870974A (zh) 基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法
Cheng et al. Dynamic modeling and parameter identification for a gantry-type automated fiber placement machine
Lam et al. Multi-axis model predictive contouring control
Guo et al. Active disturbance rejection control for PMLM servo system in CNC machining
CN105700470A (zh) 一种用于减小机床伺服进给系统跟踪误差的方法
CN114952852A (zh) 机器人nurbs曲线速度规划方法、设备及存储介质
CN114296404A (zh) 一种计算五轴点胶机加工轨迹速度的方法
CN115122329A (zh) 一种便捷的工业机器人关节空间轨迹规划方法
CN113946922A (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
GR01 Patent grant