移動機器人原理與技術 課件 第3、4章 移動機器人的傳感器、移動機器人定位

移動機器人技術原理與應用第三章

移動機器人的傳感器移動機器人外部傳感器移動機器人視覺原理3.1移動機器人內(nèi)部傳感器3.23.33.1移動機器人內(nèi)部傳感器移動機器人內(nèi)部傳感器主要是指能夠測量移動機器人自身位置、移動速度、移動加速度等直線位移相關量和偏轉(zhuǎn)、俯仰和回轉(zhuǎn)等轉(zhuǎn)動角度和轉(zhuǎn)動速度、轉(zhuǎn)動加速度等轉(zhuǎn)動相關量。3.1.1位置傳感器設定位置或者角度的零位和極限位置對保護移動機器人的運動關節(jié)的安全動作起著重要作用。3.1.1位置傳感器電位器通過電阻把位置或角度（角度和位置的原理一樣，只是把電阻做成弧形）信息轉(zhuǎn)化為隨位置或角度變化的電壓，當滑動觸頭隨位置變化在電阻器上滑動時，觸頭接觸點變化前后的電阻阻值與總阻值之比就會發(fā)生變化，在功能上電位器充當了分壓器的作用，因此其電壓輸出將與電阻阻值成比例，這樣就把位置信息轉(zhuǎn)化為電信號。3.1.2位移和角度測量傳感器旋轉(zhuǎn)運動的角度和直線運動的位移經(jīng)常會作為移動機器人的位姿信息，從測量方法上，可分為兩種。一種是模擬式測量，將要測的位移量變換成電流、電壓等進行測量，常用器件為電位器等。另一種是數(shù)字式測量，將位移量變換成脈沖每個脈沖與單位位移相對應，檢測元件輸出脈沖數(shù)。3.1.2位移和角度測量傳感器編碼器是一個廣泛使用的位置傳感器，能夠檢測細微的運動，其輸出為數(shù)字信號。為了測量位置信息，檢測細微的運動，碼盤或碼尺被劃分為若干區(qū)域。每個區(qū)域可能是透光的或不透光的。由發(fā)光二極管作為光源，與之相對應的對光信號進行檢測的是光敏傳感器，如光電晶體管。當碼盤或碼尺采用透光方式進行檢測，光源和光敏傳感器被安裝在碼盤或碼尺的兩側(cè)；當碼盤或碼尺采用反射光方式進行檢測，光源和光敏傳感器被安裝在碼盤或碼尺的同側(cè)。3.1.2位移和角度測量傳感器(a)投射式測量轉(zhuǎn)角的旋轉(zhuǎn)增量編碼器

(b)反射式測量直線運動的直線增量編碼器(c)裝配有增量編碼器的電機

(d)增量碼盤編碼器的工作原理與結(jié)構(gòu)3.1.2位移和角度測量傳感器當光源發(fā)射的光線通過碼盤或碼尺的透光或反射被光敏傳感器接收到時，傳感器導通輸出高電平；當光源發(fā)射的光線由于碼盤或碼尺的遮擋或散射等原因使光敏傳感器接收不到光信號時，傳感器關斷，輸出信號低電平。隨著碼盤的轉(zhuǎn)動或碼尺的移動，傳感器就會連續(xù)不斷的輸出信號，通過對該信號進行計數(shù)就可以測量角位移或線位移。3.1.2位移和角度測量傳感器編碼器有增量式編碼器與絕對式編碼器兩種基本形式。上圖所示的增量式編碼器中，其透光和不透光（反光或不反光）的區(qū)域尺寸相同且交替出現(xiàn)。由于這些區(qū)域尺寸相同，均勻分布在碼盤或碼尺上，因此每個區(qū)域所表示的旋轉(zhuǎn)角度或直線位移也都是相同的。增量式編碼器可以檢測運動的變化，而對于位置信息，只有在獲知編碼器初始位置的情況下才可以確切給出。3.1.2位移和角度測量傳感器使用增量式編碼器進行位置檢測的系統(tǒng)一般在開始工作時都要進行復位，在已知復位位置的前提下就可以確定系統(tǒng)任意時刻的角位移。除了獲取精確位置信息外，還需要知道系統(tǒng)是順時針運動還是逆時針運動。單一碼道（弧圈）的增量式編碼器無法對此進行判斷，一般通過在碼盤上設計兩個碼道（弧圈）的方式解決，保證每個碼道的輸出脈沖間相差1/2拍，通過判斷那個通道輸出的上升沿或下降沿先于另一通道出現(xiàn)來確定轉(zhuǎn)動方向。3.1.2位移和角度測量傳感器增量式編碼器的輸出信號3.1.2位移和角度測量傳感器對于增量式編碼器，通過對碼道輸出的上升沿和下降沿進行檢測可以在不增加碼道的情況下提高分辨率。絕對式編碼器上每個位置對應于一個唯一的編碼信息，無需已知初始位置就可以確定任意時刻碼盤上的精確位置。絕對式編碼器工作原理3.1.2位移和角度測量傳感器絕對式編碼器的碼盤設計與增量式編碼器有很大不同，其碼盤是由多圈弧段組成，每圈互不相同，沿徑向方向各弧段的透光和不透光部分（反射和未反射部分）組成唯一編碼指示精確位置。

