移動(dòng)機(jī)器人原理與技術(shù) 課件 第四章 移動(dòng)機(jī)器人定位

移動(dòng)機(jī)器人技術(shù)原理與應(yīng)用第四章

移動(dòng)機(jī)器人定位移動(dòng)機(jī)器人絕對(duì)定位移動(dòng)機(jī)器人視覺定位4.1移動(dòng)機(jī)器人相對(duì)定位4.24.3移動(dòng)機(jī)器人概率定位4.44.1移動(dòng)機(jī)器人相對(duì)定位相對(duì)定位是指通過度量移動(dòng)機(jī)器人相對(duì)于起始位置的方向和距離來推斷出移動(dòng)機(jī)器人當(dāng)前的位置信息。其基本原理是在移動(dòng)機(jī)器人位姿初始值給定的前提下，基于內(nèi)部傳感器信息計(jì)算出每一時(shí)刻位姿相對(duì)于上一時(shí)刻位姿的距離以及方向角的變化，從而實(shí)現(xiàn)位姿的實(shí)時(shí)估計(jì)。常用的傳感器包括光電編碼器及慣性導(dǎo)航系統(tǒng)，因此，應(yīng)用此類傳感器進(jìn)行相對(duì)定位的方法也通常稱為航跡推算法和慣性導(dǎo)航法，相對(duì)定位也叫做位姿跟蹤。4.1.1航跡推算法原理應(yīng)用航跡推算法定位的輪式移動(dòng)機(jī)器人，通常在驅(qū)動(dòng)輪上安裝光電編碼器來測(cè)量輪子的旋轉(zhuǎn)角度，再結(jié)合移動(dòng)機(jī)器人本身的結(jié)構(gòu)和運(yùn)動(dòng)特性計(jì)算出移動(dòng)機(jī)器人相對(duì)于初始點(diǎn)的方向和位置，從而確定出位姿信息。航跡推算是個(gè)累加的過程，在移動(dòng)機(jī)器人運(yùn)動(dòng)而航跡逐步累加的過程中，移動(dòng)機(jī)器人位姿信息的測(cè)量值以及計(jì)算值均會(huì)有累積誤差，定位精度會(huì)下降，因此，航跡推算法適用于短時(shí)間或者短距離的移動(dòng)機(jī)器人位姿跟蹤。4.1.1航跡推算法原理若雙輪差動(dòng)移動(dòng)機(jī)器人在出發(fā)的初始時(shí)刻，移動(dòng)機(jī)器人自身坐標(biāo)與世界坐標(biāo)重合，經(jīng)過運(yùn)動(dòng)時(shí)間t后，移動(dòng)機(jī)器人從原點(diǎn)運(yùn)動(dòng)到目標(biāo)點(diǎn)P。設(shè)此時(shí)移動(dòng)機(jī)器人在世界坐標(biāo)系中的位置為P(Xt,Yt)，移動(dòng)機(jī)器人坐標(biāo)系的x軸與世界坐標(biāo)X軸的夾角為θt。設(shè)l為兩輪間距，Δt為編碼器計(jì)時(shí)間隔時(shí)間，則在相同的一個(gè)采樣周期Δt內(nèi)，移動(dòng)機(jī)器人轉(zhuǎn)過的角度為：4.1.1航跡推算法原理其中,VL、VR為移動(dòng)機(jī)器人的左、右輪速度。在一個(gè)采樣周期Δt內(nèi)，移動(dòng)機(jī)器人行走的距離為：該兩輪移動(dòng)機(jī)器人的平均速度

