融合改進A*蟻群和滾動窗口法的平滑路徑規劃

2021-10-12 08:50:20殷紹偉戴菲菲

計算機與生活 2021年10期

殷紹偉，彭力+，戴菲菲

1.物聯網技術應用教育部工程研究中心（江南大學物聯網工程學院），江蘇無錫 214122

2.臺州市產品質量安全檢測研究院，浙江臺州 318000

路徑規劃是移動機器人領域的一個重要研究方向，其主要目標就是通過一定的智能算法求得避開所有障礙物的最優安全路徑。目前對于已知靜態障礙物環境下的全局路徑規劃研究已經有很多，比如人工勢場法[1]、遺傳算法[2-3]、神經網絡[4]等。人工勢場法相對簡單，且便于底層實時控制，但是對于稍微復雜的地圖，就會出現死鎖、停滯的現象，而且極易陷入局部最優；遺傳算法雖然全局搜索能力強，但是計算量大，收斂慢；神經網絡學習能力強，但是地圖變化或者存在動態障礙物時，網絡結構復雜且需要不斷地改變閾值。

蟻群算法（ant colony optimization，ACO）[5]是Dorigo受生物進化啟發提出，其因為正反饋原理、天然的并行性以及較強的魯棒性等優點，在路徑規劃[6-8]、網絡路由[9]等方面得到廣泛應用。但是蟻群算法也有一定的缺點，比如收斂速度慢、易陷入局部最優。針對蟻群算法的不足，國內外不少學者提出過改進，文獻[10]提出將人工勢場法與ACO 結合，并且對路徑進行幾何優化；文獻[11]引入動態搜索誘導算子控制算法的搜索方向，提高算法多樣性與收斂速度，再結合k 交換算子避免早熟。雖然文獻[10-11]的實驗結果表明各自改進后算法收斂速度加快，規劃的路徑長度變短，但是未解決改進后算法參數多而且難調的問題，并且文獻[10-11]都只是在靜態環境下路徑規劃，最后規劃出的路徑也并不平滑。

針對蟻群算法的不足以及動態路徑規劃難題，本文提出一種融合改進A*蟻群算法與滾動窗口法的平滑路徑規劃方法。首先以改進A*算法的評價函數為標準，規劃出一條長度相對較短的路徑，增強該路徑上的初始信息素濃度，優化信息素初值，再用改進的蟻群算法規劃出全局路徑。然后結合滾動窗口法，制定一系列的動態避障策略，在全局路徑的基礎上進行實時的局部路徑規劃。最后，引入貝塞爾曲線對路徑進行平滑處理，使移動機器人更加準確地向目標點移動。為確保改進蟻群算法展現出最好的性能，提出使用遺傳算法（genetic algorithms，GA）對本文算法中重要參數進行自主優化選擇。

1 環境建模

在二維移動區域內，假設移動機器人占地是半徑為r的圓形，對于各種形狀的障礙物，對其進行膨脹處理，膨脹寬度為r。然后用柵格法對二維環境進行建模，其中不滿一個柵格的障礙按一個柵格計算。因為建模中有對障礙物進行膨脹處理，所以本文移動機器人可以視為質點，并且假設機器人有勻速運動與暫停兩種狀態，且這兩種狀態可以任意切換。

利用柵格法進行環境建模，能夠很好地描述任何障礙物，而且可用二值信息存儲障礙物的特征和位置，方便計算機存儲與計算。計算機存儲中，每個柵格的信息包括狀態和索引。其中狀態為障礙物柵格和可行柵格，而索引包括坐標和序號兩種。如圖1所示，對柵格地圖建立直角坐標系，以地圖的左下角作為坐標原點，水平向右為X軸正方向，垂直向上為Y軸正方向。假設柵格邊長為λ，以λ為基本單位來劃分X軸和Y軸，則地圖的行數為Ny=ymax/λ，列數為Nx=xmax/λ，其中xmax和ymax分別為X軸和Y軸上的最大值。同時標記原點柵格序號為1，柵格序號從左到右、從下到上依次遞增。坐標法與序號法是同一個柵格的兩種不同表示方法，之間可以互相轉換，如i號柵格對應的柵格中心坐標(xi,yi)為：

Fig.1 Grid index圖1 柵格索引

其中，mod 表示取余操作，即取(i-1)/Nx的余數，ceil表示取整操作。

