機(jī)器人避障問(wèn)題

吳元清，廖輝,鄧志揚(yáng)，郭文靜，凌德梅

(四川職業(yè)技術(shù)學(xué)院，四川 遂寧 629000)

機(jī)器人避障問(wèn)題

吳元清，廖輝,鄧志揚(yáng)，郭文靜，凌德梅

(四川職業(yè)技術(shù)學(xué)院，四川 遂寧 629000)

本文要解決機(jī)器人避障行走的最短路徑和最短時(shí)間問(wèn)題.主要研究了在一個(gè)區(qū)域中有12個(gè)不同形狀的小區(qū)域是機(jī)器人不能與之發(fā)生碰撞的障礙物，機(jī)器人從區(qū)域中的O點(diǎn)出發(fā)避開(kāi)各種障礙物到達(dá)最終目標(biāo)點(diǎn)的最短路徑和最短時(shí)間數(shù)學(xué)模型.我們對(duì)問(wèn)題1采用初等數(shù)學(xué)中的解析幾何和三角函數(shù)知識(shí)，建立基本線圓結(jié)構(gòu)求路徑的數(shù)學(xué)模型，分內(nèi)公切線、外公切線和經(jīng)過(guò)定點(diǎn)的動(dòng)圓三種情形討論，對(duì)動(dòng)圓我們采用將圓形障礙物的半徑增加r，或把切線轉(zhuǎn)角用由定圓心到定點(diǎn)連線的夾角近似代替，都分解為基本線圓結(jié)構(gòu)數(shù)學(xué)模型來(lái)求解，用窮舉法結(jié)合matlab編程算出可能的走法的總路徑的最小值.對(duì)問(wèn)題2我們采用建立時(shí)間與行走轉(zhuǎn)彎半徑的數(shù)學(xué)模型，用搜索法結(jié)合matlab編程，求出最短時(shí)間.結(jié)果是：O→A的最短路徑為471.0372. O→B的最短路徑為858.6000.O→C的最短路徑為1093.7000.O→A→B→C→O的最短路徑為2783.7000.O→A的最短時(shí)間為94.5649.

最短路徑；搜索法；matlab；基本線圓結(jié)構(gòu)；初等數(shù)學(xué)模型

1 問(wèn)題重述

機(jī)器避障問(wèn)題是2012年全國(guó)大學(xué)生學(xué)建模竟賽的D題，該問(wèn)題如：

有一個(gè)800*800的平面場(chǎng)景圖（如圖1.1）,有一個(gè)機(jī)器人在原點(diǎn)O（0，0）處，機(jī)器人只能在此平面場(chǎng)景范圍內(nèi)活動(dòng)，而圖中有12個(gè)不同形狀的區(qū)域是障礙物，機(jī)器人不能與之發(fā)生碰撞，障礙物的數(shù)學(xué)描述如表1.1：

圖1.1

表1.1

在平面場(chǎng)景中，障礙物外指定一點(diǎn)為機(jī)器人要到達(dá)的目標(biāo)點(diǎn)，而目標(biāo)點(diǎn)與障礙物的距離至少超過(guò)10個(gè)單位，要想機(jī)器人的行走路徑為最優(yōu)路徑，則它行走的路徑由線圓結(jié)構(gòu)組成.機(jī)器人不能折線轉(zhuǎn)彎，轉(zhuǎn)彎路徑是與直線路徑相切的一段圓弧組成，也可以由兩個(gè)或者多個(gè)相切的圓弧路徑組成，要求每個(gè)圓弧的半徑和機(jī)器人行走線路與障礙物間的最小距離為10個(gè)單位，保證機(jī)器人在行走過(guò)程中不與障礙物發(fā)生碰撞;不然就會(huì)發(fā)生碰撞，如果碰撞發(fā)生，機(jī)器人行走失敗.

問(wèn)題1：平面場(chǎng)景圖中有4個(gè)點(diǎn)分別為

O(0,0)，A(300,300)，B(100,700)，C(700,640)，機(jī)器人從O(0,0)出發(fā),O→A,O→B,O→C和O→A→B→C→O的最短路徑.

