鄧云山，夏元清，孫中奇

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

1 引 言

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

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

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

2 問(wèn)題描述

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

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

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

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

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

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

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

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

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

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

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

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

同時(shí)多個(gè)機(jī)器人在運(yùn)動(dòng)過(guò)程中需要保證各個(gè)機(jī)器人相互避碰：

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

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

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

3 約束凸化

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

3.1 運(yùn)動(dòng)學(xué)模型線性化

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

其中，

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

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

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

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

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

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

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

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

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

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

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

凸化障礙規(guī)避約束(14)中，不等式左側(cè)的第一項(xiàng)幾何含義為機(jī)器人由基準(zhǔn)位置指向當(dāng)前位置的向量在由障礙物指向基準(zhǔn)位置的向量上的投影，第二項(xiàng)幾何含義為障礙物到基準(zhǔn)位置的距離。整個(gè)不等式含義為，機(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)位置向量上的投影。整個(gè)不等式幾何含義為，任意兩機(jī)器人需要在其基準(zhǔn)位置連線上保持安全距離。如果兩機(jī)器人在其基準(zhǔn)位置連線上保持了安全距離，則在二維空間上，兩機(jī)器人不會(huì)碰撞，即定理2 所述。

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

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

4.1 序列凸優(yōu)化

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

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

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

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

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

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

4.2 離散化

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

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

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

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

其中，

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

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

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

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

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

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

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

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

4.3 松弛化

為提高迭代過(guò)程中，凸優(yōu)化子問(wèn)題的可求解性，對(duì)約束(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ù)中增加懲罰項(xiàng)使約束盡可能滿足，得到松弛后的凸優(yōu)化子問(wèn)題PP3k：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖8 平均求解時(shí)間對(duì)比Fig.8 Comparison of solution time

圖9 平均求解代價(jià)對(duì)比Fig.9 Comparison of solution cost

6 結(jié) 論

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

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

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

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