3, z3, a3, b3, c3);機(jī)器 人軌跡上各點(diǎn)位姿由位置矢量（x，y，z)和RPY姿態(tài)矢量（α，β，γ)共同描述，組合成一個(gè) 6 自由度的復(fù)合矢量（X，y, ζ, α，β，γ )，即（X，y, ζ, a, b, c)。
[0041] 步驟二：求出空間圓弧的圓心、半徑、法向量、圓心角和弧長：
[0042] A、空間圓弧起點(diǎn)、輔助點(diǎn)及終點(diǎn)為不共線的空間三點(diǎn)，確定了空間圓弧所在的平 面（簡稱圓弧平面），根據(jù)圓心到三點(diǎn)的距離都為半徑，聯(lián)立方程，可求出圓心的空間坐標(biāo) P。(X。，y。，ζ。)，進(jìn)而求得半徑R ;
[0043] B、根據(jù)外積公式，求得垂直于圓弧平面的單位法向量
[0044] C、根據(jù)余弦定理和三個(gè)角之間的關(guān)系，求得空間圓弧的起點(diǎn)到輔助點(diǎn)、輔助點(diǎn)到 終點(diǎn)、起點(diǎn)到終點(diǎn)的圓心角分別為：Θ i，Θ 2, θ 3( θ 3= Θ彳Θ 2);
[0045] D、根據(jù)圓弧弧長公式，求得弧長L = R Θ 3;進(jìn)而求得空間圓弧插補(bǔ)的總位移S ;
[0046] 步驟三：姿態(tài)規(guī)劃模塊規(guī)劃出經(jīng)過空間圓弧輔助點(diǎn)的姿態(tài)；
[0047] 各軸姿態(tài)隨圓心角變化的變化率公式為：
[0049] 分別對w( Θ )積分即是各軸姿態(tài)值，故可根據(jù)極值法設(shè)計(jì)使得各軸姿態(tài)變化率連 續(xù)，并求得相應(yīng)的系數(shù)值h，k2, m2;
[0050] 起點(diǎn)到輔助點(diǎn)的各軸姿態(tài)線性變化率為Wl，輔助點(diǎn)到終點(diǎn)的各軸姿態(tài)線性變化率 為％;當(dāng)w2= 2?^時(shí)，可設(shè)計(jì)系數(shù)分別為
公式1可實(shí)例為
進(jìn)行求解各軸姿態(tài)變化率；
[0051] 步驟四：軌跡速度規(guī)劃模塊計(jì)算出每個(gè)插補(bǔ)周期的插補(bǔ)位移s(iT);
[0052] 軌跡速度規(guī)劃模塊，可以根據(jù)現(xiàn)有技術(shù)規(guī)劃基于梯形曲線加減速控制或基于S形 曲線加減速控制或其它曲線控制；以下以梯形曲線加減速控制為例，介紹計(jì)算每個(gè)插補(bǔ)周 期插補(bǔ)位移的具體步驟如下：
[0053] A、基于梯形曲線加減速控制的空間圓弧插補(bǔ)的加減速時(shí)間計(jì)算公式為：
[0055] 式中，h為空間圓弧插補(bǔ)的加速過程時(shí)間；t 2為空間圓弧插補(bǔ)的勻速過程時(shí)間；13為空間圓弧插補(bǔ)的減速過程時(shí)間；v為空間圓弧插補(bǔ)線速度，空間圓弧起點(diǎn)和終點(diǎn)的速度 都設(shè)為〇 ;a為空間圓弧插補(bǔ)加速度和減速度；
[0056] B、第i個(gè)插補(bǔ)周期的加速度計(jì)算公式為：

