本發(fā)明涉及一種多移動機器人的軌跡預(yù)測方法，具體是一種機器人分布式編隊的過程中，每個機器人通過記錄自身的路徑信息，在最短時間內(nèi)預(yù)測未來移動軌跡，從而為個體提供更多信息以便于實時避障或保持編隊穩(wěn)定性。

背景技術(shù)：

隨著技術(shù)的革新和機器人應(yīng)用領(lǐng)域的不斷拓展，多移動機器人系統(tǒng)近年來受到學(xué)者們的廣泛關(guān)注，其技術(shù)也獲得了長足的發(fā)展。采用多個結(jié)構(gòu)和傳感能力相對簡單的單個機器人組成多機器人協(xié)作系統(tǒng)在節(jié)省機器人結(jié)構(gòu)設(shè)計成本的同時也增加了安全性。多機器人協(xié)作作為多機器人系統(tǒng)研究的熱點問題之一，在經(jīng)歷了20余年的發(fā)展歷程后，無論在軍事、國防，還是工業(yè)、生活等領(lǐng)域都展現(xiàn)了其廣泛的應(yīng)用空間。

多機器人協(xié)作系統(tǒng)研究的基本問題包括編隊控制、地圖構(gòu)建、無人救援、目標(biāo)圍捕以及協(xié)同搬運等。其中，多機器人編隊控制是多機器人系統(tǒng)協(xié)同完成其它任務(wù)的前提。多移動機器人編隊控制是指機器人團隊依賴傳感器感知周圍環(huán)境和自身狀態(tài)，相互協(xié)作完成編隊，在存在障礙物的環(huán)境中實現(xiàn)面向目標(biāo)的自主運動。編隊控制行為在自然界中隨處可見，例如大雁列隊飛行，魚群結(jié)隊游行，狼群編隊捕食等。在人類活動中，編隊行為也被廣泛采用，例如軍事土的機群編隊，航母、軍艦混合編隊等。此外，研究多機器人編隊控制控制的目的是為了最終能夠在實際環(huán)境中應(yīng)用，多機器人協(xié)作搬運作為多機器人編隊控制控制的一個典型應(yīng)用實例，近年來受到了廣泛的關(guān)注?，F(xiàn)實生活中，當(dāng)需要搬運大型、沉重的物體時，往往需要多人以協(xié)作搬運的方式來完成。與此類似，若用機器人代替人類完成上述搬運任務(wù)時，當(dāng)被運輸?shù)奈矬w超過單個機器人的承載能力，使用多機器人協(xié)作搬運將是完成任務(wù)的一種有效途徑。多機器人協(xié)作搬運物體本質(zhì)上是一個具有約束條件下的多機器人隊形保持問題，該問題可以描述為:給定任意的剛性多面體，多機器人成員能夠以一條無碰撞的路徑將該物體從起始點運動到最終目標(biāo)點。

近年來，針對多移動機器人的編隊分析與研究已取得了很大的進展。linz在文獻distributedformationcontrolofmulti-agentsystemsusingcomplexlaplacian[j](ieeetransactionsonautomaticcontrol,2014,59(7):1765-1777)中利用復(fù)拉普拉斯網(wǎng)絡(luò)通信協(xié)議使多個機器人收斂到目標(biāo)圖形。linz在文獻leader–followerformationviacomplexlaplacian[j](automatica,2013,49(6):1900-1906)通過改進復(fù)拉普拉斯網(wǎng)絡(luò)通信協(xié)議，引入領(lǐng)航-跟隨結(jié)構(gòu)，使得多個機器人在兩個領(lǐng)航機器人的帶領(lǐng)下，保持目標(biāo)隊形在二維平面的移動。wangl在文獻formationcontrolofdirectedmulti-agentnetworksbasedoncomplexlaplacian[c](2012ieee51stieeeconferenceondecisionandcontrol(cdc).ieee,2012:5292-5297)中將復(fù)拉普拉斯網(wǎng)絡(luò)通信協(xié)議從無向網(wǎng)絡(luò)拓?fù)渫卣沟接邢蚓W(wǎng)絡(luò)拓?fù)?。然而上述研究都集中于移動機器人個體間的網(wǎng)絡(luò)拓?fù)涮匦裕ㄟ^改變網(wǎng)絡(luò)拓?fù)鋮?shù)及移動機器人間的控制協(xié)議達(dá)到控制目標(biāo)，而忽略了單個機器人在機器人編隊中的作用，無法從根本上進行系統(tǒng)設(shè)計使整個多移動機器人系統(tǒng)滿足一定的性能指標(biāo)要求，也很難靈活地實現(xiàn)不同的全局控制目標(biāo)。

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

