也是不斷變化的；為了對(duì)誤差橢圓重疊區(qū)域能反映出的關(guān)聯(lián) 程度進(jìn)行定量考察，將重疊區(qū)域面積與兩橢圓面積之比作為輸入變量； 對(duì)于兩橢圓相交形成的重疊區(qū)域，難以直接計(jì)算其面積；本發(fā)明使用八邊形近似的方 法，首先在橢圓內(nèi)根據(jù)半長(zhǎng)軸和半短軸與橢圓交點(diǎn)建立對(duì)稱八邊形，就可以將兩個(gè)橢圓相 交的重疊區(qū)域近似為兩八邊形的重疊區(qū)域，然后再將重疊區(qū)域分解為若干個(gè)三角形，三角 形的面積是易于計(jì)算的，這樣就可以得到兩橢圓重疊面積的近似值；將兩誤差橢圓的重疊 區(qū)域近似為兩個(gè)八邊形重疊區(qū)域，這是一個(gè)多邊形區(qū)域，將其再劃分為四個(gè)三角形，每個(gè)三 角形的面積可通過海倫公式計(jì)算：式中：p = (a+b+c)/2,a、b、c分別為三角形的三條邊長(zhǎng)；最終兩橢圓重疊區(qū)域面積area近似為三角形面積之和：再分別計(jì)算重疊區(qū)域在每個(gè)橢圓中所占比例：perobs=area/ellipseobs (21) perest=area/elIipseest (22) 其中per&JPperest分別為特征觀測(cè)值和特征估計(jì)值對(duì)應(yīng)的誤差橢圓的重疊區(qū)域在各 自橢圓中所占比例，ellipse&dPellipse&分別為兩誤差橢圓的面積； 定義描述橢圓重疊區(qū)域比例的模糊語言變量為Seldom、Little、Medium、Most和Major，這些模糊集合對(duì)應(yīng)的隸屬函數(shù)，將清晰的重疊比例量映射到上述模糊集合中進(jìn)行模 糊化處理； 將待匹配的特征觀測(cè)值和特征估計(jì)值的關(guān)聯(lián)程度作為模糊推理系統(tǒng)的輸出值，記作Degree，其取值在[0, 1]之間；定義描述關(guān)聯(lián)程度的模糊語言變量為VeryLow、Low、Medium、 High和VeryHigh，這些模糊集合對(duì)應(yīng)的隸屬函數(shù)，將清晰地輸出變量映射到模糊集合中進(jìn) 行模糊化； 數(shù)據(jù)關(guān)聯(lián)算法除了要有正確的關(guān)聯(lián)率之外，還需要防止引入虛假的新路標(biāo)；其由兩種 途徑產(chǎn)生：a、將已有環(huán)境路標(biāo)的觀測(cè)錯(cuò)誤判斷為新路標(biāo)；b、將虛假的環(huán)境路標(biāo)觀測(cè)判斷為 新的路標(biāo)；其中第一種途徑的影響更為嚴(yán)重，在特征環(huán)境中，將一個(gè)路標(biāo)識(shí)別為兩個(gè)路標(biāo)將 導(dǎo)致在兩者之間分配該路標(biāo)的觀測(cè)數(shù)據(jù)錯(cuò)誤； 定義傳感器返回M個(gè)觀測(cè)量，記為z= (Z1,...，zj;對(duì)于第m個(gè)觀測(cè)量zm，計(jì)算Znf^ 地圖中每一個(gè)路標(biāo)之間的馬氏距離De，記其中最小的為Dm。為了有效地防止虛假路標(biāo)，在基 于固定關(guān)聯(lián)閾值G1的基礎(chǔ)上，引入增廣閾值G2;若Dm<Gi，則觀測(cè)值與環(huán)境中的路標(biāo)可作 為關(guān)聯(lián)對(duì)；若G2<Dm，則其觀測(cè)值作為一個(gè)新路標(biāo)的觀測(cè)；若G1SDG2，則丟棄該觀測(cè)； 用自適應(yīng)的方法來調(diào)節(jié)增廣閾值G2; 其方法如下： 1) 計(jì)算當(dāng)前傳感器觀測(cè)中距離最近的兩個(gè)觀測(cè)：zm，zn; 2) 定義當(dāng)前機(jī)器人位姿估計(jì)為Xv和Pv，將Zm作為虛擬的新路標(biāo)加入到系統(tǒng)狀態(tài)中，即 X=[Xvg(zm)] (23)其中，g(zj為逆觀測(cè)模型，匕和Gz分別為g關(guān)于Xv和Zm的雅克比矩陣，R為觀測(cè)噪 聲； 3) 將zn#為虛擬路標(biāo)的觀測(cè)量，并計(jì)算記錄zn與虛擬路標(biāo)之間的馬氏距離D 4) 如果觀測(cè)只有一個(gè)觀測(cè)量，則隊(duì)取為所有已得到的{DJ中的最小值； 5. G2 =max(De，G21tJ，其中G21ot為設(shè)定的下限值； 在最近鄰關(guān)聯(lián)算法中，錯(cuò)誤的數(shù)據(jù)關(guān)聯(lián)將導(dǎo)致SLAM過程的發(fā)散，因此通常設(shè)置較低的 關(guān)聯(lián)閾值來保證數(shù)據(jù)關(guān)聯(lián)的正確性；但是較低的關(guān)聯(lián)閾值將導(dǎo)致較高的觀測(cè)丟棄率，為了 使數(shù)據(jù)關(guān)聯(lián)算法能夠根據(jù)系統(tǒng)狀態(tài)和環(huán)境中路標(biāo)的分布情況來自適應(yīng)的調(diào)節(jié)關(guān)聯(lián)閾值，提 出與自適應(yīng)增廣閾值類似的關(guān)聯(lián)閾值設(shè)定方法； 其方法如下： 1) 首先將關(guān)聯(lián)閾值G1初始化為預(yù)先設(shè)定的關(guān)聯(lián)閾值下限D(zhuǎn)lM，然后按下面設(shè)置自適應(yīng) 關(guān)聯(lián)閾值G1; 2) 當(dāng)首次觀測(cè)到多個(gè)路標(biāo)時(shí)，在上節(jié)中得到的馬氏距離隊(duì)后，取G1=De/4 ; 3) 在后續(xù)觀測(cè)中，G1=min{GpD6/4}，即6:取為所有{D6}中最小值的1/4 ; 4) 當(dāng)G1SDlOT時(shí)，為防止過低的關(guān)聯(lián)成功率，取GDlOT; 通過以上內(nèi)容的介紹，基于模糊-自適應(yīng)的SLAM數(shù)據(jù)關(guān)聯(lián)方法其特征在于將模糊邏輯 規(guī)則運(yùn)用到特征觀測(cè)值與估計(jì)值的數(shù)據(jù)關(guān)聯(lián)中，并能夠根據(jù)環(huán)境和噪聲的變化自適應(yīng)地確 定閾值，從而改善數(shù)據(jù)關(guān)聯(lián)效果，具體方法步驟如下： Stepl:獲取特征地圖信息Fj(j= 1，2,. . .，n)，傳感器測(cè)量信息Zi(i= 1，2,. . .，m)，通 過傳感器觀測(cè)到的特征為gj; St印2 :計(jì)算當(dāng)前觀測(cè)中歐氏距離最近的兩個(gè)觀測(cè)：Zm，zn; Step3 :將Zni作為虛擬的新路標(biāo)加入到系統(tǒng)狀態(tài)中，如式（23)與（24); Step4 :將zn作為虛擬路標(biāo)的觀測(cè)量； Step5 :根據(jù)特征觀測(cè)和估計(jì)的誤差協(xié)方差矩陣，計(jì)算特征觀測(cè)值Zn與虛擬路標(biāo)間對(duì)應(yīng) 的誤差橢圓，其誤差橢圓的參數(shù)如式（15)與（17); St印6 :計(jì)算誤差橢圓中心距離即新息，根據(jù)式（18)進(jìn)行歸一化處理，然后將其映射到 豐吳糊集合； Step7 :計(jì)算誤差橢圓的重疊區(qū)域在兩橢圓中所占的面積比例，并進(jìn)行模糊化處理； StepS:將模糊化輸入變量應(yīng)用到模糊規(guī)則中，輸出值是反映關(guān)聯(lián)程度的模糊變量； Step9:對(duì)模糊化的輸出結(jié)果進(jìn)行處理，得到單一關(guān)聯(lián)變量，并由此得出一對(duì)觀測(cè)一一估計(jì)的馬氏距離De; SteplO:取關(guān)聯(lián)閾值G1=De/4,并在后續(xù)觀測(cè)中根據(jù)反饋回來的隊(duì)自適應(yīng)的改變關(guān)聯(lián) 閾值；若G1SDlOT，則取G1=DlOT; Stepll:增廣閾值G2=max(De，G21tJ，在后續(xù)觀測(cè)中根據(jù)反饋回來的D肩適應(yīng)的改變 增廣閾值； Stepl2 :根據(jù)前一時(shí)刻的自適應(yīng)的增廣閾值G2及關(guān)聯(lián)閾值Gi來確定當(dāng)前時(shí)刻zm是否 作為關(guān)聯(lián)路標(biāo)進(jìn)行系統(tǒng)更新，還是作為一個(gè)新的路標(biāo)的觀測(cè)；當(dāng)隊(duì)<Gi時(shí)，則觀測(cè)值z(mì)m與 路標(biāo)Fi為可接受關(guān)聯(lián)對(duì)；當(dāng)DG2，則Zm將作為一個(gè)新的路標(biāo)觀測(cè)。2.根據(jù)權(quán)利要求1所述的基于模糊-自適應(yīng)的SLAM數(shù)據(jù)關(guān)聯(lián)方法，其特征在于：系統(tǒng) 圍繞一個(gè)閉環(huán)的軌跡運(yùn)動(dòng)，在移動(dòng)機(jī)器人移動(dòng)過程中，用激光傳感器持續(xù)測(cè)量了 120個(gè)路 標(biāo)；具體步驟如下： 步驟一：獲取特征地圖信息Fj(j= 1，2,. . .，n)，傳感器測(cè)量信息Zi(i= 1，2,. . .，m)， 通過傳感器觀測(cè)到的特征為gj; 步驟二：計(jì)算當(dāng)前觀測(cè)中歐氏距離最近的兩個(gè)觀測(cè)：zm，zn; 步驟三：將Zni作為虛擬的新路標(biāo)加入到系統(tǒng)狀態(tài)中，如式（23)與（24); 步驟四：將Zn作為虛擬路標(biāo)的觀測(cè)量； 步驟五：根據(jù)特征觀測(cè)和估計(jì)的誤差協(xié)方差矩陣，計(jì)算特征觀測(cè)值2"與虛擬路標(biāo)間對(duì) 應(yīng)的誤差橢圓，其誤差橢圓的參數(shù)如式（15)與（17); 步驟六：計(jì)算誤差橢圓中心距離即新息，根據(jù)式（18)進(jìn)行歸一化處理，然后將其映射 到豐吳糊集合； 步驟七：計(jì)算誤差橢圓的重疊區(qū)域在兩橢圓中所占的面積比例，并進(jìn)行模糊化處理； 步驟八：將模糊化輸入變量應(yīng)用到模糊規(guī)則中，輸出值是反映關(guān)聯(lián)程度的模糊變量； 步驟九：對(duì)模糊化的輸出結(jié)果進(jìn)行處理，得到單一關(guān)聯(lián)變量，并由此得出一對(duì)觀測(cè)一一 估計(jì)的馬氏距離De; 步驟十：取關(guān)聯(lián)閾值G1 =De/4,并在后續(xù)觀測(cè)中根據(jù)反饋回來的隊(duì)自適應(yīng)的改變關(guān)聯(lián) 閾值；若G1SDlOT，則取G1=DlOT; 步驟i^一 ：增廣閾值G2=max(DG21ot)，在后續(xù)觀測(cè)中根據(jù)反饋回來的De自適應(yīng)的改 變?cè)鰪V閾值； 步驟十二：根據(jù)前一時(shí)刻的自適應(yīng)的增廣閾值G2及關(guān)聯(lián)閾值Gl來確定當(dāng)前時(shí)刻z_"是 否作為關(guān)聯(lián)路標(biāo)進(jìn)行系統(tǒng)更新，還是作為一個(gè)新的路標(biāo)的觀測(cè)；當(dāng)隊(duì)<G:時(shí)，則觀測(cè)值z(mì)m 與路標(biāo)Fi為可接受關(guān)聯(lián)對(duì)；當(dāng)DG2，則Zm將作為一個(gè)新的路標(biāo)觀測(cè)。
【專利摘要】基于模糊-自適應(yīng)的SLAM數(shù)據(jù)關(guān)聯(lián)方法，本方法針對(duì)現(xiàn)有SLAM數(shù)據(jù)關(guān)聯(lián)方法的不足，提出一種基于誤差橢圓和關(guān)聯(lián)閾值的模糊-自適應(yīng)數(shù)據(jù)關(guān)聯(lián)方法，將模糊邏輯規(guī)則運(yùn)用到特征觀測(cè)值與估計(jì)值的數(shù)據(jù)關(guān)聯(lián)中，并能夠根據(jù)環(huán)境和噪聲的變化自適應(yīng)地確定閾值，從而改善數(shù)據(jù)關(guān)聯(lián)效果；為了有效地防止虛假路標(biāo)，本發(fā)明根據(jù)觀測(cè)數(shù)據(jù)來跟蹤環(huán)境中的最小路標(biāo)間距，將歐式距離最近的兩個(gè)觀測(cè)路標(biāo)中的一個(gè)作為虛擬的新路標(biāo)加入到系統(tǒng)狀態(tài)中，并將另一觀測(cè)量與虛擬路標(biāo)通過模糊規(guī)則進(jìn)行數(shù)據(jù)關(guān)聯(lián)。仿真實(shí)驗(yàn)和真實(shí)實(shí)驗(yàn)證明了該方法能夠解決數(shù)據(jù)關(guān)聯(lián)問題，并能很好的適應(yīng)環(huán)境和噪聲的變化，從而有效地防止虛假路標(biāo)并降低觀測(cè)丟棄率。
【IPC分類】G06N7/02
【公開號(hào)】CN104966123
【申請(qǐng)?zhí)枴緾N201510419783
【發(fā)明人】裴福俊, 武小平, 王曉君
【申請(qǐng)人】北京工業(yè)大學(xué)
【公開日】2015年10月7日
【申請(qǐng)日】2015年7月16日