世界技能大賽移動機器人運動控制系統(tǒng)設(shè)計

章安福

（廣州市工貿(mào)技師學(xué)院，廣州，510000）

0 引言

世界技能大賽由世界技能組織舉辦，被譽為“技能奧林匹克”，是世界技能組織成員展示和交流職業(yè)技能的重要平臺，比賽項目共分為6個大類，分別為結(jié)構(gòu)與建筑技術(shù)、創(chuàng)意藝術(shù)和時尚、信息與通信技術(shù)、制造與工程技術(shù)、社會與個人服務(wù)、運輸與物流。

移動機器人項目屬于制造與工程技術(shù)領(lǐng)域的賽事之一，隨著制造業(yè)的轉(zhuǎn)型升級，技能人才的培養(yǎng)也扮演著越來越重要的角色，為了更好地推廣移動機器人項目，讓更多的院校參與世界技能大賽，讓更多的學(xué)生學(xué)會利用自動控制技術(shù)設(shè)計世界技能大賽所需的移動機器人，筆者將幾年來對移動機器人電機控制、運動規(guī)劃方面的一些技巧及實現(xiàn)做了總結(jié)，旨在讓更多的參與者快速學(xué)會對移動機器人的控制，更好地推動移動機器人相關(guān)專業(yè)的發(fā)展。

1 系統(tǒng)組成

1.1 系統(tǒng)基本構(gòu)成

世界技能大賽移動機器人項目一般要求參賽隊伍所制作的移動機器人具有較為靈活的移動能力，為了滿足這一條件，普遍采用全方位移動的機器人設(shè)計。全方位移動機器人具有全方位運動能力，其實現(xiàn)方式關(guān)鍵在于全方位的輪系結(jié)構(gòu)，該結(jié)構(gòu)具備每一個大輪邊緣套有小輪的機構(gòu)，能夠避免普通輪系不能側(cè)滑帶來的非完整性運動限制，從而實現(xiàn)全方位運動。

在比賽中，機器人較為常用的底盤是用 3 個全向輪組成的底盤運動控制系統(tǒng)。其中，三個全向輪運動軸心夾角按照 120°進行設(shè)計，之間通過3條橫梁互為60°連接構(gòu)成，如圖1所示，底盤三個全向輪由獨立的電機驅(qū)動。底盤運動信息主要通過三個360線的編碼器和一個9軸陀螺儀獲取。

圖1 三輪機器人效果圖（左）及實物圖（右）

1.2 系統(tǒng)主體框架

世界技能大賽移動機器人項目所設(shè)計的機器人，既要考慮到實用性，又要考慮到使用提供指定套件來搭建。整個指定套件提供了4個直流電機、3個舵機、1個陀螺儀、2個超聲波傳感器、2個紅外傳感器、2個限位開關(guān)，設(shè)計的機器人需要依賴于上述提供的電氣元件。筆者所使用的三輪平臺由核心控制模塊（MYRIO）、傳感器檢測模塊、電機驅(qū)動模塊等部分組成。其中核心控制模塊采用美國國家儀器公司（National Instruments ，以下簡稱NI）的 1900型號 MYRIO為控制核心，主要完成各外設(shè)的底層驅(qū)動、機器人的定位、導(dǎo)航、電機的閉環(huán)控制等；傳感器檢測模塊包括陀螺儀、編碼器，超聲波，紅外，QTI，其框架如圖2所示。

圖2 三輪機器人主體框架

在機器人框架設(shè)計及制作過程中，筆者在綜合考慮移動機器人性能的同時，盡可能多地在套件基礎(chǔ)上進行設(shè)計與制作，在框架連接上也給布線和OMS留下相應(yīng)的空間和連接位置。三類機器人設(shè)計完成后，筆者發(fā)現(xiàn)，三輪機器人使用的材料最少，安裝效率最高，框架穩(wěn)定性和兼容性也能滿足要求。三類機器人在框架方面的對比如表1所示。

表1 三類機器人框架性能對比

2 移動機器人控制實現(xiàn)

2.1 LabVIEW軟件概要

