小車自動避障及路徑規(guī)劃23頁

1、第3章 系統(tǒng)總體結(jié)構及工作原理該系統(tǒng)主要以超聲波測距為基本測距原理，并在相應的硬件和軟件的支持下，達到機器人避障的效果。3.1機器人總體硬件設計3.1.1傳感器的分布要求為了全方位檢測障物的分布狀況，并及時為機器人系統(tǒng)提供全面的數(shù)據(jù)，可將所需的八個傳感器均勻排列在機器人周圍，相鄰每對傳感器互成45度角。為了避免相互干擾，八個傳感器以程序運行周期為周期，進行循環(huán)測距。傳感器排列示意圖如下：d0d1d2d3d4d5d6d7圖3.1.1 傳感器分布圖3.1.2避障系統(tǒng)總體電路框架圖設計紅外傳感器A/D轉(zhuǎn)換單 片 機左側(cè)電機超聲波信號右側(cè)電機LED顯示LED顯示圖3.1.2 硬件設計總體框架圖上圖為支

2、持機器人運行實用程序的硬件部分的總體設計框架圖，由負責相關任務的同學提供。在超聲波信號輸入單片機以后，由存儲在單片機中的主程序調(diào)用避障子程序，根據(jù)輸入信號執(zhí)行避障指令，并使相關數(shù)據(jù)返回主程序，轉(zhuǎn)而提供給電機和LED顯示器的驅(qū)動程序使用，最后，由電機執(zhí)行轉(zhuǎn)向指令，結(jié)果則顯示在LED顯示器上。3.1.3避障系統(tǒng)總體軟件框架圖設計開始初始化，開中斷有按鍵？鍵盤處理按下鍵值X？運行紅外避障運行超聲波避障電機處理程序電機處理程序輸出顯示結(jié)束返回運行顯示處理圖3.1.3 軟件總體框架圖由上圖可知，本文作者負責的超聲波避障程序為軟件總體設計中的子程序部分。在主程序運行過程中，若調(diào)用超聲波避障程序，機器人在自

3、行軌跡規(guī)劃后，將程序處理所得數(shù)據(jù)送給電機處理成立程序，控制電機動作。具體的避障程序設計將在第4章進行。3.2超聲波測距原理測距原理：超聲波是指頻率高于20KHz的機械波。為了以超聲波作為檢測手段，必須產(chǎn)生超生波和接收超聲波。完成這種功能的裝置就是超聲波傳感器，習慣上稱為超聲波換能器或超聲波探頭。超聲波傳感器有發(fā)送器和接收器，但一個超聲波傳感器也可具有發(fā)送和接收聲波的雙重作用。超聲波傳感器是利用壓電效應的原理將電能和超聲波相互轉(zhuǎn)化即在發(fā)射超聲波的時候，將電能轉(zhuǎn)換，發(fā)射超聲波；而在收到回波的時候，則將超聲振動轉(zhuǎn)換成電信號。8超聲波測距的原理一般采用渡越時間法TOF（time of flight）。

4、首先測出超聲波從發(fā)射到遇到障礙物返回所經(jīng)歷的時間，再乘以超聲波的速度就得到二倍的聲源與障礙物之間的距離，即：8D=ct/2 其中D為傳感器與障礙物之間的距離，以m計，c為超聲波速度，這里以340m/s計，t為超聲波從發(fā)送到接收的總時間，以s計。據(jù)此原理可以用超聲波傳感器測得的距離為避障程序提供所需的數(shù)據(jù)。8第4章 軌跡規(guī)劃算法的實現(xiàn)方案4.1軌跡規(guī)劃算法的層次化設計 根據(jù)上述材料分析，可以將機器人軌跡規(guī)劃算法設計分為基礎控制層、行為控制層和坐標計算層，三個層次進行。4.1.1基礎控制層設計基礎控制層可定義為基本行為層，這層算法的任務是尋找目標點，并確保機器人可以順利到達指定目標位。在確定目的地

5、位置的情況下，為了達到上述目的，計算機必須對機器人的方位進行時實計算。應用人工勢場法原理，可以將目標點設為引力極，牽引機器人運動。對此動作建立相應的模型，可以使用建立平面坐標作為虛擬勢場的方法來給機器人定義方位，將機器人關于目標點的時實偏角作為虛擬引力方向，以確定機器人下一步所需轉(zhuǎn)過的角度，并時實檢測，是否已到達目的地，若已到達，則可認為虛擬引力此刻為0，并發(fā)出信號控制程序終止運行總體程序。由此，可確定基礎控制層所需的各參數(shù)：(1) 機器人的時實坐標x, y值，由專門的坐標計算層提供，為了提高精確度，可以采用厘米為單位制。(2) 機器人的速度v，測量后設為定值使用。(3) 周期T，直接設置為定

