用于工業(yè)機器人的過輔助點姿態(tài)空間圓弧插補方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種插補方法，具體說是一種用于機器人的過輔助點姿態(tài)空間圓弧插 補方法?！颈尘凹夹g(shù)】
[0002] 機器人的軌跡規(guī)劃，在機器人控制中具有重要的作用，直接影響著控制的準確性 和快速性。而插補算法是整個機器人軌跡規(guī)劃控制過程的精華所在，占據(jù)著舉足輕重的地 位。通過示教機器人運動路徑上的某些關(guān)鍵點，然后根據(jù)軌跡特征算出這些示教點之間必 須到達的中間位置點，通過插補進行控制，從而實現(xiàn)高效率高精度的運動控制。
[0003] 當曲線軌跡為圓弧時，除了示教圓弧起點和終點外，至少還應(yīng)知道圓心或圓弧上 一個輔助點。顯然，示教圓心是很困難的，因而工業(yè)機器人終端執(zhí)行器的軌跡圓弧通常由示 教的圓弧起點、輔助點及圓弧終點決定，而這三點所決定的平面通常不一定平行于某一坐 標平面，因而需要研究空間任意三點圓弧的插補算法。
[0004] 目前普遍采用基于坐標轉(zhuǎn)換的空間圓弧插補方法，即通過坐標轉(zhuǎn)換將空間圓弧轉(zhuǎn) 化為平面圓弧，利用平面圓弧插補算法進行計算，之后再通過坐標轉(zhuǎn)換把插補點從平面結(jié) 果轉(zhuǎn)換為空間結(jié)果。此方法需要進行多次坐標變換，計算過程復雜，計算工作量大。
[0005] 目前工業(yè)機器人應(yīng)用的空間圓弧插補方法未提及過輔助點姿態(tài)；隨著工業(yè)發(fā)展的 進步，控制指令的簡便，某些特殊應(yīng)用場合如焊接、涂膠等需要經(jīng)過輔助點的姿態(tài)。

【發(fā)明內(nèi)容】

