機(jī)器人避障問題的解題分析建模集訓(xùn)

1、-機(jī)器人避障問題的解題分析摘要：本文對(duì)2021年全國(guó)大學(xué)生數(shù)學(xué)建模競(jìng)賽D題機(jī)器人避障問題進(jìn)展了全面分析，對(duì)最短路的設(shè)計(jì)進(jìn)展了理論分析和證明，建立了機(jī)器人避障最短路徑的幾何模型，對(duì)最短時(shí)間路徑問題通過建立非線性規(guī)劃模型，有效地解決了轉(zhuǎn)彎半徑、圓弧圓心位置和行走時(shí)間等問題。關(guān)鍵詞：機(jī)器人避障；最短路徑；Dijkstra算法；幾何模型；非線性規(guī)劃模型1 引言隨著科學(xué)技術(shù)的進(jìn)步和計(jì)算機(jī)技術(shù)的開展，機(jī)器人的應(yīng)用越來越廣泛，在機(jī)器人的應(yīng)用中如何使機(jī)器人在其工作圍為完成一項(xiàng)特定的任務(wù)尋找一條平安高效的行走路徑，是人工智能領(lǐng)域的一個(gè)重要問題。本文主要針對(duì)在一個(gè)場(chǎng)景中的各種靜態(tài)障礙物，研究機(jī)器人繞過障礙物到達(dá)指

2、定目的地的最短路徑問題和最短時(shí)間問題。本文以2021年“高教社杯全國(guó)大學(xué)生數(shù)學(xué)建模競(jìng)賽D題“機(jī)器人避障問題為例進(jìn)展研究。假設(shè)機(jī)器人的工作圍為800×800的平面正方形區(qū)域如圖1，其中有12個(gè)不同形狀的靜態(tài)障礙物，障礙物的數(shù)學(xué)描述如表1：圖1 800×800平面場(chǎng)景圖表1編號(hào)障礙物名稱左下頂點(diǎn)坐標(biāo)其它特性描述1正方形(300,400)邊長(zhǎng)2002圓形圓心坐標(biāo)(550,450)，半徑703平行四邊形(360,240)底邊長(zhǎng)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)邊長(zhǎng)1506

3、三角形(60,300)上頂點(diǎn)坐標(biāo)(150,435)，右下頂點(diǎn)坐標(biāo)(235,300)7長(zhǎng)方形(0,470)長(zhǎng)220，寬608平行四邊形(150,600)底邊長(zhǎng)90，左上頂點(diǎn)坐標(biāo)(180,680)9長(zhǎng)方形(370,680)長(zhǎng)60，寬12010正方形(540,600)邊長(zhǎng)13011正方形(640,520)邊長(zhǎng)8012長(zhǎng)方形(500,140)長(zhǎng)300，寬60在原點(diǎn)O(0,0)點(diǎn)處有一個(gè)機(jī)器人，它只能在該平面場(chǎng)景圍活動(dòng)，機(jī)器人不能與障礙物發(fā)生碰撞，障礙物外指定一點(diǎn)為機(jī)器人要到達(dá)的目標(biāo)點(diǎn)。規(guī)定機(jī)器人的行走路徑由直線段和圓弧組成，其中圓弧是機(jī)器人轉(zhuǎn)彎路徑。機(jī)器人不能折線轉(zhuǎn)彎，轉(zhuǎn)彎路徑由與直線路徑相切的一段圓

4、弧組成，也可以由兩個(gè)或多個(gè)相切的圓弧路徑組成，但每個(gè)圓弧的半徑最小為10個(gè)單位。為了不與障礙物發(fā)生碰撞，同時(shí)要求機(jī)器人行走線路與障礙物間的最近距離為10個(gè)單位，否則將發(fā)生碰撞，假設(shè)碰撞發(fā)生，則機(jī)器人無法完成行走。機(jī)器人直線行走的最大速度為個(gè)單位/秒。機(jī)器人轉(zhuǎn)彎時(shí)，最大轉(zhuǎn)彎速度為是轉(zhuǎn)彎半徑。如果超過該速度，機(jī)器人將發(fā)生側(cè)翻，無法完成行走。場(chǎng)景圖中有4個(gè)目標(biāo)點(diǎn)O(0,0)，A(300,300)，B(100,700)，C(700,640)，下面我們將研究機(jī)器人從O(0,0)出發(fā)，求OA、OB、OC和OABCO的最短路徑，以及機(jī)器人從O(0,0)出發(fā)，到達(dá)A的最短時(shí)間路徑問題。2 靜態(tài)避障問題中機(jī)器人

