非凸環(huán)境下基于約束控制的機器人集群算法*

曹 凱，陳陽泉，高 嵩，黨海鑫，張嘉豪

(1.西安工業(yè)大學 機電工程學院，西安 710021； 2.西安工業(yè)大學 電子信息工程學院，西安 710021；3.加州大學默塞德分校 工程學院，默塞德 95343)

多機器人系統(tǒng)因其并行性高，靈活性強，相對于單機器人而言效率更高，已逐步應用于軍事、救援、環(huán)境覆蓋等領域中更加復雜的任務場景，具有很強的研究意義與廣泛的應用前景[1-2]。目前在該領域已經有了多種控制機器人集群在環(huán)境中分布的方法，如虛擬物理力法[3]、隨機有限狀態(tài)機[4]，以及質心維諾算法(Centroidal Voronoi Tessellation,CVT)[5]。其中CVT算法最大特點是利用維諾劃分對聯(lián)合感知任務進行解耦，實現(xiàn)了待優(yōu)化目標函數的分布式計算[6]。根據CVT的固有屬性，該算法能夠保證機器人之間碰撞問題。文獻[7]將無人機集群應用于噴灑農藥等農業(yè)應用，CVT算法給無人機提供最佳的位置分布，以最大的效率對農作物進行藥物噴灑。上述這些分區(qū)劃分必須保證環(huán)境區(qū)域嚴格為凸環(huán)境，但是實際中常見的環(huán)境中大多為非凸區(qū)域，在很大程度上限制了機器人集群的應用場景，因此如何解決非凸問題下的多機器人控制是學者們目前的重要研究方向之一[8-9]。

為了消除多機器人集群在非凸環(huán)境下的局限性，文獻[10]中利用旅行商算法結合區(qū)域內路徑規(guī)劃問題將非凸多邊形區(qū)域分解為多個凸多邊形環(huán)境，將多個凸多邊形進行快速的覆蓋已完成非凸區(qū)域的覆蓋問題。文獻[11]將Lloyd算法和路徑規(guī)劃算法相結合，由機器人位置得到一個不考慮障礙物的凸形環(huán)境目標點，計算目標點對應于障礙物的投影點，將其設為目標進行跟蹤，并使用TangentBug算法提供了避障行為，引導機器人繞過障礙物，最終完成集群機器人在非凸環(huán)境下的覆蓋。文獻[12]使用測地距離來代替歐幾里得距離確定維諾細胞，并結合功率圖對環(huán)境中機器人的權重進行平均劃分。然而，這種方法涉及計算困難的幾何計算，導致其難以應用于實時控制的多機器人系統(tǒng)，并且仍然限于具有多邊形障礙的歐幾里得環(huán)境。文獻[13]通過改變機器人的控制率，使得CVT應用在在非凸環(huán)境中，將非凸點和代價函數相結合，最小化機器人系統(tǒng)的代價函數，使得集群機器人使用CVT算法可以完成任意環(huán)境的覆蓋。文獻[14]基于對覆蓋范圍函數的最小化來概括控制律，以適用于非凸空間環(huán)境，并結合圖搜索算法解決球面和具有障礙物的圓環(huán)上的最優(yōu)覆蓋問題。

集群機器人在實際運行過程中會受到各種影響因素，為了保證水下機器人推進器運行的穩(wěn)定性與可靠性，文獻[15]提出了一種狀態(tài)評估方法，經過BP神經網絡計算出推進器的健康度。當機器人規(guī)劃路徑中需要較大程度的拐彎等動作時，可能導致機器人運動速度、加速度過大時，需要對其做運動學約束、物理約束等約束，充分保證輪式移動機器人自主行駛的可行性，安全性，高效性。模型預測控制是近年來被廣泛討論的一種反饋控制策略，該方法在生成軌跡時會實時處理各類約束，在軌跡跟蹤時表現(xiàn)良好的性能[16]。因此，文獻[17]使用CVT算法將無人機系統(tǒng)部署在每個維諾細胞的質心位置，將系統(tǒng)模型中的噪聲以及外界風速的影響作為模型預測控制(Model Predictive Control,MPC)的約束，對無人機系統(tǒng)做出狀態(tài)估計，最終得到可行軌跡。

