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