基于松弛序列凸優(yōu)化的輪式機(jī)器人協(xié)同軌跡規(guī)劃

鄧云山，夏元清，孫中奇

（北京理工大學(xué)自動化學(xué)院，北京 100081）

1 引 言

隨著移動機(jī)器人技術(shù)的發(fā)展以及通信技術(shù)、計算機(jī)小型化技術(shù)的日益成熟，將一個復(fù)雜的任務(wù)分配給多個結(jié)構(gòu)、功能都相對簡單的智能體來完成已經(jīng)成為一個熱門的研究方向[1]，其中協(xié)同規(guī)劃技術(shù)是多智能體協(xié)同技術(shù)中的重要組成部分。多智能體協(xié)同軌跡從建模方式上分為兩類[2]，第一類為基于簡單的轉(zhuǎn)彎半徑與避碰約束的搜索方法，如圖搜索方法[3]、樹搜索方法[4]、人工勢場法[5]等。這些搜索方法將模型約束進(jìn)行了抽象化處理，提煉成簡單的幾何約束，從而進(jìn)行搜索，具有規(guī)劃效率高、計算需求低的特點，但缺點是未考慮完整的模型約束，規(guī)劃軌跡可能在實際中不可行。第二類為基于優(yōu)化理論的方法，將軌跡規(guī)劃問題建模為優(yōu)化問題，求解方法主要包含偽譜法[6]、混合整數(shù)規(guī)劃方法[7]、智能優(yōu)化算法[8]等。

隨著凸優(yōu)化理論的發(fā)展，內(nèi)點法可在多項式時間內(nèi)對二階錐優(yōu)化問題進(jìn)行有效求解[9-10]。國內(nèi)外眾多學(xué)者利用凸優(yōu)化技術(shù)針對不同對象進(jìn)行了協(xié)同軌跡規(guī)劃問題的求解。文獻(xiàn)[11]利用序列凸優(yōu)化方法求解了航天器集群變軌問題，并給出了分布式策略架構(gòu)；文獻(xiàn)[12]以最優(yōu)時間為代價，利用凸優(yōu)化方法求解了無人機(jī)協(xié)同軌跡規(guī)劃問題，可實現(xiàn)無人機(jī)在時間上的協(xié)同；文獻(xiàn)[13]采用滾動時域架構(gòu)，將無人機(jī)協(xié)同軌跡規(guī)劃問題轉(zhuǎn)化為二次規(guī)劃問題，在一定程度上增加了求解效率，并進(jìn)行了室內(nèi)飛行實驗驗證。同時，凸優(yōu)化方法還用于星球軟著陸[14]、再入飛行器軌跡規(guī)劃[15]等工程問題。

針對地面機(jī)器人的軌跡生成問題，通常采用第一類搜索方法，將路徑與速度進(jìn)行分解，用樣條插值等方法生成曲線路徑[16]，但面對協(xié)同軌跡規(guī)劃中的復(fù)雜約束，第一類搜索方法規(guī)劃軌跡的可行性有局限。而第二類方法通常沒有考慮優(yōu)化問題的凸化，直接使用非線性優(yōu)化工具包進(jìn)行求解，大大增加了協(xié)同軌跡生成問題的求解時間。對此，本文考慮具有非線性運動學(xué)方程的輪式機(jī)器人模型，考慮機(jī)器人物理性能約束、障礙規(guī)避約束、機(jī)器人避碰約束，建立多輪式機(jī)器人軌跡規(guī)劃優(yōu)化模型。通過凸化與離散化方法，將問題轉(zhuǎn)化為序列二階錐優(yōu)化問題，同時對問題進(jìn)行松弛處理，以提高問題可行性。最后，通過數(shù)值仿真，驗證算法有效性。

2 問題描述

通過對輪式機(jī)器人運動學(xué)模型分析，得到單個輪式機(jī)器人的物理約束。建立多輪式機(jī)器人協(xié)同軌跡規(guī)劃問題模型，含運動學(xué)方程、避障避碰約束、個體物理性能約束、終端約束及代價函數(shù)。

2.1 輪式機(jī)器人模型

