本發(fā)明屬于智能車自主導航定位領域，涉及智能車同時定位與建圖中的數(shù)據(jù)關聯(lián)算法，更具體地說，是涉及一種基于改進人工魚群算法的智能車slam數(shù)據(jù)關聯(lián)方法。

背景技術：

同時定位與建圖(simultaneouslocalizationandmapping,slam)，可以分為兩部分：狀態(tài)估計和數(shù)據(jù)關聯(lián)。數(shù)據(jù)關聯(lián)作為狀態(tài)估計的前提和基礎，為狀態(tài)估計提供了正確的輸入，確保了slam的實時性和準確性，在slam中起著至關重要的作用。

目前，智能車slam中的數(shù)據(jù)關聯(lián)算法有獨立兼容最近鄰算法(individualcompatibilitynearestneighbor,icnn)、最大似然法(ml)、多假設跟蹤(multiplehypothesistracking,mht)以及聯(lián)合兼容分枝定界算法(jointcompatibilitybranchandbound,jcbb)等。這些算法在實際的應用中都存在一定的缺陷，其中獨立兼容最近鄰算法關聯(lián)正確率受環(huán)境影響大，即系統(tǒng)位置不確定以及環(huán)境不確定性增大時，關聯(lián)正確率會下降；最大似然算法只是討論單個量測與單個特征間的關聯(lián)可能性，關聯(lián)匹配整體性能不能有效的保證；多假設跟蹤算法在大規(guī)模關聯(lián)問題中的實時應用受限；聯(lián)合相容分枝定界算法計算復雜度高，不能應用于存在大量環(huán)境特征的和動態(tài)特征的室外環(huán)境。因此，現(xiàn)有的slam數(shù)據(jù)關聯(lián)算法都存在關聯(lián)誤差大，計算復雜度高的問題，嚴重影響了濾波器的性能和slam的運行結果。

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

為了克服slam數(shù)據(jù)關聯(lián)算法中存在關聯(lián)正確率受環(huán)境影響大、關聯(lián)匹配整體性能不能有效的保證、大規(guī)模關聯(lián)實時應用受限等技術問題，針對降低算法復雜度和提高關聯(lián)準確性問題，本發(fā)明提出了一種基于改進人工魚群算法的智能車slam數(shù)據(jù)關聯(lián)方法，利用改進人工魚群算法解決組合優(yōu)化問題的優(yōu)勢，結合復合關聯(lián)假設確定準則，將改進的人工魚群算法應用于搜索slam的最優(yōu)關聯(lián)解中，從而在保證關聯(lián)正確率的前提下提高了最優(yōu)解的搜索效率，即降低了算法的運行時間。

本發(fā)明解決技術問題所采取的技術方案如下：

一種基于改進人工魚群算法的智能車slam數(shù)據(jù)關聯(lián)方法包括如下步驟：

步驟一、采用復合關聯(lián)假設確定準則將數(shù)據(jù)關聯(lián)問題轉(zhuǎn)化為組合優(yōu)化問題，并定義目標函數(shù)，所述復合關聯(lián)假設確定準則是由獨立兼容準則和聯(lián)合最大似然準則結合而成，具體的步驟是：

步驟101、采用獨立兼容準則對所有量測-特征匹配對進行檢驗，對于不滿足準則的匹配對進行濾除，對于滿足準則的量測-特征匹配對留做候選，參與組成關聯(lián)假設集合；

步驟102、根據(jù)最大似然準則獲得步驟101中滿足獨立兼容準則的量測-特征匹配對中的每個量測i與地圖特征j之間的規(guī)范距離nij；

步驟103、定義目標函數(shù)

在最大似然準則的基礎上，從考慮數(shù)據(jù)關聯(lián)整體性能角度出發(fā)，引入聯(lián)合最大似然準則，將數(shù)據(jù)關聯(lián)問題轉(zhuǎn)化為組合優(yōu)化問題，其中目標函數(shù)為：