文中針對非凸局限性和質心不可達問題，提出了基于二次約束的CVT算法，使得機器人產生的維諾區(qū)域不與非凸環(huán)境重疊，將維諾細胞質心約束在可達區(qū)域內。為了優(yōu)化機器人運動軌跡設計了模型預測控制器，進一步保證機器人的安全。以常見的凸環(huán)境和非凸環(huán)境為例，驗證文中算法在非凸環(huán)境下劃分的有效性并做出對比分析，以比例積分控制器和文中設計的模型預測控制器在機器人軌跡異常時的控制結果為對比。以5臺Turtlebot3機器人來驗證機器人集群算法，仿真對比驗證本文算法的有效性。

1 質心維諾劃分局限性描述

1.1 質心維諾劃分算法與非凸環(huán)境

zi={q∈Ω|‖q-pi‖≤‖q-pj‖,

1≤j≤k,j≠i}，

(1)

其中相鄰的維諾細胞之間不重疊。每個維諾細胞中有且只有一個生成點，所有的維諾細胞之和就是整個環(huán)境區(qū)域。根據維諾圖的特性，當機器人作為維諾細胞的生成點時，只要保證機器人尺寸小于維諾細胞，機器人之間不會發(fā)生碰撞。

假設區(qū)域Ω的密度函數定義為ρ(q)≥0，每個Voronoi細胞的質心定義為

(2)

在維諾劃分的過程中，Lloyd算法保證生成點不斷更新并移動到維諾細胞的質心位置，整體的代價函數為

(3)

區(qū)域是幾何學的基本概念之一。對于按照幾何形狀來劃分，任務環(huán)境可以分為凸型和非凸型區(qū)域。若區(qū)域中任意兩點連線不全在區(qū)域內，則為非凸區(qū)域。圖1(a)中圓形將平面分為兩個區(qū)域，其中只有一個凸區(qū)域(圓形的內部)，外部平面即為非凸區(qū)域，即經過圓形區(qū)域部分的直線和區(qū)域不存在公共點，所以圖1(a)為非凸區(qū)域。圖1(b)中，環(huán)境區(qū)域的凹陷使得該區(qū)域為非凸區(qū)域，在區(qū)域凹陷處的上下部分取一點，所連接的直線段并不能保證所有的點仍在該區(qū)域內。

圖1 非凸任務區(qū)域環(huán)境

在凸型區(qū)域環(huán)境的質心維諾劃分中，基于Lloyd算法能夠保證生成點收斂至對應的質心位置。相對于嚴格的凸區(qū)域，實際環(huán)境中更常見的是非凸型區(qū)域環(huán)境，質心維諾劃分算法對于非凸形區(qū)域環(huán)境，存在一些局限性。如圖2所示，給出了在不同環(huán)境下的劃分情況。根據對不同環(huán)境的劃分，對質心維諾劃分算法的局限性進行分析。

圖2 非凸區(qū)域劃分存在的問題

在圖2(a)所示的凸環(huán)境中隨機生成10個生成點，圖中用圓圈所示，根據Lloyd算法，生成點最終收斂到各自細胞質心位置，均勻分布在環(huán)境區(qū)域內部。得到如圖2(a)所示的分布情況。在圖2(b)所示的非凸區(qū)域環(huán)境中，內部存在障礙物導致環(huán)境區(qū)域為非凸環(huán)境，此時對存在障礙物的環(huán)境進行質心維諾劃分，其質心可能會生成在障礙物中，如圖2(b)中紅點表示維諾細胞的質心位置，當存在質心位置在障礙物內的情況時，如果機器人將質心位置作為下一時刻的目標，則機器人便會與障礙物發(fā)生碰撞。在圖2(c)所示的非凸區(qū)域環(huán)境中，環(huán)境中并沒有障礙物，但環(huán)境外部區(qū)域存在凹陷，此時環(huán)境區(qū)域也為非凸區(qū)域，當進行維諾劃分時，質心位置會出現(xiàn)在環(huán)境外部，若下一時刻機器人運動到質心位置，則會運動到環(huán)境區(qū)域外部，即與環(huán)境發(fā)生碰撞。