問(wèn)題2：機(jī)器人直線行走的最大速度為v0=5個(gè)單位∕秒.機(jī)器人轉(zhuǎn)彎的最大速度為v=v(ρ)=其中ρ是轉(zhuǎn)彎半徑.若超過(guò)該速度，機(jī)器人將無(wú)法完成行走.機(jī)器人從O（0，0）出發(fā)，到達(dá)點(diǎn)A（300，300）的最短時(shí)間路徑.

2 問(wèn)題分析

本題的主要目的是在(如圖1.1)的平面場(chǎng)景圖中求出機(jī)器人繞過(guò)障礙物到達(dá)目標(biāo)點(diǎn)的最短路徑.在此平面場(chǎng)景圖中機(jī)器人行走的路徑有很多種方法，我們通過(guò)分析發(fā)現(xiàn)每條路徑所經(jīng)過(guò)的障礙物要滿(mǎn)足下面的條件，每條路徑所經(jīng)過(guò)的障礙物都可分解為線圓結(jié)構(gòu).根據(jù)題目要求，機(jī)器人行走路徑要滿(mǎn)足這樣的約束條件：

（1）機(jī)器人只能在圖1.1的平面場(chǎng)景范圍內(nèi)活動(dòng)；

（2）若機(jī)器人行走的路徑需要有一定的安全性，則機(jī)器人與障礙物的距離至少超過(guò)10個(gè)單位；

（3）由于機(jī)器人的靈活性有限，因此它在轉(zhuǎn)彎過(guò)程中不能是折線，轉(zhuǎn)彎路徑可以由與直線路徑相切的圓弧組成，也可以由兩個(gè)或多個(gè)相切的圓弧路徑組成，且轉(zhuǎn)彎半徑最小為10個(gè)單位;

（4）在拐點(diǎn)和節(jié)點(diǎn)處都采用最小轉(zhuǎn)彎半徑，求得的路徑最短.

對(duì)問(wèn)題1，要分別求出機(jī)器人行走路徑從O→A,O→B,O→C和O→A→B→C→O四種走法的各段最短的路徑，首先求出所經(jīng)過(guò)路徑中各線段的長(zhǎng)以及各段圓弧的弧長(zhǎng).然后各段相加計(jì)算總路徑，并求出各種走法的最優(yōu)解.

對(duì)問(wèn)題2,在解決了問(wèn)題1的基礎(chǔ)上，要有約束條件：

（1）機(jī)器人直線行走以最大速度為v0=5個(gè)單位∕秒;

（3）機(jī)器人變速和轉(zhuǎn)身瞬間完成.

3 模型假設(shè)

(1)機(jī)器人能夠抽象成點(diǎn)來(lái)處理；

(2)機(jī)器人的性能足夠好，能準(zhǔn)確地沿圓弧轉(zhuǎn)彎；

(3)機(jī)器人行走過(guò)程中不會(huì)意外停止；

(4)機(jī)器人行走不小于最小轉(zhuǎn)彎半徑和最小安全距離；

(5)機(jī)器人不會(huì)進(jìn)入兩個(gè)相接觸的障礙物的死角.

4 定義與符號(hào)說(shuō)明

r，ρ:轉(zhuǎn)彎半徑.α，αi：直線傾角或夾角.t:時(shí)間. L:最短路徑總長(zhǎng).

5 模型的建立

查閱相關(guān)文獻(xiàn)知，具有圓形限定區(qū)域的最短路徑是由兩部分組成的，一部分是平面上的自然最短路徑（即直線段），另一部分是限定區(qū)域的部分邊界，這兩部分是相切的，這兩條直線段是由圓弧連接的.

對(duì)于問(wèn)題1，我們經(jīng)過(guò)深入分析知，起點(diǎn)到目標(biāo)點(diǎn)無(wú)論中間障礙物有多少，最短路徑都應(yīng)該是若干個(gè)線圓結(jié)構(gòu)所組成.在本題中存在障礙物的狀況，且障礙物在拐點(diǎn)處的危險(xiǎn)區(qū)域是一個(gè)半徑為r的圓弧，而求兩點(diǎn)之間的最短路徑中的轉(zhuǎn)彎半徑我們應(yīng)該按照最小的轉(zhuǎn)彎半徑來(lái)算才能達(dá)到最優(yōu).

5.1 基本線圓結(jié)構(gòu)的數(shù)學(xué)模型

