專利名稱：遙操作平面冗余度機械臂自主避障智能單元、系統及方法
技術領域：
本發明涉及的是一種機械控制技術領域的控制單元、系統及方法，具體是一 種遙操作平面冗余度機械臂自主避障智能單元、系統及方法。
背景技術：
在核工業、反恐排爆、救援、高電壓設備清掃、維護等危險、惡劣環境中的 檢査、維修、搬運、抓取、裝配等特種作業廣泛采用遙操作機械臂，目前的機械 臂自由度大多小于或等于6，適用于無障礙環境；而對于某些特殊的多障礙約束 環境作業，如排爆作業中從車底抓取爆炸物，救援作業中從窗戶進入靠近地面的 被困人員，高壓帶電清掃變電站中的絕緣子等作業，由于工作環境中存在較多障 礙，需要機械臂提供冗余自由度。冗余自由度主要用來擴大機械臂工作范圍、優 化機械臂運動學、動力學性能以及回避障礙。平面冗余度機械臂是指在一個平面 內機械臂自由度大于3的機械臂，是冗余度機械臂的一個重要組成部分。
通過在機械手的手臂和手爪表面上安裝距離傳感器，可實時感知機械臂及周 邊環境信息，并用于避障控制。目前大多通過直接接入或者通過通訊的方式將傳 感信息接入到機械臂控制器，并通過程序計算得出機械臂和障礙的相對位置關系 及避障策略。但是由于傳感器種類、數目較多，直接接入控制器將導致控制系統 規模龐大，布線復雜，可靠性低，而通過通訊的方式將傳感信息接入到機械臂控 制器可適度降低系統的復雜度，但集中式信息處理和優化策略仍然會導致系統實 時性降低，操作性能下降。
經對現有技術的文獻檢索發現，中國專利200710022775.9提出了一種液壓 挖掘機避障控制系統和方法，通過角度傳感器獲取挖掘機的動臂、斗桿、鏟斗和 工作平臺的狀態，獲得挖掘機工作裝置的位置參數，并通過手動設置障礙物的預 警距離及在線計算判斷是否發生碰撞。避障策略包括自動停機和向操作人員發出 聲音及圖像預警。中國專利CN1304178C提出了一種多機械臂機器人關節間的碰 撞檢測方法，通過在機械臂各關節兩端安裝傳感器，得到機械臂各關節端點的空間坐標，傳遞到計算機控制系統后通過程序計算判斷兩關節在空間是否發生碰 撞，從而實現多機械臂關節間的碰撞檢測。中國專利CN1861330A提出了一種移 動機器人的障礙感知及自主避障方法。采用超聲和紅外距離傳感器感知前部的障 礙，并將傳感信號傳遞到微處理器，避障策略則包括慢行、左轉、右轉、停止等。
上述技術在機械臂姿態檢測、碰撞檢測等方面有一定提高，但仍存在如下缺 點(1)碰撞需要通過控制器計算獲得機械臂和環境之間的位姿信息，對計算機 的計算能力以及多種傳感信息集成能力要求高；(2)避障策略需要通過機械臂控 制器對外部所有的傳感信息獲取后進行計算，信息傳遞過程復雜；(3)避障算法 簡單，不能適用于冗余度機械臂。專利200710022775. 9沒有直接的障礙檢測器 件，需要實時計算挖掘機和障礙的距離，僅適用于簡單的結構化障礙分布；專利 CN1304178C則需要采集多個障礙感知傳感器的信息并集成到控制器進行計算， 系統構成復雜，不適合特種機器人快速高效作業的要求；專利CN1861330A也是 采用傳感信息集中處理，且僅面向移動機器人。
目前特種作業機器人主要以遙操作為主，而如何在遙操作的框架下實現冗余 度機械臂高效自主避障，則未見報道。
發明內容
本發明針對現有技術的不足，提供了一種遙操作平面冗余度機械臂自主避障 智能單元、系統及方法。通過在機械臂及手爪表面設置紅外距離傳感器來實時感 知障礙，并針對每個機械臂關節設置智能單元，該智能單元采集該節機械臂表面 紅外距離傳感器的障礙感知信號，通過傳感信息測量值將關節臂狀態分為正常、 避障和停止狀態，并啟動不同的運動控制策略，通過該避障方法及智能單元，可 以簡化系統接線，使得當通過遙操作控制機械臂末端趨近目標的同時可快速感知 障礙并實現避障，從而達到高實時性、高可靠性，提高冗余度機械臂在障礙環境 下的作業效率和操作性能。
本發明是通過以下技術方案實現的
本發明所涉及的遙操作平面冗余度機械臂自主避障智能單元，包括現場總線 接口、軸運動數據存儲模塊、數字信號處理器、運動控制與避障模塊、高速脈沖 輸入接口、模擬量輸出接口、數字量輸出接口、數字量輸入接口、模擬輸入接口、 驅動單元、上表面紅外距離傳感器陣列、下表面紅外距離傳感器陣列，其中
數字信號處理器通過地址及數據總線與現場總線接口連接，用于接收微處理 器傳送的軸運動命令，并反饋軸運動指令及位置、速度及報警等狀態信息；
數字信號處理器通過數據總線和高速脈沖輸入接口電路連接，用于接收電機 的編碼器信號，反饋電機的位置、速度信息；
數字信號處理器通過數據端口和模擬量輸出接口連接，用于向驅動器提供速 度指令；
數字信號處理器通過GPIO (通用輸入輸出)端口和數字量輸入接口以及數 字量輸出接口連接，數字量輸入接口用于接入原點、限位以及驅動器狀態；數字 量輸出接口用于發送驅動器控制信號；
數字信號處理器通過數據端口與模擬量輸入接口連接，用于采集紅外距離傳 感器感知的與環境的距離信息，模擬量輸入接口實現信號放大、光電隔離、濾波 及A/D轉換；
所述的軸運動數據存儲模塊，通過i2c總線與數字信號處理器連接，用于存 儲關節軸工作參數和避障策略；
所述的驅動單元，用于驅動冗余度機械臂的關節運動，提供任意時刻電機的 位置和速度信息，并接收數字信號處理器電機運動的速度指令；
所述的上、下表面紅外距離傳感器陣列，感應方向垂直于關節臂外表面。紅 外距離傳感器通過紅外發送器以及位置感應器件(PSD)獲取與環境的距離，并 以模擬量的形式體現，如電壓；并通過模擬量輸入接口接入數字信號處理器；
所述運動控制及避障模塊運行于數字信號處理器中，用于綜合避障系統微處 理器的軸運動命令和避障策略，運動控制及避障模塊計算軸的運動速度，并根據 軸運動數據存儲模塊中設置的碰撞預警閾值將關節軸分為三種運動狀態，正常狀 態、避障狀態以及停止狀態，同時還將計算的軸運動速度通過數字信號處理器的 模擬量輸出端口發送到驅動單元的驅動器，并通過數字信號處理器的高速脈沖接 口反饋電機轉速，構成速度閉環，從而實現軸的運動。
所述運動控制及避障模塊計算軸的運動速度，軸運動首先在軸運動數據存儲 模塊中設置碰撞預警閾值1和碰撞預警閾值2，閾值1〉閾值2，并存儲于軸運動 數據存儲模塊中。運動控制及避障模塊實時采集該關節臂上表面紅外距離傳感器 陣列和下表面紅外距離傳感器陣列，并根據紅外距離傳感器陣列的輸出值，將關 節軸分為三種運動狀態(1)正常狀態。此時該關節臂上、下表面紅外距離傳感 器陣列中全部紅外距離傳感器的測量值均大于碰撞預警閾值1;表明沒有碰撞危
險，智能單元執行微處理器運動協調模塊通過總線發送的規劃運動；(2)避障狀 態。此時上、下表面紅外距離傳感器陣列中至少有一個紅外距離傳感器測量值在 閾值1和閾值2之間，智能單元將根據存儲在軸運動數據存儲模塊中的避障策略 執行避障運動；(3)停止狀態。此時上、下表面紅外距離傳感器陣列中的有至少 一個紅外距離傳感器測量值小于碰撞預警閾值2，表明該關節臂有碰撞危險，智
能單元將停止軸運動，并將狀態通過總線發送到微處理器的運動協調模塊進行處 理。
所述驅動單元，包括安裝在冗余度機械臂的驅動關節處的電機，用于驅動冗 余度機械臂的關節運動，并附帶編碼器和驅動器。編碼器通過高速脈沖輸入接口 和數字信號處理器連接，提供任意時刻電機的位置和速度信息；驅動器通過模擬 量輸入接口和數字信號處理器連接，接收電機運動的速度指令；同時通過數字量 輸入接口接收數字信號處理器的伺服使能等開關指令，并反饋電機的狀態信息。
所述上、下表面紅外距離傳感器陣列，分別由安裝在平面冗余度機械臂各關 節臂的上下表面的一組紅外距離傳感器組成，每組紅外距離傳感器的數目在1到 8之間。
本發明所涉及的遙操作平面冗余度機械臂自主避障系統，包括系統數據存儲 模塊、微處理器、串行通訊接口、現場總線接口以及智能單元l n。 n為平面冗 余度機械臂關節數。其中數據存儲模塊、微處理器、串行通訊接口、現場總線接 口安裝在冗余度機械臂上，智能單元l n安裝在各關節臂內部。
所述系統數據存儲模塊用于存儲機械臂運動控制程序、機械臂系統的工作參 數、遠程通訊地址和通訊協議，其存儲的數據掉電不丟失。數據存儲模塊與微處 理器通過i2c總線連接。
所述的微處理器用于綜合遠程操作人員提供的機械臂末端運動指令以及智 能單元返回的狀態信息，通過運動協調模塊運算得到機械臂各軸的運動指令；
所述運動協調模塊運行于微處理器中，根據操作人員發送的機械臂末端速度 指令和各智能單元發送的各關節狀態和速度實施運動規劃，并通過現場總線發送 到各智能單元。當所有智能單元都處于正常狀態時，運動協調模塊根據能量最小
原則反解各軸運動速度；當有智能單元處于避障狀態且數目小于等于冗余自由度 時，通過運動協調模塊可以通過協調其他智能單元的運動，從而保證機械臂末端 按照操作人員發送的末端速度指令運動的同時實施避障；當有智能單元處于停止 狀態時，表明該關節臂處于碰撞危險狀態，運動協調模塊對各軸智能單元發送運 動停止命令，停止機械臂運動。
所述的串行通訊接口，主要用于接收遠程的機械臂末端運動指令，并轉換為 微處理器可以處理的數據格式；同時將機械臂各軸的位置、速度及狀態信息發送 給遠程操作人員；微處理器通過其通用異步收發器(UART)端口與串行通訊接口 連接。
所述的現場總線接口，用于將微處理器計算的各軸運動指令轉化為總線數據 格式，并通過總線通訊協議發送到各個智能單元，包括總線收發器、光電隔離、 電平轉換及濾波電路，由常規的數字電路和電子元器件組成。微處理器通過地址 及數據總線與總線接口連接。
所述的智能單元，安裝于各關節臂的內部，用于綜合微處理器的軸運動命令 和紅外距離傳感器感知的信號，通過運動控制與避障模塊運算得到該軸的運動指 令，并控制驅動單元完成軸的驅動。智能單元通過總線接口與總線連接。
本發明所涉及的遙操作平面冗余度機械臂自主避障方法，包括運動控制及避 障模塊進行的運動控制及避障，以及運動協調模塊進行的運動協調兩部分，其中
所述的運動控制及避障模塊進行的運動控制及避障包括如下步驟
60401:讀取微處理器通過總線發送的軸運動速度指令；
60402:讀取關節臂上的紅外距離傳感器陣列的輸出。該值是經過模數轉換 后的整型數。
60403:若有任一紅外距離傳感器的值〈碰撞預警閾值2，表明機械臂處于危 險區域內，需要立刻使系統停止，處理內容轉60409;否則繼續。
60404:若上表面紅外距離傳感器陣列中有任一傳感器的值〈碰撞預警閾值1 ， 則表明機械臂處于避障區域，以下轉60406，否則轉60405。
60405:若下表面紅外距離傳感器陣列中有任一傳感器的值〈碰撞預警閾值1， 則表明機械臂處于避障區域，以下轉60407，否則轉60408。
60406:令驅動關節軸電機反向勻速運動進行避障。(設機械臂向上運動為正
向)，同時設關節軸狀態為"避障"，以下轉60410;
60407:令驅動關節軸電機正向勻速運動進行避障，同時設關節軸狀態為"避 障"。以下轉60410;
60408:若所有紅外距離傳感器輸出均大于碰撞預警閾值1，表明機械臂行 進過程無障礙，關節軸以規劃速度運動，同時設關節軸狀態為"正常"。以下轉 60410;
60409:令關節軸電機停止運動，同時設關節軸狀態為"停止"；
60410:將關節軸運動輸出到驅動單元，同時將電機位置、速度和狀態信息
發送到微處理器。
所述運動協調模塊進行的運動協調包括如下步驟
3001:讀取串行通訊接口接收的機械臂末端速度指令。該指令由操作人員遠 程發送；
3002:讀取智能單元發送的各關節軸位置、狀態及控制指令。
3003:如果有關節軸處于"停止"狀態，則表明有關節臂有碰撞危險，已經 停止運動，以下轉3009，否則轉3004。
3004:如果有關節軸處于"避障"狀態，則表明有關節智能單元正在執行避 障指令，需要重新規劃各關節軸電機的運動；以下轉3005，否則表示沒有智能 單元沒有檢測到障礙，以下轉3006。
3005:判斷處于"避障"狀態的關節軸的數目是否小于等于冗余自由度數， 如果"是"，這表明可以實施避障運動規劃，以下轉3007;否則表明無法協調各 關節軸運動同時滿足機械臂末端運動指令和避障運動指令，轉3008。
3006:按照操作人員的末端速度指令反解得到各軸速度指令；
通過步驟3005，可通過反解計算得到正常狀態下機械臂各關節軸電機的規 劃速度；
3007:根據各關節的位置和智能單元的避障運動速度以及操作人員發送的機 械臂末端速度指令，重新反解機械臂各軸速度指令；
通過步驟3007，微處理器可以協調各智能單元的避障策略，通過反解計算 機械臂其他各關節軸電機的規劃速度，從而保證機械臂末端在執行操作人員速度 指令的同時實現關節臂避障運動。
3008:將機械臂各軸電機速度指令置為零。
3009:通過總線將各關節軸速度指令發送至各智能單元；
3010:將各軸位置及狀態通過串行通訊接口發送給操作人員。
步驟3006， 3007中，冗余度機械臂電機速度反解算法如下 設平面冗余度機械臂的末端位姿由0c，j，a)表示，其中(jc,力表示在運動平面
內機械臂的末端位置，"則表示在運動平面內機械臂的姿態。 x = / 2,".,《)
a = /a(^2,-..,《)
式中，A、 厶分別表示末端位姿和關節運動的函數關系。 則末端速度計算如下
《
=J0
步驟3006，根據最小能量法選擇運動軸為(n-2) n， n為平面冗余度機械臂 的自由度數，第n關節為平面冗余度機械臂最前端關節。則正常狀態下機械臂第 (n—2)到第n關節電機的運動速度反解如下
<formula>formula see original document page 14</formula>第l (n-3)關節電機速度為零。
步驟3007中，設有m個智能單元處于避障狀態，m〈n-2， n為冗余度機械臂 自由度數，不失一般性，設第(n-3)軸和第(n-2)軸處于避障狀態，智能單元的 避障指令為4^和么^，機械臂末端速度指令為A少々，則機械臂第(n-4)軸、第 (n-l)軸、第n軸的速度計算如下
X or人02人
<formula>formula see original document page 15</formula>
(2)
其余關節軸電機速度為零。
式(1)和式(2)中，《(/ = 1，...，")表示平面冗余度機械臂第1關節的位置，
《0' = 1,...，《)表示平面冗余度機械臂第i關節的運動速度。
本發明針對平面冗余度機械臂，根據需要，在各關節臂表面安裝多個紅外距 離傳感器，并針對每個軸電機設置智能單元，該智能單元采集該關節臂安裝的紅 外距離傳感器信息，通過傳感信息測量值將關節臂狀態分為正常、避障和停止狀
態，并啟動不同的運動控制策略(1)當關節臂處于正常狀態時，智能單元采用
微處理器規劃的速度指令控制驅動單元；(2)當關節臂處于避障狀態時，智能單 元采用增大與障礙距離的速度指令驅動控制單元，這是一種直觀的反應式避障策 略，簡單實用，實時性和可靠性高。(3)當機械臂處于停止狀態時，智能單元停 止關節電機運動并將向運動協調模塊發送關節臂狀態。通過本發明提出的自主避 障策略和分布式智能單元，使得冗余度機械臂在執行操作人員末端速度指令的同 時回避環境中的障礙，提高了工作效率及機械臂在障礙環境的操作性能和安全 性，適用于大多數平面冗余度機械臂的避障控制。如在關節臂各個表面均安裝紅 外距離傳感器，還可以適用于空間冗余度機械臂的自主避障。
圖1為本發明遙操作平面冗余度機械臂自主避障系統結構框圖2為遙操作平面冗余度機械臂自主避障智能單元硬件結構框圖； 圖3為智能單元運動控制及避障模塊進行運動控制及避障的流程圖； 圖4為微處理器運動協調模塊進行運動協調的流程圖； 圖5為手爪的紅外距離傳感器陣列示意圖。
具體實施例方式
下面結合附圖對本發明的實施例作詳細說明本實施例在以本發明技術方案 為前提下進行實施，給出了詳細的實施方式和具體的操作過程，但本發明的保護 范圍不限于下述的實施例。
本實施例針對遙操作平面四自由度機械臂自主避障方法和智能單元，對本發 明進行詳細說明。
如圖1所示，本實施例所述遙操作平面冗余度機械臂運動控制與避障系統， 包括系統數據存儲模塊l、微處理器2、運動協調模塊3、串行通訊接口4、現場 總線接口 5以及1一4#智能單元6。其中系統數據存儲模塊l、微處理器2、串 行通訊接口 4、現場總線接口 5安裝在冗余度機械臂上，4個智能單元5安裝在 各關節臂內部。運動協調模塊3運行于微處理器2中；微處理器2與系統數據存 儲模塊1通過I2C總線連接，與串行通訊接口 3通過通用異步收發器(UART)端 口連接，與總線接口通過地址及數據總線連接。
所述系統數據存儲模塊1用于存儲機械臂運動協調控制程序、機械臂系統的 工作參數、遠程通訊地址和通訊協議，其存儲的數據掉電不丟失。本實施例中， 數據存儲模塊2采用電可擦除可編程只讀存儲器EEPR0M。
微處理器2用于綜合遠程操作人員提供的機械臂末端運動指令以及智能單 元返回的狀態信息，通過運動協調模塊運算得到機械臂各軸的運動指令；微處理 器可以是ARM、 DSP、 FPGA等可編程芯片，本實施例中，微處理器2為PHILIPS 公司的LPC2294芯片丄PC2294通過I2C總線與電可擦除可編程只讀存儲器EEPR0M 連接。
運動協調模塊3運行于微處理器2中，其主要功能為根據操作人員發送的機 械臂末端速度指令和各智能單元發送的各關節狀態和速度實施運動規劃，并通過 現場總線發送到各智能單元當所有智能單元都處于正常狀態時，運動協調模塊 根據能量最小原則反解各軸運動速度；當有智能單元處于避障狀態且數目小于等 于1時，通過協調模塊可以通過協調其他智能單元的運動，從而保證機械臂末端 按照操作人員發送的末端速度指令運動的同時實施避障；當有智能單元處于停止 狀態時，表明該關節臂處于碰撞危險狀態，運動協調模塊對各軸智能單元發送運 動停止命令，停止機械臂運動。
串行通訊接口4，主要用于接收遠程的機械臂末端運動指令，并轉換為微處 理器可以處理的數據格式；同時將機械臂各軸的位置、速度及狀態信息發送給遠 程操作人員；串行通訊接口可以是RS232， RS422以及RS485接口，本實施例中 采用RS485通訊接口 ，微處理器2通過其通用異歩收發器(UART)端口與RS485
串行通訊接口 3連接。
現場總線接口5，用于將微處理器計算的各軸運動指令轉化為總線數據格式， 并通過總線通訊協議發送到各個智能單元。本實施例中，現場總線接口 5采用 CAN總線接口 ，微處理器2通過其地址和數據端口與CAN總線接口 5連接。
智能單元6，包括現場總線接口 601，軸運動數據存儲模塊602、數字信號 處理器603、運動控制及避障模塊604、高速脈沖輸入接口 605、模擬量輸出接
口 606、數字量輸出接口 607、數字量輸入接口 608模擬輸入接口 609、驅動單 元610以及上表面紅外距離傳感器陣列611和下表面紅外距離傳感器陣列612。 其中數字信號處理器603通過地址及數據總線與現場總線接口 601連接，用于接 收微處理器傳送的軸運動命令，并反饋軸運動指令及位置、速度及報警等狀態信 息；運動控制與避障模塊604運行數字信號處理器603中，數字信號處理器603 通過數據總線和高速脈沖接口電路605連接，用于接收驅動單元610的電機編碼 器信號，反饋電機的位置、速度信息；數字信號處理器503通過數據端口和模擬 量輸出接口 505連接，用于向驅動單元509的驅動器提供速度指令；數字信號處 理器503通過GPIO (通用輸入輸出)端口和數字量輸入接口 507以及數字量輸 出接口 506連接，數字量輸入接口 507用于接入原點、限位以及驅動器狀態；數 字量輸出接口 506用于發送驅動器控制信號；數字信號處理器503通過數據端口 與模擬量輸入接口 508連接，用于采集上表面紅外距離傳感器陣列510和下表面 紅外距離傳感器陣列511感知的與環境的距離信息。模擬量輸入接口 508包括信 號放大、光電隔離、濾波及A/D轉換，由常規的數字電路和電子元器件組成。
本實施例中，數字信號處理器503為TI公司的TMS320LF2407A芯片，通過 現場總線接口與微處理器通訊，接收上位控制器的控制指令，并反饋電機的位置、 速度以及關節臂正常、避障、停止等狀態信息。并通過擴展端口與高速脈沖輸入 接口 504、數字量輸入接口 507、數字量輸出接口 506、模擬量輸出接口 505以 及模擬量輸入端口 508連接。
運動協調模塊3運行于微處理器2上，其主要功能為根據操作人員發送的機 械臂末端速度指令和各智能單元發送的各關節狀態和速度實施運動規劃，并通過 現場總線接口 5發送到1一4#智能單元6:當所有智能單元都處于正常狀態時， 運動協調模塊根據能量最小原則反解各軸運動速度；當1一4#智能單元中有1個處于避障狀態時，通過運動協調模塊可以通過協調其他智能單元的運動，從而 保證機械臂末端按照操作人員發送的末端速度指令運動的同時實施避障；當有智 能單元處于停止狀態時，表明該關節臂處于碰撞危險狀態，運動協調模塊3通過 現場總線接口 5對1一4#智能單元發送運動停止命令，停止機械臂運動。
本實施例中驅動單元610采用日本松下MINAS—A4系列交流伺服電機和驅動 器。其中電機安裝在冗余度機械臂的驅動關節處，用于驅動冗余度機械臂的關節 運動，并附帶驅動器和編碼器。編碼器通過高速脈沖輸入接口 605和數字信號處 理器603連接，提供任意時刻電機的位置和速度信息驅動器通過模擬量輸出接 口 605和數字信號處理器603連接，接收電機運動的速度指令；同時通過數字量 輸出接口 607接收數字信號處理器603的伺服使能、報警清除等信息，機械臂各 關節的限位、原點和驅動器狀態信息如報警等通過數字量輸入接口 608與數字信 號處理器603連接并反饋電機的狀態信息。
軸運動數據存儲模塊602，通過I2C總線與數字信號處理器503連接，用于 存儲關節軸工作參數和避障策略。本實施例中，數據存儲模塊602采用電可擦除 可編程只讀存儲器EEPROM。
上表面紅外距離傳感器陣列611和下表面紅外距離傳感器陣列612，分別由 安裝在平面冗余度機械臂各關節臂的上下表面的一組紅外距離傳感器組成，感應 方向垂直于關節臂外表面。本實施例中采用日本Sharp公司的GP2D12紅外距離 傳感器，感應范圍10cm—80cm，輸出形式為模擬電壓量。
本實施例中對象為平面4自由度機械臂，冗余度為1。在第3、 4關節安裝 了紅外距離傳感器陣歹U。其中第4關節即手爪的紅外距離傳感器陣列如圖5所示。 其中第四關節上表面和下表面各安裝有一個紅外距離傳感器，該關節臂通過俯仰 運動進行避障。
在第4關節還在左右側面安裝有4個紅外距離傳感器，該紅外距離傳感器也 接入智能單元，但只起防止碰撞的作用。
如圖3所示，本實施例的智能單元中設置閾值1為20cm，閾值2為llcm。 本實施例遙操作平面冗余度機械臂自主避障方法具體如下 運動控制與避障流程如下
60401:讀取微處理器通過總線發送的軸運動速度指令；
60402:讀取關節臂上的紅外距離傳感器陣列的輸出。該值是經過模數轉換 后的整型數。
60403:若有任一紅外距離傳感器的值〈碰撞預警閾值2，表明機械臂處于危 險區域內，需要立刻使系統停止，處理內容轉60409;否則繼續。
60404:若上表面紅外距離傳感器陣列中有任一傳感器的值〈碰撞預警閾值1， 則表明機械臂處于避障區域，以下轉60406,否則轉60405。
60405:若下表面紅外距離傳感器陣列中有任一傳感器的值〈碰撞預警閾值1 ， 則表明機械臂處于避障區域，以下轉60407，否則轉60408。
60406:令驅動關節軸電機反向勻速運動進行避障。(設機械臂向上運動為正 向)，同時設關節軸狀態為"避障"，以下轉60410;
60407:令驅動關節軸電機正向勻速運動進行避障，同時設關節軸狀態為"避 障"。以下轉60410;
60408:若所有紅外距離傳感器輸出均大于碰撞預警閾值1，表明機械臂行 進過程無障礙，關節軸以規劃速度運動，同時設關節軸狀態為"正常"。以下轉 60410;
60409:令關節軸電機停止運動，同時設關節軸狀態為"停止"；
60410:將關節軸運動輸出到驅動單元，同時將電機位置、速度和狀態信息
發送到微處理器。
運動協調流程如下
3001:讀取串行通訊接口接收的機械臂末端速度指令。該指令由操作人員遠 程發送；
3002:讀取智能單元發送的各關節軸位置、狀態及控制指令。
3003:如果有關節軸處于"停止"狀態，則表明有關節臂有碰撞危險，已經 停止運動，以下轉3009，否則轉3004。
3004:如果有關節軸處于"避障"狀態，則表明有關節智能單元正在執行避 障指令，需要重新規劃各關節軸電機的運動；以下轉3005，否則表示沒有智能 單元沒有檢測到障礙，以下轉3006。
3005:判斷處于"避障"狀態的關節軸的數目是否小于等于l，如果"是"， 這表明可以實施避障運動規劃，以下轉3007;否則表明無法協調各關節軸運動
同時滿足機械臂末端運動指令和避障運動指令，轉3008。
3006:按照操作人員的末端速度指令反解得到各軸速度指令；
通過步驟3005，可通過反解計算得到正常狀態下機械臂各關節軸電機的規
劃速度；
3007:根據各關節的位置和智能單元的避障運動速度以及操作人員發送的機 械臂末端速度指令，重新反解機械臂各軸速度指令；
通過步驟3007，微處理器可以協調各智能單元的避障策略，通過反解計算 機械臂其他各關節軸電機的規劃速度，從而保證機械臂末端在執行操作人員速度
指令的同時實現關節臂避障運動。
3008:將機械臂各軸電機速度指令置為零。 3009:通過總線將各關節軸速度指令發送至各智能單元； 3010:將各軸位置及狀態通過串行通訊接口發送給操作人員； 步驟300， 3007中，冗余度機械臂各軸電機速度反解算法如下
設平面4自由度冗余度機械臂的末端位姿由O，少,")表示，其中(x,力表示在 運動平面內機械臂的末端位置，"則表示在運動平面內機械臂的姿態。
"=厶的^2,《，6>4)
式中，,、/ 分別表示末端位姿和關節運動的函數關系。 則末端速度計算如下
<formula>formula see original document page 20</formula>
步驟2006，根據最小能量法選擇運動軸為2 4，第4關節為平面冗余度機 械臂最前端關節。則正常狀態下機械臂第2到第4關節電機的運動速度反解如下
<formula>formula see original document page 21</formula>(2)
其余關節軸電機速度為零。
式(1)和式(2)中，《(/ = 1,...^)表示平面冗余度機械臂第1關節的位置， 《(/ = 1，...,《)表示平面冗余度機械臂第i關節的運動速度。
權利要求
1、一種遙操作平面冗余度機械臂自主避障智能單元，其特征在于，包括現場總線接口、軸運動數據存儲模塊、數字信號處理器、運動控制與避障模塊、高速脈沖輸入接口、模擬量輸出接口、數字量輸出接口、數字量輸入接口、模擬輸入接口、驅動單元、上表面紅外距離傳感器陣列、下表面紅外距離傳感器陣列，其中數字信號處理器通過地址及數據總線與現場總線接口連接，用于接收微處理器傳送的軸運動命令，并反饋軸運動指令及位置、速度及報警狀態信息；數字信號處理器通過數據總線和高速脈沖輸入接口電路連接，用于接收電機的編碼器信號，反饋電機的位置、速度信息；數字信號處理器通過數據端口和模擬量輸出接口連接，用于向驅動器提供速度指令；數字信號處理器通過GPIO端口和數字量輸入接口以及數字量輸出接口連接，數字量輸入接口用于接入原點、限位以及驅動器狀態，數字量輸出接口用于發送驅動器控制信號；數字信號處理器通過數據端口與模擬量輸入接口連接，用于采集紅外距離傳感器感知的與環境的距離信息，模擬量輸入接口實現信號放大、光電隔離、濾波及A/D轉換；所述的軸運動數據存儲模塊，通過I2C總線與數字信號處理器連接，用于存儲關節軸工作參數和避障策略；所述的驅動單元，用于驅動冗余度機械臂的關節運動，提供任意時刻電機的位置和速度信息，并接收數字信號處理器電機運動的速度指令；所述的上、下表面紅外距離傳感器陣列，感應方向垂直于關節臂外表面，紅外距離傳感器通過紅外發送器以及位置感應器件獲取與環境的距離，并以模擬量的形式體現，并通過模擬量輸入接口接入數字信號處理器；所述運動控制及避障模塊運行于數字信號處理器中，用于綜合避障系統微處理器的軸運動命令和避障策略，運動控制及避障模塊計算軸的運動速度，并根據軸運動數據存儲模塊中設置的碰撞預警閾值將關節軸分為三種運動狀態，正常狀態、避障狀態以及停止狀態，同時還將計算的軸運動速度通過數字信號處理器的模擬量輸出端口發送到驅動單元的驅動器，并通過數字信號處理器的高速脈沖接口反饋電機轉速，構成速度閉環，從而實現軸的運動。
2、 根據權利要求1所述的遙操作平面冗余度機械臂自主避障智能單元，其 特征是，所述運動控制及避障模塊計算軸的運動速度，軸運動首先在軸運動數據 存儲模塊中設置碰撞預警閾值1和碰撞預警閾值2,閾值1〉閾值2，并存儲于軸 運動數據存儲模塊中，運動控制及避障模塊實時采集該關節臂上表面紅外距離傳 感器陣列和下表面紅外距離傳感器陣列，并根據紅外距離傳感器陣列的輸出值，將關節軸分為三種運動狀態正常狀態此時該關節臂上、下表面紅外距離傳感器陣列中全部紅外距離傳 感器的測量值均大于碰撞預警閾值l;表明沒有碰撞危險，智能單元執行微處理 器運動協調模塊通過總線發送的規劃運動；避障狀態此時上、下表面紅外距離傳感器陣列中至少有一個紅外距離傳感 器測量值在閾值1和閾值2之間，智能單元將根據存儲在軸運動數據存儲模塊中 的避障策略執行避障運動；停止狀態此時上、下表面紅外距離傳感器陣列中的有至少一個紅外距離傳 感器測量值小于碰撞預警閾值2，表明該關節臂有碰撞危險，智能單元將停止軸 運動，并將狀態通過總線發送到微處理器的運動協調模塊進行處理。
3、 根據權利要求1或2所述的遙操作平面冗余度機械臂自主避障智能單元， 其特征是，所述的上、下表面紅外距離傳感器陣列，分別由安裝在平面冗余度機 械臂各關節臂的上下表面的一組紅外距離傳感器組成，每組紅外距離傳感器的數 目在1到8之間。
4、 根據權利要求1所述的遙操作平面冗余度機械臂自主避障智能單元，其 特征是，所述的驅動單元，包括安裝在冗余度機械臂的驅動關節處的電機，用于 驅動冗余度機械臂的關節運動，并附帶編碼器和驅動器，編碼器通過高速脈沖輸 入接口和數字信號處理器連接，提供任意時刻電機的位置和速度信息；驅動器通 過模擬量輸入接口和數字信號處理器連接，接收電機運動的速度指令，同時通過 數字量輸入接口接收數字信號處理器的伺服使能開關指令，并反饋電機的狀態信 息。
5、 一種遙操作平面冗余度機械臂自主避障智能系統，其特征在于，包括系 統數據存儲模塊、微處理器、串行通訊接口、現場總線接口以及智能單元l n， n為平面冗余度機械臂關節數，其中數據存儲模塊、微處理器、串行通訊接口、 現場總線接口安裝在冗余度機械臂上，智能單元1 n安裝在各關節臂內部，所述系統數據存儲模塊用于存儲機械臂運動控制程序、機械臂系統的工作參 數、遠程通訊地址和通訊協議，其存儲的數據掉電不丟失，數據存儲模塊與微處 理器通過I2C總線連接；所述的微處理器用于綜合遠程操作人員提供的機械臂末端運動指令以及智 能單元返回的狀態信息，通過運動協調模塊運算得到機械臂各軸的運動指令；所述運動協調模塊運行于微處理器中，根據操作人員發送的機械臂末端速度 指令和各智能單元發送的各關節狀態和速度實施運動規劃，并通過現場總線發送到各智能單元；所述的串行通訊接口，用于接收遠程的機械臂末端運動指令，并轉換為微處 理器能處理的數據格式，同時將機械臂各軸的位置、速度及狀態信息發送給遠程 操作人員，微處理器通過其通用異步收發器端口與串行通訊接口連接；所述的現場總線接口，用于將微處理器計算的各軸運動指令轉化為總線數據 格式，并通過總線通訊協議發送到各個智能單元，包括總線收發器、光電隔離、 電平轉換及濾波電路，微處理器通過地址及數據總線與總線接口連接；所述的智能單元，安裝于各關節臂的內部，用于綜合微處理器的軸運動命令 和紅外距離傳感器感知的信號，通過運動控制與避障模塊運算得到該軸的運動指 令，并控制驅動單元完成軸的驅動，智能單元通過總線接口與總線連接。
6、 根據權利要求5所述的遙操作平面冗余度機械臂自主避障智能系統，其 特征是，當所有智能單元都處于正常狀態時，所述運動協調模塊根據能量最小原 則反解各軸運動速度；當有智能單元處于避障狀態且數目小于等于冗余自由度 時，通過運動協調模塊通過協調其他智能單元的運動，從而保證機械臂末端按照 操作人員發送的末端速度指令運動的同時實施避障；當有智能單元處于停止狀態 時，表明該關節臂處于碰撞危險狀態，運動協調模塊對各軸智能單元發送運動停 止命令，停止機械臂運動。
7、 一種遙操作平面冗余度機械臂自主避障方法，其特征在于，包括運動控 制及避障模塊進行的運動控制及避障，以及運動協調模塊進行的運動協調兩部 分，其中所述的運動控制及避障模塊進行的運動控制及避障包括如下步驟 60401:讀取微處理器通過總線發送的軸運動速度指令；60402:讀取關節臂上的紅外距離傳感器陣列的輸出，該值是經過模數轉換 后的整型數；60403:若有任一紅外距離傳感器的值〈碰撞預警閾值2，表明機械臂處于危 險區域內，需要立刻使系統停止，處理內容轉60409;否則繼續；60404:若上表面紅外距離傳感器陣列中有任一傳感器的值〈碰撞預警閾值1 ， 則表明機械臂處于避障區域，以下轉60406，否則轉60405;60405:若下表面紅外距離傳感器陣列中有任一傳感器的值〈碰撞預警閾值1， 則表明機械臂處于避障區域，以下轉60407，否則轉60408;60406:令驅動關節軸電機反向勻速運動進行避障，設機械臂向上運動為正 向，同時設關節軸狀態為"避障"，以下轉60410;60407:令驅動關節軸電機正向勻速運動進行避障，同時設關節軸狀態為"避 障"，以下轉60410;60408:若所有紅外距離傳感器輸出均大于碰撞預警閾值1，表明機械臂行 進過程無障礙，關節軸以規劃速度運動，同時設關節軸狀態為"正常"，以下轉 60410;60409:令關節軸電機停止運動，同時設關節軸狀態為"停止"； 60410:將關節軸運動輸出到驅動單元，同時將電機位置、速度和狀態信息 發送到微處理器；所述運動協調模塊進行的運動協調包括如下步驟3001:讀取串行通訊接口接收的機械臂末端速度指令，該指令由操作人員遠 程發送；3002:讀取智能單元發送的各關節軸位置、狀態及控制指令； 3003:如果有關節軸處于"停止"狀態，則表明有關節臂有碰撞危險，已經 停止運動，以下轉3009，否則轉3004;3004:如果有關節軸處于"避障"狀態，則表明有關節智能單元正在執行避 障指令，需要重新規劃各關節軸電機的運動；以下轉3005，否則表示沒有智能單元沒有檢測到障礙，以下轉3006;3005:判斷處于"避障"狀態的關節軸的數目是否小于等于冗余自由度數，如果"是"，這表明可實施避障運動規劃，以下轉3007;否則表明無法協調各關節軸運動同時滿足機械臂末端運動指令和避障運動指令，轉3008; 3006:按照操作人員的末端速度指令反解得到各軸速度指令； 步驟3005能通過反解計算得到正常狀態下機械臂各關節軸電機的規劃速度；3007:根據各關節的位置和智能單元的避障運動速度以及操作人員發送的機械臂末端速度指令，重新反解機械臂各軸速度指令；通過步驟3007，微處理器協調各智能單元的避障策略，通過反解計算機械臂其他各關節軸電機的規劃速度，從而保證機械臂末端在執行操作人員速度指令的同時實現關節臂避障運動；3008:將機械臂各軸電機速度指令置為零；3009:通過總線將各關節軸速度指令發送至各智能單元；3010:將各軸位置及狀態通過串行通訊接口發送給操作人員。
8、如權利要求7所述的遙操作平面冗余度機械臂自主避障方法，其特征是， 步驟3006， 3007中，冗余度機械臂電機速度反解算法如下設平面冗余度機械臂的末端位姿由(x，少,cO表示，其中(x,力表示在運動平面內機械臂的末端位置，a則表示在運動平面內機械臂的姿態，<formula>formula see original document page 6</formula>式中，/;、 厶分別表示末端位姿和關節運動的函數關系，則末端速度計算如下<formula>formula see original document page 6</formula>步驟3006，根據最小能量法選擇運動軸為n-2 n， n為平面冗余度機械臂 的自由度數，第n關節為平面冗余度機械臂最前端關節，則正常狀態下機械臂第 n—2到第n關節電機的運動速度反解如下<formula>formula see original document page 7</formula>第1 n-3關節電機速度為零；步驟3007中，設有m個智能單元處于避障狀態，m<n-2， n為冗余度機械臂 自由度數，設第n-3軸和第n-2軸處于避障狀態，智能單元的避障指令為么—3和4_2，機械臂末端速度指令為i，少,&，則機械臂第n-4軸、第n-l軸、第n軸的 速度計算如下<formula>formula see original document page 7</formula>其余關節軸電機速度為零；以上兩個公式中，《(/ = 1,...,")表示平面冗余度機械臂第i關節的位置， 總(/ = 1，...,")表示平面冗余度機械臂第i關節的運動速度。全文摘要
一種機械控制技術領域的遙操作平面冗余度機械臂自主避障智能單元、系統及方法。所述系統包括數據存儲模塊、微處理器、串行通訊接口、現場總線接口以及智能單元，智能單元針對每個軸電機設置，該智能單元采集該關節臂安裝的紅外距離傳感器信息，通過傳感信息測量值將關節臂狀態分為正常、避障和停止狀態關節臂處于正常狀態時，智能單元采用微處理器規劃的速度指令控制驅動單元；關節臂處于避障狀態時，智能單元采用增大與障礙距離的速度指令驅動控制單元；機械臂處于停止狀態時，智能單元停止關節電機運動并將向運動協調模塊發送關節臂狀態。本發明可簡化系統接線，使得當通過遙操作控制機械臂末端趨緊目標的同時可快速感知障礙并實現避障。
文檔編號B25J9/16GK101352854SQ20081004069
公開日2009年1月28日 申請日期2008年7月17日 優先權日2008年7月17日
發明者張偉軍, 李建中, 李建華, 胡傳平, 袁建軍 申請人:上海交通大學