本發(fā)明涉及機(jī)器人協(xié)同定位領(lǐng)域，一種基于平方根容積卡爾曼濾波的多機(jī)器人協(xié)同定位算法。

背景技術(shù)：

隨著人們的研究領(lǐng)域不斷拓寬，從陸地、海洋延伸到深海以至未知的外太空，多機(jī)器人協(xié)同系統(tǒng)在軍事、航天、服務(wù)、工業(yè)等領(lǐng)域發(fā)揮著越來越重要的作用。能夠?qū)ξ粗獜?fù)雜外界環(huán)境信息感知、建模并確定自身的位置，是機(jī)器人自主導(dǎo)航的前提和基礎(chǔ)。多機(jī)器人協(xié)同定位是指多機(jī)器人之間相互觀測(cè)，不依賴于外部環(huán)境通過共享環(huán)境信息，實(shí)現(xiàn)在共同環(huán)境下確定各自的位姿信息。

目前，已有多種解決多機(jī)器人協(xié)同定位的方法，Jennings和Murray利用基于視覺的方法首次實(shí)現(xiàn)了機(jī)器人的協(xié)同定位。第一個(gè)機(jī)器人采用一種新的路標(biāo)識(shí)別方法能夠自主發(fā)現(xiàn)路標(biāo)，第二個(gè)機(jī)器人將自己獲取的圖像幀與第一個(gè)機(jī)器人的圖像幀進(jìn)行比較，實(shí)現(xiàn)自身的相對(duì)定位。由于該方法依賴于準(zhǔn)確的地圖匹配，所以魯棒性不強(qiáng)。Fox等人提出蒙特卡羅定位方法，首次將概率統(tǒng)計(jì)方法應(yīng)用于多機(jī)器人協(xié)同定位。機(jī)器人所處的位置采用概率方式來描述，再交換共享其他機(jī)器人的相對(duì)觀測(cè)信息，得到新的概率分布。但是在機(jī)器人識(shí)別等問題上該算法沒有給出解決方案。Jo和lee等通過GPS數(shù)據(jù)差把不同機(jī)器人間的距離關(guān)聯(lián)起來，進(jìn)行機(jī)器人隊(duì)列的相對(duì)定位。但是在GPS誤差較大的情況下，該方法的實(shí)時(shí)性與精確性較差。王玲和邵金鑫等利用擴(kuò)展卡爾曼濾波，將機(jī)器人內(nèi)部傳感器信息與隊(duì)列中其他機(jī)器人之間的相對(duì)觀測(cè)量相融合，以確定系統(tǒng)中的每個(gè)機(jī)器人的位置。該算法具有較好的實(shí)時(shí)性，但是該算法容易產(chǎn)生誤差積累，并 且其計(jì)算量會(huì)隨地圖的增大而急劇增大。

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

本發(fā)明的目的在于提供一種基于SR-CKF的相對(duì)方位多機(jī)器人協(xié)同定位算法，以解決現(xiàn)有技術(shù)中導(dǎo)致的上述多項(xiàng)缺陷。

本發(fā)明實(shí)施例采用如下技術(shù)方案：

一種基于SR-CKF的相對(duì)方位多機(jī)器人協(xié)同定位算法，整個(gè)算法分為兩步：預(yù)測(cè)和更新。首先建立機(jī)器人運(yùn)動(dòng)方程和觀測(cè)方程，利用相對(duì)方位作為測(cè)量值，進(jìn)一步得到多機(jī)器人協(xié)同定位的動(dòng)態(tài)模型。預(yù)測(cè)階段包括：計(jì)算容積點(diǎn)集；通過狀態(tài)方程傳播容積點(diǎn)；機(jī)器人位姿狀態(tài)預(yù)估計(jì)和平方根因子預(yù)測(cè)。更新階段包括：計(jì)算容積點(diǎn)集；計(jì)算卡爾曼增益；計(jì)算位姿信息和平方根因子；更新狀態(tài)向量、協(xié)方差陣和位姿信息。

