CN116577994A - 冗余容错式非对称s型柔性速度曲线自适应规划方法 - Google Patents

冗余容错式非对称s型柔性速度曲线自适应规划方法 Download PDF

Info

Publication number
CN116577994A
CN116577994A CN202310747380.4A CN202310747380A CN116577994A CN 116577994 A CN116577994 A CN 116577994A CN 202310747380 A CN202310747380 A CN 202310747380A CN 116577994 A CN116577994 A CN 116577994A
Authority
CN
China
Prior art keywords
max
planning
section
speed
curve
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
CN202310747380.4A
Other languages
English (en)
Other versions
CN116577994B (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.)
Yingfukang Zhejiang Industrial Technology Co ltd
Original Assignee
Yingfukang Zhejiang Industrial Technology Co ltd
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 Yingfukang Zhejiang Industrial Technology Co ltd filed Critical Yingfukang Zhejiang Industrial Technology Co ltd
Priority to CN202310747380.4A priority Critical patent/CN116577994B/zh
Publication of CN116577994A publication Critical patent/CN116577994A/zh
Application granted granted Critical
Publication of CN116577994B publication Critical patent/CN116577994B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
    • 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
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/60—Other road transportation technologies with climate change mitigation effect
    • Y02T10/72—Electric energy management in electromobility

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)

Abstract

本方案公开了一种冗余容错式非对称S型柔性速度曲线自适应规划方法，在系统故障，在速度参数和加速度参数等设置不合理时，能够根据不同的规划参数进行自适应变结构曲线规划，保证系统在出现故障的情况下可靠、安全、高速、高效的运行，满足高可靠性、高安全性、高鲁棒性的控制要求。在加减速控制中对运动方向进行归一处理实现双向曲线规划。加速过程和减速过程分开控制，实现非对称的曲线规划，同时，整个规划过程中保证加速度的连续性，规划分类少，规划参数易求取，以保证无柔性冲击，满足高灵活性、高精度的控制要求，又能保证算法实现简单，实现高速、高效率的控制要求，提升控制系统的运行速度和加工效率。

Description

