一種基于最小二乘法的移動(dòng)機(jī)器人拾取臂控制系統(tǒng)研究*

邱炯智，張憲文，李汶杰，王天雷，張京玲，劉家輝，陳江源

（五邑大學(xué)智能制造學(xué)部，廣東江門(mén) 529020）

0 引言

移動(dòng)機(jī)器人主要由行走機(jī)構(gòu)、機(jī)械臂、攝像頭、圖像識(shí)別與處理，以及控制系統(tǒng)等構(gòu)成[1]，根據(jù)不同用途，其機(jī)械臂的結(jié)構(gòu)也不同[2-5]。本文研究的機(jī)械臂配置在移動(dòng)機(jī)器人上，該機(jī)器人帶有視覺(jué)識(shí)別與圖像處理，由圖像采集模塊、無(wú)線路由、計(jì)算機(jī)等組成。工作時(shí)，安裝在車體前端攝像頭捕捉到網(wǎng)球信息，路由器進(jìn)行數(shù)字信號(hào)無(wú)線傳輸，由計(jì)算機(jī)進(jìn)行圖像處理、計(jì)算目標(biāo)物位置，系統(tǒng)采集目標(biāo)物位置信息，控制機(jī)械臂完成定點(diǎn)取放。圖像采集處理系統(tǒng)硬件結(jié)構(gòu)和圖像識(shí)別定位算法流程如圖1、2所示。

圖1 圖像采集處理系統(tǒng)硬件結(jié)構(gòu)

圖2 識(shí)別定位系統(tǒng)流程圖

1 機(jī)械臂機(jī)構(gòu)分析

機(jī)械臂有4個(gè)自由度，由4個(gè)舵機(jī)驅(qū)動(dòng)。舵機(jī)1、舵機(jī)2分別控制機(jī)械臂大臂、小臂的轉(zhuǎn)動(dòng)，舵機(jī)3控制機(jī)械臂在底座上水平方向的轉(zhuǎn)動(dòng)，舵機(jī)4控制機(jī)械臂末端執(zhí)行器手爪開(kāi)合，選配不同手爪，實(shí)現(xiàn)對(duì)不同物體的取放，如圖3所示。該機(jī)械臂可安裝在移動(dòng)機(jī)器人底盤(pán)上。

圖3可簡(jiǎn)化為如圖4（a）所示的結(jié)構(gòu)簡(jiǎn)圖，仔細(xì)分析可知，該結(jié)構(gòu)實(shí)際為二連桿機(jī)構(gòu)，如圖4（b）所示。

圖3 機(jī)械臂結(jié)構(gòu)

圖4 機(jī)械臂機(jī)構(gòu)分析

圖5所示為yoz面機(jī)械臂機(jī)構(gòu)，圖中W表示機(jī)械臂水平轉(zhuǎn)動(dòng)自由度，由舵機(jī)3控制。舵機(jī)1、舵機(jī)2分別控制大臂、小臂運(yùn)動(dòng)，使末端執(zhí)行器手爪到達(dá)目標(biāo)物位置。末端執(zhí)行器手爪到達(dá)目標(biāo)物運(yùn)動(dòng)軌跡曲線采用分段控制，本文第3章詳細(xì)分析。

圖5 機(jī)械臂yoz面運(yùn)動(dòng)分析

圖中，θ1、θ2分別是舵機(jī)1、舵機(jī)2控制桿l1、桿l2的轉(zhuǎn)動(dòng)角度，機(jī)械臂末端執(zhí)行器手爪位置坐標(biāo)為P（y,z），手爪位置的數(shù)學(xué)模型為：

2 機(jī)械臂控制系統(tǒng)設(shè)計(jì)

控制系統(tǒng)原理如圖6所示。控制器從接收到的指令中解析出目標(biāo)點(diǎn)的位置信息，然后結(jié)合3個(gè)舵機(jī)的控制函數(shù)計(jì)算出每個(gè)舵機(jī)的角度，分別輸出對(duì)應(yīng)的脈寬調(diào)制信號(hào)給各個(gè)舵機(jī)，實(shí)現(xiàn)對(duì)機(jī)械臂的控制。