[0058] 式中，T為空間圓弧插補(bǔ)周期，&為$需要的插補(bǔ)周期總個(gè)數(shù)，隊(duì)為 需要的插 補(bǔ)周期總個(gè)數(shù)，隊(duì)為
需要的插補(bǔ)周期總個(gè)數(shù)，i = 〇, 1，2,…，N3;
[0059] C、第i個(gè)插補(bǔ)周期的線速度計(jì)算公式為：
[0061] D、第i個(gè)插補(bǔ)周期的插補(bǔ)位移計(jì)算公式為：
[0063] 步驟五：計(jì)算得到插補(bǔ)點(diǎn)的空間位姿；
[0064] 計(jì)算空間圓弧插補(bǔ)點(diǎn)位姿Pi (Xi, y。Zi, a;, bi, cj步驟如下：
[0065] A、計(jì)算空間圓弧每個(gè)插補(bǔ)周期的插補(bǔ)角度
[0066] B、利用以下公式可求得插補(bǔ)點(diǎn)的空間坐標(biāo)位置
[0070] C、利用以下公式可求得插補(bǔ)點(diǎn)的姿態(tài)
[0074] 步驟六：機(jī)器人控制器的中央處理器將最終得到的位姿通過通信端口提供給機(jī)器 人運(yùn)動(dòng)機(jī)構(gòu)進(jìn)行執(zhí)行。
【主權(quán)項(xiàng)】
1.用于工業(yè)機(jī)器人的過輔助點(diǎn)姿態(tài)空間圓弧插補(bǔ)方法，包括以下步驟： 步驟一：通過示教確定工業(yè)機(jī)器人的要求位姿，機(jī)器人控制器通過通信端口獲取示教 器提供的示教點(diǎn)信息，確定工業(yè)機(jī)器人軌跡的空間圓弧起點(diǎn)、輔助點(diǎn)及終點(diǎn)空間坐標(biāo)及位 跡上各點(diǎn)位姿由位置矢量（x，y，z)和RPY姿態(tài)矢量（α，β，γ)共同描述，組合成一個(gè)6自 由度的復(fù)合矢量（X，y, Ζ, α，β，γ )，即（X，y, Ζ, a, b, c); 步驟二：確定空間圓弧的圓心、半徑、法向量、圓心角和弧長： A、 空間圓弧起點(diǎn)、輔助點(diǎn)及終點(diǎn)為不共線的空間三點(diǎn)，確定了空間圓弧所在的平面，根 據(jù)圓心到三點(diǎn)的距離都為半徑，聯(lián)立方程，求出圓心的空間坐標(biāo)Pjx。，y。，ζ。），進(jìn)而求得半 徑R; B、 根據(jù)外積公式，求得垂直于圓弧平面的單位法向量C、 根據(jù)余弦定理和三個(gè)角之間的關(guān)系，求得空間圓弧的起點(diǎn)到輔助點(diǎn)、輔助點(diǎn)到終點(diǎn)、 起點(diǎn)到終點(diǎn)的圓心角分別為：Θ i，Θ 2, θ 3, ; Θ 3= θ 1+ Θ 2; D、 根據(jù)圓弧弧長公式，求得弧長L = R Θ 3;進(jìn)而求得空間圓弧插補(bǔ)的總位移S ; 步驟三：姿態(tài)規(guī)劃模塊規(guī)劃出經(jīng)過空間圓弧輔助點(diǎn)的姿態(tài)； 各軸姿態(tài)隨圓心角變化的變化率公式為：步驟四：軌跡速度規(guī)劃模塊計(jì)算出每個(gè)插補(bǔ)周期的插補(bǔ)位移； 軌跡速度規(guī)劃模塊，可以根據(jù)現(xiàn)有技術(shù)規(guī)劃基于梯形曲線加減速控制或基于S形曲線 加減速控制或其它曲線控制；根據(jù)不同的曲線控制方式，進(jìn)行速度規(guī)劃處理，計(jì)算圓弧段所 需要的插補(bǔ)總時(shí)間和插補(bǔ)信息，最后計(jì)算出每個(gè)插補(bǔ)周期的插補(bǔ)位移； 步驟五：計(jì)算得到插補(bǔ)點(diǎn)的空間位姿； 計(jì)算空間圓弧插補(bǔ)點(diǎn)位姿Pi (Xi, yi, Zi, a;, bi, Ci)，步驟如下： A、 根據(jù)圓弧弧長公式，每個(gè)插補(bǔ)周期的插補(bǔ)位移除以半徑得到空間圓弧每個(gè)插補(bǔ)周期 的插補(bǔ)角度Θ ; B、 利用以下公式可求得插補(bǔ)點(diǎn)的空間坐標(biāo)位置：C、 利用以下公式可求得插補(bǔ)點(diǎn)的姿態(tài)步驟六：機(jī)器人控制器的中央處理器將最終得到的位姿通過通信端口提供給機(jī)器人運(yùn) 動(dòng)機(jī)構(gòu)進(jìn)行執(zhí)行。
【專利摘要】本發(fā)明公開用于工業(yè)機(jī)器人的過輔助點(diǎn)姿態(tài)空間圓弧插補(bǔ)方法，包括以下步驟：機(jī)器人控制器通過通信端口確定了示教器示教的空間不共線三點(diǎn)；根據(jù)空間幾何關(guān)系直接進(jìn)行空間離散點(diǎn)的計(jì)算，求出空間圓弧的圓心、半徑、法向量、圓心角和弧長；對姿態(tài)進(jìn)行規(guī)劃使運(yùn)動(dòng)軌跡經(jīng)過輔助點(diǎn)姿態(tài)且軌跡光滑；速度軌跡規(guī)劃模塊計(jì)算出每個(gè)插補(bǔ)周期的插補(bǔ)位移；利用實(shí)時(shí)插補(bǔ)算法計(jì)算出每個(gè)插補(bǔ)周期的插補(bǔ)點(diǎn)位姿；將最終滿足示教要求的位姿通過通信端口提供給機(jī)器人運(yùn)動(dòng)機(jī)構(gòu)并執(zhí)行。本發(fā)明避免了示教圓心的困難和確定圓弧方向的問題；計(jì)算效率高、插補(bǔ)精度高，可實(shí)現(xiàn)機(jī)器人的快速插補(bǔ)、控制精度高；運(yùn)行平滑地經(jīng)過輔助點(diǎn)姿態(tài)，拓寬了機(jī)器人的應(yīng)用場合。
【IPC分類】G05B19/42, G05B19/4105
【公開號(hào)】CN105353725
【申請?zhí)枴緾N201510796042
【發(fā)明人】臧秀娟, 馮日月, 夏正仙, 王繼虎, 王明昕
【申請人】南京埃斯頓機(jī)器人工程有限公司
【公開日】2016年2月24日
【申請日】2015年11月18日