本發(fā)明屬于自動化技術(shù)領(lǐng)域，涉及一種基于合作控制機(jī)制的機(jī)器人滾動優(yōu)化控制方法。

背景技術(shù)：

路徑規(guī)劃問題是機(jī)器人領(lǐng)域中的基礎(chǔ)問題之一，相應(yīng)的機(jī)器人路徑規(guī)劃技術(shù)則成為機(jī)器人運(yùn)動控制中的一項(xiàng)基礎(chǔ)技術(shù)。該項(xiàng)技術(shù)主要是控制機(jī)器人能夠安全有效地從初始位置到達(dá)指定的目標(biāo)位置。根據(jù)環(huán)境信息是否已知，可分為全局路徑規(guī)劃技術(shù)和局部路徑規(guī)劃技術(shù)，特別是局部路徑規(guī)劃技術(shù)，由于涉及到了未知?jiǎng)討B(tài)環(huán)境，使得該技術(shù)一直以來受到全世界的強(qiáng)烈關(guān)注。傳統(tǒng)的局部路徑規(guī)劃技術(shù)將“規(guī)劃”和“控制”分開，也就是說，先完成機(jī)器人感知范圍內(nèi)的路徑設(shè)定，然后采用軌跡跟蹤控制方法控制機(jī)器人沿著路徑運(yùn)動。但此類技術(shù)計(jì)算相對復(fù)雜，不能有效控制機(jī)器人工作在動態(tài)不確定環(huán)境中，使得機(jī)器人可能會“陷入”環(huán)境中非目標(biāo)位置，在此背景下，本發(fā)明彌補(bǔ)了此技術(shù)的不足之處。

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

本發(fā)明的目標(biāo)是針對現(xiàn)有技術(shù)的不足之處，提出一種基于合作控制機(jī)制的機(jī)器人滾動優(yōu)化控制方法，具體是引入虛擬機(jī)器人，使得該虛擬機(jī)器人可以沿著障礙物的邊緣運(yùn)動；在此基礎(chǔ)上，基于合作控制理論，在滾動優(yōu)化的框架下，建立成本函數(shù)；求解成本函數(shù)，獲得最優(yōu)控制序列，僅使用最優(yōu)控制序列中第一個(gè)控制信號控制機(jī)器人運(yùn)動。該方法能夠在每一次的采樣過程中求解控制序列，并僅使用第一個(gè)控制信號，這樣就能有效的控制機(jī)器人在動態(tài)不確定環(huán)境中運(yùn)動，并降低傳統(tǒng)方法運(yùn)算的復(fù)雜性。

本發(fā)明方法的步驟包括：

(一)建立實(shí)際機(jī)器人與虛擬機(jī)器人的動力學(xué)模型。具體方法是：

①建立實(shí)際機(jī)器人的動力學(xué)模型：

其中：u(k)是實(shí)際機(jī)器人在第k時(shí)刻的控制信號；x(k)是實(shí)際機(jī)器人在第k時(shí)刻的位置；x(k+1)是實(shí)際機(jī)器人在第k+1時(shí)刻的位置；v(k)是實(shí)際機(jī)器人在第k時(shí)刻的速度；v(k+1)是實(shí)際機(jī)器人在第k+1時(shí)刻的速度。

②建立虛擬機(jī)器人動力學(xué)模型：

其中：是虛擬機(jī)器人在第k時(shí)刻的位置；是虛擬機(jī)器人在第k時(shí)刻的速度；p是2×2轉(zhuǎn)換矩陣；i是2×2單位矩陣；yk是實(shí)際機(jī)器人檢測范圍內(nèi)障礙物邊緣上的任意一點(diǎn)位置；ak是過點(diǎn)yk的單位法向量；上標(biāo)t表示矩陣的轉(zhuǎn)置。

(二)基于合作控制理論，在滾動優(yōu)化的框架下，建立成本函數(shù)，具體方法是：

①基于合作控制理論，建立實(shí)際機(jī)器人和虛擬機(jī)器人之間安全距離的成本函數(shù)：

其中：d是預(yù)先給定的安全距離；是第j個(gè)虛擬機(jī)器人在第k時(shí)刻的位置；||·||表示2范數(shù)；m是虛擬機(jī)器人的個(gè)數(shù)。

