機(jī)器人素描程序說明

1、一、 人臉?biāo)孛鑼?shí)驗(yàn)流程框圖：開始新循環(huán)拍攝一張人臉照片Canny算子邊緣檢測步驟：用高斯濾波器平滑圖像；用一階偏導(dǎo)的有限差分來計(jì)算梯度的幅值和方向；對梯度幅值進(jìn)行非極大值抑制；用雙閾值算法檢測和連接邊緣。選擇處理精細(xì)模式：詳盡、一般、簡略彩色圖轉(zhuǎn)為256階灰度256色灰度值 0.30*紅色分量值+ 0.587*綠色分量值+ 0.114*藍(lán)色分量值圖像細(xì)化先進(jìn)行水平方向細(xì)化，再進(jìn)行垂直方向細(xì)化。去除離散噪聲：如果孤立像素塊像素點(diǎn)小于一定閾值，則認(rèn)為是離散噪聲點(diǎn)，需要去除。輪廓跟蹤二、實(shí)驗(yàn)二 基礎(chǔ)運(yùn)動(dòng)控制實(shí)驗(yàn)：2.1 機(jī)器人作業(yè)空間、繪圖空間、軌跡圖像坐標(biāo)映射機(jī)器人機(jī)械機(jī)構(gòu)由兩個(gè)臂組成，大臂有效長

2、度200毫米，小臂有效長度（旋轉(zhuǎn)中心到繪圖筆中心距離）為150毫米。大臂運(yùn)動(dòng)范圍±100度，小臂運(yùn)動(dòng)范圍±55度。機(jī)器人作業(yè)空間：機(jī)器人末端點(diǎn)可以到達(dá)的區(qū)域，如下圖所示（近似為一個(gè)扇環(huán)）：機(jī)器人繪圖空間：二自由度繪圖機(jī)械臂配備了一個(gè)長方形繪圖平臺，在繪圖平臺上可以鋪上白紙，平臺的尺寸是280mm*240mm，結(jié)合機(jī)器人的環(huán)形的作業(yè)空間，我們?nèi)…h(huán)形工作空間和 繪圖平臺 交集 中的一個(gè)最大繪圖矩形作為實(shí)際人臉?biāo)孛枥L圖空間，大小取為147mm*110mm(如此選擇主要是滿足長寬比例與圖像風(fēng)辨率320*240一致)，該矩形中心點(diǎn)在機(jī)器人坐標(biāo)系中的坐標(biāo)為（0，275），如下圖所示：軌

3、跡圖像坐標(biāo)映射：人臉圖像在處理時(shí)縮放為320×240像素分辨率，在Window系統(tǒng)中繪圖坐標(biāo)系的定義為：圖像左上角為原點(diǎn)，X方向從左到右為正向，Y方向從上到下為正向。繪圖空間坐標(biāo)系定義：繪圖空間為在機(jī)器人坐標(biāo)系中中心點(diǎn)坐標(biāo)為（0，275）的大小為147×110mm的矩形，其中的坐標(biāo)系定義為：147×110繪圖矩形中心點(diǎn)為原點(diǎn)，X方向從左到右，Y方向從下到上。素描圖像處理出來后得到的筆跡曲線在軌跡圖像坐標(biāo)系中表示，大小為320×240，如果需要將筆跡曲線繪制在繪圖空間中，需要做坐標(biāo)轉(zhuǎn)化，從幾個(gè)坐標(biāo)系間的相對關(guān)系可以推導(dǎo)得到：如果 在320×240

4、 圖像坐標(biāo)系 中的 一個(gè)點(diǎn) （x0,y0）映射到 機(jī)器人繪圖空間的對應(yīng)坐標(biāo)(x1,y1): X1=x0/2.1818-147/2 Y1=110/2-y0/2.1818+275其中的縮放系數(shù)2.1818 為 將320×240的圖像縮小到147×110繪圖空間的比例系數(shù)：240/110=2.18182.2 二自由度機(jī)械臂的運(yùn)動(dòng)學(xué)解：機(jī)械臂通常具有兩種運(yùn)動(dòng)方式：關(guān)節(jié)空間運(yùn)動(dòng)和直角坐標(biāo)運(yùn)動(dòng)。關(guān)節(jié)空間運(yùn)動(dòng)模式是指機(jī)器人操作臂的運(yùn)動(dòng)直接由各個(gè)關(guān)節(jié)的運(yùn)動(dòng)坐標(biāo)來確定，所有關(guān)節(jié)變量構(gòu)成一個(gè)關(guān)節(jié)矢量。所有關(guān)節(jié)矢量構(gòu)成的空間稱為關(guān)節(jié)空間。所謂關(guān)節(jié)空間運(yùn)動(dòng)模式就是直接操作各個(gè)關(guān)節(jié)的運(yùn)動(dòng)來完成機(jī)器人

