專利名稱：帶容錯(cuò)和糾錯(cuò)功能的并聯(lián)機(jī)器人裝置及其容錯(cuò)和糾錯(cuò)方法
技術(shù)領(lǐng)域：
本發(fā)明屬于機(jī)器人制造技術(shù)領(lǐng)域，特別是一種帶容錯(cuò)和糾錯(cuò)功能的四自由度并聯(lián)機(jī)器人裝置及針對(duì)該項(xiàng)機(jī)器人傳感器輸入故障的容錯(cuò)和糾錯(cuò)方法。

背景技術(shù)：
要實(shí)現(xiàn)對(duì)并聯(lián)機(jī)器人位置、速度的精確控制，就必須通過高精度的傳感器對(duì)這些物理量進(jìn)行實(shí)時(shí)檢測(cè)，并將檢測(cè)結(jié)果轉(zhuǎn)換成數(shù)字量，反饋給控制器，通過控制器對(duì)這些數(shù)據(jù)進(jìn)行處理，處理的結(jié)果作為控制量對(duì)伺服電機(jī)進(jìn)行控制，進(jìn)而對(duì)機(jī)器人的運(yùn)動(dòng)等實(shí)現(xiàn)精確的閉環(huán)控制。所以檢測(cè)用傳感器加上反饋環(huán)節(jié)是伺服電機(jī)閉環(huán)控制系統(tǒng)的重要組成部分。在申請(qǐng)?zhí)枮?0105935.1的專利文獻(xiàn)中，公開了《一種四自由度并聯(lián)機(jī)器人機(jī)構(gòu)》，該并聯(lián)機(jī)器人的動(dòng)平臺(tái)通過四個(gè)分支(驅(qū)動(dòng)腿)與固定平臺(tái)連接，形成并聯(lián)閉環(huán)結(jié)構(gòu)，其中兩個(gè)分支各含有兩個(gè)單自由度運(yùn)動(dòng)副，一個(gè)球鉸鏈；另外兩個(gè)分支各含有一個(gè)單自由度運(yùn)動(dòng)副，兩個(gè)球鉸鏈或一個(gè)球鉸鏈和一個(gè)虎克鉸；此類并聯(lián)機(jī)器人的位置、速度(運(yùn)動(dòng))依靠設(shè)于各分支(條腿)上的含驅(qū)動(dòng)裝置(伺服電機(jī))、帶光電編碼器的傳感器在內(nèi)的傳動(dòng)系統(tǒng)及PID(比例積分微分)控制器相互配合來實(shí)現(xiàn)。PID(比例積分微分)控制器根據(jù)位置、速度傳感器的檢測(cè)信息形成對(duì)位置、速度等的閉環(huán)控制。該發(fā)明雖然具有運(yùn)動(dòng)正反解容易、高剛度、高精度、低運(yùn)動(dòng)質(zhì)量、高動(dòng)態(tài)性能、結(jié)構(gòu)簡(jiǎn)單等優(yōu)點(diǎn)；但由于PID控制器本身不具備容錯(cuò)、數(shù)據(jù)重構(gòu)等功能；因而存在缺乏在線自我修復(fù)的能力，當(dāng)機(jī)器人在運(yùn)動(dòng)過程中一旦傳感器發(fā)生故障、就將導(dǎo)致傳感器無輸出或輸出結(jié)果不正確，輕則造成位置、速度控制的精度降低，重則造成并聯(lián)機(jī)器人的失控、導(dǎo)致嚴(yán)重的安全事故等缺陷。