8位絕對格雷碼碼盤3.1.2位移和角度測量傳感器碼盤上的編碼有二進制和格雷碼兩種。采用二進制碼的碼盤從某一位置轉(zhuǎn)到其前一位置或后一位置時會有多段弧圈發(fā)生0變?yōu)?或1變?yōu)?的變化，而采用格雷碼的碼盤從某一位置轉(zhuǎn)到其前一位置或后一位置時則只用某一弧圈發(fā)生0變成1或1變?yōu)?的變化。3.1.2位移和角度測量傳感器二進制碼與格雷碼3.1.3加速度傳感器加速度計有兩種：一種是角速度加速度計，是由陀螺儀（角速度傳感器）改進的，另一種是線速度計。加速度傳感器的原理隨其應用而不同，有壓電式加速度傳感器、電容式加速度傳感器、壓阻式加速度傳感器、諧振式加速度傳感器、伺服式加速度傳感器等。3.1.3加速度傳感器1．壓電式加速度傳感器壓電式加速度傳感器具有體積小、功耗低等特點，易于集成在各種模擬和數(shù)字電路中。壓電式加速度傳感器的敏感元件有彈性梁、質(zhì)量塊、固定框組成。當有加速度a作用于傳感器時，傳感器的慣性質(zhì)量塊便會產(chǎn)生一個慣性力F作用于傳感器的彈性梁上，便會產(chǎn)生一個正比于F的應變，此時彈性梁上的壓敏電阻也會隨之產(chǎn)生一個變化量△R，由壓敏電阻組成的惠斯通電橋輸出一個與△R成正比的電壓信號V。3.1.3加速度傳感器1．壓電式加速度傳感器壓電式加速度傳感器外形3.1.3加速度傳感器2．壓阻式加速度傳感器壓阻式加速度傳感器的懸臂梁上有壓敏電阻，當慣性質(zhì)量塊發(fā)生位移時，會引起懸臂梁的身長或壓縮，改變梁上的應力分布，進而影響壓敏電阻的阻值，壓阻電阻多位于應力變化最明顯的分布。這樣，通過兩個或四個壓敏電阻形成的電橋就可以實現(xiàn)加速度的測量。3.1.3加速度傳感器2．壓阻式加速度傳感器壓阻式加速度敏感元件3.1.3加速度傳感器3．光波導加速度計光波導加速度計的原理：光源從波導1進入，經(jīng)過分束部分后分成兩部分分別通入波導4和波導2，進入波導4的一束直接被探測器2探測，而進入波導2的一束會經(jīng)過一段微小的間隙后進入波導3，最終被探測器1探測到。有加速度時，質(zhì)量塊會使得波導2玩去，進而導致其與波導3的正對面積減少，使探測器1探測到光減弱，通過比較兩個探測器檢測到的信號即可求得加速度。3.1.3加速度傳感器3．光波導加速度計光波導加速度計原理3.1.3加速度傳感器4．諧振式加速度計一根琴弦繃緊程度不同時彈奏出的聲音頻率也不同，諧振式加速度計的原理與此相同。SOA常見的結(jié)構(gòu)有S結(jié)構(gòu)和雙端固定音叉兩種。S結(jié)構(gòu)原理為：若對振梁施加確定的激振，檢測器響應就可測出其固有頻率，進而測出加速度。激振的施加和響應的檢測通常都是通過梳齒機構(gòu)實現(xiàn)的。3.1.3加速度傳感器4．諧振式加速度計諧振式加速度計原理3.1.4姿態(tài)角測量傳感器陀螺儀是用高速回轉(zhuǎn)體的動量矩敏感殼體相對慣性空間繞正交于自轉(zhuǎn)軸的角運動檢測裝置，利用其他原理制成的角運動檢測裝置起同樣功能的也稱陀螺儀，基本功能是測量敏感角位移和角速度。三軸陀螺儀分別感應Roll（左右傾斜）、Pitch（前后傾斜）、Yaw（左右搖擺）的全方位動態(tài)信息。它是構(gòu)成移動機器人慣性導航系統(tǒng)的核心敏感器件，其測量精度直接影響移動機器人姿態(tài)解算的準確性。3.1.4姿態(tài)角測量傳感器陀螺儀3.2

