本發(fā)明涉及一種基于無(wú)跡卡爾曼濾波的組合定位方法，屬于室內(nèi)定位以及可見(jiàn)光定位技術(shù)領(lǐng)域。

背景技術(shù)：

近年來(lái)，隨著智能設(shè)備的飛速發(fā)展，基于位置的服務(wù)也隨之迅速發(fā)展，個(gè)人對(duì)于位置服務(wù)精細(xì)化、準(zhǔn)確化、無(wú)縫化的要求日益提高，室內(nèi)定位技術(shù)的發(fā)展也隨之成為新的關(guān)注點(diǎn)。但由于gps等傳統(tǒng)定位技術(shù)無(wú)法滿足室內(nèi)精細(xì)定位的需求，以可見(jiàn)光定位技術(shù)為代表的室內(nèi)定位技術(shù)開(kāi)始嶄露頭角。但可見(jiàn)光定位雖然以其精度高、成本低、易實(shí)現(xiàn)的特點(diǎn)倍受青睞，卻容易受到多徑反射、地圖限制、信號(hào)遮擋等問(wèn)題的影響，定位算法容易受到外界影響而極大地影響其短時(shí)定位精度，因此其實(shí)用性受到了極大限制。

慣性導(dǎo)航利用定位目標(biāo)載體(以下簡(jiǎn)稱載體)所搭載的傳感器所測(cè)得的運(yùn)動(dòng)加速度值和角速度值或磁偏角計(jì)算出載體運(yùn)動(dòng)的方向和距離，由載體初始位置推導(dǎo)出實(shí)時(shí)位置坐標(biāo)信息。由于其工作模式完全自主，短時(shí)精度優(yōu)良，具有與生俱來(lái)的抗干擾特性，可以和可見(jiàn)光導(dǎo)航系統(tǒng)形成良好的互補(bǔ)。

本發(fā)明采用包括行人慣性導(dǎo)航技術(shù)(pdr)在內(nèi)的慣性導(dǎo)航技術(shù)對(duì)可見(jiàn)光定位進(jìn)行輔助定位，采用無(wú)跡卡爾曼濾波算法對(duì)可見(jiàn)光定位系統(tǒng)/慣性導(dǎo)航算法的數(shù)據(jù)進(jìn)行融合，從而實(shí)現(xiàn)組合定位導(dǎo)航功能，其中無(wú)跡卡爾曼濾波縮寫(xiě)為ukf。實(shí)驗(yàn)表明，組合定位導(dǎo)航系統(tǒng)有效減小了可見(jiàn)光定位算法由于信號(hào)抖動(dòng)、信號(hào)遮擋等外界干擾問(wèn)題帶來(lái)的誤差，同時(shí)也極大地克服了純慣性導(dǎo)航系統(tǒng)累積誤差隨時(shí)間增大的問(wèn)題。整個(gè)系統(tǒng)既可以應(yīng)用于機(jī)器人、agv(automatedguidedvehicle，自動(dòng)導(dǎo)引運(yùn)輸車)等工業(yè)領(lǐng)域，也可以應(yīng)用于行人導(dǎo)航等消費(fèi)級(jí)電子產(chǎn)品領(lǐng)域

目前，慣性導(dǎo)航與其它導(dǎo)航的融合算法常被應(yīng)用于如gps+慣性導(dǎo)航等室外導(dǎo)航領(lǐng)域。而在可見(jiàn)光定位配合慣性導(dǎo)航系統(tǒng)領(lǐng)域鮮有文章報(bào)道。

與本發(fā)明相關(guān)的專利共兩篇，下文分別對(duì)其進(jìn)行剖析：

(1)申請(qǐng)?zhí)朿n201410067768.0，標(biāo)題為基于慣性定位和vlc技術(shù)的室內(nèi)混合定位系統(tǒng)及方法，主要解決了純可見(jiàn)光定位系統(tǒng)的可靠性不高，無(wú)法在較為復(fù)雜的室內(nèi)環(huán)境中應(yīng)用的問(wèn)題。該專利所述系統(tǒng)在接收器可以接收到led直射光線的室內(nèi)環(huán)境下，定位結(jié)果由vlc室內(nèi)定位系統(tǒng)提供；當(dāng)接收器進(jìn)入到室內(nèi)沒(méi)有直射光線的陰影區(qū)域時(shí)，定位結(jié)果由慣性定位系統(tǒng)提供，其中慣性定位系統(tǒng)所需的初始位置，由vlc室內(nèi)定位系統(tǒng)提供；

