一種五自由度混聯(lián)機(jī)器人的雙閉環(huán)控制方法
【專利摘要】本發(fā)明公開了一種含轉(zhuǎn)動(dòng)支架的五自由度混聯(lián)機(jī)器人的雙閉環(huán)控制方法，步驟為：利用第一、第二圓光柵實(shí)時(shí)檢測(cè)第一、第四轉(zhuǎn)動(dòng)副的軸線的實(shí)際轉(zhuǎn)角，利用直線光柵實(shí)時(shí)檢測(cè)從動(dòng)長度調(diào)節(jié)裝置沿移動(dòng)副方向的移動(dòng)距離，將實(shí)際轉(zhuǎn)角和實(shí)際桿長信息通過算法轉(zhuǎn)換為驅(qū)動(dòng)三個(gè)伺服電機(jī)的實(shí)際轉(zhuǎn)角，并將其與指令值比較，經(jīng)PID調(diào)節(jié)轉(zhuǎn)換為速度指令，進(jìn)而實(shí)現(xiàn)位置環(huán)反饋控制；通過伺服電機(jī)的絕對(duì)式角度編碼器，實(shí)時(shí)檢測(cè)三個(gè)伺服電機(jī)軸的實(shí)際轉(zhuǎn)角并分別作微分處理得到電機(jī)的實(shí)際轉(zhuǎn)速，分別與相應(yīng)的速度指令進(jìn)行比較，進(jìn)而通過PI調(diào)節(jié)實(shí)現(xiàn)電機(jī)的速度環(huán)反饋控制。本發(fā)明可在線實(shí)時(shí)補(bǔ)償主動(dòng)長度調(diào)節(jié)裝置因絲杠傳動(dòng)誤差、熱伸長和因軸向彈性變形造成的桿長誤差。
【專利說明】
一種五自由度混聯(lián)機(jī)器人的雙閉環(huán)控制方法
技術(shù)領(lǐng)域
[0001 ]本發(fā)明涉及機(jī)器人的雙閉環(huán)控制方法，尤其涉及一種五自由度混聯(lián)機(jī)器人的雙閉 環(huán)控制方法。
【背景技術(shù)】
[0002] 工業(yè)機(jī)器人普遍采用半閉環(huán)控制策略，即利用編碼器通過檢測(cè)各關(guān)節(jié)伺服電機(jī)的 實(shí)際轉(zhuǎn)角構(gòu)成位置和速度半閉環(huán)控制。這種控制策略調(diào)試維修方便，穩(wěn)定性好，已得到廣泛 應(yīng)用。但半閉環(huán)控制無法補(bǔ)償機(jī)械系統(tǒng)傳動(dòng)誤差及由構(gòu)件彈性變形所造成的末端執(zhí)行器位 姿誤差。如可直接檢測(cè)到機(jī)器人機(jī)構(gòu)輸出構(gòu)件的位姿信息，則可實(shí)施全閉環(huán)反饋控制，進(jìn)而 有利于提尚機(jī)器人末端執(zhí)彳丁器的運(yùn)動(dòng)精度。
[0003] 專利US006301525B1公開了對(duì)一種五自由度混聯(lián)機(jī)器人中一平動(dòng)兩轉(zhuǎn)動(dòng)并聯(lián)機(jī)構(gòu) 的控制方法。該方法在從動(dòng)長度調(diào)節(jié)裝置與機(jī)架相連的虎克鉸兩個(gè)軸線上安裝兩個(gè)圓光 柵，沿從動(dòng)長度調(diào)節(jié)裝置軸線安裝一個(gè)直線光柵。利用這些傳感器檢測(cè)到的轉(zhuǎn)角和長度信 息，首先計(jì)算出并聯(lián)機(jī)構(gòu)動(dòng)平臺(tái)參考點(diǎn)的位置誤差，然后再經(jīng)誤差逆解模型得到三個(gè)主動(dòng) 長度調(diào)節(jié)裝置伺服電機(jī)的轉(zhuǎn)角誤差，進(jìn)而用這些轉(zhuǎn)角誤差于修改理想控制指令。然后再利 用伺服電機(jī)的編碼器檢測(cè)信息實(shí)現(xiàn)位置和速度環(huán)調(diào)節(jié)。這種方法必須經(jīng)過兩步運(yùn)算，計(jì)算 時(shí)間長，難于以伺服更新速率實(shí)現(xiàn)。此外，在從動(dòng)長度調(diào)節(jié)裝置與機(jī)架相連的虎克鉸上安裝 的圓光柵開放性不好，不便于維修和更換。

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