通過上述分析，考慮到如果要將集群機器人應用在非凸環(huán)境中，則必須要保證兩個基本問題為：障礙物內不能有劃分的維諾細胞；每個維諾細胞質心不能在障礙物內。若對維諾細胞以及質心的位置進行嚴格約束，不僅能保證機器人之間、機器人與環(huán)境之間的安全，還可使的機器人在跟隨相應質心對環(huán)境進行覆蓋時嚴格位于環(huán)境區(qū)域內部。

1.2 機器人軌跡規(guī)劃中相關約束的數學描述

為了簡化控制器的設計，本文以使用的差速兩輪機器人為例，并假設輪式移動機器人整體為一個可以自由轉動的剛體，其簡化的運動學模型如圖3所示。

圖3 機器人運動學模型

設定x為橫向坐標，y為縱向坐標，θ是方向角。機器人的狀態(tài)信息用p=(x,y,θ)T來表示。[v,ω]T為機器人的線速度和角速度。2L為兩輪之間的長度，2r為輪子的直徑。運動學狀態(tài)空間方程為

(4)

隨著工業(yè)自動化程度的提高，對機器人也提出了越來越高的要求，機器人的任務也越來越復雜，控制要求也更加嚴格。實際環(huán)境中機器人運行會受到各種影響因素，如任務環(huán)境崎嶇不平，機器人規(guī)劃路徑中存在較大程度的拐彎等現(xiàn)象，這些現(xiàn)象在以前的任務中可能被忽略。若機器人運動速度、加速度過大，為保證機器人的安全，需要對機器人運行過程做出約束和限制，以保證機器人的安全。針對上述問題，本文對基于CVT算法的參考軌跡進行模型預測控制，提高集群機器人運行的安全性。

考慮到運行安全性，往往要求線速度v和角速度ω小于一定值，假設最大線速度和最大角速度分別為vmax和ωmax，最小線速度和最小角速度分別為vmin和ωmin，則該約束形式為

(5)

2 非凸環(huán)境中的機器人集群控制

2.1 基于二次約束的CVT算法

假設機器人集群中機器人i對應的環(huán)境維諾細胞為zi,i=1,2,…,k，環(huán)境中的障礙物區(qū)域為wi,i=1,2,…,k，為了保證維諾細胞中不包含障礙物，則機器人對應的維諾細胞被約束為

Zi=zi-wi,(i,j=1,2,3…,k)。

(6)

若環(huán)境中并無障礙物或環(huán)境為嚴格的凸環(huán)境，則w=0，維諾細胞不發(fā)生改變。當維諾細胞中存在障礙物時，維諾細胞會將障礙物區(qū)域的邊界作為內邊界，重新定義維諾細胞。

在保證了每個維諾細胞都在環(huán)境內后，考慮到在非凸環(huán)境內得到的質心位置可能出現(xiàn)在區(qū)域外面的問題，本文對質心位置進行二次約束。即將維諾細胞的質心嚴格約束在維諾細胞內，表示為

(7)

若沒有得到該約束解，則該維諾質心不更新，直到其他維諾細胞發(fā)生變化從而影響該維諾細胞質心位置在維諾環(huán)境內后，質心重新開始更新迭代，直至算法收斂。

二次約束下的質心維諾劃分算法主要思想如下：在環(huán)境細胞內，隨機生成k個生成點。首先，計算每個生成點的維諾細胞，并判斷障礙物與維諾細胞否有交集，若不存在交集，則維諾細胞內沒有障礙物；若有交集，則將障礙物的邊界作為維諾細胞的邊界，更新該生成點的維諾細胞。其次，計算每個維諾細胞的質心位置，并判斷質心位置是否在維諾細胞內。若不在，則重新計算維諾細胞；若質心在維諾細胞內，則將生成點更新至質心位置，直至算法收斂。二次約束下的質心維諾劃分算法具體過程見表1。

表1 二次約束下的質心維諾劃分算法

2.2 模型預測控制器設計

圖4中給出了使用質心維諾劃分算法結合Model Predictive Control,MPC的機器人集群系統(tǒng)體系結構。

圖4 CVT結合MPC的機器人集群系統(tǒng)結構圖

由圖4可以看出，MPC在得到機器人當前狀態(tài)信息和每個維諾細胞的質心位置后，求解滿足設定目標函數以及約束二次規(guī)劃問題，并將得到的控制序列中第一個元素作為機器人的實際控制量作用于對應的機器人，直至算法迭代至收斂，則控制結束。