②基于合作控制理論，建立實(shí)際機(jī)器人與虛擬機(jī)器人速度一致性成本函數(shù)：

其中：是第j個(gè)虛擬機(jī)器人在第k時(shí)刻的速度。

③建立實(shí)際機(jī)器人與目標(biāo)位置的成本函數(shù)：

||x(k)-g||(5)

其中：g是目標(biāo)位置。

④基于式(3)、(4)、(5)，在滾動優(yōu)化框架下，建立實(shí)際機(jī)器人運(yùn)動控制的成本函數(shù)：

其中：x(l|k)是實(shí)際機(jī)器人在第k時(shí)刻預(yù)測的第k+l時(shí)刻的位置；是第j個(gè)虛擬機(jī)器人在第k時(shí)刻預(yù)測的第k+l時(shí)刻的位置；v(l|k)是實(shí)際機(jī)器人在第k時(shí)刻預(yù)測的第k+l時(shí)刻的速度；是第j個(gè)虛擬機(jī)器人在第k時(shí)刻預(yù)測的第k+l時(shí)刻的速度；x(n|k)是實(shí)際機(jī)器人在第k時(shí)刻預(yù)測的第k+n時(shí)刻的位置；n是預(yù)測長度。

(三)基于合作控制機(jī)制的滾動優(yōu)化控制器結(jié)構(gòu)如下：

其中：是第k時(shí)刻獲得的最優(yōu)控制信號序列；u^*(0|k)表示實(shí)際機(jī)器人在第k時(shí)刻獲得的最優(yōu)控制信號；u^*(n-1|k)表示實(shí)際機(jī)器人在第k時(shí)刻獲得的k+n-1時(shí)刻的最優(yōu)控制信號；u(0|k)表示實(shí)際機(jī)器人在第k時(shí)刻預(yù)測的控制信號；u(n-1|k)表示實(shí)際機(jī)器人在第k時(shí)刻預(yù)測的k+n-1時(shí)刻的控制信號；min表示取最小值；s.t.表示以下為約束條件。

(四)將實(shí)際機(jī)器人在k時(shí)刻獲得的最優(yōu)的控制序列中的第一個(gè)最優(yōu)控制信號u^*(0|k)施加給實(shí)際機(jī)器人。

本發(fā)明提出的基于合作控制機(jī)制的機(jī)器人滾動優(yōu)化控制方法彌補(bǔ)了傳統(tǒng)的局部路徑規(guī)劃技術(shù)的不足,能夠控制機(jī)器人安全有效地運(yùn)行在動態(tài)不確定的環(huán)境中，避免了機(jī)器人“陷入”環(huán)境中非目標(biāo)位置。

具體實(shí)施方式

以一具體移動機(jī)器人為例，基于合作控制機(jī)制的機(jī)器人滾動優(yōu)化控制方法的實(shí)施步驟如下：

(一)在第k＝0時(shí)刻，初始化實(shí)際機(jī)器人系統(tǒng)參數(shù)，安全距離d＝2m，實(shí)際機(jī)器人初始位置x(0)＝[0m，0m]^t，實(shí)際機(jī)器人初始速度v(0)＝[0m/s，0m/s]，目標(biāo)位置g＝[100m，100m]^t。

(二)建立實(shí)際機(jī)器人與虛擬機(jī)器人的動力學(xué)模型。具體方法是：

①建立實(shí)際機(jī)器人的動力學(xué)模型：

其中：u(k)是實(shí)際機(jī)器人在第k時(shí)刻的控制信號；x(k)是實(shí)際機(jī)器人在第k時(shí)刻的位置；x(k+1)是實(shí)際機(jī)器人在第k+1時(shí)刻的位置；v(k)是實(shí)際機(jī)器人在第k時(shí)刻的速度；v(k+1)是實(shí)際機(jī)器人在第k+1時(shí)刻的速度。

②建立虛擬機(jī)器人動力學(xué)模型：

