一種室內(nèi)移動機(jī)器人定位方法
【專利摘要】一種室內(nèi)移動機(jī)器人定位方法，屬于移動機(jī)器人定位【技術(shù)領(lǐng)域】。本發(fā)明提供一種基于單超聲波傳感器、編碼器和電子羅盤等多傳感器融合的異步時間到達(dá)（ATOA）室內(nèi)定位方法，并提出了一種雙層卡爾曼濾波（DLKF）的數(shù)據(jù)融合模型?；贒LKF模型，ATOA方法能有效消除航跡推算中的累計誤差，獲得較高的定位精度，極大地降低了定位所需的超聲波傳感器數(shù)量，有效地克服了采用超聲波傳感器定位時存在盲區(qū)的缺點。基于DLKF模型，ATOA方法能有效消除航跡推算中的累計誤差，獲得較好的定位精度，極大地降低了定位所需的超聲波傳感器數(shù)量。
【專利說明】一種室內(nèi)移動機(jī)器人定位方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于移動機(jī)器人定位【技術(shù)領(lǐng)域】，涉及一種機(jī)器人室內(nèi)定位方法。
【背景技術(shù)】
[0002]作為機(jī)器人導(dǎo)航的基礎(chǔ)，定位問題一直是機(jī)器人研究領(lǐng)域的熱點之一。室內(nèi)定位方法可分為相對定位和絕對定位，相對定位主要是利用安裝在機(jī)器人上的編碼器根據(jù)最初位姿進(jìn)行航跡推算(Dead-Reckoning),但隨著機(jī)器人航行距離的增大,定位誤差會累積，需要引入絕對定位手段輔助。絕對定位是利用超聲波傳感器、視覺傳感器、激光雷達(dá)等一種或多種外部傳感器檢測得到的周圍環(huán)境特征對機(jī)器人在參考坐標(biāo)系中的絕對位姿進(jìn)行估算。相對于其他環(huán)境感知傳感器，超聲波傳感器因其價格低廉、硬件易實現(xiàn)并且技術(shù)成熟等優(yōu)點，已被廣泛應(yīng)用于移動機(jī)器人的室內(nèi)定位。
[0003]目前，基于超聲波測距的定位一般是機(jī)器人同時接收至少三個超聲波傳感器測得的距離信息經(jīng)計算得到機(jī)器人絕對坐標(biāo)，而后與航跡推算得到的相對坐標(biāo)進(jìn)行信息融合。這些方法存在超聲波傳感器數(shù)量多、定位誤差大、存在盲區(qū)等缺點。

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