發(fā)明內(nèi)容
本發(fā)明的目的是針對(duì)背景技術(shù)存在的缺陷，研究設(shè)計(jì)一種帶容錯(cuò)和糾錯(cuò)功能的并聯(lián)機(jī)器人裝置及其容錯(cuò)和糾錯(cuò)方法，以便在驅(qū)動(dòng)腿傳感器出現(xiàn)突發(fā)性故障時(shí)，通過系統(tǒng)內(nèi)的切換和重構(gòu)實(shí)現(xiàn)容錯(cuò)及自動(dòng)糾錯(cuò)、進(jìn)行在線自維修，確保其正常工作和運(yùn)行，從而達(dá)到有效提高并聯(lián)機(jī)器人運(yùn)行的安全性和可靠性，有效避免因傳感器突發(fā)性故障造成重大損失等目的。
本發(fā)明的解決方案是在背景技術(shù)四自由度2RPS+2TPS型并聯(lián)機(jī)器人裝置(機(jī)構(gòu))的基礎(chǔ)上，在并聯(lián)機(jī)器人運(yùn)動(dòng)平臺(tái)中心與固定平臺(tái)的中心之間增設(shè)一條柔索，柔索的一端串接一位移傳感器及使柔索保持張緊狀態(tài)的彈簧，在工作過程中，將由四條驅(qū)動(dòng)腿的傳感器測(cè)量值與柔索傳感器的測(cè)量值送入故障檢測(cè)與隔離單元，用于對(duì)并聯(lián)機(jī)器人的運(yùn)行狀態(tài)進(jìn)行監(jiān)控；當(dāng)四條驅(qū)動(dòng)腿傳感器中有一傳感器出現(xiàn)了故障，判斷出故障傳感器后，則利用空間閉鏈機(jī)構(gòu)約束，由其余三條驅(qū)動(dòng)腿和柔索上傳感器的檢測(cè)值、通過現(xiàn)場(chǎng)可編程門陣列(FPGA)邏輯控制器中的數(shù)據(jù)重構(gòu)單元重新計(jì)算得到該故障傳感器的應(yīng)測(cè)值，將該應(yīng)測(cè)值替代故障傳感器輸出的錯(cuò)誤值，而將故障傳感器的原檢測(cè)(異常)值舍棄，從而實(shí)現(xiàn)對(duì)傳感器故障的容錯(cuò)和自動(dòng)糾錯(cuò)，確保機(jī)器人正常工作。因此，本發(fā)明并聯(lián)機(jī)器人裝置包括動(dòng)平臺(tái)、固定平臺(tái)(機(jī)架)及將兩平臺(tái)活動(dòng)連接以形成并聯(lián)閉環(huán)結(jié)構(gòu)的四條驅(qū)動(dòng)腿(分支)，其中兩個(gè)驅(qū)動(dòng)腿各含有兩個(gè)單自由度運(yùn)動(dòng)副、一個(gè)球鉸鏈，另外兩個(gè)驅(qū)動(dòng)腿各含有一個(gè)單自由度運(yùn)動(dòng)副和兩個(gè)球鉸鏈或一個(gè)球鉸鏈和一個(gè)虎克鉸，設(shè)于各驅(qū)動(dòng)腿上的含傳感器、伺服驅(qū)動(dòng)器、伺服電機(jī)在內(nèi)的傳動(dòng)系統(tǒng)，以及與各傳動(dòng)系統(tǒng)連接以控制機(jī)器人運(yùn)動(dòng)的控制系統(tǒng)，關(guān)鍵在于在動(dòng)平臺(tái)中心與固定平臺(tái)的中心之間還設(shè)有一條柔索，在與固定平臺(tái)連接點(diǎn)外側(cè)的該柔索上、還串接有用于監(jiān)測(cè)兩中心之間距離變化的位移傳感器和使柔索呈張緊狀態(tài)的彈簧，而控制系統(tǒng)則為含現(xiàn)場(chǎng)可編程門陣列(FPGA)邏輯控制器的控制系統(tǒng)；柔索的一端與動(dòng)平臺(tái)中心固定，另一端則與固定平臺(tái)的中心可相對(duì)滑動(dòng)式連接、并在連接點(diǎn)外側(cè)的柔索上串接位移傳感器和張緊彈簧后再緊固于固定平臺(tái)上，該位移傳感器還通過信號(hào)線與控制器連接、以傳送監(jiān)測(cè)信息。
以上所述含現(xiàn)場(chǎng)可編程門陣列(FPGA)邏輯控制器的控制系統(tǒng)，其控制系統(tǒng)包括閃存器(FLASH)，隨機(jī)存取存儲(chǔ)器(RAM)及與之連接的含中心控制單元、用于與PC機(jī)連接的通訊單元、用于插補(bǔ)計(jì)算的插補(bǔ)運(yùn)算單元、用作PID控制的PID控制單元、用于處理反饋信號(hào)的計(jì)數(shù)單元、用于將反饋參數(shù)與輸入閃存器中對(duì)應(yīng)的運(yùn)行參數(shù)進(jìn)行比較的故障檢測(cè)與隔離單元、將該故障檢測(cè)值進(jìn)行隔離處理的緩沖單元、以及用于計(jì)算故障傳感器應(yīng)測(cè)值的數(shù)據(jù)重構(gòu)單元在內(nèi)的現(xiàn)場(chǎng)可編程門陣列(FPGA)邏輯控制器。而所述柔索為在長(zhǎng)度方向的彈性變形量足夠小的金屬或非金屬類柔索；而與固定平臺(tái)中心的連接則通過環(huán)扣或環(huán)勾作可相對(duì)滑動(dòng)式連接。
而本發(fā)明并聯(lián)機(jī)器人裝置工作過程中的容錯(cuò)和糾錯(cuò)方法，包括 A.狀態(tài)監(jiān)測(cè)與故障診斷首先根據(jù)并聯(lián)機(jī)器人的運(yùn)動(dòng)要求，通過軌跡規(guī)劃，將其各條驅(qū)動(dòng)腿長(zhǎng)度、速度隨時(shí)間變化的運(yùn)行參數(shù)輸入控制系統(tǒng)的閃存器(FLASH)內(nèi)，作為驅(qū)動(dòng)腿長(zhǎng)度、速度的基礎(chǔ)參數(shù)，該參數(shù)值同時(shí)經(jīng)邏輯控制器中的插補(bǔ)運(yùn)算單元處理后將結(jié)果送入PID控制單元用于對(duì)各驅(qū)動(dòng)腿內(nèi)的伺服電機(jī)進(jìn)行控制；四條驅(qū)動(dòng)腿傳感器及柔索傳感器的在線檢測(cè)值一路經(jīng)緩沖單元輸入PID控制單元對(duì)伺服電機(jī)進(jìn)行PID控制，另一路在中心控制單元的控制下作為檢測(cè)值輸入隨機(jī)存取存儲(chǔ)器(RAM)、以對(duì)并聯(lián)機(jī)器人的運(yùn)行狀況進(jìn)行在線控制和監(jiān)測(cè)；由四條驅(qū)動(dòng)腿傳感器檢測(cè)值根據(jù)空間并聯(lián)機(jī)構(gòu)的約束關(guān)系可以計(jì)算出中間柔索傳感器的計(jì)算值，并將此柔索傳感器的計(jì)算值與柔索傳感器的實(shí)際檢測(cè)值比較，如果兩者之間的偏差值超出了設(shè)定范圍，則表明有一條驅(qū)動(dòng)腿的傳感器出現(xiàn)了故障； B.故障定位故障檢測(cè)與隔離系統(tǒng)將驅(qū)動(dòng)腿傳感器此時(shí)的基礎(chǔ)參數(shù)與驅(qū)動(dòng)腿傳感器的實(shí)際輸出值進(jìn)行對(duì)比，如果某一驅(qū)動(dòng)腿兩者之間的偏差值超出了設(shè)定范圍，則判定該驅(qū)動(dòng)腿傳感器為故障傳感器； C.故障傳感器的容錯(cuò)和糾錯(cuò)當(dāng)由B步驟確定出故障傳感器后，即由另外三條驅(qū)動(dòng)腿傳感器的檢測(cè)值和中間柔索傳感器的檢測(cè)值通過FPGA運(yùn)動(dòng)控制器中的數(shù)據(jù)重構(gòu)單元，計(jì)算出故障傳感器的應(yīng)測(cè)值，并在檢測(cè)與隔離單元的控制下通過緩沖器將故障檢測(cè)值丟棄，以該應(yīng)測(cè)值替換，用于對(duì)故障傳感器所對(duì)應(yīng)的伺服電機(jī)進(jìn)行控制，從而實(shí)現(xiàn)對(duì)故障傳感器的容錯(cuò)和糾錯(cuò)。
所述故障傳感器包括驅(qū)動(dòng)腿傳感器本身故障或其電路傳輸故障、或傳感器探頭因物理障礙所致異常輸出。
本發(fā)明帶容錯(cuò)和糾錯(cuò)功能的并聯(lián)機(jī)器人裝置由于在傳統(tǒng)四自由度2RPS+2TPS型并聯(lián)機(jī)器人裝置的運(yùn)動(dòng)平臺(tái)中心與固定平臺(tái)的中心之間連接一條柔索，柔索一端串接一位移傳感器及使柔索保持張緊狀態(tài)的彈簧，柔索對(duì)于并聯(lián)機(jī)器人的運(yùn)動(dòng)沒有干涉，其上連接的傳感器所提供的冗余信息在并聯(lián)機(jī)器人正常工作時(shí)被用于故障檢測(cè)與隔離；其容錯(cuò)和糾錯(cuò)方法則是在控制系統(tǒng)中采用具有并行計(jì)算能力和可動(dòng)態(tài)重構(gòu)特點(diǎn)的現(xiàn)場(chǎng)可編程門陣列邏輯(FPGA)控制器，對(duì)并聯(lián)機(jī)器人運(yùn)行進(jìn)行動(dòng)態(tài)監(jiān)測(cè)；當(dāng)并聯(lián)機(jī)器人驅(qū)動(dòng)腿傳感器出現(xiàn)故障時(shí)，則利用其余正常工作傳感器的測(cè)量值、通過其內(nèi)部邏輯資源的重新配置自動(dòng)進(jìn)行在線數(shù)據(jù)重構(gòu)，以確保系統(tǒng)正常運(yùn)行和修復(fù)。因而，本發(fā)明機(jī)器人裝置在工作過程中當(dāng)驅(qū)動(dòng)腿傳感器發(fā)生突發(fā)性故障時(shí)，可進(jìn)行在線自維修，對(duì)傳感器故障容錯(cuò)并自動(dòng)糾錯(cuò)，且重構(gòu)時(shí)間短，速度快，系統(tǒng)的重構(gòu)效率高，確保機(jī)器人在驅(qū)動(dòng)腿的傳感器發(fā)生故障時(shí)正常工作并及時(shí)修復(fù)，從而有效提高了并聯(lián)機(jī)器人的工作效率及工作和運(yùn)行的安全性、可靠性，可有效避免因傳感器突發(fā)性故障造成重大損失等特點(diǎn)?？朔吮尘凹夹g(shù)存在的如果傳感器一旦發(fā)生故障，將導(dǎo)致傳感器無輸出或輸出結(jié)果不正確，輕則造成位置、速度控制的精度降低，重則造成并聯(lián)機(jī)器人的失控、甚至導(dǎo)致嚴(yán)重事故的發(fā)生等弊病。