如圖5.1，設(shè)A(x1,y1)為起點(diǎn)，B(x2,y2)為目標(biāo)點(diǎn)，延長(zhǎng)直線O到CD中點(diǎn)交圓弧CD于H,過(guò)圓心作OH的垂線分別交AC、CD于F、E，圓心O(x3, y3)，C(x4,y4),和D(x5,y5)為機(jī)器人經(jīng)過(guò)拐點(diǎn)分別于脫離危險(xiǎn)線拐角小圓弧的切點(diǎn)，圓的半徑為r，∠AOB=α，∠AOC=β，∠BOD=γ，∠COD=θ.設(shè)ACDB線圓弧段的長(zhǎng)度為L(zhǎng)，解法如下：

圖5.1

5.2 三種特殊情形的轉(zhuǎn)化

下列三種情形我們不能直接采用線圓的結(jié)構(gòu)來(lái)解決，需要做簡(jiǎn)單的變換.

我們假設(shè)兩圓心坐標(biāo)分別為O1(x1,y1)和O2(x2, y2),半徑分別為r1和r2.

5.2.1 內(nèi)公切線情形

如圖5.2，設(shè)分點(diǎn)M(x3,y3)，由定比分點(diǎn)公式,得

圖5.2

這樣我們就可以利用5.1的方法，先求A到M，再求M到B，分兩段求解.

5.2.2 外公切線情形

如圖5.3，直線O1O2的傾角中點(diǎn)M(x3,y3),求得

圖5.3

我們也可以利用5.1的方法，先求A到M，再求M到B，分兩段求解.

同理如果有更多上述的轉(zhuǎn)彎，我們同樣可以按照上面方法分解.

5.2.3 從A繞過(guò)圓形障礙物經(jīng)過(guò)動(dòng)點(diǎn)P到達(dá)目標(biāo)點(diǎn)B情形

要求出機(jī)器人從A繞過(guò)圓形障礙物經(jīng)營(yíng)動(dòng)點(diǎn)P到達(dá)目標(biāo)點(diǎn)B的最短路徑，我們有兩種方法：

方法1，我們把圓形障礙物的半徑增加r,把動(dòng)態(tài)問(wèn)題轉(zhuǎn)化為上面兩種情形之一;

方法2，問(wèn)題1中要求機(jī)器人在原約束條件下從O依次經(jīng)過(guò)A,B,C三個(gè)點(diǎn)回到O，不能簡(jiǎn)單地處理為求各兩點(diǎn)間的最短路徑之和.由于機(jī)器人在經(jīng)過(guò)中間目標(biāo)點(diǎn)后不能直接轉(zhuǎn)變方向，所以要考慮機(jī)器人在到達(dá)目標(biāo)點(diǎn)前的提前轉(zhuǎn)向.同時(shí)整個(gè)過(guò)程可分為從O到A，從A到B，從B到C，從C到O四個(gè)階段.在第一階段，起始點(diǎn)位置確定，出發(fā)方向是任意的，而目標(biāo)點(diǎn)的位置和觸碰時(shí)的方向都已經(jīng)確定了.在第二、三階段，起始點(diǎn)和目標(biāo)點(diǎn)的位置和方向都是不確定的.在第四個(gè)階段，起始點(diǎn)的位置和出發(fā)方向確定了，而目標(biāo)點(diǎn)位置確定，方向不定.

為了使得機(jī)器人在目標(biāo)點(diǎn)處滿(mǎn)足最小轉(zhuǎn)彎半徑的限制，我們?cè)谥虚g目標(biāo)點(diǎn)上加一個(gè)經(jīng)過(guò)該點(diǎn)的半徑固定、圓心活動(dòng)的圓環(huán)來(lái)進(jìn)行中間目標(biāo)點(diǎn)弧化處理.圓環(huán)圓心的不固定增加了求解的復(fù)雜程度.通過(guò)分析,我們發(fā)現(xiàn)圓環(huán)的圓心位置實(shí)際上只有有限種情形。排除一切明顯不合理情形后，可以進(jìn)行窮舉.對(duì)于每一種狀態(tài)求解最優(yōu)路徑之后，再將所有狀態(tài)的最優(yōu)路徑進(jìn)行比較，取全局最優(yōu)作為本題的結(jié)果，同時(shí)也可以確定圓心的實(shí)際位置.