LabVIEW的英文全稱是Laboratory Virtual Instrument Engineering Workbench ，是一門編程語言，也是一種程序開發(fā)環(huán)境，由NI公司研制開發(fā)。LabVIEW類似于C或BASIC開發(fā)環(huán)境，但是這個軟件編程不是用代碼，而是用圖形符號，在這一點上，它明顯不同于傳統(tǒng)的編程語言，如C、C++或Java等，這些程序語言主要是用文本方式編程。LabVIEW除了是一門編程語言外，它主要為科學(xué)家和工程師等設(shè)計者提供一種編程開發(fā)環(huán)境和運行系統(tǒng)，編程只是設(shè)計者工作任務(wù)的一部分。LabVIEW開發(fā)環(huán)境可以搭建在Windows、Mac或Linux系統(tǒng)上，這樣LabVIEW的程序可以直接在相應(yīng)系統(tǒng)中運行，LabVIEW還可以運行在Microsoft Pocket PC、Microsoft Windows CE、Palm OS以及多種嵌入式平臺上，包括DSP、FPGA和微處理器等。

2.2 LabVIEW工作原理

一個LabVIEW程序是由多個(至少1個)虛擬儀器(VI)組成。LabVIEW程序的操作和外觀與實際的物理儀器很相似，因此有“虛擬儀器”的稱謂。即使這樣，它的內(nèi)部的邏輯處理思維仍然是與流行的C語言或者Java語言的編程思維一樣的，同樣具有主程序、子程序、函數(shù)等，設(shè)計者看到或者操作的圖形符號都只是封裝了這些邏輯處理的外觀形象，便于設(shè)計者使用起來更加方便高效。

為方便起，下文筆者將把“LabVIEW程序”稱之為“VI”。注意，無論其外觀和功能是否和實際的儀器相關(guān)聯(lián)，筆者總是將LabVIEW程序稱為VI 。

每一個VI都由三個主要部分組成：前面板、框圖和圖標。

2.3 LabVIEW的PID控制工具包

NI提供了在LabVIEW中使用的PID控制工具包，如圖3所示，可幫助設(shè)計者結(jié)合NI數(shù)據(jù)采集設(shè)備快速有效地搭建一個數(shù)字PID控制器，精確可靠地完成系統(tǒng)需求。

圖3 PID工具包

利用PID.vi即可搭建一個簡單的PID控制器，在該VI的輸入端給入PID的3個參數(shù)值（PID gains）：系統(tǒng)反饋值(process variable)、實際期望值(setpoint)以及微分時間(dt)，便能得到需要的輸出值(output)。該VI還能控制輸出值的范圍?；A(chǔ)PID控件結(jié)構(gòu)如圖4所示。

圖4 基礎(chǔ)PID控件

PID Advanced.vi是高級PID的快速VI，增加了一些高級的功能，如可以設(shè)定期望值的范圍（setpoint range），以及帶手動無擾至自動切換的手動模式控制、非線性積分操作、2自由度控制和誤差平方控制等功能。PID Autotuning.vi是為需要自整定的PID系統(tǒng)設(shè)計的，在給出一些基本要求后，具有自整定的功能。圖5為一種高級自整定PID控件。

圖5 高級自整定PID控件

PID Lead/Lag.vi可以對PID控制器前端由系統(tǒng)反饋來的輸入信號做動態(tài)補償。PID Setpoint Profile.vi可以在期望值間斷性變化時使其變得平滑。PID Control Input Filter.vi是一個五階的低通濾波器，放在PID控制器的“process variable”前端，可以濾去小于采樣率1/10的輸入值。PID Gain Schedule.vi可以寫入幾組增益參數(shù)，并給出條件；在執(zhí)行時,當輸入信號達到條件，VI便使用對應(yīng)的一組參數(shù)給入到PID控制器的PID Gains上。PID Output Limiter.vi對PID控制器輸出信號的變化速率進行控制，以保證外部接受控制信號部件的安全。

圖6 基礎(chǔ)PID構(gòu)建的控制系統(tǒng)

PID工具包提供15個VI，可以滿足大多數(shù)場合的應(yīng)用，根據(jù)不同的現(xiàn)場需求，使用不同的VI搭建PID控制器，十分方便。

PID控制器輸出的精準和前端的輸入信號是否精確密切相關(guān)，所以采集控制系統(tǒng)的前端輸出而得到的系統(tǒng)反饋尤其重要。NI的DAQ數(shù)據(jù)采集卡不僅工作穩(wěn)定，結(jié)合LabVIEW編程方便，兼容性好，且種類很多，可滿足設(shè)計者各類不同檔次上的需求。

在NI軟、硬件的配合下，設(shè)計者能夠使搭建PID控制系統(tǒng)方便、快速、順利，讓設(shè)計工作事半功倍。

