專利名稱：用于自動引導(dǎo)車輛的近場低頻無線定位方法
技術(shù)領(lǐng)域：
本發(fā)明屬于自動化技術(shù)領(lǐng)域，特別是涉及一種用于自動引導(dǎo)車輛的近場低頻無線 定位方法。
背景技術(shù)：
自動引導(dǎo)車輛在結(jié)構(gòu)化道路環(huán)境中自動行駛時，路徑的跟蹤是以預(yù)先設(shè)定的固定 引導(dǎo)路徑為基礎(chǔ)來控制運動方向。為了實現(xiàn)在非道路環(huán)境下車輛的自動引導(dǎo)，可以采用無 線射頻定位引導(dǎo)技術(shù)，這一工作由無線定位模塊完成。自動引導(dǎo)車輛的狀態(tài)控制以無線定 位模塊為基礎(chǔ)，根據(jù)模塊的結(jié)果，控制自動引導(dǎo)車輛完成車體的加速、減速、轉(zhuǎn)向和停車等 動作。因此，無線定位模塊的速度和精度是保證自動引導(dǎo)車輛實時、準確和可靠運動的關(guān) 鍵。無線定位技術(shù)有很多種，這里采用基于RSSI (Received Signal Strengthlndicator，接收信號強度指示)的低頻無線定位技術(shù)，它是一種定位精度高、功 耗低、成本低、反應(yīng)速度快的高性價比無線定位技術(shù)。在基于RSSI的低頻定位中，已知低頻 發(fā)射基站的發(fā)射信號強度，待定位模塊(以下稱為引導(dǎo)模塊)根據(jù)實際接收到信號強度計 算出信號的傳播損耗，利用理論和經(jīng)驗?zāi)Ｐ蛯鬏敁p耗轉(zhuǎn)化為距離，再利用一定的算法計 算出引導(dǎo)模塊的位置，位置信息包括方位角和直線距離。無線定位模塊設(shè)計過程中需要克服以下問題1、克服外界環(huán)境變化對定位結(jié)果的 影響；2、克服隨機干擾對定位精度的影響；3、克服天線方向性對定位結(jié)果的影響；4、滿足 系統(tǒng)大負荷的運算。