約束條件有兩個，一是一個地圖特征最多只能產(chǎn)生一個觀測；二是一個觀測最多只能與一個地圖特征進行關聯(lián)；

其中：omatch表示滿足獨立兼容準則的量測特征組成的集合，g表示omatch中的這部分量測可能與之相關聯(lián)的地圖特征組成的集合，nij表示的是第i個觀測量和地圖中已經(jīng)存在的第j個特征的規(guī)范距離，at＝{a1,a2,...,am}表示t時刻目標函數(shù)f最小時對應的關聯(lián)假設集合，即最小規(guī)范距離之和對應的關聯(lián)假設集合；argmin()表示使得目標函數(shù)取最小值時的變量值。

步驟二、采用基于跳躍行為和禁忌策略改進的人工魚群算法解決上述組合優(yōu)化問題，求解目標函數(shù)最小時對應的最優(yōu)數(shù)據(jù)關聯(lián)集合，采用改進的人工魚群算法進行尋優(yōu)搜索，食物濃度最高值即為目標函數(shù)f的最小值。

上述步驟二中基于跳躍行為和禁忌策略改進的人工魚群算法包括如下步驟：

步驟201、建立人工魚群模型，初始化魚群算法參數(shù)。

步驟202、初始化魚群中每條人工魚的狀態(tài)，每條人工魚包含m2個規(guī)范距離值，每個人工魚個體是一個1×m2的矩陣。其中m2是omatch集合中觀測量的個數(shù)。

步驟203、關聯(lián)問題的目標函數(shù)即為人工魚個體的適應度函數(shù)，根據(jù)適應度函數(shù)計算人工魚所在位置的食物濃度，即計算人工魚當前狀態(tài)所對應的規(guī)范距離之和。

步驟204、對每條人工魚的當前狀態(tài)進行評價，對其要執(zhí)行的行為進行選擇，包括覓食行為，聚群行為，追尾行為；

步驟205、尋找當前最優(yōu)人工魚的狀態(tài)，將魚群中最優(yōu)魚狀態(tài)對應的食物濃度值與公告板中的值進行比較，若其值小于公告板中記錄值，則對公告板中的記錄值進行更新，使得公告板中始終保持較小的。若迭代次數(shù)未達到最大次數(shù)，則迭代次數(shù)加1，重復步驟204；

步驟206、引入跳躍行為：如果連續(xù)迭代m次，公告板上記錄的值沒有發(fā)生明顯變化，即公告板上的差值均小于預設閾值eps，此時在魚群中按照概率p(0<p<1)隨機選擇一些人工魚進行跳躍行為。若達到最大迭代次數(shù)，轉(zhuǎn)至步驟207；

步驟207、當達到最大迭代次數(shù)時，終止迭代，輸出此時最優(yōu)人工魚的狀態(tài)和食物濃度值；

步驟208、將步驟207中輸出的最優(yōu)人工魚的狀態(tài)作為禁忌搜索的初始解，初始化禁忌列表；

步驟209、判斷目標函數(shù)是否滿足，是否可以終止禁忌算法，若能滿足，則算法結束并將搜索到的最優(yōu)解輸出，若不滿足則進入步驟210；

步驟210、利用當前解的領域函數(shù)產(chǎn)生若干鄰域解，并從中確定一些候選解；

步驟211、判斷候選解是否都滿足藐視準則，若能夠滿足，則可以用滿足藐視準則最佳狀態(tài)值作為新的當前解。同時用當前解對應的禁忌對象替換最早進入禁忌表的禁忌對象；若不滿足，則進入步驟212；

步驟212、判斷候選解所對應的各對象的禁忌屬性。用與之對應的禁忌對象替換最早進入禁忌表的禁忌對象元素，同時選擇候選解集中非禁忌對象對應的最佳狀態(tài)作為當前解；