6、值使用。(4) 偏轉(zhuǎn)角de，可通過機器人與橫坐標之間的夾角pe，減去機器人到目標點連線與橫坐標的夾角E得到。(5) 終止信號last值先置為0，當?shù)竭_目的地時，將其置為1?；A控制層程序流程圖如下：數(shù)據(jù)讀入1計算偏轉(zhuǎn)角de判斷是否到達目標將last值置1數(shù)據(jù)輸出2YN圖4.1.1 基本控制層程序流程圖4.1.2行為控制層行為控制層是比基本控制層更復雜，更具有決定權的層次。它的存在決定了機器人智能避障行為的可行性，是相當重要的算法層。其主要任務是讓機器人根據(jù)超聲波傳感器采集的距離信息判斷是否該進行避障行為，且給出避障的轉(zhuǎn)角值，及轉(zhuǎn)向。這些都以左右綜合距離的大小決定，當左邊綜合距離大于友邊綜合距離

7、時，可認為左邊的斥力值大與右邊斥力值，機器人左轉(zhuǎn)，反之，右轉(zhuǎn)。當前方綜合距離小于設定的最小允許接近距離，而左右綜合距離又相等時，則需要設置專門的轉(zhuǎn)角，對機器人施行強制性轉(zhuǎn)角動作。據(jù)此，可將各控制變量之間的關系以數(shù)學公式的形式列出，并做為“人工勢場法”的基本數(shù)學模型被運用于程序流程圖設計。設聲納Si的輸出為di，轉(zhuǎn)角de和速度v表示控制層行為的輸出，其中de表示機器人下一步的運動方向，v表示機器人下一步運動的速度。那么，基于勢場的控制行為可以表示如下 。de = me L<MminR<Mmin de = de0 R>=L de = -de0 L>R上式中的L表示機器人左邊

8、障礙物的迫近程度，根據(jù)圖3.1.1 ：L 為第1號到第3號傳感器返回距離值的倒數(shù)和；R表示機器人右邊障礙物的迫近程度，根據(jù)圖3.1.1 ：R 為第5號到第7號傳感器返回距離值的倒數(shù)和。Mmin為障礙物的最小迫近程度值，相當于第L組(或第R組)超聲波傳感器最大探索范圍的倒數(shù)和。當左右迫近程度的值都小于最小迫近程度值時，可簡單認為機器人周圍無障礙物，機器人按基礎控制層執(zhí)行程序，其中的me表示機器人要到達目的地需要轉(zhuǎn)過的角度。de0表示機器人執(zhí)行行為控制層程序時，所需轉(zhuǎn)動的角度大小，一般可設為定值。以上述方案為基礎，為了提高機器人的避障能力，還可對設置機器人的前方迫近程度值,因為機器人的反轉(zhuǎn)是以轉(zhuǎn)角

9、180度來實現(xiàn)的，而非后退，所以，后方迫近程度值暫時無需設置。de = de0 F>MmaxF表示機器人前方距離值障礙物的迫近程度，根據(jù)圖3.1.1：F 為第4號傳感器返回距離值的倒數(shù)，Mmax就是用于判斷障礙物是否已經(jīng)離機器人很近，假設機器人的最大速度為v，每兩步之間的時間間隔為t，那么，為了確保機器人不會與障礙物碰撞，Mmax可以表示為速度v和間隔時間t乘積的倒數(shù)。當F值大于Mmax值時，表示前方距離障礙物很近了，需要進行避障處理了。行為控制層程序流程圖如下：數(shù)據(jù)讀入0F是否大于MmaxR是否大于等于L左轉(zhuǎn)角度deL和R是否都小于Mmin數(shù)據(jù)出口1右轉(zhuǎn)角度deR是否大于等于L左轉(zhuǎn)角度

10、de右轉(zhuǎn)角度de數(shù)據(jù)輸出2YNYNYNYN圖4.1.2 行為控制層程序流程圖4.1.3坐標計算層設計坐標計算層的設計方案，主要可采用虛擬坐標技術，它能形象地定義機器人相對障礙物及目標點的具體位置，確保機器人及時避障，并順利到達目標點。在機器人初始坐標明確的條件下，機器人坐標可根據(jù)上述兩層程序提供的數(shù)據(jù)算得。設程序循環(huán)間隔的周期為T，那么，在間隔周期T時間內(nèi)，機器人行走的距離P為速度v與T的乘積。又設機器人正方向與平面坐標橫軸正方向的夾角為pe ，其初始值確定，轉(zhuǎn)動值為上述兩層提供的角度de ，那么本周期的pe值為上個周期的pe值與轉(zhuǎn)角de的差。由此，可分別計算機器人本周期內(nèi)橫、縱坐標的變化值