移動機器人外部傳感器3.2.1感覺傳感器1．觸覺感知傳感器觸覺傳感器有開關型和陣列型兩種，開關型感覺物體的存在性，陣列型感覺物體的存在和形狀，還可以感覺物體的軟硬程度。開關型是用于檢測物體是否存在的一種簡單的觸覺傳感器，可以通過切斷/導通（即導體是否有電流）來判斷是否接觸。3.2.1感覺傳感器1．觸覺感知傳感器觸覺傳感器是由幾個接觸傳感器組成的陣列，而接觸傳感器由觸桿、發(fā)光二極管和光傳感器組成。當觸覺傳感器接近物體時，觸桿將隨之縮進，遮擋了發(fā)光二極管向光傳感器發(fā)射的部分光線，于是光傳感器輸出與觸感位移或壓力成正比的信號。當觸覺傳感器與物體接觸時，依據(jù)物體的形狀和尺寸，不同的接觸傳感器將以不同的次序?qū)佑|做出不同的反應，控制器就利用此信息來確定物體的大小和形狀。3.2.1感覺傳感器1．觸覺感知傳感器觸覺傳感器3.2.1感覺傳感器2．接近覺感知傳感器接近覺傳感器用來感覺是否有目標物體接近和接近的距離，包括感應式傳感器、電容式傳感器、光學傳感器、超聲波傳感器等。感應式傳感器要求檢測的物體是金屬。電容式傳感器可以檢測任何固體和液體，測量范圍為毫米級。光學傳感器主要指紅外傳感器和超聲波傳感器。感覺是否有目標物體接近一般用紅外傳感器，測近距一般用超聲波傳感器，而且也可以用在水下測量距離、方向和速度。3.2.1感覺傳感器2．接近覺感知傳感器紅外傳感器是根據(jù)波從發(fā)射到接收的傳播過程中所受到的影響來檢測物體的接近程度。這類傳感器包括一個可以發(fā)射紅外光的固態(tài)發(fā)光二極管和一個用作接收器的固態(tài)光敏二極管（或光敏三極管）。當光強超過一定程度時光敏三極管就會導通，否則截止。把發(fā)光二極管和光敏三極管匯聚在同一面上，反射光才能被接收器看到。3.2.1感覺傳感器2．接近覺感知傳感器紅外傳感器3.2.1感覺傳感器2．接近覺感知傳感器紅外傳感器中，一般采用反射光強法進行測量，即目標物對發(fā)光二極管散射光的反射光強度進行測量。光的反射系數(shù)與目標物表面顏色、粗糙度等有關。目標顏色較深、接近黑色或透明時，其反射光很弱。若以輸出信號達到其一閾值作為“接近”時，則對不同目標物“接近”的距離是不同的。紅外光強法接近覺對大多數(shù)目標物是能找到“接近”感覺的，即能很容易地檢測出工作空間內(nèi)某物體是否存在，但是作為距離的測量并不精確。3.2.2測距傳感器超聲波傳感器發(fā)射超聲波脈沖信號，測量回波的返回時間便可得知達到物體表面的距離。如果安裝多個接收器，根據(jù)相位差還可以得到物體表面的傾斜狀態(tài)信息。但是，超聲波在空氣中衰減得很快（在1MHz的條件下為12dB/cm）,因此其頻率無法太高。超聲波傳感器可測量方向、距離、速度。移動機器人主要應用超聲波傳感器采用主動測距方式測距。主動測距方法有渡越時間法、脈沖回波法、頻率調(diào)制連續(xù)脈沖等。3.2.2測距傳感器TOF方法檢測超聲波往返距離的時間。所用的時間與超聲波通過距離成正比，記錄超聲波發(fā)射極發(fā)出脈沖時間和超聲波接收器接收到第一個返回脈沖時間，可得到時間值T，由時間T計算出距離，TOF測量方法對噪聲敏感。超聲波傳感器3.2.3視覺傳感器1．CCD圖像傳感器CCD（ChargeCoupledDevice，感光耦合組件）是攝像系統(tǒng)中可以記錄光線變化的半導體，通常以百萬像素為單位。CCD相機的原理是芯片上面整齊地排列著很多小的感光單元，光線中的光子撞擊每個單元后，在這些單元中會產(chǎn)生電子（光電效應），光子的數(shù)目與電子的數(shù)目互成比例。3.2.3視覺傳感器1．CCD圖像傳感器CCD（ChargeCoupledDevice，感光耦合組件）是攝像系統(tǒng)中可以記錄光線變化的半導體，通常以百萬像素為單位。CCD相機的原理是芯片上面整齊地排列著很多小的感光單元，光線中的光子撞擊每個單元后，在這些單元中會產(chǎn)生電子（光電效應），光子的數(shù)目與電子的數(shù)目互成比例。3.2.3視覺傳感器1．CCD圖像傳感器每一個CCD組件由上百萬個MOS電容所構(gòu)成。當數(shù)字相機的快門開啟，來自影像的光線穿過這些馬賽克色塊會讓感光點的二氧化硅材料釋放出電子〈負電〉與電洞〈正電〉。經(jīng)由外部加入電壓，這些電子和電洞會被轉(zhuǎn)移到不同極性的另一個硅層暫存起來。電子數(shù)的多寡和曝光過程光點所接收的光量成正比。在一個影像最明亮的部位，可能有超過10萬個電子被積存起來。3.2.3視覺傳感器1．CCD圖像傳感器曝光后所有產(chǎn)生的電荷都被轉(zhuǎn)移到鄰近的移位緩存器中，并且逐次逐行的轉(zhuǎn)換成信號流從矩陣中讀取出來。這些電荷訊號先被送入一個QV之中將電荷轉(zhuǎn)換成電壓；再將電壓放大進行A/D轉(zhuǎn)換。ADC將信號的連續(xù)范圍配合色塊碼賽克的分布，轉(zhuǎn)換成一個2D的平面表示列，它讓每個畫素都有一個色調(diào)值，應用這個方法，再由點組成網(wǎng)格，每一個點現(xiàn)在都有用以表示它所接受的光量的二進制數(shù)據(jù)，可以顯示強弱大小，最終再整合影像輸出。3.2.3視覺傳感器2．CMOS圖像傳感器CMOS圖像傳感器是一種典型的固體成像傳感器，通常由像元陣列、行驅(qū)動器、列驅(qū)動器、時序控制邏輯、A/D轉(zhuǎn)換器、數(shù)據(jù)總線輸出接口、控制接口等幾部分組成，其工作過程一般可分為復位、光電轉(zhuǎn)換、積分、輸出幾部分。

