本發(fā)明屬于復(fù)雜環(huán)境下組合定位技術(shù)領(lǐng)域，具體涉及一種面向室內(nèi)移動機器人的IMU/WSN組合導(dǎo)航方法。

背景技術(shù)：
近年來，隨著計算機技術(shù)、信息技術(shù)、通訊技術(shù)、微電子技術(shù)的飛速發(fā)展，面向小區(qū)域的目標(biāo)跟蹤技術(shù)的研究與應(yīng)用，逐漸成為目前該領(lǐng)域的研究熱點。然而，在外界無線電信號微弱、電磁干擾強烈等一系列復(fù)雜室內(nèi)環(huán)境中，對目標(biāo)載體導(dǎo)航信息獲取的準(zhǔn)確性、實時性及魯棒性有很大的影響。如何將室內(nèi)環(huán)境下獲取的有限信息進(jìn)行有效的融合以滿足小區(qū)域目標(biāo)高導(dǎo)航精度的要求，消除外界環(huán)境的影響，具有重要的科學(xué)理論意義和實際應(yīng)用價值。在現(xiàn)有的定位方式中，全球衛(wèi)星導(dǎo)航系統(tǒng)(GlobalNavigationSatelliteSystem,GNSS)是最為常用的一種方式。雖然GNSS能夠通過精度持續(xù)穩(wěn)定的位置信息，但是其易受電磁干擾、遮擋等外界環(huán)境影響的缺點限制了其應(yīng)用范圍，特別是在室內(nèi)、地下巷道等一些密閉的、環(huán)境復(fù)雜的場景，GNSS信號被嚴(yán)重遮擋，無法進(jìn)行有效的工作。近年來，WSN以其低成本、低功耗和低系統(tǒng)復(fù)雜度的特點在短距離局部定位領(lǐng)域表現(xiàn)出很大的潛力。伴隨著全國范圍內(nèi)無線網(wǎng)絡(luò)的普及和使用，學(xué)者們提出將基于WSN的目標(biāo)跟蹤應(yīng)用于GNSS失效環(huán)境下的行人導(dǎo)航。這種方式雖然能夠?qū)崿F(xiàn)室內(nèi)定位，但是由于室內(nèi)環(huán)境復(fù)雜多變，WSN信號十分容易受到干擾而導(dǎo)致定位精度下降甚至失鎖；除此之外，由于WSN采用的通信技術(shù)通常為短距離無線通信技術(shù)，因此若想完成大范圍的室內(nèi)目標(biāo)跟蹤定位，需要大量的網(wǎng)絡(luò)節(jié)點共同完成，這必將引入網(wǎng)絡(luò)組織結(jié)構(gòu)優(yōu)化設(shè)計、多節(jié)點多簇網(wǎng)絡(luò)協(xié)同通信等一系列問題。因此現(xiàn)階段基于WSN的目標(biāo)跟蹤在室內(nèi)導(dǎo)航領(lǐng)域仍舊面臨很多挑戰(zhàn)。為了克服上述兩種導(dǎo)航方法需要參考節(jié)點并容易產(chǎn)生失鎖的缺點，有學(xué)者提出將慣性測量單元(Inertialmeasurementunit,IMU)應(yīng)用于小區(qū)域目標(biāo)跟蹤領(lǐng)域。IMU具有全自主、運動信息全面、短時、高精度的優(yōu)點，雖然可以實現(xiàn)自主導(dǎo)航，但誤差隨時間積累，長航時運行條件下將會導(dǎo)致導(dǎo)航精度嚴(yán)重下降。