根據機器人的運動學狀態(tài)空間方程，假設機器人的實際位姿為p=[x,y,θ]T，實際速度[v,ω]T，即

(8)

其中x和u分別為機器人的位姿和速度，則可得到

(9)

(10)

(11)

離散化模型為

(12)

式中：pk為機器人當前狀態(tài)變量；uk為對機器人的控制輸入變量；yk為被控輸出變量。最后使用前向歐拉進行離散化可得到

其中T為控制周期，進一步改uk為控制量增量表達式。設

(13)

Δu(k)=u(k)-u(k-1)，

(14)

則可以得到新的狀態(tài)表達式為

(15)

從式(15)中可以體現(xiàn)出使用當前狀態(tài)和當前輸入預測下一時刻狀態(tài)，新的系數表達式為

預測時域內的系統(tǒng)輸出量為

(16)

將式(16)展開，其中

式中：p′為預測時域；c為控制時域。進行軌跡跟蹤時期望和預測的軌跡具有較高的重合度，因此常通過目標函數來表示軌跡重合情況，最終得到的目標函數值越小表示兩條軌跡的重合度越高。因此將軌跡優(yōu)化問題轉化為求目標函數最小值的問題，這里選取目標函數為

(17)

式中：ρ為權重系數；ε為松弛因子。

將式(17)轉化為式(18)的形式，

(18)

將式(17)代入式(18)代價函數表達式中，得到

(19)

其中

(20)

式中：Γy和Γu分別為輸出和輸入誤差的加權矩陣；ek為預測時域內的跟蹤誤差。在對上式求解后，可以得到

(21)

將求解后的控制序列中第一個元素作為對機器人實際輸入的控制量，即

(22)

機器人得到控制量后，得到新的狀態(tài)量進入下一個控制周期。在目標函數最小化后可得到機器人約束軌跡。

3 仿真結果及分析

為分別驗證了所提算法在凸環(huán)境和非凸環(huán)境中的適用性，仿真在Matlab中進行。在對機器人進行二階動力學建模的基礎上，使用虛擬點模擬機器人進行虛擬數值仿真。以10個機器人作為集群機器人系統(tǒng)。利用代價函數表示系統(tǒng)中每個機器人和對應維諾質心位置的誤差和。若要保證集群系統(tǒng)穩(wěn)定，誤差和要趨于足夠小，文中以0.001作為判定條件，判定系統(tǒng)是否達到穩(wěn)定。

3.1 凸環(huán)境下改進CVT算法的仿真驗證

以圖5(a)中的場景作為凸環(huán)境的代表，對該算法進行驗證。圖5給出了凸環(huán)境在該算法下的最終劃分結果圖以及對應的機器人集群系統(tǒng)代價函數圖。

圖5 凸環(huán)境實驗結果圖

在如圖5(a)所示的凸環(huán)境中，算法能較快的對環(huán)境進行維諾劃分，此時環(huán)境中不存在障礙物等其他影響維諾劃分的因素，此時改進算法中代表障礙物區(qū)域的w=0。所提算法和傳統(tǒng)CVT算法都能對環(huán)境進行質心維諾劃分，得到如圖所示的機器人分布。其中不同顏色區(qū)域代表不同機器人對應的維諾細胞，即機器人在任務環(huán)境中負責的區(qū)域。

圖5(b)給出了系統(tǒng)的代價函數，由于環(huán)境較為簡單，系統(tǒng)達到穩(wěn)定所需要的迭代次數也較小，當迭代次數為51時，系統(tǒng)已經達到穩(wěn)定狀態(tài)，機器人系統(tǒng)達到環(huán)境中的最優(yōu)分布。根據代價函數的意義，最終系統(tǒng)區(qū)域穩(wěn)定時的代價函數值趨于0，表示機器人位置和其對應的維諾質心之間誤差足夠小，認定系統(tǒng)達到一個穩(wěn)定狀態(tài)。

3.2 非凸環(huán)境下改進CVT算法的仿真驗證

本節(jié)將非凸環(huán)境分為以下3種情況，第一種情況：外部邊界為凸，內部含有障礙物；第二種情況：外部邊界為非凸，內部沒有障礙物；第三種情況：外部邊界為非凸，內部含有障礙物。實驗分別對3種非凸環(huán)境進行仿真及分析。