發(fā)明內(nèi)容
本發(fā)明的目的是針對現(xiàn)有技術(shù)的不足，提出了一種用于自動引導(dǎo)車輛的近場低頻 無線定位方法。本發(fā)明方法的具體步驟是步驟(1)對車載定位模塊的控制器初始化，初始化對象包括控制器中的單片機時 鐘頻率、輸入輸出、內(nèi)部變量、寄存器、非易失性存儲器和中斷；步驟(2)車載定位模塊的控制器從非易失性存儲器中讀取場地環(huán)境對應(yīng)的標定 參數(shù)，所述的標定參數(shù)包括路徑衰減指數(shù)λ、標準偏差為ο的正態(tài)隨機變量ξ。、屯米處引 導(dǎo)模塊收到的RSSI信號強度值RSSI (Cltl) ； λ、ξ。和RSSI (d0)采用最小二乘法根據(jù)場地環(huán) 境擬合得到；步驟(3)車載定位模塊等待來自電機控制模塊的定位啟動命令；如果收到電機控 制模塊的定位啟動命令，則執(zhí)行步驟(4)，如果未收到電機控制模塊的定位啟動命令，則重 復(fù)本步驟；步驟(4)車載定位模塊通過自帶的高頻通信模塊，在433. 05MHz 434. 79MHz頻 段內(nèi)向引導(dǎo)模塊發(fā)送初始化命令，使能弓I導(dǎo)模塊初始化；
引導(dǎo)模塊初始化其內(nèi)部的控制器中的單片機時鐘頻率、輸入輸出、內(nèi)部變量、寄存 器和中斷，使能引導(dǎo)模塊上的三維低頻接收器；步驟(5)車載定位模塊關(guān)閉所有N個低頻發(fā)射基站的低頻發(fā)射功能，使能第η個 低頻發(fā)射基站的低頻發(fā)射功能，發(fā)射頻率恒定為f。，發(fā)射RSSI信號強度值恒定為RSSI (C)， 同時車載定位模塊通過自帶的高頻通信模塊，在433. 05MHz 434. 79MHz頻段內(nèi)向引導(dǎo)模 塊發(fā)送接收命令，其中η為自然數(shù)，且η < N ；引導(dǎo)模塊接收來自第η個低頻發(fā)射基站的低頻無線電信號，得到接收RSSI信號強 度值RSSI (dn)；引導(dǎo)模塊自帶的高頻通信模塊，在433. 05MHz 434. 79MHz頻段內(nèi)向車載定 位模塊發(fā)回RSSI (dn)，車載定位模塊記錄該值并保存于內(nèi)存中；車載定位模塊計算引導(dǎo)模塊距第η個低頻發(fā)射基站的距離dn，
RSSf^-RSS/^J+C,, dn =10磁xd0',步驟(6)延時設(shè)定的時間t，重復(fù)執(zhí)行步驟(5)直到得到N個低頻發(fā)射基站中每個 低頻發(fā)射基站對應(yīng)的距離d” d2、d3. · · (V” dN ；以任意一個低頻發(fā)射基站為原點，建立空間三維坐標系，則屯、d2、d3. . . dH、dN分 別對應(yīng)N個低頻發(fā)射基站三維坐標(X1, Y1, Z1)、(x2, j2, z2)、(x3, y3, z3)…(xN，yN，zN)；步驟(7)在C^dyd3. . . d^,dN中任選3個距離值，分別標記為dml、dm2、dm3，其對應(yīng) 的三個低頻發(fā)射基站三維坐標記為(Xml ，Yml，Zmi)、(Xm2，Υηι2，Zm2)、(Xm3，Υηι3，Zm3)；
(V"^1)2+ (ν - V·/ m-ymx)2~Zml)2 =dL
解方程組<(V+ (ν V^ m-ym2):-zm2)2 二 d2m2，得到一個定位出(夂-+ (ν m~Zmi)
的引導(dǎo)模塊的三維坐標值(xm，ym，zffl)；步驟⑶重復(fù)執(zhí)行步驟(7) Ci次，得到個引導(dǎo)模塊的三維坐標值 {xx,yx,zx). (x2,y2,z2). (χ3,γ3,ζ3)- (xc^,yc,,zc3J；步驟(9)根據(jù)步驟⑶得到的C^個引導(dǎo)模塊的三維坐標值，計算引導(dǎo)模塊的最終
X1 +χ2 +χ3 + …+χ 3 yi+y2 + ^3 + …
三維坐標值(xr，yr，Zr) ；^ =--N-,yr=-—3-
CjV^N
Z1 + Z1 + Ζ-, + ...+ ζ, _ __￡ν_
^r — O
C步驟(10)車載定位模塊向電機控制模塊發(fā)送最終三維坐標值(^，yr, zr)；步驟(11)重復(fù)步驟(5)至步驟(10)，實時計算出引導(dǎo)模塊的三維坐標，以使自動 弓I導(dǎo)車輛跟隨引導(dǎo)模塊運行，直至車載定位模塊的電源被關(guān)閉為止。本發(fā)明所具有的有益效果(1)、自適應(yīng)能力強，能較好克服外界環(huán)境對定位算法的影響；(2)、無線定位結(jié)果精度高、反應(yīng)速度快；(3)、能較好克服天線方向性對定位結(jié)果的影響，能滿足系統(tǒng)大負荷運算。


圖1為本發(fā)明使用場合的結(jié)構(gòu)示意圖。
具體實施例方式以下結(jié)合附圖對本發(fā)明作進一步說明。如圖1所示，車載定位模塊1上裝有第一低頻發(fā)射基站1-1-1、第二低頻發(fā)射基站 1-1-2、第三低頻發(fā)射基站1-1-3、…、第N低頻發(fā)射基站1-1-N，共計N個低頻發(fā)射基站和 一個高頻通信模塊1-2，低頻發(fā)射基站的發(fā)射功能和高頻通信模塊的通信功能均由車載定 位模塊1上的控制器控制。在一定距離處的引導(dǎo)模塊2上裝有三維低頻接收器2-1，該接收 器能有效地接受來自低頻發(fā)射基站的低頻無線電信號，引導(dǎo)模塊上的高頻通信模塊2-2與 車載定位模塊上的高頻通信模塊2-1在一定的頻域內(nèi)進行通信。在上述基礎(chǔ)上，本發(fā)明方法的具體步驟如下步驟(1)對車載定位模塊的控制器1-3初始化，初始化對象包括控制器中的單片 機時鐘頻率、輸入輸出、內(nèi)部變量、寄存器、非易失性存儲器和中斷；車載定位模塊的控制器 采用Freescale公司的16位雙核微控制器MC9S12XEP100，該控制器內(nèi)部包含一個頻率可達 IOOMHz的協(xié)處理器，可以用來完成大量的數(shù)學(xué)運算，內(nèi)部含有非易失性存儲器，使用中采用 的CPU線運行頻率40MHz。步驟(2)無線定位模塊的車載定位模塊中的控制器從非易失性存儲器中讀取場 地環(huán)境對應(yīng)的標定參數(shù)，標定參數(shù)包括路徑衰減指數(shù)λ、標準偏差為σ的正態(tài)隨機變量 I o>d0米處引導(dǎo)模塊收到的RSSI信號強度值RSSI (d0) ； λ、ξ。、RSSI (d0)與外界環(huán)境密 切相關(guān)，可采用最小二乘法根據(jù)場地環(huán)境擬合得到；標定參數(shù)的過程固定低頻發(fā)射基站， 在距低頻發(fā)射基站0至IOm之間，每隔20cm的距離選取一個測試點，引導(dǎo)模塊位于測試點 處，測試低頻發(fā)射基站在發(fā)射RSSI信號強度值恒定為RSSI (C)時，引導(dǎo)模塊在測試點處收 到的RSSI信號強度值，測得一系列強度值，采用最小二乘法對λ、ξ。進行擬合，選取Cltl 為5m，測試引導(dǎo)模塊在該點收到的RSSI信號強度值RSSI ((Ici)，把λ、ξ。、RSSI (d0)存儲于 MC9S12XEP100的非易失性存儲器中。步驟(3)車載定位模塊等待來自電機控制模塊的定位啟動命令；如果收到電機控 制模塊的定位啟動命令，則執(zhí)行步驟(4)，如果未收到電機控制模塊的定位啟動命令，則重 復(fù)本步驟；步驟⑷車載定位模塊通過ISM (433. 05MHz 434. 79MHz)頻段的高頻無線電信 號向引導(dǎo)模塊發(fā)送初始化命令，使能引導(dǎo)模塊初始化；引導(dǎo)模塊初始化內(nèi)部的控制器2-3 中的單片機時鐘頻率、輸入輸出、內(nèi)部變量、寄存器和中斷，使能引導(dǎo)模塊上的三維低頻接 收天線；ISM頻段的高頻無線電信號頻率為采用FSK調(diào)制方式。引導(dǎo)模塊內(nèi)部的控制器選用 Freescale公司的MC9S08AC16，CPU運行頻率32MHz，CPU總線頻率16MHz，該控制器包含豐 富的輸入輸出、定期器和中斷資源。引導(dǎo)模塊上的三維低頻接收天線，由于采用三維結(jié)構(gòu)， 可以很好的接收三維空間的低頻無線信號，降低了低頻無線接收時對引導(dǎo)模塊放置姿態(tài)的 要求。步驟(5)車載定位模塊包含N (N >3)個低頻發(fā)射基站，其三維坐標依次為(Χι，Υι，
6Z1)、(X2, J2, Z2)、(x3, y3，Z3)…(xN，yN，zN)；關(guān)閉所有N個低頻發(fā)射基站的低頻發(fā)射功能，使 能第η (η < N)個低頻發(fā)射基站的低頻發(fā)射功能，發(fā)射頻率恒定為f。，發(fā)射RSSI信號強度值 恒定為RSSI (C)；車載定位模塊通過ISM頻段的高頻無線電信號向引導(dǎo)模塊發(fā)送接收命令；引導(dǎo)模 塊開始對頻率為f。的低頻無線電信號的接收，得到接收RSSI信號強度值RSSI (Cltl)；引導(dǎo)模 塊通過ISM頻段的高頻無線電信號向車載定位模塊發(fā)回RSSI (dn)，車載定位模塊記錄該值
RSSI(d0)-RSSndn)+Ca
并保存于內(nèi)存中；對RSSI(dn)，車載定位模塊根據(jù)公式j(luò) =10 ^χ<，計
算引導(dǎo)模塊距第n(n ^ N)個低頻發(fā)射基站的距離dn ；—般情況下N選取6，既可以提高后 續(xù)計算的精度，也可以降低運算量，fc為125KHz。步驟(6)延時時間30毫秒，重復(fù)執(zhí)行步驟(5)直到得到N個低頻發(fā)射基站中每個 低頻發(fā)射基站對應(yīng)的距離d” d2、d3. · · (V” dN ；以任意一個低頻發(fā)射基站為原點，建立空間三維坐標系，則屯、d2、d3. . . d^、dN分 別對應(yīng)N個低頻發(fā)射基站三維坐標(X1, Y1, Z1)、(x2, j2, z2)、(x3, y3, z3)…(xN，yN，zN)；步驟(7)從步驟(6)得到的N個距離值里挑選出3個距離值，一共有C^種挑選方 案，依次編號為1、2、3···。;假設(shè)其中某一方案中的3個距離值為dml、dm2、 dm3,對應(yīng)的三個低頻發(fā)射基站坐標記為(xml， Yml，Zmi)、(Xm2，Υηι2，Zm2)、(Xm3，Υηι3，Zm3)；
權(quán)利要求
用于自動引導(dǎo)車輛的近場低頻無線定位方法，其特征在于該方法包括如下步驟步驟(1)對車載定位模塊的控制器初始化；初始化對象包括控制器中的單片機時鐘頻率、輸入輸出、內(nèi)部變量、寄存器、非易失性存儲器和中斷；步驟(2)車載定位模塊的控制器從非易失性存儲器中讀取場地環(huán)境對應(yīng)的標定參數(shù)；所述的標定參數(shù)包括路徑衰減指數(shù)λ、標準偏差為σ的正態(tài)隨機變量ξσ、d0米處引導(dǎo)模塊收到的RSSI信號強度值RSSI(d0)；λ、ξσ和RSSI(d0)采用最小二乘法根據(jù)場地環(huán)境擬合得到；步驟(3)車載定位模塊等待來自電機控制模塊的定位啟動命令；如果收到電機控制模塊的定位啟動命令，則執(zhí)行步驟(4)，如果未收到電機控制模塊的定位啟動命令，則重復(fù)本步驟；步驟(4)車載定位模塊通過自帶的高頻通信模塊，在433.05MHz～434.79MHz頻段內(nèi)向引導(dǎo)模塊發(fā)送初始化命令，使能引導(dǎo)模塊初始化；引導(dǎo)模塊初始化其內(nèi)部的控制器中的單片機時鐘頻率、輸入輸出、內(nèi)部變量、寄存器和中斷，使能引導(dǎo)模塊上的三維低頻接收器；步驟(5)車載定位模塊關(guān)閉所有N個低頻發(fā)射基站的低頻發(fā)射功能，使能第n個低頻發(fā)射基站的低頻發(fā)射功能，發(fā)射頻率恒定為fc，發(fā)射RSSI信號強度值恒定為RSSI(C)，同時車載定位模塊通過自帶的高頻通信模塊，在433.05MHz～434.79MHz頻段內(nèi)向引導(dǎo)模塊發(fā)送接收命令，其中n為自然數(shù)，且n≤N；引導(dǎo)模塊接收來自第n個低頻發(fā)射基站的低頻無線電信號，得到接收RSSI信號強度值RSSI(dn)；引導(dǎo)模塊自帶的高頻通信模塊在433.05MHz～434.79MHz頻段內(nèi)向車載定位模塊發(fā)回RSSI(dn)，車載定位模塊記錄該值并保存于內(nèi)存中；車載定位模塊計算引導(dǎo)模塊距第n個低頻發(fā)射基站的距離dn，步驟(6)延時設(shè)定的時間t，重復(fù)執(zhí)行步驟(5)直到得到N個低頻發(fā)射基站中每個低頻發(fā)射基站對應(yīng)的距離d1、d2、d3...dN 1、dN；以任意一個低頻發(fā)射基站為原點，建立空間三維坐標系，則d1、d2、d3...dN 1、dN分別對應(yīng)N個低頻發(fā)射基站三維坐標(x1，y1，z1)、(x2，y2，z2)、(x3，y3，z3)…(xN，yN，zN)；步驟(7)在d1、d2、d3...dN 1、dN中任選3個距離值，分別標記為dm1、dm2、dm3，其對應(yīng)的三個低頻發(fā)射基站三維坐標記為(xm1，ym1，zm1)、(xm2，ym2，zm2)、(xm3，ym3，zm3)；解方程組得到一個定位出的引導(dǎo)模塊的三維坐標值(xm，ym，zm)；步驟(8)重復(fù)執(zhí)行步驟(7)次，得到個引導(dǎo)模塊的三維坐標值步驟(9)根據(jù)步驟(8)得到的個引導(dǎo)模塊的三維坐標值，計算引導(dǎo)模塊的最終三維坐標值(xr，yr，zr)；其中 <mrow><msub> <mi>x</mi> <mi>r</mi></msub><mo>=</mo><mfrac> <mrow><msub> <mi>x</mi> <mn>1</mn></msub><mo>+</mo><msub> <mi>x</mi> <mn>2</mn></msub><mo>+</mo><msub> <mi>x</mi> <mn>3</mn></msub><mo>+</mo><mo>.</mo><mo>.</mo><mo>.</mo><mo>+</mo><msub> <mi>x</mi> <msubsup><mi>C</mi><mi>N</mi><mn>3</mn> </msubsup></msub> </mrow> <msubsup><mi>C</mi><mi>N</mi><mn>3</mn> </msubsup></mfrac><mo>,</mo> </mrow> <mrow><msub> <mi>y</mi> <mi>r</mi></msub><mo>=</mo><mfrac> <mrow><msub> <mi>y</mi> <mn>1</mn></msub><mo>+</mo><msub> <mi>y</mi> <mn>2</mn></msub><mo>+</mo><msub> <mi>y</mi> <mn>3</mn></msub><mo>+</mo><mo>.</mo><mo>.</mo><mo>.</mo><mo>+</mo><msub> <mi>y</mi> <msubsup><mi>C</mi><mi>N</mi><mn>3</mn> </msubsup></msub> </mrow> <msubsup><mi>C</mi><mi>N</mi><mn>3</mn> </msubsup></mfrac><mo>,</mo> </mrow> <mrow><msub> <mi>z</mi> <mi>r</mi></msub><mo>=</mo><mfrac> <mrow><msub> <mi>z</mi> <mn>1</mn></msub><mo>+</mo><msub> <mi>z</mi> <mn>2</mn></msub><mo>+</mo><msub> <mi>z</mi> <mn>3</mn></msub><mo>+</mo><mo>.</mo><mo>.</mo><mo>.</mo><mo>+</mo><msub> <mi>z</mi> <msubsup><mi>C</mi><mi>N</mi><mn>3</mn> </msubsup></msub> </mrow> <msubsup><mi>C</mi><mi>N</mi><mn>3</mn> </msubsup></mfrac> </mrow>步驟(10)車載定位模塊向電機控制模塊發(fā)送最終三維坐標值(xr，yr，zr)；步驟(11)重復(fù)步驟(5)至步驟(10)，實時計算出引導(dǎo)模塊的三維坐標，以使自動引導(dǎo)車輛跟隨引導(dǎo)模塊運行，直至車載定位模塊的電源被關(guān)閉為止。FSA00000256393200011.tif,FSA00000256393200021.tif,FSA00000256393200022.tif,FSA00000256393200023.tif,FSA00000256393200024.tif,FSA00000256393200025.tif
全文摘要
本發(fā)明涉及一種用于自動引導(dǎo)車輛的近場低頻無線定位方法。本發(fā)明方法首先對車載定位模塊的控制器初始化、讀取標定參數(shù)、等待來自電機控制模塊的定位啟動命令；收到電機控制模塊的定位啟動命令后引導(dǎo)模塊發(fā)送初始化命令，使能引導(dǎo)模塊初始化；其次車載定位模塊中的低頻發(fā)射基站發(fā)射信號，由引導(dǎo)模塊接收后返回信號強度值RSSI(dn)，從而得到每個低頻發(fā)射基站與引導(dǎo)模塊之間的距離，選取任意三個距離值，計算引導(dǎo)模塊的三維坐標值。然后對所有的引導(dǎo)模塊的三維坐標值求平均得到最終的三維坐標值。最后將最終的三維坐標值發(fā)送給電機控制模塊，以引導(dǎo)車輛跟隨引導(dǎo)模塊運行。本發(fā)明自適應(yīng)能力強、反應(yīng)速度快。
文檔編號H04W64/00GK101951547SQ20101027211
公開日2011年1月19日 申請日期2010年9月3日 優(yōu)先權(quán)日2010年9月3日
發(fā)明者張懷相, 張翔, 戴國駿, 李二濤, 李駿 申請人:杭州電子科技大學(xué)