2012年數(shù)學(xué)建模機(jī)器人避障問題

1、機(jī)器人避障問題摘要本文主要運(yùn)用直線逼近法等規(guī)律來解決機(jī)器人避障問題.對于問題一：要求最短路徑運(yùn)用直線逼近法證得圓弧角三角形定理，得出結(jié)論：若一大圓弧角三角形完全包括另一小圓弧角三角形，則該三角形曲線周長必大于小的三角形周長.那么可知機(jī)器人在曲線過彎時(shí)，選擇最小半徑可滿足路徑最短，即為10個(gè)單位半徑，通過觀察可得可能的所有曲線，通過僅考慮直線段的大致篩選選出總長較小、長度相近（之差小于100）的曲線，然后利用平面幾何知識對相關(guān)切點(diǎn)，進(jìn)而求出各直線、曲線的長度，求和可得最段路線.對于問題二：通過對機(jī)器人過彎規(guī)律的分析可知，當(dāng)過彎半徑時(shí)，機(jī)器人速度達(dá)最大速度為個(gè)單位/秒，再大就無變化了，那么可分兩種

2、情況考慮：1）當(dāng)時(shí)，過彎速度無變化，但由圓弧角三角形定理可知，此時(shí)隨著的不斷變大，其路線總長不斷變大，這時(shí)越小所用時(shí)間最短；2）當(dāng)時(shí)，統(tǒng)計(jì)計(jì)算分別為10、11、12、13時(shí)，過彎速度也不斷變化，計(jì)算所用時(shí)間發(fā)現(xiàn)隨不斷變大，所用時(shí)間越短，此時(shí)當(dāng)時(shí)，時(shí)間最短.綜合上述可知：當(dāng)時(shí)，時(shí)間最短.關(guān)鍵詞：質(zhì)點(diǎn)機(jī)器人 安全范圍 直線逼近法 圓弧角三角形定理 單位半徑1 問題重述在一個(gè)800800的平面場景中，在原點(diǎn)O(0, 0)點(diǎn)處有一個(gè)機(jī)器人，它只能在該平面場景范圍內(nèi)活動(dòng)，其中有12個(gè)不同形狀的區(qū)域是機(jī)器人不能與之發(fā)生碰撞的障礙物，障礙物的數(shù)學(xué)描述如下表：編號障礙物名稱左下頂點(diǎn)坐標(biāo)其它特性描述1正方形(3

3、00, 400)邊長2002圓形圓心坐標(biāo)(550, 450)，半徑703平行四邊形(360, 240)底邊長140，左上頂點(diǎn)坐標(biāo)(400, 330)4三角形(280, 100)上頂點(diǎn)坐標(biāo)(345, 210)，右下頂點(diǎn)坐標(biāo)(410, 100)5正方形(80, 60)邊長1506三角形(60, 300)上頂點(diǎn)坐標(biāo)(150, 435)，右下頂點(diǎn)坐標(biāo)(235, 300)7長方形(0, 470)長220，寬608平行四邊形(150, 600)底邊長90，左上頂點(diǎn)坐標(biāo)(180, 680)9長方形(370, 680)長60，寬12010正方形(540, 600)邊長13011正方形(640, 520)邊長8

4、012長方形(500, 140)長300，寬60在該平面場景中，障礙物外指定一點(diǎn)為機(jī)器人要到達(dá)的目標(biāo)點(diǎn)（要求目標(biāo)點(diǎn)與障礙物的距離至少超過10個(gè)單位）.規(guī)定機(jī)器人的行走路徑由直線段和圓弧組成，其中圓弧是機(jī)器人轉(zhuǎn)彎路徑.機(jī)器人不能折線轉(zhuǎn)彎，轉(zhuǎn)彎路徑由與直線路徑相切的一段圓弧組成，也可以由兩個(gè)或多個(gè)相切的圓弧路徑組成，但每個(gè)圓弧的半徑最小為10個(gè)單位.為了不與障礙物發(fā)生碰撞，同時(shí)要求機(jī)器人行走線路與障礙物間的最近距離為10個(gè)單位.機(jī)器人直線行走的最大速度為個(gè)單位/秒.機(jī)器人轉(zhuǎn)彎時(shí)，最大轉(zhuǎn)彎速度為，其中是轉(zhuǎn)彎半徑.如果超過該速度，機(jī)器人將發(fā)生側(cè)翻，無法完成行走.下面建立機(jī)器人從區(qū)域中一點(diǎn)到達(dá)另一點(diǎn)的避