圖6 機(jī)械臂的開(kāi)環(huán)控制系統(tǒng)

2.1 舵機(jī)控制原理

舵機(jī)由舵盤(pán)、位置反饋電位器、減速齒輪組、直流電機(jī)和控制電路組成。電機(jī)驅(qū)動(dòng)減速齒輪組，輸出軸的位置檢測(cè)是由一個(gè)具有線性比例特性的位置反饋電位器完成，控制電路將電位器的反饋電壓與外部輸入控制脈沖進(jìn)行比較，產(chǎn)生矯正脈沖，達(dá)到轉(zhuǎn)角精確控制，控制原理如圖7所示。

圖7 舵機(jī)控制原理圖

脈沖寬度調(diào)制是一種模擬控制方法，是利用微處理器的數(shù)字輸出來(lái)對(duì)模擬電路進(jìn)行控制。舵機(jī)的輸入信號(hào)是頻率為50 Hz的脈寬調(diào)制（PWM）信號(hào)，脈沖寬度與舵機(jī)轉(zhuǎn)角對(duì)應(yīng)關(guān)系如圖8所示。

圖8 輸入PWM與舵機(jī)輸出角度關(guān)系

2.2 系統(tǒng)硬件

系統(tǒng)硬件組成如圖9所示，主控芯片采用32位處理器內(nèi)核的ARM Cortex-M3系列的單片機(jī)，選擇適合于微控制器應(yīng)用的三級(jí)流水線，運(yùn)行速度快，同時(shí)加入了內(nèi)核低功耗模式，減少了芯片的功耗。舵機(jī)驅(qū)動(dòng)板采用PCA9685模塊，可產(chǎn)生16路12位精度的脈寬調(diào)制（PWM）。工作時(shí)系統(tǒng)向外提供波特率為115 200 Bd/s的TTL電平標(biāo)準(zhǔn)串口協(xié)議通信接口，用于接收外界輸入的字符指令。主控芯片處理串口接收到的指令，并通過(guò)IIC協(xié)議控制PCA9685模塊輸出PWM信號(hào)控制機(jī)械臂，執(zhí)行完后通信接口返回操作成功的指令。

圖9 機(jī)械臂控制系統(tǒng)硬件組成框圖

2.3 系統(tǒng)軟件

系統(tǒng)接受和返回的指令都以ASCII符號(hào)“#”開(kāi)頭，以ASCII符號(hào)“!”結(jié)尾，以ASCII符號(hào)“,”為間隔，接收指令表和返回指令表如表1和表2所示。

表1 機(jī)械臂系統(tǒng)接收指令表

表2 機(jī)械臂系統(tǒng)返回指令表

圖10 系統(tǒng)軟件流程圖

系統(tǒng)從串口接收、解析指令，操作指令則解析出目標(biāo)點(diǎn)位置信息，再結(jié)合3個(gè)舵機(jī)的控制函數(shù)計(jì)算出各舵機(jī)角度，分別對(duì)各個(gè)舵機(jī)輸送對(duì)應(yīng)的脈寬調(diào)制信號(hào)，實(shí)現(xiàn)對(duì)機(jī)械臂的控制，并通過(guò)串口反饋執(zhí)行情況，如果是查詢指令則返回機(jī)械臂數(shù)據(jù)，系統(tǒng)軟件流程如圖10所示。

移動(dòng)機(jī)器人的圖像識(shí)別定位將目標(biāo)物與機(jī)器人的距離計(jì)算后，輸送到機(jī)械臂控制系統(tǒng)，通過(guò)舵機(jī)1、舵機(jī)2和舵機(jī)3實(shí)現(xiàn)目標(biāo)物的取放。末端運(yùn)動(dòng)過(guò)程要從P0開(kāi)始，通過(guò)P1到達(dá)P2，即沿軌跡曲線Ⅰ和軌跡曲線Ⅱ達(dá)到目標(biāo)點(diǎn)，參看圖5。機(jī)械臂末端沿軌跡曲線Ⅰ的空間運(yùn)動(dòng)，不會(huì)出現(xiàn)干涉、碰撞現(xiàn)象。末端由P1下移到目標(biāo)物P2時(shí)，希望是沿一條垂直向下軌跡曲線Ⅱ，但舵機(jī)驅(qū)動(dòng)的二連桿機(jī)構(gòu)很難準(zhǔn)確實(shí)現(xiàn)，本文采用近似曲線擬合。