CMOS圖像傳感器結(jié)構(gòu)框圖3.2.3視覺傳感器2．CMOS圖像傳感器光照射像元陣列，發(fā)生光電效應，在像元內(nèi)產(chǎn)生相應的電荷。行選擇邏輯單元根據(jù)需要選通相應的行像元。行像元內(nèi)的圖像信號通過各自所在列的信號總線輸出到對應的模擬信號處理單元以及A/D轉(zhuǎn)換器，轉(zhuǎn)換成數(shù)字圖像信號輸出。其中的行選擇邏輯單元可以對像元陣列逐行掃描也可以隔行掃描，行選擇邏輯單元與列選擇邏輯單元配合使用可以實現(xiàn)圖像的窗口提取功能。模擬信號處理單元的主要功能是對信號進行放大處理，提高信噪比3.2.3視覺傳感器2．CMOS圖像傳感器由于雜質(zhì)、受熱等其他原因的影響，即使沒有光照射到象素，象素單元也會產(chǎn)生電荷，這些電荷產(chǎn)生了暗電流。暗電流與光照產(chǎn)生的電荷很難進行區(qū)分。暗電流在像素陣列各處也不完全相同，它會導致圖形噪聲。另外，光輸入信號過強的話，像素單元將飽和而不能進行光電轉(zhuǎn)換，圖像會模糊。3.3移動機器人視覺原理視覺對于移動機器人，就像眼睛對人一樣重要，根據(jù)視覺系統(tǒng)所使用的攝像機數(shù)目不同，視覺系統(tǒng)分為單目視覺系統(tǒng)、雙目視覺系統(tǒng)和多目視覺系統(tǒng)。在移動機器人領域，視覺主要用于環(huán)境中目標位姿測量，典型應用包括移動機器人視覺定位、目標跟蹤、視覺避障等。3.3.1攝像機成像模型1．參考坐標系3.3.1攝像機成像模型1．參考坐標系（1）世界坐標系將世界坐標系表示成