2 改進A*蟻群算法

傳統的蟻群算法通過每代螞蟻不斷嘗試尋找目標點，并且在路線上留下信息素。下一代螞蟻總是會優先沿著信息素濃度高的道路追尋目標點。蟻群算法在用于路徑規劃時，整個尋找最優路徑的過程有以下幾點不足：第一，初始幾代螞蟻由于道路上信息素匱乏且濃度差別不大，搜索路線盲目，導致效率低；第二，它的正反饋機制雖有利于加快收斂，但是容易陷入局部最優，對于稍微復雜地圖尤其是含有凹型障礙物的地圖，還有可能出現死鎖現象；第三，信息素更新方式過于籠統，優秀螞蟻與劣質螞蟻之間的差距體現不明顯，導致算法收斂速度較慢。

2.1 改進A*算法初始化信息素

蟻群在前幾次搜索尤其是首次搜索時，路徑上的信息素都是均勻分布的，故蟻群只能進行盲目搜索，效率極低。因此提出用改進A*算法對信息素進行初始化，使蟻群算法在初期的收斂速度加快。

A*算法是一種全局路徑規劃算法，它的主要優點是速度快。其使用的評價函數為：

式中，g(n) 表示起點s到當前柵格n的移動消耗；h(n)表示當前柵格n到終點g的預估移動消耗（啟發函數）[12]。

A*算法是一種啟發式搜索算法，啟發函數h(n)對算法的路徑規劃性能有較大影響。假設H(n)為當前柵格n到終點g的實際移動消耗，當h(n)H(n)時搜索節點少，運行效率高，但一般難以找到最優路徑。

傳統的A*算法的啟發函數通常采用Euclidean 距離或Manhattan 距離定義，即：

式中，nx、ny、gx、gy分別為當前柵格點和終點。

A*算法每次搜索當前柵格的8 個相鄰柵格，如果采用曼哈頓距離來定義啟發函數，則只允許朝著上下左右四個方向移動，而本文允許對角線方向移動，因此曼哈頓距離定義啟發函數并不合適；而歐幾里德距離則允許朝著任意方向移動，不滿足本文采用柵格法對環境建模的基本條件，因此也不適合作為啟發函數。

式中，dx(n)=|nx-gx|，dy(n)=|ny-gy|。

本文將A*算法所求路徑上的初始信息素設置為：

其余路徑上的初始信息素保持不變，仍然為常數c。

2.2 改進狀態轉移概率函數

經典蟻群算法的狀態轉移概率函數主要考慮了信息素分布以及當前點與相鄰可行柵格點之間的距離。其應用于復雜環境時，尤其是有凹型障礙物的環境中，機器人很可能陷入如圖2 所示的死鎖狀態。

Fig.2 Robot deadlock state圖2 機器人死鎖狀態

針對死鎖問題，常見的解決辦法是允許陷入死鎖的螞蟻后退，更新禁忌表后再依據狀態轉移函數重新選擇下一個柵格[13]。如果是實時在線路徑規劃，這種方法在復雜環境下得到的路線很可能不是最優的；即使是離線規劃，螞蟻也需要判斷是否處于死鎖狀態，如果是則需后退重新選擇，搜索效率降低，而且死鎖點的信息素濃度需要經過多次更新后才能與其他柵格點拉開差距，從而避免下一代的螞蟻再次進入死鎖點，顯然采用這種策略來規避死鎖點可行性不高。文獻[14]通過設置進入死鎖點的螞蟻死亡，并把死亡螞蟻留下的信息素清除，可以有效避免死鎖。但是如果地圖較為復雜，每一代都會有大量螞蟻被設置為死亡，這將不利于算法得到收斂，并且會影響算法對最優解的探索能力。

本文改進狀態轉移函數，在其中加入“活躍度”因子，具體如下：

其中τij(t)為在時刻t柵格i到j之間路徑的信息素濃度[5]；Aj(t)為柵格j的“活躍度”；ηij(t)為啟發函數，式（10）表示傳統蟻群算法的啟發函數，很明顯該公式只考慮柵格i到柵格j之間的距離，因此，正方向的相鄰可行柵格比斜方向的相鄰柵格更容易被啟發函數選中。這樣選擇過于片面，故此處考慮終點柵格位置，本文將啟發函數改為式（11）。