步驟213、轉(zhuǎn)至步驟209，繼續(xù)禁忌搜索算法，直到滿足目標函數(shù)值最小，輸出全局最優(yōu)人工魚狀態(tài)及食物濃度值，此時得到的全局最優(yōu)人工魚狀態(tài)即為當前t時刻的最優(yōu)數(shù)據(jù)關聯(lián)解

本發(fā)明的優(yōu)點和有益效果是：本發(fā)明將slam中的數(shù)據(jù)關聯(lián)問題轉(zhuǎn)化為組合優(yōu)化問題，利用復合關聯(lián)假設確定準則得到此優(yōu)化問題的目標函數(shù)，利用基于跳躍行為和禁忌策略改進的人工魚群算法搜索得到最優(yōu)數(shù)據(jù)關聯(lián)解，大大提高了slam數(shù)據(jù)關聯(lián)的正確率，降低了計算量，改善了slam的性能。

附圖說明

圖1是本發(fā)明一種基于改進人工魚群算法的智能車slam數(shù)據(jù)關聯(lián)方法流程圖。

具體實施方式

下面結合附圖1對本發(fā)明做進一步詳細說明。

本發(fā)明提供一種基于改進人工魚群算法的智能車slam數(shù)據(jù)關聯(lián)方法，

假設t時刻，智能車slam環(huán)境地圖中已存在n個特征，其集合表示為：

f＝{f1,f2,...,fn}

傳感器觀測到m個特征，其集合表示為：

o＝{o1,o2,...,om}

數(shù)據(jù)關聯(lián)就是建立一個觀測到的特征oi與地圖中存在的特征fj相關的假設，可描述為:

at＝{a1,a2,...,am}

如果ai＝j，表示觀測的第i個特征和地圖中已經(jīng)存在的第j個特征相匹配；如果ai＝0，表示關聯(lián)集合為空，即觀測的第i個特征是新特征或是虛假信號。

如圖1所示，本發(fā)明的智能車slam數(shù)據(jù)關聯(lián)方法包括如下步驟：

步驟一、采用復合關聯(lián)假設確定準則將數(shù)據(jù)關聯(lián)問題轉(zhuǎn)化為組合優(yōu)化問題，并定義目標函數(shù)，其中復合關聯(lián)假設確定準則是由獨立兼容準則和聯(lián)合最大似然準則結合而成。具體的步驟是：

步驟101、首先采用獨立兼容準則對m個量測-特征關聯(lián)匹配對進行檢驗，對于不滿足準則的匹配對進行濾除，剩下的量測-特征對留做候選，參與組成新的關聯(lián)假設集合。其中獨立兼容準則(ic)表示如下：

式中，dij表示量測特征i與地圖特征j之間的馬氏距離，當馬氏距離小于設定的門限值g時，表明量測特征與地圖特征關聯(lián)成功。其中vij表示新息向量，st,ij表示新息協(xié)方差矩陣，vij和st,ij表示如下：

vij＝zi-hj(xt|t-1)

式中，zi表示量測值，hj()為非線性的觀測方程，表示觀測方程的雅可比矩陣，pt|t-1為狀態(tài)向量xt|t-1的方差，ri為觀測噪聲的協(xié)方差矩陣。

假設根據(jù)獨立兼容規(guī)則判斷出有m1個量測的可能關聯(lián)集合為空，則將其直接判斷為新觀測，用集合onew表示；這部分觀測對應的關聯(lián)假設為0，即ai＝0。剩下的m2個觀測用集合omatch表示，其中m2＝m-m1。剩下的這部分量測可能與之相關聯(lián)的地圖特征集合用表示，它們的維數(shù)分別為如集合omat中的第一個量測o1，可與之相關聯(lián)的地圖特征集為g1＝{f1,f3,f5,f8,f13}，則p1＝5，集合omatch中的第二個量測o2，可與之對應的地圖特征集合為g2＝{f2,f4,f7}，則p2＝3，其他的對應關系以此類推；