2.4 LabVIEW實現(xiàn)的電機調(diào)試

世界技能大賽指定了編程語言使用LabVIEW，所有的參賽機器人程序控制系統(tǒng)均采用LabVIEW編寫完成。導(dǎo)航任務(wù)的實現(xiàn)主要完成機器人的運動控制，根據(jù)機器人當前位置坐標得到目標點坐標信息與補償速度，再綜合計算得到底盤運動目標速度并分解為三個電機驅(qū)動全向輪的目標速度，之后采用PID 算法實現(xiàn)電機的閉環(huán)控制。在實際調(diào)試過程中，設(shè)計者需要先設(shè)計完成基礎(chǔ)PID的LabVIEW架構(gòu)，便于后期調(diào)試。

PID控制是一種在工業(yè)生產(chǎn)中應(yīng)用最廣泛的控制方法，其最大的優(yōu)點是不需要了解被控對象精確的數(shù)學(xué)模型、進行復(fù)雜的理論計算，只需要在線根據(jù)被控變量與給定值之間的偏差以及偏差的變化率等簡單參數(shù)，通過工程方法對比例系數(shù)PK、積分時間IT、微分時間DT三個參數(shù)進行調(diào)整，就可以得到令人滿意的控制效果。

PID控制算法可以分為位置PID和速度PID，電機速度控制則使用速度PID，LabVIEW提供了相關(guān)函數(shù)，PID函數(shù)支持數(shù)組模式，可用于多個電機控制。程序框圖如圖7所示。

圖7 PID構(gòu)建的三輪機器人底盤控制程序

在LabVIEW前面板建立友好的人機交互界面，方便設(shè)計者進行機器人調(diào)試。先設(shè)定好設(shè)定值，然后進行PID的調(diào)節(jié)，一開始可以緩慢增加P值，當快要接近設(shè)定值時候增加I值，調(diào)節(jié)界面如圖8、圖9所示。

圖8 比例控制器調(diào)節(jié)波形圖

圖9 比例積分控制器調(diào)節(jié)波形圖

當P和I都調(diào)好后，編碼誤差在±1之間，這時就不需要調(diào)微分量D了。微分調(diào)節(jié)參數(shù)D主要起抑制作用，每個電機的參數(shù)會有一些微量的差別，但是每個參數(shù)的調(diào)法都按照P-I-D的順序調(diào)整較為合適。

調(diào)節(jié)升降高度使用的是位置PID控制，位置PID則是將編碼器的值直接連接到過程變量，輸出結(jié)果連接到速度PID,如圖10所示。

圖10 位置PID調(diào)節(jié)程序

位置PID的調(diào)節(jié)與速度PID一致，先調(diào)P值，使其在最終目標點停穩(wěn)，如果有小幅波動則調(diào)節(jié)D參數(shù)的值抑制波動。位置波形圖類似梯形，如圖11所示。

圖11 位置PID調(diào)節(jié)波形圖

在機器人運動過程中，除了編碼值的采集外，還會因為場地因素導(dǎo)致機器人發(fā)生角度偏轉(zhuǎn)，而編碼器無法反饋角度偏轉(zhuǎn)，因此在底盤運動控制中需要引入陀螺儀來實現(xiàn)閉環(huán)控制。

在陀螺儀采集控制中，所使用的模塊通過I2C讀寫。使用LabVIEW提供的I2C操作函數(shù)，先進行打開配置，采樣率為400kbps，如圖12所示。

圖12 陀螺儀采集函數(shù)設(shè)置

根據(jù)陀螺儀手冊找到角度地址，讀取角度。由于陀螺儀讀取出的角度只有-180～180，當機器人旋轉(zhuǎn)超過180時就會回到-180，因此，機器人如果需要旋轉(zhuǎn)更多的角度則需要添加處理，如圖13所示。

圖13 陀螺儀采集函數(shù)示例

陀螺儀存在溫漂，在實際使用中，這會給機器人底盤帶來誤差。所以，設(shè)計者需要編寫濾波程序，并考慮其在整個控制系統(tǒng)中的實現(xiàn)。

首先確定重力矢量，

R=[Rx,Ry,Rz]。

