移動(dòng)機(jī)器人原理與技術(shù) 課件 第3、4章 移動(dòng)機(jī)器人的傳感器、移動(dòng)機(jī)器人定位

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

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

(b)反射式測(cè)量直線運(yùn)動(dòng)的直線增量編碼器(c)裝配有增量編碼器的電機(jī)

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

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

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

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

，此坐標(biāo)系描述空間中各個(gè)目標(biāo)的位置。其原點(diǎn)和各個(gè)坐標(biāo)軸可以由人為自行定義，通常將機(jī)器人的基坐標(biāo)系作為世界坐標(biāo)系。（2）攝像機(jī)坐標(biāo)系攝像機(jī)坐標(biāo)系

是以攝像機(jī)光心為原點(diǎn)設(shè)立的三維直角坐標(biāo)系，Zc軸即攝像機(jī)光軸，正方向?yàn)閿z像機(jī)指向景象的方向，垂直于成像面。

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

，以主點(diǎn)為原點(diǎn)建立圖像物理坐標(biāo)系

，其橫軸

和縱軸

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

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

的像素坐標(biāo)是

，

軸和

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

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

下的坐標(biāo)映射到圖像像素坐標(biāo)系

，在攝像機(jī)外部，物點(diǎn)在世界坐標(biāo)系

下的坐標(biāo)映射到攝像機(jī)坐標(biāo)系

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

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

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

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

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

，其中移動(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路