1) 內部含有障礙物

非凸環(huán)境1假設凸環(huán)境中存在一個圓形障礙物，導致整個機器人可運行的任務環(huán)境為非凸環(huán)境。

如圖6(a)所示，為10個機器人在非凸環(huán)境1中的分布及各自對應的維諾細胞，算法有效地對該任務環(huán)境進行劃分。在常密度函數下，機器人對除障礙物以外的區(qū)域進行維諾劃分。圖6(b)為集群系統(tǒng)的代價函數圖，當迭代次數達到89次時，系統(tǒng)達到穩(wěn)定狀態(tài)。

圖6 非凸環(huán)境1實驗結果圖

2) 外部環(huán)境為非凸

非凸環(huán)境2中外部邊界為非凸，環(huán)境區(qū)域向中間凹陷。圖7為10個生成點在非凸環(huán)境2中進行質心維諾分布的過程。

圖7 非凸環(huán)境2下的質心維諾分布過程圖

如圖7(a)所示，在區(qū)域左下角隨機生成10個點，橙色的圈表示生成點的初始位置，“+”代表當前時刻每個生成點對應的維諾細胞質心位置。當算法迭代次數15次，如圖7(b)所示，維諾細胞A中，下一時刻區(qū)域質心在環(huán)境區(qū)域外部，生成點不能繼續(xù)更新，其他維諾質心繼續(xù)更新，相鄰的維諾細胞在不斷變化。當迭代次數為47時，如圖7(c)所示，維諾細胞A的形狀隨其他細胞的變換而變化，此時該區(qū)域的生成點位置仍保持在區(qū)域外部。當迭代次數為48時，區(qū)域A的維諾質心位置重新生成在區(qū)域內部，如圖7(d)所示，圖中區(qū)域A的質心位置開始運動，算法繼續(xù)迭代。當迭代次數為78時，最終形成非凸環(huán)境下的質心維諾分布。

圖8為在該環(huán)境區(qū)域下系統(tǒng)的代價函數，圖中代價函數隨著迭代次數逐漸減小，最終趨于0，系統(tǒng)最終趨于穩(wěn)定，其中當系統(tǒng)迭代78次時，系統(tǒng)中每個生成點個各自對應的維諾質心位置誤差和小于0.001，認為系統(tǒng)達到穩(wěn)定狀態(tài)。

圖8 機器人集群系統(tǒng)代價函數

3) 障礙物+外部環(huán)境為非凸

非凸環(huán)境3假設環(huán)境外部邊界為非凸且內部包含障礙物。此時算法需保證質心既不能位于區(qū)域外部，也不能位于障礙物內部。最終非凸環(huán)境3的質心維諾分布如圖9(a)所示，圖中用區(qū)域左下角隨機生成的10個橙色圓圈代表機器人的初始位置，“+”代表當前時刻每個生成點對應的維諾細胞質心位置。

在該環(huán)境中，當某個維諾細胞的質心位置出現(xiàn)在環(huán)境外部或者在障礙物內部時，該維諾細胞的生成點停止迭代。當算法迭代次數12次，如圖9(b)所示，計算得到維諾細胞B中，下一時刻區(qū)域質心在環(huán)境區(qū)域外部，此時該區(qū)域的生成點停止更新。當迭代次數到達70次，如圖9(c)所示，此時該區(qū)域的質心位置仍在區(qū)域外部，該區(qū)域的生成點位置在迭代次數12～70間都沒有更新。當迭代次數為71時，區(qū)域B的維諾質心位置生成在區(qū)域內部，如圖9(d)所示，圖中區(qū)域B的質心位置開始運動。當迭代次數為132時，最終形成非凸環(huán)境下的質心維諾分布，如圖9(e)所示。

圖9 非凸環(huán)境3質心維諾分布過程圖

機器人集群系統(tǒng)代價函數如圖10所示。由于環(huán)境場景較為復雜，算法在劃分維諾細胞及計算質心位置時的計算量較大，系統(tǒng)達到穩(wěn)定狀態(tài)時的迭代次數也隨之增加，當迭代132次時，系統(tǒng)達到穩(wěn)定。

圖10 集群系統(tǒng)代價函數