11、X , Y。X = P * cos ( pe )Y = P * sin ( pe )因此，本周期的橫、縱坐標 x , y 值可由上個周期的坐標減去變化值得到。而本周期的坐標值在行為控制層允許的情況下，將被代入基本行為層，檢測是否到達目的地。坐標計算層程序流程圖如下：數(shù)據(jù)讀入2計算P計算pe計算本周期內(nèi)橫、縱坐標變化值X , Y計算本周期內(nèi)橫、縱坐標的值x , y數(shù)據(jù)輸出0圖4.1.3 坐標計算層程序流程圖上述三層算法，相互制約，相互聯(lián)系，組成了整個系統(tǒng)的基本算法。每次循環(huán)執(zhí)行程序時，都是由行為控制層入口，通過對迫近程度值大小的判斷，選擇是否中斷行為控制層，轉(zhuǎn)而執(zhí)行基礎控制層。最后執(zhí)行坐標計算層

12、，并把所得數(shù)據(jù)提供傳給程序循環(huán)的下一周期使用。依據(jù)上述各算法層之間的關系，可以組合出軌跡規(guī)劃子程序的總體程序流程圖，如下圖所示：行為控制層坐標計算層基礎控制層數(shù)據(jù)接口1數(shù)據(jù)接口2主程序調(diào)用入口0主程序調(diào)用出口0圖4.1.4 軌跡規(guī)劃子程序總體程序流程圖4.2機器人系統(tǒng)及環(huán)境的軟件化硬件及環(huán)境的軟件化技術是一種脫離硬件及實際環(huán)境對程序進行檢測、仿真的編程技術，也是對機器人軌跡進行規(guī)劃的過程中必不可少的一種技術。它將避障程序運行必需的硬件功能及環(huán)境影響，用擁有同樣功效的程序表達出來，以檢測避障程序正確與否。1.以下便是支持避障程序運行必需的各項硬件功能：(1) 測量8個超聲波傳感器與障礙物之間的距

13、離。(2) 系統(tǒng)停止調(diào)用避障子程序的功能。(3) 機器人運行軌跡的可視化功能。2.以下可視為環(huán)境對程序的影響：(1)機器人運行前，設置障礙物的大小及坐標。根據(jù)上述原理和各項需要模擬的功能，可以使用數(shù)學建模的方法進行軟件化編程。首先，需要對上述邏輯化步驟進行排序。因為機器人行為是一種仿人的行為，因此，可將上述功能及影響對應相關的“神經(jīng)傳導過程”進行排序，并在排序后設計相應的系統(tǒng)程序流程圖。排序及功能對應的圖形如下：環(huán)境識別功能神經(jīng)傳遞功能行為決策功能行為動作功能1神經(jīng)反射功能傳感器測距功能軌跡規(guī)劃子程序功能停止調(diào)用子程序行為動作功能2軌跡可視化功能行為動作功能0圖4.2.1 神經(jīng)傳導模型排序圖圖

14、4.2.1中的“神經(jīng)傳遞功能”相當于數(shù)據(jù)輸入功能，而“行為決策功能”相當于偏角計算及算法層次選擇的功能，“行為動作功能”則對應機器人下步坐標的確立過程。“神經(jīng)反射功能”的任務則是輸出必要數(shù)據(jù)，并判斷是否應該結(jié)束循環(huán)。由這些功能組成模塊，正是機器人軌跡規(guī)劃子程序部分。程序開始測量障礙物距離放置障礙物避障子程序是否停止避障子程序繪制機器人運行軌跡程序結(jié)束YN圖4.2.2 系統(tǒng)程序流程圖4.3超聲波傳感器測距功能的模擬根據(jù)圖4.2.2給出的系統(tǒng)程序流程，可知，在機器人系統(tǒng)及環(huán)境的軟件化過程中，傳感器測距功能的模擬最為繁雜。主要的解決思路是建立適當?shù)臄?shù)學模型，通過對模型的具體分析，運用幾何學知識計算超

