[發(fā)明專(zhuān)利]多移動(dòng)機(jī)器人的軌跡預(yù)測(cè)方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201710078027.6
• 申請(qǐng)日: 2017-02-14
• 公開(kāi)（公告）號(hào): CN107065859A
• 公開(kāi)（公告）日: 2017-08-18
• 發(fā)明（設(shè)計(jì)）人: 歐林林;鄒超;禹鑫燚;張集匯;朱峰;朱熠琛
• 申請(qǐng)（專(zhuān)利權(quán)）人: 浙江工業(yè)大學(xué)
• 主分類(lèi)號(hào): G05D1/02
• 分類(lèi)號(hào): G05D1/02
• 代理公司: 杭州天正專(zhuān)利事務(wù)所有限公司33201
• 代理人: 王兵,黃美娟

1.多移動(dòng)機(jī)器人的軌跡預(yù)測(cè)方法，其特征在于，首先，將機(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ī)器人的目標(biāo)位置；具體步驟如下：

步驟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ì)形表示為n為自然數(shù)，表示復(fù)數(shù)的集合；視機(jī)器人為無(wú)碰撞體積的質(zhì)點(diǎn)，表示第i個(gè)機(jī)器人在平面中的位置，是一列表示n個(gè)機(jī)器人的位置的向量：

x＝(x₁,x₂,…,x_n)^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＝{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)是無(wú)向圖，所以如果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)圖無(wú)向圖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ù)；

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

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

對(duì)角矩陣：

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

記λ_i,i＝1,2,…,n為n個(gè)矩陣L的需配置特征值，配置特征值即求解下述方程組：

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；

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

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

其中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)：

x(k+1)＝(I-εL)x(k)＝Ax(k)(14)

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

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

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

(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步；

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

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