本發(fā)明涉及多移動(dòng)機(jī)器人編隊(duì)控制技術(shù)領(lǐng)域，具體地說，是一種利用機(jī)器人行進(jìn)過程中記錄的路徑信息來預(yù)測(cè)編隊(duì)集結(jié)地點(diǎn),從而加速機(jī)器人編隊(duì)的方法。

背景技術(shù)：

隨著技術(shù)的革新和機(jī)器人應(yīng)用領(lǐng)域的不斷拓展，單機(jī)器人在某些方面已經(jīng)不能夠滿足人類生產(chǎn)生活的需要。采用多個(gè)結(jié)構(gòu)和傳感能力相對(duì)簡(jiǎn)單的單個(gè)機(jī)器人組成多機(jī)器人協(xié)作系統(tǒng)在節(jié)省機(jī)器人結(jié)構(gòu)設(shè)計(jì)成本的同時(shí)也增加了安全性。多機(jī)器人協(xié)作作為多機(jī)器人系統(tǒng)研究的熱點(diǎn)問題之一，在經(jīng)歷了20余年的發(fā)展歷程后，無論在軍事、國(guó)防，還是工業(yè)、生活等領(lǐng)域都展現(xiàn)了其廣泛的應(yīng)用空間。多機(jī)器人協(xié)作系統(tǒng)研究的基本問題包括編隊(duì)控制、地圖構(gòu)建、無人救援、目標(biāo)圍捕以及協(xié)同搬運(yùn)等，其中多機(jī)器人編隊(duì)控制作為多機(jī)器人系統(tǒng)研究領(lǐng)域中一類廣泛存在的基礎(chǔ)問題，是研究其它問題的前提。

編隊(duì)控制指的是多個(gè)可移動(dòng)設(shè)備(例如機(jī)器人，人造衛(wèi)星，無人飛行器，自主式水下探測(cè)器等)所組成的團(tuán)隊(duì)，在向目標(biāo)地點(diǎn)移動(dòng)或者在指定位置集結(jié)的過程中，相互之間保持特定的空間位置關(guān)系，即隊(duì)形，同時(shí)又要適應(yīng)環(huán)境約束的控制問題。通常而言，編隊(duì)控制借助機(jī)器人之間的局部交互實(shí)現(xiàn)多機(jī)器人系統(tǒng)的群體行為，從而解決全局性的任務(wù)。