5、障最短路徑和最短時(shí)間路徑的數(shù)學(xué)模型.對場景圖中4個(gè)點(diǎn)O(0, 0)，A(300, 300)，B(100, 700)，C(700, 640)，具體計(jì)算：(1) 機(jī)器人從O(0, 0)出發(fā)，OA、OB、OC和OABCO的最短路徑.(2) 機(jī)器人從O (0, 0)出發(fā)，到達(dá)A的最短時(shí)間路徑.2 問題分析2.1問題一：該問題要求路徑最短，即不要求速度與時(shí)間，則可認(rèn)為以最小半徑的圓過彎.如圖2.1所示：由圓弧角三角形定理（簡單證明見模型準(zhǔn)備5.3）可知過彎時(shí)，只有采用單位半徑過彎時(shí)，才會(huì)使得過彎路徑最短，因此解決問題一的過彎拐角問題均采用單位半徑過彎路徑.2.2問題二：由于OA過程中，機(jī)器人至少要經(jīng)過一

6、次轉(zhuǎn)彎；因?yàn)檗D(zhuǎn)彎時(shí)的速度一般小于直線行走的最大速度，又由分析指出轉(zhuǎn)彎次數(shù)越多，轉(zhuǎn)彎路徑越遠(yuǎn)，轉(zhuǎn)彎所花費(fèi)的時(shí)間也越長.所以可以確定有且只有一次轉(zhuǎn)彎時(shí)才存在最短時(shí)間路徑.就僅考慮只經(jīng)過一次轉(zhuǎn)彎的情形.3 模型假設(shè)1）假設(shè)機(jī)器人可準(zhǔn)確執(zhí)行運(yùn)動(dòng)軌道，無任何偏差；2）假設(shè)機(jī)器人為一可運(yùn)動(dòng)的質(zhì)點(diǎn)，即質(zhì)點(diǎn)機(jī)器人不考慮其外形尺寸；3）假設(shè)機(jī)器人的行進(jìn)速度可瞬時(shí)加減變化，不受條件限制；4）假設(shè)機(jī)器人可到達(dá)邊界線而不會(huì)發(fā)生碰撞，即對邊界線不再加10個(gè)單位.4 符號說明：機(jī)器人的行走路徑上各切點(diǎn)，表示路徑目的地（A、B、C），表示到達(dá)機(jī)器人行走路線的第種方案，表示機(jī)器人在該路線上所經(jīng)過的第個(gè)點(diǎn)；: 機(jī)器人的行走路徑

7、上的線段長或弧長，、同上定義；：機(jī)器人的行走路徑上的障礙物的頂點(diǎn)，、同上定義； ：機(jī)器人在環(huán)道中的各線切點(diǎn)、同上定義5 模型準(zhǔn)備5.1建立機(jī)器人運(yùn)動(dòng)坐標(biāo)系：以為原點(diǎn)，兩對應(yīng)坐標(biāo)軸，水平方向?yàn)檩S，垂直方向?yàn)檩S5.2建立機(jī)器人可安全運(yùn)動(dòng)到達(dá)的區(qū)域圖：由于保持安全距離個(gè)單位，則機(jī)器人的實(shí)際可到達(dá)到區(qū)域應(yīng)由各障礙物的外延10個(gè)單位的區(qū)域組成如圖所示圖5.2.1實(shí)線外的空白部分.5.3圓弧角三角形定理： 定義1：平面內(nèi)若兩不平行直線所夾的角被一同時(shí)與這兩條直線相切的圓弧段取代而形成的角，叫做圓弧角.如圖5.3.2，稱為凸圓弧角（本文主要討論）；如圖5.3.3，稱為凹圓弧角.定義2：由有一內(nèi)角為凸圓弧角的