，此坐標系描述空間中各個目標的位置。其原點和各個坐標軸可以由人為自行定義，通常將機器人的基坐標系作為世界坐標系。（2）攝像機坐標系攝像機坐標系

是以攝像機光心為原點設立的三維直角坐標系，Zc軸即攝像機光軸，正方向為攝像機指向景象的方向，垂直于成像面。

為攝像機的焦距f。3.3.1攝像機成像模型1．參考坐標系（3）圖像物理坐標系目標物體經(jīng)由攝像機采集圖像，將像點投影到成像平面上，稱光軸與成像平面的垂直交點為圖像主點

，以主點為原點建立圖像物理坐標系

，其橫軸

和縱軸

分別平行于攝像機坐標系橫軸Xc和縱軸Yc，則物理坐標(x,y)代表空間中一點在成像平面上的投影點坐標。3.3.1攝像機成像模型1．參考坐標系（4）圖像像素坐標系用攝像機采集圖像，圖像信息是模擬的，經(jīng)過圖像采集卡的處理轉(zhuǎn)變?yōu)閿?shù)字信號。在計算機中數(shù)字圖像的表達形式是M×N二維數(shù)組，數(shù)組里的元素代表圖像像素點，元素值即為像素值，代表圖像亮度。在圖像上建立圖像像素坐標系

，原點為圖像左上角點，橫軸的正方向水平向右，縱軸垂直于向下，單位是像素，像素坐標代表了該像素在二維圖像數(shù)組位于第U列、第V行。3.3.1攝像機成像模型1．參考坐標系（4）圖像像素坐標系假定主點

的像素坐標是

，

軸和

軸相對于U軸和V軸的尺度因子依次為dx和dy，那么像素坐標系和圖像物理坐標系的關系可以下式表示：式中(u,ν)表示物點像素坐標系坐標，(x,y)表示物點物理坐標系坐標。3.3.1攝像機成像模型1．參考坐標系用齊次坐標與矩陣來表示上式則為：2．攝像機針孔模型光線照射到物體上發(fā)生反射，經(jīng)由攝像機中心（針孔）投影到成像平面上，感光元件將采集到的光信號轉(zhuǎn)變?yōu)殡娦盘?，最終形成數(shù)字圖像。變換圖形方向即可使其與景物方向一致。3.3.1攝像機成像模型2．攝像機針孔模型針孔模型3.3.1攝像機成像模型2．攝像機針孔模型點P在攝像機坐標系下的坐標為(xc,yc,zc)，

