[發明專利]考慮雙程的多途經點的快速路徑規劃方法

有效

• 申請號: 202110263505.7
• 申請日: 2021-03-11
• 公開（公告）號: CN113029175B
• 公開（公告）日: 2021-09-10
• 發明（設計）人: 吉珊珊;駱劍鋒;朱展延;陳逸婷;曾雄;庾文聰
• 申請（專利權）人: 東莞職業技術學院;駱劍鋒
• 主分類號: G01C21/34
• 分類號: G01C21/34
• 代理公司: 東莞市十方專利代理事務所(普通合伙) 44391
• 代理人: 黃云

1.一種考慮雙程的多途經點的快速路徑規劃方法，其特征在于包括如下步驟：

1)先知條件：

已知起始點Ps、目的地Pe及多個途經點P1至Pm的經緯度，m＝10，形成點的集合KP；

已知所有點之間的最短路程，并且形成最短路程矩陣PTP；

通過上述條件，得到這些點的網絡圖GA；

2)建立新的坐標體系，重新計算各個點的坐標：

根據起始點Ps與目的地Pe的坐標值，求坐標旋轉角度；

根據坐標變換公式，得新坐標體系和每個點的新坐標，得到新的點的集合NKP和新圖為NP；

3)對途經點進行分類，分為去程點集合及返程點集合；

根據新的坐標體系，y坐標值為負的點為去程點，所有去程點的集合為去程點集合；

y坐標值為正的點為返程點，所有返程點的集合為返程點集合；

4)在去程點集合中，根據x坐標值，對點進行排序，計算出去程路徑規劃：

根據x坐標值使用去程排序公式，排序在前的點為去程先到的點；

點的先后順序則為去程的路徑規劃；

根據前述步驟的結果得到一個新的路徑規劃矩陣NM1和去程路徑規劃圖GPA1；

5)在返程點集合中，根據x坐標值，對點進行排序，計算出去程路徑規劃：

根據x坐標值使用返程排序公式，排序在前的點，為返程先到點；

點的先后順序則為返程的路徑規劃；

根據前述步驟結果可得到一個新的路徑規劃矩陣NM2和去程路徑規劃圖GPA2；

6)以Pe點為轉折點，合并去程與返程；

7)路徑規劃結束最后處理，如下：

合并NM1和NM2，得路徑規劃結果矩陣:

根據路徑規劃結果矩陣得最終路徑規劃圖:

根據路徑規劃結果矩陣和最終路徑規劃圖，累加路程值，得最終路程值；

KP表示：

KP＝{Pi(xi,yi)|Ps(xs,ys),P1(x1,y1),P2(x2,y2),…,Pm(xm,ym),Pe(xe,ye)}(10≤m)

其中Pi代表第i個點，它的經度為xi,表示為Pi.xi，緯度為yi,表示為Pi.yi；

求坐標旋轉角度θ的公式如下：

如果(Pe.xe-Ps.xs)≠0

θ＝arctan(Pe.ye-Ps.ys)/(Pe.xe-Ps.xs)

如果(Pe.xe-Ps.xs)＝0

(Pe.ye-Ps.ys)0,則θ＝90°

(Pe.ye-Ps.ys)0,則θ＝-90°；

所述根據坐標變換公式，得新坐標體系和每個點的新坐標，得新的點的集合NKP和新圖為NP中坐標變換公式為：

公式中，Pi.xi’和Pi.yi’是第i個點的新的坐標；

去程排序公式如下：

設GPX＝{P6.x6,P7.x7,P8.x8,P9.x9,P10.x10},它的個數N＝5；

設序列GP＝{GP[0],GP[1],GP[2],GP[3],GP[4]}

GP[0]＝MIN(GPX)

i＝1,2,3......N-1

結合已知集合NKP的值，經過上述公式，可知，GP[0]代表了P10.x10，GP[1]代表了P9.x9，GP[2]代表了P8.x8，GP[3]代表了P7.x7，GP[4]代表了P6.x6；

根據x坐標值使用返程排序公式，排序在前的點，為返程先到點：

設GPX＝{P1.x1,P2.x2,P3.x3,P4.x4,P5.x5},它的個數N＝5；

設序列GP＝{GP[0],GP[1],GP[2],GP[3],GP[4]}

GP[0]＝MAX(GPX)

i＝1,2,3......N-1

結合已知集合NKP的值，經過上述公式，可知，GP[0]代表了P5.x5，GP[1]代表了P4.x4，GP[2]代表了P3.x3，GP[3]代表了P2.x2，GP[4]代表了P1.x1。