近年來，學(xué)者和工程師對(duì)多機(jī)器人系統(tǒng)編隊(duì)進(jìn)行了廣泛而深入的研究。Abel R等人通過適當(dāng)?shù)倪x擇幾對(duì)機(jī)器人，并確定其在隊(duì)形中的相對(duì)位置關(guān)系，通過設(shè)計(jì)反饋控制控制律，最終使得多機(jī)器人系統(tǒng)趨向目標(biāo)隊(duì)形(Coordinated fault-tolerant control of autonomous agents:Geometry and communications architecture[C]//Proc.of the IFAC Congress,Prague(Czech Republic),IFAC.2005.)。Lin Z等人通過利用復(fù)拉普拉斯矩陣設(shè)計(jì)多智能體的交互網(wǎng)絡(luò)拓?fù)?，從而?shí)現(xiàn)了利用分布式控制形成任意圖形的編隊(duì)(Distributed formation control of multi-agent systems using complex laplacian[J].Automatic Control,IEEE Transactions on,2014,59(7):1765-1777.)。Olfati-Saber R等人研究了圖的剛性與分布式編隊(duì)的穩(wěn)定性(Graph rigidity and distributed formation stabilization of multi-vehicle systems[C]//Decision and Control,2002,Proceedings of the 41st IEEE Conference on.IEEE,2002,3:2965-2971.)。中國(guó)專利文獻(xiàn)CN102331711A是本發(fā)明最接近的現(xiàn)有技術(shù)。

從圖形編隊(duì)控制的角度看，大部分關(guān)于編隊(duì)控制的現(xiàn)有的文獻(xiàn)可以分為兩類。一類旨在解決如何形成特定形狀的編隊(duì)圖形。另一類則是意在解決如何保持編隊(duì)圖形的穩(wěn)定性。如何形成特定形狀的編隊(duì)圖形，通常也被稱為集結(jié)問題，主要目的在于使多個(gè)機(jī)器人形成特定的形狀。二維平面的編隊(duì)集結(jié)問題已有較好的方法解決，即Lin Z等人提供的利用復(fù)拉普拉斯進(jìn)行的分布式控制技術(shù)，其結(jié)果基本可以保證任意編隊(duì)圖形都能形成。但是此方法還存在一些不足。一方面，在實(shí)際應(yīng)用中，存在這樣一種情況，機(jī)器人最終達(dá)到的集結(jié)位置可能是不可到達(dá)點(diǎn)，如墻角，障礙物等，亦或是危險(xiǎn)點(diǎn)，如陷阱。目前的方法缺乏對(duì)機(jī)器人未來目標(biāo)位置的預(yù)測(cè)，可能最終使機(jī)器人落入這些點(diǎn)，輕則編隊(duì)失敗，重則整個(gè)系統(tǒng)崩潰。另一方面，由于分布式控制本身存在的問題，控制量隨著編隊(duì)的過程逐漸減小，使得整個(gè)編隊(duì)的收斂速度較慢，且隨著機(jī)器人個(gè)數(shù)的增多，編隊(duì)的收斂速度會(huì)更慢。針對(duì)以上兩個(gè)問題，本發(fā)明提出了最小步編隊(duì)集結(jié)預(yù)測(cè)方法。通過該方法，每個(gè)機(jī)器人在集結(jié)過程中，在最少離散步內(nèi)利用自身所具有的信息預(yù)測(cè)集結(jié)位置，為多移動(dòng)機(jī)器人在各種實(shí)際應(yīng)用中提高編隊(duì)穩(wěn)定性和編隊(duì)效率提供關(guān)鍵技術(shù)。

技術(shù)實(shí)現(xiàn)要素：

針對(duì)多個(gè)可移動(dòng)機(jī)器人在二維平面形成特定隊(duì)形的問題，本發(fā)明要克服現(xiàn)有技術(shù)不能滿足高效率編隊(duì)任務(wù)的缺點(diǎn)，在現(xiàn)有的復(fù)拉普拉斯編隊(duì)的基礎(chǔ)上，本發(fā)明提供一種多移動(dòng)機(jī)器人的最小步編隊(duì)方法，旨在每個(gè)機(jī)器人在最短時(shí)間內(nèi)能預(yù)測(cè)其在編隊(duì)最終的集結(jié)位置。

本發(fā)明的多移動(dòng)機(jī)器人的最小步編隊(duì)方法，首先，將機(jī)器人在二維平面的運(yùn)動(dòng)建模，用復(fù)數(shù)來表示機(jī)器人在二維平面內(nèi)的坐標(biāo)，以一列復(fù)數(shù)向量來表示機(jī)器組人的當(dāng)前位置，接著設(shè)計(jì)機(jī)器人的交互拓?fù)?，然后用一列?fù)數(shù)向量來表示機(jī)器人組的目標(biāo)隊(duì)形，并求取復(fù)拉普拉斯算子來設(shè)計(jì)分布式控制律，再求取穩(wěn)定矩陣，最后通過記錄機(jī)器人的位移信息，求得機(jī)器人的目標(biāo)位置。具體步驟如下：

步驟1，建立運(yùn)動(dòng)模型