冗余容错式非对称S型柔性速度曲线自适应规划方法
技术领域
本发明属于工业控制技术领域，尤其是涉及一种冗余容错式非对称S型柔性速度曲线自适应规划方法。
背景技术
工业控制即工业自动化是高端装备制造业的支柱产业，其中，可编程逻辑控制器（PLC）是现代工业自动化的三大支柱之一，给各种自动化控制设备提供更高效、更高速、更灵活、更可靠的控制，广泛应用于冶金、石油化工、电力、交通运输、机械制造、环保等领域。运动控制作为现代PLC不可或缺的功能模块之一，主要应用于数控机床、机器人、轮胎、造纸、光伏、集成电路、家电等行业。在实际加工制造中，为了避免和减小设备在启停时产生冲击、振动，需要对设备驱动轴的运行速度进行加减速控制，以提高加工精度和产品质量。因此，满足高速、高效、高灵活、高精度、高可靠要求的柔性加减速控制是现代高性能运动控制系统的关键技术之一。
传统的梯形和指数型加减速控制方法虽实现简单，但在加减速过程中存在加加速度脉冲，加速度阶跃突变的问题，由于加速度的不连续性，故存在较大的柔性冲击，特别是高速运行以及一些惯性负载和弹性机构时，会产生非期望的结果，降低产品的加工精度以及减小设备自身的使用寿命，仅适用于对控制精度较低的应用场合，难以满足高精度的控制要求。为了保证设备在加减速过程中加速度的连续性，减小系统的柔性冲击，提升加工精度，满足高速、高精度的控制要求，通常采用S型柔性加减速控制方法。常用的S型柔性加减速方法有基于三角函数的加减速控制、基于多项式的加减速控制和七段式加减速控制。基于三角函数的S曲线加减速控制方法，通过复杂的三角函数计算使运动曲线平滑，但在嵌入式系统上运行需要大量资源，计算耗时，无法保证嵌入式运控系统的实时性。基于多项式的加减速控制，虽能获得平滑的曲线，但需对时间进行连续高阶求导，阶数越高，曲线平滑度越大，但同时曲线的约束条件也越多，规划参数求解也更复杂，对应所需的最大速度、加速度和加加速度也越大，而实际中由于驱动轴电机的物理特性，所能提供的转速和扭矩都有其上限。因此，这两种柔性加减速控制方法较少应用于实际的运动控制系统中。七段式S型加减速控制通过将加减速过程分为加加速段、恒加速段、减加速段、恒速段、加减速段、恒减速段、减减速段共7个阶段，来避免在启动、运行和停止时加加速的脉冲点，保证在整个加减速过程中加速度的连续，来减小系统的柔性冲击，实际运动控制系统中应用较多。但是，七段式S型加减速控制也存在规划分类情况多，需通过繁杂的不等式判断来确定加减速边界等问题。并且，需要通过多次开方以及二分法和牛顿迭代法等数值分析方法才可求得某些条件的规划参数，运算复杂且计算量大，基于迭代的求解方法计算量会随着控制精度的提升而大幅增加，耗时长，难以满足高效率的控制要求，在某些极端情况，超出迭代求解次数时甚至会导致系统死机、不稳定等严重后果。有学者提出一种基于三次多项式的减速度控制方法，在规划分类情况较少，规划参数的计算只涉及少量的四则运算的情况下，能始终保证在整个运行过程中加速度的连续性，减小系统的柔性冲击，提升设备加工的精度，算法实现简单、便捷，可以实现运行曲线的快速规划，提升系统运行效率。但是，该算法的加速和减速过程存在耦合，没有将加速和减速分开控制，加速和减速过程是对称的关系，这将导致运行系统不管在哪种工况下进行加减速控制驱动轴电机只能输出相同的扭矩，无法完全利用驱动轴电机的特性，S曲线规划的灵活性受到了大大的限制，难以满足多场合全工况的规划与控制，未对加速和减速过程分开控制的非对称式的加减速控制做进一步的推导和研究。目前公开的关于“柔性加减速控制”的技术和资料，大多是在规划参数设置合理的基础上进行曲线规划，要求最大速度必须大于等于初速度和末速度才能进行曲线规划，而在实际运动控制系统，一方面，由于人为操作失误，可能会误输入不合理的规划参数，如规划最大速度小于初速度和末速度；另一方面，由于受设备零件老化损坏或电磁干扰的影响，可能会造成参数数据在通信传输中出错，如导致规划最大速度为0，这种故障是无法预知，也是不可避免的。不管是哪种原因导致的规划参数不合理，如果不进行有效的故障处理，在故障情况下不进行有效的曲线规划，极有可能会造成停机、碰撞、飞车、失控、设备损坏等生产事故，甚至会造成不可估量的严重后果。中国专利[CN112327954B]“非对称S型速度曲线控制的直线电机高精定位方法”在出现规划参数不合理，不满足规划的条件下，需要重设初始参数，直到参数设置合理为止。对于如光伏、锂电、芯片等有成百上千道工序的行业，有大量的点对点运动，如按照此方法进行曲线规划，需要大量的调试时间，易用性较差，增加了产品生产的时间成本，难以满足高效率的控制要求。而且，对于不可控故障导致的规划参数异常没有有效的规划处理，难以保证高可靠性的控制要求。中国专利[CN108153246B]“一种基于指定速度的参数自适应S形速度规划插补方法”在七段式S型加减速控制的基础上，通过判断规划位移与目标位移的关系以及最大速度与初速度、末速度的关系来保证部分参数不合理时的曲线规划。但是，该方法分类情况多达23种，算法复杂；加速段和减速段参数相同，是一种对称的曲线规划，加速和减速过程未分开控制，大大降低了S曲线使用的灵活性；当最大速度小于初速度和末速度时，需采用迭代求解的数值方法来求取新的最大速度，运算复杂，计算量大，耗时长，难以满足高效率的控制要求；仅考虑了最大速度、初速度、末速度设置不合理的规划处理，未考虑加速度设置不合理的规划处理，最大速度和最大加速度的规划设置未考虑驱动轴电机的物理特性上限，且未对最大速度为0进行有效规划处理；仅对目标位置为正时进行曲线规划，当目标位置为负时没有进行有效的规划处理；在某些情况下，会调整预设的初速度和末速度，而实际生产加工中会存在大量点对点运动，不同指令预设的速度参数不同，这种初、末速度的自调整会导致驱动轴电机的速度跳变，影响系统运行的连续性，降低设备的加工精度。一种在运动控制系统出现故障，规划参数设置不合理时，仍然能保证系统进行高效、高速、高精度、高灵活性、高可靠性、高安全性的曲线规划，目前关于加减速控制的技术和资料还没有公开的转化方法。因此，亟待一种在系统故障，规划参数设置不合理时，既能保证加速度连续，无柔性冲击，满足高速、高精度的要求，又能保证算法实现简单，满足高效率、高灵活性、高可靠性、高安全性要求的冗余容错式非对称S型柔性加减速控制方法。
发明内容
针对现有技术的不足，本发明提出一种冗余容错式非对称S型柔性速度曲线自适应规划方法，在系统故障，在速度参数和加速度参数等设置不合理时，能够根据不同的规划参数进行自适应变结构曲线规划，保证系统在出现故障的情况下可靠、安全、高速、高效的运行，满足高可靠性、高安全性、高鲁棒性的控制要求。在加减速控制中对运动方向进行归一处理实现双向曲线规划。加速过程和减速过程分开控制，实现非对称的曲线规划，同时，整个规划过程中保证加速度的连续性，规划分类少，规划参数易求取，以保证无柔性冲击，满足高灵活性、高精度的控制要求，又能保证算法实现简单，实现高速、高效率的控制要求，提升控制系统的运行速度和加工效率。
一种冗余容错式非对称S型柔性速度曲线自适应规划方法，该方法包括：
S1.输入位置运动指令，给定初始参数条件：目标位移S t 、初速度V s 、最大速度V max、末速度V e 、最大加速度A max、最大减速度D max；
S2.将运动方向归一成正向后进行后续的曲线规划；
S3.当V max≤V t 且V max≠0，A max（D max）≤A t 且A max≠0时执行步骤S4，
A t 表示驱动轴电机所能提供的最大加速度，
V t 表示驱动轴电机所能提供的最高速度；
S4.判断V max与V s 、V e 的大小关系，
若V max≥V s 且V max≥V e 则进行常规S曲线规划，否则进行冗余容错自适应变结构速度曲线规划：
若V max>V s 且V max<V e ，则令容错最大加速度A ft=A max，进行V s 加速至V max、（V max匀速）、V max加速至V e 的冗余容错变结构速度曲线规划；
若V max<V s且V max>V e ，则令容错最大减速度D ft=D max，进行V s 减速至V max、（V max匀速）、V max减速至V e 的冗余容错变结构速度曲线规划；
若V max<V s 且V max<V e ，则令容错最大减速度D ft=D max，容错最大加速度A ft=A max，进行V s 减速至V max、（V max匀速）、V max加速至V e 的冗余容错变结构速度曲线规划；
S5.对规划参数进行符号处理，以实现正负方向均可进行曲线规划；
S6.将各分段始末运动规划参数及分段规划时间通过控制周期T s 进行曲线离散插补输出。
在上述的冗余容错式非对称S型柔性速度曲线自适应规划方法中，步骤S2具体包括：
处理运动方向，若目标位移S t <0，则将S t 、V s 、V e 进行转换，归一成正向进行曲线规划：
令 （1）
得： （2）
步骤S5具体包括：
若目标位移S t 为正，则将得到的各分段始末运动规划参数直接输出；若目标位置S t 为负，则对运动规划参数进行符号转换处理，以实现负方向的曲线规划，具体如下：
（23）。
在上述的冗余容错式非对称S型柔性速度曲线自适应规划方法中，步骤S3具体包括：
判断V max是否为0，如果V max=0，则进行V s 减至0的冗余容错变结构速度曲线规划，计算对应的位移S 0，以及本次指令剩余未完成的位移S rem，并将S rem送入参数初始化，等待下一条指令的执行；
如果V max≠0，则判断V max是否小于驱动轴电机所能提供的最高速度V t ；
如果V max>V t ，则令V max=V t，限制规划最大速度不超过电机的物理特性上限；
如果V max≤V t ，则判断A max（D max）是否等于0；
如果A max（D max）=0，则令A max=A lastmax（D max=D lastmax），将上一次指令的加（减）速度A lastmax（D lastmax）赋给A max（D max）；
如果A max（D max）≠0，则判断A max（D max）是否小于驱动轴电机所能提供的最大加速度A t ；
如果A max（D max）>A t ，则令A max（D max）=A t ，限制规划最大加（减）速度不超过电机的物理特性上限；
如果A max（D max）≤A t ，则执行步骤S4。
在上述的冗余容错式非对称S型柔性速度曲线自适应规划方法中，步骤S4中，若V max≥V s 且V max≥V e ，则进行加速段、（匀速段）、减速段的常规S曲线规划。
在上述的冗余容错式非对称S型柔性速度曲线自适应规划方法中，步骤S4中，若V max>V s 且V max<V e ，则进行V s 加速至V max、（V max匀速）、V max加速至V e 的冗余容错变结构速度曲线规划：
令容错最大加速度A ft=A max，计算从V s 运动至V e 的单段加速位移S a0；
若S t >S a0，则冗余容错变结构S曲线轨迹规划可行；若S t ≤S a0，则根据S t 对A ft进行修正，并求取加速时间T a0，进行单段加速特殊曲线规划。
在上述的冗余容错式非对称S型柔性速度曲线自适应规划方法中，针对V max>V s 且V max<V e 的情况计算S曲线规划的参数，定义：
J sk ：第k段的初加加速度；
A sk ：第k段的初加速度；
A ek ：第k段的末加速度；
V sk ：第k段的初速度；
V ek ：第k段的末速度；
S k ：第k段的位移；
其中,k=1,2,3…；
存在两种可能性，有匀速段T u >0，无匀速段T u =0；
当T u >0时，加速段1时间为T a1，匀速段时间为T u ，加速段2时间为T a2，然后，计算相应始末运动规划参数如下：
第一段，即加速段1，由V s 加速运动至V max，加速时间为T a1，此时，k=1，第一段始末运动规划参数如下：
（19）
第二段，即匀速段，保持V max匀速运动，匀速时间为T u ，此时，k=2，且V s2=V e1，A s2=A e1，第二段始末运动规划参数如下：
（20）
第三段，即加速段2，由V max加速运动至V e ，加速时间为T a2，此时，k=3，V s3=V e2，A s3=A e2，第三段始末运动规划参数如下：
（21）
当T u ≤0时，单加速段时间为T a0，计算相应始末运动规划参数如下：
由V s 加速运动至V e ，加速时间为T a0，此时，k=1，相应始末运动规划参数如下：
（22）。
在上述的冗余容错式非对称S型柔性速度曲线自适应规划方法中，若Vmax<V s且V max>V e ，则进行V s 减速至V max、（V max匀速）、V max减速至V e 的冗余容错变结构速度曲线规划：
令容错最大减速度D ft=D max，计算从V s 运动至V e 的单段减速位移S d0；
若S t >S d0，则冗余容错变结构S曲线轨迹规划可行；若S t ≤S d0，则需要根据S t 对D ft进行修正，并求取减速时间T d0，进行单段减速特殊曲线规划。
在上述的冗余容错式非对称S型柔性速度曲线自适应规划方法中，针对Vmax<V s且V max>V e 的情况计算S曲线规划的参数，存在两种可能性：
当T u >0时，有匀速段，减速段1时间为T d1，匀速段时间为T u ，减速段2时间为T d2，然后，计算相应始末运动规划参数如下：
第一段，即减速段1，由V s 减速运动至V max，减速时间为T d1，此时，k=1，第一段始末运动规划参数如下：
（37）
第二段，即匀速段，保持V max匀速运动，匀速时间为T u ，此时，k=2，且V s2=V e1，A s2=A e1，第二段始末运动规划参数如下：
（38）
第三段，即减速段2，由V max减速运动至V e ，减速时间为T d2，此时，k=3，V s3=V e2，A s3=A e2，第三段始末运动规划参数如下：
（39）
当T u ≤0时，无匀速段
单减速段时间为T d0，计算相应始末运动规划参数如下：
由V s 减速运动至V e ，减速时间为T d0，此时，k=1，相应始末运动规划参数如下：
（40）。
在上述的冗余容错式非对称S型柔性速度曲线自适应规划方法中，若Vmax<V s 且V max<V e ，则进行V s 减速至V max、（V max匀速）、V max加速至V e 的冗余容错变结构速度曲线规划：
令容错最大减速度D ft=D max，容错最大加速度A ft=A max，根据V s 与V e 的大小关系，计算相应的单段加（减）位移，判断是否可以进行S曲线规划，具体如下：
若V s <V e ，计算从V s 运动至V e 的单段加速位移S a0，若S t >S a0，则冗余容错变结构S曲线轨迹规划可行；若S t ≤S a0，则对A ft进行修正并求取加速时间T a0，进行单段加速曲线特殊规划，此时，通过式（22）计算相应始末运动规划参数；
若V s =V e ，不存在单加速段或单减速段的情况，始终满足冗余容错变结构S曲线规划的条件；
若V s >V e ，计算从V s 运动至V e 的单段减速位移S d0，若S t >S d0，则冗余容错变结构S曲线轨迹规划可行；若S t ≤S d0，对D ft进行修正，并求取减速时间T d0，进行单段减速特殊曲线规划，此时，通过式（40）计算相应始末运动规划参数。
针对Vmax<V s且V max<V e 的情况，若满足冗余容错变结构S曲线规划的条件，计算S曲线规划的参数存在两种可能性：
当T u >0时，有匀速段，减速段1时间为T d1，匀速段时间为T u ，加速段1时间为T a1，然后，计算相应始末运动规划参数如下：
第一段，即减速段1，由V s 减速运动至V max，减速时间为T d1，此时，k=1，第一段始末运动规划参数如下：
（46）
第二段，即匀速段，保持V max匀速运动，匀速时间为T u ，此时，k=2，且V s2=V e1，A s2=A e1，第二段始末运动规划参数如下：
（47）
第三段，即加速段1，由V max加速运动至V e ，加速时间为T a1，此时，k=3，V s3=V e2，A s3=A e2，第三段始末运动规划参数如下：
（48）
当T u ≤0时，无匀速段，减速段1时间为T d1，加速段1时间为T a1，然后，计算相应始末运动规划参数如下：
第一段，即减速段1，由V s 减速运动至中间过渡速度V g ，减速时间为T d1，此时，k=1，第一段始末运动规划参数如下：
（49）
第二段，即加速段1，由V g 加速运动至V e ，加速时间为T a1，此时，k=2，V s2=V e1，A s2=A e1，第二段始末运动规划参数如下：
（50）。
在上述的冗余容错式非对称S型柔性速度曲线自适应规划方法中，步骤S6具体包括：
当V max>V s 且V max<V e 时的曲线离散插补输入如下：
当T u >0时，曲线规划为加速段1、匀速段、加速段2；
当nT s [0,T a1]时，i=nT s /T a1，其中，n=1,2,3…为正整数，根据加速段1的始末运动规划参数进行曲线离散插补输出；
当nT s [T a1,T a1+T u ]时，i= (nT s -T a1) /T u ，根据加速段1、匀速段的始末运动规划参数进行曲线离散插补输出；
当nT s [T a1+T u ,T a1+T u +T a2]时，i= (nT s -T a1-T u )/T a2，根据加速段1、匀速段和加速段2的始末运动规划参数进行曲线离散插补输出；
当T u ≤0，曲线规划为单段加速特殊曲线规划，当nT s [0,T a0]时，i=nT s /T a0，其中，n=1,2,3…为正整数，根据相应始末运动规划参数进行曲线离散插补输出；
Vmax<V s且V max>V e 和Vmax<V s且V max<V e 时的曲线离散插补输出参照V max>V s 且V max<V e 时的方法即可。
本发明的优点在于：
1. 在系统故障，速度参数和加速度参数等设置不合理时，能够根据不同的规划参数进行自适应变结构曲线规划，保证系统在出现故障的情况下可靠、安全、高速、高效的运行，以满足高可靠性、高安全性、高鲁棒性的控制要求；
2. 对加速段和减速段进行分开规划控制，实现非对称的曲线规划，提升S曲线加减速控制的灵活性，满足不同工况的规划与控制；
3.本方案曲线规划分类简明、分类情况少，规划参数求取只涉及少量的四则运算，计算简单、便捷，满足高效率、高速的控制要求，提升控制系统的运行速度和加工效率；
4. 在加减速控制中对运动方向进行归一处理实现双向曲线规划，提高系统资源空间利用率，整个规划过程中加速度始终保持连续，以保证无柔性冲击，满足高可靠、高精度的控制要求；
5. 本发明能够适用于轮胎、造纸、光伏、集成电路、家电等行业的运动控制系统。
附图说明
图1为冗余容错式非对称S型柔性速度曲线自适应规划方法总体流程图；
图2为V max>V s 且V max<V e 时的冗余容错变结构S曲线规划流程图；
图3为V max<V s 且V max>V e 时的冗余容错变结构S曲线规划流程图；
图4为V max<V s 且V max<e时的冗余容错变结构S曲线规划流程图；
图5为V max=0时的冗余容错变结构S曲线规划仿真波形图；
图6为V max>V s 且V max<V e 时的冗余容错变结构S曲线规划仿真波形图；
图7为V max<V s 且V max>V e 时的冗余容错变结构S曲线规划仿真波形图；
图8为V max<V s 且V max<V e 时的冗余容错变结构S曲线规划仿真波形图。
具体实施方式
下面结合附图和具体实施方式对本发明做进一步详细的说明。
如图1所示，首先，输入位置运动指令，给定初始参数条件：目标位移S t 、初速度V s 、最大速度V max、末速度V e 、最大加速度A max、最大减速度D max。然后，对运动方向进行处理，实现正、负双向的曲线规划。
接着，判断V max是否为0，如果V max=0，则进行V s 减至0的冗余容错变结构速度曲线规划，计算对应的位移S 0，以及本次指令剩余未完成的位移S rem，并将S rem送入参数初始化，等待下一条指令的执行；
如果V max≠0，则判断V max是否小于驱动轴电机所能提供的最高速度V t 。如果V max>V t ，则令V max=V t，限制规划最大速度不超过电机的物理特性上限；如果V max≤V t ，则判断A max（D max）是否等于0。
如果A max（D max）=0，则令A max=A lastmax（D max=D lastmax），将上一次指令的加（减）速度A lastmax（D lastmax）赋给A max（D max）；如果A max（D max）≠0，则判断A max（D max）是否小于驱动轴电机所能提供的最大加速度A t 。
如果A max（D max）>A t ，则令A max（D max）=A t ，限制规划最大加（减）速度不超过电机的物理特性上限；如果A max（D max）≤A t ，则判断V max与V s 、V e的大小关系。
如果V max≥V s 且V max≥V e ，则根据V s 、V e 的大小关系以及单加速位移S a0、单减速位移S d0与目标位移S t 的大小关系，判断是否可以进行S曲线规划，如果满足S曲线规划条件，则进行加速段、（匀速段）、减速段进行S曲线规划；
否则，进行冗余容错自适应变结构速度曲线规划，如果V max>V s 且V max<V e ，故障以及人为设置参数不合理的情况下可能出现这种情况，则令容错最大加速度A ft=A max，进行V s 加速至V max、（V max匀速）、V max加速至V e 的冗余容错变结构速度曲线规划；如果Vmax<V s且V max>V e ，令容错最大减速度D ft=D max，进行V s 减速至V max、（V max匀速）、V max减速至V e 的冗余容错变结构速度曲线规划；如果Vmax<V s 且V max<V e ，令容错最大减速度D ft=D max，容错最大加速度A ft=A max，进行V s 减速至V max、（V max匀速）、V max加速至V e 的冗余容错变结构速度曲线规划。
接着，对规划参数进行符号处理，最后，通过将曲线规划时间离散实现曲线的插补输出。
上述方法的具体过程如下：
1、输入位置运动指令，给定初始参数条件：目标位移S t 、初速度V s 、最大速度V max、末速度V e 、最大加速度A max、最大减速度D max。其中，V max、A max、D max始终为大于0的实数。
2、处理运动方向，若目标位移S t <0，即运行方向为负向，则需要将S t 、V s 、V e 进行转换，归一成正向进行曲线规划，具体如下：
令 （1）
得： （2）
3、定义T h 为加速或减速过程所需的时间，i=t/T h ，其中，t [0,T h ]。位置、速度、加速度、加加速度关于i(t)的一般表达式为/>
（3）
其中，b 0-b 4为常系数。
在加速或减速过程中需满足如下边界条件：
（4）
将式（4）的边界条件代入式（3）求得位置、速度、加速度、加加速度关于i的函数表达式如下：
（5）
令A r （D r ）为加（减）速过程中的实时加（减）速度。由式（5）可知，当i=0.5时，加速度绝对值达到整个加（减）度过程中的最大值|A rmax|，故加（减）速时间T h 与最大加速度绝对值|A rmax|满足以下关系：
（6）
令i=1，把式（6）代入式（3）的S(i)表达式可得加（减）速段的位移为：
（7）
4、判断V max是否为0，如果V max=0，无法进行正常的S曲线规划，此时，进行V s 减至0的冗余容错变结构速度曲线规划，计算对应的位移S 0，将D max代入式（6）-（7）可得S 0的表达式如下：
（8）
根据式（8），可计算得本次指令剩余未完成的位移S rem如下：
（9）
将S rem送入参数初始化，等待下一次指令执行。如果V max≠0，则进入步骤5。
5、判断V max是否小于驱动轴电机所能提供的最高速度V t ，如果V max>V t ，则令V max=V t ，限制规划最大速度不超过电机的物理特性上限。如果V max≤V t ，则进入步骤6。
6、判断A max（D max）是否为0，如果A max（D max）=0，则令A max=A lastmax（D max=D lastmax），利用上一次指令的加（减）速度A lastmax（D lastmax）进行冗余容错曲线规划。如果A max（D max）≠0，则进入步骤7。
7、判断A max是否小于驱动轴电机所能提供的最大加速度A t ，如果A max>A t ，则令A max=A t ，限制规划最大加速度不超过电机的物理特性上限。如果A max≤A t ，则进入步骤8。
8、判断V max与V s 、V e 的大小关系，如果V max≥V s 且V max≥V e ，则进行加速段、（匀速段）、减速段的常规S曲线规划，此规划与现有技术一致，这里不做赘述。如果不同时满足V max≥V s 且V max≥V e ，则进入步骤9。
9、若不能同时满足V max≥V s 且V max≥V e ，无法进行常规的S曲线规划，此时，需要进行冗余容错自适应变结构速度曲线规划。
如果V max>V s 且V max<V e ，则进行V s 加速至V max、（V max匀速）、V max加速至V e 的冗余容错变结构速度曲线规划。V max>V s ，V max<V e 时的冗余容错变结构S曲线规划流程图，如图2所示。首先，令容错最大加速度A ft=A max，计算从V s 运动至V e 的单段加速位移S a0，将A ft代入式（7）计算可得S a0如下：
（10）
若S t >S a0，则冗余容错变结构S曲线轨迹规划可行；若S t ≤S a0，则需要根据S t 对A ft进行修正，具体如下：
（11）
根据式（6）和（11）求取加速时间T a0，进行单段加速特殊曲线规划。
10、若步骤9中S t >S a0，满足冗余容错变结构S曲线规划的条件，则进一步计算S曲线规划的参数。存在两种可能性：
（10.1）情况1，有匀速段
首先假设满足情况1。据式（6）、（7）可得从V s 至V max的加速段1的时间和位移为
（12）
（13）
从V max至V e 的加速段2的时间和位移为
（14）
（15）
进一步，可求得匀速段的运行时间为
（16）
若T u >0,则存在匀速段，上述的式（12）、（14）和（16）所求得时间参数可用于加速段1、匀速段和加速段2中各分段始末运动规划参数的计算；否则，T u ≤0，这表明无匀速段，此时需要进行情况2，无匀速段时的运动规划参数及运动规划时间的计算求解。
（10.2）情况2，无匀速段
当情况2，无匀速段（T u ≤0）。此时，从V s 经过加速段1到达的中间过渡速度V g 与S t 满足以下关系：
（17）
式（17）化简后为
（18）
此时，参照式（6）和（11），根据S t 对A ft进行修正，并求取加速时间T a0，进行单段加速特殊曲线规划。
S t ≤S a0时的始末运动规划参数可以直接用式（22）。
11、由第10步可知，当T u >0时，则存在匀速段，当T u ≤0时，则不存在匀速段。
定义：
J sk ：第k段的初加加速度；
A sk ：第k段的初加速度；
A ek ：第k段的末加速度；
V sk ：第k段的初速度；
V ek ：第k段的末速度；
S k ：第k段的位置；
其中,k=1,2,3…
（11.1）当T u >0时，则存在匀速段，加速段1时间为T a1，匀速段时间为T u ，加速段2时间为T a2。然后，可计算出相应始末运动规划参数如下：
第一段，即加速段1，由V s 加速运动至V max，加速时间为T a1。此时，k=1，相应始末运动规划参数如下：
（19）
第二段，即匀速段，保持V max匀速运动，匀速时间为T u 。此时，k=2，由于需保持曲线的连续性，故V s2=V e1，A s2=A e1。第二段始末运动参数如下：
（20）
第三段，即加速段2，由V max加速运动至V e ，加速时间为T a2。此时，k=3，由于需保持曲线的连续性，故V s3=V e2，A s3=A e2。第三段始末运动参数如下：
（21）
（11.2）当T u ≤0时，则不存在匀速段，单加速段时间为T a0，可计算出相应始末运动规划参数如下：
由V s 加速运动至V e ，加速时间为T a0。此时，k=1，相应始末运动规划参数如下：
（22）
以上对运动规划时间及运动规划参数的求解仅涉及到简单四则运算，无需复杂的迭代求解，且规划分类情况少，可实现曲线的快速规划。
12、若目标位移S t 为正，第11步得到的各分段始末运动规划参数直接输出；若目标位置S t 为负，应对运动规划参数进行符号转换处理，以实现负方向的曲线规划，具体如下：
（23）
13、依据第9-12步得到的各分段始末运动规划参数及分段规划时间通过控制周期T s 进行曲线离散插补输出，具体如下：
（13.1）当T u >0，有匀速段时，曲线规划为加速段1、匀速段、加速段2。
（13.1.1）当nT s [0,T a1]时，i=nT s /T a1，其中，n=1,2,3…为正整数。据式（5）和式（19）可得，曲线离散化输出表达式为
（24）
（13.1.2）当nT s [T a1,T a1+T u ]时，i= (nT s -T a1)，据式（19）和式（20）可得，曲线离散化输出表达式为
（25）
（13.1.3）当nT s [T a1+T u ,T a1+T u +T a2]时，i= (nT s -T a1-T u )/T a2，据式（5）、式（19）-（21）可得，曲线离散化输出表达式为
（26）
（13.2）当T u ≤0，无匀速段，曲线规划为单段加速特殊曲线规划。当nT s [0,T a0]时，i=nT s /T a0，其中，n=1,2,3…为正整数。据式（5）和式（22）可得，曲线离散化输出表达式为
（27）
14、如果V max<V s 且V max>V e ，则进行V s 减速至V max、（V max匀速）、V max减速至V e 的冗余容错变结构速度曲线规划。V max<V s ，V max>V e 时的冗余容错变结构S曲线规划流程图，如图3所示。首先，令容错最大减速度D ft=D max，计算从V s 运动至V e 的单段减速位移S d0，将D ft代入式（7）计算可得S d0如下：
（28）
若S t >S d0，则冗余容错变结构S曲线轨迹规划可行；若S t ≤S d0，则需要根据S t 对D ft进行修正，具体如下：
（29）
根据式（6）和（29）求取减速时间T d0，进行单段减速特殊曲线规划。
15、若步骤14中满足冗余容错变结构S曲线规划的条件，则进一步计算S曲线规划的参数。存在两种可能性：
（15.1）情况1，有匀速段
首先假设满足情况1。据式（6）、（7）可得从V s 至V max的减速段1的时间和位移为
（30）
（31）
从V max至V e 的减速段2的时间和位移为
（32）
（33）
进一步，可求得匀速段的运行时间为
（34）
若T u >0,则存在匀速段，上述的式（30）、（32）和（34）所求得时间参数可用于减速段1、匀速段和减速段2中各分段始末运动规划参数的计算；否则，T u ≤0，这表明无匀速段，此时需要进行情况2，无匀速段时的运动规划参数及运动规划时间的计算求解。
（15.2）情况2，无匀速段
当情况2，无匀速段（T u =0）。此时，从V s 经过减速段1到达的中间过渡速度V m 与S t 满足以下关系：
（35）
式（35）化简后为
（36）
此时，参照式（6）和（29），根据S t 对D ft进行修正，并求取减速时间T d0，进行单段减速特殊曲线规划。
16、当Vmax<V s且V max>V e 时计算S曲线各分段运动规划参数，存在两种可能性：
（16.1）当T u >0时，有匀速段，减速段1时间为T d1，匀速段时间为T u ，减速段2时间为T d2，然后，计算相应始末运动规划参数如下：
第一段，即减速段1，由V s 减速运动至V max，减速时间为T d1，此时，k=1，第一段始末运动规划参数如下：
（37）
第二段，即匀速段，保持V max匀速运动，匀速时间为T u ，此时，k=2，且V s2=V e1，A s2=A e1，第二段始末运动规划参数如下：
（38）
第三段，即减速段2，由V max减速运动至V e ，减速时间为T d2，此时，k=3，V s3=V e2，A s3=A e2，第三段始末运动规划参数如下：
（39）
（16.2）当T u ≤0时，无匀速段
单减速段时间为T d0，计算相应始末运动规划参数如下：
由V s 减速运动至V e ，减速时间为T d0，此时，k=1，相应始末运动规划参数如下：
（40）。
17、参照步骤12，对运动规划参数进行符号处理。
18、参照步骤13，进行曲线规划时间离散并插补输出。
19、如果V max<V s 且V max<V e ，则进行V s 减速至V max、（V max匀速）、V max加速至V e 的冗余容错变结构速度曲线规划。V max<V s ，V max<V e 时的冗余容错变结构S曲线规划流程图，如图4所示。首先，令容错最大减速度D ft=D max，容错最大加速度A ft=A max。根据V s 与V e 的大小关系，计算相应的单段加（减）位移，判断是否可以进行S曲线规划，具体如下：
（19.1）V s <V e
参照式（10）计算从V s 运动至V e 的单段加速位移S a0。若S t >S a0，则冗余容错变结构S曲线轨迹规划可行；若S t ≤S a0，则参照式（11）对A ft进行修正并求取加速时间T a0，进行单段加速曲线特殊规划，此时，通过式（22）计算始末运动规划参数。
（19.2）V s =V e
若V s =V e ，不存在单加速段或单减速段的情况，始终满足冗余容错变结构S曲线规划的条件。
（19.3）V s >V e
参照式（28）计算从V s 运动至V e 的单段减速位移S d0，若S t >S d0，则冗余容错变结构S曲线轨迹规划可行；若S t ≤S d0，参照式（29）对D ft进行修正，并取减速时间T d0，进行单段减速特殊曲线规划，此时，通过式（40）计算始末运动规划参数。
20、若步骤19中满足冗余容错变结构S曲线规划的条件，则进一步计算S曲线规划的参数。存在两种可能性：
（20.1）情况1，有匀速段
首先假设满足情况1。参照式（30）、（31）计算从V s 至V max的减速段1的时间T d1和位移S d1 。参照式（14）、（15）计算从V max至V e 的加速段1的时间T a1和位移S a1。
进一步，可求得匀速段的运行时间为
（41）
若T u >0,则存在匀速段，上述所求的时间参数T d1、T u 、T a1 可用于减速段1、匀速段和加速段1中各分段始末运动规划参数的计算；否则，T u ≤0，这表明无匀速段，此时需要进行情况2，无匀速段时的运动规划参数及运动规划时间的计算求解。
（20.2）情况2，无匀速段
当情况2，无匀速段（T u ≤0）。此时，从Vs经过减速段1到达的中间过渡速度V g 与S t 满足以下关系：
（42）
由式（38）可求得中间过渡速度V g 为
（43）
据式（39）可计算出加速段时间T a1和减速段时间T d1如下：
（44）
（45）
21、当Vmax<V s且V max<V e 时，若满足冗余容错变结构S曲线规划的条件，计算S曲线规划的参数存在两种可能性：
（21.1）当T u >0时，有匀速段，减速段1时间为T d1，匀速段时间为T u ，加速段1时间为T a1，然后，计算相应始末运动规划参数如下：
第一段，即减速段1，由V s 减速运动至V max，减速时间为T d1，此时，k=1，第一段始末运动规划参数如下：
（46）
第二段，即匀速段，保持V max匀速运动，匀速时间为T u ，此时，k=2，且V s2=V e1，A s2=A e1，第二段始末运动规划参数如下：
（47）
第三段，即加速段1，由V max加速运动至V e ，加速时间为T a1，此时，k=3，V s3=V e2，A s3=A e2，第三段始末运动规划参数如下：
（48）
（21.2）当T u ≤0时，无匀速段，减速段1时间为T d1，加速段1时间为T a1，然后，计算相应始末运动规划参数如下：
第一段，即减速段1，由V s 减速运动至中间过渡速度V g ，减速时间为T d1，此时，k=1，第一段始末运动规划参数如下：
（49）
第二段，即加速段1，由V g 加速运动至V e ，加速时间为T a1，此时，k=2，V s2=V e1，A s2=A e1，第二段始末运动规划参数如下：
（50）。
22、参照步骤12，对运动规划参数进行符号处理。
23、参照步骤13，进行曲线规划时间离散并插补输出。
为了说明本发明提出冗余容错式非对称S型柔性速度曲线自适应规划方法，在系统故障，速度参数和加速度参数等设置不合理时，能够根据不同的规划参数进行自适应变结构曲线规划，保证系统在出现故障的情况下可靠、安全、高速、高效的运行，满足高可靠性、高安全性、高鲁棒性的控制要求。在加减速控制中对运动方向进行归一处理实现双向曲线规划。加速过程和减速过程分开控制，实现非对称的曲线规划，同时，整个规划过程中保证加速度的连续性，规划分类少，规划参数易求取，以保证无柔性冲击，满足高灵活性、高精度的控制要求，又能保证算法实现简单，实现高速、高效率的控制要求，提升控制系统的运行速度和加工效率。现对其进行虚轴仿真实验验证。
（1）V max=0时的仿真验证
仿真中所用位置运动指令1、2的参数如表1所示：
表1
图5为V max=0时的冗余容错变结构S曲线规划仿真波形图。由图5可知，当t=0.0s时，系统速度为0.0u•s -1，系统位置为0.0u，此时，输入位置运动指令1，由表1可知，给定目标位置为100.0u、初速度为30.0u•s -1、最大速度为0.0u•s -1、末速度为4.0u•s -1、最大加速度为27.0u•s -2、最大减速度为15.0u•s -2，由于某种原因导致最大速度为0.0，故无法进行常规的S曲线规划，需进行冗余容错变结构S曲线规划。从图中可以看出，当t=0.0s时，执行位置运动指令1，系统从初速度30.0u•s -1开始减速，1.5s后，减速度绝对值达到最大值D max1（15.0u•s -2），当t=3.0s时，速度减至0.0，此时，系统运动位移为S 0（45.0u），可得本次指令剩余未完成的位移S rem为55.0u，等待下一条指令的执行。接着，经过T wait（1.5s）后，即t=4.5s时，输入下一条运动控制指令，即运动控制指令2，系统从0.0u•s -1开始加速运动，经过Ta2加速后达到最大速度Vmax2（25.0u•s -1），接着进入匀速段运行T u2s进入减速段，再经过T d2s减速后，即t=10.3s时，到达两次指令的最终目标位移150.0u，此时，末速度为10.0u•s -1，系统按照给定参数约束及边界条件进行曲线规划。显然，本发明可以在系统出现V max=0的参数不合理等情况下，无法进行常规S曲线规划时，实现冗余容错变结构S曲线柔性规划。
（2）V max>V s 且V max<V e 时的仿真验证
仿真中所用位置运动指令3的参数如表2所示：
表2
图6为V max>V s 且V max<V e 时的冗余容错变结构S曲线规划仿真波形图。由图6可知，当t=0.0s时，系统速度为5.0u•s -1，系统位置为0.0u，此时，输入位置运动指令3，由表2可知，给定目标位置为40.0u、初速度为5.0u•s -1、最大速度为10.0u•s -1、末速度为15.0u•s -1、最大加速度为5.0u•s -2、最大减速度为8.0u•s -2，由于某种原因导致V max>V s 且V max<V e ，故无法进行常规的S曲线规划，需进行冗余容错变结构S曲线规划。从图6中可以看出，系统从初速度5.0u•s -1进入加速段1，开始加速运动，经过T a31（1.5s）后，到达V max3（10.0u•s -1）开始匀速运动，再经过T u3（1.0s）后进入加速段2，最后，经过T a32（1.5s），即t=4.0s时，加速运动后到达目标位移S t3（40.0u），此时末速度为15.0u•s -1。在整个规划中加速度最大值为A ft3，系统能按照给定参数约束及边界条件进行曲线规划。由上可知，本发明可以在系统出现V max>V s 且V max<V e 的参数不合理情况下，无法进行常规S曲线规划时，实现冗余容错变结构S曲线柔性规划。
（3）V max<V s 且V max>V e 时的仿真验证
仿真中所用位置运动指令4的参数如表3所示：
表3
图7为V max<V s 且V max>V e 时的冗余容错变结构S曲线规划仿真波形图。由图7可知，当t=0.0s时，系统速度为15.0u•s -1，系统位置为0.0u，此时，输入位置运动指令4，由表3可知，给定目标位置为25.0u、初速度为15.0u•s -1、最大速度为10.0u•s -1、末速度为5.0u•s -1、最大加速度为5.0u•s -2、最大减速度为7.5u•s -2，由于某种原因导致V max<V s 且V max>V e ，故无法进行常规的S曲线规划，需进行冗余容错变结构S曲线规划。从图7中可以看出，系统从初速度15.0u•s -1进入减速段1，开始减速运动，经过T d41（1.0s）后，到达V max4（10.0u•s -1）开始匀速运动，再经过T u4（0.5s）后进入减速段2，最后，经过T d42（1.0s），即t=2.5s时，加速运动后到达目标位移S t4（25.0u），此时末速度为5.0u•s -1。在整个规划中减速度绝对值最大值为D ft4，系统能按照给定参数约束及边界条件进行曲线规划。由上可知，本发明可以在系统出现V max<V s 且V max>V e 的参数不合理情况下，无法进行常规S曲线规划时，实现冗余容错变结构S曲线柔性规划。
（4）V max<V s 且V max<V e 时的仿真验证
仿真中所用位置运动指令5的参数如表4所示：
表4
图8为V max<V s 且V max<V e 时的冗余容错变结构S曲线规划仿真波形图。由图8可知，当t=0.0s时，系统速度为15.0u•s -1，系统位置为10.0u，此时，输入位置运动指令5，由表4可知，给定目标位置为60.0u、初速度为15.0u•s -1、最大速度为5.0u•s -1、末速度为20.0u•s -1、最大加速度为7.0u•s -2、最大减速度为15.0u•s -2，由于某种原因导致V max<V s 且V max<V e ，故无法进行常规的S曲线规划，需进行冗余容错变结构S曲线规划。从图7中可以看出，系统从初速度15.0u•s -1进入减速段1，开始减速运动，经过T d51（1.0s）后，到达V max5（5.0u•s -1）开始匀速运动，再经过T u5（1.96s）后进入加速段1，最后，经过T a51（3.21s），即t=6.18s时，加速运动后到达目标位移S t5（70.0u），此时末速度为20.0u•s -1。在整个规划中减速度绝对值最大值为D ft4，系统能按照给定参数约束及边界条件进行曲线规划。由上可知，本发明可以在系统出现V max<V s 且V max>V e 的参数不合理情况下，无法进行常规S曲线规划时，实现冗余容错变结构S曲线柔性规划。
以上，通过V max=0时的仿真结果、V max>V s 且V max<V e 时的仿真结果、V max<V s 且V max>V e 时的仿真结果和V max<V s 且V max<V e 时的仿真结果可知，本发明的冗余容错式非对称S型柔性速度曲线自适应规划方法能够在出现系统故障、V max=0、V max>V s 且V max<V e 、V max<V s且V max>V e 、V max<V s 且V max<V e 等参数不合理等情况下，无法进行常规S曲线规划时，实现冗余容错变结构S曲线柔性规划，保证系统在出现故障的情况下可靠、安全、高速、高效的运行，满足高可靠性、高安全性、高鲁棒性的控制要求。本发明在加减速控制中对运动方向进行归一处理实现双向曲线规划。加速过程和减速过程分开控制，实现非对称的曲线规划，同时，整个规划过程中保证加速度的连续性，规划分类少，规划参数易求取，以保证无柔性冲击，满足高灵活性、高精度的控制要求，又能保证算法实现简单，实现高速、高效率的控制要求，提升控制系统的运行速度和加工效率。
本实施例中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (10)