8、三角形為圓弧角三角形.圓弧角三角形定理：圓弧角在直線及上方范圍完全包含圓弧角（即圓弧角DGD各邊均在圓弧角的邊與線段DD所構(gòu)成的封閉區(qū)間內(nèi)，如圖5.3.1所示）時(shí)，則有曲線段的長度恒小于曲線段成立.證明：如圖5.3.1，過圓弧的一個(gè)端點(diǎn)作該圓弧在該點(diǎn)的切線的垂線交曲線于點(diǎn)，同樣過圓弧的另一個(gè)端點(diǎn)也作相應(yīng)的垂線交曲線于點(diǎn)，兩條直線的交點(diǎn)顯然為圓弧所在圓的圓心.（1）； 曲線段， 曲線段.（2） ； 曲線段；曲線段.（3）將分成等份（如圖5.3.5），每部分（見圖5.3.4）中，是與邊界的交點(diǎn).令為，兩點(diǎn)間直線長度，為,兩點(diǎn)間直線長度，則圓弧長度=，曲線長度=又容易證明，故有 .因此，圓弧長度曲線

9、長度.綜合（1）（2）（3）的證明，得曲線段+曲線段+曲線長度 +圓弧長度.結(jié)論得證.6 模型建立與求解該問題要求路徑最短，即不要求速度與時(shí)間，則可認(rèn)為過彎半徑允許以最小半徑，如圖6.1所示.由圓弧角三角形定理可得：本論文問題一求路徑最短可采用10單位過彎半徑，即以半徑為10個(gè)單位的圓弧過彎可滿足兩點(diǎn)避障過彎最短問題.6.1問題一的模型建立與求解：6.1.1：機(jī)器人從O(0, 0)出發(fā)，的最短路徑.由圓弧角三角形定理可得：采用單位半徑過彎路徑最短，解決過彎避障拐角問題采用單位半徑過彎路徑.已知機(jī)器人所走路線為直線或圓弧，那么通過實(shí)際規(guī)劃可得如下四種避障行進(jìn)方案：如圖6.1.1首先對上述四條路線

10、進(jìn)行篩選：1） 當(dāng)機(jī)器人以一個(gè)連續(xù)圓弧過彎，即選擇路線二或路線四時(shí),其中路線二：分別過點(diǎn)和障礙物5的切點(diǎn)（72.74，216.88），則可得過該三點(diǎn)的圓的方程：顯然當(dāng)時(shí)，有不等的兩個(gè)根，則該路線超出規(guī)定場地.同理路線四的圓方程： (Matlab求解程序見程序01)當(dāng)時(shí)，有不等的兩個(gè)根，則該路線也超出規(guī)定場地.2）當(dāng)機(jī)器人以直線圓弧直線的方式過彎，即有以單位半徑過彎模式的線路一和三：比較線路一與線路三：顯然路線一的總長，線路三的總長.解得則可知OA的最短路徑為路線一總長為，下表5.1.1為線路一的各點(diǎn)的詳細(xì)參數(shù)，表6.1.2為各線的參數(shù).表6.1.1 點(diǎn)號坐標(biāo)50.1451.68301.6430

11、5.55 線號長度224.509.05237.49表6.1.2：6.1.2機(jī)器人從O(0, 0)出發(fā)，的最短路徑由圓弧角三角形定理可得：采用單位半徑過彎路徑最短：通過觀察可得如下四種較短的避障行進(jìn)方案，如圖6.1.2：由于方案較多，可預(yù)先進(jìn)行粗略篩選：如圖所示：大致統(tǒng)計(jì)長度僅包括直線段長度如下表6.1.2表6.1.2線段編 號路線號123456總長路線一305.78162.2573.656096.95111.36810路線二224.50175.70230.5096.95111.36839路線三235.37240.05230.5096.95111.36914.22路線四235.37620.851

12、70.301026.52則由上表可知：路線一和路線二相差不大，且路線較短，則可進(jìn)一步僅考慮該兩路線的精確長度：設(shè)：、 分別表示點(diǎn)到點(diǎn)，點(diǎn)到點(diǎn)之間的向量；為、兩點(diǎn)之間的向量的模；表示切點(diǎn)坐標(biāo)；記為障礙物頂點(diǎn)的坐標(biāo)； 聯(lián)立方程解得(50.14,30.64)由于點(diǎn)，分別是以點(diǎn)，為圓心為半徑圓的外公切線切點(diǎn)，所以由點(diǎn)到直線的距離公式得 并且線段 由于直線由斜率相等得 聯(lián)立方程解得點(diǎn)的坐標(biāo)(51.6795,305.547)的坐標(biāo)(141.68,440.55)線路一和線路二的各段路線及總長分別如下表6.1.2,6.1.3表6.1.2總長長度305.784.24162.257.7573.6515.71609