(2)申請(qǐng)?zhí)朿n201410831922.7，標(biāo)題為基于手機(jī)慣性定位和vlc的室內(nèi)混合定位系統(tǒng)及方法，它利用當(dāng)前智能手機(jī)中集成的硬件設(shè)備實(shí)現(xiàn)了可見(jiàn)光定位與慣性導(dǎo)航系統(tǒng)的簡(jiǎn)單組合，同時(shí)簡(jiǎn)化了定位裝置，也可以利用目標(biāo)位置來(lái)實(shí)現(xiàn)位置相關(guān)信息的推送，具有一定的實(shí)用意義。該專利中所述的app模塊把從通信接口獲取的編碼信號(hào)強(qiáng)度和閾值進(jìn)行比較，當(dāng)編碼信號(hào)強(qiáng)度大于閾值時(shí)，計(jì)算出多個(gè)led相對(duì)強(qiáng)度關(guān)系，根據(jù)強(qiáng)度關(guān)系得到光傳輸?shù)乃p距離，定位出vlc定位接收器中的光電探測(cè)器所在的基于多個(gè)led光源的相對(duì)位置關(guān)系；當(dāng)編碼信號(hào)強(qiáng)度低于閾值時(shí)，把慣性定位數(shù)值作為定位值；

以上專利雖然解決了在可見(jiàn)光信號(hào)受到遮擋情況下定位系統(tǒng)無(wú)法不間斷工作的問(wèn)題，但是由于其在工作時(shí)僅是單純依賴是否接收到可見(jiàn)光信號(hào)作為選擇不同定位系統(tǒng)的判據(jù)，因此無(wú)法解決可見(jiàn)光信號(hào)受到多徑效應(yīng)等干擾時(shí)的定位問(wèn)題，也沒(méi)有將慣性導(dǎo)航和可見(jiàn)光導(dǎo)航的定位數(shù)據(jù)進(jìn)行深度融合。此外，由于行人在行走時(shí)加速度特征復(fù)雜多變，該系統(tǒng)慣性導(dǎo)航算法中采用的加速度積分來(lái)確定接收器移動(dòng)距離的方法無(wú)法應(yīng)用在行人定位領(lǐng)域。因此，可見(jiàn)光定位/慣性導(dǎo)航組合定位算法都具有很大的改進(jìn)空間。本發(fā)明的目的即是致力于解決上述可見(jiàn)光定位/慣性導(dǎo)航組合定位算法的缺陷，提出基于無(wú)跡卡爾曼濾波的可見(jiàn)光定位/慣性導(dǎo)航組合定位算法。

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

本發(fā)明旨在解決已有的可見(jiàn)光定位與慣性導(dǎo)航組合定位系統(tǒng)無(wú)法在多徑干擾區(qū)域進(jìn)行定位的缺陷，提出了一種基于無(wú)跡卡爾曼濾波的組合定位方法。

本發(fā)明的技術(shù)方案為一種基于無(wú)跡卡爾曼濾波的組合定位方法，依托一種基于無(wú)跡卡爾曼濾波的組合定位系統(tǒng)，包括慣性導(dǎo)航單元、可見(jiàn)光定位單元、濾波與位置解算單元與系統(tǒng)初始化單元；

一種基于無(wú)跡卡爾曼濾波的組合定位系統(tǒng)各單元的連接方式如下：

慣性導(dǎo)航單元與可見(jiàn)光定位單元分別和濾波解算單元以及位置解算單元相連，連接方式包括但不限于串口及藍(lán)牙為主的方式；系統(tǒng)初始化單元與位置解算單元相連；一種基于無(wú)跡卡爾曼濾波的組合定位系統(tǒng)的各模塊功能如下：