首先建立全局坐標(biāo)系，用復(fù)平面表示多機(jī)器人移動(dòng)的二維平面空間。比如，在二維平面的坐標(biāo)表示為(a,b)，那么該點(diǎn)在復(fù)平面中表示為a+bj，其中j表示單位虛數(shù)即表示復(fù)數(shù)的集合。一列n維復(fù)數(shù)向量表示n個(gè)機(jī)器人的目標(biāo)隊(duì)形。視機(jī)器人為無碰撞體積的質(zhì)點(diǎn)，第i個(gè)機(jī)器人在平面中的位置表示為用一列n維復(fù)數(shù)向量表示n個(gè)機(jī)器人的位置：

x＝(x₁,x₂,…,x_n)^T (1)

其中(·)^T為矩陣的轉(zhuǎn)置。系統(tǒng)中的每個(gè)機(jī)器人都服從單積分器運(yùn)動(dòng)模型：

是第i個(gè)機(jī)器人的速度輸入信號(hào)。

步驟2，建立多機(jī)器人系統(tǒng)的拓?fù)鋱D

將多機(jī)器人系統(tǒng)及其相互之間的局部交互表示為無向拓?fù)鋱DG＝(V,E)，其中V＝{v₁,v₂,…v_n}表示圖中的n個(gè)節(jié)點(diǎn)的集合，v_i表示圖中第i個(gè)節(jié)點(diǎn)，即第i個(gè)機(jī)器人，表示節(jié)點(diǎn)與節(jié)點(diǎn)之間的邊的集合，e_ik∈E表示機(jī)器人i能測(cè)量機(jī)器人k的相對(duì)位置d＝ρj^θ，其中ρ表示兩個(gè)機(jī)器人之間的距離，θ表示機(jī)器人k相對(duì)于機(jī)器人i的角度。由于G＝(V,E)是無向圖，所以如果e_ik∈E，那么e_ki∈E，即機(jī)器人k也能測(cè)量機(jī)器人i的相對(duì)位置。添加邊e₁₂,e₂₃,…,e_(n-1)n,e_n1至圖中使所有節(jié)點(diǎn)均在同一圓環(huán)上。

步驟3，求取復(fù)拉普拉斯矩陣

對(duì)應(yīng)圖無向圖G＝(V,E)的鄰接矩陣W，如果存在e_ik∈E，那么w_ik≠0,反之如果那么w_ik＝0，w_ik表示矩陣W第i行第k列個(gè)元素。

定義復(fù)拉普拉斯矩陣L，

式(3)中∑(·)為求和符。

編隊(duì)圖形可以由下式表示：

η＝c₁1_n+c₂ξ (4)

其中，1_n表示一列含有n個(gè)元素，且元素全為1的向量，為一列含有n個(gè)復(fù)數(shù)元素的向量，表示隊(duì)形基，且ξ≠1_n。c₁和c₂為任意復(fù)數(shù)。

通過求解矩陣方程組計(jì)算復(fù)拉普拉斯矩陣L：

步驟4，將復(fù)拉普拉斯矩陣的極點(diǎn)配置在右半平面

為保證本發(fā)明中編隊(duì)系統(tǒng)是穩(wěn)定的，復(fù)拉普拉斯矩陣的特征值必須配置在右半平面。記λ_i為n個(gè)矩陣L的需配置特征值，i＝1,2,…,n。配置特征值即求解下述方程組：

det(·)是行列式運(yùn)算符，表示計(jì)算其后括號(hào)內(nèi)矩陣的行列式值。其中，

由于有兩個(gè)特征值已存在，不失一般性，令λ_n＝λ_n-1＝0，并可設(shè)d_n＝d_n-1＝1。記可用牛頓迭代法求解式(6)，具體如下：

記：

記：

并記：

其中，表示函數(shù)f_i對(duì)d_k求偏導(dǎo)數(shù)。記初值為

迭代計(jì)算下述算式：

直至‖(·)‖表示求取式(·)的二范數(shù)，δ表示計(jì)算精度，取δ＝0.0001。

重新配置極點(diǎn)后的復(fù)拉普拉斯矩陣：

步驟5，將連續(xù)系統(tǒng)轉(zhuǎn)換為離散系統(tǒng)

