[发明专利]一种基于PLCopen规范的轨迹过渡方法

权利要求书

1.一种基于PLCopen规范的轨迹过渡方法，应用于运动控制领域，采用圆弧插补的方式实现轨迹过渡；其特征在于，包括以下步骤：

S1：从运动控制器的指令缓冲区获取当前指令和下一个指令的曲线类型，构成相邻轨迹之间的曲线类型组合，相邻的轨迹之间采用过渡圆弧连接；

S2：根据曲线类型组合，建立关于过渡圆弧参数的约束方程；

S3：求解约束方程，得出过渡圆弧参数；

S4：采用S曲线的方式对过渡圆弧进行轨迹规划，再映射至笛卡尔坐标系的各个独立轴，实现相邻轨迹之间的圆弧插补；

步骤S1的曲线类型组合包括直线与直线、直线与曲线、曲线与直线和曲线与曲线组合；

当曲线类型组合为直线与直线时，即当前指令代表起点为S1、终点为E1的直线，下一个指令代表起点为S2、终点为E2的直线，E1和S2为同一点且为曲线过渡转角；此时，步骤S2的约束方程为：

Dist(CenP,过渡圆弧)＝Radius； (1)

Dist(CenP,E1)＝Radius+MaxDev； (2)

其中，CenP为过渡圆弧的圆心，Radius为过渡圆弧的半径；Dist(CenP,过渡圆弧)为过渡圆弧的圆心CenP到过渡圆弧上任一点的距离，Dist(CenP,E1)为过渡圆弧的圆心CenP到曲线过渡转角E1的距离，MaxDev为最大允许转角偏差；

当曲线类型组合为直线与曲线时，即当前指令代表起点为S1、终点为E1的直线，下一个指令代表圆心为PostCenP、半径为PostRadius、起点为S2、终点为E2的圆弧，E1和S2为同一点且为曲线过渡转角；

此时，步骤S2的约束方程为：

Dist(CenP，过渡圆弧)＝Radius； (3)

Dist(CenP,PostCenP)＝Radius+PostRadius； (4)

Dist(CenP,E1)＝Radius+MaxDev； (5)

其中，CenP为过渡圆弧的圆心，Radius为过渡圆弧的半径；Dist(CenP,过渡圆弧)为过渡圆弧的圆心CenP到过渡圆弧上任一点的距离，Dist(CenP,PostCenP)为过渡圆弧的圆心CenP到下一段指令圆心PostCenP的距离，Dist(CenP,E1)为过渡圆弧的圆心CenP到曲线过渡转角E1的距离，MaxDev为最大允许转角偏差；

当曲线类型组合为曲线与直线时，即当前指令代表圆心为PrevCenP、半径为PrevRadius、起点为S1、终点为E1的圆弧，下一个指令代表起点为S2、终点为E2的直线，E1和S2为同一点且为曲线过渡转角；

此时，步骤S2的约束方程为：

Dist(CenP,过渡圆弧)＝Radius； (7)

Dist(CenP,PrevCenP)＝Radius+PrevRadius； (8)

Dist(CenP,E1)＝Radius+MaxDev； (9)

其中，CenP为过渡圆弧的圆心，Radius为过渡圆弧的半径；Dist(CenP,过渡圆弧)为过渡圆弧的圆心CenP到过渡圆弧上任一点的距离，Dist(CenP,PrevCenP)为过渡圆弧的圆心CenP到当前指令圆心PrevCenP的距离，Dist(CenP,E1)为过渡圆弧的圆心CenP到曲线过渡转角E1的距离，MaxDev为最大允许转角偏差；

当曲线类型组合为曲线与曲线时，即当前指令代表圆心为PrevCenP、半径为PrevRadius、起点为S1、终点为E1的圆弧，下一个指令代表圆心为PostCenP、半径为PostRadius、起点为S2、终点为E2的圆弧，E1和S2为同一点且为曲线过渡转角；

此时，步骤S2中的约束方程为：

Dist(CenP,PrevCenP)＝Radius+PrevRadius； (10)

Dist(CenP,PostCenP)＝Radius+PostRadius； (11)

Dist(CenP,E1)＝Radius+MaxDev； (12)

其中，CenP为过渡圆弧的圆心，Radius为过渡圆弧的半径；Dist(CenP,PrevCenP)为过渡圆弧的圆心CenP到当前指令圆心PrevCenP的距离，Dist(CenP,PostCenP)为过渡圆弧的圆心CenP到下一个指令圆心PostCenP的距离，Dist(CenP,E1)为过渡圆弧的圆心CenP到曲线过渡转角E1的距离，MaxDev为最大允许转角偏差。

2.根据权利要求1所述的基于PLCopen规范的轨迹过渡方法，其特征在于：当曲线类型组合为直线与曲线、曲线与直线和曲线与曲线时，此时关于过渡圆弧的约束方程采用解析法和迭代法结合的方式求解，具体为：

根据约束方程建立关于过渡圆弧圆心Cenp的多变量非线性方程组F(x)＝0，其中，x＝(CenP[0],CenP[1],CenP[2])，Cenp[0]、Cenp[1]和Cenp[2]分别代表过渡圆弧圆心Cenp的X轴、Y轴和Z轴的坐标；

利用以下迭代方法进行迭代计算：

x^(k+1)＝x^(k)-F'(x^(k))^-1F(x^(k)),k＝0,1,...., (6)

其中，F'为F的雅克比矩阵；设置迭代的初始值x⁽⁰⁾为相邻轨迹的曲线过渡转角的坐标；从而，求解过渡圆弧的圆心Cenp的数值解及起始点E1’和终点S2’。