本發(fā)明要克服現(xiàn)有技術(shù)對移動機器人未來估計信息不足的缺點，針對多個可移動機器人在二維平面內(nèi)以固定隊形移動的軌跡預(yù)測問題，在現(xiàn)有的復(fù)拉普拉斯編隊的基礎(chǔ)上，本發(fā)明提供一種多移動機器人的軌跡預(yù)測方法，以最小步預(yù)測算法在最短時間內(nèi)幫助每個機器人預(yù)測其未來軌跡。

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

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

首先建立全局坐標(biāo)系，將多機器人移動的二維平面空間用復(fù)平面表示，二維平面的任意點坐標(biāo)表示為(a,b)，那么該點在復(fù)平面中表示為a+bj，其中j表示單位虛數(shù)即a和b都表示任意實數(shù)。將目標(biāo)的隊形表示為一列復(fù)數(shù)向量n為自然數(shù)，表示復(fù)數(shù)的集合。視機器人為無碰撞體積的質(zhì)點，表示第i個機器人在平面中的位置，是一列表示n個機器人的位置的向量：

x＝(x1,x2,…,xn)^t(1)

其中(·)^t表示矩陣的轉(zhuǎn)置。

單個機器人的動力學(xué)方程：

其中表示第i個機器人的速度輸入信號，表示括號內(nèi)的式子對時間求導(dǎo)。

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

將多機器人系統(tǒng)及其相互之間的局部交互表示為無向拓?fù)鋱Dg＝(v,e)，其中v＝{v1,v2,…vn}表示圖中的n個節(jié)點的集合，vi表示圖中第i個節(jié)點，即第i個機器人，表示節(jié)點與節(jié)點之間的邊的集合，eik∈e表示機器人i能測量機器人k的相對位置d＝ρj^θ，其中ρ表示兩個機器人之間的距離，θ表示機器人k相對于機器人i的角度。由于g＝(v,e)是無向圖，所以如果eik∈e，那么eki∈e，即機器人k也能測量機器人i的相對位置。添加邊e12,e23,…,e(n-1)n,en1至圖中使所有節(jié)點均在同一圓環(huán)上。

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

對應(yīng)圖無向圖g＝(v,e)的鄰接矩陣w，如果存在eik∈e，那么wik≠0,反之如果那么wik＝0，wik表示矩陣w第i行第k列個元素。

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

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

編隊圖形可以由下式表示：

η＝c11n+c2ξ(4)

其中，1n表示一列含有n個元素，且元素全為1的向量，為一列含有n個復(fù)數(shù)元素的向量，表示隊形基，且ξ≠1n。c1和c2為任意復(fù)數(shù)。

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

可任選復(fù)拉普拉斯矩陣l的其中兩行全為0以表示其最后兩個節(jié)點為領(lǐng)航者節(jié)點，不受其余節(jié)點的影響不失一般性，令最后復(fù)拉普拉斯矩陣l最后兩排全為0。

步驟4，配置復(fù)拉普拉斯矩陣的特征值

配置復(fù)拉普拉斯矩陣的特征值，使得復(fù)拉普拉斯矩陣的特征值只有兩個在零點，剩余的特征值均在復(fù)平面的右半平面。記λi為n個矩陣l的需配置特征值，i＝1,2,…,n。配置特征值即求解下述方程組：

