CN115870974B - 基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法 - Google Patents
基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法 Download PDFInfo
- Publication number
- CN115870974B CN115870974B CN202211423987.9A CN202211423987A CN115870974B CN 115870974 B CN115870974 B CN 115870974B CN 202211423987 A CN202211423987 A CN 202211423987A CN 115870974 B CN115870974 B CN 115870974B
- Authority
- CN
- China
- Prior art keywords
- track
- acceleration
- scalar
- mvc
- time
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 62
- 230000001133 acceleration Effects 0.000 claims abstract description 93
- 238000006073 displacement reaction Methods 0.000 claims abstract description 43
- 238000004364 calculation method Methods 0.000 claims abstract description 17
- 230000007704 transition Effects 0.000 claims description 15
- 230000010354 integration Effects 0.000 claims description 11
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 238000013178 mathematical model Methods 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 6
- 101150058668 tra2 gene Proteins 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 4
- 101100152865 Danio rerio thraa gene Proteins 0.000 claims description 3
- 101000796673 Homo sapiens Transformation/transcription domain-associated protein Proteins 0.000 claims description 3
- 102100032762 Transformation/transcription domain-associated protein Human genes 0.000 claims description 3
- 238000013016 damping Methods 0.000 claims description 3
- 238000005457 optimization Methods 0.000 claims description 3
- 101150117196 tra-1 gene Proteins 0.000 claims description 3
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 229920006395 saturated elastomer Polymers 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Landscapes
- Numerical Control (AREA)
Abstract
本发明公开了一种基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，此方法先将获取的任务轨迹转化成参数路径轨迹，然后设置离散步长组，以将参数路径轨迹进行离散化；再依次计算时间最优轨迹规划，并对轨迹进行相似度比较，直到相似度比较结果大于相似度阈值；接着对后结轨迹进行插值，并判断加速度是否超过插值轨迹加速度限制；基于判断结果，先择不超限的轨迹，计算得到每个时刻各个轴的角位移；最后将各轴角位移转换为控制指令脉冲信号。本发明减少了时间最优轨迹的时间与轨迹规划的计算时间，得到了更平滑的时间最优轨迹。
Description
技术领域
本发明涉及多轴运动系统领域的技术，具体涉及一种基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法及推算系统。
背景技术
多轴运动系统得到广泛的应用，其结构包括并联结构与串联结构，轴的移动方式包括转动和移动两种。
对于时间最优轨迹规划，运用数值积分方法进行时间最优轨迹规划，减少了计算量，缩短了计算时间，其将动力学模型用参数方程表示，即将电机力矩约束转化成参数化加速度的约束，运用数值积分法寻找时间最优解决方案，使电机工作在临界饱和力矩的状态下，最大化运行速度，从而最小化运行时间。大多数数值积分算法是将给定轨迹用参数函数表示，并在相平面进行时间最优轨迹规划。在规划时，需要遍历轨迹，在约束条件下计算每一个点的相平面最大标量速度、标量加速度与实际速度，因此需要多次使用动力学模型，耗费较多规划计算时间。且对于一条确定的轨迹，其中许多轨迹点是不重要的，因此会产生不必要的计算，导致耗费更多的规划计算时间。
对于时间最优平滑轨迹规划，可在规划过程中设置动力学约束，如力矩变化率约束，或设置加运动学约束，如加速度约束，或同时设置动力学和运动学约束。在规划得到时间最优平滑轨迹之后，需要根据控制周期对轨迹进行插值，可利用多项式函数对轨迹每一个区间进行插值，得到分段多项式轨迹。对于轨迹平滑，加速度连续是必不可少的。当轨迹离散点较多，子区间较小时，分段多项式中间的点速度或是加速度并不一定满足给定的约束。
发明内容
本发明的目的是为了克服以上现有技术存在的不足，提供了一种基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法。此基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法在保证得到时间最优平滑轨迹的情况下，可减少规划时间。
本发明的目的通过以下的技术方案实现：本基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，包括以下步骤：
S1、获取任务轨迹，并将任务轨迹转化为参数路径轨迹；
S2、设置离散步长组DSL(n)＝0.1/2nm，相似度阈值为X，插值轨迹加速度限制为Y，其中，m为单位米，n＝0，1，2，…，10；基于离散步长组对参数路径轨迹进行离散化；
S3、对多轴运动系统进行动力学建模，再建立时间最优数学模型，计算出相平面最大速度限制曲线，利用类数值积分算法，计算时间最优轨迹；
S4、对时间最优轨迹进行相似度比较，是在利用类数值积分时间最优轨迹规划算法进行轨迹规划后，利用余弦相似度比较算法，对相邻离散步长对应的相平面时间最优轨迹中的标量速度数据进行相似度比较，选取出相似度高的步长x；
S5、重复步骤S3-S4，直到相似度比较结果大于相似度阈值，即x＞X；
S6、对后续轨迹进行插值，利用分段五次多项式对后续离散步长对应的轨迹进行插值，以计算得到后续轨迹的最大加速度y，并判断最大加速度y是否大于插值轨迹加速度限制Y；
S7、若y＞Y，计算得到每个时刻各个轴的角位移；
S8、将各轴角位移转换为控制指令脉冲信号。
优选的，步骤S1中，将任务轨迹转化为参数路径轨迹的具体过程为：
根据路径轨迹位移的总长度，将路径轨迹归一化，用标量参数s描述多轴运动系统末端任务轨迹的标量位移，得到参数路径轨迹。
优选的，步骤S3的具体步骤如下：
S31、对单轴动力学结构模型进行分析建模，得
其中，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)
其中，β＝[M fc fv C]T，M＝Jm+Jc1+Jc2+Js+ph(m1+m2)/2π，
其中，C为常数项；
将qm视为关于标量参数s的函数q(s)，并采用分段最小乘拟合方法，分段拟合q(s)，得
q(s)＝a0+a1s+…+aksk，(3)
其中，a0,…,ak是各关节拟合的表达式系数矩阵，k是用于最小二乘拟合的表达式阶数；
电机角速度和角加速度转化为标量参数s的函数，得：
其中，
将式(3)-(5)代入式(2)，可得多轴运动系统的动力学模型：
其中，m(s)＝M·q′(s),v(s)＝M·q″(s),f(s)＝fv·q′(s),c(s)＝fc·sign[q′(s)]+C；
S32、对于给定速度约束和加速度约束和力矩约束τmax和τmin，可得约束不等式：
时间最优轨迹的目标是使轨迹运行时间最小，则优化目标函数设置为：
类数值积分时间最优轨迹的规划算法是在相平面上进行轨迹规划，将微元dt换成ds，得：
进行规划时，对于离散点数为N的轨迹，优化目标函数最终为：
在相平面中，s(0)＝0,s(T)＝1，且在时间最优轨迹起始点必有时间最优轨迹的离散点数为N，则可得相平面中对于起始点的限制为：
联立式(4)-(7)、(10)及(11)，可得时间最优轨迹的数学模型为：
其中，每个离散点处电机转角相对于标量位移s的一阶导q′(sk)和二阶导q″(sk)可根据式(3)对s求导得到；
S33、在进行时间最优轨迹规划前，需要计算出相平面最大速度限制曲线，由式(12)可得，对于离散后的相平面轨迹，在离散点sk处有：
在相平面上，由式(13)和(14)，可得在离散点sk处，其中k＝1，2，……,N；X轴和Z轴各自的关节角加速度约束和关节力矩约束(τmin,τmax)构成标量加速度可行区域，在可行域内，离散点sk的最大标量加速度与最小标量加速度分别为：
当时，或取合适步长，从或点处开始，根据所选步长沿着轴向前搜索，计算此时处对应的与当时，说明此时位于区间内，停止搜索，利用黄金分割法在中搜索得到满足精度要求的最大速度
遍历轨迹每一个离散点sk，k＝1，2，……N，得到一系列数据，构成相平面上满足关节加速度约束与关节力矩约束的最大速度限制曲线MVCacc&tor；
再考虑关节角速度约束
由式(17)及X轴Z轴各自的关节角速度约束，可得速度约束下的最大速度限制曲线MVCvel＝min(MVCXvel,MVCZvel)，最终得到满足速度，加速度，力矩约束的最大速度限制曲线MVC＝min(MVCacc&tor,MVCvel)；
S34、假设相邻离散点之间的运动为匀加速运动，即在规划时，当向前计算时，假设sk-1到sk间以在sk-1处的最大标量加速度作匀加速运动,由匀加速运动方程：
可得：
其中，
当向后计算时，假设sk+1到sk间以在sk+1处的最小标量加速度作匀加速运动，由匀加速运动方程
可得：
其中，
为了设置转换点，用g(s)来表示MVC中s与一一对应的关系，并构造表达式如下：
其中，或 表示MVC的斜率，对于离散的轨迹，当沿着MVC向前搜索时，若k(sk-1)·k(sk)<0，则将sk处对应的g(sk)设置为转换点；
S35、在相平面上求出MVC，与对应的标量位移s，最大标量加速度最小标量加速度
S36、在起始点处，利用式(19)以最大标量加速度向前计算标量速度定义用向前计算的得到曲线为α-profile；
若α-profile与MVC相交，即从α-profile与MVC相交的交点开始，沿着MVC向前搜索转换点，定义搜索得到部分MVC曲线为γ-profile；
若α-profile与s＝sN相交，即sk＝sN，从终点以最小标量加速度向后计算标量速度直到与已规划曲线相交，定义用最小加速度向后计算的曲线为β-profile；
S37、从转换点处，利用式(21)以最小标量加速度向后计算标量速度直到与已规划的曲线相交；同时从该转换点以最大标量加速度向前计算标量速度
若规划的曲线与MVC相交，、从α-profile与MVC相交的交点开始，沿着MVC向前搜索转换点。定义搜索得到部分MVC曲线为γ-profile；
若规划的曲线与s＝sN相交，从终点以最小标量加速度向后计算标量速度直到与已规划的曲线相交，定义用最小加速度向后计算的曲线为β-profile。
S38、最终按顺序将得到的α-profile、β-profile、γ-profile和δ-profile连接构成完整的最优轨迹，即可得到时间最优轨迹。
优选的，在步骤S33中，在相平面中，MVC上的区域为不满足约束条件的不可行域，MVC本身及其下方的区域为满足约束条件的可行域，时间最优轨迹在可行域内进行规划。
优选的，步骤S4的具体步骤如下：
S41、对给定轨迹进行时间最优轨迹规划后，可得到数据集由两组一一对应的数据集组成，即标量位移数据集D(s)和标量速度数据集其中，为相平面时间最优轨迹；对前后离散步长进行规划后分别得到D1(s)与D2(s)与根据前后离散化步长对应的标量位移数据集D1(s)与D2(s)之间的关系，将较大的离散步长对应的标量速度数据集运用直线插补方法，插补至与较小离散步长对应的标量速度数据集令标量速度数据集和标量速度数据集具有相同数据长度；
S42、采用余弦相似度比较算法，度量两条时间最优轨迹相似度，假设相邻离散步长对应时间最优轨迹的速度数据集分别为Tra1与Tra2，经过上述直线插补处理后，Tra1与Tra2具有相同的维数，则其相似度为：
利用离散步长数据组0.1/2nm，对时间最优轨迹进行重新离散化，将相邻离散化步长对应的规划得到的时间最优轨迹，插补到相同数据长度后，即相同向量维数，运用式(23)进行余弦相似度比较，以得相似度比较结果x。
优选的，步骤S6的具体步骤如下：：
S61、对给定轨迹进行时间最优轨迹规划后，得到了一一对应的标量位移数据集D(s)，标量速度数据集和标量加速度数据集根据轨迹数据中相邻点的标量速度与标量加速度计算五次多项式各个系数，再根据相邻点之间的轨迹运行时间及其对应五次多项式，以伺服驱动器的控制周期，对标量位移s数据的各个子区间进行插值，得到了分段五次多项式插值轨迹，表达式如下：
s(t)＝c0+c1t+c2t2+c3t3+c4t4+c5t5，(24)
其中，c0,…,c5为五次多项式表达式系数矩阵；
S62、由台达伺服驱动器的加减速时间设定，设置插值轨迹加速度限制Ym/s2，将式(24)分段五次多项式插值轨迹s(t)对t求一阶导和二阶导得到每一个点的标量速度与标量加速度再对式(3)关节角度关于s的函数q(s)对s求一阶导和二阶导得到速度q′(sk)与加速度q″(sk)，由式(13)，可得：
又
其中，ph为导程，在设置加速度限制Y后，
联立式(25)和(26)，可得
由式(27)可判断轨迹每一点的加速度是否在限制以内，由此选取出加速度不超限的轨迹。
S63、根据相平面速度相平面加速度与控制周期，利用五次多项式对D2(s)进行插值得到D'2(s)，并对t求二阶导，计算得到最大加速度y。
优选的，在步骤S63之后，还包括以下步骤：
S64、若y<Y，令n＝n+1，计算n对应离散化步长DSL(n)的相平面时间最优曲线并且转到步骤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、对多轴运动系统进行动力学建模，再建立时间最优数学模型，计算出相平面最大速度限制曲线，利用类数值积分算法，计算时间最优轨迹；
步骤S3的具体步骤如下：
S31、对单轴动力学结构模型进行分析建模，得
其中，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)
其中，β＝[M fc fv C]T，M＝Jm+Jc1+Jc2+Js+ph(m1+m2)/2π，
其中，C为常数项,即于振动位移量vb和v微小且难以测量，可用常数项C近似拟合；
进行动力学参数辨识可得到β中的参数。将qm视为关于标量参数s的函数q(s)，并采用分段最小乘拟合方法，分段拟合q(s)，得
q(s)＝a0+a1s+…+aksk，(3)
其中，a0,…,ak是各关节拟合的表达式系数矩阵，k是用于最小二乘拟合的表达式阶数；
在计算得到q(s)后，为了在相平面进行规划，电机角速度和角加速度转化为标量参数s的函数，得：
其中，
将式(3)-(5)代入式(2)，可得多轴运动系统的动力学模型：
其中，m(s)＝M·q′(s),v(s)＝M·q″(s),f(s)＝fv·q′(s),c(s)＝fc·sign[q′(s)]+C；
S32、对于给定速度约束和加速度约束和力矩约束τmax和τmin，可得约束不等式：
时间最优轨迹的目标是使轨迹运行时间最小，则优化目标函数设置为：
类数值积分时间最优轨迹的规划算法是在相平面上进行轨迹规划，将微元dt换成ds，得：
进行规划时，对于离散点数为N的轨迹，优化目标函数最终为：
在相平面中，s(0)＝0,s(T)＝1，且在时间最优轨迹起始点必有时间最优轨迹的离散点数为N，则可得相平面中对于起始点的限制为：
联立式(4)-(7)、(10)及(11)，可得时间最优轨迹的数学模型为：
其中，每个离散点处电机转角相对于标量位移s的一阶导q′(sk)和二阶导q″(sk)可根据式(3)对s求导得到；
S33、在进行时间最优轨迹规划前，需要计算出相平面最大速度限制曲线，由式(12)可得，对于离散后的相平面轨迹，在离散点sk处有：
在相平面上，由式(13)和(14)，可得在离散点sk处，其中k＝1，2，……,N；X轴和Z轴各自的关节角加速度约束和关节力矩约束(τmin,τmax)构成标量加速度可行区域，在可行域内，离散点sk的最大标量加速度与最小标量加速度分别为：
当时，或取合适步长，如0.1m，从或点处开始(若不存在则从处开始)，根据所选步长沿着轴向前搜索，计算此时处对应的与当时，说明此时位于区间内，停止搜索，利用黄金分割法在中搜索得到满足精度要求的最大速度
遍历轨迹每一个离散点sk，k＝1，2，……N，得到一系列数据，构成相平面上满足关节加速度约束与关节力矩约束的最大速度限制曲线MVCacc&tor；
再考虑关节角速度约束
由式(17)及X轴Z轴各自的关节角速度约束，可得速度约束下的最大速度限制曲线MVCvel＝min(MVCXvel,MVCZvel)，最终得到满足速度，加速度，力矩约束的最大速度限制曲线MVC＝min(MVCacc&tor,MVCvel)；
在相平面中，MVC上的区域为不满足约束条件的不可行域，MVC本身及其下方的区域为满足约束条件的可行域，时间最优轨迹在可行域内进行规划。
S34、假设相邻离散点之间的运动为匀加速运动，即在规划时，当向前计算时，假设sk-1到sk间以在sk-1处的最大标量加速度作匀加速运动,由匀加速运动方程：
可得：
其中，
当向后计算时，假设sk+1到sk间以在sk+1处的最小标量加速度作匀加速运动，由匀加速运动方程
可得：
其中，
为了设置转换点，用g(s)来表示MVC中s与一一对应的关系，并构造表达式如下：
其中，或 表示MVC的斜率，对于离散的轨迹，当沿着MVC向前搜索时，若k(sk-1)·k(sk)<0，则将sk处对应的g(sk)设置为转换点；
S35、在相平面上求出MVC，与对应的标量位移s，最大标量加速度最小标量加速度
S36、在起始点处，利用式(19)以最大标量加速度向前计算标量速度定义用向前计算的得到曲线为α-profile；
若α-profile与MVC相交，即从α-profile与MVC相交的交点开始，沿着MVC向前搜索转换点，定义搜索得到部分MVC曲线为γ-profile；
若α-profile与s＝sN相交，即sk＝sN，从终点以最小标量加速度向后计算标量速度直到与已规划曲线相交，定义用最小加速度向后计算的曲线为β-profile；
S37、从转换点处，利用式(21)以最小标量加速度向后计算标量速度直到与已规划的曲线相交；同时从该转换点以最大标量加速度向前计算标量速度
若规划的曲线与MVC相交，、从α-profile与MVC相交的交点开始，沿着MVC向前搜索转换点。定义搜索得到部分MVC曲线为γ-profile；
若规划的曲线与s＝sN相交，从终点以最小标量加速度向后计算标量速度直到与已规划的曲线相交，定义用最小加速度向后计算的曲线为β-profile。
S38、最终按顺序将得到的α-profile、β-profile、γ-profile和δ-profile连接构成完整的最优轨迹，即可得到时间最优轨迹。
S4、对时间最优轨迹进行相似度比较，是在利用类数值积分时间最优轨迹规划算法进行轨迹规划后，利用余弦相似度比较算法，对相邻离散步长对应的相平面时间最优轨迹中的标量速度数据进行相似度比较，选取出相似度高的步长x；
步骤S4的具体步骤如下：
S41、对给定轨迹进行时间最优轨迹规划后，可得到数据集由两组一一对应的数据集组成，即标量位移数据集D(s)和标量速度数据集其中，为相平面时间最优轨迹；对前后离散步长进行规划后分别得到D1(s)与D2(s)与根据前后离散化步长对应的标量位移数据集D1(s)与D2(s)之间的关系，将较大的离散步长对应的标量速度数据集运用直线插补方法，插补至与较小离散步长对应的标量速度数据集令标量速度数据集和标量速度数据集具有相同数据长度；
S42、采用余弦相似度比较算法，度量两条时间最优轨迹相似度，假设相邻离散步长对应时间最优轨迹的速度数据集分别为Tra1与Tra2，经过上述直线插补处理后，Tra1与Tra2具有相同的维数，则其相似度为：
利用离散步长数据组0.1/2nm，对时间最优轨迹进行重新离散化，将相邻离散化步长对应的规划得到的时间最优轨迹，插补到相同数据长度后，即相同向量维数，运用式(23)进行余弦相似度比较，以得相似度比较结果x。
S5、重复步骤S3-S4，直到相似度比较结果大于相似度阈值，即x＞X；若x＞X，则该离散步长及其之后的步长均可以作为离散步长选择。
S6、对后续轨迹进行插值，利用分段五次多项式对后续离散步长对应的轨迹进行插值，以计算得到后续轨迹的最大加速度y，并判断最大加速度y是否大于插值轨迹加速度限制Y；
步骤S6的具体步骤如下：：
S61、对给定轨迹进行时间最优轨迹规划后，得到了一一对应的标量位移数据集D(s)，标量速度数据集和标量加速度数据集根据轨迹数据中相邻点的标量速度与标量加速度计算五次多项式各个系数，再根据相邻点之间的轨迹运行时间及其对应五次多项式，以伺服驱动器的控制周期，对标量位移s数据的各个子区间进行插值，得到了分段五次多项式插值轨迹，表达式如下：
s(t)＝c0+c1t+c2t2+c3t3+c4t4+c5t5，(24)
其中，c0,…,c5为五次多项式表达式系数矩阵；
S62、由台达伺服驱动器的加减速时间设定，设置插值轨迹加速度限制Ym/s2，将式(24)分段五次多项式插值轨迹s(t)对t求一阶导和二阶导得到每一个点的标量速度与标量加速度再对式(3)关节角度关于s的函数q(s)对s求一阶导和二阶导得到速度q′(sk)与加速度q″(sk)，由式(13)，可得：
又
其中，ph为导程，在设置加速度限制Y后，
联立式(25)和(26)，可得
由式(27)可判断轨迹每一点的加速度是否在限制以内，由此选取出加速度不超限的轨迹。
S63、根据相平面速度相平面加速度与控制周期，利用五次多项式对D2(s)进行插值得到D'2(s)，并对t求二阶导，计算得到最大加速度y。
优选的，在步骤S63之后，还包括以下步骤：
S64、若y<Y，令n＝n+1，计算n对应离散化步长DSL(n)的相平面时间最优曲线并且转到步骤S53。
S7、若y＞Y，计算得到每个时刻各个轴的角位移；步骤S7包括以下步骤：
输出n-1对应离散化步长DSL(n-1)的用五次样条曲线插补得到的D'2(s)，利用分段最小二乘拟合法将插值得到的D'2(s)数据转到关节空间。
S8、将各轴角位移转换为控制指令脉冲信号。
本发明将给定轨迹根据一组逐渐细化的离散化步长进行重新离散化，使用类数值积分相平面时间最优轨迹规划算法进行轨迹规划，将时间最优轨迹视为多维向量，利用余弦相似度比较算法对相邻步长对应轨迹进行比较，得出轨迹间的相似度，设置相似度阈值，当相似度大于阈值时，即认为此时两条轨迹足够相似，选取此时的离散步长及其之后的步长。再对规划出的时间最优轨迹利用五次多项式进行分段插值，同时，为了防止由于离散步长过小，导致离散点过多，同时子区间过小，插值后子区间内点的加速度可能会超限，导致轨迹不平滑的问题，针对分段五次多项式轨迹设置了加速度限制，在所选取的步长中选出加速度不超限的分段五次多项式轨迹，得到时间最优平滑轨迹，减少轨迹规划计算时间。最后运用分段最小二乘法拟合法，将轨迹转换到关节空间，计算出各轴的关节角度。将各轴角位移转换为控制指令脉冲信号，输入到控制程序中，控制多轴运动系统进行运动。本发明使用余弦相似度比较与分段五次多项式插值的方法，选取了最优的离散点数，有效减少了多轴运动系统的时间最优轨迹的时间与轨迹规划的计算时间，得到了更平滑的时间最优轨迹。
上述具体实施方式为本发明的优选实施例，并不能对本发明进行限定，其他的任何未背离本发明的技术方案而所做的改变或其它等效的置换方式，都包含在本发明的保护范围之内。
Claims (7)
1.基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：包括以下步骤：
S1、获取任务轨迹，并将任务轨迹转化为参数路径轨迹；
S2、设置离散步长组DSL(n)＝0.1/2nm，相似度阈值为X，插值轨迹加速度限制为Y，其中，m为单位米，n＝0，1，2，…，10；基于离散步长组对参数路径轨迹进行离散化；
S3、对多轴运动系统进行动力学建模，再建立时间最优数学模型，计算出相平面最大速度限制曲线，利用类数值积分算法，计算时间最优轨迹；
S4、对时间最优轨迹进行相似度比较，是在利用类数值积分时间最优轨迹规划算法进行轨迹规划后，利用余弦相似度比较算法，对相邻离散步长对应的相平面时间最优轨迹中的标量速度数据进行相似度比较，选取出相似度高的步长x；
步骤S4的具体步骤如下：
S41、对给定轨迹进行时间最优轨迹规划后，可得到数据集由两组一一对应的数据集组成，即标量位移数据集D(s)和标量速度数据集其中，为相平面时间最优轨迹；对前后离散步长进行规划后分别得到D1(s)与D2(s)与根据前后离散化步长对应的标量位移数据集D1(s)与D2(s)之间的关系，将较大的离散步长对应的标量速度数据集运用直线插补方法，插补至与较小离散步长对应的标量速度数据集令标量速度数据集和标量速度数据集具有相同数据长度；
S42、采用余弦相似度比较算法，度量两条时间最优轨迹相似度，假设相邻离散步长对应时间最优轨迹的速度数据集分别为Tra1与Tra2，经过上述直线插补处理后，Tra1与Tra2具有相同的维数，则其相似度为：
利用离散步长数据组0.1/2nm，对时间最优轨迹进行重新离散化，将相邻离散化步长对应的规划得到的时间最优轨迹，插补到相同数据长度后，即相同向量维数，运用式(23)进行余弦相似度比较，以得相似度比较结果x；
S5、重复步骤S3-S4，直到相似度比较结果大于相似度阈值，即x＞X；
S6、对后续轨迹进行插值，利用分段五次多项式对后续离散步长对应的轨迹进行插值，以计算得到后续轨迹的最大加速度y，并判断最大加速度y是否大于插值轨迹加速度限制Y；
S7、若y＞Y，计算得到每个时刻各个轴的角位移；
S8、将各轴角位移转换为控制指令脉冲信号。
2.根据权利要求1所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：步骤S1中，将任务轨迹转化为参数路径轨迹的具体过程为：
根据路径轨迹位移的总长度，将路径轨迹归一化，用标量参数s描述多轴运动系统末端任务轨迹的标量位移，得到参数路径轨迹。
3.根据权利要求1所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：步骤S3的具体步骤如下：
S31、对单轴动力学结构模型进行分析建模，得
其中，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)
其中，β＝[M fc fv C]T M＝Jm+Jc1+Jc2+Js+ph(m1+m2)/2π，
其中，C为常数项；
将qm视为关于标量参数s的函数q(s)，并采用分段最小乘拟合方法，分段拟合q(s)，得
q(s)＝a0+a1s+…+aksk， (3)
其中，a0，…，ak是各关节拟合的表达式系数矩阵，k是用于最小二乘拟合的表达式阶数；
电机角速度和角加速度转化为标量参数s的函数，得：
其中，
将式(3)-(5)代入式(2)，可得多轴运动系统的动力学模型：
其中，m(s)＝M·q′(s)，v(s)＝M·q″(s)，f(s)＝fv·q′(s)，c(s)＝fc·sign[q′(s)]+C；
S32、对于给定速度约束和加速度约束和力矩约束τmax和τmin，可得约束不等式：
时间最优轨迹的目标是使轨迹运行时间最小，则优化目标函数设置为：
类数值积分时间最优轨迹的规划算法是在相平面上进行轨迹规划，将微元dt换成ds，得：
进行规划时，对于离散点数为N的轨迹，优化目标函数最终为：
在相平面中，s(0)＝0，s(T)＝1，且在时间最优轨迹起始点必有时间最优轨迹的离散点数为N，则可得相平面中对于起始点的限制为：
联立式(4)-(7)、(10)及(11)，可得时间最优轨迹的数学模型为：
其中，每个离散点处电机转角相对于标量位移s的一阶导q′(sk)和二阶导q″(sk)可根据式(3)对s求导得到；
S33、在进行时间最优轨迹规划前，需要计算出相平面最大速度限制曲线，由式(12)可得，对于离散后的相平面轨迹，在离散点sk处有：
在相平面上，由式(13)和(14)，可得在离散点sk处，其中k＝1，2，……，N；X轴和Z轴各自的关节角加速度约束和关节力矩约束(τmin，τmax)构成标量加速度可行区域，在可行域内，离散点sk的最大标量加速度与最小标量加速度分别为：
当时，或取合适步长，从或点处开始，根据所选步长沿着轴向前搜索，计算此时处对应的与当时，说明此时位于区间内，停止搜索，利用黄金分割法在中搜索得到满足精度要求的最大速度
遍历轨迹每一个离散点sk，k＝1，2，……N，得到一系列数据，构成相平面上满足关节加速度约束与关节力矩约束的最大速度限制曲线MVCacc&tor；
再考虑关节角速度约束
由式(17)及X轴Z轴各自的关节角速度约束，可得速度约束下的最大速度限制曲线MVCvel＝min(MVCXvel，MVCZvel)，最终得到满足速度，加速度，力矩约束的最大速度限制曲线MVC＝min(MVCacc&tor，MVCvel)；
S34、假设相邻离散点之间的运动为匀加速运动，即在规划时，当向前计算时，假设sk-1到sk间以在sk-1处的最大标量加速度作匀加速运动，由匀加速运动方程：
可得：
其中，
当向后计算时，假设sk+1到sk间以在sk+1处的最小标量加速度作匀加速运动，由匀加速运动方程
可得：
其中，
为了设置转换点，用g(s)来表示MVC中s与一一对应的关系，并构造表达式如下：
其中， 或 表示MVC的斜率，对于离散的轨迹，当沿着MVC向前搜索时，若k(sk-1)·k(sk)＜0，则将sk处对应的g(sk)设置为转换点；
S35、在相平面上求出MVC，与对应的标量位移s，最大标量加速度最小标量加速度
S36、在起始点处，利用式(19)以最大标量加速度向前计算标量速度定义用向前计算的得到曲线为α-profile；
若α-profile与MVC相交，即MVCk，从α-profile与MVC相交的交点开始，沿着MVC向前搜索转换点，定义搜索得到部分MVC曲线为γ-profile；
若α-profile与s＝sN相交，即sk＝sN，从终点以最小标量加速度向后计算标量速度直到与已规划曲线相交，定义用最小加速度向后计算的曲线为β-profile；
S37、从转换点处，利用式(21)以最小标量加速度向后计算标量速度直到与已规划的曲线相交；同时从该转换点以最大标量加速度向前计算标量速度
若规划的曲线与MVC相交，、从α-profile与MVC相交的交点开始，沿着MVC向前搜索转换点，定义搜索得到部分MVC曲线为γ-profile；
若规划的曲线与s＝sN相交，从终点以最小标量加速度向后计算标量速度直到与已规划的曲线相交，定义用最小加速度向后计算的曲线为β-profile；
S38、最终按顺序将得到的α-profile、β-profile、γ-profile和δ-profile连接构成完整的最优轨迹，即可得到时间最优轨迹。
4.根据权利要求3所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：在步骤S33中，在相平面中，MVC上的区域为不满足约束条件的不可行域，MVC本身及其下方的区域为满足约束条件的可行域，时间最优轨迹在可行域内进行规划。
5.根据权利要求1所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：步骤S6的具体步骤如下：
S61、对给定轨迹进行时间最优轨迹规划后，得到了一一对应的标量位移数据集D(s)，标量速度数据集和标量加速度数据集根据轨迹数据中相邻点的标量速度与标量加速度计算五次多项式各个系数，再根据相邻点之间的轨迹运行时间及其对应五次多项式，以伺服驱动器的控制周期，对标量位移s数据的各个子区间进行插值，得到了分段五次多项式插值轨迹，表达式如下：
s(t)＝c0+c1t+c2t2+c3t3+c4t4+c5t5， (24)
其中，c0，…，c5为五次多项式表达式系数矩阵；
S62、由台达伺服驱动器的加减速时间设定，设置插值轨迹加速度限制Ym/s2，将式(24)分段五次多项式插值轨迹s(t)对t求一阶导和二阶导得到每一个点的标量速度与标量加速度再对式(3)关节角度关于s的函数q(s)对s求一阶导和二阶导得到速度q′(sk)与加速度q″(sk)，由式(13)，可得：
又
其中，ph为导程，在设置加速度限制Y后，
联立式(25)和(26)，可得
由式(27)可判断轨迹每一点的加速度是否在限制以内，由此选取出加速度不超限的轨迹；
S63、根据相平面速度相平面加速度与控制周期，利用五次多项式对D2(s)进行插值得到D′2(s)，并对t求二阶导，计算得到最大加速度y。
6.根据权利要求5所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：在步骤S63之后，还包括以下步骤：
S64、若y＜Y，令n＝n+1，计算n对应离散化步长DSL(n)的相平面时间最优曲线并且转到步骤S53。
7.根据权利要求1所述的基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法，其特征在于：步骤S7包括以下步骤：
输出n-1对应离散化步长DSL(n-1)的用五次样条曲线插补得到的D′2(s)，利用分段最小二乘拟合法将插值得到的D′2(s)数据转到关节空间。
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 CN115870974A (zh) | 2023-03-31 |
| CN115870974B true 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 (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 | 华南理工大学 | 一种结合迭代学习的时间最优轨迹规划控制器及方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106647282B (zh) * | 2017-01-19 | 2020-01-03 | 北京工业大学 | 一种考虑末端运动误差的六自由度机器人轨迹规划方法 |
-
2022
- 2022-11-15 CN CN202211423987.9A patent/CN115870974B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 |
|---|---|
| CN115870974A (zh) | 2023-03-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111399514B (zh) | 一种机器人时间最优轨迹规划方法 | |
| Lin et al. | Development of a dynamics-based NURBS interpolator with real-time look-ahead algorithm | |
| Tsai et al. | Development of an integrated look-ahead dynamics-based NURBS interpolator for high precision machinery | |
| CN114102606B (zh) | 机器人运动信息规划方法及相关装置 | |
| CN110757455B (zh) | 一种速度规划方法 | |
| CN113253678B (zh) | 精密运动平台的运动轨迹规划方法 | |
| CN101493687A (zh) | 实时前瞻全程加减速控制的nurbs曲线自适应分段插补方法 | |
| CN108227630A (zh) | 一种采用时间参数多项式插补的自由曲面数控加工方法 | |
| CN113204215B (zh) | 一种数控加工全局nurbs轨迹实时插补方法及其应用 | |
| CN111427308B (zh) | 一种用于数控平台轨迹规划的误差补偿综合控制方法 | |
| CN114296404A (zh) | 一种计算五轴点胶机加工轨迹速度的方法 | |
| Zhang et al. | Robotic curved surface tracking with a neural network for angle identification and constant force control based on reinforcement learning | |
| CN114603558B (zh) | 混合空间过渡轨迹规划方法及装置 | |
| CN115122329B (zh) | 一种便捷的工业机器人关节空间轨迹规划方法 | |
| Li et al. | Friction identification and compensation design for precision positioning | |
| CN115870974B (zh) | 基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法 | |
| Fang et al. | A high-precision non-uniform rational B-spline interpolator based on S-shaped feedrate scheduling | |
| KR100664681B1 (ko) | 매개변수곡선으로 기술된 로봇 경로의 샘플 테이터를이용한 직접보간 시스템 및 방법 | |
| Ke et al. | Tangential velocity tracking-based task coordinate frame approach for contouring control of biaxial motion systems | |
| Lam et al. | Multi-axis model predictive contouring control | |
| CN114019911B (zh) | 一种基于速度规划的曲线拟合方法 | |
| CN113486523B (zh) | 一种线性变参数振动系统全局辨识方法 | |
| Zhang et al. | Development of an adaptive-feedrate planning and iterative interpolator for parametric toolpath with normal jerk constraint | |
| CN113031510B (zh) | 面向复杂多轴系统的b样条曲线计算加速方法 | |
| Zhang et al. | Time-optimal and Smooth Trajectory Planning for Multi-axis Motion Systems Based on ISC Similarity |
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 |