技術(shù)實現(xiàn)要素：
本發(fā)明的目的就是為了解決上述問題，提出了面向室內(nèi)移動機器人的IMU/WSN組合導(dǎo)航方法，該方法能夠有效的提高室內(nèi)移動機器人的導(dǎo)航精度，可用于室內(nèi)，地下礦井等密閉復(fù)雜環(huán)境下的長距離高精度目標(biāo)定位跟蹤。為了實現(xiàn)上述目的，本發(fā)明采用如下技術(shù)方案：一種面向室內(nèi)移動機器人的IMU/WSN組合導(dǎo)航方法，包括以下步驟：(1)選取導(dǎo)航區(qū)域內(nèi)任意一個參考節(jié)點的位置作為坐標(biāo)原點，并分別選取x向和y向構(gòu)建相對坐標(biāo)系；(2)在WSN系統(tǒng)的每個無線通信通道中加入第一局部濾波器，通過第一局部濾波器分別得到當(dāng)前時刻該無線通信通道的未知節(jié)點與參考節(jié)點之間的距離平方和距離平方變化率的最優(yōu)估計；(3)根據(jù)慣性測量單元IMU采集得到的傳感器數(shù)據(jù)判斷移動機器人的運動狀態(tài)，當(dāng)移動機器人處于靜止?fàn)顟B(tài)時，利用第二局部濾波器對IMU的解算誤差進(jìn)行補償，得到當(dāng)前時刻IMU解算得到的未知節(jié)點與參考節(jié)點之間的距離平方和距離平方變化率的最優(yōu)估計；(4)分別以步驟(2)和步驟(3)得到的當(dāng)前時刻未知節(jié)點與參考節(jié)點之間的距離平方之差以及距離平方變化率之差作為觀測量，構(gòu)建主濾波器；(5)將慣性測量單元IMU采集到的當(dāng)前時刻的未知節(jié)點的位置和速度與主濾波器輸出的慣性測量單元IMU的解算誤差做差，最終得到當(dāng)前時刻的未知節(jié)點的位置、速度和姿態(tài)的最優(yōu)估計。所述步驟(2)中的第一濾波器的狀態(tài)方程具體為：其中，為WSN系統(tǒng)的第i個無線通道在k時刻的距離未知節(jié)點的距離，為WSN系統(tǒng)的第i個無線通道在k時刻的距離未知節(jié)點的距離變化率，T為第一濾波器的采樣周期，W為第一濾波器的狀態(tài)噪聲矩陣。所述步驟(2)中的第一濾波器的觀測方程具體為：其中，為WSN系統(tǒng)實際測量得到的第i個無線通道在k時刻的距離未知節(jié)點的距離，為WSN系統(tǒng)實際測量得到的第i個無線通道在k時刻的距離未知節(jié)點的距離變化率，vloc1為濾波器的觀測噪聲矩陣。所述步驟(3)中根據(jù)慣性測量單元IMU采集得到的傳感器數(shù)據(jù)判斷移動機器人的運動狀態(tài)的具體方法為：通過將慣性測量單元IMU的加速度傳感器采集到的數(shù)據(jù)計算當(dāng)前移動機器人的加速度模值：通過加速度模值完成對移動機器人運動狀態(tài)的判斷，即：其中，ax、ay、az分別為慣性測量單元IMU所搭載的加速度傳感器在載體坐標(biāo)系下采集到的x、y和z三個方向的加速度值。所述步驟(3)中第二濾波器的狀態(tài)方程具體為：其中，δVn為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的速度誤差；為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的速度誤差變化率；φ＝[φEφNφU]為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的姿態(tài)誤差；為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的姿態(tài)誤差變化率；δPn為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的位置誤差；為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的位置誤差變化率；▽n為理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的加速度誤差；為理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的加速度誤差變化率；εn為理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的角速度誤差；為理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的角速度誤差變化率；為加速度計測量得到的載體坐標(biāo)下的x、y和z軸三個方向的加速度值；為載體坐標(biāo)系到實際導(dǎo)航坐標(biāo)系下的狀態(tài)轉(zhuǎn)移矩陣。所述步驟(3)中第二濾波器的觀測方程具體為：其中，為當(dāng)前時刻觀測到的慣性測量單元IMU解算的速度誤差，δVn為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的x、y和z軸三個方向的速度誤差，vloc2為濾波器的觀測噪聲矩陣。所述步驟(4)中主濾波器的狀態(tài)方程具體為：其中，為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的速度誤差；δVn為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的速度誤差變化率；φ＝[φEφNφU]為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的姿態(tài)誤差；為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的姿態(tài)誤差變化率；δPn為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的位置誤差；為慣性測量單元IMU測量得到的理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的位置誤差變化率；▽n為理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的加速度誤差；為理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的加速度誤差變化率；εn為理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的角速度誤差；為理想導(dǎo)航坐標(biāo)系下的東向、北向和天向三個方向的角速度誤差變化率；為加速度計測量得到的載體坐標(biāo)下的x、y和z軸三個方向的加速度值；為載體坐標(biāo)系到實際導(dǎo)航坐標(biāo)系下的狀態(tài)轉(zhuǎn)移矩陣。所述步驟(4)中主濾波器的觀測方程具體為：其中，為k時刻低成本IMU解算的未知節(jié)點的x、y和z向的位置，(xi,yi,zi)為第i個參考節(jié)點的x、y和z向的位置；(δx,δy,δz)為k時刻低成本IMU解算的位置誤差，即向量δPn的標(biāo)量元素；(δvx,δvy,δvz)為k時刻低成本IMU解算的速度誤差，即向量δVn的標(biāo)量元素；為k時刻低成本IMU和WSN分別測量得到的未知節(jié)點與參考節(jié)點之間的距離平方的差；為k時刻低成本IMU和WSN分別測量得到的未知節(jié)點與參考節(jié)點之間的距離平方變化率的差；v為濾波器的觀測噪聲矩陣。所述步驟(4)中主濾波器采用15維狀態(tài)變量來對移動機器人的位置、速度和姿態(tài)進(jìn)行描述，所述15維狀態(tài)變量具體包括：在x、y、z三個方向的姿態(tài)角誤差、位置誤差、速度誤差、加速度誤差和角速度誤差。本發(fā)明的有益效果是：1、本發(fā)明方法在WSN和低成本IMU中都采用了局部濾波器，有效的提高了傳感器所采集數(shù)據(jù)的精度，有助于后續(xù)主濾波器進(jìn)行數(shù)據(jù)融合過程中精度的提升。2、主濾波器采用了姿態(tài)角誤差、位置誤差、速度誤差、加速度誤差和角速度誤差作為狀態(tài)變量，與傳統(tǒng)移動機器人定位領(lǐng)域的位置、速度和加速度誤差相比，對系統(tǒng)的運行狀態(tài)描述更加精確，有助于提高數(shù)據(jù)融合的精度。3、在主濾波器中以INS的測量值與其誤差的差值替代參數(shù)的真實值，減少了傳統(tǒng)緊組合方法由于泰勒展開后忽略二次項對定位精度造成的影響。4、該緊組合方法中將距離平方變化率作為數(shù)據(jù)濾波器的觀測向量，無需新添設(shè)備就可以完成對目標(biāo)節(jié)點速度誤差的預(yù)估，進(jìn)而完成對目標(biāo)節(jié)點的速度預(yù)估。5、為了提高主濾波器的定位精度，在WSN的每一個通信信道與低成本IMU的解算過程中都加入了局部濾波器，更好的提高了傳感器所采集數(shù)據(jù)的精度。6、本發(fā)明使用MPU6050作為導(dǎo)航系統(tǒng)的慣性導(dǎo)器件，降低組合導(dǎo)航系統(tǒng)的成本。7、可用于室內(nèi)，地下礦井等密閉復(fù)雜環(huán)境下的高精度目標(biāo)定位跟蹤。附圖說明圖1為面向室內(nèi)移動機器人的低成本IMU/WSN組合導(dǎo)航方法的系統(tǒng)示意圖；圖2為面向室內(nèi)移動機器人的低成本IMU/WSN組合導(dǎo)航方法的示意圖；圖3為本發(fā)明的方法流程圖。具體實施方式：下面結(jié)合附圖與實施例對本發(fā)明做進(jìn)一步說明：單一的導(dǎo)航方式容易受到目標(biāo)周圍導(dǎo)航環(huán)境的影響，不能提供持續(xù)穩(wěn)定的高精度導(dǎo)航信息,為了克服單一導(dǎo)航技術(shù)存在的不同程度的缺陷，本發(fā)明提出一種面向室內(nèi)移動機器人的低成本IMU/WSN組合導(dǎo)航方法。面向室內(nèi)移動機器人的低成本IMU/WSN組合導(dǎo)航系統(tǒng)，如圖1所示，包括參考節(jié)點部分和未知節(jié)點部分。參考節(jié)點部分由多個無線接收模塊組成，主要負(fù)責(zé)接收未知節(jié)點所發(fā)出的距離測量命令并測量未知節(jié)點與參考節(jié)點之間的距離；未知節(jié)點部分由無線發(fā)送模塊、無線路由模塊、IMU模塊和控制模塊組成，其中無線發(fā)送模塊主要負(fù)責(zé)無線測距命令的發(fā)送；無線路由模塊是整個WSN與外界通信的接口；主要負(fù)責(zé)與控制模塊進(jìn)行數(shù)據(jù)通信；IMU模塊主要負(fù)責(zé)通過自身的傳感器來解算當(dāng)前時刻的載體的位置、速度和姿態(tài)等導(dǎo)航信息；控制模塊主要負(fù)責(zé)控制整個導(dǎo)航系統(tǒng)的其他模塊，以及與外界進(jìn)行數(shù)據(jù)通信。面向室內(nèi)移動機器人的低成本IMU/WSN組合導(dǎo)航方法的示意圖如圖2所示，以一個參考節(jié)點的位置作為坐標(biāo)原點，并選取相對坐標(biāo)系中的x向、y向和z向。在WSN系統(tǒng)的無線通信通道中與低成本IMU中利用局部濾波器分別對未知節(jié)點與參考節(jié)點之間的距離平方和距離平方變化率和低成本IMU靜止?fàn)顟B(tài)下的解算誤差進(jìn)行預(yù)估。在此基礎(chǔ)上，以每一時刻低成本IMU和WSN各自測量的未知節(jié)點與參考節(jié)點之間的距離平方之差和距離平方變化率之差作為觀測量，構(gòu)建采用15維狀態(tài)向量的主濾波器，將低成本IMU采集到的當(dāng)前時刻的未知節(jié)點的位置和速度與主濾波器輸出的低成本IMU解算誤差做差，最終得到當(dāng)前時刻的未知節(jié)點的位置、速度和姿態(tài)的最優(yōu)估計。本發(fā)明方法的具體流程圖如圖3所示，包括以下步驟：(1)選取任意一個參考節(jié)點的位置作為坐標(biāo)原點，并選取相對坐標(biāo)系中的x向和y向；(2)在WSN系統(tǒng)的每個無線通信通道中加入了局部濾波器，通過局部濾波器得到當(dāng)前時刻該無線通信通道的未知節(jié)點與參考節(jié)點之間的距離平方和距離平方變化率的最優(yōu)估計，局部濾波器的狀態(tài)方程具體為：其中，為WSN系統(tǒng)的第i個無線通道在k時刻的距離未知節(jié)點的距離，為WSN系統(tǒng)的第i個無線通道在k時刻的距離未知節(jié)點的距離變化率，T為濾波器的采樣周期，W為濾波器的狀態(tài)噪聲矩陣；局部濾波器的觀測方程為：其中，為WSN系統(tǒng)實際測量得到的第i個無線通道在k時刻的距離未知節(jié)點的距離，為WSN系統(tǒng)實際測量得到的第i個無線通道在k時刻的距離未知節(jié)點的距離變化率，vloc1為濾波器的觀測噪聲矩陣。(3)通過將低成本IMU的3個加速度傳感器所采集到的數(shù)據(jù)通過下式進(jìn)行計算，得到當(dāng)前移動機器人的加速度模值：通過加速度模值完成對移動機器人運動狀態(tài)的判斷，即：在完成對移動機器人運動狀態(tài)的判斷之后，當(dāng)移動機器人處于靜止?fàn)顟B(tài)時，所采用的局部濾波器的狀態(tài)方程具體為：其中，δVn為低成本IMU測量得到的理想導(dǎo)航坐標(biāo)系下的x、y和z軸三個方向的速度誤差；φ為低成本IMU測量得到的理想導(dǎo)航坐標(biāo)系下的x、y和z軸三個方向的姿態(tài)誤差；δPn為低成本IMU測量得到的理想導(dǎo)航坐標(biāo)系下的x、y和z軸三個方向的位置誤差；▽n為理想導(dǎo)航坐標(biāo)系下的x、y和z軸三個方向的加速度誤差；εn為理想導(dǎo)航坐標(biāo)系下的x、y和z軸三個方向的角速度誤差；為加速度計測量得到的載體坐標(biāo)下的x、y和z軸三個方向的加速度值；為載體坐標(biāo)系到實際導(dǎo)航坐標(biāo)系下的狀態(tài)轉(zhuǎn)移矩陣；局部濾波器的觀測方程具體為：其中，為當(dāng)前時刻觀測到的低成本IMU解算的速度誤差，由于本局部濾波器工作時，移動機器人處于靜止?fàn)顟B(tài)，因此當(dāng)前時刻的理論值為零，故低成本IMU測量得到的速度均為IMU的解算誤差，vloc2為濾波器的觀測噪聲矩陣。(4)以每一時刻低成本IMU和WSN各自測量的未知節(jié)點與參考節(jié)點之間的距離平方之差和距離平方變化率之差作為觀測量，構(gòu)建采用15維狀態(tài)向量的主濾波器，主濾波器的狀態(tài)方程具體為：對移動機器人的導(dǎo)航信息解算誤差的描述，傳統(tǒng)上普遍采用三個方向的位置、速度和加速度的9維狀態(tài)變量來描述，為了更加準(zhǔn)確的描述移動機器人的運動狀態(tài)和導(dǎo)航解算誤差，本發(fā)明采用15維狀態(tài)變量來對移動機器人的位置、速度和姿態(tài)進(jìn)行描述；主濾波器的觀測方程為：其中，為k時刻低成本IMU解算的未知節(jié)點的x、y和z向的位置，(xi,yi,zi)為第i個參考節(jié)點的x、y和z向的位置；(δx,δy,δz)為k時刻低成本IMU解算的位置誤差(即向量δPn的標(biāo)量元素)；(δvx,δvy,δvz)為k時刻低成本IMU解算的速度誤差(即向量δVn的標(biāo)量元素)；為k時刻低成本IMU和WSN分別測量得到的未知節(jié)點與參考節(jié)點之間的距離平方的差；為k時刻低成本IMU和WSN分別測量得到的未知節(jié)點與參考節(jié)點之間的距離平方變化率的差；v為濾波器的觀測噪聲矩陣。(5)將低成本IMU采集到的當(dāng)前時刻的未知節(jié)點的位置和速度與主濾波器輸出的低成本IMU解算誤差做差，最終得到當(dāng)前時刻的未知節(jié)點的位置、速度和姿態(tài)的最優(yōu)估計。上述雖然結(jié)合附圖對本發(fā)明的具體實施方式進(jìn)行了描述，但并非對本發(fā)明保護(hù)范圍的限制，所屬領(lǐng)域技術(shù)人員應(yīng)該明白，在本發(fā)明的技術(shù)方案的基礎(chǔ)上，本領(lǐng)域技術(shù)人員不需要付出創(chuàng)造性勞動即可做出的各種修改或變形仍在本發(fā)明的保護(hù)范圍以內(nèi)。