優(yōu)選的，所述計(jì)算容積點(diǎn)集之前還包括：建立機(jī)器人運(yùn)動(dòng)方程。

在隊(duì)列的行進(jìn)過程中，向量為系統(tǒng)k時(shí)刻的狀態(tài)向量，為n_s維的向量；向量表示地圖特征路標(biāo)集合矩陣，機(jī)器人的運(yùn)動(dòng)方程均相同，故以機(jī)器人R_a為例其運(yùn)動(dòng)模型可以表示為：

其中表示控制輸入，為n_u維的向量；W_k為運(yùn)動(dòng)噪聲，其方差為Q_k，服從于N(0,Q_k)的高斯分布。

優(yōu)選的，所述計(jì)算容積點(diǎn)集之前還包括：建立機(jī)器人觀測(cè)方程。

在機(jī)器人運(yùn)動(dòng)的過程中，表示k時(shí)刻系統(tǒng)的觀測(cè)向量矩陣，為n_z維的向量，機(jī)器人在未知環(huán)境中存在兩種觀測(cè)值，地圖環(huán)境特征點(diǎn)的觀測(cè)值和對(duì)其他機(jī)器人的相對(duì)方位信息，機(jī)器人R_a在k時(shí)刻觀測(cè)到機(jī)器人R_b，由外部傳感器得 到它們之間的相對(duì)方位信息。分別為它們各自的運(yùn)動(dòng)方向，為k時(shí)刻機(jī)器人R_a觀測(cè)到機(jī)器人R_b的相對(duì)方位角則：

可以得到一般形式的觀測(cè)模型：

其中V_k表示觀測(cè)噪聲矩陣，其方差為R_k，服從于N(0,R_k)的高斯分布。

優(yōu)選的，所述計(jì)算容積點(diǎn)集包括：

k-1時(shí)刻的地圖特征點(diǎn)信息、運(yùn)動(dòng)控制信息和機(jī)器人的位姿信息都包含于矩陣中，以k-1到k時(shí)刻為例，將誤差協(xié)方差陣P_k-1通過Cholesky分解可得P_k-1＝L_k-1L_k-1^T，利用機(jī)器人控制輸入u_k對(duì)狀態(tài)信息矩陣S_k狀態(tài)增廣，可以得到：

優(yōu)選的，所述通過狀態(tài)方程傳播容積點(diǎn)：

優(yōu)選的，所述機(jī)器人位姿狀態(tài)預(yù)估計(jì)和平方根因子預(yù)測(cè)包括：

k時(shí)刻機(jī)器人的狀態(tài)估計(jì)：由容積變換，可得：

使用容積變換進(jìn)行，將特征誤差向量A_k|k-1進(jìn)行分解，即可獲得平方根因子矩陣C_k|k-1：

[Q R]＝qr(A_k|k-1^T)，C_k|k-1＝R^T

優(yōu)選的，所述更新階段計(jì)算容積點(diǎn)集包括：

k時(shí)刻機(jī)器人觀測(cè)到特征點(diǎn)與其他機(jī)器人的相對(duì)方位信息，此時(shí)要根據(jù)觀測(cè)值計(jì)算觀測(cè)向量的后驗(yàn)概率分布。

優(yōu)選的，所述計(jì)算卡爾曼增益包括：

其中，機(jī)器人R_a結(jié)合自身新的容積點(diǎn)和機(jī)器人R_b新的容積點(diǎn)利用觀測(cè)方程對(duì)其進(jìn)行更新，可以得到每個(gè)容積點(diǎn)的觀測(cè)值。再根據(jù)容積變換，可以得到：

其中，為觀測(cè)誤差向量，矩陣為觀測(cè)信息自相關(guān)協(xié)方差矩陣，矩陣 為觀測(cè)誤差向量和地圖特征誤差向量的互相關(guān)協(xié)方差矩陣。

