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

背景技術(shù)：

隨著技術(shù)的革新和機(jī)器人應(yīng)用領(lǐng)域的不斷拓展，多移動(dòng)機(jī)器人系統(tǒng)近年來(lái)受到學(xué)者們的廣泛關(guān)注，其技術(shù)也獲得了長(zhǎng)足的發(fā)展。采用多個(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ā)展歷程后，無(wú)論在軍事、國(guó)防，還是工業(yè)、生活等領(lǐng)域都展現(xiàn)了其廣泛的應(yīng)用空間。

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

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

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

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

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

步驟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ù)即a和b都表示任意實(shí)數(shù)。將目標(biāo)的隊(duì)形表示為一列復(fù)數(shù)向量n為自然數(shù)，表示復(fù)數(shù)的集合。視機(jī)器人為無(wú)碰撞體積的質(zhì)點(diǎn)，表示第i個(gè)機(jī)器人在平面中的位置，是一列表示n個(gè)機(jī)器人的位置的向量：

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

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

單個(gè)機(jī)器人的動(dòng)力學(xué)方程：

其中表示第i個(gè)機(jī)器人的速度輸入信號(hào)，表示括號(hào)內(nèi)的式子對(duì)時(shí)間求導(dǎo)。

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

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

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

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

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

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

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

η＝c11n+c2ξ(4)

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

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

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

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

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

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

對(duì)角矩陣：

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

記：

記：

并記：

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

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

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

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

跟隨機(jī)器人的速度控制信號(hào)由下式給出：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(3)通過(guò)下式迭代計(jì)算觀測(cè)節(jié)點(diǎn)的未來(lái)路徑

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

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

附圖說(shuō)明

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

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

圖3多移動(dòng)機(jī)器人收斂至目標(biāo)隊(duì)形并平移

圖4對(duì)節(jié)點(diǎn)3應(yīng)用算法在第12步計(jì)算出未來(lái)軌跡

具體實(shí)施方式

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

假設(shè)有一個(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)，其中，兩個(gè)領(lǐng)航節(jié)點(diǎn)均以每秒0.04米的速度向右移動(dòng)，如圖3所示。針對(duì)該案例對(duì)算法過(guò)程進(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ī)器人為無(wú)碰撞體積的質(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)及其相互之間的局部交互表示為無(wú)向拓?fù)鋱Dg＝(v,e)，其中v＝{v1,v2,…v5}表示圖中的5個(gè)節(jié)點(diǎn)的集合，vi表示圖中第i個(gè)節(jié)點(diǎn)，即第i個(gè)機(jī)器人，表示節(jié)點(diǎn)與節(jié)點(diǎn)之間的邊的集合，eik∈e表示機(jī)器人i能測(cè)量機(jī)器人k的相對(duì)位置d＝ρj^θ，其中ρ表示兩個(gè)機(jī)器人之間的距離，θ表示機(jī)器人k相對(duì)于機(jī)器人i的角度。由于g＝(v,e)是無(wú)向圖，所以如果eik∈e，那么eki∈e，即機(jī)器人k也能測(cè)量機(jī)器人i的相對(duì)位置。添加邊e12,e23,e34,e45,e51至圖中使所有節(jié)點(diǎn)均在同一圓環(huán)上。

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

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

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

得：

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

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

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

η＝c11n+c2ξ

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

η＝c11n+c2ξ＝ξ

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

得：

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

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

令對(duì)角矩陣

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

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

記：

記：

并記：

記初值為

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

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

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

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

其中ui表示第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)：

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

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

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

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

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

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

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

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

初始值

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

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

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

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

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

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

其余機(jī)器人亦可用同樣方法求得其未來(lái)軌跡。

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

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

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