[0004] 本發(fā)明的目的在于克服現(xiàn)有技術(shù)的不足，提供一種可實(shí)時(shí)補(bǔ)償其中并聯(lián)機(jī)構(gòu)三個(gè) 主動(dòng)長度調(diào)節(jié)裝置因絲杠傳動(dòng)誤差、熱誤差及構(gòu)件彈性變形引起的桿長誤差，有利于提高 五自由度混聯(lián)機(jī)器人的運(yùn)動(dòng)控制精度的五自由度混聯(lián)機(jī)器人的雙閉環(huán)控制方法。
[0005] 本發(fā)明的一種含轉(zhuǎn)動(dòng)支架的五自由度混聯(lián)機(jī)器人的雙閉環(huán)控制方法，它包括以下 步驟：
[0006] (1)安裝第一圓光柵、第二圓光柵以及直線光柵；
[0007] 其中在轉(zhuǎn)動(dòng)支架與機(jī)架連接的第一轉(zhuǎn)動(dòng)副的轉(zhuǎn)軸上安裝第一圓光柵的尺身，在第 一固定軸座上安裝第一圓光柵的讀數(shù)頭;在轉(zhuǎn)動(dòng)支架與從動(dòng)長度調(diào)節(jié)裝置連接的第四轉(zhuǎn)動(dòng) 副的轉(zhuǎn)軸上安裝第二圓光柵的尺身，在轉(zhuǎn)動(dòng)支架上安裝第二圓光柵的讀數(shù)頭;在從動(dòng)長度 調(diào)節(jié)裝置軸線移動(dòng)方向安裝直線光柵的尺身，在從動(dòng)長度調(diào)節(jié)裝置與轉(zhuǎn)動(dòng)支架所連接的第 四轉(zhuǎn)動(dòng)副的內(nèi)圈內(nèi)側(cè)安裝直線光柵讀數(shù)頭；
[0008] (2)利用第一圓光柵、第二圓光柵實(shí)時(shí)檢測(cè)第一轉(zhuǎn)動(dòng)副和第四轉(zhuǎn)動(dòng)副的軸線的實(shí) 際轉(zhuǎn)角，利用直線光柵實(shí)時(shí)檢測(cè)從動(dòng)長度調(diào)節(jié)裝置沿移動(dòng)副方向的移動(dòng)距離，從而得到從 動(dòng)長度調(diào)節(jié)裝置實(shí)際桿長，將所述的實(shí)際轉(zhuǎn)角和實(shí)際桿長信息通過算法轉(zhuǎn)換為驅(qū)動(dòng)三個(gè)主 動(dòng)長度調(diào)節(jié)裝置的伺服電機(jī)的實(shí)際轉(zhuǎn)角，并將其與指令值比較，經(jīng)位置環(huán)控制器PID調(diào)節(jié)轉(zhuǎn) 換為速度指令，進(jìn)而實(shí)現(xiàn)位置環(huán)反饋控制;通過驅(qū)動(dòng)三個(gè)主動(dòng)長度調(diào)節(jié)裝置的伺服電機(jī)的 電機(jī)軸上安裝的絕對(duì)式角度編碼器，實(shí)時(shí)檢測(cè)三個(gè)伺服電機(jī)軸的實(shí)際轉(zhuǎn)角并分別作微分處 理得到三個(gè)伺服電機(jī)的實(shí)際電機(jī)轉(zhuǎn)速，分別與相應(yīng)的所述的速度指令進(jìn)行比較，進(jìn)而通過 伺服放大器PI調(diào)節(jié)實(shí)現(xiàn)三個(gè)伺服電機(jī)的速度環(huán)反饋控制；
[0009] 所述的算法為：
[0010]
[0011]
[0012] 式中：
[0013] p一一主動(dòng)長度調(diào)節(jié)裝置中滾珠絲杠的導(dǎo)程
[0014] q4一一通過直線光柵檢測(cè)的移動(dòng)距離加上從動(dòng)長度調(diào)節(jié)裝置初始位置桿長而得 到的從動(dòng)長度調(diào)節(jié)裝置桿長
[0015] φ一一第一圓光柵檢測(cè)到的第一轉(zhuǎn)動(dòng)副的軸線的實(shí)際轉(zhuǎn)角
[0016] Θ一一第二圓光柵檢測(cè)到的第四轉(zhuǎn)動(dòng)副的軸線的實(shí)際轉(zhuǎn)角
[0017] ai 一一由第一球鏈中心到從動(dòng)調(diào)節(jié)裝置軸線的理論垂距
[0018] a2一一由第二、三球鏈中心到從動(dòng)調(diào)節(jié)裝置軸線的理論垂距
[0019] bi一一連接第一主動(dòng)長度調(diào)節(jié)裝置與第二固定軸座的虎克鉸的近架轉(zhuǎn)軸軸線到 轉(zhuǎn)動(dòng)支架與機(jī)架相連轉(zhuǎn)軸軸線間的理論距離
[0020] b2一一連接第二主動(dòng)長度調(diào)節(jié)裝置與轉(zhuǎn)動(dòng)支架的第二轉(zhuǎn)動(dòng)副的轉(zhuǎn)軸軸線到連接 從動(dòng)長度調(diào)節(jié)裝置與轉(zhuǎn)動(dòng)支架的第四轉(zhuǎn)動(dòng)副的轉(zhuǎn)軸軸線間的理論距離或者連接第三主動(dòng) 長度調(diào)節(jié)裝置與轉(zhuǎn)動(dòng)支架的第三轉(zhuǎn)動(dòng)副的轉(zhuǎn)軸軸線到連接從動(dòng)長度調(diào)節(jié)裝置與轉(zhuǎn)動(dòng)支架 的第四轉(zhuǎn)動(dòng)副的轉(zhuǎn)軸軸線間的理論距離
[0021] 0la、02a、03a--由第一圓光柵、第二圓光柵和直線光柵檢測(cè)信息計(jì)算得到的驅(qū)動(dòng) 三個(gè)主動(dòng)長度調(diào)節(jié)裝置的伺服電機(jī)的實(shí)際轉(zhuǎn)角。
[0022] 本發(fā)明方法與現(xiàn)有技術(shù)相比，可在線實(shí)時(shí)補(bǔ)償并聯(lián)機(jī)構(gòu)中三個(gè)主動(dòng)長度調(diào)節(jié)裝置 因絲杠傳動(dòng)誤差、熱伸長和因軸向彈性變形造成的桿長誤差，彌補(bǔ)了以往傳統(tǒng)半閉環(huán)控制 控制精度不高的局限性，且三個(gè)主動(dòng)長度調(diào)節(jié)裝置伺服電機(jī)轉(zhuǎn)角與轉(zhuǎn)動(dòng)支架相對(duì)機(jī)架的轉(zhuǎn) 角、從動(dòng)長度調(diào)節(jié)裝置相對(duì)轉(zhuǎn)動(dòng)支架的轉(zhuǎn)角，以及從動(dòng)長度調(diào)節(jié)裝置桿長的數(shù)學(xué)關(guān)系非常 簡單，故可以每個(gè)伺服周期更新一次的速率完成位置閉環(huán)控制，可滿足這種機(jī)器人高速高 精度運(yùn)動(dòng)控制的需求。此外，在轉(zhuǎn)動(dòng)支架上安裝的兩個(gè)圓光柵開放性好，便于維修和更換。
【附圖說明】
[0023] 圖1是五自由度混聯(lián)機(jī)器人的一個(gè)方向的結(jié)構(gòu)示意圖；
[0024] 圖2是五自由度混聯(lián)機(jī)器人的另一個(gè)方向的結(jié)構(gòu)示意圖；
[0025] 圖3是圓光柵和直光柵安裝在圖1所示的機(jī)器人中的結(jié)構(gòu)示意圖；
[0026]圖4是本發(fā)明的一種五自由度混聯(lián)機(jī)器人的雙閉環(huán)控制方法的控制原理圖；
[0027]圖5是五自由度混聯(lián)機(jī)器人的尺度參數(shù)示意圖。
【具體實(shí)施方式】
[0028]下面結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明加以詳細(xì)說明。
[0029] 本發(fā)明是對(duì)公開號(hào)為CN104985596A的中國專利公開的一種含轉(zhuǎn)動(dòng)支架的五自由 度混聯(lián)機(jī)器人的雙閉環(huán)控制方法。
[0030] 下面結(jié)合附圖簡要說明CN104985596A中公開的五自由度混聯(lián)機(jī)器人的結(jié)構(gòu)，該機(jī) 器人包括具有一個(gè)平動(dòng)和二個(gè)轉(zhuǎn)動(dòng)的三自由度并聯(lián)機(jī)構(gòu)和與之串接的具有兩個(gè)轉(zhuǎn)動(dòng)自由 度的轉(zhuǎn)頭組成，轉(zhuǎn)頭6與動(dòng)平臺(tái)5串接。所述的三自由度并聯(lián)機(jī)構(gòu)包括轉(zhuǎn)動(dòng)支架12,所述的轉(zhuǎn) 動(dòng)支架12的兩側(cè)各自通過第一轉(zhuǎn)動(dòng)副55與安裝在機(jī)架上的兩個(gè)第一固定軸座11轉(zhuǎn)動(dòng)連接。 第二主動(dòng)長度調(diào)節(jié)裝置3以及第三主動(dòng)長度調(diào)節(jié)裝置4上部分別通過第二轉(zhuǎn)動(dòng)副35、第三轉(zhuǎn) 動(dòng)副45對(duì)稱的轉(zhuǎn)動(dòng)連接在所述的轉(zhuǎn)動(dòng)支架12兩側(cè)并且底端分別通過第二球鉸34、第三球鉸 44與所述的動(dòng)平臺(tái)5轉(zhuǎn)動(dòng)連接，從動(dòng)長度調(diào)節(jié)裝置1與轉(zhuǎn)動(dòng)支架12通過第四轉(zhuǎn)動(dòng)副15轉(zhuǎn)動(dòng)相 連并且從動(dòng)長度調(diào)節(jié)裝置1底部與動(dòng)平臺(tái)固定相連，第一主動(dòng)長度調(diào)節(jié)裝置2上部通過虎克 鉸25與安裝在機(jī)架上的兩個(gè)第二固定軸座21轉(zhuǎn)動(dòng)連接并且底端通過第一球鉸24與動(dòng)平臺(tái)5 轉(zhuǎn)動(dòng)連接；
[0031] 所述的第一主動(dòng)長度調(diào)節(jié)裝置2、第二主動(dòng)長度調(diào)節(jié)裝置3以及第三主動(dòng)長度調(diào)節(jié) 裝置4分別由第一伺服電機(jī)23、第二伺服電機(jī)33、第三伺服電機(jī)43驅(qū)動(dòng)以使三個(gè)主動(dòng)長度調(diào) 節(jié)裝置2、3、4可以伸長和縮短，使得所述動(dòng)平臺(tái)5可繞第一轉(zhuǎn)動(dòng)副55的軸線以及第四轉(zhuǎn)動(dòng)副 15的軸線轉(zhuǎn)動(dòng)，及可沿從動(dòng)長度調(diào)節(jié)裝置1的軸線移動(dòng)，進(jìn)而形成一個(gè)平動(dòng)和兩個(gè)轉(zhuǎn)動(dòng)自由 度。第二轉(zhuǎn)動(dòng)副35的轉(zhuǎn)軸軸線、第三轉(zhuǎn)動(dòng)副轉(zhuǎn)軸軸線45以及第四轉(zhuǎn)動(dòng)副15轉(zhuǎn)軸軸線彼此平 行并且與第一轉(zhuǎn)動(dòng)副55的轉(zhuǎn)軸軸線正交。
[0032] 本發(fā)明的一種含轉(zhuǎn)動(dòng)支架的五自由度混聯(lián)機(jī)器人的雙閉環(huán)控制方法，它包括以下 步驟：
[0033] (1)安裝第一圓光柵8、第二圓光柵9以及直線光柵7;
[0034]其中在轉(zhuǎn)動(dòng)支架12與機(jī)架連接的第一轉(zhuǎn)動(dòng)副55的轉(zhuǎn)軸上安裝第一圓光柵8的尺 身，在第一固定軸座11上安裝第一圓光柵的讀數(shù)頭;在轉(zhuǎn)動(dòng)支架12與從動(dòng)長度調(diào)節(jié)裝置1連 接的第四轉(zhuǎn)動(dòng)副15的轉(zhuǎn)軸上安裝第二圓光柵9的尺身，在轉(zhuǎn)動(dòng)支架12上安裝第二圓光柵的 讀數(shù)頭;在從動(dòng)長度調(diào)節(jié)裝置1軸線移動(dòng)方向安裝直線光柵7的尺身，在從動(dòng)長度調(diào)節(jié)裝置 與轉(zhuǎn)動(dòng)支架所連接的第四轉(zhuǎn)動(dòng)副15的內(nèi)圈內(nèi)側(cè)安裝直線光柵讀數(shù)頭；
[0035] (2)利用第一圓光柵8、第二圓光柵9實(shí)時(shí)檢測(cè)第一轉(zhuǎn)動(dòng)副55和第四轉(zhuǎn)動(dòng)副15的軸 線的實(shí)際轉(zhuǎn)角Φ、θ，利用直線光柵7實(shí)時(shí)檢測(cè)從動(dòng)長度調(diào)節(jié)裝置1沿移動(dòng)副方向的移動(dòng)距離， 從而得到從動(dòng)長度調(diào)節(jié)裝置1實(shí)際桿長q4,將所述的實(shí)際轉(zhuǎn)角和實(shí)際桿長信息通過算法轉(zhuǎn) 換為驅(qū)動(dòng)三個(gè)主動(dòng)長度調(diào)節(jié)裝置的伺服電機(jī)的實(shí)際轉(zhuǎn)角9 la、02a、03a，并將其與指令值0ld、 0 2d、03d比較，經(jīng)位置環(huán)控制器PID調(diào)節(jié)轉(zhuǎn)換為速度指令九.,、么(/、進(jìn)而實(shí)現(xiàn)位置環(huán)反 饋控制；通過驅(qū)動(dòng)三個(gè)主動(dòng)長度調(diào)節(jié)裝置的伺服電機(jī)的電機(jī)軸上安裝的絕對(duì)式角度編碼 器，實(shí)時(shí)檢測(cè)三個(gè)伺服電機(jī)軸的實(shí)際轉(zhuǎn)角并分別作微分處理得到三個(gè)伺服電機(jī)的實(shí)際電機(jī) 轉(zhuǎn)速<,、4,,、4,,，分別與相應(yīng)的所述的速度指令進(jìn)行比較，進(jìn)而通過伺服放大器PI調(diào)節(jié) 實(shí)現(xiàn)三個(gè)伺服電機(jī)的速度環(huán)反饋控制；
[0036]所述的算法為：
[0037]
[0038]
[0039] 式中：
[0040] p一一主動(dòng)長度調(diào)節(jié)裝置中滾珠絲杠的導(dǎo)程
[0041] Q4一一通過直線光柵7檢測(cè)的移動(dòng)距離加上從動(dòng)長度調(diào)節(jié)裝置初始位置桿長而得 到的從動(dòng)長度調(diào)節(jié)裝置1桿長
[0042] Φ一一第一圓光柵8檢測(cè)到的第一轉(zhuǎn)動(dòng)副55的軸線的實(shí)際轉(zhuǎn)角
[0043] Θ一一第二圓光柵9檢測(cè)到的第四轉(zhuǎn)動(dòng)副15的軸線的實(shí)際轉(zhuǎn)角
[0044] ai一一由第一球鏈24中心到從動(dòng)調(diào)節(jié)裝置1軸線的理論垂距
[0045] a2一一由第二、三球鏈34、44中心到從動(dòng)調(diào)節(jié)裝置1軸線的理論垂距
[0046] bi一一連接第一主動(dòng)長度調(diào)節(jié)裝置2與第二固定軸座21的虎克鉸的近架轉(zhuǎn)軸軸線 到轉(zhuǎn)動(dòng)支架12與機(jī)架相連轉(zhuǎn)軸軸線間的理論距離
[0047] b2一一連接第二主動(dòng)長度調(diào)節(jié)裝置3與轉(zhuǎn)動(dòng)支架的第二轉(zhuǎn)動(dòng)副35的轉(zhuǎn)軸軸線到連 接從動(dòng)長度調(diào)節(jié)裝置1與轉(zhuǎn)動(dòng)支架的第四轉(zhuǎn)動(dòng)副15的轉(zhuǎn)軸軸線間的理論距離或者連接第三 主動(dòng)長度調(diào)節(jié)裝置4與轉(zhuǎn)動(dòng)支架的第三轉(zhuǎn)動(dòng)副45的轉(zhuǎn)軸軸線到連接從動(dòng)長度調(diào)節(jié)裝置1與 轉(zhuǎn)動(dòng)支架的第四轉(zhuǎn)動(dòng)副15的轉(zhuǎn)軸軸線間的理論距離
[0048] 0la、02a、03a-一由第一圓光柵8、第二圓光柵9和直線光柵7檢測(cè)信息計(jì)算得到的驅(qū) 動(dòng)三個(gè)主動(dòng)長度調(diào)節(jié)裝置的伺服電機(jī)的實(shí)際轉(zhuǎn)角。
[0049] 本發(fā)明提出控制方法的有益之處在于，可在線實(shí)時(shí)補(bǔ)償并聯(lián)機(jī)構(gòu)中三個(gè)主動(dòng)長度 調(diào)節(jié)裝置因絲杠傳動(dòng)誤差、熱伸長和因軸向彈性變形造成的桿長誤差，彌補(bǔ)了以往傳統(tǒng)半 閉環(huán)控制控制精度不高的局限性，且三個(gè)主動(dòng)長度調(diào)節(jié)裝置伺服電機(jī)轉(zhuǎn)角與轉(zhuǎn)動(dòng)支架相對(duì) 機(jī)架的轉(zhuǎn)角、從動(dòng)長度調(diào)節(jié)裝置相對(duì)轉(zhuǎn)動(dòng)支架的轉(zhuǎn)角，以及動(dòng)長度調(diào)節(jié)裝置桿長的數(shù)學(xué)關(guān) 系非常簡單，故可以每個(gè)伺服周期更新一次的速率完成位置閉環(huán)控制，可滿足這種機(jī)器人 高速高精度運(yùn)動(dòng)控制的需求。此外，在轉(zhuǎn)動(dòng)支架上安裝的兩個(gè)圓光柵開放性好，便于維修和 更換。
【主權(quán)項(xiàng)】
1.一種含轉(zhuǎn)動(dòng)支架的五自由度混聯(lián)機(jī)器人的雙閉環(huán)控制方法，其特征在于它包括以下 步驟： (1) 安裝第一圓光柵、第二圓光柵以及直線光柵； 其中在轉(zhuǎn)動(dòng)支架與機(jī)架連接的第一轉(zhuǎn)動(dòng)副的轉(zhuǎn)軸上安裝第一圓光柵的尺身，在第一固 定軸座上安裝第一圓光柵的讀數(shù)頭;在轉(zhuǎn)動(dòng)支架與從動(dòng)長度調(diào)節(jié)裝置連接的第四轉(zhuǎn)動(dòng)副的 轉(zhuǎn)軸上安裝第二圓光柵的尺身，在轉(zhuǎn)動(dòng)支架上安裝第二圓光柵的讀數(shù)頭;在從動(dòng)長度調(diào)節(jié) 裝置軸線移動(dòng)方向安裝直線光柵的尺身，在從動(dòng)長度調(diào)節(jié)裝置與轉(zhuǎn)動(dòng)支架所連接的第四轉(zhuǎn) 動(dòng)副的內(nèi)圈內(nèi)側(cè)安裝直線光柵讀數(shù)頭； (2) 利用第一圓光柵、第二圓光柵實(shí)時(shí)檢測(cè)第一轉(zhuǎn)動(dòng)副和第四轉(zhuǎn)動(dòng)副的軸線的實(shí)際轉(zhuǎn) 角，利用直線光柵實(shí)時(shí)檢測(cè)從動(dòng)長度調(diào)節(jié)裝置沿移動(dòng)副方向的移動(dòng)距離，從而得到從動(dòng)長 度調(diào)節(jié)裝置實(shí)際桿長，將所述的實(shí)際轉(zhuǎn)角和實(shí)際桿長信息通過算法轉(zhuǎn)換為驅(qū)動(dòng)三個(gè)主動(dòng)長 度調(diào)節(jié)裝置的伺服電機(jī)的實(shí)際轉(zhuǎn)角，并將其與指令值比較，經(jīng)位置環(huán)控制器PID調(diào)節(jié)轉(zhuǎn)換為 速度指令，進(jìn)而實(shí)現(xiàn)位置環(huán)反饋控制;通過驅(qū)動(dòng)三個(gè)主動(dòng)長度調(diào)節(jié)裝置的伺服電機(jī)的電機(jī) 軸上安裝的絕對(duì)式角度編碼器，實(shí)時(shí)檢測(cè)三個(gè)伺服電機(jī)軸的實(shí)際轉(zhuǎn)角并分別作微分處理得 到三個(gè)伺服電機(jī)的實(shí)際電機(jī)轉(zhuǎn)速，分別與相應(yīng)的所述的速度指令進(jìn)行比較，進(jìn)而通過伺服 放大器PI調(diào)節(jié)實(shí)現(xiàn)三個(gè)伺服電機(jī)的速度環(huán)反饋控制； 所述的算法為： 式中：P一一主動(dòng)長度調(diào)節(jié)裝置中滾珠絲杠的導(dǎo)程 Q4一一通過直線光柵檢測(cè)的移動(dòng)距離加上從動(dòng)長度調(diào)節(jié)裝置初始位置桿長而得到的從 動(dòng)長度調(diào)節(jié)裝置桿長 Φ一一第一圓光柵檢測(cè)到的第一轉(zhuǎn)動(dòng)副的軸線的實(shí)際轉(zhuǎn)角 Θ一一第二圓光柵檢測(cè)到的第四轉(zhuǎn)動(dòng)副的軸線的實(shí)際轉(zhuǎn)角 ai一一由第一球鏈中心到從動(dòng)調(diào)節(jié)裝置軸線的理論垂距 a2一一由第二、三球鏈中心到從動(dòng)調(diào)節(jié)裝置軸線的理論垂距 bi一一連接第一主動(dòng)長度調(diào)節(jié)裝置與第二固定軸座的虎克鉸的近架轉(zhuǎn)軸軸線到轉(zhuǎn)動(dòng)支 架與機(jī)架相連轉(zhuǎn)軸軸線間的理論距離 b2一一連接第二主動(dòng)長度調(diào)節(jié)裝置與轉(zhuǎn)動(dòng)支架的第二轉(zhuǎn)動(dòng)副的轉(zhuǎn)軸軸線到連接從動(dòng)長 度調(diào)節(jié)裝置與轉(zhuǎn)動(dòng)支架的第四轉(zhuǎn)動(dòng)副的轉(zhuǎn)軸軸線間的理論距離或者連接第三主動(dòng)長度調(diào) 節(jié)裝置與轉(zhuǎn)動(dòng)支架的第三轉(zhuǎn)動(dòng)副的轉(zhuǎn)軸軸線到連接從動(dòng)長度調(diào)節(jié)裝置與轉(zhuǎn)動(dòng)支架的第四 轉(zhuǎn)動(dòng)副的轉(zhuǎn)軸軸線間的理論距離 0la、02a、03a-一由第一圓光柵、第二圓光柵和直線光柵檢測(cè)信息計(jì)算得到的驅(qū)動(dòng)三個(gè) 主動(dòng)長度調(diào)節(jié)裝置的伺服電機(jī)的實(shí)際轉(zhuǎn)角。
【文檔編號(hào)】B25J9/16GK105945954SQ201610465824
【公開日】2016年9月21日
【申請(qǐng)日】2016年6月21日
【發(fā)明人】黃田, 劉祺, 董成林, 肖聚亮, 劉海濤, 梅江平, 田文杰, 丁雅斌
【申請(qǐng)人】天津大學(xué)