det(·)是行列式運算符，表示計算其后括號內(nèi)矩陣的行列式值。

對角矩陣：

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

記：

記：

并記：

其中，表示函數(shù)fi對dk求偏導(dǎo)數(shù)。記初值為

迭代計算下述算式：

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

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

跟隨機器人的速度控制信號由下式給出：

領(lǐng)航機器人的速度控制信號由下式給出：

ui＝μ(t),i＝n-1,n(13)

其中ui表示第i個機器人的速度控制輸入，和分別表示第i和第k個機器人的位置。μ(t)表示隨時間變化，且其z變換極點為有限個的信號。ni表示節(jié)點i的鄰居節(jié)點的集合，ni＝{vk|eik∈e}。在此控制信號輸入下，全局動態(tài)響應(yīng)為：

由于在實際應(yīng)用中控制信號以離散時間信號給出，所以考慮其對應(yīng)的離散時間響應(yīng)：

x(k+1)＝(i-εl)x(k)+u(k)＝ax(k)+u(k)(15)

其中ε為采樣時間，取值范圍λmax為最大特征值。

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

根據(jù)分布式離散控制信號，機器人漸進收斂至目標(biāo)隊形，并保持該隊形在平面內(nèi)移動。在此過程中，機器人記錄自身位移信息，并計算最終位置。選擇第i個機器人為觀測節(jié)點，算法具體方法如下:

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

x(k+1)＝ax(k)++u(k)

觀測第i個機器人的位移信息：

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

(1)第i個機器人記錄位移信息xi(k)，k＝1,2,3,…，并以此構(gòu)建hankel矩陣h：

(2)當(dāng)hankel矩陣h(xi(k+1)-xi(k))第一次失秩時，計算其零空間，并記為ρ，并記機器人移動2s+1步。

(3)通過下式迭代計算觀測節(jié)點的未來路徑

其中是一列全為1的向量。

本項技術(shù)主要目的在于幫助機器人提前預(yù)知自身的軌跡路線，以方便機器人在復(fù)雜的工作環(huán)境中，適時的根據(jù)預(yù)測信息與感知信息，調(diào)整編隊策略。以工業(yè)領(lǐng)域為例，大量機器人組成的機器人編隊在危險物品的檢查、收集和搬運的過程中，所處的環(huán)境往往是危險或且復(fù)雜的。機器人群需要根據(jù)這些環(huán)境適當(dāng)?shù)恼{(diào)整自身的控制策略，改變編隊形狀，或隊形大小，在這一過程中，每個機器人都需要根據(jù)自己未來的軌跡做出判斷。而在編隊過程中，跟隨機器人往往只能接收到領(lǐng)航機器人的控制信號，缺乏自身對未來路徑的判斷。針對這種情況，本專利提供了一種分布式預(yù)測方法，使用該方法，每個跟隨機器人都能獨立的預(yù)測自身的未來路徑。

附圖說明

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

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

圖3多移動機器人收斂至目標(biāo)隊形并平移

圖4對節(jié)點3應(yīng)用算法在第12步計算出未來軌跡

具體實施方式

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

假設(shè)有一個由5個機器人組成的多移動人系統(tǒng)，5個機器人分布在二維平面上，其坐標(biāo)為(-1.3077,-1.3499)，(-0.4336,3.0349)，(0.3426,0.7254)，(3.5784,-0.0631)，(2.7694,0.7147)，需要組成如圖1所示的特定形狀隊形，該形狀二維平面空間中可由坐標(biāo)表示為(-1,1)，(0,0)，(1,1)，(1,-1)，(-1,-1)，其中，兩個領(lǐng)航節(jié)點均以每秒0.04米的速度向右移動，如圖3所示。針對該案例對算法過程進行演繹：

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

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

其中表示第i個節(jié)點的速度輸入信號。

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

將多機器人系統(tǒng)及其相互之間的局部交互表示為無向拓?fù)鋱Dg＝(v,e)，其中v＝{v1,v2,…v5}表示圖中的5個節(jié)點的集合，vi表示圖中第i個節(jié)點，即第i個機器人，表示節(jié)點與節(jié)點之間的邊的集合，eik∈e表示機器人i能測量機器人k的相對位置d＝ρj^θ，其中ρ表示兩個機器人之間的距離，θ表示機器人k相對于機器人i的角度。由于g＝(v,e)是無向圖，所以如果eik∈e，那么eki∈e，即機器人k也能測量機器人i的相對位置。添加邊e12,e23,e34,e45,e51至圖中使所有節(jié)點均在同一圓環(huán)上。

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