[0004]本發(fā)明的目的是提供一種基于單超聲波傳感器、編碼器和電子羅盤等多傳感器融合的異步時間到達(dá)(Asynchronous Time of Arrival, ΑΤ0Α)室內(nèi)定位方法,并提出了一種雙層卡爾曼濾波(Double-layer Kalman Filter, DLKF)的數(shù)據(jù)融合模型?；贒LKF模型，ATOA方法能有效消除航跡推算中的累計誤差，獲得較高的定位精度，極大地降低了定位所需的超聲波傳感器數(shù)量，有效地克服了采用超聲波傳感器定位時存在盲區(qū)的缺點。
[0005]本發(fā)明采用超聲波網(wǎng)絡(luò)定位系統(tǒng)對室內(nèi)移動機(jī)器人進(jìn)行定位，超聲波網(wǎng)絡(luò)定位系統(tǒng)主要分為三個模塊:控制模塊、超聲波發(fā)射模塊和超聲波接收模塊。將超聲波接收模塊安裝在室內(nèi)天花板上，超聲波接收模塊的坐標(biāo)已事先通過測量獲得，并將超聲波接收模塊的坐標(biāo)數(shù)據(jù)存儲在控制模塊中。控制模塊與超聲波發(fā)射模塊通過串口相連接，固定安裝在移動機(jī)器人上。其具體定位過程如下:
(1)啟動一個安置在天花板的超聲波接收模塊，超聲波接收模塊進(jìn)入射頻信號和超聲波信號接收狀態(tài)；
(2)控制模塊向超聲波發(fā)射模塊發(fā)出定位請求，超聲波發(fā)射模塊發(fā)送射頻信號和超聲波信號，其中射頻信號攜帶發(fā)射模塊的身份標(biāo)識ID信息；
(3)接收模塊接收到射頻信號后，檢測接收到的ID信息與發(fā)射模塊的ID信息是否一致，若不一致則放棄，返回等待射頻信號接收狀態(tài)，執(zhí)行步驟(3);若一致，則執(zhí)行步驟(4)；
(4)接收模塊開始計時，直到檢測到超聲波信號或超過預(yù)定的檢測時間
Ts后停止計時，并將接收模塊、時間信息打包成數(shù)據(jù)包，將數(shù)據(jù)包通過射頻通訊的形式發(fā)送給超聲波發(fā)射模塊；
(5)超聲波發(fā)射模塊將接收到的數(shù)據(jù)包和檢測的溫度信息通過串口通訊的方式發(fā)送給控制|吳塊；
(6)控制模塊根據(jù)超聲波在一定溫度下空氣中的傳播速度和各接收模塊的時間信息，計算出超聲波接收模塊與超聲波發(fā)射模塊之間的距離；并根據(jù)超聲波接收模塊坐標(biāo)和超聲波接收模塊與超聲波發(fā)射模塊之間的距離，計算出移動機(jī)器人的平面坐標(biāo)，完成一次定位。定位流程圖如圖1所示，超聲波發(fā)射模塊和超聲波接收模塊的流程圖如圖2、3所示。
[0006]ATOA方法旨在通過利用幾次從同一個超聲波接收模塊接收得到的距離信息運算獲得機(jī)器人的坐標(biāo)，從而降低定位所需超聲波節(jié)點的數(shù)量，具體步驟如下:
1、當(dāng)機(jī)器人第一次獲得指定超聲波距離信息時，保持左右輪一定轉(zhuǎn)速差的情況下在超聲波接受模塊信號覆蓋區(qū)域圓周行駛。保持恒定速差的差動行駛具有自動定心的功能，實際轉(zhuǎn)速差可根據(jù)圓周大小確定。
[0007]2、選取行駛過程中，每到達(dá)圓周上的一個等分點Ii ,找出如圖7中
構(gòu)成一個等腰直角三角形，運用ATOA方法進(jìn)行一次計算估計出機(jī)器人的當(dāng)前位置Pt作為后面DLKF的觀測值。
[0008]3、DLKF通過之前的位置估計(即=DLKF內(nèi)的上一次估計，即前一次DLKF的后驗位姿估計)結(jié)合里程計及電子羅盤信息運算得到的位移矢量推算得到機(jī)器人這一時刻的先驗位姿估計數(shù)據(jù)。
[0009]4、通過前一次ATOA方法所獲得的觀測值對DLKF得到的先驗位姿估計進(jìn)行修正，得到機(jī)器人此時刻的后驗位姿估計。
[0010]5、當(dāng)?shù)竭_(dá)下一個點時，再次找出—組三個點(即選擇上
組的三個點逆時針相鄰的三個點構(gòu)成一個等腰直角三角形)，運用步驟3-4得到最新的機(jī)器人的位姿估計。而后每到達(dá)一個新的等分點，即重復(fù)步驟2-4，直到機(jī)器人的位姿估計方差小于所需的精度。
[0011]6、機(jī)器人停止行駛，DLKF最終的位姿估計即為此時機(jī)器人的位姿，機(jī)器人位姿初始化結(jié)束。
[0012]本發(fā)明提出的ATOA定位是一種新的移動機(jī)器人定位方法，其主要特點是只利用單超聲波傳感器測距信息與航跡推算信息來確定機(jī)器人坐標(biāo)。但是ATOA單次測算誤差較大，結(jié)果不宜直接使用，故通過DLKF將圓周定位中多次ATOA測算結(jié)果進(jìn)行數(shù)據(jù)融合，最終達(dá)到要求的精度。其最顯著的優(yōu)勢就是降低了定位所需的超聲波傳感器節(jié)點鋪設(shè)密度。而且，ATOA定位方法同樣可以被應(yīng)用到多個超聲波傳感器節(jié)點存在的情況。此時機(jī)器人的運動軌跡不受到單超聲波ATOA定位條件的約束。此外，任何測距傳感器如激光傳感器，無線射頻傳感器都可以采用ATOA方法定位，所以ATOA定位方法具有較大的實際應(yīng)用價值?；贒LKF模型，ATOA方法能有效消除航跡推算中的累計誤差，獲得較好的定位精度，極大地降低了定位所需的超聲波傳感器數(shù)量。
【專利附圖】