式（8）中liveness表示與柵格j相鄰的柵格個數，地圖中間與之相鄰的柵格個數為8，而四周（不包括頂點）與之相鄰的柵格個數為5，頂點則為3；obj表示柵格j周邊最大連續障礙物的個數，如圖2 中的柵格41 為4，柵格75 為3。如此對于柵格41 與55，如果它們非終點，其“活躍度”經過式（8）、式（9）計算為0，代入式（7）得死鎖點被選擇的概率為0。因此加入“活躍度”因子后，根據式（7）得出的狀態轉移概率是否為0，就可以判斷該點是否為死鎖點。一旦判斷為死鎖點，螞蟻就不會經過該柵格，也就不用使陷入死鎖點的螞蟻后退或直接把進入死鎖點的螞蟻設置為死亡等操作，使得該算法的效率和收斂速度得到較大提高。同時為了保證選擇的多樣性，根據計算出的狀態轉移概率值，用輪盤賭算法對下一個柵格進行選擇。

2.3 基于不平等原則的信息素更新機制

傳統的蟻群算法信息素更新策略是將前幾代螞蟻留下的信息素之和按照一定的比例揮發后加上當代螞蟻留下的信息素，具體公式如下：

式中，ρ∈[0,1]為信息素揮發系數，Q是信息素強度，Lk表示第k只螞蟻本次迭代所走的路徑長度。由式（14）可知，當代螞蟻留下的信息素只是與走過的路徑長度成簡單反比關系。當地圖變得復雜，起點與終點相距較遠時，Lk取值必將變大。同時如果信息素強度Q取值不變大，當代優秀螞蟻與劣質螞蟻留下的信息素將幾乎沒什么差距，放緩了算法的收斂的速度。故此處將式（14）改為式（15），對螞蟻之間的差距進行放大，并且直接清除掉每代最差螞蟻信息素。

算法中對于未找到終點的螞蟻，其路徑長度定義為inf，式中Lmax是指除inf以外的最大值。當螞蟻路徑長度為Lmax時，也就是在找到終點的螞蟻中最差的，其經過式（15）計算產生的信息素為0。當Lk∈[Lmin,Lmax)時，計算式（14）與式（15）之間的差值，記為E，如式（16）。將式（16）轉化為式（17），當時，將E對Lk求導記為E′，且易得E′<0，如同式（18）；同理當時，得E′>0。由此可得，相對于傳統的信息素更新策略，式（15）拉大了不同螞蟻之間的差距，使優質螞蟻與劣質螞蟻差距變大。路程更短的螞蟻，遺留的信息素相比傳統的更新方式更多；路程更長的螞蟻，遺留的信息素比傳統的更新方式更少。當路程短的螞蟻可以留下更多的信息素時，經過算法的信息素更新機制數次迭代之后，路程短的與路程長的螞蟻之間的差距迅速增大。較短路徑由于經過的螞蟻數量較多，柵格上的信息素越來越多；而相對長的路徑由于只有較少的螞蟻會經過，柵格上的信息素得不到顯著增加，而且由于信息素會慢慢揮發，以至于最后較長路徑上的信息素可以忽略不記。因此在數次迭代之后，最短路徑上的信息素濃度會比其他路徑上的更多，而且差距顯著，代入式（7）計算，得到的狀態轉移概率會逐漸接近于1，路徑規劃算法也得到收斂。因此，基于不平等原則的信息素更新機制使得改進后的算法更容易收斂。

3 基于遺傳算法的參數優化

本文改進的A*蟻群算法需要對較多參數進行初始化，并且目前還沒有嚴格的理論依據對蟻群算法參數進行正定，一般都是根據具體情況進行分析，依據實驗仿真效果與自身經驗不斷調試所得。然而這樣做不僅費時、主觀性強，而且選擇的參數容易是局部最優參數。本文需要初始化的參數共有7 個，其中較為重要的參數有：改進A*算法初始化信息素放大倍數k，信息素與啟發函數的權重α和β，信息素揮發系數ρ，信息素強度Q。這些參數在很大程度上會影響算法的性能，雖然根據經驗各自都有取值范圍，但是每個參數排列組合難以調取，因此本文提出用帶精英策略的簡單遺傳算法對這5 個較為重要參數進行自主優化選擇。

3.1 帶精英策略的遺傳算法