步驟102、根據(jù)最大似然準則獲得omatch中每個量測i與相對應的地圖特征集合g中每個地圖特征j之間的規(guī)范距離nij；最大似然準則，通常是用來考慮系統(tǒng)所獲取到的每個量測與單個地圖中特征相關聯(lián)的可能性。一般需要假設真實量測是均勻分布的，量測噪聲是高斯分布的，且給定了標準偏差和均值。因此量測i與環(huán)境特征j之間關聯(lián)的極大似然概率計算如下：

其中，fij表示量測i與特征j之間的關聯(lián)概率值，vij表示量測i與特征j之間新息誤差向量，st,ij表示量測i與特征j之間新息向量的協(xié)方差矩陣，n表示新息向量的維數(shù)。對上面的公式兩邊取對數(shù)得到：

ai＝argmin(nij)

式中：nij指與關聯(lián)概率值相對應的距離，也稱為規(guī)范距離，若滿足量測i與特征j之間的規(guī)范距離nij最小，就可以找到與量測i相關聯(lián)的環(huán)境特征，即ai＝j，表示量測i和地圖特征j相匹配；

步驟103、定義目標函數(shù)，在最大似然準則的基礎上，從考慮數(shù)據(jù)關聯(lián)整體性能角度出發(fā)，引入聯(lián)合最大似然準則，得到關聯(lián)問題的目標函數(shù)：

式中：表示t時刻目標函數(shù)最小時對應的關聯(lián)假設集合，即最小規(guī)范距離之和對應的關聯(lián)假設集合；argmin()表示使得目標函數(shù)去最小值時的變量值。其中約束條件有兩個，一是一個地圖特征最多只能產(chǎn)生一個觀測；二是一個觀測最多只能與一個地圖特征進行關聯(lián)；此時，slam數(shù)據(jù)關聯(lián)問題轉(zhuǎn)化為求規(guī)范距離之和最小時對應的最優(yōu)關聯(lián)解問題，即求解函數(shù)的全局最優(yōu)解問題；

步驟二、采用基于跳躍行為和禁忌策略改進的人工魚群算法解決上述組合優(yōu)化問題，求解最優(yōu)數(shù)據(jù)關聯(lián)集合。

上述步驟二中基于跳躍行為和禁忌策略改進的人工魚群算法包括如下步驟：

步驟201、建立人工魚群模型，初始化魚群算法參數(shù)。其中設定魚群規(guī)模大小為m，考慮到上面所述的解空間的維數(shù)是一個變化的量，因此總的魚群的規(guī)模用以下公式計算：

再根據(jù)步驟103中兩個約束條件的限定，選出滿足兩個約束條件的n條人工魚，作為魚群的初始位置。這樣可以根據(jù)數(shù)據(jù)關聯(lián)的解空間的大小自適應調(diào)整初始魚群的大小，從而提高算法的計算效率。

步驟202、初始化魚群中每條人工魚的狀態(tài)，每條人工魚包含m2個規(guī)范距離值，每個人工魚個體是一個1×m2的矩陣。

步驟203、關聯(lián)問題的目標函數(shù)即為人工魚個體的適應度函數(shù)，根據(jù)適應度函數(shù)計算人工魚所在位置的食物濃度，即計算人工魚當前狀態(tài)所對應的規(guī)范距離之和。

步驟204、對每條人工魚的當前狀態(tài)進行評價，對其要執(zhí)行的行為進行選擇，包括覓食行為，聚群行為，追尾行為；

步驟205、尋找當前最優(yōu)人工魚的狀態(tài)，將魚群中最優(yōu)魚狀態(tài)對應的食物濃度值與公告板中的值進行比較，若其值小于公告板中記錄值，則對公告板中的記錄值進行更新，使得公告板中始終保持較小的。若迭代次數(shù)未達到最大次數(shù)，則迭代次數(shù)加1，重復步驟204；