15、聲波傳感器與障礙物之間的距離值。為了方便計算，可用微分學將障礙物設為由多個已知坐標和半徑的圓組成的任意形狀。而傳感器到障礙物之間的距離可參考以下幾何建模的方法計算得出：VAX軸正方向第4號傳感器朝向wM角Xex , ya , b機器人障礙物圖4.3 機器人測距功能數(shù)學模型圖根據(jù)上圖分析可設機器人半徑為r ，障礙物半徑為R 。實線M就是需要求取的障礙物與第i號傳感器之間的距離。點劃線A是機器人中心與障礙物圓心的連線，它與X正方向的夾角記為Ze 。第i號傳感器與第4號傳感器之間的夾角記為Ke ，在圖上反映為實線M與第4號傳感器之間的夾角。因為機器人圓心坐標(x ,y)可由避障子程序求得，而障礙物圓

16、心(a ,b)又是已知值，因此，可用C語言表示出Ze值。Ze=atan2(b-y),(a-x);根據(jù)圖3.1.1可知相鄰兩個超聲波傳感器的夾角為45度，那么，Xe的值可分為兩種情況，分別求取：當i < 4時，第i號傳感器在第4號傳感器左邊。Xe = Ze-pe-(4-i)*45;當i >= 4時，第i號傳感器在第4號傳感器右邊。Xe = pe-Ze-(i-4)*45;已知Xe值后，通過正余弦公式便可求得機器人與障礙物之間的距離M 。其C程序表達式如下：N=A*sin(Xe);M = abs(A*cos(Xe)-sqrt(R*R-N*N)-r;4.4避障子程序與實用主程序的銜接組合將

17、4.1中提及的三層算法整理到一起就可以組裝成實現(xiàn)避障行為的子程序。將該子程序代替圖3.1.3的總體軟件框架中的“運行超聲波避障”部分，就可構成實用程序的流程圖了。開始初始化，開中斷有按鍵？鍵盤處理按下鍵值X？運行紅外避障電機處理程序電機處理程序輸出顯示結(jié)束返回運行顯示處理行為控制層坐標計算層基礎控制層數(shù)據(jù)接口1數(shù)據(jù)接口2主程序調(diào)用入口0主程序調(diào)用出口0圖4.4 避障機器人運行實用程序流程圖4.5難點程序化優(yōu)化處理在對程序詳細設計的過程中，難免遇到一些難以表達，或表達不到位的地方，比如角度換算問題、障礙物放置個數(shù)問題以及虛擬超聲波的穿透測距問題。在避障子程序和仿真主程序中，大量使用了角度這一變量

18、。為了表示機器人轉(zhuǎn)動這一動作，角度就必須帶上正負符號。當然，真正需要注意的并不是角度的符號問題，而是“角度”與“弧度”的互換問題。在C程序中，有關正弦、余弦、正切、余切及其反函數(shù)的計算，一般都是以弧度作為角的單位被使用的，因此，在角的單位處理上，需要特別留意。處理方法一般是根據(jù)“弧度”和“角度”的關系式進行互相轉(zhuǎn)換，“360°=2弧度”，子程序中的處理如下：E=(180*atan2(Y,X)/3.14; /*機器人圓心到目標點的連線與橫坐標X的夾角*/dx=p*cos(pe*3.14)/180); /*機器人每走一步，在橫坐標上產(chǎn)生的變量值*/dy=p*sin(pe*3.14)/18

19、0); /*機器人每走一步，在縱坐標上產(chǎn)生的變量值*/障礙物的放置動作，反映在仿真程序中，相當于由調(diào)試人員輸入多個定坐標和半徑的圓的動作。為了確?？杀惠斎雸A的個數(shù)盡量多，可采用循環(huán)輸入方式置障。在循環(huán)過程中，因無法限制輸入圓的個數(shù)，所以，必然會造成程序的死循環(huán)。因此，必須給循環(huán)定義一個結(jié)束標準。通常可以在輸入程序“scanf”中多加一個結(jié)束控制變量s，將s初值置為0，當s=0時，循環(huán)繼續(xù)，當s=1時，跳出循環(huán)。子程序中的處理如下：for(j=0;s!=1;j+)scanf("%f,%f,%f,%d",&xbj,&ybj,&rbj,&s); /

20、*依次輸入圓心坐標，圓的半徑，循環(huán)結(jié)束控制信號s的值*/setcolor(RED);setfillstyle(SOLID_FILL,RED);circle(xbj,ybj,rbj);n=j;虛擬超聲波的穿透測距問題，在實際環(huán)境中，這是不可能發(fā)生的事情，但在程序中，計算機每次循環(huán)都需要對每個圓進行測距，這是由程序的智能程度偏低造成的，也是不可避免的，為了得到真實的測量數(shù)據(jù)，需要給原算法加上智能化較高的附加模塊。根據(jù)邏輯分析，當同一傳感器得到多個返回值時，最小的那個就是真實值。因此，可在原來算法的基礎上加上一段比較算法，得出最終真實值。子程序中的處理如下：for(i=0;i<=7;i+) /

