用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位方法與系統(tǒng)的制作方法
【專利摘要】本發(fā)明提供了一種用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位方法和系統(tǒng)，該方法包括如下步驟：S01：通過慣性導(dǎo)航系統(tǒng)獲得定位主體的位置信息、類矩形環(huán)境的地圖信息，進而得到定位主體的中心到類矩形環(huán)境的四個邊緣的四個預(yù)測距離值；S02：根據(jù)超聲波傳感器相對定位主體的位置、定位主體的航向以及超聲波傳感器的測距值計算定位主體中心到類矩形環(huán)境的四個邊緣的四個實測距離值；S03：將實測距離值與預(yù)測距離值進行比較，計算兩者差值的絕對值，根據(jù)該差值的絕對值的大小設(shè)定不同實測距離值的不同的權(quán)重；S04：依據(jù)不同的權(quán)值對實測距離值進行加權(quán)平均求和，得到最終的實際距離；S05：依據(jù)最終的實際距離對定位主體進行位置的更新。
【專利說明】用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位方法與系統(tǒng)

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及慣性導(dǎo)航和室內(nèi)定位導(dǎo)航技術(shù)，特別涉及一種用于室內(nèi)導(dǎo)航的超聲波 陣列輔助定位方法與系統(tǒng)。

【背景技術(shù)】
[0002] 在室外導(dǎo)航定位技術(shù)中，衛(wèi)星導(dǎo)航系統(tǒng)，包括全球定位系統(tǒng)GPS以及我國自主研 發(fā)的北斗導(dǎo)航系統(tǒng)都已經(jīng)得到了非常廣泛的應(yīng)用，極大地豐富了人們的生活。然而，當處在 室內(nèi)環(huán)境下時，所能接收到的GPS信號或北斗衛(wèi)星信號是非常微弱的，甚至根本接收不到， 這就需要提供室內(nèi)導(dǎo)航與定位的新方法。
[0003] 目前的室內(nèi)導(dǎo)航方法主要分為兩種，一種是基于信標定位的方法，另一種是基于 慣性器件定位的方法。基于信標定位的方法，即在當前環(huán)境中先預(yù)設(shè)一定數(shù)量的位置固定 的信標，用戶通過與這些信標的信息交互來獲取自身當前的位置，如RFID定位、WIFI定位 等?；趹T性器件定位的方法，通過設(shè)備內(nèi)置的加速度計、陀螺儀、磁力計等來感知定位目 標的運動方向和運動距離，逐步計算目標位置進行定位導(dǎo)航。
[0004] 基于信標定位的方法雖然精度高，但有以下兩個缺陷：
[0005] (1).需要事先在環(huán)境中部署信標，所需成本很高；
[0006] (2).若發(fā)生災(zāi)害，信標極有可能會被摧毀，使得該條件下的室內(nèi)定位導(dǎo)航不可進 行。
[0007] 而基于慣性器件定位的方法，不需要設(shè)定信標，因而成本相對較低，且由于慣性傳 感器安裝在可便攜設(shè)備中，具有自主性，即使發(fā)生災(zāi)害或其他緊急情況也能夠進行定位和 導(dǎo)航。雖然慣性導(dǎo)航有以上的諸多優(yōu)點，但這種導(dǎo)航方法也有自己的不足之處，其最大的缺 點是長時間導(dǎo)航精度較低。因為慣性器件的誤差是可累積的，當經(jīng)過一定的時間后，導(dǎo)航定 位誤差會迅速增長。


【發(fā)明內(nèi)容】

