一種基于超聲波測(cè)量的機(jī)器人室內(nèi)導(dǎo)航方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及導(dǎo)航控制技術(shù)領(lǐng)域，尤其涉及一種基于超聲波測(cè)量的機(jī)器人室內(nèi)導(dǎo)航 方法。
【背景技術(shù)】
[0002] 在室內(nèi)環(huán)境中，移動(dòng)機(jī)器人最基本的運(yùn)行模式就是沿走廊進(jìn)行直線行進(jìn)，由于軟 硬件方面的各種因素(如驅(qū)動(dòng)輪子的電機(jī)調(diào)速特性不一致、車輪安裝誤差、地面摩擦系數(shù)不 同、控制量解算誤差、導(dǎo)航參數(shù)解算誤差等），導(dǎo)致機(jī)器人在沿直線軌跡行駛中會(huì)偏離設(shè)定 軌跡，如果不進(jìn)行糾偏則會(huì)無(wú)法完成預(yù)期功能。而在直線糾偏的過(guò)程中重要的參考信息是 車輛的定位結(jié)果。
[0003] 使用慣性導(dǎo)航對(duì)機(jī)器人進(jìn)行定位解算，由大量的機(jī)器人直線糾偏實(shí)驗(yàn)結(jié)果可知， 航位推算的機(jī)器人位置信息在直線行駛的方向上精度較高，而在行進(jìn)方向法線方向上解算 結(jié)果與實(shí)際位置誤差較大，這一誤差主要來(lái)源于慣導(dǎo)解算中隨時(shí)間誤差累積的航向角。因 此應(yīng)當(dāng)為機(jī)器人導(dǎo)航控制系統(tǒng)引入對(duì)環(huán)境中可用信息的測(cè)量，從而使用外部信息測(cè)量結(jié)果 對(duì)慣性導(dǎo)航予以修正。

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

[0004] 本發(fā)明提供一種基于超聲波測(cè)量的機(jī)器人室內(nèi)導(dǎo)航方法，通過(guò)對(duì)機(jī)器人的航向角 信息進(jìn)行修正，提高了機(jī)器人長(zhǎng)時(shí)間運(yùn)行的可靠性。
[0005] 本發(fā)明是通過(guò)以下技術(shù)方案實(shí)現(xiàn)：
[0006] -種基于超聲波測(cè)量的機(jī)器人室內(nèi)導(dǎo)航方法，應(yīng)用于室內(nèi)走廊上，其包括以下步 驟：
[0007] 步驟1，將兩個(gè)超聲波模塊分別固定于機(jī)器人左右兩側(cè)對(duì)稱的位置上，且兩個(gè)超聲 波模塊的法線方向與地面的角度為Θ ;
[0008] 步驟2，設(shè)定機(jī)器人在室內(nèi)走廊上的運(yùn)行軌跡，機(jī)器人運(yùn)行；
[0009]
分別獲得機(jī)器人與兩側(cè)墻壁之間的實(shí)際距離Dl (k)和DR(k);其中，Dui(k)、DRii(k)分別為左右兩個(gè)超聲波模塊在法線方向上測(cè)得的超聲波 模塊與墻壁之間的距離；
[0010] 根據(jù)Width(k)=DL(k)+DR(k)+W獲得走廊寬度Width(k);其中W為所述機(jī)器人左右 兩側(cè)對(duì)稱的位置之間的寬度；
[0011] 步驟3，在k時(shí)刻，若航向角變化△ Φ大于T1，則機(jī)器人繼續(xù)按設(shè)定的運(yùn)行軌跡運(yùn)行； 若航向角變化Α Φ小于或等于T1，則執(zhí)行步驟4;其中，航向角變化△ Φ為當(dāng)前航向與設(shè)定航 向的差值；
[0012] 步驟4,將上一時(shí)刻的走廊寬度結(jié)果Width(k-l)與當(dāng)前時(shí)刻的走廊寬度Width(k) 作差，若差值大于閾值T2時(shí)，則執(zhí)行步驟5 ;若差值小于或等于閾值T2時(shí)，則根據(jù)
計(jì)算k時(shí)刻機(jī)器人沿軌跡法向位置偏差的 變化量A Xn(k)，然后執(zhí)行步驟6;
[0013]其中，DL_temt^DR_ temp為歷史測(cè)距結(jié)果，通過(guò)前k-Ι個(gè)時(shí)亥幌個(gè)超聲波模塊的測(cè)距結(jié) 果分別求平均值所得；
[0014] 步驟5,將DL(k)與歷史測(cè)距結(jié)果DL_temp進(jìn)行作差，將D R(k)與歷史測(cè)距結(jié)果DR_temp進(jìn) 行作差，如果僅有一個(gè)差值大于T3，則根據(jù)令△ Xn(k)等于大于T3的差值，然后執(zhí)行步驟6;
[0015] 如果兩個(gè)差值均大于T3,則機(jī)器人繼續(xù)按設(shè)定的運(yùn)行軌跡運(yùn)行；
[0016] 如果兩個(gè)差值均小于或等于T3,則根據(jù) 計(jì)算k時(shí)刻機(jī)器人沿軌跡法向位置偏差的變化量△ Xn(k)，然后執(zhí)行步驟6;
[0017]
計(jì)算機(jī)器人修正后的偏向角Φιι5,并 根據(jù)該偏向角調(diào)整機(jī)器人的運(yùn)行軌跡，是的當(dāng)前運(yùn)行軌跡與設(shè)定運(yùn)行軌跡重合;其中，Xt (k)和Xt(k-l)是相鄰兩次航向角解算時(shí)刻的機(jī)器人沿軌跡切線方向位置結(jié)果，均通過(guò)DR解 算獲得。
[0018] 進(jìn)一步的，所述Θ為10度。
[0019] 進(jìn)一步的，所述閾值T1為5°，閾值T2為4cm，閾值T3為3cm。
[0020] 與現(xiàn)有的技術(shù)相比，本發(fā)明的有益效果是：
[0021] 經(jīng)過(guò)該算法的修正，機(jī)器人的航向角通過(guò)不斷修正可以在長(zhǎng)時(shí)間內(nèi)維持在更高的 精度，確保了系統(tǒng)的穩(wěn)定運(yùn)行。至此完成了機(jī)器人慣性/超聲波組合導(dǎo)航算法的設(shè)計(jì)。
【附圖說(shuō)明】
[0022] 圖1為本發(fā)明的超聲波模塊安裝及測(cè)距示意圖；
[0023] 圖2為本發(fā)明的超聲波測(cè)距結(jié)果有效性判別流程圖；
[0024] 圖3為本發(fā)明的超聲波測(cè)距求走廊寬度及航向角示意圖；
[0025] 圖4為本發(fā)明的使用超聲波測(cè)距結(jié)果修正機(jī)器人航向角方法。
【具體實(shí)施方式】
[0026]為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)施例，對(duì) 本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明，并 不用于限定本發(fā)明。
[0027] 如圖2所示，一種基于超聲波測(cè)量的機(jī)器人室內(nèi)導(dǎo)航方法，其包括以下步驟：
[0028] 步驟1，如圖1所示，將兩個(gè)超聲波模塊分別固定于機(jī)器人左右兩側(cè)對(duì)稱的位置上， 模塊距地面l〇cm左右。為了防止模塊發(fā)射的本應(yīng)被墻面反射的超聲波提前被地面反射，將 超聲波模塊傾斜向上安裝。測(cè)試不同安裝角度下模塊測(cè)量數(shù)據(jù)的穩(wěn)定性及精度，最終確定 安裝角度為10°。
[0029] 步驟2,設(shè)定機(jī)器人在室內(nèi)走廊上的運(yùn)行軌跡，機(jī)器人運(yùn)行在k時(shí)刻，根據(jù)
分別獲得機(jī)器人與兩側(cè)墻壁之間的實(shí)際距離DL(k)和D R(k);其中，Dui (k)、DR測(cè)(k)分別為兩個(gè)超聲波模塊在法線方向上測(cè)得的超聲波模塊與墻壁之間的距離。
[0030]實(shí)時(shí)獲取機(jī)器人當(dāng)前航向Φ，并與設(shè)定航向iharget比較。當(dāng)機(jī)器人當(dāng)前航向Φ與設(shè)定 航向iHarget不重合時(shí)，機(jī)器人距墻壁的垂直距離應(yīng)當(dāng)有
[0032]考慮到計(jì)算的復(fù)雜度及航向角慣性解算的精度，在程序中設(shè)定當(dāng)航向角偏差Δ φ =it-iharge3t小于5°時(shí)，直接使用式(1)進(jìn)行計(jì)算，降低計(jì)算量。
[0033] 根據(jù)Width(k)=DL(k)+DR(k)+W獲得走廊寬度Width(k);其中W為機(jī)器人左右兩側(cè) 對(duì)稱的位置之間的寬度；
[0034] 步驟3,行進(jìn)之前，可以控制機(jī)器人進(jìn)行原地轉(zhuǎn)彎，并不斷采集左右兩側(cè)超聲波測(cè) 距結(jié)果進(jìn)行分析，使其自動(dòng)在初始時(shí)刻沿設(shè)定運(yùn)行軌跡方向，在本申請(qǐng)中僅僅提出一個(gè)思 考，本方案中設(shè)定機(jī)器人在初始時(shí)刻沿設(shè)定運(yùn)行軌跡方向。如圖3所示，在機(jī)器人順時(shí)針原 地旋轉(zhuǎn)的過(guò)程中，測(cè)量的兩側(cè)障礙物距離是旋轉(zhuǎn)的角度α η(機(jī)器人法線與設(shè)定航向之間的 角度)的函數(shù)，即WLR(an)。若兩側(cè)墻壁平行，則有墻體寬度Width表示為：
[0035] Width=min(ffLR(an)) (2)
[0036 ]通過(guò)WLR (an)第一次取最小值時(shí)對(duì)應(yīng)的機(jī)器人順時(shí)針旋轉(zhuǎn)角度<^可以得出旋轉(zhuǎn)的 初始時(shí)刻機(jī)器人航向角即機(jī)器人行進(jìn)方向相對(duì)于走廊方向的夾角。有
[0038]其中機(jī)器人的航向角Φο及旋轉(zhuǎn)角度αη均以順時(shí)針旋轉(zhuǎn)為正方向，且認(rèn)為機(jī)器人的 初始航向與走廊方向之間的夾角不會(huì)超過(guò)90°，旋轉(zhuǎn)過(guò)程中超聲波采樣頻率越高，測(cè)量結(jié) 果的精度越高，但仍受限于超聲波模塊的測(cè)距精度。
[0039]若在k時(shí)刻，航向角變化Δφ滿足| Δφ| >??，則判斷機(jī)器人在進(jìn)行大幅度轉(zhuǎn)彎，不 使用超聲波的測(cè)距結(jié)果進(jìn)