[发明专利]一种基于高阶B样条的多轴系统实时引导轨迹规划方法

说明书

本发明涉及一种基于高阶B样条的多轴系统实时引导轨迹规划方法，包括以下步骤：将实时引导数据按照固定的通信周期压入输入队列；将实时引导数据进行B样条曲线路径规划；将规划好的B样条曲线路径按照控制周期离散的数据依次要入输出队列，当输出队列数据达到设定数量后，将队列中的数据依次取出，发个驱动设备。本发明采用高阶B样条对实时动态数据进行路径规划，保证了实时规划轨迹高阶可导。无需求解大量控制点完成轨迹规划，简化轨迹规划计算量。避免了由于引导数据不光滑造成多轴控制系统的抖动问题。因此即使引导数据不光滑也无需对数据进行滤波处理。

技术领域

本发明属于实时规划技术领域，涉及多轴控制系统实时规划算法。根据实时动态数据，实时规划运动轨迹，保证多轴控制系统运动轨迹的是高阶可导的。

背景技术

离线规划一般仅能保证多轴控制系统完成特定的任务，无法根据实时数据进行动态调整。在某些应用场景需要根据外部传感设备引导多轴控制系统进行动态跟踪运动。如：视觉相机、力反馈手柄等。要求多轴控制系统在较小跟踪误差的同时保证跟踪运行的平滑。即使外部传感设备的引导数据不平滑时，仍能保证多轴控制系统跟踪轨迹的高阶可导。多轴控制系统实时规划应用广泛，如：如医疗设备、打磨机器人、机器人的视觉伺服系统等。多轴控制系统的实时规划策略的研究具有重要意义。

发明内容

针对多轴控制系统动态跟踪运动的跟踪误差小，运动轨迹高阶可导的问题。本发明提出一种基于B样条的多轴控制系统实时规划策略。对外部传感设备的引导数据，进行B样条规划，根据引导数据之间的距离计算B样条规划数据的节点向量；并将规划时间线性映射为B样条轨迹参数，最终获得规划轨迹关于时间的描述函数，该函数的可导性由B样条轨迹的可导性决定。而B样条的可导性由B样条的阶次和节点重合度决定。跟踪误差由分段规划所使用的引导数据的数目m及引导数据的周期ts决定。滞后时间为(m-1)*ts。

本发明解决其技术问题所采用的技术方案是：一种基于高阶B样条的多轴系统实时引导轨迹规划方法，包括以下步骤：

步骤S1：将实时引导数据按照固定的通信周期压入输入队列；

步骤S2：将实时引导数据进行B样条曲线路径规划；

步骤S3：将规划好的B样条曲线路径按照控制周期离散的数据依次要入输出队列，当输出队列数据达到设定数量后，将队列中的数据依次取出，发个驱动设备。

所述将实时引导数据按照固定的引导通信周期tg压入输入队列后，队列中的数据达到设定数目m后，发出B样条曲线路径规划信号，启动步骤S2。

设定数目m≥3。

所述将实时引导数据进行B样条曲线路径规划，包括以下步骤：

1)B样条曲线路径上的起点、中间必经点以及终点分别为A_i，A_i+1，A_i+2，i＝1,2,…；

2)所述B样条曲线的前半部分按控制周期离散，即A_i至A_i+1部分曲线，同时将A_i至的所有数据点弹出队列,其中：为A_i至A_i+1之间的第n点；

3)以A_i+1作为A_i点，并返回步骤1)。

所述A_i的一阶和二阶导数在i＝1处均为零、在i1时为A_i-1处一阶和二阶导数；A_i+2的一阶和二阶导数均为零。

将S2规划的高阶B样条曲线按照多轴控制系统驱动周期ts离散，并压入输出队列。

外部引导设备通信周期tg与多轴控制系统的驱动周期ts可以相同也可不同；所述外部引导设备为力反馈手柄或视觉系统。