其中：是虛擬機(jī)器人在第k時(shí)刻的位置；是虛擬機(jī)器人在第k時(shí)刻的速度；p是2×2轉(zhuǎn)換矩陣；i是2×2單位矩陣；yk是實(shí)際機(jī)器人檢測范圍內(nèi)障礙物邊緣上的任意一點(diǎn)位置；ak是過點(diǎn)yk的單位法向量；上標(biāo)t表示矩陣的轉(zhuǎn)置。從式(2)可以看出，根據(jù)實(shí)際機(jī)器人在第k時(shí)刻的位置x(k)和速度v(k)，以及機(jī)器人檢測到的障礙物邊緣的任意一點(diǎn)yk和過該點(diǎn)的法向量ak，可以獲得第k時(shí)刻虛擬機(jī)器人的位置和速度

(三)基于合作控制理論，在滾動優(yōu)化的框架下，建立成本函數(shù)，具體方法是：

①基于合作控制理論，建立實(shí)際機(jī)器人和虛擬機(jī)器人之間安全距離的成本函數(shù)：

其中：d是預(yù)先給定的安全距離；是第j個(gè)虛擬機(jī)器人在第k時(shí)刻的位置；||·||表示2范數(shù)；m是虛擬機(jī)器人的個(gè)數(shù)。在實(shí)際機(jī)器人的檢測范圍內(nèi)，存在m個(gè)障礙物，每一個(gè)障礙物邊緣都存在一個(gè)虛擬機(jī)器人沿著障礙物邊緣運(yùn)動，也就存在m個(gè)虛擬機(jī)器人。

②基于合作控制理論，建立實(shí)際機(jī)器人與虛擬機(jī)器人速度一致性成本函數(shù)：

其中：是第j個(gè)虛擬機(jī)器人在第k時(shí)刻的速度。

③建立實(shí)際機(jī)器人與目標(biāo)位置的成本函數(shù)：

||x(k)-g||(5)

其中：g是目標(biāo)位置。

④基于式(3)、(4)、(5)，在滾動優(yōu)化框架下，建立實(shí)際機(jī)器人運(yùn)動控制的成本函數(shù)：

其中：x(l|k)是實(shí)際機(jī)器人在第k時(shí)刻預(yù)測的第k+l時(shí)刻的位置；是第j個(gè)虛擬機(jī)器人在第k時(shí)刻預(yù)測的第k+l時(shí)刻的位置；υ(l|k)是實(shí)際機(jī)器人在第k時(shí)刻預(yù)測的第k+l時(shí)刻的速度；是第j個(gè)虛擬機(jī)器人在第k時(shí)刻預(yù)測的第k+l時(shí)刻的速度；x(n|k)是實(shí)際機(jī)器人在第k時(shí)刻預(yù)測的第k+n時(shí)刻的位置；n是預(yù)測長度。

(四)基于合作控制機(jī)制的滾動優(yōu)化控制器如下：

其中：是實(shí)際機(jī)器人在第k時(shí)刻獲得的最優(yōu)控制信號序列；u^*(0|k)表示實(shí)際機(jī)器人在第k時(shí)刻獲得的最優(yōu)控制信號；u^*(n-1|k)表示實(shí)際機(jī)器人在第k時(shí)刻獲得的k+n-1時(shí)刻的最優(yōu)控制信號；u(0|k)表示實(shí)際機(jī)器人在第k時(shí)刻預(yù)測的控制信號；u(n-1|k)表示實(shí)際機(jī)器人在第k時(shí)刻預(yù)測的k+n-1時(shí)刻的控制信號；min表示取最小值；s.t.表示以下為約束條件。對于每一個(gè)在第k時(shí)刻預(yù)測的控制信號序列u(0|k)，...，u(n-1|k)，實(shí)際機(jī)器人預(yù)測的位置和速度可以采用式(1)計(jì)算，相應(yīng)的虛擬機(jī)器人預(yù)測的位置和速度可以采用式(2)計(jì)算，因此可以計(jì)算實(shí)際機(jī)器人運(yùn)動控制成本函數(shù)(6)。執(zhí)行式(7)獲得使實(shí)際機(jī)器人運(yùn)動控制成本函數(shù)(6)最小的最優(yōu)控制序列

(五)根據(jù)實(shí)際要求，在每個(gè)采樣時(shí)間上，根據(jù)(7)式計(jì)算實(shí)際機(jī)器人在第k時(shí)刻獲得的最優(yōu)控制信號序列并將第一個(gè)控制信號u^*(0|k)控制實(shí)際機(jī)器人，最終控制機(jī)器人安全到達(dá)目標(biāo)位置。