要求出機(jī)器人從A繞過(guò)障礙物經(jīng)過(guò)M點(diǎn)到達(dá)目標(biāo)點(diǎn)B的最短路徑，我們采用以下方法：

用一根釘子將一個(gè)圓環(huán)定在M點(diǎn)，使這個(gè)圓環(huán)能夠繞M點(diǎn)轉(zhuǎn)動(dòng).然后連接A和B的繩子并以這些轉(zhuǎn)彎處的圓弧為支撐（這里轉(zhuǎn)彎處圓弧的半徑均按照最小轉(zhuǎn)彎半徑來(lái)計(jì)算），拉緊繩子，那么繩子的長(zhǎng)度就是A到B的最短距離.我們可以把路徑圖抽象為圖5.4的幾何圖形.下面我們對(duì)這段路徑求解：

圖5.4

如圖，A(x1,y1)是起點(diǎn)，B(x2,y2)是終點(diǎn)，O1(x3, y3)和O3(x4,y4)是兩個(gè)固定的圓，O2是一個(gè)可以繞點(diǎn)M(p,q)轉(zhuǎn)動(dòng)的圓環(huán)，三個(gè)圓的半徑均為r，C、D、E、F、G、H均為切點(diǎn).a、b、c、e、f分別是AO1、O1O2、AO2、AO3、O2O3的長(zhǎng)度.A、B、O1、O3均是已知點(diǎn)，O2是未知點(diǎn).那么最短路徑就可以表示為：

因?yàn)镺2點(diǎn)的坐標(biāo)未知，所以我們不能直接用模型5.1的線圓結(jié)構(gòu)對(duì)其進(jìn)行求解.故得先求出O2點(diǎn)的坐標(biāo).

設(shè)O2坐標(biāo)為(m，n),∠AO1C、∠AO1O2、∠AO2

O1、∠AO2O3、∠AO3O2分別為αi（i=1、2、3、4、5），∠C O1D、∠EO2F、∠EO2M分別為θ1、θ2、θ.這樣便有以下關(guān)系:

在Rt△AO1C中，有在△AO1O2中，有在△AO2O3中，有在Rt△NO2F中，有于是有,又因?yàn)镸O2一定會(huì)在∠EO2 F的角平分線上，所以有我們采用向量的形式來(lái)求，易知的一個(gè)方向向量與垂直，故其一個(gè)方向向量又有,=(p-m,q-n) 從 而 有 θ=arccos

綜合以上式子可以求得O2的坐標(biāo)，從而可以得出路徑的長(zhǎng)為

這可以采用模型5.1的線圓結(jié)構(gòu)來(lái)求解.

由于動(dòng)態(tài)問(wèn)題計(jì)算太復(fù)雜，我們對(duì)模型作了進(jìn)一步處理，為了保證機(jī)器人由起點(diǎn)A經(jīng)過(guò)M到B，我們考慮作下面的近似計(jì)算，設(shè)A(x1,y1)，B(x2,y2)，M (x3,y3)，動(dòng)點(diǎn)圓心O(a,b)，如圖5.5，則有從而轉(zhuǎn)化為線圓結(jié)構(gòu)來(lái)求解.

表6.1

圖5.5

假設(shè)機(jī)器人從起點(diǎn)O到目標(biāo)點(diǎn)P有有限種走法，每種走法由前面分析知路徑一定是由圓弧和線段組成，設(shè)有m條線段，n條圓弧，那么目標(biāo)函數(shù)可以表示為：

用此模型就可以對(duì)起點(diǎn)到目標(biāo)點(diǎn)之間的路徑進(jìn)行優(yōu)化求解.

對(duì)于問(wèn)題2，設(shè)從O經(jīng)過(guò)直線段OP、圓弧段PQ和直線段QA，我們可以建立從O到A所用的時(shí)間t與轉(zhuǎn)彎半徑ρ的函數(shù)關(guān)系：

建立數(shù)學(xué)優(yōu)化模型為

6 模型的求解

對(duì)問(wèn)題1，我們用窮舉法，結(jié)合mat lab編程算出可能走法的總路徑的最小值分述如下：