13、.8996.956.15111.36853.73表6.1.3總長長度224.58.37178.1212.22230.499.8996.956.15111.36878.05由上兩表可知：線路一總長最短.同理可解得各點(diǎn)坐標(biāo)如下表6.2.4表6.1.450.1451.68141.68147.94220230230225.5144.5140.69301.64305.55440.55444.78460470530538.35591.65596.35的最短路徑為：6.1.3機(jī)器人從O(0, 0)出發(fā)，的最短路徑由圓弧角三角形定理可得：采用單位半徑過彎路徑最短：通過觀察可得如下避障行進(jìn)方案，如圖6.1.3由

14、于該線路同樣較復(fù)雜，可通過大致篩選，僅考慮其中的直線段長度.將通過障礙物1上邊沿的線路稱為上線路，通過下邊沿的線路稱為下線路1） 考慮上線路中最短路徑：上線路中如圖6.1.3.1分兩大段，上半段：線路A、B、C，下半段：線路D、E對上半段的線路進(jìn)行只計(jì)算線段的粗篩選：計(jì)算統(tǒng)計(jì)可得三線路的粗選長度：如表6.1.5線段號路線號1234線段A(695.94)306.6957162.249875.6637151.3275線段B(728.32)225.4398178.1151306.3087線段C(780.77)237.54543.23如表統(tǒng)計(jì)可知，線段A距離最短.對下半段的線路進(jìn)行只計(jì)算線段的粗篩選：

15、計(jì)算統(tǒng)計(jì)可得三線路的粗選長度：如表6.1.6表6.1.6線段號路線號12345總長線段D151.33119.16413094.34494.834線段E200160808043.59563.59如表統(tǒng)計(jì)可知，線段D距離最短2） 考慮下線路中最短路徑：如圖圖6.1.3.2對下線路的線路進(jìn)行只計(jì)算線段的篩選：計(jì)算統(tǒng)計(jì)可得線路的長度： 下表6.1.7為路線一的各段線路總長對于同一條路徑上的兩個(gè)相鄰點(diǎn)、來說，如果這兩點(diǎn)之間的路徑為直線段時(shí)，用通式計(jì)算；如果這兩點(diǎn)之間的路徑為弧線段時(shí)，可用通式計(jì)算：表6.1.7總長長度237.540184.47.31330.73387.86.5806.943.6950.8

16、4下表6.2.8為路線二的各段線路總長表6.1.8長度237.548.5293318.43378.8448169.705647.5281707.8958806.891743.589237.548.5293總長960.5814下線路的兩段線路對比得：線路一最短為：950.84綜合上線段、下線段可得：線路一最短.各切點(diǎn)坐標(biāo)如下表6.1.9表6.1.9切點(diǎn)號232.9412.6418.7492492.4728.3730.4730.4728.151.291.195.3206206.9514.8520.9600.9607.2的最短路徑為：6.1.4機(jī)器人從O(0, 0)出發(fā)，的最短路徑由圓弧角三角形定理

17、可得：采用單位半徑過彎路徑最短：6.1.4.1的最短路徑求解：通過實(shí)際規(guī)劃可得如下的避障行進(jìn)最短方案：如圖6.1.4.16.1.4.2的最短路徑求解：通過實(shí)際規(guī)劃可得如下的避障行進(jìn)最短方案：如圖6.1.4.1對線路一、二進(jìn)行大致選可得下表線段編號路線號12345總長路線一97.9860119.1613094.34501.48路線二243.52160808043.59607.11表6.1.10則可知路線一距離最近對于同一條路徑上的兩個(gè)相鄰點(diǎn)、來說，如果這兩點(diǎn)之間的路徑為直線段時(shí)，用通式計(jì)算；如果這兩點(diǎn)之間的路徑為弧線段時(shí)，可用通式計(jì)算：6.1.4.3線路經(jīng)過、的圓弧處理問題為使經(jīng)過、的圓弧路線最

18、短，在A與相鄰切點(diǎn)的連線形成的夾角的平分線，以該角的平分線為基礎(chǔ)，在該線上做與點(diǎn)A相切的半徑為10個(gè)單位的圓，則此時(shí)通過該構(gòu)造圓與相鄰圓弧的切線連接就產(chǎn)生了，進(jìn)而保證了機(jī)器人的圓弧過彎和線路最短.點(diǎn)A的圓弧處理結(jié)果如圖6.1.4.3則綜上所述：求得各線短的最短路徑，則可計(jì)算并統(tǒng)計(jì)出線段總長及各切點(diǎn)坐標(biāo)如下表表6.1.11切點(diǎn)號12345678910111270.50677.797294.34300.597229.54144.5140.8698.96108.96270.87368.01435.59213.14219.7295.03307.485533.01591.65595.93690.0670

