[发明专利]一种基于速度规划的曲线拟合方法

权利要求书

1.一种基于速度规划的曲线拟合方法，其特征在于，所述方法具体包括以下步骤：

步骤一、输入待拟合曲线的参数，并根据设定的离散段数量N对待拟合曲线进行分段，得到各分段点的坐标以及各分段点处的切线矢量；

步骤二、读入第一个分段点和第二个分段点，将第一个分段点作为首点并记为点A(x_A,y_A,z_A)，将第二个分段点作为尾点并记为点C(x_C,y_C,z_C)，待拟合曲线在点A处的切线矢量和在点C处的切线矢量的交点为点B(x_B,y_B,z_B)，其中，x,y,z是空间直角坐标系的三个坐标轴，(x_A,y_A,z_A)是第一个分段点的坐标，(x_C,y_C,z_C)是第二个分段点的坐标；

在直线段AB上，从点A开始进行减速，直到点B时速度减为0，在直线段BC上，从点B开始进行加速，直到点C时速度最大，且在直线段AB与直线段BC上的运动是同时开始并同时结束的，即当在直线段AB上运动到点B时，在直线段BC上运动到点C；

在整条直线段AB上或整条直线段BC上走完所需的时间均为T，则根据直线段AB的长度计算得到在点A时的初始速度v_A，再根据v_A计算直线段AB上的加速度a_AB；

所述根据直线段AB的长度计算得到在点A时的初始速度v_A，再根据v_A计算直线段AB上的加速度a_AB；其具体过程为：

其中，s_AB为直线段AB的长度，

根据直线段BC的长度计算得到在点C的速度v_C，再根据v_C计算直线段BC上的加速度a_BC；

所述根据直线段BC的长度计算得到在点C的速度v_C，再根据v_C计算直线段BC上的加速度a_BC；其具体过程为：

其中，s_BC为直线段BC的长度，

根据v_A、a_AB、v_C和a_BC，计算出在首点与尾点之间的拟合曲线上任意时刻t的合成速度矢量及位置矢量再根据位置矢量得到在首点与尾点之间的拟合曲线上时刻t对应的坐标点P_t＝[x_t,y_t,z_t]；

所述根据v_A、a_AB、v_C和a_BC，计算出在首点与尾点之间的拟合曲线上任意时刻t的合成速度矢量其具体过程为：

其中，为直线段AB上任意时刻t的速度矢量，为直线段BC上任意时刻t的速度矢量；

其中，v_{t_AB}为直线段AB上任意时刻t的速度，v_{t_AB}＝v_A-a_AB×t，直线段AB的矢量方向vec_AB为：

其中，vec_{AB_x}为vec_AB在x轴方向分量，vec_{AB_y}为vec_AB在y轴方向分量，vec_{AB_z}为vec_AB在z轴方向分量；

其中，v_{t_BC}为直线段BC上任意时刻t的速度，v_{t_BC}＝a_BC×t，直线段BC的矢量方向vec_BC为：

其中，vec_{BC_x}为vec_BC在x轴方向分量，vec_{BC_y}为vec_BC在y轴方向分量，vec_{BC_z}为vec_BC在z轴方向分量；

所述根据v_A、a_AB、v_C和a_BC，计算出在首点与尾点之间的拟合曲线上任意时刻t的位置矢量其具体过程为：

其中，为经过任意时刻t在直线段AB上的距离矢量，为经过任意时刻t在直线段BC上的距离矢量；

其中，s_{t_AB}为经过任意时刻t在直线段AB上走过的距离；

其中，s_{t_BC}为经过任意时刻t在直线段BC上走过的距离；

即坐标点P_t＝[x_t,y_t,z_t]，其中，x_t＝s_{t_AB}×vec_{AB_x}+s_{t_BC}×vec_{BC_x}，y_t＝s_{t_AB}×vec_{AB_y}+s_{t_BC}×vec_{BC_y}，z_t＝s_{t_AB}×vec_{AB_z}+s_{t_BC}×vec_{BC_z}；

计算出点A与点C的中点D的坐标，计算在首点与尾点之间的拟合曲线上时刻T/2对应的坐标点P_T/2与中点D的距离，再将该距离作为拟合误差ε；

步骤三、判断拟合误差ε是否满足要求，若满足，则执行步骤四；否则，将中点D作为新插入的分段点，再将点A作为首点，将中点D作为尾点来执行步骤二；

步骤四、将当前尾点作为首点，判断是否还有分段点待读取，若是，则输出本次迭代步骤二时获得的速度和位置，并读入下一个分段点作为新的尾点，再返回步骤二，若否，则输出本次迭代步骤二时获得的速度和位置，并结束曲线拟合过程；

步骤五、数控系统根据步骤一至步骤四的曲线拟合过程中所获得的各时刻速度与位置，进行速度规划和插补处理。

2.根据权利要求1所述的一种基于速度规划的曲线拟合方法，其特征在于，所述时间T的取值为1秒。