(1)要解決的是O到目標(biāo)點(diǎn)A的最短路徑，圖中畫(huà)出了機(jī)器人可能的路線只有兩條（如圖6.1），運(yùn)用mat lab編程計(jì)算（程序見(jiàn)mat lab.m），得出最短路徑為圖6.1標(biāo)識(shí)的粗線條，求得的具體數(shù)據(jù)見(jiàn)表6.1.

(2)要解決O到目標(biāo)點(diǎn)B的最短路徑，圖中標(biāo)出了可能的路徑（如圖6.2），并對(duì)每條路徑分析，運(yùn)用mat lab編程計(jì)算（程序見(jiàn)main12.m），得出最短路徑為圖6.2中標(biāo)識(shí)的粗線條，求出的具體數(shù)據(jù)見(jiàn)表6.2.

圖6.1

圖6.2

表6.2

（3）要解決O到目標(biāo)點(diǎn)C的最短路徑，圖中標(biāo)出了可能的幾條路徑（如圖6.3），并對(duì)每條路徑分析，從O到C的路徑中要繞過(guò)障礙物經(jīng)過(guò)動(dòng)點(diǎn)P，為了方便求解，我們把圓障礙物的半徑增加r，又把此條路徑轉(zhuǎn)化為內(nèi)公切線情形和外公切線的兩種情形，再運(yùn)用mat lab編程（程序見(jiàn)main13.m）計(jì)算出最短路徑為圖6.3中粗線條，求得具體數(shù)據(jù)見(jiàn)表6.3.

圖6.3

表6.3

（4）解決的是O→A→B→C→O得最短路徑，圖中標(biāo)出了可能的幾條路徑（如圖6.4），對(duì)每條路徑分析，從O→A→B→C→O的路徑中要繞過(guò)障礙物經(jīng)過(guò)動(dòng)點(diǎn)P，為方便求解我們把圓障礙物的半徑增加r，又把此條路徑轉(zhuǎn)化為模型建立中內(nèi)公切線和外公切線的兩種情形，再運(yùn)用mat lab編程（程序見(jiàn)main14.m和main15.m）計(jì)算，得出最短路徑為圖6.4中線條，求得具體數(shù)據(jù)見(jiàn)表6.4.

圖6.4

表6.4

（5）對(duì)于問(wèn)題2，我們用搜索法和mat lab編程（程序見(jiàn)main2.m）,求出結(jié)果如表6.5所示.

表6.5

7 模型評(píng)價(jià)與推廣

7.1 模型的優(yōu)點(diǎn)

1.用解析幾何計(jì)算簡(jiǎn)單易懂，精確度高；

2.運(yùn)用mat lab運(yùn)算出每個(gè)路徑進(jìn)行比較，得出最短路徑；

3.模型簡(jiǎn)單易懂，用實(shí)際檢驗(yàn)相當(dāng)吻合；4.模型的通用性強(qiáng)，適用范圍廣.

7.2 模型的不足

本題中有12個(gè)障礙物，是按照線圓結(jié)構(gòu)和線圓結(jié)構(gòu)的變化從起點(diǎn)到目標(biāo)點(diǎn)的路徑總是有限的，我們可以依次列舉出這些路徑進(jìn)行計(jì)算和比較大小，得到結(jié)果.如果平面場(chǎng)景圖如圖1.1的區(qū)域中有n個(gè)障礙物，那么按照線圓結(jié)構(gòu)和線圓結(jié)構(gòu)的變化從起點(diǎn)到目標(biāo)點(diǎn)的路徑就有無(wú)限多條，這樣我們用列舉法解決是無(wú)法完成的.所以就得找到更好的方法來(lái)解決這個(gè)問(wèn)題.窮舉法計(jì)算量大，且采用近似算法有一定的誤差.

7.3 模型推廣