對應(yīng)圖2無向圖g＝(v,e)的鄰接矩陣w，如果存在eik∈e，那么wik≠0,反之如果那么wik＝0。根據(jù)步驟2設(shè)計的無向g＝(v,e)，可知

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

得：

根據(jù)圖1機器人目標(biāo)的隊形，為：

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

該隊形可以由下式表示：

η＝c11n+c2ξ

不失一般性，取c1＝0，c2＝1:

η＝c11n+c2ξ＝ξ

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

得：

配置復(fù)拉普拉斯矩陣的特征值，使得復(fù)拉普拉斯矩陣的特征值只有兩個在零點，剩余的特征值均在復(fù)平面的右半平面。不失一般性，令最后復(fù)拉普拉斯矩陣l最后兩排全為0，表示其最后兩個節(jié)點為領(lǐng)航者節(jié)點，不受其余節(jié)點的影響。

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

令對角矩陣

目標(biāo)配置極點為λ1＝1,λ2＝2,λ3＝3,λ4＝0,λ5＝0。配置特征值即求解下述方程組：

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

記：

記：

并記：

記初值為

迭代計算下述算式：

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

步驟5，計算分布式離散控制信號

每個機器人的速度控制信號由下式給出：

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

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

x(k+1)＝(i-εdl)x(k)＝ax(k)+u(k)

其中ε為采樣時間，取值范圍λmax為最大特征值，即λmax＝3。取秒。

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

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

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

x(k+1)＝ax(k)+u(k)

測記錄第三個機器人的位移信息：

x3(k)＝(0,0,1,0,0)^tx(k)

初始值

2.第3個機器人記錄位移信息x3(k)，k＝1,2,3,…，并以此構(gòu)建hankel矩陣：

3.當(dāng)hankel矩陣h(x3(k+1)-x3(k))第一次失秩時，計算其零空間，并記為ρ，

ρ＝(0.0264,-0.2196,0.6149,-0.7028,0.2811)

機器人共移動7步。

4.通過下式計算觀測節(jié)點的

其預(yù)測軌跡如圖4所示。

其余機器人亦可用同樣方法求得其未來軌跡。

以上闡述的是本發(fā)明給出的實例，仿真結(jié)果體現(xiàn)了本發(fā)明所提出的技術(shù)方案的有效性。需要指出的是，本發(fā)明不只限于上述實施例，當(dāng)系統(tǒng)中不存在全局坐標(biāo)系統(tǒng)時，機器人若能根據(jù)左右輪轉(zhuǎn)速計算自身位移信息，采用本發(fā)明的技術(shù)方案，亦能有效預(yù)測未來軌跡。

本發(fā)明所涉及的預(yù)測算法，主要用于幫助機器人在最小步內(nèi)預(yù)知自身的未來軌跡，一方面，為避開不可達(dá)點，危險點提供了一種參考信息，另一方面，軌跡預(yù)測有利于機器人避障。

本說明書實施例所述的內(nèi)容僅僅是對發(fā)明構(gòu)思的實現(xiàn)形式的列舉，本發(fā)明的保護范圍不應(yīng)當(dāng)被視為僅限于實施例所陳述的具體形式，本發(fā)明的保護范圍也及于本領(lǐng)域技術(shù)人員根據(jù)本發(fā)明構(gòu)思所能夠想到的等同技術(shù)手段。