[發(fā)明專利]基于參數(shù)化Thiele連分式插值的路徑規(guī)劃方法及系統(tǒng)

在審

• 申請?zhí)枺?/td>: 202010936239.5
• 申請日: 2020-09-08
• 公開（公告）號(hào): CN112099498A
• 公開（公告）日: 2020-12-18
• 發(fā)明（設(shè)計(jì)）人: 鄒樂;吳志澤;張琛;王曉峰;丁澤盛;檀明
• 申請（專利權(quán)）人: 合肥學(xué)院
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 合肥維可專利代理事務(wù)所(普通合伙) 34135
• 代理人: 吳明華

1.基于參數(shù)化Thiele連分式插值的路徑規(guī)劃方法，其特征在于：應(yīng)用于機(jī)器人路徑規(guī)劃，包括如下步驟：

(11)獲取包括機(jī)器人路徑起點(diǎn)和終點(diǎn)所在區(qū)域的三維柵格地圖；

(12)獲取所述柵格地圖上預(yù)設(shè)靜態(tài)障礙物所占柵格位置；

(13)以路徑起點(diǎn)開始逐步進(jìn)行路徑搜索，獲取避障的初步規(guī)劃路徑中的節(jié)點(diǎn)；

(14)基于二元參數(shù)化Thiele連分式插值法在初步規(guī)劃的路徑中的相鄰兩節(jié)點(diǎn)之間插值，獲取平滑的曲線路徑。

2.根據(jù)權(quán)利要求1所述的基于參數(shù)化Thiele連分式插值的路徑規(guī)劃方法，其特征在于：所述步驟(14)采用二元含單參數(shù)Thiele連分式插值。

3.根據(jù)權(quán)利要求2所述的基于參數(shù)化Thiele連分式插值的路徑規(guī)劃方法，其特征在于：所述二元含單參數(shù)Thiele連分式插值為：

其中，

對j＝0,1,...,n；p＝1,2,...,m；i＝p,p+1,...,m，

a_k,0,a_k,1,...,a_k,l,a_k,l+1,a_k,l+2,...,a_k,n為該式中引進(jìn)參數(shù)λ后經(jīng)過計(jì)算的的數(shù)值。

對p＝0,1,...,k-1,k+1,...,m；q＝1,2,...,n；j＝q,q+1,...,n，

4.根據(jù)權(quán)利要求3所述的基于參數(shù)化Thiele連分式插值的路徑規(guī)劃方法，其特征在于：所述步驟(14)之后還包括：

基于插值后的平滑曲線路徑，判斷曲線路徑是否滿足預(yù)設(shè)條件，若不滿足，則調(diào)節(jié)Thiele連分式中的參數(shù)，重復(fù)步驟(14)，直至獲得滿足預(yù)設(shè)條件的插值曲線路徑。

5.根據(jù)權(quán)利要求4所述的基于參數(shù)化Thiele連分式插值的路徑規(guī)劃方法，其特征在于：所述判斷曲線路徑是否滿足預(yù)設(shè)條件，包括：

(51)判斷插值曲線段所在兩相鄰節(jié)點(diǎn)之間的連線是否穿過障礙物所在區(qū)域，若是，則進(jìn)入步驟(52)，否則進(jìn)入步驟(53)；

(52)判斷插值曲線段和障礙物邊緣線之間的最短距離是否在第一預(yù)設(shè)閾值區(qū)間內(nèi)，若是，則進(jìn)入步驟(53)，否則，不滿足預(yù)設(shè)條件；

(53)判斷插值曲線的彎折角度是否小于第二預(yù)設(shè)閾值，若是，則曲線路徑滿足預(yù)設(shè)條件，否則，不滿足預(yù)設(shè)條件。

6.根據(jù)權(quán)利要求1所述的基于參數(shù)化Thiele連分式插值的路徑規(guī)劃方法，其特征在于：所述步驟(13)采用A*算法獲取初步規(guī)劃的路徑，以當(dāng)前所在柵格位置搜索相鄰柵格位置，并以到達(dá)相鄰柵格的代價(jià)函數(shù)最小為優(yōu)化目標(biāo)函數(shù)。

7.根據(jù)權(quán)利要求6所述的基于參數(shù)化Thiele連分式插值的路徑規(guī)劃方法，其特征在于：所述A*算法的代價(jià)函數(shù)f(n)為：

f(n)＝g(n)+h(n)+w*N

其中，f(n)是節(jié)點(diǎn)n的估價(jià)函數(shù)，h(n)表示從節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的滿足優(yōu)化條件的啟發(fā)函數(shù)，g(n)表示從機(jī)器人起點(diǎn)到節(jié)點(diǎn)n的實(shí)際代價(jià)，(x_n，y_n，z_n)為當(dāng)前節(jié)點(diǎn)坐標(biāo)位置，(x_d，y_d,z_d)為下一柵格節(jié)點(diǎn)的坐標(biāo)位置，S為機(jī)器人由當(dāng)前柵格位置移動(dòng)單位距離的步長代價(jià)，w為轉(zhuǎn)折點(diǎn)機(jī)器人的消耗，N為轉(zhuǎn)折點(diǎn)個(gè)數(shù)。

8.根據(jù)權(quán)利要求7所述的基于參數(shù)化Thiele連分式插值的路徑規(guī)劃方法，其特征在于：所述以當(dāng)前所在柵格位置搜索相鄰柵格位置之前，還包括如下步驟：

(81)判斷當(dāng)前柵格位置是否為機(jī)器人終點(diǎn)所在柵格，若是，則初步路徑規(guī)劃結(jié)束，否則進(jìn)入步驟(82)；

(82)判斷當(dāng)前柵格位置和機(jī)器人終點(diǎn)柵格位置的連線是否穿過障礙物所在柵格區(qū)域，若是，則以當(dāng)前所在柵格位置搜索相鄰柵格位置，否則，當(dāng)前所在柵格位置為初步規(guī)劃路徑終點(diǎn)前的最后一個(gè)節(jié)點(diǎn)，初步規(guī)劃路徑結(jié)束。