選擇遺傳算法主要是因為遺傳算法尋優能力強，參數少，且參數取值范圍小，易調節。文獻[15]用粒子群算法（particle swarm optimization，PSO）優化蟻群算法中的參數α和β，但是PSO 本身要對5 個參數進行初始化，其中慣性權值w，學習因子c1和c2對算法性能有較大影響，并且PSO 易早熟于局部最優。

傳統遺傳算法主要參數只有交叉概率pc和變異概率pm，且這兩個參數的取值范圍較小，一般pc∈[0.7,1.0)，pm∈(0,0.3]。當交叉概率過小時，種群不能進行有效的更新，收斂結果不具有說服力；而交叉概率過大時，隨機性增大，容易破壞已有的最優解，錯失最優個體。變異概率過小時，種群的多樣性下降快，會導致收斂不穩定；而變異概率過大時，雖然會增加種群的多樣性，但是可能會使優秀個體尚未得到保存就被破壞，最后得到的結果并不是最優解。但是總的來說遺傳算法參數相對于蟻群算法個數少，取值范圍小，因此更易選擇。本文遺傳算法的參數是根據自身經驗以及經過不斷調試所得，同時為加快遺傳算法的收斂速度，本文在經典遺傳算法的基礎上添加精英策略，即從父代種群中抽取一定比例的精英不經過遺傳算子操作直接復制到子代。

3.2 參數優化步驟

本文遺傳算法的適應度函數設置為路徑的長度，依據一般經驗與算法本身的約束條件，將改進的A*蟻群算法中5個重要參數限定為以下取值范圍：k∈(1,5),α∈(0,2),β∈(5,25),ρ∈(0,1),Q∈(5,35)。具體步驟如下：

步驟1初始化帶精英策略的遺傳算法最大迭代次數為200，交叉概率pc與變異概率pm分別為0.8 和0.1，種群大小m為200。

步驟2對改進蟻群算法中5 個重要的參數進行有約束的編碼，編碼方式為二進制編碼，約束條件是各參數的取值范圍。

步驟3計算適應值，依據適應值進行排序，將前5%的精英不經過交叉變異直接復制給下一代。

步驟4父代進行交叉與變異操作，將產生的子代與父代合并，用二元錦標賽法從中選出種群大小m的95%傳入下一代。

步驟5判斷遺傳算法是否達到最大迭代次數或者收斂。若是，則選取出最優解作為本文改進A*蟻群算法的參數，否則轉到步驟3。

4 基于滾動窗口法的動態防碰策略

設滾動窗口刷新時間為T，窗口的大小（即機器人的探測范圍）為以機器人為中心半徑為R的圓形區域，機器人在時間T內的最大移動距離為Sr，動態障礙物在相同時間內的移動距離為So[16]。根據滾動窗口的定義，想要滾動窗口有效，需滿足So≤Sr的前提，其中Sr∈(0,R/2]。

4.1 對直線運動的障礙物防碰策略

對于以速度V0沿直線運動的障礙物，可以預測出在滾動窗口周期內障礙物的運動軌跡。根據此軌跡與滾動窗口內機器人的全局規劃路徑，可以判斷出二者是否有碰撞的風險。

（1）若無碰撞，則機器人按照全局規劃路徑移動一個周期，進入下次窗口刷新。

（2）若側面碰撞，則在預估碰撞點前一個柵格點等待Δt。Δt為：

式中，d0為動態障礙物的長度。

（3）若正面碰撞，將全局路徑與滾動窗口的交點視為局部規劃的子目標點，預估碰撞點視為靜態障礙物，以當前點為起始點，用本文算法進行局部路徑規劃。此時，分為兩種情形：局部路徑規劃有解與無解。針對無解的情形，如圖3 所示，在滾動窗口內，機器人r與動態障礙物O相向運動，預估二者將在A點相遇。按照上面局部規劃方法，子目標點B將到達不了。因此，針對上述局部路徑規劃無解的情況，將不再進行局部規劃，重新開始全局規劃。

Fig.3 Sub-goal point is unreachable圖3 子目標點不可達

4.2 對運動方向不確定的障礙物防碰策略

對于以速度V0運動且方向不確定的動態障礙物，文獻[16]的策略是將動態障礙物的活動范圍全部視為靜態障礙物。這種策略過于保守，且動態障礙物的整個運動范圍可能包含機器人當前的位置。