5、的運(yùn)動(dòng)。下圖 a)為二自由度機(jī)械臂的關(guān)節(jié)坐標(biāo)空間定義，關(guān)節(jié)坐標(biāo)空間由兩個(gè)關(guān)節(jié)組成。直角坐標(biāo)運(yùn)動(dòng)模式是指機(jī)器人末端工具的位置和方位通常是在直角坐標(biāo)空間中描述。直角坐標(biāo)空間運(yùn)動(dòng)模式通過指定機(jī)器人末端工具在直角坐標(biāo)空間中的運(yùn)動(dòng)來完成機(jī)器人操作任務(wù)。下圖b)為二自由度機(jī)械臂直角坐標(biāo)空間的定義，這是一個(gè)二維的直角坐標(biāo)空間。 a)關(guān)節(jié)坐標(biāo)空間 b)直角坐標(biāo)空間實(shí)際運(yùn)行是通過對關(guān)節(jié)運(yùn)動(dòng)軸的伺服控制來實(shí)現(xiàn)的，也就是最終的機(jī)械臂的控制是在關(guān)節(jié)坐標(biāo)空間中進(jìn)行的，但是對于操作者來說，直角坐標(biāo)空間更容易讓人理解和接受，操作者對機(jī)械臂的操作一般是在直角坐標(biāo)空間中進(jìn)行的，這樣就需要建立一種關(guān)節(jié)坐標(biāo)空間和直角坐標(biāo)空間的對應(yīng)

6、關(guān)系，也就是說如果已知機(jī)械臂各個(gè)關(guān)節(jié)的坐標(biāo)參數(shù)需要求解機(jī)械手末端在直角坐標(biāo)空間中的坐標(biāo)，反過來如果已知機(jī)械手末端在直角坐標(biāo)空間的坐標(biāo)需要求解各個(gè)運(yùn)動(dòng)關(guān)節(jié)的坐標(biāo)參數(shù)，前一個(gè)問題稱為機(jī)器人的運(yùn)動(dòng)學(xué)正解，后一個(gè)問題稱為機(jī)器人的運(yùn)動(dòng)學(xué)反解。從數(shù)學(xué)的角度定義機(jī)器人的運(yùn)動(dòng)學(xué)正解和反解如下：設(shè)機(jī)器人的關(guān)節(jié)坐標(biāo)空間中的關(guān)節(jié)變量記為，機(jī)器人末端工具在直角坐標(biāo)空間中的坐標(biāo)記為，關(guān)節(jié)變量和直角坐標(biāo)空間坐標(biāo)間存在如下運(yùn)動(dòng)學(xué)約束： 這是一個(gè)隱式方程，如果能夠從上式式求解出： 即由關(guān)節(jié)坐標(biāo)變量表示的直角坐標(biāo)變量，這就是機(jī)器人的運(yùn)動(dòng)學(xué)正解，一般的可以得到機(jī)器人運(yùn)動(dòng)學(xué)的唯一正解。如果能夠從4-1式求解出： 也就是已知機(jī)器人末

7、端工具的直角坐標(biāo)參數(shù)，求解出對應(yīng)關(guān)節(jié)坐標(biāo)空間中的各個(gè)關(guān)節(jié)變量，這就是運(yùn)動(dòng)學(xué)的反解，通常難以得到解析的運(yùn)動(dòng)學(xué)反解，而且運(yùn)動(dòng)學(xué)反解一般也是不唯一的，實(shí)際應(yīng)用中常采用幾何法和數(shù)值解法來求解。下面我們來推導(dǎo)二自由度機(jī)械臂的運(yùn)動(dòng)學(xué)解：1） 二自由度機(jī)械臂運(yùn)動(dòng)學(xué)正解已知：關(guān)節(jié)1連桿長度，關(guān)節(jié)變量（關(guān)節(jié)控制軸1角度位置），關(guān)節(jié)2連桿長度，關(guān)節(jié)變量（關(guān)節(jié)控制軸2角度位置）。求解：關(guān)節(jié)連桿末端工具安裝點(diǎn)在直角坐標(biāo)空間的坐標(biāo)。二自由度機(jī)械臂運(yùn)動(dòng)學(xué)正解上圖為二自由度機(jī)械臂的運(yùn)動(dòng)學(xué)正解的示意圖，從圖上可以得到機(jī)械臂末端的直角坐標(biāo)： 2） 二自由度機(jī)械臂運(yùn)動(dòng)學(xué)反解 已知：關(guān)節(jié)1連桿長度，關(guān)節(jié)2連桿長度， 關(guān)節(jié)連桿末端工