機(jī)器人的控制信號(hào)由機(jī)器人與其鄰居機(jī)器人位置差的加權(quán)組合決定：

其中u_i表示第i個(gè)機(jī)器人的速度控制輸入，和分別表示第i和第k個(gè)機(jī)器人的位置。N_i表示節(jié)點(diǎn)i的鄰居節(jié)點(diǎn)的集合，N_i＝{v_k|e_ik∈E}。在此控制信號(hào)輸入下，全局動(dòng)態(tài)響應(yīng)為：

由于在實(shí)際應(yīng)用中控制信號(hào)以離散時(shí)間信號(hào)給出，所以其對(duì)應(yīng)的離散時(shí)間響應(yīng)：

其中ε為采樣時(shí)間，取值范圍max{·}表示求取集合{·}的最大值。

步驟6，記錄位移信息并計(jì)算最終位置

根據(jù)基于復(fù)拉普拉斯矩陣的離散時(shí)間分布式控制律，機(jī)器人漸進(jìn)收斂至目標(biāo)隊(duì)形。在此過程中，每個(gè)機(jī)器人記錄自身位移信息，并計(jì)算最終位置。為敘述簡(jiǎn)便，選擇第i個(gè)機(jī)器人對(duì)算法加以說明，算法具體思路如下:

(1)機(jī)器人系統(tǒng)在下述離散時(shí)間響應(yīng)下逐漸收斂至目標(biāo)隊(duì)形，

x(k+1)＝Ax(k)

觀測(cè)第i個(gè)機(jī)器人的位移信息：

其中表示一列除第i個(gè)元素為1以外全為0的向量。

(2)第i個(gè)機(jī)器人記錄位移信息x_i(k)，k＝1,2,3,…，并以此構(gòu)建Hankel矩陣H：

(3)當(dāng)Hankel矩陣H(x_i(k+1)-x_i(k))第一次失秩時(shí)，計(jì)算其零空間，并記為ρ，并記機(jī)器人移動(dòng)2s+1步，s是正整數(shù)。

(4)通過下式計(jì)算觀測(cè)節(jié)點(diǎn)的

其中是一列全為1的向量，x_i(∞)表示第i個(gè)節(jié)點(diǎn)最終位置。對(duì)一個(gè)n個(gè)機(jī)器人的系統(tǒng)，對(duì)任意機(jī)器人而言，均有2s+2≤2n，即機(jī)器人至多移動(dòng)2n-1步即可算出最終位置。

(5)通過調(diào)整控制律加快編隊(duì)速度：

每個(gè)機(jī)器人根據(jù)計(jì)算得到的最終位置調(diào)整控制律：

其中(x_i(∞)-x_i)是加速系統(tǒng)收斂控制項(xiàng)。

在實(shí)際的應(yīng)用過程中，需要多移動(dòng)機(jī)器人以各種各樣的編隊(duì)形狀以滿足各種任務(wù)需求，現(xiàn)有機(jī)器人編隊(duì)技術(shù)整體收斂速度慢，影響機(jī)器人工作效率。本發(fā)明利用了機(jī)器人編隊(duì)控制內(nèi)在的線性關(guān)系，使得機(jī)器人在最短時(shí)間內(nèi)通過分析自身的軌跡路徑能夠得出最終位置，加入收斂加速控制項(xiàng)，從而加速機(jī)器人群的編隊(duì)過程。本發(fā)明是完全分布式的，不依賴全局坐標(biāo)系，機(jī)器人不需要安裝

附圖說明

圖1為本發(fā)明示例的目標(biāo)隊(duì)形圖形

圖2為本發(fā)明的拓?fù)涫疽鈭D

圖3原復(fù)拉普拉斯矩陣編隊(duì)收斂過程

圖4為在本發(fā)明算法控制下的機(jī)器人編隊(duì)收斂過程

具體實(shí)施方式