由上述分析我們有了這樣一個(gè)想法：先求出所有的切線，包括出發(fā)點(diǎn)和目標(biāo)點(diǎn)到所有圓弧的切線以及所有圓弧與圓弧之間的切線，給這些定點(diǎn)賦一個(gè)等于切線長(zhǎng)度的權(quán)值，如果某兩條切線有一個(gè)公切圓弧，則代表這兩條曲線的頂點(diǎn)是一條直線的兩個(gè)端點(diǎn)，邊上的權(quán)值等于這兩條切線之間的劣弧長(zhǎng)度.然后在這張圖中加一個(gè)源點(diǎn)和終點(diǎn)，那么在所有代表出發(fā)點(diǎn)與其它圓弧之間切線的頂點(diǎn)與源點(diǎn)連成一條邊，權(quán)值均為0，同理在所有代表目標(biāo)點(diǎn)到其它圓弧切線的頂點(diǎn)與終點(diǎn)連成一條邊，權(quán)值均為0，這樣題目就轉(zhuǎn)化成了求源點(diǎn)到達(dá)終點(diǎn)之間的最短路徑問(wèn)題了，這里最短路徑就是指經(jīng)過(guò)所有頂點(diǎn)與邊的權(quán)值之和最小，我們可以采用Di jkst ra算法進(jìn)行求解.

注：此文獲2012年全國(guó)大學(xué)生數(shù)學(xué)建模競(jìng)賽國(guó)家二等獎(jiǎng).

[1]王正林，龔純，何倩.精通mat lab科學(xué)計(jì)算[M].北京：電子工業(yè)出版社，2009.

[2]全國(guó)大學(xué)生數(shù)學(xué)建模競(jìng)賽組委會(huì).全國(guó)大學(xué)生建模競(jìng)賽優(yōu)秀論文匯編[G].北京：中國(guó)物價(jià)出版社，2002.

[3]王晶，羅璋，鄧輝.基于切線網(wǎng)絡(luò)模型的機(jī)[EB/OL].（20 12-09-08）.http://www.docin.com/p-243098224.html.

[4]全國(guó)大學(xué)生數(shù)學(xué)建競(jìng)賽模組委會(huì).機(jī)器人行走問(wèn)題[EB/ OL].（201 2-09-07）.ht tp://wenku.baidu.com/viw/59f d857aa26925c52cc5df4c.html.

[5]王海英，黃強(qiáng)，李傳濤.圖論算法及其mat lab實(shí)現(xiàn)[M].北京：北京航空航天大學(xué)出版社，2010.

On theProblemsofRobotsAvoidingObstacle

W UYuan q ing,LIAOHui,D EN G Z hiyang,G UO W enj ing,LI N G Demei

(Sichuan V ocational and Technical Col lege,Suining Sichuan 629000)

This paper wants to solve the problems of the shor test path and time for robots avoiding obstacle.There are 12 smal l dif ferent shapes in a region that a robot cannot occur col l ision with them,it mainly researches the shortest path and the shor test time mathematical model for the robot. F or q uestion 1,we use analytic geomet ry and t rigonomet ric knowledge of elementary mathematics,set up a l ine and round st ructure mathematical model,we discuss it f rom three cases the internal common tangent,e x ternal common tangent,and the circle.F or the circle,we wi l l increase the radius of the circular obstacle r,or substitute the tangent angleby centeringangle,break it down into basic l ine and round st ructure to solve a mathematical model,using e x haustive method combined with M ATLA B programming to calculate theminimumvalueof the total pathof possiblemoves.F or q uestion2,we set up a time andwalking radiusmathematicalmodel,using the searchmethod combinedwith M ATLA B programming to calculate the shor test time.The resul ts:the shor test pathof O→Ais 471.0372,O→B is 858.60000, O→Cis 1093.70000,O→A→B→C→Ois 2783.70000,the shortest time of O→Ais 94.5649.

The Shor test Path;Search M ethod;M ATLA B;B asic Line and R ound St ructure;E lementary M athematics M ode

TP242

A

1672-2094(2013)02-0146-08

責(zé)任編輯：張隆輝

2013-02-10

吳元清(1964-),男,四川蓬溪人,四川職業(yè)技術(shù)學(xué)院應(yīng)用數(shù)學(xué)與經(jīng)濟(jì)系副教授.研究方向:應(yīng)用數(shù)學(xué).

廖 輝(1957-),男,四川蓬溪人,四川職業(yè)技術(shù)學(xué)院應(yīng)用數(shù)學(xué)與經(jīng)濟(jì)系副教授,碩士.研究方向:抽象代數(shù),數(shù)學(xué)分析.