慣性導(dǎo)航單元用于采集慣性導(dǎo)航運(yùn)動(dòng)數(shù)據(jù)；可見(jiàn)光定位單元用于采集可見(jiàn)光定位信號(hào)并解算可見(jiàn)光定位位置坐標(biāo)數(shù)據(jù)；系統(tǒng)初始化單元用于初始化循環(huán)計(jì)數(shù)值、設(shè)置工作模式布爾值和循環(huán)計(jì)數(shù)最大值；濾波解算單元用于設(shè)置無(wú)跡卡爾曼濾波器各方程與各參數(shù)，并使用無(wú)跡卡爾曼濾波算法進(jìn)行濾波；位置解算單元對(duì)載體的慣性運(yùn)動(dòng)數(shù)據(jù)和可見(jiàn)光定位坐標(biāo)數(shù)據(jù)進(jìn)行融合處理，得到濾波后的載體位置；

其中，無(wú)跡卡爾曼濾波算法，簡(jiǎn)稱ukf算法；

本發(fā)明所述一種基于無(wú)跡卡爾曼濾波的組合定位方法，包括以下步驟：

步驟1：系統(tǒng)初始化單元初始化循環(huán)計(jì)數(shù)值、設(shè)置工作模式布爾值和循環(huán)計(jì)數(shù)最大值，設(shè)置k＝1時(shí)刻組合定位初始坐標(biāo)值，設(shè)置初始協(xié)方差；

其中，循環(huán)計(jì)數(shù)值記為k、循環(huán)計(jì)數(shù)最大值記為kmax、工作模式布爾值記為bool；初始化k＝1；設(shè)置bool值和kmax值，當(dāng)bool＝0時(shí)系統(tǒng)處于實(shí)時(shí)工作狀態(tài)，kmax為無(wú)窮大值；bool＝1時(shí)系統(tǒng)處于離線工作狀態(tài)，kmax為一常數(shù)；設(shè)置第1時(shí)刻組合定位位置的初始坐標(biāo)值為(m,n)，設(shè)置初始協(xié)方差p＝0；

步驟2：慣性導(dǎo)航單元采集慣性導(dǎo)航運(yùn)動(dòng)參數(shù)，可見(jiàn)光定位單元解算可見(jiàn)光定位位置坐標(biāo)，具體為：

步驟2.1：慣性導(dǎo)航單元采集載體的慣性運(yùn)動(dòng)數(shù)據(jù)；

其中，設(shè)當(dāng)前時(shí)刻為k，所述載體的慣性運(yùn)動(dòng)數(shù)據(jù)根據(jù)本方法所應(yīng)用的場(chǎng)景不同分為兩種：

若為agv場(chǎng)景，則慣性運(yùn)動(dòng)數(shù)據(jù)包含k時(shí)刻的橫軸方向加速度與縱軸方向加速度由于加速度數(shù)據(jù)容易受到外界環(huán)境的干擾，噪聲較大，因此在進(jìn)行下一個(gè)步驟之前需要對(duì)加速度數(shù)據(jù)進(jìn)行低通濾波；

若為行人導(dǎo)航場(chǎng)景，則慣性運(yùn)動(dòng)數(shù)據(jù)包含k-1時(shí)刻到k時(shí)刻的平均運(yùn)動(dòng)距離sk與平均航向角θk；

步驟2.2：可見(jiàn)光定位單元采集并計(jì)算載體的可見(jiàn)光定位數(shù)據(jù)；

其中，可見(jiàn)光定位數(shù)據(jù)通過(guò)現(xiàn)有可見(jiàn)光定位算法進(jìn)行計(jì)算；

現(xiàn)有可見(jiàn)光定位算法主要包括但不限于基于rss算法的可見(jiàn)光定位算法；

依然假設(shè)當(dāng)前時(shí)刻為k，則所述可見(jiàn)光定位數(shù)據(jù)記為zk＝[xkyk]^t；xk、yk分別為載體的可見(jiàn)光定位位置橫縱坐標(biāo)，符號(hào)t表示矩陣轉(zhuǎn)置；

步驟3：濾波與解算單元設(shè)置ukf各方程與各參數(shù)，對(duì)載體的慣性運(yùn)動(dòng)數(shù)據(jù)和可見(jiàn)光定位數(shù)據(jù)進(jìn)行融合濾波處理，得到濾波后的載體位置；

步驟3.1：設(shè)置無(wú)跡卡爾曼濾波器各方程包括濾波狀態(tài)方程xk＝f(xk-1)與可見(jiàn)光定位觀測(cè)方程zk＝hxk；其中xk為狀態(tài)向量，zk為觀測(cè)向量，h為觀測(cè)矩陣；