3 機(jī)械臂末端定位標(biāo)定

由式（1）可知，系統(tǒng)如能分別控制大、小臂轉(zhuǎn)度θ1、θ2和水平轉(zhuǎn)角φ，即能使末端執(zhí)行器手爪到達(dá)拾取物位置。對(duì)式（1）求反函數(shù)，得：

由式（2）可看出，θ1、θ2的實(shí)現(xiàn)過(guò)程呈非線性耦合關(guān)系，且復(fù)雜，加之各連桿的幾何參數(shù)誤差，以及制造和安裝過(guò)程中產(chǎn)生的連桿實(shí)際幾何參數(shù)與理論參數(shù)值之間的偏差，并考慮大、小臂均為舵機(jī)驅(qū)動(dòng)等。本文提出了一種簡(jiǎn)易標(biāo)定方法，該方法可用于精度要求不很高的機(jī)械臂取放工作。

（1）機(jī)械臂工作范圍確定

圖11 大臂與小臂角度變化圖

機(jī)械臂各桿尺寸分別為：大臂l1=140 mm，小臂長(zhǎng)度l2=152 mm，手爪的高度為60 mm，機(jī)械結(jié)構(gòu)限制抓取距離必須大于85 mm。將數(shù)據(jù)代入機(jī)械臂數(shù)學(xué)模型，在Matlab平臺(tái)分析，繪制出大小臂的角度隨目標(biāo)點(diǎn)距離變化曲線，如圖11所示。確定拾取范圍為：最近點(diǎn)85 mm，最遠(yuǎn)點(diǎn)285 mm。

（2）機(jī)械臂末端位置標(biāo)定

系統(tǒng)舵機(jī)驅(qū)動(dòng)選用PCA9685模塊，輸出頻率設(shè)定為50 Hz，輸出信號(hào)為12位分辨率（4 096級(jí)）的PWM信號(hào)的占空比。

占空比n=Di/4 069，其中Di為設(shè)定數(shù)據(jù)。設(shè)定數(shù)據(jù)Di與輸出PWM的占空比n呈線性關(guān)系，顯然，設(shè)定數(shù)據(jù)Di也與舵機(jī)的轉(zhuǎn)角呈線性關(guān)系。

末端位置標(biāo)定方法按是否需要外部測(cè)量設(shè)備可以分為開(kāi)環(huán)標(biāo)定和閉環(huán)標(biāo)定兩大類[6-7]。本文采用開(kāi)環(huán)標(biāo)定方法，設(shè)計(jì)了用于末端定位標(biāo)定系統(tǒng)，包括一套專門(mén)用于本系統(tǒng)的控制指令表，供外接設(shè)備介入本系統(tǒng)控制機(jī)械臂，如圖12所示。

標(biāo)定時(shí)，點(diǎn)動(dòng)按鍵操作該系統(tǒng)可分段控制各舵機(jī)轉(zhuǎn)動(dòng)從P0準(zhǔn)確到達(dá)目標(biāo)點(diǎn)P1、P2，執(zhí)行完成后，PC機(jī)采集各舵機(jī)運(yùn)行時(shí)產(chǎn)生的Di數(shù)據(jù)，進(jìn)行最小二乘法分段多次擬合，使軌跡曲線逐步平滑。

圖12 標(biāo)定系統(tǒng)

（3）舵機(jī)控制曲線最小二乘法分段擬合

控制舵機(jī)曲線F（x）是根據(jù)Di與樣本點(diǎn)距離臂回轉(zhuǎn)中心尺寸xi進(jìn)行分段擬合[8]，最小二乘法分段曲線擬合方程為：

式中：xi為標(biāo)定樣本點(diǎn)距離臂回轉(zhuǎn)中心尺寸。