5、行走最短路徑的分析2.1 行走路徑的設(shè)計(jì)在本例中障礙物有4種不同形狀：矩形、平行四邊形、三角形和圓形?？紤]到機(jī)器人本身的形狀和大小，為研究方便起見，將機(jī)器人視為一個(gè)點(diǎn)。機(jī)器人與障礙物之間的距離至少為10個(gè)單位，因此可以先用包絡(luò)線畫出機(jī)器人行走的危險(xiǎn)區(qū)域如圖2，包絡(luò)線是機(jī)器人的禁入?yún)^(qū)。圖2 障礙物包絡(luò)圖對(duì)障礙物的一個(gè)角點(diǎn)來說，其禁入?yún)^(qū)的邊界應(yīng)由兩條直線和一條圓弧組成，兩條直線分別平行于角點(diǎn)的兩條邊，間距為10個(gè)單位，圓弧是以障礙物角點(diǎn)為圓心，半徑為10個(gè)單位的四分之一圓弧?？梢宰C明具有圓形限定區(qū)域的最短路徑由兩局部組成，一局部是平面上的自然最短路徑直線段，另一局部是限定區(qū)域的局部邊界即繩子拉到最

6、緊時(shí)的圓弧局部，這兩局部是相切的，互相連接如圖3所示。由A繞過半圓形障礙物到達(dá)B點(diǎn)的路徑有多條，其中最短路徑為(E、F為切點(diǎn))，其他路徑與AB直線圍成的區(qū)域都覆蓋這一路徑與AB直線圍成的區(qū)域，由此證明1。圖3由此可以確定機(jī)器人的行走路徑應(yīng)為線圓構(gòu)造，則是否是轉(zhuǎn)彎半徑越小，行走路徑就越短呢.為此需要求在兩個(gè)固定點(diǎn)和圓弧圓心坐標(biāo)的情況下，圓弧半徑r為何值，才能使機(jī)器人的行走路徑最短。圖4如圖4，兩個(gè)固定點(diǎn)，圓心，可以求得兩切點(diǎn)坐標(biāo)，設(shè)半徑為，圓弧所對(duì)的圓心角為，的路徑長(zhǎng)度為,則將路徑函數(shù)對(duì)求導(dǎo)，得因?yàn)?，所以.，則函數(shù)為單調(diào)遞增函數(shù)，因此當(dāng)圓弧半徑逐漸增加時(shí)，機(jī)器人的行走路徑會(huì)增大，逐漸降低時(shí)，機(jī)

7、器人的行走路徑會(huì)減小2，此題規(guī)定轉(zhuǎn)彎半徑最小為10個(gè)單位，所以在路徑設(shè)定時(shí)應(yīng)將轉(zhuǎn)彎半徑設(shè)定為最小值10個(gè)單位。根據(jù)以上分析，對(duì)于靜態(tài)障礙物機(jī)器人的行走路徑應(yīng)遵循以下三個(gè)原則：原則一：機(jī)器人的行走路徑為線圓構(gòu)造，由兩條切線和一段圓弧組成；原則二：每個(gè)路口至多發(fā)生一次轉(zhuǎn)彎，并以障礙物頂點(diǎn)為轉(zhuǎn)彎圓弧的中心；原則三：機(jī)器人轉(zhuǎn)彎圓弧半徑為最小允許半徑10個(gè)單位。2.2 最短路徑的選擇 從起點(diǎn)到達(dá)目標(biāo)點(diǎn)有多條路徑，根據(jù)Dijkstra算法可以找出從起點(diǎn)到達(dá)每一個(gè)目標(biāo)點(diǎn)的最短路徑。本文采用帶權(quán)的有向圖表示機(jī)器人的行走路徑，途中節(jié)點(diǎn)為障礙物的角點(diǎn)，邊表示障礙物之間的聯(lián)系，權(quán)表示線路的長(zhǎng)度節(jié)點(diǎn)之間的直線距離。從