設(shè)置無(wú)跡卡爾曼濾波器各參數(shù)包括根據(jù)系統(tǒng)所應(yīng)用的場(chǎng)景而設(shè)置的系統(tǒng)噪聲qk、觀測(cè)噪聲r(shí)、狀態(tài)向量xk及觀測(cè)向量zk；

其中，狀態(tài)向量xk包含有載體的慣性運(yùn)動(dòng)數(shù)據(jù)，觀測(cè)向量zk為可見(jiàn)光定位數(shù)據(jù)zk＝[xkyk]^t，xk、yk分別為載體的可見(jiàn)光定位位置橫縱坐標(biāo)；符號(hào)t表示矩陣轉(zhuǎn)置；

步驟3.2：計(jì)算預(yù)測(cè)估計(jì)值xk+1/k；

所述計(jì)算預(yù)測(cè)估計(jì)值的方法為：

其中，加權(quán)系數(shù)

n為狀態(tài)向量維數(shù)，在本方法中根據(jù)實(shí)際場(chǎng)景確定向量維數(shù)n；λ為尺度參數(shù)，計(jì)算方法為λ＝ε²(n+κ)-n，該式中ε為一小量，取值范圍為[10^-4,1]，常量κ可取0或者3-n；

步驟3.3：計(jì)算預(yù)測(cè)協(xié)方差pk+1/k；

所述計(jì)算預(yù)測(cè)協(xié)方差的方法為：

符號(hào)t表示矩陣轉(zhuǎn)置；

步驟3.4：計(jì)算預(yù)測(cè)測(cè)量值z(mì)k+1/k；

所述計(jì)算預(yù)測(cè)測(cè)量值的方法為：

步驟3.5：計(jì)算卡爾曼增益kk+1；

所述計(jì)算卡爾曼增益的方法為：

kk+1＝pxy，k+1|kpyy，k+1|k^-1，

其中，

步驟3.6：更新協(xié)方差pk+1/k+1；

所述更新協(xié)方差的方法為：

pk+1|k+1＝pk+1|k-kk+1pyy，k+1|kkk+1^t

符號(hào)t表示矩陣轉(zhuǎn)置；

步驟3.7：更新?tīng)顟B(tài)向量xk+1；

所述更新?tīng)顟B(tài)向量的方法為：

xk+1＝xk+1|k+kk+1|k(zk+1-zk+1|k)。

上述步驟3.2-3.7為ukf算法；

步驟4：位置解算單元計(jì)算k時(shí)刻的組合定位位置坐標(biāo)，具體為：

將步驟3.7中狀態(tài)向量xk+1中包含的坐標(biāo)值，作為k+1時(shí)刻組合定位位置坐標(biāo)；

步驟5：位置解算單元判斷循環(huán)計(jì)數(shù)值是否已經(jīng)達(dá)到循環(huán)計(jì)數(shù)最大值，并決定是否完成本方法，具體為：

步驟5.1：若是，即k＝kmax，則位置解算單元輸出k時(shí)刻的組合定位位置坐標(biāo)，完成了本方法；

步驟5.2：若否，即k＜kmax，則位置解算單元輸出k時(shí)刻的組合定位位置坐標(biāo)，并令k＝k+1，跳至步驟2；

至此，經(jīng)過(guò)步驟1到步驟5，完成了一種基于無(wú)跡卡爾曼濾波的組合定位方法。

有益效果

一種基于無(wú)跡卡爾曼濾波的組合定位方法，與現(xiàn)有技術(shù)相比，具有如下有益效果：

1、由于可見(jiàn)定位算法容易受到外界影響與干擾，其在單獨(dú)工作時(shí)非線性度大，相比較與ekf等其他濾波算法，采用ukf能夠更好地適應(yīng)可見(jiàn)光定位環(huán)境，可以有效減小了可見(jiàn)光定位系統(tǒng)的信號(hào)抖動(dòng)、受可見(jiàn)光覆蓋區(qū)域限制、信號(hào)遮擋等問(wèn)題帶來(lái)的誤差；

2、本方法相較于單純依靠選擇濾波來(lái)對(duì)可見(jiàn)光定位數(shù)據(jù)和慣性導(dǎo)航定位數(shù)據(jù)進(jìn)行融合的方式相比，融合效果更好，融合后誤差更小；