[0008] 本發(fā)明要解決的技術(shù)問題是如何使慣性導(dǎo)航系統(tǒng)的定位結(jié)果更加精確，導(dǎo)航性能 更好。
[0009] 為了解決這一技術(shù)問題，本發(fā)明提供了一種用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位 方法，用以輔助慣性導(dǎo)航系統(tǒng)，提供了定位主體和若干設(shè)于所述定位主體外圍的超聲波傳 感器，若干所述超聲波傳感器的發(fā)射超聲波的方向的反向延長線均通過所述定位主體的中 心，所述超聲波傳感器用以測量超聲波發(fā)射點到對應(yīng)類矩形環(huán)境一條邊緣的垂線距離，得 到測距值；
[0010] 該方法包括如下步驟：
[0011] S01 :通過慣性導(dǎo)航系統(tǒng)獲得所述定位主體的位置信息、類矩形環(huán)境的地圖信息， 進而得到所述定位主體的中心到類矩形環(huán)境的四個邊緣的四個預(yù)測距離值；
[0012] S02:根據(jù)所述超聲波傳感器相對定位主體的位置、定位主體的航向以及超聲波傳 感器的測距值計算所述定位主體中心到類矩形環(huán)境的四個邊緣的四個實測距離值；
[0013] S03:將實測距離值與預(yù)測距離值進行比較，計算兩者差值的絕對值，根據(jù)該差值 的絕對值的大小設(shè)定不同實測距離值的不同的權(quán)重，其中，差值的絕對值越大，權(quán)重越小；
[0014] S04 :依據(jù)不同的權(quán)值對實測距離值進行加權(quán)平均求和，得到最終的實際距離；
[0015] S05 :依據(jù)最終的實際距離對定位主體進行位置的更新。
[0016] 在所述步驟S02之前，還包括對所述超聲波傳感器篩選的過程，在該過程中，先計 算每個所述超聲波傳感器相對于類矩形環(huán)境地圖的朝向，然后根據(jù)朝向的角度閾值過濾掉 部分超聲波傳感器，僅就未被過濾的超聲波傳感器實施后續(xù)步驟。
[0017] 在所述步驟S03中，還包括對實測距離值篩選的過程，在該過程中，將實測距離值 與預(yù)測距離值差值的絕對值與一個閾值比較，若大于該閾值，則將該實測距離值過濾掉，僅 就未被過濾掉的實測距離值進行權(quán)重設(shè)定，進而實施后續(xù)步驟。
[0018] 本發(fā)明還提供了一種用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位系統(tǒng)，用以輔助慣性導(dǎo) 航系統(tǒng)，包括粗略位置獲取模塊、地圖信息獲取模塊、距離預(yù)測模塊、超聲波陣列測距模塊、 航向獲取模塊和導(dǎo)航濾波模塊，
[0019] 所述粗略位置獲取模塊，用以通過所述慣性導(dǎo)航系統(tǒng)獲得定位主體的位置信息；
[0020] 所述地圖信息獲取模塊，用以獲得定位主體所處類矩形環(huán)境的地圖信息；
[0021] 所述距離預(yù)測模塊，用以根據(jù)定位主體的位置信息和其所處類矩形環(huán)境的地圖信 息計算定位主體到類矩形環(huán)境四個邊緣的距離，得到相應(yīng)的預(yù)測距離值；
[0022] 所述超聲波陣列測距模塊，至少包括若干安裝于定位主體上的超聲波傳感器，所 述超聲波傳感器用以測量超聲波發(fā)射點到對應(yīng)類矩形環(huán)境一條邊緣的垂線距離，得到測距 值；若干所述超聲波傳感器分布于所述定位主體的外圍，且每個所述超聲波傳感器發(fā)射超 聲波的方向的反向延長線均通過所述定位主體的中心。
[0023] 所述航向獲取模塊，用以通過所述慣性導(dǎo)航系統(tǒng)獲得定位主體的前進方向；使用 的是慣性導(dǎo)航系統(tǒng)中輸出的航向信息，記為heading。
[0024] 所述導(dǎo)航濾波模塊，用以根據(jù)所述預(yù)測距離值、測距值和定位主體的前進方向，計 算得到定位主體到類矩形環(huán)境四條邊的實際距離，進而實現(xiàn)對定位主體位置的位置更新。
[0025] 所述導(dǎo)航濾波模塊在用于計算得到定位主體到類矩形環(huán)境四條邊的實際距離時，
[0026] 所述導(dǎo)航濾波模塊先用以根據(jù)所述超聲波傳感器相對定位主體的位置、定位主體 的航向以及超聲波傳感器的測距值計算所述定位主體中心到類矩形環(huán)境的四個邊緣的四 個實測距離值；
[0027] 所述導(dǎo)航濾波模塊再用以將實測距離值與預(yù)測距離值進行比較，計算兩者差值的 絕對值，根據(jù)該差值的絕對值的大小設(shè)定不同實測距離值的不同的權(quán)重，其中，差值的絕對 值越大，權(quán)重越??；
[0028] 然后，所述導(dǎo)航濾波模塊依據(jù)不同的權(quán)值對實測距離值進行加權(quán)平均求和，得到 最終的實際距離；
[0029] 最后，所述導(dǎo)航濾波模塊依據(jù)最終的實際距離對定位主體進行位置的更新。
[0030] 在計算得到所述實測距離值之前，所述導(dǎo)航濾波模塊還用以實施所述超聲波傳感 器篩選的過程，在該過程中，先計算每個所述超聲波傳感器相對于類矩形環(huán)境地圖的朝向， 然后根據(jù)朝向的角度閾值過濾掉部分超聲波傳感器，僅就未被過濾的超聲波傳感器實施后 續(xù)步驟。
[0031] 在對實測距離值設(shè)置不同的權(quán)重之前，所述導(dǎo)航濾波模塊還用以實施對實測距離 值篩選的過程，在該過程中，將實測距離值與預(yù)測距離值差值的絕對值與一個閾值比較，若 大于該閾值，則將該實測距離值過濾掉，僅就未被過濾掉的實測距離值進行權(quán)重設(shè)定，進而 實施后續(xù)步驟。
[0032] 本發(fā)明在慣性器件定位的基礎(chǔ)上，添加超聲波陣列測距裝置，將超聲波傳感器設(shè) 于定位主體上，從而進行融合定位導(dǎo)航，解決了累積誤差問題，進一步的，本發(fā)明還將預(yù)測 距離值和實測距離值有效的運用與整合，最終使慣性導(dǎo)航系統(tǒng)的定位結(jié)果更加精確，導(dǎo)航 性能更好。