表2給出了幾種不同環(huán)境下，機器人集群系統(tǒng)穩(wěn)定時需要的迭代次數對比，實驗結果表明，在不同的非凸環(huán)境下，文中提出的算法最終都能對環(huán)境進行較為理想的劃分，但由表2也可以看出在相同機器人數量下，凸環(huán)境場景較為簡單，達到系統(tǒng)穩(wěn)定時所需的迭代次數最小，而非凸環(huán)境3是由外部環(huán)境和內部障礙物組成的較為復雜的環(huán)境，該種情況下穩(wěn)定所需的代價函數最大。即環(huán)境場景越復雜的區(qū)域，所需的代價函數越大。

表2 不同環(huán)境區(qū)域下的參數對比

3.3 比例積分控制仿真實驗及分析

在10*10的仿真環(huán)境里，給出了一個非凸幾何形狀場景，如圖11所示。

用改進后的CVT算法進行區(qū)域劃分，10個機器人從非凸環(huán)境的左下角出發(fā)，黑色的點表示每次迭代過程中對應的維諾細胞質心位置，形成的軌跡為機器人參考軌跡，“o”表示機器人位置，使用比例積分控制對其進行跟蹤，得到如圖11所示灰色軌跡。過程中出現(xiàn)兩次較為明顯的拐點，在圖11中分別用1、2進行標注?？梢钥吹剑贑VT算法計算維諾質心時得到了如圖中點線所示的參考軌跡，包括在拐點處也進行了較好的跟隨，大多數軌跡段都擬合的較好。出現(xiàn)明顯拐點是因為沒有考慮機器人和環(huán)境因素可能引起的安全性問題。但在實際環(huán)境中，當機器人運行到這兩個拐點處，由于拐點角度過大，可能會因為其速度方向突變而引發(fā)一些不安全問題，不能保證機器人運行的安全性。

圖11 基于比例積分控制的軌跡跟蹤圖

圖12給出了圖11中場景邊緣處的局部放大圖，可以看出，由于維諾細胞的非凸性，對應區(qū)域的質心位置會生成到環(huán)境外部，但機器人的運動軌跡如圖中灰色曲線所示，并沒有離開環(huán)境區(qū)域，當質心位置在環(huán)境外部時，機器人停止跟蹤，直至質心位置重新回到環(huán)境內部。再次驗證了改進CVT算法的有效性。

圖12 場景邊緣處的局部放大圖

3.4 模型預測控制控制仿真實驗及分析

對提出的基于CVT的模型預測控制控制進行仿真分析，并對機器人軌跡增加約束，驗證本文算法的有效性。給出機器人的系統(tǒng)參數，見表3。在環(huán)境的左下角隨機生成10個位置作為機器人的初始位置，目標位置由對應維諾細胞的質心給定，其他相關參數經過系統(tǒng)的不斷調試，得到見表4的參數值。

表3 機器人系統(tǒng)參數表

表4 系統(tǒng)仿真參數表

為了保證機器人運行的安全性，在機器人的系統(tǒng)參數基礎上，文中將機器人參數中最大速度的90%作為機器人的速度約束，同時對角速度也進行了限制，給出了系統(tǒng)的物理約束為

(23)

利用給出的約束條件進行了數值仿真，仿真結果如圖13所示。

圖13 基于模型預測控制的軌跡跟蹤圖

圖中黑色的點為CVT算法給出的質心軌跡，“o”為機器人當前位置，灰色線條為MPC得到的跟蹤軌跡，顯然在約束了速度和角速度后，可以看出兩個拐角部分相對于比例積分控制的效果更加平滑，圖13中仍然使用1、2進行標記，這種更加平滑的運行軌跡更加符合機器人在環(huán)境中的實際運行情況，提高了機器人的安全性。在環(huán)境場景邊緣處，改進CVT算法有也保持了自己的特性，保證機器人不會運動到環(huán)境外部。

為了更好的說明跟蹤效果，將其中一條機器人軌跡跟蹤結果分解為三個量(x,y,θ)進行分析，圖14分別給出了三個狀態(tài)量分別的控制效果，紅色線條為參考軌跡，黑色線條為跟蹤軌跡，從兩條軌跡上看，在采樣時間25 s之前的跟蹤軌跡稍有波動，之后的軌跡基本重合。

