[發明專利]一種基于PLCopen規范的軌跡過渡方法

有效

• 申請號: 201910732642.3
• 申請日: 2019-08-09
• 公開（公告）號: CN110488758B
• 公開（公告）日: 2021-05-25
• 發明（設計）人: 楊曉文;張圣;錢巍
• 申請（專利權）人: 南京埃斯頓自動化股份有限公司
• 主分類號: G05B19/41
• 分類號: G05B19/41
• 代理公司: 南京天翼專利代理有限責任公司 32112
• 代理人: 蔣家華

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’。