以下結(jié)合附圖和實(shí)際編隊(duì)案例對(duì)本發(fā)明新型的技術(shù)方案作進(jìn)一步描述。

針對(duì)有一個(gè)由5個(gè)機(jī)器人組成的多移動(dòng)人系統(tǒng)，5個(gè)機(jī)器人分布在二維平面上，其坐標(biāo)為(-1.3077,-1.3499)，(-0.4336,3.0349)，(0.3426,0.7254)，(3.5784,-0.0631)，(2.7694,0.7147)，需要組成如圖1所示的特定形狀隊(duì)形，該形狀二維平面空間中可由坐標(biāo)表示為(-1,1)，(0,0)，(1,1)，(1,-1)，(-1,-1)，針對(duì)該案例對(duì)算法過程進(jìn)行演繹：

步驟1，建立復(fù)平面內(nèi)的多移動(dòng)機(jī)器人運(yùn)動(dòng)模型

首先建立全局坐標(biāo)系，將多機(jī)器人移動(dòng)的二維平面空間用復(fù)平面表示，二維平面的任意點(diǎn)坐標(biāo)表示為(a,b)，那么該點(diǎn)在復(fù)平面中表示為a+bj，其中j表示單位虛數(shù)即視機(jī)器人為無碰撞體積的質(zhì)點(diǎn)，是一列表示5個(gè)機(jī)器人的位置的向量，表示第i個(gè)機(jī)器人在平面中的位置。每個(gè)機(jī)器人的動(dòng)力學(xué)方程由下式給出：

其中表示第i個(gè)節(jié)點(diǎn)的速度輸入信號(hào)。

步驟2，建立多機(jī)器人系統(tǒng)的拓?fù)鋱D

將多機(jī)器人系統(tǒng)及其相互之間的局部交互表示為無向拓?fù)鋱DG＝(V,E)，其中V＝{v₁,v₂,…v₅}表示圖中的5個(gè)節(jié)點(diǎn)的集合，v_i表示圖中第i個(gè)節(jié)點(diǎn)，即第i個(gè)機(jī)器人，表示節(jié)點(diǎn)與節(jié)點(diǎn)之間的邊的集合，e_ik∈E表示機(jī)器人i能測(cè)量機(jī)器人k的相對(duì)位置d＝ρj^θ，其中ρ表示兩個(gè)機(jī)器人之間的距離，θ表示機(jī)器人k相對(duì)于機(jī)器人i的角度。由于G＝(V,E)是無向圖，所以如果e_ik∈E，那么e_ki∈E，即機(jī)器人k也能測(cè)量機(jī)器人i的相對(duì)位置。添加邊e₁₂,e₂₃,e₃₄,e₄₅,e₅₁至圖中使所有節(jié)點(diǎn)均在同一圓環(huán)上。最后得到如圖2所示的多機(jī)器人系統(tǒng)的拓?fù)鋱D。

步驟3，求取復(fù)拉普拉斯矩陣

對(duì)應(yīng)圖無向圖G＝(V,E)的鄰接矩陣W，如果存在e_ik∈E，那么W_ik≠0,反之如果那么W_ik＝0。根據(jù)步驟2設(shè)計(jì)的無向G＝(V,E)，可知

由復(fù)拉普拉斯矩陣L，

得：

根據(jù)要求，將目標(biāo)的隊(duì)形表示為

η＝[-1+j,0,1+j,1-j,-1-j]^T

該隊(duì)形可以由下式表示：

η＝c₁1_n+c₂ξ

不失一般性，取c₁＝0，c₂＝1:

η＝c₁1_n+c₂ξ＝ξ

通過求解矩陣方程組計(jì)算復(fù)拉普拉斯矩陣L：

得：

配置復(fù)拉普拉斯矩陣的特征值，使得復(fù)拉普拉斯矩陣的特征值只有兩個(gè)在零點(diǎn)，剩余的特征值均在復(fù)平面的右半平面。

步驟4，求取穩(wěn)定矩陣