3、相較于其他濾波器，本方法所采用的ukf無(wú)需選擇濾波等其他濾波器輔助，整個(gè)算法只需要一個(gè)ukf濾波器工作，可靠性更強(qiáng)，穩(wěn)定性更好；

4、本發(fā)明所提系統(tǒng)既可以應(yīng)用于機(jī)器人、agv等工業(yè)領(lǐng)域，也可以應(yīng)用于行人導(dǎo)航等消費(fèi)級(jí)電子產(chǎn)品領(lǐng)域，應(yīng)用廣泛，實(shí)用性強(qiáng)，所需硬件設(shè)備成熟，成本低廉，易于推廣。

附圖說(shuō)明

圖1為本發(fā)明“一種基于無(wú)跡卡爾曼濾波的組合定位方法”流程示意圖；

圖2為本發(fā)明“一種基于無(wú)跡卡爾曼濾波的組合定位方法”實(shí)施例3中本方法與現(xiàn)有其他定位方法的定位結(jié)果對(duì)比圖；

圖3為本發(fā)明“一種基于無(wú)跡卡爾曼濾波的組合定位方法”實(shí)施例3中本方法與現(xiàn)有其他定位方法定位結(jié)果對(duì)比局部放大圖；

圖4為本發(fā)明“一種基于無(wú)跡卡爾曼濾波的組合定位方法”實(shí)施例3中本方法與現(xiàn)有其他定位方法的定位結(jié)果誤差對(duì)比圖；

圖5為本發(fā)明“一種基于無(wú)跡卡爾曼濾波的組合定位方法”實(shí)施例3中本方法與現(xiàn)有其他定位方法的定位結(jié)果誤差分布對(duì)比。

具體實(shí)施方式

以下實(shí)施例結(jié)合附圖對(duì)本發(fā)明進(jìn)行了詳細(xì)的說(shuō)明，但本發(fā)明的具體實(shí)施形式并不局限于此。

實(shí)施例1

以agv可見(jiàn)光/慣性組合定位系統(tǒng)為例，采用本發(fā)明“一種基于無(wú)跡卡爾曼濾波的可見(jiàn)光定位/慣性導(dǎo)航組合定位方法”實(shí)現(xiàn)可見(jiàn)光定位/慣性定位組合定位，具體步驟如圖1所示。

步驟a：系統(tǒng)初始化單元初始化循環(huán)計(jì)數(shù)值k、設(shè)置工作模式布爾值bool；設(shè)置循環(huán)最大值kmax；設(shè)置k＝1時(shí)刻組合定位初始坐標(biāo)值，設(shè)置初始協(xié)方差；

具體到本實(shí)施例，k被初始化為1；設(shè)置初始協(xié)方差p＝0；

步驟b：判斷bool值是否為0，并進(jìn)行相應(yīng)操作：

步驟b.1若是，對(duì)應(yīng)圖1中的“bool＝0？”輸出的y，設(shè)置循環(huán)最大值kmax為無(wú)窮大；

步驟b.2若否，對(duì)應(yīng)圖1中的“bool＝0？”輸出的n，循環(huán)最大值kmax為一常數(shù)；

具體到本實(shí)施例，對(duì)應(yīng)于b.1，bool＝0，循環(huán)最大值kmax設(shè)置為無(wú)窮大；系統(tǒng)處在實(shí)時(shí)工作模式；

若非本實(shí)施例bool＝0情況，則對(duì)應(yīng)于b.2，bool＝1，循環(huán)最大值kmax為常數(shù)，系統(tǒng)處在離線工作模式；

步驟c：慣性導(dǎo)航單元采集慣性導(dǎo)航運(yùn)動(dòng)參數(shù)，可見(jiàn)光定位單元解算可見(jiàn)光定位位置坐標(biāo)；

步驟c.1：假設(shè)當(dāng)前時(shí)刻為k，慣性導(dǎo)航單元采樣周期為t，慣性導(dǎo)航單元輸出的k時(shí)刻的原始橫向和縱向加速度數(shù)據(jù)通過(guò)低通濾波器進(jìn)行濾波處理；其中，為防止低通濾波器濾掉有用信號(hào)，低通濾波器截止頻率不能過(guò)低，具體到本實(shí)施例，設(shè)定低通濾波器截止頻率5hz；

