專利名稱：：遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)及方法
技術(shù)領(lǐng)域：
：本發(fā)明涉及的是一種控制
技術(shù)領(lǐng)域：
的控制器及控制方法，具體是一種適應(yīng)多障礙約束空間作業(yè)的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)及方法。技術(shù)背景在核工業(yè)、反恐排爆、救援、高電壓設(shè)備清掃、維護等危險、惡劣環(huán)境中的檢查、維修、搬運、抓取、裝配等特種作業(yè)廣泛采用遙操作機械臂，目前的機械臂自由度大多小于或等于6，適用于無障礙環(huán)境；而對于某些特殊的多障礙約束環(huán)境作業(yè)，如排爆作業(yè)中從車底抓取爆炸物，救援作業(yè)中從窗戶進入靠近地面的被困人員，高壓帶電清掃變電站中的絕緣子等作業(yè)，由于工作環(huán)境中存在較多障礙，需要機械臂提供冗余自由度。冗余自由度主要用來擴大機械臂工作范圍、優(yōu)化機械臂運動學(xué)、動力學(xué)性能以及回避障礙。平面冗余度機械臂是指在一個平面內(nèi)機械臂自由度大于3的機械臂，是冗余度機械臂的一個重要組成部分。面向平面冗余度機械手的主端指令輸入和控制裝置主要包括按鈕、操縱桿、同構(gòu)主手以及和手臂相捆綁的隨動主手等，其中同構(gòu)主手需要和從手完全相同的機械構(gòu)型，結(jié)構(gòu)復(fù)雜，通常需要位置傳感器獲取主手各關(guān)節(jié)位置信息，成本較高；采用和手臂隨動的主手則同樣存在尺寸較大、結(jié)構(gòu)復(fù)雜、造價昂貴的缺點。由于冗余度機械臂除了可以滿足操作者對末端位置和姿態(tài)的控制要求，還可以進一步調(diào)整機械手各關(guān)節(jié)臂的相對位置關(guān)系即構(gòu)型以適應(yīng)多障礙環(huán)境，而目前以按鈕和操縱桿為主的指令輸入和控制器尚沒有相應(yīng)的指令輸入和處理方法。經(jīng)對現(xiàn)有技術(shù)的文獻檢索發(fā)現(xiàn)，中國專利ZL200510042571.2研制了一種多關(guān)節(jié)操作桿，包括基座、尾擺動桿、變向擺動桿、同項擺動桿、頭擺動桿、回轉(zhuǎn)桿、按鈕依次首尾連接而成，各個擺動和回轉(zhuǎn)關(guān)節(jié)的中心位置安裝中空薄膜電位器和空心軸，按鈕開關(guān)則裝有薄膜開關(guān)，用于控制機械臂基座回轉(zhuǎn)、擺動關(guān)節(jié)l一3、手腕回轉(zhuǎn)和開關(guān)關(guān)節(jié)運動。此操作桿線纜內(nèi)藏，可根據(jù)從手形式組合，結(jié)構(gòu)緊湊；專利200610082456.2則提出一種主從機械手用于外科手術(shù)，其中主手采用與從手同構(gòu)的連桿串聯(lián)連接而成，各個連桿驅(qū)動時采用雙向離合器用來防止操作者向錯誤方向操作機械臂。上述機械臂指令輸入和控制器，在操作靈活性、安全性等方面有一定提高，但仍存在如下的缺點(1)控制的對象是非冗余度機械手。(2)控制器僅僅考慮末端的運動，沒有專門針對機械臂構(gòu)型實施控制。而機械臂構(gòu)型調(diào)整則體現(xiàn)了對機械臂對環(huán)境的適應(yīng)能力；(3)結(jié)構(gòu)復(fù)雜，造價高。又發(fā)現(xiàn)，中國專利ZL200510042571.2結(jié)構(gòu)緊湊，但是沒有考慮機械臂構(gòu)型的調(diào)整方法，實質(zhì)上是一種集成的單軸運動控制器；專利200610082456.2是一種同構(gòu)主手，該方案可以用于冗余度機械臂構(gòu)型控制，但需要設(shè)計和從手同構(gòu)的主手機械結(jié)構(gòu)，并配備了位姿傳感器，結(jié)構(gòu)復(fù)雜，造價高，僅僅適合于重要的應(yīng)用場合。目前大多數(shù)機械臂的指令輸入和控制器仍然應(yīng)用通用操作桿(最大控制軸數(shù)為3)和按鈕組合的方式輸入指令，如何在此基礎(chǔ)上實現(xiàn)冗余度機械臂末端運動和構(gòu)型調(diào)整尚未見報道。
發(fā)明內(nèi)容本發(fā)明針對現(xiàn)有技術(shù)的不足，提供一種冗余自由度小于或等于3的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)及方法，使得既可以直接控制機械臂的末端位姿，又可以障礙環(huán)境中調(diào)整機械臂的構(gòu)型，從而可實現(xiàn)在障礙環(huán)境中快速、高效接近目標(biāo)，簡單實用，提高了平面冗余度機械臂的操作性能。本發(fā)明是通過以下技術(shù)方案實現(xiàn)的本發(fā)明所涉及的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)，包括冗余度機械臂控制臺、冗余度機械臂上位控制器、上位機無線數(shù)據(jù)模塊、下位機無線數(shù)據(jù)模塊、冗余度機械臂下位機控制器以及冗余度機械臂組成，其中所述冗余度機械臂控制臺提供人機交互，負責(zé)接收操作員的指令并顯示機械臂當(dāng)前位置、限位、報警等狀態(tài)信息；所述冗余度機械臂上位控制器安裝在冗余度機械臂控制臺上，和冗余度機械臂上位控制器之間通過控制電路連接，冗余度機械臂上位控制器通過串行通訊接口與上位機無線數(shù)據(jù)模塊連接與通訊，將編碼的控制命令發(fā)送到遠端的下位機無線數(shù)據(jù)模塊，并同時獲取遠端機械臂的狀態(tài)信息。所述上位機和下位機無線數(shù)據(jù)模塊提供冗余度機械臂上位控制器和冗余度機械臂下位控制器之間的通訊功能，由工作在同一工作頻率的一對模塊完成，其間的距離為100米。上位機和下位機無線數(shù)據(jù)模塊和冗余度機械臂上、下位控制器之間均通過串行通訊接口交換數(shù)據(jù)；所述冗余度機械臂下位控制器安裝在冗余度機械臂上，接收冗余度機械臂上位控制器對冗余度機械臂各關(guān)節(jié)軸的運動指令，并輸出到冗余度機械臂的各關(guān)節(jié)電機上，同時將冗余度機械臂的位置、速度、限位和報警等狀態(tài)信息發(fā)送給下位機無線數(shù)據(jù)模塊。所述的冗余度機械臂控制臺上設(shè)有模式選擇開關(guān)、單軸指令輸入開關(guān)組、構(gòu)型指令輸入開關(guān)組、單軸速度輸入旋鈕、速度指令輸入操縱桿、指示燈和液晶顯示模塊。通過模式選擇開關(guān)可選擇機械臂的三種控制模式單軸運動模式，末端運動模式和構(gòu)型調(diào)整模式，在任一時刻只能選擇其中一種控制模式。在單軸運動模式下，單軸指令輸入開關(guān)組和單軸速度輸入旋鈕的輸入有效。單軸指令輸入開關(guān)組控制機械臂每個電機的正反轉(zhuǎn)，單軸速度輸入旋鈕表示軸電機的運動速度，單軸運動指令由單軸指令輸入開關(guān)組和單軸速度輸入旋鈕組合實現(xiàn)。在末端運動模式下，速度指令輸入操縱桿和構(gòu)型指令輸入開關(guān)組有效。速度指令輸入操縱桿直接控制末端機械臂在平面內(nèi)的運動速度，操作直觀性強，所述的操縱桿為三維操縱桿，內(nèi)部包含三個獨立的電位計，可輸入三個軸的速度指令；構(gòu)型指令輸入開關(guān)組包括l一3個冗余軸選擇波段開關(guān)，對應(yīng)于平面冗余度為1到3的情況。在此模式下構(gòu)型指令輸入開關(guān)所選擇的軸表示該軸電機鎖定。在構(gòu)型調(diào)整模式下，速度指令輸入操縱桿和構(gòu)型指令輸入開關(guān)組有效。構(gòu)型調(diào)整模式指的是機械臂末端位置和姿態(tài)均保持不變的情況下，通過輸入冗余關(guān)節(jié)的運動，調(diào)整機械臂各關(guān)節(jié)之間的相對位置關(guān)系。該功能是這樣實現(xiàn)的由構(gòu)型指令輸入開關(guān)組選擇要調(diào)整的關(guān)節(jié)軸，此時速度指令輸入操縱桿三個電位計的輸入表示由構(gòu)型指令輸入開關(guān)組設(shè)定的關(guān)節(jié)軸的運動速度。所述的指示燈，主要功能為顯示各關(guān)節(jié)軸的限位信息以及電機運動過程的報警信息。所述的液晶顯示模塊，主要功能為顯示關(guān)節(jié)軸電機的位置及報警的詳細信息。所述的冗余度機械臂上位控制器，主要由數(shù)字量輸入接口、濾波電路、模擬量輸入接口、數(shù)字量輸出擴展模塊、數(shù)字量輸出接口、主控芯片、數(shù)據(jù)存儲模塊和串行通訊接口組成。其中，主控芯片通過通用1/0端口與數(shù)字量輸入接口、數(shù)字量輸出擴展模塊連接，其中數(shù)字量輸入接口用于接入模式選擇開關(guān)、單軸指令輸入開關(guān)組以及構(gòu)型指令輸入開關(guān)組的開、合指令輸入；數(shù)字量輸出擴展模塊用于擴展輸出端口，經(jīng)數(shù)字量輸出接口完成光電隔離、電平轉(zhuǎn)換后與液晶顯示模塊以及指示燈連接；主控芯片通過A/D端口和模擬量輸入接口連接，經(jīng)濾波電路后采集冗余度機械臂控制臺的單軸速度控制旋鈕和速度指令輸入操縱桿輸入的電壓量；主控芯片通過12(:總線與靜態(tài)數(shù)據(jù)存儲模塊連接。數(shù)據(jù)存儲模塊用于存儲上位機命令解析程序、機械臂系統(tǒng)的工作參數(shù)、遠程通訊地址和通訊協(xié)議，其存儲的數(shù)據(jù)掉電不丟失。所述的冗余度機械臂下位控制器，與機械臂固定安裝，包括串行通訊接口，數(shù)字信號處理器、高速脈沖輸入接口、數(shù)字量輸入接口、數(shù)字量輸出接口以及模擬量輸出接口。其中，數(shù)字信號處理器通過數(shù)據(jù)總線和高速脈沖輸入接口電路連接，用于接收電機的編碼器信號，反饋電機的位置、速度信息；數(shù)字信號處理器通過GPIO(通用輸入輸出)端口和數(shù)字量輸入接口以及數(shù)字量輸出接口連接，數(shù)字量輸入接口用于接入原點、限位以及驅(qū)動器狀態(tài)；數(shù)字量輸出接口用于接入驅(qū)動器控制信號；數(shù)字信號處理器通過數(shù)據(jù)端口和模擬量輸出接口連接，用于向驅(qū)動器提供速度指令。數(shù)字信號處理器通過串行通訊接口與無線數(shù)據(jù)模塊通訊，接收上位控制器的控制指令，并反饋電機的位置、速度以及報警等狀態(tài)信息。所述的冗余度機械臂由安裝在其關(guān)節(jié)處的電機進行驅(qū)動，電機附帶編碼器和驅(qū)動器。編碼器通過高速脈沖輸入接口和數(shù)字信號處理器連接，提供任意時刻電機的位置和速度信息；驅(qū)動器通過模擬量輸入接口和數(shù)字信號處理器連接，接收電機運動的速度指令；同時通過數(shù)字量輸入接口接收數(shù)字信號處理器的伺服使能等開關(guān)指令，并反饋電機的狀態(tài)信息。本發(fā)明所涉及的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制方法，包括如下的步驟2001:讀取無線數(shù)據(jù)模塊發(fā)送的機械臂當(dāng)前位置和狀態(tài)。通過模式選擇按鈕，操作人員可選擇單軸運動模式，末端運動模式和構(gòu)型調(diào)整模式中的一種；2002:如果選擇單軸運動模式，則讀取單軸指令輸入開關(guān)組和單軸速度控制旋鈕的值，表示電機的運動方向和速度指令輸入；從而確定機械臂全部關(guān)節(jié)電機的運動速度指令。以下轉(zhuǎn)20072003:如選擇末端運動模式，則讀取構(gòu)型指令控制開關(guān)組的值，構(gòu)型指令開關(guān)的數(shù)目等于冗余自由度數(shù)目，通過構(gòu)型指令開關(guān)組可選擇末端運動時不運動的關(guān)節(jié)電機；2004:讀取末端指令操縱桿的值，包括X軸電位計、Y軸電位計和旋轉(zhuǎn)電位計的值，表示機械臂末端的運動指令，其中X軸電位計表示機械臂末端X軸的平動，Y軸電位計表示機械臂末端Y軸的平動，旋轉(zhuǎn)電位計表示機械臂末端在平面內(nèi)的轉(zhuǎn)動。通過2003，2004的步驟，可唯一確定冗余度機械臂的末端運動。并通過反解確定機械臂全部關(guān)節(jié)電機的運動速度指令。以下轉(zhuǎn)20072005:如選擇構(gòu)型調(diào)整模式，則讀取構(gòu)型指令開關(guān)組的值，通過構(gòu)型指令開關(guān)組選擇的軸表示將要實施構(gòu)型調(diào)整運動的電機。構(gòu)型調(diào)整運動的電機數(shù)目等于冗余自由度數(shù)。2006:讀取末端指令操縱桿的值，包括X軸電位計、Y軸電位計和旋轉(zhuǎn)電位計的值，表示構(gòu)型調(diào)整運動指令。其中X軸、Y軸和旋轉(zhuǎn)電位計表示構(gòu)型指令開關(guān)組設(shè)定的電機的運動。通過2005，2006的步驟，可保證機械臂末端位置和姿態(tài)不變的情況下調(diào)整機械臂的構(gòu)型。并通過反解運算確定機械臂全部關(guān)節(jié)電機的運動速度指令。2007:將機械臂關(guān)節(jié)電機運動速度指令通過遠程通訊發(fā)送到下位控制器。如果需要執(zhí)行新的控制命令，則需要重新重復(fù)步驟200卜2007。步驟2003、2004以及2005、2006中，冗余度機械臂電機速度反解算法如下設(shè)平面冗余度機械臂的末端位姿由(x,少,a)表示，其中(x,力表示在運動平面內(nèi)機械臂的末端位置，《則表示在運動平面內(nèi)機械臂的姿態(tài)。x=/t(6^2，...，《)"/2,...，《),""6"=厶(《,《，...,《)式中，y;、/y、厶分別表示末端位姿和關(guān)節(jié)運動的函數(shù)關(guān)系。則末端速度計算如下步驟2003和2004中，設(shè)通過構(gòu)型指令開關(guān)組選擇的軸為4n，n為平面冗余度機械臂的自由度數(shù)，末端速度指令由操縱桿指定，則軸13的速度反解如下<formula>formulaseeoriginaldocumentpage12</formula>步驟2005和2006中，設(shè)通過構(gòu)型指令開關(guān)組選擇的軸為4n，其構(gòu)型調(diào)整速度指令由操縱桿指定，其余13軸的速度計算如下<formula>formulaseeoriginaldocumentpage12</formula>(2)式(1)和式(2)中，《(/=1，...，")表示平面冗余度機械臂第1關(guān)節(jié)的位置，《々、1，...，w)表示平面冗余度機械臂第i關(guān)節(jié)的運動速度。所述的冗余度機械臂，是指在某一平面內(nèi)自由度數(shù)目大于3的冗余度機械臂，對機械臂在該平面內(nèi)的運動控制可采用單軸運動指令輸入、末端運動指令輸入和構(gòu)型調(diào)整指令輸入三種控制模式；除此之外，該機械臂還可具備多個自由度，在本發(fā)明中，這些自由度只能輸入單軸運動指令。本發(fā)明針對平面冗余度機械臂，提出了末端運動指令輸入和構(gòu)型控制方法其中機械臂末端運動指令輸入采用三自由度商用操縱桿控制平面冗余度機械臂末端運動，采用構(gòu)型指令輸入開關(guān)組選擇鎖定關(guān)節(jié)，并通過控制算法反解得到機械臂各關(guān)節(jié)的運動指令，簡單直觀，適合于空曠無障礙環(huán)境；而基于末端位姿保持的構(gòu)型指令輸入采用通用操縱桿輸入待調(diào)整構(gòu)型的軸的運動，并通過控制算法解算得到機械臂構(gòu)型，該方法適合于障礙環(huán)境下機械臂構(gòu)型的調(diào)整。通過這兩種指令輸入裝置和控制算法的組合，使得既可以直接控制機械臂的末端運動，又可以在障礙環(huán)境下調(diào)整機械臂的構(gòu)型，簡單實用，適用于大多數(shù)平面冗余度機械臂的控制指令輸入，如采用六維鼠標(biāo)等空間運動指令輸入裝置，還可應(yīng)用于空間冗余度機械臂的控制。圖l為本發(fā)明系統(tǒng)結(jié)構(gòu)框圖；圖2為上位控制器硬件連接框圖；圖3為下位控制器硬件連接框圖；圖4為機械臂指令輸入和構(gòu)型控制流程圖。圖5為實施例中的冗余度機械臂結(jié)構(gòu)示意圖。具體實施方式下面結(jié)合附圖對本發(fā)明的實施例作詳細說明本實施例在以本發(fā)明技術(shù)方案為前提下進行實施，給出了詳細的實施方式和具體的操作過程，但本發(fā)明的保護范圍不限于下述的實施例。本實施例以平面四自由度機械臂的指令輸入和構(gòu)型控制為例，對本發(fā)明進行詳細說明。如圖1所示，遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)包括冗余度機械臂控制臺1、冗余度機械臂上位控制器2、上位機無線數(shù)據(jù)模塊3、下位機無線數(shù)據(jù)模塊4、冗余度機械臂下位機控制器5以及冗余度機械臂6。冗余度上位控制器2和上位機無線數(shù)據(jù)模塊3安裝在機械臂控制臺1上，與機械臂控制臺1通過控制電路連接；下位機無線數(shù)據(jù)模塊4和冗余度機械臂下位控制器5安裝在冗余度機械臂6上，冗余度機械臂上位控制器2與上位機無線數(shù)據(jù)模塊3通過串行通訊接口連接，冗余度機械臂下位控制器5和下位機無線數(shù)據(jù)模塊4之間也通過串行通訊接口連接。如圖2所示，所述的冗余度機械臂控制臺1，包括模式選擇開關(guān)101、單軸指令輸入開關(guān)組102、構(gòu)型指令輸入開關(guān)組103、單軸速度輸入旋鈕104、速度指令輸入操縱桿105、指示燈106和液晶顯示模塊107。這些器件和上位機控制器通過控制電路連接。模式選擇開關(guān)101為波段開關(guān)，可選擇機械臂的三種控制模式單軸運動模式，末端運動模式和構(gòu)型調(diào)整模式。在任一時刻只能選擇其中一種控制模式；在單軸運動模式下，單軸指令輸入開關(guān)組102和單軸速度輸入旋鈕104的輸入有效；單軸指令輸入開關(guān)組101的作用是控制機械臂每個電機的正反轉(zhuǎn)，單軸速度輸入旋鈕103表示電機的運動速度，單軸運動指令由單軸指令輸入開關(guān)組101和單軸速度輸入旋鈕103組合實現(xiàn)；在末端運動模式下，速度指令輸入操縱桿105和構(gòu)型指令輸入開關(guān)組102有效。速度指令輸入操縱桿103的功能是直接控制平面冗余度機械臂末端的運動速度，操作直觀性強，速度指令輸入操縱桿105為三維操縱桿，內(nèi)部包含三個獨立的電位計，可輸入三個軸的速度指令；本實施例中選擇的操縱桿為日本Sakae公司的30JHK-ZT-30R3G三維操縱桿；構(gòu)型指令輸入開關(guān)組102包括1個冗余軸選擇開關(guān)，表示可用于構(gòu)型調(diào)整的自由度數(shù)為1，設(shè)在此模式下構(gòu)型指令輸入開關(guān)所選擇的軸為1#軸，則表示在末端運動模式下該軸電機鎖定。在構(gòu)型調(diào)整模式下，速度指令輸入操縱桿105和構(gòu)型指令輸入開關(guān)組106有效。構(gòu)型調(diào)整模式指的是機械臂末端位置和姿態(tài)均保持不變的情況下，通過輸入冗余關(guān)節(jié)的運動，調(diào)整機械臂各關(guān)節(jié)之間的相對位置關(guān)系。該功能是這樣實現(xiàn)的由構(gòu)型指令輸入開關(guān)組102選擇要調(diào)整的冗余關(guān)節(jié)，設(shè)選擇3#軸；此時速度指令輸入操縱桿105的三個電位計的輸入中，X軸電位計的輸入表示3#軸的運動速度，Y軸電位計和旋轉(zhuǎn)電位計的輸入無效。指示燈106，主要功能為顯示各關(guān)節(jié)軸的限位信息以及電機運動過程的報警信息；液晶顯示模塊107，主要功能為顯示關(guān)節(jié)軸電機的位置及報警的詳細信息。冗余度機械臂上位控制器2，主要由數(shù)字量輸入接口201、濾波電路202、模擬量輸入接口203、數(shù)字量輸出擴展模塊204、數(shù)字量輸出接口205、主控芯片206、數(shù)據(jù)存儲模塊207和串行通訊接口208組成。這些部件均采用常規(guī)的數(shù)字電路或電子元器件實現(xiàn)。主控芯片可以是ARM、DSP、FPGA等可編程芯片。本實施例中，主控芯片206為PHILIPS公司的LPC2131芯片，通過片上通用1/0端口與數(shù)字量輸入接口201、數(shù)字量輸出擴展模塊205連接，其中數(shù)字量輸入接口201用于接入模式選擇開關(guān)、單軸指令輸入開關(guān)組以及構(gòu)型指令輸入開關(guān)組的開、合指令輸入；數(shù)字量輸出擴展模塊205用于擴展輸出端口，經(jīng)數(shù)字量輸出接口204完成光電隔離、電平轉(zhuǎn)換后與液晶顯示模塊107以及指示燈106連接；主控芯片206通過A/D端口和模擬量輸入接口203連接，經(jīng)濾波電路202后采集冗余度機械臂控制臺的單軸速度控制旋鈕104和速度指令輸入操縱桿105輸入的電壓量；主控芯片206通過12(:總線與靜態(tài)數(shù)據(jù)存儲模塊207連接。數(shù)據(jù)存儲模塊207用于存儲上位機命令解析程序、機械臂系統(tǒng)的工作參數(shù)、遠程通訊地址和通訊協(xié)議，其存儲的數(shù)據(jù)掉電不丟失。數(shù)據(jù)存儲模塊207用于存儲上位機命令解析程序、機械臂系統(tǒng)的工作參數(shù)、遠程通訊地址和通訊協(xié)議，其存儲的數(shù)據(jù)掉電不丟失；數(shù)據(jù)存儲模塊207采用電可擦除可編程只讀存儲器EEPROM，EEPR0M207與LPC2131通過I2C總線連接。冗余度機械臂上位控制器2，通過串行通訊接口208與上位機無線數(shù)據(jù)模塊3連接與通訊，將編碼的控制命令發(fā)送到遠端的無線數(shù)據(jù)模塊，并同時獲取遠端機械臂6的狀態(tài)信息。本例的上位機無線數(shù)據(jù)模塊3和下位機無線模塊4，選用北京捷麥通信器材有限公司生產(chǎn)的F—49P無線模塊。如圖3所示，冗余度機械臂下位控制器4，與機械臂6固定安裝，包括串行通訊接口301，數(shù)字信號處理器302、高速脈沖輸入接口303、數(shù)字量輸入接口304、數(shù)字量輸出接口305以及模擬量輸出接口306。其中，數(shù)字信號處理器302通過串行通訊接口301與無線數(shù)據(jù)模塊通訊，數(shù)字信號處理器302通過數(shù)據(jù)總線和高速脈沖輸入接口303電路連接，用于接收電機的編碼器信號，反饋電機的位置、速度信息；其中高速脈沖輸入接口303的功能包括正交編碼信號處理、計數(shù)以及地址譯碼；可以由常規(guī)數(shù)字電路和分立器件組成，也可由CPLD/FPGA等集成電路組成；數(shù)字信號處理器302通過GPIO(通用輸入輸出)端口和數(shù)字量輸入接口304以及數(shù)字量輸出接口305連接，其中數(shù)字量輸入接口304的功能為光電隔離、端口擴展以及電平轉(zhuǎn)換，由常規(guī)的數(shù)字電路和電子元器件組成；數(shù)字量輸出接口305的功能為信號放大、端口擴展以及繼電器控制，也由常規(guī)的數(shù)字電路和電子原器件組成。數(shù)字量輸入接口304用于接入原點、限位以及驅(qū)動器狀態(tài)如報警等信號；數(shù)字量輸出接口305用于接入驅(qū)動器控制信號如驅(qū)動器使能以及報警清除等信號；數(shù)字信號處理器302通過數(shù)據(jù)端口和模擬量輸出接口306連接，模擬量輸出接口306的功能包括D/A轉(zhuǎn)換和電壓放大，由常規(guī)的數(shù)字電路和電子元器件組成。模擬量輸出端口用于向驅(qū)動器提供速度指令。本實施例中，數(shù)字信號處理器302為TI公司的TMS320LF2407A芯片，通過串行通訊接口與無線數(shù)據(jù)模塊通訊，接收上位控制器的控制指令，并反饋電機的位置、速度以及報警等狀態(tài)信息。并通過擴展端口與高速脈沖輸入接口303、數(shù)字量輸入接口304、數(shù)字量輸出接口305以及模擬量輸出接口306連接。本實施例中采用日本松下MINAS—A4系列交流伺服電機和驅(qū)動器。其中電機安裝在冗余度機械臂的驅(qū)動關(guān)節(jié)處，用于驅(qū)動冗余度機械臂的關(guān)節(jié)運動，并附帶驅(qū)動器和編碼器。編碼器通過高速脈沖輸入接口303和數(shù)字信號處理器連接，提供任意時刻電機的位置和速度信息；驅(qū)動器通過模擬量輸出接口306和數(shù)字信號處理器連接，接收電機運動的速度指令；同時通過數(shù)字量輸出接口304接收數(shù)字信號處理器302的伺服使能、報警清除，機械臂各關(guān)節(jié)的限位、原點和驅(qū)動器狀態(tài)信息如報警等通過數(shù)字量輸入接口與數(shù)字信號處理器302連接并反饋電機的狀態(tài)信息。實施例中的冗余度機械臂如圖5所示。冗余度機械臂6包括腰部機構(gòu)501、第一臂502、第一推桿503、第二臂504、第二推桿505、第三臂506、第三推桿507、連桿機構(gòu)508、手腕機構(gòu)509、第四推桿510、第四臂511(手抓)。第一臂502的一端采用銷釘方式連接腰部機構(gòu)501，第一推桿503采用銷釘方式連接腰部機構(gòu)501和第一臂502，第一臂502在第一推桿503的伸縮作用下可以實現(xiàn)相對于腰部機構(gòu)501的俯仰運動，其運動角度范圍不小于110度。第二臂504的一端采用銷釘方式連接第一臂502的另一端，第二推桿505采用銷釘方式連接第一臂502和第二臂504，第二臂504在第二推桿505的伸縮作用下可以實現(xiàn)相對于第一臂502的俯仰運動，其運動角度范圍不小于90度。第三臂506的一端采用銷釘方式連接第二臂504的另一端，連桿機構(gòu)508采用銷釘方式連接第三臂506和第二臂504，第三推桿507采用銷釘方式連接第二臂504和連桿機構(gòu)508，第三臂506在第三推桿507的伸縮作用和連桿機構(gòu)508的放大作用下可以實現(xiàn)相對于第二臂504的俯仰運動，其運動角度范圍不小于145度。手腕機構(gòu)509采用銷釘方式連接第三臂506的另一端，第四推桿510采用銷釘方式連接第三臂506和手腕機構(gòu)509，手腕機構(gòu)509在第四推桿510的伸縮作用下可以實現(xiàn)相對于第三臂506的俯仰運動，其運動角度范圍不小于90度。手爪511采用軸方式連接到手腕機構(gòu)509，手爪511為外配裝置，可以進行開合動作，在手腕機構(gòu)509帶動下相對于手腕機構(gòu)9進行回轉(zhuǎn)。其回轉(zhuǎn)角度范圍為連續(xù)回轉(zhuǎn)，也可以設(shè)定為非連續(xù)回轉(zhuǎn)的某一角度范圍。在本實施例中，第一、二、三、四臂的構(gòu)成平面冗余度機械臂，其控制模式包括末端運動控制和構(gòu)型調(diào)整模式；而腰部回轉(zhuǎn)和手腕回轉(zhuǎn)則僅具備單軸運動模式。如圖4所示，本實施例的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制方法，包括如下步驟2001:讀取下位機無線數(shù)據(jù)模塊4發(fā)送的機械臂當(dāng)前位置和狀態(tài)。通過模式選擇開關(guān)101，操作人員可選擇單軸運動模式，末端運動模式和構(gòu)型調(diào)整模式2002:如果選擇單軸運動模式，則讀取單軸指令輸入開關(guān)組102和單軸速度控制旋鈕104的值，表示電機的運動方向和速度指令輸入；從而確定機械臂各個關(guān)節(jié)電機的運動速度指令。以下轉(zhuǎn)20072003:如選擇末端運動模式，則讀取構(gòu)型指令控制開關(guān)組103的值，本實施例中，構(gòu)型指令開關(guān)的數(shù)目為l，通過構(gòu)型指令開關(guān)組可選擇末端運動時鎖定的關(guān)節(jié)電機，設(shè)選擇第一臂502為鎖定關(guān)節(jié)；2004:讀取速度指令操縱桿的值，包括X軸電位計、Y軸電位計和旋轉(zhuǎn)電位計的值，表示機械臂末端的運動指令，其中X軸電位計表示機械臂末端X軸的平動，Y軸電位計表示機械臂末端Y軸的平動，旋轉(zhuǎn)電位計表示機械臂末端在平面內(nèi)的轉(zhuǎn)動。通過2003，2004的步驟，可通過式3唯一確定冗余度機械臂的末端運動。并通過反解確定機械臂全部關(guān)節(jié)電機的運動速度指令。以下轉(zhuǎn)20072005:如選擇構(gòu)型調(diào)整模式，則讀取構(gòu)型指令開關(guān)組103的值，本實施例中，構(gòu)型指令開關(guān)的數(shù)目為1，設(shè)選擇第三臂506進行構(gòu)型調(diào)整運動。2006:讀取末端指令操縱桿的值，本實施例中，僅需讀取X軸電位計的值，，表示對第三臂506的構(gòu)型調(diào)整速度指令，Y軸電位計和旋轉(zhuǎn)電位計的輸入無效。通過2005，2006的步驟，可保證機械臂末端位置和姿態(tài)不變的情況下調(diào)整機械臂的構(gòu)型。并通過式4反解確定機械臂各個關(guān)節(jié)電機的運動速度指令。2007:將機械臂關(guān)節(jié)電機運動速度指令通過遠程通訊發(fā)送到下位控制器。如果需要執(zhí)行新的控制命令，則需要重新重復(fù)步驟2001-2007。步驟2003、2004以及2005、2006中，冗余度機械臂電機速度反解算法如下設(shè)平面冗余度機械臂的末端位姿由O,少，")表示，其中(;c,力表示在運動平面內(nèi)機械臂的末端位置，a則表示在運動平面內(nèi)機械臂的姿態(tài)。"=厶3，《)則末端速度計算如下<formula>formulaseeoriginaldocumentpage18</formula>步驟2003和2004中，通過構(gòu)型指令開關(guān)組選擇第一臂502為鎖定關(guān)節(jié)，0，則軸13的速度反解如下<formula>formulaseeoriginaldocumentpage18</formula>(3)步驟2005和2006中，通過構(gòu)型指令開關(guān)組選擇的軸為第三臂506，其構(gòu)型調(diào)整速度指令由操縱桿X軸電位計指定，其余13軸的速度計算如下本實施例針對平面4自由度機械臂，實現(xiàn)了末端運動指令輸入和構(gòu)型控制，實現(xiàn)方法簡單直觀，適合于空曠無障礙環(huán)境，既可以直接控制機械臂的末端運動，又可以在障礙環(huán)境下調(diào)整機械臂的構(gòu)型，簡單實用。<formula>formulaseeoriginaldocumentpage19</formula>權(quán)利要求1、一種遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)，其特征在于包括冗余度機械臂控制臺、冗余度機械臂上位控制器、上位機無線數(shù)據(jù)模塊、下位機無線數(shù)據(jù)模塊、冗余度機械臂下位機控制器以及冗余度機械臂組成，其中所述冗余度機械臂控制臺提供人機交互，負責(zé)接收操作員的指令并顯示機械臂當(dāng)前位置、限位、報警狀態(tài)信息，該冗余度機械臂控制臺上設(shè)有模式選擇開關(guān)、單軸指令輸入開關(guān)組、構(gòu)型指令輸入開關(guān)組、單軸速度輸入旋鈕、末端指令輸入操縱桿、指示燈和液晶顯示模塊，通過模式選擇開關(guān)選擇機械臂的單軸運動模式、末端運動模式和構(gòu)型調(diào)整模式中一種控制模式；所述冗余度機械臂上位控制器安裝在冗余度機械臂控制臺上，和冗余度機械臂上位控制器之間通過控制電路連接，冗余度機械臂上位控制器通過串行通訊接口與上位機無線數(shù)據(jù)模塊連接與通訊，將編碼的控制命令發(fā)送到遠端的下位機無線數(shù)據(jù)模塊，并同時獲取遠端機械臂的狀態(tài)信息；所述無線數(shù)據(jù)模塊提供冗余度機械臂上位控制器和冗余度機械臂下位控制器之間的通訊功能，由工作在同一工作頻率的一對模塊完成，其間的距離為100米，上位機、下位機無線數(shù)據(jù)模塊和冗余度機械臂上、下位控制器之間均通過串行通訊接口交換數(shù)據(jù)；所述冗余度機械臂下位控制器安裝在冗余度機械臂上，接收冗余度機械臂上位控制器對冗余度機械臂各關(guān)節(jié)軸的運動指令，并輸出到冗余度機械臂的各關(guān)節(jié)電機上，同時將冗余度機械臂的位置、速度、限位和報警狀態(tài)信息發(fā)送給下位機無線數(shù)據(jù)模塊。2.根據(jù)權(quán)利要求1所述的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)，其特征是，在所述單軸運動模式下，單軸指令輸入開關(guān)組和單軸速度輸入旋鈕的輸入有效，單軸指令輸入開關(guān)組控制機械臂每個電機的正反轉(zhuǎn)，單軸速度輸入旋鈕表示軸電機的運動速度，單軸運動指令由單軸指令輸入開關(guān)組和單軸速度輸入旋鈕組合實現(xiàn)。3.根據(jù)權(quán)利要求1所述的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)，其特征是，在所述木端運動模式下，速度指令輸入操縱桿和構(gòu)型指令輸入開關(guān)組有效，速度指令輸入操縱桿直接控制末端機械臂在平面內(nèi)的運動速度，操縱桿為三維操縱桿，內(nèi)部包含三個獨立的電位計，可輸入三個軸的速度指令；構(gòu)型指令輸入開關(guān)組包括1—3個冗余軸選擇波段開關(guān)，對應(yīng)于平面冗余度為1到3的情況，在此模式下構(gòu)型指令輸入開關(guān)所選擇的軸表示該軸電機鎖定。4.根據(jù)權(quán)利要求1所述的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)，其特征是，在所述構(gòu)型調(diào)整模式下，速度指令輸入操縱桿和構(gòu)型指令輸入開關(guān)組有效，由構(gòu)型指令輸入開關(guān)組選擇要調(diào)整的關(guān)節(jié)軸，此時速度指令輸入操縱桿三個電位計的輸入表示由構(gòu)型指令輸入開關(guān)組設(shè)定的關(guān)節(jié)軸的運動速度，從而實現(xiàn)機械臂末端位置和姿態(tài)均保持不變的情況下，通過輸入冗余關(guān)節(jié)的運動，調(diào)整機械臂各關(guān)節(jié)之間的相對位置關(guān)系。5.根據(jù)權(quán)利要求1所述的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)，其特征是，所述的冗余度機械臂上位控制器，主要由數(shù)字量輸入接口、濾波電路、模擬量輸入接口、數(shù)字量輸出擴展模塊、數(shù)字量輸出接口、主控芯片、數(shù)據(jù)存儲模塊和串行通訊接口組成，其中主控芯片通過通用1/0端口與數(shù)字量輸入接口、數(shù)字量輸出擴展模塊連接，數(shù)字量輸入接口用于接入模式選擇開關(guān)、單軸指令輸入開關(guān)組以及構(gòu)型指令輸入開關(guān)組的開、合指令輸入；數(shù)字量輸出擴展模塊用于擴展輸出端口，經(jīng)數(shù)字量輸出接口完成光電隔離、電平轉(zhuǎn)換后與液晶顯示模塊以及指示燈連接；主控芯片通過A/D端口和模擬量輸入接口連接，經(jīng)濾波電路后采集冗余度機械臂控制臺的單軸速度控制旋鈕和速度指令輸入操縱桿輸入的電壓量；主控芯片通過I2C總線與靜態(tài)數(shù)據(jù)存儲模塊連接，數(shù)據(jù)存儲模塊用于存儲上位機命令解析程序、機械臂系統(tǒng)的工作參數(shù)、遠程通訊地址和通訊協(xié)議，其存儲的數(shù)據(jù)掉電不丟失。6.根據(jù)權(quán)利要求1所述的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)，其特征是，所述的冗余度機械臂下位控制器，與機械臂固定安裝，包括串行通訊接口，數(shù)字信號處理器、高速脈沖輸入接口、數(shù)字量輸入接口、數(shù)字量輸出接口以及模擬量輸出接口，其中，數(shù)字信號處理器通過數(shù)據(jù)總線和高速脈沖輸入接口電路連接，用于接收電機的編碼器信號，反饋電機的位置、速度信息；數(shù)字信號處理器通過GPIO端口和數(shù)字量輸入接口以及數(shù)字量輸出接口連接，數(shù)字量輸入接口用于接入原點、限位以及驅(qū)動器狀態(tài)；數(shù)字量輸出接口用于接入驅(qū)動器控制信號；數(shù)字信號處理器通過數(shù)據(jù)端口和模擬量輸出接口連接，用于向驅(qū)動器提供速度指令，數(shù)字信號處理器通過串行通訊接口與無線數(shù)據(jù)模塊通訊，接收上位控制器的控制指令，并反饋電機的位置、速度以及報警信息。7.根據(jù)權(quán)利要求1所述的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)，其特征是，所述的冗余度機械臂由安裝在其關(guān)節(jié)處的電機進行驅(qū)動，該電機附帶編碼器和驅(qū)動器，編碼器通過高速脈沖輸入接口和數(shù)字信號處理器連接，提供任意時刻電機的位置和速度信息；驅(qū)動器通過模擬量輸入接口和數(shù)字信號處理器連接，接收電機運動的速度指令，同時通過數(shù)字量輸入接口接收數(shù)字信號處理器的開關(guān)指令，并反饋電機的狀態(tài)信息。8.—種遙操作平面冗余度機械臂指令輸入及構(gòu)型控制方法，其特征在于，包括如下的步驟2001:讀取無線數(shù)據(jù)模塊發(fā)送的機械臂當(dāng)前位置和狀態(tài)，通過模式選擇按鈕，操作人員選擇單軸運動模式、末端運動模式和構(gòu)型調(diào)整模式中的一種；2002:如果選擇單軸運動模式，則讀取單軸指令輸入開關(guān)組和單軸速度控制旋鈕的值，表示電機的運動方向和速度指令輸入，從而確定機械臂全部關(guān)節(jié)電機的運動速度指令，轉(zhuǎn)步驟2007;2003:如選擇末端運動模式，則讀取構(gòu)型指令控制開關(guān)組的值，構(gòu)型指令開關(guān)的數(shù)目等于冗余自由度數(shù)目，通過構(gòu)型指令開關(guān)組選擇末端運動時不運動的關(guān)節(jié)電機；2004:讀取末端指令操縱桿的值，包括X軸電位計、Y軸電位計和旋轉(zhuǎn)電位計的值，表示機械臂末端的運動指令，其中X軸電位計表示機械臂末端X軸的平動，Y軸電位計表示機械臂末端Y軸的平動，旋轉(zhuǎn)電位計表示機械臂末端在平面內(nèi)的轉(zhuǎn)動；通過2003，2004的步驟，能唯一確定冗余度機械臂的末端運動，并通過反解確定機械臂全部關(guān)節(jié)電機的運動速度指令，轉(zhuǎn)步驟2007;2005:如選擇構(gòu)型調(diào)整模式，則讀取構(gòu)型指令開關(guān)組的值，通過構(gòu)型指令開關(guān)組選擇的軸表示將要實施構(gòu)型調(diào)整運動的電機，構(gòu)型調(diào)整運動的電機數(shù)目等于冗余自由度數(shù)；2006:讀取末端指令操縱桿的值，包括X軸電位計、Y軸電位計和旋轉(zhuǎn)電位計的值，表示構(gòu)型調(diào)整運動指令，其中X軸、Y軸和旋轉(zhuǎn)電位計表示構(gòu)型指令開關(guān)組設(shè)定的電機的運動；通過2005，2006的步驟，保證機械臂末端位置和姿態(tài)不變的情況下調(diào)整機械臂的構(gòu)型，并通過反解運算確定機械臂全部關(guān)節(jié)電機的運動速度指令；2007:將機械臂關(guān)節(jié)電機運動速度指令通過遠程通訊發(fā)送到下位控制器，如果需要執(zhí)行新的控制命令，則重新重復(fù)步驟2001-2007。9.根據(jù)權(quán)利要求8所述的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制方法，其特征是，所述步驟2003、2004以及2005、2006中，冗余度機械臂電機速度反解運算如下-設(shè)平面冗余度機械臂的末端位姿由Oc，y,")表示，其中(x，力表示在運動平面內(nèi)機械臂的末端位置，"則表示在運動平面內(nèi)機械臂的姿態(tài)，<formula>formulaseeoriginaldocumentpage5</formula>則末端速度計算如下<formula>formulaseeoriginaldocumentpage5</formula>式中，/;、/y、y;分別表示末端位姿和關(guān)節(jié)運動的函數(shù)關(guān)系。10.根據(jù)權(quán)利要求8或9所述的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制方法，其特征是，所述步驟2003和2004中，設(shè)通過構(gòu)型指令開關(guān)組選擇的軸為4n，n為平面冗余度機械臂的自由度數(shù)，末端速度指令由操縱桿指定，則軸13的速度反解如下<formula>formulaseeoriginaldocumentpage5</formula>步驟2005和2006中，設(shè)通過構(gòu)型指令開關(guān)組選擇的軸為4n，其構(gòu)型調(diào)整速度指令由操縱桿指定，其余13軸的速度計算如下《(/=1，...,")表示平面冗余度機械臂第i關(guān)節(jié)的位置，《(/=1,...,")表示平面冗余度機械臂第i關(guān)節(jié)的運動速度。<table>tableseeoriginaldocumentpage31</column></row><table>全文摘要本發(fā)明公開一種機械控制
技術(shù)領(lǐng)域：
的遙操作平面冗余度機械臂指令輸入及構(gòu)型控制系統(tǒng)及方法。本發(fā)明包括冗余度機械臂控制臺、冗余度機械臂上位控制器、上位機無線數(shù)據(jù)模塊、下位機無線數(shù)據(jù)模塊、冗余度機械臂下位機控制器以及冗余度機械臂組成，通過機械臂控制臺向機械臂發(fā)出控制指令，選擇單軸運動模式、末端運動模式和構(gòu)型調(diào)整模式中的一種，其中通過單軸運動模式操作人員控制機械臂各軸的運動；通過末端運動模式，操作人員直接控制機械臂末端的運動；通過構(gòu)型調(diào)整模式，操作人員可在保持機械臂末端位置和姿態(tài)不變的情況下，調(diào)整機械臂的構(gòu)型，適用于障礙環(huán)境，提高了冗余度機械臂在復(fù)雜環(huán)境下的操作性能。文檔編號B25J9/18GK101327589SQ200810040450公開日2008年12月24日申請日期2008年7月10日優(yōu)先權(quán)日2008年7月10日發(fā)明者張偉軍,琰徐,強李,李建華,袁建軍申請人:上海交通大學(xué)