8、頂點(diǎn)出發(fā)，沿圖的邊到達(dá)另一頂點(diǎn)所經(jīng)過的路徑中，各邊上權(quán)值之和最小的一條路徑就是所求最短路徑，Dijkstra算法就是按路徑的長(zhǎng)度遞增次序產(chǎn)生最短路徑的算法3。下面以為例，確定的最短路徑。如圖5所示，根據(jù)障礙物的形狀和位置，本文給出了機(jī)器人從O(0,0)出發(fā)避過障礙物到達(dá)目標(biāo)B點(diǎn)的4條較優(yōu)路徑。 圖5畫出的非循環(huán)網(wǎng)絡(luò)圖如圖6：B621306O230601121712259921647017978162238B5B3B6B2B4B1B9B77B8017111299569237399705159508329圖6運(yùn)用Dijkstra算法算出的最短路徑，最短路算法如下：1、 起點(diǎn)O記為,終點(diǎn)B記為；2、

9、 從網(wǎng)絡(luò)的終點(diǎn)開場(chǎng)，令它的標(biāo)號(hào)為零，并用方框記錄在圖6中；3、 計(jì)算結(jié)點(diǎn)的標(biāo)號(hào)，設(shè)結(jié)點(diǎn)已標(biāo)號(hào)，結(jié)點(diǎn)指向，則的標(biāo)號(hào)可按算式：求出，其中是的標(biāo)號(hào)，是結(jié)點(diǎn)與之間的直線距離；4、 重復(fù)上述計(jì)算，直到求得起點(diǎn)的標(biāo)號(hào)為止，此標(biāo)號(hào)即為最短路的長(zhǎng)度；5、 確定最短路徑，從起點(diǎn)開場(chǎng)，順網(wǎng)絡(luò)的箭線前進(jìn)，假設(shè)有幾條箭線，則選取箭線所指標(biāo)號(hào)最小且滿足條件的結(jié)點(diǎn)為最短路徑所經(jīng)過的結(jié)點(diǎn)。在圖6中，最短路徑為：.應(yīng)用上述算法可得到從點(diǎn)出發(fā)，分別到達(dá)各目標(biāo)點(diǎn)的最短路徑：圖7的最短路徑為： 如圖7圖8的最短路徑為：如圖8圖9的最短路徑為:3 最短路徑計(jì)算模型3.1 單個(gè)目標(biāo)點(diǎn)的最短路徑根據(jù)前面制定的行走路徑原則，起點(diǎn)到目標(biāo)點(diǎn)無

10、論中間障礙物有多少，最短路徑都應(yīng)該是假設(shè)干個(gè)線圓構(gòu)造所組成，圓弧中心為障礙物的頂點(diǎn)，半徑為機(jī)器人轉(zhuǎn)彎最小半徑10個(gè)單位。觀察這四條路徑，發(fā)現(xiàn)所有行走路徑都可歸結(jié)為以下三種類型：類型一 圖10線圓構(gòu)造1如圖10，設(shè)O為起點(diǎn)，A為目標(biāo)點(diǎn)，C和D分別為直線與轉(zhuǎn)彎圓弧的切點(diǎn)，障礙物的頂點(diǎn)即轉(zhuǎn)彎圓弧的圓心，圓的半徑為，的長(zhǎng)度為，的長(zhǎng)度為，的長(zhǎng)度為，設(shè)的長(zhǎng)度為L(zhǎng)，則，由圖10可得以下關(guān)系：在中：在中：在中：所以：從而可得：這個(gè)模型運(yùn)算簡(jiǎn)潔，只需將起點(diǎn)、目標(biāo)點(diǎn)和障礙物頂點(diǎn)坐標(biāo)輸入模型，MATLAB就能很快計(jì)算出來4，計(jì)算程序見附錄1。類型二：對(duì)于圖11這種線圓構(gòu)造，需要做簡(jiǎn)單的變換，才能求出的路徑長(zhǎng)度。 圖