步驟c.2：載體在k時(shí)刻的可見(jiàn)光定位數(shù)據(jù)通過(guò)現(xiàn)有可見(jiàn)光定位算法進(jìn)行計(jì)算，得到載體的可見(jiàn)光定位位置，并將其記為zk＝[xkyk]^t，其中符號(hào)t表示矩陣轉(zhuǎn)置；

步驟d：濾波與解算單元設(shè)置ukf各方程與各參數(shù)，使用ukf對(duì)載體的慣性運(yùn)動(dòng)數(shù)據(jù)和可見(jiàn)光定位數(shù)據(jù)進(jìn)行融合濾波處理，得到濾波后的載體位置；具體到本實(shí)施例，其方法說(shuō)明如下：

步驟d.1：設(shè)定狀態(tài)向量其中上標(biāo)t表示矩陣轉(zhuǎn)置，其中，xk為該時(shí)刻載體橫坐標(biāo)，yk為該時(shí)刻載體縱坐標(biāo)，為該時(shí)刻載體橫軸方向速度，為該時(shí)刻載體縱軸方向速度，為該時(shí)刻載體橫軸方向加速度，為該時(shí)刻載體橫軸方向加速度；同時(shí)設(shè)定濾波狀態(tài)方程為：

設(shè)定系統(tǒng)噪聲為：

設(shè)定觀測(cè)噪聲為：

步驟d.2：開(kāi)始進(jìn)行ukf濾波，具體與步驟3.2-3.7相同；

步驟e：判斷是否kmax<k，并決定是否完成本方法，具體為：

步驟e.1若是，對(duì)應(yīng)圖1中的“kmax<k？”輸出的y，則輸出本時(shí)刻位置坐標(biāo)(xk,yk)，并令k＝k+1，跳至步驟c；

步驟e.2若否，對(duì)應(yīng)圖1中的“kmax<k？”輸出的n，則輸出本時(shí)刻位置坐標(biāo)(xk,yk)，完成了本方法；

至此，從步驟a到e，完成了本實(shí)施例一種基于無(wú)跡卡爾曼濾波的可見(jiàn)光組合定位方法。

實(shí)施例2

以行人導(dǎo)航可見(jiàn)光/慣性組合定位系統(tǒng)為例，采用本發(fā)明“一種基于無(wú)跡卡爾曼濾波的可見(jiàn)光定位/慣性導(dǎo)航組合定位裝置”實(shí)現(xiàn)可見(jiàn)光定位/慣性定位組合定位：

步驟a：系統(tǒng)初始化單元初始化循環(huán)計(jì)數(shù)值、設(shè)置工作模式布爾值和循環(huán)計(jì)數(shù)最大值，設(shè)置k＝1時(shí)刻組合定位初始坐標(biāo)值，設(shè)置初始協(xié)方差，具體與步驟a相同；

步驟b：判斷bool值是否為0，并進(jìn)行相應(yīng)操作，具體與步驟b相同：

步驟c：慣性導(dǎo)航單元采集慣性導(dǎo)航運(yùn)動(dòng)參數(shù)，可見(jiàn)光定位單元解算可見(jiàn)光定位位置坐標(biāo)；

步驟c.1：假使當(dāng)前時(shí)刻為k，計(jì)算周期為行人步行的周期，慣性導(dǎo)航單元輸出k-1時(shí)刻到k時(shí)刻這一周期內(nèi)的行人行走距離sk、平均航向變化角δθk；

步驟c.2:行人的可見(jiàn)光定位數(shù)據(jù)通過(guò)現(xiàn)有可見(jiàn)光定位算法進(jìn)行計(jì)算，得到載體的可見(jiàn)光定位位置；具體與步驟c.2相同；

步驟d：濾波與解算單元設(shè)置ukf各方程與各參數(shù)，使用ukf對(duì)載體的慣性運(yùn)動(dòng)數(shù)據(jù)和可見(jiàn)光定位數(shù)據(jù)進(jìn)行融合濾波處理，得到濾波后的行人位置；具體到本實(shí)施例，其方法說(shuō)明如下：

步驟d.1：設(shè)定狀態(tài)向量xk＝[θkxkyk]^t，θk為該時(shí)刻行人行走方向航向角，xk、yk分別為行人該時(shí)刻的橫縱坐標(biāo)，其中上標(biāo)t表示矩陣轉(zhuǎn)置；設(shè)定濾波狀態(tài)方程為：