19、4.09689.96670.2671.71切點(diǎn)號1314151617181920212223534.41679.77699.21701.31727.67730.3851727.94492.06418.35412.17232.17738.29732.11642.27637.97606.41520513.92206.0894.4990.2450.24表6.1.12線序號123456789101112長度224.5230.3815.417236.459.8896.955.7103.0320.767162.5297.9860線序號131415161718192021222324長度5.92119.16

20、5.9213013.5891.924.8341.137.85806.54387.81線序號252627長度7.685184.4237.49的最短路徑為6.2問題二的模型建立與求解：由于OA過程中，機(jī)器人至少要經(jīng)過一次轉(zhuǎn)彎；因?yàn)檗D(zhuǎn)彎時(shí)的速度一般小于直線行走的最大速度，又由分析指出轉(zhuǎn)彎次數(shù)越多，轉(zhuǎn)彎路徑越遠(yuǎn)，轉(zhuǎn)彎所花費(fèi)的時(shí)間也越長.所以可以確定有且只有一次轉(zhuǎn)彎時(shí)才存在最短時(shí)間路徑.故以下就僅考慮只經(jīng)過一次轉(zhuǎn)彎的情形.機(jī)器人由起點(diǎn)到終點(diǎn)所用時(shí)間，對于每種固定的轉(zhuǎn)彎半徑來說，轉(zhuǎn)彎路徑所在的圓的圓心與點(diǎn)連線垂直平分該轉(zhuǎn)彎路徑所在的圓弧時(shí)，所得的總路徑長度最短.如圖6.2.1所示.對于已知條件中的最大轉(zhuǎn)彎

21、速度為 ，其中是轉(zhuǎn)彎半徑通過matlab畫出其圖像，見程序02 如圖6.2.2根據(jù)圖6.1中所示，當(dāng)時(shí)，隨增加而增加；當(dāng)時(shí)，已非常趨近于5單位/秒，此時(shí)可以看做不隨增加而變化了.于是可以分兩種情況解決本問題：1)當(dāng)時(shí)，由于OA整個(gè)過程的平均速度可以達(dá)到最大單位/秒，以這樣的速度沿最小的路徑就可以使到達(dá)A的時(shí)間最短.通過問題一中對機(jī)器人OA最短路徑的分析，可知其最小時(shí)間路徑應(yīng)在連線左上方區(qū)域；同時(shí)根據(jù)所建立并證明的圓弧角三角形定理可以知道，所得路徑的轉(zhuǎn)彎半徑應(yīng)為13個(gè)單位（如圖6.2.3）,總長度：總時(shí)間：（秒）2)當(dāng)時(shí)，圖6.2取自原題目圖中的一部分，其中，點(diǎn)的坐標(biāo)均已給出.、分別為OQ1和O

22、Q2與的切點(diǎn)，其中又與相切于點(diǎn).假設(shè)半徑已知，、的坐標(biāo)分別為、，則可列出如下方程組：分別取=10，11，12，13并解方程組可以得到總時(shí)間隨轉(zhuǎn)彎半徑變化的數(shù)據(jù)，根據(jù)弧長公式得,最終計(jì)算數(shù)據(jù)如下表：6.2.1表6.2.1項(xiàng)目轉(zhuǎn)彎速度（單位/秒）總時(shí)間（秒）10224.49958.8405237.69732.595.975511224.20319.966236.89134.45594.455912223.773310.8816236.43654.93994.245213236.139211.8001223.1903594.2259通過比較總時(shí)間的變化趨勢可以知道，當(dāng)時(shí)，總時(shí)間隨增加而增加，最終會(huì)趨于94.22秒.因此，可以確定出最短時(shí)間路徑.經(jīng)過以上兩種情況的討論，可得最短時(shí)間路徑， 具體坐標(biāo)信息見表6.2.2表6.2.2坐標(biāo)起點(diǎn)切點(diǎn)切點(diǎn)終點(diǎn)圓弧圓心0130.5224138.4764300142.86850226.6562234.8207300222.58517 模型的評價(jià)與推廣7.1優(yōu)點(diǎn)：