本文的策略是：當滾動窗口內檢測到動態障礙物時，則開始以其所在位置為圓心，以速度V0向外進行膨脹，膨脹區域則可視為靜態障礙物。機器人因此可以判斷滾動窗口內的全局規劃路徑是否會有與動態障礙物碰撞的風險。若有，則按照5.1 節的策略進行局部路徑規劃，若無法規劃出可行的局部路徑，則需要重新進行全局路徑規劃。

5 貝塞爾曲線平滑路徑

路徑規劃是求得移動機器人從起始點到終點的最優路徑。因此，其所產生的路徑需滿足平滑性的需求，盡可能地保證所規劃出的路徑與實際運動路徑相同。由于本文采用柵格法表示環境地圖，可能會在路徑轉折處產生尖峰。

貝塞爾曲線的最大優點是具有優良的擬合特性，即如果控制點構成的多邊形是凸的，那么貝塞爾曲線也會是凸的。因此，本文采用貝塞爾曲線來平滑規劃出的路徑。

5.1 貝塞爾曲線方程

給定R3:B:[0,1]→R3，它代表n+1 個控制點，i=0,1,…,n。n次貝塞爾曲線可由n次Bernestein 基多項式組成的參數曲線表示，定義為[17]：

式中，t表示歸一化時間變量；Bi,n(t)是Bernestein 基多項式，代表貝塞爾曲線中的基函數，其表達式定義如下：

貝塞爾曲線的導數由控制點決定，其一階導數可以表示為：

在二維空間中，貝塞爾曲線相對于t的曲率可以表示為：

式中，R(t)為曲率半徑；分別為貝塞爾曲線坐標分量對x和y的一階導數和二階導數。

5.2 貝塞爾曲線平滑路徑

在改進A*蟻群算法在存在靜態或動態障礙物的情況下得到最短路徑后，由貝塞爾曲線對路徑進行平滑處理，稱為路徑的后續處理環節，其具體流程圖如圖4 所示。

Fig.4 Smoothing process of Bessel curve圖4 貝塞爾曲線的平滑流程

6 仿真分析

本文對傳統蟻群算法在路徑規劃應用上的一些不足之處進行了改進，下面借助Matlab，在CPU 為i5-6500、系統為Win10 的電腦上進行仿真實驗。

6.1 與原蟻群算法比較

將本文改進的A*蟻群算法與原蟻群算法在20×20 的柵格地圖上進行對比實驗，本文算法的初始條件為：最大迭代數為100，蟻群個數為30，遺傳算法的最大迭代數為20，種群大小為25，交叉概率為0.8，變異概率為0.05，剩余5 個重要參數的變化范圍見表1。由帶精英策略的遺傳算法得到它們的最佳組合為k=3.9，α=0.98，β=14.01，ρ=0.69，Q=18.53。設起始點為柵格1，終點為柵格400。對于原有的蟻群算法，所有參數都需按照經驗自行設定，現設置具體參數為：最大迭代數為100，蟻群個數為30，α=0.60，β=11.00，ρ=0.50，Q=15.50 。圖5 為未進行參數優化的原蟻群算法和本文改進的A*蟻群算法路徑圖的對比結果。將本文算法得到的最佳組合參數（k除外）應用于原蟻群算法，其得到的路徑與本文改進的A*蟻群算法路徑對比圖如圖6 所示。以上三種算法分別對應的收斂曲線圖如圖7 所示。圖8 為本文改進的A*蟻群算法規劃出的路徑和采用貝塞爾曲線平滑后的路徑對比圖，對比原路徑可知貝塞爾曲線對其平滑后的路徑更加符合實際運動路線。

Table 1 Variation range of important parameters表1 重要參數的變化范圍

Fig.5 Compared with ACO without parameter optimization圖5 與未參優蟻群算法對比

Fig.6 Compared with ACO after parameter optimization圖6 與參優后蟻群算法對比

Fig.7 Convergence curves of three algorithms圖7 三種算法的收斂曲線

Fig.8 Smooth path diagram under simple obstacles圖8 簡單障礙物下的平滑路徑圖

Table 2 Compared with original ACO表2 與原蟻群算法對比