表示為：在Δt時(shí)間間隔內(nèi)，移動(dòng)機(jī)器人的位置變化表示為：4.1.1航跡推算法原理在t時(shí)刻后的t+Δt時(shí)刻，移動(dòng)機(jī)器人位置信息可表示為：對(duì)于長(zhǎng)時(shí)間的運(yùn)動(dòng)，航跡推算法存在累積誤差，此類誤差來源于移動(dòng)機(jī)器人安裝的陀螺儀、加速度計(jì)和傾斜角傳感器產(chǎn)生的漂移誤差，此類誤差可以通過建立誤差模型進(jìn)行補(bǔ)償，也可以應(yīng)用航跡推算法和其他的傳感器配合相關(guān)的定位算法進(jìn)行校正。4.1.2移動(dòng)機(jī)器人里程計(jì)定位在雙輪差動(dòng)移動(dòng)機(jī)器結(jié)構(gòu)中，里程計(jì)的工作原理是根據(jù)安裝在移動(dòng)機(jī)器人左右兩個(gè)驅(qū)動(dòng)輪電機(jī)上的光電編碼器來檢測(cè)車輪在一定時(shí)間內(nèi)轉(zhuǎn)過的弧度，進(jìn)而推算移動(dòng)機(jī)器人相對(duì)位姿的變化。將光電編碼器與驅(qū)動(dòng)輪同軸安裝，實(shí)現(xiàn)光電編碼器與驅(qū)動(dòng)輪同步旋轉(zhuǎn)。根據(jù)碼盤分辨率、驅(qū)動(dòng)輪上驅(qū)動(dòng)電機(jī)和減速器的變速比、驅(qū)動(dòng)輪的直徑等物理參數(shù)，將驅(qū)動(dòng)脈沖數(shù)轉(zhuǎn)換成驅(qū)動(dòng)輪旋轉(zhuǎn)的角度和位移，即移動(dòng)機(jī)器人相對(duì)于某一參考點(diǎn)的瞬時(shí)位置，就是里程計(jì)。4.1.2移動(dòng)機(jī)器人里程計(jì)定位若碼盤分辨率Cm、驅(qū)動(dòng)電機(jī)和減速器的變速比n、驅(qū)動(dòng)輪的直徑D，則每個(gè)脈沖對(duì)應(yīng)的移動(dòng)機(jī)器人車輪圓周上的距離為：在一個(gè)單位時(shí)間內(nèi)，若左、右輪的脈沖數(shù)Nl、Nr已知，則可確定左右輪的速度分別為：根據(jù)一個(gè)采樣周期Δt內(nèi)脈沖數(shù)算出移動(dòng)機(jī)器人轉(zhuǎn)過的角度和行走的距離，即里程計(jì)的數(shù)值，獲得移動(dòng)機(jī)器人位姿。4.1.2移動(dòng)機(jī)器人里程計(jì)定位1．差動(dòng)輪式移動(dòng)機(jī)器人直線定位若差動(dòng)輪式移動(dòng)機(jī)器人左右驅(qū)動(dòng)輪的運(yùn)動(dòng)方向、速度都相同，則機(jī)器人沿直線運(yùn)動(dòng)直線定位原理圖4.1.2移動(dòng)機(jī)器人里程計(jì)定位1．差動(dòng)輪式移動(dòng)機(jī)器人直線定位機(jī)器人在Pi+1點(diǎn)的位姿為：di為光電碼盤測(cè)得的機(jī)器人行駛路程，用左、右輪分別測(cè)得的行駛路程的算術(shù)平均值計(jì)算。4.1.2移動(dòng)機(jī)器人里程計(jì)定位2．差動(dòng)輪式移動(dòng)機(jī)器人機(jī)器人旋轉(zhuǎn)定位若差動(dòng)輪式移動(dòng)機(jī)器人左、右驅(qū)動(dòng)輪的運(yùn)動(dòng)方向相反但速度相同，則移動(dòng)機(jī)器人在原地旋轉(zhuǎn)運(yùn)動(dòng)旋轉(zhuǎn)運(yùn)動(dòng)定位原理圖4.1.2移動(dòng)機(jī)器人里程計(jì)定位2．差動(dòng)輪式移動(dòng)機(jī)器人機(jī)器人旋轉(zhuǎn)定位兩輪轉(zhuǎn)動(dòng)的角度為：d為車輪行駛里程。機(jī)器人在Pi+1點(diǎn)的位姿方向角為：4.1.3移動(dòng)機(jī)器人慣性導(dǎo)航法定位慣性導(dǎo)航法通常指采用陀螺儀和加速度計(jì)實(shí)現(xiàn)定位。根據(jù)陀螺儀和加速度計(jì)分別測(cè)得移動(dòng)機(jī)器人的回轉(zhuǎn)速度和加速度，利用一次積分和二次積分求出角度和位置參量。陀螺儀通過對(duì)所測(cè)量的角度和速度進(jìn)行積分，計(jì)算出相對(duì)于起始方向的偏轉(zhuǎn)角度。δ為t時(shí)刻相對(duì)起始方向的偏轉(zhuǎn)角度；ω為瞬時(shí)角速度；t0為起始時(shí)間。4.1.3移動(dòng)機(jī)器人慣性導(dǎo)航法定位基于光電編碼器的里程計(jì)定位和基于陀螺儀和加速度計(jì)的慣性導(dǎo)航法定位方法中，都存在不可避免的誤差。對(duì)于常見的輪式移動(dòng)機(jī)器人來說，通常將誤差源分為兩大類：系統(tǒng)誤差和非系統(tǒng)誤差。其中系統(tǒng)誤差包括：車輪半徑不相等、車輪半徑均值與標(biāo)稱輪半徑不等、車輪不平行、有效輪距未知、傳感器分辨率有限、傳感器采樣頻率有限。非系統(tǒng)誤差包括：行駛的地面不平坦、地面有不可預(yù)知的物體、由于有些原因?qū)е萝囕喰旭傔^程當(dāng)中的打滑（光滑的地面、加速度過大、急速轉(zhuǎn)彎、外界的作用力等）。4.2移動(dòng)機(jī)器人絕對(duì)定位絕對(duì)定位能確定移動(dòng)機(jī)器人在全局參考框架下的位姿信息，不依賴于時(shí)間和初始位姿，因此，不但沒有累積誤差問題，而且還具有精度高、可靠性強(qiáng)等特點(diǎn)?；谛艠?biāo)的定位、基于地圖匹配方法的定位以及基于衛(wèi)星的定位都屬于絕對(duì)定位方法的范疇。從傳感器角度，基于紅外、超聲波、激光、以及相機(jī)的定位都屬于絕對(duì)定位。從技術(shù)手段上，偽衛(wèi)星、Wi-Fi、射頻標(biāo)簽（RFID）、藍(lán)牙（Bluetooth，BT）、超寬帶（UltraWideBand，UWB）、地磁以及光跟蹤等，也屬于絕對(duì)定位范圍。4.2.1BDS定位中國(guó)北斗衛(wèi)星導(dǎo)航系統(tǒng)(BeiDouNavigationSatelliteSystem，BDS)是中國(guó)自行研制的全球衛(wèi)星導(dǎo)航系統(tǒng)，采用三球交匯定位原理進(jìn)行定位。在空間中已知A、B、C三點(diǎn)的位置，待定位移動(dòng)機(jī)器人看作質(zhì)點(diǎn)D，D點(diǎn)到上述三點(diǎn)的距離皆已知的情況下，可以確定出移動(dòng)機(jī)器人D的空間位置，D點(diǎn)一定位于分別以A、B、C為圓心，AD、BD和CD為半徑的三個(gè)圓球的交匯點(diǎn)上。中國(guó)北斗衛(wèi)星導(dǎo)航系統(tǒng)分為三個(gè)部分，分別為空間段、地面段、用戶段。4.2.1BDS定位空間段：北斗衛(wèi)星導(dǎo)航系統(tǒng)的空間段計(jì)劃由35顆衛(wèi)星組成，包括5顆靜止軌道衛(wèi)星、27顆中地球軌道衛(wèi)星、3顆傾斜同步軌道衛(wèi)星。5顆靜止軌道衛(wèi)星定點(diǎn)位置為東經(jīng)58.75°、80°、110.5°、140°、160°，中地球軌道衛(wèi)星運(yùn)行在3個(gè)軌道面上，軌道面之間為相隔120°均勻分布。地面段：由中心控制系統(tǒng)和標(biāo)校系統(tǒng)組成。中心控制系統(tǒng)主要用于衛(wèi)星軌道的確定、電離層校正、用戶位置確定、用戶短報(bào)文信息交換等。標(biāo)校系統(tǒng)可提供距離觀測(cè)量和校正參數(shù)。系統(tǒng)的地面段由主控站、注入站、監(jiān)測(cè)站組成。4.2.1BDS定位主控站用于系統(tǒng)運(yùn)行管理與控制等。主控站從監(jiān)測(cè)站接收數(shù)據(jù)并進(jìn)行處理，生成衛(wèi)星導(dǎo)航電文和差分完好性信息，而后交由注入站執(zhí)行信息的發(fā)送。注入站用于向衛(wèi)星發(fā)送信號(hào)，對(duì)衛(wèi)星進(jìn)行控制管理，在接受主控站的調(diào)度后，將衛(wèi)星導(dǎo)航電文和差分完好性信息向衛(wèi)星發(fā)送。監(jiān)測(cè)站用于接收衛(wèi)星的信號(hào)，并發(fā)送給主控站，可實(shí)現(xiàn)對(duì)衛(wèi)星的監(jiān)測(cè)，以確定衛(wèi)星軌道，并為時(shí)間同步提供觀測(cè)資料。4.2.1BDS定位用戶段：用戶段即用戶的終端，既可以是專用于北斗衛(wèi)星導(dǎo)航系統(tǒng)的信號(hào)接收機(jī)，也可以是同時(shí)兼容其他衛(wèi)星導(dǎo)航系統(tǒng)的接收機(jī)。接收機(jī)需要捕獲并跟蹤衛(wèi)星的信號(hào)，根據(jù)數(shù)據(jù)按一定的方式進(jìn)行定位計(jì)算，最終得到用戶的經(jīng)緯度、高度、速度、時(shí)間等信息。移動(dòng)機(jī)器人控制系統(tǒng)中應(yīng)用北斗衛(wèi)星導(dǎo)航系統(tǒng)用戶端軟硬件，即可以實(shí)現(xiàn)絕對(duì)定位。4.2.2GPS定位GPS導(dǎo)航系統(tǒng)是以全球24顆定位人造衛(wèi)星為基礎(chǔ)，向全球各地全天候地提供三維位置、三維速度等信息的一種導(dǎo)航定位系統(tǒng)。該系統(tǒng)衛(wèi)星分布在6個(gè)軌道面上，在地球表面的任何地域，任何時(shí)間都可以至少同步接收4顆以上的衛(wèi)星信號(hào)。GPS不斷向地面發(fā)送導(dǎo)航電文，地面上任何位置就可以根據(jù)4顆衛(wèi)星信道的電文信息推算出當(dāng)前接收者的三維位置，實(shí)現(xiàn)導(dǎo)航定位目的。GPS定位系統(tǒng)由三部分構(gòu)成，一是空間部分；二是地面控制部分；三是用戶裝置部分。4.2.2GPS定位空間部分：GPS定位系統(tǒng)的空間部分是由24顆GPS工作衛(wèi)星所組成，這些GPS工作衛(wèi)星共同組成GPS衛(wèi)星星座，其中21顆為可用于導(dǎo)航的衛(wèi)星，3顆為備用衛(wèi)星。這24顆衛(wèi)星分布在6個(gè)傾角為55°的軌道上繞地球運(yùn)行，衛(wèi)星的運(yùn)行周期約為12恒星時(shí)。每顆GPS工作衛(wèi)星都發(fā)出用于導(dǎo)航定位的信號(hào)，GPS用戶正是利用這些信號(hào)來進(jìn)行工作的。4.2.2GPS定位地面控制部分：由分布在全球的由若干個(gè)跟蹤站所組成的監(jiān)控系統(tǒng)所構(gòu)成。根據(jù)其作用的不同，這些跟蹤站又被分為主控站、監(jiān)控站和注入站。主控站的作用是根據(jù)各監(jiān)控站對(duì)GPS的觀測(cè)數(shù)據(jù)，計(jì)算出衛(wèi)星的星歷和衛(wèi)星鐘的改正參數(shù)等，并將這些數(shù)據(jù)通過注入站注入到衛(wèi)星中去，同時(shí)，它還對(duì)衛(wèi)星進(jìn)行控制，向衛(wèi)星發(fā)布指令，當(dāng)工作衛(wèi)星出現(xiàn)故障時(shí)調(diào)度備用衛(wèi)星，替代失效的工作衛(wèi)星工作；另外，主控站也具有監(jiān)控站的功能；注入站的作用是將主控站計(jì)算出的衛(wèi)星星歷和衛(wèi)星鐘的改正數(shù)等注入到衛(wèi)星中去。4.2.2GPS定位用戶部分：由GPS接收機(jī)、數(shù)據(jù)處理軟件及相應(yīng)的用戶設(shè)備如計(jì)算機(jī)氣象儀器等所組成。它的作用是接收GPS衛(wèi)星所發(fā)出的信號(hào)，利用這些信號(hào)進(jìn)行導(dǎo)航定位等工作。4.2.3路標(biāo)定位路標(biāo)是指具有明顯特征而能夠被移動(dòng)機(jī)器人傳感器識(shí)別的特殊物體，路標(biāo)有人工路標(biāo)和自然路標(biāo)兩種類型，人工路標(biāo)就是專門設(shè)計(jì)的物體或者標(biāo)識(shí)物，只具有給機(jī)器人導(dǎo)航的唯一應(yīng)用；自然路標(biāo)是一些物體或者特征，除導(dǎo)航外，還具有其他功能。移動(dòng)機(jī)器人定位的主要任務(wù)就是應(yīng)用路標(biāo)觀測(cè)的傳感器如超聲波傳感器、激光雷達(dá)、視覺傳感器等可靠地辨識(shí)路標(biāo)，并據(jù)此計(jì)算出移動(dòng)機(jī)器人的位置。位精度的高低取決于對(duì)路標(biāo)的識(shí)別以及位置信息提取的準(zhǔn)確程度。4.2.3路標(biāo)定位三邊定位測(cè)量法是根據(jù)移動(dòng)機(jī)器人與路標(biāo)之間的距離來確定移動(dòng)機(jī)器人位置的方法。三邊測(cè)量定位系統(tǒng)至少需要將3個(gè)已知位置的信號(hào)發(fā)射器（或者接收器），而接收器（或者發(fā)射器）安裝在移動(dòng)機(jī)器人上。在三邊測(cè)量法導(dǎo)航系統(tǒng)中，有三個(gè)以上在環(huán)境中已知的固定地點(diǎn)，移動(dòng)機(jī)器人根據(jù)這些固定點(diǎn)的距離，應(yīng)用幾何三角學(xué)確定自身在已知坐標(biāo)系中的坐標(biāo)。測(cè)距方法采用時(shí)間—路程計(jì)算方法，根據(jù)超聲波發(fā)射波傳播時(shí)間，系統(tǒng)可以計(jì)算固定點(diǎn)的發(fā)射器與移動(dòng)機(jī)器人上接收器之間的距離。4.2.3路標(biāo)定位三邊測(cè)量法示意圖4.2.3路標(biāo)定位R(x0,y0)為移動(dòng)機(jī)器人當(dāng)前位置，S1、S2、S3分別為三個(gè)固定位置,已知移動(dòng)機(jī)器人到三點(diǎn)S1、S2、S3的距離I1、I2和I3，以I1,I2,I3