圖14 路徑跟蹤軌跡

圖15給出了三個狀態(tài)量的誤差效果圖，采樣時間小于25 s時的跟蹤效果的縱向位置誤差范圍不超過(-2 m,2 m)，θ的誤差也在(-1 rad,1 rad)內，在采樣時間大于25 s后，縱向位置與θ的跟蹤效果更佳，最終三個量的誤差基本都趨于0。

圖15 路徑跟蹤誤差

在比例控制下，機器人出現(xiàn)速度和角速度較大的情況，即采樣時間為35 s左右時，機器人出現(xiàn)速度和角速度較大的情況，如圖16(c)所示。在實際環(huán)境中，當機器人在跟蹤軌跡的某一點出現(xiàn)超出限制時，在很大程度上不能保證環(huán)境中機器人的安全性；在模型預測控制的情況下，當控制量超出了約束條件范圍外，則將控制量給定為最大控制量0.5 rad·s-1處，保證了控制量在約束范圍內，不會出現(xiàn)加速度過大的現(xiàn)象，從而保證機器人運行時的安全性。通過對比兩種情況下的機器人控制情況，可以得到本文提出基于CVT的模型預測控制能夠對機器人進行速度和加速度的約束，生成符合約束條件的可行軌跡。

兩種控制方法在一定程度上，都可以較好的對機器人軌跡進行跟蹤，但是模型預測的路徑更為平滑，為了充分說明所提算法對條件約束的處理能力，將文中2號拐點所在的路徑進行再一次的分析，對比了兩種控制算法中的控制量，如圖16所示。

圖16 控制量約束對比圖(左邊為比例控制，右邊為模型預測控制)

4 集群機器人實驗結果與分析

4.1 機器人平臺簡介

采用的Turtlebot3機器人是一款小型，低成本，完全可編程，基于ROS(Robot Operating System)的移動機器人。基本組件有用于驅動的舵機、運行ROS操作系統(tǒng)的SBC、激光雷達、用于底層控制的嵌入式開發(fā)板OpenCR，其尺寸大小為138 mm*178 mm *192 mm，是機器人體積中較小的一款產品，更利于做集群機器人研究。機器人如17圖所示。

4.2 凸環(huán)境下機器人實驗結果與分析

仿真為3.5 m*3.5 m的正方形場景，假設機器人為一階動力學模型，如圖18(a)中，圈表示機器人當前位置，在左下角生成5個隨機點作為機器人初始位置；圖18(b)為對應的實驗場景，實驗場景為3.5 m*3.5 m的正方形，在左下角隨機擺放5臺Turtlebot3機器人作為初始位置。

圖17 Turtlebot3 機器人

圖18 集群機器人初始位置

根據機器人當前位置，質心維諾劃分算法對實驗場景進行劃分，得到對應的維諾細胞以及維諾細胞質心位置，在迭代25次后，得到如圖19(a)所示的機器人分布情況，灰色線條為機器人運行軌跡。圖19(b)為機器人集群運動過程的分布圖，為了更加直觀的對比仿真和實驗結果，在實驗場景中將維諾細胞用紅的線條畫出，并使用黃色的點標注出當前機器人維諾細胞所對應的質心位置，即機器人的下一個目標位置。對比兩幅圖可以看出，實驗場景中機器人的走向以及當前位置和仿真基本一致。

圖19 迭代25次時集群機器人分布

當仿真趨于穩(wěn)定時，機器人位置和維諾質心位置重合，得到如圖20(a)所示的機器人分布以及機器人運動軌跡，在模型預測控制器作用下，機器人運動軌跡更加平滑。圖20(b)所示的實驗場景中得到了5臺機器人的最終位置分布，可以看出機器人基本到達最終目標位置。粗略對比兩幅結果圖，仿真和實驗最終的機器人位置分布情況基本相同。

圖20 迭代100次時集群機器人分布

圖20(b)中從左上角開始，順時針為機器人1、機器人2、機器人3、機器人4和機器人5，為了驗證實驗場景中機器人是否達準確到達了目標位置，表5中統(tǒng)計出了5臺機器人在實驗中的目標位置以及實驗場景中實際到達的位置，并計算了目標位置和實際位置的誤差。誤差越大表示該機器人與目標位置的距離越遠。