8、具安裝點(diǎn)在直角坐標(biāo)空間的坐標(biāo), 求解：關(guān)節(jié)變量（關(guān)節(jié)控制軸1角度位置）， 關(guān)節(jié)變量（關(guān)節(jié)控制軸2角度位置）。下圖為求解二自由度機(jī)械臂運(yùn)動(dòng)學(xué)反解的示意圖，如果不考慮奇異點(diǎn)則有如下推導(dǎo)： 得到運(yùn)動(dòng)學(xué)反解的計(jì)算公式： 其中atan2(x ,y)為計(jì)算的反正切值（單位為弧度）的數(shù)學(xué)函數(shù)，利用雙變量函數(shù) atan2(x ,y)計(jì)算的優(yōu)點(diǎn)在于利用了x和y的符號能夠確定角度所在的象限，例如，而，如果使用單變量的反正切函數(shù)則不能區(qū)分這兩種情況，atan2(x ,y)的值域?yàn)?，?dāng)雙變量反正切函數(shù)的兩個(gè)參數(shù)都為零時(shí)，函數(shù)值不定。二自由度機(jī)械臂運(yùn)動(dòng)學(xué)反解3）運(yùn)動(dòng)學(xué)反解的不唯一性機(jī)器人運(yùn)動(dòng)學(xué)反解一般是不唯一的，也就是

9、可能存在多組關(guān)節(jié)坐標(biāo)變量值使得機(jī)械手末端達(dá)到給定的目標(biāo)位置。二自由度機(jī)械手在工作空間中運(yùn)動(dòng)時(shí)存在兩組運(yùn)動(dòng)學(xué)反解，如下所示。圖 二自由度機(jī)械臂的兩組反解實(shí)際運(yùn)行中如何從機(jī)器人運(yùn)動(dòng)學(xué)的多組反解中選擇一組合適的解是一個(gè)值得研究的問題，一般的具有多組反解的機(jī)器人運(yùn)動(dòng)學(xué)問題可以采用優(yōu)化算法來解決，定義一定的優(yōu)化準(zhǔn)則，選擇滿足優(yōu)化準(zhǔn)則的最優(yōu)解作為機(jī)器人運(yùn)動(dòng)學(xué)的確定反解。優(yōu)化準(zhǔn)則的選取分兩種情況：一種是不存在障礙物時(shí)可以使用“最短行程”準(zhǔn)則，因?yàn)闄C(jī)器人的運(yùn)動(dòng)是連續(xù)的，結(jié)合前一次機(jī)器人運(yùn)動(dòng)學(xué)反解得到的關(guān)節(jié)坐標(biāo)位置，選取使機(jī)器人關(guān)節(jié)運(yùn)動(dòng)量最小的一組解作為反解，這種選取方法可以使得機(jī)器人運(yùn)動(dòng)保持連續(xù)；如果存在障礙

10、物，沿“最短行程”運(yùn)動(dòng)可能導(dǎo)致碰撞，這時(shí)應(yīng)該結(jié)合機(jī)器人的有障礙路徑規(guī)劃和智能決策等方法來選取反解。4）運(yùn)動(dòng)學(xué)反解中的特殊情況² 特殊情況1：如下圖所示。圖 運(yùn)動(dòng)學(xué)反解特殊情況1在這時(shí)目標(biāo)位置點(diǎn)(x, y)超出了機(jī)械臂運(yùn)動(dòng)空間范圍，計(jì)算機(jī)數(shù)值計(jì)算的判別條件為： 如果滿足： 則說明給定的目標(biāo)位置超出了機(jī)械臂的運(yùn)動(dòng)空間范圍，運(yùn)動(dòng)學(xué)反解不存在。其中為一個(gè)正小數(shù)，比如可以選?。?² 特殊情況2：如下圖所示。圖 運(yùn)動(dòng)學(xué)反解特殊情況2這時(shí)目標(biāo)位置剛好在機(jī)械臂工作空間邊界上，也就是目標(biāo)位置點(diǎn)處于奇異位置，在奇異點(diǎn)位置附近機(jī)械臂的運(yùn)動(dòng)特性比較差，控制軸運(yùn)動(dòng)可能會(huì)產(chǎn)生跳變，一般實(shí)驗(yàn)時(shí)應(yīng)該避開這