輪式機(jī)器人是一個典型的具有非完整性約束的欠驅(qū)動系統(tǒng)[17-18]。全局坐標(biāo)系下，其運動學(xué)方程可描述為

其中，［x,y］T為機(jī)器人在全局坐標(biāo)系下的位置，θ∈( -π,π] 為速度方向與坐標(biāo)軸X軸正向的夾角，v,?為實際控制量，v為線速度，?為角速度。

如圖1 所示，vL與vR分別為左右輪驅(qū)動線速度，由于驅(qū)動電機(jī)機(jī)械特性，驅(qū)動速度大小有界，其中，maxv為單個輪子線速度上界。在無打滑假設(shè)下，線速度v，角速度?與雙輪線速度滿足如下等式[19]：

圖1 輪式機(jī)器人模型示意圖Fig.1 Model of wheeled robot

其中，ρ是左右輪間的半軸距。因此，單個輪式機(jī)器人需滿足約束：

2.2 協(xié)同軌跡規(guī)劃問題

多個輪式機(jī)器人在運動過程中，需要盡可能以較低能量抵達(dá)目標(biāo)位置，同時在運動過程中，滿足個體性能約束，規(guī)避障礙物以及其余個體。

為方便后續(xù)序列迭代中的線性化，將線速度也視為狀態(tài)量， 則個體i的狀態(tài)量為增加線速度導(dǎo)數(shù)a為控制量，則個體i控制量為從而運動學(xué)方程可寫為

其中，N為機(jī)器人個數(shù)。

為保證機(jī)器人運動安全，各個機(jī)器人在任何時刻都應(yīng)位于所有障礙區(qū)域之外，后續(xù)凸化中，圓形障礙較好處理，而實際運動中的障礙可被有限個圓形障礙覆蓋，因此本文僅考慮圓形靜態(tài)障礙。則障礙規(guī)避約束可表示為

同時多個機(jī)器人在運動過程中需要保證各個機(jī)器人相互避碰：

每個機(jī)器人需滿足自身物理性能約束：

每個機(jī)器人的初始狀態(tài)與終端狀態(tài)給定，分別為

其中，t0為初始時刻，通常取t0= 0；tf為終端時刻，均為給定的已知量。 綜上，以狀態(tài)量與控制量加權(quán)二次型為性能指標(biāo)，其中狀態(tài)量二次型加權(quán)物理意義包含了任務(wù)本身的特殊需求；控制量二次型加權(quán)物理意義為能量最優(yōu)需求。將多機(jī)器人協(xié)同軌跡規(guī)劃問題建立為如下問題（P1）：

3 約束凸化

P1 包含非線性等式約束(4)，非凸不等式約束(6)(7)，是一個典型的非凸優(yōu)化問題，由于現(xiàn)有的凸優(yōu)化求解器要求目標(biāo)函數(shù)和不等式約束函數(shù)均為凸函數(shù)，等式約束函數(shù)是仿射函數(shù)[2]。因此，建立P1 的凸化近似模型，同時將優(yōu)化問題描述為多項式時間求解的二階錐優(yōu)化問題。

3.1 運動學(xué)模型線性化

將式(4)所示的非線性動力學(xué)在基準(zhǔn)處線性化為

其中，

為確保線性化精度，增加信賴域約束

3.2 障礙規(guī)避約束凸化

將障礙規(guī)避約束(6)在基準(zhǔn)軌跡處線性化，得到障礙規(guī)避約束的近似仿射不等式表達(dá)：

定理1：當(dāng)si滿足凸化障礙規(guī)避約束(14)時，必然滿足原始障礙規(guī)避約束(6)。

證明：由于范數(shù)滿足三角不等式，因此有

因此當(dāng)凸化障礙規(guī)避約束(14)滿足時，障礙規(guī)避約束(6)也滿足。

3.3 機(jī)器人避碰約束凸化

定理2：當(dāng)滿足凸化避碰約束(17)時，必然滿足原始避碰約束(7)。

其證明與上述障礙規(guī)避證明類似，在此不做贅述。事實上，從幾何意義上也可對障礙規(guī)避約束凸化與避碰約束凸化進(jìn)行說明，如圖2、圖3所示。其中，

圖2 障礙規(guī)避約束凸化示意圖Fig.2 Obstacle avoidance constraint convexity