目標(biāo)配置極點(diǎn)為λ₁＝1,λ₂＝2,λ₃＝3,λ₄＝0,λ₅＝0。配置特征值即求解下述方程組：

其中對(duì)角矩陣

由于有兩個(gè)特征值已存在，可設(shè)d₄＝d₅＝1。記用牛頓迭代法求解，具體如下：

記：

記：

并記：

記初值為

迭代計(jì)算下述算式：

直至‖(·)‖表示求取式(·)的模，δ表示計(jì)算精度，取δ＝0.0001。循環(huán)10次即滿足條件：

重新配置極點(diǎn)后的復(fù)拉普拉斯矩陣：

步驟5，計(jì)算分布式離散控制信號(hào)

每個(gè)機(jī)器人的速度控制信號(hào)由下式給出：

其中u_i表示第i個(gè)機(jī)器人的速度控制輸入，和分別表示第i和第k個(gè)機(jī)器人的位置。在此控制信號(hào)輸入下，全局動(dòng)態(tài)響應(yīng)為：

在實(shí)際應(yīng)用中控制信號(hào)以離散時(shí)間信號(hào)給出，其對(duì)應(yīng)的離散時(shí)間響應(yīng)：

其中ε為采樣時(shí)間，取

步驟6，記錄位移信息并計(jì)算最終位置

根據(jù)基于復(fù)拉普拉斯矩陣的離散時(shí)間分布式控制律，機(jī)器人漸進(jìn)收斂至目標(biāo)隊(duì)形。在此過程中，機(jī)器人記錄自身位移信息，并計(jì)算最終位置。不失一般性，選擇第3個(gè)機(jī)器人例說明，算法具體如下:

(1)機(jī)器人系統(tǒng)在下述離散時(shí)間響應(yīng)下逐漸收斂至目標(biāo)隊(duì)形，其過程如圖3所示

x(k+1)＝Ax(k)

測(cè)記錄第三個(gè)機(jī)器人的位移信息：

x₃(k)＝(0,0,1,0,0)^Tx(k)

初始值

(2)第3個(gè)機(jī)器人記錄位移信息x₃(k)，k＝1,2,3,…，并以此構(gòu)建Hankel矩陣：

(3)當(dāng)Hankel矩陣H(x₃(k+1)-x₃(k))第一次失秩時(shí)，計(jì)算其零空間，并記為ρ，

ρ＝(-0.0485,0.3559,-0.7765,0.5177)

機(jī)器人共移動(dòng)7步。

(4)通過下式計(jì)算觀測(cè)節(jié)點(diǎn)的

其中

其余機(jī)器人亦可用同樣方法求得其最終位置。

(5)通過調(diào)整控制律加快編隊(duì)速度

每個(gè)機(jī)器人根據(jù)計(jì)算得到的最終位置調(diào)整控制律：

其中(x_i(∞)-x_i)是加速系統(tǒng)收斂控制項(xiàng)。在調(diào)整控制律后，系統(tǒng)編隊(duì)過程如圖4所示。

以上闡述的是本發(fā)明給出的實(shí)例，仿真結(jié)果體現(xiàn)了本發(fā)明所提出的技術(shù)方案的有效性。需要指出的是，本發(fā)明不只限于上述實(shí)施例，當(dāng)系統(tǒng)中不存在全局坐標(biāo)系統(tǒng)時(shí)，機(jī)器人可通過對(duì)速度積分計(jì)算自身位移信息，采用本發(fā)明的技術(shù)方案，亦能有效預(yù)測(cè)最終位置。

本發(fā)明所涉及的預(yù)測(cè)算法，主要用于幫助機(jī)器人在最小步內(nèi)預(yù)測(cè)自身的目標(biāo)位置，一方面，為避開不可達(dá)點(diǎn)，危險(xiǎn)點(diǎn)提供了一種參考信息，另一方面，也為加快分布式編隊(duì)提供了關(guān)鍵技術(shù)。