11、些位置。對于這種奇異情況計(jì)算機(jī)數(shù)值計(jì)算的判別條件為： 這種情況下機(jī)械臂運(yùn)動(dòng)學(xué)反解為:2.3 電機(jī)脈沖當(dāng)量 機(jī)器人大臂驅(qū)動(dòng)電機(jī)為100W松下交流伺服電機(jī)，小臂驅(qū)動(dòng)電機(jī)為50W松下交流伺服電機(jī)，電機(jī)后面都配備了光電編碼器，光電編碼器的分辨率為2500脈沖/轉(zhuǎn)，光電編碼器的信號通過運(yùn)動(dòng)控制器中處理電路做了4倍頻處理，也就是說最終在使用運(yùn)動(dòng)控制器時(shí)我們得到的電機(jī)角度分辨率為10000脈沖/轉(zhuǎn)。機(jī)器人關(guān)節(jié)電機(jī)在驅(qū)動(dòng)關(guān)節(jié)時(shí)還連接了諧波減速器，大臂減速比100，小臂減速比99. 如果需要大臂轉(zhuǎn)動(dòng)1度，需要下發(fā)脈沖數(shù)為：10000脈沖/轉(zhuǎn) × 100 / 360 度/轉(zhuǎn) = 2777.7778 脈沖

12、/度； 如果需要小臂轉(zhuǎn)動(dòng) 1 度， 需要下發(fā)脈沖數(shù)為： 10000 脈沖/轉(zhuǎn) × 99 /360度/轉(zhuǎn) =2750 脈沖/度。2.4 素描筆跡繪制策略： 圖像處理后得到的素描軌跡為一個(gè)筆畫一個(gè)筆畫存儲，在筆畫的起點(diǎn)需要落下繪圖筆，在筆畫的終點(diǎn)需要抬起繪圖筆，一個(gè)筆畫的終點(diǎn)到下一個(gè)筆畫的起點(diǎn)間為抬臂快速移動(dòng)，不需要做插補(bǔ)運(yùn)算（可以提高設(shè)備運(yùn)行速度），筆畫由一系列坐標(biāo)點(diǎn)組成，筆畫中的繪制策略為：取一個(gè)筆畫坐標(biāo)點(diǎn)，轉(zhuǎn)化到機(jī)器人直角坐標(biāo)系，通過運(yùn)動(dòng)學(xué)反解運(yùn)算得到兩個(gè)機(jī)器人關(guān)節(jié)的角度，通過角度和電機(jī)脈沖數(shù)的變化關(guān)系轉(zhuǎn)化為電機(jī)目標(biāo)脈沖數(shù)，下發(fā)給運(yùn)動(dòng)控制器，運(yùn)動(dòng)控制器協(xié)調(diào)電機(jī)驅(qū)動(dòng)器控制關(guān)節(jié)電機(jī)運(yùn)動(dòng)

13、到目標(biāo)位置，此時(shí)繪圖比落下也就在繪圖板白紙上繪制出筆跡，連續(xù)移動(dòng)則繪制出一條連續(xù)筆畫。三、 實(shí)驗(yàn)三 機(jī)器人實(shí)時(shí)控制： 現(xiàn)有網(wǎng)絡(luò)結(jié)構(gòu) 設(shè)計(jì)為 主從結(jié)構(gòu)（服務(wù)器/客戶機(jī)結(jié)構(gòu)），在實(shí)驗(yàn)系統(tǒng)中一共有4套二自由度機(jī)械臂，每套機(jī)械臂具有兩個(gè)運(yùn)動(dòng)關(guān)節(jié)（電機(jī)），總共8個(gè)運(yùn)動(dòng)控制軸，使用一套8軸Galil嵌入式網(wǎng)絡(luò)運(yùn)動(dòng)控制器來統(tǒng)一控制4套機(jī)器人的8個(gè)運(yùn)動(dòng)軸。設(shè)置一臺計(jì)算機(jī)為服務(wù)器（教師監(jiān)督計(jì)算機(jī)），該服務(wù)器處理所有與運(yùn)動(dòng)控制相關(guān)細(xì)節(jié)，也就是說只有服務(wù)器才能和運(yùn)動(dòng)控制器通訊連接直接控制運(yùn)動(dòng)控制器和機(jī)器人。網(wǎng)絡(luò)組織結(jié)構(gòu)中有一臺16口網(wǎng)絡(luò)交換機(jī)，8軸運(yùn)動(dòng)控制器具有192.168.0.101 IP 地址 連接到交換機(jī)的