點在圖像物理坐標系下的坐標為(x,y)，直線OcOβ的模為攝像機焦距f，由三角形相似原理可知：3.3.1攝像機成像模型3．攝像機內(nèi)參模型在攝像機內(nèi)部，物點在攝像機坐標系

下的坐標映射到圖像像素坐標系

，在攝像機外部，物點在世界坐標系

下的坐標映射到攝像機坐標系

。物點由三維空間映射到成像平面的像點是失真的，成像平面上的像點對應的是起點為光心且經(jīng)過該像點的射線。得到攝像機坐標系和像素坐標系之間的關系，如下：3.3.1攝像機成像模型3．攝像機內(nèi)參模型令

，用齊次坐標和矩陣來表示，如下：3.3.1攝像機成像模型4．攝像機外參模型攝像機外參模型描述的是攝像機坐標系和世界坐標系的映射關系，用其次坐標和矩陣可表示為：(xc,yc,zc)表示景物點在攝像機坐標系中坐標；(xw,yw,zw)表示景物點在世界坐標系下的坐標；

為攝像機外參矩陣3.3.2雙目視覺原理模型1．平行雙目視覺模型平行雙目視覺模型3.3.2雙目視覺原理模型1．平行雙目視覺模型在平行雙目視覺模型中，2個攝像機的光軸平行，X軸在一條直線上且所指方向一致。左攝像機坐標系的原點為Ocl，X軸為Xcl；右攝像機坐標系的原點為Ocr，軸為Xcr；將左右攝像機光心連線OclOcr稱為基線，其長度為b；物點p在左右攝像機坐標系下的坐標分別為和；物點在左右攝像機內(nèi)部的圖像像素坐標系下的坐標為(ul,vl)和(ur,vr)。設左右攝像機的焦距為f，旋轉(zhuǎn)矩陣R是單位矩陣，平移矩陣

，根據(jù)三角形相似原理可知：3.3.2雙目視覺原理模型1．平行雙目視覺模型因此，物點p在左攝像機坐標系下的坐標可以解出：3.3.2雙目視覺原理模型2．一般雙目視覺模型一般雙目視覺模型3.3.2雙目視覺原理模型2．一般雙目視覺模型在左攝像機中可以確定物點p在射線Oclpl上，在右攝像機中可以確定物點p在射線Ocrpr上，則射線Oclpl和射線Ocrpr的交點p點。同平行雙目視覺模型，設p點在世界坐標系下的坐標為(xw,yw,zw)，左右攝像機的投影矩陣為L和

，可以得到p點世界坐標和圖像像素坐標之間的關系：3.3.2雙目視覺原理模型2．一般雙目視覺模型3.3.3攝像機標定攝像機標定方法可分為三類：傳統(tǒng)的攝像機標定方法，主動視覺攝像機標定方法和攝像機自標定方法。傳統(tǒng)的攝像機標定方法可以使用于任意的攝像機模型，精度很高，但不可以用在無法使用標定塊的環(huán)境下。主動視覺攝像機標定方法對標定環(huán)境的要求高，不能使用于攝像機運動未知的場合，且精度較低；攝像機自標定方法不能非線性標定，魯棒性不高。Faugeras的標定方法作為傳統(tǒng)的攝像機標定方法不僅算法實現(xiàn)簡單，而且可線性求解參數(shù)。3.3.3攝像機標定Faugeras標定方法選用4參數(shù)內(nèi)參矩陣，將像點和物點的成像幾何關系在齊次坐標下寫成透視投影矩陣的形式：3.3.3攝像機標定每個點可以得到兩個方程，而未知數(shù)只有11個，所以，最少取六個點即可通過最小二乘法求出

，其中移動機器人技術原理與應用第四章

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

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

為半徑作三個圓，根據(jù)畢達哥拉斯定理，可得出交點即未知點R(x0,y0)的位置計算公式方程組如下：求解方程，即可得到移動機器人當前位置R(x0,y0)的值。4.2.3路