【附圖說明】
[0013]圖1為定位流程圖；
圖2為超聲波發(fā)射模塊的流程圖； 圖3為超聲波接收模塊的流程圖；
圖4為三個超聲波傳感器節(jié)點定位平面示意圖；
圖5為三個超聲波傳感器節(jié)點定位三維示意圖；
圖6為單超聲波傳感器節(jié)點定位示意圖；
圖7為圓周定位方法示意圖；
圖8為機(jī)器人運動模型示意圖；
圖9為雙層卡爾曼濾波關(guān)系圖；
圖10為機(jī)器人軌跡跟蹤(全局)；
圖11為機(jī)器人軌跡跟蹤(t=40iT90S)；
圖12為坐標(biāo)誤差平方和變化(全局)；
圖13為坐標(biāo)誤差平方和變化(t=18iT68S)(局部)。
[0014]【具體實施方式】
下面結(jié)合附圖對本發(fā)明的技術(shù)方案作進(jìn)一步的說明，但并不局限如此，凡是對本發(fā)明技術(shù)方案進(jìn)行修改或者等同替換，而不脫離本發(fā)明技術(shù)方案的精神和范圍，均應(yīng)涵蓋在本發(fā)明的保護(hù)范圍中。
[0015]本發(fā)明采用被動式超聲波網(wǎng)絡(luò)定位系統(tǒng)對室內(nèi)移動機(jī)器人進(jìn)行定位，超聲波網(wǎng)絡(luò)定位系統(tǒng)主要分為三個模塊:控制模塊、超聲波發(fā)射模塊和超聲波接收模塊。超聲波發(fā)射模塊和接收模塊都含有射頻模塊，可發(fā)射和接收射頻信號。超聲波發(fā)射模塊和接收模塊之間的距離通過超聲波發(fā)射模塊發(fā)射的超聲波信號測得。將超聲波接收模塊安裝在室內(nèi)天花板上，接收模塊的坐標(biāo)已事先通過測量獲得，并將接收模塊的坐標(biāo)數(shù)據(jù)存儲在控制模塊中。控制模塊與超聲波發(fā)射模塊通過串口相連接，固定安裝在移動機(jī)器人上。其具體定位過程如下:
(1)啟動一個安置在天花板的超聲波接收模塊，超聲波接收模塊進(jìn)入射頻信號和超聲波信號接收狀態(tài)；
(2)控制模塊向超聲波發(fā)射模塊發(fā)出定位請求，發(fā)射模塊同時發(fā)送射頻信號和超聲波信號，其中射頻信號攜帶發(fā)射模塊的身份標(biāo)識ID信息；
(3)接收模塊接收到射頻信號后，檢測接收到的ID信息與發(fā)射模塊的ID信息是否一致。若不一致則放棄，返回等待射頻信號接收狀態(tài)，執(zhí)行步驟(3);若一致，則執(zhí)行步驟(4)；
(4)接收模塊開始計時，直到檢測到超聲波信號后或超過預(yù)定的檢測時間4停止計
時，并將時間信息打包成數(shù)據(jù)包，將數(shù)據(jù)包通過射頻通訊的形式發(fā)送給發(fā)射模塊；
(5)發(fā)射模塊將接收到的數(shù)據(jù)包和檢測的溫度信息通過串口通訊的方式發(fā)送給控制模
塊；
(6)控制模塊根據(jù)超聲波在一定溫度下空氣中的傳播速度和接收模塊的時間信息,計算出接收模塊與發(fā)射模塊之間的距離；并根據(jù)接收模塊坐標(biāo)和接收模塊與發(fā)射模塊之間的距離，計算出移動機(jī)器人的平面坐標(biāo)，完成一次定位。定位流程圖如圖1所示，超聲波發(fā)射模塊和超聲波接收模塊的流程圖如圖2、3所示。
[0016]在步驟(4)中，預(yù)先設(shè)定的檢測時間石內(nèi)檢測超聲波信號是否到達(dá)，若檢測到超聲波信號到達(dá)，則記錄此刻時間為Dmf，并等待檢測時間結(jié)束；在檢測時間結(jié)束時，若未檢測到超聲波信號，則令= 為無效數(shù)據(jù)。預(yù)定的檢測時間唇是根據(jù)超聲波發(fā)射模塊
發(fā)射的超聲波在接收模塊接收靈敏度范圍內(nèi)所傳播的最遠(yuǎn)距離S而消耗的時間確定，由式(2-10)求得。設(shè)定檢測時間可有效地避免超聲波傳感器之間的串?dāng)_和超聲波信號的多次反射問題。
[0017]
【權(quán)利要求】
1. 一種室內(nèi)移動機(jī)器人定位方法，其特征在于所述定位方法步驟如下: (1)啟動一個安置在天花板的超聲波接收模塊，超聲波接收模塊進(jìn)入射頻信號和超聲波信號接收狀態(tài)； (2)控制模塊向超聲波發(fā)射模塊發(fā)出定位請求，超聲波發(fā)射模塊發(fā)送射頻信號和超聲波信號，其中射頻信號攜帶發(fā)射模塊的身份標(biāo)識ID信息； (3)接收模塊接收到射頻信號后，檢測接收到的ID信息與發(fā)射模塊的ID信息是否一致，若不一致則放棄，返回等待射頻信號接收狀態(tài)，執(zhí)行步驟(3);若一致，則執(zhí)行步驟(4)； (4)接收模塊開始計時，直到檢測到超聲波信號或超過預(yù)定的檢測時間^后停止計時，并將時間信息打包成數(shù)據(jù)包，將數(shù)據(jù)包通過射頻通訊的形式發(fā)送給超聲波發(fā)射模塊； (5)超聲波發(fā)射模塊將接收到的數(shù)據(jù)包和檢測的溫度信息通過串口通訊的方式發(fā)送給控制|吳塊； (6)控制模塊根據(jù)超聲波在一定溫度下空氣中的傳播速度和各接收模塊的時間信息，計算出超聲波接收模塊與超聲波發(fā)射模塊之間的距離； (7)當(dāng)機(jī)器人第一次獲得指定超聲波距離信息時，保持左右輪一定轉(zhuǎn)速差的情況下在超聲波接受模塊信號覆蓋區(qū)域圓周行駛； (8)選取行駛過程中，每到達(dá)圓周上的一個等分點Ft，找出由構(gòu)成的等腰直角三角形，運用ATOA方法進(jìn)行一次計算估計出機(jī)器人的當(dāng)前位置Pt作為后面DLKF的觀測值； (9)DLKF通過之前的位置估計結(jié)合里程計及電子羅盤信息運算得到的位移矢量推算得到機(jī)器人的這一時刻的先驗位姿估計數(shù)據(jù)； (10)通過前一次ATOA方法所獲得的觀測值對DLKF得到的先驗位姿估計進(jìn)行修正，得到機(jī)器人的此時刻的后驗位姿估計； (11)當(dāng)?shù)竭_(dá)下一個點Pm時，再次找出Ρ^Λ-^Λ-?ν^ι一組三個點，運用步驟(9)-(10)得到最新的機(jī)器人的位姿估計，而后每到達(dá)一個新的等分點，即重復(fù)步驟(8)-(10)，直到機(jī)器人的位姿估計方差小于所需的精度； (12)機(jī)器人停止行駛，DLKF最終的位姿估計即為此時機(jī)器人的位姿，機(jī)器人位姿初始化結(jié)束。
2.根據(jù)權(quán)利要求1所述的室內(nèi)移動機(jī)器人定位方法，其特征在于所述預(yù)定的檢測時間石是根據(jù)超聲波發(fā)射模塊發(fā)射的超聲波在接收模塊接收靈敏度范圍內(nèi)所傳播的最遠(yuǎn)距離S而消耗的時間確定，由下式求得:
3.根據(jù)權(quán)利要求1所述的室內(nèi)移動機(jī)器人定位方法，其特征在于所述ATOA方法表述為:通過一個超聲波傳感器相鄰時刻測得的絕對距離信息和航跡推算得到的相對距離信息計算期間某一時刻的機(jī)器人坐 標(biāo)。
【文檔編號】G01C21/00GK103487050SQ201310469003
【公開日】2014年1月1日 申請日期:2013年10月10日 優(yōu)先權(quán)日:2013年10月10日
【發(fā)明者】高云峰, 劉文濤, 李瑞峰, 周倫, 呂明睿 申請人:哈爾濱工業(yè)大學(xué)