步驟206、引入跳躍行為：如果連續(xù)迭代m次，公告板上記錄的值沒有發(fā)生明顯變化，即公告板上的差值均小于預設閾值eps，此時在魚群中按照概率p(0<p<1)隨機選擇一些人工魚進行跳躍行為，若達到最大迭代次數(shù)，轉(zhuǎn)至步驟207；

步驟207、當達到最大迭代次數(shù)時，終止迭代，則該條人工魚對應的量測與特征的匹配關系即為最優(yōu)數(shù)據(jù)關聯(lián)，輸出此時最優(yōu)人工魚的狀態(tài)和食物濃度值，即輸出當前時刻量測值與環(huán)境特征之間的最佳匹配關系和規(guī)范距離之和的最小值；

步驟208、將步驟207中輸出的最優(yōu)人工魚的狀態(tài)作為禁忌搜索的初始解，初始化禁忌列表；

步驟209、判斷目標函數(shù)是否滿足，是否可以終止禁忌算法，若能滿足，則算法結束并將搜索到的最優(yōu)解輸出，若不滿足則進入步驟210；

步驟210、利用當前解的領域函數(shù)產(chǎn)生若干鄰域解，并從中確定一些候選解；

步驟211、判斷候選解是否都滿足藐視準則，若能夠滿足，則可以用滿足藐視準則最佳狀態(tài)值作為新的當前解。同時用當前解對應的禁忌對象替換最早進入禁忌表的禁忌對象；若不滿足，則進入步驟212；

步驟212、判斷候選解所對應的各對象的禁忌屬性。用與之對應的禁忌對象替換最早進入禁忌表的禁忌對象元素，同時選擇候選解集中非禁忌對象對應的最佳狀態(tài)作為當前解；

步驟213、轉(zhuǎn)至步驟209，繼續(xù)禁忌搜索算法，直到滿足目標函數(shù)值最小，輸出全局最優(yōu)人工魚狀態(tài)及食物濃度值，此時得到的全局最優(yōu)人工魚狀態(tài)即為當前t時刻的最優(yōu)數(shù)據(jù)關聯(lián)解例如，最終算法得到當目標函數(shù)為時取值最小，對應的最優(yōu)人工魚狀態(tài)是[n12n24n31n45n56]，說明a1＝2，a2＝4，a3＝1，a4＝5，a5＝6,量測1與地圖上的特征2關聯(lián)，量測2與地圖上的特征4關聯(lián)，量測3與地圖上的特征1關聯(lián)，量測4與地圖上的特征5關聯(lián)，量測5與地圖上的特征6關聯(lián)。

本發(fā)明的基于改進人工魚群算法的智能車slam數(shù)據(jù)關聯(lián)方法，首先利用獨立兼容準則和聯(lián)合最大似然準則來確定關聯(lián)假設，將slam數(shù)據(jù)關聯(lián)問題轉(zhuǎn)化為組合優(yōu)化問題，其次，利用基于跳躍行為和禁忌策略改進的人工魚群算法解決組合優(yōu)化問題，求解最優(yōu)數(shù)據(jù)關聯(lián)集合，在人工魚群算法中引入跳躍行為，使得一部分人工魚跳出局部極值，盡可能達到全局最優(yōu)，之后利用基于跳躍行為改進的人工魚群算法搜索到全局次優(yōu)解作為禁忌搜索算法的初始解，利用禁忌搜索算法去搜索局部最優(yōu)解，從而增強全局尋優(yōu)能力和尋優(yōu)效率。本發(fā)明有效解決了室外大范圍場景內(nèi)智能車slam數(shù)據(jù)關聯(lián)問題，提高了數(shù)據(jù)關聯(lián)的正確率和最優(yōu)關聯(lián)集合的搜索效率，降低了運算時間。