本發(fā)明涉及UWB定位與慣導定位
技術(shù)領域：
，尤其涉及一種基于UWB測距的慣導原點定位方法及系統(tǒng)。
背景技術(shù)：
：隨著現(xiàn)代社會日新月異的發(fā)展，在各個領域內(nèi)，需要被精確定位的目標隨之越來越多，因此人們對定位技術(shù)也提出了更高層次的要求，尤其是對動態(tài)移動目標的精確定位，逐漸成為了當代人們關注的關鍵問題。然而在現(xiàn)代社會某些領域中，一些我們常用的動態(tài)定位方式已經(jīng)無法滿足當前日益增強的精確定位的需求，在這種背景下，超寬帶無線通訊UWB(UltraWideband，超寬帶)定位系統(tǒng)應運而生，使得對動態(tài)目標的精確定位成為可能。UWB是一種全新的、與傳統(tǒng)通信技術(shù)有極大差異的通信新技術(shù)。它不需要使用傳統(tǒng)通信體制中的載波，而是通過發(fā)送和接收具有納秒或納秒級以下的極窄脈沖來傳輸數(shù)據(jù)，從而具有GHz量級的帶寬。UWB可用于室內(nèi)精確定位，例如戰(zhàn)場士兵的位置發(fā)現(xiàn)、機器人運動跟蹤等，UWB系統(tǒng)與傳統(tǒng)的窄帶系統(tǒng)相比，具有穿透力強、功耗低、抗多徑效果好、安全性高、系統(tǒng)復雜度低、能提供精確定位精度等優(yōu)點。慣導系統(tǒng)(InertialNavigationSystem，INS)是以陀螺和加速度計為敏感器件的導航參數(shù)解算系統(tǒng)，該系統(tǒng)根據(jù)陀螺的輸出建立導航坐標系，根據(jù)加速度計輸出解算出運載體在導航坐標系中的速度和位置，屬于一種推算導航方式。其中捷聯(lián)式慣導系統(tǒng)(Strap-downInertialNavigationSystem，SINS)是將加速度計和陀螺儀直接安裝在載體上，然后根據(jù)導航參數(shù)進行相關導航計算，由于其具有可靠性高、功能強、重量輕、成本低、精度高以及使用靈活等優(yōu)點，使得SINS已經(jīng)成為當今慣導系統(tǒng)發(fā)展的主流。現(xiàn)有技術(shù)中有獨立使用慣導定位的系統(tǒng)和慣導協(xié)同UWB共同實現(xiàn)定位的系統(tǒng)。在獨立使用慣導定位應用中，慣導初始化時所選原點與方向盡可能與實際環(huán)境相匹配，多個慣導同時工作時的初始化原點和方向必須相同，確保指揮中心能準確的掌握多個慣導的位置，做出正確判斷和合理的指揮。在協(xié)同UWB技術(shù)的定位應用中，在移動目標上安裝捷聯(lián)慣導模塊、超寬帶無線接收器和協(xié)同定位模塊，在定位區(qū)域內(nèi)布置超寬帶無線發(fā)射器，利用捷聯(lián)慣導模塊和超寬帶無線傳感器對移動目標的參數(shù)進行感知，建立捷聯(lián)慣導和超寬帶無線耦合模型，由捷聯(lián)慣導模塊輸出移動目標高精度姿態(tài)信息，在捷聯(lián)慣導模塊輸出的運動參數(shù)補償下，超寬帶無線傳感器輸出移動目標高精度的位置信息，并對捷聯(lián)慣導模塊輸出的位置信息進行校正，從而實現(xiàn)封閉環(huán)境下定位區(qū)域內(nèi)移動目標輸出位置和姿態(tài)信息。但是，現(xiàn)有慣導協(xié)同UWB協(xié)同定位技術(shù)，主要是解決在沒有地面或空中常用的GPS校準的封閉空間中，慣導傳感器漂移和累計誤差造成的長時間后位置信息嚴重失信的問題，沒有涉及慣導的初始化原點及方向問題，且必須事先在封閉空間中安裝多個位置已知且固定的無線傳感器及其它配套設施，無法適應室外的情況，靈活性及移動性差。因此，對慣導協(xié)同UWB定位中的慣導原點的初始化問題進行研究具有現(xiàn)實的意義。技術(shù)實現(xiàn)要素：本發(fā)明要解決的技術(shù)問題就在于：針對現(xiàn)有技術(shù)存在的技術(shù)問題，本發(fā)明提供一種可任意選擇慣導初始化狀態(tài)，慣導初始化靈活、方便，在有多個慣導設備時，不需要保持多個慣導設備初始化時的一致性，可方便的適用于室內(nèi)、室外各種環(huán)境，簡單實用的基于UWB測距的慣導原點定位方法及系統(tǒng)。為解決上述技術(shù)問題，本發(fā)明提出的技術(shù)方案為：一種基于UWB測距的慣導原點定位方法，通過UWB定位用戶節(jié)點和慣導定位設備對同一目標進行定位，包括如下步驟：S1.布設UWB定位錨節(jié)點，并確定UWB定位坐標系和UWB定位錨節(jié)點的坐標，設置慣導定位設備，確定慣導定位坐標系；S2.獲取UWB定位用戶節(jié)點的定位數(shù)據(jù)和慣導定位設備的定位數(shù)據(jù)；S3.通過所述UWB定位用戶節(jié)點的定位數(shù)據(jù)和慣導定位設備的定位數(shù)據(jù)解算慣導定位坐標系的原點參數(shù)。作為本發(fā)明的進一步改進，所述步驟S1中UWB定位錨節(jié)點的個數(shù)至少為2個；所述UWB定位錨節(jié)點優(yōu)選布置在同一水平面上；確定所述UWB定位錨節(jié)點在UWB定位坐標系中的坐標；所述慣導定位坐標系為所述慣導設備初始化時所確定的坐標系。作為本發(fā)明的進一步改進，所述步驟S2中所述UWB定位用戶節(jié)點的定位數(shù)據(jù)包括UWB定位用戶節(jié)點的UWB定位坐標；所述UWB定位坐標至少包括2組，包括第一定位時刻獲取的UWB定位坐標和第二定位時刻獲取的UWB定位坐標；所述慣導定位設備的定位數(shù)據(jù)包括慣導定位設備的慣導定位坐標；所述慣導定位坐標至少包括2組，包括第一定位時刻獲取的慣導定位坐標和第二定位時刻獲取的慣導定位坐標。作為本發(fā)明的進一步改進，所述UWB定位坐標由UWB定位用戶節(jié)點通過TOF測距得到所述UWB定位用戶節(jié)點與所述UWB定位錨節(jié)點之間的距離，并根據(jù)所述距離與所述UWB定位錨節(jié)點的坐標計算得到。作為本發(fā)明的進一步改進，所述步驟S3的具體步驟包括：S3.1.通過所述第一定位時刻的UWB定位坐標、第二定位時刻的UWB定位坐標、第一定位時刻的慣導定位坐標、第二定位時刻的慣導定位坐標計算所述UWB定位坐標系與所述慣導定位坐標系之間的夾角；S3.2.根據(jù)所述UWB定位坐標系與所述慣導定位坐標系之間的夾角、UWB定位坐標和慣導定位坐標計算所述慣導定位坐標系的原點相對于所述UWB定位坐標系的原點之間的相對平移，從而確定所述慣導定位坐標系的原點。一種基于UWB測距的慣導原點定位系統(tǒng)，包括UWB定位模塊、慣導定位模塊、融合控制模塊和中心處理模塊；所述UWB定位模塊包括UWB定位錨節(jié)點和UWB定位用戶節(jié)點；所述UWB定位錨節(jié)點用于確定UWB定位坐標系；所述UWB定位用戶節(jié)點用于獲取UWB定位信息，并發(fā)送至所述中心處理模塊；所述慣導定位模塊用于獲取慣導定位信息，并發(fā)送至所述中心處理模塊；所述融合控制模塊用于控制所述UWB定位用戶節(jié)點和所述慣導定位模塊獲取同一定位時刻的UWB定位信息和慣導定位信息；所述中心處理模塊用于設置UWB定位坐標系信息及UWB定位錨節(jié)點的坐標，并根據(jù)所述UWB定位錨節(jié)點的坐標、所述UWB定位信息和所述慣導定位信息解算獲得所述慣導定位模塊的原點。作為本發(fā)明的進一步改進，所述UWB定位模塊包括至少2個UWB定位錨節(jié)點；所述UWB定位錨節(jié)點優(yōu)選布置在同一水平面上；所述2個UWB定位錨節(jié)點優(yōu)選設置在具有已知長度的支架上。作為本發(fā)明的進一步改進，所述融合控制模塊還用于對所述UWB定位信息和慣導定位信息進行融合處理，所述UWB定位用戶節(jié)點將所述UWB定位信息發(fā)送至所述融合控制模塊，所述慣導定位模塊將所述慣導定位信息發(fā)送至所述融合控制模塊，所述融合控制模塊對所述UWB定位信息和所述慣導定位信息進行融合打包后發(fā)送至所述中心處理模塊。作為本發(fā)明的進一步改進，所述中心處理模塊還包括數(shù)據(jù)存儲單元，用于存儲所接收到的UWB定位用戶節(jié)點的UWB定位信息，以及慣導定位設備的慣導定位信息。作為本發(fā)明的進一步改進，所述UWB定位用戶節(jié)點、慣導定位模塊、融合控制模塊、中心處理模塊之間通過無線通信方式進行數(shù)據(jù)通信交互，所述無線方式包括藍牙、Wifi、移動通信等方式。與現(xiàn)有技術(shù)相比，本發(fā)明的優(yōu)點在于：1、本發(fā)明在慣導設備初始化時，可任意選擇慣導設備的原點位置和方向，慣導設備初始化難度低，系統(tǒng)靈活度高；當同時包含有多個慣導設備時，無需要保持多個慣導設備在初始化時的一致性，從而避免初始化偏差，消除因突發(fā)狀況對整個定位系統(tǒng)產(chǎn)生的不利影響，降低系統(tǒng)的復雜度。2、本發(fā)明的UWB定位錨節(jié)點可靈活布設，可移動、操作簡單，可方便的適用于室內(nèi)、室外各種環(huán)境，從而快速建立UWB定位與慣導定位協(xié)同的定位系統(tǒng)。附圖說明圖1為本發(fā)明具體實施例流程示意圖。圖2為本發(fā)明具體實施例結(jié)構(gòu)示意圖。圖3為本發(fā)明具體實施例布設情況府視示意圖。具體實施方式以下結(jié)合說明書附圖和具體優(yōu)選的實施例對本發(fā)明作進一步描述，但并不因此而限制本發(fā)明的保護范圍。如圖1所示，本實施例基于UWB測距的慣導原點定位方法，通過UWB定位用戶節(jié)點和慣導定位設備對同一目標進行定位，包括如下步驟：S1.布設UWB定位錨節(jié)點，并確定UWB定位坐標系和UWB定位錨節(jié)點的坐標，設置慣導定位設備，確定慣導定位坐標系；S2.獲取UWB定位用戶節(jié)點的定位數(shù)據(jù)和慣導定位設備的定位數(shù)據(jù)；S3.通過UWB定位用戶節(jié)點的定位數(shù)據(jù)和慣導定位設備的定位數(shù)據(jù)解算慣導定位坐標系的原點參數(shù)。在本實施例中，步驟S1中UWB定位錨節(jié)點的個數(shù)至少為2個；UWB定位錨節(jié)點優(yōu)選布置在同一水平面上；確定UWB定位錨節(jié)點在UWB定位坐標系中的坐標；慣導定位坐標系為慣導設備初始化時所確定的坐標系。如圖3所示，在本實施例中，包括2個UWB定位錨節(jié)點M1和M2，UWB定位錨節(jié)點M1和M2布置在同一水平面上，以M1與M2的中點為UWB定位坐標系的原點O1，以M1與M2的連接線所在的水平線為X軸，以垂直與X軸的水平線為Y軸，X軸與Y軸所在的平面為水平平面，與地面平行。設本實施例中M1與M2之間的距離為2米，則可設定UWB定位錨節(jié)點M1的坐標為(-1，0)，UWB定位錨節(jié)點M2的坐標為(1，0)。當然，當具有多個UWB定位錨節(jié)點時，可以選擇以UWB定位錨節(jié)點的幾何中心點為UWB定位坐標系的原點。在本實施例中，UWB定位用戶節(jié)點和慣導定位設備將加載在同一載體上，如圖3中A，慣導定位坐標系為慣導定位設備初始化時所確定的坐標系，在本實施例中，該坐標系即載體坐標系，該載體坐標系以初始化時載體的質(zhì)心為載體坐標系的原點，載體坐標系的X軸為載體的水平右側(cè)，Y軸為與X軸垂直，水平向前，Z軸豎直向上，其中，載體坐標系中的X軸與Y軸所在的平面為與地面平等的水平平面。在本實施例中，慣導定位坐標系的X軸方向如圖3中所示的X′，Y軸方向如圖3中所示的Y′。當然，UWB定位坐標系和慣導定位坐標系并不僅限于本實施例中所記載的方法，可根據(jù)實際需要靈活設置。在本實施例中，步驟S2中UWB定位用戶節(jié)點的定位數(shù)據(jù)包括UWB定位用戶節(jié)點的UWB定位坐標；UWB定位坐標至少包括2組，包括第一定位時刻獲取的UWB定位坐標和第二定位時刻獲取的UWB定位坐標；慣導定位設備的定位數(shù)據(jù)包括慣導定位設備的慣導定位坐標；慣導定位坐標至少包括2組，包括第一定位時刻獲取的慣導定位坐標和第二定位時刻獲取的慣導定位坐標。UWB定位坐標由UWB定位用戶節(jié)點通過TOF測距得到UWB定位用戶節(jié)點與UWB定位錨節(jié)點之間的距離，并根據(jù)距離與UWB定位錨節(jié)點的坐標計算得到。在本實施例中，通過式(1)所示公式即可計算UWB定位用戶節(jié)點在UWB定位坐標系中的UWB定位坐標，(xT-x1)2+(yT-y1)2=d1(xT-x2)2+(yT-y2)2=d2---(1)]]>式(1)中，(xT,yT)為UWB定位坐標，(x1,y1)為UWB定位錨節(jié)點M1的坐標，(x2,y2)為UWB定位錨節(jié)點M1的坐標，d1為UWB定位用戶節(jié)點與UWB定位錨節(jié)點M1之間的距離，d2為UWB定位用戶節(jié)點與所述第二UWB定位錨節(jié)點M2之間的距離。TOF是基于信號飛行時間的測距算法，在本實施例中，采用兩次雙邊測距求信號飛行時間平均值的方式進行距離計算，用于減少由于UWB定位用戶節(jié)點和UWB定位錨節(jié)點的晶振不同引起的誤差。在本實施例中，步驟S3的具體步驟包括：S3.1.通過第一定位時刻的UWB定位坐標、第二定位時刻的UWB定位坐標、第一定位時刻的慣導定位坐標、第二定位時刻的慣導定位坐標計算UWB定位坐標系與慣導定位坐標系之間的夾角；S3.2.根據(jù)UWB定位坐標系與慣導定位坐標系之間的夾角、UWB定位坐標和慣導定位坐標計算慣導定位坐標系的原點相對于UWB定位坐標系的原點之間的相對平移，從而確定慣導定位坐標系的原點。如圖3所示，在本實施例中，根據(jù)式(1)所示公式，可計算得到在第一定位時刻的UWB定位坐標A(xT1,yT1)和在第二定位時刻的UWB定位坐標A′(xT2,yT2)。同時，通過慣導定位設備可獲得在第一定位時刻的慣導定位坐標A(xgd1,ygd1)和在第二定位時刻的慣導定位坐標A′(xgd2,ygd2)。設慣導定位坐標系的原點相對于UWB定位坐標系的原點的相對平移位移為(Δx,Δy)，Δx為X軸平移量，Δy為Y軸平移量。則在第一定位時刻UWB定位坐標與慣導定位坐標之間的關系可通過式(2)所示公式表示，xT1=Δx+ygd1sinθ+xgd1cosθyT1=Δy+ygd1cosθ-xgd1sinθ---(2)]]>式(2)中，θ為所述UWB定位坐標系與慣導定位坐標系之間的夾角，(xT1,yT1)為第一定位時刻的UWB定位坐標，(xgd1,ygd1)為第一定位時刻的慣導定位坐標，(Δx,Δy)為慣導定位坐標系的原點相對于UWB定位坐標系的原點的相對平移位移。在第二定位時刻UWB定位坐標與慣導定位坐標之間的關系可通過式(3)所示公式表示，xT2=Δx+ygd2sinθ+xgd2cosθyT2=Δy+ygd2cosθ-xgd2sinθ---(3)]]>式(3)中，θ為所述UWB定位坐標系與慣導定位坐標系之間的夾角，(xT2,yT2)為第二定位時刻的UWB定位坐標，(xgd2,ygd2)為第二定位時刻的慣導定位坐標，(Δx,Δy)為慣導定位坐標系的原點相對于UWB定位坐標系的原點的相對平移位移。根據(jù)式(2)和式(3)可以得到式(4)，(xT1-xT2)=(ygd1-ygd2)sinθ+(xgd1-xgd2)cosθ(yT1-yT2)=(ygd1-ygd2)cosθ-(xgd1-xgd2)sinθ---(4)]]>式(4)中，θ為所述UWB定位坐標系與慣導定位坐標系之間的夾角，(xT1,yT1)為第一定位時刻的UWB定位坐標，(xgd1,ygd1)為第一定位時刻的慣導定位坐標，(xT2,yT2)為第二定位時刻的UWB定位坐標，(xgd2,ygd2)為第二定位時刻的慣導定位坐標。由式(4)可得式(5)，cosθ=(xT1-xT2ygd1-ygd2+yT1-yT2xgd1-xgd2)/(ygd1-ygd2xgd1-xgd2+xgd1-xgd2ygd1-ygd2)---(5)]]>式(5)中對各參數(shù)的定義與式(4)中相同。通過式(5)則可以解算得出UWB定位坐標系與慣導定位坐標系之間的夾角θ的值。再將UWB定位坐標系與慣導定位坐標系之間的夾角θ的值代入到式(2)或者式(3)中，即可計算得到慣導定位坐標系的原點相對于UWB定位坐標系的原點的相對平移位移中Δx和Δy的值，從而即可以確定慣導定位坐標系的原點。如圖2所示，本實施例基于UWB測距的慣導原點定位系統(tǒng)，包括UWB定位模塊、慣導定位模塊、融合控制模塊和中心處理模塊；UWB定位模塊包括UWB定位錨節(jié)點和UWB定位用戶節(jié)點；UWB定位錨節(jié)點用于確定UWB定位坐標系；UWB定位用戶節(jié)點用于獲取UWB定位信息，并發(fā)送至中心處理模塊；慣導定位模塊用于獲取慣導定位信息，并發(fā)送至中心處理模塊；融合控制模塊用于控制UWB定位用戶節(jié)點和慣導定位模塊獲取同一定位時刻的UWB定位信息和慣導定位信息；中心處理模塊用于設置UWB定位坐標系信息及UWB定位錨節(jié)點的坐標，并根據(jù)UWB定位錨節(jié)點的坐標、UWB定位信息和慣導定位信息解算獲得慣導定位模塊的原點。在本實施例中，如圖3所示，UWB定位模塊包括至少2個UWB定位錨節(jié)點；UWB定位錨節(jié)點優(yōu)選布置在同一水平面上；2個UWB定位錨節(jié)點優(yōu)選設置在具有已知長度的支架上。在本實施例中，UWB定位錨節(jié)點可以根據(jù)需要進行布置，為了更快的確定UWB定位坐標系的原點，以及UWB定位錨節(jié)點的坐標，優(yōu)選將UWB定位錨節(jié)點布置在預設設計好的具有已知長度的支架上。本實施例所選用的支架長度為2米，將2個UWB定位錨節(jié)點分別布置在支架的兩端，在布設時，只需要將該支架按照需要布設的方位水平布設，根據(jù)需要設定支架的中心UWB定位坐標系的原點，那么2個UWB定位錨節(jié)點的坐標分別為(-1，0)和(1，0)。當然，為了更方便的實現(xiàn)水平布設，還可以在支架上設置一個水平指示裝置。同時，該支架可設置為可升縮或可折疊的支架，以更方便的攜帶?；蛘撸谥Ъ苌显O置距離刻度表，在UWB定位錨節(jié)點的安裝時，可根據(jù)需要方便的設置UWB定位錨節(jié)點之間的距離。在UWB定位錨節(jié)點布設完成后，將UWB定位錨節(jié)點的坐標信息輸入至中心處理模塊，確定UWB定位坐標系。在本實施例中，融合控制模塊還用于對UWB定位信息和慣導定位信息進行融合處理，UWB定位用戶節(jié)點將UWB定位信息發(fā)送至融合控制模塊，慣導定位模塊將慣導定位信息發(fā)送至融合控制模塊，融合控制模塊對UWB定位信息和慣導定位信息進行融合打包后發(fā)送至中心處理模塊。中心處理模塊還包括數(shù)據(jù)存儲單元，用于存儲所接收到的UWB定位用戶節(jié)點的UWB定位信息，以及慣導定位設備的慣導定位信息。UWB定位用戶節(jié)點、慣導定位模塊、融合控制模塊、中心處理模塊之間通過無線通信方式進行數(shù)據(jù)通信交互，無線方式包括藍牙、Wifi、移動通信等方式。移動通信方式包括3G、4G等移動通信。在本實施例中，如圖3所示，M1和M2是布設在支架上的2個UWB定位錨節(jié)點，A為UWB定位用戶節(jié)點和慣導定位設備的載體，即被定位的人或物，其中A為載體在第一定位時刻時的位置，A’為載體在第二定位時刻時的位置。通過融合控制模塊，分別在第一定位時刻獲取載體的UWB定位坐標(xT1,yT1)和慣導定位坐標(xgd1,ygd1)，在第二定位時刻獲取載體的UWB定位坐標(xT2,yT2)和慣導定位坐標(xgd2,ygd2)。其中，UWB定位坐標是UWB定位用戶節(jié)點接收UWB定位錨節(jié)點發(fā)送的UWB信號，采用TOF算法測距由式(1)所示公式計算得到的UWB定位坐標，在第一定位時刻UWB定位用戶節(jié)點距兩個UWB定位錨節(jié)點的距離分別為d1和d2，在第二定位時刻UWB定位用戶節(jié)點距兩個UWB定位錨節(jié)點的距離分別為d1′和d2′。UWB定位用戶節(jié)點將UWB定位坐標發(fā)送至融合控制模塊，慣導定位設備將慣導定位坐標發(fā)送至融合控制模塊，由融合控制模塊進行融合打包后，發(fā)送至中心處理模塊。當然，也可以采用其它的方式，如由UWB定位用戶節(jié)點直接將UWB定位坐標發(fā)送至中心處理模塊，慣導定位設備直接將慣導定位坐標發(fā)送至中心處理模塊來實現(xiàn)定位坐標數(shù)據(jù)的發(fā)送。中心處理模塊接收獲得UWB定位坐標和慣導定位坐標，并根據(jù)所輸入的UWB定位坐標系和UWB定位錨節(jié)點坐標，通過式(2)或式(3)，以及式(5)所示的公式，計算慣導定位坐標系的原點相對于UWB定位坐標系的原點的相對平移位移，從而確定慣導定位坐標系的原點。同時，中心處理模塊還可以將所接收到的數(shù)據(jù)以及計算得到的數(shù)據(jù)存儲至數(shù)據(jù)存儲單元。在本實施例中，融合控制模塊可以是智能手機，中心處理模塊可以是智能手機、平板電腦、筆記本電腦等。上述只是本發(fā)明的較佳實施例，并非對本發(fā)明作任何形式上的限制。雖然本發(fā)明已以較佳實施例揭露如上，然而并非用以限定本發(fā)明。因此，凡是未脫離本發(fā)明技術(shù)方案的內(nèi)容，依據(jù)本發(fā)明技術(shù)實質(zhì)對以上實施例所做的任何簡單修改、等同變化及修飾，均應落在本發(fā)明技術(shù)方案保護的范圍內(nèi)。當前第1頁1 2 3