圖5～圖7 的具體對比數據見表2，對比參數優化與非參數優化情況下原蟻群的路徑長度，可知蟻群算法的性能很大程度上受參數的影響，雖然原蟻群的參數是依據經驗而非胡亂選得，但是也是局部最優參數，可見本文用遺傳算法優化參數有一定的必要性與優越性。對比本文算法與參數優化后的原蟻群算法，不僅路徑長度由30.041 6 縮短到28.627 4，而且收斂情況也由未收斂變為迭代11 次后收斂，可見本文算法尋優能力與收斂速度都大幅度提高。

6.2 與改進蟻群算法比較

為了證明本文算法在復雜靜態環境中的優越性，選用其他改進的蟻群算法進行對比。文獻[11]提出了一種動態搜索策略的蟻群算法（ant colony algorithm for dynamic search strategy），并在50×50 的復雜環境中進行驗證。由于在文獻[11]中已用其方法與其他改進算法在相同環境下進行了對比實驗，證明了其優越性，故在此只需與文獻[11]進行對比。本文算法的參數初始化同6.1 節（下文同），圖9 為兩種方法的路徑對比圖，其中本文算法的收斂曲線圖如圖10 所示，相關數據對比見表3，其中標準差按照文獻[11]中的定義，為算法運行10 次的誤差。而圖11 為本文算法規劃出的路徑與采用貝塞爾曲線對其進行平滑處理后的路徑對比圖。

Fig.9 Compared with algorithm in ref.[11]圖9 與文獻[11]算法對比

Fig.10 Convergence curve of algorithm in this paper圖10 本文算法收斂曲線

Table 3 Compared with improved ACO表3 與改進蟻群算法對比

Fig.11 Smooth path diagram under complex obstacles圖11 復雜障礙物下的平滑路徑圖

從路徑對比圖與表3 中的數據對比可知，本文算法無論是路徑長度、收斂迭代次數，還是穩定性（標準差比較）都略優于文獻[11]。而且文獻[11]中的參數是通過經驗與不斷調試所得，本文改進A*蟻群的重要參數是自主尋優所得。

6.3 動態條件下驗證可行性

仿真實驗1在20×20 和40×40 的柵格地圖中，矩形動態障礙物沿豎直方向，以小于機器人的速度做勻速往返運動。當動態障礙物與全局規劃路徑未發生碰撞時，機器人的路徑如圖12 所示；當動態障礙物影響到既定的全局規劃路徑時，路徑圖如圖13 所示。圖13 中虛線是原定路線，實線是根據動態避障策略，進行局部路徑規劃的實際路線。

仿真實驗2實驗1 驗證了障礙物沿直線運動的情況，下面驗證障礙物運動的方向隨機的情況。如圖14 所示，在50×50 的復雜環境中，機器人1 從S1(0.5,0.5)到G1(49.5,49.5)，而機器人2 從S2(0.5,31.5)到G2(49.5,13.5) 。兩個機器人相互獨立，速度相同（滿足滾動窗口前提），對于兩個機器人而言，另一方均可視為運動方向不確定的動態障礙物。圖中的交點為（17.730 8,20.730 8），由于兩機器人速度相同，故二者并未同時到達，圖中路徑為安全路徑。

7 結束語

本文針對靜態與動態環境下移動機器人路徑規劃問題，提出一種融合改進A*蟻群算法與滾動窗口法的平滑路徑規劃方法。該方法克服了傳統蟻群算法收斂慢、易陷入局部最優等一系列問題。通過對三組仿真數據的分析與對比，證明了該算法每個環節改進的必要性與優越性。仿真結果表明，本文算法不僅能實現復雜靜態障礙物環境下機器人的路徑規劃，而且對于一定的動態障礙物也能實現高效的避讓。

Fig.12 Path planning without collision圖12 未發生碰撞時路徑規劃圖

Fig.13 Path planning when avoiding dynamic obstacles圖13 避開動態障礙物時路徑規劃圖

Fig.14 Dynamic obstacles avoidance diagram with uncertain motion direction圖14 對運動方向不確定的動態障礙物避障圖

本文也尚存在不足之處，例如本文在對動態障礙物的仿真實驗中假設移動機器人一直處于勻速運動狀態，然而在實際生活中機器人并不會一直處于勻速運動狀態。在以后的研究工作中，針對這個問題將會引入不同速度以及存在加速度的機器人進行各種仿真實驗，進一步提高該算法的實際應用價值。