11、11線圓構(gòu)造2 假設(shè)兩圓心坐標(biāo)分別為和,M點(diǎn)為兩圓心連線和兩圓公切線的交點(diǎn)，坐標(biāo)為，則很容易可以求得:這樣就可以利用類型一中的方法，先求A到M的長(zhǎng)度，再求M到B的長(zhǎng)度，分兩段就可以求解。同理如果有更多的轉(zhuǎn)彎，同樣可以按照此種方法分解。 類型三圖12 線圓構(gòu)造3如圖12，如果兩圓弧的公切線平行于兩圓圓心連線，求的路徑長(zhǎng)度。設(shè)各點(diǎn)坐標(biāo)分別為起點(diǎn)A (,),目標(biāo)點(diǎn)B(),障礙物頂點(diǎn),障礙物頂點(diǎn)),半徑為分別是的長(zhǎng)度，設(shè)的長(zhǎng)度為,則解法如下：由圖12，可以得到以下關(guān)系：a=,b=,c =, ,在中，由余弦定理可得：在中,=arccos所以：同理：；=arccos；則運(yùn)用MATLAB進(jìn)展計(jì)算，MATLA

12、B計(jì)算程序見附錄2.3.2 多個(gè)目標(biāo)點(diǎn)的最短路徑 機(jī)器人從起點(diǎn)出發(fā)，依次經(jīng)過指定的中間目標(biāo)點(diǎn)最后到達(dá)終點(diǎn)，是多個(gè)目標(biāo)點(diǎn)的最短路徑問題。比方的最短路徑的計(jì)算。由于機(jī)器人的行走路線為線圓構(gòu)造，不能折線轉(zhuǎn)彎，因此中間目標(biāo)點(diǎn)應(yīng)位于*個(gè)半徑為的圓周上，這里我們?nèi)园凑兆钚≡试S半徑為10個(gè)單位，則只需計(jì)算出過A、B、C三點(diǎn)的圓心位置即可，這樣就將多目標(biāo)點(diǎn)的最短路徑問題轉(zhuǎn)化成了單目標(biāo)點(diǎn)的最短路徑問題。求過A、B、C三點(diǎn)的圓心位置的問題可通過建立非線性規(guī)劃模型求得。（1） 過A點(diǎn)圓弧的圓心圖13如圖13，障礙物頂點(diǎn),頂點(diǎn),，切點(diǎn)，過A的圓弧圓心，最短路徑為,則建立非線性規(guī)劃模型s.t. 運(yùn)用LINGO13編程計(jì)

13、算，計(jì)算程序見附錄3.（2） 過B點(diǎn)圓弧的圓心圖14如圖14，障礙物頂點(diǎn),頂點(diǎn),，過B的圓弧圓心，最短路徑為,則建立非線性規(guī)劃模型運(yùn)用LINGO13編程計(jì)算，計(jì)算程序見附錄4.（3） 過C點(diǎn)圓弧的圓心圖15如圖15，障礙物頂點(diǎn),頂點(diǎn),，過C的圓弧圓心，圓與圓的公切線為,切點(diǎn)，圓與圓的公切線為,切點(diǎn)，最短路徑為,則建立非線性規(guī)劃模型運(yùn)用LINGO13編程計(jì)算，計(jì)算程序見附錄5.運(yùn)用LINGO對(duì)模型求解，過A、B、C的圓弧圓心坐標(biāo)計(jì)算結(jié)果(如表2)：表2 過A、B、C的圓弧圓心坐標(biāo)經(jīng)過點(diǎn)圓弧的圓心坐標(biāo)A290.8854，304.1140B107.3884，693.2612C709.6622，637

14、.4229 3.3切點(diǎn)坐標(biāo)的計(jì)算模型要準(zhǔn)確計(jì)算出機(jī)器人行走路徑的長(zhǎng)度，必須要知道每一段圓弧的起點(diǎn)和終點(diǎn)，即切點(diǎn)坐標(biāo)，通過觀察上述三種線圓構(gòu)造的切點(diǎn)主要有兩種類型，一種是兩圓圓心連線與公切線相交，另一種是兩圓圓心連線與公切線平行。（1） 第一種類型的切點(diǎn)兩圓圓心連線與公切線相交，則圓心連線的中點(diǎn)在切線上，可由兩圓圓心坐標(biāo)確定中點(diǎn)坐標(biāo)，此問題就可以轉(zhuǎn)化為求圓弧外的點(diǎn)與障礙物的轉(zhuǎn)彎圓弧形成的切線的切點(diǎn)如圖16圖16設(shè)切點(diǎn),起點(diǎn)O(,),圓心M(,)，求切點(diǎn)C的坐標(biāo)在中由勾股定理可得：，即又因?yàn)榍悬c(diǎn)C在圓M上，故聯(lián)立方程組運(yùn)用MATLAB解方程，求出切點(diǎn)的坐標(biāo), MATLAB程序見附錄6。 （2） 第