1.一种冗余容错式非对称S型柔性速度曲线自适应规划方法，其特征在于，该方法包括：
S1.输入位置运动指令，给定初始参数条件：目标位移S t 、初速度V s 、最大速度V max、末速度V e 、最大加速度A max、最大减速度D max；
S2.将运动方向归一成正向后进行后续的曲线规划；
S3.当V max≤V t 且V max≠0，A max（D max）≤A t 且A max≠0时执行步骤S4，
A t 表示驱动轴电机所能提供的最大加速度，
V t 表示驱动轴电机所能提供的最高速度；
S4.判断V max与V s 、V e 的大小关系，
若V max≥V s 且V max≥V e 则进行常规S曲线规划，否则进行冗余容错自适应变结构速度曲线规划：
若V max>V s 且V max<V e ，则令容错最大加速度A ft=A max，进行V s 加速至V max、（V max匀速）、V max加速至V e 的冗余容错变结构速度曲线规划；
若V max<V s且V max>V e ，则令容错最大减速度D ft=D max，进行V s 减速至V max、（V max匀速）、V max减速至V e 的冗余容错变结构速度曲线规划；
若V max<V s 且V max<V e ，则令容错最大减速度D ft=D max，容错最大加速度A ft=A max，进行V s 减速至V max、（V max匀速）、V max加速至V e 的冗余容错变结构速度曲线规划；
S5.对规划参数进行符号处理，以实现正负方向均可以进行曲线规划；
S6.将各分段始末运动规划参数及分段规划时间通过控制周期T s 进行曲线离散插补输出。
2.根据权利要求1所述的冗余容错式非对称S型柔性速度曲线自适应规划方法，其特征在于，步骤S2具体包括：
处理运动方向，若目标位移S t <0，则将S t 、V s 、V e 进行转换，归一成正向进行曲线规划：
令 （1）
得： （2）
步骤S5具体包括：
若目标位移S t 为正，则将得到的各分段始末运动规划参数直接输出；若目标位置S t 为负，则对运动规划参数进行符号转换处理，以实现负方向的曲线规划，具体如下：
（23）。
3.根据权利要求1所述的冗余容错式非对称S型柔性速度曲线自适应规划方法，其特征在于，步骤S3具体包括：
判断V max是否为0，如果V max=0，则进行V s 减至0的冗余容错变结构速度曲线规划，计算对应的位移S 0，以及本次指令剩余未完成的位移S rem，并将S rem送入参数初始化，等待下一条指令的执行；
如果V max≠0，则判断V max是否小于驱动轴电机所能提供的最高速度V t ；
如果V max>V t ，则令V max=V t，限制规划最大速度不超过电机的物理特性上限；
如果V max≤V t ，则判断A max（D max）是否等于0；
如果A max（D max）=0，则令A max=A lastmax（D max=D lastmax），将上一次指令的加（减）速度A lastmax（D lastmax）赋给A max（D max）；
如果A max（D max）≠0，则判断A max（D max）是否小于驱动轴电机所能提供的最大加速度A t ；
如果A max（D max）>A t ，则令A max（D max）=A t ，限制规划最大加（减）速度不超过电机的物理特性上限；
如果A max（D max）≤A t ，则执行步骤S4。
4.根据权利要求1所述的冗余容错式非对称S型柔性速度曲线自适应规划方法，其特征在于，步骤S4中，若V max≥V s 且V max≥V e ，则进行加速段、（匀速段）、减速段的常规S曲线规划。
5.根据权利要求4所述的冗余容错式非对称S型柔性速度曲线自适应规划方法，其特征在于，步骤S4中，若V max>V s 且V max<V e ，则进行V s 加速至V max、（V max匀速）、V max加速至V e 的冗余容错变结构速度曲线规划：
令容错最大加速度A ft=A max，计算从V s 运动至V e 的单段加速位移S a0；
若S t >S a0，则冗余容错变结构S曲线轨迹规划可行；若S t ≤S a0，则根据S t 对A ft进行修正，并求取加速时间T a0，进行单段加速特殊曲线规划。
6.根据权利要求5所述的冗余容错式非对称S型柔性速度曲线自适应规划方法，其特征在于，针对V max>V s 且V max<V e 的情况计算S曲线规划的参数，定义：
J sk ：第k段的初加加速度；
A sk ：第k段的初加速度；
A ek ：第k段的末加速度；
V sk ：第k段的初速度；
V ek ：第k段的末速度；
S k ：第k段的位移；
其中,k=1,2,3…；
存在两种可能性，有匀速段，即T u >0，无匀速段，即T u ≤0；
当T u >0时，加速段1时间为T a1，匀速段时间为T u ，加速段2时间为T a2，然后，计算相应始末运动规划参数如下：
第一段，即加速段1，由V s 加速运动至V max，加速时间为T a1，此时，k=1，第一段始末运动规划参数如下：
（19）
第二段，即匀速段，保持V max匀速运动，匀速时间为T u ，此时，k=2，且V s2=V e1，A s2=A e1，第二段始末运动规划参数如下：
（20）
第三段，即加速段2，由V max加速运动至V e ，加速时间为T a2，此时，k=3，V s3=V e2，A s3=A e2，第三段始末运动规划参数如下：
（21）
当T u ≤0时，单加速段时间为T a0，计算相应始末运动规划参数如下：
由V s 加速运动至V e ，加速时间为T a0，此时，k=1，相应始末运动规划参数如下：
（22）。
7.根据权利要求6所述的冗余容错式非对称S型柔性速度曲线自适应规划方法，其特征在于，若Vmax<V s且V max>V e ，则进行V s 减速至V max、（V max匀速）、V max减速至V e 的冗余容错变结构速度曲线规划：
令容错最大减速度D ft=D max，计算从V s 运动至V e 的单段减速位移S d0；
若S t >S d0，则冗余容错变结构S曲线轨迹规划可行；若S t ≤S d0，则需要根据S t 对D ft进行修正，并求取减速时间T d0，进行单段减速特殊曲线规划。
8.根据权利要求7所述的冗余容错式非对称S型柔性速度曲线自适应规划方法，其特征在于，针对Vmax<V s且V max>V e 的情况计算S曲线规划的参数，存在两种可能性：
当T u >0时，有匀速段，减速段1时间为T d1，匀速段时间为T u ，减速段2时间为T d2，然后，计算相应始末运动规划参数如下：
第一段，即减速段1，由V s 减速运动至V max，减速时间为T d1，此时，k=1，第一段始末运动规划参数如下：
（37）
第二段，即匀速段，保持V max匀速运动，匀速时间为T u ，此时，k=2，且V s2=V e1，A s2=A e1，第二段始末运动规划参数如下：
（38）
第三段，即减速段2，由V max减速运动至V e ，减速时间为T d2，此时，k=3，V s3=V e2，A s3=A e2，第三段始末运动规划参数如下：
（39）
当T u ≤0时，无匀速段
单减速段时间为T d0，计算相应始末运动规划参数如下：
由V s 减速运动至V e ，减速时间为T d0，此时，k=1，相应始末运动规划参数如下：
（40）。
9.根据权利要求8所述的冗余容错式非对称S型柔性速度曲线自适应规划方法，其特征在于，若Vmax<V s 且V max<V e ，则进行V s 减速至V max、（V max匀速）、V max加速至V e 的冗余容错变结构速度曲线规划：
令容错最大减速度D ft=D max，容错最大加速度A ft=A max，根据V s 与V e 的大小关系，计算相应的单段加（减）位移，判断是否可以进行S曲线规划，具体如下：
若V s <V e ，计算从V s 运动至V e 的单段加速位移S a0，若S t >S a0，则冗余容错变结构S曲线轨迹规划可行；若S t ≤S a0，则对A ft进行修正并求取加速时间T a0，进行单段加速曲线特殊规划，此时，通过权利要求6所述的式（22）计算相应始末运动规划参数；
若V s =V e ，不存在单加速段或单减速段的情况，始终满足冗余容错变结构S曲线规划的条件；
若V s >V e ，计算从V s 运动至V e 的单段减速位移S d0，若S t >S d0，则冗余容错变结构S曲线轨迹规划可行；若S t ≤S d0，对D ft进行修正，并求取减速时间T d0，进行单段减速特殊曲线规划，此时，通过权利要求8所述的式（40）计算相应始末运动规划参数；
针对V max<V s且V max<V e 的情况，若满足冗余容错变结构S曲线规划的条件，计算S曲线规划的参数存在两种可能性：
当T u >0时，有匀速段，减速段1时间为T d1，匀速段时间为T u ，加速段1时间为T a1，然后，计算相应始末运动规划参数如下：
第一段，即减速段1，由V s 减速运动至V max，减速时间为T d1，此时，k=1，第一段始末运动规划参数如下：
（46）
第二段，即匀速段，保持V max匀速运动，匀速时间为T u ，此时，k=2，且V s2=V e1，A s2=A e1，第二段始末运动规划参数如下：
（47）
第三段，即加速段1，由V max加速运动至V e ，加速时间为T a1，此时，k=3，V s3=V e2，A s3=A e2，第三段始末运动规划参数如下：
（48）
当T u ≤0时，无匀速段，减速段1时间为T d1，加速段1时间为T a1，然后，计算相应始末运动规划参数如下：
第一段，即减速段1，由V s 减速运动至中间过渡速度V g ，减速时间为T d1，此时，k=1，第一段始末运动规划参数如下：
（49）
第二段，即加速段1，由V g 加速运动至V e ，加速时间为T a1，此时，k=2，V s2=V e1，A s2=A e1，第二段始末运动规划参数如下：
（50）。
10.根据权利要求9所述的冗余容错式非对称S型柔性速度曲线自适应规划方法，其特征在于，步骤S6具体包括：
当V max>V s 且V max<V e 时的曲线离散插补输入如下：
当T u >0时，曲线规划为加速段1、匀速段、加速段2；
当nT s [0, T a1]时，i=nT s /T a1，其中，n=1,2,3…为正整数，根据加速段1的始末运动规划参数进行曲线离散插补输出；
当nT s [T a1, T a1+T u ]时，i = (nT s -T a1)，根据加速段1、匀速段的始末运动规划参数进行曲线离散插补输出；
当nT s [T a1+T u , T a1+T u +T a2]时，i = (nT s -T a1-T u )/T a2，根据加速段1、匀速段和加速段2的始末运动规划参数进行曲线离散插补输出；
当T u ≤0，曲线规划为单段加速特殊曲线规划，当nT s [0, T a0]时，i=nT s /T a0，其中，n=1,2,3…为正整数，根据相应始末运动规划参数进行曲线离散插补输出；
Vmax<V s且V max>V e 和Vmax<V s且V max<V e 时的曲线离散插补输出参照V max>V s 且V max<V e 时的方法。
CN202310747380.4A 2023-06-25 2023-06-25 冗余容错式非对称s型柔性速度曲线自适应规划方法 Active CN116577994B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310747380.4A CN116577994B (zh) 2023-06-25 2023-06-25 冗余容错式非对称s型柔性速度曲线自适应规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310747380.4A CN116577994B (zh) 2023-06-25 2023-06-25 冗余容错式非对称s型柔性速度曲线自适应规划方法