圖1.為本發(fā)明并聯(lián)機(jī)器人裝置結(jié)構(gòu)示意圖； 圖2.為本發(fā)明并聯(lián)機(jī)器人裝置控制系統(tǒng)及與PC機(jī)和各驅(qū)動(dòng)腿連接關(guān)系示意圖； 圖3.為本發(fā)明實(shí)施方式與圖1中A1B1B3A3環(huán)路的運(yùn)動(dòng)等效的平面并聯(lián)機(jī)構(gòu)示意圖。
圖中1.動(dòng)平臺(tái)，2.固定平臺(tái)、2-1.環(huán)扣，3.驅(qū)動(dòng)腿，3-1(l1)、3-2(l2)、3-3(l3)、3-4(l4)(分別為第1、2、3、4驅(qū)動(dòng)腿)，4(l5).柔索、4-1.位移傳感器、4-2.張緊彈簧，5(O1P).指針，6.閃存器(FLASH)，7.隨機(jī)存取存儲(chǔ)器(RAM)，8.現(xiàn)場(chǎng)可編程門陣列(FPGA)邏輯控制器，9.PC機(jī)，I.數(shù)模轉(zhuǎn)換器、II.伺服驅(qū)動(dòng)器、III.伺服電機(jī)、IV.驅(qū)動(dòng)腿傳感器；A1、A3轉(zhuǎn)動(dòng)副，A2、A4虎克鉸或球鉸，B1、B2、B3、B4球鉸。