15、二種類型的切點(diǎn)圖17兩圓連線與公切線平行如圖17，設(shè)切點(diǎn),圓心,圓心()，半徑為,求切點(diǎn)的坐標(biāo)。解法如下：直線的斜率為，的直線方程為，因?yàn)?所以DE的直線的斜率也為在DE直線上找一點(diǎn)，則DE直線方程為,即，又因?yàn)榍悬c(diǎn)D在圓上，滿足圓的方程，故，建立方程組，解方程可求得D點(diǎn)的坐標(biāo), MATLAB程序見附錄7。如果公切線在障礙物中心連線的下方，模型需要做以下變換再計(jì)算。根據(jù)以上模型可計(jì)算出、以及的所有切點(diǎn)坐標(biāo)，直線段長(zhǎng)度和圓弧長(zhǎng)度，計(jì)算結(jié)果見附錄8。4 最短時(shí)間路徑模型的建立和求解機(jī)器人的行走速度與轉(zhuǎn)彎半徑有關(guān)，假設(shè)行走速度與轉(zhuǎn)彎半徑之間滿足其中為直線行走速度，則與最短路徑問題不同，轉(zhuǎn)彎半徑不再是

16、越小越好，轉(zhuǎn)彎半徑越小，雖然行走的距離也越短，但是速度會(huì)變慢，這樣行走速度反而可能會(huì)增加，因此，應(yīng)選擇一個(gè)適當(dāng)?shù)霓D(zhuǎn)彎半徑，使得行走時(shí)間最短5。 以為例，研究最短時(shí)間路徑問題。以機(jī)器人從原點(diǎn)出發(fā)到達(dá)點(diǎn)的時(shí)間最少為目標(biāo)建立優(yōu)化模型。轉(zhuǎn)彎半徑越大速度越快，走最短距離的時(shí)間不一定是最短的到達(dá)時(shí)間，因此應(yīng)對(duì)轉(zhuǎn)彎半徑、轉(zhuǎn)彎所走的圓弧的圓心進(jìn)展重新搜素，建立非線性規(guī)劃模型6。圖18如圖18，起點(diǎn)，目標(biāo)點(diǎn)，障礙物5的頂點(diǎn)，切點(diǎn)，轉(zhuǎn)彎圓弧的圓心，圓心角為，半徑為，的路徑為，時(shí)間為.則建立目標(biāo)函數(shù)編寫LINGO程序，應(yīng)用LINGO13求解，計(jì)算程序見附錄9，計(jì)算結(jié)果如表3：表3的最短時(shí)間路徑路徑起點(diǎn)坐標(biāo)終點(diǎn)坐標(biāo)圓

17、弧圓心坐標(biāo)圓弧半徑總路程總時(shí)間0.00000.000069.8049212.7391469.896894.340569.8049212.739176.9877220.117880.9394209.085611.718676.9877220.1178300.0000300.00005 模型的評(píng)價(jià)與推廣5.1 模型的優(yōu)點(diǎn)1將機(jī)器人避障行走路線用假設(shè)干個(gè)線圓構(gòu)造組成建立的模型各點(diǎn)坐標(biāo)和長(zhǎng)度都能直接得出結(jié)果，用解析幾何方法進(jìn)展計(jì)算，準(zhǔn)確度較高。2運(yùn)用多個(gè)方案進(jìn)展優(yōu)化，在相對(duì)優(yōu)化中能取得最優(yōu)解。3模型簡(jiǎn)單易懂，便于實(shí)際檢驗(yàn)及應(yīng)用。5.2 模型的缺點(diǎn)1此模型需要全局優(yōu)化來求解，求解結(jié)果往往因?yàn)榈a(chǎn)生一定