[0006] 本發(fā)明所要解決的技術(shù)問題，在于針對以上問題，克服現(xiàn)有技術(shù)存在的缺陷，提出 了一種用于工業(yè)機器人的過輔助點姿態(tài)空間圓弧插補方法。該插補方法示教任意不共線的 空間三點，不需要坐標轉(zhuǎn)換將空間點轉(zhuǎn)換成平面點進行計算，而是直接進行空間離散點的 計算，并且對姿態(tài)進行規(guī)劃使其過輔助點姿態(tài)，能根據(jù)給定的圓弧起點、輔助點及終點完成 任意空間圓弧插補。
[0007] 為實現(xiàn)上述發(fā)明目的，本發(fā)明采取的技術(shù)方案是：
[0008] 用于工業(yè)機器人的過輔助點姿態(tài)空間圓弧插補方法，包括以下步驟：
[0009] 步驟一：通過示教確定工業(yè)機器人的要求位姿，機器人控制器通過通信端口獲取 示教器提供的示教點信息，確定工業(yè)機器人軌跡的空間圓弧起點、輔助點及終點空間坐標 及位姿，分別為：Pi (Xi, y。Z!，a!，b!，c!)、P2(x2, y2, z2, a2, b2, c2)、P3(x3, y3, z3, a3, b3, c3);機器 人軌跡上各點位姿由位置矢量（x，y，z)和RPY姿態(tài)矢量（α，β，γ)共同描述，組合成一個 6 自由度的復合矢量（X，y, ζ, α，β，γ )，即（X，y, ζ, a, b, c)。
[0010] 步驟二：求出空間圓弧的圓心、半徑、法向量、圓心角和弧長：
[0011] A、空間圓弧起點、輔助點及終點為不共線的空間三點，確定了空間圓弧所在的平 面（簡稱圓弧平面），根據(jù)圓心到三點的距離都為半徑，聯(lián)立方程，可求出圓心的空間坐標 P。(X。，y。，ζ。)，進而求得半徑R ;
[0012] B、根據(jù)外積公式，求得垂直于圓弧平面的單位法向量
[0013] C、根據(jù)余弦定理和三個角之間的關(guān)系，求得空間圓弧的起點到輔助點、輔助點到 終點、起點到終點的圓心角分別為：Θ i，θ 2, θ 3( θ 3= Θ θ 2);
[0014] D、根據(jù)圓弧弧長公式，求得弧長L = R Θ 3;進而求得空間圓弧插補的總位移s ;
[0015] 步驟三：姿態(tài)規(guī)劃模塊規(guī)劃出經(jīng)過空間圓弧輔助點的姿態(tài)；
[0016] 各軸姿態(tài)隨圓心角變化的變化率公式為：
[0018] 分別對w( Θ )積分即是各軸姿態(tài)值，故可根據(jù)極值法設(shè)計使得各軸姿態(tài)變化率連 續(xù)，并求得相應(yīng)的系數(shù)值h，k2, m2;
[0019] 步驟四：速度軌跡規(guī)劃模塊計算出每個插補周期的插補位移；
[0020] 速度軌跡規(guī)劃模塊，可以根據(jù)現(xiàn)有技術(shù)規(guī)劃基于梯形曲線加減速控制或基于S形 曲線加減速控制或其它曲線控制；根據(jù)不同的曲線控制方式，進行速度規(guī)劃處理，計算圓弧 段所需要的插補總時間和插補信息，最后計算出每個插補周期的插補位移；
[0021] 步驟五：計算得到插補點的空間位姿；
[0022] 計算空間圓弧插補點位姿Pi (Xi, yi, Zi, a;, bi, cj步驟如下：
[0023] A、根據(jù)圓弧弧長公式，每個插補周期的插補位移除以半徑得到空間圓弧每個插補 周期的插補角度Θ ;
[0024] B、利用以下公式可求得插補點的空間坐標位置
[0028] C、利用以下公式可求得插補點的姿態(tài)
[0032] 步驟六：機器人控制器的中央處理器將最終得到的位姿通過通信端口提供給機器 人運動機構(gòu)進行執(zhí)行。
[0033] 本發(fā)明方法可以實現(xiàn)工業(yè)機器人的高精高效的運動控制，經(jīng)過輔助點姿態(tài)，并且 運動平滑。
[0034] 本發(fā)明的優(yōu)點：一是，本發(fā)明方法根據(jù)空間任意三點進行圓弧插補，避免了示教圓 心的困難和確定圓弧方向的問題；二是，本發(fā)明方法不需要進行坐標轉(zhuǎn)換計算平面圓弧插 補，而是直接計算空間角度和空間離散點得到實際空間圓弧插補點坐標，方法過程簡潔方 便、算法易于實現(xiàn)、計算效率高、插補精度高；三是，本發(fā)明方法通過姿態(tài)規(guī)劃模塊，使得實 際空間圓弧插補點經(jīng)過輔助點姿態(tài)，滿足某些特殊應(yīng)用場合；四是，本發(fā)明方法實現(xiàn)姿態(tài)變 化率與插補角度的連續(xù)關(guān)系，使得各軸姿態(tài)變化連續(xù)，機器人運動平滑。
[0035] 本方法利用空間幾何關(guān)系直接進行離散點計算可以實現(xiàn)多軸快速插補、控制精度 高；通過姿態(tài)規(guī)劃算法經(jīng)過輔助點姿態(tài)，同時實現(xiàn)姿態(tài)連續(xù)且姿態(tài)變化率連續(xù)的關(guān)系，可以 使其更好的應(yīng)用于某些特殊場合。
【附圖說明】
[0036] 圖1為本發(fā)明用于工業(yè)機器人的過輔助點姿態(tài)空間圓弧插補方法的流程圖。
[0037] 圖2為本發(fā)明中的空間圓弧示例圖。
【具體實施方式】
[0038] 下面結(jié)合附圖和實施例，對本發(fā)明做進一步詳細說明。
[0039] 參照圖1所示，本發(fā)明用于工業(yè)機器人的過輔助點姿態(tài)空間圓弧插補算法，包括 如下步驟：
[0040] 步驟一：通過示教確定工業(yè)機器人的要求位姿，機器人控制器通過通信端口獲取 示教器提供的示教點信息，確定工業(yè)機器人軌跡的空間圓弧起點、輔助點及終點空間坐標 及位姿，分別為：Pi (Xi, y。Z!，a!，b!，c!)、P2(x2, y2, z2, a2, b2, c2)、P3(x3, y