[发明专利]一种拐角平滑样条的轨迹实时插补方法和装置

权利要求书

1.一种拐角平滑样条的轨迹实时插补方法，其特征在于，包括：

S1：建立拐角平滑样条对应的弦高误差优化模型；

S2：将弦高误差估算方法代入所述弦高误差优化模型得到采样参数序列和采样点序列，利用所述采样点序列计算采样弧长序列；

S3：利用所述采样参数序列和所述采样弧长序列建立最小二乘拟合方程；实时求解所述最小二乘拟合方程建立弧长参数映射模型；

S4：将所述拐角平滑样条的样条参数输入所述弧长参数映射模型，获取下一插补点对应的样条参数和坐标，以实现轨迹实时插补；

所述S1包括：

建立所述弦高误差优化模型，改变采样点分布以减少弦长与弧长的偏差；所述弦高误差优化模型表示为：

其中，所述弦高误差优化模型满足实时性要求的上限约束，ε_i为采样点序列间的弦高误差序列；给定采样点个数上限n；随着采样循环次数i的增加，采样点个数t(i)不断增加直到达到采样点个数上限n时停止采样；

所述S2包括：

S21：将所述弦高误差估算方法带入所述弦高误差优化模型中，得到所述采样参数序列U和所述采样点序列P：

S22：将累积弦长近似弧长得到采样弧长序列S＝[s₀ s₁ s₂ ··· s_n-1]，其中，

所述S2中所述弦高误差估算方法包括：

S201：将拐角平滑样条曲线上两相邻采样点p_k与p_k+1的切矢V_k与V_k+1表示为：

V_k＝α_kt_k V_k+1＝α_k+1t_k+1；

其中，所述拐角平滑样条曲线是构建于两直线段拐角处的平面曲线，p_k＝C(u_k)，p_k+1＝C(u_k+1)，α_k与α_k+1分别是p_k与p_k+1处的切矢模长，t_k与t_k+1分别是p_k与p_k+1处的单位切矢，θ_k与θ_k+1分别是切矢V_k与V_k+1与弦线矢量的夹角；

S202：对所述拐角平滑样条曲线上弦高误差最大点对应的参数值u_chord进行估算得到：

u_k、u_k+1分别为p_k与p_k+1间的弦高误差，q为比例系数；

S203：利用参数值u_chord估算得到曲线段上弦高误差值ε_chord及弦高误差最大值对应的点p_chord；