18、的誤差，但是這個(gè)誤差在可允許的圍。2在障礙物較多時(shí)，且形狀不規(guī)則時(shí)，模型顯得較為繁瑣。非線性變量越來越多會(huì)導(dǎo)致求解時(shí)間越來越長(zhǎng)，解的可求性也越來越差。5.3 模型的改進(jìn)及推廣此題只涉及12個(gè)障礙物，如果障礙物較多，到達(dá)目標(biāo)點(diǎn)的路徑就較多，這時(shí)可應(yīng)用網(wǎng)絡(luò)模型計(jì)算最短路。如果障礙物形狀較復(fù)雜，單純用解析幾何知識(shí)計(jì)算較困難，模型需要進(jìn)一步改進(jìn)。機(jī)器人避障模型可以應(yīng)用于貨物運(yùn)輸、管道輸送等領(lǐng)域，應(yīng)用此模型能較好地解決運(yùn)輸線路最短、輸送管道最短等問題。參考文獻(xiàn)1 百度文庫(kù).行走機(jī)器人避障問題:2012-09-08wenku.baidu./view/251ee65677232f60ddcca17b.htm

19、l.2 百度文庫(kù).關(guān)于機(jī)器人避障行走問題的研究:2013-02-28 wenku.baidu./view/645d1f07bed5b9f3f90f1c4b.html.3邦迪.圖論及其應(yīng)用M.:科學(xué),1984.4 章棟恩，馬玉蘭.MATLAB高等數(shù)學(xué)實(shí)驗(yàn)M.：電子工業(yè)，2021.5蔡志杰.機(jī)器人避障問題J.數(shù)學(xué)建模及其應(yīng)用,2021,2(1):53-59.6 王琦.線性-二次雙層規(guī)劃的滿意解與基于LP與NLP過程的算法J.系統(tǒng)工程理論與實(shí)踐,2007,27(8).7 周志明.LINGO及其在化工過程優(yōu)化中的應(yīng)用J.計(jì)算機(jī)與應(yīng)用化學(xué), 2021, 27(7).8 夏伯男.基于最短時(shí)間的公交乘車路徑

20、查詢模型J.工業(yè)大學(xué)學(xué)報(bào),2021, 30(2).9譚永基.數(shù)學(xué)模型.:復(fù)旦大學(xué)，2021.附錄1、線圓構(gòu)造類型一的MATLAB程序例如：求圖7中的最短路徑，為，半徑，起點(diǎn)，目標(biāo)點(diǎn),障礙物頂點(diǎn)，運(yùn)用MATLAB計(jì)算得的最短距離為471.0372，MATLAB算法如下：在MATLAB中編寫M文件：fun.mfunction L=fun(*1,y1,*2,y2,*3,y3)a=sqrt(*2-*1)2+(y2-y1)2);b=sqrt(*3-*1)2+(y3-y1)2);c=sqrt(*3-*2)2+(y3-y2)2);alpha1=acos(b2+c2-a2)/(2*b*c);alpha2=ac

21、os(10/b);alpha3=acos(10/c);theta=2*pi-alpha1-alpha2-alpha3;L=sqrt(b2-102)+sqrt(c2-102)+10*theta;在命令窗口鍵入：fun(0,0,300,300,80,210)ans =471.03722、線圓構(gòu)造類型三的MATLAB程序比方圖9中計(jì)算從起點(diǎn)繞過障礙物5，障礙物4，到障礙物4與障礙物12的中點(diǎn)的路徑長(zhǎng)度，起點(diǎn),障礙物5的頂點(diǎn)230，60，障礙物4的頂點(diǎn)410,100，障礙物4與障礙物12的中點(diǎn)455,150。編寫MATLAB程序輸入起點(diǎn)、目標(biāo)點(diǎn)、兩障礙物頂點(diǎn)坐標(biāo)及半徑，即可計(jì)算出路徑長(zhǎng)度為496.86

22、96。 編寫M文件：fun1.mFunctionL=fun1(*1,y1,*2,y2,*3,y3,*4,y4) a=sqrt(*4-*1)2+(y4-y1)2);b=sqrt(*4-*3)2+(y4-y3)2); c=sqrt(*3-*1)2+(y3-y1)2); d=sqrt(*3-*2)2+(y3-y2)2); e=sqrt(*4-*2)2+(y4-y2)2);alpha1=acos(b2+c2-a2)/(2*b*c);beta1=acos(10/c);theta1=3*pi/2-alpha1-beta1; alpha2=acos(b2+e2-d2)/(2*b*e); beta2=acos