【專利附圖】

【附圖說明】
[0033] 圖1是本發(fā)明一實施例中用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位系統(tǒng)的模塊示意 圖；
[0034] 圖2是本發(fā)明一實施例中定位主體距離預(yù)測示意圖；
[0035] 圖3是本發(fā)明一實施例中超聲波陣列均勻分布示意圖；
[0036] 圖4是本發(fā)明另一實施例中超聲波陣列非均勻分布示意圖；
[0037] 圖5是本發(fā)明一實施例中超聲波傳感器測距示意圖；
[0038] 圖6是本發(fā)明一實施例中導(dǎo)航濾波模塊的操作流程圖。

【具體實施方式】
[0039] 以下將結(jié)合圖1至圖6對本發(fā)明提供的用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位方法 與系統(tǒng)進行詳細的描述，其為本發(fā)明一可選的實施例，可以認為，本領(lǐng)域的技術(shù)人員在不改 變本發(fā)明精神和內(nèi)容的范圍內(nèi)能夠?qū)ζ溥M行修改和潤色。
[0040] 本實施例提供了一種用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位方法，用以輔助慣性導(dǎo) 航系統(tǒng)，提供了定位主體和若干設(shè)于所述定位主體外圍的超聲波傳感器，若干所述超聲波 傳感器的發(fā)射超聲波的方向的反向延長線均通過所述定位主體的中心，所述超聲波傳感器 用以測量超聲波發(fā)射點到對應(yīng)類矩形環(huán)境一條邊緣的垂線距離，得到測距值；
[0041] 該方法包括如下步驟：
[0042] S01 :通過慣性導(dǎo)航系統(tǒng)獲得所述定位主體的位置信息、類矩形環(huán)境的地圖信息， 進而得到所述定位主體的中心到類矩形環(huán)境的四個邊緣的四個預(yù)測距離值；
[0043] S02 :根據(jù)所述超聲波傳感器相對定位主體的位置、定位主體的航向以及超聲波傳 感器的測距值計算所述定位主體中心到類矩形環(huán)境的四個邊緣的四個實測距離值；
[0044] S03:將實測距離值與預(yù)測距離值進行比較，計算兩者差值的絕對值，根據(jù)該差值 的絕對值的大小設(shè)定不同實測距離值的不同的權(quán)重，其中，差值的絕對值越大，權(quán)重越小；
[0045] S04 :依據(jù)不同的權(quán)值對實測距離值進行加權(quán)平均求和，得到最終的實際距離；
[0046] S05 :依據(jù)最終的實際距離對定位主體進行位置的更新。
[0047] 在所述步驟S02之前，還包括對所述超聲波傳感器篩選的過程，在該過程中，先計 算每個所述超聲波傳感器相對于類矩形環(huán)境地圖的朝向，然后根據(jù)朝向的角度閾值過濾掉 部分超聲波傳感器，僅就未被過濾的超聲波傳感器實施后續(xù)步驟。
[0048] 在所述步驟S03中，還包括對實測距離值篩選的過程，在該過程中，將實測距離值 與預(yù)測距離值差值的絕對值與一個閾值比較，若大于該閾值，則將該實測距離值過濾掉，僅 就未被過濾掉的實測距離值進行權(quán)重設(shè)定，進而實施后續(xù)步驟。
[0049] 本實施例還提供了一種用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位系統(tǒng)，用以輔助慣性 導(dǎo)航系統(tǒng)，包括粗略位置獲取模塊、地圖信息獲取模塊、距離預(yù)測模塊、超聲波陣列測距模 塊、航向獲取模塊和導(dǎo)航濾波模塊，具體來說，請參閱圖1，本實施例中，所述粗略位置獲取 模塊、地圖信息獲取模塊均與距離預(yù)測模塊相連，所述距離預(yù)測模塊、超聲波陣列實際測距 模塊、航向獲取模塊均與導(dǎo)航濾波模塊相連。
[0050] 所述粗略位置獲取模塊，用以通過所述慣性導(dǎo)航系統(tǒng)獲得定位主體的位置信息； 所述粗略位置獲取模塊使用的是慣性導(dǎo)航系統(tǒng)的輸出信息，即慣性導(dǎo)航系統(tǒng)確定的定位主 體的位置信息，在二維地圖中可表示為（x，y)。
[0051] 所述地圖信息獲取模塊，用以獲得定位主體所處類矩形環(huán)境的地圖信息；所述地 圖信息獲取模塊用于獲取定位主體所處環(huán)境的地圖信息，本發(fā)明針對的特定環(huán)境為類矩形 環(huán)境，比如方形的房間，長直走廊等等，地圖信息即該類矩形環(huán)境的寬和長，記為wide和 length。
[0052] 所述距離預(yù)測模塊，用以根據(jù)定位主體的位置信息和其所處類矩形環(huán)境的地圖信 息計算定位主體到類矩形環(huán)境四個邊緣的距離，得到相應(yīng)的預(yù)測距離值；具體地，請參閱圖 2,分別為右距離d_right，上距離d_up，左距離d_left，下距離d_down。對應(yīng)地，有：
[0053] d_right = wide-x
[0054] d_up = long-y
[0055] d_left = x
[0056] d_down = y
[0057] 以上的模塊距離預(yù)測模塊、地圖信息獲取模塊和粗略位置獲取模塊可用以實現(xiàn)步 驟S01 :通過慣性導(dǎo)航系統(tǒng)獲得所述定位主體的位置信息、類矩形環(huán)境的地圖信息，進而得 到所述定位主體的中心到類矩形環(huán)境的四個邊緣的四個預(yù)測距離值；
[0058] 所述超聲波陣列測距模塊，至少包括若干安裝于定位主體上的超聲波傳感器，所 述超聲波傳感器用以測量超聲波發(fā)射點到對應(yīng)類矩形環(huán)境一條邊緣的垂線距離，得到測距 值；
[0059] 若干所述超聲波傳感器分布于所述定位主體的外圍，且每個所述超聲波傳感器發(fā) 射超聲波的方向的反向延長線均通過所述定位主體的中心。
[0060] 請參閱圖3和圖4,超聲波陣列分布在定位主體外圍，超聲發(fā)射方向的反向延長線 通過定位主體中心，這些傳感器可均勻分布，如圖3所示，在其他可選的實施例中，也可非 均勻分布，如圖4所示，數(shù)量為不限定個數(shù)，但它們的分布位置是已知的，另外定位主體外 圍不一定為圓形，可以為其他任何形狀，比如若定位主體為遙控車，其外圍可能呈圓形，也 可能為方形。亦即，超聲波傳感器的個數(shù)和位置分布完全可以是任意的。超聲波陣列的分 布位置可用多個二元數(shù)組表示，即（Α，Θ J，（r2，Θ 2)，（r3，Θ 3)，…，（rn，θ n)，& (i = 1? n)表示超聲波傳感器到定位主體中心的距離，Θ = 1?n，n是超聲波陣列裝置中所有 傳感器的個數(shù)）表不超聲波傳感器相對于定位主體坐標系中的正右方的夾角，逆時針時為 正值。
[0061] 超聲波傳感器由于其價格便宜，精度高而得到了廣泛的應(yīng)用，實際中使用超聲波 傳感器時大部分都是將超聲波垂直入射到物體表面，通過飛行時間換算為距離獲得測量距 離。在實驗中我們發(fā)現(xiàn)，當超聲波的入射角方向在一個較小的范圍內(nèi)時，測量得到的距離也 是垂直入射距離，即超聲波傳感器到物體表面的最短距離。此時，在已知地圖信息的條件下 我們可以利用這個垂直入射距離對需要定位的主體進行位置修正，獲取更加精確的定位結(jié) 果。超聲波測量時是以扇形方式發(fā)射超聲波，本實施例所用的是入射角較小時超聲波發(fā)射 點到矩形環(huán)境一條邊緣的垂線距離。
[0062] 所述航向獲取模塊，用以通過所述慣性導(dǎo)航系統(tǒng)獲得定位主體的前進方向；所述 航向獲取模塊使用的是慣性導(dǎo)航系統(tǒng)中輸出的航向信息，記為heading。如圖5所示，其中 heading可以記做航向與地圖坐標系中的正右方的夾角，逆時針時為正值，而heading與定 位主體坐標系的正右方的夾角為+90°。
[0063] 所述導(dǎo)航濾波模塊，用以根據(jù)所述預(yù)測距離值、測距值和定位主體的前進方向，計 算得到定位主體到類矩形環(huán)境四條邊的實際距離，進而實現(xiàn)對定位主體位置的位置更新。 [0064] 亦即，所述導(dǎo)航濾波模塊通過融合距離預(yù)測模塊、超聲波陣列實際測距模塊、航向 獲取模塊的信息，對定位主體的位置進行更新，得到更加精確的定位導(dǎo)航結(jié)果。導(dǎo)航濾波模 塊是超聲波陣列輔助定位方法的核心。用以實現(xiàn)以上所述的步驟S2至S5。
[0065] 所述導(dǎo)航濾波模塊在用于計算得到定位主體到類矩形環(huán)境四條邊的實際距離時， [0066] 所述導(dǎo)航濾波模塊先用以根據(jù)所述超聲波傳感器相對定位主體的位置、定位主體 的航向以及超聲波傳感器的測距值計算所述定位主體中心到類矩形環(huán)境的四個邊緣的四 個實測距離值；在計算得到所述實測距離值之前，所述導(dǎo)航濾波模塊還用以實施所述超聲 波傳感器篩選的過程，在該過程中，先計算每個所述超聲波傳感器相對于類矩形環(huán)境地圖 的朝向，然后根據(jù)朝向的角度閾值過濾掉部分超聲波傳感器，僅就未被過濾的超聲波傳感 器實施后續(xù)步驟。
[0067] 所述導(dǎo)航濾波模塊再用以將實測距離值與預(yù)測距離值進行比較，計算兩者差值的 絕對值，根據(jù)該差值的絕對值的大小設(shè)定不同實測距離值的不同的權(quán)重，其中，差值的絕對 值越大，權(quán)重越?。辉趯崪y距離值設(shè)置不同的權(quán)重之前，所述導(dǎo)航濾波模塊還用以實施對 實測距離值篩選的過程，在該過程中，將實測距離值與預(yù)測距離值差值的絕對值與一個閾 值比較，若大于該閾值，則將該實測距離值過濾掉，僅就未被過濾掉的實測距離值進行權(quán)重 設(shè)定，進而實施后續(xù)步驟。
[0068] 然后，所述導(dǎo)航濾波模塊依據(jù)不同的權(quán)值對實測距離值進行加權(quán)平均求和，得到 最終的實際距離；
[0069] 最后，所述導(dǎo)航濾波模塊依據(jù)最終的實際距離對定位主體進行位置的更新。
[0070] 具體來說，請參考圖5和圖6,導(dǎo)航濾波模塊的操作流程可以以下步驟來實現(xiàn)：
[0071] 步驟A :以地圖坐標系右方為參考方向，逆時針時為正值，計算各個超聲波傳 感器在地圖坐標系中發(fā)射超聲波的方向，即9 i+heading-90 ° , Θ 2+heading_90 ° , 03+heading_9O。，…，0n+heading_9〇。，且通過取模運算轉(zhuǎn)化為0。?360。之內(nèi)。 因為只有入射角在±20度范圍內(nèi)的超聲波得到的是垂直入射距離，所以篩選保留入 射角在±20°范圍內(nèi)的超聲波傳感器信息。即對于可用的超聲波傳感器，其入射角為 0° (360。）±20。，90° ±20。，180° ±20。，270° ±20。，這里用多個三元數(shù)組表 示，即（A，hd)，（r2, β2，(12)，（r3, β3，(13)，···，（rnl, 3nl，dnl)，其中?是角度過濾后的 超聲波傳感器的個數(shù)，rji = 1?ηι)表示超聲波傳感器到定位主體中心的距離， =1?nl)表示超聲波傳感器相對于地圖坐標系正右方的夾角，逆時針為正值，即β i = mod( Θ i+heading-90。，360° )，ddi = 1?nl)表示超聲波傳感器的初始測距信息；
[0072] 步驟B :在經(jīng)過步驟A篩選保留下來的超聲波傳感器陣列中，計算每個超聲波傳感 器的三元組信息換算到定位主體到類矩形環(huán)境的四個邊緣的信息。當然，每個超聲波傳感 器只換算到類矩形環(huán)境的一邊。具體地，請參閱圖5,假設(shè)超聲波傳感器i相對于在地圖坐 標系中的入射方向在±20°內(nèi)，則該超聲波傳感器可對應(yīng)定位主體到類矩形環(huán)境的右邊的 距離，設(shè)其三元數(shù)組信息為（A，βρφ)，利用I獲取入射角 Yi，這里假設(shè)β, = 10°，則 、=10。（若 1 = 95。，則、=|95-90|。=5。，若1 = 165。，則、=|165-180|。 = 15° )，得到入射角、后，可以計算該實測距離值(1/=1^(3〇8(、)+屯；
[0073] 步驟C :比較d/ (i = 1?nl)和相應(yīng)的4個預(yù)測距離右距離d_right，上距離d_ up，左距離d_left，下距離d_down中的一個進行比較。具體地，仍以步驟B中給出的L = 10°為例，d/將和d_right進行比較。如果兩者相差過大，即絕對值差超過一個閾值，比如 20cm，有| d/ -d_right | >20,則舍棄該d/，否則保留該距離信息，并記錄和據(jù)測距離的差絕 對值Δ i ;
[0074] 步驟D :在保留下來的超聲測距信息中，對于右、上、左、下每一邊緣，根據(jù)距離差 Mi = l?n2，n2是距離差值過濾后的超聲波傳感器的個數(shù)）給予每個實測計算距離一定 的權(quán)值 Wi，若距離差\越小則權(quán)重越大，反之權(quán)重越小。具體計算方法為，以右側(cè)邊緣為 例，假設(shè)Aji = l?n_right，n_right是對應(yīng)于右方的所有合適的超聲傳感器的個數(shù)）是 對應(yīng)于右方的第i個實測計算距離的差值，d/則該d/的權(quán)重為：

【權(quán)利要求】
1. 一種用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位方法，用以輔助慣性導(dǎo)航系統(tǒng)，提供了定 位主體和若干設(shè)于所述定位主體外圍的超聲波傳感器，若干所述超聲波傳感器的發(fā)射超聲 波的方向的反向延長線均通過所述定位主體的中心，所述超聲波傳感器用以測量超聲波發(fā) 射點到對應(yīng)類矩形環(huán)境一條邊緣的垂線距離，得到測距值； 該方法包括如下步驟： 501 :通過慣性導(dǎo)航系統(tǒng)獲得所述定位主體的位置信息、類矩形環(huán)境的地圖信息，進而 得到所述定位主體的中心到類矩形環(huán)境的四個邊緣的四個預(yù)測距離值； 502 :根據(jù)所述超聲波傳感器相對定位主體的位置、定位主體的航向以及超聲波傳感器 的測距值計算所述定位主體中心到類矩形環(huán)境的四個邊緣的四個實測距離值； 503 :將實測距離值與預(yù)測距離值進行比較，計算兩者差值的絕對值，根據(jù)該差值的絕 對值的大小設(shè)定不同實測距離值的不同的權(quán)重，其中，差值的絕對值越大，權(quán)重越??； 504 :依據(jù)不同的權(quán)值對實測距離值進行加權(quán)平均求和，得到最終的實際距離； 505 :依據(jù)最終的實際距離對定位主體進行位置的更新。
2. 如權(quán)利要求1所述的用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位方法，其特征在于：在所 述步驟S02之前，還包括對所述超聲波傳感器篩選的過程，在該過程中，先計算每個所述超 聲波傳感器相對于類矩形環(huán)境地圖的朝向，然后根據(jù)朝向的角度閾值過濾掉部分超聲波傳 感器，僅就未被過濾的超聲波傳感器實施后續(xù)步驟。
3. 如權(quán)利要求1所述的用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位方法，其特征在于：在所 述步驟S03中，還包括對實測距離值篩選的過程，在該過程中，將實測距離值與預(yù)測距離值 差值的絕對值與一個閾值比較，若大于該閾值，則將該實測距離值過濾掉，僅就未被過濾掉 的實測距離值進行權(quán)重設(shè)定，進而實施后續(xù)步驟。
4. 一種用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位系統(tǒng)，用以輔助慣性導(dǎo)航系統(tǒng)，包括粗略 位置獲取模塊、地圖信息獲取模塊、距離預(yù)測模塊、超聲波陣列測距模塊、航向獲取模塊和 導(dǎo)航濾波模塊， 所述粗略位置獲取模塊，用以通過所述慣性導(dǎo)航系統(tǒng)獲得定位主體的位置信息； 所述地圖信息獲取模塊，用以獲得定位主體所處類矩形環(huán)境的地圖信息； 所述距離預(yù)測模塊，用以根據(jù)定位主體的位置信息和其所處類矩形環(huán)境的地圖信息計 算定位主體到類矩形環(huán)境四個邊緣的距離，得到相應(yīng)的預(yù)測距離值； 所述超聲波陣列測距模塊，至少包括若干安裝于定位主體上的超聲波傳感器，所述超 聲波傳感器用以測量超聲波發(fā)射點到對應(yīng)類矩形環(huán)境一條邊緣的垂線距離，得到測距值； 所述航向獲取模塊，用以通過所述慣性導(dǎo)航系統(tǒng)獲得定位主體的前進方向； 所述導(dǎo)航濾波模塊，用以根據(jù)所述預(yù)測距離值、測距值和定位主體的前進方向，計算得 到定位主體到類矩形環(huán)境四條邊的實際距離，進而實現(xiàn)對定位主體位置的位置更新。
5. 如權(quán)利要求4所述的用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位系統(tǒng)定位系統(tǒng)，其特征在 于：若干所述超聲波傳感器分布于所述定位主體的外圍，且每個所述超聲波傳感器發(fā)射超 聲波的方向的反向延長線均通過所述定位主體的中心。
6. 如權(quán)利要求4所述的用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位系統(tǒng)，其特征在于：所述 導(dǎo)航濾波模塊在用于計算得到定位主體到類矩形環(huán)境四條邊的實際距離時， 所述導(dǎo)航濾波模塊先用以根據(jù)所述超聲波傳感器相對定位主體的位置、定位主體的航 向以及超聲波傳感器的測距值計算所述定位主體中心到類矩形環(huán)境的四個邊緣的四個實 測距離值； 所述導(dǎo)航濾波模塊再用以將實測距離值與預(yù)測距離值進行比較，計算兩者差值的絕對 值，根據(jù)該差值的絕對值的大小設(shè)定不同實測距離值的不同的權(quán)重，其中，差值的絕對值越 大，權(quán)重越?。? 然后，所述導(dǎo)航濾波模塊依據(jù)不同的權(quán)值對實測距離值進行加權(quán)平均求和，得到最終 的實際距離； 最后，所述導(dǎo)航濾波模塊依據(jù)最終的實際距離對定位主體進行位置的更新。
7. 如權(quán)利要求6所述的用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位系統(tǒng)，其特征在于：在計 算得到所述實測距離值之前，所述導(dǎo)航濾波模塊還用以實施所述超聲波傳感器篩選的過 程，在該過程中，先計算每個所述超聲波傳感器相對于類矩形環(huán)境地圖的朝向，然后根據(jù)朝 向的角度閾值過濾掉部分超聲波傳感器，僅就未被過濾的超聲波傳感器實施后續(xù)步驟。
8. 如權(quán)利要求6所述的用于室內(nèi)導(dǎo)航的超聲波陣列輔助定位系統(tǒng)，其特征在于：在對 實測距離值設(shè)置不同的權(quán)重之前，所述導(dǎo)航濾波模塊還用以實施對實測距離值篩選的過 程，在該過程中，將實測距離值與預(yù)測距離值差值的絕對值與一個閾值比較，若大于該閾 值，則將該實測距離值過濾掉，僅就未被過濾掉的實測距離值進行權(quán)重設(shè)定，進而實施后續(xù) 步驟。
【文檔編號】G01S5/18GK104111445SQ201410324167
【公開日】2014年10月22日 申請日期:2014年7月9日 優(yōu)先權(quán)日:2014年7月9日
【發(fā)明者】劉成旋, 錢久超, 裴凌, 鄒丹平, 劉佩林, 郁文賢, 籍晨 申請人:上海交通大學(xué)