專(zhuān)利名稱(chēng)：基于航向角的wsn/ins組合導(dǎo)航系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本實(shí)用新型涉及一種基于航向角的WSN/INS組合導(dǎo)航系統(tǒng)，屬于多傳感器數(shù)據(jù)融合領(lǐng)域。
背景技術(shù)：
全球定位系統(tǒng)(Globalpositioning systems, GPS)和慣性導(dǎo)航系統(tǒng)(Inertialnavigation system, INS)均為目前應(yīng)用最廣泛的導(dǎo)航系統(tǒng)之一。其中GPS能夠提供精確地，具有持續(xù)穩(wěn)定導(dǎo)航精度的導(dǎo)航信息，但是在室內(nèi)、高層建筑密集的市區(qū)、礦井、隧道等環(huán)境下，GPS信號(hào)失鎖，不能進(jìn)行定位。INS雖然具有全自主、運(yùn)動(dòng)信息且全面、短時(shí)、高精度的優(yōu)點(diǎn)，雖然可以實(shí)現(xiàn)自主導(dǎo)航，但誤差隨時(shí)間積累，長(zhǎng)航時(shí)運(yùn)行條件下將導(dǎo)致導(dǎo)航精度嚴(yán)重 下降。因此，INS對(duì)GPS導(dǎo)航信息的補(bǔ)償只能是短期補(bǔ)償，而目前最為常用的GPS/INS組合導(dǎo)航系統(tǒng)的導(dǎo)航精度依賴(lài)于GPS的導(dǎo)航精度，在GPS長(zhǎng)時(shí)間失鎖的情況下，組合導(dǎo)航系統(tǒng)無(wú)法提供高精度的導(dǎo)航信息。近年來(lái),無(wú)線(xiàn)傳感器網(wǎng)絡(luò)(Wireless Sensors Network, WSN)以其低成本、低功耗和低系統(tǒng)復(fù)雜度的特點(diǎn)在短距離定位領(lǐng)域表現(xiàn)出很大的潛力。WSN為在無(wú)GPS信號(hào)地區(qū)，即所謂的“盲區(qū)”時(shí)，如室內(nèi)、高層建筑密集的市區(qū)、礦井、隧道等環(huán)境下未知節(jié)點(diǎn)定位提供了可能。但由于WSN采用的通信技術(shù)通常為短距離無(wú)線(xiàn)通信技術(shù)(如ZigBee、WIFI等)，因此若想完成長(zhǎng)距離的目標(biāo)跟蹤定位，需要大量的網(wǎng)絡(luò)節(jié)點(diǎn)共同完成，這增加了 WSN的網(wǎng)絡(luò)負(fù)擔(dān)。除此之外，WSN只能提供位置和速度信息，不能提供全面的運(yùn)動(dòng)信息。為了在GPS長(zhǎng)失鎖環(huán)境下得到長(zhǎng)時(shí)間的穩(wěn)定的導(dǎo)航信息，許多學(xué)者提出將WSN定位技術(shù)引入到低成本的INS系統(tǒng)中，構(gòu)建WSN/INS組合導(dǎo)航系統(tǒng)，如東南大學(xué)Y. Xu,雖然這一組合方式很好的解決了地下密閉環(huán)境下的長(zhǎng)距離目標(biāo)跟蹤和導(dǎo)航設(shè)備成本高的問(wèn)題，但是這種方法只是運(yùn)用了載體的速度、未知節(jié)點(diǎn)與參考節(jié)點(diǎn)之間的距離信息來(lái)對(duì)載體的導(dǎo)航信息進(jìn)行預(yù)估，并沒(méi)有將INS得到的載體的姿態(tài)信息(如航向角，加速度計(jì)等)考慮進(jìn)去，沒(méi)有有效的利用周?chē)沫h(huán)境信息。
實(shí)用新型內(nèi)容為了解決上述問(wèn)題，本實(shí)用新型提出了一種基于航向角的WSN/INS組合導(dǎo)航系統(tǒng)。本實(shí)用新型為解決其技術(shù)問(wèn)題采用如下技術(shù)方案一種基于航向角的WSN/INS組合導(dǎo)航系統(tǒng)，包括參考節(jié)點(diǎn)部分和未知節(jié)點(diǎn)部分，參考節(jié)點(diǎn)部分包括參考節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊、超聲測(cè)距模塊和時(shí)間同步模塊，其中，參考節(jié)點(diǎn)部分中的參考節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊負(fù)責(zé)完成遠(yuǎn)程命令的收發(fā)，在接到遠(yuǎn)程命令之后首先控制時(shí)間同步模塊完成測(cè)距的時(shí)間同步，接著打開(kāi)超聲測(cè)距模塊測(cè)量相應(yīng)的距離信息，最后，將超聲測(cè)距模塊測(cè)量得到的距離信息通過(guò)參考節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊返回給未知節(jié)點(diǎn)；未知節(jié)點(diǎn)部分包括未知節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊、INS導(dǎo)航模塊和中央數(shù)據(jù)處理模塊，其中，未知節(jié)點(diǎn)部分中的未知節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊負(fù)責(zé)完成未知節(jié)點(diǎn)與參考節(jié)點(diǎn)之間的距離信息的采集，并將采集得到的數(shù)據(jù)傳送給中央數(shù)據(jù)處理模塊；INS導(dǎo)航模塊完成的是陀螺加表信息的采集，并將采集得到的數(shù)據(jù)傳送給中央數(shù)據(jù)處理單元；中央數(shù)據(jù)處理單元負(fù)責(zé)將未知節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊和INS導(dǎo)航模塊采集到的數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，從而得到導(dǎo)航信息。所述的基于航向角的WSN/INS組合導(dǎo)航系統(tǒng)的控制方法，包括下列步驟(I)將導(dǎo)航過(guò)程分為培訓(xùn)過(guò)程和自適應(yīng)補(bǔ)償過(guò)程兩部分，將有WSN信號(hào)的導(dǎo)航過(guò)程稱(chēng)為培訓(xùn)過(guò)程，而只有INS信號(hào)的導(dǎo)航過(guò)程稱(chēng)之為自適應(yīng)補(bǔ)償過(guò)程；(2)在培訓(xùn)過(guò)程中，在本地相對(duì)坐標(biāo)系中將INS和WSN進(jìn)行集成，通過(guò)擴(kuò)展卡爾曼濾波對(duì)得到的同步導(dǎo)航數(shù)據(jù)進(jìn)行數(shù)據(jù)融合；(3)構(gòu)建擴(kuò)展卡爾曼濾波器的系統(tǒng)方程，該系統(tǒng)方程以INS每一時(shí)刻兩個(gè)方向的位直"]^差(^雄，氣V占)、速度"^差)、加速度計(jì)"^差)和航向角性作為狀
態(tài)變量，濾波器的系統(tǒng)方程如式(I)所示，其中r為采樣周期w力系統(tǒng)噪聲^力載體的·航向角，k為系統(tǒng)的采樣周期計(jì)數(shù)；
「 ， I 0 T 0 cos --JeSinai 0「 ]「 刁
2 2
e^M o I o r siii^ I i^cosf> o 氣v’k m^k
eVjrfM22em^k %,*■
0 — 0 0 I 0 rcosffi -Tsmm 0 0 ,(I)
p0 0 0 1 rsina Tcosm 0 p m
cOxMlk
0 0 0 0 I0 0 . m ,
^+1警,
0 0 0 0 0I Offl
興+i」L興」L
00 0 000IL J
%fi LJ xk
A(4)通過(guò)INS測(cè)量每個(gè)時(shí)刻未知節(jié)點(diǎn)在x和y方向的位置，將得到的位置信息與參考節(jié)點(diǎn)的位置信息通過(guò)距離公式計(jì)算出INS測(cè)量得到的未知節(jié)點(diǎn)和第I個(gè)參考節(jié)點(diǎn)之間的
距離如式⑵所示Lf) = I x_ - Xi f + (y_ - )2:1 = 1，2,…,N(2)其中，為INS測(cè)量得到的未知節(jié)點(diǎn)的坐標(biāo)，(Ii5y.)為第i個(gè)參考節(jié)點(diǎn)的坐標(biāo)N力參考節(jié)點(diǎn)的數(shù)目；通過(guò)超聲測(cè)距模塊測(cè)量出未知節(jié)點(diǎn)和第i個(gè)參考節(jié)點(diǎn)之間的距離將Lf)的平方與I^sa)的平方作差，差值定義為，在此基礎(chǔ)上，以
Ml-AL1G 22)作為濾波器的觀(guān)測(cè)量，與此同時(shí)，將INS測(cè)量的每個(gè)時(shí)刻的兩個(gè)方向的速度與WSN中包含的測(cè)速傳感器測(cè)量得到的速度作差，將差值也作為濾波器的觀(guān)測(cè)量，同時(shí)，將INS測(cè)量得到的航向角也作為觀(guān)測(cè)量，通過(guò)上述的觀(guān)測(cè)量構(gòu)建觀(guān)測(cè)方程，濾波器觀(guān)測(cè)方程如式⑶所示
權(quán)利要求1.一種基于航向角的WSN/INS組合導(dǎo)航系統(tǒng)，其特征在于包括參考節(jié)點(diǎn)部分和未知節(jié)點(diǎn)部分，參考節(jié)點(diǎn)部分包括參考節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊、超聲測(cè)距模塊和時(shí)間同步模塊，其中，參考節(jié)點(diǎn)部分中的參考節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊負(fù)責(zé)完成遠(yuǎn)程命令的收發(fā)，在接到遠(yuǎn)程命令之后首先控制時(shí)間同步模塊完成測(cè)距的時(shí)間同步，接著打開(kāi)超聲測(cè)距模塊測(cè)量相應(yīng)的距離信息，最后，將超聲測(cè)距模塊測(cè)量得到的距離信息通過(guò)參考節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊返回給未知節(jié)點(diǎn)；未知節(jié)點(diǎn)部分包括未知節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊、INS導(dǎo)航模塊和中央數(shù)據(jù)處理模塊，其中，未知節(jié)點(diǎn)部分中的未知節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊負(fù)責(zé)完成未知節(jié)點(diǎn)與參考節(jié)點(diǎn)之間的距離信息的采集，并將采集得到的數(shù)據(jù)傳送給中央數(shù)據(jù)處理模塊；INS導(dǎo)航模塊完成的是陀螺加表信息的采集，并將采集得到的數(shù)據(jù)傳送給中央數(shù)據(jù)處理單元；中央數(shù)據(jù)處理單元負(fù)責(zé)將未知節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊和INS導(dǎo)航模塊采集到的數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，從而得到導(dǎo)航信息。
專(zhuān)利摘要一種基于航向角的WSN/INS組合導(dǎo)航系統(tǒng)，屬于多傳感器數(shù)據(jù)融合領(lǐng)域。該組合導(dǎo)航系統(tǒng)包括參考節(jié)點(diǎn)部分和未知節(jié)點(diǎn)部分，參考節(jié)點(diǎn)部分包括參考節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊、超聲測(cè)距模塊和時(shí)間同步模塊；未知節(jié)點(diǎn)部分包括未知節(jié)點(diǎn)無(wú)線(xiàn)網(wǎng)絡(luò)接收模塊、INS導(dǎo)航模塊和中央數(shù)據(jù)處理模塊。利用擴(kuò)展卡爾曼濾波得到系統(tǒng)中INS測(cè)量的位置誤差和速度誤差最優(yōu)預(yù)估值，將INS本身測(cè)出的導(dǎo)航信息與最優(yōu)預(yù)估值作差，得出最優(yōu)的導(dǎo)航信息。本實(shí)用新型通過(guò)運(yùn)用WSN/INS組合導(dǎo)航系統(tǒng)，克服了在地下巷道、狹長(zhǎng)隧道等GPS信號(hào)長(zhǎng)期失鎖的環(huán)境下INS導(dǎo)航的誤差隨時(shí)間漂移的問(wèn)題。能滿(mǎn)足地面城市交通、狹長(zhǎng)隧道、小型智能機(jī)器人等中低精度的定位和定向的要求。
文檔編號(hào)G01C21/20GK202562486SQ20122019039
公開(kāi)日2012年11月28日 申請(qǐng)日期2012年5月2日 優(yōu)先權(quán)日2012年5月2日
發(fā)明者陳熙源, 徐元, 李慶華, 黃浩乾, 申沖 申請(qǐng)人:東南大學(xué)