14、一個(gè)端口，服務(wù)器（教師機(jī)）需要設(shè)置一個(gè)在 192.168.0.* 的 唯一IP地址 供 客戶機(jī) 通過指定 IP地址訪問。8套客戶機(jī)（學(xué)生機(jī)）也指定唯一的在192.168.0.*網(wǎng)段的 IP地址，比如從 192.168.0.110到192.168.0.117，客戶機(jī)的所有操作和請求均先發(fā)送到服務(wù)器，通過服務(wù)器解釋后，合法并授權(quán)的命令將被服務(wù)器解釋執(zhí)行并發(fā)送到運(yùn)動(dòng)控制器，通過服務(wù)器和客戶機(jī)兩個(gè)層次的設(shè)置確保了多套客戶機(jī)對4臺機(jī)器人的分時(shí)共享及其客戶權(quán)限的設(shè)置。在客戶機(jī)上運(yùn)行客戶端程序，對客戶機(jī)端的要求做前端預(yù)處理，歸一化得到筆畫數(shù)據(jù)，如果申請到了服務(wù)器對機(jī)器人的訪問權(quán)限則將筆畫數(shù)據(jù)通過網(wǎng)絡(luò)發(fā)送到服

15、務(wù)器，由服務(wù)器解釋執(zhí)行；在服務(wù)器上運(yùn)行服務(wù)器程序，服務(wù)器程序處理客戶機(jī)的權(quán)限管理和機(jī)器人的實(shí)際控制?？蛻舳撕头?wù)器件網(wǎng)絡(luò)通訊協(xié)議：客戶端和服務(wù)器間通訊流程：1. 客戶端-（CONNECT 請求）-服務(wù)器提請建立網(wǎng)絡(luò)通訊連接請求，為了避免其他無關(guān)計(jì)算機(jī)連接到服務(wù)器干擾實(shí)驗(yàn)流程，在這里采用了 客戶機(jī) 網(wǎng)卡 MAC 地址校驗(yàn)，也就是說 我們先 保存了 能夠連接服務(wù)器 的 所有 客戶計(jì)算機(jī) 網(wǎng)卡的 MAC地址（MAC地址在客戶端軟件運(yùn)行后能夠在窗口中顯示出來），客戶端發(fā)送請求時(shí)服務(wù)器會(huì)在允許聯(lián)機(jī)的地址列表中查找，如果找到則分配給客戶機(jī)一個(gè)連接序號，并在服務(wù)器上顯示連接客戶機(jī)的學(xué)號、姓名、地址等信息；如

16、果客戶機(jī)的地址與服務(wù)器不匹配則拒絕客戶機(jī)的連接請求。2. 客戶機(jī)正確聯(lián)機(jī)到服務(wù)器后，可以向服務(wù)器提交申請實(shí)時(shí)控制請求：客戶機(jī)（CONTROLAPPLY）服務(wù)器，此時(shí)如果服務(wù)器允許客戶機(jī)控制實(shí)際機(jī)器人硬件并且套機(jī)器人硬件有空閑，則分配機(jī)器人編號給客戶端，接下來客戶端可以將處理好的素描數(shù)據(jù)或者鼠標(biāo)繪圖數(shù)據(jù)發(fā)送到服務(wù)器做實(shí)時(shí)繪制實(shí)驗(yàn)?？蛻舳送瓿蓪?shí)時(shí)繪制實(shí)驗(yàn)后應(yīng)該自覺提請釋放實(shí)際機(jī)器人硬件：客戶端（CONTROLNOT）服務(wù)器，此時(shí)服務(wù)器收回分配給客戶機(jī)的機(jī)器人硬件資源，可以供其他客戶器實(shí)驗(yàn)使用。在服務(wù)器上我們也顯示了每套客戶機(jī)聯(lián)機(jī)時(shí)間，在服務(wù)器端也可以強(qiáng)制將長時(shí)間占用機(jī)器人資源而不主動(dòng)釋放的客戶機(jī)斷開資源連接。3. 客戶機(jī)申請到機(jī)器人硬件資源后可以做三種類型實(shí)驗(yàn)：人臉?biāo)孛枥L制、鼠標(biāo)繪圖繪制、手寫板實(shí)時(shí)控制，在實(shí)際實(shí)現(xiàn)中將人臉?biāo)孛枥L制和鼠標(biāo)繪圖繪制歸一為文件發(fā)送繪