一種基于最小二乘法的移動機器人拾取臂控制系統研究*

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

（五邑大學智能制造學部，廣東江門 529020）

0 引言

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

圖1 圖像采集處理系統硬件結構

圖2 識別定位系統流程圖

1 機械臂機構分析

機械臂有4個自由度，由4個舵機驅動。舵機1、舵機2分別控制機械臂大臂、小臂的轉動，舵機3控制機械臂在底座上水平方向的轉動，舵機4控制機械臂末端執行器手爪開合，選配不同手爪，實現對不同物體的取放，如圖3所示。該機械臂可安裝在移動機器人底盤上。

圖3可簡化為如圖4（a）所示的結構簡圖，仔細分析可知，該結構實際為二連桿機構，如圖4（b）所示。

圖3 機械臂結構

圖4 機械臂機構分析

圖5所示為yoz面機械臂機構，圖中W表示機械臂水平轉動自由度，由舵機3控制。舵機1、舵機2分別控制大臂、小臂運動，使末端執行器手爪到達目標物位置。末端執行器手爪到達目標物運動軌跡曲線采用分段控制，本文第3章詳細分析。

圖5 機械臂yoz面運動分析

圖中，θ1、θ2分別是舵機1、舵機2控制桿l1、桿l2的轉動角度，機械臂末端執行器手爪位置坐標為P（y,z），手爪位置的數學模型為：

2 機械臂控制系統設計

控制系統原理如圖6所示。控制器從接收到的指令中解析出目標點的位置信息，然后結合3個舵機的控制函數計算出每個舵機的角度，分別輸出對應的脈寬調制信號給各個舵機，實現對機械臂的控制。

圖6 機械臂的開環控制系統

2.1 舵機控制原理

舵機由舵盤、位置反饋電位器、減速齒輪組、直流電機和控制電路組成。電機驅動減速齒輪組，輸出軸的位置檢測是由一個具有線性比例特性的位置反饋電位器完成，控制電路將電位器的反饋電壓與外部輸入控制脈沖進行比較，產生矯正脈沖，達到轉角精確控制，控制原理如圖7所示。

圖7 舵機控制原理圖

脈沖寬度調制是一種模擬控制方法，是利用微處理器的數字輸出來對模擬電路進行控制。舵機的輸入信號是頻率為50 Hz的脈寬調制（PWM）信號，脈沖寬度與舵機轉角對應關系如圖8所示。

圖8 輸入PWM與舵機輸出角度關系

2.2 系統硬件

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

圖9 機械臂控制系統硬件組成框圖

2.3 系統軟件

系統接受和返回的指令都以ASCII符號“#”開頭，以ASCII符號“!”結尾，以ASCII符號“,”為間隔，接收指令表和返回指令表如表1和表2所示。

表1 機械臂系統接收指令表

表2 機械臂系統返回指令表

圖10 系統軟件流程圖

系統從串口接收、解析指令，操作指令則解析出目標點位置信息，再結合3個舵機的控制函數計算出各舵機角度，分別對各個舵機輸送對應的脈寬調制信號，實現對機械臂的控制，并通過串口反饋執行情況，如果是查詢指令則返回機械臂數據，系統軟件流程如圖10所示。

移動機器人的圖像識別定位將目標物與機器人的距離計算后，輸送到機械臂控制系統，通過舵機1、舵機2和舵機3實現目標物的取放。末端運動過程要從P0開始，通過P1到達P2，即沿軌跡曲線Ⅰ和軌跡曲線Ⅱ達到目標點，參看圖5。機械臂末端沿軌跡曲線Ⅰ的空間運動，不會出現干涉、碰撞現象。末端由P1下移到目標物P2時，希望是沿一條垂直向下軌跡曲線Ⅱ，但舵機驅動的二連桿機構很難準確實現，本文采用近似曲線擬合。

3 機械臂末端定位標定

由式（1）可知，系統如能分別控制大、小臂轉度θ1、θ2和水平轉角φ，即能使末端執行器手爪到達拾取物位置。對式（1）求反函數，得：

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

（1）機械臂工作范圍確定

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

機械臂各桿尺寸分別為：大臂l1=140 mm，小臂長度l2=152 mm，手爪的高度為60 mm，機械結構限制抓取距離必須大于85 mm。將數據代入機械臂數學模型，在Matlab平臺分析，繪制出大小臂的角度隨目標點距離變化曲線，如圖11所示。確定拾取范圍為：最近點85 mm，最遠點285 mm。

（2）機械臂末端位置標定

系統舵機驅動選用PCA9685模塊，輸出頻率設定為50 Hz，輸出信號為12位分辨率（4 096級）的PWM信號的占空比。

占空比n=Di/4 069，其中Di為設定數據。設定數據Di與輸出PWM的占空比n呈線性關系，顯然，設定數據Di也與舵機的轉角呈線性關系。

末端位置標定方法按是否需要外部測量設備可以分為開環標定和閉環標定兩大類[6-7]。本文采用開環標定方法，設計了用于末端定位標定系統，包括一套專門用于本系統的控制指令表，供外接設備介入本系統控制機械臂，如圖12所示。

標定時，點動按鍵操作該系統可分段控制各舵機轉動從P0準確到達目標點P1、P2，執行完成后，PC機采集各舵機運行時產生的Di數據，進行最小二乘法分段多次擬合，使軌跡曲線逐步平滑。

圖12 標定系統

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

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

式中：xi為標定樣本點距離臂回轉中心尺寸。

具體方法為：將機械臂固定在工作平臺上，在抓取范圍內的確定高度，設置10個標定樣本點，如圖5所示。點動控制舵機1、2沿曲線Ⅰ、Ⅱ運動，讀取運動過程產生的數據Di（Di=n×4 069），如表3所示。分別與10個標定樣本點的距離數據進行最小二乘法分段曲線擬合，得出兩段機械臂運動控制曲線。

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

在Matlab平臺將實驗測量數據進行曲線擬合，分別得到軌跡曲線Ⅰ、Ⅱ的舵機1、舵機2對應的數據擬合曲線Fij（x），如圖13所示，及擬合方程系數如表4所示。

圖13 舵機數據回歸分析仿真圖

表4 二次擬合方程系數表

機械臂大小臂角度回歸擬合優度R2分別為0.997 3和0.993 4，擬合程度較好，可用于對機械臂的開環控制系統。

（4）工作過程與測試分析

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

實際測試時抓取的目標物為網球，如圖14所示。測試結果表明，該方法抓取目標物的成功率可達100%。定位精度可控制在2～3 mm。

圖14 實際抓取測試圖

4 結束語

本文設計了一種基于最小二乘法的移動機器人拾取系統，采用ARM處理器作為主控芯片，PCA9685作為驅動模塊的拾取臂控制系統。對占空比中的Di值與拾取位置xi之間的關系，進行了最小二乘法的曲線擬合，并設計了一套專門用于該系統的控制指令表，供外接設備介入該系統控制機械臂。

測試和實踐表明，該方法抓取目標物的成功率可達100%。定位精度可控制在2～3 mm。簡化了機械臂控制系統，適用于對精度要求不特別高工作場合。