[发明专利]运动轨迹平滑转接的方法和装置及其相关设备

权利要求书

1.一种运动轨迹平滑转接的方法，其特征在于，所述方法包括：

获取待平滑转接的第一轨迹和第二轨迹以及预设轮廓误差；

根据所述第一轨迹和所述第二轨迹以及所述预设轮廓误差确定特征插补点的位置；

根据所述第一轨迹、第二轨迹、所述特征插补点以及插补曲线通用方程拟合出连接所述第一轨迹、所述特征插补点和所述第二轨迹的插补曲线，以使得所述第一轨迹与所述插补曲线在二者连接点的曲率与切向角相等，所述第二轨迹与所述插补曲线在二者连接点的曲率与切向角相等；

其中，所述根据所述第一轨迹、第二轨迹、所述特征插补点以及插补曲线通用方程拟合出连接所述第一轨迹、所述特征插补点和所述第二轨迹的插补曲线的步骤包括：拟合出连接所述第一轨迹和所述特征插补点的第一子曲线以及连接所述特征插补点和所述第二轨迹之间的第二子曲线，所述第一子曲线和所述第二子曲线分别满足所述插补曲线通用方程且具有不同的曲线参数，所述第一子曲线和所述第二子曲线在所述特征插补点处的曲率与切向角均相等；所述第一子曲线为第一回旋曲线，所述第二子曲线为第二回旋曲线；

其中，所述第一轨迹为线段，所述第二轨迹为圆弧，其中所述根据所述第一轨迹、第二轨迹、所述特征插补点以及插补曲线通用方程拟合出连接所述第一轨迹、所述特征插补点和所述第二轨迹的插补曲线的步骤包括：

将所述线段设置在直角坐标系的一坐标轴上，且所述线段的未与所述圆弧连接的一端设置成与所述直角坐标系的原点重合；

通过联立以下两组方程求解出所述第一回旋曲线和所述第二回旋曲线的曲线参数：

其中，x₀为所述第一回旋曲线与所述线段的连接点在所述坐标轴上的坐标，c₀为所述第一回旋曲线的曲率变化率，s₀为所述第一回旋曲线从其与所述线段的连接点到所述特征插补点之间的弧长，c₁为所述第二回旋曲线的曲率变化率，s₁为所述第二回旋曲线从所述特征插补点到其与所述圆弧的连接点之间的弧长，α为所述圆弧从所述第二回旋曲线与所述圆弧的连接点到所述圆弧与所述线段的连接点之间的圆弧片段的圆心角，T_ccw为旋转矩阵，T_ccw、x₀、c₀、s₀、c₁、s₁以及α为需要通过所述两组方程进行求解的未知量；

其中，L为所述线段的长度，E为所述预设轮廓误差，β为所述线段与所述圆弧的夹角，R为所述圆弧的半径，x_c为所述圆弧的圆心在所述坐标轴上的坐标，y_c为所述圆弧的圆心在所述直角坐标系另一坐标轴上的坐标，为所述圆弧的圆心到所述圆弧与所述线段的连接点的方向向量，x_c、y_c、L、β和R为能够通过所述线段和所述圆弧及二者的相对位置关系获得的已知量；或者，

所述第一轨迹为第一圆弧，所述第二轨迹为第二圆弧，其中所述根据所述第一轨迹、第二轨迹、所述特征插补点以及插补曲线通用方程拟合出连接所述第一轨迹、所述特征插补点和所述第二轨迹的插补曲线的步骤包括：

将所述第一圆弧在未与所述第二圆弧连接的一端处的切线设置在直角坐标系的一坐标轴上，将所述第一圆弧未与所述第二圆弧连接的一端设为与所述直角坐标系的原点重合；

通过联立以下两组方程求解出所述第一回旋曲线和所述第二回旋曲线的曲线参数：

其中，c₀为所述第一回旋曲线的曲率变化率，s₀为所述第一回旋曲线从其与所述第一圆弧的连接点到所述特征插补点之间的弧长，c₁为所述第二回旋曲线的曲率变化率，s₁为所述第二回旋曲线从所述特征插补点到其与所述第二圆弧的连接点之间的弧长，α₁为所述第一圆弧从所述第一圆弧未与所述第二圆弧连接的一端到所述第一圆弧与所述第一回旋曲线的连接点之间的圆弧片段的圆心角，α₂为所述第二圆弧从所述第二回旋曲线与所述第二圆弧的连接点到所述第二圆弧与所述第一圆弧的连接点之间的圆弧片段的圆心角，T_cw为旋转矩阵；c₀、s₀、c₁、s₁、α₁、α₂、T_cw为需要通过所述两组方程进行求解的未知量；

其中，R₁为所述第一圆弧的半径，R₂为所述第二圆弧的半径，E为所述预设轮廓误差，x₁为所述第一圆弧与所述第二圆弧的连接点在所述坐标轴上的坐标，y₁为所述第一圆弧与所述第二圆弧的连接点在所述直角坐标系另一坐标轴上的坐标，x₂为所述第二圆弧的圆心在所述坐标轴上的坐标，y₂为所述第二圆弧的圆心在所述直角坐标系另一坐标轴上的坐标，为所述第一圆弧在所述第一圆弧与所述第二圆弧的连接点的切向量，为所述第二圆弧在所述第一圆弧与所述第二圆弧的连接点的切向量，为所述第二圆弧的圆心到所述第一圆弧与所述第二圆弧的连接点的方向向量，R₁、R₂、x₁、y₁、x₂、y₂、为能够通过所述第一圆弧和所述第二圆弧及二者的相对位置关系获得的已知量。