本發(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ī)器人之間相互觀測，不依賴于外部環(huán)境通過共享環(huán)境信息，實現(xiàn)在共同環(huán)境下確定各自的位姿信息。

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

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

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

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

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

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

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

其中表示控制輸入，為維的向量；為運(yùn)動噪聲，其方差為，服從于的高斯分布。

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

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

可以得到一般形式的觀測模型：

其中表示觀測噪聲矩陣，其方差為，服從于的高斯分布。

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

時刻的地圖特征點信息、運(yùn)動控制信息和機(jī)器人的位姿信息都包含于矩陣中，以到時刻為例，將誤差協(xié)方差陣通過Cholesky分解可得，利用機(jī)器人控制輸入對狀態(tài)信息矩陣狀態(tài)增廣，可以得到：

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

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

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

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

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

時刻機(jī)器人觀測到特征點與其他機(jī)器人的相對方位信息，此時要根據(jù)觀測值計算觀測向量的后驗概率分布。

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

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

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

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

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

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

其中是機(jī)器人在時刻通過外部傳感器觀測到隊列中其他個機(jī)器人相對方位角的實際觀測值。

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

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

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

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

附圖說明

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

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

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

具體實施方式

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

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

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

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

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

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

多個機(jī)器人組成隊列在二維環(huán)境下移動，為機(jī)器人在時刻的位姿，其中、和分別表示機(jī)器人在時刻的橫坐標(biāo)，縱坐標(biāo)和運(yùn)動方向，則時機(jī)器人隊列的位姿信息可以表示為：

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

其中表示控制輸入，為維的向量；為運(yùn)動噪聲，其方差為，服從于的高斯分布。

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

可以得到一般形式的觀測模型：

其中表示觀測噪聲矩陣，其方差為，服從于的高斯分布。

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

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

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

其中表示控制輸入，為維的向量；為運(yùn)動噪聲，其方差為，服從于的高斯分布。

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

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

可以得到一般形式的觀測模型：

其中表示觀測噪聲矩陣，其方差為，服從于的高斯分布。

所述計算容積點集包括：

時刻的地圖特征點信息、運(yùn)動控制信息和機(jī)器人的位姿信息都包含于矩陣中，以到時刻為例，將誤差協(xié)方差陣通過Cholesky分解可得，利用機(jī)器人控制輸入對狀態(tài)信息矩陣狀態(tài)增廣，可以得到：

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

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

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

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

所述更新階段計算容積點集包括：

時刻機(jī)器人觀測到特征點與其他機(jī)器人的相對方位信息，此時要根據(jù)觀測值計算觀測向量的后驗概率分布。

所述計算卡爾曼增益包括：

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

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

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

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

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

其中是機(jī)器人在時刻通過外部傳感器觀測到隊列中其他個機(jī)器人相對方位角的實際觀測值。

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

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

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

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