由于實際環(huán)境中機器人需要考慮到大小、地面摩擦以及通訊延遲等因素，故實驗場景中的機器人并沒有完全準確的達到目標位置，但從表5中可以看到，誤差值都在合理范圍內。根據實驗可得，當算法趨于穩(wěn)定后，實驗場景中每個機器人都基本到達了場景的目標位置。

表5 凸環(huán)境下機器人位置誤差

4.3 非凸環(huán)境下機器人實驗結果與分析

如圖21(a)所示，仿真場景為3.5 m*3.5 m的正方形，環(huán)境中間黑色部分為一個直徑為0.7m障礙物，為非凸型區(qū)域場景。在左下角隨機生成5生成點作為維諾劃分的初始位置。圖21(b)為對應的實驗場景，為3.5 m*3.5 m的正方形中間放置一個藍色圓柱形障礙物，其直徑為0.7 m。在實驗場景的左下方放置了5臺Turtlebot3機器人，作為機器人初始位置。

圖21 集群機器人初始位置

當仿真迭代32次時，達到如圖22(a)所示位置，在前文提出的基于二次約束下的質心維諾劃分算法的基礎下，并沒有將維諾細胞的質心位置計算至障礙物內，保證了每個機器人不會碰撞到環(huán)境中的圓形障礙物。機器人在運動過程中，利用模型預測控制得到可行軌跡。從圖22(b)的實驗場景中可以看出，實驗場景中機器人的運動過程和達到的位置與仿真中基本一致。

圖22 集群機器人運動過程

由于環(huán)境密度函數為常數，機器人集群在場景中趨于均勻分布，平分任務場景區(qū)域。根據圖23(a)的仿真實驗，5臺機器人最終收斂后到達目標位置，機器人基本上均勻地分布在場景中，對應的維諾細胞平分了整個任務區(qū)域，并且仿真中，機器人的運動軌跡也都避開了障礙物，實現(xiàn)了對非凸任務場景的合理劃分。實驗場景23(b)中，5臺機器人均勻分布在場景內部，和實驗中機器人最終到達的目標位置基本一致。

圖23 集群機器人分布圖

從左上角開始，順時針為機器人1、機器人2、機器人3、機器人4和機器人5，為了驗證機器人最終是否到達目標位置，表6中統(tǒng)計出了5臺機器人在實驗中的目標位置以及實驗場景中實際到達的位置，并計算了目標位置和實際位置的誤差，表6中位置誤差都在合理范圍內。結果表明，當算法趨于穩(wěn)定后，實驗場景中每個機器人都基本達到了場景的最優(yōu)位置。

表6 非凸環(huán)境下機器人位置誤差

實驗過程中，通過二次約束下的質心維諾劃分算法對非凸環(huán)境區(qū)域進行劃分，得到在非凸區(qū)域環(huán)境下的質心軌跡，結合模型預測控制保證了機器人在運動到過程的安全性；在實驗場景中，機器人的運動過程和對應的仿真環(huán)境大致相同，最終都得到了場景中機器人的最終分布。表明了本文算法能夠完成真實場景的集群任務，并安全準確地到達最終目標位置。

5 結 論

1) 針對CVT算法在凸環(huán)境應用中的局限性，通過二次約束的方法，對維諾細胞及質心位置分別進行約束，以得到符合環(huán)境的集群機器人最終分布。將改進的CVT算法得到的質心軌跡作為MPC的參考軌跡，生成符合機器人動力學約束的可行軌跡，保證了機器人安全可行的達到CVT收斂位置。

2) 通過不同非凸環(huán)境的質心維諾劃分仿真，對二次約束的CVT算法進行驗證表明算法的可行性；結合MPC算法進行仿真，表明該方法可以有效解決各類約束問題，充分驗證了機器人在非凸環(huán)境中自主運行的安全性。在5臺ROS機器人上進行了實驗驗證，文中算法可以保證機器人在不同環(huán)境下安全的達到最終目的，完成集群相應任務。

3) 在未來的工作中，應當考慮到機器人的健康退化，通過模型預測控制在保證機器人安全的同時保證機器人集群整體性能、壽命的延長，設計一種可以平衡機器人集群系統(tǒng)性能，使機器人集群壽命達到最大化的方法的同時，也是保證機器人集群完成任務的堅實基礎。