21、*比較出每個傳感器返回值中的最小值，并作為傳感器的真實返回值，送入避障子程序處理*/for(j=1;j<=n;j+)if(poij>poij-1)poij=poij-1;min=poij;di=min;以上設計難點僅為詳細編程時碰到的幾個理論上的疑難問題，而具體調(diào)試過程中出現(xiàn)的實際問題將在第5章，給出相應解決方案。第5章 程序的仿真分析5.1仿真數(shù)據(jù)處理及分析為了檢測避障程序能否正常運行，必須對其進行仿真處理。在開始仿真處理前，為了便于對仿真結(jié)果進行分析，首先需在仿真主程序中將機器人起點設置為黃色，目標點設為紅色，障礙物為綠色，機器人自身設為藍色，這可以方便觀察、分析。為了提高仿真

22、的精確度，程序中衡量長度的數(shù)據(jù)都以分米為單位。做好上述準備后，就可以在TurboC2.0環(huán)境下開始程序仿真了。為了對程序功能進行全面的檢測，障礙物放置方法越多越好。下面，從簡入難對程序進行仿真。1. 對單障礙物阻擋的仿真(1) 機器人左偏避障以下圖形中黃色的圓代表機器人行動的起點坐標，其圓心坐標為（20，20），半徑為20；藍色的圓組成的是機器人移動的軌跡；綠色圓代表障礙物，其圓心坐標為（150，150），半徑為50；紅色的圓代表目的地，圓心坐標為（400，400），半徑為20。程序中設定的程序循環(huán)周期為4秒，速度為10分米每秒。圖5.1.1 單障礙物環(huán)境下，機器人的左偏運動根據(jù)對圖形5.1.

23、1中，不同顏色的圓的相對位置分析可知，機器人可以成功的以左偏方式避開正前方的障礙物，但卻穿越了目的地限定的紅色區(qū)域，這可視為沒有成功到達目的地?？梢姡苷铣绦虻难h(huán)周期過大了，因此，可將程序的循環(huán)周期改為2秒，從而可以得到較完美的圖形，圖5.1.2顯示了程序改進后的效果。圖5.1.2單障礙物環(huán)境下，機器人的左偏運動的改進圖(2) 機器人右偏避障以修改后的上述仿真為基礎，保持起點圓和目的地圓的圓心坐標及半徑不變，以函數(shù)輸入的形式，重新調(diào)整障礙物的圓心坐標及半徑大小，可得到仿真圖5.1.3。改動后的障礙物圓心坐標為（150，140），半徑為50分米。圖5.1.3單障礙物環(huán)境下，機器人的右偏運動從圖

24、5.1.3可看出機器人根據(jù)程序判斷出從右邊到目的地的距離比從左邊去短，并選擇了短距離避障的運行方式。而且，機器人可以順利到達目的地。但圖中，機器人運行軌跡仍有與障礙物接觸的點，表明程序循環(huán)周期依然過大，繼續(xù)進行改進，其改進效果在“機器人對雙障礙物的同側(cè)避障”圖5.1.4中顯得較為可觀。2. 對多障礙物阻擋情況的仿真(1) 機器人對雙障礙物的同側(cè)避障在保證起點和目的地圓心坐標及半徑不變的條件下，可繼續(xù)添加障礙物來檢測機器人是否有避開多個障礙的能力。輸入一號障礙物圓心坐標（150，150），半徑50分米；二號障礙物圓心坐標（300，300），半徑50分米?？紤]到新加障礙物可能影響機器人行走的精確度

25、，可將程序循環(huán)周期改為1秒，仿真后得到圖5.1.4。圖5.1.4機器人對雙障礙物的同側(cè)避障1根據(jù)上圖中機器人的軌跡分析可知機器人擁有對多重障礙物的避障功能，并可以按指令到達目標點。當然，這里的多重障礙物是非連續(xù)分布。(2) 機器人對雙障礙物的異側(cè)避障為了檢測機器人在進行多重障礙物避障的行為過程中是否依然具有近距離選擇，這一智能行為，可繼續(xù)改變障礙物的位置分布狀況。入一號障礙物圓心坐標（150，150），半徑50分米；二號障礙物圓心坐標（250，150），半徑50分米。仿真后，生成下圖5.1.5。圖5.1.5機器人對雙障礙物的異側(cè)避障 觀察圖5.1.5，可發(fā)現(xiàn)機器人并不是從兩個障礙物的同側(cè)饒過，而是選擇了相對較短的路程，從障礙物中間穿過，并成功避開了障礙物，到達目的地，這說明