Publications (2)

Publication Number Publication Date
CN116577994A true CN116577994A (zh) 2023-08-11
CN116577994B CN116577994B (zh) 2024-02-27

Family

ID=87534261

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310747380.4A Active CN116577994B (zh) 2023-06-25 2023-06-25 冗余容错式非对称s型柔性速度曲线自适应规划方法

Country Status (1)

Country Link
CN (1) CN116577994B (zh)

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106168790A (zh) * 2016-02-29 2016-11-30 华南理工大学 一种在线改变目标速度和位置的s形加减速控制方法
CN106444635A (zh) * 2015-08-04 2017-02-22 深圳市雷赛智能控制股份有限公司 一种运动控制系统的非对称s曲线加减速控制方法和装置
CN109240215A (zh) * 2018-11-22 2019-01-18 山东易码智能科技股份有限公司 一种基于改进型s曲线加减速的pvt控制方法
WO2019019707A1 (zh) * 2017-07-28 2019-01-31 珞石（山东）智能科技有限公司 非零初始状态s型停止曲线速度规则及其在线求解方法
CN109313429A (zh) * 2017-04-24 2019-02-05 深圳配天智能技术研究院有限公司 S型速度规划方法、装置、系统、机器人以及数控机床
CN109426151A (zh) * 2017-09-01 2019-03-05 沈阳高精数控智能技术股份有限公司 基于自适应前瞻和预测校正的实时柔性加减速控制算法
CN111596543A (zh) * 2020-06-10 2020-08-28 嘉兴市像景智能装备有限公司 一种快速规划6阶s型运动曲线的方法
CN111897216A (zh) * 2020-07-16 2020-11-06 华中科技大学 一种多运动段速度规划和插补方法
CN114879609A (zh) * 2022-05-11 2022-08-09 合肥安迅精密技术有限公司 加加速度连续的三角函数式曲线规划方法及系统、贴片机
CN114995118A (zh) * 2022-07-18 2022-09-02 英孚康（浙江）工业技术有限公司 一种抑制过冲和反向的跃度自适应s型速度曲线规划方法
CN115070762A (zh) * 2022-06-17 2022-09-20 电子科技大学 一种高速分拣机器人的非对称s型速度曲线规划方法
CN115657608A (zh) * 2022-10-26 2023-01-31 英孚康（浙江）工业技术有限公司 基于Brent迭代法的全局非对称式S型柔性速度曲线规划方法

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106444635A (zh) * 2015-08-04 2017-02-22 深圳市雷赛智能控制股份有限公司 一种运动控制系统的非对称s曲线加减速控制方法和装置
CN106168790A (zh) * 2016-02-29 2016-11-30 华南理工大学 一种在线改变目标速度和位置的s形加减速控制方法
CN109313429A (zh) * 2017-04-24 2019-02-05 深圳配天智能技术研究院有限公司 S型速度规划方法、装置、系统、机器人以及数控机床
WO2019019707A1 (zh) * 2017-07-28 2019-01-31 珞石（山东）智能科技有限公司 非零初始状态s型停止曲线速度规则及其在线求解方法
CN109426151A (zh) * 2017-09-01 2019-03-05 沈阳高精数控智能技术股份有限公司 基于自适应前瞻和预测校正的实时柔性加减速控制算法
CN109240215A (zh) * 2018-11-22 2019-01-18 山东易码智能科技股份有限公司 一种基于改进型s曲线加减速的pvt控制方法
CN111596543A (zh) * 2020-06-10 2020-08-28 嘉兴市像景智能装备有限公司 一种快速规划6阶s型运动曲线的方法
CN111897216A (zh) * 2020-07-16 2020-11-06 华中科技大学 一种多运动段速度规划和插补方法
CN114879609A (zh) * 2022-05-11 2022-08-09 合肥安迅精密技术有限公司 加加速度连续的三角函数式曲线规划方法及系统、贴片机
CN115070762A (zh) * 2022-06-17 2022-09-20 电子科技大学 一种高速分拣机器人的非对称s型速度曲线规划方法
CN114995118A (zh) * 2022-07-18 2022-09-02 英孚康（浙江）工业技术有限公司 一种抑制过冲和反向的跃度自适应s型速度曲线规划方法
CN115657608A (zh) * 2022-10-26 2023-01-31 英孚康（浙江）工业技术有限公司 基于Brent迭代法的全局非对称式S型柔性速度曲线规划方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
徐剑琴等: "冗余度机械臂初始位置误差容错研究", 《控制工程》, no. 12, pages 2351 - 2359 *
王凯等: "适应S型曲线对电气布线机的速度规划", 《现代电子技术》, no. 1, pages 114 - 118 *