xk+1＝[θk+δθkxk+skcosθkyk+sksinθk]^t；

設(shè)定可見(jiàn)光觀測(cè)方程中觀測(cè)矩陣為：

設(shè)定系統(tǒng)噪聲為：

其中，vθ為航向角測(cè)量方差，在本實(shí)施例中取0.01²；vs為步長(zhǎng)估計(jì)方差，在本實(shí)施例中取0.01²。

設(shè)定觀測(cè)噪聲為：

步驟d.2：具體與步驟d.2相同；

步驟e：判斷循環(huán)計(jì)數(shù)值是否已經(jīng)達(dá)到計(jì)數(shù)最大值，并決定是否完成本方法，具體與步驟e相同；

至此，從步驟a到e，完成了本實(shí)施例一種基于無(wú)跡卡爾曼濾波的可見(jiàn)光組合定位方法。

實(shí)施例3

本實(shí)施例按照實(shí)施例1所述的參數(shù)具體闡述了執(zhí)行本發(fā)明步驟a到步驟c所得的組合定位位置結(jié)果，同時(shí)與現(xiàn)有可見(jiàn)光定位方法、慣性導(dǎo)航方法所得定位位置結(jié)果進(jìn)行比較，比較結(jié)果如圖2。

圖2中，橫軸、縱軸方向的單位均為米；虛線“實(shí)際路線”為agv小車的真實(shí)運(yùn)動(dòng)軌跡；十字劃線“ukf濾波結(jié)果”為經(jīng)本發(fā)明所提出的“一種基于無(wú)跡卡爾曼濾波的可見(jiàn)光組合定位方法”步驟a到步驟c所得的組合定位位置軌跡解算結(jié)果；星劃線“慣性導(dǎo)航結(jié)果”為采用傳統(tǒng)慣性導(dǎo)航方法得到的定位位置軌跡；圓圈劃線“可見(jiàn)光定位結(jié)果”為背景技術(shù)中專利(1)中提到的定位方法所得定位位置軌跡；需要說(shuō)明的是：由于專利(1)和專利(2)核心技術(shù)相似，因此我們僅選取專利(1)中所述技術(shù)與本專利的定位效果進(jìn)行對(duì)比。

從圖2中可以看出，“ukf濾波結(jié)果”曲線與“可見(jiàn)光定位結(jié)果”和“慣性導(dǎo)航結(jié)果”兩條曲線相比，與“實(shí)際路線”軌跡更為接近，且“ukf濾波結(jié)果”曲線抖動(dòng)更小，定位誤差更小。

為了更好的看清楚本方法的優(yōu)勢(shì)，將圖2中橫坐標(biāo)(0.95,1.02)，縱坐標(biāo)(0,0.2)內(nèi)的區(qū)域放大，放大后的結(jié)果圖如圖3所示。由圖3可以清晰的看出，該區(qū)域內(nèi)中pf曲線抖動(dòng)較小，與真實(shí)軌跡更加吻合；

為了更好地說(shuō)明本發(fā)明對(duì)于減小可見(jiàn)光定位方法與慣性導(dǎo)航方法定位誤差的作用，我們將圖2中“ukf濾波結(jié)果”曲線、“可見(jiàn)光定位結(jié)果”曲線、“慣性導(dǎo)航結(jié)果”相對(duì)于“實(shí)際路線”曲線之間的誤差、誤差分布分別作圖，如圖4、圖5所示。由圖4、圖5可以看出，該區(qū)域內(nèi)“ukf濾波結(jié)果”曲線所表明的誤差相比有其他兩條曲線所表明的有大幅減小，充分證明了本方法可以有效減小可見(jiàn)光定位系統(tǒng)的定位誤差。

需要說(shuō)明的是，本說(shuō)明書(shū)所述的只是本發(fā)明的較佳具體實(shí)施例，以上實(shí)施例僅用于說(shuō)明本發(fā)明的技術(shù)方案而非對(duì)本發(fā)明的限制。凡本領(lǐng)域技術(shù)人員依本發(fā)明的構(gòu)思通過(guò)邏輯分析、推理或者有限的實(shí)驗(yàn)可以得到的技術(shù)方案，皆應(yīng)在本發(fā)明的范圍之內(nèi)。