具體實(shí)施例方式 本實(shí)施方式的控制系統(tǒng)中FPGA邏輯控制器8采用型號(hào)為SPARTAN-3的FPGA邏輯器件并通過內(nèi)部邏輯資源的重新配置，組成包含中心控制單元、通訊單元、插補(bǔ)運(yùn)算單元、PID控制單元、數(shù)據(jù)重構(gòu)單元、故障檢測(cè)與隔離單元、計(jì)數(shù)單元、緩沖單元在內(nèi)的控制器；閃儲(chǔ)器(FLASH)6型號(hào)為Am29LV160D，隨機(jī)存取存儲(chǔ)器(RAM)7采用型號(hào)為HY57V641620HG的同步動(dòng)態(tài)隨機(jī)存取存儲(chǔ)器，數(shù)模轉(zhuǎn)換器I型號(hào)為AD7564，伺服電機(jī)III型號(hào)為MSMA042A1G、與之配套的伺服驅(qū)動(dòng)器II型號(hào)為MSDA043A1A，驅(qū)動(dòng)腿傳感器IV采用型號(hào)為CN65M121759的光電編碼器。
本實(shí)施方式對(duì)傳感器故障的容錯(cuò)和自動(dòng)糾錯(cuò)方法，以在并聯(lián)機(jī)器人仿真運(yùn)行為例并聯(lián)機(jī)器人結(jié)構(gòu)參數(shù)如下固定平臺(tái)2和動(dòng)平臺(tái)1都為正方形布局，其外接圓的半徑分別為R＝175mm，r＝100mm，動(dòng)平臺(tái)1中心O1點(diǎn)到固定平臺(tái)(A1、A2、A3、A4)的距離h＝350mm，在動(dòng)平臺(tái)1的中心固定有一指針5(O1P)，指針垂直于動(dòng)平臺(tái)1，指針5(O1P)長(zhǎng)為100mm；柔索4(l5)選用的是鋼絲繩，鋼絲繩通過圓環(huán)2-1改變了走向，鋼絲繩另一端連接型號(hào)為CN63M/GXC的光柵尺傳感器及彈簧4-2，彈簧4-2使柔索4(l5)始終處于張緊狀態(tài)；規(guī)劃指針5(O1P)繞固定平臺(tái)2的Z軸做勻速圓周運(yùn)動(dòng)，O1P與Z軸的夾角θ＝45°，運(yùn)動(dòng)周期為360秒。
其方法如下 A.首先根據(jù)上述運(yùn)動(dòng)要求，由PC機(jī)9通過軌跡規(guī)劃將各條驅(qū)動(dòng)腿長(zhǎng)度隨時(shí)間變化的運(yùn)行參數(shù)輸入控制系統(tǒng)的閃存器6，作為驅(qū)動(dòng)腿長(zhǎng)度的基礎(chǔ)參數(shù)同時(shí)交由插補(bǔ)運(yùn)算單元處理后送入PID控制單元，其輸出控制信號(hào)經(jīng)數(shù)摸轉(zhuǎn)換器I轉(zhuǎn)換后通過伺服驅(qū)動(dòng)器II控制伺服電機(jī)III的運(yùn)行；由各傳感器IV檢測(cè)所得的反饋值，輸入計(jì)數(shù)單元處理后，送入PID控制單元用于對(duì)伺服電機(jī)III的反饋控制，同時(shí)，該反饋數(shù)據(jù)被存入隨機(jī)存取存儲(chǔ)器7中； B.在t＝115秒時(shí)，在傳感器IV引入突發(fā)性誤差(如傳感器的輸出值突然消失)，故障檢測(cè)與隔離單元由四條驅(qū)動(dòng)腿長(zhǎng)度的檢測(cè)值、并根據(jù)空間并聯(lián)機(jī)構(gòu)的約束關(guān)系計(jì)算出柔索4(l5)位于固定平臺(tái)2和動(dòng)平臺(tái)1中心之間長(zhǎng)度的計(jì)算值，將該段柔索長(zhǎng)度的計(jì)算值與該段柔索長(zhǎng)度的實(shí)際檢測(cè)值比較，如果偏差值超出了設(shè)定范圍，則表明有一條驅(qū)動(dòng)腿的傳感器出現(xiàn)了故障；故障檢測(cè)與隔離系統(tǒng)將此時(shí)刻閃存器6中的驅(qū)動(dòng)腿長(zhǎng)度的基礎(chǔ)參數(shù)與隨機(jī)存取存儲(chǔ)器7中的反饋的驅(qū)動(dòng)腿傳感器的實(shí)際輸出值進(jìn)行對(duì)比，此時(shí)由于驅(qū)動(dòng)腿3-4(l4)兩者之間的偏差超出了設(shè)定范圍，故判定驅(qū)動(dòng)腿3-4(l4)傳感器為故障傳感器； C.在由B步驟檢測(cè)并定位出驅(qū)動(dòng)腿3-4(l4)傳感器出現(xiàn)故障時(shí)，即通過故障檢測(cè)與隔離單元向緩存單元發(fā)出隔離指令，同時(shí)，中心控制單元啟動(dòng)數(shù)據(jù)重構(gòu)單元進(jìn)行數(shù)據(jù)重構(gòu)處理，本實(shí)施方式數(shù)據(jù)重構(gòu)單元按下述方法自動(dòng)進(jìn)行數(shù)據(jù)重構(gòu) 建立坐標(biāo)系如圖1所示，坐標(biāo)系設(shè)定如下基礎(chǔ)坐標(biāo)系R的坐標(biāo)原點(diǎn)位于固定平臺(tái)的中心O點(diǎn)，X軸選取為OA3方向，Z軸選取為垂直于平面A1A2A3A4的方向；動(dòng)坐標(biāo)系R’的坐標(biāo)原點(diǎn)位于動(dòng)平臺(tái)的中心O1點(diǎn)，X’軸選取為O1B3方向，Z’軸選取為垂直于平面B1B2B3B4的方向。
圖1所示并聯(lián)機(jī)器人中A1B1B3A3環(huán)路的運(yùn)動(dòng)與圖3所示的平面并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)等效，動(dòng)平臺(tái)上鉸接點(diǎn)B1、B3在基礎(chǔ)坐標(biāo)系R中的坐標(biāo)可表示為 B1＝(l5cosθ1-rcosθ2，0，l5sinθ1-rsinθ2)T(1) B3＝(l5cosθ1+rcosθ2，0，l5sinθ1+rsinθ2)T 固定平臺(tái)上鉸接點(diǎn)A1、A3在基礎(chǔ)坐標(biāo)系R中的坐標(biāo)可表示為 A1＝(-R，0，0)T，A3＝(R，0，0)T(2) 驅(qū)動(dòng)腿的長(zhǎng)度li可以用鉸鏈點(diǎn)Ai、Bi的坐標(biāo)表示出來，即 (4a)+(4b)，整理可得 由上式可求解出θ2的值，值得注意的是θ2有兩個(gè)解。將θ2的值代入式(4a)，可得 由上式可求解出θ1的值，θ1有兩個(gè)解。
動(dòng)平臺(tái)與其相固連的坐標(biāo)系R’相對(duì)于基礎(chǔ)坐標(biāo)系R的姿態(tài)可通過如下的兩次旋轉(zhuǎn)變操得到(1)繞Y軸旋轉(zhuǎn)角度θ2，(2)繞X’軸(由旋轉(zhuǎn)變換1得到)旋轉(zhuǎn)角度θ3。于是旋轉(zhuǎn)變換矩陣R可表示為 動(dòng)平臺(tái)上鉸接點(diǎn)B2在基礎(chǔ)坐標(biāo)系R中的坐標(biāo)可表示為 驅(qū)動(dòng)腿的長(zhǎng)度l2可以用鉸鏈點(diǎn)A2、B2的坐標(biāo)表示出來，這就是機(jī)構(gòu)的約束方程 (8) 上式整理后可得 由上式可求解出θ3的值，θ3有兩個(gè)解。
動(dòng)平臺(tái)上鉸接點(diǎn)B4在基礎(chǔ)坐標(biāo)系R中的坐標(biāo)表示為 而A4＝(0，R，0)T 驅(qū)動(dòng)腿的長(zhǎng)度l4可以用鉸鏈點(diǎn)A4、B4的坐標(biāo)表示出來 l4即為故障傳感器當(dāng)前的應(yīng)測(cè)值，數(shù)據(jù)重構(gòu)單元將該應(yīng)測(cè)值送入PID控制單元以代替故障傳感器的檢測(cè)值對(duì)驅(qū)動(dòng)腿四的伺服電機(jī)進(jìn)行PID控制，從而實(shí)現(xiàn)對(duì)驅(qū)動(dòng)腿四傳感器故障的容錯(cuò)和自動(dòng)糾錯(cuò)，確保了并聯(lián)機(jī)器人的正常運(yùn)行。
權(quán)利要求
1.一種帶容錯(cuò)和糾錯(cuò)功能的并聯(lián)機(jī)器人裝置，包括動(dòng)平臺(tái)、固定平臺(tái)及將兩平臺(tái)活動(dòng)連接以形成并聯(lián)閉環(huán)結(jié)構(gòu)的四條驅(qū)動(dòng)腿，其中兩個(gè)驅(qū)動(dòng)腿各含有兩個(gè)單自由度運(yùn)動(dòng)副、一個(gè)球鉸鏈，另外兩個(gè)驅(qū)動(dòng)腿各含有一個(gè)單自由度運(yùn)動(dòng)副和兩個(gè)球鉸鏈或一個(gè)球鉸鏈和一個(gè)虎克鉸，設(shè)于各驅(qū)動(dòng)腿上的含傳感器、伺服驅(qū)動(dòng)器、伺服電機(jī)在內(nèi)的傳動(dòng)系統(tǒng)，以及與各傳動(dòng)系統(tǒng)連接以控制機(jī)器人運(yùn)動(dòng)的控制系統(tǒng)，關(guān)鍵在于在動(dòng)平臺(tái)中心與固定平臺(tái)的中心之間還設(shè)有一條柔索，在與固定平臺(tái)連接點(diǎn)外側(cè)的該柔索上、還串接有用于監(jiān)測(cè)兩中心之間距離變化的位移傳感器和使柔索呈張緊狀態(tài)的彈簧，而控制系統(tǒng)則為含現(xiàn)場(chǎng)可編程門陣列邏輯控制器的控制系統(tǒng)；柔索的一端與動(dòng)平臺(tái)中心固定，另一端則與固定平臺(tái)的中心可相對(duì)滑動(dòng)式連接、并在連接點(diǎn)外側(cè)的柔索上串接位移傳感器和張緊彈簧后再緊固于固定平臺(tái)上，該位移傳感器還通過信號(hào)線與控制器連接、以傳送監(jiān)測(cè)信息。
2.按權(quán)利要求1所述帶容錯(cuò)和糾錯(cuò)功能的并聯(lián)機(jī)器人裝置，其特征在于所述含現(xiàn)場(chǎng)可編程門陣列邏輯控制器的控制系統(tǒng)，其控制系統(tǒng)包括閃存器，隨機(jī)存取存儲(chǔ)器及與之連接的含中心控制單元、用于與PC機(jī)連接的通訊單元、用于插補(bǔ)計(jì)算的插補(bǔ)運(yùn)算單元、用作PID控制的PID控制單元、用于處理反饋信號(hào)的計(jì)數(shù)單元、用于將反饋參數(shù)與輸入閃存器中對(duì)應(yīng)的運(yùn)行參數(shù)進(jìn)行比較的故障檢測(cè)與隔離單元、將該故障檢測(cè)值進(jìn)行隔離處理的緩沖單元、以及用于計(jì)算故障傳感器應(yīng)測(cè)值的數(shù)據(jù)重構(gòu)單元在內(nèi)的現(xiàn)場(chǎng)可編程門陣列邏輯控制器。
3.按權(quán)利要求1所述帶容錯(cuò)和糾錯(cuò)功能的并聯(lián)機(jī)器人裝置，其特征在于所述柔索為在長(zhǎng)度方向的彈性變形量足夠小的金屬或非金屬類柔索；而與固定平臺(tái)中心的連接則通過環(huán)扣或環(huán)勾作可相對(duì)滑動(dòng)式連接。
4.按權(quán)利要求1所述并聯(lián)機(jī)器人裝置的容錯(cuò)和糾錯(cuò)方法，包括
A.狀態(tài)監(jiān)測(cè)與故障診斷首先根據(jù)并聯(lián)機(jī)器人的運(yùn)動(dòng)要求，通過軌跡規(guī)劃，將其各條驅(qū)動(dòng)腿長(zhǎng)度、速度隨時(shí)間變化的運(yùn)行參數(shù)輸入控制系統(tǒng)的閃存器(FLASH)內(nèi)，作為驅(qū)動(dòng)腿長(zhǎng)度、速度的基礎(chǔ)參數(shù)，該參數(shù)值同時(shí)經(jīng)邏輯控制器中的插補(bǔ)運(yùn)算單元處理后將結(jié)果送入PID控制單元用于對(duì)各驅(qū)動(dòng)腿內(nèi)的伺服電機(jī)進(jìn)行控制；四條驅(qū)動(dòng)腿傳感器及柔索傳感器的在線檢測(cè)值一路經(jīng)緩沖單元輸入PID控制單元對(duì)伺服電機(jī)進(jìn)行PID控制，另一路在中心控制單元的控制下作為檢測(cè)值輸入隨機(jī)存取存儲(chǔ)器(RAM)、以對(duì)并聯(lián)機(jī)器人的運(yùn)行狀況進(jìn)行在線控制和監(jiān)測(cè)；由四條驅(qū)動(dòng)腿傳感器檢測(cè)值根據(jù)空間并聯(lián)機(jī)構(gòu)的約束關(guān)系可以計(jì)算出中間柔索傳感器的計(jì)算值，并將此柔索傳感器的計(jì)算值與柔索傳感器的實(shí)際檢測(cè)值比較，如果兩者之間的偏差值超出了設(shè)定范圍，則表明有一條驅(qū)動(dòng)腿的傳感器出現(xiàn)了故障；
B.故障定位故障檢測(cè)與隔離系統(tǒng)將驅(qū)動(dòng)腿傳感器此時(shí)的基礎(chǔ)參數(shù)與驅(qū)動(dòng)腿傳感器的實(shí)際輸出值進(jìn)行對(duì)比，如果某一驅(qū)動(dòng)腿兩者之間的偏差值超出了設(shè)定范圍，則判定該驅(qū)動(dòng)腿傳感器為故障傳感器；
C.故障傳感器的容錯(cuò)和糾錯(cuò)當(dāng)由B步驟確定出故障傳感器后，即由另外三條驅(qū)動(dòng)腿傳感器的檢測(cè)值和中間柔索傳感器的檢測(cè)值通過FPGA運(yùn)動(dòng)控制器中的數(shù)據(jù)重構(gòu)單元，計(jì)算出故障傳感器的應(yīng)測(cè)值，并在檢測(cè)與隔離單元的控制下通過緩沖器將故障檢測(cè)值丟棄，以該應(yīng)測(cè)值替換，用于對(duì)故障傳感器所對(duì)應(yīng)的伺服電機(jī)進(jìn)行控制，從而實(shí)現(xiàn)對(duì)故障傳感器的容錯(cuò)和糾錯(cuò)。
5.按權(quán)利要求4所述并聯(lián)機(jī)器人裝置的容錯(cuò)和糾錯(cuò)方法，其特征在于所述故障傳感器包括驅(qū)動(dòng)腿傳感器本身故障或其電路傳輸故障、或傳感器探頭因物理障礙所致異常輸出。
全文摘要
該發(fā)明屬于一種帶容錯(cuò)和糾錯(cuò)功能的并聯(lián)機(jī)器人裝置及容錯(cuò)和糾錯(cuò)方法。裝置包括動(dòng)平臺(tái)、固定平臺(tái)及其驅(qū)動(dòng)腿，串接有位移傳感器和張緊彈簧的柔索，傳動(dòng)系統(tǒng)，含F(xiàn)PGA的控制系統(tǒng)；而容錯(cuò)和糾錯(cuò)方法包括狀態(tài)監(jiān)測(cè)與故障診斷、故障定位及故障傳感器的容錯(cuò)和糾錯(cuò)。該發(fā)明由于在動(dòng)平臺(tái)與固定平臺(tái)中心之間增設(shè)了帶傳感器和張緊彈簧的柔索，并在控制系統(tǒng)中采用FPGA控制器；因而具有對(duì)驅(qū)動(dòng)腿傳感器故障可進(jìn)行在線重構(gòu)和自維修，且重構(gòu)時(shí)間短，速度快，系統(tǒng)的重構(gòu)和自維修效率高，可確保機(jī)器人在驅(qū)動(dòng)腿傳感器發(fā)生故障時(shí)能正常工作，從而有效提高了并聯(lián)機(jī)器人的工作效率及工作和運(yùn)行的安全性、可靠性，可避免因驅(qū)動(dòng)腿傳感器突發(fā)性故障造成重大損失等特點(diǎn)。
文檔編號(hào)G05B19/414GK101745916SQ20091026343
公開日2010年6月23日 申請(qǐng)日期2009年12月16日 優(yōu)先權(quán)日2009年12月16日
發(fā)明者范守文, 熊靜琪, 邵旭光, 黃洪鐘, 胡天友 申請(qǐng)人:電子科技大學(xué)