Also Published As

Publication number Publication date
CN116577994B (zh) 2024-02-27

Similar Documents

Publication Publication Date Title
Meike et al. Energy efficient use of multirobot production lines in the automotive industry: Detailed system modeling and optimization
CN103135501B (zh) 基于s型曲线的加减速控制方法和装置及数控机床
Martínez et al. Assessment of jerk performance s-curve and trapezoidal velocity profiles
CN114995118B (zh) 一种抑制过冲和反向的跃度自适应s型速度曲线规划方法
CN206862600U (zh) 一种开闭比连续可调的风洞试验段控制装置
CN116577994B (zh) 冗余容错式非对称s型柔性速度曲线自适应规划方法
CN202019332U (zh) 一种单台逆变器拖动四台异步电机的驱动系统
CN205367158U (zh) 分拣机器人
CN103454998A (zh) 基于工业以太网总线的伺服刚性调试装置及方法
CN101892954B (zh) 一种基于连续定位方式的风电变桨驱动方法
CN116483026B (zh) 多模式多类型非对称s型柔性速度曲线双向快速规划方法
CN103676900A (zh) 一种可重构的开放式运动控制系统架构设计
CN108621250B (zh) 定心ac五轴联动水切割机
CN105415363A (zh) 位移装置、机器人及机器人奇异点处理方法
Beata Fuzzy logic controller for robot manipulator control system
CN206662444U (zh) 一种电火花机床的回零系统
CN206465078U (zh) 曲线轨道机器人装置及自动化加工系统
CN114524351B (zh) 一种电梯门机控制器的自动识别方法
JP2012170197A (ja) 交流電源の電源特性に応じてモータの出力を制限するモータ駆動制御装置
CN113715023B (zh) 一种基于plc控制的桁架五轴机械手的防碰撞方法
RU2489798C1 (ru) Следящий электропривод
CN203346433U (zh) 焊管生产线退火炉的控制装置
Wang et al. A Smooth and Time-Optimal Trajectory Planning Approach for Automated Systems
CN103853098B (zh) 一种应用于雕铣机的伺服位置控制方法
CN103872963A (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
GR01 Patent grant