具體方法為：將機(jī)械臂固定在工作平臺(tái)上，在抓取范圍內(nèi)的確定高度，設(shè)置10個(gè)標(biāo)定樣本點(diǎn)，如圖5所示。點(diǎn)動(dòng)控制舵機(jī)1、2沿曲線Ⅰ、Ⅱ運(yùn)動(dòng)，讀取運(yùn)動(dòng)過(guò)程產(chǎn)生的數(shù)據(jù)Di（Di=n×4 069），如表3所示。分別與10個(gè)標(biāo)定樣本點(diǎn)的距離數(shù)據(jù)進(jìn)行最小二乘法分段曲線擬合，得出兩段機(jī)械臂運(yùn)動(dòng)控制曲線。

表3 采樣讀取Di值 （Di=n×406 9）

在Matlab平臺(tái)將實(shí)驗(yàn)測(cè)量數(shù)據(jù)進(jìn)行曲線擬合，分別得到軌跡曲線Ⅰ、Ⅱ的舵機(jī)1、舵機(jī)2對(duì)應(yīng)的數(shù)據(jù)擬合曲線Fij（x），如圖13所示，及擬合方程系數(shù)如表4所示。

圖13 舵機(jī)數(shù)據(jù)回歸分析仿真圖

表4 二次擬合方程系數(shù)表

機(jī)械臂大小臂角度回歸擬合優(yōu)度R2分別為0.997 3和0.993 4，擬合程度較好，可用于對(duì)機(jī)械臂的開(kāi)環(huán)控制系統(tǒng)。

（4）工作過(guò)程與測(cè)試分析

系統(tǒng)工作時(shí)，根據(jù)視覺(jué)識(shí)別系統(tǒng)提供的位置信息，判斷目標(biāo)物已在拾取范圍后，主控芯片將收到抓取指令；先控制舵機(jī)3轉(zhuǎn)動(dòng)到目標(biāo)點(diǎn)正前方向，然后打開(kāi)手爪；再根據(jù)距離xi，調(diào)取舵機(jī)控制擬合方程f11（x）、f12（x），獲得曲線Ⅰ階段的設(shè)定數(shù)據(jù)Di值，計(jì)算PWM信號(hào)的占空比n，控制舵機(jī)1和舵機(jī)2完成軌跡曲線Ⅰ的運(yùn)動(dòng)，到達(dá)目標(biāo)點(diǎn)上方；同理，根據(jù)擬合方程f21（x）、f22（x），得到相應(yīng)的設(shè)定數(shù)據(jù)Di值，控制舵機(jī)1和舵機(jī)2完成軌跡曲線Ⅱ的運(yùn)動(dòng)，下行到達(dá)目標(biāo)點(diǎn)；舵機(jī)4控制手爪閉合完成目標(biāo)物抓取，并提取上行。完成一輪抓取指令操作。

實(shí)際測(cè)試時(shí)抓取的目標(biāo)物為網(wǎng)球，如圖14所示。測(cè)試結(jié)果表明，該方法抓取目標(biāo)物的成功率可達(dá)100%。定位精度可控制在2～3 mm。

圖14 實(shí)際抓取測(cè)試圖

4 結(jié)束語(yǔ)

本文設(shè)計(jì)了一種基于最小二乘法的移動(dòng)機(jī)器人拾取系統(tǒng)，采用ARM處理器作為主控芯片，PCA9685作為驅(qū)動(dòng)模塊的拾取臂控制系統(tǒng)。對(duì)占空比中的Di值與拾取位置xi之間的關(guān)系，進(jìn)行了最小二乘法的曲線擬合，并設(shè)計(jì)了一套專門(mén)用于該系統(tǒng)的控制指令表，供外接設(shè)備介入該系統(tǒng)控制機(jī)械臂。

測(cè)試和實(shí)踐表明，該方法抓取目標(biāo)物的成功率可達(dá)100%。定位精度可控制在2～3 mm。簡(jiǎn)化了機(jī)械臂控制系統(tǒng)，適用于對(duì)精度要求不特別高工作場(chǎng)合。