23、(10/e);theta2=3*pi/2-alpha2-beta2;L=sqrt(c2-102)+10*theta1+b+10*theta2+sqrt(e2-102);在命令窗口輸入 fun1(0,0,455,150,230,60,410,100)結(jié)果為ans =496.8696 3、過A點(diǎn)圓心編寫LINGO程序5,6model:min=(m-80)2+(n-210)2-400)(1/2)+(m-220)2+(n-530)2)(1/2)+10*(acos(-1)+atan(n-530)/(m-220)-atan(y2-y1)/(*2-*1);(*2-*1)2+(y2-y1)2)(1/2)=(m

24、-80)2+(n-210)2-400)(1/2);(*1-80)2+(y1-210)2=100;(*2-m)2+(y2-n)2=100;(n-y2)/(m-*2)*(y2-y1)/(*2-*1)=-1;(300-m)2+(300-n)2=100;n>=y2;y1>210;end運(yùn)行結(jié)果： Objective value: 482.2310 Variable Value Reduced Cost M 290.8854 0.000000 N 304.1140 0.000000 Y2 294.6634 0.000000 Y1 219.4505 0.000000 *2 294.1546 0

25、.000000*1 76.73080 0.0000004、過B點(diǎn)圓心編寫LINGO程序model:min=(m-150)2+(n-600)2)(1/2)+(m-180)2+(n-680)2)(1/2)+10*(acos(-1)-atan(680-n)/(270-m)-atan(600-n)/(150-m);(m-100)2+(n-700)2=100;end運(yùn)行結(jié)果: Objective value: 219.9993Variable Value Reduced CostM 107.3884 0.000000N 693.2612 0.0000005、過C點(diǎn)圓心編寫LINGO程序model:min

26、=(m-670)2+(n-730)2-400)(1/2)+(m-720)2+(n-600)2-400)(1/2)+10*(atan(y2-y1)/(*2-*1)-atan(y4-y3)/(*4-*3);(*1-720)2+(y1-600)2=100;(*3-670)2+(y3-730)2=100;(*2-m)2+(y2-n)2=100;(*4-m)2+(y4-n)2=100; (700-m)2+(640-n)2=100;(n-y4)/(m-*4)*(y4-y3)/(*4-*3)=-1;(n-y2)/(m-*2)*(y2-y1)/(*2-*1)=-1;(y2-y1)/(*2-*1)>(n-

27、600)/(m-720);(y4-y3)/(*4-*3)<(n-730)/(m-670);m>700;y4>=640;y2<=640;*3>670;y1>600;end運(yùn)行結(jié)果： Objective value: 130.7340 Model Class: NLP Variable Value Reduced Cost M 709.6622 0.000000 N 637.4229 0.000000 Y2 634.6639 0.000000 Y1 600.0000 0.000000 *2 700.0503 0.000000 *1 710.0000 0.0000

28、00 Y4 641.3609 0.000000 Y3 733.9381 0.000000 *4 718.8542 0.000000 *3 679.1919 0.0000006、 第一種類型的切點(diǎn)例如求起點(diǎn)O繞過障礙物5到達(dá)A點(diǎn)的第一個(gè)切點(diǎn)坐標(biāo).編寫M文件，保存為“切點(diǎn)1.m:syms * y *1 y1 *2 y2f1=('(*2-*)2+(y2-y)2=100'); f2=('(*2-*1)2+(y2-y1)2-100=(*-*1)2+(y-y1)2');*,y=solve(f1,f2,*,y);在命令窗口輸入>> *1=0;y1=0;*2=80;

29、y2=210;>> eval(*,y)ans = 70.5060 213.140689.1772 206.0277根據(jù)題意取障礙物左側(cè)的點(diǎn)，得第一個(gè)切點(diǎn)坐標(biāo)為70.5060，213.1406。7、 第二種類型的切點(diǎn)例如求從A點(diǎn)到障礙物7的切點(diǎn)，運(yùn)用MATLAB求解.編寫M文件，保存為“切點(diǎn)2.m:syms * y *1 y1 *2 y2f1=('(*1-*)2+(y1-y)2=100');f2=('y=y1+10*sqrt(1+(y2-y1)/(*2-*1)2)+(y2-y1)/(*2-*1)*(*-*1)');*,y=solve(f1,f2,*,y

30、);在命令窗口輸入：>> *1=290.8814;y1=304.1051;*2=220;y2=530;>> eval(*,y)ans = 300.4227 307.0990公切線的切點(diǎn)的坐標(biāo)為300.4227，307.0990，同樣算法，只需交換一下坐標(biāo)就可得到另一切點(diǎn)的坐標(biāo)為229.5413，532.9939.8、最短路徑計(jì)算結(jié)果匯總：的最短路徑路徑起點(diǎn)坐標(biāo)終點(diǎn)坐標(biāo)圓心坐標(biāo)圓弧半徑直線或圓弧長(zhǎng)線段0.00000.000070.5060213.1406224.4995圓弧70.5060213.140676.6064219.406680.0000210.000010.00

31、009.0510線段76.6064219.4066300.0000300.0000237.4868總路程：471.0373 總時(shí)間：96.0177的最短路徑路徑起點(diǎn)坐標(biāo)終點(diǎn)坐標(biāo)圓心坐標(biāo)圓弧半徑直線或圓弧長(zhǎng)線段0.00000.000050.1353 301.6396305.7776圓弧50.1353301.639651.6795305.547060.0000300.000010.00004.2330線段51.6795305.5470141.6795440.5470162.2498圓弧141.6795440.5470147.9621444.7901150.0000435.000010.00007.

32、7756線段147.9621 444.7901222.0379460.209975.6637圓弧222.0379 460.2099230.0000470.0000220.0000470.000010.000013.6557線段230.0000470.0000230.0000530.000060.0000圓弧230.0000530.0000225.4967538.3538220.0000530.000010.0000.9.8883線段225.4967 538.3538144.5033591.646296.9537圓弧144.5033 591.6462140.6916596.3458150.000

33、0600.000010.00006.1474線段140.6916 596.3458100.0000700.0000111.3553總路程：853.7001 總時(shí)間：179.0800的最短路徑路徑起點(diǎn)坐標(biāo)終點(diǎn)坐標(biāo)圓心坐標(biāo)圓弧半徑直線或圓弧長(zhǎng)線段0.00000.0000232.1149 50.2262237.4868圓弧232.1149 50.2262232.1693 50.2381230.000060.000010.00000.7860線段232.1693 50.2381412.1693 90.2381184.3909圓弧412.1693 90.2381418.3448 94.4897410.0

34、000100.000010.00007.6852線段418.3448 94.4897491.6552 205.5103133.0413圓弧491.6552 205.5103492.0623 206.0822500.0000200.000010.00003.3127線段492.0623 206.0822727.9377 513.9178387.8144圓弧727.9377 513.9178730.0000520.0000720.0000520.000.10.00006.5381線段730.0000520.0000730.0000600.000080.0000圓弧730.0000600.00007

35、27.7178 606.3589720.0000600.000010.00006.8916線段727.7178 606.3589700.0000640.000043.5889總路程：1091.5359 總時(shí)間：223.3499的最短路徑路徑起點(diǎn)坐標(biāo)終點(diǎn)坐標(biāo)圓心坐標(biāo)圓弧半徑直線或圓弧長(zhǎng)線段0.00000.000070.5060213.1406224.4995圓弧70.5060,213.140676.7308 219.450580.0000210.000010.00009.1704線段76.7308 219.4505294.1546 294.6635230.9169圓弧294.1546 294.6

36、635300.4266 307.1081290.8854304.114010.000015.4315線段300.4266 307.1081229.5412 532.9941236.7619圓弧229.5412 532.9941225.4967 538.3538220.0000530.000010.00006.8481線段225.498538.3538144.5033 591.646296.9590圓弧144.5033 591.6462140.9044 595.8442150.0000600.000010.00005.6022線段140.9044 595.844298.2928 689.1054

37、102.5349圓弧98.2928 689.1054108.2021 703.2281107.3884693.261210.000020.8073線段108.2021 703.2281270.8128 689.9669163.1514圓弧270.8128 689.9669272.0000 689.7980270.0000680.000010.00005.2668線段272.0000 689.7980368.0000 670.202197.9778圓弧368.0000 670.2021370.0000670.0000370.0000680.000010.00002.0136線段370.00006

38、70.0000430.0000670.000060.0000圓弧430.0000670.0000435.5878 671.7068430.0000680.000010.00005.9291線段435.5878 671.7068534.4122 738.2932119.1638圓弧534.4122 738.2932540.0000740.0000540.0000730.000010.00005.9291線段540.0000740.0000670.0000740.0000130.0000圓弧670.0000740.0000679.7899732.0390670.0000730.000010.000013.6359線段679.7909 732.