本發(fā)明涉及一種移動機器人室內定位系統(tǒng)及其計算方法，具體涉及一種基于超寬帶(UWB)技術和電機編碼器信息的移動機器人室內定位系統(tǒng)及其定位計算方法。
背景技術：
：機器人利用傳感器感知周圍環(huán)境來確定自身在環(huán)境中位置的自主定位技術，是機器人能夠完成諸如路徑規(guī)劃、自主導航等復雜任務的前提，移動機器人是通過自身攜帶的傳感器來獲取自身狀態(tài)和周圍環(huán)境信息，從而實現(xiàn)自主定位。目前，常用的傳感器有里程計、陀螺儀和加速度計等傳感器，常用的定位系統(tǒng)有GPS和北斗導航定位系統(tǒng)。由于里程計、陀螺儀和加速度計傳感器在進行計算時，具有誤差累積效應，不適合長距離定位。GPS和北斗定位系統(tǒng)可實現(xiàn)室外無遮擋環(huán)境下全球的定位，但若應用在室內環(huán)境中，由于衛(wèi)星信號被建筑物等阻隔，定位精度低，難以達到室內定位要求。利用藍牙、RFID和紅外技術組成的網(wǎng)絡，傳輸數(shù)據(jù)速率低，傳輸距離短；超聲波定位系統(tǒng)定位精度雖然較高，但超聲波受多徑效應和非視距傳播影響很大，同時需要大量的底層硬件設施投資，成本太高。WLAN(WirelessLocalAreaNetwork)技術和RFID(RadioFrequencyIdentification)技術是新興的定位技術，該技術的顯著特點是設備簡單、安裝方便、信息傳輸速率高。目前，WLAN應用于小范圍的室內定位，成本較低。但無論是用于室內還是室外定位，Wi-Fi收發(fā)器都只能覆蓋半徑90米以內的區(qū)域，而且很容易受到其他信號的干擾，從而影響其精度，定位器的能耗也較高。技術實現(xiàn)要素：本發(fā)明是為了解決上述問題而進行的，目的在于提供一種定位精度高、能耗小、定位精準度及數(shù)據(jù)傳送率高，且適合無線傳感器網(wǎng)絡的室內移動機器人自主定位系統(tǒng)。DecaWave公司的DWM1000模塊定位精度高、能耗小，集成了射頻無線電設計所需的全部元器件，包括DW1000定位芯片、天線、平衡-不平衡變換器、石英晶體及無源元件。其定位精度為±10厘米，數(shù)據(jù)傳輸率高達6.8Mb/s，適合無線傳感器網(wǎng)絡(WSN)應用，直線距離達290米，非直視距離為35米。本發(fā)明是基于超寬帶(UWB)技術，并且融合了電機編碼器的信息設計出的進一步提高了定位精度的移動機器人室內定位系統(tǒng)，實現(xiàn)了移動機器人在室內的精準定位。本發(fā)明提供了一種室內移動機器人的室內定位系統(tǒng)，是基于超寬帶技術和電機編碼器信息的自主移動的室內移動機器人定位系統(tǒng)，具有這樣的特征，包括：室內移動機器人，是用美國國家儀器有限公司的myRIO做主控器的，在一定空間范圍內可以自主移動的三全向輪移動機器人；電子羅盤，又稱航向角傳感器，是設置在室內移動機器人上用于測定該室內移動機器人自身的航向角的；電機編碼器，是設置在室內移動機器人上用于提供自身定位信號的；以及三塊DWM1000模塊，第三DWM1000模塊設置在移動機器人上，第一和第二DWM1000模塊設置在一定空間范圍內，其中，設置在移動機器人上的第三DWM1000模塊為標簽，固定設置在一定空間范圍內的第一和第二DWM1000模塊為第一和第二基站。本發(fā)明還提供了一種定位計算方法，是用于前述室內移動機器人的定位系統(tǒng)的定位計算方法，具有這樣的特征，包括：步驟一，確定坐標原點，開啟基站，再開啟室內移動機器人，以開啟時室內移動機器人所測得的位姿2為初始位姿；步驟二，通過電機編碼器得到t時刻的移動機器人的即時位姿1；步驟三，標簽接收基站發(fā)射的高頻電磁脈沖，通過計算方法確定出移動機器人自身的位置；步驟四，用電子羅盤所測得的航向角擴維步驟三得到的移動機器人位置，得到t時刻的即時位姿2；步驟五，用計算法融合即時位姿1和即時位姿2，得到t的位姿估計，后回到步驟二。在本發(fā)明提供的定位計算方法中，還可以具有這樣的特征：其中，步驟一中初始位姿的定位方法：計算步驟I，在一定空間中建立XOY坐標系，第一和第二基站的高度為h1、h2，第一和第二基站的距相鄰墻面的距離為l1、l2，第一和第二基站與移動機器人間的距離，由TOF算法算得為d1、d2，再設定移動機器人在一定空間中的坐標為(x，y)，則由幾何關系可得：(y-l1)2+x2+h12=d12(x-l2)2+(Ymax-y)+h22=d22]]>式中，ymax為一定空間y軸方向的寬度，令，m=d12-d22+l22-l12+h22-h12+Ymax22l2,]]>n=2m(Ymax-2l1)l2,]]>k=(Ymax-2l1)2l22,]]>則得到y(tǒng)=(2l1+n)±(2l1+n)2-4(k+1)(l12+m2+h12-d12)2(k+1)x=m-y(Ymax-2l1)l2]]>上式中，舍去不在0～ymax范圍內的y值得到移動機器人在一定空間中的坐標(x，y)；計算步驟II，電子羅盤測量得到航向角θ；計算步驟III，用室內移動機器人同時刻的位置坐標(x，y)和朝向航向角θ組合描述該時刻室內移動機器人所處狀態(tài)，得到初始位姿(x，y，θ)。在本發(fā)明提供的定位計算方法中，還可以具有這樣的特征：其中，步驟二中即時位姿的定位方法：室內移動機器人的參考坐標系為XROYR，全局坐標系為XWOYW坐標系，θ為室內移動機器人的自轉角度，VRx、VRy分別為機器人在其參考坐標系XROYR上的運動速度，VWx、VWy分別為機器人在其全局坐標系XWOYW上的運動速度，VWx、VWy是由VRx、VRy分別往XW軸和YW方向分解、合成而得，T為時間，因此，在參考坐標系XROYR中室內移動機器人的速度為：[VRx，VRy，θ]T，在全局坐標系XWOYW中室內移動機器人的速度為：[VWx，VWy，θ]T，定位系統(tǒng)工作時，每間隔Δt時間內的位移[VWxΔt，VWyΔt，θΔt]T，循環(huán)n次，得到的n組位移[VWxΔt，VWyΔt，θΔt]jT對得到的n組位移[VWxΔt，VWyΔt，θΔt]jT做中值濾波處理，得到n組濾波后的位移[VWxΔt，VWyΔt，θΔt]kT，上述中：j、k均為1,2,3，...n的整數(shù)，是循環(huán)次數(shù)，對濾波處理后的位移求和，即得到n*Δt時間內的即時位姿1(xr，yr，θr)t，(xr,yr,θr)=Σk=1n[VWxΔt,VWyΔt,θ·Δt]kT.]]>在本發(fā)明提供的定位計算方法中，還可以具有這樣的特征：其中，步驟三的計算規(guī)則是TimeofFlight計算法或是TimeDiffe-renceofArrival計算法。在本發(fā)明提供的定位計算方法中，還可以具有這樣的特征：其中，步驟四中，航向角為θ，室內移動機器人的位置為(x，y)，得到的t時刻的即時位姿1(x，y，θ)。在本發(fā)明提供的定位計算方法中，還可以具有這樣的特征：其中，步驟五的即時位姿1和即時位姿2的融合方法，是卡爾曼濾波融合方法。發(fā)明的作用與效果根據(jù)本發(fā)明所涉及的定位計算方法，因為包含了：確定坐標原點，開啟基站，再開啟室內移動機器人，以開啟時室內移動機器人所測得的位姿2為初始位姿的步驟一；通過電機編碼器得到t時刻的移動機器人的即時位姿1的步驟二；標簽接收基站發(fā)射的高頻電磁脈沖，通過計算方法確定出移動機器人自身的位置的步驟三；用電子羅盤所測得的航向角擴維步驟三得到的室內移動機器人位置，得到t時刻的即時位姿2的步驟四；和用計算法融合即時位姿1和即時位姿2，得到t的位姿估計，后回到步驟二的步驟五，所以，本發(fā)明的定位計算方法是對從異質傳感器獲取的兩種位姿進行融合，獲得了高精度的室內移動機器人的位姿，使用該定位計算方法的定位系統(tǒng)定位的結果更加可靠、精度更高，且具有很強的環(huán)境適用性。附圖說明圖1是本發(fā)明實施例的室內移動機器人硬件架構圖；圖2是本發(fā)明實施例的室內移動機器人的室內定位系統(tǒng)框圖；圖3是本發(fā)明實施例的室內移動機器人的工作流程圖；圖4是本發(fā)明實施例的室內移動機器人的水平位置坐標系統(tǒng)的模型圖；圖5是本發(fā)明實施例的基站和標簽的位置關系模型圖；圖6是本發(fā)明實施例的室內移動機器人的電子羅盤測航向角的程序框圖；以及圖7是本發(fā)明實施例的基于卡爾曼濾波的效果示意圖。具體實施方式為了使本發(fā)明實現(xiàn)的技術手段、創(chuàng)作特征、達成目的與功效易于明白了解，以下實施例結合附圖對本發(fā)明的基于超寬帶(UWB)技術的移動機器人室內定位系統(tǒng)及其定位計算方法作具體闡述。圖1是本發(fā)明實施例的室內移動機器人硬件架構圖。如圖1所示，本實施例的室內定位系統(tǒng)使用的是三全向輪移動機器人，其主控制器為NationalInstruments(美國國家儀器有限公司，簡稱NI)生產的myRIO。其他還有一個電子羅盤(航向角感應器)及一塊DWM1000模塊安裝在移動機器人上。另外，還有兩塊DWM1000模塊安放在本實施例的移動機器人活動的空間中。安裝到移動機器人上的一塊DWM1000模塊作為標簽，固定設置在移動機器人活動的空間里的兩塊DWM1000模塊作為基站。本實施例的移動機器人的軟件系統(tǒng)是基于Labview編寫的程序，經過調試后，下載到移動機器人的控制器中運行。圖2是本發(fā)明實施例的室內移動機器人的室內定位系統(tǒng)框圖。圖3是本發(fā)明實施例的室內移動機器人的工作流程圖。如圖2、圖3所示，在本發(fā)明實施例中的移動機器人中：步驟一，以室內的某個墻角作為XOY坐標系的坐標原點，先使基站開始工作，然后移動機器人開機。將開機時刻所測得的移動機器人的位姿2(xa，ya，θa)t0作為移動機器人的初始位姿(x，y，θ)t0。圖4是本發(fā)明實施例的室內移動機器人的水平位置坐標系統(tǒng)的模型圖。如圖4所示，本實施例使用的帶有三個全向輪的，且三個全向輪按等間隔120°分布的室內移動機器人，在圖中坐標系XWOYW為全局坐標系和圖中坐標系XROYR為室內移動機器人參考坐標系的雙坐標系統(tǒng)中。d為全向輪距室內移動機器人中心的距離，V1、V2、V3分別為三個輪子的轉速，θ表示其自轉的角度，а為XR軸正方向與XW軸正方向的夾角，XRx、YRy分別為機器人在其參考坐標系XROYR上的運動速度，VWx、VWy分別為機器人在其全局坐標系XWOYW上的運動速度，VWx、VWy是由XRx、YRy分別往XW軸和YW方向分解、合成而得，則：電機所用編碼器為增量式光電編碼器，編碼器的分辨率為P，電機減速箱減速比為G，輪子半徑為r，三者都為已知。機器人運動時，如測得第i(i＝1,2,3)個編碼器的脈沖數(shù)為Mi，則與之對應的輪速用下式計算：Vi=Mi·r2πPG---(2)]]>在此定義：在參考坐標系XROYR中室內移動機器人的速度為[VRx，VRy，θ]T，在全局坐標系XWOYW中室內移動機器人的速度為[VWx，VWy，θ]T，θ是室內移動機器人的自轉角速度。則由機器人模型可得：V1V2V3=01dsinπ3-cosπ3d-sinπ3-cosπ3d·VRxVRyθ·---(3)]]>由上式(3)可得：VRxVRyθ·=033-3323-13-1313d13d13d·V1V2V3---(4)]]>由式(2)和式(4)即可求得[VRx，VRy，θ]T，進而即可求得[VWx，VWy，θ]T。定位系統(tǒng)工作時，每間隔Δt(如Δt＝0.01s)讀取一次[VWx，VWy，θ]T，進而可得Δt時間內的位移[VWxΔt，VWyΔt，θΔt]T，然后循環(huán)n次，將得到n組位移[VWxΔt，VWyΔt，θΔt]jT(j為循環(huán)次數(shù)，j＝1,2,...n)。然而，用編碼器得到位移時會有誤差，誤差分為系統(tǒng)誤差和非系統(tǒng)誤差。系統(tǒng)誤差與外界環(huán)境無關，它以常量進行誤差累積，因此不適合做長距離的定位；非系統(tǒng)誤差與外界環(huán)境有關。中值濾波是一種能有效抑制噪聲的非線性信號處理技術，其對脈沖噪聲有良好的濾除作用。對上面得到的n組位移[VWxΔt，VWyΔt，θΔt]jT(j為循環(huán)次數(shù)，j＝1,2,...n)做中值濾波處理，以減小測得其誤差。處理后的n組位移為[VWxΔt，VWyΔt，θΔt]kT(k為循環(huán)次數(shù)，k＝1,2,...)，然后將處理后的位移求和，即得到n*Δt時間內的位姿1(xr，yr，θr)t，即：(xr,yr,θr)=Σk=1n[VWxΔt,VWyΔt,θ·Δt]kT]]>通過以上的做法獲得位姿1。步驟二，移動機器人開始運動，將移動機器人的電機編碼器測得的值經過運算得到在移動機器人參考坐標系下的x、y方向及旋轉方向的速度，速度乘以時間Δt則得到位移。再對位移做中值濾波處理，以盡量減小其測量的誤差，得到t時刻的位姿1(xr，yr，θr)t。圖5是本發(fā)明實施例的基站和標簽的位置關系模型圖。如圖5所示，本實施例共使用三個DWM1000模塊，其中，一個DWM1000模塊裝到室內移動機器人上作為標簽，另兩個DWM1000模塊固定在室內已知位置點作為基站?；竞褪覂纫苿訖C器人在房間中的位置模型如圖5。兩基站的高度為h1、h2，兩基站距相鄰墻面的距離為l1、l2，這些均為已知。d1、d2為由TOF算法測得的兩基站距室內移動機器人的距離，設室內移動機器人在房間中的坐標為(x，y)，則由幾何關系可得：(y-l1)2+x2+h12=d12(x-l2)2+(Ymax-y)+h22=d22---(5)]]>式中，ymax為房間在y軸方向的寬度。令，m=d12-d22+l22-l12+h22-h12+Ymax22l2,]]>n=2m(Ymax-2l1)l2,]]>則得：y=(2l1+n)±(2l1+n)2-4(k+1)(l12+m2+h12-d12)2(k+1)x=m-y(Ymax-2l1)l2---(6)]]>上式(6)中，y有兩個值，舍去不在0～ymax范圍內的取值，則得到室內移動機器人在房間中的位置坐標(x，y)。步驟三，在移動機器人的電機編碼器測量計算的同時，基站向標簽發(fā)射高頻電磁脈沖。標簽接收到高頻電磁脈沖后，通過TOF(TimeofFlight)計算法或TDOA(TimeDifferenceofArrival)計算法算出標簽距基站的距離。將得到的距離根據(jù)幾何運算法可得到機器人在室內的x、y坐標位置。步驟四，將電子羅盤(又稱航向角傳感器)測得的航向角θ對步驟三得到的x、y坐標進行擴維，得到t時刻的移動機器人的位姿2(xa，ya，θa)t。圖6是本發(fā)明實施例的室內移動機器人的電子羅盤測航向角的程序框圖。對于室內移動機器人的航向角θ，則用電子羅盤測量。如圖6所示，本實施例采用基于Labview的電子羅盤航向角測量，其程序框圖即圖6。定位系統(tǒng)工作時，獲得同時刻的(x，y)和θ，且每隔n*Δt個時間段更新一次數(shù)據(jù)，如此便可獲得與位姿1(xr，yr，θr)，同步的位姿2(xa，ya，θa)。通過以上做法獲得位姿2(xa，ya，θa)。步驟五，用卡爾曼(Kalman)濾波算法將位姿1(xr，yr，θr)t和位姿2(xa，ya，θa)t融合，即用位姿2對位姿1進行修正，以得到移動機器人在t時刻的位姿估計(x，y，θ)t。圖7是本發(fā)明實施例的基于卡爾曼濾波的效果示意圖。如圖7所示，位姿1和位姿2按照以下方法融合。本實施例采用KF算法融合位姿1和位姿2。設U(t)T＝(xr,yr,θr)，Z(t)T＝(xa,ya,θa)，X(t)為每個n*Δt時間內的位姿估計，是三維列向量，則融合過程如下：X(t＝0)＝Z(t＝0)(7)式(7)是系統(tǒng)的初始化方程；X(t)＝X(t-1)+U(t)+W(t)(8)式(8)為系統(tǒng)的狀態(tài)方程；Z(t)＝X(t)+V(t)(9)式(9)為觀測方程；X(t|t-1)＝X(t-1|t-1)+U(t)(10)式(10)是用t-1時刻的估計值預測t時刻的值；P(t|t-1)＝P(t-1|t-1)+Q(11)式(11)是用t-1時刻的P值預測t時刻的P值；Kg(t)=P(t|t-1)P(t|t-1)+R---(12)]]>式(12)是計算t時刻的卡爾曼增益；X(t|t)＝X(t|t-1)+Kg(t)[Z(t)-X(t|t-1)](13)式(13)結合預測值和觀測值得到t時刻的估算值；P(t|t)＝[I-Kg(t)]P(t|t-1)(14)式(14)用于計算t時刻的P值。上述式(7)～(14)中，W(t)和V(t)分別為獲取位姿1和位姿2時的噪聲，Q和R分別為W(t)和V(t)的噪聲協(xié)方差，P為X(t)的協(xié)方差，W(t)、V(t)、Q、R、P均為三維列向量。對Q、R和P取恰當?shù)闹?，則由式(10)～(14)即可得到每個n*Δt時間內的位姿估計。用卡爾曼濾波融合位姿1和位姿2的效果示意圖表示在圖7中。然后保存t時刻的位姿估計值，并跳轉至步驟二，獲得下一個時刻，即t+1時刻的位姿估計，如此不斷循環(huán)，即可得到每個時刻的位姿估計。實施例的作用與效果根據(jù)本發(fā)明所涉及的定位計算方法，因為包含了：確定坐標原點，開啟基站，再開啟室內移動機器人，以開啟時室內移動機器人所測得的位姿2為初始位姿的步驟一；通過電機編碼器得到t時刻的移動機器人的即時位姿1的步驟二；標簽接收基站發(fā)射的高頻電磁脈沖，通過計算方法確定出移動機器人自身的位置的步驟三；用電子羅盤所測得的航向角擴維步驟三得到的室內移動機器人位置，得到t時刻的即時位姿2的步驟四；和用計算法融合即時位姿1和即時位姿2，得到t的位姿估計，后回到步驟二的步驟五，所以，本發(fā)明的定位計算方法是對從異質傳感器獲取的兩種位姿進行融合，獲得了高精度的室內移動機器人的位姿，使用該定位計算方法的定位系統(tǒng)定位的結果更加可靠、精度更高，且具有很強的環(huán)境適用性。又因為本發(fā)明一種基于UWB技術的室內移動機器人的室內自主定位系統(tǒng)應用了本發(fā)明所涉及的定位計算方法，所以，與現(xiàn)有技術相比具有以下優(yōu)點：第一、位姿1的獲得是基于電機編碼器的，而目前大多數(shù)電機都自帶編碼器，這增強了本方法的適用性；第二、航向角傳感器眾多，因此測得航向角也比較容易，這也增強了本方法的適用性；第三、DWM1000模塊精度高，能耗低，且尺寸小，安裝和使用極為方便，這也增強了本方法的適用性；第四、位姿1與位姿2的獲得采用的是兩種異質傳感器，使用融合算法兼顧了位姿1與位姿2，使得定位結果更加可靠。上述實施方式為本發(fā)明的優(yōu)選案例，并不用來限制本發(fā)明的保護范圍。當前第1頁1 2 3