[发明专利]一种误差可控的小线段轨迹光顺方法

权利要求书

1.一种误差可控的小线段轨迹光顺方法，其特征在于，包括如下步骤：

步骤1、机器人轨迹点预处理：对整条轨迹的所有轨迹点进行遍历，根据轨迹点之间的距离和夹角进行分段，将整条轨迹划分为若干段折线段集合；

步骤2、轨迹点光顺：遍历步骤1生成的折线段集合，对每一条折线段按照轨迹点误差阈值、弦高误差阈值、连续性要求和光顺要求，计算光顺轨迹；以一条折线段的光顺为例，其具体步骤如下：

步骤2.1、初始迭代参数决定，设当前折线段包含的原始轨迹点为轨迹点误差阈值记为ε_max；弦高误差阈值记为δ_max，要求ε_max≤δ_max，轨迹点误差是指光顺后的曲线与原始轨迹点之间的轨迹点误差；弦高误差是光顺后的曲线与原始轨迹点组成的折线段之间的弦高误差，最大迭代次数记为k_max，设置当前迭代次数为k＝0，迭代轨迹点记为对除首尾外的内部轨迹点设置迭代弦高误差阈值，记为迭代弦高误差是指迭代轨迹点组成的折线段与过渡曲线之间的弦高误差，因最大弦高误差点出现在轨迹点处，该最大迭代弦高误差也是迭代轨迹点与过渡曲线之间的最大轨迹点误差；

步骤2.2、对迭代轨迹点按照迭代弦高误差阈值连续性要求和保形要求生成(N-1)段对称的过渡曲线，即将按照迭代弦高误差阈值采用对称的光顺曲线进行过渡，得到(N-1)段过渡曲线，记为其中连续性要求是指过渡曲线与线性部分的连接处的连续性满足连续性要求；

步骤2.3、计算(N-1)段过渡曲线上轨迹点误差最大点，记为因采用对称的过渡曲线，因此轨迹点误差最大点为：遍历j＝1,…N-1，进而计算最大轨迹点误差：

步骤2.4、若最大轨迹点误差Δ_k小于轨迹点误差阈值ε_max或k＝k_max，终止迭代并输出光顺轨迹此处的轨迹点误差阈值ε_max为步骤2.1设置的弦高误差阈值用来决定迭代是否终止；否则，转步骤2.5；

步骤2.5、遍历j＝1,…N-1,根据原始轨迹点P_j和轨迹点误差最大点计算偏移向量然后更新迭代轨迹点：并计算迭代弦高误差阈值其中为向量和之间夹角的一半；然后令k＝k+1，转步骤2.2；

步骤2.6、对步骤2.4输出的光顺轨迹进行整理，光顺后的轨迹由N条直线段部分和(N-1)条光顺曲线部分组合而成，其按顺序依次是：直线段光顺曲线直线段光顺曲线…光顺曲线直线段

2.根据权利要求1所述的误差可控的小线段轨迹光顺方法，其特征在于，所述步骤1是对输入的轨迹点进行预处理，具体包括如下步骤：

步骤1.1、距离分段，以轨迹点之间的距离作为第一分段条件：若相邻轨迹点之间的距离小于距离阈值，则在这两个轨迹点处分段；轨迹点分为三段折线段；

步骤1.2、夹角分段，在分段之后的第三段折线段内部，以相邻轨迹段之间的夹角作为第二分段条件：若相邻两个轨迹段之间夹角大于角度阈值，则在两个轨迹段的中间点处进行分段；

步骤1.3、对步骤1.1及步骤1.2两次分段后的折线段进行整理，折线段中轨迹点个数小于3的折线段不进行光顺，没有参与光顺的折线段在光顺后的轨迹中按线性轨迹输出；折线段中轨迹点个数大于等于3的折线段进行光顺。

3.根据权利要求2所述的误差可控的小线段轨迹光顺方法，其特征在于，所述步骤1.1中，分段的两个轨迹点不进行光顺，即距离小于距离阈值的线段不需要光顺。

4.根据权利要求1所述的误差可控的小线段轨迹光顺方法，其特征在于，所述步骤2.2中，连续性要求根据实际应用决定，是G¹连续、G²连续或G³连续中任意一种；根据连续性要求选取过渡曲线，G¹连续时过渡曲线选取圆弧曲线、抛物线或四个控制点的三次B样条曲线，G²连续时选取五个控制点的三次B样条曲线或四个控制点的五次B样条曲线或其它多项式曲线；保形要求是指相邻的两条过渡曲线之间不出现打折且光顺后整个过渡曲线光顺。