可得，卡爾曼濾波增益矩陣W_k：

優(yōu)選的，所述計(jì)算位姿信息和平方根因子包括：

根據(jù)卡爾曼增益可計(jì)算得到機(jī)器人的位姿信息矩陣

其中是機(jī)器人在k時(shí)刻通過外部傳感器觀測(cè)到隊(duì)列中其他N-1個(gè)機(jī)器人相對(duì)方位角的實(shí)際觀測(cè)值。

平方根因子矩陣可以由下式得到：

優(yōu)選的，所述更新狀態(tài)向量、協(xié)方差陣和位姿信息包括:

當(dāng)同一時(shí)刻多個(gè)特征點(diǎn)同時(shí)被觀測(cè)到時(shí)，需要對(duì)以下兩個(gè)式子進(jìn)行計(jì)算，然后再重復(fù)預(yù)測(cè)和更新的步驟：

S_k|k-1＝S_k

C_k|k-1＝C_k

本發(fā)明的優(yōu)點(diǎn)在于：1、本發(fā)明算法將機(jī)器人通過傳感器得到的自身位姿信息與其他機(jī)器人的相對(duì)方位信息融合起來，整個(gè)機(jī)器人隊(duì)列內(nèi)部共享這些觀測(cè)信息提高了系統(tǒng)的魯棒性和效率。2、在更新過程中直接傳遞目標(biāo)狀態(tài)均值和協(xié)方差矩陣的平方根因子，使得系統(tǒng)更穩(wěn)定，有效的提高了定位的精度。3、在計(jì)算均值和方差時(shí)采用基于容積準(zhǔn)則的數(shù)值積分方法，減少了截?cái)嗾`差，降低了計(jì)算復(fù)雜度，縮短了算法耗時(shí)，實(shí)時(shí)性強(qiáng)。

附圖說明

圖1為本發(fā)明一種基于平方根容積卡爾曼濾波的多機(jī)器人協(xié)同定位算法流程圖；

圖2為本發(fā)明一種基于平方根容積卡爾曼濾波的多機(jī)器人協(xié)同定位算法R_a、R_b之間的相對(duì)觀測(cè)量示意圖；

圖3為本發(fā)明一種基于平方根容積卡爾曼濾波的多機(jī)器人協(xié)同定位算法協(xié)同定位方案示意圖。

具體實(shí)施方式

為使本發(fā)明實(shí)現(xiàn)的技術(shù)手段、創(chuàng)作特征、達(dá)成目的與功效易于明白了解，下面結(jié)合具體實(shí)施方式，進(jìn)一步闡述本發(fā)明。

根據(jù)本發(fā)明一個(gè)實(shí)施例，如圖1所示，整個(gè)算法分為兩步：預(yù)測(cè)和更新。首先建立機(jī)器人運(yùn)動(dòng)方程和觀測(cè)方程，利用相對(duì)方位作為測(cè)量值，進(jìn)一步得到多機(jī)器人協(xié)同定位的動(dòng)態(tài)模型。預(yù)測(cè)階段包括：計(jì)算容積點(diǎn)集；通過狀態(tài)方程傳播容積點(diǎn)；機(jī)器人位姿狀態(tài)預(yù)估計(jì)和平方根因子預(yù)測(cè)。更新階段包括：計(jì)算容積點(diǎn)集；計(jì)算卡爾曼增益；計(jì)算位姿信息和平方根因子；更新狀態(tài)向量、協(xié)方差陣和位姿信息。

另外，如圖2所示，當(dāng)N個(gè)機(jī)器人組成隊(duì)列在某一環(huán)境中沿不同方向運(yùn)動(dòng)，為了確保多機(jī)器人協(xié)同定位條件以及描述機(jī)器人之間的相對(duì)觀測(cè)信息，對(duì)隊(duì)列中機(jī)器人所需要的條件進(jìn)行如下假設(shè)：

(1)每個(gè)機(jī)器人都配有可感知自身位姿信息的內(nèi)部傳感器和感知外部環(huán)境的外部傳感器，通過攜帶的外部傳感器可有效的探測(cè)到附近機(jī)器人對(duì)其的觀測(cè)信息；

(2)通過通信協(xié)議每個(gè)機(jī)器人感知的外部環(huán)境可與其他機(jī)器人感知的數(shù)據(jù)之間進(jìn)行交流融合；

(3)隊(duì)列中每個(gè)機(jī)器人的外部傳感器是相同的，因而可以用同一數(shù)學(xué)模型描述它們之間的觀測(cè)信息。

多個(gè)機(jī)器人組成隊(duì)列在二維環(huán)境下移動(dòng)，為機(jī)器人在k時(shí)刻的位姿，其中s_x,k、s_y,k和s_θ,k分別表示機(jī)器人R_a在k時(shí)刻的橫坐標(biāo)，縱坐標(biāo)和運(yùn)動(dòng)方向，則k時(shí)機(jī)器人隊(duì)列的位姿信息可以表示為：

在隊(duì)列的行進(jìn)過程中，向量為系統(tǒng)k時(shí)刻的狀態(tài)向量，為n_s維的向量；向量表示地圖特征路標(biāo)集合矩陣，機(jī)器人的運(yùn)動(dòng)方程均相同，故以機(jī)器人R_a為例其運(yùn)動(dòng)模型可以表示為：

其中表示控制輸入，為n_u維的向量；W_k為運(yùn)動(dòng)噪聲，其方差為Q_k，服從于N(0,Q_k)的高斯分布。

在機(jī)器人運(yùn)動(dòng)的過程中，表示k時(shí)刻系統(tǒng)的觀測(cè)向量矩陣，為n_z維的向量，機(jī)器人R_a在k時(shí)刻觀測(cè)到機(jī)器人R_b，由外部傳感器得到它們之間的相對(duì)方位信息。如圖1所示，分別為它們各自的運(yùn)動(dòng)方向，為k時(shí)刻機(jī)器人R_a觀測(cè)到機(jī)器人R_b的相對(duì)方位角則：

可以得到一般形式的觀測(cè)模型：

其中V_k表示觀測(cè)噪聲矩陣，其方差為R_k，服從于N(0,R_k)的高斯分布。

如圖3所示，當(dāng)機(jī)器人通過可感知自身位姿信息的內(nèi)部傳感器和感知外部環(huán)境的外部傳感器，獲得運(yùn)動(dòng)數(shù)據(jù)時(shí)，通過運(yùn)動(dòng)模型結(jié)合傳感器所獲得的相對(duì) 觀測(cè)信息，完成對(duì)整個(gè)隊(duì)列中機(jī)器人的位姿和平方根因子的更新。在k時(shí)刻，由于每個(gè)機(jī)器人狀態(tài)信息更新的流程是相同的，故以機(jī)器人R_a為例詳細(xì)說明提出的多機(jī)器人協(xié)同定位算法。

所述計(jì)算容積點(diǎn)集之前還包括：建立機(jī)器人運(yùn)動(dòng)方程。

在隊(duì)列的行進(jìn)過程中，向量為系統(tǒng)k時(shí)刻的狀態(tài)向量，為n_s維的向量；向量表示地圖特征路標(biāo)集合矩陣，機(jī)器人的運(yùn)動(dòng)方程均相同，故以機(jī)器人R_a為例其運(yùn)動(dòng)模型可以表示為

其中表示控制輸入，為n_u維的向量；W_k為運(yùn)動(dòng)噪聲，其方差為Q_k，服從于N(0,Q_k)的高斯分布。

所述計(jì)算容積點(diǎn)集之前還包括：建立機(jī)器人觀測(cè)方程。

在機(jī)器人運(yùn)動(dòng)的過程中，表示k時(shí)刻系統(tǒng)的觀測(cè)向量矩陣，為n_z維的向量，機(jī)器人R_a在k時(shí)刻觀測(cè)到機(jī)器人R_b，由外部傳感器得到它們之間的相對(duì)方位信息。分別為它們各自的運(yùn)動(dòng)方向，為k時(shí)刻機(jī)器人R_a觀測(cè)到機(jī)器人R_b的相對(duì)方位角。則：

可以得到一般形式的觀測(cè)模型：

其中V_k表示觀測(cè)噪聲矩陣，其方差為R_k，服從于N(0,R_k)的高斯分布。

所述計(jì)算容積點(diǎn)集包括：

k-1時(shí)刻的地圖特征點(diǎn)信息、運(yùn)動(dòng)控制信息和機(jī)器人的位姿信息都包含于矩陣中，以k-1到k時(shí)刻為例，將誤差協(xié)方差陣P_k-1通過Cholesky分解可得P_k-1＝L_k-1L_k-1^T，利用機(jī)器人控制輸入u_k對(duì)狀態(tài)信息矩陣S_k狀態(tài)增廣，可以得到：

所述通過狀態(tài)方程傳播容積點(diǎn)：

所述機(jī)器人位姿狀態(tài)預(yù)估計(jì)和平方根因子預(yù)測(cè)包括：

k時(shí)刻機(jī)器人的狀態(tài)估計(jì)：由容積變換，可得：

使用容積變換進(jìn)行，將特征誤差向量A_k|k-1進(jìn)行分解，即可獲得平方根因子矩陣C_k|k-1：

[Q R]＝qr(A_k|k-1^T)，C_k|k-1＝R^T

所述更新階段計(jì)算容積點(diǎn)集包括：

k時(shí)刻機(jī)器人觀測(cè)到特征點(diǎn)與其他機(jī)器人的相對(duì)方位信息，此時(shí)要根據(jù)觀測(cè)值計(jì)算觀測(cè)向量的后驗(yàn)概率分布。

所述計(jì)算卡爾曼增益包括：

其中，機(jī)器人R_a結(jié)合自身新的容積點(diǎn)和機(jī)器人R_b新的容積點(diǎn)利用觀測(cè)方程對(duì)其進(jìn)行更新，可以得到每個(gè)容積點(diǎn)的觀測(cè)值。再根據(jù)容積變換，可以 得到：

其中，為觀測(cè)誤差向量，矩陣為觀測(cè)信息自相關(guān)協(xié)方差矩陣，矩陣為觀測(cè)誤差向量和地圖特征誤差向量的互相關(guān)協(xié)方差矩陣。

可得，卡爾曼濾波增益矩陣W_k：

所述計(jì)算位姿信息和平方根因子包括：

根據(jù)卡爾曼增益可計(jì)算得到機(jī)器人的位姿信息矩陣

其中是機(jī)器人在k時(shí)刻通過外部傳感器觀測(cè)到隊(duì)列中其他N-1個(gè)機(jī)器人相對(duì)方位角的實(shí)際觀測(cè)值。

平方根因子矩陣可以由下式得到：

所述更新狀態(tài)向量、協(xié)方差陣和位姿信息包括:

當(dāng)同一時(shí)刻多個(gè)特征點(diǎn)同時(shí)被觀測(cè)到時(shí)，需要對(duì)以下兩個(gè)式子進(jìn)行計(jì)算，然后再重復(fù)預(yù)測(cè)和更新的步驟：

S_k|k-1＝S_k

C_k|k-1＝C_k

由技術(shù)常識(shí)可知，本實(shí)用發(fā)明可以通過其它的不脫離其精神實(shí)質(zhì)或必要特征的實(shí)施方案來實(shí)現(xiàn)。因此，上述公開的實(shí)施方案，就各方面而言，都只是舉例說明，并不是僅有的。所有在本發(fā)明范圍內(nèi)或在等同于本實(shí)用發(fā)明的范圍內(nèi)的改變均被本發(fā)明包含。