為半徑作三個(gè)圓，根據(jù)畢達(dá)哥拉斯定理，可得出交點(diǎn)即未知點(diǎn)R(x0,y0)的位置計(jì)算公式方程組如下：求解方程，即可得到移動(dòng)機(jī)器人當(dāng)前位置R(x0,y0)的值。4.2.3路標(biāo)定位相對(duì)定位方法能夠根據(jù)移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型對(duì)每一時(shí)刻的移動(dòng)機(jī)器人位姿進(jìn)行迭代遞推，不依賴于外界環(huán)境信息，短時(shí)精度較好。但該類定位方法難以避免地存在誤差累積問題；絕對(duì)定位方法雖然具有精度商、可靠性強(qiáng)等優(yōu)點(diǎn)，但該類方法同樣具有自身的局限性，比如基于導(dǎo)航路標(biāo)的定位方法只適用于設(shè)有導(dǎo)航路標(biāo)的環(huán)境中，方法本身局限性較大；基于衛(wèi)星的定位方法依賴于衛(wèi)星信號(hào)的可獲得性及信號(hào)質(zhì)量，在室外空曠的環(huán)境下，定位精度可達(dá)到很高，但在室內(nèi)由于信號(hào)無法獲得而失效。4.2.3路標(biāo)定位在城市道路環(huán)境下，由于建筑物的遮擋，或信號(hào)反射造成的多徑效應(yīng)，道路兩旁的樹木的遮擋等原因，定位精度也會(huì)受到很大影響。為了實(shí)現(xiàn)以上兩種定位方法的優(yōu)勢(shì)互補(bǔ)，形成了基于航位推算的相對(duì)定位信息和絕對(duì)定位信息相結(jié)合的組合定位方法。4.3移動(dòng)機(jī)器人視覺定位對(duì)于安裝在移動(dòng)機(jī)器人上的單目相機(jī)，使其光軸平行于地面。如果相機(jī)本身的內(nèi)參數(shù)已知，并且已知一直平行于相機(jī)坐標(biāo)系的X或Y軸的平面上的任意兩個(gè)點(diǎn)的位置，則可以通過這兩個(gè)點(diǎn)的圖像坐標(biāo)計(jì)算出移動(dòng)機(jī)器人的位置和姿態(tài)，即實(shí)現(xiàn)移動(dòng)機(jī)器人單目視覺定位。根據(jù)計(jì)算機(jī)視覺原理，對(duì)于一個(gè)平面上的點(diǎn)P在兩個(gè)視點(diǎn)Oc1、Oc2下的圖像齊次坐標(biāo)I1=[u1v11]T和I2=[u2v21]T，存在一組單應(yīng)性矩陣H，即下式成立：4.3移動(dòng)機(jī)器人視覺定位相機(jī)坐標(biāo)系與平面坐標(biāo)系4.3移動(dòng)機(jī)器人視覺定位設(shè)在相機(jī)坐標(biāo)系下平面上的點(diǎn)P的歐幾里得空間坐標(biāo)為Pc=[xcyczc]T，已知相機(jī)內(nèi)參數(shù)矩陣為Min，則：He稱為歐幾里得空間單應(yīng)性矩陣。He即為相機(jī)在兩個(gè)視點(diǎn)Oc1、Oc2的坐標(biāo)系之間的變換。通過平面上的3個(gè)點(diǎn)，可以確定He的參數(shù)。當(dāng)移動(dòng)機(jī)器人運(yùn)動(dòng)時(shí)，Oc2視點(diǎn)相對(duì)于視點(diǎn)Oc1作平移運(yùn)動(dòng)，相機(jī)的姿態(tài)繞軸旋轉(zhuǎn)。4.3移動(dòng)機(jī)器人視覺定位θ為z