【文章內(nèi)容簡(jiǎn)介】 碼盤打滑，編碼器一般是以從動(dòng)輪的形式安裝在機(jī)器人上。這種定位方式致命的弱點(diǎn)是：其無法檢測(cè)橫向的偏移，當(dāng)機(jī)器人受到側(cè)面方向的撞擊而發(fā)生側(cè)移時(shí)，編碼器無法檢測(cè)出這種情況下的偏移量，從而給定位帶來問題。因此，很少有隊(duì)伍單獨(dú)使用編碼其進(jìn)行定位，編碼器很多情況下只是用來輔助定位，或者用來計(jì)算旋轉(zhuǎn)的角度。在我們使用的控制核心TMS320LF2407A中集成了編碼器控制模塊，其分別在兩路方波的上跳沿和下跳沿進(jìn)行采樣，因此該控制模塊相當(dāng)于將輸入的方波進(jìn)行了4倍頻，極大地提高了編碼器的精度。圖42 編碼器原理圖 方案總結(jié)根據(jù)以上幾個(gè)小節(jié)的分析，我們可以看出，如果不在現(xiàn)有的基礎(chǔ)上做出改進(jìn)的話，只有尋線定位方式比較適合做該比賽。但我們仔細(xì)研究了一下編碼的定位方式，發(fā)現(xiàn)可以換一種思路來利用編碼器所獲得的路程值，并且其精度和抗干擾能力上有了很大的提升。從而使得機(jī)器人使用了以碼盤作為基礎(chǔ)的定位方式。 機(jī)器人定位系統(tǒng)以前利用編碼器的時(shí)候，只是對(duì)其當(dāng)前值進(jìn)行處理，并沒有實(shí)時(shí)地對(duì)歷史值進(jìn)行積分，因此，機(jī)器人在每個(gè)時(shí)刻并不知道自己當(dāng)前角度和位置信息，只知道當(dāng)前所處的狀態(tài)。如當(dāng)機(jī)器人正在走直線的時(shí)候，它只知道自己在走直線，并不知到自己在行走過程中的位置和角度信息，旋轉(zhuǎn)時(shí)也是如此。如果對(duì)從初始狀態(tài)之后所有的值進(jìn)行積分的話機(jī)器人就可以知道自己當(dāng)前的位置和角度信息了。 定位算法模型機(jī)器人的定位信息使用來描述。其中分別表示機(jī)器人在全局坐標(biāo)系下的坐標(biāo)，表示機(jī)器人的方位角。圖43是定位算法模型示意圖，其中表示全局坐標(biāo)系，表示機(jī)器人的局部坐標(biāo)系。圖43 定位算法模型當(dāng)機(jī)器人從第n個(gè)狀態(tài)(藍(lán)色)，經(jīng)過很小的運(yùn)動(dòng)到達(dá)第n+1個(gè)狀態(tài)(紅色)時(shí)，左邊和右邊的從動(dòng)輪各自行走了一段很小的圓弧。設(shè)機(jī)器人左邊編碼器的變化量為，右邊的變化量為。機(jī)器人在這個(gè)過程中旋轉(zhuǎn)的角度可以表示為：其中與分別是兩邊編碼器的矯正系數(shù)，其與編碼器的分辨率N和從動(dòng)輪半徑r有關(guān)；為兩邊編碼器從動(dòng)輪的中心輪距。同時(shí)根據(jù)值可以得出在機(jī)器人坐標(biāo)系中x, y方向的變化量：。為了將機(jī)器人坐標(biāo)系中的,轉(zhuǎn)換為全局坐標(biāo)系中的位置和角度信息，需要進(jìn)行如下變換：，其中表示前一時(shí)刻的定位信息，表示當(dāng)前時(shí)刻的位置信息。這樣累加出來的結(jié)果就可以實(shí)時(shí)地得出機(jī)器人的定位信息了。從整個(gè)算法上來看，如果機(jī)器人在橫向出現(xiàn)便偏移的話，位置信息會(huì)出現(xiàn)比較大的誤差，而角度值由于是使用差值，故不會(huì)出現(xiàn)較大的誤差。因此，整個(gè)運(yùn)動(dòng)過程中，角度值受到影響的程度很小。同時(shí)需要指出的是，在輪式機(jī)器人運(yùn)動(dòng)過程中，其旋轉(zhuǎn)中心可以認(rèn)為在兩個(gè)主動(dòng)輪的連線中心上，但整個(gè)定位系統(tǒng)的反饋信息全部是從從動(dòng)輪上的編碼器獲得的，如果兩者的中心不在同一個(gè)垂線上，則會(huì)使得機(jī)器人在旋轉(zhuǎn)的時(shí)候所計(jì)算得到的信息是實(shí)際值的一個(gè)分量，并且這個(gè)分量不是線性的，無法對(duì)其進(jìn)行矯正。因此，必須在安裝上盡量保證從動(dòng)輪連線的中心點(diǎn)和主動(dòng)輪連線的中心點(diǎn)在同一垂線上。理想的安裝方式的示意圖如圖44所示：圖44 編碼器的安裝要求另外，該定位算法對(duì)編碼器的采樣頻率也有要求。理論上來說，采樣頻率越高，積分越準(zhǔn)確。但是，由于整個(gè)計(jì)算過程中，不管是使用定點(diǎn)數(shù)還是浮點(diǎn)數(shù)運(yùn)算，都會(huì)產(chǎn)生計(jì)算精度的損失，因此采樣頻率不能太高，當(dāng)然也不能太低。一般保證每次采樣的時(shí)候，編碼器的變化量為10左右。 編碼器參數(shù)矯正由于安裝、工藝、測(cè)量等原因，導(dǎo)致編碼器上的一些參數(shù)存在誤差。主要表現(xiàn)在r和Len的值不準(zhǔn)。為了得到精確的定位信息，必須對(duì)這兩個(gè)參數(shù)進(jìn)行矯正。矯正的過程中應(yīng)該先矯正r再矯正Len。先手推機(jī)器人，保證其行走的路線是一條直線，再通過無線調(diào)試系統(tǒng)記錄其軌跡，通過其軌跡的形狀，來調(diào)整兩邊的r值。如果發(fā)現(xiàn)軌跡是向左的弧線，則說明左邊的從動(dòng)輪的半徑比右邊小，需要增大左邊的值；如果是向右的弧線，處理方式類似。直到調(diào)試系統(tǒng)記錄的軌跡為直線為止。接下來等比例放大(縮小)兩個(gè)r值，直到其調(diào)試系統(tǒng)上反映的機(jī)器人所走的長(zhǎng)度和實(shí)際長(zhǎng)度相同位置。完成對(duì)從動(dòng)輪半徑的矯正之后，需要對(duì)其中心距進(jìn)行矯正。用手推機(jī)器人，保證機(jī)器人作旋轉(zhuǎn)運(yùn)動(dòng)，旋轉(zhuǎn)足夠大的角度之后(一般是10圈)，對(duì)比調(diào)試系統(tǒng)上記錄的值和實(shí)際機(jī)器人所旋轉(zhuǎn)的角度值，通過差量的正負(fù)和機(jī)器人的旋轉(zhuǎn)方向，對(duì)中心距Len進(jìn)行矯正。實(shí)驗(yàn)證明，經(jīng)過參數(shù)矯正后的機(jī)器人，在沒有受到嚴(yán)重撞擊的情況下，可以正常行走10個(gè)島。 累計(jì)誤差的消除由于系統(tǒng)的所有定位信息都是由編碼器的值來確定的，雖然對(duì)編碼器的參數(shù)進(jìn)行了矯正，但是誤差仍然是存在的，在長(zhǎng)時(shí)間的工作之后，累計(jì)誤差會(huì)導(dǎo)致機(jī)器人無法正常定位。為了保證機(jī)器人能夠長(zhǎng)時(shí)間的工作，需要想辦法消除或者減少累計(jì)誤差。要修正累計(jì)誤差，必須依靠一些事先知道的“絕對(duì)”信息[17]。在本次比賽中，可以事先知道的是每個(gè)島的位置和自動(dòng)區(qū)外圍木欄的角度?？梢岳眠@些信息對(duì)機(jī)器人的定位信息進(jìn)行矯正。如圖45所示，由于場(chǎng)地上的島均為圓形，而機(jī)器人前端的形狀保證其接近島的時(shí)候島的中心和從動(dòng)輪中心連線的中心的距離為一個(gè)可以測(cè)量的常數(shù)。正如前面所提及的那樣，機(jī)器人在運(yùn)動(dòng)過程中，角度的誤差是很小的，而坐標(biāo)信息卻很容易受到干擾。因此，在這里，我們可以通過比較精確的值來矯正不精確的值，即，使用角度來矯正坐標(biāo)。圖45 累計(jì)誤差修正設(shè)島的中心和從動(dòng)輪中心連線的中心的距離為l，島的坐標(biāo)為，則通過以下公式可以得到機(jī)器人的當(dāng)前位置：實(shí)驗(yàn)證明，經(jīng)過累計(jì)誤差修正之后的機(jī)器人，在沒有受到嚴(yán)重撞擊的情況下可以連續(xù)運(yùn)行10分鐘而不丟失自己的位置。這種性能用在3分鐘的比賽中已是綽綽有余。 機(jī)器人導(dǎo)航算法在解決了機(jī)器人定位的問題之后，接來需要解決的問題就是如何讓機(jī)器人到達(dá)給定的目標(biāo)點(diǎn)。由于系統(tǒng)在每個(gè)時(shí)刻知道自己的位置和角度，因此可以利用這些信息來對(duì)機(jī)器人做反饋。具體的，本系統(tǒng)使用目標(biāo)點(diǎn)和編碼器中心點(diǎn)的連線與當(dāng)前機(jī)器人行走方向的夾角作為反饋參數(shù)，使用機(jī)器人與目標(biāo)點(diǎn)的距離作為判斷是否到達(dá)的條件。其可用如下模型來描述。圖46 導(dǎo)航算法示意圖如圖46所示，設(shè)機(jī)器人車頭方向和目標(biāo)點(diǎn)的夾角為，目標(biāo)點(diǎn)的坐標(biāo)為，通過如下公式可將目標(biāo)點(diǎn)的坐標(biāo)轉(zhuǎn)化為機(jī)器人的局部坐標(biāo)值：；然后通過來求出值（當(dāng)0時(shí)取負(fù)號(hào)），使用PID反饋的調(diào)節(jié)方式來根據(jù)的值對(duì)車頭方向進(jìn)行調(diào)整。調(diào)整公式如下所示：式中，PIDresult表示調(diào)整量，P，I，D分別為PID調(diào)節(jié)的三個(gè)參數(shù)為當(dāng)前時(shí)刻的值，為上一時(shí)刻的值。當(dāng)機(jī)器人與目標(biāo)點(diǎn)的距離小于一定閾值時(shí)，機(jī)器人便可以認(rèn)為自己已經(jīng)到達(dá)目標(biāo)點(diǎn)附近了，應(yīng)該退出導(dǎo)航模式，執(zhí)行后繼動(dòng)作。根據(jù)應(yīng)用的需要，我們使用了兩種導(dǎo)航方式。一種是讓機(jī)器人的車頭方向?qū)?zhǔn)目標(biāo)，并不斷地接近目標(biāo)，稱為前進(jìn)導(dǎo)航模式(本文中不加說明，導(dǎo)航均采用該方式)；另外一種是機(jī)器人的車頭方向也是對(duì)準(zhǔn)目標(biāo)，但其是向后倒車，即不斷地遠(yuǎn)離目標(biāo)，稱為后退導(dǎo)航模式。 機(jī)器人避障策略在機(jī)器人運(yùn)動(dòng)過程中，有可能遇到一些可知和不可知的障礙物，必須對(duì)這種情況進(jìn)行處理[16]。對(duì)障礙物的探測(cè)是通過安裝在機(jī)器人身上和側(cè)面的6個(gè)光電開關(guān)實(shí)現(xiàn)的。圖47顯示了機(jī)器人右側(cè)傳感器的安裝位置。圖47 機(jī)器人右側(cè)避障傳感器分布經(jīng)過多次實(shí)驗(yàn)，我們總結(jié)了以下的避障策略：216。 當(dāng)左1(右1)檢測(cè)到信號(hào)時(shí)，機(jī)器人立即掛起導(dǎo)航，并往左后(右后)倒車，直到傳感器沒有信號(hào)時(shí)才恢復(fù)導(dǎo)航。216。 當(dāng)左2(右2)檢測(cè)到信號(hào)時(shí)，機(jī)器人立即掛起導(dǎo)航，同時(shí)左(右)邊驅(qū)動(dòng)輪加速，讓機(jī)器人向左(右)轉(zhuǎn)彎，直到傳感器沒信號(hào)時(shí)才恢復(fù)導(dǎo)航。216。 當(dāng)左3(右3)檢測(cè)到信號(hào)時(shí)，機(jī)器人立即掛起導(dǎo)航，同時(shí)讓機(jī)器人向左后(右后)倒車一段固定的時(shí)間，再讓其往前走一段固定的時(shí)間，然后恢復(fù)導(dǎo)航。216。 當(dāng)左1，右1，同時(shí)檢測(cè)到信號(hào)時(shí)，機(jī)器人立即掛起導(dǎo)航，并采用后退導(dǎo)航模式，直到傳感器沒有信號(hào)時(shí)，才恢復(fù)前進(jìn)導(dǎo)航模式。216。 當(dāng)左2，右2，或者左3，右3同時(shí)檢測(cè)到信號(hào)時(shí)，機(jī)器人立即掛起導(dǎo)航，并向前走一段固定的時(shí)間后，恢復(fù)導(dǎo)航。各傳感器的優(yōu)先級(jí)是1號(hào)3號(hào)2號(hào)。通過實(shí)驗(yàn)證明，這些避障方案，可以讓機(jī)器人順利通過一般的障礙物。 路徑規(guī)劃算法 問題提出在完成了底層的定位、導(dǎo)航、避障之后，接下來要解決的問題是如何讓機(jī)器人采用正確的路徑到達(dá)指定的目標(biāo)點(diǎn)?，F(xiàn)有了路徑規(guī)劃算法有很多[18]，如A*算法（圖48）等，但這些路徑規(guī)劃算法都是基于網(wǎng)格的，即其假設(shè)所有的路徑都是沿著一定的網(wǎng)格進(jìn)行規(guī)劃的，其行走方式為“橫平豎直”型。而我們對(duì)機(jī)器人的控制方式?jīng)Q定了這種基于網(wǎng)格的規(guī)劃方式不適合我們使用。因?yàn)閷?dǎo)航算法所產(chǎn)生的控制效果是機(jī)器人走的所有路線都是曲線，很難保證其走很短的橫線或豎線。比較適合我們的定位方式的路徑應(yīng)該是盡量少的路徑點(diǎn)和比較平滑的折線。圖48 A*算法演示圖圖49 是本次比賽自動(dòng)區(qū)的場(chǎng)地圖，以綠方出發(fā)區(qū)中網(wǎng)格交叉點(diǎn)為坐標(biāo)圓點(diǎn)。如圖，整個(gè)場(chǎng)地可以被分成6塊區(qū)域，每個(gè)區(qū)域有5個(gè)出口點(diǎn)(入口點(diǎn))和相鄰的區(qū)域連通，每個(gè)出口點(diǎn)都有一定的權(quán)值。機(jī)器人從一個(gè)區(qū)域運(yùn)動(dòng)到另外的一個(gè)區(qū)域，必須從其中的一個(gè)出口出發(fā)，而從目標(biāo)區(qū)域的一個(gè)入口進(jìn)入。圖49 路徑規(guī)劃原理圖可以看出，機(jī)器人從圖中任意點(diǎn)出發(fā)，到任意目標(biāo)點(diǎn)，最多經(jīng)歷3個(gè)區(qū)域(包括出發(fā)區(qū)域和目標(biāo)區(qū)域)，因此，規(guī)劃路徑的時(shí)候可以分成3種情況：A) 目標(biāo)點(diǎn)和出發(fā)點(diǎn)屬于同一區(qū)域。這種情況下，不需要在目標(biāo)點(diǎn)和出發(fā)點(diǎn)之間插入路徑規(guī)劃點(diǎn)。B) 目標(biāo)點(diǎn)和出發(fā)點(diǎn)屬于相鄰的區(qū)域。在這種情況下，機(jī)器人需要找到一個(gè)最優(yōu)的出口點(diǎn)，到達(dá)目標(biāo)區(qū)域。C) 目標(biāo)點(diǎn)和出發(fā)點(diǎn)之間相隔一個(gè)中間區(qū)域。這時(shí)，對(duì)與出發(fā)區(qū)域而言，需要找一個(gè)最優(yōu)的出口點(diǎn)，對(duì)于目標(biāo)點(diǎn)而言，需要找一個(gè)最優(yōu)的入口點(diǎn)。根據(jù)上面的分析，可以看出，需要解決的問題就是，如何找出各種最優(yōu)的出口(入口)。 算法模型設(shè)目標(biāo)點(diǎn)為坐標(biāo)為,出發(fā)點(diǎn)的坐標(biāo)為，考慮到目標(biāo)點(diǎn)有可能是區(qū)域邊界上的島，這種情況下，很難判斷出目標(biāo)點(diǎn)屬于哪個(gè)區(qū)域，為了解決這個(gè)問題，我們使用了以下方法：如圖410所示，在目標(biāo)點(diǎn)和出發(fā)點(diǎn)的連線上添加一個(gè)輔助點(diǎn)，該輔助點(diǎn)和目標(biāo)點(diǎn)很近，因此，可以將輔助點(diǎn)當(dāng)成目標(biāo)點(diǎn)，而不影響具體的導(dǎo)航結(jié)果。這樣便解決了區(qū)域邊界點(diǎn)的問題，而且這種方法適用于所有的目標(biāo)點(diǎn)。圖410 輔助點(diǎn)模型圖411為路徑規(guī)劃算法流程圖：圖411 路徑規(guī)劃程序流程圖需要指出的是，上圖中所說的最優(yōu)出口(入口)點(diǎn)是通過點(diǎn)的權(quán)值與距離平方的倒數(shù)之積取最大而產(chǎn)生的。最后所有的存儲(chǔ)點(diǎn)即為路徑規(guī)劃點(diǎn)。下圖是自編的路徑規(guī)劃測(cè)試程序的界面：圖412 路徑規(guī)劃算法測(cè)試效果 超時(shí)處理由于比賽時(shí)機(jī)器人不可避免的會(huì)出現(xiàn)碰撞，死鎖，卡死等讓機(jī)器人不能完成當(dāng)前給定任務(wù)的情況。為了提高機(jī)器人在這些情況下的生存能力，必須對(duì)機(jī)器人的動(dòng)作做超時(shí)處理[18]