圖3 機(jī)器人避碰約束凸化示意圖Fig.3 Collision avoidance constraint convexity

凸化障礙規(guī)避約束(14)中，不等式左側(cè)的第一項幾何含義為機(jī)器人由基準(zhǔn)位置指向當(dāng)前位置的向量在由障礙物指向基準(zhǔn)位置的向量上的投影，第二項幾何含義為障礙物到基準(zhǔn)位置的距離。整個不等式含義為，機(jī)器人需要在基準(zhǔn)位置與障礙物連線上保持安全距離。如果機(jī)器人在基準(zhǔn)位置與障礙物連線上保持了安全距離，則一定在二維空間上與障礙物保持了安全距離，即定理1 所述。

凸化避碰約束(17)中，不等式左側(cè)幾何含義為由機(jī)器人j當(dāng)前位置指向機(jī)器人i當(dāng)前位置向量在由機(jī)器人j基準(zhǔn)位置指向機(jī)器人i基準(zhǔn)位置向量上的投影。整個不等式幾何含義為，任意兩機(jī)器人需要在其基準(zhǔn)位置連線上保持安全距離。如果兩機(jī)器人在其基準(zhǔn)位置連線上保持了安全距離，則在二維空間上，兩機(jī)器人不會碰撞，即定理2 所述。

4 序列凸優(yōu)化求解

本節(jié)將建立序列二階錐優(yōu)化子問題PP1k，進(jìn)而求解原始多機(jī)器人協(xié)同軌跡規(guī)劃問題P1。同時進(jìn)行離散化，得到有限維二階錐優(yōu)化子問題PP2k。最后，為增加迭代過程中優(yōu)化子問題的可行性，對某些約束進(jìn)行松弛處理得到松弛子問題PP3k。

4.1 序列凸優(yōu)化

序列凸優(yōu)化將原始非凸優(yōu)化問題轉(zhuǎn)化為一系列可用現(xiàn)有求解器求解的凸優(yōu)化子問題進(jìn)行迭代求解，進(jìn)而得到原始問題的近似解，圖4 為序列凸優(yōu)化（SCP）算法流程。記所有機(jī)器人狀態(tài)為，控制量為。迭代求解步驟如下：

(1)初始化迭代次數(shù)k= 0，選擇初始基準(zhǔn)剖面

(2)迭代求解凸優(yōu)化子問題PPk，得到解

(4)算法退出，獲得原問題解。

圖4 序列凸優(yōu)化（SCP）算法流程圖 Fig.4 SCP algorithm

結(jié)合第3 節(jié)中相關(guān)約束的處理，原始多機(jī)器人協(xié)同軌跡規(guī)劃問題 P1 的序列凸優(yōu)化子問題（PP1k）為

4.2 離散化

由于PP1k為無窮維問題，因此需要對時間進(jìn)行離散化，將時間平均離散為n段，進(jìn)而將無窮維優(yōu)化問題轉(zhuǎn)化為有限維優(yōu)化問題。

將目標(biāo)函數(shù)離散化為

由于時間步長Δt為常數(shù)，因此可以忽略。

參考文獻(xiàn)[20]狀態(tài)方程離散方式，將機(jī)器人線性運動學(xué)方程(11)離散如下：

其中，

凸化障礙規(guī)避約束(14)離散化為

離散化后的障礙規(guī)避約束僅可保證在離散點處位于圓形障礙之外，無法保證兩離散點間的障礙規(guī)避，因此增加約束(24)，確保離散點間也滿足障礙規(guī)避約束。

定理3[12]：當(dāng)機(jī)器人i在各個離散時刻的狀態(tài)滿足約束(23)(24)時，其相鄰兩離散時刻位置的連線滿足原始障礙規(guī)避約束。

證明：考察時刻tl，由于tl,tl-1時刻狀態(tài)滿足約束(24)，而約束(24)為凸約束，因此任意連線上的點滿足該凸約束(24)。由定理1 得，tl,tl–1時刻位置的連線上的點均滿足初始障礙規(guī)避約束(6)。

利用數(shù)學(xué)歸納法可得，當(dāng)各個離散時刻的狀態(tài)滿足約束(23)(24)時，相鄰兩離散時刻位置的連線也滿足原始障礙規(guī)避約束。