R可由其他值推導(dǎo)出來，如Axr，Ayr，Azr或者cosX，cosY，cosZ，根據(jù)這些值能得到設(shè)備相對地平面的傾角值。這些數(shù)值只是由加速度計數(shù)據(jù)推導(dǎo)出來的，如果直接將它們用于程序，可能會產(chǎn)生難以忍受的噪聲。

為了避免混亂，筆者重新定義加速度計的測量值。

Racc 是由加速度計測量到得的慣性力矢量，它可分解為下面的分量（在XYZ軸上的投影）：

RxAcc = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity，

RyAcc = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity，

RzAcc = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity。

首先，根據(jù)加速度計得到一組只來自于加速度計ADC的值。筆者把這組數(shù)據(jù)叫做“vector”，并使用下面的符號表示：

Racc = [RxAcc,RyAcc,RzAcc]。

因為Racc的各個分量可由加速度計數(shù)據(jù)得到，筆者把它當做算法的輸入。

請注意，Racc測量的是重力，如果得到的矢量長度約等于1g，那么就是正確的：

|Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2)。

但把矢量轉(zhuǎn)換成下面的矢量非常重要：

Racc(normalized) = [RxAcc/|Racc| , RyAcc/|Racc| ,zAcc/|Racc|]，

這可以確保標準化Racc始終是1。

接下來，筆者引進一個新的向量：

Rest = [RxEst,RyEst,RzEst]。

Rest就是算法的輸出值，它基于上一次估算的值并經(jīng)過陀螺儀數(shù)據(jù)修正。

在數(shù)列初始，筆者先認定加速度值正確并賦值：

Rest(0) = Racc(0)，

Rest和Racc是向量，所以上面的公式可以用3個簡單的公式代替：

RxEst（0）= RxAcc（0），

RyEst（0）= RyAcc（0），

RzEst（0）= RzAcc（0）。

然后，每隔T秒做一次測量，得到新的測量值，并定義為Racc（1），Racc（2），Racc（3）……。與此同時，在每個時間間隔，筆者也計算出新的估算值Rest（1）,Rest（2），Rest（3）……。

假設(shè)在第n步，筆者有兩列已知的值可以用：

Rest(n-1) – 前一個估算值，

Rest(0) = Racc(0)。

Racc(n) 為當前加速度計測量值。

在計算Rest（n）前，筆者先引進一個新的值Rgyro，它可由陀螺儀和前一個估算值得到。

同理，Rgyro是1個矢量，并由3個分量組成：

Rgyro = [RxGyro,RyGyro,RzGyro]。

分別計算這個矢量的分量，這樣就能更好地實現(xiàn)目標要求。

2.5 移動機器人路徑規(guī)劃

三輪全向移動機器人的模型如圖14所示。在這個模型中，定義一個移動機器人參考坐標系R，機器人正向前行為坐標系Y方向，右方為坐標系X方向，數(shù)學(xué)表示為：

為機器人建立一個局部參考坐標系，分析每個輪子各自的矢量速度對機器人參考坐標系所作出的貢獻稱為正運動學(xué)。

圖14 三輪運動模型建立

通俗地，已知每個輪子的轉(zhuǎn)速，可以計算得到機器人參考坐標系下的速度，即每個輪子在坐標系R下的投影，這常用于機器人運動估計、推算定位、世界坐標系定位等。

反之，已知參考坐標系下的速度，需要求解每個輪子的矢量速度稱為逆運動學(xué)問題。逆運動學(xué)在這里可以理解為設(shè)定機器人X方向、Y方向的速度，計算得到每個電機的矢量速度。

這里定義一個基向量為v ，方向為0°，長度r為圓半徑，則每個輪子的坐標表達式如下:

機器人定位計算任務(wù)主要實現(xiàn)機器人實時定位，通過程序讀取得到的陀螺儀、編碼器數(shù)據(jù)進行計算，得到底盤實時的坐標、位姿、速度等信息；完成底盤的全場定位，獲取機器人的實時運動速度，執(zhí)行導(dǎo)航控制得到目標速度，將目標速度實時分解為三個電機驅(qū)動全向輪的目標速度，完成每個電機驅(qū)動全向輪的速度閉環(huán)控制；從而實現(xiàn)機器人從任意坐標位姿（x1，y1，θ1）跟蹤 Bezier 曲線路徑到達另一坐標位姿（x2，y2，θ2）。

位姿控制程序框圖如圖15所示。

圖15 位姿控制程序

