章安福

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

0 引言

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

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

1 系統(tǒng)組成

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

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

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

圖1 三輪機(jī)器人效果圖（左）及實(shí)物圖（右）

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

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

圖2 三輪機(jī)器人主體框架

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

表1 三類機(jī)器人框架性能對比

2 移動機(jī)器人控制實(shí)現(xiàn)

2.1 LabVIEW軟件概要

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

2.2 LabVIEW工作原理

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

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

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

2.3 LabVIEW的PID控制工具包

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

圖3 PID工具包

利用PID.vi即可搭建一個簡單的PID控制器，在該VI的輸入端給入PID的3個參數(shù)值（PID gains）：系統(tǒng)反饋值(process variable)、實(shí)際期望值(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è)計(jì)的，在給出一些基本要求后，具有自整定的功能。圖5為一種高級自整定PID控件。

圖5 高級自整定PID控件

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

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

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

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

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

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

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

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

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

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

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

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

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

當(dāng)P和I都調(diào)好后，編碼誤差在±1之間，這時就不需要調(diào)微分量D了。微分調(diào)節(jié)參數(shù)D主要起抑制作用，每個電機(jī)的參數(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值，使其在最終目標(biāo)點(diǎn)停穩(wěn)，如果有小幅波動則調(diào)節(jié)D參數(shù)的值抑制波動。位置波形圖類似梯形，如圖11所示。

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

在機(jī)器人運(yùn)動過程中，除了編碼值的采集外，還會因?yàn)閳龅匾蛩貙?dǎo)致機(jī)器人發(fā)生角度偏轉(zhuǎn)，而編碼器無法反饋角度偏轉(zhuǎn)，因此在底盤運(yùn)動控制中需要引入陀螺儀來實(shí)現(xiàn)閉環(huán)控制。

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

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

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

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

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

首先確定重力矢量，

R=[Rx,Ry,Rz]。

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

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

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

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

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

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

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

Racc = [RxAcc,RyAcc,RzAcc]。

因?yàn)镽acc的各個分量可由加速度計(jì)數(shù)據(jù)得到，筆者把它當(dāng)做算法的輸入。

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

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

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

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

這可以確保標(biāo)準(zhǔn)化Racc始終是1。

接下來，筆者引進(jìn)一個新的向量：

Rest = [RxEst,RyEst,RzEst]。

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

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

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）……。與此同時，在每個時間間隔，筆者也計(jì)算出新的估算值Rest（1）,Rest（2），Rest（3）……。

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

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

Rest(0) = Racc(0)。

Racc(n) 為當(dāng)前加速度計(jì)測量值。

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

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

Rgyro = [RxGyro,RyGyro,RzGyro]。

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

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

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

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

圖14 三輪運(yùn)動模型建立

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

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

這里定義一個基向量為v ，方向?yàn)?°，長度r為圓半徑，則每個輪子的坐標(biāo)表達(dá)式如下:

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

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

圖15 位姿控制程序

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

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

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

圖16 全場定位控制程序

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

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

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

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

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

圖19 LabVIEW繪制場地效果圖

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

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

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

圖21 坐標(biāo)運(yùn)算填寫完成情況

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

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

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