凸化避碰約束(17)離散化為

信賴域約束(13)，物理性能約束(8)，離散化后變?yōu)?/p>

進(jìn)而得到離散化后的凸優(yōu)化子問題PP2k:

4.3 松弛化

為提高迭代過程中，凸優(yōu)化子問題的可求解性，對約束(21)(23)(24)(25)進(jìn)行松弛處理，松弛后的約束變?yōu)?/p>

其中，δ1,i,l,δ2,i,l,δ3,i,l,δ4,i,j,l為松弛因子，需在約束中約束其大于0，在目標(biāo)函數(shù)中增加懲罰項使約束盡可能滿足，得到松弛后的凸優(yōu)化子問題PP3k：

其中，α1,α2,α3,α4為松弛因子的懲罰因子，通常定義為足夠大的正數(shù)。

5 仿真結(jié)果及分析

面向多輪式機(jī)器人協(xié)同軌跡規(guī)劃問題，以障礙環(huán)境下的隊形重構(gòu)為例，開展數(shù)值仿真，仿真硬件環(huán)境為Intel Core i5-9400F 2.9GHz PC，編程環(huán)境為 Matlab 2019b，凸優(yōu)化子問題采用YALMIP 進(jìn)行問題建模，采用ECOS 求解器進(jìn)行求解。最后進(jìn)行不同規(guī)模軌跡規(guī)劃仿真，與直接使用非線性最優(yōu)控制工具包進(jìn)行效率對比，非線性優(yōu)化采用最優(yōu)控制工具箱ICLOCS 進(jìn)行問題建模，采用IPOPT 求解器進(jìn)行求解。

障礙環(huán)境下的隊形重構(gòu)任務(wù)需要多機(jī)器人在給定時間從初始狀態(tài)抵達(dá)目標(biāo)狀態(tài)，并且在過程中，保證機(jī)器人與圓形障礙的避撞及機(jī)器人之間的相互避碰。

以8 對輪式機(jī)器人N= 16為例，展示位置互換規(guī)劃結(jié)果。輪式機(jī)器人左右輪間的半軸距ρ=0.0267 m ，單個輪子線速度上界vmax=0.13 m/s。一對機(jī)器人以狀態(tài)1/2 分別為初始狀態(tài)與目標(biāo)狀態(tài)進(jìn)行隊形重構(gòu)，狀態(tài)設(shè)置如表1 所示。

初始信賴域約束δ1的選擇應(yīng)恰好包括所有個體在執(zhí)行任務(wù)時可能的狀態(tài)，取值過大可能會降低求解效率。信賴域衰減是收斂性的保障，但衰減過快會導(dǎo)致求解失敗，衰減過慢則增加求解時間，應(yīng)選擇適中的信賴域衰減方式。

表1 輪式機(jī)器人始末狀態(tài)Table 1 Starting and ending states of robot

三個圓形障礙物M=3 設(shè)置如表2 所示。

表2 圓形障礙物參數(shù)Table 2 Circular obstacle setting

序列凸優(yōu)化算法參數(shù)設(shè)置如表3 所示。

表3 序列凸優(yōu)化算法參數(shù)Table 3 Algorithm parameters of SCP

由于狀態(tài)量第四個分量為實際控制量中的v，因此狀態(tài)量加權(quán)因子Qs的前三個對角元為零，僅用第四個對角元對實際控制量v進(jìn)行二次加權(quán)。同理，控制量加權(quán)因子Qu第一個對角元表示了對實際控制量?的二次加權(quán)，因此其與Qs第四個對角元量級相同。上述兩參數(shù)取值物理意義為輪式機(jī)器人運動過程中能量最優(yōu)。而控制量加權(quán)因子Qu第二個對角元減少了實際控制量v的變化，使得軌跡更加平滑，一般選取數(shù)值的數(shù)量級較小。

懲罰因子α1,α2,α3,α4, 對約束進(jìn)行懲罰，一般取值較大以保證約束的滿足。