關(guān)于全向底盤的運動控制，LabVIEW提供了相應(yīng)的函數(shù)，可以使設(shè)計者更快速實現(xiàn)調(diào)試。

在移動機器人調(diào)試過程中，無論機器人在場地的任意一個角落，機器人需要知道自己所在位置的坐標，這樣才能夠?qū)崿F(xiàn)機器人的自動控制。因此，在控制過程中，機器人需要實現(xiàn)全場定位功能。

全場定位即是將函數(shù)Get Velocity from Motors輸出的坐標進行積分，程序框圖如圖16所示，坐標通過陀螺儀讀取的值旋轉(zhuǎn)然后積分。

圖16 全場定位控制程序

路徑規(guī)劃與速度規(guī)劃是機器人具備行走能力的基礎(chǔ)，對輪式機器人更是如此。機器人理想的運動路徑應(yīng)是一條光滑連續(xù)的曲線，并且運動速度應(yīng)對時間可導(dǎo)，由此實現(xiàn)平穩(wěn)運動。三階Bezier 曲線具有光滑、連續(xù)等數(shù)學(xué)性質(zhì)，是比較合適的路徑規(guī)劃方法，并已在比賽中得到廣泛應(yīng)用，如圖17所示；良好的速度規(guī)劃可以減小底盤因加速度不恰當引起的沖擊，利于機器人平穩(wěn)運動。筆者考慮到比賽中機器人跑動的路徑相對固定，采用三階 Bezier 曲線規(guī)劃機器人路徑，之后通過插補法完成機器人位姿、速度規(guī)劃。

圖17 路徑規(guī)劃效果演示

在實際比賽中，移動機器人需要在不同的場地環(huán)境下完成相似的任務(wù)，對場地自動規(guī)劃及建模會減少比賽過程中測量場地及計算坐標的過程?？赏ㄟ^LabVIEW繪制場地圖，繪制場地圖使用LabVIEW提供的圖片繪制函數(shù)，如圖18所示。

圖18 LabVIEW繪圖功能函數(shù)

事先準備好每個模塊，如零件架、工作站、零件庫、機器人、場地隔板等元素，方便調(diào)用，然后根據(jù)現(xiàn)場測量記錄繪制場地圖，如圖19所示。

圖19 LabVIEW繪制場地效果圖

在比賽過程中，移動機器人需要移動到不同的工作站點，并完成抓取、放置等動作，機器人必須在場地中繞開隔板進行移動，所以，在繪制好場地圖之后，需要放上機器人的模型作為路徑點，這樣獲取的理論路徑點就可以實際應(yīng)用到比賽場地的調(diào)試中。將機器人模型放置到場地中各個位置，模擬移動機器人實際比賽過程中的路徑點，在此基礎(chǔ)上完成相應(yīng)的控制動作，實現(xiàn)對機器人的運動控制，機器人在場地中的實際效果如圖20所示。

圖20 移動機器人在場地中各個位置的效果圖

在此基礎(chǔ)上，通過場地坐標運算得出相應(yīng)的坐標地點，通過數(shù)據(jù)轉(zhuǎn)換程序計算出實際機器人需要移動的坐標位置，程序得出所有位置并填到機器人控制面板中，如圖21所示。

圖21 坐標運算填寫完成情況

3 經(jīng)驗總結(jié)

本文詳細介紹了基于LabVIEW電機PID控制實現(xiàn)Bezier曲線路徑規(guī)劃的機器人導(dǎo)航算法，并以第45屆世界技能大賽參賽機器人為平臺，研究三輪全向輪底盤的運動特性，結(jié)合外部傳感器實現(xiàn)對機器人的運動控制，使其能夠跟隨基于 Bezier曲線規(guī)劃的路徑運動。實踐表明，該套控制系統(tǒng)具有良好的運動控制精度，在應(yīng)用中滿足比賽對高速、精確的底盤運動控制的要求，可以為全國參賽院校提供參考。

需要指出的是，在實際調(diào)試中，機器人會有底盤誤差和傳感器誤差，使得調(diào)試工作并不簡單。想要在更短的時間內(nèi)完成調(diào)試工作，設(shè)計者需要平時不斷的訓(xùn)練、發(fā)現(xiàn)并解決問題、積累各種經(jīng)驗。隨著經(jīng)驗的不斷積累，設(shè)計者將會更快速適應(yīng)新場地的調(diào)試，運動控制系統(tǒng)的學(xué)習(xí)也將變得更加便捷。