仿真中，初始基準(zhǔn)剖面選取為起始狀態(tài)到目標(biāo)狀態(tài)的平均離散，該方法選取的初始基準(zhǔn)剖面不一定滿足初始問題P1 的動力學(xué)約束，隨著迭代過程的增加，信賴域逐漸減小，最終收斂軌跡可在設(shè)定誤差范圍內(nèi)滿足P1 問題動力學(xué)約束。

針對上述設(shè)置的場景，求解機(jī)器人編隊重構(gòu)軌跡規(guī)劃問題，軌跡規(guī)劃結(jié)果如圖5 所示，各個機(jī)器人均可抵達(dá)目標(biāo)狀態(tài)并且與圓形障礙保持安全距離，且在約束觸發(fā)時各個機(jī)器人都從障礙邊擦過，位于約束的臨界狀態(tài)。機(jī)器人相互避碰約束滿足情況如圖6 所示，機(jī)器人兩兩最小間距始終大于避碰安全距離，機(jī)器人在編隊重構(gòu)過程中滿足相互避碰約束。物理性能約束滿足情況如圖7所示，物理性能指標(biāo)（即式(3)左端）的最大值始終小于單輪速度上界，各個機(jī)器人在編隊重構(gòu)過程中滿足個體物理性能約束。

圖5 規(guī)劃軌跡結(jié)果圖Fig.5 Planning trajectory result

圖6 避碰約束滿足情況Fig.6 Obstacle avoidance constraint satisfaction result

圖7 個體物理性能約束滿足情況Fig.7 Collision avoidance constraint satisfaction result

針對上述設(shè)置的場景，分別求解機(jī)器人對數(shù)為1 到8 的編隊重構(gòu)軌跡規(guī)劃，對比SCP 方法與非線性優(yōu)化工具包ICLOCS 在不同問題規(guī)模下的性能。當(dāng)編隊數(shù)量為8（N=8）時，編隊由1~4對機(jī)器人組成。

使用SCP 方法和ICLOCS 分別求解不同編隊數(shù)量的隊形重構(gòu)軌跡規(guī)劃，多次統(tǒng)計求解時間與代價值進(jìn)行對比。平均求解時間對比結(jié)果如圖8所示，兩種方法求解時間均隨機(jī)器人個數(shù)增加而增加，SCP 求解時間遠(yuǎn)遠(yuǎn)小于ICLOCS 工具包求解時間。但從代價對比結(jié)果（圖9）來看，SCP 在機(jī)器人數(shù)量較少時最優(yōu)性更好，數(shù)量增加時ICLOCS代價值更小。其原因為ICLOCS 在優(yōu)化過程中具有一定的隨機(jī)性，每次優(yōu)化的結(jié)果不同，這就使得其在面對多峰值高維問題時，較容易跳出局部 最優(yōu)的狀態(tài)，進(jìn)而在時間足夠的情況下得到較優(yōu)的優(yōu)化結(jié)果。實際應(yīng)用中，需要考慮最優(yōu)性與計算時間兩方面的均衡，SCP 方法在盡可能保證最優(yōu)性的同時大大縮短了計算時間，具有工程意義。

圖8 平均求解時間對比Fig.8 Comparison of solution time

圖9 平均求解代價對比Fig.9 Comparison of solution cost

6 結(jié) 論

本文針對多輪式機(jī)器人協(xié)同軌跡規(guī)劃問題，根據(jù)現(xiàn)實物理約束，建立優(yōu)化問題。將原始非線性優(yōu)化問題轉(zhuǎn)化為二階錐優(yōu)化問題，利用序列凸優(yōu)化思想進(jìn)行求解，進(jìn)行仿真驗證并得出了下面主要結(jié)論：

(1)建立了多輪式機(jī)器人協(xié)同軌跡規(guī)劃的非線性優(yōu)化問題，考慮了障礙規(guī)避、相互避碰以及機(jī)器人實際物理約束。

(2)對問題進(jìn)行了凸化，包含運動學(xué)線性化、避障約束凸化、避碰約束凸化，并討論了約束凸化的物理含義。使用梯形近似對問題進(jìn)行離散化，并給出問題松弛處理方法。

(3)給出松弛SCP 方法求解協(xié)同軌跡規(guī)劃問題的框架，并通過具體算例的數(shù)值仿真和對比驗證了方法的有效性。